Degeneration-Aware Localization with Arbitrary Global-Local Sensor Fusion

Abstract

Global localization is a fundamental ability for mobile robots. Considering the limitation of single type of sensor, fusing measurements from multiple sensors with complementary properties is a valuable task for study. In this paper, we propose a decoupled optimization-based framework for global–local sensor fusion, which fuses the intermittent 3D global positions and high-frequent 6D odometry poses to infer the 6D global localization results in real-time. The fusion process is formulated as estimating the relative transformation between global and local reference coordinates, translational extrinsic calibration, and the scale of the local pose estimator. We validate the full observability of the system under general movements, and further analyze the degenerated movement patterns where some related system state would be unobservable. A degeneration-aware sensor fusion method is designed which detects the degenerated directions before optimization, and adds constraints specifically along these directions to relieve the effect of the noise. The proposed degeneration-aware global–local sensor fusion method is validated in both simulation and real-world datasets with different sensor configurations, and shows its effectiveness in terms of accuracy and robustness compared with other decoupled sensor fusion methods for global localization.

1. Introduction

Precise global localization is of high importance in many applications, such as navigation and mapping. When there is no access to a pre-built environment map, the robot can only obtain the global positioning information via global-aware sensors like global positioning system (GPS) and ultra wide band (UWB). Generally these sensors can access global positional information with bounded error. However, due to the installation limitation of the ground stations and the signal block from obstacles, their measurements might be intermittent and suffer from large noise. Besides, the 3 degree-of-freedom (3D) positioning measurements are inadequate for some applications that require 6 degree-of-freedom (6D) global pose estimation. On the other hand, local sensors that measure the local environmental or kinematic information can achieve consecutive 6D pose estimation in real time [1,2,3,4,5]. However, the poses are estimated relative to the origins of the trajectories, which is unobservable in the global reference frame. The estimation results would drift along with time and distance.

In this paper, we set to integrate the global and local sensors to achieve 6D global drift-free localization. To do this, the extrinsic calibration between sensors is required, and so is the mapping that transforms the local estimations to the global coordinate. Besides, the scale of the local poses is also required since some local estimators such as monocular visual odometry can not recover it. Many researches investigate the sensor-fusion problem on specific sensor configurations, and the methods can be divided into two categories.

One kind of fusing methods tightly couple the measurements from different sensors under the filtering or optimization based frameworks [6,7]. Generally, the reference frames of the local estimators are directly transformed to the global coordinates after initialization. During localization, the poses of the local estimators are propagated on the global coordinate. These methods are usually carefully designed for specific sensor configurations, and cannot be generalized to arbitrary configuration. To the best of our knowledge, none of these methods are compatible with the local estimators that cannot recover the absolute scale. Another kind of methods utilize the outputs from different pose estimation systems as observation, and loosely couple them to achieve global localization [2,8,9]. In this way, the pose estimation systems are decoupled, and the erroneous estimations or measurements from one system would not affect the others. Besides, the loosely-coupled methods are applicable to arbitrary sensor configurations due to the independency of the pose estimation front-ends [8,9,10].

However, few of these methods have analyzed the observability of the global–local sensor fusion system. As noted in [6], the sensor fusion systems would still remain globally unobservable even if the global measurements are given. Besides, the observability of the system would degenerate when the robot moves along certain motion patterns [11,12]. Many studies analyze the observability to guide the online movements or calibration design [11,13], but few of them pays attention to avoiding degeneration from worsening localization performance. One possible solution to address degeneration is to add heuristic constraints on the system, e.g., priors like constant velocity or planar motion models, which however would introduce false constraints on observable subspace if the hypothesis is violated. Some works [14,15] address the degeneration occurred in laser odometry and calibration, respectively. They first determine the unobservable directions according to the eigenvalues of systems, and truncate the state update along with these directions. These methods theoretically prevent noise from deteriorating unobservable states, but the eigenvalue threshold for observability distinction might change from case to case, and the optimizer has to be adapted for the truncated update.

In this paper, we propose a novel global–local sensor fusion framework that estimates the translational extrinsic calibration, the scale of the local poses, and the relative transformation from the reference frame of the local estimator to the global coordinate within a sliding-window, which are utilized to map the real-time local poses to the global frame for localization. We prove the full observability of this formulation under general motion patterns. To avoid the degeneration from deteriorating state estimation, we distinguish the degenerated subspace before each optimization, and add prior constraints specifically along with the unobservable directions. We validate the effectiveness of the proposed method in both simulated and real-world datasets with different sensor configurations. The experimental results also show that our method can effectively restrain the drift along degenerated directions and work out robust global localization results. To summarize, the contributions of the paper are listed as follow

• A novel global-local sensor fusion framework is proposed which is applicable to arbitrary sensor configurations. The observability and degenerated motion patterns of the proposed system are detailedly analyzed;
• A degeneration-aware optimization approach is designed to avoid the sensor fusion framework being deteriorated by the noise on the degenerated directions;
• Both simulated and real-world datasets with multiple sensor configurations are utilized to validate the generalization and effectiveness of the degeneration-aware sensor fusion framework for global localization.

2. Related Works

