0.教学资源

以下是本节课的微课，配合文档学习效果更佳喔。 5.mp4 (29.3MB)动手1-云台跟踪小球.pptx

1.课前导入

今天我们要做一个跟踪小球（或者说是色块）的云台。那么我们通过下面这个视频来看看我们最终要达到的效果。 舵机云台自动跟踪目标.mp4 (3.64MB)视频中右侧的就是云台，它会实时跟踪颜色片。跟踪颜色？这个词是不是很耳熟，我们在第一课：机器视觉与颜色识别中就讲到了如何识别并跟踪颜色。赶紧来复习一下吧！

1.1复习Lab模型

Lab模型—锁定颜色

Lab色彩模型，又称亮度 - 对比度模型，被设计来接近人类视觉。
image.png
图4.Lab模型示意图
在OpenMVIDE中，点击工具-机器视觉-阈值编辑器，选择帧缓冲区后可以将摄像头拍摄的画面进行颜色捕捉。通过调整Lab阈值，得到想要追踪的颜色区域，如下图演示的追踪红色色块。复制最下方的“Lab阈值”，即可获得“红色”区域的Lab值。
image.png
图5.Lab阈值编辑器
将刚才复制的“红色”Lab值粘贴在代码区，创建一个变量“red”，并将值赋予给它。

  1. red=(41,60,44,84,-10,63)

颜色跟踪函数img.find_blobs()

  1. red=(41,60,44,84,-10,63)
  2. img.find_blobs([red],pixels_threshold=200,area_threshold=200)

通过img.find_blobs()，找到红色色块。
[red]：刚刚定义的阈值变量。
pixels_threshold=200：如果色块像素数量小于200，会被过滤掉。
area_threshold：如果色块被框起来的面积小于200，会被过滤掉。

2.知识准备

云台和舵机

舵机也叫“伺服电机”，它可以作为机器人的驱动单元，带动机械臂、门等装置，相当于人类的肌肉。本节课，我们使用两个舵机组成云台，使云台能够在上下、左右方向进行摆动。
将舵机的电源线插在OpenMV上，可以用夹子固定起来。
image.png
图6.云台
如何用代码驱动舵机呢？我们可以通过Servo.angle控制舵机转动角度。

  1. from pyb import Servo            #调用库
  3. #将左右舵机插在接口1；上下舵机插在接口2
  4. x_servo=Servo(1) #创建舵机对象
  5. y_servo=Servo(2)
  6. x_servo.angle(90) #将左右舵机旋转到90度位置
  7. y_servo.angle(45) #上下舵机旋转到45度位置

LCD屏幕

LCD屏幕也就是我们平时说的液晶屏，安装在OpenMV上的方法特别简单，直接插在侧边的插槽中即可。
控制LCD屏幕的代码也非常简单，只需要初始化并且在while(Ture)中调用lcd.dipsplay(img)就可以了。

  1. import sensor,image,lcd
  3. sensor.reset() # 初始化传感器
  4. sensor.set_pixformat(sensor.RGB565)
  5. sensor.set_framesize(sensor.QQVGA)
  6. sensor.skip_frames(10) # 让新设置生效
  7. sensor.set_auto_whitebal(False) # 关闭自动白平衡
  8. lcd.init() # 初始化lcd屏幕
  10. while(True):
  11.     img = sensor.snapshot()
  12.     lcd.display(img) #让lcd屏幕显示画面

image.png
图7.LCD屏幕

PID算法

