ROS局部路径规划器插件teb_local_planner规划流程概括总结

---

---

   本部分文章主要用于对ROS局部路径规划器插件teb_local_planner的规划流程进行梳理，并概括总结，本部分计划包含5~6篇文章，各文章主要内容如下：

---

   ☆第一篇：主要介绍ROS导航包Navigation中的 Movebase节点中的路径规划的相关流程，并对其进行梳理概括

   文章链接如下：

   ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理

–

   ☆第二篇和第三篇：主要介绍ROS局部路径规划器插件teb_local_planner工作过程的详细流程

   文章链接如下：

   ROS局部路径规划器插件teb_local_planner流程梳理（上）

   ROS局部路径规划器插件teb_local_planner流程梳理（下）

–

   ☆第四篇：也就是本文，主要对ROS局部路径规划器插件teb_local_planner规划流程进行纯文字的概括总结，是对第二三篇文章的进一步概括和总结，此外还补充了一些使用teb_local_planner过程中常见报错或警告信息发生的阶段。

   文章链接如下：

   ROS局部路径规划器插件teb_local_planner规划流程概括总结

–

   ☆☆☆第五篇：主要对teb_local_planner规划器插件的各个参数进行详细的介绍，并分析它们在teb的规划流程中发挥着怎样的作用，是怎么发挥作用的，以及各个参数之间的相互影响，我对这篇文章的定位是teb_local_planner路径规划器参数调试的参考手册。

–

   目前该手册的初版已完成，目前共38页word，12061字，是根据对teb_local_planner规划器源码进行了详细的阅读及理论分析后，以及加入了我实际调试的一些经验后，编写而成。

   后续会在调试中，根据新的经验，进一步对该手册进行完善，因保密需要，本手册暂不公开，后续合适的时机会进行公开。

---

---

   接下来，正式开始本文主要内容的介绍，teb_local_planner规划流程，大体可分为三个阶段，第一部分是调用plan函数进行规划前的准备工作，第二部分是轨迹规划部分，也是核心部分，第三部分是得到经过g2o优化后的轨迹后，计算速度指令，下发给下位机进行控制。

   以下是这三个部分的概括总结：

