动载体目标物姿态视觉与双惯性组合测量方法综述

2023-03-11孙长库何晶晶付鲁华

电光与控制 2023年2期

孙长库，何晶晶，王鹏，付鲁华

(天津大学精密测试技术及仪器国家重点实验室，天津 300000)

0 引言

随着科学技术的不断进步，自动化、智能化成为工业制造、生产生活、军事作战和航空航天等领域的发展趋势[1]。姿态角为自动化和智能化的实现提供了重要信息和依据，其精确的测量尤为关键。目前的姿态测量技术大多数解决的是在惯性参考坐标系下的绝对姿态测量问题[2]。随着船舶、飞机、车辆等运动载体的飞速发展，对车载、船载、机载仪器设备的导航追踪与智能化控制成为了亟需研究的关键技术，这也使得对姿态测量的要求从静基座延伸至动载体[3]。比如在头盔瞄准线测量系统中，飞行员头部相对于运动飞机的姿态测量要考虑到动载体对测量带来的干扰。视觉传感器与双惯性传感器组合测量是实现非惯性参考坐标系下的相对姿态测量的重要方式之一，对其所涉及的系统结构设计、视觉与惯性传感器外参标定、双惯性传感器差分、数据融合算法与视觉惯性组合方式等关键技术进行深入研究具有重要意义。本文简要总结了各关键技术的研究发展现状，并对组合系统未来的发展趋势进行了展望。

1 系统结构设计

大多数组合姿态测量系统主要解决了相对于惯性参考坐标系的绝对姿态测量问题，比如将飞行导航中的姿态角定义为机体系相对于以地球为参考的坐标系东北天坐标系的转动欧拉角[4]。目前，国内外学者围绕着动基座上的相对姿态组合测量的研究工作较少，主要围绕着双惯性传感器与视觉组合系统开展。美国Thales Visionix公司的FOXLIN提出了相对于运动车辆的头部跟踪模拟系统，系统由2个陀螺仪、2个声波发射器和4个声波接收器组成。2个陀螺仪分别固定于头部和运动载体上，通过双惯性差分技术将动载体的运动剔除。声波发射器和声波接收器分别被安装于头部和运动载体上，提供冗余的姿态解算信息，并通过卡尔曼滤波与惯性传感器输出进行融合[5]。FOXLIN对该系统进行了仿真测试，证明了设计思路的正确性。随后在文献[6]中，FOXLIN又搭建了由外向内结构与由内向外结构混合的视觉/惯性组合测姿系统，采用了多组相机和惯性测量单元(IMU)提高测量冗余度，改善系统精度和鲁棒性，如图1所示。

图1 FOXLIN搭建的实验系统Fig.1 Experimental system built by FOXLIN

系统由被测目标、3套相机/陀螺仪组合传感器、10个LED点和运动平台组成。其中，2套相机/陀螺仪组合传感器安装于运动平台上，1套组合传感器安装于被测目标上。4个LED点安装于运动平台上，作为目标物上相机的视觉标示点；6个LED点固定于被测目标上，作为运动平台上相机的视觉标示点。所以，同一时刻可能存在0，1或2组视觉解算姿态。当视觉姿态存在时，通过卡尔曼滤波器与IMU进行融合。实验验证该系统的姿态解算精度能达到毫弧度级别。文献[7]针对骑行时人体行为分析研究出一套视觉/双惯性组合系统，并进行了可观性和后验Cramer-Rao分析。图2展示了搭建出的系统实物图，用以测量人体相对于自行车的姿态。

文献[3]将视觉传感器与两个IMU组合以实现动基座下的头盔位姿测量，如图3所示。

在双IMU组合系统中，两个惯性器件被分别安装在基座和被测目标上，根据二者的输出差值计算目标与基座之间相对运动的角速度和加速度信息，之后再利用积分方法求取目标与基座之间的相对位姿。与此同时，利用固定在基座上的相机采用视觉的方法求取相机与目标之间的相对位姿，并根据标定好的各个坐标系之间的变换矩阵将位姿测量结果统一到一个坐标系下。最后，采用鲁棒容积卡尔曼滤波对不同传感器获得的位姿进行数据融合。实验表明，该系统的姿态测量精度可达到0.6°以内。郭肖亭提出的姿态测量系统采用了人工特征标识，即在被测物上安装易于识别的视觉标示点，如红外LED点。虽然人工特征标识具有简单、稳定、不易受到干扰等优点[8]，但在一些特殊的应用场景下仍存在难以克服的缺点。比如在飞行员头部相对姿态测量场景中，由于战斗机大多采用玻璃座舱，在驾驶舱中很难找到合适的位置用以安装相机。相机只能被安装于飞行员座椅两侧，这大大地限制了测量范围，影响了视觉测量的精度。此外，只有当特征标识位于视场范围内时，才能实现视觉姿态解算。为了解决视觉传感器安装和测量范围受限问题，何晶晶等针对飞行员头盔瞄准线测量提出了一种非合作目标视觉与双惯性组合姿态测量系统，如图4所示。

