pid巡线算法程序_技术分享——从单个到多个颜色传感器巡线原理解析

巡线竞速是常见的机器人比赛项目，它对速度和精准度要求都比较高。该项目大多要求现场进行结构搭建，并且赛前只提供培训参考地图，比赛使用的地图和巡线任务一般是现场才进行公布，对于参赛选手的能力要求较高。

巡线任务的基础要求是车体(符合规格)的正投影始终覆盖着黑线，从起点通过巡线的方式沿着轨迹运动到终点，轨迹一般为8-20mm粗细的黑线，15mm粗的黑线轨迹居多，底色多为白色，地图材质为喷绘居多。

1

一、参考值的功能及计算方法

巡线使用的传感器为颜色传感器，它能检测三种不同类型的数值：颜色(0-7)、反射光强度(0-100)、环境光强度。巡线使用的参数基本为反射光强度或者称为灰度值。

(一)何为灰度值？

颜色传感器发出的光在地图表面经过漫反射，重新被颜色传感器接收到的光线强度，它的变化范围是0-100，数值越大，表示反射回来的光线强度越大。

(二)如何利用灰度值来区分地面和黑线？

灰度值表示的是反射回来的光线强度，不同颜色、材质的物体距离颜色传感器同样距离的前提下，反射回来的光线强度不一样。颜色越深，反射回来的光线越少，颜色传感器检测到的灰度值也就越小，所以黑线上的灰度值会比较低，而白色地图上的灰度值会比较高，但是同样是黑线上，不同位置的灰度值并不相等，不同位置的白色地图检测的灰度值也不一样，如何有效区分地面和黑线呢？我们引入参考值这一概念来进行说明。

(三)参考值的计算

通过实际的检测，我们发现虽然黑线和白色地图上的灰度值不是每个地方都相等，但是他们的数值变化范围不大，如：

黑线：10、13、9、8

白色地图：50、48、52、49

用折线图表示如下：

 

在EV3程序模块中一般需要使用一个参数来区分白色地图和黑线，从上图可以知道，只要这个值大于黑线上灰度值的最大值，小于地面上灰度值的最小值就可以用来作为参考值，但是我们没必要去找到参考值的上下限，因为需要大量的测量，一般采用以下三种方式处理：

第一种：地面和黑线各测三次，计算地面和黑线上的灰度值的平均值，将两者的平均值作为参考值，也就是：

灰地平=(地1+地2+地3)/3

灰黑平=(黑1+黑2+黑3)/3

参考值=(灰地平+灰黑平)/2

第二种：地面和黑线各测一次灰度值，取两者的平均值作为参考值，即

参考值=(灰地+灰黑)/2

第三种：因为黑线附近的灰度值是接近线性变化的(已在另一个PID巡线推送中测试证明)，测量黑线和地面的分界处的灰度值作为参考值，即

参考值=灰分界

参考值的功能在于区分地面和黑线，个人比较倾向于第二种方法，并且一般都让小朋友近似取整，不会影响分辨效果，并且能避免小数点的存在引发输入失误。

结合参考值的功能，以及黑线和地面上的灰度值分布图，我们可以知道黑线上的灰度小于参考值，地面上的灰度大于参考值。

知道了如何区分地面和黑线，接下来我们看看几种简单的基础巡线和P巡线(比例巡线)。

2

1、2、3、4个光感的基础巡线

(一)单光感Z字巡线——一个光感，两种动作状态

在只使用一个颜色传感器巡线的情况下，并不能通过程序来区分颜色传感器在线的左侧还是右侧，所以巡线过程中，颜色传感器只能在线的其中一侧巡线，下面以颜色传感器在黑线的右侧巡线为例来说明巡线原理。

当颜色传感器看到黑线时，车子需要右转，才能保证不越过黑线；当颜色传感器看到地面时需要左转才能不飞线，程序实现如下：

也可以使用等待模块实现，程序如下：

使用等待进行程序编写时需要注意模块顺序，为了降低车体放置的位置要求，可以控制车体先左转看到黑线，然后右转看到地面，依此循环。如果等待模块放在前面，就需要在开始时把机器人放在黑线上，否则就不能正常巡线了。

