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

权利要求书

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

将更新后的偏航角yaw_M、俯仰角pitch_A和横滚角roll_A转换成姿态四元数，即当前时刻后验姿态四元数基于磁力计的量测模型，利用后验姿态四元数和磁感应强度数据y_M，t对铁磁干扰估计量进行更新。

2.如权利要求1所述的一种基于多传感器融合的运动体姿态解算方法，其特征在于，第二级损失函数F₂(x)＝ε₂(x)^Tε₂(x)，

其中，x＝[q d]，y_M，t为t时刻磁力计所测的磁感应强度数据，d为铁磁干扰，q表示载体姿态，表示载体坐标系下所观测的当地地球磁场。