基于高阶多项式迭代的捷联姿态更新优化方法

刘锡祥，赵苗苗，张玉鹏，郭小乐，王 磊

（1. 东南大学 仪器科学与工程学院，南京 210096；2. 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；3. 浙江大学光电科学与工程学院，杭州 310027）

在捷联惯导系统中，姿态更新算法的优劣直接影响系统的导航精度，而在姿态解算中不可避免地存在不可交换性误差（又称圆锥误差），多年来学者们对其进行了广泛而深入的研究[1-3]。

文献[4]通过引入同步旋转坐标系提出了新姿态解算算法，仿真验证该算法可以更精确真实地模拟姿态更新。文献[5-6]考虑了Bortz 方程的三叉积项，在旋转矢量的四阶毕卡分量解的基础上提出了改进算法，但是其推导过程比较繁琐。文献[7]推导了一种新的系数优化算法，在文献[5]基础上简化了其高阶误差补偿算法的推导过程。文献[8]结合角速度拟合多项式和姿态四元数高阶泰勒级数展开，提出了一种新的四子样姿态算法，提高了姿态解算的精度。文献[9]利用切比雪夫多项式拟合角速度矢量，通过迭代求得罗德里格参数微分方程的精确解。文献[10]提出了基于多项式迭代的旋转矢量微分方程数值算法（RVPI），通过角速度拟合多项式迭代求解Bortz 方程，避免Bortz方程求解中的近似误差，取得了较好效果。

上述姿态更新算法基本上都是基于角速度多项式运动模型直接或间接补偿不可交换误差，提高算法精度，然而角速度拟合精度仍是制约这些算法精度的重要因素。目前，增加角速度的多项式拟合精度方法有增加子样数、利用前周期的采样角增量提高角速度多项式拟合阶次、增加其它约束构建高阶角速度多项式等，文献[11]是在子样数确定的条件下，通过利用前一个或多个周期的采样信息来构建更高阶子样算法，有效提高了精度。而本文则是通过引入圆锥约束增大了拟合阶次，从而提高了算法精度。

本文在文献[10]基础上，利用圆锥环境下角速度矢量的各阶导数在圆锥轴上数值之间的解析关系，建立高一阶或高二阶近似模型，从而实现对传统角速度拟合多项式的改进。该算法在确定的采样频率下，无需增加陀螺仪输出的角增量数量便可以获得拟合角速度的高阶多项式，为等效旋转矢量微分方程提供更精确的角速度矢量输入，有效地实现姿态更新。

1 多项式方法及多项式迭代方法

1.1 多项式描述

实际惯导系统中，陀螺输出的信息为角增量，而姿态更新算法常常采用角速度作为输入，因此需要给出由角增量转化为角速度的方法。令陀螺角增量的采样周期为h0，在时间段 [-ph0,nh0]内进行了N次采样（p,n均为整数，p≥ 0,n＞0且p+n=N），角增量分别记为Δθj（j=-p+ 1, -p+ 2, … ,n），可以得到角速度ω为时间t的有限次（N- 1）拟合多项式，表示为

式中，

1.2 多项式迭代

等效旋转矢量微分方程（Bortz 方程）为

式中，Φ为等效旋转矢量，。

将式(2)中第三项的系数由泰勒级数展开后得

将式(3)代入到式(2)，并对其进行积分，再改成迭代形式[10]，整理后得

式中，Φ(0)=0，Φ(i)(i=0,1,2, …) 表示第i次迭代的等效旋转矢量。

式(4)即为求解等效旋转矢量的主要公式，接下来分别对式中积分进行求解。

1) 将式(1)代入到式(4)的积分项中，进一步计算得：

式中，运算符“*”表示两个多项式系数向量之间的卷积运算。

2) 对式(4)第三个积分项中的系数进行求解，其表达式为

2 高阶改进多项式

