基于卡尔曼滤波的自平衡车数据融合应用

文/袁文玉

1 引言

两轮自平衡车是一种环保安全、灵活便捷且能够自动调节姿态的新型轮式车，可应用于较多的特殊场合。其基本原理类似倒立摆，是一种多变量、不稳定、非线性及强耦合的系统。自平衡车系统的控制模块大体分为速度控制模块、方向控制模块和姿态平衡控制模块，本文主要讨论自平衡车的姿态调整问题，分析车体的受力情况及运动特性，并将其理想化建立数学模型。

在传统的平衡车姿态调整方法中，常采用单一的姿态传感器向控制器输入车体的倾角信号，但由于传感器本身存在的零漂和测量误差使得在长时间测量时，难以得到精确的车体倾角。故本文采用由陀螺仪和加速度计构成的多传感器姿态检测模块，提出一种基于卡尔曼滤波的自平衡车数据融合方法，设置固定的信任权重，将两种传感器的输出数据进行融合，同时滤除噪声，以消除采用单个传感器存在的误差累积及提高车体角度测量的实时在线估计，得到最优化的自平衡车姿态角度。

2 自平衡车运动特性模型分析

自平衡车以两平行车轮为支撑，通过姿态检测模块测量车体倾角，经控制器处理输出控制信号，以此来驱动两轮电机维持车体平衡。在只考虑自平衡车静止和直线行驶的情况下，其机械结构类似于一级倒立摆模型,可以将其简化为倒立摆模型，在实际操作中车体姿态平衡时，其重心位置与竖直方向呈一定的倾角，但为简化模型分析假设车体平衡时重心位置位于垂直方向，自平衡车的结构剖面图和简化模型如图1所示。

由于自平衡车的车轮与电机通过车轴连接，陀螺仪、加速度计、单片机和运算放大器PCB 板靠近电机安装使重心偏下，可将其视为一个整体作为两车轮的质量和m；忽略轮胎与地面的摩擦力以及行驶时的空气阻力；忽略电机和编码器齿轮之间的摩擦力。假设车体正常行驶时的加速度为a，车体与竖直方向之间的夹角为θ，使重心恢复竖直方向所需拉力为：

当角度θ 较小时，正弦值与角度值基本相等，且因为电机的驱动力受车体倾角的影响，则平衡车的加速度与倾角存在一定的比例关系，假设系数为k,由此可将公式简化为：

图1：自平衡车结构剖面图和简化模型

图2：加速度敏感轴与重力加速度关系

图3：卡尔曼滤波器仿真模型

自平衡车的姿态调整在保证测量角度精确的前提下，还需要确保恢复垂直状态的迅速性，可添加有关角速度 的阻尼力：

由此可以看出，需要通过控制电机使车体的加速度维持在：

可见实时地进行倾角测量，给予电机控制信号才能使车体维持平衡。

3 姿态检测系统组成

3.1 加速度计测量原理与误差分析

本文采用加速度计与陀螺仪组成姿态检测模块，经K60 核心控制器进行A/D 转换后由卡尔曼滤波器进行数据融合与滤波，得出精确的角度信号。应用MPU6050 中的3 轴加速度计模块，安装时使z 轴与车体倾倒方向一致，以重力加速度为输入，静止时测得的数据为重力加速度在各轴上的分量，当车体的倾角发生变化时，加速度计三个敏感轴的方向随之发生变化由此引起加速度分量变化，由图2可知，α、β、θ 分别为x 轴、z 轴、y 轴与重力加速度的夹角，α1、β1、θ1分别为各轴与水平方向的夹角，各轴倾角与加速度分量的关系为：

由上式可知，测得数据与倾角呈非线性正弦关系，可通过对输出数据进行反正弦变化得出车体的倾角值，但是当平衡车运动时，车体所受加速度不再是单一的重力加速度还包括电机驱动产生的加速度，同时存在着系统的振动和机械噪声，这些因素都会导致加速度计测量角度不精确，且加速度计动态响应较慢不适合跟踪需要迅速应变的动态系统，所以需要与陀螺仪共同合作实现追踪。

3.2 陀螺仪测量原理与误差分析

陀螺仪主要用来测量车体的瞬间角速度，分x 轴、y 轴和z 轴分别可以用来检测物体的俯仰角、翻滚角和偏航角。由于安装在自平衡车上，陀螺仪的主要作用是测量车体的俯仰角变化的角速度，一般通过积分运算得到车体的当前角度值。在短时间内，陀螺仪的测量精确度较高但是由于车体颠簸和机械振动会造成陀螺仪测得的角速度信号出现微小漂移，使得积分出现误差，最终形成误差累积导致在长时间测量时无法得到真实的车体倾角，这就需要加速计对其测量的角度进行定期纠正。

