W-VSLAM: A Visual Mapping Algorithm for Indoor Inspection Robots

Abstract

In recent years, with the widespread application of indoor inspection robots, high-precision, robust environmental perception has become essential for robotic mapping. Addressing the issues of visual–inertial estimation inaccuracies due to redundant pose degrees of freedom and accelerometer drift during the planar motion of mobile robots in indoor environments, we propose a visual SLAM perception method that integrates wheel odometry information. First, the robot’s body pose is parameterized in SE(2) and the corresponding camera pose is parameterized in SE(3). On this basis, we derive the visual constraint residuals and their Jacobian matrices for reprojection observations using the camera projection model. We employ the concept of pre-integration to derive pose-constraint residuals and their Jacobian matrices and utilize marginalization theory to derive the relative pose residuals and their Jacobians for loop closure constraints. This approach solves the nonlinear optimization problem to obtain the optimal pose and landmark points of the ground-moving robot. A comparison with the ORBSLAM3 algorithm reveals that, in the recorded indoor environment datasets, the proposed algorithm demonstrates significantly higher perception accuracy, with root mean square error (RMSE) improvements of 89.2% in translation and 98.5% in rotation for absolute trajectory error (ATE). The overall trajectory localization accuracy ranges between 5 and 17 cm, validating the effectiveness of the proposed algorithm. These findings can be applied to preliminary mapping for the autonomous navigation of indoor mobile robots and serve as a basis for path planning based on the mapping results.

1. Introduction

In recent years, the rapid development of social productivity has led to a significant increase in the demand for security, especially with the continuous expansion of urban areas, which poses greater challenges for building and community security [1]. Additionally, there has been a notable rise in the need for round-the-clock patrol and surveillance in fields such as industrial parks, power facilities, and chemical enterprises. However, the security industry is facing challenges such as an aging workforce and rising labor costs, leading to an urgent need for efficient security robots to meet the demands of environmental perception in complex settings. In this context, visual SLAM technology, a multi-sensor fusion-based localization framework, has gained significant attention [2]. Its application in wheeled robots is particularly notable, as it enables autonomous perception in indoor environments lacking GPS signals, playing a critical role in the development of robot localization and mapping technology.

The most commonly used sensors for SLAM localization are laser and vision sensors. Laser SLAM technology is relatively mature, primarily based on TOF (time-of-flight) LiDAR. For research in laser SLAM, He et al. [3] successfully proposed a laser-inertial pose-estimation and map-construction method using B-spline curves, achieving robust registration steps. In the context of cooperative localization between drones and unmanned vehicles, Ryoga et al. [4] successfully achieved cooperative localization estimation by integrating drone point estimation results with the observation likelihood of unmanned vehicles. Bo et al. [5] combined the IESKF (Iterated Extended Kalman Filter) and factor graphs to propose a more precise local pose estimation method. By filtering LiDAR and IMU data with IESKF, they integrated the laser-inertial odometry with a back-end factor graph, offering new insights into the development of SLAM technology. To address challenges in different scenarios, researchers have proposed SLAM systems combining multi-line LiDAR and visual perception [6,7,8,9], offering diverse solutions for different environments. In the field of firefighting robots, a mapping system based on Cartographer SLAM theory provides autonomous patrol and automatic fire extinguishing functions in unknown indoor environments [10]. However, despite its notable advantages in meeting real-time demands, laser SLAM’s loop closure detection strategies can still present issues with stability, particularly in geometrically symmetric environments. Overall, laser SLAM ensures real-time performance, but its loop closure detection strategies can lag behind, especially in some geometrically symmetric environments where they can lead to unstable loop closures.

The VINS (Visual–Inertial Navigation System) [11] for wheeled robots integrates low-frequency wheel encoder data to enable scale observation. Zhang et al. [12] designed a novel visual-based localization algorithm by combining camera, IMU, and wheel odometer data, providing more comprehensive information for ground robots. Liu et al. [13] tightly fused camera, IMU, and wheel encoder data, proposing a new extended visual–inertial odometry calculation method that significantly improves localization accuracy. In the field of ground vehicles, Lee et al. [14] effectively fused multi-modal visual, inertial, and 2D wheel odometry measurements in a visual–inertial-wheel odometry system using a sliding window filter approach, providing comprehensive information support for ground vehicles. Zheng et al. [15] improved visual estimation accuracy by directly parameterizing the pose of ground vehicles on SE(2) through an efficient pre-integration algorithm based on SE(2). Carlos et al. [16] developed a tightly integrated visual–inertial SLAM system, ORB-SLAM3, that is capable of performing visual, visual–inertial, and multi-map SLAM using monocular, binocular, and RGB-D cameras. Researchers have proposed various innovative methods across different scenarios, such as the planar PnP algorithm by Li J. et al. [17] and Zou’s analysis and comparison of indoor navigation methods based on LiDAR SLAM. Zhu et al. [18] introduced a stereo visual–inertial fusion system based on VINS-Mono that closely integrates visual and inertial measurements. Additionally, Li J. et al. [19] proposed the Attention-SLAM method that combines a visual saliency model with traditional monocular visual SLAM to simulate human navigation patterns. In summary, pure visual SLAM can suffer from unobservable degrees of freedom under certain conditions, leading to estimation errors, particularly for wheeled robots during straight-line motion or constant local acceleration motion. In such cases, the IMU may not provide sufficient information.

In response to the planar movement of wheeled mobile robots in indoor environments, this paper proposes a multi-sensor visual SLAM perception method that integrates visual information with encoder-based wheel odometry information under the SE(2) framework. The high-precision relative pose observations provided by the wheel encoder speed data over short periods allow for accurate inter-frame estimations of visual keyframes. Additionally, the wheel odometry offers reliable pose estimations when visual signals fail (e.g., due to low texture or pure rotation). The integrated visual estimation information also helps to eliminate accumulated pose estimation errors from the wheel encoders, ultimately enabling accurate perception of the pose of the wheeled mobile robot.

2. Method Construction

The overall system framework is illustrated in Figure 1. The perception system for the wheeled mobile robot consists primarily of three threads: tracking, local optimization, and global optimization.

The tracking thread runs in real time at a frequency higher than the camera frame rate. Camera information and wheel speed information are soft-synchronized, with the first frame serving as the temporal origin, where the origin of the world coordinate system coincides with the origin of the robot coordinate system, making the first image frame the starting keyframe. First, the wheel speed information with a rate higher than the frame rate is pre-integrated to estimate the initial pose of the mobile robot. Each new image frame undergoes ORB feature extraction, and RANSAC [20,21] is used to remove outliers from feature matches. The thread matches the features in the current frame with the reference keyframe and estimates the initial map points using the pre-integrated information between the feature pairs and image frames. If the current frame meets the criteria for becoming a keyframe, it is added as a new reference keyframe to the local map.

The local optimization thread maintains the local covisibility graph that is generated during the robot’s movement, including keyframes and covisible map points with strong covisibility relationships. When a new keyframe is added to the local map, a search is conducted in the covisibility graph up to three levels deep to find keyframes with a covisibility relationship to the latest keyframe, which are considered local keyframes. Covisible map points associated with each local keyframe are treated as local map points. Local covisibility graph optimization is then performed using the nonlinear optimization library g2o [22] to update the poses of keyframes and the coordinates of map points. After optimization, map points with errors exceeding the threshold are considered outliers and are removed.

The global optimization thread maintains all keyframes and map points within the entire map. Keyframes that have undergone local optimization enter the global optimization thread, where similar keyframes are identified from the entire map using the “bag of words” approach [23] as loop closure candidate frames. Once loop closure is confirmed, the current keyframe, the loop closure keyframe, and their covisible map points form a subgraph. Optimization of the poses of the two keyframes and map points leads to an inter-frame loop closure constraint between the poses of the two frames. Visual constraints, wheel speed pre-integration constraints, and loop closure constraints are then incorporated into the global map to optimize the pose of the mobile robot and global map points, thereby eliminating accumulated errors caused by local optimization.

