Обзор

Контроллер MPC имеет внутреннюю модель, которая может использоваться для прогнозирования поведения установки. Учитывая текущее состояние установки, основываясь на модели прогнозирования, 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 object), многоступенчатый нелинейный 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;

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

Поскольку 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. 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.