从拉格朗日动力学到 LQR 控制：完整理论推导

记录从广义坐标选取、拉格朗日建模、平衡点线性化、状态空间建立，到哈密顿矩阵法求解 CARE 方程、计算 LQR 反馈增益 $K$ 的完整理论链路。以底部铰接匀质杆为简单实例，公式逐步展开。

---

目录

1. 广义坐标的选取：从约束与自由度出发
2. 拉格朗日动力学建模
3. 平衡点线性化
4. 状态空间表示
5. LQR 最优控制问题描述
6. 哈密顿矩阵法解 CARE 方程
7. 数值实例与 Python 实现
8. 总结与调参备忘

---

1. 广义坐标的选取：从约束与自由度出发

1.1 完整约束与自由度

对于由 $N$ 个质点组成的系统，在三维空间中共有 $3N$ 个直角坐标。若受到 $m$ 个完整约束（几何约束，可积分为仅含坐标和时间的方程 $f(\mathbf{r},t)=0$，r为质点的坐标），则系统的自由度为：

$$n=3N-m$$

当系统为平面机构时，可采用$n=3N（不含机架）-2p_l-p_h$ 快速计算（$p_l$ 为低副数，$p_h$ 为高副数），典型为机械臂

广义坐标的数目必须等于自由度 $n$。选取一组相互独立、且能完整描述系统位形的变量 $q_1,q_2,\dots ,q_n$，使得所有直角坐标均可表示为广义坐标的函数：

$${\mathbf{r}}_i={\mathbf{r}}_i(q_1,q_2,\dots ,q_n,t)$$

约束一定是要不冗余的，即若一个约束可由其他约束推导出来，则它不独立，不能计入 $m$。完整约束又分为时变与不变的，$f(\mathbf{r},t)=0$均为完整约束

1.2 选取原则

1. 独立性：各广义坐标之间不存在函数关系，即满足约束后彼此独立变化。
2. 完备性：给定一组广义坐标的数值，能唯一确定系统在该时刻的空间位形。
3. 便利性：优先选取与实际几何运动直接对应的变量，使动能 $T$ 和势能 $V$ 的表达式最简。

1.3 示例：底部铰接的匀质杆

考虑一根质量为 $m$、长度为 $l$ 的匀质杆，一端通过铰链固定于支点，在竖直平面内转动。

若用直角坐标描述杆上各点，需引入大量坐标与约束（杆长固定、端点固定）。但直接取杆与竖直向上方向的夹角 $\theta$ 作为广义坐标，即可完整描述位形，且动能、势能表达式简洁。

系统仅 1 个自由度，广义坐标取：

$$q=\theta$$

---

2. 拉格朗日动力学建模

2.1 动能与势能

以底部铰接匀质杆为例，绕端点转动惯量 $I=\frac{1}{3}m{l}^2$。

动能：

$$T=\frac{1}{2}I{\dot{\theta }}^2$$

势能（以支点水平面为零点，质心在杆中点）：

$$V=mg\frac{l}{2}\cos \theta$$

2.2 拉格朗日量

$$\mathcal{L}=T-V=\frac{1}{2}I{\dot{\theta }}^2-mg\frac{l}{2}\cos \theta$$

2.3 欧拉-拉格朗日方程

广义坐标 $q=\theta$，广义力为控制力矩 $\tau$：

$$\frac{d}{dt}\left(\frac{\partial \mathcal{L}}{\partial \dot{\theta }}\right)-\frac{\partial \mathcal{L}}{\partial \theta }=\tau$$

计算各项：

• $\frac{\partial \mathcal{L}}{\partial \dot{\theta }}=I\dot{\theta }$
• $\frac{d}{dt}(\cdot )=I\ddot{\theta }$
• $\frac{\partial \mathcal{L}}{\partial \theta }=mg\frac{l}{2}\sin \theta$

得到非线性动力学方程：

$$I\ddot{\theta }-mg\frac{l}{2}\sin \theta =\tau \text{\hspace{1em}(1)}$$

符号说明：当 $\theta >0$（杆向右偏）时，重力矩 $mg\frac{l}{2}\sin \theta$ 使杆向右加速（$\ddot{\theta }>0$），故重力矩项取正号。

---

3. 平衡点线性化

目标：使杆稳定在竖直向上位置（$\theta =0$）。在平衡点附近小角度近似：

$$\sin \theta \approx \theta$$

代入 (1)：

