树莓派4B AI视觉智能小车循迹（摄像头采集图像，PID控制）

树莓派4B AI视觉智能小车循迹（PID控制）

---

1.循迹原理

智能小车的前置摄像机实时采集前方的图像，调整底部舵机和顶部舵机的角度，使轨道能被摄像机捕捉到并将采集到的图像传回树莓派系统。利用OpenCV对图像进行处理，系统通过处理后得到的图像信息调整小车的运动状态，使小车沿着轨道行驶。OpenCV对图像的处理过程如下：
（1）图像裁剪：对采集的图像进行进一步的裁剪，得到含有轨道的图像；
（2）图像灰度化处理：调用cv2模块中的cvtColor函数，将图像转换为灰度图；
（3）高斯模糊去除噪声干扰：调用GaussianBlur函数对其进行高斯模糊处理，去除噪声的干扰，根据实际情况调整卷积核的大小，使噪声干扰尽可能被去除；
（4）图像二值化处理：根据轨道与周围环境颜色偏差选择较优的阈值，调用threshold函数对图像进行二值化处理得到二值图像，此时轨道部分变为白色，周围环境的颜色变为黑色。
（5）图像膨胀、腐蚀处理：由于光线、周围环境等干扰的存在，得到的二值化图像依然存在一些干扰，此时调用erode、dilate函数对图形进行膨胀、腐蚀处理，使剩下的干扰尽可能被去除；
（6）求最大轮廓中心点：调用findContours函数检测图像的轮廓，并找出最大的轮廓（轨迹），调用drawContours绘制出最大轮廓，求出最大轮廓的中心点坐标。

计算两个中心点横坐标的偏差，利用PID控制实现对小车左右轮速度的控制。假设中心点偏差在区间内，则左右轮电机转速相同，智能小车直行；假设中心点偏差大于区间最大值（小车偏左），此时小车应该右转，左轮的电机转速大于右轮的电机转速，使小车右转，回到轨道上，中心点偏右的调节过程一样。经过这样反复调节，使小车沿着轨道行驶。

2.轨道图

下图为笔者所使用的小车循迹的轨道图。读者可根据实际轨道调节图像灰度化处理的阈值，使得到的二值化出现能满足循迹要求。

3.循迹视频

由于文件大小有限，只截取了部分循迹结果如下所示。

4.代码实现

import numpy as np
import cv2
import Adafruit_PCA9685
import RPi.GPIO as GPIO
import time

video_capture = cv2.VideoCapture(0)
video_capture.set(3, 300)
video_capture.set(4, 300)

# 树莓派小车电机驱动初始化
PWMA = 18
AIN1 = 22
AIN2 = 27

PWMB = 23
BIN1 = 25
BIN2 = 24

BtnPin = 19
Gpin = 5
Rpin = 6

# 初始化舵机
pwm = Adafruit_PCA9685.PCA9685()
# Configure min and max servo pulse lengths
servo_min = 150  # Min pulse length out of 4096
servo_max = 600  # Max pulse length out of 4096


# 辅助功能，使设置舵机脉冲宽度更简单。
def set_servo_pulse(channel, pulse):
    pulse_length = 1000000  # 1,000,000 us per second
    pulse_length //= 60  # 60 Hz
    print('{0}us per period'.format(pulse_length))
    pulse_length //= 4096  # 12 bits of resolution
    print('{0}us per bit'.format(pulse_length))
    pulse *= 1000
    pulse //= pulse_length
    pwm.set_pwm(channel, 0, pulse)


def set_servo_angle(channel, angle):
    angle = 4096 * ((angle * 11) + 500) / 20000
    pwm.set_pwm(channel, 0, int(angle))


# 频率设置为50hz，适用于舵机系统。
pwm.set_pwm_freq(50)
set_servo_angle(5, 94)  # 底座舵机 94
set_servo_angle(4, 105)  # 顶部舵机 105

time.sleep(0.5)


# 树莓派小车运动函数
def t_up(speed1, speed2, t_time): 
    L_Motor.ChangeDutyCycle(speed1)
    GPIO.output(AIN2, False)  # AIN2
    GPIO.output(AIN1, True)  # AIN1

    R_Motor.ChangeDutyCycle(speed2)
    GPIO.output(BIN2, False)  # BIN2
    GPIO.output(BIN1, True)  # BIN1
    time.sleep(t_time)


def t_stop(t_time):   #停止函数
    L_Motor.ChangeDutyCycle(0)
    GPIO.output(AIN2, False)
    GPIO.output(AIN1, False)

    R_Motor.ChangeDutyCycle(0)
    GPIO.output(BIN2, False)
    GPIO.output(BIN1, False)
    time.sleep(t_time)


