机器人学：（3）机器人运动学

机器人运动学（Kinematics）是从几何角度描述和研究机器人的位置、速度和加速度随时间的变化规律的科学，它不涉及机器人本体的物理性质和加在其上的力。这里主要介绍机器人运动学的建模方法及逆运动学的求解方法。

3.0 引言

机器人运动学问题主要在机器人的工作空间与关节空间中讨论，包括正运动学（Forward Kinematics）和逆运动学（InverseKinematics）两部分内容。如图 3.1 所示，由机器人关节空间到机器人工作空间的映射称为正运动学，由机器人工作空间到机器人关节空间的映射称为逆运动学。正运动学也被称为运动学建模，而逆运动学也被称为运动学求逆或求逆解。

图 3.1 正运动学与逆运动学

3.1 运动学建模

机器人运动学建模方法主要包括几何建模法、D-H建模法等。几何建模方法适用于结构简单的机器人，尤其是适合平面机器人。D-H建模方法具有较强的通用性，对于串联机器人、并联机器人都适用。本节重点介绍D-H建模方法。

1955年，Denavit和Hartenberg提出一种基于齐次变换矩阵的低副机构建模方法，该方法为每个连杆固连了一个坐标系，用 4x4 的变换矩阵描述相邻连杆的位姿关系，通过齐次变换建立运动学模型，被称为Denavit-Hartenberg参数模型，简称D-H模型，这种建模方法被称为D-H法。

目前，D-H建模方法主要有两种：标准 D-H 方法（Standard D-H method）和改进的 D-H 方法（Modified D-H method）。这两种方法主要区别在于坐标系建立的位置不同。Denavit和Hartenberg提出的方法被称为标准 D-H 方法。

https://blog.csdn.net/maple_2014/article/details/105612912

3.1.1 标准D-H方法

DH表示法一般步骤：首先为每个关节指定参考坐标系，然后确定如何实现任意两个相邻坐标系之间的变换，最后写出机器人的总变换矩阵。如图所示表示了三个顺序关节和两个连杆，每个关节都是可以转动和平移的。第一个关节指定为关节 i-1，第二个关节指定为关节 i，第三个关节指定为关节 i+1。在这些关节前后可能还有其他关节，连杆也是如此表示，连杆 i 位于关节 i 与关节 i+1 之间。

1、SDH建立每个关节参考坐标系步骤：

（1）所有关节，无一例外用 Z 轴表示。如果是关节是旋转的，Z 轴位于按右手定则选装的方向，如果关节是滑动的，Z 轴为沿实现运动的方向。在每一种情况下，关节 i 处的 Z 轴（以及该关节的本地参考坐标系）的下标为 i-1。
（2）坐标系 {i} 的原点设在关节 i 和关节 i+1 的轴线公垂线与关节 i+1 的轴线相交处。
（3）通常关节不一定平行或相交。因此，通常 Z 轴是斜线，但是总有一条距离最短的公垂线，它正交于任意两条斜线。通常在公垂线方向上定义 X 轴。所以如果 ai 表示 Zi−1与 Zi 之间的公垂线，则 Xi 的方向沿 ai。
（4）如果两个关节的 Z 轴平行，选取与前一关节的公垂线共线的一条公垂线，可简化模型；如果两个相邻关节的Z轴是相交的，那么它们之间没有公垂线，可选取两条 Z 轴的叉积方向作为 X 轴，可简化模型。

θ 角表示绕 Z 轴旋转角，d 表示在 Z 轴上两条相邻的公垂线之间的距离，a 表示每一条公垂线的长度，角 α 表示两个相邻的 Z 轴之间的角度。

图 3.2 SDH 建模方法

2、SDH建模步骤：
（1）绕 Zi−1轴旋转 θi，使 Xi−1和 Xi 同向；
（2）沿 Zi−1轴平移 di 距离，使 Xi−1 和 Xi 共线；
（3）沿 Xi 轴平移 ai 距离，使 Xi − 1 和 Xi 的原点重合；
（4）将 Zi − 1 轴绕 Xi 旋转 αi，使 Zi−1 轴与 Zi 轴共线。

通过右乘四个运动的四个矩阵就可以得到变换矩阵 ${}^{i-1}{T}_i$：

3.1.2 改进D-H方法

1、MDH建立每个关节参考坐标系步骤：
（1）规定 Zi 轴沿关节轴 i 的指向。
（2）找出关节轴 i 和 i+1之间的公垂线或关节轴 i 和 i+1的交点，以关节轴 i 和 i+1的交点或公垂线与关节轴 i 的交点作为连杆坐标系｛i｝的原点。
（3）关节轴 i 和 i+1不相交时， Xi 轴与公垂线重合，指向从 i 到 i+1；如果关节轴 i 和 i+1相交，则规定 Xi 垂直于关节轴 i 和 i+1所在平面。
（4）按照右手定则确定 Yi 轴。

ai：沿 Xi 轴，从 Zi 移动到 Zi+1 的距离；
αi：绕 Xi 轴，从 Zi 旋转到 Zi+1 的角度；
di：沿 Zi 轴，从 Xi−1 移动到 Xi 的距离；
θi：沿 Zi 轴，从 Xi−1 旋转到 Xi 的角度；

图 3.3 MDH 建模方法

2、MDH建模步骤：
（1）将 Zi − 1 轴绕 Xi-1 旋转 αi-1，使 Zi−1 轴与 Zi 轴同向；
（2）沿 Xi-1 轴平移 ai-1 距离，使 Zi − 1 和 Zi 共线；
（3）绕 Zi 轴旋转 θi，使 Xi−1和 Xi 同向；
（4）沿 Zi 轴平移 di 距离，使 Xi−1 和 Xi 共线。

变换矩阵可以写成：

3.1.3 SDH与MDH比较

（1）固连坐标系不同
SDH方法关节 i 上固连的是 { i-1 } 坐标系，即坐标系建在连杆的输出端；MDH关节 i 上固连的是 { i } 坐标系，即坐标系建在连杆的输入端。
（2）坐标系变换顺序不同
SDH方法是 ZX 类变换：先绕着 { i-1 } 坐标系的的 Zi-1 轴旋转和平移，再绕着坐标系 { i } 的 Xi 轴进行旋转和平移；MDH方法是 XZ 类变换：先绕着 { i } 坐标系的的 Xi 轴旋转和平移，再绕着坐标系 { i } 的 Zi 轴进行旋转和平移。

图 3.4 SDH 和 MDH 区别示意图

3.1.4 天机TR8 SDH建模

图 3.5 TR8 实拍图                          图 3.6 TR8 各部位及各轴名称

建立坐标系

图 3.7 TR8 SDH建模

SDH表

	θ	d	a	α
L1	0	0	0.04	-pi/2
L2	-pi/2	0	0.345	0
L3	0	0	0.04	-pi/2
L4	0	0.431	0	pi/2
L5	0	0	0	-pi/2
L6	0	0.08	0	0

关节范围：（单位/deg）
S：-170 ~ 170
L：-65 ~ 145
U：-70 ~ 190
R：-190 ~ 190
B：-135 ~ 135
T：-360 ~ 360

3.1.5 D-H法优势

空间任意两个坐标系的变换关系确实需要6个参数来表示，然而，在建立各个连杆的坐标系时，我们可以拟定一些规则，使得坐标系满足某些约束，从而只需4个参数则可以表示两个坐标系的变换关系。
若坐标轴 xi 与坐标轴 zi−1垂直相交(示例如下图)，则只需4个参数(杆件长度 ai，杆件扭角 αi，关节距离 di，关节转角 θi )就可以表示两个坐标系的变换关系。下面的证明来于书籍：Robot Modeling and Control(First Edition),by Mark W. Spong, Seth Hutchinson, and M. Vidyasagar.

若坐标系 { x 1 y 1 z 1 } \{x_1y_1z_1\} {x1​y1​z1​}与坐标系 { x 0 y 0 z 0 } \{x_0y_0z_0\} {x0​y0​z0​}垂直相交，则坐标系 { x 1 y 1 z 1 } \{x_1y_1z_1\} {x1​y1​z1​}到坐标系 { x 0 y 0 z 0 } \{x_0y_0z_0\} {x0​y0​z0​}的齐次变换矩阵为：

$$A=Ro{t}_{z,\theta }Tran{s}_{z,d}Tran{s}_{x,a}Ro{t}_{x,\alpha }\text{\hspace{1em}(1)}$$

(1)式可以写成：

$$A=\left[\begin{array}{cc}{R}_1^0& O_1^0\\ 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$

因为 x1 垂直于 z0，故有：
$$0=x_1^0\cdot z_0^0=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=r_{31}\text{\hspace{1em}(3)}$$

由于$r_{31}=0$，只要证明存在唯一的杆件扭角 α ，关节转角 θ，使得：

$$R_1^0=Ro{t}_{z,\theta }Ro{t}_{x,\alpha }=\left[\begin{array}{ccc}c\theta & -s\theta c\alpha & s\theta s\alpha \\ s\theta & c\theta c\alpha & -c\theta s\alpha \\ 0& s\alpha & c\alpha \end{array}\right]\text{\hspace{1em}(4)}$$

因此，存在唯一的 θ , α 满足：
$$\{\begin{array}{l}(r_{11},r_{21})=(c\theta ,s\theta )\\ (r_{32},r_{33})=(c\alpha ,s\alpha )\end{array}\text{\hspace{1em}(5)}$$

易得：

$$\{\begin{array}{r}\theta =atan2(r_{21},r_{11})\\ \alpha =atan2(r_{32},r_{33})\end{array}\text{\hspace{1em}(6)}$$

根据旋转矩阵的性质，可以推导得到：

$$\{\begin{array}{r}-s\theta c\alpha =r_{12}\\ s\theta s\alpha =r_{13}\\ c\theta c\alpha =r_{22}\\ -c\theta s\alpha =r_{23}\end{array}\text{\hspace{1em}(7)}$$

可见，坐标系 { x 1 y 1 z 1 } \{x_1y_1z_1\} {x1​y1​z1​}到坐标系 { x 0 y 0 z 0 } \{x_0y_0z_0\} {x0​y0​z0​}的旋转变换关系用 θ , α 来表达就足够了。
若 x1与 z0 相交，坐标系 { x 1 y 1 z 1 } \{x_1y_1z_1\} {x1​y1​z1​}的原点在坐标系 { x 0 y 0 z 0 } \{x_0y_0z_0\} {x0​y0​z0​}下的坐标(或平移向量)为：

$$O_1^0=O_0^0+d{z}_0^0+a{x}_1^0=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]+d\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]+a\left[\begin{array}{c}c\theta \\ s\theta \\ 0\end{array}\right]=\left[\begin{array}{c}ac\theta \\ as\theta \\ d\end{array}\right]\text{\hspace{1em}(8)}$$

综上，坐标系 { x 1 y 1 z 1 } \{x_1y_1z_1\} {x1​y1​z1​}到坐标系 { x 0 y 0 z 0 } \{x_0y_0z_0\} {x0​y0​z0​}的齐次变换关系用杆件长度 a，杆件扭角 α，关节距离 d，关节转角 θ 这4个参数就够了。

3.2 正逆运动学

正运动学指，从机器人的关节空间描述计算笛卡尔空间描述的机器人末端执行器的位置和姿态，该问题通常是一个几何问题，给定一组关节角度，计算末端坐标系相对于基坐标系的位置和姿态。
逆运动学指，从笛卡尔空间描述下的机器人末端执行器位置和姿态反算出机器人关节空间应该达到的关节角度组合，是实现机器人控制的一个基本问题。通常因为正运动学方程是非线性的，因此逆运动学问题较为困难，很难得到封闭解，甚至无解。

https://blog.csdn.net/hanmingjunv5/article/details/106555302
https://zhuanlan.zhihu.com/p/450749372

3.2.1 机器人正运动学

机器人正运动学就是给定机器人各关节位置，计算机器人连杆上任意点的位姿。
此过程为：建立机器人运动学方程。

位姿矩阵

机器人位置和姿态可以由一个方阵描述:

其中，n, o, a 为末端姿态，p 为末端位置。

正运动学求解

$$T_6^0=T_1^0{T}_2^1{T}_3^2{T}_4^3{T}_5^4{T}_6^5$$

$T_k^i$ 为 k 关节坐标系在 i 关节坐标系下的描述。

对于确定的机器人结构，每个关节在上一关节的描述是确定的，因此正运动学直接计算即可。

3.2.2 机器人逆运动学

逆运动学求解通常有两大类方法：解析解、数值解。

一、解析解（Analytical Solution）

解析解通常被称为封闭解（Closed Form Solution），就是可以将求解的关节变量表示成解析表达式的形式，是精确解。
特点：运算速度快（达到us级），通用性差，可以分为代数法与几何法进行求解。
串联机械臂有逆运动学解析解的充分条件是满足Pieper准则。即如果机器人满足两个充分条件中的一个，就会得到封闭解，这两个条件是：

• （1）三个相邻关节轴线交于一点；
• （2）三个相邻关节轴线相互平行。

现在的大多数商品化机器人都满足封闭解的两个充分条件之一。如PUMA和STANFORD机器人满足第一条件，而ASEA和MINIMOVER机器人满足第二条件。
以PUMA560机器人为例，它的最后3个关节轴相交于一点，该点通常被称为腕点。

图 3.8 PUMA560 示意图

（1）Pieper方法

Pieper方法求解参考：

https://www.cnblogs.com/21207-iHome/p/9452896.html

（2）Paul方法

Paul方法也被称为Paul反变换法，是比较常用的机器人运动学逆解计算方法。该方法要建立机器人的运动学矩阵方程，如下第1式，等式右边矩阵已知，等式左边矩阵中的关节变量未知。首先用矩阵${}_0^1{T}^{-1}$左乘第1式矩阵方程，然后从等式两边矩阵元素中寻找并建立含有单关节变量的等式，解出该变量，再寻找并建立其他的单变量等式，如果没能解出所有的关节变量，则再在等式两侧左乘矩阵${}_1^2{T}^{-1}$，然后再寻找并建立可求解的单变量等式…直到所有的变量都解出。

求解关节一

${\theta }_1=atan2(p_y,p_x)-atan2(0,\pm \sqrt{p_x^2+p_y^2})$

求解关节三

$k_1=cos({\theta }_1)p_x+sin({\theta }_1)p_y-a_1$

$k_2=-p_z+d_1$

$k=\frac{k_1^2+k_2^2-(a_2^2+a_3^2+d_4^2)}{2a_2}$

${\theta }_{31,32}=atan2(a_3,d_4)-atan2(k,\pm \sqrt{a_3^2+d_4^2-k^2})$

由上式解出的角度范围是 [ − π , 3 π/2 ] , 且周期为2π，因此，当 θ > π / 2 时， 取 θ = θ − 2 π , 否则，θ 不变。

求解关节二

求解关节四

需要从两个解中筛选一个值，通过判断与上一插补值的绝对值大小，选取绝对值小的作为真实解。

求解关节五

求解关节六

以上过程共解出8组解。

二、数值解（Numerical Solution）

数值解就是采用某种计算方法得到一组近似解，能够在给定精度的情况下使条件成立。只能求出方程特解，不能求出所有解。通常设定一个优化目标函数，把逆解求解问题转化为一个优化问题求数值解。

优点：计算简单，不需要做矩阵转换，通用性高（ms级）；
缺点：迭代次数多，实时性差，且机器人运动过程中的位形不可预测，不适合用于障碍空间中机器人的运动解算。

除了一些特殊的机械臂构型外，机械臂逆运动学问题很难用解析解求解，因此在许多情况下会使用数值解求解。

三、雅可比矩阵求逆法（ Jacobian Inverse）

根据微分运动方程，可得

$$\dot{\mathbf{p}}=\mathbf{J}\cdot \dot{\mathbf{q}}$$

对雅可比矩阵求逆可得

$$\dot{\mathbf{q}}=J^{-1}\dot{\mathbf{p}}$$

如果机械臂的初始关节状态 $\mathbf{q}(0)$已知，最终的目标关节位置可以通过速度对时间的积分进行计算。

$$\mathbf{q}(t)={\int }_0^t\dot{\mathbf{q}}(\varsigma )d\varsigma +\mathbf{q}(0)$$
积分的计算可以通过数值方法对时间离散来实现，最简单是基于欧拉积分法，给定一个积分间隔 $\Delta t$ ，如果 $t_k$ 时刻的关节位置与速度已知，$t_{k+1}=t_k+\Delta t$ 时刻的关节位置可以通过以下实现。

$$\mathbf{q}\left(t_{k+1}\right)=\mathbf{q}\left(t_k\right)+\dot{\mathbf{q}}\left(t_k\right)\Delta t$$

因此我们的逆解求解的关注点便聚焦于求解 $\Delta \theta$，即 $\dot{\mathbf{q}}\left(t_k\right)\Delta t$。

基于微分运动学的思想，机械臂末端点的世界坐标系中的运动$\Delta x$可以近似用关节的运动$\Delta q$叠加得到，$\Delta x$越小，线性关系越准确，即假设机器人从初始位置$q_s$一步步地运动到目标位置 $q_e$，每一步的步长为$\Delta \theta$。

$$\Delta \theta =J^{-1}\Delta x$$
一般迭代求解会对求解的步长做处理，即增加一个变量$\alpha$，这样可以加快求解速度。最后得到如下的迭代公式

$$\Delta \theta =\alpha J^{-1}\Delta x$$

---

$\mathbf{J}$是一个 m x n 的矩阵，m等于机器人工作空间的维数，n 等于机器人关节空间的维数。根据 m 和 n 的关系，可将机器人分成以下三种类型：
（1）n>m：冗余机器人，如7自由度机器人；
（2）n=m：常规机器人，如6自由度机器人；
（3）n<m：欠驱动机器人，如特殊结构的空间5自由度机器人。

对于常规机器人，雅可比矩阵 $\mathbf{J}$ 是方阵，如果 $\mathbf{J}$ 满秩，可直接利用公式 $\dot{\mathbf{q}}=J^{-1}\dot{\mathbf{p}}$ 进行逆解计算。
但在某些位形时，如果$|\mathbf{J}|=0$，则机器人处于奇异位形或奇异状态，$\mathbf{J}$ 的逆不存在，不能直接进行逆解计算，需要采用特殊的解法。

奇异点

https://www.sohu.com/a/195817325_468638

在反向运动学中，当末端位于奇异点时，一个末端位置会对应无限多组解；起因于运动学中使用Jacobian矩阵来转换轴角度及机械手臂末端的关系，当机械手臂中的两轴共线时，矩阵内并非完全线性独立，造成Jacobian矩阵的秩(Rank)会减少，其行列式值(Determinant)为零，使得Jacabian矩阵无反函数，反向运动学无法运算，是为奇异点发生处。
如：Wrist Singularity (腕关节奇异点)：
当第4轴与第6轴共线，会造成系统尝试著将第4轴与第6轴瞬间旋转180度。

如何避免奇异点
奇异点常发生于两轴共线时，当机械手臂的轴数量增加时，发生奇异点的位置与机会同时增加。但因为机械手臂的自由度变多，也表示有更多可以避开奇异点的运动路径可以选择。六轴机械手臂拥有六个自由度，可以达到空间中任何位置，而七轴机械手臂就是为了避开奇异点而产生，多一个自由度来增加避开奇异点的路径选择性，也同时可进行複杂度更高的运动，因为这额外的轴，七轴机械手臂又被称作Redundant Robot，
也有人提出将工具与法兰面(Flange)的关系进行调整，当工具的方向平行于法兰面法线方向时，把工具调整一个微小的角度(5°~15°)，可减少运动路径遇到奇异点的机会。虽无法完全避免，但因成本低且可简单地进行测试，不失为一个好方法。

四、逆解特性分析

机器人工作空间是指机器人末端抓手能够到达的空间点的集合，是在笛卡儿坐标系下描述的。机器人的工作空间包括两类：可达工作空间和灵活工作空间。
可达工作空间是指机器人末端抓手至少能以一种姿态达到的点的集合。灵活工作空间是指机器人末端抓手能够以任意位姿到达的点的集合。很显然，灵活工作空间是可达工作空间的子集。

机器人的工作空间与运动学逆解的关系是：如果机器人末端位姿在其可达工作空间中，其运动学逆解是存在的，如果不在其可达空间中，则其逆解不存在。

机器人运动学逆解的数目与其关节数目、连杆参数、关节变量范围三个因素相关。一般来讲，非零的连杆参数越多则到达某一位姿的方式也越多，即运动学反解数量也越多。如表 3.2 所示是某6自由度机器人（全旋转关节）的运动学逆解数目与非零连杆长度的关系。从表中可看出，当有三个连杆长度为零时，逆解数目最多为4，随着非零连杆长度数目的增多，逆解数目逐渐增多，当连杆长度都不为零时，逆解的数目达到16个。

表 3.2 某6自由度机器人逆解数目与非零连杆长度的关系

ai	逆解数目
a1=a3=a5=0	n≤4
a3=a5=0	n≤8
a3=0	n≤16
都不为0	n≤16

逆解数目并不是越多越好，因为其中存在伪解。伪解只是在数学意义上成立的，但受机器人关节转动范围等因素限制，无法实现相应运动。

在求运动学逆解过程中，求得逆解只是第一步，更重要的是找到最优解。
一般来讲，确定机器人的最优逆解需要采用选优准则，如距离最短、力矩最小等，可结合具体的应用情况选择和确定。确定机器人最优逆解的通用准则是“最短行程”原则，即使每个关节的移动量都为最小的解，这样可以保证机器人的快速响应。由于典型工业机器人的前三个关节大而后三个关节小，因此在采用最短行程原则时又可根据“多移动小关节，少移动大关节”的原则设定每个关节的“加权系数”，在寻优时找到最合适的逆解。

3.3 机器人库

为便于机器人开发，一些机器人库、仿真工具等被开发出来用于运动学及动力学计算、轨迹规划等。

3.3.1 Robotics Library

Robotics Library（RL） 是一个独立的C++库，用于机器人运动学、运动规划和控制。它涵盖了数学、运动学和动力学、硬件抽象、运动规划、碰撞检测和可视化。

API 概述

机器人库的结构是一组层次分明的组件。运动规划的接口建立在基本数学功能、运动学和动力学计算、场景图抽象、硬件抽象、XML解析和实用程序功能的基础上。

1）基本结构、算法和操作系统抽象（Basic Structures, Algorithms and OS Abstraction）

• rl::math 一般数学数据结构和算法
  [1] 基本线性代数数据结构： rl::math::Vector, rl::math::Matrix, rl::math::Array, rl::math::Transform, rl::math::Quaternion
  [2] 空间向量代数： rl::math::MotionVector, rl::math::ForceVector, rl::math::PlueckerTransform, rl::math::RigidBodyInertia, rl::math::ArticulatedBodyInertia
  [3] 泛型函数： rl::math::Function, rl::math::Polynomial, rl::math::Circular
  [4] 分段多项式样条函数：rl::math::Spline
  [5] L2的通用指标：rl::math::metrics::L2 and squared L2 rl::math::metrics::L2Squared
  [6] 最临近搜索：rl::math::LinearNearestNeighbors, rl::math::GnatNearestNeighbors, rl::math::KdtreeNearestNeighbors, rl::math::KdtreeBoundingBoxNearestNeighbors
  [7] 通用PID 控制器：rl::math::Pid
  [8] 通用卡尔曼滤波器：rl::math::Kalman 低通滤波：rl::math::LowPass
• rl::util 操作系统抽象和实用程序函数
• rl::xml xml抽象，包括XPath支持

2）硬件接口（Hardware Interfaces）

• rl::hal 传感器、执行器、和机器人的硬件抽象层
  [1] 抽象设备：rl::hal::JointPositionSensor, rl::hal::JointPositionActuator, rl::hal::Camera, rl::hal::RangeSensor
  [2] 跨平台接口实现： rl::hal::Socket, 跨平台串行通信： rl::hal::Serial
  [3] 机器人驱动程序：rl::hal::Coach, rl::hal::MitsubishiH7, rl::hal::UniversalRobotsRtde
  [4] 抓手驱动程序：rl::hal::WeissWsg50
  [5] 激光扫描仪和测距传感器驱动程序：rl::hal::LeuzeRs4, rl::hal::SchmersalLss300, rl::hal::SickLms200, rl::hal::SickS300, rl::hal::SchunkFpsF5
  [6] 相机驱动程序： rl::hal::Dc1394Camera
  [7] 力矩传感器驱动器：rl::hal::Ati, rl::hal::Jr3, rl::hal::WeissKms40

3）运动学、动力学和轨迹生成

• rl::mdl 运动学和动态空间矢量计算，包括集中关节类型和分支运动学
  [1] 通用模型，具有关节类型的模型： rl::mdl::Revolute, rl::mdl::Prismatic, rl::mdl::Cylindrical, rl::mdl::Helical, rl::mdl::Spherical
  [2] 运动学模型专业化：rl::mdl::Kinematic 前向运动学：rl::mdl::Kinematic::forwardPosition
  [3] 动态模型专业化： rl::mdl::Dynamic 前向动力学：rl::mdl::Dynamic::forwardDynamics ，通过指定向量算法、包含额外约束的反向动力学：rl::mdl::Dynamic::inverseDynamics
  [4] 逆运动学：rl::mdl::JacobianInverseKinematics, rl::mdl::NloptInverseKinematics
  [5] 用于运动学和动力学的 XML 加载程序： rl::mdl::XmlFactory
  [6] 用于运动学和动力学的 URDF 加载程序：rl::mdl::UrdfFactory

4）碰撞检查、可视化和路径规划

• rl::sg 用于碰撞检查、距离查询、光线投射和可视化的场景图抽象
  [1] Basic scene graph hierarchy of a ：rl::sg::Scene containing multiple rl::sg::Model containing multiple rigid rl::sg::Body
  [2] 用于XML\VEML 文件的场景加载程序：rl::sg::Scene::load
  [3] 抽象碰撞检查调用：rl::sg::SimpleScene::areColliding
  [4] 用于多个碰撞检查库的透明包装器：Bullet Physics rl::sg::bullet::Scene, FCL rl::sg::fcl::Scene, Open Dynamics Engine rl::sg::ode::Scene, Proximity Query Package rl::sg::pqp::Scene, SOLID rl::sg::solid::Scene
  [5] 使用VRML场景节点的Coin3D数据结构进行可视化：rl::sg::so::Scene::root
• rl::plan 机器人路径规划算法
  [1] 机器人运动学和碰撞场景的路径规划问题： rl::plan::Model
  [2] 配置空间中的采样技术：rl::plan::UniformSampler, rl::plan::GaussianSampler, rl::plan::BridgeSampler
  [3] 无碰撞路径验证：rl::plan::SequentialVerifier, rl::plan::RecursiveVerifier
  [4] 配置空间度量： rl::plan::Metric 和工作区度量：rl::plan::WorkspaceMetric
  [5] 配置空间最近邻搜索：rl::plan::LinearNearestNeighbors, rl::plan::GnatNearestNeighbors, rl::plan::KdtreeNearestNeighbors, rl::plan::KdtreeBoundingBoxNearestNeighbors
  [6] 与快速探索随机树相关的路径规划算法：rl::plan::Rrt, rl::plan::RrtDual, rl::plan::RrtCon, rl::plan::RrtExtCon, rl::plan::RrtConCon, rl::plan::RrtExtExt
  [7] 与概率图路线相关的路径规划算法： rl::plan::Prm, rl::plan::PrmUtilityGuided
  [8] 探索/利用树路径规划器：rl::plan::Eet
  [9] 路径优化算法：rl::plan::SimpleOptimizer, rl::plan::AdvancedOptimizer

3.3.2 Matlab Robotics toolbox

Robotics Toolbox工具箱是一个功能强大的机器人工具箱，包含了机器人正、逆向运动学，正、逆向动力学，轨迹规划等，其中可视化仿真使得学习抽象的机器人学变得相对直观、好理解。学习这个工具箱，对理解机器人学很有帮助。
建立机器人模型

clear,clc;

% 1、定义连杆参数
%L1 =  Link([ 0,    0,       0.04,      -pi/2], 'standard'); SDH方式
%L1 =  Link([ 0,    0,       0.04,      -pi/2], 'modify');	MDH方式

%          theta    d        a        alpha
L1 =  Link([ 0,    0,       0.04,      -pi/2], 'standard');
L2 =  Link([ 0,    0,       0.345,     0    ], 'standard');
L3 =  Link([ 0,    0,       0.04,      -pi/2], 'standard');
L4 =  Link([ 0,    0.431,   0,         pi/2 ], 'standard');
L5 =  Link([ 0,    0,       0,         -pi/2], 'standard');
L6 =  Link([ 0,    0.08,    0,         0    ], 'standard');

L2.offset = -pi/2; % 设置初始偏置

% 2、定义各关节转动范围
L1.qlim=[-170*pi/180,170*pi/180];
L2.qlim=[-65*pi/180,145*pi/180];
L3.qlim=[-190*pi/180,70*pi/180];
L4.qlim=[-190*pi/180,190*pi/180];
L5.qlim=[-135*pi/180,135*pi/180];
L6.qlim=[-2*pi,2*pi];

% 3、连接连杆，机器人取名TR8
robot_standard=SerialLink([L1,L2,L3,L4,L5,L6], 'name', 'TR8'); 

q0=[0 0 0 0 0 0];
robot_standard.display();
view(3); % 解决robot.teach()和plot的索引超出报错
robot_standard.teach(q0);
robot_standard.display(); %输出模型参数

输出

robot_standard = 
 
TR8:: 6 axis, RRRRRR, stdDH, slowRNE                             
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|       0.04|    -1.5708|          0|
|  2|         q2|          0|      0.345|          0|    -1.5708|
|  3|         q3|          0|       0.04|    -1.5708|          0|
|  4|         q4|      0.431|          0|     1.5708|          0|
|  5|         q5|          0|          0|    -1.5708|          0|
|  6|         q6|       0.08|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+

运动学正逆解

T=robot_standard.fkine(q0) %fkine正解函数，根据给定的关节角q0，求解出末端位姿矩阵
rpy=trprint(T, 'rpy') %根据位姿矩阵得到位置坐标和RPY角


q=robot.ikine(T)	%使用数值解法求逆解
%qlun=robot_standard.ikine6s(T, 'lun');  % 使用封闭解法求逆解，会报错，不建议使用

不同逆解函数：

• ikine6s： 六轴球关节手腕转动机器人逆运动学
• ikine： 采用数值迭代法求解逆运动学
• ikunc： 采用最优化法求解逆运动学
• ikcon： 有关节限位下采用最优化法求解逆运动学
• ikine_sym： 用符号表示的方法解析逆运动学

ikine6s(T, ‘lun’) //逆运动学封闭解，但使用时报错：不支持此运动学结构。
‘l’ arm to the left (default)
‘r’ arm to the right
‘u’ elbow up (default)
‘d’ elbow down
‘n’ wrist not flipped (default)
‘f’ wrist flipped (rotated by 180 deg)
根据手臂左右、手肘上下、手腕翻转情况共有8种分组：lun、luf、ldn、ldf、run、ruf、rdn、rdf

输出

T = 
         0         0         1     0.551
         0        -1         0         0
         1         0         0     0.385
         0         0         0         1

rpy =

    't = (0.551, 0, 0.385), RPY/zyx = (0, -90, -180) deg'

3.3.3 MoveIt! 运动学求解插件

MoveIt!是ROS中一个重要的集成化开发平台，由一系列移动操作的功能包组成，提供运动规划、操作控制、3D感知、运动学等功能模块，是ROS社区中使用度排名前三的功能包，目前已经支持众多机器人硬件平台。下面简要介绍一下MoveIt!中的运动学求解器。

KDL

Kinematics and Dynamics Library (KDL)是MoveIt!中的默认运动学插件，在使用MoveIt! Setup Assistant进行模型配置时，可以进行配置。

配置完成后，就可以运行demo.launch控制虚拟机械臂进行运动规划了。但是KDL有自己的优缺点：

http://xiazuomo.com/2018/kdl-intro/

TRAC-IK

TRAC-IK和KDL类似，也是一种基于数值解的运动学插件，但是在算法层面上进行了很多改进，求解效率高了很多。比如在下边这张图中，左侧的红点是KDL无法求解的姿态点，但是在右侧使用TRAC-IK是可以求解的。

IKFAST

IKFAST是一种基于解析算法的运动学插件，可以保证每次求解的一致性，并且能够自动生成优化的 C++ 代码。