速度环+直立环+转向环

2023-05-16

文章目录

直立环
直立环调节
速度环
速度环调节
转向环

直立环

车模平衡控制也是通过负反馈来实现的，与上面保持木棒直立比较则相对简单。因为车模有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了
在这里插入图片描述

    平衡小车直立环使用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
}

分析：
　　Mechanical_balance一般取0，因为想要小车平衡，期望值的角度就是0°，用小车的俯仰角来作为反馈，让小车保持在0°周围。
角速度的积分等于角度、那么角度的微分就是角速度
使用PD调节
在这里插入图片描述
E(k) 　　　　　　　　　　 = 　　　　　Bias
(E(k) - E(k-1)) 　　　　　　= 　　　　　Gyro
PID.Balance_Kp 　　　　　　　　　　比例系数
PID.Balance_Kd　　　　　　　　　　微分系数

直立环调节

平衡小车直立环的调试过程包括确定平衡小车的机械中值、确定kp值的极性（也就是正负号）和大小、kd值的极性和大小等步骤。
1、确定平衡小车的机械中值：
把平衡小车放在地面上，绕电机轴旋转平衡小车，记录能让小车接近平衡的角度，一般都在0°附近的。我们调试的小车正好是0度，所以就是Bias=Angle-0
2. 确定kp值的极性（令kd=0）
首先我们估计kp的取值范围。我们的PWM设置的是8000代表占空比100%，再考虑避免电机的死区，我们直立环返回的PWM在6000左右的时候电机就会满载。
假如我们设定kp值为800，那么平衡小车在±10°的时候就会满转。根据我们的感性认识，这显然太大了，那我们就可以估计kp值在0~800之间。
首先大概我们给一个值kp=-200,我们可以观察到，小车往哪边倒，电机会往那边加速让小车到下，就是一个我们不愿看到的正反馈的效果。说明kp值的极性反了，接下来我们设定kp=200,这个时候可以看到平衡小车有直立的趋势，虽然响应太慢，但是，我们可以确定kp值是正的。具体的数据接下来再仔细调试。
设定kp=100,这个时候我们可以看到，小车虽然有平衡的趋势，但是显然响应有点慢了。
设定kp=250,这个时候我们可以看到，小车虽然有平衡的趋势，而且响应有所加快，总体感觉良好。
设定kp=400,这个时候我们可以看到，小车的响应明显加快，而且来回推动小车的时候，会有一定幅度的低频抖动。说明这个时候kp值已经足够大了，需要增加微分控制削弱p控制，抑制低频抖动。
经过总体比较：我们选择参数为kp = 200；
我们得到的MPU6050输出的陀螺仪的原始数据，通过观察数据，我们发现最大值不会超过4位数，再根据8000代表占空比100%，所以我们估算kd值应该在0~2之间
我们先设定kd=-0.5，当我们拿起小车旋转的时候，车轮会反向转动，并没有能够实现跟随效果。这说明了kd的极性反了。
接下来，我们设定kd=0.5,这个时候我们可以看到，当我们旋转小车的时候，车轮会同向以相同的速度跟随转动，这说明我们实现了角速度闭环，至此，我们可以确定kd的极性是正的。具体的数据接下来再仔细调试。
最终我们选择kd = 1;

速度环

（1）假设车模在上面直立控制调节下已经能够保持平衡了，但是由于安装误差，传感器实际测量的角度与车模角度有偏差，因此车模实际不是保持与地面垂直，而是存在一个倾角。在重力的作用下，车模就会朝倾斜的方向加速前进。如果没有速度调节是难以维持0速度的
（2）对于直立车模速度的控制相对于普通车模的速度控制则比较复杂。由于在速度控制过程中需要始终保持车模的平衡，因此车模速度控制不能够直接通过改变电机转速来实现。也是需要通过PID速度调节来实现

在这里插入图片描述
　　　　

