Этот пример демонстрирует планирование движения беспилотного летательного аппарата (БПЛА) с использованием алгоритма быстрого исследования случайного дерева (RRT) с учетом позы начала и цели на карте 3-D. БПЛА имеет неголономный характер и должен подчиняться аэродинамическим ограничениям, таким как максимальный угол крена, угол траектории полета и воздушная скорость при движении между ППМ.
В этом примере Вы настроите 3D карту, обеспечить позу начала и позу цели, запланировать путь с RRT использование 3D примитивов движения Dubins, сглаживать полученный путь и моделировать полет БПЛА.
% Set RNG seed for repeatable result rng(1,"twister");
Загрузить карту заполняемости 3-D uavMapCityBlock.mat, который содержит набор предварительно созданных препятствий, в рабочую область. Равномерно надуть карту на 1 м для повышения безопасности пути и учета размаха крыльев БПЛА. Карта занятости находится в кадре RUS (восток-север-вверх).
mapData = load("uavMapCityBlock.mat","omap"); omap = mapData.omap; % Consider unknown spaces to be unoccupied omap.FreeThreshold = omap.OccupiedThreshold; inflate(omap,1) figure("Name","CityBlock") show(omap)
Используя карту для справки, выберите незанятую начальную позу и целевую позу.
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 объект для проверки столкновения БПЛА с окружающей средой.
В этом примере представлено предопределенное пространство состояний, ExampleHelperUavStateSpace, для планирования пути. Пространство состояний определяется как [x y z headingAngle], где [x y z] определяет положение БПЛА и headingAngle задает угол курса в радианах. В примере используется uavDubinsConnection объект как кинематическая модель для БПЛА, которая ограничена максимальным углом крена, воздушной скоростью и углом траектории полета. Создайте объект пространства состояния, указав максимальный угол крена, воздушную скорость и угол траектории полета, ограничивающие свойства БПЛА как пары имя-значение. Используйте "Bounds" аргумент пары имя-значение для указания положения и границ ориентации БПЛА в виде матрицы 4 на 2, где первые три строки представляют x-, y-, и 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 занято на карте. Движение между двумя состояниями является действительным только в том случае, если все промежуточные состояния являются действительными, что означает, что БПЛА не проходит через какое-либо занятое место на карте. Создать 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 в 3-D пространстве. Плановик находит путь, который свободен от столкновений и подходит для полета самолета.
[pthObj,solnInfo] = plan(planner,startPose,goalPose);
Визуализация запланированного пути. Интерполяция планируемого пути на основе соединений БПЛА «Дубины». Постройте график интерполированных состояний в виде зеленой линии.
Имитировать полет БПЛА с помощью предусмотренной функции помощника, exampleHelperSimulateUAV, что требует наличия ППМ, скорости и времени для достижения цели (на основе скорости и длины пути). Вспомогательная функция использует fixedwing модель наведения для моделирования поведения БПЛА на основе управляющих входных сигналов, генерируемых из ППМ. Постройте график моделируемых состояний красной линией.
Обратите внимание, что имитируемый полет БПЛА незначительно отклоняется от запланированного пути из-за небольших ошибок слежения за управлением. Также путь 3-D Дубиных предполагает мгновенные изменения угла крена БПЛА, но фактическая динамика имеет более медленный отклик на команды крена. Один из способов компенсировать это отставание - планировать пути с более консервативными аэродинамическими ограничениями.
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
Первоначальный запланированный путь делает некоторые ненужные повороты во время навигации к цели. Упростите 3D путь Dubins при помощи алгоритма сглаживания пути, которому предоставляют пример, exampleHelperUAVPathSmoothing. Эта функция удаляет промежуточные 3-D дубинские позы, основанные на итеративной стратегии. Для получения дополнительной информации о стратегии сглаживания см. [1]. Функция сглаживания соединяет несистематические 3-D Дубины позируют друг с другом, пока это не приводит к столкновению. Плавные пути, создаваемые этим процессом, улучшают характеристики отслеживания для имитационной модели крыла. Смоделировать модель БПЛА с этими новыми сглаженными ППМ.
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] Борода, Рэндал У. и Тимоти У. Маклейн. Малый беспилотный летательный аппарат: теория и практика. Принстон, Нью-Джерси: Princeton University Press, 2012.
[2] Хорнунг, Армин, Кай М. Вурм, Марен Бенневиц, Сирил Стахнисс и Вольфрам Бургард. «OctoMap: эффективная вероятностная структура отображения 3D на основе Octrees». Автономные роботы 34, № 3 (апрель 2013): 189-206. https://doi.org/10.1007/s10514-012-9321-0.