CN107065867A - 一种面向未知崎岖地形的四足机器人运动规划方法 - Google Patents

Abstract

本发明公开了一种面向未知崎岖地形的四足机器人运动规划方法，四足机器人在面向崎岖地形时采用爬行步态。在单腿摆动相，摆动腿沿着规划的足端轨迹进行摆动，同时，机器人的重心通过支撑腿的运动沿着规划的路径进行前向运动；在四足支撑相，机器人的重心在沿着规划的路径进行前向运动的同时进行侧向移动。本发明基于COG与ZMP双稳定性判据实现了机器人的在线轨迹规划，针对地形未知的情况，充分考虑摆动腿提前落地以及滞后落地的可能，通过摆动腿的落地规划和感知策略估算未知地形的参数，实现了机器人对未知地形高度与坡度的自适应，根据运动规划结果，机器人实现了面向未知崎岖地形的连续自适应行走。

Description

一种面向未知崎岖地形的四足机器人运动规划方法

技术领域

本发明属于机器人运动规划领域，具体涉及一种四足机器人运动规划方法，使得机器人能够平稳地通过未知崎岖地形。

背景技术

长期以来，轮式和履带式机器人由于移动速度快、运动效率高、控制方便等优点，一直是地面移动机器人的首选。但是，现有的轮式和履带式机器人主要应用于平地或者近似平坦的地形，很难适应各种复杂的地形，而人与动物可以通过腿足的运动方式到达陆地上任何地方，这就促使了腿式机器人的发展。

四足机器人作为常见四足哺乳动物的仿生再造，其依靠腿足运动几乎能在任何地面上活动，可以应对各种复杂地形。进一步从制造成本和控制难易程度等方面综合衡量，四足机器人是当前最佳的腿式机器人形式，其在非结构化地形环境下的物资运输、勘探侦查以及救援救灾等领域具有较好的应用前景。

机器人运动规划是解决移动机器人运动的核心问题之一，其实质是根据运动目标与环境地形，在构型空间中求解一条从起始位姿点到目标位姿点的连续路径。在构型空间中，机器人被当作一个点，每一个位姿点代表着机器人在物理空间中的位置和方位，而对于腿式机器人，其对应于一个具体的腿关节构型。

经检索，目前国内外对四足机器人的研究已经较为丰富，主要集中于机械结构设计与四足步态研究，如美国专利申请号7598695的发明专利公开了一种可以实现爬行步态的小型四足机器人，但是对于面向复杂崎岖地形的四足机器人运动规划方法研究较少，中国专利申请号201310014887.5的发明专利公开了一种面向复杂地形的四足机器人运动规划方法，但是该方法需要激光与立体视觉传感器对四足机器人的运动环境进行局部检测与精确建模，其运动规划结果的好坏决定于机器人与地形模型的准确性，而实际环境下很难做到精确建模；中国专利申请号201410579726.5的发明专利公开了一种四足机器人自由步态生成方法，该步态有效提高了四足机器人对地形的适应性，但是该方法需要预先知道地形的信息以搜寻最优的落脚点，而且机器人仅在四足支撑阶段进行前向运动，使得机器人移动速度较慢且运动不连续。上述研究并没有考虑在无崎岖地形先验知识的情况下，四足机器人的自适应行走问题。本发明以提高机器人对地形的适应能力为目标，兼顾移动速度，保证运动连续性，实现机器人面向未知崎岖地形的自适应行走。

发明内容

本发明针对现有的四足机器人运动规划方法存在的不足，提出一种面向未知崎岖地形的四足机器人运动规划方法，在保证运动连续的前提下，使四足机器人能够稳定地通过未知崎岖地形。

为了实现上述目的，本发明采用如下技术方案：

四足机器人在面向崎岖地形时宜采用爬行步态。作为一种静态步态，爬行步态具有更加强大的地形适应能力，可最大化提高机器人在崎岖地形的通行能力。爬行步态四条腿的摆腿顺序为：FL(前左腿，front left)→HR(后右腿，hind right)→FR(前右腿，frontright)→HL(后左腿，hind left)，每个时刻最多只有一条腿在摆动，换而言之，每个时刻至少有三条腿在支撑。

