机器人学入门笔记

机器人学主要包含三个方面：

  • 描述一个物体的坐标

  • 运动学

    • 顺运动学：根据肌肉等的状态知道“手”的位置和动作

    • 逆运动学：根据想要的动作，逆解出肌肉的状态

  • 运动轨迹问题

旋转矩阵

描述一个平面中的物体需要三个自由度，即 xx 轴、yy 轴和转动，同理，描述三维中的物体就需要六个自由度，即 xx 轴、yy 轴、zz 轴和三种相应的转动。

要描述一个三维空间的物体，可以在质心标记它的位置，要记录它的转动形态，就可以在质心上建立三个坐标（body frame），然后计算它们和世界坐标（world frame）的投影：

其中，BAR^A_BR 表示 body frame BB 对 world frame AA 的坐标，依次用 XX、YY、ZZ 的 column vector （长度都是 1）表示，也表示三主轴对 frame AA 的投影。叫做旋转矩阵。

可以得到一个特性：

BAR=[X^A⋅X^BX^A⋅Y^BX^A⋅Z^BY^A⋅X^BY^A⋅Y^BY^A⋅Z^BZ^A⋅X^BZ^A⋅Y^BZ^A⋅Z^B]=[X^B⋅X^AY^B⋅X^AZ^B⋅X^AX^B⋅Y^AY^B⋅Y^AZ^B⋅Y^AX^B⋅Z^AY^B⋅Z^AZ^B⋅Z^A]=[X^B⋅X^AX^B⋅Y^AX^B⋅Z^AY^B⋅X^AY^B⋅Y^AY^B⋅Z^AZ^B⋅X^AZ^B⋅Y^AZ^B⋅Z^A]T=(ABR)T⇒BAR=ABRT\begin{align*} {}^{A}_{B}R &= \begin{bmatrix} \hat{X}_A \cdot \hat{X}_B & \hat{X}_A \cdot \hat{Y}_B & \hat{X}_A \cdot \hat{Z}_B \\ \hat{Y}_A \cdot \hat{X}_B & \hat{Y}_A \cdot \hat{Y}_B & \hat{Y}_A \cdot \hat{Z}_B \\ \hat{Z}_A \cdot \hat{X}_B & \hat{Z}_A \cdot \hat{Y}_B & \hat{Z}_A \cdot \hat{Z}_B \end{bmatrix} \\ &= \begin{bmatrix} \hat{X}_B \cdot \hat{X}_A & \hat{Y}_B \cdot \hat{X}_A & \hat{Z}_B \cdot \hat{X}_A \\ \hat{X}_B \cdot \hat{Y}_A & \hat{Y}_B \cdot \hat{Y}_A & \hat{Z}_B \cdot \hat{Y}_A \\ \hat{X}_B \cdot \hat{Z}_A & \hat{Y}_B \cdot \hat{Z}_A & \hat{Z}_B \cdot \hat{Z}_A \end{bmatrix} \\ &= \begin{bmatrix} \hat{X}_B \cdot \hat{X}_A & \hat{X}_B \cdot \hat{Y}_A & \hat{X}_B \cdot \hat{Z}_A \\ \hat{Y}_B \cdot \hat{X}_A & \hat{Y}_B \cdot \hat{Y}_A & \hat{Y}_B \cdot \hat{Z}_A \\ \hat{Z}_B \cdot \hat{X}_A & \hat{Z}_B \cdot \hat{Y}_A & \hat{Z}_B \cdot \hat{Z}_A \end{bmatrix}^ T \\ &= ({}^{B}_{A}R)^{T} \end{align*} \\ \Rightarrow \boxed{^A_BR= {}^{B}_{A}R^{T}}

而且显然可以得到：

BART⋅BART=I3^A_B R^ T \cdot ^A_BR^T =I_3

所以旋转矩阵属于正交矩阵，它的逆可以很简单地得到：

R−1=RTR^{-1}=R^T

正交矩阵的行列式 ∣R∣=±1|R|=\pm 1，

  • 当 ∣R∣=1|R|=1 时，该操作为纯旋转
  • 当 ∣R∣=−1|R|=-1 时，该操作为反射，包含了镜像，改变了坐标系的手性

另外，虽然这里 RR 有九个数字，但是它需要满足：

  • 组成它们的方向向量模长为 1
  • 方向向量互相垂直

这就嵌入了六个条件，相当于九元方程组已经有了六个确定的方程，还剩三个方程组，也就和“需要三个自由度”是等价的了。

Fixed angle 旋转

Fixed angle 旋转指的是这个物体依次绕空间中固定不动的坐标轴（world frame）进行旋转，最后也可以得到一个旋转矩阵。

绕固定坐标系 X,Y,ZX,Y,Z 轴旋转角度 α,β,γ\alpha,\beta,\gamma，则对应的基本旋转矩阵为：

Rx(α)=[1000cos⁡α−sin⁡α0sin⁡αcos⁡α]Ry(β)=[cos⁡β0sin⁡β010−sin⁡β0cos⁡β]Rz(γ)=[cos⁡γ−sin⁡γ0sin⁡γcos⁡γ0001]R_x(\alpha)= \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\alpha & -\sin\alpha\\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} \\ R_y(\beta)= \begin{bmatrix} \cos\beta & 0 & \sin\beta\\ 0 & 1 & 0\\ -\sin\beta & 0 & \cos\beta \end{bmatrix} \\ R_z(\gamma)= \begin{bmatrix} \cos\gamma & -\sin\gamma & 0\\ \sin\gamma & \cos\gamma & 0\\ 0 & 0 & 1 \end{bmatrix}

