【路径生成 + 轨迹优化--无人驾驶场景规划器】OpenPlanner算法

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

在项目和平时的学习中，我对机器人/无人驾驶的决策规划模块进行了划分，当然划分的方法有很多，我的划分方式仅供参考
（1）动态障碍物行为预测模块（Behavior prediction）–结合感知和高精度地图信息，估计周围障碍物未来运动状态
（2）执行机构的轨迹规划模块（Trajectory_planning）–执行机构如机器人载体上的机械臂、串行云台等的运动轨迹规划
（3）任务决策模块（Mission_planning）–任务决策模块比较偏业务层了，处理机器人/无人驾驶的各种任务，主要分为三个方面：车底盘航线业务决策（交规、横向换道等等）、执行应用机构业务决策（机械臂、人机交互等等）、不同场景的导航方案切换决策（组合导航、融合导航）
（4）前端路径探索模块（path_finding）–全局路径规划算法难度不算复杂，找到一条可通行的（必须满足）、考虑动力学的（尽可能满足）、可以是稀疏的路径base_waypoints【由于其只考虑了环境几何信息，往往忽略了无人机本身的运动学与动力学模型。因此，其得到的轨迹往往显得比较“突兀”，并不适合直接作为无人机的控制指令】
（5）后端轨迹处理模块（motion_planning）–我主要归纳整理为三个方向：（1）对base_waypoints进行简单处理及生成方向、（2）对base_waypoints轨迹优化方向【一般是二次优化，这里用的较多的事优化方面的知识】、（3）进行对应功能的replan方向（replan之前的预处理、进入replan的条件、停障replan、避障replan、纠偏replan、换道replan、自动泊车replan、穿过狭窄道路replan等等），这部分内容使用的方法比较专
（6）路径跟踪模块（trajectory_following）–这个模块就得针对机器人载体了，如无人驾驶使用得阿克曼模型可以采用几何的pure pursuit纯追踪算法，更好的可以用模型预测控制MPC方法，还有强化学习做的（效果怎样我就没验证过了）；当然也可能事麦克纳姆轮车、差速车PID、还有无人机的三维轨迹跟踪等等
（7）碰撞检测模块
（8）集群多机器人规划模块

当然，这种划分方式是我权衡了原理和功能粗略划分的，在实际产品研发过程中，需要理解了各个算法的功能和定位的基础上融汇贯通，不能生搬硬套，如全段通过hybrid A探索出来的路径与A、RRT*探索出来的路径更平滑，后端轨迹优化的任务就不用这么重了；又如，机器人/无人驾驶项目研发的需求业务还没发展到能响应很多功能阶段，任务决策使用简单的状态机（fsm）就可以对现有任务进行状态转移了

本文先对OpenPlanner算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

一、OpenPlanner整体效果

二、OpenPlanner介绍

1.OpenPlanner介绍

规划执行最优路径，同时避免障碍，触发行为例如在交通信号灯处停车

2.OpenPlanner开源框架的特点

（1）OpenPlanner框架足够通用，可剪裁才增加，【防盗标记–盒子君hzj】只需调整其参数即可与任何移动机器人配合使用
（2）OpenPlanner更适合于遵守交通规则的自主移动机器人导航系统，更适用于车道线场景的自动驾驶领域，OpenPlanner通用性强，只需相应的调整参数即可部署在一台新的无人驾驶车上
(3)OpenPlanner它结合了矢量地图或道路网络地图中包含的所有离散信息（例如交通信号灯，交通标志，十字路口，停车线等的位置），这相对于其他的开源的导航系统OMPL和Navigation Stack来说是主要的优势
事先有一张矢量地图，车载还有激光雷达和相机的detector模块
（4）OpenPlanner不完全适合杂乱无章的农业场景，【防盗标记–盒子君hzj】需要改造，这里就是论文的切入点，农业场景它仅需要矢量地图和目标位置即可进行全局规划，而对于局部规划和行为状态生成，则仅需要当前位置和检测到的障碍即可

3.Autoware和OpenPlanner的关系

Autoware是ROS软件包的集合，包括OpenPlanner，以及其他功能包
.
.

三、OpenPlanner的整体框架及原理

1.感知模块支持

通过相机detect，得到障碍物信息（detected obstacles）、交通标志（traffic light color）等
.
.

2.地图与定位模块支持

通过SLAM和定位模块提供到高清矢量地图（vector_map）【防盗标记–盒子君hzj】（基于用传感器信息更新的代价地图）和高精度定位信息（goal_pose、start_pose、current_pose）

3.全局路径规划（分场景使用不同的规划算法）

目的：全局路径规划的分场景的，不同的场景用不同的路径规划算法，最终得到一条静态的全局参考路径

（1）结构化的环境场景的规划–根据矢量地图选择生成一条全局路径

1）场景
在结构化环境中，我们必须遵循交通规则，例如在车道中心驾驶，向正确的方向行驶，仅在被允许进入正确的车道以向右转或向左转时才可改变方向，在该环境中我们可以清楚地定义道路，行车线，十字路口等，以及交通标志，所有这些都可以在矢量地图中进行描述。

