Обзор

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

u0 = zeros(2,1);

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

В этом примере объект имеет четыре состояния и два входных параметров (оба 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, вы не должны задавать этап, стоивший за последнюю стадию, 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 running...\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 running...
Calculation Time = 21.8251; Objective cost = 251.8455; ExitFlag = 1; Iterations = 48

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

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

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

Можно попробовать другие начальные положения X-Y в разделе Automatic Parking Problem путем изменения первых двух параметров initialPose, пока положения допустимы.

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