红外循迹传感器PID循迹算法
前一段时间参加了一个小车循迹竞速的比赛，获得了一个还行的成绩，所以在这里想把里面的核心部分PID寻线算法给大家分享一下。
关于做好的实物视频我上传到了B站，大家可以点击查看：小车双向PID循迹视频

两轮差速PID红外循迹视频

红外循迹模块的视频演示：红外循迹模块使用演示视频
大家如果要购买产品的话可以点击我的淘宝链接：小车驱动板链接
红外循迹模块链接：三路PID红外循迹模块实物链接
红外循迹模块百度云资料链接：三路PID红外循迹模块资料链接  提取码：bh0w
循迹小车程序下载链接：开源程序下载链接
复制这段内容后打开百度网盘手机App，操作更方便哦

（本程序在小车前后安装了两个红外循迹模块具有双向循迹功能，会根据到终点的距离来调节小车速度，可以进行三级调速。本程序获得过软银杯机器人竞速组的冠军，也是个小比赛，作者水平有限，只是想跟大家分享下经验，如果有需要的可以下载）
 我做了两个关于红外循迹模块的原理讲解和使用视频上传到了B站上，如果大家感觉文档说明的不是很清楚的话也可以看视频，本人水平有限，如果视频中有讲的不好的地方希望大家多多包涵，如果有需要的话可以在上方实物链接中购买循迹模块和小车驱动板产品，感谢大家支持。
三路pid红外循迹模块原理视频讲解：三路pid红外循迹模块原理视频讲解
三路pid红外循迹模块参数配置视频讲解：三路pid红外循迹模块参数配置视频讲解
红外传感器循迹原理如图1所示：
图1 红外传感器原理

 红外二极管发射红外光，接收管接受反射的红外光信号。不同的颜色反光效果不同，如果红外光照射在黑色物体上，由于黑色物体对光的吸收能力强，反射的光很少。但照在白色物体上，由于白色物体对光的吸收能力弱，反射的光较多。从而当红外二极管发出的光照射在不同颜色的物体上时，接收管接受的红外光强度也不同，从而可以判断出前面物体的颜色。


图2 赛道
 红外循迹就是根据这个原理，将红外模块安装在寻迹小车上，然后再赛道上面贴上黑色胶带，当检测到黑色时说明小车在黑线上，检测到白色是说明小车偏离黑线，则需要向黑线靠近，赛道如图2所示。市面上大多数红外循迹传感器采用数字式的，也就是将红外接收管接受到的模拟量信号通过电压比较转换为0和1的数值信号，按照这种方法寻线的话至少需要两个红外传感器，固定在黑线两侧，当左边红外传感器检测到偏离黑线时往右偏，当右边红外传感器检测到偏离黑线时往左偏。这样确实可以寻线，但是寻线效果相当不好，这样寻线有以下几个缺点：

①寻线精度差

②容易偏离轨迹

③寻线速度慢

 不过也有优点，优点就是程序简单。市场上也有五路的和七路的但是这样效果也就好那么一点。其实市场上这种将模拟量变成数字量的方法本身就将精度很高的传感器变为精度低的了。因为采集红外接收管信号的方式是通过AD采集，如果是10位AD的话精度位1024，采用STM32F103C8T6的话有12位AD，采集的精度位4096。但这种采集精度高的处理方法是比较好，但是处理的过程相对麻烦，大致思路就是单片机通过内部AD采集循迹传感器模拟量，然后通过PID参数控制舵机角度。

 下面我要介绍的红外循迹模块也就是这种直接通过单片机内部AD采集红外循迹传感器数据，然后根据数据PID动态调调节舵机角度的方法。采用的红外循迹模块为三路循迹模块实物如图3所示，电路原理图如图4所示。

图3 三路红外循迹传感器实物图
图4  循迹模块电路原理图

 其中三个红外管的功能不同，左右两个红外管的作用是判断传感器离黑线边界的距离，获取的AD值越大说明离黑线越远，则舵机需要转动的角度也就越大。中间的一个红外管的作用是判断是否传感器在黑线内，我将用一个曲线图来向大家表示原理，便于大家理解，图5位循迹模块安装图。


图5 循迹传感器安装图
 将循迹传感器固定在小车上，传感器离轨道的高度为3cm左右，假设左两个红外传感器之间的距离和轨道黑线的宽度相同为3cm，传感器从左到右移动获得的图像如图6所示：


图6 左右传感器向左移动图像
 将右传感器值减去左传感器值可得到图像如图7所示：


图7 左右传感器相减数据整合图
 这时获取的数据有效长度只能在3cm黑线轨迹之内，当数据为0时表示小车循迹方向没有偏差，大于0说明小车往左偏，小于0说明小车往右偏，数值越大说明偏离的越大。但是由于数据有效长度只有3cm当小车速度快时，容易冲出有效循迹黑线范围，同时我们也注意到，有效长度左边和右边有一段数据没有用到，我可以尝试将左有两段没用用到的数据纳入到有效处理数据。


图8 根据中传感器获取左右阈值点
 如上图8所示，我们可以通过中间的红外传感器来获取，当中间传感器小于阈值时，通过判断左右两个红外传感器相减值得正负来处理左右两边的数据，数据翻转将斜率变为原来2倍。得到的最终效果图如下如所示，最终得到的宽度为原来3倍左右，为9cm。即使传感器完全偏离黑线，最终也会回来，最终达到处理后的效果为图9所示。


图9 最终数据处理效果
 综上所述，需要经过这几步达到最终效果：

(1)确定左右传感器差值(D_AD_VALUE)：

确定方法：将传感器放到白色纸上，将左右两个传感器的数据通过串口发送到电脑上，获取左右两个传感器差值，D_AD_VALUE=Left_AD-Right_AD。

(2)确定左传感器峰值点(LEFT_MAX)和右传感器峰值点(RIGHT_MAX)：

确定方法：将左红外传感器和右红外传感器分别从左到右移动，将数据通过串口发送到电脑上观察数据获取最大值，一般当传感器移动到黑线中间是获得数据最大值。

(3)确定左阈值点(LEFT_THREASH)和右阈值点(RIGHT_THREASH):

确定方法：当达到左传感器达到峰值时，观察中间红外传感器数值，此时中间红外传感器数值即为左阈值点。同样当达到右传感器达到峰值时，观察中间红外传感器数值，此时中间红外传感器数值即为右阈值点。

(4)进行数据整合：

①当中间传感器获取的AD数值在左阈值点和右阈值点之间时，将获取到的左传感	器数据(Left_AD)与右传感器数据(Right_AD)相减加上左右传感器差	Data_Out=(Left_AD-Right_AD+D_AD_VALUE)。		

②当①中获取的数据大于零(DATA_OUT>0)且中间传感器获取的数据小于左阈	值时(Mid_AD<LEFT_THREASH),  Data_Out=(2*LEFT_MAX-Left_AD)2

③当①中获取的数据大于零(DATA_OUT>0)且中间传感器数据获取的数据小于左阈	值时(Mid_AD<RIGHT_THREASH),  Data_Out=(2RIGHT_MAX-RIGHT_AD)*2
综上所述换成用C语言形式写即为：
//获取循迹传感器输出函数
//返回值：int类型，范围循迹传感器数据，根据此值来调节小车舵机角度
int GetTraceDate()
{
	int Data_Out;//定义数据输出变量
	int Left_AD,Right_AD,Mid_AD;//定义左右中传感器AD值变量
	Left_AD=Get_Adc(2);
 	Mid_AD=Get_Adc(3);
	Right_AD=Get_Adc(4);
	Data_Out=(Left_AD-Right_AD+D_AD_VALUE);
	if((Data_Out>0)&&(Mid_AD<LEFT_THERSH))
	{	
		Data_Out=(2*LEFT_MAX-Left_AD)*2;
	} 
	else if((Data_Out<0)&&(Mid_AD<RIGHT_THERSH))
	{
		Data_Out=(Right_AD-2*RIGHT_MAX)*2;
	}
	return Data_Out;
}

注意：这是最理想情况下的调节步骤，在调试时需要根据自己的模块安装位置和黑线宽度调节一些参数，但大致调试步骤就是这样,关于详细的调节步骤和细节方面可以参考我发布到B站上的视频教程。
如果大家感觉配置麻烦的话我做了一款一键配置的循迹模块，配置简单，串口直接输出数据，使用于1-4cm线宽，如果大家感兴趣的话可以查看此篇博客有详细介绍：串口直输型循迹传感器。

	本文介绍我买的37款传感器套件中的循迹传感器模块，它的实物图如下:
 	模块的左边是红外发射和接收装置，模块中间靠右位置是一个调节测量距离的可调电阻，它总共有三个引脚，从上到下分别为：
	VCC：接电源正极
	OUT：信号输出引脚，输出1表示测量面颜色较深，输出0表示测量面颜色比较浅
	GND：接电源负极

	参考卖家和参考文献中给的代码进行测试，测试代码如下：
int Led = 13; //定义LED 接口
int buttonpin = 9; //定义寻线传感器接口
int val;//定义数字变量val
void setup()
{
  pinMode(Led, OUTPUT); //定义LED 为输出接口
  pinMode(buttonpin, INPUT); //定义寻线传感器为输出接口
}
void loop()
{
  val = digitalRead(buttonpin); //将数字接口3的值读取赋给val
  if (val == HIGH) //当寻线传感器检测有信号时，LED 闪烁
  {
    digitalWrite(Led, HIGH);
  }
  else
  {
    digitalWrite(Led, LOW);
  }
}
	实物连接图如下：
 
	最后是演示视频：

注意事项：
	1）实际应用时，红外发射接收装置应该朝向地面，之前测试时不清楚，红外装置朝上，信号输出引脚的输出值始终没有变化；
	2）实际应用时，一般是多个循迹模块同时使用，Arduino通过判断不同位置的循迹模块的输出值，来判断小车或者其它设备该如何移动。	
	3）需要调节可调电阻，找到最佳的检测距离，否则循迹模块的输出引脚始终输入高电平或者低电平。

参考文献：
[1]http://s.etao.com/detail/38038171970.html?tbpm=20161226
[2]http://www.fishing-sh.com/et/item.php?id=528132899161
[3]https://sites.google.com/site/studyarduino/04mblock/03mblock-xun-ji-che

一、介绍
  红外跟踪循迹传感器使用TCRT5000循迹模块。TCRT5000采用的是蓝色的LED发射管，通电后发出人眼看不到的红外线。传感器的黑色部分用于接收，内部电阻器的电阻随所接受的红外光而变化。
二、组件
★Raspberry Pi 3主板*1
★树莓派电源*1
★40P软排线*1
★循迹传感器模块*1
★双色LED模块*1
★面包板*1
★跳线若干
三、实验原理



  循迹的检测原理是红外发射管发射光线到路面，红外光遇到白线等颜色较浅的地面则被反射，接收管接收到反射光，经施密特触发器整形后输出低电平；当红外光遇到黑线等颜色较深的地面时则被吸收，接收管没有接收到反射光，经施密特触发器整形后输出高电平。
四、实验步骤
  第1步： 连接电路。




树莓派
T型转接板
循迹传感器




GPIO0
G17
SIG


5V
5V
VCC


GND
GND
GND






树莓派
T型转接板
双色LED灯




GPIO1
G18
R


-
-
G


GND
GND
GND




  第2步： 我使用的是数字输出DO口，所以控制程序很简单。运行程序，不断循环打印检测信息。当检测到白色物品时打印’White line is detected’，且LED灯亮红色；当检测到黑色物品时，打印’…Black line is detected’，LED灯熄灭。
#!/usr/bin/env python
import RPi.GPIO as GPIO

TrackPin = 11
LedPin   = 12

