【路径生成 + 轨迹优化--水上无人船\无人驾驶车场景】lexicographic_planner算法

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
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）就可以对现有任务进行状态转移了

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

---

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

一、lexicographic_planner介绍

open_planner更适合用于无人驾驶车道线场景，【防盗标记–盒子君hzj】lexicographic_planner更适合于农业的一垄一垄林场和仓库的场景，或者数原论文中阿无人船场景【属于replan局部规划的范畴，因为全局是给定路线的】

二、lexicographic_planner的整体流程

1.路径探索的步骤

（1）伪代码：

（2）步骤

（1）步骤一：当在规划过程的开始时收到全局路径G时，我们赋值全局路径globalPathMessage为探索路径并executePath
【planning中收到全局路径时的回调函数处理】

---

（2）步骤二：当机器人未完全执行全局路径G时，我们使用感知到的传感器数据S【局部代价地图】检查探索路径σ的可行性

---

（3）步骤三：（仅仅执行一次）判断给定的全局路径和所有障碍物是否相撞【即判断全局路径是否能被直接执行】，如果不能被直接执行且障碍物位于全局路径g上，【防盗标记–盒子君hzj】我们就以全局路径g为参考，生成候选的机器人规划的起点v（generation nodes V）

---

（4）步骤四：通过传感器以代价地图的方式更新障碍物的信息，【防盗标记--盒子君hzj】当机器人到达规划的起点v时，开始规划一条新的路径，具体replan的步骤如下：

【第一步】通过采样的（类似PRM）方法，生成一张有向的搜索图
这里和open_planner很像，但是open_planner不生成有向搜索图。lexicographic_planner会确定rollout的密度和数量，然后把候选的路径相互连接形成有向搜索的，生成rollout候选路径的采样方法是和open_planner一致的，只是lexicographic_planner多了把候选路径相互连接成有向图的过程
（类似于open_plannerde rollout阶段+PRM采样建立有向图的阶段）。【防盗标记–盒子君hzj】但注意的是，采样的过程不是用PRM均匀采样的方式，而是根据关键参数d(span)，d(roll)和d(sensor)决定采样范围

采样函数buildAdjacencyMatrix()
建立一张PRM有向搜索图函数connectAdjacencyMatrix()
.
.
【第二步】lexicographic search在有向搜索图上进行路径搜索
（相当于PRM的有向图搜索阶段-搜索的方式使用Dijkstra，当然原理懂了图搜索的方法可以改）若搜索到一条可通行的路径，则输出可行路径。若搜索不到可通行的路径，则停下来并返回机器人当前的状态

（1）第0行：输入采样得到的有向搜索图、机器人的起始位置、终点位置
（2）第1行：根据有向图在队列Xqueue中填充图的节点
（3）第2~4行：为每个节点进行代价初始化，其中代价的初始值根据词袋优化代价的方法进行计算
（4）第5~24行：使用Dijkstra的方法进行路径探索，得到一条可通行的路径

---

.
.

2.路径优化的步骤

词典优化的方法将导航过程中需要解决的各种挑战建模成最小化代价，这样就将规划问题转化为多目标的优化问题。通过一种词典式高效的多目标搜索算法对所有目标进行分层排名快速解决多目标的规划问题，而无需对参数进行调整【防盗标记–盒子君hzj】

（1）代价函数的设计

1、碰撞代价设计
（1）作用：保证机器人和其他车辆的安全
（2）碰撞代价函数的设计逻辑

希望在机器人与障碍物之间放置一块安全区域，机器人尽量避开该区域，以最大程度地减少其对其他船只的影响。创建该区域的另一种方法是单纯地对障碍区域进行膨胀。 但是，【防盗标记–盒子君hzj】如果障碍物之间离得很近，膨胀后也可能阻塞整个通行区域，即使是有一条可行的路径可以在它们之间通过

（3）沿着全局路径σ的风险碰撞计算公式

其中函数Risk函数评估单个机器人状态下的风险。假设R（x）定义为x与离x的最近障碍物之间的距离的倒数。如果R（x）大于风险阈值T hrisk，则会激活Risk（）函数。例如，让T hrisk = 2，当机器人在障碍物的0.5 m以内时，Risk（）会给出非零值。令让Thrisk = ∞时，在C（free）中任何一处Risk()都会返回0。

---

2、航向代价设计
（1）作用：惩罚了机器人规划的航迹和全局参考路径之间的航向差异，正确的打方向
（2）航向代价计算公式：

其中函数H（x）给出x与全局航线G上最接近x的路径段的航向之间的航向差。由于风浪干扰，将机器人的航向与G完美对齐几乎是不可能的。 为了避免全面的控制工作，我们定义了航向差异阈值Thhead。 【防盗标记–盒子君hzj】当误差H（x）大于T hhead时，Heading（x）返回一个非零值。 合并此代价可确保生成的路径在保持全局路径G航向的同时能相对平滑

---

3、行进距离代价设计
（1）作用：将行进距离定义为第三代价，严格来说这是正数。 这样可以确保不会像主要或次级代价那样频繁地发生
（2）行进代价计算公式：

---

（2）通过代价函数挑选最优路径【词袋优化的方法】

当两个或多个约束同时存在时会对这些约束进行分级惩罚的管理，【防盗标记–盒子君hzj】优先级高的约束承担惩罚的代价也就越高，当主要的惩罚代价没增加时，则会引入次要的代价或者次次要的代价

定义总代价函数为ck(σ)， ck : Σ → R(+0)，k∈ {1, 2, …, K}, K表示在多代价目标规划中子代价函数的数量，这些子代价函数适用于词典优化

