Journal of Shenyang Aerospace University >

2026 , Vol. 43 >Issue 3: 61 - 68

DOI: https://doi.org/10.3969/j.issn.2095-1248.2026.03.008

Information Science and Engineering

Relocalization algorithm for unmanned vehicles based on wheel odometry assisted vision

  • Pingping QU , 1 ,
  • Haonan YU 1 ,
  • Tengli YU 2 ,
  • Yunhao CHEN 4, 5 ,
  • Zhonghong ZHOU 1 ,
  • Wenqiang HUO 1 ,
  • Ershen WANG 1, 3
Expand
  • 1. College of Electronic and Information Engineering，Shenyang Aerospace University，Shenyang 110136，China
  • 2. College of Aerospace，Shenyang Aerospace University，Shenyang 110136，China
  • 3. College of Civil Aviation，Shenyang Aerospace University，Shenyang 110136，China
  • 4. School of Electrical and Information Engineering，Yunnan Minzu University，Kunming 650500，China
  • 5. Yunnan Key Laboratory of Unmanned Autonomous Systems，Yunnan Minzu University，Kunming 650500，China

Received date: 2025-05-07

  Revised date: 2025-05-23

  Accepted date: 2025-05-25

  Online published: 2026-07-06

Fold

Abstract

To address the issue of SLAM-based visual relocalization failure in unmanned vehicles under visual degradation conditions，an adaptive collaborative relocalization algorithm was proposed，which combined wheel odometry （WO） with visual odometry （VO）.This algorithm constructed a WO/VO fusion model based on error-state Kalman filter （ES-KF），enhancing the positioning accuracy of the unmanned vehicle’s path planning.To tackle the challenge of insufficient visual feature points that hinder relocalization during unmaned vehicle operation，an adaptive collaborative relocalization algorithm for WO/VO was proposed.The system included a switching mechanism between WO/VO and WO，improving the continuity of the unmanned vehicle’s localization and the reliability of path planning.Experiments were conducted and compared on the developed unmanned vehicle platform under various conditions.The results demonstrate that the ES-KF-based WO/VO fusion method significantly improves localization performance.The absolute trajectory error （ATE） is reduced by 39.1% and 59.2%，respectively，compared to VO-only and WO-only methods，and path overlap is increased by 9.2% and 11.5%，respectively.The algorithm also successfully achieves WO-assisted adaptive collaborative relocalization in visual degradation scenarios.Compared to the extended Kalman filter （EKF） and the unscented Kalman filter （UKF），the ES-KF demonstrates superior performance in both trajectory accuracy and pose estimation stability，significantly enhancing path fusion effectiveness and system robustness.

Key words： relocalization; visual odometry; wheel odometry; unmanned vehicle; error-state Kalman filter

Cite this article

Pingping QU , Haonan YU , Tengli YU , Yunhao CHEN , Zhonghong ZHOU , Wenqiang HUO , Ershen WANG . Relocalization algorithm for unmanned vehicles based on wheel odometry assisted vision[J]. Journal of Shenyang Aerospace University, 2026 , 43(3) : 61 -68 . DOI: 10.3969/j.issn.2095-1248.2026.03.008

