基于 OpenCV 的巡线自动驾驶

在本章教程中我们会在使用 OpenCV 的基础功能来从画面中检测到画面中黄色（默认颜色）的线条，并通过检测该黄色线条的位置来控制底盘转向（本例程中的底盘不会移动，本例程只在画面中展示 OpenCV 的算法），我们这里由于安全方面的原因不会讲运动控制结合在例程里面，因为该功能受外界因素影响比较大，用户需完整理解代码功能后在增加对应的运动控制功能。

如果你想通过本例程来控制机器人移动，请结合前面的 Python 底盘运动控制章节来添加相关的运动控制函数（我们的开源例程位于 robot_ctrl.py 中）。

准备工作

由于产品开机默认会自动运行主程序，主程序会占用摄像头资源，这种情况下是不能使用本教程的，需要结束主程序或禁止主程序自动运行后再重新启动机器人。

这里需要注意的是，由于机器人主程序中使用了多线程且由 crontab 配置开机自动运行，所以常规的 sudo killall python 的方法通常是不起作用的，所以我们这里介绍禁用主程序自动运行的方法。

如果你已经禁用了机器人主程序的开机自动运行，则不需要执行下面的结束主程序章节。

结束主程序

1. 点击上方本页面选项卡旁边的 “+”号，会打开一个新的名为 Launcher 的选项卡。

2. 点击 Other 内的 Terminal，打开终端窗口。

3. 在终端窗口内输入 bash 后按回车。

4. 现在你可以使用 Bash Shell 来控制机器人了。

5. 输入命令： sudo killall -9 python

例程

以下代码块可以直接运行：

1. 选中下面的代码块

2. 按 Shift + Enter 运行代码块

3. 观看实时视频窗口

4. 按 STOP 关闭实时视频，释放摄像头资源

如果运行时不能看到摄像头实时画面

需要点击上方的 Kernel - Shut down all kernels
关闭本章节选项卡，再次打开
点击 STOP 释放摄像头资源后重新运行代码块
重启设备

注意事项

如果使用USB摄像头则需要取消注释 frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) 这一句。

运行

运行以下代码块后，你可以讲黄色胶带放在摄像头前面，观察黑色的画面中是否有黄色胶带的轮廓，能否使用两条目标检测线来检测到黄色胶带。

import cv2  # 导入 OpenCV 库，用于图像处理
import imutils, math  # 辅助图像处理和数学运算的库
from picamera2 import Picamera2 # 用于访问 Raspberry Pi Camera 的库
import numpy as np
from IPython.display import display, Image # 用于在 Jupyter Notebook 中显示图像
import ipywidgets as widgets # 用于创建交互式界面的小部件，如按钮
import threading # 用于创建新线程，以便异步执行任务

# Stop button
# ================
stopButton = widgets.ToggleButton(
    value=False,
    description='Stop',
    disabled=False,
    button_style='danger', # 'success', 'info', 'warning', 'danger' or ''
    tooltip='Description',
    icon='square' # (FontAwesome names without the `fa-` prefix)
)


# findline autodrive

# 上检测线，0.6代表位置，数值越大
sampling_line_1 = 0.6

# 下检测线，数值需要大于 sampling_line_1 且小于 1
sampling_line_2 = 0.9

# 检测线斜率对转弯的影响
slope_impact = 1.5

# 下检测线检测到的线位置对转弯的影响
base_impact = 0.005

# 当前速度对转弯的影响
speed_impact = 0.5

# 巡线速度
line_track_speed = 0.3

# 斜率对巡线速度的影响
slope_on_speed = 0.1

# 目标线的颜色，HSV色彩空间
line_lower = np.array([25, 150, 70])
line_upper = np.array([42, 255, 255])

