CN102506862B - 基于2阶不可交换误差补偿模型的圆锥补偿方法 - Google Patents

Abstract

本发明公开一种基于2阶不可交换误差补偿模型的圆锥算法，属于捷联惯性导航领域，步骤为：在单位算法更新周期内，对陀螺仪的输出采样3次得到采样值；求出整个算法更新周期内的角增量；计算采样值的各1次叉乘项和各2次叉乘项；从陀螺仪的3次采样值出发，利用Bortz方程的旋转矢量1阶不可交换误差补偿模型对角增量进行1阶补偿：利用Bortz方程的2阶不可交换误差补偿模型对前述步骤求得的旋转矢量再进行2阶不可交换误差补偿；把经1阶补偿和2阶补偿后的旋转矢量代替角增量，代入捷联求姿算法，求取机体的飞行姿态。此算法可在不增加采样频率和更新周期的前提下，有效减小传统圆锥算法中的截断误差，提高精度。

Description

基于2阶不可交换误差补偿模型的圆锥补偿方法

技术领域

本发明属于捷联惯性导航领域，涉及对机体角运动产生的不可交换性误差的补偿方法，适用于角增量陀螺仪的纯捷联惯性导航系统求姿，也可适用于由捷联惯导构成的各种组合导航系统，对于角速率陀螺仪构成的捷联求姿系统以及捷联惯导中求速度的划桨算法，发明思想同样适用。 

背景技术

捷联惯性导航系统在求解机体姿态时一般采用四元数算法，在更新四元数时必须对机体的角速度ω进行积分以求取角增量Δθ＝∫ωdt。而角速度ω是变化的矢量，对其积分会产生不可交换性误差。为了消除不可交换性误差，引入旋转矢量Φ代替四元数算法中的角增量Δθ，并把圆锥运动下算法的精度高低做为衡量旋转矢量算法优劣的标准。 

在此基础上，Miller于1983年提出了经典的三子样圆锥算法，其数学基础来源于Bortz旋转矢量微分方程的1阶不可交换误差补偿模型，故Miller的圆锥算法只能对不可交换性误差进行1阶补偿，对2阶和2阶以上的不可交换性误差没有补偿，影响了算法精度；同时该算法还存在着1阶补偿不完全引起的常值漂移误差。后续学者陆续提出了改进的圆锥算法，如Lee提出的4子样圆锥优化算法，Jiang提出的3子样增强圆锥算法，Chan Gook提出的利用前次周期采样值的N子样圆锥算法通式等，这些算法的思路都是通过增加更新周期内的采样数，减小传统圆锥算法中的常值漂移误差，从而提高算法精度。从数学本质上看，它们和Miller的3子样圆锥算法一样，都是建立在Bortz方程的1阶不可交换误差补偿模型的数学基础上，并不能解决高阶的不可交换性误差补偿问题。 

采用四元数算法的捷联导航系统，其过程是先计算出机体飞行时对应的四元 数Q，再根据四元数和姿态阵 的对应关系，分别求出姿态阵和姿态角。 

四元数的计算公式为： 

$Q(n+1)=[I\cos \frac{\mathrm{\Δ\θ}}{2}+\mathrm{\Δ\Θ}\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}]Q\left(n\right)---\left(1\right)$

其中： 