视觉传感器［1］因成本低、信息量大，成为SLAM发展的关键推动力［2-4］。但其高度依赖于环境纹理与光照，在极端场景下易出现特征匹配失败，导致定位漂移甚至系统崩溃［5-7］。高速运动或低纹理区域异常造成纯视觉定位失效［8-10］，因此，研究适用于视觉退化场景的重定位与路径规划技术，对提升无人车视觉SLAM系统在复杂环境下的适应性与行驶安全性具有重要意义。
传统视觉SLAM依赖特征点匹配，退化场景下易因特征稀疏或误匹配而失效。曹学伟等［1］引入签名直方图（signature of histograms of orientations，SHO）点云配准，位姿精度提升10%。朱代先等［2］改进定向FAST与旋转BRIEF（oriented FAST and rotated BRIEF，简称ORB）算法，结合边缘采样一致性增强（marginalizing sample consensus++，MAGSAC++）算法，匹配率提升14%；在低纹理场景中，多传感器融合尤为关键［5，11-12］。邓鹏等［13］系统分析了复杂环境下机器人多传感器融合的定位性能与优化策略。集成激光雷达、惯性测量单元（inertial measurement unit，IMU）、轮式里程计（wheel odometry，WO）与超宽带（ultra-wideband，UWB）［14］成为趋势：马小陆等［5］提出视觉-激光协同方法；岑志豪等［6］采用误差状态卡尔曼滤波（error-state Kalman filter，ES-KF）融合多源感知，实现0.07 m定位精度；张智博等［11］构建激光-视觉系统，误差5.1 mm。但激光成本高、IMU易累积误差，限制实用性［15］。深度学习方法如图神经网络（graph neural network，GNN）、YOLOv5等也取得进展［3，16-17］，但对数据与算力依赖度大，难以部署于嵌入式平台［8，18］。而ORB-SLAM3是基于视觉-惯性融合的SLAM算法，通过融合ORB特征点与IMU数据实现定位建图，并生成稀疏点云表征环境拓扑［19］。该算法适用于实时定位，但稀疏点云缺乏几何与语义信息，因而需依赖外部建模模块支撑路径规划。
综上，视觉退化下的重定位面临如下问题：1）视觉传感器稳定性差、激光雷达与IMU等多源传感器辅助方案则系统复杂度过高［5-6，11］；2）自适应蒙特卡洛定位（adaptive Monte Carlo localization，AMCL）算法计算量大，仅适用于激光雷达［4，20］；3）缺乏适用于路径规划的替代定位方案［21］。为此，本文提出一种WO辅助VO的自适应协同重定位方法。该方法基于ES-KF构建WO/VO融合算法，并设计WO/VO与WO系统间的切换机制，提升路径规划时的定位精度与视觉失效下的定位连续性。

1 基于视觉的EPnP重定位算法

在ORB-SLAM3中，视觉重定位结合了词袋（bag of words，BoW）方法与高效透视n点相机姿态估计（efficient perspective-n-point，EPnP）算法。BoW用于从数据库中检索与当前帧最相似的关键帧，获得对应的3D-2D匹配点对。接着，EPnP算法利用这些匹配点估计当前相机的位姿。
每个3D点 P i通过4个虚拟控制点 C j线性组合表示，如式（1）所示。
P i = ∑ j = 1 4 α i j C j   , ∑ j = 1 4 α i j = 1
式中： α i j为重心坐标。3D点 P i c = ( X i c , Y i c , Z i c )在相机坐标系下的投影为
u i v i 1 ~ K 1 Z i c X i c Y i c Z i c
式中： u i   v i为 P i c在像素坐标系下的坐标。
那么，控制点 C j在相机坐标系下的坐标如式（3）所示。
C j c = R v o C j + t v o
式中： R v o为旋转矩阵； t v o为平移向量。
将控制点代入相机投影方程，得到线性方程系统为
M ⋅ x = 0
求得 M，从而得到旋转矩阵 R v o和平移向量 t v o。

2 WO/VO自适应协同重定位算法

