Generate control command and duration
Create Path Representing Control-Based Kinematic Trajectory
Create a state propagator and specify the distance metric for estimating propagation cost.
propagator = mobileRobotPropagator(DistanceEstimator="dubins");
Create a Dubins state space.
dubinsSpace = stateSpaceDubins([0 25; 0 25; -pi pi]);
Update the state space of the state propagator using the created state space.
propagator.StateSpace = dubinsSpace; setup(propagator)
navPath object based on multiple waypoints in a Dubins space.
path = navPath(dubinsSpace); waypoints = [8 10 pi/2; 7 14 pi/4; 10 17 pi/2; 10 10 -pi]; append(path,waypoints)
Interpolate that path so that it contains exactly 250 points.
numStates = 250; interpolate(path,numStates)
Extract the sequence of motions from the path.
q1 = path.States(1:end-1,:); % Initial states q2 = path.States(2:end,:); % Final states
Estimate the cost of propagating to target state.
cost = distance(propagator,q1,q2);
Generate a series of control commands and number of steps to move from the current state
q1 with control command
u toward the target state
u = zeros(size(q1,1),propagator.NumControlOutput); steps = zeros(size(q1,1),1); for i = 1:size(q1,1) [u(i+1,:),steps(i)] = sampleControl(propagator,q1(i,:),u(i,:),q2(i,:)); end
Create a control-based path object with the specified state propagator and a sequence of specified states, controls, targets, and durations.
states = path.States; controls = u(2:end,:); targets = q2; durations = steps*propagator.ControlStepSize; path2 = navPathControl(propagator,states,controls,targets,durations);
Visualize the results.
figure grid on axis equal hold on plot(path2.States(:,1),path2.States(:,2),".b") plot(waypoints(:,1),waypoints(:,2),"*r","MarkerSize",10)
mobileProp — Mobile robot state propagator
Mobile robot state propagator, specified as a
q0 — Initial state
Initial state of the system, specified as an s-element vector. s is the number of state variables in the state space.
u0 — Initial control on the initial state
Initial control input, specified as an c-element vector. c is the number of control inputs.
qTgt — Target state
Target state of the system, specified as an s-element vector. s is the number of state variables in the state space.
u — Control inputs for propagating states
Control inputs for propagating states, returned as an c-element vector. c is the number of control inputs.
steps — Number of steps from each state and control input to next
n-element vector of positive integers
Number of steps from each state and control input to next, returned as an n-element vector of positive integers.
Introduced in R2021b