В этом примере показано, как создать автоматизированную систему камердинера парковки. В этом примере вы узнаете об инструментах и методах в поддержку планирования пути, генерации траектории и управления транспортным средством. В то время как этот пример фокусируется на рабочем процессе MATLAB®-oriented, эти инструменты также доступны в Simulink®. Для версии Simulink® этого примера смотрите Автоматизированного Камердинера Парковки в Simulink.
Автоматически парковка автомобиля, который оставляют перед парковкой, является сложной проблемой. Автоматизированные системы транспортного средства, как ожидают, будут принимать управление и вести транспортное средство на доступное место для парковки. Такая функция использует несколько встроенных датчиков. Например:
Передняя сторона и камеры стороны для обнаружения маркировок маршрута, дорожные знаки (знаки Стоп, выходные маркировки, и т.д.), другие транспортные средства и пешеходы
Лидар и датчики ультразвука для обнаружения препятствий и вычисления точных измерений расстояния
Датчики ультразвука для обнаружения препятствия
IMU и энкодеры колеса для точного расчета
Встроенные датчики используются, чтобы чувствовать среду вокруг транспортного средства. Воспринятая среда включает понимание дорожных разметок, чтобы интерпретировать дорожные правила и вывести drivable области, распознавание препятствий и обнаружение доступных мест для парковки.
Когда датчики транспортного средства чувствуют мир, транспортное средство должно запланировать, путь через среду к бесплатной парковке определяют и выполняются, последовательность действий управления должна была управлять к нему. При выполнении так, это должно ответить на динамические изменения в среде, такие как пешеходы, пересекающие ее путь, и приспособить ее план.
Этот пример реализует подмножество функций, требуемых реализовывать такую систему. Это фокусируется на планировании выполнимого пути через среду, и выполнение действий должно было пересечь путь. Сопоставьте создание, и динамическое предотвращение препятствия исключены из этого примера.
Модель среды представляет карту среды. Для системы камердинера парковки эта карта включает доступные и занятые места для парковки, дорожные разметки и препятствия, такие как пешеходы или другие транспортные средства. Карты заполнения являются общим представлением для этой формы модели среды. Такая карта обычно создается с помощью Одновременной локализации и картографии (SLAM) путем интеграции наблюдений от датчиков лидара и камеры. Этот пример концентрируется на более простом сценарии, где карта уже предоставлена, например, транспортным средством к инфраструктуре (V2X) систему или камеру, выходящую на целое парковочное место. Это использует статическую карту парковки и принимает, что самолокализация транспортного средства точна.
Пример парковки, используемый в этом примере, состоит из трех слоев сетки заполнения.
Стационарные препятствия: Этот слой содержит стационарные препятствия как стенки, барьеры и границы парковки.
Дорожные разметки: Этот слой содержит информацию о заполнении, имеющую отношение к дорожным разметкам, включая дорожные разметки для парковочных мест.
Припаркованные автомобили: Этот слой содержит информацию, о которой уже заняты места для парковки.
Каждый слой карты содержит различные виды препятствий, которые представляют разные уровни опасности для автомобиля, перешедшего через него. С этой структурой каждый слой может быть обработан, обновлен и обеспечен независимо.
Загрузите и отобразите три слоя карты. В каждом слое темные ячейки представляют занятые ячейки, и легкие ячейки представляют свободные ячейки.
mapLayers = loadParkingLotMapLayers; plotMapLayers(mapLayers)
Для простоты, объединение эти три слоя в один costmap.
costmap = combineMapLayers(mapLayers); figure plot(costmap, 'Inflation', 'off') legend off
Объединенный costmap
объект, который представляет среду транспортного средства как 2D сетку заполнения. Каждая сетка в ячейке имеет значения между 0 и 1, представляя стоимость навигации через ячейку. Препятствия имеют более высокую стоимость, в то время как свободное пространство имеет более низкую цену. Ячейка рассматривается препятствием, если его стоимость выше, чем vehicleCostmap
OccupiedThreshold
свойство, и свободный, если его стоимость ниже, чем FreeThreshold
свойство.
costmap
покрывает целую 75m-by-50m область парковки, разделенную на 0.5m-by-0.5m квадратные ячейки.
costmap.MapExtent % [x, width, y, height] in meters costmap.CellSize % cell size in meters
ans = 0 75 0 50 ans = 0.5000
Создайте
объект для хранения размерностей транспортного средства, которое припаркуется автоматически. Также задайте максимальный руководящий угол транспортного средства. Это значение определяет пределы на поворачивающемся радиусе во время планирования движения и управления.vehicleDimensions
vehicleDims = vehicleDimensions;
maxSteeringAngle = 35; % in degrees
Обновите VehicleDimensions
свойство costmap средства проверки столкновения с размерностями транспортного средства, чтобы припарковаться. Эта установка настраивает степень инфляции в карте вокруг препятствий, чтобы соответствовать размеру припаркованного транспортного средства, гарантируя, что пути без коллизий могут быть найдены через парковку.
costmap.CollisionChecker.VehicleDimensions = vehicleDims;
Задайте стартовое положение транспортного средства. Положение получено посредством локализации, которая упущена из этого примера для простоты. Положение транспортного средства задано как в мировых координатах. представляет позицию центра задней оси транспортного средства в системе мировой координаты. представляет ориентацию транспортного средства относительно мировой оси X. Для получения дополнительной информации смотрите Системы координат в Automated Driving Toolbox.
currentPose = [4 12 0]; % [x, y, theta]
Планирование включает организацию всей уместной информации на иерархические слои. Каждый последовательный слой ответственен за более мелкомодульную задачу. Поведенческий слой [1] находится наверху этого стека. Это ответственно за активацию и управление различные части миссии путем предоставления последовательности задач навигации. Поведенческий слой собирает информацию от всех соответствующих частей системы, включая:
Локализация: поведенческий слой смотрит модуль локализации для оценки текущего местоположения транспортного средства.
Модель Environment: Восприятие и системы сплава датчика сообщают о карте среды вокруг транспортного средства.
Определение места для парковки: поведенческий слой анализирует карту, чтобы определить самое близкое доступное место для парковки.
Нахождение глобального маршрута: модуль маршрутизации вычисляет глобальный маршрут через дорожную сеть, полученную или от сервиса отображения или от инфраструктуры V2X. При разложении глобального маршрута, когда серия дорожных ссылок позволяет траектории для каждой ссылки быть запланированной по-другому. Например, итоговый маневр парковки требует различного профиля скорости, чем подход к месту для парковки. В более общей установке это становится крайне важным для навигации по улицам, которые включают различные ограничения скорости, количества маршрутов и дорожные знаки.
Вместо того, чтобы использовать датчики транспортного средства, чтобы создать карту среды, этот пример использует карту, которая прибывает с умной парковки через коммуникацию V2X. Для простоты примите, что карта в форме сетки заполнения с дорожными ссылками и местоположениями доступных мест для парковки, обеспеченных V2X.
HelperBehavioralPlanner
класс подражает интерфейсу поведенческого слоя планирования. HelperBehavioralPlanner
создается с помощью карты и глобального плана маршрута. Этот пример использует статический глобальный план маршрута, сохраненный в таблице MATLAB, но обычно алгоритм маршрутизации, предоставленный локальной инфраструктурой парковки или сервисом отображения, определяет этот план. Глобальный план маршрута описан как последовательность сегментов маршрута, чтобы пересечь, чтобы достигнуть места для парковки.
Загрузите MAT-файл, содержащий план маршрута, который хранится в таблице. Таблица имеет три переменные: StartPose
, EndPose
, и Attributes
. StartPose
и EndPose
задайте положения начала и конца сегмента, описанного как. Attributes
задает свойства сегмента, такие как ограничение скорости.
data = load('routePlan.mat'); routePlan = data.routePlan %#ok<NOPTS>
routePlan = 4×3 table StartPose EndPose Attributes ______________ ________________ ____________ 4 12 0 56 11 0 [1×1 struct] 56 11 0 70 19 90 [1×1 struct] 70 19 90 70 32 90 [1×1 struct] 70 32 90 53 39 180 [1×1 struct]
Постройте транспортное средство в текущем положении, и вдоль каждой цели в плане маршрута.
% Plot vehicle at current pose hold on helperPlotVehicle(currentPose, vehicleDims, 'DisplayName', 'Current Pose') legend for n = 1 : height(routePlan) % Extract the goal waypoint vehiclePose = routePlan{n, 'EndPose'}; % Plot the pose legendEntry = sprintf('Goal %i', n); helperPlotVehicle(vehiclePose, vehicleDims, 'DisplayName', legendEntry); end hold off
Создайте поведенческий объект помощника планировщика. requestManeuver
метод запрашивает поток задач навигации от поведенческого планировщика, пока место назначения не достигнуто.
behavioralPlanner = HelperBehavioralPlanner(routePlan, maxSteeringAngle);
Транспортное средство перемещается по каждому сегменту пути с помощью этих шагов:
Планирование движения: Запланируйте выполнимый путь через карту среды с помощью оптимального быстро исследующего случайного дерева (RRT*) алгоритм (pathPlannerRRT
).
Сглаживание пути: Сглаживайте ссылочный путь путем подбора кривой сплайнам к нему с помощью
.smoothPathSpline
Генерация траектории: Преобразуйте сглаживавший путь в траекторию путем генерации профиля скорости с помощью helperGenerateVelocityProfile
.
Управление транспортным средством: Учитывая сглаживавший ссылочный путь, HelperPathAnalyzer
вычисляет ссылочное положение и скорость на основе текущего положения и скорость транспортного средства. Предоставленный ссылочные значения,
вычисляет держащийся угол, чтобы управлять заголовком транспортного средства. lateralControllerStanley
HelperLongitudinalController
вычисляет ускорение и команды замедления, чтобы обеспечить желаемую скорость транспортного средства.
Целевая Проверка: Проверяйте, достигло ли транспортное средство итогового положения сегмента с помощью helperGoalChecker
.
Остальная часть этого примера описывает эти шаги подробно, прежде, чем собрать их в полное решение.
Учитывая глобальный маршрут, планирование движения может использоваться, чтобы запланировать путь через среду, чтобы достигнуть каждого промежуточного звена waypoint, пока транспортное средство не достигает конечного пункта назначения. Запланированный путь для каждой ссылки должен быть выполнимым и без коллизий. Выполнимый путь является тем, который может быть понят транспортным средством, учитывая движение и динамические ограничения, наложенные на него. Система камердинера парковки включает низкие скорости и низкие ускорения. Это позволяет нам безопасно игнорировать динамические ограничения, являющиеся результатом инерционных эффектов.
Создайте
объект сконфигурировать планировщика пути, использующего оптимальное быстро исследующее случайное дерево (RRT*) подход. Семейство RRT планирования алгоритмов находит путь путем построения дерева связанных, положений транспортного средства без коллизий. Положения соединяются с помощью регулирования Dubins или Reeds-Shepp, гарантируя, что сгенерированный путь кинематическим образом выполним.pathPlannerRRT
motionPlanner = pathPlannerRRT(costmap, 'MinIterations', 1000, ... 'ConnectionDistance', 10, 'MinTurningRadius', 20);
Запланируйте путь от текущего положения до первой цели при помощи plan
функция. Возвращенный
объект, driving.Path
refPath
, выполнимый и ссылочный путь без коллизий.
goalPose = routePlan{1, 'EndPose'};
refPath = plan(motionPlanner, currentPose, goalPose);
Ссылочный путь состоит из последовательности сегментов пути. Каждый сегмент пути описывает набор маневров Dubins или Reeds-Shepp, используемых, чтобы соединиться со следующим сегментом. Смотрите сегменты пути.
refPath.PathSegments
ans = 1×6 DubinsPathSegment array with properties: StartPose GoalPose MinTurningRadius MotionLengths MotionTypes Length
Ссылочный путь содержит положения перехода по пути, представляя точки вдоль пути, соответствующего переходу от одного маневра до следующего. Они могут также представлять изменения направления, например, от форварда, чтобы инвертировать движение вдоль пути Тростников-Shepp.
Получите положения перехода и направления от запланированного пути.
[transitionPoses, directions] = interpolate(refPath);
% Visualize the planned path
plot(motionPlanner)
В дополнение к запланированному ссылочному пути заметьте красные области на графике. Эти области представляют области costmap, где источник транспортного средства (центр задней оси) не должен пересекаться во избежание удара никаких препятствий.
находит пути, которые избегают препятствий путем проверки, чтобы гарантировать, что сгенерированные положения транспортного средства не лежат на этих областях.pathPlannerRRT
Ссылочный путь, сгенерированный планировщиком пути, составлен любой из сегментов Dubins или Reeds-Shepp. Искривление на перекрестках двух таких сегментов не непрерывно и может привести к резким изменениям в держащемся углу. Чтобы избежать такого неестественного движения и гарантировать пассажирский комфорт, путь должен быть непрерывно дифференцируемым и поэтому сглаживать [2]. Один подход к сглаживанию пути включает подбор кривой параметрическому кубическому сплайну. Подбор кривой сплайна позволяет вам сгенерировать сглаженный путь, который может выполнить контроллер.
Используйте
соответствовать параметрическому кубическому сплайну, который проходит через все точки перехода в ссылочном пути. Сплайн приблизительно совпадает с запуском и конечными направлениями с запуском и конечным углом рыскания транспортного средства.smoothPathSpline
% Specify number of poses to return using a separation of approximately 0.1 m approxSeparation = 0.1; % meters numSmoothPoses = round(refPath.Length / approxSeparation); % Return discretized poses along the smooth path [refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses); % Plot the smoothed path hold on hSmoothPath = plot(refPoses(:, 1), refPoses(:, 2), 'r', 'LineWidth', 2, ... 'DisplayName', 'Smoothed Path'); hold off
Затем преобразуйте сгенерированный сглаженный путь к траектории, которая может быть выполнена с помощью профиля скорости. Вычислите профиль скорости для каждого пути как последовательность трех фаз: ускорение к максимальной скорости набора, поддержание максимальной скорости и замедление к терминальной скорости. helperGenerateVelocityProfile
функция генерирует такой профиль скорости.
Задайте начальную букву, максимум и терминальные скорости так, чтобы транспортное средство запустилось стационарный, ускоряются к скорости 5 метров/секунда и останавливаются.
maxSpeed = 5; % in meters/second startSpeed = 0; % in meters/second endSpeed = 0; % in meters/second
Сгенерируйте скоростной профиль
refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, startSpeed, endSpeed, maxSpeed);
refVelocities
содержит ссылочные скорости для каждой точки вдоль сглаживавшего пути. Постройте сгенерированный скоростной профиль.
plotVelocityProfile(cumLengths, refVelocities, maxSpeed)
Задающие скорости, вместе со сглаживавшим путем, включают выполнимую траекторию, за которой может следовать транспортное средство. Контроллер обратной связи используется, чтобы следовать за этой траекторией. Контроллер корректирует ошибки в отслеживании траектории, которые являются результатом уменьшения шины и других источников шума, таких как погрешности в локализации. В частности, контроллер состоит из двух компонентов:
Боковое управление: Настройте держащийся угол, таким образом, что транспортное средство следует за ссылочным путем.
Продольное управление: При следовании за ссылочным путем обеспечьте желаемую скорость путем управления дросселем и тормозом.
Поскольку этот сценарий включает низкие скорости, можно упростить контроллер, чтобы учесть только кинематическую модель. В этом примере боковое управление понято
функция. Продольное управление понято Системой помощника object™ lateralControllerStanley
HelperLongitudinalController
, это вычисляет ускорение и команды замедления на основе Пропорционально-интегрального закона.
Контроллер обратной связи требует средства моделирования, которое может выполнить желаемые команды контроллера с помощью подходящей модели транспортного средства. HelperVehicleSimulator
класс симулирует такое транспортное средство с помощью следующей кинематической модели велосипеда:
В вышеупомянутых уравнениях, представляет положение транспортного средства в мировых координатах., и представляйте скорость заднего колеса, ускорение заднего колеса, колесную базу и держащийся угол, соответственно. Положение и скорость переднего колеса могут быть получены:
% Close all the figures closeFigures; % Create the vehicle simulator vehicleSim = HelperVehicleSimulator(costmap, vehicleDims); % Set the vehicle pose and velocity vehicleSim.setVehiclePose(currentPose); currentVel = 0; vehicleSim.setVehicleVelocity(currentVel); % Configure the simulator to show the trajectory vehicleSim.showTrajectory(true); % Hide vehicle simulation figure hideFigure(vehicleSim);
Создайте HelperPathAnalyzer
объект вычислить ссылочное положение, ссылочную скорость и направление движения для контроллера.
pathAnalyzer = HelperPathAnalyzer(refPoses, refVelocities, directions, ... 'Wheelbase', vehicleDims.Wheelbase);
Создайте HelperLongitudinalController
возразите, чтобы управлять скоростью транспортного средства и задать шаг расчета.
sampleTime = 0.05;
lonController = HelperLongitudinalController('SampleTime', sampleTime);
Используйте HelperFixedRate
объект гарантировать выполнение с фиксированной процентной ставкой контроллера обратной связи. Используйте уровень управления, чтобы быть сопоставимыми с продольным контроллером.
controlRate = HelperFixedRate(1/sampleTime); % in Hertz
Пока цель не достигнута, сделайте следующее:
Вычислите регулирование и команды ускорения/замедления, требуемые отслеживать запланированную траекторию.
Накормите командами управления средство моделирования.
Запишите возвращенное положение транспортного средства и скорость, чтобы питаться в контроллер в следующей итерации.
reachGoal = false; while ~reachGoal % Find the reference pose on the path and the corresponding velocity [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel); % Update driving direction for the simulator updateDrivingDirection(vehicleSim, direction); % Compute steering command steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ... 'Direction', direction, 'Wheelbase', vehicleDims.Wheelbase); % Compute acceleration and deceleration commands lonController.Direction = direction; [accelCmd, decelCmd] = lonController(refVel, currentVel); % Simulate the vehicle using the controller outputs drive(vehicleSim, accelCmd, decelCmd, steeringAngle); % Check if the vehicle reaches the goal reachGoal = helperGoalChecker(goalPose, currentPose, currentVel, endSpeed, direction); % Wait for fixed-rate execution waitfor(controlRate); % Get current pose and velocity of the vehicle currentPose = getVehiclePose(vehicleSim); currentVel = getVehicleVelocity(vehicleSim); end % Show vehicle simulation figure showFigure(vehicleSim);
Это завершает первый матч плана маршрута и демонстрирует каждый шаг процесса. Следующие разделы запускают средство моделирования для целого маршрута, который садится в транспортное средство близко к месту для парковки, и наконец выполняет маневр парковки, чтобы поместить транспортное средство в место для парковки.
Теперь объедините все предыдущие шаги в процессе планирования и запустите симуляцию для полного плана маршрута. Этот процесс включает слияние поведенческого планировщика.
% Set the vehicle pose back to the initial starting point currentPose = [4 12 0]; % [x, y, theta] vehicleSim.setVehiclePose(currentPose); % Reset velocity currentVel = 0; % meters/second vehicleSim.setVehicleVelocity(currentVel); while ~reachedDestination(behavioralPlanner) % Request next maneuver from behavioral layer [nextGoal, plannerConfig, speedConfig] = requestManeuver(behavioralPlanner, ... currentPose, currentVel); % Configure the motion planner configurePlanner(motionPlanner, plannerConfig); % Plan a reference path using RRT* planner to the next goal pose refPath = plan(motionPlanner, currentPose, nextGoal); % Check if the path is valid. If the planner fails to compute a path, % or the path is not collision-free because of updates to the map, the % system needs to re-plan. This scenario uses a static map, so the path % will always be collision-free. isReplanNeeded = ~checkPathValidity(refPath, costmap); if isReplanNeeded warning('Unable to find a valid path. Attempting to re-plan.') % Request behavioral planner to re-plan replanNeeded(behavioralPlanner); continue; end % Retrieve transition poses and directions from the planned path [transitionPoses, directions] = interpolate(refPath); % Smooth the path numSmoothPoses = round(refPath.Length / approxSeparation); [refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses); % Generate a velocity profile refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, startSpeed, endSpeed, maxSpeed); % Configure path analyzer pathAnalyzer.RefPoses = refPoses; pathAnalyzer.Directions = directions; pathAnalyzer.VelocityProfile = refVelocities; % Reset longitudinal controller reset(lonController); reachGoal = false; % Execute control loop while ~reachGoal % Find the reference pose on the path and the corresponding velocity [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel); % Update driving direction for the simulator updateDrivingDirection(vehicleSim, direction); % Compute steering command steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ... 'Direction', direction, 'Wheelbase', vehicleDims.Wheelbase); % Compute acceleration and deceleration commands lonController.Direction = direction; [accelCmd, decelCmd] = lonController(refVel, currentVel); % Simulate the vehicle using the controller outputs drive(vehicleSim, accelCmd, decelCmd, steeringAngle); % Check if the vehicle reaches the goal reachGoal = helperGoalChecker(nextGoal, currentPose, currentVel, speedConfig.EndSpeed, direction); % Wait for fixed-rate execution waitfor(controlRate); % Get current pose and velocity of the vehicle currentPose = getVehiclePose(vehicleSim); currentVel = getVehicleVelocity(vehicleSim); end end % Show vehicle simulation figure showFigure(vehicleSim);
Теперь, когда транспортное средство около места для парковки, специализированный маневр парковки используется, чтобы припарковать транспортное средство в итоговом месте для парковки. Этот маневр требует прохождения через узкий коридор между ребрами места для парковки на обоих концах. Такой маневр обычно сопровождается с датчиками ультразвука или лазерными сканерами, постоянно проверяющими на препятствия.
% Hide vehicle simulation figure
hideFigure(vehicleSim);
vehicleCostmap
использует основанную на инфляции проверку столкновения. Во-первых, визуально смотрите текущее средство проверки столкновения в использовании.
ccConfig = costmap.CollisionChecker;
figure
plot(ccConfig)
title('Current Collision Checker')
Проверка столкновения выполняется путем расширения препятствий в costmap радиусом инфляции и проверки ли центр круга, показанного выше лжи на расширенной ячейке сетки. Итоговый маневр парковки требует более точного, менее консервативного проверяющего столкновение механизма. Это обычно решается путем представления формы транспортного средства с помощью (приблизительно 3-5) перекрывающиеся круги вместо одного круга.
Используйте большее число кругов в средстве проверки столкновения и визуально смотрите средство проверки столкновения. Это позволяет планировать через узкие проходы.
ccConfig.NumCircles = 4;
figure
plot(ccConfig)
title('New Collision Checker')
Обновите costmap, чтобы использовать это средство проверки столкновения.
costmap.CollisionChecker = ccConfig;
Заметьте, что радиус инфляции уменьшал, позволяя планировщику найти свободный путь к месту для парковки.
figure plot(costmap) title('Costmap with updated collision checker') % Set up the pathPlannerRRT to use the updated costmap parkMotionPlanner = pathPlannerRRT(costmap, 'MinIterations', 1000); % Define desired pose for the parking spot, returned by the V2X system parkPose = [36 44 90]; preParkPose = currentPose; % Compute the required parking maneuver refPath = plan(parkMotionPlanner, preParkPose, parkPose); % Plot the resulting parking maneuver figure plotParkingManeuver(costmap, refPath, preParkPose, parkPose)
Если маневр найден, повторите предыдущий процесс, чтобы определить комплексный план: сглаживайте путь, сгенерируйте профиль скорости и следуйте за траекторией с помощью контроллера обратной связи.
% Retrieve transition poses and directions from the planned path [transitionPoses, directions] = interpolate(refPath); % Smooth the path numSmoothPoses = round(refPath.Length / approxSeparation); [refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses); % Set up the velocity profile generator to stop at the end of the trajectory, % with a speed limit of 5 mph refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, currentVel, 0, 2.2352); pathAnalyzer.RefPoses = refPoses; pathAnalyzer.Directions = directions; pathAnalyzer.VelocityProfile = refVelocities; % Reset longitudinal controller reset(lonController); reachGoal = false; while ~reachGoal % Find the reference pose on the path and the corresponding velocity [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel); % Update driving direction for the simulator updateDrivingDirection(vehicleSim, direction); % Compute steering command steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ... 'Direction', direction, 'Wheelbase', vehicleDims.Wheelbase); % Compute acceleration and deceleration commands lonController.Direction = direction; [accelCmd, decelCmd] = lonController(refVel, currentVel); % Simulate the vehicle using the controller outputs drive(vehicleSim, accelCmd, decelCmd, steeringAngle); % Check if the vehicle reaches the goal reachGoal = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction); % Wait for fixed-rate execution waitfor(controlRate); % Get current pose and velocity of the vehicle currentPose = getVehiclePose(vehicleSim); currentVel = getVehicleVelocity(vehicleSim); end % Show vehicle simulation figure closeFigures; showFigure(vehicleSim);
Альтернативный способ припарковать транспортное средство состоит в том, чтобы отступить в место для парковки. Когда транспортное средство должно отойти назад в место, планировщик движения должен использовать метод связи Тростников-Shepp, чтобы искать выполнимый путь. Связь Тростников-Shepp допускает противоположные движения во время планирования.
% Specify a parking pose corresponding to a back-in parking maneuver parkPose = [49 47.2 -90]; % Change the connection method to allow for reverse motions parkMotionPlanner.ConnectionMethod = 'Reeds-Shepp';
Чтобы найти выполнимый путь, планировщик движения должен быть настроен. Используйте больший радиус превращения и расстояние связи, чтобы допускать сглаженную спину - в.
parkMotionPlanner.MinTurningRadius = 10; % meters parkMotionPlanner.ConnectionDistance = 15; % Reset vehicle pose and velocity currentVel = 0; vehicleSim.setVehiclePose(preParkPose); vehicleSim.setVehicleVelocity(currentVel); % Compute the parking maneuver replan = true; while replan refPath = plan(parkMotionPlanner, preParkPose, parkPose); % The path corresponding to the parking maneuver is small and requires % precise maneuvering. Instead of interpolating only at transition poses, % interpolate more finely along the length of the path. numSamples = 10; stepSize = refPath.Length / numSamples; lengths = 0 : stepSize : refPath.Length; [transitionPoses, directions] = interpolate(refPath, lengths); % Replan if the path contains more than one direction switching poses % or if the path is too long replan = sum(abs(diff(directions)))~=2 || refPath.Length > 20; end % Visualize the parking maneuver figure plotParkingManeuver(costmap, refPath, preParkPose, parkPose)
% Smooth the path numSmoothPoses = round(refPath.Length / approxSeparation); [refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses, 0.5); % Generate velocity profile refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, currentVel, 0, 1); pathAnalyzer.RefPoses = refPoses; pathAnalyzer.Directions = directions; pathAnalyzer.VelocityProfile = refVelocities; % Reset longitudinal controller reset(lonController); reachGoal = false; while ~reachGoal % Get current driving direction currentDir = getDrivingDirection(vehicleSim); % Find the reference pose on the path and the corresponding velocity. [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel); % If the vehicle changes driving direction, reset vehicle velocity in % the simulator and reset longitudinal controller if currentDir ~= direction currentVel = 0; setVehicleVelocity(vehicleSim, currentVel); reset(lonController); end % Update driving direction for the simulator. If the vehicle changes % driving direction, reset and return the current vehicle velocity as zero. currentVel = updateDrivingDirection(vehicleSim, direction, currentDir); % Compute steering command steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ... 'Direction', direction, 'Wheelbase', vehicleDims.Wheelbase); % Compute acceleration and deceleration commands lonController.Direction = direction; [accelCmd, decelCmd] = lonController(refVel, currentVel); % Simulate the vehicle using the controller outputs drive(vehicleSim, accelCmd, decelCmd, steeringAngle); % Check if the vehicle reaches the goal reachGoal = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction); % Wait for fixed-rate execution waitfor(controlRate); % Get current pose and velocity of the vehicle currentPose = getVehiclePose(vehicleSim); currentVel = getVehicleVelocity(vehicleSim); end % Take a snapshot for the example closeFigures; snapnow; % Delete the simulator delete(vehicleSim);
Этот пример показал как:
Запланируйте выполнимый путь в полуструктурированной среде, такой как парковка, с помощью RRT* алгоритм планирования пути.
Сглаживайте путь с помощью сплайнов и сгенерируйте профиль скорости вдоль сглаживавшего пути.
Управляйте транспортным средством, чтобы следовать за ссылочным путем на желаемой скорости.
Поймите различные поведения парковки при помощи различных настроек планировщика движения.
[1] Buehler, Мартин, Карл Иэгнемма и Сэнджив Сингх. DARPA Городская проблема: Автономные Транспортные средства в Городском движении (1-й редактор). Springer Publishing Company, Incorporated, 2009.
[2] Lepetic, Марко, Грегор Клэнкэр, Игорь Скрянк, Драго Матко, Bostjan Potocnik, "Время Оптимальное Планирование пути, Рассматривая Ускоряющие Пределы". Робототехника и Автономные системы. Объем 45, Выпуски 3-4, 2003, стр 199-210.
среда Загрузки loadParkingLotMapLayers сопоставляет слои для парковки
function mapLayers = loadParkingLotMapLayers() %loadParkingLotMapLayers % Load occupancy maps corresponding to 3 layers - obstacles, road % markings, and used spots. mapLayers.StationaryObstacles = imread('stationary.bmp'); mapLayers.RoadMarkings = imread('road_markings.bmp'); mapLayers.ParkedCars = imread('parked_cars.bmp'); end
struct Графика plotMapLayers, содержащий слои карты
function plotMapLayers(mapLayers) %plotMapLayers % Plot the multiple map layers on a figure window. figure cellOfMaps = cellfun(@imcomplement, struct2cell(mapLayers), 'UniformOutput', false); montage( cellOfMaps, 'Size', [1 numel(cellOfMaps)], 'Border', [5 5], 'ThumbnailSize', [300 NaN] ) title('Map Layers - Stationary Obstacles, Road markings, and Parked Cars') end
Объединение combineMapLayers сопоставляет слои в один costmap
function costmap = combineMapLayers(mapLayers) %combineMapLayers % Combine map layers struct into a single vehicleCostmap. combinedMap = mapLayers.StationaryObstacles + mapLayers.RoadMarkings + ... mapLayers.ParkedCars; combinedMap = im2single(combinedMap); res = 0.5; % meters costmap = vehicleCostmap(combinedMap, 'CellSize', res); end
configurePlanner Конфигурируют планировщика пути с заданными настройками
function configurePlanner(pathPlanner, config) %configurePlanner % Configure the path planner object, pathPlanner, with settings specified % in struct config. fieldNames = fields(config); for n = 1 : numel(fieldNames) if ~strcmpi(fieldNames{n}, 'IsParkManeuver') pathPlanner.(fieldNames{n}) = config.(fieldNames{n}); end end end
профиль скорости Графика plotVelocityProfile
function plotVelocityProfile(cumPathLength, refVelocities, maxSpeed) %plotVelocityProfile % Plot the generated velocity profile % Plot reference velocity along length of the path plot(cumPathLength, refVelocities, 'LineWidth', 2); % Plot a line to display maximum speed hold on line([0;cumPathLength(end)], [maxSpeed;maxSpeed], 'Color', 'r') hold off % Set axes limits buffer = 2; xlim([0 cumPathLength(end)]); ylim([0 maxSpeed + buffer]) % Add labels xlabel('Cumulative Path Length (m)'); ylabel('Velocity (m/s)'); % Add legend and title legend('Velocity Profile', 'Max Speed') title('Generated velocity profile') end
closeFigures
function closeFigures() % Close all the figures except the simulator visualization % Find all the figure objects figHandles = findobj('Type', 'figure'); for i = 1: length(figHandles) if ~strcmp(figHandles(i).Name, 'Automated Valet Parking') close(figHandles(i)); end end end
Отображение plotParkingManeuver сгенерированный маневр парковки на costmap
function plotParkingManeuver(costmap, refPath, currentPose, parkPose) %plotParkingManeuver % Plot the generated parking maneuver on a costmap. % Plot the costmap, without inflated areas plot(costmap, 'Inflation', 'off') % Plot reference parking maneuver on the costmap hold on plot(refPath, 'DisplayName', 'Parking Maneuver') title('Parking Maneuver') % Zoom into parking maneuver by setting axes limits lo = min([currentPose(1:2); parkPose(1:2)]); hi = max([currentPose(1:2); parkPose(1:2)]); buffer = 6; % meters xlim([lo(1)-buffer hi(1)+buffer]) ylim([lo(2)-buffer hi(2)+buffer]) end