Сопоставьте среду для планирования движения Используя лидар UAV
Этот пример демонстрирует, как использовать UAV, оборудованный датчиком лидара, чтобы сопоставить среду в 3D карте заполнения. Та сгенерированная карта заполнения может затем использоваться, чтобы выполнить планирование движения. UAV следует за заданной траекторией через среду. Знание положения БПЛА и облаков точек, полученных в соответствующих местоположениях, используется, чтобы создать карту среды. Положение UAV принято, чтобы быть полностью известным в течение рейса отображения; это также известно как построение карты при известном местоположении. Сгенерированная карта используется для планирования движения. Цель планирования движения состоит в том, чтобы запланировать путь UAV в жилом комплексе, где это используется, чтобы привезти пакеты, поставленные в логическом элементе, к желаемому местоположению крыши в комплексе.
Система координат "восточного севера" (ENU) используется в этом примере.
Оглавление
Создайте сценарий
Создайте простой uavScenario создавать представление простой сцены жилого комплекса. Во время симуляции сценария сцена будет использоваться, чтобы создать, симулируют точки данных лидара.
Задайте время и частоту обновления симуляции. В этом случае симулированный рейс занимает 60 секунд, и симуляция обновляется на уровне 2 Гц.
close all close all hidden simTime = 60; % in seconds updateRate = 2; % in Hz scene = uavScenario("UpdateRate",updateRate,"StopTime",simTime);
Добавьте статические сетки в сценарий, чтобы представлять различные создания в среде. Мы задаем каждое создание с прямоугольным местом и различной высотой. Все координаты исчисляются в метрах.
% Floor addMesh(scene, "Polygon",{[0 0;80 0;80 80;40 80;40 40;0 40],[-1 0]},[0.3 0.3 0.3]); % Features addMesh(scene, "Polygon",{[10 0;30 0;30 20;10 20],[0 40]},[0.4660 0.6740 0.1880]); % Building 1 addMesh(scene, "Polygon",{[45 0;80 0;80 30;45 30],[0 60]},[0.9290 0.6980 0.1250]); % Building 2 addMesh(scene, "Polygon",{[0 35;10 35;10 40;0 40],[0 5]},[0 0.5 0]); % Generator Room addMesh(scene, "Polygon",{[50 40;80 40;80 70;50 70],[0 5]},[0 0.4470 0.7410]); % Swimming Pool addMesh(scene, "Polygon",{[0 0;2 0;2 4;0 4],[0 3]},[0.6350 0.0780 0.1840]); % Security Room
Просмотрите созданный сценарий в 3D.
show3D(scene);
axis equal
view([-115 20])
Создайте траекторию отображения
Задайте траекторию, за которой UAV следует для отображения среды. Задайте waypoints и присвойте ориентацию для каждого waypoint.
Полный вектор положения для UAV следующие
pose = [x y z a b c d]
xY и z задайте положение UAV относительно системы отсчета сценария. aBC и d части номера кватерниона, который задает ориентацию UAV относительно системы координат сценария. действительной части кватерниона.
% Waypoints
x = -20:80;
y = -20:80;
z = 100*ones(1,length(x));
waypoints = [x' y' z']; Задайте ориентацию как Углы Эйлера (в радианах) и преобразуйте Углы Эйлера в кватернион:
orientation_eul = [0 0 0]; orientation_quat = quaternion(eul2quat(orientation_eul)); orientation_vec = repmat(orientation_quat,length(x),1);
Задайте временной вектор
time = 0:(simTime/(length(x)-1)):simTime;
Сгенерируйте траекторию от заданного waypoints и ориентаций с помощью waypointTrajectory системный объект. Задайте систему координат как 'ENU'.
trajectory = waypointTrajectory("Waypoints",waypoints,"Orientation",orientation_vec, ... "SampleRate",updateRate,"ReferenceFrame","ENU","TimeOfArrival",time);
Задайте начальное положение UAV
initial_pose = [-20 -20 100 1 0 0 0];
Создайте платформу UAV
Создайте uavPlatform возразите и обновите сценарий с сеткой UAV.
plat = uavPlatform("UAV",scene,"Trajectory",trajectory,"ReferenceFrame","ENU"); updateMesh(plat,"quadrotor",{4},[1 0 0],eye(4));
Введите 3D лидар в сценарий с помощью uavLidarPointCloudGenerator системный объект. Задайте соответствующие параметры датчика лидара. Например, лидар в этом примере имеет максимальную область значений 200 метров, но можно настроить параметры по мере необходимости.
lidarmodel = uavLidarPointCloudGenerator("AzimuthResolution",0.6, ... "ElevationLimits",[-90 -20],"ElevationResolution",2.5, ... "MaxRange",200,"UpdateRate",2,"HasOrganizedOutput",true); lidar = uavSensor("Lidar",plat,lidarmodel,"MountingLocation",[0 0 -1],"MountingAngles",[0 0 0]);
Симулируйте создание рейса и карты отображения
[ax,plotFrames] = show3D(scene); xlim([-15 80]); ylim([-15 80]); zlim([0 80]); view([-115 20]); axis equal hold on colormap('jet'); ptc = pointCloud(nan(1,1,3)); scatterplot = scatter3(nan,nan,nan,1,[0.3020 0.7451 0.9333],... "Parent",plotFrames.UAV.Lidar); scatterplot.XDataSource = "reshape(ptc.Location(:,:,1), [], 1)"; scatterplot.YDataSource = "reshape(ptc.Location(:,:,2), [], 1)"; scatterplot.ZDataSource = "reshape(ptc.Location(:,:,3), [], 1)"; scatterplot.CDataSource = "reshape(ptc.Location(:,:,3), [], 1) - min(reshape(ptc.Location(:,:,3), [], 1))"; hold off; lidarSampleTime = []; pt = cell(1,((updateRate*simTime) +1)); ptOut = cell(1,((updateRate*simTime) +1));
Создайте карту заполнения для более эффективного способа хранить данные об облаке точек. Используйте минимальное разрешение 1 ячейки на метр.
map3D = occupancyMap3D(1);
Симулируйте рейс отображения в сценарии. Сохраните показания датчика лидара для каждого шага симуляции в массиве ячеек после удаления недопустимых точек. Вставьте облака точек в карту с помощью функции. Убедитесь, что вектор положения составляет возмещенный датчик.
setup(scene); ptIdx = 0; while scene.IsRunning ptIdx = ptIdx + 1; % Read the simulated lidar data from the scenario [isUpdated,lidarSampleTime,pt{ptIdx}] = read(lidar); if isUpdated % Get Lidar sensor's pose relative to ENU reference frame. sensorPose = getTransform(scene.TransformTree, "ENU","UAV/Lidar",lidarSampleTime); % Process the simulated Lidar pointcloud. ptc = pt{ptIdx}; ptOut{ptIdx} = removeInvalidPoints(pt{ptIdx}); % Construct the occupancy map using Lidar readings. insertPointCloud(map3D,[sensorPose(1:3,4)' tform2quat(sensorPose)],ptOut{ptIdx},500); figure(1) show3D(scene,"Time",lidarSampleTime,"FastUpdate",true,"Parent",ax); xlim([-15 80]); ylim([-15 80]); zlim([0 110]); view([-110 20]); refreshdata drawnow limitrate end % Show map building real time figure(2) show(map3D); view([-115 20]); axis equal advance(scene); updateSensors(scene); end
Выполните планирование движения
определяет вероятность наблюдательное значение 0,4 для незанятых ячеек и 0.7 для занятых ячеек. Задайте то же самое как пороги для карты заполнения.
map3D.FreeThreshold = 0.4; map3D.OccupiedThreshold = 0.7;
Используйте в качестве объекта пространства состояний для блока проверки допустимости, когда вектор состояния пространства состояний - то же самое как вектор положения.
ss = stateSpaceSE3([0 80;0 40;0 120;inf inf;inf inf;inf inf;inf inf]); sv = validatorOccupancyMap3D(ss); sv.Map = map3D; sv.ValidationDistance = 0.1;
Используйте оптимального планировщика, RRT* для планирования. Планирование продолжается даже после того, как цель достигнута и завершает работу только, когда предел итерации достигнут.
planner = plannerRRTStar(ss,sv); planner.MaxConnectionDistance = 20; planner.ContinueAfterGoalReached = true; planner.MaxIterations = 500;
Задайте пользовательскую целевую функцию, которая решает, что путь достигает цели, если Евклидово расстояние до цели ниже порога 1 метра.
planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3))<1); planner.GoalBias = 0.1;
Задайте запускаются и целевые положения и планируют путь.
start = [3 5 5 1 0 0 0]; goal = [60 10 65 1 0 0 0]; rng(1,"twister"); % For repeatable results [pthObj,solnInfo] = plan(planner,start,goal);
Просмотрите запланированный путь
close all close all hidden show(map3D) axis equal view([-115 20]) hold on scatter3(start(1,1),start(1,2),start(1,3),'g','filled') % draw start state scatter3(goal(1,1),goal(1,2),goal(1,3),'r','filled') % draw goal state plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),'r-','LineWidth',2) % draw path
