标签：树莓 避障 Freertos int 小车 float PID Encoder kp
基于树莓派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
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。