空间表示与坐标变换

主要分为两个部分，一是定点在不同坐标系下坐标的变换，二是点在同一坐标系下坐标的变换。

坐标系变换

考虑将坐标系的变换分成两个部分：旋转与平移。

旋转变换

BAR=[AX^BAY^BAZ^B]{}^A_B R = \begin{bmatrix}{}^A\hat X_B & {}^A\hat Y_B & {}^A\hat Z_B \end{bmatrix}

其中 AX^B{}^A\hat X_B 是 B 坐标系的 X 轴在 A 坐标系下的坐标。

AP=BARBP{}^A P = {}^A_B R{}^BP

由于 BAR{}^A_BR 是一个正交矩阵，所以 ABR=BAR−1=BART{}^B_A R = {}^A_B R^{-1} = {}^A_B R^T。

平移和旋转变换

AP=BARBP+APBORG{}^AP = {}^A_B R{}^BP+{}^AP_{BORG}

构建矩阵 BAT{}^A_BT 如下：

BAT=[BARAPBORG0001]{}^A_BT = \begin{bmatrix}{}^A_B R & {}^AP_{BORG}\\ \begin{matrix}0&0&0\end{matrix}&1\end{bmatrix}

可得

[AP1]=BAT[BP1]\begin{bmatrix}{}^AP \\ 1\end{bmatrix} = {}^A_BT\begin{bmatrix}{}^BP \\ 1\end{bmatrix}

由于 BAT{}^A_BT 是一个齐次矩阵，所以可以连续左乘来表示一系列变换。

另外，BAT{}^A_BT 的逆矩阵为：

BAT−1=ABT=[BART−BARTAPBORG0001]{}^A_BT^{-1} = {}^B_A T = \begin{bmatrix}{}^A_B R^T & -{}^A_B R^T {}^AP_{BORG}\\ \begin{matrix}0&0&0\end{matrix}&1\end{bmatrix}

位置变换

位置变换同样分为旋转与平移两部分。

平移变换

AP2=AP1+AQ{}^AP_2 = {}^AP_1+{}^AQ

为了统一表达，我们尝试将平移变换也表示为矩阵乘法的形式，构建矩阵 DQ(q)D_Q(q) 如下：

DQ(q)=[100qx010qy001qz0001]D_Q(q) = \begin{bmatrix}1&0&0&q_x\\ 0&1&0&q_y\\ 0&0&1&q_z\\ 0&0&0&1\end{bmatrix}

其中 [qx,qy,qz]T[q_x, q_y, q_z]^T 是 AQ{}^AQ 的坐标。

旋转变换

为了方便，我们仅考虑绕某一坐标轴旋转的情况，其他更普遍的情况可以通过绕三个坐标轴的旋转来实现。

绕 X、Y、Z 轴旋转的矩阵分别为：

RX(θ)=[10000cos⁡θ−sin⁡θ00sin⁡θcos⁡θ00001]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}

RY(θ)=[cos⁡θ0sin⁡θ00100−sin⁡θ0cos⁡θ00001]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}

RZ(θ)=[cos⁡θ−sin⁡θ00sin⁡θcos⁡θ0000100001]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}

最终的位置变换矩阵可以通过平移和旋转矩阵的乘积来表示，以绕 Z 轴旋转为例：

T=DQ(q)RZ(θ)=[cos⁡θ−sin⁡θ0qxsin⁡θcos⁡θ0qy001qz0001]T = D_Q(q)R_Z(\theta) = \begin{bmatrix}\cos\theta&-\sin\theta&0&q_x\\ \sin\theta&\cos\theta&0&q_y\\ 0&0&1&q_z\\ 0&0&0&1\end{bmatrix}

[AP21]=T[AP11]\begin{bmatrix}{}^AP_2 \\ 1\end{bmatrix} = T\begin{bmatrix}{}^AP_1 \\ 1\end{bmatrix}

朝向表示

有关欧拉角表示朝向导致的万向锁问题和四元数的详细介绍，可参考这两篇文章：四元数与三维旋转和 Bonus: Gimbal Lock，这里仅做简单说明。

