三维空间的坐标变换及其应用

        三维空间的坐标变换分为绕固定轴的旋转平移变换和绕动态轴的旋转平移变换，假设我们有1个全局坐标系A和1个局部坐标系B，Ａ为固定坐标系，Ｂ为动态坐标系，开始时两坐标系重合，对于局部坐标系B，其中有一点$p_b$，我们求变换后$p_b$在全局坐标系下的坐标$p_a$，下面分别进行介绍：

绕固定轴的旋转平移变换

        此时我们让局部坐标系Ｂ整体绕世界坐标系各轴依次旋转，根据旋转顺序的不同，共有６种方式，分别是ＸＹＺ，ＸＺＹ，ＹＸＺ，ＹＺＸ，ＺＸＹ，ＺＹＸ，我们分别求每种方式变换后，$p_b$在全局坐标系下的坐标$p_a$。这里先分别介绍绕各轴旋转和平移对应的变换矩阵。
　　让局部坐标系B先绕全局坐标系A的X轴逆时针旋转 $\alpha$弧度，再沿着A坐标系的各轴分别平移$T{x}_x，T{x}_y,T{x}_z$，则：
$$p_a=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\alpha )& -sin(\alpha )\\ 0& sin(\alpha )& cos(\alpha )\end{array}\right]p_b+\left[\begin{array}{c}T{x}_x\\ T{x}_y\\ T{x}_z\end{array}\right]=R_x{p}_b+Tx$$
　　让局部坐标系B先绕全局坐标系A的Ｙ轴逆时针旋转 $\beta$弧度，再沿着A坐标系的各轴分别平移$T{y}_x，T{y}_y,T{y}_z$，则：
$$p_a=\left[\begin{array}{ccc}cos(\beta )& 0& sin(\beta )\\ 0& 1& 0\\ -sin(\beta )& 0& cos(\beta )\end{array}\right]p_b+\left[\begin{array}{c}T{y}_x\\ T{y}_y\\ T{y}_z\end{array}\right]=R_y{p}_b+Ty$$
　让局部坐标系B先绕全局坐标系A的Z轴逆时针旋转 $\gamma$弧度，再沿着A坐标系的各轴分别平移$T{z}_x，T{z}_y,T{z}_z$，则：
$$p_a=\left[\begin{array}{ccc}cos(\gamma )& -sin(\gamma )& 0\\ sin(\gamma )& cos(\gamma )& 0\\ 0& 0& 1\end{array}\right]p_b+\left[\begin{array}{c}T{z}_x\\ T{z}_y\\ T{z}_z\end{array}\right]=R_z{p}_b+Tz$$

ＸＹＺ变换方式

        即局部坐标系B先绕世界坐标系A的X轴进行旋转，然后在A中做平移，之后依次绕A的Y和Z轴做类似操作，3个旋转平移合在一起如下所示:
$$\begin{array}{cc}{p}_a=& R_z(R_y(R_x{p}_b+Tx)+Ty)+Tz\\ ....=& R_z(R_y{R}_x{p}_b+R_{y}Tx+Ty)+Tz\\ ....=& R_z{R}_y{R}_x{p}_b+R_z{R}_{y}Tx+R_{z}Ty+Tz\\ ....=& Rxyz\ast p_b+Txyz\end{array}$$
从上式可知，合成后的变换也对应一个旋转矩阵和平移向量，旋转矩阵是3次旋转对应的旋转矩阵的乘积，按左乘顺序结合，而平移向量是3次平移向量的组合。

Ｘ Z Y变换方式

        即局部坐标系B先绕世界坐标系A的X轴进行旋转，然后在A中做平移，之后依次绕A的Z和Y轴做类似操作，3个旋转平移合在一起如下所示:
