Cruise Turnpikes: Certifying Steady-Flight Structure in Kinodynamic Motion Planning

When a robot travels far, its optimal trajectory usually settles into a boring middle section. A car cruises straight, and a quadrotor holds a fixed tilt and speed. This is the turnpike property: for long time horizons, optimal trajectories rush to a steady regime, stay there for most of the trip, and leave it only near the end. The standard theory explains turnpikes through strict dissipativity, which asks for an "energy-like" storage function showing that every deviation from the optimal steady state costs something extra. That theory assumes the robot comes to rest at a fixed point. A cruising robot never comes to rest. Its steady regime is a relative equilibrium, or trim: a motion with constant body velocity whose position keeps changing.

This proposal puts dissipativity theory into coordinates where trims become ordinary fixed points. In these symmetry-reduced coordinates, the dynamics are written in the robot's body frame, so position drops out and only velocities, tilt and actuator states remain. Reaching the goal pose then becomes a constraint on accumulated displacement. Its Lagrange multiplier is a "price per meter," and it plays the role of the storage-function gradient. Building on that, we aim to certify when a long-distance kinodynamic planning problem has a trim turnpike and to predict which trim it is. We also want to say when the certificate fails, in which case the turnpike is periodic, non-unique, or absent. Finally, we turn the certificate into initial guesses for a trajectory optimizer and into a modified ("rotated") cost for it.

Core Claim

Take a robot with dynamics that are invariant under translations, and under yaw where gravity is present. Reduce it to body coordinates $z$ (velocities, roll and pitch, actuator states) with controls $u$ and stage cost $l(z,u)$. A long-distance plan of length $D$ with free final time has a cruise turnpike at the trim $(z^*,u^*)$ that solves the static problem

$$\min_{z,u}\; \frac{l(z,u)}{v(z)} \quad \text{s.t.}\quad \dot z = f(z,u) = 0,$$

where $v(z)$ is the forward speed along the goal direction. This is the turnpike precisely when the reduced problem is strictly dissipative with respect to the supply rate $s(z,u) = l(z,u) - \lambda^* v(z) - (l^* - \lambda^* v^*)$. Here $\lambda^*$ is the multiplier ("price per meter") of the displacement constraint, and $l^*, v^*$ are the cost and speed at the trim. The supply rate is just the ordinary steady-state-shifted stage cost with a price paid on distance travelled.

We claim three things that would be new. First, this dissipativity certificate can be computed by Sum-of-Squares (SOS) programming, a convex method for proving polynomial inequalities, in the low-dimensional reduced space for the DynoBench robot classes. Second, when it succeeds, it predicts how long the entry and exit arcs last. Third, its failure identifies the instances where warm-starting the optimizer from a trim misleads it.

Relation to Prior Work

The theory we reuse comes from On turnpike and dissipativity properties of continuous-time optimal control problems. That paper proves that strict dissipativity plus exponential reachability yields turnpikes, proves converse results, and builds a degree-5 SOS storage function for a chemical reactor. It treats only fixed-point steady states in Euclidean space. Our contribution is to carry the result over to relative equilibria through symmetry reduction, with distance replacing time as the quantity being averaged.

Two papers supply the "price as storage gradient" mechanism. System-Theoretic Analysis of Dynamic Generalized Nash Equilibrium Problems - Turnpikes and Dissipativity shows that steady-state multipliers equal the negative storage gradient and that linear terminal penalties built from them suppress the exit arc. Periodic optimal control, dissipativity and MPC extends dissipativity to periodic orbits, but only with a known period. Neither addresses symmetry or robots.

The motivating gap is Geodesic turnpikes for robot motion planning. That paper finds turnpikes in purely kinematic planning on $SE(2)$ and $SE(3)$ under swept-volume metrics and identifies them with saddle points on the unit sphere of the Lie algebra. It reports a 10x speedup, from about 1.5 s to 123 ms. The authors leave both the saddle-point identification and the extension to dynamics with momentum and forces open. Our framework covers their kinematic case as a special case, so it gives an independent test of their conjecture.

Two papers constrain the design. Turnpike in optimal control of PDEs, ResNets, and beyond and Additional material to the paper "On necessity and robustness of dissipativity in economic model predictive control" show that dissipativity alone does not give a turnpike without local controllability. Underactuated systems such as quadrotors must therefore be checked for local controllability around the trim, and we expect the certificate to fail where this check fails. The SOS scalability limits noted in the research file are another constraint. They are the reason we work in reduced coordinates, which have roughly 2 to 8 dimensions instead of 12 or more.

Research Program