1、如果是在线的左侧巡线，车体的动作刚好相反，具体如下：

车体沿着黑线的左侧巡线和黑线右侧巡线的程序，可以通过改变等待或者切换的条件进行更改——大于号换成小于号，小于号换成大于号。

2、转弯的幅度的大小取决于左右两侧电机的速度差，速度差越大，摆动幅度越大，左转和右转的速度差也不需要保持一致。

3、这种巡线方式为基础巡线，通过不断地左转和右转的方式进行巡线，车体的运动轨迹呈Z字形。

(二)单光感分段巡线——一个光感，多种运动状态

单光感Z字巡线的摆动幅度较大，并且巡线过程中没有直行部分，巡线速度也比较慢，可以根据灰度值变化的区间进行分段，在颜色传感器红色光圈接触黑线但是并未全部在黑线上时，让车体保持直行状态。

下面以黑线灰度为10，地面灰度为50，参考值为30的巡线条件为例进行原理说明和程序编写，先把灰度变化范围分为三段10-25、25-35、35-50。因为黑线的宽度有限，为了保证巡线的稳定性，直行部分的灰度区间不宜过大，程序实现如下：

灰度10和灰度50这样的数值并未在程序中得以体现，是因为不属于直行部分的就是需要左转和右转的部分，划定处于中间段的直行区间，另外两个区间自然就区分开了。

上面的程序也可以通过数值运算将三种情况和三个不同的数值对应起来，利用数值进行切换，参考程序如下：

25为直行灰度区间的下限，10为直行部分的灰度变化幅度(灰度为25-35时直行，变化幅度为10)，经过这样的运算后，巡线状态分为：5-15，大幅右转；15-25，小幅右转；25-35，直行；35-45，小幅左转；45到55，大幅左转，一共五种状态。

在编写程序时，应该确保程序涉及到灰度区间比实际测的变化区间大，这样能避免出现运算结果不再列举的数值内而执行默认情况导致飞线的情况产生。

(三)双光感巡线——两个光感，四种运动状态

使用一个光感能够顺利完成简单的巡线任务，但是涉及到有十字路口或者需要在指定的路口停车的巡线任务时，一个光感是难以完成比赛任务的，下面我们来看看使用两个颜色传感器可以如何巡线。

在不飞线的情况下，两个光感与黑线的位置关系有以下四种：

四种情况下，左、右侧光感的实测灰度值和参考值的大小关系及车体需要采取的动作如下：

可以看到车体的动作都是为了让车体保持在情况1的状态下，程序实现方式如下：

1、为了保证巡线过程中，任一光感看到黑线时车身没有大幅度的停顿，在进行功率设置时，最好保证直行的功率和转弯状态下功率大的那个电机的功率相同

2、巡线过程中，任一光感看到黑线后，控制电机时保持开启状态而不是开启一定的幅度(圈数、度数、时间)，因为每次看到黑线时需要完成的转弯幅度不一样。

3、巡线过程中车体飞线时，可以先观察飞线的原因，是转弯方向错了、幅度不够还是切入角度过大，转弯方向错了就检查程序里面需要转弯的分支的执行条件和转弯参数，幅度不够就调整转弯的速度差，切入角度过大可以尝试调整光感之间的距离，以及光感距离动力轮的距离。

使用两个光感进行巡线时，巡线的速度比单光感的快很多，但是使用两个光感进行巡线时经过直角弯容易飞线，经过弯道后需要一定的时间才能摆脱摆动频繁的状态。

下面我们看看巡线过程中摆动幅度较小的三光感巡线程序如何实现。

(四)三光感巡线——三个光感，7种运动状态

使用三个光感进行巡线时，光感和黑线的位置关系有以下8种情况，车体有7种不同的应对方式：

左侧、中间、右侧光感的实测灰度值和参考值的大小关系及车体需要采取的动作如下：

情况7对应有两种不同的位置关系——黑线处在左侧和中间光感之间，黑线处在中间和右侧光感之间。

