循迹识别小车：（四）OpenMV4部分

要完成功能

主要完成循迹部分图像数据采集：
1、寻线：OpenMV4摄像头将获得的轨迹进行处理，得到轨迹的一个偏离角度，然后通过偏转角判断给STM32发送左转或右转的信号。
2、停止：MV4通过模板匹配和识别圆双重判断是否目标为圆，如果是则给STM32发出信号来停止小车运动并开始树莓派识别。

1、寻线部分：

通过获得摄像头中线条进行几个块区域的数学运算，计算出轨迹偏离角度。
例程：

# 机器人巡线例程
#
# 跟随机器人做一条线需要很多努力。 本示例脚本显示了如何执行跟随机器人的
# 机器视觉部分。 您可以使用此脚本的输出来驱动差分驱动机器人遵循一条线。
# 这个脚本只是产生一个转动的值，告诉你的机器人向左或向右。
#
# 为了使这个脚本正常工作，你应该把摄像机指向45度左右的一条线。请确保只有线在相机的视野内。
import sensor, image, time, math

# 跟踪一条黑线。使用[(128,255)]来跟踪白线。
GRAYSCALE_THRESHOLD = [(0, 64)]
#设置阈值，如果是黑线，GRAYSCALE_THRESHOLD = [(0, 64)]；
#如果是白线，GRAYSCALE_THRESHOLD = [(128，255)]


# 每个roi为(x, y, w, h)，线检测算法将尝试找到每个roi中最大的blob的质心。
# 然后用不同的权重对质心的x位置求平均值，其中最大的权重分配给靠近图像底部的roi，
# 较小的权重分配给下一个roi，以此类推。
ROIS = [ # [ROI, weight]
        (0, 100, 160, 20, 0.7), # 你需要为你的应用程序调整权重
        (0, 050, 160, 20, 0.3), # 取决于你的机器人是如何设置的。
        (0, 000, 160, 20, 0.1)
       ]
#roi代表三个取样区域，（x,y,w,h,weight）,代表左上顶点（x,y）宽高分别为w和h的矩形，
#weight为当前矩形的权值。注意本例程采用的QQVGA图像大小为160x120，roi即把图像横分成三个矩形。
#三个矩形的阈值要根据实际情况进行调整，离机器人视野最近的矩形权值要最大，
#如上图的最下方的矩形，即(0, 100, 160, 20, 0.7)

# Compute the weight divisor (we're computing this so you don't have to make weights add to 1).
weight_sum = 0 #权值和初始化
for r in ROIS: weight_sum += r[4] # r[4] is the roi weight.
#计算权值和。遍历上面的三个矩形，r[4]即每个矩形的权值。

sensor.reset() # 初始化sensor

sensor.set_pixformat(sensor.GRAYSCALE) # use grayscale.
#设置图像色彩格式，有RGB565色彩图和GRAYSCALE灰度图两种

sensor.set_framesize(sensor.QQVGA) # 使用QQVGA的速度。
#设置图像像素大小

sensor.skip_frames(30) # 让新的设置生效。
sensor.set_auto_gain(False) # 颜色跟踪必须关闭自动增益
sensor.set_auto_whitebal(False) # 颜色跟踪必须关闭白平衡
clock = time.clock() # 跟踪FPS帧率