针对视觉退化场景下单一视觉重定位失效的问题，本文采用能够提供连续、稳定位姿信息的WO辅助VO实现协同重定位。为提升路径规划的鲁棒性与精度，提出一种基于误差状态扩展卡尔曼滤波（ES-KF）的WO/VO融合算法。WO/VO的自适应协同重定位算法框图如图1所示，在视觉重定位正常的情况下，系统采用ES-KF融合算法，实现高精度位姿估计与路径规划；当视觉重定位失效时，则依赖WO单独完成路径规划，保障系统连续运行。主要算法流程如下：输入VO、WO数据；判断VO可用性；当VO可用时，经ES-KF进行WO/VO融合算法路径规划：构建误差状态向量、动力学及多模态观测模型，做状态预测并更新，实现重定位。当VO不可用时，切换至WO更新位置；输出融合位姿用于路径规划并迭代。
图1 WO/VO的自适应协同重定位算法框图
在视觉正常时，使用ES-KF融合算法，主要过程如下：
1） 误差状态向量
为同时估计位姿、速度与传感器偏差，构建15维误差状态向量 δ x如式（5）所示。
δ x = δ p T , δ v T , δ θ T , δ b a T , δ b g T T ∈ R 15
式中： δ p ∈ R 3，为全局坐标系下的位置误差； δ v ∈ R 3，为车身坐标系下的速度误差； δ θ ∈ R 3为姿态角误差； δ b a与 δ b g分别为加速度计与陀螺仪的零偏误差。
2）动力学模型
基于WO的轮速测量值 ω l 、 ω r与IMU角速度 ω，建立离散时间状态传播方程如式（6）所示。
δ x k k - 1 = F k δ x k - 1 + G k w k
式中： G k为过程噪声输入矩阵；系统矩阵 F k由运动学雅可比矩阵导出。过程噪声 w k服从 𝒩 0 , Q k分布。
F k = I 3 I 3 Δ t 0 0 0 0 I 3 - S v Δ t 0 0 0 0 I 3 - ω × Δ t 0 - I 3 Δ t 0 0 0 I 3 0 0 0 0 0 I 3
式中： S v为速度向量的斜对称矩阵； ω ×为角速度的叉乘矩阵； Δ t为采样时间间隔。
3）多模态观测模型
VO通过EPnP计算得到相邻帧间的相对位姿变换 T v o = R v o , t v o，因此观测方程表示为
z v o = t v o l o g R v o      = H v o δ x + v v o , v v o ∼ 𝒩 0 , R v o
式中： l o g ⋅将旋转矩阵转换为李代数向量；VO的观测矩阵 H v o用于描述状态变量与观测之间的关系； R v o为噪声协方差。
WO通过左右轮速解算线速度 v与角速度 ω，结合IMU数据融合得到绝对位姿增量 z w o为
z w o = Δ p w o Δ θ w o      = H w o δ x + v w o , v w o ∼ 𝒩 0 , R w o
式中： Δ p w o为位置增量； Δ θ w o为姿态增量； R w o为噪声协方差； H w o为WO的观测矩阵。
4）状态预测方程
根据视觉与WO更新系统状态，采用加权平均的方式融合两者的观测。
计算VO卡尔曼增益 K v o为
K v o = P k | k - 1 H v o T ( H v o P k | k - 1 H v o T + R v o ) - 1
式中： P k为预测误差协方差矩阵。
根据观测残差修正状态 δ x ^ k为
δ x ^ k = δ x ^ k | k - 1 + K v o z v o - H v o δ x ^ k | k - 1
更新误差协方差矩阵为
P k = ( I - K v o H v o ) P k | k - 1
计算WO卡尔曼增益 K w o为
K w o = P k | k - 1 H w o T ( H w o P k | k - 1 H w o T + R w o ) - 1
根据观测残差修正状态 δ x ^ k为
δ x ^ k = δ x ^ k | k - 1 + K w o z w o - H w o δ x ^ k | k - 1
更新误差协方差矩阵为
P k = ( I - K w o H w o ) P k | k - 1
5）状态更新方程
在卡尔曼滤波器的框架下，更新时将视觉与WO的增益和残差结合为
δ x ^ k = δ x ^ k | k - 1 + K v o ( z v o - H v o δ x ^ k | k - 1 )        + K w o ( z w o - H w o δ x ^ k | k - 1 )
更新误差协方差矩阵为
P k = ( I - K v o H v o - K w o H w o ) P k | k - 1
视觉与WO的观测残差 r v o和 r w o根据卡尔曼增益 K v o和 K w o加权，得到更新后系统的估计状态 δ x ^ k。
当视觉传感器在无人车运行过快或低纹理环境中无法匹配有效特征点时，VO将失效，导致无人车无法准确感知自身位姿，从而失去路径规划能力。为确保导航任务的连续性与可靠性，本系统设计了WO/VO的自适应协同机制，当检测到VO失效时，系统可实时无缝切换至WO工作模式，利用WO提供的连续位姿信息维持路径规划与导航控制，确保无人车在视觉信息受限条件下依然能够稳定运行，完成路径规划任务。

3 实验平台搭建与验证分析

3.1 无人车平台建立

为评估WO/VO自适应协同重定位算法的性能与有效性，本文搭建了一套全向移动无人车平台，如图1所示。该平台集成多种传感器，如图2a所示，核心硬件包括 Jetson Nano B01（4 GB）（工控主板）、STM32F103RCT6（微控制器）、Astra Pro Plus深度相机、ADIS16505惯性测量单元（IMU）、Jgb-520减速电机（含编码器）及麦克纳姆轮驱动系统。
图2 全向移动无人车平台
硬件系统架构如图2b所示，主要包括上位机/主机、下位机、从机与多个传感器模块。Jetson NanoB01 4 GB兼作主控与上位机，负责获取IMU数据、彩色图像与深度图像，并下发运动控制指令。STM32F103RCT6作为下位机，执行车体控制与基础数据采集任务。PC机（运行Ubuntu系统）作为从机，与Jetson Nano B01 4 GB协同工作，基于ROS运行ORB-SLAM3算法，实现RGB-D+IMU建图、室内重定位及路径规划功能。

3.2 WO/VO自适应协同重定位算法验证