若使用 Fixed angle，按绕 XX轴、YY 轴、ZZ 轴的旋转顺序分别旋转角度 α,β,γ\alpha,\beta,\gamma，它的整体旋转矩阵为

R=Rz(γ) Ry(β) Rx(α)=[cos⁡γ−sin⁡γ0sin⁡γcos⁡γ0001]⋅[cos⁡β0sin⁡β010−sin⁡β0cos⁡β]⋅[1000cos⁡α−sin⁡α0sin⁡αcos⁡α]=[cγcβcγsβsα−sγcαcγsβcα+sγsαsγcβsγsβsα+cγcαsγsβcα−cγsα−sβcβsαcβcα]，其中，cθ=cos⁡θ,sθ=sin⁡θ=[r11r12r13r21r22r23r31r32r33]\begin{align*} R &= 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}\cdot \begin{bmatrix} \cos\beta & 0 & \sin\beta\\ 0 & 1 & 0\\ -\sin\beta & 0 & \cos\beta \end{bmatrix} \cdot\begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\alpha & -\sin\alpha\\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} \\&= \begin{bmatrix} c_\gamma c_\beta & c_\gamma s_\beta s_\alpha - s_\gamma c_\alpha & c_\gamma s_\beta c_\alpha + s_\gamma s_\alpha \\ s_\gamma c_\beta & s_\gamma s_\beta s_\alpha + c_\gamma c_\alpha & s_\gamma s_\beta c_\alpha - c_\gamma s_\alpha \\ - s_\beta & c_\beta s_\alpha & c_\beta c_\alpha \end{bmatrix} \text{，其中，$c_\theta=\cos\theta, s_\theta=\sin\theta$} \\&=\textcolor{red}{ \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33}\\ \end{bmatrix}} \end{align*}

那么同时也可以通过得到的 R=[r11r12r13r21r22r23r31r32r33]R=\begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33}\\ \end{bmatrix}，反推出三个角的值。当 β≠90°\beta\ne 90\degree 时[1]，可以得到下面的解：

β=atan2(−r31,r112+r212),α=atan2(r21/cβ,r11/cβ),γ=atan2(r32/cβ,r33/cβ)\begin{align*} \beta &= \mathrm{atan2}(-r_{31}, \sqrt{r_{11}^2+r_{21}^2}),\\ \alpha &= \mathrm{atan2}(r_{21} / c_\beta, r_{11} / c_\beta),\\ \gamma &= \mathrm{atan2}(r_{32} / c_\beta, r_{33} / c_\beta) \end{align*}

β=±90°\beta=\pm 90\degree 时可以得到其中一组比较特别的解：

{β=90°α=0γ=atan2(r12,r22){β=−90°α=0γ=−atan2(r12,r22)\begin{cases} \beta=90\degree \\ \alpha=0\\ \gamma=\mathrm{atan2}(r_{12}, r_{22}) \end{cases} \qquad \begin{cases} \beta=-90\degree \\ \alpha=0\\ \gamma=-\mathrm{atan2}(r_{12}, r_{22}) \end{cases}

另外需要注意，虽然描述顺序是 X→Y→ZX \rightarrow Y \rightarrow Z，但矩阵乘法顺序是从右往左的，因为后发生的旋转在左侧。

Euler angle 旋转

Euler angle 旋转就是绕当前坐标轴 ZZ、YY、XX 进行旋转，注意这里的轴是物体自己的轴，会随旋转变化。

它可以和 fixed angle 旋转对应起来：

Fixed XYZ≡Euler ZYX\text{Fixed } XYZ \equiv \text{Euler } ZYX

但是一般使用 ZYZZYZ Euler angle 旋转来举例。例如旋转过程为：

  • 绕 ZZ 轴旋转 α\alpha，
  • 绕新的 YY 轴旋转 β\beta，
  • 绕新的 ZZ 轴旋转 γ\gamma，

就可以得到对应的旋转矩阵，注意这里是先旋转的操作放在左边了：

R=Rz(α) Ry(β) Rz(γ)R = R_z(\alpha)\,R_y(\beta)\,R_z(\gamma)

同理可以得到解：

{β=atan2(r312+r322,r33)≠0α=atan2(r23/sβ,r13/sβ)γ=atan2(r32/sβ,−r31/sβ){β=90°α=0γ=atan2(−r12,r11){β=180°α=0γ=−atan2(r12,−r11)\begin{cases} \beta &= \mathrm{atan2}(\sqrt{r_{31}^2+r_{32}^2}, r_{33})\ne 0\\ \alpha &= \mathrm{atan2}(r_{23} / s_\beta, r_{13} / s_\beta)\\ \gamma &= \mathrm{atan2}(r_{32} / s_\beta, -r_{31} / s_\beta) \end{cases} \qquad \begin{cases} \beta&=90\degree \\ \alpha&=0\\ \gamma&=\mathrm{atan2}(-r_{12}, r_{11}) \end{cases} \qquad \begin{cases} \beta&=180\degree \\ \alpha&=0\\ \gamma&=-\mathrm{atan2}(r_{12}, -r_{11}) \end{cases}

参考和注解

  1. 因为等于 90 度的时候这个解就不唯一了，这个叫万向节锁。 ↖
具身智能
机器人学入门笔记
https://blog.kisechan.space/2026/robotics/
作者
Kisechan
发布于
2026年3月26日
更新于
2026年3月26日
许可协议