CN110842921A - 四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法 - Google Patents

Abstract

一种四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法，进行身体姿态调整，使得机器人在大坡度地形上也能稳定的运动；根据归一化能量稳定裕度计算方法计算能量稳定裕度最大处质心位置，再根据足底力传感器的反馈进行质心的调整；设定四足机器人的迈腿顺序为左前腿‑右前腿‑右后腿‑左后腿，以增加机器人的稳定性和越障能力。本发明采用先迈两条前腿再迈两条后腿的策略，增加机器人纵向跨距，极大地提高机器人的稳定性和运动过程中鲁棒性，在保证运动连续的前提下，四足机器人斜坡上的自适应调整，使得四足机器人能够稳定地通过复杂地形。

Description

四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法

技术领域

本发明涉及一种用于四足机器人在大坡度地形或高障碍物上进行攀爬跨越的步态规划方法，属于腿足式机器人步态规划的控制技术领域。

背景技术

腿足式机器人是移动机器人领域的一个重要分支。目前最为常见的机器人移动方式为轮式或履带式，这类移动机器人能够较好地适应平坦的路面，且近年来其在越野性能上也取得了相当不错的成就。然而，轮式或履带式机器人在崎岖复杂路面的行进依旧有很大的困难和局限性。与之相比，自然界中的腿足式动物能够几乎可以到达地面上的任何位置，例如，羚羊可以在悬崖峭壁上行走，松鼠能够自如地在树枝上奔跑，猎豹具有较快的奔跑速度，马、骡等在驮运较重的货物时依然可以自如行走。腿足式动物的这些特质使得研究人员越来越重视腿足式机器人的研制，腿足式机器人在灵活性与地形适应性上比传统的轮式或履带式机器人有更大的潜力。

腿足式机器人按照腿的数量分类，主要包括单足机器人、双足机器人、四足机器人、六足机器人或更多足的机器人。其中四足机器人相比六足或更多足机器人，在结构上更加简单；与单足或双足机器人相比，四足机器人在稳定性与负重能力上更有优势。同时，从仿生角度讲，哺乳动物作为进化最高等的脊椎动物，绝大部分均采用四足行走形式。

中国专利文献CN107065908A公开的《一种规划四足机器人静步态机身运动轨迹的方法》，包括规划静步态行走时机身和足端的摆动顺序及步态时序，计算三条腿支撑地面时四足机器人的稳定区域和同侧腿连续迈步时对应的稳定区域，并且求取机身运动的目标位置，拟合机身运动轨迹曲线。上述方法在普通的斜面或者不规则地形下运行良好，但是在面对大坡度地形或者高障碍物时，仍显得运动能力不足，主要原因是在大坡度地形下机器人的稳定性大大降低，质心的移动需要特别地进行规划才能保持稳定，这一点可以通过引入足底力反馈来提高机器人在质心调整过程中的稳定性。

中国专利文献CN107065867A公开的《一种面向未知崎岖地形的四足机器人运动规划方法》中提到的爬行步态与前面提到的CN107065908A中所用爬行步态相同，采用的摆腿顺序都是：左前腿->右后腿->右前腿->左右腿。该方法在面对大坡度地形时没有充分利用四足机器人的运动空间，导致机器人总体稳定裕度较小，稳定性低，而这点可以通过定义其迈步顺序提高。

发明内容

本发明针对现有的四足机器人运动规划方法存在的不足，提出一种面向大坡度地形和高障碍物地形的四足机器人运动规划方法，确定四足机器人的腿部拓扑结构，在保证运动连续的前提下，四足机器人进行斜坡上的自适应调整，使得四足机器人能够稳定地通过复杂地形。

为了实现上述目的，本发明的步态规划方法，包括以下步骤：

1.一种四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法，其特征在于：四足机器人在面向大坡度地形和高障碍物地形时采用爬行步态，爬行步态四条腿的摆腿顺序为：左前腿-右前腿-右后腿-左后腿；

将四足机器人爬行步态的运动周期按照上述的摆腿顺序划分为八个状态相：LF摆动相、四足支撑相、RF摆动相、四足支撑相、RH摆动相、四足支撑相、LH摆动相和四足支撑相；在单腿摆动相，摆动腿沿着规划的足端轨迹进行摆动，机身静止不动；在四足支撑相，机器人的重心在沿着规划的路径进行前向运动的同时进行侧向移动；