本实验旨在验证基于误差状态卡尔曼滤波（ES-KF）融合算法的室内重定位性能，重点评估其在相同环境不同路径条件下的定位精度及与真实路径的重合程度。实验在图2a所示的无人车平台上测试，测试环境如图3所示。
为实现有效对比，实验分别测试了3种情况下的里程计性能：单独使用VO、WO及二者融合后的ES-KF里程计。实验中，真实轨迹由LD600-I UWB模块提供，该模块采用Deca Wave DW1000核心测距芯片，提供厘米级的轨迹精度。无人车依次沿直线、旋转和Z字形3种不同运动轨迹行驶。每种运动轨迹分别对比3种里程计估计结果与真实轨迹之间的重合度。
图4为3种运动轨迹下，VO、WO、ES-KF融合轨迹与真实路径的对比情况。在图4a中，起始轨迹接近，随后VO波动加剧，WO平滑但有偏差，ES-KF融合轨迹始终贴合真实轨迹。在图4b中，起始轨迹重合，VO偏离后带动WO误差，ES-KF融合轨迹保持良好。在图4c中，整体贴合较好，ES-KF融合轨迹几乎重合，VO和WO轨迹略有偏差但稳定。因此，ES-KF融合轨迹算法在3种路径场景下均表现出优于单一传感器的定位性能，在轨迹估计的准确性和稳定性方面具有明显优势。
图4 3种运动轨迹下，V0、W0、ES-KF融合轨迹与真实路径的对比情况
本文采用绝对轨迹误差（absolute trajectory error，ATE）、相对位姿误差（relative pose error，RPE）及路径重合度3个重要指标评估路径规划定位精度［22］。表1为在直线路径、旋转路径和Z字形路径3种不同路径类型下，VO、WO及ES-KF融合里程计3种算法的ATE、RPE和路径重合度的轨迹误差对比表。由表1可知，ES-KF融合里程计在各路径下的ATE小于WO，和VO、RPE相近，路径重合度更优，重定位性能最佳。
表1 轨迹误差对比表
路径类型 算法 ATE/m↓ RPE/m↓ 路径重合度↑
直线行驶 VO 0.445 7 0.002 4 0.765 0
WO 0.718 3 0.002 6 0.853 0
ES-KF 0.219 5 0.002 7 0.896 0
旋转路径 VO 0.623 1 0.002 2 0.896 0
WO 0.744 7 0.002 2 0.775 0
ES-KF 0.410 1 0.002 3 0.908 0

Z字形

路经

 VO 0.230 0 0.006 1 0.865 0
WO 0.424 8 0.006 1 0.842 0
ES-KF 0.155 3 0.006 8 0.945 0

注：↓表示数值越小越好，↑表示数值越高越好，下同。

ES-KF/EKF/UKF融合路径误差对比如表2所示。本文基于ATE与RPE评估ES-KF在VO/WO融合中的定位性能。实验环境如图3所示，以UWB数据为参考，分别在3种轨迹下使用ES-KF、EKF和UKF进行融合实验，计算对应的ATE与RPE均值。结果显示，ES-KF在ATE与RPE上均优于EKF和UKF，表现出更高的轨迹精度与估计稳定性。
表2 ES-KF/EKF/UKF融合路径误差对比
算法 ATE/m↓ RPE/m↓
ES-KF 0.131 4 0.006 2
EKF 0.141 2 0.036 4
UKF 0.163 3 0.040 4
图5为视觉重定位丢失的无人车路径规划。
图5 视觉重定位丢失的无人车路径规划
如图5所示，当无人车在路径规划过程中视觉退化时，VO无法为无人车提供正确的位姿，那么由WO替代VO失效时的位姿来继续进行路径规划。
经实验验证对比，ES-KF融合框架有效提升了VO与WO的协同精度，引入的智能切换机制可在VO失效时实现无缝过渡，保障系统连续运行。在直线、旋转、Z字形3类路径测试中，ES-KF在ATE、RPE及路径重合度3项指标上均优于VO与WO单一方案，ATE较单传感器降低50.8%、34.2%、32.5%，路径重合度提升超10%，且RPE波动更小。与传统EKF、UKF算法相比，ES-KF在轨迹精度与位姿估计稳定性上也展现出明显优势。

4 结论

本文提出一种基于WO辅助VO的自适应协同重定位算法，克服了视觉退化环境中SLAM重定位失败的问题。该算法采用ES-KF融合WO/VO，有效提高了路径规划精度。实验结果表明，相比单一VO或WO，融合方法的ATE分别降低39.1%和59.2%，路径重合度分别提高9.2%和11.5%，且ES-KF在精度与稳定性上优于EKF和UKF。该算法在视觉退化场景下实现了WO辅助的自适应协同重定位，显著提升了系统鲁棒性。

