Модель квадротора

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

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

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

где

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

$${}_{\mathrm{}}^{\mathrm{}}{}_{\mathrm{}}^{\mathrm{}}{}_{\mathrm{}}^{\mathrm{}}{}_{\mathrm{}}^{\mathrm{}}$$[ω12,ω22,ω32,ω42].

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

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

getQuadrotorDynamicsAndJacobian;

ans = function_handle with value:
    @QuadrotorStateFcn

ans = function_handle with value:
    @QuadrotorStateJacobianFcn

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,\; («x\mathrm{,\; y,\; z}$$,»), Поскольку количество 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];

Кроме того, наказывать агрессивные контрольные действия, указывая веса настройки для скоростей изменения СН.

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,\; (X\mathrm{,\; Y,\; Z}$$,

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

animateQuadrotorTrajectory;

Заключение

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

Ссылки

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

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