基于PID控制算法的自平衡机器人平衡小车设计

叶绿体不忘呼吸

于 2025-02-27 16:19:02 发布

阅读量1.2k

点赞数 22

分类专栏：单片机文章标签：机器人 pid 控制算法单片机平衡小车自平衡机器人

本文链接：https://blog.csdn.net/m0_46653805/article/details/145906508

版权

单片机专栏收录该内容

22 篇文章

订阅专栏

文末获取完整源码源文件+配套论文+答辩PPT等

在这里插入图片描述

随着机器人研究的进一步深入，在工业生产、安防系统、智能家居、物流网等领域的应用更加广泛，在实际应用中，可能遇到复杂的任务环境。相比多轮的轮式机器人，两轮自平衡机器人体积小，运动灵活，能够在比较狭窄、需要大转角的场合中运动。这种机器人两轮共轴，可以通过运动保持自身平衡，能实现前进、后退、转向、原地静止等基本运动功能，由于其结构特殊，能适应不同的地形环境，研究两轮自平衡机器人，具有重要的意义。

本设计以两轮车模为研究平台，以恩智浦公司32位微处理器MK60FX512VLQ15为控制核心。通过加速度传感器和陀螺仪，设计滤波算法，实现了倾角的测量；通过设计MOS电机驱动电路，使用单片机输出PWM，实现了电机速度控制；通过编写控制算法，实现了两轮车模的自平衡功能；通过编码器测量电机转速，对速度进行闭环控制，实现了两轮自平衡机器人的速度控制功能。最终通过无线遥控器，使小车能够在自平衡的基础上，实现自由运动的任务。

关键词：单片机，PID，倾角测量，MOS驱动，自平衡

1 绪论

1.1研究目的和意义
1.2国内外发展现状
1.3设计内容
使用ARM Cortex-M4内核的Freescale K60单片机为主控制器，对加速度计和陀螺仪的数据进行融合，得到车身倾角最优估计，设计MOS电机驱动电路，编写PID控制算法控制车轮，达到自主直立的目的。利用编码器构成速度反馈回路，使用PID控制算法进行速度和转向的控制，最终使其保持平衡、实现期望的运动。本文将就倾角融合算法、机器人控制算法等问题展开研究，具体内容如下：
1）硬件电路设计：设计制作主控电路板、电机驱动控制板。
2）传感器数据融合：将陀螺仪所测数据和加速度计所测数据进行融合，得到稳定、可靠的倾角值。
3）控制程序设计：通过对状态反馈控制、PID算法等方法和理论的研究，设计控制程序，使小车能够完成直立、前进、后退、转向等运动功能。
4）单片机程序设计：设计单片机程序，使其具备人机交互、控制等功能。

2 总体设计方案

2.1 设计思路

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

程序设计中，三个控制任务独立进行计算。但是每一个任务的控制对象都是电机，因此它们直接也存在着干扰与耦合。在设计每一个控制任务时，为了便于分析，都假设其他两个任务是稳定的。例如，在进行速度控制程序设计时，车模是能够稳定直立的；在进行方向控制程序设计时，车模的直立控制和速度控制都是稳定的；在进行直立控制时，车模的速度控制和方向控制都是稳定的。
这三个任务中保持车模平衡是最关键的。由于车模同时受到三种控制的影响，从车模平衡控制的角度来看，其它两个控制就成为它的干扰。为了避免影响车模平衡控制，这个车模倾角的改变需要非常缓慢的进行。因此，虽然三个控制任务独立运行，但是它们之间有优先级，即控制应该最优先满足直立的要求，其次是方向控制的要求，最后才是速度控制的要求[7]。

在这里插入图片描述

