Raspberry Pi vision car -- OpenCV line inspection (HSL color space, PID)

Table of contents

trial

Trial and Error 1: Morphological Processing

Trial and error 2: HSV color space

basic theory

1. HSV and HSL color space

2. PID adjustment

1. OpenCV image processing

1. Obtain a binary image in the HSL color space

2. Morphological processing of binary images

3. Find the outline and center point coordinates of the line

Two, PID

3. Motion Control

total code

---

trial

Trial and Error 1: Morphological Processing

The morphological processing used at the beginning changed the threshold by itself. After debugging, it was processed and found that the effect was not very good, so it was changed to HSV color space.

Trial and error 2: HSV color space

Didn't notice before that the HSV color space has a hard time recognizing white:

HSV： 

 It is not difficult to see that if you are looking for a white line, the HSV color space is not a good choice. The following introduces the HSL color space :

 HSL：

Therefore, if it is to patrol white, it is recommended to use HSL color space .

Note: The camera of the line patrol car should not be too low, if it is too low, the shadow of the car may block the light .

The effect in hsv:

 

The effect in hsl:

It can be seen that the white line can be roughly found.

basic theory

1. HSV and HSL color space

 HSV： 

 It is not difficult to see that if you are looking for a white line, the HSV color space is not a good choice. The following introduces the HSL color space :

 HSL：

Therefore, if it is to patrol white, it is recommended to use HSL color space .

2. PID adjustment

Personal understanding:

P: tension

I: impetus

D: resistance 

1. OpenCV image processing

1. Obtain a binary image in the HSL color space

 

# 在HSV色彩空间下得到二值图
def Get_HSV(image):
    # 1 get trackbar's value
    hmin = cv2.getTrackbarPos('hmin', 'h_binary')
    hmax = cv2.getTrackbarPos('hmax', 'h_binary')
    smin = cv2.getTrackbarPos('smin', 's_binary')
    smax = cv2.getTrackbarPos('smax', 's_binary')
    lmin = cv2.getTrackbarPos('lmin', 'l_binary')
    lmax = cv2.getTrackbarPos('lmax', 'l_binary')

    # 2 to HSV
    hls = cv2.cvtColor(image, cv2.COLOR_BGR2HLS)
    cv2.imshow('hls', hls)
    h, l, s = cv2.split(hls)

    # 3 set threshold (binary image)
    # if value in (min, max):white; otherwise:black
    h_binary = cv2.inRange(np.array(h), np.array(hmin), np.array(hmax))
    s_binary = cv2.inRange(np.array(s), np.array(smin), np.array(smax))
    l_binary = cv2.inRange(np.array(l), np.array(lmin), np.array(lmax))

    # 4 get binary（对H、S、V三个通道分别与操作）
    binary = 255 - cv2.bitwise_and(h_binary, cv2.bitwise_and(s_binary, l_binary))

    # 5 Show
    cv2.imshow('h_binary', h_binary)
    cv2.imshow('s_binary', s_binary)
    cv2.imshow('l_binary', l_binary)
    cv2.imshow('binary', binary)

    return binary

2. Morphological processing of binary images

 

# 图像处理
def Image_Processing():
    global frame, binary
    # Capture the frames
    ret, frame = camera.read()

    # to binary
    binary = Get_HSV(frame)

    blur = cv2.GaussianBlur(binary, (5, 5), 0)
    cv2.imshow('blur', blur)

    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (35, 35))
    Open = cv2.morphologyEx(blur, cv2.MORPH_OPEN, kernel)
    cv2.imshow('Open', Open)

    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
    Erode = cv2.morphologyEx(Open, cv2.MORPH_ERODE, kernel)
    cv2.imshow('Erode', Erode)

    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
    Dilate = cv2.morphologyEx(Erode, cv2.MORPH_DILATE, kernel)
    cv2.imshow('Dilate', Dilate)

    binary = Erode#Dilate

3. Find the outline and center point coordinates of the line

 

 

