胡乱捣鼓03——PID定身12cm直线追踪小车做起来~

1.通俗理解PID

本项目的小车用到位置式PID，所以主要描述位置式PID的用法，通过这个简单的例子就能领悟PID的精髓：

1.1PID是什么呢？

首先，PID有两个层面的意思，一个是PID这种算法，是一种控制算法，用于稳定控制。

再者，PID是P(比例项)、I（积分项）、D（微分项）的和，是算出来的一个值，也就是说：PID=P+I+D，那么P、I、D又代表什么呢？

1.2 P、I、D （用来干嘛？）

既然PID是算出来的一个值，那么P、I、D都是算出来的值。与其说是比例项，积分项、微分项，不如直接列公式说明更清楚。

P——比例系数（kp > 0）与本次误差（error）的积，也就是P = kp * error。打个比方，小车的目标是走到距离石墙10cm的地方，而小车一开始在距离石墙100cm处，此时以距离10cm作为目标，误差error = 100 - 10 = 90cm，那么P就是kp * 90。好的问题来了，P是算出来了，那怎么用呢？我们知道小车前进的速度是由马达转速决定的，马达的转速又是由PWM来决定的，PWM越大，马达更快，小车速度更快，这时候如果我的P项和PWM是成比例关系的话，那么我的P项就可以用在小车的速度控制上。试想一下，第一次P是kp * 90 ，也就是速度的值是kp * 90 ，第二次因为小车前进了，假设前进了20cm，那么第二次的误差就是80 - 10 = 70cm，所以第二次的P值就是kp * 70 ，速度的值也就是kp * 70，速度变慢了，以此类推直到第N次，小车到达了目的地，error = 0，P=0，小车速度自然也为0。所以总的来说，P就是和电机转速成比例关系的一个值，可以利用这个值来控制电机的转速。

I——积分系数（ki > 0）与误差的积的累积，记住是累积！也就是 I = Σki * error。那么I项算出来又有什么用呢？再打个比方，还是拿上述的小车做例子，假设在第5次计算P时，算出P = 100，也就是电机的PWM值为100，而真实情况下能够驱动电机使小车往前走的最小PWM值是200，此时小车动不了了，也还没达到目的地，这时候I项就发挥作用了。因为还存在误差（没到达目的地），所以 I = Σki * error会一直积累，而在引入I项后电机的PWM = P + I，P因为误差不变而不会再变化了，而I会因为存在误差而一直累加，所以当累积后的PWM值超过200后，小车又能往前走了，直到没有误差。

D——微分系数（kd > 0）与两次误差之间的斜率的乘积，也就是D = kd*((error - error_pre)/Δt)。高中开始学数学开始我们就懂斜率这个东西，表示的是变化的快慢，当然这个D值既然能算出来，那么它有时用来干什么的呢？再举个例子，我们在骑单轮车的时候是怎么平衡自己的，当我要往前面倒的时候我们是不是需要加速往前骑才能保持稳定。同理，现在换成电机驱动单轮车，当我发现两次误差特别大的时候，也就是单轮车要倒了的时候，D值会随着斜率的增大而增大，算出来的PID = P + I +D 也会随之变大，那么电机的PWM值也会变大，电机速度同样增大从而保持平稳状态。

所以总的来说，PID算出来的值是用来驱动PWM电机的，无论什么系统，PID计算的值都对应着电机的PWM。

2.小车搭建

材料：
①4轮马达小车
②arduino uno
③测距模块HY-SRF05
④两节18650电池
⑤18650电池盒
⑥转压模块，转至5v
⑦L298N，马达驱动模块
⑧杜邦线若干

（具体的连接在程序的注释里）

3.PID代码不过12行

整个PID算法的代码不多，也就12行，不连注释的哈！

       /*位置式PID计算*/
       error = distance - target;
       
       P = kp*error;                       //P
		//积分分离，根据实际情况，防止不断累加而产生震荡
       if(error > 0 && error < 0.8) ki = 0;
       if(error < 0 && error > -0.8)ki = 0;
       else ki = 0.08;
       if(-10 < error && error < 10) I += ki*error;                                   
       else     I = 0;                     //I ,在一定误差内I才作用
         
       //D = kd*((error - error_pre)/deta_t); //D，误差的变化率

       D = 0;  //这里没有用到D项，因为没有突然的变化可以不需要用D项
   
       PID=P + I + D ;                      //PID

      /*限幅*/
      if (PID>200)  PID=200;                            
      if (PID<-200) PID=-200;                       
      error_pre = error;                   //记录此次误差为上一刻误差

以下是完整代码如下。说明一下为什驱动PWM时要在某段范围乘以几倍，因为小车重量以及车轮安装的紧实度情况，PWM50以下我的小车都动不了…所以迫不得已了。

#define echo 2                //测距模块的接收端         
#define trig 3                //测距模块的控制端（触发端）          
#define out1 10               //L298N的out1口，控制小车右边两个马达的+端
#define out2 11               //L298N的out2口，控制小车右边两个马达的-端
#define out3 5                //L298N的out3口，控制小车左边两个马达的+端
#define out4 6                //L298N的out4口，控制小车左边两个马达的-端

