How to work with PID controller for A.R Drone 2.0?

Question

I have been looking at codes for autonomous drones and encountered this one on this repository:https://github.com/puku0x/cvdrone . Im trying to understand the code, I am new to controller algorithms and OpenCV. I tried going on the OpenCV website and understand functions but it didn't help very much. Any help would be appreciated.

        // Tracking

    if (contour_index >= 0) {
        // Moments
        cv::Moments moments = cv::moments(contours[contour_index], true);
        double marker_y = (int)(moments.m01 / moments.m00);
        double marker_x = (int)(moments.m10 / moments.m00);

        // Show result
        cv::Rect rect = cv::boundingRect(contours[contour_index]);
        cv::rectangle(image, rect, cv::Scalar(0, 255, 0));

        if (track) {
            const double kp = 0.005;
            vx = kp * (binalized.rows / 2 - marker_y);;
            vy = 0.0;
            vz = kp; 
            vr = kp * (binalized.cols / 2 - marker_x);
            std::cout << "(vx, vy, vz, vr)" << "(" << vx << "," << vy << "," << vz << "," << vr << ")" << std::endl;
            std::cout << "Altitude = " << ardrone.getAltitude() << "%" << std::endl;
        }
              // Marker tracking
    if (track) {
        // PID gains
        const double kp = 0.001;
        const double ki = 0.000;
        const double kd = 0.000;

        // Errors
        double error_x = (binalized.rows / 2 - marker.y);   // Error front/back
        double error_y = (binalized.cols / 2 - marker.x);   // Error left/right

        // Time [s]
        static int64 last_t = 0.0;
        double dt = (cv::getTickCount() - last_t) / cv::getTickFrequency();
        last_t = cv::getTickCount();

        // Integral terms
        static double integral_x = 0.0, integral_y = 0.0;
        if (dt > 0.1) {
            // Reset
            integral_x = 0.0;
            integral_y = 0.0;
        }
        integral_x += error_x * dt;
        integral_y += error_y * dt;

        // Derivative terms
        static double previous_error_x = 0.0, previous_error_y = 0.0;
        if (dt > 0.1) {
            // Reset
            previous_error_x = 0.0;
            previous_error_y = 0.0;
        }
        double derivative_x = (error_x - previous_error_x) / dt;
        double derivative_y = (error_y - previous_error_y) / dt;
        previous_error_x = error_x;
        previous_error_y = error_y;

        // Command velocities
        vx = kp * error_x + ki * integral_x + kd * derivative_x;
        vy = 0.0;//kp * error_y + ki * integral_y + kd * derivative_y;
        vz = 0.0;
        vr = 0.0;

    }
    }

    // Display the image
    cv::putText(image, (track) ? "track on" : "track off", cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, (track) ? cv::Scalar(0, 0, 255) : cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
    cv::imshow("camera", image);
    ardrone.move3D(vx, vy, vz, vr);
}

Answer 1

Your question is a little general, but I'll see if I can give you an overview to help you get started. (Also see this similar question: Tracking objects from camera; PID controlling; Parrot AR Drone 2.)

This code (which is from sample_tracking.cpp in the cvdrone repository) is trying to do the following:

1. Find the biggest object (of the desired color) in view of the camera.
2. Move the drone so that the object is in the center of the camera's view.

It uses OpenCV to accomplish the first task, and PIDs to accomplish the second task.

Find the biggest object (of the desired color) in view of the camera

To do this, the code grabs a frame from the drone's video camer, looks for the biggest blob of the desired color, and finds the center of that blob.

It uses OpenCV to achieve this by doing the following:

1. Use InRange to threshold the image, which turns on pixels that are close to the target color and turns off pixels that are far away. Now you have an image that only contains white and black pixels, where the white pixels correspond to the color you're looking for. This blog post has a good example of using InRange to find the cubelets of a certain color on a Rubik's Cube: Color spaces in OpenCV.

2. Use morphologyEx with MORPH_CLOSE to remove noise from the image. This should make it easier to find the desired blobs. See section 4, "Closing" on this page for an example of what the result of this processing looks like.

