找回密码
 立即注册

扫一扫,访问微社区

QQ登录

只需一步,快速开始

搜索
MakerMare-创客海社区 门户 查看主题

OpenMV单线、双线寻线、直角与角度判断(内容较多)

发布者: 凡哥 | 发布时间: 2019-3-6 08:05 PM| 查看数: 1451| 评论数: 19|帖子模式

目前支持微信扫码直接登陆 或邮箱验证注册登陆,结交更多好友,享用更多功能,让你轻松玩转社区!

您需要 登录 才可以下载或查看,没有帐号?立即注册

x
本帖子内容较多,如果加载不了,请访问电脑版
find_black_line_algrithm.jpg
openmv单线
[Python] 纯文本查看 复制代码
# Black Grayscale Line Following Example
#
# Making a line following robot requires a lot of effort. This example script
# shows how to do the machine vision part of the line following robot. You
# can use the output from this script to drive a differential drive robot to
# follow a line. This script just generates a single turn value that tells
# your robot to go left or right.
#
# For this script to work properly you should point the camera at a line at a
# 45 or so degree angle. Please make sure that only the line is within the
# camera's field of view.

import sensor, image, time, math

# Tracks a black line. Use [(128, 255)] for a tracking a white line.
GRAYSCALE_THRESHOLD = [(0, 64)]
#设置阈值,如果是黑线,GRAYSCALE_THRESHOLD = [(0, 64)];
#如果是白线,GRAYSCALE_THRESHOLD = [(128,255)]


# Each roi is (x, y, w, h). The line detection algorithm will try to find the
# centroid of the largest blob in each roi. The x position of the centroids
# will then be averaged with different weights where the most weight is assigned
# to the roi near the bottom of the image and less to the next roi and so on.
ROIS = [ # [ROI, weight]
        (0, 100, 160, 20, 0.7), # You'll need to tweak the weights for you app
        (0, 050, 160, 20, 0.3), # depending on how your robot is setup.
        (0, 000, 160, 20, 0.1)
       ]
#roi代表三个取样区域,(x,y,w,h,weight),代表左上顶点(x,y)宽高分别为w和h的矩形,
#weight为当前矩形的权值。注意本例程采用的QQVGA图像大小为160x120,roi即把图像横分成三个矩形。
#三个矩形的阈值要根据实际情况进行调整,离机器人视野最近的矩形权值要最大,
#如上图的最下方的矩形,即(0, 100, 160, 20, 0.7)

# Compute the weight divisor (we're computing this so you don't have to make weights add to 1).
weight_sum = 0 #权值和初始化
for r in ROIS: weight_sum += r[4] # r[4] is the roi weight.
#计算权值和。遍历上面的三个矩形,r[4]即每个矩形的权值。

# Camera setup...
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.GRAYSCALE) # use grayscale.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(30) # Let new settings take affect.
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
#关闭白平衡
clock = time.clock() # Tracks FPS.

while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    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.pixels() > most_pixels:
                    most_pixels = blobs.pixels()
                    #merged_blobs[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_rectangle((0,0,30, 30))
            #将此区域的像素数最大的颜色块画矩形和十字形标记出来
            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-80)/60)
    #角度计算.80 60 分别为图像宽和高的一半,图像大小为QQVGA 160x120.
    #注意计算得到的是弧度值

    # Convert angle in radians to degrees.
    deflection_angle = math.degrees(deflection_angle)
    #将计算结果的弧度值转化为角度值

    # Now you have an angle telling you how much to turn the robot by which
    # incorporates the part of the line nearest to the robot and parts of
    # the line farther away from the robot for a better prediction.
    print("Turn Angle: %f" % deflection_angle)
    #将结果打印在terminal中

    print(clock.fps()) # Note: Your OpenMV Cam runs about half as fast while
    # connected to your computer. The FPS should increase once disconnected.

