基于加性对偶四元数的惯性／天文组合导航算法

钱华明 孙 龙 黄 蔚

（哈尔滨工程大学 自动化学院，哈尔滨 150001）

刘 璇

（黑龙江科技学院 电气与信息工程学院，哈尔滨 150029）

惯 性／天 文 组 合 导 航 系 统 （SINS／CNS，Strap-down Inertial Navigation Systems／Celestial Naviga-tion Systems）是一种利用天体测量信息和惯性测量信息获取高精度导航参数的导航系统，具有自主性强、测姿精度高的特点，而且还可以通过天文量测信息有效地修正陀螺漂移，因此被广泛地应用到高空无人机、弹道导弹、卫星和深空探测等高性能导航载体.

对于SINS／CNS组合导航系统，传统方法是将旋转和平移拆分开来，利用方向余弦阵或四元数描述转动，利用向量描述平移.然而实际载体坐标系之间的转换是二者兼具且一般同时发生的，与质点只存在平移而没有旋转的运动不同，因此旋转和平移是不能完全解耦分离开来的，特别是高动态的载体.因此迫切需要一种能将旋转和平移统一起来、简单又高效的计算方法.

文献［1］提出的对偶四元数能够将旋转和平移统一考虑，以最简洁的形式表示一般的刚体运动.文献［2］将对偶四元数算法应用到捷联惯导系统中，并详细分析了算法误差.文献［3］从理论上证明了在高动态环境中，对偶四元数算法比传统算法精度高.同时对偶四元数在机械、机器人视觉等领域也得到了广泛地应用.文献［4］将对偶四元数应用到卫星姿态确定中；文献［5］将对偶四元数应用到航天器的相对导航与控制，但是上述应用都是针对单一系统，若将其应用于SINS／CNS组合导航系统能够充分体现子系统的优势，各子系统相互弥补对方的缺点，将会进一步提高导航的精度.

针对SINS／CNS组合导航系统所应用的载体大部分都是高动态载体这一特点，将对偶四元数应用于基于修正陀螺漂移的SINS／CNS组合导航系统中，并通过仿真对所提出方案进行实验验证.

1 对偶四元数及螺旋矢量

对偶四元数继承了所有四元数特性，定义为

其中，q和q′分别为对偶四元数的标量和对偶部分；ε2＝0，但ε≠0；＊＝－1为共轭对偶四元数.由泰勒展开定理，可得

根据Chasles定理［5］：三维空间的任何有限位移都可以由一个螺旋运动获得.由此引入螺旋矢量来表示螺旋运动为

对偶四元数可以由螺旋运动的参数表示为

其中，为坐标系绕其旋转和平移的三维空间向量（螺旋轴）为旋转和平移的对偶角.

其中，n为单位向量，其方向代表平移的方向；d为螺距；φ为旋转角度.图1描述了原始坐标系O1经过螺旋运动变换到O2的过程.旋转是绕具有向量n且过点m的直线旋转θ角（θ＝φ）；O1m垂直于n，其方向矢量为p；坐标系原点之间的连线为矢量t；旋转和平移的顺序是可以颠倒的，因此对偶四元数又有如下表示［6］：

其中，◦为四元数乘子；q为转动四元数；tO1 和tO2为平移向量，上标O1，O2为t在O1，O2系上的投影.结合式（1）和式（6）可以看出，对偶四元数标量部分是转动四元数，对偶部分是转动四元数和平移向量的函数，因此对偶四元数能将旋转和平移统一考虑.

图1 旋转和平移几何图示

2 系统的对偶四元数误差模型

2.1 坐标系定义［3］

T为推力速度坐标系，与体坐标系平行，从地心到坐标原点的向量等于推力速度.

G为引力速度坐标系，与地球坐标系平行，从地心到坐标原点的向量等于引力速度.

E为地球坐标系，原点在地心，一个轴与地轴重合，其他两个轴在赤道平面.

