Self Driving Car V1

My Robo
My Robo

The idea:

I wanted to create a quick project that can demonstrate the power of computer vision and its applications in real world. With the advancement in the self driving technology I wanted to create a small scale working model of an actual car inspired by Tesla.
Tesla basically follows lanes on highways/roads to go from one place to another with help of GPS. We are not going to create a full self driving car overtaking model but a lane follower will be the first step to driving autonomously.
notion image
notion image
notion image

The hardware:

I wanted to create a small scale lane following car using computer vision. So i immediately thought about Raspberry Pi and camera along with a Robot Chassis.
  1. Raspberry Pi 2 or newer (I used Pi 3B+ because it was laying around)
  1. Raspberry Pi camera color (I used the v1 also laying around)
  1. Robot Chassis (I used a chassis i had made with some acrylic sheets some time ago. Any can work)
  1. L298/L293D motor driver (i used L298 as it is more powerful and was already mounted on the robo)
  1. Battery (LiPo for motors and a powerbank for Pi)
  1. SD Card
  1. Wires, Glue Gun, Other tools

Step 1: Building the Robot Chassis

The first step of the project is ti build your own robot chassis. If bought from somewhere follow the steps. But you can make one for yourself with some acrylic or plastic sheets. I made a 4 wheel drive setup and two motors of one side are controlled by same signal.

Step2: Setting up Raspberry Pi

The next important step is to setup Raspberry Pi with OS and libraries required to do the task.

First start my flashing Raspbian on an SD card (16GB recommended). A wonderful guide is here: https://www.raspberrypi.org/documentation/installation/installing-images/
Note: Please install buster for RPi 4 otherwise Raspbian for other boards
notion image
After you install Raspbian on an SD card connect it to your wifi network and enable VNC as it can be used to access the desktop of RPi from your computer.
It is recommended to use Python3 for the project as Python2 is going out of support in few weeks and OpenCV easily gets installed if you use Python3!

Now we need to install some Libraries:

notion image
# install all dependent libraries of OpenCV (yes, this is one long command)

pi@raspberrypi:~ $  sudo apt-get install libhdf5-dev -y && sudo apt-get install libhdf5-serial-dev -y && sudo apt-get install libatlas-base-dev -y && sudo apt-get install libjasper-dev -y && sudo apt-get install libqtgui4 -y && sudo apt-get install libqt4-test -y# install OpenCV and other libraries

pi@raspberrypi:~ $ pip3 install opencv-python
Collecting opencv-python
Installing collected packages: numpy, opencv-python

Successfully installed numpy-1.16.2 opencv-python- $ pip3 install matplotlib
Collecting matplotlib
Collecting pyparsing!=2.0.4,!=2.1.2,!=2.1.6,>=2.0.1 (from matplotlib)
Successfully installed cycler-0.10.0 kiwisolver-1.1.0 matplotlib-3.0.3 numpy-1.16.3 pyparsing-2.4.0 python-dateutil-2.8.0 setuptools-41.0.1 six-1.12.0

Step3: Connections

notion image
This is the basic schematic but you can change the pin connections to the Raspberry Pi according to your wish. But remember to change it in the code.
You can replace a 9V battery with a LiPo (preferably for 4WD) as L298N have input voltage upto 20V easily.
Also you can directly connect the 5V out from the motor driver to RPi but I would not recommend it as it fluctuates and can harm your Pi.
notion image
I have used the pins 5,12,13,19 to connect pins IN1,IN2,IN3,IN4 of the motor driver.
Make sure to not connect any high voltage source directly to your Pi as it will kill it!

Step4: Full Code and Explanation

So first of all we need to import libraries

from picamera.array import PiRGBArray  
import RPi.GPIO as GPIO  
from picamera import PiCamera  
import time  
import cv2  
import numpy as np  
import math
Picamera for interfacing with the camera attached to the Pi.
RPi.GPIO to access the GPIOs to give commands to L298N motor driver to control the motors.
time for delays.
OpenCV (cv2) the main library to be used to do image processing
Numpy and math to perform certain operations.

Next we declare the GPIOs and some variables.

GPIO.setmode(GPIO.BCM) #Sets the mode to be used. BOARD will be numbered according to mapping of pins (1,2,3,4,...) and BCM for GPIOs as they are.  
#Declaring functions of the GPIOs  
GPIO.setup(13, GPIO.OUT)  
GPIO.setup(19, GPIO.OUT)  
GPIO.setup(5, GPIO.OUT)  
GPIO.setup(12, GPIO.OUT)  
#Setting all the GPIO to LOW uring startup. Refer to schmeatic for your mapping  
GPIO.output(5, GPIO.LOW)  
GPIO.output(12, GPIO.LOW)  
GPIO.output(19, GPIO.LOW)  
GPIO.output(13, GPIO.LOW)  
#Variable to store data about track and rotation.  
minLineLength = 5  
maxLineGap = 10

Next we use the PiCamera and link it to variables and perform startup commands

