[发明专利]一种基于多传感器融合的运动体姿态解算方法

摘要

本发明公开一种基于多传感器融合的运动体姿态解算方法，将微机电惯性及磁测量单元固定在待测载体上，采集加速度、角速率和磁感应强度，根据加速度计的量测模型建立第一级损失函数，利用加速度通过梯度下降法估计当前姿态四元数，并将其与通过角速率更新的姿态四元数加权，得到第一级后验姿态四元数利用更新载体的俯仰角和横滚角；在考虑铁磁干扰的情况下根据磁力计的量测模型建立第二级损失函数，利用上一时刻计算得到的铁磁干扰估计量和当前时刻磁感应强度通过梯度下降法估计当前姿态四元数，并将其与通过角速率更新的姿态四元数加权得到第二级后验姿态四元数利用更新载体的偏航角，本发明能对外界铁磁干扰进行估计实现对铁磁干扰的补偿。

主权项

1.一种基于多传感器融合的运动体姿态解算方法，其特征在于，将微机电惯性及磁测量单元固定在待测载体上，采集载体加速度、角速率和磁感应强度数据；根据加速度计的量测模型建立第一级损失函数F₁(·)，利用加速度数据y_A，t通过梯度下降法估计当前姿态四元数并将其与通过角速率数据y_G，t更新的姿态四元数加权，得到第一级后验姿态四元数利用更新载体的俯仰角pitch_A和横滚角roll_A；在考虑铁磁干扰的情况下根据磁力计的量测模型建立第二级损失函数F₂(·)，利用上一时刻计算得到的铁磁干扰估计量和当前时刻磁感应强度数据y_M，t通过梯度下降法估计当前姿态四元数并将其与通过角速率数据y_G，t更新的姿态四元数加权，得到第二级后验姿态四元数利用更新载体的偏航角yaw_M。