Model predictive control for trajectory-tracking and formation of wheeled mobile robots

Abstract

Model predictive control (MPC) naturally guarantees optimal transient process and constraints satisfaction. Most mature MPC theories concern with linear time-invariant systems, it is not trivial to develop MPC for time-varying or nonlinear systems. A linear time-varying model predictive control strategy is proposed in this paper for single-wheeled mobile robot trajectory tracking subject to the non-holonomic constraint and control constraints. The kinematic equation of the robot is converted into a linear time-varying form after linearizing and discretizing, the time-varying MPC is capable to be applied in consequence. The proposed trajectory tracking task is extended to the formation control among multiple robots solved by means of nonlinear model predictive control directly. The extended formation MPC is in the leader-following framework. Recursive feasibility and stability are proved for the closed-loop system.

Appendix

1.1 Proof of Proposition 1

From Proposition 1, optimal problem has feasible solution (24) at sampling time interval k:

$$\begin{aligned} \overline{{\mathbf {U}}}(k)^{*}=\left\{ \tilde{{\mathbf {u}}}^{*}(k\mid k),\tilde{{\mathbf {u}}}^{*}(k+1\mid k),\ldots ,\tilde{{\mathbf {u}}}^{*}(k+N-1\mid k)\right\} . \end{aligned}$$

Corresponding predictive states are

$$\begin{aligned} \overline{{\mathbf {X}}}(k+1)^{*}=\left\{ \tilde{{\mathbf {x}}}^{*}(k+1 \mid k), \tilde{{\mathbf {x}}}^{*}(k+2 \mid k),\ldots ,\tilde{{\mathbf {x}}}^{*}(k+N-1 \mid k), 0\right\} \end{aligned}$$

The terminal constraint (22) results in the last element of predictive state series identically equal to zero.

Considering the MPC is carried on by (25). Then, it is noted that

$$\begin{aligned} \tilde{{\mathbf {x}}}(k+1)={\mathbf {F}}(k) \tilde{{\mathbf {x}}}(k)+{\mathbf {G}}(k) \tilde{{\mathbf {u}}}^{*}(k \mid k)=\tilde{{\mathbf {x}}}^*(k+1|k) \end{aligned}$$

When $k+1$ comes, the optimization (24) has a feasible (not necessarily optimal) solution allocated by transferring the control sequence at time k:

$$\begin{aligned} \overline{{\mathbf {U}}}(k+1)= & {} \left[ \begin{array}{c}\tilde{{\mathbf {u}}}(k+1\mid k+1)\\ \tilde{{\mathbf {u}}}(k+2\mid k+1)\\ \vdots \\ \tilde{{\mathbf {u}}}(k+N-1\mid k+1)\\ \tilde{{\mathbf {u}}}(k+N\mid k+1)\end{array}\right] \\= & {} \left[ \begin{array}{c}\tilde{{\mathbf {u}}}^{*}(k+1\mid k)\\ \tilde{{\mathbf {u}}}^{*}(k+2\mid k)\\ \vdots \\ \tilde{{\mathbf {u}}}^{*}(k+N-1\mid k)\\ 0\end{array}\right] \end{aligned}$$

(60)

and the relevant predictive states are

$$\begin{aligned} \overline{{\mathbf {X}}}(k+2)= & {} \left[ \begin{array}{c}\tilde{{\mathbf {x}}}(k+2\mid k+1)\\ \tilde{{\mathbf {x}}}(k+3\mid k+1)\\ \vdots \\ \tilde{{\mathbf {x}}}(k+N\mid k+1)\\ \tilde{{\mathbf {x}}}(k+N+1\mid k+1)\end{array}\right] \\= & {} \left[ \begin{array}{c}\tilde{{\mathbf {x}}}^{*}(k+2\mid k)\\ \tilde{{\mathbf {x}}}^{*}(k+3\mid k)\\ \vdots \\ \tilde{{\mathbf {x}}}^{*}(k+N-1\mid k)\\ 0\\ 0\end{array}\right] \end{aligned}$$

(61)

mentioning that there exists at least one solution to (24) satisfied with control constraints (21) and the terminal constraints (22).It implies that, if there is an optimization (24) viable at initial time, then it will be viable at any time in the future, and this achieves the recursive feasibility of the system.

1.2 Proof of Proposition 2

Select the optimal objective function $J^{*}(k)$ to be the Lyapunov candidate (time-dependent) V(k) under time k condition:

$$\begin{aligned} V(k)= & {} J^{*}(k) \\= & {} \sum _{i=0}^{N-1}\left( \left\| \tilde{{\mathbf {x}}}^{*}(k+i \mid k)\right\| _{Q}^{2}+\left\| \tilde{{\mathbf {u}}}^{*}(k+i \mid k)\right\| _{R}^{2}\right) . \end{aligned}$$

