# Mobile Robot Localization Based on Vision and Multisensor

Authors: Lina Yao; Fengzhe Li
Journal: Journal of Robotics
 (2020)

---

## Abstract

To deal with the low accuracy of positioning for mobile robots when only using visual sensors and an IMU, a method based on tight coupling and nonlinear optimization is proposed to obtain a high-precision visual positioning scheme by combining measured value of the preintegrated inertial measurement unit (IMU) and values of the odometer and characteristic observations. First, the preprocessing part of the observation data includes tracking of the image data and the odometer data, and preintegration of IMU data. Second, the initialization part of the above three sensors includes IMU preintegration, odometer preintegration, and gyroscope bias calculation. It also includes the alignment of speed, gravity, and scale. Finally, a local BA (bundle adjustment) joint optimization and global graph optimization are established, so as to obtain more accurate positioning results.


---

## Body

## 1. Introduction

Positioning technology is undoubtedly the basic premise for a wide range of applications such as robot navigation, autonomous driving, and virtual reality. The visual positioning system has won the attention of researchers because of its small size, low cost, and simple hardware setup [1–4]. In recent years, the visual SLAM (simultaneous location and mapping) has experienced rapid development, among which the more classic methods include the PATM [5] algorithm proposed by Klein et al. in 2007 and its successor ORB-SLAM [6]. Other notable methods are LSD-SLAM [7] for the monocular vision direct method, SVO [8] based on the sparse method, DSO [9] and DVO-SLAM [10], etc. Since the discussion cannot be fully discussed, this section discusses the method of multisensor fusion localization, skipping the above method of localization using only visual sensors. Due to a large amount of information measured by the camera and the low cost, the application on mobile robots is very extensive. Therefore, the following mainly introduces the positioning method combining the camera and other sensors.The way the camera is integrated with other sensors can be divided into loose coupling [11, 12] and tight coupling. Among them, the loose-coupling method is used to use the camera and other sensors as separate modules to calculate the respective pose. The fusion information methods generally include Kalman filter (KF), extended Kalman filter (EKF), and other methods, while other sensor information is used for state propagation. The tight-coupling methods are mainly divided into two kinds: EKF [13, 14] and graph-based optimization [15, 16]. A widely concerned VIO method based on EKF is the MSCKF [13] algorithm proposed by MARS Laboratory of the University of Minnesota. It can marginalize old map points and postures to ensure accuracy and computational efficiency under the condition that the number of map points and postures in the state vector is constant. However, with the development of the optimization technology in SLAM, VINS based on graph optimization has also been studied more and more. The limitation of the filtering algorithm is analyzed in reference [17], and the method based on optimization is superior to the method based on the filter. The ICE-BA algorithm proposed in reference [18] improves the BA optimization part of VI-SLAM, changes the incremental nature of SLAM measurement, resolves the direct conflict between sliding window edge deviation and global closed-loop detection constraints, and improves the computational efficiency. In [8], the IMU preintegration technology was extended to Lie algebra in 2015 and implemented in the open-source factor graph optimization library GTSAM. Based on this, [19, 20] designed the VINS-MONO algorithm and obtained the experimental results that the dynamic performance of continuous-time preintegration is better than the discrete preintegration. However, there are many differences in the adaptability of sensors on different platforms. In the mobile robot, because the robot is mostly in the plane uniform motion [4], the IMU cannot be fully excited, which causes numerical stability problems during the initialization of the scale. Therefore, the applicability of the IMU is not as good as the mileage. At the same time, external calibration and initialization issues between sensors pose significant challenges for highly nonlinear visual-inertial systems.These problems will affect the positioning accuracy and robustness of mobile robots. To solve these problems, a tightly coupled and optimization-based approach is used to fuse monocular vision sensor, IMU, and wheeled odometer to achieve accurate pose estimation in this paper. The main work of this paper is shown as follows:(1)
In this paper, the odometer-inertial navigation system is used to estimate the scale to solve the problem that the absolute scale cannot be obtained by location algorithm using monocular camera and scale stability cannot be guaranteed.(2)
A BA tight-coupling optimization model is established, including marginalization (removal of pose and feature point constraints from the sliding window), IMU residual between adjacent frames in the sliding window, visual reprojection error, and measurement error of odometer. Finally, accurate positioning results are obtained by minimizing the error cost function.


## 2. System Composition and Measurement Data Preprocessing

### 2.1. System Composition and Coordinate System Definition

In Figure1, the system starts with data preprocessing (Section 2.2), including image feature extraction and tracking, preintegration of IMU measurements between two consecutive frames, and calculation of pose measurement by two consecutive interframe odometers. In the second part (Section 3), the initialization process provides the contents including attitude, velocity, gravity vector, gyroscope bias, and three-dimensional feature location for subsequent nonlinear optimization. The third part is local BA joint optimization (Section 4) and global map optimization, including a sliding window BA optimization model; the fourth part is loop detection, eliminating drift and ensuring map consistency. The main application method is DOW2 [21], so it will not be discussed in this paper.Figure 1
The overall system block diagram.The relevant coordinate system in this paper is shown in Figure2, which defines the coordinate system and symbols used in this paper. Among them, oxyzw represents the world coordinate system, oxyzc represents the camera coordinate system, oxyzw represents the IMU coordinate system, oxyzd represents the odometer coordinate system, and oxyzb represents the robot coordinate system, which is defined as the same as the IMU coordinate system. The rotation matrix R and quaternion q are used simultaneously to represent the rotation. qbw and pbw represent the rotation and translation from the robot coordinate system to the world coordinate system. bj represents the coordinate system of the robot when the j frame image is obtained. cj represents the camera coordinate system when the j frame image is obtained. ⊗ denotes multiplication between quaternions. •˜ denotes the measured or estimated value of a specific quantity.Figure 2
The overall coordinate system.


### 2.2. Data Preprocessing

#### 2.2.1. Feature Point Tracking

The tracking and matching of feature points in monocular vision are used to inherit or generate new feature points from the previous frame to the next frame. The specific process is shown as follows: firstly, the strongestf corner features are found on the first frame image, and the nonmaximum suppression radius is set at the same time. Then, when the next frame image arrives, these feature points are tracked by the optical flow method [22], and the matching points are found in the next frame image. The matching points in the front and back frames are corrected to get the final corrected position. Based on the corrected position, the F-matrix and RANSAC algorithm are used to remove the outliers. Finally, the strongest new feature points are searched in the area beyond the matched feature points, and the number of feature points is complemented to f, which ensures that the matching number of feature points is consistent with that of the next frame. The value of f is generally in the range of 100–300, and the specific content is referenced in [23].


