Получите нелинейную динамическую модель ракеты

Первый принцип нелинейная динамическая модель ракеты имеет шесть состояний и 2 входных параметров. Оба входных параметров являются переменными, которыми управляют.

Состояния:

Входные параметры: тяга # слева, в Ньютонах # тяга справа, в Ньютонах

Модель непрерывного времени ракеты реализована с RocketStateFcn функция. Чтобы ускорить оптимизацию, ее аналитический якобиан вручную выведен в RocketStateJacobianFcn функция. Модель допустима, только если ракета выше или в земле ().

Во время выполнения вы принимаете, что все состояния измеримы и существует датчик, читая, который обнаруживает грубое приземление (-1), мягкое приземление (1), или в воздухе (0) условие.

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

MPC использует внутреннюю модель, чтобы предсказать поведение объекта в будущем. Учитывая текущие состояния объекта, на основе модели предсказания, MPC находит последовательность оптимального управления, которая минимизирует стоимость и удовлетворяет всем ограничениям, заданным через горизонт предсказания. Поскольку MPC находит траекторию состояния объекта в будущем, можно использовать MPC, чтобы решить задачи оптимизации траектории. Такие проблемы включают автономную парковку транспортного средства, планирование движения манипулятора и нахождение приземляющегося пути для ракеты.

Для задач оптимизации траектории объект, функция стоимости и ограничения часто нелинейны. Поэтому необходимо сформулировать и решить проблему планирования с помощью нелинейного MPC. Можно передать сгенерированный оптимальный путь к контроллеру следования траектории как опорный сигнал, так, чтобы он мог выполнить запланированный маневр.

По сравнению с типовым нелинейным контроллером MPC, реализованное использование nlmpc объект, многоступенчатый нелинейный MPC предоставляет более гибкому и эффективному внедрению подготовленные затраты и ограничениям. Эта гибкость и КПД особенно полезны для планирования траектории.

Многоступенчатый нелинейный контроллер MPC с горизонтом предсказания p задает p+1 этапы, которые представляют времена k (текущее время) через k+p. Для каждого этапа можно задать поэтапную стоимость, ограничение неравенства и функции ограничения равенства. Каждая функция зависит только от состояния объекта и входных значений на соответствующем этапе.

Учитывая текущие состояния объекта, x[k], контроллер MPC находит траекторию переменной, которой управляют, (MV) (со времени k к k+p-1) это оптимизирует суммированные затраты этапа (со времени k к k+p) при удовлетворении всем ограничениям этапа (со времени k к k+p).

В этом примере выберите горизонт предсказания p и шаг расчета Ts таким образом, что временем предсказания является p*Ts = 10 секунды.

Ts = 0.2;
pPlanner = 50;

Создайте многоступенчатый нелинейный контроллер MPC для заданного горизонта предсказания и шага расчета.

planner = nlmpcMultistage(pPlanner,6,2);
planner.Ts = Ts;

Задайте модель предсказания и ее аналитический якобиан.

planner.Model.StateFcn = 'RocketStateFcn';
planner.Model.StateJacFcn = 'RocketStateJacobianFcn';

Задайте твердые границы на этих двух ускорителях. Можно настроить максимальную тягу и наблюдать ее удар на приземляющуюся стратегию, выбранную планировщиком. Типичные максимальные значения между 6 и 10 ньютонами. Если максимальная тяга слишком мала, вы не можете смочь посадить ракету успешно, если исходное положение сложно.

planner.MV(1).Min = 0;
planner.MV(1).Max = 8;
planner.MV(2).Min = 0;
planner.MV(2).Max = 8;

Чтобы постараться не отказывать, задайте твердую нижнюю границу на вертикальном положении Y.

planner.States(2).Min = 10;

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

Используйте ту же функцию стоимости для всех этапов. Начиная с MVS только допустимы от этапа 1 до этапа p, вы не должны задавать этап, стоивший за заключительный этап, p+1.

for ct=1:pPlanner
    planner.Stages(ct).CostFcn = 'RocketPlannerCostFcn';
    planner.Stages(ct).CostJacFcn = 'RocketPlannerCostGradientFcn';
end

Чтобы гарантировать успешное приземление в цели, задайте конечное состояние для заключительного этапа.

planner.Model.TerminalState = [0;10;0;0;0;0];

В этом примере, определенном максимальный номер итераций к большому значению, чтобы вместить большое пространство поиска и неидеальное исходное предположение по умолчанию.

planner.Optimization.SolverOptions.MaxIterations = 1000;

После создания вас нелинейный контроллер MPC проверяйте, существует ли какая-либо проблема с вашим состоянием, стоимостью, и ограничительными функциями, а также их аналитическими Функциями Якоби. Для этого вызовите validateFcns функции со случайными начальными состояниями объекта и входными параметрами.

validateFcns(planner,rand(6,1),rand(2,1));

Model.StateFcn is OK.
Model.StateJacFcn is OK.
"CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50] are OK.
"CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50] are OK.
Analysis of user-provided model, cost, and constraint functions complete.

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

