捷联惯导四子样旋转矢量姿态更新算法
张泽，段广仁
（哈尔滨工业大学控制理论与制导技术研究中心，黑龙江哈尔滨15000）
摘 要：姿态更新算法是捷联惯性导航系统的关键算法，目前姿态更新算法有欧拉角法、四元数法、方向余弦法和旋转矢量法。旋转矢量法可以采用多子样算法实现对不可交换误差的补偿。针对利用陀螺角增量输出进行姿态更新计算带来的不可交换性误差，考虑到导航坐标系在姿态更新周期内旋转比较缓慢的特点，研究了捷联惯导姿态更新的旋转矢量的修正算法，以此为基础详细推导了四子样的旋转矢量算法，得出利用陀螺角增量求解等效旋转矢量的显式形式。该显式形式中直接利用陀螺的增量输出，便于工程实际中应用。
关键词：姿态更新；等效旋转矢量；四子样；姿态修正
中图分类号：TP 27 文献标识码：A
1引言
姿态更新是实时地解算从机体坐标系到导航坐标系的方向余弦矩阵，姿态更新算法是捷联惯性导航系统的关键算法。传统的姿态更新算法有欧拉角法、方向余弦法和四元数法，其中，四元数皮卡算法简单、计算量小，因而在工程实际中常采用。但是该算法对不可交换误差的补偿不****，特别是运载体高动态时，这种误差更加严重。而等效旋转矢量法对这种误差作了适当补偿，特别适用于高动态的环境下工作。本文推导了四予样旋转矢量算法，并推导了利用旋转矢量进行姿态解箅时的修正算法。
2基本关系式
1)向量坐标变换的四元数乘表示法和坐标变换矩阵表示法如果将向量r（向量r在R系中的投影）和r6（向量r在6系中的投影）看作零标量的四元数，则rR和r6闻的变换关系可采用四元数乘式中，O表示四元数乘；Q表示从R系到6系的旋转四元数；Q 8表示其共轭四元数。
而坐标变换矩阵表示方法为
式中，cR为从b系到R系的坐标变化矩阵。
2)四元数的三角式及四元数微分方程Q=cos（0/2）+URsiri（0/2），当用其描述刚体定点转动，即当只关心b系相对月系的角位置时，可认为b系是由R系经过无中间过程的一次性等效旋转形成，为瞬时旋转轴和旋转方向，θ为转过的角度。
下面不加证明地给出四元数的微分方程：
3)姿态四元数如果上述中b系表征运载体机体坐标系，R系表征运载体的导航坐标系n，则Q为运载体的姿态四元数。
3旋转矢量和姿态四元数的关系
设tk时刻的机体坐标系为b(k)，导航坐标系为n(k)，tk+1时刻的机体坐标系为b(k+1)，导航坐标系为n(k+1)。记6(k)至b(k+1)的旋转四元数为g(h)，n( k)至b(k)的旋转四元数Q(tk)，即为tk；时刻的姿态四元数，n（k十1）至b(k+1)的旋转四元数为Q(tk+1)，即为tk=1
，时刻的姿态四元数，n(k)至n(k+1)的旋转四元数为p(h)，其中，h=tk-1-tk为姿态更新周期：根据式(2)可得：
还可以得到：
由式(1)可以得到下式：
依据式(1)还能得到如下各式：
联立以上各式可得到：
四元数的乘法结合律，上式可以写作：
比较上式和式(5)可得：
设姿态更新周期h=tk-1-tk内，导航坐标系的变化较缓慢，设导航坐标系的变换四元数p(h)=cos（θ/2）+uRsin（θ/2），所以由四元数表示刚体转动的意义可得θ=O，所以p(h)=1+θ，所以式(9)可写成：
由式(6)~式(8)可知，此时得到的姿态四元数上是从b(λ+1)坐标系到T(k)坐标系的姿态四元数，也就是说姿态更新很短的时间周期内忽略了导航坐标系的变化，下面研究如何修正由于忽略导航坐标系的变化而引起的姿态矩阵的解算误差。
4姿态四元数修正算法
姿态四元数理应按照式(9)进行更新，由于导航坐标系在姿态更新周期内旋转比较缓慢，采用式(10)来进行更新，但是经过若干姿态更新周期后要作适当修正，修正方法分析如下：
由式(10)算得到的Q（tk+I）。假设修正计算的周期为tk-1-tk=Mh，其中，h为姿态更新周期，M为某一正整数，那么根据式(10)更新解算：
此时得到的姿态四元数对应的姿态矩阵,也就说忽略r导航坐标系的更新，而ti=1时刻正确的姿态矩阵应该为
导航坐标系的变化可以根据下面近似得到。
假设在修正周期内（Mh）内，运载体的经度和纬度的增量分别为△λ和△L，且均为微小角，则导航坐标系在这段时间间隔内的旋转矢量为
5旋转矢量微分方程及其四子样算法研究
1) 旋转矢量的微分方程形式 由式( 10)可知，进行姿态四元数的更新，只要能够求得旋转矢量q(h)就可以进行姿态的实时更新，而q（h）由等效旋转矢量φ确定，所以姿态更新问题最终归结到求解等效旋转矢量φ
下面不加证明地给出等效旋转矢量的微分方程，即****的Borlz方程：
对其中的三角画数展开，可以得到更为简洁的近似方程。
2)等效旋转矢量的四子样算法研究按照式(1l)和式(12)求解旋转矢量有诸多不便，主要是：
①激光陀螺的输出一般为角增量，如果将角增量折算成角速率，则微商将引起严重的噪声放大效应。
②即使能够得到角速率的输出，对于式(11)和式(12)所示微分方程也只能求其数值解，所以必须对角速率采样，这样造成姿态更新周期内丢失了很多信息。
③式(10)说明，姿态更新只需求时刻，机体坐标系旋转所对应的等效旋转矢量，而不必要知道这个时间段内旋状矢量是如何变化的。因此，可采用Taylor级数展开法求解旋转矢量[2]。
设φ (h)为[tk=1，tk]时间段内的等效旋转矢量，其中，h =tk+1-tk姿态更新周期，运载体的角速度用三次抛物线拟合：
其中，τ [O，h]，对φ (h)在h一O进行Taylor级数展开：
将式(13)代入得：
则由式(13)和式(14)：
由于姿态更新周期特别短，由可以设为小量，根据式(12)计算西(r)在r=0时的各阶导数时，可略去第3项，并且近似使用φ(τ)=△θ(τ)，这样式(12)可写成：
上式求各阶导数，并考虑式(15)和式(16)可以得到如下诸式：为便于书写，把ωnbb(tk+τ)简写为ω，把△θ(τ)简写作△θ，φ(τ)简写为φ。
对式(17)求各阶导数，并利用式(15)和式(16)代人可以得到以下诸式：
将式(15)和式(16)代人上述诸式：
所以可以得到：
用陀螺输出代替上式中的参数：在一个姿态更新周期内对陀螺输出进行四次采样，记：
求解上述诸式：
将其代入式(18)得到四子样旋转矢量算法：
至此，就得到了等效旋转矢量和陀螺输出角增量间的关系，实时解算等效旋转矢量，，就可以求得姿态更新周期内载体坐标系的旋转四元数q(h)，然后根据式(10)进行姿态四元数的更新，从而得到运载体姿态的更新。
6结语
在姿态更新周期内，采用了三次抛物线来拟合运载体的角速度，得到了旋转矢量四子样算法，实际上采用不同的函数来拟合运载体的角运动，就决定了旋转矢量解算的子样数，文中用三次抛物线来拟合运载体的角运动，对应的是四子样算法。
运载体的角运动具有很大的任意性，采用某一特定函数来拟合它，本身就有一定的近似性，在实际导航系统应用过程中，应该根据运载体运动特性来选择子样数，也就是在精度和实时性间进行折衷选择。如运输机、轰炸机、舰船等角运动比较缓慢的运载体，单子样算法即满足其导航精度的需要，而对于一些精确制导式武器等则需要高子样算法。
|
