Введение в Kino-динамическое планирование

Описание планировщика

Обычные геометрические специалисты по планированию, такие как RRT, RRT*, и hybridA*, являются быстрыми и расширяемыми алгоритмами, способными к нахождению полных и оптимальных решений большого разнообразия планирования проблем. Один компромисс, однако, то, что они делают предположения о пробеле планирования, который не может сохраняться для реальной системы. Обычные геометрические специалисты по планированию принимают, что любые два состояния на пробеле могут быть соединены траекторией без остаточной ошибки, и что пути, возвращенные геометрическим специалистом по планированию, могут быть прослежены физической системой.

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

Запланировать данную систему, the plannerControlRRT функция запрашивает эту информацию:

Этот пример демонстрирует, как сформулировать эти различные утилиты для системы тягача и адаптировать их в инфраструктуру планирования. Этот предварительный просмотр показывает результат:

% Attempt to generate MEX to accelerate collision checking
genFastCC

Code generation successful.

% load preplannedScenarios
parkingScenario = load("parkingScenario.mat");
planner = parkingScenario.planner;
start   = parkingScenario.start;
goal    = parkingScenario.goal;

% Display the scenario and play back the trajectory
show(planner.StatePropagator.StateValidator);
axis equal 
hold on
trajectory = plan(planner,start,goal);
demoParams = trajectory.StatePropagator.StateSpace.TruckParams;
exampleHelperPlotTruck(demoParams,start); % Display start config
exampleHelperPlotTruck(demoParams,goal);  % Display goal config

% Plan path using previously-built utilities
rng(1)
trajectory = plan(planner,start,goal);

% Animate the trajectory
exampleHelperPlotTruck(trajectory);
hold off

Kino-динамический алгоритм планирования

Если вы уже знакомы с геометрическими специалистами по планированию, итерацией plannerControlRRT алгоритм поиска должен выглядеть знакомым, но с несколькими дополнительными шагами:

Компоненты, необходимые plannerControlRRT Среда

Чтобы адаптировать планировщика к определенной проблеме, необходимо реализовать пользовательский nav.StatePropagator и nav.StateSpace классы. Раздел ниже сопоставляет шаги, описанные в разделе Kino-Dynamic Planning Algorithm к классу и ответственному методу:

Для более подробного синтаксиса функций требования аргумента и примеры, видят plannerControlRRT, nav.StateSpace, и nav.StatePropagator.

Адаптация plannerControlRRT к системе трейлера грузовика

Этот раздел демонстрирует, как сформулировать, смоделировать, и управлять системой трейлера грузовика 2D тела. Эта система по сути нестабильна во время противоположного движения, таким образом, пути, возвращенные обычными геометрическими специалистами по планированию, вряд ли приведут к хорошим результатам во время следования траектории. Чтобы точно смоделировать эту систему, вы должны:

Задайте функцию изменения состояния

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

Запаздывающая Настройка (оставленный, M <0), Настройка OverCenter (право, M> 0)

Используйте геометрическую настройку для системы трейлера грузовика 2D тела от Грузовика и Трейлера Автоматическая Парковка Используя Многоступенчатый Нелинейный пример MPC, описанный как:

$${\mathit{q}}_{\mathrm{sys}}=\left[{\mathit{x}}_2{\mathit{y}}_2{\theta }_2\beta \right]$$,

где:

Для планирования целей добавьте флаг направления, ${\mathit{v}}_{\mathrm{sign}}$, и общее расстояние переместилось из настройки запуска, ${\mathit{s}}_{\mathrm{tot}}$ к вектору состояния, для обозначения конечного состояния:

Эта функция изменения состояния является расширением TruckTrailerStateFcn. Для полной деривации функции изменения состояния смотрите exampleHelperStateDerivative, который является упрощением этой модели [1] N-трейлера:

Для простоты, эта модель обработки ${\mathit{u}}_{\mathrm{app}}=\left[\mathit{v},{\alpha }_{\mathrm{steer}}\right]$ когда мгновенная скорость и держащаяся угловая команда применились к системе, сгенерированной высокоуровневым контроллером. Для получения дополнительной информации смотрите, Создают Законы о надзоре для Устойчивого Прямого и Противоположного Движения.

Задайте геометрические параметры модели и продемонстрируйте нестабильность

Кинематические характеристики этого транспортного средства в большой степени под влиянием выбора параметров модели. Создайте структуру, которая описывает систему тягача,

truckParams = struct;
truckParams.L1 = 6;     % Truck body length
truckParams.L2 = 10;    % Trailer length
truckParams.M  = 1;     % Distance between hitch and truck rear axle along truck body +X-axis

Задайте свойства, связанные с визуализацией и проверкой столкновения.

truckParams.W1 = 2.5;               % Truck width
truckParams.W2 = 2.5;               % Trailer width
truckParams.Lwheel = 1;             % Wheel length
truckParams.Wwheel = 0.4;           % Wheel width

