写在前面
在使用机器人进行导航时，一个常见的场景是先获得包含位置、速度、朝向等信息的轨迹，然后让机器人跟随这条轨迹。在跟随过程中，我们希望机器人能顺滑且精准地跟随轨迹。为此，通常会引入路径跟踪控制器，如PID、LQR、MPC等。
接下来，以阿克曼的线性离散单车模型为例，我将推导如何使用MPC进行路径跟踪。
构建状态预测方程(State Prediction Equation)与输出预测方程(Output Prediction Equation)
写出模型的离散状态空间方程
首先从单车模型的线性化与离散化推导详解 已经得到了用于跟踪路径的的最终状态方程有：
\begin{aligned}\tilde{\chi}(k+1) & =(TA+I)\tilde{\chi}(k)+TB\tilde{\mathbf{u}}(k)\\ & =\begin{bmatrix}1 & 0 & -Tv_{r}\sin\varphi_{r}\\ 0 & 1 & Tv_{r}\cos\varphi_{r}\\ 0 & 0 & 1\end{bmatrix}\tilde{\chi}(k)+\begin{bmatrix}T\cos\varphi_{r} & 0\\ T\sin\varphi_{r} & 0\\ \frac{T\tan\varphi_{r}}{L} & \frac{v_rT}{L\cos^2\delta_r}\end{bmatrix}\tilde{\mathbf{u}}(k)\\ & =\tilde{A}\tilde{\chi}(k)+\tilde{B}\tilde{\mathbf{u}}(k)\end{aligned}
其中：
\tilde{\chi}(k) 为当前状态与参考点状态之间状态量误差\begin{bmatrix}x-x_{r}\\ y-y_{r}\\ \varphi-\varphi_{r}\end{bmatrix} \tilde{\mathbf{u}}(k) 为当前状态与参考点状态之间的控制量误差\begin{bmatrix}v-v_{r}\\ \delta-\delta_{r}\end{bmatrix} T 为采样步长
令a=\tilde{A} ，b=\tilde{B} ,有：
\tilde{\chi}(k+1) = a\tilde{\chi}(k) + b\tilde{\mathbf{u}}(k)
上式就是我们的状态转移方程 ，然后同样的，我们也可以写出输出方程 为：
\begin{align}
y(k) &= \mathbf{c}\tilde{\chi}(k) + \mathbf{d}\tilde{\mathbf{u}}(k) \\
&= \begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix} \tilde{\chi}(k) +
\begin{bmatrix}
0 & 0 & 0 \\
0 & 0 & 0 \\
0 & 0 & 0
\end{bmatrix} \tilde{\mathbf{u}}(k) = \mathbf{c}\tilde{\chi}(k)
\end{align}
其中,
\mathbf{c} 矩阵定义了需要输出哪些状态量，这里使用一个单位矩阵，说明了所有的三个状态\begin{bmatrix}x-x_{r}\\ y-y_{r}\\ \varphi-\varphi_{r}\end{bmatrix} 都要输出\mathbf{d} 矩阵为0，这是因为当前时刻的输出y(k) 与当前时刻的输入\tilde{\mathbf{u}}(k) 是无关的，而是与上一时刻的输入\tilde{\mathbf{u}}(k-1) 有关，所以这里是零矩阵
构建新的状态空间方程
我们现在相当于有两套系统，
第一套系统为将单车模型线性化的系统，在这个系统中，我们通过路径上的参考点进行非线性模型的线性化，也就是获得参考点附近的线性化模型
第二套系统为使用这套线性化模型进行路径的跟踪，在进行路径的跟踪时，参考点会不断变化，也就是这个线性化模型会不断变化，但是这个变化由第一套系统进行处理，不用管他 ，那么在第二套系统中，我们依然希望我们的控制器计算出的控制量是一个增量，因为这是便于控制的 ，所以我们这个地方需要构建一个新的状态空间方程来引入\Delta\tilde{\mathbf{u}}(k) = \tilde{\mathbf{u}}(k)-\tilde{\mathbf{u}}(k-1) ，来让我们控制器输出的控制量变为一个增量
所以为了引入\Delta\tilde{\mathbf{u}}(k) 我们重新定义了一个状态量为：
\xi(k)=\begin{bmatrix}
\tilde{\chi}(k)\\
\tilde{u}(k-1)
\end{bmatrix}
因此我们可以根据这个状态量写出新的状态转移方程
\begin{align*}
\xi(k+1) &= \begin{bmatrix}
\tilde{\chi}(k+1) \\
\tilde{u}(k)
\end{bmatrix} \\
&= \begin{bmatrix}
a \tilde{\chi}(k) + b \tilde{u}(k) \\
\tilde{u}(k)
\end{bmatrix} \\
&= \begin{bmatrix}
a \tilde{\chi}(k) + b \tilde{u}(k-1) + b (\tilde{u}(k) - \tilde{u}(k-1)) \\
\tilde{u}(k-1) + (\tilde{u}(k) - \tilde{u}(k-1))
\end{bmatrix} \\
&= \begin{bmatrix}
a \tilde{\chi}(k) + b \tilde{u}(k-1) + b \Delta \tilde{u}(k) \\
\tilde{u}(k-1) + \Delta \tilde{u}(k)
\end{bmatrix} \\
&= \begin{bmatrix}
a \tilde{\chi}(k) \\
\tilde{u}(k-1)
\end{bmatrix} + \begin{bmatrix}
b \tilde{u}(k-1) \\
0
\end{bmatrix} + \begin{bmatrix}
b \Delta \tilde{u}(k) \\
\Delta \tilde{u}(k)
\end{bmatrix} \\
&= \begin{bmatrix}
a & b \\
0 & I_{Nu}
\end{bmatrix} \begin{bmatrix}
\tilde{\chi}(k) \\
\tilde{u}(k-1)
\end{bmatrix} + \begin{bmatrix}
b \\
I_{Nu}
\end{bmatrix} \Delta \tilde{u}(k) \\
&= \begin{bmatrix}
a & b \\
0 & I_{Nu}
\end{bmatrix} \xi(k) + \begin{bmatrix}
b \\
I_{Nu}
\end{bmatrix} \Delta \tilde{u}(k) \\
&= A \xi(k) + B \Delta \tilde{u}(k)
\end{align*}
其中的N_u 指的是\tilde{\mathbf{u}}(k) 中的控制量的个数，在这当中，I_{Nu} = \begin{bmatrix}1 & 0 \\0 & 1\end{bmatrix}
同样的，我们也可以写出输出方程为：
\eta(k)=\begin{bmatrix} I_{Nx} & 0 \end{bmatrix} \begin{bmatrix} \tilde{\chi}(k) \\ \tilde{\mathbf{u}}(k-1) \end{bmatrix} = C\xi(k)
整理一下，我们的新状态空间方程为：
\begin{align*}
\xi(k+1) &= \begin{bmatrix}
a & b \\
0 & I_{N_u}
\end{bmatrix} \xi(k) + \begin{bmatrix}
b \\
I_{N_u}
\end{bmatrix} \Delta \tilde{u}(k)
= A \xi(k) + B \Delta \tilde{u}(k) \\
\eta(k) &= \begin{bmatrix} I_{N_x} & 0 \end{bmatrix} \begin{bmatrix} \tilde{\chi}(k) \\ \tilde{u}(k-1) \end{bmatrix}
= C \xi(k)
\end{align*}
构建状态预测方程与输出预测预测方程
我们定义预测区间(predication horizon)为N_p ，控制区间(control horizon)为N_c ,并且N_p \ge N_c
则，状态预测方程为：
\begin{align*}
\xi(k+1) &= A \xi(k) + B \Delta \tilde{u}(k) \\
\xi(k+2) &= A \xi(k+1) + B \Delta \tilde{u}(k+1) = A^2 \xi(k) + AB \Delta \tilde{u}(k) + B \Delta \tilde{u}(k+1) \\
\xi(k+3) &= A \xi(k+2) + B \Delta \tilde{u}(k+2) = A^3 \xi(k) + A^2 B \Delta \tilde{u}(k) + AB \Delta \tilde{u}(k+1) + B \Delta \tilde{u}(k+2) \\
&\vdots \\
\xi(k+N_c) &= A^{N_c} \xi(k) + A^{N_c-1} B \Delta \tilde{u}(k) + A^{N_c-2} B \Delta \tilde{u}(k+1) + \cdots + A^0 B \Delta \tilde{u}(k+N_c-1) \\
\xi(k+N_p) &= A^{N_p} \xi(k) + A^{N_p-1} B \Delta \tilde{u}(k) + A^{N_p-2} B \Delta \tilde{u}(k+1) + \cdots + A^0 B \Delta \tilde{u}(k+N_p-1)
\end{align*}
输出预测方程为：
\begin{align*}
\eta(k+1) &= C \xi(k+1) = C A \xi(k) + C B \Delta \tilde{u}(k) \\
\eta(k+2) &= C \xi(k+2) = C A^2 \xi(k) + C A B \Delta \tilde{u}(k) + C B \Delta \tilde{u}(k+1) \\
\eta(k+3) &= C \xi(k+3) = C A^3 \xi(k) + C A^2 B \Delta \tilde{u}(k) + C A B \Delta \tilde{u}(k+1) + C B \Delta \tilde{u}(k+2) \\
&\vdots \\
\eta(k+N_c) &= C A^{N_c} \xi(k) + C A^{N_c-1} B \Delta \tilde{u}(k) + C A^{N_c-2} B \Delta \tilde{u}(k+1) + \cdots + C A^0 B \Delta \tilde{u}(k+N_c-1) \\
\eta(k+N_p) &= C A^{N_p} \xi(k) + C A^{N_p-1} B \Delta \tilde{u}(k) + C A^{N_p-2} B \Delta \tilde{u}(k+1) + \cdots + C A^0 B \Delta \tilde{u}(k+N_p-1)
\end{align*}
通过总结上方的迭代式可以得到：
输出预测方程为
Y = \psi \xi(k) +\Theta\Delta{u}
其中有：
Y=
\begin{bmatrix}
\eta(k+1) \\
\eta(k+2) \\
\eta(k+3) \\
\vdots \\
\eta(k+N_c)\\
\eta(k+N_p)
\end{bmatrix},
\psi=
\begin{bmatrix}
CA \\
CA^2 \\
CA^3 \\
\vdots \\
CA^{N_C} \\
CA^{N_P}
\end{bmatrix},
\xi(k)
\begin{bmatrix}
\tilde{\chi}(k) \\
\tilde{\mathbf{u}}(k-1)
\end{bmatrix},
\Theta =
\begin{bmatrix}
CB & 0 & 0 & \cdots & 0\\
CAB & CB & 0 & \cdots & 0\\
\vdots & \vdots & \vdots & \vdots & \vdots \\
CA^{N_c-1}B & CA^{N_c-2}B & CA^{N_c-3}B & \cdots & CA^{0}B \\
CA^{N_p-1}B & CA^{N_p-2}B & CA^{N_p-3}B & \cdots & CA^{N_p-N_c}B \\
\end{bmatrix},
\Delta{u}=
\begin{bmatrix}
\Delta \tilde{u}(k) \\
\Delta \tilde{u}(k+1) \\
\Delta \tilde{u}(k+2) \\
\vdots \\
\Delta \tilde{u}(k+N_c-1) \\
\end{bmatrix}
预测时域与控制时域(滚动优化)
MPC是一种滚动优化(receding horizon control)的控制方法。在每个采样时间，MPC会通过求解一个有限时间内的最优化问题来计算最优控制序列，这一有限时间就被称为 预测区间(predication horizon)N_p 。通常因为要考虑系统的不确定性、测量误差等因素，在实际控制中，只选取预测区间内的最优控制序列中的第一项施加到系统中。
在上述的例子中，例如我们设置Np=Nc=5 ,则系统从k 时刻开始，初始状态为\xi(k) ，滚动优化控制会通过求解预测区间N_p 内最优化问题，也就是对上面得到的输出预测方程进行最优化，从而得到了控制区间(control horizon)N_c 个未来时间的最优控制策略\Delta\tilde{u}(k),\Delta\tilde{u}(k+1),\Delta\tilde{u}(k+2),\cdots ,\Delta\tilde{u}(k+N_c-1)
继续回到例子中，首先计算出最优的控制序列
\begin{bmatrix} \Delta\tilde{u}(k)\mid\Delta\tilde{u}(k) \end{bmatrix},\begin{bmatrix} \Delta\tilde{u}(k+1)\mid\Delta\tilde{u}(k) \end{bmatrix},\begin{bmatrix} \Delta\tilde{u}(k+2)\mid\Delta\tilde{u}(k) \end{bmatrix},\begin{bmatrix} \Delta\tilde{u}(k+3)\mid\Delta\tilde{u}(k) \end{bmatrix},\begin{bmatrix} \Delta\tilde{u}(k+4)\mid\Delta\tilde{u}(k) \end{bmatrix}
然后选取第一个最优控制策略\begin{bmatrix} \Delta\tilde{u}(k)\mid\Delta\tilde{u}(k) \end{bmatrix} 作为控制输入，舍弃掉之后的控制策略
这里的\begin{bmatrix} \Delta\tilde{u}(k+1)\mid\Delta\tilde{u}(k) \end{bmatrix} 代表的是在k 时刻通过最优化得到的k+1 时刻的最优控制策略
然后根据这些控制序列和状态预测方程，我们可以通过状态预测方程计算出在这样的控制序列下的状态值的变化:
\begin{bmatrix} \xi(k+1)\mid\xi(k) \end{bmatrix} ,\begin{bmatrix} \xi(k+2)\mid\xi(k) \end{bmatrix},\begin{bmatrix} \xi(k+3)\mid\xi(k) \end{bmatrix},\begin{bmatrix} \xi(k+4)\mid\xi(k) \end{bmatrix},\begin{bmatrix} \xi(k+5)\mid\xi(k) \end{bmatrix}
这里的\begin{bmatrix} \xi(k+1)\mid\xi(k) \end{bmatrix} 代表的是在k 时刻通过状态预测方程得到的k+1 时刻的状态量
总结一下为
先使用初始状态\xi(k) 与输出预测方程Y = \psi \xi(k) +\Theta\Delta{u} 在预测区间Np 内求解最优化问题
当N_p=N_c 时，会获得N_c 个不同的最优控制策略
当N_p>N_c 时，会获得N_c 个不同的最优控制策略，与N_p-N_c 个相同的最优控制策略(当N_p>N_c 时，超出的部分用N_c 的最后一个控制策略保持一致。)
然后选取第一个最优控制策略作为控制输入，舍弃掉之后的控制策略
使用这些最优控制策略所组成的最优控制序列，可以通过状态预测方程\xi(k+1) = A \xi(k) + B \Delta \tilde{u}(k) 预测出之后的状态变量
当达到下一个时间步时重复以上步骤
*但是这里有一个问题，模型预测的部分其实在求解最优化问题时的那个输出预测方程就已经体现了，那么最后拿最优控制策略计算出来的状态变量在MPC中有用吗？我看起来好像是没有的，应该是可以在其他地方用？*
可以将使用最优控制策略预测出来的状态变量再次带入求解最优化问题时的那个输出预测方程处进行迭代，从而使得求解出的最优控制序列更加稳定
优化目标函数
定义目标函数
通过上面的输出预测方程，我们可以写出我们的目标函数(性能指标函数)为
J =\sum_{i=1}^{N_p}\parallel\eta(k+i)-\eta_{ref}(k+i)\parallel^2_Q+\sum_{i=0}^{N_c-1}\parallel\Delta \mathbf{u}(k+i)\parallel^2_R+\rho\epsilon^2
其中，
状态误差项 \sum_{i=1}^{N_p}\parallel\eta(k+1)-\eta_{ref}(k+1)\parallel^2_Q 表示在预测区间N_p 内，当前系统输出(来自当前状态空间方程的输出，注意不是输出预测方程 )与目标输出之间的误差值的平方和(L_2 范数)，Q为权重矩阵
目标是最小化预测区间内系统输出与目标输出之间的误差
控制增量项 \sum_{i=0}^{N_c-1}\parallel\Delta \mathbf{u}(k+i)\parallel^2_R 表示在控制区间Nc 中，所有控制增量
\Delta{u}=
\begin{bmatrix}
\Delta \tilde{u}(k) \\
\Delta \tilde{u}(k+1) \\
\Delta \tilde{u}(k+2) \\
\vdots \\
\Delta \tilde{u}(k+N_c-1) \end{bmatrix}
的平方和(L_2 范数),所以这里(k+i) 指的是\Delta{u} 包含到了(k+i) 项，R为权重矩阵
软约束项 \rho\epsilon^2 引入松弛变量\epsilon 和乘法系数\rho ，用于处理约束条件的违背乘法
定义输出参考量
Y_r = \begin{bmatrix} \eta_{ref}(k+1) + \eta_{ref}(k+2)+\cdots+\eta_{ref}(k+N_c)+\cdots+\eta_{ref}(k+N_p) \end{bmatrix}^T=\begin{bmatrix} 0&0&\cdots&0&\cdots&0 \end{bmatrix}^T
这里将参考量定义为\mathbf{0} 量是因为，\eta(k) = \begin{bmatrix} I_{N_x} & 0 \end{bmatrix} \begin{bmatrix} \tilde{\chi}(k) \\ \tilde{u}(k-1) \end{bmatrix}= C \xi(k) ,而这里的\tilde{\chi}(k)=\begin{bmatrix}x-x_{r}\\ y-y_{r}\\ \varphi-\varphi_{r}\end{bmatrix} 所以它本身就已经是一个误差项了，所以我们这里希望的是这个误差逼近于0，所以这个地方我们定义输出参考量为一个\mathbf{0} 向量
将目标函数改写为二次规划的标准形式
我们在目标函数中引入了L_2 范数的原因之一就是为了引入二次规划来对目标函数进行优化，所以我们现在需要改写我们的目标函数，让其变为二次规划问题与拉格朗日乘数求解法 中的标准二次规划的形式，之后只要丢入二次规划求解器就可以轻松进行求解了
令E=\psi \xi(k)，Q=I_{N_p}\otimes Q,R=I_{N_p}\otimes R ，其中I_{N_p} 的目的是为了对预测区间内的每一时刻都带上权重
再通过输出预测方程Y = \psi \xi(k) +\Theta\Delta{u} 我们可以将状态误差项进行二次型标准化：
\begin{align*}
\sum_{i=1}^{N_p} \| \eta(k+i) \|^2_Q
&= \sum_{i=1}^{N_p} \| Y \|^2_Q \\
&= \sum_{i=1}^{N_p} \| \psi \xi(k) + \Theta \Delta{u} \|^2_Q \\
&= \sum_{i=1}^{N_p} \| E + \Theta \Delta{u} \|^2_Q \\
&= (E + \Theta \Delta{u})^T Q (E + \Theta \Delta{u})
\end{align*}
接下来我们把控制增量项也进行二次型标准化：
\sum_{i=0}^{N_c-1}\parallel\Delta \mathbf{u}(k+i)\parallel^2_R =\Delta\mathbf{u}^TR\Delta\mathbf{u}
因此，最终的目标函数为：
J=(E + \Theta \Delta\mathbf{u})^T Q (E + \Theta \Delta\mathbf{u})+\Delta\mathbf{u}^TR\Delta\mathbf{u}+\rho\epsilon^2 \\
=E^TQE+E^TQ\Theta \Delta\mathbf{u}+(\Theta \Delta\mathbf{u})^TQE+(\Theta \Delta\mathbf{u})^TQ(\Theta \Delta\mathbf{u})+\Delta\mathbf{u}^TR\Delta\mathbf{u}+\rho\epsilon^2 \\
=E^TQE+2E^TQ\Theta \Delta\mathbf{u}+\Delta\mathbf{u}^T\Theta^T Q\Theta \Delta\mathbf{u}+\Delta\mathbf{u}^TR\Delta\mathbf{u}+\rho\epsilon^2\\
=E^TQE+2E^TQ\Theta \Delta\mathbf{u}+\Delta\mathbf{u}^T(\Theta^TQ\Theta+R)\Delta\mathbf{u}+\rho\epsilon^2
因此当我们要写成标准型f(x)=(1/2)x^{T}Hx+f^{T}x+\text{const} 则为
J=\frac{1}{2}\Delta\mathbf{u}^TH\Delta\mathbf{u}+f^T\Delta\mathbf{u}+\text{const}
其中：
H=2(\Theta^TQ\Theta+R) f=2\Theta^TQE \mathrm{const}=E^TQE+\rho\epsilon^2
在这之中，E^TQE 和\rho\epsilon^2 因为与\Delta\mathbf{u} 无关，所以可以当作常数项，而在二次规划中，我们通常忽略常数项，所以最终的目标函数为
\min_{\Delta \mathbf{u}}J=\frac{1}{2}\Delta\mathbf{u}^TH\Delta\mathbf{u}+f^T\Delta\mathbf{u}
添加约束
写出整个控制区间之间的控制量，我们可以得到：
\begin{align*}
\tilde{u}(k) &= \tilde{u}(k-1) + \Delta \tilde{u}(k) \\
\tilde{u}(k+1) &= \tilde{u}(k) + \Delta \tilde{u}(k+1) = \tilde{u}(k-1) + \Delta \tilde{u}(k) + \Delta \tilde{u}(k+1) \\
&\vdots \\
\tilde{u}(k+N_c-1) &= \tilde{u}(k+N_c-2) + \Delta \tilde{u}(k+N_c-1) \\
&= \tilde{u}(k-1) + \Delta \tilde{u}(k) + \Delta \tilde{u}(k+1) + \cdots + \Delta \tilde{u}(k+N_c-1)
\end{align*}
进行规律总结，有：
\mathbf{U} =
\begin{bmatrix}
\tilde{u}(k) \\
\tilde{u}(k+1) \\
\vdots \\
\tilde{u}(k+N_c-1)
\end{bmatrix}
=
\begin{bmatrix}
\tilde{u}(k-1) \\
\tilde{u}(k-1) \\
\vdots \\
\tilde{u}(k-1)
\end{bmatrix}
+
\begin{bmatrix}
I_2 & 0 & 0 & \cdots & 0 \\
I_2 & I_2 & 0 & \cdots & 0 \\
I_2 & I_2 & I_2 & \cdots & 0 \\
\vdots & \vdots & \vdots & \ddots & \vdots \\
I_2 & I_2 & I_2 & \cdots & I_2
\end{bmatrix}
\begin{bmatrix}
\Delta \tilde{u}(k) \\
\Delta \tilde{u}(k+1) \\
\Delta \tilde{u}(k+2) \\
\vdots \\
\Delta \tilde{u}(k+N_c-1)
\end{bmatrix}
= U_t + A_I \Delta\mathbf{u}
其中：
I_2=\begin{bmatrix} 1&0\\0&1 \end{bmatrix} 对应了\tilde{\mathbf{u}}(k)=\begin{bmatrix}v-v_{r}\\ \delta-\delta_{r}\end{bmatrix}
所以，我们可以写出约束条件为
\mathbf{U}_{\text{min}} =
\begin{bmatrix}
\tilde{u}_{\text{min}} \\
\tilde{u}_{\text{min}} \\
\tilde{u}_{\text{min}} \\
\vdots \\
\tilde{u}_{\text{min}}
\end{bmatrix}
\leq
\begin{bmatrix}
\tilde{u}(k) \\
\tilde{u}(k+1) \\
\tilde{u}(k+2) \\
\vdots \\
\tilde{u}(k+N_c-1)
\end{bmatrix}
\leq
\begin{bmatrix}
\tilde{u}_{\text{max}} \\
\tilde{u}_{\text{max}} \\
\tilde{u}_{\text{max}} \\
\vdots \\
\tilde{u}_{\text{max}}
\end{bmatrix}
= \mathbf{U}_{\text{max}} \\
\Rightarrow\mathbf{U}_{\text{min}} \leq \mathbf{U} \leq \mathbf{U}_{\text{max}} \\
\Rightarrow\mathbf{U}_{\text{min}} \leq U_t + A_I \Delta\mathbf{u} \leq \mathbf{U}_{\text{max}}
进行化简后可得：
A_I \Delta\mathbf{u} \leq \mathbf{U}_{\text{max}} - U_t \\
-A_I \Delta\mathbf{u} \leq -\mathbf{U}_{\text{min}} + U_t
然后得到，不等式约束为
\begin{bmatrix}
A_I \\
-A_I
\end{bmatrix}
\Delta\mathbf{u}
\leq
\begin{bmatrix}
\mathbf{U}_{\text{max}} - U_t \\
-\mathbf{U}_{\text{min}} + U_t
\end{bmatrix}
TODO
约束部分应该写得问题很大，理解并不够透彻，接下来把代码看过之后，再过来改正
参考
线性时变模型预测控制推导详解
控制之美（卷2）——最优化控制MPC与卡尔曼滤波器
MPC模型预测控制
【离散系统】传递函数和状态空间方程离散化
