1、第 36 卷 第 2 期2023 年 2 月传 感 技 术 学 报CHINESE JOUNAL OF SENSOS AND ACTUATOSVol.36No.2Feb 2023项目来源:航空科学基金项目(ASFC201951048002)收稿日期:20220214修改日期:20220429Adaptive Fusion Algorithm of Visual-Inertial Attitude Measurementin Moving Carrier Environment*WANG Peng1，2*，WANG Chengbo1，Zhang Baoshang1，2，SUN Changku1，2
2、(1State Key Lab of Precision Measuring Technology Instruments，Tianjin University，Tianjin 300072，China;2Key Laboratory of Electro-Optical Control Technology，Luoyang He nan 471000，China)Abstract:To deal with the problem that the time-varying and abrupt interference of inertial device noise make the re
3、sult of Kalman filterinaccurate，a Sage-Husa adaptive strong tracking extended Kalman filter is studied The algorithm adopts Sage-Husa adaptive filter to es-timate the noise covariance of inertial devices in real time and uses multiple fading factors to suppress abrupt interference to enhance therobu
4、stness of the fusion algorithm Experiments show that the proposed algorithm is superior to the Sage-Husa adaptive extended Kalmanfilter and the strong tracking extended Kalman filter and has good robustness when there is system noise variation and abrupt interfer-enceKey words:attitude measurement;v
5、isual-inertial fusion;adaptive strong tracking filter;multiple fading factorsEEACC:7230doi:103969/jissn10041699202302008动载体环境视觉惯性自适应融合物体姿态测量算法*王鹏1，2*，王丞博1，张宝尚1，2，孙长库1，2(1天津大学精密测试技术及仪器国家重点实验室，天津 300072;2光电控制重点实验室，河南 洛阳 471000)摘要:针对动载体环境下视觉惯性组合姿态测量的过程中，惯性器件噪声时变和突变干扰会使卡尔曼滤波器估计不准确的问题，研究了一种 Sage-Husa 自适应
6、强跟踪扩展卡尔曼算法。算法采用 Sage-Husa 自适应滤波来实时估计惯性器件的噪声协方差，引入多重渐消因子来抑制突变干扰，增强融合算法的鲁棒性。实验证明，当存在系统噪声变化和突变干扰时，所提算法优于 Sage-Husa 自适应扩展卡尔曼算法与强跟踪扩展卡尔曼算法，具有良好的鲁棒性。关键词:姿态测量;视觉惯性融合;自适应强跟踪滤波器;多重渐消因子中图分类号:TP2129文献标识码:A文章编号:10041699(2023)02021807目前常见的姿态测量方法为惯性器件和视觉融合测量。惯性器件测量频率高，但由于器件零偏的存在，积分运算会使测量误差随时间增大。视觉测量具有长期稳定性和较高精度，但
7、是受到曝光时间的限制，测量频率较低。惯性器件和视觉融合测量数据融合可以弥补二者的缺点，实现高精度高频率测量15。目前常使用的数据融合算法为卡尔曼滤波算法。卡尔曼滤波算法的性能会受到先验信息的影响。系统噪声信息的不准确会导致卡尔曼滤波算法精度下降，甚至发散。系统噪声的变化分为缓慢变化和错误突变。测量环境会导致噪声特性的缓慢变化。数据通信错误和传感器自身突变干扰则会导致噪声的突然变化，这种变化本身是错误的，需要进行处理。针对器件数据突变，使用渐消因子的强跟踪扩展卡尔曼(STEKF)是一种较常使用的算法。孙长库等6 在动载体视觉惯性组合姿态测量算法中使用了标量渐消因子加强扩展卡尔曼算法的跟踪性，提高
8、了扩展卡尔曼滤波算法的鲁棒性。但是标量渐消因子不能正确调节复杂状态向量中不同状态量的噪声信息。Jiang 等7 在渐消因子的基础上研究了多重渐消因子，可以根据先验信息和协方差矩阵对状态量的每一个维度以不同的速率进行渐消，从而获得更强的鲁棒性。强跟踪卡尔曼算法成功应用于捷联惯导系 统8，GPS/INS 综 合 导 航 系 统9 等第 2 期王鹏，王丞博等:动载体环境视觉惯性自适应融合物体姿态测量算法场景。为了解决系统噪声估计的问题，可以采用自适应滤波的方法来实时更新噪声信息。Gao 等10 使用了 Sage-Husa 自适应滤波器。根据极大后验估计原理，利用观测序列估计卡尔曼滤波算法中系统噪声的
9、期望和协方差。目前，Sage-Husa 自适应滤波已经推广至无迹卡尔曼滤波算法11，容积卡尔曼滤波算法12 和平方根容积卡尔曼滤波算法13 等，并应用于光纤陀螺仪信号处理14，潜水器导航1516 等场景。但是，由于噪声信息的估计具有滞后性，Sage-Husa 自适应滤波对于突变数据的处理要弱于强跟踪滤波器。现有的动载体环境下的姿态测量算法使用的标量渐消因子会错误地调节状态量的协方差，状态量的错误估计会导致滤波器发散。同时，单纯使用渐消因子并不能解决惯性器件噪声变化的问题。对此，本文研究了一种自适应强跟踪扩展卡尔曼算法，并运用于动载体环境视觉惯性组合物体姿态测量系统。本文使用了实际数据进行了模拟
10、和测试，将本文算法和强跟踪扩展卡尔曼滤波算法(STEKF)、使用 Sage-Husa 滤波器的自适应扩展卡尔曼滤波算法(SH-EKF)进行了比较，证明了算法的有效性，算法可以实现动载体环境下目标物体姿态的准确测量。本文的主要贡献如下:推导了动载体环境下视觉惯性组合姿态测量模型;在算法引入了特定的多重渐消因子为不同的滤波器信道提供不同的渐消速率，解决了标量渐消因子导致滤波器发散的问题;将 Sage-Husa 自适应滤波与多重渐消因子结合，使算法能实时估计惯性器件的噪声信息，进一步增强算法鲁棒性。1动载体环境视觉惯性组合姿态测量系统11系统组成动载体环境下视觉惯性组合姿态测量系统的目标是在载体运动
11、的情况下实时解算目标物体相对于载体的姿态信息。本文所使用的测量系统采用两个惯性器件和一个视觉传感器组合测量的方案。本方案将视觉和惯性传感器相结合，系统示意图如图 1所示。视觉传感器能周期性地修正惯性传感器零偏导致的误差。惯性传感器能短期内在目标物体转速过快、特征点无法识别或者错误识别地情况下提供较为可靠的信息。测量系统具体由主惯性传感器、辅助惯性传感器、视觉传感和特征标识点组成，使用二维转台模拟目标物体的旋转运动，使用运动平台模拟载体的运动。图 1测量系统组成示意图辅助惯性传感器用于获取载体在自身坐标系下的角速度运动信息，记为 IMUc。视觉传感器为一台面阵 CCD 相机，用于获取目标物体的视
12、觉信息。辅助惯性传感器和视觉传感器组成载体传感模块固定于载体上，随着载体一起运动。主惯性传感器用于获取目标物体在自身坐标系下的角速度运动信息，记为 IMUt。特征标识点为可被视觉传感器识别的可见光或者红外 LED 发光管。特征标识点和主惯性传感器组成目标物体传感模块，固定于目标物体上，随着目标物体一起运动。本文的坐标系定义如下，主惯性传感器 IMUt 坐标系定义为 OItXItYItZIt，辅助惯性传感器 IMUc 坐标系定义为 OIcXIcYIcZIc，特征点所在的目标物体坐标系定义为 OObjXObjYObjZObj，相机坐标系定义为OCXCYCZC。其中主惯性传感器与辅助惯性传感器的坐标
13、系与各自的测量坐标系一致，相机坐标系的Z 轴与相机的光轴重合。本文中，BA表示坐标系 A到坐标系 B 的旋转矩阵。12状态量定义本算法采用的滤波器状态量为:X=IcObjqTbTgcbTgtObjpfT1ObjpfTnT(1)式中:IcObjq 为辅助惯性传感器相对于目标物体的姿态四元数。bgc和 bgt为辅助惯性传感器和主惯性传感器的角速度输出零偏。Objpfn为目标物体上 n 个特征标识点的三维坐标。误差状态为真实状态 X 和估计状态X 之间的误差，辅助惯性传感器和主惯性传感器的角速度零偏误差为加性误差，即?X=XX。姿态误差四元数IcObjq 定义为IcObjq=IcObjqIcObjq
14、，为四元数乘法。由于误差状态较小，所以姿态误差四元数为:IcObjq12IcObjT1T(2)误差状态量为:?X=IcObj?qT?bTgc?bTgtObj?pTf1Obj?pTfnT(3)912传感技术学报chinatransducersseueducn第 36 卷13惯性器件模型双惯性器件误差状态量微分方程的形式为:?XIMU=FIMU?XIMU+GIMUnIMU(4)式中:nIMU=nTcgnTtgnTbcgnTbtgT为系统噪声。FIMU和 GIMU分别为:FIMU=IcObjObjItItI33IcObjObjIt033033033033033033(5)GIMU=I33IcObjO
15、bjIt033033033033I33033033033033I33(6)表示 的反对称矩阵，表示辅助惯性传感器相对于目标物体的角速度。在考虑惯性器件偏置和噪声的条件下，辅助惯性传感器相对于目标物体的角速度 为:=(mIcbcgncg)IcObjObjIt(mItbtgntg)(7)式中:mIc和 mIt为辅助惯性传感器和主惯性传感器的角速度测量值。ncg和 ntg为辅助惯性传感器和主惯性传感器的角速度噪声，均为高斯噪声。将系统误差状态方程进行离散化:IMU=I+FIMUdt(8)扩展卡尔曼总状态量协方差矩阵更新方程为Pk|k1=FkPk1|k1FTk+GkQcGTk(9)式中:Fk=IMU0
16、0I，Gk=dtIMUGIMU0(10)14相机观测模型本文系统采用单目相机，利用目标物体上的非共面特征点进行视觉位姿解算。在已知特征点在目标物体坐标系下的坐标Objpfj的情况下，使用 POSIT算法解算相机坐标系相对于目标物体坐标系的位置关系ObjpC。相机观测模型采用针孔模型。在进行系统测量之前，首先对相机进行内参数标定，得到内参数矩阵。卡尔曼滤波算法的观测量为当前相机帧中第 j 个特征点 fj的观测量:z(j)=1CZjCXjCYj+n(j)(11)Cpfj=CXjCYjCZj=CIcIcObj(ObjpfjObjpC)(12)式中:IcObj为辅助惯性传感器到目标物体的旋转矩阵，n(j)为系统的观测噪声。根据获得的图像计算每个特征标识点的观测量，计算残差:?z(j)=z(j)z(j)(13)将方程线性化:?z(j)=H(j)X?X+H(j)pObj?pC+n(j)(14)z(j)Cpfj=1CZj0CXjCZ2j01CZjCXjCZ2j(15)H(j)X=z(j)CpfjCpfj CIc026(16)H(j)p=z(j)CpfjCIcIcObj(17)得到观测总方程为:?z=
