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

Летающий робот в этом примере имеет четыре двигателя малой тяги для перемещения его в 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.

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