3D LiDAR SLAM – Graph SLAM Explained

Graph-based SLAM (also known as Graph SLAM) uses a graph to represent the environment and the robot’s pose estimates. It is widely used in many robotics applications like autonomous vehicles, mobile robots and unmanned aerial vehicles. This blog discusses the basics of graph SLAM, its key components, working and applications.

Key Components of Graph SLAM

Graph SLAM involves several key components, including:

1. Graph representation: The environment and robot’s pose estimates are represented as a graph, where each node represents a pose estimate, and each edge represents a measurement or a motion estimate between two poses. The graph is a mathematical model that allows us to represent and manipulate the information from the robot’s sensors and control inputs. The nodes in the graph represent the robot’s position and orientation in the environment, and the edges represent the constraints between these poses. 

2. Motion model: The motion model is a mathematical model that predicts the pose of the robot based on previous pose estimate and control input obtained from sensors like wheel encoders or accelerometers. The motion model takes into account robot’s kinematics and dynamics to estimate the robot’s position and orientation in the environment. 

3. Sensor model: The sensor model is a mathematical model that predicts the expected sensor measurement given the current pose estimate and the environment’s map. The sensor model is used to evaluate the consistency between the sensor measurements and the predicted measurements. If there is a mismatch between the predicted and actual measurements, the optimization algorithm adjusts the pose estimate to reduce the error. 

4. Optimization algorithm: The optimization algorithm estimates the robot’s pose and the environment’s map by minimizing the error between the predicted and actual measurements. The error is represented as a cost function, which is minimized using optimization techniques such as gradient descent or Gauss-Newton. The cost function is a sum of the errors between the predicted and actual measurements, and it includes terms that account for the uncertainty in the sensor measurements and the motion estimates. 

5. Error Function: Error e_i is typically the difference between the predicted and actual measurement. It is assumed that the measurement error has zero mean and is normally distributed.  

Where, z_i is the observations or predicted measurements.  

The goal is to find the Find the state x* which minimizes the error given all measurements. 

A general solution is to derive the global error function and find its nulls. Then solve the problem by iterative local linearization.  

6. Solving via iterative linearization: Linearize the error terms around the current solution (or the initial guess).  Approximate the error functions around an initial guess x via Taylor expansion. 

Now compute the first derivative of the squared error function which can be set to zero and the resulting linear system can be solved. Obtain the new state and keep iterating till we obtain a new state closer to the minimum.  

Intuition behind Graph SLAM 

• Use a graph to represent the problem 
• Every node in the graph will correspond to a pose of the robot 
• Every edge between two nodes represents a spatial constraint between them 
• A Graph-Based SLAM builds the graph and finds a node configuration that minimizes the error due to constraints. This best configuration will ensure that the real and predicted observations are as similar as possible. 

Algorithm Break-down 

Graph SLAM algorithm involves the following steps: 

1. Map initialization: The map is initialized with some prior knowledge or assumptions about the environment. The robot’s initial pose estimate is also obtained through some means, such as GPS, odometry, or motion sensors. Map initialization is important to provide a starting point for the optimization algorithm and to reduce the uncertainty in the robot’s pose estimate. 

2. Graph construction: The robot moves in the environment and takes sensor measurements, which are used to construct the graph. Each node in the graph represents a pose estimate, and each edge represents a measurement or a motion estimate between two poses. 

3. Loop closure detection: As the robot moves, it may revisit a previously visited location. This is known as a loop. Loop closure detection detects loops in the robot’s trajectory and adds new edges to the graph, connecting the previously visited pose with the current pose estimate. Loop closure detection is important to reduce the accumulation of errors in the robot’s pose estimate, which can cause the robot to lose track of its position in the environment. 

4. Optimization: The optimization algorithm estimates the robot’s pose and the environment’s map by minimizing the error between the predicted and actual measurements. This is achieved by iteratively adjusting the pose estimates and map parameters until the error is minimized. 

5. Map update: The updated map is used to improve the robot’s pose estimate and sensor measurements. This completes a single iteration of the Graph SLAM algorithm. 

6. Repeat steps 2-5 until convergence: Steps 2-5 are repeated until the error is minimized, or a certain convergence criterion is met. 

In a SLAM process, a pose-graph representation [shown in figure 4] is used to represent the environment and the robot’s pose estimates. Each node corresponds to a pose estimate, and adjacent poses are connected by edges that represent spatial constraints between robot poses based on measurements. The edges connecting consecutive poses, denoted as e_{t-1 t} , model odometry measurements, while the other edges represent spatial constraints resulting from multiple observations of the same part of the environment. 

Applications of Graph SLAM 

Graph SLAM has numerous applications in robotics, including: 

1. Autonomous Vehicles: Graph SLAM is used in self-driving cars to accurately estimate the vehicle’s position and map the environment in real-time. 

2. Mobile Robots: Graph SLAM is used in mobile robots to create maps of indoor environments and assist in navigation. 

3. Unmanned Aerial Vehicles: Graph SLAM is used in UAVs for mapping and exploration of unknown environments. 

4. Virtual Reality: Graph SLAM is used in virtual reality to map virtual environments and track the user’s movement. 

Conclusion 

In conclusion, Graph SLAM is an effective approach for simultaneous localization and mapping in robotics. It uses a graph to represent the environment and the robot’s pose estimates and optimizes the graph to estimate the robot’s pose and the environment’s map. Graph SLAM has numerous applications in robotics, including autonomous vehicles, mobile robots, and unmanned aerial vehicles. With further research and development, Graph SLAM has the potential to significantly improve the accuracy and efficiency of robotic systems. 

Ignitarium’s Robotics and Perception AI Software team brings expertise on enabling the software stack for Autonomous Navigation, SLAM, Localization, Path Planning and Perception, making reliable implementations a reality, hence driving up ROI for adopters in the long run.