I为惯性坐标系，t＝0时刻与地球坐标系重合.

U为位置坐标系，与地球坐标系平行，从地心到坐标原点的向量等于载体的位置向量.

B为载体坐标系，捷联惯性器件的坐标系.

N为地理坐标系，导航中常用的东北天坐标系.

2.2 状态误差模型

对偶四元数的加性误差定义为

其中，为计算或者测量得到的对偶四元数；δq＾为 ADQE（Additive Dual Quaterrion Error）.

对偶四元数的微分方程为

由上式可得到其误差的微分方程为

将式（7）代入式（9）得

忽略摄动误差的乘积，得

同理，G系与I系之间的误差方程为

U系的旋量表示为

其误差表示为

由式（16）得

将其实部、虚部分开为

将陀螺和加速度计的常值误差和随机误差分离，将常值误差作为状态量，随机误差作为系统噪声，并连同式（14）、式（22）、式（23）组合在一起依据如下误差状态向量的定义：

当四元数写成q＝［s，vT］T，v＝［v1v2v3］T时，四元数乘法可以表示成如下形式［3］：

矩阵q＋和q-定义为

其中，I3为3×3单位阵；［v×］为反对矩阵：

因此，式（24）中的F，G可表示为

上述的0矩阵均为4×4零矩阵.

2.3 量测方程

星敏感器是天文导航中重要的测量仪器，利用拍摄到的星图确定其光轴在惯性空间中的瞬时指向，从而确定载体的姿态.由星敏感器输出参数可得到B系相对于I系的四元数qX.将qX－qIB，可得量测方程为

其中，H＝［I4×404×20］；v为星敏感器的测量噪声.

2.4 导航参数误差量计算

建立对偶四元数线性导航模型之后，用卡尔曼滤波进行在线估计.得到状态量后就可以计算速度、位置、姿态误差.载体相对N系的速度为

将式（18）代入式（27）得到速度误差为

位置误差由rU＝rE＝2q＊IU◦q′IU得

由于δqIU＝0则载体相对N系的位置误差为

B系相对于N系的姿态矩阵计算如下：

载体姿态角为

则载体姿态角误差为

以δC12为例，由四元数与方向余弦的关系得

其中，δq（2）由状态变量δqIT得到，q（2）由qIT得到，将得到的计算值代入式（32）即可得姿态误差角.同理式（30由qIG计算得到，即

3 对偶四元数更新算法

1）使用螺旋矢量的双子样优化算法［8］计算螺旋矢量Φ＾.

其中，Φce和ΔVre分别为传统捷联算法中的圆锥误差补偿项和划船误差补偿项.

2）用得到的螺旋矢量代入式（37）计算q（ΔT）和q′（ΔT）.由式（4）得

结合式（2）将式（36）所有对偶四元数实数部分和对偶部分拆开得

3）将得到的q（ΔT）和q′（ΔT）代入式（39）计算tm时刻的对偶四元数.

设tm时刻的对偶四元数可表示为

将式（38）按照定义拆分成标量部分和对偶部分为

4 仿真与分析

仿真初始条件：初始位置东经120°，北纬36°；陀螺常值漂移和随机漂移分别为0.1（°）／h和0.05（°）／h；加速度计常值和随机偏置分别为100μg和50μg；星敏感器精度为10″（1σ）.分高、低动态两种环境对比传统SINS／CNS和对偶四元数组合导航系统（两种动态环境是验证算法的正确性而假设的，不代表载体的真实运动情况，表1是载体运行过程中爬升前后的参数对比）.

表1 载体爬升前后导航参数

限于篇幅本文仿真和图表数据均以东向为例，低动态环境下的仿真曲线如图2所示.

图2 低动态环境仿真曲线

高动态环境仿真曲线如图3所示.

图3 高动态环境仿真曲线

