基于四元数粒子滤波的飞行器姿态估计算法研究

乔相伟，周卫东，吉宇人

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

0 引言

飞行器姿态确定系统是飞行器导航及姿态控制系统的重要组成部分，其精度直接影响到载体的姿态控制精度及导航定位精度。常用的姿态描述参数主要有方向余弦矩阵、欧拉角、姿态四元数(AQ)、旋转矢量、罗德里格参数(RPs)、修正罗德里格参数(MRPs)、凯莱-哈密尔顿参数等［1］。其中姿态四元数因为计算量小、非奇异性、可全姿态工作而广泛应用于实际系统当中。但由于四元数本质属于旋转矢量，其4 个变量不独立，使得它必须满足规范化的限制。如何克服四元数规范化限制及旋转矢量方差计算问题，让它与先进的滤波算法相结合成为国内外研究的热点问题［2-3］。

作为一个典型的非线性系统，飞行器姿态确定的精度不仅取决于姿态测量系统硬件配置的性能与精度，还与所采用的姿态估计算法密切相关。在四元数和非线性滤波算法的结合中，最常用的算法就是基于四元数的扩展卡尔曼滤波(EKF)算法。其中文献［4 -6］提出的乘性四元数EKF 算法因为利用乘性误差四元数来表示估计四元数点与真实点之间的距离，解决了姿态四元数协方差计算问题，而得到了广泛的应用。然而当系统非线性程度较强时，EKF 算法较高的截断误差将大大地降低滤波精度甚至会导致滤波发散。针对此，Vathsal 等［7］又给出了基于四元数的二阶EKF 算法，使得滤波精度达到了泰勒展开的二阶水平，但却涉及到Hessian 矩阵的计算，使得计算负担显著增加。之后，Crassidis 等提出了基于MRPs 和AQ 相互转换的飞行器姿态确定无迹卡尔曼滤波(UKF)的算法［8］，通过参数之间的相互转换，避免了四元数归一化问题;有效提高了姿态估计的精度。但是和EKF 一样，UKF 算法也是基于高斯逼近的滤波算法，采用均值和方差来表征状态的后验分布。对于实际中的强非线性、非高斯问题，这种简单的采用以均值和方差为特征的高斯逼近方法已不足以精确表示状态的真实分布。而采用样本形式对状态概率密度进行描述的粒子滤波(PF)算法则完全克服了系统线性、高斯的假设，成为目前最适合于强非线性系统的滤波方法［9-10］。基于此，Cheng 等［9］给出了基于MRPs 的姿态确定粒子滤波算法。事实上，影响四元数直接与粒子滤波结合主要存在2 个问题:1)粒子滤波中涉及到四元数的加权和运算，而四元数本质是旋转矢量无法像普通向量那样直接进行数学意义上的四则运算;2)正则化过程中，扰动粒子点的选取涉及到四元数协方差阵均方根的计算问题，即如何保证计算出来的扰动四元数点为规范化四元数。针对问题1)，文献［11 -12］给出了构造姿态矩阵代价函数法将四元数均值问题转化为代价函数最小时的四元数向量求解问题。在此基础上，Barfoot 等［13］又提出了四元数加权求和的拉格朗日代价函数法，进一步降低了计算的复杂度，简化了计算量。针对问题2)，本文通过对四元数协方差矩阵的迹进行研究，给出一种通过协方差阵的对角线元素开平方得到扰动四元数的方法，保证了扰动四元数的规范化。另外，为减少粒子滤波的计算量，文章将状态向量分为非线性部分和线性部分，对非线性部分采用粒子滤波，线性部分采用卡尔曼滤波算法，进一步减小了计算量，一定程度地提高了系统的实时性。

综上，本文给出了四元数粒子滤波的具体流程，针对大初始姿态误差角的飞行器姿态确定仿真实验表明，与基于四元数的EKF 算法和文献［8］的UKF算法相比，该算法具有更高的估计精度和稳定性。

1 飞行器姿态空间模型建立

1.1 姿态四元数微分方程

设四元数q 为一四维向量，定义如下:

式中:ρ≡［q1q2q3］T为向量部分;q0为实数部分。则四元数满足如下的规范化限制

由导航坐标系(n 系)到载体坐标系(b 系)的姿态矩阵用四元数表示如下:

式中:I3×3为3 ×3 的单位矩阵;〈ρ ×〉为斜对称矩阵，有

姿态四元数满足如下微分方程［13］:

1.2 传感器模型

本文采用光纤陀螺来提取角速度，其输出模型可表示为

1.3 CCD 星敏感器量测模型建立

直接采用CCD 星敏感器的四元数输出来测量陀螺的姿态，其模型如下:

式中:δq 对应的是星跟踪器的四元数形式的量测噪声，用欧拉角形式可表示为ηs，它是均值为零方差为σ2s的高斯白噪声。

2 粒子滤波