文献[10]基于多项式迭代的算法有效避免了Bortz方程的原理性近似误差，而多项式迭代算法的精度主要取决于角速度多项式的拟合精度。角速度拟合多项式的阶次越高，反映运载体的角运动的准确度也越高。在确定的姿态更新周期和采样频率下，相关学者[11]利用前周期和本周期的角增量采样值进行了角速度的多项式拟合，提高了算法的子样阶次；本文基于文献[10]，在子样数n不变的情况下，通过引入圆锥运动增加约束条件，提高了拟合阶次，使得拟合角速度的多项式阶次由n-1 增大到n或n+1 ，从而提高了多项式迭代算法精度。

2.1 圆锥运动下角速度各系数之间的关系

圆锥运动的更新四元数和角速度矢量分别表示为：

由于任意曲线都可以由多项式拟合得到，角速度矢量由拟合多项式[12]表示为

对上式求导，并令其在t=0 时取值，于是有

在经典圆锥运动环境中，捷联姿态计算的漂移误差主要体现在圆锥轴（x 轴），而周期项误差主要体现在y 轴和z 轴上。因此，角速度在x 轴方向上存在的偏差对该漂移误差产生了主要影响，接下来主要对角速度导数ω(i)的x 轴方向上的值进行讨论分析。令ω(i)(0)和ω(j)(0)叉乘（i≠j），可以得到

比较上述各式，可以发现以下规律：

式中，l=-1 或 1。其中，当i为偶数、j为奇数时，l= （-1）(i+j+3)/2； 当i为奇数、j为偶数时，l= （-1）(i+j+1)/2。

2.2 多项式改进

传统算法中，角速度拟合多项式的阶次为n-1 ，n为子样数。本文提出一种改进型算法，其角速度的拟合多项式阶次为n或n+1 。下面以三子样算法为例进行说明。

传统三子样算法是在对［t k,tk+1］时间段内的运载体角速度采用抛物线拟合推导得到的。h为姿态更新周期，h=tk+1-tk。角速度ω的表达式为

为了用角增量表示上式中的参数c0、c1、c2，记

即

对上述方程组进行求解，得到角速度各系数的表达式为

改进型三子样算法是在对［t k,tk+1］时间段内的运载体角速度采用三阶多项式拟合推导得到的。角速度ω'表达式为

接下来求解ω'各系数，在求解过程中，将看成未知量，求解步骤和传统三子样算法一样。因此得到

对比式(17)和式(19)，可以得到

由2.1 节式(13)总结出来的规律可以得到

只考虑多项式系数之间的叉积在圆锥轴上的关系，可近似得到

将式(20)代入式(21)，得

式(22)中的高阶项影响较小，可忽略不计。于是有

即

式中，“+”代表矩阵的广义逆。

当子样数n= 4时，由2.1 节式(13)总结出来的规律可以得到因此四阶拟合多项式不能通过此规律获得，转为利用五阶拟合多项式，多项式系数可由分别与的比例关系获得；当子样数n= 5时，多项式系数可通过与的比例关系获得。表1 列出了3 子样、4 子样、5 子样算法的角速度高阶拟合多项式的表达式。由此类推，即可获得任意n子样的n或n+1 阶拟合多项式，当子样n为奇数，阶数为n；当子样n为偶数，阶数为n+1 。

表1 3～5 子样高阶拟合多项式Tab.1 Higher-order fitting polynomials with 3 ～ 5 subsamples

3 仿真与分析

以圆锥运动和多项式角运动两种环境作为测试输入，利用MATLAB 对算法的漂移误差进行仿真。仿真对比了三种不同的算法，分别为改进算法（New）、传统优化圆锥算法（Classical）与文献[10]提出的高阶旋转矢量微分方程精确数值算法（RVPI）。

3.1 圆锥运动仿真

圆锥运动仿真参数设置为：圆锥频率f=1 Hz，子样数n=4 ，采样时间为0.01 s，仿真时长为3 s，半锥角α=10°。仿真结果如图1 所示。

图1 圆锥误差漂移对比Fig.1 Comparison of coning error drift

由图1 可知，三种算法在锥轴上的漂移误差随时间逐渐变大，在非锥轴上的误差呈周期性变化。传统算法的漂移误差远大于其它两种算法，因此主要对RVPI 算法和改进算法进行对比分析。RVPI 算法在锥轴上波动较大，最大达到了 7.62 ×10-5''，其在非锥轴上漂移误差较小，最大达到了 1.55 ×10-6''；而改进算法的三轴误差始终都很小，将锥轴上误差图放大后仍然观察不出其量级，因此取t=3 s 时δφx的值，得到其在锥轴上最大误差为 8.71 ×10-7''，而其在非锥轴上的误差值近似为0。由此验证了改进算法的精度，且在相同条件下，改进算法要优于传统算法和RPVI 算法。

