openmv（opencv）无人机巡线代码（参考用，还没有上机）
最近在做无人机巡线相关的东西，写了很多的代码。但是实现得很拙劣，目前这个文章里面
https://download.makermare.com/upload/tid/35/file/c5cb33d025de3b316580ec7fbc47d4a8.zip原文找不到了，是国内外在OpenMV上面实现得唯一得无人机循迹代码，欢迎主人来认领~
功能描述
完整功能包括：
* 直线巡线
* 直角转弯判定 (左转or右转)
* T字形路口判定
* 十字形路口判定
其中Ｔ字形跟十字形可以用作四轴悬停的参考点。
原理介绍
算法的主要核心在于，讲整个画面分割出来5个ROI区域
* 上方横向采样
* 中间横向采样
* 下方横向采样
* 左侧垂直采样
* 右侧垂直采样
通过判断5个图片的组合关系给出路口类型的判断
'''功能描述 完整功能包括： * 直线巡线 * 直角转弯判定 (左转or右转) * T字形路口判定 * 十字形路口判定 其中Ｔ字形跟十字形可以用作四轴悬停的参考点。原理介绍 算法的主要核心在于，讲整个画面分割出来5个ROI区域 * 上方横向采样 * 中间横向采样 * 下方横向采样 * 左侧垂直采样 * 右侧垂直采样 通过判断5个图片的组合关系给出路口类型的判断'''import sensorimport imageimport timeimport mathimport pybfrom pyb import Pin, Timer, UART,LEDfrom GeometryFeature import GeometryFeature
LED(4).on()is_debug = True#--------------感光芯片配置 START -------------------
DISTORTION_FACTOR = 1.5 # 设定畸变系数IMG_WIDTH = 64IMG_HEIGHT = 64def init_sensor(): ''' 初始化感光芯片 ''' sensor.reset() sensor.set_pixformat(sensor.GRAYSCALE) sensor.set_framesize(sensor.B64X64) # 分辨率为B64X64 sensor.skip_frames(time=2000) sensor.set_auto_gain(False) # 颜色追踪关闭自动增益 sensor.set_auto_whitebal(False) # 颜色追踪关闭白平衡
init_sensor()#--------------感光芯片配置 END -------------------
#--------------串口UART部分 START -------------------uart = pyb.UART(3,115200,timeout_char = 1000) #串口初始化
def get_symbol(num): ''' 根据数值正负，返回数值对应的符号 正数：'+’， 负数'-’ 主要为了方便C语言解析待符号的数值。 ''' if num >=0: return '+' else: return '-'
def data_format_wrapper(yaw_angle, sum_x, sum_y, cx_mean, cx, cy, is_left_angle, last_x, last_y): ''' 根据通信协议封装数据 TODO 重新编写通信协议 与配套C解析代码
yaw_angle,sum_x, sum_y 没有用到 cx_mean: roi 1 2 3 对应的bolb中心的x坐标加权平均， 如果没有就补32 都没有就赋值为原来的值 cx : roi 1 3 blob 中心坐标的加权平均， 如果一方丢失， 就赋值为另外一个 都没有就赋值为原来的值 cy : roi 4 5 blob 中心坐标的加权平均，如果一方丢失， 就赋值为另外一个， 都没有就赋值为原来的值 is_left_angle :代表是否有直角，！！！信息丢失 其实，可以用一个字符来表示，是左转还是右转 T(T字形) L(Left) R(Right) last_x, last_y 是两个直线的交叉点的坐标 改写为 intersect_x，intersect_y ''' args = [ get_symbol(yaw_angle), # 偏航角符号 abs(int(yaw_angle)), # 偏航角 get_symbol(sum_x), # 光流数据sux_x的符号 abs(int(sum_x)), # 光流数据sum_x get_symbol(sum_y), # 光流数据sum_y的符号 abs(int(sum_y)), # 光流数据 sum_y int(cx_mean), # x的中心，三个取样区域色块中心x坐标的平均值 int(cx), int(cy), int(is_left_angle), int(last_x), int(last_y) ] # 将数值列表按照通信协议，转换为待发送的字符 info = 's%c%.2d%c%.2d%c%.2d%.2d%.2d%.2d%.2d%.2d%.2d#'%tuple(args) global is_debug if is_debug: print('s%c%.2d%c%.2d%c%.2d | cx_mean=%.2d cx=%.2d cy=%.2d Turn Left: %.2d | %.2d%.2d#'%tuple(args)) return info#--------------串口UART部分 END -------------------
#--------------定时器部分 START -------------------
is_need_send_data = False # 是否需要发送数据的信号标志def uart_time_trigger(timer): ''' 串口发送数据的定时器，定时器的回调函数 ''' global is_need_send_data is_need_send_data = True
# 初始化定时器 频率为20HZ 每秒执行20次tim = Timer(4, freq=20)# 设定定时器的回调函数tim.callback(uart_time_trigger)#--------------定时器部分 END -------------------
#--------------直线与直角检测部分 START -------------------
INTERSERCT_ANGLE_THRESHOLD = (45,90)
# 直线灰度图颜色阈值LINE_COLOR_THRESHOLD = [(0, 120)]# 如果直线是白色的，阈值修改为：# LINE_COLOR_THRESHOLD = [(128, 255)]
# 取样窗口ROIS = { 'down': (0, 55, 64, 8), # 横向取样-下方 1 'middle': (0, 28, 64, 8), # 横向取样-中间 2 'up': (0, 0, 64, 8), # 横向取样-上方 3 'left': (0, 0, 8, 64), # 纵向取样-左侧 4 'right': (56, 0, 8, 64) # 纵向取样-右侧 5}
def find_blobs_in_rois(img): ''' 在ROIS中寻找色块，获取ROI中色块的中心区域与是否有色块的信息 ''' global ROIS global is_debug
roi_blobs_result = {} # 在各个ROI中寻找色块的结果记录 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) # 绘制x的均值点 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: # 十字形或者T形 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))
#--------------直线与直角检测部分 END -------------------
#---------------------MAIN-----------------------last_cx = 0last_cy = 0
while True: if not is_need_send_data: # 不需要发送数据 continue is_need_send_data = False
# 拍摄图片 img = sensor.snapshot() # 去除图像畸变 img.lens_corr(DISTORTION_FACTOR) # 创建画布 # canvas = img.copy() # 为了IDE显示方便，直接在代码结尾 用IMG绘制
# 注意：林林的代码里 计算直线之间的交点的代码没有用到 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
# 判断是否为T形的轨道 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 用于确定视角中的轨道中心 # 用于表示左右偏移量 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
# cx, cy 只有在T形区域检测出来的时候才有用， # 用于确定轨道中圆形的大致区域， 用于定点， 是计算圆心的一种近似方法
cx = 0 cy = 0
if is_cross or is_t: # 只在出现十字形或者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
# 为了兼容之前的程序，按照之前的数据通信协议发送 # 林林的代码里没有用到的变量均设为 0， 且巡线演示历程中，只用到了左转 # 发送信息格式，可以自行改造, 例如 添加 is_turn_left info = data_format_wrapper(0, 0, 0, cx_mean, cx, cy, is_turn_left, 0, 0) uart.write(info)
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)
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 None对于源码级别的分析，在过几天的文章里面会有。这篇用来记录使用
两份py文件，可以参考。可以自己去调试
非原创，用来记录而已
赞 (0)