Abstract
We propose a sampling-based trajectory optimization methodology for constrained problems. We extend recent works on stochastic search to deal with box control constraints, as well as nonlinear state constraints for discrete dynamical systems. Regarding the former, our strategy is to optimize over truncated parameterized distributions on control inputs. Furthermore, we show how non-smooth penalty functions can be incorporated into our framework to handle state constraints. Simulations on cartpole and quadcopter show that our approach outperforms previous methods on constrained sampling-based optimization, in terms of quality of solutions and convergence speed.
Introduction
Trajectory optimization is a cornerstone of modern robotics, yet it remains challenging in environments with complex dynamics or non-differentiable cost functions. While gradient-based methods offer speed, they often struggle with discontinuities—such as those found in contact dynamics—or when a differentiable model of the system is unavailable.
Sampling-based methods, such as Path Integral control, provide a robust alternative by avoiding reliance on dynamics gradients. However, these methods traditionally struggle to handle constraints effectively. Common workarounds, such as "clamping" control inputs or assigning arbitrary "High Costs" to obstacle collisions, often lead to slow convergence and poor local minima. This paper introduces a mathematically principled framework to integrate constraints directly into the sampling process using Stochastic Approximation (SA).
Methodology
The core of the proposed approach is to optimize the parameters ($\rho$) of a probability distribution from which trajectories are sampled, rather than optimizing a single trajectory directly. This effectively smooths the optimization landscape.
Truncated Distributions for Box Constraints
To respect control limits (e.g., maximum motor torque), the framework optimizes over truncated distributions. Unlike post-sampling "clamping," which can bias the optimization, this method defines the distribution only within allowed bounds. This ensures every sample is inherently feasible regarding control limits and that gradient updates correctly account for the truncation boundaries.
Non-smooth Penalties for State Constraints
For state constraints like obstacle avoidance, the authors employ non-smooth penalty functions (specifically $L_1$-type exact penalties). Unlike standard quadratic penalties that require infinite weights to achieve true feasibility, non-smooth penalties can reach the exact feasible region with finite weights, facilitating more efficient and stable optimization.
The Algorithm
The "Constrained sampling-based controller" iteratively samples control parameters, performs rollouts using system dynamics, and calculates a penalized cost. It then updates the distribution's mean and variance via SA-derived gradient updates, gradually increasing the penalty coefficient to enforce constraints strictly.
Experimental Evaluation
The method was benchmarked on two simulated tasks: a Cartpole Swing-up with track and velocity limits, and a Quadcopter Navigation task involving 3D obstacle avoidance and tilt-angle constraints.
Results and Discussion
In quadcopter simulations, the proposed method converged in approximately one-third of the iterations required by the Constrained Cross Entropy (CCE) method. Furthermore, as shown in the cartpole trajectories, the SA-based approach finds solutions with lower expected costs and better adherence to state boundaries.
Key insights from the evaluation include:
- Smoothing via Expectation: Optimizing expected cost allows the solver to navigate through discontinuous regions that would break standard gradient-based solvers.
- Efficient Truncation: Incorporating truncation into exponential family distributions maintains a simple gradient form, making box constraints computationally efficient.
- Early Penalization: The framework provides a signal for constraints early in the optimization, guiding the distribution toward feasibility more effectively than "High Cost" methods.
Conclusion
This framework offers a robust, faster-converging alternative for constrained trajectory optimization in robotics. Future work will explore higher-order update rules to further accelerate convergence and the use of local approximations for higher-dimensional state spaces.
Summary updated