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

权利要求书

1.一种基于姿态运动学模型的惯性测量单元数据融合方法，其特征是：包括以下步骤：

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

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

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

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

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

2.根据权利要求1所述的一种基于姿态运动学模型的惯性测量单元数据融合方法，其特征是：步骤S1的具体方法为：

S11、归一化三轴加速度计实时测量数据：

载体姿态运动的k时刻，加速度计的测量数据为a_g(k)＝[a_x(k),a_y(k),a_z(k)]^T，通过下式对其进行归一化得到实时标准化加速度计数据：

其中，v_ng(k)＝[v₁(k),v₂(k),v₃(k)]^T为k时刻标准加速度计测量数据；

S12、归一化三轴磁力计实时测量数据：

磁力计在k时刻的测量数据为m_g(k)＝[m_x(k),m_y(k),m_z(k)]^T，通过下式对其进行归一化得到实时标准化磁力计数据：

其中，m_ng(k)＝[m₁(k),m₂(k),m₃(k)]^T为k时刻标准磁力计测量数据；

S13、归一化三轴陀螺仪实时测量数据：

陀螺仪在k时刻的测量数据为w_g(k)＝[w_x(k),w_y(k),w_z(k)]^T，通过下式对其进行归一化得到实时标准陀螺仪测量数据：

其中，ω_ng(k)＝[ω_g1(k),ω_g2(k),ω_g3(k)]^T为k时刻标准陀螺仪测量数据。

3.根据权利要求1所述的一种基于姿态运动学模型的惯性测量单元数据融合方法，其特征是：步骤S2的具体方法为：

S21、计算陀螺仪的误差矩阵：

将惯性测量单元单独放置水平面上，将其内部陀螺仪每个轴都相应的转动n圈，通过下式计算陀螺仪的误差矩阵：

其中，E代表陀螺仪的误差矩阵；B_O为陀螺仪输出旋转矩阵；为B_O的逆矩阵；

S22、校正标准陀螺仪的实时测量数据：

利用陀螺仪误差矩阵、比例因子和零点漂移偏差将步骤S13得到的标准陀螺仪实时测量数据进行校正，具体如下：

其中，ω(k)＝[ω₁(k),ω₂(k),ω₃(k)]^T校正后的k时刻陀螺仪测量数据；h_x,h_y,h_z分别为陀螺仪x,y,z轴的输出比例因子；b_x,b_y,b_z为陀螺仪x,y,z轴的零点漂移偏差。