第4讲：圆锥误差(2-1)

1、第二部分高动态环境下捷联惯导系统姿态算法研究1.引言在常规的捷联惯性导航 系统的姿态算法中，总是认为 载体坐标系和参考坐标系间 的转换是通过一系列的转动 来实现的，而这些转动间的次 序并不重要，这是基于无限小 转动是矢量的原理来得到的。(1) 欧拉角由参考系OXKZ至载体 系OXbYbZb的变换可以通过 依次绕不同坐标轴的三次连3-1-2欧拉角转动K续转动来定义。姿态欧拉角的运动学方程:(cox COS 0 + CDz sin 0)COS 0 cos 0 + (cox sin 6 - coz cos 0)sin 0 coz cos 0 -cdx sin 6方程中存在三角函数，给实时计算带来困难

2、；且当0 = 90°时，方 程中出现奇点，使0厂的解变得不确定，因而使其使用受到限制，不 能用于全姿态飞行器上。(2)方向余弦阵由参考系OXYZ至载体系OXbYbZb的姿态矩阵为C("0,&)=Cy(&)q(0)C&)第1次转动:cos/-sin/sin/cos/0 0 11 0第 2 次转动：C*(0)= 0 COS00 sin0cos。0第3次转动：Cy(e)= o 1sin。0方向余弦矩阵的微分方程为：0sin0COS0-sin 60COS&C = CQ式中°z£1 = coz 0方程的解为:(3)元数四元数是具有四

3、个元素的超复数，它可以描述一个坐标系或一个矢 量相对于某一坐标系的旋转Q0q =实数为四元数的标量部分，03为四元数的矢量部分。s、<71= cos03 =Q2= esmQ3式中，0是表示旋转轴方向的单位向量，0是旋转角。 四个元素满足正交约束方程亦 +qf +泾 + 二 1第一次转动:第二次转动:q"=C°S2 ° °sin?2cos? sin?2 2第三次转动:A八 cosi0.esm 02由上述3-1-2欧拉角转动得到合成四元数 q=qQq" 把上式表示为三次坐标转换矩阵的乘积，有A（q） = A（q，® q"

4、® qw）=禺（q°人（q） （q） 由四元数表征的运动学微分方程为 0 = 1%式中0coz方程的解为我们在对运动学方程求解时，都用了角速度矢量的积分，即A& = J :+人方 codt o而上述积分有意义的前提条件是角速度矢量方向不变。在力学中，刚体的有限转动是不可交换的。这个转动的不可交换性 决定了转动不是矢量，也就是两次以上的转动不能相加。根据欧拉旋转定理，飞行器的转动从任一给定方位到任一其它方位 可通过连续绕瞬时轴转动获得，而瞬时角速度方向在空间不断地改变， 对一个在空间方向随时间变换的角速度矢量进行积分是无意义的。当不是定轴转动时，即血C）的方向在空间变

5、化时，式是不成立的，即角改变量不是矢量。因此，在采用角速度矢量积分时， 使得计算产生了误差，称为转动不可交换性误差。对于捷联惯导姿态更新来说，锥运动是最恶劣的工作环境条件，此 运动造成的不可交换性误差影响最大，会诱发数学平台的严重漂移。因 此，锥运动通常被作为检验姿态算法优劣的条件，也就说如果能够确保 锥运动环境条件下的算法漂移最小，就一定能确保在其余环境条件下的算法漂移最小。2.锥运动与锥误差圆锥效应是刚体运动的一种几何现象。刚体受到环境振动影响或本 身具有的角运动，使得其在二个正交轴方向存在频率相同的角振动速率 时，第三个正交轴在空间将绕其平均位置作锥面或近似锥面的运动，称 为刚体的圆锥运

6、动或圆锥效应。Y轴：QCOS（0t）Z轴：Qsin（0）圆锥运动对应的角速度矢量在载体坐标系上的分量为co? - -2 sin2 bcoy = 一屮 sm q sm 0bcoz =i/sma cos y/t该圆锥运动会在载体坐标系毛轴上产生常值角速度，该角速度具有 与陀螺常值漂移相同的性质，该角速度必为屯轴陀螺所敏感，从而产生 视在的测量误差，即圆锥误差。以上所讨论的两个角振动的相位差为90。，当相位差为°时，同样 可以导出毛轴的常值角速度为b c 2 G cox - 一2肖 sm 一 sm ©由于角振动速度的幅值、频率和相位一般是随机变量，由上式给出 的圆锥误差表达式不能

7、用于实时的修正计算，只能用来说明圆锥误差的 存在和对误差量级的估计。圆锥误差与刚体有限转动产生的不可交换误差具有相同性质。换言 之，圆锥误差是在三维角振动环境下刚体有限转动产生的不可交换误 差。因此，可以在姿态算法中用一切解决不可交换性误差的方法来减少 圆锥误差的影响。3.旋转矢量算法3.1旋转矢量的定义由刚体定点转动的欧拉定 理，参考坐标系可通过绕欧拉 轴旋转特定的角度与固结于刚 体的动坐标系重合。设欧拉轴 上的单位矢量为力，旋转角度为 0，则旋转矢量定义为=0龙二血yF '当刚体的定轴转动用旋转矢量表示为如下时，刚体的运动可称为圆 锥运动： 0 =Q COS（0）a sin（0）_

8、其中Q表示圆锥运动的锥半角，肖表示圆锥运动的角频率。刚体在三维空间作任意转动时，其等效旋转矢量w在i轴的分量 与刚体运动角速度在i轴上的分量之间有如下关系式： 2二 J： a）idt + At式中，外为由于测量的不可交换性而造成的不可测量项。对圆锥误差的 补偿，实际上就是求外的大小，旋转矢量法能够对圆锥误差进行有效的 补偿。3. 2旋转矢量微分方程旋转矢量微分方程为计算捷联惯性系统的姿态矩阵建立了全面的 理论基础。根据Euler理论,旋转矢量是姿态矩阵大小为+1的特征值所对应的 特征向量，即(C /)=0且有厂 r sin 0不(1 cos0)不 IC = / + - x + 式中，二yzF表

9、示旋转矢量，C为机体坐标系与参考坐 标系之间的方向余弦矩阵，/表示单位矩阵，0 =(卩J。 1 1= 69 + -Ox69 + 2办旋转矢量微分方程为1x（X0）2 （1 cos0）式中，cocox CDy血zF为机体角速度向量，方程右边第二项与第 三项之和就是有限转动引起的不可交换项，即圆锥误差。将上式右边第三项系数按泰勒级数展开，则上式简化为11二血+ X0 + x（212上式即为旋转矢量微分方程的常用形式。分析上式可以看出：当载体作定轴转动时，旋转矢量的方向与角速度血的方向一致，因 此上式右边的后两项为零，这时相当于直接釆用角速度血进行姿态解 算，是不会产生误差的。但在实际情况中，载体运

10、动的角速度血的方向是不断变化的，从而导 致某一时刻角速度血与当前时刻所积累的旋转矢量方向不一致，上式 右边后两项不为零，倘若仍然直接采用角速度血直接进行积分解算就会 带来刚体转动的不可交换性误差。而且，当载体作锥运动时，旋转矢量和角速度血相互垂直，此时上 式右边的后两项最大，所以锥运动反映了不可交换误差最为恶劣的情 况。3.3旋转矢量微分方程的求解在实际应用中，为保证实时性并考虑运算方便，仅取前两项，得=血+】xty2在姿态更新周期h二tk -%-1内，通过对陀螺的角增量进行等间隔 采样，可求得等效旋转矢量的估值。且根据等间隔釆样次数，求解方法 分为单子样法、双子样法、三子样法与四子样法。其中

11、，单子样法就是 四元数法。（1）等效旋转矢量算法的一般表达式设®）为"-1，"时间段内的等效旋转矢量，其中h = tk-tk_v （0） = 0。设对内陀螺的角增量进行N次等间隔采样，并假 定载体角速度可用关于时间f的N -1次多项式进行拟合,可得到等效旋 转矢量算法的一般表达式八NN-l N (&(町二£仇+工乞KijGxOji=l i=l j>i式中，N为子样数，0为陀螺输出的角增量。(2)优化准则对捷联惯导系统来说，锥运动是最恶劣的工作环境条件，它会诱发 数学平台的严重漂移，所以对旋转矢量算法作优化处理时常以锥运动作 为环境条件。这就

12、是说，如果能确保锥运动环境条件下的算法漂移最小, 就一定能确保在其余环境条件下的算法漂移最小。取围绕惯性坐标系兀轴旋转的圆锥运动：=a COS（0）a sin（0）旋转矢量算法的优化设计的误差准则是使误差达到最小。X(3)姿态更新在捷联系统的实际姿态解算中，为了补偿由于有限转动造成的不可 交换性误差，采用旋转矢量算法利用角增量计算姿态更新周期内的姿态 变化量，但进行姿态更新时是釆用四元数实现。记仪_1时刻导航系至机体系的姿态四元数为时刻导航系 至机体系的姿态四元数为Q(tk设在姿态更新周期htk-tk_x内，机体系的姿态变化四元数为 q(h),则姿态更新如下側)(A)I 2 丿 0"

13、)1 2 Jq(/z) = cos3.4简化形式的旋转矢量优化算法假设在t,t + h间隔内，对陀螺输出信号进行N次采样，0表示第i次 釆样的陀螺输出角增量信号，有可第孤严（如口2川2 7 2 Q2(2i 1 )sm ii/ t +hI 2N丿一肋smNC 屮h一 2 sm a sm -2N(2i 1J屮t+h2N丿 cosC 屮h2 sm a sm -2N可得0X0 =-4sMsir?型l2日SI3(2z-lh -cos w f +2N丿r(2z-l Y|A<sir u/ th -sirH2N丿14i/h .A<cos| t +V IriA = -"sm Nsin(ci

14、f)sinih2N )2N丿2N丿"J-'i/h、Jn,由上式可以看出：在0 xq中，X轴分量只与相对时间（Z - j）h有关, 而与绝对时间无关；y轴和Z轴分量是绝对时间t的余弦、正弦函数。在本文的圆锥运动下，能引起漂移的误差出现在兀轴上。因此，具 有相同时间间隔的两个角增量向量的叉乘对圆锥误差的贡献是相等的， 可以不考虑它们与绝对时间的关系。利用这个性质，可以将圆锥误差补偿公式简化为N1=1(NT A k i=l 丿x&n可以看出，简化后的算法计算量大大减小。3.5改进旋转矢量优化算法的一般形式(1)利用前一周期陀螺角增量输出的优化算法显然，通过增加对陀螺的釆样次

15、数N,可以提高旋转矢量的估算精 度，但是，与此同时计算机的计算量和存储量也增加了。为了进一步提高计算精度，可采用利用前一圆锥补偿周期的角增量 输出值进行补偿的圆锥补偿方法，补偿公式为N/ 1=1P工+i=lN-ii=lx&n式中，©、是加权系数，p是要利用的前一圆锥补偿周期的角增量的 个数，erN_M是前一圆锥补偿周期的陀螺输出角增量。（2）利用前一周期角增量累加和的优化算法若是考虑利用前一圆锥补偿周期总的角度变化，即前一周期的角增 量累加和，则有如下形式的圆锥补偿算法公式N/ =工0 +1=1N1i=lX 轴 + Gff x 0N 式中，&'为前一周期角增量

16、累加和，G为相应的加系数，&二丫耳。1=1（3）改进旋转矢量优化算法的一般形式推广到更一般的情形，综合考虑前两种改进方法，可得到更通用的 圆锥补偿算法N1=1pN1妫T+1+ 工®© i=li=lx 0N +G0f x0上述各种形式的圆锥误差补偿算法的系数如下表所示。子样数Ga4«30bib4单子样1/12一 了舞2/3-1/3011/15一 J 4T1/140-13/210323/420-1/18032/45三子样9/2027/203/28057/140393/280-1/4201/40157/4201207/8401/1848-31/462061/15

17、401607/462013487/92401/3360243/5601539/1120nnM拌54/10592/105214/105-1/315168/315262/315656/3151/1386-31/34651277/23102762/34657321/3465四J忏-1/600643/18018-733/450455717/1001069337/9009096163/450451/25740-19/3003059/12012-1097/4504577531/360364489/600635303/60060-1/693008992/1732514912/173251696/825五子样

18、125/25225/24325/2521375/5045/55441355/27722955/27723455/277215335/5544-5/24024215/7207217285/360361090/100187355/72072201335/720721/20592-19/24024295/4804816921/3603653321/4804828451/24024405673/144144-1/8751673/350064-743/40840824845/2450448281287/61261283795/742561422973/1225224693648/24504481/151

19、3512374375/7567561586875/1513512241250/189189518750/1891894圆锥误差补偿算法性能的仿真测试设载体沿x轴作典型的圆锥运动_ o=a cos（2 初） a sin（2 初）圆锥误差的大小取决于锥运动参数，即锥半角Q与锥运动频率/, 而各种圆锥误差补偿算法的补偿精度与角增量的采样间隔4有密切关 系。4. 1仿真实现流程仿真过程包含以下几步：(1) 设定仿真初始条件(2) 求取真实姿态角载体作典型锥运动时的真实姿态四元数Q (” = qoq <72 §3的精确解是已知的，从而可以求得从导航系n至载体系b的方向余弦矩阵为Qo+Ql

20、 -Q2 -於2（0§2 §093）2（91§3 + §0§2）2（如§2 +903）2 2 2 2 Q0 Q1 + §2 §32（§2§3 一 §00）2（qW3 一 §0§2）2（§2§3 + §0如）2 2 2 2 Qo -Q1 §2 + §3设从导航系到载体系的三次旋转顺序为：0(z)T&(y)Ty(“即 按照321的转动顺序实现的，则有51C2C13需=y(x)&(y)0(z)= C21 C

21、22 C23c13 = sin&cn = cos cos c12 = sin cos c21 = cos (p sin sin / - sin (p cos y c22 = sin (p sin & sin 厂 + cos cp cos y c23 = cos sin/c31 = cos (p sin 6 cos / + sin 9 sin y C32 = sin (p sin 0 cos y - cos (p sin y C33 = cos & cos 厂由此可以求得真实姿态角为（ 、=arctan 亠1 =c33）arctanf （笔3+摯1卫0 - <71

22、02+03 丿0 - - arcsin（Q3） = 一 arcsin（2（3 一 QoQ2）arctanC 7（P - arctan =Wii丿2（qW2 +加3）飞 2221% + <71 一 02 - 如b迟=_泌_-2.2.sin2fa(3)姿态角估计值的求取载体绕X轴作锥运动时的角速度血"是已知的，即2丿-2吋sin asin（2初）2吋sin acos（2初）对血”积分得到陀螺的角增量输出t+Nco(T)dr i = 12 、NT一 2 sin a sin2 . 2 a2rriT sm “ 2 sin 2/( t +2 sin a sinN2T2N2/T211/2N

23、> c ( 2d -、 L I 2N 丿J1接下来，计算出T+厂时刻的姿态四元数e(r+r)=e(o®(r)最后将e(r + T)代入V表达式,求出姿态角估计值八0与0。(4) 计算姿态角误差将姿态角误差定义为姿态角估计值与真实值之差，即A(p =(p-(p姿态角误差的大小用于衡量各种补偿算法的性能。4. 2仿真条件仿真条件 1： a = l°f f = lHz, 7 = 0.0245, Tf=24s仿真条件 2： a = l°f f = 10Hz,厂二 0.024s, 7）=24$其中，T二N 't为姿态更新周期（N为一个姿态更新周期内的釆样次 数

24、，即补偿算法的子样数），为仿真测试时间。4. 3仿真算法考察如下6种算法。 算法1：算法2：算法3：算法4：（单子样算法）=&（单子样加前一周期角增量算法） = & + £（*&）（双子样优化算法）2=&1 + &2 + 3 （“I X &2 ）（双子样加前一周期角增量算法）=& + &2 +&扌 H &i X §3015算法5：(三子样优化算法)927=&1 +&2 +&3 + 函(&1 "3)+亦&2 x(&3 -&1) 算法

25、6：(四子样优化算法)214315(0 X &2 + &3 x &4 ) +46105(& X &3 + &2 X &4 )54714 z、+ 105如&4)+311(/&3)4. 4仿真结果仿真结果如下面的各图所示。55555520154UUUJUUUiooa1015时间砂2025-iooa-2UUIJ：JUUU-4000Q1015时间砂20254UUU：JUUU2UIJIJiooa-iooa-2UUU3UULI-4000Q1015时间砂2025555555条件I下的真实姿态角曲线555510152025时间於.4.3,

26、20.0 0-O.20.30-0.4510152025时间於.4.30.0.2,20.30-0.4510152025-2-4-8O246-18单子样算法在条件1下的姿态角误差曲线2510152025时间於-O.J2 -O.J7 -O.JE:03 4 5 6 o o o o 0 0 0-0.-1.5x W56o 5.-O. sass5101520时间於25-1.5x 101.510.50-0.5-1510152025时间於单子样加前一周期角增量算法在条件1下的姿态角误差曲线52加5少10矿5O8642024681 bbbb 000-0.論遐蛰芻醫双子样加前一周期角增量算法在条件1下的姿态角误差曲线d-2-3-4-65 W 152025时间砂5