【平衡小车入门】（PID、FreeRTOS、hal库）

前言

从代码上看，平衡小车的实现是比较简单的，特别是只实现平衡。在平衡的基础上可以加上其他功能：视觉、循迹、避障、蓝牙控制等。项目源码直接使用b站up主：会飞的摄影师呀，然后自己对标准库用的比较多，hal库的库函数大多数不熟悉，在复刻完成之后又在b站看了另一个up（天下行走）的视频，使用标准库+裸机开发，代码很简单，之后可能会在发一篇讲讲后面这个。

一、硬件需求

①MPU6050
②STM32F103C8T6
③oled液晶屏
④18650电池两节
⑤DRV8833电机驱动模块
⑥MP1584EN（5V固定输出降压模块）
⑦HC-05蓝牙模块
⑧JGA25-370直流减速电机（6V/280转每分钟）
⑨面包板或者pcb
⑩底座（用于将两电机连接固定）
其余就是一些小东西：铜柱（M3*60+6）、螺丝，螺母（M3）、杜邦线、万用表、电烙铁套件、双面胶或热熔胶、拨动开关（SS-12F44 3脚2档立式）
下面是pcb板：

pcb版小车：

面包板小车：

搞下面这个面包板的原因是pcb版的效果不好，调参调了挺久还是不那么稳，我想着应该是结构问题，pcb过宽，高度也太高，小车因为惯性就不好平稳，然后换成面包板之后重心降低了很多，重量也轻了，所以之后可能会自己画一个板子，画窄一些。

二、最终效果

①pcb平衡小车：

②面包板平衡小车：

从视频也可以看出来，面包板小车更加平稳。

三、整体流程

第一步：stm32通过DRV8833电机驱动模块使用PWM驱动直流减速电机

这是所有小车的第一步，电机不动那还怎么接下来的步骤，STM32使用定时器可以输出不同的PWM波，将PWM波输出至电机驱动模块，就可以驱动电机转起来了。


本项目中就使用上图所示的PWM控制电机速度表来控制两个电机，AIN1连接PA7、AIN2连接PA3（电机1）；BIN1连接PA6、BIN2连接PA4（电机2）。AO1、AO2接电机的驱动口，一般是最左和最右那两个，中间四个接口是电机的编码器接口。在进行组装之前，先测试电机的转动方向和转速，并且测量电机的死区，这在设置电机转速时会用到。死区的测法就是从低开始给PWM，看PWM为多少时电机开始转动，那死区就是不转动的那部分PWM区，要想让电机转动起来，那给的PWM就必须大于死区。设置电机最终转速的函数如下：

void Set_PWM(int motor1,int motor2)
{
    if(motor1>0)//正转 
		{
        PWMA2=Dead_Zone+(abs(motor1))*1.17;
        PWMA1=0;
    }
    else//反转
    {
        PWMA2 =0;
        PWMA1=Dead_Zone+(abs(motor1))*1.17;
    }
    if(motor2>0)//正转
		{
        PWMB2 = Dead_Zone+(abs(motor2))*1.17;
        PWMB1 = 0;
    }
    else
    {   PWMB2 = 0;
        PWMB1=Dead_Zone+(abs(motor2))*1.17;
    }
}

其中Dead_Zone就是死区值，abs是绝对值函数。

第二步：理解PID算法在平衡小车中的应用

在平衡小车中想实现PID算法，编码器和姿态传感器（比如MPU6050）是必不可少的。
平衡小车实现PID控制算法，一共需要三个部分：直立环、速度环、转向环。如果只想小车上电之后保持直立，不施加外部干扰的话，只需要直立环就够了；如果想要直立，并且能够抗外界干扰的话就需要直立环+速度环；如果需要蓝牙遥控啥的那就需要再加上转向换。

PID控制就是对偏差进行比例、积分和微分控制，最终目的就是使偏差接近于0。可以看出PID分成三个部分，可以任意组合，直立环一般使用PD、速度环使用PI、转向环使用PD。

总的公式是：PWM = Kpe（k）+Kie（k）的积分+Kd*e（k）的微分
e（k）就是所谓的偏差，也就是真实值-设定值。
各个环的输入都是不一样的：
①直立环的输入就是目标角度、真实角度、y轴角速度，这里的目标角度就是机械中值，需要在调试PID参数之前确定，确定方法可以看视频，由于mpu6050摆放的方向不一样，那么需要观察的值也就不一样。真实角度就是mpu6050的姿态角，根据我的摆放位置，此值就是pitch。y轴角速度就是角度偏差的微分（此次偏差-上次偏差），就是上面总的公式中Kd乘的那部分。
②速度环的输入是左右编码器读取的值、目标速度，这里如果只实现直立的话，那么目标速度肯定是0，因为想要小车静止不动的。
③转向环，根据不同的需求，参数各不相同，本项目的转向环输入是目标偏航角、z轴角速度。偏航角顾名思义就是转向相关，z轴角速度就是转向偏差的微分(此次偏差-上次偏差)。
上面说的这些直接看视频更加直接，而且很容易理解：https://www.bilibili.com/video/BV1j7411z7uX/?p=3&spm_id_from=pageDriver&vd_source=2a10d30b8351190ea06d85c5d0bfcb2a

