非线性滤波在SINS中的应用*

鱼少少，裴 军，胡 超

(1. 中国科学院大学，北京 100049；2. 中国科学院国家天文台，北京 100012)

随着微小型飞行器和机器人等的快速发展，对其姿态研究显得越来越重要。一般载体的姿态常常采用滤波器对来自微机械(Micro-Electro-Mechanical System, MEMS)惯性测量单元(IMU)的多传感器数据进行融合得到，常用的微机械惯性测量器件包括三轴陀螺仪、三轴加速度计和三轴磁力计。文[1]建立了四元数卡尔曼滤波方程并进行了数据仿真，文[2]对文[1]提出的四元数卡尔曼滤波进行性能分析。同时很多人也进行了粒子滤波在姿态估计中的研究[3]。文[4]和文[5]通过对四元数的分析，设计了四元数粒子滤波算法求取姿态。本文通过对微机械器件数据采集和处理，设计非线性四元数卡尔曼滤波和粒子滤波，并对其姿态解算结果进行进一步分析。

1 四元数简介

载体上的微机械惯性测量器件的输出转换成载体的姿态，即载体自身坐标系(xb,yb,zb)相对于导航坐标系(xn,yn,zn)的角位置，写成矩阵形式如下:

(1)

欧拉角是载体的3个姿态角，即俯仰角(Pitch)、横滚角(Roll)和偏航角(Yaw)。根据欧拉旋转定律，可以通过3次旋转使得载体坐标系与导航坐标系重合，每一次旋转都绕导航坐标系的一个坐标轴转动，转过的角度就是欧拉角，每次旋转后坐标关系可由一个旋转矩阵表示，即方向余弦矩阵，(1)式采用z-y-x的旋转顺序依次旋转：

Az-y-x(ψ,θ,φ)=Az(φ)Ay(θ)Ax(ψ)=

(2)

其中，ψ,φ,θ分别代表偏航角、横滚角、俯仰角。为了避免欧拉角在表示姿态时可能出现的奇异问题，四元数在载体的姿态表示方面有广泛的应用。设计载体姿态四元数为

q=q0,q1,q2,q3=q0+q1i+q2j+q3k,

(3)

导航坐标系与载体坐标系之间的坐标变换可以用方向余弦矩阵表示，其四元数形式为

(4)

简记为：

(5)

(6)

四元数的微分方程如下：

(7)

其中，q为描述载体转动的四元数；ω为载体相对导航坐标系的角速度，也表示为四元数的形式：

ω=0+ωxi+ωyj+ωzk，

(8)

(9)

(10)

通过三轴陀螺仪测量3个轴的角速度就可以实现实时更新四元数的值，进而更新姿态角获得姿态信息。

2 四元数卡尔曼滤波

在四元数和非线性滤波算法的结合中，最常用的算法就是基于四元数卡尔曼滤波，本文使用文[1]的四元数卡尔曼滤波，滤波过程简介如下：

(1)滤波器初始化

为Q和R选取初始值，可以通过对静态下三轴陀螺仪、三轴加速度计和三轴磁力计进行测量，计算出各个轴的方差作为滤波器数据误差；通过初始加速度计测量出俯仰角和横滚角，用磁力计测量出方位角作为初始姿态，并把初始姿态角转变为初始四元数q0/0，选择P0/0=I4作为系统初始噪声。

(2)状态方程传播

文[1]中的Hamilton算子定义如下：

(11)

其中，s表示四元数的标量；v表示四元数的矢量。

(12)

(13)

qk+1/k=Φk+1/kqk/k,

(14)

(15)

trMk/kI4-Mk/kQk.

(16)

(3)量测方程更新

首先通过预测得到的qk+1/k应用(4)式计算出观测方程中的系数矩阵H(qk+1/k).

(17)

bk+1,

(18)

(19)

(20)

(21)

qk+1/k+1=qk+1/k+Kk+1bk+1-Hqk+1/knk+1,

(22)

(23)

3 粒子滤波

粒子滤波是通过寻找一组在状态空间中传播的随机样本近似表示状态变量，用样本均值代替积分运算，进而获得系统最小方差的过程，这些样本被称为粒子。采用数学语言描述如下：对于平稳的随机过程，假设k-1时刻系统的后验概率为p(xk-1zk-1),依据一定原则选取n个随机样本，k时刻获得测量更新后，经过状态和时间过程，n个粒子的后验概率密度可近似为p(xkzk)，随着粒子数目的增加，粒子的概率密度函数逐渐逼近状态的概率密度函数，从而粒子滤波达到最优贝叶斯估计的效果[3]。

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

(24)

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

粒子滤波算法步骤如下：

(2)通过重要性采样更新样本粒子状态，

(25)

(26)

(3)计算更新后粒子的权值

(27)

(4)归一化粒子权值

(28)

(29)

(30)

(7)令k=k+1,重复以上步骤。

4 数据实验

