Полет роботом

Летающий робот в этом примере имеет четыре ускорителя, чтобы переместить его на 2D пробеле. Модель имеет шесть состояний:

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

Планирование траектории

Робот первоначально покоится в [-10,-10] с углом ориентации pi/2 радианы (стоящий перед севером). Летающий маневр для этого примера должен переместить и припарковать робота в итоговом местоположении [0,0] с углом 0 радианы (стоящий перед востоком) в 12 секунды. Цель состоит в том, чтобы найти оптимальный путь таким образом, что общая сумма топлива, использованного ускорителями во время маневра, минимизирована.

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

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

Ts = 0.4;
p = 30;
nx = 6;
nu = 4;
nlobj = nlmpcMultistage(p,nx,nu);
nlobj.Ts = Ts;

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

Задайте функцию состояния модели предсказания с помощью имени функции. Можно также задать функции с помощью указателя на функцию. Для получения дополнительной информации на функции состояния, откройте FlyingRobotStateFcn.m. Для получения дополнительной информации об определении модели предсказания смотрите, Задают Модель Предсказания для Нелинейного MPC.

nlobj.Model.StateFcn = "FlyingRobotStateFcn";

Задайте якобиан функции состояния с помощью указателя на функцию. Это - лучшая практика, чтобы обеспечить аналитический якобиан для модели предсказания. Выполнение так значительно повышает эффективность симуляции. Для получения дополнительной информации на Функции Якоби, откройте FlyingRobotStateJacobianFcn.m.

nlobj.Model.StateJacFcn = @FlyingRobotStateJacobianFcn;

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

for ct = 1:p
    nlobj.Stages(ct).CostFcn = 'FlyingRobotCostFcn';
end

Цель маневра состоит в том, чтобы припарковать робота в [0,0] с углом 0 радианы в 12-ю секунду. Задайте эту цель как ограничение конечного состояния, где каждое состояние положения и скорости на последнем шаге предсказания (этап p+1) должно быть нулем. Для получения дополнительной информации об определении ограничительных функций смотрите, Задают Ограничения для Нелинейного MPC.

nlobj.Model.TerminalState = zeros(6,1);

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

Каждая тяга имеет рабочий диапазон между 0 и 1, который переводится в нижние и верхние границы на MVS.

for ct = 1:nu
    nlobj.MV(ct).Min = 0;
    nlobj.MV(ct).Max = 1;
end

Задайте начальные условия для робота.

x0 = [-10;-10;pi/2;0;0;0];  % robot parks at [-10, -10], facing north
u0 = zeros(nu,1);           % thrust is zero

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

validateFcns(nlobj,x0,u0);

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] are OK.
Analysis of user-provided model, cost, and constraint functions complete.

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

[~,~,info] = nlmpcmove(nlobj,x0,u0);

Постройте оптимальную траекторию. Оптимальная стоимость 7.8.

FlyingRobotPlotPlanning(info,Ts);

Optimal fuel consumption =   7.797825

Первый график показывает оптимальную траекторию шести состояний робота во время маневра. Второй график показывает соответствующие оптимальные профили мВ для четырех тяг. Третий график показывает траекторию положения X-Y робота, перемещающегося от [-10 -10 pi/2] к [0 0 0].

Управление с обратной связью для следования траектории

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

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

ny = 6;
nlobj_tracking = 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_tracking.Model.StateFcn = nlobj.Model.StateFcn;
nlobj_tracking.Jacobian.StateFcn = nlobj.Model.StateJacFcn;

Для отслеживания приложений управления уменьшайте вычислительное усилие путем определения более короткого горизонта предсказания (никакая потребность далеко изучить будущее) и горизонт управления (например, свободные перемещения выделяются на первых нескольких шагах predictrion).

nlobj_tracking.Ts = Ts;
nlobj_tracking.PredictionHorizon = 10;
nlobj_tracking.ControlHorizon = 4;

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

nlobj_tracking.Weights.ManipulatedVariablesRate = 0.2*ones(1,nu);
nlobj_tracking.Weights.OutputVariables = 5*ones(1,nx);

Установите те же границы для входных параметров ускорителя.