$$\begin{array}{cc}{p}_a=& R_y(R_z(R_x{p}_b+Tx)+Tz)+Ty\\ ....=& R_y(R_z{R}_x{p}_b+R_{z}Tx+Tz)+Ty\\ ....=& R_y{R}_z{R}_x{p}_b+R_y{R}_{z}Tx+R_{y}Tz+Ty\\ ....=& Rxzy\ast p_b+Txzy\end{array}$$

ＹＸＺ变换方式

        即局部坐标系B先绕世界坐标系A的Y轴进行旋转，然后在A中做平移，之后依次绕A的X和Z轴做类似操作，3个旋转平移合在一起如下所示:
$$\begin{array}{cc}{p}_a=& R_z(R_x(R_y{p}_b+Ty)+Tx)+Tz\\ ....=& R_z(R_x{R}_y{p}_b+R_{x}Ty+Tx)+Tz\\ ....=& R_z{R}_x{R}_y{p}_b+R_z{R}_{x}Ty+R_{z}Tx+Tz\\ ....=& Ryxz\ast p_b+Tyxz\end{array}$$

ＹＺＸ变换方式

        即局部坐标系B先绕世界坐标系A的Y轴进行旋转，然后在A中做平移，之后依次绕A的Z和X轴做类似操作，3个旋转平移合在一起如下所示:
$$\begin{array}{cc}{p}_a=& R_x(R_z(R_y{p}_b+Ty)+Tz)+Tx\\ ....=& R_x(R_z{R}_y{p}_b+R_{z}Ty+Tz)+Tx\\ ....=& R_x{R}_z{R}_y{p}_b+R_x{R}_{z}Ty+R_{x}Tz+Tx\\ ....=& Ryzx\ast p_b+Tyzx\end{array}$$

ＺＸＹ变换方式

        即局部坐标系B先绕世界坐标系A的Z轴进行旋转，然后在A中做平移，之后依次绕A的X和Y轴做类似操作，3个旋转平移合在一起如下所示:
$$\begin{array}{cc}{p}_a=& R_y(R_x(R_z{p}_b+Tz)+Tx)+Ty\\ ....=& R_y(R_x{R}_z{p}_b+R_{x}Tz+Tx)+Ty\\ ....=& R_y{R}_x{R}_z{p}_b+R_y{R}_{x}Tz+R_{y}Tx+Ty\\ ....=& Rzxy\ast p_b+Tzxy\end{array}$$

ＺＹＸ变换方式

        即局部坐标系B先绕世界坐标系A的Z轴进行旋转，然后在A中做平移，之后依次绕A的Y和X轴做类似操作，3个旋转平移合在一起如下所示:
$$\begin{array}{cc}{p}_a=& R_x(R_y(R_z{p}_b+Tz)+Ty)+Tx\\ ....=& R_x(R_y{R}_z{p}_b+R_{y}Tz+Ty)+Tx\\ ....=& R_x{R}_y{R}_z{p}_b+R_x{R}_{y}Tz+R_{x}Ty+Tx\\ ....=& Rzyx\ast p_b+Tzyx\end{array}$$

绕动态轴的旋转平移变换

        此时我们让局部坐标系Ｂ整体绕自身坐标系的各个轴依次旋转和平移，由于自身坐标系姿态是随着每一步的旋转和平移发生变化的，故称为绕动态轴的旋转平移变换，同样，根据旋转顺序的不同，共有６种方式，分别是ＸＹＺ，ＸＺＹ，ＹＸＺ，ＹＺＸ，ＺＸＹ，ＺＹＸ，我们分别求每种方式变换后，$p_b$在全局坐标系下的坐标$p_a$。这里先分别介绍绕各轴旋转和平移对应的变换矩阵。
