1. Introduction

1.1 Why do we need autonomous navigation?

Imagine a warehouse robot moving between shelves, picking up goods precisely, and avoiding forklifts and pedestrians that suddenly appear. Or consider a self-driving car traveling safely for hundreds of kilometers in complex traffic. Both depend on a carefully designed autonomous navigation system.

The core problems solved by autonomous navigation can be simply summarized into three problems:

1. Where am I? (Localization)
2. What is around me? (Perception and mapping)
3. How should I move? (Planning and control)

These three issues are intertwined and together constitute a complete robot navigation loop.

1.2 Navigation Algorithm Stack Overview

In a complete robot navigation system, sensor data passes through successive processing layers before driving the actuators. The following diagram shows the overall data flow of the navigation algorithm stack:

%%{init: {'themeVariables': {'fontSize': '18px'}, 'flowchart': {'nodeSpacing': 24, 'rankSpacing': 32}}}%%
flowchart TB
    subgraph Sensors["Sensors"]
        direction LR
        L["LiDAR"]
        C["Camera"]
        I["IMU"]
        O["Odometry"]
    end

    subgraph Perception["Perception"]
        direction LR
        PF["Point Cloud Filtering"]
        FE["Feature Extraction"]
        SF["Sensor Fusion"]
    end

    subgraph LocalizationMapping["Localization and Mapping"]
        direction LR
        LOC["Localization<br/>EKF / PF / NDT"]
        MAP["Mapping / SLAM"]
    end

    subgraph Planning["Planning"]
        direction LR
        GP["Global Planner<br/>A* / RRT*"]
        LP["Local Planner<br/>DWA / TEB"]
        CM["Costmap"]
    end

    subgraph Control["Control"]
        PT["Path Tracking<br/>Pure Pursuit / LQR"]
    end

    subgraph Actuator["Actuation"]
        ACT["Chassis Drive"]
    end

    Sensors --> Perception
    Perception --> LocalizationMapping
    LocalizationMapping --> Planning
    CM --> Planning
    Planning --> Control
    Control --> Actuator
    Actuator -->|odometry feedback| Perception

1.3 Traditional navigation vs. learning navigation

Before the rise of deep learning, robot navigation mainly relied on modular, interpretable traditional algorithm stacks. Each module has clear responsibilities and can be independently debugged and optimized. This article will systematically introduce the core components of this algorithm stack.

This article focuses on the traditional algorithm stack. For information about end-to-end learning navigation (VLN/VLA), please refer to this site VLN Survey Series.

2. Perception

Perception is the “eyes” of the navigation system. Sensors collect raw data and process it to provide reliable input for localization, mapping and planning.

2.1 Sensor types and characteristics

LiDAR

LiDAR calculates distance by emitting laser pulses and measuring the return time (Time of Flight, ToF). It can work under any lighting conditions and output accurate spatial point cloud (Point Cloud).

2D LiDAR (such as Hokuyo UTM-30LX, SICK TiM) scans and outputs a polar coordinate point set on a plane each time, which is suitable for indoor flat environments. 3D LiDAR (such as Velodyne VLP-16, Ouster OS1) outputs a complete three-dimensional point cloud through multi-line rotation scanning, which is the core sensor for autonomous driving perception.

Depth Camera/RGB-D

RGB-D cameras (such as Intel RealSense D435, Microsoft Kinect) simultaneously acquire color images and the depth value of each pixel through the structured light or time-of-flight (ToF) principle. The main limitations are: strong outdoor light will interfere with structured light, and the detection range is limited (usually 0.3–6m).

IMU and wheel odometry

An IMU (inertial measurement unit) combines gyroscopes, which measure angular velocity, and accelerometers, which measure linear acceleration. Its output rate is high (100–1000 Hz), but errors accumulate through integration over time, causing drift.

wheel odometry calculates displacement through wheel encoders, and has good accuracy on flat roads, but will produce cumulative errors on slippery or uneven roads.

The common features of both are: high short-term accuracy, long-term use requires fusion and correction with other sensors.

2.2 Perception Data Processing

Point cloud filtering

Raw point clouds usually contain noise and irrelevant points and require preprocessing:

Voxel Grid Filter: Divide the point cloud space into regular small cubes (voxels), and replace the points in each voxel with the centroid. This not only retains the overall shape of the point cloud, but also greatly reduces the point cloud density and improves the subsequent processing speed.

Radius Outlier Removal: For each point, check the number of neighboring points within its radius r. If the number of adjacent points is less than the threshold, the point is considered to be noise and deleted. Suitable for removing isolated noise.

Pass Through Filter: Directly intercept the point cloud of the area of interest, for example, only retain points within the 0.1m to 2m height range above the ground.

Rectangle Fitting Detection

obstacle bounding-box estimation based on LiDAR point cloud: Fit the clustered obstacle point cloud to a minimum bounding rectangle (Minimum Bounding Rectangle) to estimate the length, width, orientation and center position of the obstacle. This is a classic method for obstacle perception in autonomous driving and is often used for vehicle detection.

Feature extraction

In visual SLAM and camera calibration, feature extraction is crucial:

• corner feature (Corner): such as FAST (extremely fast), Harris (classic)
• descriptor (Descriptor): such as ORB (rotation invariant + binary, fast), SIFT (scale/rotation invariant, high accuracy but slow)
• Line Characteristics (Line): for structured indoor environments (corridors, walls)

2.3 Sensor Extrinsic Calibration

When a system uses multiple sensors, it is necessary to know the Relative pose (extrinsic parameters, Extrinsic Parameters) to convert data from different sensors to the same coordinate frame.

Estimation of extrinsic parameters based on UKF: Online estimation of extrinsic parameters using unscented Kalman filter (UKF). Compared with the offline calibration of the calibration target, this method can dynamically estimate and correct the extrinsic parameters during the movement of the robot, which is suitable for scenarios where the sensor installation position may change slightly.

Sensor Time Synchronization

In addition to spatial calibration (extrinsic parameters) of multi-Sensor frames, Time synchronization Also crucial. If the data timestamps of different sensors are not aligned, it will cause the data to be “out of sync” - for example using 0.1 The IMU attitude seconds ago is used to process the laser point cloud of the current frame, and the error cannot be ignored when the robot is moving at high speed.

Hardware trigger synchronization (Hardware Trigger): All sensors are sampled at the same time through circuit signals. For example, the PPS (pulses per second) signal from GPS acts as a master clock, triggering camera shutters and LiDAR scans. This is the most accurate method, with time errors as low as microseconds, but requires dedicated hardware circuit support.

Software timestamp interpolation (Software Interpolation): When hardware triggering is not feasible, timestamp each sensor data packet through a high-precision system clock (such as NTP/PTP), and then align the data by timestamp at the software layer. A common practice is to align the IMU data to the nearest laser frame time by linear interpolation.

The impact of misaligned timestamps on SLAM: The robot continues to move during one frame of laser scanning (approximately 100 ms). If the IMU timestamp is not used for motion compensation (Motion Distortion Correction), the scanning point cloud will appear distortion (Distortion) - the first half frame and the second half frame point cloud are misaligned, seriously affecting the scanning matching accuracy. Tightly coupled solutions such as LIO-SAM solve this problem through IMU pre-integration.

2.4 Sensor Fusion

A single sensor often has limitations, and fusing multiple sensors can make up for each other’s weaknesses.

EKF (Extended Kalman Filter) fusion: Unified fusion of sensor data of different frequencies and different error characteristics (such as IMU high-frequency attitude + GPS low-frequency position + odometry displacement). EKF proceeds alternately through the prediction step (predicting the state using the motion model) and the update step (correcting the prediction using sensor observations).

UKF (unscented Kalman filter) fuses: EKF uses first-order linearization to approximate the nonlinear system, while UKF uses Sigma point sampling to more accurately approximate the mean and covariance of the nonlinear transformation, with higher accuracy in high nonlinear scenarios.

EKF Sensor fusion data flow (taking multi-sensor robot as an example):

flowchart LR
    subgraph Sensors["sensor input"]
        IMU["IMU\n100–1000 Hz\nHigh frequency, short term accuracy"]
        ODO["wheel odometry\n50–200 Hz\nAccurate on flat surfaces"]
        GPS["GPS/RTK\n1–10 Hz\nGlobal coordinates, low frequency"]
        LID["LiDAR matching\n10–40 Hz\nMedium frequency, can provide position correction"]
    end

    subgraph EKF["EKF fusion core"]
        P["prediction step\nusing kinematic models +  odometry /IMU\ncovariance P increase"]
        U["Update steps\nKalman Gain K\ncovariance P reduce"]
    end

    OUT["Fused pose\nPosition + velocity + orientation\nHigh frequency output"]

    IMU -->|Predict| P
    ODO -->|Predict| P
    GPS -->|update| U
    LID -->|update| U
    P --> U
    U --> OUT
    OUT -->|Recursive feedback| P

3. Localization

Localization solves the problem of “where am I”: given a map, the robot needs to estimate its position and orientation in the map in real time (i.e. pose Pose = position + orientation).

3.1 Problem Definition

Localization problems can be divided into two categories:

• Global Localization (Global Localization): The robot does not know the initial position and needs to determine its own pose from scratch. The most difficult, particle filtering is good at dealing with this type of problem.
• Pose Tracking: The approximate initial pose is known and is continuously corrected during movement. EKF/UKF specializes in dealing with these types of issues.
• kidnapped robot problem (Kidnapped Robot Problem): The robot was suddenly moved to an unfamiliar location during motion and needs to be repositioned.

3.2 EKF Localization

Intuitive understanding: Imagine you are walking blindfolded, estimating your position based on step count and turning angle (this is “prediction”); every time you take off the blindfold and glance at a landmark on the map, you use the landmark position to correct your estimate (this is “update”). EKF (Extended Kalman Filter) does the mathematical version of this.

state vector: $\mathbf{x} = [x, y, \theta]^T$ (position + orientation)

two steps to:

1. Prediction step (Predict): Use motion models (such as odometry data) to predict the pose at the next moment, while the error covariance increases (uncertainty increases):

1. update step (Update): Use sensor observations (such as LiDAR to see road signs) to correct the prediction, and the error covariance is reduced (uncertainty is reduced):

Here, \(\mathbf{K}_t\) is the Kalman gain, which determines whether to believe the prediction or the observation.

Intuitive understanding of Kalman gain K: Imagine that your friend tells you “You are now at the entrance of the library”, but your step count estimates that you are inside the library. Who should you trust? The size of K determines this trade-off:

• K → 1 (trust the observation): When the sensor noise is low and the prediction uncertainty is large → the observation correction weight is large
• K → 0 (believe the prediction): When the sensor noise is high → less correction, mainly relying on the motion model

Here, $\mathbf{P}$ is the prediction uncertainty (the larger → the larger K → the more confident the sensor), and $\mathbf{R}$ is the sensor noise (the larger → the smaller K → the more confident the prediction). The figure below shows the change of the uncertainty ellipse in the prediction → update process:

applicable scenarios: known map, known initial position, low non-linear system. The calculation efficiency is high and suitable for real-time operation.

3.3 UKF Localization

Difference from EKF: EKF uses Taylor expansion to make a first-order linear approximation of nonlinear functions, and the error is larger in highly nonlinear systems. UKF (Unscented Kalman Filter) approximates the probability distribution after nonlinear transformation through the carefully selected Sigma point set, without the need for derivatives and with higher accuracy.

Sigma point sampling: Extract $2n+1$ Sigma points from the current mean and covariance, propagate through the nonlinear function, and recalculate the mean and covariance.

✅ Higher accuracy than EKF, especially suitable for scenes with strong nonlinearity in motion models ❌ The computational cost is slightly larger than that of EKF (about 2–3 times that of EKF)

3.4 Particle Filter Localization

Intuition: Use thousands of “particles” (each particle represents a possible pose hypothesis) to represent the probability distribution of the robot’s position. Each particle moves according to the motion model (adding random noise), and then each particle is given a score (weight) based on the sensor observation. The closer the particle is to the real observation, the higher the weight. Finally, resampling (Resampling) is used to eliminate particles with low weight and copy particles with high weight.

AMCL (Adaptive Monte Carlo Localization): The widely used particle filtering localization package in ROS supports adaptive particle number (reducing particles after localization convergence saves calculation).

The following figure shows the three core stages of the MCL algorithm:

MCL algorithm flow:

flowchart TD
    INIT["Initialization:\nSpread particles evenly\n(global localization)\nor Gaussian distribution\n(known initial pose)"]
    MOTION["Motion updates (sampling)\nMove each particle according to the motion model\nAdd random motion noise"]
    OBS["Observation weighting\nUsing sensor data (laser/RGB-D)\nCalculate observation probability for each particle\nwt = p(zt | xt, map)"]
    NORM["normalization\nThe sum of all particle weights = 1"]
    RESAMP[" resampling \nSampling with replacement with weights N particles\n(Low weights are eliminated, high weights are copied)"]
    EST["pose estimation\nGet the particle with the largest weight / weighted mean"]
    NEXT["next moment"]

    INIT --> MOTION
    MOTION --> OBS
    OBS --> NORM
    NORM --> RESAMP
    RESAMP --> EST
    EST --> NEXT
    NEXT -->|New motion instructions| MOTION

resampling details: Naive random resampling will introduce particle diversity loss (the same particle is copied multiple times). system resampling (Systematic Resampling) takes a random starting point within $[0, 1/N]$, and then samples N times at even intervals, ensuring that each interval is sampled exactly once, effectively avoiding diversity loss, and the computational complexity is still $O(N)$.

