基于ZUPT与闭环位姿估计的麦克纳姆轮移动机器人高精度里程计标定方法

《Sensors》：A High-Precision Odometry Calibration Method for Mecanum-Wheeled Mobile Robots Based on ZUPT and Closed-Loop Pose Estimation

　　

研究人员提出了一种两级闭环标定框架，用于减小麦克纳姆轮移动机器人在运动过程中的里程计尺度误差及静止期间的位姿漂移。在上层标定级别，采用L2范数计算初始位姿与最终位姿之间的平面位移，从而降低横向偏差对基于单一坐标轴距离测量的影响。旋转位移通过累加归一化角增量获得，从而避免偏航角跨越±π边界时的不连续性。研究人员还引入了一种带容差死区的继电控制器，以减少低速标定过程中静摩擦引起的停滞及目标附近的振荡。在下层里程计接口，根据指令零速状态调整轮式里程计测量的协方差。在静止期间，该调整增强了近零速度测量的贡献，并限制了残余速度估计与传感器噪声对融合位姿的影响。识别出的纵向与旋转补偿因子随后通过ROS 2服务在航位推算节点中在线更新。与传统ZUPT实现不同，所提方法不需要额外的零速伪测量节点。实验在三种近水平表面（瓷砖、环氧树脂和沥青）上进行。在720次双向原地旋转试验中，角度误差降低率介于59.13%至96.58%之间。在覆盖9种表面类型与目标距离组合的540次直线行驶试验中，整体平均绝对误差从标定前的53.22 mm降至标定后的9.69 mm。进一步采用扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、鲁棒容积卡尔曼滤波（RCKF）以及由slam_toolbox实现的基于图的SLAM优化框架进行了间歇性停走实验。对于每个估计后端，通过计算其在固定协方差与所提出的基于ZUPT的自适应协方差配置下相对于相应同步/odom轨迹的偏差来评估估计轨迹；/odom被用作通用比较基线而非绝对定位真值。自适应协方差策略在评估的滤波后端上将位置均方根误差（RMSE）降低了19.38%–67.44%。这些结果表明，所提出的框架在测试表面条件下可同时减少运动相关的里程计尺度误差与静止位姿漂移。

**基于ZUPT与闭环位姿估计的麦克纳姆轮移动机器人高精度里程计标定方法论文解读**

**一、研究背景与拟解决的关键问题**

现代智能物流与仓储系统中，自动导引车（AGV）与仓储基础设施、机器人工作站、机械臂及自动充电站协同作业的需求日益增长。麦克纳姆轮机器人凭借全向移动能力和优越机动性在该领域得到广泛应用，其导航系统对鲁棒性提出了严格要求，而底层航位推算作为控制回路的主要反馈来源和多传感器融合定位的基础，其精度直接决定整体定位性能。然而，麦克纳姆轮里程计精度受到多重因素制约。机械层面，麦克纳姆轮采用45°斜置从动辊轮与地面接触，固有的多边形效应引发高频微滑移，使车辆纵向与横向运动学强耦合；实际滚动半径与等效轮距偏离标称参数时，纯开环时间积分将导致系统性尺度漂移。算法层面，传统多传感器融合框架中，底层驱动节点以固定协方差矩阵向融合后端传递信息，即使机器人静止，编码器量化噪声、电机死区效应及微小残余运动仍会产生非零里程计读数，固定协方差无法反映运动状态变化，导致静止位姿估计波动。此外，现有研究往往将运动学参数标定、低速控制与零速辅助定位作为独立课题分别处理，缺乏将动态尺度校正与静止漂移抑制相统一的闭环标定框架。同时，传统单轴轨迹标定易受横向滑移影响，大角度旋转在偏航角跨越±π边界时存在不连续问题，且多数实验仅在单一表面或单一融合后端下开展。针对上述局限，研究人员开发了一种两级闭环标定框架，在不改变下游位姿估计框架基本结构的前提下，从里程计输入层面同时改善运动过程中的尺度误差与静止期间的位姿漂移。该研究成果发表于《Sensors》。

**二、主要技术方法**

