越野环境下轮速里程计与VIO紧耦合的定位算法

Positioning Algorithm with Tightly Coupled Wheel Odometry and VIO in Off-Road Environments

摘要: 针对视觉惯性里程计(Visual-Inertial Odometry, VIO)在越野环境中定位性能显著下降的问题，本文提出了一种基于轮速里程计与VIO紧耦合的算法HW-VIO (Hybrid Wheel-VIO)。该算法融合了IMU与轮速里程计的特点，设计了混合预积分观测模型，并利用轮速里程计的零速更新校正IMU加速度计和陀螺仪的偏置误差。为改善轮速计异常值频发的问题，本文引入卡方检验算法，对混合预积分残差进行评估，从而稳健识别并剔除异常数据。最后，在三种难度不同的野外农田场景中对算法进行了测试。实验结果表明，本文算法能够显著提高VIO系统的性能，平均定位精度提升47%。此外，通过消融实验进一步验证了混合预积分观测模型的有效性，相较于直接进行轮速融合的W-VIO (Wheel-VIO)算法，平均定位精度提升达50%。

Abstract: The declining localization performance of Visual-Inertial Odometry (VIO) in off-road environments is a significant challenge. To address this issue, a tightly coupled algorithm named HW-VIO (Hybrid Wheel-VIO) is proposed, combining wheel odometry and VIO. The method leverages the complementary properties of IMU and wheel odometry by introducing a hybrid pre-integration observation model, where zero-velocity updates from wheel odometry are employed to dynamically correct accelerometer and gyroscope biases in the IMU. To handle the frequent occurrence of outliers in wheel odometry measurements, a chi-squared test is applied to evaluate residuals from the hybrid pre-integration process, enabling robust identification and rejection of abnormal data. The algorithm is validated through experiments conducted in three off-road farmland scenarios with varying levels of difficulty. Results show that HW-VIO significantly improves localization accuracy, achieving an average accuracy improvement of 47%. Furthermore, ablation studies confirm the effectiveness of the hybrid pre-integration model, demonstrating a 50% improvement in localization accuracy compared to the W-VIO (Wheel-VIO) algorithm, which directly fuses wheel odometry.

1. 引言

随着移动机器人技术的快速发展，地面机器人在越野环境中的应用需求显著增加，特别是在太空探索、灾害救援和农业等关键领域[1]。然而，这些复杂场景中精确定位始终是一项挑战。由于多径效应和遮挡等因素，全球定位系统(GPS)的信号经常出现退化甚至完全不可用的情况，难以满足高精度的定位需求。因此，如何利用相机、惯性测量单元(IMU)、激光雷达(LiDAR)以及轮速里程计等多种传感器实现高精度状态估计，成为了机器人定位领域的研究热点[2]。

视觉惯性测量(Visual-Inertial Odometry, VIO)系统因其环境信息丰富、性价比高等特点，在机器人定位领域得到了广泛研究与应用[3]。现有的VIO系统主要分为两类：基于卡尔曼滤波的滤波方法和基于非线性优化的最大后验估计(MAP)方法。前者包括ROVIO [4]、S-MSCKF [5]和OpenVINS [6]，具有较低的计算复杂度，适合实时应用；后者包括OKVIS [7]、VINS-Fusion [8]和ORB-SLAM3 [9]，通过全局优化以实现更高的定位精度。研究表明，上述VIO系统在地面越野环境中的性能存在显著下降。例如，文献[10]在田间环境中对现有VIO系统进行了测试，而文献[11]在沙地越野场景下进行了实验评估。结果均表明，当前的VIO系统在复杂的越野环境中面临严重的性能限制。

这种性能下降的原因主要体现在两方面：首先，越野环境中地形崎岖[12]、光照变化剧烈以及纹理特征高度相似等问题显著增加了特征提取与匹配的难度；其次，地面机器人在越野场景中的匀速或匀加速运动模式会改变VIO系统的可观测性特性，导致某些自由度(例如尺度)变得不可观测[13]。

针对上述问题，本文提出了一种基于轮速里程计与VIO紧耦合的HW-VIO (Hybrid Wheel Visual-Inertial Odometry)算法，主要贡献包括：

1) 通过结合轮速里程计与IMU各自的特点，设计了一种新颖的混合预积分观测模型，在越野环境下能够提供更为可靠的观测信息；

2) 利用轮速里程计进行零速更新，以矫正IMU的加速度计与陀螺仪的偏置，降低传感器误差；

