惩罚靠近障碍物的路径优化

REF：基于激光 SLAM 的移动机器人自主导航技术研究

1. SLAM 模型

SLAM 实质上是一个“预测—更新—地图”的概率过程，移动机器人根据传感器信息对自身位置进行预测，并通过不断更新地图来提高环境地图的准确性，机器人需要不断地收集数据，对当前位置和地图进行推理和更新，以保持对环境的准确认知。

• Gmapping 算法：通过改进了建议分布和引入了自适应重采样策略，两种关键改进相互协作来提高 RB的性能，以解决栅格地图的 SLAM 问题，其特殊之处是把定位系统与建图过程加以隔离，而且每一种粒子都携带着一个地图。

db2e02243a6a982ed5f6bcd9a831027f.png

• Hector SLAM 算法：利用激光传感器不断扫描周围环境，获取障碍物的位置信息，采用单次扫描匹配技术，在运动过程中实时地将这些信息用于地图构建和定位。
• Cartographer 算法：综合了独立的局部和全局方法，强调通过雷达观测实现位姿优化，整个框架包括数据源输入、前端和后端，实施包括激光雷达数据的预处理、初始化位姿估计、子地图构建与优化，以及前后端数据传递和处理流程

587376282c98f7aefe5c11066bc17c56.png

2. 全局路径规划

• 传统A-star算法综合代价函数：f(n)=g(n)+h(n)f(n)=g(n)+h(n)f(n)=g(n)+h(n)，其中g(n)表示从起点到当前节点 的实际代价，而 h(n ) 是从当前节点 n 到目标节点的估计代价，即启发函数。
• 双向A-star算法：通过从起始点和目标点发起的交替双向搜索来优化搜索过程，直至两个方向的搜索路径相遇，即交替选择前向和反向搜索中当前最优节点作为各自的新目标，确保了两个搜索方向能够有效地朝向对方进行。
• 实际代价函数优化：对于其实点到当前节点的实际代价，增加障碍物最小距离因子，通过安全代价权重进行障碍物安全判定，考虑上已遍历历史节点的障碍物安全性。
• 自适应启发函数：启发函数调整为：ek⋅h(n)e^k\cdot h(n)ek⋅h(n)，其中k随着搜索路径增加而减小，表示待搜索权重减少。
• 沉余节点进行优化：如果路径上非相邻节点之间的距离小于规划节点间的距离，且非相邻节点路径组成的直线不与障碍物相撞且该直线与障碍物的垂直距离大于等于安全半径距离，则中间节点属于沉余节点，可以删除。
• 路径平滑度优化：常见路径平滑度优化方式有贝塞尔曲线法、三次样条曲线、B 样条曲线法和圆弧处理法等，以圆弧处理法为例，通过对转折点进行圆弧处理，使得路径长度减小且转折度降低，满足移动机器人的几何特性

981bf172ab987903c25c8bb0e7cee6a8.png

3. 局部路径规划

• 传统时间弹力带法（TEB）：利用弹性带模型描述移动机器人轨迹，在考虑环境、运动学和动态约束的基础上，全面描述移动机器人运动状态，包括：
• 速度约束：此约束对移动机器人的最大线速度和最大角速度进行了规定，采用损失的方式表示
• 最快路径约：旨在优化路径，使机器人能够在最短时间内到达目标点
• 通过点约束和障碍物约束：在规划过程中既要依照全局规划路径点又要避开障碍物。全局规划路径点对该算法规划路径点产生吸引，障碍物约束以惩罚函数的形式实施。
• 传统动态窗口法(DWA)：在速度空间中使用采样为机器人生成模拟运动轨迹，考虑机器人的运动学约束和环境信息来定义可行速度的“动态窗口”，用于在短时间范围内生成一组模拟轨迹。
• 建立移动机器人运动学模型： 依据机器人线速度、角速度、运动方向等信息来确定其位置信息
• 速度采样的约束空间：自身速度约束、受电机转矩形成的最大加速度约束、有效躲避障碍的约束
• 评价函数：G(μ,w)=α⋅heading(μ,w)+β⋅dist(μ,w)+γ⋅velocity(μ,w)
• 增加安全距离惩罚（penalty）：当移动机器人与障碍物距离小于两倍安全半径时对评价函数进行惩罚，以避开障碍物，最终评价函数如下： G(μ,w)=α⋅heading(μ,w)+β⋅[dist(μ,w)+penalty]+γ⋅velocity(μ,w)

4. 算法融合

3bf649bb4a8cf6f49c6c7d9059f0e923.png