#### 2.2.2. Inertial Navigation Preintegration

The original accelerometer and gyroscope can measure the accelerationa˜ and angular velocity ω˜ of the robot. It is assumed that the additional noise in the accelerometer and gyroscope is the Gaussian white noise na∼N0,σa2 and nω∼N0,σω2. The bias of accelerometer and gyroscope is assumed to be a random walk model, whose derivatives obey Gaussian normal distribution m˙at∼N0,σba2 and m˙ωt∼N0,σbω2. The measurement results are illustrated as follows:(1)a˜t=at+mat+Rwtgw+na,ω˜t=ωt+mωt+nω.From the above formula, the position, velocity, and direction of the next moment can be calculated, and the time interval between the two moments is given asΔt=j,j+1:(2)pbj+1w=pbjw+vbjwΔt+∬t∈j,j+1Rbtwa¯t−gwdt2,vbj+1w=vbjw+∫t∈j,j+1Rbtwa¯t−gwdt,qbj+1w=qbjw⊗∫t∈j,j+112Ωω¯tqtbjdt,where(3)a¯t=a˜t−mat−na,ω¯t=ω^t−mwt−nw,Ωω=−ω×ω−ωT0,ω×=0−ωzωyωz0−ωx−ωyωx0.The reference coordinate system is transformed from the world coordinate system to the robot coordinate system. Formula (2) is multiplied by the transformation matrix Rwbj from the world coordinate system to the bj frame coordinate system. By using bj as the reference frame of a given bias, the preintegration term can be obtained only by the IMU measurement, and the covariance matrix Qbj+1bj of α, β, and q propagates accordingly. Then, when the IMU bias changes slightly, the first-order approximation estimation of the bias state is adjusted based on α, β, and q, and the resulting preintegral is expressed as(4)αbj+1bj≈α˜bj+1bj+Jbaαδmaj+Jbwαδmwj,βbj+1bj≈β˜bj+1bj+Jbaβδmaj+Jbwβδmwj,qbj+1bj≈q˜bj+1bj⊗112Jbwqδmwj.The preintegration method does not need to repeat IMU measurements, and it saves a lot of computing time and resources for subsequent graph-based optimization algorithms.


#### 2.2.3. Odometer Preintegration

The wheel odometer can measure the linear velocityv˜d and the angular velocity ω˜d of the robot. It is assumed that the noise between the measured value and the true value is Gaussian noise, n=nv,ng. The measurement model is given as follows:(5)v˜d=vd+nv,ω˜d=ωd+ng.Considering the problem of slipping off the wheeled robot, the angular velocity measured by the gyroscope can be used as the acceleration of the robot. Referring to the IMU preintegration algorithm, the preintegration is calculated for the odometer measurement and the angular velocity measured by the gyroscope; the preintegration of the velocity is simpler, and there is no offsetting effect in the acceleration. The odometer preintegration results are shown as follows:(6)pdj+1w=pdjw+∫t∈j,j+1Rdtwv˜dt−nvdt,qdj+1w=qdjw⊗∫t∈j,j+112Ωω˜dt−mωt−nωdt.Similar to IMU preintegral, the preintegral measurement is obtained by separating the initial value and the constant term of the integral. When the gyroscope bias changes slightly, the approximation of the previous moment is used to updateαdj+1dj and qdj+1dj, instead of iteratively calculating the preintegration measurements:(7)qdj+1dj≈q˜dj+1dj⊗1Jbωqδmωj.


## 2.1. System Composition and Coordinate System Definition

In Figure1, the system starts with data preprocessing (Section 2.2), including image feature extraction and tracking, preintegration of IMU measurements between two consecutive frames, and calculation of pose measurement by two consecutive interframe odometers. In the second part (Section 3), the initialization process provides the contents including attitude, velocity, gravity vector, gyroscope bias, and three-dimensional feature location for subsequent nonlinear optimization. The third part is local BA joint optimization (Section 4) and global map optimization, including a sliding window BA optimization model; the fourth part is loop detection, eliminating drift and ensuring map consistency. The main application method is DOW2 [21], so it will not be discussed in this paper.Figure 1
The overall system block diagram.The relevant coordinate system in this paper is shown in Figure2, which defines the coordinate system and symbols used in this paper. Among them, oxyzw represents the world coordinate system, oxyzc represents the camera coordinate system, oxyzw represents the IMU coordinate system, oxyzd represents the odometer coordinate system, and oxyzb represents the robot coordinate system, which is defined as the same as the IMU coordinate system. The rotation matrix R and quaternion q are used simultaneously to represent the rotation. qbw and pbw represent the rotation and translation from the robot coordinate system to the world coordinate system. bj represents the coordinate system of the robot when the j frame image is obtained. cj represents the camera coordinate system when the j frame image is obtained. ⊗ denotes multiplication between quaternions. •˜ denotes the measured or estimated value of a specific quantity.Figure 2
The overall coordinate system.


## 2.2. Data Preprocessing

### 2.2.1. Feature Point Tracking

The tracking and matching of feature points in monocular vision are used to inherit or generate new feature points from the previous frame to the next frame. The specific process is shown as follows: firstly, the strongestf corner features are found on the first frame image, and the nonmaximum suppression radius is set at the same time. Then, when the next frame image arrives, these feature points are tracked by the optical flow method [22], and the matching points are found in the next frame image. The matching points in the front and back frames are corrected to get the final corrected position. Based on the corrected position, the F-matrix and RANSAC algorithm are used to remove the outliers. Finally, the strongest new feature points are searched in the area beyond the matched feature points, and the number of feature points is complemented to f, which ensures that the matching number of feature points is consistent with that of the next frame. The value of f is generally in the range of 100–300, and the specific content is referenced in [23].


### 2.2.2. Inertial Navigation Preintegration

