Local reference filter for life-long vision aided inertial navigation

Filter based system state estimation is widely used for hard-realtime applications. In long-term filter operation the estimation of unobservable system states can lead to numerical instability due to unbounded state uncertainties. We introduce a filter concept that estimates system states in respect to changing local references instead of one global reference. In this way unbounded state covariances can be reset in a consistent way. We show how local reference (LR) filtering can be integrated into filter prediction to be used in square root filter implementations. The concept of LR-filtering is applied to the problem of vision aided inertial navigation (LR-INS). The results of a simulated 24 h quadrotor flight using the LR-INS demonstrate longterm filter stability. Real quadrotor flight experiments show the usability of the LR-INS for a highly dynamic system with limited computational resources

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

Small flying robots have numerous potential applications, from quadrotors for search and rescue, infrastructure inspection and package delivery to free-flying satellites for assistance activities inside a space station. To enable these applications, a key challenge is autonomous navigation in 3D, near obstacles on a power, mass and computation constrained platform. This challenge requires a robot to perform localisation, mapping, dynamics-aware trajectory planning and control. The current state-of-the-art uses separate algorithms for each component. Here, the aim is for a more homogeneous approach in the search for improved efficiencies and capabilities. First, an algorithm is described to perform Simultaneous Localisation And Mapping (SLAM) with physical, 3D map representation that can also be used to represent obstacles for trajectory planning: Non-Uniform Rational B-Spline (NURBS) surfaces. Termed NURBSLAM, this algorithm is shown to combine the typically separate tasks of localisation and obstacle mapping. Second, a trajectory optimisation algorithm is presented that produces dynamically-optimal trajectories with direct consideration of obstacles, providing a middle ground between path planners and trajectory smoothers. Called the Admissible Subspace TRajectory Optimiser (ASTRO), the algorithm can produce trajectories that are easier to track than the state-of-the-art for flight near obstacles, as shown in flight tests with quadrotors. For quadrotors to track trajectories, a critical component is the differential flatness transformation that links position and attitude controllers. Existing singularities in this transformation are analysed, solutions are proposed and are then demonstrated in flight tests. Finally, a combined system of NURBSLAM and ASTRO are brought together and tested against the state-of-the-art in a novel simulation environment to prove the concept that a single 3D representation can be used for localisation, mapping, and planning

Study and development of a reliable fiducials-based localization system for multicopter UAVs flying indoor

openThe recent evolution of technology in automation, agriculture, IoT, and aerospace fields has created a growing demand for mobile robots capable of autonomous operation and movement to accomplish various tasks. Aerial platforms are expected to play a central role in the future due to their versatility and swift intervention capabilities. However, the effective utilization of these platforms faces a significant challenge due to localization, which is a vital aspect for their interaction with the surrounding environment. While GNSS localization systems have established themselves as reliable solutions for open-space scenarios, the same approach is not viable for indoor settings, where localization remains an open problem as it is witnessed by the lack of extensive literature on the topic. In this thesis, we address this challenge by proposing a dependable solution for small multi-rotor UAVs using a Visual Inertial Odometry localization system. Our KF-based localization system reconstructs the pose by fusing data from onboard sensors. The primary source of information stems from the recognition of AprilTags fiducial markers, strategically placed in known positions to form a “map”. Building upon prior research and thesis work conducted at our university, we extend and enhance this system. We begin with a concise introduction, followed by a justification of our chosen strategies based on the current state of the art. We provide an overview of the key theoretical, mathematical, and technical aspects that support our work. These concepts are fundamental to the design of innovative strategies that address challenges such as data fusion from different AprilTag recognition and the elimination of misleading measurements. To validate our algorithms and their implementation, we conduct experimental tests using two distinct platforms by using localization accuracy and computational complexity as performance indices to demonstrate the practical viability of our proposed system. By tackling the critical issue of indoor localization for aerial platforms, this thesis tries to give some contribution to the advancement of robotics technology, opening avenues for enhanced autonomy and efficiency across various domains.The recent evolution of technology in automation, agriculture, IoT, and aerospace fields has created a growing demand for mobile robots capable of autonomous operation and movement to accomplish various tasks. Aerial platforms are expected to play a central role in the future due to their versatility and swift intervention capabilities. However, the effective utilization of these platforms faces a significant challenge due to localization, which is a vital aspect for their interaction with the surrounding environment. While GNSS localization systems have established themselves as reliable solutions for open-space scenarios, the same approach is not viable for indoor settings, where localization remains an open problem as it is witnessed by the lack of extensive literature on the topic. In this thesis, we address this challenge by proposing a dependable solution for small multi-rotor UAVs using a Visual Inertial Odometry localization system. Our KF-based localization system reconstructs the pose by fusing data from onboard sensors. The primary source of information stems from the recognition of AprilTags fiducial markers, strategically placed in known positions to form a “map”. Building upon prior research and thesis work conducted at our university, we extend and enhance this system. We begin with a concise introduction, followed by a justification of our chosen strategies based on the current state of the art. We provide an overview of the key theoretical, mathematical, and technical aspects that support our work. These concepts are fundamental to the design of innovative strategies that address challenges such as data fusion from different AprilTag recognition and the elimination of misleading measurements. To validate our algorithms and their implementation, we conduct experimental tests using two distinct platforms by using localization accuracy and computational complexity as performance indices to demonstrate the practical viability of our proposed system. By tackling the critical issue of indoor localization for aerial platforms, this thesis tries to give some contribution to the advancement of robotics technology, opening avenues for enhanced autonomy and efficiency across various domains

