2022年电子设计竞赛B题倒库闭环的思考

- - 具有自动泊车功能的电动车（B题）简介
  - 误差的获得
  - 如何获取初始偏置
  - 库内有箭头的搜索方法
  - - 箭头方位的获得
    - 车库边缘位置的确定

2022年我队友FKR带我打电赛。作为智能车队的一员，我们肯定要做和车有关的。
2022年电子设计竞赛B题部分要求如下：

具有自动泊车功能的电动车（B题）简介

设计制作具有自动泊车功能的电动车，可在图1所示的作品测试泊车场地上，分别独立完成“倒车入库/出库”或“侧方入库/出库”的单项操作，也可连续完成这两项入库/出库的操作。
在这里插入图片描述
（1）单项倒车入库/出库①：如图2所示，一键启动摆放在“发车区1”内的电动车，电动车以“右侧垂直泊车方式”自动倒车进入库2内居中位置停车（详见图2中库区abcd，此时库1、库3内均停有车辆），倒车入库时间越短越好（定义见本题说明，>30s的测试项成绩记0分）。电动车在库内停车到位5s后，沿车头方向右转出库，车身整体出库时间不超过15s。（20分）
在这里插入图片描述
这里就以倒库部分的闭环控制讲解为例。

在如图所示的小车上方加一根杆子，然后添加一个OpenMV摄像头角度略微朝下，能够看到倒库时视角如下图所示

需要注意的是，库内的箭头大小无所谓。我们最需要关注的点就是希望小车能够在库内沿着中轴线前进，也就是说，我们的目标是找到需要停车库的边线位置。

误差的获得

先考虑库内没有箭头的情况
首先，我们需要明确几个概念：当小车沿着库内中线走的时候，那么在后视镜看到的视角当中库边侧两条线应该是沿着自身滑动的，即在视野中是相对静止的。因此我们可以通过寻找跳变沿，即像素点从黑变白的位置处作为库边缘的位置。我们在屏幕中选择一条水平的基准线，那么如果在没有箭头，而且车身沿着库内平行于库边的直线倒退的时候，该基准线上的黑变白的跳变沿的位置应该是不变的。
在这里插入图片描述
以屏幕中心为起始点，向外侧扫描跳变沿，左侧的距离为 $r_{1}$ ，右侧的距离为 $r_{2}$ ，由于摄像头的位置可能不是非常正好的正中央，所以当车身沿着平行于库边缘的中心线后退的时候，应满足如下两个式子：
$\left\{\begin{matrix} r_{1} + r_{2} = W_{1}\\ r_{2} - r_{1} = b_{1} \end{matrix}\right.$
其中 $W_{1}$ 是给定扫描行下库边缘的宽度， $b_{1}$ 是车沿着平行于库边缘的中心线后退的时候由于摄像头初始位置不是完全的视野中心而造成的初始偏置。
如果车身并没有沿着中心线运行的话，我们就取误差如下：
$e=r_{2} - r_{1} - b_{1}$

很容易理解，该量具有极性。当车身偏向左侧时， $r_{1}$ 增大， $r_{2}$ 减小， $e$ 减小，甚至过零变负，反之亦然。将该量作为小车舵机 $P I D$ 控制的输入量就可以实现入库闭环自校正。

如何获取初始偏置

获取初始偏置的方法建议手动调试。即给舵机上电且机械调零后，舵机就可以稳定在机械零点的位置。这时手扶着小车在库中线前后移动，在不触碰边线的情况下如果 $r_{2} - r_{1}$ 在车移动的过程中保持恒定了，这证明小车走的路径确实是沿着平行于库边缘的直线运行。这时在视觉上车身就是在车库中线上运行时且 $r_{2} - r_{1}$ 仍保持恒定，记录 $r_{2} - r_{1} = b_{1}$ 作为初始偏置。

库内有箭头的搜索方法

箭头方位的获得

首先对OpenMV进行初始化：

import pyb
import sensor, image, time, math
from math import *
from machine import UART
from pyb import Pin

sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)      # we run out of memory if the resolution is much bigger...
sensor.set_brightness(2000)             # 设置图像亮度 越大越亮
sensor.skip_frames(time = 20)
sensor.set_auto_gain(False)             # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)         # must turn this off to prevent image washout...
clock = time.clock()

