专利名称：一种多足机器人平衡控制方法
技术领域：
本发明涉及机器人控制领域，具体是一种多足机器人平衡控制方法，对多足机器人在复杂外界环境下的运动失稳进行平衡恢复控制。
背景技术：
多足机器人是一种具有冗余驱动、多支链的机器人，具有运动灵活、负载能力强、稳定性好、可实现多种步态等特点，具备一定的越障、爬陡坡和环境适应能力，在科学探索、山地运输、农林业开发、地质勘探、抢险救灾乃至军事行动等领域有着良好的应用前景，受到全世界范围内机器人研究人员的广泛关注与重视。多足机器人按足的数目一般可分为四足、六足、八足和八足以上的机器人。多足机器人承受负载时的自主运动能力为其最核心的要素，多足机器人在外界非结构化地形条件下(包括凸凹地面、低摩擦地面、碎石地面、松软地面等)运动时的平衡稳定控制方法是当前机器人研究面临的难点和挑战。随着机器人学科的发展，多足机器人在稳定性判断方面已较成熟。目前对多足机器人的稳定性判断大多采用零力矩点(ZMP，Zero Moment Point)方法。ZMP方法的核心是判断机器人质心沿合外力方向的投影是否在机器人支撑腿所形成的凸多边形之内，如果投影在支撑腿所形成的凸多边形之内，则机器人稳定，否则机器人失稳。多足机器人失稳的情形主要有两种，即静态失稳和动态失稳。静态失稳是指在不考虑机身加速度和所受外力的情况下，机器人质心在支撑面的投影位于支撑腿所构成的凸多边形之外；动态失稳是指考虑到外力冲击、惯性扰动、系统加速度等因素，利用达朗伯原理将上述外力及惯性力等效至系统质心处，机器人质心沿系统合力方向在支撑面的投影位于支撑腿所构成的凸多边形之外。搜索相关文献，对多足机器人平衡方面的研究主要集中于如何判定机器人稳定以及在保证机器人稳定的前提下对机器人进行步态规划，而少见多足机器人在失稳后该采取的控制策略。本发明利用多足机器人位姿状态信息和足端脚力信息对机器人进行稳定性判别，针对多足机器人失稳的特点，提出一种平衡控制方法和失稳后应该采取的控制策略。
发明内容
本发明的目的在于提供一种多足机器人平衡控制方法，该方法可以对失稳后的多足机器人进行平衡恢复，具有计算量少、可靠性高、适合在线实时计算等特点。本发明提供的一种多足机器人平衡控制方法，其特征在于，该方法包括下述步骤(I)获取当前的机器人各关节角度值和机身位置姿态值，以初始化机器人各关节角度、机身位姿参数；(2)实时获取各关节角度、机身位姿信息，并利用这些信息对机器人进行正运动学计算，得到机身重心、腿部各部分重心在机器人世界坐标系中的坐标；、
(3)利用获取的各足端脚力信息、机器人各关节角度信息和机身位姿信息，构建出当前运动时刻的支撑平面；(4)使用零力矩点方法判断机器人的稳定性，根据零力矩点的位置判定机器人是否失稳并计算稳定裕度Clm ；(5)根据机器人当前的稳定性情况采取相应控制策略若机器人稳定，则实施预设的正常行走步态，并转入步骤(2);若机器人失稳，进入步骤(6)；(6)计算调整腿末端点HAt的位置约束条件；(7)取步骤⑷中计算的稳定裕度Clm与预设的稳定裕度阈值S进行比较若稳定裕度为负值且Id1J < S，则判定为机器人失稳程度较小，并进入步骤(8);若稳定裕度为负值且Id1J彡S，则判定为机器人失稳程度较大，执行步骤(9)；
(8)根据调整腿末端点位于机器人所受合力在支撑面上的投影线上或者投影延长线上的原则，计算调整腿末端点HAt的位置坐标，并转入步骤(10)；(9)根据髋关节到倾翻轴线的垂足与调整腿末端点的连线，垂直于倾翻轴线的原贝U，计算调整腿末端点HAt的位置坐标，并转入步骤(10)；(10)计算出调整腿末端的位置后经机器人运动学反解得到机器人各关节角度值，然后向执行机构输出各关节角度序列，执行系统动作使机器人到达控制器要求达到的位置。本发明是一种多足机器人平衡控制方法，用于多足机器人在非结构化地形条件下运动时的平衡控制，控制流程简洁明确。本发明的核心是检测到机器人失稳后，快速计算出调整腿末端下一个运动周期的落点位置，执行控制策略后，将使得机器人由失稳状态迅速转换为静态稳定状态。足端落点位置应满足的约束条件和足端落点位置均由解析式给出，计算效率较高，适合于在线实时计算和实时控制。整套控制方法利用机器人各关节角度信息、机身位姿信息和足端脚力信息对机器人各腿的运动进行位置控制，该控制方法具备自主调整腿的位置姿态使机器人迅速恢复至静态稳定的功能，适应于多足机器人在非结构化环境下的平衡控制。
图I是六足机器人结构示意图和各坐标系定义；图2是四足机器人结构示意图；图3是本发明提出的多足机器人平衡控制方法流程图；图4是构建机器人足端支撑平面的示意图；图5是调整腿末端位置约束示意图；图6是合力分量方向一致原则的落脚点选择示意图；图7是最大迈腿距离原则的落脚点选择示意图。
具体实施例方式以下结合实施例及附图对本发明作进一步详细的说明，但本发明实施方式不限于此。该具体实施例选取多足机器人中的六足机器人(如图I所示)作为作用对象，但实施对象不限于此，同样可作用于四足机器人(如图2所示)、八足机器人等其他多足机器人。
图I为六足足机器人结构示意图和各坐标系定义。机器人由机身101和六条腿组成，六条腿按逆时针分别编号为1、2、3、4、5、6。每条腿由基节103、大腿105和小腿107组成。机身101和基节103通过髋关节102连接，基节103和大腿105通过膝关节104连接，大腿105和小腿107通过踝关节106连接，小腿107与地面108的接触点为着地点，各条腿的着地点分别标记为Ai, 1 = 1，2，. ..，6，其中1表示腿的编号。六条腿的结构完全相同。取任意直角坐标系为世界坐标系E 0，作为机器人位姿的基准坐标系。机身坐标系E C的原点与机身的几何中心重合，机身坐标系E C的X轴垂直于机器人冠状面，机身坐标系E C的Y轴与机器人前进方向重合，Z轴由右手法则确定。第i条腿的坐标系E Bi的原点位于髋关节轴线上，X轴、Y轴分别与机身坐标系E C的X轴、Y轴平行，Z轴由右手法则确定。机器人的基节、大腿、小腿长度分别为li、l2、l3。机身上安装传感器用于获取机器人机身的位置和姿态，所有腿的旋转关节处安装传感器获取关节角度，腿末端位置安装传感器获取脚力信息。如图3所示，本发明实例提供的一种多足机器人平衡控制方法包括下述步骤(I)用传感器获取当前的机器人各关节角度值和机身位置姿态值来初始化机器人各关节角度、机身位姿参数，并检查机器人通讯系统，当检测到通讯系统不正常时，再次进行初始化，直至通讯正常。(2)利用机器人搭载的传感器系统获取的各关节角度、机身位姿信息对机器人进行正运动学计算，可得到机身重心、腿部各部分重心在机器人世界坐标系E 0中的坐标。多足机器人重心坐标计算公式给出如下
权利要求
1.一种多足机器人平衡控制方法，其特征在于，该方法包括下述步骤 (1)获取当前的机器人各关节角度值和机身位置姿态值，以初始化机器人各关节角度、机身位姿参数； (2)实时获取各关节角度、机身位姿信息，并利用这些信息对机器人进行正运动学计算，得到机身重心、腿部各部分重心在机器人世界坐标系中的坐标； (3)利用获取的各足端脚力信息、机器人各关节角度信息和机身位姿信息，构建出当前运动时刻的支撑平面； (4)使用零力矩点方法判断机器人的稳定性，根据零力矩点的位置判定机器人是否失稳并计算稳定裕度dm ； (5)根据机器人当前的稳定性情况采取相应控制策略若机器人稳定，则实施预设的正常行走步态，并转入步骤(2);若机器人失稳，进入步骤(6)； (6)计算调整腿末端点HAt的位置约束条件； (7)取步骤(4)中计算的稳定裕度dm与预设的稳定裕度阈值S进行比较若稳定裕度为负值且Id1J < S，则判定为机器人失稳程度较小，并进入步骤(8);若稳定裕度为负值且dj彡S，则判定为机器人失稳程度较大，执行步骤(9)； (8)根据调整腿末端点位于机器人所受合力在支撑面上的投影线上或者投影延长线上的原则，计算调整腿末端点HAt的位置坐标，并转入步骤(10)； (9)根据髋关节到倾翻轴线的垂足与调整腿末端点的连线，垂直于倾翻轴线的原则，计算调整腿末端点HAt的位置坐标，并转入步骤(10)； (10)计算出调整腿末端的位置后经机器人运动学反解得到机器人各关节角度值，然后向执行机构输出各关节角度序列，执行系统动作使机器人到达控制器要求达到的位置。
2.根据权利要求I所述的多足机器人平衡控制方法，其特征在于，步骤(7)中，稳定裕度用零力矩点到支撑多边形的各边距离的最小值来表示
3.根据权利要求I所述的多足机器人平衡控制方法，其特征在于，步骤(3)中，构建支撑平面的过程为在机器人运动过程中，首先根据机器人传感系统获得的脚力信息确定脚力最大的三个着地足，假设为足k、足I、足m，由着地点的位置信息求出当前运动时刻的支撑平面的法向向量，所述支撑平面的法向向量eVA计算公式为Va — (xi-k，Yi-k，zi-k) X (xm_k，ym_k，zm_k)式 I —(yi-kZm-k_Zl-kym-kJ Zl-kXm-k_Xl-kZm-kJ Xl-kym-k_yl-kXm-k) 其中 Xl-k — Xl_Xk，Yl-k — Yl-Ykj Zl-k — Zl_Zk ;Xm_k — Xm_Xk，Ym-k — Ym-Ykj Zm_k — Zm_Zk Ak=(xk, yk, Zk), cPai = (X1, y1； Z1), cPAm = (xm, ym, zm)为脚力最大的三个着地足 k、I、m 末端点在机身坐标系E C中的位置坐标。
4.根据权利要求I所述的多足机器人平衡控制方法，其特征在于，步骤￠)中，按照下述方式计算调整腿末端点HAt的位置约束条件； 调整腿末端点HAt需位于绕调整腿髋关节旋转的圆弧上，HAt点位置的解析表达式如下 f XHAt = xAti — XVA (XVA iXAtl — XAm ).3 (X*l — VAm ) + ZVA (ZAtl ~ 2Am ) I yHM = }!An-^4in) +yn(yArl -式 II IzHAl = 2M - 2VA (XVA (XA,l _ XAn,) + ^— Xto ) + iZAtl — ZAm )其中，(xMt，YffiU，Zmt)为点HAt在机身坐标系E C下的坐标，(xAtl,ymtl,zmtl)为调整腿各关节运动至最大位置时的末端点Atl在机身坐标系下的坐标，(Xaiii, yAffl, zj为翻转轴线AmAk端点Am在机身坐标系下的坐标，cVa = (xVA, yVA, zVA)为支撑平面的法向向量。
5.根据权利要求I所述的多足机器人平衡控制方法，其特征在于，步骤(8)中，利用下式III与式II计算HAt的位置
6.根据权利要求I所述的多足机器人平衡控制方法，其特征在于，步骤(8)中，利用下式IV与式II计算HAt位置(XHA_XB) xm-k+ (Yha-Yb) Ym-k+ (ZHA_ZB) zm-k — O式 IV 其中，(xm，ym，zHA)为落脚点HAt在机身坐标系中的位置坐标,xm_k = Xm-Xk, ym_k = ym_yk，zm-k = Zm-Xk, (xm, ym, zm)是编号为m的腿在机身坐标系中的坐标，(xB, yB, zB)是编号为w的腿髋关节点Bw在机身坐标系中的坐标。
全文摘要
本发明公开一种多足机器人平衡控制方法，用于多足机器人在非结构化地形条件下运动时的平衡控制，控制流程简洁明确。本发明是检测到机器人失稳后，快速计算出调整腿末端下一个运动周期的落点位置，执行控制策略后，将使得机器人由失稳状态迅速转换为静态稳定状态。足端落点位置应满足的约束条件和足端落点位置均由解析式给出，计算效率较高，适合于在线实时计算和实时控制。整套控制方法利用机器人各关节角度信息、机身位姿信息和足端脚力信息对机器人各腿的运动进行位置控制，该控制方法适应于多足机器人在非结构化环境下的平衡控制。
文档编号G05D1/02GK102749919SQ201210198798
公开日2012年10月24日 申请日期2012年6月15日 优先权日2012年6月15日
发明者严可涵, 刘志江, 李栓柱, 罗欣, 钟建锋, 陈学东, 陈思宇, 韩斌 申请人:华中科技大学