HuskyLens摄像头系列 | 写给小学生看的视觉PID巡线算法
哈士奇第一期回顾→撸狗初体验 | 手把手教你上手 HuskyLens 哈士奇人工智能摄像头
Hello，大家好，光天化日之下我又来撸狗了。距离上次撸狗已经过去了个把月时间了，那么这次又有什么新惊喜呢？
先来看一下本期的演示视频吧。
https://www.bilibili.com/video/av79068760/
什么？巡线？这也太简单了吧。不要急，听我慢慢道来。
# 哈士奇图形模块来了 #
上次视频里说了，DF 只给我提供了 HuskyLens （中文名：哈士奇）人工智能摄像头的通信协议，所以只能靠撸代码来体验哈士奇。你还不知道哈士奇是什么？更应该看一看上期的教程了。
但是这次，我拿到了哈士奇的新狗粮！那就是哈士奇的 Arduino 库和 Mind+ 图形模块（Mind+ v1.6.2 测试版）。也就是说，我们现在只要轻松拖动图形模块，就可以很轻松地玩转哈士奇了，而且哈士奇的界面，终于支持中文了！
注意：哈士奇的图形模块和 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）的坐标。
完整的巡线程序如下图所示。
在上面的程序中，我们定义了一个麦昆运动控制函数，可以分别接收左、右轮的速度参数，然后控制麦昆的运动（下同，不再赘述）。在主程序或者巡线算法中，我们只要调用这个函数并改变速度就可以了。
巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看文末的对比视频。
https://upload-images.jianshu.io/upload_images/273380-7d9bdc2d4e9b7204.gif
这时的巡线的平均速度是 60 左右，走完上面地图 1 圈需要 62 秒。试试改变巡线速度，看看你能以多快的速度巡线。我在编程时尝试了更大的速度，发现巡线效果不是很理想。
在改变速度的过程中，发现什么问题了么？
[*]小车在前进中明显左右晃动，速度变化不连贯，不能稳定直行运动；
[*]速度不能设置太快，速度太快很容易脱线；
[*]不同的转弯角度，需要的转弯速度也不一样，当巡线地图中有好几种转弯角度时很容易脱线。
我们先来看第 1 个问题：小车在前进中明显左右晃动，不能稳定直行运动。这是为什么呢？因为小车直行的位置区间太小了，只有当 x1 = 160 时，才能直行。由于小车在运动过程中带有惯性，所以这是很难达到的条件。
## 巡线算法2
那应该怎么办呢？很简单，增大直行的位置区间就好了。如下图所示，我们将 150 ≤ x1 ≤ 170 的区间设置为小车直行区间，当检测到黑线的起点坐标 x1 在这个区间内时，控制小车直行；当 x1 < 150 时，控制小车左转；当 x1 > 170 时，控制小车右转。
这样一来，是不是直行区间变大了？我们改变一下程序试试看效果吧。
巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看文末的对比视频。
https://upload-images.jianshu.io/upload_images/273380-3b609ee15f8833e6.gif
通过这些调整，我们发现，麦昆小车的巡线速度快了一些，**这时的巡线的平均速度是 70 左右，走完上面地图 1 圈需要 55 秒**，而且在直行路径上，速度变化也顺滑了一些，但是仍然会左右轻微晃动。
可是这样还是不能改善第 3 个问题，同一种转弯速度并不能适应所有的转弯角度，一旦设置的速度太快了，在转弯角度变化比较大的巡线地图中，还是很容易脱线。
## 巡线算法3
我们继续来调节，既然可以将检测到的黑线位置分成 3 个区间，我们是不是能如法炮制，将区间继续分为 5 个呢？黑线位置越靠两边，转弯速度越大；黑线位置越趋近于中间，越趋向于直行运动。
或者更进一步，直接分为 7 个调速区间呢？
我们来改编程序试试效果吧。
巡线效果如下面的动图所示，如果觉得动图有点卡顿，可以看文末的对比视频。
https://upload-images.jianshu.io/upload_images/273380-ba90acd08dacf062.gif
这时，麦昆小车的巡线速度可以更快了，此时的巡线的平均速度是 80 左右，走完上面地图 1 圈需要 51 秒，而且不管是转弯还是直行，速度变化都顺滑很多了。
# PID来了 #
经过上面的调速过程，你发现什么？
既然麦昆巡线时的运动状态或巡线区间可以分为 2 档、3 档、5 档、7 档，那是不是还能继续细分呢？9 档？11 档？……直到无限细分。调速区间越细分，巡线效果越好。但是这样写程序会越来越长，有什么办法呢？这里就要引出 PID 调速算法了。
PID 算法，是控制理论中最常用、最经典的算法，在我们的生产和生活中应用的非常广泛。它并不是什么很神圣的东西，大家一定都见过 PID 的实际应用：比如四轴飞行器，再比如平衡小车，还有汽车的定速巡航、3D 打印机上的温度控制器、恒温热水器等。就是类似于这种：需要将某一个物理量“保持稳定”的场合（比如维持平衡，稳定温度、转速等），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`变量就是屏幕中轴线的坐标，其他参数前面都有说明了，这里不再解释。
巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看文末的对比视频。
此时的巡线的平均速度是 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 参数至合适的数值，程序如下：
巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看文末的对比视频。
此时的巡线的平均速度是 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 的值很小。程序如下：
巡线效果如下面的动图所示，如果觉得 GIF 动图有点卡顿，可以看下面的对比视频。
此时的巡线的平均速度是 120 左右，走完上面地图 1 圈需要 38 秒。跟 PD 算法巡线效果差不多。
# 总结 #
最后放一个视频，做一下文中几个算法整体的效果对比：
https://www.bilibili.com/video/av79068862/
当然本文讲的是最基本的 PID 算法，这个算法在实际应用中还有很多可以进一步完善的地方。基于 PID 控制理论设计的新算法也很多，但是最基本的原理都是类似的。
最后用一张动图来总结一下本文讲的 PID 算法。从下图中，我们可以直观理解 PID 中三个参数 Kp、Ki、Kd 的作用。图中红色虚线为目标位置，蓝色实线为系统输出。
- 当调节 Kp 参数时，可以让系统输出越来越接近目标位置，但是存在抖动、以及存在稳定误差调节力度不够的问题；
- 调节 Ki 参数，可以弥补 Kp 参数调节力度不够的问题，但是也会增加抖动；
- 调节 Kd 参数，会让抖动减小，变化曲线更加平滑。
# 补充思考 #
在本文中，我们利用哈士奇摄像头得到了黑线位置，这里我们只用了黑线的其中一个位置参数：起点坐标 x1。但是另外三个参数都没有用上（起点坐标 y1，终点坐标 x2、y2）。
请你思考，是否将另外几个参数利用起来效果会更好呢？我们在巡线过程中，发现哈士奇屏幕中的黑线经常是斜着的，是否让黑线整体保持竖直，也就是让黑线与哈士奇屏幕的中轴线重合效果会更好呢？不仅去调节黑线与屏幕中间位置的偏差，还要去调节黑线的方向。
赶紧用本文讲的 PID 算法试试吧，你能挑战更快的速度么？
# 代码下载 #
代码与哈士奇激光切割源文件下载链接：https://t.zsxq.com/qZVzrbQ
# 参考文章 #
[*]我对『PID算法』的理解 —— 原理介绍：https://mc.dfrobot.com.cn/thread-14783-1-1.html
[*]用于乐高机器人的PID控制器：https://bbs.cmnxt.com/thread-5688-1-1.html
哈士奇第一期回顾→撸狗初体验 | 手把手教你上手 HuskyLens 哈士奇人工智能摄像头
青出于蓝 发表于 2020-1-1 16:33
可不可以在哈士奇的下面装一个舵机，当测到拐弯时判断弯度。弯度大时，减慢速度，弯度小时，正常速度。 ...
可以的，你可以试试哦 小含糊online 发表于 2020-6-24 16:33
学习了！感谢分享，讲的很详细，需要时间慢慢消化，抽空动手做一遍！
实践是检验真理的唯一标准~~~ 不错 火钳刘明{:5_144:} 膜拜大神{:5_165:} 牛贴。。。。。。。。。。 我居然没看懂、、、丢脸了{:5_140:} 学习了，收藏一下 超用心，赞！ 可不可以在哈士奇的下面装一个舵机，当测到拐弯时判断弯度。弯度大时，减慢速度，弯度小时，正常速度。 哈哈，只看懂了一小点点{:6_215:} pid是一个大的知识点，必考，强烈建议置顶。 这是位置式，请问做巡线增量式有比较过吗？ 面包板 发表于 2020-2-3 17:29
这是位置式，请问做巡线增量式有比较过吗？
这种还没有研究过 微笑的rockets 发表于 2020-1-23 10:32
pid是一个大的知识点，必考，强烈建议置顶。
{:6_204:} 跪求点拨那个PID动图（改变参数曲线变化那个）的做法{:6_215:} 那个日本的极速巡线小车的视频就是用的这个种算法优化结果吧 Xiongkaiyu 发表于 2019-12-24 15:42
学习了，收藏一下
去收藏夹里吃灰去吧 学习了！就是没有麦昆套件！{:6_215:} 跟着高手去学习! 外型打不开
这个柱子怎么切割呀？ 打开了网站，进不了
憋屈！https://mc.dfrobot.com.cn/forum.php?mod=image&aid=101977&size=300x300&key=e510103239b828d7&nocache=yes&type=fixnone