150_机器人系统辨识
博士前数学路线图 · 第三批 · 专题 3.15：机器人系统辨识 (System Identification for Robot Dynamics)¶
档位：核心档位 3（博士入学）＋ 进阶档位 4（博士毕业＋） 建议时长：档位 3 约 18–22 h；档位 4 额外 10–15 h 前置：控制理论/50_LQR_LQG与Riccati方程；刚体动力学/20_Lagrange与Hamilton力学；纯数学基础/20_向量空间与线性变换 (线性代数/SVD) 下游：控制理论/120_MPC数值求解与实时实现；控制理论/90_DDP_iLQR原理与实现；刚体动力学/50_动力学解析微分；控制理论·自适应控制（规划中）（辨识结果直接驱动自适应律）
0. 前置自测¶
在开始本章之前，请确认你能回答以下问题。如果某题卡住，先回到对应专题补课。
| 编号 | 自测题 | 对应前置 |
|---|---|---|
| Q0.1 | 写出刚体机器人关节空间运动方程 \(M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau\) 中每一项的物理含义和矩阵维度 | 刚体动力学/20_Lagrange与Hamilton力学 |
| Q0.2 | 给定超定方程组 \(Ax = b\)，写出 OLS 解的闭式表达式，说明何时解唯一 | 纯数学基础/20_向量空间与线性变换 |
| Q0.3 | 什么是 SVD？对 \(A \in \mathbb{R}^{m \times n}\) 写出 SVD 分解形式，说明奇异值的几何意义 | 纯数学基础/20_向量空间与线性变换 |
| Q0.4 | LQR 闭环需要哪些模型参数？这些参数从哪里来？ | 控制理论/50_LQR_LQG与Riccati方程 |
| Q0.5 | 用 Lagrange 方程推导单摆的运动方程 | 刚体动力学/20_Lagrange与Hamilton力学 |
如果你对 Q0.1--Q0.3 都很熟悉，可以直接进入正文。
1. 本章目标¶
学完本章后，你将能够：
- 对任意串联机器人，将运动方程写成**线性回归形式** \(Y(q,\dot{q},\ddot{q})\pi = \tau\)
- 识别**基参数**（base parameters），知道哪些惯性参数可辨识、哪些不可辨识
- 设计**激励轨迹**，使辨识精度最大化（Fourier 参数化 + D-最优准则）
- 用 OLS/WLS 从实验数据中提取动力学参数，并给出参数的**置信区间**
- 理解**子空间辨识**方法，在不知道模型结构时辨识状态空间模型
- 实现**在线递归辨识**（RLS），对运行中的机器人实时更新参数
- 区分白箱/灰箱/黑箱方法，设计 sim-to-real 辨识流水线
- 用 Pinocchio/Drake 完成完整的辨识实验
- 理解**频域辨识**方法（Bode 图辨识、多正弦激励），并与时域方法对比
- 将辨识结果无缝对接到**自适应控制**（控制理论·自适应控制（规划中））中
2. 为什么需要系统辨识 ⭐¶
2.1 动机：CAD 参数不靠谱¶
你刚组装好一台 7-DOF 机械臂，CAD 模型告诉你每个连杆的质量、质心位置和惯性张量。你兴冲冲地把这些参数填入 MPC 控制器，结果——
- 末端执行器抖动 ±5 cm，无法稳定抓取
- 抓起 1 kg 负载后轨迹跟踪误差暴涨 300%
- 低速运动时关节"粘滞"，高速时超调
这不是控制器的问题，而是模型的问题。工业实践表明：
| 参数来源 | 质量误差 | 质心误差 | 惯性矩误差 | 摩擦参数 |
|---|---|---|---|---|
| CAD 标称值 | 5–15% | 10–30% | 20–50% | 完全未知 |
| 系统辨识后 | < 2% | < 5% | < 10% | 已知 |
为什么 CAD 参数不准？ 原因很多：(1) CAD 模型忽略线缆、润滑脂、紧固件等附加质量；(2) 加工公差导致实际几何偏离设计值；(3) 减速器内部惯量通常不在 URDF 中；(4) 摩擦模型（Coulomb + viscous + Stribeck）在 CAD 中完全不存在。
2.2 反面案例：sim-to-real gap¶
假设你在 MuJoCo 仿真器中用 CAD 参数训练了一个 RL 策略，直接部署到真实机器人上。会发生什么？
- 仿真中：策略学会了利用精确的惯性矩来做"甩臂"运动，节省力矩
- 真实中：惯性矩偏差 30%，力矩不够 → 末端到不了目标位置 → 碰撞
- 更糟的情形：摩擦力在仿真中不存在 → 策略学会了"瞬间反向"以节能 → 真实中被 Coulomb 摩擦卡死
这就是 sim-to-real gap。系统辨识的目标之一，就是**让仿真模型尽可能逼近真实**，从根源上缩小这个 gap。
2.3 历史：线性参数化的发现¶
系统辨识是一个古老而庞大的领域（Ljung 的经典教材 1987 年出版），但**机器人动力学参数辨识**的现代理论始于 1986 年的一篇关键论文：
Atkeson, An, and Hollerbach, "Estimation of Inertial Parameters of Manipulator Loads and Links," International Journal of Robotics Research, 5(3):101--119, 1986.
这篇论文的核心发现：虽然刚体动力学方程 \(M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau\) 对 \(q\) 高度非线性，但对惯性参数 \(\pi\)（质量、质心位置、惯性矩）是线性的。这意味着可以写成
其中 \(Y\) 是一个只依赖运动学量的**回归矩阵**（regressor），\(\pi\) 是待辨识的**惯性参数向量**。这个"线性参数化"发现是一切后续工作的基石——因为线性问题可以用最小二乘精确求解。
后续里程碑： - Khalil & Dombre (1986)：提出基参数概念 - Gautier (1991)：系统地研究了可辨识性和基参数的数值计算 - Swevers et al. (1997)：优化激励轨迹设计 - Atkeson, An, Hollerbach (1986) + Khosla & Kanade (1985)：奠定了整个方法论框架
2.4 系统辨识与自适应控制的深度联系¶
为什么要在这里提 控制理论·自适应控制（规划中）？ 因为系统辨识和自适应控制是一枚硬币的两面：
- 辨识：先收集数据，再离线/在线估计参数，然后用估计出的参数设计控制器
- 自适应控制：边控制边辨识——控制律中直接嵌入参数估计，两者同时运行
经典的 Slotine & Li (1987) 自适应控制律为：
其中 \(s = \dot{q} - \dot{q}_r\) 是滑模面变量，\(\hat{\pi}\) 是在线参数估计，参数更新律为：
注意：这里用到了**同一个回归矩阵 \(Y\)**！本章学到的回归矩阵构造方法，在 控制理论·自适应控制（规划中） 中会直接复用。辨识质量直接决定了自适应控制的收敛速度和稳态精度。
⭐ 自测检查点 2.1：解释为什么"对惯性参数线性"这个性质对辨识至关重要。如果动力学方程对参数是非线性的，辨识会变成什么问题？
提示：非线性参数估计是非凸优化，可能有多个局部极值，初始猜测敏感，计算量大。线性参数化把辨识变成凸的最小二乘——全局最优、闭式解、计算量 \(O(n^3)\)。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：URDF 惯性参数格式混淆
错误做法：直接从 URDF 的
<inertia>标签读取 \(I_{xx}, I_{xy}, \ldots\) 并当作关于连杆原点的惯性使用现象：辨识出的参数与真实值偏差极大（即使激励轨迹和算法都正确），残差不收敛
根本原因：URDF 的
<inertia>标签定义的是**关于质心**的惯性张量，而回归矩阵中使用的是**关于连杆坐标系原点**的惯性。两者通过**平行轴定理**关联：\(I_{\text{origin}} = I_{\text{com}} + m(c^\top c \cdot E_3 - c c^\top)\)正确做法：提取 URDF 惯性时，必须用平行轴定理转换到连杆原点。见 3.5 节的 Pinocchio 代码
💡 概念误区：认为"辨识精度越高，控制越好"
新手想法：花大量时间把参数精度从 3% 提到 1%，一定会显著改善控制性能
实际上：控制器对参数误差的敏感度取决于控制律的鲁棒性。一个好的鲁棒控制器（如 \(H_\infty\)、滑模控制）可以容忍 10--20% 的参数误差。辨识的边际收益递减：从 50% 到 5% 改善巨大，从 5% 到 1% 可能几乎无感
正确认知：辨识精度应该匹配控制需求。MPC 和前馈控制对参数敏感（需要 < 5%），PD+重力补偿对参数不敏感（< 20% 即可）
延伸：这正是自适应控制（控制理论·自适应控制（规划中））的动机——即使参数不精确，自适应律也能在线补偿
🧠 思维陷阱：认为"数据越多辨识越准"
新手想法：采集 1 小时的数据一定比 1 分钟好
实际上：如果轨迹不"激励"某些参数方向，再多数据也辨识不出来。100 万个共线性数据点不如 100 个正交的数据点。关键是**信息矩阵 \(Y^\top Y\) 的条件数**，不是数据量
正确思维：先设计激励轨迹（最大化 \(\det(Y^\top Y)\)），再决定采集时长。通常一个周期（10--30 秒）的 Fourier 激励轨迹就够了
练习¶
练习 2.1 ⭐：列出你使用过的（或你实验室的）机器人的 URDF 文件。对比 URDF 中的惯性参数和实际测量值（如果有），估算 CAD 参数的误差量级。
练习 2.2 ⭐：在 MuJoCo 或 PyBullet 中加载一个机械臂模型，将所有连杆质量乘以 1.3（模拟 30% 误差），然后用原始参数设计的 PD 控制器跟踪一个正弦轨迹。记录跟踪误差，与无参数误差的情况对比。
练习 2.3 ⭐⭐：解释为什么 Slotine & Li 自适应控制律中，参数更新律 \(\dot{\hat{\pi}} = -\Gamma Y^\top s\) 能保证闭环稳定性。提示：构造 Lyapunov 函数 \(V = \frac{1}{2}s^\top M s + \frac{1}{2}\tilde{\pi}^\top \Gamma^{-1}\tilde{\pi}\)，其中 \(\tilde{\pi} = \pi - \hat{\pi}\)。
3. 刚体机器人的线性参数化 ⭐⭐¶
3.1 从运动方程到回归形式¶
回顾 刚体动力学/20_Lagrange与Hamilton力学中推导的机器人运动方程：
其中： - \(q \in \mathbb{R}^n\)：关节角度向量 - \(M(q) \in \mathbb{R}^{n \times n}\)：质量矩阵（对称正定） - \(C(q,\dot{q}) \in \mathbb{R}^{n \times n}\)：Coriolis/离心矩阵 - \(g(q) \in \mathbb{R}^n\)：重力矩向量 - \(\tau \in \mathbb{R}^n\)：关节力矩
每个连杆 \(i\) 有 10 个惯性参数（standard inertial parameters）：
其中 \(m_i\) 是质量，\((c_{x,i}, c_{y,i}, c_{z,i})\) 是质心在连杆坐标系中的位置（注意：我们使用"第一矩" \(m_i c_{x,i}\) 而非 \(c_{x,i}\)，正是为了保证线性性），\(I_{xx,i}, \ldots\) 是关于连杆坐标系原点的惯性张量分量。
关键观察：运动方程的每一项（\(M(q)\ddot{q}\)、\(C(q,\dot{q})\dot{q}\)、\(g(q)\)）都可以写成"运动学量乘以惯性参数之和"的形式。具体地：
其中 \(Y \in \mathbb{R}^{n \times 10n}\) 是回归矩阵，\(\pi = [\pi_1^\top, \ldots, \pi_n^\top]^\top \in \mathbb{R}^{10n}\) 是全部惯性参数。
为什么对参数线性？ 直觉上：动能 \(T = \frac{1}{2}\dot{q}^\top M(q)\dot{q}\)，而 \(M(q)\) 的每个分量都是惯性参数的线性函数（参看 刚体动力学/20_Lagrange与Hamilton力学中 \(M(q) = \sum_i J_{v_i}^\top m_i J_{v_i} + J_{\omega_i}^\top R_i I_{c_i} R_i^\top J_{\omega_i}\)，其中 \(J_{v_i}, J_{\omega_i}, R_i\) 只依赖 \(q\)）。Lagrange 方程中的 \(\frac{d}{dt}\frac{\partial T}{\partial \dot{q}} - \frac{\partial T}{\partial q}\) 对 \(T\) 是线性算子，所以最终结果对惯性参数仍然线性。
更严格的证明：设单个连杆 \(i\) 对总动能的贡献为
其中 \(v_{c_i}\) 是质心线速度，\(\omega_i\) 是角速度。展开：
但在连杆坐标系中 \(c_i\) 是常数，所以 \(v_{c_i}\) 最终是 \(q, \dot{q}\) 和 \(c_i\) 的函数。将动能对惯性参数 \(m_i, m_i c_{x,i}, \ldots\) 展开，可以验证每一项都是惯性参数的一次齐次函数。Lagrange 方程中的偏导和时间导数都是线性算子，因此最终的广义力对惯性参数仍然是线性的。
3.2 完整推导：2-DOF 平面机械臂¶
为了把抽象理论变得具体，我们对一个 2-DOF 平面机械臂**手推回归矩阵**。
模型描述： - 关节 1 和关节 2 都是旋转关节，在竖直平面内运动 - 连杆长度 \(l_1, l_2\)，质心到关节的距离 \(l_{c1}, l_{c2}\) - 连杆质量 \(m_1, m_2\)，转动惯量 \(I_1, I_2\)（关于质心） - 重力加速度 \(g_0\)
动能：
势能：
Lagrange 方程 \(\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = \tau_i\) 给出：
现在定义**回归参数**（将原始物理参数重组为与线性参数化一致的形式）：
则运动方程变为：
其中回归矩阵为：
观察：原始参数有 \(m_1, m_2, l_{c1}, l_{c2}, I_1, I_2\) 共 6 个，但在回归形式中只出现 5 个独立组合 \(\theta_1, \ldots, \theta_5\)。这就是**基参数**概念的雏形。
3.3 基参数（Base Parameters）¶
对于一般的 \(n\)-DOF 机器人，标准惯性参数有 \(10n\) 个。但其中很多参数**不可辨识**（unidentifiable），原因有两类：
(A) 线性依赖：某些参数组合在回归矩阵中**始终以同一线性组合**出现。上面 2-DOF 例子中，\(I_1\) 和 \(m_1 l_{c1}^2\) 永远以 \(I_1 + m_1 l_{c1}^2\) 的形式出现——你无法从力矩数据中将它们分开。
(B) 对力矩无贡献：某些参数完全不影响力矩。例如，第一个连杆（固定于基座）的质量 \(m_1\) 只通过 \(m_1 l_{c1}^2\) 和 \(m_1 l_{c1} g_0\) 影响力矩——纯质量项 \(m_1\) 乘以基座转动时的零力臂，贡献为零。
形式化定义：设完整回归矩阵为 \(Y_{\text{full}} \in \mathbb{R}^{n \times 10n}\)（或经过多个采样点堆叠后的大矩阵）。对 \(Y_{\text{full}}\) 做列空间分析：
通过 QR 分解（列主元 pivoting）或 SVD，可以找到 \(p\) 个独立的参数组合，称为**基参数** \(\pi_b \in \mathbb{R}^p\)，使得
其中 \(Y_b \in \mathbb{R}^{n \times p}\) 是满秩的。
Gautier (1991) 算法： 1. 在多个随机构型 \((q_k, \dot{q}_k, \ddot{q}_k)\) 处计算 \(Y_k\)，堆叠成大矩阵 \(\bar{Y}\) 2. 对 \(\bar{Y}\) 做列主元 QR 分解：\(\bar{Y}P = QR\)，其中 \(P\) 是置换矩阵 3. \(R\) 的前 \(p\) 个对角元非零（\(|R_{ii}| > \epsilon\)），对应的列就是基参数的"骨架" 4. 剩余 \(10n - p\) 个参数要么是零，要么可以吸收进基参数
典型数字：7-DOF 机械臂有 \(10 \times 7 = 70\) 个标准参数，但基参数通常只有 40--50 个（加上摩擦参数后约 50--64 个）。
3.4 加入摩擦模型¶
实际机器人的摩擦力不可忽略。最常用的模型是 Coulomb + viscous 摩擦：
其中 \(f_{v,i}\) 是粘性摩擦系数，\(f_{c,i}\) 是 Coulomb 摩擦系数。加入摩擦后，回归矩阵扩展为：
其中 \(Y_{\text{fric}}\) 是对角矩阵，每行只有 \(\dot{q}_i\) 和 \(\operatorname{sign}(\dot{q}_i)\) 两个非零元。
更精细的 **Stribeck 摩擦模型**为：
但 Stribeck 模型对参数 \(v_{s,i}\) 是非线性的，不能直接放入线性回归。实践中通常先用 Coulomb + viscous 辨识，再单独拟合 Stribeck 参数。
3.5 代码：Pinocchio computeJointTorqueRegressor 完整工作流¶
Pinocchio 提供了直接计算回归矩阵的 API。下面给出一个**完整的、可运行的工作流**，从加载 URDF 到验证回归矩阵的正确性，再到基参数提取。
Step 1: 先讲为什么要用 Pinocchio 而不是手写回归矩阵
手写回归矩阵（如 3.2 节）对于 2-DOF 还可行，但对于 7-DOF 机械臂（70 个参数、7 行 70 列的矩阵），手推不仅容易出错，而且无法复用。Pinocchio 基于 Featherstone 的递归算法，在 \(O(n)\) 复杂度内计算回归矩阵——速度快、数值稳定、且自动处理任意运动链拓扑。
Step 2: 正确写法——完整的 Pinocchio 工作流
import numpy as np
import pinocchio as pin
# ====== 1. 加载模型 ======
model = pin.buildModelFromUrdf("panda.urdf")
data = model.createData()
n = model.nv # 关节自由度
nb = model.nbodies - 1 # 连杆数 (排除 universe/world)
print(f"机器人: {model.name}")
print(f"关节自由度: {n}")
print(f"连杆数: {nb}")
print(f"标准惯性参数总数: {10 * nb}")
# ====== 2. 提取标准惯性参数向量 ======
def extract_standard_parameters(model):
"""从 Pinocchio 模型提取标准惯性参数向量 pi
每个连杆 10 个参数: [m, mc_x, mc_y, mc_z, I_xx, I_xy, I_xz, I_yy, I_yz, I_zz]
其中惯性矩是关于连杆坐标系原点的 (不是质心!)
"""
pi = []
for i in range(1, model.nbodies): # 跳过 universe (index 0)
inertia = model.inertias[i]
m = inertia.mass
lever = np.array(inertia.lever) # 质心位置 (连杆坐标系)
mc = m * lever # 第一矩
# 关于质心的惯性矩阵
I_com = np.array(inertia.inertia) # 3x3 对称矩阵
# 平行轴定理: I_origin = I_com + m*(c'c*I - c*c')
# 这一步非常容易出错! 见陷阱 3.2
I_origin = I_com + m * (np.dot(lever, lever) * np.eye(3)
- np.outer(lever, lever))
pi.extend([m, mc[0], mc[1], mc[2],
I_origin[0,0], I_origin[0,1], I_origin[0,2],
I_origin[1,1], I_origin[1,2], I_origin[2,2]])
return np.array(pi)
pi = extract_standard_parameters(model)
# ====== 3. 计算回归矩阵并验证 ======
np.random.seed(0)
q = pin.randomConfiguration(model)
v = np.random.randn(n)
a = np.random.randn(n)
# Pinocchio 的回归矩阵 API
Y = pin.computeJointTorqueRegressor(model, data, q, v, a)
print(f"\n回归矩阵维度: {Y.shape}") # (n, 10*nb)
# 用 RNEA 计算"地面真值"力矩
tau_rnea = pin.rnea(model, data, q, v, a)
# 验证 Y @ pi == tau_rnea
tau_regressor = Y @ pi
error = np.linalg.norm(tau_rnea - tau_regressor)
print(f"RNEA 力矩: {tau_rnea}")
print(f"回归矩阵 @ pi: {tau_regressor}")
print(f"误差范数: {error:.2e}") # 应为 ~1e-14 (机器精度)
assert error < 1e-10, f"验证失败! 误差 {error} 过大"
# ====== 4. 基参数提取 ======
def find_base_parameters(model, data, n_samples=1000, tol=1e-6):
"""通过随机采样 + QR 分解找基参数"""
from scipy.linalg import qr
# 堆叠多个构型的回归矩阵
Y_stack = []
for _ in range(n_samples):
q_rand = pin.randomConfiguration(model)
v_rand = np.random.randn(model.nv) * 2.0
a_rand = np.random.randn(model.nv) * 5.0
Y_k = pin.computeJointTorqueRegressor(model, data, q_rand, v_rand, a_rand)
Y_stack.append(Y_k)
Y_full = np.vstack(Y_stack)
print(f"\n堆叠回归矩阵维度: {Y_full.shape}")
print(f"数值秩: {np.linalg.matrix_rank(Y_full, tol=tol)}")
# 列主元 QR 分解
Q, R, P = qr(Y_full, pivoting=True)
diag_R = np.abs(np.diag(R))
# 确定基参数个数
p = np.sum(diag_R > tol * diag_R[0])
print(f"基参数个数: {p}/{Y_full.shape[1]}")
# 基参数对应的列索引
base_indices = np.sort(P[:p])
return base_indices, p
base_idx, p = find_base_parameters(model, data)
# ====== 5. 用基参数重建回归矩阵 ======
Y_base = Y[:, base_idx]
pi_base = pi[base_idx]
tau_base = Y_base @ pi_base
print(f"\n基参数回归重建误差: {np.linalg.norm(tau_rnea - tau_base):.2e}")
print(f"基参数回归矩阵条件数: {np.linalg.cond(Y_base):.1f}")
Step 3: 错误写法及分析
# ❌ 错误 1: 不做平行轴定理转换
def extract_params_WRONG_v1(model):
pi = []
for i in range(1, model.nbodies):
inertia = model.inertias[i]
m = inertia.mass
mc = m * np.array(inertia.lever)
I_com = np.array(inertia.inertia)
# 错! 直接用质心惯性, 没有转换到原点
pi.extend([m, mc[0], mc[1], mc[2],
I_com[0,0], I_com[0,1], I_com[0,2],
I_com[1,1], I_com[1,2], I_com[2,2]])
return np.array(pi)
# 后果: Y @ pi_wrong != tau_rnea, 误差量级 ~1e-1 到 ~1e+1
# 辨识结果完全错误, 且这个 bug 非常隐蔽
# ❌ 错误 2: 惯性矩阵元素顺序搞错
# URDF 格式: <inertia ixx="..." ixy="..." ixz="..." iyy="..." iyz="..." izz="..."/>
# Pinocchio 内部存储为完整 3x3 矩阵
# 但有些用户直接按 [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] 排列 —— 顺序不对!
# 正确顺序 (Pinocchio convention): [Ixx, Ixy, Ixz, Iyy, Iyz, Izz]
# ❌ 错误 3: 遗漏 universe 连杆
def extract_params_WRONG_v3(model):
pi = []
for i in range(model.nbodies): # 错! 包含了 universe (index 0)
inertia = model.inertias[i]
# ... universe 的惯性是零, 会导致回归矩阵维度不匹配
return np.array(pi)
Step 4: C++ Eigen 实现对比
// C++ 版本的回归矩阵计算 (使用 Pinocchio C++ API)
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/regressor.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <Eigen/Dense>
#include <iostream>
int main() {
// 加载模型
pinocchio::Model model;
pinocchio::urdf::buildModel("panda.urdf", model);
pinocchio::Data data(model);
const int n = model.nv;
const int nb = model.nbodies - 1;
// 随机关节状态
Eigen::VectorXd q = pinocchio::randomConfiguration(model);
Eigen::VectorXd v = Eigen::VectorXd::Random(n);
Eigen::VectorXd a = Eigen::VectorXd::Random(n);
// ✅ 正确: 计算回归矩阵
// 返回 n x (10*nb) 的矩阵
Eigen::MatrixXd Y = pinocchio::computeJointTorqueRegressor(
model, data, q, v, a
);
// RNEA 验证
Eigen::VectorXd tau_rnea = pinocchio::rnea(model, data, q, v, a);
// 提取标准惯性参数
Eigen::VectorXd pi(10 * nb);
for (int i = 1; i < model.nbodies; ++i) {
const auto& inertia = model.inertias[i];
double m = inertia.mass();
Eigen::Vector3d lever = inertia.lever();
Eigen::Vector3d mc = m * lever;
// 平行轴定理
Eigen::Matrix3d I_com = inertia.inertia().matrix();
Eigen::Matrix3d I_origin = I_com
+ m * (lever.dot(lever) * Eigen::Matrix3d::Identity()
- lever * lever.transpose());
int offset = (i - 1) * 10;
pi(offset + 0) = m;
pi(offset + 1) = mc(0);
pi(offset + 2) = mc(1);
pi(offset + 3) = mc(2);
pi(offset + 4) = I_origin(0, 0);
pi(offset + 5) = I_origin(0, 1);
pi(offset + 6) = I_origin(0, 2);
pi(offset + 7) = I_origin(1, 1);
pi(offset + 8) = I_origin(1, 2);
pi(offset + 9) = I_origin(2, 2);
}
Eigen::VectorXd tau_Y = Y * pi;
double error = (tau_rnea - tau_Y).norm();
std::cout << "回归矩阵验证误差: " << error << std::endl;
// 应输出 ~1e-14
return 0;
}
⭐ 自测检查点 3.1：运行上述代码，验证
tau_rnea和tau_regressor的差异在机器精度范围内。如果差异较大，检查惯性参数的提取方式（特别是平行轴定理的应用）。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：忽略基参数直接辨识全部 \(10n\) 个参数
错误做法：把全部 70 个参数放进最小二乘中辨识
现象：\(Y_{\text{full}}\) 列不满秩 → \((Y^\top Y)\) 奇异 → 解不唯一或条件数极大 → 辨识出的参数**物理上荒谬**（如负质量 \(m = -2.3\) kg）
根本原因：\(10n\) 个标准参数中有很多线性相关或对力矩无贡献。列秩亏的最小二乘有无穷多解，
numpy.linalg.lstsq返回的是最小范数解，但这个解没有物理意义正确做法：**必须**先通过 QR 或 SVD 降到基参数。辨识基参数后，如需还原物理参数，可附加先验约束（如 \(m_i > 0\)）做约束优化
💡 概念误区：认为"质心位置"是线性参数化中的待辨识量
新手想法：辨识目标是每个连杆的质量 \(m_i\) 和质心位置 \((c_{x,i}, c_{y,i}, c_{z,i})\)
实际上：直接辨识 \(c_{x,i}\) 会破坏线性性（因为动能中出现 \(m_i c_{x,i}^2\) 等二次项）。线性参数化中使用的是**第一矩** \(m_i c_{x,i}\)。从 \(m_i c_{x,i}\) 和 \(m_i\) 可以恢复 \(c_{x,i}\)，但前提是 \(m_i\) 也是可辨识的
延伸：对于第一个连杆（旋转关节固定在基座），\(m_1\) 本身不可辨识（只有 \(m_1 l_{c1}^2\) 和 \(m_1 l_{c1}\) 的组合可辨识），所以 \(c_{x,1}\) 也不可恢复——这就是基参数的本质
🧠 思维陷阱：认为"回归矩阵中 \(\ddot{q}\) 必须来自传感器测量"
新手想法：需要加速度传感器才能计算回归矩阵
实际上：几乎没有机器人安装关节加速度传感器。\(\ddot{q}\) 的获取方式有三种： 1. 数值微分（最简单但噪声大） 2. Fourier 轨迹的解析导数（推荐，4.6 节详述） 3. 积分消除法（Gautier & Poignet 2001，通过分部积分消去 \(\ddot{q}\)）
自检：如果你的方案依赖加速度传感器，重新考虑
⚠️ 编程陷阱：Pinocchio 的 computeJointTorqueRegressor 输入顺序
错误做法：
pin.computeJointTorqueRegressor(model, data, v, q, a)—— 把v和q的位置搞反了现象：不会报错（因为
q和v都是VectorXd），但回归矩阵完全错误根本原因：C++ / Python 动态类型不会检查语义，只检查类型和维度
正确做法：始终按
(model, data, q, v, a)的顺序传入。养成用关键字参数的习惯
练习¶
练习 3.1 ⭐：对 2-DOF 平面机械臂（3.2 节），如果关节 2 的连杆是对称圆柱体（\(l_{c2} = l_2/2\)），写出 \(I_2\) 关于 \(m_2, l_2, r_2\)（半径）的表达式。代入基参数公式，验证基参数个数是否改变。
练习 3.2 ⭐⭐：用 Pinocchio 加载一个 7-DOF 机械臂的 URDF，计算其基参数个数。提示：使用 find_base_parameters 函数。
练习 3.3 ⭐⭐：推导当连杆 \(i\) 是平面关节（prismatic）而非旋转关节时，标准惯性参数中哪些变得不可辨识？提示：平移关节的速度雅可比形式不同。
练习 3.4 ⭐⭐⭐：Khalil & Dombre 的基参数提取使用 QR 分解。解释为什么不能用 SVD 直接做基参数提取。提示：SVD 给出的是正交基，但基参数需要保持物理可解释性（SVD 给出的线性组合可能混合了不同连杆的参数）。
4. 最小二乘辨识 ⭐⭐¶
4.1 问题建立¶
假设我们在 \(K\) 个时刻 \(t_1, \ldots, t_K\) 采集了数据 \((q_k, \dot{q}_k, \ddot{q}_k, \tau_k)\)。在每个时刻：
其中 \(\epsilon_k\) 是测量噪声。堆叠所有时刻：
即 \(\bar{Y}\pi_b = \bar{\tau} + \bar{\epsilon}\)。这是一个**超定线性方程组**（\(nK \gg p\)），我们需要从含噪数据中估计 \(\pi_b\)。
4.2 OLS（普通最小二乘）的完整推导¶
目标：最小化残差平方和
展开：
对 \(\pi_b\) 求梯度：
令梯度为零，得到**正规方程**（normal equation）：
若 \(\bar{Y}\) 列满秩（\(\operatorname{rank}(\bar{Y}) = p\)），则 \(\bar{Y}^\top\bar{Y}\) 正定可逆：
这就是 OLS 估计。\((\bar{Y}^\top\bar{Y})^{-1}\bar{Y}^\top\) 称为 \(\bar{Y}\) 的**伪逆**（Moore-Penrose pseudoinverse）\(\bar{Y}^+\)。
Hessian 矩阵：\(\nabla^2_{\pi_b} J = 2\bar{Y}^\top\bar{Y} \succ 0\)（正定），所以 \(\hat{\pi}_b\) 是唯一的全局最小值。
4.2.1 C++ Eigen 实现：OLS（正确 vs 错误 vs 对比）¶
Step 1: 为什么要用 Eigen 的 ldlt/colPivHouseholderQr 而不是直接求逆？
正规方程 \((\bar{Y}^\top\bar{Y})\hat{\pi} = \bar{Y}^\top\bar{\tau}\) 的解可以通过求逆 \((\bar{Y}^\top\bar{Y})^{-1}\) 得到，但直接求逆在数值上不稳定——当条件数大时，逆矩阵的元素会有很大的舍入误差。更好的做法是使用矩阵分解求解线性方程组。
Step 2: 正确写法
#include <Eigen/Dense>
#include <iostream>
#include <chrono>
// ✅ 方式 A: 使用 QR 分解直接求解 (最推荐, 数值最稳)
Eigen::VectorXd ols_qr(const Eigen::MatrixXd& Y, const Eigen::VectorXd& tau) {
// Y: (nK x p), tau: (nK x 1)
// 直接对 Y 做 QR 分解求解 min ||tau - Y*pi||^2
return Y.colPivHouseholderQr().solve(tau);
}
// ✅ 方式 B: 使用正规方程 + LDLT 分解 (Y'Y 对称正定时)
Eigen::VectorXd ols_normal_ldlt(const Eigen::MatrixXd& Y, const Eigen::VectorXd& tau) {
Eigen::MatrixXd YtY = Y.transpose() * Y;
Eigen::VectorXd Ytau = Y.transpose() * tau;
return YtY.ldlt().solve(Ytau);
}
// ✅ 方式 C: 使用 SVD (最稳但最慢, 可处理秩亏情况)
Eigen::VectorXd ols_svd(const Eigen::MatrixXd& Y, const Eigen::VectorXd& tau) {
return Y.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau);
}
Step 3: 错误写法及分析
// ❌ 错误 1: 直接求逆
Eigen::VectorXd ols_WRONG_inverse(const Eigen::MatrixXd& Y, const Eigen::VectorXd& tau) {
Eigen::MatrixXd YtY = Y.transpose() * Y;
// 危险! 当 cond(YtY) > 1e12 时, inverse() 的结果充满舍入误差
return YtY.inverse() * Y.transpose() * tau;
// 问题: (1) inverse() 是 O(p^3) 且数值不稳; (2) 不处理秩亏
}
// ❌ 错误 2: 未检查条件数就使用正规方程
Eigen::VectorXd ols_WRONG_no_check(const Eigen::MatrixXd& Y, const Eigen::VectorXd& tau) {
Eigen::MatrixXd YtY = Y.transpose() * Y;
// 没有检查条件数! 如果 YtY 接近奇异, llt() 会默默给出垃圾结果
return YtY.llt().solve(Y.transpose() * tau);
// 更糟: llt() 假设正定, 如果 YtY 不正定 (因为秩亏), 行为未定义
}
// ❌ 错误 3: 用 float 精度
Eigen::VectorXf ols_WRONG_float(const Eigen::MatrixXf& Y, const Eigen::VectorXf& tau) {
// float 只有 ~7 位有效数字
// YtY 的条件数可能是 1e8, 求解后只剩 ~0 位有效数字!
// 机器人辨识必须用 double
return Y.colPivHouseholderQr().solve(tau);
}
Step 4: 三种方式性能对比
| 方法 | 数值稳定性 | 速度 (\(p=50\), \(nK=7000\)) | 能否处理秩亏 | 推荐场景 |
|---|---|---|---|---|
| QR (colPivHouseholderQr) | 高 | ~2 ms | 是 (自动) | 通用首选 |
| 正规方程 + LDLT | 中 | ~0.5 ms | 否 (需满秩) | 已确认满秩、需要速度 |
| SVD (bdcSvd) | 最高 | ~8 ms | 是 (自动) | 调试、秩亏分析 |
| 直接求逆 | 差 | ~1 ms | 否 | 永远不用 |
4.3 WLS（加权最小二乘）¶
如果不同时刻的噪声方差不同（异方差性），OLS 不再最优。设噪声协方差为 \(\operatorname{Cov}(\bar{\epsilon}) = \Sigma\)，则 WLS 最小化：
解为：
实践中如何估计 \(\Sigma\)？常用方法： 1. 先用 OLS 得到初步估计 \(\hat{\pi}_b^{(0)}\) 2. 计算残差 \(\hat{\epsilon}_k = \tau_k - Y_k\hat{\pi}_b^{(0)}\) 3. 估计每个关节的噪声方差 \(\hat{\sigma}_j^2 = \frac{1}{K}\sum_k \hat{\epsilon}_{j,k}^2\) 4. 构造 \(\Sigma = \operatorname{diag}(\hat{\sigma}_1^2, \ldots, \hat{\sigma}_n^2) \otimes I_K\) 5. 用 WLS 重新估计 → 迭代（IRLS, Iteratively Reweighted Least Squares）
4.4 Gauss-Markov 定理¶
定理（Gauss-Markov）：在线性回归模型 \(\bar{\tau} = \bar{Y}\pi_b + \bar{\epsilon}\) 中，若噪声满足： 1. \(\mathbb{E}[\bar{\epsilon}] = 0\)（零均值） 2. \(\operatorname{Cov}(\bar{\epsilon}) = \sigma^2 I\)（同方差、不相关）
则 OLS 估计 \(\hat{\pi}_b = (\bar{Y}^\top\bar{Y})^{-1}\bar{Y}^\top\bar{\tau}\) 是**所有线性无偏估计中方差最小的**（BLUE: Best Linear Unbiased Estimator）。
证明：设任意线性无偏估计为 \(\tilde{\pi}_b = A\bar{\tau}\)，其中 \(A\bar{Y} = I\)（无偏条件）。则：
令 \(D = A - (\bar{Y}^\top\bar{Y})^{-1}\bar{Y}^\top\)，由无偏条件 \(D\bar{Y} = 0\)，则：
因为 \(D\bar{Y} = 0\)，交叉项为零。所以：
不等号对所有 \(D\) 成立（\(DD^\top \succeq 0\)），等号当且仅当 \(D = 0\) 时成立。\(\square\)
工程含义：在 Gauss-Markov 条件满足时，OLS 已是最优——不需要更复杂的估计方法。但如果噪声存在异方差性或相关性（在实际机器人中很常见，因为力矩传感器精度在不同关节可能不同），则需要 WLS。
4.5 Cramer-Rao 下界¶
定理（Cramer-Rao Lower Bound, CRLB）：对于任意无偏估计 \(\hat{\pi}_b\)，其协方差满足
其中 \(\mathcal{I}(\pi_b)\) 是 Fisher 信息矩阵：
对于高斯噪声 \(\bar{\epsilon} \sim \mathcal{N}(0, \sigma^2 I)\)，Fisher 信息矩阵为
从而 CRLB 为 \(\operatorname{Cov}(\hat{\pi}_b) \succeq \sigma^2(\bar{Y}^\top\bar{Y})^{-1}\)。这恰好等于 OLS 的协方差——在高斯噪声下 OLS 达到 CRLB，是渐近有效的（等价于 MLE）。
4.6 激励轨迹设计¶
辨识精度不仅取决于算法，更取决于**实验设计**——你让机器人跑什么轨迹来采集数据。
核心原则：\(\hat{\pi}_b\) 的协方差正比于 \((\bar{Y}^\top\bar{Y})^{-1}\)，要使辨识精度高，就要**最大化 \(\bar{Y}^\top\bar{Y}\) 的"大小"**。
D-最优性（D-optimality）：选择轨迹使
等价于最小化参数估计协方差椭球的体积。
为什么是 \(\det\) 而不是 \(\lambda_{\min}\) 或 \(\text{trace}\)？ 不同的最优性准则对应不同的设计目标：
| 准则 | 目标函数 | 几何含义 | 适用场景 |
|---|---|---|---|
| D-最优 | \(\max \det(\bar{Y}^\top\bar{Y})\) | 最小化椭球体积 | 所有参数同等重要 |
| A-最优 | \(\min \text{trace}((\bar{Y}^\top\bar{Y})^{-1})\) | 最小化方差之和 | 关注总体精度 |
| E-最优 | \(\max \lambda_{\min}(\bar{Y}^\top\bar{Y})\) | 最小化最大方差 | 关注最差参数 |
实践中 D-最优 最常用，因为它等价于最大化 Fisher 信息矩阵的行列式，有清晰的统计学解释。
4.6.1 Fourier 参数化的完整数学推导¶
Swevers et al. (1997) 提出将轨迹参数化为有限阶 Fourier 级数：
其中 \(\omega_f = 2\pi/T_f\) 是基频，\(T_f\) 是轨迹周期，\(a_{l,j}, b_{l,j}\) 是待优化的 Fourier 系数，\(N_h\) 是最高谐波阶数。
为什么要除以 \(l\omega_f\)？ 这是为了让速度和加速度的表达式更简洁。对 \(q_j(t)\) 求导：
观察关键性质： 1. 速度的 Fourier 系数就是 \(a_{l,j}\) 和 \(b_{l,j}\)——这意味着优化速度幅度直接就是优化 Fourier 系数 2. 加速度的幅度正比于 \(l\omega_f\)——高次谐波产生更大的加速度，但也需要更大的力矩 3. 轨迹自动满足 \(q_j(0) = q_j(T_f)\)（周期性），因为 \(\sin(0) = \sin(l\omega_f T_f) = 0\)，但 \(\cos\) 项给出 \(q_j(0) = q_{0,j} - \sum_l b_{l,j}/(l\omega_f)\) 和 \(q_j(T_f)\) 相同
约束处理：轨迹必须满足关节极限、速度极限和力矩极限：
前两个约束可以在时域上以有限个采样点检查（将连续约束离散化）。力矩约束需要已知参数的粗略估计（CAD 标称值），是一个软约束。
优化问题的完整形式：
设决策变量为 \(\xi = [q_{0,1}, a_{1,1}, b_{1,1}, \ldots, a_{N_h,1}, b_{N_h,1}, q_{0,2}, \ldots]^\top \in \mathbb{R}^{n(2N_h+1)}\)。
使用 \(\log\det\) 而非 \(\det\) 的原因：(1) 避免数值溢出（\(\det\) 可能是 \(10^{100}\) 量级）；(2) \(\log\det\) 是凹函数（虽然整体问题仍然非凸，但有更好的数值性质）。
求解方法：这是一个非线性约束优化问题（\(\bar{Y}\) 通过回归矩阵非线性地依赖于 \(\xi\)），常用方法： - SQP（Sequential Quadratic Programming）：适合中等规模，scipy.optimize.minimize 支持 - CMA-ES（Covariance Matrix Adaptation Evolution Strategy）：无梯度优化，适合高维 - 粒子群优化（PSO）：简单易实现，适合快速原型
⚠️ 常见陷阱¶
⚠️ 编程陷阱：数值微分放大噪声
错误做法：\(\ddot{q}_k \approx (\dot{q}_{k+1} - \dot{q}_k)/\Delta t\)（前向差分）
现象：\(\ddot{q}\) 的估计值充满高频噪声脉冲，回归矩阵的条件数暴增 10-100 倍，辨识出的参数方差是理论值的 100 倍
根本原因：数值微分是一个不适定问题（ill-posed）——它放大输入信号中比 \(1/\Delta t\) 更高频的噪声分量。如果编码器噪声为 \(\sigma_q\)，则 \(\sigma_{\ddot{q}} \approx 2\sigma_q/\Delta t^2\)，在 \(\Delta t = 1\) ms, \(\sigma_q = 10^{-4}\) rad 时，\(\sigma_{\ddot{q}} \approx 200\) rad/s\(^2\)——远大于实际加速度
正确做法： - 使用 Fourier 参数化轨迹 → \(\dot{q}, \ddot{q}\) 由解析公式计算 - 或者：先对 \(q(t)\) 做低通滤波（截止频率取轨迹最高频率的 2-3 倍），再做中心差分 - 或者：使用"消除加速度"方法——将回归方程在整个周期上积分，\(\ddot{q}\) 通过分部积分消去（Gautier & Poignet, 2001） - 自检方法：画出 \(\ddot{q}\) 的功率谱，如果噪声底层比信号只低 10 dB，说明数值微分质量不够
⚠️ 编程陷阱：电机侧 vs 关节侧力矩
错误做法：直接将电机电流乘以力矩常数当作关节力矩
现象：辨识出的惯性矩比 CAD 值大 \(N^2\) 倍（\(N\) 是减速比），摩擦系数也偏大
根本原因：工业机器人通常测量的是**电流**（正比于电机侧力矩 \(\tau_m\)），而非关节侧力矩 \(\tau_j\)。两者关系为 \(\tau_j = N \tau_m - I_m N^2 \ddot{q} - f_{m,v} N^2 \dot{q} - f_{m,c} N \text{sign}(\dot{q})\)，其中 \(I_m\) 是电机转子惯量
正确做法：在回归模型中加入电机转子惯量项 \(I_m N^2 \ddot{q}\)（反映到关节侧的电机惯量），以及电机侧摩擦反映到关节侧的项
💡 概念误区：认为 D-最优轨迹是唯一的
新手想法：存在一条"最优激励轨迹"
实际上：D-最优问题通常有多个局部最优。不同的初始 Fourier 系数会收敛到不同的轨迹，但它们的 \(\det(\bar{Y}^\top\bar{Y})\) 值相近。实践中选一个条件数低于 100 的轨迹就够了——不需要全局最优
正确做法：多次随机初始化优化，取条件数最小的那个
🧠 思维陷阱：忽略数据预处理
新手想法：直接把原始采集的 \((q, \tau)\) 数据塞进辨识算法
实际上：原始数据通常需要：(1) 去除直流偏置（力矩传感器零漂）；(2) 低通滤波（消除高频噪声和混叠）；(3) 去除异常点（力矩突变、碰撞）；(4) 降采样（减少数据量但不丢信息，通常从 1 kHz 降到 100--200 Hz）
正确思维：数据质量 >> 算法花哨程度。80% 的辨识问题出在数据预处理上
⚠️ 编程陷阱：摩擦模型选择
错误做法 A：只用粘性摩擦（\(f_v \dot{q}\)）——低速时残差很大，因为 Coulomb 摩擦占主导
错误做法 B：用 Coulomb + viscous（\(f_v \dot{q} + f_c \operatorname{sign}(\dot{q})\)）——在零速附近 \(\operatorname{sign}\) 不连续，引入数值问题
现象：做法 A 的残差在低速段系统性偏大；做法 B 在零速穿越时产生力矩脉冲
正确做法：在零速附近用 \(\tanh(\alpha \dot{q})\) 近似 \(\operatorname{sign}(\dot{q})\)，\(\alpha\) 取 50--200。这保持了线性参数化（\(f_c\) 前面的 \(\tanh\) 只依赖 \(\dot{q}\)，不依赖参数），同时避免了不连续性
4.7 完整的辨识流程代码¶
import numpy as np
import pinocchio as pin
from scipy.optimize import minimize
def generate_fourier_trajectory(t, omega_f, coeffs, q0):
"""生成 Fourier 参数化的激励轨迹
Args:
t: 时间数组 (K,)
omega_f: 基频
coeffs: Fourier 系数 (n_joints, 2*N_h) -- [a1,b1,a2,b2,...]
q0: 关节偏移 (n_joints,)
Returns:
q, qd, qdd: 位置/速度/加速度 (K, n_joints)
"""
n_joints = len(q0)
N_h = coeffs.shape[1] // 2
K = len(t)
q = np.tile(q0, (K, 1))
qd = np.zeros((K, n_joints))
qdd = np.zeros((K, n_joints))
for j in range(n_joints):
for l in range(1, N_h + 1):
a_l = coeffs[j, 2*(l-1)]
b_l = coeffs[j, 2*(l-1)+1]
lw = l * omega_f
q[:, j] += a_l/(lw) * np.sin(lw*t) - b_l/(lw) * np.cos(lw*t)
qd[:, j] += a_l * np.cos(lw*t) + b_l * np.sin(lw*t)
qdd[:, j] += -a_l*lw * np.sin(lw*t) + b_l*lw * np.cos(lw*t)
return q, qd, qdd
def build_stacked_regressor(model, data, q_traj, qd_traj, qdd_traj):
"""堆叠所有时刻的回归矩阵
Returns:
Y_bar: (n*K, 10*nbodies) 堆叠回归矩阵
"""
K = q_traj.shape[0]
n = model.nv
nb = model.nbodies - 1 # 排除 universe
Y_bar = np.zeros((n * K, 10 * nb))
for k in range(K):
Y_k = pin.computeJointTorqueRegressor(
model, data, q_traj[k], qd_traj[k], qdd_traj[k]
)
Y_bar[k*n:(k+1)*n, :] = Y_k
return Y_bar
def compute_base_parameters(Y_bar, tol=1e-6):
"""通过 QR 分解找基参数
Returns:
col_indices: 基参数对应的列索引
p: 基参数个数
"""
from scipy.linalg import qr
Q, R, P = qr(Y_bar, pivoting=True)
# 找秩: R 对角线元素 > tol 的个数
diag_R = np.abs(np.diag(R))
p = np.sum(diag_R > tol * diag_R[0])
col_indices = P[:p]
return col_indices, p
def identify_parameters(Y_bar, tau_bar, col_indices):
"""OLS 辨识基参数
Returns:
pi_hat: 辨识得到的基参数
sigma_pi: 参数标准差
cond_number: 回归矩阵条件数
"""
Y_b = Y_bar[:, col_indices]
# OLS
pi_hat = np.linalg.lstsq(Y_b, tau_bar, rcond=None)[0]
# 残差与噪声方差估计
residual = tau_bar - Y_b @ pi_hat
n_samples = len(tau_bar)
p = len(pi_hat)
sigma2 = np.dot(residual, residual) / (n_samples - p)
# 参数协方差
cov_pi = sigma2 * np.linalg.inv(Y_b.T @ Y_b)
sigma_pi = np.sqrt(np.diag(cov_pi))
# 条件数
cond_number = np.linalg.cond(Y_b)
return pi_hat, sigma_pi, cond_number
# ---- 使用示例 ----
model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()
# 生成激励轨迹
T_f = 10.0 # 周期
omega_f = 2 * np.pi / T_f
dt = 0.001
t = np.arange(0, T_f, dt)
N_h = 5 # Fourier 阶数
n_joints = model.nv
np.random.seed(42)
coeffs = 0.5 * np.random.randn(n_joints, 2 * N_h)
q0 = np.zeros(n_joints)
q_traj, qd_traj, qdd_traj = generate_fourier_trajectory(t, omega_f, coeffs, q0)
# 构建回归矩阵
Y_bar = build_stacked_regressor(model, data, q_traj, qd_traj, qdd_traj)
# 模拟真实力矩（加噪声）
pi_true = np.random.rand(10 * (model.nbodies - 1)) # 假设的真实参数
tau_bar = Y_bar @ pi_true + 0.1 * np.random.randn(Y_bar.shape[0])
# 找基参数
col_indices, p = compute_base_parameters(Y_bar)
print(f"基参数个数: {p}/{Y_bar.shape[1]}")
# 辨识
pi_hat, sigma_pi, cond = identify_parameters(Y_bar, tau_bar, col_indices)
print(f"条件数: {cond:.1f}")
print(f"参数相对误差: {np.linalg.norm(pi_hat - pi_true[col_indices])/np.linalg.norm(pi_true[col_indices]):.4f}")
⭐ 自测检查点 4.1：在上述代码中，如果
cond_number > 1e6，说明什么问题？应该怎么解决？答：条件数过大说明回归矩阵接近秩亏——某些参数组合几乎线性相关。可能的原因：(1) 激励轨迹不够"丰富"，没有充分激励所有参数；(2) 基参数选取有误。解决方案：重新设计激励轨迹（增加 Fourier 阶数、扩大关节运动范围），或使用 D-最优准则优化。
⭐ 自测检查点 4.2：Gauss-Markov 定理的两个条件（零均值、同方差不相关）在机器人辨识中是否成立？如果不成立，应该怎么办？
答：通常不完全成立。(1) 零均值：如果模型结构正确（含摩擦），基本成立；(2) 同方差：不同关节的力矩传感器精度不同，且大力矩时噪声可能更大——应使用 WLS。不相关：相邻采样点的噪声可能有时间相关性——应降采样或使用 GLS（广义最小二乘）。
练习¶
练习 4.1 ⭐：证明 WLS 估计在 \(\operatorname{Cov}(\bar{\epsilon}) = \Sigma\) 的条件下是 BLUE。提示：用变量替换 \(\tilde{Y} = \Sigma^{-1/2}\bar{Y}\), \(\tilde{\tau} = \Sigma^{-1/2}\bar{\tau}\)，把 WLS 变成 OLS，然后套用 Gauss-Markov 定理。
练习 4.2 ⭐⭐：实现 D-最优激励轨迹设计。使用 scipy.optimize.minimize 优化 Fourier 系数，目标函数为 \(-\log\det(\bar{Y}^\top\bar{Y})\)，约束为关节位置和速度极限。对比优化前后回归矩阵的条件数。
练习 4.3 ⭐⭐：用 C++ Eigen 实现 4.2.1 节中的三种 OLS 求解方法，并在 7-DOF 机械臂的辨识数据上比较运行时间和数值精度。绘制 \(p\) (参数个数) vs 运行时间的曲线。
练习 4.4 ⭐⭐⭐：推导"消除加速度"方法的数学细节。提示：将 \(\int_0^{T_f} Y_k \pi_b \, dt = \int_0^{T_f} \tau_k \, dt\) 展开，对含 \(\ddot{q}\) 的项做分部积分，利用周期性条件消去边界项。
5. 频域辨识方法 ⭐⭐⭐¶
5.1 动机：为什么要在频域做辨识？¶
时域方法（OLS/WLS）将所有频率的数据混在一起处理。但在实际机器人中，不同频率范围的数据质量差异很大：
- 低频 (< 1 Hz)：重力项和 Coulomb 摩擦占主导，信噪比高
- 中频 (1--10 Hz)：Coriolis 和惯性项显著，正是辨识的"黄金频段"
- 高频 (> 10 Hz)：传感器噪声、结构柔性振动、量化噪声占主导
频域方法允许我们**选择性地利用特定频率范围的数据**，避开噪声严重的频段。
5.2 从时域到频域：离散 Fourier 变换¶
对于周期激励信号（周期 \(T_f\)，采样频率 \(f_s\)，\(N\) 个采样点），信号可以用 DFT 表示：
其中 \(k\) 对应频率 \(f_k = k f_s / N\)。
时域的线性回归 \(Y(t)\pi = \tau(t)\) 在频域变为：
关键：每个频率点 \(f_k\) 给出一组独立的方程。我们可以只选取感兴趣的频率点组成方程组。
5.3 多正弦激励（Multisine Excitation）¶
Fourier 参数化轨迹的频域解读：4.6 节中的 Fourier 轨迹在频域上恰好是一个**多正弦信号**——只在 \(f = l\omega_f/(2\pi)\), \(l = 1, \ldots, N_h\) 处有能量。
多正弦激励的设计：
其中 \(f_l\) 是选定的激励频率，\(A_{l,j}\) 是幅度，\(\phi_{l,j}\) 是相位。
Schroeder 相位（最小波峰因子）：为了在给定频率内容下最小化信号的峰值，使用
这给出的信号波峰因子接近 \(\sqrt{2}\)（正弦波的理论下限），使得在力矩限制下可以用更大的激励幅度。
频率选择策略：
| 策略 | 频率分布 | 优点 | 缺点 |
|---|---|---|---|
| 等间距 | \(f_l = l \cdot \Delta f\) | 简单 | 高频采样过多 |
| 对数间距 | \(f_l = f_{\min} \cdot r^{l-1}\) | 每十倍频等密度 | 低频分辨率低 |
| 奇数次谐波 | \(f_l = (2l-1)\Delta f\) | 可检测非线性 | 需要更多频率 |
为什么奇数次谐波能检测非线性？ 如果系统是线性的，输入只含奇数次谐波时，输出也只在奇数次谐波处有响应。如果偶数次谐波处出现能量，就说明系统存在非线性——这是诊断模型结构是否正确的有力工具。
5.4 Bode 图辨识¶
对于**单关节**（或解耦后的单轴），可以用经典的频率响应方法。将单关节运动方程在工作点 \(q_0\) 附近线性化：
其中 \(J_{\text{eff}}\) 是等效惯量（包含电机惯量和连杆惯量），\(f_v\) 是粘性摩擦，\(K\) 是弹性项（柔性关节或重力刚度）。
频率响应函数（FRF）：
从 Bode 图可以直接读出：
- 低频增益 \(|G(0)| = 1/K\) → 得到弹性刚度 \(K\)
- 谐振频率 \(\omega_n = \sqrt{K/J_{\text{eff}}}\) → 结合 \(K\) 得到 \(J_{\text{eff}}\)
- 谐振峰高度 → 得到阻尼比 \(\zeta = f_v/(2\sqrt{KJ_{\text{eff}}})\) → 得到 \(f_v\)
- 高频渐近斜率 \(-40\) dB/dec → 确认是二阶系统
import numpy as np
from scipy import signal
import matplotlib.pyplot as plt
def bode_identification(freq_hz, frf_mag, frf_phase):
"""从 Bode 图数据辨识二阶系统参数
Args:
freq_hz: 频率数组 [Hz]
frf_mag: FRF 幅值 [dB]
frf_phase: FRF 相位 [deg]
Returns:
J_eff, f_v, K: 等效惯量, 粘性摩擦, 弹性刚度
"""
omega = 2 * np.pi * freq_hz
# 1. 低频增益 -> K
K = 1.0 / (10 ** (frf_mag[0] / 20))
# 2. 谐振频率 (幅值最大处)
idx_peak = np.argmax(frf_mag)
omega_n = omega[idx_peak]
# 3. 等效惯量
J_eff = K / omega_n**2
# 4. 阻尼比 (半功率带宽法)
peak_mag = frf_mag[idx_peak]
half_power = peak_mag - 3.0 # -3 dB
# 找半功率带宽
above_half = np.where(frf_mag > half_power)[0]
if len(above_half) > 1:
bw = omega[above_half[-1]] - omega[above_half[0]]
zeta = bw / (2 * omega_n)
else:
zeta = 0.1 # 默认值
# 5. 粘性摩擦
f_v = 2 * zeta * np.sqrt(K * J_eff)
return J_eff, f_v, K
# 验证: 生成理论 Bode 图并辨识
J_true, fv_true, K_true = 0.5, 2.0, 100.0
num = [1.0]
den = [J_true, fv_true, K_true]
sys_tf = signal.TransferFunction(num, den)
freq = np.logspace(-1, 2, 500) # 0.1 到 100 Hz
omega_eval = 2 * np.pi * freq
w, mag, phase = signal.bode(sys_tf, omega_eval)
J_id, fv_id, K_id = bode_identification(freq, mag, phase)
print(f"J_eff: 真实={J_true}, 辨识={J_id:.4f}")
print(f"f_v: 真实={fv_true}, 辨识={fv_id:.4f}")
print(f"K: 真实={K_true}, 辨识={K_id:.4f}")
5.5 频域最小二乘¶
对于多输入多输出（MIMO）系统，频域辨识可以这样做：
在每个激励频率 \(f_l\) 处，对输入 \(\hat{\tau}(f_l)\) 和输出 \(\hat{q}(f_l)\) 做 DFT，得到频域回归方程：
堆叠所有频率：
注意：DFT 结果是复数。可以将实部和虚部分开，得到实数最小二乘问题：
频域 WLS 的权重可以自然地设为每个频率点的噪声方差的倒数——用多次重复实验估计。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：DFT 频率分辨率不匹配激励频率
错误做法：激励频率 \(f_l\) 不是 DFT 频率网格 \(k f_s/N\) 的整数倍
现象：频谱泄漏——激励能量分散到多个频率 bin 中，FRF 估计出现偏差
根本原因：DFT 假设信号是周期的，只有当信号周期恰好是采样窗口长度的整数分之一时，频谱才是"干净"的
正确做法：确保激励周期 \(T_f\) 和采样窗口 \(T_{\text{window}} = N/f_s\) 满足 \(T_{\text{window}} = M \cdot T_f\)（\(M\) 为正整数）。或者用 Hanning 窗减轻泄漏
💡 概念误区：认为"时域辨识和频域辨识结果应该相同"
新手想法：同样的数据，时域和频域应该给出相同的参数估计
实际上：只有在所有频率都等权重、且数据无噪声时两者才完全等价。频域方法的优势在于**选择性加权**——可以丢弃噪声严重的高频数据，或给谐振频率附近更高的权重
延伸：频域方法还有一个独特优势——通过观察**频率响应函数的相干性**（coherence），可以判断每个频率点的信噪比，自动决定加权
🧠 思维陷阱：对非线性系统盲目使用频域方法
新手想法：Bode 图辨识对所有系统都适用
实际上：频率响应函数假设系统是线性时不变的（LTI）。机器人动力学是强非线性的——\(M(q)\) 依赖构型，Coriolis 项是 \(\dot{q}\) 的二次。在固定构型 \(q_0\) 附近做小幅扰动时可以近似线性化，但全范围运动时 Bode 图辨识不适用
正确做法：频域方法用于单关节参数辨识（固定其他关节）或机电参数辨识（电机+减速器）。全身动力学参数仍然用时域线性参数化方法
练习¶
练习 5.1 ⭐：对一个已知的二阶系统（\(J = 0.3\) kg\(\cdot\)m\(^2\), \(f_v = 1.5\) N\(\cdot\)m\(\cdot\)s/rad, \(K = 50\) N\(\cdot\)m/rad），生成多正弦激励信号（\(N_h = 20\), Schroeder 相位），模拟系统响应（加噪声），然后用 Bode 图方法辨识参数。比较辨识值与真实值。
练习 5.2 ⭐⭐：实现频域最小二乘辨识。将 4.7 节的辨识流程代码改为频域版本——对力矩和回归矩阵做 DFT，只取前 \(N_h\) 个谐波频率处的数据做最小二乘。比较时域和频域方法的辨识精度。
练习 5.3 ⭐⭐⭐：对一个 7-DOF 机械臂，设计奇数次谐波多正弦激励。分析偶数次谐波处的响应能量，判断 Coulomb + viscous 摩擦模型是否充分描述了实际摩擦。
6. 子空间辨识 (N4SID / MOESP) ⭐⭐⭐¶
6.1 动机：不知道模型结构怎么办？¶
第 3--4 节的方法有一个前提：我们知道机器人的运动方程结构，只是不知道参数。但在很多场景下：
- 柔性关节机器人的弹性参数未知，且结构复杂
- 机器人与环境交互（如打磨、擦拭），接触力学未知
- 你想对一个**黑箱**系统（如液压驱动器）建模
此时，我们需要一种**不假设模型结构**的辨识方法。子空间辨识就是这样的工具。
6.2 从输入输出到状态空间¶
考虑离散时间线性时不变系统：
其中 \(x_k \in \mathbb{R}^n\) 是状态（未知），\(u_k \in \mathbb{R}^m\) 是输入，\(y_k \in \mathbb{R}^l\) 是输出，\(w_k, v_k\) 是过程噪声和测量噪声。
目标：从输入输出数据 \(\{u_k, y_k\}_{k=1}^{N}\) 直接估计 \((A, B, C, D)\) 和系统阶数 \(n\)。
6.3 Hankel 矩阵构造¶
定义过去和未来的输入/输出 Hankel 矩阵。选取"窗口长度" \(i\)（通常 \(i > n\)），将数据排成块 Hankel 矩阵：
将此矩阵分为"过去"和"未来"两块：
类似地定义 \(Y_p, Y_f\)。
6.4 SVD 揭示系统阶数¶
子空间方法的核心步骤是构造**斜投影**（oblique projection）：
这个投影的物理含义是：从过去的输入输出中提取状态信息，去除未来输入的直接影响。
对 \(\mathcal{O}_i\) 做 SVD：
其中 \(\Sigma_s = \operatorname{diag}(\sigma_1, \sigma_2, \ldots)\)，\(\sigma_1 \geq \sigma_2 \geq \cdots\)。
系统阶数判定：观察奇异值的"间隙"（gap）。如果 \(\sigma_n \gg \sigma_{n+1} \approx 0\)，则系统阶数为 \(n\)。保留前 \(n\) 个奇异值：
6.5 从 SVD 到状态空间矩阵¶
扩展可观性矩阵：
从 \(\Gamma_i\) 可以直接提取 \(C\)（取前 \(l\) 行）和 \(A\)（利用 shift 性质 \(\underline{\Gamma}_i A = \overline{\Gamma}_i\)，其中 \(\underline{\Gamma}_i\) 去掉最后 \(l\) 行、\(\overline{\Gamma}_i\) 去掉前 \(l\) 行）：
状态序列：\(\hat{X} = \Sigma_n^{1/2} V_n^\top\)。
B, D 矩阵：做最小二乘
6.6 三大变体的统一视角¶
| 方法 | 提出者 | 核心技术 | 加权方式 | 优点 |
|---|---|---|---|---|
| N4SID | Van Overschee & De Moor (1994) | 斜投影 | 工具变量 + 加权 | 数值稳健 |
| MOESP | Verhaegen & Dewilde (1992) | LQ/RQ 分解 | 行空间分离 | 计算高效 |
| CVA | Larimore (1990) | 典型相关分析 | 典型相关权重 | 高斯下渐近有效 |
Van Overschee & De Moor (1995) 统一定理：三种方法的唯一区别在于对 \(\mathcal{O}_i\) 做 SVD 之前乘的**左右加权矩阵** \(W_1, W_2\)。统一形式：\(W_1 \mathcal{O}_i W_2 = U \Sigma V^\top\)。
6.7 与 DMD/Koopman 的联系¶
动态模式分解（Dynamic Mode Decomposition, DMD）是流体力学社区发展的方法，本质上是子空间辨识的特例：
- DMD：假设 \(x_{k+1} = Ax_k\)（无输入），从快照矩阵 \(X = [x_1, \ldots, x_{K-1}]\), \(X' = [x_2, \ldots, x_K]\) 求 \(A = X' X^+\)
- **带输入的 DMD（DMDc）**等价于子空间辨识的一步版本
**Koopman 算子理论**进一步将非线性系统嵌入到无穷维线性空间中：对可观函数 \(\phi(x)\)，Koopman 算子 \(\mathcal{K}\) 满足 \(\mathcal{K}\phi = \phi \circ F\)（\(F\) 是动力学映射）。有限维近似就变回了子空间辨识的设置——只是"状态"变成了提升空间（lifted space）中的特征。
实践含义：对非线性机器人（如柔性关节、接触），可以选择合适的可观函数（如 \([q, \dot{q}, \sin q, \cos q, q\dot{q}]\)），然后对提升数据做标准的子空间辨识。
# 子空间辨识的 Python 实现骨架
import numpy as np
from scipy.linalg import svd, lstsq
def n4sid(u_data, y_data, i, n_order):
"""N4SID 子空间辨识
Args:
u_data: 输入序列 (N, m)
y_data: 输出序列 (N, l)
i: 块行数 (> n_order)
n_order: 期望的系统阶数
Returns:
A, B, C, D: 状态空间矩阵
"""
N = len(u_data)
m = u_data.shape[1] if u_data.ndim > 1 else 1
l = y_data.shape[1] if y_data.ndim > 1 else 1
j = N - 2*i + 1 # 列数
# 构建 Hankel 矩阵
def hankel_block(data, start, rows, cols):
if data.ndim == 1:
data = data.reshape(-1, 1)
d = data.shape[1]
H = np.zeros((rows * d, cols))
for r in range(rows):
for c in range(cols):
H[r*d:(r+1)*d, c] = data[start + r + c]
return H
U_p = hankel_block(u_data, 0, i, j)
U_f = hankel_block(u_data, i, i, j)
Y_p = hankel_block(y_data, 0, i, j)
Y_f = hankel_block(y_data, i, i, j)
# 斜投影 (简化版: 用正交投影近似)
W_p = np.vstack([U_p, Y_p])
# 去除 U_f 的影响
proj_Uf = U_f.T @ np.linalg.pinv(U_f @ U_f.T) @ U_f
Y_f_perp = Y_f @ (np.eye(j) - proj_Uf.T)
W_p_perp = W_p @ (np.eye(j) - proj_Uf.T)
O_i = Y_f_perp @ np.linalg.pinv(W_p_perp) @ W_p
# SVD
U_svd, S_svd, Vt_svd = svd(O_i)
# 截断
U_n = U_svd[:, :n_order]
S_n = np.diag(S_svd[:n_order])
V_n = Vt_svd[:n_order, :]
# 扩展可观性矩阵
Gamma = U_n @ np.sqrt(S_n)
# 提取 C
C = Gamma[:l, :]
# 提取 A (shift property)
Gamma_up = Gamma[:-l, :] # 去掉最后 l 行
Gamma_down = Gamma[l:, :] # 去掉前 l 行
A = np.linalg.lstsq(Gamma_up, Gamma_down, rcond=None)[0]
# 状态序列
X_hat = np.sqrt(S_n) @ V_n
# 估计 B, D
# x_{k+1} = A x_k + B u_k
# y_k = C x_k + D u_k
n_data = min(X_hat.shape[1] - 1, j - 1)
LHS = np.vstack([X_hat[:, 1:n_data+1], y_data[i:i+n_data].T])
RHS = np.vstack([X_hat[:, :n_data], u_data[i:i+n_data].T])
ABCD = LHS @ np.linalg.pinv(RHS)
B = ABCD[:n_order, n_order:]
D = ABCD[n_order:, n_order:]
return A, B, C, D
# 使用 scipy 的 N4SID (更工业化)
# from sippy import system_identification
# sys_id = system_identification(y_data, u_data, 'N4SID', SS_order=n)
⚠️ 常见陷阱¶
⚠️ 编程陷阱：Hankel 矩阵窗口长度 \(i\) 选取不当
错误做法：\(i\) 选得太小（\(i \leq n\)，其中 \(n\) 是系统阶数）
现象：SVD 奇异值没有明显间隙，辨识出的系统矩阵数值不稳定
根本原因：\(i\) 必须大于系统阶数 \(n\)，否则扩展可观性矩阵 \(\Gamma_i\) 不满秩。但 \(i\) 太大（\(i \gg n\)）会导致 Hankel 矩阵列数减少（\(j = N - 2i + 1\)），统计效率下降
正确做法：经验法则 \(i \approx 2n\) 到 \(3n\)。如果 \(n\) 未知，从大的 \(i\) 开始，逐渐减小，观察奇异值间隙
💡 概念误区：认为子空间辨识可以直接用于机器人动力学参数辨识
新手想法：子空间方法不需要模型结构，可以直接辨识机器人
实际上：子空间方法辨识的是线性时不变系统的 \((A, B, C, D)\) 矩阵——机器人是非线性的，需要先在工作点线性化。辨识出的矩阵只在该工作点附近有效。而且 \((A, B, C, D)\) 是一个整体，不容易分解为物理参数（质量、惯性矩等）
正确用途：子空间方法适用于 (1) 柔性关节的弹性模态辨识；(2) 未知黑箱子系统（液压、气动）；(3) 控制器设计不需要物理参数时（直接用状态空间做 LQR/MPC）
🧠 思维陷阱：认为系统阶数选择只看奇异值间隙
新手想法：奇异值间隙明显，阶数就确定了
实际上：噪声会模糊间隙。多种方法应该交叉验证：(1) 奇异值间隙；(2) 不同阶数下的模型在验证数据上的拟合度（BIC/AIC 准则）；(3) 系统极点的稳定性（随阶数增加，物理极点应不变，虚假极点会变化）
正确做法：画出"稳定图"（stabilization diagram）——横轴是阶数，纵轴是极点位置，稳定极点（多个阶数都出现的）是真实的
⭐ 自测检查点 6.1：子空间辨识的核心步骤是对 Hankel 矩阵做 SVD。解释：(1) 奇异值的"间隙"为什么能指示系统阶数？(2) 如果没有明显间隙，说明什么？
答：(1) 在无噪声情况下，扩展可观性矩阵的秩恰好等于系统阶数 \(n\)，所以只有前 \(n\) 个奇异值非零。有噪声时，非零奇异值会有小的扰动，但前 \(n\) 个仍然显著大于后面的——这就是"间隙"。(2) 没有明显间隙可能意味着：系统阶数难以确定（需要更多数据），或者系统是非线性的（线性模型不适用），或者信噪比太低。
练习¶
练习 6.1 ⭐⭐：用 N4SID 辨识一个弹簧-阻尼-质量系统（\(m = 1\) kg, \(c = 0.5\) N\(\cdot\)s/m, \(k = 10\) N/m）。生成仿真数据（白噪声激励），验证辨识出的极点与理论值一致。
练习 6.2 ⭐⭐：比较 N4SID 和 MOESP 在同一组数据上的辨识结果。两者的 \((A, B, C, D)\) 不同，但传递函数应该相同（相似变换）——验证这一点。
练习 6.3 ⭐⭐⭐：对 2-DOF 机械臂（3.2 节），在某个固定构型附近线性化，然后用子空间方法辨识。将辨识出的 \(A\) 矩阵的特征值与理论线性化模型的特征值对比。
7. 递归/在线辨识 ⭐⭐¶
7.1 动机：为什么需要在线辨识？¶
离线辨识（batch identification）需要先采集完所有数据再计算。但在很多场景下，参数是**时变的**：
- 机器人抓起一个未知负载，惯性突然改变
- 减速器磨损，摩擦系数缓慢漂移
- 温度变化导致连杆热膨胀
我们需要一种**每收到一个新数据点就更新参数估计**的方法——这就是递归辨识。
7.2 RLS（递归最小二乘）的完整推导¶
出发点：batch OLS 的正规方程
定义：\(P_k = \left(\sum_{i=1}^{k} Y_i^\top Y_i\right)^{-1}\)（参数协方差矩阵的比例因子）。
递推关系：当第 \(k+1\) 个数据 \((Y_{k+1}, \tau_{k+1})\) 到来时：
直接对 \(P_k^{-1}\) 递推需要矩阵求逆——计算量 \(O(p^3)\)。利用 矩阵求逆引理（Woodbury identity）：
令 \(A = P_k^{-1}\), \(B = Y_{k+1}^\top\), \(C = I\), \(D = Y_{k+1}\)，得到：
定义**增益矩阵**：
则协方差更新为：
参数更新为：
完整的 RLS 算法：
初始化：\(\hat{\pi}_0 = 0\)（或 CAD 标称值），\(P_0 = \alpha I\)（\(\alpha\) 取较大值，如 \(10^4\)）。
计算复杂度：每步 \(O(p^2)\)（\(p\) 是参数个数），比 batch OLS 的 \(O(Kp^2 + p^3)\) 好得多（对于在线应用）。
7.3 遗忘因子¶
标准 RLS 赋予所有历史数据相同权重。对时变参数，旧数据应该"淡忘"。引入**遗忘因子** \(\lambda \in (0, 1]\)：
修改后的 RLS 更新：
\(\lambda\) 的选择：
| \(\lambda\) 值 | 等效记忆长度 \(\approx 1/(1-\lambda)\) | 适用场景 |
|---|---|---|
| 1.0 | \(\infty\)（不遗忘） | 恒定参数 |
| 0.99 | 100 步 | 缓慢漂移 |
| 0.95 | 20 步 | 快速变化 |
| 0.90 | 10 步 | 突变检测 |
7.4 RLS 与 Kalman 滤波的等价性¶
惊人的事实：RLS 可以看作一个特殊的 Kalman 滤波器。
将辨识问题建模为：
这是一个**线性高斯状态估计问题**——Kalman 滤波给出最优解。Kalman 滤波的更新步骤恰好就是 RLS（\(F = I\)，\(H_k = Y_k\)，\(Q = 0\)）：
| Kalman 滤波 | RLS |
|---|---|
| 状态 \(\hat{x}_k\) | 参数 \(\hat{\pi}_k\) |
| 状态协方差 \(P_k\) | 参数协方差 \(P_k\) |
| 观测矩阵 \(H_k\) | 回归向量 \(Y_k\) |
| 过程噪声 \(Q\) | \(0\)（恒定参数）或 \((1/\lambda - 1)P_k\)（遗忘因子） |
| Kalman 增益 \(K_k\) | RLS 增益 \(K_k\) |
推论：如果参数本身是随机游走 \(\pi_{k+1} = \pi_k + w_k\)（\(w_k\) 代表参数漂移），那么 Kalman 滤波（\(Q \neq 0\)）是比带遗忘因子 RLS 更好的选择——它可以**分别调整每个参数方向的遗忘速率**。
7.5 与自适应控制 (控制理论·自适应控制（规划中）) 的无缝对接¶
RLS 在线辨识是自适应控制的核心组件。以 Certainty Equivalence 自适应控制为例：
- 辨识：每一步用 RLS 更新参数估计 \(\hat{\pi}_k\)
- 控制：假设 \(\hat{\pi}_k\) 就是真实参数，设计控制律（如 computed torque）
这就是 间接自适应控制（indirect adaptive control）。与 2.4 节中 Slotine & Li 的**直接自适应控制**的区别：
| 直接自适应 (Slotine & Li) | 间接自适应 (RLS + CE) | |
|---|---|---|
| 参数更新律 | 梯度下降式 \(\dot{\hat{\pi}} = -\Gamma Y^\top s\) | RLS（最优滤波） |
| 稳定性保证 | Lyapunov 直接证明 | 需要 PE 条件（持续激励） |
| 收敛速度 | 慢（梯度法） | 快（二阶方法，等价于 Newton） |
| 适用范围 | 非线性系统（含 Coriolis） | 需要参数分离性（regressor 形式） |
| 实践难度 | \(\Gamma\) 调参困难 | \(\lambda\), \(P_0\) 调参较直观 |
关键联系：两种方法都依赖**同一个回归矩阵 \(Y\)**——本章 3--4 节的构造方法是共通的基础。
7.6 应用：在线负载估计¶
一个经典应用：机器人抓取未知负载后，在线估计负载的质量、质心和惯性。
负载的效果等价于修改末端连杆的惯性参数。设负载参数为 \(\pi_{\text{load}} = [m_L, m_L c_{x,L}, m_L c_{y,L}, m_L c_{z,L}, I_{xx,L}, \ldots]^\top\)，则：
如果 \(\pi_{\text{robot}}\) 已经离线辨识好，残差 \(\tau - Y_{\text{robot}}\pi_{\text{robot}} = Y_{\text{load}}\pi_{\text{load}}\) 就可以用 RLS 在线估计 \(\pi_{\text{load}}\)。
7.7 C++ Eigen 实现：RLS（正确 vs 错误 vs 对比）¶
Step 1: 为什么 RLS 在实时控制中比 batch OLS 更合适？
Batch OLS 需要存储所有历史回归矩阵（内存 \(O(Kp)\)，计算 \(O(Kp^2)\)）。在 1 kHz 控制循环中，每秒产生 1000 行数据，一分钟就是 60000 行——存储和计算都不可行。RLS 只维护 \(p \times p\) 的协方差矩阵和 \(p \times 1\) 的参数向量，每步 \(O(p^2)\)，内存恒定。
Step 2: 正确写法
// ✅ 正确: 工业级 RLS 实现
#include <Eigen/Dense>
class RecursiveLeastSquares {
public:
RecursiveLeastSquares(int n_params, double alpha = 1e4, double lambda = 0.99)
: n_(n_params), lambda_(lambda)
{
// ✅ 显式初始化所有成员
theta_ = Eigen::VectorXd::Zero(n_);
P_ = alpha * Eigen::MatrixXd::Identity(n_, n_);
}
void update(const Eigen::MatrixXd& Y, const Eigen::VectorXd& tau) {
// 预测误差
Eigen::VectorXd e = tau - Y * theta_;
// 增益矩阵
Eigen::MatrixXd S = lambda_ * Eigen::MatrixXd::Identity(Y.rows(), Y.rows())
+ Y * P_ * Y.transpose();
Eigen::MatrixXd K = P_ * Y.transpose() * S.inverse();
// 参数更新
theta_ += K * e;
// ✅ 协方差更新 (Joseph 形式, 数值更稳定)
Eigen::MatrixXd I_KY = Eigen::MatrixXd::Identity(n_, n_) - K * Y;
P_ = (1.0 / lambda_) * (I_KY * P_ * I_KY.transpose()
+ K * Eigen::MatrixXd::Identity(Y.rows(), Y.rows()) * K.transpose());
// ✅ 强制对称性 (防止数值漂移)
P_ = 0.5 * (P_ + P_.transpose());
}
const Eigen::VectorXd& getParams() const { return theta_; }
const Eigen::MatrixXd& getCovariance() const { return P_; }
Eigen::VectorXd getStdDev() const {
return P_.diagonal().cwiseSqrt();
}
// ✅ 置信区间检查
bool isConverged(double threshold = 0.01) const {
Eigen::VectorXd rel_std = getStdDev().array() / theta_.array().abs().max(1e-10);
return (rel_std.array() < threshold).all();
}
private:
int n_;
double lambda_;
Eigen::VectorXd theta_;
Eigen::MatrixXd P_;
};
Step 3: 错误写法及分析
// ❌ 错误 1: 使用简单形式的协方差更新
void update_WRONG_simple(const Eigen::MatrixXd& Y, const Eigen::VectorXd& tau) {
Eigen::VectorXd e = tau - Y * theta_;
Eigen::MatrixXd K = P_ * Y.transpose() *
(lambda_ * Eigen::MatrixXd::Identity(Y.rows(), Y.rows()) + Y * P_ * Y.transpose()).inverse();
theta_ += K * e;
// ❌ 简单形式: P = (I - KY)P / lambda
// 由于浮点舍入, P 会逐渐变得不对称, 然后不正定, 最终算法崩溃
P_ = (1.0 / lambda_) * (Eigen::MatrixXd::Identity(n_, n_) - K * Y) * P_;
}
// ❌ 错误 2: 遗忘因子导致协方差爆炸, 无保护
void update_WRONG_no_covariance_bound(const Eigen::MatrixXd& Y, const Eigen::VectorXd& tau) {
// 当 Y ≈ 0 (无激励) 时, P = P / lambda 指数增长!
// 没有上界保护, P 的元素可能到 1e100, 下一次激励时参数跳变
Eigen::VectorXd e = tau - Y * theta_;
Eigen::MatrixXd K = P_ * Y.transpose() *
(lambda_ * Eigen::MatrixXd::Identity(Y.rows(), Y.rows()) + Y * P_ * Y.transpose()).inverse();
theta_ += K * e;
Eigen::MatrixXd I_KY = Eigen::MatrixXd::Identity(n_, n_) - K * Y;
P_ = (1.0 / lambda_) * (I_KY * P_ * I_KY.transpose()
+ K * Eigen::MatrixXd::Identity(Y.rows(), Y.rows()) * K.transpose());
// 缺少: P_ = P_.cwiseMin(max_cov); 或 trace-bounded 修正
}
// ❌ 错误 3: 初始化 P0 太小
RecursiveLeastSquares rls_wrong(10, 0.01, 0.99);
// P0 = 0.01 * I -> 增益 K 很小 -> 参数更新极慢 -> 可能需要 10000 步才收敛
// 正确: P0 应为 1e3 ~ 1e5 * I (表示"对初始参数不确定")
Step 4: 不同实现策略对比
| 策略 | 数值稳定性 | 每步计算量 | 适用场景 |
|---|---|---|---|
| Joseph 形式 + 对称化 | 高 | \(O(p^2 + mp)\) | 通用推荐 |
| 简单形式 | 低 | \(O(p^2)\) | 仅用于短期运行 |
| Square-root RLS | 最高 | \(O(p^2)\) | 高精度需求 |
| UD 分解 RLS | 高 | \(O(p^2)\) | 嵌入式系统 |
// 使用示例: 7-DOF 机械臂在线负载估计
/*
// 已知: 机器人基参数 pi_robot (离线辨识)
// 目标: 在线估计负载参数 pi_load (10 个: m, mc_x, mc_y, mc_z, Ixx,...,Izz)
RecursiveLeastSquares rls(10, 1e4, 0.995);
// 控制循环 (1 kHz)
for (int k = 0; k < N_steps; ++k) {
// 读取传感器
Eigen::VectorXd q = readEncoders();
Eigen::VectorXd v = readVelocities();
Eigen::VectorXd a = computeAcceleration(v); // 滤波 + 微分
Eigen::VectorXd tau = readTorqueSensors();
// 计算已知机器人部分的力矩
Eigen::VectorXd tau_robot = pinocchio::rnea(model, data, q, v, a);
// 残差 = 负载贡献
Eigen::VectorXd tau_load = tau - tau_robot;
// 负载回归矩阵 (只对末端连杆)
Eigen::MatrixXd Y_load = computeEndEffectorRegressor(model, data, q, v, a);
// RLS 更新
rls.update(Y_load, tau_load);
// 读取估计结果
double load_mass = rls.getParams()(0);
double load_mass_std = rls.getStdDev()(0);
if (k % 100 == 0) {
std::cout << "Step " << k
<< " | Load mass: " << load_mass
<< " +/- " << 2*load_mass_std << " kg" << std::endl;
}
}
*/
7.8 实际案例：四足机器人在线质量估计¶
四足机器人在搬运货物时需要实时估计负载质量以调整步态。这是 RLS 的一个重要应用场景。
问题设置：四足机器人站立状态下，总的垂直支撑力等于总重力：
其中 \(F_{z,i}\) 是第 \(i\) 条腿的垂直地面反作用力（可从关节力矩和运动学计算得到）。
更一般地，在运动中：
简化模型（只估计负载质量 \(m_L\) 和质心位置 \(\mathbf{c}_L\)）：
这是 4 个参数，RLS 可以在 50--200 步内收敛（在 1 kHz 控制循环中，仅需 50--200 ms）。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：遗忘因子导致协方差爆炸
错误做法：\(\lambda = 0.95\) 但机器人长时间保持静止（无激励）
现象：\(P\) 的 trace 从初始的 \(10^4\) 增长到 \(10^{15}\)，下一次运动时参数估计"跳"到荒谬的值
根本原因：\(\lambda < 1\) 且 \(Y_{k+1} \approx 0\) 时，\(P_{k+1} = P_k / \lambda\)——协方差指数增长。增益 \(K\) 随之暴增，导致参数跳变
正确做法： - 使用**可变遗忘因子**：\(\lambda_k = 1 - (1-\lambda_0)\|Y_k P_k Y_k^\top\|_F / \text{threshold}\) - 协方差上限截断：if \(\text{trace}(P) > P_{\max}\) then \(P \leftarrow P_{\max}/\text{trace}(P) \cdot P\) - 改用 directional forgetting（只在受激励的方向上遗忘）
💡 概念误区：认为 RLS 的初始参数 \(\hat{\pi}_0\) 不重要
新手想法：反正 RLS 会收敛，初始值随便设
实际上：如果 \(\hat{\pi}_0\) 偏离真实值太远，前几步的控制力矩可能很大（因为 computed torque 控制律直接使用 \(\hat{\pi}_k\)），导致机器人抖动甚至不稳定。应该用 CAD 标称值初始化
正确做法：\(\hat{\pi}_0\) = CAD 标称值（或离线辨识值），\(P_0\) 的对角元设为参数不确定度的平方
🧠 思维陷阱：认为"持续激励条件"(PE) 自动满足
新手想法：机器人在运动中就自然满足 PE 条件
实际上：PE 要求 \(\sum_{i=k-T}^{k} Y_i^\top Y_i \succ \alpha I\)（某个窗口内的信息矩阵正定）。如果机器人只做重复运动（如工业流水线上的 pick-and-place），某些参数方向可能始终不被激励——RLS 在这些方向上不收敛
正确做法：(1) 检查 \(P_k\) 的最大特征值——如果某个方向长期不减小，说明该方向缺乏激励；(2) 定期注入激励信号（dither signal）——在期望轨迹上叠加小幅随机扰动
连接 控制理论·自适应控制（规划中）：PE 条件是自适应控制收敛性证明的核心假设。本章的 RLS 和 控制理论·自适应控制（规划中） 的自适应律都需要它
⭐ 自测检查点 7.1：RLS 的协方差更新中，为什么使用 Joseph 形式 \((I - KY)P(I-KY)^\top + KK^\top\) 而非简单形式 \((I-KY)P\)？
答：简单形式虽然数学上等价，但由于浮点舍入误差的累积，\(P\) 可能逐渐变得不对称或不正定——一旦不正定，整个算法就崩溃了。Joseph 形式**自动保持 \(P\) 的对称正定性**，代价是多做一次矩阵乘法。
练习¶
练习 7.1 ⭐：实现 RLS 并在 2-DOF 机械臂上测试。在 \(t = 5\) s 时突然改变连杆 2 的质量（模拟抓取负载），观察参数估计的收敛速度。比较 \(\lambda = 0.99, 0.995, 0.999\) 的效果。
练习 7.2 ⭐⭐：实现可变遗忘因子 RLS。在机器人静止阶段自动切换到 \(\lambda = 1\)（不遗忘），运动阶段恢复 \(\lambda < 1\)。验证这是否解决了协方差爆炸问题。
练习 7.3 ⭐⭐：用 Kalman 滤波实现在线辨识，设过程噪声 \(Q = \text{diag}(\sigma_q^2)\)（每个参数独立漂移）。比较 Kalman 滤波和 RLS（遗忘因子）在参数突变和缓慢漂移两种场景下的表现。
练习 7.4 ⭐⭐⭐：在四足机器人仿真中（PyBullet 或 MuJoCo），实现在线负载估计。机器人行走时在背上放置不同质量的负载（0.5, 1.0, 2.0 kg），用 RLS 实时估计负载质量。绘制估计值随时间的收敛曲线。
8. 子空间辨识与数据驱动方法 ⭐⭐⭐¶
8.1 动机：当你不知道模型结构¶
前面所有方法都假设你**已经知道**系统的结构（刚体动力学方程），只是不知道参数值。但现实中很多情况下：
- 柔性机器人的柔性建模非常复杂，你不确定用几阶模态
- 液压驱动系统的非线性摩擦模型未知
- 机器人与环境交互时，环境动力学完全未知
这时需要**黑箱辨识**：直接从 I/O 数据辨识状态空间模型 \((A, B, C, D)\)，不假设任何物理结构。
8.2 子空间辨识 (N4SID/MOESP) 核心思想¶
核心思想：利用 Hankel 矩阵的列空间结构，从 I/O 数据直接提取系统的可观测子空间。
输入输出 Hankel 矩阵： $\(\mathcal{H} = \begin{bmatrix} Y_p \\ Y_f \\ U_p \\ U_f \end{bmatrix}\)$
其中 \(Y_p, Y_f\) 是"过去"和"未来"输出的 Hankel 块，\(U_p, U_f\) 类似。
关键定理（Van Overschee-De Moor 1996）：对 LTI 系统，\(Y_f\) 在 \(U_f\) 消去后的列空间的左奇异向量包含可观测矩阵 \(\mathcal{O} = [C; CA; CA^2; \ldots]\)，奇异值揭示系统阶数 \(n\)。
算法步骤： 1. 构建 Hankel 矩阵 2. 做 oblique/orthogonal 投影消去输入影响 3. SVD 确定系统阶数 \(n\)（奇异值 gap） 4. 从左奇异向量提取 \(\mathcal{O}\)，进而得到 \((A, C)\) 5. 用最小二乘从状态序列估计 \((B, D)\)
8.3 与 DeePC 的深层联系¶
回顾 §3.13.D（鲁棒 MPC 专题）：DeePC 利用 Willems 基本引理，用 Hankel 矩阵的列空间**直接**预测未来输出，跳过了显式辨识。
| 方法 | 步骤 | 优势 | 劣势 |
|---|---|---|---|
| 子空间辨识 + MPC | 数据 → 辨识 \((A,B,C,D)\) → 设计 MPC | 可解释、可验证 | 两步误差累积 |
| DeePC | 数据 → 直接预测控制 | 一步到位、简单 | 不可解释、线性限制 |
等价性定理：对确定性 LTI 系统，DeePC 与"子空间辨识 + 基于辨识模型的 MPC"给出**完全相同**的控制输入。两者的区别只在于噪声处理方式不同。
8.4 Koopman 算子辨识（2020-2025 前沿）¶
动机：子空间辨识只适用于线性系统。对非线性系统，能否也用数据驱动方法直接辨识一个"线性"模型？
Koopman 算子（1931 年提出，2010 年后复兴）：对非线性系统 \(x_{k+1} = f(x_k)\)，存在无穷维线性算子 \(\mathcal{K}\) 作用于**观测函数空间**：
有限维近似：选择字典函数 \(\psi(x) = [\psi_1(x), \ldots, \psi_L(x)]^\top\)（如多项式、RBF、Fourier），学习有限维 Koopman 矩阵 \(K \in \mathbb{R}^{L \times L}\)：
Extended DMD (EDMD)：最简单的学习方法 $\(K = \Psi_+ \Psi^+, \quad \Psi = [\psi(x_1), \ldots, \psi(x_N)]\)$
其中 \(\Psi_+ = [\psi(x_2), \ldots, \psi(x_{N+1})]\)。
与机器人辨识的联系： - 四足机器人的步态切换：不同步态阶段用不同 Koopman 模型 - 柔性机器人：Koopman 自动提取柔性模态 - 接触动力学：接触-非接触切换对 Koopman 是挑战（需要 switching Koopman）
Brunton 2022 综述（SIAM Review）的核心观点：Koopman 辨识是**非线性系统辨识的未来范式之一**——它把"非线性"转化为"高维线性"，使得所有线性控制工具（LQR、Kalman、MPC）都可以直接用。代价是维度 \(L\) 可能很大（100-1000 维）。
8.5 与自适应控制的联系¶
子空间/Koopman 辨识给出的线性模型可以直接用于： - Model Reference Adaptive Control (MRAC)：用辨识模型作为参考模型 - L1 Adaptive Control：辨识提供的名义模型 + 自适应补偿未建模动力学 - Indirect Adaptive MPC：每隔 \(T_{\text{id}}\) 步重新辨识，更新 MPC 模型
跨领域类比：子空间辨识之于系统辨识，就像 PCA 之于统计学——都是"从高维数据中提取低维结构"的方法。子空间辨识的"系统阶数"对应 PCA 的"主成分个数"，两者都通过"奇异值 gap"确定。
⚠️ 常见陷阱¶
💡 概念误区：认为 Koopman 辨识"把非线性系统变成了线性系统"
新手想法："学到了 Koopman 矩阵 \(K\) 后，我的系统就是线性的了，所有线性工具都能用！"
实际上：Koopman **不改变**系统本身——它只是在更高维空间中找了一个线性表示。(1) 近似是有误差的（有限维截断）；(2) 提升空间的状态 \(\psi(x)\) 需要通过非线性变换从物理状态 \(x\) 获取；(3) 控制设计在提升空间中是线性的，但映射回物理空间可能有约束处理问题
正确认知：Koopman 是一种**近似**工具，适合"弱非线性"或"可以用适当字典捕捉"的系统。强非线性、混杂动力学仍需要专门方法
🧠 思维陷阱：认为"数据驱动方法不需要物理知识"
新手想法："DeePC/Koopman 都是 data-driven，所以不需要了解系统物理"
实际上：数据驱动方法的成功**高度依赖**数据质量和激励设计，而好的激励设计需要物理直觉。例如：辨识腿足机器人需要在不同步态、不同速度下采集数据；辨识柔性臂需要激励低频模态。没有物理知识，你连"什么算好的激励"都不知道
正确认知：数据驱动方法是"物理知识 + 数据"的组合，不是"无需物理"的黑魔法
练习¶
练习 8.1 ⭐⭐：用 Python sklearn 的 PCA 对一组机器人 I/O 数据做奇异值分析，确定系统阶数。比较 SVD gap 方法与 AIC/BIC 准则给出的阶数。
练习 8.2 ⭐⭐⭐：对一个简单的非线性系统（如 van der Pol 振荡器），实现 EDMD。选择多项式字典 \(\psi(x) = [1, x_1, x_2, x_1^2, x_1 x_2, x_2^2, \ldots]\)（阶 ≤ 3），验证 Koopman 近似在多大范围内有效。
练习 8.3 ⭐⭐⭐（跨章综合）：结合本章 §4（激励轨迹设计）和 §3.13.D（DeePC），设计一个完整的数据驱动控制流程：(1) 设计最优激励轨迹；(2) 用激励数据构建 Hankel 矩阵；(3) 实现 DeePC 控制器；(4) 在仿真中验证跟踪性能。
本章常见误解汇总¶
| 误解 | 正确理解 |
|---|---|
| 辨识出的惯性参数就是物体的真实物理参数 | 辨识得到的是基参数（base parameters）——多个物理参数的线性组合；由于参数耦合（如连杆质量和相邻关节的电机惯量），单个物理参数通常不可辨识 |
| 随便给一条运动轨迹就能做辨识 | 激励轨迹必须满足持续激励条件（回归矩阵 \(\Phi\) 列满秩），否则参数不可辨识；D-最优设计通过最大化 \(\det(\Phi^\top \Phi)\) 来优化激励信号 |
| 辨识精度只取决于数据量 | 关节速度和加速度的数值微分引入巨大噪声，低通滤波器的截止频率选择和采样率对精度的影响远大于数据量 |
| 子空间辨识（N4SID）可以精确辨识非线性系统 | 子空间辨识假设系统是线性时不变（LTI），对非线性系统只能在工作点附近给出局部线性模型；Koopman/EDMD 方法通过升维将非线性"提升"为高维线性，但有效范围仍有限 |
| 在线辨识（RLS）可以无限期运行而不退化 | 不带遗忘因子的 RLS 随时间推移协方差矩阵趋零（"估计器冻结"），失去对新数据的响应能力；必须使用遗忘因子或协方差重置机制 |
| 辨识出的模型可以直接用于控制而无需验证 | 辨识模型可能在训练数据范围外失效（外推不可靠），且可能不满足物理一致性（如负质量、不对称惯量张量）；部署前需交叉验证和物理约束检查 |
| 频域辨识和时域辨识给出相同的结果 | 两者在理想条件下渐近等价，但在有限数据/有色噪声下结果可能不同；频域方法对周期噪声更鲁棒，时域方法对瞬态响应建模更直接 |
本章小结¶
| 方法 | 核心思想 | 适用场景 | 数学工具 | 与控制的联系 |
|---|---|---|---|---|
| OLS/WLS | 线性回归 | 离线辨识已知结构 | 最小二乘 | computed torque |
| 基参数 | 找可辨识组合 | 减少参数维度 | QR 分解/SVD | 所有模型控制 |
| 激励轨迹 | D-最优设计 | 最大化信息 | 优化/Fourier | 提高辨识精度 |
| 频域辨识 | Bode 图拟合 | 单入单出 | FFT/传递函数 | 经典控制 |
| RLS | 递归更新 | 在线/实时 | Kalman 滤波 | 自适应控制 |
| 子空间辨识 | 黑箱状态空间 | 未知结构系统 | SVD/投影 | MPC |
| Koopman | 非线性→高维线性 | 弱非线性 | DMD/EDMD | 线性 MPC |
| DeePC | 数据直接预测 | 无模型控制 | Hankel/引理 | 数据驱动 MPC |
故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关节 |
|---|---|---|---|
| OLS 残差很大但不收敛 | 激励不足或模型结构错误 | 1.检查 \(Y^\top Y\) 条件数；2.增加 Fourier 阶数；3.检查是否遗漏摩擦项 | §4, §3.4 |
| 辨识参数物理不合理（负质量） | 噪声/数值问题 | 1.加物理约束(NNLS)；2.增加数据量；3.检查加速度滤波 | §5.3 |
| RLS 参数突然跳变 | 协方差爆炸或突然激励 | 1.检查 \(\text{trace}(P)\)；2.加协方差上限；3.改用可变遗忘因子 | §7.6 |
| 子空间辨识阶数不确定 | SVD gap 不明显 | 1.增加数据长度；2.降低噪声（均值滤波）；3.用 AIC/BIC 交叉验证 | §8.2 |
| 辨识后仿真轨迹发散 | 辨识模型不稳定 | 1.检查 \(A\) 特征值；2.加稳定性约束；3.用频域方法交叉验证 | §6 |
累积项目：本章新增模块¶
在"从零构建机械臂控制器"累积项目中，本章新增： 1. 离线辨识模块：Fourier 激励 + OLS/WLS + 基参数提取 2. 在线辨识模块：RLS 负载估计 + 可变遗忘因子 3. 验证模块：用辨识参数做 computed torque 控制，与 CAD 参数对比
项目代码位于 projects/manipulator_control/sysid/。
9. 频域辨识方法 ⭐⭐¶
9.1 动机：为什么需要频域视角¶
时域辨识（OLS/WLS/RLS）是机器人辨识的主流方法，但频域方法在某些场景下有独特优势：
场景 1：柔性机器人。柔性关节/连杆有多个谐振频率，在频域中表现为 Bode 图的峰值。时域方法需要猜测模态数量，频域方法可以直接从 Bode 图"看到"有几个模态。
场景 2：执行器建模。电机+减速器+传感器的组合传递函数在频域中清晰可见——低频增益、带宽、谐振、高频衰减一目了然。
场景 3：控制器设计验证。闭环带宽、相位裕度等经典控制指标是频域概念。如果辨识目的是设计频域控制器（如 \(H_\infty\)、loop shaping），直接在频域辨识更自然。
9.2 多正弦激励与 DFT¶
多正弦信号： $\(u(t) = \sum_{k=1}^{K} A_k \sin(2\pi f_k t + \phi_k)\)$
选择频率 \(\{f_1, \ldots, f_K\}\) 对数等间距覆盖感兴趣的频带（如 0.1 Hz - 100 Hz），幅值 \(A_k\) 按 Schroeder 相位设计使信号峰值因子最小。
频率响应估计（非参数化）： $\(G(j\omega_k) = \frac{Y(j\omega_k)}{U(j\omega_k)}\)$
其中 \(Y, U\) 是输出和输入的 DFT。多周期平均可显著降低噪声影响。
9.3 与时域方法的对比¶
| 维度 | 时域 (OLS/WLS) | 频域 (Bode 辨识) |
|---|---|---|
| 模型结构 | 需要已知（如刚体动力学） | 可以非参数化 |
| 噪声处理 | 需要加速度估计（放大噪声） | 频率选择性强（抗噪） |
| 非线性处理 | 直接处理（线性参数化） | 需要小信号线性化 |
| 多输入多输出 | 需要解耦 | 自然支持 MIMO |
| 在线应用 | RLS 直接支持 | 需要完整周期 |
工程建议：对刚体机器人用时域方法（§3-7），对柔性/执行器子系统用频域方法。两者互补而非替代。
练习¶
练习 9.1 ⭐⭐：对一个单关节机器人（电机 + 谐波减速器），设计多正弦激励信号，用 FFT 估计从力矩输入到关节角输出的频率响应。识别谐振频率。
练习 9.2 ⭐⭐⭐：用辨识到的频率响应设计一个 notch filter 抑制谐振，与没有 notch filter 的 PD 控制对比性能。
延伸阅读¶
| 资料 | 内容 | 难度 |
|---|---|---|
| Atkeson et al. 1986 IJRR | 奠基论文 | ⭐⭐ |
| Khalil & Dombre 2002 Ch.13 | 教科书处理 | ⭐⭐ |
| Swevers et al. 2007 IEEE CSM | 激励设计综述 | ⭐⭐⭐ |
| Ljung 1999 教材 | 系统辨识经典 | ⭐⭐⭐⭐ |
| Brunton et al. 2022 SIAM Review | Koopman/DMD 综述 | ⭐⭐⭐ |
| Coulson et al. 2019 ECC | DeePC 原始论文 | ⭐⭐⭐ |
| Pinocchio 文档 | computeJointTorqueRegressor | ⭐⭐ |
| Drake 教程 | InverseDynamicsController | ⭐⭐ |
中文资源：
- 知乎"机器人动力学参数辨识"系列（含 Pinocchio 代码）
- B 站"机械臂系统辨识实验"（Fourier 激励 + OLS 全流程）
- CSDN"Pinocchio computeJointTorqueRegressor 使用教程"
- GitHub robot-identification 中文注释项目
跨章综合练习¶
综合练习 1（辨识 + MPC + 自适应） ⭐⭐⭐
设计一个完整的"辨识-控制-自适应"流程： 1. 用 Fourier 激励 + WLS 离线辨识 7-DOF 机械臂的基参数（本章 §3-5） 2. 用辨识参数设计 computed torque 控制器 + NMPC 跟踪控制器（3.11-3.12） 3. 在运行中用 RLS 在线更新参数（本章 §7），当参数变化超过阈值时自动更新控制器
讨论：在线辨识和 Tube MPC（3.13）如何互补？什么时候该用在线辨识更新模型，什么时候该用 Tube 容忍模型误差？
综合练习 2（辨识 + sim-to-real） ⭐⭐⭐
对一个在 MuJoCo 中仿真的四足机器人： 1. 在仿真中辨识动力学参数 2. 故意引入参数偏差（模拟 sim-to-real gap） 3. 比较三种策略的跟踪性能：(a) 直接使用仿真参数；(b) 在真实数据上重新辨识；(c) 使用 domain randomization 训练的 RL 策略 4. 讨论：辨识在 sim-to-real 中的角色是什么？
本质洞察专栏¶
本质洞察：系统辨识的本质不是"拟合数据"，而是**从有限观测中提取物理系统的内禀结构**。一个好的辨识结果应该能预测**从未见过的运动模式**下的系统行为——这要求辨识出的参数具有物理意义（质量、惯性、摩擦），而不仅仅是最小化训练集上的拟合残差。这就是为什么物理一致性约束（§6）不是可选的"加分项"，而是辨识质量的根本保证。
本质洞察：激励轨迹设计（§4）的核心矛盾是**信息量 vs 安全性**。信息论告诉我们，参数辨识精度正比于 Fisher 信息矩阵 \(\mathcal{I} = Y^\top \Sigma^{-1} Y\)（回归矩阵的加权外积）——要使 \(\mathcal{I}\) 的最小特征值尽量大，轨迹必须"充分激励"所有方向。但激励意味着大加速度、大速度变化，这在真实机器人上受到关节限位、力矩饱和、安全约束的严格限制。Fourier 参数化 + 条件数优化是这对矛盾的工程最优折衷。
本质洞察：基参数（Base Parameters）的概念揭示了一个深刻的数学事实：并非所有物理参数都能从输入-输出数据中辨识出来。\(10n\) 个标准惯性参数通过动力学方程映射到力矩空间时，存在退化方向——某些参数组合对可观测的关节力矩没有任何影响。基参数正是消除这些退化后的最小独立参数集。这与可观测性理论（Kalman rank condition）有着深刻的对偶关系：状态估计中有"不可观测状态"，参数辨识中有"不可辨识参数"。
补充练习题¶
练习 S.1 ⭐（辨识精度评估）：对已辨识的参数 \(\hat{\pi}\)，定义以下验证指标：(a) 相对力矩预测误差 \(e_{\text{rel}} = \|\tau_{\text{pred}} - \tau_{\text{meas}}\| / \|\tau_{\text{meas}}\|\)；(b) 参数不确定性椭球 \(\text{cov}(\hat\pi) = \sigma^2 (Y^\top Y)^{-1}\)。在新的（未用于辨识的）验证轨迹上计算这两个指标。讨论：\(e_{\text{rel}} < 5\%\) 对 MPC 来说够用吗？
练习 S.2 ⭐⭐（条件数与激励质量）：对 2-DOF 平面机械臂，生成三种激励轨迹：(a) 单频正弦；(b) 多频 Fourier；(c) 随机关节抖动。分别计算回归矩阵的条件数 \(\kappa(Y)\)。用 Monte Carlo 加入不同噪声水平 \(\sigma \in \{0.01, 0.1, 1.0\}\) Nm，比较三种轨迹的参数估计方差。验证 \(\text{var}(\hat\pi) \propto \kappa(Y)^2 \sigma^2\)。
练习 S.3 ⭐⭐（物理一致性约束）：对辨识得到的参数 \(\hat\pi\)，检验以下物理约束是否满足：(a) 所有连杆质量 \(m_i > 0\)；(b) 惯性张量 \(I_i\) 正定（即三角不等式 \(I_{xx} + I_{yy} \ge I_{zz}\) 等）；(c) 摩擦系数 \(f_{c,i} \ge 0\)。如果不满足，用 SDP（半正定规划）重新辨识，比较有无物理一致性约束时的预测精度差异。
练习 S.4 ⭐⭐⭐（在线辨识 vs 离线辨识）：实现 RLS（递推最小二乘）在线辨识器。在仿真中，让机器人执行正常任务（如码垛），中途改变负载质量（模拟抓起物体）。(a) 画出参数估计随时间的变化曲线；(b) 测量参数收敛到新值所需的时间；(c) 讨论：遗忘因子 \(\lambda\) 越小收敛越快但方差越大——对于 MPC 来说，最佳的 \(\lambda\) 是多少？
练习 S.5 ⭐⭐⭐（跨章综合 — 辨识误差对控制性能的影响）：回顾 §3.13（Tube MPC）。设辨识参数有 20% 的相对误差 \(\|\Delta\pi\|/\|\pi\| = 0.2\)，将其转化为动力学模型的加法扰动界 \(\|w\| \le \delta\)。(a) 估计 \(\delta\)（提示：\(w \approx Y\Delta\pi\)）；(b) 用 \(\delta\) 设计 Tube MPC 的扰动集 \(\mathcal{W}\)；(c) 比较"辨识精度提升到 5% + 窄 Tube"与"保持 20% 误差 + 宽 Tube"两种策略的控制性能（跟踪精度 vs 计算量）。
术语对照表¶
| 英文术语 | 中文 | 首次出现节 |
|---|---|---|
| Base Parameters | 基参数 | §3 |
| Regressor Matrix | 回归矩阵 | §3 |
| Fourier Excitation Trajectory | Fourier 激励轨迹 | §4 |
| Condition Number | 条件数 | §4 |
| Weighted Least Squares (WLS) | 加权最小二乘 | §5 |
| Recursive Least Squares (RLS) | 递推最小二乘 | §7 |
| Persistent Excitation (PE) | 持续激励 | §4 |
| Physical Consistency | 物理一致性 | §6 |
| Semidefinite Constraint | 半正定约束 | §6 |
| Cross-Validation | 交叉验证 | §5 |