Robust Tightly Coupled Pose Measurement Based on Multi-Sensor Fusion in Mobile Robot System

Abstract

Currently, simultaneous localization and mapping (SLAM) is one of the main research topics in the robotics field. Visual-inertia SLAM, which consists of a camera and an inertial measurement unit (IMU), can significantly improve robustness and enable scale weak-visibility, whereas monocular visual SLAM is scale-invisible. For ground mobile robots, the introduction of a wheel speed sensor can solve the scale weak-visibility problem and improve robustness under abnormal conditions. In this paper, a multi-sensor fusion SLAM algorithm using monocular vision, inertia, and wheel speed measurements is proposed. The sensor measurements are combined in a tightly coupled manner, and a nonlinear optimization method is used to maximize the posterior probability to solve the optimal state estimation. Loop detection and back-end optimization are added to help reduce or even eliminate the cumulative error of the estimated poses, thus ensuring global consistency of the trajectory and map. The outstanding contribution of this paper is that the wheel odometer pre-integration algorithm, which combines the chassis speed and IMU angular speed, can avoid the repeated integration caused by linearization point changes during iterative optimization; state initialization based on the wheel odometer and IMU enables a quick and reliable calculation of the initial state values required by the state estimator in both stationary and moving states. Comparative experiments were conducted in room-scale scenes, building scale scenes, and visual loss scenarios. The results showed that the proposed algorithm is highly accurate—2.2 m of cumulative error after moving 812 m (0.28%, loopback optimization disabled)—robust, and has an effective localization capability even in the event of sensor loss, including visual loss. The accuracy and robustness of the proposed method are superior to those of monocular visual inertia SLAM and traditional wheel odometers.

1. Introduction

At present, the influential simultaneous localization and mapping (SLAM) systems with monocular vision include ORB-SLAM2 [1], LSD-SLAM [2], DSO [3], and SVO [4]. Among them, ORB-SLAM2 [4] has high adaptability and strong robustness to indoor and outdoor environments. However, the algorithm itself is based on pure vision. When there are no visual features, the accuracy and robustness of the pose estimation will decrease rapidly, and the algorithm may fail. Therefore, in the follow-up development of visual SLAM, in order to overcome the shortcomings of pure vision, the strategy of multisensor fusion is adopted.

Accordingly, sensors with scale measurement capabilities and monocular vision sensors are used to perform fusion vision SLAM to increase accuracy and robustness. Relatively stable and reliable solutions can be obtained with lasers [5]; however, at present, this method is generally only suitable for large-scale scenarios, such as unmanned driving, and is unsuitable for applications with limited costs. The inertial measurement unit (IMU) has become a generally accepted option. However, it exhibits a non-negligible cumulative error if run for a long period of time [6], especially in visually restricted conditions without texture or under weak illumination, in which case the visual mile cannot be used to correct the IMU error. In [7], the scale observability of a monocular visual inertial odometer on a ground mobile robot was analyzed in detail. When the robot moved at a constant speed, due to the lack of acceleration excitation, the constraint on the scale was lost, resulting in a gradual increase in the scale uncertainty and positioning error.

A wheel speed inertial odometer was integrated with a monocular visual odometer using the extended Kalman filter (EKF) [8,9], assuming the robot to be running on an ideal plane, and 3-DOF pose measurement was performed. The wheel speed inertial odometer uses wheel speed measurement for EKF status prediction; the angular speed measurement is integrated for dead reckoning; and the visual odometer method is used for an EKF measurement update. In the above-mentioned EKF-based loose coupling method, when the visual odometer cannot accurately calculate the pose due to insufficient visual characteristics, some proportion of its output in the filter will fall, resulting in ineffective visual observation. Consequently, the accuracy is reduced. VINS-Mono used tightly coupled nonlinear optimization [10,11]. By combining the IMU pre-integral measurement and visual measurement in a tightly coupled form to achieve the maximum posterior probability, we use the nonlinear optimization method to reach the optimal state. An open-source visual inertia SLAM algorithm VINS-Fusion [12,13] was developed based on VINS-Mono, supporting multiple sensor combinations (binocular camera + IMU; monocular camera + IMU; binocular camera only); GPS is used to improve the accuracy of the global path. In pursuit of better performance, wheel speed is used in SLAM system [14]—the wheel speed sensor and the visual odometer were integrated tightly. In this way, they make least-squares rather than optimization. However, they do not consider the unreliability of wheel speed. When a robot moves on uneven surfaces, or a wheel slip occurs, an incorrect wheel speed measurement will significantly affect the scale accuracy, and can potentially lead to system failure. In [15], a batch optimization framework with formulated edges was proposed. Moreover, tightly coupled nonlinear optimization methods were used to integrate vision, IMU, and wheel speed, and to perform pose. The error cost function was composed of vision errors, inertial measurement errors, and wheel speed sensor dead reckoning errors. In addition, assuming that the vehicle was moving on an approximate plane, a “soft” plane constraint term was added to the error cost function. While the robot is running with fixed acceleration or going straight, we do not know how long it moves in real time or what the direction is. Therefore, the encoder and soft plane constraints will make the task easier.

According to whether the visual feature points are added to the state vector of the filter or the optimization equation, the visual inertial odometry method based on filtering can be divided into two types: loose coupling and tight coupling. Further, the loose coupling method is divided into two parts: the visual odometer and state estimator. The visual odometer tracks the feature points and calculates the current camera pose, and the state estimator uses the IMU to measure and adjust the camera pose obtained by the visual odometer. As the visual features are invisible to the state estimator, they cannot be adjusted based on the IMU measurement, and the poor position accuracy of the visual features causes the accuracy of the visual odometer to decline. As a result, the correlation between measurements is not fully utilized for pose measurement, thereby reducing the accuracy. In the tight coupling method, the carrier pose and position of the feature point are used as variables to be estimated, and the filter method or optimized method is used for estimation.

Thus far, for the existing visual inertia SLAM algorithm, when the robot is moving at a constant speed, or purely rotating, and encounters scenes with insufficient visual features, the problems of low accuracy and poor robustness arise. Tightly coupled wheel speed information can help solve this problem. However, there are few studies on multi-sensor fusion SLAM for wheeled mobile robots based on vision, inertia, and wheel speed measurements that are tightly coupled and optimized. There is no complete solution similar to ORB-SLAM. Therefore, research on tightly coupled monocular visual odometer combined with wheel speed measurement is significant. Therefore, we use IMU and wheel odometry to make a monocular visual odometry system to gain the scale reconstruction.

The main novelty and contributions of this paper include:

• A multi-sensor fusion SLAM algorithm using monocular vision, inertial measurement, and wheel speed measurement is proposed. The wheel speed and IMU angular velocity pre-integration of the wheel odometer can avoid the repeated integration caused by the linearization point change in the iterative optimization process.
• Based on the state initialization of the wheel odometer and IMU, the initial state value required by the state estimator can be quickly and reliably calculated in both stationary and moving states.

The method in this paper solves the problem of poor pose estimation accuracy caused by the weak observability of monocular visual inertial SLAM, and further improves the robustness of pose estimation.

2. Multi-Sensor State Estimation Based on Tight Coupling Optimization

2.1. SLAM Based on Multi-Sensor Fusion

