PID循迹控制算法

前言

 让小车进行循迹最重要的是获取车相对于循迹线的位置,关于循迹模块的原理我已经在之前的一篇博客上讲过,如果大家没看过的话可以点此链接:PID循迹模块原理

大家最关心的整车资料链接(百度云):整车资料链接
提取码:wki1

视频演示链接:视频演示链接

PID循迹原理简介

循迹车

图1 循迹车寻线行走图示

 PID循迹的原理我们大致可以用常识性思维去思考下,如图1所示为循迹车寻线行走图示,现在小车要按照中间那条黑线循迹行走,如果车头往左偏则需要控制方向盘让车往右转,车头往又偏则需要让车往左转。但是仅仅只有左转和右转的话,车一到左边就向右转,一到右边就向左转,则车会来回在线左右晃动,速度一快就会偏离航向。这种循迹方法是最简单的方法,市场上常用的循迹传感器控制方法也就是这种,因为这种循迹传感器只能输出0或1,如图2所示。

在这里插入图片描述

图2 市场常见的红外循迹传感器只能输出0和1

 所以这种传感器控制效果很差,速度慢了还好,速度稍微一块就会很容易偏离循迹线。如果我们需要实现更为精确的控制就需要引入比例P控制,也就是我们需要通过考验获取到线性输出的循迹传感器,获取到小车偏离黑线的程度,偏离的越远就向黑线方向转的程度就越大,偏离的越近就向黑线方向转的程度就越小,这就是引入比例P控制。但是转的时候又不能使小车转的过快,这就需要引入微分d控制,也就是PI转向控制器即可控制小车的循迹转向。小车在循迹的的时候有时也需要控制小车的循迹速度,控制小车的循迹速度需要PI速度控制器,前提当然我们也需要一个获取小车速度的传感器,也就是编码器,下面开始介绍下PID控制器原理。

PID控制器原理

 PID控制器是一种线性控制方法,控制原理如图3-1所示,它根据给定值r(t)与实际输出值y(t)构成控制偏差e(t),即e(t)=r(t)-y(t)。对偏差进行比例、积分、微分运算,将三种运算结果相加,即可得到PID控制器的控制输出u(t)。在连续时间域中,PID控制器算法表达式如下:
在这里插入图片描述

式中:kp为比例系数,Ti为积分时间常数;Td为微分时间常数。
在这里插入图片描述

图3 PID控制系统原理框图

 在循迹控制中,只需要PD控制器即可,公式中输入偏差e(t)为循迹传感器输出值,经过PD控制器后输出u(t)直接控制小车转向。在循迹控制中我们也需要控制循迹车的前进的速度,控制循迹车前进速度需要通过PI控制器来实现。循迹小车的整个控制原理系统框图如图所示。
在循迹小车控制原理系统框图

图4 循迹小车控制原理系统框图

PID控制器代码实现

PID控制器实现代码如下:

//定义pid结构体
struct PID
{
	float kp;
	float ki;
	float kd;
};
typedef struct PID _PID;

//定义转向pid参数结构体
_PID TurnPdate=
{
	.kp=100,			//赋值比例值60
	.ki=0,			//赋值积分值
	.kd=-34			//赋值微分值-45
};		
//定义速度pid参数结构体
_PID SpdPdate=
{
	.kp=-50,			//赋值比例值-80
	.ki=-6,			//赋值积分值-10
	.kd=0			//赋值微分值
};	

/*@brief: 位置式PID控制器
 * @param:
 *        [in] float deviation: 和目标值得偏差
 *        [in] _PID pid: 位置式pid的参数
 * @return: 调节占空比的一个整形数
 */
int PositionPID(float deviation,_PID pid)
{
	float Position_KP=pid.kp,Position_KI=pid.ki,Position_KD=pid.kd;
	static float Bias,Pwm,Integral_bias,Last_Bias;
	Bias=deviation;                         		         //计算偏差
	Integral_bias+=Bias;	                                 //求出偏差的积分
	Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
	Last_Bias=Bias;                                      	 //保存上一次偏差 
	return Pwm;    
}

/*@brief: 为速度pid使用位置式PID控制器,对积分值进行限幅
 * @param:
 *        [in] float deviation: 和目标值得偏差
 *        [in] _PID pid: 位置式pid的参数
 * @return: 调节占空比的一个整形数
 */
int PositionPIDToSpd(float deviation,_PID pid)
{
	float Position_KP=pid.kp,Position_KI=pid.ki,Position_KD=pid.kd;
	static float Bias,Pwm,Integral_bias,Last_Bias,pwmKI=0;
	Bias=deviation;                         		         //计算偏差
	Integral_bias+=Bias;	                                 //求出偏差的积分
	pwmKI=Position_KI*Integral_bias;
	if(pwmKI>MAX_MOTOR_PWM) Integral_bias=MAX_MOTOR_PWM/Position_KI;
	Pwm=Position_KP*Bias+pwmKI+Position_KD*(Bias-Last_Bias);       //位置式PID控制器
	Last_Bias=Bias;                                      	 //保存上一次偏差 
	return Pwm;    
}

 代码中PositionPID(float deviation,_PID pid)中函数deviation偏差参数输入循迹传感器输出值,pid参数需要根据自己车型和循迹场地调试设置。

