Load the Map and Simulink Model

Load the occupancy map, which defines the map limits and obstacles within the map.exampleMaps.matcontain multiple maps includingsimpleMap, which this example uses.

loadexampleMaps.mat

Specify a start and end locaiton within the map.

startLoc = [5 5]; goalLoc = [20 20];

Model Overview

Open the Simulink model.

open_system('pathPlanningSimulinkModel.slx')

The model is composed of three primary parts:

Planning

ThePlannerMATLAB® function block uses themobileRobotPRMpath planner and takes a start location, goal location, and map as inputs. The blocks outputs an array of waypoints that the robot follows. The planned waypoints are used downstream by thePure Pursuitcontroller block.

Control

Pure Pursuit

ThePure Pursuitcontroller block generates the linear velocity and angular velocity commands based on the waypoints and the current pose of the robot.

Check if Goal is Reached

TheCheck Distance to Goalsubsystem calculates the current distance to the goal and if it is within a threshold, the simulation stops.

Plant Model

TheDifferential Drive Kinematic Modelblock creates a vehicle model to simulate simplified vehicle kinematics. The block takes linear and angular velocities as command inputs from thePure Pursuitcontroller block, and outputs the current position and velocity states.

Run the Model

simulation = sim('pathPlanningSimulinkModel.slx');

Visualize The Motion of Robot

After simulating the model, visualize the robot driving the obstacle-free path in the map.

map = binaryOccupancyMap(simpleMap); robotPose = simulation.Pose; thetaIdx = 3;% Translationxyz = robotPose; xyz(:, thetaIdx) = 0;% Rotation in XYZ euler anglesθ= robotPose (:, thetaIdx);thetaEuler = zeros(size(robotPose, 1), 3 * size(theta, 2)); thetaEuler(:, end) = theta;% Plot the robot poses at every 10th step.fork = 1:10:size(xyz, 1) show(map) holdon;% Plot the start location.plotTransforms([startLoc, 0], eul2quat([0, 0, 0])) text(startLoc(1), startLoc(2), 2,'Start');% Plot the goal location.plotTransforms([goalLoc, 0], eul2quat([0, 0, 0])) text(goalLoc(1), goalLoc(2), 2,'Goal');% Plot the xy-locations.plot(robotPose(:, 1), robotPose(:, 2),'-b')% Plot the robot pose as it traverses the path.quat = eul2quat(thetaEuler(k, :),'xyz'); plotTransforms(xyz(k,:), quat,'MeshFilePath',。..'groundvehicle.stl'); light; drawnow; holdoff;end

© Copyright 2019 The MathWorks, Inc.