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

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

【技术实现步骤摘要】
一种基于自适应参数互补滤波的机器人姿态融合方法
本专利技术属于机器人
，具体涉及一种基于自适应参数互补滤波的机器人姿态融合方法。
技术介绍
随着微机电系统(MEMS)传感器技术和计算机技术的日新月异，MEMS惯性测量单元(IMU)逐渐向着低成本、低功耗、体积小、质量轻、可靠高的趋势快速发展，被广泛应用于移动机器人、无人机定位导航等领域中，旨在获取准确的姿态信息。为了在低成本的IMU上满足姿态解算精度更高、可靠性更高、环境自适应能力更强等性能指标，需要一种高效的IMU姿态解算算法。IMU主要由加速度计、陀螺仪、磁力计三个基本元件组成，由于单个传感器自身固有特性，其输出信号往往存在着噪声和误差，传感器获得的姿态角原始数据信息不能直接使用，IMU姿态融合算法就是利用这三个元件的数据加以解算获得更准确的姿态信息。目前，国外学者已研发出了很多IMU姿态融合算法，应用较为广泛的有Mahony互补滤波、EKF融合、Madgwick融合算法等。其中，互补滤波算法，通过把陀螺仪的高频部分和磁力计或加速度计的低频部分叠加在一起，得到更准确更稳定的姿态，该算法简单可靠，能够有效滤除噪声和抑制漂移，并且对惯性器件IMU的精度要求较低，在无人机、轮式机器人等姿态解算中的应用愈加广泛；扩展卡尔曼算法(EKF)，是一种高精度、应用广泛的姿态解算算法，作为一种对非线性系统的滤波方法，能够在处理过程中不断对姿态进行预测和修正，具有权重参数动态调整的特点；Madgwick融合算法利用梯度下降的核心思想，将加速度计和磁力计通过梯度下降得到的姿态四元数与由陀螺仪积分得到的姿态，进行线性融合得到最优的姿态，该算法具有计算量小、低频有效的特点，能够显著降低对处理器速度的要求。然而，互补滤波融合算法是建立在加速度计平稳运行的条件上，如果移动机器人等在场景中快速运动时，姿态解算的误差较大，互补滤波融合算法就显得力不从心；EKF算法计算量相对较大，当线性化假设不成立时直接线性化会导致滤波器极度不稳定，并且在实际应用中解算的姿态角易受噪声干扰；Madgwick梯度下降融合算法，只有当算法的收敛速度快于移动机器人的运动速度，该算法才可以实时跟踪到姿态，所以对于机器人高速运动时该算法会失效，此外，地磁计的误差也容易影响其姿态精度。因此，对于IMU广泛应用的自动驾驶、无人飞行等领域，一种能在高速运动下有效处理的IMU姿态解算方案，获取得到更准确的位姿信息，是极为重要并且具有较大实用价值。
技术实现思路
本专利技术的目的是：旨在提供一种基于自适应参数互补滤波的机器人姿态融合方法，用于解决机器人在场景中大机动运动的姿态角度误差的问题。为实现上述技术目的，本专利技术采用的技术方案如下：一种基于自适应参数互补滤波的机器人姿态融合方法，包括如下步骤：A1、对加速度值进行预处理并选取频率在1-100Hz的分量，依据滤波后的加速度计算加速度和磁力计的置信度；A2、计算加速度与标准重力向量的误差以及磁力计与NED坐标系下的方向向量误差；A3、结合置信度和误差向量对陀螺仪测量值进行矫正；A4、结合四元数微分方程对姿态向量进行更新。进一步，步骤A1的具体实现如下：A101、读取三轴加速度am＝[axayaz]和三轴角速度数据wm＝[wxwywz]，以及三轴磁力计数据mm＝[mxmymz]；A102、对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量af，并利用衰减函数计算加速度测量值的置信度K(t)：af＝highpass1:100Hz(am)其中A表示衰减系数，可根据实际系统进行设置。进一步，步骤A2具体实现如下：A201、计算加速度测量值与标准参考重力[0,0,g]之间的误差eaea＝am×av其中，A202、计算磁力计测量值与参考磁力方向向量之间的误差em进一步，步骤A3的具体实现如下：A301、由加速度向量误差、磁力计向量误差对陀螺仪测量值wm进行动态修正eInt＝eInt+e*Ki*0.5*Tw＝wm+eInt+K(t)*e其中e＝ea+em。进一步，步骤A4的具体实现如下：A401、利用四元数微分方程更新姿态四元数：qk+1＝(I+Omega(w)*0.5*T)qk其中，本专利技术还包括如下步骤：B1、初始化系统变量：根据采集到的加速度和磁力计数据计算初始的欧拉角和四元数初始值roll＝atan2(-ay,az)*57.3B2、姿态预测：设系统的状态变量为的状态变量,状态更新方程为：其中：状态转移矩阵:Φ＝I+F*dt,而四元数q部分的状态更新情况，即状态转移矩阵的前四行四列为：B3、构建观测方程：观测部由加速度计和磁力计对重力向量提供观测，加速度部分参考值则是将标准的重量加速度[0,0,g]T和磁力计投影到机体坐标系的值与加速度测量值构建误差向量加速度误差向量ea为：磁力计部分的观测方程有：其中am＝[axayaz]和mm＝[mxmymz]分别为加速度和磁力计的测量值，磁力计部分的误差向量为：其中mb＝[bxbybz]，因此系统的误差向量e写为：当系统状态变量为时，将误差向量对系统状态变量求导，计算得到系统的观测矩阵HX为：B4、计算加速度置信度：对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量af，通过af分量来判断加速度运动剧烈程度，动态调整加EKF滤波器观测噪声的值R(t)，以达到实时调整加速度融合的权重，实现精准的位姿估计，其中，af＝highpass1:100Hz(am)B5、EKF更新姿态融合：K＝Pk|k-1HT(HPk|k-1HT+R(t))-1Pk|k＝(I-KH)Pk|k-1xk|k-1＝k|k-1+Kr。采用上述技术方案的专利技术，具有如下优点：1、本专利技术方法通过降低加速度计的权重，使得系统陀螺仪积分值信任程度增加，减小输出角度的误差；2、本专利技术方法基于置信度函数动态修正加速度和磁力计传感器融合权重，提升IMU在大机动运动时姿态计算精度和鲁棒性；3、本专利技术融合方法使得IMU具有更高精度、抗干扰能力更强，有利于低成本IMU的实际应用。附图说明本专利技术可以通过附图给出的非限定性实施例进一步说明；图1为本专利技术融合方法流程图；图2为本专利技术平稳运动条件下加速度和角速度测量值一；图3为本专利技术平稳运动条件下改进算法对比原始算法输出的欧拉角曲线一；图4为本专利技术大机动运动本文档来自技高网...

【技术保护点】
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