一种新的圆锥误差优化补偿算法

A New Optimized Compensation Algorithm of Conic Error

摘要: 为了解决捷联惯导系统中存在的不可交换误差的问题，本文提出了一种改进的圆锥误差优化算法来提高解算精度。该算法对重叠式圆锥误差优化算法进行了进一步优化。首先对经典圆锥运动建立误差准则，其次推导出优化的补偿算法的公式，给出了优化补偿系数，最后，在某一圆锥运动环境下对改进的算法进行仿真。结果表明，改进的算法得到的姿态解算精度与传统算法和重叠式算法相比，都得到了一定的提高，并且与二子样和三子样相比，四子样的解算精度效果最好。

Abstract: In order to solve the noncommutative error existing in the strap down inertial navigation system, this paper presents an improved coning error optimization algorithm to improve the solution ac-curacy. The algorithm optimizes the overlapped optimization algorithm further. Firstly, the error criterion of the classical coning motion is established; secondly, the optimized formula of the compensation algorithm is deduced and the optimal compensation coefficient is obtained; finally, the optimized algorithm under different coning motion environment is simulated. The results show that attitude calculation accuracy through improved algorithm is better than the traditional algorithm and overlapping algorithm; what’s more, the calculation accuracy of four-sample is better than the two-sample and three-sample.

1. 引言

在捷联惯导系统中，姿态解算在整个惯性导航系统中是非常重要的 [1] 。在载体上固联着的导航系统，可以直接敏感载体的运动 [2] ，因此，姿态实时解算和更新是捷联惯导的尤为关键的技术之一。在载体运动过程中，刚体的有限转动会产生不可交换误差。锥运动是捷联惯导系统最恶劣的工作环境，这一不可交换误差作为整个姿态解算过程中的主要误差源，很多学者都用对这一误差优化的结果来评价一个姿态更新算法的优劣。由此可见，有效抑制圆锥误差对提高导航精度有显著的意义。

目前，大量国内外学者都对圆锥误差的优化算法这一领域进行了深入研究：等效旋转矢量的概念由外国学者Bortz和Jordon提出，这一概念的提出为之后研究的大量关于姿态更新的多子样算法提供了强有力的理论依据 [3] [4] ；秦永元等人在旋转矢量法的基础上,推导出了二子样到四子样等多子样算法的公式 [5] ；陈建锋等 [6] 提出一种基于角速率输入的进一步优化的补偿算法；丁杨斌通过已经得到的当前时刻之前的两个周期的姿态数据对结果进行优化修正 [7] ；肖胜从运动学角度给出了圆锥误差的诠释，提出了流水式旋转矢量姿态更新算法 [8] ；王真等主要针对三子样，提出了二次迭代优化算法 [9] ；杨浩天利用前两个计算周期陀螺输出的角增量信息和当前时刻陀螺采样值通过重叠式采样的方式进行圆锥误差补偿 [10] 。

目前研究出的的圆锥补偿算已经比较成熟，但是还存在一些问题，如在相同的采样频率下，增加子样数，会降低导航系统的姿态更新频率，但同时会增加导航计算机的计算量，给硬件造成一定的负担。

本文在抑制圆锥误差的传统算法的基础上，提出一种改进的补偿算法，即对重叠式采样算法的周期项进行优化，分别对二子样，三子样和四子样进行仿真，验证所提算法的正确性。

2. 圆锥运动与圆锥误差

圆锥效应是刚体运动中一种比较常见的现象。若某一载体作锥运动时，此时的不可交换误差所反映出的情况最为恶劣。

模拟载体角运动时，我们通常用锥运动来进行建模。用如下的单位矢量表示：

$a\left(t\right)/a={\left[\cos \omega t\sin \omega t0\right]}^{\text{T}}$ (1)

其中， $\omega =2\text{π}f$ ， $f$ 为锥运动频率，半锥角 $a={\left(a\cdot a\right)}^{1/2}$ 。

在我们所表示的经典圆锥运动的环境下，表示坐标系相对运动的姿态四元数 $q\left(t\right)$ 和角速率 $\omega \left(t\right)$ 通常可以用下列公式来表示：

