使用模型预测控制（MPC）的移动机器人路径

我正在尝试实现一种基于MPC（模型预测控制）的路径跟踪算法，该算法在存在速度约束的情况下遵循移动机器人的路径

原理：使用机器人模型和路径，该算法会在N个未来步骤中预测机器人的行为，以计算一系列命令$（v，\ omega）$，从而使机器人能够沿路径行驶而不会超出轨迹，从而可以在急转弯之前减速，等
$ v：$线速度
$ \ omega：$角速度

机器人：我有一个像这样的非完整机器人（图片摘自本文）上面）：

这是我的问题：在移动机器人上实现之前，我正在尝试计算所需的矩阵（使用Matlab）以测试该算法的效率。在矩阵计算的最后，其中一些具有维度不匹配

我做了什么：
对于那些感兴趣的人，该计算来自§4（4.1，4.2，4.3，4.4）p6-本文的7。


4.1模型

$ z_ {k + 1} = Az_k + B_ \ phi \ phi_k + B_rr_k $（18）
具有：
$ A = \ begin {bmatrix} 1和电视\\ 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 $：采样周期
$ v $：线性速度
$ k $：采样指数（即$ t = kT $）
$ z_k：$状态向量$ z_k =（d_k，\ theta_k）^ T $位置和角度差到参考路径
$ r_k：$带有$ \ psi_k $的参考矢量$ r_k =（0，\ psi_k）^ T $是步骤k处路径的参考角度

4.2准则

后退预测性水平控制器基于准则的最小化
$ J = \ Sigma ^ N_ {n = 0}（\ hat {z} _ {k + n }-r_ {k + n}）^ TQ（\ hat {z} _ {k + n}-r_ {k + n}）+ \ lambda \ phi ^ 2_ {k + n} $，（20）
受不平等约束的约束
$ P \ begin {bmatrix} v_n \\ v_n \ phi_n \ end {bmatrix} \ leq q，$
$ n = 0，...，N，$
其中$ \ hat {z} $是预测的输出，$ Q $是权重矩阵，$ \ lambda $是标量权重，$ N $是预测范围。

4.3 Predictor

An从迭代中很容易找到n阶预测变量$ \ hat {z} _ {k + n | k} $（18）。将预测$ \ hat {z} _ {k + n | k}，n = n，...，N $堆叠在向量$ \ hat {Z} $中会产生
$ \ 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 $，
和
$ 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} $ />

其中索引$ i $应替换为$ \ phi $或$ r $


4.4控制器

使用N步预测器（22）可将准则（20）简化为$ J_k =（\ hat {Z} _k-R_k）^ T I_q（\ hat {Z} _k-R_k）+ \ lambda \ Phi ^ T_k \ Phi_k $，（23）
其中$ I_q $是适当d的对角矩阵对角线中的Q实例具有无穷大的感觉。通过对$ \ Phi $最小化（23）来找到不受约束的控制器：
$ \ 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）$


我正在尝试计算$ \ Phi_k = -L_zz_k-L_rR_k $，但$ L_r $和$ R_k $的维数与矩阵乘法不匹配。 >

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

$：
$ R_k $ a（ 11x2）矩阵（N + 1个大小为2x1的元素，已转置）
$ G_w $ a（22x11）矩阵
$ G ^ T_w $ a（11x22）矩阵
$ I_q $ a（22x22 ）矩阵
$ F $ a（22x2）矩阵
$ G_r $ a（22x22）矩阵

因此Lz计算得出（根据矩阵大小）
$ L_z =（标量+（11x22）（22x22）（22x11））^ {-1}（11x22）（22x22）（22x22）$
（11x2）矩阵。
因为$ z_k $是（ 2x1）矩阵，从（24）做$ L_zz_k $很好。

，Lr计算得出（根据矩阵大小）
$ L_r =（标量+（11x22）（22x22）（22x11））^ {-1}（11x22）（22x22） （（（22x22）-（22x22））$
（11x22）矩阵。由于$ R_k $是（11x2）矩阵，因此无法从（24）执行$ L_rR_k $。
我有一个（11x22）矩阵乘以（11x2）矩阵。

我确定我在这里错过了一些大东西，但无法确切看到。
任何帮助表示感谢。

感谢