$\left\{\begin{array}{c}\mathrm{\Δ\Θ}=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\θ}}_x& -{\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z\\ {\mathrm{\Δ\θ}}_x& 0& {\mathrm{\Δ\θ}}_z& -{\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z& 0& {\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_x& 0\end{array}\right]\\ \mathrm{\Δ\θ}=\∫\mathrm{\ωdt}\≈\sqrt{{\mathrm{\Δ\θ}}_x^2+{\mathrm{\Δ\θ}}_y^2+{\mathrm{\Δ\θ}}_z^2}\end{array}\right.---\left(2\right)$

可见式(1)中的Δθ需要对角速度矢量ω进行积分，而矢量的有限转动不符合矢量相加的交换律，具有不可交换性，所以对矢量的积分会产生不可交换性误差。如图1所示，机体是先绕x轴逆时针旋转90度，再绕y轴逆时针旋转90度；与先绕y轴逆时针旋转90度，再绕z轴逆时针旋转90度最后所停留的位置是不同的。 

为了消除四元数求姿中的不可交换性误差，引入旋转矢量Φ来描述刚体相对惯性空间的转动。其定义为：刚体的转动以旋转矢量Φ为转轴，转动角度等于Φ的幅值。利用旋转矢量Φ可以唯一确定飞行器在给定时间内的位置变化。Bortz提出了著名的旋转矢量微分方程用于求解旋转矢量Φ： 

$\stackrel{\·}{\mathrm{\Φ}}=\mathrm{\ω}+\frac{1}{2}\mathrm{\Φ}\×\mathrm{\ω}+\frac{1}{12}\mathrm{\Φ}\×(\mathrm{\Φ}\×\mathrm{\ω})...---\left(3\right)$

对式(3)两端积分得： 

$\mathrm{\Φ}=\mathrm{\Δ\θ}+\frac{1}{2}{\∫}_t^{t+h}\mathrm{\Φ}\×\mathrm{\ωdt}+\frac{1}{12}{\∫}_t^{t+h}\mathrm{\Φ}\×(\mathrm{\Φ}\×\mathrm{\ω})\mathrm{dt}...---\left(4\right)$

式(4)中等号后的第2项即为旋转矢量对刚体转动产生的不可交换性误差的1阶补偿，第3项为不可交换性误差的2阶补偿。 

传统的圆锥算法为了简化一般省略第3项及后面项。则旋转矢量微分方程近似为： 

$\mathrm{\Φ}=\mathrm{\Δ\θ}+\frac{1}{2}{\∫}_t^{t+h}\mathrm{\Φ}\×\mathrm{\ωdt}---\left(5\right)$

式(5)的两端都有Φ，不容易求解。等式右端取近似Φ≈Δθ得： 

$\mathrm{\Φ}=\mathrm{\Δ\θ}+\frac{1}{2}{\∫}_t^{t+h}\mathrm{\Δ\θ}\×\mathrm{wdt}---\left(6\right)$

式(6)是求解旋转矢量的理论表达式，由它推导出的实际旋转矢量的算法形式一般与机体的角运动状态Δθ有关。当机体在2个正交轴方向存在频率相同而相位正交的角振动时，机体第3个轴会在空间绕其平均位置做锥面或近似锥面的运动，这种现象称为圆锥运动，如图2所示。圆锥运动产生的几何效应会給捷联惯导系统的姿态解算带来较大误差，因此一般把圆锥运动作为衡量旋转矢量算法优劣的标准运动。 

假设机体实际的角运动是3次多项式： 

Δθ＝aτ+bτ²+cτ³         (7) 

一次算法更新周期内采样3次，则： 

$\left\{\begin{array}{c}\mathrm{ah}={\mathrm{\Δ\θ}}_3-\frac{7}{2}{\mathrm{\Δ\θ}}_2+\frac{11}{2}{\mathrm{\Δ\θ}}_1\\ {\mathrm{bh}}^2=\frac{9}{2}(-{\mathrm{\Δ\θ}}_3+{3\mathrm{\Δ\θ}}_2-{2\mathrm{\Δ\θ}}_1)\\ {\mathrm{ch}}^3=\frac{9}{2}({\mathrm{\Δ\θ}}_3-{2\mathrm{\Δ\θ}}_2+{\mathrm{\Δ\θ}}_1)\end{array}\right.---\left(8\right)$

把式(7)、(8)代入式(6)得： 

Φ＝Δθ₁+Δθ₂+Δθ₃+k₁Δθ₁×Δθ₃+k₂Δθ₂×(Δθ₃-Δθ₁)， 

该式是通用旋转矢量算法。为了在圆锥运动下达到最优，还需对算法系数k₁，k₂进行优化。设机体做圆锥运动如下式所示： 

$Q\left(t\right)=\left[\begin{array}{cccccc}\cos \frac{\mathrm{\α}}{2}& 0& \sin \frac{\mathrm{\α}}{2}\cos & \mathrm{wt}& \sin \frac{\mathrm{\α}}{2}\sin & \mathrm{wt}\end{array}\right]---\left(10\right)$

其中α是圆锥运动的锥半角，ω是圆锥运动的角速度。此时机体对应的角速度为： 

$\mathrm{\ω}=\left[\begin{array}{c}-2w{\sin }^2\left(\frac{\mathrm{\α}}{2}\right)\\ -\mathrm{\ω}\sin \left(\mathrm{\α}\right)\sin \left(\mathrm{\ωt}\right)\\ \mathrm{\ω}\sin \left(\mathrm{\α}\right)\cos \left(\mathrm{\ωt}\right)\end{array}\right]---\left(11\right)$

则一次算法更新周期间隔对应的更新四元数真值为： 

$q\left(h\right)=\left[\begin{array}{c}1-2{\sin }^2\frac{\mathrm{\α}}{2}{\sin }^2\frac{\mathrm{\ωh}}{2}\\ -{\sin }^2\frac{\mathrm{\α}}{2}\sin \mathrm{\ωh}\\ -\sin \mathrm{\α}\sin \left(\frac{\mathrm{\ωh}}{2}\right)\sin \mathrm{\ω}(t+\frac{h}{2})\\ \sin \mathrm{\α}\sin \left(\frac{\mathrm{\ωh}}{2}\right)\cos \mathrm{\ω}(t+\frac{h}{2})\end{array}\right]---\left(12\right)$

而旋转矢量算法对应的更新四元数为： 

$\widehat{q\left(h\right)}=\left[\begin{array}{c}\cos \left(\right|\mathrm{\Φ}\left|\right)/2\\ ({\mathrm{\Φ}}_x/|\mathrm{\Φ}\left|\right)\sin \left(\right|\mathrm{\Φ}|/2)\\ ({\mathrm{\Φ}}_y/|\mathrm{\Φ}\left|\right)\sin \left(\right|\mathrm{\Φ}|/2)\\ ({\mathrm{\Φ}}_z/|\mathrm{\Φ}\left|\right)\sin \left(\right|\mathrm{\Φ}|/2)\end{array}\right]=\left[\begin{array}{c}C\\ {\mathrm{\Φ}}_{x}S\\ {\mathrm{\Φ}}_{y}S\\ {\mathrm{\Φ}}_{z}S\end{array}\right]---\left(13\right)$

式中C＝cos(|Φ|/2)，S＝sin(|Φ|/2)/|Φ|。则旋转矢量对应的更新四元数与更新四元数的真值之间的误差为： 

$\tilde{q}\left(h\right)=q\left(h\right)*\widehat{q^{-1}}\left(h\right)=\left[\begin{array}{c}{q}_{0}C-S(-q_1{\mathrm{\Φ}}_x-q_2{\mathrm{\Φ}}_y-q_3{\mathrm{\Φ}}_z)\\ q_{1}C-S(q_0{\mathrm{\Φ}}_x-q_3{\mathrm{\Φ}}_y+q_2{\mathrm{\Φ}}_z)\\ q_{2}C-S(q_3{\mathrm{\Φ}}_x+q_0{\mathrm{\Φ}}_y-q_1{\mathrm{\Φ}}_z)\\ q_{3}C-S(-q_2{\mathrm{\Φ}}_x+q_1{\mathrm{\Φ}}_y+q_0{\mathrm{\Φ}}_z)\end{array}\right]---\left(14\right)$

q₂、q₃、Φ_y、Φ_z都是周期量，因此式(14)中的 也是周期量。而 包含常值误差项q₁C-Sq₀Φ_x。考虑到目前导航计算机的算法更新周期很短，因此有： 

C＝cos(|Φ|/2)≈1，S≈sin(|Φ|/2)/|Φ|≈1/2，q₀≈1    (15) 

则 的常值误差为： 

${\tilde{q}}_1\left(h\right)\≈q_1-\frac{1}{2}{\mathrm{\Φ}}_x---\left(16\right)$

显然 越小越好。把式(9)、式(11)代入式(16)，并令 得：  $k_1=\frac{9}{20},$ $k_2=\frac{27}{20}.$ 即： 

$\mathrm{\Φ}={\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2+{\mathrm{\Δ\θ}}_3+\frac{9}{20}{\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3+\frac{27}{40}{\mathrm{\Δ\θ}}_2\×({\mathrm{\Δ\θ}}_3-{\mathrm{\Δ\θ}}_1)---\left(17\right)$

式(17)即为Miller经典的3子样圆锥算法。其对应的误差四元数 很 小，单位时间内的残差为： 

${\mathrm{\Φ}}_{\mathrm{\ϵ}}={\sin }^2\frac{\mathrm{\α}}{2}\·\left(\frac{w^7{h}^7}{51030}\right)/h---\left(18\right)$

式(18))即是传统3子样圆锥算法的常值漂移误差，其本质是因为圆锥算法对1阶不可交换性误差的补偿不完全，使得 所引起。 

在此基础上，后续算法努力消除式(18)所代表的常值漂移误差。通过理论可以证明，增加更新周期内的采样数可以有效的减少圆锥算法的常值漂移。例如4子样圆锥优化算法，在一次更新周期内采样4次，可以得到4子样圆锥优化算法： 

$\mathrm{\Φ}={\mathrm{\θ}}_1+{\mathrm{\θ}}_2+{\mathrm{\θ}}_3+{\mathrm{\θ}}_4+\frac{54}{105}({\mathrm{\θ}}_1\×{\mathrm{\θ}}_4)+\frac{92}{105}({\mathrm{\θ}}_1\×{\mathrm{\θ}}_3)+\frac{214}{105}({\mathrm{\θ}}_1\×{\mathrm{\θ}}_2)---\left(19\right)$

4子样圆锥优化算法消除了 项常值误差，其单位时间内的常值漂移误差为： 

${\mathrm{\Φ}}_{\mathrm{\ϵ}}=\left(\frac{{\mathrm{\α}}^2{w}^9{h}^9}{82575360}\right)/h---\left(20\right)$

对比式(20)和式(18)可以看出，1次更新周期内采样4次的4子样圆锥算法，其常值漂移误差比1次更新周期内采样3次的3子样圆锥算法大大减小。 

此外，比较著名的改进算法还有Jiang提出的3子样增强圆锥算法，ChanGook提出的利用前次周期采样值的N子样圆锥算法通式等。这些算法的思路也是通过增加更新周期内的采样数，减小常值漂移误差，从而提高算法精度。本质上它们和传统3子样，4子样圆锥算法一样，都是建立在Bortz方程的1阶不可交换误差补偿模型的数学基础上。 

发明内容

本发明所要解决的技术问题，是针对前述背景技术中的缺陷和不足，对传统圆锥算法进行改进，提供一种基于2阶不可交换误差补偿模型的圆锥算法，其可在不增加采样频率和更新周期的前提下，有效减小传统圆锥算法中的截断误差，提高精度。 

本发明为解决以上技术问题，所采用的技术方案是： 

一种基于2阶不可交换误差补偿模型的圆锥算法，包括如下步骤： 

(1)在单位算法更新周期内，对陀螺仪的输出采样3次得到采样值Δθ₁，Δθ₂，Δθ₃； 

(2)根据下式求出整个算法更新周期内的角增量Δθ： 

Δθ＝Δθ₁+Δθ₂+Δθ₃

(3)计算采样值的各1次叉乘项和各2次叉乘项，其中，采样值的1次叉乘项包含以下3项： 

Δθ₁×Δθ₃，Δθ₁×Δθ₂，Δθ₂×Δθ₃

采样值的2次叉乘项包含以下3项： 

Δθ₁×(Δθ₁×Δθ₂)，Δθ₃×(Δθ₁×Δθ₂)，Δθ₁×(Δθ₁×Δθ₃) 

(4)从陀螺仪的3次采样值出发，利用Bortz方程的旋转矢量1阶不可交换误差补偿模型对角增量Δθ进行1阶补偿： 

所述Bortz关于1阶不可交换误差补偿模型为： 

$\stackrel{\·}{\mathrm{\Φ}}=\mathrm{\ω}+\frac{1}{2}\mathrm{\Φ}\×\mathrm{\ω}$

其中的Φ代表更新周期内的旋转矢量，ω代表机体的角速度，两边积分得： 

$\mathrm{\Φ}=\mathrm{\Δ\θ}+\frac{1}{2}{\∫}_t^{t+h}\mathrm{\Φ}\×\mathrm{\ωdt}$

得到下式，其中δθ′代表1阶不可交换误差补偿： 

$\left\{\begin{array}{c}\mathrm{\Φ}=\mathrm{\Δ\θ}+\mathrm{\δ}{\mathrm{\Φ}}^{\′}\\ \mathrm{\δ}{\mathrm{\Φ}}^{\′}=\frac{9}{20}{\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3+\frac{27}{40}{\mathrm{\Δ\θ}}_2\×({\mathrm{\Δ\θ}}_3-{\mathrm{\Δ\θ}}_1)\end{array}\right.$

(5)利用Bortz方程的2阶不可交换误差补偿模型对步骤(4)求得的旋转矢量再进行2阶不可交换误差补偿： 

所述Bortz关于2阶不可交换误差补偿模型为： 

$\stackrel{\·}{\mathrm{\Φ}}=\mathrm{\ω}+\frac{1}{2}\mathrm{\Φ}\×\mathrm{\ω}+\frac{1}{12}\mathrm{\Φ}\×(\mathrm{\Φ}\×\mathrm{\ω})...$

根据该模型进一步补偿不可交换性误差的方法为： 

$\left\{\begin{array}{c}\mathrm{\Φ}=\mathrm{\Δ\θ}+\mathrm{\δ}{\mathrm{\Φ}}^{\′}+\mathrm{\δ}{\mathrm{\Φ}}^{\′\′}\\ {\mathrm{\δ\Φ}}^{\′\′}=l_1\mathrm{\Δ}{\mathrm{\θ}}_1\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_2)+l_2{\mathrm{\Δ\θ}}_3\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_2)+l_3{\mathrm{\Δ\θ}}_1\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3)\\ l_1=-\frac{14153}{73},l_2=\frac{13742}{75},l_3=\frac{11401}{61}\end{array}\right.$

上式中的δΦ″即是圆锥算法中的旋转矢量对不可交换性误差的2阶补偿，它一般由不同值的Δθ_i×(Δθ_j×Δθ_k)项组合而成(i＝1，2，3；j＝1，2，3；k＝1，2，3)，需要特别指出的是，尽管前式中仅列出采样值叉乘的3种形式的表达式，但由圆锥运动下的叉乘公式可知，对于θ₁、θ₂、θ₃的其它任意叉乘组合，均可以变换为前述3个表达式中的某一个(见式(27))，因此，尽管前式中仅列出3种形式的表达式，但其保护范围可涵盖所有叉乘组合；l是2阶不可交换性误差补偿项的适当系数，可以根据实际情况选取，例如：对于角增量陀螺，上式中可以选取i＝j＝1，k＝2，对应的系数是l₁＝-14153/73；i＝3，j＝1，k＝2，对应的系数是l₂＝13742/75；i＝1，j＝1，k＝3，对应的系数是l₃＝11401/61。 

(6)把经1阶补偿和2阶补偿后的旋转矢量Φ代替角增量，代入捷联求姿算法，求取机体的飞行姿态。 

以下将就本发明的改进点进行说明。 

传统的3子样圆锥算法总共包含两种算法误差：式(18)代表的常值漂移误差和由式(15)近似所产生的截断误差。前已所述，常值漂移误差的实质是圆锥算法对1阶不可交换性误差补偿不完全所引起的残差，理论上它可以通过增加更新周期内的采样数来不断减小，而截断误差在传统圆锥算法中一般被忽略不计。为了更好的分析和减小圆锥算法误差，发明人对传统圆锥算法的截断误差进行了分析计算，把C，S，q₀展开到更高阶次： 

$\left\{\begin{array}{c}C=\cos \frac{\left|\mathrm{\Φ}\right|}{2}=1-\frac{{\left|\mathrm{\Φ}\right|}^2}{8}+...\≈1-\frac{{\left|\mathrm{\Φ}\right|}^2}{8}\\ S=\frac{\sin \frac{\left|\mathrm{\Φ}\right|}{2}}{\left|\mathrm{\Φ}\right|}=\frac{\frac{\left|\mathrm{\Φ}\right|}{2}-\frac{1}{3!}\·{\left(\frac{\left|\mathrm{\Φ}\right|}{2}\right)}^3}{\left|\mathrm{\Φ}\right|}+...\≈\frac{1}{2}-\frac{{\left|\mathrm{\Φ}\right|}^2}{48}\\ q_0=1-2{\sin }^2\frac{\mathrm{\α}}{2}{\sin }^2\frac{\mathrm{wh}}{2}\end{array}\right.---\left(21\right)$

而： 

$\left|\mathrm{\Φ}\right|=\sqrt{{{\mathrm{\Φ}}^2}_x+{{\mathrm{\Φ}}^2}_y+{{\mathrm{\Φ}}^2}_z}$

$\≈\sqrt{16{\sin }^2\frac{\mathrm{\α}}{2}{\sin }^2\frac{\mathrm{wh}}{2}+{\sin }^4\frac{\mathrm{\α}}{2}\·[O{\left(\mathrm{wh}\right)}^4+...]}---\left(22\right)$

$\≈4\sin \frac{\mathrm{\α}}{2}\sin \frac{\mathrm{wh}}{2}$

把式(21)、(22)代入式(14)得： 

${\tilde{q}}_1=q_{1}C-S(q_0{\mathrm{\Φ}}_x-q_3{\mathrm{\Φ}}_y+q_2{\mathrm{\Φ}}_z)\≈q_{1}C-{\mathrm{Sq}}_0{\mathrm{\Φ}}_x$

$=({\text{-sin}}^2\frac{\mathrm{\α}}{2}\sin \mathrm{wh}-\frac{1}{2}{\mathrm{\Φ}}_x)+(\frac{{\left|\mathrm{\Φ}\right|}^2}{8}\·{\sin }^2\frac{\mathrm{\α}}{2}\sin \mathrm{wh}+\frac{1}{48}{\left|\mathrm{\Φ}\right|}^2{\mathrm{\Φ}}_x+{\sin }^2\frac{\mathrm{\α}}{2}{\sin }^2\frac{\mathrm{wh}}{2}{\mathrm{\Φ}}_x)$

$={\sin }^2\frac{\mathrm{\α}}{2}\·\frac{{\left(\mathrm{wh}\right)}^7}{102060}+{\sin }^4\frac{\mathrm{\α}}{2}[\frac{1}{30}{\left(\mathrm{wh}\right)}^5-\frac{29}{7290}{\left(\mathrm{wh}\right)}^7+\frac{2339}{10333575}{\left(\mathrm{wh}\right)}^9...]$

--常值漂移-------------截断误差--------- 

(23) 

从式(23)可以看出，一般情况下传统3子样圆锥算法中的截断误差要大于常值漂移误差，是误差的主项，应该优先补偿。而目前现有的改进圆锥算法一般通过增加更新周期内的子样数来提高算法精度。这种方法虽然能够减小其中的常值漂移误差，但对截断误差并没有改善作用。 

为了优先补偿传统算法的截断误差，发明人从Bortz方程的2阶旋转矢量模型着手(即式(4))，在圆锥算法中增加对不可交换性误差的2阶补偿，即： 

$\mathrm{\δ}{\mathrm{\Φ}}^{\′\′}=\frac{1}{12}{\∫}_{t_0}^{t_0+h}\mathrm{\Δ\θ}\×(\mathrm{\Δ\θ}\×w)\mathrm{dt}---\left(24\right)$

把式(7)、(8)代入式(26)得： 

${\mathrm{\δ\Φ}}^{\′\′}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\underset{j=1}{\overset{N-1}{\mathrm{\Σ}}}\underset{k=j+1}{\overset{N}{\mathrm{\Σ}}}{l}_{\mathrm{ijk}}{\mathrm{\Δ\θ}}_i\×({\mathrm{\Δ\θ}}_j\×{\mathrm{\Δ\θ}}_k),N=3---\left(25\right)$

3子样圆锥运动时角增量的采样值为： 

${\mathrm{\Δ\θ}}_i=\left[\begin{array}{c}-\frac{2}{3}\mathrm{wh}{\sin }^2\frac{\mathrm{\α}}{2}\\ -2\sin \mathrm{\α}\sin \left(\frac{\mathrm{wh}}{6}\right)\sin w(t+\frac{2i-1}{6}h)\\ 2\sin \mathrm{\α}\sin \left(\frac{\mathrm{wh}}{6}\right)\cos w(t+\frac{2i-1}{6}h)\end{array}\right]---\left(26\right)$

代入式(25)得： 

${[{\mathrm{\Δ\θ}}_i\×({\mathrm{\Δ\θ}}_j\×{\mathrm{\Δ\θ}}_k)]}_x=-\frac{16}{3}\mathrm{wh}{\sin }^2\left(\frac{\mathrm{\α}}{2}\right){\sin }^2\mathrm{\α}{\sin }^2\left(\frac{\mathrm{wh}}{6}\right)\sin \left(\frac{k-j}{6}\mathrm{wh}\right)\sin \left(\frac{2i-j-k}{6}\mathrm{wh}\right)$

(27) 

圆锥运动的锥半角α一般很小， 对比式(27)和式(23)的截断误差主项可以看出：两者的量级相同，都是 也就是说传统1阶圆锥算法的截断误差正好可以用Bortz方程的2阶不可交换误差补偿模型来消除。 

式(27)的值只与|k-j|和|2i-j-k|有关，共有下列3种组合： 

$\left\{\begin{array}{c}{[{\mathrm{\Δ\θ}}_1\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_2)]}_x=\frac{16}{3}\mathrm{wh}{\sin }^2\left(\frac{\mathrm{\α}}{2}\right){\sin }^2\mathrm{\α}{\sin }^4\left(\frac{\mathrm{wh}}{6}\right)\≈\frac{64}{3}{\sin }^4\left(\frac{\mathrm{\α}}{2}\right)\mathrm{wh}{\sin }^4\left(\frac{\mathrm{wh}}{6}\right)\\ {[{\mathrm{\Δ\θ}}_3\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_2)]}_x=-\frac{16}{3}{\mathrm{wh}\sin }^2\left(\frac{\mathrm{\α}}{2}\right){\sin }^2\mathrm{\α}{\sin }^3\left(\frac{\mathrm{wh}}{6}\right)\sin \left(\frac{1}{2}\mathrm{wh}\right)\≈-\frac{64}{3}{\sin }^4\left(\frac{\mathrm{\α}}{2}\right)\mathrm{wh}{\sin }^3\left(\frac{\mathrm{wh}}{6}\right)\sin \left(\frac{\mathrm{wh}}{2}\right)\\ {[{\mathrm{\Δ\θ}}_1\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3)]}_x=\frac{16}{3}{\mathrm{wh}\sin }^2\left(\frac{\mathrm{\α}}{2}\right){\sin }^2\mathrm{\α}{\sin }^2\left(\frac{\mathrm{wh}}{6}\right){\sin }^2\left(\frac{\mathrm{wh}}{3}\right)\≈\frac{64}{3}{\sin }^4\left(\frac{\mathrm{\α}}{2}\right)\mathrm{wh}{\sin }^2\left(\frac{\mathrm{wh}}{6}\right){\sin }^2\left(\frac{\mathrm{wh}}{3}\right)\end{array}\right.$

(28) 

因此x轴的2阶不可交换误差补偿δΦ″简化形式为： 

δΦ″_x＝l₁Δθ₁×(Δθ₁×Δθ₂)+l₂Δθ₃×(Δθ₁×Δθ₂)+l₃Δθ₁×(Δθ₁×Δθ₃)   (29) 

把式(28)代入式(29)，对(wh)进行泰勒级数展开，合并 同类项： 

${\mathrm{\δ\Φ}}_x^{\′\′}={\sin }^4\frac{\mathrm{\α}}{2}\left\{l_1\right[\frac{4}{243}{\left(\mathrm{wh}\right)}^5-\frac{2}{6521}{\left(\mathrm{wh}\right)}^7+\frac{1}{393660}{\left(\mathrm{wh}\right)}^9]+l_2[-\frac{4}{81}{\left(\mathrm{wh}\right)}^5$ (30) 

$+\frac{2}{729}{\left(\mathrm{wh}\right)}^7-\frac{23}{393660}{\left(\mathrm{wh}\right)}^9]+l_3[\frac{16}{243}{\left(\mathrm{wh}\right)}^5-\frac{20}{6521}{\left(\mathrm{wh}\right)}^7+\frac{2}{32805}{\left(\mathrm{wh}\right)}^9\left]\right\}$

把式(30)与式(23)比较，当下式成立时，误差四元数 最小。 

$\left\{\begin{array}{c}{l}_{1\≈}-\frac{14153}{73}\\ l_2\≈\frac{13742}{75}\\ l_3\≈\frac{11401}{61}\end{array}\right.---\left(31\right)$

即新的2阶3子样圆锥优化算法形式为： 

$\mathrm{\Φ}={\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2+{\mathrm{\Δ\θ}}_3+\frac{9}{20}{\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3+\frac{27}{40}{\mathrm{\Δ\θ}}_2\×({\mathrm{\Δ\θ}}_3-{\mathrm{\Δ\θ}}_1)$ (32) 

$-\frac{14153}{73}{\mathrm{\Δ\θ}}_1\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_2)+\frac{13742}{75}{\mathrm{\Δ\θ}}_3\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_2)+\frac{11401}{61}{\mathrm{\Δ\θ}}_1\×({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3)$

为了验证发明的实用价值。下面对比一下3种不同的圆锥算法的算法误差： 

算法I：传统3子样圆锥优化算法(见式(17))： 

算法I建立在Bortz方程的1阶补偿模型基础上，其单位时间内旋转矢量Φ_x的误差(e₁)包含常值漂移误差项e₁(Φ′)和截断误差e₁(Φ″)： 

$e_1:\left\{\begin{array}{c}{e}_1\left({\mathrm{\Φ}}^{\′}\right)={\sin }^2\frac{\mathrm{\α}}{2}\·\frac{w^7{h}^7}{51030}/h\\ e_1\left({\mathrm{\Φ}}^{\′\′}\right)={\sin }^4\frac{\mathrm{\α}}{2}[\frac{1}{15}{\left(\mathrm{wh}\right)}^5-\frac{29}{3645}{\left(\mathrm{wh}\right)}^7+\frac{37561}{94478400}{\left(\mathrm{wh}\right)}^9...]/h\end{array}\right.---\left(33\right)$

算法II：传统4子样简化圆锥算法(见式(19))： 

算法II也是建立在Bortz方程的1阶补偿模型基础上。因为子样数的提高，单位时间内其算法漂移误差e₂(Φ′)变小，但截断误差e₂(Φ″)不变。 

$e_2:\left\{\begin{array}{c}{e}_2\left({\mathrm{\Φ}}^{\′}\right)=\frac{{{\mathrm{\α}}^{2}w}^9{h}^9}{82575360}/h\\ e_2\left({\mathrm{\Φ}}^{\′\′}\right)={\sin }^4\frac{\mathrm{\α}}{2}[\frac{1}{15}{\left(\mathrm{wh}\right)}^5-\frac{29}{3645}{\left(\mathrm{wh}\right)}^7+\frac{37561}{94478400}{\left(\mathrm{wh}\right)}^9...]/h\end{array}\right.---\left(34\right)$

算法III：本发明的2阶3子样圆锥优化算法见式(32)： 

算法III建立在Bortz方程的2阶不可交换误差补偿模型上，因此对不可交换误差有了更多的补偿。具体表现为单位时间内算法误差(e₃)中的常值漂移误差项(e₃(Φ′)不变，但截断误差项e₃(Φ″)大大减小： 

$e_3:\left\{\begin{array}{c}{e}_3\left({\mathrm{\Φ}}^{\′}\right)={\sin }^2\frac{\mathrm{\α}}{2}\·\frac{w^7{h}^7}{51030}/h\\ e_3\left({\mathrm{\Φ}}^{\′\′}\right)\≈{\sin }^4\frac{\mathrm{\α}}{2}[O{\left(\mathrm{wh}\right)}^{11}+...]/h\end{array}\right.---\left(35\right)$

忽略高次误差项，表1列出了3种算法在单位时间内x轴旋转矢量误差漂移对比e_i： 

表1 

由表1可以看出，一般情况下e₃＜＜e₂＜e₁。例如当α＝1°，w＝2πrad/s，h＝10ms时，e₁＝3.84×10^-14rad/s，e₂＝3.79×10^-14rad/s，e₃＝5.77×10^-16rad/s。即单位时间内算法III的误差比算法I和II减小了近2个数量级。基于2阶补偿模型所新发明的算法明显优于传统的圆锥算法。 

本发明的算法思想同样适用于其它形式的2阶圆锥算法 更高子样的传统角增量圆锥算法以及对角速率陀螺仪的圆锥算法的改进中。以4子样的传统角增量圆锥算法为例，2阶4子样圆锥优化算法形式应为： 

$\mathrm{\Φ}=\mathrm{\Δ\θ}+{\mathrm{\δ\Φ}}^{\′}+{\mathrm{\δ\Φ}}^{\′\′}$

$=\mathrm{\Δ\θ}+\underset{i=1}{\overset{4-1}{\mathrm{\Σ}}}\underset{j=i+1}{\overset{4}{\mathrm{\Σ}}}{K}_{\mathrm{ij}}({\mathrm{\Δ\θ}}_i\×{\mathrm{\Δ\θ}}_j)---\left(36\right)$

$+\underset{i=1}{\overset{4}{\mathrm{\Σ}}}\underset{j=1}{\overset{4-1}{\mathrm{\Σ}}}\underset{k=j+1}{\overset{4}{\mathrm{\Σ}}}{L}_{\mathrm{ijk}}{\mathrm{\Δ\θ}}_i\×({\mathrm{\Δ\θ}}_j\×{\mathrm{\Δ\θ}}_k)$

式(36)中系数K_ij的最优值已有算法给出。类似于式(21)～(32)，即可求出式(36)中L_ijk项的最优值，即新的2阶4子样圆锥优化算法。 

此外当机体同时存在线运动和角运动时，产生的划桨效应会给速度计算带来较大误差。因为划桨效应和圆锥效应存在对偶性，因此划桨算法和圆锥算法也是 对偶的。而传统的划桨算法同样也是基于1阶误差补偿模型。例如传统3子样的划桨算法为： 

${\mathrm{\ΔV}}_{\mathrm{sculm}}=\frac{9}{20}[\mathrm{\Δ\θ}\left(1\right)\×\mathrm{\ΔV}\left(3\right)+\mathrm{\ΔV}\left(1\right)\×\mathrm{\Δ\θ}\left(3\right)]+\frac{27}{40}[\mathrm{\Δ\θ}\left(1\right)\×\mathrm{\ΔV}\left(2\right)$ (37) 

$+\mathrm{\Δ\θ}\left(2\right)\×\mathrm{\ΔV}\left(3\right)+\mathrm{\ΔV}\left(1\right)\×\mathrm{\Δ\θ}\left(2\right)+\mathrm{\ΔV}\left(2\right)\×\mathrm{\Δ\θ}\left(3\right)]$

对比式(37)和式(17)可以发现，传统3子样划桨算法与传统3子样圆锥算法是对偶的。它和传统的圆锥算法一样同样存在误差补偿不够充分问题，本发明的思想对于传统划桨算法的改进同样适用。 

综上，本发明在Bortz方程的2阶不可交换误差补偿模型基础上，增加角增量的2次叉乘项 快速准确地求解机体的姿态信息，从而更好地补偿圆锥运动下的不可交换性误差，而不需要增加采样频率和算法更新周期内的子样数，算法简单，且精度提高。 

附图说明

图1是刚体转动的不可交换性误差示意图； 

图2是圆锥运动示意图； 

图3是本发明的原理示意图。 

具体实施方式

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

如图3所示，本发明提供一种基于2阶不可交换误差补偿模型的圆锥算法，应用于捷联系统求姿算法，其具体步骤为： 

一、在单位算法更新周期内，对角增量陀螺仪的输出采样3次，分别得到采样值为：Δθ₁，Δθ₂，Δθ₃； 

二、为简化计算，先计算出角增量的各2次叉乘积。 

X₁＝Δθ₁×Δθ₃，X₂＝Δθ₁×Δθ₂，X₃＝Δθ₂×Δθ₃

三、把X₁，X₂，X₃的值代入到新的2阶圆锥算法公式(32)中，求取旋转矢 量Φ。具体方法如下： 

$\mathrm{\Φ}={\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2+{\mathrm{\Δ\θ}}_3+\frac{9}{20}{X}_1+\frac{27}{40}(X_2+X_3)+{\mathrm{\Δ\θ}}_1\×(\frac{11401}{61}{X}_1-\frac{14153}{73}{X}_2)+\frac{13742}{75}({\mathrm{\Δ\θ}}_3\×X_2)$

四、求得的旋转矢量Φ已包含对不可交换误差的1阶补偿和2阶补偿，把它代入四元数更新方程(式(1))中，代替其中的角增量Δθ，即可求出准确的旋转四元数。 

五、计算旋转四元数对应的姿态阵和姿态角，完成捷联求姿过程。 

需要说明的是，本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有理论和技术。以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。 

Claims (2)

1.一种基于2阶不可交换误差补偿模型的圆锥补偿方法，其特征在于包括如下步骤：

（1）在单位算法更新周期内，对陀螺仪的输出采样3次得到采样值Δθ₁,Δθ₂,Δθ₃；

（2）根据下式求出整个算法更新周期内的角增量Δθ：

Δθ=Δθ₁+Δθ₂+Δθ₃   (1)

（3）计算采样值的各1次叉乘项和各2次叉乘项，其中，采样值的1次叉乘项包含以下3项：

Δθ₁×Δθ₃,Δθ₁×Δθ₂,Δθ₂×Δθ₃   (2)

采样值的2次叉乘项包含以下3项：

Δθ₁×(Δθ₁×Δθ₂),Δθ₃×(Δθ₁×Δθ₂),Δθ₁×(Δθ₁×Δθ₃)   (3)

（4）从陀螺仪的3次采样值出发，利用Bortz方程的旋转矢量1阶不可交换误差补偿模型对角增量Δθ进行1阶补偿：

所述Bortz方程的旋转矢量1阶不可交换误差补偿模型为：

$\stackrel{\·}{\mathrm{\Φ}}=\mathrm{\ω}+\frac{1}{2}\mathrm{\Φ}\×\mathrm{\ω}---\left(4\right)$

其中的Φ代表更新周期内的旋转矢量，ω代表机体的角速度，两边积分得：

$\mathrm{\Φ}=\mathrm{\Δ\θ}+\frac{1}{2}{\∫}_t^{t+h}\mathrm{\Φ}\×\mathrm{\ωdt}---\left(5\right)$

其中，h表示更新周期；

式(5)通过变换最终得到下式，其中δΦ′代表1阶不可交换误差补偿：

$\left\{\begin{array}{c}\mathrm{\Φ}=\mathrm{\Δ\θ}+\mathrm{\δ}{\mathrm{\Φ}}^{\′}\\ \mathrm{\δ}{\mathrm{\Φ}}^{\′}=\frac{9}{20}\mathrm{\Δ}{\mathrm{\θ}}_1\×\mathrm{\Δ}{\mathrm{\θ}}_3+\frac{27}{40}\mathrm{\Δ}{\mathrm{\θ}}_2\×(\mathrm{\Δ}{\mathrm{\θ}}_3-\mathrm{\Δ}{\mathrm{\θ}}_1)\end{array}\right.---\left(6\right)$

（5）利用Bortz方程的2阶不可交换误差补偿模型对步骤(4)求得的旋转矢量再进行2阶不可交换误差补偿：

所述Bortz方程的2阶不可交换误差补偿模型为：

$\stackrel{\·}{\mathrm{\Φ}}=\mathrm{\ω}+\frac{1}{2}\mathrm{\Φ}\×\mathrm{\ω}+\frac{1}{12}\mathrm{\Φ}\×(\mathrm{\Φ}\×\mathrm{\ω})...---\left(7\right)$

根据该模型进一步补偿不可交换性误差的方法为：

$\left\{\begin{array}{c}\mathrm{\Φ}=\mathrm{\Δ\θ}+\mathrm{\δ}{\mathrm{\Φ}}^{\′}+\mathrm{\δ}{\mathrm{\Φ}}^{\′\′}\\ \mathrm{\δ}{\mathrm{\Φ}}^{\′\′}=l_1\mathrm{\Δ}{\mathrm{\θ}}_1\×(\mathrm{\Δ}{\mathrm{\θ}}_1\×\mathrm{\Δ}{\mathrm{\θ}}_2)+l_2\mathrm{\Δ}{\mathrm{\θ}}_3\×(\mathrm{\Δ}{\mathrm{\θ}}_1\×\mathrm{\Δ}{\mathrm{\θ}}_2)+l_3\mathrm{\Δ}{\mathrm{\θ}}_1\×(\mathrm{\Δ}{\mathrm{\θ}}_1\×\mathrm{\Δ}{\mathrm{\θ}}_3)\end{array}\right.---\left(8\right)$

其中，δΦ′′即是圆锥算法中的旋转矢量对不可交换性误差的2阶补偿，l₁、l₂、l₃分别表示2阶补偿中各叉乘项的系数；

（6）把经1阶补偿和2阶补偿后的旋转矢量Φ代替角增量，代入捷联求姿算法，求取机体的飞行姿态。

2.如权利要求1所述的基于2阶不可交换误差补偿模型的圆锥补偿方法，其特征在于：所述步骤（5）中，对于角增量陀螺，l₁、l₂、l₃的取值分别 $l_1=-\frac{14153}{73},l_2=\frac{13742}{75},l_3=\frac{11401}{61}.$