Change MPC Formulation to Exploit Sparsity

[AhmadAmine998]

Hello,

The current Model Predctive Control algorithm is formulated as a QP with substitution. Given that the system is described by the following dynamics

$$ \begin{gather} x_{k+1}=Ax_{k}+Bu_{k}+w_{k}, y_{k}=Cx_{k} \tag{1} \\ x_{k}\in R^{n},u_{k}\in R^{m},w_{k}\in R^{n}, y_{k}\in R^{l}, A\in R^{n\times n}, B\in R^{n\times m}, C\in R^{l \times n} \end{gather} $$

the current implementation uses equation (1) to substitute the initial state iteratively to get each of the states along the trajectory as a function of the control inputs up to time $k$ and the initial state.

when $k=2$, using also equation (2), we get

$$ \begin{align} x_{2} & = Ax_{1} + Bu_{1} + w_{1} \\ & = A(Ax_{0} + Bu_{0} + w_{0}) + Bu_{1} + w_{1} \\ & = A^{2}x_{0} + ABu_{0} + Aw_{0} + Bu_{1} + w_{1} \\ & = A^{2}x_{0} + \begin{bmatrix}AB & B \end{bmatrix}\begin{bmatrix}u_{0}\ u_{1} \end{bmatrix} + \begin{bmatrix}A & I \end{bmatrix}\begin{bmatrix}w_{0}\ w_{1} \end{bmatrix} \tag{3} \end{align} $$

While this formulation results in a QP problem where only the control inputs are the decision variables, it results in a dense, lower triangular matrix that can suffer from numerical instability especially for a large horizon $N$.

If $k=n$ , then

$$ \begin{align} x_{n} = A^{n}x_{0} + \begin{bmatrix}A^{n-1}B & A^{n-2}B & \dots & B \end{bmatrix}\begin{bmatrix}u_{0}\\ u_{1} \\ \vdots \\ u_{n-1} \end{bmatrix} + \begin{bmatrix} A^{n-1} & A^{n-2} & \dots & I \end{bmatrix}\begin{bmatrix}w_{0}\\ w_{1} \\ \vdots \\ w_{n-1} \end{bmatrix} \tag{5} \end{align} $$

Putting all of them together with (2) to (5) yields the following matrix equation;

$$ \begin{align} \begin{bmatrix}x_{1}\\ x_{2} \\ x_{3} \\ \vdots \\ x_{n} \end{bmatrix} = \begin{bmatrix}A^{1}\\ A^{2} \\ A^{3} \\ \vdots \\ A^{n} \end{bmatrix}x_{0} + \begin{bmatrix}B & 0 & \dots & & 0 \\ AB & B & 0 & \dots & 0 \\ A^{2}B & AB & B & \dots & 0 \\ \vdots & \vdots & & & 0 \\ A^{n-1}B & A^{n-2}B & \dots & AB & B \end{bmatrix}\begin{bmatrix}u_{0}\\ u_{1} \\ u_{2} \\ \vdots \\ u_{n-1} \end{bmatrix} \\ + \begin{bmatrix}I & 0 & \dots & & 0 \\ A & I & 0 & \dots & 0 \\ A^{2} & A & I & \dots & 0 \\ \vdots & \vdots & & & 0 \\ A^{n-1} & A^{n-2} & \dots & A & I \end{bmatrix}\begin{bmatrix}w_{0}\\ w_{1} \\ w_{2} \\ \vdots \\ w_{n-1} \end{bmatrix} \tag{6} \end{align} $$

The numerical instability results from the powers of A up to $N$, which can result in matrices of zero if the system is stable (i.e $\forall_i |\lambda_i| < 1$) and $N$ is large enough (depends on precision).

I suggest that the current formulation be replace with a QP formulation without replacement, where the decision variables of the optimization are now the states along the trajectory $x_1 \dots x_N$ as well as the control inputs $u_0 \dots u_{N-1}$. This formulation results in the following matrix equation:

$$ \begin{align} \begin{bmatrix}x_{0}\\ x_{1} \\ x_{2} \\ \vdots \\ x_{n} \end{bmatrix} = \begin{bmatrix}I & 0 & \dots & & 0 \\ 0 & A & 0 & \dots & 0 \\ 0 & 0 & A & \dots & 0 \\ \vdots & \vdots & & & 0 \\ 0 & 0 & \dots & 0 & A \end{bmatrix}\begin{bmatrix}x_{0}\\ x_{1} \\ x_{2} \\ \vdots \\ x_{n} \end{bmatrix} + \begin{bmatrix}0 & \dots & & 0 \\ B & 0 & \dots & 0 \\ 0 & B & \dots & 0 \\ \vdots & & & 0 \\ 0 & \dots & 0 & B \end{bmatrix}\begin{bmatrix}u_{0}\\ u_{1} \\ u_{2} \\ \vdots \\ u_{n-1} \end{bmatrix} \\ + \begin{bmatrix}I & 0 & \dots & & 0 \\ 0 & I & 0 & \dots & 0 \\ 0 & 0 & I & \dots & 0 \\ \vdots & \vdots & & & 0 \\ 0 & 0 & \dots & 0 & I \end{bmatrix}\begin{bmatrix}w_{0}\\ w_{1} \\ w_{2} \\ \vdots \\ w_{n-1} \end{bmatrix} \tag{7} \end{align} $$

This can result in a new QP that can be rewritten as a function of the new decision variable

$$ \tilde{z} = \begin{bmatrix} x_1 & \dots & x_N & u_0 & \dots & u_N\end{bmatrix} $$

as

$$ \begin{align} \min_{\tilde{z}} \quad & \begin{bmatrix}\tilde{z}& x(0)\end{bmatrix}\begin{bmatrix} \bar{H} & 0 \\ 0 & Q \end{bmatrix}\begin{bmatrix}\tilde{z}\\ x(0)\end{bmatrix}\\\ \textrm{s.t.} \quad & G_{0,eq}\tilde{z} = E_{0,eq}x(0) \tag{11.30}\\\ &G_{0,in}\tilde{z} \leq w_{0,in} + E_{0,in}x(0)\\\ \end{align} $$

by using the following matrices

For reference on the above image, please refer to "Predictive Control for Linear and Hybrid Systems", pages 218-219 (242-243 in the PDF version)

This formulation is used by OSQP Eigen in their MPC example. This new formulation is more numerically stable as it bypasses matrix powers. Additionally, using sparse matrices to construct the $G_0$ and $\bar{H}$ and passing those to OSQP should result in faster solutions as OSQP can exploit the sparsity.

[rej55]

Hello, thank you for your suggestion!

It sounds nice for me.
obstacle_avoidance_planner also uses a MPC like algorithm.
So, the same suggested method may work in obstacle_avoidance_planner.

[TakaHoribe]

It seems the proposed formulation makes a great improvement in computation time in the obstacle_avoidance_planner.
autowarefoundation/autoware.universe#4600

[takayuki5168]

Thank you for your sharing, Horibe-san.

@AhmadAmine998 I've been struggling with the too-heavy computation cost of obstacle_avoidance_planner, which is an MPC-based path planning.
The method you suggested works significantly. I really appreciate your suggestion!