视频链接（含中文字幕）：(中英强推!)2024斯坦福最值得学习的【机器人学导论】通俗易懂秒上手！CS223A-Introduction To Robotics

空间表示与坐标变换

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

坐标系变换

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

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 表示法

首先对机械臂的关节进行建系，对于每个关节 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 轴的旋转角度

这里针对几种特殊情况作出补充：

1. 当 Z^i\hat Z_i 和 Z^i+1\hat Z_{i+1} 轴平行时，X^i\hat X_i 轴可以任意选取一个垂直于 Z^i\hat Z_i 和 Z^i+1\hat Z_{i+1} 轴的位置，通常我们选取使 did_i 为 00 的位置
2. 当 Z^i\hat Z_i 和 Z^i+1\hat Z_{i+1} 轴重合时，X^i\hat X_i 轴可以任意选取一个垂直于 Z^i\hat Z_i 轴的位置和方向，通常我们选取使 did_i 和 θi\theta_i 都为 00 的 X^i\hat X_i 轴
3. 当 Z^i\hat Z_i 和 Z^i+1\hat Z_{i+1} 轴垂直时，X^i\hat X_i 轴选取 Z^i\hat Z_i 和 Z^i+1\hat Z_{i+1} 轴的公垂线。公垂线有两个方向，可以随意选取，通常我们选取使 aia_i 为正的方向
4. 对于最后一个关节，由于 没有 Z^n+1\hat Z_{n+1} 轴，所以 X^n\hat X_n 轴可以任意选取一个垂直于 Z^n\hat Z_n 轴的位置和方向，通常我们选取使 dn+1d_{n+1} 和 θn+1\theta_{n+1} 都为 00 的 X^n\hat X_n 轴。

简单来说，有多种选取方式时，尽量选取使 D-H 参数中 00 尽量多的的方式，这样可以简化后续的计算。

其中，对于转动关节，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，旋转部分为相应的旋转角度。

雅可比矩阵

首先我们需要明确一点：在数学上，“雅可比矩阵”仅仅指的是一个向量函数对另一个向量的一阶偏导数矩阵，而不是一个特指的矩阵。所以在这一部分的分析中我们可能会看到有好几个不同意义的矩阵都叫雅可比矩阵，这是没有问题的，但是在机器人学中，我们通常情况下所说的雅可比矩阵是指末端执行器坐标系的速度与关节速度之间的关系矩阵。

另外，为了表述方便，将关节变量统一为 qi=ϵˉiθi+ϵidiq_i = \bar\epsilon_i\theta_i+\epsilon_i d_i，其中 ϵˉi=1−ϵi\bar\epsilon_i=1-\epsilon_i，若关节 ii 是转动关节，则 ϵi=0\epsilon_i=0，qi=θiq_i=\theta_i；若关节 ii 是移动关节，则 ϵi=1\epsilon_i=1，qi=diq_i=d_i，下面的内容中如果出现 ϵ\epsilon 和 ϵˉ\bar\epsilon，含义相同。

微分运动

在正运动学里面，我们已经得到了末端执行器坐标系相对于基坐标系的变换矩阵 n+10T{}^0_{n+1}T，通过这个矩阵我们建立起了末端执行器坐标系与基坐标系之间的位置和朝向的关系，现在我们考虑，如果关节发生微小的变化，那么末端执行器的坐标系会发生什么样的变化，即，我们想要建立起 δq\delta q 与 δx\delta x 之间的关系。

一个非常自然的想法是，既然我们可以用 n+10T{}^0_{n+1}T 来表示末端执行器坐标系与基坐标系之间的位置和朝向的关系，那么，我们将这个矩阵重新写成一个向量，然后求这个向量对关节变量 qq 的雅可比矩阵，就可以得到 δq\delta q 与 δx\delta x 之间的关系了。

然而，这样存在几个问题：