Implementation of the autonomous functionalities on an electric vehicle platform for research and education

Self-driving cars have recently captured the attention of researchers and car manufacturing markets. Depending upon the level of autonomy, the cars are made capable of traversing from one point to another autonomously. In order to achieve this, sophisticated sensors need to be utilized. A complex set of algorithms is required to use the sensors data in order to navigate the vehicle along the desired trajectory. Polaris is an electric vehicle platform provided for research and education purposes at Aalto University. The primary focus of the thesis was to utilize all the sensors provided in Polaris to their full potential. So that, essential data from each sensor is made available to be further utilized either by a specific automation algorithm or by some mapping routine. For any autonomous robotic system, the first step towards automation is localization. That is to determine the current position of the robot in a given environment. Different sensors mounted over the platform provide such measurements in different frames of reference. The thesis utilizes the GPS based localization solution combined with the LiDAR data and wheel odometry to perform autonomous tasks. Robot Operating System is used as the software development tool in thesis work. Autonomous tasks include the determination of the global as well as the local trajectories. The endpoints of the global trajectories are dictated by the set of predefined GPS waypoints. This is called target-point navigation. A path needs to be planned that avoids all the obstacles. Based on the planned path, a set of velocity commands are issued by the embedded controller. The velocity commands are then fed to the actuators to move the vehicle along the planned trajectory

Submap Matching for Stereo-Vision Based Indoor/Outdoor SLAM

Autonomous robots operating in semi- or unstructured environments, e.g. during search and rescue missions, require methods for online on-board creation of maps to support path planning and obstacle avoidance. Perception based on stereo cameras is well suited for mixed indoor/outdoor environments. The creation of full 3D maps in GPS-denied areas however is still a challenging task for current robot systems, in particular due to depth errors resulting from stereo reconstruction. State-of-the-art 6D SLAM approaches employ graph-based optimization on the relative transformations between keyframes or local submaps. To achieve loop closures, correct data association is crucial, in particular for sensor input received at different points in time. In order to approach this challenge, we propose a novel method for submap matching. It is based on robust keypoints, which we derive from local obstacle classification. By describing geometrical 3D features, we achieve invariance to changing viewpoints and varying light conditions. We performed experiments in indoor, outdoor and mixed environments. In all three scenarios we achieved a final 3D position error of less than 0.23% of the full trajectory. In addition, we compared our approach with a 3D RBPF SLAM from previous work, achieving an improvement of at least 27% in mean 2D localization accuracy in different scenarios

Reliable Navigation for SUAS in Complex Indoor Environments

Indoor environments are a particular challenge for Unmanned Aerial Vehicles (UAVs). Effective navigation through these GPS-denied environments require alternative localization systems, as well as methods of sensing and avoiding obstacles while remaining on-task. Additionally, the relatively small clearances and human presence characteristic of indoor spaces necessitates a higher level of precision and adaptability than is common in traditional UAV flight planning and execution. This research blends the optimization of individual technologies, such as state estimation and environmental sensing, with system integration and high-level operational planning. The combination of AprilTag visual markers, multi-camera Visual Odometry, and IMU data can be used to create a robust state estimator that describes position, velocity, and rotation of a multicopter within an indoor environment. However these data sources have unique, nonlinear characteristics that should be understood to effectively plan for their usage in an automated environment. The research described herein begins by analyzing the unique characteristics of these data streams in order to create a highly-accurate, fault-tolerant state estimator. Upon this foundation, the system built, tested, and described herein uses Visual Markers as navigation anchors, visual odometry for motion estimation and control, and then uses depth sensors to maintain an up-to-date map of the UAV\u27s immediate surroundings. It develops and continually refines navigable routes through a novel combination of pre-defined and sensory environmental data. Emphasis is put on the real-world development and testing of the system, through discussion of computational resource management and risk reduction

