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

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

Synchronization estimation algorithm for attitude algorithm and external force acceleration

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/s². The method can effectively restrain the influence of the external force acceleration on the attitude algorithm, and accurately estimate the external force.