1. 刚体在三维空间中实际只有 6 个自由度，但是我们却用了 12 个参数来表示其运动，这其中有许多冗余信息，使得整个雅可比矩阵变得非常庞大，计算起来非常麻烦。
2. 我们使用的这 12 个参数并不是唯一能够表示末端执行器位置与朝向的方式，这就导致了我们求得的雅可比矩阵并不是一个特指的矩阵，而是一个依赖于我们选择的参数化方式的矩阵，比如，我们也可以选择一个 7×17\times 1 的向量来表示末端执行器的位置和朝向，其中前 3 个参数表示位置，后 4 个参数表示朝向的四元数，这样我们求得的雅可比矩阵就会与之前的雅可比矩阵完全不同。

因此，我们需要找到一种更简洁、更直接的方式来表示末端执行器的运动，这样我们就可以得到一个更小、更特指的雅可比矩阵。

基本雅可比矩阵

一个简单的想法是：直接使用线速度与角速度来表示末端执行器的运动不就行了吗。

还真是，所以接下来我们推导一下将 q˙\dot q 映射到末端执行器速度与角速度的雅可比矩阵。

考虑两相邻关节坐标系速度的递推关系，对于线速度 vi+1v_{i+1} 而言，它由三部分组成，一部分是坐标系 ii 的线速度，一部分是由于坐标系 ii 绕 Z 轴旋转引起的线速度，最后一部分是坐标系 i+1i+1 自身的线速度（平移关节），即：

vi+1=vi+ωi×Pi+1+d˙i+1Zi+1v_{i+1} = v_i + \omega_i \times P_{i+1} + \dot d_{i+1}Z_{i+1}

对于角速度 ωi+1\omega_{i+1} 而言，它由两部分组成，一部分是坐标系 ii 的角速度，另一部分是坐标系 i+1i+1 自身的角速度（转动关节），即：

ωi+1=ωi+θ˙i+1Zi+1\omega_{i+1} = \omega_i + \dot\theta_{i+1}Z_{i+1}

由于线速度与角速度矢量都是方向矢量，因此转换坐标系时不用考虑平移的问题，直接左乘旋转矩阵即可。我们将上述关系式的坐标系转换一下得到：

i+1vi+1=ii+1R(ivi+iωi×Pi+1)+d˙i+1i+1Zi+1i+1ωi+1=ii+1Riωi+θ˙i+1i+1Zi+1\begin{aligned} {}^{i+1}v_{i+1} &= {}^{i+1}_iR({}^iv_i + {}^i\omega_i \times P_{i+1})+ \dot d_{i+1} {}^{i+1}Z_{i+1}\\ {}^{i+1}\omega_{i+1} &= {}^{i+1}_iR {}^i\omega_i + \dot\theta_{i+1} {}^{i+1}Z_{i+1} \end{aligned}

初始条件：0ω0=0,0v0=0{}^0\omega_0 = 0, {}^0v_0 = 0，通过瞪眼法数学推导可以得到：

0ωn=∑i=1nθ˙i0Zi0vn=∑i=1nθ˙i0Zi×Pi,n+∑i=1nd˙i0Zi\begin{aligned} {}^0\omega_n &= \sum_{i=1}^n \dot\theta_i {}^0Z_i\\ {}^0v_n &= \sum_{i=1}^n \dot\theta_i {}^0Z_i \times P_{i,n} + \sum_{i=1}^n \dot d_i {}^0Z_i \end{aligned}

用之前提到的统一的关节变量 qiq_i 来表示上述关系式：

0ωn=∑i=1n(ϵˉi0Zi)q˙i0vn=∑i=1n[ϵi0Zi+ϵˉi(0Zi×Pi,n)]q˙i\begin{aligned} {}^0\omega_n &= \sum_{i=1}^n (\bar\epsilon_i {}^0Z_i)\dot q_i\\ {}^0v_n &= \sum_{i=1}^n [\epsilon_i {}^0Z_i+\bar\epsilon_i ({}^0Z_i \times P_{i,n})]\dot q_i \end{aligned}

由此，我们就得到了最后一个关节坐标系的线速度与角速度与关节速度之间的关系矩阵：

