[发明专利]一种基于IMU的自适应EKF姿态测量改进方法

说明书

本发明公开了一种基于IMU的自适应EKF姿态测量改进方法，包括以下步骤：S1、建立拓展卡尔曼滤波模型；S2、求取第一级量测噪声方差阵；S3、求取第二级量测噪声方差阵；S4、通过步骤S2、S3得到总量测噪声方差阵；S5、将总量测噪声方差阵代入步骤S1得到的拓展卡尔曼滤波模型中即可。本发明以拓展卡尔曼滤波模型为基本框架，对陀螺仪和加速度计输出数据进行了数据融合，通过对量测噪声的两级自适应处理来提高方法精度。通过仿真实验表明，在物体动态姿态运动过程中，本发明中的方法优于标准Sage‑Husa自适应滤波方法，具有较好的鲁棒性，能有效提高姿态测量精度。

技术领域

本发明涉及机器人运动学领域，尤其涉及一种基于IMU的自适应EKF姿态测量改进方法。

背景技术

机器人运动学参数标定是建模、测量、辨识、补偿的集成过程。其末端位姿测量精度的准确性直接影响到后续参数辨识的结果，进而影响最终标定结果的准确性。位姿由位置和姿态两部分组成，其中姿态的测量可通过多种方法实现，例如：惯性测量单元(Inertial Measurement Unit,IMU)、光学姿态测量系统、GPS姿态测量系统等。而IMU有着其他姿态测量系统无法比拟的如下优势：不受气候条件、空间条件限制，方便携带，相对成本低，适用于对测量精度、动态性能、实时性均有较高要求的领域，更适合在实际加工项目中推广。IMU一般由陀螺仪、加速度计(磁力计)组成。

然而，IMU在进行机器人动态姿态测量时,发现了其加速度计信号中存在有害加速度(除重力加速度之外的其他加速度)信号叠加、噪声统计特性参数不易获取、陀螺仪信号随时间发生漂移等影响测量精度的问题。

史露强等提出用互补滤波进行数据融合，选定量子粒子优化群方法作为滤波器的参数寻优方法。S.Sabatelli等提出用拓展卡尔曼滤波进行多传感器数据融合的姿态测量方法。Yun X等结合高斯牛顿迭代法和EKF进行参数优化。Madgwick SOH等用梯度下降法代替了高斯牛顿迭代法，再通过EKF进行参数优化。梁松等使用Sage-Husa自适应滤波方法自适应估计系统噪声和量测噪声，提高姿态求解精度。以上方法对提高姿态测量精度都有一定效果，但在姿态动态测量过程中，以上方法要么没有针对有害加速度叠加的处理，要么存在计算复杂、稳定性差等不足。因此，有必要提供一种提高测量精度的方法来解决上述问题。

发明内容

本发明目的是针对上述问题，提供一种测量精度更高的基于IMU的自适应EKF姿态测量改进方法。

为了实现上述目的，本发明的技术方案是：

一种基于IMU的自适应EKF姿态测量改进方法，包括以下步骤：

S1、建立拓展卡尔曼滤波模型；

S2、求取第一级量测噪声方差阵；

S3、求取第二级量测噪声方差阵；

S4、通过步骤S2、S3得到总量测噪声方差阵；

S5、将总量测噪声方差阵代入步骤S1得到的拓展卡尔曼滤波模型中即可。

进一步的，所述步骤S1中建立拓展卡尔曼滤波模型包括以下步骤：

S11、构建非线性系统方程，其计算公式为：

式中：q_k是系统状态向量；y_k是量测向量；f(·)和h(·)均为非线性向量函数；B_k-1是已知的系统结构参数；W_k-1是系统噪声，v_k是量测噪声；

S12、根据捷联式惯性导航系统理论，得到四元数的微分方程，其计算公式为：