In recent years, the integration of robotic systems and autonomous navigation technologies has advanced significantly, with quadruped robots, often referred to as robot dogs, emerging as versatile platforms for applications such as environmental monitoring, industrial operations, and indoor services. These quadruped robots exhibit exceptional terrain adaptability and dynamic stability, making them particularly suitable for indoor environments characterized by frequent dynamic obstacles and dense, unstructured layouts. However, such complex settings impose stringent demands on Simultaneous Localization and Mapping (SLAM) techniques, requiring high real-time performance, robustness, and accuracy. Traditional SLAM approaches often struggle to balance these factors, leading to issues like insufficient localization precision, low mapping efficiency, and reduced system resilience in dynamic conditions. To address these challenges, we propose a multi-module optimized Fast-SLAM framework that enhances the core algorithm through improvements in laser odometry, state estimation, and particle filtering. This framework is designed to overcome the limitations of existing methods, which frequently focus on single-module optimizations and lack integrated multi-sensor coordination, thereby achieving a better trade-off between accuracy and computational efficiency in complex indoor scenarios.
The Fast-SLAM algorithm serves as the foundation for our approach, utilizing particle filtering to estimate the trajectory of a robot dog. Each particle independently maintains a map, and through Bayesian incremental reasoning, the joint estimation process is decomposed into separate localization and mapping tasks. The fundamental equation is expressed as:
$$ p(x_{1:t}, m | z_{1:t}, u_{1:t}) = p(x_{1:t} | z_{1:t}, u_{1:t}) \prod_{i=1}^{N} p(m_i | x_{1:t}, z_{1:t}) $$
where $x_{1:t}$ represents the estimated robot path, and $p(m | x_{1:t}, z_{1:t})$ denotes the map construction given the robot poses and sensor observations. To reduce computational complexity, the algorithm approximates $p(x_{1:t} | u_{1:t}, z_{1:t})$ using particle filtering, transforming it into an incremental state estimation problem. The update formula is given by:
$$ p(x_t | x_{1:t-1}, u_{1:t}, z_t) = \eta \cdot p(z_t | x_t) \cdot p(x_t | x_{1:t-1}, u_{1:t}) $$
Here, $\eta$ is a normalization factor, $p(x_t | x_{1:t-1}, u_{1:t})$ represents the motion model, and $p(z_t | x_t)$ is the observation model. In the particle filter framework, each particle propagates its state through sampling and updates its weight based on observations. The final trajectory estimate is derived from normalized weighted averages, and map updates are performed using the current trajectory and control inputs. However, conventional Fast-SLAM implementations face issues such as high memory consumption due to large particle counts and particle diversity loss from frequent resampling, especially when odometry accuracy is poor. Our multi-module optimization aims to mitigate these problems while enhancing overall performance for quadruped robot navigation.
In the front-end odometry phase, we focus on optimizing laser-based motion estimation using a range flow model, which analyzes geometric changes in consecutive laser scan frames to estimate the local pose transformation of the robot dog. The range flow method establishes temporal constraints between frames by relating the distance derivatives of laser points to angular velocities, enabling continuous estimation of translation and rotation without explicit feature extraction. The core equation of the range flow algorithm is:
$$ \frac{dr}{dt} = -\dot{x} \cos \theta – \dot{y} \sin \theta + r \dot{\theta} \sin \theta $$
where $r$ is the range measurement, $\theta$ is the angle, and $\dot{x}$, $\dot{y}$, and $\dot{\theta}$ represent the translational and rotational velocities. This equation captures how the motion of laser scan points influences pose estimation. To address noise and outliers in laser data, which can degrade accuracy in complex environments, we integrate an adaptive Iterative Reweighted Least Squares (IRLS) strategy. Traditional IRLS methods use a fixed number of iterations, leading to inefficiencies in low-noise scenarios and instability in high-noise conditions. Our adaptive approach dynamically adjusts the iteration count based on data complexity, defined by a metric $\gamma$:
$$ \gamma = \frac{1}{N} \sum_{i=1}^{N} | \rho_i | $$
where $\rho_i$ denotes the residual for each measurement point, reflecting the overall noise level. The adaptive iteration rule is formulated as:
$$ M = \begin{cases}
a & \text{if } \gamma \leq \gamma_1 \\
b & \text{if } \gamma_1 < \gamma \leq \gamma_2 \\
c & \text{if } \gamma_2 < \gamma \leq \gamma_3 \\
d & \text{otherwise}
\end{cases} $$
with parameters set to $a = 3$, $b = 5$, $c = 6$, and $d = 8$. During each iteration, weights $w(\rho_i)$ are updated to reduce the impact of noise:
$$ w(\rho_i) = \frac{1}{1 + \left( \frac{\rho_i}{c} \right)^2} $$
where $c$ is a tuning constant. The pose estimate is then refined using weighted least squares, with the weighted matrix $A_w$ and target vector $B_w$ computed as:
$$ A_w = J^T W J, \quad B_w = J^T W b $$
where $J$ is the Jacobian matrix, $W$ is the weight matrix, and $b$ is the residual vector. The updated pose estimate is obtained by solving:
$$ \Delta \xi = (A_w)^{-1} B_w $$
To prevent unnecessary iterations, a convergence threshold $\epsilon$ is applied, terminating the process if the change in pose estimates between consecutive iterations satisfies:
$$ \| \Delta \xi_{k} – \Delta \xi_{k-1} \| < \epsilon $$
This adaptive IRLS method significantly improves computational efficiency and estimation accuracy for the robot dog in varying noise conditions.
For state estimation, we employ an Extended Kalman Filter (EKF) to fuse odometry data, enhancing robustness in dynamic indoor environments. The EKF linearizes nonlinear process and observation models at each time step, combining prediction and update steps for recursive state estimation. The standard EKF equations include the prediction step:
$$ \hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_k) $$
$$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k $$
and the update step:
$$ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} $$
$$ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k – h(\hat{x}_{k|k-1})) $$
$$ P_{k|k} = (I – K_k H_k) P_{k|k-1} $$
where $F_k$ and $H_k$ are Jacobians of the process and observation models, $Q_k$ is the process noise covariance, and $R_k$ is the measurement noise covariance. Traditional EKF uses a fixed $R_k$, which is inadequate for handling time-varying sensor errors. To address this, we introduce a fuzzy adaptive mechanism that dynamically adjusts $R_k$ based on the observation residual $v_k = z_k – h(\hat{x}_{k|k-1})$. The norm of the residual, $q_k = \| v_k \|$, is used to construct a fuzzy adjustment function, updating the covariance matrix as:
$$ R_k = \begin{bmatrix} r(q_k) & 0 & 0 \\ 0 & r(q_k) & 0 \\ 0 & 0 & r_\theta \end{bmatrix} $$
The position observation noise function $r(q_k)$ is defined as:
$$ r(q_k) = r_{\text{min}} + (r_{\text{max}} – r_{\text{min}}) \cdot \exp(-\lambda q_k) $$
with $r_{\text{max}}$ and $r_{\text{min}}$ representing the maximum and minimum covariance values, $q_{\text{max}}$ as a normalization reference, and $r_\theta$ as the fixed angular noise covariance. This adjustment ensures that when $q_k$ is small (indicating stable observations), $r(q_k)$ approaches $r_{\text{max}}$ to reduce observation weight, and when $q_k$ is large (indicating high volatility), $r(q_k)$ approaches $r_{\text{min}}$ to increase observation weight. This fuzzy adaptive EKF enhances the robustness of the quadruped robot’s state estimation against dynamic disturbances.
In the particle filtering stage, we build upon the Gmapping algorithm, which optimizes Fast-SLAM by addressing particle diversity and computational load. Gmapping uses scan matching to determine the optimal pose for each particle via Maximum Likelihood Estimation (MLE), reducing the need for extensive sampling. The MLE score for a candidate pose $x_t$ is computed as:
$$ S(x_t) = \sum_{i=1}^{N} \log p(z_t | x_t, m) $$
where $p(z_t | x_t, m)$ is the likelihood of observation $z_t$ given pose $x_t$ and map $m$. The algorithm evaluates multiple candidate points in six directions and selects the pose with the highest score. However, this discrete search method is computationally intensive and prone to local optima. To improve efficiency and accuracy, we replace discrete search with a gradient-based optimization approach. The gradient of the objective function $S(x_t)$ is calculated using finite differences:
$$ \nabla S(x_t) = \left[ \frac{\partial S}{\partial x}, \frac{\partial S}{\partial y}, \frac{\partial S}{\partial \theta} \right] $$
with partial derivatives approximated as:
$$ \frac{\partial S}{\partial x} \approx \frac{S(x + \delta x, y, \theta) – S(x, y, \theta)}{\delta x} $$
The pose is then updated iteratively using gradient ascent:
$$ x_t^{(k+1)} = x_t^{(k)} + \alpha \cdot [l_d \cdot \frac{\partial S}{\partial x}, l_d \cdot \frac{\partial S}{\partial y}, a_d \cdot \frac{\partial S}{\partial \theta}] $$
where $\alpha$ is the learning rate, $l_d$ is the linear step size, and $a_d$ is the angular step size. The iteration terminates when the pose change falls below a threshold $\epsilon$:
$$ \| x_t^{(k+1)} – x_t^{(k)} \| < \epsilon $$
This gradient method reduces computational overhead and improves pose estimation precision for the robot dog. Additionally, Gmapping’s effective sample size $N_{\text{eff}}$ is used to control resampling frequency:
$$ N_{\text{eff}} = \frac{1}{\sum_{i=1}^{N} (w_i)^2} $$
where $w_i$ are particle weights. Resampling is performed only when $N_{\text{eff}}$ drops below a threshold, conserving particle diversity and reducing the risk of particle depletion.
To validate our multi-module optimized Fast-SLAM algorithm, we conducted experiments on a real quadruped robot platform operating in an indoor office environment. The robot dog was equipped with a Raspberry Pi 4B as the main controller, a six-axis IMU, and an LD19 laser radar with a 360° scanning range, 10 Hz frequency, and ±2 cm ranging accuracy. The experimental setup included typical indoor obstacles such as desks, chairs, cabinets, and partitions, with the robot following a fixed path for multiple navigation tasks. Data from topics like /scan, /odom, and /imu were collected and analyzed offline using MATLAB to assess performance metrics.
We designed three comparative experiments to evaluate the effectiveness of our optimizations. The first experiment compared the adaptive IRLS strategy against fixed IRLS in laser odometry, measuring trajectory root mean square error (RMSE) and computation time. The results demonstrated that the adaptive approach achieved similar accuracy with significantly reduced processing time, as summarized in the table below:
| Algorithm | Average Time per Frame (ms) | RMSE (m) |
|---|---|---|
| Fixed IRLS | 0.1629 | 0.1446 |
| Adaptive IRLS | 0.0830 | 0.1444 |
The second experiment evaluated the fuzzy adaptive EKF against the standard EKF under dynamic disturbances. The fuzzy adaptive method maintained low localization errors even after initial frames, outperforming the traditional approach:
| Algorithm | Error in First 400 Frames (m) | Error After 400 Frames (m) |
|---|---|---|
| Standard EKF | 0.0284 | 0.1890 |
| Fuzzy Adaptive EKF | 0.0169 | 0.0116 |
The third experiment analyzed the gradient-based particle filter optimization compared to discrete search MLE. The gradient method achieved higher accuracy and faster computation, as shown below:
| Algorithm | Average Time per Frame (ms) | RMSE (m) |
|---|---|---|
| Discrete Search MLE | 0.1208 | 0.0688 |
| Gradient Optimization | 0.0988 | 0.0356 |
Overall, the multi-module optimized Fast-SLAM framework demonstrated substantial improvements in localization accuracy, computational efficiency, and system robustness for the quadruped robot. The integrated approach reduced trajectory errors by up to 48.3% and decreased processing time by 18-49% across modules, confirming its suitability for complex indoor navigation. Additionally, mapping results in real-world environments showed clear boundaries and well-aligned structures, with minimal ghosting or misalignment, further validating the practical applicability of our method.
In conclusion, our multi-module optimized Fast-SLAM algorithm effectively addresses key challenges in indoor navigation for quadruped robots, leveraging adaptive laser odometry, fuzzy EKF, and gradient-based particle filtering to enhance performance. The proposed framework achieves a balance between precision and efficiency, making it viable for dynamic indoor applications. Future work will focus on integrating temporal prediction models and semantic segmentation to handle extreme dynamic scenarios, such as densely moving obstacles, further improving real-time capabilities and overall robustness for robot dog operations.