HuskyLens摄像头系列 | 写给小学生看的视觉PID巡线算法

封面.png

Hello，大家好，光天化日之下我又来撸狗了。距离上次撸狗已经过去了个把月时间了，那么这次又有什么新惊喜呢？

先来看一下本期的演示视频吧。

https://v.qq.com/x/page/m3034lv677y.html

什么？巡线？这也太简单了吧。不要急，听我慢慢道来。

哈士奇图形模块来了

上次视频里说了，DF 只给我提供了 HuskyLens （中文名：哈士奇）人工智能摄像头的通信协议，所以只能靠撸代码来体验哈士奇。你还不知道哈士奇是什么？更应该看一看上期的教程了。

上期回顾：撸狗初体验 | 手把手教你上手 HuskyLens 哈士奇人工智能摄像头

但是这次，我拿到了哈士奇的新狗粮！那就是哈士奇的 Arduino 库和 Mind+ 图形模块（Mind+ v1.6.2 测试版）。也就是说，我们现在只要轻松拖动图形模块，就可以很轻松地玩转哈士奇了，而且哈士奇的界面，终于支持中文了！

在Mind+中新增的哈士奇摄像头模块

哈士奇图形模块

注意：哈士奇的图形模块和 Arduino 代码库目前处于工程测试阶段，正式版很有可能会发生调整和改动，请对照本文描述的原理酌情修改程序使用。

巡线介绍

既然有了这些功能，那么玩哈士奇就简单多了。所以这一期我将带大家来玩一玩哈士奇的视觉巡线功能！

为了配合完成巡线功能，我用激光切割给麦昆小车做了一个可调节支架，将哈士奇安装到了麦昆小车上，如下图所示。其中哈士奇的角度是可以旋转调节的。

麦昆与哈士奇

智能车巡线，相信大家都已经很熟悉了，就是让小车沿着规定的轨道（通常是黑线，下图视觉效果图用了蓝线）、按照一定的速度进行移动。一般情况下，我们只需要使用普通的巡线传感器，就可以完成相应的功能。既然麦昆装上了哈士奇，我们这次就不用麦昆自带的巡线传感器来进行巡线了，我们来试试哈士奇视觉巡线有什么奇特的功效。

麦昆视觉巡线场景

视觉巡线跟普通巡线传感器巡线有什么区别呢？采用普通巡线传感器时，一般根据线宽、巡线传感器的间距、巡线传感器的数量等，会有好几种巡线模式，我们这里以麦昆小车自带的双巡线传感器为例，这也是最常见的巡线模式。如下图所示，我们可以很容易就可以根据巡线传感器在黑线（视觉效果图用了蓝线）和白线上的位置来判断麦昆小车什么时候需要直行、什么时候需要左转、什么时候需要右转。

麦昆巡线传感器普通巡线

那么哈士奇视觉巡线的逻辑又是怎么样的呢？其实跟巡线传感器的逻辑非常类似，我们只需要知道小车相对黑线是什么位置就行了：

• 当麦昆小车在黑线的偏左位置时，控制小车向右转弯；
• 当麦昆小车在黑线的偏右位置时，控制小车向左转弯；
• 当麦昆小车与黑线相对居中时，控制小车直行。

在本文中，我们希望麦昆小车以尽可能快的恒定速度进行巡线，即麦昆小车的几何中心速度保持不变。

开始巡线吧

我们先来看一下，麦昆小车带着哈士奇巡线时，哈士奇屏幕上显示的信息。需要注意的是，为了巡线，我们需要将摄像头斜向下调节，这样能够看到离麦昆小车更近距离的黑线，巡线效果更佳。

巡线时哈士奇屏幕显示的信息

为了方便说明问题，我们将上图中哈士奇屏幕上的信息剥离出来，抽象成下图的几何数学模型。

哈士奇巡线视角

我们知道哈士奇屏幕的分辨率是 320×240，屏幕左上角的顶点为屏幕的坐标原点（0, 0），水平向右方向为 X 轴正方向，竖直向下方向为 Y 轴正方向，因此屏幕右下角的坐标为（320, 240）。上图中红色虚线为屏幕的中轴线，这条线的横坐标 x = 160。上图中黑色的线，为哈士奇摄像头“看到”的巡线地图，蓝色箭头为哈士奇计算出来的线条方向。蓝色箭头的起点坐标为（x1, y1），终点坐标为（x2, y2）。

