机械臂 VR 遥操作调试日志记录

硬件：RK3588 (Orange Pi 5 plus)，Meta Quest 2，6-DoF 步进臂 (STM32F4 + CAN)

软件栈：ROS 2 Humble，OpenXR NDK C++，Foxglove Studio

本文记录真实踩坑过程，每一条都在硬件上摔过跤。

0. 项目概览

目标：用 Meta Quest 2 手柄实时遥操作一台六自由度机械臂，同时通过 Orbbec 深度相机把第一视角视频回传到 Quest 显示。

资源清单

主控：Orange Pi 5 Plus（RK3588，Ubuntu 22.04 + ROS 2 Humble）
VR：Meta Quest 2，OpenXR 原生 C++ NDK 开发（无 Unity）
机械臂：STM32F4 主控，步进电机 + CAN 总线，末端夹爪（CAN ID = 7）
相机：Orbbec Gemini 336L RGB-D，GStreamer 推流

ROS2节点拓扑

bash 复制代码

Quest 2 (OpenXR NDK)
      │  UDP 9000  HandPacket (位置/朝向/按键/trigger)
      ▼
quest_bridge_node
      │  /quest/hand  (HandPacket.msg)
      ▼
hand_servo_node  ──→  /robot/joint_stream_raw (>J1..J6,spd)
      │                /robot/joint_target (Float32MultiArray)
      ▼
robot_serial_driver_node
      │  ttyACM0 / 115200
      ▼
STM32 固件 ←→ CAN ←→ 步进驱动器 + 夹爪电机

关键 DH 参数 & 关节限位

bash 复制代码

DOF6Kinematic(0.1265, 0.035, 0.146, 0.117, 0.052, 0.0755)  // 单位 m

J1: -170° ~ +170°   J2: -75° ~ +90°    J3: 35° ~ +180°  ← < 35° 为机械干涉区！
J4: -180° ~ +180°   J5: -100° ~ +120°  J6: -180° ~ +180°

HOME 固件: {0, 0, 90, 0, 0, 0}
REST 代码: {0, -73, 180, 0, 0, 0}

第一章：串口驱动层（非时间顺序，按照问题分类）

1.1 死锁：辅助函数在已加锁上下文中二次加锁

现象：机械臂偶发完全失联，串口节点 hang 住，只有重启才能恢复。

根因：robot_serial_driver_node 中有一个辅助函数 ctrl_fail_all_waiters_locked_()，函数名带 _locked_ 本意是提示"调用者已持有锁"，但函数体内部又执行了 std::lock_guard<std::mutex> lk(ctrl_mtx_)，形成自锁死锁。

bash 复制代码

// 有问题的原始代码
void ctrl_fail_all_waiters_locked_(const char* reason) {
    std::lock_guard<std::mutex> lk(ctrl_mtx_); // 内部重复加锁
    for (auto& w : waiters_) w.set_exception(...);
}
// 调用处已经持有ctrl_mtx_，再次lock就会死锁

修复：将 ctrl_mtx_ 改为 std::recursive_mutex，或将函数体拆分为不加锁的 _nolock 版本，保证调用链只持有一次锁。

1.2 串口轮询与发送线程锁竞争导致指令丢失

现象：机械臂动作明显滞后，有时连续两条指令只执行一条。

根因：poll_tick（ROS2的timer线程，10 Hz 发 #GETJPOS）和 tx_loop（独立线程，负责关节指令队列）都写同一个串口，靠 serial_tx_mtx_ 互斥。两个线程争锁时关节指令被迫等待，固件侧 FIFO 最多 16 条，积压后新指令直接被丢弃。

修复：取消GETLPOS轮询，只保留GETJPOS（10 Hz），tx_loop 独占发送权，彻底消除锁竞争。最后修改为为三优先级发送架构（见第十二章）。

1.3 串口重连机制缺失

现象：USB 串口掉线后节点进入connected_=false状态，永远不自动恢复，必须 Ctrl+C 重启。

修复：加看门狗定时器，定期尝试重新 open() 串口，连接成功后重发 !START 恢复状态机。

第二章：IK 解算层的问题（最严重）

最初依赖固件做IK，后面还是决定把IK完全移植到ROS2端，原因是固件修改起来太麻烦了。

2.1 早期方案：依赖固件路径指令（LPOS）

现象：机械臂能动，但末端轨迹走弧线，部分姿态无响应，速度无法连续控制。

根因：早期通过 !LPOS x,y,z 将笛卡尔目标发给固件，固件内部再做 IK。这条路径有三个致命问题：

固件 IK 是阻塞式的，每条指令要等运动完成才处理下一条，遥操作 100 Hz 完全无法跟随。
COMMAND_CONTINUES_TRAJECTORY 与 COMMAND_TARGET_POINT_SEQUENTIAL 共用一段 case 代码（switch-case 穿透 bug），导致每条轨迹指令都阻塞。
固件没有暴露 IK 解族选择接口，机械臂越过奇异点时固件自行切换解族，外部毫无感知。

修复：彻底放弃 LPOS 路径，把 IK 完全移植到 ROS 2 端，计算好六个关节角后通过 >J1,J2,J3,J4,J5,J6,spd 直接发关节指令，固件只负责执行。

2.2 IK 代码从固件到ROS2端的移植修改

现象：dof6_kinematic.cpp 直接从STM32固件移植，需要进行一些修改。

根因：固件代码包含 arm_math.h，STM32 HAL 头文件，FreeRTOS 类型及 osDelay()。

修复：