$q\left(t\right)=\left[\cos \left(a/2\right)\sin \frac{a}{2}\cos \omega t\sin \frac{a}{2}\sin \omega t0\right]$ (2)

$\omega \left(t\right)=\left[-\omega \sin a\sin \gamma t\omega \sin a\cos \omega t-2\omega {\sin }^2\frac{a}{2}\right]$ (3)

3. 优化算法的误差准则

载体运动过程中的姿态四元数，通常以如下形式进行姿态更新

$q\left(t+\Delta t\right)=q\left(t\right)\circ q\left(\Delta t\right)$ (4)

其中， $q\left(t+\Delta t\right)$ 为 $t+\Delta t$ 时刻的姿态四元数， $q\left(t\right)$ 为 $t$ 时刻的姿态四元数， $q\left(\Delta t\right)$ 为这一时间间隔 $\left[t,t+\Delta t\right]$ 内姿态四元数的变化量，可以由下式得到：

$q\left(\Delta t\right)={\left[\cos \frac{\Delta \sigma }{2}\frac{\Delta \sigma }{\Delta \sigma }\sin \frac{\Delta \sigma }{2}\right]}^{\text{T}}$ (5)

这个公式中， $\Delta \sigma$ 为这一时间间隔 $\Delta t$ 内旋转矢量的变化量，其大小为 $\Delta \sigma ={\left(\Delta \sigma \cdot \Delta \sigma \right)}^{1/2}$ 。

在这一时间间隔 $\left[t,t+\Delta t\right]$ 内，对Bortz方程积分并取近似，可以得到 $\Delta \sigma$ 的理想值 $\Delta \bar{\sigma }$ ：

$\Delta \bar{\sigma }\approx \theta +\frac{1}{2}{\displaystyle {\int }_t^{t+\Delta t}\theta \left(\tau \right)}\times \omega \text{d}\tau$ (6)

经典圆锥运动下， $\Delta \bar{\sigma }$ 可以表示为：

$\Delta \bar{\sigma }\text{=}\left[\begin{array}{l}\Delta {\bar{\sigma }}_x\\ \Delta {\bar{\sigma }}_y\\ \Delta {\bar{\sigma }}_z\end{array}\right]=\left[\begin{array}{c}-2\left(\sin \frac{\omega h}{2}+\left(-\omega h\cos \frac{\omega h}{2}+2\sin \frac{\omega h}{2}\right){\sin }^2\frac{a}{2}\right)\sin a\sin \omega \left(t+\frac{h}{2}\right)\\ 2\left(\sin \frac{\omega h}{2}+\left(-\omega h\cos \frac{\omega h}{2}+2\sin \frac{\omega h}{2}\right){\sin }^2\frac{a}{2}\right)\sin a\cos \omega \left(t+\frac{h}{2}\right)\\ -2\omega h{\sin }^2\frac{a}{2}+\frac{1}{2}{\sin }^{2}a\left(\omega h-\sin \omega h\right)\end{array}\right]$ (7)

若将时间间隔 $\left[t,t+\Delta t\right]$ 分为N个宽度，则可以得到N个角增量信号，传统圆锥算法以及改进的重叠式优化算法针对的都是非周期项，本文在此基础上，提出一种基于重叠式采样的二次优化算法。

假设旋转矢量的变化量为：

$\begin{array}{l}\Delta \sigma ={\left[\Delta {\sigma }_x\Delta {\sigma }_y\Delta {\sigma }_z\right]}^{\text{T}}\\ ={\displaystyle \underset{i=1}{\overset{N}{\sum }}{\theta }_i}+{\displaystyle \underset{i=1}{\overset{N-1}{\sum }}{K}_i}\left({\theta }_i\times {\theta }_N\right)+G\left({\theta }^{\prime }\times \theta \right)\end{array}$ (8)

其中， $K_i=diag\left[{\kappa }_i{\lambda }_i{\lambda }_i\right]$ ， ${\kappa }_i$ 为重叠式算法的优化系数， ${\lambda }_i$ 为本文算法的优化补偿系数。

