controllerPurePursuit

Create controller to follow set of waypoints

Description

The controllerPurePursuit System object™ creates a controller object used to make a car-like or differential-drive vehicle follow a set of waypoints. The object computes the linear velocity and curvature for the vehicle given the current pose. Successive calls to the object with updated poses provide updated velocity commands for the vehicle. Use the MaxCurvature and DesiredLinearVelocity properties to update the curvature and velocity based on the vehicle's performance.

The LookaheadDistance property computes a look-ahead point on the path, which is a local goal for the vehicle. The curvature command is computed based on this point. Changing LookaheadDistance has a significant impact on the performance of the algorithm. A higher look-ahead distance results in a smoother trajectory for the vehicle, but can cause the vehicle to cut corners along the path. A low look-ahead distance can result in oscillations in tracking the path, causing unstable behavior. For more information on the pure pursuit algorithm, see Pure Pursuit Controller.

To compute linear velocity and curvature control commands:

1. Create the controllerPurePursuit object and set its properties.

2. Call the object with arguments, as if it were a function.

To learn more about how System objects work, see What Are System Objects?

Creation

Syntax

controller = controllerPurePursuit

controller = controllerPurePursuit(Name,Value)

Description

controller = controllerPurePursuit creates a pure pursuit object that uses the pure pursuit algorithm to compute the linear velocity and curvature inputs for a car-like or differential-drive vehicle.

controller = controllerPurePursuit(Name,Value) creates a pure pursuit object with additional options specified by one or more Name,Value pairs. Name is the property name and Value is the corresponding value. Name must appear inside single quotes (' '). You can specify several name-value pair arguments in any order as Name1,Value1,...,NameN,ValueN. Properties not specified retain their default values.

Example: controller = controllerPurePursuit('DesiredLinearVelocity', 0.5)

Properties

DesiredLinearVelocity — Desired constant linear velocity

0.1 (default) | scalar in meters per second

Desired constant linear velocity, specified as a scalar in meters per second. The controller assumes that the vehicle drives at a constant linear velocity and that the computed curvature is independent of the linear velocity.

Data Types: double

LookaheadDistance — Look-ahead distance

1.0 (default) | scalar in meters

Look-ahead distance, specified as a scalar in meters. The look-ahead distance changes the response of the controller. A vehicle with a higher look-ahead distance produces smooth paths but takes larger turns at corners. A vehicle with a smaller look-ahead distance follows the path closely and takes sharp turns, but potentially creating oscillations in the path.

Data Types: double

MaxCurvature — Maximum curvature

1.0 (default) | positive real scalar

Maximum curvature, specified as a positive real scalar in 1/meters. The controller saturates the absolute curvature output at the specified value.

Data Types: double

Waypoints — Waypoints

[ ] (default) | n-by-2 array

Waypoints, specified as an n-by-2 array of [x y] pairs, where n is the number of waypoints. You can generate the waypoints from the mobileRobotPRM (Robotics System Toolbox) class or from another source.

Data Types: double

Usage

Syntax

[vel,curvature] = controller(pose)

[vel,curvature,lookaheadpoint] = controller(pose)

Description

[vel,curvature] = controller(pose) processes the vehicle position and orientation, pose, and outputs the linear velocity, vel, and curvature, curvature.

[vel,curvature,lookaheadpoint] = controller(pose) returns the look-ahead point, which is a location on the path used to compute the velocity and curvature commands. The controller object computes this location on the path using the LookaheadDistance property.

Input Arguments

pose — Position and orientation of vehicle

3-by-1 vector in the form [x y theta]

Position and orientation of vehicle, specified as a 3-by-1 vector in the form [x y theta]. The vehicle pose is an x and y position with angular orientation θ (in radians) measured from the x-axis.

Output Arguments

vel — Linear velocity

scalar in meters per second

Linear velocity, returned as a scalar in meters per second.

Data Types: double

curvature — Curvature

positive real scalar

Curvature, returned as a positive real scalar in 1/meters. You can calculate angular velocity by multiplying the curvature by the linear velocity.

Data Types: double

lookaheadpoint — Look-ahead point on path

[x y] vector