用欧拉角等绕坐标轴旋转的方法表示朝向时，存在万向锁问题，即当某个旋转角度达到特定值时，两个旋转轴会重合，导致失去一个自由度，无法区分两个旋转轴的旋转，具体表现为无法通过旋转矩阵求出所有的欧拉角。

比如，当绕 Y 轴旋转角度 β=90∘\beta = 90^\circ 时，

BARXYZ(γ,β,α)=[0cos⁡αsin⁡γ−sin⁡αcos⁡γcos⁡αcos⁡γ+sin⁡αsin⁡γ0sin⁡αsin⁡γ+cos⁡αcos⁡γsin⁡αcos⁡γ−cos⁡αsin⁡γ−100]{}^A_BR_{XYZ}(\gamma, \beta, \alpha) = \begin{bmatrix}0&\cos\alpha\sin\gamma-\sin\alpha\cos\gamma&\cos\alpha\cos\gamma+\sin\alpha\sin\gamma\\ 0&\sin\alpha\sin\gamma+\cos\alpha\cos\gamma&\sin\alpha\cos\gamma-\cos\alpha\sin\gamma\\ -1&0&0\end{bmatrix}

此时通过给出的旋转矩阵，无法求得 α\alpha 和 γ\gamma 的值，其他的旋转方式也是同样，而使用四元数表示朝向时则不会出现上述情况。

四元数的表示方式为：坐标系绕单位向量 (kx,ky,kz)(k_x, k_y, k_z) 旋转角度 θ\theta，其四个参数分别为：

ϵ1=kxsin⁡θ2,ϵ2=kysin⁡θ2,ϵ3=kzsin⁡θ2,ϵ4=cos⁡θ2\begin{aligned} \epsilon_1 &= k_x\sin\frac{\theta}{2},\\ \epsilon_2 &= k_y\sin\frac{\theta}{2},\\ \epsilon_3 &= k_z\sin\frac{\theta}{2},\\ \epsilon_4 &= \cos\frac{\theta}{2} \end{aligned}

旋转矩阵为：

Rϵ=[1−2ϵ22−2ϵ322ϵ1ϵ2−2ϵ3ϵ42ϵ1ϵ3+2ϵ2ϵ42ϵ1ϵ2+2ϵ3ϵ41−2ϵ12−2ϵ322ϵ2ϵ3−2ϵ1ϵ42ϵ1ϵ3−2ϵ2ϵ42ϵ2ϵ3+2ϵ1ϵ41−2ϵ12−2ϵ22]R_{\epsilon} = \begin{bmatrix}1-2\epsilon_2^2-2\epsilon_3^2&2\epsilon_1\epsilon_2-2\epsilon_3\epsilon_4&2\epsilon_1\epsilon_3+2\epsilon_2\epsilon_4\\ 2\epsilon_1\epsilon_2+2\epsilon_3\epsilon_4&1-2\epsilon_1^2-2\epsilon_3^2&2\epsilon_2\epsilon_3-2\epsilon_1\epsilon_4\\ 2\epsilon_1\epsilon_3-2\epsilon_2\epsilon_4&2\epsilon_2\epsilon_3+2\epsilon_1\epsilon_4&1-2\epsilon_1^2-2\epsilon_2^2 \end{bmatrix}

若给定旋转矩阵，则四元数的参数可以通过以下公式求得：

ϵ1=r32−r234ϵ4,ϵ2=r13−r314ϵ4,ϵ3=r21−r124ϵ4,ϵ4=121+r11+r22+r33\begin{aligned} \epsilon_1 &= \frac{r_{32}-r_{23}}{4\epsilon_4},\\ \epsilon_2 &= \frac{r_{13}-r_{31}}{4\epsilon_4},\\ \epsilon_3 &= \frac{r_{21}-r_{12}}{4\epsilon_4},\\ \epsilon_4 &= \frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}} \end{aligned}

正运动学

正运动学是指已知机器人各个关节的参数，求解机器人末端执行器的位置和朝向的过程。

D-H 表示法