假设非线性动态离散系统为

式中:xk∈Rn为k 时刻的n 维状态向量;zk∈Rm为m 维观测向量;wk和vk分别为过程噪声和观测噪声。

粒子滤波算法步骤如下:

1)初始化:k =0 时，产生服从初始概率密度p(x0)的N 个粒子点

2)时间更新:从重要性密度函数q(xk|z1:k)进行重要性采样，并且设置得到采样粒子xik.一般重要性函数选取为q(xk|x0:k-1，y1:k)=p(xk|xk-1).

3)测量更新:在得到新的量测值zk后，计算粒子的权值

归一化粒子权值为

4)状态估计及经验协方差计算:

5)重采样:序贯粒子滤波中一个不可避免的问题就是粒子的退化问题，为降低粒子退化的影响，一个有效的方法就是引入重采样。重采样的基本思想就是删除权值小的粒子，复制权值大的粒子。在重采样开始之前首先要判断粒子退化的程度，它可由下面方法来度量:

式中有效粒子数定义为

当Neff小于预定的门限值Nth时，进行重采样。重采样的过程只取决于归一化后的权值，而与粒子的维数、大小无关。

6)正则化。重采样虽然消除了小权值粒子在估计中的影响，但随之而来带来了粒子多样性减小的问题，即粒子的贫化问题，为此需要引入粗化［15］或正则化重采样步骤［16］。

3 四元数粒子滤波

3.1 四元数粒子加权求和计算问题

由文献［12］可知，四元数加权求和计算问题最终都归结为求代价函数的极值问题。由文献［13］，当取四元数的向量部分为状态变量时，其加权求和问题即为如下代价函数极值问题:

式中:qi为时间更新得到的四元数;q 为待求的加权和四元数。

将误差四元数的向量部分δρi用qi和q 表示为

从而(14)式的最小化问题可转为

又待求的四元数满足规范化限制，所以在这里采用拉格朗日乘法构造如下的代价函数为

式中λ 为拉格朗日乘性因子。

解(18)式可得

(19)式表示对应矩阵的特征值问题，其中要求的四元数就转化为使(q)最小时的矩阵N 对应的最小特征向量。

3.2 四元数粒子滤波

滤波时取状态变量为x =［xqxb］T，将状态量分为四元数部分和陀螺漂移部分。四元数部分涉及到非线性运算采用粒子滤波方法，而陀螺漂移部分为线性运算，采用卡尔曼滤波算法。

3.2.1 时间更新

四元数部分时间更新:

当采用的重要性函数为q(xk|x0:(k-1)，y1:k)=p(xk|xk-1)时，由时间更新得到的四元数粒子点为

非四元数部分主要考虑陀螺漂移，其时间更新计算为

3.2.2 测量更新

当得到k 时刻的测量输出值qs，k及四元数粒子的量测预测输出值qi，s，k|(k-1)时，k 时刻四元数权值更新为粒子后验更新满足

归一化四元数权值

3.2.3 状态更新

由于四元数为旋转矢量，则四元数加权状态更新用(19)式计算。对应地，利用乘性误差四元数表示真实四元数点与估计点之间的距离。定义乘性四元数如下:

则对应的四元数误差协方差矩阵为

由于真实四元数点无法得到，在这里用四元数重采样点与估计四元数来表示四元数的误差方差矩阵:

3.2.4 重采样

四元数再采样采用基本粒子滤波的方法进行，如(13)式所述。

3.2.5 正则化

重采样虽然一定程度地避免了粒子的退化现象，但却带来了粒子贫化问题，针对此，本节主要讨论旋转四元数的正则化问题。在正则化过程中涉及到四元数方差的均方根计算问题。

四元数均方根可由(29)式求得

式中，qvari的4 个元素分别由()ii阵的4 个对角线元素开平方得到。

一般地，估计四元数与采样四元数之间的距离误差为小误差，此时误差四元数δq 可表示为

此时误差四元数的方差矩阵可简化为

这样，其均方根就可以表示为

(36)式产生了一组对称的Nk(i)个向量{δξ(j)}Nk(i)i=1.那么第i 个四元数粒子将被估计利用(37)式:

式中δq(j)=［δξT(j)1］T.

正则采样获得后代粒子之后，重新加权可以通过前面的(8)式进行，但为了使得到的估计粒子更加准确，在这里重新计算后验粒子的似然度。即

式中c=pyk|qk(Yk|(k)).

从而，得到的更新四元数值用(20)式计算为

由于陀螺漂移没有量测信息，所以陀螺漂移更新为

从而k 时刻用于计算的陀螺输出为

4 仿真实验

本文以软件SINS/CCD 姿态估计系统为平台，实验初始条件设为:光纤陀螺测量白噪声为0.02°/h，驱动白噪声为0.002°/h，陀螺输出采样频率为100 Hz，星敏感器采用2 个光轴垂直安装，其输出频率为1 Hz，飞行器初始姿态误差设定:横滚角、俯仰角、偏航角分别为1°、5°、-30°.初始陀螺漂移在3 轴上分别为1°/h、1°/h、1°/h.星敏感器运动速率为0.05°/s，测量白噪声标准差为20″.卡尔曼滤波器中的初始姿态和陀螺漂移估计值均设定为0，初始粒子数选为2 000 个。

根据以上条件分别采用EKF，UKF 和PF 算法进行仿真实验。如图1所示，当姿态误差角为小角度时，3 种算法估计精度相当;如图2～图3所示，特别是图3可看出，当初始姿态误差角很大，非线性程度很强时，EKF 算法滤波精度明显下降且有发散趋势。而PF 算法因为对强非线性系统具有较好地逼近效果，滤波精度最高;从收敛速度看，PF 算法的收敛速度也和UKF 算法相当，这是由于本文采用的PF 算法中陀螺漂移部分进行了线性化处理，从而减少了粒子滤波的维数，使得计算量降低。

图1 横滚角姿态误差对比Fig.1 Comparison of roll angle errors

图2 俯仰角姿态误差对比Fig.2 Comparison of pitch angle errors

图3 偏航角姿态误差对比Fig.3 Comparison of yaw angle errors

5 结论

针对飞行器非线性姿态确定问题，本文将四元数与粒子滤波相结合，给出了一种基于四元数的粒子滤波算法。针对四元数在粒子滤波应用中的规范化及协方差奇异性问题，考虑四元数为一旋转矢量，解决了四元数的加权和计算、协方差奇异性及正则化扰动四元数求取等问题，扩展了粒子滤波的应用范围。最后，利用SINS/CCD 组合姿态估计系统为平台，进行了仿真实验，实验结果表明:当初始姿态误差角为大角度时，本文的算法具有更好的估计精度和稳定性。

References)

［1］ Markley F L.Attitude error representations for Kalman filtering［J］.Journal of Guidance，Control，and Dynamics，2003，26(2):311 -317.

［2］ Shuster M D.Constraint in attitude estimation part I:constrained estimation［J］.Journal of the Astronautical Sciences，2003，51(1):51 -74.

［3］ Choukroun D，Bar-Itzhack I Y，Oshman Y.A novel quaternion filter［C］∥AIAA Guidance，Navigation，and Control Conference.Monterey:CSA，2002，42(1):174 -190.

［4］ Murrell W.Precision attitude determination for multimission spacecraft［C］∥AIAA Guidance and Control Conference.New York:American Institute of Aeronautics and Astronautics，1978:70 -87.

［5］ Pittelkau M E.Rotation vector attitude estimation［J］.Journal of Guidance，Control，and Dynamics，2003，26(6):855 -860.

［6］ Psiaki M L.The super-iterated extended Kalman filter［J］.Journal of Guidance，Control，and Dynamics，2004，21(8):342 -347.

［7］ Vathsal S.Spacecraft attitude determination using a second-order nonlinear filter［J］.Journal of Guidance，Control，and Dynamics，1987，10(5):559 -566.

［8］ Crassidis J L，Markley F L.Unscented filtering for spacecraft attitude estimation［J］.Journal of Guidance，Control，and Dynamics，2007，26(4):536 -542.

［9］ Cheng Y，Crassidis J L.Particle filtering for sequential spacecraft attitude estimation［C］∥AIAA Guidance，Control，and Control Conference and Exhibit.Providence:American Institute of Aeronautics and Astronautics，2004:1 -18.

［10］ 梁军.粒子滤波算法及其应用研究［D］.哈尔滨:哈尔滨工业大学，2009.LIANG Jun.Research on particle filter algorithm and its application［D］.Harbin:Harbin Institute of Technology，2009.(in Chinese)

［11］ Moakher M.Means and averaging in the group of rotations［J］.SIAM Journal on Matrix Analysis and Applications，2002，24(1):1 -16.

［12］ Markley F L，Cheng Y，Crassidis J L，et al.Averaging quaternions［J］.Journal of Guidance，Control，and Dynamics，2007，30(4):1193 -1196.

［13］ Barfoot T，Forbes J R，Furgale P T.Pose estimation using linearized rotations and quaternion algebra ［J］.Acta Astronautica，2010，49(6):1 -12.

［14］ 秦永元.惯性导航［M］.北京:科学出版社，2006:358-361.QIN Yong-yuan.Inertial navigation ［M］.Beijing:Science Press，2006:358 -361.(in Chinese)

［15］ Gordon N J，Salmond D J.Novel approach to nonlinear/non-Gaussian Bayesian state estimation ［J］.IEEE Proceedings on Radar and Signal Processing，1993，140(2):107 -113.

［16］ Doucet A，Freitas N D，Gordon N.Sequential Monte Carlo methods in practice［M］.New York:Springer，2001.