设计状态机，在单腿摆动相，摆动腿落地触发下一状态相；在四足支撑相，以当前所规划的四足支撑相的时间触发下一状态相；状态相一旦发生变化，机器人通过摆动腿的落地规划和感知策略估计未知地形的参数，并结合当前自身运动状态对新状态相进行在线的运动规划；在当前状态相的整个过程中，机器人一直执行当前状态相初始时刻运动规划所生成的轨迹，直到当前状态相结束；机器人按照前述的状态机依次进入下一个状态相，直至完成一个完整的运动周期，然后再进入下一运动周期；具体过程如下：

(1)上一状态相结束，机器人进入新状态相；更新支撑域，根据当前支撑域，基于COG 稳定性判据，进行身体重心的路径规划；

(2)根据步骤(1)所规划的路径，基于NESM稳定性判据，进行身体重心的轨迹规划；

(3)根据上述的方法，引入足底力反馈，通过将力反馈得到的质心位置与运动学算出的位置进行加权平均，得到最终的质心位置；

(4)规划静步态行走时机身和足端的摆动顺序及步态时序；

(5)拟合足端摆动轨迹。

在单腿摆动过程中，为避免腿部与障碍物发生碰撞，在进行足端轨迹规划时，采用三段式控制，三段时间互有重叠，设定参数α为调节重叠时间的比例，比例越大，轨迹曲线越光滑；

阶段一：z轴方向上采用三次曲线规划上升，且x轴保持不动，保证了足端直线向上地运动；

阶段二：x轴进行三次曲线规划前行，z轴保持不动；

阶段三：z轴进行二次曲线规划，直至与地面发生接触，x轴保持不动。

在四足支撑阶段，构造伪俯仰角，调整各个腿长，使得伪俯仰角为0，同时使得横滚角为0，伪俯仰角的计算公式如下：

ψ＝arctan(((l_lf+l_rf)/2-(l_lh+l_rh)/2)/S)

根据上面取得的伪俯仰角的大小的反馈，来改变条腿的长度，并且考虑横滚角的大小，即得如下腿长更新公式：

l_lf＝l_lf+K_αψ+K_βφ

l_rf＝l_rf+K_αψ-K_βφ

l_lh＝l_lh-K_αψ+K_βφ

l_rh＝l_rh+K_αψ-K_βφ

其中，l_lf为左前腿在z轴方向的坐标，l_rf为右前腿在z轴方向的坐标，l_lh为左后腿在z 轴方向的坐标，l_rh为右后腿在z轴方向的坐标，ψ为伪俯仰角大小，φ为IMU反馈得到的机身横滚角的大小，S为机身纵向跨距，K_α为伪俯仰角的反馈增益值，K_β为横滚角的反馈增益值。

所述步骤(1)进行身体重心的轨迹规划是求取机身重心在支撑平面上的投影点，当所述投影点位于支撑三角形内部，根据机身与足端的受力力矩平衡分析，计算投影点到支撑三角形边缘的最小稳定裕度，得到稳定区域。

本发明采用先迈两条前腿再迈两条后腿的策略，增加机器人纵向跨距，极大地提高机器人的稳定性和运动过程中鲁棒性，在保证运动连续的前提下，四足机器人斜坡上的自适应调整，使得四足机器人能够稳定地通过复杂地形。

附图说明

图1是四足机器人的各坐标系间的关系示意图。

图2是机器人在支撑面上的姿态控制示意图。

图3是计算支撑域三边的NESM大小的示意图.

图4是调整质心COG位置使得S_min最小的示意图。

图5是采用力反馈来估计机器人的位置的示意图。

图6是定义四条腿的编号示意图。

图7是机器人单腿运动学建模。

图8是机器人步态时序图。

图9是一个完整的运动周期示意图。

图10是身体质心运动规划曲线图。

图11是摆动腿轨迹规划示意图。其中(a)为阶段一，(b)为阶段二，(c)为阶段三。

图12是四足机器人攀爬60度坡webots仿真图。

具体实施方式

本发明的面向大坡度地形和高障碍物地形的四足机器人运动规划方法，在保证运动连续的前提下，使得四足机器人能够稳定地通过复杂地形。

本发明中四足机器人的腿部构造为全肘式，四足机器人采用全肘式拓扑结构，防止机器人腿部与地面发生碰撞；进行身体姿态调整，使得机器人在大坡度地形上也能稳定的运动；根据归一化能量稳定裕度计算方法(NESM，normalized energy stabilitymargin)计算能量稳定裕度最大处质心位置，再根据足底力传感器的反馈进行质心的调整；设定四足机器人的迈腿顺序为左前腿-右前腿-右后腿-左后腿，以增加机器人的稳定性和越障能力。

