基于差分信息的加速度快速校正算法
时间：2023-04-21 09:50:03 来源：千叶帆 本文已影响人
覃晓兰，黄美兰，刘运毅
(广西大学计算机与电子信息学院，广西南宁 530004)
IMU(inertial measurement unit，惯性测量单元)是测量物体三轴运动加速度与角速度的装置，其核心组成部件为陀螺仪、磁力计与加速度计。常用于卫星定位[1]、无人机的姿态控制[2]、自动驾驶[3]、解算机器人的位姿信息[4]以及行人位姿估计[5]等。在姿态解算方面，目前主要包括基于互补滤波[6]、卡尔曼滤波及其非线性滤波算法[7]以及深度学习[8]等数据融合体系。这些体系融合多个传感器的信息，通过不同方式对陀螺仪与加速度计的数据进行处理，以得到精准的姿态信息。其中，基于卡尔曼滤波体系如扩展卡尔曼滤波(extended Kalman filter，EKF)[9]、误差状态卡尔曼滤波(error-state Kalman filter，ESKF)[10]等算法为当前求解姿态的主要算法之一。
在姿态解算算法中，由角速度得到姿态四元数的过程需要积分运算，但仅依靠角速度往往难以解算出准确的姿态信息，因此通常需要加速度作为辅助数据参与多传感器的融合算法。但是在加速度计受到干扰的情况下基于卡尔曼体系的融合算法会产生误差积累，导致解算姿态的效果变差。因此对于加速度计易受到干扰的场景中，不可通过主流的融合算法求解准确姿态。而由加速度直接计算姿态的方式可避免积分积累误差问题。此外，在资源受限的空间环境中，如何以轻量级算法对加速度进行快速校正也十分重要。故对IMU传感器中的加速度信息进行快速且准确的校正成了需要解决的问题。
针对IMU中加速度的校正问题，国内外也做了较多研究。如乔美英等针对加速度计的噪声建立线性误差模型，将其转换为椭球方程且优化了去噪参数，最后对姿态倾角进行误差补偿，有效提高了姿态解算精度[11]。燕斌对矿用随钻轨迹测量系统进行分析并建立了误差模型，实现MEMS加速度传感器的误差校正[12]。刘旭航等通过建模的方式求解非重力加速度和外部非重力加速度，对加速度计输出值进行修正[13]。W.Youn等通过卡尔曼滤波器对加速度读数中的非重力加速度成分进行估算，提高了无GPS信号情况下姿态计算的鲁棒性[14]。谭拥等通过角速度对加速度进行卡尔曼滤波修正[15]。
通过对干扰成分建模以实现误差补偿的算法需根据不同情形对干扰分量进行分析，而卡尔曼滤波器依赖历史状态且会产生误差积累。为了避免这些问题，在原有研究的基础上[16]，本文提出了一种低复杂度的加速度校正算法，该算法通过角速度融合计算可以得到准确的加速度差分信息这一特点，根据此差分校正加速度。在校正加速度时，通过当前及前段时刻传感器读数列出加速度与差分信息的方程组，接着解算此方程组与重力方程组成的平面方程组，进而解算出加速度。由于该算法未使用主流的卡尔曼滤波体系，因此不存在积累误差问题。此外，仅需选取短时的角速度与加速度测量值便可快速校正加速度，方便后续获得各轴的姿态角度。
1.1 算法基础
加速度与姿态是密切关联的，通过重力加速度在各个坐标轴的分量可以计算其对应于三轴的夹角信息。因此只需获得传感器坐标系下的重力加速度分量便可实现姿态角度计算。但是IMU中加速度计的读数还包含其他分量干扰，故需从加速度计的读数中校正加速度。
仅依靠单个传感器很难实现加速度的校正，因此结合IMU中陀螺仪的读数具有瞬时较为稳定的特点，考虑将陀螺仪融合加速度计进行加速度的校正。根据文献[16]可知，加速度的导数与角速度之间的关系如下：
(1)
式中：Jf为通过角速度融合计算得到的3个轴加速度导数组成的矩阵；
ax、ay、az为三轴的加速度；
wx、wy、wz为陀螺仪3个方向轴的角速度。
文献[16]证明了融合角速度求解加速度微分信息能有效抵御不同强度噪声的干扰，得到较为稳定准确的加速度差分信息。因此本文考虑根据此特点在噪声干扰情况下解算出目标加速度。
1.2 加速度校正算法推导
首先讨论加速度差分的关系，第i时刻与上一时刻的加速度差分计算公式如下：
Δar,i=ar,i-ar,i-1=Jf,i×Δt
(2)
式中：ar,i、ar,i-1为i时刻与i-1时刻的目标加速度真值；
Jf,i为由角速度参与计算的的i时刻加速度导数；
Δt为传感器采样时间间隔，由采样频率决定。
将公式拓展到前m个样本，计算i时刻与前m个时刻的加速度差分：
(3)
式中Δar,i,m表示i时刻的加速度与前m个时刻的加速度差值。
观察式(3)，可看出共有m+1个加速度真值为未知数，将每个加速度扩写为3个轴的分量，此时共有3(m+1)个未知数。再将加速度差分展开为三轴的加速度差分可得3m个方程，因此还需要其他约束方程才能解出目标加速度。考虑到目标加速度受重力约束，其约束方程表达如下：
(4)
式中：arx,i、ary,i、arz,i为3个方向的加速度真值，其组成了向量at,i。
此时对于需求解的m+1个加速度样本，式(4)可以扩写出m+1个方程。因此，对于由式(3)与式(4)组成的方程组，只要样本数大于3，便可通过此方程组解算出样本的目标加速度。下面进行方程组的求解，由式(3)可知，目标加速度可由上一时刻的目标加速度与该时刻的差分相加得到，将其代入到重力约束方程可得：
(5)
对式(5)展开并利用重力约束消除加速度平方部分，可得：
Δarx,i,1arx,i+Δary,i,1ary,i+Δarz,i,1arz,i
(6)
由式(6)得到了当前时刻加速度与加速度差分的关系，而加速度差分可利用角速度计算，因此可认为得到了加速度与角速度的关系。同理，将式(6)扩展到其他时刻的加速度差分关系式中，便能得到其他时刻的加速度差分信息与目标加速度之间的关系：
(7)
但是式(7)的未知数ar,i-k并非需要求解的当前时刻加速度，因此考虑对等式左边进行简化，期望得到仅包括加速度差分与当前时刻加速度的表达式：
Δarx,i-k,1arx,i-k+Δary,i-k,1ary,i-k+Δarz,i-k,1arz,i-k
=Δarx,i-k,1(arx,i-Δarx,i,k)+Δary,i-k，1·
(ary,i-Δary,i,k)+Δarz,i-k,1(arz,i-Δarz,i,k)
=Δarx,i-k,1arx,i+Δary,i-k,1ary,i+Δarz,i-k,1arz,i-
Δarx,i-k,1Δarx,i,k-Δary,i-k,1Δary,i,k-
Δarz,i-k,1Δarz,i,k
(8)
将式(8)代入式(7)，可得加速度的差分与当前时刻目标加速度的关系表达式：
(9)
从式(9)可看出目标加速度与多个样本的加速度差分存在联系。将差分以融合角速度的方式计算，此时仅需当前时刻与前m个时刻的角速度便可求出加速度差分，进而解算出目标加速度。而式(9)的形式可看作某平面方程，其中未知数arx,i、ary,i、arz,i可看作3个平面在3个轴的坐标集合，而差分信息为已知量，可看作平面方程的常系数参数与常量。
由一个差分公式可得到一个平面方程，对于m个样本，根据式(9)可写出多个平面方程，此时可通过求解平面方程组的方式解算出目标加速度。平面方程组至少有一个解，即公共交点，当平面方程组有多个解时，平面方程组相交于某条交线。假设由式(9)扩写的平面方程有多个解，其中一个交点为(x0，y0，z0)，交线方向为(l，n，p)，则公共交线可表示为
(10)
与此同时，目标加速度仍受重力约束关系的影响，而重力约束方程可看作由三轴目标加速度作为参数组成的球面，表示如下：
(11)
下一步求解公共交线在重力约束下的表达形式，将交线方程代入式(11)中并展开可得：
g2=x2+y2+z2
=(x0+lt)2+(y0+nt)2+(z0+pt)2
=(l2+n2+p2)t2+2(x0l+y0n+z0p)t+
(12)
式(12)是一个一元二次方程的表达式，通过求根公式求解两个根即可求解出未知数。此时t有2个解，分别将其代入公共交线方程可求解出2组目标加速度，最后再筛选出一个最优解即可。至此得到了解算目标加速度的方法，仅需陀螺仪测量值参与计算加速度差分，再根据此信息求解由式(9)展开式与重力约束方程组成的方程组。在解算方程组时，根据最小二乘法获得公共交线的方向与交点坐标，再根据式(12)计算方程的2个解并将解代入公共交线方程获得2组重力加速度，最后通过选择与平滑滤波后的加速度测量值最接近的一组解作为最优的目标加速度。
为了验证本文提出的加速度校正算法的性能，选取经典的EuRoC数据集进行实验，该数据集由搭载在微型飞行器上的IMU测量得到，采样频率为200 Hz，包括了状态测量值以及真值信息[17]。程序运行环境为MATLAB2020a，选取该数据集的多个序列进行加速度的求解，其中某一序列V1_01_easy的x轴的某段时刻采样点求解结果如图1所示。
图1 V1_01_easy序列解算的x轴加速度(部分)
图1展示了加速度计测量值、参考真值加速度、本文算法求解的x轴加速度。由图1可看出本文算法计算的目标加速度可以在干扰较大的环境中解算出与加速度真值较为贴近的加速度，说明了所提算法可以获得较准确的加速度。
为了进一步探讨本文提出的算法的性能，将其与Matlab的imufilter模型比较，该算法通过融合卡尔曼滤波器对状态误差进行建模以求解六轴状态。对EuRoC数据集的多个序列分别用2种算法求解加速度，对比不同序列的RMSE(root mean squard error，均方根误差)，最终结果如表1所示。
表1 经不同算法处理后不同序列的RMSE
表1进一步证明了本文提出的加速度求解算法能有效地求解出目标加速度分量，与基于卡尔曼滤波的融合算法的加速度校正效果近似，均能恢复出较准确的加速度。
在求解目标加速度时，选取的样本为一小段数据。实际情况中加速度计采样频率较高，因此选取较短时间的样本点即可完成目标加速度的计算，避免了融合算法中依赖历史状态并随着时间增加导致的误差积累问题。同时观察整个求解过程可发现大部分是较简单的加法与乘法运算，避免了卡尔曼滤波融合算法中如更新后验错误等步骤所需的三角函数等运算，可有效提高求解速度。为对比2种算法的运行时间，将EuRoC数据集的每个序列分别运行100次统计运行时长，结果如表2所示。
表2 EuRoC数据集中不同序列运行时间结果 s
表2说明了数据集中不同序列利用不同算法计算加速度的运行时间均值与方差。由表2可知运行速度较基于卡尔曼滤波的融合算法提高了10倍，证明了本文提出的求解目标加速度的算法可快速求解出较准确的目标加速度。
本文提出了一种快速校正加速度的算法，利用融合角速度的方式得到较准确的加速度差分信息，并根据加速度差分的定义推导出以目标加速度为未知量的平面方程组。然后，根据部分数据解算该方程组，便可实现加速度的校正。算法的优点在于可以快速计算出IMU的加速度信息，且不需要依赖历史状态，仅需小段数据便能计算出较准确的加速度。同时，不同于基于卡尔曼滤波融合算法，极大避免了积累误差的问题。
经过实验测试，所提出的算法可以在保持与基于卡尔曼滤波的融合算法近似的校正性能基础上，运行速度提高10倍。在后续工作中，计划深入研究提升校正精度，以期将其运用于加速度计受到干扰而无法作为辅助信息与角速度进行融合计算姿态的场景中。
猜你喜欢加速度计方程组角速度深入学习“二元一次方程组”中学生数理化·七年级数学人教版(2022年5期)2022-06-05基于加速度计的起重机制动下滑量测量中国特种设备安全(2021年5期)2021-11-06《二元一次方程组》巩固练习语数外学习·初中版(2020年5期)2020-09-10一类次临界Bose-Einstein凝聚型方程组的渐近收敛行为和相位分离数学物理学报(2019年3期)2019-07-23圆周运动角速度测量方法赏析中学生数理化·高一版(2017年3期)2017-07-08基于遗传算法的加速度计免转台标定方法中国惯性技术学报(2017年1期)2017-06-09“挖”出来的二元一次方程组中学生数理化·七年级数学人教版(2016年4期)2016-11-19常温下硅微谐振加速度计零偏稳定性的提高光学精密工程(2016年5期)2016-11-07半捷联雷达导引头视线角速度提取火控雷达技术(2016年1期)2016-02-06基于构架点头角速度的轨道垂向长波不平顺在线检测中国铁道科学(2015年4期)2015-06-21 相关热词搜索：加速度，校正，算法，
