2022年电子设计竞赛B题倒库闭环的思考
- 具有自动泊车功能的电动车（B题）简介
- 误差的获得
- 如何获取初始偏置
- 库内有箭头的搜索方法
- 箭头方位的获得
- 车库边缘位置的确定
2022年我队友FKR带我打电赛。作为智能车队的一员，我们肯定要做和车有关的。
2022年电子设计竞赛B题部分要求如下：
具有自动泊车功能的电动车（B题）简介
设计制作具有自动泊车功能的电动车，可在图1所示的作品测试泊车场地上，分别独立完成“倒车入库/出库”或“侧方入库/出库”的单项操作，也可连续完成这两项入库/出库的操作。
（1）单项倒车入库/出库①：如图2所示，一键启动摆放在“发车区1”内的电动车，电动车以“右侧垂直泊车方式”自动倒车进入库2内居中位置停车（详见图2中库区abcd，此时库1、库3内均停有车辆），倒车入库时间越短越好（定义见本题说明，>30s的测试项成绩记0分）。电动车在库内停车到位5s后，沿车头方向右转出库，车身整体出库时间不超过15s。（20分）
这里就以倒库部分的闭环控制讲解为例。
在如图所示的小车上方加一根杆子，然后添加一个OpenMV摄像头角度略微朝下，能够看到倒库时视角如下图所示
需要注意的是，库内的箭头大小无所谓。我们最需要关注的点就是希望小车能够在库内沿着中轴线前进，也就是说，我们的目标是找到需要停车库的边线位置。
误差的获得
先考虑库内没有箭头的情况
首先，我们需要明确几个概念：当小车沿着库内中线走的时候，那么在后视镜看到的视角当中库边侧两条线应该是沿着自身滑动的，即在视野中是相对静止的。因此我们可以通过寻找跳变沿，即像素点从黑变白的位置处作为库边缘的位置。我们在屏幕中选择一条水平的基准线，那么如果在没有箭头，而且车身沿着库内平行于库边的直线倒退的时候，该基准线上的黑变白的跳变沿的位置应该是不变的。
以屏幕中心为起始点，向外侧扫描跳变沿，左侧的距离为，右侧的距离为
，由于摄像头的位置可能不是非常正好的正中央，所以当车身沿着平行于库边缘的中心线后退的时候，应满足如下两个式子：
其中是给定扫描行下库边缘的宽度，
是车沿着平行于库边缘的中心线后退的时候由于摄像头初始位置不是完全的视野中心而造成的初始偏置。
如果车身并没有沿着中心线运行的话，我们就取误差如下：
很容易理解，该量具有极性。当车身偏向左侧时，增大，
减小，
减小，甚至过零变负，反之亦然。将该量作为小车舵机
控制的输入量就可以实现入库闭环自校正。
如何获取初始偏置
获取初始偏置的方法建议手动调试。即给舵机上电且机械调零后，舵机就可以稳定在机械零点的位置。这时手扶着小车在库中线前后移动，在不触碰边线的情况下如果在车移动的过程中保持恒定了，这证明小车走的路径确实是沿着平行于库边缘的直线运行。这时在视觉上车身就是在车库中线上运行时且
仍保持恒定，记录
作为初始偏置。
库内有箭头的搜索方法
箭头方位的获得
首先对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 # 探索跳变沿的跳变沿阈值
我们采用的扫描方式只是简单的单行扫描，如果只是静态的扫描我们无法获得箭头的真实位置。如下图所示
车库边缘位置的确定
我们在基准线中心分别向左右两侧寻找跳变沿，然后存放在两个列表当中：
# 从中心向外扫描边界，捕获黑变白上升沿
img_Llist = [] # 存储从中心点向左侧存储的跳变沿，中心点为0
img_Rlist = [] # 存储从中心点向右侧存储的跳变沿，中心点为0
然后根据阈值WID_THRES ，依次选择
当某一个值大于WID_THRES时，确定该时刻的差值就是车库边缘宽度。
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)
文章出处登录后可见！
已经登录？立即刷新