四足机器人在面向大坡度地形和高障碍物地形时采用爬行步态，爬行步态四条腿的摆腿顺序为：左前腿(LF，left-front)-右前腿(RF，right-front)-右后腿(RH，right-hind)-左后腿(LH，left-hind)；

将四足机器人爬行步态的运动周期按照上述的摆腿顺序划分为八个状态相：LF摆动相、四足支撑相、RF摆动相、四足支撑相、RH摆动相、四足支撑相、LH摆动相和四足支撑相；在单腿摆动相，摆动腿沿着规划的足端轨迹进行摆动，机身静止不动；在四足支撑相，机器人的重心在沿着规划的路径进行前向运动的同时进行侧向移动；

设计状态机，在单腿摆动相，摆动腿落地触发下一状态相；在四足支撑相，以当前所规划的四足支撑相的时间触发下一状态相；状态相一旦发生变化，机器人通过摆动腿的落地规划和感知策略估计未知地形的参数，并结合当前自身运动状态对新状态相进行在线的运动规划；在当前状态相的整个过程中，机器人一直执行当前状态相初始时刻运动规划所生成的轨迹，直到当前状态相结束；机器人按照前述的状态机依次进入下一个状态相，直至完成一个完整的运动周期，然后再进入下一运动周期；具体过程如下：

(1)上一状态相结束，机器人进入新状态相；更新支撑域，根据当前支撑域，基于COG 稳定性判据，进行身体重心的路径规划；

(2)根据步骤(1)所规划的路径，基于NESM(归一化能量稳定裕度计算方法，normalized energy stability margin)稳定性判据，进行身体重心的轨迹规划；

(3)根据上述的方法，引入足底力反馈，通过将力反馈得到的质心位置与运动学算出的位置进行加权平均，得到最终的质心位置；

(4)规划静步态行走时机身和足端的摆动顺序及步态时序；

(5)拟合足端摆动轨迹。

四足机器人斜坡上的自适应调整。当机器人在斜坡上行走时，质心沿重力方向的投影点在四足支撑面内的位置会向斜面负梯度方向偏移，影响机器人的稳定性。此时，在四足机器人的步态规划中，各个腿关节的角度值除了与规划的足端轨迹有关外，还应与所处地形的坡度有关。

以下详细说明本发明的步态规划方法。

(1)机器人在斜坡上的自适应调整

为简化机器人步态规划的方法，将基本的足端轨迹规划与斜坡上的步态调整和身体姿态调整分离，并依据姿态传感器信息对足端位置进行自动调整。机器人在斜坡上的自适应调整，包括2个部分，足部支撑面的调整和躯干姿态的调整。

由图1所示各坐标系间的关系可得：

其中∑_B为机体坐标系，∑_L为水平坐标系，∑_P为轨迹规划坐标系，∑_S为支撑面坐标系，^ShipP_toe为∑_S中足尖相对于髋关节的位置，^PhipP_toe为∑_P中足尖相对于髋关节的位置，^SP_hip和^PP_hip为∑_S和∑_P中髋关节相对于原点的位置。调整θ_gnd和ψ_gnd可使足端位置产生偏移，实现对斜面坡度的适应。

^BhipP_toe为在机体坐标系∑_B中，足尖相对于髋关节的位置。调整θ_ref、φ_ref和

可控制机器人在支撑面上的姿态，如图2所示。

将躯干姿态调整与斜面坡度适应相结合，并根据姿态反馈设置坡度适应和姿态调整的参数，参见图7给出的机器人单腿运动学建模，化简上式为：

^BhipP_toe＝R_x(-ψ_IMU)R_Y(-θ_IMU)^PP_S+^ShipP_toe。

(2)四足机器人在面向大坡度地形和高障碍物地形时采用爬行步态

作为一种静步态，爬行步态具有更加强大的地形适应能力，可最大化提高机器人在复杂地形的通行能力，设计爬行步态四条腿的摆动顺序为：左前腿(LF，left-front)->右前腿(RF， right-front)->右后腿(RH，right-hind)->左后腿(LH，left-hind)，每个时刻最多只有一条腿在摆动，也就是说每个时刻至少有三条腿在支撑。参见图8给出的步态时序图。

(3)将四足机器人爬行步态的运动周期按照上述摆动腿顺序划分为八个状态相：左前腿摆动相、短暂四足支撑相、右前腿摆动相、四足支撑相、右后腿摆动相、短暂四足支撑相、左后腿摆动相以及短暂四足支撑相。在单腿摆动相，机器人指定的一条腿处于摆动相状态，其余的三条腿处于支撑状态，摆动腿沿着规划的足端轨迹进行摆动，此时机器人的质心位置保持不变；在四足支撑相，机器人的四条腿均处于支撑状态，机器人的重心在沿着规划的路径进行前向运动的同时进行侧向调整。

