[发明专利]一种用于IMU系统的姿态自适应校正方法

说明书

本发明提供一种用于IMU系统的姿态自适应校正方法，包括：获取系统姿态的状态方程、加速度计量测方程和磁强计量测方程；获取系统状态方程的2n+1个采样点、加速度计和磁强计量测方程的n+2个采样点；将获取的采样点对应输入系统的状态方程，加速度计量测方程和磁强计量测方程，进行状态预测；根据状态预测值获取量测预测残差；根据量测预测残差获取加速度计自适应校正因子和磁强计自适应校正因子判断自适应校正因子之间的大小对系统姿态进行自适应校正。本发明所述的用于IMU系统的姿态自适应校正方法具有计算效率高，精确等特点，可广泛应用于姿态参数估计领域。

技术领域

本发明涉及姿态参数估计领域，特别是涉及一种用于IMU(Inertial MeasurementUnit，惯性测量单元)系统的姿态自适应校正方法。

背景技术

传统的姿态校正方法是通过卡尔曼滤波的方法利用加速度计或磁强计对陀螺仪输出的系统姿态进行校正，适用于线性系统，但在煤矿井下定向钻进场合大多都为非线性系统，传统的方法并不适用。对于非线性系统，扩展卡尔曼滤波(EKF)则是对将非线性系统线性化，再进行卡尔曼滤波。但是在进行线性化的过程中忽略了其余高阶项，会引入较大误差，并且处理非线性较强的动态系统时，可能会导致滤波发散。无迹卡尔曼滤波的提出利用概率分布来逼近非线性，可达到EKF的二阶精度，被广泛的应用于航天和无人驾驶等姿态参数估计领域。然而在许多环境下，由于UKF需要的采样数据繁多，会导致算法效率较低。同时，在数值计算过程中状态误差协方差也可能会出现负定情况而无法抑制算法发散。

由此可见，在现有技术中，无迹卡尔曼滤波还存在计算效率低和算法发散等问题。

发明内容

有鉴于此，本发明的主要目的在于提供一种用于IMU系统的姿态自适应校正方法。

为了达到上述目的，本发明提出的技术方案为：

一种用于IMU系统的姿态自适应校正方法，包括如下步骤：

步骤1、根据IMU(Inertial Measurement Unit，惯性测量单元)系统陀螺仪输出角速度计算陀螺仪姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统状态方程；根据IMU系统加速度计输出加速度和IMU系统磁强计输出磁场，分别建立系统加速度计量测方程和系统磁强计量测方程。

步骤2、根据UT(Unscented Transformation，无迹变换)形式对系统状态方程采样2n+1个点，对系统加速度计量测方程按照超球体单行采样规则采样n+2个点，对系统磁强计量测方程按照超球体单行采样规则采样n+2个点，其中，n为系统状态方程中系统状态向量的维数，且n为自然数。

步骤3、将步骤2获取的2n+1个采样点输入系统状态方程，对系统状态进行预测，得到第k时刻系统状态预测值，并获取第k时刻系统状态预测误差协方差；同时，将第k时刻系统状态预测值、加速度计n+2个采样点、磁强计n+2个采样点，对应输入系统加速度计量测方程、系统磁强计量测方程，得到第k时刻加速度计量测预测值和第k时刻磁强计量测预测值；其中，第k时刻为当前时刻。

步骤4、根据第k时刻加速度计量测预测值、第k时刻磁强计量测预测值和加速度计输出值、磁强计输出值，获取加速度计量测预测残差和磁强计量测预测残差。

步骤5、根据加速度计量测预测残差和磁强计量测预测残差，获取加速度计自适应校正因子和磁强计自适应校正因子其中，表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子，表示从第k-1时刻到第k时刻磁强计的量测残差自适应校正因子，ΔA^a为第k和第k-1时刻加速度计输出值差值，ΔH^h为第k和第k-1时刻磁强计输出值差值。

步骤6、判断步骤5得到的加速度计自适应校正因子与磁强计自适应校正因子之间的大小：当加速度计自适应校正因子大于磁强计自适应校正因子时，则执行步骤7；当磁强计自适应校正因子大于加速度计自适应校正因子时，则执行步骤8。