Используя exampleHelperShowOpenLoopDynamics функционируйте и заданные параметры модели, распространите системное состояние использование его динамики разомкнутого контура, чтобы продемонстрировать, что внутренний угол автостабилизируется во время движения вперед, и нестабильный во время противоположного движения для любого ненулевого ${\beta }_0$.

exampleHelperShowOpenLoopDynamics(truckParams);

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

Этот пример нанимает многоуровневый контроллер переключения режимов, чтобы управлять моделью трейлера грузовика. В верхнем уровне чистый контроллер преследования вычисляет контрольную точку, $\mathit{P}=[{\mathit{X}}_{\mathit{p}},{\mathit{Y}}_{\mathit{p}}]$, между текущим положением, ${\mathit{q}}_{\mathit{i}}$, и целевое состояние, ${\mathit{q}}_{\mathrm{tgt}}$. У контроллера есть два режима для форварда и реверса. Для получения информации о базисе для этого закона о надзоре см. [1]. Для движения вперед контроллер вычисляет держащийся угол, который, когда сохранено постоянный, управляет задней осью грузовика вдоль дуги, которая пересекает контрольную точку:

Для противоположного движения задняя ось трейлера становится управляемым состоянием, ${\mathit{q}}_2=\left[{\mathit{x}}_2,{\mathit{y}}_2,{\theta }_2\right]$. Кроме того, потому что система по сути нестабильна во время противоположного движения, закон о надзоре обрабатывает держащийся угол, возвращенный контроллером верхнего уровня как ссылочное значение запланированному на усиление контроллеру LQ, который стремится стабилизировать внутренний угол транспортного средства:

Стабилизация обратной связи LQ

Во время противоположного движения можно добавить усиление в держащийся угол, который управляет внутренним углом к точке равновесия. Используйте контроллер LQ, чтобы вычислить желаемый руководящий угол и усиления обратной связи. Усиления являются оптимальным решением Алгебраического уравнения Рикатти, сохраненного как интерполяционная таблица, зависящая от желаемого руководящего угла. Для получения дополнительной информации о деривации этого контроллера обратной связи, смотрите exampleHelperCalculateLQGains:

% Define Q/R weight matrices for LQR controller
Q = 10; % Weight driving betaDot -> 0
R = 1; % Weight minimizing steering angle command

% Derive geometric steering limits and solve for LQR feedback gains
[alphaLimit, ...        % Max steady-state steering angle before jack-knife
 betaLimit, ...         % Max interior angle before jack-knife
 alphaDesiredEq, ...    % Sampled angles from stable alpha domain
 alphaGain ...          % LQ gain corresponding to desired alpha
] = exampleHelperCalculateLQGains(truckParams,Q,R);

% Add limits to the truck geometry
truckParams.AlphaLimit = alphaLimit;
truckParams.BetaLimit = betaLimit;

Задайте предварительные расстояния для чистого контроллера преследования, exampleHelperPurePursuitGetSteering, и уровень и информация о промежутке для нашего средства моделирования модели, exampleHelperPropagateTruck. Из-за нестабильности во время противоположного движения, и поскольку вы только имеете косвенный контроль над внутренним углом, задают большее предварительное расстояние наоборот. Это дает системе больше времени, чтобы минимизировать виртуальную руководящую ошибку при поддержании устойчивого внутреннего угла.

% Select forward and reverse lookahead distances. For this example,
% use a reverse lookahead distance twice as long as the forward
% lookahead distance, which itself is slightly longer than the 
% wheelbase. You can tune these parameters can be tuned for
% improved performance for a given geometry.
rFWD = truckParams.L1*1.2;
rREV = rFWD*2;

% Define parameters for the fixed-rate propagator and add them to the
% control structure
controlParams.MaxVelocity = 3; % m/s
controlParams.StepSize = .1; % s
controlParams.MaxNumSteps = 50; %#ok<STRNU> % Max number of steps per propagation

Создайте структуру связанной с управлением информации.

% Store gains and control parameters in a struct
controlParams = struct(...
    'MaxSteer',alphaLimit, ...          % (rad)
    'Gains',alphaGain, ...              % ()
    'AlphaPoints',alphaDesiredEq, ...   % (rad)
    'ForwardLookahead',rFWD, ...        % (m)
    'ReverseLookahead',rREV, ...        % (m)
    'MaxVelocity', 3, ...               % (m/s)
    'StepSize', 0.1, ...                % (s)
    'MaxNumSteps', 200 ...               % ()
    );

Задайте эвристику расстояния

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

distanceHeuristic = exampleHelperCalculateDistanceMetric(truckParams,controlParams);

Адаптируйте систему к использованию с кинематическим планировщиком

Адаптируйте систему в среду, применимую кинематическим планировщиком пути, plannerControlRRT. Создайте 3 пользовательских класса: exampleHelperTruckStateSpace, exampleHelperTruckPropagator, и exampleHelperTruckValidator, которые наследовались nav.StateSpace, nav.StatePropagator, и nav.StateValidator подклассы, соответственно.

Инициализируйте пространство состояний

Объект пространства состояний, в основном, ответственен за:

Задайте пределы xy-для доступной для поиска области. Инициализируйте пространство состояний с помощью этих пределов и геометрических свойств транспортного средства прицепа для грузовика, содержащего $\beta$- пределы, определенные геометрическими свойствами грузовика и трейлера.

Поскольку кинематический планировщик использует distance функция распространителя состояния для ее поиска ближайшего соседа, оставьте пространство состояний distance неопределенный метод. Оставьте interpolate и sampleGaussian методы столь же неопределенного пространства состояний.

% Define limits of searchable region
xyLimits = [-60 60; -40 40];

% Construct our state-space
stateSpace = exampleHelperTruckStateSpace(xyLimits, truckParams);

Инициализируйте блок проверки допустимости состояния

В этом примере блок проверки допустимости состояния использует ориентированные ограничительные рамки (OBBs), чтобы проверить, является ли транспортное средство в столкновении со средой. Грузовик представлен двумя прямоугольниками с положениями, заданными состоянием и геометрией транспортного средства грузовика, содержавшегося в объекте пространства состояний. Среда представлена списком прямоугольников с их местоположениями, ориентациями и размерностями, заданными в структуре 1 на n. Загрузите файл MAT, содержащий среду в рабочую область, и используйте препятствия и объект пространства состояний создать блок проверки допустимости состояния.

% Load a set of obstacles
load("Slalom.mat");

% Construct the state validator using the state space and list of obstacles
validator = exampleHelperTruckValidator(stateSpace,obstacles);

Инициализируйте распространителя состояния

В кинематическом планировании объект распространителя состояния ответственен за:

Для лучших результатов, distance функция должна оценить стоимость траектории, сгенерированной при распространении между состояниями.

% Construct the state propagator using the state validator, control parameters,
% and distance lookup table
propagator = exampleHelperTruckPropagator(validator,controlParams,distanceHeuristic);

Создайте путь планировщика и плана

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

% Define start configuration
start = [35 5 pi/2 0 0 0];

% Define the goal configuration such that the truck must reverse into
% position.
goal  = [-35 20 -pi/2 0 -1 nan];

% Display the problem
figure
show(validator)
hold on
configs = [start; goal];
quiver(configs(:,1),configs(:,2),cos(configs(:,3)),sin(configs(:,3)),.1)

% Define a function to check if planner has reached the true goal state
goalFcn = @(planner,q,qTgt)exampleHelperGoalReachedFunction(goal,planner,q,qTgt);

% Construct planner
planner = plannerControlRRT(propagator,GoalReachedFcn=goalFcn,MaxNumIteration=30000);
planner.NumGoalExtension = 3;
planner.MaxNumTreeNode = 30000;
planner.GoalBias = .25;

% Search for the path
rng(0)
[trajectory,treeInfo] = plan(planner,start,goal)

trajectory = 
  navPathControl with properties:

    StatePropagator: [1x1 exampleHelperTruckPropagator]
             States: [23x6 double]
           Controls: [22x4 double]
          Durations: [22x1 double]
       TargetStates: [22x6 double]
          NumStates: 23
        NumSegments: 22

treeInfo = struct with fields:
     IsPathFound: 1
        ExitFlag: 1
     NumTreeNode: 2201
    NumIteration: 1196
    PlanningTime: 17.1705
        TreeInfo: [6x6602 double]

% Visualize path and waypoints
exampleHelperPlotTruck(trajectory);
hold off

Вместо того, чтобы производить оптимальные решения, как сгенерированные в Грузовике и Трейлере Автоматическая Парковка Используя Многоступенчатый Нелинейный пример MPC, преимущество этого планировщика, вместо этого, заключается в своей способности найти жизнеспособные траектории для большого разнообразия проблем. В сценарии ниже, например, сгенерированный путь может быть использован как есть, или как исходное предположение для решателя MPC, помогая избежать локальных минимумов в невыпуклом пространстве задач:

% Load scenario obstacles
nonConvexProblem = load("NonConvex.mat");

% Update validator and state space
validator.Obstacles = nonConvexProblem.obstacles;
validator.StateSpace.StateBounds(1:2,:) = [-100 100; -40 40];
figure
show(validator)

% Define start and goal
start = [10 0 0 0 NaN 0];
goal  = [-10 0 pi 0 -1 0];

% Update goal reached function
goalFcn = @(planner,q,qTgt)exampleHelperGoalReachedFunction(goal,planner,q,qTgt);
planner.GoalReachedFcn = goalFcn;

% Turn off optional every-step goal propagation
planner.NumGoalExtension = 0;

% Increase goal sampling frequency
planner.GoalBias = .25;

% Balanced search vs path optimality
propagator.TravelBias = .5;

% Plan path
rng(0)
trajectory = plan(planner,start,goal);

% Visualize result
exampleHelperPlotTruck(trajectory);

Ссылки

[1] Holmer, Олов. “Движение, Планирующее Инвертирующую Полномасштабную Систему Грузовика и Трейлера”. Тезис M.S., Университет Linköping, 2016.