Scalable, Multi-Sensor Aided Inertial Navigation for Agile Robots

Abstract

In this thesis a versatile, scalable solution for autonomous navigation of mobile robots is developed. The ability of autonomous navigation is essential to bring mobile systems from laboratory environments to real life scenarios. The focus is set on the special class of inherently unstable, highly dynamic Micro Aerial Vehicles (MAVs) as the systems cover many constraints and navigation aspects of general mobile robots. These are for example limitations in payload and computational resources, hard realtime requirements in state estimation for control and the required ability of full 3D motion close to obstacles in cluttered environments. In this thesis, both algorithmic and resulting system architecture aspects are elaborated. Considering algorithms, common state estimation approaches for MAVs use efficient filter- ing techniques to fuse data from Inertial Measurement Units (IMUs) with further comple- mentary, exteroceptive sensors like light-weight cameras. Measurement delays introduced by data processing and communication pipelines are often ignored resulting in a limitation of bandwidth of the state estimator. Furthermore, most estimation approaches are globally metric limiting spatial and (depending on the approach) temporal scalability. Considering system architecture, common designs either ignore inter sensor and system synchroniza- tion issues or depend on specialized hardware. The developed navigation solution tackles these limitations with three main contributions: Firstly, the Local Reference (LR) Inertial Navigation System (INS) algorithm is intro- duced. It is based on a delayed error state space Kalman Filter. Augmentation techniques are used to process (time delayed) relative poses from multiple odometry measurements as well as (time delayed) absolute state measurements. State augmentation, especially if used for delay compensation, can lead to numerical instability in standard Kalman Filter implementations. Therefore, the square root UD (Upper triangular/Diagonal matrix fac- torization) filter algorithm is extended to integrate augmentation and marginalization in closed, factorized covariance matrix form. Stabilizing an INS by odometry measurements only results in unbounded position and yaw angle errors. This can lead to an increase in unmodeled errors due to violated small error assumptions during linearization and lim- itations in numerical precision. With the LR-INS, uncertainties of unobservable system states can be bounded in an efficient and consistent way. Instead of state estimation in a global frame, the system states are transformed into a local reference frame decreasing state uncertainty. Repeated reference switching makes the hard realtime state estimation spatially and temporally scalable. All operations of LR filtering are directly integrated in closed decomposed covariance form into a square root UD prediction step exploiting its superior numerical properties. The second contribution is the development of a flexible system architecture for au- tonomous navigation of mobile robots considering hardware and software aspects. Es- pecially on inherently unstable systems, the separation of system critical and non-critical tasks in terms of hardware can improve overall system robustness. Furthermore, a dis- tributed system concept enables the transparent exchange of algorithms between com- puter boards and hardware accelerators as for example Field Programmable Gate Ar- rays (FPGAs). In such a configuration, inter sensor and system time synchronization is essential for consistent realtime state estimation with measurement delay compensa- tion. The developed system architecture defines minimal requirements on the underlaying hardware. This enables on the one hand the use of Commercial Off-The-Shelf (COTS) components and on the other hand a flexible and fast hardware upgrade to the most recent and powerful modules. The third contribution is the demonstration of the entire autonomous navigation solu- tion including stereo vision aided hard realtime state estimation, control, environment mapping, path planning and obstacle avoidance in real life scenario quadrotor flights. Be- sides indoor and outdoor experiments for algorithmic evaluation, autonomous flights in challenging, cluttered environments with indoor/outdoor transitions and in a dusty and gloomy coal mine demonstrate the usability and robustness of the developed solution for autonomous navigation of mobile robots.