(62)

It is obvious to see $V(0)=0$, $V(k)>0$ as to $\tilde{{\mathbf {x}}}(k)\ne 0$ and $\tilde{{\mathbf {u}}}(k)\ne 0$. Moreover, V(k) is quadratic positive definite holding inequality:

$$\begin{aligned} V(k)\ge \Vert \tilde{{\mathbf {x}}}(k)\Vert _{Q}^2. \end{aligned}$$

(63)

Owing to the uniform boundness of coefficient matrix ${\mathbf {F}}(k)$, ${\mathbf {G}}(k)$ and the inputs $\tilde{{\mathbf {u}}}$, it is easy to see from (19) that there exist a constant $\alpha >0$ satisfying the inequality condition:

$$\begin{aligned} V(k)\le \alpha \Vert \tilde{{\mathbf {x}}}(k)\Vert _{Q}^2 \end{aligned}$$

(64)

indicating V(k) is decrescent.

From (60) and (61), $J(k+1)$ (not necessarily optimal) at $k+1$ will be acquired by

$$\begin{aligned} J(k+1) =J^{*}(k)-\left\| \tilde{{\mathbf {x}}}^{*}(k \mid k)\right\| _{Q}^{2}-\left\| \tilde{{\mathbf {u}}}^{*}(k \mid k)\right\| _{R}^{2} \end{aligned}$$

and

$$\begin{aligned} {V}(k+1)-V(k)&=J^{*}(k+1)-J^{*}(k) \le J(k+1)-J^{*}(k)\nonumber \\ &\le -\left\| \tilde{{\mathbf {x}}}^{*}(k \mid k)\right\| _{Q}^{2}-\left\| \tilde{{\mathbf {u}}}^{*}(k \mid k)\right\| _{R}^{2}\nonumber \\ &\le -\left\| \tilde{{\mathbf {x}}}^{*}(k \mid k)\right\| _{Q}^{2} \end{aligned}$$

(65)

implying the change of V(k) is negative.

It is concluded from (63), (64) and (65) that the linearized closed-loop system meets the condition of uniform asymptotic stability.

The nonlinear model of WMR can be deemed as a vanishing disturbance (vanishes at $\tilde{{\mathbf {x}}}=0$) brought to the linearized model, and it straightforwardly has the conclusion that around the reference trajectory, nonlinear tracking error closed-loop system has the locally uniform asymptotically stability.

1.3 Proof of Proposition 3

The optimal control and state feasible sequences (46) from Proposition 3 at time k are chosen by

$$\begin{aligned} \overline{{\mathbf {U}}}_{{Le}}^{*}(k)= & {} \left\{ {\mathbf {u}}_{{Le}}^{*}(k \mid k), {\mathbf {u}}_{{Le}}^{*}(k+1 \mid k), \ldots , {\mathbf {u}}_{{Le}}^{*}(k+N-1 \mid k)\right\} \nonumber \\ \overline{{\mathbf {U}}}_{f e}^{*}(k)= & {} \left\{ {\mathbf {u}}_{f e}^{*}(k \mid k), {\mathbf {u}}_{f e}^{*}(k+1 \mid k), \ldots , {\mathbf {u}}_{f e}^{*}(k+N-1 \mid k)\right\} \nonumber \\ \overline{{\mathbf {X}}}_{{Le}}^{*}(k)= & {} \left\{ {\mathbf {x}}_{{Le}}^{*}(k \mid k), {\mathbf {x}}_{{Le}}^{*}(k+1 \mid k), \ldots , {\mathbf {x}}_{{Le}}^{*}(k+N-1 \mid k), 0\right\} \nonumber \\ \overline{{\mathbf {X}}}_{f e}^{*}(k)= & {} \left\{ {\mathbf {x}}_{f e}^{*}(k \mid k), {\mathbf {x}}_{f e}^{*}(k+1 \mid k), \ldots , {\mathbf {x}}_{f e}^{*}(k+N-1 \mid k), 0\right\} . \end{aligned}$$

The predictive errors in next sample moment are calculated with (52), (53) by

$$\begin{aligned} {\mathbf {x}}_{{Le}}(k+1)= & {} {\mathbf {x}}_{L e}(k)+T{\mathbf {f}}\left( {\mathbf {x}}_{{Le}}(k), {\mathbf {u}}_{{Le}}^{*}(k)\right) \nonumber \\= & {} {\mathbf {x}}_{{Le}}^{*}(k+1 \mid k), \end{aligned}$$

(66)

