基于PID控制的智能平衡车设计与实现

难度系数：4分工作量：4分创新点：3分1 课题背景

随着机器人研究的进一步深入，在工业生产、安防系统、智能家居、物流网等领域

的应用更加广泛， 在实际应用中， 可能遇到复杂的任务环境。

相比多轮的轮式机器人，两轮自平衡机器人体积小， 运动灵活， 能够在比较狭窄、 需要大转角的场合中运动。 这种机器人两轮共轴，可以通过运动保持自身平衡，能实现前进、 后退、 转向、 原地静止等基本运动功能， 由于其结构特殊， 能适应不同的地形环境， 研究两轮自平衡机器人，具有重要的意义。

2 设计内容

使用 ARM Cortex-M4 内核的 Freescale K60 单片机为主控制器， 对加速度计和陀螺

仪的数据进行融合， 得到车身倾角最优估计， 设计 MOS 电机驱动电路， 编写 PID 控制算法控制车轮， 达到自主直立的目的。 利用编码器构成速度反馈回路， 使用 PID 控制算法进行速度和转向的控制， 最终使其保持平衡、 实现期望的运动。 本文将就倾角融合算法、机器人控制算法等问题展开研究， 具体内容如下：

1） 硬件电路设计： 设计制作主控电路板、 电机驱动控制板。2） 传感器数据融合： 将陀螺仪所测数据和加速度计所测数据进行融合， 得到稳定、可靠的倾角值。3） 控制程序设计： 通过对状态反馈控制、 PID 算法等方法和理论的研究， 设计控制程序， 使小车能够完成直立、 前进、 后退、 转向等运动功能。4）单片机程序设计： 设计单片机程序， 使其具备人机交互、 控制等功能。

3 设计方案3.1 设计思路

从控制的角度来看， 电机是系统唯一的控制对象。 车模运动控制任务可以分解成以下三个基本控制任务：

（1） 直立控制任务： 车模的倾角作为控制的输入量， 使用PD算法， 控制车模稳定在平衡位置。（2） 速度控制任务： 直立车模的速度控制与普通的车模速度控制不同， 在直立系统中， 速度控制是通过改变车模倾角来完成的。 具体实施思路是， 对电机转速加入干扰，使车身偏离平衡位置， 以此刺激直立控制任务， 从而达到控制速度的目的， 速度控制使用PI算法。（3）方向控制任务： 通过控制两个电机的差速来达到转向的目的， 方向控制使用PD算法， 使用X轴的角速度作为微分项的因子， 可以极大改善转向的动态性能， 避免振荡。

程序设计中， 三个控制任务独立进行计算。 但是每一个任务的控制对象都是电机，因此它们直接也存在着干扰与耦合。 在设计每一个控制任务时， 为了便于分析， 都假设其他两个任务是稳定的。 例如， 在进行速度控制程序设计时， 车模是能够稳定直立的；在进行方向控制程序设计时， 车模的直立控制和速度控制都是稳定的；

在进行直立控制时， 车模的速度控制和方向控制都是稳定的。这三个任务中保持车模平衡是最关键的。 由于车模同时受到三种控制的影响， 从车模平衡控制的角度来看， 其它两个控制就成为它的干扰。 为了避免影响车模平衡控制，这个车模倾角的改变需要非常缓慢的进行。 因此， 虽然三个控制任务独立运行， 但是它们之间有优先级， 即控制应该最优先满足直立的要求， 其次是方向控制的要求， 最后才是速度控制的要求。

3.3 硬件设计

主控电路板主要包括以下部分： 微控制器电路、 电源管理电路、 微控制器接口、 按键电路、蜂鸣器电路。

其中，电源管理电路分为 3. 3V 电源管理电路和 5V 电源管理电路，5V 管理电路使用 LM2940 三端线性稳压器， 输入 7. 2V 电池电压， 输出 5V 电压。

3.3V 管理电路使用 LM1117 三端线性稳压器， 输入接 LM2940 的 5V 电压， 输出 3. 3V 电压。

考虑到本系统中器件、 传感器较多， 因此 5V 管理电路和 3. 3V 管理电路均使用两个。

微控制器接口主要包括： OLED 接口、 蓝牙接口、 MMA7361 传感器接口、 L3G4200D 传感器接口、编码器正交解码接口 2 个、 四通道 PWM 接口、 遥控器解码接口， 以及预留 IO，方便调试使用。

