Trajectory Optimization

Definition

Trajectory optimization computes a state/input history that drives a space manipulator from an initial
state to a desired final state while minimizing a cost functional (typically motor power, control
effort, or a singularity/manipulability measure) subject to the system dynamics and motion constraints.
For a satellite-manipulator system it is most naturally posed in the optimal-control form: minimize
$G={\int }_{t_0}^{t_f}Ldt$ subject to the equality constraint $\dot{\mathbf{x}}=\mathbf{g}(\mathbf{x},\mathbf{u},t)$,
yielding a two-point boundary value problem (BVP) via the calculus of variations (rybus2017control),
or as a constrained nonlinear program — e.g. a configuration-space planner that minimizes a task cost
subject to a singularity-avoidance (S-Map distance) constraint enforced at the discretized via-points
(calzolari2020singularity). Both cited sources adopt the
free-floating regime (uncontrolled, reactive base with momentum conservation); for our free-flying
system the base is fully actuated, which changes both the dynamics constraint $\mathbf{g}$ and the
admissible final states.

Key Equations

Symbols per notation.md.

Optimal-control (BVP) form (rybus2017control, Eqs. 27–34). The augmented
functional with Lagrange multipliers $\mathbf{\lambda }$ enforcing the system dynamics:

$$\underset{\mathbf{u}(\cdot )}{\min }G={\int }_{t_0}^{t_f}[L(\mathbf{x},\mathbf{u},t)+{\mathbf{\lambda }}^{\top }(\mathbf{g}(\mathbf{x},\mathbf{u},t)-\dot{\mathbf{x}}\left)\right]dt,L=\frac{1}{2}{\mathbf{u}}^{\top }\mathbf{u},$$

where the dynamics constraint is $\dot{\mathbf{x}}=\mathbf{g}$, with $\mathbf{g}=[{\mathbf{M}}^{-1}(\mathbf{Q}-\mathbf{C}{\mathbf{q}}_v);{\mathbf{q}}_v]$
(state $\mathbf{x}=[{\mathbf{q}}_v;{\mathbf{q}}_p]$ stacking generalized velocities/positions; $\mathbf{Q}$ the
generalized force; $\mathbf{M},\mathbf{C}$ the coupled inertia/Coriolis of notation.md).
Stationarity $\partial H/\partial \mathbf{u}=0$ on the Hamiltonian $H=L+{\mathbf{\lambda }}^{\top }\mathbf{g}$ plus the
costate equation $\dot{\mathbf{\lambda }}=-\partial H/\partial \mathbf{x}$ closes the BVP, solved with the given
boundary conditions on $\mathbf{x}(t_0)$ and on $\mathbf{\lambda }(t_f)$.

Constrained-NLP form with explicit singularity avoidance (calzolari2020singularity, Eq. 23):

$$\underset{\mathbf{q}}{\min }\gamma (\mathbf{q})\text{s.t.}{d}_s(\mathbf{q})>\Delta ,$$

where $\gamma$ is the task cost, $d_s$ is the distance to the nearest singular surface (from the precomputed
S-Map), and $\Delta >0$ is the required clearance — recasting singularity avoidance as a collision-avoidance
constraint. (Symbols $L,\mathbf{\lambda },\mathbf{x},\mathbf{Q},\mathbf{u},{\mathbf{q}}_v,{\mathbf{q}}_p,\gamma ,d_s,\Delta$
are source-faithful and not yet in notation.md; note $\mathbf{u}$ here is the joint-torque
input, and $\gamma$ a scalar cost — distinct from the load-bearing matrix $\mathbf{\Gamma }$.)

Source Support

• rybus2017control — primary. Uses calculus of variations (the
  Seweryn–Banaszkiewicz algorithm) to minimize a quadratic motor-power norm $L=\frac{1}{2}{\mathbf{u}}^{\top }\mathbf{u}$,
  reducing to a BVP solved with bvp4c; the optimal trajectory then feeds an NMPC tracker. Free-floating,
  planar 2-DOF demo; reports a 38.8% cost reduction vs. a non-optimal trajectory.
• calzolari2020singularity — global, gradient-based trajectory
  planning as an NLP that minimizes $\gamma (\mathbf{q})$ under an S-Map distance constraint, giving a
  weak feasibility guarantee w.r.t. singularity avoidance at the (discretized) NLP via-points
  (source’s own qualifier). Free-floating 6-DOF arm
  on a 6-DOF body; notably states the method “may be applicable also for feedback controllers which use
  base actuation.”

Related Topics

• motion_planning — trajectory optimization is the optimize-a-cost specialization of
  motion planning; planners supply the feasible path that optimization then refines against a functional.
• optimal_control_bvp — the indirect (calculus-of-variations) route used by
  rybus2017control: stationarity + costate dynamics yield the two-point BVP.
• sequential_convex_programming — an alternative direct solver class for
  the constrained-NLP form; cited in calzolari2020singularity (GuSTO)
  as a way to handle the nonconvex dynamics/avoidance constraints.
• trajectory_tracking — the downstream consumer: the optimized reference trajectory
  is realized by a tracking controller (NMPC in rybus2017control).
• seweryn2008optimization — source (no topic page): the
  Seweryn–Banaszkiewicz variational power-minimization algorithm that rybus2017control
  builds on.
• See also dynamic_singularity — the avoidance constraint in
  calzolari2020singularity targets exactly the configuration-space
  dynamic singularities of the generalized Jacobian.

Open Questions

• Both sources optimize over the free-floating dynamics (momentum conservation fixes the base reaction).
  Under our free-flying base actuation the dynamics constraint $\mathbf{g}$ and the reachable final states
  differ — does the same cost functional (motor power) remain the right objective, or should base-actuator
  effort enter the functional?
• calzolari2020singularity explicitly conjectures its S-Map
  constraint “may be applicable also for feedback controllers which use base actuation” — does the S-Map,
  computed from the free-floating generalized Jacobian, transfer to our circumcentroidal Jacobian
  ${\mathbf{J}}_{{\nu }_e}^{\oplus }$ singularities, or must it be recomputed?
• The BVP/NLP solves are offline and computationally heavy (rybus: planning is “computationally
  demanding and requires considerable amount of time,” “performed while waiting in a safe point” — the
  paper does not quantify the wall-clock; the separate 170-hour figure is calzolari’s one-off S-Map
  build, not rybus’s per-trajectory solve). Is a closed-form or warm-started reoptimization feasible for
  online replanning against a tumbling target?