　　让局部坐标系B先绕自身坐标系的X轴逆时针旋转$\alpha$弧度，再沿着旋转后的坐标系的各轴分别平移$T{x}_x，T{x}_y,T{x}_z$，则：
$$p_a=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\alpha )& -sin(\alpha )\\ 0& sin(\alpha )& cos(\alpha )\end{array}\right](p_b+\left[\begin{array}{c}T{x}_x\\ T{x}_y\\ T{x}_z\end{array}\right])=R_x(p_b+Tx)$$
　　让局部坐标系B先绕自身坐标系的Ｙ轴逆时针旋转 $\beta$弧度，再沿着旋转后的坐标系的各轴分别平移$T{y}_x，T{y}_y,T{y}_z$，则：
$$p_a=\left[\begin{array}{ccc}cos(\beta )& 0& sin(\beta )\\ 0& 1& 0\\ -sin(\beta )& 0& cos(\beta )\end{array}\right](p_b+\left[\begin{array}{c}T{y}_x\\ T{y}_y\\ T{y}_z\end{array}\right])=R_y(p_b+Ty)$$
　　让局部坐标系B先绕自身坐标系的Z轴逆时针旋转 $\gamma$弧度，再沿着旋转后的坐标系的各轴分别平移$T{z}_x，T{z}_y,T{z}_z$，则：
$$p_a=\left[\begin{array}{ccc}cos(\gamma )& -sin(\gamma )& 0\\ sin(\gamma )& cos(\gamma )& 0\\ 0& 0& 1\end{array}\right](p_b+\left[\begin{array}{c}T{z}_x\\ T{z}_y\\ T{z}_z\end{array}\right])=R_z(p_b+Tz)$$

X YＺ变换方式

        即局部坐标系B先绕自身坐标系的X轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的Y和Z轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
$$\begin{array}{cc}{p}_a=& R_x(R_y(R_z(p_b+Tz)+Ty)+Tx)\\ ....=& R_x(R_y(R_z{p}_b+R_{z}Tz+Ty)+Tx)\\ ....=& R_x(R_y{R}_z{p}_b+R_y{R}_{z}Tz+R_{y}Ty+Tx)\\ ....=& R_x{R}_y{R}_z{p}_b+R_x{R}_y{R}_{z}Tz+R_x{R}_{y}Ty+R_{x}Tx\\ ....=& Rxyz\ast p_b+Txyz\end{array}$$
从上式可知，合成后的变换同样对应一个旋转矩阵和平移向量，旋转矩阵是3次旋转对应的旋转矩阵的乘积，注意这里按右乘顺序结合，而绕固定轴旋转后的旋转矩阵是按左乘顺序结合，平移向量仍是3次平移向量的组合。

X Z Y变换方式

        即局部坐标系B先绕自身坐标系的X轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的Z和Y轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
$$\begin{array}{cc}{p}_a=& R_x(R_z(R_y(p_b+Ty)+Tz)+Tx)\\ ....=& R_x(R_z(R_y{p}_b+R_{y}Ty+Tz)+Tx)\\ ....=& R_x(R_z{R}_y{p}_b+R_z{R}_{y}Ty+R_{z}Tz+Tx)\\ ....=& R_x{R}_z{R}_y{p}_b+R_x{R}_z{R}_{y}Ty+R_x{R}_{z}Tz+R_{x}Tx\\ ....=& Rxzy\ast p_b+Txzy\end{array}$$

ＹＸＺ变换方式

        即局部坐标系B先绕自身坐标系的Y轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的X和Z轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
$$\begin{array}{cc}{p}_a=& R_y(R_x(R_z(p_b+Tz)+Tx)+Ty)\\ ....=& R_y(R_x(R_z{p}_b+R_{z}Tz+Tx)+Ty)\\ ....=& R_y(R_x{R}_z{p}_b+R_x{R}_{z}Tz+R_{x}Tx+Ty)\\ ....=& R_y{R}_x{R}_z{p}_b+R_y{R}_x{R}_{z}Tz+R_y{R}_{x}Tx+R_{y}Ty\\ ....=& Ryxz\ast p_b+Tyxz\end{array}$$

ＹZ X变换方式

        即局部坐标系B先绕自身坐标系的Y轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的Z和X轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
