基于对偶四元数的圆锥及划船误差补偿改进算法

邢丽, 熊智, 刘建业, 杭义军

(南京航空航天大学 导航研究中心， 江苏 南京 211100)

基于对偶四元数的圆锥及划船误差补偿改进算法

邢丽, 熊智, 刘建业, 杭义军

(南京航空航天大学 导航研究中心， 江苏 南京 211100)

为提高高动态环境下的对偶四元数捷联惯性导航算法解算精度，将梯形数字积分算法应用于圆锥和划船误差补偿算法中，改进了姿态和速度解算算法，提高了对偶四元数捷联惯性导航算法的解算精度。在单个采样周期内，利用前一时刻采集的陀螺角速率信号和当前时刻采集的陀螺角速率信号，通过梯形积分方式计算角增量进行圆锥误差补偿；利用前一时刻采集的加速度计信号和当前时刻的加速度计信号，通过梯形积分方式计算速度增量并结合同一时刻的角增量进行划船误差补偿。通过设计的多组动态模拟仿真航迹验证表明，当角速率和比力作为圆锥和划船误差补偿算法输入时，梯形积分算法的精度高于传统的矩形积分算法，且航迹的动态性越高，改进算法的性能优势越显著。同时，通过动态跑车实验结果的分析对比，进一步验证了该改进算法的实用性。

控制科学与技术； 对偶四元数； 捷联惯性导航算法； 圆锥误差补偿算法； 划船误差补偿算法； 梯形积分方法

0 引言

捷联惯性导航(简称惯导)的计算任务包含姿态解算和速度、位置解算。在大机动环境中，高精度捷联惯导算法性能的关键要素是姿态的更新是否能够准确实时跟踪载体姿态的变化[1]。姿态更新过程中旋转不可交换性误差是主要的误差来源，为了补偿旋转不可交换性误差，现有的文献集中在圆锥误差补偿算法研究中，且大部分采用截断泰勒级数展开进行旋转矢量的近似计算，实现圆锥误差的补偿[2-13]。现有圆锥误差补偿算法的研究大致可以分为3个方面：多子样圆锥误差补偿算法的系数设计[3-6]，不同输入方式(角速率或角增量)的圆锥误差补偿算法研究[7-11]以及圆锥误差补偿算法的结构设计[12]。在多子样圆锥误差补偿算法的系数设计方面，主要采用了基于时域的泰勒展开方法[4-6]和基于圆锥运动频域的泰勒展开方法[7]。在文献[12]中，Savage进一步扩展了Miller的方法，并且提出了一种显式频率成形方法用于圆锥误差补偿算法设计中，即圆锥误差补偿系数的设计是通过最小二乘法而不是频域展开方式获取最优圆锥误差补偿性能。根据不同陀螺仪的输出信号，圆锥误差补偿算法中旋转矢量的计算分别采用了角增量和角速率作为输入。而在圆锥算法结构设计方面目前的研究也分为两类，一类是双速结构，另一类是单速结构。

与姿态解算相似，速度更新过程中存在积分误差，对于积分误差的补偿算法称为划船误差补偿算法。虽然圆锥和划船误差补偿具有对偶性[13]，但是很少有参考文献针对性研究划船误差补偿算法。在文献[14-15]中，武元新采用了对偶四元数算法，由于对偶四元数可以同时描述刚体的转动和平移，因此可以同时进行姿态和速度更新，实现圆锥和划船误差的同步补偿[16-18]，简化了圆锥和划船误差补偿算法。虽然对偶四元数捷联惯导算法可以简化圆锥和划船误差补偿算法，但是目前给出的补偿算法是基于角增量和速度增量输入的圆锥和划船误差补偿算法，如果惯性传感器输出为角速率和比力时，使用基于角增量和速度增量输入的圆锥和划船误差补偿算法，就涉及到角速率和比力积分为角增量和速度增量的过程。若采用传统矩形积分方法，积分过程中会避免不了引入积分计算误差。另外，虽然目前有算法提出采用角速率输入的圆锥误差补偿算法，但是基于角增量和角速率输入的圆锥误差补偿算法在一定意义上是具有等效性的[19]，且基于角速率的圆锥误差算法的补偿通式相对较为复杂。因此，在角速率和比力输入条件下，需要对传统基于增量形式的对偶四元数圆锥及划船误差补偿算法进行改进，从而提高其补偿精度。

