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\)的旋转变换，旋转矩阵分别为（关于旋转矩阵的推导可以参考附录1）：
\[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 平移和旋转坐标系变换
2.1 平移坐标变换
矢量相加
2.2 旋转坐标变换
左乘旋转矩阵
3 欧拉角表示旋转变换
对于旋转矩阵或是四元数，虽能描述旋转，但对人类来说非常不直观。欧拉角提供了一种非常直观的方式描述旋转，即用三个分离的转角
3.1 RPY组合变换
附录I 二维坐标变换