3.4 软件设计

小车的控制主干流程如下：

3.4.1 关键技术 -PID控制算法

在自动控制中， 按照偏差（目标值与反馈值之差） 的比例§ 、 积分（I） 、 微分（D）

的组合进行控制的方法称为PID控制算法， 由于其原理简单、 稳定性好、 可靠性高、 易

于调整等优点， 在自动控制领域应用最为广泛， 已有近70年历史， 现有的很多控制方法

都是基于PID控制算法发展演变而来。 当我们对目标系统和控制对象的模型不够了解，或者不能得到控制系统的参数时， PID控制算法尤为适用。

1） 比例控制部分： 成比例地反应控制系统的偏差信号e(t) ， 偏差一旦产生， 调节器

立即产生控制作用以减小偏差， 其控制作用最为明显。

2） 积分控制部分： 积分控制作用的强弱取决于积分时间常数Ti， Ti越大， 积分作用

越弱， 反之则越强。 积分控制主要用于消除静差， 提高系统的无差度。

3）微分控制部分： 微分控制的计算因子是偏差的变化率， 能够抑制偏差的变化， 并偏差的值变得太大之前， 引入一个早期修正量， 达到加快系统响应调节速度的目的。积分控制可以提高系统的动态性能， 提高反应速度， 克服振荡。

PID控制算法的计算结果是偏差信号的比例、 积分、 微分三部分的线性叠加， 偏差值e(t) 即目标值r(t) 与反馈值c(t) 的差， 属于闭环控制系统， 因此需要使用传感器构成反馈回路。

（代码实现）

3.4.2 关键技术 - 倾角估计算法

要控制车模平衡， 首先就需要测量车身倾角。 测量倾角主要有两种方法:

1. 利用加速度传感器。 加速度计静止放置的时候， 因为受到重力加速度的影响， 三个轴感应到的加速度跟传感器与地平面之间的倾角有关， 具体关系如下图所示。

加速度传感器的值与地面之间的倾角呈三角函数关系：

然而小车在运动过程中， 会产生极大的噪声， 取加速度传感器瞬时值计算倾角误差太大。 难以获得稳定的倾角数据。

2.利用陀螺仪测量的角速度进行积分。

角速度积分可以得到变化的角度。 但是传感器

都有误差， 陀螺仪也不例外， 而且陀螺仪还存在温漂， 因此长时间的积分过程， 很难保

证得到正确的角度。

因此需要一种算法， 能够结合陀螺仪的动态性能和加速度传感器的静态精确度， 从而得到可靠的倾角数据。

3.4.3 关键技术 - 直立控制算法

车模平衡控制需要负反馈， 就需要能够测量车体的倾角， 构成反馈回路。 在能够测量倾角的情况下， 设定目标平衡角度， 使用 PID 算法进行闭环控制。 因为车体只会在一个的方向上运动， 只存在一个维度的倾斜， 因此只需要测量一个维度的倾角， 然后控制轮子转动， 抵消车体在该维度上的倾斜， 就可以完成平衡控制

我们对系统进行简单建模， 首先， 两轮自平衡车可以简化为一个一阶倒立摆模型，假设车体的高度为 L， 质量为 m ， 在外力作用下， 车模产生角加速度为 x(t) 。

如下图所示， 沿着垂直于底盘的方向进行受力分析， 可以得到运动加速度 a(t) 以及外力干扰加速度 x(t) 之间的运动方程

控制流程