arm_sin_f32 / arm_cos_f32 → 直接用 sinf / cosf
去掉所有 osDelay、RTOS 类型依赖
CMakeLists.txt 中将 dof6_kinematic.cpp 加入 target_sources

2.3 IK 解跳变导致机械臂乱动（最重要的 bug）

现象：遥操作过程中机械臂运行平稳一段时间后突然剧烈抖动，J4 轴像被电击，有时整臂乱甩。

阶段一（错误方向） ：怀疑是轴映射参数错误导致目标点跳变，反复修改 axis_rx_src/ry_src/rz_src，改了十几次都没解决。

阶段二：解族截断 bug

bash 复制代码

select_solution() 中有一处 i < 4 的截断：

// 错误代码（只搜索前 4 个解，遗漏elbow-up族）
for (int i = 0; i < 4; i++) {    // 应该是 i < 8
    if (solutions[i].valid && cost(solutions[i]) < best_cost)
        best = &solutions[i];
}

当机械臂运动到某些姿态时最优解恰好落在 index 4～7，被截断后只能拿到次优解，造成关节角突跳 90°+。

修复：将 i < 4 改为 i < 8，恢复完整 8 解搜索。

阶段三（真正根因）：on_jpos 回调破坏 IK 种子

这才是关节持续跳变的核心 bug，极其隐蔽：

cpp 复制代码

// 追踪期间 JPOS 反馈无条件覆盖 cur_joints_（IK 种子）
void on_jpos(const Float32MultiArray::SharedPtr msg) {
    for (int i = 0; i < 6; i++)
        cur_joints_.a[i] = msg->data[i];  // 无条件覆盖
}

时序分析：

Quest 回调 100 Hz 运行，每帧 cur_joints_ = *best_IK_solution（IK 种子连续传递）
100 ms 后 on_jpos 触发（10 Hz 串口反馈），强制把 cur_joints_ 覆盖为硬件实际位置
硬件位置由于电机延迟比 IK 计算落后约 2-5 帧，造成种子突变
下一帧 IK 以突变后的种子为起点，select_solution 选到不同解族，J4/J5 跳变 60°+
机械臂乱走，种子与真实位置偏差越来越大，正反馈发散

修复：

cpp 复制代码

//追踪期间不碰 IK 种子
void on_jpos(const Float32MultiArray::SharedPtr msg) {
    for (int i = 0; i < 6; i++)
        real_joints_.a[i] = msg->data[i];
    real_joints_valid_ = true;
    if (!is_tracking_) {              //追踪期间种子只由 IK 结果更新
        for (int i = 0; i < 6; i++)
            cur_joints_.a[i] = msg->data[i];
    }
}

2.4 wrap()函数 bug：π写成了2π

现象：偶发关节角计算偏差，尤其 J4 在大范围旋转时出错。

根因：固件移植时带入了一个角度归一化函数错误：

cpp 复制代码

// if 应改为 while，减量应为 2π 而非 π
float wrap(float v) {
    if (v > M_PI)  return v - M_PI;
    if (v < -M_PI) return v + M_PI;
    return v;
}

// 正确版本
float wrap(float v) {
    while (v > M_PI)  v -= 2.f * M_PI;
    while (v < -M_PI) v += 2.f * M_PI;
    return v;
}

固件上这个 bug 跑了很久没触发，是因为正常工作范围内关节角几乎不超过 ±π。移植后保留，属于低优先级待修项。

第三章：Quest 坐标轴映射的坑

3.1 坐标系定义

bash 复制代码

Quest OpenXR world space（右手系）：
  +X = 右
  +Y = 上（反重力方向）
  -Z = 用户正前方（玩家面向方向）

机械臂 base frame（DH 定义）：
  +X = 臂前方（末端伸出方向）
  +Y = 臂左侧
  +Z = 向上

两个坐标系没有任何一个轴天然对齐，需要手动映射。

3.2 后续引入 double-negative 符号错误

现象：部署后机械臂 X 轴方向完全反向，向前推手柄，臂向后退。

根因：是硬编码的正确映射，重构参数化时符号取反了两次：

bash 复制代码

//正确：
tx = anc_rx_ + (-delta_qz) * scale;  // Quest -Z 方向 → robot +X

// 错误：参数化后引入双重取反
tx = anc_rx_ + axis_rx_sign * (-dqz) * scale;
// axis_rx_sign = -1，dqz 本身已是 Quest Z 差量
// 结果：(-1) * (-dqz) = +dqz，符号反了

修复：回退到之前的硬编码映射，逐轴确认符号后再参数化。

3.3 Quest play space 旋转导致轴映射混乱

现象：调整好一个轴，另一个轴又乱了；有时手前后移动，机械臂左右动。

根因：Quest play space 的朝向取决于 Guardian 设置时用户面朝的方向，并非固定。如果用户设置 Guardian 时的朝向与站在机械臂前时不一致，整个水平面坐标系就旋转了。

诊断方法 ：订阅 /robot/servo/raw，每次只移动一个轴，记录 dq[0]/dq[1]/dq[2] 的主响应轴：

bash 复制代码

ros2 topic echo /robot/servo/raw
# 输出：dq[0]=Quest_X  dq[1]=Quest_Y  dq[2]=Quest_Z  target_x  target_y  target_z
#
# 测试步骤：
# 1. 缓慢向前推手 → 观察哪个 dq[i] 变化最大
# 2. 缓慢向左移手 → 同上
# 3. 缓慢向上抬手 → 同上
# 根据主轴对应关系填写 yaml

经过多轮实测，最终确认的正确配置：

bash 复制代码

