Time-optimal control

Overview

The problem is to drive a linear system from a start state to a goal region while trading total duration against control effort. The cost is the integral of the squared control plus a weight ρ times the duration T [arxiv:1709.05401 §2] eq. 4. The paper applies this to each axis of a quadrotor, modelled as a chain of integrators whose highest derivative is the control [arxiv:1709.05401 §2] eq. 2. The weight ρ sets the trade-off between speed and effort; the paper does not fix a value for it 1709.05401 §3.3.

The invariant every variant shares is that the optimal cost is a function of the start, the goal and T, so a planner can minimise over T directly 1709.05401 §3.4.2 eq. 10.

Only one paper in the wiki covers this concept so far.

Variants

Lattice search with a closed-form heuristic

The control set is discretised into a finite lattice of constant inputs. Each input is applied for a fixed duration τ, and the resulting end states form a graph. An A* search finds the cheapest route through it [arxiv:1709.05401 §3.1] §3.2 §3.3. The heuristic is the closed-form LQMT cost, which is admissible because it lower-bounds the continuous optimum [arxiv:1709.05401 §3.4.2]. A simpler minimum-time bound, ρ times the largest per-axis distance over the speed limit, is also admissible and consistent [arxiv:1709.05401 §3.4.1] eq. 7. Card: Quadrotor LQMT motion planning.

Velocity and acceleration control

For velocity control (n = 1) the optimal input is constant, equal to the displacement divided by T, and the cost is the squared distance over T plus ρT 1709.05401 §3.4.2. For acceleration control (n = 2) the paper gives closed-form polynomial coefficients and a closed-form cost in terms of the start and goal positions and velocities 1709.05401 §3.4.2. The optimal T is then the positive real root of the cost's derivative with T at or above the minimum-time bound 1709.05401 §3.4.2. These closed forms are the source of the paper's speed claim 1709.05401 §5.2 tab. 1.

Jerk control, no closed form

For jerk control (n = 3) the text gives no closed form for the heuristic. Evaluating the cost needs the real roots of a polynomial, and the paper notes that roots of polynomials above order 4 have no closed form, which limits the higher-dimensional case 1709.05401 §5.1. Jerk-control planning is measurably slower in the paper's timing table: its 3-D average is 2.98 s, against 0.094 s for acceleration control 1709.05401 §5.2 tab. 1.

Refinement with a smoothing QP

The lattice route gives waypoints and a time allocation per segment. A quadratic program then minimises the integral of the squared n-th derivative along the route, with the waypoints as constraints 1709.05401 §4 eq. 13. The paper states that this refined trajectory "might be unsafe and infeasible" and describes no check for it 1709.05401 §4.

When to use which

The paper's timings come from one start state for the heuristic study and from hundreds of trajectories for the run-time table; the hardware for the run-time table is not stated 1709.05401 §5.1 tab. 1. Compare numbers within this paper only.

Implementation sketch

Lattice successors and A* are shown in the card's pseudocode [arxiv:1709.05401 alg. 1]. The heuristic for acceleration control is the core of the variant.

# LQMT cost for acceleration control, minimised over T  [arxiv:1709.05401 §3.4.2]
# Input:  dp = goal position minus state position (3,); v0, v1 (3,)
#         rho; t_bar: minimum-time bound  [arxiv:1709.05401 §3.4.1]
# Output: lower bound on the cost-to-go

def heuristic_lqmt_acceleration(dp, v0, v1, rho, t_bar):
    # Cost as a function of the duration T  [arxiv:1709.05401 §3.4.2] [eq. 10]
    def cost(T):
        return (12 * dot(dp, dp) / T**3
                - 12 * dot(v0 + v1, dp) / T**2
                + 4 * (dot(v0, v0) + dot(v0, v1) + dot(v1, v1)) / T
                + rho * T)
    # Candidates: t_bar and the positive real roots of cost'(T) = 0  [§3.4.2]
    candidates = [t_bar] + positive_real_roots_of_derivative(cost)
    return min(cost(T) for T in candidates if T >= t_bar)

The velocity term is written v_1 in the cost, and v_f elsewhere in the paper 1709.05401 §3.4.2; use one name throughout.

Contradictions

• Default heuristic in code versus paper. The paper presents the LQMT heuristic as the tighter one and uses it to speed up the search 1709.05401 §5.1. The released code defaults to ignoring dynamics in the heuristic, which selects the minimum-time bound env_base.h. The paper does not state which heuristic its re-planning runs used 1709.05401 §5.3.
• Collision sampling rule. Equation 12 is printed with ≥, while the text requires consecutive samples to be no more than R apart, which needs ≤ 1709.05401 §3.5 eq. 12. The paper does not resolve the sign.

Papers

• Quadrotor LQMT motion planning: plans minimum-time quadrotor trajectories by A* over motion primitives, with a closed-form LQMT heuristic.