Matlab之在城市环境中基于动态占用网格图的的运动规划仿真（附源码）

此示例演示如何使用 Frenet 参考路径在城市驾驶场景中执行动态重新规划。在此示例中，将使用本地环境的动态占用网格地图估计值来查找最佳本地轨迹。

一、介绍

自动驾驶汽车的动态重新规划通常由当地的运动规划器完成。本地运动规划器负责根据全局计划和有关周围环境的信息生成最佳轨迹。有关周围环境的信息主要可以用两种方式来描述：

1. 周围环境中具有已定义几何图形的离散对象集。

2. 离散化网格，估计周围环境中的自由和占用区域。

在环境中存在动态障碍物的情况下，本地运动规划师需要对周围环境的信息进行短期预测，以评估计划轨迹的有效性。环境表示的选择通常由上游感知算法控制。对于规划算法，基于对象的表示形式提供了对环境的内存高效描述。它还允许一种更简单的方法来定义用于行为预测的对象间关系。另一方面，基于网格的方法允许无对象模型的表示，这有助于在具有大量对象的复杂场景中进行有效的碰撞检查。基于网格的表示对对象提取的缺陷（如假目标和未命中目标）的敏感性也较低。通过从基于网格的表示中提取对象假设，也可以混合使用这两种方法。

在此示例中，将周围环境表示为动态占用格网地图。动态占用网格图是对自我车辆周围局部环境的基于网格的估计。除了估计占用概率外，动态占用网格还估计每个像元的运动学属性，例如速度、转弯率和加速度。此外，动态网格的估计可以预测未来短时间，以评估不久的将来当地环境的占用情况。在此示例中，可以通过融合安装在 ego 车辆上的六个激光雷达的点云来获得基于网格的环境估计值。

二、设置场景和基于网格的跟踪器

此示例中使用的方案表示城市交叉路口场景，并包含各种对象，包括行人、骑自行车的人、汽车和卡车。自我车辆配备了六个均匀的激光雷达传感器，每个传感器的视野为90度，在自我车辆周围提供360度的覆盖。

现在，使用跟踪器GridRFS系统对象定义基于网格的跟踪器。跟踪器输出环境的对象级和网格级估计值。网格级估计值描述了本地环境的占用率和状态，并且可以作为跟踪器的第四个输出获得。

三、设置运动规划器

设置局部运动规划算法，以规划 Frenet 坐标中沿全局参考路径的最佳轨迹。

通过在驾驶场景的笛卡尔坐标系中提供航点，使用 referencePathFrenet（导航工具箱）对象定义全局参考路径。此示例中使用的引用路径定义了在交叉点处向右转动的路径。

此示例中的本地运动规划算法包含三个主要步骤：

1. 样本局部轨迹

2. 找到可行且无碰撞的轨迹

3. 选择最优性标准并选择最优轨迹

样本局部轨迹

在模拟的每一步，规划算法都会生成一个样本轨迹列表，供自营车辆选择。通过将自我车辆的当前状态连接到所需的终端状态来对局部轨迹进行采样。​使用轨迹生成器Frenet（导航工具箱）​对象连接当前状态和终端状态，以生成局部轨迹。通过为轨迹及时提供参考路径和所需分辨率来定义对象。该对象使用五阶多项式连接 Frenet 坐标中的初始状态和最终状态。

在 Frenet 坐标中对终端状态进行采样的策略通常取决于道路网络和自我车辆在全球路径的不同阶段的所需行为。在此示例中，您使用两种不同的策略对终端状态进行采样，具体取决于车辆在参考路径上的位置，在下图中显示为蓝色和绿色区域。

当自我车辆位于绿色区域时，使用以下策略对局部轨迹进行采样。自我车辆的终端状态之后断续器时间定义为：

 其中，变量的离散样本是使用以下预定义集获取的： 

在终端状态下使用使对象能够自动计算在最小抖动轨迹上行进的纵向距离。该策略产生一组轨迹，使自我车辆能够加速到最大速度限制 以不同的速率降速或减速至完全停止。此外，横向偏移的采样选择允许自我车辆在这些机动中改变车道。

当自我车辆位于轨迹的蓝色区域时，使用以下策略对局部轨迹进行采样：

 断续器选择以最小化轨迹期间的抖动。此策略使车辆能够停在所需的距离在右侧车道上，具有最小抖动轨迹。

寻找可行且无碰撞的轨迹

