代码收藏家 速度环+直立环+转向环
文章目录
直立环
车模平衡控制也是通过负反馈来实现的，与上面保持木棒直立比较则相对简单。因为车模有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了
平衡小车直立环使用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模块的中值
来源：蜡笔小新学电子