这里面的画幅大小模式为sensor.QQVGA，即160*120大小。
我们先定义几个变量：

DIRECTFLAG = 0                              # 扫线模式，0左1右
ROUTE_WID = 40                              # 赛道宽，按经验取
FIRSTFLAG = 0                               # 第一次运行循环标志位
LINE_SCAN = 75                              # 扫线位置
BAIS = 16                                   # 后倒库角度偏置
WID_THRES = 90                              # 判断左右探索大于多少判定为车库边界的阈值
CHECK_MODE = 0                              # 查线方式判断箭头偏向镜头哪一侧
JUMPGATE = 30                               # 探索跳变沿的跳变沿阈值

我们采用的扫描方式只是简单的单行扫描，如果只是静态的扫描我们无法获得箭头的真实位置。如下图所示
在这里插入图片描述
在这幅图中，扫描行上检测出了三个跳变点。但是单凭这一时刻的跳变沿位置我们是无法判断这三个跳变沿究竟哪个是箭头，哪个是车库边缘。由于箭头在车库中心，因此这三个跳变沿之间的距离应该十分接近。既有可能是左侧两个是车库边缘，中间是箭头；又有可能是中间是车库边缘，两侧是箭头。为了区分上面两个矛盾，我们必须要找到跳变沿的变化规律。
在这里插入图片描述
我们将视野分成两部分，左侧（红色）为视野偏左侧的跳变沿，其大小为与中线的距离，越靠近左侧其值越大，右侧（蓝色）同理

当小车开始进入库内的时候，基准线两侧扫描到的第一个跳变沿之间的距离就应该是库的宽度。当箭头出现在画幅偏左侧的时候，从基准线中央向左侧扫描获得的第一个跳变沿会有一个剧烈的减小，这个阈值我们设置为JUMPGATE ，定义为30个像素宽。如果我们检测到基准线左侧跳变沿的第一个跳变有下降沿，那么我们断定箭头在视野偏左侧的位置，即基准线中心在箭头右侧；若基准线右侧跳变沿有一个下降沿，那么我们断定基准线中心在箭头左侧；若基准线左侧、右侧跳变沿均有一个下降沿，则基准线中心在箭头内部。

车库边缘位置的确定

我们在基准线中心分别向左右两侧寻找跳变沿，然后存放在两个列表当中：

    # 从中心向外扫描边界，捕获黑变白上升沿
    img_Llist = []          # 存储从中心点向左侧存储的跳变沿，中心点为0
    img_Rlist = []          # 存储从中心点向右侧存储的跳变沿，中心点为0

然后根据阈值WID_THRES ，依次选择
$\left |img_Llist[0]- img_Rlist[0] \right |$

$\left |img_Llist[1]- img_Rlist[0] \right |$

$\left |img_Llist[1]- img_Rlist[1] \right |$

$\left |img_Llist[2]- img_Rlist[1] \right |$

$\left |img_Llist[2]- img_Rlist[2] \right |$
$\dots \dots$
当某一个值大于WID_THRES时，确定该时刻的差值就是车库边缘宽度。
在这里插入图片描述
但是这里有一个非常关键的问题，那就是第一次究竟应该先探索哪一侧，即应该是先左后右还是先右后左。上面的例子展示的是左侧探索优先。但是如果箭头在扫描中心的右侧，左侧探索优先一定会首先探测到左侧旁侧库的箭头位置，进而误认为两库内部箭头为库的边缘线了，这并不是我们想要的。
在这里插入图片描述
相反，如果我们采用右侧探索优先，就能够得到正确的车库边缘

因此这里我们就需要上一步得到的箭头的方位。如果箭头的位置在扫描基准行中心点左侧，则采用左侧探索优先，若箭头的位置在扫描基准行中心点右侧，则采用右侧探索优先。
在这里插入图片描述
完整代码如下，供大家参考学习：文章来源地址https://uudwc.com/A/EoDD