将四足机器人爬行步态的运动周期按照上述的摆腿顺序划分为六个状态相：①FL摆动相，②四足支撑相，③HR摆动相，④FR摆动相，⑤四足支撑相，⑥HL摆动相。在单腿摆动相，机器人指定的一条腿处于摆动状态，其余的三条腿处于支撑状态，摆动腿沿着规划的足端轨迹进行摆动，同时，机器人的重心通过支撑腿的运动沿着规划的路径进行前向运动；在四足支撑相，机器人的四条腿均处于支撑状态，机器人的重心在沿着规划的路径进行前向运动的同时进行侧向移动。

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

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

步骤二：根据步骤一所规划的路径，基于ZMP(Zero Moment Point)稳定性判据，进行身体重心的轨迹规划；

步骤三：若当前状态相是因为摆动腿落地而触发，以摆动腿落脚点表征未知地形，更新支撑足，并根据当前支撑足的相对位置估计未知地形的参数，规划机器人躯体质心高度以及躯体姿态以自适应地形高度与坡度的变化；

步骤四：若当前状态相是单腿摆动相，指定摆动腿进行足端轨迹规划；

步骤五：机器人处于当前状态相，执行上述运动规划所生成的轨迹，直到触发下一状态相，转到步骤一，重复整个过程，持续运动直至机器人到达目标位姿点。

进一步地，所述步骤一具体包括以下子步骤：

(1.1)上一状态相结束，机器人进入新状态相；

(1.2)若当前状态相是因为摆动腿落地而触发，根据刚落地的摆动腿落脚点位置更新当前支撑域；

(1.3)根据运动目标，设定下一摆动腿的步长，预估下一摆动腿落脚点位置，得到下一支撑域；

(1.4)计算当前支撑域和下一支撑域的内心位置，以当前支撑域内心为起点，下一支撑域内心为终点，基于COG稳定性判据，规划一条连续的路径。

进一步地，所述步骤二具体包括以下子步骤：

(2.1)采用ZMP动态稳定性判据。

(2.2)采用cart-table ZMP模型，考虑前向x方向的运动：

其中，g是重力加速度，z为竖直方向。同理可得侧向y方向的y_ZMP；

在每时每刻，都必须保证ZMP位于支撑域内；

(2.3)约束当前轨迹的初始位置、速度、加速度分别等于上一规划轨迹的末端位置、速度、加速度。

进一步地，所述步骤三具体包括以下子步骤：

(3.1)若当前状态相是因为摆动腿落地而触发，以摆动腿落脚点表征未知地形；

(3.2)地形高度的变化会导致落脚点竖直位置发生变化进而影响机器人身体高度，反过来，通过计算机器人身体高度的变化量，以表征未知地形的高度变化；

(3.3)根据刚落地的摆动腿更新当前支撑足，以支撑足所在的斜面表征未知地形的坡度。在世界坐标系中，通过三个支撑足的相对位置，计算支撑足斜面的单位法向量则未知地形的坡度估计方程为：

其中，θ_Roll、θ_Pitch、θ_Yaw分别为机器人的期望滚动角、期望俯仰角、期望偏摆角，绕Z轴旋转θ角的旋转矩阵

(3.4)根据机器人身体高度的变化量，规划机器人躯体质心高度以自适应未知地形的高度变化；

(3.5)根据未知地形的坡度估计值(θ_Pitch,θ_Roll)规划机器人躯体姿态，使机器人以平行于未知地形斜面的躯体姿态自适应未知地形的坡度变化。

进一步地，所述步骤四具体包括以下子步骤：

(4.1)若当前状态相是单腿摆动相，指定摆动腿，根据运动目标，设定摆动腿步长以及摆腿高度；

(4.2)由于地形信息未知，摆动腿的落脚点高度存在大于、等于以及小于当前抬脚点高度的可能，充分考虑摆动腿提前落地以及滞后落地这两者情况，对摆动腿进行足端轨迹规划。

进一步地，所述步骤五具体包括以下子步骤：

(5.1)根据当前状态相初始时刻运动规划所生成的轨迹，通过逆运动学模型，解算到关节角空间，生成各关节角轨迹。

(5.2)执行各关节角轨迹，直至触发下一状态相，当前状态相结束。