The original accelerometer and gyroscope can measure the accelerationa˜ and angular velocity ω˜ of the robot. It is assumed that the additional noise in the accelerometer and gyroscope is the Gaussian white noise na∼N0,σa2 and nω∼N0,σω2. The bias of accelerometer and gyroscope is assumed to be a random walk model, whose derivatives obey Gaussian normal distribution m˙at∼N0,σba2 and m˙ωt∼N0,σbω2. The measurement results are illustrated as follows:(1)a˜t=at+mat+Rwtgw+na,ω˜t=ωt+mωt+nω.From the above formula, the position, velocity, and direction of the next moment can be calculated, and the time interval between the two moments is given asΔt=j,j+1:(2)pbj+1w=pbjw+vbjwΔt+∬t∈j,j+1Rbtwa¯t−gwdt2,vbj+1w=vbjw+∫t∈j,j+1Rbtwa¯t−gwdt,qbj+1w=qbjw⊗∫t∈j,j+112Ωω¯tqtbjdt,where(3)a¯t=a˜t−mat−na,ω¯t=ω^t−mwt−nw,Ωω=−ω×ω−ωT0,ω×=0−ωzωyωz0−ωx−ωyωx0.The reference coordinate system is transformed from the world coordinate system to the robot coordinate system. Formula (2) is multiplied by the transformation matrix Rwbj from the world coordinate system to the bj frame coordinate system. By using bj as the reference frame of a given bias, the preintegration term can be obtained only by the IMU measurement, and the covariance matrix Qbj+1bj of α, β, and q propagates accordingly. Then, when the IMU bias changes slightly, the first-order approximation estimation of the bias state is adjusted based on α, β, and q, and the resulting preintegral is expressed as(4)αbj+1bj≈α˜bj+1bj+Jbaαδmaj+Jbwαδmwj,βbj+1bj≈β˜bj+1bj+Jbaβδmaj+Jbwβδmwj,qbj+1bj≈q˜bj+1bj⊗112Jbwqδmwj.The preintegration method does not need to repeat IMU measurements, and it saves a lot of computing time and resources for subsequent graph-based optimization algorithms.


### 2.2.3. Odometer Preintegration

The wheel odometer can measure the linear velocityv˜d and the angular velocity ω˜d of the robot. It is assumed that the noise between the measured value and the true value is Gaussian noise, n=nv,ng. The measurement model is given as follows:(5)v˜d=vd+nv,ω˜d=ωd+ng.Considering the problem of slipping off the wheeled robot, the angular velocity measured by the gyroscope can be used as the acceleration of the robot. Referring to the IMU preintegration algorithm, the preintegration is calculated for the odometer measurement and the angular velocity measured by the gyroscope; the preintegration of the velocity is simpler, and there is no offsetting effect in the acceleration. The odometer preintegration results are shown as follows:(6)pdj+1w=pdjw+∫t∈j,j+1Rdtwv˜dt−nvdt,qdj+1w=qdjw⊗∫t∈j,j+112Ωω˜dt−mωt−nωdt.Similar to IMU preintegral, the preintegral measurement is obtained by separating the initial value and the constant term of the integral. When the gyroscope bias changes slightly, the approximation of the previous moment is used to updateαdj+1dj and qdj+1dj, instead of iteratively calculating the preintegration measurements:(7)qdj+1dj≈q˜dj+1dj⊗1Jbωqδmωj.


## 2.2.1. Feature Point Tracking

The tracking and matching of feature points in monocular vision are used to inherit or generate new feature points from the previous frame to the next frame. The specific process is shown as follows: firstly, the strongestf corner features are found on the first frame image, and the nonmaximum suppression radius is set at the same time. Then, when the next frame image arrives, these feature points are tracked by the optical flow method [22], and the matching points are found in the next frame image. The matching points in the front and back frames are corrected to get the final corrected position. Based on the corrected position, the F-matrix and RANSAC algorithm are used to remove the outliers. Finally, the strongest new feature points are searched in the area beyond the matched feature points, and the number of feature points is complemented to f, which ensures that the matching number of feature points is consistent with that of the next frame. The value of f is generally in the range of 100–300, and the specific content is referenced in [23].


## 2.2.2. Inertial Navigation Preintegration

The original accelerometer and gyroscope can measure the accelerationa˜ and angular velocity ω˜ of the robot. It is assumed that the additional noise in the accelerometer and gyroscope is the Gaussian white noise na∼N0,σa2 and nω∼N0,σω2. The bias of accelerometer and gyroscope is assumed to be a random walk model, whose derivatives obey Gaussian normal distribution m˙at∼N0,σba2 and m˙ωt∼N0,σbω2. The measurement results are illustrated as follows:(1)a˜t=at+mat+Rwtgw+na,ω˜t=ωt+mωt+nω.From the above formula, the position, velocity, and direction of the next moment can be calculated, and the time interval between the two moments is given asΔt=j,j+1:(2)pbj+1w=pbjw+vbjwΔt+∬t∈j,j+1Rbtwa¯t−gwdt2,vbj+1w=vbjw+∫t∈j,j+1Rbtwa¯t−gwdt,qbj+1w=qbjw⊗∫t∈j,j+112Ωω¯tqtbjdt,where(3)a¯t=a˜t−mat−na,ω¯t=ω^t−mwt−nw,Ωω=−ω×ω−ωT0,ω×=0−ωzωyωz0−ωx−ωyωx0.The reference coordinate system is transformed from the world coordinate system to the robot coordinate system. Formula (2) is multiplied by the transformation matrix Rwbj from the world coordinate system to the bj frame coordinate system. By using bj as the reference frame of a given bias, the preintegration term can be obtained only by the IMU measurement, and the covariance matrix Qbj+1bj of α, β, and q propagates accordingly. Then, when the IMU bias changes slightly, the first-order approximation estimation of the bias state is adjusted based on α, β, and q, and the resulting preintegral is expressed as(4)αbj+1bj≈α˜bj+1bj+Jbaαδmaj+Jbwαδmwj,βbj+1bj≈β˜bj+1bj+Jbaβδmaj+Jbwβδmwj,qbj+1bj≈q˜bj+1bj⊗112Jbwqδmwj.The preintegration method does not need to repeat IMU measurements, and it saves a lot of computing time and resources for subsequent graph-based optimization algorithms.


## 2.2.3. Odometer Preintegration

