捷联惯导四子样旋转矢量姿态更新算法
张泽,段广仁
(哈尔滨工业大学控制理论与制导技术研究中心,黑龙江哈尔滨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)可得:
|