2022年电赛C题小车之OpenMV篇
前言
OpenMV是一个开源，功能强大的机器视觉模块。通过调用函数可以轻松实现色块识别等很多基础功能，巧妙利用这些功能，可以实现用OpenMV代替其他外设。我们省赛的小车上的外设非常精简，只有OpenMV，设置模式的键盘，声音提示的蜂鸣器、领头小车和跟从小车之间通信的蓝牙。一开始担心小车跑的太快，OpenMV回传速率不够，但测试发现OpenMV帧率能达到50~60fps，完全足够了。
也就是说，OpenMV同时实现了循迹、识别停止线和测距三个功能。
一、回顾赛题C题
1.任务
设计一套小车跟随行驶系统，采用 TI 的 MCU，由一辆领头小车和一辆跟随小车组成，要求小车具有循迹功能，且速度在 0.3 ~ 1m/s 可调，能在指定路径上完成行驶操作，行驶场地的路径如图 1 所示。其中，路径上的 A 点为领头小车每次行驶的起始点和终点。当小车完成一次行驶到达终点，领头小车和跟随小车要发出声音提示。领头小车和跟随小车既可以沿着 ABFDE 圆角矩形（ 简称为内圈 ）路径行驶，也可以沿着 ABCDE 的圆角矩形（ 简称为外圈 ）路径行驶。当行驶在内圈 BFD 段时，小车要发出灯光指示。此外，在测试过程中，可以在路径上 E 点所在边的直线区域，由测试专家指定位置放上“等停指示”标识（见图1 左侧），指示领头小车在此处须停车，等待 5 秒后再继续行驶。
2.要求
- 将领头小车放在路径的起始位置 A 点，跟随小车放在其后 20cm 处，设定领头小车速度为 0.3m/s，沿着外圈路径行驶一圈停止，要求：（20 分）
(1) 领头小车的平均速度误差不大于 10%；
(2) 跟随小车能跟随领头小车行驶，全程不能发生小车碰撞；
(3) 完成一圈行驶后领头小车到达 A 点处停车，跟随小车应及时停止，停止时间差不超过 1s，且与领头小车的间距为 20cm，误差不大于 6cm。 - 将领小车放在路径轨迹的起始位置 A 点，跟随小车放在路径上 E 点所在边的直线区域，由测试专家指定的位置，设定领头小车速度为 0.5m/s，沿着外圈路径行驶两圈停止，要求：（20 分）
(1) 领头小车的平均速度误差不大于 10%；
(2) 跟随小车能快速追上领头小车，然后按 20cm 间距跟随领头小车行驶，全程不能发生小车碰撞：
(3) 完成两圈行驶后领头小车达到 A 点停止，跟随小车应及时停止，两车停止的时间差不超过 1s，且与领头小车的间距为 20cm，误差不大于 6cm。 - 将领头小车放在路径的起始位置 A 点，跟随小车放在其后 20cm 处，领头小车和跟随小车连续完成三圈路径的行驶。第一圈领头小车和跟随小车都沿着外圈路径行驶。第二圈领头小车沿着外圈路径行驶，跟随小车沿着内圈路径行驶，实现超车领跑。第三圈跟随小车沿着外圈路径行驶，领头小车沿着内圈路径行驶，实现反超和再次领跑。要求：（30 分）
(1) 全程两个小车行驶平稳，顺利完成两次超车，且不能发生小车碰撞；
(2) 完成三圈行驶后领头小车到达 A 点停止，跟随小车应及时停止，两车停
止的时间差不超过 1s，且与领头小车的间距为 20cm，误差不大于 6cm；
(3) 小车行驶速度可自主设定，但不得低于 0.3m/s，且完成所规定的三圈轨迹行驶所需时间越短越好。 - 由测试专家在路径的 E 点所在边的直线区域指定位置，放上“等停指示”标识。然后，将领头小车放在路径的起始位置 A 点，跟随小车放在其后 20cm 处，设定领头小车速度为 1m/s，沿着外圈路径行驶一圈，行驶中两小车不得发生碰
闯。要求：（20 分）
(1) 领头小车的平均速度误差不大于 10%；
(2) 领头小车达到“等停指示”点停车，停车位置准确，误差不大于 5cm；
(3) 在“等停指示”处停车时间为 5s，误差不超过 1s。
二、OpenMV实现功能思路及代码
1.循迹
OpenMV用于循迹模块的代码网上已经有很多博主分享了。一般思路是：
1.将图像视野分为三部分，上中下，同时寻找最大的黑色色块（也可以寻找其他色块，看要求循什么颜色的线），即调用OpenMV自带的函数img.find_blobs，再筛选出最大色块。
2.通过三个色块中心点的cx和cy值计算当前轨迹相对小车的偏移角，以便于PID计算。
OpenMV的色块识别有很多细节方面，可以更加巧妙的利用，若有时间可以整理出一篇blog记录一下
def car_run():
centroid_sum = 0
#利用颜色识别分别寻找三个矩形区域内的线段
for r in ROIS:
blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True)
# r[0:4] is roi tuple.
#找到视野中的线,merge=true,将找到的图像区域合并成一个
#目标区域找到色块
if blobs:
# Find the index of the blob with the most pixels.
most_pixels = 0
largest_blob = 0
for i in range(len(blobs)):
#目标区域找到的颜色块（线段块）可能不止一个，找到最大的一个，作为本区域内的目标直线
if blobs[i].pixels() > most_pixels:
most_pixels = blobs[i].pixels()
#merged_blobs[i][4]是这个颜色块的像素总数，如果此颜色块像素总数大于 #most_pixels，则把本区域作为像素总数最大的颜色块。更新most_pixels和largest_blob
largest_blob = i
# Draw a rect around the blob.
img.draw_rectangle(blobs[largest_blob].rect())
#将此区域的像素数最大的颜色块画矩形和十字形标记出来
img.draw_cross(blobs[largest_blob].cx(),
blobs[largest_blob].cy())
centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight.
#计算centroid_sum，centroid_sum等于每个区域的最大颜色块的中心点的x坐标值乘本区域的权值
center_pos = (centroid_sum / weight_sum) # Determine center of line.
#中间公式
# Convert the center_pos to a deflection angle. We're using a non-linear
# operation so that the response gets stronger the farther off the line we
# are. Non-linear operations are good to use on the output of algorithms
# like this to cause a response "trigger".
deflection_angle = 0
#机器人应该转的角度
# The 80 is from half the X res, the 60 is from half the Y res. The
# equation below is just computing the angle of a triangle where the
# opposite side of the triangle is the deviation of the center position
# from the center and the adjacent side is half the Y res. This limits
# the angle output to around -45 to 45. (It's not quite -45 and 45).
deflection_angle = -math.atan((center_pos-160)/120)
#角度计算.80 60 分别为图像宽和高的一半，图像大小为QQVGA 160x120.
#注意计算得到的是弧度值
# Convert angle in radians to degrees.
deflection_angle = math.degrees(deflection_angle)
#将计算结果的弧度值转化为角度值
A=deflection_angle+90
return int(A)
#由于数据需要传回给单片机，不适合传输负数，因此+90调节数据
但是该题有内圈和外圈之分，到了分叉口识别单个色块显然不合适，因此在上中下每个区域识别两个最大色块，同时为了避免背景干扰，过滤掉识别到的小色块。这样一来就存在两种情况：
1.没有分叉口，只有一条路径。此时每个区域只有一个色块，计算原理与上面代码相同。
2.有分叉口，会出现两条路径。有可能是最前面区域出现分叉，也可能是两个区域分叉，也可能三个区域都出现分叉。这时候分别计算左边和右边两条路径的偏移角，出现两个色块的区域分别取左右计算，只有一个色块的区域都用这个色块计算。
进入岔路口：
出岔路口（外圈视角，内圈同理）：
代码如下：
def car_run():
centroid_sum = [0,0]
left_center=[-1,-1,-1]
right_center=[-1,-1,-1]
for r in range(3):
blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=ROIS[r][0:4], merge=True,area_threshold=100,margin=3)
if blobs:
max_ID=[-1,-1]#保存两个最大色块的ID
max_ID=find_max(blobs)
img.draw_rectangle(blobs[max_ID[0]].rect())
img.draw_cross(blobs[max_ID[0]].cx(),
blobs[max_ID[0]].cy())
if max_ID[1]!=-1:#如果识别到两个色块
img.draw_rectangle(blobs[max_ID[1]].rect())
img.draw_cross(blobs[max_ID[1]].cx(),
blobs[max_ID[1]].cy())
#区分左边和右边
if blobs[max_ID[0]].cx()<blobs[max_ID[1]].cx():
left_center[r]=blobs[max_ID[0]].cx()
right_center[r]=blobs[max_ID[1]].cx()
else:
left_center[r]=blobs[max_ID[1]].cx()
right_center[r]=blobs[max_ID[0]].cx()
else:
left_center[r]=right_center[r]=blobs[max_ID[0]].cx()
centroid_sum[0] += left_center[r] * ROIS[r][4]
centroid_sum[1] += right_center[r] * ROIS[r][4]
center_pos =[0,0]
center_pos[0] = (centroid_sum[0] / weight_sum)
center_pos[1] = (centroid_sum[1] / weight_sum)
deflection_angle = [0,0]
deflection_angle[0] = -math.atan((center_pos[0]-80)/60)
deflection_angle[1] = -math.atan((center_pos[1]-80)/60)
#使用的QQVGA像素是160*120，因此中心点是（80,60）
deflection_angle[0] = math.degrees(deflection_angle[0])
deflection_angle[1] = math.degrees(deflection_angle[1])
if center_pos[0]==center_pos[1]==0:
deflection_angle[1]=deflection_angle[0]=0
A=[int(deflection_angle[0])+90,int(deflection_angle[1])+90]
return A
将数据处理好后，发送给单片机，剩下的就交给单片机处理了。这里也简单阐述一下单片机代码思路：用小车视角走一遍内道和外道，发现走外道时取的都是右边的数据，走内道时用的都是左边的数据。如此一来，如果走直线时传回的数据左边右边相等，那么单片机只需要根据内道外道模式的选择来选择左边偏移角或者右边偏移角，即可以实现内道和外道的行驶。
2.识别停止线
停止线有5cm宽，利用这个特征直接在识别偏移角的时候解决了停止线的识别问题。后来根据这个思路同时也解决了等待停止线的问题。
简单来说，就是如果一个色块像素面积大于一定值就认为是停止线。
1.首先是在左偏移角等于右偏移角，即没有分叉口时进行停止线识别，避免分叉口的影响。
2.利用的并不是色块面积的大小，这样会造成误判。应该使用的是像素面积的大小，blob.pixels()函数。（有空多读读手册会有很多意想不到的收获）
3.摄像头的倾斜角度、离地面的高度都会影响blob.pixels()的值，而且倾斜角度越大，上中下三个区域识别到的停止线像素大小不一样，因此要设置三个阈值。
4.这样识别停止线未免有点太简单粗暴，会出现误判，因此还需要单片机的配合。代码后面会简单描述一下单片机的思路。
def car_run():
centroid_sum = [0,0]
left_center=[-1,-1,-1]
right_center=[-1,-1,-1]
flag_cross=0
flag_Stop=0 #停止线标志
flag_Wait=[0,0]#等停线标志
for r in range(3):
blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=ROIS[r][0:4], merge=True,area_threshold=100,margin=3)
if blobs:
max_ID=[-1,-1]
max_ID=find_max(blobs)
img.draw_rectangle(blobs[max_ID[0]].rect())
img.draw_cross(blobs[max_ID[0]].cx(),
blobs[max_ID[0]].cy())
if max_ID[1]!=-1:
img.draw_rectangle(blobs[max_ID[1]].rect())
flag_cross=1
img.draw_cross(blobs[max_ID[1]].cx(),
blobs[max_ID[1]].cy())
if blobs[max_ID[0]].cx()<blobs[max_ID[1]].cx():
left_center[r]=blobs[max_ID[0]].cx()
right_center[r]=blobs[max_ID[1]].cx()
else:
left_center[r]=blobs[max_ID[1]].cx()
right_center[r]=blobs[max_ID[0]].cx()
else:
#print(blobs[max_ID[0]].pixels(),blobs[max_ID[0]].w())
if flag_cross==0:
if blobs[max_ID[0]].pixels()>range_stop[r]:#range_stop为三个区域停止线的像素阈值
flag_Stop=r+1 #停止线标志的值为3,2，1，表示所在区域
if blobs[max_ID[0]].w()>range_wait[r]:
flag_Wait[0]=flag_Wait[0]+1#等待停止线同理
left_center[r]=right_center[r]=blobs[max_ID[0]].cx()
centroid_sum[0] += left_center[r] * ROIS[r][4]
centroid_sum[1] += right_center[r] * ROIS[r][4]
center_pos =[0,0]
center_pos[0] = (centroid_sum[0] / weight_sum)
center_pos[1] = (centroid_sum[1] / weight_sum)
if flag_Wait[0]==2:
flag_Wait[1]=1
deflection_angle = [0,0]
K=0.8
deflection_angle[0] = -math.atan((center_pos[0]-80)/60)
deflection_angle[1] = -math.atan((center_pos[1]-80)/60)
deflection_angle[0] = math.degrees(deflection_angle[0])
deflection_angle[1] = math.degrees(deflection_angle[1])
if center_pos[0]==center_pos[1]==0:
deflection_angle[1]=deflection_angle[0]=0
A=[int(deflection_angle[0])+90,int(deflection_angle[1])+90,flag_Stop,flag_Wait[1]]
return A
单片机思路：小车是逐渐靠近停止线的，因此停止线所在的位置从远到近，因此返回的值是3->2->1,当顺着收到3,2，1即认为识别到停止线，立刻停止。顺着收到并不意味着连续收到，因为有部分视野区域是不识别色块的，即视野盲区，停止线会经过视野盲区，返回0。且这钟方法也可以顺便避免在分岔口的误判。
等待停止线原理和停止线相似，识别等待停止线时也会识别到停止线，因此很取巧地在要求四时，同时识别到等待停止线和停止线时当等待停止线处理，识别完一次等待停止线后即便再看到也不作处理。也就是第一次停止是等待停止，第二次停止是停止。
3.距离识别
其实一开始就想用OpenMV测距，使用的是自带的AprilTag标记跟踪，相当于识别一个标记得出距离。当时是粘了个纸板在小车背后，再贴上这个标志。
实验发现并不可行，对我们的小车来说，摄像头是放在上面。（当然如果是一辆车有很多摄像头装在不同地方也行，可是俺们没钱）这个角度很刁钻，即便用的很小的AprilTag标记，也是两车距离到了20cm才能勉强看完整。看到就已经20cm了，已经比较近了，万一没看见岂不是直接撞上了。
基于这个想法，既然能看到就比较近了，那直接搞一个色块识别。OpenMV的色块识别真的是让我又恨又爱，调阈值调的有多痛苦，这色块识别就有多好用。我们给小车后面贴了一整块蓝色的板子，顺便把小车轮子也盖住了，避免了后车将前车轮子识别成轨迹。
识别色块判断距离也是想了很多方法，色块面积、像素面积、色块长度、色块中心点等等，最后拍拍脑袋，直接用色块的下边沿y值，再根据实际测量设置参数k，调节成距离值。LAB色域中B的负数那边就是蓝色。当时蓝色区域取尽，L和A也取尽，就不用担心光线变化对颜色的影响了。
LAB这里简单说一下：L*代表明度，取值0~100， a*代表从绿色到红色的分量 ，取值-128~127，b*代表从蓝色到黄色的分量，取值-128~127
测距大概就这么多，剩下的调速就交给单片机处理了。
代码：
blobs=img.find_blobs([(30,60,-30,-10,-25,-12)],pixels_threshold=300,area_threshold=300,merge=False)
max_size=0
if blobs:
for blob in blobs:
if blob.cy()+0.5*blob.h() > max_size:
img.draw_rectangle(blob.rect(),(255,0,0))
max_size = blob.cy()+0.5*blob.h()
row_data[2]=int(k*(120-max_size))
总结
OpenMV的参考文档需要多多看，多多用，灵活调用成员函数，很多问题其实没有很复杂。其他的就是好好利用单片机的算力，处理好OpenMV传递的数据。
加上MSP的代码：https://download.csdn.net/download/weixin_52385589/86393773?spm=1001.2014.3001.5503
import sensor, image, time, math
from pyb import UART,LED
LED(3).on()
uart = UART(3, 115200, timeout_char=1000)
u_start=bytearray([0xb3,0xb3])
u_over=bytearray([0x0d,0x0a])
GRAYSCALE_THRESHOLD = [(-125, 20, -21, 13, -28, 14)]#巡线的阈值
ROIS = [
(0, 90, 160, 20, 0.7),
(0, 050, 160, 20, 0.4),
(0, 000, 160, 20, 0.05)
]#三个区域
weight_sum = 0
range_stop=[390,190,100]#停止线像素最小值
range_wait=[60,40,0] #等待停止线像素最小值
for r in ROIS: weight_sum += r[4]
#摄像头设置
sensor.reset()
sensor.set_contrast(1)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(30)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
clock = time.clock()
sensor.set_vflip(True)
sensor.set_hmirror(True)
thresholds=[(-100, 72, -128, -16, -128, 127)]
#该阈值用于测距
#寻找两个最大的色块，ID存在max_ID中，便于调用
def find_max(blobs):
max_size=[0,0]
max_ID=[-1,-1]
for i in range(len(blobs)):
if blobs[i].pixels()>max_size[0]:
max_ID[1]=max_ID[0]
max_size[1]=max_size[0]
max_ID[0]=i
max_size[0]=blobs[i].pixels()
elif blobs[i].pixels()>max_size[1]:
max_ID[1]=i
max_size[1]=blobs[i].pixels()
return max_ID
def car_run():
centroid_sum = [0,0]
left_center=[-1,-1,-1] #存放左边色块的中心cx值用于计算左边的偏移角
right_center=[-1,-1,-1] #存放右边色块的中心cx值用于计算右边的偏移角
flag_cross=0 #是否有分岔口
flag_Stop=0 #停止标志
flag_Wait=[0,0] #等待停止标志
for r in range(3): #三个区域分别寻找色块
blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=ROIS[r][0:4], merge=True,area_threshold=100,margin=3)
if blobs:
max_ID=[-1,-1]
max_ID=find_max(blobs) #找最大色块
img.draw_rectangle(blobs[max_ID[0]].rect())
img.draw_cross(blobs[max_ID[0]].cx(),
blobs[max_ID[0]].cy())
if max_ID[1]!=-1: #如果有两个色块，即有分岔口，分成左边右边存入数组
img.draw_rectangle(blobs[max_ID[1]].rect())
flag_cross=1
img.draw_cross(blobs[max_ID[1]].cx(),
blobs[max_ID[1]].cy())
if blobs[max_ID[0]].cx()<blobs[max_ID[1]].cx():
left_center[r]=blobs[max_ID[0]].cx()
right_center[r]=blobs[max_ID[1]].cx()
else:
left_center[r]=blobs[max_ID[1]].cx()
right_center[r]=blobs[max_ID[0]].cx()
else: #只有一个色块
#print(blobs[max_ID[0]].pixels(),blobs[max_ID[0]].w())
if flag_cross==0: #没有分岔口，进行判断停止线
if blobs[max_ID[0]].pixels()>range_stop[r]:
flag_Stop=r+1
if blobs[max_ID[0]].w()>range_wait[r]:
flag_Wait[0]=flag_Wait[0]+1
left_center[r]=right_center[r]=blobs[max_ID[0]].cx()
centroid_sum[0] += left_center[r] * ROIS[r][4] #乘权值
centroid_sum[1] += right_center[r] * ROIS[r][4]
center_pos =[0,0]
center_pos[0] = (centroid_sum[0] / weight_sum)
center_pos[1] = (centroid_sum[1] / weight_sum)
if flag_Wait[0]==2:
flag_Wait[1]=1
deflection_angle = [0,0]
deflection_angle[0] = -math.atan((center_pos[0]-80)/60)#计算角度
deflection_angle[1] = -math.atan((center_pos[1]-80)/60)
deflection_angle[0] = math.degrees(deflection_angle[0])#弧度制换成角度制
deflection_angle[1] = math.degrees(deflection_angle[1])
if center_pos[0]==center_pos[1]==0:
deflection_angle[1]=deflection_angle[0]=0
A=[int(deflection_angle[0])+90,int(deflection_angle[1])+90,flag_Stop,flag_Wait[1]]
return A
def degrees(radians):
return (180 * radians) / math.pi
k=1
while(True):
times=0
clock.tick()
img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)#不断拍照，进行鱼眼校正
row_data=[0,0,0,0,0]
row_data[0],row_data[1],row_data[3],row_data[4]=car_run()
blobs=img.find_blobs([(30,60,-30,-10,-25,-12)],pixels_threshold=300,area_threshold=300,merge=False)
max_size=0
if blobs:
for blob in blobs:
if blob.cy()+0.5*blob.h() > max_size:
img.draw_rectangle(blob.rect(),(255,0,0))
max_size = blob.cy()+0.5*blob.h()
row_data[2]=int(k*(120-max_size))#计算距离，k可调
print(row_data)
#传输数据给单片机
uart_buf = bytearray(row_data)
uart.write(u_start)
uart.write(uart_buf)
uart.write(u_over)