AMCL Adaptive particle number (KLD sampling): Fixed particle number is a waste of calculation (there is no need for so many particles after localization convergence), and it is unsafe (too few particles during initialization may miss the true position). KLD Sampling Dynamically calculates the required number of particles based on the state space volume covered by the current particle set: the more fully the state space is explored (the more grids are covered), the fewer particles are needed. A typical range in AMCL is 100–5000 particles.

✅ Supports global localization (multi-hypothesis parallel, can handle kidnapped robot problem) ✅ Friendly to nonlinear systems, no need for linearization ❌ When the number of particles is large, the calculation overhead is high ❌ Efficiency decreases in high-dimensional state space (curse of dimensionality)

3.5 Localization based on scan matching

Scan matching is another type of localization idea: directly align the current LiDAR scan with the reference map (or previous frame scan) to solve the pose transformation.

NDT (Normal Distributions Transform)

idea: Divide the reference point cloud space into regular grids, and the points in each grid are represented by normal distribution (mean + covariance). The probability of the currently scanned point cloud in these normal distributions is the matching score, and the matching probability is maximized by optimizing the pose.

✅ Robust to point cloud density changes ✅ High computational efficiency (especially three-dimensional scenes) ✅ It is one of the mainstream methods of automatic driving localization (HDMap-based Localization)

ICP (Iterative Closest Point)

Idea: Match the current point cloud with the nearest point pair in the target point cloud, calculate the rigid body transformation (rotation + translation) that minimizes the distance between the matching point pairs, and then iterate and repeat until convergence.

✅ Simple and intuitive, high accuracy (after convergence) ❌ Sensitive to initial pose, easy to fall into local optimum ❌ The calculation complexity is high, and the real-time performance is affected by the point cloud density. ❌ prone to degeneracy in repetitive structures such as corridors

3.6 Comparison of localization methods

4. Mapping & SLAM

SLAM (simultaneous localization and mapping, Simultaneous Localization and Mapping) solves a “chicken or egg” problem: localization requires a map, and mapping requires knowing the location. The goal of SLAM is to complete localization and mapping at the same time when has no prior map.

4.1 Map Representations

Different scenarios require different map representations:

Binary Occupancy Grid Map

Divide the environment space into squares of equal size (usually 5–20 cm/square), and store a probability value in each square, indicating whether the square is occupied (obstructed = 1, passable = 0, unexplored = 0.5). This is the most commonly used map format in indoor robot navigation and is directly supported by ROS map_server.

Costmap

Based on the occupied grid, the area around the obstacle Inflation Create a cost layer: the closer you are to the obstacle, the higher the cost. In this way, the robot will automatically maintain a safe distance from obstacles during path planning without additional collision checks. ROS Navigation Stack costmap_2d Supports multi-layer costmap (static layer + obstacle layer + inflation layer).

Potential Field Map

The target point is regarded as the “lowest point of potential energy”, the obstacles are regarded as the “repulsive force source”, and the entire space forms a potential energy field. The robot can find the path by moving in the direction of gradient descent. Intuitively, it is similar to a ball rolling naturally to the lowest point on an incline. Main disadvantage: easy to fall into local minimum (Local Minimum).

NDT Map

The normal distribution transformation map mentioned above is suitable for high-precision autonomous driving scenarios. Each grid stores the statistical distribution of the point cloud instead of the original points, greatly compressing the storage space while maintaining localization accuracy.

OctoMap (octree 3D map)

OctoMap is the de facto standard for 3D robot mapping and is directly supported by the octomap_server package in ROS. The core idea: use octree (Octree) to recursively subdivide the three-dimensional space. Each leaf node represents a voxel (Voxel) and stores the occupation probability of the voxel.

spatial subdivision principle:

graph TD
    R["🌐 root node<br/>the whole space"]
    R --> N1["📦 child node 1<br/>spatial 1/8"]
    R --> N2["📦 child node 2<br/>spatial 1/8"]
    R --> ND["… total 8 child node"]
    R --> N8["📦 child node 8<br/>spatial 1/8"]
    N1 --> L1["🟩 Leaf node (voxel)<br/>Small enough, stop subdividing"]
    N1 --> L2["🟥 Leaf node (voxel)<br/>Small enough, stop subdividing"]
    N2 --> N21["📦 Continue to subdivide<br/>of child nodes 1/8"]
    N21 --> L3["🟨 Leaf node (voxel)"]

    style R fill:#4a90d9,color:#fff,stroke:#2c6fad
    style N1 fill:#6db8f2,color:#fff,stroke:#4a90d9
    style N2 fill:#6db8f2,color:#fff,stroke:#4a90d9
    style N8 fill:#6db8f2,color:#fff,stroke:#4a90d9
    style ND fill:#ccc,color:#555,stroke:#aaa
    style N21 fill:#a8d4f5,color:#333,stroke:#6db8f2
    style L1 fill:#52c41a,color:#fff,stroke:#389e0d
    style L2 fill:#ff4d4f,color:#fff,stroke:#cf1322
    style L3 fill:#faad14,color:#fff,stroke:#d48806

🟩 Free voxels 🟥 Occupied voxels 🟨 Unknown voxels Dark nodes = can still be subdivided

The voxel size (resolution) is typically set to 5–20 cm and can be adjusted as needed.

Occupancy probability update (Bayesian update): Each time the light from the laser/depth sensor passes through a voxel, update the logarithmic probability of occupancy (Log-Odds) of the voxel:

After accumulating multiple observations, the voxels are classified as: occupied (occupied), free (free), unknown (unknown).

Memory efficiency: OctoMap only stores occupied and free nodes, and unknown spaces do not occupy memory. Large-scale indoor scenes (200m², 10cm resolution) usually only require tens of MB, which is far better than directly storing the 3D occupancy grid.

✅ Three-dimensional obstacle perception (drone obstacle avoidance, robotic arm grabbing planning) ✅ Supports dynamic updates (probability decays after obstacles are removed) ❌ Does not retain color/semantic information (needs to be extended to ColorOctoMap / SemanticOctoMap)

Topological Map

All the aforementioned maps (occupancy grid, NDT, OctoMap) are metric maps (Metric Map) - accurately recording the geometric information of the space. The topological map is completely different: it abstracts the environment into the graph structure of nodes (locations) + edges (connected relationships), and does not care about precise geometry.

graph LR
    A["entrance hall"] -->|"Corridor 15m"| B["Office area A"]
    A -->|"stairs"| C["second floor"]
    B -->|"Corridor 8m"| D["conference room"]
    B -->|"Corridor 8m"| E["Rest area"]
    C -->|"Corridor 20m"| F["Office area B"]

node usually corresponds to a semantic location (room, doorway, corridor intersection), edge records the reachability relationship between adjacent nodes (sometimes with distance/direction information).

Hybrid scheme in practical applications: Most actual systems use topological-metric hybrid map (Hybrid Topological-Metric Map): The topology layer does high-level path planning across rooms (selecting which nodes to pass through), and the metric layer does fine local navigation and obstacle avoidance near each node. A typical implementation is the topological_navigation package of ROS.

4.2 SLAM Problem Overview

The input of SLAM is a sensor data stream (laser scan sequence/image sequence + odometry), and the output is:

1. Trajectory: Robot historical motion path
2. Map: Spatial representation of the environment

The SLAM system is usually divided into two parts: front-end (Front-end) and back-end (Back-end):

flowchart LR
    subgraph Frontend["Front-end (data association)"]
        A["sensor data"] --> B["Feature extraction\nscan match"]
        B --> C["Initial estimate of pose\n odometry"]
    end

    subgraph Backend["Backend (optimized)"]
        C --> D["Factor graph construction"]
        D --> E["loop closure detection \nLoop Closure"]
        E --> F["Graph optimization\ng2o / GTSAM"]
    end

    F --> G["Optimized trajectory\n+ global map"]

SLAM vs. Pure localization (Localization): SLAM is “locating while building a map” and is suitable for the first exploration of unknown environment. Once the map is built and saved, subsequent deployment only requires pure localization on the known map - input sensor data and output the robot’s position in the known map, without maintaining or updating the map. The computational cost of pure localization is much lower than that of SLAM: the laser solution uses AMCL (adaptive Monte Carlo localization, particle filtering) or NDT to position, and the vision solution uses Relocalization (Relocalization). In actual product deployment, the vast majority of robots are in “pure localization mode”, and SLAM is only started during the mapping phase or when there are major changes in the environment.

4.3 LiDAR SLAM

Cartographer(Google)

Project homepage

Google’s open source LiDAR SLAM system supports 2D and 3D mapping. Core idea:

• submap (Submap): Insert scanned data into local submaps in segments, and each submap maintains its own consistency
• scan matching: A new scan is coming, first use CSM (correlative scan matching) to give an initial pose, and then use Ceres Solver for fine optimization
• loop closure detection: When the robot returns to the previously explored area, loop closure is detected through brute force search matching to eliminate accumulated errors.

✅ Supports real-time 2D/3D mapping ✅ Excellent effects for large-scale indoor scenes ✅ ROS 2 has perfect support (cartographer_ros)

GMapping

Project home page

2D LiDAR SLAM based on particle filtering, each particle maintains an independent grid map and pose estimate. Using Rao-Blackwellized particle filtering, the SLAM problem is decomposed into conditionally independent localization and mapping parts.

✅ Simple to implement, suitable for small indoor scenes ✅ Good real-time performance ❌ Large scenes require large particle numbers and high memory overhead ❌ Does not support 3D mapping

LOAM(LiDAR Odometry and Mapping)

Ji Zhang et al. proposed it in 2014, which is a milestone work in 3D LiDAR SLAM. Core idea:

• Extract edge line feature (Edge) and plane feature (Planar) from point cloud
• Use these two types of features to perform scan matching and estimate inter-frame poses
• Separate two threads, high-frequency odometry (Odometry) and low-frequency mapping (Mapping), to run in parallel.

Author project homepage

LeGO-LOAM

Project homepage

A lightweight version of LOAM, specially optimized for ground mobile robot. Using ground segmentation, only features extracted from ground points and non-ground points are used, which greatly reduces the amount of calculation and can run in real time on embedded platforms (such as Jetson).

LIO-SAM

Project homepage

tightly couples laser inertial odometry, and jointly optimizes LiDAR and IMU data under the factor graph framework. Point cloud deskewing and initial pose estimation are provided through IMU pre-integration, and then laser matching is used for correction. The accuracy and robustness are better than the loosely coupled scheme.

LIO-SAM tightly coupled architecture (IMU pre-integration + LiDAR factor graph):

flowchart LR
    subgraph Input["Input data"]
        IMU2["IMU\nhigh frequency ~200 Hz"]
        LID2["3D LiDAR\n10–20 Hz"]
        GPS2["GPS(optional)\nglobal constraints"]
    end

    subgraph Frontend2["front end"]
        PREINT["IMU pre-integration\nInitial value of attitude between frames\nPoint cloud deskewing"]
        FEAT["Feature extraction\nedge line feature\nplanar features"]
        SCAN["scan match\nFeature points→map point\nLM Optimize"]
    end

    subgraph Backend2["backend(GTSAM factor graph)"]
        FG["factor graph"]
        IMUFac["IMU preintegration factor"]
        LIDFac["LiDAR odometry factor"]
        GPSFac["GPS factors (optional)"]
        LOOP2["loop closure detection factor\nKd-tree Search"]
        OPT2["iSAM2 incremental optimization"]
    end

    OUT2["Optimized trajectory\n+ Global point cloud map"]

    IMU2 --> PREINT
    LID2 --> FEAT
    PREINT --> SCAN
    FEAT --> SCAN
    GPS2 --> GPSFac
    SCAN --> LIDFac
    PREINT --> IMUFac
    IMUFac --> FG
    LIDFac --> FG
    GPSFac --> FG
    LOOP2 --> FG
    FG --> OPT2
    OPT2 --> OUT2

4.4 Visual SLAM

Visual SLAM uses cameras instead of LiDAR, which is cheaper but more sensitive to light.

ORB-SLAM3

Currently one of the most mature visual SLAM systems, supporting monocular / stereo / RGB-D / fisheye camera + IMU.

Project homepage

key technology:

• Feature extraction: ORB (Oriented FAST and Rotated BRIEF) descriptor, fast and rotation-invariant
• Tracking: The current frame matches the map point, and PnP is used to find the pose.
• Local mapping: maintain a local map and perform Bundle Adjustment optimization
• loop closure detection: Appearance similarity detection based on bag of words model (Bag of Words, BoW)

ORB-SLAM3 three-thread architecture:

flowchart TB
    CAM["camera input\n(Monocular/Stereo/RGB-D/fish eye)"]

    subgraph T1["thread①: Tracking(tracking, real-time)"]
        ORB_EXT["ORB Feature extraction"]
        POSE_EST["pose estimation\nMap point matching→PnP\nConstant speed model initial value"]
        TRACK_ST{"Tracking successful?"}
        RELOC["Relocalization\nBoW Retrieve candidate frames\nPnP + RANSAC"]
    end

    subgraph T2["thread②: Local Mapping(Local mapping, slightly slower)"]
        KF_INSERT["Keyframe insertion"]
        MAP_PT["Map point triangulation\nNew map point creation"]
        LOCAL_BA["local Bundle Adjustment\nCovisible keyframes + Joint optimization of map points"]
        KF_CULL["Keyframe culling\n90% Map points are observed in other frames → Remove redundancy KF"]
    end

    subgraph T3["thread③: Loop Closing(loop closure, slower)"]
        BOW_DETECT["BoW  loop closure detection \nSimilar frame candidates"]
        GEOM_VERIFY["Geometric consistency verification\nEssential matrix verification"]
        LOOP_FUSE["loop closure fusion\nMap point merge"]
        GLOBAL_BA["Global Bundle Adjustment\ng2o / Graph optimization"]
    end

    CAM --> ORB_EXT
    ORB_EXT --> POSE_EST
    POSE_EST --> TRACK_ST
    TRACK_ST -->|"No"| RELOC
    TRACK_ST -->|"Yes, and the keyframe conditions are met"| KF_INSERT
    KF_INSERT --> MAP_PT
    MAP_PT --> LOCAL_BA
    LOCAL_BA --> KF_CULL
    KF_INSERT --> BOW_DETECT
    BOW_DETECT --> GEOM_VERIFY
    GEOM_VERIFY --> LOOP_FUSE
    LOOP_FUSE --> GLOBAL_BA

