Разработка угловой стабилизации квадрокоптера

    Данная статья скорее логическое продолжение моей статьи о балансере: «Создание робота балансера на arduino».
    В ней будут очень кратко освещены: простая модель угловой стабилизации квадрокоптера с использованием кватернионов, линеаризация, построение управления для объекта и проверка его в Matlab Simulink, а так же проверка на реальном объекте. В качестве подопытного будет выступать Crazyflie 1.0.

    Сейчас оно летает так (на момент съемок я не очень правильно выставил управление):



    Построение динамической системы


    Введем 2 системы координат: локальную, привязанную к земле, и вторую, связанную с коптером.



    Вращение тела удобнее представлять, используя кватернионы, в связи с меньшим количеством необходимых вычислений. О них написано много статей, в том числе и на хабре. Я рекомендую к прочтению книгу «Бранец В.Н., Шмыглевский И.П. Применение кватернионов в задачах ориентации», спасибо Slovak из центра компетенций MathWorks за подсказку.

    Воспользуемся основным законом динамики вращательного движения:

    , где
    — моменты, действующие на тело,
    I — тензор инерции, а
    — угловые скорости по главным осям(в связанной системе координат).
    Таким образом:
    .

    В силу теоремы о приведении тензора инерции к главным осям, тензор инерции представим в виде: .

    Внешние моменты определим через управления: , где


    Таким образом, уравнения угловых скоростей в связанной системе координат:


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

    Сила тяги пропеллера может быть примерно описана как . Тогда уравнения можно записать через угловые частоты пропеллеров, если вы сможете управлять напрямую частотой моторов и знаете конкретное b:
    где
    — углы эйлера
    Замечу, что подбор коэффициента b у меня произведен вручную, простым подбором.

    Также необходимо выписать уравнение для кватерниона вращения. Из свойств кватернионов следует, что
    , где являются угловыми скоростями в связанной с ЛА системе координат, в ней гироскопы измеряют угловую скорость [1].

    Попробуем стабилизировать только углы и угловые скорости:


    Или подробнее


    Введем вектор пространства состояний:
    .
    Необходимо заметить, что если в вектор пространства входит компонента система перестает быть управляемой. Однако мы можем считать, что и убрать ее из вектора состояний, тем самым уменьшив количество координат [2].



    Вектор управлений:
    ,

    Система представима в стандартном виде

    .

    В нашем случае

    , а



    Линеаризация и построение управления


    Линеаризируя систему вблизи начала координат получим следующие матрицы A и B:

    ,



    Как и в прошлый раз используем линейно-квадратичный регулятор. Напомню команду Matlab для его расчета:
    [K,S,e]=lqr(A,B,Q,R)
    

    Матрицы Q и R являются весовыми матрицами. Q штрафует за отклонение от нуля, а R за расход энергии управлением.
    В результате получили матрицу K. В моей матрице коэффициентов все недиагональные элементы были очень малы (порядка 10^-4) и я не стал учитывать их.
    Напомню, что для получения управления необходимо умножить матрицу K на вектор X. Конечно, в коде можно не вводить понятие матрицы и просто умножить каждую координату на некоторый коэффициент для быстродействия.

    Проверка модели


    Для проверки полученных результатов была создана модель в Matlab Simulink. Запустим ее с ненулевыми начальными условиями.



    Первый график показывает как ведут себя угловые скорости, второй — изменение составляющих кватерниона. Заметьте, что скалярная величина кватерниона приходит в единицу, не смотря на то, что она не входит в уравнения линеаризованной системы. Как видно из графиков — модель стабилизируется.

    Код


    Crazyflie использует систему Free RTOS, где весь код разбит на модули, нас интересует код sensfusion6.c и stabilizer.c.
    К счастью, фильтрация показаний акселерометра и гироскопа производится в кватернионах, проблема заключается в том, что сенсоры на коптере расположены для + схемы. Модель же я рассчитывал для X схемы. Отличие заключается только в выборе управлений U1 и U2.



    Необходимо добавить код получения кватерниона в sensfusion6.c:

    void sensfusion6GetQuaternion(float* rq0,float* rq1,float* rq2,float* rq3){
      *rq0=q0;
      *rq1=q1;
      *rq2=q2;
      *rq3=q3;
    }
    


    Я не стал добавлять отдельный модуль для LQR регулятора, вместо этого я изменил stabilizer.c. Да, возможно, это и не самый интеллигентный способ, однако для проверки модели он подойдет.

    Начать стоит с добавления переменных текущего и желаемого положения аппарата, а так же управлений:

    static float q0Actual;
    static float q1Actual;
    static float q2Actual;
    static float q3Actual;
    
    static float q1Desired;
    static float q2Desired;
    static float q3Desired;
    
    int16_t  actuatorU1;
    int16_t  actuatorU2;
    int16_t  actuatorU3;
    


    Желаемое положение по q0 не указываем в силу того, что нам не нужно его стабилизировать.

    Произведем изменения в код получения команд. Коптер получает угол в градусах, математически правильнее сделать так:

    сommanderGetRPY(&q1Desired, &q2Desired, &q3Desired);
          q1Desired=cos((-q1Desired/2+90)*0.01745);//*3.14/180/2;
          q2Desired=cos((q2Desired/2+90)*0.01745);
          q3Desired=cos((q3Desired/2+90)*0.01745);
    


    Изменим «быстрый» цикл (250Гц) стабилизатора:

    sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
    sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);
    sensfusion6GetQuaternion(&q0Actual, &q1Actual,&q2Actual,&q3Actual);
    sensfusion6UpdateP(FUSION_UPDATE_DT);
    sensfusion6UpdateV(acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
    
    actuatorU1=50*(1*(-gyro.x)+245*(q1Actual-q1Desired));
    actuatorU2=50*(1*(gyro.y)-200*(q2Actual-q2Desired));
    actuatorU3=50*(1.5*(gyro.z)+0*(q3Actual-q3Desired));
    

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

    Также я изменил функцию распределения мощностей моторов:
    static void distributePower(const uint16_t thrust, const int16_t u2, const int16_t u3, const int16_t u4)
    {
      motorPowerM1=limitThrust((thrust/4+u3/2+u4/4)*5);
      motorPowerM2=limitThrust((thrust/4-u2/2-u4/4)*5);
      motorPowerM3=limitThrust((thrust/4-u3/2+u4/4)*5);
      motorPowerM4=limitThrust((thrust/4+u2/2-u4/4)*5);
    
      motorsSetRatio(MOTOR_M1, motorPowerM1);
      motorsSetRatio(MOTOR_M2, motorPowerM2);
      motorsSetRatio(MOTOR_M3, motorPowerM3);
      motorsSetRatio(MOTOR_M4, motorPowerM4);
    }
    


    Заключение


    Исходя из того, что коптер стабилизирует свои углы, можно заключить, что математическая модель разработана верно. К сожалению, пока нет возможности получать свои координаты и скорости (интегрирование акселерометра дает огромную ошибку), поэтому коптер не гасит начальную скорость и не возвращается в начальную позицию.
    Для решения этой задачи MIT, например, использует камеры и метки на своих коптерах.

    Дополнительные материалы и источники


    1. Бранец В.Н., Шмыглевский И.П. «Применение кватернионов в задачах ориентации»
    2. Yaguang Yang «Analytic LQR Design for Spacecraft Control System Based on Quaternion Model»
    3. Ветка модифицированной прошивки на github


    P.S. К сожалению, не могу поделиться моделью, а так же рассказать о расширенной модели с автопилотом и координатной стабилизацией в силу того, что это является частью моего будущего диплома, а все дипломы теперь проверяются на новизну и антиплагиат.
    P.P.S. Я публикую данную статью на хабре, а не на новом GT в связи с тем, что остальные мои статьи схожей тематики остались именно на хабре.
    Поделиться публикацией
    Реклама помогает поддерживать и развивать наши сервисы

    Подробнее
    Реклама
    Комментарии 17
    • +1
      Класс! Спасибо большое!

      С нетерпением жду продолжения и исходников, когда диплом будет защищен!
      • +1
        Спасибо за теплые слова. Обязательно выложу прогресс. Исходный код текущей прошивки лежит на github (ссылка в конце статьи).
      • +2
        радует, что кто-то таки делает реальные дипломы, а не сферических коней в вакууме.
        • +2
          Еще бы это нужно было кому-либо кроме себя и небольшого числа энтузиастов.
          • +3
            Дипломный проект нужен не потому, что кому-то нужен, а потому, что нарабатываются навыки разработки. Кстати, кроме технических навыков, еще иногда приобретается так же и опыт общения с дятлами. Поверьте, потом в профессии это очень пригодится ;)
        • 0
          Будет пытаться занять угловое положение или всё же исходного положения в комнате? Одно дело когда его пнул, а он сам встал прямо и другое, когда его пнул, а он сам вернулся на то же самое место.
          • 0
            В данной статье он гасит угловые скорости и, как вы сказали, встает прямо. Для того, чтобы он вернулся на тоже самое место, то есть стабилизировал и свои координаты в комнате (точке взлета), необходимы данные о текущем положении. В комнатах для этого используют камеры, а на открытых пространствах сигнал GPS/ГЛОНАСС.
            • 0
              А разве перемещение не является интегралом интеграла ускорения?
              • 0
                Да это так, но акселерометры имеют свойство шуметь. Чтобы избавиться от шума необходим фильтр, как предложил kahi4. Кроме того, необходимо учитывать, что на него все время действует ускорение свободного падения.
            • 0
              В данной статье он гасит угловые скорости и, как вы сказали, встает прямо. Для того, чтобы он вернулся на тоже самое место, то есть стабилизировал и свои координаты в комнате (точке взлета), необходимы данные о текущем положении. В комнатах для этого используют камеры, а на открытых пространствах сигнал GPS/ГЛОНАСС.
            • 0
              Большое спасибо за статью. Действительно, захватывающий проект!
              • +2
                Не так давно делали лабораторную работу, где нужно было моделировать движение квадрокоптера по траектории, а так же стабилизировать платформу (на которой типа была установлена камера). К счастью (или к сожалению), на мат. моделях в матлабе (точнее, симулинке) все и закончилось. Только мы для управления использовали PID-регуляторы (настраивать пид-регуляторы, да еще для двух контуров — ад).

                Скажу по поводу навигации. Действительно, примитивные фильтры типа апериодического звена или скользящего среднего дают большой уход, вдобавок к этому добавляют заметное запаздывание. Однако есть методы, благодаря которым можно обойтись набором акселерометров и ДУСов. Покуда у вас уже есть мат. модель — сложностей не должно возникнуть. Собственно, это любимый на хабре фильтр Калмана в режиме комплексирования. На самом деле он даст достаточных для того, чтобы лететь по заданному маршруту, уход.
                • +1
                  настраивать пид-регуляторы, да еще для двух контуров — ад).
                  Я недавно вебинар проводил, в нем как раз есть пример настройки каскадного соединения ПИД. Думаю Вы будете удивлены узнав, что это не такой уж и Адъ))))
                  matlab.ru/webinars/nastroyka-sistem-upravleniya-v-simulink
                  • 0
                    Я не спорю — в матлабе есть шикарные методы настройки ПИДов. Он действительно сильно упрощает работу. Но нас, зачем-то, просили их настраивать ручками метода тыка, как бы проверяя таким образом, понимаем ли мы, что происходит или нет (т.е. — недостаточный наклон, большое время перегулирования — увеличиваем пропорциональную, большое перегулирование — увеличиваем диф. состовляющую и так далее).
                    На самом деле, в симулинке в блоке «aerospace» есть чуть ли не готовые модели (полагаю, вы это лучше меня знаете), такую лабу там можно набросать за 5 минут. И матрицы поворота там есть, и фильтры, и все, что нужно. Но у нас только ручками, только хардкор…
                    • 0
                      Отчасти верно с педагогической точки зрения. Жаль, что в конце не рассказали о более продуктивных методах.
                      • 0
                        ИМХО нужно было сделать так: сначала на отдельной лабе проверить как мы умеем настраивать ПИДы и понимаем, что тут происходит, а потом уже делаем так, как в голову придет. Ну хорошо хоть ТАУ не стали у нас «прокачивать», с многоконтурными системами, каналами по производным и прочим :) С другой стороны, даже жаль. Как-то мы теоретический курс на эту тему прослушали и на этом закончилось. А ведь тема интересная, хоть и достаточно сложная.
                • 0
                  Ребята из Drexel University похожую штуку моделируют:
                  www.facebook.com/MATLAB/posts/10152391752451641
                  у них как раз можно скачать модельки

                  Только полноправные пользователи могут оставлять комментарии. Войдите, пожалуйста.