2023年光电赛——小车题
整体方案
OpenMV4Plus识别和规划 + OpenMV4循迹 + STM32运动控制
其中OpenMV4Plus作为上位机,负责处理高算力的规划任务,识别宝藏图和骨牌宝藏。OpenMV4作为功能单一的循迹模块,通过串口不断的向STM32传递前方视野内的循迹信息。STM32作为下位机,通过串口接收上位机OpenMV4Plus的控制指令,执行相应的运动。
OpenMV4Plus作为寻宝小车的大脑,负责处理高算力,高延时的任务,规划小车的运动。STM32就像人的小脑,负责实时的运动控制,处理低算力,实时性高的任务。
OpenMV4Plus上位机部分
识别宝藏图
因为OpenMV4Plus同时运行矩形识别和圆形识别的帧率过低,所以每用透视矫正的参考点确定宝藏图的相对位置。而是通过宝藏图部分的总体灰度值相对于背景来说更低来框取出地图的位置,再在框取的地图里寻找圆的位置,根据圆心相对于地图整体的像素坐标确定宝藏点在10x10的地图坐标中的位置,并验证地图坐标是否呈中心对称。
代码如下:
def ditushibie():
i = 1
m = 0
n = 0
t = (100, 255)
roih = 0
roiw = 0
while (n != 8):
i = 1
m = 0
n = 0
while (i == 1):
clock.tick()
img = sensor.snapshot().to_grayscale().laplacian(1, sharpen=True)
blobs = img.find_blobs([(100, 0)], area_threshold=30000, pixels_threshold=2300, merge=True)
for b in blobs:
img.draw_rectangle(b.rect())
roi = b.rect()
roih = b.h()
roiw = b.w()
juxinglist = b.corners()
# print(b.corners())
# print(b.pixels())
# print(roi)
test = img.copy(roi=b.rect(), copy_to_fb=True).binary([t])
test.save("img3.jpg")
if (b[0] != 0):
i = 2
break
if (i == 2):
# print("start!")
circle_list = []
map_list = []
for c in test.find_circles(threshold=4000, x_margin=10, y_margin=10, r_margin=10, r_min=4, r_max=5,r_step=1):
# print(c)
pyb.LED(1).on()
circle_list.append((c.x(), c.y()))
a = int(c.x() / 17.5)
b = int((180 - c.y()) / 16 + 0.5)
if (a > 10):
a = 10
if (b > 10):
b = 10
map_list.append((a, b))
m += 1
if (roih < 200 and m >= 7 and roiw < 230):
pyb.LED(3).on()
else:
pyb.LED(3).off()
if (m == 8):
# print(circle_list)
for i in range(8):
a = map_list[i][0]
b = map_list[i][1]
for j in range(8):
c = (a + map_list[j][0]) / 2.0
d = (b + map_list[j][1]) / 2.0
if ((c == 5.5) & (d == 5.5)):
n += 1
if (n == 8):
print(map_list)
pyb.LED(1).off()
pyb.LED(3).off()
pyb.LED(2).on()
pyb.delay(200)
pyb.LED(2).off()
return map_list
print(m)
print("End!")
识别宝藏
因为不同的宝藏有不一样的颜色组合,所以可以通过获取宝藏的颜色组合实现宝藏类型的识别。通过设置4种目标颜色的颜色范围,依次用OpenMV的find_blobs函数过滤出图片中的4个目标颜色的色块,通过设置pixels_threshold参数,忽略掉较少像素的色块,防止出现误检测。在比赛现场就有出现背景色偏蓝,导致每次都会检测到蓝色,这种情况会影响到红色宝藏的检测,解决办法是给红色宝藏添加一种带有蓝色的颜色组合。
这个方案受环境影响很大,一套阈值一般只适用于一个特定的环境,如果环境光与调试时有较大差异,会导致OpenMV的自动曝光改变,使得获取到的颜色有差异,导致识别出错。
代码如下:
# 目标颜色在LAB颜色空间的阈值范围
thresholds = [(13, 90, 21, 127, -6, 44), #red
(10, 60, -128, -13, -10, 40), #green
(40, 95, -30, 20, 25, 127), #yellow
(35, 90, -35, 2, -128, -10)] #blue
# 宝藏识别函数
def baozhangshibie():
color_list = [0, 0, 0, 0]
clock.tick()
img = sensor.snapshot()
# 识别红色
blobs = img.find_blobs([thresholds[0]], pixels_threshold=1000)
if blobs:
color_list[0] = 1
# 识别绿色
blobs = img.find_blobs([thresholds[1]], pixels_threshold=400)
if blobs:
color_list[1] = 1
# 识别黄色
blobs = img.find_blobs([thresholds[2]], pixels_threshold=500)
if blobs:
color_list[2] = 1
# 识别蓝色
blobs = img.find_blobs([thresholds[3]], pixels_threshold=1000)
if blobs:
color_list[3] = 1
# 不记录图片============================================================
if color_list == [1, 1, 0, 0] or color_list == [1, 1, 0, 1]: # red; 加入蓝色,环境干扰色
return 0
elif color_list == [0, 0, 1, 1]: # blue
return 1
elif color_list == [1, 0, 1, 0] or color_list == [1, 0, 1, 1]: # red_feak
return 2
elif color_list == [0, 1, 0, 1]: # blue_feak
return 3
# =====================================================================
# 记录每张图片==========================================================
# if color_list == [1, 1, 0, 0] or color_list == [1, 1, 0, 1]: # 红真; 加入蓝色,环境干扰色
# save_a_img_in_tfcard(begin_point,0,color_list,img)
# return 0
# elif color_list == [0, 0, 1, 1]: # 蓝真
# save_a_img_in_tfcard(begin_point,1,color_list,img)
# return 1
# elif color_list == [1, 0, 1, 0] or color_list == [1, 0, 1, 1]: # 红假; 加入蓝色
# save_a_img_in_tfcard(begin_point,2,color_list,img)
# return 2
# elif color_list == [0, 1, 0, 1]: # 蓝假
# save_a_img_in_tfcard(begin_point,3,color_list,img)
# return 3
# else:
# save_a_img_in_tfcard(begin_point,4,color_list,img)
# ====================================================================
# 记录未识别的图片============================================================
# if color_list == [1, 1, 0, 0] or color_list == [1, 1, 0, 1]: # red
# return 0
# elif color_list == [0, 0, 1, 1]: # blue
# return 1
# elif color_list == [1, 0, 1, 0] or color_list == [1, 0, 1, 1]: # red_feak
# return 2
# elif color_list == [0, 1, 0, 1]: # blue_feak
# return 3
# else:
# save_a_img_in_tfcard(begin_point,4,color_list,img)
# =====================================================================
路径规划
两点间的最短距离
我们将地图转换为21x21的地图矩阵,使用Dijkstra算法寻找地图中任意两点间的最短距离。
地图矩阵如下,1表示墙,0表示通道
lap = [
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1],
[1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1],
[1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1],
[1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1],
[1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1],
[1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1],
[1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1],
[1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1],
[1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1],
[1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1],
[1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1],
[1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1],
[1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1],
[1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1],
[1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
]
通过DijKstra算法获得最短路径要经过的所有坐标点,再通过每个路口坐标的前一个坐标点和后一个坐标点计算出这个路口是左转,右转,或直行,得出路口控制列表,将路口控制列表发送给下位机STM32,STM32根据这个列表在相应路口执行相应事件,使小车到达OpenMV4Plus规划前往的目标点。DijKstra算法视频:【算法】最短路径查找—Dijkstra算法 哔哩哔哩 bilibili
路径坐标列表转换为路口控制列表的代码如下:
def road_list_2_control_list(road_list):
crossing_turn_r = [[-1, 0, 0, 1], [0, -1, -1, 0], [1, 0, 0, -1], [0, 1, 1, 0]]
crossinf_turn_l = [[-1, 0, 0, -1], [0, 1, -1, 0], [1, 0, 0, 1], [0, -1, 1, 0]]
crossing_no_turn = [[0, 1, 0, 1], [0, -1, 0, -1], [1, 0, 1, 0], [-1, 0, -1, 0]]
crossing_turn_list = [] # 路口控制列表
road_crossing_zb_sy_list = [] # 路口坐标索引列表
for index, value in enumerate(road_list):
if value in crossing_list:
road_crossing_zb_sy_list.append(index) # 获得路径中的路口在路径列表的索引
for i in road_crossing_zb_sy_list:
crossing_turn_list.append(0) # 初始化路口控制列表, 全部置零
# 遍历每个路口的状况
for k_sy, k in enumerate(road_crossing_zb_sy_list):
temp = [road_list[k - 1][0] - road_list[k][0], road_list[k - 1][1] - road_list[k][1],
road_list[k][0] - road_list[k + 1][0], road_list[k][1] - road_list[k + 1][1]]
if temp in crossinf_turn_l:
crossing_turn_list[k_sy] = 1
elif temp in crossing_turn_r:
crossing_turn_list[k_sy] = 2
elif temp in crossing_no_turn:
crossing_turn_list[k_sy] = 0
else:
print('error')
print(road_list[k])
print(temp)
return crossing_turn_list
基于DijKstra算法的两点间路径规划的完整代码如下:
# 传入起点和目标点
def hey(begin_zb, target_zb):
global lap
# 屎山代码。调用时传入起点和终点,返回一个数组,包括路口控制列表和路径长度
# 0818 如果道路被阻塞,无法规划出路径,data[0]将为空数组[],data[1]为0
# 如果lap已定义为全局变量,则将此处的注释掉,因为某些情况,可能需要改变地图状体,比如宝藏放在了路上
# lap = [
# [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
# [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
# [1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1],
# [1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1],
# [1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1],
# [1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1],
# [1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1],
# [1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1],
# [1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1],
# [1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
# [1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1],
# [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1],
# [1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1],
# [1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1],
# [1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1],
# [1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1],
# [1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1],
# [1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1],
# [1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1],
# [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
# [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
# ]
jiedian_list = [[1, 1], [1, 2], [1, 3], [1, 4], [1, 5], [1, 6], [1, 7], [1, 8], [1, 9], [1, 11], [1, 12], [1, 13], [1, 14], [1, 15], [1, 17], [1, 18], [1, 19], [1, 20], [2, 1], [2, 3], [2, 7],
[2, 9], [2, 11], [2, 15], [2, 17], [3, 1], [3, 3], [3, 5], [3, 6], [3, 7], [3, 9], [3, 10], [3, 11], [3, 12], [3, 13], [3, 15], [3, 17], [3, 18], [3, 19], [4, 1], [4, 5], [4, 9],
[4, 13], [4, 15], [4, 19], [5, 1], [5, 3], [5, 4], [5, 5], [5, 6], [5, 7], [5, 9], [5, 10], [5, 11], [5, 12], [5, 13], [5, 15], [5, 17], [5, 18], [5, 19], [6, 3], [6, 5], [6, 7],
[6, 13], [6, 17], [7, 1], [7, 2], [7, 3], [7, 5], [7, 7], [7, 8], [7, 9], [7, 10], [7, 11], [7, 13], [7, 14], [7, 15], [7, 16], [7, 17], [7, 19], [8, 1], [8, 5], [8, 7], [8, 15],
[8, 19], [9, 1], [9, 3], [9, 4], [9, 5], [9, 7], [9, 8], [9, 9], [9, 10], [9, 11], [9, 12], [9, 13], [9, 14], [9, 15], [9, 16], [9, 17], [9, 18], [9, 19], [10, 1], [10, 7],
[10, 13], [10, 19], [11, 1], [11, 2], [11, 3], [11, 4], [11, 5], [11, 6], [11, 7], [11, 8], [11, 9], [11, 10], [11, 11], [11, 12], [11, 13], [11, 15], [11, 16], [11, 17], [11, 19],
[12, 1], [12, 5], [12, 13], [12, 15], [12, 19], [13, 1], [13, 3], [13, 4], [13, 5], [13, 6], [13, 7], [13, 9], [13, 10], [13, 11], [13, 12], [13, 13], [13, 15], [13, 17], [13, 18],
[13, 19], [14, 3], [14, 7], [14, 13], [14, 15], [14, 17], [15, 1], [15, 2], [15, 3], [15, 5], [15, 7], [15, 8], [15, 9], [15, 10], [15, 11], [15, 13], [15, 14], [15, 15], [15, 16],
[15, 17], [15, 19], [16, 1], [16, 5], [16, 7], [16, 11], [16, 15], [16, 19], [17, 1], [17, 2], [17, 3], [17, 5], [17, 7], [17, 8], [17, 9], [17, 10], [17, 11], [17, 13], [17, 14],
[17, 15], [17, 17], [17, 19], [18, 3], [18, 5], [18, 9], [18, 11], [18, 13], [18, 17], [18, 19], [19, 0], [19, 1], [19, 2], [19, 3], [19, 5], [19, 6], [19, 7], [19, 8], [19, 9],
[19, 11], [19, 12], [19, 13], [19, 14], [19, 15], [19, 16], [19, 17], [19, 18], [19, 19]]
jvli = [900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900,
900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900,
900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900,
900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900,
900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900,
900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900, 900]
biaoji = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
before_zb_sy = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# 地图上半部分
jiedian_list_part_1 = [[1, 1], [1, 2], [1, 3], [1, 4], [1, 5], [1, 6], [1, 7], [1, 8], [1, 9], [1, 11], [1, 12], [1, 13], [1, 14], [1, 15], [1, 17], [1, 18], [1, 19], [1, 20], [2, 1], [2, 3],
[2, 7], [2, 9], [2, 11], [2, 15], [2, 17], [3, 1], [3, 3], [3, 5], [3, 6], [3, 7], [3, 9], [3, 10], [3, 11], [3, 12], [3, 13], [3, 15], [3, 17], [3, 18], [3, 19], [4, 1],
[4, 5], [4, 9], [4, 13], [4, 15], [4, 19], [5, 1], [5, 3], [5, 4], [5, 5], [5, 6], [5, 7], [5, 9], [5, 10], [5, 11], [5, 12], [5, 13], [5, 15], [5, 17], [5, 18], [5, 19],
[6, 3], [6, 5], [6, 7], [6, 13], [6, 17], [7, 1], [7, 2], [7, 3], [7, 5], [7, 7], [7, 8], [7, 9], [7, 10], [7, 11], [7, 13], [7, 14], [7, 15], [7, 16], [7, 17], [7, 19],
[8, 1], [8, 5], [8, 7], [8, 15], [8, 19], [9, 1], [9, 3], [9, 4], [9, 5], [9, 7], [9, 8], [9, 9], [9, 10], [9, 11], [9, 12], [9, 13], [9, 14], [9, 15], [9, 16], [9, 17],
[9, 18], [9, 19], [10, 1], [10, 7]]
# 地图下半部分
jiedian_list_part_2 = [[19, 19], [19, 18], [19, 17], [19, 16], [19, 15], [19, 14], [19, 13], [19, 12], [19, 11], [19, 9], [19, 8], [19, 7], [19, 6], [19, 5], [19, 3], [19, 2], [19, 1], [19, 0],
[18, 19], [18, 17], [18, 13], [18, 11], [18, 9], [18, 5], [18, 3], [17, 19], [17, 17], [17, 15], [17, 14], [17, 13], [17, 11], [17, 10], [17, 9], [17, 8], [17, 7], [17, 5],
[17, 3], [17, 2], [17, 1], [16, 19], [16, 15], [16, 11], [16, 7], [16, 5], [16, 1], [15, 19], [15, 17], [15, 16], [15, 15], [15, 14], [15, 13], [15, 11], [15, 10], [15, 9],
[15, 8], [15, 7], [15, 5], [15, 3], [15, 2], [15, 1], [14, 17], [14, 15], [14, 13], [14, 7], [14, 3], [13, 19], [13, 18], [13, 17], [13, 15], [13, 13], [13, 12], [13, 11],
[13, 10], [13, 9], [13, 7], [13, 6], [13, 5], [13, 4], [13, 3], [13, 1], [12, 19], [12, 15], [12, 13], [12, 5], [12, 1], [11, 19], [11, 17], [11, 16], [11, 15], [11, 13],
[11, 12], [11, 11], [11, 10], [11, 9], [11, 8], [11, 7], [11, 6], [11, 5], [11, 4], [11, 3], [11, 2], [11, 1], [10, 19], [10, 13]]
crossing_list = [[1, 1], [1, 3], [1, 7], [1, 9], [1, 11], [1, 15], [1, 17], [3, 5], [3, 7], [3, 9], [3, 11], [3, 13], [3, 17], [3, 19], [5, 3], [5, 5], [5, 7], [5, 9], [5, 13], [5, 17], [5, 19],
[7, 1], [7, 3], [7, 7], [7, 13], [7, 15], [7, 17], [9, 5], [9, 7], [9, 13], [9, 15], [9, 19], [11, 1], [11, 5], [11, 7], [11, 13], [11, 15], [13, 3], [13, 5], [13, 7], [13, 13],
[13, 17], [13, 19], [15, 1], [15, 3], [15, 7], [15, 11], [15, 13], [15, 15], [15, 17], [17, 1], [17, 3], [17, 7], [17, 9], [17, 11], [17, 13], [17, 15], [19, 3], [19, 5], [19, 9],
[19, 11], [19, 13], [19, 17], [19, 19]]
# begin_zb = [1, 20]
# begin_zb = jiedian_list[17]
# target_zb = [9, 3]
# target_zb = jiedian_list[106]
# 上面是乱七八糟的变量的定义
# def func1():
# # num 0 have 208
# jd = []
# for x_index, x_value in enumerate(lap):
# for y_index, y_value in enumerate(x_value):
# if y_value == 0:
# zb = [x_index, y_index]
# jd.append(zb)
# # print('({},{})'.format(x_index,y_index))
# print(len(jd))
# print(jd)
def func2():
for index, value in enumerate(jiedian_list):
if begin_zb == value:
jvli[index] = 0
biaoji[index] = 1
before_zb_sy[index] = 666
return index
# func3()
def func3():
print('jvli:\t\t', jvli)
print('biaoji:\t\t', biaoji)
print('before_zb:\t', before_zb_sy)
print('#' * 64)
def func4(start_zb_sy, target_zb_sy):
start_zb = jiedian_list[start_zb_sy]
maybe = [0, 0, 0, 0]
target_x = start_zb[0]
target_y = start_zb[1]
if target_x - 1 > -1 and lap[target_x - 1][target_y] == 0:
maybe[0] = [target_x - 1, target_y]
if target_x + 1 < 21 and lap[target_x + 1][target_y] == 0:
maybe[1] = [target_x + 1, target_y]
if target_y - 1 > -1 and lap[target_x][target_y - 1] == 0:
maybe[2] = [target_x, target_y - 1]
if target_y + 1 < 21 and lap[target_x][target_y + 1] == 0:
maybe[3] = [target_x, target_y + 1]
# print(maybe)
for i in maybe:
if i != 0:
for index, value in enumerate(jiedian_list): # 用于找到相邻坐标 i 的索引
if i == value:
begin_to_i_jvli = jvli[start_zb_sy] + 1
if begin_to_i_jvli < jvli[index]:
jvli[index] = begin_to_i_jvli
before_zb_sy[index] = start_zb_sy
break
# jvli_fuben = jvli # b=a表示b引用列表a;这样的话a,b指向同一个列表,改变b也会改变a
jvli_fuben = jvli[:] # 列表复制要用切片的方式
for index, value in enumerate(biaoji):
if value == 1:
jvli_fuben[index] = 900
next_target_zb_jvli = min(jvli_fuben)
next_target_zb_sy = jvli_fuben.index(next_target_zb_jvli)
biaoji[next_target_zb_sy] = 1
if biaoji[target_zb_sy] == 1: # 找到最短路径
# exit(200) // 这个语句会使程序结束运行
# pass
return 666
else:
# print(next_target_zb_jvli)
# print(next_target_zb_sy)
# func3()
if next_target_zb_jvli == 900:
# exit(900) # mpy的标准库好像没有exit函数
print('Error')
# 无法到达的目标点, 结束规划
return 555
else: # 正常情况
return next_target_zb_sy
# func4(next_target_zb_sy, target_zb_sy) # fuck递归; openmv运行效率低
def func5(target_zb_sy):
temp_list = []
dd_sy = target_zb_sy # 给临时值赋目标值
while True:
temp_list.append(jiedian_list[dd_sy])
if jiedian_list[dd_sy] == begin_zb:
break
dd_sy = before_zb_sy[dd_sy]
return temp_list
# 输出一个对应于坐标中的路径坐标数组
def out_road_list():
print('Begin: ', begin_zb)
print('Target:', target_zb)
can_out_road_list = 1
begin_sy = func2()
target_sy = begin_sy
for i, v in enumerate(jiedian_list):
if target_zb == v:
target_sy = i
break
# func3()
next_sy = func4(begin_sy, target_sy)
while True:
if next_sy != 666:
next_sy = func4(next_sy, target_sy)
if next_sy == 555:
can_out_road_list = 0
break
else:
break # next_sy=666 说明找到了最短路径
if can_out_road_list:
road_list = func5(target_sy) # 最终路径列表; 规划出了路径
else:
road_list = [] # 没有规划出路径
# 绘制原地图
# for x, i in enumerate(lap):
# for y, j in enumerate(i):
# if j == 1:
# lap[x][y] = '0'
# # lap[x][y] = '■'
# elif j == 0:
# lap[x][y] = ' '
# # 绘制路径
# for i in road_list:
# x = i[0]
# y = i[1]
# lap[x][y] = '*'
# for i in lap:
# for j in i:
# print(' ', j, end='')
# print('')
# road_list原来是倒序的,最先记录的是终点,最后记录的是起点
return road_list[::-1]
# 传入一个路径坐标数组,输出路口控制列表
def road_list_2_control_list(road_list):
crossing_turn_r = [[-1, 0, 0, 1], [0, -1, -1, 0], [1, 0, 0, -1], [0, 1, 1, 0]]
crossinf_turn_l = [[-1, 0, 0, -1], [0, 1, -1, 0], [1, 0, 0, 1], [0, -1, 1, 0]]
crossing_no_turn = [[0, 1, 0, 1], [0, -1, 0, -1], [1, 0, 1, 0], [-1, 0, -1, 0]]
crossing_turn_list = [] # 路口控制列表
road_crossing_zb_sy_list = [] # 路口坐标索引列表
for index, value in enumerate(road_list):
if value in crossing_list:
road_crossing_zb_sy_list.append(index) # 获得路径中的路口在路径列表的索引
for i in road_crossing_zb_sy_list:
crossing_turn_list.append(0) # 初始化路口控制列表, 全部置零
# 遍历每个路口的状况
for k_sy, k in enumerate(road_crossing_zb_sy_list):
temp = [road_list[k - 1][0] - road_list[k][0], road_list[k - 1][1] - road_list[k][1], road_list[k][0] - road_list[k + 1][0], road_list[k][1] - road_list[k + 1][1]]
if temp in crossinf_turn_l:
crossing_turn_list[k_sy] = 1
elif temp in crossing_turn_r:
crossing_turn_list[k_sy] = 2
elif temp in crossing_no_turn:
crossing_turn_list[k_sy] = 0
else:
print('error')
print(road_list[k])
print(temp)
return crossing_turn_list
# 返回路口控制列表和路径长度
road_list_ori = out_road_list()
road_list_len = len(road_list_ori)
road_control_list = road_list_2_control_list(road_list_ori)
out_data = [road_control_list, road_list_len]
return out_data
决策与记录
决策将要前往的点
将剩余目标点全部规划一遍,找到最近的目标点前往。整个规划采用的是局部最优,并没有进行全局最优的规划。根据已知识别的宝藏,可以推断出相邻宝藏的颜色,根据这个可以排除到一些没必要前往的点。在小车识别完所有点后,小车将规划前往出口。
记录数据到TF卡中
每次规划前,将用于规划的数据保存到TF卡中,用于中断后的恢复。
保存和恢复的代码如下:
# 保存用于规划的数据
def save_data():
global lap
global begin_point
global target_list_1
global target_list_2
global en_list_2_append
global team_color
global xx_part_1
global xx_part_2
global new_file
global file_num
# 文件序号加1, 文件序号即规划前往第n个宝藏时的记录
file_num += 1
# 将各种数据按顺序填充到数组中
data_l = [lap,begin_point,target_list_1,target_list_2,en_list_2_append,team_color,xx_part_1,xx_part_2,file_num]
# 将数据保存到data.json中
with open(new_file, "w") as f:
data_json = ujson.dumps(data_l)
f.write(data_json)
# 将数据按序号保存
file_name = f'data_{file_num}.json'
with open(file_name, "w") as f:
data_json = ujson.dumps(data_l)
f.write(data_json)
# 判断文件是否存在,存在返回真,不存在返回假
def get_file_exists(f_path):
file_list = uos.listdir()
if f_path in file_list:
return True
else:
return False
# 读取保存的数据,传入文件路径f_path
def read_file(f_path):
global lap
global begin_point
global target_list_1
global target_list_2
global en_list_2_append
global team_color
global xx_part_1
global xx_part_2
global file_num
with open(f_path,"r") as f:
data = ujson.loads(f.read())
# 把各种信息赋值
lap = data[0]
pre_begin = data[1]
target_list_1 = data[2]
target_list_2 = data[3]
en_list_2_append = data[4]
team_color = data[5]
xx_part_1 = data[6]
xx_part_2 = data[7]
file_num = data[8]
begin_point = [19, 0]
OpenMV4循迹部分
OpenMV4在GRAYSCALE+QQVGA分辨率模式下可以达到固件现在的最高帧率,150FPS,在前期慢速的情况下,这个帧率是足够的,但在最后的优化过程中,我们将小车速度加到一定值后,循迹速度开始跟不上车的速度,表现为车不能准确的在路口位置转弯和停下,因为OpenMV4的循迹速度达不到车需要的反馈速度了。所以我们最后只能将车的速度限制在OpenMV4的反馈速度。
获取并处理循迹线信息
我们将图像进行二值化处理,只保留黑色循迹线,通过划定感兴趣判断路口和终点末端。通过OpenMV的线性回归函数+PID实现循迹矫正。线性回归循迹的官方示例:linear_regression_fast 快速线性回归 · OpenMV中文入门教程
通过while循环不断的判断感兴趣内是否有符合要求的色块,如果符合条件,则设置检测标记,将检测标记通过串口发送到STM32。如果没有检测到路口或终点,则对图像进行线性回归,如果回归性好,则将计算出矫正值,将矫正值和检测标记发送给STM32。
传输循迹数据
传回STM32的数据,一个数据包6字节,包含1字节帧头,1字节帧尾和4字节数据。4字节数据中,第1字节是检测标记,表示这一数据包的传回的事件类型。第2,3,4字节数据只有在传回循迹矫正数据时才有效,其它情况下这3字节是无效数据。
完整代码如下:
import sensor, image, time
from pyb import UART,LED
from pid import PID
import ustruct,pyb
# 9000速度
rho_pid = PID(p=0.62,i=0.00,d=0)
theta_pid = PID(p=0.52,i=0,d=0)
line_th = [(0,80)] # 保留路径为白色 复赛阈值
roi_l = (0,71,15,39) # (0,81,15,39)
roi_r = (145,71,15,39) #(145,81,15,39) 5000; 7000
roi_over = (30,25,100,8) # 终点检测
roi_line = (25,30,110,120) # 线性回归区域
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()
uart = UART(3) #定义串口3变量
uart.init(115200, bits=8, parity=None, stop=1) # init with given parameters
def led_open():
LED(1).on()
LED(2).on()
LED(3).on()
# 正常矫正信息
def send_data_1_1(sj):
global uart
if(sj<0):
b = 0xA1 # 右轮加速
sj = -sj
else:
b=0xA0 # 左轮加速
data = ustruct.pack("<bbbHb",
0x24,
0xAA,
b,
int(sj*1000),
0x63)
uart.write(data)
# 车身无角度偏差的矫正信息
def send_data_1_2(sj):
global uart
if(sj<0):
b = 0xA1 # 右轮加速
sj = -sj
else:
b=0xA0 # 左轮加速
data = ustruct.pack("<bbbHb",
0x24,
0xAB,
b,
int(sj*1000),
0x63)
uart.write(data)
# 框出感兴区域的位置
def debug_draw_rect(img2,roi2:tuple):
img2.draw_rectangle(roi2[0],roi2[1],roi2[2],roi2[3],80)
en_blobs_over = 0
pyb.delay(1000)
while(True):
# pyb.delay(2)
rho_err = 0
theta_err = 0
rho_output = 0
theta_output = 0
sj = 0
clock.tick()
img = sensor.snapshot().binary(line_th) #线白
blobs_r = img.find_blobs([(255,255)], x_stride = 2, y_stride = 3, pixels_threshold=80, merge=True, roi=roi_r)
blobs_l = img.find_blobs([(255,255)], x_stride = 2, y_stride = 3, pixels_threshold=80, merge=True, roi=roi_l)
if(blobs_r or blobs_l):
en_blobs_over = 0
data = ustruct.pack("<bbbHb",
0x24,
0xAC,
0x00,
int(0),
0x63)
uart.write(data)
uart.write(data)
uart.write(data)
uart.write(data)
# 调试串口打印
#print("FPS %f" % (clock.fps()))
# print('0xAC')
else:
blobs_over = img.find_blobs([(255,255)], x_stride = 4, y_stride = 3, pixels_threshold=60, merge=True, roi=roi_over)
if(blobs_over): # 划定区域有色块
line = img.get_regression([(255,255)], roi = roi_line)
if (line):
rho_err = abs(line.rho())-img.width()/2 #直线偏移距离
if line.theta()>90: #直线角度
theta_err = line.theta()-180
else:
theta_err = line.theta()
if (line.magnitude()>3):
en_blobs_over = 1
# img.draw_line(line.line(), color = 127) # 画出回归线
rho_output = rho_pid.get_pid(rho_err,1)
theta_output = theta_pid.get_pid(theta_err,1)
sj = rho_output+theta_output
if theta_err<1: # 可以用于矫正陀螺仪
send_data_1_2(sj)
else:
send_data_1_1(sj)
else: # 线性回归效果差
data = ustruct.pack("<bbbHb",
0x24,
0xEE,
0x00,
int(0),
0x63)
uart.write(data)
# print('0xEE')
else:
# 线性回归失败
data = ustruct.pack("<bbbHb",
0x24,
0x78,
0x00,
int(0),
0x63)
uart.write(data)
# print('0x78')
# 调试串口打印
#print("FPS %f" % (clock.fps()))
# print('线性回归效果:{}'.format(line.magnitude() if (line) else 0)) # 加了判断,没line也不会程序崩溃
# print('直线偏移:{},pid:{}'.format(rho_err,rho_output))
# print('角度:{},pid:{}'.format(theta_err,theta_output))
# print('sj:{}'.format(sj))
# print('')
elif (en_blobs_over == 1): # 划定区域没有色块 同时 前一次检测是直线循迹,即前一次检测没有路口
data = ustruct.pack("<bbbHb",
0x24,
0xAE,
0x00,
int(0),
0x63)
uart.write(data)
uart.write(data)
# 调试串口打印
#print("FPS %f" % (clock.fps()))
# print('0xAE')
else:
# 前一次是路口,现在识别到了终点,未定义的情况;
data = ustruct.pack("<bbbHb",
0x24,
0x1A,
0x00,
int(0),
0x63)
uart.write(data)
# print('None')
# print("FPS %f" % (clock.fps()))
# 调试绘制
# debug_draw_rect(img,roi_l)
# debug_draw_rect(img,roi_r)
# debug_draw_rect(img,roi_over)
# debug_draw_rect(img,roi_line)
STM32下位机部分
PID电机控制
我们使用增量式PID控制器实现电机转速的闭环控制,增量式PID的优点有响应速度快,中途改变目标速度不会产生巨大抖动。
增量式PID公式为:
pwm_1=pwm_2+K_p [e(k)-e(k-1)]+K_i e(k)+K_d [e(k)-2e(k-1)+e(k-2)]
实现代码如下:
typedef struct
{
float Kp; // 比例增益
float Ki; // 积分时间
float Kd; // 微分时间
float setValue; // 实际设定速度
float baseValue; // 基础目标速度
float preError; // 上一次误差; e(k-1)
float prepreError; // 上上次误差; e(k-2)
float preOutput; // 上一次输出
} tPid;
void PID_Struct_Init(tPid *pid)
{
pid->Kp = 0.03;
pid->Ki = 0.09;
pid->Kd = 0.00;
pid->setValue = 0.0;
pid->baseValue = 0.0;
pid->preError = 0.0;
pid->prepreError = 0.0;
pid->preOutput = 0.0;
}
float PID(tPid *pid, float currentValue)
{
currentError = pid->setValue - currentValue;
Vp = pid->Kp * (currentError - pid->preError);
Vi = pid->Ki * currentError;
Vd = pid->Kd * (currentError - (pid->preOutput) * 2 + pid->prepreError);
currentOutput = pid->preOutput + Vp + Vi + Vd; // 计算输出值
if (currentOutput > 3500)
{
currentOutput = 3500;
}
else if (currentOutput < 0)
{
currentOutput = 0;
}
pid->prepreError = pid->preError; // 更新记录
pid->preError = currentError;
pid->preOutput = currentOutput;
return currentOutput;
}
通过定时器每50ms中断一次,在中断中更新一次PID
代码如下:
void TIM6_IRQHandler(void) // 定时器5 的中断函数
{
if (TIM_GetITStatus(TIM6, TIM_IT_Update) == SET)
{
Get_Motor_Speed(); // 获取现在的速度
if (CarStatus == 1) // 只有车在运行时,pid和数据发送才生效
{
PID_L.setValue = PID_L.baseValue + SpeedIncL + angle_SpeedIncL;
PID_R.setValue = PID_R.baseValue + SpeedIncR + angle_SpeedIncR;
if (LSpeedNow < 0)
LSpeedNow = -LSpeedNow;
if (RSpeedNow < 0)
RSpeedNow = -RSpeedNow;
pwm_l = PID(&PID_L, LSpeedNow);
pwm_r = PID(&PID_R, RSpeedNow);
Set_Motor_PWM(pwm_l, pwm_r);
}
TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
}
}
陀螺仪转弯角度控制
为实现准确的90,180度转弯,我们需要陀螺仪来反馈车的转弯信息。我们使用的是JY61模块,它在内部已经实现了MPU6050陀螺仪原始数据的处理,STM32通过串口与JY61通信,就可得到小车的水平角度信息。
在转弯控制时,为使小车能准确的停下,我们使小车在转过目标角度的1/2时,减慢小车的转向速度,使小车能在到达目标角度时立即停下。
结语
通过参加第十一届光电设计大赛,我觉的自己学到很多新的东西,提升了自己的代码编写能力。同时也发现了一些新的问题,知道了视觉方案的优点和不足,为以后的比赛提供了参考经验。
此博客不具有参考意义。
记录于2023年9月。