Autonomous Navigation in Complex Indoor and Outdoor Environments with Micro Aerial Vehicles

Micro aerial vehicles (MAVs) are ideal platforms for surveillance and search and rescue in confined indoor and outdoor environments due to their small size, superior mobility, and hover capability. In such missions, it is essential that the MAV is capable of autonomous flight to minimize operator workload. Despite recent successes in commercialization of GPS-based autonomous MAVs, autonomous navigation in complex and possibly GPS-denied environments gives rise to challenging engineering problems that require an integrated approach to perception, estimation, planning, control, and high level situational awareness. Among these, state estimation is the first and most critical component for autonomous flight, especially because of the inherently fast dynamics of MAVs and the possibly unknown environmental conditions. In this thesis, we present methodologies and system designs, with a focus on state estimation, that enable a light-weight off-the-shelf quadrotor MAV to autonomously navigate complex unknown indoor and outdoor environments using only onboard sensing and computation. We start by developing laser and vision-based state estimation methodologies for indoor autonomous flight. We then investigate fusion from heterogeneous sensors to improve robustness and enable operations in complex indoor and outdoor environments. We further propose estimation algorithms for on-the-fly initialization and online failure recovery. Finally, we present planning, control, and environment coverage strategies for integrated high-level autonomy behaviors. Extensive online experimental results are presented throughout the thesis. We conclude by proposing future research opportunities

Safe and accurate MAV Control, navigation and manipulation

This work focuses on the problem of precise, aggressive and safe Micro Aerial Vehicle (MAV) navigation as well as deployment in applications which require physical interaction with the environment. To address these issues, we propose three different MAV model based control algorithms that rely on the concept of receding horizon control. As a starting point, we present a computationally cheap algorithm which utilizes an approximate linear model of the system around hover and is thus maximally accurate for slow reference maneuvers. Aiming at overcoming the limitations of the linear model parameterisation, we present an extension to the first controller which relies on the true nonlinear dynamics of the system. This approach, even though computationally more intense, ensures that the control model is always valid and allows tracking of full state aggressive trajectories. The last controller addresses the topic of aerial manipulation in which the versatility of aerial vehicles is combined with the manipulation capabilities of robotic arms. The proposed method relies on the formulation of a hybrid nonlinear MAV-arm model which also takes into account the effects of contact with the environment. Finally, in order to enable safe operation despite the potential loss of an actuator, we propose a supervisory algorithm which estimates the health status of each motor. We further showcase how this can be used in conjunction with the nonlinear controllers described above for fault tolerant MAV flight. While all the developed algorithms are formulated and tested using our specific MAV platforms (consisting of underactuated hexacopters for the free flight experiments, hexacopter-delta arm system for the manipulation experiments), we further discuss how these can be applied to other underactuated/overactuated MAVs and robotic arm platforms. The same applies to the fault tolerant control where we discuss different stabilisation techniques depending on the capabilities of the available hardware. Even though the primary focus of this work is on feedback control, we thoroughly describe the custom hardware platforms used for the experimental evaluation, the state estimation algorithms which provide the basis for control as well as the parameter identification required for the formulation of the various control models. We showcase all the developed algorithms in experimental scenarios designed to highlight the corresponding strengths and weaknesses as well as show that the proposed methods can run in realtime on commercially available hardware.Open Acces

On-board Obstacle Avoidance in the Teleoperation of Unmanned Aerial Vehicles

Teleoperation von Drohnen in Umgebungen ohne GPS-Verbindung und wenig Bewegungsspielraum stellt den Operator vor besondere Herausforderungen. Hindernisse in einer unbekannten Umgebung erfordern eine zuverlässige Zustandsschätzung und Algorithmen zur Vermeidung von Kollisionen. In dieser Dissertation präsentieren wir ein System zur kollisionsfreien Navigation einer ferngesteuerten Drohne mit vier Propellern (Quadcopter) in abgeschlossenen Räumen. Die Plattform ist mit einem Miniaturcomputer und dem Minimum an Sensoren ausgestattet. Diese Ausstattung genügt den Anforderungen an die Rechenleistung. Dieses Setup ermöglicht des Weiteren eine hochgenaue Zustandsschätzung mit Hilfe einer Kaskaden-Architektur, sehr gutes Folgeverhalten bezüglich der kommandierten Geschwindigkeit, sowie eine kollisionsfreie Navigation. Ein Komplementärfilter berechnet die Höhe der Drohne, während ein Kalman-Filter Beschleunigung durch eine IMU und Messungen eines Optical-Flow Sensors fusioniert und in die Softwarearchitektur integriert. Eine RGB-D Kamera stellt dem Operator ein visuelles Feedback, sowie Distanzmessungen zur Verfügung, um ein Roboter-zentriertes Modell umliegender Hindernisse mit Hilfe eines Bin-Occupancy-Filters zu erstellen. Der Algorithmus speichert die Position dieser Hindernisse, auch wenn sie das Sehfeld des Sensors verlassen, mit Hilfe des geschätzten Zustandes des Roboters. Das Prinzip des Ausweich-Algorithmus basiert auf dem Ansatz einer modell-prädiktiven Regelung. Durch Vorhersage der wahrscheinlichen Position eines Hindernisses werden die durch den Operator kommandierten Sollwerte gefiltert, um eine mögliche Kollision mit einem Hindernis zu vermeiden. Die Plattform wurde experimentell sowohl in einer räumlich abgeschlossenen Umgebung mit zahlreichen Hindernissen als auch bei Testflügen in offener Umgebung mit natürlichen Hindernissen wie z.B. Bäume getestet. Fliegende Roboter bergen das Risiko, im Fall eines Fehlers, sei es ein Bedienungs- oder Berechnungsfehler, durch einen Aufprall am Boden oder an Hindernissen Schaden zu nehmen. Aus diesem Grund nimmt die Entwicklung von Algorithmen dieser Roboter ein hohes Maß an Zeit und Ressourcen in Anspruch. In dieser Arbeit präsentieren wir zwei Methoden (Software-in-the-loop- und Hardware-in-the-loop-Simulation) um den Entwicklungsprozess zu vereinfachen. Via Software-in-the-loop-Simulation konnte der Zustandsschätzer mit Hilfe simulierter Sensoren und zuvor aufgenommener Datensätze verbessert werden. Eine Hardware-in-the-loop Simulation ermöglichte uns, den Roboter in Gazebo (ein bekannter frei verfügbarer ROS-Simulator) mit zusätzlicher auf dem Roboter installierter Hardware in Simulation zu bewegen. Ebenso können wir damit die Echtzeitfähigkeit der Algorithmen direkt auf der Hardware validieren und verifizieren. Zu guter Letzt analysierten wir den Einfluss der Roboterbewegung auf das visuelle Feedback des Operators. Obwohl einige Drohnen die Möglichkeit einer mechanischen Stabilisierung der Kamera besitzen, können unsere Drohnen aufgrund von Gewichtsbeschränkungen nicht auf diese Unterstützung zurückgreifen. Eine Fixierung der Kamera verursacht, während der Roboter sich bewegt, oft unstetige Bewegungen des Bildes und beeinträchtigt damit negativ die Manövrierbarkeit des Roboters. Viele wissenschaftliche Arbeiten beschäftigen sich mit der Lösung dieses Problems durch Feature-Tracking. Damit kann die Bewegung der Kamera rekonstruiert und das Videosignal stabilisiert werden. Wir zeigen, dass diese Methode stark vereinfacht werden kann, durch die Verwendung der Roboter-internen IMU. Unsere Ergebnisse belegen, dass unser Algorithmus das Kamerabild erfolgreich stabilisieren und der rechnerische Aufwand deutlich reduziert werden kann. Ebenso präsentieren wir ein neues Design eines Quadcopters, um dessen Ausrichtung von der lateralen Bewegung zu entkoppeln. Unser Konzept erlaubt die Neigung der Propellerblätter unabhängig von der Ausrichtung des Roboters mit Hilfe zweier zusätzlicher Aktuatoren. Nachdem wir das dynamische Modell dieses Systems hergeleitet haben, synthetisierten wir einen auf Feedback-Linearisierung basierten Regler. Simulationen bestätigen unsere Überlegungen und heben die Verbesserung der Manövrierfähigkeit dieses neuartigen Designs hervor.The teleoperation of unmanned aerial vehicles (UAVs), especially in cramped, GPS-restricted, environments, poses many challenges. The presence of obstacles in an unfamiliar environment requires reliable state estimation and active algorithms to prevent collisions. In this dissertation, we present a collision-free indoor navigation system for a teleoperated quadrotor UAV. The platform is equipped with an on-board miniature computer and a minimal set of sensors for this task and is self-sufficient with respect to external tracking systems and computation. The platform is capable of highly accurate state-estimation, tracking of the velocity commanded by the user and collision-free navigation. The robot estimates its state in a cascade architecture. The attitude of the platform is calculated with a complementary filter and its linear velocity through a Kalman filter integration of inertial and optical flow measurements. An RGB-D camera serves the purpose of providing visual feedback to the operator and depth measurements to build a probabilistic, robot-centric obstacle state with a bin-occupancy filter. The algorithm tracks the obstacles when they leave the field of view of the sensor by updating their positions with the estimate of the robot's motion. The avoidance part of our navigation system is based on the Model Predictive Control approach. By predicting the possible future obstacles states, the UAV filters the operator commands by altering them to prevent collisions. Experiments in obstacle-rich indoor and outdoor environments validate the efficiency of the proposed setup. Flying robots are highly prone to damage in cases of control errors, as these most likely will cause them to fall to the ground. Therefore, the development of algorithm for UAVs entails considerable amount of time and resources. In this dissertation we present two simulation methods, i.e. software- and hardware-in-the-loop simulations, to facilitate this process. The software-in-the-loop testing was used for the development and tuning of the state estimator for our robot using both the simulated sensors and pre-recorded datasets of sensor measurements, e.g., from real robotic experiments. With hardware-in-the-loop simulations, we are able to command the robot simulated in Gazebo, a popular open source ROS-enabled physical simulator, using computational units that are embedded on our quadrotor UAVs. Hence, we can test in simulation not only the correct execution of algorithms, but also the computational feasibility directly on the robot's hardware. Lastly, we analyze the influence of the robot's motion on the visual feedback provided to the operator. While some UAVs have the capacity to carry mechanically stabilized camera equipment, weight limits or other problems may make mechanical stabilization impractical. With a fixed camera, the video stream is often unsteady due to the multirotor's movement and can impair the operator's situation awareness. There has been significant research on how to stabilize videos using feature tracking to determine camera movement, which in turn is used to manipulate frames and stabilize the camera stream. However, we believe that this process could be greatly simplified by using data from a UAV’s on-board inertial measurement unit to stabilize the camera feed. Our results show that our algorithm successfully stabilizes the camera stream with the added benefit of requiring less computational power. We also propose a novel quadrotor design concept to decouple its orientation from the lateral motion of the quadrotor. In our design the tilt angles of the propellers with respect to the quadrotor body are being simultaneously controlled with two additional actuators by employing the parallelogram principle. After deriving the dynamic model of this design, we propose a controller for this platform based on feedback linearization. Simulation results confirm our theoretical findings, highlighting the improved motion capabilities of this novel design with respect to standard quadrotors

