摘要：着重分析了两轮自平衡小车的设计原理与控制算法，采用卡尔曼滤波算法融合陀螺仪与加速度计信号，得到系统姿态倾角与角速度最优估计值，通过双闭环数字PID 算法实现系统的自平衡控制。设计了以MPU-6050传感器为姿态感知的两轮自平衡小车系统，选用8位单片机HT66FU50A为控制核心处理器，完成对传感器信号的采集处理、车身控制以及人机交互的设计，实现小车自主控制平衡状态、运行速度以及转向角度大小等功能。本文引用地址：http://www.eepw.com.cn/article/201602/287501.htm

引言

两轮自平衡小车一般都是以倒立摆的结构模型为基础，是移动机器人研究中的一个重要领域，是一个非线性、强耦合、多变量和不稳定的动态系统，可以很好地验证控制理论及控制方法的优秀平台，具有很高的研究价值[1]。

平衡车论文普遍侧重理论研究[2]，首先建立直立车体的运动学和动力学数学模型(Euler-Lagran方法)，设计反馈控制器来保证车体的平衡(极点配置和LQR两种算法)，在此基础上进行基于LabVIEW的计算及仿真，或者基于Matlab搭建控制系统模型，给定参数，显示机器人的运动仿真图和系统响应曲线，获得两种算法的最优反馈矩阵

，系数K对应图1四个电位器实时修正。

本文的侧重点是理论与实践结合，硬件电路设计、软件设计与调试。其中，传感器数据融合与电机控制PID算法是实现小车平衡的核心内容，硬件设计结合关键源代码分析是本文的亮点。

平衡车设计过程中一般是整定PID，难点是如何运用仿真模拟车体平衡的参数，换算不同系数，将控制器设计实施到实际的设计当中。

1 基本工作原理

利用HT66FU50微控制器，使用PID 闭环控制算法和卡尔曼滤波算法完成数据融合处理，得到平滑而稳定车体控制值，通过电机驱动模块来驱动电机产生前进或后退的加速度来控制车体保持平衡，同时系统还要根据速度的反馈量来完成对车体速度和方向的控制。通过蓝牙模块与外部设备连接来控制小车的运动状态。自平衡小车的基本原理框图如图1所示。

2 加速度传感器及其源代码分析

加速度传感器的输出=地球重力(1g)×倾斜角的正弦(sin)(如图2)。当倾斜角接近90°时，相对角度变化加速度传感器的输出变化就会越来越小。将加速度传感器的输出换算成倾斜角度时需使用反正弦函数arcsin。

根据单轴加速度求角度(含弧度换算)：

Accel=arcsin(Ax)*180/π (1)

Ax加速度传感器的输出值，Accel倾斜角X轴。单轴求角度范围为+90°~-90°，超出部分出错，因此，程序要限幅，1≤Ax≤-1。

如图3(y=sinθ)，若倾斜角接近±30°的话，倾斜角与传感器输出的关系就会接近正比例关系，就没必要取正弦函数进行计算了。

Accel≈k*Ax*180/π (2)

当系数取0.92时，角度范围可以扩大到-45°~+45°。

2条检测轴需要相互正交，并且都与旋转轴垂直，随着一个轴的灵敏度下降，另一个轴的灵敏度会上升，通过使用双轴加速度传感器就能精确测量倾斜角。双轴求角度范围为+180° ~-180°，通过公式(3)计算得到。

Accel=arctan(Ax/Ay)*180/π (3)

Ax与Ay加速度传感器的输出值，Accel倾斜角。

关键源代码如下：

//加速度范围为±2g时，换算关系：16384 LSB/g，角度较小时，也可以采用近似x=sinx得到角度(弧度), deg = rad*180/3.14。

Accel_x= GetData(ACCEL_XOUT_H);

//读取X轴加速度

Angle_ax = (Accel_x - 800) /16384;

//去除零点偏移800，计算得到加速度g

Angle_ax = asin(Angle_ax)*180/3.14;

//Angle_ax = Angle_ax*180/3.14;

//反正弦求角度，而后弧度转换为度

//倾斜角与arctan(Ax/Ay)成正比(如图4)

// acc = atan2(Ax,Ay)*180/π

//角速度范围为2000deg/s时，换算关系16.4 LSB/(deg/s)

Gyro_y = GetData(Gyro_Yout_H);

//静止时角速度Y轴输出为-80左右，去除偏移

Gyro_y = -(Gyro_y + 80)/16.4;

//去零点偏移，计算角速度值，负号为方向处理

Kalman_Filter(Angle_ax,Gyro_y);

//执行卡尔曼滤波，平滑角度与角速度。

3 卡尔曼滤波及其源代码分析

卡尔曼滤波器可分为：时间更新方程和测量更新方程。时间更新方程也可视为预估方程，测量更新方程可视为校正方程。根据上一状态的估计值和当前状态的观测值推出当前状态的估计值。

