https://orcid.org/0009-0009-0494-3851
Figures
Abstract
This paper addresses the navigation problem of automated guided vehicles (AGVs) in complex environments by proposing a collaborative trajectory optimization strategy based on the Set Membership Filter (SMF) and Model Predictive Control (MPC). In contrast, the traditional probability distribution-based navigation method, UKF-MPC, often fails to provide a strict safety margin when dealing with UBB noise, thereby increasing the risk of navigation failure. Therefore, this paper proposes a set-membership filter algorithm that constructs a minimum enclosing ellipsoid to enclose the system’s true state in real time and incorporates the geometric characteristics of this ellipsoid as a time-varying penalty term into the MPC optimization objective, thereby achieving dynamic risk avoidance. Finally, comparative experiments under three different types of environmental noise were conducted using MATLAB. The MATLAB simulation results indicate that, under complex environmental disturbances, the SMF-MPC control strategy exhibits a higher navigation success rate and greater robustness than both the traditional UKF-MPC control strategy and the Robust MPC control strategy.
Citation: Ding Z, Chen L, Wang J, Qu Q, Chang L, Cheng Y (2026) AGV navigation in complex environments: A cooperative trajectory optimization strategy based on SMF-MPC. PLoS One 21(8): e0355960. https://doi.org/10.1371/journal.pone.0355960
Editor: B. Omkar Lakshmi Jagan, Vignan’s Institute of Information Technology, INDIA
Received: April 7, 2026; Accepted: July 28, 2026; Published: August 26, 2026
Copyright: © 2026 Ding et al. This is an open access article distributed under the terms of the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original author and source are credited.
Data Availability: All simulation codes required to reproduce the results of this study are openly available at GitHub repository: https://github.com/DZH-cwp/SMF-MPC-Code. No restrictions apply to code access.
Funding: This work was partially supported by the Anhui Provincial Higher Education Institutions Outstanding Young Teachers Training Project (YQYB2024098) and the 2023 Provincial Higher Education Research Project (2023AH052495). There was no additional external funding received for this study. The funders were not involved in the study design, data collection and analysis, decisions regarding publication, or preparation of the manuscript.
Competing interests: There is no conflict of interest.
1. Introduction
With the deep integration of Industry 4.0 and smart logistics, AGVs have become indispensable core equipment in smart manufacturing systems, warehousing and logistics, and automated terminals [1,2]. In complex dynamic environments (such as high-density warehouses and human-robot collaborative workshops), AGVs must not only possess efficient path-tracking capabilities but also ensure extremely high navigation safety and robustness in the face of environmental disturbances [3,4]. However, sensor measurement noise, mechanical transmission errors, and random disturbances caused by uneven surfaces—which are prevalent in real-world industrial settings—pose significant challenges to AGV positioning accuracy and trajectory control [5,6].
Traditional AGV navigation and control frameworks largely rely on stochastic filtering algorithms based on probability distributions, such as the Extended Kalman Filter (EKF) and the Unbiased Kalman Filter (UKF) [7,8]. These methods perform well under the assumption that noise follows a Gaussian distribution [9]. However, in real-world operating conditions, environmental disturbances often exhibit unstructured and time-varying characteristics, manifesting as UBB noise with unknown boundaries rather than an ideal Gaussian distribution [10,11]. In such scenarios, probability-based filtering methods often fail to provide strict state estimation bounds, causing the system to potentially exceed its predefined range under extreme disturbances, thereby increasing the risk of collisions or even leading to navigation failure [12,13].
MPC has been widely applied in the field of AGV motion control due to its ability to effectively handle system constraints and multi-objective optimization. In [14], the authors focused on solving the robust collision-avoidance formation navigation problem for a class of multi-unmanned surface vehicles and developed a novel controller based on distributed MPC to achieve collision-free formation navigation. In [15], a convex optimization model was derived to optimize dynamic feedback laws for linear systems subject to polyhedral uncertainty constraints, and a computationally efficient robust model predictive control law was defined.In recent years, researchers have attempted to combine UKF with MPC to enhance the controller’s robustness against disturbances by predicting the probability distribution of the state [16]. In [17], a highly nonlinear dynamic environment was considered; since simple fixed-parameter linear controllers were unable to cope with the constantly changing dynamics and the various disturbance sources affecting the solar farm, a model predictive control strategy based on the UKF was designed and successfully applied in this field.However, when dealing with safety-critical tasks subject to strict hard constraints, such schemes struggle to mathematically guarantee that the system state remains within the safety domain at all times, due to the lack of an accurate description of state uncertainty [18,19].
As a deterministic estimation method for handling UBB noise, the Set-Membership Filtering algorithm offers a new approach for navigation tasks with high safety requirements [20,21]. By constructing a minimal set that encloses the true state, SMF describes state uncertainty and provides the controller with a well-defined feasible state space. In [22], a set-membership filter is designed for all permissible unknown but bounded noise, such that the ellipsoidal set containing all possible states can be determined using a convex optimization method subject to probabilistic constraints. In [23], a new data-selective adaptive filtering algorithm—the Set-Membership Affine Projection (SM-AP) algorithm—was proposed. This algorithm demonstrates excellent performance in terms of convergence rate, final error, and reduced computational complexity, particularly when processing colored input signals. In [24], the distributed set-membership filtering problem was investigated for a class of time-varying multi-rate systems in sensor networks employing communication protocols. A set of distributed filters was designed, and the effectiveness of the proposed protocol-based distributed set-membership filtering algorithm was verified through numerical simulations. The aforementioned studies demonstrate that, compared to probabilistic filtering, SMF offers significant advantages in terms of robustness. Therefore, exploring a deep cooperative algorithm that integrates SMF and MPC, which transforms the geometric characteristics of state uncertainty into dynamic constraints for the control law, represents a key direction for enhancing the reliability of AGV navigation in complex environments [25,26].
In response to the aforementioned challenges, this paper proposes a collaborative trajectory optimization strategy based on SMF-MPC. The main contributions of this work are as follows:
- Kinematics and Noise Modeling: A kinematic model of the AGV that accounts for nonlinear characteristics was developed, and a refined UBB noise model was introduced to address disturbances in complex environments [27,28];
- SMF-MPC Collaboration Framework: Existing SMF-MPC research has primarily focused on trajectory tracking problems. These studies typically treat the SMF’s estimated ellipsoid merely as a “more accurate single-point estimate,” without actively incorporating the ellipsoid’s geometric information into the MPC optimization objective. This paper provides a detailed derivation of the time update and measurement update algorithms for nonlinear ellipsoid membership filtering. It innovatively introduces the geometric features of the real-time generated minimum enclosing ellipsoid as a time-varying penalty term into the MPC objective function, thereby formulating an MPC optimization problem that incorporates ellipsoid membership constraints;
- Multi-scenario experimental validation: Under three types of environmental noise interference, the proposed SMF-MPC strategy was thoroughly compared with the traditional UKF-MPC. The experimental results demonstrate that this strategy exhibits higher navigation success rates and greater robustness under complex environmental interference [29–31].
2. Development of a system model
2.1. Model development
In SMF-based state estimation, system modeling is a critical component. Appropriate kinematic and observation models not only determine the estimation accuracy of the filter but also influence the formulation of constraints in subsequent MPC optimization. This paper focuses on a typical four-wheel differential-drive AGV, whose motion in a planar environment can be modeled using incomplete constrained kinematic equations, as shown in Fig 1.
When a differential-drive AGV moves on a plane, it exhibits the following characteristics: The AGV controls its linear and angular velocities by adjusting the speeds of its left and right wheels; due to incomplete constraints, its lateral velocity is uncontrollable, and its lateral displacement is determined solely by the heading angle and linear velocity; since control errors, tire slip, and environmental disturbances are present during actual operation, process noise is explicitly incorporated into the model.
Where: represents the AGV’s position in the global coordinate system,
represents the heading angle, and
represents the linear velocity.
Using simplified AGV kinematic equations, the continuous-time state equations are as follows:
The control inputs are:
Using the Euler method to discretize the time with a sampling period , we obtain:
The nonlinear function is defined as:
The observational equation is:
To simplify the problem, this paper adopts a linearly observable model that directly observes all states:
where: is the front-wheel steering angle,
is the acceleration,
is the wheelbase,
is the process noise, and
is the measurement noise.
2.2. UBB Noise assumption
All noise sources in actual AGV systems are physically bounded. These physical boundaries are objectively present, measurable, and tied to specific components, whereas the specific statistical distribution of the noise may vary with factors such as time, temperature, and aging. The classical Kalman filter framework implicitly assumes that the noise follows a Gaussian distribution; however, in industrial environments, a more natural modeling approach is to treat the statistical distribution as unknown while the physical boundaries are known. AGV navigation is a safety-critical task. Probabilistic methods (such as UKF) can only provide a guarantee of “high-probability safety” and cannot rule out low-probability but catastrophic events.
Assumption 1 (UBB noise). The process noise and measurement noise
belong to a known compact ellipsoidal set:
In particular, are known shape matrices that describe the amplitude boundaries of the noise, but do not assume any specific probability distribution for the noise.
The process noise boundary matrix for AGVs is primarily caused by model mismatch, while the measurement noise boundary matrix
originates from the sensors, as follows:
is determined by the maximum unmodeled acceleration;
is determined by the maximum angular velocity fluctuation;
is determined by the maximum acceleration;
are determined by the maximum position measurement error,
is determined by the maximum heading measurement noise, and
is determined by the maximum velocity measurement noise.
Taking into account the process noise and measurement noise described above, we obtain the following boundary matrix:
The choice of the boundary matrix here directly determines the compactness of the ellipsoid estimated by the SMF method and the reliability of the inclusion guarantee. Specifically, the shape matrix is chosen to be a diagonal matrix whose diagonal elements are equal to the square of the maximum amplitude of each component. This choice ensures that the actual noise vector lies inside the ellipsoid and, when the components are independent of one another, that the ellipsoid precisely encloses the rectangular region of the noise.
3. Design of set-membership filter algorithm
The objective of set-member filtering is to recursively estimate the feasible set for the state. It assumes that the noise is unknown but bounded, and continuously adjusts the size and orientation of the ellipsoid based on observed data so that, while satisfying the constraints, the true state
satisfies
. In set-member filtering,
is approximated by an ellipsoid:
Where: is the center of the ellipsoid, and
is the shape matrix, which determines the size and orientation of the ellipsoid. Assuming that the feasible set at time
is
, we consider the nonlinear system (2) together with the UBB process noise (8) and (9) to further compute the predicted feasible set.
3.1. Linearization of nonlinear functions
Perform a Taylor expansion of the nonlinear function at
, retaining the first-order term:
Where: is the linearized residual term, and
is the Jacobian matrix.
The specific form of the Jacobian matrix is:
In the time update of the SMF, the linearization residual term of the nonlinear function
must be bounded by the ellipsoid
. The Lipschitz constant provides the theoretical basis for this bound.
Lemma 1 (Ellipsoidal Bounds for the Taylor Residual). If has a Lipschitz-continuous gradient on
, then the linearized residual satisfies:
Where: is the Lipschitz constant of
.
Furthermore, using , we obtain:
Therefore, .
For the AGV model, the Lipschitz constants of the components of are as follows:
The gradients of the components of are:
An upper bound on the gradient norm is:
Where: .
The Lipschitz constant is defined as an upper bound on the gradient norm:
Based on Lemma 1 above, we obtain:
Where: is the Lipschitz constant of
at
.
Lemma 2 (Minkowski sum of ellipsoids). For any two ellipsoids and
, their vector sum lies within the ellipsoid:
Proof of Lemma 2. For any , there exist
. By Cauchy’s inequality, for any
:
From Formula (24), we can obtain . It can also be expressed as
. By adjusting the coefficients, we obtain the standard form. □.
Theorem 1. If , then we further predict that state
is contained within ellipsoid
.
Parameter is selected through optimization by minimizing the ellipsoid volume:
Where: ,
,
,
.
Proof of Theorem 1. From the properties of ellipsoidal linear transformations, we can see that . The vector sum of the UBB noise ellipsoid
and the linearization error ellipsoid
is
. By Lemma 1, for any
, we have.
Since the linearization error and process noise are independent of each other, the prediction set is the sum of the two ellipsoids mentioned above:
Therefore, formula (29) verifies that the predicted state lies within the ellipsoid.
3.2. Measurement update
Suppose the prior feasible set is ; after obtaining a new observation
, we need to further compute the posterior feasible set. Expand the observation function
at
:
Where: is the linearized residual term, enclosed by the ellipsoid
. For a linear observation model,
is a constant matrix,
, and therefore
.
Create an observation strip. the observation information defines the strip area:
Taking linearization errors into account, this can be approximated as an expansion band:
Where: is the expansion factor used to compensate for linearization errors, and may be set to an upper bound of
. For linear observations,
applies.
Theorem 2. Predict that the intersection of ellipsoid and strip
is contained within ellipsoid
, where:
the remaining terms are as follows:
In the measurement update, is determined by minimizing the posterior ellipsoid volume:
Where: ,
Constraints: To ensure that , the following conditions must be satisfied:
.
is equivalent to
.
Proof of Theorem 2. The minimum-volume enveloping ellipsoid where an ellipsoid intersects a strip can be obtained by solving the following optimization problem:
For any A that satisfies the two constraints, we have:
Two constraints:
Substituting (38) gives:
By the definition of .
(otherwise the intersection is empty), therefore:
Parameter is determined by minimizing
, by the matrix determinant lemma:
Substituting the expression for yields an expression for
in terms of
. By solving for
, we obtain the optimal
. The analytical solution is as follows:
Where:
Parameter is determined by minimizing
; this result is in full agreement with Theorem 2. □
4. Design of the SMF-MPC obstacle avoidance algorithm
Model Predictive Control has become the mainstream method for path planning due to its ability to handle constraints and the forward-looking nature of rolling optimization. However, traditional MPC methods assume that the system state is fully known or accurately estimated through filtering, which poses two major challenges in practical applications: first, the problem of dynamic obstacle avoidance caused by environmental complexity; and second, estimation errors resulting from perception uncertainty. The set-membership filtering method adopted in this paper provides a deterministic framework for state estimation under UBB noise. Unlike traditional Kalman filtering, SMF does not assume a specific noise distribution; instead, it computes a minimal ellipsoid set containing the true state through recursive calculations, thereby providing a deterministic uncertainty boundary. By combining SMF with MPC, state uncertainty can be accounted for during the control process, enabling safer obstacle-avoidance navigation.
In this section, building upon the theory of set-membership filter, we propose a set-membership filtering-based MPC obstacle avoidance algorithm. The system block diagram is shown in Fig 2. The main contributions include:
- introducing an ellipsoid-based state estimation into the MPC framework to establish an ellipsoid-obstacle distance constraint;
- designing a time-varying penalty function based on the geometric characteristics of the ellipsoid to achieve dynamic risk avoidance.
4.1. Definition of the obstacle model and position estimation ellipsoid
Suppose there are static circular obstacles in a complex environment, and the
th obstacle is described by its center position
and radius
as follows:
The AGV’s body is simplified to a circle centered at point with a radius of
. The obstacle avoidance constraints are as follows:
Where: represents the AGV’s position, and
represents the safety margin.
Define the position estimation ellipsoid:
where: , and
is a 2 × 2 submatrix consisting of the components at the corresponding positions in
.
4.2. Calculation of distance between an ellipsoid and an obstacle
In the presence of state estimation uncertainty, the true state is unknown; it is only known to lie within the estimated ellipsoid
. The requirements for deterministic obstacle avoidance are as follows: the obstacle avoidance constraints must be satisfied for all possible states, as shown below:
This is equivalent to the minimum distance between the ellipsoid and the obstacle being greater than zero:
The ellipsoid-to-obstacle distance is defined as:
For a circular obstacle and an ellipsoid
, the minimum distance can be obtained by solving an optimization problem:
In (53), is subject to
.
Theorem 3 (Minimum Distance from an Ellipsoid to a Point). The minimum distance from the ellipsoid to the point
is:
Proof of Theorem 3. Using the Lagrange multiplier method, construct the Lagrangian:
Given the KKT conditions, the optimal solution satisfies:
Substituting into the constraints yields:
Substituting into the original expression proves the point.
Therefore, the distance between the ellipsoid and the obstacle is:
4.3. Soft constraint handling for obstacle avoidance
Directly imposing a deterministic obstacle avoidance constraint may render the optimization problem infeasible, particularly when the prediction horizon is long. This paper employs a penalty function approach to treat the obstacle avoidance constraint as a soft constraint. The risk metric function is defined as:
Where: represents the volume of the position estimation ellipsoid, reflecting the magnitude of position uncertainty;
is a tuning parameter that controls the extent to which uncertainty affects risk; and
is a small constant used to prevent the denominator from becoming zero.
The risk metric has the following properties: when the ellipsoid is far from the obstacle,
is large and
; when the ellipsoid is close to the obstacle,
is small and
; the larger the volume of the ellipsoid (the higher the uncertainty), the larger the risk metric, even if the center-to-center distance remains the same.
4.4. SFM-MPC basic framework
Based on the set-membership filtering estimation algorithm, we address the soft constraint problem of obstacle avoidance in the MPC control algorithm and derive the SFM-MPC optimization algorithm.
At time step , the optimization variables for the MPC are:
Where: denotes the control input at time
predicted at time
. The corresponding state trajectory is:
Where: ,
.
Objective Function :
The costs for each phase are as follows:
The terminal cost is:
Where:is the terminal weight matrix,
is the Obstacle Avoidance Penalty Coefficient.
The MPC optimization problem is subject to the following constraints:
Input Constraints:
Where: rad,
5 rad,
m/s2,
m/s2.
State Constraints:
At the same time, soft constraints are implemented through the risk metric in the objective function to ensure that the optimization problem has a feasible solution under all circumstances.
In summary, at each time step k, SMF-MPC solves the following optimization problem:
Where:
The SMF-MPC obstacle avoidance navigation algorithm is summarized as follows:
Step 1: Initialization process. Given an initial state estimate and an initial ellipsoid
, generate a reference trajectory
;
Step 2: For k = 0, 1, 2, …, the set-membership filter estimator performs a state update. First,
and
are computed (Theorem 1), and the observation
is obtained; then, the ensemble filter performs a measurement update, computing
and
(Theorem 2);
Step 3: The MPC performs rolling optimization by first constructing the sequence of state ellipsoids within the prediction horizon, then calculating the risk metric
for each obstacle at each time step, and solving the optimization problem (67); finally, the optimal control sequence
is obtained;
Step 4: Continue until , then exit the loop.
5. Simulation and analysis
To verify the effectiveness of the SMF-MPC algorithm proposed in this paper, simulation tests were conducted in the MATLAB environment. The tests focused on evaluating the SMF-MPC algorithm’s ability to resist interference under various external noise conditions, ensuring that the AGVs could accurately avoid obstacles and reach its destination, and comparing its performance with that of the UKF-MPC algorithm and the robust MPC algorithm. The simulation experiments were divided into the following two parts:
- The filtering performance of the proposed SMF filter is tested against that of the UKF filter under complex conditions;
- Comparing the navigation accuracy of the proposed SMF-MPC algorithm with that of the UKF-MPC algorithm and the robust MPC algorithm under complex conditions.
5.1. SFM vs. UKF
In this experiment, based on the AGV model, the state vector consists of four components: position coordinates and
(
), heading angle
(
), and longitudinal velocity
(
). The control inputs are the front-wheel steering angle
and the longitudinal acceleration
.Using MATLAB tools, set the sampling period
, simulation step size
, initial state vector
, and initial control vector
. To evaluate the state estimation performance of the SMF filter in complex environments, this paper constructs a simulation scenario comprising 200 time steps and conducts experimental comparisons with the UKF filter. Rather than being a repetition of a single operating condition, the scenario is divided into seven distinct phases, each corresponding to typical complex factors such as acceleration, sharp turns, and deceleration. The purpose of this approach is to thoroughly test the filter’s adaptive capabilities under non-stationary conditions through phased transitions between operating conditions.The seven specific phases are as follows:
- Steps 1–30: Gentle S-curve acceleration
, testing smooth tracking capability;
- Steps 31–55: Reverse S-curve with slight deceleration
, testing directional reversal response;
- Steps 56–80: Sharp right turn
with acceleration, the most intense maneuver;
- Steps 81–110: Continuous left turn
at constant speed, testing steady-state curve tracking;
- Steps 111–150: Oscillating turns with gradual acceleration, testing dynamic response to rapidly changing inputs;
- Steps 151–175: Emergency left turn
, simulating an obstacle avoidance scenario;
- Steps 176–200: Recovery from oscillation with slight acceleration, testing stability after maneuvering.
Based on the reference trajectory and parameter settings described above, the comparison of SMF and UKF trajectory tracking generated using MATLAB is shown in Fig 3 below:
Fig 3 clearly shows that the actual trajectory (black line) follows a complex S-shaped path with multiple sharp turns. The SMF trajectory (blue) tracks the true trajectory more closely than the UKF trajectory (red), particularly during and after sharp turns (steps 56–80 and 151–175). When outlier measurements occur (marked with orange crosses), the UKF trajectory shifts noticeably toward the outliers, while the SMF trajectory remains largely unaffected.
Fig 4 illustrates the temporal structure of estimation accuracy, with orange vertical bars marking the moments when outlier measurements occurred. Position error : The UKF error exhibits sharp spikes that precisely correspond to the outlier events, typically reaching 2–4 times the nominal error level. The SMF error remains relatively stable amidst these disturbances, demonstrating effective outlier rejection. Heading error (
): The UKF’s heading error spikes at the time of outliers cascade into sustained position drift, whereas the SMF’s robust heading estimation prevents this chain of error propagation. Velocity error (
): Both filters exhibit similar behavior during normal operation, but the UKF displays significantly larger transient errors at the time of outliers. Since velocity directly influences the next position prediction, these UKF velocity errors accumulate over time.
Fig 5 shows the sliding window RMSE (window length = 10 steps = 1 second), which provides a localized measure of estimation accuracy capable of capturing both persistent and transient errors. The shaded area between the UKF RMSE and SMF RMSE curves intuitively illustrates the advantage of the SMF at each time step.
Across all four state dimensions, the SMF RMSE curve generally lies below the UKF RMSE curve. During periods of concentrated outliers, the gap widens significantly, with the UKF RMSE surging to 3–5 times the level of the SMF RMSE. Even during intervals without outliers, the SMF maintains a slight advantage.
Fig 6 provides a comprehensive statistical comparison of position error CDFs: the median position error of the SMF at the 50th percentile (median) is significantly smaller than that of the UKF, indicating superior performance under typical conditions. 95th percentile (worst-case scenario): This metric is particularly important for safety-critical AGV applications. The 95th percentile error of SMF is significantly tighter, implying a substantial reduction in uncertainty in worst-case estimates. The SMF CDF curve rises more steeply and reaches saturation earlier, confirming that its error distribution is more concentrated around zero.
Fig 7 shows that SMF achieves consistent improvements across all four state dimensions, further validating the advantages of SMF.
Fig 8, the state-space plot, provides qualitative verification that both filters produce physically meaningful estimates. The gray scatter plot shows significant noise contamination, with measurements deviating significantly from the true trajectory, particularly at outlier points. The SMF estimate (blue) tracks the true value (black) more closely throughout the simulation, particularly during periods of aggressive maneuvering (steps 56–80 and 151–175). The UKF estimate (red) exhibits noticeable oscillations and occasional overshoot at time steps affected by outliers, consistent with the error analysis in Fig 4.
Fig 9 shows the control input curves, which demonstrate the complexity of the simulation scenario. The steering angle varies between approximately −0.50 and +0.55 radians, covering both gentle curves and sharp turns. The acceleration curves include acceleration, deceleration, and constant-speed phases. This diversity in control inputs ensures that the filter is not biased toward any specific operating condition, and the observed superiority of the SMF holds true across the full range of complex AGV maneuvers.
5.2. The comparison of SMF-MPC, UKF-MPC, and Robust-MPC
To verify the effectiveness of the SMF-MPC algorithm proposed in this paper, simulation tests were conducted in the MATLAB environment. The primary focus was on evaluating the SMF-MPC algorithm’s ability to resist interference under various external noise conditions, ensuring that the AGV could accurately avoid obstacles and reach its destination. Comparative tests were also conducted with the UKF-MPC algorithm and robust-MPC(R-MPC). In this study, the prediction time window was set to , the control time window to
, the sampling time to
, and the simulation step size to
. This paper uses the fmincon solver, with the maximum number of iterations set to 500 and the maximum function evaluation set to 2000. The reference speed of the AGV is set to
, the steering angle is set to
, the safe distance from obstacles is set to
, the workspace dimensions for obstacle configuration are set to
, and a total of 5 cylindrical obstacles of size
are placed. Refer to Table 1 for specific settings. Each obstacle avoidance test is performed 100 times:
Specifically: Obstacle blocks the AGV’s direct path from A to B (gap =
), so the AGV’s planned path must first deviate slightly to the right of
; the center of
is only
from the target B, so the AGV’s planned path must pass to the left of
(safety margin
).
Based on an analysis of the above assumptions, an optimal reference trajectory was determined. Using the SFM-MPC control algorithm designed in this paper, the AGV’s obstacle-avoidance performance was tested in MATLAB under Gaussian noise, uniform noise, and mixed noise conditions. The test results are shown in the figure below:
Fig 10 shows that, under Gaussian noise conditions, all three algorithms are generally capable of navigating from point A to point B, with success rates exceeding 90% for all of them. The trajectory of SMF-MPC closely matches the optimal reference path, with a maximum deviation and an average deviation
, indicating good path alignment. The trajectories of UKF-MPC and R-MPC are generally feasible, with a slight lateral deviation at point
.
Fig 11 shows that, under the influence of white noise, the success rates of UKF-MPC and R-MPC navigation drop significantly, with noticeable trajectory deviations appearing in the gap region at point . However, SMF-MPC is still able to maintain close tracking of the reference trajectory.
Fig 12 illustrates the most challenging scenario. The sigma points of the UKF are subject to impulse disturbances, leading to increased errors in the estimated peak position. Consequently, the UKF-MPC performs calculations based on the erroneous position estimates, causing the actual trajectory to enter the obstacle safety zone. In contrast, the SMF-MPC and RMPC relatively ensure a high navigation success rate.
Table 2 clearly illustrates the navigation accuracy of the three approaches under different noise test environments. Among them, the SMF-MPC algorithm proposed in this paper demonstrates the best overall performance; even under mixed noise conditions, it maintains a navigation accuracy of over 80%, providing a solid guarantee for accurate navigation in complex environments (Fig 13).
The 3 × 4 submatrix shown in Fig 13 covers all 12 (scenario × state variable) combinations. The velocity error column (Column 4) is most pronounced under mixed noise: the UKF velocity RMSE exhibits pulsed spikes, with peaks reaching approximately 4.5 times the baseline, directly leading to the MPC calculating abnormal acceleration commands and increasing the probability of collision. The shadow confidence intervals () for each state variable in the SMF are consistently narrower than those in the UKF, demonstrating the effective suppression of estimation uncertainty by the bounded set constraint.
As shown in Fig 14, under mixed noise conditions, the steering angles of the UKF-MPC and R-MPC exhibited several brief instances of saturation, whereas the control signal of the SMF-MPC remained within the constraint range at all times, thereby providing a more generous safety margin.
A comparison of the experimental results from the three groups in Fig 15 shows that the speed curves for both algorithms exhibit a reasonable S-shaped pattern, namely acceleration—constant-speed cruise—deceleration: First, during the acceleration phase, both algorithms smoothly accelerate to a cruise speed of approximately, with SMF-MPC exhibiting significantly less overshoot than UKF-MPC and R-MPC. Subsequently, during the cruising phase, the standard deviation of velocity fluctuations for SMF-MPC was approximately 60%, 45%, and 35% of that of UKF-MPC, respectively; finally, during the deceleration phase toward the endpoint, SMF-MPC began to decelerate smoothly in advance at a distance of approximately from target B, whereas UKF-MPC and R-MPC began decelerating too late (Fig 16).
Fig 16 provides a theoretical explanation for the performance gap from an information-theoretic perspective: Gaussian noise (): the gap between the three algorithms is minimal, and the UKF-MPC operates under optimal conditions; uniform noise (
): the tails of the flat-top distribution are heavier than those of the Gaussian distribution, making the UKF’s Gaussian assumption suboptimal; mixed noise (
): the tails of the bilateral impulse response are pronounced, and the UKF suffers the most severe degradation.
The success rate of the UKF shows a strong negative correlation with the KL divergence (Pearson r ≈ −0.998), quantitatively confirming that the fundamental driver of UKF-MPC performance degradation is the degree to which the noise distribution deviates from the Gaussian distribution. The bounded set method of SMF-MPC is insensitive to the KL metric, demonstrating the inherent advantage of distribution-independent robustness.
6. Conclusions
This paper comprehensively examines the impact of noise on the navigation reliability of AGVs in complex environments and designs an SMF-MPC algorithm to ensure stable navigation despite various external disturbances. First, building upon the development of the SMF algorithm, this paper uses MATLAB simulations to demonstrate that, under complex noise conditions, the filtering performance of SMF outperforms that of UKF. Based on this, the paper further develops the SMF-MPC algorithm, innovatively incorporating the geometric characteristics of the estimated ellipsoid as a time-varying penalty term into the MPC objective function. Through MATLAB simulations involving three different noise conditions and obstacle-containing scenarios, this study demonstrates that the SMF-MPC algorithm achieves significantly improved performance compared to the traditional UKF-MPC algorithm, thereby providing a reference for research on AGV navigation in complex environments. In the future, this research will conduct tests based on a differential-drive AGV prototype to further validate the feasibility of the designed algorithm.
References
- 1. Fragapane G, de Koster R, Sgarbossa F, Strandhagen JO. Planning and control of autonomous mobile robots for intralogistics: Literature review and research agenda. European Journal of Operational Research. 2021;294(2):405–26.
- 2. Wang X, Lu J, Ke F, Wang X, Wang W. Research on AGV task path planning based on improved A* algorithm. Virtual Reality & Intelligent Hardware. 2023;5(3):249–65.
- 3. Amer NH, Zamzuri H, Hudha K, Kadir ZA. Modelling and Control Strategies in Path Tracking Control for Autonomous Ground Vehicles: A Review of State of the Art and Challenges. J Intell Robot Syst. 2016;86(2):225–54.
- 4. Wang H, Liu B, Ping X, An Q. Path Tracking Control for Autonomous Vehicles Based on an Improved MPC. IEEE Access. 2019;7:161064–73.
- 5. Gasparetto A, Boscariol P, Lanzutti A, Vidoni R. Path Planning and Trajectory Planning Algorithms: A General Overview. Mechanisms and Machine Science. Springer International Publishing. 2015. p. 3–27.
- 6. Wang Q, Wu J, Liao Y. Research on multi-sensor fusion localization for forklift AGV based on adaptive weight extended Kalman filter. Sensors. 2025;25(18):5670.
- 7. Giannitrapani A, Ceccarelli N, Scortecci F, Garulli A. Comparison of EKF and UKF for Spacecraft Localization via Angle Measurements. IEEE Trans Aerosp Electron Syst. 2011;47(1):75–84.
- 8. He H, Qin H, Sun X, Shui Y. Comparison Study on the Battery SoC Estimation with EKF and UKF Algorithms. Energies. 2013;6(10):5088–100.
- 9. Zeng Y, Zhang M, Han F, Gong Y, Zhang J. Spectrum Analysis and Convolutional Neural Network for Automatic Modulation Recognition. IEEE Wireless Commun Lett. 2019;8(3):929–32.
- 10. Zhu H, Luo J, Luo M, Minane J. A Recursive Robust Set-Membership Estimator for WSN-Assisted Moving Targets Tracking With UBB Anchor Location Uncertainty. IEEE Trans Veh Technol. 2023;72(5):6547–57.
- 11. Asgari M, Khaloozadeh H. Robust extended Kalman filtering for non‐linear systems with unknown input: a UBB model approach. IET Radar Sonar & Navi. 2020;14(11):1837–44.
- 12. Zhao Y, Zhang J, Hu G, Zhong Y. Set-Membership Based Hybrid Kalman Filter for Nonlinear State Estimation under Systematic Uncertainty. Sensors (Basel). 2020;20(3):627. pmid:31979194
- 13. Liu Y, Hou Z, Tan Y, Liu H, Song C. Research on Multi-AGVs Path Planning and Coordination Mechanism. IEEE Access. 2020;8:213345–56.
- 14. Wen G, Lam J, Fu J, Wang S. Distributed MPC-Based Robust Collision Avoidance Formation Navigation of Constrained Multiple USVs. IEEE Trans Intell Veh. 2024;9(1):1804–16.
- 15. Cannon M, Kouvaritakis B. Optimizing prediction dynamics for robust MPC. IEEE Trans Automat Contr. 2005;50(11):1892–7.
- 16. Bao J, Lin Z, Jing H, Feng H, Zhang X, Luo Z. Research on Longitudinal Control of Electric Vehicle Platoons Based on Robust UKF–MPC. Sustainability. 2024;16(19):8648.
- 17. Gallego AJ, Sánchez AJ, Berenguel M, Camacho EF. Adaptive UKF-based model predictive control of a Fresnel collector field. Journal of Process Control. 2020;85:76–90.
- 18. Chen R, Li Y, Chai T. Risk-Constrained MPC With Application in Safety-Critical Control of Turbofan Engines. IEEE Trans Automat Sci Eng. 2025;22:872–82.
- 19. Esfahani HN, Ahmadi S, Velni JM. In: 2024. 1658–63.
- 20. Zou L, Wang Z, Geng H, Liu X. Set-Membership Filtering Subject to Impulsive Measurement Outliers: A Recursive Algorithm. IEEE/CAA J Autom Sinica. 2021;8(2):377–88.
- 21. Ding D, Wang Z, Han Q-L. A Set-Membership Approach to Event-Triggered Filtering for General Nonlinear Systems Over Sensor Networks. IEEE Trans Automat Contr. 2020;65(4):1792–9.
- 22. Wei G, Liu S, Song Y, Liu Y. Probability-guaranteed set-membership filtering for systems with incomplete measurements. Automatica. 2015;60:12–6.
- 23. Werner S, Diniz PSR. Set-membership affine projection algorithm. IEEE Signal Process Lett. 2001;8(8):231–5.
- 24. Liu S, Wang Z, Wei G, Li M. Distributed Set-Membership Filtering for Multirate Systems Under the Round-Robin Scheduling Over Sensor Networks. IEEE Trans Cybern. 2020;50(5):1910–20. pmid:30629526
- 25. Liang Z, Hu J, Zhang Y. Set‐membership filtering based model predictive control for trajectory tracking of automated guided vehicles. International Journal of Adaptive Control and Signal Processing. 2024;38(12):3819–29.
- 26. Yang H, Zhang Y, Gu W, Yan H, Yang F. A Novel Global Set-Membership Filtering Approach for Localization of Automatic Guided Vehicles. IEEE Trans Ind Inf. 2023;19(3):3219–28.
- 27.
Yang H, Zhang Y, Gu W. Quantized set-membership filtering for automatic guided vehicles with unknown-but-bounded noises. In: 2021 40th Chinese Control Conference (CCC), 2021. 4769–74. https://doi.org/10.23919/ccc52363.2021.9549551
- 28.
Weckx S, Vandewal B, Rademakers E, et al. Open experimental AGV platform for dynamic obstacle avoidance in narrow corridors. In: 2020 IEEE Intelligent Vehicles Symposium (IV), 2020. 844–51. https://doi.org/10.1109/IV47402.2020.9304749
- 29. Dundar YC. Dynamic path finding method and obstacle avoidance for automated guided vehicle navigation in Industry 4.0. Procedia Computer Science. 2021;192:3945–54.
- 30. Digani V, Sabattini L, Secchi C, Fantuzzi C. Ensemble Coordination Approach in Multi-AGV Systems Applied to Industrial Warehouses. IEEE Trans Automat Sci Eng. 2015;12(3):922–34.
- 31. Bogdan S, Vasiljevic G, Kovacic Z, Miklic D, Draganjac I. Decentralized Control of Multi-AGV Systems in Autonomous Warehousing Applications. IEEE Trans Automat Sci Eng. 2016;13(4):47–1433.