```


## 算法不足分析


只适用于单条线, 如果是多条线,就不能识别

如何识别直角?

问题其实还有很多, 这个算法比较简单, 但是适用的情景比较局限。




follow_black_line.py (5.76 KB, 下载次数: 28)

最新评论

凡哥 发表于 2019-4-10 10:09:25

OpenMV巡线模块原理讲解-凡哥带你玩转OpenMV.pdf

  
OpenMV巡线模块原理讲解-凡哥带你玩转OpenMV.pdf (1.28 MB, 下载次数: 63)
凡哥 发表于 2019-3-6 20:09:30
OpenMV双线




双轨寻线.py (3.64 KB, 下载次数: 30)
凡哥 发表于 2019-3-6 20:13:04
OpenMV直角

right_angle.png






直角检测.py (3.17 KB, 下载次数: 22)
凡哥 发表于 2019-4-4 14:38:50
OpenMV3R2代码.zip (6.95 KB, 下载次数: 27)
凡哥 发表于 2019-4-4 16:50:51
直线与直角识别_重构林林的代码_By凡哥20180402.zip (5.81 KB, 下载次数: 22)
凡哥 发表于 2019-4-4 16:51:25
[Python] 纯文本查看 复制代码
'''
功能描述

    因为之前飞控结合OpenMV进行的巡线代码比较杂乱,另外也由于新版固件的一些改动,所以凡哥重构了这部分的代码
    在原有的代码的功能基础上进行了拓展。完整功能包括:

    * 直线巡线
    * 直角转弯判定 (左转or右转)
    * T字形路口判定
    * 十字形路口判定

    其中T字形跟十字形可以用作四轴悬停的参考点。
原理介绍
    算法的主要核心在于,讲整个画面分割出来5个ROI区域
    * 上方横向采样
    * 中间横向采样
    * 下方横向采样
    * 左侧垂直采样
    * 右侧垂直采样
    通过判断5个图片的组合关系给出路口类型的判断 详情见文档。

'''
import sensor
import image
import time
import math
import pyb
from pyb import Pin, Timer, UART,LED
from GeometryFeature import GeometryFeature

LED(4).on()
is_debug = True
#--------------感光芯片配置  START -------------------

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)                  # 分辨率为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 = 0
last_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)

凡哥 发表于 2019-4-4 16:51:55
[Python] 纯文本查看 复制代码
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

凡哥 发表于 2019-4-4 17:02:25
请将这main、GeometryFeature这2个文件同时保存到OPenMV中运行,保存方法: QQ截图20190403115820.png
如果 发表于 2019-4-9 14:03:26
凡哥 发表于 2019-4-4 05:02 PM
请将这main、GeometryFeature这2个文件同时保存到OPenMV中运行,保存方法:

凡哥,怎么同时保存啊,不是一次只能固化一个程序吗?固化另一个就替换掉了怎么办
凡哥 发表于 2019-4-9 19:37:14
如果 发表于 2019-4-9 02:03 PM
凡哥,怎么同时保存啊,不是一次只能固化一个程序吗?固化另一个就替换掉了怎么办 ...

还有一种方法就是连接电脑以后有个USB存储设备,保存进去就行了
如果 发表于 2019-4-9 19:54:01
凡哥 发表于 2019-4-9 07:37 PM
还有一种方法就是连接电脑以后有个USB存储设备,保存进去就行了

直接复制粘贴吗?
凡哥 发表于 2019-4-9 20:17:18
是的,但是复制粘贴以后注意得等等,等OpenMV的灯灭了,再安全删除硬件
如果 发表于 2019-4-9 22:48:17
凡哥 发表于 2019-4-9 08:17 PM
是的,但是复制粘贴以后注意得等等,等OpenMV的灯灭了,再安全删除硬件

就是将main.py和Geome这两个文件放进去就行了?不需要点main.py文件在IDE中的将打开的脚本保存到openmv Cam吗?我只打开main。py函数烧进了openmv中,Geome这个我没有固化进去可以吗?
123456 发表于 2019-4-13 10:57:49
对应的C解析代码在哪里
凡哥 发表于 2019-4-13 14:50:22
微信截图_20190413145254.png

一寸灰 发表于 2019-4-16 20:02:55

能分享一下最小车和寻线的代码吗
一寸灰 发表于 2019-4-16 20:09:06

这个双线的代码两个有什么区别吗
凡哥 发表于 2019-4-16 21:10:09
一寸灰 发表于 2019-4-16 08:02 PM
能分享一下最小车和寻线的代码吗

购买OpenMV寻线小车,就有配这个资料哦
凡哥 发表于 2019-4-16 21:11:29
一寸灰 发表于 2019-4-16 08:09 PM
这个双线的代码两个有什么区别吗

一个是V1,另V2