Этот пример демонстрирует планирование движения беспилотного воздушного транспортного средства (UAV) фиксированного крыла с помощью быстро исследующего случайного дерева (RRT) алгоритм, учитывая запуск и целевое положение на 3-D карте. Фиксированное крыло UAV является неголономным по своей природе и должен выполнить аэродинамические ограничения как максимальный крен, угол угла тангажа и скорость полета при перемещении между waypoints.
В этом примере вы настроите 3-D карту, обеспечить положение запуска и целевое положение, запланировать путь с RRT использование 3-D примитивов движения Dubins, сглаживать полученный путь и симулировать рейс UAV.
% Set RNG seed for repeatable result rng(1,"twister");
Загрузите 3-D карту заполнения uavMapCityBlock.mat, который содержит набор предсгенерированных препятствий в рабочую область. Карта заполнения находится в системе координат ENU (Восточный Север).
mapData = load("uavMapCityBlock.mat","omap"); omap = mapData.omap; % Consider unknown spaces to be unoccupied omap.FreeThreshold = omap.OccupiedThreshold;
Используя карту для ссылки, выберите незанятое положение запуска и целевое положение.
startPose = [12 22 25 pi/2]; goalPose = [150 180 35 pi/2]; figure("Name","StartAndGoal") hMap = show(omap); hold on scatter3(hMap,startPose(1),startPose(2),startPose(3),30,"red","filled") scatter3(hMap,goalPose(1),goalPose(2),goalPose(3),30,"green","filled") hold off view([-31 63])
RRT является основанным на дереве планировщиком движения, который создает дерево поиска инкрементно из случайных выборок данного пространства состояний. Дерево в конечном счете охватывает пространство поиска и соединяет начальное состояние и целевое состояние. Соедините два состояния с помощью uavDubinsConnection объект, который удовлетворяет аэродинамическим ограничениям. Используйте validatorOccupancyMap3D объект для проверки столкновения между фиксированным крылом UAV и средой.
Этот пример обеспечивает предопределенное пространство состояний, ExampleHelperUavStateSpace, для планирования пути. Пространство состояний задано как [x y z headingAngle], где [x y z] задает положение UAV и headingAngle задает угол рыскания в радианах. Пример использует uavDubinsConnection возразите как кинематическая модель для UAV, который ограничивается максимальным креном, скоростью полета и углом угла тангажа. Создайте объект пространства состояний путем определения максимального крена, скорости полета, и угол угла тангажа ограничивает свойства UAV как пары "имя-значение". Используйте "Bounds" аргумент пары "имя-значение", чтобы задать положение и контуры ориентации UAV как 4 2 матрица, где первые три строки представляют xY-, и z- контуры оси в 3-D карте заполнения и последней строке представляют угол рыскания в области значений [-pi, pi] радианы.
ss = ExampleHelperUAVStateSpace("MaxRollAngle",pi/6,... "AirSpeed",6,... "FlightPathAngleLimit",[-0.1 0.1],... "Bounds",[-20 220; -20 220; 10 100; -pi pi]);
Установите пороговые границы рабочей области на основе целевого целевого положения. Этот порог диктует, насколько большой целевая область цели рабочей области вокруг целевого положения, который используется для выборки смещения подхода области цели рабочей области.
threshold = [(goalPose-0.5)' (goalPose+0.5)'; -pi pi];
Используйте setWorkspaceGoalRegion функционируйте, чтобы обновить целевое положение и область вокруг этого.
setWorkspaceGoalRegion(ss,goalPose,threshold)
validatorOccupancyMap3D объект решает, что состояние недопустимо, если xyz-местоположение занято на карте. Движение между двумя состояниями допустимо, только если все промежуточные состояния допустимы, что означает, что UAV не проходит ни через какое занятое местоположение на карте. Создайте validatorOccupancyMap3D объект путем определения пространства состояний возражает и расширенная карта. Затем установите расстояние валидации, в метрах, для интерполяции между состояниями.
sv = validatorOccupancyMap3D(ss,"Map",omap);
sv.ValidationDistance = 0.1;Создайте plannerRRT объект путем определения пространства состояний и блока проверки допустимости состояния как входные параметры. Установите MaxConnectionDistance, GoalBias, и MaxIterations свойства объекта планировщика, и затем задают пользовательскую целевую функцию. Эта целевая функция решает, что путь достиг цели, если Евклидово расстояние до цели ниже порога 5 м.
planner = plannerRRT(ss,sv); planner.MaxConnectionDistance = 50; planner.GoalBias = 0.10; planner.MaxIterations = 400; planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3)) < 5);
Выполните основанное на RRT планирование пути в трехмерном пространстве. Планировщик находит путь, который без коллизий и подходит для рейса фиксированного крыла.
[pthObj,solnInfo] = plan(planner,startPose,goalPose);
Визуализируйте запланированный путь. Интерполируйте запланированный путь на основе связей Dubins UAV. Постройте интерполированные состояния как зеленую линию.
Симулируйте рейс UAV с помощью обеспеченной функции помощника, exampleHelperSimulateUAV, который требует, чтобы waypoints, скорость полета, и время достигли цели (на основе скорости полета и длины пути). Функция помощника использует fixedwing модель руководства, чтобы симулировать поведение UAV на основе входных параметров управления, сгенерированных от waypoints. Постройте симулированные состояния как красную линию.
Заметьте, что симулированный рейс UAV отклоняется немного от запланированного пути из-за небольших ошибок отслеживания управления. Кроме того, 3-D путь Dubins принимает мгновенные изменения в крене UAV, но фактические движущие силы имеют более медленный ответ, чтобы прокрутить команды. Один способ компенсировать эту задержку состоит в том, чтобы запланировать пути с более консервативными аэродинамическими ограничениями.
if (solnInfo.IsPathFound) figure("Name","OriginalPath") % Visualize the 3-D map show(omap) hold on scatter3(startPose(1),startPose(2),startPose(3),30,"red","filled") scatter3(goalPose(1),goalPose(2),goalPose(3),30,"green","filled") interpolatedPathObj = copy(pthObj); interpolate(interpolatedPathObj,1000) % Plot the interpolated path based on UAV Dubins connections hReference = plot3(interpolatedPathObj.States(:,1), ... interpolatedPathObj.States(:,2), ... interpolatedPathObj.States(:,3), ... "LineWidth",2,"Color","g"); % Plot simulated UAV trajectory based on fixed-wing guidance model % Compute total time of flight and add a buffer timeToReachGoal = 1.05*pathLength(pthObj)/ss.AirSpeed; waypoints = interpolatedPathObj.States; [xENU,yENU,zENU] = exampleHelperSimulateUAV(waypoints,ss.AirSpeed,timeToReachGoal); hSimulated = plot3(xENU,yENU,zENU,"LineWidth",2,"Color","r"); legend([hReference,hSimulated],"Reference","Simulated","Location","best") hold off view([-31 63]) end
Исходный запланированный путь делает некоторые ненужные повороты при навигации к цели. Упростите 3-D путь Dubins при помощи алгоритма сглаживания пути, которому предоставляют пример, exampleHelperUAVPathSmoothing. Эта функция удаляет промежуточные 3-D положения Дубинса на основе итеративной стратегии. Для получения дополнительной информации о стратегии сглаживания см. [1]. Функция сглаживания соединяет непоследовательные 3-D положения Дубинса друг с другом, пока выполнение так не приводит к столкновению. Сглаженные пути, сгенерированные этим процессом, улучшают характеристики отслеживания для имитационной модели фиксированного крыла. Симулируйте модель UAV фиксированного крыла с ними новый, сглаживавший waypoints.
if (solnInfo.IsPathFound) smoothWaypointsObj = exampleHelperUAVPathSmoothing(ss,sv,pthObj); figure("Name","SmoothedPath") % Plot the 3-D map show(omap) hold on scatter3(startPose(1),startPose(2),startPose(3),30,"red","filled") scatter3(goalPose(1),goalPose(2),goalPose(3),30,"green","filled") interpolatedSmoothWaypoints = copy(smoothWaypointsObj); interpolate(interpolatedSmoothWaypoints,1000) % Plot smoothed path based on UAV Dubins connections hReference = plot3(interpolatedSmoothWaypoints.States(:,1), ... interpolatedSmoothWaypoints.States(:,2), ... interpolatedSmoothWaypoints.States(:,3), ... "LineWidth",2,"Color","g"); % Plot simulated flight path based on fixed-wing guidance model waypoints = interpolatedSmoothWaypoints.States; timeToReachGoal = 1.05*pathLength(smoothWaypointsObj)/ss.AirSpeed; [xENU,yENU,zENU] = exampleHelperSimulateUAV(waypoints,ss.AirSpeed,timeToReachGoal); hSimulated = plot3(xENU,yENU,zENU,"LineWidth",2,"Color","r"); legend([hReference,hSimulated],"SmoothedReference","Simulated","Location","best") hold off view([-31 63]); end
Сглаживавший путь намного короче и показывает улучшенное отслеживание в целом.
[1] Борода, Рэндал В. и Тимоти В. Маклэйн. Маленький беспилотный самолет: теория и практика. Принстон, Нью-Джерси: Издательство Принстонского университета, 2012.
[2] Hornung, Армин, Кай М. Верм, Марен Бенневитс, Кирилль Штахнисс и Вольфрам Burgard. “OctoMap: Эффективная Вероятностная 3D Среда Отображения На основе Деревьев октантов”. Автономные Роботы 34, № 3 (апрель 2013): 189–206. https://doi.org/10.1007/s10514-012-9321-0.