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

Этот пример использует:

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

В этом примере вы будете:

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

Содержание

Введение

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

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

Исследуйте городской сценарий для локального планирования

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

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

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

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

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

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

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

exampleHelperPlotUrbanScenario;

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

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

localPlanner = trajectoryOptimalFrenet(globalPlanPoints,stateValidator);

Исследуйте свойства `localPlanner`

The 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 для демонстрации поведения Adaptive Cruise Control (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;

Установите автомобиль , оборудованный датчиком в положение A и задайте его начальную скорость и ориентацию (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 для согласования поворота во втором сегменте из положения B в положение C

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

Свойства текущего набора localPlanner достаточны для согласования плавного поворота. Однако во втором сегменте головное транспортное средство выбирается из otherVehicles (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 для выполнения маневра изменения маршрута из положения C в положение 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

Симулируйте автомобиль , оборудованный датчиком выполнение, чтобы достичь целевой точки

The 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 переменная, доступная в базовом рабочем пространстве. Вы можете воспроизвести путь транспортного средства в DrivingScenario с помощью 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` для выполнения маневра изменения маршрута

Симулируйте автомобиль , оборудованный датчиком выполнение, чтобы достичь целевой точки

Заключение

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

Поддержка

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

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

Содержание

Введение

Исследуйте городской сценарий для локального планирования

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

Исследуйте свойства localPlanner

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

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

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

Симулируйте автомобиль , оборудованный датчиком выполнение, чтобы достичь целевой точки

Заключение

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

Поддержка

Исследуйте свойства `localPlanner`

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

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

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