一种基于改进Kalman滤波的视觉/惯性组合导航算法

屈桢深,楚翔宇,赵霄洋,李葆华

(1.哈尔滨工业大学空间控制与惯性技术研究中心,哈尔滨 150001； 2. 北京空间飞行器总体设计部, 北京 100094)

一种基于改进Kalman滤波的视觉/惯性组合导航算法

屈桢深1,楚翔宇1,赵霄洋2,李葆华1

(1.哈尔滨工业大学空间控制与惯性技术研究中心,哈尔滨 150001； 2. 北京空间飞行器总体设计部, 北京 100094)

提出了一种基于改进Kalman滤波的视觉/惯性直接组合导航算法,首先分析了相关坐标系的定义和敏感器的误差模型,然后分别构建了姿态滤波方程和位置滤波方程,接着对滤波方程进行离散化处理以便在计算机中实现运算,最后采用该方法进行了仿真验证,结果表明系统在不同情况下进行组合导航都能够满足任务要求。

改进Kalman滤波；惯性导航；视觉导航

0 引言

视觉/惯性组合导航技术在进入21世纪后逐渐成为研究热点,在车载导航、无人机导航等方面获得了不少成果[1-3]。2007年,为了提高车载雷达的导航精度,陈林等提出了一种基于地标定位的INS/Vision组合导航方法,利用相机对地标识别和动态定位来确定惯性导航系统(INS)的位置,最后采用Kalman滤波将视觉数据和惯导数据进行融合,来对惯导系统误差进行修正[4]。2009年,针对无人机编队leader-follower(领航-跟随)模式,崔乃刚等提出了一种解决视觉解算系统输出存在滞后的INS/Vision相对导航法,采用无迹Kalman滤波估计leader与follower之间的相对位置、速度和姿态,结合INS姿态角信息来检测跑道特征点,求解飞机相对位置矢量[5]。2010年,为了减小无人机INS漂移误差累积的影响,李耀军等提出了基于INS/SMNS紧耦合的无人机导航模式,无人机的粗定位靠IMU的位姿信息完成,航拍图像可以实时校正IMU的积累误差,提高了景像匹配导航系统的实时性和适配区景象匹配的精确度[6]。2011年,为了解决相机低精度造成的远距离导航误差,冯国虎等提出了一种基于单目视觉/惯性组合导航定位算法,观测量采用的是INS解算的速度和视觉计算的速度之差,利用Kalman滤波修正导航信息[7]。

近几年来,国外的学者利用Kalman滤波的各种衍生技术,在对应用视觉/惯性器件进行导航的系统进行滤波研究时,获得了较多的研究成果。针对视觉/惯性组合导航方式的不同采样率问题,国内外许多学者提出了不同的信息融合算法。

Strelow在进行滤波研究时提出,可以使用改进的EKF算法对视觉/惯性组合导航系统进行滤波,在使用算法对状态进行估计时,判断量测内容是否更新了,如果这一时刻没有量测内容更新,那么只利用上一时刻的量测值进行时间上的更新；如果这一时刻量测内容有更新,那么既进行时间更新又进行量测更新[8]。Wu和Smyth提出在进行惯性导航滤波的过程中,位置和加速度信号应该采取不同的数据更新频率,通常在进行位置信息更新时采用低频采样防止高频噪声,在进行加速度信息更新时采用高频采样以达到高精度测量,在进行滤波更新时经常采用较高的采样频率作为Kalman滤波的滤波频率[9]。Rehbinder和Ghosh在解决快速移动中的视觉导航问题时,利用高频陀螺测量信息和延迟的低频视觉量测信息估计了相机在惯性坐标系中的位置和姿态[10]。

1 导航原理

组合导航一般分为直接法和间接法,根据系统设计的特点,本文采用直接法。直接组合导航算法是直接将惯性和相机信息进行融合,在滤波中Kalman滤波器接收各敏感器的输出量,经过滤波计算可以得到导航参数的最优估计。滤波过程主要由2个子滤波器构成,将加速度计的加速度信息和相机的相对位置信息作为位置滤波器的输入,陀螺仪的角速度信息和相机的相对姿态信息作为姿态滤波器的输入,通过Kalman滤波器进行最优滤波估计,估计出最优的相对位置、相对姿态参数,基本原理图如图1所示。

图1 直接组合导航法的基本原理图Fig.1 The basic principle of direct combined navigation method

2 数学模型

2.1 陀螺误差模型

陀螺能输出追踪航天器的本体坐标系相对于惯性坐标系的姿态角速度在追踪航天器本体坐标系中的分量,其优点是可靠性高,依靠自主导航,短时间内精度较好,能够提供较高频率的姿态信息。根据大量的统计规律,陀螺仪的误差主要由以下三部分构成：

