基于Jerk约束的加速度校正算法

刘运毅，黄美兰

(广西大学计算机与电子信息学院，广西南宁 530004)

0 引言

惯性传感器又称为IMU传感器，包括三轴的陀螺仪与加速度计，其被广泛应用于行人室内位置估计[1]、无人机姿态估计[2]等领域。目前，经典的姿态估计算法可分为2类：一类是基于陀螺仪数据的算法；另一类是基于加速度计与磁强计的融合算法。

对于前者，由于陀螺仪自身的漂移，在长时间通过角速度积分求取姿态的过程中，难免会产生误差积累，导致姿态解算不准确。为了提高姿态估计精度，此类算法常利用加速度计数据联合卡尔曼滤波来校正姿态角[3-6]。虽然各类卡尔曼滤波能在一定程度上缓解误差积累问题，但使算法实现变得更为复杂。此外，神经网络[7]等方法也被用于校正陀螺仪的误差。但是，这类算法在实际上并没有消除积分误差积累，且计算精度在一定程度上受到有用加速度分量的准确性的影响。对于另一类算法，加速度计易受到噪声以及机体运动等因素的干扰，需对由此带来的非重力加速度进行抑制处理，以获得较为精准的、有用的重力加速度成分[8-9]。简言之，这两类算法都需从加速度计读数中分离出准确的重力加速度。

为了获得准确的加速度，文献[10]联合卡尔曼滤波与全球定位系统来构建加速度修正模型，以消除非重力加速度的影响。此外，基于导航滤波[11]、互补滤波[12]等多种用于消除加速度计的外部非重力加速度以及噪声干扰的算法被提出，以确保加速度具有高度容错性。但是，这些算法的滤波处理过程会使算法复杂度显著增加。

为了快速获得目标加速度，本文提出一种新的Jerk(加加速度)信息提取方法，并在此法的约束下对加速度信号进行校正。此时，仅利用当前的陀螺仪与加速度计读数即可获得较准确的加速度信号，而不存在积分误差积累。

1 Jerk提取模型

1.1 基本IMU数学模型

IMU的姿态可以用四元数q表示，四元数微分方程[13]及其矩阵展开形式可以表示如下：

(1)

(2)

式中：w为陀螺仪输出的三轴角速度，w=[wx,wy,wz]T；q为姿态四元数,q=[qw,qx,qy,qz]T。

对于式(1)，通常使用Runge-Kutta积分迭代求解姿态四元数q，再利用卡尔曼滤波融合加速度计的输出进行姿态校正。

然而，加速度计会受到机体运动以及外界干扰因素的影响，导致其读数不仅表现为重力加速度成分。假定IMU系统处于静止状态，即无线运动加速度时，加速度计输出的加速度与姿态四元数在旋转矩阵的映射下，有如下关系：

(3)

式中：a为加速度计输出的三轴加速度,a=[ax,ay,az]T；g为重力加速度。

根据式(2)和式(3)，可知陀螺仪、加速度计与姿态间的关系，根据二者互相约束关系，可构建卡尔曼滤波姿态融合系统。

基于四元数的卡尔曼滤波姿态融合算法得到了广泛的应用，但在式(2)的长时间积分迭代时，会从根源上引入积分误差积累，使得陀螺仪的噪声和漂移在积分过程中积累，进而影响融合算法输出的精度。为避免误差积累对计算精度的影响，从加速度信息求取姿态的角度入手，提出一种无积分积累的加加速度提取算法。

1.2 Jerk模型

Jerk(加加速度)，可以用加速度的一阶导数直接计算，标记为Jd，表示如下：

(4)

式中Δt为时间常数，与IMU传感器采样频率有关，本文采样频率为200 Hz。

本文基于陀螺仪和加速度计直接融合的思路，提出新的Jerk提取算法。此法直接对传感器的输出信号进行信息融合，在获得更准确的姿态信息的同时，有效避免积分误差积累。以X轴为例进行分析，首先对X轴的加速度ax进行微分操作，得到偏微分方程，如下：

(5)

根据式(3)，有：

(6)

将式(6)按式(5)的形式进行整理，再将式(2)代入，可得到X轴在连续时间域下的Jerk：

(7)

进一步简化式(7)，可得：

(8)

以相同的方式处理Y和Z轴，获得系统在离散时间域下的三轴Jerk的新计算方法，将其标记为Jf，则有：

(9)

