Как работать с ПИД-контроллером для A.R Drone 2.0?

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

Ваш вопрос немного общий, но я посмотрю, смогу ли я дать вам обзор, чтобы помочь вам начать. (Также см. Этот похожий вопрос: Отслеживание объектов с камеры; ПИД-контроль; Попугай AR Drone 2.)

Этот код (который из sample_tracking.cpp в cvdrone хранилище) пытается сделать следующее:

1. Найдите самый большой объект (нужного цвета) в поле зрения камеры.
2. Переместите дрон так, чтобы объект находился в центре обзора камеры.

Он использует OpenCV для выполнения первой задачи и PID для выполнения второй задачи.

Найдите самый большой объект (нужного цвета) в поле зрения камеры

Для этого код берет кадр с видеокамеры дрона, ищет самый большой шарик нужного цвета и находит центр этого шарика.

Он использует OpenCV для достижения следующих целей:

1. использование InRange пороговое значение изображения, которое включает пиксели, близкие к целевому цвету, и отключает пиксели, которые находятся далеко. Теперь у вас есть изображение, которое содержит только белые и черные пиксели, где белые пиксели соответствуют цвету, который вы ищете. Этот пост блога имеет хороший пример использования InRange найти кубики определенного цвета на кубике Рубика: Цветовые пространства в OpenCV.

2. использование morphologyEx с MORPH_CLOSE убрать шум с изображения. Это должно облегчить поиск нужных капель. Смотрите раздел 4, «Закрытие» на эта страница для примера того, как выглядит результат этой обработки.

3. использование findContours чтобы найти пятна пикселей на изображении. Контур — это «кривая, соединяющая все непрерывные точки (вдоль границы), имеющие одинаковый цвет или интенсивность», так что это позволит найти контуры всех белых пятен на изображении.

Пример ввода:

Пример результата:

4. использование contourArea найти самый большой контур по площади. Это будет объект, который будет отслеживать дрон.

5. использование moments найти центр тяжести самого большого контура. Так определяется координаты изображения объекта. Эта страница документирует особенности контура имеет больше информации о моментах.

Так что теперь в коде есть координаты объекта, который он хочет отслеживать. Затем следует часть, где он фактически перемещает дрон, чтобы выполнить отслеживание.

Переместите дрон так, чтобы объект находился в центре обзора камеры.

ПИД-контроль — большая тема. Я просто опишу основы и то, что делает этот код; если вы хотите получить больше информации, есть много вводных ресурсов, как эта статья, «Введение в ПИД-контроль», или это видео: «ПИД-контроль — краткое введение».

ПИД-регулятор учитывает 3 вещи:

1. Пропорциональный термин: как далеко мы от того, где мы хотим быть?
2. Производный термин: как быстро мы движемся к (или от), где мы хотим быть?
3. Интегральный термин: какова накопленная ошибка с течением времени между тем, где мы находимся и где мы хотим быть? (интегральный термин)

Пропорциональный член перемещает вас к цели. Производный термин замедляет вас, когда вы быстро двигаетесь к цели, поэтому вы не промахнетесь. Интегральный термин может помочь подтолкнуть вас, когда вы остановились совсем недалеко от цели.

Оказывается, что из-за следующих 3 строк кода эта функция на самом деле не работает с полным ПИД-контроллером:

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

Поскольку коэффициенты усиления для интегральной и производной частей контроллера равны 0, это просто простой пропорциональный контроллер: код просто смотрит на разницу между координатами цели и центра изображения и использует ее для определения того, как водить дрон.

Во-первых, вот что cvdrone код использует для системы координат AR.Drone:

Передняя часть AR.Drone — это ось X, слева — ось Y, верхняя — ось Z. Также
фронт составляет 0,0 [рад], каждая ось против часовой стрелки положительна.

        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, но давайте предположим, что это 320×480. Это означает, что центр изображения находится у = 240.

Если центр цели был близко к верху изображения, скажем, у = 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 для отслеживания объекта. Как правило, у вас фактически есть отдельный ПИД-регулятор для каждой оси, в который вы можете перемещать дрон: перемещение вперед / назад, перемещение влево / вправо, вращение по часовой стрелке / против часовой стрелки. Вы можете попробовать установить kd а также ki чтобы определить, как это меняет поведение беспилотника, с небольшим отрицательным и небольшим положительным значением соответственно. Это видео это тот, который я сделал, который демонстрирует использование AR.Drone и двух контроллеров PID, один для прямого / обратного и один для левого / правого, для отслеживания ряда путевых точек.

Есть и другие способы использования OpenCV для отслеживания объектов. Это видео демонстрирует использование техники смещения / средства смещения (не на дроне). Это видео демонстрирует использование отслеживания цветовой гистограммы на AR.Drone.