移动机器人PID控制-(实现电机控速以及小车沿线行驶)

基础概念介绍

闭环控制

  • 控制系统中,有输入、输出和最后的被控量,而反馈出现的原因是因为控制量和最后的真实的被控量是不一样的,因为中间存在干扰,而反馈的目的就是告诉控制器,我有偏差,并且你要调整我。闭环控制就是我们的输出量还作为一定的反馈量返回到输入端,通过输出和输入的偏差信号来控制系统使得偏差不断缩小,如果是输入和输出量相减我们又叫做**负反馈系统。**在下面的PID理论模型可以知道PID中常用的一个模型就是一个典型的负反馈闭环控制系统。

在这里插入图片描述

PID理论

  • 将偏差的比例(proportion)、积分(integral)、微分(derivative),通过线性组合构成控制量,用控制量对被控对象进行控制,这样的控制器称为PID控制器。在连续空间中,我们通常探讨模拟PID的控制原理,如图所示:

image.png

  • 我们这里用电机速度控制为例,讲解PID控制系统。r(t)为设定电机速度、y(t)为实际电机速度、e(t)=y(t)-r(t)为速度差值作为PID控制器的输入、u(t)为PID控制器的输出,作用到被控对象电机上。根据模拟PID控制器,科学家们也得出了模拟PID控制的公式,如图所示:

image.png

  • 其中Kp、Ti、Td,分别为控制器的比例系数、积分系数、微分系数。该理论用在控制的例子比比皆是。但是模拟PID控制系统是在连续空间的上描述的,无法在计算机上用代码实现。于是就有数字PID控制理论,将连续空间的PID控制系统在离散空间上描述。积分变成了求和、微分变成了求斜率,于是就出现数字PID控制系统的理论公式,如图所示:

image.png

  • 其中Kp、Ti、Td和上面描述的一样,T为采用周期,ek是本次差值,ek-1上一次的差值,直接通过模拟PID转化的数字PID又叫做位置式PID,该方式的PID的输出直接是控制量,非常不适合经常出现异常的系统,另外一种方式是增量式PID,每次只输出一个正向或者反向的调节量,就算出现异常,也不会产生巨大的影响。具体数学公式如下所示:该方法较多的应用于生产生活中,本文中电机的速度PID控制当然也不例外。(这里就是上文说的,负反馈闭环控制系统)
    • 位置式PID公式:
      在这里插入图片描述
    • 位置式PID最终作用公式在这里插入图片描述
    • 增量式PID公式:
      image.png
    • 增量式PID作用公式:
      在这里插入图片描述
    • 总结:增量式是根据偏差的变化对被控对象进行控制(个人理解就是找到你误差变化的规律,而不是直接用误差大小来控制)对控制量影响较小,而位置式直接是从误差输出一个控制量其如果误差出现异常,则系统也容易异常。实际应用中可能更多是增量式的控制,我们下面的实践代码也均是增量式的控制
    • 个人理解:位置式可能对应一一对应的关系,比如我们要控制水位高度在一个高度上,那么高度下降了我们就打开阀门,高度达到了,就关闭阀门。而如果我们要控制的是水流的速度,那可能要根据速度的变化,来慢慢调节阀门,使得最终达到合适的速度吧。

PID特点

  • pid控制是基于负反馈的偏差控制,特点是不依赖系统模型,计算量小,响应速度快,因此对于底层的电机速度和位置控制,以及工业上的压力控制、液位控制等,而上层依赖系统模型(比如机器人动力学模型)的控制算法(例如最优控制)就不适用!归结到底,pid在底层得到广泛应用,主要还是因为底层的逻辑需求简单,而且没法提供复杂控制算法的计算原件。

实践代码

电机控速代码

