Sensor Fusion – Part 2
Authors: Harsh Vardhan Singh, Akash Parikh | Date: 21 March 2023 | Category: Automotive
Introduction
This blog is the second part of a two-part series where we will introduce the usage of sensor fusion in Autonomous Mobile Robots (AMRs) that are enabled to work with ROS (Robot Operating System) 2. In the first part, we introduced sensor fusion, briefly covered a sensor fusion algorithm called EKF (Extended Kalman Filters), and then walked through some of the experiments we did in simulation and on our AMR. The first part can be found in Sensor Fusion – Part 1. In this part, we are going to discuss some of the implications of using sensor fusion and showcase how accurately knowing the state of our AMR is a fundamental step that allows the AMR to robustly perform other functions like navigating, avoiding obstacles, mapping, etc. We will do this by observing the effects of using sensor fusion on two separate VSLAM algorithms, namely RTAB-Map and Isaac ROS VSLAM (and Nvblox).
Before moving forward, let’s quickly go through what VSLAM is. VSLAM stands for Visual Simultaneous Localization and Mapping. It is a type of simultaneous localization and mapping (SLAM) technique used in robotics and computer vision. VSLAM uses visual information from cameras, such as RGB (Red-Green-Blue) and depth sensors, to build a map of the environment while simultaneously localizing the robot’s position within that map. The visual data can be used to identify landmarks in the environment, track the robot’s motion, and estimate its position and orientation.
RTAB-MAP
RTAB-Map is an open-source framework that enables simultaneous localization and mapping (SLAM) in several types of robots, including drones, mobile robots, and humanoid robots. This visual-based approach uses sensors such as cameras to build a map of the environment while also localizing the robot within the map. The framework also integrates odometry and loop closure detection, resulting in highly accurate localization and mapping in challenging environments. Some of the features of RTAB-Map include point cloud mapping, graph-based optimization, loop closure detection, and 3D visualization. For details on how to use RTAB-Map, we encourage our readers to go through our blog Autonomous navigation of AMR using RTAB-Map and ToF Camera.
When mapping our office premises, we experimented with two different sensors: once with the AMR’s Realsense RGBD data and then with its Time-of-Flight sensor. Through the usage of RTAB-Map, we will show how mapping is affected by a poor odometry source and then improve upon this by providing odometry via sensor fusion.
RTAB-MAP Observations with Realsense RGBD
In our experiments of using RTAB-Map with Realsense RGBD data, we obtained very robust results in feature-rich environments, and the visual odometry performed well. However, when working in regions with low features, the visual odometry would be lost to a point of no recovery, and the map creation process had to be restarted. We also observed that even if the odometry is not lost, the quality of the map is a bit degraded, and the overall map is not clean enough to allow proper navigation. This could be due to large odometry drifts or lots of similar features observed in the environment. In Figure 1 we show what a degraded map would look like.
After using sensor fusion, we were able to create a good map of our low feature environment by providing a more robust odometry source to RTAB-Map. This was done by fusing IMU and wheel odometry. Figure 2 shows the result. As can be seen, we obtained a clean map with well-defined non-overlapping boundaries. Such a map can be saved and used for indoor navigation purposes.
RTAB-Map observations with Time-of-Flight sensor
When running RTAB-Map with point cloud data coming from our Time-of-Flight sensor, we used wheel odometry as an odometry source. The wheel odometry drifts, especially when turning, and affects the quality of the map being generated. In Figure 3, we can see that loop closure is not proper leading to the walls to be misaligned.
After fusing IMU data with wheel odometry, we were able to provide a more stable source of odometry, resulting in better overall mapping and proper loop closure. As shown in Figure 4, the final map does not contain any stray unknown obstacles, making it more suitable for navigation.
Figure 1: Degraded mapping in low feature environment
Figure 2: Clean map in low feature environment after using sensor fusion.
Figure 3: Improper loop closure
Figure 4: Proper wall alignment after using sensor fusion.
Architecture
Isaac ROS VSLAM + Nvblox
In this section, we will use a different approach to sensor fusion. Instead of simply using EKF as an odometry source (local pose estimation), we will use it to perform both local and global pose estimation. Please note that the odometry source can be viewed as a local pose estimator that gives the state estimate of our AMR with respect to its initial position, while global pose estimation would mean estimating the robot’s position with respect to a given global map of the environment. We will use the Isaac ROS VSLAM and Nvblox algorithms to demonstrate this.
Isaac ROS Visual SLAM
Isaac_ros_visual_slam is a ROS2 package that provides a powerful VSLAM solution. The package utilizes stereo VSLAM and leverages the Isaac Elbrus GPU-accelerated library to estimate stereo visual inertial odometry. By taking in a time-synced pair of stereo images in grayscale format, along with their respective camera intrinsic parameters, the package publishes the current pose of the camera relative to its starting pose.
Isaac_ros_nvblox
Nvblox is a powerful tool for constructing accurate 3D models of a robot’s environment. This process is accelerated using NVIDIA CUDA, allowing for real-time operation rates. It provides a significant advantage for path planning and navigation in dynamic environments, enabling robots to operate safely and efficiently. The package is integrated with ROS 2, making it easy to use with ROS 2 – based robots.
Readers can find further details about running Isaac ROS VSLAM and Nvblox in our blog AMR navigation using Isaac ROS VSLAM and Nvblox with Intel Realsense camera
Isaac ROS: VSLAM+Nvblox Observations
During our experiments with VSLAM, we noticed that although the global state estimation accuracy of VSLAM was satisfactory, there was a point of concern in the odometry generated by VSLAM. In Figure 7, we show the odometry poses of our AMR when it was run in a closed-loop trajectory. The poses generated by VSLAM when the AMR turns are kinematically infeasible because our AMR only has two control parameters, i.e., x-velocity and angular velocity in the z direction. The AMR is incapable of such lateral movement. This kind of behavior could be due to a lack of a motion model within the algorithm.
When running VSLAM with NVBLOX, we observed that there was a gradually increasing misalignment between the local and global cost maps generated by the NVBLOX cost map plugin. A snapshot of this is given in Figure 8. This started to negatively affect the navigation stack, wherein the AMR started observing obstacles in the local cost map where there were none. Also, the AMR was a bit unstable when aligning with a given goal pose, and under certain circumstances, the overall NVBLOX cost map got smudged to the point of no recovery.
Just like RTAB-Map, the performance in low-feature environments reduces, and the above observations are even more prominent.
Figure 7: Visual Odometry of Isaac ROS VSLAM gives kinematically infeasible poses.
Figure 8: Global and Local costmaps are misaligned
Solution and Architecture
Conclusion
In this blog, we went through two use cases where sensor fusion can be used to improve the mapping and navigation capabilities of an AMR. The first was by providing a stable odometry source to RTAB-Map, and in the second case, by providing global and local pose estimates of our AMR to Nvblox to improve mapping results. These experiments also helped with our AMR’s navigation capabilities.
Authors