The multi-sensor fusion state estimator in this study uses monocular vision, IMU, and wheel odometer measurements based on feature point optical flow tracking [16]. None of these sensors can measure the absolute pose. Therefore, the multi-sensor fusion state estimator, as an odometer algorithm, has an unavoidable cumulative error. Therefore, we used key frame selection, loop detection [17], and back-end optimization algorithms based on the VINS-Mono framework and applied them to the multi-sensor fusion state estimator to form a complete SLAM system. Figure 1 shows a system block diagram of the SLAM method.

In a multi-sensor state estimation process, the main data processing and analysis processes include raw sensor input, data pre-processing (including calibration compensation, time alignment, and pre-fused wheel odometer), state initialization, state estimation problem solving, loopback detection, and backend optimization. The processes of optimal state estimator, wheel odometer pre-integration, and initialization reflect the contributions and innovations of this paper. The rest is our flexible use of existing methods. Section 2.2, Section 2.3, Section 2.4, Section 2.5, Section 2.6 and Section 2.7 analyze the core multi-sensor fusion state estimation problem and the pre-integration process of IMU and wheel speed. Section 2.8 analyzes the initialization process of the state.

2.2. Variables to Be Estimated and Observation

Considering that the IMU zero offset always exists, when defining the variable $\mathcal{X}$ to be estimated, the IMU zero offset of each key frame needs to be included. Therefore, in the k key frame, we have:

$$\begin{array}{c}\mathcal{X}={\{x_k,{\lambda }_l\}}_{k\in \mathcal{K},l\in ℒ}\\ x_k=\left[p_{B_k}^W,v_{B_k}^W,q_{B_k}^W,b_{ak},b_{gk}\right]\end{array}$$

(1)

For the IMU, $x_k\in R^{16\times 1}$ is the state, $p_B^W\in R^{3\times 1}$ denotes the position, $q_B^W\in R^{4\times 1}$ is the attitude (quaternion form), $v_B^W\in R^{3\times 1}$ denotes the speed, and $b_a\in R^{3\times 1}$ and $b_g\in R^{3\times 1}$ are the accelerometer and gyroscope bias, respectively. $\mathcal{K}$ is the key frames and $ℒ$ is the feature points. Further, ${\lambda }_l$ is the inverse depth of $ℒ$ (the inverse of the Z-axis coordinate).

Additionally, pre-fusion wheel odometer observations are added based on VINS-Mono. The original wheel odometer is defined as:

$${\widehat{m}}_{odomk}\triangleq {\left[\begin{array}{ccc}{p}_{xk}& p_{yk}& {\theta }_k\end{array}\right]}^T$$

(2)

where {$p_{xk}$ $p_{yk}$} is the coordinate of the wheel odometer relative to the starting point and ${\theta }_k$ is the angle of rotation relative to the starting direction. Therefore, the observation $\mathcal{Z}$ used to constrain the $\mathcal{X}$:

$$\mathcal{Z}={\{{\mathcal{Z}}_{C_i},{ℬ}_{ij},{\mathcal{O}}_{ij}\}}_{(i,j)\in \mathcal{K}}$$

(3)

where visual feature point observation ${\mathcal{Z}}_{C_i}={\{{\widehat{z}}_{il}\}}_{l\in {ℒ}_i}$, containing all feature points ${ℒ}_i$ observed under the i key frame; ${ℬ}_{ij}={\{{\widehat{a}}_t,{\widehat{\omega }}_t\}}_{t_i\le t\le t_j}$ is IMU pre-integration of accelerated speed and angular speed; and ${\mathcal{O}}_{ij}={\{\Delta {\widehat{m}}_{odomt},{\widehat{a}}_{avgt},{\widehat{\omega }}_{avgt}\}}_{t_i\le t\le t_j}$ is pre-fusion wheel odometer.

2.3. Optimal Estimation Problem

The maximum posterior estimation and Bayes’ theorem shows:

$${\mathcal{X}}^{\ast }=\underset{\mathcal{X}}{\mathrm{argmax}}p(\mathcal{Z}|\mathcal{X})p(\mathcal{X})$$

(4)

Here, $p(\mathcal{Z}|\mathcal{X})$ is the conditional probability of observing the occurrence of $\mathcal{Z}$ under the existed state $\mathcal{X}$, which can be obtained by the observation equation and the covariance. $p(\mathcal{X})$ is the prior probability (edge probability) of $\mathcal{X}$. According to formula (3), we have:

$$\begin{array}{cc}p(\mathcal{Z}|\mathcal{X})p(\mathcal{X})& = p(\mathcal{X}){\displaystyle {\displaystyle \prod }_{(i,j)\in \mathcal{K}}}p({\mathcal{Z}}_{C_i},{ℬ}_{ij},{\mathcal{O}}_{ij} | \mathcal{X})\hfill \\ & = p(\mathcal{X}){\displaystyle {\displaystyle \prod }_{i\in \mathcal{K}}}{\displaystyle {\displaystyle \prod }_{l\in ℒ}}p({\widehat{z}}_{il}|x_i,{\lambda }_l){\displaystyle {\displaystyle \prod }_{(i,j)\in \mathcal{K}}}p({\{{\widehat{a}}_t,{\widehat{\omega }}_t\}}_{t_i\le t\le t_j}|x_i,x_j)\hfill \\ & \times {\displaystyle {\displaystyle \prod }_{(i,j)\in \mathcal{K}}}p({\{\Delta {\widehat{m}}_{odomt},{\widehat{a}}_{avgt},{\widehat{\omega }}_{avgt}\}}_{t_i\le t\le t_j}|x_i,x_j)\hfill \end{array}$$

(5)

We use the least-squares method to find the maximum posterior estimate, and use Mahalanobis distance to measure the difference between the residual and the covariance matrix.

$$\begin{array}{ll}{\mathcal{X}}^{\ast }& = \underset{\mathcal{X}}{\mathrm{argmin}}\{\Vert r_p-H_p\mathcal{X}{\Vert }^2+{\displaystyle {\displaystyle \sum }_{i\in \mathcal{K}}}{\displaystyle {\displaystyle \sum }_{l\in ℒ}}\mathsf{\rho }(\Vert r_C{({\widehat{z}}_{il},x_i,{\lambda }_l)\Vert }_{{\mathsf{\Sigma }}_{C_{il}}}^2)\\ & + {\displaystyle {\displaystyle \sum }_{(i,j)\in \mathcal{K}}}\Vert r_{ℬ}{({\{{\widehat{a}}_t,{\widehat{\omega }}_t\}}_{t_i\le t\le t_j},x_i,x_j)\Vert }_{{\mathsf{\Sigma }}_{{ℬ}_{ij}}}^2\hfill \\ & + {\displaystyle {\displaystyle \sum }_{(i,j)\in \mathcal{K}}}\mathsf{\rho }(\Vert r_{\mathcal{O}}{({\{\Delta {\widehat{m}}_{odomt},{\widehat{a}}_{avgt},{\widehat{\omega }}_{avgt}\}}_{t_i\le t\le t_j}|x_i,x_j)\Vert }_{{\mathsf{\Sigma }}_{{\mathcal{O}}_{ij}}}^2)\}\hfill \end{array}$$

(6)

