精华内容
下载资源
问答
  • 在这项工作中,我们使用 PID 控制和滑模控制开发了自主车辆横向控制策略,以最大限度地减少自主车辆的横向位移,以遵守参考路径。
  • 车辆横向运动学模型 如下图所示为自行车模型。 存在假设:前轮和后轮的运动方向都是沿着轮胎旋转方向,也就是轮胎的slip angle是0。在低速(<5m/s)时,这个假设是合理的。 从几何关系进行推导: 在三角形OCA中,...

    本文参考自Rajesh Rajamani的《Vehicle Dynamics and Control》加入了一些忽略的推导步骤。

    车辆横向运动学模型

    如下图所示为自行车模型。
    存在假设:前轮和后轮的运动方向都是沿着轮胎旋转方向,也就是轮胎的slip angle是0。在低速(<5m/s)时,这个假设是合理的。

    从几何关系进行推导:
    在三角形OCA中,根据正弦定理有:
    sin ⁡ ( δ f − β ) l f = sin ⁡ ( π 2 − δ f ) R (1) \frac{\sin(\delta_f-\beta)}{l_f} = \frac{\sin(\frac{\pi}{2}-\delta_f)}{R} \tag{1} lfsin(δfβ)=Rsin(2πδf)(1)
    同理在三角形OCB中,有:
    sin ⁡ ( β − δ r ) l r = sin ⁡ ( π 2 + δ r ) R (2) \frac{\sin(\beta-\delta_r)}{l_r} = \frac{\sin(\frac{\pi}{2}+\delta_r)}{R} \tag{2} lrsin(βδr)=Rsin(2π+δr)(2)

    将式(1)(2)三角函数展开得到:
    sin ⁡ ( δ f ) cos ⁡ ( β ) − sin ⁡ ( β ) cos ⁡ ( δ f ) l f = cos ⁡ ( δ f ) R (3) \frac{\sin(\delta_f)\cos(\beta)-\sin(\beta)\cos(\delta_f)}{l_f} = \frac{\cos(\delta_f)}{R} \tag{3} lfsin(δf)cos(β)sin(β)cos(δf)=Rcos(δf)(3)
    cos ⁡ ( δ r ) sin ⁡ ( β ) − cos ⁡ ( β ) sin ⁡ ( δ r ) l r = cos ⁡ ( δ r ) R (4) \frac{\cos(\delta_r)\sin(\beta)-\cos(\beta)\sin(\delta_r)}{l_r} = \frac{\cos(\delta_r)}{R} \tag{4} lrcos(δr)sin(β)cos(β)sin(δr)=Rcos(δr)(4)

    在本模型假设下,在车辆行驶的任何一个瞬间,质心侧偏角,转向半径,以及前后轮转角及车辆几何参数都满足上述几何关系。

    对等式(3)进行化简,可得:
    tan ⁡ ( δ f ) cos ⁡ ( β ) − sin ⁡ ( β ) = l f R (5) \tan(\delta_f)\cos(\beta)-\sin(\beta)=\frac{l_f}{R} \tag{5} tan(δf)cos(β)sin(β)=Rlf(5)
    sin ⁡ ( β ) − tan ⁡ ( δ r ) cos ⁡ ( β ) = l r R (6) \sin(\beta)-\tan(\delta_r)\cos(\beta) = \frac{l_r}{R} \tag{6} sin(β)tan(δr)cos(β)=Rlr(6)

    将等式(5)(6)相加,可得:
    ( tan ⁡ ( δ f ) − tan ⁡ ( δ r ) ) cos ⁡ ( β ) = l f + l r R (7) (\tan(\delta_f)-\tan(\delta_r))\cos(\beta)=\frac{l_f+l_r}{R} \tag{7} (tan(δf)tan(δr))cos(β)=Rlf+lr(7)
    在低速下有:
    ψ ˙ = V R (8) \dot{\psi}=\frac{V}{R} \tag{8} ψ˙=RV(8)
    将(8)代入(7)可得车辆横摆角速度:
    ψ ˙ = V cos ⁡ ( β ) l f + l r ( tan ⁡ ( δ f ) − tan ⁡ ( δ r ) ) (9) \dot{\psi}=\frac{V\cos(\beta)}{l_f+l_r} (\tan(\delta_f)-\tan(\delta_r)) \tag{9} ψ˙=lf+lrVcos(β)(tan(δf)tan(δr))(9)
    在这里插入图片描述
    将式(5)(6)变换为:
    tan ⁡ ( δ f ) cos ⁡ ( β ) − sin ⁡ ( β ) l f = 1 / R (10) \frac{\tan(\delta_f)\cos(\beta)-\sin(\beta)}{l_f} = 1/R \tag{10} lftan(δf)cos(β)sin(β)=1/R(10)
    sin ⁡ ( β ) − tan ⁡ ( δ r ) cos ⁡ ( β ) l r = 1 / R (11) \frac{\sin(\beta)-\tan(\delta_r)\cos(\beta)}{l_r} = 1/R \tag{11} lrsin(β)tan(δr)cos(β)=1/R(11)
    将式(10)(11)相减得:
    l r tan ⁡ ( δ f ) cos ⁡ ( β ) − l r sin ⁡ ( β ) = l f sin ⁡ ( β ) − l f tan ⁡ ( δ r ) cos ⁡ ( β ) (12) l_r\tan(\delta_f)\cos(\beta)-l_r\sin(\beta)=l_f\sin(\beta)-l_f\tan(\delta_r)\cos(\beta) \tag{12} lrtan(δf)cos(β)lrsin(β)=lfsin(β)lftan(δr)cos(β)(12)
    进一步化简可得:
    β = tan ⁡ − 1 ( l f tan ⁡ δ r + l r tan ⁡ δ f l f + l r ) (13) \beta=\tan^{-1}(\frac{l_f\tan\delta_r+l_r\tan\delta_f}{l_f+l_r}) \tag{13} β=tan1(lf+lrlftanδr+lrtanδf)(13)
    从式(13)可以看出:
    (1)在质心侧偏角受车辆几何参数以及前后轮转向角的影响。当给定车辆几何参数以及前后轮转向角,就可以唯一的确定质心侧偏角。
    (2)这里“质心”其实也是几何上的概念,并不是真正的“质心”。从上图中可以看出,我们其实可以选取空间中任何一点,对这一点进行解三角形,进而获得该点的运动速度方向,也就是“质心侧偏角”。
    (3)如果将求解的点放到后轴中心,可以发现在后轴上是没有侧偏角的;如果将求解的点放在前轴中心,则侧片角就是前轮转角,这一点和假设一致。

    车辆横向动力学模型

    在高速情况下,“每一个车轮的车速是沿车轮方向”这个假设不再成立。因此考虑引入动态模型。
    考虑二自由度车辆模型:这里的二自由度是指车辆的横向位置和车辆在大地坐标系下的方向角。
    在这里插入图片描述

    考虑 y y y方向的运动,有:
    m a y = F y f + F y r (14) ma_y=F_{yf}+F_{yr} \tag{14} may=Fyf+Fyr(14)

    考虑 y y y方向的加速度:
    a y = y ¨ + V x ψ ˙ (15) a_y = \ddot{y}+V_x\dot{\psi} \tag{15} ay=y¨+Vxψ˙(15)
    将公式(15)代入公式(14)得:
    m ( y ¨ + ψ ˙ V x ) = F y f + F y r (16) m(\ddot{y}+\dot{\psi}V_x)=F_{yf}+F_{yr} \tag{16} m(y¨+ψ˙Vx)=Fyf+Fyr(16)
    考虑绕z轴旋转方向:
    I z ψ ¨ = l f F y f − l f F y r (16.1) I_z\ddot{\psi}=l_fF_{yf}-l_fF_{yr} \tag{16.1} Izψ¨=lfFyflfFyr(16.1)

    在这里插入图片描述
    为了进一步描述轮胎所受的侧向力,如上图所示引入轮胎侧偏角概念:
    α f = δ − θ V f (17) \alpha_f = \delta-\theta_{Vf} \tag{17} αf=δθVf(17)
    对于后轮:
    α r = − θ V f (18) \alpha_r = -\theta_{Vf} \tag{18} αr=θVf(18)

    由于轮胎所受侧向力和轮胎侧偏角成正比,有:
    F y f = 2 C α f ( δ − θ V f ) (19) F_{yf}=2C_{\alpha f}(\delta-\theta_{Vf}) \tag{19} Fyf=2Cαf(δθVf)(19)
    其中, C α f C_{\alpha f} Cαf为轮胎侧偏刚度,2表示有两个轮子!!
    同样的,后轮侧向力表达为:
    F y r = 2 C α r ( − θ V r ) (20) F_{yr}=2C_{\alpha r}(-\theta_{Vr}) \tag{20} Fyr=2Cαr(θVr)(20)
    下面求 θ V f \theta_{Vf} θVf θ V r \theta_{Vr} θVr
    tan ⁡ ( θ V f ) = V y + l f ψ ˙ V x (21) \tan(\theta_{Vf})=\frac{V_y+l_f\dot{\psi}}{V_x} \tag{21} tan(θVf)=VxVy+lfψ˙(21)

    tan ⁡ ( θ V r ) = V y − l r ψ ˙ V x (22) \tan(\theta_{Vr})=\frac{V_y-l_r\dot{\psi}}{V_x} \tag{22} tan(θVr)=VxVylrψ˙(22)
    利用小角度线性的假设:
    θ V f = y ˙ + l f ψ ˙ V x (23) \theta_{Vf}=\frac{\dot{y}+l_f\dot{\psi}}{V_x} \tag{23} θVf=Vxy˙+lfψ˙(23)
    θ V r = y ˙ − l r ψ ˙ V x (24) \theta_{Vr}=\frac{\dot{y}-l_r\dot{\psi}}{V_x} \tag{24} θVr=Vxy˙lrψ˙(24)

    这里,对式(23)(24)进行变换,可得:
    θ V f = β + l f ψ ˙ V x (24.1) \theta_{Vf}=\beta+\frac{l_f\dot{\psi}}{V_x} \tag{24.1} θVf=β+Vxlfψ˙(24.1)
    θ V r = β − l r ψ ˙ V x (24.2) \theta_{Vr}=\beta-\frac{l_r\dot{\psi}}{V_x} \tag{24.2} θVr=βVxlrψ˙(24.2)
    上述两个表达式将前轮和后轮速度与质心侧偏角关联起来,后文推导质心侧偏角时有用。

    将式(17)至式(24)代入式(16)并整理,可以得到状态空间方程:
    在这里插入图片描述

    关于偏差的动力学模型表达

    在控制车辆运动的时候主要是消除车辆和目标线的偏差,因此考虑偏差的动力学方程,引入误差状态量
    e 1 e_1 e1: 车辆质心到道路中心线的距离(这里应该就是指最短的距离)
    e 2 e_2 e2: 车辆的方向和道路方向的偏差(这里应当具体参考frenet坐标,感觉描述的并不是很准确)
    考虑纵向车速 V x V_x Vx,车道半径 R R R,期望的车辆横摆角速度为:
    ψ ˙ = V x R (26) \dot{\psi}=\frac{V_x}{R} \tag{26} ψ˙=RVx(26)
    期望的车辆横向加速度为:
    V x 2 R = V x ψ ˙ (27) \frac{V_x^2}{R}=V_x \dot{\psi} \tag{27} RVx2=Vxψ˙(27)
    e 1 ¨ \ddot{e_1} e1¨ e 2 e_2 e2定义如下:
    e 1 ¨ = ( y ¨ + V x ψ ˙ ) − V x 2 R = y ¨ + V x ( ψ ˙ − ψ ˙ d e s ) (28) \ddot{e_1}=(\ddot{y}+V_x \dot{\psi}) - \frac{V_x^2}{R} = \ddot{y}+V_x (\dot{\psi}-\dot{\psi}_{des}) \tag{28} e1¨=(y¨+Vxψ˙)RVx2=y¨+Vx(ψ˙ψ˙des)(28)
    e 2 = ψ − ψ d e s (29) e_2=\psi-\psi_{des} \tag{29} e2=ψψdes(29)
    其中 e 1 ¨ \ddot{e_1} e1¨是用车辆当前的横向加速度,减去期望的车辆横向加速度。
    e 1 ¨ \ddot{e_1} e1¨进行积分得到:
    e ˙ 1 = y ˙ + V x ( ψ − ψ d e s ) (30) \dot{e}_1=\dot{y}+V_x(\psi-\psi_{des}) \tag{30} e˙1=y˙+Vx(ψψdes)(30)
    e 1 e_1 e1的表达式在这里并没有用到, e ˙ 2 = ψ ˙ − ψ ˙ d e s \dot{e}_2=\dot{\psi}-\dot{\psi}_{des} e˙2=ψ˙ψ˙des,并且 e ¨ 2 = ψ ¨ − 0 \ddot{e}_2=\ddot{\psi}-0 e¨2=ψ¨0
    将上述式子代入公式(16)和(16.1)中进行化简,可得:
    在这里插入图片描述
    在这里插入图片描述
    写成状态空间方程:
    在这里插入图片描述
    在这里插入图片描述

    横摆角速度和质心侧偏角表达的动力学方程

    在这里插入图片描述
    依照上图标注,有:
    β = y ˙ V x , d β d t = y ¨ V x \beta=\frac{\dot{y}}{V_x}, \frac{d\beta}{dt}=\frac{\ddot{y}}{V_x} β=Vxy˙,dtdβ=Vxy¨
    上式将车辆横向加速度与质心侧偏角的导数进行了关联,下面考虑将侧向力与质心侧偏角进行关联,利用上文公式(24.1)与(24.2)有:
    F y f = C α f α f , α f = δ − θ V f = δ − β − l f ψ ˙ V x F_{yf}=C_{\alpha f}\alpha_f, \alpha_f=\delta-\theta_{Vf}=\delta-\beta-\frac{l_f \dot{\psi}}{V_x} Fyf=Cαfαf,αf=δθVf=δβVxlfψ˙
    F y r = C α r α r , α r = − θ V r = − β + l r ψ ˙ V x F_{yr}=C_{\alpha r}\alpha_r, \alpha_r=-\theta_{Vr}=-\beta+\frac{l_r\dot{\psi}}{V_x} Fyr=Cαrαr,αr=θVr=β+Vxlrψ˙
    再结合式(16)(16.1)可以求出以横摆角速度和质心侧偏角为状态量的状态方程。如下

    在这里插入图片描述

    展开全文
  • 车辆前轮转角(横向)、车辆油门开度,刹车开度(纵向) pure pursuit 控制核心思想 纯跟踪控制算法是基于车辆运动学模型(将车辆简单描述为两轮自行车模型)的一种算法。 该算法的步骤是: 找到path(输入的局部...

    控制模块的输入输出:

    输入:
    局部路径、车辆状态、车辆位置

    输出
    车辆前轮转角(横向)、车辆油门开度,刹车开度(纵向)

    pure pursuit 控制核心思想
    纯跟踪控制算法是基于车辆运动学模型(将车辆简单描述为两轮自行车模型)的一种算法。

    该算法的步骤是:

    1. 找到path(输入的局部路径)上距离车辆后轴(点B)最近的点A
    2. 沿着A往前一段前视距离找到点C
    3. 要控制前轮的转角,使B点经过点C
    4. 当然可以用一直线连接点B、C,也可以用圆弧连接两点,连接的直线或者圆弧就能算出前轮转角
    5. 按照3计算得出的车轮转角,控制车辆沿着直线或者圆弧到达C点
    展开全文
  • 文章目录 概述 仿真 开环系统 闭环系统 仿真代码 结论 概述 根据车辆模型-跟踪误差模型,在小滑移角假设下,车辆横向动力学模型的状态空间方程可以写为 x˙=Ax+B1δ+B2ψ˙des(1) \dot{x} = Ax + B_1\delta + B_2\...


    概述

    根据车辆模型-跟踪误差模型,在小滑移角假设下,车辆横向动力学模型的状态空间方程可以写为
    x ˙ = A x + B 1 δ + B 2 ψ ˙ d e s (1) \dot{x} = Ax + B_1\delta + B_2\dot{\psi}_{des} \tag{1} x˙=Ax+B1δ+B2ψ˙des(1)

    其中, x = [ e y , e ˙ y , e ψ , e ˙ ψ ] T x = [e_y,\dot{e}_y,e_\psi,\dot{e}_{\psi}]^T x=[ey,e˙y,eψ,e˙ψ]T e y e_y ey代表车辆重心与目标曲线的距离, e ψ e_{\psi} eψ代表车辆与目标跟踪曲线之间的偏航角误差, δ \delta δ代表前轮转向角, ψ ˙ d e s \dot{\psi}_{des} ψ˙des代表期望偏航角速度,该参数由道路曲率和车速决定,矩阵 A A A B 1 B_1 B1 B 2 B_2 B2车辆模型-跟踪误差模型中已经提出。

    仿真

    使用实际车辆的参数带入上述动力学模型中

    物理量数值单位
    质量( M M M)1573 k g kg kg
    z z z轴转动惯量( I z I_z Iz)2873 k s / m 2 ks/m^2 ks/m2
    前轴到重心距离( l f l_f lf)1.10 m m m
    后轴到重心距离( l r l_r lr)1.58 m m m
    前轮总侧偏刚度( C α f C_{\alpha f} Cαf)80000 N / r a d N/rad N/rad
    后轮总侧偏刚度( C α r C_{\alpha r} Cαr)80000 N / r a d N/rad N/rad
    x x x轴速度( V x V_x Vx)30 m / s m/s m/s
    转弯半径( R R R)1000 m m m

    开环系统

    如下图所示,对于开环系统,矩阵 A A A有两个特征值位于坐标原点,所以系统是不稳定的。

    闭环系统

    研究表明, ( A , B 1 ) (A,B_1) (A,B1)对是可控的,因此使用状态反馈法
    δ = − K x = − k 1 e y − k 2 e ˙ y − k 3 e ψ − k 4 e ˙ ψ (2) \delta = -Kx = -k_1e_y - k_2\dot{e}_y - k_3e_{\psi} - k_4\dot{e}_{\psi} \tag{2} δ=Kx=k1eyk2e˙yk3eψk4e˙ψ(2)
    闭环矩阵 ( A − B K ) (A - BK) (ABK)的特征值可以放置于任何想要的位置,闭环系统的状态反馈控制如下:
    x ˙ = ( A − B 1 K ) x + B 2 ψ ˙ d e s (3) \dot{x} = (A - B_1K)x + B_2\dot{\psi}_{des} \tag{3} x˙=(AB1K)x+B2ψ˙des(3)
    使用Python的control库中的place函数去设置闭环系统的特征值

    K = ct.place(A,B1,P)
    

    上述函数产生一个反馈矩阵 K K K,使得矩阵 ( A − B 1 K ) (A - B_1K) (AB1K)的特征值位于指定向量 P P P的位置。
    下面将特征值放置在 [ − 5 − 3 j − 5 + 3 j − 7 − 10 ] T \begin{bmatrix}-5-3j&-5+3j&-7&-10\end{bmatrix}^T [53j5+3j710]T位置,零极点分布如下:

    阶跃响应如下:

    仿真代码

    import numpy as np
    import control as ct
    import matplotlib.pyplot as plt
    
    # 车辆参数
    M   = 1573.0 #(kg) 总质量 mass
    I_z = 2873.0 # (kg/m^2) 绕Z轴的转动惯量 
    l_f = 1.10  # (m)
    l_r = 1.58  # (m)
    C_alpha_f = 80000.0 #(N/rad) 前轮总侧偏刚度
    C_alpha_r = 80000.0 #(N/rad) 后轮总侧偏刚度
    V_x = 30.0 # (m/s)
    
    A = np.array([
            [0.,1.,0.,0.],
            [0.,-2.*(C_alpha_f + C_alpha_r)/(M*V_x),2.*(C_alpha_f + C_alpha_r)/M,-2.*(C_alpha_f*l_f - C_alpha_r*l_r)/(M*V_x)],
            [0.,0.,0.,1.],
            [0.,-2.*(C_alpha_f*l_f - C_alpha_r*l_r)/(I_z*V_x),2.*(C_alpha_f*l_f - C_alpha_r*l_r)/I_z,-2.*(C_alpha_f*l_f**2 + C_alpha_r*l_r**2)/(I_z*V_x)]
            ])
    
    B1 = np.array([
            [0.],
            [2.*C_alpha_f/M],
            [0.],
            [2.*l_f*C_alpha_f/I_z]
            ])
    
    B2 = np.array([[0.],[-2.*(C_alpha_f*l_f - C_alpha_r*l_r)/(M*V_x) - V_x],[0.],[-2.*(C_alpha_f*l_f**2 + C_alpha_r*l_r**2)/(I_z*V_x)]])
    
    C = np.array([[1., 0., 0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.]])
    
    D = np.array([[0.],[0.],[0.],[0]])
    
    w,v = np.linalg.eig(A) # 求取数组的特征值
    print("特征值:",w)
    #print(v)
    P = np.array([-5.-3.j,-5.+3.j,-7.,-10.])
    K = ct.place(A,B1,P)
    print("反馈值K:",K)
    
    sys_init = ct.ss(A,B1,C,D)
    ct.pzmap(sys_init)
    sys_task = ct.ss(A-B1*K,B2,C,D)
    ct.pzmap(sys_task)
    
    t = np.linspace(0, 10, 101)
    u = np.zeros(len(t))
    u[11:101] = 0.03 #rad/s
    
    f_t,f_yout,f_xout = ct.forced_response(sys_task,t,u)
    
    #plt.close()
    plt.figure()
    plt.clf()
    
    plt.subplot(3,1,1)
    plt.grid()
    plt.ylabel("(rad/s)")
    plt.plot(f_t,u)
    plt.subplot(3,1,2)
    plt.grid()
    plt.ylabel("e1:(m)")
    plt.plot(f_t,f_yout[0])
    plt.subplot(3,1,3)
    plt.grid()
    plt.xlabel("time:(s)")
    plt.ylabel("e2:(rad)")
    plt.plot(f_t,f_yout[2])
    

    结论

    由于 B 2 ψ ˙ d e s B_2\dot{\psi}_{des} B2ψ˙des项的存在,,即使矩阵 ( A − B 1 K ) (A - B_1K) (AB1K)稳定的情况下,跟踪误差也无法达到0。稳定状态下的 e y e_y ey e ψ e_{\psi} eψ不为0,主要因为道路曲率 ψ ˙ d e s \dot{\psi}_{des} ψ˙des不为0。

    展开全文
  • 即通过滚动时域的方法, 保障步骤三的操作可以满足最终的控制目标,而控制过程又符合优化损失函数的预期.两者相互配合,形成闭环. 值得一提的是,滚动时域的策略使MPC的优化求解都是在一个有限时间的窗口下进行的,因此是...

    求解MPC: 在滚动时间窗内建立并求解QP问题

    该小节旨在根据上一节得到的离散车辆横向动力学模型,在滚动时间窗内建立并求解QP问题,实现MPC横向控制.
    在开始求解之前,我们需要回答以下两个问题:
    1. 什 么 是 Q P 问 题 ? 其 标 准 形 式 的 什 么 样 的 ? 2. 怎 么 把 M P C 中 的 优 化 问 题 转 化 为 求 解 Q P 问 题 ? \begin{aligned} &1.什么是QP问题?其标准形式的什么样的? \\ &2.怎么把MPC中的优化问题转化为求解QP问题? \end{aligned} 1.QP??2.MPCQP?
    简要回答第一个问题: QP(Quadratic programming),即二次规划.旨在求解最小化一个带线性约束的多元二次型代价函数的最优变量值.其标准形式为
    m i n x J = m i n x 1 2 x T Q x + x T g s . t . A x ≤ b \begin{aligned} &\mathop{min}\limits_{x}\quad J \quad=\quad \mathop{min}\limits_{x} &&\frac{1}{2}x^TQx+x^Tg\\ &s.t. && Ax \leq b\\ \end{aligned} xminJ=xmins.t.21xTQx+xTgAxb
    其中,当且仅当 Q ⪰ 0 Q \succeq 0 Q0,即 Q Q Q 为半正定矩阵时,QP问题为一个凸优化问题.

    目前有很多开源的QP求解库,如 O S Q P / q p O A S E S OSQP / qpOASES OSQP/qpOASES,以及 M a t l a b Matlab Matlab 中的 q u a d p r o g quadprog quadprog 都为以上QP问题提供了求解的API.

    接下来回答第二个问题,如何将MPC转化为求解QP问题?
    通常的,MPC控制算法包含以下四个基本步骤:
    1.根据已知的系统模型,控制序列与当前的系统状态变量,对预测时域内各状态变量作出预测 2.根据第一步的预测结果,计算预测时域内,满足约束的条件,最小化损失函数  J  的控制序列 3.将第二步中得到的控制序列的第一个控制量输入到系统中 4.在下一次采样时间,将时间往前推动一步,更新第一步中的当前的系统状态变量,重复步骤1-3 \begin{aligned} &\text{1.根据已知的系统模型,控制序列与当前的系统状态变量,对预测时域内各状态变量作出预测}\\ &\text{2.根据第一步的预测结果,计算预测时域内,满足约束的条件,最小化损失函数 $J$ 的控制序列}\\ &\text{3.将第二步中得到的控制序列的第一个控制量输入到系统中}\\ &\text{4.在下一次采样时间,将时间往前推动一步,更新第一步中的当前的系统状态变量,重复步骤1-3} \end{aligned} 1.根据已知的系统模型,控制序列与当前的系统状态变量,对预测时域内各状态变量作出预测2.根据第一步的预测结果,计算预测时域内,满足约束的条件,最小化损失函数 J 的控制序列3.将第二步中得到的控制序列的第一个控制量输入到系统中4.在下一次采样时间,将时间往前推动一步,更新第一步中的当前的系统状态变量,重复步骤1-3
    步骤一中,我们需要根据当前系统状态和已知的控制序列对整个时间窗内的状态变量值作出预测. 我们可以通过迭代系统的离散模型得到 t + k t+k t+k 时刻的状态变量与 t t t 时刻(当前时刻)的状态变量和控制序列之间的关系,即系统的预测方程.

    步骤二中,我们将时间窗内的系统性能期望构造为一个二次型损失函数,并将控制过程中所涉及到的约束条件都改写为 A x ⪯ b Ax \preceq b Axb 的形式.至此,我们便构造了一个标准QP问题,接下来就是采用合适的优化算法(GD,Newton,ADMM等)对问题进行求解,得到时间窗内的最优控制序列.

    步骤三中,我们只将控制序列中的第一个控制量作为系统的真实输入.采用这种策略,我认为原因有二.
    原因一是因为步骤一和步骤二中的预测以及优化过程均为开环.在系统不确定度的影响下,这种计算是粗略的.且预测时域越长,与实际系统的偏差越大.因此,我们需要引入真实系统的反馈去修正预测.那么在当前时刻,新的系统反馈抵达之前,采用控制序列中的第一个控制量作为系统的真实输入是合适的.

    略作延伸, 我们可以把MPC算法与Kalman滤波算法作对比.Kalman滤波算法中也有相似的预测过程,同理,若想要预测结果收敛于真值,单有预测是不够的.我们需要持续将最新的观测纳入考虑,利用其中蕴含的信息对预测过程中的相关量进行修正.Kalman的做法是通过贝叶斯定理,利用观测量对协方差矩阵进行修正.

    那么MPC是如何利用最新的观测的呢? MPC的策略是在进入下一次采样时间后, 将最新的观测量转化为预测步骤的初始条件,然后在新的时间窗内进行预测,优化,即步骤四.这种方法称为滚动时域优化,因此MPC也称作滚动时域控制.
    由此,我们便引出了原因二.通过滚动时域的方法,利用真实系统的反馈不断修正控制量,保障步骤三的操作可以满足最终的控制目标.而每一个时间片的控制过程又符合优化损失函数的预期.两者相互配合,形成闭环.

    值得一提的是,滚动时域的策略使MPC的优化求解都是在一个有限时间的窗口下进行的,因此是无法保障得到整个时域的全局最优.但是因为持续优化,最终也能获得较好的结果. 虽然MPC策略损失了全局最优性,但其获得了更大灵活性.M因此相较于如LQR/LQG等最优控制器,MPC对于复杂非线性约束的对应更加自如,以及模型误差容忍度更高.

    综上,回答第二个问题:通过滚动时域优化的方法,将MPC算法转化为在每一个时间窗下求解一个QP问题.

    预测方程

    我们将离散误差动力学模型改写为以控制量增量 Δ δ \Delta\delta Δδ 的输入的增量式模型.当然了你也可以不用增量式模型,直接用原有模型也是可以的.这并不影响预测方程的形式,只不过我选择了增量式模型作为例子,MPC的模型和损失函数的构造都是很灵活的.回到主题,增量式模型如下:
    x ( k + 1 ) = A d x ( k ) + B d δ ( k ) + B c d ψ ˙ d e s ( k ) = A d x ( k ) + B d [ δ ( k − 1 ) + Δ δ ( k ) ] + B c d ψ ˙ d e s ( k ) \begin{aligned} x(k+1) &= A_dx(k)+B_d\delta(k)+B_{cd}\dot{\psi}_{des}(k) \\ &= A_dx(k)+B_d[\delta(k-1)+\Delta\delta(k)]+B_{cd}\dot{\psi}_{des}(k) \end{aligned} x(k+1)=Adx(k)+Bdδ(k)+Bcdψ˙des(k)=Adx(k)+Bd[δ(k1)+Δδ(k)]+Bcdψ˙des(k)

    进一步的, 将以上模型改写为矩阵形式,可得
    [ x ( k + 1 ) δ ( k ) ] = [ A d B d 0 I ] [ x ( k ) δ ( k − 1 ) ] + [ B d I ] Δ δ ( k ) + [ B c d 0 ] ψ ˙ d e s ( k ) y ( k ) = [ C d 0 ] [ x ( k ) δ ( k − 1 ) ] \begin{aligned} &\begin{bmatrix} x(k+1) \\ \delta(k) \end{bmatrix} = \begin{bmatrix} A_d & B_d\\ 0 & I \end{bmatrix} \begin{bmatrix} x(k) \\ \delta(k-1) \end{bmatrix} + \begin{bmatrix} B_d \\ I \end{bmatrix}\Delta\delta(k)+ \begin{bmatrix} B_{cd} \\ 0 \end{bmatrix}\dot{\psi}_{des}(k)\\ \\ &y(k) = \begin{bmatrix} C_d & 0 \end{bmatrix} \begin{bmatrix} x(k) \\ \delta(k-1) \end{bmatrix} \end{aligned} [x(k+1)δ(k)]=[Ad0BdI][x(k)δ(k1)]+[BdI]Δδ(k)+[Bcd0]ψ˙des(k)y(k)=[Cd0][x(k)δ(k1)]

    定义
    ξ ( k ∣ t ) = [ x ( k ) δ ( k − 1 ) ] A ~ d = [ A d B d 0 I ] B ~ d = [ B d I ] B ~ c d = [ B c d 0 ] C ~ d = [ C d 0 ] \begin{aligned} &\xi(k|t) = \begin{bmatrix} x(k) \\ \delta(k-1) \end{bmatrix} \quad \widetilde{A}_d = \begin{bmatrix} A_d & B_d\\ 0 & I \end{bmatrix} \\ &\widetilde{B}_d = \begin{bmatrix} B_d \\ I \end{bmatrix} \quad \widetilde{B}_{cd} = \begin{bmatrix} B_{cd} \\ 0 \end{bmatrix} \quad \widetilde{C}_d = \begin{bmatrix} C_d & 0 \end{bmatrix} \end{aligned} ξ(kt)=[x(k)δ(k1)]A d=[Ad0BdI]B d=[BdI]B cd=[Bcd0]C d=[Cd0]

    其中, ξ ( k ∣ t ) \xi(k|t) ξ(kt) 代表在 t t t时刻,预测 k k k个周期得到的状态.为了简便, 在后续的使用中 ξ ( k ) \xi(k) ξ(k) ξ ( k ∣ t ) \xi(k|t) ξ(kt) 通用.

    综上,增量式离散误差动力学模型为
    ξ ( k + 1 ) = A ~ d ξ ( k ) + B ~ d Δ δ ( k ) + B ~ c d ψ ˙ d e s ( k ) y ( k ) = C ~ d ξ ( k ) \begin{aligned} &\xi(k+1) = \widetilde{A}_d\xi(k)+\widetilde{B}_d\Delta\delta(k)+\widetilde{B}_{cd}\dot{\psi}_{des}(k)\\ &y(k) = \widetilde{C}_d\xi(k) \end{aligned} ξ(k+1)=A dξ(k)+B dΔδ(k)+B cdψ˙des(k)y(k)=C dξ(k)

    假设预测步数为 k k k,即MPC的预测时域为 k k k个周期,则预测方程有
    ξ ( 1 ) = A ~ d ξ ( 0 ) + B ~ d Δ δ ( 0 ) + B ~ c d ψ ˙ d e s ( 0 ) ξ ( 2 ) = A ~ d ξ ( 1 ) + B ~ d Δ δ ( 1 ) + B ~ c d ψ ˙ d e s ( 1 ) = A ~ d [ A ~ d ξ ( 0 ) + B ~ d Δ δ ( 0 ) + B ~ c d ψ ˙ d e s ( 0 ) ] + B ~ d Δ δ ( 1 ) + B ~ c d ψ ˙ d e s ( 1 ) = A ~ d 2 ξ ( 0 ) + A ~ d B ~ d Δ δ ( 0 ) + B ~ d Δ δ ( 1 ) + A ~ d B ~ c d ψ ˙ d e s ( 0 ) + B ~ c d ψ ˙ d e s ( 1 ) ξ ( 3 ) = A ~ d ξ ( 2 ) + B ~ d Δ δ ( 2 ) + B ~ c d ψ ˙ d e s ( 2 ) = A ~ d [ A ~ d 2 ξ ( 0 ) + A ~ d B ~ d Δ δ ( 0 ) + B ~ d Δ δ ( 1 ) + A ~ d B ~ c d ψ ˙ d e s ( 0 ) + B ~ c d ψ ˙ d e s ( 1 ) ] + B ~ d Δ δ ( 2 ) + B ~ c d ψ ˙ d e s ( 2 ) = A ~ d 3 ξ ( 0 ) + A ~ d 2 B ~ d Δ δ ( 0 ) + A ~ d B ~ d Δ δ ( 1 ) + B ~ d Δ δ ( 2 ) + A ~ d 2 B ~ c d ψ ˙ d e s ( 0 ) + A ~ d B ~ c d ψ ˙ d e s ( 1 ) + B ~ c d ψ ˙ d e s ( 2 )   . . . ξ ( k ) = A ~ d ( k ) ξ ( 0 ) + ∑ i = 0 k − 1 A ~ d ( i ) B ~ d Δ δ ( k − i − 1 ) + ∑ j = 0 k − 1 A ~ d ( j ) B ~ c d ψ ˙ d e s ( k − j − 1 ) y ( k ) = C ~ d ξ ( k ) = C ~ d A ~ d ( k ) ξ ( 0 ) + C ~ d ∑ i = 0 k − 1 A ~ d ( i ) B ~ d Δ δ ( k − i − 1 ) + C ~ d ∑ j = 0 k − 1 A ~ d ( j ) B ~ c d ψ ˙ d e s ( k − j − 1 ) \begin{aligned} &\xi(1) &&=\widetilde{A}_d\xi(0)+\widetilde{B}_d\Delta\delta(0)+\widetilde{B}_{cd}\dot{\psi}_{des}(0) \\ \\ &\xi(2) &&=\widetilde{A}_d\xi(1)+\widetilde{B}_d\Delta\delta(1)+\widetilde{B}_{cd}\dot{\psi}_{des}(1) \\ & &&=\widetilde{A}_d[\widetilde{A}_d\xi(0)+\widetilde{B}_d\Delta\delta(0)+\widetilde{B}_{cd}\dot{\psi}_{des}(0)] +\widetilde{B}_d\Delta\delta(1)+\widetilde{B}_{cd}\dot{\psi}_{des}(1) \\ & &&=\widetilde{A}^2_d\xi(0)+\widetilde{A}_d\widetilde{B}_d\Delta\delta(0)+\widetilde{B}_d\Delta\delta(1)+ \widetilde{A}_d\widetilde{B}_{cd}\dot{\psi}_{des}(0)+\widetilde{B}_{cd}\dot{\psi}_{des}(1) \\ \\ &\xi(3) &&=\widetilde{A}_d\xi(2)+\widetilde{B}_d\Delta\delta(2)+\widetilde{B}_{cd}\dot{\psi}_{des}(2) \\ & &&=\widetilde{A}_d[\widetilde{A}^2_d\xi(0)+\widetilde{A}_d\widetilde{B}_d\Delta\delta(0)+\widetilde{B}_d\Delta\delta(1)+ \widetilde{A}_d\widetilde{B}_{cd}\dot{\psi}_{des}(0)+\widetilde{B}_{cd}\dot{\psi}_{des}(1)]+\widetilde{B}_d\Delta\delta(2)+\widetilde{B}_{cd}\dot{\psi}_{des}(2)\\ & &&=\widetilde{A}^3_d\xi(0)+\widetilde{A}^2_d\widetilde{B}_d\Delta\delta(0)+\widetilde{A}_d\widetilde{B}_d\Delta\delta(1)+\widetilde{B}_d\Delta\delta(2)+ \widetilde{A}^2_d\widetilde{B}_{cd}\dot{\psi}_{des}(0)+\widetilde{A}_d\widetilde{B}_{cd}\dot{\psi}_{des}(1)+\widetilde{B}_{cd}\dot{\psi}_{des}(2) \\ \\ & \ ... \\ \\ &\xi(k) &&= \widetilde{A}^{(k)}_d\xi(0)+\sum_{i=0}^{k-1}\widetilde{A}_d^{(i)}\widetilde{B}_d\Delta\delta(k-i-1)+ \sum_{j=0}^{k-1}\widetilde{A}_d^{(j)}\widetilde{B}_{cd}\dot{\psi}_{des}(k-j-1) \\ &y(k) &&=\widetilde{C}_d\xi(k)=\widetilde{C}_d\widetilde{A}^{(k)}_d\xi(0)+\widetilde{C}_d\sum_{i=0}^{k-1}\widetilde{A}_d^{(i)}\widetilde{B}_d\Delta\delta(k-i-1)+ \widetilde{C}_d\sum_{j=0}^{k-1}\widetilde{A}_d^{(j)}\widetilde{B}_{cd}\dot{\psi}_{des}(k-j-1) \end{aligned} ξ(1)ξ(2)ξ(3) ...ξ(k)y(k)=A dξ(0)+B dΔδ(0)+B cdψ˙des(0)=A dξ(1)+B dΔδ(1)+B cdψ˙des(1)=A d[A dξ(0)+B dΔδ(0)+B cdψ˙des(0)]+B dΔδ(1)+B cdψ˙des(1)=A d2ξ(0)+A dB dΔδ(0)+B dΔδ(1)+A dB cdψ˙des(0)+B cdψ˙des(1)=A dξ(2)+B dΔδ(2)+B cdψ˙des(2)=A d[A d2ξ(0)+A dB dΔδ(0)+B dΔδ(1)+A dB cdψ˙des(0)+B cdψ˙des(1)]+B dΔδ(2)+B cdψ˙des(2)=A d3ξ(0)+A d2B dΔδ(0)+A dB dΔδ(1)+B dΔδ(2)+A d2B cdψ˙des(0)+A dB cdψ˙des(1)+B cdψ˙des(2)=A d(k)ξ(0)+i=0k1A d(i)B dΔδ(ki1)+j=0k1A d(j)B cdψ˙des(kj1)=C dξ(k)=C dA d(k)ξ(0)+C di=0k1A d(i)B dΔδ(ki1)+C dj=0k1A d(j)B cdψ˙des(kj1)

    基于以上的预测方程,我们可以通过 t t t时刻的状态量初值 ξ ( 0 ) \xi(0) ξ(0),建立状态量 ξ \xi ξ k k k 个时域预测周期内与各周期控制增量 Δ δ \Delta\delta Δδ 之间的联系,即
    [ ξ ( t + 1 ∣ t ) ξ ( t + 2 ∣ t ) ξ ( t + 3 ∣ t ) ⋮ ξ ( t + k ∣ t ) ] = [ A ~ d A ~ d 2 A ~ d 3 ⋮ A ~ d k ] ξ ( 0 ) + [ B ~ d 0 0 … 0 A ~ d B ~ d B ~ d 0 … 0 A ~ d 2 B ~ d A ~ d B ~ d B ~ d … 0 ⋮ ⋮ ⋮ ⋱ ⋮ A ~ d k − 1 B ~ d A ~ d k − 2 B ~ d A ~ d k − 3 B ~ d … B ~ d ] [ Δ δ ( 0 ) Δ δ ( 1 ) Δ δ ( 2 ) ⋮ Δ δ ( k − 1 ) ] + [ B ~ c d 0 0 … 0 A ~ d B ~ c d B ~ c d 0 … 0 A ~ d 2 B ~ c d A ~ d B ~ c d B ~ c d … 0 ⋮ ⋮ ⋮ ⋱ ⋮ A ~ d k − 1 B ~ c d A ~ d k − 2 B ~ c d A ~ d k − 3 B ~ c d … B ~ c d ] [ ψ ˙ d e s ( 0 ) ψ ˙ d e s ( 1 ) ψ ˙ d e s ( 2 ) ⋮ ψ ˙ d e s ( k − 1 ) ] \begin{bmatrix} \xi(t+1 | t) \\ \xi(t+2 | t) \\ \xi(t+3 | t) \\ \vdots \\ \xi(t+k | t)\end{bmatrix} = \begin{bmatrix} \widetilde{A}_d \\ \widetilde{A}^2_d \\ \widetilde{A}^3_d \\ \vdots \\ \widetilde{A}^k_d\end{bmatrix}\xi(0)+ \begin{bmatrix} \widetilde{B}_d & 0 & 0 & \dots & 0 \\ \widetilde{A}_d\widetilde{B}_d & \widetilde{B}_d & 0 & \dots & 0 \\ \widetilde{A}_d^2\widetilde{B}_d & \widetilde{A}_d\widetilde{B}_d & \widetilde{B}_d & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{A}_d^{k-1}\widetilde{B}_d & \widetilde{A}_d^{k-2}\widetilde{B}_d &\widetilde{A}_d^{k-3}\widetilde{B}_d & \dots & \widetilde{B}_d \end{bmatrix} \begin{bmatrix} \Delta\delta(0) \\ \Delta\delta(1) \\ \Delta\delta(2) \\ \vdots \\ \Delta\delta(k-1) \end{bmatrix} + \begin{bmatrix} \widetilde{B}_{cd} & 0 & 0 & \dots & 0\\ \widetilde{A}_d\widetilde{B}_{cd} & \widetilde{B}_{cd} & 0 & \dots & 0 \\ \widetilde{A}_d^2\widetilde{B}_{cd} & \widetilde{A}_d\widetilde{B}_{cd} & \widetilde{B}_{cd} & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{A}_d^{k-1}\widetilde{B}_{cd} & \widetilde{A}_d^{k-2}\widetilde{B}_{cd} &\widetilde{A}_d^{k-3}\widetilde{B}_{cd} & \dots & \widetilde{B}_{cd} \end{bmatrix} \begin{bmatrix} \dot{\psi}_{des}(0) \\ \dot{\psi}_{des}(1) \\ \dot{\psi}_{des}(2) \\ \vdots \\ \dot{\psi}_{des}(k-1) \end{bmatrix} ξ(t+1t)ξ(t+2t)ξ(t+3t)ξ(t+kt)=A dA d2A d3A dkξ(0)+B dA dB dA d2B dA dk1B d0B dA dB dA dk2B d00B dA dk3B d000B dΔδ(0)Δδ(1)Δδ(2)Δδ(k1)+B cdA dB cdA d2B cdA dk1B cd0B cdA dB cdA dk2B cd00B cdA dk3B cd000B cdψ˙des(0)ψ˙des(1)ψ˙des(2)ψ˙des(k1)

    定义
    X = [ ξ ( t + 1 ∣ t ) ξ ( t + 2 ∣ t ) ⋮ ξ ( t + k ∣ t ) ] Δ U = [ Δ δ ( 0 ) Δ δ ( 1 ) ⋮ Δ δ ( k − 1 ) ] Λ = [ A ~ d A ~ d 2 ⋮ A ~ d k ] Ψ = [ ψ ˙ d e s ( 0 ) ψ ˙ d e s ( 1 ) ψ ˙ d e s ( 2 ) ⋮ ψ ˙ d e s ( k − 1 ) ] Γ = [ B ~ d 0 0 … 0 A ~ d B ~ d B ~ d 0 … 0 A ~ d 2 B ~ d A ~ d B ~ d B ~ d … 0 ⋮ ⋮ ⋮ ⋱ ⋮ A ~ d k − 1 B ~ d A ~ d k − 2 B ~ d A ~ d k − 3 B ~ d … B ~ d ] Υ = [ B ~ c d 0 0 … 0 A ~ d B ~ c d B ~ c d 0 … 0 A ~ d 2 B ~ c d A ~ d B ~ c d B ~ c d … 0 ⋮ ⋮ ⋮ ⋱ ⋮ A ~ d k − 1 B ~ c d A ~ d k − 2 B ~ c d A ~ d k − 3 B ~ c d … B ~ c d ] \begin{aligned} &X = \begin{bmatrix} \xi(t+1 | t) \\ \xi(t+2 | t) \\ \vdots \\ \xi(t+k | t)\end{bmatrix} \quad \Delta U = \begin{bmatrix} \Delta\delta(0) \\ \Delta\delta(1) \\ \vdots \\ \Delta\delta(k-1) \end{bmatrix} \quad \Lambda = \begin{bmatrix} \widetilde{A}_d \\ \widetilde{A}^2_d \\ \vdots \\ \widetilde{A}^k_d\end{bmatrix} \quad \Psi = \begin{bmatrix} \dot{\psi}_{des}(0) \\ \dot{\psi}_{des}(1) \\ \dot{\psi}_{des}(2) \\ \vdots \\ \dot{\psi}_{des}(k-1) \end{bmatrix}\\ \\ &\Gamma = \begin{bmatrix} \widetilde{B}_d & 0 & 0 & \dots & 0 \\ \widetilde{A}_d\widetilde{B}_d & \widetilde{B}_d & 0 & \dots & 0 \\ \widetilde{A}_d^2\widetilde{B}_d & \widetilde{A}_d\widetilde{B}_d & \widetilde{B}_d & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{A}_d^{k-1}\widetilde{B}_d & \widetilde{A}_d^{k-2}\widetilde{B}_d &\widetilde{A}_d^{k-3}\widetilde{B}_d & \dots & \widetilde{B}_d \end{bmatrix}\\ \\ &\Upsilon = \begin{bmatrix} \widetilde{B}_{cd} & 0 & 0 & \dots & 0\\ \widetilde{A}_d\widetilde{B}_{cd} & \widetilde{B}_{cd} & 0 & \dots & 0 \\ \widetilde{A}_d^2\widetilde{B}_{cd} & \widetilde{A}_d\widetilde{B}_{cd} & \widetilde{B}_{cd} & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{A}_d^{k-1}\widetilde{B}_{cd} & \widetilde{A}_d^{k-2}\widetilde{B}_{cd} &\widetilde{A}_d^{k-3}\widetilde{B}_{cd} & \dots & \widetilde{B}_{cd} \end{bmatrix} \end{aligned} X=ξ(t+1t)ξ(t+2t)ξ(t+kt)ΔU=Δδ(0)Δδ(1)Δδ(k1)Λ=A dA d2A dkΨ=ψ˙des(0)ψ˙des(1)ψ˙des(2)ψ˙des(k1)Γ=B dA dB dA d2B dA dk1B d0B dA dB dA dk2B d00B dA dk3B d000B dΥ=B cdA dB cdA d2B cdA dk1B cd0B cdA dB cdA dk2B cd00B cdA dk3B cd000B cd

    进一步的,定义
    Y = [ y ( t + 1 ∣ t ) y ( t + 2 ∣ t ) ⋮ y ( t + k ∣ t ) ] Ω = [ C ~ d A ~ d C ~ d A ~ d 2 ⋮ C ~ d A ~ d k ] Θ = [ C ~ d B ~ d 0 0 … 0 C ~ d A ~ d B ~ d C ~ d B ~ d 0 … 0 C ~ d A ~ d 2 B ~ d C ~ d A ~ d B ~ d C ~ d B ~ d … 0 ⋮ ⋮ ⋮ ⋱ ⋮ C ~ d A ~ d k − 1 B ~ d C ~ d A ~ d k − 2 B ~ d C ~ d A ~ d k − 3 B ~ d … C ~ d B ~ d ] Φ = [ C ~ d B ~ c d 0 0 … 0 C ~ d A ~ d B ~ c d C ~ d B ~ c d 0 … 0 C ~ d A ~ d 2 B ~ c d C ~ d A ~ d B ~ c d C ~ d B ~ c d … 0 ⋮ ⋮ ⋮ ⋱ ⋮ C ~ d A ~ d k − 1 B ~ c d C ~ d A ~ d k − 2 B ~ c d C ~ d A ~ d k − 3 B ~ c d … C ~ d B ~ c d ] \begin{aligned} &Y = \begin{bmatrix} y(t+1 | t) \\ y(t+2 | t) \\ \vdots \\ y(t+k | t)\end{bmatrix} \quad \Omega = \begin{bmatrix} \widetilde{C}_d\widetilde{A}_d \\ \widetilde{C}_d\widetilde{A}^2_d \\ \vdots \\ \widetilde{C}_d\widetilde{A}^k_d\end{bmatrix} \\ \\ &\Theta = \begin{bmatrix} \widetilde{C}_d\widetilde{B}_d & 0 & 0 & \dots & 0 \\ \widetilde{C}_d\widetilde{A}_d\widetilde{B}_d & \widetilde{C}_d\widetilde{B}_d & 0 & \dots & 0 \\ \widetilde{C}_d\widetilde{A}_d^2\widetilde{B}_d & \widetilde{C}_d\widetilde{A}_d\widetilde{B}_d & \widetilde{C}_d\widetilde{B}_d & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{C}_d\widetilde{A}_d^{k-1}\widetilde{B}_d & \widetilde{C}_d\widetilde{A}_d^{k-2}\widetilde{B}_d &\widetilde{C}_d\widetilde{A}_d^{k-3}\widetilde{B}_d & \dots & \widetilde{C}_d\widetilde{B}_d \end{bmatrix}\\ \\ &\Phi = \begin{bmatrix} \widetilde{C}_d\widetilde{B}_{cd} & 0 & 0 & \dots & 0\\ \widetilde{C}_d\widetilde{A}_d\widetilde{B}_{cd} & \widetilde{C}_d\widetilde{B}_{cd} & 0 & \dots & 0 \\ \widetilde{C}_d\widetilde{A}_d^2\widetilde{B}_{cd} & \widetilde{C}_d\widetilde{A}_d\widetilde{B}_{cd} & \widetilde{C}_d\widetilde{B}_{cd} & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{C}_d\widetilde{A}_d^{k-1}\widetilde{B}_{cd} & \widetilde{C}_d\widetilde{A}_d^{k-2}\widetilde{B}_{cd} &\widetilde{C}_d\widetilde{A}_d^{k-3}\widetilde{B}_{cd} & \dots & \widetilde{C}_d\widetilde{B}_{cd} \end{bmatrix} \end{aligned} Y=y(t+1t)y(t+2t)y(t+kt)Ω=C dA dC dA d2C dA dkΘ=C dB dC dA dB dC dA d2B dC dA dk1B d0C dB dC dA dB dC dA dk2B d00C dB dC dA dk3B d000C dB dΦ=C dB cdC dA dB cdC dA d2B cdC dA dk1B cd0C dB cdC dA dB cdC dA dk2B cd00C