DeepPiCar — Part 4: Autonomous Lane Navigation via OpenCV | by David Tian | Towards Data Science

With all the hardware (Part 2) and software (Part 3) set up out of the way, we are ready to have some fun with the car! In this article, we will use a popular, open-source computer vision package, called OpenCV, to help PiCar autonomously navigate within a lane. Note this article will just make our PiCar a “self-driving car”, but NOT yet a deep learning, self-driving car. The deep learning part will come in Part 5 and Part 6.
Currently, there are a few 2018–2019 cars on the market that have these two features onboard, namely, Adaptive Cruise Control (ACC) and some forms of Lane Keep Assist System (LKAS). Adaptive cruise control uses radar to detect and keep a safe distance with the car in front of it. This feature has been around since around 2012–2013. Lane Keep Assist System is a relatively new feature, which uses a windshield mount camera to detect lane lines, and steers so that the car is in the middle of the lane. This is an extremely useful feature when you are driving on a highway, both in bumper-to-bumper traffic and on long drives. When my family drove from Chicago to Colorado on a ski trip during Christmas, we drove a total of 35 hours. Our Volvo XC 90, which has both ACC and LKAS (Volvo calls it PilotAssit) did an excellent job on the highway, as 95% of the long and boring highway miles were driven by our Volvo! All I had to do was to put my hand on the steering wheel (but didn’t have to steer) and just stare at the road ahead. I didn’t need to steer, break, or accelerate when the road curved and wound, or when the car in front of us slowed down or stopped, not even when a car cut in front of us from another lane. The few hours that it couldn’t drive itself was when we drove through a snowstorm when lane markers were covered by snow. (Volvo, if you are reading this, yes, I will take endorsements! :) Curious as I am, I thought to myself: I wonder how this works, and wouldn’t it be cool if I could replicate this myself (on a smaller scale)?
Today, we will build LKAS into our DeepPiCar. Implementing ACC requires a radar, which our PiCar doesn’t have. In a future article, I may add an ultrasonic sensor on DeepPiCar. Ultrasound, similar to radar, can also detect distances, except at closer ranges, which is perfect for a small scale robotic car.
A lane keep assist system has two components, namely, perception (lane detection) and Path/Motion Planning (steering). Lane detection’s job is to turn a video of the road into the coordinates of the detected lane lines. One way to achieve this is via the computer vision package, which we installed in Part 3, OpenCV. But before we can detect lane lines in a video, we must be able to detect lane lines in a single image. Once we can do that, detecting lane lines in a video is simply repeating the same steps for all frames in a video. There are many steps, so let’s get started!
When I set up lane lines for my DeepPiCar in my living room, I used the blue painter’s tape to mark the lanes, because blue is a unique color in my room, and the tape won’t leave permanent sticky residues on the hardwood floor.
Above is a typical video frame from our DeepPiCar’s DashCam. The first thing to do is to isolate all the blue areas on the image. To do this, we first need to turn the color space used by the image, which is RGB (Red/Green/Blue) into the HSV (Hue/Saturation/Value) color space. (Read this for more details on the HSV color space.) The main idea behind this is that in an RGB image, different parts of the blue tape may be lit with different light, resulting them appears as darker blue or lighter blue. However, in HSV color space, the Hue component will render the entire blue tape as one color regardless of its shading. It is best to illustrate with the following image. Notice both lane lines are now roughly the same magenta color.
Below is the OpenCV command to do this.
Note that we used a BGR to HSV transformation, not RBG to HSV. This is because OpenCV, for some legacy reasons, reads images into BGR (Blue/Green/Red) color space by default, instead of the more commonly used RGB (Red/Green/Blue) color space. They are essentially equivalent color spaces, just order of the colors swapped.
Once the image is in HSV, we can “lift” all the blueish colors from the image. This is by specifying a range of the color Blue.
In Hue color space, the blue color is in about 120–300 degrees range, on a 0–360 degrees scale. You can specify a tighter range for blue, say 180–300 degrees, but it doesn’t matter too much.
Here is the code to lift Blue out via OpenCV, and rendered mask image.
Note OpenCV uses a range of 0–180, instead of 0–360, so the blue range we need to specify in OpenCV is 60–150 (instead of 120–300). These are the first parameters of the lower and upper bound arrays. The second (Saturation) and third parameters (Value) are not so important, I have found that the 40–255 ranges work reasonably well for both Saturation and Value.
Note this technique is exactly what movie studios and weatherperson use every day. They usually use a green screen as a backdrop, so that they can swap the green color with a thrilling video of a T-Rex charging towards us (for a movie), or the live doppler radar map (for the weatherperson).
Next, we need to detect edges in the blue mask so that we can have a few distinct lines that represent the blue lane lines.
The Canny edge detection function is a powerful command that detects edges in an image. In the code below, the first parameter is the blue mask from the previous step. The second and third parameters are lower and upper ranges for edge detection, which OpenCV recommends to be (100, 200) or (200, 400), so we are using (200, 400).
Putting the above commands together, below is the function that isolates blue colors on the image and extracts edges of all the blue areas.
From the image above, we see that we detected quite a few blue areas that are NOT our lane lines. A closer look reveals that they are all at the top half of the screen. Indeed, when doing lane navigation, we only care about detecting lane lines that are closer to the car, where the bottom of the screen. So we will simply crop out the top half. Boom! Two clearly marked lane lines as seen on the image on the right!
Here is the code to do this. We first create a mask for the bottom half of the screen. Then when we merge themask with the edgesimage to get the cropped_edges image on the right.
In the cropped edges image above, to us humans, it is pretty obvious that we found four lines, which represent two lane lines. However, to a computer, they are just a bunch of white pixels on a black background. Somehow, we need to extract the coordinates of these lane lines from these white pixels. Luckily, OpenCV contains a magical function, called Hough Transform, which does exactly this. Hough Transform is a technique used in image processing to extract features like lines, circles, and ellipses. We will use it to find straight lines from a bunch of pixels that seem to form a line. The function HoughLinesP essentially tries to fit many lines through all the white pixels and return the most likely set of lines, subject to certain minimum threshold constraints. (Read here for an in-depth explanation of Hough Line Transform.)
Here is the code to detect line segments. Internally, HoughLineP detects lines using Polar Coordinates. Polar Coordinates (elevation angle and distance from the origin) is superior to Cartesian Coordinates (slope and intercept), as it can represent any lines, including vertical lines which Cartesian Coordinates cannot because the slope of a vertical line is infinity. HoughLineP takes a lot of parameters:
rho is the distance precision in pixel. We will use one pixel.
angle is angular precision in radian. (Quick refresher on Trigonometry: radian is another way to express the degree of angle. i.e. 180 degrees in radian is 3.14159, which is π) We will use one degree.
min_threshold is the number of votes needed to be considered a line segment. If a line has more votes, Hough Transform considers them to be more likely to have detected a line segment
minLineLength is the minimum length of the line segment in pixels. Hough Transform won’t return any line segments shorter than this minimum length.
maxLineGap is the maximum in pixels that two line segments that can be separated and still be considered a single line segment. For example, if we had dashed lane markers, by specifying a reasonable max line gap, Hough Transform will consider the entire dashed lane line as one straight line, which is desirable.
Setting these parameters is really a trial and error process. Below are the values that worked well for my robotic car with a 320x240 resolution camera running between solid blue lane lines. Of course, they need to be re-tuned for a life-sized car with a high-resolution camera running on a real road with white/yellow dashed lane lines.
If we print out the line segment detected, it will show the endpoints (x1, y1) followed by (x2, y2) and the length of each line segment.
INFO:root:Creating a HandCodedLaneFollower...DEBUG:root:detecting lane lines...DEBUG:root:detected line_segment:DEBUG:root:[[  7 193 107 120]] of length 123DEBUG:root:detected line_segment:DEBUG:root:[[226 131 305 210]] of length 111DEBUG:root:detected line_segment:DEBUG:root:[[  1 179 100 120]] of length 115DEBUG:root:detected line_segment:DEBUG:root:[[287 194 295 202]] of length 11DEBUG:root:detected line_segment:DEBUG:root:[[241 135 311 192]] of length 90
Now that we have many small line segments with their endpoint coordinates (x1, y1) and (x2, y2), how do we combine them into just the two lines that we really care about, namely the left and right lane lines? One way is to classify these line segments by their slopes. We can see from the picture above that all line segments belonging to the left lane line should be upward sloping and on the left side of the screen, whereas all line segments belonging to the right lane line should be downward sloping and be on the right side of the screen. Once the line segments are classified into two groups, we just take the average of the slopes and intercepts of the line segments to get the slopes and intercepts of left and right lane lines.
The average_slope_intercept function below implements the above logic.
make_points is a helper function for the average_slope_intercept function, which takes a line’s slope and intercept, and returns the endpoints of the line segment.
Other than the logic described above, there are a couple of special cases worth discussion.
One lane line in the image: In normal scenarios, we would expect the camera to see both lane lines. However, there are times when the car starts to wander out of the lane, maybe due to flawed steering logic, or when the lane bends too sharply. At this time, the camera may only capture one lane line. That’s why the code above needs to check len(right_fit)>0 and len(left_fit)>0Vertical line segments: vertical line segments are detected occasionally as the car is turning. Although they are not erroneous detections, because vertical lines have a slope of infinity, we can’t average them with the slopes of other line segments. For simplicity’s sake, I chose to just to ignore them. As vertical lines are not very common, doing so does not affect the overall performance of the lane detection algorithm. Alternative, one could flip the X and Y coordinates of the image, so vertical lines have a slope of zero, which could be included in the average. But then the horizontal line segments would have a slope of infinity, but that would be extremely rare, since the DashCam is generally pointing at the same direction as the lane lines, not perpendicular to them. Another alternative is to represent the line segments in polar coordinates and then averaging angles and distance to the origin.
One lane line in the image: In normal scenarios, we would expect the camera to see both lane lines. However, there are times when the car starts to wander out of the lane, maybe due to flawed steering logic, or when the lane bends too sharply. At this time, the camera may only capture one lane line. That’s why the code above needs to check len(right_fit)>0 and len(left_fit)>0
Vertical line segments: vertical line segments are detected occasionally as the car is turning. Although they are not erroneous detections, because vertical lines have a slope of infinity, we can’t average them with the slopes of other line segments. For simplicity’s sake, I chose to just to ignore them. As vertical lines are not very common, doing so does not affect the overall performance of the lane detection algorithm. Alternative, one could flip the X and Y coordinates of the image, so vertical lines have a slope of zero, which could be included in the average. But then the horizontal line segments would have a slope of infinity, but that would be extremely rare, since the DashCam is generally pointing at the same direction as the lane lines, not perpendicular to them. Another alternative is to represent the line segments in polar coordinates and then averaging angles and distance to the origin.
Putting the above steps together, here is detect_lane() function, which given a video frame as input, returns the coordinates of (up to) two lane lines.
We will plot the lane lines on top of the original video frame:
Here is the final image with the detected lane lines drawn in green.
Now that we have the coordinates of the lane lines, we need to steer the car so that it will stay within the lane lines, even better, we should try to keep it in the middle of the lane. Basically, we need to compute the steering angle of the car, given the detected lane lines.
This is the easy scenario, as we can compute the heading direction by simply averaging the far endpoints of both lane lines. The red line shown below is the heading. Note that the lower end of the red heading line is always in the middle of the bottom of the screen, that’s because we assume the dashcam is installed in the middle of the car and pointing straight ahead.
If we only detected one lane line, this would be a bit tricky, as we can’t do an average of two endpoints anymore. But observe that when we see only one lane line, say only the left (right) lane, this means that we need to steer hard towards the right(left), so we can continue to follow the lane. One solution is to set the heading line to be the same slope as the only lane line, as shown below.
Now that we know where we are headed, we need to convert that into the steering angle, so that we tell the car to turn. Remember that for this PiCar, the steering angle of 90 degrees is heading straight, 45–89 degrees is turning left, and 91–135 degrees is turning right. Below is some trigonometry to convert a heading coordinate to a steering angle in degrees. Note that PiCar is created for common men, so it uses degrees and not radians. But all trig math is done in radians.
We have shown several pictures above with the heading line. Here is the code that renders it. The input is actually the steering angle.
Initially, when I computed the steering angle from each video frame, I simply told the PiCar to steer at this angle. However, during actual road testing, I have found that the PiCar sometimes bounces left and right between the lane lines like a drunk driver, sometimes go completely out of the lane. I then found out that it is caused by the steering angles, computed from one video frame to the next frame, are not very stable. You should run your car in the lane without stabilization logic to see what I mean. Some times, the steering angle may be around 90 degrees (heading straight) for a while, but, for whatever reason, the computed steering angle could suddenly jump wildly, to say 120 (sharp right) or 70 degrees(sharp left). As a result, the car would jerk left and right within the lane. Clearly, this is not desirable. We need to stabilize steering. Indeed, in real life, we have a steering wheel, so that if we want to steer right, we turn the steering wheel in a smooth motion, and the steering angle is sent as a continuous value to the car, namely, 90, 91, 92, .... 132, 133, 134, 135 degrees, not 90 degrees in one millisecond, and 135 degrees in next millisecond.
So my strategy to stable steering angle is the following: if the new angle is more than max_angle_deviation degree from the current angle, just steer up to max_angle_deviation degree in the direction of the new angle.
In the above code, I used two flavors of max_angle_deviation, 5 degrees if both lane lines are detected, which means we are more confident that our heading is correct; 1 degree if only one lane line is detected, which means we are less confident. These are parameters one can tune for his/her own car.
The complete code to perform LKAS (Lane Following) is in my DeepPiCar GitHub repo. In DeepPiCar/driver/code folder, these are the files of interest:
deep_pi_car.py: This is the main entry point of the DeepPiCar
hand_coded_lane_follower.py: This is the lane detection and following logic.
Just run the following commands to start your car. (Of course, I am assuming you have taped down the lane lines and put the PiCar in the lane.)
# skip this line if you have already cloned the repopi@raspberrypi:~ $ git clone https://github.com/dctian/DeepPiCar.gitpi@raspberrypi:~ $ cd DeepPiCar/driver/codepi@raspberrypi:~/DeepPiCar/driver/code $ python3 deep_pi_car.py INFO :2019-05-08 01:52:56,073: Creating a DeepPiCar...DEBUG:2019-05-08 01:52:56,093: Set up cameraDEBUG:2019-05-08 01:52:57,639: Set up back wheelsDEBUG "back_wheels.py": Set debug offDEBUG "TB6612.py": Set debug offDEBUG "TB6612.py": Set debug offDEBUG "PCA9685.py": Set debug offDEBUG:2019-05-08 01:52:57,646: Set up front wheelsDEBUG "front_wheels.py": Set debug offDEBUG "front_wheels.py": Set wheel debug offDEBUG "Servo.py": Set debug offINFO :2019-05-08 01:52:57,665: Creating a HandCodedLaneFollower...
If your setup is very similar to mine, your PiCar should go around the room like below! Type Q to quit the program.
In this article, we taught our DeepPiCar to autonomously navigate within lane lines (LKAS), which is pretty awesome, since most cars on the market can’t do this yet. However, this is not very satisfying, because we had to write a lot of hand-tuned code in python and OpenCV to detect color, detect edges, detect line segments, and then have to guess which line segments belong to the left or right lane line. In this article, we had to set a lot of parameters, such as upper and lower bounds of the color blue, many parameters to detect line segments in Hough Transform, and max steering deviation during stabilization. In fact, we did not use any deep learning techniques in this project. Wouldn’t it be cool if we can just “show” DeepPiCar how to drive, and have it figure out how to steer? This is the promise of deep learning and big data, isn't it? In the next article, this is exactly what we will build, a deep learning, autonomous car that can learn by observing how a good driver drive. See you in Part 5.
Here are the links to the whole guide:
Part 1: Overview
Part 2: Raspberry Pi Setup and PiCar Assembly
Part 3: Make PiCar See and Think
Part 4: Autonomous Lane Navigation via OpenCV (This article)
Part 5: Autonomous Lane Navigation via Deep Learning
Part 6: Traffic Sign and Pedestrian Detection and Handling