图2 骑行姿态组合测量系统Fig.2 Integrated riding attitude measurement system

系统由主IMU、辅助IMU、转台、摇摆台、左相机和右相机、驾驶舱仪表盘模型组成。主IMU和相机固连，并安装于转台上，用以输出目标物运动的角速度。辅助IMU与摇摆台相连，用以输出基座运动的角速度。在进行姿态测量之前，对驾驶舱仪表盘模型进行双目视觉三维重构。采用AKAZE特征提取算法和运动结构恢复(SFM)技术，建立仪表盘的特征点库，从而将非合作目标转化为合作目标。此时，左右相机同时工作以获取特征点的深度信息。在姿态测量时，仅左相机工作，将左相机所采集图像中的特征信息与特征点库进行匹配，获得特征点在世界坐标系下的三维坐标。

图3 视觉与双IMU组合测量系统Fig.3 Vision and dual IMU integration measurement system

图4 非合作目标的视觉与双IMU组合测量系统Fig.4 Vision and dual IMU integration measurement system for non-cooperative target

在系统结构设计中，要充分考虑应用场景的特点、测量指标等。相比于合作目标，非合作目标视觉测量不受视场限制，具有更广阔的应用前景。因此，非合作目标视觉与双惯性组合测量系统将是未来研究发展的重心。

2 视觉与惯性传感器外参标定

相机与惯性传感器外参标定是组合测量的前提。近些年，随着国内外对视觉与惯性组合测量[9-10]关注度的上升，学者们研究出许多种相机与惯性传感器外参标定方法。相关的标定方法将状态估计理论引入外参标定，以传感器原始数据(角速度、加速度和视觉图像)为输入信息，以运动方程(传感器输出数据满足的动力学方程)、观测方程(传感器运动路径在世界坐标系下的表达)为约束条件，以相机与惯性传感器外参为被隐藏状态变量，对包含噪声信息的输入数据，最小化测量误差，解算出可信的外参估计值。按照迭代方法的不同可将外参标定方法分为递归滤波算法[11-12]、非线性优化算法[13-14]和滑动窗口算法[15-16]。这些方法无需借助转台、支架等特定结构辅助，方便简单，易于操作。

1) 滤波标定方法。

相机惯性传感器外参标定，属于非线性非高斯系统状态估计问题。传统的线性估计模型，例如线性卡尔曼滤波(KF)无法迭代出满足精度要求的结果。针对这一问题，文献[11-12]采用扩展卡尔曼滤波器(EKF)解算两种传感器外参。文献[17]借助无迹卡尔曼滤波器(UKF)自校准估计标定参数，配合摄像机标定板进行实验。滤波算法估计速度快，非常适用于在线状态估计。但无论EKF还是UKF解算模型，都建立在马尔可夫假设的基础上，滤波算法并未关注整个轨迹中各状态间可能存在的制约关系，对于同一个估计问题，不同滤波算法收敛于不同结果。

2) 优化标定方法。

文献[18]借助静态优化法进行标定，将IMU测量、视觉约束信息放入优化函数，根据数值优化原理，解算相机与IMU坐标系转换矩阵。文献[19]提出一种离线标定方法，采用批量估计方式，解算相机与IMU外参。文献[20]提出了应用于即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)系统的相机与IMU联合标定方法，采用手眼标定思路计算相机与IMU外参初始值，在此基础上建立超定方程，将剔除离群点得到的最小二乘解作为外参估计值。优化方法在估计参数时考虑整个运动轨迹各状态量间可能存在的约束关系，因此状态估计假设模型更加合理，但由于需要遍历整个轨迹的信息，计算速度较慢。

3) 滑动窗方法。

文献[21]提出了滑动窗迭代算法，通过固定数量时间步长、批量非线性优化的形式，可以兼顾状态估计结果的收敛性以及估计速度，该方法被应用于SLAM系统在线校准。为适应SLAM系统运动环境，视觉传感器通过LK光流跟踪法抓拍周围场景[22]，并采用灰度不变假设，计算相机运动变化。LK光流跟踪计算相机位姿，能够满足SLAM系统测量场景需要，并且速度较快。不过，相比采用已知几何特征的靶标进行视觉位姿检测，LK光流跟踪法难以兼顾速度和精度。

3 双惯性传感器差分研究