axis_rx_src: 2       # Quest Z → 机械臂前后 (robot X)
axis_rx_sign: -1.0   # Quest -Z（前方）→ robot +X（臂前）
axis_ry_src: 0       # Quest X → 机械臂左右 (robot Y)
axis_ry_sign: 1.0
axis_rz_src: 1       # Quest Y → 机械臂上下 (robot Z)
axis_rz_sign: 1.0

3.4 各轴灵敏度不均匀

现象：scale_pos=1000 时，前后方向灵敏度 2.6 mm/cm，左右方向仅 0.7 mm/cm。

根因：Quest 手柄在 X 轴方向（左右）的物理运动范围受肩关节限制，天然比 Z 轴（前后）小得多。单一 scale_pos 无法处理各轴灵敏度差异。

修复：拆分为三个独立参数：

复制代码

scale_rx: 2000.0   # 前后方向
scale_ry: 6000.0   # 左右方向（灵敏度天然低，需要大幅放大）
scale_rz: 800.0    # 上下方向（手腕敏感，适当降低）

第四章：Z 轴方向随位置反转的奇异点问题

根因在物理层面。

4.1 现象

机械臂在 HOME 位置以上运动时，手上移 → 臂上移（正常）；到了 HOME 以下，手上移 → 臂下移（反向）。方向反转的边界恰好是 HOME 高度（Z ≈ 325 mm）。

4.2 排查过程

花了大量时间在代码层面找：

检查 axis_rz_sign 符号------正确
检查 anc_rz_ 锚点更新逻辑------正确
回退 v8/v9/v10/v11 各版本对比------现象一致
怀疑 alpha_pos 滤波导致锚点漂移------排除

前后换了很多版本，问题始终复现，说明不是某次改动引入的，而是系统固有特性。

4.3 根因：IK 在 HOME 附近存在奇异点，解族切换

机械臂 HOME 姿态 {0,0,90,0,0,0} 是一个特殊构型：J3=90° 时，J2 与 J3 的旋转轴几乎共线，这个区域在数学上是 IK 奇异点附近。当末端从 HOME 往下运动时，IK 自动切换到了另一个解族（elbow-down 变 elbow-up，或反之），J2、J3 符号整体翻转，导致相同的末端目标位移在不同解族下对应完全相反的关节变化方向，在 Cartesian 空间看起来就是"Z 轴反转"。

4.4 修复方案

方案一（软限位） ：在 hand_servo_node 中限制末端 Z 坐标不低于 HOME 以下某个阈值：

cpp 复制代码

float z_min = home_pose_z_ - 0.06f;  // 低于 HOME 60mm 开始限制
if (target_z < z_min) target_z = z_min;

方案二（解族锁定，最终采用） ：在 select_solution() 中强制只选 elbow-up 解族：

cpp 复制代码

// yaml 参数
elbow_up_min_deg: 35.0   // J3 必须 >= 35°（正值 = elbow-up）

// select_solution() 中增加过滤
if (sol.a[2] < elbow_up_min_deg) continue;  // 跳过 elbow-down 解

elbow_up_min_deg 从 45° 调整到 35° 以扩大可达工作空间。

第五章：多节点冲突与仿真调试

5.1 两套控制节点同时启动

现象：加了 sim:=true 参数后，仿真模式下机械臂模型完全不动。

根因：robot_viz.launch.py 和 bringup.launch.py 都启动了 hand_servo_node 和 quest_bridge_node，同一 namespace 下出现两个同名节点，指令相互覆盖，模型不动。

修复：robot_viz.launch.py 只保留可视化节点（robot_state_publisher、joint_state_bridge、foxglove_bridge），控制节点的启动权完全交给 bringup.launch.py。

5.2 /joint_states 话题从未发布

现象：robot_state_publisher 正常运行，Foxglove 中模型 T-pose 静止不动。

根因：仿真模式下 hand_servo_node 发布 /robot/joint_target（关节角，度），robot_state_publisher 需要 /joint_states（弧度，sensor_msgs/JointState），中间缺少桥接节点。

修复：实现 joint_state_bridge_node，订阅 /robot/joint_target，转换度→弧度后发布 /joint_states：

bash 复制代码

优先级：/robot/joint_target（仿真，100 Hz）> /robot/joint_angles（真实反馈，10 Hz）
joint_target 超过 1 秒无更新 → 退出仿真优先模式，转用真实反馈
两者都没有 → 显示 REST_POSE 静止

5.3 yaml 参数未加载

现象：修改了 robot_params.yaml 里的参数，重启后行为完全没变化。

根因：launch 文件没有正确传递 parameters=[params_file]，节点用的全是代码里的 declare_parameter 默认值。

验证方法：

bash 复制代码

ros2 param get /hand_servo_node scale_ry
# 输出是代码默认值（如 1.0）→ yaml 没加载
# 输出是 yaml 里的值（如 6000.0）→ 加载成功

第六章：Quest 端数据结构 bug

6.1 UDP 结构体字段顺序不对齐：X/Y 始终为 0

现象：/robot/servo/raw 显示 dq[0]=0.0000, dq[1]=0.0000，只有 dq[2]（Z 轴）有数据，不是噪声，是真的 0。

根因：Quest 端发送的结构体字段顺序与 bridge 端解析的顺序不一致：

bash 复制代码

// Quest 端发送顺序
struct HandPacket { float z; float x; float y; float qx; float qy; float qz; float qw; };

// Bridge 端接收顺序（错误）
struct HandPacketV2Wire { float x; float y; float z; float qx; float qy; float qz; float qw; };

