Что бы разбавить зубодробительную математику лекций Козлова Олега Степановича "Управление в технических системах", публикуем здесь пример применения знаний из этих лекций на практике.
В данной статье, Александр Щекатуров, ученик Олега Степановича описывает создание модели октокоптера, попутно раскрывая секреты и демонстрируя приемы работы в среде структурного моделирования физических систем.
В настоящей статье приведено описание логически завершённой, но всего лишь части полной работы по моделированию, но этой части достаточно для введения в тему динамического моделирования БПЛА коптерного типа.
Здесь опущено моделирование эффектов прецессии, принимается что и реактивный момент каждой ВМГ равен нулю, а именно: каждая ВМГ имеет два двигателя и винта, вращающихся с одинаковой скоростью в противоположные стороны. Не моделируются отказы оборудования и предполагается что объект находится только в воздухе (в штатном режиме полёта), режимы посадки и взлёта, аварийные ситуации, захват груза и разгрузка — в приведённой модели не реализованы, а также не рассматриваются вопросы подробного моделирования датчиков, фильтрации сигналов и шумов, изгиб рамы коптера и/или винтов, работа на запредельных нагрузках, написание драйверов к той или иной аппаратуре и т.д. и т.п., — всё это темы более расширенной статьи или даже книги.
Дано: объект массой порядка 10…25 кг, с 4, 6 или 8-ю парными соосными винтомоторными группами (ВМГ), расположенными по традиционной схеме квадро, гекса или октокоптера на жёсткой раме, с 8, 12 или 16 двигателями типа T-motors Antigravity 6007 KV320 или аналогичными и соответствующими им винтами (исходим из грузоподъёмности одной ВМГ порядка 2 кг на 50% газа). Предположим что конструкция аппарата до конца не определена.
Требуется: сконструировать и реализовать модель динамики объекта параметрически, в общем виде и в объёме, достаточном для проектирования полётного контроллера и наземного пульта управления коптером.
В литературе и сети приведено довольно много моделей квадрокоптеров, есть некоторые модели гекса- и октокоптеров. Однако, изложенного в системном и методичном виде со всеми подробностями — практически ничего нет (по крайней мере, в русскоязычном сегменте, из тех материалов что удалось найти). Наиболее методично теоретический подход к моделированию мультироторного БПЛА изложен в работах [1] и [2].
Опуская некоторые выкладки, изложенные в этих работах (они легко гуглятся), можно выделить следующие допущения и упрощения, принятые в модели:
Для того чтобы коптер управлялся (хотя бы немного) по курсу, в нашем случае отсутствия реактивного момента двигателей, вектора сил тяги каждой ВМГ должны быть немного отклонены от вертикального направления (повёрнуты вокруг каждого луча на небольшой угол порядка 1…5 градусов, причем в разные стороны – четные в одну а нечетные в другую). Если обозначить этот угол как , орты можно получить в следующем виде:
С учетом принятых допущений, и того что коптер моделируется как единое твёрдое тело, основа модели динамики коптера очень проста – это второй закон Ньютона, который в векторной форме выглядит следующим образом, всего два простых уравнения:
Легко видеть, что всё было бы очень просто и очевидно, если бы правые части были небольшими, а тензор инерции не зависел бы от ориентации коптера (как если бы коптер был шаром). Но из-за вращения коптера, а также из-за обилия сил и моментов, конечные уравнения, записанные в инерциальной системе координат, получатся очень громоздкими, даже без учета прецессии и реактивных моментов ВМГ. Поэтому используется следующий математический приём – уравнения, записанные в инерциальной системе координат, переводятся в систему координат B, связанную с коптером, в которой слагаемые правой части (их запись) получается гораздо более лаконичной и удобной. Подробнее этот приём описан в [1], приведём здесь лишь основные соотношения.
Подставив в приведенные уравнения второго закона Ньютона значения для импульса и момента импульса коптера, для инерциальной системы координат получим:
В математике есть два основных подхода к преобразованию векторов из одной системы координат в другую и обратно – матрицы поворота и кватернионы. Последние более универсальны, первые – проще. В настоящей модели используются матрицы поворота. Если ориентацию коптера представить тремя углами: крена ? (roll), тангажа ? (pitch) и рыскания/курса ? (yaw), а матрицы преобразования из системы I в B и обратно обозначить как и , то любой вектор записанный для системы координат I, можно перевести в систему координат B, и наоборот, используя умножение соответствующей матрицы на вектор, например: , или .
Чтобы лучше понимать написанное далее, прокомментируем ещё раз как понимать матрицы поворота и вектора в пространстве: система координат I неподвижна, относительно неё летает и вращается коптер, а вместе с ним и связанная система координат B. Просуммировав все силы, которые действуют на коптер, можно получить вектор (аналогично и с моментом сил), и в каждый момент времени он является вектором с вполне определенной длиной и направлением в пространстве. Но раскладывая его на проекции по осям – в разных системах координат мы получим разные величины проекций. Всё что делает матрица поворота – это переводит одни проекции вектора в другие, при этом сам вектор никуда не поворачивается и не изменяет своей длины, в выбранный момент времени. Если проекции сравнить с тенями вектора, то матрицы поворота преобразуют одни тени в другие, всё. Больше они ничего не делают и сложностей кроме вычислительных не представляют.
Нам они нужны только из-за того, что вычислять и суммировать силы и моменты сил, действующие на коптер, гораздо проще в связанной системе координат B. В ней же проще провести численное интегрирование уравнений, чтобы получить величины угловой и линейной скоростей коптера и , а потом обратной матрицей поворота вычислить (вычислить алгебраически – матрицы поворота это простые уравнения) эти же скорости для инерциальной системы координат I и там уже, интегрируя дальше, вычислить линейные и угловые координаты коптера.
Приведем используемое выражение для матрицы поворота из системы I в B, записав для краткости функции косинуса и синуса как cos() = c() и sin() = s():
Реализованная методом структурного моделирования, матрица в среде SimInTech выглядит как показано на рисунке 1.
Тогда, для вектора линейной скорости можно записать:
а для момента импульса Опуская промежуточные выкладки (в том числе производную матрицы поворота, которая получается равной в итоге векторному произведению угловой скорости объекта на саму матрицу поворота, взятое с обратным знаком), для первого из рассматриваемых уравнений динамики получим следующее выражение в связанной с коптером системе координат B:Итого, запись II закона Ньютона для вращающейся системы B, по сравнению с исходными уравнениями, дополнилась двумя векторными произведениями.
Переменными состояния коптера в такой записи являются две векторных величины (или 6 скалярных) – вектор линейной скорости и вектор угловой скорости. Алгебраически это будет 6 переменных – три проекции линейной скорости и три проекции угловой скорости. И мы имеем записанную в форме Коши систему из 6 нелинейных уравнений первого порядка, которую легко можно реализовать в среде структурного моделирования SimInTech или Simulink или Scilab и даже успешно решить её тем или иным методом интегрирования.
Получив значения скоростей коптера (сначала в системе B), их можно матрицей обратного поворота преобразовать к системе I, еще раз проинтегрировать и получить уже значения координат и, следовательно, положение объекта в пространстве, в инерциальной системе координат.
Единственное что мы еще не сделали по уравнениям – не записали выражения для сил и моментов, действующих на коптер. Сделаем это ниже, в системе координат B. Согласно допущениям, учитываем и обозначим индексами: M — работу двигателей, только в части создаваемой силы тяги и моментов от неё, D — силу сопротивления воздуха (вместе с ветром), O — внешнее возмущение, вообще нулевое, и задаваемое произвольно пользователем, силу тяжести — она поворотного момента сил не создаёт:
Распишем подробнее чему равны слагаемые:
Сила сопротивления воздуха (при отсутствии ветра):
Момент сил тяги двигателей запишем как:
Ещё раз отметим, что расчет прецессии и реактивных моментов ВМГ в данной статье для краткости изложения опущен.
Чтобы не ошибиться при переходе от векторных уравнений к скалярным, записанным по осям, проще воспользоваться пакетом типа MathCAD или Maple, в котором большинство преобразований можно выполнить автоматизированно, в символьном виде и получить требуемые 6 уравнений динамики, записанные по осям подвижной системы координат B.
В наиболее компактной форме полученные и решаемые уравнения динамики выглядят так:
Интегрируя их и получив значения скоростей в системе В, можно посчитать скорость и углы ориентации в инерциальной системе координат I:
Переходим к самому интересному – к реализации полученных уравнений динамики средствами среды динамического моделирования SimInTech. Как известно, любую задачу можно всегда решить несколькими способами, и чем она сложнее и многомернее – тем больше количество способов. В настоящей статье предложен один из вариантов решения, он не единственный, и в некоторых местах – не самый оптимальный. Выбор решения определялся простотой реализации с одной стороны, и удобством для методичного рассказа, с другой.
Итак, что мы получили с точки зрения математики – в системе координат B мы имеем 2 векторных дифференциальных уравнения, которые при переходе к проекциям (и скалярным уравнениям) дают 6 нелинейных дифференциальных уравнений первого порядка, относительно 6 переменных: трёх скоростей и трёх угловых скоростей. Это так называемая 6DOF задача, т.е. задача с шестью степенями свободы. Сначала можно подумать, что раз у коптера имеется 6 степеней свободы, то должно быть и 6 переменных состояния (дифференциальных переменных). Но это так только на первый взгляд. Кроме скоростей, дальше нам придется получить еще и координаты (три линейных и три угла положения в пространстве) – для чего еще раз проинтегрировать скорости. Таким образом, всего у коптера есть 12 степеней свободы. А если учесть еще то, что правые части дифференциальных уравнений есть ни что иное как ускорения коптера по осям, то получим как бы 18 степеней свободы. Это важно понимать, так как в дальнейшем для построения регулятора нам потребуются все 12 фазовых координат объекта плюс ещё 6 измеренных (или вычисленных) ускорения коптера.
Для решения дифференциального уравнения методом структурного моделирования используются либо типовые звенья первого и второго порядков – если дифференциальное уравнение линейно и подходит под один из типов, либо используется блок типа «Интегратор», на вход которому подаётся правая часть уравнения, а на выходе будет искомая интегральная величина. Приведём пример – заготовку для решения нашей системы из 6-ти дифференциальных уравнений, см. рисунок 2.
На рисунке 2 представлена «основа» динамической части модели октокоптера, которую формируют 6+3+3 блоков типа «интегратор». Первые шесть блоков, получая на вход правые части дифференциальных уравнений (вычислим их ниже) – ускорения коптера по осям , – занимаются интегрированием и вычислением скоростей коптера по этим же осям (тоже, в связанной с коптером системе В).
Следующие три интегратора принимают линейные скорости в системе координат I (полученные алгебраически из скоростей в системе B путём применения матрицы поворота) и, интегрируя их, получают координаты центра масс коптера в инерциальной системе координат
И, еще три блока типа «интегратор» занимаются вычислением углов ориентации коптера, интегрируя их производные (угловые скорости) в системе I, полученные из угловых скоростей коптера в системе B применением матрицы . Здесь же реализовано и вычисление нужных тригонометрических функций от углов поворота.
Обилие блоков «в память» и «из памяти» обусловлено многократным использованием в других частях модели полученных величин – например, по рисунку 1 видно что тригонометрические функции от углов ориентации используются многократно и оптимально вычислить их в одном месте схемы а потом уже использовать по мере необходимости.
Если бы у нас была более простая ситуация – одно дифференциальное уравнение второго порядка (классический второй закон Ньютона a = F/m), например в проекции на ось x, то его решение таким же способом выглядело бы как на рисунке 3:
В случае коптера используется такой же приём, т.е. если на выходе из интегратора имеется какая-то величина, то слева от него – обязательно производная этой же величины. Таким образом записываются и решаются дифференциальные уравнения методом структурного моделирования, если не прибегать к встроенному языку программирования SimInTech (где зачастую удаётся сделать то же самое еще более просто, но менее очевидно).
В субмодели W(B->I) реализована матрица преобразования из угловой скорости в системе B в производные углов Эйлера, см. рисунок 4 и [1] для подробностей.
Для замыкания модели необходимо посчитать и реализовать все правые части у шести основных уравнений, то есть спроецировать полученные в разделе 2 векторные уравнения на оси и результат нарисовать в схеме SimInTech. Опуская выкладки (их читатель может выполнить самостоятельно на листке бумаги, или при помощи какого-то символьного математического ПО), приведем окончательный вид уравнений. Из-за громоздкости, приводить будем по слагаемым в правых частях.
Сила тяги винтомоторных групп
Сила сопротивления воздуха при отсутствии ветра – формулы приведены выше, проекции будут равны:
Сила тяжести, в проекции на оси подвижной системы координат, слагаемое очевидно будет равно:
И последнее слагаемое в уравнении линейной скорости коптера – векторное произведение скоростей:
Если аккуратно подставить полученные проекции в уравнение для производной линейной скорости коптера, и дальше реализовать всё это в SimInTech, по осям, получим следующие структурные схемы, представленные на рисунках 5, 6 и 7 (показаны проекции только на ось , на другие оси результат аналогичен с точностью до слагаемых):
Итого, на данном этапе мы до конца реализовали одно из шести основных уравнений (согласно рисунку 2 – вычислили вход в первый интегратор). Остальные оборванные там входы для сил вычисляются аналогично. Рассмотрим уравнение для моментов и для вычисления угловой скорости, а точнее – его слагаемые в правой части:
Сумма моментов сил тяги всех ВМГ:
где – сила тяги i-ой ВМГ в текущий момент времени, -плечи сил (длины лучей рамы коптера для нечетных и четных ВМГ).Момент сопротивления воздуха (при отсутствии ветра):
Векторное произведение угловой скорости на произведение тензора инерции и угловой скорости:
Итого, после подстановки этих слагаемых в дифференциальное уравнение для угловой скорости, и реализации полученного в среде SimInTech, получим следующие структурные схемы:
Итого, если представить все реализованные уравнения рядом на схеме, получим следующую картину (рисунок 11). Как видно, она довольно громоздка, даже без включения сюда других дифференциальных уравнений для вычисления координат коптера и углов поворота. А поскольку в дальнейшем на каждое уравнение будут навешиваться еще дополнительные сервисные вычисления (ускорений, различных составляющих сил и моментов, для отладки модели и регуляторов), целесообразнее эти уравнения структурно разделить на 6+ субмоделей, что и было сделано, в каждой из которых решается своё уравнение, см. рисунок 12.
В принципе, все приведенные уравнений и структуры можно было реализовать и средствами языка программирования SimInTech, но тогда они были бы не так наглядны.
Входными величинами здесь являются силы тяги двигателей (они считаются вне этих уравнений, в зависимости от текущей скорости вращения каждой ВМГ и её силовой характеристики), внешняя возмущающая сила и момент сил (задаются пользователем произвольно) и сила тяжести. Также, возможно ещё влияние ветра/прецессия/реактивный момент или что-то ещё — в настоящей статье это не рассматривается, для краткости мы ограничились силами тяги двигателей и притяжения Земли.
Выходными величинами являются ускорения, скорости и положение (координаты) коптера – ускорения и скорости в системах координат B и I, положение – в системе I (положение коптера в системе B практической ценности почти не имеет т.к. сама система движется вместе с коптером и совпадает по положению с ним – то есть там положение коптера всегда будет нулевым).
В продолжении мы рассмотрим создание системы управления для октокоптера, а так же рассмотрим принципы создания видеокадров управления.
Модель коптера, рассмторенную в статье, можно взять по этой ссылке.
К сожалению, не доступен сервер mySQL