与平台式惯导系统不同, 捷联惯导系统通过陀螺仪测量载体角速度构建相对于导航坐标系的数学平台实现导航系下速度位置计算[1], 所以姿态更新是导航解算的基础和关键。姿态变化与3个轴的转动顺序有关, 使用陀螺输出的角速度或角增量进行姿态解算时存在不可交换误差。为减小不可交换误差的影响, 通常根据陀螺输出角增量求解等效旋转矢量, 再根据等效旋转矢量更新姿态矩阵或姿态四元数[2]。利用等效旋转矢量补偿不可交换误差的理论基础是Bortz给出的等效旋转矢量微分方程, 即Bortz方程[3]
|
(1) |
式中: φ表示等效旋转矢量; ω表示角速度; φ=‖φ‖。忽略(1)式中第三项, 进行Picard迭代积分可得时间t内的等效旋转矢量为
|
(2) |
式中，Δθ为[0, t]内角增量, 等式右边第二项为不可交换误差补偿项。
对不可交换误差补偿项的估计是捷联惯导算法研究的重要内容。Miller[4]把等效旋转矢量做三阶泰勒展开计算误差补偿系数, 然后在圆锥运动下分析等效旋转矢量估计误差, 得到圆锥误差补偿系数。Ignagni[5-6]给出了圆锥校正算法, 并且推导了锥运动下角增量叉乘的相关性质。宋敏[7]在圆锥补偿中建立不可交换误差各阶导数与角增量叉乘间的关系, 提出了扩展圆锥误差补偿算法, 充分利用更新周期内角增量信息, 提升圆锥误差补偿精度。王茂松等[8-9]根据Bortz方程, 考虑方程中二次项的影响, 推导了锥运动下角增量三次叉乘补偿项和四次叉乘补偿项。以上捷联惯导姿态更新算法都是在圆锥运动条件下推导角增量叉乘的圆锥误差补偿系数, 除此之外还可以从等效旋转矢量微分方程数值算法考虑, 研究高精度姿态微分方程数值解的计算方法。通常将载体角运动表示成多项式形式, 根据角增量采样值还原多项式系数, 再利用角速率多项式求解微分方程。严恭敏等[10]根据四元数微分方程利用Picard级数解法计算姿态更新四元数, 在大锥角条件时算法精度优于传统补偿算法。武元新等[11-12]给出了基于方程迭代计算的四元数微分方程和罗德里格参数微分方程数值解。将角速度视为多项式形式, 姿态微分方程采用级数展开或者函数迭代的数值方法都可以得到精确的数值解, 在计算过程中不直接使用角增量, 而是还原出角速度多项式代入微分方程的高阶数值解中。当级数展开次数和函数迭代次数足够高时, 算法误差来自角增量拟合多项式过程的误差。
Legendre多项式是正交多项式, 所以在数值计算中表现出更好的特性。利用Legendre多项式进行函数逼近, 效果会优于一般的非正交多项式, 因此可以假设角速度函数为Legendre多项式形式, 再以陀螺输出完成函数逼近得到角速度函数。基于Legendre多项式的数值计算方法已经被应用在函数拟合[13]、插值计算[14]、大地测量[15]以及控制算法[16-17]等研究领域。既然在姿态计算中姿态微分方程展开次数够高时函数拟合误差会成为算法误差的主要来源, 因此有必要使用Legendre多项式这类正交多项式作为角速度拟合的正交基, 提升惯导解算中的函数逼近精度。
目前姿态更新算法的研究主要分为在圆锥运动下求解优化圆锥误差补偿系数和在多项式角运动假设条件下做泰勒级数展开求解误差补偿系数[18]。圆锥误差补偿算法在进行姿态更新时需要利用补偿系数计算更新周期内角增量叉乘构成的误差补偿项, 而使用角速率多项式假设的微分方程数值解法, 不需要求解误差补偿系数来构建误差补偿项, 可直接使用角速率多项式计算四元数或等效旋转矢量的高阶导数, 代入对应的泰勒展开式中计算姿态变化四元数或等效旋转矢量。方向余弦矩阵表示姿态时需要计算的元素过多, 浪费导航计算机算力; 而等效旋转矢量微分方程含有高阶误差项且微分方程复杂, 不便于进行数值计算; 四元数微分方程结构简洁, 元素个数较少可减少计算量。基于以上分析运用微分方程数值算法进行姿态微分方程求解时, 应采用四元数微分方程。
在Legendre多项式形式的角速率函数假设下, 根据四元数微分方程计算四元数高阶导数, 再代入四元数泰勒展开式中求解姿态更新四元数的方法是一种结构简单, 且计算精度较高姿态更新算法。与传统的优化圆锥补偿算法相比本文提出的高精度SINS姿态数值算法具有精度高、算法简洁的优势。
1 Legendre多项式角运动描述Legendre多项式由{1, x, x2, …, xn, …}正交化得到, Legendre多项式的Rodrigues表示为
|
(3) |
且具有如下递推关系
|
(4) |
捷联惯导系统中大多数陀螺采样都是角增量形式的, 为了便于在姿态更新时求解姿态微分方程的数值解, 需要根据一个更新周期内的角增量向量序列, 构建角速度函数。使用前n个Legendre多项式的线性组合表示角速度, 即
|
(5) |
并以Pk(t)(k=0, 1, …, n)为基完成函数拟合。
假设在更新周期[0, T]完成m次陀螺采样, 则由(5)式得更新周期内第j个角增量为
|
(6) |
式中, Gi(t)表示多项式Pi(t)的积分。
令
|
(7) |
由(6)式和(7)式得
|
(8) |
定义ΔΘ=[Δθ1 Δθ2 … Δθm]T和A=[a0 a1 … an]T, 则(8)式可以简写为
|
(9) |
因此, 当m=n时, 更新周期内的陀螺采样数等于角速度的多项式次数, 可直接计算系数矩阵
|
(10) |
当m＞n时, 陀螺采样点数大于角速度多项式次数, 系数矩阵可以按(11)式用最小二乘法计算
|
(11) |
这样就可以根据陀螺仪输出角增量采样序列, 拟合出Legendre多项式表示的角速度函数。
2 四元数微分方程泰勒级数算法 2.1 算法推导角速度函数如(5)式所示, 求导可得
|
(12) |
式中, Fk(1)(t)表示Legendre多项式Pk(t)的一阶导数, 即
|
(13) |
类似地, Fk(s)(t)表示多项式Pk(t)的s阶导数。所以, 角速度的各阶导数为
|
(14) |
这说明在完成(5)式中系数计算后, 就能很容易地计算出角速度的各阶导数。
因为已经得到了角速率多项式, 适合使用数值计算方法求解姿态微分方程完成姿态更新, 而常用的几种姿态表示中, (15)式所示的四元数微分方程最为简洁, 且使用四元数表示姿态有算法简单, 计算量小等优点, 所以此处在四元数微分方程的基础上推导姿态更新算法。
|
(15) |
式中，q(t)为载体坐标系到参考坐标系的姿态四元数, ⊗表示四元数乘法。
在[0, T]内, 姿态变化四元数可以展开为
|
(16) |
因此计算出四元数在初始时刻的各阶导数, 就可以根据四元数的泰勒级数计算出姿态变化四元数q(0, T)。
根据(15)式可得
|
(17) |
式中, 
从(15)式和(17)式可以看出, 只需要给定初始四元数, 并根据(14)式计算出角速度的各阶导数值, 就可以迭代计算出四元数的各阶导数。再将以上四元数导数代入(16)式就能完成姿态变化四元数计算。
2.2 运算量分析本算法主要有三部分: 角速度多项式系数计算、四元数导数计算和姿态更新四元数计算。假设算法的子样数为n, 角速度多项式系数计算乘法运算次数为
|
(18) |
(16) 式进行l次展开时, 需计算l个四元数导数值。(17)式中每一个四元数乘法项包含1次四元数乘法和1次系数乘法。四元数乘法包含12次乘法运算, 系数乘法包含4次乘法运算。计算四元数k阶导数时需要完成的四元数乘法和系数乘法次数为
|
(19) |
所以计算全部四元数导数的乘法运算次数为
|
(20) |
姿态更新四元数计算中乘法运算次数
|
(21) |
当采用六子样算法, 四元数进行10次展开时, 由(18)~(21)式, 得到算法乘法计算量
|
(22) |
而文献[10]中Picard迭代姿态算法在六子样与10次Picard展开条件下乘法计算量MP=3 708, 文献[7]中扩展圆锥误差补偿算法六子样结构的乘法运算量MU=135。Legendre多项式泰勒级数数值算法虽然运算量大于扩展圆锥补偿算法, 但是算法精度具有优势, 而且和Picard迭代姿态算法相比运算量下降约20.5%。
3 高精度捷联惯导姿态更新捷联惯导解算中, tk时刻的姿态更新公式为
|
(23) |
为了便于区分, 使用Q表示载体相对参考坐标系的姿态, q表示一次更新过程的姿态变化四元数。(23)式中, q(tk-1, tk)表示tk-1到tk的姿态变化, 因为每一次更新是独立的, 所以通常将时间区间[tk-1, tk]平移映射到[0, T], 这样用第2节中的算法可以直接计算q(0, T)。捷联惯导系统在完成初始对准, 确定Q(0)后, 就能根据陀螺输出计算每个更新时刻对应的姿态变化四元数q(0, T), 再根据(23)式完成姿态更新。
此算法的流程如图 1所示。
|
| 图 1 姿态更新流程图 |
因为此算法在计算过程中只使用了数值算法求解微分方程, 所以算法精度完全由数值计算精度决定。通过增加(16)式的展开次数, 能提升数值计算精度, 直到满足计算要求; 在角速度拟合时选择使用正交多项式构造关于时间的函数, 拟合效果优于常用的幂级数多项式, 所以此算法有很高的算法精度, 后文将通过不同条件的算法仿真与对比，验证这一结论。
4 算法性能评估方法为了比较新的姿态算法与已有的高精度姿态算法的解算精度, 在圆锥运动和大机动条件下, 完成其他5种姿态更新算法仿真, 并与新提出算法比较, 分析算法精度。参与对比的6种算法名称以及缩写如表 1所列。
根据(16)式可知, 新的姿态更新算法精度与展开次数l相关, 因此首先在圆锥运动条件下, 比较不同展开次数l下, 姿态解算精度。圆锥运动条件可以在圆锥轴上激励出算法的圆锥误差, 因此常被用来测试姿态算法精度, 四元数描述的圆锥运动如(24)式所示。
|
(24) |
仿真中圆锥运动参数如表 2所列, 角增量数据采样率为100 Hz。
圆锥运动姿态仿真中, 四子样条件下新算法2~10次展开下的算法误差对比如图 2所示, 从图中可以看出来, 随着展开次数的增加算法精度也不断提升, 其中7次展开时算法精度最高, 而8~10次展开的算法精度基本一致。由(16)式可知, 当确定采用l次展开计算四元数时, 所求姿态四元数精度满足
|
| 图 2 圆锥运动下姿态误差 |
在表 2表示的圆锥运动条件下, 全部使用四子样算法完成姿态更新, 新提出算法使用7次展开(l=7)的计算结果, 6种算法的解算误差如图 3所示, 算法漂移如表 3所列。
|
| 图 3 不同算法姿态误差对比 |
| 序号 | 姿态算法 | 算法漂移/((″)·h-1) |
| 1 | OCC-4 | 112.731 6 |
| 2 | PNC-4 | 28.559 4 |
| 3 | UCC-4 | 40.037 6 |
| 4 | 3CC-4 | 708.897 2 |
| 5 | 4CC-4 | 6.888 3 |
| 6 | LPT-4 | 2.053 9 |
从以上圆锥运动仿真结果可以得出结论: 新提出的基于Legendre多项式的高精度姿态算法和其他5种姿态算法相比算法精度显著提升; 在仿真圆锥运动条件下, 新算法的算法漂移仅有2.053 9″/h, 和圆锥运动条件下算法性能优异的4CC-4相比, 新提出算法的误差不到其1/3。
4.2 不同参数圆锥运动下算法漂移对比为了在不同圆锥运动条件下对比几种算法的误差, 首先设置圆锥角频率为4π rad/s, 选取半锥角0.009°≤α≤90°进行导航解算仿真, 分析算法漂移与圆锥运动的关系。算法漂移对比如图 4所示。
|
| 图 4 不同半锥角下算法漂移对比(Ω=4π rad/s) |
图 4中所有算法漂移都随着半锥角增大而增大, 当半锥角小于1°时, 4种圆锥补偿算法有较高的算法精度, 而基于数值积分的PNC与LPT算法虽然算法误差不大, 但是与几种圆锥补偿算法有明显差距; 当半锥角大于1°以后, LPT的算法漂移随半锥角增长的趋势放缓; 半锥角在10°附近时, 7次展开的Legendre多项式姿态算法精度最高, 尽管随着半锥角继续增大, 其精度下降到与4CC算法相当, 但是在这种更大的机动条件下, 只需要增加展开次数, 就可以提升算法精度, LPT 8次展开算法在半锥角大于20°以后精度最高。这也说明在较大半锥角的条件下, 增加LPT算法的展开次数可以有效提升算法精度。
在相同半锥角取值范围内, 设置圆锥运动角频率为2π rad/s, 不同算法姿态仿真的结果如图 5所示。由于圆锥运动角频率下降, LPT的7次和8次展开算法精度没有较大差异, 即使在半锥角大于20°时, 也不需要增加展开次数, LPT算法在大锥角下的精度远高于其他算法。
|
| 图 5 不同半锥下算法漂移对比(Ω=2π rad/s) |
① 在小半锥角(0.1°以下)条件下, 基于圆锥误差补偿的算法具有更高的精度, 这是因为圆锥补偿算法推导中有半锥角为小角度的假设;
② 在大半锥角(10°以上)条件下, 基于Legendre多项式的姿态算法精度最高, 因为此算法直接求解姿态微分方程, 只需要选择合适的展开次数就可以在大半锥角下准确计算姿态;
③ 随着半锥角变大, 算法漂移也随之增大, 基于Legendre多项式的姿态算法漂移变化更加平稳, 因此此算法的适用范围更广。
4.3 角运动大机动条件大机动仿真是捷联惯导姿态算法研究中另一个重要的算法精度评估方法, 文献[7-10]中使用的角速度多项式模拟载体的大机动状态, 仿真时间2 s, 具体的角速度函数如(25)式所示, 角速度单位为rad/s，角速度和角加速度变化如图 6所示(图中角速度单位为°/s)。
|
(25) |
|
| 图 6 大机动条件角速度与角加速度 |
同样地, 首先分析不同展开次数l下新的姿态算法的解算精度。大机动条件下, 2~10次展开后的算法误差如图 7所示。从图 7可以看出随着展开次数增加, 算法精度不断提升, 8次及更高的展开已不能显著提升算法精度。
|
| 图 7 大机动条件姿态误差 |
完成6种算法在大机动条件下的姿态解算仿真, 比较其在大机动环境下的算法精度, 其中新的基于Legendre多项式的姿态算法采用8次展开与其他算法比较。2 s内3个轴的算法误差对比结果如图 8~10所示。
|
| 图 8 大机动条件x轴姿态误差 |
|
| 图 9 大机动条件y轴姿态误差 |
|
| 图 10 大机动条件z轴姿态误差 |
在大机动仿真条件下算法误差并不是以固定的漂移率随时间增加, 因此以仿真过程各个算法的姿态均方根误差(RMSE)为指标评估大机动条件下的算法精度, 大机动仿真过程的各轴上算法误差如表 4所示。
| 序号 | 姿态算法 | 算法误差/(″) | ||
| x | y | z | ||
| 1 | OCC-4 | 1.429 | 0.606 | 0.648 |
| 2 | PNC-4 | 0.040 7 | 0.047 1 | 0.073 1 |
| 3 | UCC-4 | 0.040 7 | 0.047 1 | 0.073 1 |
| 4 | 3CC-4 | 0.026 3 | 0.014 3 | 0.011 8 |
| 5 | 4CC-4 | 0.000 262 | 0.000 107 | 8.10×10-5 |
| 6 | LPT-4 | 5.72×10-6 | 3.41×10-6 | 3.53×10-6 |
1) 在大机动条件下依然是基于Legendre多项式的高精度姿态算法精度最好;
2) 三次叉乘补偿高阶圆锥算法、四次叉乘补偿高阶圆锥算法和Legendre多项式泰勒级数数值算法, 在大机动条件下的精度优于其他算法, 能满足大机动环境姿态解算需要;
3) 6种算法的精度高低排序为: LPT＞4CC＞3CC＞UCC≈PNC＞OCC。
5 结论捷联惯导姿态算法通过补偿不可交换误差提升算法精度, 或者利用数值迭代算法求解姿态微分方程。圆锥算法在大锥角环境和大角度机动环境会产生较大的姿态漂移误差, 继续提升精度需要展开Bortz方程高阶项, 算法优化难度大; 以前的数值迭代姿态算法根据Picard算法求解姿态微分方程, 但是在角速度拟合时没有以正交基进行拟合。针对大机动环境的捷联惯导姿态算法研究, 从姿态微分方程高精度数值求解的方向更容易展开, 虽然运算量大, 但是不需要圆锥算法中的小角度半锥角假设, 提升硬件性能即可满足算法需求。本文首先以Legendre多项式完成角速度多项式拟合, 并在Legendre多项式形式基础上推导了四元数微分方程泰勒级数数值解, 最后给出完整的高精度捷联惯导姿态算法流程。通过圆锥运动条件下和大机动条件下的姿态解算仿真, 分析了新算法在不同展开次数下的精度, 并同其他算法进行算法误差比较。验证了新算法在2种条件下都有最优的算法精度, 相比其他算法优势显著。论文提出的基于Legendre多项式的泰勒级数数值算法可用于大机动环境捷联惯导姿态解算, 并作为今后为原子陀螺惯导系统配套的高精度捷联惯导算法的理论基础之一。
| [1] |
秦永元. 惯性导航[M]. 2版. 北京: 科学出版社, 2014.
QIN Yongyuan. Inertial Navigation[M]. 2nd ed. Beijing: Science Press, 2014. (in Chinese) |
| [2] |
严恭敏, 翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.
YAN Gongmin, WENG Jun. The strapdown inertial naugation algorithm and intergrated nauigotion theory[M]. Xi'an: Northwestern Polytechnical University, Press, 2019. (in Chinese) |
| [3] | BORTZ J E. A new mathematical formulation for strapdown inertial navigation[J]. IEEE Trans on Aerospace Electronic Systems, 1971, AES-7(1): 61-66. DOI:10.1109/TAES.1971.310252 |
| [4] | MILLER R B. A new strapdown attitude algorithm[J]. Journal of Guidance Control and Dynamics, 1983, 6(4): 287-291. DOI:10.2514/3.19831 |
| [5] | IGNAGNI M B. Optimal strapdown attitude integration algorithms[J]. Journal of Guidance Control and Dynamics, 1990, 13(2): 363-369. DOI:10.2514/3.20558 |
| [6] | IGNAGNI M B. Efficient class of optimized coning compensation algorithms[J]. Journal of Guidance Control and Dynamics, 1996, 19(2): 424-429. DOI:10.2514/3.21635 |
| [7] |
宋敏. 高动态下捷联惯性导航算法误差分析与优化方法研究[D]. 长沙: 国防科学技术大学, 2012 SONG Min. Research on error analysis and optimization methods for strapdown inertial navigation algorithm under highly dynamic environment[D]. Changsha: National University of Defense Technology, 2012(in Chinese) |
| [8] | WANG M S, WU W Q, WANG J L, et al. High-order attitude compensation in coning and rotation coexisting environment[J]. IEEE Trans on Aerospace and Electronic Systems, 2015, 51(2): 1178-1190. DOI:10.1109/TAES.2014.140084 |
| [9] | WANG M S, WU W Q, HE X F, et al. Higher-order rotation vector attitude updating algorithm[J]. Journal of Navigation, 2019, 72(3): 721-740. DOI:10.1017/S0373463318000954 |
| [10] |
严恭敏, 翁浚, 杨小康, 等. 基于毕卡迭代的捷联姿态更新精确数值解法[J]. 宇航学报, 2017, 38(12): 1307-1313.
YAN Gongmin, WENG Jun, YANG Xiaokang, et al. An accurate numerical solution for strapdown attitude algorithm based on Picard iteration[J]. Journal of Astronautics, 2017, 38(12): 1307-1313. (in Chinese) DOI:10.3873/j.issn.1000-1328.2017.12.007 |
| [11] | WU Y X, YAN G M. Attitude reconstruction from inertial measurements: QuatFIter and its comparison with RodFIter[J]. IEEE Trans on Aerospace and Electronic Systems, 2019, 55(6): 3629-3639. DOI:10.1109/TAES.2019.2910360 |
| [12] | WU Y X. RodFIter: attitude reconstruction from inertial measurement by functional iteration[J]. IEEE Trans on Aerospace and Electronic Systems, 2018, 54(5): 2131-2142. DOI:10.1109/TAES.2018.2808078 |
| [13] | YANG C, YAO F, ZHANG M, et al. Adaptive sliding mode pid control for underwater manipulator based on legendre polynomial function approximation and its experimental evaluation[J]. Applied Sciences, 2020, 10(5): 1728. DOI:10.3390/app10051728 |
| [14] | SINGH A K, MEHRA M. Wavelet collocation method based on Legendre polynomials and its application in solving the stochastic fractional integro-differential equations[J]. Journal of Computational Science, 2021, 51: 101342. DOI:10.1016/j.jocs.2021.101342 |
| [15] | PIRETZIDIS D, SIDERIS M G. Analytical expressions and recurrence relations for the Pn-1(t)-Pn+1(t) function, derivative and integral[J]. Journal of Geodesy, 2021, 95(6): 1-12. |
| [16] | ZIRKOHI M M. Direct adaptive function approximation techniques based control of robot manipulators[J]. Journal of Dynamic Systems, Measurement and Control, 2018, 140(1): 011006. DOI:10.1115/1.4037269 |
| [17] | KHORASHADIZADEH S, FATEH M M. Robust task-space control of robot manipulators using Legendre polynomials for uncertainty estimation[J]. Nonlinear Dynamics, 2015, 79(2): 1151-1161. DOI:10.1007/s11071-014-1730-5 |
| [18] |
严恭敏, 杨小康, 翁浚, 等. 一种求解姿态不可交换误差补偿系数的通用方法[J]. 宇航学报, 2017, 38(7): 723-727.
YAN Gongmin, Yang Xiaokang, Weng Jun, et al. A general method to obtain noncommutativity error compensation coefficients for strapdown attitude algorithm[J]. Journal of Astronautics, 2017, 38(7): 723-727. (in Chinese) |
