Оптимальная генерация траектории для городского вождения

В этом примере используются:

В этом примере показано, как выполнять динамическое повторное планирование в городском сценарии с использованием trajectoryOptimalFrenet.

В этом примере будут выполнены следующие действия:

Ознакомьтесь с городским сценарием с предварительно определенными транспортными средствами.
Использовать trajectoryOptimalFrenet для выполнения локального планирования для навигации по сценарию Urban.

Содержание

Введение

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

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

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

Этот пример сценария имеет четыре других транспортных средства (синие прямоугольники), которые движутся по заранее определенным траекториям с различными скоростями. На рисунке ниже показан этот сценарий и глобальный план (сплошная красная линия), используемый в этом примере. Сплошные красные точки на рисунке ниже представляют ППМ глобального плана между начальной и конечной позициями. Зеленый прямоугольник представляет эго-транспортное средство.

Транспортное средство ego использует trajectoryOptimalFrenet для перемещения из положения A в положение D в трех сегментах с тремя различными параметрами конфигурации.

Во-первых (от A до B), транспортное средство демонстрирует поведение адаптивного круиз-контроля (ACC).
Во-вторых (B-C), транспортное средство договаривается о повороте, чтобы следовать глобальному плану.
В-третьих (C-D) транспортное средство выполняет маневр изменения полосы движения.

Настройте требуемые данные и переменные среды:

% Load data from urbanScenarioData.mat file, initialize required variables
[otherVehicles,globalPlanPoints,stateValidator] = exampleHelperUrbanSetup;

другие транспортные средства: [1 x 4] Структурный массив, содержащий поля: Position, Yaw, Velocity, и SimulationTime, каждого транспортного средства в сценарии.
globalPlanPoints: [18 x 2] Матрица содержит предварительно рассчитанный глобальный план, состоящий из восемнадцати ППМ, каждый из которых представляет позицию в сценарии.
stateValidator: validatorOccupancyMap объект, который действует как средство проверки состояния на основе заданной карты ручек 2-D. Полностью занятая эгоцентрическая карта занятости обновляется на основе информации о препятствиях и границах дорог. На основе приложения можно также использовать средство проверки пользовательского состояния. Дополнительные сведения см. в разделе nav.StateValidator.

Постройте график сценария.

exampleHelperPlotUrbanScenario;

Создание локального плановика

Укажите средство проверки состояния и глобальный план для создания локального плановика с помощью trajectoryOptimalFrenet.

localPlanner = trajectoryOptimalFrenet(globalPlanPoints,stateValidator);

Изучение свойств `localPlanner`

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

localPlanner.TerminalStates

Longitudinal: [30 45 60 75 90]: Определяет продольное расстояние отбора проб в метрах. Это значение может быть скаляром или вектором.
Lateral: [-2 -1 0 1 2]: Определяет боковое отклонение в метрах от опорной траектории (в нашем случае глобальный план).
Time: 7: Время в секундах для достижения конца траектории.
Speed: 10: Скорость в метрах в секунду, достигаемая в конце траектории
Acceleration: 0: Ускорение в конце траектории в м/с ^ 2.

localPlanner.FeasibilityParameters

MaxCurvature: 0.1 : Максимально допустимое значение кривизны в м ^ -1
MaxAcceleration: 2.5: Максимально возможное ускорение в м/с ^ 2.

localPlanner.TimeResolution: 0.1: Интервал дискретизации траектории в секундах

Использовать `trajectoryOptimalFrenet` демонстрация поведения адаптивного круиз-контроля (ACC)

В этом разделе присвойте свойства, необходимые для настройки localPlanner для демонстрации поведения адаптивного круиз-контроля (ACC).

Чтобы продемонстрировать ACC, эго-транспортное средство должно следовать за ведущим транспортным средством, поддерживая безопасное расстояние. Ведущее транспортное средство в этом сегменте выбирается с помощью otherVehicles(1).

% Get leadVehicle in segment from Postion A to Position B
leadVehicle = otherVehicles(1);

% Define ACC safe distance
ACCSafeDistance = 35; % in meters
% Adjusting the time resolution of planner object to make the ego vehicle
% travel smoothly
timeResolution = 0.01;
localPlanner.TimeResolution = timeResolution;

Установите эго-транспортное средство в положение А и определите его начальную скорость и ориентацию (Yaw).

% Set positions A, B, C and D
positionA =  [5.1, -1.8, 0];
positionB =  [60, -1.8, 0];
positionC =  [74.45, -30.0, 0];
positionD =  [74.45, -135, 0];
goalPoint =  [145.70, -151.8, 0];