2.2 硬件设计方案
本系统采用一个主控电路板+一个电机驱动电路板的结构，将两者分开设计，一是可以避免电机驱动对主控的影响及电磁干扰，二是出于经济的考虑。
主控电路板主要包括以下部分：微控制器电路、电源管理电路、微控制器接口、按键电路、蜂鸣器电路。其中，电源管理电路分为3.3V电源管理电路和5V电源管理电路，5V管理电路使用LM2940三端线性稳压器，输入7.2V电池电压，输出5V电压。3.3V管理电路使用LM1117三端线性稳压器，输入接LM2940的5V电压，输出3.3V电压。考虑到本系统中器件、传感器较多，因此5V管理电路和3.3V管理电路均使用两个。微控制器接口主要包括：OLED接口、蓝牙接口、MMA7361传感器接口、L3G4200D传感器接口、编码器正交解码接口2个、四通道PWM接口、遥控器解码接口，以及预留IO，方便调试使用。

在这里插入图片描述
2.3 机械结构设计方案

3 硬件电路设计

3.1 单片机最小系统电路
3.2 加速度传感器模块
MMA7361是恩智浦公司（原飞思卡尔）的微型电容式加速度传感器，其功耗低，体积小，价格便宜。内部集成一级低通滤波器，具有温度补偿、自我测试，自由落体检测等功能。运行电压2.2-3.6V，模拟量输出，具有两种不同的灵敏度可选择。正常工作状态下电流消耗400uA，在休眠模式下仅需3uA电流。MMA7361的芯片引脚图如下图3-1所示，引脚排列如下表3.1所示：
在这里插入图片描述

在这里插入图片描述

3.3 陀螺仪模块
3.4 电机驱动模块
单片机要控制电机转动，需要电机驱动电路。电机驱动电路的性能直接关系到车模的性能，进而关系到整个系统的稳定性。常见的电机驱动方案有两种：
（1）采用集成电机驱动芯片；
使用集成电机驱动芯片设计（常用的电机驱动芯片有BTS7970、L298N等），外围电路简单、可靠性高，但是驱动的性能有限。由于本系统车模的电机的内阻小于10毫欧，而集成驱动芯片内部的每个场效应管导通电阻在 120 毫欧以上，这样，电机驱动回路总电阻就太大了，驱动电路的输出功率和发热量都很大，电机性能得不到充分发挥。
（2）采用N沟道场效应管和专用栅极驱动芯片设计。
分立的 N 沟道场效应管的导通电阻很低，一般小于10毫欧，大大减小了电机驱动回路总电阻。另外，专用的栅极驱动芯片可以提高场效应管的开关速度，使PWM控制方式的调制频率提高，从而减少电枢电流脉动[9]。
经过多次试验与调试，我们最终选用大电路电机半桥驱动芯片IR2184，它可以驱动高端和低端两个N沟道场效应管，能提供较大的栅极驱动电流，并具有硬件死区、硬件防同臂导通等功能。使用两片 IR2184型半桥驱动芯片可以组成完整的直流电机 H 桥式驱动电路。其性能优异、价格便宜，所以我们选择它进行设计[10]。

在这里插入图片描述
3.5 电源管理模块

4 软件程序设计

4.1 系统流程图
在这里插入图片描述
4.2 程序初始化

4.3 PID控制算法
4.3.1 PID控制算法介绍
4.3.2 PID控制算法参数整定

4.4 倾角估计算法
要控制车模平衡，首先就需要测量车身倾角。测量倾角主要有两种方法:
1.利用加速度传感器。加速度计静止放置的时候，因为受到重力加速度的影响，三个轴感应到的加速度跟传感器与地平面之间的倾角有关，具体关系如下图所示[20]。
在这里插入图片描述

加速度传感器的值与地面之间的倾角呈三角函数关系：
在这里插入图片描述

然而小车在运动过程中，会产生极大的噪声，取加速度传感器瞬时值计算倾角误差太大。难以获得稳定的倾角数据。
2. 利用陀螺仪测量的角速度进行积分。角速度积分可以得到变化的角度。但是传感器都有误差，陀螺仪也不例外，而且陀螺仪还存在温漂，因此长时间的积分过程，很难保证得到正确的角度[21]。
因此需要一种算法，能够结合陀螺仪的动态性能和加速度传感器的静态精确度，从而得到可靠的倾角数据[22]。

