Enhanced Visual SLAM and Path Planning for Autonomous Navigation of Wheeled Mobile Robots

Summary

This study presents an approach to improve WMR autonomous indoor navigation by optimizing visual SLAM and path planning algorithms. It integrates multi-sensor fusion, enhances feature extraction, and applies trajectory optimization techniques for better localization, obstacle avoidance, and smoother paths, demonstrating superior performance in real-world and simulated environments.

Abstract

This research focuses on important technologies used in wheeled mobile robot autonomous navigation, such as path planning optimization, system integration, and advancements in visual simultaneous localization and mapping (SLAM) techniques. An enhanced approach is suggested to overcome localization issues in traditional visual odometry brought on by duplicated or unevenly distributed feature points. This approach combines Efficient Perspective-n-Point (EPNP) feature matching, iterative closest point (ICP) pose optimization, and quadtree-based feature management. According to experimental findings, the suggested method greatly increases localization accuracy and stability. A dense point cloud reconstruction technique based on RGB-D data is developed to improve the completeness and detail of environmental representation while mitigating the sparsity often seen in point cloud maps produced by conventional SLAM systems. In order to enhance path quality and computational efficiency, an enhanced rapidly-exploring random tree (RRT) method is presented, which incorporates adaptive step-size management, goal biasing, and B-spline-based path smoothing. Furthermore, real-time local obstacle avoidance in dynamic situations is made possible by the integration of the Timed Elastic Band (TEB) algorithm. Comprehensive real-world tests have confirmed the usefulness of the suggested solutions in terms of efficiency, robustness, and practical applicability after they were implemented on an experimental platform based on the Robot Operating System (ROS).

Introduction

The potential and application patterns of robotics are undergoing a period of rapid transformation, driven by advancements in artificial intelligence technologies. In recent years, Visual Simultaneous Localization and Mapping (Visual SLAM) and its extension to Visual-Inertial Navigation Systems (VINS) have made substantial progress in terms of robustness and localization accuracy¹. To enhance the initialization reliability under challenging conditions such as low texture and poor illumination, Campos et al. proposed ORB-SLAM3, which introduces a multi-map system and improved initialization for visual and visual-inertial systems². For improved feature matching in challenging scenarios, DeTone et al. developed SuperPoint, a self-supervised interest point detection and description method³, while Sarlin et al. created SuperGlue, a graph neural network-based feature matcher that handles difficult visual conditions⁴. For dense 3D reconstruction, Dai et al. proposed BundleFusion, a real-time globally consistent 3D reconstruction system that uses on-the-fly surface reintegration to handle large-scale environments and loop closures⁵.

In the field of path planning, Rapidly-exploring Random Trees (RRT) and their variants remain widely adopted for robotic motion planning. The foundational RRT algorithm was first introduced by LaValle as a new tool for path planning, providing an efficient sampling-based method for solving complex high-dimensional problems⁶. This was significantly advanced by Karaman and Frazzoli, who developed the RRT* algorithm that provides asymptotic optimality guarantees in motion planning⁷. Building upon these core algorithms, modern research has focused on hybrid approaches that combine sampling-based methods with other techniques. For example, Rösmann et al. developed the Timed Elastic Band (TEB) method, which enables locally optimal trajectory generation and has been widely integrated with global planners⁸. Similarly, the Dynamic Window Approach (DWA) introduced by Fox et al. provides an effective method for local obstacle avoidance in dynamic environments⁹.

At the local planning and semantic perception level, Chen et al. proposed a semantic-aware informative path planning strategy for micro aerial vehicles (MAVs), enhancing both search efficiency and safety during target exploration¹⁰. Kabiri et al. integrated 5G Time-of-Arrival (ToA) measurements into a VINS framework to enable global-local SLAM fusion, effectively improving localization accuracy in environments with limited GNSS coverage¹¹. To facilitate high-frequency real-time mapping, Xu et al. developed FAST-LIO2, a tightly coupled LiDAR-IMU odometry method capable of producing accurate and dense 3D maps¹². For path planning in complex environments, Gammell et al. introduced an informed RRT* method that incorporates bidirectional tree growth and adaptive sampling, significantly improving path quality and search efficiency in dynamic environments¹³. Additionally, for narrow-passage scenarios, Coleman et al. presented a sampling-based motion planning method with variable probability sampling, which improves planning success rates and computational efficiency¹⁴.

