基于FPGA平衡小车的设计

2016-03-10天津理工大学王孟轩陈子娟李秀顺

电子世界 2016年24期

天津理工大学王孟轩陈子娟汪煊李秀顺

基于FPGA平衡小车的设计

天津理工大学王孟轩陈子娟汪煊李秀顺

随着科技进步，生活水平的提高，人们追求智能与舒适的愿望也日益强烈。从而催生了许多智能化的产品。如智能电视、智能小车等。如何实现小车的自动快捷驾驶，也成为人们心中的向往与疑问，基于这种趋势与需求，着眼于实际情况。本文介绍了基于MPU6050的自平衡小车系统的设计。系统基于陀螺仪等传感器采集姿态信息，通过对比分析一阶互补滤波和卡尔曼滤波的优缺点，采用卡尔曼滤波算法，并利用PID平衡算法，对小车的速度倾斜角度平衡状态来进行检测，并通过FPGA来控制电机来实现双轮小车自如平衡地运动。从而实现小车智能自主控制的目的。本设计获得了2016年“京微雅格杯FPGA应用大赛”一等奖。

FPGA；平衡车；PID算法；卡尔曼滤波

1 设计原理

1.1 产品调研

目前，电动平衡车主控芯片大多数是使用ST的一款32单片机——STM32F103系列，单片机由于是单线程，程序语句需要等待单片机周期才能执行。因此基于单片机设计的程序运行速度较慢，而FPGA由于是硬件电路，运行速度直接取决于晶振速度，速度较快且系统稳定。所以本设计拟采用基于FPGA的CME—M7系列芯片和TB6612FNG、HC-05、MPU6050完成双轮电动车的设计。

1.2 原理示意图

图1 原理示意图

1.3 设计原理

双轮平衡车的控制思想是基于一级倒立摆的模型，当车体发生倾斜时，传感器会检测到车体角度的变化，控制系统根据车体角度的变化，控制电机向倾斜方向转动，从而产生抵抗倾斜的力矩，使车身维持在平衡状态。当车体不发生倾斜时，平衡车会在小范围内实现动态平衡。所以，倾角越大加速度越快。在此策略下，平衡车能够实现静止、前进、后退等各种运动状态，从而保证平衡车的稳定运行。小车作为一个控制对象，它的输出量是两个电机的转动速度[1]。小车的运动控制任务可以分解成以下三个基本任务：

控制小车平衡：通过控制两个电机正反向运动保持小车直立平衡状态；

控制小车速度：通过调节小车的倾角来实现小车速度控制，实际上最后还是演变成通过控制电机的转速来实现车轮速度的控制。

控制小车方向：通过控制两个电机之间的转动差速实现小车转向控制。

小车直立和方向控制任务都是直接通过控制小车两个电机完成的。在实际控制中，是将控制小车直立和方向的控制信号叠加在一起加载电机上，只要电机处于线性状态就可以同时完成上面两个任务。小车的速度是通过调节小车倾角来完成的。小车不同的倾角会引起小车的加减速的变化，从而达到对于速度的控制。

由上述原理分析可知，车身需要采集的主控芯片信息有车身加速度、车身角加速度、电机速度和位置信息等。因此本设计以陀螺仪和加速度计作为传感器，采用FPGA作为主控芯片，通过电机驱动电路驱动直流电机实现电动双轮玩具小车的姿态控制。控制器通过IIC串口通信协议读取传感器的姿态信息，经过滤波算法后进行数据的融合分析，得到小车的姿态，再通过姿态更新后，进入控制模块，控制算法根据小车的姿态计算出驱动电机所需的PWM信号，电机控制部分主要算法采用PID算法，再通过双闭环反馈控制（速度闭环控制和位置闭环控制），使小车稳定运行达到动态平衡。

2 设计流程

图2 设计流程图

2.1 姿态信息采集模块

为实现小车的直立行走，需要利用MPU6050姿态传感器和霍尔正交编码器采集以下信息：

（1）小车倾角速度陀螺仪信号，获得小车的倾角和角速度。（2）重力加速度信号（z轴信号），补偿陀螺仪的漂移。

（3）小车电机转速脉冲信号，获得小车运动速度和位置信息，进行速度控制。

2.2 卡尔曼滤波器

两轮自平衡小车系统具有多变量、非线性、强耦合、参数不确定等特性，是一个集动态决策和规划、环境感知、行为控制和执行等多种功能于一体的综合复杂系统.因此，采集小车的姿态信息很困难，所以我们必须增加滤波模块以排除信号噪声和动态加速度的干扰[2]。通常滤波分为一阶滤波、互补滤波和卡尔曼滤波。

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。在本系统中，测量角度和测量角速度存在着导数关系，在这种情况下，我们不直接估计车体的角速度，而是采用加速度计的测量值来估算偏差gyro_bias , gyro_bias可以作为状态向量的另一个元素。我们把车体的角度和角速度用入下关系时表示:

