Improved nonlinear model predictive control with inequality constraints using particle ﬁ ltering for nonlinear and highly coupled dynamical systems

: Motion planning and controller design are challenging tasks for highly coupled and nonlinear dynamical systems such as autonomous vehicles and robotic applications. Nonlinear model predictive control (NMPC) is an emerging technique in which sampling-based methods are used to synthesize the control and trajectories for complex systems. In this study, we have developed the sampling-based motion planning algorithm with NMPC through Bayesian estimation to solve the online nonlinear constrained optimization problem. In the literature, di ﬀ erent ﬁ ltration techniques have been applied to extract knowledge of states in the presence of noise. Due to the detrimental e ﬀ ects of linearization, the Kalman ﬁ lter with NMPC only achieves modest e ﬀ ectiveness. Moving horizon estimation (MHE), on the other hand, frequently relies on simplifying assumptions and lacks an e ﬀ ective recursive construction. Additionally, it adds another optimization challenge to the regulation problem that has to be solved online. To address this problem, particle ﬁ ltering is implemented for Bayesian ﬁ ltering in nonlinear and highly coupled dynamical systems. It is a sequential Monte Carlo method that involves representing the posterior distribution of the state of the system using a set of weighted particles that are propagated through time using a recursive algorithm. For nonlinear and strongly coupled dynamical systems, the novel sampling-based NMPC technique is e ﬀ ective and simple to use. The e ﬃ ciency of the suggested method has been assessed using simulated studies.


