Calculating Cross Track Error (CTE) and steering the robot

I am one step ahead to build a full self driving toy car which is a big step toward creating a full size car.

I am now able to calculate cross track error and steer the robots front wheel to correct it. The steering driver is very crude at this moment- it has three positions Left, Middle and Right. I'll have to send more positions using a second parameter that tells exact angle. The servo motor can position itself between 0 to 180 degree angle with 1 degree resolution where 90 degree is straight forward. Wanted to make it work first then make it precise. Here is the video- please pay attention to the robots front wheel and where the lane is:

I first detect two lane marking lines and group them together (if one line is in 100 pixel, configurable, from another line they form one group). Then calculate the mid point for each group that gives me the list of lane marking. The lanes can be curved (I have to calculate a 3rd degree  bezier spline instead of straight lines that I use right now) - but still all lines should be within 100 pixel of each other for one lane marking- its like the snake algorithm to track the lanes while not exact. 

After I get all the lane markings I take the middle point of the lane from each neighboring pair of lane marking. Then from the image position I get the current car/ camera position and calculate the cross track error (CTE) by getting the difference from the lane mid point. 

I use kalman filter to eliminate single frame errors and over multiple frames the values stables (in less than 20 frames it becomes almost correct). Jetson TX1 GPU array can process 20 frames per second without any optimization effort. Here is how it looks for a single frame (this is bird eye view- transformed using wrap transform of original frame):

The image shows detected lane marking using thicker straight line, lane mid point (green) and current car position (pink) and on top it shows frame #, car position, CTE value and which way it should steer the the car.

Detecting lines using proabilistic hough transform

We use Canny algorithm to create a binary image and then use the HoughLinesP function detects line in binary image.

First we take the bird eye view image of straight labe for processing.

When the HoughLinesP is applied we get following resulting lines:

Then we take a curved lane image

When the HoughLinesP is applied we get following resulting lines:

Here is the code.

int hough(const char *input_path, const char *output_path)
{
    Mat src_o, src, dst;
    src_o = imread(input_path);

    cvtColor(src_o, src, CV_BGR2GRAY );

    Canny(src, dst, 200, 255, 3 );

    vector lines;
    HoughLinesP(dst, lines, 1, CV_PI/180, 20, 10, 20 );

    std::map  candidate;

    for( size_t i = 0; i < lines.size(); i++ )
    {
        line( src_o, Point(lines[i][0], lines[i][1]),
              Point(lines[i][2], lines[i][3]), Scalar(0,0,255), 3, 8 );
    }

    imwrite(output_path, src_o);

    return 0;
}

Next step is to take the lines and calculate the lane midpoint.

The self driving car platform (GTAR)

We are using the Traxxas Slash 4x4 and Jetson TX1 (which provides 1 TeraFLOPs peak performance) to build the self driving car platform.

First we remove the body and the radio controller from the car and added 4 narrow bolts to hold to plastic platforms.

We are going to use two piece of plastic sheets like below. Both has a few holes.

Here is how it looks after adding the lower platform.

Now we detach the Jetson TX1 bottom plastic base and remove the plastic legs.

Attach the Jetson TX1 plastic base to the lower platform. The onboard camera is facing front.

Add the Jetson TX1 board and the power supply to the platform

.

And add the upper platform and the sonar sensor in the front.

I have not received the camera yet. The camera will be placed on the upper platform and the GPS, gyroscope and compass will also be placed on the top platform.

The lower platform needs some cut to free the front and rear wheels. Other than that its complete and we are ready to add the remaining electronics and start coding.

Finally the sensor detecting obstacle in front of it:

Lane Detection: Step 1: Inverse/Wrap perspective mapping

To detect lane we first do some image transformation. First lets consider the following image. It has lane marking (green). The lanes are almost parallel in real world, but as we see in the image the lanes seem wider at the bottom and narrower at the top.

To make it easier we want to have a bird eye view so that the lane lines become parallel to each other. OpenCV has built-in function to do the transformation. So we just use that. The process is called wrap perspective mapping (WPM) and we need to provide 4 points on the original image (I have marked them red in the image above) and a corresponding real world co-ordinate. I have just made up the numbers to make them parallel.
Here is the result:

And here is the code (just a demo- without error handling):

#include "iostream"

#include "cv.h"
#include "highgui.h"

using namespace std;
using namespace cv;


void ipm(const char *input_path, const char *output_path)
{
    Mat src, dst;
    src=imread(input_path, 1);

    Point2f img_vertices[4];
    img_vertices[0] = Point(213, 104);
    img_vertices[1] = Point(410, 106);
    img_vertices[2] = Point(6, 372);
    img_vertices[3] = Point(540, 400);

    Point2f world_vertices[4];
    world_vertices[0] = Point(26, 104);
    world_vertices[1] = Point(540, 106);
    world_vertices[2] = Point(26, 400);
    world_vertices[3] = Point(540, 400);

    Mat warpMatrix = getPerspectiveTransform(img_vertices, world_vertices);
    
    warpPerspective(src, dst, warpMatrix, dst.size(), INTER_LINEAR);

    imwrite(output_path, dst);    
}


int main(int argc, char** argv)
{
    ipm(argv[1], "ipm.jpg");
    return 0;
}

At the end of all steps the goal is to detect all paths and objects 

in the http://www.cvlibs.net/datasets/kitti/index.php dataset.

Self driving toy car

While companies are building self driving cars for adults we are building self driving cars for kids. We took a car like this image and converted it into a self driving one.

Kids (3-5 years) can ride on it. The car runs at 5mph maximum speed and uses rechargeable batteries as power source.

If you are interested about the first edition of the cool car (or interested to get the source code of the probabilistic algorithm) subscribe to the mailing list on the right (Follow by Email). We will not use it for any other purpose than infrequent communication about the self driving toy car and we will not give it to others without your permission. You will be able to unsubscribe anytime.