基于间接EKF的MEMS惯性系统最优姿态估计*

孙 伟,吴家骥

(辽宁工程技术大学测绘与地理科学学院,辽宁 阜新 123000)

基于MEMS的测姿系统在虚拟现实、无人机和自动车辆导航等领域有着广泛应用[1-3]。国内外有学者围绕MEMS惯性测姿算法开展大量对应研究工作,Madgwick提出采用PID控制理论的梯度下降算法[4],存在比例系数难以精确问题;Mahony提出互补滤波算法[5],但是当载体处于静止、匀速运动或缓慢变速运动环境下时存在着互补滤波系数难调整问题;而基于四元数的EKF方法[6-8]应用最为广泛,当状态方程和测量方程为非线性方程时,应用EKF模型获取的是状态向量的次优估计[9],同时存在释放四元数归一化约束的缺点。

围绕上述问题,论文将状态扰动作为状态向量,构建线性状态方程并通过广义加法设计一种间接EKF姿态估计方案。通过建立合理的惯性器件输出模型补偿各类误差的影响。在选择姿态描述方法时,考虑单一姿态描述方法存在自身缺陷的问题[10-11],选择用归一化约束的四元数进行姿态解算,用方向余弦阵进行二者间的转换[12]。与EKF姿态估计算法相比,在水平方向上姿态估计收敛快且航向误差小,姿态估计更加可靠。

1 四元数运动模型建立

(1)

根据欧拉定理:假设三维空间里刚体位移时,刚体内部至少有一点固定不动,则此位移等价于一个绕着包含那固定点的固定轴的旋转。旋转四元数的数学形式为:

(2)

如图1所示。

图1中,单位矢量rn=[rxryrz]。在b系相对n系运动过程中,采用极限思想计算转动速率:

(3)

并代入式(3):

(4)

式中:θ=rn·θ;ω=[ωxωyωz]T表示投影在b系下的IMU旋转角速率:

(5)

四元数的积分过程相当于求解一阶微分方程。假设在很短的积分周期内b系相对于n系的旋转角速率呈线性变化。则ti+1时刻的四元数在ti时刻的邻域内的Taylor公式如式(6),出于简洁省去坐标系信息。

(6)

将式(4)四元数微分结果代入式(6),整理得到一阶四元数积分:

(7)

2 MEMS输出建模

由于低成本MEMS制造工艺等因素的影响,其输出信号包含多种噪声[13],如果不进行误差校正而直接使用,会使估计姿态误差累积甚至解算失效。所以需要对MEMS惯性器件量测值进行补偿校正。建立如式(8)包含常见噪声的马尔科夫过程模型[14]:

(8)

相关协方差分别为:

(9)

3 间接EKF原理

(10)

等式右边由Taylor定理可知:

(11)

顾及上述等式得到线性连续时间系统

(12)

式中:F(t)

3.1 状态方程

为推导出扰动状态方程及非线性离散系统,设计包含四元数以及陀螺仪偏差的七参数状态向量:

(13)

由式(4)四元数导数方程和式(8)陀螺仪输出数学模型可知:

(14)

(15)

由前面推导知扰动四元数微分:

(16)

(17)

结合式(14)和Δbg的定义:

(18)

进一步得到连续系统扰动状态方程:

(19)

选定离散化时间间隔T,得到状态转移矩阵离散化过程:

(20)

噪声协方差离散化过程:

(21)

式中:

(22)

Ql表示连续系统噪声协方差矩阵,根据建立的MEMS输出模型得:

(23)

3.2 量测方程

连续系统量测方程设计为:

(24)

MEMS惯性传感器静止或匀速直线运动时,加速度测量值对姿态估计起平衡稳定的作用,取n系下重力投影相对值作为观测向量

(25)

为得到量测方程相对扰动状态的Jacobi矩阵,采用链式法则求偏导数如下

H

(26)

式中:Hx取决于加速度计的量测值,由式(25)知

Hx

(27)

(28)

(29)

3.3 间接EKF方程

根据EKF理论[15],采用迭代更新求取扰动状态向量估计值,进一步求得系统状态向量的估计值:

①状态一步预测

③状态一步预测均方误差

④滤波增益

⑤状态估计

代入式(15)得出最优估计姿态。

⑥状态估计均方误差

Pk=(I-KkHk)Pk/k-1。

由此可知间接EKF方块图如图2所示。

图2 间接EKF方块图

4 实验结果及分析

将 MTi-G-710惯性测量单元采样频率设置为100 Hz。实验开始时将MTi-G-710静止一段时间后开始缓慢无规则运动,然后再静止一段时间后继续缓慢无规则运动。将传感器采集的原始数据通过串口传至PC机,利用MATLAB软件编写误差状态EKF程序,获取准确的姿态估计。

三种不同方法解算的载体姿态对比结果如图3所示。

图3 姿态解算对比结果

通过对图3分析可看出,MTi-G-710缓慢无规则动态运动400 s过程中,间接EKF算法有较好的姿态估计误差优于传统EKF方案,根据图3(b)求取间接EKF与EKF方法获取的三个姿态角平均误差、均方误差及最大误差结果如表1和表2所示。

表2 EKF姿态解算误差统计 单位:°

对比表1和表2可看出,采用间接EKF方法获取的载体姿态最大平均误差、均方误差和最大误差分别小于0.25°、0.5°和8.1°,该结果优于传统EKF方法解算的载体姿态误差。实验结果验证了论文提出的基于间接EKF姿态解算算法具有更高的姿态解决可信度,且对于运动环境变化具有较好的适用性。

5 结论

提出间接EKF姿态更新方法。将姿态误差和陀螺仪偏差设计为六参数扰动状态向量,通过应用Taylor定理确定六参数扰动状态方程为线性方程,应用标准线性Kalman滤波理论使得扰动状态及其协方差最优更新。与原有的七参数状态向量EKF姿态估计算法不同,间接EKF无需为获得可接受估计性能而对噪声协方差进行重复调整,且不释放四元数归一化约束。对比性实验结果表明,在水平方向上姿态估计收敛快且航向误差小,姿态估计更加可靠,与成熟商用软件解算结果精度相当。间接EKF姿态更新方法可满足运动状态变化环境下载体姿态信息的连续准确获取。