Multisensor fusion for state estimation via factor graphs

State estimation is a fundamental requirement for mobile robots. An accurate and robust estimate of a platform’s location and environment is a key input for many other robot systems, including control, navigation, and task planning. The aim of this thesis is to develop a state estimation system based on probabilistic factor graph optimisation. The proposed algorithm is called VILENS (Visual Inertial Lidar Legged Navigation System). It uses different combinations of camera, lidar, inertial, and kinematic sensors to estimate the robot’s state, and is designed to be robust to individual sensor failure. This thesis presents several novel contributions. First is a new method of integrating kinematic measurements into a factor graph. The proposed preintegrated twist factor aims to reduce the drift caused by slippage or deformation by estimating kinematics biases online. These effects are not modelled in other works. Second, a novel method for tracking and optimising sparse geometric features from lidar scans is presented. This differs from the literature which focuses on scan-to-scan or scan-to-map matching. Third, the complete VILENS system, which is the first attempt at integrating visual, inertial, lidar, and kinematic measurements into a factor graph. This makes use of a novel camera-lidar synchronisation method to reduce the graph size and complexity. A key benefit of this tight fusion is the natural handling of degenerate scenarios without hard switching. This algorithm has been widely tested on a variety of platforms, including legged robots and handheld sensor rigs. This thesis includes one of the first extensive real-world demonstrations of factor graphs for legged robot state estimation and has achieved relative position errors as low as 0.4% per distance travelled in challenging underground environments. Several large-scale deployments of mobile robots have either motivated its development or directly incorporated VILENS for state estimation. These include sewer inspection, 3D building reconstruction, and the DARPA Subterranean Challenge.