为此，本文提出一种对偶四元数圆锥及划船误差补偿的改进算法，即在圆锥和划船误差补偿算法中采用梯形数字积分算法，将角速率和比力输入积分为角增量和速度增量，减少了传统方法中采用矩形数字积分方法带来的积分误差。改进的算法性能通过多组不同参数设置的动态模拟航迹进行了仿真验证，仿真结果表明当陀螺仪角速率和加速度计比力作为输入时，梯形积分算法的精度高于传统的矩形积分算法，且航迹的动态性越高，改进算法的性能优势越显著。同时，为了更充分地验证改进算法性能，设计了动态跑车实验，通过采集的实际系统惯性器件输出，分别采用改进算法和传统算法进行捷联惯导解算，实际数据验证结果也同样证明了改进算法能够进一步提高对偶四元数捷联惯导算法的解算精度。

1 基于对偶四元数的传统圆锥和划船误差补偿算法

(1)

式中：qIt、q′It都是普通四元数；ε为对偶算子；下标I代表惯性坐标系，下标t代表推力速度坐标系。

(2)

(3)

(4)

(5)

(6)

(7)

(8)

0≤τ≤ΔT, ΔT为更新周期。

与等效旋转矢量类似，微分方程(6)式在实际计算过程中可以简化为

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(aω+εaf)=(bω+εbf),

(17)

(18)

(19)

(20)

(21)

(22)

将(21)式和(22)式代入(20)式中得

(23)

由(23)式可以看出当采用二子样算法对等效螺旋矢量进行更新时，其表达式与等效旋转矢量的二子样算法的求解式子相似，由此类比推导得到文献[14]中等效螺旋矢量多子样算法：

(24)

式中：螺旋误差补偿的优化系数Kij的数值与圆锥误差补偿的优化系数数值完全相同。

(25)

2)传统方法Ⅱ，为了减少传统方法I的积分计算误差，在圆锥误差补偿算法中，提出了基于角速率输出条件下的圆锥误差补偿算法，推导了基于角速率的等效旋转矢量计算方法，但其推导过程和补偿通式较为复杂。

因此，借鉴角速率输入条件下的等效旋转矢量计算方法的推导理论，将其进一步扩展得到角速率和比力输入条件下的等效螺旋矢量更新过程中，改进对偶四元数算法中传统的圆锥和划船误差补偿算法。

2 基于对偶四元数圆锥和划船误差补偿改进算法

(26)

式中：

(27)

图和等效螺旋矢量关系图Fig.1 Relationship among , and

(28)

对比(26)式和(28)式可以看出，采用传统方法Ⅰ推导得到的(26)式，由于积分误差而带来了计算精度的损失；而(28)式的补偿通式和计算过程又相对复杂。因此，如果将(26)式的计算过程进行改进，用梯形数字积分方法计算螺旋增量，如(29)式和(30)式所示：

(29)

(30)

将(29)式和(30)式代入(24)式，则可得

(31)

(32)

(33)

图2 基于对偶四元数的改进及传统圆锥和划船误差补偿算法的对比Fig.2 Comparison of improved coning and sculling algorithms based on dual quaternions and conventional coning and sculling algorithms

3 仿真验证

3.1 仿真参数设置

为了验证本文所提出的基于对偶四元数的圆锥及划船误差补偿改进算法的优势及有效性，设计了多组不同参数条件下的数字仿真航迹，进行捷联惯导解算，并且将其与传统方法Ⅰ进行对比分析。为了模拟圆锥和划船效应，仿真航迹中将角运动和线运动设置为周期性变化的正余弦形式，并且为了模拟高动态航迹，按照文献[20]中给出的高动态航迹定义，将运动加速度设置为50g，正余弦变化频率设置为5～12 Hz.

数字仿真航迹中设计的姿态角角速率和载体运动加速度为

(34)

ax=ay=az=nsin (2πf1t)，

(35)

仿真总时间为300 s，导航解算周期为0.01 s，惯性测量单元数据生成频率为200 Hz.

为了充分验证不同动态航迹下提出的改进算法的有效性，将(34)式和(35)式中的相关参数m、n和f1进行多组设计，如表1所示。这3组仿真参数的设置主要从以下两个方面进行考虑：1)第1组和第2组的参数设置目的是验证同一运动频率下，不同的姿态角波动幅值和不同的运动速度幅值条件下，改进算法与传统算法相比的精度改善情况；2)第2组和第3组的参数设置目的是验证姿态角和运动速度幅值相同，而运动频率不同条件下改进算法与传统算法相比的精度改善情况。

表1 航迹参数m、n和f1的多组设计

3.2 仿真结果分析

图3 第1组航迹的导航误差对比Fig.3 Navigation error comparison of Track 1

