一种基于压缩结构的光纤捷联惯导系统姿态解算方法

本发明涉及一种基于压缩结构的光纤捷联惯导系统姿态解算方法，属于捷联惯性导航姿态解算技术领域。

背景技术：

干涉型光纤陀螺仪是一种新型的全固态惯性器件，因其体积小、不受加速度干扰和动态范围宽等优点而被广泛应用在捷联式惯性导航系统(strapdowninertialnavigationsystem,sins)中。由于惯性测量单元直接固联在载体上，因此陀螺仪测量的是载体相对于惯性空间的角运动，并通过导航计算机对传感器的输出进行数值积分，从而获得姿态、速度和位置等导航参数。其中姿态算法的精度直接影响比力积分变换的精度，进而影响sins速度和位置解算的精度，但由于刚体的有限转动存在不可交换性，导致在利用数值积分法求解姿态的过程中不可避免地会引入不可交换性误差，且该误差在高动态环境下尤为突出。虽然通过高频率的姿态计算可有效抑制不可交换性误差，但姿态更新频率受限于陀螺仪的采样频率。savage认为导航算法的误差应该低于惯性器件引入误差的5％，所以随着惯性器件不断发展的同时，导航算法的精度也需要改进，以满足高精度器件对算法的要求。对于一般的航空航天、航海和陆地导航等应用，传统捷联惯性导航算法的精度已经能满足要求，但随着捷联惯导系统在高动态环境下的应用，传统导航算法面临挑战，因此有必要研究一种高精度姿态算法来匹配高精度传感器并满足高动态环境下的导航定位要求。

1983年，miller首次提出将纯圆锥运动作为最复杂的环境来设计等效旋转矢量算法的系数。在此基础上，ignagni,lee,jiang和park等学者陆续提出了基于频率泰勒级数展开的多子样圆锥补偿算法。基于最小二乘法的优化准则，savage于2010年提出了显示频率整形的姿态算法，虽然其算法在广义的振动环境下和纯圆锥环境下不是最优的，但是其在固定的圆锥频率区间内是最优的。宋敏设计的等效旋转矢量算法是对传统算法的扩展，既保证了算法在纯圆锥运动下的最优精度，又提高其在大机动环境下的精度。为提高算法在高动态环境下的精度，王茂松、严恭敏和武元新等学者考虑了旋转矢量微分方程中的高阶项，提出了高阶姿态补偿算法，但算法计算量太大，无法用于实时处理系统。以上算法的实现都是利用陀螺仪输出的角增量信息，但当陀螺仪输出信号为角速率时，若利用数值积分方法将角速率信息转换为角增量信息，然后再应用传统圆锥误差补偿算法，算法精度会明显降低，无法满足惯性器件对姿态解算精度的要求。于是就有学者研究了纯角速率输入下的姿态误差补偿算法，然而这些角速率算法的精度有限，只适用于低动态环境，无法满足高动态环境下的高精度导航性能的要求。

采用毕卡逼近法求解四元数微分方程时，首先要计算出载体运动时刻所对应的姿态四元数q(t)，再将姿态四元数q(t)转化为姿态矩阵最后从姿态矩阵中求出姿态角。

求解四元数微分方程的毕卡逼近法：

式中：

由式(20)可知，需要对角速率矢量ω在一个姿态更新周期内进行积分才能得到一阶旋转矢量项(角增量)δθ，虽然在一个姿态更新周期内的角增量δθ较小，但矢量的有限转动不满足矢量相加的交换律，所以对角速率矢量的积分会产生不可交换性误差。为了消除四元数求姿中的不可交换性误差，bortz引入等效旋转矢量的概念来描述刚体相对惯性空间的转动，并提出了著名的旋转矢量微分方程用于求解旋转矢量φ：

式中，φ是旋转矢量，|φ|为旋转矢量φ的模，ω是载体角速率矢量。

由于在一个姿态更新周期h内φ较小，忽略φ的6次项及其高阶项，式(21)可简化为：