${\theta }_i\times {\theta }_N=\left[\begin{array}{c}-8\frac{\omega h}{N}{\sin }^2\frac{a}{2}\sin a\sin \frac{\omega h}{2N}\sin \frac{N-i}{2N}\omega h\sin \omega \left(t+\frac{N+i-1}{2N}h\right)\\ 8\frac{\omega h}{N}{\sin }^2\frac{a}{2}\sin a\sin \frac{\omega h}{2N}\sin \frac{N-i}{2N}\omega h\cos \omega \left(t+\frac{N+i-1}{2N}h\right)\\ 4{\sin }^{2}a{\sin }^2\frac{\omega h}{2N}\sin \omega \left(N-i\right)\frac{h}{N}\end{array}\right]$ (9)

${\displaystyle \underset{i=1}{\overset{N}{\sum }}{\theta }_i}=\left[\begin{array}{c}-2\sin a\sin \frac{\omega h}{2}\sin \omega \left(t+\frac{h}{2}\right)\\ 2\sin a\sin \frac{\omega h}{2}\cos \omega \left(t+\frac{h}{2}\right)\\ -2\omega h{\sin }^2\frac{a}{2}\end{array}\right]$ (10)

${\theta }^{\prime }\times \theta =\left[\begin{array}{c}-8\omega h{\sin }^2\frac{a}{2}\sin a{\sin }^2\frac{\omega h}{2}\sin \omega t\\ 8\omega h{\sin }^2\frac{a}{2}\sin a{\sin }^2\frac{\omega h}{2}\cos \omega t\\ 4{\sin }^{2}a{\sin }^2\frac{\omega h}{2}\sin \omega h\end{array}\right]$ (11)

具体的二次优化的误差准则定义为：

$\epsilon =\left|\Delta {\bar{\sigma }}_x-\Delta {\sigma }_x\right|$ (12)

将(9) (10) (11)代入(8)，并将(7) (8)代入(12)得：

$\begin{array}{c}\epsilon =A|\left(-\omega h\cos \frac{\omega h}{2}+2\sin \frac{\omega h}{2}\right)\sin \omega \left(t+\frac{h}{2}\right)\\ -4\frac{\omega h}{N}\sin \frac{\omega h}{2N}{\displaystyle \underset{i=1}{\overset{N-1}{\sum }}{\lambda }_i}\sin \frac{N-i}{2N}\omega h\sin \omega \left(t+\frac{N+i-1}{2N}h\right)\\ -4G\omega h{\sin }^2\frac{\omega h}{2}\sin \omega t|\end{array}$ (13)

其中，

$A=2{\sin }^2\frac{a}{2}\sin a$ (14)

4. 确定优化算法的系数

当 ${\lambda }_i={\kappa }_i$ 时，式(8)是未进行优化的重叠式算法的表达式。对重叠式算法进行优化后的系数如表1所示。

本文所提出的新算法的补偿系数可以通过以下步骤来确定：

1) 将式(13)中的正弦函数全都展开成关于 $\omega \left(t+\frac{h}{2}\right)$ 的三角函数，并对同类项进行合并；

2) 令关于 $\omega \left(t+\frac{h}{2}\right)$ 的余弦函数项的系数为0，得到：

Table 1. Optimized coefficient of overlapping algorithm

表1. 重叠式算法的优化系数

$4\frac{\omega h}{N}\sin \frac{\omega h}{2N}{\displaystyle \underset{i=1}{\overset{N-1}{\sum }}{\lambda }_i}\sin \frac{N-i}{2N}\omega h\sin \frac{i-1}{2N}\omega h+4G\omega h{\sin }^2\frac{\omega h}{2}\sin \frac{\omega h}{2}=0$ (15)

3) 对下述公式中关于 $\omega \left(t+\frac{h}{2}\right)$ 的正弦函数项的系数X进行幂级数展开，即：