def setup():
	GPIO.setmode(GPIO.BOARD)       # Numbers GPIOs by physical location
	GPIO.setup(LedPin, GPIO.OUT)   # Set LedPin's mode is output
	GPIO.setup(TrackPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
	GPIO.output(LedPin, GPIO.LOW) # Set LedPin LOW to off led

def loop():
	while True:
		if GPIO.input(TrackPin) == GPIO.LOW:
			print 'White line is detected'
			GPIO.output(LedPin, GPIO.HIGH)  # led on
		else:
			print '...Black line is detected'
			GPIO.output(LedPin, GPIO.LOW) # led off

def destroy():
	GPIO.output(LedPin, GPIO.LOW)     # led off
	GPIO.cleanup()                     # Release resource

if __name__ == '__main__':     # Program start from here
	setup()
	try:
		loop()
	except KeyboardInterrupt: 
 # When 'Ctrl+C' is pressed, the child program destroy() will be  executed.
		destroy()

  实验结果示例：

一、先编写一个循迹传感器的发布者

原理扫盲： 

使用的传感器如下 


这是一种红外循迹传感器，可以辨认出黑色轨迹的白色跑道，即可以使用两个这样的传感器达到跟随黑色轨迹行走， 

因为当传感器碰到黑线时，输出的电平就会由低电平变为高电平，若在此时改变相应电机的转向，则可以达到一直跟随黑线行走的效果。

因为Bool变量的值只有 真 （true) 和假 （false），为了便于理解，先编写一个发布Bool数据类型的发布者，具体代码如下



#include "mbed.h"
#include <ros.h>
#include <std_msgs/Bool.h>

ros::NodeHandle  nh;

std_msgs::Bool tracking_msg;
ros::Publisher pub_tracking("Tracking", &tracking_msg);


Timer t;
DigitalIn sig1(p9);

int main() {
    t.start();

    nh.initNode();
    nh.advertise(pub_tracking);

    long publisher_timer = 0;

    while (1) {

        if (t.read_ms() > publisher_timer) {
            tracking_msg.data = !sig1;
            pub_tracking.publish(&tracking_msg);
            publisher_timer = t.read_ms() + 1000;
        }
        nh.spinOnce();
    }
}



二、将Bool型发布的消息改为Twist型消息

需要修改的代码如下： 

①首先修改头文件 

将



#include <std_msgs/Bool.h>

改为



#include <geometry_msgs/Twist.h>

因为Bool类型只能发布True和Flase两个数据，要发布小车接收者的Twist的数据类型才能驱动小车，因此就要引入Twist的数据类型的头文件。 

② 

将



std_msgs::Bool tracking_msg;

改为



geometry_msgs::Twist tracking_msg;

③修改发布的数据 

将



tracking_msg.data = !sig1;

改为



if(sig1==1){                   //黑
                tracking_msg.linear.x = 0.5;
            }
            else if(sig1==0){  //白
                tracking_msg.linear.x = -0.5;
            }

这样就可以将红外循迹传感器接收到的信息转化成Twist数据类型，只要小车订阅了 Tracking 这个话题，小车就可以根据循迹传感器进行巡线了。

最终循迹传感器Twist数据类型的发布者代码如下



#include <ros.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle  nh;

geometry_msgs::Twist tracking_msg;
ros::Publisher pub_tracking("Tracking", &tracking_msg);


Timer t;
DigitalIn sig1(p9);

int main() {
    t.start();

    nh.initNode();
    nh.advertise(pub_tracking);

    long publisher_timer = 0;

    while (1) {

        if (t.read_ms() > publisher_timer) {
            if(sig1==1){    //黑
                tracking_msg.linear.x = 0.5;
            }
            else if(sig1==0){  //白
                tracking_msg.linear.x = -0.4;
            }
            pub_tracking.publish(&tracking_msg);
            publisher_timer = t.read_ms() + 1000;
        }
        nh.spinOnce();
    }
}



三、编写一个发布Twist消息的超声波发布者

加上超声波的初衷是怕出现小车会离开黑色轨道，然后碰到障碍物上就会一直卡死不能再动。具体代码如下：



#include "mbed.h"
#include "ultrasonic.h"
#include <ros.h>
#include <ros/time.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle  nh;

geometry_msgs::Twist ultrasonic_msg;
ros::Publisher pub_ultrasonic("ultrasonic", &ultrasonic_msg);

void dist(int distance)
{
    if(distance<100){
        ultrasonic_msg.linear.x = 0.5;
    }
    else{
        ultrasonic_msg.linear.x = -0.4;
    }
}