The study in this paper is relative to multi-sensor fusion and degeneration-aware state estimation. Considering the limitation of the single type of sensor, multi-sensor fusion is a widely researched area in robotics. Generally, the sensors selected for fusion possess complementary properties. For example, the monocular visual inertial system fuses external visual information with internal inertial information to estimate 6D pose with scale. In this paper, we pay attention to the fusion between local-aware sensors (e.g., light detection and ranging (LIDAR), camera, inertial measurement unit (IMU)), and global-aware sensors (e.g., GPS, Motion Capture) for localization. The fusion frameworks can be divided into filtering and optimization-based methods. Typically, most of the filtering-based frameworks are developed based on Extended Kalman Filter (EKF) [6,8,16] and utilize global and local information to update the propagated inertial data. To reduce the first-order linearization error occurred in the EKF, some systems achieve sensor fusion taking advantages of the other filtering solutions such Partical Filter [17,18] or Sigma-point Kalman Filter [19,20]. Lynen et al. [8] propose a general multi-sensor fusion (MSF) method that can utilize arbitrary global or local measurements to update the propagated pose, in which the scale of the local estimator and the extrinsic calibration could also be estimated during the localization. However, the inertial instrument is indispensable and the observability is not validated in this method. Lee et al. [6] tightly couple the visual, inertial, and GPS information for global localization, and simultaneously estimates the extrinsic calibration and time-offset between sensors. Optimization-based methods are proven to be more accurate than filtering-based methods [21]. During optimization, the system states are iteratively estimated based on all of the valid measurements, which however require a large number of computational resources. Many works relieve the computational burden by only maintaining a sliding-window of states to achieve efficient optimization [9,22,23]. However, few of these fusion methods address the degeneration problem during online localization.

Observability is the vital property of dynamic systems which reflects whether the inner states of the systems can be estimated from the observed measurements [24]. In the global–local sensor fusion tasks, Lee et al. [6] prove that if directly adding the relative transformation between the reference frames of the visual inertial odometry (VIO) and GPS systems into the original VIO state for estimation, there still exists four unobservable directions even though the global measurements exist. They address this problem by transforming the states of the local VIO system to the global frame to achieve full observability, which is inherently equivalent to marginalizing out the relative transformation between the reference frames of VIO and GPS systems after initialization, and directly estimating the state of the local VIO system in the global frame. This parameterization is also utilized in some of the other global–local sensor fusion methods [7,10]. Another approach decouples the local and global estimators and estimates the relative transformation between their reference frames [25]. In this way the output of the high-frequency local estimator can be directly transformed to the global frame without latency, and makes the system more robust as the local and global estimators are decoupled. In this paper, besides the relative transformation between reference frames, we further add the scale of the local estimator and the extrinsic calibration between local and global sensors into the state, which makes the system applicable to arbitrary sensor configurations. We also prove its full observability under general movements.

Even though the fusion system is proved to be fully observable in general conditions, the observability would degenerate if the robot moves with insufficiently excited motion. Ref. [11] proves in detail that there exist several motion patterns that would introduce unobservable directions even if the global measurement is provided in the inertial navigation system (INS) aided framework. However, few works propose efficient solutions towards preventing degeneration from deteriorating estimation results. Refs. [14,15] address the degeneration problem by projecting the incremental update to the observable direction, which needs to modify the update step in general optimizers. In this paper, we automatically detect the degenerated directions before optimization, and specifically add constraints based on the calculated directions to prevent the unobservable parameters from being deteriorated by noise, which can be easily plugged in many existing systems developed with general optimizers.

3. Method

In this section, first the notations used throughout the paper are presented. Then we introduce the formulation of the global–local sensor fusion system and its detailed implementation for batch optimization. As a special discussion, we show that some commonly-appeared motions in robotics are actually degenerated for sensor fusion. Finally, we present our degeneration-aware optimization method that is designed to address the degeneration problem occurred during online localization.

3.1. Notation

The nomenclature used throughout the paper is defined in

Table 1. Nomenclature.

Notation	Explanation
C	The sensor frame of the local estimator;
G	The sensor frame of the global estimator;
O	The reference frame of the local estimator;
W	The reference frame of the global estimator, which also is
	the world frame of the localization system;
L	The first frame in the sliding-window;
$\mathit{T}$	The transformation matrix in $\mathbb{SE}\left(3\right)$, where ${\mathit{T}}_B^A$ represents
	the relative transformation from frame B to frame A defi-
	ned in frame A;
$\mathit{R}$	The rotation matrix in $\mathbb{SO}\left(3\right)$, where ${\mathit{R}}_B^A$ represents the
	rotation from frame B to frame A;
$\mathit{q}$	Unit quaternion in Hamilton notation, with ${\mathit{q}}_B^A$ correspon-
	ding to ${\mathit{R}}_B^A$;
$\mathit{p}$	Translation vector in ${\mathbb{R}}^3$, where ${\mathit{p}}_B^A$ denotes the translation
	from frame B to frame A defined in frame A. ${\mathit{p}}_{B_x}^A,{\mathit{p}}_{B_y}^A$
	and ${\mathit{p}}_{B_z}^A$ denote the components of ${\mathit{p}}_B^A$ along $x,y,z$ axises;
$\mathit{x}$	The system state;
s	The scale of the local estimator in ${\mathbb{R}}^{+}$;
t	The timestamp of the received message;
n	The number of global measurements in the sliding-window;
${ϵ}_a,{ϵ}_b,{ϵ}_r$	The thresholds used for degeneration detection.

