Jetson 21 基于 OpenCV 的巡线自动驾驶
来自Waveshare Wiki
基于 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 # 辅助图像处理和数学运算的库
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):
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()