式(1)中等号后的{φ×ω/2}项被称为二阶圆锥校正项，{(1/12+|φ|²/720)φ×(φ×ω)}项被称为三叉积项，二阶圆锥校正项和三叉积项可统称为圆锥校正项。传统基于角速率输入的圆锥算法忽略了三叉积项的影响，基于旋转矢量的二阶模型设计圆锥算法，故只针对二阶圆锥校正项进行了补偿。

当载体在2个正交轴方向上存在频率相同但相位不同的角振动时，载体会绕第三个轴做近似锥面的运动，这种现象称为圆锥运动，如图3所示。捷联惯导系统在圆锥运动下进行姿态解算时会产生不可交换性误差(又称为圆锥误差)，因此常把圆锥运动作为评价旋转矢量算法优劣的标准输入。

假设载体相对导航坐标系做典型圆锥运动，其对应的四元数为：

其中ω是圆锥运动的角频率，α是圆锥运动的半锥角。此时载体角速率矢量ω(t)在载体坐标系下的投影为：

其中，q^*(t)称为q(t)的共轭四元数，为q(t)关于时间t的微分。

在实际应用中，若陀螺仪型号确定后，陀螺采样周期ts是固定的。

传统的基于角速率的二子样旋转矢量表达式为：

式中，h＝2ts，ω0、ω1、ω2分别为tm-1、tm-1+ts、tm-1+2ts时刻陀螺仪的角速率输出，分别为姿态更新周期h内的旋转矢量、角增量和二阶旋转矢量项的估计值。

传统的基于角速率的三子样旋转矢量表达式为：

式中，h＝3ts，ω3表示tm-1+3ts时刻陀螺仪的角速率输出。

基于角速率的二子样三阶旋转矢量算法表达式为：

式中，h＝2ts，ω-1表示tm-1-ts时刻陀螺仪的角速率输出。

传统的基于角速率的多子样旋转矢量算法仅对二阶圆锥校正项进行了补偿，在一个姿态更新周期内不断地增加陀螺采样频率只能减少二阶圆锥校正项中的剩余误差，但无法降低未补偿的三叉积项所带来的不可交换性误差，且其角增量估计的代数精度和二阶圆锥校正项估计的精度完全取决于当前姿态更新周期内所利用的角速率数量。基于角速率的二子样三阶旋转矢量算法虽然补偿了部分三叉积项，但未补偿的三叉积项在高动态条件下激发出的误差会大于三阶旋转矢量项中的剩余误差，在低动态环境下该误差项较小。在实际应用中姿态更新频率受限于陀螺采样频率和计算机的处理能力，因此无法通过增加陀螺采样频率和姿态更新频率来不断提高姿态算法的精度。

技术实现要素：

本发明的目的是针对传统的角速率输入的圆锥误差补偿算法因受限于姿态更新周期内的采样数和忽视三叉积项的影响而导致姿态算法在高动态环境下计算精度低的问题，提供一种基于压缩结构的光纤捷联惯导系统姿态解算方法，其可在不提高采样频率和姿态更新频率的前提下，基于等效旋转矢量的五阶模型设计具有8阶估计精度的旋转矢量，有效地降低高动态环境下产生的不可交换性误差。

上述的目的通过以下技术方案实现：

一种基于压缩结构的光纤捷联惯导系统姿态解算方法，该方法包括如下步骤：

(1)根据等效旋转矢量微分方程计算在一个姿态更新周期内的五阶毕卡分量解，分别得到一阶旋转矢量项δθ、二阶旋转矢量项δφ2、三阶旋转矢量项δφ3、四阶旋转矢量项δφ4和五阶旋转矢量项δφ5的理论值；

(2)在一个姿态更新周期h内，对陀螺仪的输出采样n次得角速率信号ω1、ω2、...、ωn，并利用上一姿态更新周期内的角速率信号ω-n+1、...、ω-1、ω0；