The wheel odometer can measure the linear velocityv˜d and the angular velocity ω˜d of the robot. It is assumed that the noise between the measured value and the true value is Gaussian noise, n=nv,ng. The measurement model is given as follows:(5)v˜d=vd+nv,ω˜d=ωd+ng.Considering the problem of slipping off the wheeled robot, the angular velocity measured by the gyroscope can be used as the acceleration of the robot. Referring to the IMU preintegration algorithm, the preintegration is calculated for the odometer measurement and the angular velocity measured by the gyroscope; the preintegration of the velocity is simpler, and there is no offsetting effect in the acceleration. The odometer preintegration results are shown as follows:(6)pdj+1w=pdjw+∫t∈j,j+1Rdtwv˜dt−nvdt,qdj+1w=qdjw⊗∫t∈j,j+112Ωω˜dt−mωt−nωdt.Similar to IMU preintegral, the preintegral measurement is obtained by separating the initial value and the constant term of the integral. When the gyroscope bias changes slightly, the approximation of the previous moment is used to updateαdj+1dj and qdj+1dj, instead of iteratively calculating the preintegration measurements:(7)qdj+1dj≈q˜dj+1dj⊗1Jbωqδmωj.


## 3. System Initialization State Estimation

The initial value of the system is obtained by a loosely coupled method. Firstly, the structure from motion (SFM) is used to estimate the camera pose and feature 3D position. Then, the SFM results are aligned with IMU preintegration and odometer preintegration, respectively, to correct the gyroscope bias. At the same time, the corresponding velocity of each frame, gravity vector direction, and scale factor are solved. It should be noted that the scale is the final result obtained by weighting the scale estimated by the two alignment processes according to the dynamic weight method.


### 3.1. Vision-Only Initialization Based on the Sliding Window

The monocular initialization uses SFM to estimate the camera scale pose and feature point spatial position of each frame within the sliding window. Firstly, the feature matching between the latest frame and any other frame in the window is checked. If a stable feature tracking and sufficient parallax are found, the scale and pose between the two frames are restored by the five-point method [24]. Then, the feature triangulation and the reprojection error are performed to be minimized. Otherwise, the current latest frame is saved, continued to wait for the new frame. The pose pcjc0,qcjc0 of all frames in the sliding window is obtained by the above procedure. It is assumed that the external parameter between camera and IMU is pcb,qcb, the external parameter between camera and the odometer is pcd,qcd. The pose is converted to the robot coordinate system and odometer coordinate system, respectively. The attitude is converted to the IMU coordinate system as(8)s1p¯bjc0=s1p¯cjc0−Rbjc0pcb,qbjc0=qcjc0⊗qcb−1.Similarly, the conversion to the odometer coordinate system is shown as(9)s2p¯djc0=s2p¯cjc0−Rdjc0pcd,qdjc0=qcjc0⊗qcd−1,where s1 and s2 both represent the scale information of the visual measurement and are used for the subsequent dynamic weighting to solve the final scale parameter s.


### 3.2. Vision and IMU Preintegral Alignment

#### 3.2.1. Gyro Offset Calibration

Two consecutive framesbj and bj+1 in the sliding window are rotated by visual SFM to obtain qbjc0 and qbj+1c0. The relative constraint q˜bj+1bj is obtained by IMU preintegration. Then, the linearization of the IMU preintegral term is obtained for the gyroscope bias and the cost function is minimized as(10)minδbω∑k∈Bqbj+1c0−1⊗qbjc0⊗qbj+1bj2,where B represents all keyframes in the sliding window, where(11)qbj+1bj=q˜bj+1bj⊗112Jδbωjδθbj+1bjδmωj.After the initial calibration value of the gyroscope biasmω is obtained, the IMU preintegration is recalculated based on the new gyroscope bias.


#### 3.2.2. Velocity, Gravity Vector, and Scale Initialization

After the gyro bias is initialized, the gyro measurement is regarded as accurate and continues to be used to initialize other variables to be optimized:(12)χI=vb0b0,vb1b1,…,vbnbn,gc0,s1T,where vbjbj is the velocity of the j frame image robot coordinate system, gc0 is the gravity vector in the c0 coordinate system, and s1 is based on the monocular scale obtained by the IMU preintegration.The world coordinate systemw in the preintegral terms αbj+1bj and βbj+1bj is replaced with the c0 coordinate system and converted into the form of Hx=b:(13)Hbj+1bjχI=α˜bj+1bkj−pcb+Rc0bjRbj+1c0pcbβ˜bj+1bj≈z˜bj+1bj,where(14)Hbj+1bj=−IΔtj012Rc0bjΔtj2Rc0bj−IRc0bjRbj+1c0Rc0bjΔtj0,pc0=p¯cj+1c0−p¯cjc0.By solving the linear least-squares problem,(15)minχI∑j∈Bz˜bj+1bj−Hbj+1bjχI2.According to the above formulas,χI can be optimized, including all keyframe velocities, gravity vectors, and monocular scales in sliding windows.


### 3.3. Vision and Odometer Preintegral Alignment

Since the gyro measurement is used for the rotation measurement of the odometer preintegration, the gyro offset after calibration can be used directly. The variables to be optimized are defined as(16)χП=vd0d0,vd1d1,…,vdmdm,s2T.Similarly, the odometer preconcentration term is transferred to thec0 coordinate system and converted into the form of Hx=b:(17)Hdj+1djχП=α˜bj+1bkj−pcd+Rc0djRdj+1c0pcd≈z˜dj+1dj,Hdj+1dj=−IΔtj0Rc0djp¯cj+1c0−p¯cjc0.By solving the linear least-squares problem,(18)minχП∑j∈Bz˜dj+1dj−Hdj+1djχП2.The coordinate system speed of odometer in each frame in the window and the parameters of the monocular scale can be obtained.


### 3.4. Dynamic Weighted Scale Parameter

The idea of dynamic weighting method is used to design a sliding window with a size of 5 keyframes based on the original sliding window of the system and reoperate the sliding window of the system with a size of 10 keyframes. Therefore, the keyframeBi∈1,2,3,4,5 is selected from the sliding window, where 1–5 frames within the window are included in B1, 2–6 frames within the window are included in B2, and so on, and B5 is derived. The scale parameters s1,i and s2,i at Bi are calculated according to Sections 3.2 and 3.3, respectively, and the minimum mean square errors e1,i and e2,i are obtained. The scale parameter s=W1s1+W2s2 is calculated by dynamically adjusting the weights W1 and W2 according to the above parameters, wherein ∑n=12Wn=1. The initial values of W1 and W2 are both 0.5 as shown in Figure 3.Figure 3
Dynamic weighting flowchart.


## 3.1. Vision-Only Initialization Based on the Sliding Window

