Обзор

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

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

Этот пример требует программного обеспечения Optimization Toolbox и Robotics System Toolbox.

Грузовые автомобили и одиночная прицепная система

Следующий рисунок показывает нелинейную динамическую систему грузовика и прицепа.

Состояниями для этой модели являются:

Входные входы для этой модели:

Для этой модели длина и положение указаны в метрах, скорость - в м/с, а углы - в радианах.

Задайте следующие параметры модели.

params = struct('M', 1, ...
    'L1',6,...
    'W1',2.5,...
    'L2',10,...
    'W2',2.5,...
    'Lwheel',1,...
    'Wwheel',0.4);

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

В этом примере, поскольку MPC используется в качестве планировщика пути вместо низкоуровневого контроллера следования пути, velocity используется как одна из манипулируемых переменных вместо acceleration.

Автоматическая проблема парковки

Площадь парковки составляет 100 метров в ширину и 60 метров в длину. Цель состоит в том, чтобы найти жизнеспособный путь, который доставляет систему грузовика и прицепа из любого начального положения в целевое положение (зеленый крест на следующем рисунке) за 20 секунд с помощью обратной парковки. В процессе плановый путь должен избегать столкновений с двумя препятствиями рядом с местом парковки.

initialPose = [0;0;0;0];
targetPose = [0;-25;pi/2;0];
TruckTrailerPlot(initialPose,targetPose,params);

Начальные входы объекта управления (угол поворота и продольная скорость) равны 0.

u0 = zeros(2,1);

Исходная позиция должна быть действительной. Используйте функцию ограничения неравенства, чтобы проверить, что это так. Подробности об этой функции обсуждаются ниже в примере. Обнаружение столкновения осуществляется определенными функциями Robotics System Toolbox.

cineq = TruckTrailerIneqConFcn(1,initialPose,u0,...
    [params.M;params.L1;params.L2;params.W1;params.W2]);
if any(cineq>0)
    fprintf('Initial pose is not valid.\n');
    return
end

Планировщик пути с использованием многоступенчатого нелинейного MPC

По сравнению с типовым нелинейным контроллером MPC (nlmpc объект), многоступенчатый нелинейный MPC предоставляет вам более гибкий и эффективный способ реализации MPC с поэтапными затратами и ограничениями. Эта гибкость и эффективность особенно полезны для планирования траектории.

