智能车坐标变换推导

原创

已于 2024-04-22 11:29:28 修改 · 1k 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#python #算法 #人工智能

于 2024-04-19 00:02:37 首次发布

一个坐标系可以通过一个原点 $O$ 和三个基来 $e1,e2,e3\boldsymbol{e_1},\boldsymbol{e_2},\boldsymbol{e_3}$ 确定。

那么对于世界坐标系，其原点 $O_w$ 为：
$O_w = \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ \end{array} \right)$
其基为：
$\boldsymbol{e_1} = \left( \begin{array}{c} 1 \\ 0 \\ 0 \\ \end{array} \right) \ \boldsymbol {e_2} = \left( \begin{array}{c} 0 \\ 1 \\ 0 \\ \end{array} \right) \ \boldsymbol {e_3} = \left( \begin{array}{c} 0 \\ 0 \\ 1 \\ \end{array} \right)$
现在设另一个坐标系 $O_c$ ，其原点坐标 $O_c$ 在世界坐标系内的坐标为 $B = (t_1,t_2,t_3)^{T}$ ，所以有：
$O_c = O_w +\left( \begin{array}{c} \boldsymbol{e_{1}} \ \boldsymbol{e_{2}} \ \boldsymbol{e_{3}} \end{array} \right) \left( \begin{array}{c} t_1 \\ t_2 \\ t_3 \\ \end{array} \right)$
其基 $ec1,ec2,ec3\boldsymbol{e_{c1}},\boldsymbol{e_{c2}},\boldsymbol{e_{c3}}$ 与 $O_w$ 坐标系的基 $e1,e2,e3\boldsymbol{e_1},\boldsymbol{e_2},\boldsymbol{e_3}$ 的变化矩阵为：
$\left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c3}} \end{array} \right)= \left( \begin{array}{c} \boldsymbol{e_{1}} \ \boldsymbol{e_{2}} \ \boldsymbol{e_{3}} \end{array} \right) \boldsymbol{M}$
设一点 $P$ 在 $O_w$ 的坐标为 $X_w,Y_w,Z_w)^T$ ：
$O_w + \left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c2}} \end{array} \right) \left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ \end{array} \right)$
由（3）和（4）式得：
$O_w = O_c - \left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c3}} \end{array} \right) \boldsymbol{M^{-1}} \left( \begin{array}{c} t_1 \\ t_2 \\ t_3 \\ \end{array} \right)$

最低0.47元/天解锁文章