惯性定位系统的卡尔曼滤波器设计

1、文章编号:10056734(2000 04000505惯性定位系统的卡尔曼滤波器设计高钟毓(清华大学精密仪器与机械学系, 北京100084摘要:本文讨论陆用惯性定位系统零速修正卡尔曼滤波器的设计问题。关于这个问题, 自从20世纪80年代初期以来, 已有不少文章发表。本文与文献中的各种方法相比较, 不仅考虑了陀螺漂移的常值分量, 而且考虑了斜坡分量, 因此, 更能适应于零速修正时间间隔较长的情况。文中, 首先推导了陆用惯性系统的增广误差状态模型。其次, 利用三个相邻时刻的零速误差构造了观测向量模型。第三, 在前述基础上设计了离散的线性卡尔曼滤波器; 特别地, 对初始状态向量及其协方差矩阵的估计,

2、 提出了具体的实施方法。最后, 给出了在北京郊区实地跑车试验结果的误差曲线。跑车实验所用系统由导航误差3. 7km/h 的航空惯性导航系统改装。实践表明:利用本文的卡尔曼滤波器进行零速修正, 可提高惯性系统定位精度两个数量级以上。关键词:惯性定位; 系统误差模型; 卡尔曼滤波器设计中图分类号:U 666. 12文献标识码:AKalman Filter Design of Inertial Positioning SystemGAO Zhong yu(T singhua University, Beijing 100084, ChinaAbstract :Design of zero veloc

3、ity update Kalman filter for inertial positioning system applied in land vehicle is discussed . There are many references about this issue which have been published since eighty decade early of twenty century . Comparing with the different methods mentioned in the refernces , the new Kalman filter p

4、resented in this paper considers not only constant element but also ramp element of gyro random rate, so it is more suitable to be used in the systems with longer interval of zero velocity update. Firstly, the extend error state model of the land inertial system is derived. Secondly, the observation

5、 vector is constituted with zero velocity errors of three adjacent points . Thirdly , discrete linear Kalman filter is designed based on the state equations and observation vector mentioned above . Especially , specific accomplishment of estimate means of initial states and its covariance matrix is

6、presented . Finally , the resultant curves of vehicle tests in Beijing suburb are given. The applied system in vehicle tests is a refit aviation inertial navigation system with position accuracy of 3. 7km/h. Test results show that the position accuracy of inertial system can be improved by more than

7、 2orders if using the Kalman filter presented in this paper to put into effect of zero velocity update . Key wor ds :inertial positioning ; system error model ; Kalman filter design 前言自从20世纪80年代初以来, 许多国家应用航空惯性导航系统附加零速修正算法, 形成惯性定位系统, 使系统精度提高了23个数量级, 以用于陆地导航或大地测量。收稿日期:20000917作者简介:高钟毓(1936, 男, 清华大学精密仪

8、器系教授, 长期从事惯性技术的教学和科研工作。中国惯性技术学报2000年12月第8卷第4期零速修正存在多种算法, 如曲线拟合、卡尔曼滤波、波法14, 等等。在这些零速修正算法中, 把作为系统主要误差源的陀螺漂移速率视为随机常数, 或者一阶马尔可夫过程。这对于零速修正间隔相对陀螺漂移相关时间较短时, 是比较合适的。当零速修正间隔比较长(例如, 10min 时, 为了获得更高的定位精度, 陀螺漂移误差模型需要进一步精化。本文采用随机斜坡函数漂移误差模型, 讨论了由此而产生的相关问题的处理方法, 其中, 包括系统误差模型建立、观测向量选取、初值估计等。通过实际跑车试验, 结果表明:本文设计的零速修正

9、卡尔曼滤波器, 可以达到进一步改进和提高惯性定位系统精度的目的。1系统误差状态模型众所周知, 惯性导航系统的加速度误差可以表示为:v õ=W ×A +$G +¨(1. 1式中, W 为平台角度误差; A 为加速度; $G 为重力加速度误差; ¨为加速度计误差。或者, 用矩阵方程表示为:v õL 1v õL 2v õU=0-W U W L 2W U 0-W L1-W L 2W L 1A L 1A L 2A U+N g G g $g+¨L 1¨L 2¨U(1. 2在载体低速运动的条件下, A L 1

10、=A L 20, A U =-g 。因此, 上式可以简化为:v a L 1v a L 2va U =-W L 2g +N g +¨L 1W L 1g +G g +¨L 2$g +¨U(1. 3同时, 平台角度误差可以表示为:W a =-X ×W +D X +E(1. 4式中, X 为平台转动角速度; D X 为平台转动角速度误差; E 为陀螺漂移误差。在低速载体上,X =X L1X L 20, DX =-v L 2/R e v L 1/R e0, E =E L 1E L2E U 式中, X L 1, X L 2为地球表观运动角速度在平台水平轴上的投影。这

11、样, 平台角度误差方程(1. 4 可改写为下列矩阵方程:7õL 17õL 27õU=00-X L 200X L1X L 2-X L17L 17L 27U+-v L 2/R ev L 1/R e 0+E L1E L 2E U(1. 5若忽略导航误差方程中的付科振荡分量, 即, X L 17L2-X L 27L 1=0, 并且令方位陀螺的漂移误差为常量, 即E U =const . , 那么, 7U =7U (0 +E U t (1. 6于是, 平台角度误差方程(1. 5 可以简化为6高钟毓:惯性定位系统的卡尔曼滤波器设计7õL17õL=-v L

12、2R e +E L 1-X L 27U (0 -X L 2E U t v L 1R e +E L 2+X L 17U (0 +X L 1E U t (1. 7组合方程(1. 3 和方程(1. 7 , 可得下列两组结构相同的误差方程:v õL 17õL =0-1/R e0v L 17L+N g +¨L 1E L 2+X L 17(0 +X L 1E U t v õL 27õL =-1/R e v L 27L+G g +¨L 2E L 1-X L 27(0 -X L 2E U t 因此, 我们只要求解其中一组方程, 所得结果同样适用于另一

13、组方程。为了简单起见, 略去所有变量的上下标, 统一由下列是微分方程表达:v õ(t =-g 7(t +¨(t (1. 8 7õ(t =R v (t -E (t (1. 9假设在短时间间隔内, 加速度计的零偏¨(t =const. , 陀螺漂移速率为斜坡函数: E õ(t =L(1. 10 式中, L 为随机常值。将方程(1. 9 代入方程(1. 8 , 可得:v b (t =-X 2v (t +g E (t (1. 11式中, X =g /R 为舒拉角频率。引入状态变量:x 1x 2x 3x T=v v a g E g T(1. 12利用方程

14、(1. 11 和方程(1. 10 , 可得下列状态方程:x a 1x a 2x a 3x a =010-X 20100001000x 1x 2x3x +w 1(t w 2(t w 3(t w 4(t (1. 13这个增广误差状态方程的离散形式可以表示为:x 1(t k x 2(t k x 3(t kx 4(t k =cos X $t X X X -X sin X $tcos X $t X X 2001$t 01x 1(t k -1x 2(t k -1 x 3(tk -1x 4(t k -1+w d 1(t k w d 2(t k w d 3(t k w d 4(t k (1. 14式中, $t

15、 =t k -t k -1; w dj , j =1, , 4, 为模型噪声。或者, 将方程(1. 14 简记为:X (t k =5(t k , t k -1 X (t k -1 +W d (t k (1. 15由于状态x 1(t 为速度v (t 的误差, 所以, 积分tkt k -1v (S d S 应等于时间区间t k -1, t 的距离误差:7高钟毓:惯性定位系统的卡尔曼滤波器设计s (t k =sin X $t X x 1(t k -1 +1-cos X $tXx 2(t k -1 +X $t -sin X $t X x 3(t k -1 +22-1+cos X $tXx 4(t k

16、-1 (1. 16至此, 我们可以看出, 在小时间区间, 状态x 1, x 2, x 3, x 4分别代表了速度误差、加速度误差、加速度的一次导数误差以及加速度的二次导数误差。如果我们能够借助某种辅助导航手段(如零速修正, 计程仪等 获得惯性系统的速度误差时间序列, 那么, 通过曲线拟合就可以得到各项误差系数, 并且, 在初始位置精确给定的条件下, 通过速度误差曲线的时间积分可获得位置误差。然而, 由于速度误差时间序列的测量值总是存在随机测量误差的, 只做曲线拟合, 其精度是不充分的。所以, 需要进一步作最优滤波卡尔曼滤波处理, 以获得更加理想的最优估计结果。2观测向量模型假设在连续三个时刻t

17、 k -1, t k 及t k +1获得测量值:v a k -1, v k -1, v k 及v k +1。它们与状态变量的关系可以表示如下:v k -1v a k -1v kv k +=I 2×202×2A 2×2B 2×x 1(t k -1 x 2(t k -1 x 3(t k -1x 4(t k -1+e v (t k -1 e v a (t k -1 w d 1(t k +e v (t k <11(t k +1, t k w d 1(t k +w d 1(t k +1 +e v (t k +1(2. 1式中, A 2×2=<

18、11(t k , t k -1 <12(t k , t k -1<11(t k +1, t k -1 <12(t k +1, t k -1 , B 2×2=<13(t k , t k -1 <14(t k , t k -1<13(t k +1, t k -1 <14(t k +1, t k -1, e v a , e v 分别为加速度和速度测量噪声。令观测向量为:z 1(t k z 2(t k z 3(t k z 4( t k =5(t k , t k -1I 2×202×2-B -1A B -1v (t k -1v a

19、(t k -1 v (t k v (t k +1=x 1(t k x 2(t k x 3(t k x 4(t k +V d (t k (2. 2其中, 观测噪声向量为:V d (t k =-5(t k , t k -1I 2×202×2-B -1A B-1e v (t k -1 e v a (t k -1 w 1(t k +e v (t k <11(t k +1, t k w 1(t k +w 1(t k +1 +e v (t k +1-W (t k (2. 3 方程(2. 2 可简记为Z (t k =X (t k +V d (t k (2. 4 3卡尔曼滤波器设计系

20、统状态模型和观测向量模型分别采用方程(1. 16 和方程(2. 4 。假设模型噪声和观测噪声都为零均值高斯白噪声, 协方差矩阵分别为W d (t i W T d (t j =Q d (t i D ij(3. 1 和V d (t i V Td (t j =R d (t i D ij(3. 2式中, 克劳耐刻尔D 函数定义为:D ij =1, i =j 0, i j。在上述假设条件下, 可以设计卡尔曼滤波器算法如下:¹测量修正方程:8高钟毓:惯性定位系统的卡尔曼滤波器设计X (t +k =X (t -k +K (t i Z k -X (t -k (3. 3 其中,K (t k =P (t

21、 -k P (t -k +R d (t k -1(3. 4 P (t +k =I -K (t k P (t -k (3. 5 º时间传播方程:X (t -k +1 =5(t k +1, t k X (t +k (3. 6 P (t -k +1 =5(t k +1, t k P (t +k 5T(t k +1, t k +Q d (t k (3. 7»初始状态估计:在系统初始对准结束后并转入导航后, 保持载体不动, 观测3min 以上的导航误差数据。首先, 取初始1min 的速度数据, 通过最小二乘拟合, 算出初始速度误差x 1(t 0 和初始加速度误差x 2(t 0 ; 连

22、同终点时刻的位置误差s (t 1 和速度误差x 1(t 1 作为已知量。其次, 构造下列矩阵方程:I 2×202×2C 2×2D 2×2X (t 0 =v (t 0 v a (t 0 v (t 1s (t 1 +n 1n 2n 3n (3. 8式中, C 2×2=<11(t 1, t 0<12(t 1, t 0t1t<11(S , t 0 d St1t<12(S , t 0 d , D 2×2=<13(t 1, t 0<14(t 1, t 0t1t<13(S , t 0 d St1t<1

23、4(S , t 0 d ,于是, t 1时刻的状态预报估计可表示为:X (t -1 =5(t 1, t 0I 2×202×2-D -1C D-1v (t 0 v a (t 0 v (t 1 s (t 1 (3. 9估计误差协方差矩阵为:P (t -1=5(t 1, t 0I-D -1C D-1P nI-D -1C D-1T5T (t 1, t 0 (3. 10式中, P n 为观测误差向量n 1n 2n 3n 4T的协方差矩阵。至此, 惯性定位系统的卡尔曼滤波器全部设计完毕。然后, 根据实际系统和运行条件, 通过调整, 决定噪声协方差阵。图1原航空惯导系统动态定位误差图2惯

24、性定位系统动态定位误差4试验结果利用导航误差为3. 7km/h 航空惯性导航系统, 经过改装后, 在北京郊外半山区作了实际跑车试(下转第19页9高钟毓:惯性定位系统的卡尔曼滤波器设计熊崴等: INS/ ESGM/ Doppler 组合导航系统 中的 Kalman 滤波方法 19 º 可以提高对 ESGM 极陀螺误差的估计精度, 实现对 ESGM 进行校准, 进一步遏制 ESGM 误差 的发散程度; 统的精度, 进一步延长水下组合导航系统的重调周期。 参考文献: ( 总 2 : 5356. 2 庄 良杰, 翁海娜. 静电陀螺监控器系统各工作过程设计报告 R , 1998. 3 翁 海娜

25、, 李滋刚, 万德钧. INS/ ESGM 组合导航数据处理技术研究 J . 中国惯性技术学报. 1999, 7( 1 : 2327. 4 惯性导航系统 编著小组. 惯性导航系统 M . 北京: 国防工业出版社 , 1983. 5 俞 济祥. 卡尔曼滤波及其在惯性导航中的应用 M . 西安: 航 空专业教材编审室, 1984. 6 Bierman G J . Factor ization Methods for discrete sequential estimation M . New York : Academic P ress , 1977. 7 Anfinogenov A S. Ele

26、ctr ostat ic Gyr o A . The Second Soviet Chinese Symposium of Inertial Technology C . 1994. » 在系统有定时校准的情况下, 可以提高 ESGM 的精度, 从而进一步提高 INS 及整个组合导航系 1 刘 玉峰. 攻克高精度仍是 我国舰船惯性技术 发展的当务之急 A . 惯性技术发展 动态发展方向研讨 会文集 C . 1994 ( 上接第 9 页 验。每次跑车时间为 2 h, 车速为 3540 km/ h, 零速修正时间间隔为 10 min。图 1 是不作零速修正, 原 航空惯性导航系统的经纬度误差曲线, 最大定位误差大