[0015] （8）根据调整腿末端点位于机器人所受合力在支撑面上的投影线上或者投影延长线上的原则，计算调整腿末端点HAt的位置坐标，并转入步骤（10）；
[0016] （9）根据髋关节到倾翻轴线的垂足与调整腿末端点的连线，垂直于倾翻轴线的原则，计算调整腿末端点HAt的位置坐标，并转入步骤（10）；
[0017] （10）计算出调整腿末端的位置后经机器人运动学反解得到机器人各关节角度值，然后向执行机构输出各关节角度序列，执行系统动作使机器人到达控制器要求达到的位置。
[0018] 本发明是一种多足机器人平衡控制方法，用于多足机器人在非结构化地形条件下运动时的平衡控制，控制流程简洁明确。本发明的核心是检测到机器人失稳后，快速计算出调整腿末端下一个运动周期的落点位置，执行控制策略后，将使得机器人由失稳状态迅速转换为静态稳定状态。足端落点位置应满足的约束条件和足端落点位置均由解析式给出，计算效率较高，适合于在线实时计算和实时控制。整套控制方法利用机器人各关节角度信息、机身位姿信息和足端脚力信息对机器人各腿的运动进行位置控制，该控制方法具备自主调整腿的位置姿态使机器人迅速恢复至静态稳定的功能，适应于多足机器人在非结构化环境下的平衡控制。
附图说明
[0019] 图1是六足机器人结构示意图和各坐标系定义；
[0020] 图2是四足机器人结构示意图；
[0021] 图3是本发明提出的多足机器人平衡控制方法流程图；
[0022] 图4是构建机器人足端支撑平面的示意图；
[0023] 图5是调整腿末端位置约束示意图；
[0024] 图6是合力分量方向一致原则的落脚点选择示意图；
[0025] 图7是最大迈腿距离原则的落脚点选择示意图。
具体实施方式
[0026] 以下结合实施例及附图对本发明作进一步详细的说明，但本发明实施方式不限于此。该具体实施例选取多足机器人中的六足机器人（如图1所示）作为作用对象，但实施对象不限于此，同样可作用于四足机器人（如图2所示）、八足机器人等其他多足机器人。
[0027] 图1为六足足机器人结构示意图和各坐标系定义。机器人由机身101和六条腿组成，六条腿按逆时针分别编号为1、2、3、4、5、6。每条腿由基节103、大腿105和小腿107组成。机身101和基节103通过髋关节102连接，基节103和大腿105通过膝关节104连接，大腿105和小腿107通过踝关节106连接，小腿107与地面108的接触点为着地点，各条腿的着地点分别标记为Ai，i=1，2，···，6，其中i表示腿的编号。六条腿的结构完全相同。取任意直角坐标系为世界坐标系ΣO，作为机器人位姿的基准坐标系。机身坐标系ΣC的原点与机身的几何中心重合，机身坐标系ΣC的X轴垂直于机器人冠状面，机身坐标系ΣC的Y轴与机器人前进方向重合，Z轴由右手法则确定。第i条腿的坐标系ΣBi的原点位于髋关节轴线上，X轴、Y轴分别与机身坐标系ΣC的X轴、Y轴平行，Z轴由右手法则确定。机器人的基节、大腿、小腿长度分别为l1、l2、l3。机身上安装传感器用于获取机器人机身的位置和姿态，所有腿的旋转关节处安装传感器获取关节角度，腿末端位置安装传感器获取脚力信息。
[0028] 如图3所示，本发明实例提供的一种多足机器人平衡控制方法包括下述步骤：
[0029] （1）用传感器获取当前的机器人各关节角度值和机身位置姿态值来初始化机器人各关节角度、机身位姿参数，并检查机器人通讯系统，当检测到通讯系统不正常时，再次进行初始化，直至通讯正常。
[0030] （2）利用机器人搭载的传感器系统获取的各关节角度、机身位姿信息对机器人进行正运动学计算，可得到机身重心、腿部各部分重心在机器人世界坐标系ΣO中的坐标。
[0031] 多足机器人重心坐标计算公式给出如下：
[0032]
[0033] 其中，n为多足机器人足的数目，t为多足机器人每条腿组成部分的数目。m0、m1、mij分别为机器人搭载的负重块质量、机身质量和机器人第i(i=1,2,…n,)条腿上第j(j…=部分的质量。(X0，Y0，Z0)、(X1，Y1，Z1)、(Xij，Yij，Zij)分别为机器人搭载的负重块重心、机身重心和机器人第i(i=1,2，…,6)条腿上第j(j=1,2,3)部分的重心在机器人世界坐标系ΣO中坐标值，(Xcog、Ycog、Zcog)为机器人重心在机器人世界坐标系ΣO中的坐标。
[0034] （3）利用机器人传感系统获取的各足端脚力信息、机器人各关节角度信息和机身位姿信息，构建出当前运动时刻的支撑平面。
[0035] 如图4所示，上述构建支撑平面的过程为：在机器人运动过程中，首先根据机器人传感系统获得的脚力信息确定脚力最大的三个着地足（假设为足k、足l、足m，其足末端力大小分别为Ffk、Ffl、Ffm），然后由不在同一直线上的三个点确定一个平面的原则，便可以由C着地点的位置信息求出当前运动时刻的支撑平面的法向向量。支撑平面法向向量 VA计算公式为：
[0036] CVA=(xl-k,yl-k,zl-k)×(xm-k,ym-k,zm-k)
[0037] (2)
[0038] =(yl-kzm-k-zl-k,ym-k,zl-kxm-k-xl-kzm-k,xl-kym-k-yl-kxm-k)
[0039] 其中xl-k=xl-xk,yl-k=yl-yk,zl-k=zl-zk；xm-k=xm-xk,ym-k=ym-yk,zm-k=zm-zk，CPAK=(xk,yk,zC Ck),PAL=(xl,yl,zl),PAM=(xm,ym,zm)为脚力最大的三个着地足末端点在机身坐标系ΣC中的位置坐标。
[0040] （4）使用零力矩点（ZMP，Zero Moment Point）方法判断机器人的稳定性，根据零力矩点的位置可判定机器人是否失稳并求解稳定裕度dm。此处的稳定裕度用零力矩点到支撑多边形的各边距离的最小值来表示，预设机器人稳定阈值S为5%-10%L，L为机器人机身长度。
[0041] （5）根据机器人当前的稳定性情况采取相应的控制策略：若机器人稳定，则实施预设的正常行走步态，并转入步骤（2）；若机器人失稳，则立即采取失稳后平衡恢复控制策略。
[0042] （6）根据六足机器人失稳的特点，求解调整腿末端位置的约束条件。
[0043] （7）取步骤（4）中计算的稳定裕度dm与预设的稳定裕度阈值S进行比较：若稳定裕度为负值且|dm|
[0044] （8）根据调整腿末端点位于机器人所受合力在支撑面上的投影线上或者投影延长线上的原则（即合力分量方向一致原则），计算调整腿末端点HAt的位置坐标，并转入步骤（10）。
[0045] （9）根据髋关节到倾翻轴线的垂足与调整腿末端点连线，垂直于倾翻轴线的原则（即最大迈腿距离原则），计算调整腿末端点HAt的位置坐标，并转入步骤（10）。
[0046] （10）计算出调整腿末端的位置后经机器人运动学反解得到机器人各关节角度值。然后向执行机构输出各关节角度序列，执行系统动作使机器人到达控制器要求达到的位置。通过上述操作，使机器人由失稳状态转化为平衡状态。
[0047] 步骤（6）中所述的失稳后平衡恢复控制策略的具体方法如下：
[0048] 图5是调整腿（设该腿的编号为w）末端位置解析约束示意图。多足机器人在其运动过程中，最常见的失稳形态是机身绕着由两个支撑脚的连线发生的倾翻。在机器人发生失稳后，可以假定调整腿在着地前将根据膝关节和踝关节的关节转角范围到达其中的最大伸长距离，如图5中的虚线KwAt1所示。在随后的翻转过程中，假定除调整腿外，机器人机身的其他部分没有发生相对位置的改变，则在随后的倾翻过程中，调整腿的脚点将以倾翻边线（设为足m和足k末端点的连线）为轴线进行旋转直至达到支撑平面，即图5中调整腿各关节运动至最大位置时的末端点At1绕调整腿髋关节旋转至At2。由此过程可以得到调整腿末端点At2点所满足的三个约束条件：（1）At2点到倾翻轴线AmAk的距离等于At1点到倾翻轴线AmAk的距离；（2）At1点与At2点的连线与倾翻轴线垂直；（3）At2点位于支撑平面上。上述约束条件不能求得解析解，为能求得解析解，作如下几何处理，经At2做一垂线交倾翻轴线AmAk于点SW，再过点At2做一垂线交SwAt2于点HAt，用如图5中所示的HAt点来取代At2点作为落脚点，则HAt点将满足以下约束：（1）At1点到HAt点连线与支撑平面的法向量同向；（2）HAt点位于支撑平面上。由此可求得HAt点位置约束为，调整腿末端点HAt需位于如图6和图7中所示的绕调整腿髋关节旋转的圆弧上。HAt点位置的解析表达式如下：
[0049]
[0050] 其中，(xHAt,yHAt,zHAt)为点HAt在机身坐标系下的坐标，(xAt1,yAt1,zAt1)为点At1在机身坐标系下的坐标，(xAm,yAm,zAm)为翻转轴线AmAk起点Am在机身坐标系下的坐标，CVA=(xVA,yVA,zVA)为支撑平面的法向向量。
[0051] 由上式可得出，HAt点的约束为如图5中绕髋关节旋转的一段圆弧，HAt点只与调整腿的各关节转角有关，且在髋关节角度确定后，便可求解调整腿落脚点在机身坐标系中坐标。
[0052] 步骤（8）中所述的合力分量方向一致原则的示意图如图6所示。其实现原理如下：C
在发生边线失稳时，机器人所受合力可分解为垂直于支撑平面的分量 Fp和支撑面上的分C C C
量 Fh，分量 Fp将可由所有着地腿均分，但分量 Fh则有可能由于与倾翻轴线不是垂直关系而造成其中的某些着地脚受到比较大的力，为避免此时由于与地面过大的接触力造成着地腿的损伤，可通过调整腿来分担其中的一部分，此发明提出调整腿的着地点与合力在支撑面的分量同向的原则。
C C
[0053] 合力 F=(xF,yF,zF)在支撑平面内的投影 Fh计算公式如下：C C C C C
[0054] Fh=(xFh,yFh,zFh)F-(F·VA)·VA=(xF-μxVA,yF-μyVA,zF-μzVA) （4） 其 中，C(xFh,yFh,zFh)为 Fh在机身坐标系下的向量表示，μ=xFxVA+yFyVA+zFzVA。
[0055] 为保证落脚点位于合力作用线上则有从质心到HAt的连线矢量在支撑面内的投影C CPHAth与 Fh同向,于是得：
C C C C C
[0056] PHAth=PHAt-(PHAt·VA)·VA=(xHAt-VxVA,yHAty-vyVA,zHAtz-vzVA) （5）C
[0057] 其中 PHAt=(xHAt,yHAt,zHAt)为点HAt在机身坐标系下的坐标，C
[0058] v=xHAtxVA+yHAtyVA+zHAtzVA，VA=(xVA,yVA,zVA)为支撑平面的法向向量。
[0059] 由共线约束可得合力分量方向一致原则下的落脚点HAt位置的计算式：
[0060]
[0061] 由式（6）和步骤（6）中的式（3）最终可得落足点的HAt在机身坐标系中的坐标值。
[0062] 步骤（9）中所述的最大迈腿距离原则的平衡恢复策略，此时最大程度地增大着地腿末端与倾翻线的垂直距离。如图7所示，假设机器人沿为编号为m和编号为k腿末端连线AmAk方面翻转，此时通过调整编号为w的腿末端位置使得机器人恢复至静态平衡。调整C腿的末端点HAt点与髋关节的连线向量 VBtHAt与倾翻边线向量AmAk垂直，设定(xHA,yHA,zHA)为落脚点HAt在机身坐标系中的位置坐标，于是，调整腿的落脚点HAt的位置可通过如下公式给出：
[0063] (xHA-xB)xm-k+(yHA-yB)ym-k+(zHA-zB)zm-k=0 （7）
[0064] 其中，xm-k=xm-xk,ym-k=ym-yk,zm-k=zm-zk，(xm,ym,zm)是编号为m的腿在机身坐标系中的坐标，(xB,yB,zB)是编号为w的腿髋关节点Bw在机身坐标系中的坐标。
[0065] 由式（7）和步骤（6）中的式（3）最终可得落足点的HAt在机身坐标系中的坐标值。
[0066] 上述实施例主要以六足机器人作为实施对象进行描述，但本发明提出的平衡控制方法同样适用于四足和八足等其它多足机器人。
[0067] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
