Stochastic Time-Varying Model Predictive Control for Trajectory Tracking of a Wheeled Mobile Robot

In this paper, a stochastic model predictive control (MPC) is proposed for the wheeled mobile robot to track a reference trajectory within a finite task horizon. The wheeled mobile robot is supposed to subject to additive stochastic disturbance with known probability distribution. It is also supposed that the mobile robot is subject to soft probability constraints on states and control inputs. The nonlinear mobile robot model is linearized and discretized into a discrete linear time-varying model, such that the linear time-varying MPC can be applied to forecast and control its future behavior. In the proposed stochastic MPC, the cost function is designed to penalize its tracking error and energy consumption. Based on quantile techniques, a learning-based approach is applied to transform the probability constraints to deterministic constraints, and to calculate the terminal constraint to guarantee recursive feasibility. It is proved that, with the proposed stochastic MPC, the tracking error of the closed-loop system is asymptotically average bounded. A simulation example is provided to support the theoretical result.


INTRODUCTION
Model predictive control (MPC) is a useful tool when dealing with stabilization or tracking problem with constraints. It solves a finite horizon optimal control problem in a receding horizon manner while only implementing one-step-ahead predictive control at next time step. please see the survey paper Mayne et al. (2000) for more detailed directions of MPC. Some recent results on MPC include (but do not limited to) adaptive MPC (Zhu et al., 2020), distributed MPC (Zhu et al., 2018;Wang et al., 2020), robust MPC (Yang et al., 2021), and stochastic MPC (Hewing and Zeilinger, 2018). Due to its advantages on improving system performance and handling constraints, MPC can be applied to motion control (Alcalá et al., 2020), process control (Wu et al., 2020), energy systems (Stadler et al., 2018;Rodas et al., 2021), etc. Several approaches of Robust MPC for wheeled mobile robots have been studied in Gonzalez et al. (2011) and Sun et al. (2018). However, conservative results will be obtained owing to disturbances (Mayne, 2016).
Stochastic MPC resolves the contradiction of safety and economy by taking advantage of uncertainties with known distributions. In addition, violations of constraints are permitted in a predetermined level and thus the soft constraints can balance system performance and limits on states in the meantime. Such characteristic renders stochastic MPC a promising method in a wide range of applications in energy or under actuated systems (Farina et al., 2016), such as energy scheduling (Rahmani-andebili and Shen, 2017), (Scarabaggio et al., 2021), (Jørgensen et al., 2016), energy management for vehicles (Cairano et al., 2014), temperature control in buildings (Hewing and Zeilinger, 2018), racing car Carrau et al. (2016), quadrotors (Yang et al., 2017), overhead cranes (Wu et al., 2015), automated driving vehicles (Suh et al., 2018) and mobile robots (Goncalves et al., 2018). The majority of approaches in Stochastic MPC community are developed for LTI systems (Mesbah, 2016). Several attempts in motion control have been made for LTV or nonlinear systems with ignoring requirement of recursive feasibility (Yang et al., 2017;Goncalves et al., 2018). This paper presents a stochastic MPC method for wheeled mobile robots based on framework of Hewing et al. (2020). The proposed method remains valid for other LTV systems or nonlinear systems after linearization. The main contribution is to develop a stochastic MPC method to forecast and control the wheeled mobile robot to track its reference trajectory against constraints and additive random disturbance with known distributions. In the proposed stochastic MPC, the cost function is designed to penalize both the tracking error and the energy consumption. The learning-based technique and probabilistic reachable set are applied, such that probability constraints can be transformed to deterministic constraints, and the terminal constraint can be calculated to ensure the recursive feasibility and asymptotic average boundedness. A simulation example with its simulated statistical data is provided to validate the proposed theoretical results.
The rest of the paper is arranged as follows. In Section 2, the LTV kinematic model for the mobile robot is introduced, and the control problem is formulated. In Section 3, the proposed stochastic MPC is designed in detail. In Section 4, recursive feasibility of optimization and asymptotic average boundedness are analysed in detail. In Section 5, the simulation result is provided to show the efficacy of the method.