(5.3)若触发下一状态相，转到步骤一，重复整个过程，持续运动直至机器人到达目标位姿点。

本发明的有益效果是：本发明基于COG与ZMP双稳定性判据实现了机器人的在线轨迹规划，针对地形未知的情况，充分考虑摆动腿提前落地以及滞后落地的可能，通过摆动腿的落地规划和感知策略估算未知地形的参数，实现了机器人对未知地形高度与坡度的自适应，根据运动规划结果，机器人实现了面向未知崎岖地形的连续自适应行走。

附图说明

图1四足机器人的结构模型示意图；

图2爬行步态腿摆动时间顺序分析模型示意图；

图3在线规划流程图；

图4支撑域示意图；

图5路径规划示意图；

图6未知地形坡度估计示意图；

图7摆动腿的足端轨迹XZ平面示意图。

具体实施方式

下面以图1所示的十二自由度四足机器人为具体实施例，对本发明进行详细说明。

四足机器人在面向未知崎岖地形时采用爬行步态，其腿摆动时间顺序分析模型如图2所示，由图可知，四足机器人的每条腿可分为摆动相和支撑相，而爬行步态自然地将一个运动周期划分为六个状态相。机器人将采用如图所示的摆腿顺序进行周期运动，直至运动到目标位姿点。

在单腿摆动相，摆动腿落地触发下一状态相；在四足支撑相，以当前所规划的四足支撑相的时间触发下一状态相。四足机器人在线规划流程如图3所示，一旦进入新的状态相，此时机器人处于运动规划阶段，机器人将会通过摆动腿的落地规划和策略感知未知地形的信息，并结合当前自身运动状态对新状态相进行实时规划，实现在线的运动规划；否则，机器人处于执行阶段，机器人一直执行当前状态相初始时刻运动规划所生成的轨迹，直到当前状态相结束。机器人按照前述的状态机依次进入下一个状态相，直至完成一个完整的运动周期，然后再进入下一运动周期。

若机器人处于运动规划阶段，则当前状态相的运动规划具体包含以下步骤：

步骤一：身体重心的路径规划：

1)若当前状态相是因为摆动腿落地而触发，根据刚落地的摆动腿落脚点位置更新当前支撑域；

图4所示为左侧腿摆动时机器人的支撑域。当HL摆动时，FL与右侧支撑腿构成支撑三角形(右侧三角形)；当FL摆动时，HL与右侧支撑腿构成支撑三角形(左侧三角形)。两个支撑三角形生成一个公有支撑三角形。为了降低机器人运动时跌倒的可能性，COG与ZMP应该保证在支撑域的内部并且离支撑域的边沿有一定的距离。引入稳定裕度，取缩小后的公有支撑三角形(虚线三角形)为支撑域。

2)根据运动目标，设定下一摆动腿的步长，预估下一摆动腿落脚点位置，得到下一支撑域；

3)计算当前支撑域和下一支撑域的内心位置，以当前支撑域内心为起点，下一支撑域内心为终点，基于COG稳定性判据，在当前支撑域内规划一条连续的路径，使得在每时每刻COG一直位于支撑域内。

图5所示为一个完整运动周期的路径规划示意图，由图可知，COG始终处于支撑域的内部并且离支撑域边沿有足够的距离。在单腿摆动相，机器人的重心只进行前向运动；在四足支撑相，机器人的重心在进行前向运动的同时还有侧向移动，四足支撑相的侧向移动保证了机器人能够在两侧支撑三角形之间实现平稳过渡。

步骤二：身体重心的轨迹规划：

1)采用ZMP动态稳定性判据；

2)采用cart-table ZMP模型，考虑前向x方向的运动：

其中，g是重力加速度，z为竖直方向。同理可得侧向y方向的y_ZMP。在每时每刻，都必须保证ZMP位于支撑域内。

3)为了保证轨迹的光滑性，约束当前轨迹的初始位置、速度、加速度分别等于上一规划轨迹的末端位置、速度、加速度。

对于每个状态相，均可采用五次样条进行插值，即可保证整个身体重心轨迹的光滑性。对于四足支撑相，其侧向移动的轨迹在初始和末端位置的速度与加速度应规划为0。