结果：bridge 把 Quest 发来的 z 当 x 读，Quest X 轴和 Y 轴数据被赋给了错误的字段。而 Quest Z 轴（向用户方向）恰好运动幅度最大，掩盖了问题。

修复：对齐两端结构体定义，按字段名而非偏移量解包。

6.2 A/B/X/Y 按键数据未写入 UDP 包

现象：手柄按下 A 键后ROS2端毫无响应。

根因：Quest 端 OpenXR 代码注册了按键 Action，但每帧读取状态后没有写入 UDP 发送结构体。

修复：在 rk_server.cpp 里注册 XrActionStateBoolean，每帧读取后打包进 HandPacket.buttons 字段发送。

第七章：状态机与按键防抖

7.1 ABXY 状态机定义

bash 复制代码

DISABLED 状态：
  A 键 → send_control("START") → arm_enabled_=true → 切换到 ENABLED
  B/X/Y → 忽略

ENABLED 状态：
  A 键 → 切换 START/STOP
  B 键 → send_control("HOME") → 机械臂回 {0, 0, 90, 0, 0, 0}
  X 键 → send_control("RESET") → 机械臂回 {0, -73, 180, 0, 0, 0}
  Y 键 → send_control("DISABLE") → arm_enabled_=false → 回 DISABLED
  squeeze > 0.80 且已释放 → engage()（开始跟手）
  squeeze < 0.20           → disengage()（停止跟手）

7.2 按键抖动

现象：单次按键触发 2-3 次动作，机械臂反复进出 HOME 位置。

根因：ROS 2 timer 以 100 Hz 检测按键状态，一次物理按下持续约300 ms，会被采样30次。

修复：加按键冷却时间 btn_cooldown_ms = 800 ms，上升沿触发后进入冷却，冷却期间忽略同一按键。

第八章：夹爪集成的坑

8.1 J6 被 gripper_angle 覆盖导致抖动加剧

现象：机械臂跟手越来越差，关节抖动随时间加剧，尤其 J4/J5 不稳定。

根因：发送关节指令时，J6 被 gripper_angle_（trigger 值，0°～25°）覆盖，而 IK 种子 cur_joints_.a[5] 仍然保存着 IK 算出的值（可能是 -45°）：

bash 复制代码

//错误：发给电机的 J6 ≠ IK 种子里的 J6
ss << best->a[4] << ","
   << gripper_angle_ << ","  // ← 发送 5°（trigger 值）
   << spd;
cur_joints_ = *best;  // cur_joints_.a[5] = -45°（IK 算的）

// 第 2 帧：IK 种子 J6 = -45°，电机实际 J6 = 5°
// 偏差 50°，IK 被迫调整 J4/J5 来补偿 → 抖动

修复：发完指令后立即把种子 J6 同步为 gripper_angle_：

bash 复制代码

//修复
cur_joints_ = *best;
cur_joints_.a[5] = gripper_angle_;  // 种子 J6 = 夹爪实际角度

8.2 StepHand 夹爪指令格式

StepHand 夹爪（CAN ID = 7）通过串口透传 CAN，指令格式与关节电机不同：

bash 复制代码

关节指令（6 轴电机）：>J1,J2,J3,J4,J5,J6,speed
夹爪指令（StepHand）：@7,angle    // angle 范围 ±115°
                      // 0° = 全开，25° = 完全闭合（根据实物标定）

第九章：Foxglove 可视化与 URDF 调试

9.1 URDF 关节轴方向多次错误

现象：Foxglove 中模型关节运动方向与真实机械臂相反，或者运动轴不对。

根因：URDF 中 <axis xyz="..." /> 需要与 DH 参数约定的关节轴方向精确匹配。

经过实测逐轴确认的正确配置：

bash 复制代码

joint1: axis="0 0 1"    ← Z 轴，底座旋转
joint2: axis="0 1 0"    ← Y 轴，肩关节 pitch
joint3: axis="0 1 0"    ← Y 轴，肘关节 pitch
joint4: axis="0 0 -1"   ← 负 Z 轴！前臂 roll（注意负号）
joint5: axis="0 -1 0"   ← 负 Y 轴！腕关节 pitch（注意负号）
joint6: axis="0 0 1"    ← Z 轴，末端 roll

joint4 和 joint5 的负号是这个机械臂 DH 定义的特殊之处，非常容易漏掉

9.2 Foxglove WebSocket 延迟

现象：某次测试后 Foxglove 网页端出现 1-2 秒延迟，本地 ros2 topic hz 显示 10 Hz 正常。

根因：订阅了相机话题（1280×720@30fps），推流数据量迅速撑满 send_buffer_limit（10 MB），所有话题跟着排队。

修复：取消相机话题订阅，或强制刷新 Foxglove 标签页（Ctrl+Shift+R）清除积压。实际使用 rviz2 本地可视化效果更稳定，完全零延迟。

第十章：后半段调试记录

10.1 左右运动带来的前后串扰（跨轴耦合）

现象：手柄向左移动时机械臂不只左移，同时向前伸出约 30%；向右移动串扰相对较小，左右不对称，与灵敏度调节无关。

根因：人体肩关节的物理耦合。手臂向左平移时，肩关节旋转必然带动上臂轻微前伸，Quest在X 轴方向的读数会随 Z 轴运动产生约 -30% 的联动分量。这是人体运动学固有特性，不是软件 bug。

通过 /robot/servo/raw 可量化观察：

bash 复制代码

