Этот пример демонстрирует, как избежать столкновения с движущимися препятствиями на основе концепции [1] препятствия скорости.The uavScenario используемый в этом примере на основе, "Симулируют Радарный Датчик, Смонтированный На UAV" пример, который показывает, как сгенерировать обнаружения дорожки движущихся БПЛА близко к автомобилю, оборудованному датчиком.
Сценарий тестирования имеет два БПЛА, из которых одно эго UAV несет радарный датчик и другие действия UAV как движущееся препятствие. Радарный датчик на автомобиле, оборудованном датчиком генерирует целевые дорожки, содержащие целевую информацию о положении и скорости на основе обнаружений. Эго UAV может выполнить маневр предотвращения на основе информации об обнаружении.
rng(0) % For repeatable results. % Create a scenario that runs for 10 seconds. s = uavScenario("StopTime",30,"HistoryBufferSize",200); % Create a fixed-wing target that moves from [30 0 0] to [20 10 0]. target = uavPlatform("Target",s,"Trajectory",waypointTrajectory([30 0 0; 0 30 0],"TimeOfArrival",[0 30])); updateMesh(target,"fixedwing",{1},[1 0 0],eul2tform([0 0 pi])); % Create a quadrotor as the ego vehicle. egoVelocity = [1 1 0]; egoMultirotor = uavPlatform("EgoVehicle",s,"InitialVelocity", egoVelocity); updateMesh(egoMultirotor,"quadrotor",{1},[0 1 0],eul2tform([0 0 pi])); % Create a radar sensor and set up its properties. radarSensor = radarDataGenerator("no scanning","SensorIndex",1,"UpdateRate",10,... "FieldOfView",[120 80],... "HasElevation", true,... "ElevationResolution", 3,... "AzimuthResolution", 1, ... "RangeResolution", 10, ... meters "RangeRateResolution",3,... "RangeLimits", [0 750],... "TargetReportFormat","Tracks",... "TrackCoordinates",'Scenario',... "HasINS", true,... "HasFalseAlarms",true,... "FalseAlarmRate",1e-5,... "HasRangeRate",true,... "FalseAlarmRate", 1e-7); % Mount the radar sensor on the ego vehicle. ExampleHelperUAVRadar inherits % from the uav.SensorAdaptor class. radar = uavSensor("Radar",egoMultirotor,ExampleHelperUAVRadar(radarSensor),"MountingAngles", [0 0 0]);
В сценарии, если вы позволяете эго, UAV продолжает лететь вдоль его начального направления, это в конечном счете столкнется с фиксированным крылом UAV как показано в симуляции.
% Set up the 3D view of the scenario. [ax, plotFrames] = show3D(s); % Represent the ego UAV as a green marker. plot3(0,0,0,"Marker","diamond","MarkerFaceColor","green","Parent",plotFrames.EgoVehicle.BodyFrame); % Represent the other UAV as a red marker. plot3(0,0,0,"Marker","diamond","MarkerFaceColor","red","Parent",plotFrames.Target.BodyFrame); xlim([-5,35]); ylim([-5,35]); % Start simulation. setup(s); while advance(s) % Move ego UAV along its velocity vector. motion = read(egoMultirotor); motion(1:2) = motion(1:2) + motion(4:5)/s.UpdateRate; move(egoMultirotor, motion); show3D(s,"FastUpdate", true,"Parent",ax); pause(0.02); end
Чтобы избежать таких столкновений, необходимо обнаружить потенциальное столкновение с помощью радарных показаний датчика и сгенерировать маневр предотвращения с помощью скоростного подхода препятствия. Сначала вы устанавливаете параметр предотвращения, который управляет радиусом безопасности алгоритма предотвращения и период времени, в который алгоритм предотвращения начнет работать.
% Set up safety radius radiusUAV = 0.5; % UAV radius (m) radiusObs = 2; % Obstacle radius (m) safetyDist = 2; % Collision safety radius (m) - same for all obstacles effectiveRadius = radiusUAV+radiusObs+safetyDist; % Time Horizon (time until collision to perform avoidance maneuver) Th = 10; % sec
В этом разделе вы симулируете сценарий UAV, чтобы протестировать алгоритм предотвращения препятствия. Алгоритм предотвращения управляет движением мультиротора эго согласно желаемой скорости выход для предотвращения столкновения. Алгоритм предотвращения пытается найти столкновение свободным направлением на основе скоростей и автомобиля, оборудованного датчиком и целевого транспортного средства, и затем направить автомобиль, оборудованный датчиком к столкновению свободное направление при поддержании той же величины скорости.
% Set up the 3D view of the scenario. [ax,plotFrames] = show3D(s); % Represent the ego UAV as a green marker. plot3(0,0,0,"Marker","diamond","MarkerFaceColor","green","Parent",plotFrames.EgoVehicle.BodyFrame); % Represent the other UAV as a red marker. plot3(0,0,0,"Marker","diamond","MarkerFaceColor","red","Parent",plotFrames.Target.BodyFrame); xlim([-5,35]); ylim([-5,35]); % Start simulation. restart(s); setup(s); desVel = egoVelocity(1:2); while advance(s) % Move the ego UAV along its velocity vector. motion = read(egoMultirotor); motion(1:2) = motion(1:2) + desVel/s.UpdateRate; motion(4:5) = desVel; move(egoMultirotor, motion); % Update sensor readings and read data. updateSensors(s); % Obtain detections from the radar. [isUpdated,time,confTracks,numTracks,config] = read(radar); % Perform obstacle avoidance maneuver by adjusting ego vehicle's % velocity desVel = egoVelocity(1:2); if numTracks > 0 % Detect imminent collision using % exampleHelperDetectImminentCollision function. obstacleStates = [confTracks.State]; [isOnCollisionPath, VOFrontAngle, VOBackAngle, VOMinAngle, VOMaxAngle] ... = exampleHelperDetectImminentCollision(motion(1:2)', motion(4:5)', obstacleStates([1,3],:), ... obstacleStates([2,4],:),effectiveRadius,Th); % Find obstacles that are on imminent collision path. collisionObsIdx = find(isOnCollisionPath); if any(isOnCollisionPath) % Compute the velocity required to avoid collision using the % exampleHelperCVHeuristic2D function. desVel = exampleHelperCVHeuristic2D(norm(egoVelocity), ... motion(1:2)', ... motion(4:5)', ... obstacleStates([1,3],collisionObsIdx), ... obstacleStates([2,4],collisionObsIdx), ... effectiveRadius, ... VOFrontAngle(collisionObsIdx), ... VOBackAngle(collisionObsIdx), ... VOMinAngle(collisionObsIdx), ... VOMaxAngle(collisionObsIdx), Th); desVel = desVel'; end end show3D(s,"FastUpdate", true,"Parent",ax); pause(0.02); end
[1] Fiorini P, Шиллер З. Моушн Плэннинг в Динамических средах Используя Препятствия Скорости. Международный журнал Исследования Робототехники. 1998; 17 (7):760-772. doi:10.1177/027836499801700706
