一种基于IMU的自适应EKF姿态测量改进方法技术

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

【技术实现步骤摘要】
一种基于IMU的自适应EKF姿态测量改进方法
本专利技术涉及机器人运动学领域，尤其涉及一种基于IMU的自适应EKF姿态测量改进方法。
技术介绍
机器人运动学参数标定是建模、测量、辨识、补偿的集成过程。其末端位姿测量精度的准确性直接影响到后续参数辨识的结果，进而影响最终标定结果的准确性。位姿由位置和姿态两部分组成，其中姿态的测量可通过多种方法实现，例如：惯性测量单元(InertialMeasurementUnit,IMU)、光学姿态测量系统、GPS姿态测量系统等。而IMU有着其他姿态测量系统无法比拟的如下优势：不受气候条件、空间条件限制，方便携带，相对成本低，适用于对测量精度、动态性能、实时性均有较高要求的领域，更适合在实际加工项目中推广。IMU一般由陀螺仪、加速度计(磁力计)组成。然而，IMU在进行机器人动态姿态测量时,发现了其加速度计信号中存在有害加速度(除重力加速度之外的其他加速度)信号叠加、噪声统计特性参数不易获取、陀螺仪信号随时间发生漂移等影响测量精度的问题。史露强等提出用互补滤波进行数据融合，选定量子粒子优化群方法作为本文档来自技高网...

【技术保护点】
1.一种基于IMU的自适应EKF姿态测量改进方法，其特征在于：包括以下步骤：/nS1、建立拓展卡尔曼滤波模型；/nS2、求取第一级量测噪声方差阵；/nS3、求取第二级量测噪声方差阵；/nS4、通过步骤S2、S3得到总量测噪声方差阵；/nS5、将总量测噪声方差阵代入步骤S1得到的拓展卡尔曼滤波模型中即可。/n

【技术特征摘要】
1.一种基于IMU的自适应EKF姿态测量改进方法，其特征在于：包括以下步骤：
S1、建立拓展卡尔曼滤波模型；
S2、求取第一级量测噪声方差阵；
S3、求取第二级量测噪声方差阵；
S4、通过步骤S2、S3得到总量测噪声方差阵；
S5、将总量测噪声方差阵代入步骤S1得到的拓展卡尔曼滤波模型中即可。


2.如权利要求1所述的基于IMU的自适应EKF姿态测量改进方法，其特征在于：所述步骤S1中建立拓展卡尔曼滤波模型包括以下步骤：
S11、构建非线性系统方程，其计算公式为：



式中：qk是系统状态向量；yk是量测向量；f(·)和h(·)均为非线性向量函数；Bk-1是已知的系统结构参数；Wk-1是系统噪声，vk是量测噪声；
S12、根据捷联式惯性导航系统理论，得到四元数的微分方程，其计算公式为：



式中：表示b系下b系相对于n系的三轴陀螺仪实时角速度；符号表示四元数乘法；
将式(9)转换为矩阵形式，其计算公式为：






式中，为b系相对于n系的角速度矩阵；
S13、采用毕卡逐次逼近法取有限项，得四元数一阶近似求解方法，计算系统的先验估计，其计算公式为：



计算得到状态方程中的雅克比矩阵，其计算公式为：



式中，Δt为采样时间；式(12)给出了系统的先验估计，因此系统状态上标有“-”，“-”表示系统实际状态未知，卡尔曼滤波器提供了一个估计值；
S15、计算先验噪声均方误差，其计算公式为：



式中：Pk-1为前一次滤波迭代的后验噪声均方误差；Qk-1为系统噪声方差；
S16、n系下，加速度计水平静止摆放时，三轴加速度用向量表示为[axayaz]T＝[...

【专利技术属性】
技术研发人员：任国营，班朝，郑庆国，裴丽梅，崔剑秋，郭斯邑，位恒政，王为农，
申请(专利权)人：中国计量科学研究院，
类型：发明
国别省市：北京;11