3) 针对越野场景中频繁出现异常值的问题，本文引入卡方检验算法，用于对混合预积分观测残差进行评估，从而稳健地识别并剔除异常数据。

2. 相关工作

轮速里程计不仅能提供可靠的尺度信息，还具有易于安装和低成本的优势，与VIO的结合可显著提升系统的精度和鲁棒性[14]。然而，在越野环境下，轮速里程计的融合仍面临两大关键挑战：一是角速度测量误差，二是车轮打滑问题[15]，这对系统的稳定性与定位精度提出了更高的要求。

针对角速度测量误差，文献[16]提出了基于MSCKF的VIWO (视觉–惯导–轮速计里程计)系统，通过对轮速信息进行预积分，将2D结果与3D视觉惯性数据融合。文献[17]则采用非线性优化的方法，将编码器误差、陀螺仪误差和重投影误差纳入代价函数中进行优化。这些方法在平坦场景中取得了一定的性能提升，但大多假设机器人在预积分期间运动接近平稳，难以适应越野环境中地形复杂多变的情况。当车辆在颠簸或不平坦地面行驶时，观测误差显著增加，导致定位精度下降。

为进一步地解决上述问题，文献[18]提出了一种改进方法，在预积分阶段融合IMU与车轮编码器的数据，通过4-DoF非线性优化实现更准确的尺度估计，并仅需假设机器人在几毫秒内完成平面运动，从而显著提高了观测值的准确性与定位精度。然而，该方法未能有效解决车轮打滑带来的编码器测量误差问题。

为应对车轮打滑的问题，部分研究提出了基于视觉信息的异常检测与剔除策略。例如，文献[19]通过分析视觉特征点的极线误差检测打滑行为并剔除异常数据。文献[20]则通过检测视觉–车轮优化中的异常值来解决这一问题。然而，这些方法高度依赖视觉特征点的准确性，而在纹理复杂或光照变化显著的越野环境中，特征点匹配的稳定性难以保证，可能导致误检测的发生。

Figure 1. Diagram of system coordinate frames

图1. 系统坐标系示意图

3. HW-VIO算法框架

本文算法在四个不同的坐标系下构建，如图1所示。定义IMU载体坐标系为$\left\{I\right\}$ ，相机坐标系为$\left\{C\right\}$ ，而VIO使用的局部世界坐标系为$\left\{V\right\}$ ，其方向与重力方向一致，最后定义轮速里程计的坐标系为$\left\{O\right\}$ 。

算法框架如图2所示，整体由VIO状态估计和轮速里程计融合两部分组成。其中，VIO状态估计通过IMU角速度和加速度进行状态预测，提取视觉传感器中的关键点并进行光流跟踪，以提供帧间位移信息，并结合视觉观测进行状态更新。

Figure 2. Framework of the HW-VIO algorithm

图2. HW-VIO算法框架

轮速里程计融合是HW-VIO算法的核心，其主要流程如下：

1) 静止状态下的偏置校正：

当轮速传感器检测到机器人处于静止状态时，利用该信息对IMU加速度计和陀螺仪的偏置进行校正，以降低传感器漂移误差，提高状态估计的稳定性。

2) 运动状态下的混合预积分观测：

在机器人运动状态下，计算轮速里程计与IMU数据的置信度评分，并基于评分结果决定是否采用 IMU角速度数据进行拟合，以优化混合预积分观测，提高里程计数据的可靠性。

3) 异常观测剔除：

最后，通过卡方检验评估混合预积分观测残差，剔除异常数据，确保数据融合的稳健性，提高最终的状态估计精度。

4. 轮速里程计和IMU混合预积分

轮速里程计在测量线速度时具有较高的准确性，但角速度信息易受噪声干扰。而IMU在六自由度运动感知中表现灵敏，但长期测量会出现累积误差。因此，本文通过一致性评估机制，结合IMU对轮速里程计的角速度进行动态拟合，并对两者数据进行预积分，生成更可靠的混合预积分观测值以提高越野环境下机器人的定位精度，具体示意图如图3所示。本节将详细介绍该方法的模型。

4.1. 轮速里程计测量模型

轮速里程计通过其内在特性，可以将其车轮转速转换成二维运动中的线速度和角速度信息，该运动状态与车辆的结构参数密切相关。$\left\{O\right\}$ 坐标系下机器人轮速里程计的线速度和角速度的达式为：