The present study addresses fundamental challenges in autonomous indoor navigation for wheeled mobile robots (WMRs) by improving both the path planning strategy and the SLAM front-end. Specifically, the proposed system is designed for typical structured indoor environments such as laboratories and corridors, operating under conditions with moderate lighting and minimal GNSS access. The navigation system primarily utilizes a stereo RGB-D camera, an inertial measurement unit (IMU), and wheel encoders, with all sensors configured to sample at no less than 20 Hz. To ensure reliable system performance, the robot's maximum speed is constrained to under 1.5 m/s. The following are the key contributions:

A multi-sensor fusion autonomous navigation platform for wheeled mobile robots (WMRs) has been developed using a depth camera as the primary sensor. To achieve accurate localization and efficient obstacle avoidance in typical indoor environments, the system integrates wheel odometry and an inertial measurement unit (IMU). The synergy between these components plays a critical role in enhancing the overall navigation performance.

Combining EPnP and ICP algorithms with a quadtree-based feature extraction technique has helped the tracking module in ORB-SLAM2 improve. Better tracking accuracy and robustness follow from these developments.

A new path planning method is proposed that emphasizes trajectory optimization. It is based on an improved RRT technique with goal biasing and adjustable step sizes and uses B-spline curves for trajectory smoothing. The TEB algorithm is also included to manage obstacle avoidance in dynamic environments.

The system's performance is confirmed by real-world testing and simulation. Typical indoor environments allow quantitative and qualitative analysis to evaluate map accuracy, path quality, and navigation performance. In terms of robustness, real-time processing, and trajectory smoothness, the proposed approach beats current solutions.

Protocol

1. Hardware platform

1. Prepare the two-wheeled differential-drive mobile robot platform suitable for indoor navigation (see Figure 1). This platform uses two independently driven wheels aligned along the center of the chassis and passive caster wheels at the front and rear to ensure mechanical balance and maneuverability.
2. Mount the differential drive wheels along the central longitudinal axis of the chassis. Use a hex screwdriver to align and fasten the wheel shafts into the motor hubs. Ensure the wheels are firmly attached but rotate freely without axial wobble. Verify that both wheels are aligned precisely to maintain straight-line motion and accurate odometry.
3. Install the front and rear caster wheels at both ends of the chassis to provide mechanical support during turns. Poor alignment may lead to instability or tilting during high-speed directional changes.
4. Mount a structured-light depth camera on the upper front panel of the chassis. Use an adjustable bracket or adhesive mount to affix the camera securely. Orient it such that the field of view covers approximately 0.3 m to 3.0 m ahead of the robot.
5. Connect the IR projector and receiver modules into the camera housing, ensuring all optical centers are properly aligned. Adjust the pitch angle of the camera to optimize depth perception.
6. Tilt the camera downward by 15°-30° using the adjustable mount. Make sure no part of the chassis obstructs the projected IR pattern. This angle helps in capturing near-field terrain features and avoiding blind spots.
7. Verify the camera's real-time depth output using visualization software such as RViz (version 1.14.1). Launch the camera node and observe the depth image stream. Connect the depth camera to the Microcontroller Unit (MCU) mounted at the center of the chassis.
   NOTE: Ensure power is off during all connections. Keep cables organized and away from moving parts to prevent entanglement during motion.

2. Optimization of ORB-SLAM2 for indoor mapping

1. Prepare the ORB-SLAM2 environment. Calibrate the camera (RGB-D) using standard ROS calibration tools. Configure the launch file to specify camera topics, resolution (e.g., 640 x 480), and frame rate (e.g., 30 fps). Launch the SLAM system using: xtark@tarkbot: $ roslaunch robot_platform slam map.launch slam _methods:=gmapping. Verify live camera feed and SLAM initialization messages in the terminal. Keyframes should appear after motion begins.
2. Modify ORB-SLAM2 to support dense mapping. Extend the default mapping module to include a dense reconstruction thread that processes depth data from keyframes.
3. For each selected keyframe: Extract synchronized RGB and depth images, convert depth pixels to 3D points using camera intrinsics, and fuse accumulated point clouds across keyframes using pose information. Recursively subdivide any region with more than one key point into four quadrants. Continue until each leaf node contains at most one dominant key point , or the region size is below 10 x 10 pixels.
4. Enhance feature distribution using a quadtree (see Figure 2). Modify the ORB feature extraction module to include a quadtree-based spatial partitioning strategy. Divide the image into hierarchical grid regions, apply FAST corner detection in each region, and retain only the most salient feature per region to ensure uniform spatial coverage.
5. From each valid region, select the candidate with the highest saliency response as the representative feature.
6. Improve pose estimation with EPnP. Replace default pose estimation (e.g., iterative methods) with the Efficient Perspective-n-Point (EPnP) algorithm using OpenCV's solvePnP. Use 2D image features and their corresponding 3D map points to solve the camera pose.
7. Deploy, visualize, and control the robot. Assign a static IP address to the robot's onboard system for stable communication (e.g., ROBOT IP: 172.20.10.13). On the host PC, open RViz (v1.14.1) and load the configuration to visualize the robot's trajectory, sparse and dense point cloud maps, keyframes, and detected features.
8. Manually control the robot using the keyboard arrow keys to navigate the space for mapping. Ensure the trajectory line appears in RViz, and camera pose frames update in real time.
   NOTE: Figure 3 illustrates the keyboard layout for manual robot control during mapping.

3. Feature point processing using the Quadtree algorithm

1. Perform ORB feature extraction as described below.
   1. Load the input image from a ROS image topic or a local dataset using OpenCV (version 4.5.3).
   2. Build a Gaussian pyramid with four levels, divide the image into uniform grid cells (8 x 8 cells per level). Within each cell, apply the FAST detector with a threshold of 20 to identify local keypoints.
2. Construct a quadtree-based feature refinement as described below.
   1. For each set of keypoints at a given pyramid level, construct a quadtree structure: Start with the full image as the root node. Recursively subdivide any region with more than one keypoint into four quadrants. Continue until each leaf node contains at most one dominant keypoint , or the region size is below 10 x 10 pixels.
3. Apply feature saliency assessment as described below.
   1. Evaluate the saliency of each candidate keypoint within a node using Equation:
           (1)
      where I_p is the intensity value of the center pixel in a local neighborhood, and I_i represents the intensity values of its 16 neighboring pixels. The absolute difference |I_p - I_i| measures the local contrast between the center pixel and each neighbor. The sum over all 16 neighbors provides a measure of the overall local contrast or texture strength around the center pixel.
   2. Rank all candidates using a dynamic priority queue sorted by the saliency score. From each valid region, select the candidate with the highest saliency response as the representative feature.
4. Optimize and validate feature selection
   1. Combine all selected features across pyramid levels. Ensure uniform spatial coverage across the image. Store the final feature points and their descriptors using the ORB descriptor extractor, version aligned with OpenCV.
   2. Verify that features are not clustered in a few image areas. Feature points should exhibit uniform spatial distribution, supporting robust tracking. Avoid executing image processing in a physical robot system while in motion. Ensure the camera stream is stable and the workspace is cleared.

4. Pose estimation using EPnP

1. Establish 2D-3D correspondences by selecting at least four matched pairs of 3D map points (in world coordinates) and their corresponding 2D image keypoints. Ensure these correspondences are extracted from valid ORB feature matches obtained in the tracking thread.
2. Solve initial pose with EPnP. Continue until each leaf node contains at most one dominant keypoint , or the region size is below 10 x 10 pixels. Use OpenCV's solvePnP function with the cv::SOLVEPNP_EPNP flag to estimate the camera pose.