Timer t;    
ultrasonic mu(p9, p10, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
                                        //have updates every .1 seconds and a timeout after 1
                                        //second, and call dist when the distance changes

int main()
{
    t.start();
    mu.startUpdates();//start mesuring the distance
    nh.initNode();
    nh.advertise(pub_ultrasonic);
    long range_time = 0;
    while(1)
    {
         //Do something else here
        mu.checkDistance();     //call checkDistance() as much as possible, as this is where
                                //the class checks if dist needs to be called.
        if (t.read_ms() >= range_time) {
            pub_ultrasonic.publish(&ultrasonic_msg);
            range_time =  t.read_ms() + 50;
        }
        nh.spinOnce();       
    }
}



四、将以上代码与电机驱动的代码整合起来



#include "mbed.h"
#include "Motor.h"
#include "ultrasonic.h"
#include <ros.h>
#include <ros/time.h>
#include <geometry_msgs/Twist.h>


DigitalIn sig1(p11);
DigitalIn sig2(p12);
Motor A(p21, p22); 
Motor B(p23, p24);
int sig3;

void messageCb(const geometry_msgs::Twist& msg)
{
  if (msg.angular.z == 0 && msg.linear.x == 0)
  {
    A.speed(0);
    B.speed(0);
  }
  else
  {
    if (msg.angular.z < 0)
    {
      A.speed(0.5);
      B.speed(-0.5);
      wait_ms(20);
      A.speed(0);
      B.speed(0);      
    }
    else if (msg.angular.z > 0)
    {
      A.speed(-0.5);
      B.speed(0.5);
      wait_ms(20);
      A.speed(0);
      B.speed(0);      
    }
    else if (msg.linear.x < 0)
    {
      A.speed(-0.5);
      B.speed(-0.5);
      wait_ms(20);
      A.speed(0);
      B.speed(0);
    }
    else if (msg.linear.x > 0)
    {
      A.speed(0.5);
      B.speed(0.5);
      wait_ms(20);
      A.speed(0);
      B.speed(0);
    }
  }
}

void dist(int distance)
{
    if(distance<100){
        sig3=0;
    }
    else{
        sig3=1;
    }
}

Timer t;
ultrasonic mu(p9, p10, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
                                        //have updates every .1 seconds and a timeout after 1
                                        //second, and call dist when the distance changes
ros::NodeHandle  nh;
geometry_msgs::Twist tracking_msg;
ros::Publisher pub_tracking("Tracking", &tracking_msg);
ros::Subscriber<geometry_msgs::Twist> sub("Tracking", &messageCb);

int main() {
  t.start();
  mu.startUpdates();//start mesuring the distance
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(pub_tracking);
  long publisher_timer = 0;

  while (1)
  {
    //Do something else here
    mu.checkDistance();     //call checkDistance() as much as possible, as this is where
                            //the class checks if dist needs to be called.
    if (t.read_ms() > publisher_timer) {
            if(sig1==0 && sig2==0){
                if(sig3==1){
                   tracking_msg.linear.x = 0.5;
                   tracking_msg.angular.z = 0.0;
                }
                else if(sig3==0){
                   tracking_msg.linear.x = 0.0;
                   tracking_msg.angular.z = 0.5;
                }
            }
            else if(sig1==1 && sig2==1){
                tracking_msg.linear.x = -0.5;
                tracking_msg.angular.z = 0.0;
            }
            else if(sig1==0 && sig2==1){
                tracking_msg.linear.x = 0.0;
                tracking_msg.angular.z = -0.5;
            }
            else if(sig1==1 && sig2==0){
                tracking_msg.linear.x = 0.0;
                tracking_msg.angular.z = 0.5;
            }
            pub_tracking.publish(&tracking_msg);
            publisher_timer = t.read_ms() + 10;
     }
     nh.spinOnce();
  }
}


运行代码： 

1.roscore 

2.sudo chmod 666 /dev/ttyACM0 

3.rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 

运用 rqt_graph 显示当前运动的节点和话题: 

rosrun rqt_graph rqt_graph 

运行后得到如下图所示



其中 Tracking 是话题名称，传感器发布消息到 Tracking 话题中，然后电机的驱动订阅 Tracking 这个话题，于是传感器就可以控制电机的转动。