How can I minimize the LQR cost function?

1.5k Views Asked by At

LQR and MPC is equal if the MPC controller has no constraints(e.g saturation on inputs/outputs).

But LQR is closed loop feedback by using the LQR control law matrix. MPC is open loop, which means no feedback system.

The LQR control law matrix is created by the Riccati equations and MPC is created by minimize the LQR const fuction.

I wonder if this is the rigth method to use to minimize the LQR const function. First I get my discrete state space model:

$$x(k+1) = Ax(k) + Bu(k)\\y(k) = Cx(k) + Du(k)$$

MPC is a LQR with prediction and prediction means that I want to find the future state if I know the initial state $x_0$. I can show you by summing all the states. Assume that we are at state $x$ and we want to find state $x_n$.

$$x_2 = Ax + Bu_1 \\ x_3 = Ax_2 + Bu_2 = A[Ax + Bu_1] + Bu_2\\ x_4 = Ax_3 + Bu_3 = A[A[Ax + Bu_1] + Bu_2] + Bu_3\\ x_5 = Ax_4 + Bu_4 = A[A[A[Ax + Bu_1] + Bu_2] + Bu_3] + Bu_4\\ x_6 = Ax_5 + Bu_5 = A[A[A[A[Ax + Bu_1] + Bu_2] + Bu_3] + Bu_4] + Bu_5\\ x_7 = Ax_6 + Bu_6 = A[A[A[A[A[Ax + Bu_1] + Bu_2] + Bu_3] + Bu_4] + Bu_5] + Bu_6$$

This will result:

$$x_7 = A^6x + A^5Bu_1 + A^4Bu_2 + A^3Bu_3 + A^2Bu_4 + ABu_5 + Bu_6$$

And it can be expressed as:

$$x_n = A^{n-1}x_0 + \sum_{k = 0}^{n-2} A^{k}Bu_{n-k-1}$$

Where $n$ is the n-step prediction. So if we are at state $x_0$ and we want to find the state $x_n$, we can find it if we know the inputs $u_{n-k-1}$. And this is why MPC is about, finding the inputs $u_{n-k-1}$ to reach the state $x_n$.

Now we want to minimize the quadratic cost function $J$:

$$J =\sum_{k=0}^{n}(x_k^TQx_k + u_k^TRu_k)$$

Where $Q, R$ are tuning matrices. The cost function $J$ can be expressed as:

$$J = \begin{bmatrix} x_0\\ x_1\\ x_2\\ x_3\\ \vdots\\ x_n \end{bmatrix}^T\begin{bmatrix} Q & 0 & 0& 0 & \dots & 0\\ 0 & Q & 0 & 0 & \dots & 0 \\ 0 & 0 &Q & 0 & \dots & 0 \\ 0 & 0 & 0 & Q & \dots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & 0&\dots & Q \end{bmatrix}\begin{bmatrix} x_0\\ x_1\\ x_2\\ x_3\\ \vdots\\ x_n \end{bmatrix} + \begin{bmatrix} u_0\\ u_1\\ u_2\\ u_3 \\ \vdots\\ u_n \end{bmatrix}^T\begin{bmatrix} R & 0 & 0& 0 & \dots & 0\\ 0 & R & 0 & 0 & \dots & 0 \\ 0 & 0 &R & 0 & \dots & 0 \\ 0 & 0 & 0 & R & \dots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & 0 & \dots & R \end{bmatrix}\begin{bmatrix} u_0\\ u_1\\ u_2\\ u_3 \\ \vdots\\ u_n \end{bmatrix}$$

And we do the same with $$x_n = A^{n-1}x_0 + \sum_{k = 0}^{n-2} A^{k}Bu_{n-k-1}$$

Express:

$$\begin{bmatrix} x_0\\ x_1\\ x_2\\ x_3\\ \vdots\\ x_{n} \end{bmatrix} = \begin{bmatrix} A\\ A^2\\ A^3\\ A^4\\ \vdots\\ A^{n-1} \end{bmatrix}x_0+\begin{bmatrix} B & 0 & 0 & 0 & \dots & 0\\ AB & B & 0 & 0 & \dots & 0\\ A^2B & AB & B & 0 & \dots& 0\\ A^3B & A^2B & AB & B & \dots& 0\\ \vdots & \vdots & \vdots &\ddots &\ddots & 0\\ A^{n-2}B & A^{n-3}B & A^{n-4}B & A^{n-5}B & \dots & B \end{bmatrix} \begin{bmatrix} u_0\\ u_1\\ u_2\\ u_3\\ \vdots\\ u_n \end{bmatrix}$$

