minsnappolytraj
Syntax
Description
[
generates a minimum snap polynomial trajectory that passes through a set of input waypoints
with their corresponding time points. The function returns positions, velocities,
accelerations, jerks, and snaps at the number of samples that you specified. The function
also returns the piecewise polynomial of the trajectory, time points for the output
trajectory, time points for the output trajectory samples, and the trajectory generation
status.q,qd,qdd,qddd,qdddd,pp,tPoints,tSamples,status] = minsnappolytraj(waypoints,timePoints,numSamples)
[
specifies options using one or more name-value arguments in addition to the input arguments
from the previous syntax. For example, q,qd,qdd,qddd,qdddd,pp,tPoints,tSamples,status] = minsnappolytraj(___,Name=Value)VelocityBoundaryCondition=[1 0 -1 -1; 1 1 1
-1] specifies the velocity boundary conditions for each waypoint of the minimum
snap trajectory.
Examples
Specify the trajectory waypoints in these XY coordinates:
(1,0) m
(4,1) m
(4,2) m
(3,4) m
(-2,3) m
(0,1) m
wpts = [1 4 4 3 -2 0; 0 1 2 4 3 1];
Specify the time points for trajectory waypoints, starting at 0 seconds at the first waypoint, to 5 second at the last waypoint, at 1 second increments.
tpts = 0:5;
Specify the number of samples in the output trajectory.
numsamples = 100;
Specify the maximum number of iterations, and the maximum solver time.
MaxIter = 1600; MaxTime = 15;
Compute minimum snap trajectory.
[q,qd,qdd,qddd,qdddd,pp,timepoints,tsamples,status] = minsnappolytraj(wpts,tpts,numsamples,TimeAllocation=true,MaxNumIterations=MaxIter,MaxSolverTime=MaxTime);
Plot the x- and y-positions of the minimum snap trajectory with respect to the output trajectory sample time.
plot(tsamples,q) hold on plot(timepoints,wpts,"x") xlabel("Sample time (s)") ylabel("Position (m)") legend("X-positions","Y-positions") grid on hold off title("Minimum Snap Trajectory Position vs Sample Time")
Plot the minimum snap trajectory in the 2-D plane, and show the waypoints.
plot(q(1,:),q(2,:),".b") hold on plot(wpts(1,:),wpts(2,:),"or") xlabel("X-positions (m)") ylabel("Y-positions (m)") legend("Minimum Snap Trajectory Samples","Waypoints") grid on hold off
Show the trajectory generation status. The status indicates that the function is able to converge to an optimal trajectory without singularity, and within the maximum number of iterations and solver time.
status
status = struct with fields:
Singularity: 0
MaxNumIterations: 0
MaxSolverTime: 0
Input Arguments
Name-Value Arguments
Output Arguments
Algorithms
The minsnappolytraj function generates an optimized trajectory using the
polynomial trajectory optimization algorithm [1]. The algorithm minimizes a cost
function that consists of squares of the derivatives of the polynomial, and trajectory segment
times. To constrain the optimization algorithm, you can specify the velocity, acceleration,
jerk, and snap boundary conditions for each waypoint using the
VelocityBoundaryCondition,
AccelerationBoundaryCondition,
JerkBoundaryCondition, and SnapBoundaryCondition
arguments, respectively.
You can also add the total segment time to the cost function by setting the
TimeAllocation argument to true, and adjust the
weight using the TimeWeight argument. Increasing the weight of time
allocation reduces the total segment time of the output trajectory at the expense of more snap
[2]. If you set the
TimeAllocation argument to true, the function uses
the time points you specify for the trajectory waypoints as initial guesses from which to
calculate the time of arrival at each waypoint.
The minsnappolytraj function performs optimization until convergence, or
until the solver reaches the maximum time or number of iterations. To verify whether the
solver has achieved convergence before reaching the maximum time or number of iterations, see
the status output. To adjust the maximum number of iterations and solver
time, specify the MaxNumIterations and MaxSolverTime
arguments, respectively.
References
[1] Bry, Adam, Charles Richter, Abraham Bachrach, and Nicholas Roy. “Aggressive Flight of Fixed-Wing and Quadrotor Aircraft in Dense Indoor Environments.” The International Journal of Robotics Research 34, no. 7 (June 2015): 969–1002. https://doi.org/10.1177/0278364914558129.
[2] Richter, Charles, Adam Bry, and Nicholas Roy. “Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments.” In Robotics Research: The 16th International Symposium ISRR, edited by Masayuki Inaba and Peter Corke. Springer International Publishing, 2016. https://doi.org/10.1007/978-3-319-28872-7_37.
