Capsizing-Guided Trajectory Optimization for Autonomous Navigation with Rough Terrain