J=[ϵ10Z1+ϵˉ1(0Z1×P1,n)ϵ20Z2+ϵˉ2(0Z2×P2,n)⋯ϵn0Zn+ϵˉn(0Zn×Pn,n)ϵˉ10Z1ϵˉ20Z2⋯ϵˉn0Zn]J = \begin{bmatrix}\epsilon_1 {}^0Z_1+\bar\epsilon_1 ({}^0Z_1 \times P_{1,n})&\epsilon_2 {}^0Z_2+\bar\epsilon_2 ({}^0Z_2 \times P_{2,n})&\cdots&\epsilon_n {}^0Z_n+\bar\epsilon_n ({}^0Z_n \times P_{n,n})\\ \bar\epsilon_1 {}^0Z_1&\bar\epsilon_2 {}^0Z_2&\cdots&\bar\epsilon_n {}^0Z_n \end{bmatrix}

然而，这个雅可比矩阵的计算看上去就很困难，尤其是前三行线速度对应的部分，有没有什么简单一点的方法来计算呢？

有的，兄弟，有的。

尽管我们并不能通过对欧拉角等直接求导来计算角速度，但是我们可以直接对位移向量的分量求偏导来计算线速度啊，因此，我们可以直接把第一行改为对从坐标系 00 到坐标系 nn 的位移向量 P0,nP_{0,n} 求 qq 的偏导。同时，第二行我们也可以改成含有旋转矩阵的形式，因为之前我们求 n+10T{}^0_{n+1}T 时已经得到了旋转矩阵，这里也就可以直接拿过来用。

令 Z=[001]TZ = \begin{bmatrix} 0 & 0 & 1\end{bmatrix}^T，则雅可比矩阵可表示为：

J=[∂P0,n∂q1∂P0,n∂q2⋯∂P0,n∂qnϵˉ110RZϵˉ220RZ⋯ϵˉnn0RZ]J = \begin{bmatrix}\frac{\partial P_{0,n}}{\partial q_1}&\frac{\partial P_{0,n}}{\partial q_2}&\cdots&\frac{\partial P_{0,n}}{\partial q_n}\\ \bar\epsilon_1{}^0_1RZ&\bar\epsilon_2{}^0_2RZ&\cdots&\bar\epsilon_n{}^0_nRZ \end{bmatrix}

得到了最后一个关节坐标系的雅可比矩阵之后，我们还差最后一步，从最后一个关节坐标系推到末端执行器坐标系。

由于末端执行器自身不进行旋转或者平移，因此可以得到：

nve=nvn−Pn,e×nωnnωe=nωn\begin{aligned} {}^{n}v_{e} &= {}^{n}v_{n}-P_{n,e}\times{}^n\omega_{n}\\ {}^{n}\omega_{e} &= {}^n\omega_n \end{aligned}

将叉乘转换为矩阵乘法的形式，就可以得到：

[nvenωe]=[I−nP^n,e0I][nvnnωn]\begin{bmatrix} {}^{n}v_{e} \\ {}^{n}\omega_{e} \end{bmatrix} = \begin{bmatrix} I & -{}^n\hat P_{n,e}\\ 0 & I \end{bmatrix} \begin{bmatrix} {}^{n}v_{n} \\ {}^{n}\omega_{n} \end{bmatrix}

其中 P^n,e=[0−Pn,e,zPn,e,yPn,e,z0−Pn,e,x−Pn,e,yPn,e,x0]\hat P_{n,e} = \begin{bmatrix}0&-P_{n,e,z}&P_{n,e,y}\\ P_{n,e,z}&0&-P_{n,e,x}\\ -P_{n,e,y}&P_{n,e,x}&0\end{bmatrix}，P^n,ex=Pn,e×x\hat P_{n,e}x = P_{n,e} \times x。

转换到基坐标系下，就可以得到：

[0ve0ωe]=[I−n0RnP^n,en0RT0I][0vn0ωn]\begin{bmatrix} {}^{0}v_{e} \\ {}^{0}\omega_{e} \end{bmatrix} = \begin{bmatrix} I & -{}^0_nR{}^n\hat P_{n,e}{}^0_nR^T\\ 0 & I \end{bmatrix} \begin{bmatrix} {}^{0}v_{n} \\ {}^{0}\omega_{n} \end{bmatrix}

