一种微惯性与磁组合测量单元的姿态解算方法

An Attitude Determination Algorithm Based on MEMS Inertial and Magnetic Measurement Unit

目前无人机、机器人、人体动作捕捉等多个领域都需要全姿态测量方法支持,采用微惯性与磁传感器组合测量姿态是成本低、性能好、普遍使用的一种方案。针对磁力计易受外界铁磁干扰问题,提出一种基于旋转四元数、两级更新的姿态解算方法:第1级根据建立的陀螺仪、加速度计量测模型构建损失函数,采用梯度下降法对载体俯仰角和横滚角进行更新;第2级根据建立的磁力计量测模型,采用梯度下降法对载体偏航角进行更新。利用后验估计四元数计算铁磁干扰估计量,并将其作为下一时刻的先验铁磁干扰。实验结果表明,该算法可以有效校正角速率积分带来的累积误差,提高姿态解算精度,并具有一定抗铁磁干扰能力,增强了微惯性与磁组合测量单元姿态解算方法的环境适应性。

Full-attitude determination methods are needed in many fields,such as UAV,robot,and human motion capture. Using MEMS inertial/magnetic sensors to determine the attitude of a object is a low cost,high performance,and commonly used scheme in these fields. To alleviate the problem that the magnetometer is susceptible to external ferromagnetic interference,the paper proposes a two-stage quaternion-based attitude update method: in the first stage,a cost function is constructed by establishing the gyroscope and accelerometer measurement models,which can be optimized by gradient descent to update the pitch and roll angles;in the second stage,a cost function is constructed by using the magnetometer measurement model,which can be optimized by gradient descent to update the yaw angle. The posteriori estimated quaternion is used to calculate the estimated ferromagnetic interference is regarded as the priori ferromagnetic interference at the next step. The experimental result shows that the proposed method can be used to effectively correct the cumulative error caused by angular rate integration and improve the attitude determination accuracy,and has the capability of anti-ferromagnetic interference,which can improve the environmental adaptability of the attitude determination system.