1 位姿和坐标系描述
1.1 位置描述
对于直角坐标系{A}，空间任一点p的位置可用3×1的列矢量\(^Ap\)表示
\[^Ap=\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix} \]\(p_x,p_y,p_z\)是点p在坐标系{A}中x,y,z三个轴方向的坐标分量，上标A代表参考坐标系{A}，\(^Ap\)称为位置矢量
1.2 方位描述
物体的方位可由某个固接于此物体的坐标系描述。为了规定空间某刚体B的方位，设置一直角坐标系{B}与此刚体固接。用坐标系{B}的三个单位主矢量\(x_B,y_B,z_B\)相对于参考坐标系{A}的方向余弦组成的3×3矩阵
\[^A_BR=\begin{bmatrix} ^Ax_{_B} & ^Ay_{_B} & ^Az_{_B} \end{bmatrix} =\begin{bmatrix} r_{_{11}}&r_{_{12}}&r_{_{13}}\\ r_{_{21}}&r_{_{22}}&r_{_{23}}\\ r_{_{31}}&r_{_{32}}&r_{_{33}}\\ \end{bmatrix} \]表示激光雷达传感器L相对于车体坐标系的方位。\(^A_BR\)称为“旋转矩阵”，其中，上标A代表参考坐标系{A}，下标B代表被描述的坐标系{B}，即B相当于A的旋转矩阵。\(^A_BR\)共有9个元素，但只有3个是独立的。由于\(^A_BR\)的三个列矢量都是单位向量，且两两垂直，所以旋转矩阵\(^A_BR\)是正交矩阵，满足
\[^A_BR^{-1}= ^A_BR^{T}\\ |^A_BR|=1 \]对应于轴x，y或z作转角为\(\theta\)的旋转变换，旋转矩阵分别为：
\[R(x,\theta)=\begin{bmatrix} 1&0&0 \\ 0&c\theta&-s\theta \\ 0&s\theta&c\theta \end{bmatrix} \\ R(y,\theta)=\begin{bmatrix} c\theta&0&s\theta \\ 0&1&0 \\ -s\theta&0&c\theta \end{bmatrix} \\ R(x,\theta)=\begin{bmatrix} c\theta&-s\theta&0 \\ s\theta&c\theta&0 \\ 0&0&1 \end{bmatrix} \]其中，s表示sin，c表示cos
1.3 位姿描述
要完全描述刚体B在空间的位姿（位置和姿态），通常将其与某一坐标系{B}相固接。{B}的坐标原点一般选刚体的特征点，如质心。相对于参考系{A}，坐标系{B}的原点位置和坐标轴的方位，分别由位置矢量和旋转矩阵描述，即刚体B的位姿由坐标系{B}来描述：
\[{B}=\{^A_BR~~~^Ap_{_Bo} \} \]2 平移和旋转坐标系变换
TODO...
标签：平移,begin,end,BR,变换,坐标,bmatrix,theta,坐标系 From： https://www.cnblogs.com/xdhisgood-xy/p/18182548