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.
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.
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.
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.
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.
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.
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.