A Linear MPC with Control Barrier Functions for Differential Drive Robots

The need for fully autonomous mobile robots has surged over the past decade, with the imperative of ensuring safe navigation in a dynamic setting emerging as a primary challenge impeding advancements in this domain. In this paper, a Safety Critical Model Predictive Control based on Dynamic Feedback Linearization tailored to the application of differential drive robots with two wheels is proposed to generate control signals that result in obstacle-free paths. A barrier function introduces a safety constraint to the optimization problem of the Model Predictive Control (MPC) to prevent collisions. Due to the intrinsic nonlinearities of the differential drive robots, computational complexity while implementing a Nonlinear Model Predictive Control (NMPC) arises. To facilitate the real-time implementation of the optimization problem and to accommodate the underactuated nature of the robot, a combination of Linear Model Predictive Control (LMPC) and Dynamic Feedback Linearization (DFL) is proposed. The MPC problem is formulated on a linear equivalent model of the differential drive robot rendered by the DFL controller. The analysis of the closed-loop stability and recursive feasibility of the proposed control design is discussed. Numerical experiments illustrate the robustness and effectiveness of the proposed control synthesis in avoiding obstacles with respect to the benchmark of using Euclidean distance constraints. Keywords: Model Predictive Control, MPC, Autonomous Ground Vehicles, Nonlinearity, Dynamic Feedback Linearization, Optimal Control, Differential Robots.

UWB Ranging and IMU Data Fusion: Overview and Nonlinear Stochastic Filter for Inertial Navigation

This paper proposes a nonlinear stochastic complementary filter design for inertial navigation that takes advantage of a fusion of Ultra-wideband (UWB) and Inertial Measurement Unit (IMU) technology ensuring semi-global uniform ultimate boundedness (SGUUB) of the closed loop error signals in mean square. The proposed filter estimates the vehicle's orientation, position, linear velocity, and noise covariance. The filter is designed to mimic the nonlinear navigation motion kinematics and is posed on a matrix Lie Group, the extended form of the Special Euclidean Group $\mathbb{SE}_{2}\left(3\right)$. The Lie Group based structure of the proposed filter provides unique and global representation avoiding singularity (a common shortcoming of Euler angles) as well as non-uniqueness (a common limitation of unit-quaternion). Unlike Kalman-type filters, the proposed filter successfully addresses IMU measurement noise considering unknown upper-bounded covariance. Although the navigation estimator is proposed in a continuous form, the discrete version is also presented. Moreover, the unit-quaternion implementation has been provided in the Appendix. Experimental validation performed using a publicly available real-world six-degrees-of-freedom (6 DoF) flight dataset obtained from an unmanned Micro Aerial Vehicle (MAV) illustrating the robustness of the proposed navigation technique. Keywords: Sensor-fusion, Inertial navigation, Ultra-wideband ranging, Inertial measurement unit, Stochastic differential equation, Stability, Localization, Observer design.

Nonlinear Deterministic Observer for Inertial Navigation using Ultra-wideband and IMU Sensor Fusion

Navigation in Global Positioning Systems (GPS)-denied environments requires robust estimators reliant on fusion of inertial sensors able to estimate rigid-body's orientation, position, and linear velocity. Ultra-wideband (UWB) and Inertial Measurement Unit (IMU) represent low-cost measurement technology that can be utilized for successful Inertial Navigation. This paper presents a nonlinear deterministic navigation observer in a continuous form that directly employs UWB and IMU measurements. The estimator is developed on the extended Special Euclidean Group $\mathbb{SE}_{2}\left(3\right)$ and ensures exponential convergence of the closed loop error signals starting from almost any initial condition. The discrete version of the proposed observer is tested using a publicly available real-world dataset of a drone flight. Keywords: Ultra-wideband, Inertial measurement unit, Sensor Fusion, Positioning system, GPS-denied navigation.

Observer-based Controller for VTOL-UAVs Tracking using Direct Vision-Aided Inertial Navigation Measurements

This paper proposes a novel observer-based controller for Vertical Take-Off and Landing (VTOL) Unmanned Aerial Vehicle (UAV) designed to directly receive measurements from a Vision-Aided Inertial Navigation System (VA-INS) and produce the required thrust and rotational torque inputs. The VA-INS is composed of a vision unit (monocular or stereo camera) and a typical low-cost 6-axis Inertial Measurement Unit (IMU) equipped with an accelerometer and a gyroscope. A major benefit of this approach is its applicability for environments where the Global Positioning System (GPS) is inaccessible. The proposed VTOL-UAV observer utilizes IMU and feature measurements to accurately estimate attitude (orientation), gyroscope bias, position, and linear velocity. Ability to use VA-INS measurements directly makes the proposed observer design more computationally efficient as it obviates the need for attitude and position reconstruction. Once the motion components are estimated, the observer-based controller is used to control the VTOL-UAV attitude, angular velocity, position, and linear velocity guiding the vehicle along the desired trajectory in six degrees of freedom (6 DoF). The closed-loop estimation and the control errors of the observer-based controller are proven to be exponentially stable starting from almost any initial condition. To achieve global and unique VTOL-UAV representation in 6 DoF, the proposed approach is posed on the Lie Group and the design in unit-quaternion is presented. Although the proposed approach is described in a continuous form, the discrete version is provided and tested. Keywords: Vision-aided inertial navigation system, unmanned aerial vehicle, vertical take-off and landing, stochastic, noise, Robotics, control systems, air mobility, observer-based controller algorithm, landmark measurement, exponential stability.

