Uncertainty-Constrained Differential Dynamic Programming in Belief Space for Vision Based Robots

Most mobile robots follow a modular sense-planact system architecture that\ncan lead to poor performance or even catastrophic failure for visual inertial\nnavigation systems due to trajectories devoid of feature matches. Planning in\nbelief space provides a unified approach to tightly couple the perception,\nplanning and control modules, leading to trajectories that are robust to noisy\nmeasurements and disturbances. However, existing methods handle uncertainties\nas costs that require manual tuning for varying environments and hardware. We\ntherefore propose a novel trajectory optimization formulation that incorporates\ninequality constraints on uncertainty and a novel Augmented Lagrangian based\nstochastic differential dynamic programming method in belief space.\nFurthermore, we develop a probabilistic visibility model that accounts for\ndiscontinuities due to feature visibility limits. Our simulation tests\ndemonstrate that our method can handle inequality constraints in different\nenvironments, for holonomic and nonholonomic motion models with no manual\ntuning of uncertainty costs involved. We also show the improved optimization\nperformance in belief space due to our visibility model.\n