keyframe selection strategy: ORB-SLAM3 does not insert keyframes in every frame, but is triggered according to the following conditions: ① The time from the previous keyframe exceeds the threshold; ② The number of map points that can be observed in the current frame drops below the threshold (tracking becomes weak); ③ There are not too many keyframes to be processed in the local map (to avoid backlog). This strategy ensures that keyframes are evenly distributed in space and time, avoiding redundancy.

Map point management: Each map point records which key frames it was observed in, and the ORB descriptor in each frame (the average is taken as the representative descriptor). Map points are divided into local map point (seen in recent key frames) and global map point (complete history). During tracking, only local map points are used for matching (fast), and during BA optimization, only Covisible keyframess are optimized (local BA).

Relocation after loss of tracking: When continuous frame tracking fails, the system enters relocation mode: ① Use the BoW vector of the current frame to retrieve similar frames in the key frame database; ② Use PnP+RANSAC to verify geometric consistency for candidate frames; ③ After finding the matching frame, restore the pose and reinitialize tracking. BoW retrieval is extremely fast (O(1) magnitude) and can be completed within milliseconds.

✅ High accuracy and supports multiple camera types ✅ Strong closed-loop detection capabilities in large-scale scenarios ❌ Pure vision can easily lose tracking in low light/fast motion

VINS-Mono / VINS-Fusion

The visual-inertial SLAM system is open sourced by the Hong Kong University of Science and Technology. The camera and IMU are tightly coupled, and the pose is jointly estimated through nonlinear optimization (sliding window BA). VINS-Fusion further supports stereo and GPS fusion and is a common solution in the fields of autonomous driving and drones.

✅ Tight coupling, high precision ✅ Better robustness to camera pure rotation and weak texture scenes ✅ Support online extrinsic parameters calibration

DSO (direct sparse odometry)

Project homepage

direct method A representative work that directly minimizes the grayscale of image pixels without extracting feature points. Photometric Error .

Comparison with feature method (ORB-SLAM3):

RTAB-Map(Real-Time Appearance-Based Mapping)

Project homepage

Open sourced by IntRoLab, it is currently the most widely used multi-modal graph optimization SLAM system in the ROS ecosystem. It also supports RGB-D cameras, stereo cameras and 3D LiDAR input, and outputs occupied grid maps and dense point cloud maps.

Core design: Appearance-based memory management

The most unique feature of RTAB-Map is its online memory management mechanism (Memory Management): The system divides key frames into “Working Memory (WM)” and “Long-Term Memory (LTM)”. When the number of nodes in WM exceeds the upper limit, the node that has not been accessed for the longest time is transferred to LTM (similar to memory paging in the operating system). During loop closure detection, only search within WM to ensure real-time performance; after loop closure is detected, relevant nodes are awakened from LTM to participate in optimization. This mechanism enables RTAB-Map to maintain real-time (>1 Hz) operation during large-scale long-term mapping.

key technology module:

• Appearance loop closure detection: Extract SURF/ORB features from key frames based on the Bag of Words model (BoW) and calculate the similarity between frames; when the threshold is exceeded, geometric verification is triggered (PnP + RANSAC)
• graph optimization backend: Use g2o/GTSAM to globally optimize the pose graph to eliminate cumulative drift
• multi-sensor front end:
  • RGB-D mode: ICP point cloud matching + visual odometry (VO)
  • Laser Mode: ICP/NDT Scan Match
  • Stereo mode: Parallax depth estimation + VO
• output map: 2D occupancy grid map (can be used directly in ROS Navigation Stack) + 3D color point cloud

flowchart LR
    subgraph Input["input"]
        RGBD["RGB-D / Stereo\n/ 3D LiDAR"]
        IMU2["IMU(optional)"]
    end

    subgraph Frontend["Frontend: odometry"]
        VO["Visual/LiDAR odometry\nICP / VO"]
    end

    subgraph Memory["memory management"]
        WM["working memory WM\n(Recently N frame keyframe)"]
        LTM["long term memory LTM\n(Historical keyframes, sleep)"]
        WM <-->|"Swap in/Swap out"| LTM
    end

    subgraph Loop[" loop closure detection "]
        BOW2["BoW Similarity search\n(only WM (within)"]
        GEO["Geometry verification\nPnP + RANSAC"]
    end

    subgraph Backend["Backend: Graph Optimization"]
        PG["pose graph\ng2o / GTSAM"]
    end

    subgraph Output["Output map"]
        OCC["2D occupancy grid map"]
        PC["3D Color point cloud"]
    end

    RGBD --> VO
    IMU2 --> VO
    VO -->|"new keyframe"| WM
    WM --> BOW2
    BOW2 --> GEO
    GEO -->|"loop closure constraints"| PG
    VO -->|"odometry constraints"| PG
    PG --> OCC
    PG --> PC

✅ Multi-sensor support, RGB-D/stereo/LiDAR can be used in one system ✅ Output standard occupation grid map and seamlessly connect with ROS Navigation Stack ✅ Online memory management, suitable for long-term large-scale mapping ✅ Built-in 3D point cloud map, which can be used for downstream tasks such as crawling and 3D reconstruction ❌ The default parameters consume high memory for large scenes, and parameters such as Mem/STMSize need to be tuned. ❌ Pure visual mode has reduced reliability in weak texture/low light environments

Dense Visual Mapping

ORB-SLAM3 and DSO output sparse/semi-dense point cloud, which cannot be directly used for downstream tasks such as capture planning and three-dimensional reconstruction. Dense mapping systems output complete three-dimensional surface models at the cost of higher computational effort.

TSDF Map Representations

Truncated Signed Distance Function (TSDF) is the core map representation of dense mapping. Divide the space into a grid of voxels, with each voxel storing:

• SDF value: Signed distance of this voxel to the nearest surface (positive = outside of surface, negative = inside, zero = surface)
• Weight: Confidence of cumulative observations

Here, $\delta$ is the truncation distance, and $d(v)$ is the distance from the voxel center to the nearest surface. After the multi-frame depth map is fused, the triangular mesh is extracted at TSDF=0 through the Marching Cubes algorithm, which is the reconstructed three-dimensional surface.

KinectFusion

Introduced by Microsoft Research in 2011, KinectFusion was the first system for real-time dense reconstruction using an RGB-D camera, running entirely on the GPU.

process:

flowchart LR
    RGBD["RGB-D frame"] --> ICP["ICP pose estimation\n(Point-to-plane ICP, GPU)"]
    ICP --> TSDF_UPD["TSDF voxel update\n(GPU parallel writing)"]
    TSDF_UPD --> MC["Marching Cubes\nsurface extraction"]
    MC --> MESH["triangle mesh"]

• front-end: Use GPU point-to-plane ICP to estimate camera pose in real time (no feature extraction, directly align the depth map point cloud)
• Mapping: Project the depth map along the light direction and update the TSDF voxels (GPU parallel, extremely fast)
• Limitations: The map resolution is fixed, and the scene size is limited by GPU memory (usually within 3m³); there is no loop closure detection, and long-term drift is obvious

ElasticFusion

ICCV 2015, based on KinectFusion, adds the elastic deformation (Elastic Deformation) mechanism, uses Surfel (directed surface element) to replace the voxel storage map, and supports non-rigid global optimization:

• Surfel map: Each Surfel stores location, normal vector, color, radius, and confidence
• Elastic loop closure: After detecting loop closure, instead of rigidly translating the map, non-rigid elastic deformation is applied to the map to smoothly align historical frames.
• effect: suitable for medium-scale indoor scenes (single room), the reconstruction quality is much better than KinectFusion

Dense visual mapping comparison

Neural Implicit SLAM

Since 2021, NeRF and 3D Gaussian Splatting introduced SLAM, bringing a new map representation paradigm - instead of using discrete voxels or point clouds, neural networks are used to implicitly encode the geometry and appearance of the scene.

NeRF Basics Review

Neural Radiation Field (NeRF) Use an MLP to map spatial coordinates and viewing direction into color and volume density:

The pixel color is obtained through volume rendering (Volume Rendering) integration, the photometric loss is calculated by comparing with the real image, and the network weight is optimized by backpropagation. The optimized network serves as an “implicit map” of the scene, allowing new views to be rendered from any perspective.

IMAP(2021)

The first system to use NeRF for real-time SLAM. Simultaneously optimize camera pose and scene representation with a single MLP:

• tracking: fixed network weights, optimizing the current frame pose (minimizing rendering error)
• Mapping: Fixed pose, optimized network weights (update implicit map with historical keyframes)
• Limitations: The capacity of a single MLP is limited, and details of large scenes are lost; the speed is slow and cannot be real-time

NICE-SLAM(2022)

Introduce Multi-Resolution Feature Grid to replace a single MLP and solve the capacity bottleneck of iMAP:

• Three layers of feature grids: coarse, medium and fine, respectively capture geometric information at different scales
• Local update: only update the currently observed grid area to avoid global forgetting
• Faster than iMAP, suitable for medium-sized indoor scenes (TUM, Replica datasets)

3D Gaussian Splatting SLAM(2023-2024)

3DGS-SLAM Use three-dimensional Gaussian ellipsoid (3D Gaussian) to replace NeRF’s volume density field, and the rendering speed is increased by more than 100 times (real-time rendering >30 fps), spawned a number of real-time SLAM systems:

Comparison between NeRF/3DGS SLAM and traditional SLAM:

Outlook: Neural implicit SLAM currently lags behind traditional SLAM in terms of localization accuracy and real-time performance, but the high-quality renderable map it outputs has unique value for the visual perception and simulation data generation of embodied AI (embodied AI), and is a current research hotspot.

4.5 SLAM backend optimization

The SLAM front-end gives an initial estimate of the pose for each frame, but due to noise accumulation, the error will become larger and larger after a long run. The goal of backend optimization is global consistency.

Bundle Adjustment (BA) intuitive understanding

Bundle Adjustment (beam adjustment method) is the core of visual SLAM back-end optimization. The goal is to simultaneously adjust the camera pose and the position of the three-dimensional map point so that the reprojection error of all map points across all camera frames is minimized.

Intuitive analogy: Imagine you have multiple photos of the same scene taken from different angles, and the initial 3D coordinates (with errors) of the landmarks in the scene. BA is to simultaneously fine-tune the camera position/orientation of each photo, as well as the 3D coordinates of each landmark, so that the sum of the deviations between the “projection point of the landmark calculated based on the camera parameters” and the “actually seen feature point position in the photo” is minimized.

Here, $P_i$ is the camera pose, $X_j$ is the 3D coordinate of the map point, $u_{ij}$ is the observed image coordinate, and $\pi(\cdot)$ is the projection function.

Local BA vs. Global BA trade-offs:

• Local BA (ORB-SLAM3 real-time optimization): Only optimize the latest Covisible keyframess and their observed map points, the computational cost is small, and it can run in real time (millisecond level). Disadvantage: Global drift cannot be eliminated by local BA.
• Global BA (triggered after loop closure): Optimize all key frames and map points of the entire map, with good global consistency. Disadvantages: The amount of calculation is proportional to the map size. Large scenes may take seconds or even minutes. It can only be executed offline or once after loop closure detection is triggered.

Factor Graph and graph optimization

Model the SLAM problem as a factor graph: nodes (variables) represent robot poses and map points, and edges (factors) represent sensor constraints (such as relative poses between adjacent frames, loop closure constraints). The solution process is to find the variable estimate that minimizes the sum of all constrained errors.

graph LR
    X0((X0)) -->| odometry | X1((X1))
    X1 -->| odometry | X2((X2))
    X2 -->| odometry | X3((X3))
    X3 -->| odometry | X4((X4))
    X4 -->| loop closure detection | X0
    X0 -->|GPS| G0["GPSfactor"]
    X2 -->|LiDAR| L0["laser factor"]
    style X0 fill:#4a9,color:#fff
    style X4 fill:#4a9,color:#fff
    style G0 fill:#fa4,color:#fff
    style L0 fill:#fa4,color:#fff

Loop Closure Detection

The task of loop closure detection: determine whether the robot has returned to the place it has explored before, so as to add loop closure constraints to eliminate accumulated errors. The entire process is divided into two stages: appearance retrieval (quick recall of candidate frames) and geometry verification (elimination of false recalls).

Stage 1: Appearance retrieval - Bag of Words model

The most mainstream loop closure retrieval scheme in visual SLAM is implemented by the DBoW2 / DBoW3 library (the ORB-SLAM series and RTAB-Map are all based on this).

(1) offline: building a visual dictionary

Extract ORB / BRIEF descriptors on a large number of images, and use k-means clustering (usually a hierarchical k-ary tree) to quantize the high-dimensional descriptor space into $K$ “visual words”. The dictionary is solidified once training is completed and does not need to be updated during runtime.