2.1. Coordinate System and Parameter Representation

As shown in Figure 2, the perception problem for wheeled mobile robots primarily involves three coordinate systems: the world coordinate system w, the robot coordinate system b, and the camera coordinate system c. The robot coordinate system can be any frame of reference fixed to the robot body, but in this context, it is defined as the odometry coordinate system. The origin, $O_1$, is located at the midpoint of the line connecting the wheel axes, with the positive x-axis pointing in the forward direction of the robot. The y-axis is aligned with the wheel axis line and positive to the left, while the positive z-axis points vertically upward from the origin. The camera coordinate system is centered at the camera’s optical center $O_2$, with the positive z-axis pointing outwards from the camera lens, and the x and y axes aligned with the u and v directions of the pixel plane, respectively.

The Lie groups SO(3) and SE(3) are used to represent rotation R and pose T in three-dimensional space, respectively:

$$SO\left(3\right)=\left\{R\in {\mathbb{R}}^{3\times 3}\left|R{R}^{\mathrm{T}}=\mathbf{I},det\left(\mathbf{R}\right)=1\right.\right\}$$

(1)

$$\mathrm{SE}\left(3\right)=\left\{T=\left[\begin{array}{cc}R& t\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\left|R\in \mathrm{SO}\left(3\right),t\in {\mathbb{R}}^3\right.\right\}$$

(2)

Since the Lie group space does not support addition, making it impossible to perform small iterations on optimized variables, Lie algebra $\xi ={\left[\varphi ,r\right]}^{\mathrm{T}}\in {\mathbb{R}}^6$ is introduced to enable rotations and poses to be mapped as follows:

$$R=exp\left({\varphi }^{\wedge }\right)$$

(3)

$$T=exp\left({\xi }^{\wedge }\right)$$

(4)

In Equation (3), the Lie algebra $\varphi \in {\mathbb{R}}^3$ of the rotation matrix corresponds to the rotation vector, which can also be obtained using the Rodrigues formula:

$$t=J_l\left(\varphi \right)r$$

(5)

The left Jacobian $J_l\left(\varphi \right)$ is expressed as follows:

$$J_l\left(\varphi \right)={\displaystyle \frac{sin\left|\varphi \right|}{\left|\varphi \right|}}I+\left({\displaystyle \frac{\left|\varphi \right|-sin\left|\varphi \right|}{{\left|\varphi \right|}^3}}\right)\varphi {\varphi }^{\mathrm{T}}+{\displaystyle \frac{1-cos\left|\varphi \right|}{{\left|\varphi \right|}^2}}{\varphi }^{\wedge }$$

(6)

Similarly, the pose on SE(2) corresponding to the three-dimensional vector $\phi ={\left[\nu ,\psi \right]}^{\mathrm{T}}$ can be expressed as follows:

$$\vartheta =\left[\begin{array}{cc}\mathsf{\Phi }& \nu \\ 0^{\mathrm{T}}& 1\end{array}\right]$$

(7)

where

$$\mathsf{\Phi }=exp{\left(\psi \right)}^{\wedge }=\left[\begin{array}{cc}cos\psi & -sin\psi \\ sin\psi & cos\psi \end{array}\right]$$

(8)

The correspondence between the Lie algebra se(3) and se(2) can be expressed as follows:

$$\mathrm{se}\left(3\right)\mapsto \mathrm{se}\left(2\right):\phi ={\left[r_x,r_y,{\varphi }_z\right]}^{\mathrm{T}}$$

(9)

$$\mathrm{se}\left(2\right)\mapsto \mathrm{se}\left(3\right):\xi ={\left[{\nu }_1,{\nu }_2,0,0,0,\psi \right]}^{\mathrm{T}}$$

(10)

In general, the transformation matrix $T_{\mathrm{AB}}$ represents the transformation from coordinate system B to coordinate system A. The matrix includes a rotation matrix $R_{\mathrm{AB}}$ and a translation vector $t_{\mathrm{AB}}$. The pose of a ground mobile robot is represented in SE(2) and denoted as $T_{\mathrm{wb}}$, while the pose of the camera is represented in SE(3) and denoted as $T_{\mathrm{cw}}$. The external calibration transformation matrix between the robot and camera coordinate systems is also represented in SE(3) and denoted as $T_{\mathrm{bc}}$. A 3D map point is expressed in the world coordinate system as $P_{\mathrm{w}}$, and the map point observed in the camera coordinate system is denoted as $P_{\mathrm{c}}$. The transformation relationship between these coordinates is expressed as follows:

$$P_{\mathrm{c}}=T_{\mathrm{cw}}{P}_{\mathrm{w}}$$

(11)

2.2. Visual SLAM Perception Algorithm with Wheel Speed Fusion

Since optimization-based SLAM back-end methods can iteratively achieve more accurate state estimates, the sensing problem of wheeled mobile robots moving on indoor planes is modeled here as a maximum a posteriori estimation problem (MAP). Given the initial pose $T_{\mathrm{w}{\mathrm{b}}_0}$ of the wheeled mobile robot, the camera intrinsic matrix K, and the external calibration matrix $T_{\mathrm{cb}}$, and considering a series of continuous input images $c_i$ (i = 1, 2, ⋯) and wheeled odometer speed information ${\varpi }_j$ (j = 1, 2, ⋯), the goal is to estimate a series of poses $T_{\mathrm{w}{\mathrm{b}}_i}$ (i = 1, 2, ⋯) of the wheeled mobile robot on SE(2) and the corresponding map points $P_{\mathrm{w}}^k$ (k = 1, 2, ⋯) throughout the observation process. Next, we construct visual observation constraints, wheeled pre-integration constraints, and loop closure observation constraints to form the final MAP problem.

2.3. Visual Observation Constraint

As shown in Figure 3, the camera uses a pinhole model to map the 3D coordinate point $P_{\mathrm{w}}$ in the world coordinate system to the 2D coordinate $P_{uv}(u,v)$ on the pixel plane.

In the camera coordinate system, the relationship between the 3D point $P_{\mathrm{c}}$ and the 2D point $P_{uv}$ on the pixel plane is as follows:

$$\left[\begin{array}{c}u\\ v\end{array}\right]={\displaystyle \frac{1}{z}}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\end{array}\right]P_{\mathrm{c}}$$

(12)

In the formula, z is the z-axis component of the 3D coordinate point. The parameters $f_x$ and $f_y$ represent the camera focal lengths along the x and y axes, respectively. The parameters $c_x$ and $c_y$ denote the coordinates of the image center point on the pixel plane.

This can be abbreviated as the projection function:

$$P_{uv}=\pi \left(P_{\mathrm{c}}\right)$$

(13)

Given that visual measurements may be affected by noise, under the assumption of a Gaussian model, the measurement equation can be expressed as follows:

$$P_{uv}=\pi \left(P_{\mathrm{c}}\right)+{\eta }_{uv}$$

(14)

where ${\eta }_{uv}\sim \mathcal{N}(0,{\sigma }_{uv}^2)$.

To facilitate the nonlinear optimization solution of the SLAM backend, we differentiate the previous equation with respect to the 3D point $P_{\mathrm{c}}$ in the camera coordinate system to obtain the Jacobian matrix of the pixel observation with respect to the camera coordinate point $P_{\mathrm{c}}$ [24]:

$$J_{\pi }\left(P_{\mathrm{c}}\right)={\displaystyle \frac{1}{z}}\left[\begin{array}{ccc}{f}_x& 0& -x{\displaystyle \frac{f_x}{z}}\\ 0& f_y& -y{\displaystyle \frac{f_y}{z}}\end{array}\right]$$

(15)

When the i-th keyframe observes a landmark point $P_{\mathrm{w}}^j$ and the camera extrinsic parameter rotation matrix $R_{\mathrm{cb}}$ and translation vector $t_{\mathrm{cb}}$ are known, the observation model in the camera coordinate system is given by

$$P_{uv}(T_{\mathrm{w}{\mathrm{b}}_i},P_{\mathrm{w}}^j)=\pi (R_{\mathrm{cb}}{R}_{\mathrm{w}{\mathrm{b}}_i}^{\mathrm{T}}(P_{\mathrm{w}}^j-P_{\mathrm{w}{\mathrm{b}}_i})+t_{\mathrm{cb}})+{\eta }_{uv}$$

(16)

In the equation, $R_{\mathrm{w}{\mathrm{b}}_i}$ and $P_{\mathrm{w}{\mathrm{b}}_i}$ represent the SE(3) expressions corresponding to the robot’s pose parameterized on SE(2), while $\pi (·)$ denotes the camera projection equation.

Unlike the unconstrained state of SE(3), a mobile robot undergoing planar motion is parameterized as a pose on SE(2). Given the minor vibrations that occur during the motion of the robot [15], perturbations in SE(3) are added to the SE(2) pose of the robot to account for the existing visual measurement noise. Assuming the translational perturbation experienced by the robot’s pose is ${\eta }_t\sim \mathcal{N}(0_{3\times 1},{\sigma }_t^2)$ and the rotational perturbation is ${\eta }_{\theta }\sim \mathcal{N}(0_{3\times 1},{\sigma }_{\theta }^2)$, the actual pose of the robot should be represented as

$$R_{\mathrm{w}{\mathrm{b}}_i}\leftarrow exp\left({\eta }_{\theta }\right)R_{\mathrm{w}{\mathrm{b}}_i}$$

(17)

$$P_{\mathrm{w}{\mathrm{b}}_i}\leftarrow P_{\mathrm{w}{\mathrm{b}}_i}+{\eta }_t$$

(18)

Therefore, the observation equation in Equation (16) is modified as follows:

$$P_{uv}(T_{\mathrm{w}{\mathrm{b}}_i},P_c^j)=\pi (R_{\mathrm{cb}}exp\left({\eta }_{\theta }\right)R_{\mathrm{w}{\mathrm{b}}_i}^{\mathrm{T}}(P_{\mathrm{c}}^j-P_{\mathrm{w}{\mathrm{b}}_i}-{\eta }_t)+t_{\mathrm{cb}})+{\eta }_{uv}$$

(19)

From the above Equation (19), it is evident that the pixel observation is influenced by three types of noise: ${\eta }_{\theta }$, ${\eta }_t$, and ${\eta }_{uv}$. To calculate the covariance of the camera observation, a first-order Taylor expansion is performed:

$$P_{uv}(T_{\mathrm{w}{\mathrm{b}}_i},P_c^j)\approx \pi (T_{\mathrm{w}{\mathrm{b}}_i},P_c^j)+J_{{\eta }_{\theta }}{\eta }_{\theta }+J_z{\eta }_t+{\eta }_{uv}$$

(20)

The Jacobian matrix of the camera observation with respect to the rotational perturbation ${\eta }_{\theta }$ and translational perturbation ${\eta }_t$ is given by

$$J_{{\eta }_{\theta }}=J^{\pi }(T_{\mathrm{w}{\mathrm{b}}_i},P_{\mathrm{c}}^j)R_{\mathrm{cb}}{R}_{\mathrm{w}{\mathrm{b}}_i}^{\mathrm{T}}{(P_{\mathrm{c}}^j-P_{\mathrm{w}{\mathrm{b}}_i})}^{\wedge }$$

(21)

$$J_z{\eta }_t=-J^{\pi }({\rho }_i,l_j)R_{\mathrm{cb}}{R}_{\mathrm{w}{\mathrm{b}}_i}^{\mathrm{T}}$$

(22)

The term $J^{\pi }$ refers to the Jacobian matrix of the camera projection equation, as shown in Equation (15).

Thus, we have

$$\delta \eta =J_{{\eta }_{\theta }}{\eta }_{\theta }+J_t{\eta }_t\delta +{\eta }_{uv}$$

(23)

Therefore, the covariance propagation equation for camera observation information under perturbation can be derived as follows:

$${\mathsf{\Sigma }}_{ij}=J_{{\eta }_{\theta }}{\mathsf{\Sigma }}_{\theta }{J}_{{\eta }_{\theta }}^{\mathrm{T}}+J_{{\eta }_t}{\mathsf{\Sigma }}_t{J}_{{\eta }_t}^{\mathrm{T}}+{\mathsf{\Sigma }}_{uv}$$

(24)

Subsequently, under the aforementioned constraints, the camera’s reprojection observation error equation can be constructed as follows:

$$e_{ij}=\pi (T_{\mathrm{w}{\mathrm{b}}_i},P_c^j)-P_{uv}(T_{\mathrm{w}{\mathrm{b}}_i},P_c^j)$$

(25)

The Jacobian matrices of the reprojection observation error with respect to the rotational and translational components of the robot’s pose are, respectively,

$${\displaystyle \frac{\mathsf{\partial }{e}_{ij}}{T_{\mathrm{w}{\mathrm{b}}_i}}}=\left[\begin{array}{cc}{\displaystyle \frac{\mathsf{\partial }{e}_{ij}}{\mathsf{\partial }{\theta }_i}}& {\displaystyle \frac{\mathsf{\partial }{e}_{ij}}{\mathsf{\partial }{\nu }_i}}\end{array}\right]$$

(26)

$${\displaystyle \frac{\mathsf{\partial }{e}_{ij}}{\mathsf{\partial }{\theta }_i}}=-J^{\pi }(T_{\mathrm{w}{\mathrm{b}}_i},l_j)R_{\mathrm{cb}}{R}_{\mathrm{w}{\mathrm{b}}_i}^{\mathrm{T}}{\mathsf{\Lambda }}_{12}$$

(27)

$${\displaystyle \frac{\mathsf{\partial }{e}_{ij}}{\mathsf{\partial }{\nu }_i}}=J^{\pi }(T_{\mathrm{w}{\mathrm{b}}_i},l_j)R_{\mathrm{cb}}{R}_{\mathrm{w}{\mathrm{b}}_i}^{\mathrm{T}}{(P_{\mathrm{c}}^j-P_{\mathrm{w}{\mathrm{b}}_i})}^{\wedge }{e}_3$$

(28)

The Jacobian matrix of the reprojection error with respect to the landmark point is:

$${\displaystyle \frac{\mathsf{\partial }{e}_{ij}}{\mathsf{\partial }{P}_{\mathrm{c}}^j}}=J^{\pi }(T_{\mathrm{w}{\mathrm{b}}_i},P_{\mathrm{c}}^j)R_{\mathrm{cb}}{R}_{\mathrm{w}{\mathrm{b}}_i}^{\mathrm{T}}$$

(29)

At this point, we have derived the camera reprojection observation error, its corresponding covariance, and the Jacobian matrix of the error with respect to the variables to be optimized.

2.4. Wheel Speed Pre-Integration Constraint

In wheeled mobile robots, encoder information is typically generated at a higher frequency than visual data, resulting in each frame of visual data corresponding to multiple instances of encoder data. Consequently, updating the camera pose at each iteration of back-end optimization, alongside the robot poses represented by the multiple sets of encoder data, can either lead to redundant computations or affect the accuracy of perception. Therefore, the concept of pre-integration is employed [25], as shown in Figure 4, to construct constraints between frames of visual information using pre-integrated encoder data.

First, the linear velocity v and angular velocity $\omega$ of a wheeled mobile robot can be derived from the left and right wheel speeds, as well as the robot’s intrinsic parameters.

$$\left[\begin{array}{c}v\\ \omega \end{array}\right]={\displaystyle \frac{1}{2B}}\left[\begin{array}{cc}B& B\\ 1& -1\end{array}\right]\left[\begin{array}{c}{v}_r\\ v_l\end{array}\right]+n_{v\omega }$$