. We consider that the local estimator outputs 6D transformation ${\mathit{T}}_C^O$ from the local sensor frame C to the reference frame of the odometry O. We assume that the global estimator outputs 3D positional measurement ${\mathit{p}}_G^W$ of the global sensor frame G on the robot with respect to a fixed world frame W. To fuse the outputs from the global and local estimators, we estimate the extrinsic calibration ${\mathit{T}}_G^C$ between sensors, the scale s of the local estimator, and the relative transformation ${\mathit{T}}_O^W$ during online localization, which constitute the system state $\mathit{x}$. $\mathit{T}\left(s\right)$ denotes that the transformation $\mathit{T}$ is mapped to the real scale by s. We draw the relation of the frames in Figure 1.

3.2. Global–Local Sensor Fusion System

The proposed sensor fusion system is demonstrated in Figure 2. It receives outputs from arbitrary local and global estimators, and produces the global localization results at the highest frequency of the local estimator. We estimate the transformation ${\mathit{T}}_O^W$ to transform the output of the local estimator to the global frame instead of directly estimating the current pose in the global frame. To restrict the computational requirement and estimate the scale accurately, the fusion process is carried out on a sliding-window of recent n global sensor measurements and the related local sensor frames within the corresponding time duration, and we denote the first local sensor frame in the sliding-window as L. The length of the sliding-window in this work is maintained according to the distance, but can be modified by any other criterion. Upon the sliding-window based formulation, we change the estimated relative transformation from ${\mathit{T}}_O^W$ to ${\mathit{T}}_L^W$ and the system state is maintained accordingly as

$$\mathit{x}=\{{\mathit{R}}_L^W,{\mathit{p}}_L^W,{\mathit{p}}_G^C,s\}$$

Generally, the global sensors possess lower frequency than the local estimators. Therefore, the system performs the fusion process each time it receives the measurement from the global estimator. As shown in Figure 2, during each fusion process, we first align the newly received global measurement with the two closest local frames to obtain the accurate local pose at the exact timestamp of the global measurement. Then, we detect whether there exists degenerated subspace according to the information within the sliding-window. If the degeneration occurs, the corresponding degenerated directions would be calculated, which further are utilized to construct prior constraints for degeneration-aware optimization.

3.2.1. Measurement Alignment

We first align the global and local measurements according to the timestamps. The global measurement ${\mathit{p}}_{G_k}^W$ received at timestamp $t_k$ is aligned with two closest local measurements at timestamp $t_{ka}$ and $t_{kb}$ as shown in Figure 1. We interpolate the corresponding local pose ${\mathit{T}}_{C_k}^O$ at the timestamp $t_k$ according to

$$\begin{array}{cc}\hfill & \lambda =\frac{t_k-t_{ka}}{t_{kb}-t_{ka}}\hfill \\ \hfill {\mathit{q}}_{C_k}^O=& \mathrm{slerp}({\mathit{q}}_{C_{ka}}^O,{\mathit{q}}_{C_{kb}}^O,\lambda )\hfill \\ \hfill {\mathit{p}}_{C_k}^O=& (1-\lambda ){\mathit{p}}_{C_{ka}}^O+\lambda {\mathit{p}}_{C_{kb}}^O\hfill \end{array}$$

(1)

in which $\mathrm{slerp}(·)$ denotes the spherical linear interpolation function [26].

3.2.2. General Batch Optimization for Localization

After aligning the newly received global measurement, the system state $\mathit{x}=\{{\mathit{R}}_L^W,{\mathit{p}}_L^W,{\mathit{p}}_G^C,s\}$ is optimized according to the time-aligned measurements within the current sliding-window. The error function constructed by the measurements at timestamp $t_k$ is formulated as

$${\mathit{e}}_{p_k}={\overline{\mathit{p}}}_{G_k}^W-{\mathit{p}}_{G_k}^W$$

(2)

in which ${\overline{\mathit{p}}}_{G_k}^W$ is derived by

