[人工智能]OpenMV：15追踪其他物体的小车

追踪矩形的小车

其中car.py和pid.py与"追小球的小车"中一致，不需要更改
需要更改的地方

• a.返回摄像头中的矩形：rects = find_rects(threshold)函数
• b.计算方向误差(左右)：原先为：x_error = max_blob[5]-img.width()/2—>视野中最大色块的中心x位置-摄像头的中心位置 用于计算目标在摄像头中偏离中心的距离
• 改成：x_error = max_rect[0]+max_rect[2]/2-img.width()/2—>rect类无法直接调出矩形的中心x坐标，因此得进行计算视野中最大矩形左上角的x坐标 + 矩形一半的宽度 - 摄像头中心的x坐标
• c.在色块的中心(cx,cy)画十字：img.draw_cross(max_blob[5], max_blob[6])
• 改成：img.draw_cross(int(max_rect[0]+max_rect[2]/2), int(max_rect[1]+max_rect[3]/2))此处要强制类型转换！

# Blob Detection Example
#
# This example shows off how to use the find_blobs function to find color
# blobs in the image. This example in particular looks for dark green objects.

import sensor, image, time
import car  #引入控制电机运动的代码
from pid import PID     #从pid模块中导入pid类

# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # 重置感光原件
sensor.set_pixformat(sensor.RGB565) # 设置为彩图
sensor.set_framesize(sensor.QQVGA) # 设置分辨率 
sensor.skip_frames(10) # 跳过几帧使图像生效 
sensor.set_auto_whitebal(False) # 关闭白平衡
clock = time.clock() # 设置时钟 

# For color tracking to work really well you should ideally be in a very, very,
# very, controlled enviroment where the lighting is constant...
green_threshold   = (76, 96, -110, -30, 8, 66)      #设置颜色阈值，不同颜色要修改！！！
size_threshold = 2000       #如果摄像头检测到像素块的面积大于2000，就说明我们的小车离小球非常近，那么需要后退一段距离

x_pid = PID(p=0.5, i=1, imax=100)       #x_pid是方向pid：控制小车电机的方向（如果在小车拐弯时发现小车拐的角度非常大，可以把x_pid的参数调得小一点）
h_pid = PID(p=0.05, i=0.1, imax=50)     #h_pid是距离pid：用于控制小车的速度（如果速度偏快，可以把h_pid的p参数调得再小一点）

# 定义一个函数去找到视野中最大的块：视野中肯定不止一个小球，我们让小车去追踪最近（最近的在摄像头里就最大）的一个小球
# 原理和追小球的小车一致，不需要更改
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # 从sensor中截取一段图像

#修改：    blobs = img.find_blobs([green_threshold])       #调用颜色识别的函数
    rects = img.find_rects()
    if rects:#如果小球找到矩形
        max_rect = find_max(rects)      #调用find_max()函数找到视野中最大的矩形去追踪
        #pid的差值，后面用于计算pid的参数
#修改：        x_error = max_blob[5]-img.width()/2       
        x_error = max_rect[0]+max_rect[2]/2-img.width()/2 # rect类无法直接调出矩形的中心x坐标，因此得进行计算左上角的x坐标+一半的宽度-摄像头中心的x坐标
        h_error = max_rect[2]*max_rect[3]-size_threshold
        print("x error: ", x_error)
        '''
        for b in blobs:
            # Draw a rect around the blob.
            img.draw_rectangle(b[0:4]) # rect
            img.draw_cross(b[5], b[6]) # cx, cy
        '''
        
        
        img.draw_rectangle(max_blob[0:4]) # 在找到的色块上画一个矩形
#修改：        img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
        img.draw_cross(int(max_rect[0]+max_rect[2]/2), int(max_rect[1]+max_rect[3]/2)) # 在色块的中心cx，cy画一个十字
        
        #调用了pid.py中的函数，用于得到最终的结果，控制小车的运动
        x_output=x_pid.get_pid(x_error,1)
        h_output=h_pid.get_pid(h_error,1)
        print("h_output",h_output)
        car.run(-h_output-x_output,-h_output+x_output)      #两个轮胎的h_pid应该是一样的（距离），x_pid应该是相反的
   
    else:#没有找到矩形
        car.run(18,-18) #左右电机向相反的方向慢速运动，让小车在原地以非常慢的速度旋转，可以让小车在原地搜寻小球

