树莓派ROS stm32 slam Freertos VFH+A*避障路径规划-智能平衡计划（四）
作者：互联网
基于树莓派ROSstm32搭载Freertos智能平衡车Day4
前言
PID控制功能原理与实现
提示：以下是本篇文章正文内容，下面案例可供参考
一、PID控制功能
小车直立行走任务分解
我们要求车模在直立的状态下以两个轮子在地面上随意行走，相比四轮
着地状态，车模控制任务更为复杂。为了能够方便找到解决问题的办法，首先将复杂的问题分解成简单的问题进行讨论。
从控制角度来看，车模作为一个控制对象，它的控制输入量是两个电机的转动速度。车模运动控制任务可以分解成以下三个基本控制任务
（1） 控制车模平衡：通过控制两个电机正反向运动保持车模直立平衡状态；
（2） 控制车模速度：通过调节车模的倾角来实现车模速度控制，实际上最后还是演变成通过控制电机的转速来实现车轮速度的控制。
（3） 控制车模方向：通过控制两个电机之间的转动差速实现车模转向控制
平衡控制
控制车模平衡的直观经验来自于人们日常生活经验。一般的人通过简单练习就可以让一个直木棒在手指尖上保持直立。这需要两个条件：一个是托着木棒的手掌可以移动；另一个是眼睛可以观察到木棒的倾斜角度和倾斜趋势（角速度）。通过手掌移动抵消木棒的倾斜角度和趋势，从而保持木棒的直立。这两个条件缺一不可，实际上就是控制中的负反馈机制。
PID控制理论
所谓PID算法，是一种在工程应用领域被使用最为广泛的反馈调节方法，通过PID算法中比例、积分、微分三个部分的作用，达到使系统稳定的效果
（1）比例调节：反应系统的基本（当前）偏差 e(t)，系数大，可以加快调节，减小误差，但过大的比例使系统稳定性下降，甚至造成系统不稳定；
（2）积分调节：反应系统的累计偏差，使系统消除稳态误差，提高无差度，因为有误差，积分调节就进行，直至无误差；
（3）微分调节：反映系统偏差信号的变化率 e(t)-e(t-1)，具有预见性，能预见偏差变化的趋势，产生超前的控制作用，在偏差还没有形成之前，已被微分调节作用消除，因此可以改善系统的动态性能。但是微分对噪声干扰有放大作用，加强微分对系统抗干扰不利。
注：积分和微分都不能单独起作用，必须与比例控制配合。
PID调节
根据直立小车平衡任务分解：
直立环
速度环
方向环
start
配置参考
树莓派ROS stm32 slam Freertos VFH+A避障路径规划-智能平衡计划
树莓派ROS stm32 slam Freertos VFH+A避障路径规划-智能平衡计划（三）
控制GPIO原理图也是参考以上
控制库代码contrl.c：
#include "math.h"
#include "stdlib.h"
#include "stm32f4xx_hal.h"
#include "contrl.h"
int Dead_Zone=1200; //电机死区
int control_turn=64; //转向控制
//PID调节参数
struct pid_arg PID = {
.Balance_Kp=200,
.Balance_Kd=1,
.Velocity_Kp=-52,
.Velocity_Ki=-0.26,
.Turn_Kp = 18,
.Turn_Kd = 0.18,
};
/**************************************************************************************************************
*函数名:Read_Encoder()
*功能:读取编码器值(当作小车当前前进的速度)
*形参:(u8 TIMX):x为编码器1或者2
*返回值:无
*************************************************************************************************************/
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 1: Encoder_TIM= (short)TIM1 -> CNT; TIM1 -> CNT=0;break;
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
/**************************************************************************************************************
*函数名:Vertical_Ring_PD()
*功能:直立环PD控制
*形参:(float Angle):x轴的角度/(float Gyro):x轴的角速度
*返回值:经过PID转换之后的PWM值
**************************************************************************************************************/
//直立环的PD
int Vertical_Ring_PD(float Angle,float Gyro)
{
float Bias;
int balance;
Bias=Angle-Mechanical_balance;
balance=PID.Balance_Kp*Bias+ Gyro*PID.Balance_Kd;
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;
static float Encoder_Integral;
Encoder_Least =(encoder_left+encoder_right)-0; //获取最新速度偏差=测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.8f; //一阶低通滤波器 ，上次的速度占85%
Encoder += Encoder_Least*0.2f; //一阶低通滤波器， 本次的速度占15%
Encoder_Integral +=Encoder; //积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement;
if(Encoder_Integral>10000) Encoder_Integral=10000; //积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //积分限幅
Velocity=Encoder*PID.Velocity_Kp+Encoder_Integral*PID.Velocity_Ki; //速度控制
if(Turn_off(Angle)==1) Encoder_Integral=0; //电机关闭后清除积分
return Velocity;
}
/**************************************************************************************************************
*函数名:Vertical_turn_PD()
*功能:转向环PD
*形参:无 CCD小于34左转、CCD大于64右转。 yaw = z轴陀螺仪数值
*返回值:无
***************************************************************************************************************/
int Vertical_turn_PD(u8 CCD,short yaw)
{
float Turn;
float Bias;
Bias=CCD-64;
Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
return Turn;
}
/**************************************************************************************************************
*函数名:PWM_Limiting()
*功能:PWM限幅函数
*形参:无
*返回值:无
***************************************************************************************************************/
void PWM_Limiting(int *motor1,int *motor2)
{
int Amplitude=5800;
if(*motor1<-Amplitude) *motor1=-Amplitude;
if(*motor1>Amplitude) *motor1=Amplitude;
if(*motor2<-Amplitude) *motor2=-Amplitude;
if(*motor2>Amplitude) *motor2=Amplitude;
}
/**************************************************************************************************************
*函数名:Turn_off()
*功能:关闭电机
*形参:(const float Angle):x轴角度值
*返回值:1:小车当前处于停止状态/0:小车当前处于正常状态
***************************************************************************************************************/
u8 FS_state;
u8 Turn_off(const float Angle)
{
u8 temp;
if(fabs(Angle)>80){
FS_state=1;
temp=1;
AIN2(0), AIN1(0);
BIN1(0), BIN2(0);
}
else
temp=0;
FS_state=0;
return temp;
}
/**************************************************************************************************************
*函数名:Set_PWM()
*功能:输出PWM控制电机
*形参；(int motor1):电机1对应的PWM值/(int motor2):电机2对应的PWM值
*返回值:无
*************************************************************************************************************/
void Set_PWM(int motor1,int motor2)
{
if(motor1>0) AIN2(1), AIN1(0);
else AIN2(0), AIN1(1);
PWMA=Dead_Zone+(abs(motor1))*1.17;
if(motor2>0) BIN1(1), BIN2(0);
else BIN1(0), BIN2(1);
PWMB=Dead_Zone+(abs(motor2))*1.17;
// printf("PWMA = %d\n",PWMA);
// printf("PWMB = %d\n",PWMB);
}
控制库代码contrl.h：
#ifndef _CONTRIL_H_
#define _CONTRIL_H_
#include "sys.h"
//机械0点
#define Mechanical_balance 0
#define AIN1(PinState) HAL_GPIO_WritePin( GPIOE, GPIO_PIN_13, (GPIO_PinState)PinState)
#define AIN2(PinState) HAL_GPIO_WritePin( GPIOE, GPIO_PIN_15, (GPIO_PinState)PinState)
#define BIN1(PinState) HAL_GPIO_WritePin( GPIOC, GPIO_PIN_3, (GPIO_PinState)PinState)
#define BIN2(PinState) HAL_GPIO_WritePin( GPIOA, GPIO_PIN_3, (GPIO_PinState)PinState)
#define PWMA TIM4->CCR1
#define PWMB TIM5->CCR3
extern volatile int Encoder_Left,Encoder_Right; //编码器左右速度值
struct pid_arg{
float Balance_Kp;
float Balance_Ki;
float Balance_Kd;
float Velocity_Kp;
float Velocity_Ki;
float Velocity_Kd;
float Turn_Kp;
float Turn_Ki;
float Turn_Kd;
};
extern struct pid_arg PID;
int Read_Encoder(u8 TIMX);
int Vertical_Ring_PD(float Angle,float Gyro);
int Vertical_speed_PI(int encoder_left,int encoder_right,float Angle,float Movement );
int Vertical_turn_PD(u8 CCD,short yaw);
void PWM_Limiting(int *motor1,int *motor2);
u8 Turn_off(const float Angle);
void Set_PWM(int motor1,int motor2);
#endif
结构
直立环速度环转向环PID控制原理以及实现
小车直立环调节
平衡小车直立环使用PD（比例微分）控制器，其实一般的控制系统单纯的P控制或者PI控制就可以了，但是那些对干扰要做出迅速响应的控制过程需要D（微分）控制。
//形参:(float Angle):小车的俯仰角 /(float Gyro):x轴的角速度
Int Vertical_Ring_PD(float Angle, float Gyro)
{
float Bias;
int balance;
Bias=Angle-Mechanical_balance; //计算直立偏差
balance=PID.Balance_Kp*Bias + Gyro*PID.Balance_Kd; //计算直立PWM
return balance; //返回直立PWM
}
直立环的调试过程包括确定平衡小车的机械中值、确定kp值的极性（也就是正负号）和大小、kd值的极性和大小等步骤。
1、确定平衡车的机械中值：
把平衡小车放在地面上，绕电机轴旋转平衡小车，记录能让小车接近平衡的角度，一般都在0°附近的。我们调试的小车正好是0度，所以就是Bias=Angle-0
2_1、确定kp值的极性（令kd=0）
首先我们估计kp的取值范围。我们的PWM设置的是8000代表占空比100%，再考虑避免电机的死区，我们直立环返回的PWM在6000左右的时候电机就会满载。
假如我们设定kp值为800，那么平衡小车在±10°的时候就会满转。根据我们的感性认识，这显然太大了，那我们就可以估计kp值在0~800之间。
首先大概我们给一个值kp=-200,我们可以观察到，小车往哪边倒，电机会往那边加速让小车到下，就是一个我们不愿看到的正反馈的效果。说明kp值的极性反了，接下来我们设定kp=200,这个时候可以看到平衡小车有直立的趋势，虽然响应太慢，但是，我们可以确定kp值是正的。具体的数据接下来再仔细调试。
2_2 、确定kp值的大小（令kd=0）
设定kp=100,这个时候我们可以看到，小车虽然有平衡的趋势，但是显然响应有点慢了。
设定kp=250,这个时候我们可以看到，小车虽然有平衡的趋势，而且响应有所加快，总体感觉良好。
设定kp=400,这个时候我们可以看到，小车的响应明显加快，而且来回推动小车的时候，会有一定幅度的低频抖动。说明这个时候kp值已经足够大了，需要增加微分控制削弱p控制，抑制低频抖动。
经过总体比较： 我们选择参数为kp = 200；
3_1、确定kd值的极性（令kp=0）
我们得到的MPU6050输出的陀螺仪的原始数据，通过观察数据，我们发现最大值不会超过4位数，再根据8000代表占空比100%，所以我们估算kd值应该在0~2之间
我们先设定kd=-0.5，当我们拿起小车旋转的时候，车轮会反向转动，并没有能够实现跟随效果。这说明了kd的极性反了。
接下来，我们设定kd=0.5,这个时候我们可以看到，当我们旋转小车的时候，车轮会同向以相同的速度跟随转动，这说明我们实现了角速度闭环，至此，我们可以确定kd的极性是正的。具体的数据接下来再仔细调试。
最终我们选择kd = 1;
小车速度环调节
为什么要PID速度调节？
（1）假设车模在上面直立控制调节下已经能够保持平衡了，但是由于安装误差，传感器实际测量的角度与车模角度有偏差，因此车模实际不是保持与地面垂直，而是存在一个倾角。在重力的作用下，车模就会朝倾斜的方向加速前进。如果没有速度调节是难以维持0速度的
（2）对于直立车模速度的控制相对于普通车模的速度控制则比较复杂。由于在速度控制过程中需要始终保持车模的平衡，因此车模速度控制不能够直接通过改变电机转速来实现。也是需要通过PID速度调节来实现
int Vertical_speed_PI(int encoder_left,int encoder_right,float Angle,float Movement )
{
static float Velocity,Encoder_Least,Encoder;
static float Encoder_Integral;
//获取最新速度偏差=测量速度（左右编码器之和）-目标速度（此处为零）
Encoder_Least =(encoder_left+encoder_right)-0;
Encoder *= 0.85f; //一阶低通滤波器 ，上次的速度占85% Encoder += Encoder_Least*0.15f; //一阶低通滤波器， 本次的速度占15% ，减缓速度突变对直立的干扰
Encoder_Integral +=Encoder; //积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement;
if(Encoder_Integral>10000) Encoder_Integral=10000; //积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //积分限幅
Velocity=Encoder*PID.Velocity_Kp+Encoder_Integral*PID.Velocity_Ki; //速度控制
if(Turn_off(Angle)==1) Encoder_Integral=0; //电机关闭后清除积分
return Velocity; //返回速度环PWM
}
确定kp的范围：
积分项由偏差的积分得到,所以积分控制和比例控制的极性相同的,而根据工程经验,在不同的系统中,PID 参数相互之间会有一定的比例关系。在我们的平衡小车速度控制系统里面,一般我们可以把ki 值设置为
ki = kp/200
这样,只要我们可以得到kp 值的大小和极性,就可以完成速度控制部分的参数整定了。显然,这样大大缩短了PID 参数整定的时间。
我们通过STM32定时器的编码器接口模式对编码器进行四倍频,并使用M 法测速(每10ms 的脉冲数)得到小车的速度信息,通过观察数据,我们发现两路编码器相加最大值在160左右,而由经验可知,一般平衡小车行驶的最快速度不会超过电机最大速度的40%,再根据PWM = 6000时，在加上电机死区、占空比接近100%,我们可以大概估算
kp 最大值=6000/(160*40%)=93.75
2_1、确定kp值的极性（关闭直立环）
确定速度环调节为正负馈的调节：
当小车以一定的速度运行的时候,我们要让小车停下来,小车需要行驶更快的速度去“追”,小车运行的速度越快,去“追”的速度也就越快,所以这是一个正反馈的效果。如果使用常规的速度负反馈,当小车以一定的速度运行的时候,我们通过减速让小车慢下来,小车会因为惯性向前倒下。
判断速度控制是正反馈还是负反馈：
根据之前的估计,先设定kp=50,ki=kp/200,当我们拿起小车,旋转其中一个小车轮胎的时候,根据我们设定的速度偏差
Encoder_Least =(Encoder_Left+Encoder_Right)-0;
另外一个车轮会反向转动,让偏差趋向于零。这就是常规的速度控制里面的负反馈,不是我们需要的效果。接下来设定kp=-50,ki=kp/200,此时,当我们旋转其中一个小车轮胎的时候,两个轮胎会往相同的方向加速,直至电机的最大速度,这是典型的正反馈效果,也是我们期望看到的。至此,我们可以确定kp,ki
2_1、确定kp值的极性（打开直立环）
首先,设定kp=-30,ki=kp/200这个时候我们可以看到,小车的速度控制比较弱,很难让速度恒定。
设定kp=-50,ki=kp/200这个时候我们可以看到,小车的速度控制的响应有所加快, 静止抖动可接受。
设定kp=-70,ki=kp/200这个时候我们可以看到,小车虽然回正力度增大了,而且响应更加快了,但是稍微加入一点的干扰都会让小车大幅度摆动,抗干扰能力明显不足,所以这组参数不可取。
至此,速度控制调试部分就告一段落了
小车转向环调节
int Vertical_turn_PD(u8 CCD,short yaw)
{
float Turn;
float Bias; //目标角度
Bias=CCD-64;
Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
return Turn;
}
实现代码
#include "car_task.h"
#include "mpu6050.h"
#include "inv_mpu_user.h"
#include "contrl.h"
int Balance_Pwm,Velocity_Pwm,Turn_Pwm; //PID计算的PWM值
int Motor1, Motor2; //左右电机PWM值
int Encoder_left, Encoder_right; //检测速度
float Movement = 0; //速度调节
int Contrl_Turn = 64; //转向调节变量
//环境数据采集任务
void Car_Task_200HZ(void)
{
static struct mpu6050_data Last_Data;
if(mpu_dmp_get_data() !=0 )
OutMpu = Last_Data;
else
Last_Data = OutMpu;
}
void Car_Task_100HZ(void)
{
Encoder_left = Read_Encoder(1);
Encoder_right = -Read_Encoder(2);
//1、确定直立环PWM
Balance_Pwm = Vertical_Ring_PD(OutMpu.pitch, OutMpu.gyro_x);
//2、确定速度环PWM
Velocity_Pwm = Vertical_speed_PI(Encoder_left,Encoder_right,OutMpu.pitch, Movement );
//3、确定转向环PWM
Turn_Pwm = Vertical_turn_PD(Contrl_Turn, OutMpu.gyro_z);
//4、确定最终左右电机的PWM
Motor1 = Balance_Pwm + Velocity_Pwm + Turn_Pwm;
Motor2 = Balance_Pwm + Velocity_Pwm - Turn_Pwm;
PWM_Limiting(&Motor1,&Motor2);
//5、设置电机
Set_PWM(Motor1,Motor2);
}
void Car_Task_5HZ(void)
{
// printf("acc_x = %d\n",OutMpu.acc_x);
// printf("acc_y = %d\n",OutMpu.acc_y);
// printf("acc_z = %d\n",OutMpu.acc_z);
// printf("gyro_x = %d\n",OutMpu.gyro_x);
// printf("gyro_y = %d\n",OutMpu.gyro_y);
// printf("gyro_z = %d\n",OutMpu.gyro_z);
// printf("pitch = %f\n",OutMpu.pitch);
// printf("roll = %f\n",OutMpu.roll);
// printf("yaw = %f\n",OutMpu.yaw);
printf("Encoder_left = %d\n",Encoder_left);
printf("Encoder_left = %d\n",Encoder_right);
printf("\r\n");
}
控制库代码freertos.c：
void StartTask_200HZ(void const * argument)
{
/* USER CODE BEGIN StartTask_200HZ */
//printf("环境采集进程运行\n");
MPU_Init();
while(mpu_dmp_init());
/* Infinite loop */
for(;;)
{
Car_Task_200HZ();
osDelay(5);
}
/* USER CODE END StartTask_200HZ */
}
/* StartTask_100HZ function */
void StartTask_100HZ(void const * argument)
{
/* USER CODE BEGIN StartTask_100HZ */
printf("PID控制进程运行\n");
/* Infinite loop */
for(;;)
{
Car_Task_100HZ();
osDelay(10);
}
/* USER CODE END StartTask_100HZ */
}
其中PID控制理论见大学课本
标签：树莓,避障,Freertos,int,小车,float,PID,Encoder,kp 来源： https://blog.csdn.net/qq_44691051/article/details/113723933