Research on Visual/Ins Integrated Navigation Technology Based on FGO

1. Introduction

Since the navigation and positioning information is the basic information to realize the functions of control, obstacle avoidance, path planning, the research on navigation and positioning system has become a research hotspot in the field of unmanned vehicle. Users' demands for navigation and positioning systems are mainly: high accuracy, high reliability, high real-time and high availability. Currently, there are three main types of navigation and positioning systems: 1) Global Navigation Satellite System (GNSS), which has high positioning accuracy, stable error, and can provide continuous real-time high-precision positioning information in the outdoor open area, but it is easily affected by the environmental impacts such as building obstruction and wall reflections; 2) Inertial Navigation System (INS), which has good anti-interference ability, high short-term accuracy, but the error accumulation is fast; 3) visual navigation and positioning system, which can provide rich environmental information and can directly estimate the position, but is easily affected by environmental interference. Because of the limitations of the above navigation systems, multi-sensor fusion localization technology came into being [1].

Multi-sensor fusion localization refers to synthesizing information from different sensors describing a certain target or environmental feature to achieve high-precision localization. This technique can overcome the limitations of using single sensor information for positioning, make full use of the complementary nature of each information source, and continuously and robustly output high-precision navigation information. The more mature data fusion algorithms can be summarized as filter-based methods and optimization-based methods [2,3]. Data fusion based on filtering commonly used Extended Kalman Filter (EKF), the method in the Jacobian matrix linearization stage, only retains the Taylor series first-order information, can not make full use of the historical information and the EKF storage and the state volume is the relationship between the square growth, is not suitable for large-scale scenarios [4] . Optimization-based methods use IMU data and image measurements to jointly minimize the residuals to obtain the optimal estimate, and jointly optimize the information of all moments of the system to make the system's position estimation more accurate. [5]. Compared with the filter-based method, this method has stronger robustness and higher positioning accuracy. It is able to improve navigation accuracy by iteratively reducing the linearization error and using all historical information for optimization.

Compared with GNSS, INS and visual sensors have the advantages of small size, light weight and low power consumption. [6] In this paper, a stereo camera is chosen as the visual sensor, and the visual/INS integrated navigation technology is investigated by using the factor graph optimization method.

2. Visual/INS Integrated Navigation Algorithmic Scheme Design

The visual/INS integrated navigation system is divided into two threads: front-end and back-end. The front-end thread focuses on camera and INS data pre-processing and initialization of the combined navigation system. The data preprocessing includes: image deblurring, feature point extraction, matching and tracking, and IMU pre-integration. The back-end thread uses graph optimization to implement camera and INS data fusion. The multi-sensor localization problem is constructed as an optimization problem, which first determines the state variables to be optimized; then constructs the objective function of the factor graph, which consists of the sum of the squares of several error terms. To obtain the optimal value of the state variables, the sum of squares of the errors must be minimized, so the nature of this optimization problem is a nonlinear least-squares problem [7] The nature of this optimization problem is therefore a nonlinear least squares problem. In this paper, the objective function is minimized by the Levenberg-Marquardt (LM) method, in which the carrier motion state with the smallest error values in a certain gradient range can be obtained, which is regarded as the accurate position information output by the visual/INS integrated navigation algorithm system. The overall scheme of this paper is shown in Figure 1.

3. Visual/INS Integrated Navigation Algorithmic Front-end Algorithm

The rapid change of the image captured by the camera and the change of the vehicle's attitude will lead to the phenomenon of blurring of the image, which makes the key information missing and thus affects the accuracy of the navigation calculations. Therefore, image de-blurring algorithms are one of the key techniques in the research of visual/INS integrated navigation. Image de-blurring algorithms can be divided into blind deconvolution (BID) and non-blind deconvolution (NBID) [8]. The NBID algorithm can only be used in the case when the blur kernel is known,since the visual image used in this paper belongs to the case where the blur kernel is unknown, the BID algorithm is used to process the blurred images. Aiming at the traditional BID algorithm has the problem of large computation, this paper proposes Retinex combined with BID image de-blurring algorithm. The Retinex algorithm is an image enhancement algorithm based on the theory of color consistency. Through Retinex [9]algorithm the original blurred image can be processed to get the image with enhanced edge information.

The motion blur is modeled as Equation (1):

$$R=K\ast L+N$$

