巡线PID算法

相信很多电子专业的同学都做过循迹小车这个小玩意儿，而在我们刚刚接触巡线的时候都是用的两个循迹模块（如下图）

 

左边的模块检测到黑线了就说明我车子的方向偏右了就需要往左转，同理，右边检测到黑线时我就需要往左转。

这样就会形成一种这样的蛇形巡线效果

这种巡线的方法虽然简单，但也十分具有局限性。只有在小车方向偏离程度很大时（也就是小车的一边的传感器碰到黑线时）才能进行方向的校正。而方向的校正就是让车子保持往一个方向的旋转（例如左边轮子往前，右边轮子往后，就达成顺时针旋转的效果），然后通过不同长度的延时完成转弯角度的大小，这种方法实在是不够优雅，于是在实验室小伙伴的介绍下，通过PID算法完成一种更优化的巡线。

 

当然细心的同学可能会发现问题，就两个循迹模块用PID的意义何在？这里的前提是我们的实验是基于四个循迹模块的巡线。

 

PID巡线具体思路

 

来一段伪代码:

if(2号循迹检测到黑线)

计偏差为-1

if(1号和2号黑线同时检测到黑线)

计偏差为-2

if(1号检测到黑线)

计偏差为-3

同理，当3号、3，4同时、4号检测到黑线时，分别记偏差为1、2、3

 

然后通过PID运算来调整两边电机的差速就可以实现一种比较顺滑的巡线

 

 

实现代码：

 

/*PID巡线算法程序*/

/*假设检测到黑线状态为1*/

#define TRACK_1    //1号循迹模块  

#define TRACK_2    //...

#define TRACK_3  

#define TRACK_4

/*PID参数修改处*/

float  kp = 0,

 ki = 0,

 kd = 0;

/*巡线状态检测及PID运算*/

float PID_Output(void)

{

float error,last_error;    //此次和上次的误差      

static float integral;    //积分累加项

float output；      //PID输出

 

if( (TRACK_1 == 0) && (TRACK_2 == 1) && (TRACK_3 == 0) && (TRACK_4 == 0) )    //0 1 0 0

 error = -1;

else if( (TRACK_1 == 1) && (TRACK_2 == 1) && (TRACK_3 == 0) && (TRACK_4 == 0) )   //1 1 0 0

 error = -2;

else if( (TRACK_1 == 1) && (TRACK_2 == 0) && (TRACK_3 == 0) && (TRACK_4 == 0) )   //1 0 0 0  

 error = -3;

else if( (TRACK_1 == 0) && (TRACK_2 == 0) && (TRACK_3 == 1) && (TRACK_4 == 0) )   //0 0 1 0  

 error = 1;                    

else if( (TRACK_1 == 0) && (TRACK_2 == 0) && (TRACK_3 == 1) && (TRACK_4 == 1) )   //0 0 1 1

 error = 2;  

else if( (TRACK_1 == 0) && (TRACK_2 == 0) && (TRACK_3 == 0) && (TRACK_4 == 1) )   //0 0 0 1

 error = 3;

else

 error = 0;

 

integral += error;

 

output = kp * error + ki * integral + kd * (error - last_error);

 

last_error = error;

 

return output;   //最后PID运算得到的是一个电机的差速调整值，左边的的机就加上相应的PID输出，右边则减去（具体是加还是减不重要，调整KP极性即可）

}