The monocular initialization uses SFM to estimate the camera scale pose and feature point spatial position of each frame within the sliding window. Firstly, the feature matching between the latest frame and any other frame in the window is checked. If a stable feature tracking and sufficient parallax are found, the scale and pose between the two frames are restored by the five-point method [24]. Then, the feature triangulation and the reprojection error are performed to be minimized. Otherwise, the current latest frame is saved, continued to wait for the new frame. The pose pcjc0,qcjc0 of all frames in the sliding window is obtained by the above procedure. It is assumed that the external parameter between camera and IMU is pcb,qcb, the external parameter between camera and the odometer is pcd,qcd. The pose is converted to the robot coordinate system and odometer coordinate system, respectively. The attitude is converted to the IMU coordinate system as(8)s1p¯bjc0=s1p¯cjc0−Rbjc0pcb,qbjc0=qcjc0⊗qcb−1.Similarly, the conversion to the odometer coordinate system is shown as(9)s2p¯djc0=s2p¯cjc0−Rdjc0pcd,qdjc0=qcjc0⊗qcd−1,where s1 and s2 both represent the scale information of the visual measurement and are used for the subsequent dynamic weighting to solve the final scale parameter s.


## 3.2. Vision and IMU Preintegral Alignment

### 3.2.1. Gyro Offset Calibration

Two consecutive framesbj and bj+1 in the sliding window are rotated by visual SFM to obtain qbjc0 and qbj+1c0. The relative constraint q˜bj+1bj is obtained by IMU preintegration. Then, the linearization of the IMU preintegral term is obtained for the gyroscope bias and the cost function is minimized as(10)minδbω∑k∈Bqbj+1c0−1⊗qbjc0⊗qbj+1bj2,where B represents all keyframes in the sliding window, where(11)qbj+1bj=q˜bj+1bj⊗112Jδbωjδθbj+1bjδmωj.After the initial calibration value of the gyroscope biasmω is obtained, the IMU preintegration is recalculated based on the new gyroscope bias.


### 3.2.2. Velocity, Gravity Vector, and Scale Initialization

After the gyro bias is initialized, the gyro measurement is regarded as accurate and continues to be used to initialize other variables to be optimized:(12)χI=vb0b0,vb1b1,…,vbnbn,gc0,s1T,where vbjbj is the velocity of the j frame image robot coordinate system, gc0 is the gravity vector in the c0 coordinate system, and s1 is based on the monocular scale obtained by the IMU preintegration.The world coordinate systemw in the preintegral terms αbj+1bj and βbj+1bj is replaced with the c0 coordinate system and converted into the form of Hx=b:(13)Hbj+1bjχI=α˜bj+1bkj−pcb+Rc0bjRbj+1c0pcbβ˜bj+1bj≈z˜bj+1bj,where(14)Hbj+1bj=−IΔtj012Rc0bjΔtj2Rc0bj−IRc0bjRbj+1c0Rc0bjΔtj0,pc0=p¯cj+1c0−p¯cjc0.By solving the linear least-squares problem,(15)minχI∑j∈Bz˜bj+1bj−Hbj+1bjχI2.According to the above formulas,χI can be optimized, including all keyframe velocities, gravity vectors, and monocular scales in sliding windows.


## 3.2.1. Gyro Offset Calibration

Two consecutive framesbj and bj+1 in the sliding window are rotated by visual SFM to obtain qbjc0 and qbj+1c0. The relative constraint q˜bj+1bj is obtained by IMU preintegration. Then, the linearization of the IMU preintegral term is obtained for the gyroscope bias and the cost function is minimized as(10)minδbω∑k∈Bqbj+1c0−1⊗qbjc0⊗qbj+1bj2,where B represents all keyframes in the sliding window, where(11)qbj+1bj=q˜bj+1bj⊗112Jδbωjδθbj+1bjδmωj.After the initial calibration value of the gyroscope biasmω is obtained, the IMU preintegration is recalculated based on the new gyroscope bias.


## 3.2.2. Velocity, Gravity Vector, and Scale Initialization

After the gyro bias is initialized, the gyro measurement is regarded as accurate and continues to be used to initialize other variables to be optimized:(12)χI=vb0b0,vb1b1,…,vbnbn,gc0,s1T,where vbjbj is the velocity of the j frame image robot coordinate system, gc0 is the gravity vector in the c0 coordinate system, and s1 is based on the monocular scale obtained by the IMU preintegration.The world coordinate systemw in the preintegral terms αbj+1bj and βbj+1bj is replaced with the c0 coordinate system and converted into the form of Hx=b:(13)Hbj+1bjχI=α˜bj+1bkj−pcb+Rc0bjRbj+1c0pcbβ˜bj+1bj≈z˜bj+1bj,where(14)Hbj+1bj=−IΔtj012Rc0bjΔtj2Rc0bj−IRc0bjRbj+1c0Rc0bjΔtj0,pc0=p¯cj+1c0−p¯cjc0.By solving the linear least-squares problem,(15)minχI∑j∈Bz˜bj+1bj−Hbj+1bjχI2.According to the above formulas,χI can be optimized, including all keyframe velocities, gravity vectors, and monocular scales in sliding windows.


## 3.3. Vision and Odometer Preintegral Alignment

Since the gyro measurement is used for the rotation measurement of the odometer preintegration, the gyro offset after calibration can be used directly. The variables to be optimized are defined as(16)χП=vd0d0,vd1d1,…,vdmdm,s2T.Similarly, the odometer preconcentration term is transferred to thec0 coordinate system and converted into the form of Hx=b:(17)Hdj+1djχП=α˜bj+1bkj−pcd+Rc0djRdj+1c0pcd≈z˜dj+1dj,Hdj+1dj=−IΔtj0Rc0djp¯cj+1c0−p¯cjc0.By solving the linear least-squares problem,(18)minχП∑j∈Bz˜dj+1dj−Hdj+1djχП2.The coordinate system speed of odometer in each frame in the window and the parameters of the monocular scale can be obtained.


## 3.4. Dynamic Weighted Scale Parameter