(30)

In the equations, the linear velocity and angular velocity are subject to Gaussian noise, $n_{vw}\sim \mathcal{N}(0,{\sigma }_{vw}^2)$. To mitigate errors from manual measurements, accurate intrinsic parameters of the robot should be obtained through joint calibration with the camera [26].

Then, the discrete integration model for wheel speed at two consecutive time points can be derived, with a sampling interval of T.

$${\theta }_{k+1}={\theta }_k+{\omega }_{k}T$$

(31)

$$p_{k+1}=p_k+R\left({\theta }_k\right)\left[\begin{array}{c}0\\ v_{k}T\end{array}\right]$$

(32)

where $R\left({\theta }_k\right)=\left[\begin{array}{cc}cos{\theta }_k& -sin{\theta }_k\\ sin{\theta }_k& cos{\theta }_k\end{array}\right]$.

Given ${\varpi }_k={\left[v_k,{\omega }_k\right]}^{\mathrm{T}}$, the above equation can be expressed in a compact form as

$$q_{k+1}=f(q_k,{\varpi }_k)$$

(33)

Performing a Taylor expansion on the equation yields the error state equation as follows:

$$\delta q={\displaystyle \frac{\mathsf{\partial }f}{\mathsf{\partial }{q}_k}}{q}_k+{\displaystyle \frac{\mathsf{\partial }f}{\mathsf{\partial }{\varpi }_k}}{\varpi }_k$$

(34)

$${\displaystyle \frac{\mathsf{\partial }f}{\mathsf{\partial }{q}_k}}=\left[\begin{array}{ccc}1& 0& -v_{k}sin{\theta }_{k}T\\ 0& 1& v_{k}cos{\theta }_{k}T\\ 0& 0& 1\end{array}\right]$$

(35)

where ${\displaystyle \frac{\mathsf{\partial }f}{\mathsf{\partial }{q}_k}}=\left[\begin{array}{ccc}1& 0& -v_{k}sin{\theta }_{k}T\\ 0& 1& v_{k}cos{\theta }_{k}T\\ 0& 0& 1\end{array}\right]$, ${\displaystyle \frac{\mathsf{\partial }f}{\mathsf{\partial }{\varpi }_k}}=\left[\begin{array}{cc}cos{\theta }_k& 0\\ sin{\theta }_k& 0\\ 0& T\end{array}\right]$.

Given $F_k={\displaystyle \frac{\mathsf{\partial }f}{\mathsf{\partial }{q}_k}}{G}_k={\displaystyle \frac{\mathsf{\partial }f}{\mathsf{\partial }{\varpi }_k}}$, the error state equation between two consecutive wheel speed moments can be obtained as follows:

$$\left[\begin{array}{c}\delta {\theta }_{k+1}\\ \delta p_{k+1}\end{array}\right]=F_k\left[\begin{array}{c}\delta {\theta }_k\\ \delta p_k\end{array}\right]+G_k\left[\begin{array}{c}{n}_{\omega }\\ n_v\end{array}\right]$$

(36)

In this way, the covariance propagation equation for wheel speed observations at two consecutive time intervals can be derived [27]:

$$P_{k+1}=F_k{P}_k{F}_k^{\mathrm{T}}+G_k{Q}_{v\omega }{G}_k^{\mathrm{T}}$$

(37)

The initial value of $P_k$ is generally set as a zero matrix, and $Q_{vw}$ is derived from the measurement noise $n_{vw}$ of the encoder wheel speed:

$$Q_{v\omega }=\left[\begin{array}{cc}{\sigma }_v^2& 0\\ 0& {\sigma }_{\omega }^2\end{array}\right]$$

(38)

Assuming there are n instances of wheel speed information ${\varpi }_{k+1},{\varpi }_{k+2},\cdots ,{\varpi }_{k+n}$ between two consecutive keyframes $c_i$ and $c_{i+1}$, as indicated by Equation (33), the wheel speed pre-integration observation between the two keyframes is as follows:

$$\Delta q_{i,i+1}=\sum _{i=k+1}^{k+n}f(q_i,{\varpi }_i)$$

(39)

Accordingly, the covariance matrix of the pre-integrated quantities ${\mathsf{\Sigma }}_{i,i+1}$ between two visual keyframes can be recursively obtained from Equation (37).

Then, let the pre-integrated observation be denoted as ${\overline{T}}_{i,i+1}=\Delta q_{i,i+1}$, and based on the pre-integrated quantities derived in Equation (39), the wheel speed measurement error equation can be constructed as follows:

$$e_{i,i+1}={\overline{T}}_{i,i+1}{T}_{\mathrm{w}{\mathrm{b}}_i}{T}_{\mathrm{w}{\mathrm{b}}_{i+1}}^{-1}$$

(40)

By dividing the residuals into rotational and translational components, we obtain

$$\left[\begin{array}{c}{e}_{i,i+1}^{\theta }\\ e_{i,i+1}^{\rho }\end{array}\right]=\left[\begin{array}{c}{\overline{\theta }}_{i,i+1}-({\theta }_{i+1}-{\theta }_i)\\ {\overline{\rho }}_{i,i+1}-R^{\mathrm{T}}\left({\theta }_i\right)(p_{i+1}-p_i)\end{array}\right]$$

(41)

The Jacobian matrices of the wheel speed observation error with respect to the robot poses at two different time points are as follows:

$${\displaystyle \frac{\mathsf{\partial }{e}_{i,i+1}}{\mathsf{\partial }{q}_i}}=\left[\begin{array}{cc}1& 0\\ R^{\mathrm{T}}{(p_{i+1}-p_i)}^{\wedge }& R^{\mathrm{T}}\end{array}\right]$$

(42)

$${\displaystyle \frac{\mathsf{\partial }{e}_{i,i+1}}{\mathsf{\partial }{q}_{i+1}}}=\left[\begin{array}{cc}-1& 0\\ 0& -R^{\mathrm{T}}\end{array}\right]$$

(43)

The Jacobian matrix of the translational component of the error with respect to the rotational component of the i-th robot state is calculated using the right perturbation approach. Consequently, the observation error of the wheel speed pre-integration, its associated covariance, and the Jacobian matrix of the error with respect to the variables to be optimized have been obtained.

2.5. Loop Closure Co-Visibility Observation Constraint

In addition to the visual observation constraints and wheel speed pre-integration observation constraints introduced in the previous sections, there is also a loop closure constraint in the robot’s movement. This constraint is important for eliminating cumulative errors that arise during the robot’s state estimation process. When the robot’s pose $v_1$ at a certain time forms a loop closure with the pose $v_2$ at a previous time, there will be a large number of co-visible landmarks between the two frames. This allows the system to construct a loop closure constraint to correct the robot’s pose.

By taking the current frame, the loop closure frame, and the co-visible landmarks as nodes, and using the observation error of the landmarks in the camera coordinate system as edges, a subgraph optimization problem can be constructed. For instance, consider the observation of the k-th landmark ${\overline{P}}_k^{{\mathrm{c}}_i}$ from the perspective of the current frame $c_i$ (defined as $c_1$ for the current frame and $c_2$ for the loop closure frame). The measurement error can be expressed as

$$e_{{\mathrm{c}}_i,k}\left(X_i\right)={\overline{P}}_k^{{\mathrm{c}}_i}-T_{{\mathrm{c}}_i\mathrm{w}}{P}_k$$

(44)

The Jacobian matrix of the measurement error $e_{{\mathrm{c}}_i,k}$ with respect to the optimization variable X is

$$\begin{array}{c}{\displaystyle \frac{\mathsf{\partial }{e}_{i,k}}{\mathsf{\partial }{P}_k}}=-R_{{\mathrm{c}}_i\mathrm{w}}\\ {\displaystyle \frac{\mathsf{\partial }{e}_{i,k}}{\mathsf{\partial }\delta \psi }}=\left[\begin{array}{cc}-I& P_k^{\wedge }\end{array}\right]\end{array}$$