ε=εc+ωg+εr

(1)

式中,εc为陀螺仪随机常值漂移；ωg为陀螺仪的白噪声漂移,均方差为σωg；εr为陀螺仪的随机游走,漂移斜率白噪声的均方差为σεr。

2.2 加速度误差模型

加速度计能输出加速度在追踪航天器本体坐标系中的比力投影,其优点与陀螺仪相同,两者一起组成的惯组单元是航天器导航的首选配置。在大量数据的统计下,加速度计的误差由以下三部分构成

(2)

2.3 相机误差模型

基于相机的视觉导航系统误差主要由测量系统的标定误差、光学镜头的几何畸变误差、参数求解的舍入误差等组成,在进行数学仿真时,把各种随机误差都以白噪声的形式表示,这种等价对位置测量和姿态测量都适合。误差形式如下

ζ=ωc

(3)

式中,ωc为相机的白噪声误差,均方差为σωc。

2.4 姿态滤波方程

2.4.1 状态方程的建立

对于航天器运动来说,其姿态方程的描述不仅可以使用欧拉角方法,还可以使用方向余弦阵方法、罗德里格斯参数方法、等效旋转矢量法和四元数方法来进行描述,虽然使用欧拉角所描述的姿态方程其物理意义较为明确,但存在奇异性问题,并且三角函数运算比较麻烦。使用四元数表示坐标变换相对简单,比用9个方向余弦表示简洁,更适合对航天器姿态进行实时控制,因此姿态运动学方程和陀螺仪误差方程可得：

(4)

(5)

(6)

则式(4)中的状态变量X(t)可以表示为

(7)

系统噪声矩阵为

(8)

整理式(7)和式(8)可得,状态方程是非线性的,表示为

2.4.2 量测方程的建立

姿态滤波器输入的量测值是相机信息经过处理后得到的相对姿态角θ1、θ2、θ3(3-2-1转序),即

(9)

又由四元数和欧拉角之间的关系,得到量测方程

2.5 位置滤波方程

2.5.1 状态方程的建立

假设目标航天器运行在圆形轨道,追踪航天器相对目标航天器的相对运动学方程在轨道坐标系中的表示为

(10)

(11)

又有

(12)

(13)

式中,vd为加速度计的测量噪声；d为加速度计的漂移偏置,且满足

(14)

式中,va为加速度计偏置斜率白噪声。

由式(13)和式(14)整理可得,选取状态变量为

系统噪声矩阵W(t)为

控制向量U(t)为

那么,位置滤波器的状态方程为

(15)

式中,状态转移矩阵F(t)为

控制向量矩阵B(t)为

系统噪声驱动矩阵G(t)为

2.5.2 量测方程的建立

相机进来的数据经过处理后,得到目标航天器相对追踪航天器的相对位置在追踪航天器中的表示,即

状态向量中的相对位置(xH,yH,zH)表示的是追踪航天器相对目标航天器在轨道坐标系中的分量,则有

故量测矩阵H(t)为

2.6 滤波方程的离散化

将状态方程离散化为

Xk+1=Φk+1,kXk+Γk+1,kWk

(16)

或

Xk+1=Φk+1,kXk+Ψk+1.kUk+Γk+1,kWk

(17)

式中,Φk+1,k是状态一步转移阵。在本课题中,假设等时间间隔采样,采样间隔Ts=tk+1-tk(k=0,1,2…)。一般Kalman滤波周期Ts较短,可以近似看成Ts→0,则F(t),B(t),G(t)可以看作常数阵,即有

(18)

Qk是滤波器噪声序列的方差强度矩阵,一般为非负定阵,即

或

将量测方程离散化为

Zk+1=Hk+1Xk+1+Vk+1

(19)

由于量测方程的离散化形式中的每个量都是在k+1时刻的值,故Hk与H(t)的形式一样。Zk+1是系统的观测值,由外界输入到滤波器中。Vk+1是滤波器的量测噪声序列,一般满足高斯白噪声,即

或

3 改进Kalman滤波算法的推导

改进Kalman滤波算法基于Kalman滤波基本公式的基础上,将5个基本公式分为2个部分。

1)时间更新过程

状态的一步预测

(20)

一步预测误差的方差阵

(21)

2)量测更新过程

滤波的增益矩阵

(22)

状态的估计

(23)

(24)

时间更新过程等价本课题中的惯性导航,量测更新过程等价视觉导航。当视觉信息可靠时,才启用视觉导航,即进行时间更新和量测更新；当视觉信息不可靠时,隔离视觉导航,只进行时间更新。视觉信息的可靠性由量测噪声方差阵Rk衡量。