The idea of dynamic weighting method is used to design a sliding window with a size of 5 keyframes based on the original sliding window of the system and reoperate the sliding window of the system with a size of 10 keyframes. Therefore, the keyframeBi∈1,2,3,4,5 is selected from the sliding window, where 1–5 frames within the window are included in B1, 2–6 frames within the window are included in B2, and so on, and B5 is derived. The scale parameters s1,i and s2,i at Bi are calculated according to Sections 3.2 and 3.3, respectively, and the minimum mean square errors e1,i and e2,i are obtained. The scale parameter s=W1s1+W2s2 is calculated by dynamically adjusting the weights W1 and W2 according to the above parameters, wherein ∑n=12Wn=1. The initial values of W1 and W2 are both 0.5 as shown in Figure 3.Figure 3
Dynamic weighting flowchart.


## 4. Back-End Tight Coupling Optimization

### 4.1. State Variables and Cost Functions

To improve computational efficiency, a sliding window is used to achieve graph optimization in this paper, so the state variables defining the sliding window optimization at timej are shown as(19)χ=x0,x1,…,xn,λ0,λ1,…,λk,xi=pbiw,viw,qbiw,ma,mω,i∈0,n,where xi represents the position, velocity, the orientation of the robot in the global coordinate system, and the bias of the IMU. λk represents the inverse depth of the kth feature point. n and k represent the number of keyframes and the total number of 3D feature points, respectively.In the sliding window, the residual of all measurement models is minimized to optimize the state variables:(20)minχρrp−Jpχ2+∑i∈BρrBz˜bi+1bi,χPbi+1bi2+∑i∈DρrDz˜di+1di,χPdi+1di2+∑i,j∈CρrCz˜icj,χPicj2,where rBz˜bi+1bi,χ and rDz˜di+1di,χ are the residual measurements of the IMU and odometer between state xi and xi+1, and B and D are the preintegral measurements of the IMU and odometer; rCz˜ici,χ is the 3D feature point reprojection error; rp is the prior information in the window; Jp is the Jacobian matrix obtained from the last optimization result; ρ is the Huber kernel function to make the optimization more robust.


### 4.2. Camera Measurement Residual

In this paper, a pinhole model is used to calculate the reprojection error. First, the reprojection errorrCz˜icj,χ is defined according to the observation value zicj=uicj,vicj,1T of the kth feature point in the image frame cj in the normalized camera coordinate system:(21)rCz˜icj,χ=xcjzcj−uicjycjzcj−vicj,where(22)ucjvcjzcj=RbcRwbjRbjwRbc1λkucivci1+pcb+pbiw−pbjw−pcb.The above formula is a three-dimensional coordinate form of the reprojection equation, and the optimization variable is based on the estimated value and the reprojection error in the normalized camera coordinate system, including the state quantitiespbiw,qbiw and pbjw,qbjw and the inverse depth λl at two moments. To minimize the reprojection error, it is necessary to optimize the above state quantities, and the corresponding Jacobian matrix is obtained as shown in equations (23)–(25).


### 4.3. IMU Measurement Residual

According to the IMU measurement in the two consecutive framesbi and bi+1 and the content in formula (4), the residual of the IMU is obtained as follows:(23)rBz˜bi+1bi,χ=δαbi+1biδβbi+1biδθbi+1biδmaδmω=Rwbipbi+1w−pbiw+12gwΔti2−vbiwΔti−α˜bi+1biRwbivbi+1w+gwΔti−vbiw−β˜bi+1bi2qbiw−1⊗qbi+1w⊗q˜bi+1bi−1xyzmabi+1−mabimwbi+1−mabi.Here,α˜bi+1bi, β˜bi+1bi, and q˜bi+1bi are the measurements obtained after the corrected bias. δθbi+1bi is a three-dimensional error state representation of the quaternion, and qxyz represents the imaginary part of the extracted quaternion q. In the graph optimization, the state quantities xi and xi+1 need to be optimized, and the corresponding Jacobian matrix is obtained as shown in equations (27)–(29).


### 4.4. Odometer Measurement Residual

It is assumed that the axle is on the central axis of the robot. The amount of displacement measured by the odometer can be used to obtain the displacement of the center of the robot. Therefore, the odometer measurement residual is defined as(24)rDz˜di+1di,χ=ΔRdi+1diΔpdi+1di=RwdiRdi+1wExpδθdi+1di−RwdiRdjwpwdi+1+pwdi+δpdi+1di.In the graph optimization model, we need to optimize the statesxi and xi+1 under two consecutive frames bi and bi+1 and obtain the corresponding Jacobian matrices such as formulas (31) and (32):(25)JC0=∂rC∂pbiw∂rC∂qbiw=RbcRwbj−RbcRwbjRbiwRbc1λlp˜ci+pcb×,(26)JC1=∂rC∂pbjw∂rC∂qbjw=−RbcRwbjRbcRwbjRbiwRbc1λlp˜ci+pcb+pbiw−pbjw×,(27)JC2=∂rC∂λl=−RbcRwbjRbiwRcbp˜ciλl2,(28)JB0=−RwbiRwbiΔp×−RwbiΔti−Jbaα−Jbwα0−Lqbi+1w−1⊗qbiwRq˜bi+1bi00−Lqbi+1w−1⊗qbiw⊗q˜bi+1biJbwqi+1i0Rwbivbi+1w−vbiw+gwΔti×−Rwbi−Jbaβ−Jbwβ000−I00000−I,(29)JB1=Rwbi00000Lq˜bi+1bi−1⊗qbiw−1qbi+1w00000Rwbi00000I00000I,where(30)q=w,x,y,z=w,s,Δp=pbi+1w−pbiw−vbiwΔti+12gwΔti2,Lq=w×w−wT0+sI,Rq=−w×w−wT0+sI,(31)JD0=∂rD∂δxi=0qbi+1w−1⊗qbiwExpδθbibi+1000I0viw00,(32)JD1=∂rD∂δxi+1=0qbi+1bi−1⊗qbiw−1Expδθbibi+1000−RbiwRwbi+10−RbiwRwbi+1vi+1w00.


## 4.1. State Variables and Cost Functions

To improve computational efficiency, a sliding window is used to achieve graph optimization in this paper, so the state variables defining the sliding window optimization at timej are shown as(19)χ=x0,x1,…,xn,λ0,λ1,…,λk,xi=pbiw,viw,qbiw,ma,mω,i∈0,n,where xi represents the position, velocity, the orientation of the robot in the global coordinate system, and the bias of the IMU. λk represents the inverse depth of the kth feature point. n and k represent the number of keyframes and the total number of 3D feature points, respectively.In the sliding window, the residual of all measurement models is minimized to optimize the state variables:(20)minχρrp−Jpχ2+∑i∈BρrBz˜bi+1bi,χPbi+1bi2+∑i∈DρrDz˜di+1di,χPdi+1di2+∑i,j∈CρrCz˜icj,χPicj2,where rBz˜bi+1bi,χ and rDz˜di+1di,χ are the residual measurements of the IMU and odometer between state xi and xi+1, and B and D are the preintegral measurements of the IMU and odometer; rCz˜ici,χ is the 3D feature point reprojection error; rp is the prior information in the window; Jp is the Jacobian matrix obtained from the last optimization result; ρ is the Huber kernel function to make the optimization more robust.