camera = PiCamera()  
camera.resolution = (640, 480)  
camera.framerate = 15  
rawCapture = PiRGBArray(camera, size=(640, 480))  

Then comes the actual program logic (broken into two parts here)

for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):  
   GPIO.output(5, GPIO.LOW)  
   GPIO.output(12, GPIO.LOW)  
   GPIO.output(13, GPIO.LOW)  
   GPIO.output(19, GPIO.LOW)  
   image = frame.array  
   gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) #Convert the image from color to Grayscale  
   blurred = cv2.GaussianBlur(gray, (5, 5), 0)  
   edged = cv2.Canny(blurred, 85, 85)  
   lines = cv2.HoughLinesP(edged,1,np.pi/180,10,minLineLength,maxLineGap)  
   if(lines !=[]):  
       for x in range(0, len(lines)):  
           for x1,y1,x2,y2 in lines[x]:  
First off all we open a loop that will go on continuosly to take input from the camera.
Then we set all our GPIOs LOW as its begining of detection (after first iteration)
OpenCV marks color in order of BGR (Blue, Green, Red) instead of RGB(Red, Green, Blue) for seemingly no reason.
Now we start with Image Processing.
First we convert all pixels to GrayScale
then we 'Smoothen the Images"
and use "Canny Edge Detection" to bring out the borders between the objects
And ultimately we use Hough Line Transformation basically identifies shapes and lines.
The explanation of these are greatly explained here:
opencv tutorial and explanation on Hough Line Transformation Hough Tranformation is a really good tool and very usefull.
Explaining the turn decision:
After drawing the lines we map out the distance as vectors and and measure their angles from horizontal. Its a very interesting technique. We add all the measured angles and predict the path of the lane.
notion image
As you can see in this image that most of the lines are drawn <Pi/2 so it return the value as right.
notion image
Even in this diffult scenarios when one lane is only visible because it measures the angles it can tell to go left which is correct
Note: the angle measured is in Radians not in degrees.

Now about controlling the robot with those data

       GPIO.output(5, GPIO.LOW)  
       GPIO.output(12, GPIO.HIGH)  
       GPIO.output(19, GPIO.LOW)  
       GPIO.output(13, GPIO.HIGH)  
       GPIO.output(12, GPIO.LOW)  
       GPIO.output(5, GPIO.HIGH)  
       GPIO.output(13, GPIO.LOW)  
       GPIO.output(19, GPIO.HIGH)  
      GPIO.output(5, GPIO.LOW)  
      GPIO.output(12, GPIO.HIGH)  
      GPIO.output(13, GPIO.LOW)  
      GPIO.output(19, GPIO.HIGH)  
      print ("straight")  
   key = cv2.waitKey(1) & 0xFF  
   if key == ord("q"):  
please see this code from gist linked below because indentation might be incorrect here.
First we declarte the threshold that we want I found 6 to be a good spot thoug 6.4 was best suited for me.
We check if the sum of angles (the length of line is also taken into consideration while measuring the angle) was greater than threshold so when it detected lines more that 90 degrees it says to go right. The GPIO output is done accordingly.
If it is lesser tha -6 that means it is in dfferent quadrant we know that it is because more lines are bend towards left.
Similarly for Straight going but you may have to change the threshold to make it go perfectly.
The we show the lines on the image as output and check if 'q' is pressed to quit and then the llop above goes on.


notion image
Video Demo
This is the first iteration that just uses some Image processing, I am working on next version where ML is used to train it to follow a path....


Overall I was very happy with the project even though it didn't perform extraordinary in navigating lanes.
Checkout the video if not yet.
But I had made the project in just 4 hours so I would call it a day.
I would like to add a better navigation by training a custom model and then predicting the path ahead. But that is for another day.
The next thing I learnt was that robot was too fast in moving and so the frame would freeze and it wouldn't detect any line and give error for no line detected and stop there. So it is better to create a new function and then use PWM signals to control the speed of the motors.
notion image
I would love to use more of OpenCV's inbuilt features to just use Image processing to navigate because once we involve training a model and then Deep Learning Implementation that is very heavy for the Pi.
Here you can see that even when the SBC is making critical decisions the CPU usage is not much and it stays cool.
Here you can see that even when the SBC is making critical decisions the CPU usage is not much and it stays cool.
You may also see some inefficiencies in the code because I learnt and gathered sources to make it in one day so it is rough. Let us make it a huge project and let us all collaborate to work on the existing framework to built on top a better Self-Driving car system.
What I learnt is that you can not go to the Tesla level because a lot of very smart engineers are working on the system to make it really "Autonomous". This makes me really optimistic for the future of Computer Vision.

Update 1:

As per dubbie 's comment I reduced the speed of the motor. I just connected both the enable pins (ENA and ENB) to GPIO 18 on Raspberry Pi. Then I enabled 1KHz 50% duty-cycle on Raspberry Pi code and that worked out very well. It can follow lanes now very well but the ride has become very jerky. Tying to find a sweet spot between them.