电赛OpenMV巡线巡迹

一段小废话：我是22年参加的电赛，当时只有省赛，巡迹我们也是用的openmv，当时也没用过，就是在官网现学，也不难，但是里面不会教你具体怎么巡迹，只是一些基础使用的语法和算法，我也在网上查了很多，CSND上要么查出来一堆废话，或者要么源码要收钱，或者代码可读性很差，几乎用处不大。最后还是在b站上看到一位老哥做送药小车的比赛，从他的视频得到一些思路。然后自己根据具体的任务，把算法结合起来，完成任务。对于想要快速上手的人来说，思路是最重要的，给一堆函数有什么作用，不会组合使用就完全没有作用，有了思路之后再去查某个任务要用什么函数才是最正确的。

我们当时的任务是要完成邻库有车和邻库无车的倒车入库。规定是要倒入第二个库。

因此我的思路就是从起点出发，找到第三个直角的位置停下。后面侧方停车的原理同样。

下面直接上代码讲解：每一步的实现效果图当时没有保存，因为我本来不喜欢写东西，就没记录，但是看到这部分内容确实不太行，所以就想着写一下，让大家能够快速上手思路

 1：读取图片并预处理（建议使用灰度图，RGB的干扰比较多）

sensor.set_pixformat(sensor.GRAYSCALE)#转为灰度图
sensor.set_framesize(sensor.QVGA)#分辨率
sensor.skip_frames(time = 2000)#初始给2秒的停顿让相机稳定
sensor.set_auto_gain(False)#灰度图巡迹的时候建议关闭自动增益调节功能，可能会影响要二值图的完整性。
sensor.set_auto_whitebal(False)#同样也不建议开启，白平衡

第二行用来设置分辨率的，分辨率太高的话，运行速度会很慢，而且会出来卡顿的情况，不建议用，因为使用QQQVGA、QQVGA就差不多了，不需要特别清晰的图像。这个分辨率改了的话，图像画布大小就变了，后面设置roi区域就需要重新绘制。

2：通信。设置好和主板一样的波特率之后，通过openmv的tx给板子的rx发送信息，使用openmv的rx接收板子tx发送来的信息。

uart = UART(3,115200)#通信的波特率设置，与主板需要设置成一样才能进行通信

这个部分我们当时没用，因为板子当时的通信有一个脚坏了，信息只有一方能接收到，当时因为时间比较紧，我们也就没去继续排查。如果想使用rx和tx通信的可以去别的博主那边参考一下。我是使用的下面这个方法。

#开启管脚
ain1 =  Pin('P0', Pin.OUT_PP)
ain2 =  Pin('P1', Pin.OUT_PP)
ain3 =  Pin('P2', Pin.OUT_PP)
ain4 =  Pin('P3', Pin.OUT_PP)

使用openmv上的管脚输出高低电平，来给主板发送指令。用二进制的组合方式，可以发送16种，对于只需要发送离散信息的来说肯定够用了。比如，只发：左转、右转等，如果是想要发送具体的偏转角之类的，就需要使用异步uart进行通信。

我们当时是发送停车，左转，右转，左急转等

3：巡迹

roi1 = [(1, 34, 42, 207),#0

(283,36,37,207),#1

(145,0,37,35),#2

(108,0,26,34),#3

(196,1,27,34),#4

(147,72,38,23),#5

(68,64,46,31),#6

(216,63,32,32),#7

(82,201,162,39),#8

(72,126,37,57),#9

(208,130,39,51),#10

(0,0,320,240),#11

(0,79,320,162)]#12

我使用很多的roi框，来区分当前看到图像属于什么情况。

直接用鼠标在摄像头显示页面上框选区域，下面就会出现roi的4个坐标，复制过来即可。

图像二值化    img = sensor.snapshot().binary([THRESHOLD])

 这个图可能会有点乱，因为都是黑白线。

解释一下：不想看的直接可以跳到后面，有清晰的讲解

多个 roi 框的组合应用，生成直角，边线，平行，距离过近，距 离过远，距离正好，无效等 例如：

