基于跳点搜索算法的路径规划

REF：基于激光 SLAM 的移动机器人路径规划算法研究

1. 基础模型

• SLAM技术：核心目标是在机器人同时执行自身定位和地图构建的过程中，有效地捕捉和维护环境的拓扑结构和特征信息，不仅仅提高了机器人对未知环境的感知能力，还能够在机器人导航过程中持续更新地图，以适应环境变化。 SLAM 问题可以划分为两个核心模块，第一个模块专注于通过外部数据解析机器人的位姿，即估计机器人在所处环境中的精确位置与姿态；第二个模块致力于利用机器人的位姿信息和激光雷达扫描数据来构建环境地图，这两个模块相互依赖，协同工作。
• RBPF 粒子滤波算法模型

9fccb42aa604d75d8b56b2d100e89988.png

• 基于图优化的 SLAM 算法模型
• 机器人导航系统模型

2134b838bc21a40a184be0b205ca56a2.png

2. SLAM建模

• Gmapping SLAM 算法：目的是在已知里程计数据和激光数据的情况下，计算机器人的位姿 x 和环境地图 m 的后验概率，通过在 RBPF 算法框架的基础上，融合更精确的激光雷达数据信息，减少了粒子分布数量。
• 基于图优化的 SLAM 算法：在图优化 SLAM 中，问题被一般分为两个主要步骤，首先是构建图，也称前端，该过程基于传感器信息和机器人位姿数据构建节点图，构建阶段涵盖了扫描匹配和闭环检测，扫描匹配聚焦于局部数据的关系处理，而闭环检测则处理全局数据，两者协同工作完成了图的构建过程；其次是优化图，也称后端，专注于优化之前构建的图，通过求解得到在当前约束下的最优位姿，从而构建一个具有最小预测与观测误差的最优位姿图。

004b52f92b916e5cbbacc6817177596f.png

• Cartographer SLAM 算法：整体架构分为前端（局部 SLAM）和后端（全局 SLAM）两个主要组成部分，采用了图优化理论框架，前端涵盖数据匹配与局部回环检测，通过匹配不同帧间传感器数据，建立相对位姿关系的子图，随后，新的传感器数据与已建立的子图进行匹配，完成子图的构建，子图建立完成后进行局部回环检测；后端主要由全局闭环检测和图优化求解两部分，首先连续的 n 个扫描帧数据构建一个位姿图，下一个连续的 n 个扫描帧数据用于构建新的子图，获取每帧扫描数据相对于对应子图的位姿以及子图在全局坐标系中的位姿。

3. 全局路径规划

• 传统跳点搜索算法（Jump Point Search，JPS）算法：JPS 算法是在 Astar 算法的基础上进行的改进，其算法流程与 Astar 算法基本相同，规则本质上是搜索方向可能发生改变的点，其筛选跳点规则为：

1. 若节点 x 是目标点，则节点 x 是跳点；
2. 若节点 x 存在至少一个强制邻点，则节点 x 是跳点；
3. 斜向搜索时，若节点 x 在水平或者垂直分量上有跳点，则节点 x 是跳点

• 动态双向搜索优化：在双向搜索的框架下引入了动态目标点的一种路径规划算法，动态目标点是根据对方搜索方向扩展出的具有最小代价值的节点，分别作为双向搜索的目标点进行路径扩展，当两个方向的搜索过程中，某一时刻两个方向的目标点相遇或其中一个方向的 OPEN 列表为空时，被视为规划结束，最终的路径即可被确定。
• 代价函数改进：h(n)=aLc,g−bcosθh(n)=aL_{c,g}-bcos\thetah(n)=aLc,g−bcosθ，其中Lc,g=∣xcurrent−xgoal∣+∣ycurrent−ygoal∣L_{c,g}=|x_{current}-x_{goal}|+|y_{current}-y_{goal}|Lc,g=∣xcurrent−xgoal∣+∣ycurrent−ygoal∣表示当前节点与目标节点之间的曼哈顿距离； cosθcos\thetacosθ为当前节点和父节点的连线与当前节点和目标节点的连线形成的角度信息，角度的值越小，则当前节点到目标点的代价值越小，反之越大，这样可以是的在节点扩展过程中更有利于偏向目标点； a、b为距离与角度信息之间的权重；xcurrent、ycurrent为当前节点的横纵坐标；xgoal、ygoal为目标节点的横纵坐标。
• 二次轨迹优化：一次优化针对障碍物拐点处存在的两个跳点进行优化，使规划的路径远离障碍物（在障碍物两边的拐点处引入新的辅助节点，帮助规划出的路径远离障碍物）；二次优化针对整个划出的路径进行拐点优化处理，引入关键点提取方法以及安全阈值 d_safe 来减少路径转折点并保证路径安全性。

4. 局部路径规划

• 传统 DWA 算法：通过对当前状态下各个速度和方向组合进行评估，然后筛选出最为合适的运动策略，使得机器人在复杂环境中能够快速而灵活地规划路径，同时高效地避障。
• 评价函数：最大限度地让机器人避开障碍物（用 dist(v,w)表示，如果此轨迹上没有障碍物，将其设置为常数），以最高速度（用 velocity(v,w)表示）向目标点移动（用 heading(v,w)表示），归一化处理（ρ\rhoρ）评价函数的目的是将这些具有不同数学单位的因素转换为无单位的量，使得比较更具有意义。

• 自适应障碍物和速度协同权重：当前节点和下一个关键节点组成的区域内障碍物占比高时，提高障碍物评价函数的权重，并同时降低机器人的速度评价函数的权重，使机器人尽可能避免发生碰撞，最终评价函数如下：

5. 规划融合

在全局路径规划中，算法依赖已知地图，通过计算确保机器人在不同环境中能够选择最安全的路径达到目标点。相对应的，局部路径规划则在机器人运动过程中通过搭载的传感器实时采集周围环境信息，以避开全局地图中没有的标出的 未知障碍物。

63ecdca6747ce1c38d9f1da13089206f.png

95aa796d6550516c3d8e53ab4005fbf0.png