底层控制与动力学

本文覆盖具身智能系统中从关节空间到任务空间的完整控制链路。内容包括运动学建模、刚体动力学、经典与现代控制方法、全身控制优化、实时计算约束、接触力学、以及步态控制。每一节给出数学推导、工程实现细节与开源工具链。

---

1. 运动学基础 (Kinematics Fundamentals)

1.1 刚体变换与 SE(3)

三维空间中刚体的位姿用特殊欧几里得群 SE(3) 表示。SE(3) 是 SO(3) 与 $\mathbb{R}^3$ 的半直积：

$$ SE(3) = \left{ T = \begin{bmatrix} R & p \ 0 & 1 \end{bmatrix} ;\middle|; R \in SO(3),; p \in \mathbb{R}^3 \right} $$

其中 $R$ 为 $3 \times 3$ 旋转矩阵（满足 $R^TR = I$，$\det(R) = 1$），$p$ 为平移向量。齐次变换矩阵 (Homogeneous Transformation Matrix) $T \in \mathbb{R}^{4\times4}$ 将点 $q$ 从坐标系 B 变换到坐标系 A：

$$ {}^A q = T^A_B \cdot {}^B q $$

SE(3) 的李代数 $\mathfrak{se}(3)$ 为：

$$ \mathfrak{se}(3) = \left{ \hat{\xi} = \begin{bmatrix} [\omega]_\times & v \ 0 & 0 \end{bmatrix} ;\middle|; \omega, v \in \mathbb{R}^3 \right} $$

其中 $[\omega]_\times$ 为反对称矩阵（$\omega$ 的叉积矩阵）：

$$ [\omega]_\times = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \ \omega_3 & 0 & -\omega_1 \ -\omega_2 & \omega_1 & 0 \end{bmatrix} $$

指数映射 $\exp: \mathfrak{se}(3) \to SE(3)$ 将旋量 (twist) 映射为刚体变换。对数映射 $\log: SE(3) \to \mathfrak{se}(3)$ 提供逆操作，在 IK 误差计算中使用。

相邻连杆之间的变换可通过连乘获得末端执行器 (end-effector) 相对于基座标系的位姿：

$$ T^0_n = T^0_1 \cdot T^1_2 \cdots T^{n-1}_n $$

1.2 DH 参数 (Denavit-Hartenberg Parameters)

Denavit-Hartenberg 约定使用四个参数描述相邻两个连杆坐标系之间的关系：

参数	符号	含义
连杆长度	$a_i$	沿 $x_i$ 轴从 $z_{i-1}$ 到 $z_i$ 的距离
连杆扭转角	$\alpha_i$	绕 $x_i$ 轴从 $z_{i-1}$ 到 $z_i$ 的旋转角
连杆偏移	$d_i$	沿 $z_{i-1}$ 轴从 $x_{i-1}$ 到 $x_i$ 的距离
关节角	$\theta_i$	绕 $z_{i-1}$ 轴从 $x_{i-1}$ 到 $x_i$ 的旋转角

对于旋转关节 (revolute joint)，$\theta_i$ 为关节变量；对于移动关节 (prismatic joint)，$d_i$ 为关节变量。

单个连杆变换矩阵：

$$ T^{i-1}_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i \cos\alpha_i & \sin\theta_i \sin\alpha_i & a_i \cos\theta_i \ \sin\theta_i & \cos\theta_i \cos\alpha_i & -\cos\theta_i \sin\alpha_i & a_i \sin\theta_i \ 0 & \sin\alpha_i & \cos\alpha_i & d_i \ 0 & 0 & 0 & 1 \end{bmatrix} $$

示例：3-DOF 平面机械臂 DH 表

关节 $i$	$a_i$	$\alpha_i$	$d_i$	$\theta_i$
1	$L_1$	0	0	$\theta_1$
2	$L_2$	0	0	$\theta_2$
3	$L_3$	0	0	$\theta_3$

DH 参数法存在两种约定（Standard DH 和 Modified DH），差别在于坐标系安放方式。Craig 教科书使用 Modified DH（坐标系固定在连杆近端），Siciliano 教科书使用 Standard DH（坐标系固定在连杆远端）。实际使用时需确认所用约定。

1.3 正运动学 (Forward Kinematics, FK)

给定关节变量 $q = [\theta_1, \theta_2, \ldots, \theta_n]^T$，正运动学计算末端执行器位姿 $T^0_n(q)$：

$$ T^0_n(q) = \prod_{i=1}^{n} T^{i-1}_i(\theta_i) $$

计算复杂度为 $O(n)$（$n$ 次 $4\times4$ 矩阵乘法）。

Product of Exponentials (PoE) 公式是 DH 的替代方法，直接使用螺旋轴 (screw axis) $\mathcal{S}_i \in \mathbb{R}^6$ 表示：

$$ T^0_n(q) = e^{[\mathcal{S}_1]\theta_1} \cdot e^{[\mathcal{S}_2]\theta_2} \cdots e^{[\mathcal{S}_n]\theta_n} \cdot M $$

其中 $M$ 为零位 ($q = 0$) 时的末端位姿，$[\mathcal{S}_i] \in \mathfrak{se}(3)$ 为螺旋轴的李代数表示。PoE 公式无需满足 DH 坐标系安放规则，在树形结构和闭链机构中更灵活。

1.4 逆运动学 (Inverse Kinematics, IK)

逆运动学求解：给定目标位姿 $T_{\text{goal}}$，求关节角 $q$ 使得 $T^0_n(q) = T_{\text{goal}}$。

1.4.1 解析解 (Analytical/Closed-form Solutions)

适用于具有特定几何结构的机器人（如球形腕、平面机构）。满足 Pieper 条件（末三轴交于一点）的 6-DOF 机器人可将位置和姿态解耦：

1. 利用腕心位置求解前三个关节角（位置逆解，几何方法）
2. 利用腕部姿态求解后三个关节角（姿态逆解，Euler 角分解或 $ZYZ$ 分解）

6-DOF 球形腕机器人最多有 8 组解析解。考虑关节限位后，有效解通常为 2~4 组。

工具：IKFast（OpenRAVE 的解析 IK 生成器）可为给定机器人 URDF 自动生成 C++ 解析 IK 代码。生成后的代码执行时间 < 1 μs，适合实时控制。

1.4.2 数值解 (Numerical Solutions)

基于迭代优化求解，适用于任意构型：

$$ q_{k+1} = q_k + \Delta q $$

Jacobian 伪逆法：

$$ \Delta q = J^{\dagger}(q_k) \cdot e_k $$

$e_k \in \mathbb{R}^6$ 为当前末端位姿与目标之间的误差（可通过 $\log(T_{\text{goal}} \cdot T^{-1}_{\text{current}})$ 计算），$J^{\dagger} = J^T(JJ^T)^{-1}$ 为 Moore-Penrose 伪逆。

阻尼最小二乘法 (Damped Least Squares / Levenberg-Marquardt)：

$$ \Delta q = J^T(JJ^T + \lambda^2 I)^{-1} \cdot e_k $$

阻尼因子 $\lambda$ 在接近奇异构型时增大，保证数值稳定性。可操作度 $w$ 小于阈值时增大 $\lambda$：

$$ \lambda = \begin{cases} 0 & \text{if } w \geq w_0 \ \lambda_{\max}(1 - w/w_0)^2 & \text{if } w < w_0 \end{cases} $$

TRAC-IK 算法并行运行两个求解器：

1. 带随机重启的 KDL（基于 Newton-Raphson）
2. 带关节限位约束的 SQP (Sequential Quadratic Programming)

两个求解器并行启动，先收敛者返回结果。SQP 的优化目标：

$$ \min_{q} |q - q_{\text{seed}}|^2 \quad \text{s.t.} \quad FK(q) = T_{\text{goal}},; q_{\min} \leq q \leq q_{\max} $$

在 1000 次随机测试中，TRAC-IK 成功率典型值为 99.5%，KDL 约为 60~70%。

IK 求解器对比表：

求解器	方法	典型耗时	成功率	特点
IKFast	解析（代码生成）	< 1 μs	100%（在工作空间内）	离线生成，仅支持特定结构
TRAC-IK	伪逆 + SQP 双线程	1~5 ms	>99%	鲁棒，通用
KDL	Newton-Raphson	5~20 ms	60~70%	ROS 默认，可能陷入局部最优
Pinocchio	Levenberg-Marquardt	1~10 ms	95%+	现代 C++，支持约束

1.5 雅可比矩阵 (Jacobian Matrix)

1.5.1 定义与推导

几何雅可比 (Geometric Jacobian) 将关节速度映射到末端执行器的线速度和角速度：

$$ \begin{bmatrix} v \ \omega \end{bmatrix} = J(q) \dot{q} $$

其中 $v \in \mathbb{R}^3$ 为线速度，$\omega \in \mathbb{R}^3$ 为角速度，$J(q) \in \mathbb{R}^{6 \times n}$。

对于旋转关节 $i$，雅可比矩阵的第 $i$ 列为：

$$ J_i = \begin{bmatrix} z_{i-1} \times (o_n - o_{i-1}) \ z_{i-1} \end{bmatrix} $$

对于移动关节 $i$：

$$ J_i = \begin{bmatrix} z_{i-1} \ 0 \end{bmatrix} $$

其中 $z_{i-1}$ 为关节 $i$ 的旋转轴方向（在世界坐标系中），$o_{i-1}$ 为关节 $i$ 坐标系原点的位置，$o_n$ 为末端执行器位置。

雅可比矩阵的另一重要用途是力映射（对偶关系）：

$$ \tau = J^T(q) F $$

末端施加的力/力矩 $F \in \mathbb{R}^6$ 等价于关节力矩 $\tau \in \mathbb{R}^n$。

1.5.2 奇异性分析 (Singularity Analysis)

当 $\text{rank}(J(q)) < \min(6, n)$ 时，机器人处于奇异位形 (singular configuration)。此时：

1. 某些任务空间方向上的运动不可实现
2. 某些方向上需要无穷大的关节速度
3. 末端产生的力在某些方向上无法控制

可操作度 (Manipulability)：

$$ w(q) = \sqrt{\det(J(q)J(q)^T)} $$

当 $w = 0$ 时为奇异位形。可操作度椭球 (manipulability ellipsoid) 由 $JJ^T$ 的特征分解确定，其半轴长度为特征值的平方根，半轴方向为特征向量。椭球越接近球形，运动各向同性越好。

条件数 (Condition Number)：

$$ \kappa(J) = \frac{\sigma_{\max}}{\sigma_{\min}} $$

$\sigma_{\max}$、$\sigma_{\min}$ 为雅可比矩阵的最大、最小奇异值。$\kappa \to \infty$ 表示接近奇异。$\kappa = 1$ 为各向同性位形 (isotropic configuration)。