Theory. We first state and prove the reduced dissipativity theorem. In the reduced coordinates, goal attainment becomes an integral (isoperimetric) constraint on velocity, and the proof will adapt the integral dissipation-inequality argument of Faulwasser et al. to handle it. We then derive the converse direction: if a trim turnpike exists, the reduced problem must be dissipative. For the kinematic swept-volume case, we test whether strict dissipativity holds exactly at the Lie-algebra points that de Mont-Marin et al. conjecture to be turnpikes. We also test whether non-strict dissipativity, the known gap between strict and non-strict versions, appears exactly at degenerate directions.

Computation. For each DynoBench robot class with translation symmetry, we compute the trim by static optimization and attempt an SOS storage certificate at increasing polynomial degree. The classes are the unicycle (first and second order), the car with trailer, the planar rotor, and the quadrotor. The acrobot has no such symmetry and is excluded as a structural control case. Each class receives one of three labels: certified, not certified at degree 6, or not locally controllable.

Use in planning. A certified instance gets two things. Its initial guess is an entry arc, the trim cruise, and an exit arc, with arc durations predicted from the storage function's decay rate. Its optimizer cost is rotated by the term $-\lambda^* v$, following Hall et al., to suppress premature exit arcs.

Baselines

The naive reference is the trajectory optimizer used inside iDb-A*, run from a straight-line interpolated initial guess. It uses the same solver, tolerances and 120 s budget as our method and differs only in how it is initialized. We beat it if we raise success rates and reduce solve time on long-distance instances without raising final cost.

The strongest existing method is iDb-A from iDb-A*: Iterative Search and Optimization for Optimal Kinodynamic Motion Planning. It interleaves search over motion primitives with trajectory optimization and reports solving 41 out of 43 DynoBench problems within a 120 s budget. Its predecessor db-A*: Discontinuity-bounded Search for Kinodynamic Mobile Robot Motion Planning reports a quadrotor recovery cost of 1.6 s against 2.6 s for RRT. We will run the released iDb-A* with its default primitives on identical hardware. We will also report a variant in which our turnpike guess replaces its first optimization seed, which tests whether the two methods complement each other. Matching its success rate while reducing time-to-first-solution on long or open instances counts as a win. So does matching cost with fewer motion primitives.

For the kinematic $SE(2)$ case, the reference is the turnpike-informed solver of de Mont-Marin et al. at its reported 123 ms. There our aim is predictive agreement rather than speed.

The key ablation is trim warm start without the certificate. It initializes every instance from the trim found by static optimization, whether or not dissipativity holds, and isolates what the dissipativity analysis adds. A second ablation removes the rotated cost.

Why not just…?

Why not just warm-start from the static-optimal trim and skip dissipativity entirely? This is the most serious threat. On instances where the turnpike is a clean trim, it will probably perform as well as our full method. Dissipativity earns its place only if it predicts which instances will mislead the naive trim warm start. Those include underactuated systems lacking local controllability, cases where cost-per-distance has several near-minimizers (the non-uniqueness problem raised by Geshkovski and Zuazua), and cases where a periodic gait beats steady cruise. The decisive experiment compares the ablation against the certified method on the certificate-negative instances. If the naive warm start does no worse there, our practical claim collapses, and the contribution shrinks to the theory and the test of the saddle-point conjecture.

Why not just use search with motion primitives, which already solves almost all of DynoBench? iDb-A*'s own authors name the offline primitive generation, and its exponential growth with state dimension, as the main limitation. A trim plus two arcs is a three-piece initial guess that needs no primitive library. Even so, cluttered DynoBench maps break the symmetry, so we expect search to stay superior there. We will report results separately by how much of each instance is open space, so as not to hide this.

Why not just extend the saddle-point heuristic of de Mont-Marin et al. to dynamic systems? That heuristic is kinematic and unproven, and it carries no notion of actuator effort or gravity. Our reduced static problem includes both. Where the two predictions coincide on $SE(2)$, our method supplies the missing proof. Where they disagree, a direct solve decides which prediction is right.

Proposed Experiments

The first experiment is predictive. For every certified instance, we solve long-horizon problems at several distances with a direct method. We then measure what fraction of the horizon the trajectory spends within a small neighborhood $\epsilon$ of the trim, and compare observed entry and exit arc durations with the predicted ones. The certificate passes if the fraction approaches one as distance grows and if the arc durations stay roughly constant, independent of $D$. We count false positives and false negatives of the certificate against these measured turnpikes.

The second experiment is the planning comparison against the baselines above on DynoBench. It reports success within 120 s, time to first solution, and final cost. Results are split into certificate-positive and certificate-negative instances and by obstacle density.

The third experiment is the $SE(2)$ swept-volume check, where we compare dissipativity-predicted turnpike directions with the conjectured saddle points.

The research file contains no reported per-instance timing for iDb-A* on long-distance subsets; on that point, no reported result found on Lacuna, so we will measure it ourselves. The project is decisive if it shows two things together. First, the certificate separates instances where the trim warm start helps from those where it hurts. Second, this separation outperforms the uncertified-trim ablation.