实验通过选择传感器模块mpu9250，它包含三轴陀螺仪、三轴加速度计和三轴磁力计，能够通过自身所有的低通滤波器和A/D变换模块直接输出九轴传感器数据。实验通过将mpu6050固定在转台上测量，在安装过程中远离环境磁场的干扰。采用单片机读取mpu9250测量数据后通过I2C串口传输给电脑进行处理，以检验算法的可用性和精度。

(1)首先将传感器模块mpu9250在静止状态下测量输出数据，采样速率为100 Hz，采样1 min，分析静态情况下的九轴输出数据。陀螺仪三轴静态数据如图1。

图1 陀螺仪三轴静态输出数据
Fig.1 Three-axis gyroscope static output data

图1是1 min采集的陀螺仪三轴数据，自上而下依次是x轴、y轴和z轴，运用MATLAB软件分析x轴的均值和方差分别为：0.0205(°)/s和1.238e(-4)(°)/s；y轴的均值和方差分别为：-0.004 8(°)/s和1.57e(-4)(°)/s；z轴的均值和方差分别为：0.015 4(°)/s和2.369 9e(-4)(°)/s。用同样的方法处理三轴加速度计和三轴陀螺仪的原始数据，结果见表1。

通过以上测量，加速度计在水平位置x轴和y轴均值不为0，所以在做姿态解算时引入均值误差提高测量精度。

(2)通过上面静态数据的读取，将各个传感器的方差作为四元数卡尔曼滤波和粒子滤波程序的方差参考，静止状态下四元数卡尔曼滤波和粒子滤波的姿态角结算误差如图2、图3。其中横坐标表示时间，单位是秒；纵坐标表示输出的角度误差，单位是(°)

表1 加速度计和陀螺仪输出数据的均值和方差Table 1 Mean and variance of accelerometer and gyro output data

图2 四元数卡尔曼滤波静态三轴姿态角误差Fig.2 Quaternion Kalman filter static triaxial attitude angle error

图3 粒子滤波静态三轴姿态角误差Fig.3 Particle filter static triaxial attitude angle error

静止状态下四元数卡尔曼滤波和粒子滤波三轴姿态角均值和误差如表2。通过对静态数据的分析可以看出，两种算法对均值改善相差不多，但是粒子滤波比四元数卡尔曼滤波在方差上有所改善，即粒子滤波静态测量相对稳定。

(3)由于没有转台等实验设备，通过对静态数据添加一个稳定的姿态仿真轨迹计算四元数卡尔曼滤波和粒子滤波在动态情况下的结算结果。通过对三轴姿态角分别添加角速率为2，3，5(°)/s，幅度为5仿真，得到如图4的仿真结果。其中横坐标表示时间，单位是秒；纵坐标表示输出的角度误差，单位是(°)

表2 静止状态两种算法的均值和方差Table 2 Mean and variance of the two algorithms for the quiescent state

图4 两种算法姿态角误差比较
Fig.4 Comparison of Attitude Angle Errors of Two Algorithms

表3 仿真状态两种算法的均值和方差Table 3 The mean and variance of the two algorithms for Simulation states

从表3可以看出，通过对动态的仿真实验分析，两种算法在均值改善和静态数据基本相同，但是粒子滤波比四元数卡尔曼滤波在方差上有所改善，即动态情况下粒子滤波相对稳定。

5 结 论

本文通过对四元数卡尔曼滤波和粒子滤波进行简单介绍，并通过对mpu9250进行数据采集和仿真实验，验证粒子滤波和四元数卡尔曼滤波的可行性，以误差均值和标准差为检验标准，验证了粒子滤波相对四元数卡尔曼滤波提高了标准差。

[1] Choukroun D, Bar-Itzhack I Y, Oshman Y. Novel quaternion Kalman filter[J]. IEEE Transactions on Aerospace and Electronics Systems, 2006, 42(1): 174-190.

[2] 高显忠, 侯中喜, 王波, 等. 四元数卡尔曼滤波组合导航算法性能分析[J]. 控制理论与应用, 2013, 30(2): 171-177.

Gao Xianzhong, Hou Zhongxi, Wang Bo, et al. Quaternion-based Kalman filter and its performance analysis in integrated navigation[J]. Control Theory & Applications, 2013, 30(2): 171-177.

[3] 梁军. 粒子滤波算法及其应用研究[D]. 哈尔滨: 哈尔滨工业大学, 2009.

[4] 乔相伟, 周卫东, 吉宇人. 基于四元数粒子滤波的飞行器姿态估计算法研究[J]. 兵工学报, 2012, 33(9): 1070-1075.

Qiao Xiangwei, Zhou Weidong, Ji Yuren. Study on aerial vehicle attitude estimation based on quaternion particle filter algorithm[J]. Acta Armamentarii, 2012, 33(9): 1070-1075.

[5] 吴海亮, 王惠南, 陈志明, 等. 基于粒子滤波的微小卫星姿态确定算法[J]. 中国惯性技术学报, 2007, 15(4): 427-430.

Wu Hailiang, Wang Huinan, Chen Zhiming, et al. Particle filtering-based algorithm for micro-satelliteattitude determination[J]. Journal of Chinese Inertial Technology,2007,15(4):427-430.