(3)对步骤(2)得到的2n个角速率信号进行拉格朗日插值，并在当前姿态更新周期内对该拉格朗日插值多项式积分，得到一阶旋转矢量项的估计值；

(4)利用前一姿态更新周期和当前姿态更新周期内的陀螺仪角速率信号，通过角速率矢量二叉积和三叉积的压缩组合来估计当前姿态更新周期内的二阶旋转矢量项和三阶旋转矢量项，并利用基于频率级数的泰勒展开法优化二叉积和三叉积的系数，使得二阶旋转矢量项和三阶旋转矢量项的估计值具有8阶精度；

(5)利用前一姿态更新周期和当前姿态更新周期内的陀螺仪角速率信号，通过角速率矢量三叉积和四叉积的压缩组合来估计当前姿态更新周期内的四阶旋转矢量项和五阶旋转矢量项，并利用基于频率级数的泰勒展开法优化三叉积和四叉积的系数，使得四阶旋转矢量项和五阶旋转矢量项的估计值具有8阶精度；

(6)将步骤(3)、(4)和(5)得到的估计值相加以作为一个姿态更新周期h内的旋转矢量估计值由旋转矢量估计值得到一个姿态更新周期h内的姿态变化四元数q(h)；

(7)用姿态变化四元数q(h)求得一个姿态更新周期h内的姿态四元数q(tm)。

所述的一种基于压缩结构的光纤捷联惯导系统姿态解算方法，步骤(1)的具体方法是：

根据等效旋转矢量微分方程得到近似简化的等效旋转矢量微分方程：

式中，φ是旋转矢量，|φ|为旋转矢量φ的模，ω是载体角速率矢量；

利用毕卡迭代法求解式(1)在一个姿态更新周期h内的五阶毕卡分量解，分别得到一阶旋转矢量项δθ、二阶旋转矢量项δφ2、三阶旋转矢量项δφ3、四阶旋转矢量项δφ4和五阶旋转矢量项δφ5的理论值：

δφ＝δθ+δφ2+δφ3+δφ4+δφ5(7)

式中，δφ、δθ、δφ2、δφ3、δφ4和δφ5分别为一个姿态更新周期h内的旋转矢量、一阶旋转矢量、二阶旋转矢量项、三阶旋转矢量项、四阶旋转矢量项和五阶旋转矢量项的理论值；姿态更新周期h＝tm-tm-1；tm-1是前一姿态更新周期的结束时刻，tm是当前姿态更新周期的结束时刻，m表示捷联惯性导航系统开始工作后的第m次姿态解算；θ(h)为从tm-1时刻至tm-1+h时刻累积的角增量矢量；|θ(h)|为θ(h)的模；ω(tm-1+h)为载体在tm-1+h时刻的角速率矢量,0≤h≤h。

所述的一种基于压缩结构的光纤捷联惯导系统姿态解算方法，步骤(3)的具体方法是：

先利用前一姿态更新周期和当前姿态更新周期内的全部采样角速率信息来构造拉格朗日插值多项式l(t)：

式中，tm-2<t≤tm，tm-2是前一姿态更新周期的起始时刻，n为一个姿态更新周期h内的陀螺采样次数，ts是陀螺采样周期，当陀螺的型号固定时，ts是固定值；lk(t)为插值基函数，ωk是tm-1+kts时刻的角速率矢量，k和i分别表示tm-1+kts时刻和tm-1+its时刻距tm-1时刻的采样间隔数；k＝-n+1,...,0,...,n；i＝-n+1,...,0,...,n；

再求拉格朗日插值多项式l(t)在一个姿态更新周期h内的积分，即可得到一阶旋转矢量δθ的估计值

式中，是一阶旋转矢量δθ的估计值，h＝tm-tm-1。

所述的一种基于压缩结构的光纤捷联惯导系统姿态解算方法，步骤(4)的具体方法是：

