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

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

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

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

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

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

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

nx = 6;
ny = 6;
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.

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

nlobj.Model.StateFcn = "FlyingRobotStateFcn";

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

nlobj.Jacobian.StateFcn = @FlyingRobotStateJacobianFcn;

В данном примере временем целевого прогноза является 12 секунды. Поэтому задайте шаг расчета 0.4 секунды и горизонт предсказания 30 шаги.

Ts = 0.4;
p = 30;
nlobj.Ts = Ts;
nlobj.PredictionHorizon = p;

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

nlobj.ControlHorizon = p;

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

nlobj.Optimization.CustomCostFcn = @(X,U,e,data) Ts*sum(sum(U(1:p,:)));

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

nlobj.Optimization.ReplaceStandardCost = true;

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

nlobj.Optimization.CustomEqConFcn = @(X,U,data) X(end,:)';

Это - лучшая практика обеспечить аналитические Функции Якоби для вашей пользовательской стоимости и ограничительных функций также. Однако этот пример намеренно пропускает их так, чтобы их якобиан был вычислен нелинейным контроллером 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.
Jacobian.StateFcn is OK.
No output function specified. Assuming "y = x" in the prediction model.
Optimization.CustomCostFcn is OK.
Optimization.CustomEqConFcn is OK.
Analysis of user-provided model, cost, and constraint functions complete.

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

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

Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard.

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

FlyingRobotPlotPlanning(info);

Optimal fuel consumption =   7.797825

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

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

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

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

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.Jacobian.StateFcn;

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

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.707633

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

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

Ссылки

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