${}^{O}w=\frac{v_r{r}_r-v_l{r}_l}{b},{}^{O}v=\frac{v_r{r}_r+v_l{r}_l}{2}$ (1)

其中，$v_l$ 、$v_r$ 分别是左轮和右轮的旋转速度，$r_l$ 、$r_r$ 分别是它们的相应半径，b表示车轮之间的轴距。进一步地，通过机器人运动学模型[16]，在参考帧$\left\{O_k\right\}$ 中的二维位置变化关系可通过如下公式描述：

$\begin{array}{l}{}_{O_k}{}^{O_i}\dot{\theta }={}^{O_i}\omega \\ {}^{O_k}\dot{x}{}_{O_i}={}^{O_i}v\cos \left({}_{O_k}{}^{O_i}\theta \right)\\ {}^{O_k}\dot{y}{}_{O_i}={}^{O_i}v\sin \left({}_{O_k}{}^{O_i}\theta \right)\end{array}$ (2)

其中，${}^{O_k}\dot{x}{}_{O_i}$ 、${}^{O_k}\dot{x}{}_{O_i}$ 和${}^{O_k}\dot{y}{}_{O_i}$ 分别表示从$O_k$ 到$O_i$ 的，相对二维位置变化的速率，i表示时间$t_i\in \left[t_k,t_{k+1}\right]$ 内的某一时刻。

Figure 3. Diagram of hybrid pre-integration of wheel odometry and IMU

图3. 轮速里程计和IMU混合预积分示意图

4.2. IMU与轮速里程计混合预积分方法

为监测轮速里程计角速度异常情况，本文引入基于CM距离(Cramér-von Mises Distance)的评估方法。CM距离在检测传感器数据不一致性方面响应迅速，能够有效评估数据可靠性[21]。其定义如下：

$D_{CM}={\left(2{\displaystyle {\int }_{-\infty }^{\infty }{\left(F_P\left(x\right)-F_Q\left(x\right)\right)}^2}\right)}^{1/2}$ (3)

其中，$F_P\left(x\right)$ 和$F_Q\left(x\right)$ 分别为两个经验累积分布函数。本文在时间窗口$\Delta T$ 内，基于轮速计和IMU的速度计算CM距离，并通过评分指标$S_O$ 表示可靠性：

$\begin{array}{l}{C}_{I,O}=D_{CM}^{\text{Δ}T}\left(I,O\right)\\ S_O=\frac{\text{2}}{1+\exp \left({\lambda }_i\cdot C_{I,O}\right)}\end{array}$ (4)

其中，$C_{I,O}$ 为时间窗口$\Delta T$ 内IMU和轮速里程计的一致性指标。$S_O$ 使用改进的Sigmoid函数实现0~1之间的平滑映射。参数${\lambda }_i$ 用于调整函数的陡峭程度，从而控制评分对一致性指标的敏感性。

根据置信度评分指标，判断轮速计的角速度是否需要IMU拟合。拟合后的角速度定义如下：