1.3 Jerk抗噪的理论分析

假设IMU传感器输出的加速度a和角速度w的噪声为互相独立的高斯白噪声na与nw，噪声均值均为零，方差为σna和σnw。

首先，根据式(4)分析Jd的均值，则有：

(10)

式(10)Jd表明的均值与噪声无关。

再分析Jd的方差，其计算关系式为

(11)

从式(11)可知，Jd的差分计算倍增了噪声的影响。

其次，根据式(9)分析X轴加加速度Jfx的均值，有：

E(Jfx)=-E(az+na)E(wy+nw)+E(ay+na)E(wz+nw)

=E(-azwy+aywz)

(12)

式(12)表明Jfx的均值同样与噪声无关。

假定IMU传感器各轴信号的方差一致，X轴Jfx的差分计算如下：

D(Jfx)=D[(ay+na)(wz+nw)-(az+na)(wy+nw)] =D[aywz-azwy+(ay-az)nw+(wz-wy)na] =D(aywz-azwy)+D[(ay-az)nw]+D[(wz-wy)na] =D(aywz-azwy)+2D(anw)+2D(wna)

(13)

从式(13)可以发现，噪声对Jfx方差的影响分别通过角速度、加速度分量与噪声相乘来实现。与卡尔曼滤波的核心一样，2个高斯分布信号相乘得到1个新的高斯分布，为最优估计。此时，得到方差更小、更准确的高斯分布。因此，本文提出的Jerk融合计算方法能减小噪声的影响。

1.4 Jerk抗噪的实验与分析

利用仿真实验进一步验证新的Jerk提取算法的抗噪性能。实验采用Matlab中的y120p60r30.mat数据模型，在该模型中有7个运动阶段：前3个分别是2 s内偏航120°，接着1 s内俯仰60°，然后0.5 s内滚转30°；后续的3个过程为前3个阶段的反向运动；最后一个动作综合了3个轴的转动。

表1 不同Jerk算法在不同噪声下的RMSE

表1的数据表明：直接差分计算的Jd的RMSE仅与加速度噪声有关，并且随着噪声的增加，RMSE急剧增加；本文提出的Jf的RMSE与加速度噪声以及陀螺仪噪声有关，即使噪声增加到极大的水平，Jf的RMSE仍保持较低的水平，展现了该法的优秀抗噪能力。

为了进一步验证算法在实际应用环境下的抗噪性，采用知名实验数据集EuRoC，计算并对比直接差分算法与本文Jerk提取算法的各轴的均方根误差，结果如表2所示。

表2 不同Jerk算法对EuRoc数据集的RMSE

表2的计算结果表明：本文Jerk提取算法的抗噪性能远远优于直接差分计算，且与参考真值接近。

然而，EuRoC数据集的数据序列时间较短，为了进一步验证算法的性能，应用知名实验数据集TUM-VI[14]来分析算法的抗噪性能。利用数据集中长达111 h的实验数据序列calib-imu-static2，计算出Jd×Δt的RMSE等于0.017 7，而Jf×Δt的RMSE低至0.000 7。这两个数据的对比表明，本文Jerk算法的抗噪性能比差分法提高了2个量级，而且针对超长时间的实验没有误差积累。

2 基于Jerk的加速度校正模型

在前面的讨论中，基于融合算法提出了新的Jerk提取算法。该算法具有良好的抗噪性能，其计算模型仅仅取决于当前的IMU输出，不存在积分误差积累。但算法仅仅获得Jerk信息，还未能获得直接与姿态相关的数据。为了避免引入积分误差积累，下面将进一步探讨基于Jerk的加速度校正算法。

2.1 计算模型

IMU加速度计当前时刻i的输出ai由重力加速度分量ag,i、运动加速度分量av,i、漂移bi和噪声ni组成：

ai=ag,i+av,i+bi+ni

(14)

其中，重力加速度分量ag,i为求解目标。基于前一小节的讨论，所提出的Jerk法在较高的干扰和噪声情况下，仍然可获得接近真值的Jerk信息，因而可以绕过式(14)，直接构建关于求解目标ag,i的差分方程，表示如下：

(15)

同时，考虑求解目标ag,i在X，Y，Z3个轴上的分量，它们的重力加速度的幅度具有如下约束关系：

(16)

把式(15)和式(16)扩展到m个样本，则有3m个未知变量，3(m-1)+m个方程。理论上仅仅需要3个样本数据就可以求解一组目标参数。为了简化计算，进一步整理式(15)，有：