追踪AprilTags的小车

其中car.py和pid.py与"追小球的小车"中一致，不需要更改
需要更改的地方

• a.返回摄像头中的AprilTags：rects = find_apriltags()函数
• b.计算方向误差(左右)：原先为：x_error = max_blob[5]-img.width()/2—>视野中最大色块的中心x位置-摄像头的中心位置 用于计算目标在摄像头中偏离中心的距离
• 改成：x_error = max_apriltag[6]-img.width()/2 —>AprilTags返回值的数组apriltag索引值和blob不一样
• c.在色块的中心(cx,cy)画十字：img.draw_cross(max_blob[5], max_blob[6])
• 改成：img.draw_cross(max_apriltag[6],max_apriltag[7])AprilTags返回值的数组apriltag索引值和blob不一样
• d.如果想查找二维码就改成image.find_qrcodes([roi])

# Blob Detection Example
#
# This example shows off how to use the find_blobs function to find color
# blobs in the image. This example in particular looks for dark green objects.

import sensor, image, time
import car  #引入控制电机运动的代码
from pid import PID     #从pid模块中导入pid类

# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # 重置感光原件
sensor.set_pixformat(sensor.RGB565) # 设置为彩图
sensor.set_framesize(sensor.QQVGA) # 设置分辨率 
sensor.skip_frames(10) # 跳过几帧使图像生效 
sensor.set_auto_whitebal(False) # 关闭白平衡
clock = time.clock() # 设置时钟 

# For color tracking to work really well you should ideally be in a very, very,
# very, controlled enviroment where the lighting is constant...
green_threshold   = (76, 96, -110, -30, 8, 66)      #设置颜色阈值，不同颜色要修改！！！
size_threshold = 2000       #如果摄像头检测到像素块的面积大于2000，就说明我们的小车离小球非常近，那么需要后退一段距离

x_pid = PID(p=0.5, i=1, imax=100)       #x_pid是方向pid：控制小车电机的方向（如果在小车拐弯时发现小车拐的角度非常大，可以把x_pid的参数调得小一点）
h_pid = PID(p=0.05, i=0.1, imax=50)     #h_pid是距离pid：用于控制小车的速度（如果速度偏快，可以把h_pid的p参数调得再小一点）

# 定义一个函数去找到视野中最大的块：视野中肯定不止一个小球，我们让小车去追踪最近（最近的在摄像头里就最大）的一个小球
# 原理和追小球的小车一致，不需要更改
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # 从sensor中截取一段图像 

#修改：    blobs = img.find_blobs([green_threshold])       #调用颜色识别的函数
    rects = img.find_apriltags()
    if apriltags:#如果小球找到ApriTags
        max_apriltag = find_max(apriltags)      #调用find_max()函数找到视野中最大的Apriltag去追踪
        #pid的差值，后面用于计算pid的参数
#修改：        x_error = max_blob[5]-img.width()/2       
        x_error = max_apriltag[6]-img.width()/2 
        h_error = max_rect[2]*max_rect[3]-size_threshold
        print("x error: ", x_error)
        '''
        for b in blobs:
            # Draw a rect around the blob.
            img.draw_rectangle(b[0:4]) # rect
            img.draw_cross(b[5], b[6]) # cx, cy
        '''
        
        
        img.draw_rectangle(max_blob[0:4]) # 在找到的色块上画一个矩形
#修改：        img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
        img.draw_cross(max_apriltag[6],max_apriltag[7]) # 在色块的中心cx，cy画一个十字
        
        #调用了pid.py中的函数，用于得到最终的结果，控制小车的运动
        x_output=x_pid.get_pid(x_error,1)
        h_output=h_pid.get_pid(h_error,1)
        print("h_output",h_output)
        car.run(-h_output-x_output,-h_output+x_output)      #两个轮胎的h_pid应该是一样的（距离），x_pid应该是相反的
   
    else:#没有找到矩形
        car.run(18,-18) #左右电机向相反的方向慢速运动，让小车在原地以非常慢的速度旋转，可以让小车在原地搜寻小球

追踪模板的小车