也就是说：

Je=[I−n0RnP^n,en0RT0I]JnJ_e = \begin{bmatrix} I & -{}^0_nR{}^n\hat P_{n,e}{}^0_nR^T\\ 0 & I \end{bmatrix}J_n

关于 −n0RnP^n,en0RT-{}^0_nR{}^n\hat P_{n,e}{}^0_nR^T 项，它其实是 −0P^n,e-{}^0\hat P_{n,e} 的坐标变换形式。
因为 0P×0ω=n0R(nP×nω)=n0R(nP^0nR0ω)=n0RnP^n0RT0ω{}^0P\times{}^0\omega = {}^0_nR({}^n P \times {}^n\omega) = {}^0_nR({}^n\hat P{}^n_0R{}^0\omega) = {}^0_nR{}^n\hat P{}^0_nR^T{}^0\omega，所以 0P^=n0RnP^n0RT{}^0\hat P = {}^0_nR{}^n\hat P{}^0_nR^T。

这样我们就得到了关节速度与末端执行器线速度和角速度之间的雅可比矩阵，这时再考虑关节角速度与最开始说的末端执行器位置与朝向之间的关系，我们发现好像只需要再在速度左边再乘一个转换矩阵就可以了。

举个例子，从速度向量映射到用笛卡尔坐标系表示位置，用欧拉角表示朝向的向量的矩阵是：

X=(x,y,z),xR=(α,β,γ)X = (x, y, z), x_R = (\alpha, \beta, \gamma)

EP(X)=[100010001],ER(xR)=[−sin⁡αcos⁡βsin⁡βcos⁡αcos⁡βsin⁡β1cos⁡αsin⁡α0sin⁡αsin⁡β−cos⁡αsin⁡β0]E_P(X) = \begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1\end{bmatrix}, E_R(x_R) = \begin{bmatrix} -\frac{\sin\alpha\cos\beta}{\sin\beta}&\frac{\cos\alpha\cos\beta}{\sin\beta}&1\\ \cos\alpha&\sin\alpha&0\\ \frac{\sin\alpha}{\sin\beta}&-\frac{\cos\alpha}{\sin\beta}&0 \end{bmatrix}

[X˙x˙R]=[EP(X)00ER(xR)][vω]\begin{bmatrix} \dot X \\ \dot x_R\end{bmatrix} = \begin{bmatrix} E_P(X) & 0\\ 0 & E_R(x_R) \end{bmatrix} \begin{bmatrix} v \\ \omega \end{bmatrix}

像这样，当我们需要使用不同的位置和朝向表示方法时，只需要使用对应的转换矩阵就可以了，而这个转换矩阵是固定的，这也降低了求解雅可比矩阵的复杂度。

另外可以看到，当 β=kπ\beta = k\pi 时，ER(xR)E_R(x_R) 中的一些元素值会变成无穷大，这也是欧拉角万向锁问题的具体体现之一。

运动学奇异点

运动学奇异点是指雅可比矩阵的行列式为零，或者说不满秩的点，此时末端执行器在某些方向上的运动无法通过关节的运动来实现，也就是丢失了自由度，常引发关节速度突变、停滞或路径失控的现象。

例如，将你的手臂想象成一个有两个转动关节（肩膀和手肘）的机械臂，当你的手臂完全伸直，也就是第二个关节的旋转角度为 00 时，你的手臂就丧失了在手臂方向的运动能力，也就是说你不能把手伸得更长了。

以这个机械臂为例。

其雅可比矩阵为（仅考虑 x,yx,y 时）：

J=[−l1sin⁡θ1−l2sin⁡(θ1+θ2)−l2sin⁡(θ1+θ2)l1cos⁡θ1+l2cos⁡(θ1+θ2)l2cos⁡(θ1+θ2)]J = \begin{bmatrix}-l_1\sin\theta_1-l_2\sin(\theta_1+\theta_2)&-l_2\sin(\theta_1+\theta_2)\\ l_1\cos\theta_1+l_2\cos(\theta_1+\theta_2)&l_2\cos(\theta_1+\theta_2) \end{bmatrix}

