主题：逆动力学、控制分配、摩擦与限位处理；操作空间控制（Operational Space Control, OSC）与力/位置混合控制；梯度/雅可比的数值稳定性与实现细节。
目标：以教科书式严格推导为主线，把这些方法的数学基础、KKT / 最优化表述、稳定性证明与数值实现要点全部展开，做到读者能够把推导直接映射为实现代码或求解器接口。

---

8.1 符号与基础模型

在开始之前统一符号与基本动力学方程。

8.1.1 基本动力学方程（Lagrange 形式）

机器人刚体动力学（无约束）：

8.2 逆动力学（Inverse Dynamics）基础与约束处理

8.2.1 纯逆动力学问题（无约束）

8.2.2 约束逆动力学：带接触与约束的情形（Lagrange multiplier）

8.3 控制分配（Control Allocation）与力矩/摩擦/限位处理

控制分配问题出现在多驱动器冗余（actuator redundancy）或在受到力矩限位与摩擦影响时，需要把期望的 task-space 力/加速度映射到实际可实现的关节力矩/驱动器输入上，满足驱动器饱和、摩擦、和优先级。

8.3.1 最小二乘分配（无约束）

8.3.2 有限力矩/饱和约束（QP 表述）

8.3.4 接触不确定性与摩擦锥（锥体约束）

8.4 操作空间控制（Operational Space Control, OSC）——理论推导与稳定性证明

操作空间控制由 Khatib 提出，目标是直接在任务（操作）空间上指定動力學/控制律，使末端（或一般任务）呈现期望动力行为，同时正确考虑机器人动力学耦合。

8.4.1 任务空间动力学的推导

8.4.2 动力学一致的逆（dynamically consistent inverse）与零动力投影

8.4.3 OSC 控制律与稳定性证明（基本形式）

8.4.4 Nullspace 控制与优先级任务

8.5 力/位置混合控制（Hybrid Position/Force Control）与接触场景

混合控制用于在存在约束/接触的方向上控制力，在自由方向控制位置。例如在平面表面上沿法向控制力，在切向控制位置轨迹。

8.5.1 线性分解与任务空间分解

8.5.2 力控制的实现与接触刚度

8.6 数值与稳定性问题：雅可比逆、伪逆、和阻尼正则化

雅可比及其伪逆在许多控制步骤中是核心（IK、逆动力学映射、nullspace 投影）。但在奇异或接近奇异处会数值不稳定。

8.6.1 Moore–Penrose 伪逆与最小范数解

8.7 约束与接触的整数互补/时间步进——LCP 与时间步算法

接触建模有两条常见途径：连续接触力的反演（使用摩擦锥）或时间步进法（每个时间步通过 LCP / QP 求解接触冲量与速度）。这里给出常见的时间步离散化（Moreau / Stewart & Trinkle）并导出 LCP。

8.7.1 非光滑动力学与冲量法（离散时间）

8.7.2 数值/稳定性注意

• LCP/时间步求解在接触广泛时规模大（约束个数多），需稀疏求解器与预条件；

• 时间步法对步长依赖显著：较大步长会导致较差的近似且可能产生穿透，步长选取与求解器精度权衡；

• 线性化/凸化摩擦锥能把问题转化到可解 QP（多边形化）或 SOCP，便于使用高效的二次规划求解器（OSQP, ECOS）。

8.8 数值实现要点、算法伪代码与工程建议

下面汇总实现这些控制/优化方法时的具体工程建议与伪代码骨架。

8.8.1 计算惯量与动力学量（高效算法）

8.8.2 伪代码：OSC 控制律（单任务，理想补偿）

Given q, qdot, desired x_d, xdot_d, xddot_d
1. Compute x, xdot, J(q), dotJdotq
2. Compute M, b = C(q,qdot) qdot + g(q)
3. Compute Lambda = inv(J * inv(M) * J^T)      # solve A = J M^{-1} J^T for A^{-1}
4. Compute mu = Lambda * (J * M^{-1} * b) - Lambda * dotJdotq
5. Choose a_d = xddot_d + Kp*(x_d - x) + Kd*(xdot_d - xdot)
6. Compute F_ctrl = Lambda * a_d + mu
7. tau_task = J^T * F_ctrl
8. Choose tau0 for nullspace (e.g., posture)
9. tau = tau_task + (I - J^# J)^T * tau0
10. Send tau to low-level actuator controller (with torque limits)

8.8.3 伪代码：约束逆动力学（KKT 解）

Given q, qdot, desired joint accel qdd_des (or desired task accel)
1. Compute M, b
2. Compute Jc, dotJc_qdot, desired a_c
3. Build A = Jc * inv(M) * Jc^T
4. Solve for lambda: A lambda = Jc * inv(M)*(tau - b) + dotJc_qdot - a_c
   (if tau unknown and qdd desired, rearrange accordingly)
5. Compute tau = M * qdd_des + b - Jc^T * lambda
6. Apply torque limits / saturate / use QP if needed

8.8.4 在存在饱和/优先级时使用 QP 控制分配

8.9 小结：理论、稳定性与实用原则

1. 分析层面：OSC 在理想模型补偿情形下可把高维关节耦合转换为等价的任务空间单体控制问题，利用任务惯量 Λ\LambdaΛ 能设计自然的 PD 控制并证明稳定性（Lyapunov）。Nullspace 投影允许优先级任务的层序化实现。

2. 约束/接触：用 Lagrange multiplier/KKT 形式化受约束动力学并通过 Schur 补或 KKT 系统消元求解乘子/驱动力。接触/摩擦引入非线性与互补条件，常用 LCP / SOCP / QP 进行时间步或连续处理。

3. 数值注意：关键在于避免显式矩阵逆、使用 Cholesky/稀疏求解、对小奇异值用阻尼正则化、在接触切换与约束奇异时用正则化/哈希/active-set 切换策略。

4. 实现建议：把复杂/昂贵计算（如 SVD、KKT 因子化）放在较低频率或用 warm-start；控制回路中采用鲁棒补偿（阻尼、力/位置滤波）保证闭环稳定；对摩擦/死区采用识别/观测器策略而不是硬补偿。