1.5.3 奇异类型

类型	条件	示例
边界奇异 (Boundary)	臂完全伸展或折叠	肘关节 $\theta_2 = 0$ 或 $\pi$
内部奇异 (Internal)	两个旋转轴对齐	球形腕的万向节锁 (Gimbal Lock)
结构奇异 (Structural)	由运动链拓扑决定	冗余机器人的自运动

工程处理：奇异性回避策略包括阻尼最小二乘法、任务优先级切换、轨迹重规划。在 WBC 中，通常通过增大正则化项来处理接近奇异的情况。

1.6 工作空间分析 (Workspace Analysis)

可达工作空间 (Reachable Workspace)：末端至少以一种姿态可达的所有位置。 灵巧工作空间 (Dexterous Workspace)：末端以任意姿态可达的所有位置。

对于 $n$-DOF 机器人：

• $n < 6$：任务空间受约束，机器人在 6D 空间中运动受限
• $n = 6$：恰好满足 6D 位姿，通常无冗余
• $n > 6$：冗余自由度，存在自运动 (self-motion)

冗余机器人的零空间 (null space) 投影：

$$ \dot{q} = J^{\dagger} v_{\text{task}} + (I - J^{\dagger}J)\dot{q}_0 $$

$(I - J^{\dagger}J)$ 将 $\dot{q}_0$ 投影到雅可比的零空间，可用于次要任务（如避障、关节限位优化、操作度最大化）而不影响主任务。

工作空间的数值计算方法：

1. 蒙特卡洛采样：在关节空间均匀采样，通过 FK 映射到任务空间
2. 边界追踪：对 $\det(JJ^T) = 0$ 的曲面进行追踪
3. 解析方法：对简单构型（平面臂、球坐标构型）直接推导边界方程

---

2. 刚体动力学 (Rigid Body Dynamics)

2.1 运动方程的一般形式

$n$ 自由度机器人的动力学方程：

$$ M(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) = \tau + J^T(q) F_{\text{ext}} $$

符号	含义	维度
$q$	广义坐标（关节角）	$n \times 1$
$M(q)$	质量矩阵（对称正定）	$n \times n$
$C(q, \dot{q})$	科里奥利力与离心力矩阵	$n \times n$
$G(q)$	重力项	$n \times 1$
$\tau$	关节驱动力矩	$n \times 1$
$F_{\text{ext}}$	末端外力	$6 \times 1$

$M(q)$ 的性质：

• 对称：$M = M^T$
• 正定：$\forall x \neq 0,; x^T M x > 0$
• 有界：$\lambda_{\min}(M) |x|^2 \leq x^T M x \leq \lambda_{\max}(M) |x|^2$

$C(q, \dot{q})$ 的重要性质：$\dot{M} - 2C$ 为反对称矩阵（当 $C$ 使用 Christoffel 符号构造时）。这一性质在被动性 (passivity) 分析和自适应控制中起关键作用。

完整动力学还可能包含：

• 关节摩擦：$f(\dot{q}) = f_v \dot{q} + f_c \text{sgn}(\dot{q})$（粘性 + 库仑）
• 执行器动力学：电机转子惯量、减速器柔性
• 弹性关节：$\tau_{\text{spring}} = K_s(q_m / N - q_l)$

2.2 Lagrange-Euler 方法

基于系统总动能 $K$ 和总势能 $P$ 构造拉格朗日量 $L = K - P$：

$$ \frac{d}{dt}\frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = \tau_i, \quad i = 1, \ldots, n $$

动能计算：

$$ K = \frac{1}{2} \sum_{i=1}^{n} \left[ m_i \dot{p}{c_i}^T \dot{p}{c_i} + \omega_i^T {}^i I_i \omega_i \right] $$

其中 $m_i$ 为连杆 $i$ 质量，$p_{c_i}$ 为质心位置，${}^i I_i$ 为在连杆坐标系中表示的惯性张量，$\omega_i$ 为连杆角速度。

将动能写成二次型：

$$ K = \frac{1}{2}\dot{q}^T M(q) \dot{q} $$

质量矩阵的元素：

$$ M_{ij}(q) = \sum_{k=\max(i,j)}^{n} \left[ m_k J_{v_k,i}^T J_{v_k,j} + J_{\omega_k,i}^T {}^0 I_k J_{\omega_k,j} \right] $$

势能计算：

$$ P = -\sum_{i=1}^{n} m_i g^T p_{c_i} $$

科里奥利矩阵通过 Christoffel 符号计算：

$$ C_{ij}(q, \dot{q}) = \sum_{k=1}^{n} c_{ijk} \dot{q}k, \quad c{ijk} = \frac{1}{2}\left(\frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} - \frac{\partial M_{jk}}{\partial q_i}\right) $$

Lagrange-Euler 方法适合符号推导和教学，但展开所有 Christoffel 符号后的计算复杂度为 $O(n^4)$，不适合实时计算。

2.3 Newton-Euler 递推算法 (RNEA)

Newton-Euler 算法的计算复杂度为 $O(n)$，是实时逆动力学计算的标准方法。

前向递推 (Outward Iteration)：从基座到末端，逐连杆计算速度和加速度。

初始化: ω_0 = 0, α_0 = 0, a_0 = -g (将重力吸收到基座加速度中)

for i = 1 to n:
    # 角速度传递
    ω_i = R_{i}^{i-1} * ω_{i-1} + dq_i * z_i
    
    # 角加速度传递
    α_i = R_{i}^{i-1} * α_{i-1} + ddq_i * z_i 
          + dq_i * (R_{i}^{i-1} * ω_{i-1}) × z_i
    
    # 连杆原点线加速度
    a_i = R_{i}^{i-1} * a_{i-1} + α_i × r_{i-1,i} 
          + ω_i × (ω_i × r_{i-1,i})
    
    # 质心线加速度
    a_{ci} = a_i + α_i × r_{i,ci} + ω_i × (ω_i × r_{i,ci})

后向递推 (Inward Iteration)：从末端到基座，逐连杆计算力和力矩。

初始化: f_{n+1} = 0, n_{n+1} = 0 (或等于外力)

for i = n to 1:
    # 连杆净力
    F_i = m_i * a_{ci}
    
    # 连杆净力矩
    N_i = I_i * α_i + ω_i × (I_i * ω_i)
    
    # 从子连杆传递来的力
    f_i = R_{i}^{i+1} * f_{i+1} + F_i
    
    # 从子连杆传递来的力矩
    n_i = R_{i}^{i+1} * n_{i+1} + N_i 
          + r_{i,ci} × F_i 
          + r_{i,i+1} × (R_{i}^{i+1} * f_{i+1})
    
    # 提取关节力矩
    τ_i = n_i^T * z_i        # 旋转关节
    # τ_i = f_i^T * z_i      # 移动关节

RNEA 的总计算量约为 $150n$ 次乘法和 $131n$ 次加法（对旋转关节）。

2.4 正动力学 vs 逆动力学

	正动力学 (Forward Dynamics)	逆动力学 (Inverse Dynamics)
输入	$\tau, q, \dot{q}$	$q, \dot{q}, \ddot{q}$
输出	$\ddot{q}$	$\tau$
公式	$\ddot{q} = M^{-1}(\tau - C\dot{q} - G)$	$\tau = M\ddot{q} + C\dot{q} + G$
用途	物理仿真	前馈控制（计算力矩法）、力矩估计
标准算法	ABA ($O(n)$) 或 CRBA+Cholesky ($O(n^3)$)	RNEA ($O(n)$)

CRBA (Composite Rigid Body Algorithm)：先计算 $M(q)$，再通过 Cholesky 分解求 $\ddot{q} = M^{-1}(\tau - h)$。复杂度 $O(n^2)$ 计算 $M$，$O(n^3)$ 做 Cholesky。

ABA (Articulated Body Algorithm)：Featherstone 提出的 $O(n)$ 正动力学算法，使用 “articulated body inertia” 概念避免显式构造 $M$。三遍递推：

1. 前向递推：计算速度、偏置力
2. 后向递推：计算 articulated body inertia
3. 前向递推：计算加速度

ABA 是 MuJoCo、Drake 等物理引擎中正动力学计算的核心。

2.5 模型表示格式

URDF (Unified Robot Description Format)：ROS 生态的标准格式。

<robot name="7dof_arm">
  <link name="link3">
    <inertial>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
      <mass value="3.5"/>
      <inertia ixx="0.035" ixy="0" ixz="0" 
               iyy="0.035" iyz="0" izz="0.005"/>
    </inertial>
    <visual>
      <geometry><mesh filename="package://robot/meshes/link3.stl"/></geometry>
    </visual>
    <collision>
      <geometry><cylinder length="0.3" radius="0.04"/></geometry>
    </collision>
  </link>
  
  <joint name="joint3" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 0.3" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.96" upper="2.96" effort="87" velocity="2.175"/>
    <dynamics damping="0.3" friction="0.1"/>
  </joint>
</robot>

URDF 局限性：

• 仅支持树状结构（无闭链 closed-loop）
• 不支持 tendon/pulley 传动
• 不支持复杂执行器模型

SDF (Simulation Description Format)：Gazebo 使用，支持多机器人、闭链、世界环境。

MJCF (MuJoCo XML)：MuJoCo 专用，支持 tendon、site、actuator model（位置/速度/力矩/肌肉）、接触参数的精细定义。

2.6 动力学库

库	语言	核心算法	特点	适用场景
Pinocchio	C++/Python	RNEA, ABA, CRBA	解析导数、SE(3) 操作、代码生成	控制器开发、MPC
RBDL	C++	RNEA, ABA, CRBA	轻量、Featherstone spatial algebra	嵌入式实时控制
Drake	C++/Python	MultibodyPlant	多体仿真 + 优化 + 控制一体化	研究、规划
MuJoCo	C	ABA + 凸接触	接触仿真极快、GPU/并行支持	RL 训练、大规模仿真
Bullet	C++	Sequential Impulse	开源、游戏级速度	快速原型

Pinocchio 代码示例（常用操作）：

import pinocchio as pin
import numpy as np

# 加载 URDF 模型
model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()

q = pin.neutral(model)            # 中性位形
dq = np.zeros(model.nv)           # 关节速度
ddq = np.zeros(model.nv)          # 关节加速度

# 逆动力学 (RNEA)
tau = pin.rnea(model, data, q, dq, ddq)

# 正动力学 (ABA)
ddq_result = pin.aba(model, data, q, dq, tau)

# 质量矩阵 (CRBA)
M = pin.crba(model, data, q)

# 非线性项 (Coriolis + Gravity)
h = pin.nle(model, data, q, dq)