当框 0，1，2，5 全为 True 同时 3 和 4 为 False 时，识别为直 角，统计直角数量，当记到 3 时，给 MSP432 发送倒车指令，同时将 直角数量置零。当完成倒车入库后，出库之后进入侧方停车的过程中， 会识别倒车入库赛道剩下的直角，造成干扰，两种解决方案：①令小 车在驶出倒车入库赛道后，MSP432 给 openmv 发送将直角数置零。② 下一次统计直角数量变为 5 时，开始侧方停车。

当框 2，3，4 全为 True 时，识别为箭头干扰或者车辆干扰，不 识别成直角

当框 6 和 7 为 True 时，识别为距离过远，需要右转

当框 9 和 10 为 True 时，识别为距离正好 当框 8 为 True 时，识别为距离过近，需要左转

在框 12 内进行霍夫变换 get_regression([(255,255)]，阈值为 255，全白，返回 r=xcosθ+ysinθ, 计算 rho_err = abs(line.rho())-img.width()/2，

if line.theta()>90:

        theta_err = line.theta()-180

else:

        theta_err = line.theta()

因此根据 theat_err 的正负值来进行判断左右转向。 当 theat_err 过于倾斜时，会触发左急转弯，防止触线。

我没保留其他的图，只有这个图，下面我用其他的颜色来区分一下：

当上面3个绿框都存在白色的情况下，就代表是直角，当然我们摄像头的位置，不会是这样的，角度会架的比较低。像下面这样，我用画笔画的一个示意图 。因此如果想要识别直角就可以用这三个框。要完成别的识别的话就再加框。

代码示例如下，其他框的判断同理：第一个参数【（255，95）】这个是二值图的阈值，这个是事先试出来的，试出来哪个阈值范围是白色。如果赛道识别的颜色，也一样同理。可以使用阈值选择器来选择阈值。

if img.find_blobs([(255, 95)],roi=roi1[0],area_threshold=100):
    left_flag=1
if img.find_blobs([(255, 95)],roi=roi1[1],area_threshold=100):
    right_flag=1

选择阈值选择器，进去之后自然就知道怎么用

直角示意图

 到了侧方停车的时候，因为有一个横着的箭头，也能满足这个情况。所以这个情况会有bug

因此我才把上面那个框，换成了3个，只有中间框有白色，并且下面那个红框也有白色，旁边两个框没有的情况，才是直角 。当统计到3个直角的时候，发送信息。其实是一直都在发送信息，只不过当在第3个直角时，发送停车信息。

然后为了保持和边线在一定的距离。我又增加了两组的框。

 如果当白线在框1和框2的位置的话，说明距离比较远了，需要向靠近边线的地方靠近。如果在框3和框4的位置，说明距离比较近了，需要远离。如果在框5内了，说明距离已经特别近了。需要急转逃离，不然正常角度的转弯可能还是会压线，当然这种情况是比较少见的，只要说预防，万一出现，还有补救的可能，不至于直接g了，因为在场上比赛的时候，电机是最大的不确定因素，很有可能给一样的pwm波，但是转的速度会慢很多。注:图中的框的位置需要根据直接摄像头在车上的位置进行调节。自己到时候连上电脑，在场地试一下就知道。

接下来就是偏转角的问题。

line = img.get_regression([(255,255)],roi=roi1[12])

 在框12内做像素值的线性计算，使用get_regression函数即可，第一个参数表示像素范围，我们只需要计算255即白色区域，第二个参数指定roi区域。

得到的y=wx+b的形式。

得到line里面包括line.rho()和line.theta()，可以理解成w和b，但是b是0~180的，所以我当时判断如果b大于90的话，减去180，变成负的，或者直接判断是小于90还是大于90，就可以判断是往左偏，还是往右偏。

之后的侧方也是一样的道理

然后是邻库有车和无车的区别

两种解决方案：（1）：把摄像头的角度架第一点，就看不到车了，那样干扰也会变少，甚至箭头的干扰也没有了，邻库有车和无车就用同样的算法。

（2）把有车的情况下，因为看到的是车头，把它当成和侧方停车那个横着的箭头一样的情况排除就行了。同样还是检测第三个直角。

下面是完整代码：不知道为什么openmv的注释会自己删了。因为最近忙的很，就不写注释啦，代码也比较简单，既然都知道我的思路了，没必要硬看代码，不同题目代码也不一样。注：当时是拿了省一，所以这个代码思路还是有用的。大伙加油吧，好好备赛，电赛没多难。还有目标识别部分没写，等下回什么时候比较空的时候再写。

import sensor, image, time,ustruct
from pyb import Pin, Timer
from pyb import UART,LED
sensor.reset()
#sensor.set_pixformat(sensor.RGB565)
sensor.set_pixformat(sensor.GRAYSCALE)#转为灰度图
sensor.set_framesize(sensor.QVGA)#分辨率
sensor.skip_frames(time = 2000)#初始给2秒的停顿让相机稳定
sensor.set_auto_gain(False)#灰度图巡迹的时候建议关闭自动增益调节功能，可能会影响要二值图的完整性。
sensor.set_auto_whitebal(False)#同样也不建议开启，白平衡
uart = UART(3,115200)#通信的波特率设置，与主板需要设置成一样才能进行通信
LED(1).on()
LED(2).on()
LED(3).on()
#开启管脚
ain1 =  Pin('P0', Pin.OUT_PP)
ain2 =  Pin('P1', Pin.OUT_PP)
ain3 =  Pin('P2', Pin.OUT_PP)
ain4 =  Pin('P3', Pin.OUT_PP)
ain1.high()
clock = time.clock()
roi1 =	 [(1, 34, 42, 207),
            (283,36,37,207),
            (145,0,37,35),
            (108,0,26,34),
            (196,1,27,34),
            (147,72,38,23),
            (68,64,46,31),
            (216,63,32,32),
            (82,201,162,39),
            (72,126,37,57),
            (208,130,39,51),
            (0,0,320,240),
            (0,79,320,162),
            (69,1,171,48),#小车
            (2,1,148,47)]##小车和边线

THRESHOLD = (0, 95)
r_num=0
car=0
car_1=0
flag_r=0
flag_c=0
#发送信息
def outuart(w,b,flag1,flag2):
    ain2.low()
    ain3.low()
    ain4.low()
    global uart;
    f_w=0
    f_b=0
    if flag2==0:
        if flag1==1:
            f_w=1
        else:
            f_w=0
        if flag1==1:
            f_b=1
        else:
            f_b=0
    if flag1==2 and flag2==1:
        w,b,f_w,f_b=(0,0,0,0)
    if flag2==2:
        print('准备倒车')
        ain4.high()
        time.sleep_ms(200)
    else:
        if  b>0 and f_w==0 and f_b==0 and flag1!=3:
            print("不平行左转")
            ain3.high()
        if  b<0 and f_w==0 and f_b==0:
            print('不平行右转')
            ain3.high()
            ain4.high()
        if  b>0 and f_w==1 and f_b==1:
            print('直角且不平行左转')
            ain3.high()
        if  b<0 and f_w==1 and f_b==1:
            print('直角且不平行右转')
            ain3.high()
            ain4.high()
        if w==0 and b==0 and f_w==0 and f_b==0:
            print('平行不调整')
        if  w>33 and b>0 and f_w==0 and f_b==0 and flag1==3:
            print("距离过近-左转急")
            ain2.high()
            ain3.high()
while(True):
    clock.tick()
    img = sensor.snapshot().binary([THRESHOLD])
    #像素值线性计算，计算偏转角,y=wx+b
    line = img.get_regression([(255,255)],roi=roi1[12])

    if (line):#如果存在白色区域
        #w
        rho_err = abs(line.rho())-img.width()/2
        if line.theta()>90:
            #b
            theta_err = line.theta()-180
        else:
            theta_err = line.theta()
        img.draw_line(line.line(), color = 127)
        if line.magnitude():
            outdata=[rho_err,theta_err,0]
            print(outdata)
            #标志位
            left_flag,right_flag,up_flag,down_flag,left_up_flag,right_up_flag=0,0,0,0,0,0
            up_down_flag,up_down_left_flag,up_down_right_flag,bian_left,bian_right=0,0,0,0,0
            car_flag=0
            #绘制前面的rio框，方便查看
            for rec in roi1:
                img.draw_rectangle(rec, color=(255,0,0))
            #根据框内是否有白色区域来判断当前图像是什么情况
            if img.find_blobs([(255, 95)],roi=roi1[0],area_threshold=100):
                left_flag=1
            if img.find_blobs([(255, 95)],roi=roi1[1],area_threshold=100):
                right_flag=1
            if img.find_blobs([(255, 95)],roi=roi1[2],area_threshold=100):
                print('up')
                up_flag=1
            if img.find_blobs([(255, 95)],roi=roi1[3],area_threshold=100):
                print('left_up')
                left_up_flag=1
            if img.find_blobs([(255, 95)],roi=roi1[4],area_threshold=100):
                print('right_up')
                right_up_flag=1
            if img.find_blobs([(255, 95)],roi=roi1[5],area_threshold=100):
                print('up_down')
                up_down_flag=1
            if img.find_blobs([(255, 95)],roi=roi1[6],area_threshold=100):
                print('up_down_left')
                up_down_left_flag=1
            if img.find_blobs([(255, 95)],roi=roi1[7],area_threshold=100):
                print('up_down_right')
                up_down_right_flag=1
            if img.find_blobs([(255, 95)],roi=roi1[8],area_threshold=100):
                down_flag=1
            if (left_flag==1 and right_flag==1 and up_flag==1 and left_up_flag==0 and right_up_flag==0 and up_down_flag==1) or (left_flag==1 and up_flag==1 and left_up_flag==0 and right_up_flag==0 and up_down_flag==1) or (right_flag==1 and up_flag==1 and left_up_flag==0 and right_up_flag==0 and up_down_flag==1) :
                print('直角')
                if flag_r==0 and flag_c==1:
                    flag_r=1
                    flag_c=0
                    r_num+=1
                outuart(outdata[0],outdata[1],1,0)
            if up_flag==1 and left_up_flag==1 and right_up_flag==1 and up_down_flag==0:
                print('箭头')
            if img.find_blobs([(255, 95)],roi=roi1[13],area_threshold=3500):
                car_flag=1
            if left_flag==1 and right_flag==1 and up_down_flag==0 and down_flag==0:
                print('边线')
                if car_flag==0:
                    flag_c=1
                    flag_r=0
                if img.find_blobs([(255, 95)],roi=roi1[12]) and (-88>=outdata[1]>=-90 or 88<=outdata[1]<=90):
                    print('平行')
                    if img.find_blobs([(255, 95)],roi=roi1[9],area_threshold=100) :
                        bian_left=1
                    if img.find_blobs([(255, 95)],roi=roi1[10],area_threshold=100) :
                        bian_right=1
                    if bian_right==1 and bian_left==1:
                        outuart(0,0,2,1)
                        print('距离正好')
                    if up_down_left_flag==1 and up_down_right_flag==1:
                        print('距离过远')
                        outuart(outdata[0],outdata[1],2,0)
                        outuart(0,-10,2,0)
                else:
                    outuart(outdata[0],outdata[1],2,0)
            if down_flag==1:
                print('距离过近')
                outuart(outdata[0],10,3,0)

            if img.find_blobs([(255, 95)],roi=roi1[13],area_threshold=3500):
                if (flag_r==0 and flag_c==1) or (flag_r==1 and flag_c==0):
                    car+=1
                    flag_c=0
                    flag_r=0

            #if car==1 and img.find_blobs([(255, 95)],roi=roi1[14],area_threshold=3000) :
                #car_1=1
            if car==2:
                outuart(0,0,2,2)
                r_num=0
                car=0
                car_1=0
    else:
        print('不符合')
    #print(r_num)
    print(car)
    #print(car_1)
    if r_num==3:
        outuart(0,0,2,2)
        r_num=0