Эта модель реализует waypoint после наряду с предотвращением препятствия на UAV в симулированном сценарии. Модель берет набор waypoints и использует 3D VFH + алгоритм, чтобы обеспечить путь без препятствий.
Создайте сценарий UAV и установите его локальный источник.
Scenario = uavScenario("UpdateRate",100,"ReferenceLocation",[0 0 0]);
Добавьте маркер, чтобы указать на положение запуска UAV.
addMesh(Scenario,"cylinder",{[0 0 1] [0 .01]},[0 1 0]);Задайте исходное положение и ориентацию UAV на северо-востоке вниз (NED) система координат.
InitialPosition = [0 0 -7]; InitialOrientation = [0 0 0];
Создайте платформу UAV в сценарии.
platUAV = uavPlatform("UAV",Scenario, ... "ReferenceFrame","NED", ... "InitialPosition",InitialPosition, ... "InitialOrientation",eul2quat(InitialOrientation));
Добавьте mesh quadrotor для визуализации.
updateMesh(platUAV,"quadrotor",{1.2},[0 0 1],eul2tform([0 0 pi]));Задайте разрешение лидара.
AzimuthResolution = 0.5; ElevationResolution = 2;
Укажите диапазон лидара.
MaxRange = 7; AzimuthLimits = [-179 179]; ElevationLimits = [-15 15];
Создайте статистическую модель датчика, чтобы сгенерировать облака точек для датчика лидара.
LidarModel = uavLidarPointCloudGenerator("UpdateRate",10, ... "MaxRange",MaxRange, ... "RangeAccuracy",3, ... "AzimuthResolution",AzimuthResolution, ... "ElevationResolution",ElevationResolution, ... "AzimuthLimits",AzimuthLimits, ... "ElevationLimits",ElevationLimits, ... "HasOrganizedOutput",true);
Создайте датчик лидара и смонтируйте датчик на quadrotor.
uavSensor("Lidar",platUAV,LidarModel, ... "MountingLocation",[0 0 -0.4], ... "MountingAngles",[0 0 180]);
Предварительно просмотрите сценарий с помощью show3D функция.
show3D(Scenario);
Добавьте препятствия кубоида, представляя создания, к сценарию.
ObstaclePositions = [10 0; 20 10; 10 20]; % Locations of the obstacles ObstacleHeight = 15; % Height of the obstacles ObstaclesWidth = 3; % Width of the obstacles for i = 1:size(ObstaclePositions,1) addMesh(Scenario,"polygon", ... {[ObstaclePositions(i,1)-ObstaclesWidth/2 ObstaclePositions(i,2)-ObstaclesWidth/2; ... ObstaclePositions(i,1)+ObstaclesWidth/2 ObstaclePositions(i,2)-ObstaclesWidth/2; ... ObstaclePositions(i,1)+ObstaclesWidth/2 ObstaclePositions(i,2)+ObstaclesWidth/2; ... ObstaclePositions(i,1)-ObstaclesWidth/2 ObstaclePositions(i,2)+ObstaclesWidth/2], ... [0 ObstacleHeight]},0.651*ones(1,3)); end show3D(Scenario); legend("Start Position","Obstacles")
Модель состоит из этих основных компонентов:
UAV scenario — Конфигурирует сценарий и визуализирует траекторию.
Waypoint following and obstacle avoidance — Реализации waypoint после с предотвращением препятствия.
Controller and plant — Контроллер положения для UAV.
Control Panel — Используйте эту панель, чтобы включить или отключить предотвращение препятствия, а также изменить предварительное расстояние для предотвращения препятствия.
Откройте модель.
open_system("ObstacleAvoidanceDemo.slx");
Блоки сценария конфигурируют сценарий и визуализируют препятствия, траекторию и данные об облаке точек лидара.
Эта подсистема содержит эти блоки:
Настройка Сценария UAV — Конфигурирует блоки сценария, чтобы использовать сгенерированный сценарий для симуляции.
Чтение Движения Сценария UAV — Чтения текущий UAV утверждают из сценария.
Лидар Сценария UAV — Считывает данные об облаке точек из сценария.
Запись Движения Сценария UAV — Обновления новое состояние UAV.
Осциллограф Сценария UAV — Визуализирует траекторию UAV и данные об облаке точек лидара.
Waypoint following and obstacle avoidance подсистема находит желаемое положение без препятствий и желаемое рыскание согласно текущему состоянию UAV и данным об облаке точек.
Эта подсистема включает эти блоки и подсистемы:
Последователь Waypoint — Вычисляет предварительную точку для UAV в направлении следующего waypoint.
Предотвращение препятствия — Использование 3D VFH + алгоритм, чтобы вычислить направление без препятствий и рыскание для рейса без коллизий и обновлений предварительная точка, вычисленная блоком Waypoint Follower.
Преобразование — Эта подсистема управляет частотой, на которой предотвращение препятствия используется во время рейса и другого типа данных, и преобразуйте преобразования.
Предварительный блок Distance — Constant, значение которого умножается на единичный вектор в желаемом направлении, и затем добавляется к текущему положению UAV, чтобы вычислить желаемое положение.
Включите Предотвращение Препятствия — Эта подсистема включает или отключает предотвращение препятствия.
'Waypoints' Набор waypoints, через который UAV, как ожидают, полетит.
Задайте waypoints для UAV.
Waypoints = [InitialPosition; 0 20 -7; 20 20 -7; 20 0 -7];
Добавьте маркеры, чтобы указать на waypoints.
for i = 2:size(Waypoints,1) addMesh(Scenario,"cylinder",{[Waypoints(i,2) Waypoints(i,1) 1] [0 0.1]},[1 0 0]); end show3D(Scenario); hold on plot3([InitialPosition(1,2); Waypoints(:,2)],[InitialPosition(1,2); Waypoints(:,1)],[-InitialPosition(1,3); -Waypoints(:,3)],"-g") legend(["Start Position","Obstacles","","","Waypoints","","","Direct Path"])
Controller and plant подсистема генерирует команды управления и обновляет состояние UAV на основе предварительной точки.
Эта подсистема включает эти блоки:
Controller — Эта подсистема вычисляет команды управления (крен, тангаж, рыскание и тяга), чтобы переместить UAV к желаемой позиции. Это использует несколько циклов ПИДа, чтобы реализовать управление положением.
Quadrotor Plant — Этот блок Model Руководства обновляет состояние UAV использование команд управления.
Преобразование — Эта подсистема извлекает положение и ориентацию от состояния UAV, и выполняет данные, и координата преобразовывает для визуализации.
Задайте параметры контроллера. Эти параметры основаны на подходе хита-и-испытания и могут быть настроены для более сглаженного рейса.
% Proportional Gains Px = 6; Py = 6; Pz = 6.5; % Derivative Gains Dx = 1.5; Dy = 1.5; Dz = 2.5; % Integral Gains Ix = 0; Iy = 0; Iz = 0; % Filter Coefficients Nx = 10; Ny = 10; Nz = 14.4947065605712;
Задайте силу тяжести, массу беспилотника и шаг расчета для блоков объекта и контроллера.
UAVSampleTime = 0.001; Gravity = 9.81; DroneMass = 0.1;
Переключатель включает или отключает обновления предварительной точки от блока Obstacle Avoidance.
Ползунок обновляется, предварительное расстояние использовалось для расчета предварительной точки.
На больших предварительных расстояниях рейс UAV быстрее, но имеет больший риск столкновения с препятствием.
В нижних значениях рейс медленнее, но имеет более низкий риск столкновения с препятствием.
Сконфигурируйте и запустите модель и наблюдайте движение UAV.
UAV летит через waypoints при предотвращении препятствий, и затем остановок симуляции.
Измените предварительное расстояние, чтобы изменить скорость UAV.
Измените параметры блока Obstacle Avoidance и обратите внимание на изменения в угле тангажа.
out = sim("ObstacleAvoidanceDemo.slx");
Постройте фактическую траекторию UAV и waypoints, чтобы показать эффект предотвращения препятствия на рейсе UAV.
hold on points = squeeze(out.trajectoryPoints(1,:,:))'; plot3(points(:,2),points(:,1),-points(:,3),"-r"); legend(["Start Position","Obstacles","","","Waypoints","","","Direct Path","UAV Trajectory"])
