Fast Algorithms for Vision Based Navigation

For autonomous navigation, the car must be able to determine the angle at which it is heading with respect to the lane on the road. The biggest problem with taking a vision based approach is the computation time. The algorithm developed for this project overcomes this by being able to compute heading angles at speeds above 100 Hz. For reference, a typical video displays images at 30 or 60 Hz.

Raw Image Captured by the onboard, low-cost Pi Camera.

Size and Speed

Digital images are represented by 3-dimensional arrays in computers: one dimension for each axis of the image and another for the color channel. Then a sequence of matrix operations extracts the angle at which the car is heading . To make the algorithm fast, it is desired to reduce the 3-dimensional array to a 2-dimensional array, without performing any computations. To do so, only one color channel of the image was chosen and the other two were discarded.

Blue Color Channel of Image. The “bluest” pixels are represented by white (255), and the least blue pixels is represented by black (0).
Red Color Channel of Image. The “reddest” pixels are represented by white (255), and the least red pixels is represented by black (0).
Green Color Channel Of Image. The “greenest” pixels are represented by white (255), and the least red pixels is represented by black (0).

The red color channel shows the highest contrast between the lane and the road, so the algorithm proceeds using only this image. The resulting matrix is now only one third of the size as the original three dimensional array, which makes the image processing algorithm three times as fast.

Edge Detection

The next step is to produce a binary image where the edges between the lane and the road is white while the rest is black. The underlying idea behind this step of the algorithm is to determine whether the difference between adjacent pixels are large enough to constitute an edge.

Since the computer cannot distinguish the darker pebbles on the road from the black paint, the algorithm might think that some of them are dark enough to be considered edges. Blurring the image with a Gaussian kernel reduces the difference between adjacent pixels. This ensures that the only large differences between adjacent pixels correspond to the boundary between the lane and the road.

Red color channel of image after applying a Gaussian Kernel.

To emphasize the difference between the lane and the road, the algorithm reassigns the pixels larger than a threshold value to white (255) and the rest to black (0). This threshold value is determined by making a histogram of the pixel values (not online, because the threshold does not change).

At this stage, a Canny edge detector will produce a binary image where the edges of the lane are white and the rest is black. For our project, we used the Canny edge detector provided by the OpenCV library for python.

The large Bump centered at around 200 are the values of the pixels corresponding to the road, while the smaller bump at around 150-175 represents the darker lane. The threshold value for our algorithm is 170.
Thresholded Image

Canny Edge Detector Output

Computing The Heading Angle Using a Hough Transform

The hough transform computes the angle between the horizontal axis of the image and a line perpendicular to the largest “line” produced in the output of the image in the canny edge detector. The OpenCV library for python provides the code for the Hough transform. It finds all of the lines between white dots, and outputs the angle and distance to the origin a perpendicular line to the collinear points in the image. We elected to use the angle of the line with the most collinear points, and converted that angle to the vehicle body frame of reference. This is the angle used that is fed back into the control system.

The red line on the original image is the found by the Hough transform. Computing the angle of it with respect to the body frame of the vehicle yields the heading angle of the car needed for controlling the steering of the car.

Optimal Control For Steering

For our project, a Linear Quadratic Regulator was designed to regulate the heading angle and yaw rate to zero. At a constant velocity, this means that the car will be traveling in parallel to the lane. A linear time state space model adapted from Chapter 2 of Vehicle Dynamics and Control by Rajesh Rajamani describes the steering dynamics. In this model, psi denote the heading angle of the car, r denotes the yaw rate, and delta denotes the steering angle input. in the vehicle body frame.

To determine the parameters of the model, an experiment was devised. The car was set to a constant velocity, and allowed to drive straight. After 1 second, a 0.5 constant radian turn was commanded for 4 seconds. The gyroscope readings of the yaw rate was recorded. This data was fitted to a first order transfer function, where the input is the steering angle and the output is the yaw rate. The resulting model is then used for control design.

This is a plot of the experimental data collected along with a simulation of the response of the first-order approximated transfer function to the experimental input.

Using this state space model, a linear quadratic regulator was designed to regulate psi as quickly as possible without exceeding the saturation limits of the steering angle. To do so, the penalty on the heading angle was as large as possible without exceeding the saturation limit, while the yaw rate was left unpenalized. The following plot is a simulation of the performance of the car traveling at a constant velocity when a sudden pi/6 radian turn is detected by the image processing algorithm.