向左移手时：
  dq[0](Quest X) = +0.031   ← 前后轴出现串扰分量
  dq[2](Quest Z) = +0.102   ← 左移主轴

修复：在前后轴计算时减去左右轴的耦合量：

bash 复制代码

// 去耦公式：前后轴 = 原始前后分量 - 左右分量 × 耦合系数
const double dq_rx = dq[ax_src_[0]] + cross_ry_rx_ * dq[ax_src_[1]];

通过实测标定，cross_ry_to_rx 约为 0.30（向左实测 0.29，向右约 0.32，取保守值）。同时大幅降低前后灵敏度：

bash 复制代码

cross_ry_to_rx: 0.30
scale_rx: 400.0   # 从 2000 降至 400，进一步压制残余串扰

前后串扰降低，左右运动体验改善

10.2 夹爪"永远只开一半"------相对编码器零点漂移

这是整个调试过程中持续时间最长、最迷惑的子问题，先后尝试了低通滤波，死区调整等若干方向，最终定位到固件层面的根本原因

10.2.1 现象

按 START（A 键）后夹爪打开，但只开到约一半，不是最大量程
完全闭合后再松开，夹爪开到最大，随后缓缓合拢
每次 DISABLE 后重新 START，行为完全一样

10.2.2 错误的排查方向

最初认为是 lp_trigger_ 低通滤波器的残留值导致 START 后立即发送了非零的 HAND_POS 命令：

bash 复制代码

// 尝试：在 START 时重置滤波器
lp_trigger_.reset(0.0);
// → 没有任何帮助，现象完全一样

随后将问题归结为 trigger 自然搭触的读数（约 0.25）高于死区阈值，调整死区到 0.30、0.40，加 snap-to-zero 逻辑，均未根本解决。

10.2.3 真正根因：CtrlStep步进电机使用相对编码器，上电零点即当前物理位置

bash 复制代码

上电后：编码器复位，当前物理位置 = 0°
!HAND_POS 0   → SetAngleWithVelocityLimit(OpenedAngle = -115°)
               → "从上电零点往 -115° 走"
!HAND_POS 100 → SetAngleWithVelocityLimit(ClosedAngle = +115°)
               → "从上电零点往 +115° 走"

用户断电时夹爪处于完全闭合状态，下次上电后编码器复位，物理闭合 = 编码器 0°：

上电物理位置	`!HAND_POS 0` 实际效果	`!HAND_POS 100` 实际效果
闭合（编码器 0°）	走到 0°-115° = 物理中间位置（不是全开）	走到 0°+115° → 撞限位 → 全闭
全开（编码器已在全开处）	走 -115° → 撞限位 → 全开	走 +115° → 物理中间位置（不是全闭）

这完全解释了为什么START 后只开一半------命令是正确的，但电机的零点参考是错的。

10.2.4 固件修复：扩大 OpenedAngle 覆盖全范围

物理全量程约 230°，从闭合状态出发要走完全程需要 -230°：

bash 复制代码

// dummy_robot.h
// 修改前
StepHand hand { .OpenedAngle = -115.0f, .ClosedAngle = 115.0f, ... };

// 修改后（上电位置固定为闭合时）
StepHand hand { .OpenedAngle = -230.0f, .ClosedAngle = 0.0f, ... };
// CtrlStepMotor 限位同步修改：(-230, 115)，留 115° 裕量防闭合时过冲

修改后：

!HAND_POS 0 → 走 -230°，必然越过物理全开限位撞停 → 全开
!HAND_POS 100 → 走到 0° = 上电闭合位置 → 全闭

10.2.5 ROS 2 侧同步修复：trigger 死区重映射

即使固件修正了零点，trigger 自然搭触值（约 0.25）仍会导致 !HAND_POS 25，夹爪在全开后缓慢合拢。最终采用死区加重映射：

bash 复制代码

float raw = msg->r_trigger;
float pct_f = 0.0f;
if (raw > trigger_dz_) {   // trigger_dz_ = 0.25（yaml 可调）
    pct_f = (raw - trigger_dz_) / (1.0f - trigger_dz_);
}
gripper_target_pct_ = std::clamp((int)roundf(pct_f * 100.0f), 0, 100);

死区内（手指自然搭触）视为完全松开 = 0%，超出死区后重新映射到 [0%, 100%]，保证完全按下仍是 100% 闭合。maybe_send_gripper()以rate-limit发送：变化 ≥3% 或距上次发送 ≥200ms 且变化 ≥1% 时才发指令，避免高频串口污染。

10.3 夹爪失能问题：DISABLE 后电机持续锁死、过热

现象：按 DISABLE 后夹爪完全闭合，但电机持续上电保持锁死力矩，发出电流声，外壳过热。

根因：固件!DISABLE 命令只调用了 dummy.SetEnable(false)（失能六轴关节电机），未对夹爪电机（hand2）调用 SetEnable(false)，夹爪 CtrlStep 电机始终保持位置闭环，闭合状态下持续输出保持力矩。

修复：在固件 DISABLE 处理器中集成完整夹爪关闭序列：

cpp 复制代码

// ascii_protocol.cpp --- DISABLE handler
dummy.SetEnable(false);                      // 立即失能机械臂各轴
if (dummy.hand2) {
    dummy.hand2->SetPercentToAngle(100.0f);  // 发出闭合指令
    osDelay(4000);                           // 等待夹爪运动完成
    dummy.hand2->SetEnable(false);           // 失能夹爪电机
}
Respond(_responseChannel, "Disabled ok");

