基于姿态参数切换的四元数快速传递对准算法

周卫东， 吉宇人， 乔相伟

(哈尔滨工程大学自动化学院，黑龙江哈尔滨150001)

0 引言

Kain J E提出的速度+姿态匹配算法是目前最为常用的传递对准算法之一［1］，其推导的快速传递对准模型在小角条件下进行了合理的线性化处理。在舰载武器的传递对准中，粗对准阶段完成之后，主子惯导间的相对误差依然较大，无法进行线性简化(比如舰载机与载舰的方位失准角往往为一个大角)。由于欧拉角法具有简单、直观的几何意义，因此基于欧拉角的非线性误差模型得到了较多的应用［2］。但在大角度的姿态描述中，欧拉角法会受到奇异性及超越函数的限制。与此相比，四元数法具有非奇异性、计算量小、对大角度旋转描述更精确等优点，因此被越来越多地应用于传递对准研究中［3－4］。同时，建立四元数误差模型也可使滤波器直接采用惯导设备即时修正得到的姿态四元数，因此精确度更高。

与之对应的非线性四元数滤波算法主要有四元数扩展卡尔曼滤波算法(quaternion extended Kalman filter，QEKF)和四元数无迹卡尔曼滤波(quaternion unsented Kalman filter，QUKF)。文献［5］将 EKF 算法与四元数结合，利用误差四元数的量测更新保证后验姿态四元数的规范性。但EKF算法存在高阶截断误差及雅克比矩阵求解的限制，而基于sigma点选取的UKF算法在保持与EKF计算量相当的同时，其滤波精确度达到泰勒展开式的二阶［6］，因此将其与四元数结合可在保证实时性的前提下，进一步提高滤波精确度［7－11］。但在四元数UKF算法中，需要解决四元数sigma点选取及加权求和的问题。针对前一个问题，文献［8］提出取四元数向量部分参与sigma点计算，再通过单位化约束得到标量的方法。而针对加权求和问题，文献［9］提出构建代价函数的方法，文献［10］通过奇异值分解进行计算，但都受到计算量较大的限制。对此，文献［11］提出了利用修正罗德里格斯参数(modified rodriguez parameters，MRPs)与四元数相互转换以保证四元数规范性的方法。基于该思路，将两种参数间的映射关系引入四元数UKF算法中，利用MRPS较低的转换复杂度及其不受单位化限制的特性，解决四元数UKF算法中的加权求和问题，从而使其计算量较其他四元数UKF算法更低。

针对舰载武器中大失准角的快速传递对准问题，在建立四元数误差模型的基础上，利用姿态参数转换的方法解决四元数的加权求和问题，并对四元数sigma点的求取及误差方差阵进行推导。

1 基于乘性四元数的误差模型

1.1 姿态误差模型

首先对推导过程中将要用到的符号变量进行定义:n为导航坐标系;e为地球坐标系;s*为子惯导系统解算的载体坐标系;s为子惯导系统所在的真实载体坐标系;m为主惯导系统解算的载体坐标系;ψm为s*系与m系之间的量测失准角;ψa为s系与m系之间的实际失准角;为从m系到s系的方向余弦矩阵;为从m系到n系的旋转角速度在m系上的投影，其他角速度定义与此类似;rn为杆臂在n系上的投影;为加速度计随机偏置误差为陀螺仪随机偏置误差;为从m系到s系的乘性四元数为其转置;为从m系到s*系的乘性四元数为其转置。

快速传递对准算法引入了相对失准角的概念，即s*系与m系之间的量测失准角ψm和s系与m系之间的实际失准角ψa。该算法在由Kain J E首次提出时，其姿态误差模型是在小失准角的假设下推导建立的。在舰载武器的实际应用中，经常会出现初始误差角为大角度的情况。此时，基于欧拉角的非线性姿态误差模型［1，3］为

四元数法作为一种非奇异姿态描述方式，其定义为

式中:q0为四元数Q的标量部分;ρ为四元数Q的向量部分。将向量与看作零标量的四元数，其四元数乘法［12］表示为

为使推导清晰，定义向量自动转化为零标量的四元数后再进行四元数乘法运算，其结果自动转回向量形式。根据式(3)，式(1)可写为

再根据四元数微分方程及角速度定义式，即

得到基于乘性四元数的非线性姿态误差模型为

1.2 速度误差模型

考虑杆臂效应的速度误差模型为

式中:δV为主、子惯导间的速度误差。根据四元数的相继转动性［12］，即

得到基于乘性四元数的速度误差模型为

1.3 量测方程

