基于改进容积卡尔曼滤波的惯性/光流组合自主测速方法

闫宝龙，赵东花，刘晓杰，吴新冬，闫德利，王晨光，申 冲

(1. 中北大学仪器科学与动态测试教育部重点实验室，太原 030051；2. 中北大学仪器与电子学院，太原 030051；3. 中北大学信息与通信工程学院，太原 030051)

0 引言

随着无人机、无人车等无人运动平台的发展，具有自主导航能力的惯性导航系统(Inertial Navigation System，INS)以及高精度的全球定位系统(Global Positioning System，GPS)等逐渐发展成为研究热点。但是，在复杂环境下，GPS信号易受干扰，INS单独工作误差易随时间积累，而利用视觉传感器进行导航参数测量不存在误差随时间漂移的问题，且具有体积小、成本低、自主性强等优点。因此，视觉/惯性组合导航可以校正INS因积分造成的组合导航误差，逐渐成为研究热点[1]。

N.Gageilk[2]和D.Honegger[3]等利用光流传感器完成了无人机的定点悬停。2015年，M.Bloesch等提出了鲁棒视觉惯性里程计[4],该算法将视觉信息与惯性测量信息进行紧耦合，在保持精度的同时减少了计算量。文献[5]使用双输入光流估计网络(FlowNetCorr)[6]的卷积层部分实现视觉特征提取，再使用惯性信息与图像的融合特征进行姿态解算，能够提升里程计的解算精度，但由于FlowNetCorr层数较多，训练成本较大，不适合包含深度信息的全导航参数估计。文献[7]运用Lucas-Kanade(LK)算法和块匹配算法，从两帧图像中得到图像光流信息，再与INS的输出信息和高度计输出信息进行融合，运用卡尔曼滤波(Kalman Filter，KF)得出无人机在惯性坐标系下的速度。文献[8]提出了一种光流法和改进ORB特征点匹配算法融合的单目视觉里程计方法，利用光流和改进ORB基于阈值循环运行，相较于传统的光流视觉里程计，精度得到了提高。文献[9]基于Elman神经网络预测速度，在环境适宜的情况下，在线训练神经网络模型。同时，使用基于INS动态误差模型的卡尔曼滤波器，通过融合INS及速度数据得到误差向量，从而对导航参数进行补偿修正，可在一定时间内得到较为准确的位置和姿态。

目前，KF已经广泛应用于导航定位及运动控制中[10]。由于KF只能应用于线性系统，而实际中所有的运动系统几乎都是非线性的，因此通过对非线性方程进行线性化来处理各种非线性问题，提出了扩展卡尔曼滤波器(Extended Kalman Filter，EKF)。由于EKF使用一阶泰勒展开近似非线性方程，引入了高阶项截断误差，因此只适用于弱非线性系统[11]。容积卡尔曼滤波器(Cubature Kalman Filter，CKF)基于三阶球面径向容积准则，构造2n个相同权值的容积点，经过非线性函数转换后给出下一时刻系统状态的预测，无需对非线性系统进行线性化就可以适用于非线性运动系统[12]。

由于载体各个系统之间的数据输出频率不同，若直接使用CKF方法，将导致各个系统数据在数据融合时由于频率不一致造成融合精度不高。此外，在纯惯性导航系统或者纯光流测速系统长时间单独运行时，测速会因为累积误差逐渐增大而发散。因此，本文采用一种基于多速率残差校正的CKF方法对惯导以及光流数据进行数据融合。该方法首先使用金字塔LK光流算法得到两个图像之间的运动信息，再运用多速率CKF融合INS数据与光流数据，对系统速度进行估计。具体为当光流数据与INS数据同时存在时，进行残差和状态估计误差的计算；在光流数据的采样间隔，即只有INS数据时，利用上一时刻的状态估计误差对当前时刻的状态估计误差进行迭代计算，并用当前时刻的状态误差估计系统残差，再利用残差对当前时刻的状态估计量进行补偿，以提高融合系统的速度测量精度。

1 改进CKF的惯性/光流测速原理

由于在光流数据的采样间隔无法对系统状态量进行补偿，因此为了提高惯性/光流组合导航系统的测速精度，本文使用改进CKF，即多速率残差CKF对INS数据和光流传感器的数据进行融合。把载体在导航坐标系的速度作为状态量，使用光流传感器的输出作为观测量估计载体速度信息，具体算法过程如下：