import pyb
import sensor, image, time, math
from math import *
from machine import UART
from pyb import Pin
from math import atan

sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)      # we run out of memory if the resolution is much bigger...
sensor.set_brightness(2000)             # 设置图像亮度 越大越亮
sensor.skip_frames(time = 20)
sensor.set_auto_gain(False)             # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)         # must turn this off to prevent image washout...
clock = time.clock()
                                            # 加载模型
WIDTH, HEIGHT = 160, 120                    # 画幅宽高
CARCENTER_X, CARCENTER_Y = 80, -40          # 车中心坐标
KTHETA = 40                                 # 角度p
THRESHOLD = (0,100)                         # 灰度二值化阈值

DIRECTFLAG = 0                              # 扫线模式，0左1右
ROUTE_WID = 40                              # 赛道宽，按经验取
FIRSTFLAG = 0                               # 第一次运行循环标志位
WAVETHRES = 30                              # 限幅滤波幅值
LINE_SCAN = 75                              # 扫线位置
BAIS = 16                                   # 后倒库角度偏置
WID_THRES = 90                              # 判断左右探索大于多少判定为车库边界的阈值
CHECK_MODE = 0                              # 查线方式判断箭头偏向镜头哪一侧
JUMPGATE = 30                               # 探索跳变沿的跳变沿阈值
lastimg_L, lastimg_R = [], []
lastimg_RDIS, lastimg_LDIS = 0, 0
uart = UART(3, 115200)

THETA = 0
img_LDIS, img_RDIS = 0, 0
LASTSTEER_THETA = 0

