We propose a bilevel optimization framework to plan a trajectory subject to variable time allocation and safety-set constraints. Our formulation is similar to that of [1]}: the lower-level problem is convex and solved for the global minimum; the upper-level problem is non-convex, and a new “solution” is obtained by one-step gradient descent to reduce the optimization cost.
The uniqueness of our approach is that we exclude the inequality constraint for the safety set in the lower-level problem, making it a QP with equality constraints only. Specifically, the feasible waypoints in the safety set originally characterized by the inequality constraints are fixed in the lower-level problem. Such a formulation can significantly reduce the computation time on the lower-level QP since it can be turned into an unconstrained optimization problem.
Correspondingly, the upper-level problem will adjust the time allocation and the feasible waypoints using analytical gradients that are efficiently obtained via OptNet [2]}. Compared with the upper-level problem formulation in [1]}, where only the time allocation is updated via analytical gradients, the extra computation time to obtain the gradient for the spatial update (of the feasible waypoints) is negligible. This comparison concludes the major reason why our formulation can significantly reduce the computation time of that in [1]}, where we validated the comparison in simulations with a scalability test. While both methods minimize the cost to a similar level, our approach's computation time scales linearly with the number of waypoints, whereas the one by [1]} scales exponentially.
