Летающий робот

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

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

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

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

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

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

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, который переводится в нижнюю и верхнюю границы на MV.

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.

Оптимальное состояние и траектории MV можно найти, позвонив в 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].

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

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

Для отслеживания можно использовать различные методы управления с обратной связью. В этом примере вы используете другой нелинейный контроллер 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 является стандартной квадратичной функцией стоимости, подходящей для отслеживания уставки и подавления помех. Для отслеживания состояния имеют более высокий приоритет (большие веса штрафов), чем движения MV.

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.