// START handler
dummy.SetEnable(true);
if (dummy.hand2) {
    dummy.hand2->SetEnable(true);
    dummy.hand2->SetPercentToAngle(0.0f);   // 张开到最大
}
Respond(_responseChannel, "Started ok");

注意 osDelay 风险 ：如果 USB CDC 处理函数在中断上下文中被调用（而非 FreeRTOS 任务），osDelay(4000) 会触发 HardFault，导致 STM32 崩溃，USB 设备消失。若遇到"运动过程中串口突然断开"，需检查 OnUsbAsciiCmd 的调用上下文。

10.4 UART4 夹爪命令静默失效

现象：ROS 2 侧的 !HAND_POS N、!HAND_EN、!HAND_DIS 命令发出后夹爪完全没有响应，但通过 USB CDC 手动发送同样指令可以正常执行。

根因：ROS 2 robot_serial_driver_node 通过 ttyACM0（USB CDC）与固件通信，对应 OnUsbAsciiCmd。而 OnUart4AsciiCmd（UART4 物理串口路径）里缺少完整的 !HAND_* case，命令进入 ! 分支后没有匹配项，静默返回。

影响范围：如果部署时改为 UART 物理串口连接，所有夹爪控制将完全失效。

修复：在 OnUart4AsciiCmd 的 ! 处理块中镜像添加完整的 !HAND_* 处理逻辑。

10.5 手腕偏航（Yaw）追踪的实现

动机：原有控制只追踪手柄的三维平移，朝向在 engage 时锁死。实际操作中用户向低处够取物品时，手腕自然下翻，但夹爪始终朝 engage 时的方向，需要多次脱离重新 engage，操作效率极低。

实现方案 ：engage 时记录控制器四元数为基准 anc_q_，每帧计算相对 engage 时刻的旋转增量：

cpp 复制代码

// engage 时
anc_q_ = tf2::Quaternion(orientation.x, y, z, w);
lp_yaw_.reset(0.0);

// 追踪中，每帧
tf2::Quaternion dq = q_cur * anc_q_.inverse();
if (dq.w() < 0.0)
    dq = tf2::Quaternion(-dq.x(), -dq.y(), -dq.z(), -dq.w());  // 取最短路径

// 提取 Quest Y 轴旋转分量（手腕左右转 = 绕竖直轴旋转）
double raw_yaw = scale_yaw_ * dq.getAxis().y() * dq.getAngle();
if (std::abs(raw_yaw) < 0.017) raw_yaw = 0.0;    // 1° 死区，抑制追踪噪声
raw_yaw = std::clamp(raw_yaw, -1.047, 1.047);     // ±60° 硬限，防止 IK 无解
double dyaw = lp_yaw_.filter(raw_yaw, 0.7);

// 叠加到 engage 时的末端姿态
tf2::Quaternion dq_arm;
dq_arm.setRPY(0.0, 0.0, dyaw);
target.R = dR * anc_R_;

为何用 getAxis().y() * getAngle() 而非 RPY 分解 ：对于小角度旋转，这等价于旋转向量的 Y 分量，计算开销低，且当 Quest Y 轴（物理竖直轴）为主旋转轴时误差很小。正式场景可换用 getRPY() 进行更严格的轴分离。

调试发现的编译问题：

cpp 复制代码

//tf2::Quaternion 的 operator- 存在歧义
dq = -dq;
dq = tf2::Quaternion(-dq.x(), -dq.y(), -dq.z(), -dq.w());

效果：夹爪偏航跟随手腕旋转，抓取角度控制自由度从 0 提升到 1，操作灵活性显著提升。

10.6 Pitch 轴实现与稳定性问题

在 Yaw 验证通过后尝试加入俯仰（Pitch）追踪：

cpp 复制代码

double raw_pitch = scale_pitch_ * dq.getAxis().x() * dq.getAngle();
raw_pitch = std::clamp(raw_pitch, -1.047, 1.047);
double dpitch = lp_pitch_.filter(raw_pitch, 0.7);
dq_arm.setRPY(0.0, dpitch, dyaw);

问题：启用 scale_pitch: 1.0 后出现两个问题：

臂灵敏度下降 ：IK 由原来的位置+Yaw 两约束变为位置+Yaw+Pitch 三约束，可解空间缩小，ik_fail_ 计数上升，帧跳频增加，操作手感变迟钝。
串口断开频率上升：Quest X 轴存在持续高频噪声（比 Y 轴噪声大），即使不主动旋转手腕，Pitch 约束也在每帧微变，IK 每帧都在寻找新的三约束解，关节微调量增大，电机电流持续波动，USB 5V 供电出现瞬间压降触发 USB 重置。

处置：默认禁用 Pitch：

bash 复制代码

scale_yaw:   1.0   # 已验证，保持开启
scale_pitch: 0.0   # 默认关闭，确认轴方向后改 1.0 或 -1.0

教训：每新增一个旋转自由度，IK 可解空间收缩是确定性影响，不适合直接以 scale=1 默认开启，应以 0 为默认值，要求操作者明确选择。

10.7 IK 解的"非最短路径"问题

现象：理论上手腕左右偏转只需 J5（腕关节）转动，但实际 J4（前臂 roll）有时也参与，动作冗余。

根因：select_solution() 用加权关节距离评分选最优解：

cpp 复制代码

float score = 0;
for (int k = 0; k < 6; k++) {
    float d = j.a[k] - cur_joints_.a[k];
    score += IK_WEIGHTS[k] * d * d;
}
// IK_WEIGHTS = {3.0, 1.5, 1.5, 1.0, 1.0, 0.5}
// J4 与 J5 权重相同（均为 1.0）