Here, $r{\Vert (·)\Vert }_{\mathsf{\Sigma }}^2$ is the Mahalanobis distance and $\mathsf{\Sigma }$ is the covariance matrix. $\{r_p,H_p\}$ is the prior information from marginalization; $r_C\in R$ is the visual residual; $r_{\mathcal{O}}\in R^{3\times 1}$ is the wheel odometer residual; and $r_{ℬ}\in R^{15\times 1}$ is the IMU residual. In addition, the Huber loss function $\mathsf{\rho }$ [18] is used to improve the robustness, where $\mathsf{\rho }$ is

$$\mathsf{\rho }(\mathrm{s})=\{\begin{array}{c}1 s\ge 1\\ 2\sqrt{s}-1 s<1\end{array}$$

(7)

2.4. Visual Measurement Constraints

The specific process is as follows: while the feature point $l$ is first observed in the key frame i, it is recorded and tracked. Its spatial pose is defined as a function of the key frame i pose $(p_{C_i},q_{C_i})$ and the inverse depth ${\lambda }_l$ of the feature point. While the feature point $l$ is observed again in the key frame j, a visual residual term is generated. The residual term $r_{C_{jl}}$ represents the error of the feature point l in the position of j. It is also called the re-projection error, which is a function of the key frame i pose $(p_{C_i},q_{C_i})$.

$$r_C({\widehat{z}}_{jl},\mathcal{X})=r_{C_{jl}}(p_{B_i}^W,p_{B_j}^W,q_{B_i}^W,q_{B_j}^W,{\lambda }_l)={\left[\begin{array}{cc}{b}_1& b_2\end{array}\right]}^T·({{\hat{\mathcal{P}}}_l}^{C_j}-\frac{{{\mathcal{P}}_l}^{C_j}}{\Vert {{\mathcal{P}}_l}^{C_j}\Vert })$$

(8)

Here,

$$\begin{gathered} {{\hat{\mathcal{P}}}_l}^{C_j}={\pi }_c^{-1}({\widehat{z}}_{il})={\pi }_c^{-1}\left(\left[\begin{array}{c}{\widehat{u}}_l^{c_j}\\ {\widehat{v}}_l^{c_j}\end{array}\right]\right) \\ {{\mathcal{P}}_l}^{C_j}=(R_B^C(R_W^{B_j}\left(R_{B_i}^W\left(R_C^B·\frac{1}{{\lambda }_l}·{\pi }_c^{-1}\left(\left[\begin{array}{c}{u}_l^{c_u}\\ v_l^{c_u}\end{array}\right]\right)+p_C^B\right)+p_{B_i}^W-p_{B_j}^W\right)-p_C^B) \end{gathered}$$

(9)

In the formula, $R_{{ℱ}_2}^{{ℱ}_1}\in SO(3)$ represents the rotation matrix from ${ℱ}_2$ coordinate system to ${ℱ}_1$ coordinate system; ${{\hat{\mathcal{P}}}_l}^{c_j}\in R^{2\times 1}$ is the position where the feature point l is projected onto the unit ball in the key frame j; ${\pi }_c^{-1}$ is the back projection function, which can project the pixel coordinates into the camera coordinate system $C_j; {{\mathcal{P}}_l}^{C_j}\in R^{2\times 1}$ is the position projected on the unit ball in the key frame j; to compare the error with ${{\mathcal{P}}_l}^{C_j}$, it needs to be transformed into the camera coordinate system C_j of the key frame j; and $\left[\begin{array}{cc}{b}_1& b_2\end{array}\right]$ are the two orthogonal base vectors on the tangent plane of the unit ball and the projection lines with the feature points in the orthogonal direction.

2.5. IMU Constraints

In the visual odometer method based on bundle adjustment, the state of the carrier is optimized and visual measurement is used to constrain. The IMU measurement between frames is added as a constraint on the optimization framework, thereby improving robustness.

2.5.1. IMU Pre-Integration

To reduce the complicated operation caused by reintegration, we used the IMU pre-integration method [19] to fuse IMU measurements between two consecutive key frames.

2.5.2. Residual Term

The IMU pre-integration processes the IMU measurement for a continuous period based on the given IMU zero offset and obtains the relative pose constraint between the initial and end states of the time period. The IMU pre-integration residual term is defined as:

$$\begin{array}{cc}& r_{ℬ}({\{{\widehat{a}}_t,{\widehat{\omega }}_t\}}_{t_k\le t\le t_{k+1}},x_k,x_{k+1})\hfill \\ & =\left[\begin{array}{c}\delta {\alpha }_{B_k+1}^{B_k}\\ \delta {\beta }_{B_k+1}^{B_k}\\ \begin{array}{c}\delta {\theta }_{B_k+1}^{B_k}\\ \delta b_a{}_{B_k+1}^{B_k}\\ \delta b_g{}_{B_k+1}^{B_k}\end{array}\end{array}\right]=\left[\begin{array}{c}R{\{q_{B_k}^W\}}^T(p_{B_k+1}^W-p_{B_k}^W-v_{B_k}^W\Delta t+\frac{1}{2}g\Delta t^2)-{\widehat{\alpha }}_{B_k+1}^{B_k}\\ R{\{q_{B_k}^W\}}^T(v_{B_k+1}^W-v_{B_k}^W+g\Delta t)-{\widehat{\beta }}_{B_k+1}^{B_k}\\ \begin{array}{c}2{\{{\widehat{q}}_{B_k+1}^{B_k}\ast \otimes q_{B_k}^W\ast \otimes q_{B_k+1}^W\}}_{xyz}\\ b_{a{B}_{k+1}}-b_{a{B}_k}\\ b_{g{B}_{k+1}}-b_{g{B}_k}\end{array}\end{array}\right]\hfill \end{array}$$

(10)

The random distribution of the residual term $r_{ℬ}$ conforms to $N(0,{\mathsf{\Sigma }}_B)$ and ${\mathsf{\Sigma }}_B$ is obtained by updating the covariance equation. $\delta {\theta }_{B_k+1}^{B_k}\in R^{3\times 1}$ is three-dimensional small perturbation and $\delta$ represents the error term. The IMU pre-integration provides constraints on the variables to be optimized contained in the two key frames before and after. In the process of nonlinear optimization, the essence of the constraint is to provide the direction and gradient of the variable to be optimized by calculating the Jacobian matrix of the residual. As the direction of gravity is obtained during the initialization of the visual inertial odometer, the gravity acceleration $g^W$ is not used as a variable to be optimized.

2.6. Wheeled Odometer Constraints

On ground mobile robots, a wheel speed meter is typically used to perform dead reckoning to obtain continuous relative poses of the robot. The continuous position and reliable scale estimation of the wheel odometer make it suitable for tasks such as path planning and navigation.

2.6.1. Two-Dimensional Wheel Odometer Algorithm

The two-dimensional wheel odometer has an unavoidable cumulative, error but can provide a continuous carrier trajectory. As the wheel speed meter measures the average wheel speed over a period of time, the chassis speed measurement ${\widehat{m}}_{base}$ measures the average speed during this time. The position and attitude update methods of the wheel speed odometer include Euler points, median points, and higher-order Runge–Kutta methods. As the sampling speed of the wheel speed meter is high (1 kHz), the Euler integration method is used to reduce the calculation time of the main control microcontroller. This is achieved while assuming that the chassis moves with fixed direction and speed in the original direction during the period and rotates to a new direction at the end of the time period.

The initial state of the wheel odometer is ${\widehat{m}}_{odom0}={\left[p_{x0} p_{y0} {\theta }_0\right]}^T=\mathbf{0}$. Given the previous state ${\widehat{m}}_{odomk-1}={\left[p_{xk-1} p_{yk-1} {\theta }_{k-1}\right]}^T$ of the wheel odometer, the current chassis speed measurement ${\widehat{v}}_{basek}={\left[v_{xk} v_{yk} {\omega }_k\right]}^T$, and the time difference $\mathrm{d}{t}_k=t_k-t_{k-1}$, we can obtain the new wheel odometer state ${\widehat{m}}_{odomk}$ as:

$${\widehat{m}}_{odomk}={\left[p_{xk} p_{yk} {\theta }_k\right]}^T$$

(11)

$$\left[\begin{array}{c}{p}_{xk}\\ p_{yk}\end{array}\right]=\left[\begin{array}{c}{p}_{xk-1}\\ p_{yk-1}\end{array}\right]+\left[\begin{array}{cc}cos{\theta }_{k-1}& -sin{\theta }_{k-1}\\ sin{\theta }_{k-1}& cos{\theta }_{k-1}\end{array}\right]\left[\begin{array}{c}{v}_{xk}\\ v_{yk}\end{array}\right]d{t}_k$$

(12)

$${\theta }_k={\theta }_{k-1}+{\omega }_{k}d{t}_k$$

(13)

2.6.2. Wheel Odometer Pre-Integration

In this paper, we propose a pre-integration method for wheel odometer. We use IMU and wheel speed to measure the relative pose between two key frames. The data of the two sensors makes pre-fusion ${\widehat{m}}_{fused odom}$. Thereafter, only the ${\widehat{m}}_{fused odom}$ is measured, according to the wheel odometer kinematics equation, and a continuous calculation and integration is made to find the displacement.

The incremental update equation of the wheel odometer is:

$$\{\begin{array}{c}{p}_{O_{i+1}}^{O_k}=p_{O_i}^{O_k}+R\{q_{O_i}^{O_k}\}\Delta p_{O_{i+1}}^{O_i}=p_{O_i}^{O_k}+R\{q_{O_i}^{O_k}\}(\Delta {\widehat{p}}_{O_{i+1}}^{O_i}+D_{O_{k+1}}^{O_k}{\eta }_{os})\hfill \\ q_{O_{i+1}}^{O_k}=q_{O_i}^{O_k}\otimes q_{O_{i+1}}^{O_i}=q_{O_i}^{O_k}\otimes q\{R_B^O({\widehat{\omega }}_{avgi+1}-b_g-{\eta }_g)\Delta t\}\hfill \\ b_{gi+1}=b_{gi}+{\eta }_{b_g}\mathsf{\Delta }t\hfill \end{array}$$

(14)

where $\Delta {\widehat{p}}_{O_{i+1}}^{O_i}\triangleq {\left[\Delta {\widehat{p}}_{xi+1}\Delta {\widehat{p}}_{yi+1}0\right]}^T$ denotes wheel odometer with noise measurement; the initial state value $p_0=0, q_0={\left[1 0 0 0\right]}^T$. $\mathrm{x}={\left[p^{O_k}{q}^{O_k}{b}_g\right]}^T$ is the pre-credit term.

The nominal weight of the wheel odometer pre-integration item can be incrementally updated based on the pre-fusion wheel odometer measurement:

$$\{\begin{array}{c}{\widehat{p}}_{O_{i+1}}^{O_k}={\widehat{p}}_{O_i}^{O_k}+R\{{\widehat{q}}_{O_i}^{O_k}\}\Delta {\widehat{p}}_{O_{i+1}}^{O_i}\hfill \\ {\widehat{q}}_{O_{i+1}}^{O_k}={\widehat{q}}_{O_i}^{O_k}\otimes {\widehat{q}}_{O_{i+1}}^{O_i}={\widehat{q}}_{O_i}^{O_k}\otimes \widehat{q}\{R_B^O({\widehat{\omega }}_{avgi+1}-b_g)\Delta t\}\hfill \\ {\widehat{b}}_{gi+1}={\widehat{b}}_{gi}\hfill \end{array}$$

(15)

The initial value of nominal weight: ${\widehat{p}}_0=0, {\widehat{q}}_0={\left[1 0 0 0\right]}^T$.

According to the definition of the error amount of the pre-integration term, an incremental update formula of the error amount of the pre-integration term is:

$$\{\begin{array}{c}\delta p_{O_{i+1}}^{O_k}=\delta p_{O_i}^{O_k}+R\{q_{O_i}^{O_k}\}{D}_{O_{k+1}}^{O_k}{\eta }_{os}\hfill \\ \delta {\theta }_{O_{i+1}}^{O_k}=\delta {\theta }_{O_i}^{O_k}-({\widehat{\omega }}_{avgi+1}-b_g)^\delta {\theta }_{O_i}^{O_k}\Delta t-\delta b_g\Delta t+{\eta }_g\Delta t\}\hfill \\ \delta b_{gi+1}=\delta b_{gi}+{\eta }_{b_g}\mathsf{\Delta }t\hfill \end{array}$$

(16)

${(·)}^{^}$ is $3\times 3$ anti-symmetric matrix of Lie algebra SO (3). The nominal value of the wheel odometer pre-integration term depends on the pre-fusion wheel odometer measurement and the gyroscope zero offset. For the variable to be optimized, the zero bias of the gyroscope needs to be continuously adjusted in the pose measurement process to reduce the residual error. Therefore, in the optimization process, the partial derivatives $J_{b_g}^p$ and $J_{b_g}^{\theta }$ of the nominal value of the pre-integral term of the wheel odometer with respect to the zero offset of the gyroscope need to be used.

According to the incremental update of the error value of the pre-integration term of the wheel odometer, the Jacobian matrix of the error value between the two frames before and after can be obtained as:

$$J_{\delta x_i}^{\delta x_{i+1}}=\frac{\delta x_{i+1}}{\delta x_i}=I+F_i$$

(17)

According to the definition of the nominal value of the pre-integration item of the wheel odometer, the Jacobian matrix of the error value is the nominal Jacobian matrix of $J_{{\widehat{x}}_i}^{{\widehat{x}}_{i+1}}=J_{\delta x_i}^{\delta x_{i+1}}$. Therefore, according to the chain-derivation rule, $J_{{\widehat{x}}_0}^{{\widehat{x}}_{i+1}}=J_{{\widehat{x}}_i}^{{\widehat{x}}_{i+1}}{J}_{{\widehat{x}}_0}^{{\widehat{x}}_i}$, the update equation of the nominal value of the pre-integration term of the wheel odometer with respect to the zero-biased Jacobian matrix is:

$$J_{{\widehat{x}}_0}^{{\widehat{x}}_{i+1}}=(I+F_i)J_{{\widehat{x}}_0}^{{\widehat{x}}_i}$$

(18)

The initial value of the Jacobian matrix: $J_0=J_{{\widehat{x}}_0}^{{\widehat{x}}_0}=diag(9)$.

2.6.3. Residual Term

Definition: In the least-squares problem of robot pose measurement, the wheel odometer residual term $r_{\mathcal{O}}$ represents the error distance between the frame-to-frame relative displacement ${\widehat{p}}_{O_{k+1}}^{O_k}$ and the key frame displacement $p_{O_{k+1}}^{O_k}$ in the variable to be optimized, where ${\widehat{p}}_{O_{k+1}}^{O_k}$ is the observation, and $p_{O_{k+1}}^{O_k}$ is the estimator.

$$r_{\mathcal{O}}({\{\Delta {\widehat{m}}_{odomt},{\widehat{\omega }}_{avgt}\}}_{t_k\le t\le t_{k+1}},x_k,x_{k+1})=\left[\delta p_{O_{k+1}}^{O_k}\right]=p_{O_{k+1}}^{O_k}-{\widehat{p}}_{O_{k+1}}^{O_k}$$

(19)

The wheel odometer residual does not include the errors $\delta {\theta }_{O_{k+1}}^{O_k}$ and $\delta b_g{}_{O_{k+1}}^{O_k}$ with respect to the rotation and gyro zero offset. This is as these terms are already defined in the residual term of the IMU pre-integration. The IMU pre-integration uses the original IMU measurement as the angular velocity input, which provides higher rotational integration accuracy than the wheel odometer pre-integration measured with a lower frequency pre-fused wheel odometer.

To use the variable x_k, x_k+1 to represent the wheel odometer pre-integration residual term, $p_{O_{k+1}}^{O_k}$ needs to be transformed:

$$\begin{array}{cc}\hfill p_{O_{k+1}}^{O_k}& =R_W^{O_k}(p_{O_{k+1}}^W-p_{O_k}^W)\hfill \\ & =R_O^{B-1}R\{q_{B_k}^{W-1}\}(p_{B_{k+1}}^W-p_{B_k}^W)+R_O^{B-1}R\{q_{B_k}^{W-1}\}R\{q_{B_{k+1}}^W\}{p}_O^B-R_O^{B-1}{p}_O^B\hfill \end{array}$$

(20)

We obtain the residual term expressed using only the variables to be optimized and the wheel odometer pre-integration:

$$r_{\mathcal{O}}=R_O^{B-1}R\{q_{B_k}^{W-1}\}(p_{B_{k+1}}^W-p_{B_k}^W)+R_O^{B-1}R\{q_{B_k}^{W-1}\}R\{q_{B_{k+1}}^W\}{p}_O^B-R_O^{B-1}{p}_O^B-{\widehat{p}}_{O_{k+1}}^{O_k}$$

(21)

Here, $R_O^B$ and $p_O^B$ are known constants.

As a maximum posterior problem, the robot pose measurement is the same as least-squares by introducing a covariance matrix of the residuals to transform the residuals with dimensions into a unified probability representation. The wheeled odometer residual $r_{\mathcal{O}}$ obeys the covariance matrix ${\mathsf{\Sigma }}_{\mathcal{O}}$ of the wheeled odometer pre-integration, $r_{\mathcal{O}}~N(0, {\left[{\mathsf{\Sigma }}_{\mathcal{O}}\right]}_p)$. Here, ${\left[{\mathsf{\Sigma }}_{\mathcal{O}}\right]}_p$ represents the displacement covariance in the wheel odometer pre-integration covariance matrix ${\mathsf{\Sigma }}_{\mathcal{O}}$, ${\left[{\mathsf{\Sigma }}_{\mathcal{O}}\right]}_p={\left[{\mathsf{\Sigma }}_{\mathcal{O}}\right]}_{left right 3\times 3}$.

Regarding the Jacobian matrix, according to the definition of the wheel odometer residual $r_{\mathcal{O}}$, in the optimization process, the residual value $r_{\mathcal{O}}$ will change with the adjustment of the previous key frame poses $p_{B_k}^W$ and $q_{B_k}^W$, and the poses $p_{B_{k+1}}^W$ and $q_{B_{k+1}}^W$ of the next key frame, and the gyroscope zero offset $b_{gi}$ of the previous frame. To provide the necessary gradient direction for optimization, the system needs to be linearized in the current state $x_k,x_{k+1}$ and the ratio between the increment of the residual $\partial r_{\mathcal{O}}$ and the increment of the variable to be optimized is calculated. Thus, the Jacobian matrix $J_{x_k,x_{k+1}}^{r_{\mathcal{O}}}$ is defined:

$$\partial r_{\mathcal{O}}=J_{x_k,x_{k+1}}^{r_{\mathcal{O}}}\left[\begin{array}{c}\partial x_k\\ \partial x_{k+1}\end{array}\right]$$

(22)

$$\left[\delta p_{O_{k+1}}^{O_k}\right]=J_{x_k,x_{k+1}}^{r_{\mathcal{O}}}\left[\begin{array}{c}\delta p_{B_k}^W\\ \delta v_{B_k}^W\\ \begin{array}{c}\delta {\theta }_{B_k}^W\\ \delta b_{a{B}_k}\\ \begin{array}{c}\delta b_{g{B}_k}\\ \delta p_{B_k}^W\\ \begin{array}{c}\delta v_{B_k}^W\\ \delta {\theta }_{B_k}^W\\ \begin{array}{c}\delta b_{a{B}_k}\\ \delta b_{g{B}_k}\end{array}\end{array}\end{array}\end{array}\end{array}\right]$$

(23)

Here, $J_{x_k,x_{k+1}}^{r_{\mathcal{O}}}\in R^{3\times 30}$. As the increment is small, using the quaternion definition will produce additional degrees of freedom. The increment for the rotation state in the formula is defined as the shaft angle representation.

As the wheel odometer residual is only related to some variables in the previous key frame state $x_k$ and next key frame state $x_{k+1}$, the value of the Jacobian matrix $J_{x_k,x_{k+1}}^{r_{\mathcal{O}}}$ is:

$$J_{x_k,x_{k+1}}^{r_{\mathcal{O}}}=\left[\frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta p_{B_k}^W} 0 \frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta {\theta }_{B_k}^W} 0 \frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta b_{gi}} \frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta p_{B_{k+1}}^W} 0 \frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta {\theta }_{B_{k+1}}^W} 0 0\right]$$

