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控制器

你可能感兴趣的:(HuskyLens摄像头系列 | 写给小学生看的视觉PID巡线算法)