四、使用MoveGroup C++接口——混合规划（四）

目录

0.引言

1.混合规划概念

2.混合规划架构

2.1混合规划管理器

2.2全局规划器

2.3局部规划器

3.混合规划器工作流程

---

0.引言

        MoveIt 的运动规划架构遵循“感知-规划-行动”的方法。为了规划和执行动作，首先需要感知环境和机器人状态（“感知”），然后由规划器计算机器人的轨迹（“规划”），最后使用轨迹控制器一次性执行（“行动”）。

        虽然这种解决方案在众所周知的静态环境中进行全局运动规划时效果很好，但在许多实际应用中，特别是在不稳定或动态的环境中，这种方法并不适用。例如，在桌子上为某人倒一杯水或在不平整的黑板上写字这样的任务，就需要更精细的方法来应对不可预测的变化。例如，机器人的环境可能会动态变化，或者任务本身存在一定的不确定性，比如用粉笔写字时需要适应板子上的压力，同时粉笔在使用过程中也会变短。

        解决这些问题需要一种能够根据即时条件调整已执行的动作的方法，甚至在环境发生不可预见的变化时通过重新规划来做出反应。混合规划架构试图通过结合全局和局部规划器来解决这个问题。

1.混合规划概念

        混合规划（Hybrid Planning）是一种运动规划方法，它结合了不同类型的运动规划器，以产生更健壮或更具反应性的解决方案。这种方法在导航社区已经得到了很好的建立，并成功地应用于像navigation2这样的流行项目中。

        MoveIt的混合规划架构结合了全局规划器和局部规划器，它们并行且反复地运行，具有不同的规划速度和应用范围。

	控制器	规划器
功能差异	负责实时监控系统状态，并根据预设的控制规律来调整系统，以保持或改变系统状态。控制器的输出通常直接与执行器（如电机）相连，以确保系统按照期望的方式运行	负责生成从当前状态到目标状态的路径或轨迹。规划器通常在运动前一次性或几次调用，以生成整个运动过程的计划
执行频率	控制器的执行频率通常很高，需要以足够快的速率运行来保证系统的稳定性和响应性，这可能达到每秒钟数百或数千次	规划器的执行次数通常较低，因为它只在需要新的运动计划时才被调用，例如机器人开始新任务或遇到障碍时

        全局规划器的任务是解决一个全局运动规划问题，这与“感知-规划-行动”应用中使用的规划器非常相似。所使用的规划算法应该是完整的，因此假设在计算时间上相对较慢。此外，全局规划器不需要是实时安全的，这意味着不能保证规划器在特定期限内找到解决方案。根据规划器的实现，全局规划器可能在执行期间生成一个初始解决方案或迭代优化解决方案。

        局部规划器在执行期间连续运行，产生迭代的机器人命令以跟随全局轨迹。在某种程度上，局部规划器类似于控制器，只是架构允许解决更复杂的问题和约束。其理念是规划器能够对全局进行推理的同时维护内部实时状态。这使得它可以用于解决任意组合的局部规划问题，例如：

• 展开、混合或拼接后续的全局参考轨迹

• 在遵循全局路径时动态避免附近碰撞

• 使全局轨迹适应局部约束（例如，在不平坦的表面上所需的力压力，根据视觉反馈重新调整工具）

• 局部轨迹优化和时间参数化（在局部环境中优化轨迹在计算上更便宜、更快速）

        为了使局部规划器能够解决这些局部问题，它必须快速、能够响应传感器反馈，并且在许多情况下需要是实时安全的。此外，它应该是确定性的，以避免剧烈或不可预测的运动。

        总的来说，局部规划器依赖于全局规划器产生的参考轨迹，以避免陷入局部最小值。由于局部最小值有时仍然无法排除，可能需要触发全局规划器进行重新规划以仍然达到期望的目标。这种事件需要特定的方式与规划器通信触发并进行相应的处理。为此，混合规划架构允许实现基于事件的处理，处理方式可以针对特定的用例和规划器类型进行定制。

	全局规划器	本地规划器
概念	求解全局规划轨迹 一次性/间隔优化轨迹路径	遵循全局参考轨迹 解决局部问题约束 可处理传感器输入 在局部优化解决方案 计算控制器命令
特点	没有计算时间限制 非实时安全 不具有确定性 计算控制器指令	可能会陷入局部最小值 计算时间短 实时性 确定性
举例	OMPL 规划器 STOMP TrajOpt Cartesian motion planner Pilz 工业运动规划器 MTC	IK解算器，雅可比矩阵 势场规划器 轨迹优化算法 模型预测控制（MPC） 基于传感器的最优控制

混合规划可以在广泛的用例中发挥作用,大多数应用可以归纳为以下三种场景：

• Online motion planning：在线运动规划是指在机器人执行任务的过程中，实时地进行运动规划。这种规划通常需要一个初始的全局解决方案，并在执行过程中不断进行优化。在线规划考虑到了环境的动态变化，能够对新的信息做出快速响应，更新规划路径。
• Reactive Motion：反应运动是指机器人在遇到意外情况或障碍物时，能够迅速做出反应并重新规划路径的能力。这种规划通常是局部的，侧重于避开即时的碰撞风险，而不是重新计算整个路径。反应运动要求规划器能够快速响应传感器输入，并生成避障动作。
• Adaptive Motion：自适应运动，局部规划器用于适应全局解决方案以适应动态条件，如保持与不均匀表面的工具接触。

        在实际应用中，这三种规划策略可以结合使用，以实现更加灵活和稳健的机器人运动控制。例如，一个机器人可能在执行任务时使用在线运动规划来适应环境变化，当遇到障碍物时使用反应运动来避免碰撞，同时在需要精细操作时使用自适应运动来调整其动作。

