AGV navigation in complex environments: A cooperative trajectory optimization strategy based on SMF-MPC Zonghe Ding, Ling Chen, Jintao Wang, Qi Qu, Liming Chang, Yajing Cheng 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. 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 . 10.1371/journal.pone.0355960.g001 Fig 1 Four-wheel differential drive AGV. 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. X = [ @ c c c c @ x y θ v ] T (1) Where: ( @ c c @ x y ) represents the AGV’s position in the global coordinate system, θ represents the heading angle, and v represents the linear velocity. Using simplified AGV kinematic equations, the continuous-time state equations are as follows: X ˙ = [ @ c @ x ˙ y ˙ θ ˙ v ˙ ] = [ @ c @ v c o s θ v s i n θ v t a n δ L a ] (2) The control inputs are: u = [ @ c c @ δ a ] T (3) Using the Euler method to discretize the time with a sampling period T s , we obtain: X k + 1 = f ( X k , u k ) + w k (4) The nonlinear function f : R 4 × R 2 → R 4 is defined as: f ( X k , u k ) = [ @ c @ x k + T s v k c o s θ k y k + T s v k s i n θ k θ k + T s v k t a n δ k L v k + T s a k ] (5) The observational equation is: Y k = h ( X k ) + v k (6) To simplify the problem, this paper adopts a linearly observable model that directly observes all states: h ( X k ) = H X k = [ @ c c c c @ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ] X k (7) where: δ k is the front-wheel steering angle, a k is the acceleration, L is the wheelbase, w k ∈ R 4 is the process noise, and v k ∈ R 4 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 w k and measurement noise v k belong to a known compact ellipsoidal set: w k ∈ W = { ω : ω T Q − 1 ω ≤ 1 } (8) v k ∈ V = { v : v T R − 1 v ≤ 1 } (9) In particular, Q ≻ 0 a n d R ≻ 0 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 Q for AGVs is primarily caused by model mismatch, while the measurement noise boundary matrix R originates from the sensors, as follows: w k = [ w x , w y , w θ , w v ] T (10) v k = [ v x , v y , v θ , v v ] T (11) w x , w y is determined by the maximum unmodeled acceleration; w θ is determined by the maximum angular velocity fluctuation; w v is determined by the maximum acceleration; v x , v y are determined by the maximum position measurement error, v θ is determined by the maximum heading measurement noise, and v v 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: Q = diag ( q x 2 , q y 2 , q θ 2 , q v 2 ) (12) R = diag ( r x 2 , r y 2 , r θ 2 , r v 2 ) (13) 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 χ k 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 χ k satisfies X k ∈ χ k . In set-member filtering, χ k is approximated by an ellipsoid: ε ( X ^ k , P k ) = { X : ( X − X ^ k ) T P k − 1 ( X − X ^ k ) ≤ 1 } (14) Where: X ^ k is the center of the ellipsoid, and P k ≻ 0 is the shape matrix, which determines the size and orientation of the ellipsoid. Assuming that the feasible set at time k is ε ( X ^ k , P k ) , 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 f at X ^ k , retaining the first-order term: f ( X k , u k ) = f ( X ^ k , u k ) + F k ( X k − X ^ k ) + e f (15) Where: e f is the linearized residual term, and F k = ∂ f ∂ X | X ^ k is the Jacobian matrix. The specific form of the Jacobian matrix F k is: F k = [ @ c c c c @ 1 0 − T s v k s i n θ k T s c o s θ k 0 1 T s v k c o s θ k T s s i n θ k 0 0 1 T s t a n δ k / L 0 0 0 1 ] (16) In the time update of the SMF, the linearization residual term e f of the nonlinear function f must be bounded by the ellipsoid E ( 0 , 𝐄 f ) . The Lipschitz constant provides the theoretical basis for this bound. Lemma 1 (Ellipsoidal Bounds for the Taylor Residual). If f i has a Lipschitz-continuous gradient on E ( 𝐱 ^ k , 𝐏 k ) , then the linearized residual satisfies: | e f , i | ≤ 1 2 L i · ∥ 𝐱 − 𝐱 ^ k ∥ 2 (17) Where: L i is the Lipschitz constant of ∇ f i . Furthermore, using ∥ 𝐱 − 𝐱 ^ k ∥ 2 ≤ λ max ( 𝐏 k − 1 ) , we obtain: | e f , i | ≤ 1 2 L i · λ max ( 𝐏 k − 1 ) ≜ e ¯ f , i (18) Therefore, 𝐄 f = diag ( e ¯ f , 1 2 , … , e ¯ f , n 2 ) . For the AGV model, the Lipschitz constants of the components of f = [ f 1 , f 2 , f 3 , f 4 ] T are as follows: f 1 = x + T s v c o s θ , f 2 = y + T s v s i n θ , f 3 = θ + T s v t a n δ / L , f 4 = v + T s a (19) The gradients of the components of 𝐱 = [ x , y , θ , v ] T are: ∇ f 1 = [ 1 , 0 , − T s v sin θ , T s cos θ ] T ∇ f 2 = [ 0 , 1 , T s v cos θ , T s sin θ ] T ∇ f 3 = [ 0 , 0 , 1 , T s tan δ / L ] T ∇ f 4 = [ 0 , 0 , 0 , 1 ] T (20) An upper bound on the gradient norm is: ∥ ∇ f 1 ∥ ≤ 1 + ( T s v m a x ) 2 + T s 2 ≈ 1 . 012 ∥ ∇ f 2 ∥ ≤ 1 + ( T s v m a x ) 2 + T s 2 ≈ 1 . 012 ∥ ∇ f 3 ∥ ≤ 1 + ( T s tan δ m a x / L ) 2 ≈ 1 . 002 ∥ ∇ f 4 ∥ = 1 (21) Where: T s = 0 . 1s , v m a x = 1 . 5m / s , δ m a x = 0 . 5rad . The Lipschitz constant is defined as an upper bound on the gradient norm: L 1 = L 2 = 1 . 012 , L 3 = 1 . 002 , L 4 = 1 (22) Based on Lemma 1 above, we obtain: E f = d i a g ( [ L 1 2 4 , L 2 2 4 , L 3 2 4 , L 4 2 4 ] ) (23) Where: L i is the Lipschitz constant of f i at ε ( X ^ k , P k ) . Lemma 2 (Minkowski sum of ellipsoids). For any two ellipsoids ε ( 0 , A ) and ε ( 0 , \hspace{0.17em } B ) , their vector sum lies within the ellipsoid: ε ( 0 , \hspace{0.17em } A ) + ε ( 0 , B ) ⊆ ε ( 0 , ( 1 + p ) A + ( 1 + p − 1 ) B ) , ∀ p > 0 (24) Proof of Lemma 2 . For any a ∈ ε ( 0 , A ) , b ∈ ε ( 0 , B ) , there exist a T A − 1 a ≤ 1 , b T B − 1 b ≤ 1 . By Cauchy’s inequality, for any p > 0 : ( a + b ) T [ ( 1 + p ) A + ( 1 + p − 1 ) B ] − 1 ( a + b ) ≤ a T A − 1 a + b T B − 1 b ≤ 2 (25) From Formula (24), we can obtain 1 2 ( a + b ) ∈ ( 0 , ( 1 + p ) A + ( 1 + P − 1 ) B ) . It can also be expressed as a + b ∈ ε ( 0 , 2 [ ( 1 + p ) A + ( 1 + p − 1 ) B ] ) . By adjusting the coefficients, we obtain the standard form. □. Theorem 1. If X k ∈ ε ( X ^ k , P k ) , w k ∈ ( 0 , Q ) , then we further predict that state X k + 1 | k is contained within ellipsoid ε ( X ^ k + 1 | k , P k + 1 | k ) . X ^ k + 1 | k = f ( X ^ k , u k ) (26) P k + 1 | k = ( 1 + p ) F k P k F k T + ( 1 + p − 1 ) ( E f + Q ) (27) Parameter p > 0 is selected through optimization by minimizing the ellipsoid volume: p * = a r g m i n p > 0 d e t ( P k + 1 | k ( p ) ) (28) Where: P k + 1 | k ( p ) = ( 1 + p ) M 1 + ( 1 + p − 1 ) M 2 , M 1 = F k P k F k T , M 2 = E f + Q , p * ∈ [ @ c c @ 0.1 2.0 ] . Proof of Theorem 1 . From the properties of ellipsoidal linear transformations, we can see that F k ε ( X ^ k , P k ) ⊆ ε ( F k X ^ k , F k P k F k T ) . The vector sum of the UBB noise ellipsoid ε ( 0 , Q ) and the linearization error ellipsoid ε ( 0 , E f ) is ε ( 0 , E f ) + ε ( 0 , Q ) . By Lemma 1, for any p > 0 , we have. ε ( 0 , E f ) + ε ( 0 , Q ) ⊆ ε ( 0 , ( 1 + p ) E f + ( 1 + p − 1 ) Q ) (29) Since the linearization error and process noise are independent of each other, the prediction set is the sum of the two ellipsoids mentioned above: χ k + 1 | k = F k ε ( X ^ k , P k ) + [ ε ( 0 , E f ) + ε ( 0 , Q ) ] (30) Therefore, formula (29) verifies that the predicted state lies within the ellipsoid. 3.2. Measurement update Suppose the prior feasible set is ε ( X ^ k + 1 | k , P k + 1 | k ) ; after obtaining a new observation Y k + 1 , we need to further compute the posterior feasible set. Expand the observation function h at X ^ k + 1 | k : h ( x ) = h ( X ^ k + 1 | k ) + H k + 1 ( X − X ^ k + 1 | k ) + e h (31) Where: H k + 1 = ∂ h ∂ X | X ^ k + 1 | k , e h is the linearized residual term, enclosed by the ellipsoid ε ( 0 , E h ) . For a linear observation model, H k + 1 = H is a constant matrix, e h = 0 , and therefore E h = 0 . Create an observation strip. the observation information defines the strip area: S k + 1 = { X : ( Y k + 1 − h ( x ) ) T R − 1 ( Y k + 1 − h ( x ) ) ≤ 1 } (32) Taking linearization errors into account, this can be approximated as an expansion band: S ~ k + 1 = { X : ∥ Y k + 1 − h ( X ^ k + 1 | k ) − H k + 1 ( X − X ^ k + 1 | k ) | | R − 1 ≤ 1 + δ } (33) Where: δ is the expansion factor used to compensate for linearization errors, and may be set to an upper bound of δ = e h T R − 1 e h . For linear observations, δ = 0 applies. Theorem 2. Predict that the intersection of ellipsoid ε ( X ^ k + 1 | k , P k + 1 | k ) and strip S ~ k + 1 is contained within ellipsoid ε ( X ^ k + 1 | k , P k + 1 ) , where: X ^ k + 1 = X ^ k + 1 | k + λ P k + 1 | k H k + 1 T Δ − 1 v (34) P k + 1 = β ( P k + 1 | k − λ P k + 1 | k H k + 1 T Δ − 1 H k + 1 P k + 1 | k ) (35) the remaining terms are as follows: v = Y k + 1 − h ( X ^ k + 1 | k ) Δ = H k + 1 P k + 1 | k H k + 1 T + 1 + δ λ R β = 1 + δ − λ v T Δ − 1 v In the measurement update, λ is determined by minimizing the posterior ellipsoid volume: λ * = arg min λ ∈ [ 0 , 1 ] det ( P k + 1 ( λ ) ) (36) Where: P k + 1 ( λ ) = β ( λ ) ( P k + 1 | k − λ P k + 1 | k H T Δ ( λ ) − 1 H P k + 1 | k ) , λ * ∈ [ @ c c @ 0.05 0.95 ] Constraints: To ensure that P k + 1 ≻ 0 , the following conditions must be satisfied: P k + 1 | k − λ P k + 1 | k H T Δ ( λ ) − 1 H P k + 1 | k ≻ 0 . β ( λ ) > 0 is equivalent to 1 + δ − λ ν T Δ ( λ ) − 1 ν > 0 . Proof of Theorem 2. The minimum-volume enveloping ellipsoid where an ellipsoid intersects a strip can be obtained by solving the following optimization problem: m i n X ^ , P d e t ( P ) s . t . ε ( X ^ k + 1 | k , P k + 1 | k ) ⋂ S ~ k + 1 ⊆ ε ( X ^ , P ) (37) For any A that satisfies the two constraints, we have: ( X − X ^ + ) T P + − 1 ( X − X ^ + ) ≤ 1 (38) ( X − X ^ + ) T P + − 1 ( X − X ^ + ) = 1 β p [ @ l @ ( X − X ^ ) T P − 1 ( X − X ^ ) + 1 δ 2 ( v − H ( X − X ^ ) ) T R − 1 ( v − H ( X − X ^ ) ) − 1 δ 2 v T Δ p − 1 v ] (39) Two constraints: ( X − X ^ ) T P − 1 ( X − X ^ ) ≤ 1 (40) ( v − H ( X − X ^ ) ) T R − 1 ( v − H ( X − X ^ ) ) ≤ δ ― 2 (41) Substituting (38) gives: ( X − X ^ + ) P + − 1 ( X − X ^ + ) ≤ 1 β c ( 1 + λ − 1 δ ― 2 v T Δ p − 1 v ) (42) By the definition of β p . β p = δ ― 2 − λ v T Δ P − 1 v , δ ― 2 ≥ λ v T Δ P − 1 v (otherwise the intersection is empty), therefore: ( X − X ^ + ) T P + − 1 ( X − X ^ + ) ≤ 1 (43) Parameter λ is determined by minimizing d e t ( P + ) , by the matrix determinant lemma: d e t ( P + ) = β p n X d e t ( P ) d e t ( I − λ P 1 2 H T Δ p − 1 H P 1 2 ) (44) Substituting the expression for Δ p yields an expression for d e t ( P + ) in terms of λ . By solving for d d λ d e t ( P + ) = 0 , we obtain the optimal λ . The analytical solution is as follows: X ^ + = X ^ + λ P H T Δ p − 1 v (45) P + = β p ( P − λ P H T Δ p − 1 H P ) (46) Where: Δ p = H P H T + δ ― 2 λ R β p = δ ― 2 − λ v T Δ P − 1 v Parameter λ ∈ [ @ c c @ 0 1 ] is determined by minimizing d e t ( P + ) ; 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: 10.1371/journal.pone.0355960.g002 Fig 2 Block Diagram of the SFM-MPC System. 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 N o b s static circular obstacles in a complex environment, and the j th obstacle is described by its center position o j = [ o j , x , o j , y ] T and radius r j as follows: O j = { p ∈ R 2 : | | p − o j | | ≪ r j } (47) The AGV’s body is simplified to a circle centered at point ( x , y ) with a radius of r A G V . The obstacle avoidance constraints are as follows: ‖ p k − o j ‖ ≥ r j + r A G V + d s a f e , ∀ j , ∀ k (48) Where: p k = [ x k , y k ] T represents the AGV’s position, and d s a f e represents the safety margin. Define the position estimation ellipsoid: E k p o s = { p = [ x , y ] T : ( p − p ^ k ) T ( p k p o s ) − 1 ( p − p ^ k ) ≤ 1 } (49) where: p ^ k = [ x ^ k , y ^ k ] T , and p k p o s is a 2 × 2 submatrix consisting of the components at the corresponding positions in p . 4.2. Calculation of distance between an ellipsoid and an obstacle In the presence of state estimation uncertainty, the true state x t is unknown; it is only known to lie within the estimated ellipsoid E ( x ^ t , P t ) . The requirements for deterministic obstacle avoidance are as follows: the obstacle avoidance constraints must be satisfied for all possible states, as shown below: E t p o s ∩ O j = ⌀ , ∀ j , ∀ t (50) This is equivalent to the minimum distance between the ellipsoid and the obstacle being greater than zero: d ( E t p o s , O j ) > 0 , ∀ j , ∀ t (51) The ellipsoid-to-obstacle distance is defined as: d ( E , O ) = min p ∈ E , o ∈ O ∥ p − o ∥ (52) For a circular obstacle O j ( o j , r j ) and an ellipsoid E ( p ^ , p p o s ) , the minimum distance can be obtained by solving an optimization problem: d ( E , O j ) = max { 0 , min p ∈ E ∥ p − o j ∥ − r j } (53) In (53), min p ∥ p − o j ∥ is subject to ( p − p ^ ) T ( p p o s ) − 1 ( p − p ^ ) ≤ 1 . Theorem 3 (Minimum Distance from an Ellipsoid to a Point). The minimum distance from the ellipsoid E ( p ^ , p pos ) to the point o j is: d m i n = max { 0 , ∥ p ^ − o j ∥ − ( p ^ − o j ) T p p o s ( p ^ − o j ) } (54) Proof of Theorem 3. Using the Lagrange multiplier method, construct the Lagrangian: L = ∥ p − o j ∥ 2 + μ [ ( p − p ^ ) T ( p p o s ) − 1 ( p − p ^ ) − 1 ] (55) Given the KKT conditions, the optimal solution satisfies: p − o j = − μ 1 + μ ( p ^ − o j ) (56) Substituting into the constraints yields: μ = ∥ p ^ − o j ∥ ( p ^ − o j ) T P p o s ( p ^ − o j ) − 1 (57) Substituting into the original expression proves the point. Therefore, the distance between the ellipsoid and the obstacle is: d ( E t p o s , O j ) = ∥ p ^ t − o j ∥ − ( p ^ t − o ) T p t p o s ( p ^ t − o j ) − r j (58) 4.3. Soft constraint handling for obstacle avoidance Directly imposing a deterministic obstacle avoidance constraint d ( E t p o s , O j ) ≥ 0 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: ρ ( E t p o s , O j ) = exp ( − d ( E t p o s , O j ) 2 α · vol ( E t p o s ) + ϵ ) (59) Where: vol ( E t p o s ) ∝ det ( 𝐏 t p o s ) represents the volume of the position estimation ellipsoid, reflecting the magnitude of position uncertainty; α > 0 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, d is large and ρ → 0 ; when the ellipsoid is close to the obstacle, d is small and ρ → 1 ; 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 k , the optimization variables for the MPC are: U k = { u k | k , u k + 1 | k , … , u k + N − 1 | k } (60) Where: 𝐮 t | k ∈ R 2 denotes the control input at time t predicted at time k . The corresponding state trajectory is: X k = { x k | k , x k + 1 | k , … , x k + N | k } (61) Where: 𝐱 t | k ∈ R 4 , 𝐱 k | k = 𝐱 ^ k . Objective Function J O : min U k J O ( X k , U k ) = ∑ t = k k + N − 1 l ( x t | k , u t | k ) + V f ( x k + N | k ) (62) The costs ℓ ( x , u ) for each phase are as follows: ℓ ( x , u ) = ∥ x − x r e f ∥ W 2 ( Tracking Costs ) + ∥ u ∥ U 2 ( Control Costs ) + γ ∑ j = 1 N o b s ρ ( E t | k p o s , O j ) ( Soft Constraint Penalty ) (63) The terminal cost is: V f ( x ) = ∥ x − x r e f ∥ W f 2 (64) Where: 𝐖 f ≽ 0 is the terminal weight matrix, γ is the Obstacle Avoidance Penalty Coefficient. The MPC optimization problem is subject to the following constraints: Input Constraints: δ m i n ≤ δ t | k ≤ δ m a x , a m i n ≤ a t | k ≤ a m a x (65) Where: δ m i n = − 0 . 5 5 rad, δ m a x = 0 . 5 5 rad, a m i n = − 0 . 25 m/s 2 , a m a x = 0 . 25 m/s 2 . State Constraints: v m i n ≤ v t | k ≤ v m a x , v min = 0 . 05m / s , v m a x = 1 . 5 m / s (66) At the same time, soft constraints are implemented through the risk metric ρ ( E t | k p o s , O j ) 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: min U k J ∑ t = k k + N − 1 ( ∥ x t | k − x t r e f ∥ W 2 + ∥ u t | k ∥ U 2 + γ ∑ j = 1 N o b s ρ ( E t | k p o s , O j ) ) + ∥ x k + N | k − x k + N r e f ∥ W f 2 s . t . x t + 1 | k = f ( x t | k , u t | k ) , t = k , … , k + N − 1 δ m i n ≤ δ t | k ≤ δ m a x , t = k , … , k + N − 1 a m i n ≤ a t | k ≤ a m a x , t = k , … , k + N − 1 0 ≤ v t | k ≤ v m a x , t = k , … , k + N − 1 x k | k = x ^ k (67) Where: ρ ( E t|k pos ,O j )=exp ( - d(E t|k pos ,O j ) 2 α · det(P t|k pos ) + ε ) d ( ℰ t | k p o s , O j ) = ∥ p ^ t | k − o j ∥ − ∥ p ^ t | k − o j ∥ ( p ^ t | k − o j ) T ( P t | k p o s ) − 1 ( p ^ t | k − o j ) − r j − r A G V − d s a f e The SMF-MPC obstacle avoidance navigation algorithm is summarized as follows: Step 1: Initialization process. Given an initial state estimate x ^ 0 and an initial ellipsoid p 0 , generate a reference trajectory {x t ref } ; Step 2: For k = 0, 1, 2, … k N , the set-membership filter estimator performs a state update. First, x ^ k+1|k and P k+1|k are computed (Theorem 1), and the observation Y k+1 is obtained; then, the ensemble filter performs a measurement update, computing x ^ k+1 and P k+1 (Theorem 2); Step 3: The MPC performs rolling optimization by first constructing the sequence of state ellipsoids {E t|k pos } t=k k+N-1 within the prediction horizon, then calculating the risk metric ρ ( E t|k pos ,O j ) for each obstacle at each time step, and solving the optimization problem (67); finally, the optimal control sequence u k:k+N-1 * is obtained; Step 4: Continue until k = k N , 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 x and y ( m ), heading angle θ ( r a d ), and longitudinal velocity v ( m / s ). The control inputs are the front-wheel steering angle δ and the longitudinal acceleration a .Using MATLAB tools, set the sampling period T s = 0.1 s , simulation step size N = 200 , initial state vector X = [ @ c c c c @ x y θ v ] T = [ @ c c c c @ 0 0 0 1 ] T , and initial control vector u = [ @ c c @ δ a ] T = [ @ c c @ 0 0.25 ] T . 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 a = 0.25 m / s 2 , testing smooth tracking capability; Steps 31–55: Reverse S-curve with slight deceleration a = − 0.08 m / s 2 , testing directional reversal response; Steps 56–80: Sharp right turn δ = 0.55 r a d ≈ 31 . 5 ∘ with acceleration, the most intense maneuver; Steps 81–110: Continuous left turn δ = − 0.40 r a d 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 δ = − 0.50 r a d , 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: 10.1371/journal.pone.0355960.g003 Fig 3 Comparison of SMF and UKF Trajectory Tracking. 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 ( x , y ) : 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 ( v ): 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. 10.1371/journal.pone.0355960.g004 Fig 4 State-Space Error Plot. 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. 10.1371/journal.pone.0355960.g005 Fig 5 Comparison of RMSE for SMF and UKF. 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. 10.1371/journal.pone.0355960.g006 Fig 6 Position Error CDF. Fig 7 shows that SMF achieves consistent improvements across all four state dimensions, further validating the advantages of SMF. 10.1371/journal.pone.0355960.g007 Fig 7 SMF RMSE Improvement Rate. 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 . 10.1371/journal.pone.0355960.g008 Fig 8 Comparison of State Tracking Methods. 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. 10.1371/journal.pone.0355960.g009 Fig 9 Comparison of State Tracking Methods. 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 N p = 12 , the control time window to N c = 5 , the sampling time to T = 15 s , and the simulation step size to Δ t = 0.05 s . 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 [ @ c c @ 0.05 1.5 ] m / s , the steering angle is set to [ @ c c @ − 0.55 0.55 ] r a d , the safe distance from obstacles is set to 0.2 m , the workspace dimensions for obstacle configuration are set to 14 × 12 m , and a total of 5 cylindrical obstacles of size O 1 − O 5 are placed. Refer to Table 1 for specific settings. Each obstacle avoidance test is performed 100 times: 10.1371/journal.pone.0355960.t001 Table 1 Table of Obstacle Location Parameters. Number Center X ( m ) Center Y ( m ) Radius ( m ) Itinerary O 1 3.0 3.5 0.80 Early detour O 2 6.0 2.0 0.85 Yield O 3 5.0 6.5 0.95 Yield O 4 8.5 4.5 0.80 obstacle O 5 9.5 8.0 0.70 obstacle Specifically: Obstacle O 1 blocks the AGV’s direct path from A to B (gap = − 0.09 m ), so the AGV’s planned path must first deviate slightly to the right of O 1 ; the center of O 5 is only 3.2 m from the target B, so the AGV’s planned path must pass to the left of O 5 (safety margin 1.4 m ). 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 < 0.18 m and an average deviation < 0.06 m , indicating good path alignment. The trajectories of UKF-MPC and R-MPC are generally feasible, with a slight lateral deviation at point O 1 − O 2 . 10.1371/journal.pone.0355960.g010 Fig 10 Comparison of Obstacle Avoidance Performance of SMF-MPC, UKF-MPC, and R-MPC Under Gaussian Noise. 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 O 5 . However, SMF-MPC is still able to maintain close tracking of the reference trajectory. 10.1371/journal.pone.0355960.g011 Fig 11 Comparison of Obstacle Avoidance Performance of SMF-MPC, UKF-MPC, and R-MPC Under Uniform Noise. 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. 10.1371/journal.pone.0355960.g012 Fig 12 Comparison of Obstacle Avoidance Performance of SMF-MPC, UKF-MPC, and R-MPC Under Mixed Noise. 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 ). 10.1371/journal.pone.0355960.t002 Table 2 Comparison of Navigation Success Rates. Noisy Scenes SMF-MPC R-MPC UKF-MPC Gaussian noise 97.4% 93.5% 90.7% Uniform noise 90.8% 76.2% 64.4% mixed noise 83.3% 58.5% 36.7% 10.1371/journal.pone.0355960.g013 Fig 13 Noise Error: SMF-MPC vs. UKF-MPC vs. R-MPC. 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 ( ± 1.35 σ ) 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. 10.1371/journal.pone.0355960.g014 Fig 14 Control Inputs: SMF-MPC vs. UKF-MPC vs. R-MPC. 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 ). 10.1371/journal.pone.0355960.g015 Fig 15 Speed Comparison: SMF-MPC vs. UKF-MPC vs. R-MPC. 10.1371/journal.pone.0355960.g016 Fig 16 Process Noise PDF: SMF-MPC vs. UKF-MPC vs. RMPC. Fig 16 provides a theoretical explanation for the performance gap from an information-theoretic perspective: Gaussian noise ( K L = 0.000 ): the gap between the three algorithms is minimal, and the UKF-MPC operates under optimal conditions; uniform noise ( K L = 0.092 ): the tails of the flat-top distribution are heavier than those of the Gaussian distribution, making the UKF’s Gaussian assumption suboptimal; mixed noise ( K L = 0.387 ): 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.