# 正运动学
pin.forwardKinematics(model, data, q)
frame_id = model.getFrameId("end_effector")
pin.updateFramePlacement(model, data, frame_id)
ee_pose = data.oMf[frame_id]  # SE(3)

# Frame Jacobian（世界坐标系表示）
J = pin.computeFrameJacobian(model, data, q, frame_id,
                              pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)

# RNEA 的解析导数（用于 MPC/优化）
pin.computeRNEADerivatives(model, data, q, dq, ddq)
dtau_dq = data.dtau_dq      # n x n
dtau_dv = data.dtau_dv      # n x n
dtau_da = data.M            # = M(q)

---

3. PID 控制 (PID Control)

3.1 基本形式

PID 控制器的连续时间形式：

$$ u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt} $$

离散时间形式（采样周期 $T_s$）：

$$ u[k] = K_p e[k] + K_i T_s \sum_{j=0}^{k} e[j] + K_d \frac{e[k] - e[k-1]}{T_s} $$

其中 $e[k] = q_{\text{desired}}[k] - q_{\text{actual}}[k]$。

增量式 PID（避免积分累加的数值问题）：

$$ \Delta u[k] = K_p(e[k] - e[k-1]) + K_i T_s \cdot e[k] + K_d \frac{e[k] - 2e[k-1] + e[k-2]}{T_s} $$

3.2 级联控制架构 (Cascade Control Architecture)

工业伺服驱动器采用三环嵌套结构：

┌─────────────────────────────────────────────────────────────────────────┐
│                                                                         │
│  位置环 (Position Loop)                                                  │
│  频率: 100~500 Hz                                                       │
│  控制器: P 或 PD                                                         │
│  输入: 目标位置 q_d        输出: 目标速度 dq_d                             │
│  ┌───────────────────────────────────────────────────────────────────┐  │
│  │  速度环 (Velocity Loop)                                            │  │
│  │  频率: 1~5 kHz                                                     │  │
│  │  控制器: PI                                                         │  │
│  │  输入: 目标速度 dq_d      输出: 目标电流 i_q_d                        │  │
│  │  ┌─────────────────────────────────────────────────────────────┐  │  │
│  │  │  电流环 (Current Loop / FOC)                                  │  │  │
│  │  │  频率: 10~40 kHz                                              │  │  │
│  │  │  控制器: PI (d/q 轴分别)                                       │  │  │
│  │  │  输入: 目标电流 i_q_d    输出: PWM 占空比 (三相)                  │  │  │
│  │  │  坐标变换: abc → αβ (Clarke) → dq (Park)                      │  │  │
│  │  └─────────────────────────────────────────────────────────────┘  │  │
│  └───────────────────────────────────────────────────────────────────┘  │
└─────────────────────────────────────────────────────────────────────────┘

各环的设计原则：内环带宽必须为外环的 5~10 倍，确保动态解耦。

控制环	典型频率	控制器	传感器	带宽目标
电流环	10~40 kHz	PI	电流传感器（Hall/Shunt）	1~3 kHz
速度环	1~5 kHz	PI	编码器差分/观测器	100~500 Hz
位置环	100~500 Hz	P 或 PD	编码器绝对值	10~50 Hz

FOC (Field-Oriented Control) 电流环的坐标变换：

Clarke 变换 ($abc \to \alpha\beta$)：

$$ \begin{bmatrix} i_\alpha \ i_\beta \end{bmatrix} = \frac{2}{3}\begin{bmatrix} 1 & -1/2 & -1/2 \ 0 & \sqrt{3}/2 & -\sqrt{3}/2 \end{bmatrix}\begin{bmatrix} i_a \ i_b \ i_c \end{bmatrix} $$

Park 变换 ($\alpha\beta \to dq$)：

$$ \begin{bmatrix} i_d \ i_q \end{bmatrix} = \begin{bmatrix} \cos\theta_e & \sin\theta_e \ -\sin\theta_e & \cos\theta_e \end{bmatrix}\begin{bmatrix} i_\alpha \ i_\beta \end{bmatrix} $$

在 $dq$ 坐标系中，$i_d$ 控制磁场（通常设为0），$i_q$ 控制力矩。力矩 $\tau = \frac{3}{2} p \lambda_m i_q$（$p$ 为极对数，$\lambda_m$ 为永磁体磁链）。

3.3 调参方法

Ziegler-Nichols 临界振荡法

1. 将 $K_i = 0$, $K_d = 0$
2. 增大 $K_p$ 直到系统出现等幅持续振荡
3. 记录临界增益 $K_u$ 和振荡周期 $T_u$
4. 按表设定参数：

控制器	$K_p$	$T_i = K_p/K_i$	$T_d = K_d/K_p$
P	$0.5 K_u$	$\infty$	0
PI	$0.45 K_u$	$T_u / 1.2$	0
PID	$0.6 K_u$	$T_u / 2$	$T_u / 8$

Ziegler-Nichols 给出的参数通常偏激进（超调约 25%），实际需根据响应微调。

手动调参经验法则

1. 电流环优先：确保电流跟踪准确（带宽 > 1 kHz），阶跃响应无超调
2. 速度环次之：从小 $K_p$ 开始，逐步增大直到轻微振荡，回退 20~30%。加 $K_i$ 消除稳态误差
3. 位置环最后：通常纯 P 控制就足够（速度环已保证速度精度），$K_p$ 决定位置环带宽
4. 前馈：加入速度前馈 $v_{ff} = \dot{q}d$ 和加速度前馈 $\tau{ff} = M\ddot{q}_d$ 可显著提高轨迹跟踪性能而不影响稳定性

3.4 抗积分饱和 (Anti-windup)

当执行器输出达到限幅（如电流达到最大值）时，积分项持续累积，导致解除饱和后出现严重超调和长整定时间。

条件积分法 (Conditional Integration)：

// 当输出未饱和时才积分
if (fabs(output_before_clamp) <= output_max) {
    integral += error * Ts;
} 
// 否则冻结积分

反馈抑制法 (Back-calculation)：

$$ \frac{d}{dt} e_I = e + \frac{1}{T_t}(u_{\text{sat}} - u) $$

$T_t$ 为跟踪时间常数（通常取 $T_t = \sqrt{T_i T_d}$ 或 $T_t = T_d$），$u_{\text{sat}}$ 为限幅后输出，$u$ 为限幅前输出。当输出饱和时，$(u_{\text{sat}} - u)$ 为负值，抑制积分增长。

钳位法 (Clamping)：限制积分项的范围 $|e_I| \leq e_{I,\max}$。简单但粗暴，可能在快速动态中产生不连续行为。

3.5 完整工程示例：单关节位置控制

目标：控制一个关节，电机侧惯量 $J_m = 5 \times 10^{-5};\text{kg}\cdot\text{m}^2$，减速比 $N = 100$，负载侧惯量 $J_L = 0.5;\text{kg}\cdot\text{m}^2$，粘性阻尼 $b = 0.1;\text{N}\cdot\text{m}\cdot\text{s/rad}$（负载侧）。电机力矩常数 $K_t = 0.1;\text{Nm/A}$，最大电流 $I_{\max} = 10;\text{A}$。

等效到负载侧的总惯量：$J_{\text{total}} = J_L + N^2 J_m = 0.5 + 10000 \times 5\times10^{-5} = 1.0;\text{kg}\cdot\text{m}^2$

（注：高减速比时电机惯量的等效贡献 $N^2 J_m$ 往往与负载惯量同量级甚至更大。）

负载侧传递函数：

$$ G(s) = \frac{1}{J_{\text{total}} s^2 + b s} = \frac{1}{s^2 + 0.1s} $$

位置环 PD 设计：选择闭环自然频率 $\omega_n = 30;\text{rad/s}$（约 5 Hz），阻尼比 $\zeta = 0.9$。

闭环特征方程：$J_{\text{total}} s^2 + (b + K_d)s + K_p = 0$

归一化为标准形式 $s^2 + 2\zeta\omega_n s + \omega_n^2 = 0$：

$$ K_p = J_{\text{total}} \cdot \omega_n^2 = 1.0 \times 900 = 900;\text{Nm/rad} $$

$$ K_d = 2\zeta\omega_n \cdot J_{\text{total}} - b = 2 \times 0.9 \times 30 \times 1.0 - 0.1 = 53.9;\text{Nm\cdot s/rad} $$

验证：

• 最大力矩需求：对于 1 rad 阶跃，初始力矩 $\tau = K_p \times 1 = 900;\text{Nm}$
• 电机侧力矩：$900/100 = 9;\text{Nm}$
• 所需电流：$9 / K_t = 90;\text{A}$，超过 $I_{\max}$

这说明需要限制指令斜率（使用梯形速度曲线或 S 曲线），或降低 $K_p$。

实际部署（带力矩限制）：

#define DT 0.001f  // 1 kHz

typedef struct {
    float Kp, Kd;
    float q_prev;
    float tau_max;
} JointPDController;

float joint_pd_control(JointPDController* c, float q_des, float q_act, 
                       float dq_des, float dq_act) {
    float e_pos = q_des - q_act;
    float e_vel = dq_des - dq_act;
    
    float tau = c->Kp * e_pos + c->Kd * e_vel;
    
    // 力矩限幅
    if (tau > c->tau_max) tau = c->tau_max;
    if (tau < -c->tau_max) tau = -c->tau_max;
    
    return tau;
}

---

4. 阻抗控制 (Impedance Control)

4.1 基本思想

阻抗控制的目的不是精确跟踪轨迹，而是规定末端执行器与环境交互时的动态关系。核心目标：让机器人末端表现为一个具有指定质量、阻尼和刚度的弹簧-阻尼-质量系统。

这在接触任务中尤为重要：纯位置控制在接触时会产生极大力（因为位置误差导致的力与环境刚度成正比）；纯力控制在自由空间中无法定位。阻抗控制在力和位置之间建立了可控的动态关系。

4.2 阻抗方程推导

目标阻抗（在任务空间，以 1-DOF 为例）：

$$ M_d(\ddot{x} - \ddot{x}_d) + D_d(\dot{x} - \dot{x}d) + K_d(x - x_d) = F{\text{ext}} $$

参数	含义	作用	典型范围
$M_d$	期望惯量	决定对外力的加速响应	1~10 kg
$D_d$	期望阻尼	耗散能量，抑制振荡	10~200 Ns/m
$K_d$	期望刚度	决定稳态偏差与外力的关系	50~5000 N/m
$x_d$	期望轨迹	由规划层给出
$F_{\text{ext}}$	外部接触力	由力传感器测量

在自由空间 ($F_{\text{ext}} = 0$) 中，阻抗方程退化为阻尼二阶系统追踪 $x_d$。接触发生时，末端偏离 $x_d$，偏离量取决于阻抗参数和外力大小。

