Constrained Trajectory Optimization for Hybrid Dynamical Systems

Hybrid dynamical systems pose significant challenges for effective planning and control, especially when additional constraints such as obstacle avoidance, state boundaries, and actuation limits are present. In this letter, we extend the recently proposed Hybrid iLQR method to handle state and input constraints within an indirect optimization framework, aiming to preserve computational efficiency and ensure dynamic feasibility. Specifically, we incorporate two constraint handling mechanisms into the Hybrid iLQR: Discrete Barrier State and Augmented Lagrangian methods. Comprehensive simulations across various operational situations are conducted to evaluate and compare the performance of these extended methods in terms of convergence and their ability to handle infeasible starting trajectories. Results indicate that while the Discrete Barrier State approach is more computationally efficient, the Augmented Lagrangian method outperforms it in complex and real-world scenarios with infeasible initial trajectories.

The developed algorithms go by the name of Discrete-Barrier State Hybrid iLQR (DBaS-HiLQR) and Augmented Lagrangian Hybrid iLQR (AL-HiLQR), respectively.

Content

Feasible Starting Rollout Trajectory

In this test case, we assessed the methods using randomly generated scenarios with obstacles of various shapes, sizes, and positions. The initial rollout trajectories were created from randomly generated control input sequences, but only those that resulted in initially feasible state trajectories were considered.

On the top row, we can observe the DBaS-HiLQR algorithm, optimizing only inside of the feasible set, while on the bottom row, the AL-HiLQR is performing an unconstrained optimization enforcing the constraints as a penalty term iteratively increasing.

Infeasible Starting Rollout Trajectory

In this test case, we evaluated how well the methods handle infeasible starting trajectories. The initial rollout trajectories were generated from randomly created control inputs, but only those resulting in infeasible state trajectories were considered.

We further tested AL-HiLQR by introducing constraints during mode transitions. The following figures shows that it successfully replans the contact sequence in these cases.

@misc{crestaz2024constrainedtrajectoryoptimizationhybrid,

      title={Constrained Trajectory Optimization for Hybrid Dynamical Systems}, 

      author={Pietro Noah Crestaz and Gokhan Alcan and Ville Kyrki},

      year={2024},

      eprint={2410.22894},

      archivePrefix={arXiv},

      primaryClass={eess.SY},

      url={https://arxiv.org/abs/2410.22894}, 

}