Look-ahead point on the path, returned as an [x y] vector. This value is calculated based on the LookaheadDistance property.

Object Functions

To use an object function, specify the System object as the first input argument. For example, to release system resources of a System object named obj, use this syntax:

release(obj)

Specific to controllerPurePursuit

info	Characteristic information about controllerPurePursuit object

Common to All System Objects

step	Run System object algorithm
release	Release resources and allow changes to System object property values and input characteristics
reset	Reset internal states of System object

Examples

Get Additional Pure Pursuit Object Information

Use the info method to get more information about a controllerPurePursuit object. The info function returns two fields, RobotPose and LookaheadPoint, which correspond to the current position and orientation of the robot and the point on the path used to compute outputs from the last call of the object.

Create a controllerPurePursuit object.

pp = controllerPurePursuit;

Assign waypoints.

pp.Waypoints = [0 0;1 1];

Compute control commands using the pp object with the initial pose [x y theta] given as the input.

[v,k] = pp([0 0 0]);

Get additional information.

s = info(pp)

s = struct with fields:
         RobotPose: [0 0 0]
    LookaheadPoint: [0.7071 0.7071]

Follow Waypoints Using Pure Pursuit Controller

Guide a differential drive vehicle along a series of waypoints using a pure pursuit controller.

To set up the physical constraints of the vehicle for the simulation, define the parameters of a differential drive vehicle. These parameters determine how fast the vehicle moves and how sharply it turns.

wheelRadius   = 0.05;           % [m]   Radius of each wheel
trackWidth    = 0.2;            % [m]   Distance between left and right wheels
maxWheelSpeed = 100*2*pi/60;    % [rad/s] Maximum wheel speed (100 RPM converted to rad/s)

To simulate the motion of the vehicle, create a differential drive kinematics model and specify the relevant parameters. For a differential drive robot, set the wheel speed range, track width, and wheel radius.

vehicle = differentialDriveKinematics(VehicleInputs="VehicleSpeedHeadingRate");
vehicle.WheelSpeedRange = [-1 1]*maxWheelSpeed; 
vehicle.TrackWidth = trackWidth;
vehicle.WheelRadius = wheelRadius;

To create the path for the vehicle, specify a series of waypoints as an array of [x y] coordinates. Choose a desired linear speed to set how quickly the vehicle travels between waypoints.

waypoints = [0 0;     % Start position [x y] in meters
              1 0;
              1 1.5;
              4 1.5;
              4 0;
              5 0];   % End position

desiredSpeed = 0.2;   % [m/s]

To enable the vehicle to follow the waypoints, set up the pure pursuit controller. Specify the waypoints, desired speed, maximum curvature, and lookahead distance. A smaller lookahead distance enables the vehicle to follow the path more closely, but can cause oscillations. A larger lookahead distance produces smoother paths, but can result in larger curvatures near corners.

controller = controllerPurePursuit;
controller.Waypoints = waypoints;                                              % Path waypoints to follow
controller.DesiredLinearVelocity = desiredSpeed;                               % Target speed [m/s]

% Calculate maximum angular velocity based on vehicle physical limits.
% For a differential drive vehicle, maximum angular velocity occurs when
% the wheels rotate in opposite directions at maximum speed.
maxAngularVelocity = wheelRadius*2*maxWheelSpeed/trackWidth;                   % rad/s

% Set maximum curvature the controller can command 
% (curvature = angular velocity / linear velocity). 
controller.MaxCurvature = maxAngularVelocity/desiredSpeed;                     % [1/m] 

% Set lookahead distance based on desired lookahead time.
lookaheadTime = 2;                                                             % [seconds]
controller.LookaheadDistance = lookaheadTime*controller.DesiredLinearVelocity; % [meters]

To prepare the simulation environment, set the simulation parameters, initialize the vehicle pose, and preallocate arrays for storing the vehicle pose and velocity commands.

% Define simulation parameters
sampleTime = 0.1;                        % Time step for simulation [seconds]
maxSimTime = 50;                         % Maximum simulation time [seconds]
simSteps = ceil(maxSimTime/sampleTime);  % Total number of simulation steps
timeVec = (0:simSteps-1)*sampleTime;


