水下机器人使用openmv巡线
- 使用色块进行巡线
- 使用findblobs进行颜色识别
- 使用快速线性回归循迹
- 自动颜色跟踪
使用色块进行巡线
使用findblobs进行颜色识别
GeometryFeature.py
class GeometryFeature:
def __init__(self, img):
self.img = img
@staticmethod
def trans_line_format(line):
'''
将原来由两点坐标确定的直线，转换为 y = ax + b 的格式
'''
x1 = line.x1()
y1 = line.y1()
x2 = line.x2()
y2 = line.y2()
if x1 == x2:
# 避免完全垂直，x坐标相等的情况
x1 += 0.1
# 计算斜率 a
a = (y2 - y1) / (x2 - x1)
# 计算常数项 b
# y = a*x + b -> b = y - a*x
b = y1 - a * x1
return a,b
@staticmethod
def calculate_angle(line1, line2):
'''
利用四边形的角公式， 计算出直线夹角
'''
angle = (180 - abs(line1.theta() - line2.theta()))
if angle > 90:
angle = 180 - angle
return angle
@staticmethod
def find_verticle_lines(lines, angle_threshold=(70, 90)):
'''
寻找相互垂直的两条线
'''
return GeometryFeature.find_interserct_lines(lines, angle_threshold=angle_threshold)
@staticmethod
def find_interserct_lines(lines, angle_threshold=(10,90), window_size=None):
'''
根据夹角阈值寻找两个相互交叉的直线， 且交点需要存在于画面中
'''
line_num = len(lines)
for i in range(line_num -1):
for j in range(i, line_num):
# 判断两个直线之间的夹角是否为直角
angle = GeometryFeature.calculate_angle(lines[i], lines[j])
# 判断角度是否在阈值范围内
if not(angle >= angle_threshold[0] and angle <= angle_threshold[1]):
continue
# 判断交点是否在画面内
if window_size is not None:
# 获取串口的尺寸 宽度跟高度
win_width, win_height = window_size
# 获取直线交点
intersect_pt = GeometryFeature.calculate_intersection(lines[i], lines[j])
if intersect_pt is None:
# 没有交点
continue
x, y = intersect_pt
if not(x >= 0 and x < win_width and y >= 0 and y < win_height):
# 交点如果没有在画面中
continue
return (lines[i], lines[j])
return None
@staticmethod
def calculate_intersection(line1, line2):
'''
计算两条线的交点
'''
a1 = line1.y2() - line1.y1()
b1 = line1.x1() - line1.x2()
c1 = line1.x2()*line1.y1() - line1.x1()*line1.y2()
a2 = line2.y2() - line2.y1()
b2 = line2.x1() - line2.x2()
c2 = line2.x2() * line2.y1() - line2.x1()*line2.y2()
if (a1 * b2 - a2 * b1) != 0 and (a2 * b1 - a1 * b2) != 0:
cross_x = int((b1*c2-b2*c1)/(a1*b2-a2*b1))
cross_y = int((c1*a2-c2*a1)/(a1*b2-a2*b1))
return (cross_x, cross_y)
return Nonemain.py
'''
原理介绍
算法的主要核心在于，讲整个画面分割出来5个ROI区域
* 上方横向采样
* 中间横向采样
* 下方横向采样
* 左侧垂直采样
* 右侧垂直采样
通过判断5个图片的组合关系给出路口类型的判断
'''
import sensor
import image
import time
import math
import pyb
from pyb import Pin, Timer,LED
from GeometryFeature import GeometryFeature
LED(4).on()
is_debug = True
DISTORTION_FACTOR = 1.5#畸变矫正因子
IMG_WIDTH = 64#像素点宽度
IMG_HEIGHT = 64#像素点高度
def init_sensor():#初始化感光器
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)#设置为灰度图
sensor.set_framesize(sensor.B64X64) #设置像素大小
sensor.skip_frames(time=2000) #最大像素点个数
sensor.set_auto_gain(False) #颜色追踪关闭自动增益
sensor.set_auto_whitebal(False) #颜色追踪关闭自动白平衡
init_sensor()
INTERSERCT_ANGLE_THRESHOLD = (45,90) #设置角度阈值
LINE_COLOR_THRESHOLD = [(0, 120)] #设置巡线的颜色阈值
ROIS = { #ROIS将镜头的画面分割为5个区域分别找寻色块
'down': (0, 55, 64, 8),
'middle': (0, 28, 64, 8),
'up': (0, 0, 64, 8),
'left': (0, 0, 8, 64),
'right': (56, 0, 8, 64)
}
def find_blobs_in_rois(img):
'''
在ROIS中寻找色块，获取ROI中色块的中心区域与是否有色块的信息
'''
global ROIS
global is_debug
roi_blobs_result = {}
for roi_direct in ROIS.keys():
roi_blobs_result[roi_direct] = {
'cx': -1,
'cy': -1,
'blob_flag': False
}
for roi_direct, roi in ROIS.items():
blobs=img.find_blobs(LINE_COLOR_THRESHOLD, roi=roi, merge=True, pixels_area=10)
if len(blobs) == 0:
continue
largest_blob = max(blobs, key=lambda b: b.pixels())
x,y,width,height = largest_blob[:4]
if not(width >=5 and width <= 15 and height >= 5 and height <= 15):
continue
roi_blobs_result[roi_direct]['cx'] = largest_blob.cx()
roi_blobs_result[roi_direct]['cy'] = largest_blob.cy()
roi_blobs_result[roi_direct]['blob_flag'] = True
if is_debug:
img.draw_rectangle((x,y,width, height), color=(255))
return roi_blobs_result
def visualize_result(canvas, cx_mean, cx, cy, is_turn_left, is_turn_right, is_t, is_cross):
'''
可视化结果
'''
if not(is_turn_left or is_turn_right or is_t or is_cross):
mid_x = int(canvas.width()/2)
mid_y = int(canvas.height()/2)
canvas.draw_circle(int(cx_mean), mid_y, 5, color=(255))
canvas.draw_circle(mid_x, mid_y, 8, color=(0))
canvas.draw_line((mid_x, mid_y, int(cx_mean), mid_y), color=(255))
turn_type = 'N' #判断为直线
if is_t or is_cross:
canvas.draw_cross(int(cx), int(cy), size=10, color=(255))
canvas.draw_circle(int(cx), int(cy), 5, color=(255))
if is_t:
turn_type = 'T' #判断为T字路口
elif is_cross:
turn_type = 'C' #判断为十字路口
elif is_turn_left:
turn_type = 'L' #判断为左转
elif is_turn_right:
turn_type = 'R' #判断为右转
canvas.draw_string(0, 0, turn_type, color=(0))
last_cx = 0
last_cy = 0
while True:
img = sensor.snapshot() #拍取一张图片
img.lens_corr(DISTORTION_FACTOR) #进行镜头畸形矫正，里面的参数是进行鱼眼矫正的程度
lines = img.find_lines(threshold=1000, theta_margin = 50, rho_margin = 50)
intersect_pt = GeometryFeature.find_interserct_lines(lines, angle_threshold=(45,90), window_size=(IMG_WIDTH, IMG_HEIGHT))
if intersect_pt is None:
intersect_x = 0
intersect_y = 0
else:
intersect_x, intersect_y = intersect_pt
reslut = find_blobs_in_rois(img)
is_turn_left = False
is_turn_right = False
if (not reslut['up']['blob_flag'] ) and reslut['down']['blob_flag']:
if reslut['left']['blob_flag']:
is_turn_left = True
if reslut['right']['blob_flag']:
is_turn_right = True
is_t = False
is_cross = False
cnt = 0
for roi_direct in ['up', 'down', 'left', 'right']:
if reslut[roi_direct]['blob_flag']:
cnt += 1
is_t = cnt == 3
is_cross = cnt == 4
cx_mean = 0
for roi_direct in ['up', 'down', 'middle']:
if reslut[roi_direct]['blob_flag']:
cx_mean += reslut[roi_direct]['cx']
else:
cx_mean += IMG_WIDTH / 2
cx_mean /= 3 #表示为直线时区域的中心x坐标
cx = 0 #cx,cy表示当测到为T型或者十字型的时候计算的交叉点的坐标
cy = 0
if is_cross or is_t:
cnt = 0
for roi_direct in ['up', 'down']:
if reslut[roi_direct]['blob_flag']:
cnt += 1
cx += reslut[roi_direct]['cx']
if cnt == 0:
cx = last_cx
else:
cx /= cnt
cnt = 0
for roi_direct in ['left', 'right']:
if reslut[roi_direct]['blob_flag']:
cnt += 1
cy += reslut[roi_direct]['cy']
if cnt == 0:
cy = last_cy
else:
cy /= cnt
last_cx = cx
last_cy = cy
if is_debug:
visualize_result(img, cx_mean, cx, cy, is_turn_left, is_turn_right, is_t, is_cross)使用快速线性回归循迹
THRESHOLD = (5, 70, -23, 15, -57, 0) # Grayscale threshold for dark things...
import sensor, image, time
from pyb import LED
#开启LED灯进行补光
LED(1).on()
LED(2).on()
LED(3).on()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)#设置图像为彩模式
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames(time = 2000) # WARNING: If you use QQVGA it may take seconds
clock = time.clock() # to process a frame sometimes.
while(True):
clock.tick()
img = sensor.snapshot().binary([THRESHOLD])
line = img.get_regression([(100,100,0,0,0,0)], robust = True)
if (line):
rho_err = abs(line.rho())-img.width()/2
if line.theta()>90:
theta_err = line.theta()-180
else:
theta_err = line.theta()
img.draw_line(line.line(), color = 127)
print(rho_err,line.magnitude(),rho_err)
#print(clock.fps())自动颜色跟踪
由于每次开启小车之前都需要手动调试阈值，因此，我们可以使用自动颜色追踪，只需要在每次开始时将小车放到正中央进行阈值矫正。
# 自动灰度颜色追踪例程
#
# 这个例子展示了使用OpenMV的单色自动灰度色彩跟踪。
import sensor, image, time
print("Letting auto algorithms run. Don't put anything in front of the camera!")
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # 颜色跟踪必须自动增益
sensor.set_auto_whitebal(False) # 颜色跟踪必须关闭白平衡
clock = time.clock()
# 捕捉图像中心的颜色阈值。
r = [(320//2)-(50//2), (240//2)-(50//2), 50, 50] # 50x50 center of QVGA.
print("Auto algorithms done. Hold the object you want to track in front of the camera in the box.")
print("MAKE SURE THE COLOR OF THE OBJECT YOU WANT TO TRACK IS FULLY ENCLOSED BY THE BOX!")
for i in range(60):
img = sensor.snapshot()
img.draw_rectangle(r)
print("Learning thresholds...")
threshold = [128, 128] # Middle grayscale values.中间灰度值。
for i in range(60):
img = sensor.snapshot()
hist = img.get_histogram(roi=r)
lo = hist.get_percentile(0.01) # 获取1％范围的直方图的CDF（根据需要调整）！
hi = hist.get_percentile(0.99) # 获取99％范围的直方图的CDF（根据需要调整）！
# 平均百分位值。
threshold[0] = (threshold[0] + lo.value()) // 2
threshold[1] = (threshold[1] + hi.value()) // 2
for blob in img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10):
img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy())
img.draw_rectangle(r)
print("Thresholds learned...")
print("Tracking colors...")
while(True):
clock.tick()
img = sensor.snapshot()
for blob in img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10):
img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy())
print(clock.fps())
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