针对状态方程中过程噪声为乘性噪声的情况，将过程噪声扩维至状态变量中。取系统状态为

式中:Xe为扩维后的非四元数部分;2c和εc分别为加速度计和陀螺仪的常值偏置和常值漂移;ω为过程噪声;ρm和ρa分别为量测失准角及实际失准角的等效姿态四元数的向量部分，满足

快速传递对准在观测方程中选择主子惯导间的相对姿态与速度误差作为量测量，其量测方程为

2 基于姿态参数转换的四元数UKF算法设计

2.1 姿态参数转换方法

在采用四元数法描述三维空间中刚体的定点转动时，其4个元素并不相互独立，因此需要对获得的四元数进行规范化运算以保持其单位性，即

在四元数UKF算法中，sigma点经非线性函数传播后需进行加权求和得到后验均值。而姿态四元数作为描述刚体一次旋转的旋转矢量，直接进行四则运算意义下的计算将破坏均值四元数的规范性。对此，本文基于姿态参数转换原理，对四元数的加权求和公式进行推导。首先给出另一种姿态参数—修正罗德里格斯参数(MRPs)的定义及其与姿态四元数间的相互转换关系。MRPs是一种基于刚体定点转动的3参数姿态描述方法，其各个分量之间相互独立，因此并不受到规范化的限制。其定义为

MRPs转化为姿态四元数的公式为

式中，Φ为MRPs，姿态四元数转化为MRPs的公式为

2.2 姿态四元数加权求和算法

对其加权求和，得到均值MRPs为

2.3 基于单位四元数的sigma点选取

在四元数UKF算法中，选取sigma点时需保证其四元数部分为单位四元数。对此，文献［11］提出只取四元数向量部分参与sigma点计算，之后再通过单位化约束得到标量部分的方法。其计算步骤为

1)计算扰动sigma点，即

式中:Pk为状态误差方差阵;n为系统状态维数;为方阵第i行的转置;λ为尺度参数;为扰动sigma点中的四元数向量部分;为 非四元 数部分。

2)根据单位化约束计算标量部分，即

从而得到单位扰动四元数为

sigma点的计算公式为

式中:χq，k为 sigma 点的四元数部分;χe，k为非四元数部分。

2.4 基于单位四元数的误差方差阵计算

在四元数法中，两次旋转之差一般通过右乘旋转四元数的逆实现。因此在计算误差方差阵时，需要与非四元数部分区别实行。定义均值四元数与扰动四元数之差为

非四元数部分之差为

从而得到自协方差公式为

2.5 四元数UKF滤波算法

根据前4节的推导，得到基于姿态参数转换的四元数UKF滤波算法流程如下。

设定状态初值为

式中:P0为非噪声部分的误差方差阵，根据2.4节的推导进行计算，为使表述清晰，将采用经典UKF;算法的表达式，具体方法则按前4节推导进行操作;Rw为过程噪声方差阵。

1)sigma点选取与权值计算

2)时间更新

sigma点通过非线性四元数误差状态方程传播得到，即

3)量测更新

sigma点通过四元数量测方程传播得到，即

4)状态更新

3 仿真分析

以舰载武器的快速传递对准为例，对不同姿态参数下的非线性误差模型及相应的非线性滤波算法进行仿真对比。在风浪及减摇鳍作用下舰船产生的摇摆运动模型为

摇摆幅度为:θxm=5°;θym=4°;θzm=2°。摇摆频率为:ωx=0.15 rad/s;ωy=0.1 rad/s;ωz=0.06 rad/s。初始角为:θx0=0°;θy0=0°;θz0=45°。

舰船的对地运动模型为

子惯导系统陀螺常值漂移取为0.1°/h，随机漂移取为0.01°/h，加速度计零偏取为10－3g，随机偏差为10－4g。杆臂取为(15 15 5)，单位为m;水平方向上的两个姿态失准角分别为ψx=20′，ψy=40′，航向失准角为ψz=10°。系统状态非四元数部分初始值设为0，四元数部分设为［1，0，0，0］T;设 INS 解算周期为0.01 s，滤波周期为0.5 s，仿真时间为50 s。

方案1采用欧拉角模型和经典UKF滤波算法，方案2采用四元数模型和基于奇异值分解的四元数UKF滤波算法，方案3采用四元数模型和基于姿态参数切换的四元数UKF滤波算法。

3个方案对姿态失准角的估计误差如图1～图3所示。