$$\begin{aligned} {\mathbf {x}}_{f e}(k+1)= & {} {\mathbf {x}}_{f e}(k)+T{\mathbf {h}}\left( {\mathbf {x}}_{f e}(k), {\mathbf {u}}_{f e}^{*}(k)\right) \nonumber \\= & {} {\mathbf {x}}_{f e}^{*}(k+1 \mid k). \end{aligned}$$

(67)

The corresponding state and control sequences are

$$\begin{aligned} \overline{{\mathbf {U}}}_{{Le}}(k+1)= & {} \left[ \begin{array}{c} {\mathbf {u}}_{{Le}}(k+1 \mid k+1) \\ {\mathbf {u}}_{{Le}}(k+2 \mid k+1) \\ \vdots \\ {\mathbf {u}}_{{Le}}(k+N-1 \mid k+1) \\ {\mathbf {u}}_{{Le}}(k+N \mid k+1) \end{array}\right] \nonumber \\= & {} \left[ \begin{array}{c} {\mathbf {u}}_{{Le}}^{*}(k+1 \mid k) \\ {\mathbf {u}}_{{Le}}^{*}(k+2 \mid k) \\ \vdots \\ {\mathbf {u}}_{{Le}}^{*}(k+N-1 \mid k) \\ 0 \end{array}\right] \end{aligned}$$

(68)

$$\begin{aligned} \overline{{\mathbf {U}}}_{f e}(k+1)= & {} \left[ \begin{array}{c} {\mathbf {u}}_{f e}(k+1 \mid k+1) \\ {\mathbf {u}}_{f e}(k+2 \mid k+1) \\ \vdots \\ {\mathbf {u}}_{f e}(k+N-1 \mid k+1) \\ {\mathbf {u}}_{f e}(k+N \mid k+1) \end{array}\right] \nonumber \\= & {} \left[ \begin{array}{c} {\mathbf {u}}_{f e}^{*}(k+1 \mid k) \\ {\mathbf {u}}_{f e}^{*}(k+2 \mid k) \\ \vdots \\ {\mathbf {u}}_{f e}^{*}(k+N-1 \mid k) \\ 0 \end{array}\right] . \end{aligned}$$

(69)

The first $N-1$ individuals of the feasible given inputs sequences are generated by transforming the optimization solution of time k. The reason of selecting zero as the last element is that predict states ${\mathbf {x}}_{{Le}}(k+N+1|k)$, ${\mathbf {x}}_{f e}(k+N+1|k)$ will be equal to zero under the influence of ${\mathbf {u}}_{{Le}}(k+N|k)=0$, ${\mathbf {u}}_{f e}(k+N|k)=0$.

The corresponding $k+1$ time interval prediction state sequences are:

$$\begin{aligned} \overline{{\mathbf {X}}}_{{Le}}(k+1)= & {} \left[ \begin{array}{c} {\mathbf {x}}_{{Le}}(k+1 \mid k+1) \\ {\mathbf {x}}_{{Le}}(k+2 \mid k+1) \\ {\mathbf {x}}_{{Le}}(k+3 \mid k+1) \\ \vdots \\ {\mathbf {x}}_{{Le}}(k+N \mid k+1) \\ {\mathbf {x}}_{{Le}}(k+N+1 \mid k+1) \end{array}\right] \nonumber \\= & {} \left[ \begin{array}{c} {\mathbf {x}}_{{Le}}^{*}(k+1 \mid k) \\ {\mathbf {x}}_{{Le}}^{*}(k+2 \mid k) \\ {\mathbf {x}}_{{Le}}^{*}(k+3 \mid k) \\ \vdots \\ {\mathbf {x}}_{{Le}}^{*}(k+N-1 \mid k) \\ 0 \\ 0 \end{array}\right] \end{aligned}$$

(70)

$$\begin{aligned} \overline{{\mathbf {X}}}_{f e}(k+1)= & {} \left[ \begin{array}{c} {\mathbf {x}}_{f e}(k+1 \mid k+1) \\ {\mathbf {x}}_{f e}(k+2 \mid k+1) \\ {\mathbf {x}}_{f e}(k+3 \mid k+1) \\ \vdots \\ {\mathbf {x}}_{f e}(k+N \mid k+1) \\ {\mathbf {x}}_{f e}(k+N+1 \mid k+1) \end{array}\right] \nonumber \\= & {} \left[ \begin{array}{c} {\mathbf {x}}_{f e}^{*}(k+1 \mid k) \\ {\mathbf {x}}_{f e}^{*}(k+2 \mid k) \\ {\mathbf {x}}_{f e}^{*}(k+3 \mid k) \\ \vdots \\ {\mathbf {x}}_{f e}^{*}(k+N-1 \mid k) \\ 0 \\ 0 \end{array}\right] . \end{aligned}$$

