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

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, ψ τ : τ + T = { ψ τ , ψ τ + 1 , … , ψ τ + T } , 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:

The NMPC problem represented in Eq. (3) forms an optimization problem that is solved to design the optimal control signals ψ τ : τ + T * 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.

2.2 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 ρ ( ξ k : k + H , ψ k : k + H ∣ ξ k : k + H r ) given the observations up to time k ≤ t ≤ k + H as follows:

Eq. (7) shows the relationship for the probability distribution of forward trajectories ρ ( ξ k : t , ψ k : t ∣ ξ k : t r ) from previous trajectories ρ ( ξ k : t − 1 , ψ k : t − 1 ∣ ξ k : t − 1 r ) . The state estimation is then used to decide the control action. The backward smoothing stage from ρ ( ξ k : t + 1 , ψ k : t + 1 ∣ ξ k : k + H r ) to ρ ( ξ k : t , ψ k : t ∣ ξ k : k + H r ) 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.

2.3 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].

Formally, let ξ k : t be the state variable, ξ k : t r be the observation variable, and ψ k : t be a set of parameters to be estimated. The target distribution is the joint posterior distribution (unknown distribution) of ξ k : t and ψ k : t given the observations up to time k ≤ t ≤ k + H : ρ ( ξ k : t , ψ k : t ∣ ξ k : t r ) . The importance distribution (known distribution) is the proposal distribution used to generate the particles: q ( ξ k : t , ψ k : t ∣ ξ k : t r ) . 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 ρ ( ξ k : t , ψ k : t ∣ ξ k : t r ) ≈ ∑ i = 1 N w k ( i ) δ ( ξ k : t , ψ k : t − ξ k : t ( i ) , ψ k : t ( i ) ) :

Eq. (10) shows that the importance weights w t ( i ) are updated recursively. Furthermore, consider q ( ξ t , ψ t ∣ ξ t − 1 , ψ t − 1 , ξ k : t r ) = ρ ( ξ t , ψ t ∣ ξ t − 1 , ψ t − 1 ) , and samples ( ξ t ( i ) , ψ t ( i ) ) ≈ ρ ( ξ t , ψ t ∣ ξ t − 1 ( i ) , ψ t − 1 ( i ) ) 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), ρ ( ξ t + 1 , ψ t + 1 ∣ ξ k : t r ) is replaced with ∫ ρ ( ξ t + 1 , ψ t + 1 ∣ ξ t , ψ t ) ρ ( ξ t , ψ t ∣ ξ k : t r ) d ( ξ t , ψ t ) , 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 ( ξ t , ψ t ) from ξ k : k + T r 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.

2.4 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 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–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 ρ ( ξ t r , v t ∣ ξ t , ψ t ) = ρ ( ξ t r ∣ ξ t , ψ t ) ρ ( v t ∣ ξ t , ψ t ) .

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.

3.1 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., ϕ b = 0 . 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 of 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:

Figure 1

Dynamical model of a robot vehicle.

3.2 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.

3.3 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 d = 3 cos ( 0.3 δ t ) + 3 sin ( 0.2 δ t ) , where δ t is the interval of 0.5 with the total length of 50 as follows: δ t = 0.5 , 1 , 1.5 , … , 50 . Furthermore, the upper and lower boundaries are given as d upper = d + 0.25 and d lower = d − 0.25 , respectively.

Figure 2

Reference track with upper and lower boundaries.

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 ξ = [ x r , y r , V r , μ r ] T with initial values ξ 0 = [ 0 , 1.8 , 3 , − π ∕ 4 ] T , 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 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 3

Comparison between improved-NMPC with barrier function to incorporate inequality constraints and 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.

Figure 4

Comparison of errors between improved-NMPC and normal-NMPC.

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.

Figure 5

Comparison of control signals for both normal-NMPC and improved-NMPC.