在这本电子书中，您将了解运动规划如何工作，以及如何将其应用于广泛的自主系统。您还将了解MATLAB中可用的运动规划算法®而且导航工具箱™以及如何为您的应用程序选择最佳算法。
简介
汽车或机器需要司机或物理控制器的时代已经一去不复返了——今天的自动驾驶汽车已经足够智能，可以变道，允许行人通过，甚至可以把自己停在平行位置。
运动规划是使自动驾驶汽车、机器人操纵器、ugv和无人机等系统自主的三个组成部分之一。另外两个是感知和控制。
自主系统工作流。
就像人类一样，自主系统通过扫描新环境来了解自己在哪里以及周围有什么。一旦有了环境地图，运动规划算法就会规划出一条通往给定目的地的无障碍路径。基于对这条路径上的下一步所做的决定，控制器通过向执行器发送命令来帮助移动系统。
运动规划的常见应用包括:
仓库中的自主移动机器人导航。
最后一英里无人机送货。
用机械手挑选和放置。
什么是运动规划?
运动规划是一个计算问题，寻找一系列动作，使机器人或车辆从初始状态移动到目标状态。“运动规划”和“路径规划”经常互换使用，但有一个关键的区别。运动规划生成车辆随时间变化位置的运动，而路径规划只生成车辆的路径。通过运动规划，车辆的运动可以在它所遵循的路径上发生变化，就像下面两个涉及自动驾驶汽车的场景:
场景1:汽车在红灯前减速，红灯变绿后继续行驶——这是一种运动上的变化，但不是在计划的路线上。
场景一:运动规划。
场景2:汽车在高速公路上变道——路径和运动的改变。
场景2:路径规划。
状态空间和其他运动规划的关键概念
对于实际应用，运动规划需要几个功能部分来工作。其中包括环境地图，使用同步定位和映射(SLAM)算法生成状态(位置和方向)机器人或车辆。机器人从一个状态到另一个状态的转换决定了它的运动。可以应用于机器人的变换集合称为状态空间或者是配置空间(C空间)．配置空间可以包括自由空间(其中机器人状态被认为是有效的)和障碍空间(其中机器人状态被认为是无效的)。
例如，在自动驾驶汽车中，汽车的位置和它的航向或方向共同代表它的状态。对于自动驾驶汽车的自动停车，停车场的地图标识了自由空间和障碍物空间，状态空间表示使用运动模型定义的所有可能的前进和反向机动的集合。
路径代价、最优性和完整性
路径成本
当机器人或车辆寻找路径时，它所走的每一步都可能与成本相关。在自由空间中旅行的成本通常设为零，而在有障碍物的空间中旅行的成本设为无穷大。
最优
如果路径规划算法总能找到最优路径，则称为最优路径规划算法。对于一条最优路径，其从初始位置到目标位置的所有可能路径的过渡代价(边缘代价)之和必须最小。
完整性
如果路径规划算法需要有限的时间才能找到路径(当存在路径时)，如果不存在路径则报告，则称为完整路径规划算法。
类提供的路径优化和完整路径规划算法可能不是最短的，但它的代价是最小的。在某些特定的情况下——例如，沿着走廊移动一个室内机器人——您可以将机器人沿着走廊中心移动的成本定义为小于将其移动到墙壁附近的成本。这种情况下的最佳路径将使机器人沿着中心移动，减少与墙壁碰撞的机会。
运动规划的常见类型
运动规划有许多不同类型的方法。最常见的有:
- 基于搜索和基于抽样的规划方法，这些方法基于创建搜索树或图的方式
- 全局和局部路径规划方法，这些方法基于规划是在整个地图中完成还是在一个子集中完成
让我们更详细地看看每种类型的方法。
基于搜索的计划
基于搜索的规划创建了一个可搜索的图，其中每个车辆状态或配置都被标识为一个节点。该图使用代价和基于启发式的方法从开始扩展到目标节点，以找到最短路径。基于搜索的规划通常在离散化地图上执行，其中地图被细分为网格单元，状态的数量是有限的或可数的无限的(每个状态可以分配一个唯一的整数)。
离散状态空间通常用二维网格图表示，其中单元的中心是要搜索的状态。一个常见的映射表示是占用网格图．
A*算法是一种常用的基于搜索的方法，用于在离散网格映射中查找路径。
基于搜索的计划。
应用于网格地图的*算法。
在网格地图上基于搜索的规划通常是合适的，当车辆或机器人可以被视为一个点，并且在规划阶段不涉及运动模型或运动学方程。一旦路径规划算法为机器人提供了路径点，就可以使用控制算法来考虑运动学约束。
Sampling-Based规划
在基于抽样的规划中，通过在状态空间中随机添加节点来创建搜索树或路线图。利用连续运动模型找到可能的无碰撞路径。基于抽样的规划通常使用启发式方法来探索搜索空间并使搜索方向偏向。一旦创建，树或路线图就会使用碰撞检查或搜索方法来找到通往目标的最短路径。
RRT算法是一种常用的基于采样的方法，用于在连续状态空间中寻找路径。
基于搜索的计划。
基于采样的运动规划可以适用于高维搜索空间，例如那些用于为机械臂找到一组有效的配置来拾取物体的空间。基于抽样的规划适用于各种实际应用，尽管它不能提供完整的解决方案，但它很受欢迎。如果搜索树的密度使样本足够接近，则找到解决方案(如果存在)的概率收敛为1。这使得一些基于抽样的规划器，如RRT和RRT*概率完备。
全局和本地路径规划
全局路径规划，或基于地图的规划，涉及到根据先天的环境知识。全局规划算法规划一个初始路径，以避免环境中的已知和静态障碍。例如，一个自主移动机器人可能会规划一条全局路径，在有墙壁等静态障碍物的走廊上将一本书从一个办公室送到另一个办公室。
局部路径规划(或动态重规划)重新计算路径以避免未知和动态障碍。局部规划算法跟踪全局规划并创建局部轨迹，同时避免新引入的障碍。例如，自动驾驶汽车可能会规划局部轨迹以改变车道以避开其他车辆，并合并回全局路径以到达目的地。
四步运动规划工作流程MATLAB
Navigation Toolbox提供了实现各种规划算法的类，包括常见的基于搜索的规划器，如A*和基于采样的规划器，如RRT和RRT*。工具箱还提供了路径度量来评估计划路径的障碍清除和平滑性。
路径指标:平滑度。
路径指标:间隙。
此外，导航工具箱提供了一个界面，允许您在系统的四步工作流中实现基于采样的运动规划算法:
- 表示状态空间。
- 定义一个状态验证器。
- 对新状态进行采样并检查其有效性。
- 将有效状态的集合表示为路径。
表示状态空间
自定义状态空间类导航。StateSpace允许您定义包含任何应用程序的可能状态或配置的状态空间。例如,stateSpaceDubins而且stateSpaceReedsShepp通过连接状态空间中的任意两个状态来支持自动停车的规划，从而使状态空间模仿类似汽车的机器人或具有Ackermann转向的机器人的运动。
导航工具箱提供以下随时可用的状态空间。
| 状态空间 | 配置 | 环境 | 应用程序 |
|---|---|---|---|
stateSpaceSE2 |
(x, y， θ) | 二维 | 完整的机器人 |
stateSpaceSE3 |
(x, y, z, qw, qx, qy, qz) | 3 d | 机器人机械手 |
stateSpaceDubins |
(x, y， θ) | 二维 | 具有最小转弯半径的非完整车辆 |
stateSpaceReedsShepp |
(x, y， θ) | 二维 | 具有最小转弯半径的非完整车辆 |
状态空间SE2。
状态空间杜宾斯。
定义状态验证器
状态验证器基于状态空间，对应于从SLAM算法获得的映射。它检查一个状态或两个采样状态之间的运动的有效性。例如，碰撞检查器是一种状态验证器，用于指示机器人状态或配置何时与障碍物发生碰撞。
“导航工具箱”提供以下状态验证器，用于验证2D和3D占用地图中的状态和离散化运动。
| 状态验证器 | 类型 | 应用程序 |
|---|---|---|
validatorOccupancyMap |
occupancyMap, binaryOccupancyMap |
2D占用图 |
validatorVehicleCostmap |
vehicleCostmap |
2D占用图 |
validatorOccupancyMap3D |
occupancyMap3D |
三维占用图 |
这些状态验证器是从工具箱中可用的自定义状态验证器派生出来的，导航。StateValidator，可用于确定一个状态或任意两个状态之间的运动的有效性。
采样新状态并检查有效性
基于采样的规划算法在定义的状态空间中随机采样状态，并使用状态验证器创建从起点到目标的无障碍路径。RRT和PRM等算法使用不同的抽样方案对状态进行抽样，并创建搜索树或路线图。
要对给定映射(从SLAM算法获得)内的状态进行采样，应用对应于映射外部限制的状态空间边界。
表示采样状态的集合
您可以使用计划函数将规划算法输出合并为树状数据结构。你可以使用这个类navPath将状态集合存储在给定的状态空间中，并对它们进行插值以获得路径。
选择运动规划算法
以下运动规划算法在导航工具箱中可用。
| 算法 | 类型及范围 | 好处 |
|---|---|---|
| 基于网格的一个* | 全球计划 |
|
| 混合一个* | 全球计划 |
|
| 概率路线图(prm) | 全球计划 |
|
| 快速探索随机树(RRT) | 全球计划 |
|
| RRT * | 全球计划 |
|
| 轨迹最优Frenet | 局部轨迹发生器 |
|
基于网格的一个*
A*算法是一个离散路径规划器，它可以通过创建连接开始节点和目标节点的加权图来工作在网格地图上。A*在离散网格映射上工作，并使用x-y线性连接扩展搜索树。根据估计从一个节点到另一个节点的旅行成本的成本函数，一个一个地探索节点。
工作原理
为了找到代价最小的路径，A*使代价函数f(n)最小:
F (n) = g(n) + h(n)
在哪里
- N是路径上的下一个节点
- G (n)是从起始节点到n的路径代价
- H (n)是一个启发式函数，用于估计从n到目标的最便宜路径的代价
A *算法。
MATLAB中的一个*算法
plannerAStarGrid在导航工具箱中提供了最常用的启发式，如欧几里得和曼哈顿。对于h(n)和g(n)，可以使用预定义的代价函数，也可以使用自定义的代价函数。你可以指定abinaryOccupancyMap或occupancyMap对象作为计划器的输入。
应用程序
A*算法直接适用于全驱动轮式机器人。A*在计算机应用中也很流行，比如视频游戏中的寻路。
混合一个*
杂化A*是A*的扩展。与A*类似，Hybrid A*工作在离散搜索空间中，但它将每个网格单元与车辆的连续3D状态(x, y， θ)相关联。它使用由运动原语组成的连续状态空间来生成平滑和可驾驶的路径。
工作原理
Hybrid A*使用有效的引导启发式，使搜索树朝着目标的方向扩展。它还使用了路径的解析展开，提高了精度，减少了规划时间。
混合动力A根据所提供的精确目标姿态为车辆生成平稳和可驾驶的路径。
起始节点，S = (1.5, 2.5, pi/2)
节点1,G1 = (5.5, 5.5, 0)
目标节点2,G2 = (5.5, 5.5， -pi/2
MATLAB中的混合A*
导航工具箱提供plannerHybridAStar，它使用状态验证器，例如validatorOccupancyMap或validatorVechicleCostmap用于占用映射中的冲突检查，并包括用于特定于应用程序的调优的成本和参数。
应用程序
与传统的A*不同，混合动力A*适用于具有非完整约束的车辆，例如自动驾驶汽车。它保证了运动的可行性，并考虑了车辆的方向和速度的微分约束。
混合A*自动停车。
人口、难民和移民事务局
基于搜索的算法不适用于具有高自由度的应用程序或地图尺寸非常大的应用程序。存储来自大型网格地图的数据在计算上是昂贵的。PRM是一种基于抽样的规划算法，在这种情况下很有帮助。
工作原理
PRM是地图中不同可能路径的网络图。该图是由给定区域内有限数量的随机点或节点生成的。PRM算法对每个节点进行随机抽样后，将固定半径内的所有节点连接起来，形成多个节点集群。
一旦构建了路线图，就可以在地图上查询从给定起始位置到给定目标位置的路径。由于PRM允许在同一路线图中针对不同的起始和目标位置进行多个查询，因此如果地图是静态的(不随时间变化)，则可以节省计算时间。PRM使用图搜索算法(如a * planner)在它创建的路线图中搜索路径。
人口、难民和移民事务局计划。
MATLAB中的PRM
机器人系统工具箱™提供mobileRobotPRM，这需要一个binaryOccupancyMap作为输入，并通过连接随机采样的节点在地图的空闲空间中创建一个路线图。您可以使用findpath函数查询从起点到目标位置的路径。由于PRM在寻找无障碍路径时不考虑车辆的尺寸，因此可以使用膨胀函数。
应用程序
MATLAB中的PRM实现适用于完整的移动机器人应用，例如没有未知障碍的静态仓库环境，在那里您想要改变开始和目标位置，而不需要机器人重新学习整个地图。
人口、难民和移民事务局计划。
RRT
RRT是一种适用于非完整约束的基于采样的算法。RRT算法可以有效地搜索非凸高维空间。它通过在已定义的状态空间中使用随机样本增量地创建搜索树。搜索树最终横跨搜索空间，并将开始状态连接到目标状态。
工作原理
RRT计划器生长以开始状态X为根的搜索树开始以下步骤:
- 计划器对一个随机状态X进行抽样兰德在状态空间中。
- 计划者找到一个状态X附近它已经在搜索树中，并且离X最近兰德(基于状态空间中的距离定义)。
- 计划从X展开附近对X兰德，直到状态X新是达到了。
- 新的状态X新被添加到搜索树中。
重复这个过程，直到树到达X目标．每次一个新的节点X新，则检查其与其他节点的连接是否发生冲突。实现一条从X出发的可驱动路径开始X目标，您可以使用运动原语或运动模型，如reed - shepp。
MATLAB中的RRT
导航工具箱提供plannerRRT，遵循第3章中描述的可定制的基于采样的规划接口。
双向RRT (biRRT)是RRT的一种变体，它创建两棵树，同时从开始状态和目标状态开始。biRRT算法在提高机器人在高维空间的搜索速度方面具有重要的应用价值。可通过以下方式获取manipulatorRRT在机器人系统工具箱。
应用程序
RRTs特别适用于涉及障碍物、高自由度机器人(如机器人操纵器)的微分约束和移动机器人路径规划的路径规划问题。注意，RRT规划可能导致路径突然转弯。您可以使用路径平滑算法来补偿这些不规则情况。
人口、难民和移民事务局计划。
RRT *
RRT算法给出了一个有效路径，但不一定是最短路径。RRT*是RRT算法的优化版本。虽然现实上不可行，但理论上RRT*算法可以在节点数接近无穷大时提供到达目标的最短路径。
工作原理
RRT*的基本原理与RRT相同，但它有两个关键的补充，使其能够产生显著不同的结果:
- RRT*包含每个节点的成本，由其与父节点的相对距离定义。它总是在X附近的固定半径内寻找代价最小的节点新．
- RRT*检查节点成本的降低，并重新连接搜索树以获得更短、更平滑的路径。
RRT *计划。
RRT*的MATLAB
plannerRRTStar中的导航工具箱适用于求解几何规划问题。
应用程序
RRT*给出了一个渐近最优解，这使得它特别适用于高维问题，如机器人操纵器。它在包含许多障碍的密集环境中也很有用。虽然RRT*能找到节点最少的最短路径，但不适用于非完整车辆。相比之下，RRT可以用于非完整车辆，并能够处理差分约束
平行泊车采用RRT*和MPC跟踪控制器。
轨迹最优Frenet
轨迹优化Frenet是一种基于参考全局路径规划轨迹的局部规划器。轨迹是一组状态，其中状态包含时间函数变量。轨迹规划在需要考虑速度的情况下是有用的。
工作原理
作为局部规划器，轨迹最优Frenet需要一个全局参考路径，以一组\([\，\，]\)路点的形式提供。对于沿着弯曲和连续的参考路径(如高速公路)进行规划，它使用由运行长度和到参考路径的横向距离组成的弗莱内坐标。
轨迹最优弗莱内采样从初始状态的交替轨迹，偏离参考路径的一些横向距离。它使用弗莱内参照系，有两种状态:
- 笛卡尔态:\([\，\，\，\，̇\，]\)
- Frenet state: \([\,̇ \,̈\, \,̇ \,̈] =ℎ, =ℎ\)
初始状态通过五阶多项式连接到采样的终端状态，试图最小化扰动并保证状态的连续性。采样轨迹评估的运动学可行性，碰撞和成本。
高速公路行驶轨迹规划。
弹道最优Frenet的MATLAB
trajectoryOptimalFrenet在导航工具箱中沿着参考路径查找最优轨迹，其中参考路径点由全局规划器(如Hybrid a *或RRT)生成。trajectoryOptimalFrenet生成多个备选路径，并根据最终状态与参考路径的偏差、路径平滑度、时间和距离评估它们的代价。它使用状态验证器，例如validatorOccupancyMap检查状态的有效性。
应用程序
轨迹最优Frenet可作为全局规划器与飞行器或机器人控制器之间的局部规划器。适用于变道机动、自适应巡航控制(含速度保持)等高速公路驾驶应用。它也可以用于移动机器人和其他阿克曼型车辆的动态重新规划。
您也可以从以下列表中选择一个网站: