完美的教程，没有之一，收藏学习。
目的
本文手把手教你在 Mathematica 软件中搭建机器人的仿真环境，具体包括以下内容（所使用的版本是 Mathematica 11.1，更早的版本可能缺少某些函数，所以请使用最新版。[email protected]）。1 导入机械臂的三维模型
2 （正/逆）运动学仿真
3 碰撞检测
4 轨迹规划
5 （正/逆）动力学仿真
6 控制方法的验证
不妨先看几个例子：
无论你是从事机器人研发还是教学科研，一款好用的仿真软件能对你的工作起到很大的帮助。那么应该选择哪种仿真软件呢？最方便的选择就是现成的商业软件（例如 Webots、Adams）。你的钱不是白花的，商业软件功能完善又稳定，而且性能一般都经过了优化。可是再强大的商业软件也无法面面俱到。从学习和研究的角度出发，最好选择代码可修改的开源或半开源软件。
在论文数据库中简单检索一下就会发现，很多人都选择在 Matlab 的基础上搭建仿真环境。这并不奇怪，因为 Matlab 具有优秀的数值计算和仿真能力，使用它开发会很便利。如果你非要舍近求远，用 C++ 编写一套仿真软件，先不要说仿真结果如何显示，光是矩阵计算的实现细节就能让你焦头烂额（本来我们要造一辆汽车，可是制作车轮就耗费了大量的精力，而实际上车轮直接买现成的就行了）。
| Matlab | Mathematica | |
| 可视化效果 | 一般 | 优秀 |
| 导入机器人模型 | 需要自己写函数 | 自带函数 |
| 机器人工具箱（包） | Robotic Toolbox、SpaceLib等 | Screws、Robotica等 |
| 调试功能 | 方便易用 | 目前还不太方便，有点繁琐 |
| 代码长度（以Matlab为标准） | 左右 |
1. 准备工作：获取机器人的真实外观模型
制作逼真的仿真需要机器人的外观模型。如果你有机器人的三维模型最好，没有的话也不要紧。著名的机器人制造商都在其官网提供其各种型号机器人的逼真或者真实模型，例如 ABB、安川，供大家免费下载和使用。
说明：为了防止抄袭，这些模型都是不可通过建模软件修改的格式，例如 IGES 和 STEP 格式，但我们只用来显示和碰撞检测，所以并不影响仿真。
2. 导入机器人的外观模型
获得模型后要导入 Mathematica 中进行处理并显示，下面我们借助一个例子说明具体的步骤。Motoman ES165D 是安川公司生产的一款6自由度点焊机器人，其最后三个关节轴线相交于1点，这是一种非常经典而且有代表性的设计，因此我们选择以这款机器人为例进行讲解（这个机器人的模型点击此处下载）。
用 SolidWorks 2014 软件打开机器人的装配体文件，并选择“另存为”STL 格式。然后打开当前页面下方的“选项”对话框，如下图。这里我们要设置三个地方：
1. “品质”表示对模型的简化程度，我们如果想非常逼真的效果，可以选择“精细”，但缺点是数据点很多导致文件很大、处理起来比较慢。一般选择“粗糙”就够了；
2. 勾选“不要转换 STL 输出数据到正的坐标空间”；
3. 不要勾选“在单一文件中保存装配体的所有零件”。
小技巧：STL格式是一种三维图形格式，被很多三维建模软件支持（Mathematica也支持，所以我们要保存为这个格式）。STL格式只存储三维图形的表面信息，而且是用很多的三角形对图形进行近似的表示。如果你的机器人外形比较简单（规则的几何体），那么得到的STL文件大小一般只有几十KB ；可是如果外形很复杂（比如包含很多的孔洞、曲面），生成的STL文件会很大（几MB∼几十MB）。对于一般配置的计算机，模型文件超过100KB用Mathematica处理起来就比较慢了。这时可以利用免费软件MeshLab对其进行化简，MeshLab通常能够在基本不改变外形的前提下将尺寸缩减为原来的十分之一甚至更多。
MeshLab的安装和操作都是傻瓜式的，打开后进入如下图左所示的菜单中，出现右图的页面，这里的“Percentage reduction”表示缩减的百分比（1 表示不缩减，0.1 则表示缩减为原来的10%），设置好后点击Apply并保存即可。
然后在 Mathematica中导入生成的STL 文件，使用的代码如下（假设 STL 文件保存在 D:\MOTOMAN-ES165D 文件夹下）：
-
SetDirectory[
"D:\\MOTOMAN-ES165D"]; (*设置文件的存储位置，注意双斜杠*)
-
n =
6; (* n 是机械臂的自由度，后面还会用到*)
-
partsName = {
"1.stl",
"2.stl",
"3.stl",
"4.stl",
"5.stl",
"6.stl",
"7.stl",
"8.stl",
"9.stl"}; (*组成机械臂的
9个零件*)
-
robotPartsGraphics = Import[
#,
"Graphics3D"
] & /@ partsName; (*一次性导入所有零件，并且导入为直接可以显示的图形格式*)
-
robotParts = robotPartsGraphics
[[;; , 1]]; (*提取出三维图形的几何数据：顶点的三维坐标和边*)
- 1
- 2
- 3
- 4
- 5
- 1
- 2
- 3
- 4
- 5
这里我偷了个懒，为了少打些字，我把导出零件的文件名改成了从1到9的数字（这个机械臂的装配体一共包含9个零件）。想要显示导入的机器人模型可以使用以下代码，显示效果如下图：
Graphics3D[{frame3D, robotParts}]
- 1
- 1
说明：frame3D是三维（右手）坐标系图形，因为我们会用到很多坐标系及其变换，将坐标系显示出来更直观。定义 frame3D 的代码如下。这个坐标系默认的位置在原点，我们称这个坐标系为全局坐标系。
frame3D = {RGBColor[#], Arrowheads[0.03], Arrow@Tube[{{0, 0, 0}, 0.5 #}, 0.01]} & /@ IdentityMatrix[3]; (*改变数值可以改变坐标系的长度、坐标轴的粗细等显示效果*)
- 1
- 1
你可能会好奇：导入的零件是以什么样的格式存储的呢？
存储机器人外形数据的
robotParts 变量包含9个零件的数据，假如你想看第一个零件（对应的是基座，它通常用来将机械臂固定在大地上），可以输入：
robotParts[[1]] (*双层方括号中的数字表示对应第几个零件*)
- 1
- 1
运行后的输出结果是一堆由 GraphicsComplex 函数包裹着的数字，稍加分辨会发现这些数字又包含两部分：第一部分是零件所有顶点的三维坐标；第二部分是组成零件外形的三角形（构成每个三角形的三个顶点是第一部分点的序号，而不是坐标）。我们可以用以下代码将其分别显示出来：
-
pts = robotParts[[
1,
1]];
(*第一部分：顶点的三维坐标数据*)
-
triangles = robotParts[[
1,
2]];
(*第二部分：三角形面片*)
-
trianglesB = triangles /.
{EdgeForm[] -> EdgeForm[Blue]};
(*三角形的边显示为蓝色 Blue*)
-
Graphics3D[
{Red, Point[pts], White, GraphicsComplex[pts, trianglesB]}]
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
当然你可以任性而为，用哪个都可以。不过根据国家标准《GBT 16977-2005 工业机器人 坐标系和运动命名原则》，基座坐标系的 轴应该垂直于机器人基座安装面（也就是地面）、朝向为重力加速度的反方向， 轴指向机器人工作空间中心点。制定国标的都是些经验丰富的专家老手，我们最好跟国标保持一致（国标的作图水平就不能提高点吗？这图怎么感觉像小学生画的）。
为此，定义旋转矩阵：
-
Xaxis =
{1, 0, 0}; Yaxis =
{0, 1, 0}; Zaxis =
{0, 0, 1};
(*定义旋转轴，更简洁的写法是： {Xaxis,Yaxis,Zaxis}=IdentityMatrix[3];*)
-
rot = RotationMatrix[
90 Degree, Zaxis].RotationMatrix[
90 Degree, Xaxis];
(*注意第二次变换是在左边乘*)
-
rot = RotationMatrix[
90 Degree, Xaxis].RotationMatrix[
90 Degree, Yaxis];
(*注意第二次变换是在右边乘*)
- 1
- 2
- 3
- 1
- 2
- 3
然后用 rot 矩阵旋转每个零件（的坐标，即保存在第一部分 robotParts[[i, 1]] 中的数据）：
robotParts=Table[GraphicsComplex[rot.# & /@ robotParts[[i, 1]], robotParts[[i, 2]]], {i, 9}];
- 1
- 1
经过姿态变换后的机器人看起来舒服点了，只是有些苍白。为了给它点个性（也方便区分零件），我们给机械臂设置一下颜色，代码如下。你可能注意到了，这里我没有使用循环去为9个零件一个一个地设置颜色，而是把相同的元素（颜色）写在一起，这样做的好处就是代码比较简洁、清晰。以后我们会经常这么做。
-
colors =
{Gray, Cyan, Orange, Yellow, Gray, Green, Magenta, Lighter[Green], Pink};
(*1~9 各零件的颜色*)
-
robotPartsColored = Transpose[
{colors, robotParts}];
(*把颜色分配给各零件*)
-
Graphics3D[robotPartsColored]
- 1
- 2
- 3
- 1
- 2
- 3
说明：现在的机器人姿势（大臂竖直、小臂前伸）是6自由度机械臂的“零位”状态，我们将此时机械臂各关节的角度认为是0。一般机械臂上都有各关节的零点位置标记，用于指示各关节的零点。我们用控制器控制机械臂时，发出的角度指令都是相对于这个零点位置的。零点位置不是必须遵守的，你可以将任意的角度设置为零位，不过为了统一，最好用机械臂固有的零位——也就是当前的姿势。
3. 运动学仿真
前面的工作只是让机械臂的模型显示出来，如果你想让机器人动起来，那就要考虑运动学了。机器人听起来高大上，可实际上现在大多数工业机器人的控制方式还是比较低级的，它们只用到了运动学，高级一点的动力学很少用，更不要提智能了（它们要说自己有智能，我们家的洗衣机和电视机都不服）。有的公司（例如倍福），更是将支持不同类型的机械臂的运动学作为宣传的噱头。看来要使用机器人，运动学是必不可少的，所以我们先来实现运动学。
3.1 零件的局部坐标系
机器人的运动也就是其构成连杆（零件）的运动。而为了描述连杆的运动，我们要描述每个连杆的位置和姿态（合称为“位姿”）。通常的做法是在每个连杆上固定一个坐标系（它跟随连杆一起运动），这个坐标系称为“局部坐标系”。通过描述局部坐标系的位姿我们就可以描述每个连杆的位姿。如何选择局部坐标系呢？理论上你可以任意选择，不过局部坐标系影响后续编程和计算的难易程度，所以我们在选择时最好慎重。在运动学建模和动力学建模中，坐标系的选择通常是不同的。
● 运动学建模时，连杆的局部坐标系一般放置在关节处，这是因为常用的 D-H 参数是根据相邻关节轴定义的。
● 动力学建模时，连杆的局部坐标系一般放置在质心处，这是因为牛顿方程是关于质心建立的，而且关于质心的转动惯量是常数，这方便了计算。
我们先考虑运动学，因此将局部坐标系设置在关节处。在 SolidWorks 中打开任何一个零件，都能看到它自己有一个坐标系。构成一个零件的每一条边、每一个孔的数据都以这个坐标系为参考，我们称它为“绘图坐标系”。绘图坐标系通常不在质心处，因为在你还没画之前你根本不知道质心在哪里。绘图坐标系通常在零件的对称中心或者关节处，我们不妨将每个零件的绘图坐标系当做它的局部坐标系。
那么下一个问题是每个零件的绘图坐标系在哪儿呢？我们以第三个零件为例，如下图左所示。我们点击左侧的“原点”标签，图中就会显示绘图坐标系的原点。（如果你想将绘图坐标系显示出来，可以先选中“原点”标签，然后点击上方菜单栏中的“参考几何体”，再选择“坐标系”，然后直接回车即可看到新建的绘图坐标系，如右图，可见它位于一个关节轴）
drawInGlobalSW = {{0, 0, 0}, {0, 650, 0}, {-315, 1800, 285}, {-53.7, 1800, 285}, {0, 2050, 1510}, {0, 2050, 1510}, {0, 2050, 1720.5}}/1000;
- 1
- 1
因为我们是在 SolidWorks 中测量的位置，所以这些位置值还是相对于 SolidWorks 的坐标系（ 轴朝上），要变到 轴朝上，方法仍然是乘以旋转矩阵 rot：
drawInGlobal = Table[rot.i, {i, drawInGlobalSW}];
- 1
- 1
以后会经常用到对坐标的旋转变换，而且多数时候是用一个旋转矩阵同时对很多坐标进行变换（例如上面的这个例子），我们不如定义一个算子以简化繁琐的代码。我们定义算子（其实是一个函数）：
CircleDot[Matrix_,Vectors_]:=(Matrix.#)&/@Vectors;
- 1
- 1
所以前面的变换用我们自定义的算子表示就是（复制到 Mathematica中后 \[CircleDot] 会变成一个Mathematica内部预留的图形符号，这个符号没有被占用，所以这里我征用了）：
drawInGlobal = rot\[CircleDot]drawInGlobalSW; (*哈哈！写起来是不是简单多了*)
- 1
- 1
Mathematica 自带的函数首字母都是大写。为了与官方函数区分，我自定义的函数一般采用小写字母开头。本文使用的自定义的函数都会给出实现代码，而且为了方便，我将常用的自定义函数打包成一个函数包，每次运行程序时导入此函数包即可使用里面的函数。该函数包依赖另一个函数包 Screws.m （我修改了部分函数的名字，为此重新定义了 myScrews.m）。两个函数包点击此处下载。在程序中导入函数包的代码如下（假设函数包位于你的程序笔记本文件的同一目录下）：
SetDirectory[NotebookDirectory[]]
<< myFunction.m
还有印象吗？最开始导出和导入零件模型时，各零件的位置都已经按照装配关系确定好了，所以它们的数据也是相对于全局坐标系描述的。可是现在我们要让机械臂动起来（而且还要显示出来），这就要移动这些数据。为了方便起见，最好能将每个零件的模型数据表示在自己的绘图坐标系中，因为这样我们只需要移动绘图坐标系就行了，而各点的数据相对它们所属的绘图坐标系是不动的。应该怎么做呢？很简单，将零件模型的数据减去绘图坐标系的原点在全局坐标系中的坐标即可：
-
partsName = {
"1.stl",
"2.stl",
"3.stl",
"6.stl",
"7.stl",
"8.stl",
"9.stl"}; (*已经去除了弹簧缸的
2个零件：
4号和
5号*)
-
robotPartsGraphics = Import[
#,
"Graphics3D"
] & /@ partsName;
-
robotParts = robotPartsGraphics
[[;; , 1]];
-
robotParts = Table[GraphicsComplex[rot\[CircleDot]robotParts
[[i, 1]], robotParts
[[i, 2]]], {i,
7}];
-
robotParts = Table[GraphicsComplex[(
# - drawInGlobal
[[i]]
) & /@ robotParts
[[i, 1]]
, robotParts
[[i, 2]]
], {i,
7
}];
-
colors = {Gray, Cyan, Orange, Green, Magenta, Yellow, Pink}; (*重新定义零件的颜色*)
-
robotPartsColored = Transpose@{colors, robotParts};
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 1
- 2
- 3
- 4
- 5
- 6
- 7
移动后的零件模型如下图所示（图中的坐标系是各个零件自己的绘图坐标系，我没有对数据转动，所以绘图坐标系和全局坐标系的姿态相同）。我们一开始从 SolidWorks 导出文件时是一次性地导出整个装配体的。其实，如果我们挨个打开各个零件并且一个一个的导出这些零件，那么得到数据就是相对于各自的绘图坐标系的，只不过这样稍微麻烦一点。
3.2 利用旋量建立运动学模型
下面我们讨论如何建立运动学模型。描述机器人连杆之间几何关系的经典方法是采用 D-H 参数（Denavit - Hartenberg parameters）。能留下自己名字的人都不是一般人，那么 D-H 参数巧妙在什么地方呢？我们知道，完全确定两个坐标系（或者刚体）的位姿关系需要6个参数，因为有6个自由度。如果不考虑关节转动（平移）仍需要5个参数。然而 D-H 参数居然只用了4个参数就能够确定相邻连杆的位姿关系，可见 Denavit 和 Hartenberg 这哥俩确实动了番脑筋。不过为了避免 D-H 参数的一些缺点，我们弃之不用而采用旋量的表示方法。旋量有什么性质、它和刚体运动的关系是什么、这些问题数学家用了很长时间才搞清楚。在本文中你可以把旋量简单想象成一个表示关节转动的量。表示一个关节旋量需要确定一个关节轴线的方向向量（3个参数）和轴线上任意一点的坐标（又要3个参数）。
旋量和向量相似的地方是，它也要相对于一个坐标系来描述。我们选择哪个坐标系呢？这里我们要参考 D-H 参数，每一个连杆坐标系在定义时都相对于前一个连杆的坐标系。所以我们将每个关节轴的旋量表示在前一个连杆中。这次我们以2号零件为例说明如何确定关节轴的旋量：
1. 首先来看关节轴线的方向，这个要相对于2号零件的绘图坐标系。（我们要确定关节2的旋量，至于关节1的旋量最好在零件1中确定）。从下图中看关节2的轴线方向似乎是 轴，可是我们前面将绘图坐标系的姿态和全局坐标系的姿态设定为一样的，所以应该在全局坐标系（基座坐标系）中确定，也就是 轴。
２. 关节轴线上任意一点的坐标，这个同样要相对于2号零件的绘图坐标系。我们在轴线上任选一点即可。步骤是：点击 SolidWorks 上方菜单栏的“参考几何体”，选择“点”，然后在左侧面板选择“圆弧中心”，然后选择图中的关节轴周围的任意同心圆弧即可创建一个参考点，这个点就是我们想要的。我们可以在零件视图中测量这个点的坐标，也可以在机器人完整装配体中测量，这里我选择后者。（测量步骤参照前面测量“零件绘图坐标系的原点”）
-
axesPtInGlobal = rot\[CircleDot]{{
0,
257,
0}, {
-
88,
650,
285}, {
-
280.86,
1800,
285}, {
0,
2050,
1318}, {
134,
2050,
1510}, {
0,
2050,
1720.5}}/
1000;
-
axesPtInDraw = axesPtInGlobal - drawInGlobal
[[1 ;; -2]];
-
axesDirInDraw = axesDirInGlobal = {Zaxis, Yaxis, Yaxis, Xaxis, Yaxis, Xaxis};
-
\[Xi]r = Table[\[Omega]r[i] = axesDirInDraw
[[i]]; lr[i] = axesPtInDraw
[[i]];
Join[Cross[-\[Omega]r[i], lr[i]], \[Omega]r[i]], {i, n}];
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
我们对关节的运动进行了建模，要建立运动学还缺少最后一样东西：零件间的初始相对位姿（初始的意思是机械臂处于“零位”的状态下）。零位下，我们将所有零件的姿态都认为和全局坐标系一样，所以不用计算相对姿态了。至于它们的相对位置嘛，我们已经知道了绘图坐标系原点在全局坐标系中的坐标，两两相减就可以得到它们的相对位置了，很简单吧！（见下面的代码）
Do[g[L[i], L[i + 1], 0] = PToH[drawInGlobal[[i + 1]] - drawInGlobal[[i]]], {i, n}];
- 1
- 1
其中，PToH 函数能将位置向量转换为一个齐次变换矩阵，这是借助 RPToH 函数实现的（RPToH 函数就是 Screws 工具包中的RPToHomogeneous 函数），它可以将一个旋转矩阵和位移向量组合成一个齐次变换矩阵。将旋转矩阵和位移向量合成为齐次变换矩阵是我们以后会经常用到的操作。类似的，也可以定义 RToH 函数将旋转矩阵生成对应的齐次变换矩阵，代码如下：
-
RToH[R_]:= RPToH[R,
{0,0,0}]
-
PToH[P_]:= RPToH[
IdentityMatrix[
3],P]
- 1
- 2
- 1
- 2
说明：本文中，用符号 I 表示全局坐标系（同时也是惯性坐标系）；符号 L[i] 表示第 i 个连杆，变量 g[L[i], L[i+1]] 表示第 i+1 个连杆相对于第 i 个连杆的位姿矩阵（它是一个的齐次变换矩阵）；变量 g[I, L[i]] 表示什么你肯定猜到了，它表示第 i 个连杆相对于全局坐标系的位姿矩阵。如果不特别说明，本文总是用 g （或者 g 开头的变量）表示一个（或一组）齐次变换矩阵，这是约定俗成的。
现在可以正式推导机械臂的运动学模型了。在使用机械臂时，大家一般只关心其最末端连杆的位姿，更确切的说，是最末端连杆的位姿与关节角度的关系。不过为了得到最末端连杆的位姿，我们需要计算中间所有连杆的位姿。这里利用相邻连杆的递归关系——每个连杆的位姿依赖前一个连杆的位姿——来提升计算效率。所以，可以定义机械臂所有连杆的运动学函数为：
-
robotPartsKinematics[configuration
_] := Module[{
q, g2To7},
-
{g[I, L[
1]],
q} = configuration;
-
g2To7 = Table[g[L[i], L[i +
1]] = TwistExp[\[Xi]r
[[i]],
q
[[i]]].g[L[i], L[i +
1],
0];
-
g[I, L[i +
1]] = g[I, L[i]].g[L[i], L[i +
1]], {i, n}];
-
Join[{g[I, L[
1]]}, g2To7] ]
- 1
- 2
- 3
- 4
- 5
- 1
- 2
- 3
- 4
- 5
robotPartsKinematics函数的输入是：基座的位姿矩阵 g[I, L[1]] 和所有关节的角度向量q，这组变量完整地描述了一个串联机械臂的位置和姿势（用机器人中的专业术语应该叫“构型”： configuration，注意不要翻译为“配置”），而输出则是所有连杆相对于全局坐标系的位姿（即 g[I, L[i]]，其中i = 1~7）。
其中，TwistExp 函数来自于 Screws 工具包，作用是构造旋量的矩阵指数。
说明：在大多数的机器人教科书中，连杆的记号是从0开始的，也就是说将基座记为0号连杆，然后是1号连杆，最末端的连杆是号（假设机械臂的自由度是）；而关节的记号是从1开始，也就是说1号关节连接0号连杆和1号连杆。这样标记的好处是记号一致，推导公式或编程时不容易出错：比如说我们计算第 个连杆的速度时要利用第 个关节的转动速度。可是本文中连杆的记号是从1开始的（基座标记为1号连杆），我们保留0号标记是为了以后将机械臂扩展到装在移动基座的情况，这时0号就用来表示移动基座（比如一个AGV小车）。
可以看到，只要定义好关节旋量，建立运动学模型非常简单。可是这样得到的运动学模型对不对呢？我们来检验一下。借助Manipulate 函数，可以直接改变机械臂的各关节角度，并直观地查看机械臂姿势（应该叫构型了哦）的变化，如以下动画所示。可以看到，机械臂各连杆的运动符合我们设置的关节值，这说明运动学模型是正确的。
-
Manipulate[qs =
{##}
[[;; ,
1
,
1
]];
-
gs = robotPartsKinematics[
{IdentityMatrix[4], qs}];
-
Graphics3D[
{MapThread[move3D, {robotPartsColored, gs}]}
-
,
PlotRange ->
{{-2, 3},
{-2, 3},
{0, 3}}],
##,
ControlPlacement
->
Up
] & @@
Table
[
{{q[i], 0}
, -
Pi
,
Pi
,
0.1
},
{i, n}
]
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
move3D[shape_,g_]:=GeometricTransformation[shape,TransformationFunction[g]];
- 1
- 1
验证使用的代码如上。其中，move3D 函数的功能是用一个齐次变换矩阵（g）移动一个几何图形（shape）。这里还值得一提的是 MapThread 函数。虽然我们可以用 move3D 函数去一个一个地移动连杆（写起来就是：move3D[part1, g1], move3D[part2, g2], move3D[part3, g3]……），这样写比较清楚也很容易读懂，可就是太麻烦了，想象你的机械臂有一百个连杆就不得不用循环了。但是使用 MapThread 函数写起来就非常简单了，而且得到的结果与前面完全一样（MapThread[move3D, {{part1, part2, part3}, {g1, g2, g3}}]）。这就是为什么我一直强调最好把同类型的元素放到一起，因为操作的时候可以一起批量化进行。
可以看到，Mathematica 提供的控件类函数 Manipulate 支持用户使用鼠标交互式地改变变量的值，同时动态更新对应的输出。如果一段代码运行时间足够快，就可以放在Manipulate 内部，比如运动学函数robotPartsKinematics，它包含的计算并不复杂，但如果是后面要介绍的动力学函数就不适合放在Manipulate里面了，因为动力学的计算比较耗时，窗口会显得很“卡顿”。
4. 逆运动学仿真
借助运动学，我们成功地通过改变关节角度实现了对机械臂的控制。当然这没什么值得炫耀的，本质上不过是矩阵相乘罢了。本节我们考虑一个更好玩的问题。如果告诉你所有连杆（局部坐标系）的位姿，你能不能算出机械臂的各个关节角来？你一定会说这很简单，求一下反三角函数就行了。但是实际应用时经常会遇到比这个稍难些的问题：只告诉你机械臂最后一个连杆的位姿，如何得到各关节的角度？这个问题被称为逆运动学。Robotic Toolbox工具箱中给出了两个解逆运动学问题的函数：ikine 和 ikine6s，分别是数值解法和符号解析解法，本文我们也用两种方式解决逆运动学问题。
说明：其它求解逆运动学的软件工具还有 IKFast——适用于6自由度机械臂，求得的是解析解，求解速度贼快；Kinematics and Dynamics Library（KDL）——适用于任意自由度，求得的是数值解。这些代码都是开源的，你可以研究研究。
4.1 数值解法之——解方程
上一节的运动学函数 robotPartsKinematics 能得到所有连杆的位姿。大多数时候，人们只关心最后一个连杆的位姿（因为它上面要装载操作工具），即 Last@robotPartsKinematics[{IdentityMatrix[4], q}]（注意q是一个六维向量，即q=()），结果如下图所示（另存为可以看大图）。这里关节角没有设置数值，因此得到的是符号解，有些长哦。这也是为什么机器人领域经常使用缩写的原因：比如把 记为。在中提供了一个函数 SimplifyTrigNotation，可以用来对下式进行缩写。
如果我们想让机械臂末端（连杆）到达某个（已知的）位姿
gt，也就是让上面的矩阵等于这个位姿矩阵：
Last@robotPartsKinematics[{IdentityMatrix[4], {q1, q2, q3, q4, q5, q6}}] = gt (*逆运动学方程*)
- 1
- 1
通过解上面这个以6个关节角 为未知量的方程组就能知道机械臂的构型了。也就是说，逆运动学问题的本质就是解方程。从小到大我们解过无数的方程。数学有很大一部分就是在研究怎么解方程、解各种各样的方程：大的小的、线性的非线性的、代数的微分的。Mathematica 提供了不止一个函数用来解方程：Solve、NSolve、DSolve、LinearSolve、FindRoot 等等。面对这么多工具，我们应该用哪个好呢？你选用的求解方法取决于方程的类型，我们看看这个方程是什么类型呢？首先它是个代数方程，其次里面含有三角函数，所以是非线性代数方程。代数方程有数值解法和解析解法。我们非常想得到用符号表示的解析解，因为只需要解一次以后直接带入数值即可，计算速度非常快。但是非线性方程一般很难得到符号解，所以我们只好退而求其次找数值解了，这样就把范围缩小到 NSolve、FindRoot 这两个函数了。NSolve 会得到所有解（这个方程有不止一个解哦），而 FindRoot 会根据初始值得到最近的解。一番试验表明只有 FindRoot 函数能满足我们的需求。
说明：在求解逆运动学方程前还需要解决一个小问题：如何在 Mathematica 中表示一个期望的目标位姿 gt 呢？Mathematica 提供了 RollPitchYawMatrix 函数和 EulerMatrix 函数用来表示三维转动（你用哪个都可以），然后利用前面的 RPToH 函数合成为位姿矩阵 gt 即可，示例代码如下。其中，cuboid 函数用于绘制一个长方体。如果你使用 Matlab ，那我要可怜你了。因为 Matlab 没有绘制长方体的函数，一切你都要自己画。 而 Mathematica 定义了一些常用几何图形，可以直接用。
cuboid[center_, dim_]:= Cuboid[center - dim/2, center + dim/2]
- 1
- 1
-
object = cuboid[{0, 0, 0}, {0.3, 0.2, 0.05}];
-
Manipulate[
-
gt = RPToH[RollPitchYawMatrix[{\[Alpha], \[Beta], \[Gamma]}], {
x,
y, z}];
-
Graphics3D[{Yellow, move3D[{frame3D, object},
gt]}, PlotRange ->
{{-0.5, 0.5}, {-0.5, 0.5}, {-0.5, 0.5}}
], Grid[
{{Control[{{x, 0}, -0.5, 0.5, 0.1}], Control[{{\[Alpha], 0}, 0, 2 Pi, 0.1}]}, {Control[{{y, 0}, -0.5, 0.5, 0.1}],Control[{{\[Beta], 0}, 0, 2 Pi, 0.1}]}, {Control[{{z, 0}, -0.5, 0.5, 0.1}], Control[{{\[Gamma], 0}, 0, 2 Pi, 0.1}]}}
], TrackedSymbols :> True]
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
不过这个方程组是由 齐次变换矩阵得到的，里面有 个方程，去掉最后一列对应的4个恒等式还有12个方程，超过了未知量（6个）的个数，这是因为 旋转矩阵的各项不是独立的，因此要舍去一部分。该保留哪三项呢？只要不选择同一行或同一列的三项就可以了，这里我保留了三项。
-
Manipulate[
-
gts = Last@robotPartsKinematics[{IdentityMatrix[
4], {q1, q2, q3, q4, q5, q6}}];
-
gt = RPToH[RollPitchYawMatrix[{\[Alpha], \[Beta], \[Gamma]}], {
x,
y, z}];
-
Quiet[qts = {q1, q2, q3, q4, q5, q6}/.FindRoot[gts
[[1, 4]] ==
gt
[[1, 4]] && gts
[[2, 4]] ==
gt
[[2, 4]] && gts
[[3, 4]] ==
gt
[[3, 4]] && gts
[[1, 2]] ==
gt
[[1, 2]] && gts
[[2, 3]] ==
gt
[[2, 3]] && gts
[[3, 3]] ==
gt
[[3, 3]], {q1,
0}, {q2,
0}, {q3,
0}, {q4,
0.3}, {q5,
0.3}, {q6,
0.3}]];
-
planeXY = {FaceForm[], EdgeForm[Thickness[
0.005]], InfinitePlane[{
x,
y, z}, {{
0,
1,
0}, {
1,
0,
0}}], InfinitePlane[{
x,
y, z}, {{
0,
1,
0}, {
0,
0,
1}}]};
-
lines = {Red, Thickness[
0.0012], Line[{{
x,
y, z} + {
100,
0,
0}, {
x,
y, z} + {-
100,
0,
0}}], Line[{{
x,
y, z} + {
0,
100,
0}, {
x,
y, z} + {
0, -
100,
0}}], Line[{{
x,
y, z} + {
0,
0,
100}, {
x,
y, z} + {
0,
0, -
100}}]};
-
Graphics3D[{planeXY, lines, MapThread[move3D, {robotPartsColored, robotPartsKinematics[{IdentityMatrix[
4], qts}]}], move3D[frame3D, g[I, L[
7]]]}, PlotRange -> {{-
1.5,
2.5}, {-
2.5,
2.5}, {
0,
3}}],
-
Grid[{{Control[{{
x,
1.3}, -
2,
3,
0.1}], Control[{{
y,
0}, -
2,
2,
0.1}]},
-
{Control[{{z,
2},
0,
3,
0.1}], Control[{{\[Alpha],
0},
0, Pi,
0.1}]},
-
{Control[{{\[Beta],
0},
0, Pi,
0.1}], Control[{{\[Gamma],
0},
0, Pi,
0.1}]}}], TrackedSymbols :> True]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
同样借助 Manipulate 函数改变值的大小，试验的效果如下图。
4.2 数值解法之——迭代法
解方程的方法很多，下面我们换一种思路求解逆运动学方程，其思想来自于（英文版187页），代码如下：
-
forwardKinematicsJacobian[argList_, gst0_] :=
-
Module[
{g = IdentityMatrix[4], \[Xi], n = Length[argList]},
-
Js =
{};
(*注意空间雅可比矩阵Js是全局变量，后面会用*)
-
Do[\[Xi] = Ad[g].argList[[i,
1]];
-
Js =
Join[Js,
{\[Xi]}];
-
g = g.TwistExp[argList[[i,
1]], argList[[i,
2]]]
-
,
{i, n}];
-
Js = Transpose[Js];
-
g.gst0 ]
-
\[Xi]a = Table[\[Omega]a[i] = axesDirInGlobal[[i]]; la[i] = axesPtInGlobal[[i]];
Join[Cross[-\[Omega]a[i], la[i]], \[Omega]a[i]],
{i, n}];
-
(*forwardKinematicsJacobian函数是从 Screws.m 中抄的，它使用了表示在全局坐标系的旋量，因此定义\[Xi]a*)
-
inverseKinematics[gt_, q0_, errorthreshold_:
0.0001] :=
-
Module[
{gReal, q = q0, Jb, Jg, F, error, theta, axis, positionerror, angleerror, maxIter = 20},
(*输入期望的机械臂末端位姿 gt 和初始关节角 q0*)
-
Do[gReal = forwardKinematicsJacobian[Transpose@
{\[Xi]a, q}, g[I, L[
7],
0]];
-
Jb = Ad[Inverse[gReal]].Js;
-
Jg = diagF[gToR[gReal], gToR[gReal]].Jb;
-
positionerror = gToP[gt - gReal];
-
angleerror =
Reverse@RollPitchYawAngles[gToR[gt.Inverse[gReal]]];
(*注意Reverse函数*)
-
error = Flatten[N[
{positionerror, angleerror}]];
(*误差向量 error 包括位置和角度分量在全局坐标系中表示*)
-
F = PseudoInverse[Jg].error;
-
q = q + modToPiPi[F];
-
If[Norm[error] < errorthreshold,
Break[]]
-
,
{maxIter}];
-
q]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
forwardKinematicsJacobian 函数用于计算（空间）雅可比矩阵和最后一个连杆的位姿，它修改自 Screws 工具包。逆运动学计算函数 inverseKinematics 的输入是期望的末端连杆位姿 gt，迭代的初始角度 q0 ，以及误差阈值 errorthreshold （默认值为 0.0001）。
其中的 modToPiPi 函数（实现代码如下）用于将角度值转换到 的范围之间。这里为什么需要 modToPiPi 函数呢？因为角度是个小妖精，如果我们不盯紧它，它可能会时不时的捣乱。从外部看，机械臂的一个转动关节位于角度 和角度 没什么区别。可是如果我们放任角度这样随意跳变，会导致轨迹不连续，这样机械臂在跟踪轨迹时就会出现麻烦。
-
modToPiPi[angle_]:=
Module[
{a = Mod[angle,2.0*Pi]},
If[
Re[a]>=
Pi, a-
2.0*
Pi, a]]
-
SetAttributes[modToPiPi,
Listable];
- 1
- 2
- 1
- 2
其中，Ad 函数就是 Screws 工具包中的 RigidAdjoint 函数，它表示一个齐次变换矩阵的伴随变换（Adjoint Transformation），diagF 函数用于将多个矩阵合成为块对角矩阵，实现代码如下：
diagF=SparseArray[Band[{1,1}]->{##}]& (*用法为 A = {{1,2},{3,4}}; B = {{5,6},{7,8}}; diagF[A,B]//MatrixForm *)
- 1
- 1
gToR 函数和 gToP 函数分别用于提取一个齐次变换矩阵中的旋转矩阵（R）和位移向量（P），代码如下。
-
gToR[g_]:= Module[{n=(Dimensions[g])
[[1]]
-
1}, g
[[1;;n,1;;n]]]
-
gToP[g_]:= Module[{n=(Dimensions[g])
[[1]]
-
1}, g
[[1;;n,n+1]]]
- 1
- 2
- 1
- 2
我们以后会用到很多矩阵操作（比如转置、求逆）。而 Mathematica 的函数名太长，为了写起来方便，我定义了简写的转置和求逆函数，代码如下：
-
T[g_]:=
Transpose[g]
-
Iv[g_]:=
Inverse[g]
- 1
- 2
- 1
- 2
我们想让机械臂（的末端）依次到达一些空间点（这些点可能是机械臂运动时要经过的）。为此首先生成一些三维空间中的点：
-
Clear[x,y];
-
pts2D = Table[
{Sin[i], Cos[i]}/
1.4,
{i, 0, 4 Pi, Pi/400}];
(*先生成二维平面上的点，它们均匀地分布在一个圆上*)
-
pts3D = pts2D /.
{x_, y_} ->
{1.721, x, y + 1.4};
(*再将二维坐标变换成三维坐标*)
-
Graphics3D[Point[pts3D]]
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
然后调用逆运动学函数 inverseKinematics 挨个计算不同点处的关节值，代码如下：
-
gStars = PToH /@ pts3D;
(*将三维点的坐标转换成齐次变换矩阵，转动部分始终不变*)
-
q = ConstantArray[
0, n];
(*inverseKinematics函数包含一个迭代过程，因此需要提供一个初始值*)
-
g[I, L[
7],
0] = (robotPartsKinematics[
{IdentityMatrix[4], q}]; g[I, L[
7]]);
(*forwardKinematicsJacobian函数需要零位状态下的末端连杆位姿*)
-
qs = Table[q = inverseKinematics[i, q],
{i, gStars}];
//AbsoluteTiming (*依次遍历所有点，我们用每次计算得到的 q 作为下一次迭代的初始值，这样迭代速度更快*)
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
计算结果 qs 中存储了机械臂到达不同点处的关节向量，如果以后我们想让机械臂跟踪这个向量序列，可以对其插值得到连续的关节函数，这是靠 Interpolation 函数实现的，代码如下。关于 Interpolation 函数我要啰嗦几句，因为以后我们可能会经常用到它。对于每个关节来说， Interpolation 得到的是一个插值函数（InterpolatingFunction），更确切地说是“Hermite多项式” 或“Spline 样条”插值函数。它与其它的纯函数没什么区别，可以对它求导、求积分。例如，我们可以对这6个关节的插值函数求导从而得到关节速度和加速度函数：
-
time =
10;
(
*time是自己定义的，表示机械臂运动经过所有点的总时间*
)
-
Do[qt[i] = T@
{Range[0, time, time/(Length[(T@qs)[[i]]] - 1)], (T@qs)[[i]]},
{i, n}];
-
Do[qfun[i] = Interpolation[qt[i]];
-
dqfun[
i][
x_] = D[
qfun[i][
x], x];
-
ddqfun[
i][
x_] = D[
dqfun[i][
x], x],
{i, n}]
- 1
- 2
- 3
- 4
- 5
- 1
- 2
- 3
- 4
- 5
画出插值后各关节的角度、角速度、角加速度的变化趋势，如下图。能看到有两个关节角速度变化剧烈。理论上说，这个曲线不适合让机器人跟踪。
-
pq = Plot[
Evaluate@Table[qfun[i][
x], {i,
6}], {
x,
0,
time}, PlotRange -> All];
-
pdq = Plot[
Evaluate@Table[dqfun[i][
x], {i, n}], {
x,
0,
time}, PlotRange -> All];
-
pddq = Plot[
Evaluate@Table[ddqfun[i][
x], {i, n}], {
x,
0,
time}, PlotRange -> All];
-
GraphicsGrid[{{pq, pdq, pddq}}]
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
4.3 雅克比矩阵的零空间
在上一节求解逆运动学问题时我们使用了机械臂的雅克比矩阵。雅克比矩阵能够将关节速度映射到末端连杆的速度。由于末端连杆的速度有不止一种定义方式（例如有：空间速度、本体速度、全局速度，它们的定义见我的另一篇博客），所以对应了不同的雅克比形式（也就是逆运动学函数中的 js、Jb、Jg）。
雅克比矩阵有一些有趣的性质，其中一个比较有意思的是它的零空间。只要关节速度在（雅克比矩阵的）零空间中，那末端连杆的速度总是零（零空间由此得名）。通俗的说就是：不管关节怎么动，末端连杆始终不动（就像被钉死了一样）。这个性质还挺有用的，因为有些场合要求机械臂在抓取东西的时候还能躲避障碍物。在其它领域，例如摄影，为了保证画面稳定需要摄像机能防抖动；在动物王国中，动物觅食时头部要紧盯猎物（被恶搞的稳定鸡）；在军事领域（例如坦克、武装直升机），要求炮口始终瞄准目标，不管车身如何移动和颠簸。
对于本文中的 6 自由度机械臂，由于它不是冗余的，所以大多数时候计算零空间会得到空（说明不存在零空间）。我为了展示零空间的效果只用了平移的部分。以下代码展示了机械臂在（平移）零空间中的一种运动，如下图所示。不管机械臂如何运动，末端连杆的位置始终不动（但是姿势会改变，矩阵
mask 的作用就是滤掉转动分量，只剩下沿 轴的平移运动）。
-
q =
ConstantArray[
0, n]; dt =
0.05;
-
g[
I, L[
7],
0] =
Last@robotPartsKinematics[
{IdentityMatrix[4], q}];
-
{xl, zl, yl, xr, zr, yr} =
IdentityMatrix[
6];
-
mask = T[StackCols[xl, zl, yl]];
-
Animate[Jb = BodyJacobian[T@
{\[Xi]a, q}, g[
I, L[
7],
0]];
-
gI7 =
Last@robotPartsKinematics[
{IdentityMatrix[4], q}];
-
Jg = diagF[gToR[gI7], gToR[gI7]].Jb;
-
Jgm = mask.Jg;
-
dq =
Total[
NullSpace[Jgm]];
(*零空间的一种线性组合方式，可以改为其它线性组合*)
-
q = q + dq*dt;
-
Graphics3D[
{MapThread[move3D, {robotPartsColored, robotPartsKinematics[{IdentityMatrix[4], q}]}]
-
, move3D[frame3D, g[
I, L[
7],
0]]}],
{i, 1, 1000, 1}]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
5. 碰撞检测
我们生活的物质世界有一个法则：两个物体不能同时占据同一个空间位置，否则会有很大的力将它们分开。可是仿真是在虚拟的数字世界中进行的，这个世界可不遵守物质世界的那套力的法则，因此不够真实。为了让机器人仿真更真实，我们需要考虑“碰撞检测”（Collision Detection）。为了追求效率，工业机器人的运动速度通常比较快，而且抓着较重的负载，它一旦碰到障碍物或者人类，结果一般是“物毁人伤”。而且在一些用到规划算法中，碰撞检测也是很重要的一部分。所以在仿真时提前检测是否有碰撞很有必要。
值得一提的是，现在一些先进的机器人控制器开始配备简易的碰撞检测功能，如果在机器人工作时有人突然挡住了它，它会自动停止。这是通过检测机械臂关节处电机的电流大小实现的。当机械臂碰到人时，它相当于受到了一个阻力，电机要想保持原来的速度运行需要加大电流，灵敏的控制器会感知到电流的波动，这样我们就能通过监视电流来判断机械臂有没有发生碰撞，如果电流超过一定范围就认为机械臂发生碰撞了，需要紧急刹车。可是这种碰撞检测方法只适用于小负载（<5kg）的机械臂。因为对于重型机械臂，即便它也会停下来，可是它的惯性太大需要一段刹车距离，这足以对人造成伤害。
碰撞检测是一个比较有难度的几何问题，目前有很多成熟的算法（AABB、GJK）。我们的关注点在机器人，所以不想在碰撞检测上浪费太多时间。为此，我们使用 Mathematica 自带的 RegionDisjoint 函数实现碰撞检测。在帮助文档中，我们了解到RegionDisjoint 函数用于判断多个几何体是否相交，如果两两之间都不相交则返回 True ，而两个几何体出现了相交，就表示它们发生了碰撞（太好了，这简直是为碰撞检测量身定做的函数）。
RegionDisjoint 函数可以用于二维图形，也可以用于三维图形，甚至可以用于非凸的图形，如下面的例子所示，其中使用了Locator 控件。如果你使用了较早的软件版本，可能没有RegionDisjoint 函数，这时可以用 Graphics`Mesh`IntersectQ 代替，不过前面要加一个取反操作。
-
pts =
{{0.95, 0.31}, {0.36, -0.12}, {0.59, -0.81}, {0., -0.38}, {-0.59, -0.81}, {-0.36, -0.12}, {-0.95, 0.31}, {-0.22, 0.31}, {0., 1.}, {0.22, 0.31}, {0.95, 0.31}}
;
-
Manipulate[
-
obstacle1 = Disk[pt1,
1];
-
obstacle2 = Polygon[pt2 + # & /@ pts];
-
color =
If[RegionDisjoint[obstacle1, obstacle2], Green, Red];
-
(*!Graphics`Mesh`IntersectQ[{obstacle1,obstacle2}]*)
-
Graphics[
{EdgeForm[Black], color, obstacle1, obstacle2}, PlotRange ->
3],
{{pt1, {-1, -1}}
, Locator},
{{pt2, {1, 1}}
, Locator}, TrackedSymbols :> True]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 1
- 2
- 3
- 4
- 5
- 6
- 7
不过有了 RegionDisjoint 函数并不意味着一劳永逸。“碰撞检测”是有名的计算资源吞噬者，它会占用大量CPU资源。我们一般希望碰撞检测越快越好，可是精度和速度是一对矛盾，追求速度只能牺牲一定的精度。如果不追求很高的精度，碰撞检测应该保守一些。也就是说，在实际没发生碰撞时允许误报，但在发生碰撞时不能漏报——宁可错杀一千，不可放过一个。碰撞检测的计算量与模型的复杂程度有关。我们导入的机器人模型虽然已经经过了“瘦身”，但用于碰撞检测还是有些复杂。为此，我们需要进一步缩减。为了保守一点，我们采用比真实机械臂零件稍大些的模型，比如零件的凸包（Convex Hull）。虽然 Meshlab 软件可以制作凸包，但是效果不太好。好在 Mathematica 自带的 ConvexHullMesh 函数可以计算任意几何体的凸包。我采用的方法是先用 ConvexHullMesh 分别计算各零件的凸包，再导出零件用 Meshlab 进一步简化，最后再导入。计算零件凸包及导出所需的代码如下。（注意：由于零件数据已经是变换后的了，简化后的零件导入后不需要旋转等变换）
-
robotPartsCH = Table[
-
pts = robotParts
[[i, 1]];
-
poly = robotParts
[[i, 2, 2, 1]];
-
R = ConvexHullMesh[pts];
-
pts = MeshCoordinates[R];
-
poly = MeshCells[R,
2];
-
R = MeshRegion[pts, poly];
-
Export[
"D:\\MOTOMAN-ES165D-C" <> partsName
[[i]], R];
-
GraphicsComplex[pts, poly], {i,
7}];
-
Graphics3D[robotPartsCH]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
我们检验一下机械臂和外部障碍物的碰撞检测（至于机械臂连杆之间的碰撞我们暂时不考虑），代码如下（效果如下图所示）。
-
Robs = cuboid[
{1.3, 0, 0.5},
{0.5, 0.5, 1.0}];
(*障碍物，一个长方体*)
-
Manipulate[
-
gs = robotPartsKinematics[
{IdentityMatrix[4], {q1, q2, q3, q4, q5, q6}}];
-
Rparts =
Table[MeshRegion[ptTransform[gs[[i]]] /@ robotParts[[i,
1]], robotParts[[i,
2,
2]]],
{i, 7}];
-
bool =
And @@ (RegionDisjoint[Robs,
#] & /@ Rparts);
-
color =
If[bool,
Black,
Red]; txt =
If[bool,
"哈哈，没事",
"啊...碰撞了！"];
-
Graphics3D[
{Gray, Robs, Text[Style[txt, FontSize -> 20, FontFamily -> "黑体", FontColor -> color], {-0.5, 1, 1.5}],
{MapThread[move3D, {robotPartsColored, gs}]}}, plotOptions],
-
Grid[
{{Control[{{q1, 0}, -
Pi,
Pi,
0.1}],
Control[
{{q2, 0}, -
Pi,
Pi,
0.1}]},
{Control[{{q3, 0}, -
Pi,
Pi,
0.1}],
Control[
{{q4, 0}, -
Pi,
Pi,
0.1}]},
{Control[{{q5, 0}, -
Pi,
Pi,
0.1}],
Control[
{{q6, 0}, -
Pi,
Pi,
0.1}]}}],
TrackedSymbols :>
True]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
其中，ptTransform[g][pt3D] 函数的功能是用齐次变换矩阵 g 对三维坐标 pt3D 做变换，代码如下：
-
ptTransform[
g_][
pt3D_]:=Module[{hPt3D,transfomredPt},
-
hPt3D =
Join[pt3D,{
1.0}];
-
transfomredPt = g.hPt3D;
-
transfomredPt[[
1;;
3]] ]
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
6. 轨迹规划
轨迹规划的目的是得到机器人期望的参考运动轨迹，然后机器人控制器再跟踪这条参考轨迹完成最终的动作，它是机器人领域非常重要的一部分。机器人要干活就离不开运动，可是该如何运动呢？像搭积木、叠衣服、拧螺钉这样的动作对人类来说轻而易举，可要是让机器人来实现就非常困难。工业机器人既没有会思考的大脑，也缺少观察世界的眼睛（又瞎又傻），要让它们自己运动真是太难为它们了。它们所有的运动都是人教给它的。你可以把机器人想象成木偶，它的运动都是人灌输的。实际工厂中，是由工程师操作着控制面板，一点点调节机械臂的各个关节角度，让它到达某个位置。控制程序会记录机械臂的角度变化，只要工程师示教一次，机械臂就能精确而忠实地重复无数次。不过这种不得已而为之的方法实在是太笨了。如果有一种方法能够自动根据任务生成机器人的参考轨迹多好，下面我们将介绍一种常用的轨迹规划方法。
6.1 路径、轨迹——傻傻分不清楚
“轨迹”是什么？要理解轨迹可离不开路径。路径（Path）和轨迹（Trajectory）是两个相似的概念，它们的区别在于：
● 路径只是一堆连续空间坐标，它不随时间变化。例如下图左侧的三维曲线就是一段路径。
● 轨迹是运动的坐标，它是时间的函数，一个时刻对应一个空间坐标点。轨迹包含的信息更多，我们可以对它微分得到速度、加速度等等信息，而路径是没有这些的。下图右侧展示了两条轨迹，它们虽然经过相同的路径，但却具有不同的速度——黑色轨迹开始运动较快，随后被红色反超，最后二者又同时到达终点。
如果我们画出红色和黑色轨迹的 坐标分量，就会看到它们从同一位置出发，又在另一个位置碰头，却经历了不同的过程，如下图所示（注意红黑两组曲线的开始和结尾）。
制作上面的轨迹需要以下几个步骤：
1. 首先随机生成一些三维空间中的点。
pts = RandomReal[{-1,1},{6,3}]; (*6个三维坐标点*)
- 1
- 1
2. 然后利用 BSplineFunction 函数对点插值。
bfun = BSplineFunction[pts];
- 1
- 1
所得到的 bfun 是一个（ B 样条曲线）插值函数，它的自变量的取值范围是 0∼1，你可以用ParametricPlot3D[bfun[t], {t, 0, 1}] 画出这条曲线。
3. 二次插值。我们虽然得到了插值函数，但它是一个向量值函数，难以进一步处理（比如求积分、微分）。所以，我们需要在bfun 函数的基础上再处理。首先得到 bfun 函数图像上若干离散点（按照 0.001的间隔取）：
bfpts = bfun /@ Range[0, 1, 0.001];
- 1
- 1
然后分别对各坐标轴进行单独插值（这里我同样将自变量的取值范围设定在 0∼1 之间）：
-
nb = Length[bfpts];
-
ifunx=Interpolation[Transpose[{Range[
0,
1,
1/(nb-
1)],bfpts
[[;;,1]]}]];
-
ifuny=Interpolation[Transpose[{Range[
0,
1,
1/(nb-
1)],bfpts
[[;;,2]]}]];
-
ifunz=Interpolation[Transpose[{Range[
0,
1,
1/(nb-
1)],bfpts
[[;;,3]]}]];
- 1
- 2
- 3
- 4
- 1
- 2
- 3
- 4
并定义一个新的插值函数为各分量的合成。这样我们就人工制作了一段轨迹（或者说，是一个向量值函数）。
ifun[t_] := {ifunx[t], ifuny[t], ifunz[t]}
- 1
- 1
我们能对这段轨迹做什么呢？
● 可以计算它的弧长：
ArcLength[ifun[t], {t, 0, 1}]
- 1
- 1
● 既然可以计算弧长，就能用弧长对这条曲线重新参数化（我以前在学高等数学时，一直想不通怎么用弧长对一个曲线参数化，现在通过编程实践就很容易理解了）：
-
arcLs = Table[ArcLength[Line[bfpts
[[1 ;; i]]]], {i, Length[bfpts]}]/ArcLength[Line[bfpts]];
-
ifunArcx = Interpolation[Transpose[{arcLs, bfpts
[[;; , 1]]}]];
-
ifunArcy = Interpolation[Transpose[{arcLs, bfpts
[[;; , 2]]}]];
-
ifunArcz = Interpolation[Transpose[{arcLs, bfpts
[[;; , 3]]}]];
-
ifunArc[t_]:= {ifunArcx[t], ifunArcy[t], ifunArcz[t]}
- 1
- 2
- 3
- 4
- 5
- 1
- 2
- 3
- 4
- 5
我们可以观察两种参数化的轨迹的图像：
Animate[ParametricPlot3D[{ifun[t], ifunArc[t]}, {t, 0, end}, PlotStyle -> {{Thick, Black}, {Thin, Dashed, Red}}, PlotRange -> 1], {end, 0.01, 1, 0.01}]
- 1
- 1
我们说轨迹比路径包含更多的信息，可是如果单看路径，我们能提取出什么信息呢？
路径只包含几何信息：对于一个三维空间中的路径（曲线），我们能计算路径上每一点的切线和法线，它们刚好能唯一地确定一个直角坐标系（这个坐标系又被称为 Frenet 标架），如下图所示（对应的代码如下）。大家都知道，平面上的曲线可以用曲率描述它的弯曲程度，可是要描述三维空间曲线的弯曲程度还需要一个量，叫挠率（它是描述扭曲程度的）。如果把Frenet 标架想象成过山车，你坐在上面就能更直观地感受曲率和挠率的含义。
-
basis = Last@FrenetSerretSystem[ifun[x],x];
-
p1 = ParametricPlot3D[ifun[t],{t,
0,
1},PlotRange->
1];
-
Manipulate[pt = ifun[t];
-
tangent = Arrow[Tube[{pt,pt+(basis
[[1]]/.x->t)/
3}]];
-
normal = Arrow[Tube[{pt,pt+(basis
[[2]]/.x->t)/
3}]];
-
binormal= Arrow[Tube[{pt,pt+(basis
[[3]]/.x->t)/
3}]];
-
p2 = Graphics3D[{Arrowheads[
0.03],Red,tangent,Green,normal,Blue,binormal}];
-
Show[p1,p2],{t,
0,
1,Appearance->{
"Open"}}]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
6.2 轨迹规划方法
“轨迹规划”中的“规划”又是什么意思呢？
规划的英文是 plan，也翻译为“计划、打算”。你肯定知道“计划”是什么意思，计划就是在做事之前先想想应该怎么做才好。而且，通常你有一个要到达的目标，没有目标谈不上计划（当然一般还得有一个出发点，但这不是必需的）。假如我想放假出去玩，在制定了详细的开车路线后我连要去哪都不知道，那我是不是神经病呢。正常人都是先决定去哪，然后才选择交通线路。此外，计划还有个评价的标准——怎么样才算“好”呢？如果没有标准，那我们还计划个什么劲儿啊（反正没有好坏之分）？把目标和评价标准推广到机器人的轨迹规划领域就是：机器人怎么（运动）才能到达一个目标，而且不仅仅是到达目标，有时我们还想以最好的方式（比如最快、消耗能量最少）到达，这就是轨迹规划的任务。“轨迹规划”的叫法挺多，有叫“轨迹生成”的，有叫“运动规划”的，但不管怎么叫其实大概都是一个意思。
对于机械臂来说，轨迹规划方法可以根据有没有障碍物来划分。如果没有障碍物，那就简单些了，我们可以直接规划轨迹；如果有障碍物则一般先规划路径（因为路径包含信息更少，相对更简单），然后对路径设置速度得到轨迹（因为主要的工作都在规划路径，因此也可称其为“路径规划”）。
路径规划都有哪些方法呢？比较流行的有：图搜索、势场法、RRT 等等。下面我们来实现 RRT 方法。
RRT（快速探索随机树） 是一种通用的方法，不管什么机器人类型、不管自由度是多少、不管约束有多复杂都能用。而且它的原理很简单，这是它在机器人领域流行的主要原因之一。不过它的缺点也很明显，它得到的路径一般质量都不是很好，可能包含棱角，不够光滑，也可能远离最优。
天下武功唯快不破，“快”是 RRT 的一大优点。RRT 的思想是快速扩张一群像树一样的路径以探索（填充）空间的大部分区域，伺机找到可行的路径。之所以选择“树”是因为它能够探索空间。我们知道，阳光是树木唯一的能量来源。为了最大程度地利用阳光，树木要尽量用较少的树枝占据尽量多的空间。当然能探索空间的不一定非得是树，比如Peano曲线也可以做到，如上图左所示的例子。虽然像Peano曲线这样的也能探索空间，但是它们太“确定”了。在搜索轨迹的时候我们可不知道出路应该在哪里，如果不在“确定”的搜索方向上，我们怎么找也找不到（找到的概率是0）。这时“随机”的好处就体现出来了，虽然不知道出路在哪里，但是通过随机的反复试探还是能碰对的，而且碰对的概率随着试探次数的增多越来越大，就像买彩票一样，买的数量越多中奖的概率越大（RRT名字中“随机”的意思）。可是随机试探也讲究策略，如果我们从树中随机取一个点，然后向着随机的方向生长，那么结果是什么样的呢？见上图右。可以看到，同样是随机树，但是这棵树并没很好地探索空间，它一直在起点（红点）附近打转。这可不好，我们希望树尽量经济地、均匀地探索空间，尽量不过度探索一个地方，也不能漏掉大部分地方。这样的一棵树怎么构造呢？
RRT 的基本步骤是：
1. 起点作为一颗种子，从它开始生长枝丫；
2. 在机器人的“构型”空间中，生成一个随机点 ；
3. 在树上找到距离 最近的那个点，记为 吧；
4. 朝着 的方向生长，如果没有碰到障碍物就把生长后的树枝和端点添加到树上，返回 2；
随机点一般是均匀分布的，所以没有障碍物时树会均匀地向各个方向生长，这样可以快速探索空间（RRT名字中“快速探索”的意思），如下图所示。当然如果你事先掌握了最有可能发现路径的区域信息，可以集中兵力重点探索这个区域，这时就不宜用均匀分布了。
RRT 的一个弱点是难以在有狭窄通道的环境找到路径。因为狭窄通道面积小，被碰到的概率低，找到路径需要的时间要看运气了。下图展示的例子是 RRT 应对一个人为制作的狭窄通道，有时RRT很快就找到了出路，有时则一直被困在障碍物里面。对应的代码如下（这段代码只用于演示 RRT 的原理，不是正式代码，但它有助于理解正式代码的运算过程）：
-
(*RRT示例：此段程序不依赖任何自定义函数，可独立运行。这是我能想到的最短的RRT演示代码了*)
-
step =
3; maxIters =
2000; start =
{50, 50}; range =
{0, 100};
-
pts =
{start}; lines =
{{start, start}};
-
obstaclePts =
{{20, 1},
{25, 1},
{25, 25},
{-25, 25},
{-25, -25},
{25, -25},
{25, -1},
{20, -1},
{20, -20},
{-20, -20},
{-20, 20},
{20, 20}} +
50;
-
Do[randomPt =
RandomReal[range,
2];
-
{nearestPt} =
Nearest[pts, randomPt,
1];
-
grownPt = nearestPt + step*
Normalize[randomPt - nearestPt];
-
If[!
Graphics`
Mesh`IntersectQ[
{Line[{nearestPt, grownPt}],
Polygon[obstaclePts]}],
-
AppendTo[lines,
{nearestPt, grownPt}];
-
AppendTo[pts, grownPt]],
{maxIters}];
-
Animate[
Graphics[
{Polygon[obstaclePts], Line[lines[[1 ;; i]]], Blue, PointSize[0.004], Point[pts[[1 ;; i]]], Red, Disk[start, 0.7]},
PlotRange ->
{range, range}],
{i, 1, Length[pts] - 1, 1}]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
RRT探索空间的能力还是不错的，例如下图左所示的例子，障碍物多而且杂乱（实现这个例子所需的所有代码不会超过30行）。还有没有环境能难住RRT呢？下图右所示的迷宫对RRT就是个挑战。这个时候空间被分割得非常严重，RRT显得有些力不从心了，可见随机策略不是什么时候都有效的。
“随机”使得RRT有很强的探索能力。但是成也萧何败也萧何，“随机”也导致 RRT 很盲目，像个无头苍蝇一样到处乱撞。一个改进的办法就是给它一双“慧眼”（慧眼代表信息）。在势场法中，势函数携带了障碍物和目标的信息，如果能把这个信息告诉 RRT ，让它在探索空间时有倾向地沿着势场的方向前进会更好。这样，RRT 出色的探索能力刚好可以弥补势场法容易陷入局部极小值的缺点。
将RRT方法用在机械臂上的效果如下图所示（绿色表示目标状态）。我设置了4个障碍物（其中一个是大地），这对机械臂是个小小的挑战。由于我们生活在三维空间，没办法看到6维关节空间，所以我把6维关节空间拆成了2个三维空间，分别对应前三个关节和后三个关节：
nodes 和树枝列表 edges 组成，即 tree = {nodes, edges}。其中节点列表 nodes 存储树中所有节点（每个节点都是一个 6 维向量，表示机械臂的关节值），树枝列表 edges 中存储所有树枝，树枝定义为两个节点的代号（节点的代号定义为节点被添加到树的顺序，比如添加新节点时树中已经有4个节点了，那么新节点的代号就是 5）。
-
obsCenters =
{{0,0,-0.15},
{1.4,-0.8,0.5},
{1,1,0.7},
{0.5,0,2.3}};
-
obsDims =
{{8,8,0.2},
{0.5,0.8,1.0},
{0.7,0.3,1.4},
{3,0.1,0.9}};
-
Robs =
MapThread[cuboid,
{obsCenters, obsDims}];
(*定义4个长方体障碍物*)
-
step =
0.2;
(*树枝每次生长的长度，这里简单设置为固定值*)
-
q0 =
{-1.54, 0.76, 0.66, -1.14, -1.44, 0};
(*起点*)
-
qt =
{1.76, 0.76, 0.46, 0, 0.36, 0};
(*目标点*)
-
nodes =