系列文章目录

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

文章目录

（3）应用案例

（4）总结

2、【多选一】OpenPlanner

三、OpenPlanner的整体框架及原理

4.局部路径规划（open_planner）

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

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

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

五、总结

六、参考资料

2.lexicographic_planner

2.路径优化的步骤

三、lexicographic_planner源码解读

四、总结

五、参考资料

---

前言

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

本文先对线性加权轨迹代价损失评分，轨迹多选一方法–DWA_planner、Open_Planner、lexicographic_planner做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

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

1、设计代价损失函数进行评分、线性加权法、迹评分轨迹多选一的轨迹优化设计思想

（1）设计代价函数进行评分、然后选择代价最低的轨迹核心思想

若是有多条候选的局部路径，进行轨迹选择

（1）这种方法【防盗标记–盒子君hzj】并没有用到优化理论的知识–本质是从多选优
（2）更好的多准则问题，通常是考虑了时间、能量、行进距离、碰撞、交通规则等更多约束的路径
.
.

（2）代价评分的方向

1.代价函数挑选最优路径

在采样节点的形成搜索树/搜索图时候，【防盗标记–盒子君hzj】对搜索树/搜索图的边赋予代价信息
.
.

2.代价函数初始化节点的代价值

在采样节点的形成搜索树/搜索图时候，【防盗标记–盒子君hzj】对搜索树/搜索图的节点赋予代价信息
.
.

3. 一般的代价函数（loss function）的类型（具体公式后续补充）

（2）子代价函数的考虑可能结合以下因素
路径长度、曲率平滑度、速度平滑度、车辆偏离车道中心线（目标位置距离）、与障碍物的距离、安全性、对车辆的压力等等。

举例：换道路径曲线的筛选原则–曲率


.
（1）L2 loss代价函数

---

（2）L1 loss代价函数

---

（3）Huber loss代价函数

---

（4）Barrir loss代价函数

（5）单项式函数
（1）绝对值平方V型函数（优化函数）
（2）二元一次抛物线函数（优化函数）
（3）一元二次直线函数（优化函数）
（4）更复杂的优化函数…（建议看看TEB的五个目标函数）

（6）多项式函数
把多个单项式函数累加起来，并为每个项赋予一个权重

4.通过线性加权的方式计算总代价，挑选出一条整体代价最低的一条局部路径【实现多目标代价】

比例分权方法求总的代价【本质用在最优控制中的多准则问题】,比例分权方法的一般套路如下：

举例一下：比例分权方法求总的代价

（3）应用案例

（1）矢量地图路线的选择评分
（2）open_planner的路径评分
（3）DWA的路径选择评分

（4）总结

1、工程常用方法：线性加权法–如上面的计算路径长度和路径平均曲率两个目标的代价
线性加权法并没有用到优化的知识–从多选优

2、学术常用方法：利用了优化理论的NSGA、Pareto最前沿

.
.

2、【多选一】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）结构化和非结构化的场景

Open_planner也是使用增量式采样的方式进行路径探索和后端轨迹选择的

---

六、参考资料

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

.
.

2.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

---