${}^O\widehat{\omega }=\{\begin{array}{l}{e}_3^{\text{T}}{}_I{}^{O}R\left({\omega }_a^I+\frac{{\omega }_b^I-{\omega }_a^I}{t_b-t_a}\left(t-t_a\right)-b_{\omega }\right),S_O<S_O^{th}\\ {}^O\omega ,S_O\ge S_O^{th}\end{array}$ (5)

在其中，当轮速计评分$S_O$ 小于$S_O^{th}$ 时。利用IMU的角速度信息进行拟合[15]，$t_a$ 和$t_b$ 为最接近当前轮速计测量值的IMU数据，左乘$e_3^{\text{T}}$ 表示从IMU的角速度信息中提取z轴旋转方向。若评分高于阈值，则直接保留当前轮速计的角速度信息${}^O\omega$ 。因此，$t_k$ 到$t_{k+1}$ 的二维位置信息的混合预积分观测$z_{O_k}$ 可表示为：

$z_{O_k}=\left[\begin{array}{l}{}_{O_{k-1}}^{O_k}\theta \\ {}_{O_{k-1}}^{O_k}x\\ {}_{O_{k-1}}^{O_k}y\end{array}\right]=\left[\begin{array}{l}{\displaystyle {\int }_{t_{k-1}}^{t_k}{}^{O_t}\widehat{\omega }\text{d}t}\\ {\displaystyle {\int }_{t_{k-1}}^{t_k}{}^{O_t}v\cos \left({}_{O_{k-1}}^{O_t}\theta \right)\text{d}t}\\ {\displaystyle {\int }_{t_{k-1}}^{t_k}{}^{O_t}v\sin \left({}_{O_{k-1}}^{O_t}\theta \right)\text{d}t}\end{array}\right]$ (6)

5. 基于轮速里程计紧耦合的HW-VIO算法

5.1. 系统状态向量

在第$t$ 时刻下，状态向量由当前惯性状态$x_{I_t}$ 和多个历史IMU位姿$x_{C_t}$ 组成[22]。所有的状态向量基于参考系$\left\{V\right\}$ 进行描述：

$\begin{array}{l}{x}_t={\left[\begin{array}{cc}{}^{V}x{}_{I_t}^{\text{T}}& {}^{V}x{}_{C_t}^{\text{T}}\end{array}\right]}^{\text{T}}\\ x_{I_t}={\left[\begin{array}{ccccc}{}_V{}^{I_t}\overline{q}{}^{\text{T}}& {}^{V}p{}_{I_t}^{\text{T}}& {}^{V}v{}_{I_t}^{\text{T}}& b_{{\omega }_t}^{\text{T}}& b_{a_t}^{\text{T}}\end{array}\right]}^{\text{T}}\\ x_{C_t}={\left[\begin{array}{ccccc}{}_V{}^{I_{t-1}}\overline{q}{}^{\text{T}}& {}^{V}p{}_{I_{t-1}}^{\text{T}}& \cdots & {}_V{}^{I_{t-n}}\overline{q}{}^{\text{T}}& {}^{V}p{}_{I_{t-n}}^{\text{T}}\end{array}\right]}^{\text{T}}\text{}\end{array}$ (7)

其中，${}_V{}^{I_t}\overline{q}$ 描述的是从$\left\{V\right\}$ 到$\left\{I\right\}$ 的旋转，向量${}^{V}p{}_{I_t}^{}$ 和${}^{V}v{}_{I_t}^{}$ 分别表示$\left\{I\right\}$ 在$\left\{V\right\}$ 下的位置信息和速度、$b_{{\omega }_t}^{}$ 和$b_{a_t}^{}$ 分别为陀螺仪和加速度计的偏置。

5.2. 状态预测

系统利用IMU的加速度$a_m$ 和角速度$w_m$ 信息进行状态预测，在考虑传感器偏置和噪声的情况下，定义为：

$\begin{array}{l}{a}_m=a+{}_V{}^{I}Rg+b_a+n_a,\\ w_m=w+b_w+n_w.\end{array}$ (8)

其中，$a$ 和$w$ 为真实值，重力加速度$g\approx {\left[0\text{ 0 9}\text{.81}\right]}^{\text{T}}$ ，$n_a$ 和$n_w$ 为测量噪声其服从高斯分布。利用$t-1$ 时刻状态信息，对t时刻状态进行预测可表示为：

$\begin{array}{l}{}^V{\widehat{x}}_{t|t-1}=f\left({}^V{\widehat{x}}_{t-1|t-1},a_{m_t},{\omega }_{m_t}\right)\\ P_{t|t-1}={\Phi }_{t-1}{P}_{t-1|t-1}{\Phi }_{t-1}^{\text{T}}+Q_{t-1}\end{array}$ (9)

其中，${\widehat{x}}_{i|j}$ 表示利用j时刻的测量信息，对i时刻的结果进行预测。$P_{t|t-1}$ 为协方差矩阵预测结果，${\Phi }_{t-1}$ 和$Q_{t-1}$ 是状态转移矩阵和噪声协方差矩阵。

5.3. 状态更新

5.3.1. 视觉更新

相机运行时，会提取图像中的特征点并通过光流进行跟踪。接着，通过相机位姿计算得到特征点的位置估计值${\widehat{p}}_{f_t}$ ，并与时刻t的特征点观测值${\widehat{z}}_{c_t}$ 进行比较，$H_{x_t}$ 和$H_{f_t}$ 为雅可比矩阵，测量残差${\tilde{z}}_{c_t}$ 为：

${\tilde{z}}_{c_t}\approx H_{x_t}{\tilde{x}}_t+H_{f_t}{\tilde{p}}_{f_t}+n_{f_t}$ (10)

$r_{c_t}={H^{\prime }}_{x_t}{}^V\tilde{x}{}_t+{n^{\prime }}_{f_t}$ (11)

其中，特征点$f_t$ 的位置估计误差用${\tilde{p}}_{f_t}$ 表示，$n_{f_t}$ 为图像观测噪声，其协方差矩阵为$R_{f_t}$ 。进一步对式(10)中矩阵$H_{f_t}$ 进行左零空间运算，并应用于特征观测残差[22]，构建新的可以直接用于EKF的更新过程测量残差函数，记为式(11)中的$r_{c_t}$ 。

5.3.2. 零速更新

当轮式里程计检测到机器人静止(即速度为零)时，系统将定义观测到的速度、加速度和角速度均为零。在此情况下，零速更新对应的速度、加速度和角速度残差$r_v$ 、$r_a$ 和$r_{\omega }$ 分别表示为：

$\begin{array}{l}{r}_v=z_v-{\widehat{z}}_v=-v^w\\ r_a=z_a-{\widehat{z}}_a=-\left(\tilde{a}-b_a-R_w^{I}g\right)\\ r_{\omega }=z_{\omega }-{\widehat{z}}_{\omega }=-\left(\tilde{\omega }-b_g\right)\end{array}$ (12)

其中，静止假设下$z_v=z_a=z_{\omega }=0$ ，系统将通过与实际估计观测值的差异来修正状态变量。其相应的雅可比矩阵定义如下：

$\begin{array}{l}{H}_v=\frac{\partial z_v}{\partial v}=I_{3\times 3}{H}_w=\frac{\partial z_{\omega }}{\partial b_g}=-I_{3\times 3},\\ H_a=[\frac{\partial z_a}{\partial {\theta }_{bw}}\frac{\partial z_a}{\partial b_a}]=[-{\left[R_{bw}g\right]}_{\times }-I_{3\times 3}].\end{array}$ (13)

5.3.3. 混合预积分观测更新

为了使混合预积分的二维约束与系统状态向量建立观测联系，需要通过几何约束[16]将系统状态向量映射到二维预积分观测空间，定义如下：

$z_{O_t}=h\left(x_I,x_C\right)=\left[\begin{array}{l}{}_{O_{t-1}}^{O_t}\theta \\ {}_{O_{t-1}}^{O_t}x\\ {}_{O_{t-1}}^{O_t}y\end{array}\right]$ (14)

$z_{O_t}=\left[\begin{array}{c}{e}_3^{\text{T}}\log \left({}_I^{O}R{}_G^{I_t}R{}_G^{I_{t-1}}{R}^{\text{T}}{}_I{}^{O}R{}^{\text{T}}\right)\\ \begin{array}{l}{e}_{\text{1}}^{\text{T}}\left({}_I^{O}R{}_G^{I_{t-1}}R\left({}^G{p}_{I_t}+{}_G^{I_t}{R}^{\text{T}I}{p}_O-{}^G{p}_{I_{t-1}}\right)+{}^O{p}_I\right)\\ e_{\text{2}}^{\text{T}}\left({}_I^{O}R{}_G^{I_{t-1}}R\left({}^G{p}_{I_t}+{}_G^{I_t}{R}^{\text{T }I}{p}_O-{}^G{p}_{I_{t-1}}\right)+{}^O{p}_I\right)\end{array}\end{array}\right]$ (15)

其中，$e_i$ 是第i个标准单位基向量，$\log (\cdot )$ 是从$SO\left(\text{3}\right)$ 到其李代数$\mathfrak{s}\mathfrak{o}\left(3\right)$ 的对数映射，用于将旋转矩阵映射为李代数中的旋转向量。

建立观测方程后，在获取到新的混合预积分观测值后，将预积分观测量$z_{O_t}$ ，与状态向量计算得到的2D位置估计值$h\left({\widehat{x}}_I,{\widehat{x}}_C\right)$ 进行做差，得到混合预积分观测的残差记为$r_{O_t}$ ：

$\begin{array}{l}{r}_{O_t}=z_{O_t}-h\left({\widehat{x}}_I,{\widehat{x}}_C\right)\approx H_t{\tilde{x}}_t+n_{O_t}\\ H_t=\left[\frac{\partial h}{\partial x_I},\frac{\partial h}{\partial x_C}\right]\end{array}$ (16)

其中，$H_t$ 是$h\left(x_{I_t},x_{C_t}\right)$ 关于误差状态${\tilde{x}}_t$ 的雅可比矩阵。$n_{O_t}$ 为混合预积分观测的噪声向量，其协方差矩阵为$R_{O_t}$ 。最后，滤波器增益、协方差更新以及状态更新方程如下：

$\begin{array}{l}{K}_t={\widehat{P}}_{t|t-1}{H}_t^{\text{T}}{\left(H_t{\widehat{P}}_{t|t-1}{H}_t^{\text{T}}+R_t\right)}^{\text{-1}}\\ {\widehat{x}}_{t|t}={\widehat{x}}_{t|t-1}+K_t{r}_t\\ P_{t|t}=\left(I-K_t{H}_t\right)P_{t|t-1}+K{R}_t{K}^{\text{T}}\end{array}$ (17)

其中，$r_t$ 为视觉和轮速的观测残差，$R_t$ 表示为视觉和轮速观测信息的噪声矩阵。

5.4. 卡方检验异常剔除

当状态估计方程中的模型参数不包含异常误差时，残差量$r_{O_t}$ 应服从高斯分布，其协方差矩阵为$P_{O_t}$ ，均值为零，概率密度函数可表示为：

$\begin{array}{c}\rho \left(r_{O_t}\right)=N\left(r_{O_t};0;P_{O_t}\right)\\ =\frac{1}{\sqrt{{\left(2\pi \right)}^m\left|P_{O_t}\right|}}\exp \left(-\frac{1}{2}{r}_{O_t}^{\text{T}}{\left(P_{O_t}\right)}^{-1}{r}_{O_t}\right)\end{array}$ (18)

然而，当误差存在时，式(18)将不再适用。为此，本文通过构建检验统计量，并采用假设检验方法对异常值进行分析。采用在滤波定位中常用的卡方(${\chi }^2$ )检测的方法[23]，检测统计量使用马氏距离的平方来反映误差，表示为：

${\gamma }_{O_t}=M_{O_t}^2=\sqrt{r_{O_t}^{\text{T}}{\left(P_{O_t}\right)}^{-1}{r}_{O_t}}=r_{O_t}^{\text{T}}{\left(P_{n_t}\right)}^{-1}{r}_{O_t}~{\chi }_{\alpha }^2\left(m\right)$ (19)

其中，$M_t$ 即为残差$r_{O_t}$ 的马氏距离，且该统计量${\gamma }_{O_t}$ 服从显著性水平为$\alpha$ ，自由度为m的卡方分布。进一步地，判定观测值是否异常的条件为：

$\{\begin{array}{l}{\gamma }_t<{\chi }_{\alpha }^2\left(m\right),\text{ fault}\\ {\gamma }_t\ge {\chi }_{\alpha }^2\left(m\right),\text{ no fault}\end{array}$ (20)

具体而言，当误差参数值${\gamma }_t$ 小于预设的卡方临界值${\chi }_{\alpha }^2\left(m\right)$ 时，认为观测值正常。反之，则判定为异常。在此基础上，可进一步用概率形式来表示：

$\mathrm{Pr}\left[{\gamma }_t>{\chi }_{\alpha }^2\left(m\right)\right]<a$ (21)

其中，$\mathrm{Pr}[\cdot ]$ 表示事件发生的概率，当${\gamma }_t$ 超过卡方临界值${\chi }_{\alpha }^2\left(m\right)$ 的概率小于预设显著性水平a时，则认为当前混合预积分观测值正常，可用于状态更新。否则，则判定为异常，将不进行状态更新。

6. 实验结果评估与分析

6.1. 实验平台

本文提出的HW-VIO算法的VIO部分基于OpenVINS框架实现，轮速里程计部分则通过本文所设计的相关算法模块完成紧耦合。实验数据选自除草机器人在大豆田环境中采集的Rosario [24]数据集。图4展示了数据集配置的传感器及各坐标系的定义。该数据集包含了六个序列，并依据农作物的干燥程度、植被密度等因素被划分为三种难度等级：Easy、Medium和Difficult。

Rosario数据集具有多种越野环境中的复杂特性，如重复或缺失的纹理信息导致特征点提取困难(如图5所示)、阳光反射与聚焦问题，以及崎岖不平的路面所带来的振动干扰等。因此，该数据集非常适合作为实验测试对象，用以评估本文算法在越野环境下的性能。图6(a)、图6(b)和图6(c)分别展示了在三种难度序列下的采样图像。

实验结果通过EVO评估工具[25]分析，采用Umeyama方法[26]对估计轨迹与真值进行对齐，并以平均绝对位姿误差(Absolute Pose Error, APE)和其均方根误差(Root Mean Square Error, RMSE)作为主要评估指标。此外，为保证实验结果的可靠性，每组实验重复运行20次，并取平均值以减小随机性的影响。所有实验均在统一的实验平台上进行，实验环境为Core i5-8265U CPU (1.6 GHz主频)、8 GB内存，操作系统为Ubuntu 18.04。

Figure 4. Sensors configuration and coordinate frame definition of the field robot

图4. 农田机器人传感器配置及坐标系定义

Figure 5. The feature point extraction performance of the OpenVINS algorithm in agricultural field scenarios

图5. OpenVINS算法在农田场景中的特征点提取效果

(a) (b) (c)

Figure 6. Sample images and APE error curves for three difficulty levels in the Rosario dataset. (a) 02_Easy sequence; (b) 03_Medium sequence; (c) 05_Difficult sequence

图6. Rosario数据集的三种难度序列的采样图像及APE误差曲线。(a) 02_Easy序列；(b) 03_Medium序列；(c) 05_Difficult序列

6.2. 系统定位精度评估

为验证本文算法在越野环境中的定位性能，本节使用Rosario数据集中的02至06序列对不同算法的定位精度进行评估。考虑到01序列中的轮速里程计数据存在较大偏差，因此未纳入本次实验。实验共分为两个部分：首先，分析HW-VIO算法对原始VIO系统性能的提升效果；其次，开展消融实验，验证混合预积分观测模型的有效性。表1汇总了不同算法在绝对位姿误差(APE)指标下的评估结果，清晰呈现了各算法之间的性能差异。为了进一步分析误差来源，图6展示了在三种难度序列下，OpenVINS算法、原始轮速里程计(Wheel-Odom)和本文HW-VIO算法的绝对位姿误差曲线。

6.2.1. VIO性能提升分析

通过对表1的分析，可以明确看出OpenVINS算法在各序列中的误差显著高于其在结构化环境中的表现[6]，各序列的平均RMSE为3.44 m。误差增大的主要原因包括：图像纹理信息的单一性、路面崎岖带来的影响以及特殊运动对算法的干扰。例如，在02序列中，存在两个大角度转弯(如图7中RTK轨迹的红色框所示)，这导致速度和角速度的快速变化，进而影响算法光流跟踪的功能。具体表现为图6(a)中的APE误差曲线，OpenVINS算法出现了两个明显的误差尖峰，误差峰值接近12 m，极大地影响了整体定位轨迹的精度。

另一方面，原始轮速里程计(Wheel-Odom)数据在越野环境中也存在较大误差，通过深入分析其误差分布，发现线速度数据准确性较好，x轴平移数据与真值相对接近，如图8所示。然而，由于路面崎岖和打滑问题，轮速计偏航角误差较大，表现为y轴误差显著，图8中的y轴分量清晰反映了这一误差。在05序列中，由于其y轴误差的影响，导致其原始轮速数据的RMSE达到了15 m。

与之相比，本文提出的HW-VIO算法在五个序列中的表现均明显优于原始OpenVINS算法。其主要原因在于通过混合预积分有效弥补了原始轮速计在y轴上的误差，从而形成更为可靠的观测来修正VIO中的误差。如图6所示，本文算法的误差曲线较低且更为平滑稳定。在03序列中，平均APE降低至0.76 m。而在06序列中，由于序列长度为其他序列的两倍且路面更为复杂，难度较高，因此混合预积分观测的提升有限，性能改进不如其他序列明显。总体而言，较原始OpenVINS算法本文算法在五个序列中的RMSE平均降低了47%。这些结果充分验证了本文提出的紧耦合轮速里程计的HW-VIO算法在提升VIO系统定位精度方面的有效性。

Table 1. Comparison of absolute pose error (APE) for different algorithms on the Rosario dataset

表1. Rosario数据集下各算法的绝对位姿误差比较

Dataset Rosario		OpenVINS		Wheel-Odom		W-VIO (无混合预积分)		HW-VIO (Ours)
Seq	Length (m)	RMSE (m)	均值 (m)	RMSE (m)	均值 (m)	RMSE (m)	均值 (m)	RMSE (m)	均值 (m)
02_Easy	320	4.27	4.02	5.15	4.98	4.93	4.07	2.16	1.87
03_Medium	170	1.72	1.61	4.33	3.86	2.27	2.06	0.81	0.76
04_Medium	150	2.71	2.36	3.42	3.06	1.95	1.56	1.25	0.98
05_Difficult	330	3.79	3.55	19.03	15.97	4.83	4.12	2.05	1.49
06_Difficult	530	4.72	4.43	4.92	4.59	4.92	4.66	3.09	2.78

Figure 7. RTK Trajectory of Sequence (02~06)

图7. (02~06)序列的RTK轨迹

(a) (b)

Figure 8. Comparison of 2D trajectories and APE component errors of Algorithms in 02 and 05 sequences. (a) 02_Easy sequence; (b) 05_Difficult sequence

图8. 02和05序列下算法二维轨迹与APE分量误差对比。(a) 02_Easy序列；(b) 05_Difficult序列

6.2.2. 混合预积分观测消融实验

为验证混合预积分观测的有效性，本文进行了消融实验。实验中去除了HW-VIO算法中的混合预积分部分，改为直接对原始轮速计二维信息进行EKF融合，得到W-VIO (Wheel-VIO)算法。实验结果(见表1)显示，W-VIO算法仅在04序列中相较于原VIO系统误差有所降低，而在其他序列中，误差反而高于原始VIO系统。

为进一步分析误差来源，本文从残差分布和异常值角度进行讨论。表2显示了03和05序列中W-VIO与HW-VIO算法的异常值情况，图9则展示了两个序列下APE误差曲线及异常点的分布情况。结果表明，W-VIO算法的异常值占比显著高于HW-VIO算法，03序列中其异常值占比达到了83%。这些异常值主要来源于前述原始轮速计的y轴误差(见图8中Wheel-Odom的y轴分量对比)。因此，W-VIO算法中实际有效的轮速计观测相对较少。图9的误差曲线结果表明，直接融合轮速计数据未能有效修正VIO误差，反而由于轮速里程计的噪声较大，误差在融合后普遍高于原始VIO系统。

相比之下，本文提出的HW-VIO算法通过对轮速里程计角速度进行判断并结合IMU数据进行拟合，从而得到更为可靠的观测残差。图9和表2显示，在03和05序列中，混合预积分观测的残差更为平稳，异常值占比显著降低。其误差曲线明显低于不包含混合预积分观测的W-VIO算法，这进一步验证了混合预积分观测的有效性。

Table 2. Abnormal Values in 03 and 05 Sequences

表2. 03和05序列异常值情况

算法	03_Medium		05_Difficult
	异常值数量	异常值占比%	异常值数量	异常值占比%
W-VIO	1666	83.1	2306	73.5
HW-VIO	366	18.2	201	6.4

(a)

(b)

Figure 9. Outlier removal and APE error curve in 03 and 05 sequences. (a) 03_Medium sequence; (b) 05_Difficult sequence

图9. 03和05序列的异常值剔除情况及APE误差曲线。(a) 03_Medium序列；(b) 05_Difficult序列

Figure 10. Box diagram of absolute pose error (APE) for (03, 04, 06) sequences

图10. (03、04、06)序列的绝对位姿误差(APE)箱型图

最后，图10展示了03、04和06序列的绝对位姿误差(APE)箱型图，直观地对比了各算法间的性能差异。结合表1的结果，HW-VIO相较于不含有混合预积分的W-VIO算法，其RMSE平均降低了50%。因此，上述结果均表明混合预积分模型能够有效提升轮速里程计在越野环境下数据融合的质量，并且在不同难度场景下均表现出良好的适用性。

7. 结束语

本文提出了一种基于轮速里程计与视觉惯性里程计(VIO)紧耦合的定位算法HW-VIO，通过融合IMU和轮速里程计的信息，设计了混合预积分观测模型，并利用零速更新技术校正IMU传感器的偏置误差。此外，引入卡方检验方法识别并剔除异常观测值，以提高系统的鲁棒性和精度。

实验结果表明，HW-VIO算法在越野环境中的定位精度明显优于传统VIO系统。在Rosario农田数据集的测试中，该算法相较于OpenVINS，平均定位精度提升了47%。消融实验进一步验证了混合预积分观测模型的有效性，相较于仅直接融合轮速数据的W-VIO算法，定位精度提升达50%。这些结果表明，混合预积分方法能够有效提高轮速观测数据的可靠性，并增强VIO系统在复杂越野环境中的适用性。

未来研究将重点优化异常情况下的数据融合策略，以增强系统对传感器异常值的适应能力。此外，进一步改进系统的初始化方法，以提升算法在不同环境中的泛化能力和稳定性。