利用前一姿态更新周期和当前姿态更新周期内的全部角速率信息，使角速率矢量二叉积和三叉积的压缩组合的z轴分量逼近二阶旋转矢量项δφ2和三阶旋转矢量项δφ3的z轴分量，使其具有8阶估计精度，从而得到二阶旋转矢量项的估计值和三阶旋转矢量项的估计值

式中，是二阶旋转矢量项δφ2的估计值，ωi(i＝-n+1,...,0,...,n-1)是tm-1+its时刻的角速率矢量，ωn是tm时刻的角速率矢量，kn-i(n-i＝1,...,2n-1)为二阶旋转矢量项估计值的待优化系数；是三阶旋转矢量项δφ3的估计值，gg(g＝1,...,2n-2)为三阶旋转矢量项估计值待优化系数，ωj(j＝-n+1,...,0,...,n-1)为[tm-2,tm]时间段内tm-1+jts时刻的角速率矢量；在典型圆锥运动环境下对估计值的系数kn-i、gg进行优化，使得算法漂移误差最小。

所述的一种基于压缩结构的光纤捷联惯导系统姿态解算方法，步骤(5)的具体方法是：

利用步骤(4)中得到的角速率矢量二叉积和三叉积，对已计算得到的二叉积进行叉积可得到四叉积，使角速率矢量三叉积和四叉积的压缩组合的z轴分量逼近四阶旋转矢量项δφ4和五阶旋转矢量项δφ5的z轴分量，使其具有8阶估计精度，从而得到四阶旋转矢量项的估计值和五阶旋转矢量项的估计值

式中，是四阶旋转矢量项的估计值；aa(a＝1,...,2n-3)，bb(b＝1,...,2n-2)为四阶旋转矢量项估计值待优化系数；ωf(f＝-n+1,...,0,...,n-1)和ωk(k＝-n+1,...,0,...,n-1)分别为[tm-2,tm]时间段内tm-1+fts和tm-1+kts时刻的角速率矢量且(ωf×ωi)、(ωj×ωk)可由步骤(4)得到；是五阶旋转矢量项的估计值；pp(p＝1,...,2n-3)为五阶旋转矢量项估计值待优化系数；在典型圆锥运动环境下对估计值的系数aa、bb、pp进行优化，使得算法漂移误差最小。

所述的一种基于压缩结构的光纤捷联惯导系统姿态解算方法，步骤(6)的具体方法是：将步骤(3)、(4)和(5)所述的估计值相加，作为一个姿态更新周期h内旋转矢量估计值

由一个姿态更新周期h内旋转矢量估计值得到一个姿态更新周期h内的姿态变化四元数q(h)，计算公式为：

式中，是一个姿态更新周期内的旋转矢量的估计值，为旋转矢量估计值的模，q(h)为姿态更新周期h内载体姿态变化四元数。

所述的一种基于压缩结构的光纤捷联惯导系统姿态解算方法，步骤(7)的具体方法是：

利用姿态变化四元数q(h)和tm-1时刻的姿态四元数q(tm-1)求得tm时刻的姿态四元数q(tm)：

设q(t)＝[q0q1q2q3]^t，q0、q1、q2、q3是实数，且满足t表示转置，则姿态四元数与姿态矩阵的关系为：

从中提取载体航向角ψ、俯仰角η和横摇角γ：

式中，ψ、η、γ分别为载体坐标系相对导航坐标系的航向角、俯仰角和横摇角；cij(i＝1,2,3；j＝1,2,3)为姿态矩阵的元素。

有益效果：

(1)本发明所提出的旋转矢量求解算法可分解为对一阶旋转矢量项、二阶旋转矢量项、三阶旋转矢量项、四阶旋转矢量项和五阶旋转矢量项的估计，通过建立等效旋转矢量的五阶模型，提高旋转矢量的估计精度。

(2)将前一姿态更新周期与当前姿态更新周期内的角速率进行二叉积、三叉积和四叉积，利用角速率叉积的压缩组合补偿不可交换性误差，在不增加采样频率和姿态更新频率的前提下，通过对等效旋转矢量微分方程的五阶毕卡分量解的逼近，使一个姿态更新周期内的旋转矢量δφ具有8阶估计精度，进一步降低载体在高动态圆锥环境下引起的姿态解算误差。

