基于卡尔曼滤波和互补滤波的改进型姿态解算方法

Improved Attitude Solving Method Based on Kalman Filter and Complementary Filtering

四元数融合互补滤波通常采用三轴陀螺仪的角速度积分来获取角度,进而利用角度求得四元数,再由四元数解算出姿态角。但三轴陀螺仪由角速度积分得到的角度由于温漂、单次迭代等原因,往往偏差较大,难以消除,这就导致求得的四元数精度不够,最终影响互补滤波解算出的姿态角的精度。针对这个问题,提出采用卡尔曼滤波融合陀螺仪、加速度计的数据进行基于误差协方差最小的迭代估计,并通过对过程噪声和观测噪声的滤波,最终得到姿态角的最优估计,再将这个估计值代入互补滤波中求得四元数,利用该四元数进行误差负增益调节。本工作基于STM32F4搭建实验平台进行验证,结果表明:该改进型姿态解算方法明显地提高了姿态角的精度,具有良好的动态和静态特性。

The quaternion fusion complementary filter usually uses the angular velocity integral of the three-axis gyroscope to obtain the angle,and then uses the angle to obtain the quaternion,and then the quaternion solution calculates the attitude angle.However,the angle obtained by the angular velocity integral of the three-axis gyroscope tends to be large and difficult to eliminate due to temperature drift,single iteration,etc.,which leads to insufficient accuracy of the obtained quaternion,and finally affects the attitude angle calculated by the complementary filtering solution.Aiming at this problem,it is proposed to use Kalman filter fusion gyroscope and accelerometer data to perform iterative estimation based on minimum error covariance,and filter the process noise and observation noise,and finally obtain the optimal estimation of the attitude angle.The value is substituted into the complementary filter to obtain the quaternion,and the method of calculating the attitude angle is solved.Based on the STM32F4 construction experimental platform,the results show that the improved attitude settlement method significantly improves the accuracy of the attitude angle and has good dynamic and static characteristics.