基于RRT的运动规划器和双足机器人平衡控制器

米科拉伊·瓦西里察和多米尼克·贝尔特
Institute of Control and Information Engineering, Poznan University of Technology, 60-965 Poznan, Poland ∗E-mail: mikolaj.r.wasielica@doctorate.put.poznan.pl, dominik.belter@put.poznan.pl

本文提出了一种针对双足机器人的新型运动规划方法。该方法利用环境的高程图，在崎岖地形上规划脚部路径和机器人本体的路径。我们展示了如何将确定落脚点、机器人姿态以及检查规划路径是否安全的模块集成到一个基于快速探索随机树的统一框架中。我们描述了在执行规划路径过程中稳定机器人的控制器。本文还介绍了该方法在Gazebo环境中针对Atlas机器人实现的情况。

Keywords ：双足机器人；运动规划；平衡控制。

1. 引言

目前，大多数搜救机器人都是轮式或履带式机器人。最近，在福岛第一核电站核事故期间，这些机器人的实用性得到了证实。然而，这些机器人存在局限性。如果走廊被瓦砾或洪水、风灾带来的杂物堵塞，它们可能会被困住。为了促进专用于搜救任务的机器人发展，DARPA机器人挑战赛应运而生。机器人和团队在预先准备好的环境中进行比赛。比赛设定了八项与灾害响应相关的任务，包括独立驾驶、穿越瓦砾、复位跳闸的断路器、旋转阀门和爬楼梯。许多团队参加了此次挑战，并使用他们的人形机器人展开竞争。

本研究聚焦于Atlas机器人的运动能力。我们结合慎思方法与反应式控制器，使机器人能够在崎岖地形上行走。论文中表明，为六足行走机器人设计的框架2具有通用性，也可用于双足机器人的运动规划。

1.1. 相关工作

我们的运动规划框架基于快速探索随机树（RRT），这是一种基于采样的、概率完备的规划算法。3 RRT在处理具有运动学和动力学约束的问题时非常高效。4该算法的贪婪RRT‐连接版本5会生长两棵树——一棵从机器人的初始位置开始，另一棵从目标位置开始。每棵树都向另一棵树最近添加的节点扩展，直到两者连接。Kuffner等将RRT应用于人形机器人在高度受限的三维环境中的运动规划。6在这种情况下，规划器假设脚部与地面的接触是恒定的，因此该规划器不能用于行走。Perrin等7使用RRT规划器为HRP‐2人形机器人寻找无碰撞步序序列。然而，机器人仅避开了位于平坦地面上的三维障碍物。利用分层RRT方法，可以实现人形机器人的完整的三维运动规划。8但遗憾的是，该方法仅在三维离散障碍物上进行了验证。最近，Deits和Tedrake提出了针对Atlas机器人的落脚点规划方法。9地形被分为两类：规划器排除了地形中的陡峭区域。另一种用于人形机器人运动规划的方法通过减少可能的步数，使操作员能够选择最合适的序列。10我们假设人类操作员仅定义机器人的目标位置，而路径由机器人自主地找到。

2. 运动规划器与控制器

2.1. 运动规划器

在我们之前关于六足机器人的研究中，我们详细介绍了落脚点选择、腿和身体轨迹规划器以及姿态优化算法，这些方法随后被统一到六足机器人的运动规划框架中。在本论文中，我们证明了相同的框架也可用于双足机器人的运动规划。

运动规划方法利用了Kuffner和LaValle提出的RRT‐Connect元算法。5为了规划机器人从初始位置到目标位置的路径，该算法创建了两棵树。第一棵树的根节点 Ta位于机器人的初始位置，而第二棵树的根节点 Tb位于目标位置。该算法尝试扩展这两棵树，直到它们连接在一起。

Extend该过程（图1c）用于扩展树并规划机器人的单步动作。该 Extend过程尝试将新节点添加到现有树中由元算法给出的方向。该方向由x, y在水平面上的位置确定。 Extend过程计算机器人的全身构型（机身参考坐标系的位置和脚部位姿）。首先，确定机器人机身位姿集合x在朝向目标的方向上。我们考虑机器人的最大可能步长，计算 n新的位姿。对于每一个可能的位姿x(k)，我们选择落脚点，优化机器人的姿态，规划脚步轨迹，并检查运动是否可行。我们从与机器人最长步长相关的位姿x(k= 0)开始此过程。如果该过程无法找到到达目标位姿x(k)的可执行路径，则减小步长（k:= k+ 1）。如果树可以扩展（成功），则该过程返回规划路径，否则返回（被困）信号（图1c）。

2.2. 支持模块

运动规划过程是通用的，但某些子过程是平台相关的，必须针对特定类型的机器人进行实现。在图1c中，依赖于机器人类型的部分子过程以绿色标出。该规划器由落足点选择、姿态优化、足部轨迹规划以及验证规划动作是否可执行的方法提供支持。规划器使用的第一种方法是落足点选择方法。

我们在从高程图中围绕标称落脚点选取的方形区域内搜索落脚点。根据以下公式评估每个可能的落脚点（高程图的单元格[i, j]）：

$$ Q(i, j)= k_1d(i, j)+ k_2v(i, j)+ k_3N(i, j) $$

其中 $ d(i, j) $ 是所考虑的落脚点与 RRT 给出的标称落脚点之间的距离，$ v(i, j) $ 是为脚底区域计算的单元高度方差，$ N(i, j) $ 是针对脚底所考虑区域的平均法向量与重力向量计算的点积，而 $ k_1, k_2, k_3 $ 是常数值。

当落脚点确定后，我们计算机器人本体离地高度（优化姿态），并确保机身与地面之间的最小离地间隙得到保证。机身的水平位置设置在机器人两脚之间中点处。最后，我们在新节点与树中的最近节点之间规划机身和脚部的轨迹。机身路径为线性，机身的位置和姿态在线性节点间线性变化。脚部的轨迹通过高程图计算获得，其在水平面上的路径为线性，离地高度则根据每个脚部与障碍物之间的最小距离要求来确定。

对于规划器找到的机器人每个构型，我们检查其运动学约束。我们在与机器人本体相关的坐标系中定义围绕机器人脚部的矩形工作空间（图 1a, b）。该工作空间累积了运动学和自碰撞约束。当所有要求都满足时，Extend过程返回success，并且新节点被添加到树中。

2.3. 机器人控制器

为了稳定Atlas机器人，我们开发了静态稳定性控制器，该控制器根据当前机器人构型和质量分布计算控制输出。我们的目标是实现具有高定位精度的稳定步态。该策略允许将脚部放置在落足点选择方法所给出的位置上。

我们专注于静态稳定性控制器而非动态控制器，因为机器人行走在崎岖且潜在危险的地形上。在高度动态运动过程中可能出现的任何定位偏差都可能导致机器人跌倒。

为了生成机器人的平滑运动，我们开发了两种生成关节轨迹的方法。第一种方法计算机器人从构型qinit到构型qgoal的关节空间轨迹q，采用五次（5阶）多项式生成速度和加速度曲线。第二种轨迹生成算法计算从初始位姿Tinit到目标位姿Tgoal的笛卡尔轨迹T，同样使用5阶多项式生成速度和加速度曲线。轨迹生成方法能够使机器人的脚从任意可达起始位姿移动到目标位姿。

为了保持静态稳定性，我们基于倒立摆模型实现了控制器。我们将 Atlas机器人表示为一个单一质量位于支点上方，由踝关节控制。机器人在单支撑相和双支撑相中均使用其踝关节。机器人的质心（COM）位置通过各个连杆的质量、关节构型以及正向运动学模型获得。当Atlas机器人的质心位于脚部支撑多边形内时，可保持静态稳定性。为了最小化跌倒的可能性，我们的控制器在单支撑相中使质心保持在足底中心上方，或在双支撑相中保持在连接两脚中心的线段上方，从而扩展安全裕度。我们的控制器运行频率比机器人的状态更新频率慢十倍。

3. 结果

我们在机器人操作系统和Gazebo环境中验证了该系统。a,b实验结果如图 2所示。第一次实验的地形图和规划路径如图2a所示。规划器规划足部路径以避开障碍物。落足点选择方法以及检查机器人工作空间的方法能够有效拒绝可能对机器人造成危险的步态。因此，机器人行走在可保证脚部支撑的地形上。路径执行过程中从仿真器截取的截图如图2c所示。在图2e、f中可以找到机器人执行的轨迹。控制器会修改机器人本体的初始路径以保持静态稳定性。行走过程中，机身位置被周期性地移动到落脚点上方区域。

在第二次实验中（图2b、d、f），环境与第一次实验相同。我们通过 Π/2rad修改了机器人的初始朝向（偏航角），以检验规划器是否能够旋转机器人来找到通往目标的路径。由于机器人在前进方向上的工作空间最长，因此首选运动是向前行走。因此，机器人一开始会调整朝向，使其正对第二个凹陷区域的前进方向。随后，机器人横向移动以通过狭窄通道，最后直线行走至目标位置（图2e）。

在第三次实验中（图3a、c、e），我们修改了环境以检测该算法如何应对狭窄通道。对于RRT等基于采样的算法而言，狭窄通道具有挑战性。在对环境进行采样时，所选择的机器人位置位于其中的概率较小。然而，我们的方法能够处理这一问题。基于RRT的算法得到了针对行走机器人的特定方法的支持。落足点选择方法在RRT给出的位置周围区域内进行搜索。因此，即使RRT给出的位置并未直接位于通道中间，仍然有可能找到合适的路径。执行的路径如图3e所示。机器人必须侧向移动并改变其方向以应对该障碍物。

行走机器人还应应对城市环境中常见的障碍物。因此，我们还在楼梯上进行了实验。规划路径如图3b所示。平衡控制器使机器人能够执行规划路径。为了保持机器人稳定，控制器会调整机器人本体的水平位置（图 3d, f）。

我们还计算了10次规划试验的平均时间。图2a、2b和图3a中展示的地图的平均规划时间分别为0.14、0.09、4.1和0.038秒。这意味着对于短程运动规划和相对简单的障碍物，该方法可以在没有显著延迟的情况下使用。当地形更具挑战性时，规划时间会增加（对于图2c所示地图，达到4.1秒）。

4. 结论

该论文提出了一种用于双足机器人的基于采样的运动规划方法。我们展示了该方法在Atlas机器人上的实现。文中给出了该方法在各种地形和障碍物条件下的效率。该规划器能够区分可通行的障碍物（如楼梯的台阶）和不可通行的障碍物（如深凹处、墙壁），并确定穿过可通行区域的路径。我们证明了该规划器是快速的，且结果具有可重复性。该规划器直观地结合了腿式机器人运动中的多种约束，例如运动学约束、自碰撞和稳定性。

该论文的另一项贡献是控制器，它执行规划路径并通过平衡机器人躯干来稳定机器人。该控制器定义了关节轨迹，在执行参考运动的过程中，它可以修改计算轨迹以稳定机器人。未来，我们将在配备机载地图构建和定位系统的真實双足机器人上验证提出的方法。