2.混合规划架构

下图描绘了构成混合规划架构的基本插件类型和 ROS 接口

该架构由三个 ROS 组件节点构成：

• 混合规划管理器 ：为混合规划请求提供 ROS 动作通信请求接口， 运行混合规划逻辑并协调全局规划期和局部规划器

• 全局规划器 ： 解决全局规划问题并发布解决方案轨迹

• 局部规划器 ： 处理传入的全局轨迹更新 ，解决基于机器人状态、外界和参考轨迹的局部规划问题 ， 向机器人驱动器发送位置/速度命令

        架构组件具有通用且高度可定制性。由于组件仅通过 ROS 2 消息接口进行交互，因此很容易替换架构的每个组件或插件的实现。插件接口被设计为最小化，并尽可能地从实际算法实现中抽象出来。这使开发人员能够完全专注于隔离逻辑或求解器，而无需实现基础设施的任何部分。这也允许将相同的组件重复用于不同的设置或规划问题。

2.1混合规划管理器

        这个组件就是架构的“大脑”。其主要目的是处理混合规划的动作请求，并基于规划逻辑插件协调运动规划和执行过程。规划逻辑在 PlanningLogic 插件中实现，并由特定（如传感器输入、时间触发或其他系统生成的信号）的事件触发。事件由字符串格式来定义和识别，并根据这些事件来触发对全局或局部规划器的调用或取消。下图显示了简单规划逻辑的示例事件日志：

        

       事件由混合规划动作请求以及全局和局部规划器的动作反馈消息触发。在这个例子中，混合规划管理器在接收到混合规划请求后启动全局规划器。全局轨迹到达后启动局部规划器，当局部规划器完成工作时，混合规划管理器返回一个混合规划响应。

       规划逻辑插件的自定义实现支持将通用事件映射到架构提供的各种动作上，例如“启动全局规划”、“停止轨迹执行”或“切换到局部规划器的约束x”。这样，运动规划的行为就可以高度定制化，并且具有很强的适应性。

2.2全局规划器

      全局规划器是架构中最基本的部分。它提供动作通信服务端来处理包括标准运动规划请求在内的全局规划器请求，这些请求由全局规划器插件处理。默认情况下，使用的是 MoveIt 的规划管道，但也可以在这里使用任何类型的规划器，甚至是多任务控制器。规划结果通过动作反馈来报告，并将解决方案轨迹发布给局部规划器进行后续处理。

2.3局部规划器

        局部规划器也运行着一个动作通信服务端，该服务端处理来自混合规划管理器的请求。这个动作用于启动和停止局部规划器与机器人的运动，同时能够配置运行时参数，指定约束条件或求解器类型等。

        局部规划器的实现基于两个插件：

        轨迹操作插件（Trajectory Operator）：这个插件维护全局参考轨迹，处理来自全局规划器的轨迹更新，并监控当前机器人状态的进程。

        局部约束求解器插件（Local Constraint Solver）：这个插件实现了迭代求解器算法，它根据参考轨迹和局部约束生成机器人命令。它可能包括用于动态处理传感器输入或事件更新的额外接口。

        下图显示了混合规划管理器执行请求操作时局部规划器的循环周期示例：

        在每次迭代中，收到局部规划器请求后查看当前的规划场景，并与参考轨迹中当前机器人状态匹配。如果目标已达到，局部规划动作成功完成。否则，根据当前机器人状态识别出当前的局部规划问题并随后求解解决。最后，将生成的控制命令发布到机器人控制器。

3.混合规划器工作流程

        通过绘制工作流图，可以最好地理解混合规划器的运行时行为，该工作流图可视化了不同组件的通信渠道和事件。

下图显示了成功执行轨迹的运行时逻辑

        混合规划器由一个混合规划请求调用，这也是混合规划管理器（Hybrid Planning Manager）首先响应的事件。在这个例子中，规划逻辑简单地依次运行两个规划器。在初始的混合规划请求之后，混合规划管理器调用全局规划器（Global Planner）。全局规划器计算并发布一个轨迹，这个轨迹被混合规划管理器和局部规划器组件（Local Planner Component）接收。需要注意的是，局部规划器组件仅处理这个新的轨迹，并且直到被混合规划管理器调用之前，它不会开始执行。一旦由混合规划管理器请求，局部规划器组件开始展开参考轨迹，并在达到最终状态时成功返回动作响应。之后，混合规划管理器返回一个成功的混合规划响应（HybridPlanningResponse）。

        现在让我们考虑一个更困难的场景，其中混合规划器被设计为在执行过程中通过重新规划来避开障碍物。下面的动画显示了一个简单的运动，该运动由于碰撞对象的变化而在运行时被修正。

        在计算全局轨迹后，在全局规划过程中存在的碰撞对象会消失。取而代之的是，出现了两个新的碰撞对象，它们使初始的全局轨迹无效。局部规划器检测到即将发生的碰撞，并暂停执行，直到全局规划器提供更新的无碰撞轨迹。

        所描述行为的工作流程如下

        启动与第一个示例相同，但在展开参考轨迹期间，局部规划器检测到碰撞。这时，规划逻辑通过重新调用全局规划器来做出反应。在新全局解决方案的计算过程中，局部规划器必须防止机器人与碰撞物体发生碰撞，即通过保持其当前位置。全局规划器完成计算后，新的全局解决方案被发布到局部规划器，局部规划器的轨迹操作插件将更新融合到参考轨迹中。之后，局部规划器组件继续跟随参考轨迹，因为更新后的解决方案使其能够绕过碰撞物体。