巡线算法1

经过这样的数学抽象之后，其实巡线逻辑就很简单了。当哈士奇检测到黑线在屏幕的左边时，也就是蓝色箭头的坐标 x1 < 160 时，控制小车左转；当黑线在屏幕的右边时，此时 x1 > 160 时，控制小车右转；当黑线在屏幕的中间时，此时 x1 = 160 时，控制小车直行。

左转与右转

逻辑是不是很简单？我们马上来编写个简单的巡线程序试试吧。

首先我们可以在 Mind+ 软件或者 Arduino 软件中，通过一个简单的图形模块或者一行简单的代码，就可以获取（x1, y1）、（x2, y2）的坐标。

哈士奇巡线图形模块

完整的巡线程序如下图所示。

普通巡线1-程序

在上面的程序中，我们定义了一个麦昆运动控制函数，可以分别接收左、右轮的速度参数，然后控制麦昆的运动（下同，不再赘述）。在主程序或者巡线算法中，我们只要调用这个函数并改变速度就可以了。

巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看文末的对比视频。

普通巡线1

这时的巡线的平均速度是 60 左右，走完上面地图 1 圈需要 62 秒。试试改变巡线速度，看看你能以多快的速度巡线。我在编程时尝试了更大的速度，发现巡线效果不是很理想。

在改变速度的过程中，发现什么问题了么？

1. 小车在前进中明显左右晃动，速度变化不连贯，不能稳定直行运动；
2. 速度不能设置太快，速度太快很容易脱线；
3. 不同的转弯角度，需要的转弯速度也不一样，当巡线地图中有好几种转弯角度时很容易脱线。

我们先来看第 1 个问题：小车在前进中明显左右晃动，不能稳定直行运动。这是为什么呢？因为小车直行的位置区间太小了，只有当 x1 = 160 时，才能直行。由于小车在运动过程中带有惯性，所以这是很难达到的条件。

巡线算法2

那应该怎么办呢？很简单，增大直行的位置区间就好了。如下图所示，我们将 150 ≤ x1 ≤ 170 的区间设置为小车直行区间，当检测到黑线的起点坐标 x1 在这个区间内时，控制小车直行；当 x1 < 150 时，控制小车左转；当 x1 > 170 时，控制小车右转。

三档调速-左转-直行-右转

这样一来，是不是直行区间变大了？我们改变一下程序试试看效果吧。

普通巡线2-程序

巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看文末的对比视频。

普通巡线2

通过这些调整，我们发现，麦昆小车的巡线速度快了一些，这时的巡线的平均速度是 70 左右，走完上面地图 1 圈需要 55 秒，而且在直行路径上，速度变化也顺滑了一些，但是仍然会左右轻微晃动。

可是这样还是不能改善第 3 个问题，同一种转弯速度并不能适应所有的转弯角度，一旦设置的速度太快了，在转弯角度变化比较大的巡线地图中，还是很容易脱线。

巡线算法3

我们继续来调节，既然可以将检测到的黑线位置分成 3 个区间，我们是不是能如法炮制，将区间继续分为 5 个呢？黑线位置越靠两边，转弯速度越大；黑线位置越趋近于中间，越趋向于直行运动。

五档调速

或者更进一步，直接分为 7 个调速区间呢？

七档调速

我们来改编程序试试效果吧。

普通巡线3-程序

巡线效果如下面的动图所示，如果觉得动图有点卡顿，可以看文末的对比视频。

普通巡线3

这时，麦昆小车的巡线速度可以更快了，此时的巡线的平均速度是 80 左右，走完上面地图 1 圈需要 51 秒，而且不管是转弯还是直行，速度变化都顺滑很多了。

PID来了

经过上面的调速过程，你发现什么？

既然麦昆巡线时的运动状态或巡线区间可以分为 2 档、3 档、5 档、7 档，那是不是还能继续细分呢？9 档？11 档？……直到无限细分。调速区间越细分，巡线效果越好。但是这样写程序会越来越长，有什么办法呢？这里就要引出 PID 调速算法了。