original descriptor space (128dimensional floating point or 256bit binary)
        ↓  Hierarchical k-means
Visual dictionary (tree structure, leaf nodes = visual word)

(2) Online: Image → BoW Vector

Extract the ORB descriptor for each key frame, and convert each descriptor quantize to the nearest visual word, count the word frequency, and weight it to get a sparse vector:

Here, TF-IDF weight $= \text{tf}(w, f) \times \log\frac{N}{n_w}$, $N$ is the total number of frames in the database, and $n_w$ is the number of frames containing the word $w$. Common words (such as edge corners) have low weights, and rare words (such as unique textures) have high weights.

(3) candidate frame retrieval

When a new keyframe arrives, use its BoW vector to compare cosine similarity with all frames in the database, and select the Top-K frames as loop closure candidates. Since the vectors are sparse and the dictionary lookup table is $O(1)$, the retrieval speed is extremely fast (on the order of milliseconds).

Key limitations: BoW is a pure appearance method, similar lighting/scenes will produce false recalls (false positive), so it must be geometrically verified.

Phase 2: Geometric consistency verification - RANSAC

For each candidate frame recalled by BoW, verify whether the 3D geometry between the current frame and the candidate frame is self-consistent.

(1) feature point matching

For the current frame and the candidate frame, do brute-force matching (Brute-Force Matching) based on the ORB descriptor, and obtain a batch of corresponding point pairs ${(p_i, p_i’)}$.

(2) RANSAC estimated essential matrix / homography matrix

Since matching point pairs contain a large number of Outlier (Mismatching caused by occlusion and repeated textures), directly solving with all points will result in wrong geometric relationships. RANSAC’s approach:

Repeat N Times:
  1. Randomly sample the minimum set of points (the essential matrix requires 5 point, the homography matrix requires 4 points)
  2. Solve with minimum point set E or H matrix
  3. Statistically satisfied E/H interior points of (inlier)Quantity
Return the solution with the largest number of interior points

essential matrix $E$ (pure rotation + translation scene, no plane assumption) satisfies the epipolar constraint:

Here, $R$ is the rotation matrix, and $t^\wedge$ is the antisymmetric matrix of the translation vector.

(3) inlier threshold judgment

• Number of interior points $\geq$ Threshold (such as 30) → Confirm loop closure, calculate the relative pose of the current frame relative to the candidate frame $T_{loop}$, and add it as a constraint to the pose graph
• Insufficient inliers → rejects candidate, no loop closure is generated this time

flowchart LR
    KF["new keyframe"] --> BOW["BoW Search\nTop-K candidate frame"]
    BOW --> MATCH["ORB descriptor\nBrute force matching"]
    MATCH --> RANSAC["RANSAC\nestimate E / H"]
    RANSAC --> CHECK{"Inliers ≥ threshold?"}
    CHECK -->|"Yes"| LOOP["Confirm loop closure\nAdd pose graph constraints"]
    CHECK -->|"No"| REJECT["reject candidate"]
    LOOP --> OPT["Trigger global graph optimization\n(g2o / GTSAM)"]

Loop closure detection for LiDAR SLAM

LiDAR SLAM has no image and cannot use BoW. The following solutions are commonly used:

Scan Context is the mainstream solution for laser loop closure detection in recent years: project the 3D point cloud onto a top-view polar coordinate grid, record the maximum height value for each grid, and generate a fixed-size 2D matrix as a “point cloud fingerprint”. The similarity between two frames is rotationally invariant via column shift search, and the retrieval speed is much faster than frame-by-frame ICP.

Main backend optimization libraries

• g2o (General Graph Optimization): General graph optimization library, used by ORB-SLAM2/3
• GTSAM (Georgia Tech Smoothing and Mapping): factor graph optimization library, used by LIO-SAM
• iSAM2 (Incremental Smoothing and Mapping 2): Incremental optimization algorithm in GTSAM, supporting real-time updates without re-optimizing the entire graph each time

4.6 Laser vs. Visual SLAM comparison

RTAB-Map spans two columns: it supports laser and visual dual-mode input, and its accuracy and applicable scenarios are between the two. It is the most comprehensive out-of-the-box solution in the ROS ecosystem.

4.7 SLAM degeneracy scenarios and actual deployment challenges

SLAM systems often perform well in laboratory environments, but encounter various degeneracy scenarios in real deployments, resulting in localization failures or map errors. Understanding these scenarios and selecting algorithms accordingly is the key to engineering.

4.7.1 Geometric Degeneracy

Corridor degeneracy (Corridor Degeneracy): LiDAR SLAM faces severe challenges in long corridors. The walls on both sides of the corridor are highly symmetrical, and the characteristics of the laser point cloud along the corridor direction are almost the same, resulting in the displacement along the corridor direction being unable to be constrained (the normal direction constraint of scan matching is missing). The performance is as follows: drifting along the corridor and good horizontal localization.

Countermeasures:

• Combined with IMU or wheel odometry to constrain movement along corridors
• Supplementary constraints with ceiling and floor structures using 3D LiDAR
• LIO-SAM’s IMU pre-integration provides constraints in the degeneracy direction

Open outdoor degeneracy: In environments that lack three-dimensional structures such as large parking lots and fields, the laser point cloud is sparse and NDT/ICP convergence is difficult. Countermeasure: Fusion of GPS global constraints.

4.7.2 Dynamic environment challenges

SLAM assumes that the environment is static, but in reality pedestrians, vehicles, and moving furniture will cause dynamic interference:

• Fake map point: Dynamic obstacles are incorrectly built into the static map
• tracking failure: A large number of dynamic objects cause scan matching failure
• loop closure false detection: BoW recalls wrong keyframes after scene layout changes

Countermeasures:

• Point cloud dynamic object filtering (based on motion consistency detection)
• Semantic segmentation filters point clouds for dynamic categories (pedestrians, vehicles)
• Using Motion Segmentation in Visual SLAM

4.7.3 Light and weather effects (visual SLAM)

Visual SLAM is extremely sensitive to light:

Countermeasures:

• Visual-Inertial SLAM (VINS-Mono/ORB-SLAM3 IMU mode): IMU maintains short-term localization when feature tracking fails
• HDR camera or active lighting (structured light) adapts to lighting changes
• Event Camera is immune to motion blur and is a current research hotspot

4.7.4 Long-term map maintenance and scene changes

Long-term deployment faces the problem of map aging: seasonal changes (falling leaves, snow), renovations, furniture movement, etc. will make the original map invalid.

Mitigation strategies:

• Incremental map update: When the sensor observation is inconsistent with the existing map beyond the threshold, a local map update is triggered
• Multiple map management (ORB-SLAM3 support): Create new submaps when scene switching or tracking is lost, merge after recovery
• Semantic Map: Replace original points with semantic labels (door/wall/column), semantic features are more stable than geometric features

4.7.5 Embedded platform computing constraints

Robots are often equipped with embedded platforms (Jetson Nano / Xavier / Orin) with limited computing power, and a complete SLAM system requires higher computing power:

Practical suggestions:

• Prioritize the use of lightweight systems: LeGO-LOAM (embedded friendly), AMCL (low computing power localization)
• Front-end (feature extraction, scan matching) should be accelerated on GPU as much as possible
• Back-end optimization can run at reduced frequency (10 Hz front-end + 1–2 Hz back-end BA)
• Use iSAM2 incremental optimization to avoid full-graph re-optimization

Note: More details in the real deployment of visual SLAM (scenario adaptability research related to TITS 2026) will be added later.

4.8 Summary of commonly used datasets

5. Path Planning

Path planning solves the problem of “how should I go”: find a collision-free path from the starting point to the end point in a known (or locally known) map.

5.1 Global Path Planning: Search Methods

The search algorithm searches for the optimal path on the discretized grid map.

Dijkstra’s algorithm

idea: Starting from the starting point, like ripples spreading, gradually explore all reachable nodes in the order of cost from small to large until the end point is found. Guaranteed to find the path with the least cost (optimality).

✅ Guaranteed optimal solution ❌ No directionality, large number of nodes to expand on a large map, low efficiency ❌ Time complexity $O(V \log V + E)$, $V$ is the number of nodes, $E$ is the number of edges

A* (A-Star) algorithm

idea: Add the heuristic function h(n) (estimate the cost from the current node to the end point, usually using Euclidean distance or Manhattan distance) based on Dijkstra, so that the search has a clear direction and priority is given to exploring nodes that “look closer to the end point”.

Here, $g(n)$ is the actual cost from the starting point to the node $n$, and $h(n)$ is the estimated value of the heuristic function.

✅ Guaranteed optimal solution (when $h(n)$ does not overestimate the actual cost) ✅ Much faster than Dijkstra (directional search) ❌ The amount of calculation in high-dimensional space (such as 3D) is still large

Selection of heuristic function h(n): Different heuristic functions are suitable for different movement constraints:

Admissibility: $h(n)$ Never overestimate the true cost and ensure that A* finds the optimal solution. If $h(n) = 0$, A* reduces to Dijkstra (the slowest and optimal). The larger $h(n)$, the faster it is, but it may be overestimated and lose optimality.

The following figure shows the search process of A* on the 10×10 grid:

Bidirectional A*

Search in both directions from the start and end points simultaneously, stopping when the two search wavefronts meet. The average number of search nodes is about half that of one-way A*, which is suitable for situations where the start and end points are far apart.

Hybrid A*

Problem with standard A*: Pathfinding on the grid, ignoring the kinematic constraints of the vehicle. A car cannot move sideways, it has a minimum turning radius.

Improvement of Hybrid A*: Discretize the continuous state space of the vehicle ($x, y, \theta$), consider executable steering operations (such as arcs with different curvatures) when expanding each node, and ensure that the generated path is suitable for vehicles with nonholonomic constraints (differential wheeled/Ackermann ) is actually feasible.

✅ Generate kinematic feasible paths ✅ Suitable for parking scenes and narrow passages ❌ The computational cost is larger than standard A* ❌ Need to be combined with post-processing smoothing such as Reeds-Shepp curve

A* algorithm engineering optimization

In actual engineering applications (such as autonomous driving or mobile robots), the paths generated by the original A* often have problems such as redundant nodes and sharp corners. Mainstream optimization strategies include:

1. adaptive heuristic function weight: Introduce the dynamic weight coefficient $\lambda$ to make the heuristic function change with the position: \(f(n) = g(n) + (1 + \lambda \cdot \frac{d_{to\_end}}{d_{total}})h(n)\) Increase the weight when approaching the starting point to increase the search speed (similar to greedy search); decrease the weight when approaching the end point to ensure the optimality of the path.

2. Path Simplification (Douglas-Peucker Algorithm): The raw paths produced by A* often contain a large number of collinear redundant nodes. The Douglas-Peucker (DP) algorithm can eliminate unnecessary intermediate points while maintaining the path topological characteristics, greatly simplifying the path description, and improving the efficiency of subsequent trajectory smoothing.

3. Quadratic smoothing (B-spline curve): For the streamlined key points, use B-Spline or fifth-order polynomial for fitting. This converts the polygonal path of A* into a continuous, higher-order differentiable smooth arc, ensuring that it adheres to the dynamic constraints of the chassis (such as centripetal acceleration limits).

Fast Marching Method (FMM)

Idea: FMM was proposed by J.A. Sethian in 1996, which models the path planning problem as a wavefront propagation problem. Propagate the “time wavefront” from the target point to the surroundings, record the shortest time $T(\mathbf{x})$ for the wave to arrive at each grid, and construct a global arrival time field; during planning, gradient descent in the $-\nabla T$ direction from the starting point can be used to obtain the global optimal path.

Eikonal Equation:

Here, $T(\mathbf{x})$ is the minimum time (path cost) for the wave to reach the point $\mathbf{x}$ from the target, $F(\mathbf{x}) > 0$ is the propagation speed of the point - free space $F=1$, near the edge of the obstacle $F \to 0$, through Costmap’s distance transform assignment naturally incorporates terrain costs.

core algorithm steps (narrow-band propagation, Dijkstra-like):

1. initializes: target point $T_{goal}=0$, all other nodes $T=\infty$; add the target point to the min-heap
2. pops up the top of the heap (the current smallest node $T$), marked as “determined”
3. For each undetermined neighbor, use the Eikonal update formula to calculate the candidate arrival time:

Assume $T_h = \min(T_{left}, T_{right})$, $T_v = \min(T_{up}, T_{down})$ (take the determined minimum value in adjacent directions), then:

1. If $T_{new}$ is smaller, insert (or update) the neighbor into the min-heap
2. Repeat steps 2–4 until the heap is empty

path extraction: starting from the starting point, perform gradient descent along the $-\nabla T$ direction. Since the Eikonal equation guarantees that the $T$ field is monotonic and has no local minimum, the gradient descent will definitely converge to the target point and generate the global optimal path.

✅ Path naturally smooth: continuous wave front propagation, no raster aliasing ✅ is built once, and the whole picture can be checked.: After the $T$ field is built, any starting point can be directly gradient-descent, which is suitable for querying the same target multiple times. ✅ Naturally integrate non-uniform terrain cost through $F(\mathbf{x})$ (slope, ground type, etc.) ✅ is the global optimal (continuous space meaning), and there is no local minimum problem of the traditional potential field method. ❌ Heuristic function cannot be used to accelerate, must traverse the entire map, $O(N \log N)$ ($N$ is the total number of rasters) ❌ single-end single-end planning that is not suitable for very large maps (A* is faster) ❌ When the resolution is low, the gradient descent path is still slightly jagged and needs to be smoothed by post-processing.