(45)

Let $G_k=\left[\begin{array}{cc}{\displaystyle \frac{\mathsf{\partial }{e}_{i,k}}{\mathsf{\partial }{P}_k}}& {\displaystyle \frac{\mathsf{\partial }{e}_{i,k}}{\mathsf{\partial }\delta \psi }}\end{array}\right]$. By applying a first-order Taylor expansion, Equation (44) can be expressed as

$$e_{c_i,k}\left(X\right)\simeq e_{c_i,k}\left(X^{\ast }\right)+G_{i,k}\Delta X$$

(46)

Therefore, the error function to be optimized is

$$J\left(X\right)={\displaystyle \frac{1}{2}}\sum _{i=1}^2\sum _{k=1}^N{(e_{{\mathrm{c}}_i,k}\left(X^{\ast }\right)+G_{i,k}\Delta X)}^{\mathrm{T}}{\mathsf{\Sigma }}_k^{-1}(e_{{\mathrm{c}}_i,k}\left(X^{\ast }\right)+G_{i,k}\Delta X)$$

(47)

After the iteration converges, the optimized poses of the two frames can be obtained as poses $T_{{\mathrm{c}}_1\mathrm{w}}^{\ast }$ and $T_{{\mathrm{c}}_2\mathrm{w}}^{\ast }$.

In the next step, to obtain the covariance of the relative observations, we need to calculate the covariance of each pose individually. Under the assumption of a Gaussian distribution, let the current pose to be optimized and the loop closure pose be variables $X={\left[X_1,X_2\right]}^{\mathrm{T}}$, respectively, while the shared 3D point is denoted as P. The joint probability distribution of $(X,P)$ can be represented as the product of the marginal and conditional probabilities [28], which can be expressed as follows:

$$P(X,P)=P\left(X\right)P\left(P\right|X)\propto exp\left({\displaystyle \frac{1}{2}}{\left[\begin{array}{c}X\\ P\end{array}\right]}^{\mathrm{T}}{\left[\begin{array}{cc}{\mathsf{\Sigma }}_{XX}& {\mathsf{\Sigma }}_{XP}\\ {\mathsf{\Sigma }}_{PX}& {\mathsf{\Sigma }}_{PP}\end{array}\right]}^{-1}\left[\begin{array}{c}X\\ P\end{array}\right]\right)$$

(48)

After iterative optimization, the Hessian matrix used for solving the optimization problem in the graph can be obtained. This matrix is denoted as H:

$$H=\left[\begin{array}{cc}{H}_{ii}& H_{ik}\\ H_{ki}& H_{kk}\end{array}\right]$$

(49)

The matrix block $H_{ii}(12\times 12)$ in the top left corner corresponds to the current frame and loop closure frame, while the matrix block $H_{kk}(3k\times 3k)$ in the bottom right corner corresponds to the landmark points.

For the likelihood distribution, the information matrix is the expected Hessian matrix of the negative log-likelihood problem [29]. Given the information matrix of the joint distribution, the information matrix of the marginal probability can be derived through decomposition [30]. By marginalizing the landmark points using the Schur complement, the information matrix for the marginal probability $P\left(X\right)$ can be obtained as follows:

$${H^{\prime }}_{ii}=H_{ii}-H_{ik}{H}_{ik}^{-1}{H}_{ki}$$

(50)

Next, we rewrite ${H^{\prime }}_{ii}$ in the following form:

$${H^{\prime }}_{ii}=\left[\begin{array}{cc}{H^{\prime }}_{11}& {H^{\prime }}_{12}\\ {H^{\prime }}_{21}& {H^{\prime }}_{22}\end{array}\right]$$

(51)

Assuming the current frame pose and loop closure frame pose are mutually independent, their information matrices correspond to the respective blocks in ${H^{\prime }}_{ii}$. Since the covariance matrix is the inverse of the information matrix, the information matrices for the current frame and the loop closure frame can be obtained as follows:

$${\mathsf{\Sigma }}_{11}^{-1}={H^{\prime }}_{11}$$

(52)

$${\mathsf{\Sigma }}_{22}^{-1}={H^{\prime }}_{22}$$

(53)

Consequently, the perturbed poses of the current frame and the loop closure frame can be represented as follows:

$${\overline{T}}_{{\mathrm{c}}_1\mathrm{w}}=exp\left({ϵ}_1^{\wedge }\right)T_{{\mathrm{c}}_1\mathrm{w}}^{\ast }$$

(54)

In the context of this statement, ${ϵ}_1\in {\mathbb{R}}^6,{ϵ}_1\sim \mathcal{N}(0,{\mathsf{\Sigma }}_{11})$ refers to the left perturbation in small quantities.

Similarly:

$${\overline{T}}_{{\mathrm{c}}_2\mathrm{w}}=T_{{\mathrm{c}}_2\mathrm{w}}^{\ast }exp\left({ϵ}_2^{\wedge }\right)$$

(55)

In the context of this statement, ${ϵ}_1\in {\mathbb{R}}^6,{ϵ}_1\sim \mathcal{N}(0,{\mathsf{\Sigma }}_{11})$ refers to the right perturbation in small quantities. Both the left and the right perturbations are equivalent; however, we choose the right perturbation in this context for convenience in subsequent calculations.

Therefore, the relative observation between the current frame and the loop closure frame is expressed as follows:

$${\overline{T}}_{12}={\overline{T}}_{{\mathrm{c}}_1\mathrm{w}}\ast {\overline{T}}_{{\mathrm{c}}_2\mathrm{w}}^{-1}$$

(56)

Given the relative observation perturbation as $ϵ$, the perturbation mechanism leads to the following equation:

$$exp\left({ϵ}^{\wedge }\right){\overline{T}}_{12}=exp\left({ϵ}_1^{\wedge }\right)T_{{\mathrm{c}}_1\mathrm{w}}{(T_{{\mathrm{c}}_2\mathrm{w}}exp\left({ϵ}_2^{\wedge }\right))}^{-1}$$

(57)

From identity,

$$exp\left({\left(\mathcal{T}\mathbf{x}\right)}^{\wedge }\right)=\mathbf{T}exp\left({\mathbf{x}}^{\wedge }\right){\mathbf{T}}^{-1}$$

(58)

We obtain

$$exp\left({ϵ}^{\wedge }\right){\overline{T}}_{12}=exp\left({ϵ}_1^{\wedge }\right)T_{{\mathrm{c}}_1\mathrm{w}}exp\left({(-{\mathcal{T}}_{{\mathrm{c}}_1\mathrm{w}}{ϵ}_2)}^{\wedge }\right)T_{{\mathrm{c}}_1\mathrm{w}}{T}_{{\mathrm{c}}_2\mathrm{w}}$$

(59)

which is

$$exp\left({ϵ}^{\wedge }\right)=exp\left({ϵ}_1^{\wedge }\right)exp\left({(-{\mathcal{T}}_{{\mathrm{c}}_1\mathrm{w}}{ϵ}_2)}^{\wedge }\right)$$

(60)

Given the formula ${{ϵ}^{\prime }}_2=-{\mathcal{T}}_{{\mathrm{c}}_1\mathrm{w}}{ϵ}_2$, the covariance of $ϵ$ (to the second-order term only) can be derived using the Baker–Campbell–Hausdorff (BCH) formula:

$${\overline{T}}_{12}=exp\left({ϵ}^{\wedge }\right)T_{12}$$

(61)

where $ϵ\sim \mathcal{N}(0,{\mathsf{\Sigma }}_{12})$.

Therefore, the observation error between the loop closure frame and the current frame can be expressed as

$$e_{12}=log{\left({\overline{T}}_{12}{T}_{\mathrm{w}{\mathrm{b}}_1}{T}_{\mathrm{w}{\mathrm{b}}_2}^{-1}\right)}^{\vee }$$

(62)