def view(button):
    # 如果你使用的是CSI摄像头 需要取消注释 picam2 这些代码，并注释掉 camera 这些代码
    # 因为新版本的 OpenCV 不再支持 CSI 摄像头（4.9.0.80），你需要使用 picamera2 来获取摄像头画面
    
    # picam2 = Picamera2()  # 创建 Picamera2 的实例
    # 配置摄像头参数，设置视频的格式和大小
    # picam2.configure(picam2.create_video_configuration(main={"format": 'XRGB8888', "size": (640, 480)}))
    # picam2.start()  # 启动摄像头

    camera = cv2.VideoCapture(-1) # 创建摄像头实例
    #设置分辨率
    camera.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
    camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
    
    display_handle=display(None, display_id=True)
    
    while True:
        # img = picam2.capture_array()
        _, img = camera.read() # 从摄像头捕获一帧图像
        # frame = cv2.flip(frame, 1) # if your camera reverses your image

        height, width = img.shape[:2]
        center_x, center_y = width // 2, height // 2
        # 图像预处理，包括转换颜色空间、高斯模糊、颜色范围筛选等
        hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

        line_mask = cv2.inRange(hsv, line_lower, line_upper)  # 根据颜色范围筛选出目标线
        line_mask = cv2.erode(line_mask, None, iterations=1)  # 腐蚀操作去除噪点
        line_mask = cv2.dilate(line_mask, None, iterations=1)  # 膨胀操作增强目标线

        # 根据上下两个采样线的位置进行目标线检测，并根据检测结果计算转向和速度控制信号
        sampling_h1 = int(height * sampling_line_1)
        sampling_h2 = int(height * sampling_line_2)
        
        get_sampling_1 = line_mask[sampling_h1]
        get_sampling_2 = line_mask[sampling_h2]

        # 计算上、下采样线处的目标线宽度
        sampling_width_1 = np.sum(get_sampling_1 == 255)
        sampling_width_2 = np.sum(get_sampling_2 == 255)

        if sampling_width_1:
            sam_1 = True
        else:
            sam_1 = False
        if sampling_width_2:
            sam_2 = True
        else:
            sam_2 = False

        # 获取上下采样线处目标线的边缘索引
        line_index_1 = np.where(get_sampling_1 == 255)
        line_index_2 = np.where(get_sampling_2 == 255)

        # 如果在上采样线处检测到目标线，计算目标线中心位置
        if sam_1:
            sampling_1_left  = line_index_1[0][0]  # 上采样线目标线最左侧的索引
            sampling_1_right = line_index_1[0][sampling_width_1 - 1]  # 上采样线目标线最右侧的索引
            sampling_1_center= int((sampling_1_left + sampling_1_right) / 2)  # 上采样线目标线中心的索引
        # 如果在下采样线处检测到目标线，计算目标线中心位置
        if sam_2:
            sampling_2_left  = line_index_2[0][0]
            sampling_2_right = line_index_2[0][sampling_width_2 - 1]
            sampling_2_center= int((sampling_2_left + sampling_2_right) / 2)

        # 初始化转向和速度控制信号
        line_slope = 0
        input_speed = 0
        input_turning = 0
        
        # 如果在两个采样线处都检测到了目标线，计算线条的斜率，以及根据斜率和目标线位置计算速度和转向控制信号
        if sam_1 and sam_2:
            line_slope = (sampling_1_center - sampling_2_center) / abs(sampling_h1 - sampling_h2) # 计算线条斜率
            impact_by_slope = slope_on_speed * abs(line_slope) # 根据斜率计算对速度的影响
            input_speed = line_track_speed - impact_by_slope # 计算速度控制信号
            input_turning = -(line_slope * slope_impact + (sampling_2_center - center_x) * base_impact) #+ (speed_impact * input_speed) # 计算转向控制信号
        elif not sam_1 and sam_2: # 如果只在下采样线处检测到了目标线
            input_speed = 0 # 设置速度为0
            input_turning = (sampling_2_center - center_x) * base_impact # 计算转向控制信号
        elif sam_1 and not sam_2: # 如果只在上采样线处检测到了目标线
            input_speed = (line_track_speed / 3) # 减慢速度
            input_turning = 0 # 不进行转向
        else: # 如果两个采样线都没有检测到目标线
            input_speed = - (line_track_speed / 3) # 后退
            input_turning = 0 # 不进行转向

        # base.base_json_ctrl({"T":13,"X":input_speed,"Z":input_turning})

        cv2.putText(line_mask, f'X: {input_speed:.2f}, Z: {input_turning:.2f}', (center_x+50, center_y+0), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
        # 可视化操作，包括在采样线位置绘制直线，标记采样结果，以及显示转向和速度控制信号
        cv2.line(line_mask, (0, sampling_h1), (img.shape[1], sampling_h1), (255, 0, 0), 2)
        cv2.line(line_mask, (0, sampling_h2), (img.shape[1], sampling_h2), (255, 0, 0), 2)

        if sam_1:
            # 在上采样线处的目标线两端绘制绿色的标记线
            cv2.line(line_mask, (sampling_1_left, sampling_h1+20), (sampling_1_left, sampling_h1-20), (0, 255, 0), 2)
            cv2.line(line_mask, (sampling_1_right, sampling_h1+20), (sampling_1_right, sampling_h1-20), (0, 255, 0), 2)
        if sam_2:
            # 在下采样线处的目标线两端绘制绿色的标记线
            cv2.line(line_mask, (sampling_2_left, sampling_h2+20), (sampling_2_left, sampling_h2-20), (0, 255, 0), 2)
            cv2.line(line_mask, (sampling_2_right, sampling_h2+20), (sampling_2_right, sampling_h2-20), (0, 255, 0), 2)
        if sam_1 and sam_2:
            # 如果上下采样线处都检测到目标线，绘制一条从上采样线中心到下采样线中心的红色连线
            cv2.line(line_mask, (sampling_1_center, sampling_h1), (sampling_2_center, sampling_h2), (255, 0, 0), 2)
        
        _, frame = cv2.imencode('.jpeg', line_mask)
        display_handle.update(Image(data=frame.tobytes()))
        if stopButton.value==True:
            # picam2.close()
            cv2.release() # 如果是，则关闭摄像头
            display_handle.update(None)


# 显示“停止”按钮并启动显示函数的线程
# ================
display(stopButton)
thread = threading.Thread(target=view, args=(stopButton,))
thread.start()

导航菜单

Jetson 21 基于 OpenCV 的巡线自动驾驶

目录

基于 OpenCV 的巡线自动驾驶

准备工作

结束主程序

例程

如果运行时不能看到摄像头实时画面

注意事项

运行