坐标变化

世界坐标

  • 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} 并且由于坐标轴正交,有: pxw=xpxwxw+ypywxw+zpzwxw=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 同理: pyw=yp \vec{p}\cdot \vec{y_w} = y_p

pzw=zp \vec{p}\cdot \vec{z_w} = z_p

向量在某坐标系下的坐标即是在该坐标方向下的投影,令: p=xplxl+yplyl+zplzl \vec{p} = x_{pl} \cdot \vec{x_l} + y_{pl} \cdot \vec{y_l} + z_{pl} \cdot \vec{z_l} 因此有: xpl=pxl=[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=pyl=[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=pzl=[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} 因此: M1M[xpypzp]=M1[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]=M1[xplyplzpl] \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} = M^{-1} \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} 由于 MM 为正交矩阵: M1=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}