PROBLEM STATEMENT
The wheeled mobile robot consists of two differential driving wheels and a guide wheel, as illustrated by Figure 1. Translational velocity and angular velocity of the robot can be expressed by rotation rates of driving wheels as v r(ω right +ω left ) 2 and ω r(ω right −ω left ) l , where r and l denote the radius and the distance of/between driving wheels, respectively; ω left and ω right are the rotational rates of the left and right wheels, respectively. Choosing generalized coordinate as state, velocity as control and taking additive disturbance into consideration, the kinematic model is described by where q [x,y,θ] T denotes the position and orientation of the mobile robot; u [v,ω] T denotes the translational velocity and angular velocity, which are considered as the inputs of the system; w [w 1 , w 2 , w 3 )] T is the disturbance with bounded or unbounded support and is subject to a known probability distribution under zero-mean assumption (Guassian distribution, e.g.).
The reference model to be tracked is given by where x r , y r , θ r are the reference position and orientation, respectively. Define the tracking error byq q − q r . The dynamics of the tracking error can be linearized by whereũ u − u r ; A c (t) and B c (t) are time-varying matrices calculated by If the sampling interval T is chosen small enough, it is appropriate to apply Euler discretization to obtain the timevarying linear discrete-time model: where T is the sampling period; k is the sampling time; and It is supposed that the mobile robot is subject to the probability constraints: where V q and V u are constant matrices with proper dimensions; p x and p u are constant vectors with proper dimensions. It can be Frontiers in Energy Research | www.frontiersin.org November 2021 | Volume 9 | Article 767597 seen from (6) and 7 that, if p x 1 or p u 1, then the constraints appear to be deterministic. The objective of this paper is that, design a model predictive controller, such that the mobile robot (subject to stochastic disturbance) is capable of tracking its reference trajectory within a finite task horizon N > 0, or equivalently, its tracking errorq is subject to zero mean and bounded covariance within a finite task horizon.

STOCHASTIC LINEAR TIME-VARYING MPC
In this section, the robust dual-mode MPC is applied as the fundamental structure. At time k, the predictive control are set by.
where the notation (·) (i|k) denotes the ith step prediction at time k; N is the predictive horizon. In this paper, the control horizon is set equal to the predictive horizon. The tracking errorq is decomposed into the nominal state s and the error e, namely.
where v is the nominal MPC for the nominal state s; K is obtained from solving the following Riccati equation at every time instant: where Q and R are the positive-definite weighting matrices in MPC design; S is to be solved based on the given Q and R. In (14) and 15, we have dropped k for brevity.

Cost Function
The cost function in MPC is designed by where Q and R are the positive-definite weighting matrices, and p(k) is solved from the following Lyapunov equation at every time step k − 1: In (16), the first term is to penalize the tracking error; the second term is to penalize the control power (which reflects the energy efficiency); and the third term is the terminal cost to ensure the overall feasibility and average performance.
The forecasting equation of the nominal part of the system dynamics can be rewritten in the compact form: where.
(21) Then the cost function can be written by The cost function can be further rewritten by where and It can be seen that d(k) is independent of v(k).

Constraints
In MPC design, the constraints (6) and (7) have to be handled. Let Here, we apply the technique of probabilitic reacheable set (Hewing and Zeilinger, 2018). The k-step probabilistic reachable sets can be calculated by. where for i 1, 2, . . . , N, and N ≫ N. Several methods for computing the quantiles c x (k) and c u (k) are summarized in Farina et al. (2016). One numerical approximation is to discretize the distributions of w, e.g. Kouvaritakis et al. (2010), while another adopts the framework of scenario MPC.
The distribution of e(k) can be computed with distributions of w(k) by the predictive error dynamics Constraints for nominal states s(k) can be calculated by or specifically where j 1, . . . , n V denotes the jth row of the matrix. The constraint (37) can be rewritten in compact form by The constraints for nominal inputs v(k) can be calculated in the similar process, namely, where or specifically For the recursive feasibility of optimization in MPC algorithm, terminal state constraints are imposed on the nominal states: where S T is a positive invariant set for the nominal system. The terminal state set S T can be defined based on the terminal control set: S T s|v K(k)s ∈ V inf , A(k)s + B(k)v ∈ S T , k 1, 2, . . . , N .
where v inf is the smallest v k in (40).
An iterative learning-based procedure (Gonzalez et al., 2011) can be applied to calculate the terminal constraint: where the sets are nested, i.e. Z(i) 4 Z (i − 1) 4/ 4 Z (0), and whereS inf ∩ k 1 N S k s|V q s ≤ s inf , and s inf is the smallest s k in (38).
The calculation of Z(1) continues with Z (i + 1) Z(1) ∩ Q(i), where Then the terminal set can be approximated by S T Z(1) when Z (i + 1) Z(i).

Optimization and Implementation
Based on the above cost function and constraints, the optimization in each time instant of MPC can be constructed by subject to.
The nominal MPC can be implemented by receding horizon scheme: and the stochastic MPC for the mobile robot can be implemented by

Feasibility of the Optimization
In this paper, the recursive feasibility is slightly different from the conventional feasibility of MPC. The aim is to prove that the optimization 48) is recursively feasible throughout the task horizon N. Specifically, it is to prove that, if the optimization 48) is feasible at k 0, then it is feasible at k 1, 2, . . . , N − N, where N ≫ N. To prove the recursive feasibility within the task horizon, the "tail" method can be applied here. Suppose that the optimization is feasible at time k. At time k + 1, a feasible solution can be constructed by using the "tail" of the optimal solution at time k, namely, which satisfies the input constraint (50). Moreover, in (54), K (k + N)s (N|k) ∈ V inf 4 V k+N , indicating that the terminal constraint (51) is satisfied. Satisfaction of state constraint (49) can also be checked in a similar way. Satisfaction of constraints (49)-(51) indicates that the original stochastic constraints (6) and (7) are satisfied. Consequently, at least one feasible solution exists at time k + 1, if the optimization is feasible at time k. The optimization in (48) is recursively feasible.

