Uncertainty-Constrained Differential Dynamic Programming in Belief Space for Vision Based Robots
This work addresses the problem of poor performance or failure in vision-based robots due to trajectories lacking feature matches, offering a more robust planning solution for mobile robotics researchers and practitioners.
The paper proposes a novel trajectory optimization formulation that incorporates inequality constraints on uncertainty and an Augmented Lagrangian based stochastic differential dynamic programming method in belief space. This approach allows for robust trajectories in vision-based robots without manual tuning of uncertainty costs, demonstrating effectiveness in simulations across different environments and motion models.
Most mobile robots follow a modular sense-planact system architecture that can lead to poor performance or even catastrophic failure for visual inertial navigation systems due to trajectories devoid of feature matches. Planning in belief space provides a unified approach to tightly couple the perception, planning and control modules, leading to trajectories that are robust to noisy measurements and disturbances. However, existing methods handle uncertainties as costs that require manual tuning for varying environments and hardware. We therefore propose a novel trajectory optimization formulation that incorporates inequality constraints on uncertainty and a novel Augmented Lagrangian based stochastic differential dynamic programming method in belief space. Furthermore, we develop a probabilistic visibility model that accounts for discontinuities due to feature visibility limits. Our simulation tests demonstrate that our method can handle inequality constraints in different environments, for holonomic and nonholonomic motion models with no manual tuning of uncertainty costs involved. We also show the improved optimization performance in belief space due to our visibility model.