基于姿态检测的两轮自平衡车控制方法研究

王 文，邓三鹏＊，张香玲，祁宇明

（1.天津职业技术师范大学机器人及智能装备研究所，天津300222；2.天津博诺智创机器人技术有限公司，天津300353）

0 引言

两轮平衡车作为一种绿色环保、灵活方便的交通方式受到欢迎，同一阶倒立摆系统一样，两轮自平衡车具有欠驱动、非线性、强耦合、多变量的特点[1]。两轮自平衡车在平衡过程中需维持车体重心与轮子处于同一轴线，通过实时读取陀螺仪传感器和加速度传感器测得的偏转角度来改变状态输入值，控制电机的PWM输出值产生的扭矩达到平衡目的，所以自平衡车实时监测的偏转角度大小成为关键，自平衡车姿态解算大致分为一阶互补滤波、两阶互补滤波、小波分析、卡尔曼滤波算法、神经网络。小波分析、神经网络都存在数据计算量大、对处理器性能要求过高[2]；一阶互补滤波、两阶互补滤波是从陀螺仪或加速度计进行姿态角度信号滤波，优点是滤波收敛速度快，但存在滤波效果不平滑现象造成的超调严重；卡尔曼滤波在收敛速度和平滑效果上都一般。比较之下，将卡尔曼和互补两种滤波算法融合陀螺仪和加速度计获取的位姿数据求解出最优位姿参数[3]，采用PID算法控制两轮自平衡车，控制可靠，运算速度快。

1 建立两轮自平衡车数学模型

两轮自平衡车的力学模型不同于单级倒立摆，自平衡车系统是依靠底部电机扭矩输出来达到整个系统平衡[4]。如图1a所示，自平衡车轮子由底部电机驱动，能够产生转矩，是主动机构；车体属于是从动机构。为了能够实现系统的自平衡，必须实时支配驱动电机进行正反转动，以生成反倾覆的恰当力矩，维持两轮自平衡小车车体平衡。

图1 两轮自平衡车系统示意图

如图1b中，设自平衡车车体与y轴之间的夹角IA为偏转角θ，小车车轮的水平位移u为X轴正方向，为小车车体绕轮子中心的转动惯量，L为小车车体重心到转轴的距离，可以得出，其中为小车车轮组件在x轴方向给车体的力，为小车车轮组件在y轴方向给车体的力[5]。系统中车轮与电机位于同一轴线，可视为与车体一致，所以设车轮质量为M；假设除车轮部分的为一均匀质体，取其质量为m；在理想条件下，设立水平和垂直运动状态方程：

为了便于建模，在小角度的范围内[6]，取sinθ≈θ，cosθ≈1，由上式得系统运动方程：

2 两轮自平衡车传感器姿态检测

系统搭配数字陀螺仪MPU6050传感元件，集成了3轴MEMS陀螺仪、3轴MEMS加速度计，通过I2C实现数据通信，输出数值为角加速度值、角速度值，加速度计用来测量平衡车线性加速度，因平衡车往返震荡频繁致使加速度易敏感化，测得实时数值不准确，计算倾斜角度产生大的误差；通过对角速度进行积分，陀螺仪可以测量瞬时动态角度值的变化。小段时间内测得的角度值精度高，不会受加速度的干扰。但是积分漂移和温度会随着积分时间的增长而增长，所以这两种方法不适合单独使用。为了准确地测得自平衡车姿态信息，提出将陀螺仪测得数据信息和加速度计获得的信息进行融合处理。将卡尔曼和互补两种滤波算法融合陀螺仪和加速度计获取的位姿数据求解出最优位姿参数。

2.1 卡尔曼滤波

卡尔曼滤波器的作用是估算线性离散时间在某时刻的过程状态变量，系统状态方程如下：

系统测量方程如下：

其中如表1所示。

表1 线性离散时间在K时刻的过程状态

卡尔曼思想是要对k时刻的倾斜角度值预测，先要预测k时刻的倾斜角度值就需要依照k-1时刻的倾斜角度值，然后得到k时刻的高斯噪声偏差，这里系统预测状态：

2.2 互补滤波