# 找线
def Find_Line():
    global x, y, image
    # 1 找出所有轮廓
    bin2, contours, hierarchy = cv2.findContours(binary, 1, cv2.CHAIN_APPROX_NONE)
    
    # 2 找出最大轮廓
    if len(contours) > 0:
        # 最大轮廓
        c = max(contours, key=cv2.contourArea)
        M = cv2.moments(c)

        # 中心点坐标
        x = int(M['m10'] / M['m00'])
        y = int(M['m01'] / M['m00'])
        #print(x, y)

        # 显示
        image = frame.copy()
        # 标出中心位置
        cv2.line(image, (x, 0), (x, 720), (0, 0, 255), 1)
        cv2.line(image, (0, y), (1280, y), (0, 0, 255), 1)
        # 画出轮廓
        cv2.drawContours(image, contours, -1, (128, 0, 128), 2)
        cv2.imshow("image", image)

    else:
        print("not found the line")

        (x,y) = (0, 0)

Two, PID

def Pid():
    global turn_speed, x, y, speed
    global error, last_error, pre_error, out_pid

    error = abs(x - width / 2)

    out_pid = int(proportion * error - integral * last_error + derivative * pre_error)
    turn_speed = out_pid

    # 保存本次误差，以便下一次运算
    pre_error = last_error
    last_error = error

    # 限值
    if (turn_speed < 30):
        turn_speed = 30
    elif (turn_speed > 100):
        turn_speed = 100
    if (speed < 0):
        speed = 0
    elif (speed > 100):
        speed = 100

    print(error, out_pid, turn_speed, (x, y))

3. Motion Control

# 巡线
def Follow_Line():
    global turn_speed, x, y,speed, back_speed
        
    '''if(x < width / 2 and y>2*height/3):
        Left(turn_speed)
    elif(x>3*width/2 and y>2*height/3):
        Right(turn_speed)'''
    if(0<x<width/4):
        Left(turn_speed)
        print("turn left")
    elif(3*width/4<x<width):
        Right(turn_speed)
        print("turn right")
    #直角拐弯
    elif(y>3*height/4):
        if(x<width/2):
            Left(turn_speed*2)
            print("turn left")
        elif(x>=width/2):
            Right(turn_speed*2)
            print("turn right")
    elif(x>=width/4 and x<=3*width/4):
        Forward(speed)
    
    elif(x==0 and y==0):
        Back(back_speed)

total code

#!/usr/bin/env python2
# -*- coding: utf-8 -*-

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

l_motor = 18
left_Forward = 22
left_back = 27

r_motor = 23
right_Forward = 25
right_back = 24

pwm_servo = Adafruit_PCA9685.PCA9685()

width, height = 160, 120
camera = cv2.VideoCapture(0)
camera.set(3, width)
camera.set(4, height)

# pid
error = 0  # 当前误差e[k]
last_error = 0  # 上一次误差e[k-1]
pre_error = 0  # 上上次误差e[k-2]
proportion = 1  # 比例系数3 0.2
integral = 0.5  # 积分系数1.2
derivative = 0  # 微分系数1.2

stop_flag = 1
control_flag = 1
turn_speed = 30
speed = 30
back_speed = 30


def Motor_Init():
    global L_Motor, R_Motor
    L_Motor = GPIO.PWM(l_motor, 100)
    R_Motor = GPIO.PWM(r_motor, 100)
    L_Motor.start(0)
    R_Motor.start(0)


def Direction_Init():
    GPIO.setup(left_back, GPIO.OUT)
    GPIO.setup(left_Forward, GPIO.OUT)
    GPIO.setup(l_motor, GPIO.OUT)

    GPIO.setup(right_Forward, GPIO.OUT)
    GPIO.setup(right_back, GPIO.OUT)
    GPIO.setup(r_motor, GPIO.OUT)


def set_servo_angle(channel, angle):
    angle = 4096 * ((angle * 11) + 500) / 20000
    pwm_servo.set_pwm_freq(50)  # frequency==50Hz (servo)
    pwm_servo.set_pwm(channel, 0, int(angle))


def TrackBar_Init():
    # 1 create windows
    cv2.namedWindow('h_binary')
    cv2.namedWindow('s_binary')
    cv2.namedWindow('l_binary')
    # 2 Create Trackbar
    cv2.createTrackbar('hmin', 'h_binary', 0, 179, call_back)
    cv2.createTrackbar('hmax', 'h_binary', 110, 179, call_back)
    cv2.createTrackbar('smin', 's_binary', 0, 255, call_back)
    cv2.createTrackbar('smax', 's_binary', 51, 255, call_back)  # 51
    cv2.createTrackbar('lmin', 'l_binary', 0, 255, call_back)
    cv2.createTrackbar('lmax', 'l_binary', 255, 255, call_back)
    '''cv2.namedWindow('binary')
    cv2.createTrackbar('thresh', 'binary', 154, 255, call_back)  '''
    #   创建滑动条     滑动条值名称 窗口名称   滑动条值 滑动条阈值 回调函数