## 4.2. Camera Measurement Residual

In this paper, a pinhole model is used to calculate the reprojection error. First, the reprojection errorrCz˜icj,χ is defined according to the observation value zicj=uicj,vicj,1T of the kth feature point in the image frame cj in the normalized camera coordinate system:(21)rCz˜icj,χ=xcjzcj−uicjycjzcj−vicj,where(22)ucjvcjzcj=RbcRwbjRbjwRbc1λkucivci1+pcb+pbiw−pbjw−pcb.The above formula is a three-dimensional coordinate form of the reprojection equation, and the optimization variable is based on the estimated value and the reprojection error in the normalized camera coordinate system, including the state quantitiespbiw,qbiw and pbjw,qbjw and the inverse depth λl at two moments. To minimize the reprojection error, it is necessary to optimize the above state quantities, and the corresponding Jacobian matrix is obtained as shown in equations (23)–(25).


## 4.3. IMU Measurement Residual

According to the IMU measurement in the two consecutive framesbi and bi+1 and the content in formula (4), the residual of the IMU is obtained as follows:(23)rBz˜bi+1bi,χ=δαbi+1biδβbi+1biδθbi+1biδmaδmω=Rwbipbi+1w−pbiw+12gwΔti2−vbiwΔti−α˜bi+1biRwbivbi+1w+gwΔti−vbiw−β˜bi+1bi2qbiw−1⊗qbi+1w⊗q˜bi+1bi−1xyzmabi+1−mabimwbi+1−mabi.Here,α˜bi+1bi, β˜bi+1bi, and q˜bi+1bi are the measurements obtained after the corrected bias. δθbi+1bi is a three-dimensional error state representation of the quaternion, and qxyz represents the imaginary part of the extracted quaternion q. In the graph optimization, the state quantities xi and xi+1 need to be optimized, and the corresponding Jacobian matrix is obtained as shown in equations (27)–(29).


## 4.4. Odometer Measurement Residual

It is assumed that the axle is on the central axis of the robot. The amount of displacement measured by the odometer can be used to obtain the displacement of the center of the robot. Therefore, the odometer measurement residual is defined as(24)rDz˜di+1di,χ=ΔRdi+1diΔpdi+1di=RwdiRdi+1wExpδθdi+1di−RwdiRdjwpwdi+1+pwdi+δpdi+1di.In the graph optimization model, we need to optimize the statesxi and xi+1 under two consecutive frames bi and bi+1 and obtain the corresponding Jacobian matrices such as formulas (31) and (32):(25)JC0=∂rC∂pbiw∂rC∂qbiw=RbcRwbj−RbcRwbjRbiwRbc1λlp˜ci+pcb×,(26)JC1=∂rC∂pbjw∂rC∂qbjw=−RbcRwbjRbcRwbjRbiwRbc1λlp˜ci+pcb+pbiw−pbjw×,(27)JC2=∂rC∂λl=−RbcRwbjRbiwRcbp˜ciλl2,(28)JB0=−RwbiRwbiΔp×−RwbiΔti−Jbaα−Jbwα0−Lqbi+1w−1⊗qbiwRq˜bi+1bi00−Lqbi+1w−1⊗qbiw⊗q˜bi+1biJbwqi+1i0Rwbivbi+1w−vbiw+gwΔti×−Rwbi−Jbaβ−Jbwβ000−I00000−I,(29)JB1=Rwbi00000Lq˜bi+1bi−1⊗qbiw−1qbi+1w00000Rwbi00000I00000I,where(30)q=w,x,y,z=w,s,Δp=pbi+1w−pbiw−vbiwΔti+12gwΔti2,Lq=w×w−wT0+sI,Rq=−w×w−wT0+sI,(31)JD0=∂rD∂δxi=0qbi+1w−1⊗qbiwExpδθbibi+1000I0viw00,(32)JD1=∂rD∂δxi+1=0qbi+1bi−1⊗qbiw−1Expδθbibi+1000−RbiwRwbi+10−RbiwRwbi+1vi+1w00.


## 5. Experimental Result Analysis

The experimental scene in this paper is the indoor environment with a Vicon motion capture system, and the robot platform is Turtlebot3 mobile robot with a camera named Xiaomi. The robot moves in the area of3m×3m, and the average speed of the whole process is 0.2m/s. In the experiment, the position and posture of the robot acquired by the motion capture system in the motion capture coordinate system are taken as the true value of the motion trajectory.


### 5.1. Scale Error Analysis

The experiment mainly verifies the effects of the initialization state estimation algorithm and the dynamic weighting result proposed in Section3.4. Compared with the initialization using only IMU, in this paper, the combination of visual, IMU, and odometer can achieve more accurate scale and convergence time. The scale estimation results and time are shown in Table 1, and the root mean square error (RMSE) is used to quantitatively represent the accuracy of the algorithm.Table 1
Scale values and convergence time.
AlgorithmMonocular + IMUProposed algorithmNumberScale (m)Time (s)Scale (m)Time (s)10.074662.676050.034562.3100320.050312.315830.025351.8579930.075702.412010.024352.0931740.032622.218470.014931.6817850.030842.344070.014592.0044760.081142.750720.012581.7872470.010521.353510.016062.0022980.029751.556140.024381.9980390.033402.324410.012752.08879100.079082.795790.022331.89776Mean0.049812.274700.020191.97215RMSE0.024480.453330.006790.16796It can be seen from the results in Table1 that the initialization state estimation algorithm of this paper reduces the RMSE of the scale value by 72.2% and the convergence time RMSE by 62.9% compared with the amplification using only the IMU for initialization. Therefore, the initialization state estimation algorithm proposed has obvious improvement in the accuracy and stability of scale estimation.


### 5.2. Pose Error Analysis