图5展示了一个完整运动周期内的ZMP轨迹，在每时每刻，ZMP均位于支撑域的内部并且离支撑域的边沿有足够的距离。

步骤三：未知地形估计：

1)若当前状态相是因为摆动腿落地而触发，以摆动腿落脚点表征未知地形；

2)定义机器人身体高度为躯体质心相对于支撑足在竖直方向的相对高度，则在机器人躯体世界坐标系∑_WB下：

H＝-(^WBz_FL+^WBz_FR+^WBz_HL+^WBz_HR)/4

其中，^WBz表示各支撑足在∑_WB坐标系下Z_WB方向即竖直方向的位置。

计算当前机器人身体高度H，将其与期望的机器人身体高度H_d做比较，根据机器人身体高度的变化量，规划机器人躯体质心高度以自适应未知地形的高度变化。

3)在世界坐标系中，通过三个支撑足的相对位置，对未知地形的坡度进行估计，如图6所示。

计算支撑足斜面的单位法向量则未知地形的坡度估计方程为：

其中，θ_Roll、θ_Pitch、θ_Yaw分别为机器人的期望滚动角、期望俯仰角、期望偏摆角，绕Z轴旋转θ角的旋转矩阵

根据未知地形的坡度估计值(θ_Pitch,θ_Roll)规划机器人躯体姿态，使机器人以平行于未知地形斜面的躯体姿态自适应未知地形的坡度变化。

步骤四：摆动腿足端轨迹规划：

1)若当前状态相是单腿摆动相，指定摆动腿，根据运动目标，设定摆动腿步长以及摆腿高度；

2)由于地形信息未知，摆动腿的落脚点高度存在大于、等于以及小于当前抬脚点高度的可能，充分考虑摆动腿提前落地以及滞后落地这两者情况，对摆动腿进行足端轨迹规划。图7所示为在世界坐标系下，摆动腿的足端轨迹XZ平面示意图，图中AB段考虑了摆动腿提前落地的情况，BC段考虑了摆动腿滞后落地的情况。

若机器人不处于运动规划阶段，则其只处于执行阶段：

1)根据当前状态相初始时刻运动规划阶段所生成的轨迹，通过逆运动学模型，解算到关节角空间，生成各关节角轨迹。

2)执行各关节角轨迹，直至触发下一状态相，当前状态相结束。

3)若触发下一状态相，转到运动规划阶段。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (6)

1.一种面向未知崎岖地形的四足机器人运动规划方法，其特征在于：四足机器人在面向崎岖地形时采用爬行步态，爬行步态四条腿的摆腿顺序为：FL(前左腿，front left)→HR(后右腿，hind right)→FR(前右腿，front right)→HL(后左腿，hind left)。

将四足机器人爬行步态的运动周期按照上述的摆腿顺序划分为六个状态相：FL摆动相、四足支撑相、HR摆动相、FR摆动相、四足支撑相、HL摆动相。在单腿摆动相，摆动腿沿着规划的足端轨迹进行摆动，同时，机器人的重心通过支撑腿的运动沿着规划的路径进行前向运动；在四足支撑相，机器人的重心在沿着规划的路径进行前向运动的同时进行侧向移动。

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

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

步骤二：根据步骤一所规划的路径，基于ZMP(Zero Moment Point)稳定性判据，进行身体重心的轨迹规划；

步骤三：若当前状态相是因为摆动腿落地而触发，以摆动腿落脚点表征未知地形，更新支撑足，并根据当前支撑足的相对位置估计未知地形的参数，规划机器人躯体质心高度以及躯体姿态以自适应地形高度与坡度的变化；

步骤四：若当前状态相是单腿摆动相，指定摆动腿进行足端轨迹规划；

步骤五：机器人处于当前状态相，执行上述运动规划所生成的轨迹，直到触发下一状态相，转到步骤一，重复整个过程，持续运动直至机器人到达目标位姿点。

2.根据权利要求1所述的一种面向未知崎岖地形的四足机器人运动规划方法，其特征在于，所述步骤一具体包括以下子步骤：

(1.1)上一状态相结束，机器人进入新状态相；