while(True):
    clock.tick() # 追踪两个snapshots()之间经过的毫秒数.
    img = sensor.snapshot() # 拍一张照片并返回图像。

    centroid_sum = 0
    #利用颜色识别分别寻找三个矩形区域内的线段
    for r in ROIS:
        blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True) 
        # r[0:4] is roi tuple.
        #找到视野中的线,merge=true,将找到的图像区域合并成一个

        #目标区域找到直线
        if blobs:
            # 查找像素最多的blob的索引。
            largest_blob = 0
            for i in range(len(blobs)):
            #目标区域找到的颜色块（线段块）可能不止一个，找到最大的一个，作为本区域内的目标直线
                if blobs[i].pixels() > most_pixels:
                    most_pixels = blobs[i].pixels()
                    #merged_blobs[i][4]是这个颜色块的像素总数，如果此颜色块像素总数大于                     
                    #most_pixels，则把本区域作为像素总数最大的颜色块。更新most_pixels和largest_blob
                    largest_blob = i

            # 在色块周围画一个矩形。
            img.draw_rectangle(blobs[largest_blob].rect())
            # 将此区域的像素数最大的颜色块画矩形和十字形标记出来
            img.draw_cross(blobs[largest_blob].cx(),
                           blobs[largest_blob].cy())

            centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight.
            #计算centroid_sum，centroid_sum等于每个区域的最大颜色块的中心点的x坐标值乘本区域的权值

    center_pos = (centroid_sum / weight_sum) # Determine center of line.
    #中间公式

    # 将center_pos转换为一个偏角。我们用的是非线性运算，所以越偏离直线，响应越强。
    # 非线性操作很适合用于这样的算法的输出，以引起响应“触发器”。
    deflection_angle = 0
    #机器人应该转的角度

    # 80是X的一半，60是Y的一半。
    # 下面的等式只是计算三角形的角度，其中三角形的另一边是中心位置与中心的偏差，相邻边是Y的一半。
    # 这样会将角度输出限制在-45至45度左右。（不完全是-45至45度）。

    deflection_angle = -math.atan((center_pos-80)/60)
    #角度计算.80 60 分别为图像宽和高的一半，图像大小为QQVGA 160x120.    
    #注意计算得到的是弧度值

    deflection_angle = math.degrees(deflection_angle)
    #将计算结果的弧度值转化为角度值

    # 现在你有一个角度来告诉你该如何转动机器人。
    # 通过该角度可以合并最靠近机器人的部分直线和远离机器人的部分直线，以实现更好的预测。

    print("Turn Angle: %f" % deflection_angle)
    #将结果打印在terminal中

        print(clock.fps())              
    # 注意: 当连接电脑后，OpenMV会变成一半的速度。当不连接电脑，帧率会增加。
    # 打印当前的帧率。

2、串口通信：

import time
from pyb import UART

uart = UART(3, 19200)

while(True):
    uart.write("Hello World!\r")
    time.sleep_ms(1000)

先实例化一个19200波特率的串口3,然后调用write方法就可以了。
注意：必须是串口3，因为OpenMV2只引出了这个串口，pyb的串口有好多个的。OpenMV3又增加了串口1。

3、停止部分：

模板匹配：https://blog.csdn.net/weixin_48267104/article/details/109139208

颜色识别：https://blog.csdn.net/weixin_48267104/article/details/109020200

程序：

#-------（1）识别圆形部分--------------

        for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,r_min = 2, r_max = 100, r_step = 2): # 如果发现圆
            img.draw_circle(c.x(), c.y(), c.r(), color = (0, 255, 0)) # 框出圆
            Discern_look_circle = 1
            # print(c) # 打印出{"x":.., "y":.., "r":.., "magnitude":..} magnitude是检测圆的强度。越高越好
        # print("FPS %f" % clock.fps())

#-------（2）模板匹配部分--------------

        for t in templates1:  #如果与模板匹配
            template = image.Image(t) #template获取图片
            r = img.find_template(template, 0.70, step=4, search=SEARCH_EX) #进行相关设置,可以设置roi缩小区域
            if r: #如果有目标
                img.draw_rectangle(r) #画矩形，框出匹配的目标
                Discern_template_circle = 1
                # print("圆形")

        Discern = Discern_look_circle + Discern_template_circle

循迹总程序：

#
# 机器人巡线
#

import sensor, image, time, math, lcd
from pyb import UART
from image import SEARCH_EX, SEARCH_DS
#从imgae模块引入SEARCH_EX和SEARCH_DS。使用from import仅仅引入SEARCH_EX,
#SEARCH_DS两个需要的部分，而不把image模块全部引入。

# OpenMV上P4，P5对应的串口3
# 第二个参数是波特率。用来更精细的控制
uart = UART(3, 9600, timeout_char=1000)


EXPOSURE = 33333 #设置曝光,需要更改
GRAYSCALE_THRESHOLD = [(0, 64)] # 跟踪黑线。 如果是白线[(128，255)]
S_PIXEL = sensor.QQVGA #设置图像像素模式:QQVGA/LCD/VGA

# 每个roi为(x, y, w, h)，线检测算法将尝试找到每个roi中最大的blob的质心。
# 然后用不同的权重对质心的x位置求平均值，其中最大的权重分配给靠近图像底部的roi，
# 较小的权重分配给下一个roi，以此类推。
ROIS_QQVGA = [ # [ROI, weight]  # QQVGA分辨率: 160x120
        (0, 000, 160, 10, 0.1),
        (0, 025, 160, 10, 0.3),
        (0, 055, 160, 10, 0.5),
        (0, 085, 160, 10, 0.7),
        (0, 110, 160, 10, 0.9)
       ]
