SEW-Mimic 完整技术解析文档

一种用于上半身人形机器人遥操作的闭式几何重定向求解器

基于论文：A Closed-Form Geometric Retargeting Solver for Upper Body Humanoid Robot Teleoperation

作者：Chuizheng Kong, Yunho Cho, Wonsuh Jung, et al. (Georgia Tech, Qualcomm, Standard Bots, Florida A&M-FSU)

解析日期：2026年8月

---

目录

1. 文档概述
2. 研究背景与动机
3. 核心概念与数学预备
4. 几何子问题详解
5. SEW-Mimic算法完整解析
6. 安全滤波器设计
7. 坐标系同步与预处理
8. 实验设计与结果分析
9. 硬件演示与实现细节
10. 局限性与未来工作
11. 总结与核心创新点
12. 参考文献
13. 附录：关键公式速查表

---

1. 文档概述

1.1 文档目的

本文档旨在对SEW-Mimic论文进行全面、深入、结构化的技术解析，涵盖：

• 问题背景与动机
• 完整的数学推导
• 算法流程与伪代码
• 实验设计与结果
• 核心创新点总结
• 工程实现细节

1.2 阅读建议

读者类型	推荐阅读重点
算法工程师	第3、4、5章（数学与算法）
机器人学研究者	第2、8、10章（背景与实验）
系统集成人员	第6、7、9章（安全与硬件）
学生/初学者	全文顺序阅读

---

2. 研究背景与动机

2.1 问题定义

遥操作（Teleoperation）：人类操作员通过某种接口（VR头显、外骨骼、摄像头等）远程控制机器人执行任务。

重定向（Retargeting）：将人类姿态映射为机器人关节角度的核心过程。

核心挑战：人类与机器人在以下方面存在显著差异：

• 自由度（DOF）：人类手臂7-DOF，机器人可能6-7-DOF
• 尺寸比例：臂长、躯干宽度不同
• 运动约束：关节限位、奇异位形、自碰撞

2.2 现有方法分类与不足

方法类型	代表工作	优点	缺点	推理速度
仅手部末端	DexMimicGen, OpenTeach, OSC	简单、常用	肘部失控、奇异位形、冗余漂移	~1-5ms
全身关键点优化	H2O, CLONE, TWIST, GMR	姿态相似度高	计算极慢（~0.7s）、延迟大	~700ms
解析IK	ik-geo, EAIK	计算快	主要针对6-DOF，非重定向专用	~0.1ms
SEW-Mimic（本文）	—	快、优、无需标定	仅限上半身拟人构型	~0.33ms

2.3 本文核心思想

将重定向重新定义为"方向对齐"问题，而非"位置匹配"问题。

关键洞察：

1. 人类与机器人的尺度差异与重定向无关——我们只关心姿态（方向），不关心位置。
2. 肘部位置由肩-肘-腕三个关键点的方向自然决定，无需额外约束。
3. 方向对齐问题具有闭式几何解，无需迭代优化。

2.4 论文贡献总结

1. SEW-Mimic算法：闭式、最优、高速的上半身重定向求解器
2. 安全滤波器：利用SEW-Mimic作为高速IK的双臂自碰撞避免系统
3. 开源实现：MediaPipe/Meta Quest集成、Web Demo
4. 全面实验验证：速度、精度、用户研究、策略学习、全身加速

---

3. 核心概念与数学预备

3.1 符号系统

基本符号约定

符号	含义
$\mathbb{R}$	实数集
$\mathbb{N}$	自然数集
$\mathrm{SO}(3)$	三维旋转矩阵群
$x$	标量（斜体）
$\mathbf{x}$	向量（粗体）
$\mathbf{A}$	矩阵（粗体）
${\mathbf{I}}_n$	$n$维单位矩阵
$0_{n\times m}$	$n\times m$零矩阵
${\mathbf{A}}^{†}$	矩阵$\mathbf{A}$的伪逆
$\mathbf{v}[i]$	向量$\mathbf{v}$的第$i$个元素
$(\mathbf{a},\mathbf{b})$	向量拼接 $[{\mathbf{a}}^T,{\mathbf{b}}^T{]}^T$
下标	标识符（如$\mathbf{s}$表示肩）
上标	坐标系（如${\mathbf{v}}^{\text{in}}$表示输入坐标系）

常用操作

