如何将MATLAB运动规划应用于各种自主系统

曾几何时，汽车离不开驾驶员，机器也离不开实体控制器。但现在不一样了：如今，高度智能的自动驾驶汽车已经能够自己变道、礼让行人甚至侧方停车。

自动驾驶汽车、机器人操作臂、无人地面交通工具 (UGV) 和无人机等系统要实现自主，离不开三大支柱，运动规划就是其中之一。另外两大支柱则是感知和控制。

与人类非常相似，自主系统也是通过扫描环境来探索新环境，以了解自己所在的位置和周围环境。

一旦得出环境地图，运动规划算法就会规划一条通往指定目的地的无障碍路径。

算法会决定沿着路径要采取的下一步，控制器根据这一决定向作动器发送命令，使得系统移动。

什么是运动规划？

运动规划是一种计算问题，旨在寻找将机器人或车辆从初始状态移动至目标状态的动作序列。

“运动规划”和“路径规划”这两个词经常混用，但两者有一个关键区别。

运动规划在车辆位置随时间变化时生成车辆的运动，而路径规划只生成车辆的路径。

通过运动规划，车辆可以在遵循现有路径的同时改变运动，如以下两个自动驾驶汽车场景所示：

场景 1：红灯亮起时，汽车减速后停止；绿灯亮起时，汽车继续行驶，这是运动变化，不是规划路径变化。

状态空间和其他运动规划关键概念

在实际应用中，运动规划的实现有赖于多个功能部件。其中包括使用同步定位与地图构建 (SLAM) 算法生成的环境地图，以及机器人或车辆的状态（位置和方向）。

机器人的状态间变换定义了其运动。可以应用于机器人的变换集合称为状态空间或配置空间(Cspace)。配置空间可以包括自由空间（其中的机器人状态被视为有效）和障碍空间（其中的机器人状态被视为无效）。

例如，在自动驾驶汽车中，汽车的位置及其驶向或方向共同代表了它的状态。

对于自动驾驶汽车的自动泊车，停车场的地图标识了自由空间和障碍空间，状态空间表示使用运动模型定义的所有可能的前进和后退机动的集合。

路径代价、最优性和完备性

路径代价

当机器人或车辆在寻找路径时，它所采取的每一步都与代价相关联。穿越自由空间的代价通常设为零，穿越包含障碍物的空间的代价设为无穷大。

最优性

如果路径规划算法总能找到最优路径，则称其为最优算法。为了使路径最优，其转换代价（边缘代价）之和在从初始位置到目标位置的所有可能路径中必须是最低的。

完备性

在有限的时间内，当路径存在时，路径规划算法能找出路径，当路径不存在时，算法能报告路径不存在，则称该算法为完备的。

最优且完备的路径规划算法所提供的路径不一定是最短的，但代价会是最小的。

在某些特定的情况下（例如，让室内机器人沿着走廊移动），可以将机器人沿走廊中心移动的代价定义为低于靠近墙壁移动的代价。

在这种情况下，最优路径是让机器人沿着走廊中心移动，减少与墙壁碰撞的机会。 

运动规划的常见类型

运动规划有许多不同类型的方法。最常见的方法如下：

基于搜索的规划和基于采样的规划方法，取决于搜索树或图的创建方式

全局和局部路径规划方法，取决于规划是在整个地图中还是在某一子集中完成

接下来我们将逐一探讨每种方法。

基于搜索的规划

基于搜索的规划创建一个可搜索的图，将每个车辆状态或配置标识为一个节点。该图从起始节点扩展到目标节点，使用基于代价和启发式的方法来寻找最短路径。

基于搜索的规划通常在离散化地图上执行，其中地图被细分为栅格单元，状态数是有限的或可数无限的（可以为每个状态分配一个唯一的整数）。

离散状态空间通常用二维栅格地图表示，其中各个网格的中心是要搜索的状态。一种常见的地图表示方法是占据栅格地图。

A* 算法是一种常用的基于搜索的方法，用于在离散栅格地图中寻找路径。

当车辆或机器人可被视为一个点且规划阶段不涉及运动模型或运动学方程时，栅格地图上基于搜索的规划通常适用。