5. Fine pose refinement with ICP

1. Perform point cloud sampling as described below.
   1. Downsample the source point cloud to reduce computational load and remove redundant data.
   2. Use uniform sampling to ensure structural features are retained evenly across all directions. If needed, apply voxel grid filtering or random selection based on the input point cloud's density and noise characteristics. Ensure the filtered cloud preserves object contours while reducing total point count by at least 50%.
2. Match corresponding points by constructing a KD-Tree from the destination point cloud to enable efficient nearest neighbor searches. For each point in the down sampled source point cloud, find its closest point in the destination cloud using the KD-Tree. Ensure accuracy in point matching, as this step critically affects registration performance.
3. Estimate the optimal transformation as described below.
   1. Use the matched point pairs to compute a rigid-body transformation matrix, including both rotation and translation.
   2. Compute the optimal rigid transformation between the matched point pairs by minimizing the mean squared error (MSE) through singular value decomposition (SVD) of the cross-covariance matrix, which yields the rotation matrix directly, followed by computation of the translation vector based on the rotated centroids.
4. Apply the computed transformation to the source point cloud and update all point coordinates. Repeat the point matching and transformation estimation process iteratively. Continue iterating until either the registration error falls below a predefined threshold or the maximum number of iterations is reached.

6. Dense point cloud map construction

1. Construct a dense 3D point cloud map to achieve an accurate and detailed representation of indoor environments. Follow the steps (see Figure 4) described below.
2. Extract RGB and depth data from keyframes. Select keyframes based on visual richness and spatial coverage. From each selected keyframe, extract both the RGB image and the corresponding aligned depth map from the RGB-D sensor.
3. Convert image pixels to 3D camera coordinates. For each valid depth pixel, project the 2D pixel into 3D space using the intrinsic camera parameters. This process generates 3D coordinates in the camera coordinate system.
4. Transform camera coordinates into world coordinates. Retrieve the optimized camera pose from ORB-SLAM2 for each keyframe. Use the camera pose to transform the 3D camera coordinates into the world coordinate system, aligning all point clouds in a common global reference.
5. Generate colorized 3D points. For each transformed 3D point, assign the corresponding RGB value from the original image. This results in a colorized point cloud that captures both geometry and appearance.
6. Merge point clouds from all keyframes. Accumulate all transformed and colorized point clouds into a unified global point cloud map. Ensure correct alignment using the camera poses associated with each keyframe.
7. Register and refine the final map using PCL. Use the Point Cloud Library (PCL) to refine the final map. Apply filtering to remove noise and down sampling to improve efficiency. Perform global registration (e.g., using ICP) to fine-tune alignment between point clouds if necessary (see Figure 5).
   NOTE: As shown in Figure 6, the initial point cloud alignment during the dense mapping initialization phase may exhibit transient misalignment due to limited observational data, which rapidly converges as additional viewpoints are incorporated. By controlling the robot to traverse the environment, a complete three-dimensional model can be obtained.

7. Generate an occupancy grid map from VSLAM-derived point clouds

1. Down sample the global dense point cloud. Apply voxel grid filtering using a voxel resolution of 0.05 m to reduce redundancy and define the spatial resolution for grid construction.
2. Project 3D points into a 2D occupancy grid. Project all 3D points onto the horizontal (x-y) plane. Discretize the space into uniform grid cells, each representing a 0.05 m x 0.05 m square in the real world.
3. Estimate occupancy probabilities. Use an inverse sensor model to compute the occupancy probability of each cell based on point density and simulated ray-tracing.
   1. Set the occupied probability threshold to 0.65. Set the free probability threshold to 0.35. Classify grid cells with intermediate values as unknown.
4. Apply obstacle inflation. Inflate the occupied regions by applying a circular kernel with a radius of 0.2 m to account for robot clearance and safety margins.
5. Export the occupancy map. Save the generated occupancy grid map in the Portable GrayMap format, accompanied by a corresponding m.yaml metadata file, to ensure compatibility with ROS-based navigation systems.