ROIS_LCD = [ # [ROI, weight]  # LCD分辨率128x160
        (0, 000, 128, 10, 0.1),
        (0, 035, 128, 10, 0.3),
        (0, 075, 128, 10, 0.5),
        (0, 115, 128, 10, 0.7),
        (0, 150, 128, 10, 0.9)
       ]
ROIS_VGA = [ # [ROI, weight]  # VGA分辨率640x480
        (0, 000, 640, 40, 0.1),
        (0, 110, 640, 40, 0.3),
        (0, 230, 640, 40, 0.5),
        (0, 350, 640, 40, 0.7),
        (0, 460, 640, 40, 0.9)
       ]

if S_PIXEL == sensor.LCD:
    ROIS = ROIS_LCD
elif S_PIXEL == sensor.QQVGA:
    ROIS = ROIS_QQVGA
elif S_PIXEL == sensor.VGA:
    ROIS = ROIS_VGA


weight_sum = 0 # 权值和的初始化


for r in ROIS: weight_sum += r[4] # 计算权值和。遍历上面的三个矩形，r[4]即每个矩形的weight值。


sensor.reset() #初始化摄像头
sensor.set_contrast(1)
sensor.set_gainceiling(16)
sensor.set_pixformat(sensor.GRAYSCALE) #尽量用灰度方式 # RGB565色彩图和GRAYSCALE灰度图
sensor.set_framesize(S_PIXEL) #设置图像像素模式  # 尽量使用QQVGA
sensor.skip_frames(time = 2000) # 设置一定时间让新的设置生效。在更改设置后，跳过一些帧，等待感光元件变稳定。
sensor.set_auto_gain(False) # 颜色跟踪必须关闭自动增益
sensor.set_auto_whitebal(False) #关闭白平衡。
sensor.set_auto_exposure(False,EXPOSURE) #设置曝光,需要更改
#sensor.set_windowing(((640-80)//2, (480-60)//2, 80, 60)) #子分辨率。可设置windowing窗口来减少搜索图片
lcd.init() # 初始化lcd屏幕。

# 模板照片
# templates1 = ["/100.pgm","/101.pgm","/102.pgm","/103.pgm","/110.pgm","/111.pgm","/112.pgm","/113.pgm","/120.pgm","/121.pgm","/122.pgm","/123.pgm"] #保存正三角形多个模板
# templates2 = ["/200.pgm","/201.pgm","/202.pgm","/203.pgm","/210.pgm","/211.pgm","/212.pgm","/213.pgm","/220.pgm","/221.pgm","/222.pgm","/223.pgm"] #保存倒三角形多个模板
templates1 = ["/003.pgm"] #保存正三角形多个模板
templates2 = ["/003.pgm"] #保存倒三角形多个模板



clock = time.clock() # 跟踪FPS帧率
flag = 1

while(flag):
        clock.tick() # 追踪两个snapshots()之间经过的毫秒数.
        # img = sensor.snapshot() # 拍一张照片并返回图像。
        img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)  # 畸变校正

#----------------------------------------检查起点终点-----------------------------------------------

        #初始化计数常量
        Discern = 0  # Discern = Discern_look_circle + Discern_template_circle
        Discern_look_circle = 0  # 识别圆常数
        Discern_template_circle = 0  # 模板匹配常数

#-------（1）识别圆形部分--------------

        for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,r_min = 2, r_max = 100, r_step = 2): # 如果发现圆
            img.draw_circle(c.x(), c.y(), c.r(), color = (0, 255, 0)) # 框出圆
            Discern_look_circle = 1
            # print(c) # 打印出{"x":.., "y":.., "r":.., "magnitude":..} magnitude是检测圆的强度。越高越好
        # print("FPS %f" % clock.fps())

#-------（2）模板匹配部分--------------

        for t in templates1:  #如果与模板匹配
            template = image.Image(t) #template获取图片
            r = img.find_template(template, 0.70, step=4, search=SEARCH_EX) #进行相关设置,可以设置roi缩小区域
            if r: #如果有目标
                img.draw_rectangle(r) #画矩形，框出匹配的目标
                Discern_template_circle = 1
                # print("圆形")

        Discern = Discern_look_circle + Discern_template_circle

        # print("识别圆形部分:%d" % Discern_look_circle)
        # print("模板匹配部分:%d" % Discern_template_circle)
        # print("识别起点终点结果:%d" % Discern)