(1)

Where R represents the motion blur image obtained after processing by Retinex algorithm, K denotes the motion blur kernel, L represents the clear image after deblurring, and N represents the unknown noise.

Image recovery is estimated to be:

$$L^{\text{'}}=\arg {\min }_L\left\{\Vert R-K\ast L\Vert +{\rho }_L\left(L\right)\right\}$$

(2)

where $\Vert R-K\ast L\Vert$ is fitted to the data using the $L_2$ norm criterion, and for the vector $\overrightarrow{x}\left(x_1,\mathrm{...},x_n\right)$, the $L_2$ norm is defined as: $\Vert \overrightarrow{x}\Vert =\sqrt{x_1^2+\mathrm{...}+x_n^2}$; ${\rho }_L$ represents the regularization term.

The recovered image is first processed by simple Gaussian deconvolution to obtain an initial estimate L of the recovered image, and then image filtering is used to recover the sharp edges of the blurred image from L and suppress the noise in the smoothed region to obtain an improved estimate of the recovered image $L^{\text{'}}$.

For simple deconvolution, given an estimated blur kernel K, the recovered image L can be estimated based on the input R and K. The following energy function is used:

$$f_L\left(L\right)={{\displaystyle \sum _{{\partial }_{\ast }}{\omega }_{\ast }\Vert K\ast {\partial }_{\ast }L-{\partial }_{\ast }R\Vert }}^2+\alpha {\Vert \nabla L\Vert }^2$$

(3)

where ${\omega }_{\ast }\in \left\{{\omega }_0,{\omega }_1,{\omega }_2\right\}$ denotes the weight of each partial derivative ${\partial }_{\ast }$ and ${\partial }_{\ast }\in \left\{{\partial }_0,{\partial }_x,{\partial }_y,{\partial }_{xx},{\partial }_{xy},{\partial }_{yy}\right\}$ , $\alpha$ denotes the weight of the regularization term ${\Vert \nabla L\Vert }^2$.

Calculate the gradient mapping $\left\{P_x^R,P_y^R\right\}$ of the recovered image $L^{\text{'}}$, eliminate the excess noise in the recovered image and optimize the estimation of the image blur kernel for motion blur using the gradient mapping. Minimize the energy function:

$$f_K\left(K\right)={{\displaystyle \sum _{(P_{\ast }^R,R_{\ast })}{\omega }_{\ast }\Vert K\ast P_{\ast }^R-R_{\ast }\Vert }}^2+\beta {\Vert \nabla L\Vert }^2$$

(4)

where ${\omega }_{\ast }$ is the weight of each partial derivative,$\beta$ is the Tikhonov regularization weight, and $\left(P_{\ast }^R,R_{\ast }\right)$ satisfies.

$$\left(P_{\ast }^R,R_{\ast }\right)\in \left\{\begin{array}{l}\left(P_x^R,{\partial }_{x}R\right),\left(P_y^R,{\partial }_{y}R\right)，\\ \left({\partial }_x{P}_x^R,{\partial }_{xx}R\right),\left({\partial }_y{P}_y^R,{\partial }_{yy}R\right),\\ \left(\left({\partial }_x{P}_x^R+{\partial }_y{P}_x^R\right)/2,{\partial }_{xy}R\right)\end{array}\right\}$$

(5)

The $f_K\left(K\right)$ is converted into the form of a matrix and minimized using the conjugate gradient method, and finally the gradient formula can be obtained, and The gradient formula is then solved quickly using the Fast Fourier Transform (FFT) method to obtain an estimate of the blur kernel. If the current blur kernel estimation is the optimal estimation then the best blur kernel estimation K is utilized to perform the final deconvolution operation with the original blurred image B to obtain the final clear recovered image. Otherwise, iterative alternating optimization is used to repeat the estimation using the estimated motion blur kernel with the clear recovered image until the best blur kernel estimate is obtained.

After image deblurring, this paper selects extracting ORB feature points to complete the matching relationship between neighboring images. The feature point extraction part selects the FAST corner point [10] with high real-time performance for feature extraction. After that, the fast library for approximate nearest neighbors (FLANN) algorithm is used for feature matching [11]. And random sampling consensus (RANSAC) algorithm is used to eliminate the false matching of FLANN algorithm.

Since the sampling frequency of the IMU is much higher than that of the camera, it is necessary to integrate the IMU measurements between two consecutive image frames to calculate position, velocity and rotation. Alignment of IMU and camera sampling frequencies is realized.

The IMU pre-integration model is derived to reduce the amount of operations in the integration process and to meet the real-time requirements of the combined navigation system. The measurement errors of the IMU mainly include Gaussian white noise and bias [12]. Consider the random wandering of bias and its noise [13] the acceleration measurements ${\widehat{a}}_t^b$ and angular velocity measurements ${\widehat{\omega }}_t^b$ of the IMU are modeled as follows:

$${\widehat{a}}_t^b=a_t^b+R_{bw}{g}^w+b_{a_t}+n_a$$

(6)

$${\widehat{\omega }}_t^b={\omega }_t^b+b_{{\omega }_t}+n_{\omega }$$

(7)

In this paper, the East-North-Up (ENU) coordinate system is chosen as the world coordinate system. In Equation (6) and Equation(7), $R_{bw}$ is the rotation matrix from the world coordinate system to the IMU coordinate system;$g^w={\left[0,0,g\right]}^T$ is the gravity vector in the world coordinate system;$b_{a_t}$ and $n_a$ are the bias and noise of the acceleration; $b_{{\omega }_t}$ and $n_{\omega }$ are the bias and noise of the angular velocity, respectively. Assume that $n_a\sim \mathcal{N}\left(0,{\sigma }_a^2\right)$, $n_{\omega }\sim \mathcal{N}\left(0,{\sigma }_{\omega }^2\right)$ ; $b_{a_t}$, $b_{{\omega }_t}$ are bounded random walks and random walks, respectively [14].

To align the sampling frequencies of the IMU and the camera, the IMU measurements between two consecutive image frames are integrated. The IMU pre-integration schematic is shown in Figure 2.

IMU pre-integration term for frame $i$ and frame $i+1$:

$${\widehat{\alpha }}_{b_k{b}_{i+1}}\approx {\widehat{\alpha }}_{b_k{b}_i}+{\widehat{\beta }}_{b_k{b}_i}\delta t+\frac{1}{2}{a}_i\delta t^2$$

(8)

$${\widehat{\beta }}_{b_k{b}_{i+1}}\approx {\widehat{\beta }}_{b_k{b}_i}+a_i\delta t$$

(9)

$${\widehat{\gamma }}_{b_k{b}_{i+1}}\approx {\widehat{\gamma }}_{b_k{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\omega }_i\delta t\end{array}\right]$$

(10)

where ${\widehat{\alpha }}_{b_k{b}_{i+1}}$, ${\widehat{\beta }}_{b_k{b}_{i+1}}$ and ${\widehat{\gamma }}_{b_k{b}_{i+1}}$ denote position, velocity and rotation pre-integration respectively.$\delta t$ is the time interval between the IMU measurements at $i$ moments and $i+1$ moments, for acceleration and angular velocity in the above equation:

$$a_i=\frac{1}{2}\left[\begin{array}{l}{R}_{b_k{b}_i}\left({\widehat{a}}_i^b-b_{a_i}+n_a^{b_i}\right)+\\ R_{b_k{b}_{i+1}}\left({\widehat{a}}_{i+1}^b-b_{a_{i+1}}+n_a^{b_{i+1}}\right)\end{array}\right]$$

(11)

$${\omega }_i=\frac{1}{2}\left({\widehat{\omega }}^{b_i}+n_{\omega }^{b_i}+{\widehat{\omega }}^{b_{i+1}}+n_{\omega }^{b_{i+1}}\right)-b_{{\omega }_i}$$

(12)

Alignment of IMU and camera sampling frequencies can be accomplished by the above.

5. Experimental Verification

5.1 Image De-blurring Algorithm Validation

In order to verify the effectiveness of the image de-blurring algorithm proposed in this paper, three sets of images were selected from different public datasets for image de-blurring. Evaluation was conducted based on de-blurring results before and after image processing, image standard deviation, average gradient of the images, and feature point extraction and matching.

The comparison of de-blurring effect is shown in Figure 4, Figure 5 and Figure 6.

Using image standard deviation and average gradient to evaluate image quality is a common practice. Standard deviation and average gradient are commonly used indicators to verify image quality. Among them: the standard deviation reflects the degree of dispersion of the image pixel values from the mean value, the larger the standard deviation indicates the better the quality of the image; the average gradient reflects the clarity of the image and the texture changes, the larger the average gradient indicates the clearer the image is [15]. Calculate the standard deviation and average gradient of each group of image original and processed by algorithm as shown in Table 1 and Table 2.

As can be seen from Table 1, the standard deviation of the three groups of images after de-blurring is greater than the standard deviation before de-blurring, indicating that the image de-blurring algorithm proposed in this paper has a better de-blurring ability.

As can be seen from Table 2, the average gradient of the three groups of images after de-blurring is larger than the average gradient before de-blurring, indicating that the clarity of the images processed by the image de-blurring algorithm proposed in this paper has been improved.

Table 3 and table 4 give the matching and time required for extracting and matching the ORB feature points of the original images and the three groups of images processed by the proposed algorithm in this paper, and Fig. 7 shows the feature matching interface before and after de-blurring the first group of images visualized by OpenCV [16].

Figure 7. The feature matching of the first group of images before and after de-blurring(a)Image feature matching before de-blurring;(b)Image feature matching after de-blurring.From the results of feature extraction and matching, it can be observed that the logarithm of feature matching pairs and the time required for feature matching after deblurring three sets of images are smaller than before deblurring. This indicates that the image deblurring algorithm proposed in this paper can effectively improve the efficiency and accuracy of feature matching.

Figure 7. The feature matching of the first group of images before and after de-blurring(a)Image feature matching before de-blurring;(b)Image feature matching after de-blurring.From the results of feature extraction and matching, it can be observed that the logarithm of feature matching pairs and the time required for feature matching after deblurring three sets of images are smaller than before deblurring. This indicates that the image deblurring algorithm proposed in this paper can effectively improve the efficiency and accuracy of feature matching.

5.2. Validation of Visual/INS Integrated Navigation Algorithm Performance

To validate the positioning accuracy of the visual/INS integrated navigation based on factor graph optimization proposed in this paper, simulation experiments are conducted using open source datasets.

The dataset used in this paper is the EuRoC MAV dataset, which is collected from an AscTec Firefly UAV equipped with a stereo camera and an IMU. The camera model is MT9V034 with a frame rate of 20 Hz, and the IMU model is ADIS16448 with a frame rate of 200 Hz. The motion trajectory of the UAV was measured using a total station and motion capture system, with positioning accuracy at the sub-millimeter level, serving as ground truth for evaluating the positioning accuracy of the system proposed in this paper. The MH_01 dataset with rich texture was selected for validation. The physical diagram of the data collection platform is shown in Figure 8.

The single visual localization algorithm and the visual/INS integrated localization algorithm were run on the above data sequences to analyze the localization effect of each algorithm. The comparison graph of its localization results with the true value trajectory is shown in Figure 9.

From Figure 9 and Table 5, it can be seen that the algorithm in this paper can accurately track the estimated trajectory truth value, and its average absolute position error is about 0.02m, which is about 40% higher than that of the single visual navigation; the root mean square error is about 0.02m, which is about 45% higher than that of the single visual navigation; and the maximum error is not exceeded by 0.08m. In conclusion, the visual/INS integrated localization system designed in this paper can effectively tracking the carrier position state, and has higher positioning accuracy and higher robustness than the single vision localization system.

6. Conclusions

In order to overcome the limitations of single navigation system positioning and better meet the user's demand for positioning accuracy, this paper designs a vision/INS integrated navigation algorithm based on factor graph optimization. Firstly, to address the problem of decreased positioning accuracy of combined navigation due to image blurring in traditional visual/INS integrated navigation, the Retinex algorithm combined with the BID algorithm is used to de-blur the image; then, by deriving the objective function of the factor graph optimization, the nonlinear optimization problem is transformed into a least-squares problem and the LM algorithm is used to obtain the minimum error with multiple iterations, so as to realize the optimization of combined visual/INS navigation algorithm based on factor graph optimization, which results in the optimal navigation accuracy. of the vision/INS integrated navigation algorithm to optimize the navigation accuracy. Finally, experimental validation is carried out using public datasets, and the results show that the vision/INS integrated navigation algorithm designed in this paper improves the positioning accuracy by about 40% compared with the single vision algorithm, and has high accuracy and robustness.