$\begin{array}{c}X=-\omega h\cos \frac{\omega h}{2}+2\sin \frac{\omega h}{2}-4\frac{\omega h}{N}\sin \frac{\omega h}{2N}{\displaystyle \underset{i=1}{\overset{N-1}{\sum }}{\lambda }_i}\cos \frac{i-1}{2N}\omega h\sin \frac{N-i}{2N}\omega h\\ -4G\omega h{\sin }^2\frac{\omega h}{2}\cos \frac{\omega h}{2}\\ =\left[\frac{1}{12}{\left(\omega h\right)}^3-\frac{1}{480}{\left(\omega h\right)}^5+\cdot \cdot \cdot \right]\\ -{\displaystyle \underset{i=1}{\overset{N-1}{\sum }}{\lambda }_i}\left(N-i\right)\left[{\left(\frac{\omega h}{N}\right)}^3-\frac{1+{\left(N-i\right)}^2+3{\left(i-1\right)}^2}{24}{\left(\frac{\omega h}{N}\right)}^5+\cdot \cdot \cdot \right]\\ -\left[G{\left(\omega h\right)}^3-G\frac{5}{24}{\left(\omega h\right)}^5+\cdot \cdot \cdot \right]\end{array}$ (16)

4) 对上述公式中的同类项进行合并，并且令与 ${\lambda }_i$ 有关且阶次不超过(2N − 3)的各项的系数均为0，即：

$\{\begin{array}{l}\frac{1}{12}-{\displaystyle \underset{i=1}{\overset{N-1}{\sum }}{\lambda }_i\frac{N-i}{N^3}-G=0}\\ \frac{1}{480}-{\displaystyle \underset{i=1}{\overset{N-1}{\sum }}{\lambda }_i\frac{\left(N-i\right)\left[1+{\left(N-i\right)}^2+3{\left(i-1\right)}^2\right]}{24N^5}-\frac{5}{24}G=0}\\ ⋮\end{array}$ (17)

5) 根据式(15) (17)确定二次优化补偿系数 ${\lambda }_i$ 。

通过计算，将N = 2 − 4时的优化结果列于表2。

5. 仿真验证

假设某一载体运动的锥运动频率 $f=2\text{Hz}$ ，半锥角 $a=1\mathrm{deg}$ ，算法更新周期 $h=0.01\text{s}$ ，仿真时间为 $t=30N\text{s}$ ， $N=2,3,4$ 。

在这一运动环境下，对传统算法、重叠式优化算法以及本文所提出的优化算法的二子样(图1，图2)、三子样(图3，图4)、四子样(图5，图6)进行仿真，结果如图1~图6所示。

6. 结论

从图1~图6可以看出，传统算法随着子样数N的增加，姿态误差不仅没有提高，反而出现停滞甚至下降的趋势。本文所提出的算法和重叠式算法相比，对于二子样而言，两者的优化系数相同，因此两者的二子样的姿态误差相同，但比传统的算法误差要小很多。除了二子样外，三子样和四子样的仿真结果

Table 2. Optimized compensation coefficient of algorithm in this paper

表2. 本文算法的优化补偿系数

Figure 1. Traditional algorithm attitude error of the two-sample

图1. 传统的二子样算法姿态误差

Figure 2. Attitude error contrast of overlapping algorithm and algorithm of the two-sample in this paper

图2. 重叠式算法与本文算法的二子样姿态误差对比

Figure 3. Traditional algorithm attitude error of the three-sample

图3. 传统的三子样算法姿态误差

Figure 4. Attitude error contrast of overlapping algorithm and algorithm of the three-sample in this paper

图4. 重叠式算法与本文算法的三子样姿态误差对比

Figure 5. Traditional algorithm attitude error of the four-sample

图5. 传统的四子样算法姿态误差

Figure 6. Attitude error contrast of overlapping algorithm and algorithm of the four-sample in this paper

图6. 重叠式算法与本文算法的四子样姿态误差对比

都表明，本文所提出的算法要优于重叠式优化算法和传统算法，尤其是四子样算法，姿态误差比相应的传统算法和重叠式算法优化了1~2个数量级。