仿生机器人的SLAM导航

REF：多传感器融合的并联腿式六足步行机器人SLAM建图与路径规划研究

1. 并联足式机器人

• 四足磁吸式爬壁机器人：燕山大学研制，用于船舶表面的攀爬

da807935ff908f28a8e4c6513b908a23.png

• 四足并联腿式机器人：采用驱动端水平布置的腿部设计，使得在支撑和摆动阶段减少了脚端和地面之间的冲击

f36ccb980c298d53d5740b4fcd74a687.png

• 并联步行六足章鱼：上海交通大学高峰团队研制，机器人采用2UPS+UP 的并联机构设计，当腿部完全展开时尺寸 可达 1.5×1.5 米，对称的机身能够向任意方向移动，行进速度最高可达 每分钟 20 米，还可负重 200 公斤

1dfb63567116aa6ec4150d9380d6f8ff.png

• 并联腿式机器人：基于新型机械结构，算法基于贝塞尔曲线改进摆动和支撑阶段的足部轨迹，实现了无冲击的运动

ec68656c2b1f97b0698b0e96cbbd5fbc.png

• 并联腿式六足步行机器人：长春工业大学研制，能在复杂地面上自由移动，能成功完成爬楼梯等

59fa5797938dc5b751bb80556ba413be.png

38e49125dccdd92fc1745b80ee1db764.png

2. SLAM建图

仿生步行机器人无法使用传统的轮式里程计，基于扩展卡尔曼滤波算法，采用 IMU 与 GNSS传感器融合的方式实现机器人的定位与建图

ad5ca6135cde476ca1f59150a813b406.png

• ICP 点云配准算法：找到两个点云之间的最佳刚性变换，使它们尽可能地重合，算法将采集到的点云数据与已知的模型或其他点云进行匹配，以实现精确的配准。

02452fad19c065844f12b7610f41ebb2.png

1. 设定某个位姿作为算法的初始状态，直接计算每个点到所有点的距离，找到最小距离点
2. 根据初始点得到对应点，通过距离阈值参数过滤掉不合理的匹配点对。
3. 计算点云转换，得到最优变换
4. 算法迭代：直到相邻两次迭代的损失函数差小于系统设定的阈值，或者迭代次数达到最大迭代上限，此时程序将停止

• ICP点云流程图：

1bc733798b97af1a393e3d29775dfc27.png

• ICP点云配准迭代示例

7f77d2994ce8e5742d6886e24c3afc69.png

c2a2bc348df900a97bd8f7a45632b0bb.png

3fb4c4e8ab0ae9a21906e925da1c945b.png

f325691fdfb74cc02d07843c361b97e4.png

• KDTree数据结构：可以将激光点云数据进行高效的空间划分，将点云的坐标信息以一种层次结构的方式存储，在查找最近邻点时避免对每个点进行全局遍历，通过在特定的子空间中进行搜索，显著减少了搜索最近邻点所需的时间，在点云配准中应用能够提高匹配速度，并在高维空间中实现更快速、更有效的最近邻点搜索

7a47d39b85ffb661f3ad45ec57653c38.png

• RANSAC 算法：通过随机选择数据集中的子集来拟合模型，并通过统计学方法来找到最佳模型参数，在处理带有大量噪声和离群点的数据集时表现出极强的鲁棒性，能够迅速估计出一个粗略的模型参数

3d42063ebee0493e307924c6f3714aac.png

42cd122812bd84bf78f6a4343239b76f.png

• 结合ICP和RANSAC 算法：先使用RANSAC 算法快速的粗略估计，有效地剔除异常值，为 ICP 算法提供一个较好的起始点；然后应用 ICP 算法进行精细调整，利用其高精度的特性对配准结果进行优化。

41700457e05e7b8901bac4b04b10f0ee.png

• 多传感器时间对齐：时间同步可以减小由于传感器间时间差异引起的误差，从而提高 SLAM 系统的整体表现

a90d6c0913b70dc61cc10bfd3b0a9bfe.png

• 融合IMU、GNSS：使用扩展卡尔曼滤波（EKF）融合 IMU 数据和 GNSS 数据，IMU 更新频繁，提供高频状态估计，但会产生累计误差，GNSS 有自我校正功能，提供准确的全局位置信息，从而纠正 IMU 的累积误差，扩展卡尔曼滤波通过预测和更新两步实现对数据融合。
  • 预测阶段：状态预测和协方差预测，对于 IMU 来说，状态包括位置、速度、姿态等，而协方差则表示状态估计的不确定性
  • 更新阶段：通过融合 GNSS 模块的数据，更新系统的状态估计和协方差矩阵
  • 卡尔曼增益 K： 作用是根据预测的状态估计和观测模型的不确定性，调整预测值，使其更加接近实际测量值；增益值取决于状态估计的不确定性和观测噪声的相对大小。如果观测噪声较小或者预测的状态估计不确定性较大，增益会较大，更倾向于相信测量值

3. 路径规划

• 基于障碍物数量优化Astar启发函数：

9efe706066eb2165a2a9c0218dfff088.png

其中：

• xs 、 ys：表示起始点的 x 和 y 轴坐标
• xt 、 yt：表示目标点的 x 和 y 轴坐标
• N：表示起止点构成的矩形区域内障碍物的总数

884a8fb14191a348ff9a42e2f1b2c19f.png

• 全局搜索范围改进：跳过无法通过的节点，减少搜索的耗时和复杂度，提高搜索算法的效率
• 规划路径平滑改进：采用直线段微分节点，过多的拐点会导致运动的不连续，使得行驶路线出现频繁的停顿和转向，影响行驶的流畅性和速度

dd45190ca3b2c5968ef67848c7547025.png

• 融合局部路径规划DWA算法：DWA算法基于机器人在给定时间段内可以选择的速度范围，将速度和角速度的不同组合称为窗口，评估每个窗口的安全性和效果，从中选择最优的窗口来指导机器人运动

d9d20c1f79da39f4ac474587247a65c1.png

02bdc3c8ae99b6f9577a9a3626562f45.png