$$\begin{array}{cc}{p}_a=& R_y(R_z(R_x(p_b+Tx)+Tz)+Ty)\\ ....=& R_y(R_z(R_x{p}_b+R_{x}Tx+Tz)+Ty)\\ ....=& R_y(R_z{R}_x{p}_b+R_z{R}_{x}Tx+R_{z}Tz+Ty)\\ ....=& R_y{R}_z{R}_x{p}_b+R_y{R}_z{R}_{x}Tx+R_y{R}_{z}Tz+R_{y}Ty\\ ....=& Ryzx\ast p_b+Tyzx\end{array}$$

ＺＸＹ变换方式

        即局部坐标系B先绕自身坐标系的Z轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的X和Y轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
$$\begin{array}{cc}{p}_a=& R_z(R_x(R_y(p_b+Ty)+Tx)+Tz)\\ ....=& R_z(R_x(R_y{p}_b+R_{y}Ty+Tx)+Tz)\\ ....=& R_z(R_x{R}_y{p}_b+R_x{R}_{y}Ty+R_{x}Tx+Tz)\\ ....=& R_z{R}_x{R}_y{p}_b+R_z{R}_x{R}_{y}Ty+R_z{R}_{x}Tx+R_{z}Tz\\ ....=& Rzxy\ast p_b+Tzxy\end{array}$$

ＺY X变换方式

        即局部坐标系B先绕自身坐标系的Z轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的Y和X轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
$$\begin{array}{cc}{p}_a=& R_z(R_y(R_x(p_b+Tx)+Ty)+Tz)\\ ....=& R_z(R_y(R_x{p}_b+R_{x}Tx+Ty)+Tz)\\ ....=& R_z(R_y{R}_x{p}_b+R_y{R}_{x}Tx+R_{y}Ty+Tz)\\ ....=& R_z{R}_y{R}_x{p}_b+R_z{R}_y{R}_{x}Tx+R_z{R}_{y}Ty+R_{z}Tz\\ ....=& Rzyx\ast p_b+Tzyx\end{array}$$

综上，对于绕固定轴旋转平移后的变换，我们采用左乘法则结合，对于绕动态轴旋转平移后的变换，我们采用右乘法则结合，不同的旋转和平移顺序将对应不同的旋转和平移向量。当给定一个旋转和平移的顺序后，我们可以根据上面的式子求出局部坐标系中的点在全局坐标系中的坐标。

应用

        设IMU启动时其3轴分别与世界坐标系的3轴平行，IMU自身的坐标系为局部坐标系，世界坐标系为全局坐标系，随着IMU的旋转，其朝向将改变，我们假定IMU是按照roll—>pitch—>yaw的旋转方式达到新的姿态的，并且中间过程是绕固定轴旋转的，roll是绕X轴旋转，pitch是绕Y轴旋转,yaw是绕Z轴旋转，对应上面绕固定轴旋转的XYZ方式，IMU在运动过程中会受到外力，此时的3轴加速度由重力和外力的合力产生，我们可以根据roll—>pitch—>yaw，将重力分解到加速度计的各轴上，然后用实际加速度减去分解后的加速度，得到外力沿3轴的加速度，我们再根据roll—>pitch—>yaw将其重新变换成世界坐标系中沿各轴的加速度，用此来估计IMU在瞬态的位移。
