双足机器人设计

HANGOOD 北京汉库科技有限责任公司 ============================================================================================================================================================= HGR-3HV-C步伐源代码手册 研发部 北京汉库科技有限责任公司 目录 2目录 5一. 步伐函数解析 51. 初始站立子程序解析 8二．单一动作变速运动子程序解析 81．双腿下蹲动作变速子程序解析 9（1）下蹲第1族子程序 11（2）下蹲第2族子程序 132．双腿起立动作变速子程序解析 14（1）起立第1族子程序 16（2）起立第2族子程序 183．双腿侧身动作变速子程序解析 18（1）向左侧身第1族子程序 21（2）向左侧身第2族子程序 24（3）向右侧身第1族子程序 27（4）向右侧身第2族子程序 304．单腿运动变速子程序解析 30（1）左腿抬起第1族子程序 32（2）左腿抬起第2族子程序 34（3）右腿抬起第1族子程序 36（4）右腿抬起第2族子程序 38（5）左腿落下第1族子程序 40（6）左腿落下第2族子程序 42（7）右腿落下第1族子程序 44（8）右腿落下第2族子程序 46三．Z、P面分立原地踏步动作解析 461．舵机方向制定 472．动作解析 47（1）动作条件 47（2）动作时序分析 49（3）动作算法及程序 51四．Z、P面分立带断点前进步伐解析 511．舵机方向制定 522．前进第1族步伐解析 52（1）动作条件 52（2）动作时序分析 533．中间带速度断点的前进方法 53（1）运动特征说明 54（2）舵机实际转动方向及角度 564．新增专用子程序解析 56（1）前进中右腿抬起第1族子程序 57（2）前进中右腿落下第2族子程序 58（3）前进中左腿抬起第1族子程序 59（4）前进中左腿落下第2族子程序 605．前进第1族子程序解析 60（1）程序内容及算法 61（2）相关技术拓展 62五．Z、P面分立无断点前进步伐解析 621．舵机方向制定 632．动作时序分析 643．中间无速度断点的前进方法 64（1）运动特征说明 65（2）舵机实际转动方向及角度 674．新增专用子程序解析 67（1）前进中右腿抬起第2族子程序 68（2）前进中右腿落下第1族子程序 69（3）前进中左腿抬起第2族子程序 70（4）前进中左腿落下第1族子程序 715．前进第2族子程序解析 71（1）程序内容及算法（分立无断点步伐） 72（2）相关技术拓展 73六．第1类交互式前进步伐解析 731．舵机方向制定 742．第1类交互前进步伐特点 74（1）动作条件 74（2）动作时序分析 753．新增的4个子程序 75（1）左侧身+抬右腿子程序（第2族） 77（2）右侧身+落右腿子程序（第1族） 78（3）右侧身+抬左腿子程序（第2族） 79（4）左侧身+落左腿子程序（第1族） 804．交互后PWM_8P1与PWM_8P2子程序的合成 805．PWM_16P子程序解析 816．第1类交互式前进步伐子程序指令 82七．第2类交互式前进步伐解析 831．舵机方向制定 832．第2类交互前进步伐特点 83（1）动作条件 84（2）动作时序分析 853．改动的4个运动子程序 85（1）抬右腿前进第2族子程序 86（2）落右腿前进第1族子程序 87（3）抬左腿前进子程序第2族 88（4）落左腿前进子程序第1族 894．第2类交互式前进步伐子程序指令 一. 步伐函数解析 1. 初始站立子程序解析 打开光盘中的项目文件中的“初始烧写”文件夹 选择芯片类型； 1.选择要烧写的HEX文件； 2.设置串行端口和波特率，这里要注意端口号，波特率的选择比较任意一般为38400； 3.选择外部晶振；与下载无关；清FLASH区； 4.点击下载按钮后，打开单片机供电电源。在上电后单片机会自动进入编程状态。通过提示可以判断是否下载完成。 注意： 1. 在选择芯片时一定要看清型号，很容易选错。 2. 应选择外部晶振，如果选择内部晶振，单片可以工作，但是舵机的控制会出现问题。 3. 一定要先点编程按钮，然后在开控制板上的电源，要不下载不会成功。 二．单一动作变速运动子程序解析 1．双腿下蹲动作变速子程序解析 下蹲动作可以自由运行出许多种轨迹，我们将其归纳成2种，各代1种运动特性。 第1种：下蹲过程中只有1个阶段，速度逐渐增加，加速度不变或加速度逐渐减小，到达下极限位置时机器人的腿部会产生极大的压力；优点是可以增加脚面的附着力，缺点是系统负荷大，不宜经常采用此动作。 第2种：下蹲过程中分为的2个阶段，开始时速度最小，然后速度逐渐增大，运动至中点时速度最大，加速度是逐渐减小的函数；然后速度逐渐减小，到达下极限位置时机器人的腿部会产生最小的压力；优点是属于软着陆，平稳过渡。 其中包含2种分立运动，我们将其称作2族。 第1族：速度逐渐增大，直至最大；此过程中，加速度函数可以描述成递增或递减的函数。 第2族：速度逐渐减小，直至最小；此过程中，加速度函数可以描述成递增或递减的函数。 （1）下蹲第1族子程序 系统会自动执行每个微分元的动作位置，自动提取并装入初始位置，自动进行积分步数 合理性判断。（XD_ZU1） 扫尾系数最大为：Sa=40 下蹲过程的积分步数最大为：Ea=76， 有1个问题摆出来了，那就是76与40的换算关系。 我们发现积分步数不一定能够达到76，过程中出现在1/2*67的概率最大，即为38。 那么38≈40 ，所以在算法上采取，将Ea的直接放入Sa的初值。 我们发现当积分步数Ea较小时，腿部转动角度也较小，则启动速度可以快些，系统能够保持稳定，也就是说扫尾数初值可以小些，即Sa较小。 我们还发现当积分步数Ea较大时，腿部转动角度也较大，则启动速度要求慢些，系统能够保持稳定，也就是说扫尾数初值可以大些，即Sa较大。 依据以上的机械运动特性，我们推出以下公式： 初始时Sa = Ea ； 过程中1≤Sa≤Ea 伴随着系统运动，积分步数Ea逐渐减小，速度参数Sa逐渐减小，速度逐渐加快； 属于第1族运动方式，即积分停止时加速度反向且值最大。 用户输入：运动方式编码、积分步数（该数值可以合理也可以不合理） 程序出口：所有舵机的位置，系统稳定系数（“1”稳定，“0”不稳定） 下蹲第1族子程序算法 下蹲第1族子程序指令 程序入口：（uchar foot）放入积分运算器初值 // 函数原型：void xd_zu1(uchar foot)) // 函数名称：下蹲第1族子程序。 // 功 能：加速下蹲，速度增加有一个过程，有利于机器人平衡稳定性 // 参 数：foot, 表示积分步数。 // 返 回 值：无 // 函数编号：一 // ┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈ void xd_zu1(uchar foot) { uchar i,j; j=foot/2+zsax; //z平面扫尾值初始化计算公式 for(i=0;i5) { j--; sao_wei(j); //调用扫尾值计算子程序,速度加快 zsag=j; } else { sao_wei(5); zsag=5; } } } （2）下蹲第2族子程序 系统会自动执行每个微分元的动作位置，自动提取并装入初始位置，自动进行积分步数 合理性判断。（XD_ZU2）整个过程分为2个阶段，加速与减速阶段，积分中间点速度最大。 扫尾系数最大为：Sa=40 下蹲过程的积分步数最大为：Ea=76，1/2* Ea=38； 有1个问题摆出来了，那就是76与40的换算关系。 我们发现积分步数不一定能够达到76，过程中出现在1/2*67的概率最大，即为38。 那么38≈40 ，所以在算法上采取，将Ea的直接放入Sa的初值。 我们发现当积分步数Ea较小时，腿部转动角度也较小，则启动速度可以快些，系统能够保持稳定，也就是说扫尾数初值可以小些，即Sa较小。 我们还发现当积分步数Ea较大时，腿部转动角度也较大，则启动速度要求慢些，系统能够保持稳定，也就是说扫尾数初值可以大些，即Sa较大。 依据以上的机械运动特性，我们推出以下公式： 初始时Sa = Ea ； 过程中1≤Sa≤Ea 伴随着系统运动，积分步数Ea逐渐减小，速度参数Sa逐渐减小，速度逐渐加快； 属于第1族运动方式，即积分停止时加速度反向且值最大。 用户输入：运动方式编码、积分步数（该数值可以合理也可以不合理） 程序出口：所有舵机的位置，系统稳定系数（“1”稳定，“0”不稳定） 下蹲第2族子程序算法 下蹲第2族子程序 程序入口：（uchar foot）放入积分运算器初值 // 函数原型：void xd_zu2(uchar foot)) // 函数名称：下蹲第2族子程序 // 功 能：减速下蹲，减小脚板对地面的作用力，有利于机器人的平衡稳定性 // 参 数: foot, 表示积分步数。 // 返 回 值：无 // 函数编号：二 // ┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈ void xd_zu2(uchar foot) { uchar i,j; j=zsag; //z平面扫尾值过渡 for(i=0;i5) { j--; sao_wei(j); //调用扫尾值计算子程序，加速起立 j=zsag; } else { sao_wei(5); zsag=5; } } } （2）起立第2族子程序 系统会自动执行每个微分元的动作位置，自动提取并装入初始位置，自动进行积分步数 合理性判断。（QL_ZU1） 扫尾系数最大为：Sa=40 下蹲过程的积分步数最大为：Ea=76， 也是这个问题，76与40的换算关系。 我们发现积分步数不一定能够达到76，过程中出现在1/2*67的概率最大，即为38。 那么38≈40 ，所以在算法上采取，将Ea的直接放入Sa的初值。 我们发现当积分步数Ea较小时，腿部转动角度也较小，则启动速度可以快些，系统能够保持稳定，也就是说扫尾数初值可以小些，即Sa较小。 我们还发现当积分步数Ea较大时，腿部转动角度也较大，则启动速度要求慢些，系统能够保持稳定，也就是说扫尾数初值可以大些，即Sa较大。 依据以上的机械运动特性，我们推出以下公式： 初始时Sa = Ea ； 过程中1≤Sa≤Ea 伴随着系统运动，积分步数Ea逐渐减小，速度参数Sa逐渐减小，速度逐渐加快； 属于第1族运动方式，即积分停止时加速度反向且值最大。 用户输入：运动方式编码、积分步数（该数值可以合理也可以不合理） 程序出口：所有舵机的位置，系统稳定系数（“1”稳定，“0”不稳定） 起立第2族子程序算法 起立第2族子程序 程序入口：（MOV 1EH,#XXH）放入积分运算器初值 // 函数原型：void ql_zu2(uchar foot) // 函数名称：起立第2族子程序 // 功 能：减速下蹲，减小脚板对地面的作用力，有利于机器人的平衡稳定性 // 参 数：foot, 表示积分步数。 // 返 回 值：无 // 函数编号：四 // ┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈ void ql_zu2(uchar foot) { uchar i,j=0; j=zsag; //z平面扫尾值过渡 for(i=0;i5) { j--; j--; sao_wei(j); //加速向左侧身,提高反映速度 psag=j; } else { sao_wei(5); psag=5; } } } （2）向左侧身第2族子程序 系统会自动执行每个微分元的动作位置，自动提取并装入初始位置，自动进行积分步数 合理性判断。（L_CS_ZU2） 虽然腿部的胯关节可以大角度转动，但是脚踝关节只能从# H转到# H，由于在侧身动作中，胯骨与踝骨保持平行机构，则转动幅度受到限制。根据实际机械测量，HGR-3的侧身极限参数如下： 舵机号 P1 P2 P3 P4 位置寄存器 Position[8] Position[9] Position[10] Position[11] 端口 P2.0 P2.1 P2.2 P2.3 上极限 120 120 162 162 中间态 104 104 146 146 下极限 88 88 130 130 幅度系数最大为：Ea = 160-104=56 扫尾系数最大为：Sa = 40 同样的问题，32与40的换算关系。 我们发现积分步数不一定能够达到32，过程中出现在1/2*56的概率最大，即为28。 那么28<40 ，所以在算法上采取，将Ea的直接放入Sa的初值；这样的作会使实际的扫尾数普遍便小，速度会略增大； 避免此种现象发生，我们令：实际Sa = 计算Sa+补偿Sa； 具体补偿Sa值由实际测试得出：补偿Sa=# H； 我们发现当积分步数Ea较小时，腿部转动角度也较小，则启动速度可以快些，系统能够保持稳定，也就是说扫尾数初值可以小些，即Sa较小。 我们还发现当积分步数Ea较大时，腿部转动角度也较大，则启动速度要求慢些，系统能够保持稳定，也就是说扫尾数初值可以大些，即Sa较大。 依据以上的机械运动特性，我们推出以下公式： 初始时Sa = Ea ； 过程中1≤Sa≤Ea 伴随着系统运动，积分步数Ea逐渐减小，速度参数Sa逐渐增大，速度逐渐减慢； 属于第2族运动方式，即积分停止时速度最小或出现“0”值。 用户输入：运动方式编码、积分步数（该数值可以合理也可以不合理） 程序出口：所有舵机的位置，系统稳定系数（“1”稳定，“0”不稳定） 向左侧身第2族算法 注意： 由于执行每个微分元程序时Sa数每次必一定“+1”或“-1”，那么就不能直接采用“sao_wei(saowei)”的函数，要采用“+极限值”或“-极限值”，并判断“j”属性的方法，来判断是否Sa数到达极限值。 向左侧身第2族子程序 程序入口：（uchar foot）放入积分运算器初值 Psax P平面Sa修正数寄存器 // 函数原型：void l_cs__zu2(uchar foot) // 函数名称：向左侧身第2族子程序 // 功 能：减速向左侧身，速度的减小有一个过程，有利于机器人的平衡稳定性 // 参 数： foot, 表示积分步数。 // 返 回 值： 无 // 函数编号： 六 // ┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈ void l_cs__zu2(uchar foot) { uchar i,j; j=(foot/2)&127+psax; //p平面扫尾值初始化计算公式 for(i=0;i6) { j--; j--; sao_wei(j); //扫尾值自减，象右侧身速度加快 psag=j; } else { sao_wei(6); psag=6; } } } （4）向右侧身第2族子程序 系统会自动执行每个微分元的动作位置，自动提取并装入初始位置，自动进行积分步数 合理性判断。（R_CS_ZU2） 虽然腿部的胯关节可以大角度转动，但是脚踝关节只能从# H转到# H，由于在侧身动作中，胯骨与踝骨保持平行机构，则转动幅度受到限制。根据实际机械测量，HGR-3的侧身极限参数如下： 舵机号 P1 P2 P3 P4 位置寄存器 Position[8] Position[9] Position[10] Position[11] 端口 P2.0 P2.1 P2.2 P2.3 上极限 120 120 162 162 中间态 104 104 146 146 下极限 88 88 130 130 幅度系数最大为：Ea = 160-104=56 扫尾系数最大为：Sa = 40 同样的问题，32与40的换算关系。 我们发现积分步数不一定能够达到56，过程中出现在1/2*56的概率最大，即为28。 那么28<40 ，所以在算法上采取，将Ea的直接放入Sa的初值；这样的作会使实际的扫尾数普遍便小，速度会略增大； 避免此种现象发生，我们令：实际Sa = 计算Sa+补偿Sa； 具体补偿Sa值由实际测试得出：补偿Sa=# H； 我们发现当积分步数Ea较小时，腿部转动角度也较小，则启动速度可以快些，系统能够保持稳定，也就是说扫尾数初值可以小些，即Sa较小。 我们还发现当积分步数Ea较大时，腿部转动角度也较大，则启动速度要求慢些，系统能够保持稳定，也就是说扫尾数初值可以大些，即Sa较大。 依据以上的机械运动特性，我们推出以下公式： 初始时Sa = Ea ； 过程中1≤Sa≤Ea 伴随着系统运动，积分步数Ea逐渐减小，速度参数Sa逐渐增大，速度逐渐减慢； 属于第2族运动方式，即积分停止时速度最小或出现“0”值。 用户输入：运动方式编码、积分步数（该数值可以合理也可以不合理） 程序出口：所有舵机的位置，系统稳定系数（“1”稳定，“0”不稳定） 向右侧身第2族算法 注意： 由于执行每个微分元程序时Sa数每次必一定“+1”或“-1”，那么就不能直接采用“sao_wei(saowei)”的函数，要采用“+极限值”或“-极限值”，并判断“j”属性的方法，来判断是否Sa数到达极限值。 向右侧身第2族子程序 程序入口：（uchar foot ）放入积分运算器初值 psag P平面Sa过渡寄存器 // 函数原型：void r_cs__zu2(uchar foot) // 函数名称：向右侧身第2族子程序 // 功 能：向右侧身速度减小，速度减小有一个过程，有利于机器人的平衡稳定性 // 参 数： foot, 表示积分步数。 // 返 回 值： 无 // 函数编号： 八 // ┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈ void r_cs__zu2(uchar foot) { uchar i,j; j=psag; //p平面扫尾值过渡 for(i=0;i5) { j--; sao_wei(j); //扫尾值自减，左腿抬起速度加快 zsag=j; } else { sao_wei(5); zsag=5; } } } （2）左腿抬起第2族子程序 系统会自动执行每个微分元的动作位置，自动提取并装入初始位置，自动进行积分步数 合理性判断。（LT_UP_ZU2） 扫尾系数最大为：Sa=40 下蹲过程的积分步数最大为：Ea=76， 也是这个问题，76与40的换算关系。 我们发现积分步数不一定能够达到76，过程中出现在1/2*76的概率最大，即为38。 那么38≈40 ，所以在算法上采取，将Ea的直接放入Sa的初值。 我们发现当积分步数Ea较小时，腿部转动角度也较小，则启动速度可以快些，系统能够保持稳定，也就是说扫尾数初值可以小些，即Sa较小。 我们还发现当积分步数Ea较大时，腿部转动角度也较大，则启动速度要求慢些，系统能够保持稳定，也就是说扫尾数初值可以大些，即Sa较大。 依据以上的机械运动特性，我们推出以下公式： 初始时Sa = Ea ； 过程中1≤Sa≤Ea 伴随着系统运动，积分步数Ea逐渐减小，速度参数Sa逐渐减小，速度逐渐加快； 属于第1族运动方式，即积分停止时加速度反向且值最大。 用户输入：运动方式编码、积分步数（该数值可以合理也可以不合理） 程序出口：所有舵机的位置，系统稳定系数（“1”稳定，“0”不稳定） 左腿抬起第2族子程序 左腿抬起第2族子程序 程序入口：（uchar foot）放入积分运算器初值 Zsag Z平面Sa过渡数寄存器 // 函数原型：void lt_up__zu2(uchar foot) // 函数名称：左腿抬起第2族子程序 // 功 能：左腿抬起速度减小，速度减小有一个过程，有利于机器人的平衡稳定性 // 参 数： foot, 表示积分步数。 // 返 回 值： 无 // 函数编号： 十 // ┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈┈ void lt_up__zu2(uchar foot) { uchar i,j; j=zsag; //z平面扫尾值过渡 for(i=0;i5) { j--; sao_wei(j); //扫尾值自减，右腿抬起速度加快 zsag=j; } else { sao_wei(5); zsag=5; }