由仿真曲线和表2看出，基于修正陀螺漂移的SINS／CNS组合导航系统，使用卡尔曼滤波进行状态估计后，可以校正由陀螺漂移引起的姿态误差，速度、位置误差也起到了明显的抑制作用.但由于对加速度计偏置估计不准确，无法有效修正由加速度计引起的导航误差，所以仍然不能阻止位置和速度误差的发散.在低动态条件下，对偶四元数法精度与传统方法基本相同.因为在低动态条件下，载体的平动和角运动在滤波周期内比较小，积分运算得到的都是小量，不可交换性误差等都很小，旋转和平移可以近似的拆分开来.

表2 导航误差仿真结果

但是当载体在高动态条件下时，对偶四元数法的优势就很明显.相比于低动态条件下的曲线，虽然各导航误差都有所增加，但是变化都不大.反观传统导航方式，由于此时积分运算得到的不是误差小量，因此旋转和平移就不能完全解耦.姿态角误差、陀螺漂移误差增加明显，这就导致了速度误差和位置误差的快速发散.说明了在高动态条件下使用螺旋算法和对偶四元数算法的优越性和有效性.

5 结 论

在高动态条件下，基于加性对偶四元数的SINS／CNS组合导航算法运用螺旋矢量更新对偶四元数，将姿态和速度同时更新，统一考虑了旋转和平移，相比传统算法导航精度更高.SINS／CNS组合导航系统经常被应用于导弹、高空无人机、深空探测器等高动态载体，有着广阔的发展前景.

（References）

［1］Clifford W.Preliminary sketch of bi-quaternions ［J］.Proceedings of the London Mathematical Society，1873，4（4）：381－395

［2］Wu Yuanxin，Hu Xiaoping，Hu Dewen，et al.Strapdown inertial navigation system algorithms based on dual quaternions［J］.IEEE Transacions on Aerospace and Electronic Systems，2005，41（1）：110－132

［3］武元新.对偶四元数导航算法与非线性高斯滤波研究［D］.长沙：国防科技大学机电工程与自动化学院，2005

Wu Yuanxin.Resarch on dual quaternion navigation algorithm and nonlinear gaussion filtering［D］.Changsha：Collge of Mechatronics and Automation，National University of Defense Technology，2005（in Chinese）

［4］左俊青，王惠南，刘海颖，等.单目视觉下基于对偶四元数的卫星姿态的确定［J］.中国惯性技术学报，2008，16（5）：577－581

Zuo Junqing，Wang Huinan，Liu Haiying，et al.Satellite attitude estimation based on dual quaternion from monocular camera［J］.Journal of Chinese Inertial Technology，2008，16（5）：577－581（in Chinese）

［5］崔本杰.基于对偶四元数的航天器相对导航方法研究［D］.哈尔滨：哈尔滨工业大学卫星技术研究所，2009

Cui Benjie.Study on relative navigation method for spacecraft based on dual quaternion［D］.Haerbin：Research Center of Satellite Technology，Harbin Institute of Technology，2009（in Chinese）

［6］Wang Jianying，Liang Haizhao，Sun Zhaowei，et al.Relative motion coupled control based on dual quaternion ［J］.Aerospace Science and Technology，2011，12（13）：1－12

［7］吴高龙，刘华伟，郝顺义，等.对偶四元数导航算法的改进及仿真研究［J］.系统工程与电子技术，2011，33（4）：862－868

Wu Gaolong，Liu Huawei，Hao Shunyi，et al.Improvement of dual quaternion navigation algorithm and relevant simulation［J］.Systems Engineering and Electronics，2011，33（4）：862－868（in Chinese）

［8］陈亮.基于对偶四元数的捷联惯性导航算法［D］.哈尔滨：哈尔滨工业大学控制科学与工程学院，2011

Chen Liang.SINS algorithms based on dual quaternions［D］.Haerbin：School of Control Science and Engineering，Harbin Institute of Technology，2011（in Chinese）