Average Asymptotic Boundedness of the Closed-Loop System
In this section, the conventional concept of asymptotic stability is inapplicable, since only performances within the task horizon are concerned. We are aiming to prove that the states and inputs satisfy all constraints, while the states are average asymptotically bounded.
Let J*(k) denote the optimal cost at time step k. Let J − (k) denote the feasible cost at time step k by using the "tail" information at time k − 1. Specifically, where p denotes the optimal control or state sequence. And It follows that It then follows from (17) that Summing both sides of the above inequality for k 0, 1, 2 . . . N − N − 1, and dividing the result by N − N yields that where J p (0) is finite, and J p (  N − N) is finite based on recursive feasibility. It then follows that where P 2 diag max k (P 11 (k + 1)), max k (P 22 (k + 1)), max k (P 33 (k + 1)) , The limitation 56) indicates that the tracking error of the closed-loop system is asymptotic average bounded.

SIMULATION
A simulation example is presented to validate the performance of the proposed method. In simulation, the sampling interval is T 0.1s; and the task horizon is given by 13s. Initial conditions of the mobile robot and the reference model are supposed to be q(0) [0.05, −0.02, 0.05)] T and q r (0) [0,0,0] T , respectively. It is suppose that there exists additive disturbances subject to a normal distribution: w ∼ N (0, Σ), where Σ diag (0.4 2 , 0.3 2 , 0.1 2 ). The reference trajectory for the mobile robot to track is a circular: x r (k) 12 π sin π 6 k , y r (k) 12 π cos π 6 k − π + 12 π .
The control inputs are subject to the following constraints: −1m/s ≤ṽ ≤ 1m/s, − π 2 rad/s ≤ω ≤ π 2 rad/s, whereṽbv − v r andωbω − ω r , and v r and ω r can be calculated from (57) and 2. Here, we allow constraints (58) be violated with very small probability, i.e., they are subject to probability constraints given by (7), where The probability constraints on states are supposed to be given by (6) In the stochastic MPC, the predictive horizon is chosen to be N 10; weighting matrices are selected by Q 30 0 0 0 30 0 0 0 1 The terminal constraint can be calculated by the learningbased algorithm (42)-(47), and can be illustrated by Figure 2.
Simulation results are displayed in Figures 3-5. In Figure 3, it can be seen that, with the proposed stochastic MPC, the wheeled mobile robot is capable of tracking its reference trajectory within the task horizon. Only limited tracking errors are witnessed in Figure 4. This simulation example has been run for 500 times with stochastic w, where p x in (33) can be calculated approximately by and p x in (6) which is in well accordance with the value given in (60). It can be seen from Figure 5 that the control inputs almost always satisfy    their constraints, where the constraint onũ v has been violated only once, indicating that the probability constraints given in (59) are satisfied. Variation of the running stage cost can be displayed to evaluate the energy efficiency of the closed-loop system. The running state cost is defined by where the first term implies the accuracy of tracking, and the second term indicates the energy consumption (or the consumed control power to eliminate the tracking error). It can be seen from Figure 6 that, in case of stochastic disturbances, the running stage cost can be maintained small. It implies that, the tracking error remains asymptotically average bounded, whereas the energy consumption is small.

CONCLUSION
A stochastic MPC is proposed for the wheeled mobile robot subject to probability constraints and stochastic disturbance to track its reference trajectory within the task horizon. The motion of the mobile robot is forecast and optimized by using its linearized discretized model. The cost function in the proposed MPC is designed to penalize its tracking error and energy consumption. By using the tightening probability reachable set technique, the probability constraints can be transformed into deterministic constraints, facilitating the MPC design. The terminal constraint is calculated with a learning-based algorithm. It is proved theoretically that, the optimization is recursively feasible, and the tracking error of the closed-loop system with the proposed stochastic MPC is asymptotically average bounded within the task horizon. The tracking error and statistical data in simulation imply that the performance of the proposed stochastic MPC is satisfactory.

DATA AVAILABILITY STATEMENT
The raw data supporting the conclusions of this article will be made available by the authors, without undue reservation.

AUTHOR CONTRIBUTIONS
WZ designed the main algorithm, implemented the simulation example, produced the main results, and wrote the main parts of the paper. BZ initiated the main idea, revised and polished the paper.

FUNDING
This work was supported by National Natural Science Foundation of China under grant no. 62073015.