如何使用PID控制器为A.R Drone 2.0？

Question

我一直在寻找自动无人机的代码，并在这个存储库中遇到过这个：https://github.com/puku0x/cvdrone。我试图理解代码，我是控制器算法和OpenCV的新手。我试过去OpenCV网站并了解功能，但它没有多大帮助。任何帮助，将不胜感激。

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

你的问题有点笼统，但我会看看能不能给你一个概述来帮助你入门。 （另见类似问题：Tracking objects from camera; PID controlling; Parrot AR Drone 2。）

此代码（来自sample_tracking.cpp存储库中的cvdrone）正在尝试执行以下操作：

1. 从相机中查找最大的物体（所需颜色）。
2. 移动无人机，使对象位于摄像机视图的中心。

它使用OpenCV完成第一个任务，使用PID完成第二个任务。

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

为此，代码从无人机摄像机抓取一个帧，寻找所需颜色的最大斑点，并找到该斑点的中心。

它使用OpenCV通过执行以下操作来实现此目的：

1. 使用InRange来阈值图像，这会打开接近目标颜色的像素并关闭远处的像素。现在您的图像只包含白色和黑色像素，其中白色像素对应于您要查找的颜色。这篇博客文章有一个很好的例子，使用InRange在魔方：魔芋上查找某种颜色的魔方：Color spaces in OpenCV。
2. 使用morphologyEx和MORPH_CLOSE去除图像中的噪点。这应该可以更容易地找到所需的blob。有关此处理结果的示例，请参阅this page上的第4节“结束”。
3. 使用findContours查找图像中的像素斑点。轮廓是“连接所有连续点（沿着边界）的曲线，具有相同的颜色或强度”，因此这将找到图像中所有白色斑点的轮廓。

输入示例：

示例结果：

4. 使用contourArea按区域查找最大轮廓。这将是无人机将跟踪的对象。
5. 使用moments找到最大轮廓的质心。这是它如何确定对象的图像坐标。 This page documenting contour features有关于时刻的更多信息。

所以现在代码具有它想要跟踪的对象的坐标。接下来是实际移动无人机进行跟踪的部分。

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

PID控制是一个很大的主题。我将只描述基础知识以及此代码的作用;如果你想了解更多信息，那么有很多介绍性的资源，比如这篇文章，"Introduction to PID control"，或者这个视频："PID Control - A brief introduction"。

PID控制器考虑了3件事：

1. 比例术语：我们离我们想要的地方有多远？
2. 衍生术语：我们走向（或远离）我们想要的速度有多快？
3. 积分项：我们所处的位置和我们想要的位置之间的累积误差是多少？ （积分项）

比例项将您带向目标。当您快速移向目标时，导数项会减慢您的速度，因此您不会超调。当你在离目标稍远的地方停下来时，积分项有助于推动你。

事实证明，由于以下3行代码，此函数并未真正运行完整的PID控制器：

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

由于控制器的积分和微分部分的增益为0，这只是一个简单的比例控制器：代码只是查看目标坐标和图像中心之间的差异，并用它来决定如何驾驶无人机。

首先，这是cvdrone代码uses for the AR.Drone coordinate system：

AR.Drone的前面是X轴，左边是Y轴，上面是Z轴。前面也是0.0 [rad]，每个轴CCW是正的。

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

这是计算无人机运动然后命令的地方：

// 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);

因此，代码计算vx，这是无人机所需的前向/后向速度，为kp * error_x（其他项为零，因为kd和ki为零）。 error_x是图像中目标的Y坐标与图像的中心Y坐标之间的像素增量。

我不记得AR.Drone相机的实际分辨率，但我们假设它是320x480。这意味着图像的中心位于y = 240。

如果目标的中心靠近图像的顶部，比如y = 15像素，那么error_x = 240 - 15 = 225。然后vx = kp * error_x = 0.001 * 225 = 0.225。对move3D的调用将导致无人机以0.225的速度向前移动。

假设在下一个时间步，无人机向前移动了一点，因此静止跟踪物体在相机的视野中略微向下移动，所以现在中心位于y = 40像素处。然后error_x = 240 - 40 = 200和vx = 0.001 * 200 = 0.200。所以现在对move3D的召唤导致无人机继续前进，但比之前更慢。

当无人机向前移动并且目标移近摄像机视野的中心时，无人机将继续减速。很可能无人机将超过目标，并希望相机将看到目标略低于视野中心，这将导致负error_x和负vx，导致无人机慢慢向后移动。

这是一个超级快速的代码如何使用OpenCV和PID控制器来跟踪对象。通常，您实际上每个轴都有一个单独的PID控制器，您可以将无人机移动到：前进/后退平移，左/右平移，顺时针/逆时针旋转。您可以尝试将kd和ki术语分别设置为小的负和小正值，以查看它如何改变无人机的行为。 This video是我制作的一个演示使用AR.Drone和两个PID控制器，一个用于前进/后退，一个用于左/右，用于跟踪一系列航路点。

还有其他方法可以使用OpenCV进行对象跟踪。 This video演示了使用camshift / meanshift技术（不是在无人机上）。 This video演示了在AR.Drone上使用颜色直方图跟踪。