三维坐标系转换

一、首先需要判断坐标系是左手坐标系还是右手坐标系

首先将拇指作为X轴，食指作为Y轴，中指作为Z轴。观察（待判断的）坐标系对应哪只手对应的坐标系。然后根据对应的坐标系使用适配定则判断。右（左）手定则：用右（左）手的大拇指指向旋转轴的正方向，弯曲手指，四指所指示的方向即为正旋转方向。

从轴的正方向看向原点，右手坐标系下：逆时针方向即是旋转正向; 左手坐标系下 ：顺序时针方向即是旋转正向。

二、坐标转换关系

设有A坐标系，B坐标系和A坐标系仅存在一个绕三轴旋转的关系, 已知一点C在B坐标系下的位置为：

则C在A坐标系下的坐标为：

其中 B2A表示由B坐标系动态旋转至A坐标系的旋转向量
1、若由B坐标系动态旋转至A坐标系，先绕Z轴、再绕Y轴，再绕X轴，则B2A = Rz* Ry* Rx
2、若由B坐标系动态旋转至A坐标系，先绕X轴、再绕Y轴，再绕Z轴，则B2A = Rx* Ry* Rz
注意根据实际旋转顺序，调整B2A中的参数顺序。

以下为Rx、Ry、Rz的表达式，θ分别为绕X、Y、Z轴的旋转角（如Rx中的θ表示绕X轴的旋转角度），其中θ为弧度。

上述旋转方式1中，由A坐标系动态旋转至B坐标系则是先绕X轴、再绕Y轴，再绕Z轴。个人使用时通常是A坐标系为机器人世界坐标系，B坐标系为相机坐标系。所以由A坐标系动态旋转至B坐标系较为直观。通过A->B的旋转角度间接获取B->A的旋转角度：由A坐标系动态旋转先绕X轴、再绕Y轴，再绕Z轴变换至B坐标系，假设绕X、Y、Z轴旋转角度分别为thetax,thetay,thetaz，该过程表示的是A2B的旋转角。而在上述坐标转换关系中，B2A 则为先绕Z轴、再绕Y轴，再绕X轴,旋转角为 -thetaz，-thetay，-thetax ，用该角度去计算旋转向量。

上述旋转方式2中，若是能直接获得由B坐标系转换至A坐标系的旋转角，则直接根据实际旋转顺序计算即可。

说明：有些机器人坐标系，由A坐标系（世界坐标系）动态旋转至B坐标系（如相机坐标系）时，动态旋转过程是先绕Z轴、再绕Y轴，再绕X轴。则由B坐标系动态转换至A坐标系则是先绕X，再绕Y，再绕Z轴。使用时根据实际情况灵活调整B2A中的Rx、Ry、Rz的顺序。