1、第24卷第7期 2011年7月 传感技术学报 CHINESE JOURNAL OF SENSORS AND ACTUATORS Vo124 No7 July 2011 An Adaptive Filter Based on Stochastic Approximation for Strapdown Inertial Navigation System Alignment BAI Meng ，LI Minhua (Department of Electrical and Information Engineering，Shandong University of Science and Tec
2、hnology Jinan，Jinan 25003 1，China) Abstract：For the strapdown inertial navigation system(SINS)with unknown measurement noise covariance， applying conventional Kalman filter to initial alignment will lead to a large state estimation error or even filter diver- genceTo estimate SINS errors with unknow
3、n measurement noise covariance，an adaptive filter based on stochastic approximation is presentedIn the filter，the RobbinsMonrd scheme is applied to Kalman filter to solve the problem 0f SINS errors estimation with unknown measurement noise covarianceand the inverse operation iS simplified to improve
4、 the numerical stabilityThe simulation results demonstrate the effectiveness of the adaptive filter in initial a- lignment for SINS Key words：strapdown inertial navigation system；initial alignment；stochastic approximation；adaptive filter； measurement noise EEACC：7220 doi：103969jissn10041699201107015
5、 随机逼近自适应滤波在捷联惯导系统初始对准中的应用米 柏 猛 ，李敏花 (山东科技大学济南校区电气信息系，济南250031) 摘 要：对于测量噪声方差未知的捷联惯导系统(SINS)，采用常规Kalman滤波进行初始对准会造成较大状态估计误差，甚 至使滤波器发散。为了解决系统测量噪声方差未知或不确切知道时SINS的误差估计问题，提出一种基于随机逼近的自适应 滤波方法。该方法将RobbinsMonro算法与Kalman滤波相结合，通过简化求逆运算，解决了系统观测噪声特性未知情况下 SINS的误差估计问题，并提高了算法的数值稳定性。仿真结果表明，该方法能在系统测量噪声方差未知情况下有效实现SINS
6、初始对准。 关键词：捷联惯导系统；初始对准；随机逼近；白适应滤波；测量噪声 中图分类号：V2493 文献标识码：A 文章编号：1004-1699(2011)07-1007-04 初始对准是捷联惯导系统(SINS)的关键技术 之一，对准精度直接影响导航系统的精度J。采 用Kalman滤波技术是实现SINS初始对准的有效 方法 J。传统Kalman滤波在SINS系统噪声方 差和测量噪声方差准确已知的情况下，可以获得 较好的估计效果 。但实际系统的噪声统计特性 往往未知或不确切知道。针对噪声特性未知情况 下SINS的误差估计问题已取得一些成果 。现 有的大多数自适应滤波算法都有其应用特点，其 数值稳
7、定性和适用范围还需进一步提高。在实际 应用中，一般可以通过大量反复试验确定SINS的 项目来源：山东科技大学“春蕾”计划项目(2010AZZ049) 收稿日期：201l一0118 修改日期：201l一0302 系统噪声特性。但对于观测噪声，由于工作环境、 观测仪器精度等因素的影响，使得很难得到准确 的测量噪声方差。为了解决测量噪声方差未知情 况下SINS的误差估计问题，本文提出一种基于 RobbinsMonro算法 的随机逼近白适应卡尔曼滤 波方法。该方法通过将RobbinsMonro算法与 Kalman滤波相结合，可有效解决测量噪声方差未 知情况下线性系统的状态估计问题，适用范围较 广。仿真
8、结果表明，该自适应滤波方法能够在 SINS观量噪声方差未知的情况下，有效估计SINS 失准角，进而实现SINS初始对准。 1008 第24卷 1捷联惯导系统初始对准误差模型 假设东北天坐标系(EastNoahup)为导航坐标 系n，实际建立的导航坐标系为7, ，且 系偏离n系 的失准角为 、 u，则当 和 u为小量 时，SINS姿态和速度误差方程可表示为 o“J： =一6 + n一+n (1) 6 =一 厂一(2 ：+ )xSW+V X (26 ：+6 L， )+V (2) 其中， = ， ， T为姿态误差角(失准角)，跏 =c2a b ，C：为捷联矩阵， = ， ， r为陀螺 测量误差； 为
9、导航坐标系内运载体的运动相对于 惯性坐标系的角速度在导航坐标系内的投影，8(Oinn为 n 误差； = ， ， r为运载体速度， ： 6 ，6 ，8Vv 为速度误差 =c 为无误差的 加速度计测量值， = n ， =V ， ， r 为加速度计误差，ctJ rt=0， cosL， sinL ， 为地 球自转角速度，、A和h分别为运载体的纬度、经度 和高度， =去， ，丽VEtanL ， 和 分 别为沿子午圈和卯酉圈的主曲率半径。 在静基座下，SINS姿态和速度误差方程可简 化为： =一6 五+ ： (3) 8W=一 广一2toi +V (4) 由于初始对准时间较短，故可将陀螺和加速度 计误差简化
10、为零偏和白噪声之和，即误差可用如下 模型表示： 6 =l6+， (5) =0 (6) V =V +w (7) V =0 (8) 其中， ： h， k r为陀螺常值漂移，W ： W ，W ，W T为陀螺测量噪声；V =V V ， V如 为加速度计常值漂移， = ， ， 为 加速度计测量噪声。 由于在小失准角下SINS姿态和速度误差方程 为线性方程，故可由式(3)、(4)、(6)和(8)组成如 下形式的状态方程 ： = 捌 + 】= FX( )+W(t) (9) 其中， r=go ， ， U，艿 ， ， = h， h， k， Fl= Tl= Cl1 Gl2 C21 C22 C3l C32 0 0
11、0 0 C13 0 0 C23 0 0 C33 0 0 0 C“ C12 0 C21 C22 =03 sinL， = cosL，C：=C J 1，2，3为捷联 矩阵。 取水平速度误差6 和 为观测量，则系统 观测方程可表示为： z=嘲= = ()+ ( ) (10) 其中，()为观测噪声。 2 随机逼近自适应滤波 将系统方程(9)和观测方程(1O)离散后的模 型为： 鼍= ,k-1置一 + 一。 一 (11) = +叩 (12) 其中， 和 满足如下条件： rE =0 E ：Q ，7 ：0 EU T=R 8 【E =0 当过程噪声方差Q 和观测噪声方差R 已知 时，可采用Kalman滤波对系统
12、状态进行估计： fXk 1= k,k-1 _ll l = ， + ( 一 ) ：Pkk-1HT(HPkk-1HT+R )I1 (13) lP 一l= ,k-1P 十 lQ 一1 P IK H P 一1 在实际应用中，由于可以通过大量试验得到 SINS过程噪声特性，因此可以认为 基本已知。 本文主要考虑测量噪声方差R 未知时SINS的误差 估计问题。 设 为标量 )为对应的随机变量，则对于给 和 F T L_l l1 r _I ： 为 ， U h另 V分 1 0 0 0 O 2 0 0 0 O 0 0 0 0 0 o g 0 0 第7期 柏猛，李敏花：随机逼近自适应滤波在捷联惯导系统初始对准中的
13、应用 1009 定的 ，设方程h( )：E Ef( )： (14) 3 仿真结果及其分析 ( )= )= ( ) I一 J 一H、，7 ，J 。 。 有唯一解，则根据RobbinsMonro算法 ，利用 ， ，及所对应的随机变量 ( ) ( )，通过迭 代，可得方程(14)的解为： (尼+1)= (k)+ (k) _厂( (k) (15) 其中， (k)为加权因子，且y(k)满足 (k)0， y( )=，y2( )。 对于式(13)，若令 =YkH ，则 S ：E - T=日 Pkk-1日 +R (16) 则 可表示为： K =Pkk-1日 (17) 根据RobbinsMonro算法及式(15
14、)得S 的估 计值 可表示为： S =S + (后)( 一S ) (18) 其中， (k)在本文中取y(k)=1k。用S 替换式 (17)中的S 可得： K P弧一lH s 19 根据式(18)和(19)，只要给出 的初值。即 可在观测噪声方差 未知时求得 ，进而根据式 (13)求出x 。本文称该方法为基本随机逼近自适 应滤波算法。 在基本随机逼近自适应滤波算法中，由于求解 时需要求 的逆，运算量较大且求解时数值稳 定性较差，为提高算法稳定性，令m =(kS )一，则根 据矩阵求逆公式： A+BCD-。=A一-A BDA B+C DA (2O) 可得m 的递推公式为： m = 一m Yk- T
15、m +J m (21) 则 可表示为： =kP 一1日 Tm (22) 将式(21)和式(22)代人式(13)即可得本文所 提出的随机逼近自适应滤波算法： Xk 1= k,k-I _1l Y =YkH X l 一l X = 一1X l+ Kk=kPkk1 m (23) m=m 一m m “-JT J，l P 一l= k,k-IP + 一1Q 一1 P =(j 日 )P l 上述随机逼近自适应滤波方法可在观测噪声方差 R 未知时求得X 的估计值 本文对中等精度捷联质导系统进行仿真。仿真参 数为：纬度L=45。；陀螺仪常值漂移为002h，随机漂 移为001。h；加速度计常值漂移为100 ，随机漂移
16、 为50 Ixg ；失准角初始均取1。，系统观测噪声方差阵为 R=diag(01 ms) ，(O1 ms) 。仿真中，采用随机 逼近自适应滤波对SINS误差进行估计，m 初值取 m0=2 仿真结果如图1、图2和图3所示。 时间。 图1 东向失准角估计误差 时间s 图2北向失准角估计误差 0 200 400 600 8oo 时间s 图3方位失准角估计误差 由仿真结果可以看出，采用本文所提出的随机 逼近自适应滤波算法能够在系统观测噪声特性未知 情况下，较好地对失准角进行估计。其中，该算法能 很快估计出东向和北向失准角并达到很高的估计精 度，东向失准角的估计误差大约为 =21 ，北向 失准角估计误差
17、大约为 =20 。与东向和北向 失准角相比，方位失准角的估计收敛较慢，但仍能以 较快速度收敛且达到较高的估计精度，方位失准角 的估计误差大约为 ，=6 。由此可见，采用本文 提出的随机逼近自适应滤波算法能够较好地对 SINS误差进行估计，实现SINS初始对准。 印 如加m 0 m 一。一 O 0 O 0 0 O O 0 0 8 6 4 2 罐 10l0 传感技术学报 WWWchinatransducerscom 第24卷 4 结论 本文将随机逼近方法与Kalman滤波相结合，提 出的随机逼近自适应滤波算法能够在测量噪声方差 未知情况下对捷联惯导系统误差进行有效估计。仿 真结果验证了该方法在SI
18、NS误差估计方面的有效 性。另外，该方法是一种一般性方法，同样适用于测 量噪声未知的其它线性系统的状态变量估计问题。 本文所提出的自适应滤波方法算法相对简单，便于 实现，具有较高的实用价值。 参考文献： 2 3 4 马建萍GPS辅助捷联惯导系统动基座初始对准新方法J 传感技术学报，2010，23(11)：16561661 万德钧，房建成惯性导航初始对准M南京：东南大学出版 社，1998 All J，Ushaq MA Consistent and Robust Kalman Filter Design for InMotion Alignment of Inertial Navigation S
19、ystemJ Measurement，2009，42(4)：577582 Zhou ZGao Y，Chen JUnscented Kalman Fiher for SINS AlignmentJJournal of Systems Engineering and Electronics， 柏猛(1981一)，男，讲师，博士，主要 研究方向为机器人导航和控制、图像处 理和机器视觉等，baimengO6163con。 2007，18(2)：327333 5 向礼，刘雨，苏宝库基于QR分解的自适应差分滤波在SINS 大方位失准角初始对准中的应用J宇航学报，2010，31(2)： 509-513 6苏
20、宛新，黄春梅，刘培伟，等自适应Kalman滤波在SINS初始 对准中的应用J中国惯性技术学报，2010，18(1)：4447 7 柏猛，赵晓光，侯增广一种自适应滤波方法在捷联惯导系统 大失准角初始对准中的应用J传感技术学报，2008，21(6)： 10661069 8 裴福俊，居鹤华，崔平远基于自适应组合滤波的惯性导航系 统初始对准方法J北京工业大学学报，2009，35(11)：1454 1459 9聂赞坎，徐宗本随机逼近及自适应算法M北京，科学出版 社，2003 10魏春岭，张洪钺，郝曙光捷联惯导系统大方位失准角下的非 线性对准J航天控制，2003，21(4)：25-35 11Kong X
21、Y，Nebot E M，Durrant-Whyte HDevelopment of a Nonlinear Psi-Angle Model for Large Misalignment Errors and Its Application in INS Alignment and CalibrationCProceedings of IEEE International Conference on Robotics and Automation，1999， 2：14301435 12La S，Xie L，Chen JNew Techniques for Initial Alignment of Strapdown Inertial Navigation SystemJJournal of the Franklin Institute，2009，346(1O)：10211037