在某些手臂构型下，"J4 转少 + J5 转多"和"J4 转多 + J5 转少"两种解的加权距离得分非常接近，小幅 Quest 噪声就能让评分在两者之间来回切换，产生 J4 参与的冗余运动。

解决方向：

提高 J5 权重（1.5）、降低 J4 权重（0.8），引导选解偏向 J5
或在 select_solution 加入解族连续性约束（当前解族不变除非得分差距超过阈值）

第十一章：ROS 2 代码架构解析

11.1 整体节点拓扑

复制代码

Meta Quest 2 (OpenXR NDK App)
      │  UDP 9000  HandPacket (位置/朝向/按键/trigger)
      ▼
quest_bridge_node
      │  /quest/hand  (HandPacket.msg, best_effort QoS)
      ▼
hand_servo_node
      │  /robot/joint_stream_raw  (String, QoS1)      → robot_serial_driver_node
      │  /robot/joint_target      (Float32MultiArray)  → joint_state_bridge_node
      │  /robot/servo/status      (String, debug)
      │  /robot/servo/raw         (String, debug)
      ▼
robot_serial_driver_node
      │  ttyACM0 / 115200
      ▼
STM32 固件 ←→ CAN 总线
      │
      ├── 6× 步进电机驱动器 (J1~J6)
      └── StepHand 夹爪 (CAN ID=7)

joint_state_bridge_node
      │  /joint_states (sensor_msgs/JointState, 弧度)
      ▼
