前言

本文主要说明以下几种场景3D变换的应用：

1. 3D相机坐标系下长方体物体，有本身坐标系，沿该物体长边方向移动一段距离，并绕长边轴正旋转方向转90度，求解当前物体中心点在相机坐标系下的位置和姿态
2. 多关节机器人末端沿工具坐标系，朝y向移动一段距离，并绕y轴正向旋转90度，求解当前点在机器人坐标系下的位置和姿态

其实综合起来说便是在基坐标系O下有一个点A，A点有自己的用户坐标系O’，A点在自己用户坐标系O’下发生三维平移、旋转变换，变换完成后该点B在基坐标系O下的位置姿态求解。

正文

坐标系三维建模如下图所示：

O：灰色比较大的基坐标系

A：起始点，基座标系下点为[1,2,3]，欧拉角为[-90,0,0]

O’：A点所在处绘制的坐标系

B：结束点，在O’坐标系下A点沿[0,1,0]方向移动2个单位长度

图1 坐标系三维建模–Math3d: Online 3d Graphing Calculator

图2 坐标系旋转变换–Quaternions - Visualisation

计算方法思路

A点在O坐标系下矩阵描述：

注：前三阶代表旋转矩阵，最后一列表示位置[x,y,z,1]

$$\left[\begin{array}{cccc}1& 0& 0& 1\\ 0& 1& 0& 2\\ 0& 0& 1& 3\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(P)}$$

$$[\begin{array}{cc}1& 0\\ & \\ 0& 0\\ 0& 1\\ 0& \end{array}$$