Rodrigues旋转公式：
[
\mathcal{R}(\mathbf{h}, \alpha) = \mathbf{I}_3 + (\sin\alpha)\text{sk}(\mathbf{h}) + (1 - \cos\alpha)(\text{sk}(\mathbf{h}))^2
]
其中 $\mathbf{h}\in {\mathbb{R}}^3$ 是单位旋转轴，$\alpha \in \mathbb{R}$ 是旋转角度，$\text{sk}(\cdot )$ 是反对称矩阵算子。

反对称矩阵（Hat算子）：
$$\text{sk}(\mathbf{v})=\left[\begin{array}{ccc}0& -v_3& v_2\\ v_3& 0& -v_1\\ -v_2& v_1& 0\end{array}\right],\mathbf{v}=[v_1,v_2,v_3{]}^T$$

向量单位化：
$$\text{unit}(\mathbf{v})=\frac{\mathbf{v}}{\Vert \mathbf{v}{\Vert }_2}$$

坐标系变换

给定两个坐标系 $({\mathbf{R}}^{0,a},{\mathbf{p}}^{0,a})$ 和 $({\mathbf{R}}^{0,b},{\mathbf{p}}^{0,b})$，从 $a$ 到 $b$ 的变换：
$${\mathbf{v}}^b=[{\mathbf{R}}^{0,b}{]}^T({\mathbf{R}}^{0,a}{\mathbf{v}}^a-{\mathbf{p}}^{0,a})+{\mathbf{p}}^{0,b}$$

3.2 误差度量

向量方向误差（余弦相似度）：
$${\mu }_c(\mathbf{u},\mathbf{v})=\frac{1}{2}-\frac{1}{2}\frac{\mathbf{u}\cdot \mathbf{v}}{\Vert \mathbf{u}{\Vert }_2\cdot \Vert \mathbf{v}{\Vert }_2}\in [0,1]$$

• ${\mu }_c=0$：方向完全对齐
• ${\mu }_c=1$：方向完全相反

旋转矩阵误差（改进弦距离）：
$${\mu }_m({\mathbf{R}}_1,{\mathbf{R}}_2)=\frac{1}{2}{\Vert ({\mathbf{R}}_1^T{\mathbf{R}}_2{)}^{\frac{1}{2}}-\mathbf{I}\Vert }_F\in [0,1]$$

• 改进点：使用矩阵平方根使误差更线性
• ${\mu }_m=0$：姿态完全对齐

3.3 人体与机器人模型

人体模型

• 输入：肩($\mathbf{s}$)、肘($\mathbf{e}$)、腕($\mathbf{w}$)三维关键点 + 手部旋转矩阵$\mathbf{H}$
• 导出方向：
  • 上臂方向：$\mathbf{u}=\text{unit}(\mathbf{e}-\mathbf{s})$
  • 前臂方向：$\mathbf{l}=\text{unit}(\mathbf{w}-\mathbf{e})$

机器人模型

• 7-DOF串联运动链（拟人构型）
• 关键关节轴：
  • 第3关节轴(${\mathbf{h}}_3$)：代表上臂方向
  • 第5关节轴(${\mathbf{h}}_5$)：代表前臂方向
  • 第7关节轴(${\mathbf{h}}_7$)：代表手腕方向
• 正运动学：$\mathbf{T}(\mathbf{q})={\prod }_{i=1}^7{\mathbf{R}}^{i-1,i}(q_i)\cdot {\mathbf{R}}_{\text{align}}$

构型有效性条件

论文方法适用于满足以下条件的机器人：

1. 7-DOF旋转关节串联链
2. 相邻关节轴相互垂直
3. 肩部安装方式为拟人构型（如图2所示）

已验证平台：Kinova Gen3、Rainbow RB-Y1、Unitree G1

---

4. 几何子问题详解

4.1 子问题概述

SEW-Mimic 基于三个经典几何子问题，所有子问题均有闭式解：

子问题	问题描述	输入	输出	用途
SP1	绕单轴旋转对齐向量	${\mathbf{p}}_1,{\mathbf{p}}_2,\mathbf{k}$	角度$\theta$	腕部第7关节
SP2	双轴旋转对齐两向量	${\mathbf{p}}_1,{\mathbf{p}}_2,{\mathbf{k}}_1,{\mathbf{k}}_2$	$({\theta }_1,{\theta }_2)$ 最多2组	前4个关节
SP4	绕轴旋转向量到平面	$\mathbf{p},\mathbf{k},\mathbf{h},d$	角度$\theta$ 最多2个	SP2的依赖