2）规划方法
仅仅用到vector_map就可以实现全局的路径规划，open_planner的全局路径是从vector_map上得到的，，当然矢量地图中有多条lane车道，在哪条lane车道形式还是要用图搜索的方法确定，【防盗标记–盒子君hzj】遇到障碍物的时候截取一段做局部路径规划

常用的算法类型：A*算法、Dijkstra算法

使用（A*算法）找到从当前位置到目标所有的可能路线。【防盗标记–盒子君hzj】 在路线跟踪过程中，我们构建了可能的路径树，遵循矢量地图定义的规则，直到车辆到达目标为止，一旦达到目标，便会追踪从目标到起始位置的路线，进而选出唯一全局参考路径

在地图上搜索最短路径。 线条颜色表示路线代价
.
.
.

（2）非结构化环境场景的规划–根据路径探索生成一条全局路径

非结构化环境场景包括自由空间的平地场景、越野驾驶场景、停车泊车场景，即我们无法使用矢量地图的位置

使用向量图可以更轻松，更快地进行全局和局部规划。【防盗标记–盒子君hzj】当然，像RRT * 和Hybrid A *这样的自由空间全局规划器对于停车和越野情况都很重要。在这种情况下，我们可以切换到自由空间规划器进行全局规划，而仍然使用Open Planner的行为状态机和本地规划器

常用的算法类型：RRT * 和Hybrid A *
.
.

（3）全局路径规划源码分析

待补充

4.局部路径规划（open_planner）

（1）原理介绍

将高清矢量地图和定位数据作为先验信息输入，在全局中，指定一个起点，一个终点，通过全局规划得到一条全局静态路径，全局路径规划在车辆启动或重启的时候被确定，一旦分配了全局路径，本地马上启动运动规划器

根据静态的全局参考路径，采用轨迹生成算法Roll-outs Generator，得到局部多条平滑的采样轨迹（候选的一系列轨迹用到了共轭梯度下降轨迹优化平滑的方法），【防盗标记–盒子君hzj】再通过障碍物检测与代价函数Roll-outs Evaluator方法计算每条采样轨迹的归一化代价函数，挑选一条代价最低且局部最优路径。最后通过纯跟踪算法将规划出来的轨迹用于实际控制

op_planner的local_planner主要分为两部分：Rollouts Generator 和 Rollouts Evaluator

（2）Roll-outs Generator【局部路径探索+轨迹优化】

1）作用
Rollouts 的含义就是根据中心全局路径生成的一些列候选局部路径，Rollouts Generator根据全局中心路径生成一系列平滑的候选局部路径
根据矢量地图生成的全局路径作为参考生成一系列平滑的候选路径

---

2）确定输入输出
候选路径生成算法的输入是当前位置、规划距离、【防盗标记–盒子君hzj】生成候选路径的数量和全局路径。输出为n条平滑轨迹，从车辆中心一直延伸到最大规划距离

---

3）确定规划区域

（1）Car tip：从车辆中心到横向采样点的距离，其长度决定了车辆切换到不同轨迹的平滑度
（2）Roll in：水平横向采样点到平行横向采样点的距离，其长度与车辆速度成正比。车辆行驶的速度越快，此区域产生平滑变化的时间就越长
（3）Roll out：平行横向采样点到最大规划距离，通过从全局路径垂直移动固定的距离（称为Roll out密度）来执行直接横向采样

---

4）Roll-outs Generator三个主要步骤

（1）根据车辆的当前位置和最大规划距离从全局路径中提取所需的car tip、Roll in、Roll out三个部分；
（2）针对提取出的全局路径进行点的横向采样；对与全局路径的提取的三个部分相对应的新的垂直航路点进行采样。 采样从横向距离为零的车头边缘开始，然后逐渐增加，【防盗标记–盒子君hzj】以达到在滚动边际末尾使用每个轨迹指数计算得出的滚动密度。
（3）使用共轭梯度对每个采样轨迹进行平滑处理生成候选轨迹，共轭梯度是一种非线性迭代优化技术，可消除采样步骤导致的滚动不连续性。 这也改善了曲率，从而使转向更平稳

---

.
.

（3）Roll-outs Evaluator【代价函数挑选最优路径】

1）轨迹选择图例

---

2）作用
结合障碍物信息和其他因素计算各个Rollout的优先级代价、【防盗标记–盒子君hzj】过渡代价和碰撞代价评估每一条从Roll-outs Generator生成的候选路径，从而挑选出一条平滑、无障碍的局部路径

---

3）障碍物检测
（1）边界框检测方法
通过使用边界框，我们可以显着提高障碍物检测性能，但会牺牲准确性

（2）点云数据簇检测方法【推荐】
使用点云数据可以大大提高检测精度，仅使用点云数据簇的轮廓点样本来解决性能降低问题，每个障碍物最多16个点。轮廓点的最大数量是局部规划器的参数之一，【防盗标记–盒子君hzj】通过增加该数量，我们可以实现更精细的表示，从而可以更准确地避开障碍物

---