6-DOF 情况下，$M_d, D_d, K_d$ 均为 $6\times6$ 矩阵（通常取对角阵）。

临界阻尼条件：

$$ D_d = 2\sqrt{M_d K_d} $$

4.3 阻抗控制 vs 导纳控制 (Impedance vs Admittance)

两种方法的因果关系相反：

阻抗控制 (Impedance Control)：

• 因果链：运动偏差 → 力输出
• 输入：$\Delta x = x - x_d$（位置/速度偏差，由编码器测量）
• 输出：$\tau$（关节力矩，直接输出到电机）
• 要求：关节具有力矩控制能力（低减速比、力矩传感器或电流控制精度高）

导纳控制 (Admittance Control)：

• 因果链：外力 → 运动修正
• 输入：$F_{\text{ext}}$（外力，由力/力矩传感器测量）
• 输出：$\Delta x$（位置修正量，叠加到位置指令上）
• 要求：末端力/力矩传感器 + 位置伺服环

选择逻辑：

硬件特性	推荐方案	原因
可反驱动，低减速比（如谐波减速 1:50 以下或直驱）	阻抗控制	力矩可直接精确输出，不受摩擦/回差影响
高减速比 (>100:1)，位置控制型	导纳控制	减速器摩擦使力矩透明度差，需通过力传感器检测外力
串联弹性执行器 (SEA)	阻抗控制	SEA 弹簧提供固有柔顺和力测量
有腕部力/力矩传感器	两者皆可	传感器提供外力信息，使导纳控制可行

4.4 阻抗控制的实现

关节空间阻抗控制（最常见的实现方式）：

$$ \tau = \underbrace{M(q)\ddot{q}d + C(q,\dot{q})\dot{q} + G(q)}{\text{动力学前馈}} + \underbrace{K_q(q_d - q) + D_q(\dot{q}d - \dot{q})}{\text{关节空间阻抗}} $$

前三项补偿非线性动力学（计算力矩法，computed torque），后两项实现期望阻抗行为。$K_q, D_q \in \mathbb{R}^{n\times n}$ 为关节空间刚度和阻尼矩阵。

任务空间阻抗控制（在笛卡尔空间定义阻抗）：

1. 计算任务空间误差：$e = x_d - x$，$\dot{e} = \dot{x}_d - \dot{x}$
2. 计算期望任务空间加速度：$\ddot{x}^* = \ddot{x}d + M_d^{-1}[D_d\dot{e} + K_d e - F{\text{ext}}]$
3. 转换为关节加速度：$\ddot{q}^* = J^{-1}(\ddot{x}^* - \dot{J}\dot{q})$
4. 计算力矩：$\tau = M(q)\ddot{q}^* + C(q,\dot{q})\dot{q} + G(q)$

或等价地（操作空间动力学框架，Khatib 1987）：

$$ \tau = J^T \Lambda(x)\ddot{x}^* + J^T \mu(x,\dot{x}) + G(q) + N^T \tau_0 $$

其中 $\Lambda = (JM^{-1}J^T)^{-1}$ 为操作空间惯性矩阵，$N = I - J^T\bar{J}^T$ 为零空间投影。

4.5 变阻抗控制 (Variable Impedance Control)

固定阻抗参数无法适应不同任务阶段。变阻抗控制在运行时动态调整 $K_d(t), D_d(t)$：

基于任务阶段的切换：

阶段	$K_d$ (N/m)	$D_d$ (Ns/m)	物理意义
自由空间接近	2000	50	快速精确到达目标
接近表面 (< 5mm)	500	100	减速，增加阻尼
建立接触	50	150	低刚度避免冲击
稳态接触操作	200	80	维持稳定力
脱离	1000	30	快速离开

基于学习的变阻抗：

• DMP (Dynamic Movement Primitives) + 变阻抗参数
• 强化学习直接输出 $K_d(t), D_d(t)$ 作为策略的一部分
• GMM/GMR 从人类示教中学习刚度变化曲线

4.6 应用示例

Peg-in-hole 装配：

坐标系定义: z 轴为插入方向, xy 为横向

z 方向 (插入方向): 
  K_z = 30 N/m,  D_z = 50 Ns/m
  → 极低刚度，允许接触力引导插入深度

x,y 方向 (横向对中):
  K_x = K_y = 1500 N/m,  D_x = D_y = 30 Ns/m
  → 高刚度保持对中精度
  
旋转 (rx, ry):
  K_rx = K_ry = 5 Nm/rad,  D_rx = D_ry = 1 Nm·s/rad
  → 低旋转刚度允许角度自适应对准

人机协作搬运：

默认 (机器人自主运动):
  K = 800 N/m,  D = 60 Ns/m
  → 精确跟踪轨迹

检测到人手接触 (|F_ext| > 3N):
  K = 0 N/m (纯阻尼),  D = 80 Ns/m
  → 零刚度模式，人可自由引导；阻尼保证运动平稳

人手离开 (|F_ext| < 1N 持续 0.5s):
  渐进恢复: K 从 0 线性增加到 800 (1s 内)
  → 平滑过渡回自主运动

---

5. 全身控制 (Whole-Body Control, WBC)

5.1 问题定义

对于具有浮动基座 (floating base) 的机器人（双足、四足、人形），系统具有 $6 + n$ 个广义坐标（6 个基座自由度 + $n$ 个关节），但只有 $n$ 个执行器。系统是欠驱动的 (underactuated)：基座的 6 自由度无法直接驱动，只能通过接触力间接控制。

全身动力学方程：

$$ \underbrace{\begin{bmatrix} M_{bb} & M_{bj} \ M_{jb} & M_{jj} \end{bmatrix}}M \underbrace{\begin{bmatrix} \ddot{q}b \ \ddot{q}j \end{bmatrix}}{\ddot{q}} + \underbrace{\begin{bmatrix} h_b \ h_j \end{bmatrix}}h = \underbrace{\begin{bmatrix} 0 \ \tau \end{bmatrix}}{S^T\tau} + \underbrace{\begin{bmatrix} J{c,b}^T \ J{c,j}^T \end{bmatrix}}_{J_c^T} F_c $$

其中 $q_b \in \mathbb{R}^6$ 为基座位姿（3 平移 + 3 旋转），$q_j \in \mathbb{R}^n$ 为关节角，$h = C\dot{q} + G$ 为非线性项。

上方 6 行（基座方程）无驱动项，约束了接触力 $F_c$ 与基座运动 $\ddot{q}_b$ 的关系。

5.2 任务层级 (Task Hierarchy)

WBC 同时处理多个目标，按优先级排列：

优先级	任务	约束类型	维度
0 (最高)	动力学一致性	等式	$6 + n$
1	接触约束（支撑脚不滑动）	等式	$6 n_c$
1	摩擦锥约束	不等式	$5 n_c$
2	质心位置/速度跟踪	目标	3
2	角动量调节	目标	3
3	摆动脚轨迹跟踪	目标	6
3	躯干姿态维持	目标	3
4	上肢末端跟踪	目标	6 per arm
5	关节限位回避	不等式	$2n$
6 (最低)	默认关节姿态 + 力矩正则化	目标	$n$

5.3 QP (Quadratic Programming) 公式

决策变量 $x = [\ddot{q}^T, \tau^T, F_c^T]^T \in \mathbb{R}^{(6+n) + n + 3n_c}$：

$$ \min_x \quad \frac{1}{2} x^T H x + g^T x $$

$$ \text{s.t.} \quad A_{\text{eq}} x = b_{\text{eq}} $$

$$ \quad\quad\quad A_{\text{ineq}} x \leq b_{\text{ineq}} $$

等式约束构建：

1. 动力学方程（以矩阵形式重写）：

$$ \begin{bmatrix} M & -S^T & -J_c^T \end{bmatrix} \begin{bmatrix} \ddot{q} \ \tau \ F_c \end{bmatrix} = -h $$

2. 接触点加速度为零（支撑脚不滑动）：

$$ J_c \ddot{q} + \dot{J}_c \dot{q} = 0 $$

即 $\begin{bmatrix} J_c & 0 & 0 \end{bmatrix} x = -\dot{J}_c \dot{q}$。

不等式约束构建：

3. 摩擦锥约束（线性化）：对每个接触点 $i$，设接触法向为 $z$，摩擦系数为 $\mu$：

$$ \begin{bmatrix} 1 & 0 & -\mu \ -1 & 0 & -\mu \ 0 & 1 & -\mu \ 0 & -1 & -\mu \ 0 & 0 & -1 \end{bmatrix} F_{c,i} \leq 0 $$

4. 关节力矩限制：$\tau_{\min} \leq \tau \leq \tau_{\max}$

5. 关节加速度限制（由角度和速度限制推导）：

$$ \frac{2(q_{\min} - q - \dot{q}\Delta t)}{\Delta t^2} \leq \ddot{q}j \leq \frac{2(q{\max} - q - \dot{q}\Delta t)}{\Delta t^2} $$

目标函数 $H, g$ 的构建：

对第 $k$ 个跟踪任务（$\ddot{x}_k^{\text{des}}$ 为期望加速度，含 PD 反馈）：

$$ \ddot{x}_k^{\text{des}} = \ddot{x}_k^{\text{ref}} + K_p^k(x_k^{\text{ref}} - x_k) + K_d^k(\dot{x}_k^{\text{ref}} - \dot{x}_k) $$

任务误差：$|J_k\ddot{q} + \dot{J}_k\dot{q} - \ddot{x}_k^{\text{des}}|^2$

展开后贡献到 $H$ 和 $g$：

$$ H_k = w_k \begin{bmatrix} J_k^T J_k & 0 & 0 \ 0 & 0 & 0 \ 0 & 0 & 0 \end{bmatrix}, \quad g_k = w_k \begin{bmatrix} J_k^T(\dot{J}_k\dot{q} - \ddot{x}_k^{\text{des}}) \ 0 \ 0 \end{bmatrix} $$

总目标：$H = \sum_k H_k + H_{\text{reg}}$，$g = \sum_k g_k$。正则化项 $H_{\text{reg}} = \text{diag}(\epsilon_{\ddot{q}}, \epsilon_\tau, \epsilon_F)$ 保证 $H$ 正定。

5.4 层级 QP (Hierarchical QP, HQP)

加权 QP 通过权重 $w_k$ 平衡任务，但无法严格保证高优先级任务不受低优先级影响。HQP 实现严格优先级：

算法：

Level 1: 求解
  min  ||A_1 x - b_1||^2
  s.t. inequality constraints
  → 得到最优值 v_1* = A_1 x_1*

Level 2: 求解  
  min  ||A_2 x - b_2||^2
  s.t. A_1 x = v_1*          (锁定 Level 1 的最优结果)
       inequality constraints
  → 得到最优值 v_2*