(1.2)若当前状态相是因为摆动腿落地而触发，根据刚落地的摆动腿落脚点位置更新当前支撑域；

(1.3)根据运动目标，设定下一摆动腿的步长，预估下一摆动腿落脚点位置，得到下一支撑域；

(1.4)计算当前支撑域和下一支撑域的内心位置，以当前支撑域内心为起点，下一支撑域内心为终点，基于COG稳定性判据，规划一条连续的路径。

3.根据权利要求1所述的一种面向未知崎岖地形的四足机器人运动规划方法，其特征在于，所述步骤二具体包括以下子步骤：

(2.1)采用ZMP动态稳定性判据。

(2.2)采用cart-table ZMP模型，考虑前向x方向的运动：

<mrow> <msub> <mi>x</mi> <mrow> <mi>Z</mi> <mi>M</mi> <mi>P</mi> </mrow> </msub> <mo>=</mo> <mi>x</mi> <mo>-</mo> <mfrac> <mi>z</mi> <mi>g</mi> </mfrac> <mover> <mi>x</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> </mrow>

其中，g是重力加速度，z为竖直方向。同理可得侧向y方向的y_ZMP；

在每时每刻，都必须保证ZMP位于支撑域内；

(2.3)约束当前轨迹的初始位置、速度、加速度分别等于上一规划轨迹的末端位置、速度、加速度。

4.根据权利要求1所述的一种面向未知崎岖地形的四足机器人运动规划方法，其特征在于，所述步骤三具体包括以下子步骤：

(3.1)若当前状态相是因为摆动腿落地而触发，以摆动腿落脚点表征未知地形；

(3.2)地形高度的变化会导致落脚点竖直位置发生变化进而影响机器人身体高度，反过来，通过计算机器人身体高度的变化量，以表征未知地形的高度变化；

(3.3)根据刚落地的摆动腿更新当前支撑足，以支撑足所在的斜面表征未知地形的坡度。

在世界坐标系中，通过三个支撑足的相对位置，计算支撑足斜面的单位法向量则未知地形的坡度估计方程为：

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mrow> <mi>P</mi> <mi>i</mi> <mi>t</mi> <mi>c</mi> <mi>h</mi> </mrow> </msub> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mfrac> <msub> <mover> <mi>N</mi> <mo>&amp;RightArrow;</mo> </mover> <mi>x</mi> </msub> <msub> <mover> <mi>N</mi> <mo>&amp;RightArrow;</mo> </mover> <mi>z</mi> </msub> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mrow> <mi>R</mi> <mi>o</mi> <mi>l</mi> <mi>l</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mi>arcsin</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>N</mi> <mo>&amp;RightArrow;</mo> </mover> <mi>y</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

其中，θ_Roll、θ_Pitch、θ_Yaw分别为机器人的期望滚动角、期望俯仰角、期望偏摆角，绕Z轴旋转θ角的旋转矩阵

(3.4)根据机器人身体高度的变化量，规划机器人躯体质心高度以自适应未知地形的高度变化；

(3.5)根据未知地形的坡度估计值(θ_Pitch,θ_Roll)规划机器人躯体姿态，使机器人以平行于未知地形斜面的躯体姿态自适应未知地形的坡度变化。

5.根据权利要求1所述的一种面向未知崎岖地形的四足机器人运动规划方法，其特征在于，所述步骤四具体包括以下子步骤：

(4.1)若当前状态相是单腿摆动相，指定摆动腿，根据运动目标，设定摆动腿步长以及摆腿高度；

(4.2)由于地形信息未知，摆动腿的落脚点高度存在大于、等于以及小于当前抬脚点高度的可能，充分考虑摆动腿提前落地以及滞后落地这两者情况，对摆动腿进行足端轨迹规划。

6.根据权利要求1所述的一种面向未知崎岖地形的四足机器人运动规划方法，其特征在于，所述步骤五具体包括以下子步骤：

(5.1)根据当前状态相初始时刻运动规划所生成的轨迹，通过逆运动学模型，解算到关节角空间，生成各关节角轨迹。

(5.2)执行各关节角轨迹，直至触发下一状态相，当前状态相结束。

(5.3)若触发下一状态相，转到步骤一，重复整个过程，持续运动直至机器人到达目标位姿点。