图4 第2组航迹的导航误差对比Fig.4 Navigation error comparison of Track 2

从第1组和第2组的仿真对比曲线中可以看出，在正余弦形式变化的仿真航迹中，无论是动态性相对较小的第1组航迹，还是高动态情况下的第2组航迹，其姿态、速度和位置解算精度对比结果表明，改进算法优于传统算法。

为了更加清晰和明确地说明改进算法与传统算法相比较的优势，表2和表3中统计了两种算法在不同航迹参数设置条件下，姿态、速度和位置解算误差的最大值和均方根(RMS)对比结果。从表2和表3中可以看出，不同组参数设置的动态航迹在采用了改进算法后，导航解算误差的最大值和RMS均小于传统算法，即姿态、速度和位置的解算精度均得到了提升。

表2 导航解算误差的最大值对比分析

表3 导航解算误差RMS对比分析

图5 第1组和第2组航迹的改进算法导航精度提升对比Fig.5 Navigation precision improvements of Tracks 1 and 2

图6 第2组和第3组航迹的改进算法导航精度提升对比Fig.6 Navigation precision improvements of Tracks 2 and 3

同时图5和图6分别将表2和表3中统计的第1组和第2组航迹、第2组和第3组航迹的导航解算误差最大值的精度提升幅度以直方图的形式进行了对比。图5中宽条矩形框代表第1组航迹中改进算法相比传统算法在解算姿态、速度和位置时精度提升幅度，箭头直线代表第2组航迹中精度提升幅度。从图5中明显看出采用改进算法后，第2组航迹导航解算精度得到了更大的提升。

图6中窄条矩形框代表第3组航迹改进算法相比传统算法在解算姿态、速度和位置时精度提升幅度。从图6中看出采用改进算法后，第3组航迹导航解算精度得到了更大的提升。

从图5和图6中给出的对比图线，可以得到如下的结论：

1)图5中第1组和第2组航迹相对比，当同一运动频率下，载体的运动幅值变大时，改进算法的导航解算精度更优于传统算法；

2)图6中第2组和第3组航迹相对比，当在同一运动幅值下，载体的运动频率变高时，改进算法的导航解算精度也更优于传统算法。

综合结论1和结论2可以充分地证明当应用于高动态环境中时，本文提出的改进算法在进行了圆锥和划船误差修正后，导航解算精度优于传统算法。

4 动态跑车实验验证

为了更充分的验证本文提出的基于对偶四元数的圆锥和划船改进算法，采用了加拿大NovAtel公司的产品SPAN-CPT光纤/全球定位系统(GPS)组合导航系统输出的实际数据，设计了动态跑车实验对本文提出的改进算法进行实际数据验证。

图7 跑车实验设备示意图Fig.7 Kinematic vehicle experimental equipment

图8 跑车实验的数据实时采集Fig.8 Real-time data acquisition process of kinematic vehicle experiment

图9 跑车实验的轨迹运行曲线Fig.9 Dynamic track curve of kinematic vehicle experiment

图10 光纤/GPS组合导航系统输出的跑车位置Fig.10 Vehicle position outputs of FOG SINS/GPS integrated navigation system

图11 光纤/GPS组合导航系统输出的跑车速度Fig.11 Vehicle velocity outputs of FOG SINS/GPS integrated navigation system

跑车实验的场地及设备示意图如图7所示，实时数据的采集如图8所示。跑车实验轨迹运行曲线如图9所示，图中曲线上每点的位置是光纤/GPS组合导航系统输出的相对精确的位置信息。为了对改进算法进行验证，选取了运行曲线中开始的前300 s数据，如图9中红色曲线标注部分。这300 s的数据中光纤/GPS组合导航系统输出的跑车位置、速度和姿态如图10～图12所示，将其作为改进算法和传统算法对比时的导航参数基准值，图中λ、L、H分别表示位置的经度，纬度和高度，γ、θ、φ分别为横滚角、俯仰角和航向角。输出数据中陀螺仪和加速度计频率为100 Hz，基准导航参数输出频率为1 Hz.

图12 光纤/GPS组合导航系统输出的跑车姿态Fig.12 Vehicle attitude outputs of FOG SINS/GPS integrated navigation system

使用跑车实验采集的陀螺仪角速率和加速度计数据，在同等初始条件下分别采用两种算法进行惯导解算。对比两种算法解算得到的姿态、速度、位置误差，即用传统算法解算得到的姿态、速度和位置误差减去改进算法得到的相应导航误差，得到的误差对比曲线如图13～图15所示。同样统计这两种算法解算得到的姿态、速度和位置误差最大值以及RMS，统计结果如表4和表5所示。