Introduction
Model predictive control (MPC) has evolved as an extremely effective method for dynamic control and optimization.MPC uses a dynamic model and online measurements to estimate future output behavior as a function of present and future controlled input actions [1,2].Based on the prediction, optimization is carried out to determine a set of input actions that minimize a selected measure of the deviation of the output from their respective reference values while meeting all specified constraints.This set of input actions minimizes the deviation between selected output items from their respective reference values.Since prediction accuracy can improve with additional samples, only the first calculated input sequence is used and the entire optimization is performed at the next sampling interval.This "receiving-horizon implementation" turns MPC into a feedback-control approach [3,4].A crucial aspect that contributes to MPC's effectiveness is the ability to include diverse process constraints directly into the online optimization performed at every time interval [5,6].
The optimization problem, which consists of input controller and state variables, is solved over a finite time horizon within the MPC approach.The state trajectories are predicted by online merging the local plans, and then, these state trajectories are used for control applications in motion planning [7].The point-to-point control technique is used to solve the control problems in linear MPC scenarios.Ghazaei Ardakani et al. [8] proposed a point-topoint linear MPC approach for trajectory tracking problems.In the study by Neunert et al. [9], a nonlinear control problem is solved using an iterative sequential linear quadratic technique and optimized control inputs for feedforward and feedback control are generated with the application of a hex copter vehicle.
MPC was improved to solve the problem of nonlinear control for industrial processes in the late 1970s, and the resulting modified controller was named nonlinear model predictive control (NMPC) [1,2,10].NMPC is a technology that has gained popularity in business and academics for constructing and monitoring collision-free trajectories in real time for autonomous vehicle systems and vehicle robotics [11][12][13].NMPC is a feedback control approach that is used to develop optimal control actions for highly nonlinear systems such as autonomous cars.It entails forecasting the future behavior of the system using a mathematical model while accounting for system dynamics as well as internal and external limitations [14].This enables the controller to provide a series of control inputs that will move the system to the target state while avoiding accidents and other risks.One of the benefits of NMPC is its ability to handle nonlinear systems with restrictions, which is vital for autonomous cars operating in dynamic and unpredictable situations.Another benefit of NMPC is that it is a predictive control approach, which means that it can anticipate changes in the environment and adapt the vehicle's trajectory in real time.This makes NMPC ideal for self-driving cars that must continually adjust to changing situations [11,[13][14][15][16].
NMPC is known as the moving horizon or predictor horizon using a system dynamics model.The moving horizon or predictor horizon refers to the fact that the controller updates its predictions at every time step as new data becomes available.The controller computes a new optimal input sequence at each time step, which takes into account the current state of the system as well as any new data that have become available since the previous time step [14,17,18].The use of a dynamical model in NMPC allows the controller to take into account the nonlinear dynamics of the system and any constraints on the inputs or outputs of the system.By generating a sequence of control inputs over a prediction horizon, the controller is able to anticipate changes in the system and adapt its control actions accordingly [13,14,19].Feedback control is optimized to solve the control problem for a nonlinear system in NMPC [20].In the NMPC method, the state and input restrictions can be changed while still respecting the system dynamics nonlinearities and coupling.
NMPC has many applications in control engineering, including setpoint stabilization and route following or trajectory tracking.Setpoint stability can be used to solve the fluid-level control problem or temperature control problem [21,22].Trajectory tracking is a complex task in which reference trajectory changes with time.Hovorka et al. [23] and Aguiar and Hespanha [24] provide a number of examples from the disciplines of autonomous drones and healthcare.Meanwhile, time-invariant reference trajectories are the focus of the route-following problem, with the objective of achieving optimal route tracking independent of time.In Matschek et al. [25] and Faulwasser et al. [26], NMPC is applied to solve the trajectory-tracking problem in which a robot manipulator is proposed to track the predetermined reference trajectory.Matschek et al. [27] gave an in-depth description of the problems associated with setpoint stabilization, trajectory tracking, and route following, as well as their related characteristics and obstacles.
This study introduces a significant contribution by seamlessly integrating a sampling-based motion planning algorithm with NMPC through Bayesian filtering.This novel approach enables the real time solution of nonlinearly constrained optimization problems, specifically tailored for the task of vehicle robot path tracking.Additionally, the study introduces particle filtering, a powerful algorithm designed to implement Bayesian filtering in nonlinear and non-Gaussian systems.This technique, operating as a sequential Monte Carlo (SMC) method, effectively represents the posterior distribution by using a set of weighted particles.These particles are iteratively propagated through time via a recursive algorithm, providing an accurate estimation of system states.Moreover, this study proposes a method for estimating optimal control actions using Bayesian filtration, drawing from reference signals with predefined upper and lower boundaries.This approach empowers precise control decisions, even in scenarios fraught with uncertainties and constraints.Furthermore, the research emphasizes the efficiency of the SMC sampling method, significantly enhancing control actions by conducting a thorough search in an expansive control space facilitated by a large number of particles.This is particularly advantageous in scenarios characterized by complex or high-dimensional solution spaces.The study concludes by demonstrating the effectiveness and simplicity of the proposed sampling-based NMPC technique, particularly in the context of nonlinear and strongly coupled dynamical systems.This approach represents an effective tool for practical applications demanding precise control and estimation.Overall, these contributions collectively advance the state of the art in the seamless integration of sampling-based motion planning, NMPC, and Bayesian filtering, particularly in scenarios involving nonlinear and non-Gaussian systems.This work not only furthers theoretical foundations but also offers practical solutions for real time applications in vehicle robotics and related fields.
Following is an expanded explanation of how our work contributes to the state of the art, particularly in the implementation of the particle filtering algorithm in Bayesian filtering for nonlinear and non-Gaussian systems: • Our study introduces a novel approach that integrates a sampling-based motion planning algorithm with NMPC through Bayesian filtering.This is a significant advancement as it enables real time, online solution of the nonlinearly constrained optimization problem.This integration is particularly tailored for the application of path tracking in vehicle robotics.• We emphasize the utilization of particle filtering, a powerful algorithm for implementing Bayesian filtering in systems characterized by nonlinearity and non-Gaussian distributions.This approach uses an SMC method to represent the posterior distribution.This is a crucial contribution, as it allows for accurate estimation in scenarios where traditional linear methods may fall short.• Our work highlights the core principle of particle filtering, which involves representing the posterior distribution using a set of weighted particles.These particles are recursively propagated through time using an algorithm.This technique ensures that the estimation process is continually refined, providing accurate and up-to-date information about the system's state.• One of the key innovations of our work lies in the estimation of optimal control actions.This is achieved through Bayesian filtration using reference signals with upper and lower boundaries.By leveraging this approach, we are able to make informed decisions regarding control actions, even in the presence of uncertainties and constraints.• We highlight the efficiency of the SMC sampling method.
This technique significantly enhances the control action by exploring a vast control space using a large number of particles.This is particularly valuable in scenarios where the solution space is complex or high-dimensional.• Our work demonstrates the effectiveness and simplicity of the proposed sampling-based NMPC technique, especially in the context of nonlinear and strongly coupled dynamical systems.This approach represents a powerful tool for real-world applications where accurate control and estimation are crucial.
The rest of this article is arranged as follows: Section 2 includes the proposed method of NMPC using particle filtering for nonlinear and highly coupled dynamical systems.In this section, first, the NMPC problem is formulated as an optimization problem for a nonlinear system with constraints.Second, the Bayesian estimation approach is established that consists of forward filtering and backward smoothing framework for NMPC.Third, the particle filtering approach is proposed to estimate the posterior probability distribution for the closed-loop nonlinear system.Fourth, the penalty function is introduced in the particle filtering approach to enhance the NMPC approach so that it can tackle the inequality constraints.Section 3 illustrates the simulation results that construct that dynamical model of a discrete-time nonlinear model for a robot vehicle and the tracking problem is simulated using the proposed NMPC approach.The simulation results show that the proposed NMPC approach using particle filtering incorporating barrier function solves the tracking problem efficiently.

Proposed method 2.1 NMPC problem formation
In order to develop the NMPC problem, first we introduce the discrete-time nonlinear system and then construct the cost function using the error signal.The discrete-time nonlinear model at time τ subjected to the n number of inequality constraints can be written as follows [19,20]: where ξ τ and ψ τ are the state of the system and the control signal at time τ, and ( ) f .and ( ) g .
i are the nonlinear mapping functions, and n is the total number of constraints for the nonlinear discrete-time system.
The object of the problem is to design a control signal such that the error signal is minimized, and hence, the state of the nonlinear discrete-time system tracks the ideal reference signal.If the reference signal is represented by ξ τ r , then the error signal can be written as = − e ξ ξ τ τ τ r .The cost function is given as follows: , where T is the length of the horizon upcoming, , and W 1 and W 2 are weighting matrices.Furthermore, the cost function is the weighted quadratic sum of the tracking error and the control cost.The cost function given in Eq. ( 2) is subjected to the nonlinear discrete-time model of Eq. ( 1) and forms the NMPC problem as follows: Improved nonlinear model predictive control with inequality constraints  3 The NMPC problem represented in Eq. ( 3) forms an optimization problem that is solved to design the optimal control signals and optimize the cost function for the control signals over the T horizon.When the optimization problem is solved and the optimized control signals are evaluated, the first element of the optimized control signals ψ* τ is applied and all other control elements are discarded.The process is repeated for all steps in T and forms a model-based predictive optimization problem [17,19].
Alternatively, the NMPC problem can also be represented as an optimization problem in which states of the nonlinear discrete-time model and its control signals are estimated using the desired reference states.The overall model can be represented as follows: where ( ) ω t 1 and ( ) ω t 2 are the additive disturbances that form the cost function as follows: The state and control signal for the NMPC problem represented in Eq. ( 4) can be estimated using the following optimization problem: The optimization problem described in Eq. ( 6) is similar to the optimization problem formed in Eq. ( 3), which established a new way to deal with the NMPC problem.

Bayesian estimation approach for NMPC
Bayesian estimation is a statistical method for estimating the parameters of the model based on previous information and observed data.It is commonly used in control systems to increase the accuracy and resilience of the control algorithm, particularly in nonlinear systems [28,29].A typical way for applying Bayesian estimation is the forward filtering and backward smoothing approach.A Bayesian forward filtering and backward smoothing framework for NMPC involves estimating the posterior probability distribution over the state space at each time step based on observed data and prior knowledge and using this estimate to predict the future behavior of the system and optimize the control actions.
In the NMPC problem, the future trajectories of the states with optimized control inputs ( ) ξ ψ , k t k t : : are estimated from the reference trajectory ξ k t r : .The forward filtering stage entails predicting the system's state based on observable data up to the present moment.This is accomplished by estimating the posterior probability distribution of the state based on observations and previous knowledge of system dynamics ( | ) : : given the observations up to time ≤ ≤ + k t k H as follows: Eq. ( 7) shows the relationship for the probability distribution of forward trajectories ( | ) The state estimation is then used to decide the control action.The backward smoothing stage from ( | ) : : is then used to refine the state estimate by adding all relevant data, both past and future.This is performed by calculating the posterior probability distribution of the state given all of the observations and prior knowledge of the system dynamics: Bayesian estimate necessitates computing the posterior probability distribution across the state space, which is typically not available in closed-up nonlinear systems.One way to overcome this difficulty is to use nonlinear versions of the Kalman filter to estimate the posterior distribution of the state given the observations and the model [30,31].Their accuracy, however, is dependent on the model's quality and the system's complexity.Another way is to use particle filters, which are a type of Monte Carlo method that uses a collection of weighted samples to approximate the posterior distribution.Particle filters can handle nonlinear models and non-Gaussian distributions.

Particle filtering approach for NMPC
The idea of importance weights is fundamental to SMC sampling, commonly known as particle forward filtering and backward smoothing [28,32].SMC approaches are used to iteratively estimate the posterior distribution of a state space model based on a series of observations.The importance weights are used to scale particles from the importance distribution in the particle forward filtering and backward smoothing.The importance distribution is chosen to be the empirical distribution of the particles created in the previous time step.Importance weights are used to weigh the particles, and they are calculated as the ratio of the target distribution (the posterior distribution) to the importance distribution assessed at the particle values [28,32] : .The importance weights drawn from q, are given by: The weights are then normalized so that they sum to one, and the particles are resampled based on their weights to generate a new set of particles for the next time step.Eq. ( 9) can be written as follows using the approximation Eq. (10) shows that the importance weights are taken at time t.Then, Eq. ( 10) is updated as follows: The derived equation is called particle forward filtering.Particle degeneracy is a typical issue in particle forward filtering in which the weights of the particles are either zero in the beginning or become zero after some epochs, resulting in poor estimate efficiency.Resampling is an effective strategy for dealing with this issue.Resampling creates a new collection of particles from an existing set of particles depending on their weights [33,34].Particles with greater weights are more likely to be chosen, whereas those with lower weights are less likely.The number of particles with non-zero weights is raised by resampling, and the overall quality of the particle collection is enhanced.
Backward smoothing gives an improved estimation of samples.In Eq. ( 8), ( | ) is replaced with , and the resultant expression is written as follows: From Eq. ( 12), it can be seen that all the posterior distributions can be approximated with importance distribution, and consequently, the samples for backward smoothing can be given as follows: .
The posterior distribution in Eq. ( 12) is approximated as follows: which leads to the best estimation of ( ) ξ ψ , and optimal control inputs are given at k time step as follows: In the aforementioned method for the NMPC algorithm, the weights are improved to enhance the backward smoothing.However, it is important to note that this method does not take into account inequality constraints.As a result, it is classified as the normal-NMPC approach.Next, we will consider the constraints and develop an improvement in the proposed method for the NMPC algorithm.

