仿生机器人的SLAM导航

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

1. 并联足式机器人

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

  

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

  

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

  

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

  

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

  

  

2. SLAM建图

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

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

  

  1. 设定某个位姿作为算法的初始状态，直接计算每个点到所有点的距离，找到最小距离点

  2. 根据初始点得到对应点，通过距离阈值参数过滤掉不合理的匹配点对。

  3. 计算点云转换，得到最优变换

  4. 算法迭代：直到相邻两次迭代的损失函数差小于系统设定的阈值，或者迭代次数达到最大迭代上限，此时程序将停止

• ICP点云流程图：

  

• ICP点云配准迭代示例
  

  

  
  

  

  
  

  

  

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

  

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

  

  

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

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

  

• 融合IMU、GNSS：使用扩展卡尔曼滤波（EKF）融合 IMU 数据和 GNSS 数据，IMU 更新频繁，提供高频状态估计，但会产生累计误差，GNSS 有自我校正功能，提供准确的全局位置信息，从而纠正 IMU 的累积误差，扩展卡尔曼滤波通过预测和更新两步实现对数据融合。

  • 预测阶段：状态预测和协方差预测，对于 IMU 来说，状态包括位置、速度、姿态等，而协方差则表示状态估计的不确定性

  • 更新阶段：通过融合 GNSS 模块的数据，更新系统的状态估计和协方差矩阵

  • 卡尔曼增益 K： 作用是根据预测的状态估计和观测模型的不确定性，调整预测值，使其更加接近实际测量值；增益值取决于状态估计的不确定性和观测噪声的相对大小。如果观测噪声较小或者预测的状态估计不确定性较大，增益会较大，更倾向于相信测量值

3. 路径规划

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

  

  
  其中：

  • xs 、 ys：表示起始点的 x 和 y 轴坐标

  • xt 、 yt：表示目标点的 x 和 y 轴坐标

  • N：表示起止点构成的矩形区域内障碍物的总数

• 全局搜索范围改进：跳过无法通过的节点，减少搜索的耗时和复杂度，提高搜索算法的效率

• 规划路径平滑改进：采用直线段微分节点，过多的拐点会导致运动的不连续，使得行驶路线出现频繁的停顿和转向，影响行驶的流畅性和速度

  

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