References

Publishing order | Descend order by publishing year | Descend order by cited within
[1]
曹学伟，袁杰，梁荣光.基于特征点云配准的SLAM重建深度优化方法［J］.计算机工程与设计，2025，46（3）： 657-664.

[2]
朱代先，魏嘉鑫，刘树林.改进ORB特征匹配的视觉SLAM定位算法［J］.激光与光电子学进展，2025，62（12）： 168-175.

[3]
纪泽源，于潇颖，付文兴.基于图神经网络特征点匹配的视觉SLAM算法［J］.仪器仪表学报，2024，45（9）： 34-43.

[4]
陈宗海，裴浩渊，王纪凯，等.基于单目相机的视觉重定位方法综述［J］.机器人，2021，43（3）： 373-384.

[5]
马小陆，张睿，郑旗，等.视觉辅助激光雷达的移动机器人定位系统［J］.湖北理工学院学报，2025，41（1）： 1-6.

[6]
岑志豪，谷德峰，侯文健.可重定位的激光视觉惯性室内定位方法［J］.导航定位学报，2024，12（4）： 165-173.

[7]
Labbé M， Michaud F.Multi-session visual SLAM for illumination-invariant re-localization in indoor environments［J］.Frontiers in Robotics and AI，2022，9：801886.

[8]
谭立超.基于时间同步优化和特征筛选的无人机单目视觉惯导联合定位方法［J］.武汉大学学报（工学版），2023，56（2）： 237-243.

[9]
包加桐，杨圣奥，朱润辰，等.视觉与激光结合的室内移动机器人重定位方法［J］.中国测试，2021，47（11）： 1-7.

[10]
郭关有，姚仁和，杜白雨，等.基于双目视觉的无人机室内定位研究［J］.沈阳航空航天大学学报，2019，36（5）： 66-75.

[11]
张智博，邹喜华，邓果.基于激光与视觉融合的复合机器人高精度导航系统［J］.组合机床与自动化加工技术，2023（12）： 30-33.

[12]
龚坤，徐鑫，陈小庆，等.弱纹理环境下融合点线特征的双目视觉同步定位与建图 ［J］.光学精密工程，2024，32（5）： 752-763.

[13]
邓鹏，罗静.复杂环境下机器人多传感器融合定位方法 ［J］.电子测量与仪器学报，2023，37 （12）： 48-57.

[14]
李玉峰，王君祎，李硕磊.一种基于UWB的高精度室内定位系统设计［J］.沈阳航空航天大学学报，2021，38（6）： 68-76.

[15]
赵文龙，高建烨，何涛.融合多传感器的自主AGV定位研究［J］.现代制造工程，2021（10）： 85-90.

[16]
Zhang K， Meng X L， Wang Q.An end-to-end lear-ning framework for visual camera relocalization using RGB and RGB-D images［J］.Measurement Science and Technology，2024，35（9）：096301.

[17]
Wang Y， Zhang Y， Hu L H，et al.A semantic topo-logy graph to detect re-localization and loop closure of the visual simultaneous localization and mapping system in a dynamic environment［J］.Sensors，2023，23（20）：8445.

[18]
Liu Y J， Zhao C S， Wei Y L.A robust localization system fusion vision-CNN relocalization and progressive scan matching for indoor mobile robots［J］.Applied Sciences，2022，12（6）：3007.

[19]
Campos C， Elvira R， Rodriguez J J G，et al.ORB-SLAM3：an accurate open-source library for visual，visual-inertial，and multimap SLAM［J］.IEEE Transactions on Robotics，2021，37（6）：1874-1890.

[20]
蒋林，李云飞，汤勃，等.基于语义物体尺寸链的改进自适应蒙特卡洛动态定位研究［J］.机械工程学报，2024，60（15）： 49-59.

[21]
蔡旭东，王永才，白雪薇，等.基于先验地图的视觉重定位方法综述 ［J］.软件学报，2024，35（2）： 975-1009.

[22]
Sturm J， Engelhard N， Endres F，et al.A benchmark for the evaluation of RGB-D SLAM systems［C］//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.Vilamoura-Algarve：IEEE，2012：573-580.

Options
Outlines

/

〈 〉

Website Copyright © Editorial Department of Journal of Shenyang Aerospace University
Tel: 024-89723779　
Fax: 024-89723779　
E-mail: shxbok@163.com
Powered by Beijing Magtech Co. Ltd