(24)

$$\begin{array}{c}\frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta p_{B_k}^W}=-R_O^{B-1}R\{q_{B_k}^{W-1}\}\hfill \\ \frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta {\theta }_{B_k}^W}=-R_O^{B-1}\left[R\{q_{B_k}^{W-1}\}(p_{B_{k+1}}^W-p_{B_k}^W+R\{q_{B_{k+1}}^W\}{p}_O^B)\right]^\hfill \\ \frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta b_{gi}}=-J_{b_g}^p\hfill \\ \frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta p_{B_{k+1}}^W}=R_O^{B-1}R\{q_{B_k}^{W-1}\}\hfill \\ \frac{\partial \delta p_{O_{k+1}}^{O_k}}{\partial \delta {\theta }_{B_{k+1}}^W}=-R_O^{B-1}R\{q_{B_k}^{W-1}\}R\{q_{B_{k+1}}^W\}(p_O^B)^\hfill \end{array}$$

(25)

2.7. Marginalization and Prior Constraints

The state of the key frame and its related observations are constantly removed from the optimization equation. If all observations related to the removed key frames are directly discarded, the constraints of state estimation will be reduced, and the loss of valid information will lead to a decrease in accuracy. Here, a marginalization algorithm is used, while removing the key frames, retaining the removed observations to constrain the optimization variables. According to [6], the use of the Gauss–Newton method can be understood as adding an increment to the variable to be optimized; the objective function is the smallest.

