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

说明书

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

技术领域

本发明属于微机电惯性及磁测量单元(MIMMU)的动作捕捉领域，具体涉及一种基于多传感器融合的运动体姿态解算方法。

背景技术

基于微机电惯性及磁测量单元的姿态解算系统由于其自主性强，性价比高，体型小巧的特点，被广泛应用于飞行器、机器人、人体动作捕捉等多个领域。惯性及磁测量单元由三轴陀螺仪、三轴加速度计、三轴磁力计组成，可用于测量载体的角速率、加速度、磁感应强度。由于陀螺仪存在瞬时漂移，在使用角速率积分获得载体姿态的过程中，系统误差会随时间累积，长时间工作很可能造成发散。而利用载体坐标系下重力加速度分量只能解算出载体的横滚角与俯仰角，利用载体坐标系下地球磁场分量可以解算出载体航向角，它们的解算只基于当前瞬时数据，所以其误差不随时间积累。因此使用惯性及磁测量单元，实现基于磁感应强度、角速率、重力加速度(Magnetic、Angular Rate and Gravity， MARG)数据的信息融合共同解算载体姿态的算法，可以对横滚角、俯仰角、航向角进行有效修正，既能有效提高解算精度，又使系统可以长时间工作。基于最优化补偿的MARG组合姿态测量方法，相比于卡尔曼滤波法计算量更小，相比于互补滤波法鲁棒性更强。但目前的最优化补偿算法未考虑外界铁磁物质对于磁力计的干扰，当铁磁干扰较大时该算法无法正常工作。另外它使用同一个滤波器对MIMMU数据进行融合，加速度计数据会对偏航角的解算造成不利影响，磁力计数据会对横滚角和俯仰角的解算造成不利影响。

发明内容

有鉴于此，本发明提供了一种基于多传感器融合的运动体姿态解算方法，能够对外界铁磁干扰进行估计，以实现对铁磁干扰的补偿，确保算法在较复杂的环境下仍能正常使用。

实现本发明的技术方案如下：

一种基于多传感器融合的运动体姿态解算方法，将微机电惯性及磁测量单元(MIMMU)固定在待测载体上，采集载体加速度、角速率和磁感应强度数据，根据加速度计的量测模型建立第一级损失函数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对铁磁干扰估计量进行更新。

进一步地，第二级损失函数F₂(x)＝ε₂(x)^Tε₂(x)，