int   pwm = 0;                //最终赋给马达的pwm信号，用来控制马达的转速
float time ;                  //记录时间，配合millis函数用来计时
float echo_value;             //echo返回的值，用来计算距离
float distance;               //距离
float target=12;              //目标距离
float error;                  //当前的误差
float error_pre;              //上一次的误差
float kp=12;                  //pid的参数                            
float ki=0.08;                 //pid的参数          
float kd=0;                 //pid的参数
float P;                      //比例项误差
float I;                      //积分项误差
float D;                      //微分项误差
float PID;                    //误差总和，用来驱动马达
int   deta_t=50;              //50ms计算一次

void setup() {
  Serial.begin(9600);     //打开串口，波特率9600
  pinMode(trig, OUTPUT);  //设置触发信号
  pinMode(echo, INPUT);   //设置接收信号              
  pinMode(out1, OUTPUT);  //设置马达信号
  pinMode(out2, OUTPUT);  //设置马达信号
  pinMode(out3, OUTPUT);  //设置马达信号
  pinMode(out4, OUTPUT);  //设置马达信号
  time = millis();        //开始计时
}

void loop ()
{
     if (millis() > time + deta_t) // 没50ms进入一次以下程序
     {
       time = millis();        //记录当前时刻 

       /*给出触发信号，让测距模块工作*/ 
       digitalWrite(trig, LOW); 
       delayMicroseconds(2); 
       digitalWrite(trig, HIGH); 
       delayMicroseconds(10); 
       digitalWrite(trig, LOW);

       /*距离计算*/ 
       echo_value = pulseIn(echo, HIGH);   //读取返回的值
       distance   = (echo_value*0.034)/2; //计算距离

       /*位置式PID计算*/
       error = distance - target;
       
       P = kp*error;                       //P

       if(error > 0 && error < 0.8) ki = 0;
       if(error < 0 && error > -0.8)ki = 0;
       else ki = 0.08;
       if(-10 < error && error < 10) I += ki*error;                      
                        
       else     I = 0;                     //I ,在一定误差内I才作用
         
       //D = kd*((error - error_pre)/deta_t); //D，误差的变化率
       D = 0;  //这里没有用到D项，因为没有突然的变化可以不需要用D项
     
       PID=P + I + D ;                      //PID

      /*限幅*/
      if (PID>200)  PID=200;                            
      if (PID<-200) PID=-200;                            

      error_pre = error;                   //记录此次误差为上一刻误差

     Serial.print("距离:");    
     Serial.print(distance); 
     Serial.print("  "); 
     Serial.print("误差:");    
     Serial.print(error); 
     Serial.print("  ");    
     Serial.print(" P:");     
     Serial.print(P); 
     Serial.print("  "); 
     Serial.print(" I:");     
     Serial.print(I);
     Serial.print("  "); 
     Serial.print(" PID:");   
     Serial.print(PID);
     Serial.println("  "); 

    /*用PID的值来驱动马达*/
    pwm = map(PID, -200,200,-255,255);   //将PID的值转为能用作pwm驱动马达的值

    /*正转*/
    if( pwm > 0 )
    {
      if(PID < 100 && PID > 30 )
      {
        analogWrite(out1, 3*pwm);
        analogWrite(out2, 0);
        analogWrite(out3, 3*pwm);
        analogWrite(out4, 0);    
      }
      else
      {analogWrite(out1, pwm);
      analogWrite(out2, 0);
      analogWrite(out3, pwm);
      analogWrite(out4, 0);
      }
    }

    /*反转*/
    /*距离太小时PID的值也小，PWM的值也跟着小，没办法只能倍乘解决问题，简单粗暴*/
     if(pwm < 0 )
    {
      pwm = abs(pwm);
      analogWrite(out1, 0);
      analogWrite(out2, 4*pwm);
      analogWrite(out3, 0);
      analogWrite(out4, 4*pwm);
    }
       
     }
}

4.调参与分析

一开始，先调P，我只给一个kp值=100，这时候小车前跑跑，后倒倒的，说明冲过了头，然后P值变为了负数使得PWM也是负数，从而电机反转，又向后冲过了头，P值又变为正数，如此反复。所以我有降低P值，直接来个kp = 5，此时小车走到距离目标29cm时因为PWM值太小再也不能往前走。如此调整，最后得出kp = 12 状态较好，直接在12cm附近停了下来。

接着调I，P调好后再调I，在[0,1]之间选值，那就先让ki = 0.5，小车停下一会后往前，又往后，往前往后过程中都有停顿。这是我们就知道了ki调大了，因为累积的I值过大使得PWM值在较高的值处来回取值，也就是震荡。最后根据返回的数据，取ki = 0.08，且在误差范围为±10cm时积分才起作用，这是为了防止一开始I项偏大。最后的PID值并没有为0，不过也没有什么影响，毕竟小车的PWM值到50小车都不会往前移动。这里的误差我是设定在±0.8之间的，也符合条件。

D项的话这里不需要用到，因为没有突变的距离或者速度出现。

5.视频

PID定身12cm直线追踪小车做起来~