融合IMU和里程计的路径规划

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

85a375b875d7625e4c44971fec2bc54c.png

1. 基础模型

• 两轮差速移动机器人运动模型：机器人的运动学模型主要可以分为速度运动模型与里程计运动模型两大类，前者关注于描述机器人在特定时刻的位置和速度，而后者则侧重于记录机器人在移动过程中所经过的距离信息，通过记录机器人在运动中的轨迹信息，可以更直观地表示机器人的位置和姿态变化，有利于理解和可视化。

f647a90391a036b655cc9736eed80d00.png

• 环境地图模型：常见的地图模型包括三种：拓扑地图、几何特征地图和栅格地图。拓扑地图模型是地图学中的一种统计地图，保持点与线相对位置关系的正确性，但不一定保持图形形状、面积、距离和方向的精确性；几何特征地图主要由移动机器人通过其传感器采集的外部环境感知信息中提取的抽象几何特征构成；栅格地图模型将地图区域划分为均匀的网格单元，并为每个网格单元分配特定的属性信息。

cc930bcbc257a0a12af0078f00960954.png

• 坐标系变换模型：移动机器人中存在各种坐标系，不同的坐标系之间必须通过坐标变换来进行数据传递，TF功能包使用树形数据结构来维护多个坐标系之间的变换关系，即已知一个坐标系下的某点坐标，通过 TF 功能包可以将其转化为另一坐标系下的坐标。

99bd461393fdba3eab646f7da55ed4de.png

2. SLAM建图

• SLAM建图：在确定了机器人本身的位姿后，将其与外界的环境信息相融合，便能够建立地图，但是，在不知道位姿的情况下，必须先确定机器人所在的位置，然后再通过传感器采集周围物体数据建立地图。通过对机器人的运动轨迹进行数学描述，形成一个关于机器人状态转移的概率模型，捕捉机器人在特定时间和空间中移动时可能遇到的各种情况；观测模型也被用来建立一个观测概率模型，考虑机器人如何观察周围环境并记录这些信息的过程，通过分析数据，告诉哪 些观测是可靠的，哪些则存在误差或噪声；结合状态转移概率模型和观测概率模型，能有效地处理机器人运动中不可避免的运动噪声和观测噪声，从而提高整个系统的鲁棒性和准确性。
• 激光雷达SLAM：持续采集机器人所在区域的环境信息，并将其与机器人自身的位姿信息进行融合，实现机器人的自主定位；利用激光雷达持续采集的周围物体信息来生成局部地图，在经过特征的匹配之后，如果与现有的部分图相吻合，那么就可以将其融入到目前所匹配的全局地图中，这样就可以持续地对全局地图进行更新。

7d37c181f1ce4b46aa108a2fc7d6f371.png

• 融合里程计与 IMU 数据：在移动机器人运动过程中，当遇到阻碍或者路面不平的情况下，会导致轮子打滑，从而干扰里程计采集到的数据信息，为了防止对建图产生影响，增加 IMU 传感器，通过 EKF 来融合两者信息，减少了轮子打滑对移动机器人定位造成的误差，给移动机器人提供更精准的定位。IMU 系统模型主要包括噪声模型和测量模型，噪声模型描述了测量过程中的不确定性，主要由随机游走的偏置和高斯白噪声两部分组成；测量模型描述了根据测量的角速度和加速度来计算物体的姿态、速度和位置。

3. 全局路径规划

• 传统A-star算法综合代价函数：f(n)=g(n)+h(n)，其中g(n)表示从起点到当前节点 的实际代价，而 h(n ) 是从当前节点 n 到目标节点的估计代价，即启发函数。
• 自适应启发函数：f(n)=g(n)+(1+dD)h(n)，其中d 表示当前点到目标点的距离， D 表示起点到目标点的距离，当离目标点较远时，路径搜索效率很高，没有搜索多余节点，迅速向目标点所在区域靠近；当离目标点较近时，搜索效率降低，更偏向于搜索最优路径
• 双向搜索策略：通过正反两个方向，从起点和终点同时搜索路径，使用启发函数距离目标较远时候的高效性能，减少冗余节点的计算，能够有效减少路径规划时间，提高搜索效率
• 路径平滑处理：通过贝塞尔曲线的平滑特性来优化路径规划结果，使路径更加平滑，减少路径的折线数量，提高路径规划的效果

4. 局部路径规划

• 传统DWA 算法：通过定义一个可行速度窗口(v,w) ，限定机器人在当前时间步中可能的速度和角速度组合。这个速度窗口通常由当前速度、最大速度、最大加速度、最大角速度等因素决定，对于速度窗口内的每一个速度和角速度组合， 算法会根据每个组合对应的轨迹进行评估，考虑如果某个候选轨迹与障碍物碰撞，该轨迹不会被选择，最后，算法会从所有安全的候选轨迹中选择一个最优轨迹，通常选择使得距离目标最近且最平滑的轨迹。通过不断地搜索速度窗口内的速度和角速度组合，并评估对应的轨迹，算法在每个时间步中生成一条安全、高效的运动轨迹，使得机器人能够在局部环境中灵活地避开障碍物并达到目标。
• 评价函数：最大限度地让机器人避开障碍物（用 dist(v,w)表示，如果此轨迹上没有障碍物，将其设置为常数），以最高速度（用 velocity(v,w)表示）向目标点移动（用 heading(v,w)表示），归一化处理（ρ\rhoρ）评价函数的目的是将这些具有不同数学单位的因素转换为无单位的量，使得比较更具有意义。

5. 规划融合

c0022e1c542f9651498173a32bf3608a.png