(3)本发明根据角速率叉积的特性，利用其压缩结构来补偿二阶、三阶、四阶和五阶不可交换性误差，共需要完成6次叉积运算，故该方法计算量较小，可应用于实际系统。

附图说明

图1本发明原理示意图；

图2姿态更新周期与采样周期示意图；

图3典型圆锥运动示意图；

图4算法误差漂移随圆锥运动频率变化曲线；

图5算法误差漂移随半锥角变化曲线；图4-图5中b2虚线表示传统的基于角速率的二子样旋转矢量算法，b3点线表示传统的基于角速率的三子样旋转矢量算法，it2实线表示基于角速率的二子样三阶旋转矢量算法，if2星号实线表示本发明所提出的基于压缩结构的光纤捷联惯导系统姿态解算方法。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

实施例1：

如图1所示，本发明提供了一种基于压缩结构的光纤捷联惯导系统姿态解算方法。

具体步骤如下：

首先在典型圆锥运动下推导一个姿态更新周期内旋转矢量的五阶毕卡理论值；

当载体相对导航坐标系做典型圆锥运动，其对应的四元数如式(22)所示：

其中ω是圆锥运动的角频率，α是圆锥运动的半锥角，此时载体角速率矢量ω(t)在载体坐标系下的投影为：

其中，q^*(t)称为q(t)的共轭四元数，为q(t)关于时间t的微分。

则二阶旋转矢量项的z轴理论值δφ2|z为：

三阶旋转矢量项的z轴理论值δφ3|z为：

四阶旋转矢量项的z轴理论值δφ4|z为：

五阶旋转矢量项的z轴理论值δφ5|z为：

两个不同时刻角速率矢量的二叉积的z轴分量为：

ts²·ωi×ωj|z＝λ²sin²αsinλ(j-i)(31)

式中，ωi(i＝-n+1,...,0,...,n-1)和ωj(j＝-n+1,...,0,...,n-1)分别为ti和tj时刻的角速率矢量，ti＝tm-1+its，tj＝tm-1+jts，λ＝ωts，ts是陀螺采样周期，当陀螺的型号固定时，ts是固定值。

三个非同一时刻角速率矢量的三叉积的z轴分量为：

式中，ωk(k＝-n+1,...,0,...,n-1)为tk时刻的角速率矢量，tk＝tm-1+kts。

四个非同一时刻角速率矢量的四叉积的z轴分量为：

式中，ωf(f＝-n+1,...,0,...,n-1)为tf时刻的角速率矢量，tf＝tm-1+fts。

然后建立旋转矢量算法误差评价准则；

算法误差评价准则定义为在一个姿态更新周期h内旋转矢量估计值与理论值δφ在z轴上的分量之差的平均值，即其中，δφ|z为旋转矢量理论值δφ在z轴上的分量，为旋转矢量估计值在z轴上的分量，称为算法漂移。

假设姿态更新周期h为2ts，即n＝2，则可利用的角速率矢量有4个，如图2所示。对4个时刻的角速率进行拉格朗日插值并在当前姿态更新周期内积分，得到由角速率表达的一阶旋转矢量的估计值

在典型圆锥运动环境下对二阶旋转矢量项估计值三阶旋转矢量项估计值四阶旋转矢量项估计值和五阶旋转矢量项估计值的系数进行优化，使得算法漂移误差最小，从而得到算法的最优系数：

δφ2|z在ωh＝0的泰勒展开式为：

式中，o(λ¹³)表示λ¹³的高阶小量。

δφ3|z在ωh＝0的泰勒展开式为：

δφ4|z在ωh＝0的泰勒展开式为：

δφ5|z在ωh＝0的泰勒展开式为：

通常λ＜＜1，为了尽可能减小只需要令误差补偿后的λ低幂次项为零，根据频率级数推导方法，可得各阶旋转矢量项的估计值为：

