[发明专利]一种捷联惯性导航的误差校正方法及装置

权利要求书

1.一种捷联惯性导航的误差校正方法，其特征在于，包括：

对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据；

根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率；

将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据；

根据上一次的第二姿态数据与当前的第二姿态数据，计算第二姿态变化率；

跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，并通过所述第三姿态数据初始化陀螺仪姿态，以校正陀螺仪积分误差，其中，所述姿态可信度由所述偏差率及修正参数计算获得。

2.根据权利要求1所述的捷联惯性导航的误差校正方法，其特征在于，对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据，包括：

计算第一磁力计矢量及加速度计矢量；

计算水平矢量，所述水平矢量等于所述第一磁力计矢量乘以所述加速度计矢量；

计算第二磁力计矢量，所述第二磁力计矢量等于所述加速度计矢量乘以所述水平矢量；

根据所述水平矢量、所述第二磁力计矢量及所述加速度计矢量，计算第一姿态矩阵；

根据所述第一姿态矩阵，计算终端的第一姿态数据。

3.根据权利要求1所述的捷联惯性导航的误差校正方法，其特征在于，所述根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率，包括：

获取终端的角度轴数据，并进行归一化；

根据上一次的第一姿态数据与当前的第一姿态数据，计算角度差值；

根据归一化的所述角度轴数据、所述角度差值、上一次的第一姿态数据及当前的第一姿态数据，计算第一姿态变化率。

4.根据权利要求1所述的捷联惯性导航的误差校正方法，其特征在于，所述将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据，包括：

根据所述第一姿态数据，计算第二姿态矩阵；

计算陀螺仪的四元数，并根据所述四元数进行陀螺仪姿态解算，以获取第三姿态矩阵；

根据所述第三姿态矩阵对所述第二姿态矩阵进行更新，以获取第二姿态数据。

5.根据权利要求1所述的捷联惯性导航的误差校正方法，其特征在于，所述跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，包括：

根据所述第一姿态变化率、所述第二姿态变化率和姿态偏差率，计算偏差率；

跟踪偏差率的周期性变化，当所述偏差率达到极大值或极小值时，根据所述偏差率及修正参数，计算姿态可信度；

根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，进行融合解算，以获取第三姿态数据。

6.一种捷联惯性导航的误差校正装置，其特征在于，包括：

第一姿态计算模块，用于对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据；

第一姿态变化率计算模块，用于根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率；

第二姿态计算模块，用于将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据；

第二姿态变化率计算模块，用于根据上一次的第二姿态数据与当前的第二姿态数据，计算第二姿态变化率；

校正模块，用于跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，并通过所述第三姿态数据初始化陀螺仪姿态，以校正陀螺仪积分误差，其中，所述姿态可信度由所述偏差率及修正参数计算获得。