令其它参数不变，分别取半锥角参数为0～90°进行仿真，对比三种算法前3s 随半锥角变化情况的漂移误差，结果如图2 所示。

图2 三种算法漂移对比（n = 4）Fig.2 Comparison of three algorithms drift (n = 4)

由图可以看出，RVPI 与改进算法具有相同的漂移趋势，但在整个半锥角范围内，改进算法精度均优于RVPI 算法；但与传统算法相比，改进算法与RVPI 具有相同的特点，即当半锥角较小时，传统算法精度较高；随着半锥角的增大，改进算法与RVPI 的优势逐渐显现。

为进一步探究改进算法的解算精度和稳定性，给出算法在3 子样、5 子样条件下随半锥角变化的漂移误差。

图3 三种算法漂移对比（n = 3）Fig.3 Comparison of three algorithms drift (n = 3)

图4 三种算法漂移对比（n = 5）Fig.4 Comparison of three algorithms drift (n = 5)

对图2、图3、图4 进行对比分析，改进算法始终都比RVPI 的漂移误差更小；但在半锥角较小时，传统算法精度较高。综合图2～4 中RVPI 和改进算法与传统算法的交汇点可以发现，改进算法随着子样数的提高，交汇点逐渐向小锥角角度方向移动。改进算法在n=3、4、5 时的交汇点半锥角分别约为：10 °，0.2 °，0.03 °；而RVPI 的交汇点分别约为：90 °，2 °，0.6 °。这表明，相较于RVPI，改进算法具有一定的优势。

3.2 多项式角运动仿真

参考文献[13]中的大角速率机动仿真环境设置，给出以多项式表示的运载体角速度为

设定陀螺仪采样间隔为0.01s，仿真时长为1s，子样数n=4 。各算法的角度误差漂移如图5 所示。

图5 大角速率机动误差漂移对比Fig.5 Comparison of large angular rate error drift

由图5 可以看出，传统算法的精度远低于另外两种算法，而RVPI 和改进算法的对比不太明显，故只考虑RVPI 和改进算法，基于3～5 子样分别对其进行了仿真分析，如图6～8 所示。

分析图6～8 可以发现，在子样n=3 和n=4 两种情况下，RVPI 和改进算法两者误差均处于同一量级，三轴误差大小各有不同，精度近似相当；而在n=5 时，改进算法整体漂移量小于RVPI，且误差振荡幅值较小。这表明，改进算法引入的约束条件未降低原有RVPI 在多项式运动条件下的精度。

图6 RVPI、改进算法误差对比（n = 3）Fig.6 Comparison of error drift between RVPI and New algorithm (n = 3)

图7 RVPI、改进算法误差对比（n = 4）Fig.7 Comparison of error drift between RVPI and New algorithm (n = 4)

图8 RVPI、改进算法误差对比（n = 5）Fig.8 Comparison of error drift between RVPI and New algorithm (n = 5)

4 结 论

经过对文献[10]进行的分析，本文认为其在解决Bortz 方程存在的原理性近似误差后，角速度多项式的拟合精度成为制约导航解算精度的关键。本文在文献[10]算法基础上，进一步提出了角速度拟合多项式的改进算法。

引入圆锥运动增加约束，使得n个陀螺样本拟合的多项式阶数由n-1 增加到n或n+1 ；在理论分析基础上，分别模拟仿真了多项式角运动和不同半锥角条件下的圆锥运动，通过求解等效旋转矢量微分方程验证并对比了改进算法、传统优化圆锥算法和高阶旋转矢量微分方程数值算法（RVPI）。

试验表明，由于角速度拟合精度的提高，改进算法在高动态环境下的性能优于RVPI 算法；在多项式角运动下与RVPI 算法精度相当；且随着子样数增大，改进算法更具精度优势。