4.4.1 互补滤波法
互补滤波算法，在短时间内采用陀螺仪得到的角度值为最优解，通过加速度传感器的计算结果对角度值进行校正。加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器的特性不同，通过不同的滤波器（高通、低通），进而得到整个频带的信号。互补滤波器参数少，程序简单，在低成本的INS导航系统中应用广泛[23]。
以下是互补率程序，在使用中，根据实际情况和需要调节K1和dt，直到满意为止。其中，K1是互补滤波中的权值，K1增大，对加速度传感器的信任程度就越大[24]。

 float angle;
float   K=0.08f;               // 对加速度计取值的权重
float   dt=0.003f;             //采样时间
void Yijie_Lvbo(float accel_pro,float gyro_pro)
{
    angle=K * accel_pro+ (1-K) * (angle + gyro_pro * dt);
}

4.4.2 卡尔曼滤波法
卡尔曼滤波法源于匈牙利数学家Rudolf Emil Kalman的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。卡尔曼滤波算法是一个最优化自回归数据处理算法[25]。对于很多工程问题，它的效率很高，可靠性很高。卡尔曼滤波法广泛应用已经超过了30年，包括控制、制导、导航、通讯等现代工程等，近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等[26]。

4.5 直立控制程序设计
车模平衡控制需要负反馈，就需要能够测量车体的倾角，构成反馈回路。在能够测量倾角的情况下，设定目标平衡角度，使用PID算法进行闭环控制。因为车体只会在一个的方向上运动，只存在一个维度的倾斜，因此只需要测量一个维度的倾角，然后控制轮子转动，抵消车体在该维度上的倾斜，就可以完成平衡控制[27]。如图2.2.1所示。
在这里插入图片描述
我们对系统进行简单建模，首先，两轮自平衡车可以简化为一个一阶倒立摆模型，假设车体的高度为 L，质量为 m ，在外力作用下，车模产生角加速度为x(t)。如图4.5所示，沿着垂直于底盘的方向进行受力分析，可以得到运动加速度a(t)以及外力干扰加速度x(t)之间的运动方程。

在这里插入图片描述
其中k1,k2分别为比例与微分控制参数。其中微分参数相当于阻尼力，可以有效抑制车模荡[28]。
因此，车模的平衡控制可以简化为测量车模的倾角和倾角速度控制车模车轮的加速度来消除车模的倾角。

在这里插入图片描述
以下是直立控制程序：

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;
	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;
}

4.6 速度控制程序设计
4.7 方向控制程序设计

5 开发工具及调试结果

5.1 开发工具介绍
5.2 调试结果
5.2.1 PID算法控制电机调试结果
为了学习和验证PID算法，设计了位置式PID算法在电机空载情况下进行测试，经过大量的参数整定和观察，掌握了PID算法的参数整定方法，以下是调试结果。
在这里插入图片描述
如图所示，黄色代表速度的目标，红色是测量的实际速度。

5.2.1 互补滤波法调试结果
互补滤波主要调节的是对于加速度计的比重K,K越大，则跟踪速度越快，但滤波效果越差。根据需要，选择适合系统的K系数。下面两幅图分别为K=0.01,和K=0.05时的滤波效果。红色代表加速度计数据，黄色代表陀螺仪数据，蓝色代表最终融合数据。当K=0.01时，滤波效果更明显，平滑度很高。当K=0.05时，跟踪效果好，平滑度差。
在这里插入图片描述
5.2.1 卡尔曼滤波法调试结果
以下是卡尔曼滤波法的滤波效果，滤波效果明显，收敛速度快，本系统使用卡尔曼滤波法作为倾角估计的方法。