How to use PID for line following using computer vision and quadratics?

193 views Asked by At

I've been working on a line-following robot using computer vision for a while now and have finished the code that plots a quadratic in the centre of a line like so:

enter image description here

I want to use the values of a and b from the expanded form of the parabola (ax^2 + bx + c) and how far the mid point of the line is away from the center of the screen. However, I've never used PID before and could use some help in implementing it or even if I should. I want the PID controller to control the turn of the robot; how much it has to rotate to keep the quadratic as straight as possible.

The code works by converting the image to grayscale, blurring and thresholding it, then skeletonizing it to get the center pixels of the line. I then perform a variety of methods to eliminate some noise from the line. Once I have all the averaged pixels I fit a curve over it and plot that on the image.

Here is the code:

import numpy as np
import cv2
from skimage.morphology import medial_axis
import math
from scipy.optimize import curve_fit

cap = cv2.VideoCapture(2)
w = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
h = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)


def drawLines(Line, outlier_threshold):
    # grabs height, width and color of an image

    # filters the image to make sure there are only pixels on the screen that are close to each other (like a line)
    filtered_pixel_coords = []
    prev_point = None

    for pixel in Line:
        x, y = pixel[0]
        
        if prev_point is not None:
            prev_x, prev_y = prev_point
            distance = math.sqrt((x - prev_x) ** 2 + (y - prev_y) ** 2)
            
            if distance <= outlier_threshold:
                # forgets about any pixels on the edge cause sometimes there are a lot of them and that is bad
                if 10 < x < w - 10 and 10 < y < h - 10:
                    filtered_pixel_coords.append([x, y])
        else:
            filtered_pixel_coords.append([x, y])

        prev_point = [x, y]

    averaged_coords = []
    avg = [0, 0]
    count = 0

    # averages out every 50 pixels for smoother line
    for i, coord in enumerate(filtered_pixel_coords):
        avg[0] += coord[0]
        avg[1] += coord[1]
        count += 1

        if count == 50 or i == len(filtered_pixel_coords) - 1:
            avg[0] //= count
            avg[1] //= count
            averaged_coords.append(avg.copy())
            avg = [0, 0]
            count = 0
    
    return averaged_coords

def objective(x, a, b, c):
    # quadratic for PID values
    return a * x + b * x**2 + c

while True:
    ret, img = cap.read()

    if not ret:
        break

    # converts image to grayscale (black and white), blurs the image and only grabs white pixels (no asking how)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    blurred = cv2.medianBlur(gray, 9)
    _, thresholded = cv2.threshold(blurred, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)

    # creates contours (boxes) around the white pixels
    contours,_= cv2.findContours(thresholded,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE)

    for contour in contours:
        # contour = cv2.approxPolyDP(contour, 0.005 * cv2.arcLength(contour, True), True)
        cv2.drawContours(img, [contour], -1, (0, 255, 0), -1)

    # get everything that is zero on the white pixel only image (grabs all pixels of the line)
    inverted_thresholded = cv2.bitwise_not(thresholded)

    # gets the middle of the line by eroding it before it is only a pixel big. !!FORGOT WHAT DISTANCE DOES HERE!!
    skeleton, dist = medial_axis(inverted_thresholded, return_distance=True)

    dist_on_skel = dist * skeleton
    skeleton = (255 * dist_on_skel).astype(np.uint8)

    # grabs all pixels that werent eroded
    Line = cv2.findNonZero(skeleton)

    averaged = drawLines(Line, 30)

    x_coords = [coord[0] for coord in averaged]
    y_coords = [coord[1] for coord in averaged]

    try:
        popt, _ = curve_fit(objective, y_coords, x_coords)
        # summarize the parameter values
        a, b, c = popt

        y_line = np.arange(min(y_coords), max(y_coords), 1)
        # calculate the output for the range
        x_line = objective(y_line, a, b, c)

        # plots the line to show on the image 
        for i in range(len(y_line) - 1):
            pt1 = (int(x_line[i]), int(y_line[i]))
            pt2 = (int(x_line[i + 1]), int(y_line[i + 1]))
            cv2.line(img, pt1, pt2, (0, 0, 255), 2)
    except:
        continue

    cv2.imshow('Image', img)
    cv2.imshow('Skeletonized', skeleton)

    if cv2.waitKey(100) == 27:
        break

cap.release()
cv2.destroyAllWindows()

Thanks for the input in advance.

0

There are 0 answers