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. 输入命令： crontab -e
6. 如果询问希望使用什么编辑器，输入 1 后按回车，选择使用 nano。
7. 打开 crontab 的配置文件后，你可以看到以下两行内容
@reboot ~/ugv_pt_rpi/ugv-env/bin/python ~/ugv_pt_rpi/app.py >> ~/ugv.log 2>&1 @reboot /bin/bash ~/ugv_pt_rpi/start_jupyter.sh >> ~/jupyter_log.log 2>&1
8.在 ……app.py >> …… 这行的最前面添加一个 # 号来注释掉这行。
# @reboot ~/ugv_pt_rpi/ugv-env/bin/python ~/ugv_pt_rpi/app.py >> ~/ugv.log 2>&1 @reboot /bin/bash ~/ugv_pt_rpi/start_jupyter.sh >> ~/jupyter_log.log 2>&1
9. 在终端页面，按 Ctrl + X 退出，它会询问你 Save modified buffer? 输入 Y，按回车，保存变更。
10. 重启设备，注意该过程会暂时关闭当前的 jupyter Lab，如果你上一步没有注释掉 ……start_jupyter.sh >>…… 这一行，那么当机器人重新开机后，你仍然可以正常使用 jupyter Lab (JupyterLab 与 机器人主程序 app.py 是互相独立运行的)，可能需要重新刷新页面。
11. 这里需要注意一点，由于下位机持续通过串口与上位机通信，上位机在重启过程中有可能会由于串口电平的连续变化不能正常开机，拿上位机为树莓派的情况举例，重启时树莓派关机后不会再开机，红灯常亮绿灯不亮，此时可以关闭机器人电源开关，再打开，机器人就能够正常重启了。
12. 输入重启命令： sudo reboot
13. 等待设备重启后（重启过程中树莓派的绿灯会闪烁，当绿灯闪烁频率降低或灭掉后即代表已经启动成功），刷新页面，继续该教程的剩余部分。
例程
以下代码块可以直接运行：
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()