双轨寻线

运行效果

源代码

import sensor, image, time, math

# Tracks a black line. Use [(128, 255)] for a tracking a white line.
GRAYSCALE_THRESHOLD = [(0, 40)]
#设置阈值,如果是黑线,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.
# 在这里修改取样区
ROI = (0, 100, 320, 40)


# Camera setup...
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.GRAYSCALE) # use grayscale.
sensor.set_framesize(sensor.QVGA) # use QVGA for speed. 320* 240
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.


# 选择排序的属性, 这里我们选取的是色块宽度
def get_blob_value(blob):
    return blob.w()

# 选择最大的两个黑线色块
def compare_blob(blob1, blob2):
    comp_result = get_blob_value(blob1) - get_blob_value(blob2)

    if comp_result > 3:
        return 1
    elif comp_result < -3:
        return -1
    else:
        return 0

def get_direction(left_blob, right_blob):
    # 根据左中右三块白色部分,计算出角度值
    # ratio < 0 左拐
    # ratio > 0 右拐

    MAX_WIDTH = 320
    # 调节theta来设置中间宽度的比重, theta越高ratio越靠近0
    # 需要根据赛道宽度与摄像头高度重新设定到合适大小
    theta = 0.01
    # 这里的b是为了防止除数是0的情况发生, 设定一个小一点的值
    b = 3
    x1 = left_blob.x() - int(0.5 * left_blob.w())
    x2 = right_blob.x() + int(0.5 * right_blob.w())

    w_left = x1
    w_center = math.fabs(x2 - x1)
    w_right = math.fabs(MAX_WIDTH - x2)

    direct_ratio = (w_left + b + theta * w_center) / (w_left + w_right + 2 * b + 2 * theta * w_center) - 0.5

    return direct_ratio

def get_top2_blobs(blobs):
    # 找到最大的两个色块, 返回左色块与右色块
    for blob in blobs:
        pass
        #print(blob)
        # img.draw_rectangle(blob.rect())

    if len(blobs) < 2:
        # 已偏离轨道
        return (None, None)

    top_blob1 = blobs[0]
    top_blob2 = blobs[1]

    if compare_blob(top_blob1, top_blob2) == -1:
        top_blob1, top_blob2 = top_blob2, top_blob1


    for i in range(2, len(blobs)):
        if compare_blob(blobs[i], top_blob1) == 1:
            top_blob2 = top_blob1
            top_blob1 = blobs[i]
        elif compare_blob(blobs[i], top_blob2) == 1:
            top_blob2 = blobs[i]

    if top_blob1.cx() > top_blob2.cx():
        return (top_blob2, top_blob1)
    else:
        return (top_blob1, top_blob2)

def draw_direct(img, direct_ratio):
    # 可视化方向(左右) 标识及其幅度
    img.draw_circle(160, 80, 5)
    img.draw_line((160, 80, int(160 + direct_ratio * 200), 80))

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

    blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=ROI, merge=True)

    #目标区域找到直线
    if blobs:
        left_blob, right_blob = get_top2_blobs(blobs)

        if(left_blob == None or right_blob == None):
            print("Out Of Range")
            continue
        else:
            print("-------")
            print("left blob")
            print(left_blob)
            print("right blob")
            print(right_blob)
            img.draw_rectangle(left_blob.rect())
            img.draw_cross(left_blob.cx(), left_blob.cy())

            img.draw_rectangle(right_blob.rect())
            img.draw_cross(right_blob.cx(), right_blob.cy())

            direct_ratio = get_direction(left_blob, right_blob)
            img.draw_string(10, 10, "%.2f"%direct_ratio)
            draw_direct(img, direct_ratio)
    img.draw_rectangle(ROI, color=(255, 0, 0))

TODO

 添加算法简述  添加图示

Copyright 杭州云江科技有限公司 2017 all right reserved,powered by Gitbook该文件修订时间: 2019-04-06 01:18:49

results matching ""

    No results matching ""