其中car.py和pid.py与"追小球的小车"中一致，不需要更改
需要更改的地方

• 图像格式改为灰度图sensor.set_pixformat(sensor.GRAYSCALE)
• 导入模块from image import SEARCH_EX, SEARCH_DS
• 创建或导入一个模板，template = image.Image("/template.pgm")注意这个模板必须得是pgm格式的
• 修改查找函数img.find_template(template, 0.70, step=4, search=SEARCH_EX)

r = img.find_template(template, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
    #find_template(template, threshold, [roi, step, search])
    #threshold中的0.7是相似度阈值,roi是进行匹配的区域（左上顶点为（10，0），长80宽60的矩形），
    #注意roi的大小要比模板图片大，比frambuffer小。
    #把匹配到的图像标记出来

# Blob Detection Example
#
# This example shows off how to use the find_blobs function to find color
# blobs in the image. This example in particular looks for dark green objects.

import sensor, image, time
import car  #引入控制电机运动的代码
from pid import PID     #从pid模块中导入pid类

#修改
from image import SEARCH_EX, SEARCH_DS

# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # 重置感光原件
sensor.set_pixformat(sensor.GRAYSCALE) # 设置为彩图
sensor.set_framesize(sensor.QQVGA) # 设置分辨率 
sensor.skip_frames(10) # 跳过几帧使图像生效 
sensor.set_auto_whitebal(False) # 关闭白平衡
clock = time.clock() # 设置时钟 

# For color tracking to work really well you should ideally be in a very, very,
# very, controlled enviroment where the lighting is constant...
green_threshold   = (76, 96, -110, -30, 8, 66)      #设置颜色阈值，不同颜色要修改！！！
size_threshold = 2000       #如果摄像头检测到像素块的面积大于2000，就说明我们的小车离小球非常近，那么需要后退一段距离

x_pid = PID(p=0.5, i=1, imax=100)       #x_pid是方向pid：控制小车电机的方向（如果在小车拐弯时发现小车拐的角度非常大，可以把x_pid的参数调得小一点）
h_pid = PID(p=0.05, i=0.1, imax=50)     #h_pid是距离pid：用于控制小车的速度（如果速度偏快，可以把h_pid的p参数调得再小一点）

#导入模板
template = image.Image("/template.pgm")

# 定义一个函数去找到视野中最大的块：视野中肯定不止一个小球，我们让小车去追踪最近（最近的在摄像头里就最大）的一个小球
# 原理和追小球的小车一致，不需要更改
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # 从sensor中截取一段图像 

#修改：    blobs = img.find_blobs([green_threshold])       #调用颜色识别的函数
    r = img.find_template(template, 0.70, step=4, search=SEARCH_EX) # 返回值是一个边界框(x,y,w,h)
    if r:#如果小球找到ApriTags
        max_r = find_max(r)      #调用find_max()函数找到视野中最大的Apriltag去追踪
        #pid的差值，后面用于计算pid的参数
#修改：        x_error = max_blob[5]-img.width()/2       
        x_error = max_r[0]+max_r[2]/2-img.width()/2 
        h_error = max_rect[2]*max_rect[3]-size_threshold
        print("x error: ", x_error)
        '''
        for b in blobs:
            # Draw a rect around the blob.
            img.draw_rectangle(b[0:4]) # rect
            img.draw_cross(b[5], b[6]) # cx, cy
        '''
        
        
        img.draw_rectangle(max_blob[0:4]) # 在找到的色块上画一个矩形
#修改：        img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
        img.draw_cross(int(max_r[0]+max_r[2]/2),int(max_r[1]+max_r[3]/2)) # 在色块的中心cx，cy画一个十字
        
        #调用了pid.py中的函数，用于得到最终的结果，控制小车的运动
        x_output=x_pid.get_pid(x_error,1)
        h_output=h_pid.get_pid(h_error,1)
        print("h_output",h_output)
        car.run(-h_output-x_output,-h_output+x_output)      #两个轮胎的h_pid应该是一样的（距离），x_pid应该是相反的
   
    else:#没有找到矩形
        car.run(18,-18) #左右电机向相反的方向慢速运动，让小车在原地以非常慢的速度旋转，可以让小车在原地搜寻小球