Модель Quadrotor

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

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

$$[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 }}]$$,

где

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

$$[{\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

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

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

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

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

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 ]$$ требуются, чтобы следовать за данной ссылочной траекторией. Поскольку количество MVS (4) меньше, чем количество ссылочных выходных траекторий (6), существует недостаточно степеней свободы, чтобы отследить желаемые траектории для всех выходных переменных (OVs).

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

В этом примере MVS также имеет номинальные цели, чтобы сохранить плавание quadrotor, которое может вести, чтобы конфликтовать между мВ и целями отслеживания уставки OV. Чтобы приоритизировать цели, установите средний приоритет отслеживания мВ ниже, чем средний 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;

Поскольку количество MVS меньше, чем количество ссылочных выходных траекторий, существует недостаточно степеней свободы, чтобы отследить желаемые траектории для всего OVs.

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

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

animateQuadrotorTrajectory;

Заключение

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

Ссылки

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

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