输出转向控制

 通过PD转向控制器获取到输出数据后,需要控制舵机转向,一般循迹车有两种车型,分别为舵机转向型和差速转向型。

差速转向型

图5 差速转向循迹车

 如图5所示为差速转向车型,这种车转向方法是通过控制车轮的差速。我们可以想到如果让小车往右转,则需左轮往前转,然后右轮往后转,且差速越大转的越快。同理,让小车往左转的话需要右轮往前转,左轮往后转。实现小车向前走的话就是两个轮子同时往前转即可,控制小车循迹速度的话要映入PI速度控制器,同时需要传感器测量出小车的速度,这是就需要大家使用编码器电机了。
代码实现如下:

/*@brief:根据循迹传感器pid调节小车转向使小车处于黑线中间
 * @param:
 *        [in]int TraceDate: 循迹传感器输出的值
 *        [in]float TarSpdL:右边电机目标速度,最大速度越1.27m/s
 * @return: 返回调节电机速度的转向pwm
 */
int ChangeTraceTurn(int TraceDate)
{
	int pwm=0;
	int bias;
	bias=TraceDate;
	pwm=PositionPID(bias,TraceTurnPdate);
	if(pwm>5000)		pwm=5000;//限幅
	else if(pwm<-5000)  pwm=-5000;
	return pwm;
}

/*@brief:根据pid调节左边电机到目标速度
 * @param:
 *        [in]int EncodeSpdL: 当前左电机编码器测速值
 *        [in]float TarSpdL:左边电机目标速度,最大速度越1.19m/s
 * @return: 返回左边电机计算后的pwm占空比
 */
int ChangeSpeedMotorL(int NowEncodeSpdL,float TarSpdL)
{
	int pwm=0;
	int bias;
	int TarEncodeSpdL;
	TarEncodeSpdL=(int)((TarSpdL*ACircleEncoder)/(WheelOneCircleDis*100)+0.5f);//根据目标速度求出目标编码器速度
	bias=NowEncodeSpdL-TarEncodeSpdL;
	pwm=PositionPIDToSpd(bias,SpdPdate);
	if(pwm>5000)		pwm=5000;//限幅
	else if(pwm<-5000)  pwm=-5000;
	return pwm;
}

/*@brief:根据pid调节右边电机到目标速度
 * @param:
 *        [in]int EncodeSpdL: 当前右电机编码器测速值
 *        [in]float TarSpdL:右边电机目标速度,最大速度越1.27m/s
 * @return: 返回右边电机计算后的pwm占空比
 */
int ChangeSpeedMotorR(int NowEncodeSpdR,float TarSpdR)
{
	int pwm=0;
	int bias;
	int TarEncodeSpdR;
	TarEncodeSpdR=(int)((TarSpdR*ACircleEncoder)/(WheelOneCircleDis*100)+0.5f);//根据目标速度求出目标编码器速度
	bias=NowEncodeSpdR-TarEncodeSpdR;
	pwm=PositionPIDToSpd(bias,SpdPdate);
	if(pwm>5000)		pwm=5000;//限幅
	else if(pwm<-5000)  pwm=-5000;
	return pwm;
}

/*@brief: 让小车根据循迹黑线走
 *@param:
 *        [in]TraceDate: 循迹传感器输出的值
 *        [in]TarSpeed:循迹的目标速度
 *@return: 到达目标点返回1,否则返回0
 */
void TraceMove(int TraceDate,float TarSpeed)
{
	int turnpwm=0;
	int spdpwml=0,spdpwmr=0;
	int pwml=0,pwmr=0;
	
	turnpwm=ChangeTraceTurn(TraceDate);
	
	spdpwml=ChangeSpeedMotorL(Encode_Left,TarSpeed);
	spdpwmr=ChangeSpeedMotorR(Encode_Right,TarSpeed);
	
	
	pwmr=turnpwm+spdpwmr;
	if(pwmr>5000)		pwmr=5000;//限幅
	else if(pwmr<-5000)  pwmr=-5000;
	pwml=-turnpwm+spdpwml;
	if(pwml>5000)		pwml=5000;//限幅
	else if(pwml<-5000)  pwml=-5000;
	
	Set_Pwm_Motor1(pwmr);				//设置电机1占空比
	Set_Pwm_Motor2(pwml);				//设置电机2占空比
}

 舵机转向车结构如图6所示,控制舵机转向原理和差速转向相似,不同点是控制转向的输出最后作用于舵机上,将循迹传感器通过PD控制后的输出控制舵机即可,大家可以参考差速转向写舵机转向的代码,同样舵机转向也需要控制车的行驶速度。