–

   第一部分：规划前的准备工作

   首先对插件进行初始化，并获取机器人的当前位姿信息和速度信息，接着从接收到的全局路径点中初步选取局部路径点，然后判断机器人是否到达目标点（正常的迭代停止条件），然后判断是否进入恢复行为模式，然后根据参数判断是否要调整局部目标点的角度使其更加平滑，然后将机器人的当前位姿插入到局部路径点中，并根据参数选择更新障碍物信息的方式对障碍物信息进行更新。

   其中从接收到的全局路径点中初步选取局部路径点的过程，主要包含以下三步，首先裁剪掉已经走过的全局路径中与机器人当前位置到的距离大于设定阈值的点、然后裁剪掉与机器人当前位置的距离大于设定阈值或者从当前位置点开始向前累计的路径点的长度大于设定阈值的点，并同时将路径点的坐标系转换为局部代价地图下的坐标系，最后，对剩下的路径点进行稀疏化处理，即删除间隔小于设定阈值的路径点。在后续过程中还将机器人当前的位姿点插入到了路径点中，至此，初步得到了用于局部路径规划的路径点。

   第二部分：轨迹规划部分

   首先，进行轨迹的初始化，若接收到新的目标点，且新目标点与上一个目标点的距离差或姿态差大于设定阈值时，需要在规划前对轨迹进行初始化，若目标点没有变化，或者变化小于阈值，则认为是机器人移动过程中针对同一目标点的多次规划，此时采用所谓的热启动模式，只需要对已经走过的部分的路径点进行裁剪就可以了，不需要重新进行轨迹初始化。然后从里程计读取当前速度，作为起点处的速度，根据是是否允许在目标点处速度不为0来设定目标点处的速度，若不允许，则将目标点处的速度设为0，接着判断若允许则在构建图之前再优化一下局部路径点，则对局部路径点进行优化，然后正式开始图的构建，包括添加顶点和添加边两个过程，添加的顶点即路径点（包括姿态信息和时间差），添加的边即各种约束条件，构建好图后，便按照设定的内循环次数调用g2o对图进行优化，然后清空图，按照设定外循环次数，循环执行优化局部路径点、构建图、优化图、清空图这几个步骤。执行完外循环后根据参数决定是否计算当前的代价值，包含时间成本和障碍物成本。至此，得到了经过优化后的局部路径点，也就是优化后的轨迹。

   其中，轨迹初始部分包括以下内容：首先将当前位置处的姿态点添加到局部路径点中，并设置为固定，即优化过程中不能被改变。然后根据是否允许开始时后退来执行轨迹设置后退标志位，接着若允许覆盖全局路径中的局部路径点朝向，则根据局部路径点中相邻两个点之间连线的朝向，利用atan2函数重新计算每个局部路径点额朝向，若不允许覆盖，则保留局部路径点的原朝向。然后利用设定的最大线速度和角速度以及相邻两个路径点之间的距离，通过距离/速度，估计出相邻两个路径点之间的线运动时间差和角运动时间差，并取线运动时间差和角运动时间差中的较大值，作为这两个相邻路径点之间的时间差。然后判断是否需要强制添加局部路径点，若当前的局部路径点数量小于设定的阈值，则为了保证生成路径的路径点数量足够多，会不断在当前点与目标点之间取中点添加到局部路径点中（当前点会变化），直至局部路径点的数量等于设定的阈值。最后，将目标点添加到局部路径点的末尾处，并将其设为固定，即在优化中不能被改变。轨迹初始化完成。

   其中，构建图之前对局部路径点的优化包括以下内容：在轨迹初始化部分中，我们根据局部路径点中相邻两点的距离和最大线速度和角速度估计出了该两点间的运动时间，现在我们需要对两点间的运动时间间隔进行检查，如果某个时间间隔大于参考时间间隔加上时间滞后，且采样数小于最大采样数，则在此处插入一个新的路径点；如果时间间隔小于参考时间间隔减去时间滞后，并且采样数大于最小采样数，则删除此处的路径点。在进行这些插入和删除操作后会调整插入和删除点之间的时间间隔。如果参数fast_mode为真（ fast_mode由外部参数 include_dynamic_obstacles取反后得到），则会在一次迭代中只进行一次调整，即在本次对局部路径点的优化中，调整过一个路径点后，就返回，否则将进行多次循环，直到没有点需要调整或者达到设定的最大循环次数100才返回。

   其中，构建图时添加的边包括障碍物边、动态障碍物边、经过点的边、速度边、加速度边、最优时间边、最短路径边、机器人运动形式边（运动学约束）、首选旋转方向边、速度障碍物比边。

   第三部分：计算下发给下位机的速度指令

   首先，还是需要进行一些准备工作，首先，若机器人的外形设定为动态外形，则需要更新机器人的外形信息以及从机器人中心到其外形边缘顶点的最小和最大距离，接着检查优化后得到的路径是否可行，可行性检测通过后，正式开始计算速度指令，将得到的速度进行限幅检查，然后根据设定的参数决定是否需要将机器人的角速度指令转换为舵机的角度指令。最后将得到的线速度和角速度/角度发布出去，至此TEB的流程结束。

   其中，检查优化后得到的路径是否可行的过程如下：根据外部参数 feasibility_check_no_poses的值决定检查轨迹中路径点的个数，比如其值为4，则检测轨迹中前4个路径点处是否与障碍物相交，需要注意的是，会把机器人的外形轮廓放置到该路径点处进行碰撞检测，而不是仅仅对该点进行检测，若有碰撞，则返回可行性检测未通过，将线速度和角速度设置为0，重置规划器，开始新的规划。此外，同时还会判断局部路径上每个点之间的距离以及朝向角度差，这个距离不能超过机器人的长度，如果超过了机器人的长度则点上的模型不能完全覆盖机器人路径，可能会存在说两个姿态点处没有碰撞，但是两点间的路径之间存在障碍物的问题，这样仅仅对点进行判断是不能完全保证路径可行的。此时，要对路径按照机器人长度进行插值，直到模型覆盖整条路径为止。角度的问题也是一样的。

   其中，速度的具体计算过程如下：从局部路径点容器中依次累加两点间的时间差直至大于时间分辨率 ，至此找到了局部路径点容器中和起点之间时间差大于时间分辨率的点，该过程还会受参数 control_look_ahead_poses（默认为1）的影响，这个参数决定了最多累加几次，默认为1时，找到的点其实就是局部路径点中起点和其后面的相邻点，无论他们的时间差是否大于dt_ref，因为只允许累加1次。 然后，根据这两个点的位姿信息、时间差、当前的速度信息，调用extractVelocity函数，计算并更新速度信息，对于非完整约束的类车型机器人，其y轴速度为0，x轴速度为两点间距离除以时间差，若为完整约束的机器人，即y轴上的速度可以不为0在，则x轴与y轴速度分别为两点在x轴与y轴的距离差除以时间差，角速度为姿态角差除以时间差。返回true

   其中，速度限幅的具体过程如下：根据参数use_proportional_saturation进行处理，若该参数为默认值false，则对x、y轴速度及角速度分别按各自的比例进行处理，效果等效于若超过其对应的最大值，则限幅为其最大值。若use_proportional_saturation的值为True，则按同比例进行处理，比如当前x，y的速度及角速度分别为2、1、1，其对应最大值分别为 1.5、1.5、0.5，y轴速度未超过最大值，比例设为1，x轴速度超过最大值比例取为最大速度/当前速度，即1.5/2=0.75，同理角度比例为 0.5/1=0.5，取这三个比例的最小值，为共同的比例，即min（1,0.75,0.5）=0.5，则将当前速度都乘以该比例，由 2、1、1，变为1、0.5、0.5。

   其中，角速度转换为角度的具体过程如下：先根据当前x轴的线速度除以角速度得到转向半径radius，若该半径小于最小转弯半径 min_turning_radius，则修正为最小转弯半径，然后根据外部参数车辆模型的前后轮间距 wheelbase与该转弯半径radius的比值取反三角函数即可得到需要的转角。

---

   补充：使用teb_local_planner过程中常见报错或警告信息发生的阶段

   1、[ WARN] ：TebLocalPlannerROS: trajectory is not feasible. Resetting planner

   此警告发生于规划结束后，进行速度计算前的可行性检测部分，调用isTrajectoryFeasible函数进行可行性检测，检测轨迹中前feasibility_check _no_poses个路径点处是否与障碍物相交，需要注意的是，会把机器人的外形轮廓放置到该路径点处进行碰撞检测，而不是仅仅对该点进行检测。

   若有碰撞，则返回可行性检测未通过，将线速度和角速度设置为0，给出报错信息，重置规划器，开始新的规划。

ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner...");

   偷懒型解决思路：减少小车外形参数设定的范围或者改成点模型，减少参数feasibility_check_no_poses的值，或者进一步直接取消可行性检测过程。

   （ 所谓偷懒型解决思路，即治标不治本的解决方法 ）

---