4.2 Subproblem 1（SP1）

问题：
$${\theta }^{\ast }=\arg \underset{\theta }{\min }\Vert \mathcal{R}(\mathbf{k},\theta ){\mathbf{p}}_1-{\mathbf{p}}_2{\Vert }_2$$

算法流程：

Algorithm: SP1(p1, p2, k)
1. 将 p1, p2 投影到垂直于 k 的平面：
   p1_perp = p1 - (p1·k)k
   p2_perp = p2 - (p2·k)k
2. 单位化：
   p1_hat = p1_perp / ||p1_perp||
   p2_hat = p2_perp / ||p2_perp||
3. 计算角度：
   theta = 2 * atan2(||p1_hat - p2_hat||, ||p1_hat + p2_hat||)
4. 符号校正：
   if k^T(p1_hat × p2_hat) < 0:
       theta = -theta
Return: theta

几何直观：在垂直于$\mathbf{k}$的圆上，${\mathbf{p}}_1$到${\mathbf{p}}_2$的最短弧长对应的圆心角。

4.3 Subproblem 4（SP4）

问题：
$${\theta }^{\ast }=\arg \underset{\theta }{\min }\left|{\mathbf{h}}^T\mathcal{R}(\mathbf{k},\theta )\mathbf{p}-d\right|$$

核心推导：

展开Rodrigues公式：
$$\mathcal{R}(\mathbf{k},\theta )=\mathbf{k}{\mathbf{k}}^T+\sin \theta \cdot \text{sk}(\mathbf{k})-\cos \theta \cdot \text{sk}(\mathbf{k}{)}^2$$

代入${\mathbf{h}}^T\mathcal{R}(\mathbf{k},\theta )\mathbf{p}$：
$${\mathbf{h}}^T\mathcal{R}(\mathbf{k},\theta )\mathbf{p}=\underset{\mathbf{A}}{\underbrace{[{\mathbf{h}}^T\text{sk}(\mathbf{k})\mathbf{p},-{\mathbf{h}}^T\text{sk}(\mathbf{k}{)}^2\mathbf{p}]}}\underset{\mathbf{x}}{\underbrace{[\sin \theta ,\cos \theta {]}^T}}+\underset{b}{\underbrace{{\mathbf{h}}^T\mathbf{k}{\mathbf{k}}^T\mathbf{p}}}$$

转化为线性方程：
$$\mathbf{Ax}=d-b$$

解的情况：

条件	解数	说明
$|\mathbf{A}{|}_2^2>(d-b{)}^2$	2个	圆与平面相交于两点
$|\mathbf{A}{|}_2^2=(d-b{)}^2$	1个	相切
$|\mathbf{A}{|}_2^2<(d-b{)}^2$	1个（最小二乘）	无交点，取最近点

2个解时的偏移公式：
$$z=\sqrt{\Vert \mathbf{A}{\Vert }_2^2-(d-b{)}^2},{\mathbf{x}}_{\pm }=\mathbf{x}\pm z[\mathbf{A}[2],-\mathbf{A}[1]]$$

4.4 Subproblem 2（SP2）

问题：
$$({\theta }_1^{\ast },{\theta }_2^{\ast })=\arg \underset{{\theta }_1,{\theta }_2}{\min }\Vert \mathcal{R}({\mathbf{k}}_1,{\theta }_1){\mathbf{p}}_1-\mathcal{R}({\mathbf{k}}_2,{\theta }_2){\mathbf{p}}_2{\Vert }_2$$

求解策略（分解为两次SP4）：

1. 第一次SP4：固定${\mathbf{p}}_1$绕${\mathbf{k}}_1$，求使${\mathbf{p}}_1$能通过旋转接触${\mathbf{p}}_2$绕${\mathbf{k}}_2$轨迹平面
2. 第二次SP4：对称操作
3. 组合验证：对解对进行验证，保留使两向量对齐的组合

Algorithm: SP2(p1, p2, k1, k2)
1. 归一化：p1 = p1/||p1||, p2 = p2/||p2||
2. 第一次SP4：
   theta1_candidates = SP4(p1, k2^T·p2, k1, k2·p2)
3. 第二次SP4：
   theta2_candidates = SP4(p2, k1^T·p1, k2, k1·p1)