Comparison between and other algorithms:

Typical applications: terrain path planning (rover non-uniform ground), ROS navfn full-grid potential field construction, image distance transformation and skeleton extraction, computer graphics geodesic distance calculation.

Fast Marching Square(FM²)

FM² was proposed by Garrido et al. (2006) on the basis of FMM. The core idea is twice in series with FMM. By constructing a velocity field proportional to the distance to the obstacle, the wavefront can automatically “bypass” the dangerous area and plan a natural and safe smooth path.

Two-stage algorithm:

1. first stage - construct velocity field $W(\mathbf{x})$: run with all obstacle grids as the source point ($T=0$) FMM, obtain the arrival time distance field from each point in the whole graph to the nearest obstacle; normalize the distance field as the velocity field:

   \[W(\mathbf{x}) = f\!\left(d_{\text{obs}}(\mathbf{x})\right), \quad W \in (0,\,1]\]

The closer to the obstacle, the smaller the $W(\mathbf{x})$ (the lower the speed); the $W \to 1$ is at the center of the free space.

1. Second stage - path planning: Run FMM again with the target point as the source point and $W(\mathbf{x})$ as the propagation speed, and calculate the arrival time field $T(\mathbf{x})$; the path is obtained by descending along the $-\nabla T$ gradient from the starting point.

Why the path is naturally safe: near obstacles $W$ low → slow wave propagation → $T$ value high → automatically detours to $W$ large (away from obstacles) “high-speed zone” during gradient descent, maintaining a safety margin without any post-processing.

✅ The velocity field inherently encodes path safety, without the need to expand obstacles or set safety radius parameters ✅ Both FMMs ensure global optimality and smooth paths ✅ Applicable to any dimensional space (also applicable to high-dimensional planning of robotic arms) ❌ Two full-graph FMMs, twice as much calculation as standard FMM ❌ The velocity field may suppress the velocity too low in a narrow channel, resulting in an overly conservative path

Main variants:

5.2 Global Path Planning: Sampling Methods

The sampling algorithm constructs a path by random sampling. It does not require explicit rasterization of the map and is suitable for high-dimensional space and complex geometric constraint scenarios.

RRT (Rapidly-exploring Random Tree)

Idea: Grow a tree from the starting point, randomly pick a point each time, find the nearest node on the tree, extend a small step towards the random point, and join the tree if there is no collision. A path is found when a node of the tree is close enough to the end.

Core steps:

1. Initialization: tree $\mathcal{T}$ with starting point only $x_{start}$
2. randomly samples $x_{rand} \sim \mathcal{U}(\mathcal{X})$ (sometimes $p_{goal} \approx 5\%$ directly samples the end point with a certain probability to accelerate convergence)
3. Nearest neighbor $x_{near} = \arg\min_{x \in \mathcal{T}} |x - x_{rand}|$
4. step $x_{new} = x_{near} + \delta \cdot \frac{x_{rand} - x_{near}}{|x_{rand} - x_{near}|}$, where $\delta$ is the step size (step_size)
5. collision detection: If $x_{near} \to x_{new}$ has no obstacle, then $x_{new}$ will be added to the tree with the edge weight of $\delta$
6. Repeat until $|x_{new} - x_{goal}| < \epsilon$

Key parameter step_size: If it is too large, it will fail to cross the obstacle. If it is too small, the convergence will be extremely slow. It is usually taken as 1%–5% of the diagonal of the map.

✅ Naturally handle high-dimensional space (robotic arm planning) ✅ No rasterization required ❌ does not guarantee optimality (the path found is usually more tortuous) ❌ The final path requires additional smoothing (commonly used B-Spline or Shortcut smoothing)

RRT*

The improved version of RRT adds two key steps to the standard RRT: nearest neighbor Choose Parent and Rewiring.

Choose Parent: Rather than selecting the nearest neighbor directly, consider the nodes in the radius-$r$ neighborhood \(\mathcal{X}_{near}\) and choose the parent with the lowest cost from the start:

Rewiring: After adding $x_{new}$, check every node $x_{near}$ in \(\mathcal{X}_{near}\). If routing through $x_{new}$ reduces the cost of reaching $x_{near}$, remove the old parent edge of $x_{near}$ and make $x_{new}$ its new parent.

The search radius $r$ shrinks with the number of sampling points $n$: $r(n) = \gamma \left(\frac{\log n}{n}\right)^{1/d}$ ($d$ is the spatial dimension), ensuring asymptotic optimization while controlling the amount of calculation.

✅ Asymptotic optimality (the more paths sampled, the better) ✅ Shares the same sampling framework as RRT, easy to implement ❌ Each time a new node is added, it needs to traverse the neighbor set, and the single-step time complexity $O(\log n)$ is higher than that of RRT’s $O(1)$ ❌ The convergence speed is slow, and the sampling time may not be enough during real-time planning.

Bidirectional RRT*

Grow an RRT* tree (\(\mathcal{T}_a\), \(\mathcal{T}_b\)) from the starting point $x_{start}$ and the end point $x_{goal}$. Each iteration alternately expands the two trees:

1. Perform one-step RRT* expansion on \(\mathcal{T}_a\) to obtain the new node $x_{new}$
2. Try connecting $x_{new}$ to its nearest node $x_{b,near}$ in \(\mathcal{T}_b\) with a collision-free path
3. If the connection is successful, merge the two sub-paths to obtain the candidate complete path; retain the complete path with the smallest cost
4. The roles of the two trees are reversed (\(\mathcal{T}_a \leftrightarrow \mathcal{T}_b\)) and continued iterative optimization

The advantage of comes from: two trees “grow in opposite directions”, which effectively avoids the blind diffusion of one-way trees in a wide space. The search volume is reduced from $O(r^d)$ to $O(2 \cdot (r/2)^d)$, and the convergence speed is about one order of magnitude faster than one-way RRT*.

Informed RRT*

Further improvement: When an initial solution $c_{best}$ is found, the sampling is limited to the elliptical area. The two foci of this ellipse are $x_{start}$ and $x_{goal}$, the major semi-axis is $a = c_{best}/2$, and the minor semi-axis is:

Intuition: Only points within the ellipse can make the path cost lower than $c_{best}$ (the definition of the ellipse is exactly the sum of the distances to the two focal points $\leq c_{best}$). As the path continues to be optimized, $c_{best}$ decreases, the ellipse continues to shrink, and the sampling becomes more and more “accurate” to avoid wasting sampling in invalid areas.

When sampling, the uniform random point $x_{ball} \sim \mathcal{U}(\mathcal{B}^d)$ within the standard circle is mapped to the elliptical coordinate frame through affine transformation: $x_{rand} = C \cdot L \cdot x_{ball} + x_{center}$, where $C$ is the rotation matrix (making the main axis of the ellipse align with the $x_{start} \to x_{goal}$ direction), $L = \text{diag}(a, b, \ldots, b)$.

5.3 Local path planning (dynamic obstacle avoidance)

Global planning assumes that the map is static, but in reality there will be dynamic obstacles (pedestrians, other robots). Local planning re-plans in real time during robot movement to handle dynamic obstacles.

DWA (Dynamic Window Approach)

idea: Sampling the speed command $(v, \omega)$ in the reachable speed window (limited by acceleration) around the current speed of the robot, predict each trajectory with the motion model, according to target direction + speed + Obstacle distance is comprehensively scored, and the optimal speed command is selected for execution.

Construction of dynamic window: Velocity space $(v, \omega)$ needs to satisfy three constraints at the same time, and take the intersection:

• speed limit: $v \in [v_{min}, v_{max}]$, $\omega \in [\omega_{min}, \omega_{max}]$
• dynamic window (acceleration limit): \(v \in [v_c - \dot{v}_{max} \cdot \Delta t,\ v_c + \dot{v}_{max} \cdot \Delta t]\), $\omega$ similar
• Accessibility constraint: The distance to the nearest obstacle on the trajectory $> 0$ (and the robot can brake before reaching the obstacle)

scoring function:

The meaning of the three components:

• $\text{heading}$: The angle difference between the end of the trajectory and the direction of the target point (the smaller, the better, to encourage the robot to turn to the target)
• $\text{dist}$: Minimum distance from the nearest obstacle on the trajectory (the bigger the better, encourage staying away from obstacles)
• $\text{velocity}$: Line speed $v$ itself (bigger is better, encourages fast forwarding)
• $\sigma(\cdot)$ is the normalization function, $\alpha, \beta, \gamma$ is the weight

✅ Fast calculation (ms level), suitable for real-time obstacle avoidance ✅ ROS dwa_local_planner works right out of the box ❌ Speed search space is limited and easy to fail in narrow channels ❌ Only considers short-term trajectories (about 1–3 s) and cannot handle obstacles that require “detours”

Improved variant: Fuzzy DWA (Fuzzy DWA) The evaluation weight of traditional DWA ($\alpha, \beta, \gamma$) is fixed, making it difficult to balance “high-speed driving” and “narrow obstacle avoidance”. fuzzy DWA introduces a fuzzy inference engine, taking the target distance and nearest obstacle distance as inputs to dynamically adjust the sampling weight. For example: when the obstacle is very close, greatly increase $\beta$ (obstacle avoidance weight) and reduce $\gamma$ (speed weight) to make the obstacle avoidance behavior smoother and less stiff.

TEB (Timed Elastic Band)

Idea: Treat the path as a “rubber band”, and add the time dimension to become a “timed elastic band”. TEB models the local planning problem as a sparse nonlinear least squares optimization:

State representation: the path is represented by a series of pose sequences with timestamps \(\mathcal{B} = \{x_i, \Delta T_i\}_{i=1}^{n}\), where $x_i = (p_x, p_y, \theta)$ and $\Delta T_i$ are the time intervals between adjacent waypoints.

Optimization goal (weighted sum of multiple constraints):

The meaning of each constraint item:

The optimization uses the g2o sparse graph optimization framework: each waypoint is a node and each constraint is an edge. Levenberg–Marquardt or Gauss-Newton is used to iteratively solve the problem. Usually 10–30 times iterations can converge.

✅ Generate smooth, kinematically feasible trajectories ✅ Support dynamic obstacles (obstacles are also modeled as nodes in the graph) ✅ Support reverse (allow $v < 0$) ❌ The computational cost is larger than that of DWA, about 50–200 ms ❌ Parameter tuning is complicated (constraint weights $\gamma_k$ interact with each other) ❌ Sensitive to the quality of the initial path. When the global planning result is poor, the local area may fall into the local optimum.

Potential Field Method

The simplest and most intuitive method of local obstacle avoidance: end point generation attractive force field , obstacles are generated repulsive force field , the robot moves downward along the gradient direction of the potential field.

attractive force potential (parabolic shape, the farther away from the target, the greater the attractive force):

repulsive force potential (effective when $d(q, O) < Q^*$ away from the obstacle):

The resultant force received by the robot is a negative gradient: $F(q) = -\nabla U_{att}(q) - \nabla U_{rep}(q)$, and it moves in the direction of the resultant force.

The cause of the local minimum: In an obstacle-dense area, the attractive force direction at a certain point is exactly the same size and opposite to the repulsive force direction, the gradient is zero, and the robot falls into a “false end point”.

Mitigation method: random disturbance (Random Walk), increasing the potential weight of the global target, combined with global planning to lead the way, etc.

✅ Simple to implement and extremely fast to calculate (O (number of obstacles)) ✅ Can generate continuous force instructions, suitable for direct coupling with the controller ❌ local minimum problem (the robot may be stuck at the equilibrium point of attractive force and repulsive force) ❌ The repulsive forces on both sides of the obstacle in a narrow passage are superimposed. The resultant force is perpendicular to the direction of the passage and the robot cannot pass. ❌ When approaching the target, the attractive force approaches zero. If there is still repulsive force, the robot may not reach the end point.

MPPI (Model Predictive Path Integral)

Model Predictive Path Integral idea: It is a random variant of model predictive control (MPC). At the current moment, forward sample a large number of random control sequences (through GPU parallel sampling), use the motion model to simulate the future state of each trajectory, calculate the information-theoretic weighted average according to the trajectory cost as the current control output, and then repeat the receding horizon.

algorithm flow:

1. Current control sequence $U = {u_0, u_1, \ldots, u_{T-1}}$, sampled disturbance sequence $K$ $\epsilon^{(k)} \sim \mathcal{N}(0, \Sigma)$
2. For each trajectory $k$, parallel forward simulation $T$ steps are performed to calculate the cost: \(S^{(k)} = \sum_{t=0}^{T-1} \left[ c(x_t^{(k)}) + \lambda \, u_t^\top \Sigma^{-1} \epsilon_t^{(k)} \right] + \phi(x_T^{(k)})\) Here, $c(x_t)$ is the state cost (collision penalty, path deviation, etc.), $\phi$ is the terminal cost, and $\lambda$ is the temperature parameter.
3. Calculate the exponential weight of each trajectory (the lower the cost, the greater the weight): \(w^{(k)} = \frac{\exp\!\left(-\frac{1}{\lambda} S^{(k)}\right)}{\sum_{j=1}^K \exp\!\left(-\frac{1}{\lambda} S^{(j)}\right)}\)
4. Weighted average update control sequence: $u_t \leftarrow u_t + \sum_k w^{(k)} \epsilon_t^{(k)}$
5. Execute $u_0$, move the sliding window forward one step, repeat

Temperature parameter $\lambda$ controls the “exploration-exploitation” trade-off: when $\lambda \to 0$, almost only the trajectory with the lowest cost is used (greedy), when $\lambda \to \infty$, all trajectories are equally weighted (purely random).