在视觉和双惯性组合测量系统中，双惯性传感器组成的惯性测量模块是组合姿态中的一个关键组成部分。两个惯性器件，即主陀螺仪和辅助陀螺仪，分别固定在被测物和运动基座上。根据惯性器件固有的特点可知，辅助陀螺仪测量基座相对于空间惯性参考系的运动，而主陀螺仪测量的是被测物相对空间惯性参考系的运动。基座摇摆运动情况下，从主陀螺仪中分离出基座运动信息，保留单纯的被测物运动量，是正确进行纯惯性计算的关键，也是进一步数据融合计算的基础。FOXLIN推导了双惯性传感器差分的运动学公式，证明了双惯性器件之间的旋转矩阵采用时变而非初始时刻的旋转矩阵是正确进行差分计算的关键。文献[3]对双惯性差分进行了深入的研究，分析了差分计算中各时变矩阵的更新方法。

4 数据融合算法研究

合适的数据融合算法是保证多传感器组合系统性能的关键。卡尔曼滤波器是一种简单有效的递推线性最小方差估计，具有计算精度高、计算复杂度低等优点，是组合系统中常用的数据融合算法[23]。当系统为线性，且系统过程噪声和测量噪声服从均值为零的高斯分布时，卡尔曼滤波是最优无偏估计。然而现实中大多数系统是非线性的，无法采用卡尔曼滤波对系统状态进行估计。为了解决这个问题，学者们提出了许多卡尔曼滤波器的非线性扩展形式，其中，最常用的是EKF[13],UKF[14],容积卡尔曼滤波器(CKF)[24]和误差状态卡尔曼滤波器(ESKF)[15]等。EKF采用泰勒展开对非线性系统函数一阶线性化。然而，这种方式会引入较大线性化误差，导致滤波精度恶化甚至滤波发散。UKF不需要对系统函数线性化，而是利用无损技术从先验分布中采样若干sigma点，用sigma点来近似后验概率密度函数。CKF利用三阶球面-相径容积规则生成的若干容积点对后验分布进行近似。UKF和CKF对后验概率密度函数能达到三阶近似精度，且无需求解系统函数雅可比矩阵，滤波效果均优于EKF。当系统状态向量维度超过3时，CKF展示出更高的融合精度和数值稳定性。包含惯性传感器的组合系统，常使用ESKF来实现数据融合。ESKF的工作原理是将真实状态定义为标称状态和误差状态之和(加法或四元数乘积)。在不考虑噪声和模型误差的情况下利用惯性数据对标称状态进行预测，并通过系统观测值估计标称状态误差，即误差状态，进而修正标称状态。

过程噪声和测量噪声协方差矩阵的选取对于滤波估计效果至关重要。在编程中一般根据经验将噪声协方差矩阵设定为某固定值，即标称噪声协方差矩阵。不准确的标称噪声协方差会导致滤波精度恶化。所以，国内外学者针对噪声协方差矩阵的自适应问题研究了多种自适应滤波算法。根据估计原理，自适应滤波算法可大致分为4类：相关法、协方差匹配法、最大似然法和贝叶斯法[16,25]。相关法利用系统输出或新息的自相关来调整噪声协方差矩阵，主要适用于常系数线性系统[26]，缺点是算法无法保证噪声协方差矩阵的正定性，且计算复杂度较高。协方差匹配法使用新息或残差序列计算新息或残差的协方差矩阵，并将其与理论值进行匹配来估计噪声协方差矩阵，该方法需要通过大量数据来保证噪声协方差矩阵估计的可靠性和准确性，因此，对于快速变化的噪声协方差矩阵，滤波性能会降低[27]。此外，协方差匹配法的收敛性尚未被证实。Sage-Husa自适应卡尔曼滤波(Sage-Husa Adaptive Kalman Filter，SHAKF)是采用协方差匹配法进行自适应噪声方差估计的经典滤波算法。SHAKF不能确保估计所得的噪声协方差矩阵的收敛性和正定性，数值稳定性较低[28]。最大似然(Maximum Li-kelihood，ML)法通过最大化似然函数估计噪声协方差矩阵[29]。在4类自适应算法中，贝叶斯法是最通用和最全面的估计算法。文献[30]提出了基于变分贝叶斯(Variational Bayesian,VB)推断的自适应卡尔曼滤波算法，将系统状态和测量噪声协方差的联合分布近似为高斯分布和逆威沙特分布之积，并采用变分贝叶斯推断在线估计测量噪声协方差矩阵和系统状态。采用VB理论的自适应滤波算法对未知噪声统计量有较强的在线估计能力，吸引了许多学者关注[31-32]。文献[33]针对视觉/双惯性测量系统提出了自适应误差状态卡尔曼滤波，利用变分贝叶斯推断方法对滤波中的未知过程噪声协方差矩阵和测量噪声协方差矩阵实时地估计，提高了姿态估计的精度。

