本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。
D03 双臂协调力控——Grasp Matrix / 内力分解 / 协调阻抗 / Object Impedance¶
本章定位：D01 建立了双臂的运动学框架，D02 解决了"怎么从 A 运动到 B"的规划问题。但运动规划只保证几何路径可行——当两臂实际抓持物体并执行路径时，力的控制才是决定成败的关键。本章从 Grasp Matrix 的数学推导出发，系统建立内力/运动力分解理论、Object Impedance 控制、协调阻抗策略和双臂 QP-WBC 框架，是双臂从"会动"到"会用力"的质变。
适用范围：Grasp Matrix 对多指抓取、灵巧手操作同样适用。Object Impedance 对人形双臂、多机器人协作搬运同样适用。
前置依赖：F01-F03（阻抗控制理论）、F02（操作空间动力学）、D01（协调运动学）
下游章节：D04（双臂学习——学习型力控策略）、D09（MoveIt2 系统集成——力控参数配置）、D10（综合实战）
建议用时：2 周（理论 4 天 + 推导练习 3 天 + 仿真实验 3 天）
前置自测 ⭐¶
📋 答不出 >= 2 题 → 先回前置章节复习
| 编号 | 问题 | 答不出时回顾 |
|---|---|---|
| 1 | 阻抗控制律：写出 Hogan 阻抗律 \(F = M_d \ddot{\tilde{x}} + D_d \dot{\tilde{x}} + K_d \tilde{x}\)。\(M_d, D_d, K_d\) 分别控制什么物理行为？临界阻尼条件 \(D_d = 2\sqrt{K_d M_d}\) 的物理含义是什么？ | F01 阻抗导纳二分法 |
| 2 | 操作空间动力学：写出操作空间控制律 \(\tau = J^T \Lambda \ddot{x}_d + J^T \mu + g\)。\(\Lambda = (JM^{-1}J^T)^{-1}\) 的物理含义是什么？ | F02 操作空间动力学 |
| 3 | 摩擦锥：对于库仑摩擦模型 \(\|f_t\| \leq \mu f_n\)，为什么接触力必须在摩擦锥内？线性化摩擦锥（金字塔近似）的边数如何影响精度？ | F03 经典力控算法 |
| 4 | 增广/相对 Jacobian：写出 \(J_{\text{aug}}\) 和 \(J_{\text{rel}}\) 的定义。\(J_{\text{rel}} \dot{q} = 0\) 的物理含义是什么？ | D01 协调运动学 |
| 5 | QP 优化：写出二次规划（QP）的标准形式 \(\min \frac{1}{2}x^T H x + f^T x\) s.t. \(Ax \leq b, A_{eq} x = b_{eq}\)。OSQP 求解器的适用场景是什么？ | M05 QP/NLP 建模工程 |
本章目标¶
学完本章后，你应该能够：
- 推导 Grasp Matrix \(G \in \mathbb{R}^{6 \times 12}\) 的完整定义，理解 \(\text{rank}(G) = 6\) 意味着 6 维零空间 = 内力空间
- 执行 内力/运动力分解 \(f_c = G^+ w_o + (I - G^+ G) f_{\text{int}}\)，理解"内力不产生物体运动但维持力封闭"
- 设计 Object Impedance 控制器的参数（\(M_o, D_o, K_o\)），针对搬运、装配、人机协作三种场景
- 选择 对称/主从/相对阻抗策略，理解各自的适用条件和参数设计
- 组装 双臂 QP-WBC 优化问题，将动力学约束、Grasp 约束、摩擦锥约束、内力目标统一到一个 QP 中
D3.1 从单臂力控到双臂力控——新的挑战 ⭐¶
动机：双臂力控的独特性¶
回顾 F01-F03：单臂力控的核心问题是"如何让末端在接触环境时表现出期望的力/运动关系"。单臂与环境的交互是**开链**的——末端对环境施力，环境对末端施反力，控制器调整力矩使末端表现出期望的阻抗/导纳行为。
双臂力控的独特挑战在于：两臂与物体构成**闭链**，闭链产生了**内力空间**——一组不产生物体运动的力。这些内力在单臂系统中不存在，是双臂力控需要额外管理的新维度。
单臂力控：
控制目标：末端对环境的力/运动关系
自由度：6D 力 + 6D 运动 = 一个阻抗关系
内力：不存在（开链无内力空间）
双臂力控：
控制目标：物体运动 + 两臂内力
自由度：6D 物体运动 + 6D 内力 = 两个独立控制目标
内力：存在且必须管理
内力太小 → 物体滑脱
内力太大 → 物体变形/损坏
内力不可控 → force fight（两臂打架）
为什么单臂力控方法不够¶
反事实推理：如果对双臂的每只手独立应用单臂阻抗控制（各自跟踪自己的目标位姿），会怎样？
场景：双 Franka 共持一根刚性杆，左臂目标位姿 \(T_L^d\)，右臂目标位姿 \(T_R^d\)。
问题：两个目标位姿之间的相对关系 \(T_L^{d,-1} T_R^d\) 可能与实际杆长不完全匹配（哪怕只差 0.1mm）。
后果：两臂各自尝试达到自己的目标 → 杆被挤压或拉伸 → 产生不可控的内力。如果阻抗刚度 \(K = 1000\) N/m，0.1mm 的位置误差产生 0.1 N 的力；但如果 \(K = 10000\) N/m（高刚度位控），0.1mm 误差产生 1 N。多个关节累积后，内力可达数十牛——足以损坏精密工件。
结论：独立位控/阻抗控制在闭链系统中必然产生 force fight。这就是为什么需要 Object Impedance + 内力管理的统一框架。
跨领域类比：双臂 force fight vs 两个发电机并联。当两个发电机并联供电时，如果它们的频率/相位不完全同步，会产生"环流"（circulating current）——电流在两台发电机之间流动但不对外做功。这与双臂的内力完全类比：内力在两臂之间"流动"但不产生物体运动。电力系统通过同步器（synchroscope）精确匹配相位来消除环流；双臂系统通过 Grasp Matrix + 内力控制来管理内力。
⚠️ 常见陷阱¶
💡 概念误区：认为"双臂只要位置精度够高就不会有 force fight"
新手想法："如果两臂都能精确到 0.01mm，内力就是零"
实际上：真实系统中位控精度受限于编码器分辨率（~0.01°）、
控制器延迟（~1ms）、关节摩擦和柔性。即使精度达到 0.01mm，
乘以刚度（K=10000 N/m）仍产生 0.1N 内力。更重要的是，
两臂的精度不是独立的——一臂的误差通过物体传递到另一臂，
可能放大。只有通过力控（而非更高精度的位控）才能根本解决。
正确理解：force fight 的根源不是"精度不够"，而是"闭链系统的
几何约束与位控目标不完全一致"。解决方案是力控，不是更精确的位控。
🧠 思维陷阱：认为"力控比位控更难实现，所以尽量用位控"
新手想法："位控简单可靠，力控复杂且需要力传感器"
实际上：在闭链系统中，位控反而更危险——因为位置误差直接转化为不可控的力。
力控虽然需要力传感器，但它从根本上避免了 force fight。
现代双臂平台（Franka, iiwa）都有关节力矩传感器，力控不是障碍。
正确思维：闭链系统中，力控不是"可选的高级功能"，而是"必要的基础能力"。
练习¶
-
[估算] 双 Franka Panda 独立位控共持一根 30cm 钢杆（刚度 \(K_{\text{steel}} \approx 10^8\) N/m）。如果两臂的末端位置误差各为 \(\pm 0.05\) mm（随机独立），最坏情况下杆承受多大内力？这个力是否会损坏 Franka 的力矩传感器（额定 87 N·m）？
-
[思考] 如果不是刚性杆而是一条绳子（\(K \approx 0\)），独立位控还会产生 force fight 吗？为什么？
D3.2 Grasp Matrix——从接触力到物体 wrench ⭐⭐¶
动机：Grasp Matrix 解决什么问题¶
两臂各通过一个接触点与物体交互，每个接触点可以施加 6 维 wrench（3 维力 + 3 维力矩，假设 rigid grasp）。总共 12 维接触力 \(f_c = [f_L^T, f_R^T]^T \in \mathbb{R}^{12}\)。
本文的 6D 排列约定：
即 wrench 采用 \([F;\tau]\)，twist/速度采用 \([v;\omega]\)，功率配对为 \(f^T V = F^T v + \tau^T\omega\)。这与本章 Pinocchio 代码中 Force/Motion::toVector()、LOCAL_WORLD_ALIGNED Jacobian 的索引写法保持一致：index 0-2 是线速度/力，index 3-5 是角速度/力矩。如果你的库或论文使用 Featherstone 常见的 \([\tau;F]\)、\([\omega;v]\) 或其他空间向量顺序，必须先用置换矩阵把 \(G\)、\(J\)、\(w\)、\(f_c\) 转成同一约定；不要把本文的 \([F;\tau]\) 直接和另一种空间向量约定混用。
问题：这 12 维接触力对物体产生什么效果？
答案：Grasp Matrix \(G\) 将 12 维接触力映射到 6 维物体净 wrench：
由于 \(G\) 是 \(6 \times 12\) 矩阵（行少于列），存在 6 维零空间——零空间中的力不产生物体运动，但在物体内部产生应力。这就是**内力**。
数学推导¶
Step 1：单接触的力映射
考虑第 \(i\) 个接触点，其在物体坐标系中的位姿为 \(g_{oc_i} = (R_i, p_i) \in SE(3)\)。若接触力 \(f_{c,i} \in \mathbb{R}^6\) 在**接触坐标系**中表达，对物体质心产生的 wrench 为：
其中 \(\text{Ad}_{g}^{-T}\) 是伴随表示的逆转置。
为什么用 \(\text{Ad}^{-T}\) 而不是 \(\text{Ad}\)？
力（wrench）是协变量（covector），速度（twist）是逆变量（vector）。它们的坐标变换规则不同：twist 用 \(\text{Ad}\)，wrench 用 \(\text{Ad}^{-T}\)。这是 Lie 群上对偶性的体现——和线性代数中"行向量变换用 \(A^{-T}\)，列向量变换用 \(A\)"的原因相同。
工程约定：world-aligned wrench
上面的 \(\text{Ad}^{-T}\) 公式适合严谨推导。但本章的 Pinocchio 控制代码使用 LOCAL_WORLD_ALIGNED Jacobian，它的 twist 原点在末端/接触点，坐标轴与世界系平行。因此进入 \(-J^T f\) 的 \(f\) 也必须是在同一接触点、同一 world-aligned 坐标轴下表达的 hand-on-object wrench。后续代码统一采用这个工程约定：
如果你选择使用接触局部坐标的 \(f_i^C\)，则在关节力矩映射前必须先把它变换成 Jacobian 对偶坐标：\(f_i^W = \text{Ad}_{g_{Wi}}^{-T} f_i^C\)，再代入 \(-J_i^{W\,T}f_i^W\)。
Step 2：总 Grasp Matrix
对于 \(k = 2\) 个接触（双臂），Grasp Matrix 为：
若沿用接触局部坐标表达 \(f_i^C\)，则：
若采用本章代码的 world-aligned 工程约定，则用上一小节的 \(G_i^W\) 替代这里的 \(\text{Ad}^{-T}\) 子块。两种写法都可以，但 \(G\) 输出的 \(f_L,f_R\) 必须和后续 \(J^T f\) 使用的 Jacobian frame 一致。
物体净 wrench：
符号约定：hand-on-object vs robot-side wrench
本章 Grasp Matrix 中的 \(f_L, f_R\) 统一表示**手作用于物体**的 wrench，因此 \(G[f_L;f_R]\) 是物体受到的净 wrench。机器人动力学侧看到的是相反方向的接触 wrench：物体作用于手为 \(-f_L,-f_R\)。因此如果用这些 \(f_L,f_R\) 直接生成机器人关节力矩，且它们已经与 Jacobian 在同一 frame 中表达，任务力矩项应写成
再叠加关节空间重力/科氏补偿等项。很多教材把 \(f\) 定义成"环境作用于机器人"的 wrench，此时公式会写成 \(+J^Tf\)；两种写法并不矛盾，关键是不要把 hand-on-object 的 \(f\) 和 robot-side 的 \(f_{\text{robot}}=-f\) 混用。
Step 3：秩分析
\(G \in \mathbb{R}^{6 \times 12}\)，两个 \(6 \times 6\) 子块各自可逆（\(\text{Ad}^{-T}\) 总是可逆的），因此 \(\text{rank}(G) = 6\)。
由秩-零化度定理：\(\dim(\ker(G)) = 12 - 6 = 6\)。
这 6 维零空间正是**内力空间**：\(f_{\text{int}} \in \ker(G)\) 满足 \(G \cdot f_{\text{int}} = 0\)——不产生物体净 wrench。
内力空间的物理直觉¶
Williams 和 Khatib 在 1993 年提出了**虚拟连杆（Virtual Linkage）**模型来直觉理解 6 维内力空间：
想象两个接触点之间连一根"虚拟刚杆"。
沿杆轴方向：
挤压 (+) / 拉伸 (-) → 1 维内力
绕杆轴旋转：
相对扭转 → 1 维内力矩
垂直杆轴的两个方向：
各 1 维剪切模态 → 2 维
注意：接触点有间距时，反向剪力会形成力偶；
真正的内力模态是"剪力 + 补偿接触力矩"的组合
绕垂直杆的两轴：
各 1 维弯矩 → 2 维
总计：1 + 1 + 2 + 2 = 6 维内力空间
f_squeeze → ← f_squeeze (挤压/拉伸)
↻ τ_twist ──────── τ_twist ↺ (扭转)
↑ f_shear ──────── f_shear ↓ (剪力)
↻ τ_bend ──────── τ_bend ↺ (弯矩)
本质洞察：内力空间的 6 维恰好对应两个接触点之间所有"自平衡"力模态——这些力在物体内部传递但不对外做功。在单臂系统中不存在内力（开链无闭合路径供力"循环"），在双臂闭链中内力自然出现。内力的存在是双臂力控比单臂复杂的根本原因。
Pinocchio 中的 Grasp Matrix 计算¶
// ============================================================
// Grasp Matrix 计算——Pinocchio C++ 实现
// ============================================================
// 1. 计算两个接触点与物体在世界系中的位置
pinocchio::forwardKinematics(model, data, q);
pinocchio::updateFramePlacements(model, data);
pinocchio::SE3 T_obj = /* 物体位姿（已知或从两臂推算） */;
pinocchio::SE3 T_L = data.oMf[left_ee_id]; // 左臂末端世界系位姿
pinocchio::SE3 T_R = data.oMf[right_ee_id]; // 右臂末端世界系位姿
auto skew = [](const Eigen::Vector3d& a) {
Eigen::Matrix3d S;
S << 0.0, -a.z(), a.y(),
a.z(), 0.0, -a.x(),
-a.y(), a.x(), 0.0;
return S;
};
// 2. 计算 world-aligned Grasp Matrix
// f_i = [F_i; tau_i] 与 LOCAL_WORLD_ALIGNED Jacobian 对偶：
// 原点在接触点，坐标轴与世界系平行。
Eigen::Vector3d r_L = T_L.translation() - T_obj.translation();
Eigen::Vector3d r_R = T_R.translation() - T_obj.translation();
Eigen::Matrix<double,6,6> G_L = Eigen::Matrix<double,6,6>::Identity();
Eigen::Matrix<double,6,6> G_R = Eigen::Matrix<double,6,6>::Identity();
G_L.bottomLeftCorner<3,3>() = skew(r_L);
G_R.bottomLeftCorner<3,3>() = skew(r_R);
// 3. 组装 Grasp Matrix
Eigen::Matrix<double,6,12> G;
G << G_L, G_R;
// 4. 验证 rank(G) = 6
Eigen::JacobiSVD<Eigen::MatrixXd> svd(G, Eigen::ComputeFullV);
int rank = (svd.singularValues().array() > 1e-6).count();
assert(rank == 6);
// 5. 计算内力空间基（G 的零空间）
Eigen::MatrixXd V = svd.matrixV();
Eigen::MatrixXd null_space = V.rightCols(6); // 12×6
// null_space 的每一列是一个内力模态
// ============================================================
// ❌ 常见错误：局部接触 wrench 直接乘 LOCAL_WORLD_ALIGNED Jacobian
// ============================================================
// 错误：G 用接触局部坐标求出 f_L_local，却直接 tau=-J_LWA^T f_L_local。
// 正确：要么像上面一样让 G 直接输出 world-aligned f_L_world，
// 要么先做 f_L_world = Ad_{W<-C}^{-T} f_L_local，再乘 J_LWA^T。
Frame 单元测试（必须有）：
// 随机给一个 world-aligned hand-on-object wrench，验证功率一致性。
Eigen::VectorXd dq = Eigen::VectorXd::Random(model.nv);
Eigen::Matrix<double,6,1> fW = Eigen::Matrix<double,6,1>::Random();
Eigen::MatrixXd J_LWA(6, model.nv);
pinocchio::getFrameJacobian(model, data, left_ee_id,
pinocchio::LOCAL_WORLD_ALIGNED, J_LWA);
Eigen::Matrix<double,6,1> V_hand = J_LWA * dq;
Eigen::VectorXd tau_hand = -J_LWA.transpose() * fW;
double p_object_on_hand = (-fW).dot(V_hand);
double p_joint = tau_hand.dot(dq);
assert(std::abs(p_object_on_hand - p_joint) < 1e-9);
这个测试不关心控制效果，只检查 frame 与符号是否一致。若把局部 wrench 直接喂给 LOCAL_WORLD_ALIGNED Jacobian，旋转末端后该测试会失败。
Worked Example 1：对称抓取——Grasp Matrix 的显式计算¶
场景：两接触点在物体质心两侧对称放置，无旋转偏差。
物体坐标系原点 = 质心
左接触点：p_L = [-0.15, 0, 0]^T (m)，R_L = I
右接触点：p_R = [+0.15, 0, 0]^T (m)，R_R = I
Step 1：计算 Ad^{-T}
对于 SE(3) 元素 g = (R, p)：
Ad_g = [R, [p]_× R]
[0, R ]
Ad_g^{-1} = [R^T, -R^T [p]_×]
[0, R^T ]
Ad_g^{-T} = (Ad_g^{-1})^T
左接触点 g_oL = (I, [-0.15, 0, 0]^T)：
[p_L]_× = [ 0 0 0 ]
[ 0 0 0.15]
[ 0 -0.15 0 ]
Ad_{g_oL}^{-T} = [I, 0 ]
[[p_L]_×, I ]
= [1 0 0 | 0 0 0 ]
[0 1 0 | 0 0 0 ]
[0 0 1 | 0 0 0 ]
[0 0 0 | 1 0 0 ]
[0 0 0.15| 0 1 0 ]
[0 -0.15 0| 0 0 1 ]
右接触点 g_oR = (I, [0.15, 0, 0]^T)：
Ad_{g_oR}^{-T} = [1 0 0 | 0 0 0 ]
[0 1 0 | 0 0 0 ]
[0 0 1 | 0 0 0 ]
[0 0 0 | 1 0 0 ]
[0 0 -0.15| 0 1 0 ]
[0 0.15 0 | 0 0 1 ]
Step 2：组装 G = [G_L | G_R] ∈ R^{6×12}
G = [I₃ 0₃ | I₃ 0₃ ]
[P_L I₃ | P_R I₃ ]
其中 P_L = [p_L]_×, P_R = [p_R]_×
Step 3：零空间计算
G 的 SVD → 6 个非零奇异值，6 个零奇异值
零空间基（6 列）对应 6 种内力模态：
模态 1（挤压/拉伸）：f_int = [1,0,0,0,0,0, -1,0,0,0,0,0]^T
→ 左手推+右手拉，f_L + f_R = 0，不产生净力
模态 2（y 剪切+补偿力矩）：
f_int = [0,1,0,0,0,0.15, 0,-1,0,0,0,0.15]^T
→ 两手 y 方向反向力本身净力为零，但因接触点相距 0.3m，
会产生绕 z 的力偶；必须同时加入 τ_z 补偿力矩才属于 ker(G)
模态 3（z 剪切+补偿力矩）：
f_int = [0,0,1,0,-0.15,0, 0,0,-1,0,-0.15,0]^T
→ 两手 z 方向反向力会产生绕 y 的力偶；必须同时加入 τ_y 补偿力矩
模态 4（绕 x 扭转）：f_int = [0,0,0,1,0,0, 0,0,0,-1,0,0]^T
模态 5（绕 y 弯矩+z 力偶）：组合模态
模态 6（绕 z 弯矩+y 力偶）：组合模态
验证：G · f_int = 0 对所有 6 个模态成立 ✓
注意：如果只写 y/z 方向的反向剪切力，例如
[0,1,0,0,0,0, 0,-1,0,0,0,0]^T，确实不产生净力，
但会产生净力矩，因此不是 Grasp Matrix 的零空间向量。
Worked Example 2：倾斜抓取——非对称 Grasp Matrix¶
场景：左手从物体底部抓取，右手从侧面抓取（90 度夹角）。
左接触点：p_L = [0, 0, -0.1]^T，R_L = I（从下方）
右接触点：p_R = [0.1, 0, 0]^T，R_R = Rz(π/2)（从侧面，绕 z 转 90°）
R_R = [ 0 -1 0]
[ 1 0 0]
[ 0 0 1]
Step 1：G_R 的 Ad^{-T} 比对称情况复杂——R_R ≠ I
Ad_{g_oR}^{-1} = [R_R^T, -R_R^T [p_R]_×]
[0, R_R^T ]
Ad_{g_oR}^{-T} = 转置后得到 6×6 矩阵（含旋转耦合项）
Step 2：rank(G) 仍然 = 6（Ad^{-T} 总可逆）
Step 3：零空间变化
非对称抓取的内力模态不再沿坐标轴对齐
→ 需要 SVD 数值计算才能得到零空间基
→ 物理解释需要结合抓取几何
关键区别：
对称抓取 → 挤压/剪力/弯矩模态解耦
非对称抓取 → 模态耦合（挤压中混有弯矩分量）
→ 内力参考 f_int_ref 的设计更复杂
内力/运动力分解的几何可视化¶
理解分解 \(f_c = G^+ w_o + (I - G^+ G) f_{\text{int}}\) 最好的方式是几何直觉：
12 维接触力空间 R^12
┌─── range(G^T) = row(G) ───┐
│ (6 维运动力子空间) │
│ │
│ f_motion = G⁺ w_o │
│ ↑ │
│ │ │
────────────────────── ●───────────────────────────── ──── R^12
│ │
│ │ f_c = f_motion + f_null
│ │
│ ↓ │
│ f_null = (I-G⁺G)f_ref │
│ (6 维内力子空间) │
└─── ker(G) ─────────────────┘
两个子空间正交：row(G) ⊥ ker(G)
→ f_motion^T f_null = 0
→ 运动力和内力独立控制
可视化类比：力空间中的"正交分解"如同
3D 空间中将向量分解为水平分量和垂直分量。
G⁺ 是"沿 row(G) 方向的投影"
(I - G⁺G) 是"沿 ker(G) 方向的投影"
双重解读： - 代数视角：\(G^+\) 给出满足 \(Gf = w_o\) 的最小范数解——力分配尽量"均匀"（两臂各承担一半）。\((I - G^+ G)\) 投影到零空间——内力不影响物体运动。 - 物理视角：运动力是"两臂合力"（推动物体），内力是"两臂对力"（挤压物体）。合力决定运动，对力决定抓取稳定性。就像拔河——两队各出的力是"合力"（绳子运动方向），两队互拉的张力是"内力"（绳子绷紧程度）。
⚠️ 常见陷阱¶
💡 概念误区：认为"Grasp Matrix 只对刚性抓取适用"
新手想法："G 的推导假设 rigid grasp（6D 接触力），
如果是点接触（3D 力）呢？"
实际上：对于点接触（point contact without friction），每个接触
只有 1 维力（法向），f_c ∈ R^2（两接触），G ∈ R^{6×2}。
对于有摩擦的点接触，f_c ∈ R^6（两×3D力），G ∈ R^{6×6}。
Grasp Matrix 的定义是通用的——接触维度影响 G 的列数和零空间维度。
正确理解：G 的结构取决于接触模型。rigid grasp 给出最大的 12 维输入
和 6 维零空间。点接触的零空间更小（甚至为零），
意味着控制内力的自由度更少。
⚠️ 编程陷阱：物体坐标系的选择影响 G 的数值
问题：G 依赖于物体坐标系的原点——不同的原点给出不同的 G
现象：两个工程师用不同的物体坐标系原点，得到不同的 G，但都是"正确的"
正确做法：统一约定物体坐标系原点为物体质心。
这使得 G 的物理含义最直觉：w_o 就是质心处的净力旋。
练习¶
-
[手推] 对于两个接触点在物体质心两侧对称放置（\(p_L = [-d, 0, 0]^T\)，\(p_R = [d, 0, 0]^T\)，\(R_L = R_R = I\)），写出 \(G\) 的显式 \(6 \times 12\) 矩阵。验证 \(\text{rank}(G) = 6\)。计算 \(\ker(G)\) 的一组基。
-
[编程] 在 Pinocchio 中为双 Franka 共持 30cm 杆计算 \(G\)。用 SVD 提取零空间的 6 个基向量，物理解释每个基向量对应什么内力类型（挤压？扭转？剪力？弯矩？）。
-
[跨章综合] 回顾 D01 中的 \(J_{\text{rel}}\)（相对 Jacobian）。\(J_{\text{rel}}\) 的零空间和 \(G\) 的零空间有什么关系？提示：\(J_{\text{rel}} \dot{q} = 0\) 约束运动，\(G f_{\text{int}} = 0\) 约束力——它们是对偶的。
D3.3 内力 vs 运动力分解 ⭐⭐¶
动机：12 维接触力的两个角色¶
12 维接触力 \(f_c\) 做两件事：（1）驱动物体运动（运动力）；（2）维持抓取稳定（内力）。Grasp Matrix 让我们精确分解这两个角色。
分解公式推导¶
给定期望的物体净 wrench \(w_{o,\text{des}}\) 和内力参考 \(f_{\text{int,ref}}\)：
\(f_{\text{int,ref}}\) 在工程上更准确地理解为“希望的内力方向/幅值参考”，不要求它本身已经严格落在 \(\ker(G)\)。真正施加的内力是
也就是先按当前接触几何投影到 Grasp Matrix 零空间。这样即使参考向量里混入了会推动物体的分量，投影后也不会改变物体净 wrench。
为什么这个分解是正确的？
验证 1：运动力产生期望的物体 wrench。
（当 \(G\) 行满秩时，\(G G^+ = I_6\)。）
验证 2：内力不产生物体 wrench。
验证 3：两者正交。
\(f_{\text{motion}} = G^+ w_o\) 在 \(G\) 的行空间中（因为 \(G^+ w = G^T (GG^T)^{-1} w \in \text{range}(G^T) = \text{row}(G)\)），而 \(f_{\text{null}} = (I - G^+ G) f_{\text{int}}\) 在零空间中。行空间和零空间正交，所以 \(f_{\text{motion}}^T f_{\text{null}} = 0\)。
跨领域类比：内力/运动力分解 vs 共模/差模信号。在电子工程中，差分放大器的输入可以分解为共模信号（对外做功）和差模信号（内部循环）。运动力是"共模"（驱动物体运动），内力是"差模"（在闭链内循环不对外做功）。分解使得两个维度可以独立控制——就像 CMRR 让我们单独抑制共模噪声一样。
内力大小的工程设定¶
内力不是越大越好也不是越小越好——需要在"防滑脱"和"防损坏"之间取平衡：
力封闭条件：所有接触力必须在摩擦锥内
‖f_{t,i}‖ ≤ μ · f_{n,i}
最小内力（防滑脱）：
f_{int,min} ≥ mg / (2μ)
推导：重力 mg 由两臂摩擦力支撑
每臂最大摩擦力 = μ · f_n
两臂合计 2μf_n ≥ mg → f_n ≥ mg/(2μ)
例：m=2kg, μ=0.5 → f_{int,min} = 2×9.8/(2×0.5) = 19.6 N
最大内力（防损坏）：
f_{int,max} = F_crush / SF (SF = 安全系数, 通常 2-3)
例：鸡蛋 F_crush≈50N, SF=3 → f_{int,max} < 17N
例：铝罐 F_crush≈200N, SF=3 → f_{int,max} < 67N
安全区间：[mg/(2μ), F_crush/SF]
可能很窄！需要精确内力控制
参数表：
| 物体 | 质量 (kg) | \(\mu\) | \(f_{\text{int,min}}\) (N) | \(F_{\text{crush}}\) (N) | \(f_{\text{int,max}}\) (N) | 区间宽度 |
|---|---|---|---|---|---|---|
| 鸡蛋 | 0.06 | 0.8 | 0.37 | 50-80 | 17-27 | 宽 |
| 玻璃杯 | 0.3 | 0.5 | 2.9 | 200 | 67 | 宽 |
| 薄玻璃板 | 2.0 | 0.6 | 16.3 | 100 | 33 | 中 |
| 铝制锅 | 1.5 | 0.6 | 12.3 | 5000 | 1667 | 极宽 |
⚠️ 常见陷阱¶
🧠 思维陷阱：认为"内力控制只是设一个常数"
新手想法："f_int = 20 N 写死在代码里就行"
实际上：最优内力随操作状态变化：
- 加速阶段需更大内力（惯性力改变摩擦需求）
- 静止阶段可降低内力（减少能耗和物体应力）
- 倾斜物体需更大法向力（重力分量改变）
因此内力应时变：f_int(t) = f_base + Δf(q, q̇, q̈)
正确做法：基础内力 + 动态补偿 + 安全裕量
⚠️ 编程陷阱：伪逆选择影响力分配
问题：不同伪逆方法（Moore-Penrose / 加权 / 阻尼）给出不同的 f_motion
现象：相同 w_o 下两臂力分配不同
正确做法：标准场景用 Moore-Penrose 伪逆（最小范数解）。
需要不对称分配时用加权伪逆 G_W^+ = W^{-1}G^T(GW^{-1}G^T)^{-1}，
其中 W 的对角块可以偏向某一臂承担更多力。
练习¶
-
[计算] 搬运一个 5 kg 薄玻璃板（\(F_{\text{crush}} = 100\) N），\(\mu = 0.6\)。计算安全内力区间。如果标定误差使两臂位置偏差 1mm，在 \(K = 1000\) N/m 阻抗下产生多大额外内力？是否仍安全？
-
[手推] 证明 \(f_{\text{motion}} = G^+ w_o\) 是满足 \(Gf = w_o\) 的最小范数解。
D3.4 Object Impedance Control——把物体当作虚拟弹簧 ⭐⭐¶
动机：从末端阻抗到物体阻抗¶
回顾 F01：单臂阻抗控制让末端表现为虚拟质量-弹簧-阻尼系统。双臂的自然推广是：让**物体本身**表现为虚拟质量-弹簧-阻尼系统。
Schneider 和 Cannon 1992 年首次提出这个思想——不分别控制两臂末端的阻抗，而是直接在物体层面定义阻抗行为，再通过 Grasp Matrix 分配到两臂。
Object Impedance 控制律¶
注意：上式描述的是期望的闭环误差动力学，\(D_o\dot{\tilde{x}}_o\) 和 \(K_o\tilde{x}_o\) 是被外力激发后的恢复项。控制器计算施加到物体的命令 wrench 时，若误差按 \(\tilde{x}_o=x_o-x_{o,\text{des}}\) 定义，阻尼和弹簧项必须以负号进入：\(-D_o\dot{\tilde{x}}_o-K_o\tilde{x}_o\)，否则就是把误差越推越大的正反馈。
| 符号 | 含义 | 典型范围 |
|---|---|---|
| \(x_o\) | 物体质心位姿（6D） | SE(3) |
| \(x_{o,\text{des}}\) | 物体期望轨迹 | SE(3) |
| \(M_o\) | 期望物体惯量 | 真实值 \(\times\) 1-5 |
| \(D_o\) | 期望物体阻尼 | \(2\zeta\sqrt{K_o M_o}\)，\(\zeta = 0.7\text{-}1.0\) |
| \(K_o\) | 期望物体刚度 | 50-2000 N/m |
| \(f_{\text{ext}}\) | 外部扰动 | — |
为什么定义在物体层面而非末端层面？
- 物理直觉：工程师关心"物体如何响应碰撞"，不关心"左手如何响应碰撞"
- 自动协调：物体阻抗通过 \(G^+\) 自动处理两臂间力分配
- 抓取无关：换一种抓取方式，Object Impedance 参数不变
参数设计指南¶
| 场景 | \(K_o\) (N/m) | \(D_o\) | \(M_o\) | 设计理由 |
|---|---|---|---|---|
| 精确搬运 | 500-1500 | 临界阻尼 | 真实值 | 高刚度 → 精确跟踪 |
| 安全搬运 | 100-300 | 过阻尼 (\(\zeta=1.5\)) | 真实 \(\times 2\) | 低刚度 → 碰撞安全 |
| 装配（法向柔顺） | \(K_z = 50\), \(K_{xy} = 1000\) | 各向异性 | 真实值 | z 方向柔顺配合 |
| 人机协作 | \(K_o \to 0\) | \(D_o = 50\text{-}100\) Ns/m | 真实值 | 纯阻尼 → 人拉就动 |
反事实推理：如果装配任务中所有方向都用高刚度（\(K = 1000\) N/m），而不是法向柔顺（\(K_z = 50\)），会怎样？ 当工件与孔的对准有微小偏差时，高刚度产生极大接触力——可能损坏工件或卡死。法向柔顺允许工件在 z 方向"让步"，配合力引导策略自动对准。
从 Object Impedance 到两臂力矩——三步管线¶
步骤 1: 计算物体期望 wrench
x̃_o = x_o - x_{o,des}, Ṽ_o = V_o - V_{o,des}
w_{o,cmd} = M_o · ẍ_{o,ref} - D_o · Ṽ_o - K_o · x̃_o + g_o
步骤 2: 通过 Grasp Matrix 分配到两手
f_c = G⁺ · w_{o,cmd} + (I - G⁺G) · f_{int,ref}
f_L = f_c[0:6], f_R = f_c[6:12]
步骤 3: 各臂关节力矩
τ_L = -J_L^T · f_L + g_L(q_L)
τ_R = -J_R^T · f_R + g_R(q_R)
这里 f_L, f_R 是手作用于物体的 wrench；机器人侧接触 wrench 符号相反
完整控制架构图¶
┌──────────────────────────────────────────────────────────┐
│ Object Impedance Controller │
│ w_o = M_o·ẍ_ref - D_o·Ṽ - K_o·x̃ + g_o │
└──────────────────┬───────────────────────────────────────┘
│ w_{o,cmd} (6D)
▼
┌──────────────────────────────────────────────────────────┐
│ Grasp Matrix Force Allocation │
│ f_c = G⁺·w_{o,cmd} + (I-G⁺G)·f_{int,ref} │
│ f_L = f_c[0:6], f_R = f_c[6:12] │
└────────┬──────────────────────────────┬──────────────────┘
│ f_L │ f_R
▼ ▼
┌────────────────────┐ ┌────────────────────┐
│ Left Arm Torque │ │ Right Arm Torque │
│ τ_L = -J_L^T·f_L │ │ τ_R = -J_R^T·f_R │
│ + g_L │ │ + g_R │
└────────────────────┘ └─────────────────────┘
Object Impedance 完整实现——Pinocchio + OSQP¶
以下给出可运行的 Object Impedance 控制器完整代码框架，包含从传感器读取到力矩输出的完整管线：
// ============================================================
// Object Impedance Controller — 完整 C++ 实现
// 依赖：Pinocchio, Eigen, OsqpEigen
// 控制频率：1 kHz
// ============================================================
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <OsqpEigen/OsqpEigen.h>
using Vector6d = Eigen::Matrix<double, 6, 1>;
using Matrix6d = Eigen::Matrix<double, 6, 6>;
class ObjectImpedanceController {
public:
ObjectImpedanceController(
const pinocchio::Model& model,
pinocchio::FrameIndex left_ee,
pinocchio::FrameIndex right_ee,
const pinocchio::SE3& T_rel_fixed)
: model_(model), data_(model)
, left_ee_(left_ee), right_ee_(right_ee)
, T_rel_(T_rel_fixed)
{
// 默认阻抗参数（搬运模式）
K_o_ = Vector6d::Constant(500.0).asDiagonal(); // 500 N/m
M_o_ = Vector6d::Constant(2.0).asDiagonal(); // 2 kg
// 临界阻尼：D = 2√(KM)
D_o_.setZero();
for (int i = 0; i < 6; i++)
D_o_(i, i) = 2.0 * std::sqrt(K_o_(i, i) * M_o_(i, i));
// 默认不固定在世界 x 轴；每个控制周期根据当前接触点连线生成挤压力参考。
f_int_ref_ = Eigen::VectorXd::Zero(12);
}
// 设置阻抗参数
void setImpedance(const Matrix6d& K,
const Matrix6d& D,
const Matrix6d& M) {
K_o_ = K; D_o_ = D; M_o_ = M;
}
// 设置内力参考
void setInternalForceRef(const Eigen::VectorXd& f_ref) {
// 若传入非零向量，调用方必须保证它已经按当前 frame 表达；
// 后续仍会通过 N_G 投影到 grasp 零空间。
f_int_ref_ = f_ref;
}
void setSqueezeForce(double force_n) {
f_int_scalar_ = force_n;
f_int_ref_.setZero(); // 回到“按接触点实时生成方向”的默认模式
}
// 主控制循环——每 1ms 调用一次
Eigen::VectorXd compute(
const Eigen::VectorXd& q, // 14D 关节位置
const Eigen::VectorXd& dq, // 14D 关节速度
const pinocchio::SE3& T_obj_des, // 物体期望位姿
const Vector6d& V_obj_des, // 物体期望速度
const Vector6d& A_obj_des) // 物体期望加速度
{
// --- Step 1: 更新运动学 ---
pinocchio::forwardKinematics(model_, data_, q, dq);
pinocchio::updateFramePlacements(model_, data_);
pinocchio::computeJointJacobians(model_, data_, q);
const auto& T_L = data_.oMf[left_ee_];
const auto& T_R = data_.oMf[right_ee_];
// 物体位姿估计（两末端中点）
pinocchio::SE3 T_obj;
T_obj.translation() = 0.5 * (T_L.translation()
+ T_R.translation());
T_obj.rotation() = T_L.rotation(); // 简化：用左臂姿态
// --- Step 2: 物体阻抗控制律 ---
Vector6d x_err =
pinocchio::log6(T_obj_des.inverse() * T_obj).toVector();
// log6 误差先在局部切空间中给出；本控制器后续 G/J 均用
// world-aligned wrench/twist，因此把线/角误差旋到世界方向。
x_err.head<3>() = T_obj_des.rotation() * x_err.head<3>();
x_err.tail<3>() = T_obj_des.rotation() * x_err.tail<3>();
// 物体速度（从关节速度推算）
Eigen::MatrixXd J_L(6, model_.nv), J_R(6, model_.nv);
J_L.setZero(); J_R.setZero();
pinocchio::getFrameJacobian(model_, data_, left_ee_,
pinocchio::LOCAL_WORLD_ALIGNED, J_L);
pinocchio::getFrameJacobian(model_, data_, right_ee_,
pinocchio::LOCAL_WORLD_ALIGNED, J_R);
auto skew = [](const Eigen::Vector3d& a) {
Eigen::Matrix3d S;
S << 0.0, -a.z(), a.y(),
a.z(), 0.0, -a.x(),
-a.y(), a.x(), 0.0;
return S;
};
auto shiftJacobian = [&](const Eigen::MatrixXd& J_frame,
const Eigen::Vector3d& p_frame) {
Eigen::Matrix<double, 6, 6> X =
Eigen::Matrix<double, 6, 6>::Identity();
Eigen::Vector3d d = T_obj.translation() - p_frame;
X.topRightCorner<3, 3>() = -skew(d); // [v;omega] 顺序
Eigen::MatrixXd J_ref = X * J_frame;
return J_ref;
};
Eigen::MatrixXd J_L_obj = shiftJacobian(J_L, T_L.translation());
Eigen::MatrixXd J_R_obj = shiftJacobian(J_R, T_R.translation());
Vector6d V_obj = 0.5 * (J_L_obj + J_R_obj) * dq;
Vector6d V_err = V_obj - V_obj_des;
// x_err = log(T_des^-1 * T_obj), V_err = V_obj - V_des。
// 误差按“当前相对期望”定义时，阻尼和刚度必须是负反馈。
Vector6d w_o_cmd =
M_o_ * A_obj_des - D_o_ * V_err - K_o_ * x_err;
// 加物体重力补偿：世界 z 轴向上时，支撑物体需要 +mg 的 Fz
// wrench 顺序 [Fx,Fy,Fz,tau_x,tau_y,tau_z]，所以 index 2 是 Fz
w_o_cmd(2) += obj_mass_ * 9.81;
// --- Step 3: Grasp Matrix 力分配 ---
// 与 Step 4 的 LOCAL_WORLD_ALIGNED Jacobian 保持对偶：
// f_L/f_R 均为接触点 world-aligned hand-on-object wrench。
Eigen::Vector3d r_L = T_L.translation() - T_obj.translation();
Eigen::Vector3d r_R = T_R.translation() - T_obj.translation();
Eigen::Matrix<double, 6, 6> G_L =
Eigen::Matrix<double, 6, 6>::Identity();
Eigen::Matrix<double, 6, 6> G_R =
Eigen::Matrix<double, 6, 6>::Identity();
G_L.bottomLeftCorner<3, 3>() = skew(r_L);
G_R.bottomLeftCorner<3, 3>() = skew(r_R);
Eigen::Matrix<double, 6, 12> G;
G << G_L, G_R;
// 伪逆（SVD 稳定版）
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
G, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::MatrixXd G_pinv = svd.solve(
Eigen::MatrixXd::Identity(6, 6)); // 12×6
// 力分配 = 运动力 + 内力
Eigen::VectorXd f_motion = G_pinv * w_o_cmd; // 12D
Eigen::MatrixXd N_G = Eigen::MatrixXd::Identity(12, 12)
- G_pinv * G;
Eigen::VectorXd f_int_ref = f_int_ref_;
if (f_int_ref.squaredNorm() < 1e-12) {
// 默认挤压力参考：沿当前两接触点连线相向挤压。
// 物体旋转或接触点移动后，方向会随几何实时更新。
Eigen::Vector3d n_LR = T_R.translation() - T_L.translation();
double n_norm = n_LR.norm();
if (n_norm > 1e-9) {
n_LR /= n_norm;
f_int_ref = Eigen::VectorXd::Zero(12);
f_int_ref.segment<3>(0) = f_int_scalar_ * n_LR;
f_int_ref.segment<3>(6) = -f_int_scalar_ * n_LR;
}
}
Eigen::VectorXd f_null = N_G * f_int_ref; // 12D，显式投影到 ker(G)
Eigen::VectorXd f_c = f_motion + f_null; // 12D
// --- Step 4: 关节力矩 ---
Eigen::VectorXd f_L = f_c.head(6);
Eigen::VectorXd f_R = f_c.tail(6);
// f_L/f_R 是 world-aligned hand-on-object；
// 机器人侧受到 object-on-hand = -f，与 LOCAL_WORLD_ALIGNED J 对偶。
Eigen::VectorXd tau = -J_L.transpose() * f_L
- J_R.transpose() * f_R;
// 加关节空间重力补偿
tau += pinocchio::computeGeneralizedGravity(model_, data_, q);
return tau; // 14D 关节力矩
}
private:
pinocchio::Model model_;
pinocchio::Data data_;
pinocchio::FrameIndex left_ee_, right_ee_;
pinocchio::SE3 T_rel_;
Matrix6d K_o_, D_o_, M_o_;
Eigen::VectorXd f_int_ref_;
double f_int_scalar_ = 15.0; // N，默认沿接触点连线的挤压力
double obj_mass_ = 1.0; // kg
};
// ============================================================
// ❌ 常见错误：忘记物体重力补偿
// ============================================================
// w_o_cmd = M_o * A_ref - D_o * V_err - K_o * x_err;
// → 没有 + mg！物体会在重力下持续下坠
// → Object Impedance 看起来"能跟踪"但有稳态下垂
// ✅ 正确做法：在世界 z 方向加物体重力补偿（假设 z 轴向上）
// w_o_cmd(2) += obj_mass * 9.81; // wrench = [Fx,Fy,Fz,tau_x,tau_y,tau_z]
// 如果仿真/机器人使用 z 轴向下或任意重力向量，应按世界系重力方向投影到力分量，
// 不要机械地固定写 index 2 为正。
⚠️ 常见陷阱¶
💡 概念误区：认为"Object Impedance 只是两个单臂阻抗的平均"
新手想法："K_o = K_L = K_R，物体阻抗就是末端阻抗"
实际上：Object Impedance 在物体坐标系定义，通过 G⁺ 自动分配到两臂。
当抓取点不对称时，两臂力分配也不对称——这是 Object Impedance
自动处理的，单臂阻抗做不到。
正确理解：Object Impedance 是物体级控制律，G⁺ 负责力分配。
⚠️ 编程陷阱：物体位姿估计偏差
问题：x_o 通常从两臂末端推算 x_o ≈ (x_L + x_R)/2
现象：抓取点不对称时，中点不等于质心，阻抗力方向错误
正确做法：用 Grasp Matrix 精确计算，或用视觉/标记跟踪物体位姿
练习¶
-
[实现] 在 MuJoCo 双 Franka 中实现 Object Impedance：物体 1 kg 盒子，\(K_o = [500, 500, 500, 20, 20, 20]\)。让物体跟踪正弦轨迹。记录跟踪误差和内力。
-
[对比] 同一搬运任务中对比（a）独立位控和（b）Object Impedance + 内力控制。记录接触力——预测（a）出现 force fight，（b）内力稳定。
D3.5 协调阻抗策略——两臂刚度如何配合 ⭐⭐¶
三种协调策略¶
策略 1：对称阻抗（Symmetric）
定义：K_L = K_R = K_s
适用：对称搬运（两手等价受力）
参数：K_s = 500 N/m, D_s = 2√(K_s · m_obj/2)
优势：简单、鲁棒、两臂等价
劣势：不适合非对称任务
策略 2：Leader-Follower 阻抗
定义：一臂高刚度（Leader），另一臂低刚度（Follower）
Leader: K_L = 1000-2000 N/m → 精确定位
Follower: K_R = 50-200 N/m → 吸收误差
适用：装配（一手持物一手操作）
优势：操作臂柔顺，自动避免 force fight
劣势：对 Leader 精度要求高；非对称容错性低
策略 3：相对阻抗（Lee-Chang-Jamisola 2014）
定义：在相对坐标 x_rel 上定义阻抗
M_rel · Δẍ_rel + D_rel · Δẋ_rel + K_rel · Δx_rel = f_rel
适用：拧盖/扭转操作（相对运动为主）
优势：天然表达"一刚一柔"
劣势：需同时设计绝对+相对两套参数
策略选择决策树¶
两手等价受力？
├── 是 → 对称阻抗 (搬运)
└── 否 → 一手固定一手操作？
├── 是 → Leader-Follower (装配)
└── 否 → 相对运动为主？
├── 是 → 相对阻抗 (拧盖/撕)
└── 否 → Object Impedance + 内力 (通用)
不是X而是Y：Leader-Follower 不是"一臂主动一臂被动"——两臂都在主动控制，只是目标不同。Leader 追求位置精度（高刚度），Follower 追求力柔顺（低刚度）。选择建模范式应根据任务的物理对称性，而非框架的复杂度。
稳定性分析¶
Caccavale 等人 2000 年（Automatica）证明：在绝对/相对坐标下 PD+g 控制律是**全局渐近稳定**的，条件：
1. K_abs > 0, D_abs > 0 (正定)
2. K_rel > 0, D_rel > 0 (正定)
3. 物体刚性
4. 重力完美补偿
若重力补偿有误差 g_err：
稳态误差 ≈ g_err / K
例: g_err=1N, K=500 N/m → 2mm 误差
⚠️ 常见陷阱¶
🧠 思维陷阱：认为"阻抗越高越好"
新手想法："K = 10000 N/m 可以更精确"
实际上：高阻抗在闭链中极危险。位置误差 Δx = 0.1mm 时：
K=500 → F=0.05 N（安全）
K=10000 → F=1.0 N（可能超摩擦锥）
高阻抗放大微小误差为巨大力。
正确做法：闭链阻抗 K = 200-1000 N/m。
练习¶
-
[设计] 对"一手持瓶身一手拧瓶盖"设计协调阻抗参数。持瓶手高刚度还是低刚度？拧盖手呢？绕瓶轴方向如何设定？
-
[仿真] 在 MuJoCo 中实现对称和 Leader-Follower 策略，比较内力峰值和跟踪误差。
三种协调策略的在线切换逻辑¶
真实双臂任务通常涉及多个阶段——搬运阶段用对称阻抗，对准阶段切换到主从，装配阶段使用相对阻抗。策略切换的关键是**平滑过渡**——突变参数会产生力矩阶跃。
// ============================================================
// 协调策略在线切换——带平滑过渡的 FSM
// ============================================================
enum class CoordMode { SYMMETRIC, LEADER_FOLLOWER, RELATIVE };
using Matrix6d = Eigen::Matrix<double, 6, 6>;
struct CoordParams {
Matrix6d K_L, K_R, K_rel, D_L, D_R;
};
class CoordinationSwitcher {
public:
// 根据任务状态决定模式
CoordMode decide_mode(
double task_phase, // 0=搬运 1=对准 2=装配
double v_rel_norm, // 相对速度范数
double f_contact_margin // 摩擦锥余量
) {
if (task_phase < 0.8 && v_rel_norm < 0.01)
return CoordMode::SYMMETRIC;
if (task_phase < 1.8)
return CoordMode::LEADER_FOLLOWER;
return CoordMode::RELATIVE;
}
// 平滑过渡参数（指数滤波，时间常数 0.3s）
CoordParams smooth_transition(
CoordMode target, double dt)
{
static constexpr double tau = 0.3; // 过渡时间常数
double alpha = 1.0 - std::exp(-dt / tau);
CoordParams target_params = get_preset(target);
current_.K_L += alpha * (target_params.K_L - current_.K_L);
current_.K_R += alpha * (target_params.K_R - current_.K_R);
current_.K_rel += alpha * (target_params.K_rel - current_.K_rel);
return current_;
}
private:
CoordParams get_preset(CoordMode mode) {
CoordParams p;
switch (mode) {
case CoordMode::SYMMETRIC:
p.K_L = p.K_R = 500.0 * Matrix6d::Identity();
p.K_rel = Matrix6d::Zero();
break;
case CoordMode::LEADER_FOLLOWER:
p.K_L = 1500.0 * Matrix6d::Identity();
p.K_R = 100.0 * Matrix6d::Identity();
p.K_rel = Matrix6d::Zero();
break;
case CoordMode::RELATIVE:
p.K_L = p.K_R = 200.0 * Matrix6d::Identity();
p.K_rel = 800.0 * Matrix6d::Identity();
break;
}
return p;
}
CoordParams current_;
};
反事实推理：如果不做平滑过渡直接切换 \(K\) 从 500 到 1500 N/m，位置误差 5mm 时的力矩跳变为 \(\Delta\tau = 1000 \times 0.005 = 5\) N。虽然幅度不大，但这是一个阶跃——可能激励双 Franka 结构的 30-50 Hz 共振频率。指数过渡（\(\tau = 0.3\)s）将阶跃变为连续曲线，避免高频激励。
D3.6 双臂 QP-WBC——统一优化框架 ⭐⭐⭐¶
动机：为什么需要 QP¶
D3.2-D3.5 各自解决一个子问题。实际控制需要**同时满足**多个约束：动力学可行、摩擦锥、内力安全区间、扭矩不饱和、任务跟踪。QP 将所有约束统一到一个优化问题中。
回顾 M05：QP 的标准形式 \(\min \frac{1}{2}x^T H x + f^T x\) s.t. \(Ax \leq b\)。双臂 QP-WBC 的特色是：Grasp 约束和内力约束作为额外约束/代价项加入。
QP 完整组装¶
决策变量：\(z = [\ddot{q}^T, \tau^T, f_L^T, f_R^T]^T \in \mathbb{R}^{40}\)
代价函数：
等式约束：
| 约束 | 方程 | 维度 |
|---|---|---|
| 动力学 | \(M(q)\ddot{q} + h(q,\dot{q}) = \tau - J_L^T f_L - J_R^T f_R\) | 14 |
| Grasp | \(G \cdot [f_L; f_R] = w_{o,\text{cmd}}\) | 6 |
| 任务跟踪 | \(J_{\text{abs}} \ddot{q} + \dot{J}_{\text{abs}} \dot{q} = \ddot{x}_{\text{abs,des}}\) | 6 |
不等式约束：
| 约束 | 方程 | 维度 |
|---|---|---|
| 摩擦锥 | \(C_{\text{fc}} [f_L; f_R] \leq 0\) | \(2 \times 4k\) |
| 扭矩限 | \(\tau_{\min} \leq \tau \leq \tau_{\max}\) | 28 |
内力目标（软约束代价项）：
这里右侧也必须投影。若直接跟踪未投影的 \(f_{\text{int,ref}}\)，QP 会试图把一部分“运动力分量”当内力跟踪，轻则权重冲突，重则在 Grasp 等式约束和内力代价之间制造不必要的不可行趋势。 工程实现中推荐先计算 \(f_{\text{int,ref}}^N = N_G f_{\text{int,ref}}\)，再把软约束写成 \(\|N_G[f_L;f_R] - f_{\text{int,ref}}^N\|^2\)；这样日志和调参时看到的“内力参考”就是已经落在 grasp 零空间里的物理量。
与单臂 QP-WBC 对比¶
单臂 QP-WBC (F07):
变量: [q̈, τ, f_c] ≈ 20 维
约束: 动力学 + 摩擦锥 ≈ 30 行
双臂 QP-WBC (本章):
变量: [q̈, τ, f_L, f_R] ≈ 40 维
约束: 动力学 + 摩擦锥
+ Grasp 约束 (6 行) ← 新增
+ 内力软约束 ← 新增代价项
总: ≈ 40 变量 × 60 约束 → OSQP ≈ 0.5 ms
本质洞察：双臂 QP-WBC 的核心创新是将 Grasp 约束（\(G [f_L; f_R] = w_o\)）作为等式约束加入 QP——这一行约束同时保证了（1）物体按期望运动；（2）两臂力协调（不打架）。内力通过软约束代价项管理，允许求解器在硬约束冲突时适当调整内力，而非直接不可行。
OSQP 求解框架¶
#include <OsqpEigen/OsqpEigen.h>
const int n_var = 14 + 14 + 6 + 6; // q̈ + τ + f_L + f_R = 40
// --- 构建 H (代价矩阵, 稀疏) ---
Eigen::SparseMatrix<double> H(n_var, n_var);
// 对角块: W1(力跟踪), W2, λ_q*I(加速度正则), λ_τ*I(力矩正则)
// --- 构建约束矩阵 A ---
// 等式: 动力学 [M, -I, J_L^T, J_R^T] z = -h
// 这里 f_L/f_R 是 world-aligned hand-on-object；
// 机器人侧接触项是 -J^T f，且 J 必须使用同一 wrench frame
// Grasp [0, 0, G_L, G_R] z = w_o_cmd
// 任务 [J_abs, 0, 0, 0] z = ẍ_des - J̇q̇
// 不等式: 摩擦锥 [0, 0, C_fc] z ≤ 0
// 扭矩限 [0, I, 0, 0] z ≤ τ_max
OsqpEigen::Solver solver;
solver.settings()->setVerbosity(false);
solver.settings()->setWarmStart(true); // 利用上一步解加速
solver.data()->setNumberOfVariables(n_var);
solver.data()->setNumberOfConstraints(n_eq + n_ineq);
solver.data()->setHessianMatrix(H);
solver.data()->setGradient(f);
solver.data()->setLinearConstraintsMatrix(A);
solver.data()->setLowerBound(lb);
solver.data()->setUpperBound(ub);
solver.initSolver();
solver.solve();
// 提取结果
auto z = solver.getSolution();
Eigen::VectorXd qdd = z.head(14);
Eigen::VectorXd tau = z.segment(14, 14);
Eigen::VectorXd f_L = z.segment(28, 6);
Eigen::VectorXd f_R = z.tail(6);
⚠️ 常见陷阱¶
⚠️ 编程陷阱：QP 返回 INFEASIBLE
常见原因：
1. Grasp 约束与摩擦锥冲突（w_o 太大，锥内无法实现）
→ 降低物体加速度或增大 μ
2. 扭矩限与动力学冲突
→ 放松加速度或用软约束
3. 约束矩阵病态
→ 检查 M 的条件数
排查步骤：
1. 去掉摩擦锥看是否可行
2. 再去掉扭矩限看是否可行
3. 逐步添加约束找到冲突源
💡 概念误区：认为"QP 权重随便设"
实际上：权重决定了"运动精度 vs 力精度 vs 能耗"的权衡。
λ_q 太大 → 加速度跟踪好但力不精确
λ_int 太大 → 内力精确但运动可能偏差
正确做法：从简单场景（静态平衡）开始调权重，逐步增加复杂度。
练习¶
-
[组装] 手动写出双臂搬运 2 kg 箱子的 QP 所有矩阵。估算变量数和约束数。
-
[实现] 用 OSQP 求解，在 MuJoCo 中执行并验证：跟踪误差 < 5mm，内力在安全区间，接触力在摩擦锥内。
-
[跨章综合] 回顾 F07/F08 单臂 QP-WBC。与本章双臂 QP 对比——新增哪些约束？变量多了什么？QP 规模增大多少？
D3.7 柔性物体操作与滑动检测 ⭐⭐⭐¶
动机：超越刚体假设¶
D3.2-D3.6 假设被操作物体刚性。但许多真实任务涉及柔性物体：线缆、布料、海绵、薄板。刚性假设失效时，Grasp Matrix 随变形变化，约束流形变为时变——需要变形模型。
柔性物体的 Grasp Matrix 修正¶
刚性物体：G 固定（两接触点相对位姿不变）
柔性物体：G(x_def) 依赖变形状态 x_def
简化模型（线性弹性）：
Δx_rel = K_obj^{-1} · f_int
其中 K_obj 是物体刚度矩阵
f_int 产生变形 → 变形改变接触点位姿 → G 变化 → f_int 改变
→ 需要迭代求解或用增量式更新
FEM 模型简介——柔性物体力学基础¶
对于需要精确变形预测的场景（如柔性电路板装配、软组织操作），有限元法（FEM）是标准工具。双臂控制中 FEM 的角色是：给定接触力 \(f_c\)，预测物体变形 \(\Delta x\)，进而更新 Grasp Matrix。
FEM 在双臂力控中的层次：
控制器 (1 kHz) ──→ 力分配 (G) ──→ 关节力矩 (τ)
↑ ↑
│ │
FEM 求解器 (50-100 Hz) ─→ G(x_def) 更新
↑
│
接触力 f_c → 物体变形 x_def
注意：FEM 求解比控制循环慢 10-20 倍
→ 不能每个控制周期都做 FEM
→ 策略：FEM 在低频线程异步更新 G，控制器用最近的 G
简化 FEM 模型——弹簧网格：
对于教学目的，可用弹簧-质点网格近似 FEM：
物体离散化为 N 个质点，相邻质点用弹簧连接：
质点 i 的运动方程：
m_i ẍ_i = Σ_{j∈N(i)} k_{ij} (x_j - x_i - L_{ij}^0 · n̂_{ij}) + f_ext_i
矩阵形式：
M ẍ = K · x + f_ext
其中 K 是刚度矩阵（稀疏，N×N 块三对角）
对于双臂：
f_ext 中包含两个接触力（作用在接触节点上）
→ 求解 x 得到变形
→ 从变形更新两接触点的相对位姿
→ 更新 G
典型参数：
海绵：k ≈ 100-500 N/m，N = 20-50 节点
布料：k ≈ 10-100 N/m，N = 100-500 节点
薄金属板：k ≈ 10^4-10^5 N/m，N = 50-200 节点
跨领域类比：FEM 之于双臂力控，就像 CFD 之于飞行控制。飞行控制器不在线求解 Navier-Stokes 方程——太慢了。取而代之的是离线计算气动系数表（look-up table），在线查表。同样，双臂控制器不在线做 FEM——而是离线建立"内力 → 变形 → G 修正"的映射表，在线查表。
滑动检测与响应¶
滑动信号：
1. 力矩传感器：f_n 突降 + ‖f_t‖ 突增
检测条件：f_n(t) < 0.9·f_n(t-Δt) AND ‖f_t(t)‖ > 1.1·‖f_t(t-Δt)‖
2. 触觉传感器：接触面积减小 + 压力分布偏移
3. 视觉：物体相对末端位移 > 阈值
滑动响应（分级）：
Level 1（微滑动）：f_int ← f_int × 1.2（轻微收紧）
Level 2（明显滑动）：f_int ← f_int × 1.5 + 减速 50%
Level 3（即将脱落）：紧急停止 + 最大内力 + 报警
滑动检测算法——触觉 + 力矩联合检测¶
单一传感器的滑动检测不可靠（力矩传感器有噪声，触觉传感器有延迟）。鲁棒的方案是融合多种信号：
// ============================================================
// 多模态滑动检测器
// 融合：力矩变化率 + 法向力比 + 摩擦锥余量
// ============================================================
struct SlipDetector {
enum Level { STABLE, MICRO_SLIP, MACRO_SLIP, CRITICAL };
Level detect(
const Eigen::Vector3d& f_contact, // 接触力 [fx, fy, fz]
const Eigen::Vector3d& f_prev, // 上一时刻接触力
double mu, // 摩擦系数
double dt)
{
double f_n = f_contact.z(); // 法向力
double f_t = f_contact.head(2).norm(); // 切向力
// 指标 1：摩擦锥余量
// margin = 1 - f_t / (μ f_n)，越小越危险
double margin = 1.0 - f_t / (mu * std::max(f_n, 0.1));
// 指标 2：法向力变化率
double df_n = (f_n - f_prev.z()) / dt;
// 指标 3：切向力变化率
double df_t = (f_t - f_prev.head(2).norm()) / dt;
// 指标 4：力比率突变
double ratio = f_t / std::max(f_n, 0.1);
double ratio_prev = f_prev.head(2).norm()
/ std::max(f_prev.z(), 0.1);
double d_ratio = (ratio - ratio_prev) / dt;
// 综合判断
double score = 0.0;
if (margin < 0.3) score += 2.0; // 接近摩擦锥边界
if (margin < 0.1) score += 3.0; // 极度危险
if (df_n < -50.0) score += 2.0; // 法向力突降
if (df_t > 30.0) score += 1.5; // 切向力突增
if (d_ratio > 5.0) score += 2.0; // 力比率突变
if (score >= 7.0) return CRITICAL;
if (score >= 4.0) return MACRO_SLIP;
if (score >= 2.0) return MICRO_SLIP;
return STABLE;
}
};
// 响应策略
void respond_to_slip(SlipDetector::Level level,
double& f_int_ref,
double& v_scale)
{
switch (level) {
case SlipDetector::STABLE:
// 正常操作，可以缓慢降低内力以减少能耗
f_int_ref *= 0.999;
f_int_ref = std::max(f_int_ref, f_int_min_);
v_scale = 1.0;
break;
case SlipDetector::MICRO_SLIP:
f_int_ref *= 1.2;
v_scale = 0.8;
break;
case SlipDetector::MACRO_SLIP:
f_int_ref *= 1.5;
v_scale = 0.5;
break;
case SlipDetector::CRITICAL:
f_int_ref = f_int_max_;
v_scale = 0.0; // 紧急停止
break;
}
f_int_ref = std::clamp(f_int_ref, f_int_min_, f_int_max_);
}
⚠️ 常见陷阱¶
🧠 思维陷阱：认为"滑动总是坏事"
实际上：某些任务需要受控滑动：
- 在手操作：通过控制滑动重定向物体
- 抛接：释放阶段需要受控滑动
- 布料折叠：布料在指间滑动是操作的一部分
正确做法：区分"意外滑动"（修正）和"受控滑动"（允许）。
⚠️ 编程陷阱：滑动检测的采样频率不够
现象：物体已经滑落但检测器才报 MICRO_SLIP
根本原因：力矩传感器 1kHz，但检测算法只在 100Hz 运行
→ 漏掉了 10ms 内的快速力变化
正确做法：滑动检测必须与力控同频（1kHz），
至少滤波后的信号在 500Hz 以上
练习¶
-
[思考] 柔性物体的 Grasp Matrix 如何定义？布料的"刚度矩阵"与钢杆有何本质区别？
-
[设计] 为"双臂共持海绵搬运"设计滑动检测参数。海绵的 \(\mu \approx 0.8\)，但变形使接触面积随时间变化——这对检测算法有什么影响？
-
[跨章综合] 回顾 F03 中的摩擦锥模型。如果摩擦系数 \(\mu\) 随接触面温度变化（如热工件），滑动检测器应如何适应？提示：在线估计 \(\mu\)。
D3.8 与单臂力控的对偶关系 ⭐⭐¶
F 系列 → D 系列的系统映射¶
| 单臂（F 系列） | 双臂（D 系列） | 对偶关系 |
|---|---|---|
| 末端阻抗 \(F_{\text{cmd}} = -K\tilde{x}-D\dot{\tilde{x}}\) | Object Impedance \(w_o = -K_o\tilde{x}_o-D_o\dot{\tilde{x}}_o\) | 控制对象：末端 → 物体 |
| 操作空间惯量 \(\Lambda\) | 物体空间惯量 \(\Lambda_o\) | 投影：关节→操作/物体空间 |
| 零空间 \(N = I - J^+J\) | 内力零空间 \(N_G = I - G^+G\) | 高优先级之外的自由度 |
| \(\tau = J^T f_{\text{robot}}\) | \(\tau_L = -J_L^T f_L\), \(\tau_R = -J_R^T f_R\) | 本章 \(f_L,f_R\) 是手对物体，机器人侧符号相反 |
| 单臂 QP | 双臂 QP + Grasp 约束 | 新增内力管理 |
跨章回顾桥：回顾 F02（操作空间动力学）：Khatib 1987 的 \(\tau = J^T \Lambda \ddot{x}_d + J^T \mu + g\) 将关节控制转化为任务控制。双臂 Object Impedance 是自然推广——从"一个末端一个任务空间"到"一个物体一个物体空间"。\(G\) 的角色类似 \(J\)：\(J\) 映射速度，\(G\) 映射力。
与 F01-F06 力控知识的系统化对比表¶
以下表格系统梳理双臂力控（D 系列）如何继承和扩展单臂力控（F 系列）的每个概念：
| 维度 | F01 阻抗导论 | F02 操作空间 | F03 经典力控 | F04 力/位混合 | F05 接触过渡 | F06 约束力控 | D03 双臂协调力控 |
|---|---|---|---|---|---|---|---|
| 控制对象 | 末端(6D) | 末端(6D) | 末端(6D) | 末端(6D) | 末端(6D) | 末端(6D) | 物体(6D)+内力(6D) |
| 核心矩阵 | \(K_d, D_d, M_d\) | \(\Lambda = (JM^{-1}J^T)^{-1}\) | PID 增益 | Selection \(S\) | 接触切换矩阵 | \(J_c^T \lambda\) | \(G, K_o, D_o, M_o\) |
| 零空间 | \(N = I - J^+J\) | \(N\dot{q}_0\) | — | — | — | \(N_c\) | \(N_G = I - G^+G\)（内力空间） |
| 力映射 | \(\tau = J^T f\) | \(\tau = J^T \Lambda \ddot{x}\) | \(\tau = J^T f_{PID}\) | \(\tau = J^T(Sf + (I-S)Kx)\) | \(\tau = J^T f_{smooth}\) | \(\tau = J^T f + J_c^T \lambda\) | \(\tau = -J_L^T f_L - J_R^T f_R\)（若 \(f\) 为手对物体） |
| 稳定性 | 被动性 | Lyapunov | BIBO | 条件稳定 | 切换稳定性 | 约束兼容 | Lyapunov(abs+rel) |
| 典型频率 | 1 kHz | 1 kHz | 100-500 Hz | 500 Hz-1 kHz | 1 kHz | 1 kHz | 1 kHz |
| 独有挑战 | 参数整定 | 奇异点 | 稳态误差 | S 矩阵选择 | 碰撞检测 | 约束兼容 | force fight / 内力管理 |
从 F → D 的三个关键跳跃：
-
从"一对一"到"一对二"力映射：F 系列中 \(J^T\) 将一个 6D 任务力映射到关节力矩。D 系列中 \(G^+\) 先将 6D 物体 wrench 分配到 12D hand-on-object 接触力，再用 \(-J_L^T, -J_R^T\) 映射到两组关节力矩。多了一个"力分配"步骤，也多了一个必须明确的接触力方向约定。
-
从"无内力"到"内力管理"：F 系列的零空间（\(I - J^+J\)）用于次要任务（关节极限回避、操作度优化）。D 系列的内力零空间（\(I - G^+G\)）多了一个全新的物理维度——内力不产生物体运动但维持抓取，需要主动管理。
-
从"单一稳定性"到"双层稳定性"：F 系列只需证明末端行为的稳定性。D 系列需要同时证明物体运动（绝对变量）和抓取关系（相对变量）的稳定性——两者通过 Lyapunov 函数的两个势能项分别保证。
不是X而是Y：D 系列不是 F 系列的"升级版"——而是在 F 系列基础上增加了一个全新的维度（内力空间）。掌握 F01-F06 是学习 D03 的必要条件，但不是充分条件——内力管理、Grasp Matrix、力分配是双臂独有的概念，在单臂中完全不存在。
练习¶
-
[对比表] 写出单臂操作空间控制律和双臂 Object Impedance 的并行对比，标注每个量的维度。
-
[思考] F04（力/位混合控制）中的 Selection Matrix \(S\) 与 D03 中的 Grasp Matrix 零空间投影 \((I - G^+G)\) 有什么概念上的对应关系？提示：\(S\) 选择哪些维度做力控、哪些做位控；\((I - G^+G)\) 选择哪些力分量是"内力"（不影响物体运动）。
-
[跨章综合] 回顾 F05（接触过渡）中的碰撞检测和力突变处理。在双臂 handover（空中交接）过程中，接触状态从"单臂持物"切换到"双臂共持"再到"单臂持物"——F05 的接触过渡策略如何与 D03 的 Grasp Matrix 切换结合？
D3.9 双臂力控的稳定性分析 ⭐⭐⭐¶
Lyapunov 稳定性框架——完整证明¶
Caccavale 等人 2000 年（Automatica）的证明是双臂协调力控稳定性分析的标准方法。以下给出完整推导过程。
系统动力学：
双臂闭链系统在绝对/相对坐标下的动力学为：
其中控制律：
代入动力学方程（假设完美重力补偿 \(g(q)\) 抵消）：
Step 1：定义 Lyapunov 候选函数
\(V > 0\) 对所有 \((\dot{q}, \tilde{x}_{\text{abs}}, \tilde{x}_{\text{rel}}) \neq 0\)，因为 \(M > 0\)（惯量矩阵正定）且 \(K_{\text{abs}}, K_{\text{rel}} > 0\)（正定刚度）。
Step 2：计算 \(\dot{V}\)
由动力学方程：\(M \ddot{q} = -C\dot{q} + \tau - g\)（已代入控制律）。因此：
Step 3：利用 \(\dot{M} - 2C\) 的反对称性
由机器人动力学的标准性质：\(\dot{q}^T (\dot{M} - 2C) \dot{q} = 0\)。因此：
Step 4：利用 \(\dot{x} = J \dot{q}\) 简化
与剩余项 \(\dot{\tilde{x}}_{\text{abs}}^T K_{\text{abs}} \tilde{x}_{\text{abs}} + \dot{\tilde{x}}_{\text{rel}}^T K_{\text{rel}} \tilde{x}_{\text{rel}}\) 合并。
注意 \(\dot{\tilde{x}} = \dot{x} - \dot{x}_d\)。当期望轨迹为静态（\(\dot{x}_d = 0\)）时，\(\dot{\tilde{x}} = \dot{x}\)，因此：
Step 5：最终结果
\(\dot{V} = 0\) 仅当 \(\dot{x}_{\text{abs}} = 0\) 且 \(\dot{x}_{\text{rel}} = 0\)。
Step 6：LaSalle 不变性原理
集合 \(S = \{(\dot{q}, \tilde{x}) : \dot{V} = 0\} = \{(\dot{q}, \tilde{x}) : \dot{x}_{\text{abs}} = 0, \dot{x}_{\text{rel}} = 0\}\)。
这里必须小心：令
\(\dot{x}_c=0\) 只说明 \(\dot{q}\in\ker(J_c)\)，并不自动推出 \(\dot{q}=0\)。因此 LaSalle 的最后一步需要下面两种补充条件之一。
情况 A：无冗余/约化坐标满秩。 如果闭链约束已经消除了冗余自由度，且 \(J_c\) 在工作域内满列秩（\(\ker(J_c)=\{0\}\)），那么 \(\dot{x}_c=0 \Rightarrow \dot{q}=0\)。最大不变集还要求 \(\ddot{q}=0\)，代入动力学方程得到
在任务坐标独立、\(K_{\text{abs}},K_{\text{rel}}>0\) 的假设下，唯一不变点为 \(\tilde{x}_{\text{abs}}=0,\tilde{x}_{\text{rel}}=0\)。
情况 B：冗余双臂。 对两个 7-DOF 手臂，通常 \(J_c\) 有零空间。此时需要显式管理自运动，例如加入零空间阻尼
则 \(\dot{V}\) 额外出现
在 \(S\) 中同时有 \(J_c\dot{q}=0\) 和 \(N_c\dot{q}=0\)，因此 \(\dot{q}=0\)，再按情况 A 的不变集推理得到任务误差收敛。若既不满足满秩条件、也没有零空间阻尼，LaSalle 只能保证收敛到“任务速度为零”的最大不变集，不能排除残余自运动。
由 LaSalle 定理：在上述满秩或零空间阻尼条件成立时，系统在该工作域内渐近收敛到平衡点。\(\blacksquare\)
本质洞察：Lyapunov 证明的核心技巧是利用 \(\dot{M} - 2C\) 的反对称性消去了科氏力和离心力项——这是所有基于能量的机器人控制稳定性证明的标准"套路"。物理含义是：科氏力/离心力不做功（只改变速度方向不改变速度大小），所以它们不影响系统总能量的变化。
稳定性的实际含义与局限：
在上述工作域和秩/零空间阻尼假设下，渐近稳定意味着：
- 从该工作域内的初始状态出发，系统最终收敛到期望平衡点
- 没有极限环或混沌行为
- 外部扰动消失后系统回到期望状态
证明所需条件及现实检验：
┌────────────────────┬──────────────────┬──────────────────┐
│ 理论条件 │ 现实情况 │ 违反后果 │
├────────────────────┼──────────────────┼──────────────────┤
│ K,D > 0 正定 │ ✅ 可以满足 │ 不稳定 │
│ J_c 满秩或零空间阻尼│ ⚠️ 需显式检查/设计│ 残余自运动 │
│ 物体刚性 │ ⚠️ 近似满足 │ 约束漂移 │
│ 重力完美补偿 │ ❌ 有模型误差 │ 稳态偏差 │
│ 无关节摩擦 │ ❌ 有摩擦 │ 死区/极限环 │
│ 连续时间 │ ❌ 离散 1kHz │ 高频不稳定 │
│ 无通信延迟 │ ❌ ~1ms 延迟 │ 相位裕度降低 │
│ 无传感器噪声 │ ❌ 力传感器噪声 │ 力抖动 │
└────────────────────┴──────────────────┴──────────────────┘
实际鲁棒性保证：
- 离散化：采样时间 < 1/(10 × 最大闭环带宽) ≈ 0.5ms
→ 1kHz (1ms) 对于 < 50Hz 带宽的阻抗控制足够安全
- 延迟：相位裕度 > 30° → 延迟 < 1/(12 × 带宽)
→ 50Hz 带宽允许 ~1.7ms 延迟（1ms 安全）
- 噪声：力传感器噪声 < 0.5N → 加低通滤波（截止 100Hz）
⚠️ 常见陷阱¶
💡 概念误区：认为"稳定性证明=实际稳定"
实际上：Lyapunov 证明假设连续时间、无延迟、无噪声。
实际离散控制（1kHz）、通信延迟（~1ms）、传感器噪声
都可能破坏稳定性。需要鲁棒性分析（如 passivity 框架，见 D06）。
🧠 思维陷阱：认为"只要 K,D > 0 就一定稳定"
新手想法："正定刚度和阻尼就够了，其他条件是'理论假设'"
实际上：当控制频率不够高时，即使 K,D > 0 也可能不稳定。
例如 K = 10000 N/m，固有频率 ω_n = √(K/M) ≈ 100 rad/s ≈ 16 Hz。
如果控制频率只有 100 Hz（Nyquist 50 Hz），16 Hz 的闭环动态
只有 3 倍过采样——在延迟和相位误差下可能失稳。
正确做法：K 的上限由控制频率决定：
K_max ≈ M × (2π × f_ctrl / 10)²
对于 M=2kg, f_ctrl=1000Hz: K_max ≈ 2 × (628)² ≈ 790000 N/m
远超实际使用范围，1kHz 足够安全。
但对于 f_ctrl=100Hz: K_max ≈ 2 × (62.8)² ≈ 7900 N/m
此时 K=10000 可能不稳定！
练习¶
-
[推导] 对于对称阻抗（\(K_L = K_R = K_s\)），写出 Lyapunov 函数并验证 \(\dot{V} \leq 0\)。
-
[计算] 对于 \(M_o = 2\) kg，\(K_o = 1000\) N/m，计算固有频率 \(\omega_n\) 和临界阻尼系数 \(D_c\)。如果控制频率从 1kHz 降到 250Hz（ABB YuMi 的 EGM 频率），最大安全刚度 \(K_{\max}\) 是多少？
-
[编程] 在 MuJoCo 中测试：从同一初始偏差出发，分别用 \(K = 500, 1000, 5000\) N/m，在 1kHz 和 100Hz 控制频率下比较收敛曲线。验证高 \(K\) + 低频率的不稳定性。
本章小结¶
| 知识点 | 核心要点 | 对应小节 | 难度 |
|---|---|---|---|
| 双臂力控动机 | force fight、内力空间、闭链 | D3.1 | ⭐ |
| Grasp Matrix | \(G \in \mathbb{R}^{6 \times 12}\)，rank=6，6D 零空间=内力 | D3.2 | ⭐⭐ |
| 内力/运动力分解 | \(f_c = G^+ w_o + (I-G^+G) f_{\text{int}}\) | D3.3 | ⭐⭐ |
| Object Impedance | \(M_o\ddot{\tilde{x}}+D_o\dot{\tilde{x}}+K_o\tilde{x}=f_{\text{ext}}\) | D3.4 | ⭐⭐ |
| 协调阻抗 | 对称/Leader-Follower/相对阻抗 | D3.5 | ⭐⭐ |
| 双臂 QP-WBC | 动力学+Grasp+摩擦锥+内力 统一优化 | D3.6 | ⭐⭐⭐ |
| 柔性物体/滑动 | 变形模型、滑动检测响应 | D3.7 | ⭐⭐⭐ |
| F→D 对偶 | 单臂到双臂力控的系统映射 | D3.8 | ⭐⭐ |
| 稳定性分析 | Lyapunov 全局渐近稳定条件 | D3.9 | ⭐⭐⭐ |
累积项目：本章新增模块¶
mini_dualarm/
├── src/
│ ├── grasp_matrix.cpp # G 计算 + 零空间 + 对称/非对称抓取 (NEW)
│ ├── force_decomposition.cpp # 内力/运动力分解 + 加权伪逆 (NEW)
│ ├── object_impedance.cpp # Object Impedance 完整控制器 (NEW)
│ ├── coordinated_impedance.cpp # 三种协调策略 + 切换 FSM (NEW)
│ ├── dual_arm_qp_wbc.cpp # QP-WBC 求解 (OSQP) (NEW)
│ ├── slip_detector.cpp # 多模态滑动检测器 (NEW)
│ └── coordination_fsm.cpp # 策略切换状态机 (NEW)
├── config/
│ ├── impedance_params.yaml # 阻抗参数配置 (NEW)
│ └── slip_thresholds.yaml # 滑动检测阈值 (NEW)
├── scripts/
│ ├── grasp_matrix_viz.py # Grasp Matrix 零空间可视化 (NEW)
│ └── force_decomp_demo.py # 内力/运动力分解交互演示 (NEW)
└── tests/
├── test_grasp_matrix.cpp # rank 验证 + 零空间 (NEW)
├── test_force_decomp.cpp # G·f_null=0 验证 (NEW)
└── test_stability.cpp # Lyapunov 条件数值验证 (NEW)
本章项目与 D01/D02 模块的集成：D01 提供运动学（\(J_{\text{aug}}, J_{\text{rel}}\)），D02 提供约束规划路径，D03 提供力控执行。完整管线：D02 规划路径 → D03 Object Impedance 跟踪路径 + 内力管理 → 关节力矩输出。
延伸阅读¶
| 文献 | 类型 | 难度 | 推荐理由 |
|---|---|---|---|
| Caccavale et al. 2008, "Six-DOF Impedance Control", IEEE/ASME T-Mech | 论文 | ⭐⭐⭐ | 双臂阻抗事实标准 |
| Williams & Khatib 1993, "The Virtual Linkage", ICRA | 论文 | ⭐⭐ | 内力空间最佳直觉工具 |
| Schneider & Cannon 1992, "Object Impedance Control", IEEE T-RA | 论文 | ⭐⭐⭐ | Object Impedance 开山之作 |
| Bonitz & Hsia 1996, "Internal Force-Based Impedance", IEEE T-RA | 论文 | ⭐⭐⭐ | 内力主动整形经典方法 |
| Caccavale et al. 2000, "Task-Space Regulation", Automatica | 论文 | ⭐⭐⭐⭐ | 全局渐近稳定性证明 |
| Lee et al. 2014, "Relative Impedance Control", IEEE T-IE | 论文 | ⭐⭐⭐ | 相对阻抗完整理论 |
| Uchiyama & Dauchez 1988, "Symmetric Kinematic Formulation", ISRR | 论文 | ⭐⭐ | 对称协调运动学的起源——理解绝对/相对分解的历史脉络 |
| Murray, Li, Sastry 1994, "A Mathematical Introduction to Robotic Manipulation", Ch.5 | 教材 | ⭐⭐⭐ | Grasp Matrix 的 Lie 群视角——最严谨的数学处理 |
| Siciliano et al. "Robotics: Modelling, Planning and Control", Ch.9.4 | 教材 | ⭐⭐ | 多臂协调力控的教科书级讲解，适合从零开始系统学习 |
| Hogan 1985, "Impedance Control: An Approach to Manipulation", ASME | 论文 | ⭐⭐⭐⭐ | 阻抗控制开山之作——理解 D03 前必须精读 F01 |
故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关章节 |
|---|---|---|---|
| Force fight（内力 > 50N） | 独立位控 / 阻抗过高 | 1. 检查是否用协调控制 2. 降低 K 3. 检查 f_int_ref | D3.1, D3.5 |
| 物体滑脱 | 内力不足 | 1. 验证 f_int > mg/(2μ) 2. 增大 f_int_ref 3. 检查 μ 估计 | D3.3 |
| Object Impedance 跟踪发散 | 阻尼不足 / M_o 错误 | 1. 增大 D_o 2. 检查物体质量 3. 降低 K_o | D3.4 |
| QP 返回 INFEASIBLE | 约束冲突 | 1. 去掉摩擦锥测试 2. 降低加速度 3. 检查 G 条件数 | D3.6 |
| QP 力矩跳变 | 接触状态切换 | 1. 加平滑窗口 2. 检查接触逻辑 3. warm start | D3.6 |
| 策略切换时振荡 | 参数突变 / 过渡太快 | 1. 增大过渡时间常数 τ 2. 检查 FSM 切换条件无抖动 3. 加迟滞（hysteresis） | D3.5 |
| Grasp Matrix 条件数爆炸 | 接触点太近或共线 | 1. 打印 σ_min(G) 2. 增大两接触点距离 3. 改变抓取构型 | D3.2 |
| 滑动检测误报 | 传感器噪声 / 阈值太灵敏 | 1. 加低通滤波（100Hz截止）2. 提高检测阈值 3. 增加多传感器融合 | D3.7 |
| 柔性物体操作时 G 失效 | 变形改变接触几何 | 1. 确认 FEM/弹簧模型运行 2. 增大 G 更新频率 3. 降低内力减少变形 | D3.7 |
| Lyapunov 理论稳定但实际振荡 | 离散化/延迟/噪声 | 1. 检查控制频率 vs K 匹配 2. 降低 K 3. 增大 D（过阻尼）4. 检查通信延迟 | D3.9 |