1)时间更新。容积点ξi及权值wi如式(1)所示

(1)

其中，I表示n阶单位阵，i=1,2,…，2n，下述公式中i同义。假设k-1时刻的状态初值及其协方差阵为Xk-1和Pk-1，以状态协方差阵的平方根Sk-1代替协方差阵进行时间和观测更新，其中容积点的更新为

(2)

其中，f(·)为已知函数。

(3)

其中，Qk是量测噪声协方差。

2)观测更新。对容积点Xi,k|k-1进行预测更新，如式(4)所示

(4)

经过容积点传播计算得到的观测预测值Zi,k|k-1表达式如式(5)所示

(5)

其中，H是系统量测方程；vk-1是服从高斯分布，期望为0且协方差为R的高斯白噪声。

状态方程与观测方程的互协方差阵及增益矩阵如式(6)所示

(6)

其中，滤波器的设计过程如下:

状态方程为

Xk+1=AXk+Bkμk+ωk

(7)

观测方程为

Yk=g(Xk)+vk

(8)

多速率残差校正的滤波方法不同于低融合度滤波法，低融合度滤波法利用数据的时间戳进行同一时刻的数据判定，并对同一时刻的数据进行融合;而多速率残差校正的滤波方法可以在光流系统解算速度的同时对光流状态误差和残差进行计算，若不存在光流系统解算速度时，可根据上一时刻的状态估计误差对当前时刻状态量进行估计，并用当前时刻误差估计残差。其系统的状态估计误差如式(9)，残差如式(10)所示

(9)

(10)

(11)

(12)

其中，Φk-1是状态转移矩阵。将式(10)展开，并代入式(9)，得

σk=HkΦk-1ek-1

(13)

根据状态估计误差和残差的关系,可得

ek≈Φk-1ek-1

(14)

由于光流输出数据的间隔，滤波增益非常小，因此状态最优估计为

(15)

2 实验验证

为了验证本文提出的基于多速率残差校正CKF的惯性/光流组合测速方法的有效性，采用搭载有光流测速相机装置的无人机进行机载实验验证，见图1。速度基准(BASE)由SPAN-KVH1750分体式闭环光纤惯性/卫星组合导航系统提供，速度测量精度为0.03m/s，飞行轨迹见图2。实验设备如表1所示。无人机在300m高空进行多组实验，试验结果如图3～图6所示。最后分别使用EKF算法、基于多速率残差校正的扩展卡尔曼算法(EKF-ERC)、CKF算法及基于多速率残差校正的容积卡尔曼算法(CKF-ERC)进行比较，试验结果如表2所示。

表1 实验设备参数表

图1 机载实验装置图

图2 飞行轨迹图

图3 机载北向速度测试数据

图4 机载北向速度误差曲线图

图5 机载东向速度测试数据图

图6 机载东向速度误差曲线图

表2 各算法解算误差的方差与均方差

由图3～图6可知，无人机飞行时，本文改进的CKF算法能够较好地跟进基准值，而且机载东向速度均方根误差由EKF的0.4724m/s降低到0.2964m/s，精度提升了37.26%;机载北向速度均方根误差由EKF的0.2266m/s降低到0.06m/s,精度提升了73.45%。由此可以看出，与现有其他光流测速方法相比，本文提出的改进CKF算法的速度测量精度有所提升。

3 结论与展望

本文针对无人高空飞行时，KF算法在惯性/光流组合测速数据融合时出现由于各系统输出数据频率不一致导致融合精度有限的问题，提出了一种基于多速率残差校正的改进CKF算法来提高惯性/光流组合系统的速度测量精度,并使用无人机搭载实验进行了验证，结果表明：

1)使用多速率残差滤波器改进卡尔曼算法，针对光流采样间隔时，利用残差和状态估计误差对系统估计量进行更新与计算，可以在保证系统输出速率的同时，提高测速精度。

2)由于无人机的不规则运动造成速度非线性，EKF算法及其改进算法解算精度较差;CKF算法精度较低，但是基于多速率残差校正的CKF算法仍能够提升测速精度。

在未来惯导/光流组合测速技术的研究中，在光流算法前提条件的限制下，对光流算法中的特征点提取等方面进行改进，并开展光照强弱以及高空飞行时气流对光流测速平台影响等研究，进一步提高无人机飞行时的测速精度。