4. 组合验证：
   for t1 in theta1_candidates:
       for t2 in theta2_candidates:
           if ||R(k1,t1)·p1 - R(k2,t2)·p2|| < eps:
               solutions.append((t1, t2))
Return: solutions  (1或2组)

---

5. SEW-Mimic算法完整解析

5.1 问题形式化

优化问题（Problem 2）：
$$\underset{\mathbf{q}}{\min }\underset{\text{上臂误差}}{\underbrace{{\mu }_c(\mathbf{u},{\mathbf{R}}^{0,3}(\mathbf{q}){\mathbf{h}}_3)}}+\underset{\text{前臂误差}}{\underbrace{{\mu }_c(\mathbf{l},{\mathbf{R}}^{0,5}(\mathbf{q}){\mathbf{h}}_5)}}+\underset{\text{手腕误差}}{\underbrace{{\mu }_m(\mathbf{T}(\mathbf{q}),\mathbf{H})}}$$
约束：$\mathbf{q}$ 在关节限位内

关键观察：三个误差项分别对应不同的关节组，可依次独立求解：

• 上臂误差 → $(q_1,q_2)$
• 前臂误差 → $(q_3,q_4)$
• 手腕误差 → $(q_5,q_6,q_7)$

5.2 主算法流程

Algorithm 1: SEW-Mimic(q0, s, e, w, H)
┌─────────────────────────────────────────────────────────────┐
│ 输入：q0 - 初始机器人关节角                                  │
│       s, e, w - 人类肩、肘、腕三维关键点                    │
│       H - 人类手部旋转矩阵                                  │
├─────────────────────────────────────────────────────────────┤
│ 1.  同步坐标系（见第7章）                                   │
│ 2.  q ← q0                                                 │
│ 3.  u ← unit(e - s)        // 人类上臂方向                  │
│ 4.  l ← unit(w - e)        // 人类前臂方向                  │
│ 5.  (q1, q2) ← AlignAxis(3, q, u)   // 对齐上臂            │
│ 6.  (q3, q4) ← AlignAxis(5, q, l)   // 对齐前臂            │
│ 7.  (q5, q6, q7) ← AlignWrist(q, H) // 对齐手腕            │
│ 8.  Return: q                                              │
└─────────────────────────────────────────────────────────────┘

5.3 AlignAxis：关节轴对齐

Algorithm 2: AlignAxis(i, q0, v)
┌─────────────────────────────────────────────────────────────┐
│ 功能：将第i个关节轴 h_i 对齐到向量 v                        │
│ 输入：i - 关节索引 (3 或 5)                                 │
│       q0 - 当前关节角                                      │
│       v - 目标方向向量                                     │
├─────────────────────────────────────────────────────────────┤
│ 1.  将 v 变换到第 (i-2) 关节坐标系：                       │
│     v_local = [R^{0,i-2}(q0)]^T · v                        │
│ 2.  获取第 (i-2) 和 (i-1) 关节轴在 (i-2) 坐标系的方向：    │
│     h_{i-2} = R^{i-2,i}(q0) · h_i                         │
│     h_{i-1} = R^{i-1,i}(q0) · h_i                         │
│ 3.  调用 SP2 求解 (q_{i-2}, q_{i-1})：                    │
│     solutions = SP2(v_local, h_i_local, -h_{i-2}, h_{i-1})│
│ 4.  边界约束过滤：                                         │
│     valid = filter_by_joint_limits(solutions)              │
│ 5.  选择最接近当前位姿的解：                               │
│     best = argmin_{(a,b)} |q0[i-2]-a| + |q0[i-1]-b|       │
│ 6.  Return: (best_a, best_b)                               │
└─────────────────────────────────────────────────────────────┘

为什么用两个关节轴对齐一个方向？

• 第$(i-2)$关节轴和第$(i-1)$关节轴的组合可以调节第$i$关节轴的方向
• 使用SP2可以同时求解这两个关节角，保证最优性

5.4 AlignWrist：手腕对齐

