麦克纳姆轮车的导航规划

REF：移动机器人自主导航系统研究

1. 机器人模型

• 麦克纳姆轮：一种比较独特的车轮结构，通常由一组斜向的棍子组成，每个轮子上都有若干个斜向安装的滚轮，每个滚轮轴与轮毂呈一定角度，通常是45°，分为左旋和右旋两类

6fd419f0c0a91feab910e70228ba3d8f.png

• 麦克纳姆轮机器人：由多个麦克纳姆轮对称安装在的机器人四周，常用的布局方式有：X-正方形、X-长方形、O-正方形及 O-长方形等，通过控制轮子正反转或停止来改变水平或竖直方向的分力，从而实现对机器人的全方向移动。

78f119c6e11c08345ecd161b22931100.png

30138b7f63117b01712e5f84a253f9bb.png

4dd6fb1b9b7abb914e4362a1672cdc0e.png

• 机器人运动学模型：

e97d5e2353029648d29d47fe3ab742a0.png

c5a85ea504f902c7a4920ef71c212e58.png

其中：

• vx,vyv_x, v_yvx,vy：机器人的水平、竖直方向的移动速度
• ω\omegaω：机器人角速度
• θt\theta_tθt：为机器人的航向角
• Δt\Delta tΔt：机器人采样周期

• 机器人平台系统模型：

f2c90a530fb41ce98fb38fc908d856fa.png

2. 激光SLAM算法

8bfda31d3257ddb47cb56af14ddbb2b4.png

• 基于 EKF 的 SLAM 算法：将机器人的状态分为位置状态和地图状态，并通过扩展卡尔曼滤波方法对这些状态进行不断估计和更新
• 基于 RBPF 的 SLAM 算法：通过多个粒子来近似机器人当前位置和地图的概率分布，每个粒子包含机器人的位置信息和环境地图，并通过递归更新粒子的权重来反映传感器测量和运动模型的影响
• 基于图优化的 SLAM 算法：将机器人定位和地图构建问题转化为一个图优化问题来进行求解，图中的节点代表机器人的状态或环境中的特征点，边则表示不同状态之间的约束关系，这些约束通常来源于传感器测量数据，优化的目标是最小化图中所有边的代价函数，来优化机器人的轨迹和地图，从而获得一致性较高的状态估计。
• Gmapping SLAM 算法：通过状态空间建立、状态估计、状态更新和状态融合，四个核心步骤实现动态环境建模。

5f786abd178d8fe4bcf5cb9088ea8520.png

• Hector SLAM：一种基于高斯-牛顿优化的扫描匹配算法，主要涵盖数据预处理、地图构建、扫描匹配位姿估计、地图更新以及前端数据关联与后端优化策略 5 部分：
  1. 数据预处理：多级预处理，包括激光点云异常值滤除、传感器时延畸变补偿、点云降采样等操作
  2. 地图构建：采用多层概率栅格地图，通过多分辨率将环境划分为不同层级的栅格单元，利用快速卷积算法动态更新各层地图的占据概率。激光束反射信息通过贝叶斯概率融合方式更新栅格状态，最终通过多尺度地图叠加生成高精度环境模型。
  3. 位姿估计：扩展卡尔曼滤波结合机器人运动学模型，动态调整过程噪声与观测噪声协方差矩阵，构建运动预测-观测校正的双向闭环，实现位姿预测与状态修正。
  4. 地图更新：采用动态局部更新策略，根据实时位姿估计结果，对机器人感知范围内的局部栅格进行概率更新。
  5. 前后端融合：构建前端扫描匹配（实时位姿估计与地图构建）与后端图优化（位姿图优化技术对历史轨迹进行全局优化）的紧耦合架构。
• 里程计与 IMU 数据融合：传统激光 SLAM 依赖里程计提供运动先验信息，但轮式里程计在非结构化地面易受打滑、颠簸等因素干扰，导致运动模型的预测误差累积，EKF 算法通过融合 IMU 的角速度观测与加速度信息，动态补偿里程计的航向角漂移和平移误差。当激光雷达扫描到重复几何结构时，融合后的位姿估计能够有效抑制里程计航向漂移对栅格地图的影响，避免墙面特征出现锯齿状畸变或重叠。
  1. 运动估计修正：修正后的运动估计作为扫描匹配的初始位姿输入，提升点云配准的收敛速度
  2. 地图一致性维护：在建图过程中，IMU 提供的航向角动态约束与里程计的平移观测形成互补。

  

  78e02022eb0183b72bc41a71747c4a74.png

3. 路径规划

• Astar算法：结合实际成本和启发式估计成本来指导搜索过程，以初始栅格节点为起点，向相邻栅格节点进行辐射状扩展，每次迭代计算当前节点所有相邻子节点与目标点之间的评价函数值，从中选取评估最优的节点作为新父节点，不断重复生成相邻子节点，计算评价函数值，选取评价最优的迭代过程，直到搜索到目标点位置为止。

73b3471f427b8b66063666d334dac9b2.png

• 基于障碍物比例平衡搜索效率： 当环境中障碍率较低时，启发函数权重增加，算法缩小搜索空间，直接沿着目的地方向扩展；当环境中障碍率变高时，启发函数权重降低，通过牺牲扩展效率来换取路径最优。

36e6e2c4f07855eb3c9bbdfb0cfdf8e0.png

968aee0de5642e6c53f34ec613566749.png

• 优化子节点选取策略：通过检测障碍物的几何位置关系来提升路径安全性，避免了路径对角线穿越障碍物顶点，降低运动过程中的碰撞风险系数。
• 路径平滑优化：对规划的路径进行双向平滑度优化，并设置路径到障碍物的安全距离，实现防碰撞处理。在规划的路径中，若两不相邻的路径节点之间的规划路径距离大于两点连线距离，且连线 远离障碍物，即可舍弃两个路径节点中间多余的节点。