Обзор

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

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

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

Грузовик и одна система трейлера

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

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

Входные параметры для этой модели:

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

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

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 находит траекторию переменной manupulated (MV) (со времени k к k+p-1 оптимизировать суммированные затраты этапа (со времени k к k+p), удовлетворяя всем ограничениям этапа (со времени k к k+p).

В этом примере объект имеет четыре состояния и два входных параметров (оба MVS). Горизонт предсказания 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;

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

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

[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 в разделе Automatic Parking Problem, пока положения допустимы.

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