Abstract
This paper presents a nonlinear feedback control synthesis for general control-affine systems utilizing second-order needle variations. By leveraging the Mode Insertion Hessian (MIH), the proposed method explicitly exploits nonlinear controllability through Lie brackets. Simulation results across various underactuated models—including a differential drive cart and a dynamic underwater vehicle—demonstrate that this approach achieves convergence where first-order methods fail, often with significantly lower computational overhead.
Introduction
Controlling underactuated systems—where the number of control inputs is less than the degrees of freedom—remains a fundamental challenge in robotics. Standard feedback controllers often rely on first-order gradients of a cost function. However, for systems like drones or underwater vehicles, the gradient in the desired direction of motion is frequently zero. To navigate these "forbidden" directions, controllers must exploit nonlinear interactions, typically modeled via Lie brackets. While Nonlinear Model Predictive Control (NMPC) can address these issues, its computational cost often precludes real-time application.
Second-Order Needle Variations
The core innovation of this work lies in the use of needle variations: infinitesimal, high-amplitude bursts of control applied over a short duration $\lambda$. While previous research utilized first-order variations (the Mode Insertion Gradient), this paper derives the Mode Insertion Hessian (MIH).
The MIH provides a second-order sensitivity measure that the authors prove contains explicit Lie bracket terms. This mathematical link bridges the gap between optimal control (cost minimization) and geometric control (reachability), ensuring that the controller can "see" paths to the goal that first-order analysis misses.
Methodology: Sequential Feedback Synthesis
The researchers developed a real-time feedback loop that iteratively refines control actions. The process involves simulating the system's nominal trajectory and co-states (sensitivities) over a short horizon, calculating an analytical second-order response, and performing a line search to determine the optimal duration for the control "needle."
This analytical approach allows for extreme efficiency. In comparative tests, the algorithm ran approximately 40 times faster than Iterative Linear Quadratic Gaussian (iLQG) controllers.
Simulation Results and Discussion
The method was validated on three distinct models:
- 2D Kinematic Differential Drive: Achieved a 100% success rate in 1,000 Monte Carlo trials, whereas first-order methods often failed to orient the vehicle toward the goal.
- 3D Kinematic Rigid Body: Successfully navigated orientations where first-order methods encountered singularities.
- 3D Dynamic Underwater Vehicle: Demonstrated convergence in a high-fidelity 13-state model, successfully executing heave and roll maneuvers to reach targets unreachable by first-order logic.
Conclusion
By moving beyond simple gradients to second-order needle variations, this feedback synthesis method provides a provable descent guarantee for nonlinearly controllable systems. Future work will focus on smoothing the control pulses for hardware implementation and extending the framework to handle highly non-convex cost landscapes in complex environments.
Summary updated