4 基于卡尔曼滤波的数据融合方法设计

由于加速度计静态性能优良但动态跟踪能力不行，陀螺仪测量倾角精确简便但是存在积分误差累积，所以需要融合两传感器的输出数据，得出最优化的车体倾角，可通过卡尔曼滤波器实现数据融合过程。首先，在考虑到卡尔曼滤波器主要用于离散时间系统的前提下，由于车体倾角的角度可由角速度积分得出，可将车体的实际倾斜角度θ 与用加速度计观测的陀螺仪测量角速度的常数偏差t 作为系统的状态向量，可得如下状态与测量方程：

式中，X(k)为k 时刻自平衡车的状态（陀螺仪测得的角度及其常数误差），X(k-1)为k-1时刻自平衡车的状态，U(k-1)为k-1 时刻陀螺仪测得的角速度，为k 时刻的陀螺仪测量过程中存在的噪声，Z(k)为k 时刻加速度计经反正弦计算后输出的角度，为k 时刻的加速度计的测量过程中存在的干扰。

简化状态方程与测量方程为：

其中，A 为系统的预测矩阵，B 为控制矩阵，H 为传感器测量矩阵。

根据以上公式可知，每一个状态变量更新到一个新的状态时，仅需要上一个时刻参与估算且其仍服从高斯分布，此外，为完成对卡尔曼滤波器的校正得到最优的角度值，需要求取k 时刻的协方差矩阵P(k)，而系统的过程噪声协方差矩阵Q(k)和测量误差协方差矩阵R(k)为P(k)的组成部分需进行合理调整，如下所示：

k 时刻自平衡车系统的预测方程：

由预测状态方程得出的系统协方差为：

经传感器测量数据修正的状态方程：

上式中，Kh为卡尔曼增益，其表示方程如下：

式中，HT为H 的转置矩阵。

k 时刻 的协方差：

由此，X(k|k)和P(k|k)就是最新的最优估计，根据卡尔曼的迭代思想X(k|k)和P(k|k)将作为新的X(k-1|k-1)和P(k-1|k-1)进行k+1 时刻的最优化运算。

4 效果分析

4.1 仿真分析

利用MATLAB 建立本文设计的卡尔曼滤波器smiulink 仿真模型，以对比滤波前后波形验证滤波效果，仿真模型如图3所示。

通过对陀螺仪和加速度计信任权重的分配，以及测量噪声协方差系数的调整，以达到理想的滤波效果。滤波前后波形对比如图4所示。

4.2 实测应用分析

将基于卡尔曼滤波的数据融合的方法应用于以K60 为核心控制器的自平衡车上，以检测其有效性。由上式（16）、（18）、（19） 可知，自平衡车的姿态主要由卡尔曼滤波器分配给陀螺仪和加速度计信任权重q_acce 、q_gyro 以及测量矩阵的协方差r_acce 决定。信任权重的分配需要根据传感器的测量精度，因陀螺仪的测量精度高于加速度计，所以q_gyro应略小于q_acce，由此设置q_gyro=0.001，q_acce=0.003，r_acce=0.001，通过UART 无线通信将波形显示到上位机进行观察。加速度计与数据融合输出波形对比如图5所示。

上位机中显示出两条波形，棕色波形表示加速计z 轴加速度经反正弦变化得出的数据波形，绿色波形是经过卡尔曼滤波器进行数据融合后得出的波形，可以看出加速度计的波形具有较多毛刺，这是因为加速度计能比较准确地反映车体倾角的变化，而陀螺仪测得的角速度积分波形较平滑，但是存在一定的积分误差，经卡尔曼数据融合后的波形既具有较好的平滑性又能够准确跟踪加速度的波形，且无超调和滞后，此时自平衡车姿态良好，从而验证该方法的正确性和有效性。

5 总结

图4：滤波前后波形对比图

图5：加速度计与卡尔曼滤波输出波形对比

本文针对加速度计和陀螺仪作为单一传感器进行角度测量的缺陷分析，采用多种传感器进行角度测量，利用卡尔曼滤波算法将输出数据进行融合滤波以得出最优化的角度值，最后通过对实测数据波形分析实现对自平衡车姿态的理想调整，能够使车体维持在一定倾角且前后偏移较小，说明只要选择合理的控制参数，就可以实现自平衡车体姿态的稳定性。