Level k: 求解
  min  ||A_k x - b_k||^2
  s.t. A_1 x = v_1*
       ...
       A_{k-1} x = v_{k-1}*
       inequality constraints

每层引入松弛变量 $w_k$：

$$ \min_{x, w_k} |w_k|^2 \quad \text{s.t.} \quad A_k x - b_k = w_k,; \text{前层约束} $$

如果高优先级任务约束冲突（如摩擦力不足以同时满足平衡和跟踪），HQP 会牺牲低优先级任务。

等价零空间形式：

$$ \ddot{q} = \underbrace{J_1^{\dagger}\ddot{x}1^{\text{des}}}{\text{Level 1}} + N_1\underbrace{(J_2 N_1)^{\dagger}(\ddot{x}_2^{\text{des}} - J_2 J_1^{\dagger}\ddot{x}1^{\text{des}})}{\text{Level 2}} + \ldots $$

$N_1 = I - J_1^{\dagger}J_1$ 为零空间投影。此形式与 HQP 在无不等式约束时等价。

5.5 双足行走中的 WBC

完整任务设定（以 20-DOF 人形下半身为例）：

决策变量：$\ddot{q} \in \mathbb{R}^{26}$（6基座+20关节），$\tau \in \mathbb{R}^{20}$，$F_c \in \mathbb{R}^{12}$（双脚各 6 维力/力矩，或各 4 个接触点 $\times$ 3 维力 = 24 维）。

约束规模：

• 等式约束：26（动力学）+ 12（接触加速度=0）= 38
• 不等式约束：5×4（摩擦锥，4接触点）+ 40（力矩限制）+ 52（加速度限制）= 112

总变量数：26 + 20 + 12 = 58。总约束数：38 等式 + 112 不等式 = 150。

对于 1 kHz 控制频率，此规模的 QP 典型求解时间为 0.1~0.5 ms（使用 qpOASES 或 ProxQP with warm-start）。

5.6 QP 求解器

求解器	算法类型	适合问题规模	Warm-start	典型时间
qpOASES	Active Set	小~中（< 200 变量）	极好	0.05~1 ms
OSQP	ADMM (first-order)	大规模稀疏	好	0.1~5 ms
ECOS	Interior Point	中（支持 SOCP）	差	0.5~10 ms
ProxQP	Primal-Dual Proximal	小~大	极好	0.03~0.3 ms
HiGHS	Simplex/IPM	大规模 LP/QP	一般	1~10 ms

qpOASES 使用 online active set strategy：在相邻时步间，活跃约束集通常只变化 12 个约束，warm-start 使得迭代次数从冷启动的 1050 次降低到 1~3 次。

OSQP 使用 ADMM 算法，支持代码生成（生成无外部依赖的 C 代码），适合嵌入式部署。精度略低于 active set 方法，但大规模稀疏问题中效率更高。

---

6. 实时计算 (Real-Time Computing)

6.1 硬实时要求

底层控制要求硬实时 (hard real-time)：每个控制周期必须在规定 deadline 前完成所有计算和通信。超时一次即可能导致系统不稳定、关节振荡或跌倒。

控制环	周期	计算预算	超时后果
电流环 / FOC	25~100 μs	< 周期 50%	电机过流/退磁/烧毁
力矩/速度环	200~1000 μs	< 周期 70%	关节振荡、阻抗控制失效
位置/WBC	1~10 ms	< 周期 80%	轨迹偏差、跌倒
规划/感知	10~100 ms	软实时	性能降低但不立即危险

抖动 (Jitter)：连续两个控制周期之间的时间偏差。对于 1 kHz 控制环，可接受抖动通常 < 50 μs。抖动过大等效于采样率不确定性，会降低控制器增益裕度。

6.2 实时操作系统 (RTOS)

系统	架构	最大抖动	典型应用	成本
RT-PREEMPT	Linux 内核 patch	10~50 μs	ROS2 实时节点、EtherCAT master	免费
Xenomai (Cobalt)	双内核 (co-kernel)	1~10 μs	高精度伺服、EtherCAT	免费
VxWorks	独立 RTOS	< 1 μs	航天、军工、安全关键	商业
QNX Neutrino	微内核 RTOS	< 5 μs	汽车 ADAS、医疗机器人	商业
FreeRTOS	嵌入式 RTOS (MCU)	硬件级	STM32 电流环	免费
Zephyr	嵌入式 RTOS (MCU)	硬件级	nRF 传感器融合	免费

RT-PREEMPT 关键配置：

# 内核启动参数
GRUB_CMDLINE_LINUX="isolcpus=2,3 nohz_full=2,3 rcu_nocbs=2,3"

# 控制线程设置 (C代码)
struct sched_param param;
param.sched_priority = 99;  // 最高实时优先级
sched_setscheduler(0, SCHED_FIFO, &param);

mlockall(MCL_CURRENT | MCL_FUTURE);  // 锁定所有页面到内存

// 绑定到隔离核心
cpu_set_t cpuset;
CPU_ZERO(&cpuset);
CPU_SET(2, &cpuset);
pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset);

Xenomai 双内核架构：

用户空间
├── Xenomai RT 任务 (POSIX skin / Alchemy API)
│     → 直接访问 Cobalt 实时核，不经过 Linux 内核
│     → 中断响应 < 10 μs
└── 普通 Linux 应用 (ROS2, logging, GUI)
      → 走标准 Linux 内核路径
      → 延迟不确定 (ms 级)

内核空间
├── Cobalt 核 (实时微内核)
│     → 拦截所有中断
│     → 实时调度器 (FIFO/RR)
│     → 无页错误、无内存分配
└── Linux 核 (通用内核)
      → 运行在 Cobalt 的低优先级空闲时间
      → 文件系统、网络、驱动

硬件: I-pipe / Dovetail 中断虚拟化层

6.3 EtherCAT 通信

EtherCAT (Ethernet for Control Automation Technology) 是确定性工业以太网协议。由 Beckhoff 于 2003 年推出，现已成为机器人伺服通信的事实标准。

核心特性：

参数	值
OSI 层	Data Link Layer (L2)，直接使用以太网帧
拓扑	逻辑环形（物理线形 daisy-chain）
最小周期时间	62.5 μs（理论，取决于从站数量）
典型周期时间	125 μs（8 从站），250 μs（20 从站）
通信抖动	< 1 μs
带宽	100 Mbps (Fast Ethernet)
最大从站数	65535
处理模式	Processing on the fly（飞行处理）

工作原理：

主站发送一个以太网帧:

Master ──[Frame]──> Slave 1 ──[Frame]──> Slave 2 ──> ... ──> Slave N
                     │ read/write          │ read/write
                     │ (hardware, ~330ns)   │ (hardware, ~330ns)
                     
帧到达线路末端后自动返回:
Master <──[Frame]── Slave 1 <──[Frame]── Slave 2 <── ... <── Slave N

每个从站包含专用 ASIC (EtherCAT Slave Controller, ESC)，在帧经过时硬件级读取属于自己的数据段并写入输出数据。主站只需等待一帧往返时间即可获得所有从站的数据。

PDO 映射示例（单关节伺服驱动器，CiA 402 协议）：

方向	数据	字节
TxPDO (从站→主站)	实际位置 (int32)	4
	实际速度 (int32)	4
	实际力矩 (int16)	2
	状态字 (uint16)	2
RxPDO (主站→从站)	目标力矩 (int16)	2
	控制字 (uint16)	2
	模式切换 (int8)	1

对于 20 关节的人形机器人：每帧约 20 × (12 + 5) = 340 bytes 有效载荷，加上以太网帧头和 EtherCAT 头约 60 bytes，总帧长 < 400 bytes。以 100 Mbps 传输约 32 μs，加上 20 × 1 μs 从站延迟 = 52 μs。在 125 μs 周期内完全可行。

开源 Master 库：

库	语言	特点	实时性
SOEM	C	轻量，跨平台，用户空间	需配合 RT-PREEMPT
IgH EtherCAT Master	C (内核模块)	高性能，与 Xenomai 深度集成	硬实时
EtherLab	C	IgH 的分支	硬实时

SOEM 代码框架：

#include "ethercat.h"
#include <time.h>

char IOmap[4096];
int wkc;  // Working Counter

void control_loop() {
    struct timespec next;
    clock_gettime(CLOCK_MONOTONIC, &next);
    
    while (running) {
        // 1. 接收上一周期从站数据
        wkc = ec_receive_processdata(EC_TIMEOUTRET);
        
        if (wkc >= expected_wkc) {
            // 2. 读取各关节编码器和力矩反馈
            for (int i = 0; i < N_JOINTS; i++) {
                joint_pos[i] = *(int32_t*)(ec_slave[i+1].inputs);
                joint_vel[i] = *(int32_t*)(ec_slave[i+1].inputs + 4);
                joint_tau[i] = *(int16_t*)(ec_slave[i+1].inputs + 8);
            }
            
            // 3. 执行控制算法 (WBC/impedance/PD)
            compute_control(joint_pos, joint_vel, tau_cmd);
            
            // 4. 写入力矩指令
            for (int i = 0; i < N_JOINTS; i++) {
                *(int16_t*)(ec_slave[i+1].outputs) = tau_cmd[i];
            }
        }
        
        // 5. 发送新一周期的主站数据
        ec_send_processdata();
        
        // 6. 精确等待到下一周期
        next.tv_nsec += CYCLE_NS;  // e.g., 1000000 for 1ms
        if (next.tv_nsec >= 1000000000) {
            next.tv_nsec -= 1000000000;
            next.tv_sec++;
        }
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
    }
}

6.4 FPGA 在电流环中的应用

电流环频率 1040 kHz（周期 25100 μs），需要在极短时间内完成 ADC 采样、坐标变换、PI 控制、SVPWM 计算。FPGA 提供确定性的并行计算能力：

功能模块	FPGA 实现延迟	MCU 实现延迟
ADC 触发 + 采样	同步，0 抖动	中断延迟 1~5 μs
Clarke + Park 变换	< 100 ns	1~2 μs
d/q 轴 PI 控制器	< 100 ns	0.5~1 μs
逆 Park + SVPWM	< 200 ns	1~2 μs
编码器正交解码	硬件，0 CPU 开销	中断或 DMA
总计 (ADC→PWM)	< 1 μs	5~10 μs

典型硬件方案：

• Xilinx Zynq (ARM Cortex-A9 + FPGA)：电流环在 FPGA 逻辑中实现，速度环/位置环在 ARM 核运行
• Intel Cyclone V SoC (ARM Cortex-A9 + FPGA)：类似架构
• TI C2000 + FPGA：DSP 做电流环 + FPGA 做编码器和通信

