Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Constructive Observer Design for Visual Simultaneous Localisation and Mapping

Pieter van Goor, Robert Mahony, Tarek Hamel, Jochen Trumpf

TL;DR

The paper tackles monocular Visual SLAM by formulating a nonlinear observer on a dedicated symmetry group. It introduces the Lie group $\mathbf{VSLAM}_n(3)$ and lifts the VSLAM kinematics to its Lie algebra to build an intrinsic, globally meaningful error on the SLAM manifold, with a Lyapunov-based proof of almost semi-global asymptotic stability. The landmark-bearing and depth estimates are corrected via a structured observer that decouples bearing and depth updates, and convergence is established under persistent excitation; simulations and outdoor UAV experiments corroborate the approach. The resulting method is computationally light, scales with the number of landmarks, and advances equivariant observer design for SLAM, offering practical integrity for embedded robotics systems.

Abstract

Visual Simultaneous Localisation and Mapping (VSLAM) is a well-known problem in robotics with a large range of applications. This paper presents a novel approach to VSLAM by lifting the observer design to a novel Lie group on which the system output is equivariant. The perspective gained from this analysis facilitates the design of a non-linear observer with almost semi-globally asymptotically stable error dynamics. Simulations are provided to illustrate the behaviour of the proposed observer and experiments on data gathered using a fixed-wing UAV flying outdoors demonstrate its performance.

Constructive Observer Design for Visual Simultaneous Localisation and Mapping

TL;DR

The paper tackles monocular Visual SLAM by formulating a nonlinear observer on a dedicated symmetry group. It introduces the Lie group and lifts the VSLAM kinematics to its Lie algebra to build an intrinsic, globally meaningful error on the SLAM manifold, with a Lyapunov-based proof of almost semi-global asymptotic stability. The landmark-bearing and depth estimates are corrected via a structured observer that decouples bearing and depth updates, and convergence is established under persistent excitation; simulations and outdoor UAV experiments corroborate the approach. The resulting method is computationally light, scales with the number of landmarks, and advances equivariant observer design for SLAM, offering practical integrity for embedded robotics systems.

Abstract

Visual Simultaneous Localisation and Mapping (VSLAM) is a well-known problem in robotics with a large range of applications. This paper presents a novel approach to VSLAM by lifting the observer design to a novel Lie group on which the system output is equivariant. The perspective gained from this analysis facilitates the design of a non-linear observer with almost semi-globally asymptotically stable error dynamics. Simulations are provided to illustrate the behaviour of the proposed observer and experiments on data gathered using a fixed-wing UAV flying outdoors demonstrate its performance.

Paper Structure

This paper contains 18 sections, 6 theorems, 58 equations, 8 figures.

Table of Contents

  1. Introduction
  2. Preliminaries
  3. Problem Formulation
  4. VSLAM State Space
  5. VSLAM Kinematics
  6. System Output
  7. Symmetry of the VSLAM Problem
  8. Symmetry of the Total Space T0n(3)
  9. Symmetry of the Output Space
  10. Lift of the VSLAM Kinematics
  11. Observer Design
  12. Lifted System Kinematics
  13. Landmark Observer
  14. Total Space Representative
  15. Simulation Results
  16. ...and 3 more sections

Key Result

Lemma 4.1

The mapping $\Upsilon : \mathbf{VSLAM}_n(3) \times \mathcal{T}^\circ_n(3) \to \mathcal{T}^\circ_n(3)$ defined by is a transitive right group action of $\mathbf{VSLAM}_n(3)$ on $\mathcal{T}^\circ_n(3)$.

Figures (8)

  • Figure 1: The Disco Parrot UAV used to gather video data to test the proposed observer.
  • Figure 2: The action $\Upsilon$ of the VSLAM group on the state space for some given $(A,(Q_A,a)_i) \in \mathbf{VSLAM}_n(3)$ and $(P,p_i) \in \mathcal{T}^\circ_n(3)$. The pose $P$ is mapped to $P A$. The body fixed frame landmark points $R_P^\top (p_i - x_P)$ are rotated by $Q^\top$ and scaled by $a^{-1}$ in the body-fixed frame before transforming with the robot pose to a new point $p_i'$ which is then rewritten in the inertial frame.
  • Figure 3: An overview of the observer system. Note the distinction between the observer state $\hat{X}$ and the estimated state $(\hat{P}, \hat{p}_i)$, and that the correction term $\Delta$ is based on the output error $(d_i)$ rather than the raw measurement $(y_i)$.
  • Figure 4: The simulated trajectories of observer landmark estimates from initial positions with correct bearings but fixed depth of 10m. The observer landmark trajectories are shown in a range of colours matching those used in Figure \ref{['fig:delta_lyap']}, and the true landmark positions are shown in red. The observer robot trajectory is shown in blue, and the true robot trajectory is shown in black. The ($\circ$) and ($\star$) markers, respectively, denote the start and end of the trajectories of all the objects shown.
  • Figure 5: The evolution of the storage functions of each of the landmarks in the system shown in Figure \ref{['fig:delta_trajectories']}. The colours match the colours of the landmark trajectories in Figure \ref{['fig:delta_trajectories']}. The initial convergence of the landmarks is quick as the bearings converge, and then slows as the depths gradually converge.
  • ...and 3 more figures

Theorems & Definitions (13)

  • Lemma 4.1
  • proof
  • Proposition 4.2
  • proof
  • Lemma 4.3
  • proof
  • Lemma 4.4
  • proof
  • Theorem 5.1
  • proof
  • ...and 3 more