整理自机器人学导论第二章空间描述和变换
描述
位置描述
用三个正交单位矢量表示一个坐标系{A}，如\(X_A,Y_A,Z_A\)
对坐标系中的任何点进行定位，用一个矢量来表示该点的位置，如\(P_A\)，矢量的哥哥元素用下标x,y,z表示，即\(P^A=(p_x,p_y,p_z)\)
姿态描述：
\(X_B,Y_B,Z_B\)是\(B\)坐标系主轴方向的单位向量
\(X_B^A,Y_B^A,Z_B^A\)是\(B\)坐标系三个主轴方向的单位向量在坐标系\(A\)中的表示
则旋转矩阵
\[R_B^A=[X_B^A,Y_B^A,Z_B^A]= \begin{bmatrix}
r_{11}&r_{12}&r_{13}\\
r_{21}&r_{22}&r_{23}\\
r_{31}&r_{32}&r_{33}\\
\end{bmatrix}\]
也即
\[X_B=X_Ar_{11}+Y_Ar_{21}+Z_Ar_{31}=[X_A,Y_A,Z_A](X_B^A)^T\\Y_B=X_Ar_{12}+Y_Ar_{22}+Z_Ar_{32}=[X_A,Y_A,Z_A](Y_B^A)^T\\Z_B=X_Ar_{13}+Y_Ar_{23}+Z_Ar_{33}=[X_A,Y_A,Z_A](Z_B^A)^T
\]
每个标量\(r_{ij}\)都可以用两组标准正交基中对应向量点积(点积均表示为\(X\cdot Y\))表示，例如
\(X_A\cdot X_B=X_A\cdot (X_Ar_{11}+Y_Ar_{21}+Z_Ar_{31})=r_{11}\)
因此旋转矩阵可以表示为
\[R_B^A=[X_B^A,Y_B^A,Z_B^A]= \begin{bmatrix}
X_A\cdot X_B&X_A\cdot Y_B&X_A\cdot Z_B\\
Y_A\cdot X_B&Y_A\cdot Y_B&Y_A\cdot Z_B\\
Z_A\cdot X_B&Z_A\cdot Y_B&Z_A\cdot Z_B\\
\end{bmatrix}\]
从中也可以看出，旋转矩阵的每一行是单位矢量{\(A\)}在{\(B\)}中的表示，每一列是单位矢量{\(B\)}在{\(A\)}中的表示。
因此\(R_A^B=(R_B^A)^T\)，这表明旋转矩阵的逆等于他的转置，即正交矩阵。
坐标系描述
为了完整描述坐标系的信息，需要使用该坐标系原点相对于另一坐标系的位置，用向量\(P_B^A\)表示，和坐标系的姿态，用\(R_B^A\)表示。即
\[\{B\}=\{R_B^A,P_B^A\}
\]
映射：坐标系到坐标系的变换
平移坐标系的映射
\[P^A=P_B+P_B^A
\]
旋转坐标系的映射
\[P^A=R_B^AP^B
\]
一般坐标系的映射
\[P^A=R_B^AP_B+P_B^A
\]
写成变换矩阵的形式
\[\begin{bmatrix}P^A\\ 0\end{bmatrix}=\begin{bmatrix}
R_B^A&P_B^A\\
0&1\\
\end{bmatrix}\begin{bmatrix}P^B\\ 0\end{bmatrix}\]
从映射的角度看，原矢量\(P\)在空间中没有改变，只是求出了这个矢量相对于另一个坐标系的新的描述
算子：点的平移、旋转和变换
平移算子
\[P_2^A=D_QP_1^A
\]
\[D_Q=\begin{bmatrix}
1&0&0&q_x\\
0&1&0&q_y\\
0&0&1&q_z\\
0&0&0&1\\
\end{bmatrix}\]
旋转算子
\[P_2^A=R_K(\theta)P_1^A
\]
\(R_K(\theta)\)表示绕\(K\)轴旋转\(\theta\)角的旋转矩阵
绕坐标轴的齐次旋转矩阵形式
\[R_X(\theta)=\begin{bmatrix}
1&0&0&0\\
0&\cos\theta&-\sin\theta&0\\
0&\sin\theta&\cos\theta&0\\
0&0&0&1\\
\end{bmatrix}\]
\[R_Y(\theta)=\begin{bmatrix}
\cos\theta&0&\sin\theta&0\\
0&1&0&0\\
-\sin\theta&0&\cos\theta&0\\
0&0&0&1\\
\end{bmatrix}\]
\[R_Z(\theta)=\begin{bmatrix}
\cos\theta&-\sin\theta&0&0\\
\sin\theta&\cos\theta&0&0\\
0&0&1&0\\
0&0&0&1\\
\end{bmatrix}\]
变换算子
\[P_2^A=T_AP_1^A
\]
和映射中的数学描述相同
变换算法
混合变换
\[P_B=T_C^BP^C
\]
\[P_A=T_B^AP_B
\]
\[P_A=T_B^AT_C^BP^C
\]
因此
\[T_C^A=T_B^AT_C^B
\]
逆变换
旋转矩阵中
\[R_A^B=(R_B^A)^T
\]
将\(P_B^A\)转换为在{B}中的描述
\[(P_B^A)^B=R_A^BP_B^A+P_A^B
\]
所以
\[P_A^B=-R_A^BP_B^A=-(R_B^A)^TP_B^A
\]
因此
\[T_A^B=\begin{bmatrix}
(R_B^A)^T&-(R_B^A)^TP_B^A\\
0&1\\
\end{bmatrix}\]
姿态其他描述方法
X-Y-Z固定坐标系
\[(R_B^A)_{XYZ}(\alpha,\beta,\gamma)=R_Z(\gamma)R_Y(\beta)R_X(\alpha)\\=
\begin{bmatrix}
\cos\gamma&-\sin\gamma&0\\
\sin\gamma&\cos\gamma&0\\
0&0&1\\
\end{bmatrix}
\begin{bmatrix}
\cos\beta&0&\sin\beta\\
0&1&0\\
-\sin\beta&0&\cos\beta\\
\end{bmatrix}
\begin{bmatrix}
1&0&0\\
0&\cos\alpha&-\sin\alpha\\
0&\sin\alpha&\cos\alpha\\
\end{bmatrix}\]
\[=\begin{bmatrix}
\cos\beta\cos\gamma&\cos\gamma\sin\alpha\sin\beta-\cos\alpha\sin\gamma&\sin\alpha\sin\gamma+\cos\alpha\cos\gamma\sin\beta\\
\cos\beta\sin\gamma&\cos\alpha\cos\gamma+\sin\alpha\sin\beta\sin\gamma&\cos\alpha\sin\beta\sin\gamma-\cos\gamma\sin\alpha\\
-\sin\beta&\cos\beta\sin\alpha&\cos\alpha\cos\beta\\
\end{bmatrix}
\]
简化理解，由于是绕固定坐标轴旋转，每次旋转都要在上一次旋转得到的世界坐标基础上得到新的世界坐标系，因此绕XYZ旋转矩阵是依次每次旋转左乘。
Z-Y-X欧拉角
\[R_B^A=R_{B'}^AR_{B''}^{B'}R_{B}^{B''}
\]
其中\(B'\)和\(B''\)都是中间坐标系，\(B'\)绕\(Z\)轴旋转\(\gamma\)角，\(B''\)绕\(Y\)轴旋转\(\beta\)角，\(B\)绕\(X\)轴旋转\(\alpha\)角
\[(R_B^A)_{ZYX}(\gamma,\beta,\alpha)=R_Z(\gamma)R_Y(\beta)R_X(\alpha)\\
=\begin{bmatrix}
\cos\gamma&-\sin\gamma&0\\
\sin\gamma&\cos\gamma&0\\
0&0&1\\
\end{bmatrix}
\begin{bmatrix}
\cos\beta&0&\sin\beta\\
0&1&0\\
-\sin\beta&0&\cos\beta\\
\end{bmatrix}
\begin{bmatrix}
1&0&0\\
0&\cos\alpha&-\sin\alpha\\
0&\sin\alpha&\cos\alpha\\
\end{bmatrix}\]
\[=\begin{bmatrix}
\cos\beta\cos\gamma&\cos\gamma\sin\alpha\sin\beta-\cos\alpha\sin\gamma&\sin\alpha\sin\gamma+\cos\alpha\cos\gamma\sin\beta\\
\cos\beta\sin\gamma&\cos\alpha\cos\gamma+\sin\alpha\sin\beta\sin\gamma&\cos\alpha\sin\beta\sin\gamma-\cos\gamma\sin\alpha\\
-\sin\beta&\cos\beta\sin\alpha&\cos\alpha\cos\beta\\
\end{bmatrix}
\]
简化理解，由于是绕运动坐标轴旋转，坐标点在每次旋转之后新坐标系中的坐标不变，始终等于在最初坐标系中的坐标，因此使用这个坐标求出在世界坐标系坐标时，要依次以相反的顺序左乘，即依次求出在上一个坐标系中的坐标。
同时也可以看出三次绕固定轴的旋转最终姿态和以相反顺序绕运动坐标轴的旋转最终姿态相同
旋转本身并没有缺陷,因为每个旋转都可以用一个旋转轴和一个旋转角表示，欧拉角死锁的缺陷是将一个旋转分解为绕三个坐标轴的旋转造成的，使用欧拉角在大多数情况都是正确的，只是在\(\beta=90\)度时，旋转矩阵变为
\[\begin{bmatrix}
0&\cos\gamma\sin\alpha-\cos\alpha\sin\gamma&\sin\alpha\sin\gamma+\cos\alpha\cos\gamma\\
0&\cos\alpha\cos\gamma+\sin\alpha\sin\gamma&\cos\alpha\sin\gamma-\cos\gamma\sin\alpha\\
-\sin\beta&0&0\\
\end{bmatrix}\\
=\begin{bmatrix}
0&\sin(\alpha-\gamma)&\cos(\alpha-\gamma)\\
0&\cos(\alpha-\gamma)&-\sin(\alpha-\gamma)\\
-1&0&0\\
\end{bmatrix}
\]
里\(\alpha\)和\(\gamma\)带来的效果是一样的，损失了一个自由度。
四元数
由等效旋转轴和等效旋转角定义
\[q=\begin{bmatrix}
q_1\\
q_2\\
q_3\\
q_4\\
\end{bmatrix}=\begin{bmatrix}
n_x\sin\frac{\theta}{2}\\
n_y\sin\frac{\theta}{2}\\
n_z\sin\frac{\theta}{2}\\
\cos\frac{\theta}{2}\\
\end{bmatrix}\]
其中\(\theta\)是旋转角，\(n_x,n_y,n_z\)是旋转轴的方向余弦，满足\(q_1^2+q_2^2+q_3^2+q_4^2=1\)