图1和图2分别为3种方案下水平方向姿态失准角的估计误差曲线图。从图中可以看出，3种方案在非线性程度不高的条件下都可以保持较高的估计精确度及收敛速度，在10 s后都达到稳定。图3为航向失准角的估计误差曲线图。基于欧拉角法的方案一出现较大偏差，精确度明显降低。方案2采用基于奇异值分解的四元数UKF算法，由于受到算法复杂度的限制，其收敛速度较慢，但仍保持较高的估计精确度。方案3的收敛速度略慢于方案1，但比方案2快，这主要归因于四元数滤波算法的种种约束性条件在一定程度上提高了算法的运算量，但姿态参数适中的转换复杂度及四元数自身较小的计算量使得方案2依然能够满足实时性要求，而四元数法的应用则使其在非线性提高时依然能够保持较高的估计精确度，其对航向失准角的估计精确度较方案1提高了4倍以上。表1为3种方案的估计精确度比较。

图1 纵摇失准角估计误差ψxFig.1 Estimation error of pitching misalignment ψx

图2 横摇失准角估计误差ψyFig.2 Estimation error of rolling misalignment ψy

图3 航向失准角估计误差ψzFig.3 Estimation error of heading misalignment ψz

表1 3种方案的估计精确度比较Table 1 Comparison of estimation accuracy of 3 schemes

4 结语

本文利用修正罗德里格斯参数与四元数相互转化的方法对四元数加权求和公式进行了推导，克服了UKF算法中一步预测均值的四参数规范化限制。同时对扰动sigma点中四元数部分的单位化问题及误差方差计算问题给出了相应的解决方法。仿真结果表明，该算法与传统UKF算法相比，对大方位失准角具有更高的滤波精确度。而与基于奇异值分解的四元数UKF算法相比，其实时性更好，适用于主子惯导间姿态失准角较大的快速传递对准技术。

［1］ KAIN J E，CLOUTIER J R.Rapid transfer alignment for tactical weapon application［C］//AIAA Guidance，Navigation and Control Conference，Auguest 14 － 16，1989，Boston，USA.1989:1290－1300.

［2］ 谭红力，黄新生，岳冬雪.捷联惯导大失准角误差模型在快速传递对准中的应用［J］.国防科技大学学报，2008，30(6):19－23.

TAN Hongli，HUANG Xinsheng，YUE Dongxue.Rapid transfer alignment based on large misalignment SINS error model［J］.Journal of National University of Defense Technology，2008，30(6):19－23.

［3］ 熊芝兰，郝燕玲，孙枫.基于四元数的惯导系统快速匹配对准算法［J］.哈尔滨工程大学学报，2008，29(1):28 －34.

XIONG Zhilan，HAO Yanling，SUN Feng.Rapid matching alignment algorithm of inertial navigation system based on quaternion［J］.Journal of Harbin Engineering University，2008，29(1):28－34.

［4］ 戴邵武，李娟.一种快速传递对准方法的误差模型研究［J］.宇航学报，2009，30(3):942 －946.

DAI Shaowu，LI Juan.Study of rapid transfer alignment error model［J］.Journal of Astronautics，2009，30(3):942 －946.

［5］ LEFFERTS E J，MARKELY F L，SHUSTER M D.Kalman filtering for attitude estimation［J］.Journal of Guidance，Control and Dynamics，1982，5(5):417 －429.

［6］ JULIER S J，UHLMANN J K.Unscented filtering and nonlinear estimation［J］.Proceeding of the IEEE，2004，92(3):401－422.

［7］ CHOUKROUN D，BAR-ITZHACK I Y，OSHMAN Y.Novel quaternion Kalman filter［J］.Transactions on Aerospace and Electronic Systems，2006，42，(1):174 －190.

［8］ HENZEH LEEGHIM，YOONHYUK CHOI.Uncorrelated unscented filtering for spacecraft attitude determination［J］.Journal of Acta Astronautica，2010，67(2):135－144.

［9］ OSHMAN Y，CARMI A.Attitude estimation from vector observations using genetic algorithm embedded quaternion particle filter［J］.Journal of Guidance，Control and Dynamics，2006，29(4):879－891.

［10］ MARKLEY F L，CHENG Y，CRASSIDIS J L，et al.Averaging quaternion ［J］.Journal of Guidance，Control and Dynamics，2007，30(4):1193－1197.

［11］ CRASSIDIS J L，MARKLEY F L.Unscented filtering for spacecraft attitude estimation［J］.Journal of Guidance，Control and Dynamics，2003，26(4):536 －542.

［12］ 秦永元.惯性导航［M］.北京:科学出版社，2006:305－381.