/**************************************************************************************************************
*函数名: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;
}

PID.Velocity_Kp　＝　Kp　　　　　　　　比例系数
PID.Velocity_Ki　＝　Ki 　　　　　　　　积分系数
Movement ：正数就前进、负数就后退
encoder_left，encoder_right 是左电机、右电机的编码
Angle ：是平衡的角度
注意：这里的电机速度为什么是两个编码器之和，按道理应该要除以2.很多人不理解这里。因为这个参数可以放在Kp里面去调节，相当于参数整合。

速度环调节

确定kp的范围：
积分项由偏差的积分得到,所以积分控制和比例控制的极性相同的,而根据工程经验,在不同的系统中,PID 参数相互之间会有一定的比例关系。在我们的平衡小车速度控制系统里面,一般我们可以把ki 值设置为
ki = kp/200
这样,只要我们可以得到kp 值的大小和极性,就可以完成速度控制部分的参数整定了。显然,这样大大缩短了PID 参数整定的时间。
我们通过STM32定时器的编码器接口模式对编码器进行四倍频,并使用M 法测速(每10ms 的脉冲数)得到小车的速度信息,通过观察数据,我们发现两路编码器相加最大值在160左右,而由经验可知,一般平衡小车行驶的最快速度不会超过电机最大速度的40%,再根据PWM = 6000时，在加上电机死区、占空比接近100%,我们可以大概估算
kp 最大值=6000/(160*40%)=93.75
确定速度环调节为正负馈的调节：
当小车以一定的速度运行的时候,我们要让小车停下来,小车需要行驶更快的速度去“追”,小车运行的速度越快,去“追”的速度也就越快,所以这是一个正反馈的效果。如果使用常规的速度负反馈,当小车以一定的速度运行的时候,我们通过减速让小车慢下来,小车会因为惯性向前倒下。
判断速度控制是正反馈还是负反馈：
根据之前的估计,先设定kp=50,ki=kp/200,当我们拿起小车,旋转其中一个小车轮胎的时候,根据我们设定的速度偏差
Encoder_Least =(Encoder_Left+Encoder_Right)-0;
另外一个车轮会反向转动,让偏差趋向于零。这就是常规的速度控制里面的负反馈,不是我们需要的效果。接下来设定kp=-50,ki=kp/200,此时,当我们旋转其中一个小车轮胎的时候,两个轮胎会往相同的方向加速,直至电机的最大速度,这是典型的正反馈效果,也是我们期望看到的。至此,我们可以确定kp,ki
首先,设定kp=-30,ki=kp/200这个时候我们可以看到,小车的速度控制比较弱,很难让速度恒定。
设定kp=-50,ki=kp/200这个时候我们可以看到,小车的速度控制的响应有所加快, 静止抖动可接受。
设定kp=-70,ki=kp/200这个时候我们可以看到,小车虽然回正力度增大了,而且响应更加快了,但是稍微加入一点的干扰都会让小车大幅度摆动,抗干扰能力明显不足,所以这组参数不可取。

转向环

转向环是最不重要的一环，所以网上资料都是同样的，摸棱两可没说清楚，自己整理了一下
如果需要转向，其实需要左轮右轮有一个速度差就行了
转向是对六轴传感器的z轴的旋转，就是偏航角作为反馈信息
PD环节可能有些同学看不懂，我们一个一个来看
在这里插入图片描述
E(k)　　　　　：是期望值和反馈值的差
E(k) - E(k-1)　:　是E(k)的微分
带进转向环
E(K) = (期望的角度 -反馈的角度 )
E(k) - E(k-1) 就是角度的微分就是角速度
注：角速度的积分等于角度、那么角度的微分就是角速度

int Vertical_turn_PD(short yaw)
{
		float Turn;     
	    float Bias;	  
	  Bias=90-Yaw;
	  Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
	  return Turn;
}

Bias=90-Yaw;
90是我期望的角度
Yaw是当前的偏航角
这样就能完成原地转向90°的算法了
但是一般人脸识别，CCD模块中我们是不会这样去算的，
Bias直接取差值就行

/**************************************************************************************************************
*函数名:Vertical_turn_PD()
*功能:转向环PD
*形参:无  CCD小于64左转、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;
}

Bias=CCD-64
64是ccd模块的中值

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)