(4)状态机设计

在四足支撑相，以当前所规划的四足支撑相的时间触发下一状态相，状态相一旦发生变化，机器人将会通过摆动腿的落地规划和感知策略估计未知地形的参数，并结合当前自身运动状态对新状态相进行在线的运动规划。在单腿摆动相，摆动腿落地出发下一状态相。在当前状态相的整个过程中，机器人一直执行当前的状态相初始时刻运动规划所生成的轨迹，直到当前状态相结束，机器人按照前述的状态机依次进入下一个状态相，直至完成一个完整的运动周期(参见图9给出的一个完整的运动周期)，然后再进入下一个运动周期。

具体步骤如下所述。

步骤一：选定摆动腿，根据摆动腿更新当前支撑三角形区域。

步骤二：参见图3和图4，分别计算支撑域三边的NESM大小，调整质心COG位置使得S_min最小。

S_NESM＝h_max-h_com

S_min＝min{S_m1，S_m2，S_m3}。

其中S_NESM为支撑多边形某一边的标准能量稳定裕度，h_max为机器人绕支撑轴旋转时质心能够到达的高度，h_com为机器人质心高度。S_m1、S_m2和S_m3分别为机器人支撑三边形的三条边对应的S_NESM值，而S_min则是选择其中的最小值。

步骤三：引入足底力反馈，根据足底力在机器人腿摆动过程中微调机器人的质心位置，不能忽略腿部的质量，因此在计算质心位置时需要考虑腿部的质量，在实际机器人模型中，由于机身质量分布不均匀，因此采用力反馈来估计机器人的位置，如图5所示。以3号腿为摆动腿为例，由足底力传感器反馈f_feedback获得同向(对于支撑多边形必定有同向的两只腿触地，即前向或后向，由摆动腿决定)两条腿足底的三维力的大小⁴F_foot1和⁴F_foot2，通过关节角度反馈θ_feedback获得从足端到机器人质心位置的旋转矩阵，再通过雅克比矩阵将其转化为机器人质心坐标系下的力⁰F_support1与⁰F_support1，将两者的差值乘上一个比例系数Kp得到增益K, 最后调整机器人质心的位置，使得S_NESM1＝S_NESM2*K。

步骤四：确定机器人的迈腿顺序，首先定义四条腿的编号如图6所示：根据以上分析，机器人的迈腿顺序为1234，即左前腿(LF，left-front)->右前腿(RF，right-front)->右后腿 (RH，right-hind)->左右腿(LH，left-hind)。然后将四足机器人爬行步态的运动周期按照上述的摆腿顺序划分为八个状态相：LF摆动相、四足支撑相、RF摆动相、四足支撑相、RH摆动相、四足支撑相、LH摆动相以及四足支撑相。

由于地形坡度较大，导致机器人在机身纵向的跨距在水平面上的投影大大减小，因此本发明采取先迈两只前腿的方式，增加机器人纵向跨距。这种先迈两条前腿再迈两条后腿的策略，极大地提高机器人的稳定性和运动过程中鲁棒性。

在单腿摆动过程中，为避免腿部与障碍物发生碰撞，在进行足端轨迹规划时，参见图11 给出的摆动腿轨迹规划，采用三段式控制，三段时间互有重叠，设定参数α为调节重叠时间的比例，比例越大，轨迹曲线越光滑；

阶段一：z轴方向上采用三次曲线规划上升，且x轴保持不动，保证了足端直线向上地运动；参见图11(a)。

阶段二：x轴进行三次曲线规划前行，z轴保持不动；参见图11(b)。

阶段三：z轴进行二次曲线规划，直至与地面发生接触，x轴保持不动。参见图11(c)。

步骤五：在四足支撑阶段，构造伪俯仰角，调整各个腿长，使得伪俯仰角为0，同时使得横滚角为0。伪俯仰角的计算公式如下：

ψ＝arctan(((l_lf+l_rf)/2-(l_lh+l_rh)/2)/S)。

调整方法：根据上面取得的伪俯仰角的大小的反馈，来改变条腿的长度，并且考虑横滚角的大小，即得如下腿长更新公式：

l_lf＝l_lf+K_αψ+K_βφ，

l_rf＝l_rf+K_αψ-K_βφ，

l_lh＝l_lh-K_αψ+K_βφ，

l_rh＝l_rh+K_αψ-K_βφ，

