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

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

Скрипт Open Live Script

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

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

  1. Исследуйте городской сценарий с предопределенными транспортными средствами.

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

Содержимое

Введение

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

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

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

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

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

  • Сначала (К B), транспортное средство демонстрирует поведение Адаптивного круиз-контроля (ACC).

  • Второй (B к C), транспортное средство согласовывает поворот следовать глобальному плану.

  • Треть (C к D), транспортное средство выполняет маневр изменения маршрута.

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

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

  1. otherVehicles: [1 x 4] Массив структур, содержащий поля: Position, Yaw, Velocity, и SimulationTime, из каждого транспортного средства в сценарии.

  2. globalPlanPoints: [18 x 2] Матрица содержат предварительно вычисленный глобальный план, состоящий из восемнадцати waypoints, каждый представляющий положение в сценарии.

  3. stateValidator: validatorOccupancyMap возразите, что действия как блок проверки допустимости состояния на основе данной 2D власти сопоставляют. Полностью занятая эгоцентрическая карта заполнения обновляется на основе информации о препятствии и дорожных контуров. Пользовательский блок проверки допустимости состояния может также использоваться на основе приложения. Для получения дополнительной информации смотрите nav.StateValidator.

Постройте сценарий.

exampleHelperPlotUrbanScenario;

Figure contains an axes object and an object of type uipanel. The axes object contains 16 objects of type line, patch.

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

Укажите, что блок проверки допустимости состояния и глобальная переменная планируют создать локального планировщика, использующего 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 : Максимальное выполнимое значение для искривления в m^-1

  • MaxAcceleration: 2.5: Максимальное выполнимое ускорение в м/с^2.

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

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

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

Чтобы продемонстрировать ACC, автомобиль, оборудованный датчиком должен следовать за ведущим транспортным средством путем поддержания безопасного расстояния. Ведущее транспортное средство в этом сегменте выбирается using 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 и задайте его начальную скорость и ориентацию (Рыскание).

% 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 как below:

Чтобы обеспечить безопасное расстояние от ведущего транспортного средства, установите localPlanner.TerminalStates.Longitudinal к расстоянию, чтобы привести транспортное средство - длина транспортного средства;

Чтобы обеспечить относительную скорость относительно ведущего транспортного средства, установите localPlanner.TerminalStates.Speed привести скорость транспортного средства;

Продолжать перейти на глобальном плане, set 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

Figure contains 2 axes objects and other objects of type uipanel. Axes object 1 with title Ego Centric Binary Occupancy Map contains 5 objects of type image, patch, line. These objects represent Waypoints, Reference Path, Optimal Trajectory. Axes object 2 with title Trajectory Execution in Scenario contains 16 objects of type line, patch.

Figure contains 2 axes objects and other objects of type uipanel. Axes object 1 with title Ego Centric Binary Occupancy Map contains 5 objects of type image, patch, line. These objects represent Waypoints, Reference Path, Optimal Trajectory. Axes object 2 with title Trajectory Execution in Scenario contains 16 objects of type line, patch.

В конце этого выполнения автомобиль, оборудованный датчиком в положении 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

Figure contains 2 axes objects and other objects of type uipanel. Axes object 1 with title Ego Centric Binary Occupancy Map contains 5 objects of type image, patch, line. These objects represent Waypoints, Reference Path, Optimal Trajectory. Axes object 2 with title Trajectory Execution in Scenario contains 16 objects of type line, patch.

В конце этого выполнения автомобиль, оборудованный датчиком в положении 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

Figure contains 2 axes objects and other objects of type uipanel. Axes object 1 with title Ego Centric Binary Occupancy Map contains 5 objects of type image, patch, line. These objects represent Waypoints, Reference Path, Optimal Trajectory. Axes object 2 with title Trajectory Execution in Scenario contains 16 objects of type line, patch.

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

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

Figure contains 2 axes objects and other objects of type uipanel. Axes object 1 with title Ego Centric Binary Occupancy Map contains 5 objects of type image, patch, line. These objects represent Waypoints, Reference Path, Optimal Trajectory. Axes object 2 with title Trajectory Execution in Scenario contains 16 objects of type line, patch.

Регистрируйте положения автомобиля, оборудованного датчиком в 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 понять следующее поведение.

  • Адаптивный круиз-контроль

  • Согласование поворотов

  • Маневр изменения маршрута.

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

Поддержка

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