% Initialize poses
initialPose = [0 0 0];                   % Initial pose: [x y theta]
currentPose = initialPose;               % Set current pose to initial pose
poses = zeros(simSteps, 3);              % Preallocate array to record robot trajectory

% Initialize arrays to record velocity commands
vCmds = zeros(simSteps,1);               % Linear velocity commands
wCmds = zeros(simSteps,1);               % Angular velocity commands

% Reset the pure pursuit controller to clear any previous states from
% previous runs
reset(controller)

Create plots to visualize the trajectory and velocity commands of the vehicle during the simulation.

figure(Name="Pure Pursuit Waypoint Following With Velocity Commands");

% Subplots 1 and 3: Plot XY trajectory (pose)
subplot(2,2,[1 3])
hold on
plot(waypoints(:,1),waypoints(:,2),"k--o",LineWidth=1.5);               % Waypoints
vehicleMarker = plannerLineSpec.state(MarkerSize=5);                    % Vehicle marker
hVehicle = plot(initialPose(1),initialPose(2),vehicleMarker{:});       
hTraj = plot(initialPose(1),initialPose(2),plannerLineSpec.path{:});    % Trajectory
xlabel("X [m]") 
ylabel("Y [m]")
title('XY Pose (Trajectory)')
axis equal 
grid on

% Subplot 2: Linear velocity command
subplot(2,2,2)
hV = plot(0,0,"r"',LineWidth=1.5);
xlabel("Time [s]")
ylabel("Linear Velocity [m/s]")
title("Linear Velocity Command")
grid on
xlim([0 maxSimTime])
ylim([desiredSpeed-0.1 desiredSpeed+0.1])

% Subplot 4: Angular velocity command
subplot(2,2,4);
hW = plot(0,0,"g",LineWidth=1.5);
xlabel("Time [s]")
ylabel("Angular Velocity [rad/s]")
title("Angular Velocity Command")
grid on
xlim([0 maxSimTime])

To simulate the vehicle following the waypoints, run a closed-loop simulation. At each time step, compute the control commands, update the vehicle pose, and update the plot.

for idx = 1:simSteps
    % Get the linear velocity (vCmd) and curvature (kappaCmd) commands for the current pose.
    [vCmd,kappaCmd] = controller(currentPose);

    % Use the curvature command and velocity command to calculate the angular velocity command.
    wCmd = kappaCmd*vCmd;

    % Record the linear and angular velocity commands for later analysis or plotting.
    vCmds(idx) = vCmd;
    wCmds(idx) = wCmd;

    % Compute the velocity vector of the vehicle using the differential drive kinematic model.
    vel = derivative(vehicle,currentPose,[vCmd wCmd]);

    % Update the vehicle pose using Euler integration.
    currentPose = currentPose + vel'*sampleTime;
    poses(idx,:) = currentPose;

    % Update XY pose (merged plot).
    subplot(2,2,[1 3])
    set(hVehicle,XData=currentPose(1),YData=currentPose(2))
    set(hTraj,XData=poses(1:idx,1),YData=poses(1:idx,2))

    % Update linear velocity plot.
    subplot(2,2,2)
    set(hV,XData=timeVec(1:idx),YData=vCmds(1:idx))

    % Update angular velocity plot.
    subplot(2,2,4)
    set(hW,XData=timeVec(1:idx),YData=wCmds(1:idx))
    drawnow limitrate  % Update the animation

    % Stop the simulation if the vehicle is close to the last waypoint within a tolerance of 0.1 m.
    if norm(currentPose(1:2)-waypoints(end,:)) < 0.1
        break
    end
end

% Add legend to the  XY plot
subplot(2,2,[1 3])
legend("Waypoints","Vehicle Position","Trajectory")
hold off

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

Use in a MATLAB Function block is not supported.

For additional information about code generation for System objects, see System Objects in MATLAB Code Generation (MATLAB Coder)

Version History

Introduced in R2019b

R2026a: MaxAngularVelocity renamed to MaxCurvature

The MaxAngularVelocity property is now called MaxCurvature. The behavior remains the same.

R2026a: angvel output argument renamed to curvature

The angvel output argument is now called curvature. The behavior remains the same.