PID 算法，是控制理论中最常用、最经典的算法，在我们的生产和生活中应用的非常广泛。它并不是什么很神圣的东西，大家一定都见过 PID 的实际应用：比如四轴飞行器，再比如平衡小车，还有汽车的定速巡航、3D 打印机上的温度控制器、恒温热水器等。就是类似于这种：需要将某一个物理量“保持稳定”的场合（比如维持平衡，稳定温度、转速等），PID 都会派上大用场。

它的公式为：

模拟PID公式

或者我们在编程时更常用的是数字式的 PID 公式：

数字PID公式

PID 控制系统是一种闭环控制系统，闭环控制是根据控制对象输出的反馈来进行校正的控制方式。它能够根据测量出的实际值与计划值之间的误差 Error，按一定的标准来进行纠正。

根据公式，可以看到 PID 分为 3 个部分：比例调节（P）、积分调节（I）、微分调节（D）。那么这三者之间是什么关系呢？每个部分的调节对巡线会产生什么样的影响呢？我们一个一个来看。

注意：如标题所说，本文是写给小学生看的 PID 调速算法，所以下文中描述的是已经简化的 PID 算法，默认没有去计算每个程序的循环时间，也就是没有去计算积分和微分时间。主要有几个原因：一、是因为程序功能比较单一，每个循环时间差不多，在调节 Kd、Ki 参数时，将时间参数 T 当成一个整体进行调试了；二、省略了时间参数后，程序编写会更加简单，容易看懂与理解。

P：调节当前的误差

以视觉巡线为例，测量出来的实际值为黑线的起点坐标 x1，但是我们希望黑线的起点坐标能够在屏幕中央，也就是位于 x = 160 的位置。这时，误差 Error = x1 - offset = x1 - 160。那么接下来，我们就可以根据这个误差大小来调节转速的大小了，也就是我们上面分析的，黑线越是远离屏幕中心，误差越大，这时需要的转速也越大。根据 PID 公式，转速 V_turn 的计算公式为：

V_turn = Vp_turn = Kp × Error = Kp × (x1 - 160)

我们知道，物体的运动一般可以由直线运动和转弯运动组成，所以速度也可以由直线运动速度 V_straught 与转弯速度 V_turn 组成。为了使麦昆小车保持一定的运动速度，我们可以分别给左右轮加上或减去一个转弯速度 V_turn，这样麦昆小车中心的速度就能够保持不变。

V_left = V_straught + V_turn
V_right = V_straught - V_turn

根据这个公式，我们可以简单的理解为，P 控制器的作用是根据误差的大小产生一个转弯速度 V_turn，用于叠加在左右轮的速度上。

那么 Kp 的值取多少合适呢？这就需要我们根据实际巡线情况，手动进行调节与设置了。多尝试几次，应该就能设置出一个比较合适的 Kp 值了。

编写程序并调节 Kp 值如下。程序中的的offset变量就是屏幕中轴线的坐标，其他参数前面都有说明了，这里不再解释。

P巡线-程序

巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看文末的对比视频。

P巡线

此时的巡线的平均速度是 120 左右，走完上面地图 1 圈需要 39 秒。不仅在速度变快的情况下还能正常巡线，而且速度的变化明显顺滑多了。

到这里，你已经比大部分巡线程序实现的效果好多了。还能怎么继续完善呢？

D：预测未来的误差

上述程序增加了 P 比例调节之后，我们发现巡线效果好多了，速度的变化非常平滑，但是仍然会有晃动情况。这是因为惯性，速度越大，惯性越大，当检测到的黑线越靠近屏幕边缘时，V_turn 就越大，导致惯性就越大。能不能想办法改善这个问题呢？

这个时候就是 D 微分调节要上场了。微分调节的作用如下图，它可以理解为一个弹簧阻力，当惯性越大的时候，阻力或拉力也就越大，直到将惯性方向的速度变为 0，也就是下图中弹簧下面的方块，始终会被限制在一个小范围内运动，不会产生太大范围的抖动变化。加上 D 微分调节之后，我们就可以有效的抑制晃动。但是也要去适当的调节 D 微分调节的参数，不然限制的阻力太大，麦昆小车转弯速度 V_turn 被限制的太小也不行，这会导致转不过弯来。

