计算机应用 ›› 2016, Vol. 36 ›› Issue (5): 1469-1474.DOI: 10.11772/j.issn.1001-9081.2016.05.1469

• 行业与领域应用 • 上一篇    

姿态解算与外力加速度同步估计算法

孟唐宇1, 浦剑涛2, 方建军2, 梁岚珍1,2   

  1. 1. 新疆大学 电气工程学院, 乌鲁木齐 830047;
    2. 北京联合大学 自动化学院, 北京 100101
  • 收稿日期:2015-11-06 修回日期:2015-12-31 出版日期:2016-05-10 发布日期:2016-05-09
  • 通讯作者: 梁岚珍
  • 作者简介:孟唐宇(1990-),男,河北衡水人,硕士研究生,主要研究方向:自主移动机器人、惯性导航技术;浦剑涛(1977-),男,江苏溧阳人,讲师,博士,主要研究方向:自主移动机器人、人机交互技术;方建军(1970-),男,湖北罗田人,教授,博士,主要研究方向:特种机器人;梁岚珍(1957-),女,山西岚县人,教授,主要研究方向:计算机控制、嵌入式系统。
  • 基金资助:
    北京市属高等学校高层次人才引进与培养计划项目-长城学者(CIT&TCD20150314)。

Synchronization estimation algorithm for attitude algorithm and external force acceleration

MENG Tangyu1, PU Jiantao2, FANG Jianjun2, LIANG Lanzhen1,2   

  1. 1. School of Electrical Engineering, Xinjiang University, Urumqi Xinjiang 830047, China;
    2. College of Automation, Beijing Union University, Beijing 100101, China
  • Received:2015-11-06 Revised:2015-12-31 Online:2016-05-10 Published:2016-05-09
  • Supported by:
    This work is partially supported by the Importation and Development of High-Caliber Talents Project of Beijing Municipal Institutions(CIT&TCD20150314).

摘要: 针对惯性导航应用中,姿态解算与外力加速度估计互相干扰的问题,提出一种基于四元数和扩展卡尔曼滤波器的姿态解算与外力加速度同步估计算法。首先,利用估计的外力加速度修正传感器加速度数据得到准确的反向重力加速度,再结合地磁场向量通过梯度下降算法解算得到旋转四元数的测量值;其次,构建扩展卡尔曼滤波模型,对旋转四元数和外力加速度进行更新,得到旋转四元数的预测值和外力加速度的预测值;最后,用旋转四元数的测量值和测量得到的加速度数据对预测值通过扩展卡尔曼滤波的方法进行校正,最终得到准确的旋转四元数和参考坐标系下三轴方向上的外力加速度。实验表明,通过扩展卡尔曼滤波同时对姿态和外力加速度估计的方法,能够迅速收敛并准确得机体姿态信息以及外力加速度信息,欧拉角误差为±1.95°,加速度误差为±0.12 m/s2,并且该算法能有效抑制外力加速度对姿态解算的影响,准确估计外力加速度。

关键词: 姿态解算, 四元数, 扩展卡尔曼滤波, 梯度下降法, 外力加速度

Abstract: Aiming at the problem of mutual interference between attitude algorithm and external force acceleration estimation in inertial navigation system, a new method based on quaternion and extended Kalman filter was proposed. Firstly, the acceleration data of the sensor was corrected by using the estimated external force acceleration data to obtain the accurate reverse gravity acceleration, combined with geomagnetic field vector and calculated by the gradient descent algorithm, the rotate quaternions were obtained. Secondly, the extended Kalman filter model was constructed to update the rotate quaternions and external force acceleration, the prediction value of rotate quaternions and the external force were obtained. Finally, the measured values of rotate quaternions and the acceleration data were corrected by Kalman filtering method, the accurate rotate quaternions and the external force acceleration of the three axis directions in reference coordinate system were obtained. The experimental results show that the method for the synchronization estimation of attitude and external force acceleration by extended Calman filter can quickly converge and accurately get the information of the attitude and the external force acceleration, its Euler angle error is ±1.95° and acceleration error is ±0.12 m/s2. The method can effectively restrain the influence of the external force acceleration on the attitude algorithm, and accurately estimate the external force.

Key words: attitude algorithm, quaternion, extended Kalman filter, gradient descent algorithm, external force acceleration

中图分类号: