2023年全国电赛E题矩形巡线方案（无PID）

前言

2023年全国电赛已经过去半个多月，我们队伍选的是E题，最后评审获得了省一的成绩。博主是21年入学的，大学四年也就这一次机会打国赛了，因此想写篇博客记录一下。
我们队的配置是OpenMV+步进电机。博主在队伍中主要是负责代码部分。但巡线部分也多亏了队长师兄提供的宝贵建议才能完成。

检测矩形

题目要求激光在沿四周贴黑色电工胶带的A4靶纸上沿胶带绕一圈，因此首先需要在屏幕上检测到矩形。这里使用OpenMV内置的矩形检测函数find_rects()即可。根据OpenMV官方文档说明，该函数可以识别任意大小、角度的矩形，并返回一个rect对象的列表。
rect.corners()返回一个有四个元组的列表，每个元组代表矩形的四个顶点（x, y），从左上角的顶点开始，按照顺时针排序。因为巡线过程中需要确定矩形四个角点的坐标所以需要用到这个函数。
除此之外，题目中需要检测的矩形是有特定长宽比的，我们可以利用这一特征进行进一步筛选，降低与视野中检测到的其他矩形混淆的概率。A4纸的长宽比一般为$\sqrt{2}:1$，因此可以在代码中设置这个判断条件。
一开始打算用rect.rect()这个函数计算检测到的矩形的长宽，但发现该函数返回的长宽是外接矩形的长宽，也就是说只有在正放A4靶纸的时候该数值才会和实际长宽相同。因此只能通过手动检测角点间的距离来计算长宽比。
检测A4靶纸边线的代码如下：

import sensor, image, time

sensor.reset()
sensor.set_pixformat(sensor.RGB565) # 灰度更快(160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()

def get_distance(x1,y1,x2,y2):
    distance = math.sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))
    return distance

while(True):
    clock.tick()
    img = sensor.snapshot()

    # 下面的`threshold`应设置为足够高的值，以滤除在图像中检测到的具有
    # 低边缘幅度的噪声矩形。最适用与背景形成鲜明对比的矩形。

    for r in img.find_rects(threshold = 10000):
        img.draw_rectangle(r.rect(), color = (255, 0, 0))
        for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))
        (x1,y1),(x2,y2),(x3,y3),(x4,y4)=r.corners()
         width = get_distance(x1,y1,x2,y2)
         height = get_distance(x1,y1,x3,y3)
         #检测是否满足长宽比
         if  width > (1.2*height) and width < (1.6*height):
         	rectangle = r #检测到符合条件的矩形，返回rectangle对象

    print("FPS %f" % clock.fps())

巡线方案

检测到A4靶纸边线的位置就可以进行激光巡线了。最初在搭建云台时有两种方案，一种是只有激光笔固定在云台上，摄像头不随云台转动而是固定在一个位置；一种是激光笔和摄像头都固定在云台上。我们队采用的方案是将激光笔和摄像头固定，使得激光点移动的同时镜头中心也会跟着移动，这样就不需要在巡线过程中通过识别激光点来判定是否运动到目标位置。跟小车巡线不同的是，由于云台与屏幕距离固定，在视野范围内是始终能够看到一整个矩形的，而不是仅有黑色胶带。但又由于边线是一个矩形，因此我们可以让激光点”走矩形“而不是走线。
如何使激光点按矩形的路径运动呢？前面我们说到rect.corners()这个函数会返回矩形按顺时针的四个角点坐标，那么就可以使激光点沿着四个坐标直线运动，这样我们所看到的就是激光点在”走矩形“。但因为摄像头运动的关系，四个角点坐标会因为镜头移动而改变，所以需要设置每一帧都在检测矩形，并实时返回角点坐标。
假设返回的角点按顺时针依次为(x1,y1),(x2,y2),(x3,y3),(x4,y4),定义一个标志位，初始时控制当前运动逻辑为(x1,y1)运动到(x2,y2)，当检测到激光点已经运动到目标位置时，改变标志位，使得运动逻辑变为从(x2,y2)运动到(x3,y3)，依次改变标志位和运动逻辑，就能完成激光点沿矩形巡线的过程。
判定是否运动到目标位置的方法也很简单，就是在每一帧都计算镜头中心与目标点的距离，当距离减小到非常小的数值（代码这里设置为1）时就判定为运动到目标点。需要注意的是由于激光笔和镜头在云台上安装位置不同的关系，计算距离的时候需要加上一定的偏差，这个偏差视自己的云台搭建情况而定。

#go_flag为标志位，twopoint是自己写的两点间运动函数，offset是镜头与激光笔位置的偏差
if go_flag == 0:
    x_error = img.width()/2+offset_x - x2
    y_error = img.height()/2+offset_y - y2
elif go_flag == 1:
    twopoint(img.width()/2+offset_x,img.height()/2+offset_y,x2,y2,x1,y1)
elif go_flag == 2:
    twopoint(img.width()/2+offset_x-3,img.height()/2+offset_y,x1,y1,x4,y4)
elif go_flag == 3:
    twopoint(img.width()/2+offset_x,img.height()/2+offset_y-3,x4,y4,x3,y3)
elif go_flag == 4:
    twopoint(img.width()/2+offset_x,img.height()/2+offset_y,x3,y3,x2,y2)

if go_flag > 0:
    if abs(x_error) <= 1 and abs(y_error) <= 1 :
        cnt = cnt + 1
        if cnt == maxq :
            cnt = 0
            go_flag = go_flag + 1
            if go_flag == 5 :
                go_flag = 0

elif go_flag == 0:
    if abs(x_error) <= 1 and abs(y_error) <= 1 :
        go_flag = go_flag + 1

但这样还有一个问题，从当前点运动到下一个目标点的过程中，不一定会完全按直线轨迹走，这样就有可能跑出边线外导致扣分。队长给的建议是将当前要走的直线拆分成多段来走，这样激光点运动的轨迹就能近似为一条直线。因此我在主循环外写了两个函数来完成这个功能，具体的代码如下。

#拆分直线为多个目标点
def separate(x1,y1,x2,y2,quota):
    position_list = []
    x_distance = (x2-x1)/(quota)
    y_distance = (y2-y1)/(quota)
    for i in range(1,quota+1):
        (x,y) = (x1+x_distance*i,y1+y_distance*i)
        position_list.append((x,y))
    return position_list
    
#激光沿多个目标点走出近似直线的轨迹
def twopoint(target_x,target_y,x1,y1,x2,y2):
    global cnt
    global x_error,y_error
    position_list = separate(x1,y1,x2,y2,maxq)
    print("position_list:",position_list)
    if cnt < maxq:
        x_error = target_x - position_list[cnt][0]
        y_error = target_y - position_list[cnt][1]

主体程序写完到这觉得没问题，结果烧进OpenMV的时候云台一卡一卡的走没几步又退回去了。于是用print大法将go_flag打印出来看看是否是在正常运行，发现go_flag的值是有在改变的并且很快。想了一下觉得是可能是角点顺序有误导致的。怀疑rect.corners()这个函数返回的角点坐标不是固定顺序的，于是把四个角点按1234的顺序输出在图像上，发现居然真的是一直在变动的！！！找到问题所在就有办法解决了，我又重新写了个函数把在每次检测到矩形后重新把四个点整合成我们想要的顺序。

#找到矩形中心点
def get_center(x,y,w,h):
    center_x=x+w
    center_y=y+h
    return center_x,center_y
#将角点坐标顺序重新整合，按矩形中心的左上，右上，左下，右下排列
def trans_position(position_list,x,y,w,h):
    new_list=[]
    center_x,center_y = get_center(x,y,w,h)
    for x,y in position_list:
        if x <= center_x and y <= center_y:
            (x1,y1) = (x,y)
            new_list.append((x1,y1))
        elif x >= center_x and y <= center_y:
            (x2,y2) = (x,y)
            new_list.append((x2,y2))
        elif x <= center_X and y >= center_y:
            (x3,y3) = (x,y)
            new_list.append((x3,y3))
        elif x >= center_X and y >= center_y:
            (x4,y4) = (x,y)
            new_list.append((x4,y4))
    return new_list

把程序下载到OpenMV运行，OK，达到预期的巡线效果了。因为步进电机的精度足够高，过程也不需要用到PID。

完整代码

完整的矩形巡线代码如下：

import sensor, image, time
#from pyb import Servo
import time
from pid import PID

from time import sleep, sleep_us
from pyb import Pin
import math

go_flag=0
cnt=0

maxq=25

offset_x = 7
offset_y = -4
x_error = 0
y_error = 0

sensor.reset()
sensor.set_pixformat(sensor.RGB565) # grayscale is faster (160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()

xy_step = Pin('P2', Pin.OUT_PP)
# 方向
xy_dir = Pin('P3', Pin.OUT_PP)

xy_xianwei = Pin('P9', Pin.IN, Pin.PULL_UP)
# 脉冲
xz_step = Pin('P4', Pin.OUT_PP)
# 方向
xz_dir = Pin('P5', Pin.OUT_PP)

xz_xianwei = Pin('P0', Pin.IN, Pin.PULL_UP)
# 蜂鸣器
bibi = Pin('P6', Pin.OUT_PP)

# 参数1步数，参数2速度
def xy(s, v):
    #s=s*4
    if left_flag == 1:
        if s < 0:
            xy_dir.value(0)
        else:
            xy_dir.value(1)
    elif left_flag == 0:
        if s < 0:
            xy_dir.value(0)
        else:
            xy_dir.value(1)
    #s=abs(s)/68/3.14*200
    v=400-2*v

    for i in range(abs(s)):
        xy_step.value(1)
        sleep_us(v)
        xy_step.value(0)
        sleep_us(v)



# 参数1步数，参数2速度
def xz(s, v):
    #s=s*4
    if left_flag == 1:
        if s < 0:
            xz_dir.value(1)
        else:
            xz_dir.value(0)

        #s=abs(s)/68/3.14*200
        v=400-2*v

        for i in range(abs(s)):
            xz_step.value(1)
            sleep_us(v)
            xz_step.value(0)
            sleep_us(v)
    elif left_flag == 0 :
        if s < 0:
            xz_dir.value(0)
        else:
            xz_dir.value(1)

        #s=abs(s)/68/3.14*200
        v=400-2*v

        for i in range(abs(s)):
            xz_step.value(1)
            sleep_us(v)
            xz_step.value(0)
            sleep_us(v)
            
#拆分直线为多个目标点
def separate(x1,y1,x2,y2,quota):
    position_list = []
    x_distance = (x2-x1)/(quota)
    y_distance = (y2-y1)/(quota)
    for i in range(1,quota+1):
        (x,y) = (x1+x_distance*i,y1+y_distance*i)
        position_list.append((x,y))
    return position_list

#激光沿多个目标点走出近似直线的轨迹
def twopoint(target_x,target_y,x1,y1,x2,y2):
    global cnt
    global x_error,y_error
    position_list = separate(x1,y1,x2,y2,maxq)
    print("position_list:",position_list)
    if cnt < maxq:
        x_error = target_x - position_list[cnt][0]
        y_error = target_y - position_list[cnt][1]


#找到矩形中心点
def get_center(x,y,w,h):
    center_x=x+w
    center_y=y+h
    return center_x,center_y

#两点间距离
def get_distance(x1,y1,x2,y2):
    distance = math.sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))
    return distance

#将角点坐标顺序重新整合，按矩形中心的左上，右上，左下，右下排列
def trans_position(position_list,x,y,w,h):
    new_list=[]
    center_x,center_y = get_center(x,y,w,h)
    for x,y in position_list:
        if x <= center_x and y <= center_y:
            (x1,y1) = (x,y)
            new_list.append((x1,y1))
        elif x >= center_x and y <= center_y:
            (x2,y2) = (x,y)
            new_list.append((x2,y2))
        elif x <= center_X and y >= center_y:
            (x3,y3) = (x,y)
            new_list.append((x3,y3))
        elif x >= center_X and y >= center_y:
            (x4,y4) = (x,y)
            new_list.append((x4,y4))
    return new_list