Now! say that:

$$\bar Q = \begin{bmatrix} Q & 0 & 0& 0 & \dots & 0\\ 0 & Q & 0 & 0 & \dots & 0 \\ 0 & 0 &Q & 0 & \dots & 0 \\ 0 & 0 & 0 & Q & \dots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & 0&\dots & Q \end{bmatrix}$$

$$\bar R = \begin{bmatrix} R & 0 & 0& 0 & \dots & 0\\ 0 & R & 0 & 0 & \dots & 0 \\ 0 & 0 &R & 0 & \dots & 0 \\ 0 & 0 & 0 & R & \dots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & 0 & \dots & R \end{bmatrix}$$

$$\bar S = \begin{bmatrix} B & 0 & 0 & 0 & \dots & 0\\ AB & B & 0 & 0 & \dots & 0\\ A^2B & AB & B & 0 & \dots& 0\\ A^3B & A^2B & AB & B & \dots& 0\\ \vdots & \vdots & \vdots &\ddots &\ddots & 0\\ A^{n-2}B & A^{n-3}B & A^{n-4}B & A^{n-5}B & \dots & B \end{bmatrix}$$

$$\bar T = \begin{bmatrix} A\\ A^2\\ A^3\\ A^4\\ \vdots\\ A^{n-1} \end{bmatrix}$$

$$z = \begin{bmatrix} u_0\\ u_1\\ u_2\\ u_3\\ \vdots\\ u_n \end{bmatrix}$$

When we can express our cost function $J$ as:

$$J = ( \bar Tx_0 +\bar Sz )^T\bar Q (\bar Tx_0 +\bar Sz ) + z^T\bar R z$$

What I have done is that I have expressed the cost function, without the sum function.

To find $z$, we need to set $J = 0$.

Question:

How can I find input signal vector $z$ which contains the signals $u$.

1

There are 1 best solutions below

6
On BEST ANSWER

When expanding the cost function and collect similar terms you get

$$ J = x_0^\top \bar{T}^\top \bar{Q}\,\bar{T}\,x_0 + x_0^\top \bar{T}^\top \bar{Q}\,\bar{S}\,z + z^\top \bar{S}^\top\bar{Q}\,\bar{T}\,x_0 + z^\top \left(\bar{S}^\top\bar{Q}\,\bar{S} + \bar{R}\right) z. $$

Since $x_0$ is given and can't be changed, therefore minimizing the cost function above is equivalent to minimizing the following cost function

$$ J = f^\top z + z^\top H\,z $$

with

$$ f^\top = 2\,x_0^\top \bar{T}^\top \bar{Q}\,\bar{S}, $$

$$ H = \bar{S}^\top\bar{Q}\,\bar{S} + \bar{R}. $$

If $H$ is positive definite then the minimum $z^*$ can be found by taking the partial derivative of $J$ with respect to $z$ and set it to zero

$$ \frac{\partial\,J}{\partial\,z} = f + 2\,H\,z = 0 $$

$$ z^* = -\frac{1}{2}H^{-1}f = -\left(\bar{S}^\top\bar{Q}\,\bar{S} + \bar{R}\right)^{-1} \bar{S}^\top \bar{Q}\,\bar{T}\,x_0. $$


It can also be noted that using your definition of $\bar{Q}$ this control signal will be equivalent to finite horizon LQR. In order to guarantee stability of MPC you can add a roll-out policy. For example to make this problem really equivalent to infinite horizon LQR you could set the last diagonal term of $\bar{Q}$ equal to $X$, the solution of the discrete algebraic Riccati equation, instead of $Q$

$$ X = A^\top X\,A - A^\top X\,B (R + B^\top X\,B)^{-1} B^\top X\,A + Q. $$