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

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

Государства:

Входы: # тяга слева, в Newtons # тяга справа, в Newtons

Модель ракеты в непрерывном времени реализована с 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;

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

Используйте одну и ту же функцию затрат для всех этапов. Поскольку MV действительны только от этапа 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 = 10.2661
Objective cost = 492.9689ExitFlag = 388Iterations = 388

Извлеките оптимальную траекторию из 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;

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

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)