Квадроторная модель

Квадротор имеет четыре ротора, которые направлены вверх. От центра масс квадротора роторы помещаются в квадратную формацию с равным расстоянием. Математическая модель для квадроторной динамики получена из уравнений Эйлера-Лагранжа [1].

Двенадцать состояний квадротора:

$$[x,y,z,\varphi ,\theta ,\psi ,\underset{}{\overset{\dot{}}{x}},\underset{}{\overset{\dot{}}{y}},\underset{}{\overset{\dot{}}{z}},\underset{}{\overset{\dot{}}{\varphi }},\underset{}{\overset{\dot{}}{\theta }},\underset{}{\overset{\dot{}}{\psi }}]$$,

где

Управляющими входами для квадротора являются квадратные скорости вращения четырех роторов:

$$[{\omega }_1^2,{\omega }_2^2,{\omega }_3^2,{\omega }_4^2]$$.

Эти входы управления создают силу, крутящий момент и тягу в направлении тела $\mathit{z}$-ось. В этом примере каждое состояние поддается измерению, и входы управления ограничены в пределах [0,12] ${\left(\frac{\mathrm{rad}}{\mathit{s}}\right)}^2$.

Функция состояния и функция Якобиана состояния модели построены и выведены с помощью программного обеспечения Symbolic Math Toolbox™.

getQuadrotorDynamicsAndJacobian;

ans = function_handle with value:
    @QuadrotorStateFcn

ans = function_handle with value:
    @QuadrotorStateJacobianFcn

The getQuadrotorDynamicsAndJacobian скрипт генерирует следующие файлы:

Для получения дополнительной информации о любой функции откройте соответствующий файл.

Проектируйте нелинейный прогнозирующий контроллер модели

Создайте нелинейный объект MPC с 12 состояниями, 12 выходами и 4 входы. По умолчанию все входы являются управляемыми переменными (MV).

nx = 12;
ny = 12;
nu = 4;
nlobj = nlmpc(nx, ny, nu);

In standard cost function, zero weights are applied by default to one or more OVs because there are fewer MVs than OVs.

Задайте функцию состояния модели предсказания с помощью имени функции. Можно также задать функции с помощью указателя на функцию.

nlobj.Model.StateFcn = "QuadrotorStateFcn";

Задайте якобиан функции состояния с помощью указателя на функцию. Лучшая практика - предоставить аналитический якобиан для модели предсказания. Это значительно повышает эффективность симуляции.

nlobj.Jacobian.StateFcn = @QuadrotorStateJacobianFcn;

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

rng(0)
validateFcns(nlobj,rand(nx,1),rand(nu,1));

Model.StateFcn is OK.
Jacobian.StateFcn is OK.
No output function specified. Assuming "y = x" in the prediction model.
Analysis of user-provided model, cost, and constraint functions complete.

Задайте шаг расчета 0.1 секунд, горизонта предсказания 18 шагов и управляющего горизонта 2 шага.

Ts = 0.1;
p = 18;
m = 2;
nlobj.Ts = Ts;
nlobj.PredictionHorizon = p;
nlobj.ControlHorizon = m;

Ограничьте все четыре входов управления в области значений [0,12].

nlobj.MV = struct('Min',{0;0;0;0},'Max',{12;12;12;12});

Функция стоимости по умолчанию в нелинейном MPC является стандартной квадратичной функцией стоимости, подходящей для отслеживания уставки и подавления помех. В этом примере первые 6 состояний $$[x,y,z,\varphi ,\theta ,\psi ]$$ требуются для следования по заданной эталонной траектории. Поскольку количество MV (4) меньше, чем количество эталонных выходных траекторий (6), недостаточно степеней свободы для отслеживания желаемых траекторий для всех выходных переменных (OV).

nlobj.Weights.OutputVariables = [1 1 1 1 1 1 0 0 0 0 0 0];

В этом примере MV также имеют номинальные цели, чтобы сохранить квадротор плавающим, что может привести к конфликту между MV и OV отслеживания уставки целями. Чтобы определить приоритеты целей, установите средний приоритет отслеживания MV ниже, чем средний приоритет отслеживания OV.

nlobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];

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

nlobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1];

Симуляция замкнутой системы

Симулируйте систему в течение 20 секунд с целевой траекторией для следования.

% Specify the initial conditions
x = [7;-10;0;0;0;0;0;0;0;0;0;0];
% Nominal control that keeps the quadrotor floating
nloptions = nlmpcmoveopt;
nloptions.MVTarget = [4.9 4.9 4.9 4.9]; 
mv = nloptions.MVTarget;

Симулируйте систему с обратной связью с помощью nlmpcmove функция, установка опций симуляции с помощью nlmpcmove объект.

Duration = 20;
hbar = waitbar(0,'Simulation Progress');
xHistory = x';
lastMV = mv;
uHistory = lastMV;
for k = 1:(Duration/Ts)
    % Set references for previewing
    t = linspace(k*Ts, (k+p-1)*Ts,p);
    yref = QuadrotorReferenceTrajectory(t);
    % Compute the control moves with reference previewing.
    xk = xHistory(k,:);
    [uk,nloptions,info] = nlmpcmove(nlobj,xk,lastMV,yref',[],nloptions);
    uHistory(k+1,:) = uk';
    lastMV = uk;
    % Update states.
    ODEFUN = @(t,xk) QuadrotorStateFcn(xk,uk);
    [TOUT,YOUT] = ode45(ODEFUN,[0 Ts], xHistory(k,:)');
    xHistory(k+1,:) = YOUT(end,:);
    waitbar(k*Ts/Duration,hbar);
end
close(hbar)

Визуализация и результаты

Постройте график результатов и сравните плановые и фактические траектории замкнутой системы.

plotQuadrotorTrajectory;

Поскольку количество MV меньше, чем количество эталонных выходных траекторий, недостаточно степеней свободы для отслеживания желаемых траекторий для всех OV.

Как показано на рисунке для состояний $$[x,y,z,\varphi ,\theta ,\psi ]$$ и управляющие входы,

Можно анимировать траекторию квадротора. Квадротор перемещается близко к «целевому» квадротору, который перемещается по ссылке траектории в течение 7 секунд. После этого квадротор тесно следует по ссылке траектории. Анимация заканчивается через 20 секунд.

animateQuadrotorTrajectory;

Заключение

Этот пример показывает, как спроектировать нелинейный прогнозирующий контроллер модели для отслеживания траектории квадротора. Динамика и якобианы квадротора получают с помощью программного обеспечения Symbolic Math Toolbox. Квадротор тесно отслеживает траекторию ссылки.

Ссылки

[1] Т. Луукконен, Моделирование и управление квадрокоптером, Независимый исследовательский проект в прикладной математике, Университет Эспоо: Аалто, 2011.

[2] Е. Цораколефтеракис, и Т. Д. Мерфи. Итерационный контроль последовательного действия для стабильного, основанного на модели управления нелинейными системами. Транзакции IEEE по автоматическому контролю (2018).