如何将3D点从全局坐标系转换至局部坐标系？实例求解

一、位置坐标转换 #

已知全局坐标系中，局部参考系A的原点位置为 ( A_{pos} = (T_x=-27.95, T_y=-94.68, T_z=-49.53) )，点B的全局位置为 ( B_{pos} = (T_x=100.77, T_y=-124.18, T_z=31.33) )。

转换步骤：

1. 计算全局坐标系下B相对于A的位置差向量：
   vec_V_BA = (100.77 - (-27.95), -124.18 - (-94.68), 31.33 - (-49.53)) = (128.72, -29.5, 80.86)
2. 用A的姿态四元数的共轭（单位四元数的逆等于共轭）旋转该向量，得到局部坐标系下的位置。A的四元数共轭为 Q_A* = (0.3222, 0.0062, 0.0092, -0.9466)。
3. 通过四元数旋转计算后，得到局部位置：(T_x=-120.02, T_y=-55.62, T_z=16.23)，与期望结果一致。

二、姿态四元数转换 #

相对姿态四元数计算公式为 Q_B/A = Q_A* × Q_B，其中：

• A的四元数共轭 Q_A* = (0.3222, 0.0062, 0.0092, -0.9466)
• B的全局姿态四元数 Q_B = (0.6773, 0.6166, 0.4012, 0.0000)

四元数乘法规则（Q1=(w1,x1,y1,z1)，Q2=(w2,x2,y2,z2)）：

Q1×Q2 = (
w1w2 - x1x2 - y1y2 - z1z2,
w1x2 + x1w2 + y1z2 - z1y2,
w1y2 - x1z2 + y1w2 + z1x2,
w1z2 + x1y2 - y1x2 + z1w2
)

代入计算后，得到相对姿态四元数：(Q0=0.2105, Qx=0.5833, Qy=-0.4482, Qz=-0.6439)，与期望结果一致（微小差异来自浮点计算精度）。

内容的提问来源于stack exchange，提问作者CmBe