fprintf('Rocker landing planner running...\n');
tic;
[~,~,info] = nlmpcmove(planner,x0,u0);
t=toc;
fprintf('Calculation Time = %s\n',num2str(t));
fprintf('Objective cost = %s',num2str(info.Cost));
fprintf('ExitFlag = %s',num2str(info.Iterations));
fprintf('Iterations = %s\n',num2str(info.Iterations));

Rocker landing planner running...
Calculation Time = 307.1117
Objective cost = 492.9689ExitFlag = 384Iterations = 384

Извлеките оптимальную траекторию из info структура и график результат.

figure
subplot(2,1,1)
plot(info.Xopt(:,1),info.Xopt(:,2),'*')
title('Optimal XY Trajectory')
subplot(2,1,2)
plot(info.Topt,info.MVopt(:,1),info.Topt,info.MVopt(:,2))
title('Optimal MV Trajectory')

Анимируйте запланированную оптимальную траекторию.

plotobj = RocketAnimation(6,2);
for ct=1:pPlanner+1
    updatePlot(plotobj,(ct-1)*planner.Ts,info.Xopt(ct,:),info.MVopt(ct,:));
    pause(0.1);
end

Спроектируйте посадочный модуль и следуйте за оптимальным путем

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

pLander = 10;
lander = nlmpcMultistage(pLander,6,2);
lander.Ts = Ts;

Для контроллера следования траектории у высаживающегося на берег есть та же модель предсказания, границы тяги и минимум Y положение.

lander.Model.StateFcn = 'RocketStateFcn';
lander.Model.StateJacFcn = 'RocketStateJacobianFcn';
lander.MV(1).Min = 0;
lander.MV(1).Max = 8;
lander.MV(2).Min = 0;
lander.MV(2).Max = 8;
lander.States(2).Min = 10;

Функция стоимости для высаживающегося на берег отличается от того из планировщика. Высаживающийся на берег использует квадратичные термины стоимости, чтобы достигнуть обоих трудных отслеживаний уставки (путем наложения штрафа на ошибку отслеживания) и сглаженные действия управления (путем наложения штрафа на большие изменения в действиях управления). Эта функция стоимости высаживающегося на берег реализована в RocketLanderCostFcn функция. Соответствующая вручную выведенная функция градиента стоимости реализована в RocketLanderCostGradientFcn.

Во время выполнения вы предоставляете шесть ссылок траектории состояния высаживающемуся на берег как параметры этапа. Поэтому задайте количество параметров для каждого этапа.

for ct=1:pLander+1
    lander.Stages(ct).CostFcn = 'RocketLanderCostFcn';
    lander.Stages(ct).CostJacFcn = 'RocketLanderCostGradientFcn';
    lander.Stages(ct).ParameterLength = 6;
end

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

lander.UseMVRate = true;

Подтвердите проектирование контроллера.

simdata = getSimulationData(lander);
validateFcns(lander,rand(6,1),rand(2,1),simdata);

Model.StateFcn is OK.
Model.StateJacFcn is OK.
"CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11] are OK.
"CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11] are OK.
Analysis of user-provided model, cost, and constraint functions complete.

Симулируйте приземляющийся маневр в сценарии управления с обратной связью путем итеративного вызова nlmpcmove. Эта симуляция принимает, что все состояния измеряются. Остановите симуляцию, когда состояния ракеты будут достаточно близки к целевым состояниям.

x = x0;
u = u0;
k = 1;
references = reshape(info.Xopt',(pPlanner+1)*6,1); % Extract reference signal as column vector.
while true
    % Obtain new reference signals.
    simdata.StageParameter = RocketLanderReferenceSignal(k,references,pLander);
    % Compute the control action.
    [u,simdata,infoLander] = nlmpcmove(lander,x,u,simdata);
    % Update the animation plot.
    updatePlot(plotobj,(k-1)*Ts,x,u);
    pause(0.1);
    % Simulate the plant to the next state using an ODE solver.
    [~,X] = ode45(@(t,x) RocketStateFcn(x,u),[0 Ts],x);
    x = X(end,:)';
    % Stop if rocket has landed.
    if max(abs(x-[0;10;0;0;0;0])) < 1e-2
        % Plot the the final rocket position.
        updatePlot(plotobj,k*Ts,x,zeros(2,1));
        break
    end
    % Move to the next simulation step.
    k = k + 1;
end

Из-за более короткого горизонта и различных терминов стоимости, приземляющаяся траектория немного отличается от запланированной траектории, и занимает больше времени приземлиться. Этот результат часто, что происходит в такой двухуровневой среде управления с планированием и регулированием.

Симулируйте в Simulink Используя многоступенчатый нелинейный блок MPC

Можно реализовать ту же симуляцию с обратной связью в модели Simulink с помощью блока Multistage Nonlinear MPC.

mdl = 'RocketLanderSimulation';
open_system(mdl)

Осциллограф Штатов показывает, что состояния объекта принесены к целевым состояниям в соответствующее время.

sim(mdl)
open_system([mdl '/States'])
open_system([mdl '/MVs'])

ans = 

  Simulink.SimulationOutput:
                   tout: [81x1 double] 

     SimulationMetadata: [1x1 Simulink.SimulationMetadata] 
           ErrorMessage: [0x0 char] 

Для приложений реального времени можно сгенерировать код от блока Multistage Nonlinear MPC.

bdclose(mdl)