在滤波时刻k时,若出现相机信息没有更新或出现中断,即不可靠,则量测噪声方差阵Rk被认为趋向无穷大。由式(24)可知,滤波的增益矩阵Kk会趋向于零,那么会有

(25)

Pk=Pk,k-1

(26)

从式(25)和式(26)可以看出,状态的估计和估计误差方差阵与k-1时刻相比没有变化,间接证明没有进行量测更新。

改进Kalman滤波算法是在之前设计的滤波器基础上进行改进的,主要增加了对视觉信息的可靠性判断。若可靠性低,选择很大的量测噪声方差阵；若可靠性高,选择很小的量测噪声方差阵。由于惯性测量单元在很短时间内的精度是很高的,故在视觉信息可靠性低的情况下可以不进行量测校正,只采用惯性测量单元进行导航。

4 仿真验证与分析

仿真参数的选取

1)相机

采样时间：160ms；

相对姿态测量噪声方差：1.95×10-6rad2；

相对位置测量噪声方差：1.0×10-4m2。

2)陀螺仪

采样时间：20ms；随机常值漂移：0.05(°)/h；

漂移斜率的白噪声：均值为0,方差为0.01；

测量白噪声：均值为0,方差为0.1。

3)加速度计

采样时间：20ms；随机常值偏置：0.005m/s2；

漂移斜率的白噪声：均值为0,方差为1.0×10-6；

测量白噪声：均值为0,方差为1.0×10-4。

4)位置滤波器

滤波周期：20ms；估值误差协方差阵的初始值为P0=10-3·I9×9。

状态向量的初值为X0=[-21.5091000000]T。

激励噪声的协方差矩阵为Q=diag[10-110-110-110-310-310-3]。

量测噪声的协方差矩阵为R=diag[10-110-110-1]。

5)姿态滤波器

滤波周期：20ms；估值误差协方差阵的初始值为P0=10-2·I7×7。

状态向量的初值为X0=[0001000]T。

激励噪声的协方差矩阵为Q=diag[10-210-210-210-210-210-2]。

量测噪声的协方差矩阵为R=diag[10-510-510-5]。

为了验证改进Kalman滤波算法对提高滤波性能的作用,需对Simulink仿真模型进行修改。滤波器中所使用的量测信息都来自于视觉信息,故需对相机信息进行处理。相机信息的输出周期为160ms,将其输入采样周期为20ms延时模块,得到的相机信息整体滞后了20ms,再将这2个信号相减得到差值信号。以相对位置的x轴方向为例,处理过程如图2所示。

图2 相对位置x轴方向信息的处理过程Fig.2 Processing of relative position x axis direction information

从图 2可以看出,在一个相机信号周期内,差值信号的前20ms是有值的,后140ms保持为0。当差值信号有值时,认为是某一滤波周期里的视觉信息较上一个滤波周期进行了更新,这种更新能真实反映两航天器之间的相对位姿变化；当差值信号为0时,表明视觉信息在剩下的140ms一直得不到更新,而这个时间段内真实的相对位姿是变化的。根据这个特点可以在前20ms时使用相机信息进行量测更新,后140ms内隔离相机信息。

另外,每个系统仿真周期里还需对相机信息进行幅值检测。例如在交会对接中对两航天器的相对位置检测时,判断相对位置的3个分量是否同时处于小范围,这个小范围可以根据具体情况设定,若同时处于小范围,表明交会过程中出现了异常导致视觉系统产生的相对位置信息出现中断,这时就需要及时隔离相机信息。

在保证各参数不变的情况下,给出了常规Kalman滤波器(KF)和改进Kalman滤波器(IKF)的估计误差对比,如表1和表2所示。

表1 各估计误差的最大值(10～100s)

表2 各估计误差的均方差RMS(10～100s)

从表1和表2的数据可以看出,改进Kalman滤波器的各个估计误差量相比于常规Kalman滤波器的估计误差有明显下降。虽然姿态角的估计误差下降程度没有位置和速度的估计误差下降程度大,但从整体上看,改进Kalman滤波器对提高滤波效果起到了一定作用。

接下来研究视觉信息中断对滤波效果的影响。在仿真程序中,取50～55s这一时间段的相机信号为0,给出了IKF的各个估计误差情况。为了清晰地表示出视觉信息中断过程,这里给出10～100s的曲线,如图3～图5所示。

