《一种滤波角速率输入的圆锥误差补偿算法研究》由会员分享，可在线阅读，更多相关《一种滤波角速率输入的圆锥误差补偿算法研究（5页珍藏版）》请在金锄头文库上搜索。
1、一种滤波角速率输入的圆锥误差补偿算法研究 曾庆化 刘建业 赵 伟 赖际舟 （南京航空航天大学导航研究中心 江苏南京 210016） 摘 要 捷联惯性导航系统中，载体的圆锥运动导致系统姿态角误差累积。前人对于角速率输入圆 锥算法研究较少。本文在对比理想角速率与滤波角速率圆锥效应基础上，提出并推导了滤波角速率 输入的捷联惯性导航系统圆锥误差补偿算法。结合实际激光陀螺频谱特性的对比仿真结果表明，滤 波角速率输入圆锥误差补偿算法能大大提高系统姿态精度。 关键词 航姿算法 旋转矢量 误差分析 激光陀螺 激光陀螺捷联惯性导航系统在高速、高动态情况下，机体上多种振动经过耦合会形成圆 锥运动和划船运动。圆锥运
2、动等效于旋转轴上的净转动，会造成激光陀螺捷联惯性导航系统 的姿态角误差，即圆锥误差；类似原理，划船运动也会造成捷联惯性导航系统的划船误差。 由于划船误差与圆锥误差可以等效1，因此圆锥误差补偿算法的深入研究对于提高激光陀螺捷 联惯性导航系统的姿态精度和导航定位精度具有重要意义。 前人对圆锥误差补偿算法进行大量深入的研究25，但这些算法都是依托捷联惯性导航系 统的输出角增量，不适合于角速率输出的激光陀螺捷联惯性导航系统。由于理想圆锥运动物 理量与实际系统滤波物理量有所差别67，因此若直接将理想角增量圆锥误差补偿算法直接应 用于工程系统，无法有效提高激光陀螺系统的姿态精度8。 文中针对角速率输出的激
3、光陀螺捷联惯性导航系统进行研究，提出并推导了一种滤波角 速率输入下的圆锥误差补偿算法。结合激光陀螺IMU频谱特性，角速率输入圆锥误差补偿算法 与公认的四阶龙格库塔算法8对比仿真结果表明：文中角速率输入圆锥误差补偿算法大大优于 四阶龙格库塔法，对提高实际激光陀螺捷联惯性导航系统的精度具有重要的参考价值。 1 滤波角速率圆锥效应分析 由于工程捷联惯性系统数据经过数据采样等过程混入了噪声，从而捷联惯性导航系统输 出的物理量不再是理想的物理量。虽然具有噪声误差的激光陀螺数据经过低通消噪滤波处理 可以消除高频噪声，但是有效信息也同时经过了低通滤波，因此其性质不再与理想物理量性 质相符7。将滤波后角速率物
4、理量带入理想角增量输入圆锥算法公式，无法达到圆锥误差补偿 的高精度效果，因此，推导适应滤波角速率的捷联惯性导航系统圆锥补偿算法非常必要。 假设，圆锥角振动频率为的激光陀螺输出物理量等效滤波处理函数为 ( )F ，则该数字 滤波函数具有低通特性。滤波函数的准则如下： max 0 (0)1;lim()1;()FFFF = = (1) 若定义表达式sin ( ) sin( )/c xxx? ， 结合工程激光陀螺系统采样周期T和上述滤波函数准则， 实际系统的滤波函数可以近似表达为7: ()sin (/2)FcT (2) 在该滤波函数作用下， 滤波前后的物理量相对圆锥效应曲线如图1所示。 图1中1/2f
5、 t =处 为Nyquist频率点。从图1可以看出，滤波前后物理量的圆锥效应不同，因此无法用理想物理量 圆锥误差补偿算法公式有效地提高实际工程激光陀螺系统精度。 基金项目：本文受到“十五”国防预研重点项目资助 2 滤波后物理量分析 考虑到当前计算机的计算能力较强，因此工程 中大多采用单周期圆锥误差补偿算法。在该本文讨 论中，不妨以采样周期为T的单周期M子样圆锥算 法为例讨论。为了研究圆锥误差产生的主要原因， 先考察滤波角增量与旋转矢量关系，为后继滤波角 速率算法研究做准备。 设圆锥运动过程中，等效欧拉轴的运动方程为 0,cos(),sin() T atat，其中为角振动速率，a 为圆锥运动锥半
6、角。则机体系下陀螺角速度8为 2 ( )2sin ( /2)sin( )sinsin( )cos T b nb taatat= (3) 考虑滤波函数 ( )F ，滤波角速率值和角增量分别为： 2 ( )2 (0)sin ( /2)( )sin( )sin( )sin( )cos T b nb tFaFatFat= (4) 2 2 (0)sin ( /2) ( )( )sin( )sin (/2)sin (/2) ( )sin( )sin (/2)cos (/2) Fa hhFachth Fachth = + + (5) 考虑圆锥效应，将滤波后旋转矢量中的圆锥运动锥半角看作角振动速率的函数7，即
7、 0 ( )aa g=，则机体系上的旋转矢量表示为如下形式 2 0 0 0 2sin ( )/2)sin () sin( )sin (/2)sin (/2) sin( )sin (/2)cos (/2) a gch ha gchth a gchth = + + (6) 当和a为小量时，由旋转矢量、角增量和角振动速率之间的变化关系可得7 222 0 sin ()/2)()sin ( /2)a gFa (7) 从而机体系上的旋转矢量为： 22 2( )sin ( /2)sin () ( )sin( )sin (/2)sin (/2) ( )sin( )sin (/2)cos (/2) Fach h
8、Fachth Fachth = + + (8) 由（5）和（8）式知，角增量和旋转矢量值的Y、Z轴分量呈现周期性质变化，不会导致 姿态角度无限制发散。X轴常值分量是导致姿态角发散的直接原因，因此需要专门针对X轴误 差建立圆锥误差补偿算法通式。为此将误差准则定义为： xxx = (9) 其中，是h时间段内角增量的估计值， 为h时间段内旋转矢量的修正量估计值。 3 滤波后角速率求取角增量算法 在姿态更新周期 , t th+ 内，陀螺角速度可表示6为： 2 012 () M M tgggg+=+? (10) 将角速率子样 (/) i tih M=+ 带入(10)式得： 22 012 ()()()(0
9、,1,) MM iM iii gg hg hg hiM MMM =+=? (11) 图1 滤波前后物理量相对圆锥效应曲线 由(11)式的1M +个方程可得关于 01 T M M Ggg hg h= ?， 01 T M W=?，的 矩阵方程为： WCG= (12) 从而有： 1 GC W = (13) 由(11)式得 , t th+ 内积分角度为： 231 012 0 111 ( ) () 231 h M M htdg hg hg hg hHGh M + =+=+= + ? (14) 把(13)式带入(14)式得拟合角增量公式如下： () 1 0011 ( )() MM hHGhHCWhSWhs
10、ssh =+? (15) 其中， 1 01M SHCsss =? 。由角速率(4)式通过(15)式拟合得到的角增量估计值 ( ) h的x轴分量如下： () 22 ( )( )2 (0)sin ( /2)2sin ( /2) b xnb x hthFahha= (16) 考虑到hMT=，由(8)和(16)得X轴需要修正的误差量为： 22 2sin ( /2) 1()sin () cx MTaFcMT= (17) 把(2)式滤波函数和T = 带入(17)，并进行泰勒展开得 22 12 2232323 1 2sin ( /2) 1 sin( /2)sin () ( 1) 2 sin ( /2)(1)
11、2(1) (23)! cx kk kkk k Macc M aMMM k + + = = =+ + (18) 4 滤波后角速率圆锥误差系数补偿公式 设 nM 为第n个姿态更新周期内的第M采样值，则叉乘项 nMnMp + 的X分量如下： 222 ()()()() ( )()( )sinsin() pxnM ynM p znMpynM z CnTTTTTFap T + = (19) 显然，(19)式只与数据之间的间隔p以及数据采样间隔T有关。 将滤波函数式(2)带入(19)式，得 222 ( )() sinsin(/2)sin() px CnTacTp T= (20) 令T = ，则 121 22
12、12121 1 ( 1) ( )sin(1)2(1) (21)! kk kkk px k Cnappp k + + = =+ + (21) 设角速率输入情况下，叉乘项系数表示为 (1,2,) p xpM=? ，则在 , t th+ 内，旋转矢量的 修正量表达式为： 1 M xpxp p C x = = (22) 5 滤波角速率输入圆锥误差补偿算法 根据(18)式和(22)式得： 1 M pxpcx p C x = = (23) 从而有： 121 2212121 11 12 2232323 1 ( 1) sin(1)2(1) (21)! ( 1) 2 sin ( /2)(1)2(1) (23)!
13、 kk M kkk p pk kk kkk k apppx k aMMM k + + = + + = + + =+ + (24) 取1,2,kM=?，并将得到的M个方程联立求解，即可获得修正系数： 1 112 ,T MM Xx xxA B =? (25) 其中， ,1,11 232323 212121 ,1 (),() (1)2(1) (1)2(1), 2(22)(23) 1,2,1,2, M Mk pM MMkM kkk kkk k pk AaBb MMM apppb kk kMpM + + = + =+= + =? 由于采用有限叉乘项进行补偿，因此会存在剩余误差。而剩余误差的误差主项是理想
14、值 与补偿量的第1M +阶之差。由(9)式和(25)式可知剩余误差主项为： 23 2252525 23 2232323 1 ( 1) 2sin ( /2)(1)2(1) (25)! ( 1) sin(1)2(1) (23)! cxx MM MMM MM M MMM p p aMMM M apppx M + + + + = = =+ + + + ? (26) 6 角速率输入圆锥算法精度分析 根据相关文献8的讨论可知：1、在角速率输入情况下，以角增量输入的旋转矢量算法的 误差明显增加，无法体现其优越性；2、二子样角速率误差补偿算法中四阶龙格库塔的算法精 度最优，其误差主项为 2525 () /1440() /45ahaT= 。 通过文中算法和四阶龙格库塔法的误差主项对比可知，本文的算法比四阶龙格库塔算法 优越。为了验证上述结论，并使算法分析具有对比性，本文采用采样时间间隔相同的二子样 和四子样算法与二子样四阶龙格库塔算法进行对比仿真。 文中研究的机械抖动偏频激