其奇异点为 θ2=kπ\theta_2 = k\pi

考虑对这个雅可比矩阵求逆，得到：

J−1=1l1l2sin⁡θ2[l2cos⁡(θ1+θ2)−l2sin⁡(θ1+θ2)−l1cos⁡θ1−l2cos⁡(θ1+θ2)l1sin⁡θ1+l2sin⁡(θ1+θ2)]J^{-1} = \frac{1}{l_1l_2\sin\theta_2}\begin{bmatrix}l_2\cos(\theta_1+\theta_2)&-l_2\sin(\theta_1+\theta_2)\\ -l_1\cos\theta_1-l_2\cos(\theta_1+\theta_2)&l_1\sin\theta_1+l_2\sin(\theta_1+\theta_2) \end{bmatrix}

当 θ2=kπ\theta_2 = k\pi 时，sin⁡θ2=0\sin\theta_2 = 0，J−1J^{-1} 不存在，同时，当 θ2≈kπ\theta_2 \approx k\pi 时，sin⁡θ2≈0\sin\theta_2 \approx 0，J−1J^{-1} 的元素值会变得非常大，这时使用 q˙=J−1v\dot q = J^{-1}v 求解产生一定速度所需要的关节速度会导致关节速度急剧增大，引发一系列问题。

静力

雅可比矩阵不仅可以表示末端执行器的速度与关节速度之间的关系，还可以表示末端执行器的力与关节力矩之间的关系，这里先直接给出结论。

τ=JTF\tau = J^TF

接下来用虚功原理推导一下，理论上来说在这个课程里面 Khatib 教授用了三种方法来推导，但是我懒得写另外两种了，反正最后的结果是一样的，就选一种我个人最喜欢的方法推一推就行了。

令 τ=[τ1⋮τn]\tau = \begin{bmatrix}\tau_1\\ \vdots\\ \tau_n\end{bmatrix} 为关节力矩向量，F=[FxFyFz]F = \begin{bmatrix}F_x\\ F_y\\ F_z\end{bmatrix} 为末端执行器产生的力。假设机械臂产生了一个微小的位移 δx\delta x，由于这个系统没有能量损失，所以末端执行器产生的力做的功应该等于关节力矩做的功，即：

τTδq=FTδx\tau^T\delta q = F^T\delta x

而众所周知 δx=Jδq\delta x = J\delta q，代入上式得：

τTδq=FTJδq\tau^T\delta q = F^TJ\delta q

因此：

τT=FTJ\tau^T = F^TJ

左右同时取转置即可得到：

τ=JTF\tau = J^TF

轨迹规划

在实际应用中，我们通常需要让机器人按照特定的路径运动，这就涉及到轨迹规划的问题。轨迹规划的目标是生成一个满足特定约束条件的路径，使得机器人能够按照这个路径运动。

一般而言，当我们提到轨迹规划时，很容易想到的是让末端执行器在空间中通过一条给定的路径从起点到达终点的过程，然而，在笛卡尔坐标系下进行轨迹规划可能会存在一些问题，最典型的问题在于我们无法保证规划的路径是可行的，比如下面这些情况：

此外，在笛卡尔坐标系下进行轨迹规划还存在复杂度过大等问题，总之，在实际应用中，我们通常会选择在关节空间内进行轨迹规划，来避免以上问题,不过，在分析过程中，我们可以先用一个统一的变量 uu 来表示可能用到的变量，比如关节角度，笛卡尔坐标之类的。

轨迹规划最简单的方法当然是直接用线段连接起点和终点，然而这种方法规划出的轨迹的速度会出现突变，尤其是存在一系列中间点时，而突变的速度意味着无穷大的加速度，这显然是不现实的，所以我们需要一些更平滑的轨迹规划方法来生成满足加速度约束的轨迹。

下面简单介绍两种用于轨迹规划的方法。

三次多项式

当只存在起点和终点时，我们希望规划出的路径满足如下条件：