如果路径规划算法为机器人提供了要遵循的路点，则可以使用控制算法来添加运动学约束。

基于采样的规划

在基于采样的规划中，搜索树或路线图是通过在状态空间中随机添加节点来创建的。使用连续运动模型，可以找到可能的无碰撞路径。

基于采样的规划通常使用启发式方法来探索搜索空间并偏转搜索方向。创建后，树或路线图使用碰撞检查或搜索方法来寻找到达目标的最短路径。

RRT 算法是一种常用的基于采样的方法，用于在连续状态空间中寻找路径。

基于采样的运动规划适用于高维搜索空间，例如寻找一组有效的配置，使机械臂能够拾取物体。基于采样的规划广泛适用于多种实际应用，虽然不能提供完备解，但仍广受欢迎。

如果搜索树的密度使样本足够接近，则当解存在时，找到解的概率会收敛到 1。这使得一些基于采样的规划器（例如 RRT 和 RRT*）在概率上是完备的。

全局和局部路径规划

全局路径规划又称基于地图的规划，它根据有关环境的先验知识寻找最优路径。

全局规划算法规划初始路径，以避开环境中已知的静态障碍。

例如，一个自主移动机器人可以规划一条全局路径，在有墙壁等静态障碍物的走廊上，将一本书从一个办公室送到另一个办公室。

局部路径规划又称动态重规划，它重新计算路径，以避开未知的动态障碍。

局部规划算法跟踪全局规划并创建局部轨迹，同时避开新引入的障碍。

例如，一辆自动驾驶汽车可能会规划局部轨迹，变道以避开其他车辆，然后重新汇入全局路径以抵达目的地。

使用 MATLAB 进行运动规划的四步工作流

Navigation Toolbox 提供了用于实现各种规划算法的类，包括常见的基于搜索的规划器（例如 A*）和基于采样的规划器（例如 RRT 和 RRT*）。

该工具箱还提供路径指标，来评估所规划路径的避障间隙和平滑度。

此外，Navigation Toolbox 提供了一个接口，可让您在系统化的四步工作流中实现基于采样的运动规划算法：

表示状态空间。

定义状态校验器。

对新状态进行采样并检查有效性。

将一组有效状态表示为路径。

表示状态空间

自定义状态空间类 nav.StateSpace 允许您定义一个状态空间，在其中包含任何应用的可能状态或配置。例如，stateSpaceDubins 和 stateSpaceReedsShepp 通过连接状态空间中的任意两个状态来支持自动泊车规划，以便状态空间模拟汽车类机器人或带有阿克曼转向的机器人的运动。

Navigation Toolbox 提供以下现成的状态空间。

定义状态校验器

状态校验器基于状态空间，并与通过 SLAM 算法获得的地图相对应。它检查单个状态的有效性或两个采样状态之间的运动的有效性。例如，碰撞检查器是一种状态校验器，可指示机器人状态或配置与障碍物发生碰撞的情况。

Navigation Toolbox 提供以下状态校验器，用于校验二维和三维占据地图中的状态和离散化运动。

这些状态校验器派生自工具箱中提供的自定义状态校验器 nav.StateValidator，可用于确定单个状态的有效性或任意两个状态之间的运动的有效性。

对新状态进行采样并检查有效性

基于采样的规划算法在定义的状态空间中随机对状态采样，并使用状态校验器创建从起点到目标的无障碍路径。RRT 和 PRM 等算法使用不同的采样方案对状态进行采样，并创建搜索树或路线图。

对于通过 SLAM 算法获得的地图，为对地图内的状态进行采样，会应用与地图外侧界限相对应的状态空间边界。

表示采样状态的集合

您可以使用 Navigation Toolbox 中的 plan 函数将规划算法的输出整理成树状数据结构。您可以使用 navPath 类存储给定状态空间中的状态集合，并对它们进行插值以获得路径。

运动规划函数：https://ww2.mathworks.cn/help/nav/motion-planning.html

选择运动规划算法

Navigation Toolbox 中提供了以下运动规划算法。

进一

责任编辑：haq