def Init():
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BCM)
    Direction_Init()
    Motor_Init()
    TrackBar_Init()


def Forward(turn_speed):
    L_Motor.ChangeDutyCycle(turn_speed)
    GPIO.output(left_Forward, 1)  # left_Forward
    GPIO.output(left_back, 0)  # left_back

    R_Motor.ChangeDutyCycle(turn_speed)
    GPIO.output(right_Forward, 1)  # right_Forward
    GPIO.output(right_back, 0)  # right_back


def Back(turn_speed):
    L_Motor.ChangeDutyCycle(turn_speed)
    GPIO.output(left_Forward, 0)  # left_Forward
    GPIO.output(left_back, 1)  # left_back

    R_Motor.ChangeDutyCycle(turn_speed)
    GPIO.output(right_Forward, 0)  # right_Forward
    GPIO.output(right_back, 1)  # right_back


def Left(turn_speed):
    L_Motor.ChangeDutyCycle(turn_speed)
    GPIO.output(left_Forward, 0)  # left_Forward
    GPIO.output(left_back, 1)  # left_back

    R_Motor.ChangeDutyCycle(turn_speed)
    GPIO.output(right_Forward, 1)  # right_Forward
    GPIO.output(right_back, 0)  # right_back


def Right(turn_speed):
    L_Motor.ChangeDutyCycle(turn_speed)
    GPIO.output(left_Forward, 1)  # left_Forward
    GPIO.output(left_back, 0)  # left_back

    R_Motor.ChangeDutyCycle(turn_speed)
    GPIO.output(right_Forward, 0)  # right_Forward
    GPIO.output(right_back, 1)  # right_back


def Stop():
    L_Motor.ChangeDutyCycle(0)
    GPIO.output(left_Forward, 0)  # left_Forward
    GPIO.output(left_back, 0)  # left_back

    R_Motor.ChangeDutyCycle(0)
    GPIO.output(right_Forward, 0)  # right_Forward
    GPIO.output(right_back, 0)  # right_back


# 回调函数
def call_back(*arg):
    pass


# 在HSV色彩空间下得到二值图
def Get_HSV(image):
    # 1 get trackbar's value
    hmin = cv2.getTrackbarPos('hmin', 'h_binary')
    hmax = cv2.getTrackbarPos('hmax', 'h_binary')
    smin = cv2.getTrackbarPos('smin', 's_binary')
    smax = cv2.getTrackbarPos('smax', 's_binary')
    lmin = cv2.getTrackbarPos('lmin', 'l_binary')
    lmax = cv2.getTrackbarPos('lmax', 'l_binary')

    # 2 to HSV
    hls = cv2.cvtColor(image, cv2.COLOR_BGR2HLS)
    cv2.imshow('hls', hls)
    h, l, s = cv2.split(hls)

    # 3 set threshold (binary image)
    # if value in (min, max):white; otherwise:black
    h_binary = cv2.inRange(np.array(h), np.array(hmin), np.array(hmax))
    s_binary = cv2.inRange(np.array(s), np.array(smin), np.array(smax))
    l_binary = cv2.inRange(np.array(l), np.array(lmin), np.array(lmax))

    # 4 get binary（对H、S、V三个通道分别与操作）
    binary = 255 - cv2.bitwise_and(h_binary, cv2.bitwise_and(s_binary, l_binary))

    # 5 Show
    cv2.imshow('h_binary', h_binary)
    cv2.imshow('s_binary', s_binary)
    cv2.imshow('l_binary', l_binary)
    cv2.imshow('binary', binary)

    return binary


# 手动控制小车（上下左右，案件事件判断）
# 控制方式：w、s、a、d分别表示：上、下、左、右
def Key_Control(keyboard):
    global stop_flag, control_flag
    if keyboard == ord("w"):
        Forward(50)
        time.sleep(0.1)
        Stop()
    elif keyboard == ord("s"):
        Back(50)
        time.sleep(0.1)
        Stop()
    elif keyboard == ord("a"):
        Left(50)
        time.sleep(0.1)
        Stop()
    elif keyboard == ord("d"):
        Right(50)
        time.sleep(0.1)
        Stop()