8. Improved global path planning strategy (based on RRT algorithm)

1. Initialize the path tree. Set the robot's start position as the root node of the tree. Randomly sample points in the configuration (state) space to explore new areas.
2. Identify the nearest existing node. For each newly sampled random point, calculate the Euclidean distance to all existing nodes. Select the node with the minimum distance as the nearest node to serve as the expansion base.
3. Generate a new node toward the random sample. Create a directional unit vector from the nearest node toward the sampled point. Move a fixed step (initially) along this direction to form a new node and connect it to the tree.
4. Replace fixed step size with an adaptive mechanism. Instead of using a constant step size, dynamically adjust the step length based on the local obstacle density. Use larger steps in open environments to accelerate tree expansion. In cluttered or narrow regions, reduce the step size to improve control and obstacle avoidance.
5. Compute the adaptive step size in real time as described below.
   1. Use sensor data (e.g., LiDAR or depth camera) to estimate the density of obstacles around the current region.
   2. If the number of detected obstacles is low, slightly increase the step size. If obstacles are dense, reduce the step size proportionally to insert more intermediate nodes for safe traversal.
6. Iterate the expansion process. Continue sampling, nearest-node searching, and new-node generation using the adaptive step size.
7. Apply B-spline curves for smoothing. Replace the polyline segments in the RRT path with a continuous B-spline curve to improve smoothness. Select control points along the original RRT path, typically at turning points or key waypoints. Construct a control polygon by connecting these control points in sequence.
8. Generate the B-spline curve. Use the standard B-spline formula¹⁵:
        (2)
   This formula is used in B-spline curves, where the final curve C(u) is a weighted combination of the control points. The weights are determined by the B-spline basis functions N_i,k (u), which ensure that the curve is smooth and follows the general shape defined by the control points.
9. Set the curve degree to 3 (cubic), which ensures continuity (smooth first and second derivatives). Use the path planning module written in PyCharm 2024.3.

9. Local trajectory optimization with modified TEB

1. Introduce the shortest distance constraint as described below.
   1. To mitigate these drawbacks, integrate a shortest distance constraint into the TEB framework.
   2. Define the constraint as the Euclidean distance between the robot's current position S_t and a future pose S_i+n along the trajectory:
           (3)
      This constraint penalizes inefficient deviations by encouraging the path to stay close to the edge of the global path corridor, improving planning quality and safety.
2. Integrate constraint into TEB cost function by modifying the original TEB optimization graph to include the distance constraint as an additional edge. Adjust the total cost function to include a weighted term for f_os, balancing smoothness, feasibility, and energy efficiency.
3. Integrate constraint into TEB cost function. During optimization, solve for trajectory points that minimize the total cost, including velocity, acceleration, obstacle clearance, and the added shortest-distance term. Use TEB's underlying solver to iteratively optimize the trajectory over N time intervals. Optimize the path considering the constraint (see Figure 7).

Results

Evaluation of improved ORB-SLAM2
Feature extraction experiment
To evaluate the effectiveness of an RGB-D depth camera in practical scenarios, a feature point extraction experiment was conducted. The test was designed using two distinct background environments, each varying in object color and brightness to simulate real-world visual complexity.

Both the proposed improved extraction method and the conventional baseline approach were applied to the same ...

Discussion

The two key technologies in autonomous interior navigation systems for wheeled mobile robots that are the focus of this study are visual simultaneous localization and mapping (SLAM)^16,17and path planning¹⁸. The SLAM module proposes a quadtree-based hierarchical selection method to correct the uneven feature point distribution of ORB-SLAM2. To enhance the precision of the generated map, an asynchronous dense mapping approach is employed. Th...

Materials

List of materials used in this article

Name	Company	Catalog Number	Comments
Astra Pro Plus 3D Camera	CRBBEC	None	3D Camera
TARKBOT-R20-TWD	None	None	ROS Robot