% Set the initial state of the ego vehicle
egoInitPose     = positionA;
egoInitVelocity = [10, -0.3, 0];
egoInitYaw      = -0.165;
currentEgoState = [egoInitPose(1), egoInitPose(2), deg2rad(egoInitYaw),...
    0, norm(egoInitVelocity), 0];
vehicleLength = 4.7; % in meters
% Replan interval in number of simulation steps
% (default 50 simulation steps)
replanInterval = 50;

Визуализация результатов моделирования.

% Initialize Visualization
exampleHelperInitializeVisualization;

Поведение ACC достигается путем установки TerminalStates из localPlanner как указано ниже:

Для поддержания безопасного расстояния от ведущего транспортного средства, комплект localPlanner.TerminalStates.Longitudinal расстояние до ведущего транспортного средства - длина транспортного средства;

Для поддержания относительной скорости по отношению к ведущему транспортному средству установите localPlanner.TerminalStates.Speed для повышения скорости транспортного средства;

Чтобы продолжить навигацию по глобальному плану, установите localPlanner.TerminalStates.Lateral в 0;

В следующем фрагменте кода: localPlanner производит trajectory которая выполняется и визуализируется с помощью exampleHelperUpdateVisualization для каждого этапа моделирования. Однако повторное планирование выполняется на каждом этапе моделирования replanInterval. Ниже приведена последовательность событий во время повторного планирования:

Обновление карты заполняемости с использованием информации о транспортном средстве с помощью exampleHelperUpdateOccupancyMap.
Обновить localPlanner.TerminalStates.
Формирование траектории с помощью plan(localPlanner,currentStateInFrenet).

% Simulate till the ego vehicle reaches position B
simStep = 1;
% Check only for X as there is no change in Y.
while currentEgoState(1) <= positionB(1)
    
    % Replan at every "replanInterval"th simulation step
    if rem(simStep, replanInterval) == 0 || simStep == 1
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles,simStep,currentEgoState);
        
        % Compute distance to Lead Vehicle and leadVehicleVelocity
        distanceToLeadVehicle = pdist2(leadVehicle.Position(simStep,1:2), ...
            currentEgoState(1:2));
        leadVehicleVelocity = leadVehicle.Velocity(simStep,:);
        
        % Set localPlanner.TerminalStates for ACC behavior
        if distanceToLeadVehicle <= ACCSafeDistance
            localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength;
            localPlanner.TerminalStates.Speed = norm(leadVehicleVelocity);
            localPlanner.TerminalStates.Lateral = 0;
            desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
                localPlanner.TerminalStates.Speed;
            localPlanner.TerminalStates.Time = desiredTimeBound;
            localPlanner.FeasibilityParameters.MaxCurvature = 0.5;
        end
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner, currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1);
        hAxes1.Title.String = "Ego Centric Binary Occupancy Map";
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner,"Trajectory","optimal","Parent",hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    if(isGoalReached)
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

В конце этого выполнения эго-транспортное средство находится в положении B.

Далее сконфигурируйте trajectoryOptimalFrenet для согласования поворота во втором сегменте из положения В в положение С.

Использовать `trajectoryOptimalFrenet` согласование плавного поворота

Свойства текущего набора localPlanner достаточно для того, чтобы договориться о плавном повороте. Однако во втором сегменте ведущее транспортное средство извлекается из других транспортных средств.(2).

% Set Lead Vehicle to correspond to the vehicle in second segment
% from position B to position C
leadVehicle = otherVehicles(2);

% Simulate till the ego vehicle reaches position C
% Check only for Y as there is no change in X at C
while currentEgoState(2) >= positionC(2)
    
    % Replan at every "replanInterval"th simulation step
    if rem(simStep, replanInterval) == 0
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState);
        
        % Compute distance to Lead Vehicle and leadVehicleVelocity
        distanceToLeadVehicle = pdist2(leadVehicle.Position(simStep,1:2), ...
            currentEgoState(1:2));
        leadVehicleVelocity = leadVehicle.Velocity(simStep,:);
        
        if(distanceToLeadVehicle <= ACCSafeDistance)
            localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength;
            localPlanner.TerminalStates.Speed = norm(leadVehicleVelocity);
            localPlanner.TerminalStates.Lateral = 0;
            desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
                localPlanner.TerminalStates.Speed;
            localPlanner.TerminalStates.Time = desiredTimeBound;
            localPlanner.FeasibilityParameters.MaxCurvature = 0.5;
            localPlanner.FeasibilityParameters.MaxAcceleration = 5;
        end
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner, currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1);
        hAxes1.Title.String = "Ego Centric Binary Occupancy Map";
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :), egoPolygon(2, :), "g", "Parent", hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner, "Trajectory", "optimal", "Parent", hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    if(isGoalReached)
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