✅ No need to solve the optimal control problem (just forward simulation, no gradient) ✅ Naturally supports nonlinear systems and non-convex cost functions (such as step costs of collisions) ✅ GPU parallel sampling ($K$ up to $10^3 \text{–} 10^4$), can handle complex obstacle distribution ❌ A relatively accurate motion model is required (accumulation of simulation errors will lead to trajectory deviation) ❌ The amount of calculation is large, and a GPU is usually required to achieve real-time control frequency ($\geq 10$ Hz)

Parameter Setting Guide:

The core parameters of MPPI can be divided into three groups: sampling scale, exploration intensity, and cost design.

① Sampling scale: $K$ (number of trajectories) × $T$ (number of prediction steps) × $dt$ (step size)

The three jointly determine the computational cost and planning quality, and are the first parameters that need to be determined.

experience: First fix $dt = 0.05$ s, set $T \times dt$ to 1.5 times the time required for the robot to pass the longest obstacle corridor at maximum speed, and then decide $K$ based on the GPU computing power.

② Exploration intensity: $\lambda$ (temperature) and $\Sigma$ (noise covariance)

$\Sigma$ determines the amplitude of trajectory perturbation, and $\lambda$ determines the degree to which high-cost trajectories are suppressed.

adjustment idea: $\sigma_v, \sigma_\omega$ is first set to 30%–50% of the speed upper limit, and then the coverage of the trajectory bundle (trajectory bundle) is observed through simulation. If the trajectories are all piled together, the noise will be increased, and if the trajectories are extremely divergent, the noise will be reduced. $\lambda$ is adjusted between 0.1–1.0, using a small value (more conservative) for scenes with dense obstacles, and a large value (smoother) for open scenes.

③ Critics (cost evaluator) - Nav2 MPPI’s cost plugin system

Nav2’s nav2_mppi_controller splits the cost function $c(x_t)$ into a set of pluggable critic plug-ins. Each critic independently calculates a score, and the final weighted sum is:

Each Critic can be switched individually through enabled: true/false, and weight controls its proportion in the total cost. Nav2’s built-in Critics are as follows:

Relative weighting: The sum of the weights of the obstacle class (ObstaclesCritic + CostCritic) should usually be greater than the path tracking class (PathAlignCritic + PathFollowCritic), otherwise the robot will crash into dynamic obstacles in order to follow the path.

Recommended tuning sequence:

1. Retain only ObstaclesCritic + CostCritic, and adjusts it so that the robot can bypass obstacles without colliding.
2. Add PathAlignCritic + PathFollowCritic and adjusts the robot to follow the global path
3. Add GoalCritic + GoalAngleCritic to ensure the correct pose when reaching the goal
4. Join TwirlingCritic / PreferForwardCritic on demand to fix specific motion defects
5. Do not adjust multiple weights at the same time. Only change one at a time, and observe changes in behavior before continuing.

Example YAML configuration:

FollowPath:
  plugin: "nav2_mppi_controller::MPPIController"
  batch_size: 2000
  time_steps: 56
  model_dt: 0.05
  vx_std: 0.5
  wz_std: 0.5
  temperature: 0.3
  critics:
    - plugin: "nav2_mppi_controller::ObstaclesCritic"
      enabled: true
      weight: 10.0
    - plugin: "nav2_mppi_controller::PathAlignCritic"
      enabled: true
      weight: 14.0
    - plugin: "nav2_mppi_controller::GoalCritic"
      enabled: true
      weight: 5.0
    - plugin: "nav2_mppi_controller::TwirlingCritic"
      enabled: true
      weight: 10.0

Whole-Body Collision Checking

The previous DWA / TEB / potential field method / MPPI all approximate the robot as a point or a circle, and then expand the obstacles according to a fixed radius. This approximation is accurate for circular chassis (sweepers, circular AGVs); but for quadruped robots, forklifts, and long-strip AGV, any value for the inflation radius is wrong:

(The example body is 0.70 m × 0.31 m, which is close to the body size of Unitree Go2)

idea: Use a small number of geometric primitives to cover the robot body, so that the collision area changes with the orientation. A common practice on quadruped robots is double cylinder coverage: place two vertical cylinders along the longitudinal axis of the body, and pre-expand the occupancy map according to the cylinder radius and upper and lower clearance, so whole-body collision checking reduces to querying the voxels where the two circle centers are located. The two point queries are almost the same as the point model, but the accuracy is close to the rectangular model.

key parameters:

The pair of $d_{up}$ and $d_{down}$ is something that the 2.5D elevation map cannot do: the elevation map flattens the tabletop into a “terrain”, and it is impossible to express whether you can get under the table; and the upper and lower clearances clearly distinguish between “there is something above your head but it is high enough” and “there is something under your feet but it is low enough”.

Where does orientation come from? Including yaw as an optimization variable increases the state dimension and solver complexity. A less expensive approach derives orientation from the local tangent of the position trajectory:

Here, $Q_i$ is the B-spline control point, and $Q_{i+1} - Q_{i-1}$ is the finite difference approximation of the local motion direction.

Why this can be done: The heading of the quadruped robot can be decoupled from the direction of movement, but aligning the body with the direction of movement can exactly minimize the lateral projection in the narrow channel - this is not only the most natural behavior during execution (the controller can just track the trajectory tangent), but also makes the collision detection consistent with the actual execution, without any additional yaw control variables.

3D planning of ground robot: z-direction gradient suppression

Directly optimizing ground robots as “flying 3D rigid bodies” will lead to an absurd result: the planner found Jump over the obstacle The path is the shortest, so lift the trajectory upwards. This is fine for drones, but completely unfeasible for ground robots that must be supported by contact.

The correct approach is to avoid obstacles Mainly occurs in the horizontal direction , the vertical direction only follows the terrain trend:

1. Height regularization initialization - first use polynomial to generate the initial path, and then redistribute the height linearly between the starting point and the local target according to cumulative horizontal arc length. The $xy$ projection remains unchanged, and the $z$ becomes a linear profile along the horizontal travel distance, serving as a stable vertical baseline before optimization.
2. Projection A* Guide - After detecting the collision segment, the detour path is not searched in the complete 3D grid, but is limited to a inclined search surface that is interpolated from the height of the two end points. Each search node is only represented by “horizontal grid coordinates + interpolated height”, the search space is reduced from 3D to 2.5D, and it naturally fits the terrain slope.
3. z Gradient suppression - suppress the vertical component during optimization and update, and convert all repulsive force into lateral deformation.

Without this step, the 3D planner would give a short path “flying over the obstacle” in the stair scene; with it, the trajectory retains the climbing tendency induced by the stair while avoiding the obstacle laterally.

Local map boundaries and dead-end recovery

During long-distance navigation, the robot only maintains one Robot-centered sliding occupancy map (A fixed-size circular buffer, when the map is translated, only the replaced address is replaced and the overlapping area is retained to achieve zero copy). This brings up an old problem that DWA/TEB also encounters: The local planner gets stuck when a subgoal falls behind a newly observed obstacle, or simply outside the map boundaries. .

The boundary fallback strategy converts “planning failure” into “bounded recovery actions”:

1. Temporarily add a layer outside the rolling map boundary One-pixel wide virtual free layer , is only used to generate hypothesis paths and is not considered an executable free space;
2. If the target is unreachable within the map, A* search is allowed to enter the virtual layer;
3. Take the boundary voxel where the hypothesis path leaves the rolling map as the fallback target and replace the original local target;
4. Generate final paths with full-body collision detection withwithin the actual rolling map.

The effect is: the robot will not shake in place in a dead end. Instead, it will walk towards the edge of the map and include the new area for observation, and the next round of re-planning will naturally get out of trouble.

✅ The collision accuracy is close to the rectangular/polygon model, and the cost is close to the point model (only 2 voxels are checked for each detection point) ✅ The upper and lower clearances can clearly distinguish between overhead obstacles and low obstacles that can be crossed, which is not possible with the 2.5D elevation map. ✅ Orientation is induced by the trajectory tangent and does not increase optimization variables ✅ Sliding map + boundary fallback, the memory is bounded and you can get out of trouble under long-distance navigation ❌ The double cylinder is still a conservative coverage and is not accurate enough for a more irregularly shaped body (a mobile platform with a robotic arm) ❌ Relying on dense point cloud and high-frequency odometry (typical configuration is 3D LiDAR + LIO), pure 2D LiDAR platform cannot be used ❌ Compared with the pure 2D costmap of DWA/TEB, the memory and update overhead of 3D occupancy grids are significantly higher

When is it needed? Robot aspect ratio > 2 , and exist in the working environment Narrow passages of the same magnitude as the body size or overhead suspension structure (tables, shelves, pipes). If it is a round chassis running in an open room, isotropic expansion is enough, and adding this machinery will only increase the cost.

Representative implementation: SCAN-Planner (arXiv:2606.19555, code) was validated on Unitree Go2 with Livox Mid-360 and FAST-LIO2, with all modules running in real time on Jetson Orin NX. Simulation comparisons used MARSIM (40 m × 20 m, 100–500 random obstacles, 50 trials per group):

The long-range real robot test includes cross-floor inspection of a three-story office building (149 m / 251 s) and campus-scale last-mile delivery (367 m / 589 s, global routing is provided by a commercial navigation map, and local planning is undertaken by this method).

5.4 Costmap hierarchy

ROS1 vs ROS2: The costmap_2d plug-in has the same architecture in the two versions (layered overlay, layer plug-in with the same name), and the namespace writing method of configuring YAML is slightly different. Nav2 adds two new layers, VoxelLayer (3D voxel barrier) and SpeedLimitLayer (regional speed limit), and the rest of the concepts are exactly the same.

costmap_2d adopts layered superposition architecture, multi-layer costmaps are merged in order, and finally the maximum cost value of each grid cell is taken:

Cost Values

Each grid cell stores a uint8 (0–255) cost value, with the following meaning:

Footprint (robot footprint)

Footprint is the 2D polygon footprint of the robot in the XY plane and is the basis for costmap to calculate the dangerous area. From this two radii are derived:

• Inscribed circle radius $r_{ins}$: The maximum circle radius that can fit into the footprint. When the robot center distance is $\leq r_{ins}$, must collide with, corresponding to the INSCRIBED (253) cost
• circumscribed circle radius $r_{circ}$: the minimum circle radius that can cover the footprint. When the robot center is between $(r_{ins}, r_{circ}]$ and the obstacle, may collide with (depending on the orientation)

Configuration example:

# Round robot (simple)
robot_radius: 0.22

# Non-circular robot (polygonal footprint)
footprint: [[-0.25, -0.20], [-0.25, 0.20], [0.25, 0.20], [0.25, -0.20]]

Inflation layer cost formula

Here, $d$ is the distance to the nearest obstacle, $r_{inf}$ is inflation_radius, and $k$ is cost_scaling_factor.

parameter intuition:

• inflation_radius The bigger the obstacle is, the wider the “influence range” of the obstacle is, and the narrow channel is more difficult to pass, but the path is safer
• The larger cost_scaling_factor is → the steeper the penalty attenuation. The penalty for being close to an obstacle is high but decreases quickly when moving away.
• recommended value: inflation_radius = $r_{ins}$ + 0.1–0.3 m (safety margin depends on localization accuracy and speed)

Global costmap vs local costmap

Two sets of costmaps are configured independently and serve different planners:

Key difference: The local costmap does not load the static layer, because the static layer is a globally consistent large map, and loading it into a small window will miss the “known obstacles” outside the window. Local maps only trust real-time data within the current sensor field of view.

5.5 Comparison of Planning Algorithms

6. Path Tracking

Path planning gives an ideal path, and the task of the path tracking controller is to make the robot actually follow this path. Due to the presence of noise, model errors and external interference in the real world, the controller needs to calculate corrections in real time.

6.1 Pure Pursuit Control

Pure Pursuit Control (Pure Pursuit) Intuition: Imagine you are driving, staring at a target point at a fixed distance ahead (Lookahead distance $L_d$), and constantly adjusting the steering wheel to turn towards it. That’s the thinking behind Pure Pursuit.

core formula:

Here:

• $\delta$ = Front wheel angle (control amount)
• $L$ = wheelbase
• $\alpha$ = The angle between the target point direction and the vehicle heading
• $L_d$ = Lookahead distance (usually the distance traveled within 1–2 times the current speed)

✅ Extremely simple to implement ✅ Robust to path noise (natural smoothing effect) ❌ The lookahead distance requires manual parameter adjustment ❌ Large tracking error at high speed (pure geometric control, ignoring dynamics)

Lookahead distance $L_d$ Parameter adjustment practice suggestions:

$L_d$ is the only key parameter that needs to be adjusted for Pure Pursuit, which has a great impact on performance:

Speed Adaptive Parameter (Adaptive Pure Pursuit): Use $L_d = k \cdot v$, where the typical $k$ value is:

• Indoor robot (up to 1 m/s): $k \approx 1.5 \text{–} 2.0$
• Warehousing AGV (up to 2 m/s): $k \approx 1.0 \text{–} 1.5$
• Autonomous driving (up to 30 km/h): $k \approx 0.5 \text{–} 1.0$

Another practical tip: Set the minimum value of $L_d$ (such as 0.3 m) to avoid oscillation caused by the lookahead distance approaching zero at low speeds.

6.2 Adaptive tracking control

Adaptive Pursuit Control (Adaptive Pure Pursuit) will lookahead distance $L_d$ with speed dynamic association :

Here, $k$ is the proportional coefficient, and $v$ is the current speed. When the speed is fast, the lookahead is longer (stable), and when the speed is slow, the lookahead is shorter (accurate). This solves the problem of large differences in the performance of fixed lookahead distances at different speeds.