# 图像处理
def Image_Processing():
    global frame, binary
    # Capture the frames
    ret, frame = camera.read()

    # to binary
    binary = Get_HSV(frame)

    blur = cv2.GaussianBlur(binary, (5, 5), 0)
    cv2.imshow('blur', blur)

    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (35, 35))
    Open = cv2.morphologyEx(blur, cv2.MORPH_OPEN, kernel)
    cv2.imshow('Open', Open)

    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
    Erode = cv2.morphologyEx(Open, cv2.MORPH_ERODE, kernel)
    cv2.imshow('Erode', Erode)

    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
    Dilate = cv2.morphologyEx(Erode, cv2.MORPH_DILATE, kernel)
    cv2.imshow('Dilate', Dilate)

    binary = Erode  # Dilate


# 找线
def Find_Line():
    global x, y, image
    # 1 找出所有轮廓
    bin2, contours, hierarchy = cv2.findContours(binary, 1, cv2.CHAIN_APPROX_NONE)

    # 2 找出最大轮廓
    if len(contours) > 0:
        # 最大轮廓
        c = max(contours, key=cv2.contourArea)
        M = cv2.moments(c)

        # 中心点坐标
        x = int(M['m10'] / M['m00'])
        y = int(M['m01'] / M['m00'])
        # print(x, y)

        # 显示
        image = frame.copy()
        # 标出中心位置
        cv2.line(image, (x, 0), (x, 720), (0, 0, 255), 1)
        cv2.line(image, (0, y), (1280, y), (0, 0, 255), 1)
        # 画出轮廓
        cv2.drawContours(image, contours, -1, (128, 0, 128), 2)
        cv2.imshow("image", image)

    else:
        print("not found the line")

        (x, y) = (0, 0)


def Pid():
    global turn_speed, x, y, speed
    global error, last_error, pre_error, out_pid

    error = abs(x - width / 2)

    out_pid = int(proportion * error - integral * last_error + derivative * pre_error)
    turn_speed = out_pid

    # 保存本次误差，以便下一次运算
    pre_error = last_error
    last_error = error

    # 限值
    if (turn_speed < 30):
        turn_speed = 30
    elif (turn_speed > 100):
        turn_speed = 100
    if (speed < 0):
        speed = 0
    elif (speed > 100):
        speed = 100

    print(error, out_pid, turn_speed, (x, y))


# 巡线
def Follow_Line():
    global turn_speed, x, y, speed, back_speed

    '''if(x < width / 2 and y>2*height/3):
        Left(turn_speed)
    elif(x>3*width/2 and y>2*height/3):
        Right(turn_speed)'''
    if (0 < x < width / 4):
        Left(turn_speed)
        print("turn left")
    elif (3 * width / 4 < x < width):
        Right(turn_speed)
        print("turn right")
    # 直角拐弯
    elif (y > 3 * height / 4):
        if (x < width / 2):
            Left(turn_speed * 2)
            print("turn left")
        elif (x >= width / 2):
            Right(turn_speed * 2)
            print("turn right")
    elif (x >= width / 4 and x <= 3 * width / 4):
        Forward(speed)

    elif (x == 0 and y == 0):
        Back(back_speed)


def Control():
    global control_flag, speed, proportion, integral
    keyboard = cv2.waitKey(1)
    # 加速减速
    if (keyboard == ord('k')):
        speed += 5
    elif (keyboard == ord('l')):
        speed -= 5
    print(speed)

    if keyboard == ord("n"):
        integral += 0.01
    elif keyboard == ord("m"):
        integral -= 0.01
    print(integral)

    if (control_flag == -1):
        Follow_Line()
        if keyboard == 32:
            control_flag *= -1
            Stop()

    else:
        Key_Control(keyboard)
        if keyboard == 32:
            control_flag *= -1
            Stop()

    print(control_flag)


if __name__ == '__main__':
    Init()
    set_servo_angle(4, 140)  # top servo     lengthwise
    # 0:back    180:front
    set_servo_angle(5, 90)  # bottom servo  crosswise
    # 0:left    180:right

    while True:
        Image_Processing()
        Find_Line()
        Pid()
        Control()
        if cv2.waitKey(1) == ord('q'):
            cv2.destroyAllWindows()
            break

 In fact, at the beginning, I mainly wanted to play with machine vision. The research on the motion control of the car is not very detailed, and the research on PID is not deep (it may even be wrong).

Many of them are my own ideas. If there are mistakes, you are welcome to correct them. If you have suggestions, you are also welcome to exchange them. Thank you.