综上，当n＝2时，在一个姿态更新周期h内旋转矢量的估计值为：

为了验证本发明的实用价值，下面比较4种不同的旋转矢量算法的估计精度和计算量。

表1旋转矢量算法的估计精度和计算量比较

表1中算法b2表示传统的基于角速率的二子样旋转矢量算法，算法b3表示传统的基于角速率的三子样旋转矢量算法，算法it2表示基于角速率的二子样三阶旋转矢量算法，算法if2表示本发明所提出的基于压缩结构的光纤捷联惯导系统姿态解算方法。由表1可以看出，与已有方法相比，本发明的方法估计精度最高，可达8阶估计精度，虽然计算量有所增加，但也只是传统算法的2-3倍左右，计算机仍可实现实时计算。

实施例2：

本实施例中假设陀螺在一个姿态更新周期内进行2次采样(如图2所示)，其具体步骤为：

步骤一、在一个姿态更新周期内，对陀螺仪的输出采样2次，分别得到载体运动角速率ω1、ω2，并提取存储计算机中上一姿态更新周期内的角速率ω-1、ω0。

步骤二、利用拉格朗日插值型积分方法对4个角速率进行积分，则一个姿态更新周期内一阶旋转矢量项的估计值为：

步骤三、利用前一姿态更新周期和当前姿态更新周期内的角速率矢量的二叉积和三叉积，计算当前姿态更新周期内的二阶旋转矢量项的估计值和三阶旋转矢量项的估计值

步骤四、利用前一姿态更新周期和当前姿态更新周期内的角速率矢量的三叉积和四叉积，计算当前姿态更新周期内的四阶旋转矢量项的估计值和五阶旋转矢量项的估计值

步骤五、将步骤二、三、四所述的估计值相加以作为旋转矢量的估计值

步骤六、由可以计算得到[tm-1,tm]时间段内的姿态变化四元数q(h)：

其中，是一个姿态更新周期内的旋转矢量的估计值，为旋转矢量估计值的模，q(h)为姿态更新周期h内载体姿态变化四元数；

步骤七、利用姿态变化四元数q(h)和tm-1时刻的姿态四元数q(tm-1)可以求得tm时刻的姿态四元数q(tm)：

步骤八、设q(t)＝[q0q1q2q3]^t，则姿态四元数q(t)与姿态矩阵间存在如下关系式：

式中，q0、q1、q2、q3是实数，且满足

根据姿态矩阵可以提取载体航向角ψ、俯仰角η和横摇角γ：

仿真实验：

在陀螺输出为角速率且无随机误差的仿真环境下，对发明进行仿真实验：

典型圆锥运动如图3所示，圆锥运动描述：假设载体坐标系为o-xyz，导航坐标系为o-xyz。os为xoy平面内的一条射线，o-xyz可由o-xyz绕os旋转α角后而得，当os以角频率ω绕原点o在xoy平面内旋转时，则oz轴的轨迹形成一个锥面，锥顶位于o点，对称轴为oz轴，并且x轴和y轴将分别绕x轴和y轴做振荡运动。

1)算法误差漂移误差仿真

典型圆锥运动仿真环境下，旋转矢量表达式为：

(1)当半锥角α＝1°，陀螺仪采样频率ft＝200hz，验证旋转矢量算法误差漂移与圆锥运动频率的关系，结果曲线如图4所示。

(2)当陀螺仪采样频率ft＝200hz，圆锥运动频率fω＝1hz，验证旋转矢量算法误差漂移与半锥角的关系，结果曲线如图5所示。

由此可以看出，与已有方法相比，本发明补偿不可交换性误差的精度更高。在低动态环境下，四种方法的姿态解算误差相对于传感器误差可以忽略不计，但在高动态条件下，已有方法的计算精度有限，其误差漂移将会远大于传感器误差，但本发明方法在高动态环境下仍能保持较高的姿态求解精度，能够满足载体高精度导航需求。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。