6.3 Rear wheel feedback control

Rear Wheel Feedback uses the center point of the rear axle of the vehicle as the tracking reference point (rather than the front axle or center of gravity) to directly eliminate the lateral error and heading error of the rear wheel on the reference path. Rear Wheel Feedback has a stricter mathematical convergence guarantee than Pure Pursuit.

6.4 Stanley Controller

The control law proposed by the Stanford University autonomous driving team (for the DARPA Challenge) is based on the front axle midpoint:

Here:

• $\psi_e$ = heading error
• $e$ = Lateral deviation of front axle to path
• $k$ = Gain coefficient
• $v$ = current speed

The first term corrects the heading deviation, and the second term corrects the lateral deviation. The faster the speed, the smaller the lateral error correction (to avoid high-speed sharp turns).

✅ Better accuracy than Pure Pursuit ✅ Good accuracy at low speed (parking) ❌ At extremely low speed, the $\arctan(k \cdot e / v)$ item tends to be saturated and needs to be processed. ❌ Does not explicitly consider path curvature

6.5 LQR path tracking

Linear Quadratic Regulator models the path tracking problem as optimal control problem. Under the vehicle linearization model, LQR solves the optimal control law that minimizes the following cost function:

Here, \(\mathbf{e}_t\) is the tracking error (lateral deviation and heading error), $u_t$ is the control input (steering angle), and $\mathbf{Q}$ and $\mathbf{R}$ are weight matrices. Larger $\mathbf{Q}$ weights prioritize reducing tracking error; larger $\mathbf{R}$ weights prioritize smoother control.

✅ Theoretically optimal and high accuracy ✅ System response is smooth ❌ Rely on accurate linearization model ❌ $\mathbf{Q}$, $\mathbf{R}$ matrix parameter adjustment requires experience

6.6 Comparison of Path Tracking Algorithms

7. Robot Kinematic Models

All algorithms for path planning and motion control, their speed constraints, trajectory curvature, and control variables are based on specific kinematic models. Different chassis structures correspond to completely different state equations, which directly determine which planning/tracking algorithms are available and how constraints are modeled. This chapter systematically sorts out three types of mainstream models.

7.1 Differential Drive Model

Mechanical configuration

The differential drive chassis consists of two independent drive wheels (left wheel $\omega_L$, right wheel $\omega_R$) and a number of passive caster wheels. Steering is achieved through the difference in speed between the two wheels. Typical platforms: TurtleBot, Husky, indoor mobile robot.

Kinematic equations

Assume that the wheel radius is $r$, and the track width (distance between the wheel centers) is $2b$, then:

The robot’s state $\mathbf{q} = [x, y, \theta]^\top$ in the world frame satisfies:

Written in matrix form:

The control input is the linear velocity $v$ and the angular velocity $\omega$. The rotation speed of each wheel is obtained through inverse kinematics solution:

Constraints and Properties

Impact on planning algorithms

• DWA: The speed sampling space is the $(v, \omega)$ two-dimensional rectangle, which can support rotation in place without minimum turning radius constraints.
• TEB : Can be directly constrained $v$ , $\omega$ and its derivative (acceleration), the differential model optimization has a high degree of freedom.
• Pure Pursuit / LQR: Usually $\omega$ is used as the control output to directly control the angular velocity.

7.2 Ackermann Steering Model

Mechanical configuration

The Ackermann chassis is designed after the steering geometry of a car: rear-wheel drive, front-wheel steering; the two front wheels rotate around their respective steering knuckles. Ideally, the four-wheel instantaneous rotation centers are collinear, eliminating tire sideslip. Typical platforms: Autonomous passenger cars, AgileX Scout, Jackal (Small 4WD/Ackermann hybrid).

Geometric relations

Suppose the wheelbase (center distance between front and rear axles) is $L$, and the equivalent front wheel angle is $\delta$, then the turning radius is:

The actual front wheel angles inside and outside satisfy (complete Ackermann condition):

Here, $W$ is the track width. In practice, a single equivalent front-wheel steering angle $\delta$ is commonly used.

Kinematic equations (bicycle model approximation)

Merge the front and rear wheels into a single wheel to get the classic bicycle model:

The control inputs are longitudinal speed $v$ and front wheel angle $\delta$, curvature $\kappa = \tan\delta / L$.

Discretization (forward Euler, step $\Delta t$):

Constraints and Properties

Impact on planning algorithms

• Hybrid A*: Taking $(x, y, \theta)$ as the state, use Ackermann’s equation of motion to expand the nodes to generate a continuous path with curvature, which can be tracked directly.
• TEB: Ackermann mode needs to be turned on to constrain the upper limit of $\lvert\delta\rvert \le \delta_{\max}$ and $\dot{\delta}$, and the minimum curvature radius of the trajectory is limited.
• Pure Pursuit / Stanley: Output front wheel angle $\delta$, which is a tracking control law specially derived for Ackermann vehicles.
• MPC: Using the bicycle model as the prediction model, constraining $\delta$ and $\dot{\delta}$, it is suitable for high-speed and accurate tracking.

7.3 Omnidirectional (Holonomic) Model

Mechanical configuration

The omnidirectional chassis uses a special wheel structure (Mecanum wheels or omnidirectional wheels) to achieve independent translation in any direction within the plane without rotating the robot body. Typical structure:

Typical platforms: warehousing AMR (Amazon Kiva class), laboratory mobile operation robot (HSR, TIAGo).

Four-wheel Mecanum kinematic equations

Assume that the four-wheel layout is a rectangle, the half wheelbase is $l_x$ (longitudinal), $l_y$ (transverse), the wheel radius is $r$, and the angle between the roller and the wheel axle is $45°$, then the inverse kinematics (robot speed → wheel speed):

Wheel number convention: $\omega_1$=left front, $\omega_2$=right front, $\omega_3$=left rear, $\omega_4$=right rear.

For forward kinematics (wheel speed → robot speed), take the pseudo-inverse of the upper matrix ($\frac{1}{4}$ times transposed at full rank):

State equation in the world frame (including heading angle $\theta$):

Three-wheel omnidirectional kinematics equations

The three wheels form $120°$. Let the installation angle of the $i$ wheel be $\phi_i = 120°(i-1)$:

Here, $d$ is the distance from the wheel to the center of the chassis. Inverse solution:

Constraints and Properties

Impact on planning algorithms

• Global planning: Standard A* / Dijkstra can be used directly to plan on the grid map. The path has no curvature constraints and can be reached in any direction.
• DWA: The velocity space is expanded to $(v_x, v_y, \omega_z)$ three dimensions, or equivalently to $(v, \theta_{vel}, \omega_z)$, with a larger sampling space.
• path tracking: No complex tracking control law is required, simple PID can be used to control the three channels $x$, $y$ and $\theta$ respectively, and are independently decoupled.
• MPC: The prediction model is linear (ignoring slip), the constraint is the upper limit of each wheel speed, and the design is simpler than Ackermann.

7.4 Comprehensive comparison of three types of models

Engineering Selection Principles: Differential priority for indoor narrow roads (flexible turning, low cost); priority for outdoor high-speed Ackermann (good stability, mature tire model); omnidirectional wheels for storage/operation scenarios that require lateral movement (high efficiency, but need to compensate for slippage).

8. Motion Control

Path tracking (Chapter 6) solves the geometric problem of “which direction to go in”, while motion control solves the dynamic problem of “how to accurately execute these instructions” - the system model, physical constraints, disturbance suppression and optimality need to be considered. This chapter introduces the model-based control methods commonly used in robot navigation.

8.1 PID control

PID (Proportional-Integral-Derivative) is the most widely used controller in engineering and is the basis for understanding more complex control algorithms.

control law:

Here, $e(t)$ is the error (such as lateral deviation or heading error), and the three items are:

• P (proportional): The larger the current error, the greater the correction; the response is fast but may overshoot.
• I (integral): eliminate steady-state error (accumulated historical error); an excessive integral gain can cause oscillation
• D (derivative): predict error trend, suppress overshoot; sensitive to noise

discrete form (used in actual code):

✅ Simple implementation, intuitive parameter adjustment ✅ No system model required (pure empirical parameter adjustment) ❌ Fixed parameters, difficult to adapt to nonlinear and time-varying systems ❌ Unable to explicitly handle constraints (such as maximum speed, maximum steering angle)

8.2 Fuzzy PID control (Fuzzy PID)

Fuzzy PID is a direct evolution of PID: it retains the feedback loop structure of PID and introduces Fuzzy Inference System right $K_p, K_i, K_d$ Three gains are made Online self-tuning , allowing the controller to automatically adapt to different working conditions.

System structure

flowchart LR
    E["e(t)\nError"] --> FIS
    DE["ė(t)\nerror rate of change"] --> FIS
    subgraph FIS["fuzzy inference engine"]
        F1["Fuzzification"] --> F2["Rule inference\nRule Inference"]
        F2 --> F3["Defuzzification"]
    end
    FIS -->|ΔKp, ΔKi, ΔKd| ADD["Parameter update\nKp = Kp₀+ΔKp\nKi = Ki₀+ΔKi\nKd = Kd₀+ΔKd"]
    ADD --> PID["PID control law\nu(t)"]
    PID --> Plant["Plant"]
    Plant -->|"y(t)"| FB(( ))
    FB --> E
    FB --> DE

Fuzzification

Map the continuous error $e$ and error change rate $\dot{e}$ to 7 linguistic variables (fuzzy sets):

Each linguistic variable corresponds to a membership function (usually a triangle or trapezoidal function). The continuous input value is calculated through the membership degree to obtain the “degree of ownership” of each linguistic variable.

Fuzzy rules (Rule Base)

The rule form is IF $e$ is A AND $\dot{e}$ is B THEN $\Delta K_p$ is C. Taking $\Delta K_p$ as an example, the typical rule table (7 × 7):

Rule Intuition: When the error is large (NB/PB), the proportional gain needs to be increased ($\Delta K_p$ takes PB) and pulled back quickly; when the error approaches zero (ZO), the proportion should be reduced and the differential should be increased to prevent overshoot. The rule table design logic of $\Delta K_i$ and $\Delta K_d$ is similar but the goals are different.

Defuzzification

The most commonly used center of gravity method (Centroid Method):

Here, $\mu_j$ is the activation intensity of rule $j$, and $c_j$ is the central value of the corresponding output fuzzy set. Finally, the accurate $\Delta K_p$, $\Delta K_i$, and $\Delta K_d$ increments are obtained, which are superimposed on the basic value:

Core differences from fixed PID

✅ does not require an accurate model: purely empirical rule driven, suitable for nonlinear systems that are difficult to model ✅ is computationally efficient: a fuzzy inference only requires table lookup + interpolation, µs level, suitable for embedded platforms ✅ is highly robust: the suppression of parameter perturbations and external disturbances is better than fixed gain PID ❌ The rule table requires domain experience design, and the number of rules increases exponentially with the input dimension (7×7=49 items/gain) ❌ Lack of rigorous proof of optimality and stability (compare LQR/MPC)

8.3 MPC (Model Predictive Control)

MPC is currently one of the most popular methods in autonomous driving and high-precision robot control. core idea : In each control cycle, based on the current state and system model, solve a finite-horizon optimization problem , output an optimal control sequence, but only execute the first step, and then solve it again in the next cycle - that is, “ receding-horizon optimization with feedback correction “.

optimization problem form:

Here:

• $N$ = Prediction Horizon, typical value 10–30 steps
• $\mathbf{Q}, \mathbf{R}$ = weight matrix of state error and control quantity ($\mathbf{Q}$ is large → priority is given to reducing tracking error; $\mathbf{R}$ is large → priority is given to reducing control amplitude)
• $\mathcal{X}, \mathcal{U}$ = state constraints and control constraints (such as maximum speed, maximum steering angle)
• $\mathbf{P}$ = terminal cost matrix (to ensure closed-loop stability)

✅ explicitly handles constraints: physical constraints such as speed upper limit and acceleration limit are directly written into optimization constraints ✅ multi-step forward-looking: predict the future $N$ steps, slow down in advance before the bend, instead of waiting until the curvature increases before reacting ✅ unified framework: path tracking, speed planning, and soft obstacle avoidance can be processed in the same optimization problem ❌ Large amount of calculation, requiring efficient QP/NLP solver (OSQP, CasADi+IPOPT) ❌ Rely on accurate system model, model mismatch will affect performance ❌ Complex parameter adjustment ($N$, $\mathbf{Q}$, $\mathbf{R}$, constraint boundaries, terminal sets)

Linear MPC vs. Nonlinear MPC (NMPC):

8.4 Comparison of Controllers

9. Full Navigation Stack Integration

9.1 Coordinate frame and TF tree

Data exchange between each module of the navigation stack (sensor, localization, planning, control) requires clear “which coordinate frame this pose/point is relative to.” ROS uses the TF2 library to uniformly manage this transformation tree.

Four core coordinate frames

TF The relationship between the tree structure and each module:

flowchart TB
    MAP["🗺️ map\nGlobally consistent fixed system\norigin = Starting point of mapping"]
    ODOM["📍 odom\nodometry integral system\nContinuous but drifting"]
    BASE["🤖 base_link\nRobot Chassis Center"]
    LIDAR["📡 lidar_link"]
    CAM["📷 camera_link"]
    IMU["🔄 imu_link"]

    SLAM["SLAM / AMCL\nLocate node"]
    WHEEL["wheel odometry\n/ IMUFusion"]
    URDF["URDF\nstatic transformation"]

    MAP -->|"T_map^odom\nCorrect drift (jump)"| ODOM
    ODOM -->|"T_odom^base\ncontinuous smoothing"| BASE
    BASE -->|"Fixed extrinsic parameters"| LIDAR
    BASE -->|"Fixed extrinsic parameters"| CAM
    BASE -->|"Fixed extrinsic parameters"| IMU

    SLAM -.->|publish| MAP
    WHEEL -.->|publish| ODOM
    URDF -.->|publish| BASE

    style MAP fill:#dbeafe,stroke:#3b82f6,color:#1e3a5f
    style ODOM fill:#fef9c3,stroke:#eab308,color:#713f12
    style BASE fill:#dcfce7,stroke:#22c55e,color:#14532d
    style LIDAR fill:#f3f4f6,stroke:#9ca3af,color:#374151
    style CAM fill:#f3f4f6,stroke:#9ca3af,color:#374151
    style IMU fill:#f3f4f6,stroke:#9ca3af,color:#374151
    style SLAM fill:#ede9fe,stroke:#8b5cf6,color:#4c1d95
    style WHEEL fill:#ede9fe,stroke:#8b5cf6,color:#4c1d95
    style URDF fill:#ede9fe,stroke:#8b5cf6,color:#4c1d95

Why are map and odom separated?

• odom ensures continuity: odometry integral will not jump and the controller can track smoothly
• map ensures global consistency: SLAM loop closure or AMCL correction will update map→odom but does not affect the continuity of odom→base_link
• If merged into one system, each localization correction will cause the pose to jump instantaneously, causing the controller to become unstable.

map→odom This transformation is exactly the output of the localization node, which corrects the accumulated drift of odometry in real time.

Homogeneous transformation matrix

The transformation between coordinate frames is represented by the $4\times4$ homogeneous matrix ($SE(3)$ elements):

chain transformation: $T_{map}^{base} = T_{map}^{odom} \cdot T_{odom}^{base}$

inverse transformation: $T_B^A = \left(T_A^B\right)^{-1} = \begin{bmatrix} R^T & -R^T t \ \mathbf{0}^T & 1 \end{bmatrix}$

The plane navigation degeneracy is $SE(2)$, and the pose $(x, y, \theta)$ corresponds to:

Debugging Tips: ros2 run tf2_tools view_frames can export the current TF tree as PDF; ros2 run tf2_ros tf2_echo map base_link can print the transformation between two frames in real time.

9.2 ROS1 Navigation Stack architecture

move_base of ROS 1 provides a classic navigation stack integration solution:

flowchart TB
    subgraph MoveBase["move_base"]
        GM["global planner\nNavfn / GlobalPlanner"] --> GCM["global costmap\nGlobal Costmap"]
        LM["local planner\nDWA / TEB"] --> LCM["local costmap\nLocal Costmap"]
        GM -->|global path global_plan| LM
        LM -->|speed command cmd_vel| VEL
        REC["recovery behaviors\nRecovery Behaviors"]
    end

    GOAL[/target pose goal/] --> MoveBase
    MAP[/static map map/] --> GCM
    SCAN[/LiDAR scan/] --> GCM & LCM
    ODOM[/ odometry  odom/] --> MoveBase
    VEL[/cmd_vel/] --> Robot["Robot chassis"]
    AMCL["AMCL Localization"] --> MoveBase
    LM -- stuck/timeout --> REC
    REC -- Try again after recovery --> GM

Move_base state machine

move_base has a four-state machine inside:

stateDiagram-v2
    [*] --> PLANNING : target received
    PLANNING --> CONTROLLING : Global path planning successful
    PLANNING --> CLEARING : Planning failed
    CONTROLLING --> PLANNING : Local planner requests replanning
    CONTROLLING --> CLEARING : Robot stuck / timeout
    CONTROLLING --> [*] : reach target
    CLEARING --> PLANNING : Recovery behavior completed, try again
    CLEARING --> [*] : All recovery attempts failed

• PLANNING: Call the global planner to generate a global path from the current location to the target (low frequency, by default only triggered when a new target or local planning request is received)
• CONTROLLING: Call the local planner to continuously generate cmd_vel at a frequency of about 10–20 Hz and track the global path
• CLEARING: Enter when global or local planning fails, execute the recovery behavior list in sequence, and return to PLANNING to try again after execution.

Recovery Behaviors

move_base The default recovery_behaviors is executed in sequence:

After all failures move_base publishes the aborted result and stops.

Typical topic interface:

Frequency difference between two costmaps: The global costmap is updated slowly (only updated when the static layer changes), and the local costmap is updated frequently (synchronized with the sensor frame rate, about 5–20 Hz), ensuring real-time obstacle avoidance without wasting computing resources.

9.3 Nav2 (ROS 2) architecture

Nav2 is the navigation stack of ROS 2. Compared with move_base, the core change is to replace the hard-coded state machine with the behavior tree, which decouples the navigation logic from the code to a configurable XML file.

Five major server architectures

flowchart TB
    BTN["BT Navigator\n(read BT XML, drives the entire navigation process)"]

    BTN -->|ComputePathToPose| PS["Planner Server\nGlobal planning\nNavFn / Smac / Theta*"]
    BTN -->|FollowPath| CS["Controller Server\nlocal control\nDWB / TEB / MPPI"]
    BTN -->|Recovery Action| RS["Recovery Server\nrecovery behaviors\nclear_costmap / spin / wait / backup"]
    BTN -->|SmoothPath| SS["Smoother Server\npath smoothing\nSimple / Savitzky-Golay"]

    PS --> GCM["global costmap"]
    CS --> LCM["local costmap"]
    CS -->|cmd_vel| Robot["Robot chassis"]
    RS -->|Spin in place / Back| Robot

Each Server is an independent ROS 2 node that communicates through the Action interface. Plug-ins can be replaced independently without affecting other modules.

Lifecycle Nodes

All Nav2 nodes implement the ROS 2 life cycle interface, and the state transition is as follows:

Unconfigured → Inactive → Active → Deactivating → Inactive → Finalized

• Unconfigured → Inactive (configure): Load parameters, initialize plug-in, do not receive data
• Inactive → Active (activate): Start subscribing to topics, publishing data, and entering normal working status
• Active → Inactive (deactivate): Stop processing and retain resources (can be quickly reactivated)
• The startup sequence is managed uniformly by nav2_lifecycle_manager to avoid race conditions caused by out-of-order initialization of nodes.

Behavior tree (BT) default navigation flow

Simplified logic of Nav2 default BT (navigate_w_replanning_and_recovery.xml):

flowchart TD
    START([Navigation target received]) --> PLAN["ComputePathToPose\nGlobal planning"]
    PLAN -->|success| FOLLOW["FollowPath\nlocal control tracking"]
    PLAN -->|failed| R1["ClearEntireCostmap\nClear local costmap"]
    R1 --> PLAN

    FOLLOW -->|reach target| DONE([Navigation successful])
    FOLLOW -->|stuck / timeout| R2["ClearEntireCostmap\nClear local costmap"]
    R2 --> PLAN2["Replan globally"]
    PLAN2 -->|Still failed| R3["Spin\nSpin in place 180°"]
    R3 --> PLAN3["Replan globally"]
    PLAN3 -->|Still failed| R4["Wait\nwait 5 s"]
    R4 --> PLAN4["Replan globally"]
    PLAN4 -->|Still failed| R5["Backup\nBack 0.3 m"]
    R5 --> PLAN5["Final global planning attempt"]
    PLAN5 -->|Still failed| FAIL([Navigation failed / abort])

Detailed explanation of failure recovery chain

Design logic of execution sequence:

• First ClearEntireCostmap (the lightest, only changes data, does not move)
• Spin again (re-perceive the surrounding environment)
• Then Wait (make way for dynamic obstacles)
• Finally Backup (physical escape, highest risk, placed last)

After each recovery step, the global planning is re-triggered. As long as one step is successful, the recovery chain will be jumped out and navigation will continue. After all failures, BT returns FAILURE, and Action Server publishes aborted results.

Custom recovery behavior: Implement the nav2_core::Recovery interface, register it in the recovery_server.plugins list of nav2_params.yaml, and then add the corresponding node in BT XML.

Common recovery trigger timings (real robot experience)

The trigger sources of the recovery chain can be divided into the following four categories, corresponding to different root causes and treatment directions:

① The local map is temporarily unfeasible

Sensor noise or instantaneous occlusion “blocks” the local costmap, FollowPath cannot find the legal speed (follow_path_error), and directly triggers recovery:

② Progress check failed (robot stuck)

The robot shakes slightly in place but the cumulative displacement is not enough to exceed the threshold (Nav2 default: displacement within 10 s < 0.5 m). BT’s GoalReached or progress check node determines that it is stuck, triggering recovery:

③ TF / timing problem

map→odom→base_link The transformation is temporarily unavailable (TF timeout) or the timestamp is inconsistent. The controller cannot obtain a consistent state estimate, and recovery is triggered after continuous planning failure:

④ The global path is feasible but the local dynamics is not feasible

The path given by the global planning (A* / Smac) is geometrically collision-free, but the local planner (DWA / MPPI) is constrained by speed, acceleration and obstacles, and cannot give a safe control sequence that satisfies the constraints in a short time:

9.4 Parameter Tuning Key Points

Tuning a navigation stack means coordinating its modules: an upstream parameter error can appear as a downstream failure. Tune in the order costmap → global planner → local planner → recovery behaviors.

Costmap

Core Principles: inflation_radius $\geq$ The radius of the inscribed circle of the robot. Otherwise, the LETHAL area does not cover the robot’s footprint, and the path planner will treat inaccessible points as passable.

Global Planner

Navfn / NavFn(A* / Dijkstra):

Smac Planner(Nav2 Hybrid A*):

Local Planner/Controller

DWA(dwa_local_planner / Nav2 DWB):

TEB(teb_local_planner):

Recovery behavior (Recovery/move_base)

Nav2 core global parameters

Tuning sequence suggestions

① costmap: adjust first inflation_radius, Confirm no collision
       ↓
② Global planning: Confirm that you can A Arrive B find path
       ↓
③ Local planning: first adjust the upper speed limit, and then sim_time, Final tune bias weight
       ↓
④ Recovery behavior: After triggering recovery, observe whether the robot can get out of trouble and adjust timeout/distance threshold
       ↓
⑤ Overall joint debugging: in reality/Run the complete navigation in the simulation scene and observe the lag/Oscillation/Too many detours and other issues

10. Traditional navigation vs. end-to-end deep learning navigation

Practical suggestions:

• Factory, warehousing, medical, etc. Structured and high safety requirements Scenario → Traditional Navigation Stack
• Family services, guided tours, etc. Unstructured, need to understand natural language Scenario → Learning Navigation
• Hybrid architecture is becoming a trend: using the traditional navigation stack to handle low-level safety and precise control, and using VLM/LLM to handle high-level semantic understanding and task decomposition

11. Common Open-Source Tools and Frameworks

Sensor drivers and processing

Localization

SLAM

Path planning

Simulation

12. Summary and outlook

What This Survey Covers

This article systematically sorts out the five core modules of the traditional robot navigation algorithm stack:

1. Perception: LiDAR/camera/IMU each has its own advantages and disadvantages, and sensor fusion (EKF/UKF) is the key to improving robustness
2. localization: EKF/UKF is suitable for real-time pose tracking, particle filtering (AMCL) supports global localization, and NDT/ICP provides accurate scan matching
3. Mapping/SLAM: LiDAR SLAM (Cartographer, LIO-SAM) has high accuracy and is available around the clock; visual SLAM (ORB-SLAM3, VINS-Mono) is low-cost but sensitive to light; RTAB-Map spans multiple modes and can be used out of the box; factor graph back-end optimization is currently mainstream
4. Path planning: A/Hybrid A is used for global planning, DWA/TEB is used for local dynamic obstacle avoidance, and costmap is the “common language” of both
5. path tracking: Pure Pursuit / Stanley is a geometric method, simple to implement, suitable for low-speed scenarios
6. Motion control: PID is the foundation, LQR provides optimal linear control, MPC can explicitly handle constraints and multi-step lookahead, and fuzzy PID is more robust under nonlinear disturbances

Outlook

The traditional navigation algorithm stack has become quite mature after decades of development, but it still faces challenges:

• Corridor degeneracy, dynamic scenes, unstructured terrain: Putting forward higher requirements for SLAM robustness
• Multi-robot collaborative SLAM: Trade-off between distributed mapping and communication efficiency
• Lack of semantic understanding: The traditional algorithm lacks the semantic ability of “this is the kitchen”, limiting its application in daily service scenarios

The current research trend is the hybrid architecture of “traditional navigation + large model”: retaining the safety and reliability of the traditional navigation stack, introducing LLM/VLM in the task planning and semantic understanding layer, and building a next-generation robot system that can understand human intentions and act autonomously in the complex real world.

For vision-language navigation (VLN) and VLA foundation models, see the VLN Survey and VLA Survey (Chinese) series.

References: Thrun et al. “Probabilistic Robotics” (2005); LaValle “Planning Algorithms” (2006); ROS Navigation Wiki; Cartographer Paper (ICRA 2016); LIO-SAM (IROS 2020); ORB-SLAM3 (T-RO 2021); VINS-Mono (T-RO 2018); Hybrid A (IJRR 2010); TEB Local Planner (IROS 2013)*; http://www.autolabor.cn/usedoc/m1/navigationKit/development/slamintro; https://github.com/ShisatoYano/AutonomousVehicleControlBeginnersGuide;Embodied Intelligence Research Community;