图13 传统算法与改进算法的姿态角误差对比曲线Fig.13 Attitude error comparison of traditional algorithm and improved algorithm

从图13～图15以及表4和表5的统计结果可以看出，改进算法的解算精度整体优于传统算法，解算得到的姿态、速度和位置精度均得到了提升。但鉴于实际验证数据中惯性器件误差的存在，以及跑车实验所能模拟的动态性有限，所以两类算法在实际跑车实验对比下的性能提升相对仿真数据较小。

5 结论

通过多组不同动态特性的数字仿真航迹的设计，对比分析了传统算法和改进算法的导航解算精度，仿真实验结果充分表明当应用于高动态环境中时，改进算法将优于传统算法，并且随着动态性的提高，更能体现出改进算法的优势。与此同时，通过设计动态跑车实验，进一步验证了本文所提出的改进算法性能优势。

图14 传统算法与改进算法的速度误差对比曲线Fig.14 Velocity error comparison of traditional algorithm and improved algorithm

图15 传统算法与改进算法的位置误差对比曲线Fig.15 Position error comparison of traditional algorithm and improved algorithm

表4 跑车实验数据导航解算误差的最大值

表5 跑车实验数据导航解算误差RMS

References)

[1] 邢丽, 熊智, 刘建业, 等. 针对高动态载体应用的高精度捷联惯导姿态算法优化方法[J]. 中国惯性技术学报, 2014, 22(6):701-706. XING Li, XIONG Zhi, LIU Jian-ye, et al. Optimized method of high-precision attitude algorithm for high dynamic vehicles[J]. Journal of Chinese Inertial Technology, 2014, 22(6):701-706. (in Chinese)

[2] 蒋郡祥, 于飞. 圆锥环境作为姿态算法优化环境的适用性分析[J]. 哈尔滨工程大学学报, 2016, 37(2):231-235. JIANG Jun-xiang, YU Fei. Applicability of coning environment used as the environment for attitude algorithm design[J]. Journal of Harbin Engineering University, 2016, 37(2): 231-235. (in Chinese)

[3] Ignagni M B. Efficient class of optimized coning compensation algorithms[J]. Journal of Guidance Control & Dynamics, 2015, 19(2):424-429.

[4] Song M, Wu W, Pan X. Approach to recovering maneuver accuracy in classical coning algorithms[J]. Journal of Guidance Control & Dynamics, 2013, 36(6):1872-1881.

[5] 黄磊, 刘建业, 曾庆化. 基于高阶补偿模型的新圆锥算法[J]. 中国惯性技术学报, 2013, 21(1): 37-41. HUANG Lei, LIU Jian-ye, ZENG Qing-hua. New coning algorithm based on high-order error compensation model[J]. Journal of Chinese Inertial Technology, 2013, 21(1):37-41. (in Chinese)

[6] Huang L, Liu J Y, Zeng Q H. Optimized strapdown coning correction algorithm[J]. Transactions of Nanjing University of Aeronautics & Astronautics, 2013, 30(4):343-349.

[7] Titterton D, Weston J. Strapdown inertial navigation technology[M]. Stevenage, Hertfordshire, UK: the Institution of Electrical Engineers, 2004.

[8] 王立冬, 刘军, 鲁军. 捷联惯导摇摆基座自对准中圆锥误差补偿算法[J]. 兵工学报, 2012, 33(7): 826-830. WANG Li-dong, LIU Jun, LU Jun. Coning error compensation algorithm of SINS self-alignment on swaying base[J]. Acta Armamentarii, 2012, 33(7): 826-830. (in Chinese)

[9] 汤传业, 陈熙源, 李建利. 一种角速率输入的圆锥算法设计[J]. 中国惯性技术学报, 2013, 21(4): 456-461. TANG Chuan-ye, CHEN Xi-yuan, LI Jian-li. Coning algorithm design for angular rate inputs[J]. Journal of Chinese Inertial Technology, 2013,21(4): 456-461. (in Chinese)

[10] Tang C, Chen X. A generalized coning correction structure for attitude algorithms[J]. Mathematical Problems in Engineering, 2014: 614378.

[11] Ben Y Y, Sun F, Gao W, et al. Generalized method for improved coning algorithms using angular rate[J]. IEEE Transactions on Aerospace & Electronic Systems, 2009, 45(4):1565-1572.