其中，l_lf为左前腿在z轴方向的坐标，l_rf为右前腿在z轴方向的坐标，l_lh为左后腿在z 轴方向的坐标，l_rh为右后腿在z轴方向的坐标，ψ为伪俯仰角大小，φ为IMU反馈得到的机身横滚角的大小，S为机身纵向跨距，K_α为伪俯仰角的反馈增益值，K_β为横滚角的反馈增益值。

步骤六：在机身处于四足支撑相且短调整状态时，需要调整重心位置，使得机器人在抬起下一摆动腿后，其重心仍位于支撑域内；在长调整状态时，需要将机器人的质心沿着x轴移动一个步长的距离，此时需进行三次曲线规划，使得运动前后机身速度都为0，且位置连续，y轴方向进行微调。参见图10给出的身体质心运动规划曲线。

图12给出了四足机器人攀爬60度坡的webots仿真图。

Claims (4)

1.一种四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法，其特征在于：四足机器人在面向大坡度地形和高障碍物地形时采用爬行步态，爬行步态四条腿的摆腿顺序为：左前腿-右前腿-右后腿-左后腿；

将四足机器人爬行步态的运动周期按照上述的摆腿顺序划分为八个状态相：LF摆动相、四足支撑相、RF摆动相、四足支撑相、RH摆动相、四足支撑相、LH摆动相和四足支撑相；在单腿摆动相，摆动腿沿着规划的足端轨迹进行摆动，机身静止不动；在四足支撑相，机器人的重心在沿着规划的路径进行前向运动的同时进行侧向移动；

设计状态机，在单腿摆动相，摆动腿落地触发下一状态相；在四足支撑相，以当前所规划的四足支撑相的时间触发下一状态相；状态相一旦发生变化，机器人通过摆动腿的落地规划和感知策略估计未知地形的参数，并结合当前自身运动状态对新状态相进行在线的运动规划；在当前状态相的整个过程中，机器人一直执行当前状态相初始时刻运动规划所生成的轨迹，直到当前状态相结束；机器人按照前述的状态机依次进入下一个状态相，直至完成一个完整的运动周期，然后再进入下一运动周期；具体过程如下：

(1)上一状态相结束，机器人进入新状态相；更新支撑域，根据当前支撑域，基于COG稳定性判据，进行身体重心的路径规划；

(2)根据步骤(1)所规划的路径，基于NESM稳定性判据，进行身体重心的轨迹规划；

(3)根据上述的方法，引入足底力反馈，通过将力反馈得到的质心位置与运动学算出的位置进行加权平均，得到最终的质心位置；

(4)规划静步态行走时机身和足端的摆动顺序及步态时序；

(5)拟合足端摆动轨迹。

2.根据权利要求1所述的四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法，其特征在于：在单腿摆动过程中，为避免腿部与障碍物发生碰撞，在进行足端轨迹规划时，采用三段式控制，三段时间互有重叠，设定参数α为调节重叠时间的比例，比例越大，轨迹曲线越光滑；

阶段一：z轴方向上采用三次曲线规划上升，且x轴保持不动，保证了足端直线向上地运动；

阶段二：x轴进行三次曲线规划前行，z轴保持不动；

阶段三：z轴进行二次曲线规划，直至与地面发生接触，x轴保持不动。

3.根据权利要求1所述的四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法，其特征在于：在四足支撑阶段，构造伪俯仰角，调整各个腿长，使得伪俯仰角为0，同时使得横滚角为0，伪俯仰角的计算公式如下：

ψ＝arctan(((l_lf+l_rf)/2-(l_lh+l_rh)/2)/S)

根据上面取得的伪俯仰角的大小的反馈，来改变条腿的长度，并且考虑横滚角的大小，即得如下腿长更新公式：

l_lf＝l_lf+K_αψ+K_βφ

l_rf＝l_rf+K_αψ-K_βφ

l_lh＝l_lh-K_αψ+K_βφ

l_rh＝l_rh+K_αψ-K_βφ

其中，l_lf为左前腿在z轴方向的坐标，l_rf为右前腿在z轴方向的坐标，l_lh为左后腿在z轴方向的坐标，l_rh为右后腿在z轴方向的坐标，ψ为伪俯仰角大小，φ为IMU反馈得到的机身横滚角的大小，S为机身纵向跨距，K_α为伪俯仰角的反馈增益值，K_β为横滚角的反馈增益值。

4.根据权利要求1所述的四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法，其特征在于：所述步骤(1)进行身体重心的轨迹规划是求取机身重心在支撑平面上的投影点，当所述投影点位于支撑三角形内部，根据机身与足端的受力力矩平衡分析，计算投影点到支撑三角形边缘的最小稳定裕度，得到稳定区域。

Images