pico到机器人坐标系变换推导（最终版,以此为准）

VR 坐标系 → 机器人坐标系变换矩阵原理与推导

本文档对应配置：robot/yaocaozuo/r11_config.yaml中的coord_transform
对应代码：robot/yaocaozuo/pico_data.py中的vr_pose_to_robot()

1. 问题背景

遥操作机器人时存在两种坐标系：

注：Pico Body Tracking SDK 默认使用 Unity 风格的左手坐标系约定，但实际返回的轴方向为X=右、Y=上、Z=后（朝用户身后为正 Z）。这一点与网上常见的"Unity Z 朝前"描述相反，是 Pico SDK 的实际行为，本项目通过实测确认。

目标：已知 VR 系下的位姿T_vr，求在机器人坐标系下表示的位姿T_robot，使动作方向保持一致。

2. 核心数学原理

2.1 换基矩阵 C

设：

C : VR 坐标系 → 机器人坐标系 的基变换矩阵

本质是一个换基矩阵（Change of Basis Matrix），其几何意义是：每一列是 VR 某个坐标轴单位向量在机器人坐标系下的表示。

2.2 点的转换

一个点在 VR 系下坐标为p_vr，在机器人系下为：

p_robot = C · p_vr

2.3 VR 中的变换作用

VR 系下的变换作用在 VR 系的点上：

p_vr' = T_vr · p_vr

2.4 转到机器人坐标系

由 (2.2) 反向得p_vr = C⁻¹ · p_robot，代入 (2.3)：

p_robot' = C · p_vr' = C · (T_vr · p_vr) = C · T_vr · p_vr = C · T_vr · (C⁻¹ · p_robot) = (C · T_vr · C⁻¹) · p_robot

2.5 最终公式

┌─────────────────────────────────────┐ │ T_robot = C · T_vr · C⁻¹ │ └─────────────────────────────────────┘

2.6 直观理解（口诀：C·T·C⁻¹）

一句话总结：

C · T · C⁻¹= “把一个变换从一个坐标系，正确翻译到另一个坐标系”

2.7 线性代数角度

T_vr和T_robot是同一个线性算子，只是表达在不同基（coordinate frame）下。这种关系叫做相似变换（Similarity Transform）：

T_robot = C · T_vr · C⁻¹

3. 如何构建变换矩阵 C（按 Pico 正确定义）

3.1 坐标轴映射

3.2 构建换基矩阵 C

C 的每一列是 Pico 某个轴单位向量在 Robot 系下的表示：

• 第 1 列 = Pico X 在 Robot 系 =[0, -1, 0]
• 第 2 列 = Pico Y 在 Robot 系 =[0, 0, 1]
• 第 3 列 = Pico Z 在 Robot 系 =[-1, 0, 0]

拼起来：

importnumpyasnp# Pico VR 坐标系 → 机器人坐标系的换基矩阵C=np.array([[0,0,-1],# 第1列：Pico X(右) → Robot -Y[-1,0,0],# 第2列：Pico Y(上) → Robot +Z[0,1,0]# 第3列：Pico Z(后) → Robot -X])

3.3 验证三列映射

# Pico X 轴 [1,0,0] → Robot -Y [0,-1,0] (右)print(C @[1,0,0])# [0, -1, 0] ✓# Pico Y 轴 [0,1,0] → Robot +Z [0,0,1] (上)print(C @[0,1,0])# [0, 0, 1] ✓# Pico Z 轴 [0,0,1] → Robot -X [-1,0,0] (后)print(C @[0,0,1])# [-1, 0, 0] ✓

三列全部正确。

3.4 验证 C 是正交矩阵

# 行列式 = +1，C 是正交矩阵，且是纯旋转（不含反射）print(np.linalg.det(C))# 1.0# C^T · C = I，即 C⁻¹ = C^Tprint(C.T @ C)# 单位阵

由于 C 是正交矩阵且det(C) = 1，C⁻¹ = Cᵀ，相似变换可简化为：

T_robot = C · T_vr · Cᵀ

3.5 完齐次变换矩阵

齐次坐标下用 4×4 矩阵：

# 4x4 齐次变换矩阵（用于变换位姿矩阵）CONVERT_MATRIX=np.array([[0,0,-1,0],[-1,0,