Данная статья скорее логическое продолжение моей статьи о балансере: «Создание робота балансера на arduino».
В ней будут очень кратко освещены: простая модель угловой стабилизации квадрокоптера с использованием кватернионов, линеаризация, построение управления для объекта и проверка его в Matlab Simulink, а так же проверка на реальном объекте. В качестве подопытного будет выступать Crazyflie 1.0.
Сейчас оно летает так (на момент съемок я не очень правильно выставил управление):
Введем 2 системы координат: локальную, привязанную к земле, и вторую, связанную с коптером.
![](https://habrastorage.org/r/w1560/files/f9c/9b4/0a8/f9c9b40a8582475bbc4c2afce5d2a662.png)
Вращение тела удобнее представлять, используя кватернионы, в связи с меньшим количеством необходимых вычислений. О них написано много статей, в том числе и на хабре. Я рекомендую к прочтению книгу «Бранец В.Н., Шмыглевский И.П. Применение кватернионов в задачах ориентации», спасибо Slovak из центра компетенций MathWorks за подсказку.
Воспользуемся основным законом динамики вращательного движения:
, где
— моменты, действующие на тело,
I — тензор инерции, а
— угловые скорости по главным осям(в связанной системе координат).
Таким образом:
.
В силу теоремы о приведении тензора инерции к главным осям, тензор инерции представим в виде:
.
Внешние моменты определим через управления:
, где
![](https://habrastorage.org/files/74e/ecf/1e9/74eecf1e9dbb48648eacd1619b7b2f3b.gif)
Таким образом, уравнения угловых скоростей в связанной системе координат:
![](https://habrastorage.org/files/4a0/3c4/a8d/4a03c4a8d46349e4ad18cd56f814242b.gif)
Замечу, что если бы мы учитывали положение коптера, можно было бы не вводить отдельные функции управлений, а сразу использовать в качестве них силы тяги, что удобнее и быстрее при расчетах. В данном случае система стабилизации не имеет никаких данных о необходимой сумме сил тяги, поэтому необходимо использовать именно такие управления…
Сила тяги пропеллера может быть примерно описана как
. Тогда уравнения можно записать через угловые частоты пропеллеров, если вы сможете управлять напрямую частотой моторов и знаете конкретное b:
где
— углы эйлера
Замечу, что подбор коэффициента b у меня произведен вручную, простым подбором.
Также необходимо выписать уравнение для кватерниона вращения. Из свойств кватернионов следует, что
, где
являются угловыми скоростями в связанной с ЛА системе координат, в ней гироскопы измеряют угловую скорость [1].
Попробуем стабилизировать только углы и угловые скорости:
![](https://habrastorage.org/files/ab5/81e/ebb/ab581eebb7e34a2a8b068262caa29ed9.gif)
Или подробнее
![](https://habrastorage.org/files/d17/22e/b20/d1722eb20b274c7281f9e163cde0a139.gif)
Введем вектор пространства состояний:
.
Необходимо заметить, что если в вектор пространства входит компонента
система перестает быть управляемой. Однако мы можем считать, что
и убрать ее из вектора состояний, тем самым уменьшив количество координат [2].
![](https://habrastorage.org/files/88f/fc7/9c0/88ffc79c06104a8d950e6fe62623ec6c.gif)
Вектор управлений:
,
Система представима в стандартном виде
.
В нашем случае
, а
![](https://habrastorage.org/files/fe9/68d/de8/fe968dde8c824d3f8d0297fb0ff42350.gif)
Линеаризируя систему вблизи начала координат получим следующие матрицы A и B:
,
![](https://habrastorage.org/files/fe9/68d/de8/fe968dde8c824d3f8d0297fb0ff42350.gif)
Как и в прошлый раз используем линейно-квадратичный регулятор. Напомню команду Matlab для его расчета:
Матрицы Q и R являются весовыми матрицами. Q штрафует за отклонение от нуля, а R за расход энергии управлением.
В результате получили матрицу K. В моей матрице коэффициентов все недиагональные элементы были очень малы (порядка 10^-4) и я не стал учитывать их.
Напомню, что для получения управления необходимо умножить матрицу K на вектор X. Конечно, в коде можно не вводить понятие матрицы и просто умножить каждую координату на некоторый коэффициент для быстродействия.
Для проверки полученных результатов была создана модель в Matlab Simulink. Запустим ее с ненулевыми начальными условиями.
![](https://habrastorage.org/r/w1560/files/fbe/438/6c4/fbe4386c476c40eba29027ead1774628.png)
Первый график показывает как ведут себя угловые скорости, второй — изменение составляющих кватерниона. Заметьте, что скалярная величина кватерниона приходит в единицу, не смотря на то, что она не входит в уравнения линеаризованной системы. Как видно из графиков — модель стабилизируется.
Crazyflie использует систему Free RTOS, где весь код разбит на модули, нас интересует код sensfusion6.c и stabilizer.c.
К счастью, фильтрация показаний акселерометра и гироскопа производится в кватернионах, проблема заключается в том, что сенсоры на коптере расположены для + схемы. Модель же я рассчитывал для X схемы. Отличие заключается только в выборе управлений U1 и U2.
![](https://habrastorage.org/files/cc5/35f/f76/cc535ff766214f408cfdf96153f627bd.gif)
Необходимо добавить код получения кватерниона в sensfusion6.c:
Я не стал добавлять отдельный модуль для LQR регулятора, вместо этого я изменил stabilizer.c. Да, возможно, это и не самый интеллигентный способ, однако для проверки модели он подойдет.
Начать стоит с добавления переменных текущего и желаемого положения аппарата, а так же управлений:
Желаемое положение по q0 не указываем в силу того, что нам не нужно его стабилизировать.
Произведем изменения в код получения команд. Коптер получает угол в градусах, математически правильнее сделать так:
Изменим «быстрый» цикл (250Гц) стабилизатора:
Подбор коэффициентов произведен опытным путем, так как не было возможности узнать зависимость между посылаемой на моторы командой и силой, которую выдает мотоустановка.
Также я изменил функцию распределения мощностей моторов:
Исходя из того, что коптер стабилизирует свои углы, можно заключить, что математическая модель разработана верно. К сожалению, пока нет возможности получать свои координаты и скорости (интегрирование акселерометра дает огромную ошибку), поэтому коптер не гасит начальную скорость и не возвращается в начальную позицию.
Для решения этой задачи MIT, например, использует камеры и метки на своих коптерах.
P.S. К сожалению, не могу поделиться моделью, а так же рассказать о расширенной модели с автопилотом и координатной стабилизацией в силу того, что это является частью моего будущего диплома, а все дипломы теперь проверяются на новизну и антиплагиат.
P.P.S. Я публикую данную статью на хабре, а не на новом GT в связи с тем, что остальные мои статьи схожей тематики остались именно на хабре.
В ней будут очень кратко освещены: простая модель угловой стабилизации квадрокоптера с использованием кватернионов, линеаризация, построение управления для объекта и проверка его в Matlab Simulink, а так же проверка на реальном объекте. В качестве подопытного будет выступать Crazyflie 1.0.
Сейчас оно летает так (на момент съемок я не очень правильно выставил управление):
Построение динамической системы
Введем 2 системы координат: локальную, привязанную к земле, и вторую, связанную с коптером.
![](https://habrastorage.org/files/f9c/9b4/0a8/f9c9b40a8582475bbc4c2afce5d2a662.png)
Вращение тела удобнее представлять, используя кватернионы, в связи с меньшим количеством необходимых вычислений. О них написано много статей, в том числе и на хабре. Я рекомендую к прочтению книгу «Бранец В.Н., Шмыглевский И.П. Применение кватернионов в задачах ориентации», спасибо Slovak из центра компетенций MathWorks за подсказку.
Воспользуемся основным законом динамики вращательного движения:
![](https://habrastorage.org/files/3a2/7ec/d8c/3a27ecd8ca1b401b8f03b8ac8b2503bd.gif)
![](https://habrastorage.org/files/539/8af/f58/5398aff58f76454aa0360c6ce7877e36.gif)
I — тензор инерции, а
![](https://habrastorage.org/files/4d8/d8d/a53/4d8d8da53df44aefba2787b6dd1bf9ec.gif)
Таким образом:
![](https://habrastorage.org/files/2ad/27a/1e4/2ad27a1e46fa42759d5dd9e9d08fd7af.gif)
В силу теоремы о приведении тензора инерции к главным осям, тензор инерции представим в виде:
![](https://habrastorage.org/files/6d0/9c8/000/6d09c80004cd4a3b98561248607b2b3a.gif)
Внешние моменты определим через управления:
![](https://habrastorage.org/files/34f/30e/c76/34f30ec7653c434b8ad37ed003d74d84.gif)
![](https://habrastorage.org/files/74e/ecf/1e9/74eecf1e9dbb48648eacd1619b7b2f3b.gif)
Таким образом, уравнения угловых скоростей в связанной системе координат:
![](https://habrastorage.org/files/4a0/3c4/a8d/4a03c4a8d46349e4ad18cd56f814242b.gif)
Замечу, что если бы мы учитывали положение коптера, можно было бы не вводить отдельные функции управлений, а сразу использовать в качестве них силы тяги, что удобнее и быстрее при расчетах. В данном случае система стабилизации не имеет никаких данных о необходимой сумме сил тяги, поэтому необходимо использовать именно такие управления…
Сила тяги пропеллера может быть примерно описана как
![](https://habrastorage.org/files/961/4e3/8cc/9614e38cc57e4491bc6466e3641ddaeb.gif)
![](https://habrastorage.org/files/448/085/bca/448085bca08c41eea3d5fb142d6c5c57.gif)
![](https://habrastorage.org/files/2df/e7c/b83/2dfe7cb83c9749ebb3cd8d01f57e4f8e.gif)
Замечу, что подбор коэффициента b у меня произведен вручную, простым подбором.
Также необходимо выписать уравнение для кватерниона вращения. Из свойств кватернионов следует, что
![](https://habrastorage.org/files/469/652/b9a/469652b9a6ac43dbb19e7cb0b93a9f0a.gif)
![](https://habrastorage.org/files/4d8/d8d/a53/4d8d8da53df44aefba2787b6dd1bf9ec.gif)
Попробуем стабилизировать только углы и угловые скорости:
![](https://habrastorage.org/files/ab5/81e/ebb/ab581eebb7e34a2a8b068262caa29ed9.gif)
Или подробнее
![](https://habrastorage.org/files/d17/22e/b20/d1722eb20b274c7281f9e163cde0a139.gif)
Введем вектор пространства состояний:
![](https://habrastorage.org/files/ab2/a2c/76e/ab2a2c76ecfe43ab9e5a13a57cfcb658.gif)
Необходимо заметить, что если в вектор пространства входит компонента
![](https://habrastorage.org/files/55b/ae3/b30/55bae3b30204482ebd02503c828c46f5.gif)
![](https://habrastorage.org/files/05d/c5a/c28/05dc5ac282614dd691a47aa087a32f64.gif)
![](https://habrastorage.org/files/88f/fc7/9c0/88ffc79c06104a8d950e6fe62623ec6c.gif)
Вектор управлений:
![](https://habrastorage.org/files/743/799/3a8/7437993a8803444897237b00da70fa10.gif)
Система представима в стандартном виде
![](https://habrastorage.org/files/e63/284/808/e6328480820d49bc966c9793af394f41.gif)
В нашем случае
![](https://habrastorage.org/files/729/2b9/5e2/7292b95e27b64e49ad348fd35a01991d.gif)
![](https://habrastorage.org/files/fe9/68d/de8/fe968dde8c824d3f8d0297fb0ff42350.gif)
Линеаризация и построение управления
Линеаризируя систему вблизи начала координат получим следующие матрицы A и B:
![](https://habrastorage.org/files/f30/f38/53b/f30f3853bb364e638e53f66f6c50aa74.gif)
![](https://habrastorage.org/files/fe9/68d/de8/fe968dde8c824d3f8d0297fb0ff42350.gif)
Как и в прошлый раз используем линейно-квадратичный регулятор. Напомню команду Matlab для его расчета:
[K,S,e]=lqr(A,B,Q,R)
Матрицы Q и R являются весовыми матрицами. Q штрафует за отклонение от нуля, а R за расход энергии управлением.
В результате получили матрицу K. В моей матрице коэффициентов все недиагональные элементы были очень малы (порядка 10^-4) и я не стал учитывать их.
Напомню, что для получения управления необходимо умножить матрицу K на вектор X. Конечно, в коде можно не вводить понятие матрицы и просто умножить каждую координату на некоторый коэффициент для быстродействия.
Проверка модели
Для проверки полученных результатов была создана модель в Matlab Simulink. Запустим ее с ненулевыми начальными условиями.
![](https://habrastorage.org/files/fbe/438/6c4/fbe4386c476c40eba29027ead1774628.png)
Первый график показывает как ведут себя угловые скорости, второй — изменение составляющих кватерниона. Заметьте, что скалярная величина кватерниона приходит в единицу, не смотря на то, что она не входит в уравнения линеаризованной системы. Как видно из графиков — модель стабилизируется.
Код
Crazyflie использует систему Free RTOS, где весь код разбит на модули, нас интересует код sensfusion6.c и stabilizer.c.
К счастью, фильтрация показаний акселерометра и гироскопа производится в кватернионах, проблема заключается в том, что сенсоры на коптере расположены для + схемы. Модель же я рассчитывал для X схемы. Отличие заключается только в выборе управлений U1 и U2.
![](https://habrastorage.org/files/cc5/35f/f76/cc535ff766214f408cfdf96153f627bd.gif)
Необходимо добавить код получения кватерниона в 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, например, использует камеры и метки на своих коптерах.
Дополнительные материалы и источники
- Бранец В.Н., Шмыглевский И.П. «Применение кватернионов в задачах ориентации»
- Yaguang Yang «Analytic LQR Design for Spacecraft Control System Based on Quaternion Model»
- Ветка модифицированной прошивки на github
P.S. К сожалению, не могу поделиться моделью, а так же рассказать о расширенной модели с автопилотом и координатной стабилизацией в силу того, что это является частью моего будущего диплома, а все дипломы теперь проверяются на новизну и антиплагиат.
P.P.S. Я публикую данную статью на хабре, а не на новом GT в связи с тем, что остальные мои статьи схожей тематики остались именно на хабре.