一种基于自适应参数互补滤波的机器人姿态融合方法技术

本发明专利技术属于机器人技术领域，主要涉及一种基于自适应参数互补滤波的机器人姿态融合方法，包括如下步骤：A1、对加速度值进行预处理并选取频率在1‑100Hz的分量，依据滤波后的加速度计算加速度和磁力计的置信度；A2、计算加速度与标准重力向量的误差以及磁力计与NED坐标系下的方向向量误差；A3、结合置信度和误差向量对陀螺仪测量值进行矫正；A4、结合四元数微分方程对姿态向量进行更新。本发明专利技术方法基于置信度函数动态修正加速度和磁力计传感器融合权重，有效提升机器人IMU在大机动运动时姿态计算精度和鲁棒性。

【技术实现步骤摘要】
一种基于自适应参数互补滤波的机器人姿态融合方法
本专利技术属于机器人
，具体涉及一种基于自适应参数互补滤波的机器人姿态融合方法。
技术介绍
随着微机电系统(MEMS)传感器技术和计算机技术的日新月异，MEMS惯性测量单元(IMU)逐渐向着低成本、低功耗、体积小、质量轻、可靠高的趋势快速发展，被广泛应用于移动机器人、无人机定位导航等领域中，旨在获取准确的姿态信息。为了在低成本的IMU上满足姿态解算精度更高、可靠性更高、环境自适应能力更强等性能指标，需要一种高效的IMU姿态解算算法。IMU主要由加速度计、陀螺仪、磁力计三个基本元件组成，由于单个传感器自身固有特性，其输出信号往往存在着噪声和误差，传感器获得的姿态角原始数据信息不能直接使用，IMU姿态融合算法就是利用这三个元件的数据加以解算获得更准确的姿态信息。目前，国外学者已研发出了很多IMU姿态融合算法，应用较为广泛的有Mahony互补滤波、EKF融合、Madgwick融合算法等。其中，互补滤波算法，通过把陀螺仪的高频部分和磁力计或加速度计的低频部分叠加在一起，得到更准确更稳定的姿态本文档来自技高网...

【技术保护点】
1.一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，包括如下步骤：/nA1、对加速度值进行预处理并选取频率在1-100Hz的分量，依据滤波后的加速度计算加速度和磁力计的置信度；/nA2、计算加速度与标准重力向量的误差以及磁力计与NED坐标系下的方向向量误差；/nA3、结合置信度和误差向量对陀螺仪测量值进行矫正；/nA4、结合四元数微分方程对姿态向量进行更新。/n

【技术特征摘要】
1.一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，包括如下步骤：
A1、对加速度值进行预处理并选取频率在1-100Hz的分量，依据滤波后的加速度计算加速度和磁力计的置信度；
A2、计算加速度与标准重力向量的误差以及磁力计与NED坐标系下的方向向量误差；
A3、结合置信度和误差向量对陀螺仪测量值进行矫正；
A4、结合四元数微分方程对姿态向量进行更新。


2.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，步骤A1的具体实现如下：
A101、读取三轴加速度am＝[axayaz]和三轴角速度数据wm＝[wxwywz]，以及三轴磁力计数据mm＝[mxmymz]；
A102、对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量af，并利用衰减函数计算加速度测量值的置信度K(t)：
af＝highpass1:100Hz(am)



其中A表示衰减系数，可根据实际系统进行设置。


3.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，步骤A2具体实现如下：
A201、计算加速度测量值与标准参考重力[0,0,g]之间的误差ea
ea＝am×av
其中，
A202、计算磁力计测量值与参考磁力方向向量之间的误差em








4.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，步骤A3的具体实现如下：
A301、由加速度向量误差、磁力计向量误差对陀螺仪测量值wm进行动态修正
eInt＝eInt+e*Ki*0.5*T
w＝wm+eInt+K(t)*e
其中e＝ea+em。


5.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，步骤A4的具体实现如下：
A401、利用四元数微分方程更新姿态四元数：
qk+1＝(I+Omega(w)*0.5*T)qk

【专利技术属性】
技术研发人员：江涛，岑汝平，薛方正，苏晓杰，
申请(专利权)人：重庆大学，
类型：发明
国别省市：重庆;50