Similar to Equation (40), the transformation can be expressed in terms of the rotation and translation components on SE(2) as follows:

$$\left[\begin{array}{c}{e}_{12}^{\theta }\\ e_{12}^{\rho }\end{array}\right]=\left[\begin{array}{c}{\overline{\theta }}_{12}-({\theta }_1-{\theta }_2)\\ {\overline{\rho }}_{12}-R^{\mathrm{T}}\left({\theta }_1\right)(p_2-p_1)\end{array}\right]$$

(63)

The corresponding Jacobian matrix is similar to that in Equations (42) and (43), so it will not be repeated here. Thus, the loop closure covisual observation error, covariance matrix, and Jacobian matrix relative to the optimization variables have been obtained.

2.6. Formulation of the Maximum A Posteriori Estimation Problem

The essence of robot state estimation is maximum a posteriori estimation given known observations; when ignoring unknown prior information, this can be converted to maximum likelihood estimation. Under the assumption that the probability density function of the state variables follows a Gaussian distribution, the problem can be further transformed into a least squares estimation problem through negative logarithmic transformation. Thus, by treating the mobile robot pose and map points as state variables $\chi =(T_{\mathrm{w}{\mathrm{b}}_1},\cdots ,T_{\mathrm{w}{\mathrm{b}}_n},P_1^{\mathrm{w}},\cdots ,P_m^{\mathrm{w}})$, and based on the aforementioned reprojection error Equation (25), wheel speed pre-integration error Equation (40), and loop closure co-visibility error Equation (62), a nonlinear error function can be constructed as follows:

$$F\left(\chi \right)=\sum _{in,jm}{e}_{ij}^{\mathrm{T}}{\mathsf{\Sigma }}_{ij}^{-1}{e}_{ij}+\sum _{in-1}{e}_{i,i+1}^{\mathrm{T}}{\mathsf{\Sigma }}_{i,i+1}^{-1}{e}_{i,i+1}+\lambda \sum _{ik}{e}_{12}^{\mathrm{T}}{\mathsf{\Sigma }}_{12}^{-1}{e}_{12}$$

(64)

In the equation, $\lambda$ takes a value of 0 or 1, where it is set to 1 when loop closure and global optimization are needed.

The optimal state vector ${\chi }^{\ast }$ is obtained by minimizing the objective function $F\left(\chi \right)$:

$${\chi }^{\ast }=\underset{\chi }{argmin}F\left(\chi \right)$$

(65)

The error function can be linearized at the operating point $\chi$ using the second-order Taylor expansion, which yields

$$F(\chi +\delta \chi )\approx F\left(\chi \right)+J\delta \chi +{\displaystyle \frac{1}{2}}\delta {\chi }^{\mathrm{T}}H\delta \chi$$

(66)

To minimize the objective function, the function is differentiated with respect to $\delta \chi$ and set to zero. This results in the optimal perturbation value of the state vector, which is the solution to the following linear system:

$$H\delta {\chi }^{\ast }=-J$$

(67)

The optimal solution for the state variables is the best estimate of the pose of the wheeled mobile robot and the observed 3D map points:

$${\chi }^{\ast }\leftarrow {\chi }^{\ast }⊞\delta {\chi }^{\ast }$$

(68)

In this context, ⊞ represents the generalized summation symbol.

3. Experimental Comparison and Analysis

To verify the state estimation performance of the proposed Wheel Speed Visual SLAM (W-VSLAM) method—henceforth referred to as W-VSLAM in the text—for planar robot motion in an indoor environment without GPS, a cross-comparison with the latest visual SLAM algorithms in the field of visual methods was conducted. This comparison includes qualitative visual analysis and quantitative analysis based on general evaluation metrics, aiming to comprehensively validate the superiority of the proposed method. This section begins by detailing the experimental dataset, including the necessary parameters for different forms of data, and introduces two commonly used trajectory accuracy evaluation metrics in the SLAM field. Next, it provides an in-depth explanation of two sets of comparison experiments to validate the superiority of the W-VSLAM method. Finally, three groups of accuracy evaluation experiments are conducted to verify the effectiveness of the proposed W-VSLAM method.

3.1. Data Collection

Given that existing public datasets do not include indoor SLAM data with robot speed information or camera-encoder extrinsic parameters, a dataset was collected using a self-developed mobile robot platform in an indoor environment of an academic building. To facilitate comparison with open-source visual algorithms, the camera was mounted on top of the robot body in an upward-facing orientation (front-facing camera orientation rendered the latest visual SLAM algorithms completely ineffective). The encoders, coaxial with the drive motors, provided feedback on the wheel rotation speed of the robot platform at 1024 lines. The remotely controlled robot platform navigated an indoor environment to form a looped trajectory. During this motion, data from a 2D lidar, mounted on the front of the robot and operating at a frequency of 10 Hz, were recorded. In addition, gray-scale images were captured at a frequency of 25 Hz with a resolution of 752 × 480 pixels. Wheel speed data, calculated at the base layer, were collected at a frequency of 100 Hz and measured in RPM (revolutions per minute). The coefficients of the radial–tangential distortion model for the camera are shown in

Table 1. Performance index data of different segmentation algorithms.

Parameters	Radial Distortion ${\mathit{k}}_1$	Radial Distortion ${\mathit{k}}_2$	Tangential Distortion ${\mathit{p}}_1$	Tangential Distortion ${\mathit{p}}_2$
Value	−0.02209723	−0.00543498	0.00179311	−0.00069370

. The intrinsic parameters matrix K for the pinhole camera model are as follows:

$$K=\left[\begin{array}{ccc}366.9826923& 0& 361.35791565\\ 0& 366.7935063& 246.71152313\\ 0& 0& 1\end{array}\right]$$

(69)

3.2. Quantitative Evaluation Metrics

To assess the performance of the proposed vision algorithm, it must be compared with the ground truth trajectory, which is typically acquired using more accurate sensors. Common trajectory evaluation metrics include Absolute Trajectory Error (ATE) and Relative Pose Error (RPE). Let $T_k$ represent the pose of the trajectory being evaluated at time k, and let $T_{gt,k}$ represent the pose of the ground truth trajectory at the same time:

$$\mathrm{AT}{\mathrm{E}}_k=log{\left(T_{gt,k}^{-1}{T}_k\right)}^{\vee }$$

(70)

In the equation, $log{(·)}^{\vee }$ represents the operator that maps a transformation matrix (Lie group) to a vector (Lie algebra).

After obtaining the N trajectory errors for the two trajectories, statistical calculations can be performed to obtain common SLAM trajectory evaluation metrics, such as root mean square error (RMSE):

$$\mathrm{AT}{\mathrm{E}}_{\mathrm{RMSE}}=\sqrt{{\displaystyle \frac{1}{N}}\sum _{k=1}^N{\left|\left|log{\left(T_{gt,k}^{-1}{T}_k\right)}^{\vee }\right|\right|}^2}$$

(71)

After obtaining the absolute trajectory error, it is also necessary to evaluate the relative trajectory error. This can be conducted by considering the trajectory from time k to time $k+\Delta t$ and calculating the relative trajectory error as follows:

$$\mathrm{RP}{\mathrm{E}}_k=log{\left({\left(T_{gt,k}^{-1}{T}_{gt,k+\Delta t}\right)}^{-1}\left(T_k^{-1}{T}_{k+\Delta t}\right)\right)}^{\vee }$$

(72)

Similarly, the root mean square error (RMSE) for the relative trajectory error can be calculated as follows:

$$\mathrm{RP}{\mathrm{E}}_{\mathrm{RMSE}}=\sqrt{{\displaystyle \frac{1}{N}}\sum _{k=1}^N{\left|\left|log{\left({\left(T_{gt,k}^{-1}{T}_{gt,k+\Delta t}\right)}^{-1}\left(T_k^{-1}{T}_{k+\Delta t}\right)\right)}^{\vee }\right|\right|}^2}$$