$$I\ddot{\theta }-mg\frac{l}{2}\theta =\tau \text{\hspace{1em}(2)}$$

这是线性化的动力学方程。注意 $\theta >0$（杆向右偏）时，重力矩 $mg\frac{l}{2}\theta$ 使杆继续偏离，说明倒立平衡是不稳定的。

---

4. 状态空间表示

取状态变量：

• $x_1=\theta$（角度）
• $x_2=\dot{\theta }$（角速度）

控制输入 $u=\tau$（力矩）。

由 (2) 得状态方程：

$$\{\begin{array}{l}{\dot{x}}_1=x_2\\ {\dot{x}}_2=\frac{mgl}{2I}{x}_1+\frac{1}{I}u\end{array}$$

重力矩项与 θ 同号，使角加速度进一步增大，体现开环不稳定性。
写成 $\dot{\mathbf{x}}=A\mathbf{x}+Bu$ 形式。为简化书写，定义：

$$a=\frac{mgl}{2I},b=\frac{1}{I}$$

则系统矩阵为：

$$A=\left(\begin{array}{cc}0& 1\\ a& 0\end{array}\right),B=\left(\begin{array}{c}0\\ b\end{array}\right)$$

---

5. LQR 最优控制问题描述

对于线性时不变系统 $\dot{\mathbf{x}}=A\mathbf{x}+Bu$，LQR 寻找使如下二次性能指标最小化的控制律：

$$J={\int }_0^{\infty }\left({\mathbf{x}}^{\top }Q\mathbf{x}+R{u}^2\right)dt$$

其中：

• $Q$ 为 $2\times 2$ 半正定状态权重矩阵，通常取对角阵。
• $R>0$ 为控制权重标量，$R$ 越大表示越"省力"，收敛越慢。

最优控制律的形式：

$$u=-K\mathbf{x}$$

其中反馈增益矩阵：

$$K=R^{-1}{B}^{\top }P\text{\hspace{1em}(3)}$$

而 $P$ 是连续时间代数 Riccati 方程（黎卡提方程简称为CARE）的唯一半正定解：

$$A^{\top }P+PA-PB{R}^{-1}{B}^{\top }P+Q=0\text{\hspace{1em}(4)}$$

---

6. 哈密顿矩阵法解 CARE 方程

6.1 构造哈密顿矩阵

定义 $S=B{R}^{-1}{B}^{\top }$（$2\times 2$ 矩阵），构造 $4\times 4$ 的哈密顿矩阵：

$$H=\left(\begin{array}{cc}A& -S\\ -Q& -A^{\top }\end{array}\right)\text{\hspace{1em}(5)}$$

6.2 关键性质

哈密顿矩阵 $H$ 的特征值关于虚轴对称分布：恰好有 $n$ 个（此处 $n=2$）特征值位于左半平面（稳定），$n$ 个位于右半平面（不稳定）。

6.3 算法步骤

Step 1：对 $H$ 进行特征值分解：

$$H\left(\begin{array}{c}{V}_1\\ V_2\end{array}\right)=\left(\begin{array}{c}{V}_1\\ V_2\end{array}\right)\Lambda$$

其中 $\Lambda$ 为特征值对角阵，$\left(\begin{array}{c}{V}_1\\ V_2\end{array}\right)$ 为对应的 $4\times 4$ 特征向量矩阵（分块上、下各 $2$ 行）。

Step 2：筛选稳定特征值。选取 $H$ 的 $2$ 个具有负实部的特征值，对应特征向量组成 $4\times 2$ 矩阵，分块为 $V_1^{(s)}$（上半，$2\times 2$）和 $V_2^{(s)}$（下半，$2\times 2$）。

Step 3：若 $V_1^{(s)}$ 可逆，则 CARE 的解为：

$$P=V_2^{(s)}(V_1^{(s)}{)}^{-1}\text{\hspace{1em}(6)}$$

Step 4：代回 (3) 计算增益 $K$：

$$K=R^{-1}{B}^{\top }P$$

闭环系统矩阵：$A_{\mathrm{cl}}=A-BK$，其特征值恰好就是 $H$ 的那 $2$ 个稳定特征值。

哈密顿矩阵特征值有反对称性，即在实矩阵下，若λ为特征值，-λ，λ*（共轭），-λ* 均为特征值

---

7. 数值实例与 Python 实现

以下给出可直接运行的 Python 代码，同时展示"直接调用求解器"与"手动哈密顿矩阵法"两种途径。

import numpy as np
from scipy.linalg import solve_continuous_are, eig

# ==================== 1. 物理参数：底部铰接匀质杆 ====================
m = 1.0          # 质量 [kg]
l = 1.0          # 长度 [m]
g = 9.81         # 重力加速度 [m/s^2]
I = (1.0 / 3.0) * m * l**2   # 绕端点转动惯量

# ==================== 2. 状态空间矩阵 ====================
a = m * g * l / (2.0 * I)
b = 1.0 / I

A = np.array([[0, 1],
              [a, 0]])
n = A.shape[0]   # 状态维数，此处为2

B = np.array([[0],
              [b]])
              

# ==================== 3. LQR 权重 ====================
Q = np.diag([10, 1])   # 惩罚角度、角速度
R = np.array([[1]])      # 控制力矩权重

# ==================== 方法 A: 直接求解 CARE ====================
P_direct = solve_continuous_are(A, B, Q, R)
K_direct = (1.0 / R[0, 0]) * B.T @ P_direct

print("===== 方法 A: scipy 直接求解 =====")
print("P =\n", P_direct)
print("K =", K_direct.round(4))

# ==================== 方法 B: 哈密顿矩阵法 ====================
S = B @ (1.0 / R) @ B.T

H = np.block([[A, -S],
              [-Q, -A.T]])

# 特征值分解	
eigvals, eigvecs = eig(H)

# 选取左半平面特征值 (稳定)
stable_mask = np.real(eigvals) < 0
V_stable = eigvecs[:, stable_mask]   # 直接取，形状 (4, n)
V1 = V_stable[:n, :]   # 上半块
V2 = V_stable[n:, :]   # 下半块

P_ham = V2 @ np.linalg.inv(V1)
K_ham = (1.0 / R[0, 0]) * B.T @ P_ham

print("\n===== 方法 B: 哈密顿矩阵法 =====")
stable_ev = eigvals[stable_mask]
print("稳定特征值:", np.round(stable_ev, 4))
print("P =\n", P_ham)
print("K =", K_ham.round(4))

# ==================== 4. 闭环验证 ====================
A_cl = A - B @ K_direct
eig_cl = np.linalg.eigvals(A_cl)
print("\n===== 闭环极点 =====")
print("闭环特征值:", np.round(eig_cl, 4))

典型输出解释

• $K$ 矩阵：$1\times 2$ 向量。$k_1$ 为正且较大，意味着当 $\theta >0$（杆右偏）时，控制量 $u=-K\mathbf{x}$ 产生负力矩，使杆向左回正。
• 闭环特征值：2 个复数（或实数）均具有负实部，且恰好等于哈密顿矩阵 $H$ 左半平面的 2 个特征值。

---

8. 总结与调参备忘

阶段	核心公式/操作	物理/数学意义
广义坐标选取	$n=3N-m$，独立、完备、便利	从约束与自由度出发，避免冗余坐标
拉格朗日建模	$\mathcal{L}=T-V$，代入欧拉-拉格朗日方程	从能量出发，避免繁琐受力分析
线性化	小角度近似 $\sin \theta \approx \theta$	在目标平衡点附近用线性系统逼近，复杂模型可采用泰勒一阶展开
状态空间	$\dot{\mathbf{x}}=A\mathbf{x}+Bu$	高阶 ODE（常微分方程） 转为一阶矩阵形式
LQR 指标	$J=\int ({\mathbf{x}}^{\top }Q\mathbf{x}+R{u}^2)dt$	二次型权衡收敛速度与控制代价
CARE	$A^{\top }P+PA-PB{R}^{-1}{B}^{\top }P+Q=0$	最优控制核心代数约束
哈密顿矩阵	$H=\left(\begin{array}{cc}A& -S\\ -Q& -A^{\top }\end{array}\right)$	将 Riccati 方程求解转化为特征值问题
增益 $K$	$K=R^{-1}{B}^{\top }P$	状态反馈映射，闭环极点即 $H$ 稳定特征值

$Q$ 对角元增大 $\to$ 对应状态收敛更快
$R$ 增大 $\to$ 控制更保守、更"柔"、收敛更慢
$Q$ 与 $R$ 的数量级差异决定系统整体响应性格
“闭环极点即 H 稳定特征值”是建立在 (A,B) 可镇定且 Q 半正定、 R 正定的前提下，对于这个例子成立

---