Enhancing 3D Autonomous Navigation Through Obstacle Fields: Homogeneous Localisation and Mapping, with Obstacle-Aware Trajectory Optimisation

T he capability of small flying robots, such as quadrotors and free-floating satellites, are making them useful tools for a wide range of applications. Quadrotors can be used for search and rescue, facility inspection, infrastructure surveying and parcel delivery. Free-flying satellites can be assistants inside space stations and monitor the outside of space stations and satellites. A critical capability to enable these applications is autonomous navigation near obstacles. Autonomous navigation is a challenge for small flying robots as they have limited payload capacity, hence require low-powered, low-weight sensors, and efficient computation. The robots also need to localise, map, and plan trajectories in 3D, a significantly enhanced challenge over 2D applications. Being flying vehicles, the dynamic-feasibility of planned trajectories and the control algorithms to track the trajectories are also essential considerations. The state-of-the-art for autonomous navigation systems is heterogeneous, with a combination of many different algorithms. What is proposed here is a more homogeneous system, with the aim for enhanced efficiency. To determine the location of a robot, visual Simultaneous Localisation and Mapping (SLAM) algorithms using stereo or depth cameras are the leading approach for small flying robots. While SLAM algorithms produce a map, it is purely for localisation, so a separate 3D mapping algorithm is required: producing occupancy grids or signed distance fields for trajectory planning. No algorithms can combine SLAM and 3D mapping into one algorithm without the use of lidar. This work proposes the use of 3D objects, modelled with Non-Uniform Rational B-Spline (NURBS) surfaces, to serve both as features for SLAM and as obstacles for trajectory planning. Modelling as objects, rather than complete environments, manages the computational requirements, and using NURBS surfaces allows the resolution to be varied for different tasks. The proposed approach is demonstrated on sets of simulated data, demonstrating tracking errors of under 2% of the total path length, mapping errors as low as 2 cm and an appropriate collision-cost profile for obstacle representation in trajectory planning. Leading trajectory planning approaches are also heterogeneous, with the combination of a global path planner, local trajectory optimiser and reactive obstacle avoidance. This split of algorithms can provide sub-optimal trajectories, though, when being used for flight close to obstacles. Presented here is the Admissible Subspace Trajectory Optimiser (ASTRO), an algorithm that provides a middle ground, optimising dynamics over a large horizon with consideration for complex obstacle fields. ASTRO performs polynomial optimisation with the inclusion of constraints. The constraint formulation is flexible to include a wide range of obstacles, including dynamic obstacles with motion models and uncertainty growth. ASTRO is shown to provide comparable computation time and success rate to the state-of-the-art, through batches of simulations. Flight tests on quadrotors show that the algorithm can produce trajectories that are more dynamically-feasible (easier to track) than the state-of-the-art, by including obstacles directly in the optimisation. Trajectory tracking control for quadrotors utilises the differential flatness transformation to link position and attitude controllers. There are singularities in the transformation though, and existing methods to handle the singularities can fail in different scenarios. These methods are analysed in detail to highlight where failures occur, and a new, robust method is proposed. The new method is successfully demonstrated in aggressive flights. xi The proposed algorithms for SLAM and trajectory planning are brought together into a complete system to demonstrate the homogeneous concept. This system is compared to the state-of-the-art in a novel simulation framework. The results successfully prove the concept that a single 3D representation can be used for localisation, mapping and planning with lightweight sensors. The current implementation of NURBSLAM is shown to be less efficient and less accurate than the state-of-the-art; however, it is more robust in scenarios with sparse visual features, successfully operating in cases where other visual SLAM algorithms fail, and demonstrating better recovery from errors. The work presented in this thesis can be built upon to evolve the SLAM algorithm further, to be more efficient and accurate. Tests can be performed in more environments and with real camera data, to aid development and to further characterise where NURBSLAM provides benefits over the state-of-the-art.