u(0)=u0u(t)=ufu˙(0)=0u˙(t)=0\begin{aligned} u(0) &= u_0\\ u(t) &= u_f\\ \dot u(0) &= 0\\ \dot u(t) &= 0 \end{aligned}

即从起点出发，到终点停止，且初始和终止速度都为零。

我们使用一个三次多项式来拟合这个轨迹：

u(t)=a0+a1t+a2t2+a3t3u(t) = a_0 + a_1t + a_2t^2 + a_3t^3

由上面的约束条件可以解得：

a0=u0a1=0a2=3(uf−u0)t2a3=−2(uf−u0)t3\begin{aligned} a_0 &= u_0\\ a_1 &= 0\\ a_2 &= \dfrac{3(u_f-u_0)}{t^2}\\ a_3 &= \dfrac{-2(u_f-u_0)}{t^3} \end{aligned}

当存在中间点时，我们可以对相邻的两个中间点进行同样的拟合，但是由于我们大部分情况下不希望机械臂走走停停，而是希望它能尽量平滑地通过这些路径点，所以路径点处的速度不一定为 00，这时可以解得三次多项式的参数为：

a0=u0a1=u˙0a2=3(uf−u0)t2−2u˙0+u˙fta3=−2(uf−u0)t3+u˙0+u˙ft2\begin{aligned} a_0 &= u_0\\ a_1 &= \dot u_0\\ a_2 &= \dfrac{3(u_f-u_0)}{t^2}- \dfrac{2\dot u_0+\dot u_f}{t}\\ a_3 &= \dfrac{-2(u_f-u_0)}{t^3}+ \dfrac{\dot u_0+\dot u_f}{t^2} \end{aligned}

至于中间点的速度如何取得，这里简单介绍三种方法：

1. 指定在该点处机械臂在笛卡尔坐标系下的线速度和角速度，再通过雅可比矩阵求得关节速度。
2. 使用某些启发式算法让程序自己计算中间点的速度，比如：假设用线段将相邻中间点连起来，若某点前后两条线段斜率正负相反，则该点速度取为 00，否则取为前后两条线段斜率的平均值。
3. 采用使中间点处加速度连续的方法让程序自己计算，设置限制条件 u˙1(t)=u˙2(0),u¨1(t)=u¨2(0)\dot u_1(t) = \dot u_2(0), \ddot u_1(t) = \ddot u_2(0) 即可。

当然这种方法还可以拓展，由于三次多项式只有四个参数，所以我们最多只能设置四个不相关的约束条件，如果我们需要设置更多的约束条件，比如说加速度为零，或者说在某个时间点处的速度为某个特定值，那就可以使用更高次的多项式来拟合，这里就不展开说了，方法是类似的。

与抛物线拟合的线性函数

另一种想法是，既然线性函数在各个路径点处存在速度突变的问题，那我们在这些点处用平滑的二次函数拟合一下不就好了吗。

（这个图是从讲义中截下来的，有一点小错误，需要假设 t0=0t_0=0）

不妨假设两端的二次函数具有相同的持续时间，这样它们的加速度也就是相同的。

由方程

12u¨tb2+u¨tb(t−2tb)+12u¨tb2=uf−u0\dfrac{1}{2}\ddot ut_b^2+\ddot ut_b(t-2t_b)+\dfrac{1}{2}\ddot ut_b^2 = u_f-u_0

解得：

tb=t2−u¨2t2−4(uf−u0)u¨2u¨t_b = \dfrac{t}{2}-\dfrac{\sqrt{\ddot u^2t^2-4(u_f-u_0)\ddot u}}{2\ddot u}

其中 u¨\ddot u 是前后两段二次函数的加速度。

当存在中间点时，公式如下：

1. 第一段轨迹：

u¨1=sgn⁡(u2−u1)∣u¨1∣t1=td12−td122−2(u2−u1)u¨1u˙12=u2−u1td12−12t1t12=td12−t1−12t2\begin{aligned} \ddot u_1 &= \operatorname{sgn}(u_2-u_1)|\ddot u_1|\\ t_{1} &= t_{d12}-\sqrt{t_{d12}^2-\dfrac{2(u_2-u_1)}{\ddot u_1}}\\ \dot u_{12} &= \dfrac{u_2-u_1}{t_{d12}-\dfrac{1}{2}t_{1}}\\ t_{12} &= t_{d12}-t_{1}-\dfrac{1}{2}t_2 \end{aligned}