4）确定代价类型
（1）中心代价
中心代价每次roll-out都使用距该车道中心轨迹的绝对距离来计算，中心代价会限制车辆始终沿车道中心行驶，其他轨迹的渐变颜色代表代价

中心代价是对每条候选路径进行分级，中心路径的优先级是最高的，然后由中心路径向两边递减，这是为了在没有障碍物的情况下，让车辆保持在中心路径

（2）过渡代价
使用展开之间的归一化垂直距离以及当前选择的轨迹来计算过渡代价
过渡代价限制了车辆的roll-in，从而使转弯更平稳。
过渡代价是为了限制车辆不会频繁在候选路径之间切换，【防盗标记–盒子君hzj】这是为了确保车辆前进的平滑度

（3）碰撞代价
碰撞代价分两部分lateral_cost和longitudinal_cost来计算，以提高性能。lateral_cost候选路径到障碍物的横向水平距离，longitudinal_cost候选路径到障碍物的纵向水平距离

---

.

5）代价归一化处理
使用附加代价函数来评估每个轨迹，代价归一化处理计算三种不同的归一化代价度量，【防盗标记–盒子君hzj】包括代价，碰撞代价和过渡代价，避免评价函数中某一项代价占据过大的优势
.
.
6）通过代价函数计算代价，挑选出一条整体代价最低的一条局部路径
避开障碍的过程是展开轨迹中，先检测障碍物，再通过代价函数评估每一条展开的轨迹，最终选择出一条最优的轨迹

（4）局部路径规划器的跟踪路径

到这里得到无人车线控底盘的速度（speed）、角度（angular）指令
1.纯跟踪pure_persuit算法
原理和代码写在了我其他文章中…
.

2.模型预测控制MPC算法
原理和代码写在了我其他文章中…
.

（5）局部路径规划源码分析

后面补充

四、行为状态生成器【导航决策】

1、行为状态介绍

在实际开放道理场景下，自动驾驶要处理的场景非常繁杂：空旷的道路场景、与行人、障碍物共用道理的场景、空旷的十字路口、繁忙的十字路口、违反交通规则的行人/车辆、正常行驶的车辆/行人等等。场景虽然复杂，但都可以拆解为一系列简单行为(behavior)的组合，将这些简单的行为(behavior)组合起来，就可以完成复杂的驾驶行为

行为生成器使用预定义的交通规则和传感器数据充当协调器，【防盗标记–盒子君hzj】在局部路径规划过程中，根据道路的事件，触发行为生成器，当满足状态机的转换条件时，无人车进行路径的重新规划和速度的重新规划等，以响应道路事件。其中局部最优路径拼接起来大体时服从全局静态路径的

2、行为状态（也称任务、目标、状态）的定义

像交通规则一样，道路的事件本质上是确定的，【防盗标记–盒子君hzj】我们将对这些事件的响应称为行为，任务，目标，状态或情况，我们使用术语“行为状态”来表示所有这些事件响应

3、行为状态生成器的作用

处理诸如路径跟踪，目标跟踪，避障，紧急停车，在停车标志处停车和交通信号灯判断、改变车道、等待行人之类无人车的行为状态，重新规划无人车的相应行为

4、行为状态生成器步骤

（1）行为状态机的设计

1）背景
每个国家的交通规则有所不同。【防盗标记–盒子君hzj】因此需要随时添加或禁用特殊的交通规则。行为状态机的功能很难概括，因为它们的用法因机器人而异。OpenPlanner还提供基本的行为状态机功能，添加新状态与使用ROS一样容易。

2）交通状况和状态机的关系
OpenPlanner的行为状态生成模块充当系统的决策者。 它是一个有限状态机，其中每个状态代表一个交通状况

3）状态机的离散表示
通常使用状态机来表示任务，研究人员将连续驾驶行为状态转换为离散状态空间，然后使用搜索算法获得最佳任务序列，以在符号空间中达到目标条件

（2）状态机及状态转换条件

1）状态机转换条件

2）状态过渡参数
1）状态过度参数在每次迭代都要计算更新

2）计算控制状态过渡参数的方法
1、概率方法
从理论上讲，概率方法应该使过渡更平滑，【防盗标记–盒子君hzj】但是在广泛的应用程序中实现和维护它更慢，更复杂。

2、计时器和计数器方法【我们使用的】
解决此问题的一种方法是引入计时器和计数器

3）状态之间的转换由当前状态和状态过渡参数控制

（3）重新规划【结果】

行为生成器可以随时要求重新规划
（1）重新规划路径
（2）重新规划速度
.
.
.

---

总结

open_planner更适合用于无人驾驶（autoware）结构化和非结构化的场景

---

参考资料

OpenPlanner参考论文

Open Source Integrated Planner for Autonomous Navigation in Highly Dynamic Environments

OpenPlanner参考链接

https://blog.csdn.net/Travis_X/article/details/104607163
https://blog.csdn.net/Travis_X/article/details/115350840

视频展示

https://www.bilibili.com/video/BV1pL4y1Y78e?spm_id_from=333.999.0.0
https://www.bilibili.com/video/BV1if4y1V7Vw?spm_id_from=333.999.0.0