式中：anglek为估算的k时刻倾角；

anglek-1上一时刻加速计的测量值；

dt为采样时间。

过程噪声的协方差矩阵形式如下：

R=[R_angle]为观测噪声的协方差矩阵，且车体的角度与角速度中含有的噪声为白噪声加速度计测得的车体角度的过程噪声的协方差为Q_ angle，陀螺仪测得的车体的角速度的过程噪声协方差为Q-gyro。它们取不同的值代表了不一样的信任程度。如果认为陀螺仪的可信度更高，可以把Q-gyro设置的小一些，如果认为加速度计的可信度更高可以把Q_ angle设置的小一些。R矩阵代表观测噪声的协方差，加入R很大，说明系统具有很大的噪声。

仿真数据分析：

由式(2-1)我们可以编写具体的卡尔曼滤波算法，并加以仿真。

在仿真过程中，通过调整协方差系数，可以控制滤波效果，仿真结果如图3所示。通过图3可以看出，加上噪音后，输入卡尔曼滤波器的信号品质很差，通过卡尔曼滤波器滤波后，消除了输出信号的噪声，输出信号可以很好地跟踪理想信号。

图3 仿真结果图

通过分析仿真及实测数据，可以看出，所设计的控制系统能够很好地控制自平衡两轮车直立，且自平衡车前后摆动较小，这证明了该控制器设计合理，控制器各参数选择合适，硬件系统工作正常。

2.3 姿态信息更新

双轮玩具小车是非线性、多阶次、不稳定的系统，要实现其动态平衡，控制小车完成相应的动作，即给出相应的姿态信息，将控制信号重新写入存储器，由于其平衡状态是处于动态，所以需要不断的进行测量更新，针对两轮自平衡车倾角的测量问题,采用了卡尔曼多传感融合算法。该算法融合了加速度传感器和陀螺仪传感器数据,实现了两轮自平衡车倾角实时在线估算，控制器给出控制信息后又控制系统驱动电机工作，采用了比例微分(PID)控制算法,实现了两轮自平衡车姿态的稳定控制[3]。

2.4 电机驱动控制

2.4.1 双轮平衡车控制特点

两轮平衡车需要解决的问题是在前进、静止、退过程中都要保持车体的平衡。它使一级倒立摆的运动方式变得更加灵活，更加多样化。对于不完整非线性的系统，考虑下面介绍的PID控制方法:

PID控制是最常见的控制算法，它主要是对被控系统进行偏差调节。实现被控量的值与要求的值达到一致。但为了适应不同的过程，需要给出合理的控制规律，否则，PID控制器达不到预定效果。控制算法根据小车的姿态信息计算出PWM信号，再通过闭环反馈控制使小车完成相应的动作。根据角度角速度变化，控制电机来完成当前的平衡，以及在车体在运动过程中的各个姿态。

2.4.2 速度、位置双闭环控制

我们一般在速度闭环控制系统里面，使用增量式PI控制，而在我们的微处理器里面，因为控制器是通过软件实现其控制算法的，所以必须对模拟调节器进行离散化处理，这样它只需根据采样时刻的偏差值计算控制量。因此，我们需要使用离散的差分方程连续的差分方程。如下部分给出了小车的双闭环控制原理

速度、位置双环控制由以上两个控制器进行嵌套而成因为双闭环控制过程较复杂，故给出其控制原理图4如下：

图4 双环控制原理图

2.4.3 PWM输出

要产生频率和脉冲宽度可调的PWM波形，这可通过FPGA丰富的硬件资源和可以配置I/O引脚来实现。嵌入式系统中FPGA的应用设计关键是系统软硬件功能的划分。通过计数器的方式实现PWM的输出。

2.4.4 电机PID控制算法的反馈系数的确定

由上文中PID控制的原理可知，比例环节输出的控制信号与系统误差e(t)成比例，比例系数越大，系统反应越灵敏，精度越高，但也会使调节震荡更剧烈，甚至造成系统不稳定。积分环节输出的控制信号与误差的积分成比例，

积分系数大，可以更快地消除稳态误差，但如果积分系数太大，则会产生较大的超调和长时间的波动。微分环节输出的控制信号与误差的变化率成比例，微分调节主要作用是对系统变化做出预警，产生提前的调节。微分控制可以使系统相位超前，抵消积分作用造成的相位滞后，同时可以减小超调，但微分系数过大，可能造成系统不稳定[4]。

根据这一原理，进行了大量的实验调控，得到比较符合要求的实验数据，再通过大数据的分析和计算确定反馈比例系数。由于本系统采用CME——M7，其具有ARM内核，可以做SOC，能将算法级的复杂程序直接放在ARM内核中，降低程序的开发难度。