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

绕固定轴的旋转平移变换

此时我们让局部坐标系Ｂ整体绕世界坐标系各轴依次旋转，根据旋转顺序的不同，共有６种方式，分别是ＸＹＺ，ＸＺＹ，ＹＸＺ，ＹＺＸ，ＺＸＹ，ＺＹＸ，我们分别求每种方式变换后，pbp_bpb在全局坐标系下的坐标pap_apa。这里先分别介绍绕各轴旋转和平移对应的变换矩阵。
　　让局部坐标系B先绕全局坐标系A的X轴逆时针旋转 α\alphaα弧度，再沿着A坐标系的各轴分别平移Txx，Txy,TxzTx_x，Tx_y,Tx_zTxx，Txy,Txz，则：
pa=[1000cos(α)−sin(α)0sin(α)cos(α)]pb+[TxxTxyTxz]=Rxpb+Txp_a=\left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos(\alpha) & -sin(\alpha) \\ 0 & sin(\alpha) & cos(\alpha) \end{matrix} \right] p_b+ \left[ \begin{matrix}Tx_x\\Tx_y\\Tx_z \end{matrix} \right]= R_x p_b+Txpa=⎣⎡1000cos(α)sin(α)0−sin(α)cos(α)⎦⎤pb+⎣⎡TxxTxyTxz⎦⎤=Rxpb+Tx
　　让局部坐标系B先绕全局坐标系A的Ｙ轴逆时针旋转 β\betaβ弧度，再沿着A坐标系的各轴分别平移Tyx，Tyy,TyzTy_x，Ty_y,Ty_zTyx，Tyy,Tyz，则：
pa=[cos(β)0sin(β)010−sin(β)0cos(β)]pb+[TyxTyyTyz]=Rypb+Typ_a=\left[ \begin{matrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1& 0 \\ -sin(\beta) &0 & cos(\beta) \end{matrix} \right] p_b+ \left[ \begin{matrix}Ty_x\\Ty_y\\Ty_z \end{matrix} \right]= R_y p_b+Typa=⎣⎡cos(β)0−sin(β)010sin(β)0cos(β)⎦⎤pb+⎣⎡TyxTyyTyz⎦⎤=Rypb+Ty
　让局部坐标系B先绕全局坐标系A的Z轴逆时针旋转 γ\gammaγ弧度，再沿着A坐标系的各轴分别平移Tzx，Tzy,TzzTz_x，Tz_y,Tz_zTzx，Tzy,Tzz，则：
pa=[cos(γ)−sin(γ)0sin(γ)cos(γ)0001]pb+[TzxTzyTzz]=Rzpb+Tzp_a=\left[ \begin{matrix} cos(\gamma) & -sin(\gamma) & 0 \\sin(\gamma) & cos(\gamma) & 0 \\ 0 & 0 & 1 \end{matrix} \right] p_b+ \left[ \begin{matrix}Tz_x\\Tz_y\\Tz_z \end{matrix} \right]= R_z p_b+Tzpa=⎣⎡cos(γ)sin(γ)0−sin(γ)cos(γ)0001⎦⎤pb+⎣⎡TzxTzyTzz⎦⎤=Rzpb+Tz

ＸＹＺ变换方式

即局部坐标系B先绕世界坐标系A的X轴进行旋转，然后在A中做平移，之后依次绕A的Y和Z轴做类似操作，3个旋转平移合在一起如下所示:
pa=Rz(Ry(Rxpb+Tx)+Ty)+Tz....=Rz(RyRxpb+RyTx+Ty)+Tz....=RzRyRxpb+RzRyTx+RzTy+Tz....=Rxyz∗pb+Txyz\begin{matrix}p_a = &R_z(R_y(R_x p_b+Tx)+Ty)+Tz \\ ....=&R_z(R_yR_x p_b+R_yTx+Ty)+Tz \\ ....=&R_zR_yR_x p_b+R_zR_yTx+R_zTy+Tz \\ ....=&Rxyz*p_b+Txyz \end{matrix} pa=....=....=....=Rz(Ry(Rxpb+Tx)+Ty)+TzRz(RyRxpb+RyTx+Ty)+TzRzRyRxpb+RzRyTx+RzTy+TzRxyz∗pb+Txyz
从上式可知，合成后的变换也对应一个旋转矩阵和平移向量，旋转矩阵是3次旋转对应的旋转矩阵的乘积，按左乘顺序结合，而平移向量是3次平移向量的组合。

Ｘ Z Y变换方式

即局部坐标系B先绕世界坐标系A的X轴进行旋转，然后在A中做平移，之后依次绕A的Z和Y轴做类似操作，3个旋转平移合在一起如下所示:
pa=Ry(Rz(Rxpb+Tx)+Tz)+Ty....=Ry(RzRxpb+RzTx+Tz)+Ty....=RyRzRxpb+RyRzTx+RyTz+Ty....=Rxzy∗pb+Txzy\begin{matrix}p_a = &R_y(R_z(R_x p_b+Tx)+Tz)+Ty \\ ....=&R_y(R_zR_x p_b+R_zTx+Tz)+Ty \\ ....=&R_yR_zR_x p_b+R_yR_zTx+R_yTz+Ty \\ ....=&Rxzy*p_b+Txzy \end{matrix} pa=....=....=....=Ry(Rz(Rxpb+Tx)+Tz)+TyRy(RzRxpb+RzTx+Tz)+TyRyRzRxpb+RyRzTx+RyTz+TyRxzy∗pb+Txzy

ＹＸＺ变换方式

即局部坐标系B先绕世界坐标系A的Y轴进行旋转，然后在A中做平移，之后依次绕A的X和Z轴做类似操作，3个旋转平移合在一起如下所示:
pa=Rz(Rx(Rypb+Ty)+Tx)+Tz....=Rz(RxRypb+RxTy+Tx)+Tz....=RzRxRypb+RzRxTy+RzTx+Tz....=Ryxz∗pb+Tyxz\begin{matrix}p_a = &R_z(R_x(R_y p_b+Ty)+Tx)+Tz \\ ....=&R_z(R_xR_y p_b+R_xTy+Tx)+Tz \\ ....=&R_zR_xR_y p_b+R_zR_xTy+R_zTx+Tz \\ ....=&Ryxz*p_b+Tyxz \end{matrix} pa=....=....=....=Rz(Rx(Rypb+Ty)+Tx)+TzRz(RxRypb+RxTy+Tx)+TzRzRxRypb+RzRxTy+RzTx+TzRyxz∗pb+Tyxz

ＹＺＸ变换方式

即局部坐标系B先绕世界坐标系A的Y轴进行旋转，然后在A中做平移，之后依次绕A的Z和X轴做类似操作，3个旋转平移合在一起如下所示:
pa=Rx(Rz(Rypb+Ty)+Tz)+Tx....=Rx(RzRypb+RzTy+Tz)+Tx....=RxRzRypb+RxRzTy+RxTz+Tx....=Ryzx∗pb+Tyzx\begin{matrix}p_a = &R_x(R_z(R_y p_b+Ty)+Tz)+Tx \\ ....=&R_x(R_zR_y p_b+R_zTy+Tz)+Tx \\ ....=&R_xR_zR_y p_b+R_xR_zTy+R_xTz+Tx \\ ....=&Ryzx*p_b+Tyzx \end{matrix} pa=....=....=....=Rx(Rz(Rypb+Ty)+Tz)+TxRx(RzRypb+RzTy+Tz)+TxRxRzRypb+RxRzTy+RxTz+TxRyzx∗pb+Tyzx

ＺＸＹ变换方式

即局部坐标系B先绕世界坐标系A的Z轴进行旋转，然后在A中做平移，之后依次绕A的X和Y轴做类似操作，3个旋转平移合在一起如下所示:
pa=Ry(Rx(Rzpb+Tz)+Tx)+Ty....=Ry(RxRzpb+RxTz+Tx)+Ty....=RyRxRzpb+RyRxTz+RyTx+Ty....=Rzxy∗pb+Tzxy\begin{matrix}p_a = &R_y(R_x(R_z p_b+Tz)+Tx)+Ty \\ ....=&R_y(R_xR_z p_b+R_xTz+Tx)+Ty \\ ....=&R_yR_xR_z p_b+R_yR_xTz+R_yTx+Ty \\ ....=&Rzxy*p_b+Tzxy \end{matrix} pa=....=....=....=Ry(Rx(Rzpb+Tz)+Tx)+TyRy(RxRzpb+RxTz+Tx)+TyRyRxRzpb+RyRxTz+RyTx+TyRzxy∗pb+Tzxy

ＺＹＸ变换方式

即局部坐标系B先绕世界坐标系A的Z轴进行旋转，然后在A中做平移，之后依次绕A的Y和X轴做类似操作，3个旋转平移合在一起如下所示:
pa=Rx(Ry(Rzpb+Tz)+Ty)+Tx....=Rx(RyRzpb+RyTz+Ty)+Tx....=RxRyRzpb+RxRyTz+RxTy+Tx....=Rzyx∗pb+Tzyx\begin{matrix}p_a = &R_x(R_y(R_z p_b+Tz)+Ty)+Tx \\ ....=&R_x(R_yR_z p_b+R_yTz+Ty)+Tx \\ ....=&R_xR_yR_z p_b+R_xR_yTz+R_xTy+Tx \\ ....=&Rzyx*p_b+Tzyx \end{matrix} pa=....=....=....=Rx(Ry(Rzpb+Tz)+Ty)+TxRx(RyRzpb+RyTz+Ty)+TxRxRyRzpb+RxRyTz+RxTy+TxRzyx∗pb+Tzyx

绕动态轴的旋转平移变换

此时我们让局部坐标系Ｂ整体绕自身坐标系的各个轴依次旋转和平移，由于自身坐标系姿态是随着每一步的旋转和平移发生变化的，故称为绕动态轴的旋转平移变换，同样，根据旋转顺序的不同，共有６种方式，分别是ＸＹＺ，ＸＺＹ，ＹＸＺ，ＹＺＸ，ＺＸＹ，ＺＹＸ，我们分别求每种方式变换后，pbp_bpb在全局坐标系下的坐标pap_apa。这里先分别介绍绕各轴旋转和平移对应的变换矩阵。
　　让局部坐标系B先绕自身坐标系的X轴逆时针旋转α\alphaα弧度，再沿着旋转后的坐标系的各轴分别平移Txx，Txy,TxzTx_x，Tx_y,Tx_zTxx，Txy,Txz，则：
pa=[1000cos(α)−sin(α)0sin(α)cos(α)](pb+[TxxTxyTxz])=Rx(pb+Tx)p_a=\left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos(\alpha) & -sin(\alpha) \\ 0 & sin(\alpha) & cos(\alpha) \end{matrix} \right]( p_b+ \left[ \begin{matrix}Tx_x\\Tx_y\\Tx_z \end{matrix} \right])= R_x ( p_b+Tx)pa=⎣⎡1000cos(α)sin(α)0−sin(α)cos(α)⎦⎤(pb+⎣⎡TxxTxyTxz⎦⎤)=Rx(pb+Tx)
　　让局部坐标系B先绕自身坐标系的Ｙ轴逆时针旋转 β\betaβ弧度，再沿着旋转后的坐标系的各轴分别平移Tyx，Tyy,TyzTy_x，Ty_y,Ty_zTyx，Tyy,Tyz，则：
pa=[cos(β)0sin(β)010−sin(β)0cos(β)](pb+[TyxTyyTyz])=Ry(pb+Ty)p_a=\left[ \begin{matrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1& 0 \\ -sin(\beta) &0 & cos(\beta) \end{matrix} \right] (p_b+ \left[ \begin{matrix}Ty_x\\Ty_y\\Ty_z \end{matrix} \right])= R_y (p_b+Ty)pa=⎣⎡cos(β)0−sin(β)010sin(β)0cos(β)⎦⎤(pb+⎣⎡TyxTyyTyz⎦⎤)=Ry(pb+Ty)
　　让局部坐标系B先绕自身坐标系的Z轴逆时针旋转 γ\gammaγ弧度，再沿着旋转后的坐标系的各轴分别平移Tzx，Tzy,TzzTz_x，Tz_y,Tz_zTzx，Tzy,Tzz，则：
pa=[cos(γ)−sin(γ)0sin(γ)cos(γ)0001](pb+[TzxTzyTzz])=Rz(pb+Tz)p_a=\left[ \begin{matrix} cos(\gamma) & -sin(\gamma) & 0 \\sin(\gamma) & cos(\gamma) & 0 \\ 0 & 0 & 1 \end{matrix} \right]( p_b+ \left[ \begin{matrix}Tz_x\\Tz_y\\Tz_z \end{matrix} \right])= R_z (p_b+Tz)pa=⎣⎡cos(γ)sin(γ)0−sin(γ)cos(γ)0001⎦⎤(pb+⎣⎡TzxTzyTzz⎦⎤)=Rz(pb+Tz)

X YＺ变换方式

即局部坐标系B先绕自身坐标系的X轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的Y和Z轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
pa=Rx(Ry(Rz(pb+Tz)+Ty)+Tx)....=Rx(Ry(Rzpb+RzTz+Ty)+Tx)....=Rx(RyRzpb+RyRzTz+RyTy+Tx)....=RxRyRzpb+RxRyRzTz+RxRyTy+RxTx....=Rxyz∗pb+Txyz\begin{matrix}p_a = &R_x(R_y(R_z(p_b+Tz)+Ty)+Tx) \\ ....=& R_x(R_y(R_zp_b+R_zTz+Ty)+Tx) \\ ....=& R_x(R_yR_zp_b+R_yR_zTz+R_yTy+Tx) \\ ....=& R_xR_yR_zp_b+R_xR_yR_zTz+R_xR_yTy+R_xTx\\ ....=&Rxyz*p_b+Txyz \end{matrix} pa=....=....=....=....=Rx(Ry(Rz(pb+Tz)+Ty)+Tx)Rx(Ry(Rzpb+RzTz+Ty)+Tx)Rx(RyRzpb+RyRzTz+RyTy+Tx)RxRyRzpb+RxRyRzTz+RxRyTy+RxTxRxyz∗pb+Txyz
从上式可知，合成后的变换同样对应一个旋转矩阵和平移向量，旋转矩阵是3次旋转对应的旋转矩阵的乘积，注意这里按右乘顺序结合，而绕固定轴旋转后的旋转矩阵是按左乘顺序结合，平移向量仍是3次平移向量的组合。

X Z Y变换方式

即局部坐标系B先绕自身坐标系的X轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的Z和Y轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
pa=Rx(Rz(Ry(pb+Ty)+Tz)+Tx)....=Rx(Rz(Rypb+RyTy+Tz)+Tx)....=Rx(RzRypb+RzRyTy+RzTz+Tx)....=RxRzRypb+RxRzRyTy+RxRzTz+RxTx....=Rxzy∗pb+Txzy\begin{matrix}p_a = &R_x(R_z(R_y(p_b+Ty)+Tz)+Tx) \\ ....=& R_x(R_z(R_yp_b+R_yTy+Tz)+Tx) \\ ....=& R_x(R_zR_yp_b+R_zR_yTy+R_zTz+Tx) \\ ....=& R_xR_zR_yp_b+R_xR_zR_yTy+R_xR_zTz+R_xTx\\ ....=&Rxzy*p_b+Txzy \end{matrix} pa=....=....=....=....=Rx(Rz(Ry(pb+Ty)+Tz)+Tx)Rx(Rz(Rypb+RyTy+Tz)+Tx)Rx(RzRypb+RzRyTy+RzTz+Tx)RxRzRypb+RxRzRyTy+RxRzTz+RxTxRxzy∗pb+Txzy

ＹＸＺ变换方式

即局部坐标系B先绕自身坐标系的Y轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的X和Z轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
pa=Ry(Rx(Rz(pb+Tz)+Tx)+Ty)....=Ry(Rx(Rzpb+RzTz+Tx)+Ty)....=Ry(RxRzpb+RxRzTz+RxTx+Ty)....=RyRxRzpb+RyRxRzTz+RyRxTx+RyTy....=Ryxz∗pb+Tyxz\begin{matrix}p_a = &R_y(R_x(R_z(p_b+Tz)+Tx)+Ty) \\ ....=&R_y(R_x(R_zp_b+R_zTz+Tx)+Ty) \\ ....=& R_y(R_xR_zp_b+R_xR_zTz+R_xTx+Ty) \\ ....=& R_yR_xR_zp_b+R_yR_xR_zTz+R_yR_xTx+R_yTy\\ ....=&Ryxz*p_b+Tyxz \end{matrix} pa=....=....=....=....=Ry(Rx(Rz(pb+Tz)+Tx)+Ty)Ry(Rx(Rzpb+RzTz+Tx)+Ty)Ry(RxRzpb+RxRzTz+RxTx+Ty)RyRxRzpb+RyRxRzTz+RyRxTx+RyTyRyxz∗pb+Tyxz

ＹZ X变换方式

即局部坐标系B先绕自身坐标系的Y轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的Z和X轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
pa=Ry(Rz(Rx(pb+Tx)+Tz)+Ty)....=Ry(Rz(Rxpb+RxTx+Tz)+Ty)....=Ry(RzRxpb+RzRxTx+RzTz+Ty)....=RyRzRxpb+RyRzRxTx+RyRzTz+RyTy....=Ryzx∗pb+Tyzx\begin{matrix}p_a = &R_y(R_z(R_x(p_b+Tx)+Tz)+Ty) \\ ....=&R_y(R_z(R_xp_b+R_xTx+Tz)+Ty) \\ ....=& R_y(R_zR_xp_b+R_zR_xTx+R_zTz+Ty) \\ ....=& R_yR_zR_xp_b+ R_yR_zR_xTx+ R_yR_zTz+ R_yTy\\ ....=&Ryzx*p_b+Tyzx \end{matrix} pa=....=....=....=....=Ry(Rz(Rx(pb+Tx)+Tz)+Ty)Ry(Rz(Rxpb+RxTx+Tz)+Ty)Ry(RzRxpb+RzRxTx+RzTz+Ty)RyRzRxpb+RyRzRxTx+RyRzTz+RyTyRyzx∗pb+Tyzx

ＺＸＹ变换方式

即局部坐标系B先绕自身坐标系的Z轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的X和Y轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
pa=Rz(Rx(Ry(pb+Ty)+Tx)+Tz)....=Rz(Rx(Rypb+RyTy+Tx)+Tz)....=Rz(RxRypb+RxRyTy+RxTx+Tz)....=RzRxRypb+RzRxRyTy+RzRxTx+RzTz....=Rzxy∗pb+Tzxy\begin{matrix}p_a = &R_z(R_x(R_y(p_b+Ty)+Tx)+Tz) \\ ....=&R_z(R_x(R_yp_b+R_yTy+Tx)+Tz) \\ ....=& R_z(R_xR_yp_b+R_xR_yTy+R_xTx+Tz) \\ ....=& R_zR_xR_yp_b+R_zR_xR_yTy+R_zR_xTx+R_zTz\\ ....=&Rzxy*p_b+Tzxy \end{matrix} pa=....=....=....=....=Rz(Rx(Ry(pb+Ty)+Tx)+Tz)Rz(Rx(Rypb+RyTy+Tx)+Tz)Rz(RxRypb+RxRyTy+RxTx+Tz)RzRxRypb+RzRxRyTy+RzRxTx+RzTzRzxy∗pb+Tzxy

ＺY X变换方式

即局部坐标系B先绕自身坐标系的Z轴进行旋转，然后在旋转后的坐标系中平移，之后依次绕自身坐标的Y和X轴做类似操作，3个旋转平移合在一起如下所示，注意旋转的先后顺序:
pa=Rz(Ry(Rx(pb+Tx)+Ty)+Tz)....=Rz(Ry(Rxpb+RxTx+Ty)+Tz)....=Rz(RyRxpb+RyRxTx+RyTy+Tz)....=RzRyRxpb+RzRyRxTx+RzRyTy+RzTz....=Rzyx∗pb+Tzyx\begin{matrix}p_a = &R_z(R_y(R_x(p_b+Tx)+Ty)+Tz) \\ ....=&R_z(R_y(R_xp_b+R_xTx+Ty)+Tz) \\ ....=& R_z(R_yR_xp_b+R_yR_xTx+R_yTy+Tz) \\ ....=& R_zR_yR_xp_b+R_zR_yR_xTx+R_zR_yTy+R_zTz\\ ....=&Rzyx*p_b+Tzyx \end{matrix} pa=....=....=....=....=Rz(Ry(Rx(pb+Tx)+Ty)+Tz)Rz(Ry(Rxpb+RxTx+Ty)+Tz)Rz(RyRxpb+RyRxTx+RyTy+Tz)RzRyRxpb+RzRyRxTx+RzRyTy+RzTzRzyx∗pb+Tzyx

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

应用

设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方式：
pa=RzRyRxpb+RzRyTx+RzTy+Tzp_a=R_zR_yR_x p_b+R_zR_yTx+R_zTy+Tz pa=RzRyRxpb+RzRyTx+RzTy+Tz
　　此时不考虑平移，设pa=[0,0,g]Tp_a=[0,0,g]^Tpa=[0,0,g]T为世界坐标系中重力加速速沿各轴的分量，只有垂直方向有值，pb=[ax,ay,ay]Tp_b=[a_x,a_y,a_y]^Tpb=[ax,ay,ay]T为重力在IMU各轴上的分量，reala=[rax,ray,ray]Treal_a=[ra_x,ra_y,ra_y]^Treala=[rax,ray,ray]T为实际测得IMU各轴的加速度值，包含外力,extraa=[eax,eay,eay]Textra_a=[ea_x,ea_y,ea_y]^Textraa=[eax,eay,eay]T为外力在IMU各轴产生的加速度，worlda=[wax,way,way]Tworld_a=[wa_x,wa_y,wa_y]^Tworlda=[wax,way,way]T为外力在世界坐标系各轴产生的加速度，是我们要求解的，则：
pa=RzRyRxpbp_a=R_zR_yR_x p_b pa=RzRyRxpb
⇒[00g]=[cos(γ)−sin(γ)0sin(γ)cos(γ)0001][cos(β)0sin(β)010−sin(β)0cos(β)][1000cos(α)−sin(α)0sin(α)cos(α)][axayaz]\Rightarrow \left[ \begin{matrix}0\\0\\g \end{matrix} \right]=\left[ \begin{matrix} cos(\gamma) & -sin(\gamma) & 0 \\sin(\gamma) & cos(\gamma) & 0 \\ 0 & 0 & 1 \end{matrix} \right]\left[ \begin{matrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1& 0 \\ -sin(\beta) &0 & cos(\beta) \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos(\alpha) & -sin(\alpha) \\ 0 & sin(\alpha) & cos(\alpha) \end{matrix} \right] \left[ \begin{matrix}a_x\\a_y\\a_z \end{matrix} \right] ⇒⎣⎡00g⎦⎤=⎣⎡cos(γ)sin(γ)0−sin(γ)cos(γ)0001⎦⎤⎣⎡cos(β)0−sin(β)010sin(β)0cos(β)⎦⎤⎣⎡1000cos(α)sin(α)0−sin(α)cos(α)⎦⎤⎣⎡axayaz⎦⎤
⇒[00g]=[cos(γ)cos(β)−sin(γ)cos(α)+cos(γ)sin(β)sin(α)sin(γ)sin(α)+cos(γ)sin(β)cos(α)sin(γ)cos(β)cos(γ)cos(α)+sin(γ)sin(β)sin(α)−cos(γ)sin(α)+sin(γ)sin(β)cos(α)−sin(β)cos(β)sin(α)cos(β)cos(α)][axayaz]\Rightarrow\left[ \begin{matrix}0\\0\\g \end{matrix} \right]=\left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix}a_x\\a_y\\a_z \end{matrix} \right] ⇒⎣⎡00g⎦⎤=⎣⎡cos(γ)cos(β)sin(γ)cos(β)−sin(β)−sin(γ)cos(α)+cos(γ)sin(β)sin(α)cos(γ)cos(α)+sin(γ)sin(β)sin(α)cos(β)sin(α)sin(γ)sin(α)+cos(γ)sin(β)cos(α)−cos(γ)sin(α)+sin(γ)sin(β)cos(α)cos(β)cos(α)⎦⎤⎣⎡axayaz⎦⎤
⇒[axayaz]=[cos(γ)cos(β)sin(γ)cos(β)−sin(β)−sin(γ)cos(α)+cos(γ)sin(β)sin(α)cos(γ)cos(α)+sin(γ)sin(β)sin(α)cos(β)sin(α)sin(γ)sin(α)+cos(γ)sin(β)cos(α)−cos(γ)sin(α)+sin(γ)sin(β)cos(α)cos(β)cos(α)][00g]\Rightarrow \left[ \begin{matrix}a_x\\a_y\\a_z \end{matrix} \right] =\left[ \begin{matrix}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{matrix} \right] \left[ \begin{matrix}0\\0\\g \end{matrix} \right] ⇒⎣⎡axayaz⎦⎤=⎣⎡cos(γ)cos(β)−sin(γ)cos(α)+cos(γ)sin(β)sin(α)sin(γ)sin(α)+cos(γ)sin(β)cos(α)sin(γ)cos(β)cos(γ)cos(α)+sin(γ)sin(β)sin(α)−cos(γ)sin(α)+sin(γ)sin(β)cos(α)−sin(β)cos(β)sin(α)cos(β)cos(α)⎦⎤⎣⎡00g⎦⎤
故求出重力在各轴上的分量为:
{ax=−gsin(β)ay=gcos(β)sin(α)az=gcos(β)cos(α)\left\{ \begin{aligned} a_x & = & -gsin(\beta) \\ a_y & = & gcos(\beta)sin(\alpha)\\ a_z & = &gcos(\beta)cos(\alpha) \end{aligned} \right. ⎩⎪⎨⎪⎧axayaz===−gsin(β)gcos(β)sin(α)gcos(β)cos(α)
用实际的加速值减去重力在各轴上的加速度分量，将得到外力在IMU各轴上的分量:
{eax=rax−axeay=ray−ayeaz=raz−az\left\{ \begin{aligned} ea_x & = & ra_x - a_x \\ ea_y & = & ra_y - a_y\\ ea_z & = & ra_z - a_z \end{aligned} \right. ⎩⎪⎨⎪⎧eaxeayeaz===rax−axray−ayraz−az
再根据公式pa=RzRyRxpbp_a=R_zR_yR_x p_bpa=RzRyRxpb，求外力沿世界坐标系各轴的分量:
⇒[waxwaywaz]=[cos(γ)cos(β)−sin(γ)cos(α)+cos(γ)sin(β)sin(α)sin(γ)sin(α)+cos(γ)sin(β)cos(α)sin(γ)cos(β)cos(γ)cos(α)+sin(γ)sin(β)sin(α)−cos(γ)sin(α)+sin(γ)sin(β)cos(α)−sin(β)cos(β)sin(α)cos(β)cos(α)][eaxeayeaz]\Rightarrow\left[ \begin{matrix}wa_x\\wa_y\\wa_z \end{matrix} \right]=\left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix}ea_x\\ea_y\\ea_z \end{matrix} \right] ⇒⎣⎡waxwaywaz⎦⎤=⎣⎡cos(γ)cos(β)sin(γ)cos(β)−sin(β)−sin(γ)cos(α)+cos(γ)sin(β)sin(α)cos(γ)cos(α)+sin(γ)sin(β)sin(α)cos(β)sin(α)sin(γ)sin(α)+cos(γ)sin(β)cos(α)−cos(γ)sin(α)+sin(γ)sin(β)cos(α)cos(β)cos(α)⎦⎤⎣⎡eaxeayeaz⎦⎤
　当时间很短时，假定IMU在此期间加速度恒定，此时我们可以算出IMU在此期间的沿各轴的位移，可用于在相机高速运动时，估计其位移。

三维空间的坐标变换及其应用相关推荐

一步步推导由欧拉角到旋转矩阵的计算过程
为了便于理解,我们先来做一道简单的数学题: 如图,坐标系 O X 1 Y 1 OX_1Y_1 OX1Y1经过逆时针旋转 θ \theta θ角变换为坐标系 O X 2 Y 2 OX_2Y_2 OX ...
视觉SLAM：模型介绍、算法框架及应用场景
作者:张长鸿湖南大学校稿:董亚微编辑:郑欣欣@一点人工一点智能原文地址:视觉SLAM:模型介绍.算法框架及应用场景目录 01 什么是SLAM 1.1 相机模型 1.2 相机运动 1.3 建图 ...
欧拉角、旋转矩阵及四元数
欧拉角.旋转矩阵及四元数 1. 简介 2. 欧拉角 2.1 欧拉角定义 2.2 右手系和左手系 2.3 转换流程 3. 旋转矩阵 4. 四元数 4.1 四元数与欧拉角和旋转矩阵之间等效变换 4.2 测 ...
从零开始一起学习SLAM | 三维空间刚体的旋转
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达刚体,顾名思义,是指本身不会在运动过程中产生形变的物体,如相机的运 ...
【自动驾驶】29.坐标变换与坐标轴旋转
从一个坐标系的点变换到另一个坐标系的点,旋转矩阵的角度我们不能直接知道,但是可以通过两个坐标系之间的旋转来间接得到. 如: 世界坐标系有一个点P,我们要描述它,就得给他一个坐标系原点,如果他放在车身坐 ...
三维坐标偏转_三维坐标变换原理-平移, 旋转, 缩放
给定一个二维点(x, y),那么形如(kx, ky, k)的所有三元组就都是等价的,它们就是这个点的齐次坐标(homogeneous).齐次坐标就是将一个原本是n维的向量用一个n+1维向量来表示,是指 ...
AI学习笔记（五）坐标变换与视觉测量
AI学习笔记之坐标变换与视觉测量左右手坐标系即转换左手坐标系.右手坐标系.笛卡尔坐标系左手旋转规则.右手旋转规则左手坐标系和右手坐标系相互转换旋转矩阵欧拉角与万向锁欧拉角万向锁四元数 ...
三维空间坐标的旋转算法详解_三维空间几何坐标变换矩阵.ppt
三维空间几何坐标变换矩阵第7章三维变换 7.1 简介 7.2 三维几何变换 7.3 三维坐标变换 7.1 简介三维平移变换.比例变换可看成是二维情况的直接推广.但旋转变换则不然,因为我们可选取空 ...
【CGAL_网格处理】坐标变换
官方手册阅读文档函数链接:CGAL 5.5 - Polygon Mesh Processing: Polygon Mesh Processing Reference transform() temp ...

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

绕固定轴的旋转平移变换

ＸＹＺ变换方式

Ｘ Z Y变换方式

ＹＸＺ变换方式

ＹＺＸ变换方式

ＺＸＹ变换方式

ＺＹＸ变换方式

绕动态轴的旋转平移变换

X YＺ变换方式

X Z Y变换方式

ＹＸＺ变换方式

ＹZ X变换方式

ＺＸＹ变换方式

ＺY X变换方式

应用

三维空间的坐标变换及其应用相关推荐

最新文章

热门文章