卡尔曼滤波及其非线性扩展形式假设噪声服从高斯分布。然而在许多实际应用中，测量值可能受外部环境干扰产生异常误差，测量噪声不再服从高斯分布。非高斯测量噪声会导致滤波结果误差较大甚至发散[34]。围绕非高斯测量噪声问题，学者们研究出许多鲁棒滤波算法。粒子滤波器(Particle Filter，PF)用蒙特卡罗随机采样算法获得大量随机粒子来逼近非高斯、非线性分布[35]。然而，PF计算复杂度过高，无法满足多数实际系统的实时性要求。文献[36]提出利用具有重尾特性的Student-t分布对测量噪声进行建模，并结合VB理论估计分布参数和状态向量，降低滤波对异常测量的敏感度。算法需要预先设定Student-t分布自由度，不合适的自由度会影响算法估计效果。文献[37]提出采用Huber代价函数对噪声协方差进行调整，降低异常测量在最终估计中所占比重。算法虽然能在一定程度上抑制测量异常值，但既不能完全舍弃误差过大的测量，又对误差较小的测量分配权重过小，无法有效利用测量中有用信息[38]。最近，有学者采用信息论来改善滤波对异常噪声的鲁棒性，比如最小误差熵卡尔曼滤波[39]和最大相关熵卡尔曼滤波[40-41]，这些算法具有更优的滤波估计精度和鲁棒性，并且计算量较小，但算法在测量异常值较大时可能会出现数值不稳定等问题，需要进一步对其进行改进和完善。文献[42]针对视觉与双惯性组合测量系统研究了鲁棒QCKF视觉和双惯性数据融合算法，将H∞鲁棒滤波算法和CKF相结合，构成鲁棒CKF融合策略，进行视觉和双惯性数据融合计算，以克服系统非线性、非高斯特性以及突变型不良测量对滤波的影响，提高测量系统的鲁棒性。为了增强非合作目标视觉与双惯性组合测量系统中的特征误匹配下的数据融合鲁棒性，文献[43]设计了基于最大相关熵的鲁棒卡尔曼滤波，提高了系统融合的稳定性和抗干扰能力。

在视觉与双惯性组合系统中，视觉传感器的采集频率比较低，一般为几赫兹到几十赫兹，而惯性传感器的采集频率比较高，为几十甚至上百赫兹，数据融合存在采集频率不一致的情况，需要考虑组合系统的多频率问题[44]。文献[44]使用多频率卡尔曼滤波融合MEMS惯性数据和视觉数据，进行二维手势的跟踪和识别。文献[45]在进行电动汽车车身倾斜角度的估计与控制中,设计了采样补偿算法，以提高低频采样数据间隔控制准确度，但算法未考虑滤波增益的未知性。ARMESTO等[46]在其自我运动估计惯性视觉组合系统中也考虑了多频率问题。多频率保持和采样分别用来处理滤波过程中时间更新和观测更新。使用插值计算从一系列低频输入采样中产生连续信号，在时间更新过程中参考高频数据时间点进行连续信号的离散化。根据数值计算可知，插值将不可避免地引入误差。文献[47]研究了多频率数据融合残差补偿算法。针对组合测量系统视觉模块和双惯性模块采集频率不一致的问题，分析多频率导致的滤波不稳定性，研究基于残差补偿的多频率数据融合算法。结合状态误差和残差的定义，推导状态误差的自更新方程，实现状态误差的不间断更新，以及在每个高频采样时刻对滤波估计量进行补偿的目的。

5 视觉惯性组合方式

视觉与惯性的组合方式一般可分为松耦合和紧耦合。松耦合将视觉和惯性视作两个独立模块，首先利用图像信息解算视觉位姿，然后将视觉结果和惯性数据进行融合。松耦合算法形式简单，易于实现，稳定性较高。相比于松耦合系统，紧耦合避免了视觉位姿解算，直接将图像特征与惯性数据进行融合，紧耦合算法公式推导更复杂，但充分地利用了不同传感器数据。因此紧耦合系统精度更高，所以针对不同应用场景设计合适的视觉/惯性紧耦合组合系统也是目前国内外研究热点。图5为松耦合和紧耦合的简要示意图。

图5 松/紧耦合示意图Fig.5 Loose coupling and tight coupling

为了解决视场内特征标识数量过少导致的视觉测量失效问题，文献[48]提出了紧耦合视觉/惯性组合飞机空中加油相对位姿测量方案，利用惯性数据预测飞机上特征标识图像坐标，并将其与真实图像坐标的差值输入卡尔曼滤波器，校正惯性位姿估计。实验证明了系统在可见特征标识数目少于3的情况下，仍然可利用视觉信息对惯性数据进行测量更新。文献[49]将紧耦合视觉/惯性组合技术用于车辆导航。为了降低计算复杂度，将多个相机状态而非特征点空间坐标加入滤波状态向量中，通过多状态约束卡尔曼滤波实现对目标位姿和多个相机状态的同时估计。