融合A*和DWA的路径规划方案

REF：基于 SLAM 的煤矿救援机器人路径规划研究

1. 基础模型

• 运动模型：描述了机器人在环境中移动的物理规律，直接影响到机器人在运动过程中状态估计的准确度以及位姿（位置与方向）的计算精度。准确的运动模型可以大大提高机器人定位和地图构建的质量。

df3ff823a21a9fbe3eb8147d63870334.png

• 机器人仿真模型：机器人的物理特征、尺寸、形状、关节等能够以一种标准化的方式被表达，在 URDF 文件中，标签用于定义机器人的各个组成部分，以下是其中一些较为常见的标签：
  • Link 标签：描述机器人的一个固体部件，可以是机器人的主体、手臂、轮子等，定义了一个部件的名称、形状、尺寸和质量属性
  • Joint 标签“定义两个 Link 部件之间的连接关系，指定了如何通过关节动力学将两个部件连接起来，包括关节的类型、关节的方向和关节的运动限制，内部通常包含 parent 和 child 标签，用以指定关节连接的两个部件
• 环境地图模型：地图不仅需要精确地反映环境的特征，降低数据量以提高处理效率，还需要有足够的信息量以支持复杂的导航任务，栅格地图（将环境划分为网状的小单元格（即栅格）来实现环境的离散化表示）、特征地图（使用环境中的明显几何特征（如点、线条、多边形、曲线等）来构建地图）和拓扑地图（关注的重点是环境中的地标和路径的连接性质）三种常用的地图类型。
• Gmapping SLAM模型：采用粒子滤波算法实现机器人在未探索环境中的定位及地图构建，通过整合里程计和激光雷达的信息，从而实现了优异的定位和地图绘制性能，尤其是在小型场景下的精度显著。

7fb823ea93d33d6c28f8b83f60e914f7.png

• 图优化 SLAM模型：主要分为前端处理与后端优化两个部分：前端处理包括数据的匹配、关联以及闭环探测。数据关联用于解析传感器获取的环境信息，通过比较不同帧的信息，预测机器人的位置，并生成包含节点和边的图形结构；闭环检测用于确定机器人当前位置，并与绘图中的对应位置进行一致性校正，处理位置估计的问题；后端通过计算修正后的姿态，从而确定最准确的姿态序列，这个过程的目标是求解姿态的最大似然估计

40654ff6aa0d14faaae320da46088df8.png

• Cartographer SLAM 模型：应用 scan-to-submap 进行连续帧之间的比较匹配，采用分支定界策略来提高闭环检测的速度，在完成闭环检测任务之后，采用非线性最小二乘法对后端进行优化处理；算法生成的地图是一组 submap 的集合
• Lazy Decision 算法模型：延迟对闭环候选的最终判定，直到有足够证据支持该闭环是真实有效的，从而减少误检对系统的影响。节点𝑎和𝑏并不会即刻进行位姿优化，而是先将该候选放入一个待验证队列，在后续的若干帧中，持续观察 多个方面，最终在新回环检测点𝑐和𝑑被识别之后才执行闭环验证。
  • 是否有连续的、几何上一致的匹配
  • 是否满足时序一致性
  • 是否通过几何验证（如RANSAC + PnP、ICP等）

b26fdb856efc3dc73e9c5ad7cccfc527.png

• 子图与子图(Map to Map)的闭环检测模型：由顺序帧扫描信息采取栅格图表形式构建子图，旨在解决帧与子图之间(Scan to Map)匹配过程中所遇到的问题

2. 路径规划

• A-star算法：基于启发式方法，利用评估函数来估算从起点到终点经过当前节点的总代价，在搜索最优路径的过程中，算法通过启发函数引导搜索方向，并运用评估函数来衡量现在位置到起点以及到目标点的总预计代价。 f(n)=g(n)+h(n)，其中n 为当前位置节点，f(n)为评价函数，g(n)为起始点到当前节点的真实代价值，h(n)为当前节点和目标点之间距离的启发式评估函数
• 优化评价函数： f(n)=g(n)+(1+rR)∗h(n)，其中r 为当前点到目标点的长度，R 为起点到目标点的总长度，即为估计代价 h(n)设置系数，系数随着实时位置靠近目标位置减小，从而在靠近起点位置较高搜索效率，随着向目标点靠近搜索速度变慢但增加了最佳路径的概率
• 调整邻接窗口：从起始点到终点的路径过程中，有 4 邻接和 8 邻接两种常用的选择子节点的方式，可以使用16邻接路径拐点少、路径短，适合移动灵活的智能机器人的路径规划

80cb01934ab117f415e475c0eae7432b.png

• 贝塞尔曲线平滑处理：在一个平面上选择 3 个不同线的点 A、B、C，将三个点依次连成一条直线，在线段 AB 和 BC 上分别选择点 D 和 E，使AD/AB=BE/BC，连接 DE，并在 DE 上确定一点 F，使得 DF/DE=AD/AB=BE/BC，让选取的点 D 在第一条线段上从点 A 移动到点 B，然后确定所有点F，并将点 F 移动的轨迹连接起来，所得到的曲线就是贝塞尔曲线，二阶贝塞尔曲线公式为：

5634f43994b2e1f58fb0be633ca1215a.png

• DWA算法：基于电机扭矩（加速度）、预设最大速度以及动态响应特性（惯性和机械）共同定义了机器人在特定时间段内能够达到的最大和最小速度的界定范围，根据线速度和角速度的分辨率将速度信息划分为不同的子集，设置一个预定的时间段T，排除在此时间内预测出会发生碰撞的速度子集，继续对剩余速度进行评估，直到穷尽所有候选速度信息，最后对所有未被排除的速度子集，使用评价函数进行打分，并选取得分最高的速度组合作为机器人接下来的速度指令。
• 评价函数：最大限度地让机器人避开障碍物（用 dist(v,w)表示，如果此轨迹上没有障碍物，将其设置为常数）并以最高速度（用 velocity(v,w)表示）向目标点移动（用 heading(v,w)表示），归一化处理评价函数的目的是将这些具有不同数学单位的因素转换为无单位的量，使得比较更具有意义。 G(μ,w)=ρ[α⋅heading(μ,w)+β⋅dist(μ,w)+γ⋅velocity(μ,w)]

最终算法 结合了 Astar算法的全局路径规划能力和 DWA 算法的即时避障和局部路径规划的灵活性，有效提升了路径规划的全局性和适应性，减少了陷入局部最优和碰撞的风险，尤其适用于动态和复杂的环境中

86bc42232eb54475ef78af3399ab1997.png

6ece98eb964aaed5d3173ab86379d15e.png