#img = sensor.snapshot()
#for i in range(WIDTH*HEIGHT - 1):
counter = 0
while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(strength = 1.5, zoom = 1.0)
    img.replace(vflip=True)#.lens_corr(strength = 1.8, zoom = 1.0)
    #print(img[120])
    #img.bilateral(3, color_sigma=0.1, space_sigma=1)
    img.binary([THRESHOLD])

    ##################################################################
    # 扫线
    # img reshape，img_MAT[第几行][0表示最左侧边沿，1表示最右侧边沿]
    # 初始全部设为-1表示缺线，当检测边沿全部完成后未改变的仍未-1
    img_MAT = []
    for i in range(int(HEIGHT/INTER)):
        img_MAT.append([-1,-1,-1])

    # 从中心向外扫描边界，捕获黑变白上升沿
    img_Llist = []          # 存储从中心点向左侧存储的跳变沿，中心点为0
    img_Rlist = []          # 存储从中心点向右侧存储的跳变沿，中心点为0

    #img_LDIS, img_RDIS = int(WIDTH/2), int(WIDTH/2)
    FLAGL, FLAGR = 0, 0
    for i in range(int(WIDTH/2)+1):

        # 对称搜索的角标，i1为从中心点向左数的列数，i2为从中心点向右数的列数
        i1 = int(WIDTH/2 - 1 - i)
        i2 = int(WIDTH/2 - 1 + i)
        
        # 视野中心就是白，则默认该处为跳变沿
        if i == 0 and img[int(LINE_SCAN*WIDTH + i1)] > 0:
            img_Llist.append(0)
            img_Rlist.append(0)

        # 左侧跳变沿，若左侧该点像素值为黑，下一个为白，该点为跳变点
        if i < int(WIDTH/2) - 1 and img[int(LINE_SCAN*WIDTH + i1)] == 0 and\
            img[int(LINE_SCAN*WIDTH + i1 - 1)] > 0:
            img_Llist.append(i)

        # 右侧跳变沿，若左侧该点像素值为黑，下一个为白，该点为跳变点
        if i < int(WIDTH/2) - 1 and img[int(LINE_SCAN*WIDTH + i2)] == 0 and\
            img[int(LINE_SCAN*WIDTH + i2 + 1)] > 0:
            img_Rlist.append(i)

        # 边缘处仍为黑色，默认黑变白跳变在视野边缘
        if i == int(WIDTH/2) - 1 and img[int(LINE_SCAN*WIDTH + i1)] == 0:
            img_Llist.append(int(WIDTH/2))
        if i == int(WIDTH/2) - 1 and img[int(LINE_SCAN*WIDTH + i2)] == 0:
            img_Rlist.append(int(WIDTH/2))


    # 设置阈值输出宽度
    # 循环两次调整数线模式
    lb_l_bia, lb_r_bia = 0, 0
    for j in range(2):
        for i in range(int(len(img_Llist) + len(img_Rlist))):
            if i%2 == 0:
                lb_l0 = int(i/2)
                lb_r0 = int(i/2)
            else:
                # 左向探索优先
                if CHECK_MODE == 1:
                    lb_l0 = int((i-1)/2)
                    lb_l0 = lb_l0 + 1
                # 右向探索优先
                elif CHECK_MODE == 0:
                    lb_r0 = int((i-1)/2)
                    lb_r0 = lb_r0 + 1

            lb_l = lb_l0# + lb_l_bia
            lb_r = lb_r0# + lb_r_bia

            # 探索方式为从中心点开始，一次向左，一次向右，并比较查看的两个跳变沿之间距离与阈值的关系
            # 逐次比较，当某一侧探索完毕后补上某侧列表最后元素
            if lb_l >= len(img_Llist):
                if len(img_Llist) == 0:
                    img_LDIS = int(WIDTH/2)
                else:
                    img_LDIS = img_Llist[len(img_Llist) - 1]
            else:
                img_LDIS = img_Llist[lb_l]

            if lb_r >= len(img_Rlist):
                if len(img_Rlist) == 0:
                    img_RDIS = int(WIDTH/2)
                else:
                    img_RDIS = img_Rlist[len(img_Rlist) - 1]
            else:
                img_RDIS = img_Rlist[lb_r]

            # 当中心探索区域大于某阈值弹出获得偏差
            if img_LDIS + img_RDIS > WID_THRES:
                THETA = img_RDIS - img_LDIS# - BAIS
                break
                
            # 过滤噪点
            if len(img_Llist) >= 2 and\
                lb_l < len(img_Llist)-2 and\
                abs(img_Llist[lb_l] - img_Llist[lb_l+1]) < 4:
                lb_l_bia += 1
            if len(img_Rlist) >= 2 and\
                lb_r < len(img_Rlist)-2 and\
                abs(img_Rlist[lb_l] - img_Rlist[lb_l+1]) < 4:
                lb_r_bia += 1

            if len(img_Llist) >= 2 and\
                len(img_Rlist) >= 2 and\
                img_Rlist[0] + img_Rlist[0] < 4:
                lb_l_bia += 1
                lb_r_bia += 1


        # 改变搜索方式
        if j == 0 and FIRSTFLAG == 1 and \
            img_Llist != [] and img_Rlist != [] and\
            lastimg_L != [] and lastimg_R != []:
            # 捕获跳变沿
            # 箭头在视野偏左侧出现，捕获左侧探索的下降沿，左侧探索优先
            if img_Llist[0] - lastimg_L[0] < -30:
                CHECK_MODE = 1
            # 箭头在视野偏右侧出现，捕获右侧探索的下降沿，右侧探索优先
            if img_Rlist[0] - lastimg_R[0] < -30:
                CHECK_MODE = 0

            # 箭头在视野偏左侧消失，捕获左侧探索的上升沿，右侧探索优先
            if img_Llist[0] - lastimg_L[0] > 30:
                CHECK_MODE = 0
            # 箭头在视野偏右侧消失，捕获右侧探索的上升沿，左侧探索优先
            if img_Rlist[0] - lastimg_R[0] > 30:
                CHECK_MODE = 1

    uart.write(str(int(THETA))+'\r\n')
    print(THETA,img_Llist,img_Rlist,CHECK_MODE)
    
    FIRSTFLAG = 1
    lastimg_L = img_Llist
    lastimg_R = img_Rlist
    lastimg_RDIS = img_RDIS
    lastimg_LDIS = img_LDIS
    LASTSTEER_THETA = THETA
    img.draw_line(0, LINE_SCAN, 159, LINE_SCAN, color = (255, 255, 0), thickness = 2)