UAV Obstacle Avoidance in Simulink

Open Live Script

This model implements waypoint following along with obstacle avoidance on a UAV in a simulated scenario. The model takes a set of waypoints and uses the 3D VFH+ algorithm to provide an obstacle-free path.

Create UAV Scenario with Custom Lidar Sensor and Obstacles

Create Scenario

Create a UAV scenario and set its local origin.

Scenario = uavScenario("UpdateRate",100,"ReferenceLocation",[0 0 0]);

Add a marker to indicate the start pose of the UAV.

addMesh(Scenario,"cylinder",{[0 0 1] [0 .01]},[0 1 0]);

Define UAV Platform

Specify the initial position and orientation of the UAV in the north-east-down (NED) frame.

InitialPosition = [0 0 -7];
InitialOrientation = [0 0 0];

Create a UAV platform in the scenario.

platUAV = uavPlatform("UAV",Scenario, ...
                      "ReferenceFrame","NED", ...
                      "InitialPosition",InitialPosition, ...
                      "InitialOrientation",eul2quat(InitialOrientation));

Add a quadrotor mesh for visualization.

updateMesh(platUAV,"quadrotor",{1.2},[0 0 1],eul2tform([0 0 pi]));

Create and Mount Sensor Model

Specify the lidar resolution.

AzimuthResolution = 0.5;      
ElevationResolution = 2;

Specify the lidar range.

MaxRange = 7;
AzimuthLimits = [-179 179];
ElevationLimits = [-15 15];

Create a statistical sensor model to generate point clouds for the lidar sensor.

LidarModel = uavLidarPointCloudGenerator("UpdateRate",10, ...
                                         "MaxRange",MaxRange, ...
                                         "RangeAccuracy",3, ...
                                         "AzimuthResolution",AzimuthResolution, ...
                                         "ElevationResolution",ElevationResolution, ...
                                         "AzimuthLimits",AzimuthLimits, ...
                                         "ElevationLimits",ElevationLimits, ...                                       
                                         "HasOrganizedOutput",true);

Create a lidar sensor and mount the sensor on the quadrotor.

uavSensor("Lidar",platUAV,LidarModel, ...
          "MountingLocation",[0 0 -0.4], ...
          "MountingAngles",[0 0 180]);

Preview the scenario using the show3D function.

show3D(Scenario);

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

Add Obstacles to Scenario

Add cuboid obstacles, representing buildings, to the 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")

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 5 objects of type patch. These objects represent Start Position, Obstacles.

Model Overview

The model consists of these main components:

UAV scenario — Configures the scenario and visualizes the trajectory.
Waypoint following and obstacle avoidance — Implements waypoint following with obstacle avoidance.
Controller and plant — Position controller for the UAV.
Control Panel — Use this panel to enable or disable obstacle avoidance, as well as alter the lookahead distance for obstacle avoidance.

Open the model.

open_system("ObstacleAvoidanceDemo.slx");

UAV Scenario

The scenario blocks configure the scenario and visualize the obstacles, trajectory, and the lidar point cloud data.

This subsystem contains these blocks:

UAV Scenario Configuration — Configures the scenario blocks to use the generated scenario for simulation.
UAV Scenario Motion Read — Reads the current UAV state from the scenario.
UAV Scenario Lidar — Reads the point cloud data from the scenario.
UAV Scenario Motion Write — Updates the new UAV state.
UAV Scenario Scope — Visualizes the UAV trajectory and lidar point cloud data.

Waypoint Following and Obstacle Avoidance

The Waypoint following and obstacle avoidance subsystem finds the obstacle-free desired position and the desired yaw according to the current UAV state and point cloud data.

This subsystem includes these blocks and subsystems:

Waypoint Follower — Computes a lookahead point for the UAV in the direction of the next waypoint.
Obstacle Avoidance — Uses the 3D VFH+ algorithm to calculate the obstacle-free direction and yaw for a collision-free flight, and updates the lookahead point computed by the Waypoint Follower block.
Conversion — This subsystem controls the frequency at which obstacle avoidance is used during the flight, and other data type and transform conversions.
Lookahead Distance — Constant block, the value of which is multiplied by the unit vector in the desired direction, and then added to the current UAV position to compute the desired position.
Enable Obstacle Avoidance — This subsystem enables or disables obstacle avoidance.
Waypoints — The set of waypoints through which the UAV is expected to fly.

Specify the waypoints for the UAV.

Waypoints = [InitialPosition; 0 20 -7; 20 20 -7; 20 0 -7];

Add markers to indicate the 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"])

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 9 objects of type patch, line. These objects represent Start Position, Obstacles, Waypoints, Direct Path.

Controller and plant

The Controller and plant subsystem generates the control commands and updates the UAV state based on the lookahead point.

This subsystem includes these blocks:

Controller — This subsystem computes the control commands (roll, pitch, yaw, and thrust) to move the UAV towards the desired position. It uses multiple PID loops to implement position control.
Quadrotor Plant — This Guidance Model block updates the UAV state using the control commands.
Conversion — This subsystem extracts the position and orientation from the UAV state, and performs data and coordinate transforms for visualization.

Specify the controller parameters. These parameters are based on a hit-and-trial approach, and can be tuned for a smoother flight.

% 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;

Specify gravity, drone mass, and sample time for the controller and plant blocks.

UAVSampleTime = 0.001;
Gravity = 9.81;
DroneMass = 0.1;

Control Panel

The switch enables or disables the updates to the lookahead point from the Obstacle Avoidance block.

The slider updates the lookahead distance used to compute the lookahead point.

At greater lookahead distances, UAV flight is faster, but has greater risk of colliding with an obstacle.
At lower values, the flight is slower, but has a lower risk of colliding with an obstacle.

Simulate Model

Configure and run the model, and observe the motion of the UAV.

The UAV flies through the waypoints while avoiding obstacles, and then the simulation stops.
Alter the lookahead distance to change the UAV speed.
Change the parameters of the Obstacle Avoidance block and note the change in the flight path.

out = sim("ObstacleAvoidanceDemo.slx");

Figure UAV Scenario Scope contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 10 objects of type patch, scatter, line.

Visualize Obstacle-Free UAV trajectory

Plot the actual UAV trajectory and the waypoints to show the effect of obstacle avoidance on the UAV flight.

hold on
points = squeeze(out.trajectoryPoints(1,:,:))';
plot3(points(:,2),points(:,1),-points(:,3),"-r");
legend(["Start Position","Obstacles","","","Waypoints","","","Direct Path","UAV Trajectory"])

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 10 objects of type patch, line. These objects represent Start Position, Obstacles, Waypoints, Direct Path, UAV Trajectory.