捷联惯导四元数的四阶龙格库塔姿态算法

史 凯,刘马宝

(1.西安交通大学航天学院,陕西 西安 710049；2.西安机电信息技术研究所，陕西 西安710065)

0 引言

捷联惯性导航系统不需要任何外来信息,也不会向外辐射任何信息,仅靠惯性导航系统本身就能在全天候条件下进行三维定向[1],通过弹载三轴正交陀螺仪直接解算弹体姿态,其结构简单,抗干扰能力强。姿态解算是捷联惯性导航系统的关键技术,目前姿态解算的精度一方面受制于陀螺仪自身器件的水平;另一方面是受姿态解算算法的约束,捷联式惯导姿态更新算法的精度以及复杂程度直接影响整个姿态测量系统的解算精度。

目前姿态解算的主要方法有欧拉角法、四元数法和方向余弦法。欧拉角法不能用于飞行器全姿态解算而难以广泛应用于工程实践,且实时计算困难。方向余弦法避免了欧拉角法的“奇点”现象,但方程的计算量大,工作效率低。四元数法只需要求解四个未知量的线性微分方程组,计算量比方向余弦法小,且算法简单,易于操作,是较实用的工程方法。关于四元数的求解方法,很多文献都采用数字积分的方法解算载体的姿态[2],或只将龙格库塔算法应用到弹体的滚转角解算,并没有进行三个方向的全姿态角解算[3]。文献[3]中仅介绍了姿态更新微分方程,应用了龙格库塔法进行姿态更新,但其算法结构复杂,进行了两次迭代,运算量较大,实时性和精度不能保证。为了提高姿态解算的精度,根据捷联惯导姿态更新算法要求高精度、结构复杂度低的需求[4],解决捷联惯导姿态更新算法能够满足实际工程化需求之间的矛盾,提出了捷联惯导四元数的四阶龙格库塔姿态算法。

1 坐标系定义

首先介绍两个坐标系:导航坐标系和载体坐标系。

导航坐标系:用Oxnynzn表示,它是惯导系统在求解导航参数时作采用的坐标系,一般选导航坐标系为地理坐标系,原点为载体中心,xn指向东,yn轴指向北,zn轴指向天(当不考虑地球自转对陀螺输出的影响)时Oxnynzn即发射坐标系原点即炮位,yn轴指向射向,xn轴与yn在一个水平面指向右,zn轴指向天。

载体坐标系:用Oxbybzb表示,原点为载体重心,xb沿载体横轴向右,yn轴沿载体纵轴向前,zb轴沿载体立轴向上。载体坐标系与地理坐标系的方位关系可以用姿态矩阵或者姿态角来表示。

两个坐标系具体关系图如图1所示。

本文所有的公式推导及算法均是基于上述坐标系定义的基础上进行的,按照图中所示的坐标系各

轴所示,本文绕三轴旋转的基本坐标系(右手系)转换矩阵,如下所示[5]:

(1)

图1 坐标系示意图Fig.1 Coordinate diagram

2 四元数的四阶龙格库塔姿态算法

2.1 四元数姿态更新算法

根据第1节三次基本转换坐标变换阵可以得出由n系至b系的坐标转换阵如式(2)所示。

(2)

则根据矩阵的性质可以得出:

(3)

根据四元数可确定出b系至n系的坐标变换阵,如式(4)所示。

(4)

综合式(2)、式(4)可以根据四元数确定的矩阵解算出三个欧拉角如式(5)所示:

(5)

2.2 四阶龙格库塔法姿态解算

四元数描述了刚体的定点转动,即当只关心b系相对R系的角位置时,可认为b系是由R系经过中间过程的一次性等效旋转形成的,Q包含了这种等效旋转的全部信息,uR为旋转瞬轴和旋转方向,θ为转过的角度。根据2.1节可以看出姿态解算的实质是如何更新四元数,四元数微分方程如式(6)所示：

(6)

写成矩阵形式展开即:

(7)

四阶龙格库塔法解四元数微分方程的数值解法思想是在积分区间内进行插值,优化总的斜率得到更新结果,所以其计算精度高于直接积分,龙格库塔算法阶数越高,截断误差越小,计算精度越高。在工程上,四阶龙格库塔算法的精度已经能够满足大多数精度要求,并且计算复杂度也适宜多数处理器性能。因此,四阶龙格库塔法在工程上被广泛使用,一般情况下弹道仿真大都采用四阶龙格库塔法。针对式(7)所示的四元数微分方程,相对应的四阶龙格库塔方程公式如下[8]:

(8)

式(8)中，q为四元数向量,h为仿真步长,f为四元数微分方程。经过四阶龙格库塔解四元数微分方程后可以得出下一时刻的四元数数值,由于计算误差等因素,计算过程中四元数会逐渐失去规范化特性,因此若干次更新后,必须对四元数进行规一化处理，即：

(9)

从计算过程可以看出四阶龙格库塔姿态算法避免了直接积分算法复杂的计算过程、精度差等缺点,其实质是更新四元数微分方程,进而根据更新的四元数求解弹丸的姿态角。该方法只需迭代一次即可更新四元数,算法运算量小,四阶龙格库塔姿态算法相对于直接积分姿态算法中通过级数展开略去高次项从而保证计算精度的方法。其姿态解算的精度可以直接通过仿真步长进行控制,简单易行,便于调试[9]。

3 算法验证

3.1 仿真分析

为了验证算法的正确性和精度,本文在迫弹平台上对算法进行验证,从而为后续在迫弹平台上应用捷联式惯导系统提供理论依据。仿真模型:120迫弹6 DOF弹道模型,初速200 m/s,初始射角20°,仿真步长0.1 ms,则陀螺四元数解算弹丸姿态与6 DOF弹道模型原始数据外弹道姿态角对比如下图所示。

图2 解算偏航角与理论值对比Fig.2 The calculated yaw angle compared with the theoretical value

图3 解算滚转角与理论值对比Fig.3 The calculated roll angle compared with the theoretical value

图4 解算俯仰角与理论值对比Fig.4 The calculated pitch angle compared with the theoretical value

通过仿真结果可以看出,捷联惯导四元数的四阶龙格库塔姿态算法在迫弹全弹道环境下全向姿态解算精度很高,误差量级为10-2。由于采样率对算法的影响也不能忽略,故对不同采样率对算法精度的影响展开分析,通过改变采样频率,对不同采样率下对滚转角、偏航角、俯仰角误差进行了统计如表1，表2所示。

通过表1，表2可以看出120迫弹平台弹丸转速在10转以内采样率100 Hz已经可以满足系统要求,同时可以看出随着采样率的提高,姿态解算精度也随之提高。

表1 转速4 r/s下不同采样率算法解算误差

表2 转速14 r/s下不同采样率算法解算误差

3.2 实验验证

根据仿真结果设计了相关实验,开发了弹道飞行姿态测试系统,其结构外形如图5所示。弹道飞行参数测试装置实时获得弹体的三轴角速度,根据三轴角速度信息解算出弹体的姿态,测试装置如图所示,其中捷联惯导三轴陀螺仪的安装正方向如图6所示。

图5 弹道飞行姿态测试装置Fig.5 Ballistic flight attitude testing device

图6 三轴陀螺仪安装正方向Fig.6 The three-axis gyro’s positive direction

按照要求设计了弹丸的抛洒试验,即将弹道飞行参数测试装置固定于距离地面一定高度的装置中,点火后将测试装置弹出,此时通过弹载三轴陀螺仪实时采集到的角速度信息通过弹载记录仪进行采集试验后回收测试装置,读取弹载存储器中数据,获得弹体的三轴角速度,进而通过地面计算机计算软件解算出弹丸的姿态,完成弹道姿态测试进行弹丸姿态解算同时与高速录像观测结果比对,其中实时采集的弹丸三轴角速度和解算得到的弹丸姿态如图7—图9所示。

图7 三轴角速度原始信号Fig.7 Triaxial angular velocity original signal

图8 三轴加速度原始信号Fig.8 Triaxial acceleration original signal

图9 姿态角解算结果Fig.9 Attitude angle solution results

通过结果可以看出在抛撒过程中姿态变化不大,俯仰角有40°,滚转角和偏航角基本在20°以内,这与高速录像观测的弹丸姿态变化基本吻合,可以真实反映弹丸在抛撒过程中的飞行姿态变化,从而验证了捷联惯导四元数的四阶龙格库塔姿态算法具有工程实用性。

4 结论

本文提出了捷联惯导四元数的四阶龙格库塔姿态算法。该算法仅需一次迭代即可以计算出三个姿态角,通过120迫弹平台对算法进行了验证,仿真结果表明该算法结构简单,精度较高。根据工程实用性和弹上实时性要求,设计开发了弹道飞行姿态测试装置,同时设计了相关试验,实验结果表明算法可以实时计算出弹丸的飞行姿态,可工程化使用,为后续在迫弹平台上应用低成本捷联式惯导进行姿态解算奠定了理论基础。