(17)

把式(17)展开整理，再将式(16)的约束条件代入，可得到：

(18)

把式(18)扩展到更多样本，可得：

(19)

把式(19)扩展到m个样本，其中每个样本i有3个未知变量(agx,i，agy,i,agz,i)以及m-1个约束方程，并把方程组简化表示为

AΔaag,i=bΔa

(20)

由于式(20)为非线性方程，求解采用非线性数值计算方法，并构建目标函数如下：

f=(|ag,i|-9.81)+λ(AΔaag,i-bΔa)

(21)

式中λ为调节系数，为了保证式的强约束，本文设置λ=1/(m-1)。

对式(21)的求解可采用牛顿迭代法、梯度下降法等非线性数值计算方法。本文具体实现采用Matlab中的非线性最小二乘求解器lsqnonlin，所有参数采用系统默认参数，搜索初始值为加速度信号的当前200个样本的滑动平均值，并进行归一化操作，使得三轴重力加速度的模为9.81，每次计算的样本数量m同样为200。

2.2 实验及结果分析

为了观察本文提出的基于Jerk约束的加速度校正算法的性能，采用TUM-VI数据集中的Room6运动序列进行加速度校正实验。其中，这一序列包含了从实际环境中采集的实际加速度信息及其对应真值，将三轴的加速度真值视为参考量，与经过本文算法校正后的实际加速度进行比较。此时，3个轴的加速度的结果如图1所示。

(a)X轴加速度与参考真值的对比

(b)Y轴加速度与参考真值的对比

(c)Z轴加速度与参考真值的对比图1 三轴加速度与对应真值的对比

从图1可知，经过本文算法校正后的三轴加速度曲线与真值曲线几乎是重合的，只是在个别区域出现一些细小的差别。由此表明了本文的加速度校正算法可以获得较准确的目标加速度。

为了进一步证明本文算法的性能，通过TUM-VI数据集中的多个数据序列进行试验。将本文算法与经典卡尔曼滤波算法以及较新的神经网络算法[7]进行比较。其中，经典卡尔曼滤波算法采用Matlab中的imufilter模型，该滤波器利用9个状态向量跟踪陀螺仪和加速度计的估计误差，从而进行数据的更新。由于数据集未给出静态序列calib-imu-static2的参考真值，所以静态序列的真值设置为前10 s数据的平均值，并进行归一化操作，重力加速度常数同样取9.81。同样将RMSE作为评价指标，实验对比结果如表3所示。

表3 不同算法对TUM-VI数据集的RMSE

基于表3的数据进行分析：文献[7]中的神经网络法利用数据序列Room1、Room3、Room5进行训练，所以对相似环境的序列，即Room2、Room4、Room6，其性能表现较好；但在未训练过的场景中，如静态序列calib-imu-static2，其性能恶化到完全不能正常工作。本文算法与Matlab中的imufilter算法性能相当，二者的RMSE值都处在同一量级。此时，从静态序列calib-imu-static2的结果中可以看出，imufilter滤波器与前一个状态相关，其工作有一个收敛过程。

相比之下，本文提出的加速度校正算法不仅算法结构简洁，更重要的是每次计算仅仅依赖于当前的少量样本，不需要任何的先验状态信息，与系统的历史状态毫无关联，不需要事先训练，也不需要跟imufilter滤波模型一样必须根据具体传感器设置相关参数，也不需要初始化过程。再者，本文算法在未对求解式进行针对性优化的情况下，仍然具有较好的性能。可以乐观地估计，如果对非线性约束方程做针对性优化求解，将可进一步提高本文算法的性能。

3 结束语

本文提出了一种新的提取系统Jerk信息的方法，构建了新的计算模型，该模型仅依赖当前的陀螺仪和加速度计的输出来计算系统的Jerk信息。在TUM-VI和EuRoC数据集的数据基础上，对2种Jerk法进行抗噪实验发现，本文提出的Jerk法能够更好地逼近真值，进一步证明了新Jerk法在不引入积分误差积累的同时具有更好的抵抗噪声干扰的能力。然后，在新的Jerk法的约束下，构建了一个求解加速度的模型。实验结果表明，通过此模型可得到较精准的加速度，且本文算法结构简单，易于实现，对于具有较长时间的数据序列，算法的性能较好。