void Angle_Calculate(){int16_t AngleControlOut_P,AngleControlOut_D;angle_Accel = (A_Z-AZ_ZERO);angle_Accel*=0.0039f;angle_Accel = asin(angle_Accel);angle_Accel*=500;Gyro_Now = (T_Y-TY_OFFSET) * TY_Ratio;第 26 页 共 47 页Erjie_Lvbo(angle_Accel,Gyro_Now);if(FilterSwitch==0) myfilter=QingHua_AngleCalaulate;else if(FilterSwitch==1)myfilter=Yijie_Lvbo;else if(FilterSwitch==2) myfilter=Erjie_Lvbo;else if(FilterSwitch==3) myfilter=Kalman_Filter;if(FilterSwitch==3){myfilter(angle_Accel,Gyro_Now);ZL.error=angle_now_kal-(Balance_Point);}else{myfilter(angle_Accel,Gyro_Now);ZL.error=angle_now-(Balance_Point);}AngleControlOut_P=-(int16_t)(ZL.P*ZL.error);AngleControlOut_D=-(int16_t)(ZL.D*Gyro_Now);if(AngleControlOut_D>500) AngleControlOut_D=500;//对微分项限幅else if(AngleControlOut_D<-500) AngleControlOut_D=-500;AngleControlOut=AngleControlOut_P+AngleControlOut_D;if(AngleControlOut>800) AngleControlOut=800;else if(AngleControlOut<-800) AngleControlOut=-800;}

3.4.4 速度控制

直立车模的速度控制方法与普通车模的速度控制方法不同， 因为普通车模的电机只需要控制转动， 而直立车模的电机输出是平衡控制、 速度控制、 方向控制三部分之间的叠加。

因为直立系统的首要任务是保持直立， 当车模倾角发生改变， 车模就会在直立控制的作

用下向倾斜方向运动。 速度控制就是利用了直立系统的这种特性。

改变车模倾角有两种方法：

（1） 改变车模直立控制的目标角度， 直立控制 PD 算法中， 有一个目标倾角， 当目

标倾角不是真正的平衡位置倾角时， 车模会一直运动。 通过编码器测量转速， 形成

速度反馈回路， 就可以使用 PID 算法进行速度调节。

（2） 给电机施加干扰输出。 在直立过程中， 如果增加或者减少电机输出， 车模就很难维持平衡， 在直立控制的作用下， 车模就会运行。 通过编码器测量转速， 形成速度反馈回路， 就可以使用 PID 算法进行速度调节。

学长设计的系统中，使用第二种方法对速度进行控制， 因为第二种方法对直立的影响较小，可以提高系统的稳定性， 但是速度控制的灵敏度没有第一种高。

void Speed_Calculate(){SP.goal=(t1-t1_mid)/10;if(SP.goal>50) SP.goal=50;else if(SP.goal<-30) SP.goal=-30;SpeedControlOut_Old=SpeedControlOut;SP.error=(SP.goal)-(Speed_L+Speed_R)/2;SP.error_sum+=SP.error;if(SP.error_sum>200) SP.error_sum=200;//积分限幅else if(SP.error_sum<-200) SP.error_sum=-200;SpeedControlOut=-(int16_t)((SP.P)*SP.error+SP.I*SP.error_sum);if(SpeedControlOut>300) SpeedControlOut=300;//速度环输出限幅else if(SpeedControlOut<-300) SpeedControlOut=-300;}

直立车模的转向是通过两轮的差速来完成的， 转向控制使用 PD 算法， 小车不具备自主识别路径的功能， 因此 PD 算法的输入参数使用遥控器完成。 为了改善转向的动态特性， 避免振荡， 对 PD 算法做了修改， 微分项的因子不再使用传统的 e(t) -e(t-1) ，而是使用陀螺仪的 X 轴， 因为陀螺仪输出灵敏度更高。

void Direction_Calculate(int16_t bias){DIR.last_error=DIR.error;DIR.error=bias/6-dmp_yaw;DIR.error_sum+=DIR.error;if(DIR.error_sum>2000) DIR.error_sum=2000;else if(DIR.error_sum<-2000) DIR.error_sum=-2000;DirectionControlOut_Old=DirectionControlOut;DirectionControlOut = DIR.P*DIR.error+ DIR.I*DIR.error_sum+ DIR.D*(T_X-TX_OFFSET);DirectionControlOut_Old=DirectionControlOut;DirectionControlOut=bias*0.15f;if(DirectionControlOut>300) DirectionControlOut=300;else if(DirectionControlOut<-300) DirectionControlOut=-300;}

完整代码可进群免费领取。

嵌入式物联网的学习之路非常漫长，不少人因为学习路线不对或者学习内容不够专业而错失高薪offer。不过别担心，我为大家整理了一份150多G的学习资源，基本上涵盖了嵌入式物联网学习的所有内容。点击下方链接，0元领取学习资源，让你的学习之路更加顺畅！记得点赞、关注、收藏、转发哦！

点击这里找小助理0元领取：扫码进群领资料