Mapping with Known Poses

This example shows how to create a map of an environment using range sensor readings and robot poses for a differential drive robot. You create create a map from range sensor readings that are simulated using the rangeSensor object. The differentialDriveKinematics motion model simulates driving the robot around the room based on velocity commands. The rangeSensor gives range readings based on the pose of the robot as it follows the path.

Reference Map and Figures

Load a set of example binary occupancy grids from exampleMaps, including simpleMap, which this example uses.

load exampleMaps.mat

Create the reference binary occupancy map using simpleMap with a resolution of 1. Show the figure and and save the handle of the figure.

Initialize Motion Model and Controller

Create a differential-drive kinematic motion model. The motion model represents the motion of the simulated differential-drive robot. This model takes left and right wheels speeds or linear and angular velocities for the robot heading. For this example, use the vehicle speed and heading rate for the VehicleInputs.

Create a pure pursuit controller. This controller generates the velocity inputs for the simulated robot to follow a desired path. Set your desired linear velocity and maximum angular velocity, specified in meters per second and radians per second respectively.

Set Up Range Sensor

Create a sensor with a max range of 10 meters. This sensor simulates range readings based on a given pose and map. The reference map is used with this range sensor to simulate collecting sensor readings in an unknown environment.

sensor = rangeSensor;
sensor.Range = [0,10];

Create the Planned Path

Create a path to drive through the map for gathering range sensor readings.

path = [4 6; 6.5 12.5; 4 22; 12 14; 22 22; 16 12; 20 10; 14 6; 22 3];

Plot the path on the reference map figure.

figure(refFigure);
hold on
plot(path(:,1),path(:,2), 'o-');
hold off

Set the path as the waypoints of the pure pursuit controller.

controller.Waypoints = path;

Follow Path and Map Environment

Set the initial pose and final goal location based on the path. Create global variables for storing the current pose and an index for tracking the iterations.