PID算法是运用最广泛的一种自动控制算法，P表示比例（proportional），I表示积分（intergral），D表示积分（derivative）。
PID算法是为了让物理量“保持稳定”而生的。试想这样一个场景，我们希望控制汽车保持在50km/h的速度，当计算机测到某一时间的车速是45km/h，它立刻发出指令，告诉发动机：加速！结果，发动机猛踩油门，嗖的一下，汽车急加速到了60km/h。这时电脑又发出命令：刹车！
结果乘客肯定要吐了…对吧。
所以在大多数时候，我们不能用两个量（比如：加速和刹车）来控制一个物理量，因为这样太不稳定了。而且一些物理量是有惯性的，比如说如果我们将热水器的电源拔掉，它的“余热”可能还会使水温继续升高一会。
PID算法就是来解决这些问题的，它可以将需要控制的物理量带到目标附近，它可以“预见”这个量的变化趋势，它也可以消除因为散热、阻力等因素造成的静态误差。
动手时间1-追小球的云台 - 图7
图8.PID算法
首先我们先来讲P是如何作用的。举刚刚我们热水器的例子：当当前水温和目标水温差很多的时候，我们就让加热器的档位加大进行加热；如果两者差别不大时，就降低档位进行加热。这就是P的作用。但是大家观察上图，当P增加时，仅仅在t=1和t=2之间达到了目标值，其他时候都低于目标值。这是为什么呢？因为日常生活中，机器存在能量损失。
比如说，假如有个人把热水器带到了非常冷的地方，开始烧水了。需要烧到50℃。在P的作用下，水温慢慢升高。直到升高到45℃时，他发现了一个不好的事情：天气太冷，水散热的速度，和P控制的加热的速度相等了。
这可怎么办？P兄这样想：我和目标已经很近了，只需要轻轻加热就可以了。于是，水温永远地停留在45℃，永远到不了50℃。
这时，I变量就起作用了，随着时间的推移，只要没达到目标温度，加热功率就会随时间增加。所以I的作用就是减小静态误差（能量损失这样的误差），让物理量尽可能接近目标值。
但是大家看图片，当I增加的时候，虽然更多时候达到了目标值，但是波动比之前更大了。这时候就需要D变量进行控制。
D变量的职责就是将物理量的“变化速度”趋近于0，它像一个刹车，并且当D变量越大使，刹车力道越大。

  1. from pid import PID
  3. x_pid = PID(p=0.06,d=0.01,i=0.02, imax=100) #设置x（水平）舵机的PID值
  4. y_pid = PID(p=0.07,d=0.01,i=0.02, imax=70)  #设置y（纵向）舵机的PID值
  6. while(True):
  7.     img = sensor.snapshot()
  8.     if blobs:
  9.         x_error = max_blob.cx()-img.width()/2 #目标点的x偏移值=目标点的x坐标-画面宽度的一半
  10.         y_error = max_blob.cy()-img.height()/2#目标点的y偏移值=目标点的y坐标-画面长度的一半
  12.         x_output=x_pid.get_pid(x_error,1) #通过error值，得到output值
  13.         y_output=y_pid.get_pid(y_error,1)
  15.         x_servo.angle(x_servo.angle()-x_output/2)        #将当前角度减去output值
  16.         y_servo.angle(y_servo.angle()+y_output/2)

（在开始制作之前，需要将pid.py文件放入OpenMV的内存中。）

3.完整代码

  1. import sensor, image,lcd
  3. from pid import PID
  4. from pyb import Servo            #调用库
  6. x_servo=Servo(1)
  7. y_servo=Servo(2)
  9. blue_threshold  = (26, 41, -32, 3, -40, -7)    #蓝色色素块
  10. x_pid = PID(p=0.06,d=0.01,i=0.02, imax=100)
  11. y_pid = PID(p=0.07,d=0.01,i=0.02, imax=70)
  13. sensor.reset()
  14. sensor.set_pixformat(sensor.RGB565)
  15. sensor.set_framesize(sensor.QQVGA)
  16. sensor.skip_frames(10)
  17. sensor.set_auto_whitebal(False)
  19. lcd.init() # 初始化LCD屏幕
  21. def find_max(blobs):
  22.     max_size=0
  23.     for blob in blobs:
  24.         if blob[2]*blob[3] > max_size: #blob[2]是该色块的宽，blob[3]是高
  25.             max_blob=blob
  26.             max_size = blob[2]*blob[3]
  27.     return max_blob    #找到视野中的最大色素块
  30. while(True):
  31.     img = sensor.snapshot()
  32.     lcd.display(img)
  33.     blobs = img.find_blobs([blue_threshold])
  34.     if blobs:
  35.         max_blob = find_max(blobs)
  36.         x_error = max_blob.cx()-img.width()/2
  37.         y_error = max_blob.cy()-img.height()/2
  39.         print("x_error: ", x_error)            #在参数调试窗口打印色块偏离值，便于调试与修正
  41.         img.draw_rectangle(max_blob.rect())                 #在色块外围四周处画框
  42.         img.draw_cross(max_blob.cx(), max_blob.cy()) #色块中心坐标处画十字
  43.         lcd.display(img)
  44.         x_output=x_pid.get_pid(x_error,1)
  45.         y_output=y_pid.get_pid(y_error,1)
  46.         print("x_output",x_output)                   #在参数调试窗口打印坐标值，便于调试与修正
  47.         x_servo.angle(x_servo.angle()-x_output/2)            #输出左右方向上的PWM波控制云台追踪色块标志
  48.         y_servo.angle(y_servo.angle()+y_output/2)        #输出上下方向上的PWM波控制云台追踪色块标志