$$\begin{array}{cc}\hfill {\mathit{T}}_{C_k}^L=& \left[\begin{array}{cc}{\mathit{R}}_{C_k}^L& {\mathit{p}}_{C_k}^L\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]={\left({\mathit{T}}_L^O\right)}^{-1}{\mathit{T}}_{C_k}^O\hfill \\ \hfill {\mathit{T}}_{G_k}^W=& \left[\begin{array}{cc}{\mathit{R}}_{G_k}^W& {\mathit{p}}_{G_k}^W\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]={\mathit{T}}_L^W{\mathit{T}}_{C_k}^L\left(s\right){\mathit{T}}_G^C\hfill \\ \hfill =& \left[\begin{array}{cc}{\mathit{R}}_L^W& {\mathit{p}}_L^W\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\left[\begin{array}{cc}{\mathit{R}}_{C_k}^L& s{\mathit{p}}_{C_k}^L\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\left[\begin{array}{cc}{\mathit{R}}_G^C& {\mathit{p}}_G^C\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\hfill \\ \hfill {\overline{\mathit{p}}}_{G_k}^W=& {\mathit{R}}_L^W{\mathit{R}}_{C_k}^L{\mathit{p}}_G^C+s{\mathit{R}}_L^W{\mathit{p}}_{C_k}^L+{\mathit{p}}_L^W\hfill \\ \hfill =& {\mathit{R}}_L^W({\mathit{R}}_{C_k}^L{\mathit{p}}_G^C+s{\mathit{p}}_{C_k}^L)+{\mathit{p}}_L^W\hfill \end{array}$$

(3)

Summing up all the valid error functions in the sliding-window, the cost function for the general localization based optimization is defined as

$$\mathit{f}=\sum _k\rho ({\mathit{e}}_{p_k}^T{\mathsf{\Omega }}_e{\mathit{e}}_{p_k})$$

(4)

in which ${\mathsf{\Omega }}_e$ denotes the information matrix that can be derived as the inverse of the measurement covariance matrix.$\rho (·)$ represents the Huber’s robust kernel function [27] which can suppress the effect of outliers. In this work, we minimize this non-linear least-square cost function iteratively for system state optimization based on the Levenberg-Marquardt algorithm [28,29].

Given the latest optimized state $\mathit{x}$, we can transform the continuous output of the local estimator ${\mathit{T}}_{C_t}^L$ to the world frame for real-time 6D global localization. The 6D global localization at timestamp t can be calculated as

$${\mathit{T}}_{C_t}^W=\left[\begin{array}{cc}{\mathit{R}}_L^W{\mathit{R}}_{C_t}^L& s{\mathit{R}}_L^W{\mathit{p}}_{C_t}^L+{\mathit{p}}_L^W\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]$$

(5)

3.3. Observability Analysis

For the optimization formulated in Equation (4), we first analyze its observability in the general occasion. We linearize Equation (2) at the current estimate to compute the Jacobian

$${\mathit{H}}_k=\frac{\mathsf{\partial }{\mathit{e}}_{p_k}}{\mathsf{\partial }\mathit{x}}={\left[\begin{array}{cccc}{\mathsf{\Xi }}_k& {\mathit{I}}_{3\times 3}& {\mathit{R}}_L^W{\mathit{R}}_{C_k}^L& {\mathit{R}}_L^W{\mathit{p}}_{C_k}^L\end{array}\right]}_{3\times 10}$$

(6)

in which ${\mathsf{\Xi }}_k$ corresponds to the Jacobian of ${\mathit{R}}_L^W$ on Manifold

$${\mathsf{\Xi }}_k=-{\mathit{R}}_L^W[({\mathit{R}}_{C_k}^L{\mathit{p}}_G^C+s{\mathit{p}}_{C_k}^L)\times ]$$

(7)

We denote $[\mathit{a}\times ]$ as the $3\times 3$ skew-symmetric for vector $\mathit{a}={[a_x,a_y,a_z]}^T$, which is formulated as

$$\begin{array}{c}\hfill [\mathit{a}\times ]=\left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]\end{array}$$

(8)

As the system state $\mathit{x}$ changes slowly across time, the corresponding state transition matrix $\mathsf{\Phi }(t_0,t_k)$ between timestamps $t_0$ and $t_k$ is identity. The observability matrix $\mathit{M}$ can be constructed following [30]

$$\mathit{M}=\left[\begin{array}{c}⋮\\ {\mathit{H}}_k\mathsf{\Phi }(t_0,t_k)\\ ⋮\end{array}\right]=\left[\begin{array}{c}⋮\\ {\mathit{H}}_k\\ ⋮\end{array}\right]$$

(9)

As in general cases the columns in ${\mathit{H}}_k$ are linear independent. Thus based on Equation (9) it can be noticed that in general cases the rank of $\mathit{M}$ achieves 10 when $n\ge 4$. Therefore, when there are more than 4 global measurements in the sliding-window, the system is observable.

3.4. Degeneration Analysis

In this subsection, we summarize the common degenerated motion patterns in the robotics area and derive their corresponding unobservable directions.

3.4.1. Pure Translation along One Axis

This is a common motion pattern in autonomous driving. When the robot moves along a straight line, ${\mathit{R}}_{C_i}^L$ is invariant for any $i\in \left[m,m+n-1\right]$, in which m is utilized to denote the beginning of the sliding-window. The translational measurement of the local estimator can be derived as

$${\mathit{p}}_{C_i}^L={\alpha }_i{\overline{\mathit{p}}}_{m_0}^L$$

(10)

in which ${\overline{\mathit{p}}}_{m_0}^L$ is the unit vector of the translational direction and ${\alpha }_i$ is a scalar variable that can be derived as ${\alpha }_i=\parallel {\mathit{p}}_{C_i}^L\parallel /\parallel {\overline{\mathit{p}}}_{m_0}^L\parallel$. Thus, in ${\mathit{H}}_i$ the third block is constant, and the first two blocks are also linear dependent. The right null-space matrix $\mathit{N}$ of the observability matrix $\mathit{M}$ can be derived as

$$\mathit{N}={\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& {\overline{\mathit{p}}}_{m_0}^L\\ -{\mathit{R}}_L^W{\mathit{R}}_{C_i}^L& {\mathit{R}}_L^W[{\mathit{R}}_{C_i}^L{\mathit{p}}_G^C\times ]{\overline{\mathit{p}}}_{m_0}^L\\ {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 1}\\ 0& 0\end{array}\right]}_{10\times 4}$$

(11)

$\mathit{M}\mathit{N}=\mathbf{0}$ can be verified as ${\mathit{H}}_i\mathit{N}=\mathbf{0}$. Therefore, in this occasion, there is a four-dimensional unobservable subspace which is constructed as span($\mathit{N}$). The formation of $\mathit{N}$ shows that the first three column vectors are related to the translational components $\{{\mathit{p}}_L^W,{\mathit{p}}_G^C\}$, while the last column vector is related to one direction of ${\mathit{R}}_L^W$ which is parallel to the translational direction ${\overline{\mathit{p}}}_{m_0}^L$.

3.4.2. Random 3D Translation

This would occur when the robot performs pure translation in 2D/3D space without rotating. In this case, Equation (10) is violated while ${\mathit{R}}_{C_i}^L$ keeps invariant, which decreases the dimension of $\mathit{N}$ to 3

$$\mathit{N}={\left[\begin{array}{c}{\mathbf{0}}_{3\times 3}\\ -{\mathit{R}}_L^W{\mathit{R}}_{C_i}^L\\ {\mathit{I}}_{3\times 3}\\ 0\end{array}\right]}_{10\times 3}$$

(12)

We can find that ${\mathit{R}}_L^W$ is observable now, and there remain three unobservable directions that are related to the translational components $\{{\mathit{p}}_L^W,{\mathit{p}}_G^C\}$.

3.4.3. One-Axis Rotation

When the robot turns along a fixed rotation axis $\mathit{\omega }\in {\mathbb{R}}^3$ with non-zero translational velocity, only the translational components along $\mathit{\omega }$ remain unobservable. In this case, ${\mathit{R}}_{C_i}^L$ can be derived according to the Rodrigues’ rotation formula

$${\mathit{R}}_{C_i}^L={\mathit{R}}_{C_m}^L({\mathit{I}}_{3\times 3}+\sin \left({\theta }_i\right)\left[\mathit{\omega }\times \right]+(1-\cos \left({\theta }_i\right)){\left[\mathit{\omega }\times \right]}^2)$$

(13)

in which ${\theta }_i$ denotes the rotation angle of ${\mathit{R}}_{C_i}^{C_m}$. As $\left[\mathit{\omega }\times \right]\mathit{\omega }={\mathbf{0}}_{3\times 1}$, we can derive that multiplying the third block of ${\mathit{H}}_i$ by $\mathit{\omega }$ leads to

$${\mathit{R}}_L^W{\mathit{R}}_{C_i}^L\mathit{\omega }={\mathit{R}}_L^W{\mathit{R}}_{C_m}^L({\mathit{I}}_{3\times 3}+\sin \left({\theta }_i\right)\left[\mathit{\omega }\times \right]+(1-\cos \left({\theta }_i\right)){\left[\mathit{\omega }\times \right]}^2)\mathit{\omega }={\mathit{R}}_L^W{\mathit{R}}_{C_m}^L\mathit{\omega }$$

(14)

The result is constant for any $i\in \left[m,m+n-1\right]$. Thus, the null-space matrix that satisfies $\mathit{M}\mathit{N}=\mathbf{0}$ can be derived as

$$\mathit{N}={\left[\begin{array}{cccc}{\mathbf{0}}_{1\times 3}& -{\left({\mathit{R}}_L^W{\mathit{R}}_{C_m}^L\mathit{\omega }\right)}^T& {\mathit{\omega }}^T& 0\end{array}\right]}_{10\times 1}^T$$

(15)

It should be noticed that when the robot performs uniform motion, more dimensions maybe unobservable. One case is that, when both the angular velocity and the translational velocity are constant, the dimension of the unobservable subspace is expanded to 2 as one direction of ${\mathit{R}}_L^W$ that is along with the rotational axis would be undistinguishable. Another example is that, when the directions of the constant angular and translational velocity are perpendicular, the scale of the local estimator would be unobservable. These are likely to happen when the robot performs planar rotational movement, such as a car turns with constant velocity in autonomous driving. Detailed proof of the two claims are derived in the Appendix A.

3.5. Degeneration-Aware Batch Optimization

When there exists an unobservable subspace in the system, the related states are unconstrained and susceptible to measurement noise. To address this problem, we first detect the degenerated directions before the optimization process, then add prior constraints specifically along these directions.

Similarly to [14], we calculate the eigenvalues $\left\{{\lambda }_l\right\}$ and the corresponding eigenvectors $\left\{{\mathit{v}}_l\right\}$ by performing SVD decomposition on the Hessian matrix ${\mathit{H}}^T\mathit{H}$, in which $\mathit{H}={[{\mathit{H}}_m^T\cdots {\mathit{H}}_{m+n-1}^T]}^T$ is linearized at the latest estimated state before each optimization and $l=0\cdots 9$. We assume that $\left\{{\lambda }_l\right\}$ is sorted and ${\lambda }_9$ corresponds to the smallest eigenvalue. Theoretically the eigenvalues corresponding to the unobservable dimensions are precisely zero, while in practice they are not because of the system noise. It is apparent that a constant threshold $ϵ$ is not sufficient to distinguish zero eigenvalues for all kinds of systems, calling for more flexible construction of the threshold. We also quantitatively analyze the influence of noise on eigenvalues in Section 4.1.

We propose a heuristic method to detect the unobservable subspace as shown in Algorithm 1. The absolute thresholds ${ϵ}_a$ and ${ϵ}_b$ define the coarse but valid upper and lower bounds distinguishing the unobservable and observable subspaces. For the eigenvalue that distributes between the two bounds, it is difficult to directly categorize it by a threshold due to the unrestricted range of noise. We compute its ratio with the adjacent eigenvalues to indicate whether they have the same type of observability. If the ratio is larger than threshold ${ϵ}_r$, we assume that the adjacent eigenvalues have the same type of observability. As the method traverses from the smallest eigenvalue, the eigenvalue is categorized as relating to the unobservable subspace if it shares the same type of observability with its previous one.

Algorithm 1: Detecting unobservable directions.

After detecting out the degenerated subspace with dimension r, we collect the corresponding eigenvectors ${\mathit{V}}_u=\{{\mathit{v}}_0,\cdots ,{\mathit{v}}_{r-1}\}$ which denote the degenerated directions. Instead of projecting the incremental update to the observable directions during optimization, we restrict the update in unobservable directions by adding constraints

$$\begin{array}{cc}\hfill {\mathit{e}}_{R_j}& =log(({\widehat{\mathit{R}}}_L^W{)}^T{\mathit{R}}_L^W){\mathit{v}}_{j(0:3)}\hfill \\ \hfill {\mathit{e}}_{p_{Lj}^W}& =({\widehat{\mathit{R}}}_L^W{)}^T({\mathit{p}}_L^W-{\widehat{\mathit{p}}}_L^W){\mathit{v}}_{j(3:6)}\hfill \\ \hfill {\mathit{e}}_{e_j}& =({\widehat{\mathit{p}}}_G^C-{\mathit{p}}_G^C){\mathit{v}}_{j(6:9)}\hfill \\ \hfill {\mathit{e}}_{s_j}& =(\widehat{s}-s){\mathit{v}}_{j\left(9\right)}\hfill \end{array}$$

(16)

in which $j\in [0,r-1]$. ${\mathit{e}}_{R_j},{\mathit{e}}_{p_{Lj}^W}$, ${\mathit{e}}_{e_j}$ and ${\mathit{e}}_{s_j}$ denote the constraints applied on ${\mathit{R}}_L^W,{\mathit{p}}_L^W$, ${\mathit{p}}_G^C$ and s, respectively. $\left(\widehat{·}\right)$ denotes the related state estimation before optimization. ${\mathit{v}}_{j(a:b)}$ represents the vector that is compromised with the ath to bth elements of ${\mathit{v}}_j$. These error terms can be considered as priors to prevent the states from drifting in the unobservable subspace, and are only added for optimization when $\parallel {\mathit{v}}_{j(a:b)}\parallel >0.1$, which means that the prior constraint on this state is activated. Finally the cost function in the proposed degeneration-aware batch optimization is

$$\begin{array}{cc}\hfill \mathit{f}=& \sum _{i=m}^{m+n-1}\rho ({\mathit{e}}_{p_i}^T{\mathsf{\Omega }}_e{\mathit{e}}_{p_i})+\sum _{j\in {\mathit{S}}_R}{\mathit{e}}_{R_j}^T{\mathsf{\Omega }}_p{\mathit{e}}_{R_j}+\sum _{j\in {\mathit{S}}_{p_L^W}}{\mathit{e}}_{p_{Lj}^W}^T{\mathsf{\Omega }}_p{\mathit{e}}_{p_{Lj}^W}\hfill \\ \hfill & +\sum _{j\in {\mathit{S}}_e}{\mathit{e}}_{e_j}^T{\mathsf{\Omega }}_p{\mathit{e}}_{e_j}+\sum _{j\in {\mathit{S}}_s}{\mathit{e}}_{s_j}{\mathsf{\Omega }}_{ps}{\mathit{e}}_{s_j}\hfill \end{array}$$

(17)

in which ${\mathit{S}}_R,{\mathit{S}}_{p_L^W},{\mathit{S}}_e$ and ${\mathit{S}}_s$ denote the sets of activated prior constraints on the system states, respectively. ${\mathsf{\Omega }}_p$ and ${\mathsf{\Omega }}_{ps}$ are the information matrices of the prior constraints.

4. Experimental Results

We validate the effectiveness of our degeneration-aware sensor fusion method through both simulated and real-world datasets. In simulation experiments, we first show the sensitivity of the eigenvalues with respect to the measurement noise. Then we create four sequences of simulated trajectories that correspond to the common degenerated motion patterns analyzed in Section 3.4, and utilize them to demonstrate the effect of degeneration on state estimation and the effectiveness of our proposed method in terms of preventing the state from drifting along the degenerated directions. In real-world experiments, we show the localization performance on indoor EuRoC [31] and outdoor KAIST [32] datasets. EuRoC is a visual-inertial dataset collected by a micro aerial vehicle that performs 6D movements. We utilize the 3D Leica MS50 laser tracker as the 3D global estimator and only utilize the left images to perform monocular visual odometry, which serves as the local pose estimator with no access to the absolute scale. KAIST dataset is collected in outdoor urban environment on a car and we utilize the GPS as global estimator and stereo-IMU information to perform local visual-inertial odometry. During the experiments, we set the length of the sliding-window as 10 m for indoor dataset and 50 m for outdoor datasets. All the experiments are executed on a laptop with a 2.70 GHz Intel Core i7-7500U CPU. The codes are implemented in C++ and the optimization process is implemented with Levenberg-Marquardt algorithms in g2o [33].

In the simulation experiments, different motion patterns are controlled by setting different translational and rotational velocities for the local sensor. The minimum sampling interval is 0.1 s, and the ground truth of the trajectories can be integrated based on the velocities. The trajectories of the global sensor can be calculated based on the ground truth of the system state using Equation (3). The global measurements are simulated by corrupted the ground truth of the trajectories with Gaussian noise. To simulate the drift of the local estimator, we add Gaussian noise on the relative pose in each sampling interval, and integrate the corrupted relative pose to formulate the local measurements. As the ground truth of the system state can be accessed in the simulation experiments, the performances of different methods are evaluated by comparing the state estimation errors.

In real-world experiments, we evaluate the performances of different methods by the absolute trajectory error (ATE) and rotational error [34]. For each global pose ${\mathit{T}}_{C_t}^W$ estimated at timestamp t, we find its corresponding ground truth pose ${\mathit{T}}_{C_t}^{W*}$ provided by the datasets according to the timestamp. The localization error is derived as

$${\mathit{E}}_t={\left({\mathit{T}}_{C_t}^{W*}\right)}^{-1}{\mathit{T}}_{C_t}^W$$

(18)

The ATE error relates to the norm of the translational component in ${\mathit{E}}_t$, and the rotational error relates to the rotational component in ${\mathit{E}}_t$.

4.1. Sensitivity of the Eigenvalues

To quantitatively demonstrate the influence of the noise on the eigenvalues, we create simulated trajectories that are related to two types of the degenerated motion patterns shown on the first row of Figure 3. We add Gaussian noise on the simulated measurement and show the changing of the related eigenvalues. As the derivation of eigenvalues relates to the measurements of the local estimator, we, respectively, add Gaussian noises with standard deviations from 0 to 10 cm on the translational parts of the local measurements, and add Gaussian noises with standard deviations from 0 to 6 deg on the rotational parts. To avoid the influence of randomness, for each standard deviation we generate 200 trajectories for eigenvalue evaluation. Figure 3a shows the trajectory generated when the robot moves along a straight line and the eigenvalues drawn in Figure 3c,e are influenced by translational and rotational noises on the local estimators with different standard deviations, respectively. To keep the scale of the axis for better demonstration, we do not draw the value of the 5th eigenvalue influenced by the rotational noise in Figure 3e, which are around the same values as shown in the results that influenced by the translational noise. The right column includes the results tested when the robot moves with random one-axis rotation. The trajectory is shown in Figure 3b. The eigenvalues drawn in Figure 3d,f are influenced by the translational and rotational Gaussian noises, respectively. The curves in each figures denote the means of the eigenvalues and the colored region surrounding the curve represents the statistic of 200 trials. Some colored regions are hard to be distinguished as they are too close to the curve in the related axis scale.

Based on our analysis in Section 3.4, when the robot moves along a straight line, there should be four unobservable directions, which means that the smallest four eigenvalues should be close to zero. When the robot moves with non-constant one-axis rotation, there should be only one unobservable direction. We, respectively, draw the smallest five and three system eigenvalues tested on the two trajectories in the two columns of Figure 3. The results show that when the noise is small, the eigenvalues corresponding to the unobservable subspace are close to zeros (i.e., the 6–9th eigenvalues on the first column and the 9th eigenvalue on the second column), and become larger when the noise grows. Although the eigenvalues corresponding to the observable subspace are generally large. Thus we could set the coarse upper bound ${ϵ}_a$ and lower bound ${ϵ}_b$ to distinguish eigenvalues. However, we can notice that the 6th eigenvalue on the left column is close to the 7th and 8th eigenvalues on the right column. If we utilize one constant threshold, it is difficult to distinguish the unobservable and observable subspaces in this situation. In our proposed detection method, the difference between the adjacent eigenvalues is taken into consideration, which contributes to handle this problem. Based on the analysis of these results, we set ${ϵ}_a=5,{ϵ}_b={10}^{-2},{ϵ}_r=0.1$ throughout the left of the experiments.

4.2. Simulation Results

In this subsection, we utilize simulated data collected under degenerated movement patterns to perform ablation study on the effectiveness of the proposed degeneration-aware sensor fusion. Specifically, we evaluate the accuracy of the estimated system state optimized with general optimization formulated in Equation (4) and the optimization with the degeneration-aware constraints (DC) in Equation (17), respectively. The simulated data includes four types of trajectories in which the robot moves (a) along a straight line, (b) with random 3D translation, (c) with perpendicular constant rotational and translational velocity, and (d) with random one-axis rotation. The motion patterns are controlled by the translational and rotational velocities. We set the sampling time as 0.1 s and the trajectories are integrated based on the velocity. We add zero-mean Gaussian noise on the relative pose within each sampling time to simulate the measurement noise of the local estimator and integrate the relative poses to simulate the local measurements. The 3D global measurements are collected every 1 s along the ground truth of each trajectory and are also corrupted by the noise. The standard deviations of the rotational and translational noises for the local estimator are 0.05 rad and 0.05 m. The standard deviation of the translational noise for the global estimator is 0.5 m.

The estimation errors for the system state $\mathit{x}$ are drawn in Figure 4 and we represent the rotational error of ${\mathit{R}}_L^W$ in Euler Angle parameterization denoting as “yaw”, “pitch”, and “roll” in the first three rows of the figure. Based on Equation (11), when the robot moves along a straight line, the dimension of the unobservable subspace is four, which relates to one rotational axis of ${\mathit{R}}_L^W$ along the moving direction and 3D translational directions that are coupled in ${\mathit{p}}_L^W$ and ${\mathit{p}}_G^C$. These can be reflected from the results in the first column of Figure 4 indicated by dashed lines which do not converge in the related dimensions. On the other hand, the results optimized with the proposed degeneration-aware constraints, though could not converge to the ground truth in the unobservable directions as no valid information is provided, do not diverge during the whole process. The results in the second to fourth columns also validate our analysis in Section 3.4. Additionally, it is interesting to notice that, as one direction of the unobservable space might be related to several state variables, r dimensions of unobservable directions would cause the divergence of more than r state variables.

4.3. Real World Experiments

We evaluate the global localization performance of our proposed method on sequences MH01–MH05 in EuRoC datasets and sequence Urban 38 in KAIST dataset.We implement the monocular visual odometry based on ORB-SLAM [1] and the stereo visual inertial odometry based on Openvins [3]. We utilize the absolute trajectory error (ATE) [34] to evaluate the global localization performance and compare our results with the open-sourced multi-sensor fusion methods MSF [8] and VINS-Fusion [10].

4.3.1. EuRoC Dataset

In EuRoC dataset we utilize monocular vision to perform local pose estimation. The output of the monocular visual odometry does not include the absolute scale, which can not be utilized for global localization in many other global–local fusion methods [7,9,10]. Thus, to test the performance of VINS-Fusion [10], we change its local estimator into VINS-MONO [2], which is the classical monocular VIO provided by the VINS-Fusion system.

We draw ATE errors evaluated by different methods in Figure 5, and list the rotational errors in

Table 2. Median rotational errors (deg) tested on EuRoC datasets.

	MH01	MH02	MH03	MH04	MH05
ours	1.05	0.63	1.23	1.57	0.91
full priors	1.51	1.01	1.52	4.98	3.26
no prior	1.80	0.75	2.70	2.12	1.96
VINS-Fusion	1.42	1.13	2.35	2.56	2.52
MSF	1.40	1.63	3.15	3.29	3.73

. The results of VINS-MONO is evaluated by aligning the trajectories to the ground truth. The results show that our method could successfully fuse 3D global positions and 6D scaleless local poses for global localization, and outperform the compared methods in terms of both translational and rotational localization accuracy. We can notice that in many sequences VINS-MONO also performances well as the trajectories are not long, but the fusion results in VINS-Fusion show larger errors as the extrinsic parameter is not estimated in VINS-Fusion. Besides, compared with MSF and VINS-Fusion, our method does not require IMU data to support sensor fusion, which further demonstrates its efficiency and versatility for sensor fusion based global localization.

We also perform ablation study to evaluate the effectiveness of the degeneration-aware optimization. We first evaluate the localization performance tested on the general batch optimization formulated in Equation (4). The results are denoted as “no prior” in Figure 5. Besides, to validate the benefit of adding prior constraints specifically on the degenerated directions, we also test the localization performance by adding the full prior constraints for the whole system state. The results are denoted as “full priors” in Figure 5. From the results we can infer that, the degeneration constraints can successfully improve the localization accuracy as the influence from the noise in degenerated directions is restrained. Besides, adding constraints especially on degenerated directions can also improve the localization accuracy as the observable elements in the system state could be optimized directly without influenced by heuristic constraints which might be inaccurate.

4.3.2. KAIST Dataset

In KAIST dataset, we utilize the GPS as the global estimator, which is prone to drift when the car stops at the crossroads. Additionally, the GPS signal is inaccurate when the covariance is large. Therefore, during online localization, we drop the GPS messages with large covariance (the threshold is set as 60) and the drifted messages indicating that the car is moving while the local estimator denoting that the car does not move.

We list the localization performance of GPS and the stereo-IMU version of Openvins [3] in

Table 3. ATE error (m) tested on KAIST dataset.

	GPS	Openvins	MSF	VINS-Fusion	Full Priors	No Prior	Ours
mean	5.98	14.43	21.14	9.64	9.20	/	5.58
rmse	7.15	16.46	22.09	11.11	12.21	/	6.71
std	3.91	7.91	14.47	5.52	6.40	/	3.73

as baseline, and also compare our results with MSF [8], VINS-Fusion [10], and full priors aided results. The translational and heading errors are listed in Table 3 and

Table 4. Median heading errors (deg) tested on KAIST dataset.

MSF	Full Priors	VINS-Fusion	Ours
12.8	1.48	1.56	1.43

. The corresponding trajectories are drawn in Figure 6. The dropped GPS signals are also marked in the trajectory.

Compared with EuRoC datasets, the robot performs planar movements in Kaist dataset, in which the degeneration would likely occur. Thus, the results of “no prior” are largely influenced by the degeneration and fail to perform the whole localization. The results also show that our method achieves better performance than the compared methods and could provide drift-free global localization results.

4.4. Computational Cost Evaluation

To validate the ability of achieving real-time localization, we evaluate the computational time cost on the multi-sensor fusion process. The computational time is defined as the duration between the time at which a new global measurement is received and the time the system state is updated by the optimization results, which is independent with the processes in the front-ends. We collect the computational time tested on the KAIST dataset and draw the results in Figure 7, in which we specifically color the the time cost on “measurement alignment” and “degeneration detection” as red. As the fusion process is taken place within a sliding-window, the computational time would not increase with time. The average processing time is 18.4 ms, which satisfies the requirement of achieving real-time global–local sensor fusion. Besides, the average time of “before optimization” is 0.45 ms, which demonstrates that our solution for the degeneration motion would not introduce large computational burden towards the fusion process.

5. Conclusions

To achieve drift-free 6D global localization, in this paper we propose a general optimization-based framework that fuses the 3D intermittent global positioning information and 6D local odometry for real-time global pose estimation. The system state is specifically formulated to be fully observable under general motions. To address the impact from generated motion patterns, we propose a degeneration-aware solution to robustly detect the degenerated directions that are further utilized as indicators to add prior constraints. Though extra time is cost for degeneration detection, the whole processing time could satisfy the requirement of real-time sensor fusion. The effectiveness of our proposed method is validated on both simulated and real-world dataset. In the future work, we set to extend the configuration of our framework from 3D–6D global–local combination to multiple types of pose estimators with general degree of freedom, and utilize the multi-sensor information to further detect the failure in the local pose estimators for outlier rejection.