机器人运动学（一）-旋转矩阵

作为运动学的数学基础，这里将简单介绍一下旋转矩阵
此处为本文学习资料
旋转变换（一）旋转矩阵

1.二维平面内的旋转

假设有一点坐标\((x,y)\)，其中距离\(r=\sqrt{x^2+y^2}\)
我们可以很快地得出原坐标\((x,y)\)

\[x=r \cos\phi \]

\[y=r \sin\phi \]

而旋转后\((x',y')\)则有

\[x'=r \cos(\theta+\phi) \]

\[y'=r \sin(\theta+\phi) \]

带入\(x,y\)的表达式可以得到

\[x'=x\cos\theta-y\sin\theta \]

\[y'=x\sin\theta+y\cos\theta \]

我们将其写成矩阵

\[\begin{bmatrix}x'\\y'\end{bmatrix} = \begin{bmatrix}\cos\theta&-\sin\theta\\ \sin\theta&\cos\theta \end{bmatrix} * \begin{bmatrix}x\\y\end{bmatrix} \]

其中矩阵\(\begin{bmatrix}\cos\theta&-\sin\theta\\ \sin\theta&\cos\theta \end{bmatrix}\)则为一个二维的旋转矩阵

而对于二维平面内绕任一点的旋转，我们可以分三步解决（把大象放进冰箱）

1.把旋转点移到原点
2.绕原点旋转
3.把旋转点移回原位

此时我们已经解决了第二步的操作，接下来讨论第一和第三步该如何实现

平移是一种线性移动，因此可以通过对坐标的线性操作得到
假设我们现在需要将\((x,y)\)移动到\((x',y')\)，那么存在一个实数\(t\)使得
\(x'=x+tx\)、\(y'=y+ty\)

\[\begin{bmatrix}x'\\y'\\1\end{bmatrix} = \begin{bmatrix}1&0&tx\\ 0&1&ty \\0&0&1 \end{bmatrix} * \begin{bmatrix}x\\y\\1\end{bmatrix} \]

可以实现
所以我们可以得到平移矩阵\(\begin{bmatrix}1&0&tx\\ 0&1&ty \\0&0&1 \end{bmatrix}\)实现\((tx,ty)\)的平移

此刻还差最后一步将之前2X2的旋转矩阵扩展到3X3的即

\[\begin{bmatrix}x'\\y'\\1\end{bmatrix} = \begin{bmatrix}\cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\0&0&1 \end{bmatrix} * \begin{bmatrix}x\\y\\1\end{bmatrix} \]

再根据之前的三步可以得到二维平面绕任意一点\((x,y)\)旋转的旋转矩阵为

\[M=\begin{bmatrix}1&0&x\\ 0&1&y \\0&0&1 \end{bmatrix}*\begin{bmatrix}\cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\0&0&1 \end{bmatrix}*\begin{bmatrix}1&0&-x\\ 0&1&-y \\0&0&1 \end{bmatrix} \]

\[M=\begin{bmatrix}\cos\theta&-\sin\theta&(1-\cos\theta)x+y\sin\theta\\ \sin\theta&\cos\theta&(1-\cos\theta)y-x\sin\theta\\0&0&1 \end{bmatrix} \]

2.三维空间内的旋转

由图可得\(P(x,y,z)\)与\(P(x',y',z')\)之间有

\[x'=x\cos\theta-y\sin\theta \]

\[y'=x\sin\theta+y\cos\theta \]

\[z'=z \]

根据前面的推论可以得到三维的绕Z轴旋转的旋转矩阵

\[\begin{bmatrix}x'\\y'\\z'\\1\end{bmatrix} = \begin{bmatrix}\cos\theta&-\sin\theta&0&0\\ \sin\theta&\cos\theta&0&0\\0&0&1&0\\0&0&0&1 \end{bmatrix} * \begin{bmatrix}x\\y\\z\\1\end{bmatrix} \]

同理可得三维的绕X、Y轴旋转的旋转矩阵

\[\begin{bmatrix}x'\\y'\\z'\\1\end{bmatrix} = \begin{bmatrix}1&0&0&0\\0&\cos\theta&-\sin\theta&0\\ 0&\sin\theta&\cos\theta&0\\0&0&0&1 \end{bmatrix} * \begin{bmatrix}x\\y\\z\\1\end{bmatrix} \]

\[\begin{bmatrix}x'\\y'\\z'\\1\end{bmatrix} = \begin{bmatrix}\cos\theta&0&\sin\theta&0\\ 0&1&0&0\\ -\sin\theta&0&\cos\theta&0 \\0&0&0&1 \end{bmatrix} * \begin{bmatrix}x\\y\\z\\1\end{bmatrix} \]

（关于\(x,y,z\)的轮次变换）

而对于三维空间中绕任意轴的旋转，同样可以拆分成绕坐标轴旋转的组合

在空间中有一点\(P(x,y,z)\)，其绕向量u逆时针旋转的度数为\(\theta\)，已知向量u的单位方向向量的坐标\((u,v,w)\)，其中\(u^2+v^2+w^2=1\)

将其分为5步
1.将向量u绕x轴旋转到xoz平面
2.将向量u绕y轴旋转到z轴
3.绕z轴旋转\(\theta\)角
4.进行步骤2的逆步骤
5.进行步骤1的逆步骤

而绕x轴的旋转矩阵由上述可得

\[\begin{bmatrix}1&0&0&0\\0&\cos\theta&-\sin\theta&0\\ 0&\sin\theta&\cos\theta&0\\0&0&0&1 \end{bmatrix} \]

不妨令\(\alpha\)为其中的\(\theta\)则有\(\tan\alpha=\frac{v}{w}\)
得到旋转矩阵\(R_x(\alpha)\)（绕x轴逆时针旋转\(\alpha\)）

\[R_x(\alpha)= \begin{bmatrix} 1&0&0&0\\ 0 & \frac{w}{\sqrt{v^2+w^2}} & -\frac{v}{\sqrt{v^2+w^2}} & 0\\ 0 & \frac{v}{\sqrt{v^2+w^2}} & \frac{w}{\sqrt{v^2+w^2}} & 0\\ 0&0&0&1 \end{bmatrix} \]

同理旋转矩阵\(R_y(\beta)\)，其中\(\tan\beta=\frac{u}{\sqrt{v^2+w^2}}\)

\[R_y(\beta)= \begin{bmatrix} \frac{\sqrt{v^2+w^2}}{\sqrt{u^2+v^2+w^2}} &0&\frac{u}{\sqrt{u^2+v^2+w^2}}&0\\ 0&1&0&0\\ -\frac{u}{\sqrt{u^2+v^2+w^2}}&0&\frac{\sqrt{v^2+w^2}}{\sqrt{u^2+v^2+w^2}}&0 \\0&0&0&1 \end{bmatrix} \]

逆过程则为\(R_x(-\alpha)\)与\(R_y(-\beta)\)
所以总的旋转矩阵\(M\)为

\[M=R_x(\alpha)R_y(\beta)R_z(-\theta)R_x(-\alpha)R_y(-\beta) \]

通过一系列暴算后

\[M=\begin{bmatrix} u^2+(1-u^2)\cos\theta & uv(1-\cos\theta)-w\sin\theta & uw(1-\cos\theta)+v\sin\theta & 0\\ uv(1-\cos\theta)+w\sin\theta & v^2+(1-v^2)\cos\theta & vw(1-\cos\theta)-u\sin\theta & 0\\ uw(1-\cos\theta)-v\sin\theta & vw(1-\cos\theta)+u\sin\theta & w^2+(1-w^2)\cos\theta & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \]

此时我们便得到空间直角坐标系下绕任意轴旋转的旋转矩阵

然而三维空间中的旋转不只有旋转矩阵一种，还有听上去更加高大上的欧拉角和四元数
欲知后事如何，且听下回分解!─=≡Σ(((つ•̀ω•́)つ