Многоступенчатый нелинейный контроллер MPC с горизонтом предсказания p определяет p+1 этапы, представляющие временные k (текущее время), k+1..., k+p. Для каждого этапа можно задать специфические для этапа затраты, ограничения неравенства и функции ограничения равенства. Эти функции зависят только от состояния объекта и входных значений на этом этапе. Учитывая текущие состояния объекта x[k]MPC находит измененную траекторию переменной (MV) (от времени k на k+p-1 для оптимизации суммированных затрат на этап (время от времени k на k+p), удовлетворяющий всем ограничениям этапа (время от времени k на k+p).

В этом примере объект имеет четыре состояния и два входа (оба MV). Горизонт предсказания p и шаг расчета Ts выбраны так, что p * Ts = 20.

Создайте многоступенчатый нелинейный контроллер MPC.

p = 40;
nlobj = nlmpcMultistage(p,4,2);
nlobj.Ts = 0.5;

Задайте модель предсказания и ее аналитический якобиан в объекте контроллера. Поскольку модель требует трех параметров (M, L1, и L2), установите Model.ParameterLength на 3.

nlobj.Model.StateFcn = "TruckTrailerStateFcn";
nlobj.Model.StateJacFcn = "TruckTrailerStateJacobianFcn";
nlobj.Model.ParameterLength = 3;

Задайте жесткие границы двух управляемых переменных. Угол поворота руля должен оставаться в области значений +/- 45 степени. Максимальная прямая скорость составляет 10 м/с, а максимальная обратная - -10 м/с.

nlobj.MV(1).Min = -pi/4;     % Minimum steering angle
nlobj.MV(1).Max =  pi/4;     % Maximum steering angle
nlobj.MV(2).Min = -10;       % Minimum velocity (reverse)
nlobj.MV(2).Max =  10;       % Maximum velocity (forward)

Задайте жесткие границы четвертого состояния, который является углом между грузовиком и прицепом. Он не может выйти за пределы +/- |90| степеней из-за механических ограничений.

nlobj.States(4).Min = -pi/2;
nlobj.States(4).Max = pi/2;

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

Поскольку MV действительны только от этапа 1 до этапа pВы не можете определить какие-либо затраты на этап для последнего этапа, p+1. Пять настроек модели M, L1, L2, W1, и W являются параметрами каскада для каскадов 1- p и используются функциями ограничения неравенства.

for ct=1:p
    nlobj.Stages(ct).CostFcn = "TruckTrailerCost";
    nlobj.Stages(ct).CostJacFcn = "TruckTrailerCostGradientFcn";
    nlobj.Stages(ct).ParameterLength = 5;
end

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

В теории следует проверить такие столкновения на все шаги предсказания (от этапа 2 до этапа p+1). Однако в этом примере необходимо проверить этапы от 2 до p потому что последний этап уже обеспечивается функцией ограничения равенства.

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

for ct=2:p
    nlobj.Stages(ct).IneqConFcn = "TruckTrailerIneqConFcn";
end

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

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

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

Любая функция состояния и параметры каскада, если они определены, должны быть предоставлены контроллеру во время исполнения. StageParameter содержит все параметры каскада, сложенные в один вектор. Мы также используем TerminalState чтобы задать конечное состояние во время выполнения.

simdata = getSimulationData(nlobj,'TerminalState');
simdata.StateFcnParameter = [params.M;params.L1;params.L2];
simdata.StageParameter = repmat([params.M;params.L1;params.L2;params.W1;params.W2],p,1);
simdata.TerminalState = targetPose;
validateFcns(nlobj,[2;3;0.5;0.4],[0.1;0.2],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 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] 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] are OK.
"IneqConFcn" of the following stages [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] are OK.
Analysis of user-provided model, cost, and constraint functions complete.

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

В этом примере существует 244 переменные принятия решений: Состояния объекта и входы (всего 6) для каждого из первых этапов p (40) и состояния объекта (4) для последнего этапа p+1. The TruckTrailerInitialGuess функция использует простую эвристику, чтобы сгенерировать начальное предположение. Например, если грузовик и трейлер первоначально расположены над препятствиями, используется одна путевая точка, в противном случае используются две путевые точки (путевая точка является промежуточной точкой на маршруте движения, в котором изменяется курс). Начальное предположение отображается как точки на следующем анимационном графике.

[simdata.InitialGuess, XY0] = TruckTrailerInitialGuess(initialPose,targetPose,u0,p);

Результат симуляции траектории

Используйте nlmpcmove функция для нахождения оптимального пути парковки, который обычно занимает от десяти до двадцати секунд, в зависимости от начального положения.

fprintf('Automated Parking Planner is runing...\n');
tic;[~,~,info] = nlmpcmove(nlobj,initialPose,u0,simdata);t=toc;
fprintf('Calculation Time = %s; Objective cost = %s; ExitFlag = %s; Iterations = %s\n',...
    num2str(t),num2str(info.Cost),num2str(info.ExitFlag),num2str(info.Iterations));

Automated Parking Planner is runing...
Calculation Time = 9.4315; Objective cost = 251.8455; ExitFlag = 1; Iterations = 47

Сгенерированы два графика. Один из них - анимация процесса парковки, где синие круги указывают оптимальный путь, а начальное предположение показано как точка. Другой отображает оптимальную траекторию состояний объекта и движений управления.

TruckTrailerPlot(initialPose, targetPose, params, info, XY0);

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

Можно попробовать другие начальные положения X-Y в разделе Автоматическая задача парковки, пока позиции действительны.

Если ExitFlag отрицательно, это означает, что нелинейный контроллер MPC не смог найти оптимальное решение, и вы не можете доверять возвращенной траектории. В этом случае вам может потребоваться предоставить лучшее начальное предположение и указать его в simdata.InitialGuess перед вызовом nlmpcmove.