电机控制速度代码来自小白学移动机器人中的PID控制:

  • 有了上面的理论基础,开始代码实现的介绍。首先就是明确增量式PID系统的输入、输出、控制对象
    • 对于控制电机速度来说输入即为速度的设定值、输出即为速度的测量值,控制对象就是电机转速(但在程序中最终表征的控制对象是PWM占空比,通过调节占空比进而实现控制电机转速)
    • 对于下面的电机控制速度代码来说,增量式PID核心就只有一个公式,所以使用代码实现并不困难(每一次的输出都是在前一次的输出上叠加一个增量,这也是增量式PID的核心,我们PID控制输出的是一个调节量,并不是最后的控制量,我们增量式最后的控制量是叠加了上一时刻的输出):

image.png

#ifndef __PID_H__
#define __PID_H__

#include "stm32f10x.h"

struct pid_uint
{
	s32 U_kk;    	    //上一次的输出量
	s32 ekk;		 	//上一次的输入偏差
	s32 ekkk;			//前一次的输入偏差
	s32 Ur;				//限幅输出值,需初始化
	s32 Kp;				//比例
	s32 Ki;				//积分
	s32 Kd;				//微分
	
	u8  En;             //开关
	s16 Adjust;         //调节量
	s16 speedSet;       //速度设置
	s16 speedNow;       //当前速度
};
/****************************外接函数***************************/

extern struct pid_uint pid_Task_Letf;
extern struct pid_uint pid_Task_Right;

void  PID_Init(void);
void  reset_Uk(struct pid_uint *p);
s32   PID_common(int set,int jiance,struct pid_uint *p);
void Pid_Ctrl(int *leftMotor,int  *rightMotor);

#endif //__PID_H__

#include "pid.h"

/*===================================================================
程序功能:双路电机速度PID,不同的电机,可能出现默认参数,不会产生速度控制的效果,请自行更改
程序编写:公众号:小白学移动机器人
其他    :如果对代码有任何疑问,可以私信小编,一定会回复的。
=====================================================================
------------------关注公众号,获得更多有趣的分享---------------------
===================================================================*/

struct pid_uint pid_Task_Letf;
struct pid_uint pid_Task_Right;

/****************************************************************************
*函数名称:PID_Init(void)
*函数功能:初始化PID结构体参数
****************************************************************************/

void PID_Init(void)
{
//乘以1024原因避免出现浮点数运算,全部是整数运算,这样PID控制器运算速度会更快
/***********************左轮速度pid****************************/
	pid_Task_Letf.Kp = 1024 * 0.5;//0.4
 	pid_Task_Letf.Ki = 1024 * 0;	
	pid_Task_Letf.Kd = 1024 * 0.08; 
	pid_Task_Letf.Ur = 1024 * 4000;
	pid_Task_Letf.Adjust   = 0;
	pid_Task_Letf.En       = 1;
	pid_Task_Letf.speedSet = 0;
	pid_Task_Letf.speedNow = 0;
	reset_Uk(&pid_Task_Letf);		
/***********************右轮速度pid****************************/
	pid_Task_Right.Kp = 1024 * 0.35;//0.2
 	pid_Task_Right.Ki = 1024 * 0;	//不使用积分
	pid_Task_Right.Kd = 1024 * 0.06; 
	pid_Task_Right.Ur = 1024 * 4000;
	pid_Task_Right.Adjust   = 0;
	pid_Task_Right.En       = 1;
	pid_Task_Right.speedSet = 0;
	pid_Task_Right.speedNow = 0;
	reset_Uk(&pid_Task_Right);
}

/***********************************************************************************************
 函 数 名:void reset_Uk(PID_Uint *p)
 功    能:初始化U_kk,ekk,ekkk
 说    明:在初始化时调用,改变PID参数时有可能需要调用
 入口参数:PID单元的参数结构体 地址
************************************************************************************************/

void reset_Uk(struct pid_uint *p)
{
	p->U_kk=0;
	p->ekk=0;
	p->ekkk=0;
}