微分调节效果

可以说，D 的功能，就是根据之前的变化趋势，去防范于未来可能发生的失控问题。之前的变化趋势越大（惯性越大），接下来阻力的作用越明显。根据 PID 公式，D 调节也要计算一个 Vd_turn 的分量：

Vd_turn = Kd × derivative = Kd × (Error - lastError)

其中，Kd 是需要我们手动调节的参数，Error 为当前测量的误差，lastError 为上次测量的误差。

总的 V_turn 计算公式为：

V_turn = Vp_turn + Vd_turn = Kp × Error + Kd × derivative

编写 PD 调速程序，并调节 Kd 参数至合适的数值，程序如下：

PD巡线-程序

巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看文末的对比视频。

PD巡线

此时的巡线的平均速度是 120 左右，走完上面地图 1 圈需要 38 秒。巡线过程中的晃动已经明显减弱了很多。

I：纠正过去的误差

虽然在本文的效果中不是很明显，但是单单 P 比例调节，可能会出现一个问题：调节力度不够，永远存在一个稳定的误差。因为实际情况中可能会存在各种各种的阻力或者影响输出的情况，具体例子可以看文末的参考文章：我对『PID算法』的理解 —— 原理介绍

在这种情况下，我们就需要引入 I 积分调节了。积分调节可以产生一个新的转速 Vi_turn。它的计算公式为：

Vi_turn = Ki × integral

积分调节的作用就是可以消除那个相对稳定的误差。它能将过去误差累计起来，只要存在误差，这个累计值 integral 就会越来越大，就会使调节力度越来愈大，逐渐就能抵消阻力产生的稳定误差的影响。

总的 V_turn 的计算公式如下：

V_turn = Vp_turn + Vi_turn + Vd_turn

即：V_turn = Kp × Error + Ki × integral + Kd × derivative

编写 PID 调速程序，并调节 Ki 参数至合适的数值。由于本示例中，几乎没有稳定误差的影响，所以 Ki 的值很小。程序如下：

PID巡线-程序

巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看下面的对比视频。

PID巡线

此时的巡线的平均速度是 120 左右，走完上面地图 1 圈需要 38 秒。跟 PD 算法巡线效果差不多。

总结

最后放一个视频，做一下文中几个算法整体的效果对比：

https://v.qq.com/x/page/q3034l3ljw7.html

当然本文讲的是最基本的 PID 算法，这个算法在实际应用中还有很多可以进一步完善的地方。基于 PID 控制理论设计的新算法也很多，但是最基本的原理都是类似的。

最后用一张动图来总结一下本文讲的 PID 算法。从下图中，我们可以直观理解 PID 中三个参数 Kp、Ki、Kd 的作用。图中红色虚线为目标位置，蓝色实线为系统输出。

• 当调节 Kp 参数时，可以让系统输出越来越接近目标位置，但是存在抖动、以及存在稳定误差调节力度不够的问题；
• 调节 Ki 参数，可以弥补 Kp 参数调节力度不够的问题，但是也会增加抖动；
• 调节 Kd 参数，会让抖动减小，变化曲线更加平滑。

PID调节示意图

补充思考

在本文中，我们利用哈士奇摄像头得到了黑线位置，这里我们只用了黑线的其中一个位置参数：起点坐标 x1。但是另外三个参数都没有用上（起点坐标 y1，终点坐标 x2、y2）。

请你思考，是否将另外几个参数利用起来效果会更好呢？我们在巡线过程中，发现哈士奇屏幕中的黑线经常是斜着的，是否让黑线整体保持竖直，也就是让黑线与哈士奇屏幕的中轴线重合效果会更好呢？不仅去调节黑线与屏幕中间位置的偏差，还要去调节黑线的方向。

让黑线尽可能与屏幕中轴线重合

赶紧用本文讲的 PID 算法试试吧，你能挑战更快的速度么？

代码下载

请到知识星球下载本教程对应的源代码。

参考文章

• 我对『PID算法』的理解 —— 原理介绍
• 用于乐高机器人的PID控制器