В конце этого выполнения эго-транспортное средство находится в положении C.

Далее сконфигурируйте trajectoryOptimalFrenet для выполнения маневра смены полосы движения из положения С в положение D.

Использовать `trajectoryOptimalFrenet` для выполнения маневра изменения полосы движения

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

% Simulate till the ego vehicle reaches position D
% Set Lane Width
laneWidth = 3.6;
% Check only for Y as there is no change in X at D
while currentEgoState(2) >= positionD(2)
    
    % Replan at every "replanInterval" simulation step
    if rem(simStep, replanInterval) == 0
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles,simStep,currentEgoState);
        
        % TerminalState settings for negotiating Lane change
        localPlanner.TerminalStates.Longitudinal = 20:5:40;
        localPlanner.TerminalStates.Lateral = laneWidth;
        localPlanner.TerminalStates.Speed = 10;
        desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
            ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2);
        localPlanner.TerminalStates.Time = desiredTimeBound;
        localPlanner.FeasibilityParameters.MaxCurvature = 0.5;
        localPlanner.FeasibilityParameters.MaxAcceleration = 15;
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner,[currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner,currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1);
        hAxes1.Title.String = "Ego Centric Binary Occupancy Map";
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner,"Trajectory","optimal","Parent",hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    if(isGoalReached)
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

Моделирование выполнения ego-автомобиля для достижения цели

localPlanner теперь сконфигурирован для перемещения из положения D в положение цели.

% Simulate till the ego vehicle reaches Goal position
% Check only for X as there is no change in Y at Goal.
while currentEgoState(1) <= goalPoint(1)
    
    % Replan at every "replanInterval"th simulation step
    if rem(simStep, replanInterval) == 0
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState);
        localPlanner.TerminalStates.Longitudinal = 20;
        localPlanner.TerminalStates.Lateral = [-1 0 1];
        desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
            ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2);
        localPlanner.TerminalStates.Time = desiredTimeBound:0.2:desiredTimeBound+1;
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner, [currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner, currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1);
        hAxes1.Title.String = "Ego Centric Binary Occupancy Map";
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner,"Trajectory","optimal","Parent",hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    % Goal reached will be true only in this section.
    if(isGoalReached)        
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

Регистрировать позиции эго-транспортного средства в egoPoses переменная, доступная в базовой рабочей области. Можно воспроизвести путь к транспортному средству в CounterScription с помощью exampleHelperPlayBackInDS(egoPoses).

% Clear workspace variables that were created during the example run.
% This excludes egoPoses to allow the user to playback the simulation in DS
exampleHelperUrbanCleanUp;

Заключение

В этом примере объясняется, как выполнять динамическое повторное планирование в городском сценарии с использованием trajectoryOptimalFrenet. В частности, мы научились пользоваться trajectoryOptimalFrenet для реализации следующего поведения.

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

Документация

Оптимальная генерация траектории для городского вождения

Содержание

Введение

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

Создание локального плановика

Изучение свойств `localPlanner`

Использовать `trajectoryOptimalFrenet` демонстрация поведения адаптивного круиз-контроля (ACC)

Использовать `trajectoryOptimalFrenet` согласование плавного поворота

Использовать `trajectoryOptimalFrenet` для выполнения маневра изменения полосы движения

Моделирование выполнения ego-автомобиля для достижения цели

Заключение

Документация по панели инструментов навигации

Поддержка

Документация

Оптимальная генерация траектории для городского вождения

Содержание

Введение

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

Создание локального плановика

Изучение свойств localPlanner

Использовать trajectoryOptimalFrenet демонстрация поведения адаптивного круиз-контроля (ACC)

Использовать trajectoryOptimalFrenet согласование плавного поворота

Использовать trajectoryOptimalFrenet для выполнения маневра изменения полосы движения

Моделирование выполнения ego-автомобиля для достижения цели

Заключение

Документация по панели инструментов навигации

Поддержка

Изучение свойств `localPlanner`

Использовать `trajectoryOptimalFrenet` демонстрация поведения адаптивного круиз-контроля (ACC)

Использовать `trajectoryOptimalFrenet` согласование плавного поворота

Использовать `trajectoryOptimalFrenet` для выполнения маневра изменения полосы движения