for ct = 1:nu
    nlobj_tracking.MV(ct).Min = 0;
    nlobj_tracking.MV(ct).Max = 1;
end

Кроме того, чтобы уменьшать расход топлива, это ясно тот u1 и u2 не может быть положительным никогда во время операции. Поэтому реализуйте ограничения равенства, таким образом что u(1)*u(2) должен быть 0 для всех шагов предсказания. Примените подобные ограничения для u3 и u4.

nlobj_tracking.Optimization.CustomEqConFcn = ...
    @(X,U,data) [U(1:end-1,1).*U(1:end-1,2); U(1:end-1,3).*U(1:end-1,4)];

Подтвердите свою модель предсказания и пользовательские функции и их Якобианы.

validateFcns(nlobj_tracking,x0,u0);

Model.StateFcn is OK.
Jacobian.StateFcn is OK.
No output function specified. Assuming "y = x" in the prediction model.
Optimization.CustomEqConFcn is OK.
Analysis of user-provided model, cost, and constraint functions complete.

Нелинейная оценка состояния

В этом примере только измеряются три состояния положения (x, y и угол). Скоростные состояния не измерены и должны быть оценены. Используйте расширенный фильтр Калмана (EKF) от Control System Toolbox™ для нелинейной оценки состояния.

Поскольку EKF требует модели дискретного времени, вы используете метод трапеций для перехода от x (k) к x (k+1), который требует решения nx нелинейные алгебраические уравнения. Для получения дополнительной информации, открытый FlyingRobotStateFcnDiscreteTime.m.

DStateFcn = @(xk,uk,Ts) FlyingRobotStateFcnDiscreteTime(xk,uk,Ts);

Измерение может помочь EKF откорректировать свою оценку состояния. Только первые три состояния измеряются.

DMeasFcn = @(xk) xk(1:3);

Создайте EKF и укажите, что измерения имеют мало шума.

EKF = extendedKalmanFilter(DStateFcn,DMeasFcn,x0);
EKF.MeasurementNoise = 0.01;

Симуляция с обратной связью отслеживания управления

Симулируйте систему для 32 шаги с правильными начальными условиями.

Tsteps = 32;
xHistory = x0';
uHistory = [];
lastMV = zeros(nu,1);

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

Xopt = info.Xopt;
Xref = [Xopt(2:p+1,:);repmat(Xopt(end,:),Tsteps-p,1)];

Используйте nlmpcmove и nlmpcmoveopt команда для симуляции с обратной связью.

hbar = waitbar(0,'Simulation Progress');
options = nlmpcmoveopt;
for k = 1:Tsteps
    % Obtain plant output measurements with sensor noise.
    yk = xHistory(k,1:3)' + randn*0.01;
    % Correct state estimation based on the measurements.
    xk = correct(EKF, yk);
    % Compute the control moves with reference previewing.
    [uk,options] = nlmpcmove(nlobj_tracking,xk,lastMV,Xref(k:min(k+9,Tsteps),:),[],options);
    % Predict the state for the next step.
    predict(EKF,uk,Ts);
    % Store the control move and update the last MV for the next step.
    uHistory(k,:) = uk';
    lastMV = uk;
    % Update the real plant states for the next step by solving the
    % continuous-time ODEs based on current states xk and input uk.
    ODEFUN = @(t,xk) FlyingRobotStateFcn(xk,uk);
    [TOUT,YOUT] = ode45(ODEFUN,[0 Ts], xHistory(k,:)');
    % Store the state values.
    xHistory(k+1,:) = YOUT(end,:);
    % Update the status bar.
    waitbar(k/Tsteps, hbar);
end
close(hbar)

Сравните запланированные и фактические траектории с обратной связью.

FlyingRobotPlotTracking(info,Ts,p,Tsteps,xHistory,uHistory);

Actual fuel consumption =  10.707064

Нелинейный диспетчер обратной связи MPC успешно перемещает робота (синие блоки), после оптимальной траектории (желтые блоки), и паркует его в итоговом местоположении (красный блок) на последнем рисунке.

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

Ссылки

[1] И. Сэкоа. "Планирование траектории свободно летающего робота при помощи оптимального управления". Приложения Оптимального управления и Методы, Издание 20, 1999, стр 235-248.