两轮自平衡车系统中，为防止由于加速度计产生的瞬时值计算倾角误差大、陀螺仪测得角速度信号之间存在的微小误差，经积分漂移形成更大的误差，采用了互补滤波算法，可获得更优的平衡姿态估计值。互补滤波是通过识别被测量信号的不相同频域来消除噪声的，由低通滤波器提取高频噪声中测量信号的低频信号，同样的由高通滤波器提取低频噪声中的测量信号的高频信号。在小段时间内陀螺仪读取的瞬时值为最准的角度值，需要采取一种滤波算法对采样加速度定期优化，取角度平均值，这个过程中陀螺仪产生低频信号，加速度计产生高频信号，然后再将整个频段的信号进行拼接输出，流程如图2所示。

图2 互补滤波

2.3 互补/卡尔曼滤波融合过程

首先运用互补滤波算法将陀螺仪和加速度计所得数值进行融合处理，计算得到陀螺仪姿态误差。然后以互补陀螺仪的积分误差作为观测值，利用卡尔曼滤波对各部分误差进行补偿，得到精确的姿态参数。

当测量信息无效时，直接使用事先写好的设定值进行补正陀螺仪角度值，输出水平姿态角，融合流程如图3所示。

图3 卡尔曼/互补滤波融合流程

3 两轮自平衡车控制算法

两轮自平衡车分为直立、速度和转向控制。

自平衡车直立控制采用PD控制算法，输入变量是根据互补滤波融合卡尔曼滤波融得出更精确的倾角θ，NI myRIO控制器根据这个输入变量来改变电机的运动方向和速度，使车轮产生合适的加速度，平衡车就能产生与倾倒趋势相反的力矩，通过动态实时调整重心误差补偿，从而维持自平衡小车稳定直立平衡。平衡PWM输出公式表示为：

KP为比例系数，θ为倾斜角度，KD为微分系数，φ为角速度。

速度控制采用PI控制算法，依据自平衡小车底部两电机自带的霍尔编码器获取的信息加入速度PI控制，实现系统速度调节。速度PWM输出公式表示为：

将PD控制运用到转向控制，通过陀螺仪Z轴角度辅助控制实现小车转向，转向PWM表示为：

综合三种：

4 实验与分析

两轮自平衡系统主要由NI myRIO控制器、陀螺仪传感器、带编码器直流减速电机、驱动控制板和电源组成。NI myRIO控制器内嵌Xilinx Zynq芯片，直流供电范围为6 V～16 V，易于上手使用、编程开发简单；姿态传感器为数字陀螺仪MPU6050六轴传感元件，集成了3轴MEMS陀螺仪、3轴MEMS加速度计。实体样机平衡效果如图4所示。

图4 两轮自平衡车平衡效果展示

实验条件：（1）平整的平面；（2）中途介入人为外力干扰；（3）试验方案：自由平衡——介入外力干扰——自由平衡；（4）一次试验15 min，不同平面条件下各测十次。

根据上述系统运动状态方程，结合位姿检测策略得出的机器人倾角、角速度输出控制量。每次试验动作超过1000次，然后停止试验并重新测试验证其稳定性。

机器人机体倾斜角度、机器人角速度收敛分别如图5a和图6a所示，人为外力干扰后机器人机体倾斜角度、机器人角速度收敛分别如图5b和图6b所示。

图5 两轮自平衡车稳定直立角度和受冲击角度波形图

图6 两轮自平衡车稳定直立角速度和受冲击角速度波形图

由实验结果分析可以得出，采用融合卡尔曼滤波和互补滤波算法对陀螺仪传感器和加速度传感器获取的位姿数据作处理后，系统收敛速度更快，误差更小，系统更稳定。控制界面如图7所示。

图7 两轮自平衡车labview界面显示

5 结论

针对平衡车姿态检测手段单一，长时间运行存在控制不稳定造成人身财产损失的问题，改进了姿态检测手段，将卡尔曼滤波和互补滤波两种算法融合陀螺仪传感器和加速度传感器获取的位姿数据求解出最优姿态参数，在平衡运动过程中所采集的姿态实时数据更准确，不存在由于长时间使用累积的误差；采用PID控制算法建立控制运动方程，控制灵活。最终在搭建的自平衡车进行测试，稳定能力更好，且在外力干扰条件下能迅速回正，响应速度更快。