Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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

Shatil Rahman, Steven L. Waslander

TL;DR

This work proposes 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 and develops a probabilistic visibility model that accounts for discontinuities due 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.

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

TL;DR

This work proposes 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 and develops a probabilistic visibility model that accounts for discontinuities due 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.

Paper Structure

This paper contains 14 sections, 34 equations, 4 figures.

Table of Contents

  1. Introduction
  2. Related Work
  3. Problem Definition
  4. Probabilistic Visibility Based Belief Dynamics
  5. Belief Dynamics
  6. Probabilistic Visibility Modelling
  7. Constrained Stochastic Differential Dynamic Programming in Belief Space
  8. Inner Loop: Solving the unconstrained problem
  9. Outer Loop: Updating the penalty function
  10. Results
  11. iLQG-AL at varying levels of constraints
  12. Soft Uncertainty Costs vs Hard Uncertainty Constraints
  13. Effect of probabilistic visibility modelling
  14. Conclusion and Further Work

Figures (4)

  • Figure 1: iLQG-AL at different constraint levels in Map 1.
  • Figure 2: Estimation errors at execution time for iLQG-AL at loose, medium and tight level of constraints.
  • Figure 3: No. of violations for different weights of unconstrained iLQG and iLQG-AL in Maps 1 and 2 and for different robot models.
  • Figure 4: Planned belief trajectories with (green) and without (red) visibility modelling.