text_list=["x1","x2","x3","x4"]


while(True):

    clock.tick()
    img = sensor.snapshot()


    # 下面的`threshold`应设置为足够高的值，以滤除在图像中检测到的具有
    # 低边缘幅度的噪声矩形。最适用与背景形成鲜明对比的矩形。

    #print("start to check rectangle")

    for r in img.find_rects(threshold = 10000):
         #img.draw_rectangle(r.rect(), color = (255, 0, 0))
         x,y,w,h = r.rect()
         (x1,y1),(x2,y2),(x3,y3),(x4,y4)=r.corners()
         width = get_distance(x1,y1,x2,y2)
         height = get_distance(x1,y1,x3,y3)
         if  width < (1.2*height):
               rectangle = r



    if (rectangle):
        img.draw_cross(80, 60,color=(0, 255, 128)) # cx, cy
        img.draw_rectangle((x_max,y_max,w_max,h_max), color = (255, 0, 0))
        for p in rectangle.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))
        #(x1,y1),(x2,y2),(x3,y3),(x4,y4)=rectangle.corners()
        (x1,y1),(x2,y2),(x3,y3),(x4,y4)=trans_position(rectangle.corners(),x_max,y_max,w_max,h_max)
        img.draw_string(x1,y1,text_list[0],scale=1,color=(255,0,0))
        img.draw_string(x2,y2,text_list[1],scale=1,color=(255,0,0))
        img.draw_string(x3,y3,text_list[2],scale=1,color=(255,0,0))
        img.draw_string(x4,y4,text_list[3],scale=1,color=(255,0,0))

        if go_flag == 0:
            x_error = img.width()/2+offset_x - x2
            y_error = img.height()/2+offset_y - y2
        elif go_flag == 1:
            twopoint(img.width()/2+offset_x,img.height()/2+offset_y,x2,y2,x1,y1)
        elif go_flag == 2:
            twopoint(img.width()/2+offset_x-3,img.height()/2+offset_y,x1,y1,x4,y4)
        elif go_flag == 3:
            twopoint(img.width()/2+offset_x,img.height()/2+offset_y-3,x4,y4,x3,y3)
        elif go_flag == 4:
            twopoint(img.width()/2+offset_x,img.height()/2+offset_y,x3,y3,x2,y2)

        print("go_flag:",go_flag)
        print("x_error:",x_error)
        print("y_error:",y_error)
        print(rectangle.corners())
        print(cnt)

        if x_error > 1:
            xz(1,0)
        elif x_error < -1 :
            xz(-1,0)

        if y_error > 1:
            xy(1,0)
        elif y_error < -1:
            xy(-1,0)

        if go_flag > 0:
            if abs(x_error) <= 1 and abs(y_error) <= 1 :
                cnt = cnt + 1
                if cnt == maxq :
                    cnt = 0
                    go_flag = go_flag + 1
                    if go_flag == 5 :
                        go_flag = 0
        
        elif go_flag == 0:
            if abs(x_error) <= 1 and abs(y_error) <= 1 :
                go_flag = go_flag + 1

一些总结

最后评审的时候，我们巡矩形边线这个part的计时是21秒，应该算是跑了一个还可以的速度？但比赛后回想还是有需要改进的地方。比如A4靶纸检测的算法不够好，导致评审时候因为场地有其他矩形物品的干扰差点没演示成功。更合理的方案是检测一个矩形边框，即有包含内外两个矩形的特征才设定为检测目标。
比赛那四天三夜真的蛮累的，没到作品完成之间都很煎熬，也庆幸我们队一开始选的方案算是比较好的（就是OpenMV太容易烧了，中间还烧了一块）。最后我们队拿到了参加综测的资格，因为没想过能进综测之前也没有专门练习过综测的题目，博主又是队里唯一一个电子的奈何模电基础并不好，所以综测的时候我们寄掉了。但对我来说，作为大学四年仅有机会参加一次的国电赛，这个成绩算是没有留下遗憾了！也真的十分感谢队友的坚持和付出！