从图 3、图 4和图 5中可以看出,在视觉信息中断的时间段内,各估计误差开始积累,但当视觉信息恢复后,估计误差迅速收敛,可见改进Kalman滤波器的稳定性较好。这是因为,在视觉信息中断后,系统判断出视觉信息不可靠,则相应地选择很大的量测噪声方差阵,即等价将视觉信息隔离,不采用视觉信息进行量测更新。由于系统具有冗余性,在视觉信息中断期间可以单独使用惯性导航,故会出现图中50～55s时间段内的误差积累现象。

将视觉信息中断情景置于常规Kalman滤波器中,会出现估计误差发散的情况,导致整个系统不能正常运行。只要视觉信息中断的时间不太长,基于改进Kalman滤波算法的组合导航算法能保证估计误差收敛,满足系统最基本的稳定性要求。

图3 视觉信息失效时改进Kalman滤波器的位置估计误差Fig.3 The position estimation error with improved Kalman filter while visual information failure

图5 视觉信息失效时改进Kalman滤波器的姿态估计误差Fig.5 The attitude estimation error with improved Kalman filter while visual information failure

5 结论

论文针对近距离交会对接过程中的相对导航问题,采用视觉/惯性组合导航模式,结合了惯性测量单元高精度和视觉测量系统定时更新的优点,可获得更高的导航精度。由于惯性信息和相机信息的频率不同,会出现在一个滤波周期中采用不精确的相机信息去校正的情况,故需要对Kalman滤波算法进行改进。主要思想是只有当相机信息精确时才进行校正,若相机信息的可靠性低,则对相机信息进行隔离,避免污染信息。通过数字仿真结果可以看出,基于改进Kalman滤波的组合导航算法的估计误差明显减小,并对相机信息中断情况有较好的鲁棒性。

[1] Parten R P, Mayer J P.Development of the Gemini

operational rendezvous plan [J]. Journal of Spacecraft and Rockets, 1968,5(9):1023-1026.

[2] Mueller E, Bilimoria K D, Frost C. Dynamic coupling and control response effectson spacecraft handling qualities during docking[J]. Journal of Spacecraft and Rockets, 2009, 46(6)：1288-1297.

[3] 边广龙. 苏联/俄罗斯的空间交会对接技术[J]. 太空探索,2011(10)：34-36.

[4] 陈林. 基于动态视觉定位的惯性导航地标修正方法研究[D]. 长沙：国防科学技术大学,2007.

[5] 崔乃刚,王晓刚,郭继峰. 基于Sigma-pointKalman滤波的INS/VISION相对导航方法研究[J]. 宇航学报,2009,30 (6)：2220-2223.

[6] 李耀军,潘泉,赵春晖, 等.基于INS/ SMNS紧耦合的无人机导航[J].中国惯性技术学报, 2010,18 (3)：302-306.

[7] Feng G H,Wu W Q,Cao J L, et al. Algorithm for monocular visual Odometry/SINS integrated navigation [J].Journal of Chinese Inertial Technology,2011,19 (3)：210-215.

[8] 刘焘,庞秀芝,余静,等. 基于准三维视觉模型的组合导航系统性能研究[J].弹箭与制导学报,2009,29 (6)：51-54.

[9] Strelow D W. Motion estimation from image and inertial measurements[D]. Maryland University,2004.

[10] Smyth A, Wu M. Multi-rate Kalman filtering for the data fusion of displacementand acceleration measurements[C].Proceeding of SPIE, Smart Structures and Materials,2006.

[11] Rehbinder H, Ghosh B K. Multi-rate fusion of visualand inertial data[C].Proceedings of IEEE Conference on Multi-Sensor Fusion Integration IntelligentSystems Baden-Baden,Germany,2001：97-102.

A Visual/Inertial Integrated Navigation Algorithm Based on Improved Kalman Filter

QU Zhen-shen1, CHU Xiang-yu1, ZHAO Xiao-yang2, LI Bao-hua1

(1.Space Control and Inertial Technology Research Center, Harbin Institute of Technology, Harbin 150001, China; 2. Beijing Institute of Spacecraft System Engineering, CAST, Beijing 100094, China)

A visual/inertial integrated navigation algorithm based on improved Kalman filter is presented. Firstly, the definition of the relevant coordinate systems and the error models of the sensors are analyzed. Secondly, the attitude filter equation and the position filter equation are established respectively. Then the filter equations are discretized to achieve the operation in the computer. At last, the simulations are performed. The results show that the system can meet the requirements of the task by using the method in different situations.

Improved Kalman filter; Inertial navigation; Visual navigation

2016-10-02；

2016-11-01

国家自然基金(70971030)

屈桢深(1973-),男,副教授,博导,主要从事视觉导航方面的研究。E-mail：ocicq@126.com

10.19306/j.cnki.2095-8110.2017.02.003

U666.12

A

2095-8110(2017)02-0014-07