Exponentially Stable Observer-based Controller for VTOL-UAV without Velocity Measurements

There is a great demand for vision-based robotics solutions that can operate using Global Positioning Systems (GPS), but are also robust against GPS signal loss and gyroscope failure. This paper investigates the estimation and tracking control in application to a Vertical Take-Off and Landing (VTOL) Unmanned Aerial Vehicle (UAV) in six degrees of freedom (6 DoF). A full state observer for the estimation of VTOL-UAV motion parameters (attitude, angular velocity, position, and linear velocity) is proposed on the Lie Group of $\mathbb{SE}_{2}\left(3\right)\times\mathbb{R}^{3}$ $=\mathbb{SO}\left(3\right)\times\mathbb{R}^{9}$ with almost globally exponentially stable closed loop error signals. Thereafter, a full state observer-based controller for the VTOL-UAV motion parameters is proposed on the Lie Group with a guaranteed almost global exponential stability. The proposed approach produces good results without the need for angular and linear velocity measurements (without a gyroscope and GPS signals) utilizing only a set of known landmarks obtained by a vision-aided unit (monocular or stereo camera). The equivalent quaternion representation on $\mathbb{S}^{3}\times\mathbb{R}^{9}$ is provided in the Appendix. The observer-based controller is presented in a continuous form while its discrete version is tested using a VTOL-UAV simulation that incorporates large initial error and uncertain measurements. The proposed observer is additionally tested experimentally on a real-world UAV flight dataset. Keywords: Unmanned aerial vehicle, nonlinear filter algorithm, autonomous navigation, tracking control, feature measurement, observer-based controller, localization, exponential stability, asymptotic stability, inertial measurement unit (IMU), Global Positioning Systems (GPS), vision aided inertial navigation system.

Stochastic Observer for SLAM on the Lie Group

A robust nonlinear stochastic observer for simultaneous localization and mapping (SLAM) is proposed using the available uncertain measurements of angular velocity, translational velocity, and features. The proposed observer is posed on the Lie Group of $\mathbb{SLAM}_{n}\left(3\right)$ to mimic the true stochastic SLAM dynamics. The proposed approach considers the velocity measurements to be attached with an unknown bias and an unknown Gaussian noise. The proposed SLAM observer ensures that the closed loop error signals are semi-globally uniformly ultimately bounded. Simulation results demonstrates the efficiency and robustness of the proposed approach, revealing its ability to localize the unknown vehicle, as well as mapping the unknown environment given measurements obtained from low-cost units.

A Geometric Nonlinear Stochastic Filter for Simultaneous Localization and Mapping

Simultaneous Localization and Mapping (SLAM) is one of the key robotics tasks as it tackles simultaneous mapping of the unknown environment defined by multiple landmark positions and localization of the unknown pose (i.e., attitude and position) of the robot in three-dimensional (3D) space. The true SLAM problem is modeled on the Lie group of $\mathbb{SLAM}_{n}\left(3\right)$, and its true dynamics rely on angular and translational velocities. This paper proposes a novel geometric nonlinear stochastic estimator algorithm for SLAM on $\mathbb{SLAM}_{n}\left(3\right)$ that precisely mimics the nonlinear motion dynamics of the true SLAM problem. Unlike existing solutions, the proposed stochastic filter takes into account unknown constant bias and noise attached to the velocity measurements. The proposed nonlinear stochastic estimator on manifold is guaranteed to produce good results provided with the measurements of angular velocities, translational velocities, landmarks, and inertial measurement unit (IMU). Simulation and experimental results reflect the ability of the proposed filter to successfully estimate the six-degrees-of-freedom (6 DoF) robot's pose and landmark positions. Keywords: Simultaneous Localization and Mapping, nonlinear stochastic observer for SLAM, stochastic differential equations, pose estimator, position, attitude, Brownian motion process, inertial measurement unit, landmarks, features, SDE, SO(3), SE(3), SLAM.

Nonlinear Filter for Simultaneous Localization and Mapping on a Matrix Lie Group using IMU and Feature Measurements

Simultaneous Localization and Mapping (SLAM) is a process of concurrent estimation of the vehicle's pose and feature locations with respect to a frame of reference. This paper proposes a computationally cheap geometric nonlinear SLAM filter algorithm structured to mimic the nonlinear motion dynamics of the true SLAM problem posed on the matrix Lie group of $\mathbb{SLAM}_{n}\left(3\right)$. The nonlinear filter on manifold is proposed in continuous form and it utilizes available measurements obtained from group velocity vectors, feature measurements and an inertial measurement unit (IMU). The unknown bias attached to velocity measurements is successfully handled by the proposed estimator. Simulation results illustrate the robustness of the proposed filter in discrete form demonstrating its utility for the six-degrees-of-freedom (6 DoF) pose estimation as well as feature estimation in three-dimensional (3D) space. In addition, the quaternion representation of the nonlinear filter for SLAM is provided. Keywords: Simultaneous Localization and Mapping, Nonlinear observer algorithm for SLAM, inertial measurement unit, inertial vision system, pose, position, attitude, landmark, estimation, IMU, SE(3), SO(3), unmanned aerial vehicle, rigid-body, noise, nonlinear observer for SLAM, Gaussian filter, Kalman filtering, navigation.