/***********************************************************************************************
 函 数 名:s32 PID_commen(int set,int jiance,PID_Uint *p)
 功    能:PID计算函数
 说    明:求任意单个PID的控制量
 入口参数:期望值,实测值,PID单元结构体
 返 回 值:PID控制量
************************************************************************************************/

s32 PID_common(int set,int jiance,struct pid_uint *p)
{
	int ek=0,U_k=0;

	ek=jiance - set; //当前时刻的输入偏差                                                             
	
	U_k=p->U_kk + p->Kp*(ek - p->ekk) + p->Ki*ek + p->Kd*(ek - 2*p->ekk + p->ekkk); //当前时刻的输出等于上一时刻的输出U_kk + 当前时刻的增量变化
	
	p->U_kk=U_k;
    p->ekkk=p->ekk;
	p->ekk=ek;
	
	if(U_k>(p->Ur))		                                    
		U_k=p->Ur;
	if(U_k<-(p->Ur))
		U_k=-(p->Ur);
	
	return U_k>>10; //在先前的比例、积分、微分系数中都乘以了1024,因此最后的控制输出量要除以2^10(即1024)
}

/***********************************************************************************
** 函数名称 :void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
** 函数功能 :pid选择函数	      
***********************************************************************************/

void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
{
	/**********************左轮速度pid*************************/
	if(pl->En == 1)
	{									
		pl->Adjust = -PID_common(pl->speedSet, pl->speedNow, pl);		
	}	
	else
	{
		pl->Adjust = 0;
		reset_Uk(pl);
		pl->En = 2; 
	}
	/***********************右轮速度pid*************************/
	if(pr->En == 1)
	{
		pr->Adjust = -PID_common(pr->speedSet, pr->speedNow, pr);		
	}	
	else
	{
		pr->Adjust = 0;
		reset_Uk(pr);
		pr->En = 2; 
	}
}

/*******************************************************************************
 * 函数名:Pid_Ctrl(int *leftMotor,int  *rightMotor)
 * 描述  :Pid控制
 *******************************************************************************/

void Pid_Ctrl(int *leftMotor,int  *rightMotor)
{
	Pid_Which(&pid_Task_Letf, &pid_Task_Right); 
	*leftMotor  += pid_Task_Letf.Adjust;
	*rightMotor += pid_Task_Right.Adjust;
}
#include "sys.h"

//====================自己加入的头文件===============================
#include "delay.h"
#include "led.h"
#include "encoder.h"
#include "usart3.h"
#include "timer.h"
#include "pwm.h"
#include "pid.h"
#include "motor.h"
#include <stdio.h>
/*===================================================================
程序功能:直流减速电机的速度闭环控制测试,可同时控制两路,带编码器的直流减速电机
程序编写:公众号:小白学移动机器人
其他    :如果对代码有任何疑问,可以私信小编,一定会回复的。
=====================================================================
------------------关注公众号,获得更多有趣的分享---------------------
===================================================================*/
int leftSpeedNow  =0;
int rightSpeedNow =0;

int leftSpeeSet   = 300;//mm/s
int rightSpeedSet = 300;//mm/s

int main(void)
{ 

	GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
	
	MY_NVIC_PriorityGroupConfig(2);	//=====设置中断分组
	
	delay_init();	    	        //=====延时函数初始化
	LED_Init();                     //=====LED初始化    程序灯	
	
	usart3_init(9600);              //=====串口3初始化  蓝牙 发送调试信息

	Encoder_Init_TIM2();            //=====初始化编码器1接口
	Encoder_Init_TIM4();            //=====初始化编码器2接口
	
	Motor_Init(7199,0);             //=====初始化PWM 10KHZ,用于驱动电机 如需初始化驱动器接口
	
	TIM3_Int_Init(50-1,7200-1);     //=====定时器初始化 5ms一次中断

	PID_Init();						//=====PID参数初始化
	
	//闭环速度控制
	while(1)
	{
		//给速度设定值,想修改速度,就更该leftSpeeSet、rightSpeedSet变量的值
		pid_Task_Letf.speedSet  = leftSpeeSet;
		pid_Task_Right.speedSet = rightSpeedSet;
		
		//给定速度实时值
		pid_Task_Letf.speedNow  = leftSpeedNow;
		pid_Task_Right.speedNow = rightSpeedNow;
		
		//执行PID控制函数
		Pid_Ctrl(&motorLeft,&motorRight);
		
		//根据PID计算的PWM数据进行设置PWM
		Set_Pwm(motorLeft,motorRight);
		
		//打印速度
		printf("%d,%d\r\n",leftSpeedNow,rightSpeedNow);
		delay_ms(2);
	} 
}