Comparison of state marginalization techniques in visual inertial navigation filters

The main focus of this thesis is finding and validating an efficient visual inertial navigation system (VINS) algorithm for applications in micro aerial vehicles (MAV). A typical VINS for a MAV consists of a low-cost micro electro mechanical system (MEMS) inertial measurement unit (IMU) and a monocular camera, which provides a minimum payload sensor setup. This setup is highly desirable for navigation of MAVs because highly resource constrains in the platform. However, bias and noise of lowcost IMUs demand sufficiently accurate VINS algorithms. Accurate VINS algorithms has been developed over the past decade but they demand higher computational resources. Therefore, resource limited MAVs demand computationally efficient VINS algorithms. This thesis considers the following computational cost elements in the VINS algorithm: feature tracking front-end, state marginalization technique and the complexity of the algorithm formulation. In this thesis three state-of-the-art feature tracking front ends were compared in terms of accuracy. (VINS-Mono front-end, MSCKF-Mono feature tracker and Matlab based feature tracker). Four state-ofthe- art state marginalization techniques (MSCKF-Generic marginalization, MSCKFMono marginalization, MSCKF-Two way marginalization and Two keyframe based epipolar constraint marginalization) were compared in terms of accuracy and efficiency. The complexity of the VINS algorithm formulation has also been compared using the filter execution time. The research study then presents the comparative analysis of the algorithms using a publicly available MAV benchmark datasets. Based on the results, an efficient VINS algorithm is proposed which is suitable for MAVs