Algorithm 3: AlignWrist(q0, H)
┌─────────────────────────────────────────────────────────────┐
│ 功能：对齐机器人手腕姿态到人类手部姿态                      │
│ 输入：q0 - 当前关节角                                      │
│       H - 人类手部旋转矩阵                                 │
├─────────────────────────────────────────────────────────────┤
│ 1.  计算期望的第7关节姿态：                                │
│     R_des^{0,7} = H · R_align · [R_{local}^{7,T}]^T       │
│ 2.  用 SP2 对齐 h_7 到期望方向，求解 (q5, q6)：            │
│     (q5, q6) = AlignAxis(7, q0, R_des[:,1])               │
│ 3.  将 h_6 和期望方向变换到第7关节坐标系：                 │
│     u_6^7 = R^{7,0} · R^{0,6}(q0) · h_6                   │
│     h_6^7 = [R_{local}^{6,7}]^T · h_6                     │
│ 4.  用 SP1 求解 q7：                                       │
│     q7 = SP1(h_6^7, u_6^7, -h_7)                          │
│ 5.  边界约束过滤                                           │
│ 6.  Return: (q5, q6, q7)                                   │
└─────────────────────────────────────────────────────────────┘

手腕对齐的数学逻辑：

1. 先对齐方向（SP2）：确保第7关节轴指向与人类手部X轴一致
2. 再对齐旋转（SP1）：绕第7关节轴旋转，使手部Y/Z轴也匹配

5.5 最优性证明

Proposition 3（原文Proposition 3）：
在无关节限位、无自碰撞、相邻关节轴垂直的条件下，Algorithm 1 返回 Problem 2 的全局最优解。

证明结构：

1. 上臂最优性（Algorithm 1第5步）：

   • SP2保证 ${\mu }_c(\mathbf{u},{\mathbf{R}}^{0,3}(\mathbf{q}){\mathbf{h}}_3)=0$
   • 证明依赖Lemma 7

2. 前臂最优性（Algorithm 1第6步）：

   • SP2保证 ${\mu }_c(\mathbf{l},{\mathbf{R}}^{0,5}(\mathbf{q}){\mathbf{h}}_5)=0$
   • 证明依赖Lemma 7

3. 手腕最优性（Algorithm 1第7步）：

   • SP2保证第7关节轴对齐（第一列对齐）
   • SP1保证绕轴旋转对齐（第二、三列对齐）
   • 证明依赖Lemma 7和Lemma 8

4. 非负性：三个误差项均非负，各自达到最小值0，总和即为全局最优

Lemma依赖链：

Proposition 3
    ├── Lemma 7 (SP2最优性)
    │   └── Lemma 6 (SP4最优性)
    └── Lemma 8 (SP1最优性)

5.6 算法复杂度分析

操作	时间复杂度	说明
坐标系同步	$O(1)$	固定矩阵运算
AlignAxis ×2	$O(1)$	SP2闭式解
AlignWrist	$O(1)$	SP2 + SP1闭式解
总计	$O(1)$	与DOF无关
实测速度	~0.33ms (3kHz)	单臂，Intel i9

---

6. 安全滤波器设计

6.1 设计目标与动机

目标：在双机遥操作中避免自碰撞，同时保留操作员的有意动作。

动机：

• SEW-Mimic计算极快（3kHz），留有大量算力余量
• 可作为高速IK求解器，实时修正不安全位姿
• 设计理念：仅移除"有害"的运动分量，保留切向安全运动

6.2 胶囊体碰撞模型

胶囊体定义：
C ( p 1 , p 2 , r ) = { x ∈ R 3 ∣ ∥ x − ( t p 1 + ( 1 − t ) p 2 ) ∥ 2 ≤ r ,    t ∈ [ 0 , 1 ] } \mathcal{C}(\mathbf{p}_1, \mathbf{p}_2, r) = \{\mathbf{x} \in \mathbb{R}^3 \mid \|\mathbf{x} - (t\mathbf{p}_1 + (1-t)\mathbf{p}_2)\|_2 \leq r,\; t \in [0,1]\} C(p1​,p2​,r)={x∈R3∣∥x−(tp1​+(1−t)p2​)∥2​≤r,t∈[0,1]}

胶囊体集合（双机）：

C = {
    躯干: Capsule(躯干起点, 躯干终点, r_torso),
    左上臂: Capsule(左肩, 左肘, r_upper),
    左前臂: Capsule(左肘, 左腕, r_lower),
    左手腕: Capsule(左腕, 左手尖, r_wrist),
    右上臂: Capsule(右肩, 右肘, r_upper),
    右前臂: Capsule(右肘, 右腕, r_lower),
    右手腕: Capsule(右腕, 右手尖, r_wrist)
}

碰撞检测：使用标准胶囊体-胶囊体最近距离算法[11, §5.9.1]。

6.3 XPBD迭代

