【运动控制】连续时间状态方程的离散化
连续时间状态方程的离散化-20220719修改
-
- 1 连续时间状态方程
-
- 1.1 时域
- 1.2 频域
- 1.3 z域
- 2 离散化
-
- 2.1 前向差分
- 2.2 后向差分
-
- 2.2.1 使用 u ( k + 1 ) u(k+1) u(k+1)
- 2.2.2 使用 u ( k ) u(k) u(k)
- 2.3 双线性变换
-
- 2.3.1 使用 u ( k + 1 ) u(k+1) u(k+1)
- 2.3.2 使用 u ( k ) u(k) u(k)
- 3 结语
- 附录
- 参考资料
在众多的控制算法中，为了实际应用，不可避免的过程就是将连续时间状态方程进行离散化，以便于代码的实现。
但是，在网上的众多博客中，鲜有对此的介绍，更无具体推导过程。导致很难理解为什么这种形式就是离散化，云里雾里，感觉非常不踏实。
故作此文章，为所需之人作些许帮助。
本文于2022年7月19日重新推导，修改了之前文章存在的错误。感谢 @那行界指出文章中的错误，同时给出了可供参考的推导资料。
1 连续时间状态方程
1.1 时域
假设输入向量 u ( t ) u(t) u(t)只在等间隔采样时刻发生改变，连续时间时间状态方程和输出方程如下
x ˙ ( t ) = A x ( t ) + B u ( t ) (1-1-1) \begin{aligned} \dot{x}(t) &= Ax(t) + Bu(t) \\ \end{aligned} \tag{1-1-1} x˙(t)=Ax(t)+Bu(t)(1-1-1)
y ( t ) = C x ( t ) + D u ( t ) (1-1-2) \begin{aligned} y(t) &= Cx(t) + Du(t) \\ \end{aligned} \tag{1-1-2} y(t)=Cx(t)+Du(t)(1-1-2)
1.2 频域
对(1-1)和(1-2)进行拉氏变换，得到
s X ( s ) = A X ( s ) + B U ( s ) (1-2-1) \begin{aligned} sX(s) = AX(s) + BU(s) \end{aligned} \tag{1-2-1} sX(s)=AX(s)+BU(s)(1-2-1)
Y ( s ) = C X ( s ) + D U ( s ) (1-2-2) \begin{aligned} Y(s) = CX(s) + DU(s) \end{aligned} \tag{1-2-2} Y(s)=CX(s)+DU(s)(1-2-2)
将(1-3)代入(1-4)，消去 X ( s ) X(s) X(s)，得到
Y ( s ) = [ C ( s I − A ) − 1 B + D ] U ( s ) (1-2-3) \begin{aligned} Y(s) = \left [ C(sI - A)^{-1}B + D \right ] U(s) \end{aligned} \tag{1-2-3} Y(s)=[C(sI−A)−1B+D]U(s)(1-2-3)
定义传递函数 G ( s ) G(s) G(s)为系统输出与输入的比值，如下
G ( s ) = Y ( s ) U ( s ) (1-2-4) \begin{aligned} G(s) = \frac{Y(s)}{U(s)} \end{aligned} \tag{1-2-4} G(s)=U(s)Y(s)(1-2-4)
因此，可以得到
G ( s ) = C ( s I − A ) − 1 B + D (1-2-5) \begin{aligned} G(s) = C(sI - A)^{-1}B + D \end{aligned} \tag{1-2-5} G(s)=C(sI−A)−1B+D(1-2-5)
1.3 z域
同理，z域的系统如下
z X ( z ) = A d X ( z ) + B d U ( z ) (1-3-1) \begin{aligned} zX(z) = A_{d}X(z) + B_{d}U(z) \end{aligned} \tag{1-3-1} zX(z)=AdX(z)+BdU(z)(1-3-1)
Y ( z ) = C d X ( z ) + D d U ( z ) (1-3-2) \begin{aligned} Y(z) = C_{d}X(z) + D_{d}U(z) \end{aligned} \tag{1-3-2} Y(z)=CdX(z)+DdU(z)(1-3-2)
其中， A d 、 B d 、 C d 、 D d A_{d}、B_{d}、C_{d}、D_{d} Ad、Bd、Cd、Dd为系统的离散系数矩阵。
同样定义定义传递函数 G d ( z ) G_{d}(z) Gd(z)为系统输出与输入的比值，如下
G d ( z ) = Y ( z ) U ( z ) (1-3-3) \begin{aligned} G_{d}(z) = \frac{Y(z)}{U(z)} \end{aligned} \tag{1-3-3} Gd(z)=U(z)Y(z)(1-3-3)
因此，可以得到
G d ( z ) = C d ( z I − A d ) − 1 B d + D d (1-3-4) \begin{aligned} G_{d}(z) = C_{d}(zI - A_{d})^{-1}B_{d} + D_{d} \end{aligned} \tag{1-3-4} Gd(z)=Cd(zI−Ad)−1Bd+Dd(1-3-4)
2 离散化
离散化常用的方法包括前向差分、后向差分和双线性变换，下面一一进行推导。
2.1 前向差分
前向差分算子为
s = z − 1 T (2-1-1) s = \frac{z-1}{T} \\ \tag{2-1-1} s=Tz−1(2-1-1)
将(2-1-1)代入(1-3)中，得到
z − 1 T X ( z ) = A X ( z ) + B U ( z ) ( z − 1 ) X ( z ) = A T X ( z ) + B T U ( z ) z X ( z ) = ( I + A T ) X ( z ) + B T U ( z ) (2-1-2) \begin{aligned} \frac{z-1}{T}X(z) &= AX(z) + BU(z) \\ (z-1)X(z) &= ATX(z) + BTU(z) \\ zX(z) &= (I + AT)X(z) + BTU(z) \\ \end{aligned} \tag{2-1-2} Tz−1X(z)(z−1)X(z)zX(z)=AX(z)+BU(z)=ATX(z)+BTU(z)=(I+AT)X(z)+BTU(z)(2-1-2)
将(2-1-2)转换为差分方程，如下
x ( k + 1 ) = ( I + A T ) x ( k ) + B T u ( k ) (2-1-3) \begin{aligned} x(k+1) &= (I+AT)x(k) + BTu(k) \\ \end{aligned} \tag{2-1-3} x(k+1)=(I+AT)x(k)+BTu(k)(2-1-3)
根据(2-1-3)的形式继续进行推导，将(2-1-1)代入(1-2-5)，有
G ( s ) = C ( z − 1 T I − A ) − 1 B + D G ( s ) = C ( z I − I − A T ) − 1 B T + D G ( s ) = C [ z I − ( I + A T ) ] − 1 ( B T ) + D G d ( z ) = C d ( z I − A d ) − 1 B d + D d (2-1-4) \begin{aligned} G(s) &= C(\frac{z-1}{T}I - A)^{-1}B + D \\ G(s) &= C(zI - I-AT)^{-1}BT + D \\ G(s) &= C[zI - (I+AT)]^{-1}(BT) + D \\ G_{d}(z) &= C_{d}(zI - A_{d})^{-1}B_{d} + D_{d} \\ \end{aligned} \tag{2-1-4} G(s)G(s)G(s)Gd(z)=C(Tz−1I−A)−1B+D=C(zI−I−AT)−1BT+D=C[zI−(I+AT)]−1(BT)+D=Cd(zI−Ad)−1Bd+Dd(2-1-4)
从而，得到
A d = I + A T B d = B T C d = C D d = D (2-1-4) \begin{aligned} A_{d} &= I+AT \\ B_{d} &= BT \\ C_{d} &= C \\ D_{d} &= D \\ \end{aligned} \tag{2-1-4} AdBdCdDd=I+AT=BT=C=D(2-1-4)
因此，使用前向差分获得的差分方程为
x ( k + 1 ) = ( I + A T ) x ( k ) + B T u ( k ) y ( k ) = C x ( k ) + D u ( k ) (2-1-5) \begin{aligned} x(k+1) &= (I+AT)x(k) + BTu(k) \\ y(k) &= Cx(k) + Du(k) \\ \end{aligned} \tag{2-1-5} x(k+1)y(k)=(I+AT)x(k)+BTu(k)=Cx(k)+Du(k)(2-1-5)
2.2 后向差分
后向差分算子为
s = 1 − z − 1 T = z − 1 T z (2-2-1) \begin{aligned} s = \frac{1-z^{-1}}{T} = \frac{z-1}{Tz} \end{aligned} \tag{2-2-1} s=T1−z−1=Tzz−1(2-2-1)
将(2-2-1)代入(1-3)中，得到
z − 1 T z X ( z ) = A X ( z ) + B U ( z ) ( z − 1 ) X ( z ) = A T z X ( z ) + B T z U ( z ) z ( I − A T ) X ( z ) = X ( z ) + B T z U ( z ) z X ( z ) = ( I − A T ) − 1 X ( z ) + ( I − A T ) − 1 B T z U ( z ) (2-2-2) \begin{aligned} \frac{z-1}{Tz} X(z) &= AX(z) + BU(z) \\ (z-1)X(z) &= ATzX(z) + BTzU(z) \\ z(I-AT)X(z) &= X(z) + BTzU(z) \\ zX(z) &= (I-AT)^{-1}X(z) + (I-AT)^{-1}BTzU(z) \\ \end{aligned} \tag{2-2-2} Tzz−1X(z)(z−1)X(z)z(I−AT)X(z)zX(z)=AX(z)+BU(z)=ATzX(z)+BTzU(z)=X(z)+BTzU(z)=(I−AT)−1X(z)+(I−AT)−1BTzU(z)(2-2-2)
将(2-2-2)转换为差分方程，如下
x ( k + 1 ) = ( I − A T ) − 1 x ( k ) + ( I − A T ) − 1 B T u ( k + 1 ) (2-2-3) \begin{aligned} x(k+1) &= (I-AT)^{-1}x(k) + (I-AT)^{-1}BTu(k+1) \\ \end{aligned} \tag{2-2-3} x(k+1)=(I−AT)−1x(k)+(I−AT)−1BTu(k+1)(2-2-3)
根据(2-2-3)的形式继续进行推导，将(2-2-1)代入(1-2-5)，有
G ( s ) = C ( z − 1 T z I − A ) − 1 B + D G ( s ) = C ( z I − I − A T z ) − 1 B T z + D G ( s ) = C [ z ( I − A T ) − I ] − 1 ( B T z ) + D G ( s ) = C [ z I − ( I − A T ) − 1 ] − 1 ( I − A T ) − 1 ( B T z ) + D G ( s ) = C z [ z I − ( I − A T ) − 1 ] − 1 ( I − A T ) − 1 ( B T ) + D G d ( z ) = C d ( z I − A d ) − 1 B d + D d (2-2-4) \begin{aligned} G(s) &= C(\frac{z-1}{Tz}I - A)^{-1}B + D \\ G(s) &= C(zI - I - ATz)^{-1}BTz + D \\ G(s) &= C[z(I-AT) - I]^{-1}(BTz) + D \\ G(s) &= C[zI - (I-AT)^{-1}]^{-1}(I-AT)^{-1} (BTz) + D \\ G(s) &= Cz[zI - (I-AT)^{-1}]^{-1}(I-AT)^{-1} (BT) + D \\ G_{d}(z) &= C_{d}(zI - A_{d})^{-1}B_{d} + D_{d} \\ \end{aligned} \tag{2-2-4} G(s)G(s)G(s)G(s)G(s)Gd(z)=C(Tzz−1I−A)−1B+D=C(zI−I−ATz)−1BTz+D=C[z(I−AT)−I]−1(BTz)+D=C[zI−(I−AT)−1]−1(I−AT)−1(BTz)+D=Cz[zI−(I−AT)−1]−1(I−AT)−1(BT)+D=Cd(zI−Ad)−1Bd+Dd(2-2-4)
注：式中， z z z为数乘，可以交换位置。
式(2-2-4)需要分两种情况讨论，如下所述。
2.2.1 使用 u ( k + 1 ) u(k+1) u(k+1)
如果能够获取 k + 1 k+1 k+1时刻的输入 u ( k + 1 ) u(k+1) u(k+1)（使用模型估计或预测输入变化，但实际很难应用），则(2-2-4)使用的式子为
G ( s ) = C [ z I − ( I − A T ) − 1 ] − 1 ( I − A T ) − 1 ( B T z ) + D G d ( z ) = C d ( z I − A d ) − 1 B d + D d (2-2-1-1) \begin{aligned} G(s) &= C[zI - (I-AT)^{-1}]^{-1}(I-AT)^{-1} (BTz) + D \\ G_{d}(z) &= C_{d}(zI - A_{d})^{-1}B_{d} + D_{d} \\ \end{aligned} \tag{2-2-1-1} G(s)Gd(z)=C[zI−(I−AT)−1]−1(I−AT)−1(BTz)+D=Cd(zI−Ad)−1Bd+Dd(2-2-1-1)
则离散系数矩阵为
A d = ( I − A T ) − 1 B d = ( I − A T ) − 1 B T z C d = C D d = D (2-2-1-2) \begin{aligned} A_{d} &= (I-AT)^{-1} \\ B_{d} &= (I-AT)^{-1}BTz \\ C_{d} &= C \\ D_{d} &= D \\ \end{aligned} \tag{2-2-1-2} AdBdCdDd=(I−AT)−1=(I−AT)−1BTz=C=D(2-2-1-2)
则差分方程为
x ( k + 1 ) = ( I − A T ) − 1 x ( k ) + ( I − A T ) − 1 B T u ( k + 1 ) y ( k ) = C x ( k ) + D u ( k ) (2-2-1-3) \begin{aligned} x(k+1) &= (I-AT)^{-1}x(k) + (I-AT)^{-1}BTu(k+1) \\ y(k) &= Cx(k) + Du(k) \\ \end{aligned} \tag{2-2-1-3} x(k+1)y(k)=(I−AT)−1x(k)+(I−AT)−1BTu(k+1)=Cx(k)+Du(k)(2-2-1-3)
2.2.2 使用 u ( k ) u(k) u(k)
如果使用 u ( k ) u(k) u(k)，保持传递函数不变，则(2-2-4)使用的式子为
G ( s ) = C z [ z I − ( I − A T ) − 1 ] − 1 ( I − A T ) − 1 ( B T ) + D G d ( z ) = C d ( z I − A d ) − 1 B d + D d (2-2-2-1) \begin{aligned} G(s) &= Cz[zI - (I-AT)^{-1}]^{-1}(I-AT)^{-1} (BT) + D \\ G_{d}(z) &= C_{d}(zI - A_{d})^{-1}B_{d} + D_{d} \\ \end{aligned} \tag{2-2-2-1} G(s)Gd(z)=Cz[zI−(I−AT)−1]−1(I−AT)−1(BT)+D=Cd(zI−Ad)−1Bd+Dd(2-2-2-1)
由(2-2-2-1)，得到
A d = ( I − A T ) − 1 B d = ( I − A T ) − 1 B T C d = C z D d = D (2-2-2-2) \begin{aligned} A_{d} &= (I-AT)^{-1} \\ B_{d} &= (I-AT)^{-1}BT \\ C_{d} &= Cz \\ D_{d} &= D \\ \end{aligned} \tag{2-2-2-2} AdBdCdDd=(I−AT)−1=(I−AT)−1BT=Cz=D(2-2-2-2)
注意到(2-2-2-2)的 C d C_{d} Cd一项含有 z z z，不方便计算，需要推导其具体表达式，则新的系统输出方程为
Y ( z ) = C d X ( z ) + D d U ( z ) Y ( z ) = C z X ( z ) + D U ( z ) Y ( z ) = C [ ( I − A T ) − 1 X ( z ) + ( I − A T ) − 1 B T ] + D U ( z ) Y ( z ) = C ( I − A T ) − 1 X ( z ) + [ D + C ( I − A T ) − 1 B T ] U ( z ) (2-2-2-3) \begin{aligned} Y(z) &= C_{d}X(z) + D_{d}U(z) \\ Y(z) &= CzX(z) + DU(z) \\ Y(z) &= C[(I-AT)^{-1}X(z) + (I-AT)^{-1}BT] + DU(z) \\ Y(z) &= C(I-AT)^{-1}X(z) + [D + C(I-AT)^{-1}BT]U(z) \\ \end{aligned} \tag{2-2-2-3} Y(z)Y(z)Y(z)Y(z)=CdX(z)+DdU(z)=CzX(z)+DU(z)=C[(I−AT)−1X(z)+(I−AT)−1BT]+DU(z)=C(I−AT)−1X(z)+[D+C(I−AT)−1BT]U(z)(2-2-2-3)
由(2-2-2-3)，得到
A d = ( I − A T ) − 1 B d = ( I − A T ) − 1 B T C d = C ( I − A T ) − 1 D d = D + C ( I − A T ) − 1 B T (2-2-2-4) \begin{aligned} A_{d} &= (I-AT)^{-1} \\ B_{d} &= (I-AT)^{-1}BT \\ C_{d} &= C(I-AT)^{-1} \\ D_{d} &= D + C(I-AT)^{-1}BT \\ \end{aligned} \tag{2-2-2-4} AdBdCdDd=(I−AT)−1=(I−AT)−1BT=C(I−AT)−1=D+C(I−AT)−1BT(2-2-2-4)
则差分方程为
x ( k + 1 ) = ( I − A T ) − 1 x ( k ) + ( I − A T ) − 1 B T u ( k ) y ( k ) = C ( I − A T ) − 1 x ( k ) + [ D + C ( I − A T ) − 1 B T ] u ( k ) (2-2-2-5) \begin{aligned} x(k+1) &= (I-AT)^{-1}x(k) + (I-AT)^{-1}BTu(k) \\ y(k) &= C(I-AT)^{-1}x(k) + [D+C(I-AT)^{-1}BT]u(k) \\ \end{aligned} \tag{2-2-2-5} x(k+1)y(k)=(I−AT)−1x(k)+(I−AT)−1BTu(k)=C(I−AT)−1x(k)+[D+C(I−AT)−1BT]u(k)(2-2-2-5)
这一种推导，用u(k)代替了u(k+1)，相当于修改了B矩阵，即将z转移到了C矩阵，但是没有改变传递函数 G d G_{d} Gd，不会影响系统的输入输出传递特性。
2.3 双线性变换
双线性变换算子为
s = 2 T z − 1 z + 1 (2-3-1) \begin{aligned} s = \frac{2}{T} \frac{z-1}{z+1} \\ \end{aligned} \tag{2-3-1} s=T2z+1z−1(2-3-1)
将(2-3-1)代入(1-3)中，得到
2 T z − 1 z + 1 X ( z ) = A X ( z ) + B U ( z ) ( z − 1 ) X ( z ) = T 2 A ( z + 1 ) X ( z ) + T 2 B ( z + 1 ) U ( z ) z ( I − 1 2 A T ) X ( z ) = ( I + 1 2 A T ) X ( z ) + 1 2 B T ( z + 1 ) U ( z ) z X ( z ) = ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) X ( z ) + ( I − 1 2 A T ) − 1 [ 1 2 B T ( z + 1 ) ] U ( z ) (2-3-2) \begin{aligned} \frac{2}{T} \frac{z-1}{z+1} X(z) &= AX(z) + BU(z) \\ (z-1) X(z) &= \frac{T}{2} A (z+1) X(z) + \frac{T}{2} B (z+1) U(z) \\ z(I-\frac{1}{2} AT) X(z) &= (I+\frac{1}{2} AT) X(z) + \frac{1}{2} BT (z+1) U(z) \\ zX(z) &= (I-\frac{1}{2} AT)^{-1}(I+\frac{1}{2} AT)X(z) + (I-\frac{1}{2} AT)^{-1} [\frac{1}{2} BT (z+1)] U(z) \end{aligned} \tag{2-3-2} T2z+1z−1X(z)(z−1)X(z)z(I−21AT)X(z)zX(z)=AX(z)+BU(z)=2TA(z+1)X(z)+2TB(z+1)U(z)=(I+21AT)X(z)+21BT(z+1)U(z)=(I−21AT)−1(I+21AT)X(z)+(I−21AT)−1[21BT(z+1)]U(z)(2-3-2)
将(2-3-2)转换为差分方程，如下
x ( k + 1 ) = ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) x ( k ) + ( I − 1 2 A T ) − 1 1 2 B T [ u ( k + 1 ) + u ( k ) ] (2-3-3) \begin{aligned} x(k+1) = (I-\frac{1}{2}AT)^{-1}(I+\frac{1}{2}AT)x(k) + (I-\frac{1}{2}AT)^{-1} \frac{1}{2} BT [u(k+1)+u(k)] \\ \end{aligned} \tag{2-3-3} x(k+1)=(I−21AT)−1(I+21AT)x(k)+(I−21AT)−121BT[u(k+1)+u(k)](2-3-3)
根据(2-3-3)的形式继续进行推导，将(2-3-1)代入(1-2-5)，有
G ( s ) = C ( 2 T z − 1 z + 1 I − A ) − 1 B + D G ( s ) = C [ z I − I − 1 2 A T ( z + 1 ) ] − 1 1 2 B T ( z + 1 ) + D G ( s ) = C [ z ( I − 1 2 A T ) − ( I + 1 2 A T ) ] − 1 1 2 B T ( z + 1 ) + D G ( s ) = C [ z I − ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) ] − 1 ( I − 1 2 A T ) − 1 1 2 B T ( z + 1 ) + D G ( s ) = C ( z + 1 ) [ z I − ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) ] − 1 ( I − 1 2 A T ) − 1 1 2 B T + D G d ( z ) = C d ( z I − A d ) − 1 B d + D d (2-3-4) \begin{aligned} G(s) &= C(\frac{2}{T} \frac{z-1}{z+1}I - A)^{-1}B + D \\ G(s) &= C[ zI - I - \frac{1}{2}AT(z+1)]^{-1} \frac{1}{2}BT(z+1) + D \\ G(s) &= C[ z(I-\frac{1}{2}AT) - (I + \frac{1}{2}AT)]^{-1} \frac{1}{2}BT(z+1) + D \\ G(s) &= C[ zI - (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)]^{-1} (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT(z+1) + D \\ G(s) &= C(z+1)[ zI - (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)]^{-1} (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT + D \\ G_{d}(z) &= C_{d}(zI - A_{d})^{-1}B_{d} + D_{d} \\ \end{aligned} \tag{2-3-4} G(s)G(s)G(s)G(s)G(s)Gd(z)=C(T2z+1z−1I−A)−1B+D=C[zI−I−21AT(z+1)]−121BT(z+1)+D=C[z(I−21AT)−(I+21AT)]−121BT(z+1)+D=C[zI−(I−21AT)−1(I+21AT)]−1(I−21AT)−121BT(z+1)+D=C(z+1)[zI−(I−21AT)−1(I+21AT)]−1(I−21AT)−121BT+D=Cd(zI−Ad)−1Bd+Dd(2-3-4)
式(2-2-4)需要分两种情况讨论，如下所述。
2.3.1 使用 u ( k + 1 ) u(k+1) u(k+1)
如果使用 u ( k + 1 ) u(k+1) u(k+1)（使用模型估计或预测输入变化，但实际很难应用），则(2-3-4)使用的式子为
G ( s ) = C [ z I − ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) ] − 1 ( I − 1 2 A T ) − 1 1 2 B T ( z + 1 ) + D G d ( z ) = C d ( z I − A d ) − 1 B d + D d (2-3-1-1) \begin{aligned} G(s) &= C[ zI - (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)]^{-1} (I-\frac{1}{2}AT)^{-1} \frac{1}{2}BT(z+1) + D \\ G_{d}(z) &= C_{d}(zI - A_{d})^{-1}B_{d} + D_{d} \\ \end{aligned} \tag{2-3-1-1} G(s)Gd(z)=C[zI−(I−21AT)−1(I+21AT)]−1(I−21AT)−121BT(z+1)+D=Cd(zI−Ad)−1Bd+Dd(2-3-1-1)
由(2-3-1-1)，得到
A d = ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) B d = ( I − 1 2 A T ) − 1 1 2 B T ( z + 1 ) C d = C D d = D (2-3-1-2) \begin{aligned} A_{d} &= (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT) \\ B_{d} &= (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT(z+1) \\ C_{d} &= C \\ D_{d} &= D \\ \end{aligned} \tag{2-3-1-2} AdBdCdDd=(I−21AT)−1(I+21AT)=(I−21AT)−121BT(z+1)=C=D(2-3-1-2)
则差分方程为
x ( k + 1 ) = ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) x ( k ) + ( I − 1 2 A T ) − 1 1 2 B T [ u ( k + 1 ) + u ( k ) ] y ( k ) = C x ( k ) + D u ( k ) (2-3-1-3) \begin{aligned} x(k+1) &= (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)x(k) + (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT[u(k+1) + u(k)] \\ y(k) &= Cx(k) + Du(k) \\ \end{aligned} \tag{2-3-1-3} x(k+1)y(k)=(I−21AT)−1(I+21AT)x(k)+(I−21AT)−121BT[u(k+1)+u(k)]=Cx(k)+Du(k)(2-3-1-3)
2.3.2 使用 u ( k ) u(k) u(k)
如果使用 u ( k ) u(k) u(k)，保持传递函数不变，则(2-3-4)使用的式子为
G ( s ) = C ( z + 1 ) [ z I − ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) ] − 1 ( I − 1 2 A T ) − 1 1 2 B T + D G d ( z ) = C d ( z I − A d ) − 1 B d + D d (2-3-2-1) \begin{aligned} G(s) &= C(z+1)[ zI - (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)]^{-1} (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT + D \\ G_{d}(z) &= C_{d}(zI - A_{d})^{-1}B_{d} + D_{d} \\ \end{aligned} \tag{2-3-2-1} G(s)Gd(z)=C(z+1)[zI−(I−21AT)−1(I+21AT)]−1(I−21AT)−121BT+D=Cd(zI−Ad)−1Bd+Dd(2-3-2-1)
由(2-3-2-1)，得到
A d = ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) B d = ( I − 1 2 A T ) − 1 1 2 B T C d = C ( z + 1 ) D d = D (2-3-2-2) \begin{aligned} A_{d} &= (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT) \\ B_{d} &= (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT \\ C_{d} &= C(z+1) \\ D_{d} &= D \\ \end{aligned} \tag{2-3-2-2} AdBdCdDd=(I−21AT)−1(I+21AT)=(I−21AT)−121BT=C(z+1)=D(2-3-2-2)
注意到(2-3-2-2)的 C d C_{d} Cd一项含有 z z z，不方便计算，需要推导其具体表达式，则新的系统输出方程为
Y ( z ) = C d X ( z ) + D d U ( z ) Y ( z ) = C ( z + 1 ) X ( z ) + D U ( z ) Y ( z ) = C z X ( z ) + C X ( z ) + D U ( z ) Y ( z ) = C [ ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) X ( z ) + ( I − 1 2 A T ) − 1 1 2 B T U ( z ) ] + C X ( z ) + D U ( z ) Y ( z ) = C [ I + ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) ] X ( z ) + [ D + C ( I − 1 2 A T ) − 1 1 2 B T ] U ( z ) (2-3-2-3) \begin{aligned} Y(z) &= C_{d}X(z) + D_{d}U(z) \\ Y(z) &= C(z+1)X(z) + DU(z) \\ Y(z) &= CzX(z) + CX(z) + DU(z) \\ Y(z) &= C[(I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)X(z) + (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT U(z)] + CX(z) + DU(z) \\ Y(z) &= C[I + (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)]X(z) + [D + C(I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT]U(z) \\ \end{aligned} \tag{2-3-2-3} Y(z)Y(z)Y(z)Y(z)Y(z)=CdX(z)+DdU(z)=C(z+1)X(z)+DU(z)=CzX(z)+CX(z)+DU(z)=C[(I−21AT)−1(I+21AT)X(z)+(I−21AT)−121BTU(z)]+CX(z)+DU(z)=C[I+(I−21AT)−1(I+21AT)]X(z)+[D+C(I−21AT)−121BT]U(z)(2-3-2-3)
由(2-3-2-3)，得到
A d = ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) B d = ( I − 1 2 A T ) − 1 1 2 B T C d = C [ I + ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) ] D d = D + C ( I − 1 2 A T ) − 1 1 2 B T (2-3-2-4) \begin{aligned} A_{d} &= (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT) \\ B_{d} &= (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT \\ C_{d} &= C[I + (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)] \\ D_{d} &= D + C(I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT \\ \end{aligned} \tag{2-3-2-4} AdBdCdDd=(I−21AT)−1(I+21AT)=(I−21AT)−121BT=C[I+(I−21AT)−1(I+21AT)]=D+C(I−21AT)−121BT(2-3-2-4)
则差分方程为
x ( k + 1 ) = ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) x ( k ) + ( I − 1 2 A T ) − 1 1 2 B T u ( k ) y ( k ) = C [ I + ( I − 1 2 A T ) − 1 ( I + 1 2 A T ) ] x ( k ) + [ D + C ( I − 1 2 A T ) − 1 1 2 B T ] u ( k ) (2-3-2-5) \begin{aligned} x(k+1) &= (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)x(k) + (I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT u(k) \\ y(k) &= C[I + (I-\frac{1}{2}AT)^{-1}(I + \frac{1}{2}AT)]x(k) + [D + C(I-\frac{1}{2}AT)^{-1}\frac{1}{2}BT]u(k) \\ \end{aligned} \tag{2-3-2-5} x(k+1)y(k)=(I−21AT)−1(I+21AT)x(k)+(I−21AT)−121BTu(k)=C[I+(I−21AT)−1(I+21AT)]x(k)+[D+C(I−21AT)−121BT]u(k)(2-3-2-5)
3 结语
在Apollo的横向控制模块中，LQR的 B B B矩阵离散变换系数应该是不对的，但MRAC的 A A A和 B B B矩阵离散变换系数又是正确的，这着实让人费解。
如果Apollo的每个模块能给出设计所参考的论文，那就不会存在这么多疑问了。
附录
某个广泛流传的连续时间状态方程的离散化公式如下，这篇文章基本已经与其相同（除了双线性变换的 C d C_{d} Cd矩阵），有需要的可以自行保存。
参考资料
- https://dsp.stackexchange.com/questions/45042/bilinear-transformation-of-continuous-time-state-space-system;
- http://www.adjutojunior.com.br/controle_processos/slides_chapter8_DISCRETIZATION_OF_CONTINUOS_SYSTEMS.pdf.