研究人员构建了两级闭环标定框架：上层标定级别采用L2范数位移反馈、归一化角增量积分及带容差死区的继电控制器实现运动学参数辨识；下层里程计接口基于零速状态检测实现轮式里程计协方差自适应切换，并通过ROS 2服务在线更新补偿因子。实验平台为深圳Hiwonder四轮麦克纳姆轮机器人（总质量10.5 kg，轮半径5 cm，STM32F407VET6微控制器，MPU6050六轴IMU采样率186 Hz，软件基于ROS 2 Humble），平移真值由Leica DISTO D5激光测距仪（精度±1 mm）提供。

**三、研究结果**

**1. 多表面双向原地旋转标定**
在瓷砖、环氧树脂与沥青三种近水平表面开展双向原地旋转实验，目标角度涵盖90°、180°和360°，每条件重复20次，共720次试验。标定前360°试验平均旋转角为385.20°–391.66°，呈系统性过冲趋势；经闭环标定后，角度误差降低率介于59.13%–96.58%，其中5个360°条件的误差降低率超过90%。误差分布显示标定后中位数、四分位距及离散度均显著收窄，表明该方法在消除旋转尺度偏差的同时改善了试验可重复性。

**2. 基于L2范数的纵向直线标定**
在三种表面分别开展0.5 m、1.0 m和1.5 m目标距离的直线行驶试验，每条件重复30次，共540次试验。标定前所有表面均呈系统性欠行程，相对误差为4.75%–6.77%，绝对误差随行驶距离增大而累积；经L2范数闭环标定后，整体平均绝对误差由53.18 mm降至9.69 mm，误差降低率达82.13%，其中1.0 m瓷砖条件最高为86.08%。相同目标距离下不同表面的误差降低率接近（78.54%–86.08%），表明纵向补偿因子在近水平表面间具有一致的标定性能。

**3. 间歇性停走运动下的跨后端ZUPT自适应协方差评估**
在三种表面开展间歇性停走映射实验，每表面包含6次slam_toolbox试验及18次滤波试验，共72次独立物理试验。采用EKF、UKF、RCKF及基于图的SLAM优化框架分别处理固定协方差与ZUPT自适应协方差配置。结果显示自适应协方差策略在所有后端和所有表面上均降低了相对于/odom基准的位置RMSE与平均绝对误差（MAE），RMSE降低率为19.38%–67.44%。其中环氧树脂表面EKF的RMSE从0.3603 m降至0.1598 m；图优化框架虽在固定协方差下已具有最低绝对误差，但自适应协方差仍使其RMSE进一步降低19.96%–53.67%，证明该机制即使对低误差基线估计器也能进一步抑制静止期局部漂移。

**四、讨论与结论**

讨论部分指出研究局限性：受平台结构强度与减振能力限制，不平坦地形实验造成机械冲击并损坏控制板，故未评估剧烈垂直振动工况；旋转参考源自机载IMU而非独立外部测量系统，主要量化一致性改善而非绝对精度；受空间限制未构建可控坡度环境，结论仅限于近水平表面；机械臂连续抓取能力有限，载荷变化对有效滚动半径与转动惯量的影响未系统研究。算法层面，当前协方差更新策略采用运动/静止二值切换，无法表征起动、加速、减速及制动过程中状态不确定性的连续演化，可能在该类过渡阶段引入测量权重不连续。未来工作将强化控制板减振安装与底盘阻尼结构，开发可控粗糙度与坡度测试平台，并基于指令速度、IMU角速度与线性加速度、编码器反馈及滤波状态联合估计运动状态，以连续自适应协方差映射替代二值切换。结论部分确认：旋转标定实验覆盖3种表面、18种测试配置、共720组数据，绝对角度误差显著降低，误差降低率59.13%–96.58%；纵向标定实验涵盖540组数据，标定后整体平均绝对误差从53.18 mm降至9.69 mm，平均误差降低率82.13%；间歇性停走实验表明，EKF、UKF、RCKF及slam_toolbox图优化框架在三种表面下均获得RMSE与MAE改善，RMSE降低率19.38%–67.44%。研究验证了所提框架在测试表面条件下可同时减小运动相关里程计尺度误差与静止位姿漂移，且不依赖特定滤波架构。然而，实验仅在近水平表面（瓷砖、环氧树脂、沥青）完成，不平坦地形、坡度变化与载荷变化的影响因当前实验平台限制未予考察，方法在上述条件下的适用性仍需进一步验证。