(73)

In addition to evaluating the trajectory as a whole, one can decompose the pose into translation and rotation components and evaluate each separately. The calculation method is similar to the overall evaluation but involves extracting the corresponding translation or rotation components and calculating the differences. In the subsequent experiments, this separate evaluation approach is used, calculating the maximum error, minimum error, and root mean square error (RMSE) for each component. This allows for a detailed comparison of the estimation performance of the algorithm.

Due to the indoor environment without GPS, the robot’s true trajectory is obtained using the common laser HectorSLAM method with a 2D lidar. Then, using the ATE and RPE evaluation metrics discussed earlier, the proposed method, the latest open-source visual algorithm ORBSLAM3, and the wheel odometry trajectories are compared in terms of error accuracy relative to the true trajectory. This allows for a cross-comparison of the methods.

It should be noted that the other well-known open-source algorithms, VINS-Mono and VINS-Fusion, are not included in the comparison because the accuracy of the VINS algorithms relies significantly on precise initialization. For wheeled mobile robots in planar motion, it is difficult to fully excite the accelerometer during initialization, which leads to the divergence of the estimated robot trajectory. This renders the algorithm ineffective and not suitable for comparison. Therefore, wheel odometry trajectories are used as a substitute.

3.3. Indoor Comparative Experimental Results and Analysis

To validate the superiority of the proposed method for the state estimation of planar motion robots in indoor environments, a cross-comparison was conducted in the visual SLAM field using two sets of experiments. The comparison dataset was obtained by manually controlling a robot to follow a square trajectory in an indoor environment, as depicted in Figure 5. During the movement, there were lighting changes, and at the corners, there were pure rotations of 90 degrees. The first set of data involved running two laps at a speed of 0.08 m/s, while the second set of data consisted of one lap at a speed of 0.04 m/s (the two sets of data were not meant to be controlled for; the number of laps was chosen randomly for ease of operation, so one set involved one lap, and the other involved two laps). Figure 6 shows the feature points tracked during the algorithm’s execution. Matched feature points in the reference frame are indicated by red circles, while unmatched points are shown in blue circles. Matched feature points in the current frame are represented by green circles, while unmatched points are represented by red circles. The results of running the proposed SLAM algorithm are shown in Figure 7 and Figure 8. Black points represent landmark points, black boxes represent keyframes, blue lines represent the co-visibility between keyframes, and red represents the current orientation and landmark points.

The trajectory obtained by Laser HectorSLAM is considered the ground truth. The trajectories of the proposed W-VSLAM method, the latest visual open-source algorithm ORBSLAM3 keyframe trajectory, and the robot’s odometry trajectory were compared. The trajectory comparisons for the two sets of experiments are shown in Figure 9 and Figure 10. Given the inconsistency in time frequencies across the different trajectories, the maximum time error is set at 0.01 s. Additionally, due to the external parameter transformation between different sensors, the results were aligned using the visualization tool.

In Figure 9, the 2DLaser (ref) represents the reference trajectory obtained by 2DLaser and is shown as a dashed line. Ours represents the proposed W-VSLAM method and is depicted as a blue solid line. Odom represents the trajectory obtained from odometry and is depicted as a green solid line. ORBSLAM3 represents the latest open-source algorithm and is depicted as a solid red line. In Figure 10, the 2DLaser (ref) represents the reference trajectory obtained by Laser SLAM and is shown as a dashed line. Odom represents the trajectory obtained from odometry and is depicted as a solid blue line. Ours represents the proposed W-VSLAM method and is depicted as a solid green line. ORBSLAM3 represents the latest open-source algorithm and is depicted as a solid red line. From the two figures, it is visually apparent that the trajectory obtained using the ORBSLAM3 algorithm has significant errors in the z-axis direction, whereas the proposed visual algorithm aligns more closely with the reference trajectory.

More specifically, in Experiment One, the comparison of trajectories obtained by the four methods in terms of translation is illustrated in Figure 11. In the x and y axes, the odometry trajectory experiences drift due to accumulated error from integration. Similarly, the ORBSLAM3 algorithm requires initialization, resulting in a missing segment at the beginning and also showing accumulated error, though less than the odometry error. In contrast, the proposed W-VSLAM method aligns more closely with the reference trajectory, displaying a smaller error. In the z-axis direction, the proposed algorithm uses a 3-degree-of-freedom representation for the robot’s pose, ensuring an accurate estimation of planar motion. Meanwhile, the ORBSLAM3 algorithm uses a six-degree-of-freedom parameterization for the robot’s pose during planar motion, leading to erroneous estimation. This algorithm also exhibits significant errors in the pitch and roll directions, which are not presented here. The results of Experiment Two are similar and are not included.

After the qualitative analysis presented above, a quantitative analysis of the trajectory accuracy of the various algorithms in the two comparison experiments was conducted using two evaluation metrics: absolute trajectory error (ATE) and relative pose error (RPE). The results for Experiment One are shown in

Table 2. Absolute trajectory error (ATE) estimates of different algorithms for two loop trajectories.

Error Type	ORBSLAM3 Algorithm	Wheel Odometer	The Proposed Algorithm
Maximum Translational Error (m)	0.996424	0.663076	0.135999
Minimum Translational Error (m)	0.053682	0.191564	0.005725
Translational Root Mean Square Error (m)	0.649618	0.411756	0.070133
Maximum Rotational Error (rad)	1.798408	0.249882	0.073018
Minimum Translational Error (rad)	1.499492	0.000036	0.000234
Rotational Root Mean Square Error (rad)	1.674251	0.138140	0.024885

and

Table 3. Relative trajectory error estimates of different algorithms for two loop trajectories.

Error Type	ORBSLAM3 Algorithm	Wheel Odometer	The Proposed Algorithm
Maximum Translational Error (m)	2.258283	0.072512	0.083033
Minimum Translational Error (m)	1.679300	0.004918	0.005750
Translational Root Mean Square Error (m)	1.934542	0.030060	0.035044
Maximum Rotational Error (rad)	0.267859	0.015824	0.042052
Minimum Translational Error (rad)	0.004864	0.000011	0.000113
Rotational Root Mean Square Error (rad)	0.080078	0.002707	0.013080

, while the results for Experiment Two are presented in

Table 4. Absolute trajectory error (ATE) estimates of different algorithms for one loop trajectory.

Error Type	ORBSLAM3 Algorithm	Wheel Odometer	The Proposed Algorithm
Maximum Translational Error (m)	2.448048	0.877545	0.265917
Minimum Translational Error (m)	0.270060	0.130613	0.008439
Translational Root Mean Square Error (m)	1.263682	0.384537	0.160804
Maximum Rotational Error (rad)	2.242332	0.288801	0.255336
Minimum Translational Error (rad)	1.169881	0.001404	0.001039
Rotational Root Mean Square Error (rad)	1.844953	0.157194	0.101190

and

Table 5. Relative trajectory error (RPE) estimates of different algorithms for one loop trajectory.

Error Type	ORBSLAM3 Algorithm	Wheel Odometer	The Proposed Algorithm
Maximum Translational Error (m)	1.798007	0.100730	0.120928
Minimum Translational Error (m)	1.286840	0.064568	0.066626
Translational Root Mean Square Error (m)	1.445999	0.083673	0.094710
Maximum Rotational Error (rad)	0.375543	0.026001	0.037211
Minimum Translational Error (rad)	0.007266	0.000009	0.000061
Rotational Root Mean Square Error (rad)	0.069250	0.001821	0.010742

. For relative pose error (RPE), considering the variation in time frequencies of the different trajectories, the relative error was calculated over a relative increment of 1 m in the translation direction and a relative increment of 1 degree in the rotation estimation.

Absolute trajectory error reflects the algorithm’s accuracy over the entire trajectory. In Experiment One, as shown in Table 2, the proposed algorithm achieved a root mean square error of 0.135999 m in the translation direction, which is significantly better than the other two algorithms. The root mean square error in rotation estimation was 0.024885 rad, also outperforming the other two algorithms.