核心思想：使用Position-Based Dynamics（位置动力学）迭代推离碰撞胶囊体。

算法流程：

Algorithm 10: XPBD-Iter(C, {λ_ij})
┌─────────────────────────────────────────────────────────────┐
│ 输入：C - 胶囊体集合                                       │
│       {λ_ij} - 拉格朗日乘子                                │
│       超参数：d_min(安全距离), d_act(激活距离), α(柔度)   │
├─────────────────────────────────────────────────────────────┤
│ 1.  for each 碰撞体对 (C_i, C_j) in C × C:                │
│ 2.      (d, n_i, n_j) = CollisionCheck(C_i, C_j)          │
│ 3.      if d ≥ d_act and λ_ij == 0: continue              │
│ 4.      c = d - d_min                                     │
│ 5.      if c < 0:  // 发生碰撞                            │
│ 6.         计算梯度 g_i, g_j (每个端点)                    │
│ 7.         λ_old = λ_ij                                   │
│ 8.         Δλ = -(c + α·λ_old) / (α + Σ w_k||g_k||²)     │
│ 9.         λ_ij = max(0, λ_old + Δλ)                     │
│ 10.        更新端点位置: p += w·(λ_new-λ_old)·g          │
│ 11.        更新胶囊体                                     │
│ 12. 修复连杆长度 (Algorithm 11)                           │
│ 13. Return: 更新后的 C, {λ_ij}                            │
└─────────────────────────────────────────────────────────────┘

梯度计算规则：

• 上臂胶囊体：仅移动肘部端点（肩部固定）
• 前臂/手腕胶囊体：两端点均可移动

6.4 连续时间碰撞检查

问题：如果初始位姿和目标位姿相距很远，线性插值可能使肢体"穿越"彼此。

解决方案：在插值路径上检测首次碰撞

Algorithm 9: ContinuousCollisionCheck(q0, q_des)
┌─────────────────────────────────────────────────────────────┐
│ 1.  K0 = FK(q0), K_des = FK(q_des)                        │
│ 2.  n_interp = ceil(max(||K0 - K_des|| / r))              │
│ 3.  for i = 1 to n_interp:                                │
│ 4.      K_i = linspace(K0, K_des, i/(n_interp+1))        │
│ 5.      C_i = MakeCapsules(K_i)                           │
│ 6.      if C_i 发生碰撞:                                   │
│ 7.          i* = i; break                                 │
│ 8.  Return: C_{i*}                                        │
└─────────────────────────────────────────────────────────────┘

6.5 安全滤波器完整流程

Algorithm 4: SEW-SafetyFilter(q0, q_des)
┌─────────────────────────────────────────────────────────────┐
│ 输入：q0 - 当前位姿                                        │
│       q_des - 期望位姿                                     │
├─────────────────────────────────────────────────────────────┤
│ 1.  连续时间碰撞检测 → C                                   │
│ 2.  初始化 λ_ij = 0                                       │
│ 3.  for iter = 1 to n_iter:                               │
│ 4.      (C, {λ_ij}) = XPBD-Iter(C, {λ_ij})               │
│ 5.      if 无碰撞: break                                  │
│ 6.  从 C 恢复关键点 (s,e,w,t)                              │
│ 7.  恢复工具姿态: H = RecoverTool(T(q0), t)               │
│ 8.  重定向: q_safe = SEW-Mimic(s,e,w,H)                   │
│ 9.  Return: q_safe                                        │
└─────────────────────────────────────────────────────────────┘

6.6 性能指标

指标	数值
计算频率	~250 Hz (4ms)
碰撞降低	~50% → ~1.3%
对齐误差增加	0 → 0.019

---

7. 坐标系同步与预处理

7.1 身体中心坐标系构建

目的：将任意输入流（VR、摄像头等）的关键点映射到机器人基坐标系。

方法：使用左肩、右肩、躯干三个关键点构建正交坐标系。

Algorithm 8: MakeFrame(k_lf, k_rt, k_b)
┌─────────────────────────────────────────────────────────────┐
│ 输入：k_lf - 左肩关键点                                    │
│       k_rt - 右肩关键点                                    │
│       k_b  - 躯干下部关键点（如髋部）                      │
├─────────────────────────────────────────────────────────────┤
│ 1.  p = (k_lf + k_rt) / 2          // 原点=肩部中心      │
│ 2.  u_y = unit(k_lf - k_rt)        // Y轴=左→右方向      │
│ 3.  u_x = unit(u_y × (p - k_b))    // X轴=前方向          │
│ 4.  u_z = u_x × u_y                // Z轴=上方向          │
│ 5.  R = [u_x, u_y, u_z]                                 │
│ 6.  Return: (R, p)                                       │
└─────────────────────────────────────────────────────────────┘

