Optimal Path Planning for Free-Flying Space Manipulators via Sequential Convex Programming
Authors: Misra, Bai · Year: 2017 · Venue: Journal of Guidance, Control, and Dynamics (Engineering Note), DOI 10.2514/1.G002487
Raw: md
Summary
This Engineering Note poses point-to-point trajectory planning for a kinematically redundant space manipulator (n > 6) atop an attitude-controlled base as a nonconvex optimal control problem and solves it with sequential convex programming (SCP). The central novelty is the treatment of the terminal end-effector pose constraint: rather than precomputing a joint configuration by inverse kinematics, the final pose is enforced via a forward-kinematics integral constraint in exponential coordinates, which is then affinely linearized and discretized as a fourth-order Newton–Cotes quadrature. An exact ℓ₁ penalty (soft constraints) plus a trust-region SCP loop yields collision-free, locally optimal joint trajectories that compensate for the base motion. Validated on an 8-DOF arm over 50 trials with 92% convergence.
Key Claims
- The paper’s “free-flying” assumption is attitude-controlled base only (no translational control on the base): Ω_b = 0 is enforced, but the base is free to translate, so linear momentum is conserved [Eq. (3)]. This is a hybrid regime, not the fully-actuated 6-DOF base of our thesis — see flags.
- Starting from a fixed-base inverse-kinematics guess fails to reach the target because it ignores base motion; SCP refines this to converge to the desired pose even as the base drifts ~12 cm during the maneuver.
- Terminal-state accuracy of 10⁻⁴ achieved for both end-effector position and attitude.
- 50 random-initial-condition trials: 92% success; average 17.2 iterations; ~12 s total CPU (YALMIP + Gurobi QP, i5 @ 2.9 GHz). 4 failures: 1 infeasible from joint/control bounds, 3 conjectured stuck in non-collision-free local basins; adding 5 s of maneuver time did not help.
- As base mass m₀ → ∞, the augmented Jacobian J_a reduces to the conventional fixed-base manipulator Jacobian J_m.
- Imposing terminal pose and obstacle constraints as hard constraints causes downstream infeasibility in the SCP iterations even when the problem is feasible initially; the exact ℓ₁ penalty reformulation fixes this. Exactness holds for λ > λ* = ‖μ*ᵢ‖_∞ (KKT multiplier infinity-norm), per Nocedal & Wright.
Method
Regime. n-DOF (n > 6) revolute arm on a base with no translational control but active attitude control, so Ω_b = 0. Under zero external translational force and zero initial linear momentum, conservation of linear momentum couples base translation to joint rates. This is the authors’ “free-flying” definition; note it differs from the fully-actuated 6-DOF base (genuinely free-flying) used in our thesis, and from the classic free-floating regime (uncontrolled base attitude and translation) which the authors’ own prior work [12] addresses.
Kinematics. Base pose g_b ∈ SE(3); link position r_i = r_b + p_i [Eq. (1)]; link velocity v_i = v_b + Ω_b^× p_i + ν_i [Eq. (2)], with Ω_b = 0. Momentum conservation Σ m_i v_i = P [Eq. (3)] gives base translational velocity as an explicit function of joint rates:
v_b = −(Σ_{i=1}^n m_i J_{Ti} / Σ_{i=0}^n m_i) θ̇ = J_v(θ) θ̇ [Eq. (5)].
End-effector inertial twist via the augmented Jacobian J_a, combining the conventional manipulator Jacobian J_m and the base Jacobian J_s [Eq. (8)–(9)]:
ξ_e = [J_m − J_s (Σ m_i J_{Ti} / Σ m_i)] θ̇ = J_a(θ) θ̇, with J_s = [[E, −(r_e − r_b)^×],[0, E]].
Terminal pose constraint (the contribution). Parameterize η = [r_e, ψ]ᵀ ∈ ℝ⁶ with ψ the exponential coordinates (Rodrigues) for Q_e = exp(ψ^×); kinematics η̇ = G(η) J_a(θ) θ̇ with G(η) = diag(E, A(ψ)^{−T}) [Eqs. (11)–(12)]. Terminal pose is then the integral constraint
η(t_f) − η(0) = ∫₀^{t_f} G(η) J_a(θ) θ̇ dt [Eq. (13)].
Convexification. First-order Taylor expansion ĝ(x) = g(x^q) + ∇g(x^q)ᵀ(x − x^q) [Eq. (15)] over trust regions; the Jacobian-rate product J(θ)θ̇ is linearized in (θ, θ̇) [Eq. (16)] using Bruyninckx–De Schutter symbolic Jacobian differentiation. Obstacle avoidance via the inequality velocity scheme J_G(θ)θ̇ ≤ b_G [Eq. (14), Guo–Zhang], activated only when min link–obstacle distance < safety distance (15 cm); distances via GJK on convex polytopes.
Discretization. Double-integrator dynamics, control u_k = θ̈_k (zero-order hold). ζ_k = [θ_k, θ̇_k]ᵀ, ζ_{k+1} = A_{Δt} ζ_k + B_{Δt} u_k [Eqs. (18)–(21)]. Cost Σ ½ζ_kᵀ M ζ_k + ½u_kᵀu_k (kinetic energy + control effort) with box constraints [Eq. (22)]. Integral pose constraint discretized as fourth-order Newton–Cotes quadrature [Eq. (23)].
SCP loop. Exact ℓ₁ penalty [Eqs. (25)–(26)]; trust region expanded/contracted on the ratio of actual to predicted objective change δ^q/ψ^q vs threshold α [Eq. (27), Algorithm 1]. Initial guess from damped-least-squares IK followed by a QP without terminal/obstacle constraints.
Note on dynamics. Only kinematically feasible trajectories are planned (double integrator); full manipulator dynamics and a tracking controller are explicitly deferred to future work.
Relevance to thesis
Directly relevant to the guidance/planning layer. The forward-kinematics integral formulation of the terminal pose constraint [Eq. (13)] sidesteps inverse kinematics and is attractive for redundant arms — useful for our planner. The augmented-Jacobian machinery [Eqs. (8)–(9)] and momentum coupling are standard tools we need. Crucially, the authors’ “free-flying” = attitude-controlled-but-translationally-free base is not our fully-actuated 6-DOF base: with a fully actuated base, linear momentum is not conserved and v_b is an independent control, so Eq. (5) and the resulting J_a coupling do not apply unchanged. This paper is best read as the intermediate regime between free-floating [12] and our genuinely free-flying system, and a template for how to extend SCP planning to the fully-actuated case. The exact-penalty + trust-region SCP recipe is reusable for our nominal planner before adding the risk layer.
Connections
Topics: sequential_convex_programming · generalized_jacobian · exponential_coordinates · dynamic_singularity
Key Equations / Quotes
“The free-flying terminology defined as space robotic manipulators with an attitude controlled base is a reasonable assumption when there are attitude and communication constraints on the base.” (Sec. I)
“space manipulators can also encounter dynamic singular configurations where the end effector loses mobility in some inertial direction due to the base-arm motion coupling.” (Sec. I)
Augmented Jacobian [Eq. (8)]:
ξ_e = [J_m − J_s (Σ_{i=1}^n m_i J_{Ti}) / (Σ_{i=0}^n m_i)] θ̇ = J_a(θ) θ̇
Forward-kinematics terminal pose constraint [Eq. (13)]:
η(t_f) − η(0) = ∫₀^{t_f} G(η) J_a(θ) θ̇ dt
Exact-penalty threshold (KKT): λ* = ‖μᵢ‖_∞; exactness for λ > λ.
Open Questions
- How does the planned (kinematically feasible) trajectory perform once full manipulator dynamics and a feedback tracker are layered on? Explicitly left as future work.
- The 3 non-bound failures are conjectured to be local-minimum traps; no diagnosis or escape strategy is given beyond increasing maneuver time (which did not help).
- λ is chosen by trial-and-error (= 50 in the example); no constructive estimate of ‖μ*‖_∞ is provided.
- Dynamic singularities are named as a motivation but the planner does not explicitly detect or avoid them — is collision-/bound-feasibility enough to stay clear of them?
- No quantitative state/control bounds reported (Table 2 is empty in the source).