The algorithm proposed in this paper is an improvement based on VINS_MONO, so the experimental results are compared with the VINS_MONO. The trajectory of mono_imu in the figure represents the result of the VINS_MONO [25] algorithm running on the robot platform. The trajectory and true value comparison of the two algorithms are shown in Figures 4 and 5. The dotted line in the figure is the real trajectory obtained by the motion capture system, and the solid line is the trajectory obtained by the algorithm. As can be seen from the figure, due to the odometer constraint and more accurate initialization scale added to the algorithm in this paper, the accuracy in the positioning process of mobile robots is better than that of VINS_MONO.Figure 4
Comparison of trajectories and truth values of the two algorithms.Figure 5
Comparison ofx-axis and y-axis with true values.To quantitatively analyze the accuracy of the algorithm, the accuracy is measured by estimating the absolute position error (APE) obtained by comparing the pose result with the true motion pose of each time step. The pose and APE and the rotational APE of the algorithm proposed in this paper and the monocular inertial navigation algorithm are shown in Figure6, respectively. The overall pose APE obtained by the two algorithms is shown in Table 2.Figure 6
Pose APE comparison of the two methods.Table 2
APE statistics results of two methods.
IndexProposed algorithmMonocular + IMUMax pose APE (m)0.0754070.311616Min pose APE (m)0.0020260.046728Mean pose APE (m)0.0245330.154703Pose RMSE (m)0.0272960.169879By comparing the two methods, it is not difficult to see that the proposed algorithm is superior to the monocular inertial fusion algorithm in accuracy, which indicates that the odometer information has a certain degree of improvement on the positioning result. Moreover, the better planar constraint of the odometer makes the algorithm consistently stable at 0.03 m in thezw direction, while the monocular inertial fusion algorithm has a certain degree of trajectory divergence in the zw direction. Since the center of gravity of the rigid body model built in the Vicon system is 0.03 m, the true value keeps near 0.03 m in the zw direction. The results are shown in Figure 7.Figure 7
Comparison ofz-axis with true values.


## 5.1. Scale Error Analysis

The experiment mainly verifies the effects of the initialization state estimation algorithm and the dynamic weighting result proposed in Section3.4. Compared with the initialization using only IMU, in this paper, the combination of visual, IMU, and odometer can achieve more accurate scale and convergence time. The scale estimation results and time are shown in Table 1, and the root mean square error (RMSE) is used to quantitatively represent the accuracy of the algorithm.Table 1
Scale values and convergence time.
AlgorithmMonocular + IMUProposed algorithmNumberScale (m)Time (s)Scale (m)Time (s)10.074662.676050.034562.3100320.050312.315830.025351.8579930.075702.412010.024352.0931740.032622.218470.014931.6817850.030842.344070.014592.0044760.081142.750720.012581.7872470.010521.353510.016062.0022980.029751.556140.024381.9980390.033402.324410.012752.08879100.079082.795790.022331.89776Mean0.049812.274700.020191.97215RMSE0.024480.453330.006790.16796It can be seen from the results in Table1 that the initialization state estimation algorithm of this paper reduces the RMSE of the scale value by 72.2% and the convergence time RMSE by 62.9% compared with the amplification using only the IMU for initialization. Therefore, the initialization state estimation algorithm proposed has obvious improvement in the accuracy and stability of scale estimation.


## 5.2. Pose Error Analysis

The algorithm proposed in this paper is an improvement based on VINS_MONO, so the experimental results are compared with the VINS_MONO. The trajectory of mono_imu in the figure represents the result of the VINS_MONO [25] algorithm running on the robot platform. The trajectory and true value comparison of the two algorithms are shown in Figures 4 and 5. The dotted line in the figure is the real trajectory obtained by the motion capture system, and the solid line is the trajectory obtained by the algorithm. As can be seen from the figure, due to the odometer constraint and more accurate initialization scale added to the algorithm in this paper, the accuracy in the positioning process of mobile robots is better than that of VINS_MONO.Figure 4
Comparison of trajectories and truth values of the two algorithms.Figure 5
Comparison ofx-axis and y-axis with true values.To quantitatively analyze the accuracy of the algorithm, the accuracy is measured by estimating the absolute position error (APE) obtained by comparing the pose result with the true motion pose of each time step. The pose and APE and the rotational APE of the algorithm proposed in this paper and the monocular inertial navigation algorithm are shown in Figure6, respectively. The overall pose APE obtained by the two algorithms is shown in Table 2.Figure 6
Pose APE comparison of the two methods.Table 2
APE statistics results of two methods.
IndexProposed algorithmMonocular + IMUMax pose APE (m)0.0754070.311616Min pose APE (m)0.0020260.046728Mean pose APE (m)0.0245330.154703Pose RMSE (m)0.0272960.169879By comparing the two methods, it is not difficult to see that the proposed algorithm is superior to the monocular inertial fusion algorithm in accuracy, which indicates that the odometer information has a certain degree of improvement on the positioning result. Moreover, the better planar constraint of the odometer makes the algorithm consistently stable at 0.03 m in thezw direction, while the monocular inertial fusion algorithm has a certain degree of trajectory divergence in the zw direction. Since the center of gravity of the rigid body model built in the Vicon system is 0.03 m, the true value keeps near 0.03 m in the zw direction. The results are shown in Figure 7.Figure 7
Comparison ofz-axis with true values.


## 6. Conclusions

In this paper, aiming at the problem of low accuracy and robustness of the monocular inertial navigation algorithm in the pose estimation of mobile robots, a multisensor fusion positioning system is designed, including monocular vision, IMU, and odometer, which realizes the initial state estimation of monocular vision and the fusion location of the tightly coupled visual-inertial odometer.The method of odometer preintegration and dynamic weighted scale parameters, as well as the back-end tightly coupled pose optimization method based on graph optimization, is proposed, which greatly improves the accuracy and robustness of the positioning system. Then, the effectiveness and feasibility of the proposed algorithm is verified by theoretical derivation and practical experiments.Although the research in this paper has achieved certain results, there are still some aspects that need to be improved. First, the results are only tested in the laboratory or a small number of complex scenarios, and it cannot fully explain the ability to work robustly and stably for a long time in a complex environment. Second, binocular cameras are superior to monocular cameras in terms of depth calculation and scale estimation. In subsequent solutions, multisensor fusion schemes can be considered based on binocular vision and contribute to future map-based visual navigation studies.



---

*Source: 8701619-2020-06-08.xml*