Этот пример демонстрирует планирование движения беспилотного летательного транспортного средства (БПЛА) фиксированного типа с помощью быстро исследуемого алгоритма случайного дерева (RRT), заданного начальным и целевым положением на карте 3-D. БПЛА с неподвижным крылом является неголономичным по своей природе и должен подчиняться аэродинамическим ограничениям, таким как максимальный угол крена, угол угла тангажа и воздушная скорость при движении между путевыми точками.
В этом примере вы будете настраивать карту 3-D, предоставлять начальное положение и положение цели, планировать путь с RRT с помощью 3-D примитивов движения Дубинса, сглаживать полученный путь и моделировать рейс БПЛА.
% Set RNG seed for repeatable result rng(1,"twister");
Загрузите 3-D карту заполнения uavMapCityBlock.mat, который содержит набор предгенерированных препятствий, в рабочую область. Равномерно надьте карту на 1 м для повышения безопасности пути и учета размаха крыльев БПЛА фиксированного типа. Карта заполнения находится в системе координат ENU (East-North-Up).
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)
The 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 в трехмерном пространстве. Планировщик находит путь, который не имеет столкновения и подходит для рейса фиксированным крылом.
[pthObj,solnInfo] = plan(planner,startPose,goalPose);
Визуализация запланированного пути. Интерполяция запланированного пути на основе соединений БПЛА Dubins. Постройте график интерполированных состояний как зеленой линии.
Симулируйте рейс БПЛА с помощью предоставленной функции помощника, exampleHelperSimulateUAV, что требует путевых точек, воздушной скорости и времени для достижения цели (на основе воздушной скорости и длины пути). Функция helper использует 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
Первоначальный запланированный путь делает некоторые ненужные повороты во время движения к цели. Упростите 3-D путь Дубинса с помощью алгоритма сглаживания пути, предоставленного примером, exampleHelperUAVPathSmoothing. Эта функция удаляет промежуточные 3-D положения Дубинса, основанные на итеративной стратегии. Для получения дополнительной информации о стратегии сглаживания см. [1]. Функция сглаживания соединяет непоследовательные 3D положения Дубинса друг с другом, пока выполнение так не приводит к столкновению. Сглаживающие пути, сгенерированные этим процессом, улучшают характеристики отслеживания для модели фиксированной симуляции. Симулируйте модель фиксированного БПЛА с этими новыми сглаженными путевыми точками.
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, N.J: Princeton University Press, 2012.
[2] Hornung, Armin, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss и Wolfram Burgard. OctoMap: эффективная вероятностная среда 3D отображения на основе октреев. Автономные роботы 34, № 3 (апрель 2013): 189-206. https://doi.org/10.1007/s10514-012-9321-0.