Particle filtering approach for NMPC with inequality constraints
Incorporating inequality constraints into particle forward filtering and backward smoothing can be challenging, as traditional particle forward filtering and backward smoothing may not be able to handle constraints directly.A barrier function is a mathematical function that is used in optimization problems to enforce constraints on the variables being optimized [35].In NMPC, barrier functions are used to ensure that the system being controlled satisfies constraints on its Improved nonlinear model predictive control with inequality constraints  5 state and inputs over a finite time horizon.The basic idea behind using a barrier function in NMPC is to penalize the likelihood of particles that violate the constraints, while still allowing the particles to explore the entire state space [36][37][38].This is achieved by introducing a penalty term that grows infinitely large as the particles approach the constraint boundary, effectively creating a "barrier" that prevents the particles from crossing the boundary.To implement this approach, we would first define a virtual function as follows: where v t is the state of the virtual function, f b is a barrier function, ( ) g ξ ψ , t t are the inequality constraints defined in Eq. ( 6), and n t is the random noise with a zero mean.
The barrier function should return a value that increases as the constraints are violated and approaches infinity as the constraints are approached.We consider the following barrier function [37,38]: where k 1 and k 2 are constants and s is a variable on which barrier function depends.Finally, we would incorporate these virtual measurements into the particle forward filtering and backward smoothing algorithm.This would allow the algorithm to take into account the constraints in the estimation process, while still allowing the particles to explore the entire state space.Eq. ( 8) can be written as follows: where It is important to note that this approach may require tuning the barrier function parameters to achieve good estimation performance.However, with proper implementation, it is an effective method for handling inequality constraints in particle forward filtering and backward smoothing.The particle filter designed in Eq. ( 11) can be written as follows: Eq. ( 13) will remain the same for the backward smoothing because the virtual function does not influence this.The particle filtering approach for NMPC, augmented with a barrier function given in Eq. ( 17), integrates the treatment of inequality constraints.Consequently, the resulting NMPC framework is referred to as the improved NMPC.

Simulation and results
A nonlinear and highly coupled dynamical model of a robot vehicle is presented in this section.The dynamical model is then converted to a discrete-time model, and the proposed NMPC approach using particle filtering is applied.The results of NMPC with inequality constraints are compared with those of NMPC with equality constraints.

Dynamical model of a robot vehicle
The dynamical representation of the velocity vectors in the x-axis and y-axis are given as follows: where V is the velocity of the robot vehicle, which makes an angle ϕ called the slip angle with the robot vehicle as shown in Figure 1, and μ is the direction angle of the robot vehicle.
The angular velocity of the robot vehicle is given by: where R is the radius of the robot path and is given by the length OC, which is perpendicular to the velocity vector V , as shown in Figure 1.To draw the expression for the radius R, we apply the sine rule to the OCA and OCB triangles, respectively, as follows: Note that the net velocity vectors of the front and back wheels at point A and point B are in the direction of ϕ f and ϕ b , respectively, and the instantaneous rolling center O is the intersection of the lines OA and BO.Furthermore, r f and r b are the front and back distances from points A to C and points B to C, respectively.Eq. ( 22) can be rewritten as follows: In our case, the net velocity vector of the back wheel is zero, i.e., = ϕ 0 b .Furthermore, we can rewrite the following expression: Adding Eq. ( 24) and rearranging for R: The angular velocity of the robot vehicle is given by: Next, we will drive the expression for the angle slip ϕ; for that, we will consider Eq. ( 24) and divide them as follows: ( ) Rearranging the aforementioned equation results in the following expression: Overall, the dynamic model for the robot vehicle shown in Figure 1 is written as follows:

Discrete-time model
The dynamical model given in Eq. ( 29) is the continuous time that needs to be converted to a discrete-time model in order to apply the proposed sampling-based motion planning algorithm.Samples are taken at each time step τ with the time interval δτ.Eq. ( 29) can be written in the discrete- time domain as follows: where α τ is the discrete-time acceleration at time step τ.
The discrete-time control signals consist of discrete-time acceleration and front-wheel steering angle, respectively, and are given as follows: The purpose of the proposed algorithm is to optimize control actions u k so that the desired track is followed by the robot vehicle.

Simulation results
The MATLAB environment was used for simulation.Figure 2 shows the desired reference track with upper and lower boundaries.The desired reference track is given by ( , where δ t is the interval of 0.5 with the total length of 50 as follows: = δ 0. , respectively.The desired reference track was followed by the robot vehicle that has a discrete-time dynamical model given in Eq. ( 30) and the control signals in Eq. (31).The state vector is given by 4 T 0 , respectively.There are four prediction horizons, one for each state given in state vector ξ , and the number of particles was taken 100 for simulation.The optimized states and control signals were derived using Eq. ( 15), whereas the importance weights ( ) w k i were computed using Eq. ( 11) of particle forward filtering approach for NMPC and Eq. ( 19) of particle forward filtering approach for NMPC with inequality constraints.In both cases, particle backward smoothing was applied using the same Improved nonlinear model predictive control with inequality constraints  7   Eq.( 13). Figure 3 shows the optimized path for both cases, and results are computed.The blue line shows the track generated by the particle filtering approach for NMPC with a barrier function to incorporate the inequality constraints named improved-NMPC, whereas the green line shows the track generated by the particle filtering approach for NMPC without a barrier function named normal-NMPC.Figure 4 displays the error signals comparing improved NMPC and normal NMPC.In order to evaluate the error signal for improved NMPC, two trajectories were considered: the reference trajectory and the trajectory designed by the improved NMPC.The error signal was obtained by computing the difference between these two trajectories.Similarly, for the normal NMPC, the error signal was evaluated by comparing the reference trajectory and the trajectory designed by the normal NMPC.Again, the error signal was computed as the discrepancy between these two trajectories.Figure 4 provides a visual representation of the error signals for both improved NMPC and normal NMPC, highlighting the differences between the reference trajectories and the trajectories generated by the respective control methods.
Furthermore, Figure 5 illustrates the control signals given in Eq. (31).It represents the acceleration and steering angle of the robot vehicle.The green signals show the control effort in normal NMPC, whereas blue signals show the control effort for improved NMPC with barrier function.It can be concluded that the control effort for improved NMPC is less than that of the normal NMPC, which shows the superiority of the proposed controller.Moreover, it can also be seen from Figure 3 that the improved NMPC performs efficiently to track the desired trajectory, whereas the normal NMPC is inefficient to track the trajectory.

Conclusion
This study presents an NMPC approach using particle filtering to solve a discrete-time dynamical model of a robot vehicle.Particle filtering is an algorithm that is used to implement Bayesian filtering for nonlinear and non-Gaussian systems.It is an SMC method that involves representing the posterior distribution using a set of weighted particles that are propagated through time using a recursive algorithm.The optimal control actions are estimated using this estimation from the reference signal with upper and lower boundaries.Furthermore, the particle forward filtering and backward smoothing approach is used to iteratively estimate the posterior probability distribution over the state space at each time step based on observed data and prior knowledge and use this estimate to predict future behavior and optimize the control actions.Moreover, a barrier function is introduced to penalize the likelihood of particles that violate the constraints in NMPC.Finally, the simulations are performed to verify the performance of the proposed NMPC with constraints, and it is concluded that NMPC with penalized function can efficiently track the desired reference trajectory.

Figure 1 :
Figure 1: Dynamical model of a robot vehicle.

Figure 2 :
Figure 2: Reference track with upper and lower boundaries.

Figure 3 :
Figure 3: Comparison between improved-NMPC with barrier function to incorporate inequality constraints and normal-NMPC.

Funding information :
This research received no external funding.