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

TL;DR

This paper introduces a novel belief space trajectory optimization method for vision-based robots that incorporates uncertainty constraints and a probabilistic visibility model, improving robustness without manual tuning.

Contribution

It proposes an inequality-constrained stochastic differential dynamic programming approach in belief space with a new visibility model for better robustness and performance.

Findings

Handles uncertainty constraints without manual tuning.

Improves trajectory optimization in belief space.

Enhances robustness to feature visibility limits.

Abstract

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.