robot_state_publisher → Foxglove Studio (ws://IP:8765)

11.2 hand_servo_node 内部结构

这是整个系统的核心节点，承担状态机管理、IK 解算、轴映射、夹爪控制四个职责。

11.2.1 主回调 on_hand() 执行流

复制代码

on_hand(HandPacket)
  │
  ├─ [1] 按键处理（上升沿检测 + 800ms 冷却）
  │       Y 键 → send_control("DISABLE") → arm_enabled_=false → disengage()
  │       [DISABLED] A 键 → send_control("START") → arm_enabled_=true
  │       [ENABLED]  B 键 → send_control("HOME")
  │                  X 键 → send_control("RESET")
  │
  ├─ [2] arm_enabled_==false → return（DISABLED 状态不发任何运动命令）
  │
  ├─ [3] 夹爪控制
  │       trigger 死区重映射 → gripper_target_pct_
  │       maybe_send_gripper()  ← rate-limited（变化≥3% 或 200ms+变化≥1%）
  │
  ├─ [4] squeeze 跟手状态转换
  │       squeeze > 0.80 且已释放 → engage()（记录锚点，重置滤波器）
  │       squeeze < 0.20          → disengage()
  │
  └─ [5] IK 跟踪（仅在 is_tracking_ 时执行）
          │
          ├─ Quest 位移增量 × scale_rx/ry/rz + 跨轴去耦（cross_ry_to_rx）
          ├─ 四元数增量 → Yaw 提取（低通 + ±60° 硬限）
          ├─ 构建 Pose6D target（位置 + 旋转矩阵）
          ├─ kin_.SolveIK() → 最多 8 个解
          ├─ select_solution() → 加权最小关节距离评分
          │       约束：关节限位 + J3 ≥ elbow_up_min_deg（防 elbow-down 切换）
          └─ 发布 joint_stream_raw + joint_target

11.2.2 engage/disengage 机制

cpp 复制代码

void engage(msg) {
    // 以真实反馈位置（real_joints_）做 FK 计算锚点
    // 锚点：末端位置 (anc_fk_x/y/z)，末端旋转矩阵 (anc_R_)
    //       Quest 位置 (anc_qx/y/z)，Quest 四元数 (anc_q_)
    // 重置位置低通滤波器，重置 Yaw 低通滤波器为 0
    is_tracking_ = true;
}

void disengage() {
    is_tracking_ = false;
    // 不重置 cur_joints_，下次 engage 时仍用此作为 IK 种子
}

关键设计决策：追踪期间禁止 JPOS 反馈覆盖 IK 种子：

cpp 复制代码

void on_jpos(msg) {
    real_joints_ = msg;          // 始终更新真实位置（用于 engage 时初始化锚点）
    if (!is_tracking_) {
        cur_joints_ = msg;       // 非追踪时同步种子（为下次 engage 做准备）
    }
    // 追踪时：cur_joints_ 只由 IK 结果更新，不受 10Hz 硬件反馈干扰
}

11.2.3 IK 选解评分逻辑

cpp 复制代码

float score = Σ IK_WEIGHTS[k] * (sol[k] - cur_joints_[k])²

// 权重：J1=3.0, J2=1.5, J3=1.5, J4=1.0, J5=1.0, J6=0.5
// J1 权重最高：底座大旋转代价最大，优先保持底座稳定
// J6 权重最低：腕关节灵活，允许较大变化

// 过滤条件：J3 < elbow_up_min_deg（35°）的解直接丢弃，防肘部过低干涉

11.2.4 自适应速度映射

cpp 复制代码

// 通过每帧 Quest 位移量估算"运动意图强度"
double dist = |q_cur - q_prev|;
double intent = clamp((dist - intent_lo) / (intent_hi - intent_lo), 0, 1);
int spd = speed_min + intent * (speed_max - speed_min);
// 小幅精细操作 → 低速（60 rpm），快速大幅移动 → 高速（120 rpm）

11.3 robot_serial_driver_node 三优先级发送架构

tx_loop 是串口的唯一写入者，通过三个队列管理不同优先级的命令：

bash 复制代码

优先级 1（最高）：Control 命令
  ctrl_q_: deque<PendingCmd>   ← /robot/control service 请求
  行为：发出后设置 waiting_ok_=true，阻塞所有后续发送
        等待固件回 "xxx ok" 或超时（15 秒）
        阻塞期间：关节命令 / 轮询命令均暂停
  用途：!START / !HOME / !RESET / !DISABLE

优先级 2（中）：夹爪直发命令
  hand_cmd_queue_: deque<string>，上限 8 条
  行为：完全绕过 waiting_ok_ 检查，Control 等 ACK 期间仍可发出
  用途：!HAND_POS N（固件响应不含 " ok" 后缀）

优先级 3（低）：关节流式命令
  latest_joint_cmd_: optional<string>  ← 只保留最新一帧
  行为：仅 waiting_ok_==false 且 free_size>=14 时发送
  用途：>J1,J2,J3,J4,J5,J6,spd 高频关节角指令

轮询（2 Hz）：#GETJPOS
  poll_q_: deque<PollType>
  行为：等待 ACK 期间暂停，防止 JPOS 请求淹没固件缓冲区

串口背压机制 ：固件对每条 > 运动指令回复剩余队列空位数（free_size），驱动节点收到后更新 free_size_，下一帧判断 free_size_ >= 14（可配置）才发送新关节命令，防止固件 16 条命令 FIFO 溢出。

11.4 关键参数速查表

参数	位置	当前值	说明
`scale_rx/ry/rz`	yaml	400/800/400	前后/左右/上下灵敏度（mm/m）
`cross_ry_to_rx`	yaml	0.30	左右→前后跨轴去耦系数
`scale_yaw`	yaml	1.0	手腕偏航追踪灵敏度（0=禁用）
`scale_pitch`	yaml	0.0	手腕俯仰追踪（默认禁用）
`trigger_dead_zone`	yaml	0.25	trigger 自然搭触补偿死区
`elbow_up_min_deg`	yaml	45.0	IK 肘部最低角度（防干涉）
`engage_thresh`	yaml	0.80	握紧开始跟手的 squeeze 阈值
`disengage_thresh`	yaml	0.20	松开停止跟手的阈值
`alpha_pos`	yaml	0.50	位置低通滤波系数（越低越平滑但越滞后）
`speed_min/max`	yaml	60/120	关节速度自适应范围（rpm）
`OpenedAngle`	固件 dummy_robot.h	-230°	夹爪全开角度（从闭合上电零点出发）
`ClosedAngle`	固件 dummy_robot.h	0°	夹爪全闭角度（等于上电零点）

第十二章：总结

12.1 Bug 全景表

层级	Bug	根因	修复版本
串口驱动	死锁（递归加锁）	辅助函数内部重复持有同一锁	重构后
串口驱动	指令丢失（锁竞争）	两个线程争同一串口写锁	重构后
IK 解算	解跳变→乱动	`on_jpos` 10 Hz 覆盖 IK 种子	v8
IK 解算	解截断（i<4）	只搜索前 4 个解，遗漏解族	v5
IK 解算	`wrap()` 归一化错误	`if` 应为 `while`，π 应为 2π	待修复
轴映射	v10 double-negative	参数化时符号双重取反	v11
轴映射	各轴灵敏度不均	单一 `scale_pos` 无法区分各轴	v6/v8
轴映射	左右运动前后串扰	人体肩关节物理耦合	去耦系数
Quest 端	X/Y 恒为 0	UDP 结构体字段顺序不对齐	v3
Z 轴奇异点	Z 轴位置相关反转	IK 在 HOME 附近解族切换	v12/解族锁定
多节点	仿真模式不动	两套节点同时运行冲突	v11 launch 重构
可视化	`/joint_states` 缺失	缺少度→弧度桥接节点	`joint_state_bridge`
夹爪	J6 被覆盖→抖动加剧	发指令时 J6 用 `gripper_angle_`	v8
夹爪	只开一半	步进电机相对编码器零点漂移	固件 OpenedAngle 修改
夹爪	DISABLE 后过热	固件未失能夹爪电机	固件 DISABLE handler
夹爪	UART4 路径命令失效	`OnUart4AsciiCmd` 缺少 `!HAND_*` case	固件补全
姿态追踪	Pitch 导致串口断开	Quest X 轴噪声大，三约束 IK 每帧微调触发 USB 压降	`scale_pitch` 默认 0

12.2 最重要的问题

1. IK 种子必须与实际电机位置保持同步。 任何时候只要有一个关节的种子与实际偏差超过 20°，IK 下一帧的解就有可能跳族，造成乱动。追踪期间禁止用反馈覆盖种子，停止追踪时立即同步。这个 bug 最隐蔽，因为现象（抖动）和根因（时序竞争）之间隔着两层：100 Hz 的 IK 线程与 10 Hz 的串口反馈线程。

2. 轴映射调试必须有量化数据支撑，不能靠感觉。 订阅 /robot/servo/raw，每次只移动一个轴，记录 dq[0]/dq[1]/dq[2] 的主响应轴，再决定 yaml 怎么填。用感觉猜了十几次，最终还是靠数据一次定位。

3. 每新增一个旋转自由度，IK 可解空间收缩是确定性影响。 位置约束 3 DoF 时解空间最大；加 Yaw 变 4 DoF，开始有 IK 失败；加 Pitch 变 5 DoF，失败率明显上升，且电机微调电流波动可能触发 USB 供电问题。所有旋转轴应以 scale=0 为默认值逐轴验证，切忌一次全开。