[12] Savage P G. Coning algorithm design by explicit frequency shaping[J]. Journal of Guidance Control & Dynamics, 2010, 33(4):1123-1132.

[13] Roscoe K M. Equivalency between strapdown inertial navigation coning and sculling integrals/algorithms[J]. Journal of Guidance Control & Dynamics, 2001, 24(2):201-205.

[14] Wu Y X, Hu X P, Hu D W, et al. Strapdown inertial navigation system algorithms based on dual quaternions[J]. IEEE Transactions on Aerospace & Electronic Systems, 2005, 41(1):110-132.

[15] Wu Y X, Hu X P, Wu M P, et al. Strapdown inertial navigation using dual quaternion algebra: error analysis[J]. IEEE Transactions on Aerospace & Electronic Systems, 2006, 42(1):259-266.

[16] Wang Z H, Chen X J, Zeng Q S. Comparison of strapdown inertial navigation algorithm based on rotation vector and dual quaternion[J]. Chinese Journal of Aeronautics, 2013, 26(2):442-448.

[17] Li K Z, Yuan J P, Yue X K. Autonomous navigation algorithm for spacecrafts based on dual quaternion[C]∥ 2nd International Conference on Space Information Technology. Wuhan, Hubei:SPIE, 2007.

[18] 夏琳琳, 赵琳, 刘繁明, 等. 基于对偶四元数的航姿系统姿态更新算法研究[J]. 系统仿真学报, 2008, 20(2): 276-280. XIA Lin-lin, ZHAO Lin, LIU Fan-ming, et al. Research on attitude updating algorithm based on dual quaternion for attitude and heading reference system[J]. Journal of System Simulation, 2008, 20(2): 276-280.(in Chinese)

[19] 王立冬, 孟亚峰, 高庆. 基于角增量和角速率的旋转矢量算法的等效性[J]. 宇航学报, 2014, 35(3): 340-344. WANG Li-dong, MENG Ya-feng, GAO Qing. Equivalence analysis of rotation vector algorithms based on angle increment and angular velocity[J]. Journal of Astronautics, 2014, 35(3): 340-344. (in Chinese)

[20] Hinedi S, Statman J I. High dynamic GPS Tracking, NASA-CR-184868[R]. Washington, DC, US: NASA, 1988.

Improved Coning and Sculling Error Compensation Algorithms Based on Dual Quaternion for Strapdown Inertial Navigation System

XING Li, XIONG Zhi, LIU Jian-ye, HANG Yi-jun

(Navigation Research Center, Nanjing University of Aeronautics and Astronautics, Nanjing 211100, Jiangsu, China)

In order to improve the precision of the strapdown inertial navigation algorithm based on dual quaternion in the high dynamic environment, a trapezoid digital integral algorithm is applied to compensate the coning and sculling errors, which optimizes the attitude and velocity calculating algorithm and improves the calculating precision of the strapdown inertial navigation algorithm. During a sampling period of gyroscope and accelerometer, the angular rates of gyroscope outputs at previous and current times are both integrated by using the trapezoid digital integral algorithm, and the integral angle increment is used for the coning error compensation. The accelerometer outputs at previous and current times are integated by using the trapezoid digital integral algorithm, and the integral velocity increment and the integral angular increment are used for the sculling error compensation. Through the simulation of multi-group dynamic tracks, it is shown that the improved coning and sculling error compensation algorithms adopting the trapezoid digital integration have higher navigation precision than the traditional rectangular digital integration method when the inputs are angular rate and acceleration. The dynamics of the track is higher, and the performance advantage of the improved algorithm is more obvious. The analysis and comparison of kinematic vehicle experimental results further verify the performance advantage of the proposed improved algorithm.

control science and technology; dual quaternion; strapdown inertial navigation algorithm; coning error compensation algorithm; sculling error compensation algorithm; trapezoid digital integral algorithm

2016-10-29

国家自然科学基金项目(61533008、61374115、61533009、61673208)；江苏省研究生培养创新工程项目(CXLX13_156)；南京航空航天大学研究生创新基地(实验室)开放基金项目(KFJJ20160320)；江苏省“333工程”科研资助立项项目(BRA2016405)；留学人员科技活动项目择优资助项目(2016年)；航空科学基金项目(20165552043)

邢丽(1987—)，女，博士研究生。E-mail:nuaaxl@nuaa.edu.cn

熊智(1976—)，男，研究员，博士生导师。E-mail:xznuaa@nuaa.edu.cn

V249.32+2

A

1000-1093(2017)07-1336-12

10.3969/j.issn.1000-1093.2017.07.012