def t_down(speed, t_time):   #后退函数
    L_Motor.ChangeDutyCycle(speed)
    GPIO.output(AIN2, True)
    GPIO.output(AIN1, False)

    R_Motor.ChangeDutyCycle(speed)
    GPIO.output(BIN2, True)
    GPIO.output(BIN1, False)
    time.sleep(t_time)

def keysacn():
    val = GPIO.input(BtnPin)
    while GPIO.input(BtnPin) == False:
        val = GPIO.input(BtnPin)
    while GPIO.input(BtnPin) == True:
        time.sleep(0.01)
        val = GPIO.input(BtnPin)
        if val == True:
            GPIO.output(Rpin, 1)
            while GPIO.input(BtnPin) == False:
                GPIO.output(Rpin, 0)
        else:
            GPIO.output(Rpin, 0)


GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(Gpin, GPIO.OUT)  # 设置绿色Led引脚模式输出
GPIO.setup(Rpin, GPIO.OUT)  # 设置红色Led引脚模式输出
GPIO.setup(BtnPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)  # 设置输入BtnPin模式，拉高至高电平(3.3V)

GPIO.setup(AIN2, GPIO.OUT)
GPIO.setup(AIN1, GPIO.OUT)
GPIO.setup(PWMA, GPIO.OUT)

GPIO.setup(BIN1, GPIO.OUT)
GPIO.setup(BIN2, GPIO.OUT)
GPIO.setup(PWMB, GPIO.OUT)

L_Motor = GPIO.PWM(PWMA, 100)
L_Motor.start(0)

R_Motor = GPIO.PWM(PWMB, 100)
R_Motor.start(0)

keysacn()

# PID控制参数初始化
Kp = 0.85
Ki = 0
Kd = 0
Target_value = 150   # 设定值初始化
last_Err = 0         # 上一个误差值初始化
total_Err = 0        # 误差累加初始化
output = 0           # PID输出初始化

while 1:
    # 读取摄像头采集到的图像，ret为True或False,代表有没有读取到图片。参数frame表示截取到一帧的图片
    ret, frame = video_capture.read()
    cv2.namedWindow("input image", cv2.WINDOW_AUTOSIZE)
    crop_img = frame
    # 图像灰度化处理
    gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)
    # 高斯模糊（滤波）
    blur = cv2.GaussianBlur(gray, (15, 15), 0)  # 卷积核为：15*15
    # 图像二值化处理
    ret, thresh1 = cv2.threshold(blur, 100, 255, cv2.THRESH_BINARY)
    # 膨胀和腐蚀，去除干扰
    mask = cv2.erode(thresh1, None, iterations=2)
    mask = cv2.dilate(mask, None, iterations=2)
    cv2.imshow("mask image", mask)
    # 获取图像轮廓
    image, contours, hierarchy = cv2.findContours(mask.copy(), 1, cv2.CHAIN_APPROX_NONE)
    # 找出最大轮廓，并找出其中心点坐标
    if len(contours) > 0:
        c = max(contours, key=cv2.contourArea)
        M = cv2.moments(c)
        # 求取中心点坐标（cx,cy）
        cx = int(M['m10'] / M['m00'])
        cy = int(M['m01'] / M['m00'])
        cv2.line(crop_img, (cx, 0), (cx, 720), (255, 0, 0), 1)
        cv2.line(crop_img, (0, cy), (1280, cy), (255, 0, 0), 1)
        # 绘制轮廓图
        cv2.drawContours(crop_img, contours, -1, (0, 255, 0), 1)
        
        # PID控制
        Error = Target_value - cx   # 计算偏差
        total_Err = total_Err + Error    # 偏差累加
        output = Kp * Error + Ki * total_Err + Kd * (Error - last_Err)   # PID运算
        last_Err = Error   # 将本次偏差赋给上次一偏差
        u = output   # cx的调整值直接传给速度（调整速度）
        # print(cx)
        # print(u)
        u_l = 70 + 1.2 * u   # 根据偏差调整左轮的转速
        u_r = 70 - 1.2 * u   # 根据偏差调整右轮的转速
        # 假如左右轮的速度理论值大于等于95，则令其等于95（原因：速度最大为100，以及避免电机长时间满负荷工作）
        if u_l >= 95:
            u_l = 95
        if u_r >= 95:
            u_r = 95
        # 假如左右轮的速度理论值小于等于0，则令其等于0（原因：速度最小为0）
        if u_r<=0:
            u_r=0
        if u_l<=0:
            u_l=0
        t_up(u_l, u_r, 0)
    else:
        print("I don't see the line")
    cv2.imshow('frame', crop_img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        L_Motor.stop()
        R_Motor.stop()
        GPIO.cleanup()
        break
cv2.waitKey(0)
cv2.destroyAllWindows()

THANKS：Z,Y,T,L,X