坐标变化

世界坐标

  • xw⃗=[1,0,0]T\vec{x_w} = [1,0,0]^T
  • yw⃗=[0,1,0]T\vec{y_w} = [0,1,0]^T
  • zw⃗=[0,0,1]T\vec{z_w} = [0,0,1]^T
  • ow=[0,0,0]To_w = [0,0,0]^T

物体（局部）坐标

  • xl⃗=[e11,e12,e13]T\vec{x_l} = [e_{11},e_{12},e_{13}]^T
  • yl⃗=[e21,e22,e23]T\vec{y_l} = [e_{21},e_{22},e_{23}]^T
  • zl⃗=[e31,e32,e33]T\vec{z_l} = [e_{31},e_{32},e_{33}]^T
  • ol=[lx,ly,lz]To_l = [l_x,l_y,l_z]^T

不考虑原点

在世界坐标下的向量（位置）p⃗=[xp,yp,zp]T\vec{p}=[x_p,y_p,z_p]^T，满足： p⃗=xpxw⃗+ypyw⃗+zpzw⃗ \vec{p} = x_p\vec{x_w} + y_p\vec{y_w} + z_p\vec{z_w} 并且由于坐标轴正交，有： p⃗⋅xw⃗=xpxw⃗⋅xw⃗+ypyw⃗⋅xw⃗+zpzw⃗⋅xw⃗=xp \vec{p}\cdot \vec{x_w} = x_p\vec{x_w} \cdot \vec{x_w} + y_p\vec{y_w} \cdot \vec{x_w} + z_p\vec{z_w} \cdot \vec{x_w} = x_p 同理： p⃗⋅yw⃗=yp \vec{p}\cdot \vec{y_w} = y_p

p⃗⋅zw⃗=zp \vec{p}\cdot \vec{z_w} = z_p

向量在某坐标系下的坐标即是在该坐标方向下的投影，令： p⃗=xpl⋅xl⃗+ypl⋅yl⃗+zpl⋅zl⃗ \vec{p} = x_{pl} \cdot \vec{x_l} + y_{pl} \cdot \vec{y_l} + z_{pl} \cdot \vec{z_l} 因此有： xpl=p⃗⋅xl⃗=[xp,yp,zp]T⋅[e11,e12,e13]T x_{pl} = \vec{p} \cdot \vec{x_l} = [x_p,y_p,z_p]^T \cdot [e_{11}, e_{12}, e_{13}]^T

ypl=p⃗⋅yl⃗=[xp,yp,zp]T⋅[e21,e22,e23]T y_{pl} = \vec{p} \cdot \vec{y_l} = [x_p,y_p,z_p]^T \cdot [e_{21}, e_{22}, e_{23}]^T

zpl=p⃗⋅zl⃗=[xp,yp,zp]T⋅[e31,e32,e33]T z_{pl} = \vec{p} \cdot \vec{z_l} = [x_p,y_p,z_p]^T \cdot [e_{31}, e_{32}, e_{33}]^T

以矩阵形式表达： [xplyplzpl]=[e11e12e13e21e22e23e31e32e33][xpypzp] \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} = \begin{bmatrix} e_{11} & e_{12} & e_{13} \\ e_{21} & e_{22} & e_{23}\\ e_{31} & e_{32} & e_{33} \end{bmatrix} \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} 记： M=[e11e12e13e21e22e23e31e32e33] M = \begin{bmatrix} e_{11} & e_{12} & e_{13} \\ e_{21} & e_{22} & e_{23}\\ e_{31} & e_{32} & e_{33} \end{bmatrix} 即： [xplyplzpl]=M[xpypzp] \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} = M \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} 因此： M−1M[xpypzp]=M−1[xplyplzpl] M^{-1} M \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} = M^{-1} \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} 即： [xpypzp]=M−1[xplyplzpl] \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} = M^{-1} \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} 由于 MM 为正交矩阵： M−1=MT M^{-1} = M^T 因此： [xpypzp]=[e11e21e31e12e22e32e13e23e33][xplyplzpl] \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} =\begin{bmatrix} e_{11} & e_{21} & e_{31} \\ e_{12} & e_{22} & e_{32} \\ e_{13} & e_{23} & e_{33} \end{bmatrix} \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} 考虑原点

由于同一向量在坐标轴平行且原点不同的坐标系下表示相同，对于任何一点 pp，都可以用该坐标系下的原点指向 pp 的向量来表示。

向量 OlP⃗\vec{O_l P} 在物体坐标系下为： [xplyplzpl]=M([xpypzp]−[lxlylz])=M[xpypzp]−M[lxlylz] \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} = M (\begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} - \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix}) = M \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} - M \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} 向量 OwP⃗\vec{O_w P} 在世界坐标系下为： [xpypzp]=MT(OwOl⃗+OlP⃗)=MTOlP⃗+[lxlylz]=MT[xplyplzpl]+[lxlylz] \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} = M^T (\vec{O_w O_l} + \vec{O_lP}) =M^T\vec{O_lP} + \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} = M^T \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} + \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} 可见有矩阵加法，考虑用齐次坐标将其统一起来，这里记： [txtytz]=−M[lxlylz]=−[e11e12e13e21e22e23e31e32e33][lxlylz] \begin{bmatrix} t_{x}\\ t_{y}\\ t_{z} \end{bmatrix} = -M \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} =- \begin{bmatrix} e_{11} & e_{12} & e_{13} \\ e_{21} & e_{22} & e_{23}\\ e_{31} & e_{32} & e_{33} \end{bmatrix} \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} 世界坐标转换到物体（局部）坐标： MW2L=[e11e12e13txe21e22e23tye31e32e33tz0001] M_{W2L} =\begin{bmatrix} e_{11} & e_{12} & e_{13} & t_x\\ e_{21} & e_{22} & e_{23} & t_y\\ e_{31} & e_{32} & e_{33} & t_z\\ 0 & 0 & 0 & 1 \end{bmatrix} 物体（局部）坐标转换到世界坐标： ML2W=[e11e21e31lxe12e22e32lye13e23e33lz0001] M_{L2W} =\begin{bmatrix} e_{11} & e_{21} & e_{31} & l_x\\ e_{12} & e_{22} & e_{32} & l_y\\ e_{13} & e_{23} & e_{33} & l_z\\ 0 & 0 & 0 & 1 \end{bmatrix}