3.lexicographic_planner与open_planner的对比

生成局部路径的思想大体和open_planner一致，但是主要不同在于：
（1）【路径收敛的策略 】open_planner在roll-out阶段生成的轨迹是不会收敛到给定路径G的，但是lexicographic_planner在roll-out阶段生成的轨迹会收敛到给定路径G

---

（2）open_planner和lexicographic_planner生成多条候选探索路径的方法是一样的，【防盗标记–盒子君hzj】只不过路径挑选的方法不一样，open_planner用代价优化的方法选择其中一条，lexicographic_planner用多条路径相互连接形成一张有向搜索图，用Dijkstra图搜索的方法探索出一条最优平滑路径

---

（3）【路径挑选的策略】open_planner在挑选路径的时候，使用了代价函数的方式对每一条候选路径进行评价挑选（因为open_planner没有建立搜索图），但是lexicographic_planner通过open_planner生成的多条候选路径的基础上，把每条候选路径上的邻接节点相连接，建立了有向搜索图的基础上使用了A*（Dijkstra）的方法唯一进行路径的探索【有向图建立+Dijkstra=PRM的思想】

---

（4）【代价函数用的方向策略】open_planner的代价函数用于评价挑选候选路径，lexicographic_planner的代价函数用于初始化采样节点的代价，建立带有代价的有向搜索图

4.lexicographic_planner的效果图

三、lexicographic_planner源码解读

1.输入输出

输入规划器将机器人的当前状态xc，全局参考路径G和感知传感器数据S（局部地图、定位）
输出为n条平滑轨迹，从车辆中心线一直避障延伸，最后又收敛回到车辆中心线

2.需要调试的参数

在工程的.yaml文件
其规划范围由关键参数d(span)，d(roll)和d(sensor)决定
d(span)是采样状态和全局路径G之间的最大距离，实际上，【防盗标记–盒子君hzj】在障碍物覆盖的区域d(roll)设置的值比传感器的范围要大
d(roll)是roll-out和oll-in部分之间的总距离
d(sensor)是传感器能探测的距离

3.lexicographc_planner的运行过程

lexicographc_planner本来是在给定的全局路径下执行巡航的，没有检查到障碍物就继续执行跟踪全局路径的功能，检查在全局路径上到有障碍物才启动replan，planning先在全局路径上截取一小段局部路径（由d(sensor)参数决定），从而确定planing的起点和终点，然后就按着lexicographic的plan计划来
（1）【障碍物在全局路径上，启动重规划】obstacles on path,replanning
（2）【撞到障碍物或者规划失败，停下来】planning fail,no path found,stay
（3）停下来后需要重新给定新的目标点（起点就是机器人现在位置）

4.lexicographic_planner源码解读

先写下框架，到吗后续补充~
（1）_cloudRegister（处理三维点云，转换成全局坐标）
功能：负责接收点云信息的处理，包括坐标转换、下采样、滤波等等（大多数时用PCL库实现的），最后把处理后的点云数据发布出来

---

（2）_obstacleServer（生成局部代价地图）
功能：把三维点云地图转换成为二维的栅格地图（并带有代价值和膨胀层）【点云接收回调函数中处理】

---

（3）_pathServer（全局路径加载）
功能：创建原始全局路径（点）发布器的与平滑全局路径发布器，同时创建一个定时器，一秒执行一次updatePath()函数更新全局路径，全局路径根据机器人位置不断在两个半圆路径上进行切换，并发布平滑后的全局路径

---

（4）_pathPlanning（局部规划）
1）订阅全局参考路径回调函数【void pathHandler(const nav_msgs::Path::ConstPtr& pathMsg)】
功能：确定原始全局路径作为搜索路径，进行openPlannerRollOut的过程，生成多条候选路径

2）订阅局部代价地图回调函数【void mapHandler(const nav_msgs::OccupancyGrid::ConstPtr& mapMsg)】
功能：把局部代价地图转存到内存变量中occupancyMap2D

3）定时更新路径（与上面的原理一致）【void updatePath(const ros::TimerEvent& event)】
（1）通过采样，建立邻接矩阵【buildAdjacencyMatrix ();】
（2）通过连接邻接矩阵，建立一张PRM有向搜索图【connectAdjacencyMatrix (); 】
（3）在有向搜索图（邻接矩阵）进行搜索，得到一条新的局部路径【searchAdjacencyMatrix ();】
（4）可视化【visualization (); 】

---

总结

1.优点

（1）lexicographic_planning的横向搜索范围可以改【像open_planner一样改】
（2）lexicographic_planning在规划中间的区域，效果是和理想的

2.缺点

（1）lexicographic_planning在规划最后收敛搜索的时候若是遇到了障碍物，很容易就会规划失败
（2）【局部地图问题】使用二维的激光雷达，【防盗标记–盒子君hzj】只能扫出一个平面的点云信息，激光雷达过高或者过低都没办法得到有效的局部代价地图
（3）lexicographic_planner算法本身时没有什么问题的，但是不足的地方就是用了move_base（DWA_planner）,导致局部避障的时候会运行两个局部规划器，先是lexicographic_planner，后面差速车执行的靠DWA_planner，最后执行的路径是两个规划器的结果，相互影响，难以调试

参考资料

1、论文

A Receding Horizon Multi-Objective Plannerfor Autonomous Surface Vehicles in Urban Waterways
https://arxiv.org/abs/2007.08362

2、代码

https://github.com/TixiaoShan/lexicographic_planning

3、博客参考

https://blog.csdn.net/Travis_X/article/details/115485931

4、视频参考

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