对于双足人形机器人而言，踝关节通常是最先接触地面、承受冲击并需要精确力控的关键部位。Atom01 采用了闭链连杆（closed-chain linkage） 构型的踝关节：电机并非直接驱动 pitch/roll 关节轴，而是通过两根长度不同的推杆（rod）与摇臂（bar）构成的五杆机构，间接实现足端的俯仰与横滚运动。这种设计在提升结构刚度和抗冲击能力的同时，也带来了关节空间与电机空间非线性耦合的核心难题。

本文档聚焦 Decouple 抽象体系、DecoupleAtom01 运动学实现，以及它们在 RobotInterface 实时控制循环中的双向映射集成。阅读前建议先理解 RobotInterface 设计与实现 中的电机抽象与配置模型，再向下深入。

Sources: close_chain_mapping.hpp, decouple_atom01.hpp, robot_interface.hpp

问题背景：为什么要做闭链解耦

在基于强化学习（RL）的策略网络中，模型输出的动作向量通常定义在关节空间（joint space）：即各个自由度的期望角度/速度/力矩。对于普通串联关节，关节角度与电机编码器读数之间仅相差一个固定的减速比或方向符号，映射关系是线性的。

然而 Atom01 的踝关节属于并联闭链机构：

• 左足由电机 ID 5、6 驱动两根连杆；
• 右足由电机 ID 11、12 驱动同样的机构；
• 电机轴输出角 $\theta$ 与足端实际 pitch/roll 之间不存在简单的线性比例关系；
• 速度和力矩同样需要通过雅可比矩阵进行空间变换。

如果直接将策略输出的 pitch/roll 当作电机位置指令下发，足端姿态会严重偏离预期，甚至引发机构干涉或失稳。因此必须在策略输出层与电机驱动层之间插入一个双向运动学映射层，使得上层策略仍然以“抽象关节”进行训练与推理，下层硬件则以“真实电机”进行闭环控制。

Sources: robot.yaml, robot_interface.cpp

架构抽象：Decouple 类体系

系统采用工厂模式对闭链解耦器进行抽象，核心由三部分组成：

Decouple::create() 根据 robot.yaml 中的 type 字段实例化具体实现。当前仅支持 "atom01"，未来若更换脚踝机械结构，只需新增派生类并注册到工厂，无需修改 RobotInterface 的控制逻辑。

Sources: close_chain_mapping.hpp, close_chain_mapping.cpp

Atom01 踝关节运动学模型

DecoupleAtom01 将每只脚的闭链机构抽象为两条独立的平面连杆：长连杆（long link）与短连杆（short link）。连杆参数通过 LinkParamsAtom01 描述，并在构造函数中根据左右足镜像初始化。

几何常量

上述坐标系以踝關節中心為原點，X 指向機器人前方，Z 指向上方，Y 指向左方。足端附着点 C 在机器人发生 roll/pitch 旋转后，其空间位置由旋转矩阵 $R_y(\text{pitch}) \cdot R_x(\text{roll})$ 变换得到。

Sources: decouple_atom01.cpp

逆运动学与解耦映射：Joint → Motor

当策略网络输出足端期望的 pitch 与 roll，或 PD 控制器在关节空间计算出期望力矩后，需要将它们反解到电机空间。该过程由 get_decoupleQVT() 完成，内部拆解为两步：

1. 解析逆运动学（IK）

inverse_kinematics() 对每条连杆分别求解电机角 $\theta_i$。算法流程如下：

1. 通过 $R_y \cdot R_x$ 将足端固定点 r_C_0 旋转到当前姿态，得到 r_C_i；
2. 建立几何约束：$| \mathbf{r}{C_i} - \mathbf{r}{B_i} | = l_{\text{rod}}$，且 $\mathbf{r}_{B_i}$ 由电机角 $\theta_i$ 绕 A 点旋转得到；
3. 将约束展开为关于 $\sin\theta_i$ 和 $\cos\theta_i$ 的三角方程，整理后通过 asin 解析求解： $$ \theta_i = \text{asin}\left( \frac{b \cdot c + \sqrt{\Delta}}{a^2 + b^2} \right) $$ 其中 $\Delta$ 为判别式，且根据 $a$ 的符号对结果进行象限修正。

若判别式为负（超出工作空间），代码会将其钳位到 0 并输出警告，防止后续出现 NaN。

Sources: decouple_atom01.cpp