In Experiment Two, as shown in Table 4, the root mean square error in translation estimation was 0.160804 m, while the root mean square error in rotation estimation was 0.101190 rad. In horizontal comparisons, the proposed algorithm outperformed the other two algorithms, demonstrating its ability to integrate two relatively imprecise sensor data streams to achieve more accurate pose estimation and validate the effectiveness of the algorithm.

Moreover, a vertical comparison of Table 2 and Table 4 reveals that the translation estimation in Experiment One is superior to that in Experiment Two, as is the rotation estimation. This is likely because the trajectory in Experiment One had more loop closures, resulting in more accurate pose estimation for the robot. This finding further supports the effectiveness of the proposed algorithm. Relative trajectory error reflects the algorithm’s performance on local trajectories.

In Experiment One, as shown in Table 3, the proposed algorithm achieved a root mean square error of 0.035044 m in translation estimation and 0.013080 rad in rotation estimation. Experiment Two showed similar results, with a root mean square error of 0.094710 m in translation estimation and 0.010742 rad in rotation estimation. In horizontal comparisons, these results are slightly below the best performance, but the differences are minimal. It is notable that while the wheel odometry method accumulates errors quickly due to high-frequency integration, resulting in inaccurate global trajectory estimation, it is relatively accurate in local relative pose estimation. The proposed visual SLAM algorithm, which integrates wheel odometry, leverages this aspect. By using pre-integration to obtain accurate inter-camera frame estimations as constraints, and by shortening the wheel odometry integration interval to reduce cumulative errors, the algorithm improves the relative pose estimation performance of the camera. This ensures that the proposed algorithm can achieve more accurate global pose estimation.

In summary, for planar motion robot state estimation under significant lighting variations, the proposed method demonstrates superior performance in both global and local accuracy compared to the latest visual algorithms in the field. When compared to wheel odometry, the proposed method also exhibits superior performance globally, while achieving nearly comparable, albeit slightly lower, performance at the local level.

3.4. Results and Analysis of Multiple Indoor Experiments

Additionally, three experimental sets were conducted multiple times to validate the robustness and perceptual performance of the proposed W-VSLAM method, mitigating potential environmental factors. The comparative results with the latest visual SLAM algorithms have been discussed in the previous section and will not be repeated here. Instead, the trajectories provided by laser HectorSLAM are used as reference true values to evaluate the perceptual accuracy of the proposed visual SLAM method. The results of the robot’s environmental perception across different motion trajectories, as well as comparisons with the reference trajectory, are shown in Figure 12, Figure 13, Figure 14, Figure 15, Figure 16, Figure 17 and Figure 18. The RMSE accuracy of the W-VSLAM method across different trajectories is presented in

Table 6. Results of trajectory accuracy evaluation from multiple experiments.

Category	Experiment 1	Experiment 2	Experiment 3
Absolute Translational RMSE (m)	0.069569	0.174467	0.083839
Absolute Rotation RMSE (rad)	0.079908	0.103241	0.096109
Relative Translation RMSE (m)	0.110006	0.106630	0.107273
Relative Rotation RMSE (rad)	0.012823	0.015159	0.025358

. In the first dataset, the robot initially follows a small square trajectory and then expands its movement range. In the second dataset, the robot traverses two adjacent closed square trajectories. The third dataset involves a back-and-forth motion pattern. The first two datasets mainly highlight the impact of lighting changes during the robot’s movement on the perception algorithm, while the third emphasizes the effect of a 360-degree pure rotation at the midpoint of a straight trajectory.

The results from the three different experimental sets indicate that the proposed W-VSLAM perception algorithm can robustly perceive robot poses and map points even under conditions of strong lighting changes or 360-degree pure rotational motion. The trajectory localization accuracy ranges from 5 cm to 17 cm, aligning with the current perception levels of visual SLAM methods and further verifying the effectiveness of the proposed approach.

3.5. The Results and Analysis of the Indoor Long Corridor Environment Experiment

The indoor long corridor environment has always posed challenges for laser SLAM perception. Due to the limited range or angle of sensors, the information perceived by the sensors does not significantly change during movement, leading to inaccurate state estimation. In this experiment, a robot was controlled to navigate through a long corridor within an indoor environment, returning through the corridor to form a closed-loop trajectory. Throughout the movement, significant changes in illumination intensity were observed. The results of the proposed W-VSLAM method in this long corridor environment are shown in Figure 15.

In the aforementioned corridor environment, we operated HectorSLAM using a 2D LiDAR with a scan range parameter set to 5 m. The resulting trajectory was compared with the trajectories obtained using the proposed algorithm and ORBSLAM3. The visual comparisons of the trajectories are illustrated in Figure 16 and Figure 17. Based on the conclusions from the quantitative accuracy comparison in the previous subsection, the trajectory from the wheel speed odometer was neither included in the comparison nor reported. Observing the trajectory obtained by HectorSLAM with a 5 m detection range, it is evident that when the sensor’s detection distance is insufficient to cover the corridor length, the trajectory generated by HectorSLAM becomes distorted. This distortion occurs because the point cloud observations within the detection range do not change as the robot moves through the corridor, leading to a lack of translational estimation (in the y-axis direction in the world coordinate system). In contrast, the proposed algorithm can still accurately perceive the environment in the long corridor, demonstrating the advantages of visual SLAM methods in specific settings. Additionally, as seen in the translational component comparison in Figure 17, the trajectory obtained by ORBSLAM3 is generally accurate in the r-axis direction (perpendicular to the corridor direction), except at the end of the trajectory. The proposed algorithm’s trajectory closely aligns with the reference trajectory, except for a noticeable deviation at the trajectory’s end due to visual odometry estimation errors. The trajectory generated by the proposed method is closer to the actual path, further showcasing its advantages in environments where LiDAR fails.

Although the translational estimation of HectorSLAM’s reference trajectory is inaccurate and thus cannot be analyzed in detail, its rotational estimation can still be qualitatively assessed. The absolute and relative trajectory accuracies for rotational estimation, as achieved by the proposed algorithm compared to the reference trajectory, are shown in Figure 18 and Figure 19, respectively. In addition to the maximum and minimum errors illustrated in the figures, the root mean square error (RMSE) of the absolute rotational error is 0.0726080 rad, and the RMSE of the relative rotational error is 0.022059 rad, which is consistent with the results from the previous subsection. Overall, the comparison with the HectorSLAM laser algorithm demonstrates that the proposed visual SLAM algorithm can achieve more accurate perception in indoor long corridors or diffuse reflection environments where 2D LiDAR fails.

4. Conclusions

To address the estimation inaccuracies in indoor environments without GPS signals for ground mobile robots, caused by excess pose degrees of freedom and the lack of excitation in accelerometers, a visual SLAM perception method that integrates wheel odometry information is proposed. The method parameterizes the robot pose on SE(2) and the camera pose on SE(3). It then derives visual constraint residuals and Jacobian matrices for reprojection observations using the camera projection model and uses the pre-integration concept to derive pose-constraint residuals and Jacobian matrices from wheel odometry pre-integration quantities. Loop closure observation constraints are derived using marginalization theory to calculate relative pose residuals and Jacobians. These are used to solve the nonlinear optimization problem and obtain the optimal poses and landmark points for the ground mobile robot. Comparisons with the ORBSLAM3 algorithm and wheel odometry in experimental tests indicate that the proposed visual SLAM method integrating wheel odometry provides higher perception accuracy in planar motion estimation, particularly in indoor environments with significant lighting variations or pure rotational motion. The method’s superiority is demonstrated with accuracy in a range of 5–17 cm across multiple experiments, further confirming its effectiveness. It is worth noting that the proposed visual SLAM perception method with integrated wheel odometry is not limited to wheeled mobile platforms; it can also be applied to tracked mobile platforms.