Main Content

Avoid Moving Obstacles Based on Radar Detections

This example demonstrates how to avoid collision with moving obstacles based on the velocity obstacle concept [1].The uavScenario used in this example is based on the "Simulate Radar Sensor Mounted on UAV" example, which shows how to generate track detections of moving UAVs close to the ego vehicle.

Setup testing scenario

The testing scenario has two UAVs, of which one ego UAV carries a radar sensor and the other UAV acts as a moving obstacle. The radar sensor on the ego vehicle generates target tracks containing target position and velocity information based on detections. The ego UAV can execute avoidance maneuver based on detection information.

rng(0) % For repeatable results.

% Create a scenario that runs for 10 seconds.
s = uavScenario("StopTime",30,"HistoryBufferSize",200);

% Create a fixed-wing UAV that moves from [30 0 0] to [20 10 0].
obstacleUAV = uavPlatform("Obstacle",s,"Trajectory",waypointTrajectory([30 0 0; 0 30 0],"TimeOfArrival",[0 30]));
updateMesh(obstacleUAV,"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.
radarSensorObject = 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(radarSensorObject),"MountingAngles", [0 0 0]);

Simulate the scenario without obstacle avoidance

In the scenario, if you let the ego UAV keep flying along its initial direction, it will eventually collide with the fixed wing UAV as shown in the simulation.

% Set up the 3D view of the scenario.
[ax, plotFrames] = show3D(s);
% Represent the ego UAV as a green marker.
egoLine = plot3(0,0,0,"Marker","diamond","MarkerFaceColor","green","Parent",plotFrames.EgoVehicle.BodyFrame);
% Represent the other UAV as a red marker.
obstacleUAVLine = plot3(0,0,0,"Marker","diamond","MarkerFaceColor","red","Parent",plotFrames.Obstacle.BodyFrame);
legend([egoLine obstacleUAVLine], ["Ego", "Obstacle"])
xlim([-5,35]);
ylim([-5,35]);

% Show the radar field of view.
radarDirection = hgtransform("Parent",plotFrames.EgoVehicle.BodyFrame,"Matrix",eye(4));
coverageAngles = linspace(-radarSensorObject.FieldOfView(1)/360*pi,radarSensorObject.FieldOfView(1)/360*pi,128);
patch("XData",[0 radarSensorObject.RangeLimits(2)*cos(coverageAngles) 0], ...
    "YData",[0 radarSensorObject.RangeLimits(2)*sin(coverageAngles) 0], ...
    FaceAlpha=0.3,Parent=radarDirection);

% 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

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 5 objects of type patch, line.

Configure obstacle avoidance behavior

To avoid such collisions, you need to detect the potential collision using radar sensor readings and generate avoidance maneuver using a velocity obstacle approach. First you setup the avoidance parameter that controls the safety radius of the avoidance algorithm and the time horizon at which the avoidance algorithm will start working.

% 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

Simulate scenario and test obstacle avoidance behavior

In this section, you simulate the UAV scenario to test the obstacle avoidance algorithm. The avoidance algorithm controls the motion of the ego multirotor according to the desired velocity output for collision avoidance. The avoidance algorithm tries to find a collision free direction based on the velocities of both the ego vehicle and the target vehicle, and then direct the ego vehicle towards the collision free direction while maintaining the same velocity magnitude.

% Set up the 3D view of the scenario.
[ax,plotFrames] = show3D(s);
% Represent the ego UAV as a green marker.
egoLine = plot3(0,0,0,"Marker","diamond","MarkerFaceColor","green","Parent",plotFrames.EgoVehicle.BodyFrame);
% Represent the other UAV as a red marker.
obstacleUAVLine = plot3(0,0,0,"Marker","diamond","MarkerEdgeColor","red","MarkerFaceColor", "none", "Parent",plotFrames.Obstacle.BodyFrame);
xlim([-5,35]);
ylim([-5,35]);
zlim([-20,20]);

% Represent the radar detection.
detectionLine = plot3(nan,nan,nan,"Marker","o","MarkerFaceColor","blue", "MarkerSize", 4, "Parent",plotFrames.NED);
legend([egoLine obstacleUAVLine detectionLine], ["Ego", "Obstacle", "Detection"])

% Show the radar field of view.
radarDirection = hgtransform("Parent",plotFrames.EgoVehicle.BodyFrame,"Matrix",eye(4));
coverageAngles = linspace(-radarSensorObject.FieldOfView(1)/360*pi,radarSensorObject.FieldOfView(1)/360*pi,128);
patch("XData",[0 radarSensorObject.RangeLimits(2)*cos(coverageAngles) 0], ...
    "YData",[0 radarSensorObject.RangeLimits(2)*sin(coverageAngles) 0], ...
    FaceAlpha=0.3,Parent=radarDirection)

% 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];
        detectionLine.XData = obstacleStates(1,:);
        detectionLine.YData = obstacleStates(3,:);
        detectionLine.ZData = obstacleStates(5,:);
        [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

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 6 objects of type patch, line.

Reference

[1] Fiorini P, Shiller Z. Motion Planning in Dynamic Environments Using Velocity Obstacles. The International Journal of Robotics Research. 1998;17(7):760-772. doi:10.1177/027836499801700706