2. 雅可比计算与 QVT 变换

jacobian() 构建三条微分映射链，最终合成 2×2 的电机-关节雅可比：

1. 末端速度-关节速度：$J_q$ (6×2)，描述 roll/pitch 对足端点 C 线速度与角速度的偏导；
2. 末端速度-电机速度：$J_\theta$ (2×2)，描述各电机角速度对推杆沿杆长方向速度的投影；
3. 末端力-空间：$J_x$ (2×6)，描述足端虚位移与电机做功的关系。

通过链式法则得到中间矩阵 $J_{\text{Temp}} = J_x \cdot J_q$，再利用 Eigen 的 PartialPivLU 求解线性系统：

• $J_{\text{motor2Joint}} = J_{\text{Temp}}^{-1} \cdot J_\theta$（电机空间 → 关节空间）
• $J_{\text{Joint2motor}} = J_\theta^{-1} \cdot J_{\text{Temp}}$（关节空间 → 电机空间）

最终，get_decoupleQVT() 的执行顺序为：

$$ q_{\text{motor}} = \text{IK}(q_{\text{joint}}), \quad \dot{q}{\text{motor}} = J{\text{Joint2motor}} \cdot \dot{q}{\text{joint}}, \quad \tau{\text{motor}} = J_{\text{motor2Joint}}^\top \cdot \tau_{\text{joint}} $$

注意力矩映射使用了转置关系（虚功原理），确保关节空间与电机空间的功率等价。

Sources: decouple_atom01.cpp

正运动学与闭环反馈：Motor → Joint

在观测值采集阶段，系统读取的是电机编码器反馈（电机角 $\theta$），必须将其映射回关节空间，才能与策略训练时使用的 URDF/仿真关节定义对齐。

牛顿-拉夫森迭代求解

forward_kinematics() 负责求解非线性方程 $\text{IK}(\text{roll}, \text{pitch}) - \theta_{\text{ref}} = 0$。由于解析正解不存在，代码采用阻尼牛顿迭代：

while (error.norm() > TOLERANCE && count < MAX_ITERATIONS) {
    kinematics = inverse_kinematics(x_k[1], x_k[0], is_left);
    Jac        = jacobian(kinematics, x_k[0]);
    error      = thetaRef - kinematics.THETA;
    x_k        = x_k + ALPHA * Jac.J_motor2Joint * error;
    count++;
}

为提升实时性，迭代以 last_solution_ 作为热启动初值；收敛后将当前解写回缓存，供下一次控制周期使用。若雅可比出现 NaN（例如电机位置超出机械极限），函数立即返回 count = -1，上层控制循环将收到零位作为保护性回退。

Sources: decouple_atom01.cpp

正向 QVT 变换

get_forwardQVT() 将电机空间反馈映射回关节空间：

$$ q_{\text{joint}} = \text{FK}(q_{\text{motor}}), \quad \dot{q}{\text{joint}} = J{\text{motor2Joint}} \cdot \dot{q}{\text{motor}}, \quad \tau{\text{joint}} = J_{\text{Joint2motor}}^\top \cdot \tau_{\text{motor}} $$

这组公式保证了观测值（位置、速度、力矩）在输入策略网络前，与训练环境的关节定义完全一致。

Sources: decouple_atom01.cpp

控制循环中的双向映射集成

RobotInterface::apply_action() 是闭环映射与实际力控交汇的核心函数。以下 Mermaid 流程图展示了单次控制周期内，闭链电机与普通串联电机的差异化处理路径：

flowchart TD
    A[读取所有电机反馈] --> B{是否为闭链电机?}
    B -->|否| C[直接存入 joint_q/vel/tau]
    B -->|是| D[forwardQVT: 电机空间 → 关节空间]
    D --> E[存入 joint_q/vel/tau]
    E --> F[策略/PD 计算目标关节量]
    F --> G{是否为闭链电机?}
    G -->|否| H[下发位置指令 pos + kp + kd]
    G -->|是| I[在关节空间计算 PD 力矩]
    I --> J[decoupleQVT: 关节空间 → 电机空间]
    J --> K[下发力矩指令 tau]

实际代码路径拆解

在 apply_action() 中，逻辑被严格分左右足、按先正向后逆向的顺序执行：