If the residual function $r(x)$ is linearized at x, and the Jacobian matrix $J_x^r$ is obtained, the nonlinear least-squares problem becomes a linear least-squares problem:

$$\underset{\delta x}{\min }{\Vert r(x+\delta x)\Vert }^2⟹\underset{\delta x}{\min }{\Vert r(x)+J_x^r\delta x\Vert }^2$$

(26)

Here, ${\Vert r(x)+J_x^r\delta x\Vert }^2={\left[r(x)+J_x^r\delta x\right]}^T\left[r(x)+J_x^r\delta x\right]$. Taking the derivative of this formula with respect to $\delta x$ be 0, we obtain:

$$J_x^r{}^T{J}_x^r\delta x=-J_x^r{}^{T}r(x)$$

(27)

Let $\mathrm{H}=J_x^r{}^T{J}_x^r, b=-J_x^r{}^{T}r(x)$; thus, we obtain the incremental equation $\mathrm{H}\delta \mathrm{x}=\mathrm{b}$, where H is called the Hessian matrix.

Divide the variable x to be optimized into the part $x_a$, which needs to be removed, and the part $x_b$, $\delta \mathrm{x}={\left[{\delta \mathrm{x}}_a{\delta \mathrm{x}}_b\right]}^T$, which needs to be retained, then the incremental equation becomes:

$$\left[\begin{array}{cc}{H}_a& H_b\\ H_b{}^T& H_c\end{array}\right]\left[\begin{array}{c}\delta x_a\\ \delta x_b\end{array}\right]=\left[\begin{array}{c}{b}_a\\ b_b\end{array}\right]$$

(28)

The Schur method is used to eliminate the element to obtain the solution of $\delta x_b$:

$$\left[\begin{array}{cc}I& 0\\ -H_b^T{H}_a^{-1}& I\end{array}\right]\left[\begin{array}{cc}{H}_a& H_b\\ H_b{}^T& H_c\end{array}\right]\left[\begin{array}{c}\delta x_a\\ \delta x_b\end{array}\right]=\left[\begin{array}{cc}I& 0\\ -H_b^T{H}_a^{-1}& I\end{array}\right]\left[\begin{array}{c}{b}_a\\ b_b\end{array}\right]$$

(29)

$$\left[\begin{array}{cc}{H}_a& H_b\\ 0& H_c-H_b^T{H}_a^{-1}{H}_b\end{array}\right]\left[\begin{array}{c}\delta x_a\\ \delta x_b\end{array}\right]=\left[\begin{array}{c}{b}_a\\ b_b-H_b^T{H}_a^{-1}{b}_a\end{array}\right]$$

(30)

Intercepting the second row of the above matrix, we obtain:

$$(H_c-H_b^T{H}_a^{-1}{H}_b)\delta x_b=b_b-H_b^T{H}_a^{-1}{b}_a$$

(31)

In the above formula, only $x_b$ is unknown, and no information in H and b is lost. This process removes the rows and columns related to $x_a$ from the incremental equation, marginalizes the state $x_a$ that needs to be removed, and retains the historical observation constraints on the state $x_b$. When the next image frame arrives, the prior information in the above Formula (31) will be used as a prior constraint term to construct a nonlinear least-squares problem.

2.8. State Initialization

VINS-Mono uses multiple steps to initialize the state: gyroscope zero offset correction, initializing gravity, speed, and scale coefficients, and modifying the direction of gravity. The disadvantage of this method is that it depends on sufficient visual measurement of parallax and sufficient acceleration excitation. When there is no abnormal situation, such as skidding, the wheel odometer has better accuracy and reliability over a short distance and a short duration. Compared with monocular vision, there is no scale uncertainty, and it is easier to initialize the keyframe pose, velocity, and gravity directions.

2.8.1. Gyro Zero Offset Initialization

As the gyroscope and wheel odometer measurements are on the same rigid body, the rotations of the two are the same. The relative rotation between the two key frames can be obtained through IMU pre-integration and wheel odometer pre-integration, respectively: $q_{B_{k+1}}^{B_k}$ and $q_{O_{k+1}}^{O_k}$. The rotation term of the pre-integration of the wheeled odometer above is also obtained through the gyro integration and has no reference value. Therefore, during the initialization process, the gyroscope pre-integration will use the heading angle for rotation integration. Rotation term $q_{B_{k+1}}^{B_k}$ is a function of the gyroscope bias $q_{B_{k+1}}^{B_k}$. If the error between $q_{B_{k+1}}^{B_k}$ and $q_{O_{k+1}}^{O_k}$ is used as a constraint, the gyroscope bias $b_g$ can be estimated. Assuming that the gyro bias $b_{gk}$ of each key frame during the initialization process is the same $b_{gk}=b_g$, the construction of the least-squares problem is as follows:

$$b_g{}^{\ast }={\displaystyle \underset{b_g}{\mathrm{argmin}}}{\displaystyle {\displaystyle \sum }_{i=1}^{k-1}}{\Vert q_O^B\otimes {\widehat{q}}_{O_{i+1}}^{O_i}{}^{-1}\otimes q_B^O\otimes q_{B_{k+1}}^{B_k}\Vert }^2$$

(32)

Linearizing the rotation transform at ${\widehat{q}}_{B_{k+1}}^{B_k}$, we obtain:

$$q_{B_{k+1}}^{B_k}={\widehat{q}}_{B_{k+1}}^{B_k}\otimes q\{J_{b_g}^q{b}_g\}\approx {\widehat{q}}_{B_{k+1}}^{B_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_g}^q{b}_g\end{array}\right]$$

(33)

Here, $J_{b_g}^q$ is the partial derivative of the inter-frame rotation $q_{B_{k+1}}^{B_k}$. $b_g$ is the gyroscope zero bias. The objective function of the least-squares problem is written as:

$$\begin{array}{cc}\hfill q_O^B\otimes {\widehat{q}}_{O_{i+1}}^{O_i}{}^{-1}\otimes q_B^O\otimes q_{B_{k+1}}^{B_k}& =\left[\begin{array}{c}1\\ 0\end{array}\right]\hfill \\ \hfill {\widehat{q}}_{B_{k+1}}^{B_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_g}^q{b}_g\end{array}\right]& =q_O^B\otimes {\widehat{q}}_{O_{i+1}}^{O_i}\otimes q_B^O\hfill \\ \hfill \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_g}^q{b}_g\end{array}\right]& ={\widehat{q}}_{B_{k+1}}^{B_k}{}^{-1}\otimes q_O^B\otimes {\widehat{q}}_{O_{i+1}}^{O_i}\otimes q_B^O\hfill \end{array}$$

(34)

Considering only the imaginary part of the quaternion, we obtain:

