[发明专利]一种基于自适应参数互补滤波的机器人姿态融合方法

权利要求书

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

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

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

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

步骤A3的具体实现如下：

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

eInt＝eInt+e*Ki*0.5*T

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

其中e＝e_a+e_m；

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

其中，还包括如下步骤：

B1、初始化系统变量：

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

roll＝atan2(-ay,az)*57.3

B2、姿态预测：

设系统的状态变量为7x1的状态变量,状态更新方程为：

其中：

状态转移矩阵:Φ＝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。