舵机转向循迹车

图6 舵机转向循迹车

总结

 关于小车的PID循迹控制,最重要的点是需要一个能够获取线性输出的循迹传感器,然后通过PD控制器控制舵机循迹转向,其次我们在循迹的同时如果需要控制小车的行驶速度需要引入PI速度控制器,两者共同作用即可实现小车循迹的PID控制。

<think>好的,我现在需要帮用户查找关于差速转向PID控制算法的资料,包括实现原理和代码示例。首先,用户提到了系统级指令,需要按照格式要求来组织回答。我得先理解用户的具体需求,他们可能是在开发AGV或者机器人,需要控制差速转向,使用PID算法来实现精确的运动控制。 用户提供的参考引用有三个,其中引用[1]详细说明了PID控制器的设计,包括直行、转弯、原地旋转和平移的控制目标,以及误差计算和PID算法的应用。引用[2]讨论了运动学正解和逆解,这对理解如何将轮速转换为机器人的运动很重要。引用[3]虽然主要讲麦克纳姆轮,但可能对理解差速转向有帮助,但用户的问题是关于差速转向,所以可能需要更多关注前两个引用。 首先,我需要整理差速转向的基本原理。差速转向通常用于两轮驱动的机器人,通过调节左右轮的转速来实现转向。PID控制器在这里的作用是调整左右轮的转速差,以达到期望的转向效果。需要解释PID的三个部分:比例、积分、微分的作用,以及如何结合到差速控制中。 接下来是实现原理部分。根据引用[1],PID控制器的设计需要考虑控制目标,比如保持朝向角或实现旋转。误差计算是关键,可能需要测量当前方向与目标方向的偏差,例如使用陀螺仪或编码器数据。然后通过PID算法计算出左右轮的速度调整量。 代码示例部分,用户可能希望看到伪代码或者具体语言的实现。引用[1]提到有C++实现代码,但用户没有给出具体内容,可能需要根据描述来构造一个简单的示例。例如,如何根据误差计算P、I、D项,然后综合得到控制输出,调整左右轮速。 此外,用户的问题还要求生成相关问题,我需要考虑常见的后续问题,比如参数调整、实际应用中的挑战、与其他控制算法的对比等。 需要注意的是,用户强调行内数学公式用$...$,独立公式用$$...$$,并且正确使用LaTeX。例如,PID公式可能需要用独立公式展示: $$ u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt} $$ 另外,引用标识要自然添加在引用的句子末尾,比如提到控制目标时引用[^1]。 现在,我需要组织这些内容,确保结构清晰,分步骤解释原理,给出代码示例,并生成相关问题。同时检查是否符合格式要求,避免使用Markdown,用自然的中文口语化表达,但不需要在思考过程中体现。</think>以下为差速转向PID控制算法的实现原理和示例: ### 差速转向PID控制原理 1. **核心目标**:通过调节左右轮速差实现转向控制,常用于双轮/四轮差速底盘 2. **控制变量**: - 角速度误差 $e_\omega = \omega_{目标} - \omega_{实际}$ - 航向角误差 $e_\theta = \theta_{目标} - \theta_{实际}$ 3. **PID公式**: $$ \Delta V = K_p e_\omega + K_i \int e_\omega dt + K_d \frac{de_\omega}{dt} $$ 4. **轮速分配**: ``` 左轮速度 = 基准速度 + ΔV 右轮速度 = 基准速度 - ΔV ``` ### 代码实现示例(C++伪代码) ```cpp class DiffPIDController { public: // PID参数初始化 DiffPIDController(float kp, float ki, float kd) : Kp(kp), Ki(ki), Kd(kd), prev_error(0), integral(0) {} // 计算控制量 float compute(float target, float actual, float dt) { float error = target - actual; integral += error * dt; float derivative = (error - prev_error) / dt; prev_error = error; return Kp*error + Ki*integral + Kd*derivative; } private: float Kp, Ki, Kd; float prev_error, integral; }; // 应用示例 void controlLoop() { DiffPIDController pid(0.8, 0.01, 0.05); while(true) { float target_omega = getTargetAngularSpeed(); // 目标角速度 float actual_omega = getIMUData(); // 实际角速度 float deltaV = pid.compute(target_omega, actual_omega, 0.01); // 分配轮速 setWheelSpeed(base_speed + deltaV, LEFT); setWheelSpeed(base_speed - deltaV, RIGHT); delay(10); } } ``` ### 关键实现步骤 1. **传感器融合**:需整合编码器脉冲计数(轮速反馈)和IMU航向角数据 2. **运动学解算**:根据轮速计算实际角速度 $\omega = \frac{V_r - V_l}{D}$(D为轮距)[^2] 3. **抗积分饱和**:需限制积分项最大值,防止启动时的过冲现象 4. **动态调参**:直线运动时增大Kp值,转弯时适当降低Kp提高稳定性
评论 32
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值