本项目使用的是并级PID，还有一种串级PID，下面的图就是两种方法对比：

PID部分的代码也是非常简单，前提是移植了别人写好的MPU6050相关函数，不然要自己解算姿态角的话还是很麻烦的：

/**************************************************************************************************************
*函数名:Vertical_Ring_PD()
*功能:直立环PD控制
*形参:(float Angle):x轴的角度/(float Gyro):y轴的角速度
*返回值:经过PID转换之后的PWM值
**************************************************************************************************************/
//直立环的PD


int	Vertical_Ring_PD(float Angle,float Gyro)
{
    float Bias;//目标偏差
    int balance;
    Bias=Angle-Mechanical_balance;//目标值减去机械中值（不一定为0）即测量值-理论值这是p算法要用的
    balance=PID.Balance_Kp*Bias+ Gyro*PID.Balance_Kd;//这里的Kd是偏差的微分，离散系统中两次角度的差值也就是微分，对应就是角速度，这里传入的Gyro就是y轴角速度，
	  //这要根据平衡小车上MPU6050摆放的方向有关，因为小车只会往两个方向倒下，这里看mpu6050上的标注就好了，我这里是沿着y轴两边倒下，也就是y轴的角速度。
	 //第一个参数是当前角度，这就是读取mpu6050的原始数据，然后经过dmp解算之后得到的欧拉角，这里传进来的就是pitch，俯仰角，就是此时小车直立的角度。

    return balance;

    //printf("balance = %f\n",balance);
}


/**************************************************************************************************************
*函数名:Vertical_speed_PI()
*功能；速度环PI控制
*形参:(int encoder_left):左轮编码器值/(int encoder_right):编码器右轮的值/(float Angle):x轴角度值
*返回值:
**************************************************************************************************************/

int Vertical_speed_PI(int encoder_left,int encoder_right,float Angle,float Movement )
{
    static float Velocity,Encoder_Least,Encoder;//Encoder是经过一定程度滤波后的速度测量值，Encoder_Least是最新数据
    static float Encoder_Integral;
    Encoder_Least =(encoder_left+encoder_right)-0;    //获取最新速度偏差=测量速度（左右编码器之和）-目标速度（此处为零）
    Encoder *= 0.8f;																	//一阶低通滤波器 ，上次的速度占80%，f为滤波系数
    Encoder += Encoder_Least*0.2f;                   //一阶低通滤波器， 本次的速度占20%   
    Encoder_Integral +=Encoder;                       //积分出位移 积分时间：10ms
    Encoder_Integral=Encoder_Integral-Movement;
    //if(Movement == 0 ) Encoder_Integral=0;
    if(Encoder_Integral>10000)  	Encoder_Integral=10000;           //积分限幅
    if(Encoder_Integral<-10000)	  Encoder_Integral=-10000;            //积分限幅
 
    Velocity=Encoder*PID.Velocity_Kp+Encoder_Integral*PID.Velocity_Ki;//速度控制，P算法为Kp*误差，误差等于测量值-理论值，此时目标速度为0
	                                                                    //，所以直接Kp*测量值（这里的测量值是经过一阶低通滤波后的值）


    if(Turn_off(Angle)==1)   Encoder_Integral=0;            //电机关闭后清除积分
    return Velocity;
}


/**************************************************************************************************************
*函数名:Vertical_turn_PD()
*功能:转向环PD
*形参:taget_yaw 目标yaw， yaw 陀螺仪yaw ， gyro 陀螺仪yaw方向角速度
*返回值:无
***************************************************************************************************************/
int Vertical_turn_PD(float taget_yaw,float yaw,float gyro)
{
    float Turn;
    float Bias_yaw;
    Bias_yaw=taget_yaw-yaw;
		if (Bias_yaw<-180) Bias_yaw+=360;
		if (Bias_yaw>180) Bias_yaw-=360;
	
    Turn=-Bias_yaw*PID.Turn_Kp-gyro*PID.Turn_Kd;
    return Turn;
}

第三步：PID调参

这里强烈建议观看b站视频，跟着进行调参：https://www.bilibili.com/video/BV1ib411v7YU/?p=10&spm_id_from=pageDriver&vd_source=2a10d30b8351190ea06d85c5d0bfcb2a
调参才是整个小车制作过程中最折磨的，会一直觉得调的还不够好，但其实这是多方面因素决定的。

四、源代码获取

关注+点赞就可以私信免费获取