物理意义：

• 原点：两肩中点
• X轴：身体前方
• Y轴：从左到右
• Z轴：向上

7.2 手部姿态校正

问题：不同输入设备使用不同的手部坐标系约定。

解决方案：

1. 如果输入是旋转矩阵，应用固定旋转校正 $\mathbf{H}={\mathbf{R}}_{\text{align}}\hat{\mathbf{H}}$
2. 如果输入是指尖关键点，用 MakeFrame 构建手部坐标系

机器人末端校正：同样应用 $\mathbf{T}(\mathbf{q})={\mathbf{R}}_{\text{align}}^{\text{rb}}\hat{\mathbf{T}}(\mathbf{q})$

7.3 垂直手腕特殊处理

适用机器人：Rainbow RB-Y1, Unitree G1（末端安装方向垂直于第7关节轴）

方法：使用欧拉角分解代替SP1/SP2

1. 计算期望的第7关节姿态：${\mathbf{R}}_{\text{des}}^{5,7}=[{\mathbf{R}}^{0,5}(\mathbf{q}){]}^T{\mathbf{R}}_{\text{des}}^{0,7}$
2. 根据关节轴方向确定欧拉角顺序（X/Y/Z或X/Y/X）
3. 使用Euler分解求解 $(q_5,q_6,q_7)$

限制：当 $q_6=\pm 90^{\circ }$ 时发生万向节锁，但人类手腕活动范围通常远小于此。

---

8. 实验设计与结果分析

8.1 实验总览

实验	目的	关键结果
A. 重定向性能	速度与精度对比	SEW最优（3kHz, 近零误差）
B. 用户研究	任务成功率	SEW总成功125 vs MINK 68
C. 安全滤波消融	碰撞避免效果	碰撞率50%→1.3%
D. 策略学习	数据质量影响	成功率提升3倍
E. 全身加速	TWIST框架集成	速度提升1-3数量级

8.2 实验A：重定向性能

设置：

• 数据集：LAFAN1上半身动作
• 对比：GMR [1], xr_teleoperate [34]
• 指标：对齐误差、推理时间

结果：

方法	对齐误差中位数	推理时间中位数
SEW-Mimic	$1.57\times 10^{-13}$	0.587 ms
GMR	显著更高	~700 ms
xr_teleoperate	显著更高	~1 ms

统计检验：Kruskal-Wallis检验，$p<0.001$，SEW显著优于所有基线。

8.3 实验B：用户研究

设置：

• 被试：8人
• 任务：Cabinet, Glass Gap, Handover
• 对比：SEW-Mimic vs. MINK-IK
• 时限：每任务5分钟

结果汇总：

任务	SEW成功/失败	MINK成功/失败
Cabinet	54/37	44/29
Glass Gap	41/68	84/21
Handover	32/52	13/25
总计	125/157	68/75

统计分析：

• ART ANOVA：方法×任务交互效应显著 ($p<0.01$)
• 主效应：任务类型显著 ($p<0.05$)
• 单任务比较：未达显著（样本量限制）

定性观察：

• MINK-IK失效模式：肘部不受控，导致末端无法到达目标
• 臂长适配问题：短臂用户使用MINK时缩放因子不合适

8.4 实验C：安全滤波器消融

设置：

• 动作：双臂胸前画圆（rolling punch），10秒10圈
• 对比：有/无安全滤波器

结果：

指标	无滤波器	有滤波器
对齐误差	0	0.019
碰撞率	~50%	~1.3%
计算时间	0.8ms (1250Hz)	4.0ms (250Hz)

结论：安全滤波器以轻微的精度损失换取显著的碰撞降低。

8.5 实验D：策略学习影响

设置：

• 任务：Glass Gap
• 数据：各50次演示（同一专家）
• 策略：Diffusion Policy [7]
• 对比：SEW-Mimic vs. MINK-IK采集的数据

结果：

指标	MINK数据	SEW数据
成功率	~15%	~45%（3倍提升）
任务完成时间	较长	较短
关节速度	高、波动	低、平滑
SPARC平滑度	低	高（~2倍）
动作预测误差