一种高动态变转速载体的姿态解算方法与流程

本发明属于导航技术领域，具体涉及应用于高动态变转速载体的一种捷联惯性导航系统的姿态解算方法。

背景技术：

捷联惯性导航系统中，惯性器件直接与运载体固连，同时承受运载体的角运动和线运动，通过陀螺仪测量和软件解算建立虚拟稳定平台——“数学平台”，利用数学平台将运载体坐标系下的加速度计测量转换到导航坐标系，然后进行速度和位置积分解算。捷联惯性导航系统具有抗外界干扰能力强、不需要外界环境提供额外信息等优点。

姿态解算是捷联惯性导航系统中的关键技术，解算结果的精度受陀螺仪自身器件水平以及姿态解算算法两方面的约束。刚体做有限转动时，刚体的空间角位置与旋转次序有关，即存在旋转的不可交换性误差。为了补偿旋转引起的不可交换性误差，国内外学者提出使用多子样旋转矢量算法，但是在采样频率不变的情况下使用多子样旋转矢量算法会降低姿态更新频率，而提高采样频率会使硬件负担加重并加大量化误差。

传统的角速率旋转矢量算法建立在陀螺输出为角增量的基础上，当陀螺输出为角速率时，从角速率中提取角增量再应用传统的多子样旋转矢量算法会导致补偿精度下降。有学者提出了利用角速率的多子样旋转矢量方法，但仍存在降低姿态更新频率的问题。

技术实现要素：

为了解决多子样旋转矢量法会降低姿态更新频率的问题，提出了一种高动态变转速载体的姿态解算方法；利用当前以及上一姿态更新周期角增量信息和当前以及之前n个时刻的角速率信息的单子样旋转矢量姿态解算方法。

同时考虑到高采样率会给硬件带来较大负担，当载体转速较低时降低惯导系统的采样频率。

本发明的技术方案为：一种高动态变转速载体的姿态解算方法，具体包括如下步骤：

s1：获取当前时刻捷联惯性导航系统的数据输出；

s2：由于载体转速变化过程已知，通过当前时刻时间判断载体为高转速还是低转速：若为高转速，转至步骤s3至s5；若为低转速，转至步骤s6；

s2所述的判断载体转速的过程，其目的在于，当载体为高转速时，惯导系统采用较高的采样频率，以保证解算结果符合精度要求；当载体为低转速时，适当降低惯导系统的采样频率，以减轻硬件的负担。

s3：利用当前以及上一姿态更新周期角增量信息和当前以及之前n个时刻的角速率信息，解算姿态更新周期内的旋转矢量：

其中，h为陀螺仪采样间隔，也为姿态的更新间隔，ω0为当前时刻t陀螺仪输出的角速率，ω-i为t-ih时刻陀螺仪输出的角速率，为|t-h，t]内的角增量，为[t-2h，t-h]内的角增量，角增量由采样值的数值积分求得，gi和k为误差补偿系数；

s4：由计算[t-h，t]时间段内的姿态变化四元数q(h)，公式为：

其中，为旋转矢量的模。

s5：利用公式更新当前时刻的姿态四元数；

s6：利用毕卡法求解四元数微分方程，更新当前时刻的姿态四元数；

四阶近似的四元数毕卡法公式为：

其中，q(tk)为上一时刻的姿态四元数，q(tk+1)为更新后的姿态四元数，为载体坐标系相对导航坐标系在载体坐标系中的角速率投影，为的四元数矩阵形式，δθx、δθy、δθz为采样间隔[tk，tk+1]内的角增量，δθ为角增量模值，

s7：根据姿态四元数与姿态矩阵之间的关系求得姿态矩阵公式如下：

再通过计算得到当前时刻载体的姿态角；

s8：判断载体是否到达终点，若是，则算法结束；若否，执行s1。

以下就本发明的优化方法进行说明：

对捷联惯导系统姿态更新来说，标准圆锥运动是最恶劣的工作环境，会诱发数学平台的漂移，所以对旋转矢量算法作优化处理常以标准圆锥运动最为环境条件，假设圆锥运动用旋转矢量可以表示为：

φ1＝[0αcosωtαsinωt]^t(1)

其中，α为圆锥运动的半锥角，ω为圆锥运动的锥频率，假设载体单纯做圆锥运动。

根据式(1)可以得到实时的姿态四元数为：

式(2)中q(t)为t时刻载体坐标系相对参考参考坐标系的旋转四元数。设[t，t+h]时间段对应的坐标系的旋转四元数为q(h)，则有

根据式(2)和式)(3)可以得到：

根据四元数微分方程：

由式(2)和式(5)可得载体坐标系相对参考坐标系的角速度为：

[t，t+h]时间段对应的角增量为：

由旋转矢量与q(h)的关系可得[t，t+h]时间段对应的旋转矢量为：

下面就本发明提出的单子样旋转矢量算法进行详细说明：

高动态条件下，利用陀螺前一姿态更新周期内的角增量可以提高姿态解算精度。本发明利用当前以及前一姿态更新周期角增量信息和当前以及之前n个时刻的角速率信息，提出了一种新的姿态算法，可表示成如下形式：

