minjerkpolytraj
Syntax
Description
[
generates a minimum jerk polynomial trajectory that passes through a set of input waypoints
with their corresponding time points. The function returns positions, velocities,
accelerations, and jerks at the number of samples that you specified. The function also
returns the piecewise polynomial of the trajectory, time points for output trajectory, time
points for output trajectory samples, and trajectory generation status.q,qd,qdd,qddd,pp,tPoints,tSamples,status] = minjerkpolytraj(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,pp,tPoints,tSamples,status] = minjerkpolytraj(___,Name=Value)VelocityBoundaryCondition=[1 0 -1 -1; 1 1 1
-1] specifies the velocity boundary conditions in each dimension 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 minimum segment time.
SegmentTime = 1;
Specify the maximum number of iterations, and the maximum solver time.
MaxIter = 1600; MaxTime = 15;
Compute minimum jerk trajectory.
[q,qd,qdd,qddd,pp,timepoints,tsamples,status] = minjerkpolytraj(wpts,tpts,numsamples, ... TimeAllocation=true,MaxNumIterations=MaxIter,MaxSolverTime=MaxTime,MinSegmentTime=SegmentTime);
Plot the x- and y-positions of the minimum jerk 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 Jerk Trajectory Position vs Sample Time")
Plot the minimum jerk 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 Jerk 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 minjerkpolytraj 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,
and jerk boundary conditions for each waypoint using the
VelocityBoundaryCondition,
AccelerationBoundaryCondition, and
JerkBoundaryCondition 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 jerk
[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 minjerkpolytraj 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.
