摘 要：针对舰船捷联惯性导航系统姿态控制计算精度要求，提出一种高阶矩匹配UKF (high-order moment matching UKF,HoMM-UKF)的SINS系统四元数模型姿态估计算法.在UKF迭代递推计算过程中利用高阶矩匹配方法计算系统状态参数的预测采样点集及其权值的概率分布平均偏态和峰值，使其精确逼近状态参数最优估计.采用四元数姿态建模方法构建新型SINS状态变量与噪声向量相关的姿态方程模型，利用伪观测向量构建观测噪声与四元数相关的观测方程模型，设计系统噪声方差分离计算算法开展系统噪声方差计算，引入拉格朗日乘子算法计算四元数估计均值，最后利用SINS四元数姿态估计模型对HoMM-UKF算法开展仿真试验研究.通过UKF算法、CDKF算法与HoMM-UKF算法对比，验证了HoMM-UKF算法计算精度高，并且算法计算量负担较小，计算效率较高.
关键词：捷联惯性导航；姿态估计；高阶矩匹配；HoMM-UKF算法；Lagrangian乘子
非线性最优估计方法是舰船导航系统的一类关键技术，最优估计技术关键问题是随机向量经由系统非线性函数映射的后验概率分布计算，针对可解析概率密度函数在Bayesian最优估计框架下利用Bayes规则经由预测与更新步骤实现后验证概率分布递推计算，但是其多元变量函数最优解析解积分结果无法获得，为此人们提出很多基于数值逼近次优估计算法，如扩展Kalman估计(EKF)算法[1]、中心差分估计(CDKF)算法[2]以及UKF算法[3-5]等，通过预测概率分布统计特性的逼近计算和线性最小均方误差估计器来获得系统状态参数次优估计计算.EKF算法的Taylor级数一阶项线性化操作造成其原理上的高阶截断误差，若提高计算精度需要考虑截取其余高阶项，但这样会造成计算量增加，以致算法计算稳定性变差[6-7].CDKF算法利用Stirling插值变换替代EKF算法的Jacobian和Hessian矩阵计算，计算精度可达二阶，但是随着系统状态变量维数增加，其最优估计计算稳定性变差；UKF算法利用2n+1个采样点逼近计算后验算概率分布统计特性，计算精度也仅仅达到二阶水平.
UKF算法已经应用到很多非线性动态系统状态参数最优滤波计算中，如导航定位，组合导航系统以及移动机器人等[8-10]，并且获得很好的计算效果.UKF算法利用采样点及其权值集合经由非线性系统映射以及预测状态参数的统计特性经过加权变换来逼近状态参数概率统计特性，获得状态参数逼近最优估计计算.
UKF算法假设采样点集满足Gaussian分布，但是经由非线性系统函数映射后获得采样点集的预测值一般不能满足该假设条件，特别是噪声项显著情形中造成采样点集的预测值概率分布偏离Gauss分布情形，导致最优估计算法计算性能变差[11]，基于这一情况有必要研究非线性函数高阶矩的捕获方法，有效改善传统UKF算法的计算性能.由此提出一类高阶矩UKF(higher-order moment matching UKF, HoMM-UKF)算法来消除这种影响，即在传统UKF算法框架中的每次递推步骤内，利用高阶矩匹配方法精确匹配系统状态参数的预测采样点集及其权值概率分布平均偏态值和峰值，达到修正采样点集及其权值，使其能够精确逼近状态参数最优估计的目的.笔者利用SINS四元数姿态估计模型对HoMM-UKF算法开展仿真试验研究，经由UKF算法、CDKF算法与HoMM-UKF算法对比，本文的HoMM-UKF算法计算精度可达到四阶水平，并且算法计算量负担较小，计算效率较高.
1.1 SINS姿态方程
四元数是描述载体姿态旋转的一类特殊四维矢量，其定义为四元数是一个四维的非独立参量向量
q≡[ϑ0,ϑT]T，
其中，ϑ=[ϑ1,ϑ2,ϑ3]T为向量分量；ϑ0为实数分量，四元数运算过程中要求满足规范化限制，即有
qTq=‖q‖2=ϑTϑ
由四元数描述的姿态矩阵可表达为：
ϑTϑ)I3+2ϑϑT+2ϑ0〈ϑ×〉=
MT(q)N(q).
(1)
其中，
；
(2)
(3)
其中，I3是3×3的单位矩阵;〈ϑ×〉表示由四元数向量分量表达的斜对称矩阵,定义为：
〈ϑ
根据四元数微分计算法则，结合式(2)进一步整理获得SINS系统姿态四元数微分方程：
⊗ω=
M(q)·ω=
Ω(ω)·q，
(4)
其中
SINS系统姿态四元数微分方程中的角速率由载体坐标系中三轴速率陀螺测量得到，其表达式为：
(5)
式中
表示陀螺的量测输出角速率向量；α(t)表示陀螺随时间变化的漂移量；μV(t)和μu(t)分别表示陀螺测量噪声和陀螺漂移的随机噪声，它们是互不相关的Gauss白噪声，满足
和
).
整理获得SINS系统姿态四元数微分方程:
(6)
1.2 SINS姿态四元数观测方程
可以利用飞行器携带的辅助观测设备如星敏感器等获得精确的参考观测向量R，若在SINS载体系中得到的观测向量为B，根据QUST观测模型[12-14]得到：
(7)
其中
是一个具有观测噪声的3维观测向量，μj是观测传感器的观测高斯白噪声，具有
分布.
由此根据文献[5]得到SINS系统的伪观测模型方程:
Zj=Gjq-(1/2)M(q)μj，
(8)
其中
若按照式(8)展开k次观测获得的观测方程为：
Zk=[
q,
q,…,
q]T+μk=G(Xk,
)+μk，
(9)
其中
伪观测方程中的第一项Gjq是四元数向量的线性函数，其矩阵秩r=2.伪观测方程的噪声项-(1/2)M(q)μj是四元数相关项，和系统四元数姿态方程的过程噪声项相似，可以方便开展观测噪声方差计算[15].
1.3 SINS模型噪声方差分解算法
考虑观测噪声与向量Xk相关的观测方程写为：
Zk=Ge(Xk)wk.
(10)
状态向量均值计算：
(11)
定义Zk外积计算式：
(12)
那么，对其两边求期望值得到：
⊗
[Ge1Ge2,…,Genw]T.
(13)
令Ge=[Ge1Ge2,…,Genw]，式(13)简化为：
其中，第二项进一步化简
其中，式中⊗表示Kronecker乘积[16]，且对于i=1,2,…,Nw,有
成立，从而得到
⊗
).
(14)
则向量Z的噪声方差计算：
⊗
⊗
).
(15)
若噪声向量wk是均值为0的Gauss白噪声，则式(15)计算可以化简为：
PZk=Ge(Pwk⊗
).
(16)
1.4 四元数加权均值算法
考虑四元数由k-1时刻到k时刻更新操作产生误差四元数为：
δqi,k-1=qi,k-1⊗
其中，qi,k-1是k-1时刻更新计算的四元数
四元数一步预测值，δqi,k-1向量部分δϑi,k-1为：
若考虑HoMM-UKF算法中四元数均值计算的加权因子
则可以把四元数加权求均值问题转化为代价函数的极值计算问题，那么四元数代价函数可写为：
ϑi,k-1;
其中,
由于
‖
那么设拉格朗日算子λ，利用拉格朗日乘法构造代价函数[17]:
那么,可以通过计算
函数的最小值，得到
代价函数最小的矩阵L最小特征值的特征向量就是所求均值四元数.
针对非线性系统函数Z=f(X)，若状态向量均值为
利用Taylor级数在均值附近对其展开
….
得到映射向量Z期望为:
(17)
其中，E
3表示状态向量X的三阶中心矩，E
4表示状态向量X的四阶中心矩，定义它们分别为α和β参数
(18)
在UKF算法中引入适当的加权因子κi，有映射向量逼近估计为:
).
对其进行变换可得到
在状态变量X均值附近对f展开得到映射变量估计值
整理获得
f(2)PXX(κ1α2+κ2β2)+
f(3)
(α3κ1-β3κ2)+
).
(19)
式(18)定义的参数α和β作用在于匹配状态变量X的第三阶和第四阶中心矩，也就是状态变量的概率分布平均偏态值和峰值.对比式(17)和式(20)可以获得参数α和β的表达式为：
(20)
其中,φ1和φ2由状态向量平均偏态值和峰值计算获得:
φ1=
;
φ2=
，
其中,Slk=(
)lk表示系统状态向量的方差矩阵Cholesky分解因子矩阵的第l行第k列元素
分别表示状态向量的第三阶中心矩和第四阶中心矩，很明显所有Sigma点加权均值及其方差满足
diag{Pxx Q R};
若考虑离散非线性动态系统为：
(21)
式中：Xk∈Rn是k时刻的系统状态变量；wk-1是m1维过程噪声；Zk∈Rm是k时刻系统量测向量；vk是m2维量测噪声.
1)滤波参数初始化.
].
对系统状态向量初始方差矩阵和系统噪声方差矩阵实施Cholesky分解
；
2) Sigma点及权值系数计算.
n.
其对应的权值系数为：
κi=
, i⊂[1,n];
κi=
, i⊂[n+1,2n].
(22)
同时,系统噪声的Sigma采样点及其权值系数为:
其中，权值系数为:
κi=1/(2N),N=n+m1.
(23)
3)系统方程时间更新.
获得的Sigma点预测向量，进一步得到状态向量的一步预测值为：
同时状态向量一步预测误差方差阵可表达为：
T，
对预测方差矩阵实施Cholesky分解获到
同时,对观测噪声方差矩阵也实施Cholesky分解操作获得
相应Sigma点更新值为:
其对应权值系数取式(22)，同样地观测噪声项的Sigma采样点更新为：
i=2n+1,…,2n+m2；
i=2n+m2+1,…,2n+2m2.
其权值系数取式(23).
4)系统观测方程更新.
获得观测向量预测值：
观测向量的一步预测误差方差阵:
T，
状态向量与观测向量的交叉协方差阵为：
T.
5)Kalman滤波计算.
相应的Kalman增益阵
状态向量估计值
).
相应估计误差方差阵
笔者采用SINS姿态估计平台试验系统，针对SINS载体姿态状态变量开展估计仿真验证，仿真初始条件设置如表1所示.
为了验证HoMM-UKF算法的有效性及其数值计算优势，将其与传统UKF算法和CDKF算法进行仿真比较验证,从而获得如图1基于UKF算法的系统姿态角估计曲线，图2为基于CDKF模型算法系统姿态角的估计误差曲线，图3为HoMM-UKF模型算法获得系统姿态角的估计误差曲线.从图1和图3估计误差曲线可以看出，采用高阶矩匹配方法的HoMM-UKF算法，可以获得良好的计算效能，其横滚角和俯仰角误差曲线能够迅速地收敛并保持稳定，估计误差稳定在10′之内，其偏航角误差曲线在初始的震荡之后能够快速收敛并保持稳定.而UKF算法的横滚角和俯仰角估计精度在10′之内,但偏航角估计精度在40′之内，其偏航角误差曲线收敛困难并且稳定性差.利用高阶矩匹配方法进行估算算法得到的数据更加精确，稳定性较强.原因在于高阶矩匹配方法能够精确匹配系统状态参数的预测采样点集及其权值概率分布平均偏态和峰值，达到修正采样点集及其权值使其能够精确逼近状态参数最优估计目的.
表1 SINS姿态估计平台初始参数
Tab.1 The initial parameters set of SINS attitude estimation platform
参数名称参数值载体初始位置(N45°,E126°,H0m)载体初始速度/(m·s-1)(0,0,0)载体初始姿态角误差/(°)(2,4,15)光纤陀螺测量噪声/(°·h-1)σw=0.04光纤陀螺漂移噪声/(°·h-1)σv=0.005三轴陀螺初始漂移/(°·h-1)光纤陀螺输出频率/Hz(1,1,1)100试验仿真时间/s300
图1 系统姿态角的UKF算法估计误差曲线
Fig.1 Attitude Angle estimate error curve based on UKF algorithm
另外,SINS系统姿态估计模型采用传统CDKF模型算法和HoMM-UKF算法进行计算效能比较，获得如图2所示的基于CDKF模型算法系统姿态角的估计误差曲线.对图2和图3的误差数据进行比较，可以发现HoMM-UKF算法获得的横滚角和俯仰角估计精度在20′之内，能够快速收敛并保持稳定，而CDKF算法横滚角和俯仰角估计精度在20′之内,但偏航角估计精度在60′之内，收敛速度慢并且稳定性差.与CDKF算法相比，HoMM-UKF算法计算精度优于CDKF算法，数值计算稳定性较好，说明HoMM-UKF算法拥有良好的性能及计算精度优势.
图2 系统姿态角的CDKF算法估计误差曲线
Fig.2 Attitude Angle estimate error curve based on CDKF algorithm
图3 系统姿态角的HoMM-UKF算法估计误差曲线
Fig.3 Attitude Angle estimate error curve based on HoMM-UKF algorithm
基于SINS系统姿态估计高精度稳定数值计算要求，结合传统UKF算法设计了计算精度达到四阶的HoMM-UKF算法.利用四元数构建舰船载体SINS系统姿态估计模型，并且利用伪观测法构建了系统噪声相关的乘性噪声观测模型算法，并设计了乘性噪声方差分离算法.经由SINS试验平台仿真验证，相比于UKF 算法和CDKF算法，HoMM-UKF算法具有计算精度高、数值计算稳定的优点，在舰船载体SINS 姿态估计应用中具有适用价值．
参考文献：
[1] 王彤,马建仓,秦涛,等.基于旋转四元数的姿态解算算法[J].弹箭与制导学报,2014,34(3):15-16.
[2] 韩萍,干浩亮,何炜琨,等.基于迭代中心差分卡尔曼滤波的飞机姿态估计[J].仪器仪表学报, 2015,36(1):187-193.
[3] 杨建强,刘斌,侯建军,等.一种改进的速度加姿态匹配传递对准方法[J].光学与光电技术, 2014,12(6):61-64.
[4] 丁国强,马军霞,熊明,等.大失准角传递对准杆臂效应影响研究[J].郑州大学学报(工学版), 2015,36(3):110-115.
[5] 丁国强,徐洁,周卫东,等.SINS/CCD系统四元数中心差分姿态估计算法[J].华中科技大学学报(自然科学版), 2014,42(9):19-23.
[6] 张端金,刘雪,范鑫.基于Delta算子时变时延网络控制系统的H-∞滤波[J]郑州大学学报(工学版),2016,37(3):10-14.
[7] 宋宇,翁新武,郭昕刚.基于光流和惯性导航的小型无人机定位方法[J].传感器与微系统,2015,34(1):13-16.
[8] 杨功流,王丽芬,袁二凯,等. 大方位失准角下舰载机快速传递对准技术[J].中国惯性技术学报, 2014,22(1):45-50.
[9] 韩萍,干浩亮,何炜琨,等.基于CDKF的飞机姿态角估计[J].数据采集与处理, 2013, 28(6): 790-795.
[10]庞鸿锋,潘孟春,王伟,等.基于高斯牛顿迭代算法的三轴磁强计校正[J].仪器仪表学报,2013,34(7):1507-1511.
[11]丁国强，马军霞，熊明，等.大失准确传递对准杆臂效应影响研究[J].郑州大学学报(工学版)，2013，36(3)：110-114.
[12]邓红,刘光斌,陈昊明,等.基于UKF的导弹SINS/CNS姿态估计方法[J].系统工程与电子技术，2010,32(9):1987-1990.
[13]祝继华,郑南宁,袁泽剑,等.基于中心差分粒子滤波的SLAM算法[J].自动化学报,2010,36(2):249-253.
[14]闫林波,贾维敏,姚敏立,等.基于扩展卡尔曼滤波的动中通低成本姿态估计[J].电光与控制,2013,20(7):53-57．
[15]JIN Y, HE Y, FU J, et al. A fineinterpolationbased parametricinterpolation method with a novel real-time lookaheadalgorithm[J].Computer-aided design, 2014，55: 37-48．
[16]KARASALO M, HU X M. An optimization approach to adaptive Kalman filtering[J].Automatica, 2011, 47(8): 1785-1793.
[17]BAVDEKAR V A, DESHPANDE A P, PATWARDHAN S C. Identification of process and measurement noise covariance forstate and parameter estimation using extended Kalman filter[J].Journal of process control,2011,21(4): 585-601.
Abstract：In order to improve the precision requirement about the attitude control of the strap-down inertial navigation system, the high order moment matching UKF (Higher-order Moment Matching UKF, HoMM-UKF) algorithm was proposed, that is to estimate the SINS′ attitude parameters of based on its quaternion error model. In the recursive calculation process, for accurately approximating computational purposes, it uses high order moment matching method to calculate the average skewness value and peak value of the predicted sampling points set and their weights of the system state parameters in the view of the probability distribution. Making use of attitude quaternion method, then onlinear quaternion error model was constructed, in which model the systemnoise vector depends on system state vector, meanwhile construct its measure equation whose measurement noise vector depends on quaternion measurement vector by pseudo observation vector method was constructed, the weighted average of estimated quaternion with Lagrangian operator was calculated, the system noise variance calculation with the system noise separation algorithm was carried out, and finallyconstruct the SINS’ attitude estimation HoMM-UKF algorithms simulation on SINS attitude experiment platform was designed. It can be seen that HoMM-UKF algorithm’s calculation accuracy is higher than others and has better numerical stability, comparison of the UKF, and CDKF algorithms, and so the HoMM-UKF algorithm’s feasibility and calculation accuracy is verified.
Key words：strap-down inertial navigation system; attitude estimation; higher order moment matching; HoMM-UKF algorithm; lagrangian multiplier
收稿日期：2016-11-08；
修订日期：2016-12-18
基金项目：国家自然科学基金资助项目(U1204603)
文章编号：1671-6833(2017)04-0039-07
中图分类号：TP271
文献标志码：A
doi：10.13705/j.issn.1671-6833.2017.01.017