　　重写绕固定轴旋转的XYZ方式：
$$p_a=R_z{R}_y{R}_x{p}_b+R_z{R}_{y}Tx+R_{z}Ty+Tz$$
　　此时不考虑平移，设$p_a=[0,0,g{]}^T$为世界坐标系中重力加速速沿各轴的分量，只有垂直方向有值，$p_b=[a_x,a_y,a_y{]}^T$为重力在IMU各轴上的分量，$rea{l}_a=[r{a}_x,r{a}_y,r{a}_y{]}^T$为实际测得IMU各轴的加速度值，包含外力,$extr{a}_a=[e{a}_x,e{a}_y,e{a}_y{]}^T$为外力在IMU各轴产生的加速度，$worl{d}_a=[w{a}_x,w{a}_y,w{a}_y{]}^T$为外力在世界坐标系各轴产生的加速度，是我们要求解的，则：
$$p_a=R_z{R}_y{R}_x{p}_b$$
$$\Rightarrow \left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{ccc}cos(\gamma )& -sin(\gamma )& 0\\ sin(\gamma )& cos(\gamma )& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}cos(\beta )& 0& sin(\beta )\\ 0& 1& 0\\ -sin(\beta )& 0& cos(\beta )\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\alpha )& -sin(\alpha )\\ 0& sin(\alpha )& cos(\alpha )\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$$
$$\Rightarrow \left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{ccc}cos(\gamma )cos(\beta )& -sin(\gamma )cos(\alpha )+cos(\gamma )sin(\beta )sin(\alpha )& sin(\gamma )sin(\alpha )+cos(\gamma )sin(\beta )cos(\alpha )\\ sin(\gamma )cos(\beta )& cos(\gamma )cos(\alpha )+sin(\gamma )sin(\beta )sin(\alpha )& -cos(\gamma )sin(\alpha )+sin(\gamma )sin(\beta )cos(\alpha )\\ -sin(\beta )& cos(\beta )sin(\alpha )& cos(\beta )cos(\alpha )\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$$
$$\Rightarrow \left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{ccc}cos(\gamma )cos(\beta )& sin(\gamma )cos(\beta )& -sin(\beta )\\ -sin(\gamma )cos(\alpha )+cos(\gamma )sin(\beta )sin(\alpha )& cos(\gamma )cos(\alpha )+sin(\gamma )sin(\beta )sin(\alpha )& cos(\beta )sin(\alpha )\\ sin(\gamma )sin(\alpha )+cos(\gamma )sin(\beta )cos(\alpha )& -cos(\gamma )sin(\alpha )+sin(\gamma )sin(\beta )cos(\alpha )& cos(\beta )cos(\alpha )\end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$
故求出重力在各轴上的分量为:
$$\{\begin{array}{rlr}{a}_x& =& -gsin(\beta )\\ a_y& =& gcos(\beta )sin(\alpha )\\ a_z& =& gcos(\beta )cos(\alpha )\end{array}$$
用实际的加速值减去重力在各轴上的加速度分量，将得到外力在IMU各轴上的分量:
$$\{\begin{array}{rlr}e{a}_x& =& r{a}_x-a_x\\ e{a}_y& =& r{a}_y-a_y\\ e{a}_z& =& r{a}_z-a_z\end{array}$$
再根据公式$p_a=R_z{R}_y{R}_x{p}_b$，求外力沿世界坐标系各轴的分量:
$$\Rightarrow \left[\begin{array}{c}w{a}_x\\ w{a}_y\\ w{a}_z\end{array}\right]=\left[\begin{array}{ccc}cos(\gamma )cos(\beta )& -sin(\gamma )cos(\alpha )+cos(\gamma )sin(\beta )sin(\alpha )& sin(\gamma )sin(\alpha )+cos(\gamma )sin(\beta )cos(\alpha )\\ sin(\gamma )cos(\beta )& cos(\gamma )cos(\alpha )+sin(\gamma )sin(\beta )sin(\alpha )& -cos(\gamma )sin(\alpha )+sin(\gamma )sin(\beta )cos(\alpha )\\ -sin(\beta )& cos(\beta )sin(\alpha )& cos(\beta )cos(\alpha )\end{array}\right]\left[\begin{array}{c}e{a}_x\\ e{a}_y\\ e{a}_z\end{array}\right]$$
　当时间很短时，假定IMU在此期间加速度恒定，此时我们可以算出IMU在此期间的沿各轴的位移，可用于在相机高速运动时，估计其位移。