上一节中介绍的采样过程可能会产生运动学上不可行的轨迹，并且超过运动学属性（如加速度和曲率）的阈值。设置了一个碰撞验证器，以评估自我车辆是否可以在运动学上可行的轨迹上机动，而不会与环境中的任何其他障碍物发生碰撞。要定义验证程序，请使用帮助器类 。​ 此类使用对象的预测 MapToTime ​函数来获取周围环境占用率的短期预测。由于估计值的不确定性随时间增加，因此请将验证程序的最大时间范围配置为 2 秒。

环境的预测占用率在每一步都转换为膨胀的成本图，以考虑自我车辆的大小。路径规划器使用 0.1 秒的时间步长，预测时间范围为 2 秒。为了降低计算复杂性，假设周围环境的占用在 5 个时间步长或 0.5 秒内有效。因此，在 2 秒的计划范围内只需要 4 个预测。除了做出关于碰撞或无碰撞的二元决策外，验证器还提供了自我车辆碰撞概率的度量。该概率可以合并到最优性标准的成本函数中，以考虑系统中的不确定性，并在不增加计划员的时间范围的情况下做出更好的决策。

选择最优性标准

在针对障碍物或环境的占用区域验证可行轨迹后，通过定义轨迹的成本函数，为每个有效轨迹选择最优性标准。预计不同的成本函数将从自我车辆产生不同的行为。在此示例中，将每个轨迹的成本定义为

 Js是参考路径纵向方向上的抖动

Jd是参考路径横向的抽搐

Pc是验证者获得的碰撞概率

每个轨迹的成本计算是使用帮助程序函数帮助程序计算成本定义的。从有效轨迹列表中，具有最小成本的轨迹被视为最佳轨迹。

运行方案、估计动态地图和规划本地轨迹

运行该方案，从所有激光雷达传感器生成点云，并估计动态占用格网地图。使用动态地图估计及其预测来规划自我车辆的局部轨迹。

四、结果

分析本地路径规划算法的结果，以及地图中的预测如何帮助规划器。此动画显示整个方案中规划算法的结果。请注意，自我车辆成功地到达了所需的目的地，并在必要时在不同的动态对象周围机动。由于抽样政策增加了区域变化，自负车辆也在十字路口停了下来。

接下来，分析第一个车道变换期间的本地规划算法。本节中的快照是在模拟期间的时间 = 4.3 秒捕获的。在这张照片中，自我车辆刚刚开始在右侧车道上进行车道变换机动。

 

下面的快照显示了动态网格在同一时间步长中的估计值。网格单元格的颜色表示占据该网格单元格的对象的运动方向。请注意，表示自我车辆前方的汽车的单元格显示为红色，表示单元格被动态对象占用。此外，汽车在场景的正X方向上移动，因此基于色轮，相应网格单元格的颜色为红色。

 根据上一张图片，自我车辆的计划轨迹穿过空间的占用区域，如果执行传统的静态占用验证，则表示碰撞。然而，动态占用图和验证器通过根据每个时间步的预测占用率验证轨迹状态来解释网格的动态性质。下一个快照显示不同预测步骤 （断续器），以及自我飞行器在轨迹上的计划位置。预测的成本图被夸大以考虑自我车辆的大小。因此，如果一个代表自我车辆原点的点物体可以放置在占用地图上而没有任何碰撞，则可以解释为自我车辆不会与任何障碍物发生碰撞。成本图上的黄色区域表示有保证与障碍物碰撞的区域。碰撞概率在黄色区域外呈指数级衰减，直到暴胀区域结束。蓝色区域表示根据当前预测发生碰撞的可能性为零的区域。

请注意，表示自我车辆前方的汽车的黄色区域在成本地图上向前移动，因为地图是在未来预测的。这反映了占用率的预测考虑了周围环境中物体的速度。另请注意，在预测期间，分类为静态对象的单元格在网格上保持相对静态。最后，请注意，自我车辆原点的计划位置不会与成本图中的任何占用区域发生碰撞。这表明自我飞行器可以在这条轨迹上成功机动。

五、总结

在此示例中，学习了如何使用基于网格的跟踪器中的动态地图预测，以及如何将动态地图与局部路径规划算法集成，以便在动态复杂环境中为自我车辆生成轨迹。还了解了如何使用占用率的动态性质在环境中更有效地规划轨迹。

六、程序

使用Matlab R2021a版本，点击打开。

打开下面的“MotionPlanning...Example.mlx”文件，点击运行，就可以看到上述效果。 

关注下面公众号，后台回复关键词：基于动态占用网格图运动规划仿真，发送源码链接。