Optimal trajectory generation
traj_gen is a continuous trajectory generation package where high order derivatives
along the trajectory are minimized while satisfying waypoints (equality) and axis-parallel box constraint (inequality). The objective and constraints are formulated in quadratic programming (QP) to cater the real-time performance. C++ implementation can be found here.
To parameterize a trajectory, we use two types of curve: 1) piecewise-polynomials [1,2] and 2) a sequence of points [3].
The difference is optimization variables.
a. Piecewise-polynomials (polyTrajGen class) : It defines the primitive of the curve as polynomical spline. The optimization target is either polynomial coefficients [1] or free end-derivatives of spline segments [2] (can be set in constructor). In general, the latter has fewer optimization variables as it reduces the number of variable as much as the number of equality constraints.
b. A sequence of points (optimTrajGen class) : It does not limit the primitive of the curve. The optimization target is a finite set of points. The final curve is defined as a linear interpolant of the set of points. The point density (# of points per time) should be set in the constructor. Instead of unlimited representation capability of a curve, the size of optimization is driectly affected by the point density.
In this package, we use pin to accommodate the two constraints: equality (fix pin) and inequality (loose pin). Pin can be imposed regardless of the order of derivatives. Fix-pin refers a waypoint constraint,
and loose-pin denotes a axis-parallel box constraint. The pin is a triplets (time (t), order of derivatives (d), value (x)) where x is
a vector in case of fix-pin while two vectors [xl xu] for the loose-pin.
Also, check multiple examples in ./poly_example and ./optimal_example. The below is an example main2D.m where we can interactively select pin information.
Common
polyTrajGen
Polynomial order (N) : the order of all the polynomial segments. Although it can increase the power of representation of a curve, the size of optimization variables increases in proportion to (N x M).
Optimization target (‘end-derivative’ or ‘poly-coeff’) : the target of optimization. ‘poly-coeff’ method sets the coefficients of polynomials as optimization variables in a similar way with [1]. The ‘end-derivative’ sets the optimization variables as the free derivative values of each end point on a polynomial segment. As this method is equivalent to plugging the equality constraints (fix pin and continuity) to optimization problem, it reduces the optimization dimension at the cost of inversion of a mapping matrix. The dof of a segment is thus (poly_order - # of fix pins on the segment - maximal continuity). For the details, please refer [2].
Maximum continuity : the maximally imposed continuity order between neighboring segements. Higher value of this parameter enhances the quality of smoothness. However, too high value of this value restricts the dof for optimization, downgrading the opitimization result.
[1] Mellinger, Daniel, and Vijay Kumar. “Minimum snap trajectory generation and control for quadrotors.” 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011.
[2] Richter, Charles, Adam Bry, and Nicholas Roy. “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments.” Robotics Research. Springer, Cham, 2016. 649-666.
[3] Ratliff, Nathan, et al. “CHOMP: Gradient optimization techniques for efficient motion planning.” 2009 IEEE International Conference on Robotics and Automation. IEEE, 2009.