D-H 表示法

首先对机械臂的关节进行建系，对于每个关节 ii，取 Z^i\hat Z_i 轴与关节轴线重合，X^i\hat X_i 轴垂直于 Z^i\hat Z_i 和 Z^i+1\hat Z_{i+1} 轴，原点位于 Z^i\hat Z_i 和 X^i\hat X_i 轴的交点处，Y^i\hat Y_i 轴满足右手定则。

D-H 表示法中，每个关节 ii 的参数包括：

  • aia_i：Z^i\hat Z_i 与 Z^i+1\hat Z_{i+1} 轴沿 X^i\hat X_i 轴的平移距离
  • αi\alpha_i：Z^i\hat Z_i 与 Z^i+1\hat Z_{i+1} 轴绕 X^i\hat X_i 轴的旋转角度
  • did_i：X^i−1\hat X_{i-1} 与 X^i\hat X_i 轴沿 Z^i\hat Z_i 轴的平移距离
  • θi\theta_i：X^i−1\hat X_{i-1} 与 X^i\hat X_i 轴绕 Z^i\hat Z_i 轴的旋转角度

其中，对于转动关节，did_i 是常数，θi\theta_i 是变量；对于移动关节，did_i 是变量，θi\theta_i 是常数。

特别定义第 00 个坐标系与第 n+1n+1 个坐标系分别为基坐标系和末端执行器坐标系，它们分别与第 11 个关节和第 nn 个关节的关节变量为 00 时的坐标系重合，由此可得第 11 和第 nn 个关节的 D-H 参数（以转动关节为例，此时 θi\theta_i 为关节变量）：

a0=0,α0=0,d1=0,θ1=θa_0 = 0, \alpha_0 = 0, d_1 = 0, \theta_1 = \theta

an=0,αn=0,dn+1=0,θn+1=θa_n = 0, \alpha_n = 0, d_{n+1} = 0, \theta_{n+1} = \theta

根据上述参数，可以构建每个关节变换到前一关节的变换矩阵 ii−1T{}^{i-1}_iT：先让坐标系沿 Z^i\hat Z_i 轴平移 did_i 得到坐标系 PP，再绕 Z^i\hat Z_i 轴旋转 θi\theta_i 得到坐标系 QQ，接着沿 X^i\hat X_i 轴平移 aia_i 得到坐标系 RR，最后绕 X^i\hat X_i 轴旋转 αi\alpha_i，这一过程可以表示为矩阵乘法：

i−1P=Ri−1TQRTPQTiPTiP{}^{i-1}P = {}^{i-1}_RT{}^R_QT{}^Q_PT{}^P_iT{}^iP

坐标系变换

各个中间变换矩阵就不列出来了，总之最终可以得到：

ii−1T=[cos⁡θi−sin⁡θi0ai−1sin⁡θicos⁡αi−1cos⁡θicos⁡αi−1−sin⁡αi−1−disin⁡αi−1sin⁡θisin⁡αi−1cos⁡θisin⁡αi−1cos⁡αi−1dicos⁡αi−10001]{}^{i-1}_iT = \begin{bmatrix} \cos\theta_i&-\sin\theta_i&0&a_{i-1}\\ \sin\theta_i\cos\alpha_{i-1}&\cos\theta_i\cos\alpha_{i-1}&-\sin\alpha_{i-1}&-d_i\sin\alpha_{i-1}\\ \sin\theta_i\sin\alpha_{i-1}&\cos\theta_i\sin\alpha_{i-1}&\cos\alpha_{i-1}&d_i\cos\alpha_{i-1}\\ 0&0&0&1 \end{bmatrix}

通过连续左乘每个关节的变换矩阵，就可以得到末端执行器坐标系相对于基坐标系的变换矩阵。

另外，对于类似球关节这类具有多个自由度的关节，可以将其分解为多个转动关节来处理，这些转动关节的 X 轴的选取尽量与下一级的 X 轴平行或重合，不行就选一个符合右手定则的方向，D-H 参数中的平移部分为 00，旋转部分为相应的旋转角度。