基于状态空间方程的倒立摆控制方法详解——从数学建模到Python仿真实现

文章目录

• [1. 引言](#1. 引言)
• • [1.1 工程应用价值](#1.1 工程应用价值)
  • [1.2 为什么传统单回路 PID 控制难以胜任？](#1.2 为什么传统单回路 PID 控制难以胜任？)
  • [1.3 状态空间方法的优势](#1.3 状态空间方法的优势)
• [2. 倒立摆系统数学建模](#2. 倒立摆系统数学建模)
• • [2.1 动态微分方程](#2.1 动态微分方程)
  • [2.2 状态空间方程建模](#2.2 状态空间方程建模)
  • [2.3 系统的能控性分析](#2.3 系统的能控性分析)
• [3. 线性状态反馈控制器](#3. 线性状态反馈控制器)
• • [3.1 平衡点控制](#3.1 平衡点控制)
  • [3.2 轨迹追踪](#3.2 轨迹追踪)
• [4. Python仿真代码](#4. Python仿真代码)
• • [4.1 仿真代码](#4.1 仿真代码)
  • [4.2 仿真结果](#4.2 仿真结果)

1. 引言

在控制理论与控制工程（Control Theory and Control Engineering）领域，直线一级倒立摆系统（Inverted Pendulum System）被公认为测试各种控制策略性能的"试金石"。

倒立摆系统是一个典型的高阶次、多变量、强耦合、非线性且本质不稳定的物理系统。在自然状态下，摆杆的竖直向上平衡点是一个不稳定平稳状态，稍有外力干扰，摆杆就会在重力矩作用下迅速倒下。

观察图 10.1.1(a) 所示的指尖平衡案例，这也一是倒立摆的简化模型。在一根长度为 d d d 的连杆顶端固定一个质量为 m m m 的小球（连杆的质量忽略不计）。连杆的另一端与手指接触，为简化分析，只考虑小球在二维平面 ( x 1 , x 2 ) (x_1, x_2) (x1,x2) 上的运动。连杆与垂直方向的夹角为 ϕ ( t ) \phi(t) ϕ(t)，顺时针方向为正方向。手指的移动方向固定沿图中 x 1 x_1 x1 轴，位移为 ξ ( t ) \xi(t) ξ(t)，向右为正。忽略摩擦力，手指对连杆的作用力为 F F F 并沿着连杆方向指向小球。通过移动手指，连杆与垂直方向的角度 ϕ ( t ) \phi(t) ϕ(t) 会发生改变。控制的目标是通过手指的运动使连杆小球与垂直方向的角度 ϕ ( t ) \phi(t) ϕ(t) 沿着图 10.1.1(b) 所示的目标轨迹 ϕ d ( t ) \phi_{\text{d}}(t) ϕd(t)（下标 d 代表 Desired，目标值）移动。

1.1 工程应用价值

倒立摆的控制逻辑在许多尖端工程场景中均有深度映射：

• 自平衡双轮机器人（Self-Balancing Mobile Robots）： 如 Segway 智能出行工具、双轮腿式机器人（如 Boston Dynamics 的 Handle 机器人），其姿态自平衡控制本质上就是二维倒立摆的稳态控制。
• 火箭与运载火箭的推力矢量控制（TVC）： 火箭在起飞上升段，其质心位于发动机推力作用点上方，动力学方程与倒立摆高度相似。通过微调喷管偏转角（类似小车移动）来保持火箭轴线竖直，是极为关键的技术。
• 双足人形机器人行走控制： 在双足步态规划中，通常将机器人的简化动力学模型抽象为零时刻质心倒立摆模型（Linear Inverted Pendulum Model, LIPM），用以预测和维持动态行走过程中的姿态平衡。
• 高层建筑抗震减震（TMD 阻尼器）： 建筑结构在风载或地震作用下的摆动调控，其数学模型与倒立摆在基座振动下的稳定性控制高度关联。

1.2 为什么传统单回路 PID 控制难以胜任？

对于常规单输入单输出（SISO）系统，经典 PID 控制（基于传递函数 G ( s ) G(s) G(s)）通过闭环误差反馈能够取得优良效果。然而在倒立摆系统中，传统单回路 PID 面临严重的本质困境：

• 多变量强耦合关系： 系统包含两个核心被控量：小车位置 x x x 与摆杆角度 θ \theta θ，但控制输入却只有一个（小车水平拉力 F F F）。这是一个典型的欠驱动系统（Underactuated System）。
• 零点/极点分布异样： 如果试图通过传递函数分别对 x x x 和 θ \theta θ 设计独立 PID，会发现摆杆角度通道包含右半平面的极点（开环不稳定），而小车位置与摆杆角度之间存在相互牵制。调节 PID 参数使角度稳定的同时，往往会导致小车位置发生发散漂移。
• 缺乏状态全局调控能力： 经典 PID 基于频域或单变量偏差，无法有效协调"摆杆尽快恢复竖直"与"小车控制在轨道安全范围内"这两个相互冲突的控制目标。

1.3 状态空间方法的优势

现代控制理论（Modern Control Theory）提出的状态空间方法（State-Space Approach）为解决这一问题提供了极佳途径：

• 内部状态全面掌控： 状态空间法利用向量状态方程描述系统内所有独立状态（位移 x x x、速度 x ˙ \dot{x} x˙、角度 θ \theta θ、角速度 θ ˙ \dot{\theta} θ˙），能够精准捕获多变量间的动态耦合机制。
• 多变量统一合成： 允许通过状态反馈矩阵 K K K，将所有状态变量线性组合作用于单一控制量 u u u，一次性解决 x x x 和 θ \theta θ 的协同稳定问题。
• 最优控制的数学便利： 基于状态空间表示，可以无缝应用二次型最优控制（LQR）或极点配置（Pole Placement）技术，在明确的系统动态性能（如超调量、调节时间）与控制能量消耗之间取得最佳平衡。

2. 倒立摆系统数学建模

2.1 动态微分方程

为实现控制目标，首先要建立此动态系统的数学模型。对连杆小球进行受力分析可以得到在 x 1 x_1 x1 方向，根据牛顿第二定律： m d 2 x 1 ( t ) d t 2 = F x 1 (10.1.1) m \frac{\mathrm{d}^2 x_1(t)}{\mathrm{d}t^2} = F_{x_1} \tag{10.1.1} mdt2d2x1(t)=Fx1(10.1.1)

其中， x 1 ( t ) x_1(t) x1(t) 是小球沿 x 1 x_1 x1 方向的位移，它由手指的位移加上小球的位移组成，即 x 1 ( t ) = ξ ( t ) + d sin ⁡ ϕ ( t ) (10.1.2) x_1(t) = \xi(t) + d \sin\phi(t) \tag{10.1.2} x1(t)=ξ(t)+dsinϕ(t)(10.1.2)式 (10.1.2) 中存在非线性项 d sin ⁡ ϕ ( t ) d \sin\phi(t) dsinϕ(t)，在本书中讨论的是线性时不变系统，因此需要将它线性化。当连杆靠近垂直位置时， sin ⁡ ϕ ( t ) ≈ ϕ ( t ) \sin\phi(t) \approx \phi(t) sinϕ(t)≈ϕ(t)（关于非线性系统线性化的方法，请参考附录 A）。将其代入式 (10.1.2)，并对等号两边取时间的二次微分，可得 d 2 x 1 ( t ) d t 2 = d 2 ξ ( t ) d t 2 + d d 2 ϕ ( t ) d t 2 (10.1.3) \frac{\mathrm{d}^2 x_1(t)}{\mathrm{d}t^2} = \frac{\mathrm{d}^2 \xi(t)}{\mathrm{d}t^2} + d \frac{\mathrm{d}^2 \phi(t)}{\mathrm{d}t^2} \tag{10.1.3} dt2d2x1(t)=dt2d2ξ(t)+ddt2d2ϕ(t)(10.1.3)将式 (10.1.3) 代入式 (10.1.1)，得到 m d 2 ξ ( t ) d t 2 + m d d 2 ϕ ( t ) d t 2 = F x 1 (10.1.4) m \frac{\mathrm{d}^2 \xi(t)}{\mathrm{d}t^2} + md \frac{\mathrm{d}^2 \phi(t)}{\mathrm{d}t^2} = F_{x_1} \tag{10.1.4} mdt2d2ξ(t)+mddt2d2ϕ(t)=Fx1(10.1.4)式 (10.1.4) 等号右边的 F x 1 F_{x_1} Fx1 是手指对连杆力 F F F 在 x 1 x_1 x1 方向的分力，因此 F x 1 = F sin ⁡ ϕ ( t ) F_{x_1} = F \sin\phi(t) Fx1=Fsinϕ(t)，当 ϕ ( t ) \phi(t) ϕ(t) 靠近垂直位置时， sin ⁡ ϕ ( t ) ≈ ϕ ( t ) \sin\phi(t) \approx \phi(t) sinϕ(t)≈ϕ(t)，即 F x 1 = F ϕ ( t ) F_{x_1} = F \phi(t) Fx1=Fϕ(t)，代入式 (10.1.4)，得到 x 1 x_1 x1 方向的微分方程为 m d 2 ξ ( t ) d t 2 + m d d 2 ϕ ( t ) d t 2 = F ϕ ( t ) (10.1.5) m \frac{\mathrm{d}^2 \xi(t)}{\mathrm{d}t^2} + md \frac{\mathrm{d}^2 \phi(t)}{\mathrm{d}t^2} = F \phi(t) \tag{10.1.5} mdt2d2ξ(t)+mddt2d2ϕ(t)=Fϕ(t)(10.1.5)在 x 2 x_2 x2 方向，根据牛顿第二定律： m d 2 x 2 ( t ) d t 2 = F x 2 − m g = F cos ⁡ ϕ ( t ) − m g (10.1.6) m \frac{\mathrm{d}^2 x_2(t)}{\mathrm{d}t^2} = F_{x_2} - mg = F \cos\phi(t) - mg \tag{10.1.6} mdt2d2x2(t)=Fx2−mg=Fcosϕ(t)−mg(10.1.6)其中，小球在 x 2 x_2 x2 方向的位移为 x 2 ( t ) = d cos ⁡ ϕ ( t ) − d x_2(t) = d \cos\phi(t) - d x2(t)=dcosϕ(t)−d，当 ϕ ( t ) \phi(t) ϕ(t) 很小时， cos ⁡ ϕ ( t ) ≈ 1 \cos\phi(t) \approx 1 cosϕ(t)≈1， x 2 ( t ) = d − d = 0 x_2(t) = d - d = 0 x2(t)=d−d=0。可得 0 = F − m g 0 = F - mg 0=F−mg，将 m g = F mg = F mg=F 代入式 (10.1.5)，整理后得到连杆小球关于角度 ϕ ( t ) \phi(t) ϕ(t) 的动态微分方程，即 d 2 ϕ ( t ) d t 2 − g d ϕ ( t ) = − 1 d d 2 ξ ( t ) d t 2 (10.1.7) \frac{\mathrm{d}^2 \phi(t)}{\mathrm{d}t^2} - \frac{g}{d}\phi(t) = -\frac{1}{d} \frac{\mathrm{d}^2 \xi(t)}{\mathrm{d}t^2} \tag{10.1.7} dt2d2ϕ(t)−dgϕ(t)=−d1dt2d2ξ(t)(10.1.7)

2.2 状态空间方程建模

将式 (10.1.7) 的微分方程写成状态空间方程，首先选取两个状态变量 z 1 ( t ) z_1(t) z1(t) 和 z 2 ( t ) z_2(t) z2(t)，令 z 1 ( t ) = ϕ ( t ) (10.1.10) z_1(t) = \phi(t) \tag{10.1.10} z1(t)=ϕ(t)(10.1.10) z 2 ( t ) = d z 1 ( t ) d t = d ϕ ( t ) d t (10.1.11) z_2(t) = \frac{\mathrm{d} z_1(t)}{\mathrm{d}t} = \frac{\mathrm{d}\phi(t)}{\mathrm{d}t} \tag{10.1.11} z2(t)=dtdz1(t)=dtdϕ(t)(10.1.11)取 z 2 ( t ) z_2(t) z2(t) 对时间的导数，将式 (10.1.7) 考虑为一个动态系统，定义其输入 u ( t ) = − 1 d d 2 ξ ( t ) d t 2 u(t) = \frac{-1}{d} \frac{\mathrm{d}^2 \xi(t)}{\mathrm{d}t^2} u(t)=d−1dt2d2ξ(t)，其中， d 2 ξ ( t ) d t 2 \frac{\mathrm{d}^2 \xi(t)}{\mathrm{d}t^2} dt2d2ξ(t) 的物理意义是手指沿 x 1 x_1 x1 方向的加速度，乘以 − 1 d \frac{-1}{d} d−1 的意义是将其单位化。并将 u ( t ) = − 1 d d 2 ξ ( t ) d t 2 u(t) = -\frac{1}{d} \frac{\mathrm{d}^2\xi(t)}{\mathrm{d}t^2} u(t)=−d1dt2d2ξ(t) 和式 (10.1.7) 代入其中，可得 d z 2 ( t ) d t = d 2 ϕ ( t ) d t 2 = g d ϕ ( t ) + u ( t ) (10.1.12) \frac{\mathrm{d} z_2(t)}{\mathrm{d}t} = \frac{\mathrm{d}^2\phi(t)}{\mathrm{d}t^2} = \frac{g}{d}\phi(t) + u(t) \tag{10.1.12} dtdz2(t)=dt2d2ϕ(t)=dgϕ(t)+u(t)(10.1.12)把式 (10.1.10) 和式 (10.1.12) 写成一个紧凑的矩阵表达形式，可得 d z ( t ) d t = A z ( t ) + B u ( t ) , 其中 , A = 0 1 g d 0 , B = 0 1 (10.1.13) \frac{\mathrm{d} \boldsymbol{z}(t)}{\mathrm{d}t} = \boldsymbol{A}\boldsymbol{z}(t) + \boldsymbol{B}u(t), \quad \text{其中}, \boldsymbol{A} = \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix}, \boldsymbol{B} = \begin{bmatrix} 0 \\ 1 \end{bmatrix} \tag{10.1.13} dtdz(t)=Az(t)+Bu(t),其中,A=0dg10,B=01(10.1.13)系统的输出 y ( t ) = ϕ ( t ) y(t) = \phi(t) y(t)=ϕ(t) 也可以写成矩阵形式，即 y ( t ) = C z 1 ( t ) z 2 ( t ) + D u ( t ) , 其中 , C = 1 0 , D = 0 (10.1.14) \boldsymbol{y}(t) = \boldsymbol{C} \begin{bmatrix} z_1(t) \\ z_2(t) \end{bmatrix} + \boldsymbol{D}u(t), \quad \text{其中}, \boldsymbol{C} = \begin{bmatrix} 1 & 0 \end{bmatrix}, \boldsymbol{D} = 0 \tag{10.1.14} y(t)=Cz1(t)z2(t)+Du(t),其中,C=10,D=0(10.1.14)式 (10.1.13) 和式 (10.1.14) 构成了动态系统的状态空间方程。图 10.1.3 使用框图描述了一个标准的状态空间方程表达式。我们将以此为基础来设计控制器。

2.3 系统的能控性分析

根据给出的状态空间方程，系统的系数矩阵为： A = 0 1 g d 0 , B = 0 1 \boldsymbol{A} = \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix}, \quad \boldsymbol{B} = \begin{bmatrix} 0 \\ 1 \end{bmatrix} A=0dg10,B=01系统维数为 n = 2 n = 2 n=2（包含 2 个状态变量 z 1 ( t ) z_1(t) z1(t) 和 z 2 ( t ) z_2(t) z2(t)）。

对于二阶系统 ( n = 2 n=2 n=2)，能控性矩阵定义为： Q c = B A B \boldsymbol{Q}_c = \begin{bmatrix} \boldsymbol{B} & \boldsymbol{A}\boldsymbol{B} \end{bmatrix} Qc=BAB首先计算 A B \boldsymbol{A}\boldsymbol{B} AB： A B = 0 1 g d 0 0 1 = 1 0 \boldsymbol{A}\boldsymbol{B} = \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix} \begin{bmatrix} 0 \\ 1 \end{bmatrix} = \begin{bmatrix} 1 \\ 0 \end{bmatrix} AB=0dg1001=10将 B \boldsymbol{B} B 和 A B \boldsymbol{A}\boldsymbol{B} AB 组合起来得到： Q c = 0 1 1 0 \boldsymbol{Q}_c = \begin{bmatrix} 0 & 1 \\ 1 & 0 \end{bmatrix} Qc=0110

计算能控性矩阵 Q c \boldsymbol{Q}_c Qc 的行列式或秩： det ⁡ ( Q c ) = ( 0 ⋅ 0 ) − ( 1 ⋅ 1 ) = − 1 ≠ 0 \det(\boldsymbol{Q}_c) = (0 \cdot 0) - (1 \cdot 1) = -1 \neq 0 det(Qc)=(0⋅0)−(1⋅1)=−1=0因为行列式不为 0，所以该矩阵满秩： rank ( Q c ) = 2 = n \text{rank}(\boldsymbol{Q}_c) = 2 = n rank(Qc)=2=n

状态能控性判据 ：对于 n n n 维线性时不变系统（式(10.2.2)）而言，它的状态能控的充分必要条件是能控矩阵 C o = B A B A 2 B ⋯ A n − 1 B \boldsymbol{C}{\text{o}} = \begin{bmatrix} \boldsymbol{B} & \boldsymbol{A}\boldsymbol{B} & \boldsymbol{A}^2\boldsymbol{B} & \cdots & \boldsymbol{A}^{n-1}\boldsymbol{B} \end{bmatrix} Co=BABA2B⋯An−1B的秩为 n n n，即 Rank ( C o ) = n \text{Rank}(\boldsymbol{C}{\text{o}}) = n Rank(Co)=n

由于 rank ( Q c ) = n = 2 \text{rank}(\boldsymbol{Q}_c) = n = 2 rank(Qc)=n=2，该系统是完全状态能控的（Fully State Controllable）。

3. 线性状态反馈控制器

3.1 平衡点控制

首先分析无输入的系统，即 u ( t ) = 0 u(t)=0 u(t)=0 的情况。此时 d z ( t ) d t = A z ( t ) \frac{\mathrm{d}\boldsymbol{z}(t)}{\mathrm{d}t} = \boldsymbol{A}\boldsymbol{z}(t) dtdz(t)=Az(t)，求它的平衡点，令 d z ( t ) d t = 0 \frac{\mathrm{d}\boldsymbol{z}(t)}{\mathrm{d}t} = 0 dtdz(t)=0，可得 { 0 = z 2 f 0 = g d z 1 f ⇒ { z 1 f = 0 z 2 f = 0 (10.3.2) \begin{cases} 0 = z_{2\text{f}} \\ 0 = \frac{g}{d} z_{1\text{f}} \end{cases} \Rightarrow \begin{cases} z_{1\text{f}} = 0 \\ z_{2\text{f}} = 0 \end{cases} \tag{10.3.2} {0=z2f0=dgz1f⇒{z1f=0z2f=0(10.3.2)可以根据状态矩阵 A \boldsymbol{A} A 的特征值判断平衡点的类型。求矩阵 A \boldsymbol{A} A 的特征值，令 ∣ A − λ I ∣ = 0 \vert{}\boldsymbol{A} - \lambda \boldsymbol{I}\vert{} = 0 ∣A−λI∣=0，得到 ∣ − λ 1 g d − λ ∣ = 0 (10.3.3) \begin{vmatrix} -\lambda & 1 \\ \frac{g}{d} & -\lambda \end{vmatrix} = 0 \tag{10.3.3} −λdg1−λ =0(10.3.3)即 λ 2 − g d = 0 (10.3.4) \lambda^2 - \frac{g}{d} = 0 \tag{10.3.4} λ2−dg=0(10.3.4)计算出矩阵 A \boldsymbol{A} A 的两个特征值为 { λ 1 = − g d < 0 λ 2 = g d > 0 (10.3.5) \begin{cases} \lambda_1 = -\sqrt{\frac{g}{d}} < 0 \\ \lambda_2 = \sqrt{\frac{g}{d}} > 0 \end{cases} \tag{10.3.5} {λ1=−dg <0λ2=dg >0(10.3.5)

式 (10.3.5) 说明状态矩阵的特征值一正一负，根据表 3.3.1 可以判断系统的平衡点 z f ( t ) = 0 , 0 T \boldsymbol{z}_{\text{f}}(t) = 0, 0^{\text{T}} zf(t)=0,0T 是一个鞍点（不稳定的平衡点）， g d \frac{g}{d} dg 的值仅仅会改变相轨迹的形状，但不会影响平衡点的性质。它的相轨迹如图 10.3.1 所示（图中选取 g d = 10 \frac{g}{d} = 10 dg=10）。这意味着系统的状态变量一旦偏离平衡点，在没有外力输入的情况下，将无法回到平衡点，所以是一个不稳定的系统。

考虑设计全状态反馈控制器，可以令 u ( t ) = − K z ( t ) , 其中 , K = k 1 k 2 (10.3.7) u(t) = - \boldsymbol{K}\boldsymbol{z}(t), \quad \text{其中}, \boldsymbol{K} = \begin{bmatrix} k_1 & k_2 \end{bmatrix} \tag{10.3.7} u(t)=−Kz(t),其中,K=k1k2(10.3.7)此时的输入 u ( t ) u(t) u(t) 与所有状态变量 z ( t ) \boldsymbol{z}(t) z(t) 有关。将式 (10.3.7) 代入式 (10.3.1a)，可得 d z ( t ) d t = A z ( t ) − B K z ( t ) = ( A − B K ) z ( t ) = A cl z ( t ) (10.3.8a) \frac{\mathrm{d}\boldsymbol{z}(t)}{\mathrm{d}t} = \boldsymbol{A}\boldsymbol{z}(t) - \boldsymbol{B}\boldsymbol{K}\boldsymbol{z}(t) = (\boldsymbol{A} - \boldsymbol{B}\boldsymbol{K})\boldsymbol{z}(t) = \boldsymbol{A}{\text{cl}}\boldsymbol{z}(t) \tag{10.3.8a} dtdz(t)=Az(t)−BKz(t)=(A−BK)z(t)=Aclz(t)(10.3.8a)其中， A cl = A − B K \boldsymbol{A}{\text{cl}} = \boldsymbol{A} - \boldsymbol{B}\boldsymbol{K} Acl=A−BK，代表闭环控制系统的状态矩阵。称其为闭环控制系统是因为它将状态反馈集成到系统之中。在本例中 A cl = A − B K = 0 1 g d 0 − 0 1 k 1 k 2 = 0 1 g d − k 1 − k 2 (10.3.8b) \boldsymbol{A}{\text{cl}} = \boldsymbol{A} - \boldsymbol{B}\boldsymbol{K} = \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix} - \begin{bmatrix} 0 \\ 1 \end{bmatrix} \begin{bmatrix} k_1 & k_2 \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ \frac{g}{d} - k_1 & -k_2 \end{bmatrix} \tag{10.3.8b} Acl=A−BK=0dg10−01k1k2=0dg−k11−k2(10.3.8b)令 ∣ A cl − λ I ∣ = 0 \vert{}\boldsymbol{A}{\text{cl}} - \lambda \boldsymbol{I}\vert{} = 0 ∣Acl−λI∣=0，可以求其特征值，得到 ∣ − λ 1 g d − k 1 − k 2 − λ ∣ = 0 (10.3.9a) \begin{vmatrix} -\lambda & 1 \\ \frac{g}{d} - k_1 & -k_2 - \lambda \end{vmatrix} = 0 \tag{10.3.9a} −λdg−k11−k2−λ =0(10.3.9a)

即 ( − λ ) ( − k 2 − λ ) − ( g d − k 1 ) × ( 1 ) = 0 (-\lambda)(-k_2 - \lambda) - \left( \frac{g}{d} - k_1 \right) \times (1) = 0 (−λ)(−k2−λ)−(dg−k1)×(1)=0 ⇒ λ 2 + k 2 λ − ( g d − k 1 ) = 0 (10.3.9b) \Rightarrow \lambda^2 + k_2 \lambda - \left( \frac{g}{d} - k_1 \right) = 0 \tag{10.3.9b} ⇒λ2+k2λ−(dg−k1)=0(10.3.9b)如果希望闭环状态矩阵的两个特征值为 λ 1 , 2 = − 1 < 0 \lambda_{1,2} = -1 < 0 λ1,2=−1<0（此时平衡点将变成稳定节点），其对应的特征方程为 ( λ + 1 ) ( λ + 1 ) = 0 (\lambda + 1)(\lambda + 1) = 0 (λ+1)(λ+1)=0 ⇒ λ 2 + 2 λ + 1 = 0 (10.3.10) \Rightarrow \lambda^2 + 2\lambda + 1 = 0 \tag{10.3.10} ⇒λ2+2λ+1=0(10.3.10)对比式 (10.3.9b) 和式 (10.3.10)，可得 { − ( g d − k 1 ) = 1 k 2 = 2 \begin{cases} -\left( \frac{g}{d} - k_1 \right) = 1 \\ k_2 = 2 \end{cases} {−(dg−k1)=1k2=2 ⇒ { k 1 = 1 + g d k 2 = 2 (10.3.11) \Rightarrow \begin{cases} k_1 = 1 + \frac{g}{d} \\ k_2 = 2 \end{cases} \tag{10.3.11} ⇒{k1=1+dgk2=2(10.3.11)代入式 (10.3.7)，系统的输入为 u ( t ) = − K z ( t ) = − k 1 − k 2 z ( t ) = − 1 − g d − 2 z ( t ) (10.3.12) \boldsymbol{u}(t) = -\boldsymbol{K}\boldsymbol{z}(t) = \begin{bmatrix} -k_1 & -k_2 \end{bmatrix} \boldsymbol{z}(t) = \begin{bmatrix} -1 - \frac{g}{d} & -2 \end{bmatrix} \boldsymbol{z}(t) \tag{10.3.12} u(t)=−Kz(t)=−k1−k2z(t)=−1−dg−2z(t)(10.3.12)将式 (10.3.12) 代入式 (10.1.13)，可得 d z ( t ) d t = A cl z ( t ) = 0 1 − 1 − 2 z ( t ) (10.3.13) \frac{\mathrm{d}\boldsymbol{z}(t)}{\mathrm{d}t} = \boldsymbol{A}_{\text{cl}}\boldsymbol{z}(t) = \begin{bmatrix} 0 & 1 \\ -1 & -2 \end{bmatrix} \boldsymbol{z}(t) \tag{10.3.13} dtdz(t)=Aclz(t)=0−11−2z(t)(10.3.13)其反馈控制系统所对应的相轨迹如图 10.3.2 所示。在使用线性反馈控制 u ( t ) = − K z ( t ) \boldsymbol{u}(t) = -\boldsymbol{K}\boldsymbol{z}(t) u(t)=−Kz(t) 之后，原动态系统不稳定的平衡点变成了稳定的节点。

从本质上来讲，这依然是比例控制，但相较于传统方法（只反馈位移信息），所有的状态信息（包括位移和速度）都被用作反馈，因此有两个比例增益 k 1 k_1 k1 和 k 2 k_2 k2。

式 (10.3.10) 中的特征值 λ 1 , 2 = − 1 \lambda_{1,2} = -1 λ1,2=−1 是随意选的，只是选择了两个负数来保证系统的稳定性。在实际应用中，是否可以通过某种方式指导设计这个值呢？接下来介绍这个方法。

在设计反馈控制 K \boldsymbol{K} K 的时候，如何选择闭环状态矩阵的特征值涉及最优化控制，从这点入手可以发展出一整个单独的学科和研究领域。本节将稍作试探，下面介绍一个基础的最优化控制器------LQR 控制器。LQR 的全称为 Linear Quadratic Regulator，字面意义就是线性二次型调节器。

在式 (10.3.7) 和式 (10.3.8) 中，当使用线性状态反馈控制时，令 u ( t ) = − K z ( t ) u(t) = -\boldsymbol{K}\boldsymbol{z}(t) u(t)=−Kz(t)，可以得到闭环控制系统的状态空间方程，即 d z ( t ) d t = ( A − B K ) z ( t ) = A cl z ( t ) (10.3.14) \frac{\mathrm{d}\boldsymbol{z}(t)}{\mathrm{d}t} = (\boldsymbol{A} - \boldsymbol{B}\boldsymbol{K})\boldsymbol{z}(t) = \boldsymbol{A}_{\text{cl}}\boldsymbol{z}(t) \tag{10.3.14} dtdz(t)=(A−BK)z(t)=Aclz(t)(10.3.14)

此时通过设计不同的 K \boldsymbol{K} K 值可以改变闭环状态矩阵 A cl \boldsymbol{A}{\text{cl}} Acl 的特征值。在 10.3.1 节中选取了 λ 1 , 2 = − 1 \lambda{1,2} = -1 λ1,2=−1，这样做可以保证系统的稳定性。同时， A cl \boldsymbol{A}{\text{cl}} Acl 特征值的不同将决定不同状态的收敛速度及输入的大小。为了更好地选择特征值，引入代价函数（Cost Function） J = ∫ 0 ∞ z T ( t ) Q z ( t ) + u T ( t ) R u ( t ) d t (10.3.15) J = \int{0}^{\infty} \boldsymbol{z}^{\text{T}}(t)\boldsymbol{Q}\boldsymbol{z}(t) + u^{\text{T}}(t)\boldsymbol{R}u(t) \mathrm{d}t \tag{10.3.15} J=∫0∞zT(t)Qz(t)+uT(t)Ru(t)dt(10.3.15)控制器设计的目标是选择合适的 K \boldsymbol{K} K，从而得到 J min ⁡ J_{\min} Jmin（代价函数 J J J 的最小值）。式 (10.3.15) 中，矩阵 Q \boldsymbol{Q} Q 和矩阵 R \boldsymbol{R} R 分别是状态变量和控制量（即动态系统的输入）的权重矩阵，都是正定的对称矩阵。一般情况下， Q \boldsymbol{Q} Q 和 R \boldsymbol{R} R 会选择为对角矩阵，且对角线上的元素都大于 0，即 Q n × n = q 1 ⋯ 0 q 2 ⋯ 0 ⋮ ⋮ ⋱ ⋮ ⋮ 0 ⋯ q n − 1 0 ⋯ q n , R p × p = r 1 ⋯ 0 r 2 ⋯ 0 ⋮ ⋮ ⋱ ⋮ ⋮ 0 ⋯ r p − 1 0 ⋯ r p (10.3.16) \boldsymbol{Q}{n \times n} = \begin{bmatrix} q_1 & & \cdots & & 0 \\ & q_2 & \cdots & 0 & \\ \vdots & \vdots & \ddots & \vdots & \vdots \\ & 0 & \cdots & q{n-1} & \\ 0 & & \cdots & & q_n \end{bmatrix}, \quad R_{p \times p} = \begin{bmatrix} r_1 & & \cdots & & 0 \\ & r_2 & \cdots & 0 & \\ \vdots & \vdots & \ddots & \vdots & \vdots \\ & 0 & \cdots & r_{p-1} & \\ 0 & & \cdots & & r_p \end{bmatrix} \tag{10.3.16} Qn×n= q1⋮0q2⋮0⋯⋯⋱⋯⋯0⋮qn−10⋮qn ,Rp×p= r1⋮0r2⋮0⋯⋯⋱⋯⋯0⋮rp−10⋮rp (10.3.16)

其中， n n n 代表状态变量 z ( t ) \boldsymbol{z}(t) z(t) 的维度， p p p 代表输入 u ( t ) \boldsymbol{u}(t) u(t) 的维度。通过设计不同的对角元素的值可以得到不同的代价函数。考虑本节的例子： d z ( t ) d t = A z ( t ) + B u ( t ) , 其中 , A = 0 − 3 − 1 2 , B = 0 1 (10.3.17) \frac{\mathrm{d}\boldsymbol{z}(t)}{\mathrm{d}t} = \boldsymbol{A}\boldsymbol{z}(t) + \boldsymbol{B}u(t), \quad \text{其中}, \boldsymbol{A} = \begin{bmatrix} 0 & -3 \\ -1 & 2 \end{bmatrix}, \boldsymbol{B} = \begin{bmatrix} 0 \\ 1 \end{bmatrix} \tag{10.3.17} dtdz(t)=Az(t)+Bu(t),其中,A=0−1−32,B=01(10.3.17)它是一个二状态单输入系统， n = 2 , p = 1 n = 2, p = 1 n=2,p=1，所以 Q = q 1 0 0 q 2 , R = r \boldsymbol{Q} = \begin{bmatrix} q_1 & 0 \\ 0 & q_2 \end{bmatrix}, \boldsymbol{R} = r Q=q100q2,R=r。代入式 (10.3.15) 可得 J = ∫ 0 ∞ z 1 ( t ) z 2 ( t ) q 1 0 0 q 2 z 1 ( t ) z 2 ( t ) + u ( t ) r u ( t ) d t = ∫ 0 ∞ q 1 z 1 2 ( t ) + q 2 z 2 2 ( t ) + r u 2 ( t ) d t (10.3.18) \begin{aligned} J &= \int_{0}^{\infty} \begin{bmatrix} z_1(t) & z_2(t) \end{bmatrix} \begin{bmatrix} q_1 & 0 \\ 0 & q_2 \end{bmatrix} \begin{bmatrix} z_1(t) \\ z_2(t) \end{bmatrix} + u(t) r u(t) \mathrm{d}t \\ &= \int_{0}^{\infty} q_1 z_1^2(t) + q_2 z_2^2(t) + r u^2(t) \mathrm{d}t \end{aligned} \tag{10.3.18} J=∫0∞z1(t)z2(t)q100q2z1(t)z2(t)+u(t)ru(t)dt=∫0∞q1z12(t)+q2z22(t)+ru2(t)dt(10.3.18)观察式 (10.3.18) 可以发现不同的权重系数 q 1 , q 2 , r q_1, q_2, r q1,q2,r 对代价函数的影响。例如当 q 1 q_1 q1 远大于 q 2 q_2 q2 和 r r r 时，在求代价函数最小值的时候就会更关注 z 1 ( t ) z_1(t) z1(t) 的变化，会让它在最短时间内趋于 0 0 0。反之，如果 r r r 比较大，在求解的过程中就会更加关注 u ( t ) \boldsymbol{u}(t) u(t) 的值。

以指尖平衡系统为例分析不同权重矩阵对系统的影响，表 10.3.1 为测试方案，选择三组不同的权重矩阵，分别强调 z 1 ( t ) z_1(t) z1(t)， z 2 ( t ) z_2(t) z2(t) 和 u ( t ) \boldsymbol{u}(t) u(t)。系统的初始状态为 z ( 0 ) = π 20 , 0 T \boldsymbol{z}(0) = \begin{bmatrix} \frac{\pi}{20} , 0 \end{bmatrix}^{\text{T}} z(0)=20π,0T，它意味着在时间 t = 0 t=0 t=0 时刻，连杆与垂直方向的夹角为 π 20 \frac{\pi}{20} 20π，速度为 0。控制的目标是将它移动回到平衡位置。在测试中选择参数 g d = 10 \frac{g}{d} = 10 dg=10。表 10.3.1 中的第四列显示了当 LQR 控制器输入不同的权重矩阵后所计算出的矩阵 K \boldsymbol{K} K（即满足给定权重矩阵情况下满足代价函数最小的 K \boldsymbol{K} K）。

测试结果如图 10.3.4 所示。它显示了系统状态变量 z 1 ( t ) , z 2 ( t ) z_1(t), z_2(t) z1(t),z2(t) 以及控制量（动态系统的输入） u ( t ) u(t) u(t) 随时间的变化，可以发现在这三组测试中，三组控制系统都成功地将系统稳定到了平衡点 0 , 0 T 0,0^{\text{T}} 0,0T。其中，组 1 的状态变量 z 1 ( t ) z_1(t) z1(t) 的收敛速度是最快的，如图 10.3.3a 所示，这是因为在它的权重矩阵 Q \boldsymbol{Q} Q 中 z 1 ( t ) z_1(t) z1(t) 所对应的权重系数是 100，远大于另一个状态变量 z 2 ( t ) z_2(t) z2(t) 和输入 u ( t ) u(t) u(t) 的权重系数 1。其所对应的代价方程为 J = ∫ 0 ∞ 100 z 1 2 ( t ) + z 2 2 ( t ) + u 2 ( t ) d t J = \int_{0}^{\infty} 100z_1^2(t) + z_2^2(t) + u^2(t) \mathrm{d}t J=∫0∞100z12(t)+z22(t)+u2(t)dt， z 1 ( t ) z_1(t) z1(t) 在其中占比是最大的。所以当求 J min ⁡ J_{\min} Jmin 的时候，会考虑如何让 z 1 ( t ) z_1(t) z1(t) 迅速地接近 0 0 0。读者可以根据表 10.3.1 思考分析其他两组结果。请注意在图 10.3.3© 中，虽然组 3 的输入 u ( t ) u(t) u(t) 收敛速度慢于组 1，但是它的起始位置要远低于组 1，所以积分后的结果 ∫ 0 ∞ r u 2 ( t ) d t \int_{0}^{\infty} r u^2(t) \mathrm{d}t ∫0∞ru2(t)dt 低于组 1。

最优化是当前最热门的领域之一，有很多新的算法被引入进来。但无论是机器学习、神经网络，还是其他的智能算法，其本质都是建立代价函数，设定不同的权重系数并求其极值（极大值或者极小值）。在控制领域中，"最优"并不像体育比赛一样会有明确的标准，例如短跑，谁跑得快谁就是最优的。控制系统中的最优是综合分析的结果，例如在这个例子中，可以说组 1 是最优的，因为它迅速地将连杆恢复到了平衡的位置。也可以说组 3 是最优的，因为它的总输入最小，意味着它的能耗最小。从性能的角度看，组 1 是最好的。从节能的角度看，组 3 则是最佳。作为最优化算法工程师，根据不同的应用场景去权衡（Tradeoff）权重系数，是非常重要的工作。

上述控制器的设计始终是围绕在平衡点 z f = 0 , 0 T \boldsymbol{z}{\text{f}} = 0, 0^{\text{T}} zf=0,0T 进行分析的。但是如果在实际情况中，设计的目标是将系统的状态稳定在一个非零的目标值 z d = z 1 d , z 2 d T \boldsymbol{z}{\text{d}} = z_{1\\text{d}}, z_{2\\text{d}}^{\text{T}} zd=z1d,z2dT，（例如令连杆小球追随一定的轨迹运动）又应该如何去设计控制器呢？接下来介绍轨迹追踪。

3.2 轨迹追踪

设计 u ( t ) u(t) u(t) 使得系统的状态稳定在一个非零的目标值 z d \boldsymbol{z}_{\text{d}} zd。此时要求控制量 u ( t ) u(t) u(t)（即动态系统的输入）承担两个功能：

• 第一，改变系统的平衡点位置到 z d \boldsymbol{z}_{\text{d}} zd。
• 第二，令 z d \boldsymbol{z}_{\text{d}} zd 成为一个稳定的平衡点。

首先引入误差，令 e ( t ) = z d − z ( t ) = z 1 d − z 1 ( t ) z 2 d − z 2 ( t ) = e 1 ( t ) e 2 ( t ) (10.3.19) \boldsymbol{e}(t) = \boldsymbol{z}{\text{d}} - \boldsymbol{z}(t) = \begin{bmatrix} z{1\text{d}} - z_1(t) \\ z_{2\text{d}} - z_2(t) \end{bmatrix} = \begin{bmatrix} e_1(t) \\ e_2(t) \end{bmatrix} \tag{10.3.19} e(t)=zd−z(t)=z1d−z1(t)z2d−z2(t)=e1(t)e2(t)(10.3.19)此时的设计目标就变成了：① 令 e ( t ) \boldsymbol{e}(t) e(t) 的平衡点为 e f = 0 , 0 T \boldsymbol{e}{\text{f}} = 0, 0^{\text{T}} ef=0,0T；② e f = 0 , 0 T \boldsymbol{e}{\text{f}} = 0, 0^{\text{T}} ef=0,0T 是稳定的平衡点。

假设目标值 z 1 d z_{1\text{d}} z1d 和 z 2 d z_{2\text{d}} z2d 都是常数（或者变化缓慢），因此 d z 1 d d t = d z 2 d d t = 0 \frac{\mathrm{d}z_{1\text{d}}}{\mathrm{d}t} = \frac{\mathrm{d}z_{2\text{d}}}{\mathrm{d}t} = 0 dtdz1d=dtdz2d=0。可得 d e ( t ) d t = d z d d t − d z ( t ) d t = − d z ( t ) d t = − A z ( t ) − B u ( t ) (10.3.20) \frac{\mathrm{d}\boldsymbol{e}(t)}{\mathrm{d}t} = \frac{\mathrm{d}\boldsymbol{z}{\text{d}}}{\mathrm{d}t} - \frac{\mathrm{d}\boldsymbol{z}(t)}{\mathrm{d}t} = -\frac{\mathrm{d}\boldsymbol{z}(t)}{\mathrm{d}t} = -\boldsymbol{A}\boldsymbol{z}(t) - \boldsymbol{B}u(t) \tag{10.3.20} dtde(t)=dtdzd−dtdz(t)=−dtdz(t)=−Az(t)−Bu(t)(10.3.20)将式 (10.3.19) 和 A = 0 1 g d 0 , B = 0 1 \boldsymbol{A} = \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix}, \boldsymbol{B} = \begin{bmatrix} 0 \\ 1 \end{bmatrix} A=0dg10,B=01 代入式 (10.3.20)，得到 d e ( t ) d t = − A ( z d − e ( t ) ) − B u ( t ) = A e ( t ) − A z d − B u ( t ) ⇒ d e ( t ) d t = 0 1 g d 0 e 1 ( t ) e 2 ( t ) − 0 1 g d 0 z 1 d z 2 d − 0 1 u ( t ) (10.3.21) \begin{aligned} \frac{\mathrm{d}\boldsymbol{e}(t)}{\mathrm{d}t} &= -\boldsymbol{A}(\boldsymbol{z}{\text{d}} - \boldsymbol{e}(t)) - \boldsymbol{B}u(t) = \boldsymbol{A}\boldsymbol{e}(t) - \boldsymbol{A}\boldsymbol{z}{\text{d}} - \boldsymbol{B}u(t) \\ &\Rightarrow \frac{\mathrm{d}\boldsymbol{e}(t)}{\mathrm{d}t} = \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix} \begin{bmatrix} e_1(t) \\ e_2(t) \end{bmatrix} - \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix} \begin{bmatrix} z{1\text{d}} \\ z_{2\text{d}} \end{bmatrix} - \begin{bmatrix} 0 \\ 1 \end{bmatrix} u(t) \end{aligned} \tag{10.3.21} dtde(t)=−A(zd−e(t))−Bu(t)=Ae(t)−Azd−Bu(t)⇒dtde(t)=0dg10e1(t)e2(t)−0dg10z1dz2d−01u(t)(10.3.21)展开得到 d e 1 ( t ) d t = e 2 ( t ) − z 2 d (10.3.22a) \frac{\mathrm{d}e_1(t)}{\mathrm{d}t} = e_2(t) - z_{2\text{d}} \tag{10.3.22a} dtde1(t)=e2(t)−z2d(10.3.22a) d e 2 ( t ) d t = g d e 1 ( t ) − g d z 1 d − u ( t ) (10.3.22b) \frac{\mathrm{d}e_2(t)}{\mathrm{d}t} = \frac{g}{d}e_1(t) - \frac{g}{d}z_{1\text{d}} - u(t) \tag{10.3.22b} dtde2(t)=dge1(t)−dgz1d−u(t)(10.3.22b)观察式 (10.3.22) 可以发现，本例中 e 2 ( t ) e_2(t) e2(t) 的平衡点是无法通过输入改变的，因为无论 u ( t ) u(t) u(t) 如何选择，都无法作用在式 (10.3.22a) 上，当式 (10.3.22a) 中的 d e 1 ( t ) d t = 0 \frac{\mathrm{d}e_1(t)}{\mathrm{d}t} = 0 dtde1(t)=0 时，平衡点 e 2 f = z 2 d e_{2\text{f}} = z_{2\text{d}} e2f=z2d，这说明只有在 z 2 d = 0 z_{2\text{d}} = 0 z2d=0 时，误差的平衡点 e 2 f = 0 e_{2\text{f}} = 0 e2f=0，意味着状态变量 z 2 ( t ) z_2(t) z2(t) 的唯一平衡点为 0。

但是 z 1 ( t ) z_1(t) z1(t) 不一样，它的平衡点的位置是可以通过输入 u ( t ) u(t) u(t) 改变的。令 u ( t ) = F z d + K e e ( t ) u(t) = \boldsymbol{F}\boldsymbol{z}{\text{d}} + \boldsymbol{K}e \boldsymbol{e}(t) u(t)=Fzd+Kee(t)，其中 F z d \boldsymbol{F}\boldsymbol{z}{\text{d}} Fzd 部分称为前馈（Feedforward），用来将平衡点 z 1 f z{1\text{f}} z1f 移动到 z 1 d z_{1\text{d}} z1d。 K e e ( t ) \boldsymbol{K}e \boldsymbol{e}(t) Kee(t) 项用来配置极点，使得系统稳定，其中 K e = k e 1 , k e 2 \boldsymbol{K}e = k_{e1}, k_{e2} Ke=ke1,ke2。根据上述分析，平衡点位置为 z 1 d , 0 T z_{1\\text{d}}, 0^{\text{T}} z1d,0T。令 u ( t ) = F z d + K e e ( t ) = − g d 0 z 1 d 0 + K e e ( t ) (10.3.23) u(t) = \boldsymbol{F}\boldsymbol{z}{\text{d}} + \boldsymbol{K}e \boldsymbol{e}(t) = \begin{bmatrix} -\frac{g}{d} & 0 \end{bmatrix} \begin{bmatrix} z{1\text{d}} \\ 0 \end{bmatrix} + \boldsymbol{K}e \boldsymbol{e}(t) \tag{10.3.23} u(t)=Fzd+Kee(t)=−dg0z1d0+Kee(t)(10.3.23)将其代入式 (10.3.21)，得到 d e ( t ) d t = 0 1 g d 0 e 1 ( t ) e 2 ( t ) − 0 1 g d 0 z 1 d 0 − 0 1 ( − g d 0 z 1 d 0 + k e1 k e2 e 1 ( t ) e 2 ( t ) ) \begin{aligned} \frac{\mathrm{d}\boldsymbol{e}(t)}{\mathrm{d}t} = &\begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix} \begin{bmatrix} e_1(t) \\ e_2(t) \end{bmatrix} - \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix} \begin{bmatrix} z{1\text{d}} \\ 0 \end{bmatrix} - \\ &\begin{bmatrix} 0 \\ 1 \end{bmatrix} \left( \begin{bmatrix} -\frac{g}{d} & 0 \end{bmatrix} \begin{bmatrix} z{1\text{d}} \\ 0 \end{bmatrix} + \begin{bmatrix} k_{\text{e1}} & k_{\text{e2}} \end{bmatrix} \begin{bmatrix} e_1(t) \\ e_2(t) \end{bmatrix} \right) \end{aligned} dtde(t)=0dg10e1(t)e2(t)−0dg10z1d0−01(−dg0z1d0+ke1ke2e1(t)e2(t)) = 0 1 g d 0 e 1 ( t ) e 2 ( t ) − 0 0 k e1 k e2 e 1 ( t ) e 2 ( t ) = 0 1 g d − k e1 − k e2 e 1 ( t ) e 2 ( t ) \begin{aligned} &= \begin{bmatrix} 0 & 1 \\ \frac{g}{d} & 0 \end{bmatrix} \begin{bmatrix} e_1(t) \\ e_2(t) \end{bmatrix} - \begin{bmatrix} 0 & 0 \\ k_{\text{e1}} & k_{\text{e2}} \end{bmatrix} \begin{bmatrix} e_1(t) \\ e_2(t) \end{bmatrix} \\ &= \begin{bmatrix} 0 & 1 \\ \frac{g}{d} - k_{\text{e1}} & -k_{\text{e2}} \end{bmatrix} \begin{bmatrix} e_1(t) \\ e_2(t) \end{bmatrix} \end{aligned} =0dg10e1(t)e2(t)−0ke10ke2e1(t)e2(t)=0dg−ke11−ke2e1(t)e2(t)

此时，误差状态变量的平衡点为 e f = 0 , 0 T \boldsymbol{e}{\text{f}} = 0, 0^{\text{T}} ef=0,0T。接下来的工作就是设计 K e \boldsymbol{K}{\text{e}} Ke，使得闭环状态矩阵 0 1 g d − k e1 − k e2 \begin{bmatrix} 0 & 1 \\ \frac{g}{d} - k_{\text{e1}} & -k_{\text{e2}} \end{bmatrix} 0dg−ke11−ke2 的特征值实部都小于 0 就可以了。当然也可以使用 LQR 的方法来确定 K e \boldsymbol{K}_{\text{e}} Ke，此处不再赘述。图10.3.6描述了带有前馈的线性状态反馈控制器设计

4. Python仿真代码

4.1 仿真代码

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.linalg import solve_continuous_are

# 设置绘图风格
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']  # 正常显示中文
plt.rcParams['axes.unicode_minus'] = False  # 正常显示负号

# ==========================================
# 1. 系统参数定义与建模 (对应 2.1 - 2.3 节)
# ==========================================
g = 9.81
d = 0.981
g_over_d = g / d  # g/d = 10.0

# 状态空间矩阵: z = [phi, phi_dot]^T
A = np.array([[0, 1], 
              [g_over_d, 0]])
B = np.array([[0], 
              [1]])
C = np.array([[1, 0]])
D = np.array([[0]])

# 能控性分析
Q_c = np.block([B, A @ B])
rank_Qc = np.linalg.matrix_rank(Q_c)
print(f"--- 1. 系统建模与能控性分析 ---")
print(f"矩阵 A:\n{A}")
print(f"矩阵 B:\n{B}")
print(f"能控性矩阵 Qc:\n{Q_c}")
print(f"Rank(Qc) = {rank_Qc}, 系统{'完全状态能控' if rank_Qc == 2 else '不能控'}\n")

# ==========================================
# 2. LQR 控制器设计与三组对比仿真 (对应 3.1 节表 10.3.1)
# ==========================================
def solve_lqr(A, B, Q, R):
    """求解连续时间代数里卡蒂方程 (CARE) 并计算反馈增益 K"""
    P = solve_continuous_are(A, B, Q, R)
    K = np.linalg.inv(R) @ B.T @ P
    return K

# 表 10.3.1 中的三组测试权重
cases = {
    "组 1 (强调 z1)": {"Q": np.diag([100, 1]), "R": np.array([[1]])},
    "组 2 (强调 z2)": {"Q": np.diag([1, 100]), "R": np.array([[1]])},
    "组 3 (强调 u)":  {"Q": np.diag([1, 1]),   "R": np.array([[100]])}
}

# 初始条件: z1(0) = pi/20, z2(0) = 0
z0 = np.array([np.pi / 20, 0.0])
t_span_lqr = (0, 5)
t_eval_lqr = np.linspace(t_span_lqr[0], t_span_lqr[1], 500)

plt.figure(figsize=(12, 9))

print("--- 2. LQR 反馈增益矩阵 K ---")
for name, cfg in cases.items():
    Q = cfg["Q"]
    R = cfg["R"]
    K = solve_lqr(A, B, Q, R)
    print(f"{name}: K = [{K[0, 0]:.4f}, {K[0, 1]:.4f}]")

    # 闭环微分方程 dz/dt = (A - B*K)*z
    def lqr_system(t, z, K_val):
        return (A - B @ K_val) @ z

    sol = solve_ivp(lqr_system, t_span_lqr, z0, args=(K,), t_eval=t_eval_lqr)
    
    # 计算控制输入 u(t)
    u_hist = - (K @ sol.y).flatten()

    # 绘制 z1(t)
    plt.subplot(3, 1, 1)
    plt.plot(sol.t, sol.y[0], label=f"{name}")
    plt.ylabel("$z_1(t) = \\phi(t)$ [rad]")
    plt.grid(True)
    plt.title("LQR 控制下不同权重的系统响应对比 (图 10.3.4 仿真)")

    # 绘制 z2(t)
    plt.subplot(3, 1, 2)
    plt.plot(sol.t, sol.y[1], label=f"{name}")
    plt.ylabel("$z_2(t) = \\dot{\\phi}(t)$ [rad/s]")
    plt.grid(True)

    # 绘制 u(t)
    plt.subplot(3, 1, 3)
    plt.plot(sol.t, u_hist, label=f"{name}")
    plt.ylabel("$u(t)$ (控制量)")
    plt.xlabel("时间 $t$ [s]")
    plt.grid(True)

plt.subplot(3, 1, 1); plt.legend()
plt.subplot(3, 1, 2); plt.legend()
plt.subplot(3, 1, 3); plt.legend()
plt.tight_layout()
plt.show()

# ==========================================
# 3. 分段阶跃轨迹追踪仿真 (对应 3.2 节)
# ==========================================
# 定义 0~40s 的分段目标轨迹 phi_d(t)
def get_phi_d(t):
    if 0 <= t < 10:
        return np.pi / 20
    elif 10 <= t < 20:
        return 0.0
    elif 20 <= t < 30:
        return -np.pi / 20
    else:  # 30 ~ 40s
        return 0.0

# 反馈控制增益 (采用响应较快的组 1 增益)
Ke = solve_lqr(A, B, np.diag([100, 1]), np.array([[1]]))

def tracking_system(t, z):
    phi_d = get_phi_d(t)
    # 目标向量: 期望角速度为 0
    z_d = np.array([phi_d, 0.0])
    
    # 前馈控制项 u_ff = -(g/d) * phi_d (式 10.3.23)
    u_ff = -g_over_d * phi_d
    
    # 状态误差反馈项 u_fb = Ke * (z_d - z)
    e = z_d - z
    u_fb = (Ke @ e)[0]
    
    # 总控制量
    u = u_ff + u_fb
    
    dz1 = z[1]
    dz2 = g_over_d * z[0] + u
    return [dz1, dz2]

# 仿真时间 0 ~ 40s
t_span_track = (0, 40)
t_eval_track = np.linspace(t_span_track[0], t_span_track[1], 2000)
z0_track = np.array([0.0, 0.0])  # 从初始位置 0 开始追踪

sol_track = solve_ivp(tracking_system, t_span_track, z0_track, t_eval=t_eval_track, method='RK45')

# 计算参考轨迹点阵与控制量历史
phi_d_vals = np.array([get_phi_d(t) for t in sol_track.t])
u_track_vals = -g_over_d * phi_d_vals + Ke[0, 0] * (phi_d_vals - sol_track.y[0]) + Ke[0, 1] * (0.0 - sol_track.y[1])

# 绘制轨迹追踪图
plt.figure(figsize=(12, 6))

plt.subplot(2, 1, 1)
plt.plot(sol_track.t, phi_d_vals, 'r--', label="目标轨迹 $\\phi_d(t)$", linewidth=2)
plt.plot(sol_track.t, sol_track.y[0], 'b-', label="实际角度 $\\phi(t)$", linewidth=1.5)
plt.ylabel("角度 $\\phi$ [rad]")
plt.title("带有前馈+状态反馈的 0~40s 分段阶跃轨迹追踪 (图 10.3.6 仿真)")
plt.grid(True)
plt.legend()

plt.subplot(2, 1, 2)
plt.plot(sol_track.t, u_track_vals, 'g-', label="控制量 $u(t)$", linewidth=1.2)
plt.xlabel("时间 $t$ [s]")
plt.ylabel("控制量 $u(t)$")
plt.grid(True)
plt.legend()

plt.tight_layout()
plt.show()

# ==========================================
# 4. 开环与闭环相轨迹绘制 (对应图 10.3.1 与 图 10.3.2)
# ==========================================
z1_vals = np.linspace(-1, 1, 20)
z2_vals = np.linspace(-1, 1, 20)
Z1, Z2 = np.meshgrid(z1_vals, z2_vals)

fig, axes = plt.subplots(1, 2, figsize=(12, 5))

# 1) 开环无控制相轨迹 (鞍点)
dZ1_open = Z2
dZ2_open = g_over_d * Z1
axes[0].streamplot(Z1, Z2, dZ1_open, dZ2_open, color='b')
axes[0].set_title("开环系统相轨迹 (鞍点/不稳定，图 10.3.1)")
axes[0].set_xlabel("$z_1$ (角度)")
axes[0].set_ylabel("$z_2$ (角速度)")
axes[0].grid(True)

# 2) 极点配置闭环相轨迹 (特征值 lambda = -1)
dZ1_cl = Z2
dZ2_cl = -1 * Z1 - 2 * Z2
axes[1].streamplot(Z1, Z2, dZ1_cl, dZ2_cl, color='g')
axes[1].set_title("极点配置闭环系统相轨迹 ($\lambda_{1,2}=-1$，稳定节点，图 10.3.2)")
axes[1].set_xlabel("$z_1$ (角度)")
axes[1].set_ylabel("$z_2$ (角速度)")
axes[1].grid(True)

plt.tight_layout()
plt.show()

4.2 仿真结果

--- 1. 系统建模与能控性分析 ---
矩阵 A:
[[ 0.  1.]
 [10.  0.]]
矩阵 B:
[[0]
 [1]]
能控性矩阵 Qc:
[[0. 1.]
 [1. 0.]]
Rank(Qc) = 2, 系统完全状态能控

--- 2. LQR 反馈增益矩阵 K ---
组 1 (强调 z1): K = [24.1421, 7.0203]
组 2 (强调 z2): K = [20.0499, 11.8364]
组 3 (强调 u): K = [20.0005, 6.3254]

参考：控制之美（卷1）

以上个人整理，难免有误。若发现错误，欢迎在评论区留言指正，非常感谢！