2. 中间轨迹：

u¨k=sgn⁡(u˙kl−u˙jk)∣u¨k∣tk=u˙kl−u˙jku¨ku˙jk=uk−ujtdjktjk=tdjk−12tj−12tk\begin{aligned} \ddot u_k &= \operatorname{sgn}(\dot u_{kl}-\dot u_{jk})|\ddot u_k|\\ t_k &= \dfrac{\dot u_{kl}-\dot u_{jk}}{\ddot u_k}\\ \dot u_{jk} &= \dfrac{u_k-u_j}{t_{djk}}\\ t_{jk} &= t_{djk}-\dfrac{1}{2}t_j-\dfrac{1}{2}t_k \end{aligned}

3. 最后一段轨迹：

u¨n=sgn⁡(un−1−un)∣u¨n∣tn=td(n−1)n−td(n−1)n2−2(un−un−1)u¨nu˙(n−1)n=un−un−1td(n−1)n−12tnt(n−1)n=td(n−1)n−12tn−1−tn\begin{aligned} \ddot u_n &= \operatorname{sgn}(u_{n-1}-u_n)|\ddot u_n|\\ t_{n} &= t_{d(n-1)n}-\sqrt{t_{d(n-1)n}^2-\dfrac{2(u_n-u_{n-1})}{\ddot u_n}}\\ \dot u_{(n-1)n} &= \dfrac{u_n-u_{n-1}}{t_{d(n-1)n}-\dfrac{1}{2}t_{n}}\\ t_{(n-1)n} &= t_{d(n-1)n}-\dfrac{1}{2}t_{n-1}-t_n \end{aligned}

通常只需要指定各关节的位置 uiu_i，相邻两轨迹点之间的时间 tdijt_{dij}，以及每个轨迹点处的加速度 ∣u¨i∣|\ddot u_i| 即可。

另外，由上面那张图可以看出，这种方法拟合的轨迹并没有实际经过那些中间点，大部分情况下这是没有问题的，因为很多时候我们给轨迹设置中间点只是为了避开某些障碍，经不经过中间点其实无伤大雅，不过如果一定要实际经过某个中间点的话也有办法，那就是在这个中间点两边设置两个伪中间点：

实时轨迹规划

由上面的两种方法生成的轨迹结果都是 uu 关于时间的函数，在实时运行时，轨迹生成器利用该函数不断计算出 uu，u˙\dot u 和 u¨\ddot u，并将信息传给控制系统。

对三次多项式来说，计算过程非常简单，只需要把 tt 代入事先已经求得参数的多项式中即可算出 uu，u˙\dot u 和 u¨\ddot u，在到达轨迹终点时，需要将 tt 重置为 00，继续计算下一段轨迹。

对带抛物线拟合的线性函数来说情况就比较复杂了，需要先根据 tt 判断当前是处于直线部分还是二次函数部分。处于直线部分时，计算如下：

u=uj+u˙jktu˙=u˙jku¨=0\begin{aligned} u &= u_j + \dot u_{jk} t\\ \dot u &= \dot u_{jk}\\ \ddot u &= 0 \end{aligned}

处于二次函数部分时，计算如下：

tinb=t−(12tj+tjk)u=uj+u˙jk(t−tinb)+12u¨ktinb2u˙=u˙jk+u¨ktinbu¨=u¨k\begin{aligned} t_{inb} &=t-(\dfrac{1}{2}t_j+t_{jk})\\ u &= u_j + \dot u_{jk}(t-t_{inb}) + \dfrac{1}{2}\ddot u_k t_{inb}^2\\ \dot u &= \dot u_{jk} + \ddot u_k t_{inb}\\ \ddot u &= \ddot u_k \end{aligned}

在进入一个新的直线部分时，需要将 tt 重置为 12tk\dfrac{1}{2}t_k。