(71)

The above derivations indicate that there exists at least one set of solutions satisfying all constraints, and the optimization (46) is feasible at all predictive steps.

1.4 Proof of Proposition 4

Due to the terminal equality constraint,

$$\begin{aligned}&{\mathbf {x}}_{{Le}}(k+N \mid k)=0 \\ &{\mathbf {x}}_{f e}(k+N \mid k)=0, \quad f=1,2, \ldots , m-1, \end{aligned}$$

the cost function J(k) becomes

$$\begin{aligned} J(k)= & {} \sum _{j=0}^{N-1}\left( \left\| {\mathbf {x}}_{{Le}}(k+i \mid k) \right\| _{Q_{L}}^{2}+\left\| {\mathbf {u}}_{{Le}}(k+i \mid k)\right\| _{R_{L}}^{2}\right) \nonumber \\ &+\sum _{f=1}^{m-1}\left[ \sum _{j=0}^{N-1}\left( \left\| {\mathbf {x} }_{f e}(k+i \mid k)\right\| _{Q_{f}}^{2}+\left\| {\mathbf {u}}_{f e}(k+i \mid k)\right\| _{R_{f}}^{2}\right) \right] . \end{aligned}$$

(72)

Select the candidate for Lyapunov function V(k) providing by optimal function $J^{*}(k)$ at time k:

$$\begin{aligned} J^{*}(k)= & {} \sum _{j=0}^{N-1}\left( \left\| {\mathbf {x}}_{{Le}}^{*}(k+i \mid k)\right\| _{Q_{L}}^{2}+\left\| {\mathbf {u}}_{{Le}}^{*}(k+i \mid k) \right\| _{R_{L}}^{2}\right) \nonumber \\ &+\sum _{f=1}^{m-1}\left[ \sum _{j=0}^{N-1}\left( \left\| {\mathbf {x}}_{f e}^{*} (k+i \mid k)\right\| _{Q_{f}}^{2}+\left\| {\mathbf {u}}_{f e}^{*}(k+i \mid k) \right\| _{R_{f}}^{2}\right) \right] . \end{aligned}$$

(73)

The selected Lyapunov function V(k) is positive definite and decrescent. Substituting the predictive state and control sequences (68), (69), (70), and (71) into the objective function $J(k+1)$ at time interval $k+1$ yields

$$\begin{aligned} J(k+1)= & {} \sum _{i=0}^{N-1}\left( \left\| {\mathbf {x}}_{{Le}}(k+1+i \mid k+1)\right\| _{Q_{L}}^{2}\right. \\ &\left. +\left\| {\mathbf {u}}_{{Le}}(k+1+i \mid k+1)\right\| _{R_{L}}^{2}\right) \\ &+\sum _{f=1}^{m-1}\left[ \sum _{j=0}^{N-1}\left( \left\| {\mathbf {x}}_{f e}(k+1+i \mid k)\right\| _{{\mathcal {Q}}_{f}}^{2}\right. \right. \\ &\left. \left. +\left\| {\mathbf {u}}_{f e}(k+1+i \mid k)\right\| _{R_{f}}^{2}\right) \right] \\ =\, & {} J^{*}(k)-\left\| {\mathbf {x}}_{{Le}}^{*}(k \mid k)\right\| _{Q_{L}}^{2}-\left\| {\mathbf {u}}_{{Le}}^{*}(k \mid k)\right\| _{R_{L}}^{2} \\ &-\sum _{f=1}^{m-1}\left( \left\| {\mathbf {x}}_{f e}^{*}(k \mid k)\right\| _{Q_{f}}^{2}-\left\| {\mathbf {u}}_{f e}^{*}(k \mid k)\right\| _{R_{f}}^{2}\right) , \end{aligned}$$

and

$$\begin{aligned}V(k+1)-V(k)&=J^{*}(k+1)-J^{*}(k) \le J(k+1)-J^{*}(k) \nonumber \\ &\le -\left\| {\mathbf {x}}_{{Le}}^{*}(k \mid k)\right\| _{Q_{L}}^{2}-\left\| {\mathbf {u}}_{{Le}}^{*}(k \mid k)\right\| _{R_{L}}^{2}\nonumber \\ &\qquad -\sum _{f=1}^{m-1}\left( \left\| {\mathbf {x}}_{f e}^{*}(k \mid k)\right\| _{Q_{f}}^{2}-\left\| {\mathbf {u}}_{f e}^{*}(k \mid k)\right\| _{R_{f}}^{2}\right) , \end{aligned}$$

(74)

indicating that the Lyapunov function is decreasing, and uniform asymptotical stability is confirmed.