//5ms 定时器中断服务函数 --> 计算速度实时值,运行该程序之前,确保自己已经能获得轮速,如果不懂,可看之前电机测速的文章

void TIM3_IRQHandler(void)                            //TIM3中断
{
	if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否
	{
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update);   //清除TIMx的中断待处理位
		
		Get_Motor_Speed(&leftSpeedNow,&rightSpeedNow);//计算电机速度
		
		Led_Flash(100);                               //程序闪烁灯
	}
}

PID系数的调节

  • 完成上面的代码,只是完成速度PID的一部分,剩下的是尤为重要的PID参数整定。该整定方法丰富多样,最为准确的是模型计算,但是对于我们做机器人多使用试凑法。虽然需要调节一段时间,但是不需要对机器人进行建模。试凑法一般按照P、I、D的顺序进行调节:
    • 初始时刻将Ki和Kd都设置成0,按照经验设置Kp的初始值,就这样将系统投入运行,由小到大调节Kp。求得满意的曲线之后,若要引入积分作用,将Kp设置成之前的5/6,然后Ki由小到大开始调节。达到满意效果之后,若要引入微分作用,将Kd按照经验调节即可。经过有规律的试凑,最终达到一个我们满意的就行。
  • 下面来表示一下三个系数对响应曲线的影响
    • 在未调节任何参数情况下,响应曲线为下图所示,虚线为我们想要的理想值

原曲线

  • 接着我们需要先调节P,调节P的目的是为了使得曲线快速上升,希望在3T-5T的时间内能够达到理想值的90%左右,并且随着时间增加会逐渐趋于稳态

调节P后曲线

  • 但是只调节P,它趋于稳态的值肯定不是我们想要的,这个时候我们需要把调节好的P降低一部分值,然后调整I,使得曲线的稳态值能够接近理想值。
    • 但很多情况下I是不动的,因为调I,对这个系统影响比较大

降低P调节I后曲线

  • 调节了P I后,如果想使得振幅减小,就要调整D,我们希望振幅在3%-5%之间

调节D后曲线

小车利用PID控制进行沿线行驶

  • 机器人配备硬件:在车头的左右两边分别配备有水平+垂直电感,当沿着电感线行驶时两边电感偏差比较小,如果往某一边偏了则偏的那一边的电感值就会增大
  • 和电机控制速度的PID一样,首先明确输入、输出、被控对象:
    • 输入为:车辆沿线中间行驶时的电感差值(即我们希望车辆达到这样的状态)
    • 输出为:实际行驶过程中计算的电感差值
    • 被控对象:左右轮速度,我们希望根据偏差来调整左右轮速度最终达到车辆的正中间行驶
  • 明确了PID的三个东西以后,换汤不换药,就可以实现小车沿着电感线行驶啦,在其他功能下也是一样的。

文章参考小白学移动机器人的教程:
https://mp.weixin.qq.com/s?__biz=MzIwOTc5Njg2Mg==&mid=2247486694&idx=1&sn=a2e9bdadc77517c5b5fa5d1eaee9ee3f&chksm=976f2777a018ae61707cbe426c3a95cd954a47982a196c4a3af3d7727134078873ad6470cbe7&cur_album_id=2255869189385355271&scene=190#rd

  • 22
    点赞
  • 32
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值