$$J_{b_g}^q{b}_g=2{\{{\widehat{q}}_{B_{k+1}}^{B_k}{}^{-1}\otimes q_O^B\otimes {\widehat{q}}_{O_{i+1}}^{O_i}\otimes q_B^O\}}_{xyz}$$

(35)

The above Formula (35) conforms to the format of $\mathrm{Hx}=\mathrm{b}$, and the Cholesky decomposition can be used to find the least-squares solution:

$$J_{b_g}^q{}^T{J}_{b_g}^q{b}_g=2J_{b_g}^q{}^T{\{{\widehat{q}}_{B_{k+1}}^{B_k}{}^{-1}\otimes q_O^B\otimes {\widehat{q}}_{O_{i+1}}^{O_i}\otimes q_B^O\}}_{xyz}$$

(36)

If the robot rotates rapidly during the gyro work offset initialization process, the elasticity of the wheel, the rigid connection between the wheel and the IMU, the misalignment of the wheel odometer clock and the IMU clock, and a calibration error of the wheel odometer rotation scale factor may lead to poor gyro work offset initialization results.

2.8.2. Initialization of Key Frame Speed and Gravity

As the Mecanum wheel will tremble during movement, and the wheeled odometer algorithm can only obtain the heading angle information, it is difficult to obtain accurate relative rotation between key frames through wheeled odometer integration. In the previous step, the zero offset of the gyroscope has been initialized, and the relative rotation between all key frames can be obtained through IMU pre-integration. As the rotation is known, the key frame speed and gravity can be calculated by solving linear equations. Decomposing the position term ${\alpha }_{B_{k+1}}^{B_k}$ and speed term ${\beta }_{B_{k+1}}^{B_k}$ in the IMU pre-integration and transforming it into the form of matrix multiplication ${\widehat{z}}_{B_{k+1}}^{B_k}=H_{B_{k+1}}^{B_k}{x}_{B_{k+1}}^{B_k}$, we obtain:

$$\left[\begin{array}{c}{\alpha }_{B_{k+1}}^{B_k}-p_O^B+R_{B_{k+1}}^{B_k}{p}_O^B\\ {\beta }_{B_{k+1}}^{B_k}\end{array}\right]=\left[\begin{array}{c}\begin{array}{ccc}-I\Delta t& 0& \begin{array}{cc}\frac{1}{2}{R}_{B_0}^{B_k}\Delta t^2& R_O^B{\widehat{p}}_{O_{k+1}}^{O_k}\end{array}\end{array}\\ \begin{array}{ccc}-I& R_{B_{k+1}}^{B_k}& \begin{array}{cc}{R}_{B_0}^{B_k}\Delta t& 0\end{array}\end{array} \end{array}\right]\left[\begin{array}{c}{v}_{B_k}^{B_k}\\ \begin{array}{c}{v}_{B_{k+1}}^{B_{k+1}}\\ g^{B_0}\\ s\end{array}\end{array}\right]$$

(37)

Here, ${\widehat{z}}_{B_{k+1}}^{B_k}$ is the IMU pre-integration measurement between the key frames $B_k$ and $B_{k+1}$. The variable to be estimated related to the key frames $B_k$ and $B_{k+1}$ is defined as $x_{B_{k+1}}^{B_k}$, and $H_{B_{k+1}}^{B_k}$ represents the constraint between the measurement ${\widehat{z}}_{B_{k+1}}^{B_k}$ and the $x_{B_{k+1}}^{B_k}$. The s in the $x_{B_{k+1}}^{B_k}$ represents the distance of the wheel odometer with respect to the actual distance, that is, the X-axis and Y-axis scale factors of the wheel odometer $S_x,S_y$. If the IMU excitation is sufficient, it can be used to calibrate the scale factor. $\mathrm{S}=1$ is defined here for the reliability of initialization.

To reduce initialization errors and improve reliability, multiple key frame measurements need to be used as constraints to calculate the key frame speed and gravity direction. By combining the multiple linear equations above, we can obtain the least-squares problem:

$$x^{\ast }={\displaystyle \underset{x}{\mathrm{argmin}}}{\displaystyle {\displaystyle \sum }_{k\in Frames}}{\Vert {\widehat{z}}_{B_{k+1}}^{B_k}-H_{B_{k+1}}^{B_k}{x}_{B_{k+1}}^{B_k}\Vert }^2,x=\left[\begin{array}{c}{v}_{B_0}^{B_0}\\ v_{B_1}^{B_1}\\ \begin{array}{c}⋮\\ v_{B_k}^{B_k}\\ \begin{array}{c}{g}^{B_0}\\ s\end{array}\end{array}\end{array}\right]$$

(38)

Here, x is the variable to be estimated, and $x^{\ast }$ is the optimal estimated value of x. The program uses Cholesky decomposition to solve the least-squares problem:

$${\mathrm{H}}^{T}Hx=H^T\widehat{z}$$

(39)

In the formula, the matrix H is obtained by inserting all $H_{B_{k+1}}^{B_k}$ into empty columns at the corresponding positions of the unrelated variables and summing them, and $\widehat{z}$ is obtained by combining all ${\widehat{z}}_{B_{k+1}}^{B_k}$.

3. Experiments

In this paper, a mobile robot platform is built, using a dual-processor architecture. The high-performance PC upper computer is used for data processing of each sensor and the SLAM algorithm operation, and the embedded controller lower computer is used for the motion control of the mobile robot chassis. The parameters of the high-performance PC are Intel Core i7-9750H CPU @ 2.6 GHz, 16 GB RAM. The camera is IntelRealSense ZR300.

3.1. Accuracy Verification Experiment

3.1.1. Room-Scale Pose Measurement Experiment

Experimental conditions: In a laboratory where objects are placed in a complex environment, as shown in Figure 2, the control robot walked through all the channels. The channel width is narrow, and the width at the narrowest point is less than 1 m; the movement speed was maintained at approximately 0.5 m/s; a large number of fixed marking points are arranged inside the room to obtain the real position of the robot in order to analyze the positioning error of the robot. Abnormal conditions during the experiment included:

3.

magnetic guide bars with a height of approximately 0.5 cm were fixed on the ground, and the wheels slipped slightly as they passed;

4.

due to turning too close to the weakly textured wall surface, the visual tracking was completely lost several times.

The RViz interface at the end of the test data playback is shown in Figure 3, which includes the raw picture, feature points, matched feature points, and the path of pose measurement.

The path diagrams of pose measurement, as shown in Figure 4, start along the positive X-axis. Therefore, the distance between the start point and end point shows the experimental accuracy. Throughout the experiment, the car traveled for 184.3 s, the average speed was 0.264 m/s, the maximum speed was 0.591 m/s, the cumulative translation was 51.321 m, the cumulative rotation was 3428.318°, and the chassis had no abnormalities. As this experiment was performed in an actual scene, the experimental system did not have the equipment to measure ground truth; therefore, it could only measure the distance error of the car passing several fixed road marking points. In each experiment, the robot was controlled to accurately pass the road marking point and keep the rotation direction consistent. Therefore, the pose error in

Table 1. Room-scale pose measurement results.