对应的三光感巡线程序为：

1、在直角弯部分，转弯的速度差比较大，因为此时中间的光感就是处在黑线上的，所以需要车体先保持转弯状态一定时间，可以在转弯和等待之间加一个短暂的时间(程序图中的等待0.1秒)，这段时间需要确保中间的光感脱离黑线，然后通过等待模块控制中间的光感再次看到黑线。

2、用转弯开启和等待模块组合的方式代替转弯开启一定的幅度(时间、角度、圈数)可以减少巡线过程中的停顿，因为到达设定的时间、角度或者圈数时车体会刹车并且进入停止的状态。

使用三个光感巡线的方式可以平稳而较快地完成巡线任务，但是为了准确地分辨十字路口(三个光感同时看到黑线)，中间地光感一般在驱动轮地中轴线上，并且左右两侧地光感距离中间地光感不能太远，因为车体进入直角弯时地角度时随机地，只有三个光感距离位置不太远才能稳定识别十字路口，但是左右两侧的光感距离近了又会导致巡线过程中的转弯动作较多，下面我们看看能较好解决十字路口识别和转弯动作较少的四光感巡线。

(五)四光感巡线——四个光感，四种运动状态

从左到右的四个颜色传感器端口分别是1、2、3、4，左、右侧电机端口分别是B、C，并且2、3距离较近(1-2倍的黑线宽度)，1、4距离较远。

这种巡线方法就是使用1、4号端口的颜色传感器进行双光感巡线，使用2、3号光感来控制转弯幅度，以及十字路口的识别。路口的识别可以在这个循环外并行一个路口判断程序，实现2、3同时看到黑线，跳出01循环的效果，跳出后需要执行的动作接在循环01后面，如果不需要进行分段，可以在外侧再套一个循环。

1、1号传感器看到黑线则车体大幅度左转到2号看到黑线，针对不同的路况调整不同的速度差，转弯后加等待的效果是避免1、2同时看到黑线时(十字路口)，等待条件瞬间满足，车体转弯动作执行时间短暂而导致飞线，等待的加入可以保证看到1号看到黑线时，车体会有一定幅度转弯动作，转弯的幅度和效率取决于等待的长短和转弯速度差的大小。(4号看到黑线时，处理方法类似)

2、巡线内部等待模块里的参考值需略大于计算出来的参考值，这样的调整是为了降低转弯过程中因为车身惯性的存在而导致转弯过线的情况出现，我们需要努力保持的状态是黑线处于2、3传感器之间，左转到2号看到黑线，惯性的存在使车身在2号看到黑线时会继续多偏移一点就能实现黑线处于2、3传感器之间的效果。

3、为了降低巡线过程中的转弯幅度，可以在1、4未看到黑线时，使用2、3端口进行双光感比例巡线

4、反复调整速度差和2、3端口的参考值可以确保过弯道时，黑线处在2、3之间

3

单光感比例巡线和双光感比例巡线

(一)比例巡线的成立条件

比例巡线得以成立的基础是黑线附近的灰度值是接近线性变化的，看到黑线越多灰度值数值越小，看到地面越多，灰度值数值越大。

比例巡线的基本思路是计算实测灰度值和参考值之间的差值，利用差值来控制转弯幅度的大小，差值的绝对值越大，摆动幅度越大，差值为零时车体直行。

相比较于前面的基础巡线，比例巡线的速度差不是指定的几组，而是不断变化的，并且距离黑线越近或者越远，转弯的幅度越大，巡线过程平缓，摆幅小。

(二)单光感比例巡线

以在黑线右侧进行单光感比例巡线为例进行说明，单光感比例巡线的程序为：

转向度的变化范围时-100到100，为0时直行，小于零则左转，大于零则右转，绝对值越大，转弯幅度越大。他有两组特殊值，正负50和正负100，当它为正负50时，其中一个端口的功率会为0(定点转弯)，当它为正负100时，两个端口的功率互为相反数(原地转弯)。

使用一个光感在黑线右侧巡线，看到黑线时实测小于参考值，车体需要右转，也就是需要保证转向度是正的，可以进行这样的运算： 