3. Use findContours to find the blobs of pixels in the image. A contour is "a curve joining all the continuous points (along the boundary), having same color or intensity", so this will find the outlines of all white blobs in the image.

Example input:

Example result:

4. Use contourArea to find the largest contour, by area. This is going to be the object the drone will track.

5. Use moments to find the centroid of the largest contour. This is how it determines the image coordinates of the object. This page documenting contour features has more information on moments.

So now the code has the coordinates of the object it wants to track. Next comes the part where it actually moves the drone to do the tracking.

Move the drone so that the object is in the center of the camera's view

PID control is a big subject. I'll just describe the basics and what this code does; if you want more information there are lots of introductory resources, like this article, "Introduction to PID control", or this video: "PID Control - A brief introduction".

A PID controller takes into account 3 things:

1. Proportional term: How far away are we from where we want to be?
2. Derivative term: How fast are we moving toward (or away from) where we want to be?
3. Integral term: What is the accumulated error over time between where we are and where we want to be? (integral term)

The proportional term moves you toward the target. The derivative term slows you down when you're quickly moving toward the target, so you don't overshoot. The integral term can help nudge you when you're stopped just a little ways away from the target.

It turns out that because of the following 3 lines of code, this function isn't really running a full PID controller:

// PID gains
const double kp = 0.001;
const double ki = 0.000;
const double kd = 0.000;

Since the gains for the integral and derivative portion of the controller are 0, this is just a simple proportional controller: The code is just looking at the difference between the coordinates of the target and the center of the image, and using that to decide how to drive the drone.

First, here's what the cvdrone code uses for the AR.Drone coordinate system:

Front of the AR.Drone is X-axis, left is Y-axis, upper is Z-axis. Also front is 0.0 [rad], each axis CCW is positive.

        X
       +^-
        |
        |
Y <-----+ (0,0)
        Z

Here's where the drone movement is calculated and then commanded:

// Command velocities
vx = kp * error_x + ki * integral_x + kd * derivative_x;
vy = 0.0;//kp * error_y + ki * integral_y + kd * derivative_y;
vz = 0.0;
vr = 0.0;

// ...

ardrone.move3D(vx, vy, vz, vr);

So the code calculates vx, which is the drone's desired forward/backward velocity, as kp * error_x (the other terms are zero because kd and ki are zero). error_x is the delta in pixels between the target's Y coordinate in the image and the image's center Y coordinate.

I don't remember the actual resolution of the AR.Drone camera, but let's assume it's 320x480. That means the center of the image is at y=240.

If the center of the target was close to the top of the image, say y = 15 pixels, then error_x = 240 - 15 = 225. Then vx = kp * error_x = 0.001 * 225 = 0.225. The call to move3D would then cause the drone to move forward with velocity 0.225.

Say that at the next time step, the drone has moved forward a bit and as a result the stationary tracked object has moved slightly down in the camera's field of view, so now the center is at y = 40 pixels. Then error_x = 240 - 40 = 200, and vx = 0.001 * 200 = 0.200. So now the call to move3D causes the drone to keep moving forward, but more slowly than before.

As the drone moves forward and the target moves closer to the center of the camera's field of view, the drone will continue to slow down. Most likely the drone will overshoot the target, and hopefully the camera will see the target slightly below the center of the field of view, which will lead to a negative error_x, and a negative vx, leading the drone to move slowly backwards.

And that's a super quick into to how this code uses OpenCV and PID controllers to track an object. Typically you'll actually have a separate PID controller for each axis that you can move the drone in: forward/back translation, left/right translation, clockwise/counter-clockwise rotation. You might try setting the kd and ki terms to a small negative and small positive value, respectively, to see how that changes the drone's behavior. This video is one I made that demonstrates using an AR.Drone and two PID controllers, one for forward/back and one for left/right, to track a series of waypoints.

There are also other ways to use OpenCV to do object tracking. This video demonstrates using the camshift/meanshift technique (not on a drone). This video demonstrates using color histogram tracking on the AR.Drone.