Algorithm	∆X (m)	∆Y (m)	∆P (m)	∆P (%)	∆A (°)
wheel	−0.785	−0.389	0.87	1.71	1.907
wheel inertial	−0.905	−0.040	0.906	1.77	−0.507
VINS-Mono	0.851	−0.220	0.879	1.71	−0.574
VINS-Mono (loop)	0.009	0.020	0.022	0.04	−0.530
ours	0.148	−0.143	0.206	0.40	−0.213
ours (loop)	−0.003	0.015	0.015	0.03	−0.443

is average position error and yaw angle error among several road marking points. When the robot passes through each road marking point, a timestamp is marked. Calculate the coordinate error (∆X and ∆Y), position error (∆P(m), absolute value), and yaw angle error (∆A) between the algorithm trajectory pose and the road marking point when it is at the time stamp. For each algorithm, take the average of the errors generated by all road marking points as the final experimental result. The position error rate (∆P(%)) is the ratio of the position error to the cumulative translation. The table shows our algorithm performed best, with and without closed loops. The position error was 0.206 m and 0.015 m, respectively. As shown in Figure 4, we can clearly see the error of the different methods.

As listed in Table 1, we compared the error of the X-axis, Y-axis, position, and heading angle. According to the data presented in Table 1, the accuracy of posture estimation using the monocular vision inertial wheel odometer algorithm was better than that of the VINS-Mono algorithm, and the accumulated position error was only approximately 0.2 m after 51 m. We divided the cumulative position error by the cumulative translation distance to obtain the cumulative position error rate. The position error rate of the proposed algorithm was only 0.4%, which was lower than that of VINS-Mono (1%). This experiment verified that a multi-sensor fusion odometer algorithm could perform high-precision positioning in a room-scale indoor environment, and the result was better than that of the monocular and wheel separately. By further combining complete SLAM system, ours almost have no bias.

3.1.2. Floor Scale Experiment

Experimental conditions included:

①

On the first floor of the building where the laboratory was located, the floor area was approximately 250 × 100 m;

②

The robot ran in the central hall with 1 m/s (the area of the hall was approximately 15 × 15 m);

③

A large number of fixed marking points are arranged inside the corridor to obtain the real position of the robot in order to analyze the positioning error of the robot.

Anomalies during the experiment included:

①

There was a considerable amount of dust on the ground, which decreased the friction between the wheels, leading to a slight slip during rapid turns and a significant slip during left-to-right translation;

②

Due to the fast movement of the robot, the picture of the rolling shutter camera continued to exhibit disturbances;

③

There were cable manhole covers in numerous places in the corridor, the ground surface was uneven, and there were 2–3 cm step-like undulations. The robot vibrated significantly while passing over these obstacles;

④

The corridor contained semi-open areas and closed areas, the environment brightness changed significantly, and some areas were almost completely dark;

⑤

The walls around the robot in some areas were covered with tiles and had reflections;

⑥

The corridor and hall scenes had a high degree of similarity, lacking special landmarks. Loop detection was successfully performed only when the robot passed the hall halfway and finally returned to the hall;

⑦

During the experiment, pedestrians appeared several times.

The RViz interface at the end of test data playback is shown in Figure 5, which includes the raw picture, feature points, matched feature points, and the path.

Throughout the experiment, the car traveled for 889.2 s, the average speed was 0.87 m/s, the maximum speed was 1.36 m/s, the cumulative translation was 809.27 m, the cumulative rotation was 15,502.1°, and the chassis abnormal time was 26.983 s. As shown in Figure 6 and

Table 2. Floor-scale pose measurements results.

Algorithm	∆X (m)	∆Y (m)	∆P (m)	∆P (%)	∆A (°)
wheel	−83.768	0.295	83.768	10.31	32.868
wheel inertial	−52.108	−0.826	52.115	6.42	6.404
VINS-Mono	10.692	−31.422	33.191	4.09	4.080
VINS-Mono (loop)	0.138	−5.281	5.281	0.65	3.086
ours	−2.025	0.947	2.235	0.28	3.661
ours (loop)	−0.295	0.165	0.338	0.04	3.172

, the uneven ground made the wheel slip, so the error was very high. As shown in the floor-scale scenario, both the VINS-Mono algorithm with loop detection disabled and ours performed better; however, there was still a cumulative positioning error that could not be neglected. Compared with the complete SLAM system, VINS-Mono and ours both significantly improved the accuracy. Additionally, ours was better than that of VINS-Mono.

3.2. Robustness Verification Experiment

This time, we created experiments with sensor measurement errors or even loss. Experimental conditions included intentionally blocking the camera, resulting in the visual signal being lost for approximately 15 s. This was undertaken in the laboratory, during the movement of the robot, during which the robot kept moving and turning.

Throughout the experiment, the car traveled for 104.4 s, the average speed was 0.142 m/s, the maximum speed was 0.803 m/s, the cumulative translation was 17.809 m, the cumulative rotation was 1110.32°, and the chassis had no abnormalities. In the process of visual measurement loss, upon which the VINS-Mono is based, the state estimator was downgraded to inertial navigation dead reckoning. The error increased rapidly, thereby reducing the final positioning accuracy. As shown in Figure 7 and

Table 3. Pose measurement results when visual tracking was lost.

Algorithm	∆X (m)	∆Y (m)	∆P (m)	∆P (%)	∆A (°)
wheel	−0.042	0.106	0.114	0.64	−1.078
wheel inertial	−0.137	−0.014	0.137	0.77	−2.811
VINS-Mono	−0.442	0.890	0.994	5.58	−2.057
VINS-Mono (loop)	−0.030	0.015	0.025	0.19	−1.599
ours	0.295	0.011	0.295	1.66	−2.883
ours (loop)	0.020	−0.014	0.023	0.14	−1.718

during the loss of visual features, the path of the SLAM algorithm was the same as that measured by the wheel odometer. This shows that, although the positioning accuracy was affected by the loss of visual measurement, the pre-integration constraint can still provide absolute speed measurement; thus, the final positioning error was less than that of VINS-Mono.

4. Discussion

Aiming to solve the problems of low accuracy and poor robustness of the visual inertial SLAM algorithm, we designed and implemented a tightly coupled monocular visual inertial pose estimation algorithm that integrates wheel speed information. The state estimator, based on tightly coupled multi-sensor information, is used as the core of the algorithm, and the wheel odometer pre-integration that integrates wheel speed and IMU angular velocity avoids repeated integration in the iterative optimization process. The pose of the robot in the sliding window and the visual feature points are the state to be estimated. Then, the state is initialized based on the wheel odometer and IMU, so that the initial value of the state can be calculated quickly and reliably in both stationary and moving states. Finally, the residual constraint is added to the state estimator, and the optimal robot pose is solved through nonlinear optimization. Compared with the existing SLAM algorithm for many experiments, the SLAM algorithm in this paper can achieve higher pose accuracy and robustness in environments with more extreme situations. Experimental comparison results show that the SLAM algorithm proposed in this paper is feasible and effective.

In future research, our algorithm can be improved in the following ways. First, a motion capture system or D-GPS can be employed to obtain ground truth for quantitative pose comparison analysis. Second multiple monocular cameras with non-common view relationships can be used, which can improve robustness. When some cameras are blocked, the other cameras can still provide reliable visual measurement. Further, based on the robust robot pose measurement, using binocular or depth cameras to build a dense map of the environment can meet the requirements of robot navigation and obstacle avoidance.