Mobile robot path following using Model Predictive Control (MPC)

Question

I'am trying to implement a path following algorithm based on MPC (Model Predictive Control), found in this paper : Path Following Mobile Robot in the Presence of Velocity Constraints

Principle: Using the robot model and the path, the algorithm predict the behavior of the robot over N future steps to compute a sequence of commands $(v,\omega)$ to allow the robot to follow the path without overshooting the trajectory, allowing to slow down before a sharp turn, etc.
$v:$ Linear velocity
$\omega:$ Angular velocity

The robot: I have a non-holonomic robot like this one (Image extracted from the paper above) :

Here is my problem: Before implementing on the mobile robot, I'am trying to compute the needed matrices (using Matlab) to test the efficiency of this algorithm. At the end of the matrices computation some of them have dimension mismatch

What I did:
For those interested, this calculation is from §4 (4.1, 4.2, 4.3, 4.4) p6-7 of the paper.

4.1 Model

$z_{k+1} = Az_k + B_\phi\phi_k + B_rr_k$ (18) with:
$A = \begin{bmatrix} 1 & Tv \\ 0 & 1 \end{bmatrix}$ $B_\phi = \begin{bmatrix} {T^2\over2}v^2\\ Tv \end{bmatrix}$ $B_r = \begin{bmatrix} 0 & -Tv \\ 0 & 0 \end{bmatrix}$
$T$: sampling period
$v$: linear velocity
$k$: sampling index (i.e. $t= kT$)
$z_k:$ the state vector $z_k = (d_k, \theta_k)^T$ position and angle difference to the reference path $r_k:$ the reference vector $r_k = (0, \psi_k)^T$ with $\psi_k$ is the reference angle of the path at step k

4.2 Criterion

The predictive receding horizon controller is based on a minimization of the criterion
$J= \Sigma^N_{n=0} (\hat{z}_{k+n} - r_{k+n})^T Q(\hat{z}_{k+n} - r_{k+n}) + \lambda\phi^2_{k+n}$, (20)
Subject to the inequality constraint
$ P\begin{bmatrix} v_n \\ v_n\phi_n \end{bmatrix} \leq q,$
$n=0,..., N,$
where $\hat{z}$ is the predicted output, $Q$ is a weight matric, $\lambda$ is a scalar weight, and $N$ is prediction horizon.

4.3 Predictor

An n-step predictor $\hat{z}_{k+n|k}$ is easily found from iterating (18). Stacking the predictions $\hat{z}_{k+n|k},n = n,...,N$ in the vector $\hat{Z}$ yields
$\hat{Z} = \begin{bmatrix} \hat{z}_{k|k} \\ \vdots \\ \hat{z}_{k+N|k}\end{bmatrix} = Fz_k + G_\phi\Phi_k + G_rR_k$ (22)
with
$\Phi_k = \begin{bmatrix} \phi_k, \ldots, \phi_{k+N}\end{bmatrix}^T$,
$R_k = \begin{bmatrix} r_k, \ldots, r_{k+N}\end{bmatrix}^T$,
and
$F = \begin{bmatrix}I & A & \ldots & A^N \end{bmatrix}^T$
$G_i = \begin{bmatrix} 0 & 0 & \ldots & 0 & 0 \\ B_i & 0 & \ldots & 0 & 0 \\ AB_i & B_i & \ddots & \vdots & \vdots \\ \vdots & \ddots & \ddots & 0 & 0 \\ A^{N-1}B_i & \ldots & AB_i & B_i & 0 \end{bmatrix}$

where index $i$ should be substituted with either $\phi$ or $r$

4.4 Controller

Using the N-step predictor (22) simplifies the criterion (20) to $J_k = (\hat{Z}_k - R_k)^T I_q (\hat{Z}_k - R_k) + \lambda\Phi^T_k\Phi_k$, (23) where $I_q$ is a diagonal matrix of appropriate dimension with instances of Q in the diagonal. The unconstrained controller is found by minimizing (23) with respect to $\Phi$:
$\Phi_k = -L_zz_k - L_rR_k$, (24)
with
$L_z = (lambda + G^T_wI_qG_w)^{-1}G^T_wI_qF$ $L_r = (lambda + G^T_wI_qG_w)^{-1}G^T_wI_q(Gr - I)$

I'am trying to compute $\Phi_k = -L_zz_k - L_rR_k$ but the dimension of $L_r$ and $R_k$ does not match for matrix multiplication.

Parameters are :

• $T=0.1s$
• $N=10$
• $\lambda=0.0001$
• $Q=\begin{bmatrix} 1 & 0 \\ 0 & \delta \end{bmatrix}$ with $\delta=0.02$

I get :
$R_k$ a (11x2) matrix (N+1 elements of size 2x1, transposed)
$G_w$ a (22x11) matrix
$G^T_w$ a (11x22) matrix
$I_q$ a (22x22) matrix
$F$ a (22x2) matrix
$G_r$ a (22x22) matrix

so Lz computation gives (according to the matrix sizes)
$L_z=(scalar + (11x22)(22x22)(22x11))^{-1} (11x22)(22x22)(22x22)$
a (11x2) matrix.
as $z_k$ is (2x1) matrix, doing $L_zz_k$ from (24) is fine.

and Lr computation gives (according to the matrix sizes) $L_r=(scalar + (11x22)(22x22)(22x11))^{-1} (11x22)(22x22)((22x22) - (22x22))$
a (11x22) matrix.
as $R_k$ is (11x2) matrix, doing $L_rR_k$ from (24) is not possible.
I have a (11x22) matrix multiplicated by a (11x2) matrix.

I'm sure I'm missing something big here but unable to see what exactly. Any help appreciated.

Thanks

Answer 1

I found the solution to my problem : I was misunderstanding $R_k$ notation.

$r_k:$ the reference vector $r_k = (0, \psi_k)^T$

$R_k = \begin{bmatrix} r_k, \ldots, r_{k+N}\end{bmatrix}^T$

To compute $R_k$, I was replacing each value of $r_k$ and then, doing the transpose. But instead I have to do the transpose first, and then replace the values of $r_k$. Doing this, I get $R_k$ a (22x1) matrix and the computation works.

I did some tests with Matlab, but didn't manage to really make it work and as I didn't have the time to continue to work on it, it's in stand by. Anyway, if someone reading this, managed or trying to make a similar algorithm work, I'm interested to talk about it (but this is out of the scope of my original question).

Answer 2

The paper uses a mathematical formula for predicting the consequences of actions. That makes it difficult to debug possible failures. Improvement would be to use a physics engine like Box2D to predict actions. The advantage is, that prediction and controller are separated.

Update: Optimal Control is done with a forward model. (From the current situation a projection is made into future). I'm not sure how this can be done with "Rk=[rk,…,rk+N]T".