navPathControl

Path representing control-based kinematic trajectory

Since R2021b

Description

The navPathControl object stores paths that are typically created by control-based path planners like the plannerControlRRT object. The navPathControl object represents paths as a sequence of states, controls, durations, and targets. This object associates each path with a specific state propagator, which propagates the control commands to determine the resulting states.

This object specifies states and targets in the path in the state space of propagator. Controls are outputs from a controller that are used to update your systems state during propagation. This object applies each control for an associated duration. Controls can be reference signals or direct inputs to an integrator depending on your system design.

Creation

Syntax

pathObj = navPathControl(propagator)

pathObj = navPathControl(propagator,states,controls,targets,durations)

Description

pathObj = navPathControl(propagator) creates a path object with the specified state propagator. The propagator argument specifies the StatePropagator

pathObj = navPathControl(propagator,states,controls,targets,durations) initializes the path with a sequence of specified states, controls, targets, and durations. The input states must have one more row than the other input vectors and matrices.

Properties

StatePropagator — State propagator

object of subclass of nav.StatePropagator

State propagator, specified as an object of a subclass of nav.StatePropagator. For example, the mobileRobotPropagator object represents the state space and kinematic control behavior for different mobile robot vehicle models.

Data Types: double

States — Series of states for path

[] (default) | n-by-m matrix

Series of states for the path, specified as an n-by-m matrix. n is the number of points in the path. m is the dimension of the state vector.

You can specify this property at object creation by using the states argument.

Data Types: double

Controls — Control input for each state

[] (default) | (n–1)-by-m matrix

Control input for each state, specified as an (n–1)-by-m matrix. n is the number of points in the path. m is the dimension of the state vector.

You can specify this property at object creation by using the controls argument.

Data Types: double

TargetStates — Target state for each state in path

[] (default) | (n–1)-by-m matrix

Target state for each state in the path, specified as an (n–1)-by-m matrix. n is the number of points in the path. m is the dimension of the state vector.

You can specify this property at object creation by using the targets argument.

Data Types: double

Durations — Duration of each control input

[] (default) | (n–1)-element vector in seconds

Duration of each control input, specified as an (n–1)-element vector in seconds. n is the number of points in the path.

You can specify this property at object creation by using the durations argument.

Data Types: double

NumStates — Number of states in path

0 (default) | nonnegative integer

Number of states in the path, specified as a positive integer.

Data Types: double

NumSegments — Number of segments between states

0 (default) | nonnegative integer

Number of segments between states in the path, specified as a positive integer, which must be one less than the number of states.

Data Types: double

Object Functions

append	Add states to end of path
interpolate	Interpolate path based on propagator step size
pathDuration	Total elapsed duration of control path

Examples

Plan Kinodynamic Path with Controls for Mobile Robot

Plan control paths for a bicycle kinematic model with the mobileRobotPropagator object. Specify a map for the environment, set state bounds, and define a start and goal location. Plan a path using the control-based RRT algorithm, which uses a state propagator for planning motion and the required control commands.

Set State and State Propagator Parameters

Load a ternary map matrix and create an occupancyMap object. Create the state propagator using the map. By default, the state propagator uses a bicycle kinematic model.

load('exampleMaps','ternaryMap')
map = occupancyMap(ternaryMap,10);

propagator = mobileRobotPropagator(Environment=map); % Bicycle model

Set the state bounds on the state space based on the map world limits.

propagator.StateSpace.StateBounds(1:2,:) = ...
                    [map.XWorldLimits; map.YWorldLimits];

Plan Path

Create the path planner from the state propagator.

planner = plannerControlRRT(propagator);

Specify the start and goal states.

start = [10 15 0];
goal  = [40 30 0];

Plan a path between the states. For repeatable results, reset the random number generator before planning. The plan function outputs a navPathControl object, which contains the states, control commands, and durations.

rng("default")
path = plan(planner,start,goal)

path = 
  navPathControl with properties:

    StatePropagator: [1×1 mobileRobotPropagator]
             States: [192×3 double]
           Controls: [191×2 double]
          Durations: [191×1 double]
       TargetStates: [191×3 double]
          NumStates: 192
        NumSegments: 191

Visualize Results

Visualize the map and plot the path states.

show(map)
hold on
plot(start(1),start(2),"rx")
plot(goal(1),goal(2),"go")
plot(path.States(:,1),path.States(:,2),"b")
hold off

Display the [v psi] control inputs of forward velocity and steering angle.

plot(path.Controls)
ylim([-1 1])
legend(["Velocity (m/s)","Steering Angle (rad)"])

Combine Two Kinodynamic Paths with Controls for Mobile Robot

Set State and State Propagator Parameters

Load a ternary map matrix and create an occupancyMap object.

load("exampleMaps","ternaryMap")
map = occupancyMap(ternaryMap,10);

Create a state propagator for a bicycle kinematic model using the map.

propagator = mobileRobotPropagator(Environment=map);

Set the state bounds on the state space based on the map world limits.

propagator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; 
                                            map.YWorldLimits];

Plan Path

Create the path planner from the state propagator.

planner = plannerControlRRT(propagator);

Specify the start and goal states.

start = [10 15 0];
goal  = [40 30 0];

Plan a path between the states. For repeatable results, reset the random number generator before planning.

rng("default")
path = plan(planner,start,goal);

Check the total elapsed duration of the control path.

pathDuration(path)

ans = 
102.4000

Interpolate the path to the control step size of the propagator.

interpolate(path)

Visualize the path.

figure
show(map)
hold on
plot(start(1),start(2),"rx")
plot(goal(1),goal(2),"go")
plot(path.States(:,1),path.States(:,2),"b")
hold off

Plan Return Path

Plan a second path from the end of the previous path location back to the start.

path2 = plan(planner,path.States(end,:),start);

Check the total elapsed duration of the second path.

pathDuration(path2)

ans = 
100.3000

Visualize the path.

figure
show(map)
hold on
plot(start(1),start(2),"rx")
plot(goal(1),goal(2),"go")
plot(path2.States(:,1),path2.States(:,2),"m")
hold off

Combine Paths

Extract the sequence of motions from the second path.

states = path2.States(2:end,:);
controls = path2.Controls;
targets = path2.TargetStates;
durations = path2.Durations;

Append this sequence to the end of the first path.

append(path,states,controls,targets,durations)

Interpolate the new segments in the original path.

interpolate(path)

Check the total elapsed duration of the final path.

pathDuration(path)

ans = 
102.4000

Visualize the path.

figure
show(map)
hold on
plot(start(1),start(2),"rx")
plot(goal(1),goal(2),"go")
plot(path.States(:,1),path.States(:,2),"b")
hold off

Version History

Introduced in R2021b