Дрон на поводке - алгоритм позиционирования - PullRequest
Новая
       15

Дрон на поводке - алгоритм позиционирования

0 голосов
/ 02 октября 2019

Я построил свой собственный матовый квадрокоптер 10x10 см (на основе STMicroelectronics FCU). В настоящее время он летает на моем собственном программном обеспечении. Алгоритм PID был основан на реализации Ardupilot (фильтрация выходов D и т. Д.). У меня есть два режима полета: «ACRO» (стабилизация шага и крена на основе показаний гироскопа) и «STABILIZE» (2 вложенных цикла PID, как в Ardupilot (без блока обратной связи) - см. Прикрепленное изображение) Оба режима полета работают отлично.

stableize_loop_schematic http://ardupilot.org/dev/_images/Copter_CodeOverview_AttitudeControlPID.png

Я хотел бы добавить третий режим полета "ДЖОЙСТИК" / "ПОВЕРХНОСТЬ". Я установил небольшой джойстик Xbox (без натяжной пружины) под дроном и прикрепил поводок длиной ~ 2 м, который постоянно удерживается на земле кусочком свинца. Я бы хотел, чтобы беспилотник оставался на одном месте (он не должен быть точно над струнным якорем - потенциометр, действующий как GPS бедняка). Я пробовал несколько методов. Каждый раз было 3 варианта поведения: а) дрон медленно вращается вокруг якоря с очень медленным колебанием вокруг тангажа и крена, что время от времени вызывает изменение направления орбиты - радиус слишком велик, чтобы быть приемлемым б) дронspasmatic - лучше поддерживает радиус, но он все еще вращается, и каждая реакция быстрая и внезапная (хотя полет безопасен) c) дрон пытается убить меня, будучи слишком агрессивным. 

float PID_calc(PID_t *PID_handle, float input, float setpoint, uint32_t dt_us, uint32_t i_en, float scaler);

void VEHICLE_FLTMODE_stabilize(VEHICLE_t *VEHICLE_handle)
{
        int32_t tmp_rc_yaw = MAP_u(SBUS_get_channel(1), 204, 1836, -250, 250);
    int32_t tmp_rc_pitch = MAP_s(SBUS_get_channel(2), 204, 1820, 35, -35);
    int32_t tmp_rc_thr = MAP_s(SBUS_get_channel(3), 207, 1836, 0, 100);
    int32_t tmp_rc_roll = MAP_s(SBUS_get_channel(4), 227, 1836, -35, 35);

    int32_t tmp_pid_pitch = (int32_t)PID_calc(&pid_pitch_t, VEHICLE_handle->euler_pitch, (float)tmp_rc_pitch, 1000, 0, 1.0f);
    int32_t tmp_pid_roll = (int32_t)PID_calc(&pid_roll_t, VEHICLE_handle->euler_roll, (float)tmp_rc_roll, 1000, 0, 1.0f);

    int32_t tmp_pid_pitch_rate = (int32_t)PID_calc(&pid_pitch_rate_t, -VEHICLE_handle->ang_rate_dps_y, (float)tmp_pid_pitch, 1000, 1, 1.0f);
    int32_t tmp_pid_roll_rate = (int32_t)PID_calc(&pid_roll_rate_t, VEHICLE_handle->ang_rate_dps_x, (float)tmp_pid_roll, 1000, 1, 1.0f);
    int32_t tmp_pid_yaw_rate = (int32_t)PID_calc(&pid_yaw_rate_t, -VEHICLE_handle->ang_rate_dps_z, (float)tmp_rc_yaw, 1000, 1, 1.0f);

if(tmp_rc_thr < 10)
{
    PID_reset_i(&pid_pitch_t);
    PID_reset_i(&pid_roll_t);

    PID_reset_i(&pid_pitch_rate_t);
    PID_reset_i(&pid_roll_rate_t);
    PID_reset_i(&pid_yaw_rate_t);

    MOTORS_set(1, tmp_rc_thr);
    MOTORS_set(2, tmp_rc_thr);
    MOTORS_set(3, tmp_rc_thr);
    MOTORS_set(4, tmp_rc_thr);
}
else
{
    MOTORS_set(1, tmp_rc_thr - tmp_pid_pitch_rate - tmp_pid_roll_rate - 
              tmp_pid_yaw_rate);

    MOTORS_set(4, tmp_rc_thr - tmp_pid_pitch_rate + 
              tmp_pid_roll_rate + tmp_pid_yaw_rate);

    MOTORS_set(2, tmp_rc_thr + tmp_pid_pitch_rate - tmp_pid_roll_rate + 
              tmp_pid_yaw_rate);

    MOTORS_set(3, tmp_rc_thr + tmp_pid_pitch_rate + tmp_pid_roll_rate - 
              tmp_pid_yaw_rate);
}
}

Я верю в этопроблема как-то связана с динамикой тела беспилотника (точно так же, как с циклом «STABILIZE», вы НЕ МОЖЕТЕ просто запустить один цикл PID с углами Эйлера в качестве заданных значений). Я просто инженер-электронщик. У меня нет необходимых знаний о модельно-ориентированном дизайне и механике полета. Может ли кто-нибудь указать мне правильный алгоритм контура управления, чтобы дрон остался на месте? Btw. Я использую сенсор Мэджвика (6dof), поэтому у меня нет линейной оценки скорости (относительно Земли). Доступные вещи: - Углы тангажа, крена, углового рычага и квантерниона (относительно земли) - Угловые скорости тангажа, крена, рыскания (град / с) - Угол джойстика (относительно рамы дронов) - Скорость изменения угла джойстика (градус / 100)миллисекунды)

...