连续时间状态方程的离散化-20220719修改
在众多的控制算法中，为了实际应用，不可避免的过程就是将连续时间状态方程进行离散化，以便于代码的实现。
但是，在网上的众多博客中，鲜有对此的介绍，更无具体推导过程。导致很难理解为什么这种形式就是离散化，云里雾里，感觉非常不踏实。
故作此文章，为所需之人作些许帮助。
本文于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矩阵），有需要的可以自行保存。
3652
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
