opencv 巡线 pid 代码

下面是一个简单的 OpenCV 巡线 PID 控制的代码示例：

import cv2
import numpy as np

# 设置PID参数
kp = 0.5
ki = 0.0
kd = 0.2

# 设置误差和积分变量
last_error = 0
integral = 0

# 设置阈值
h_threshold = 60
s_threshold = 40
v_threshold = 40

# 定义ROI区域
roi_vertices = np.array([[(0, 480), (0, 280), (640, 280), (640, 480)]], dtype=np.int32)

# 初始化摄像头
cap = cv2.VideoCapture(0)

while True:
    # 读取摄像头图像
    ret, frame = cap.read()

    # 转换为HSV色彩空间
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    # 对HSV图像进行阈值处理，得到二值化图像
    h_binary = cv2.inRange(hsv, np.array([0, 0, 0]), np.array([180, h_threshold, v_threshold]))
    s_binary = cv2.inRange(hsv, np.array([0, 0, 0]), np.array([180, s_threshold, v_threshold]))
    v_binary = cv2.inRange(hsv, np.array([0, 0, 0]), np.array([180, v_threshold, v_threshold]))
    binary = cv2.bitwise_and(h_binary, s_binary)
    binary = cv2.bitwise_and(binary, v_binary)

    # 对二值化图像进行ROI裁剪
    masked = cv2.bitwise_and(binary, cv2.fillPoly(np.zeros_like(binary), roi_vertices, 255))

    # 检测直线，输出直线斜率和截距
    lines = cv2.HoughLinesP(masked, rho=1, theta=np.pi/180, threshold=50, minLineLength=50, maxLineGap=10)
    if lines is not None:
        for line in lines:
            x1, y1, x2, y2 = line[0]
            cv2.line(frame, (x1, y1), (x2, y2), (0, 0, 255), 2)
            if x2 != x1:
                slope = (y2 - y1) / (x2 - x1)
                intercept = y1 - slope * x1
                error = 320 - ((240 - intercept) / slope)
                integral += error
                derivative = error - last_error
                last_error = error
                pid_output = kp * error + ki * integral + kd * derivative
                print("PID output: ", pid_output)

    # 显示结果
    cv2.imshow("frame", frame)
    cv2.imshow("masked", masked)

    # 按下q键退出
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# 释放摄像头并关闭窗口
cap.release()
cv2.destroyAllWindows()

在这个示例中，我们首先定义了 PID 控制器的参数，包括比例系数、积分系数和微分系数。然后，我们定义了误差变量和积分变量，用于计算 PID 输出。接下来，我们将图像转换为 HSV 色彩空间，并对图像进行阈值处理，得到二值化图像。然后，我们裁剪出 ROI 区域，并使用 Hough 变换检测直线。如果检测到直线，我们计算直线的斜率和截距，并计算误差、积分和微分，最终得到 PID 输出。最后，我们显示图像，并等待按下 q 键退出程序。