转向度=(参考值-实测)X系数(正数)

看到地面时，实测大于参考值，运算得到的转向度时负数，车体会左转，满足我们的要求。

在不同弯曲程度的轨迹上巡线时，实测值可能一样，但是需要转动的幅度不一样，可以通过调整系数实现，系数的变化范围是0-5之间(100/(50-30)=5)，系数比较接近时，一般每次变化0.1。

1、沿着黑线右侧巡线时，系数需要时正数，沿着黑线左侧巡线时，可以把系数改成负数，或者改变运算关系为：

转向度=(实测-参考值)X系数(正数)

2、不同弯曲程度的轨迹，对应的最佳系数不一样，所以使用单光感比例巡线时一般会根据时间或者路口进行分段巡线。

3、比例巡线很难通过直角弯，可以利用这一特性直行通过十字路口，到那时可以使用PD巡线(微分计算较复杂，一般不作教学使用，感兴趣可以研究PID巡线)或者在转弯时降低速度解决，程序如下：

即使用参考值和实测的差值控制转向度和最大直行功率，当然你也可以把差值进行分段，差值很大时使用更大的比例系数。

4、使用移动槽或者自定义模块控制时，可以这样编写程序：

(三)双光感比例巡线

简单说，双光感比例巡线是利用左、右两侧光感实测的灰度值的差值来控制转弯幅度，程序实现如下：

直行功率为50，运算关系为：

转向度=(灰左-灰右)x系数

如果使用移动槽或者自定义模块控制，程序实现如下：

运算关系为：

P左=最大直行功率+(灰左-灰右)x系数

P右=最大直行功率-(灰左-灰右)x系数

如果想要转大弯的时候降低速度，可以这样编程： 

运算关系为：

转向度=(灰左-灰右)x系数1

功率=设定功率-(灰左-灰右)x系数2

1、使用左右两侧光感实测灰度的差值控制转向度时，为了确保两个光感同时看到黑线时，车身保持直行状态，需要保证两个光感看到地面和黑线时的灰度值接近(一般控制差值10以内)，如果差值较大就不能保证两个光感同时看到黑线或者地面时，车体是直行或者几乎直行的状态。

2、当两个光感看到地面或者黑线灰度值不接近时，有两种解决办法：

a、换两个数值比较接近的光感进行巡线

b、进行数值标准化(标准化为0-100的值)，标准化的换算公式为：

灰标准=100 *(灰实测-灰min)/(灰max-灰min)

灰max、灰min分别为地面和黑线上的灰度值，灰实测为实际检测的灰度值，灰标准为标准化后的灰度值。两个光感需要分别进行标准化，以1号(10、60)，2号(25、75)为例，标准化程序如下：

双光感比例巡线可以直行通过路口，也可以在巡线程序外并行路口判断的程序跳出巡线的循环。比例系数的调整对小朋友而言是比较难掌握好的事情。使用比例巡线时车体转弯平缓，但是车身能够保持直行的状态比较少(转向度为0才直行)，不是在转弯就是在转弯的路上，相比较常规的双光感巡线，下面这种双光感比例巡线也是可以不错的巡线效果，程序如下：

巡线使用两个单光感比例巡线组成，1、4端口分别连接左侧、右侧的光感，当其中一侧的光感看到黑线时就以看到黑线的光感进行比例巡线，两个光感都看到地面时，车体直行；两个光感同时看到黑线时，直行一段距离越过黑线。

4

尾言

以上就是几种常见的基础巡线和比例巡线的原理说明和程序参考，程序内的参数需要针对具体巡线环境做处理，截图只是为了方便大家知道原理。如果文中有错误或者表述不当的地方，欢迎指正。如果有更好玩的巡线思路，欢迎文章下方留言讨论。

声明：授权转载自公众号“LCR乐工坊”。

◆◆ 精彩回顾 ◆◆

乐高机器人终于出新品了

35所高校将设人工智能本科专业

◆◆ 扫码关注◆◆

关注公众号，更多精彩！