速度环+直立环+转向环

文章目录

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

  • 直立环

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

        平衡小车直立环使用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模块的中值

    来源:蜡笔小新学电子

    物联沃分享整理
    物联沃-IOTWORD物联网 » 速度环+直立环+转向环

    发表评论