CN113063416A - 一种基于自适应参数互补滤波的机器人姿态融合方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于机器人技术领域，具体涉及一种基于自适应参数互补滤波的机器人姿态融合方法。

背景技术

随着微机电系统(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、读取三轴加速度a_m＝[a_x a_y a_z]和三轴角速度数据w_m＝[w_x w_y w_z]，以及三轴磁力计数据m_m＝[m_x m_y m_z]；

A102、对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量a_f，并利用衰减函数计算加速度测量值的置信度K(t)：

a_f＝highpass_1:100Hz(a_m)

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

进一步，步骤A2具体实现如下：

A201、计算加速度测量值与标准参考重力[0,0,g]之间的误差e_a

e_a＝a_m×a_v

其中，

A202、计算磁力计测量值与参考磁力方向向量之间的误差e_m

进一步，步骤A3的具体实现如下：

A301、由加速度向量误差、磁力计向量误差对陀螺仪测量值w_m进行动态修正

eInt＝eInt+e*Ki*0.5*T

w＝w_m+eInt+K(t)*e

其中e＝e_a+e_m。

进一步，步骤A4的具体实现如下：

A401、利用四元数微分方程更新姿态四元数：

q_k+1＝(I+Omega(w)*0.5*T)q_k

其中，

本发明还包括如下步骤：

B1、初始化系统变量：

根据采集到的加速度和磁力计数据计算初始的欧拉角和四元数初始值

roll＝atan2(-ay,az)*57.3

B2、姿态预测：

设系统的状态变量为

的状态变量,状态更新方程为：

其中：

状态转移矩阵:Φ＝I+F*dt,而四元数q部分的状态更新情况，即状态转移矩阵的前四行四列为：

B3、构建观测方程：

观测部由加速度计和磁力计对重力向量提供观测，加速度部分参考值则是将标准的重量加速度[0,0,g]T和磁力计投影到机体坐标系的值与加速度测量值构建误差向量

加速度误差向量e_a为：

磁力计部分的观测方程有：

其中a_m＝[a_x a_y a_z]和m_m＝[m_x m_y m_z]分别为加速度和磁力计的测量值，磁力计部分的误差向量为：

其中m_b＝[b_x b_y b_z]，因此系统的误差向量e写为：

当系统状态变量为

时，将误差向量对系统状态变量求导，计算得到系统的观测矩阵H_X为：

B4、计算加速度置信度：

对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量a_f，通过a_f分量来判断加速度运动剧烈程度，动态调整加EKF滤波器观测噪声的值R(t)，以达到实时调整加速度融合的权重，实现精准的位姿估计，其中，

a_f＝highpass_1:100Hz(a_m)

B5、EKF更新姿态融合：

K＝P_k|k-1H^T(HP_k|k-1H^T+R(t))^-1

P_k|k＝(I-KH)P_k|k-1

x_k|k-1＝_k|k-1+Kr。

采用上述技术方案的发明，具有如下优点：

1、本发明方法通过降低加速度计的权重，使得系统陀螺仪积分值信任程度增加，减小输出角度的误差；

2、本发明方法基于置信度函数动态修正加速度和磁力计传感器融合权重，提升IMU在大机动运动时姿态计算精度和鲁棒性；

3、本发明融合方法使得IMU具有更高精度、抗干扰能力更强，有利于低成本IMU的实际应用。

附图说明

本发明可以通过附图给出的非限定性实施例进一步说明；

图1为本发明融合方法流程图；

图2为本发明平稳运动条件下加速度和角速度测量值一；

图3为本发明平稳运动条件下改进算法对比原始算法输出的欧拉角曲线一；

图4为本发明大机动运动条件下加速度和角速度测量值一；

图5为本发明大机动运动条件下改进算法对比原始算法输出的欧拉角曲线一；

图6为本发明平稳运动条件下加速度和角速度测量值二；

图7为本发明平稳运动条件下改进算法对比原始算法输出的欧拉角曲线二；

图8为本发明大机动运动条件下加速度和角速度测量值二；

图9为本发明大机动运动条件下改进算法对比原始算法输出的欧拉角曲线二。

具体实施方式

以下将结合附图和具体实施例对本发明进行详细说明，需要说明的是，在附图或说明书描述中，相似或相同的部分都使用相同的图号，附图中未绘示或描述的实现方式，为所属技术领域中普通技术人员所知的形式。另外，实施例中提到的方向用语，例如“上”、“下”、“顶”、“底”、“左”、“右”、“前”、“后”等，仅是参考附图的方向，并非用于限制本发明的保护范围。

如图1所示，一种基于自适应参数互补滤波的机器人姿态融合方法，包括如下步骤：

A1、对加速度值进行预处理并选取频率在1-100Hz的分量，依据滤波后的加速度计算加速度和磁力计的置信度；

A2、计算加速度与标准重力向量的误差以及磁力计与NED坐标系下的方向向量误差；

A3、结合置信度和误差向量对陀螺仪测量值进行矫正；

A4、结合四元数微分方程对姿态向量进行更新。

步骤一：读取MPU6050的三轴加速度a_m＝[a_x a_y a_z]和三轴角速度数据w_m＝[w_x w_yw_z]，以及HMC5883L的三轴磁力计数据m_m＝[m_x m_y m_z]。

步骤二：对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量a_f，并利用衰减函数计算加速度测量值的置信度K(t)。

a_f＝highpass_1:100Hz(a_m)

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

步骤三：计算加速度测量值与标准参考重力[0,0,g]之间的误差e_a

e_a＝a_m×a_v

其中，

步骤四：计算磁力计测量值与参考磁力方向向量之间的误差e_m

步骤五：由加速度向量误差、磁力计向量误差对陀螺仪测量值w_m进行动态修正

eInt＝eInt+e*Ki*0.5*T

w＝w_m+eInt+K(t)*e

其中e＝e_a+e_m

步骤六：利用四元数微分方程更新姿态四元数

q_k+1＝(I+Omega(w)*0.5*T)q_k

其中，

实施例：实验一

利用STM32F103构成的微控制器采集板采集MPU6050和HMC5883L的数据，其中MPU6050提供三轴方向的加速度和角速度，HMC5883L提供三轴方向的磁力计数据。传感器输出频率为100Hz。将IMU平台在3D空间中缓慢的移动，对比原始算法(Mahony互补滤波)和改进算法的姿态估计曲线。

图2显示了平稳运动条件下加速度和角速度传感器输出的测量数据，图3显示了平稳运动条件下改进算法和原始算法的欧拉角变化情况。从图2可以看到，加速度传感器的测量值的模长基本在1的附近，这满足使用加速度计观测重力向量的基本假设。如图3所示，此时改进算法和改进前的算法都能很好的跟踪到IMU的角度。

实施例：实验二

将采集平台放在水平面上剧烈晃动，比较改进后的算法在大机动运动下对姿态估计的性能。在这种情况下roll角和pitch角的真实角度应该是静止时候的值，即在0的附近。

图4大机动运动条件下显示了加速度和角速度传感器输出的测量数据，图5显示了大机动运动条件下改进算法和原始算法的欧拉角变化情况。从图4可以看到，传感器在0-50区间中,传感器保持静止，此时三个轴上的加速度和基本处于1个重力加速度，此时改进算法和原始算法输出的欧拉角相同。在50-300中加速度发生了剧烈运动，此时三个轴上的加速度和不在是1个重力加速度。这种情况下，由于加速度的耦合会导致融合算法错误的把剧烈运动产生的轴向加速度变成对重力向量的观测数据，导致对重力向量错误的观测。而改进算法通过降低加速度计的权重，使得系统陀螺仪积分值信任程度增加，减小输出角度的误差。

本发明还包括EKF姿态融合方法：B1、初始化系统变量：

根据采集到的加速度和磁力计数据计算初始的欧拉角和四元数初始值

roll＝atan2(-ay,az)*57.3

B2、姿态预测：

设系统的状态变量为

的状态变量,状态更新方程为：

其中：

状态转移矩阵:Φ＝I+F*dt,而四元数q部分的状态更新情况，即状态转移矩阵的前四行四列为：

B3、构建观测方程：

观测部由加速度计和磁力计对重力向量提供观测，加速度部分参考值则是将标准的重量加速度[0,0,g]T和磁力计投影到机体坐标系的值与加速度测量值构建误差向量

加速度误差向量e_a为：

磁力计部分的观测方程有：

其中a_m＝[a_x a_y a_z]和m_m＝[m_x m_y m_z]分别为加速度和磁力计的测量值，磁力计部分的误差向量为：

其中m_b＝[b_x b_y b_z]，因此系统的误差向量e写为：

当系统状态变量为

时，将误差向量对系统状态变量求导，计算得到系统的观测矩阵H_X为：

B4、计算加速度置信度：

对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量a_f，通过a_f分量来判断加速度运动剧烈程度，动态调整加EKF滤波器观测噪声的值R(t)，以达到实时调整加速度融合的权重，实现精准的位姿估计，其中，

a_f＝highpass_1:100Hz(a_m)

B5、EKF更新姿态融合：

K＝P_k|k-1H^T(HP_k|k-1H^T+R(t))^-1

P_k|k＝(I-KH)P_k|k-1

x_k|k-1＝_k|k-1+Kr。

实施例：实验三

利用STM32F103构成的微控制器采集板采集MPU6050和HMC5883L的数据，其中MPU6050提供三轴方向的加速度和角速度，HMC5883L提供三轴方向的磁力计数据。传感器输出频率为100Hz。

图6显示了平稳运动条件下加速度和角速度传感器输出的测量数据，图7显示了平稳运动条件下改进EKF算法和EKF融合算法的欧拉角变化情况。从图6可以看到，加速度传感器的测量值的模长基本在1的附近，这满足使用加速度计观测重力向量的基本假设。如图7所示，此时改进算法和改进前的算法都能很好的跟踪到IMU的角度。

实施例：实验四

将采集平台放在水平面上剧烈晃动，比较改进后的算法在大机动运动下对姿态估计的性能。在这种情况下roll角和pitch角的真实角度应该是静止时候的值，即在0的附近。

图8大机动运动条件下显示了加速度和角速度传感器输出的测量数据，图9显示了大机动运动条件下改进EKF算法和基于EKF的姿态融合算法的欧拉角变化情况。从图8可以看到，加速度发生了剧烈运动，此时三个轴度上的加速度和不在是1个重力加速度。这种情况下，由于加速度的耦合会导致融合算法错误的把剧烈运动产生的轴向加速度变成对重力向量的观测数据，导致对重力向量错误的观测。而改进算法通过降低加速度计的权重，使得系统陀螺仪积分值信任程度增加，减小输出角度的误差。

本发明对一种基于自适应参数互补滤波的机器人姿态融合方法进行了详细介绍。具体实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，改进和修饰也落入本发明权利要求的保护范围内。

Claims (6)

1.一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，包括如下步骤：

A1、对加速度值进行预处理并选取频率在1-100Hz的分量，依据滤波后的加速度计算加速度和磁力计的置信度；

A2、计算加速度与标准重力向量的误差以及磁力计与NED坐标系下的方向向量误差；

A3、结合置信度和误差向量对陀螺仪测量值进行矫正；

A4、结合四元数微分方程对姿态向量进行更新。

2.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，步骤A1的具体实现如下：

A101、读取三轴加速度a_m＝[a_x a_y a_z]和三轴角速度数据w_m＝[w_x w_y w_z]，以及三轴磁力计数据m_m＝[m_x m_y m_z]；

A102、对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量a_f，并利用衰减函数计算加速度测量值的置信度K(t)：

a_f＝highpass_1:100Hz(a_m)

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

3.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，步骤A2具体实现如下：

A201、计算加速度测量值与标准参考重力[0,0,g]之间的误差e_a

e_a＝a_m×a_v

其中，

A202、计算磁力计测量值与参考磁力方向向量之间的误差e_m

4.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，步骤A3的具体实现如下：

A301、由加速度向量误差、磁力计向量误差对陀螺仪测量值w_m进行动态修正

eInt＝eInt+e*Ki*0.5*T

w＝w_m+eInt+K(t)*e

其中e＝e_a+e_m。

5.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，步骤A4的具体实现如下：

A401、利用四元数微分方程更新姿态四元数：

q_k+1＝(I+Omega(w)*0.5*T)q_k

其中，

6.根据权利要求1所述的一种基于自适应参数互补滤波的机器人姿态融合方法，其特征在于，还包括如下步骤：

B1、初始化系统变量：

根据采集到的加速度和磁力计数据计算初始的欧拉角和四元数初始值

roll＝atan2(-ay,az)*57.3

B2、姿态预测：

设系统的状态变量为

的状态变量,状态更新方程为：

其中：

状态转移矩阵:Φ＝I+F*dt,而四元数q部分的状态更新情况，即状态转移矩阵的前四行四列为：

B3、构建观测方程：

观测部由加速度计和磁力计对重力向量提供观测，加速度部分参考值则是将标准的重量加速度[0,0,g]T和磁力计投影到机体坐标系的值与加速度测量值构建误差向量

加速度误差向量e_a为：

磁力计部分的观测方程有：

其中a_m＝[a_x a_y a_z]和m_m＝[m_x m_y m_z]分别为加速度和磁力计的测量值，磁力计部分的误差向量为：

其中m_b＝[b_x b_y b_z]，因此系统的误差向量e写为：

当系统状态变量为

时，将误差向量对系统状态变量求导，计算得到系统的观测矩阵H_X为：

B4、计算加速度置信度：

对加速度数据进行预处理，通过带通滤波器提出加速度中的直流分量，取出[1,100Hz]的分量a_f，通过a_f分量来判断加速度运动剧烈程度，动态调整加EKF滤波器观测噪声的值R(t)，以达到实时调整加速度融合的权重，实现精准的位姿估计，其中，

a_f＝highpass_1:100Hz(a_m)

B5、EKF更新姿态融合：

K＝P_k|k-1H^T(HP_k|k-1H^T+R(t))^-1

P_k|k＝(I-KH)P_k|k-1

x_k|k-1＝_k|k-1+Kr。

Images