1. 反馈正向映射（观测用）：读取电机 ID 5,6（左足）与 11,12（右足）的编码器值，调用 get_forwardQVT() 更新 joint_q_、joint_vel_、joint_tau_ 中对应的 ankle pitch/roll 索引。
2. 控制量逆向映射（执行用）：
   • 对非闭链电机，策略输出的 action[idx] 直接作为目标位置，使用配置中的 kp、kd 做 PD 位置跟踪；
   • 对闭链电机，先以关节空间的当前状态与目标状态做 PD： $$ \tau_{\text{joint}} = k_p \cdot (q_{\text{target}} - q_{\text{joint}}) + k_d \cdot (0 - \dot{q}_{\text{joint}}) $$
   • 再调用 get_decoupleQVT() 将关节力矩映射为电机力矩，最终通过 motor_mit_cmd(..., tau) 以力控模式下发。

这一设计的精妙之处在于：策略层和 PD 控制器完全工作在解耦后的关节空间，与仿真训练环境保持一致的 URDF 语义；而力矩的映射与下发隐藏在硬件抽象层，对上层透明。

Sources: robot_interface.cpp

初始化与状态刷新中的映射

除了实时控制循环，闭链解耦还深度参与机器人的生命周期管理：

关节归零（reset_joints）

当调用 reset_joints() 时，传入的是关节空间的默认角度向量。对于踝关节两个自由度，代码先调用 get_decoupleQVT() 将其转换为电机角度，再写入 motor_target_ 并缓变下发。这保证了机器人上电或复位时，足端姿态与预期默认站姿一致。

Sources: robot_interface.cpp

关节状态刷新（refresh_joints）

refresh_joints() 用于重新校准或手动刷新关节状态。它读取电机编码器后，同样通过 get_forwardQVT() 将电机空间映射回关节空间，确保 ROS 2 发布的 /joint_states 话题中的 ankle 数据是物理可解释的 pitch/roll，而非原始电机角。

Sources: robot_interface.cpp

配置与参数化

闭环解耦的行为由 config/robot.yaml 中的以下字段共同决定：

在构造函数中，RobotInterface 会遍历 close_chain_motor_id，在 motors_cfg_->motor_id_ 中查找其索引，并进一步通过 urdf2motor 反查得到关节索引 close_chain_joint_idx_。这一双重索引机制保证了电机总线顺序、URDF 语义顺序与控制向量顺序可以独立变化。

Sources: robot_interface.cpp, robot.yaml

数值稳定性与边界处理

由于运动学涉及多次三角函数、矩阵求逆与迭代，代码中内置了多层保护：

1. 判别式负值保护：IK 求解中若 $\Delta < 0$，将其钳位为 0 并输出 std::cerr 警告，防止 asin 输入越界。
2. NaN 检测：FK 迭代中一旦发现 J_motor2Joint.hasNaN()，立即终止并返回错误标记，避免将 NaN 传播到关节观测值。
3. 迭代上限：FK 牛顿法设置 MAX_ITERATIONS = 100，在 1 kHz 控制频率下，实测通常 3–10 次即可收敛到 1e-3 精度。
4. 热启动缓存：last_solution_ 以 std::map<bool, Eigen::Vector2d> 存储左右足最近收敛解，显著降低连续控制周期内的迭代次数。

这些机制共同确保了即使在极端姿态（如大角度横滚或俯仰）下，系统也能提供确定性的回退行为，而非崩溃或输出不可控的电机指令。

Sources: decouple_atom01.cpp, decouple_atom01.cpp

与观测、推理及安全模块的交互关系

闭链解耦虽然位于硬件抽象层，但其输出直接决定了策略网络“看到”的关节状态，因此与多个模块存在数据耦合：

• 观测值组装：观测值组装、归一化与帧堆叠 中的 joint_pos、joint_vel、joint_tau 在送入 ONNX 模型前，必须经过 get_forwardQVT() 的坐标转换。
• 策略推理：ONNX Runtime 推理引擎集成 输出的动作向量在 apply_action() 中被逆映射。
• 安全监控：安全监控、限位与故障处理 可基于关节空间（而非电机空间）设置更直观的 ankle 角度限位，因为关节空间与 URDF/仿真直接对应。

若你需要为新的机械结构（例如更换为六轴力矩传感器直驱脚踝）替换闭链解耦器，只需继承 Decouple 并实现新的 get_forwardQVT() 与 get_decoupleQVT()，然后在 robot.yaml 中修改 type 字段即可，无需触及 RobotInterface 的主控制逻辑。