[发明专利]一种基于姿态运动学模型的惯性测量单元数据融合方法

说明书

本发明公开了一种基于姿态运动学模型的惯性测量单元数据融合方法，涉及姿态测量传感器数据融合技术领域，其技术方案要点是：将运动载体的惯性测量单元实时输出的陀螺仪测量值、加速度计测量值和磁力计测量值进行归一化处理；根据陀螺仪误差模型对标准陀螺仪测量值进行校正；根据惯性测量单元的量测模型对标准加速度计测量值和标准磁力计测量值进行分层解算获得参考载体四元数姿态；将所得的参考四元数姿态和校正陀螺仪数据输入融合观测器获得姿态估计值；对姿态估计值进行误差评估，进一步获得姿态融合值和估计角速度。本发明最后给出的仿真实例表明，所设计的姿态融合方法可以有效、精确的估计出载体的姿态值和角速度值。

技术领域

本发明涉及姿态测量传感器数据融合技术领域，更具体地说，它涉及一种基于姿态运动学模型的惯性测量单元数据融合方法。

背景技术

姿态测量传感器包括陀螺仪，磁力计和加速度计。多个姿态测量传感器组成的测量系统也被称为惯性测量单元，常见的惯性测量单元一般由三轴陀螺仪、三轴加速度计和三轴磁力计组成。单个姿态测量传感器可以用来获取载体的部分的姿态信息。具体来说，加速度计通过重力矢量在特定方向的夹角测量相关的姿态来获得运动载体的加速度信息，通过相应的旋转矩阵计算可以获得运动载体的姿态信息，其测量值不受时间的影响，动态性能较好，但由于加速度计z轴与重力加速度方向平行，这就导致无法获得准确的z轴姿态数据。磁力计与加速度计类似，在x轴和y轴方向的测量数据是不准确的，且容易受外部环境的干扰，短时间的测量值容易产生误差。陀螺仪通过陀螺效应测量角速度的，通过内部陀螺与初始方向的偏移计算角速度和旋转方向，再通过进一步积分得到姿态信息，但是由于计算姿态信息过程涉及到积分环节，陀螺仪测量的姿态数据一直会带有累计误差。

在进行姿态测量的时候，靠单一的姿态测量传感器获得的测量数据解算出来的姿态信息精度是很低的。例如，仅靠加速度计或磁力计获得的姿态信息，只能测某一方位的姿态值，不具备参考价值。而仅靠陀螺仪获得的姿态信息，会在长时间的计算中产生累计误差，同样不具备参考价值。

在现有技术中，针对多姿态测量传感器的数据融合获得更高精度的姿态数据，一般采用对姿态传感器的误差特性进行分析补偿的方法，例如卡尔曼滤波[刘颖,杨媛,徐金涛.融合互补滤波和卡尔曼滤波的姿态解算算法[J].西安邮电大学学报,2021,26(04):98-104.]，通过将测量传感器的噪声建模为高斯噪声，估计测量传感器的内部偏差，融合惯性测量单元进一步获得姿态数据。但是，卡尔曼滤波并没有考虑非线性姿态运动学模型，并且无法处理非高斯噪声，会制约融合精度的提升。

因此，本发明旨在设计提供一种基于姿态运动学模型的惯性测量单元数据融合方法，以解决上述问题。

发明内容

本发明的目的是为了解决上述问题，提供一种基于姿态运动学模型的惯性测量单元数据融合方法，本发明以四元数作为姿态表示方式，基于刚体的姿态运动学模型，设计姿态观测器融合惯性测量单元的测量数据，进行融合误差分析得到高精度的姿态融合数据和角速度估计数据，提高载体在姿态变化时候惯性测量单元解算姿态的精度，为载体进行稳定高效的姿态控制提供保障。

本发明的上述技术目的是通过以下技术方案得以实现的：一种基于姿态运动学模型的惯性测量单元数据融合方法，包括以下步骤：

S1、获取当前时刻载体惯性测量单元的测量数据，并进行归一化处理；

S2、基于陀螺仪误差模型对惯性测量单元的陀螺仪测量数据进行实时校正；

S3、基于惯性测量单元的量测模型，将加速度计和磁力计的测量数据分层解算成实时参考四元数姿态；

S4、针对刚体姿态运动学设计融合观测器，将步骤S2中所得的校正陀螺仪测量数据和步骤S3中所得的参考四元数姿态输入融合观测器，得到实时姿态融合估计数据；

S5、对步骤S4所得的实时姿态融合估计数据进行姿态运动学误差评估，更新下一时刻姿态融合值和角速度估计值，并返回步骤S1。