基于扩展卡尔曼滤波器的四元数车辆姿态解算算法研究

开篇：润墨网以专业的文秘视角，为您筛选了一篇基于扩展卡尔曼滤波器的四元数车辆姿态解算算法研究范文，如需获取更多写作素材，在线客服老师一对一协助。欢迎您的阅读与分享！

摘要：测量车辆姿态可以实时跟踪车辆行驶信息，并通过远程发送车辆姿态信息的方式，实现对车辆实时的监控。结合车辆姿态测量只考虑俯仰角和横滚角的特殊性，通过MPU6050芯片采集角速率和加速度数据，采用基于扩展卡尔曼滤波器的四元数车辆姿态解算算法，实现对车辆姿态信息的获取。通过采集数据进行MATLAB仿真验证，结果表明该算法能提高姿态测量精度和系统稳定性。

关键词：扩展卡尔曼滤波器 姿态解算 算法研究

中图分类号：TP212 文献标识码：A 文章编号：1007-9416（2015）04-0127-01

当前，危险品运输车辆日益增多，其安全性也受到广泛关注。实时获取车辆姿态信息并发送至监控终端可实现远程对车辆的监控。采用低精度MEMS惯性传感器件进行姿态解算时，会导致解算精度很低，很难达到满意的效果。然而车辆姿态只需考虑俯仰和横滚信息，因此，本文将采用基于扩展卡尔曼滤波器的四元数车辆姿态解算算法，通过加速度信号估计出车辆姿态信息，再利用卡尔曼滤波器对车辆姿态信息估计值与角速率值进行数据融合，最终确定车辆姿态信息。

1 系统描述

定义坐标系如下：

（1）地理坐标系n系：xn，yn，zn依次指向东北天方向。

（2）载体坐标系b系：xb，yb，zb依次指向载体右前上方向。

得到姿态矩阵：

分别为运动载体的航向角，俯仰角和横滚角。其中与基本旋转顺序有关，若顺序不同则所得结果不同。

2 欧拉角姿态解算方法

设载体坐标系b系相对地理坐标系n系的角速度为wnb，再次简记为w，由wnb在载体坐标系的分量可得：

故而当前时刻欧拉角可由下式获得：

3 基于扩展卡尔曼滤波器的四元数姿态解算算法

基于卡尔曼滤波器的四元数姿态解算算法是采用扩展卡尔曼方法，将四元数[q0 q1 q2 q3]T作为状态量，将加速度计和磁力计的测量作为观测量，以陀螺测得数据来估计姿态四元数，以加速度计和磁力计测得数据来修正四元数估计量。其中，加速度计信息修正侧重水平方向，磁力计信息修正侧重航向。以下给出该法具体推导。

以四元数与陀螺输出关系建立状态方程，由四元数与角速率关系式进一步可得到状态方程：

将上式离散化后，得扩展卡尔曼一步预测方程：

4 仿真及分析

4.1 仿真条件设置

仿真采用MPU6050惯性测量芯片输出的加速度和角速度做为采样值，考虑静止状态MPU的角速度和加速度输出。采样间隔为10ms，仿真时间为1h。得到陀螺输出数据和加速度计输出数据后解算出俯仰角与横滚角。

4.2 仿真结果

图1为解算出的俯仰角与横滚角。分析可知采用基于扩展卡尔曼滤波器的四元数车辆姿态解算算法，当载体静止或匀速时，可以将姿态稳定在1°以内，同时，由于汽车行驶时，加速度变化不是非常明显。故该方法适用于对汽车姿态检测。实际系统验证也证明了该法用于汽车姿态检测的可行性。

5 结语

本文采用了基于扩展卡尔曼滤波器的四元数姿态解算算法，在静止或匀速时能够将解算精度控制在1°以内。可以将该算法应用于行驶车辆获得其姿态信息从而达到对车辆的实时监控。

参考文献

[1]张金.基于ARM的车辆姿态测量系统设计[D].北京交通大学，2008.5.

[2]戚永刚.基于卡尔曼滤波的城市路口车辆检测及分类[J].电视技术，2013，37（19）：241-245.