卡尔曼滤波标识：ˉ代表先验，^代表估计。如表1所示，对于角度估算，先预测，可以近似认为是上一时刻的角度值加上此刻陀螺仪测得的角速度值乘以时间，因为

。通过重力加速度来矫正陀螺仪的角度漂移[7]，即为多传感器数据处理，通俗称为“数据融合”。

卡尔曼方程1：先验估计。

(4)

先验估计值，

前次运算估计值，

后验估计值(算后数值，也是再次换算的

，

为此刻陀螺仪测的角速度的值。A与B是系统参数。

卡尔曼方程4：由观测变量zk，更新估计。

(5)

为卡尔曼增益，对应源代码中的K_0 、K_1，zk就是此刻测得角度Accel，

是测得角度与先验估计的误差比较，对应源代码中的Angle_err。

部分源代码如下：

float Q_angle=0.001; //初始值

float Q_gyro=0.003;

float R_angle=0.5;

char code C_0 = 1;

void Kalman_Filter(float Accel,float Gyro)

{

//Accel角度(由加速度计求得角度)

//Gyro角速度(陀螺仪测得角速度)

Angle= Angle +(Gyro - Q_bias) * dt;

…

…

…

Angle_err = Accel - Angle;//误差

Angle = Angle + K_0 * Angle_err;/

Q_bias = Q_bias + K_1 * Angle_err; /

Gyro_x = Gyro - Q_bias;

// Angle误差与卡尔曼增益来修正后的角度

// Q_bias误差与卡尔曼增益来修正偏移

// Gyro_x卡尔曼修正后的角速度

}

理解卡尔曼滤波方程1：

等号左边Angle为此刻的角度预测值，等号右边Angle为上一时刻的值，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔， Q_bias是上一时刻陀螺仪静态偏移量。(Gyro-Q_bias)*dt就是当前状态的观测值。陀螺仪有个静态漂移，而且还是变化的，计算时要除去。

陀螺仪与加速度经过卡尔曼滤波前后波形的优劣比较，采用上位机调试工具(串口虚拟示波器)Serial_Digital_Scope V2实时观察，参考文献[4]，本文略。

4 电机控制PID算法

数字式PID控制算法可以分为直接计算法(也称位置式PID)和增量计算法。所谓增量计算法就是相对于标准算法的相邻两次运算之差，得到的结果为增量。

位置式PID算法如下：

其中k是采样序号，k=0，1，2，……;

uk是k第次采样时刻的计算机输出值;

ek是k第次采样时刻输入的偏差值;

ek-1是k-1第次采样时刻输入的偏差值;

ki是积分系数，

;

kd是微分系数，

。

Arduino平衡车的PID代码如下：

Output = kp * error+ki * errSum+kd * dErr

电机PWM控制数据，静止过程的平衡，为角度与角速度采用PD算法;行驶过程的平衡，为速度与位移采用PI算法[4]。

PWM1 =Angle_Kp* Angle + Angle_Kd*Gyro_x;

Angle反映倾角，Gyro_x反映快慢。Angle_Kd是微分控制，具有超前性，他的控制是和偏差输入的变化率有关，也就是角速度。

PWM2 =moto_Kp* speed + moto_Kpn* position;

speed为速度，position为n次速度的累积，理解为速度的积分(位移)，moto_Kpn为积分系数[6]，慎重调节。此处系数代号，编者特意不写moto_Ki，根据PID系数的公式，应为累积偏差系数。诸多文献前后系数倒置，如文献[6];文献[1]解释为D系数;文献[5]，文字解释透彻，但论文图中PID系数与源代码有不符。

最终控制参数PWM = PWM1+ PWM2，也就是双闭环PID控制。

Angle_Kp、Angle_Kd、moto_Kp、moto_Kpn对应四个电位器(如图1)，经过单片机ADC转换，可以实时PID系数的调节，可以由上位机或者液晶监控数据。

PID调参步骤：

首先，程序内置PWM2=0;

第二，设定D=0，逐渐增加P，当车体出现震荡，加入D，当出现抖动，增加P，反复调节P、D即可实现直立;

第三，加入PWM2，先P由小变到大，观察各次响应，直至得到反应快、超调小的响应，再调节积分控制作用I参数来消除控制稳态误差。

电机转速和位移值计算，关键源代码如下：

speed_dot=(speed_real_LH+speed_real_RH)*0.5;

// speed_dot为此时左右车轮速度，求平均

speed *=0.85; //车轮速度滤波，k-1时刻取值

speed += speed _dot*0.15;//车轮速度滤波+k时

position+=speed; //速度的累积

position+=Speed_Need; // Speed_Need给定速度

本文来源于中国科技核心期刊《电子产品世界》2016年第2期第51页，欢迎您写论文时引用，并注明出处。