FPGA 电流环数据流：

                   ┌─────────────────────────────────┐
  3相逆变器        │          FPGA 逻辑              │
  ┌───────┐       │                                 │
  │ U V W │◄──────│── SVPWM 生成 ◄── 逆Park ◄──┐   │
  └───┬───┘       │                            │   │
      │           │                        ┌───┴─┐ │
  ┌───▼───┐       │                        │PI_d │ │
  │ Motor │       │                        │PI_q │ │
  └───┬───┘       │                        └───┬─┘ │
      │           │                            │   │
  ┌───▼───┐       │   ┌──────┐  ┌──────┐      │   │
  │ADC x3 │──────►│──►│Clarke│─►│ Park │──────┘   │
  └───────┘       │   └──────┘  └──┬───┘          │
                  │                │               │
  ┌───────┐       │   ┌────────┐   │               │
  │Encoder│──────►│──►│θ_e 计算│───┘               │
  └───────┘       │   └────────┘                   │
                  └─────────────────────────────────┘
                            │
                      AXI/SPI 接口
                            │
                  ┌─────────▼─────────┐
                  │    ARM 核          │
                  │  速度环 + 位置环    │
                  │  EtherCAT 从站栈   │
                  └───────────────────┘

6.5 计算时间预算示例

场景：18-DOF 人形机器人下半身（6 基座 + 12 关节），1 kHz WBC

总预算: 1000 μs
├── EtherCAT 通信 (125μs 周期):              150 μs
│     ├ 帧传输 + 从站处理: 80 μs
│     └ 缓冲/安全余量: 70 μs
├── 传感器处理:                               80 μs
│     ├ IMU 数据解析 + 互补滤波: 30 μs
│     ├ 编码器数据转换 + FK: 40 μs
│     └ 力传感器读取: 10 μs
├── 状态估计:                                 100 μs
│     ├ 浮动基座 EKF (position + velocity): 60 μs
│     └ 接触状态检测: 40 μs
├── WBC QP 求解:                             300 μs
│     ├ 约束/目标矩阵构建: 80 μs
│     ├ qpOASES 求解 (warm-start, ~3 iter): 180 μs
│     └ 结果提取 + 安全检查: 40 μs
├── 动力学前馈:                               60 μs
│     ├ Pinocchio RNEA: 40 μs
│     └ 重力补偿: 20 μs
├── 安全检查:                                 60 μs
│     ├ 关节限位检测: 15 μs
│     ├ 力矩限制 + 功率限制: 15 μs
│     ├ 自碰撞检测: 20 μs
│     └ 急停逻辑: 10 μs
└── 余量 (buffer):                           250 μs
      → 用于应对偶发的计算峰值

余量为总预算的 25%，确保即使在最坏情况（多次 QP 迭代、复杂接触变化）下也不超时。

---

7. 接触力学 (Contact Mechanics)

7.1 为什么接触难以仿真

接触引入了以下数学和物理上的困难：

1. 不连续性 (Discontinuity)：系统在 “无接触” 和 “有接触” 之间切换时，约束方程的结构突变（自由度瞬间减少）。动力学方程从 ODE 变成 DAE。

2. 互补约束 (Complementarity)：法向力 $\lambda_n \geq 0$ 与穿透深度 $\phi \geq 0$ 满足 $\lambda_n \cdot \phi = 0$。这是非光滑约束，标准优化器无法直接处理。

3. 刚体碰撞的冲量性质 (Impulsive)：理想刚体碰撞在零时间内改变速度。需要冲量 (impulse) 而非力来描述。

4. 摩擦的非唯一性 (Non-uniqueness)：三维库仑摩擦加滑动约束构成的 LCP 可能无解或多解（Painleve 悖论，1895）。

5. 多接触耦合 (Multi-contact coupling)：一个接触点的力通过刚体传递影响其他接触点的状态。$k$ 个接触点的 LCP 维度为 $3k$。

7.2 库仑摩擦 (Coulomb Friction)

基本模型将摩擦分为静摩擦和动摩擦：

$$ \text{静止时:} \quad |f_t| \leq \mu_s f_n \quad \text{(粘滞，tangential force within cone)} $$

$$ \text{滑动时:} \quad f_t = -\mu_k f_n \frac{v_t}{|v_t|} \quad \text{(力方向与相对速度相反)} $$

$\mu_s > \mu_k$ 导致从静止到滑动的跳变（stick-slip 现象）。这个不连续性在仿真中需要特殊处理（如正则化、时间步进法）。

常见材料摩擦系数：

接触对	$\mu_s$	$\mu_k$
橡胶/混凝土	0.8~1.0	0.6~0.8
橡胶/钢	0.6~0.8	0.4~0.6
铝/铝	0.5~0.6	0.4~0.5
聚氨酯/地板	0.5~0.7	0.4~0.6

7.3 摩擦锥 (Friction Cone)

三维接触力约束：

$$ \mathcal{FC} = {f = (f_x, f_y, f_z) ;|; \sqrt{f_x^2 + f_y^2} \leq \mu f_z,; f_z \geq 0} $$

这定义了顶点在接触点、轴沿法线方向、半角为 $\arctan(\mu)$ 的圆锥。

线性化近似：将圆锥近似为 $m$-面体锥（$m = 4, 8, 16$）。4-面近似：

$$ |f_x| + |f_y| \leq \mu f_z $$

等价于4个线性不等式。等效摩擦系数为 $\mu/\sqrt{2} \approx 0.707\mu$。8-面近似更精确，等效系数约 $0.924\mu$。

对于面接触（机器人足底），接触力还受 Center of Pressure (CoP) 约束：

$$ |m_x/f_z| \leq d_y, \quad |m_y/f_z| \leq d_x $$

$d_x, d_y$ 为足底边长的一半。CoP 必须在支撑面内。

7.4 刚体碰撞模型

牛顿恢复系数 (Coefficient of Restitution)：

$$ e = -\frac{v_n^+}{v_n^-} $$

$v_n^-$ 为碰前法向接近速度（负值），$v_n^+$ 为碰后法向分离速度（正值或零）。

冲量-动量关系（单接触点，两刚体）：

$$ M_{\text{eff}} (v_n^+ - v_n^-) = \Lambda_n $$

其中有效质量 $M_{\text{eff}} = (1/m_1 + 1/m_2 + n^T J_1^{-1}(r_1 \times n) \times r_1 + \ldots)^{-1}$。

对于多体系统：

$$ M(q)(v^+ - v^-) = J_c^T \Lambda $$

冲量 $\Lambda$ 通过恢复系数 $e$ 和摩擦锥约束联合确定。当存在摩擦时，碰撞问题本身就是一个 LCP。

7.5 线性互补问题 (LCP) 公式

接触问题的标准 LCP 形式：找 $\lambda \geq 0$ 满足：

$$ w = A\lambda + b \geq 0, \quad \lambda^T w = 0 $$

构造（以正向动力学 time-stepping 为例）：

给定当前状态 $(q, v)$ 和时间步 $h$，下一步速度 $v^+ = v + h M^{-1}(\tau + J_c^T \lambda - h)$。

接触约束：$\phi(q^+) \geq 0$（穿透深度非负），一阶近似：

$$ J_c v^+ + \frac{\phi}{h} \geq 0 $$

代入 $v^+$ 的表达式并令 $w = J_c v^+ + \phi/h$：

$$ w = \underbrace{J_c M^{-1} J_c^T}{A_n} \cdot h\lambda + \underbrace{J_c(v + h M^{-1}(\tau - h)) + \phi/h}{b} $$

加上互补条件 $\lambda \geq 0,; w \geq 0,; \lambda^T w = 0$，得到完整 LCP。

加入摩擦后，LCP 扩展到 $3k$ 维（法向 + 两切向），成为 Mixed LCP (MLCP)。

求解方法：

• Lemke’s algorithm：枢轴法，在有解时保证找到解。最坏情况指数时间
• PATH solver：大规模 NCP 的标准商业求解器
• Projected Gauss-Seidel (PGS)：迭代法，MuJoCo/Bullet 常用。不保证收敛但实践中快速
• 凸优化松弛：MuJoCo 的方法，将 LCP 松弛为凸 QP/SOCP，保证唯一解

7.6 仿真引擎对比

引擎	接触模型	积分方法	精度	速度	RL训练适用性
MuJoCo	凸优化 (complementarity-free)	隐式 Euler / RK4	中	极快	最佳
Drake	Time-stepping LCP / SAP	隐式	高	中	一般
Bullet	Sequential Impulse (PGS)	半隐 Euler	低	快	旧方案
DART	Lemke LCP	隐式	高	慢	不适合
Isaac Sim	GPU TGS	多种	中	大规模快	优秀
Raisim	Bisection + constraint	隐式	高	快	良好

MuJoCo 的接触模型（核心创新）：

MuJoCo 不解 LCP，而是将接触表述为凸优化。对于每个时步：

$$ \min_f ;; \frac{1}{2} f^T R f + f^T (J v + a_{\text{ref}} \cdot h) $$

$$ \text{s.t.} \quad f \in \mathcal{K} \quad \text{(摩擦锥)} $$

$R$ 为正定正则化矩阵，物理意义为接触的”软度”（刚度的倒数）。$R > 0$ 保证问题有唯一解，且解连续依赖于状态变化。这消除了 LCP 的多解和无解问题，代价是引入了微小穿透（由 $R$ 的大小控制）。

7.7 Sim-to-Real Gap

接触仿真是 sim-to-real transfer 最大的误差来源之一。主要差异：

仿真假设	真实世界	影响
均匀固定摩擦系数	摩擦随表面状况变化 (磨损/湿度/温度)	步态稳定性
点/线接触	面接触 + 材料形变	接触力分布
刚体	柔性连杆、关节弹性	高频振动
精确几何	制造公差、装配误差	接触位置偏差
无传感器噪声	IMU 漂移、编码器量化	状态估计误差

缓解策略：

• Domain Randomization：随机化 $\mu \in [0.3, 0.8]$、接触刚度、地形高度等
• System Identification：在真实硬件上辨识关键参数
• Residual Policy：仿真策略 + 真机残差修正网络
• Teacher-Student：教师策略在仿真中使用特权信息，学生策略只用真机可观测信息

---

8. 步态控制 (Locomotion Control)

8.1 ZMP (Zero Moment Point)

ZMP 定义：地面上使得绕该点的净水平力矩为零的等效力作用点。

推导：设系统总质量为 $m$，CoM 位置为 $(x_G, y_G, z_G)$，CoM 加速度为 $(\ddot{x}_G, \ddot{y}_G, \ddot{z}_G)$。在 ZMP $(x_Z, y_Z, 0)$ 处力矩为零：

