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

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

Штаты:

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

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

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

Планировщик проекта и поиск оптимальной траектории посадки

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;

Проверка конструкции контроллера.

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 можно реализовать такое же моделирование с замкнутым контуром, используя блок многоступенчатого нелинейного 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] 

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

bdclose(mdl)