VR 坐标系 → 机器人坐标系变换矩阵原理与推导本文档对应配置：robot/yaocaozuo/r11_config.yaml中的coord_transform对应代码：robot/yaocaozuo/pico_data.py中的vr_pose_to_robot()1. 问题背景遥操作机器人时存在两种坐标系：坐标系XYZPico VR右上后Robot前左上注：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_vr2.3 VR 中的变换作用VR 系下的变换作用在 VR 系的点上：p_vr' = T_vr · p_vr2.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_robot2.5 最终公式┌─────────────────────────────────────┐ │ T_robot = C · T_vr · C⁻¹ │ └─────────────────────────────────────┘2.6 直观理解（口诀：C·T·C⁻¹）步骤操作含义1.C⁻¹机器人坐标 → VR 坐标把输入"翻译"成 VR 听得懂的话2.T_vr在 VR 系下做变换用 VR 的语言执行变换3.CVR 坐标 → 机器人坐标把结果"翻译"回机器人语言一句话总结：C · T · C⁻¹= “把一个变换从一个坐标系，正确翻译到另一个坐标系”2.7 线性代数角度T_vr和T_robot是同一个线性算子，只是表达在不同基（coordinate frame）下。这种关系叫做相似变换（Similarity Transform）：T_robot = C · T_vr · C⁻¹3. 如何构建变换矩阵 C（按 Pico 正确定义）3.1 坐标轴映射Pico 轴物理方向Robot 轴表示说明Pico X右[0, -1, 0]右 = Robot -YPico Y上[0, 0, 1]上 = Robot +ZPico Z后[-1, 0, 0]后 = Robot -X3.2 构建换基矩阵 CC 的每一列是 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,