$$ x_Z = x_G - \frac{z_G \cdot \ddot{x}_G}{g + \ddot{z}_G} $$

$$ y_Z = y_G - \frac{z_G \cdot \ddot{y}_G}{g + \ddot{z}_G} $$

对多连杆机器人的精确公式：

$$ x_Z = \frac{\sum_{i=1}^N m_i(g + \ddot{z}i) x_i - \sum{i=1}^N m_i \ddot{x}i z_i}{\sum{i=1}^N m_i(g + \ddot{z}_i)} $$

稳定条件：ZMP 必须位于支撑多边形 (Support Polygon) 内部。ZMP 在边界上时为临界稳定，超出边界则翻倒。

线性倒立摆模型 (LIPM)：

假设 CoM 高度恒定 $z_c$，角动量变化率为零：

$$ \ddot{x}_G = \frac{g}{z_c}(x_G - x_Z) $$

这是一个不稳定线性系统（特征值 $\pm \omega_0$，$\omega_0 = \sqrt{g/z_c}$）。ZMP 成为控制输入，CoM 为状态。

Preview Control（Kajita et al., 2003）：基于 LIPM，使用 MPC 预览未来 ZMP 参考轨迹（由落脚点序列确定），优化 CoM 轨迹使 ZMP 跟踪误差最小。

8.2 Capture Point / DCM (Divergent Component of Motion)

Capture Point $\xi$ 是当前 CoM 状态下，机器人必须踏到的位置才能最终停下来：

$$ \xi = x_G + \frac{\dot{x}_G}{\omega_0}, \quad \omega_0 = \sqrt{g/z_c} $$

Capture Point 的动力学（一阶系统）：

$$ \dot{\xi} = \omega_0(\xi - x_Z) $$

这是一个不稳定一阶线性 ODE（$\xi$ 趋向远离 $x_Z$）。控制策略：将 ZMP 放在 Capture Point 的方向上使其收敛：

$$ x_Z = \xi + \frac{1}{\omega_0}\dot{\xi}^{\text{des}} = \xi - \frac{k}{\omega_0}(\xi - \xi^{\text{ref}}) $$

落脚点规划的核心逻辑：

1. 计算当前 Capture Point $\xi$
2. 下一步落脚位置设为 $\xi$ 附近（偏移量由目标速度决定）
3. 在支撑相内，用 ZMP 控制使 $\xi$ 收敛到下一步落脚位置

与 LIPM 的关系：

LIPM 的解 $x_G(t) = c_1 e^{\omega_0 t} + c_2 e^{-\omega_0 t} + x_Z$

其中 $c_1 e^{\omega_0 t}$ 为发散分量 (Divergent Component, DCM)，$c_2 e^{-\omega_0 t}$ 为收敛分量。

$\xi = x_G + \dot{x}_G/\omega_0$ 恰好提取了发散分量：$\xi = 2c_1 e^{\omega_0 t} + x_Z$。

控制发散分量 = 控制 Capture Point = 保证平衡。

8.3 质心动力学 (Centroidal Dynamics)

整个机器人的质心动力学由 Newton-Euler 方程描述：

$$ m\ddot{p}G = \sum{i=1}^{n_c} F_i + mg $$

$$ \dot{L}G = \sum{i=1}^{n_c} (r_i - p_G) \times F_i $$

$p_G$ 为质心位置，$L_G$ 为质心处的角动量（$L_G = A_G(q)\dot{q}$，$A_G$ 为质心动量矩阵），$F_i$ 为第 $i$ 个接触力，$r_i$ 为接触点位置。

质心动力学的优势：将高维全身动力学 ($6+n$ DOF) 压缩为 6 维（3 线动量 + 3 角动量），足以描述平衡和整体运动趋势。

Centroidal MPC：基于质心动力学做模型预测控制，优化变量为接触力序列 $F_i(t)$，约束为摩擦锥和运动学可达性。这是当前双足/人形 MPC 的主流方法之一。

8.4 落脚规划 (Footstep Planning)

输入：当前 CoM 状态、目标行走速度 $v^{\text{cmd}} = (v_x, v_y, \omega_z)$ 输出：落脚点序列 ${p_1, p_2, \ldots}$ 和对应时间

基于 Capture Point 的落脚规划：

$$ p^{\text{next}}_{\text{foot}} = \xi + \frac{v^{\text{cmd}}}{\omega_0} + b $$

$b$ 为固定偏移（考虑髋宽、步态参数）。

步态时序（双足行走）：

时间 t: |←T_ds→|←───── T_ss ─────→|←T_ds→|←───── T_ss ─────→|
左脚:    [支撑===][支撑==============][支撑===][摆动==============]
右脚:    [支撑===][摆动==============][支撑===][支撑==============]
         双支撑    右脚单支撑          双支撑    左脚单支撑

典型参数（成人行走速度 ~1.2 m/s）：

• 步态周期 $T_{\text{cycle}}$：0.8~1.2 s
• 双支撑期 $T_{\text{ds}}$：10~20% of $T_{\text{cycle}}$
• 步幅 (stride length)：0.3~0.7 m
• 步高 (step height)：5~15 cm

摆动腿轨迹：通常用三次或五次多项式/贝塞尔曲线参数化，满足起止位置、速度、加速度约束：

           ┌── 最高点 (mid-swing)
          / \       h = step_height
         /   \
────────/     \────────  地面高度
start          end
(lift-off)    (touch-down)

8.5 Raibert 弹跳控制器 (1986)

Marc Raibert 在 MIT 提出的经典三部分解耦控制（适用于单腿弹跳和四足对角步态）：

第一部分：跳跃高度控制（能量注入）

在支撑相通过腿部伸展注入能量：

$$ \Delta E = \frac{1}{2}k_{\text{leg}}(\Delta l_{\text{thrust}})^2 $$

稳态条件下注入能量等于每个周期的能量损耗。控制律为：

$$ \Delta l_{\text{thrust}} = k_h \cdot (h_{\text{des}} - h_{\text{actual}}) $$

$h$ 为跳跃顶点高度。

第二部分：水平速度控制（落脚位置）

落脚位置的选择是 Raibert 控制器的核心洞察：

$$ x_{\text{foot}} = \frac{\dot{x} \cdot T_s}{2} + k_v(\dot{x} - \dot{x}_{\text{des}}) $$

第一项 $\frac{\dot{x} \cdot T_s}{2}$ 为中性点 (neutral point)：如果落脚在质心正下方移动到此位置，支撑相前半段减速与后半段加速恰好抵消，水平速度不变。第二项为比例反馈，偏离中性点以产生加速/减速效果。

$T_s$ 为支撑相时长（弹跳周期的触地时间），$\dot{x}$ 为当前水平速度。

第三部分：姿态控制（髋关节力矩）

在滞空相/支撑相通过髋关节力矩维持躯干水平：

$$ \tau_{\text{hip}} = -K_\theta \theta_{\text{body}} - K_{\dot\theta} \dot\theta_{\text{body}} $$

简单 PD 控制即可，因为姿态动力学在小角度下近似线性。

Raibert 控制器的历史意义：

1. 首次证明腿式运动可以通过简单控制律实现
2. “落脚位置控制速度”这一洞察至今仍是所有步态控制器的基础
3. 三部分解耦思想影响了后续二十年的腿式机器人研究
4. Boston Dynamics 的 BigDog (2005)、Spot (2019) 均继承了这一思路

8.6 现代基于 RL 的步态控制

从 2019 年开始，强化学习在四足和双足步态控制中取得了超越传统方法的性能。代表性工作：

• ETH Zurich: ANYmal 系列（Learning Agile Locomotion, 2019~2024）
• UC Berkeley: Cassie/Digit 双足 RL
• NVIDIA/Legged Gym: 大规模并行 RL 训练框架
• Agility Robotics: Digit 仓储物流

典型 RL 步态控制流水线：

┌────────────────────────────────────────────────────────────┐
│                    训练阶段 (Simulation)                      │
│                                                            │
│  ┌───────────────┐    ┌──────────────────────┐            │
│  │ Isaac Gym      │    │ PPO / SAC            │            │
│  │ (4096 并行环境) │◄──►│ (策略/价值网络训练)    │            │
│  │ Domain Rand    │    │ MLP [256,128,64]     │            │
│  └───────────────┘    └──────────────────────┘            │
│         │                        │                         │
│    observations              actions                       │
│    (proprioception)      (joint position offsets)           │
└────────────────────────────────────────────────────────────┘
                               │
                        策略权重导出
                               │
┌──────────────────────────────▼─────────────────────────────┐
│                    部署阶段 (Real Robot)                      │
│                                                            │
│  ┌───────────────┐    ┌──────────────────────┐            │
│  │ 传感器          │    │ 策略推理 (50Hz)       │            │
│  │ IMU + 编码器    │───►│ MLP forward pass     │            │
│  └───────────────┘    │ (~0.1ms on CPU)      │            │
│                       └──────────┬───────────┘            │
│                                  │ Δq (position offsets)   │
│                       ┌──────────▼───────────┐            │
│                       │ PD 控制器 (1kHz)       │            │
│                       │ τ = Kp*(q_d+Δq - q)   │            │
│                       │    + Kd*(0 - dq)      │            │
│                       └──────────┬───────────┘            │
│                                  │ τ (torque commands)     │
│                       ┌──────────▼───────────┐            │
│                       │ 电机驱动器 (EtherCAT)  │            │
│                       └──────────────────────┘            │
└────────────────────────────────────────────────────────────┘

观测空间 (Observation) 的典型组成：

观测量	维度	来源
基座角速度	3	IMU 陀螺仪
重力方向（基座坐标系）	3	IMU 加速度计/姿态解算
关节角度	$n$	编码器
关节角速度	$n$	编码器差分或观测器
上一步动作	$n$	缓存
速度指令	3	$(v_x, v_y, \omega_z)$ 用户输入

注：不使用基座位置（无法在真机上获得），不使用关节力矩（噪声大），不使用视觉（解耦感知和控制）。

动作空间 (Action)：

• 最常见：关节位置偏移 $\Delta q$，叠加到默认站立位姿 $q_{\text{default}}$ 上
• 目标关节位置 $q_{\text{target}} = q_{\text{default}} + \Delta q$
• 由底层 PD 控制器执行：$\tau = K_p(q_{\text{target}} - q) + K_d(0 - \dot{q})$
• $K_p, K_d$ 通常固定或也作为策略输出

奖励函数设计（核心工程问题）：

$$ r_t = \sum_i w_i r_i(s_t, a_t) $$