其中，h为姿态的更新间隔，等于陀螺仪的采样间隔，ω0为当前时刻t陀螺仪输出的角速率，ω-i为t-ih时刻陀螺仪输出的角速率，为[t-h，t]内的角增量，为[t-2h，t-h]内的角增量，gi和k为误差补偿系数。

由式(6)可得：

其中只有为非周期项，其余两项都是与锥运动同频率的交变量，在误差分析中可以略去。只考虑非周期项时：

由式(7)可得：

只考虑非周期项时：

考虑sinα≈α，在标准圆锥运动环境下对该算法进行优化，使得算法漂移误差最小，从而计算得到误差补偿系数gi和k，具体表达式如下：

将式(11)和式(14)代入式(9)，再将式(8)和式(9)代入式(15)，得：

令λ＝ωh，则式(16)可以表示为：

下面以n＝2为例计算误差补偿系数的值。

将式(18)利用下式(19)进行泰勒展开：

式(18)中的前两项的展开结果为：

式(18)中的后三项的展开结果为：

令式(20)与式(21)中λ³、λ⁵、λ⁷前系数相等，可得

解得：

因此，n＝2时的旋转矢量估计值可以表示为

此时算法的漂移误差为：

可以看出算法具体较高的精度。

本发明的有益效果：本发明的一种高动态变转速载体的姿态解算方法，与现有技术相比具有以下优点：

(1)本发明在解算姿态过程中加入了判断载体转速的过程，其目的在于，当载体为高转速时，惯导系统采用较高的采样频率，以保证解算结果符合精度要求；当载体为低转速时，适当降低惯导系统的采样频率，以减轻硬件的负担。

(2)本发明所述的求取旋转矢量的方法，可以按照惯导系统的采样频率更新姿态，解决了传统多子样旋转矢量算法会降低姿态更新频率的问题。同时利用了当前以及前一姿态更新周期角增量信息和当前以及之前n个时刻的角速率信息补偿圆锥误差，误差补偿系数可通过解线性方程组求得。

附图说明

图1是本发明所述的一种高动态变转速载体的姿态解算方法的流程图。

图2是本发明所述的一种高动态变转速载体的姿态解算方法的示意图。

图3是本发明实施例中载体运动轨迹。

图4是本发明实施例中载体姿态角变化情况。

图5是本发明实施例中载体姿态角解算误差。

具体实施方式

下面结合附图和具体实施例，对本发明进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图1所示，本发明提供了一种高动态变转速载体的姿态解算方法，在实施例中旋转矢量估计值采用n＝2时的算法表达式计算，其具体步骤为：

s1：获取当前时刻捷联惯性导航系统的数据输出，得到当前时刻陀螺仪角速率输出ω0；

s2：由于载体转速变化过程已知，通过当前时刻时间判断载体为高转速还是低转速：若为高转速，转至步骤s3至s5；若为低转速，转至步骤s6；

s3：利用当前以及上一姿态更新周期角增量信息和当前以及之前2个时刻的角速率信息，解算姿态更新周期内的旋转矢量：

其中，h为陀螺仪采样间隔，也为姿态的更新间隔，ω0为当前时刻t陀螺仪输出的角速率，ω-1、ω-2分别为t-h、t-2h时刻陀螺仪输出的角速率，为[t-h，t]内的角增量，为[t-2h，t-h]内的角增量，角增量由采样值的数值积分求得；

s4：由计算[t-h，t]时间段内的姿态变化四元数q(h)，公式为：

其中，为旋转矢量的模。

s5：利用公式更新当前时刻的姿态四元数；

s6：利用毕卡法求解四元数微分方程，更新当前时刻的姿态四元数，四阶近似的四元数毕卡法公式为：

其中，q(tk)为上一时刻的姿态四元数，q(tk+1)为更新后的姿态四元数，为载体坐标系相对导航坐标系在载体坐标系中的角速率投影，为的四元数矩阵形式，δθx、δθy、δθz为采样间隔[tk，tk+1]内的角增量，δθ为角增量模值，

s7：根据姿态四元数与姿态矩阵之间的关系求得姿态矩阵公式如下：

再通过计算得到当前时刻载体的姿态角；

s8：判断载体是否到达终点，若是，则算法结束；若否，执行s1。

具体实施例：

根据上述方法步骤，本实施例利用轨迹发生器仿出一段高动态变转速载体的运动轨迹，仿真时长30s，30s内航向角从90°变化到120°，俯仰角从45°变化到-45°，横滚角起始时刻为0°,0～15s内转速从20r/s下降到5r/s，15s～20s内转速从5r/s下降到1r/s，20s～30s内转速保持1r/s。发射时刻载体经度114°e，纬度30°n，高度0米。陀螺仪的零偏稳定性设为20°/h，加速度计零偏稳定性设为100μg。

0～20s内的转速大于1r/s，判定为高转速，惯导系统采样率为1000hz，用本发明提出的单子样旋转矢量法更新姿态四元数，20～30s内的转速为1r/s，判定为低转速，惯导系统采样率为100hz，用毕卡法更新姿态四元数。

轨迹发生器生成的载体运动轨迹如图3所示，载体的姿态角变化如图4所示，利用本发明提出的高动态变转速载体的姿态解算方法得到的姿态角误差如图5所示。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。