#---------------------------------------------------------------------------------------------------
        if Discern == 2:
            time.sleep(10)#必要延时
            if uart.any():  # 串口接收到数据
                a = uart.readline().decode().strip()
                if a == 'MVwirt':
                     print("stopGr")
                     uart.write('stopGr\r\n')  #给stm32发数据,停止，开始抓取
                     time.sleep(10) #必要延时
                     flag = 0  # 退出循环

        else:
#---------------------------------判断偏转角度------------------------------------------------------
            centroid_sum = 0
            #利用颜色识别分别寻找三个矩形区域内的线段
            for r in ROIS:
                blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True) # r[0:4] is roi tuple.
                #找到视野中的线,merge=true,将找到的图像区域合并成一个

                if blobs:
                    # Find the blob with the most pixels.
                    largest_blob = max(blobs, key=lambda b: b.pixels()) # 返回最大值像素点


                    # 在色块周围画一个矩形。
                    img.draw_rectangle(largest_blob.rect())
                    # 将此区域的像素数最大的颜色块画矩形和十字形标记出来
                    img.draw_cross(largest_blob.cx(),
                                   largest_blob.cy())

                    centroid_sum += largest_blob.cx() * r[4] # r[4] is the roi weight.
                    #计算centroid_sum，centroid_sum等于每个区域的最大颜色块的中心点的x坐标值乘本区域的权值

            lcd.display(img) #lcd显示图像
            center_pos = (centroid_sum / weight_sum)
            #中间公式  确定线心

            # 将center_pos转换为一个偏角。我们用的是非线性运算，所以越偏离直线，响应越强。
            # 非线性操作很适合用于这样的算法的输出，以引起响应“触发器”。

            deflection_angle = 0
            #机器人应该转的角度

            # 80是X的一半，60是Y的一半。
            # 下面的等式只是计算三角形的角度，其中三角形的另一边是中心位置与中心的偏差，相邻边是Y的一半。
            # 这样会将角度输出限制在-45至45度左右。（不完全是-45至45度）。

            n = 0
            n_qqvga = (center_pos-80)/60  # QQVGA 160x120.
            n_lcd = (center_pos-64)/80  # LCD  128x160
            n_vga = (center_pos-320)/240  # VGA  640x480

            if S_PIXEL == sensor.LCD:
                n = n_lcd
            elif S_PIXEL == sensor.QQVGA:
                n = n_qqvga
            elif S_PIXEL == sensor.VGA:
                n = n_vga

            deflection_angle = -math.atan(n)
            # 角度计算.80 60 分别为图像宽和高的一半，图像大小为QQVGA 160x120.
            #注意计算得到的是弧度值

            deflection_angle = math.degrees(deflection_angle)
            # 将计算结果的弧度值转化为角度值

            # 现在你有一个角度来告诉你该如何转动机器人。
            # 通过该角度可以合并最靠近机器人的部分直线和远离机器人的部分直线，以实现更好的预测。
            print("Turn Angle: %f" % deflection_angle)

#-----------------------------串口发数据------------------------------------------------------------
            time.sleep(10)#必要延时
            if uart.any():  # 串口接收到数据
                a = uart.readline().decode().strip()
                if a == 'MVwirt':
                    time.sleep(10)
                    #print("Turn Angle: %f" % deflection_angle)
                    #将结果打印在terminal中

                    k1 = 4
                    k2 = 28

                    if k2 >= deflection_angle > k1:
                        print("left01")
                        uart.write('left01\r\n')  #给stm32发数据,左转
                    elif -k2 <= deflection_angle < -k1:
                        print("right1")
                        uart.write('right1\r\n')  #给stm32发数据，右转

                    elif deflection_angle > k2:
                        print("left0S")
                        uart.write('left0S\r\n')  #给stm32发数据,停下来左转
                    elif deflection_angle < -k2:
                        print("rightS")
                        uart.write('rightS\r\n')  #给stm32发数据，停下来右转

资源链接：

参考链接：链接1(OpenMV4官网)、 链接2(OpenMV4函数库)、 链接3(OpenMV4环境下载)

总项目文件：下载链接

上一篇文章：循迹识别小车：（三）树莓派部分

下一篇文章：循迹识别小车：（五）电机驱动和OLED屏