奖励/惩罚项	公式/含义	权重方向
线速度跟踪	$\exp(-|v_{xy} - v_{xy}^{\text{cmd}}|^2 / \sigma)$	正（主要目标）
角速度跟踪	$\exp(-|\omega_z - \omega_z^{\text{cmd}}|^2 / \sigma)$	正
存活奖励	1 per timestep	正
关节力矩惩罚	$|\tau|^2$	负（节能/平滑）
动作变化率	$|a_t - a_{t-1}|^2$	负（平滑性）
非期望接触	膝盖/躯干触地	负（大惩罚）
关节超限	接近关节限位	负
躯干倾斜	$|g_{\text{proj}} - [0,0,-1]|^2$	负
脚部空气时间	每只脚的滞空时间目标	正（步态频率）
脚底接触力对称	左右脚力差异	负

Domain Randomization 参数（Sim-to-Real 关键）：

参数	随机化范围	目的
摩擦系数 $\mu$	[0.3, 1.2]	适应不同地面
基座质量	[0.8, 1.2] × 标称值	建模误差
关节阻尼/摩擦	[0.5, 1.5] × 标称值	驱动器不确定性
外部推力	随机 030N, 持续 0.21s	鲁棒性
地形高度	随机凹凸 ±3cm	非平坦地面
PD 增益	[0.9, 1.1] × 标称值	控制器不确定性
通信延迟	0~20ms	真实系统延迟

与 Layer 7 的连接：RL 策略的训练属于 Layer 7（学习与策略层），其输出（关节位置/力矩指令）由 Layer 3 的底层控制器执行。Layer 3 提供安全保障（力矩限制、关节限位、急停），Layer 7 提供行为适应性。两层的接口清晰：Layer 7 输出目标 $q_{\text{target}}$ 或 $\tau_{\text{cmd}}$，Layer 3 的 PD 控制器以 1 kHz 执行跟踪。

---

9. 公司格局表

9.1 核心厂商

公司	核心产品/技术	控制架构特点	适用场景
MathWorks	Simulink, Simscape Multibody, Motor Control Blockset, Embedded Coder	模型化设计 (MBD)，自动 C/HDL 代码生成，硬件在环 (HIL) 仿真，与 TI C2000/STM32 深度集成	控制器原型验证、FOC 开发、系统级仿真
Beckhoff	TwinCAT 3, AX8000/AX5000 伺服, EL 系列 I/O, EtherCAT	软 PLC (IEC 61131-3 + C++) 运行在 Windows 实时扩展上，确定性 EtherCAT 通信，PC-based 控制架构	工业多轴协调、CNC、包装机械、机器人集成
Franka Robotics	Franka Research 3 (FR3), libfranka, franka_ros2	7-DOF 全力矩传感，1 kHz 外部力矩接口 (FCI)，关节级阻抗/导纳模式切换，内置碰撞检测和自动恢复	人机协作研究、精密装配实验、RL 策略真机部署
KUKA	iiwa 7/14 (LBR), Sunrise.OS, RSI (Robot Sensor Interface)	7-DOF 全力矩传感，Sunrise Java 框架（应用级编程），SmartServo（实时笛卡尔控制），FRI 1 kHz 外部接口	工业协作、医疗辅助手术、航空装配
Universal Robots	UR3e/5e/10e/16e/20e/30e, URScript, PolyScope, RTDE	6-DOF 协作臂，URScript 脚本（位置/速度/力控），500 Hz RTDE 外部接口，力模式 (force mode)	柔性产线、快速部署、中小企业自动化
Elephant Robotics	myCobot 280/320, myArm, mechArm	桌面级 6-DOF 协作臂，M5Stack ESP32 控制器，Python/ROS/C++ SDK，低成本 ($300~1500)	教育、轻量原型、个人开发者、AI+机器人实验

9.2 机器人平台公司

公司	平台	控制特点
Boston Dynamics	Atlas (液压人形), Spot (电动四足)	定制实时控制栈，WBC + Centroidal MPC，液压阀 1 kHz 控制，不公开 SDK
Unitree	Go2/B2 (四足), H1/G1 (人形)	自研无刷电机驱动，EtherCAT 1 kHz 力矩接口，开放 C++/Python SDK，支持 RL 部署
Agility Robotics	Digit (双足)	串联弹性执行器 (SEA)，被动动力学设计，质心动力学 + WBC，RL 步态
Figure AI	Figure 01/02 (人形)	OpenAI VLA 集成，电动全力矩控制，不公开底层 SDK
Tesla	Optimus Gen 2/3 (人形)	自研线性执行器，端到端学习探索中，量产导向设计

9.3 开发工具与控制框架

框架/工具	开源	语言	实时性	主要用途
ROS 2 Control (ros2_control)	是	C++	软实时 (RT-PREEMPT)	通用机器人控制管理框架
mc_rtc	是	C++	硬实时	QP-based WBC (CNRS/LIRMM)
Crocoddyl	是	C++/Python	离线	最优控制 (iLQR/DDP)
OCS2	是	C++	软实时	MPC + WBC (ETH RSL)
libfranka	是	C++	硬实时 (1 kHz)	Franka 专用实时控制
TwinCAT 3	否	IEC 61131-3/C++	硬实时 (< 50μs)	Beckhoff 工业控制
NVIDIA Isaac Lab	是	Python (GPU)	非实时（训练）	大规模并行仿真 + RL

---

10. 各模块关系与完整计算栈

Layer 7 (Learning)              Layer 5/6 (Planning/Perception)
     │                                   │
     │ action: Δq or τ_cmd               │ desired task-space trajectory
     │ @ 50 Hz                           │ @ 10-100 Hz
     ▼                                   ▼
┌────────────────────────────────────────────────────────────┐
│                   Layer 3: 底层控制与动力学                    │
│                                                            │
│  ┌─────────────────────────────────────────────────────┐  │
│  │  步态控制 / MPC (8.1-8.6)                             │  │
│  │  ZMP/Capture Point/Centroidal Dynamics               │  │
│  │  输出: CoM 轨迹, 落脚点, 摆动脚轨迹                    │  │
│  └────────────────────────┬────────────────────────────┘  │
│                           │                               │
│  ┌────────────────────────▼────────────────────────────┐  │
│  │  全身控制 WBC / HQP (5.1-5.6)                        │  │
│  │  多任务优化 + 约束满足                                 │  │
│  │  输入: 多层任务目标 + 物理约束 (摩擦锥, 力矩限位)       │  │
│  │  输出: 关节加速度 ddq, 关节力矩 τ, 接触力 Fc          │  │
│  │  求解器: qpOASES / OSQP / ProxQP                     │  │
│  └────────────────────────┬────────────────────────────┘  │
│                           │                               │
│      ┌────────────────────┼────────────────────┐         │
│      │                    │                    │         │
│      ▼                    ▼                    ▼         │
│  ┌────────┐      ┌───────────────┐     ┌───────────┐   │
│  │阻抗控制 │      │逆动力学前馈    │     │PID 级联    │   │
│  │(4.1-4.6)│      │RNEA (2.3)     │     │(3.1-3.5)  │   │
│  │τ_imp    │      │τ_ff=M*ddq+h   │     │τ_fb       │   │
│  └───┬────┘      └──────┬────────┘     └────┬──────┘   │
│      │                   │                    │          │
│      └───────────────────┼────────────────────┘          │
│                          │  τ_total = τ_imp + τ_ff + τ_fb │
│                          ▼                               │
│  ┌─────────────────────────────────────────────────────┐  │
│  │  实时通信层 (6.1-6.5)                                 │  │
│  │  EtherCAT @ 125μs / CAN-FD @ 1ms                    │  │
│  │  RTOS: RT-PREEMPT / Xenomai                          │  │
│  └────────────────────────┬────────────────────────────┘  │
│                           │                               │
│  ┌────────────────────────▼────────────────────────────┐  │
│  │  驱动器层 (电流环 @ 10-40 kHz)                         │  │
│  │  FOC + FPGA / DSP (3.2, 6.4)                        │  │
│  │  Clarke → Park → PI(d,q) → InvPark → SVPWM → 电机   │  │
│  └─────────────────────────────────────────────────────┘  │
│                                                            │
│  ┌─────────────────────────────────────────────────────┐  │
│  │  接触力学 (7.1-7.7)                                   │  │
│  │  贯穿仿真（训练）和真实部署（安全约束）                  │  │
│  └─────────────────────────────────────────────────────┘  │
└────────────────────────────────────────────────────────────┘
                           │
                           ▼
                    物理世界 (关节运动, 接触力)

---

参考文献

1. Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
2. Murray, R. M., Li, Z., & Sastry, S. S. (1994). A Mathematical Introduction to Robotic Manipulation. CRC Press.
3. Lynch, K. M., & Park, F. C. (2017). Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press.
4. Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.
5. Carpentier, J., et al. (2019). “The Pinocchio C++ library.” IEEE SII 2019.
6. Wensing, P. M., et al. (2023). “Optimization-Based Control for Dynamic Legged Locomotion.” Annual Review of Control, Robotics, and Autonomous Systems.
7. Raibert, M. H. (1986). Legged Robots That Balance. MIT Press.
8. Hogan, N. (1985). “Impedance Control: An Approach to Manipulation.” ASME J. Dynamic Systems.
9. Khatib, O. (1987). “A Unified Approach for Motion and Force Control of Robot Manipulators.” IEEE J. Robotics and Automation.
10. Vukobratovic, M., & Borovac, B. (2004). “Zero-Moment Point: Thirty Five Years of its Life.” Int. J. Humanoid Robotics.
11. Englsberger, J., et al. (2015). “Three-Dimensional Bipedal Walking Control Based on Divergent Component of Motion.” IEEE Trans. Robotics.
12. Kajita, S., et al. (2003). “Biped Walking Pattern Generation by using Preview Control of Zero-Moment Point.” ICRA 2003.
13. Rudin, N., et al. (2022). “Learning to Walk in Minutes Using Massively Parallel Deep Reinforcement Learning.” CoRL 2022.
14. Hwangbo, J., et al. (2019). “Learning agile and dynamic motor skills for legged robots.” Science Robotics.
15. Stellato, B., et al. (2020). “OSQP: An Operator Splitting Solver for Quadratic Programs.” Mathematical Programming Computation.
16. MuJoCo Documentation. https://mujoco.readthedocs.io
17. Drake Documentation. https://drake.mit.edu
18. SOEM (Simple Open EtherCAT Master). https://github.com/OpenEtherCATsociety/SOEM
19. Tedrake, R. (2023). Underactuated Robotics. MIT OCW.
20. TRAC-IK. https://traclabs.com/projects/trac-ik/
21. Mansard, N., et al. (2009). “A Versatile Generalized Inverted Kinematics Implementation.” Advanced Robotics.

---

本文为 520 具身智能研究项目 Layer 3 的系统性技术笔记。后续根据实际学习进度迭代更新，从 seedling 逐步标注为 budding。