在足式机器人中，踝关节通常承担俯仰（Pitch）与横滚（Roll）两个自由度的姿态调节任务。与常规串联关节不同，Atom01 采用并联闭链连杆机构实现踝关节驱动：两条长度不同的连杆（长连杆 180 mm、短连杆 110 mm）分别由独立电机驱动，通过共享足端交汇点共同约束足部的空间姿态。这种设计在提升结构刚度和承载能力的同时，引入了电机空间与关节空间非一一映射的核心问题——两个电机的旋转角度共同决定足部的俯仰与横滚，反之亦然。因此，在强化学习策略（工作在关节空间）与底层电机驱动（工作在电机空间）之间，必须建立严格的运动学解耦层，实现位置、速度、力矩的双向精确转换。

Sources: robot_interface.hpp, decouple_atom01.hpp

闭链踝关节的几何建模

Atom01 的每条腿踝关节由两组四杆机构并联构成。从几何角度看，每个连杆系统包含三个关键锚点：固定铰点 A（安装于小腿末端）、摇臂端点 B（由电机驱动旋转）、足端交汇点 C（两个连杆共享，定义足部姿态）。在初始构型下，长连杆与短连杆分别具有不同的空间排布：长连杆摇臂初始角度为 0 rad，短连杆摇臂初始角度为 π rad，两者的摇臂长度 l_bar 均为 20 mm，但驱动连杆长度 l_rod 分别为 180 mm 与 110 mm。左右腿的机构参数呈镜像对称，仅通过 l_spacing（±42.35 mm）区分 Y 方向的安装偏置。

Sources: decouple_atom01.cpp

下表汇总了单腿两组连杆的核心几何参数：

Sources: decouple_atom01.cpp

上述几何参数在 DecoupleAtom01 构造函数中被预计算并缓存为 links_left_ 与 links_right_，避免运行时重复构造，保证实时控制循环的确定性延迟。

Sources: decouple_atom01.hpp, decouple_atom01.cpp

运动学映射的核心算法

运动学解耦层围绕三条数学链路构建：**逆运动学（Inverse Kinematics, IK）**负责将关节空间姿态映射为电机角度；**雅可比矩阵（Jacobian）**负责建立速度与力矩的微分映射关系；**正运动学（Forward Kinematics, FK）**负责在观测方向将电机编码器反馈还原为关节空间状态。三者通过统一的 LinkParamsAtom01 参数模型关联，形成完整的闭链分析框架。

Sources: close_chain_mapping.hpp

逆运动学：从关节角度到电机角度

逆运动学的输入为足部的横滚角 q_roll 与俯仰角 q_pitch，输出为两根连杆对应的电机摇臂角 theta_1 与 theta_2。算法首先通过 R_y * R_x 的复合旋转矩阵，将足端初始点 r_C_0 变换到当前姿态下的 r_C_i。随后，对每组连杆建立平面几何约束：已知固定点 A、变换后的足端点 C、摇臂长度 l_bar 与连杆长度 l_rod，求解满足 |B - C| = l_rod 的摇臂转角 theta_i。

该过程最终归约为三角方程的解析求解。通过代数整理可得到关于 sin(theta_i) 与 cos(theta_i) 的线性组合，进而利用判别式求解。若判别式为负（机构处于奇异或不可达位形），系统将输出警告并将该连杆角度强制置零，避免后续计算出现 NaN。

Sources: decouple_atom01.cpp

雅可比矩阵：速度与力矩的微分同构

雅可比矩阵是实现速度级与力矩级解耦的核心。在并联机构中，足端点 C 的速度可表示为电机角速度的线性函数，同时关节角速度（足部的局部旋转）又与足端速度通过几何雅可比 J_q 关联。DecoupleAtom01::jacobian 的推导遵循标准的空间矢量法：

1. 空间速度映射矩阵 J_x：将足端线速度与角速度映射到两组连杆的瞬时运动学约束，尺寸为 2×6。
2. 电机雅可比 J_theta：描述电机角速度对连杆长度变化率的贡献，为 2×2 对角矩阵，其元素由摇臂矢量与连杆矢量的叉积在驱动轴方向的投影决定。
3. 关节雅可比 J_q：将足部俯仰/横滚角速度映射为足端空间速度，为 6×2 矩阵。

通过链式法则，J_Temp = J_x * J_q 建立了关节角速度到足端约束速度的映射；而 J_theta 建立了电机角速度到同一约束空间的映射。由此可得双向雅可比：

• 关节→电机：J_Joint2motor = J_theta^{-1} * J_Temp
• 电机→关节：J_motor2Joint = J_Temp^{-1} * J_theta

两者在数学上近似互为逆矩阵，分别服务于动作下发与观测采集两个方向。力矩映射遵循虚功原理，通过雅可比的转置实现：tau_motor = J_motor2Joint^T * tau_joint。

Sources: decouple_atom01.cpp

正运动学：从电机角度到关节角度

与逆运动学不同，正运动学没有闭合解析解。DecoupleAtom01::forward_kinematics 采用牛顿-拉夫逊迭代法数值求解：给定电机角度参考值 thetaRef，通过不断修正关节角度猜测值 x_k，使得当前猜测下的逆运动学输出 THETA 与参考值之间的误差 f_error 收敛。

迭代更新规则为：

x_{k+1} = x_k + ALPHA * J_motor2Joint * (thetaRef - THETA(x_k))

其中 ALPHA = 0.5 为阻尼系数，用于抑制 overshoot；收敛容差 TOLERANCE = 1e-3，最大迭代次数 MAX_ITERATIONS = 100。算法利用 last_solution_ 映射缓存左右腿的最近一次收敛解作为迭代初值，在相邻控制周期内关节变化微小的前提下，通常可在数次迭代内收敛。若迭代过程中雅可比出现 NaN（例如机构处于奇异位形），函数立即返回错误标志 count = -1，上层调用方可据此进行安全保护。

Sources: decouple_atom01.cpp

软件架构与接口设计

闭链解耦模块采用抽象接口加具体实现的经典多态架构，便于未来扩展其他构型的机器人（如四连杆变体或 Stewart 平台足端）。

类层次结构

classDiagram
    class Decouple {
        <<abstract>>
        +get_forwardQVT(q, vel, tau, is_left) void
        +get_decoupleQVT(q, vel, tau, is_left) void
        +create(type) shared_ptr~Decouple~$
    }
    class DecoupleAtom01 {
        -links_left_: vector~LinkParamsAtom01~
        -links_right_: vector~LinkParamsAtom01~
        -last_solution_: map~bool, Vector2d~
        +get_forwardQVT(q, vel, tau, is_left) void
        +get_decoupleQVT(q, vel, tau, is_left) void
        -inverse_kinematics(roll, pitch, is_left) IKResultAtom01
        -jacobian(ik_result, pitch) JacobianResult
        -forward_kinematics(thetaRef, is_left) ForwardMappingResult
        -get_decouple(roll, pitch, is_left) pair~Vector2d, JacobianResult~
        -get_links(is_left) vector~LinkParamsAtom01~$
    }
    Decouple <|-- DecoupleAtom01

Decouple 基类仅暴露两个对称接口：get_forwardQVT 完成电机空间到关节空间的正向映射，get_decoupleQVT 完成关节空间到电机空间的逆向映射。两个接口均通过引用修改传入的 q、vel、tau 向量，实现零额外堆分配的高性能调用。

Sources: close_chain_mapping.hpp, decouple_atom01.hpp

工厂注册

Decouple::create 作为静态工厂方法，根据配置文件中的 type 字段实例化解耦器。当前仅支持 "atom01" 类型，若未来引入新机型，只需新增派生类并扩展工厂分支即可，无需修改 RobotInterface 的调用逻辑。

Sources: close_chain_mapping.cpp

控制回路中的数据流

在实时控制循环中，闭链解耦模块并非独立运行，而是嵌入 RobotInterface::apply_action、 RobotInterface::reset_joints 与 RobotInterface::refresh_joints 三条关键路径中。理解其数据流向对于调试足端姿态偏差或力矩异常至关重要。

观测通路：电机空间 → 关节空间

在 apply_action 与 refresh_joints 中，系统首先读取参与闭链的四个电机（左右腿各两个）的编码器位置、速度与电流。随后，针对左腿（is_left = true）与右腿（is_left = false）分别调用 get_forwardQVT：

1. 电机反馈向量 [motor_q1, motor_q2] 被传入作为 q 初值。
2. forward_kinematics 通过牛顿迭代求解对应的关节俯仰与横滚。
3. 速度通过 J_motor2Joint 线性映射为关节角速度。
4. 电流（经电流-力矩系数折算后的电机力矩）通过 J_Joint2motor^T 映射为关节力矩。

转换后的关节位置、速度、力矩被写回 joint_q_、joint_vel_、joint_tau_ 的对应索引位置，供上层推理节点作为观测输入。

Sources: robot_interface.cpp, robot_interface.cpp

动作通路：关节空间 → 电机空间

推理节点输出的策略目标为关节空间角度。apply_action 对闭链关节执行以下闭环控制流程：

1. 提取当前关节状态（已通过观测通路转换到关节空间）。
2. 计算 PD 控制力矩：tau_joint = kp * (action_joint - q_joint) + kd * (0 - vel_joint)。
3. 将关节力矩通过 get_decoupleQVT 转换为电机力矩指令。
4. 电机力矩指令覆盖 action 数组对应索引，最终通过 MIT 力矩控制模式下发。

值得注意的是，闭链电机在 apply_action 的底层 CAN/CANFD 报文组装阶段被特殊处理：普通电机发送的是位置指令（pos[slot] = signed_target），而闭链电机发送的是力矩指令（tau[slot] = signed_target），且 kp、kd 字段置零。这意味着闭链踝关节完全依赖上层解耦后的力矩闭环，而非电机内部的位置伺服。

Sources: robot_interface.cpp, robot_interface.cpp

复位通路与状态刷新

reset_joints 在机器人初始化或回零时被调用。由于默认姿态 joint_default_angle 定义在关节空间，必须通过 get_decoupleQVT 预先将其转换为电机空间角度，再作为电机位置指令下发。与运行时不同，复位阶段所有电机（包括闭链电机）均以半刚度 kp/2.5 执行位置伺服，逐步过渡到目标构型，避免刚性冲击。

Sources: robot_interface.cpp

refresh_joints 则用于服务调用触发的关节状态重校准，其逻辑与 apply_action 中的观测通路完全一致，确保人工标定或故障恢复后，关节空间状态与电机反馈保持同步。

Sources: robot_interface.cpp

配置与使用边界

闭链解耦的行为由 robot.yaml 中的三个关键配置项共同决定：

RobotInterface 在构造阶段解析上述配置，建立 close_chain_motor_idx_（电机数组索引）与 close_chain_joint_idx_（URDF 关节数组索引）的双向查找表。若配置文件中未指定 type，ankle_decouple_ 将被置为 nullptr，系统退化到纯串联关节模型，所有电机均按位置模式驱动。这一设计允许同一套软件在无闭链机构的机器人平台上直接复用。

Sources: robot_interface.cpp, robot.yaml

奇异位形与数值稳定性

在实际运行中，需特别注意以下边界条件：

1. 判别式为负：当足端姿态超出机构工作空间时，逆运动学判别式可能小于零。此时算法强制 theta_i = 0 并输出警告，可能导致足端瞬间失控。应在策略训练阶段通过关节限位约束避免此类姿态。
2. 雅可比奇异：在特定俯仰/横滚组合下，J_Temp 或 J_theta 可能接近奇异，导致 J_motor2Joint 元素发散。forward_kinematics 已内置 hasNaN 检测，但上层仍需保证 kp/kd 不会将电机推向不可达区域。
3. 迭代初值敏感：正运动学依赖 last_solution_ 缓存。若机器人在断电或急停后发生大幅关节偏移，首次迭代的初值可能远离真实解，导致收敛变慢或收敛到错误分支。refresh_joints 服务可在上电后手动触发，帮助重建缓存。

Sources: decouple_atom01.cpp, decouple_atom01.cpp

系统数据流全景

下图展示了从策略输出到电机指令的完整映射链，突出闭链解耦层在其中的枢纽作用：

flowchart LR
    subgraph Inference["推理节点（关节空间）"]
        ONNX["ONNX 策略输出"]
        PD["PD 控制器<br/>tau = kp*(action-q) + kd*(0-vel)"]
    end
    
    subgraph DecoupleLayer["闭链解耦层"]
        DEC["get_decoupleQVT<br/>关节力矩 → 电机力矩"]
        FWD["get_forwardQVT<br/>电机角度 → 关节角度"]
    end
    
    subgraph Motor["电机驱动层"]
        MIT["MIT 力矩模式<br/>闭链电机"]
        POS["MIT 位置模式<br/>普通电机"]
    end
    
    ONNX -->|action| PD
    PD -->|tau_joint| DEC
    DEC -->|tau_motor| MIT
    MIT -->|编码器反馈| FWD
    FWD -->|q, vel, tau| PD
    FWD -->|观测| ONNX

从图中可见，闭链解耦层实际上构成了一个双向坐标变换器：观测方向上它将电机的非线性约束映射为策略可理解的独立关节状态；动作方向上将策略生成的独立关节力矩重新耦合为电机需要执行的协作力矩。这一双向映射的精度直接决定了强化学习策略在物理机器人上的迁移效果。

Sources: robot_interface.hpp

延伸阅读与上下游关联

闭链运动学模块并非孤立存在。若要完整理解机器人控制链路，建议按以下顺序继续阅读：

• 如需了解电机如何接收力矩/位置指令并执行底层伺服，请参考 电机驱动模块与 MIT 控制。
• 如需理解 URDF 关节索引与电机 ID 的完整映射规则，请参考 URDF 与电机映射关系。
• 如需查看推理节点如何将解耦后的关节状态组装为策略观测向量，请参考 观测堆叠与多策略切换。
• 如需修改连杆几何参数以适配新机型，请同步阅读 配置文件系统详解 中关于 robot.yaml 的字段规范。