基于 Mathematica 的机器人仿真环境（机械臂篇）[转]

完美的教程，没有之一，收藏学习。

目的 

　　本文手把手教你在 Mathematica 软件中搭建机器人的仿真环境，具体包括如下内容（所使用的版本是 Mathematica 11.1，更早的版本可能缺乏某些函数，因此请使用最新版。robinvista2@gmail.com）。 
　　1　导入机械臂的三维模型 
　　2　（正/逆）运动学仿真 
　　3　碰撞检测 
　　4　轨迹规划 
　　5　（正/逆）动力学仿真 
　　6　控制方法的验证 
　　不妨先看几个例子： 

　　　　　　　逆运动学　　　　　　　　　　　　　　　　双臂协做搬运 

　　　　　　　显示运动痕迹　　　　　　　　　　　（平移）零空间运动 
　　不管你是从事机器人研发仍是教学科研，一款好用的仿真软件能对你的工做起到很大的帮助。那么应该选择哪一种仿真软件呢？最方便的选择就是现成的商业软件（例如 Webots、Adams）。你的钱不是白花的，商业软件功能完善又稳定，并且性能通常都通过了优化。但是再强大的商业软件也没法面面俱到。从学习和研究的角度出发，最好选择代码可修改的开源或半开源软件。 
　　在论文数据库中简单检索一下就会发现，不少人都选择在 Matlab 的基础上搭建仿真环境。这并不奇怪，由于 Matlab 具备优秀的数值计算和仿真能力，使用它开发会很便利。若是你非要舍近求远，用 C++ 编写一套仿真软件，先不要说仿真结果如何显示，光是矩阵计算的实现细节就能让你焦头烂额（原本咱们要造一辆汽车，但是制做车轮就耗费了大量的精力，而实际上车轮直接买现成的就好了）。 

　　与大名鼎鼎的 Matlab 相比，Mathematica 是一款知名度不高的软件。可是不要小看它哦，我简单对比了一下两者在机器人仿真方面的特色，见下表。因为 Mathematica 不俗的表现，我选择在它的基础上搭建仿真环境。若是你对 Mathematica 不熟悉，能够看网络教程，也能够参考个人学习笔记入门（点击这里查看）。本文面向 Mathematica 的初学者，因此避免使用过于高超的编程技巧。

 

	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 文件夹下）：

1.  
   SetDirectory[ "D:\\MOTOMAN-ES165D"]; (*设置文件的存储位置，注意双斜杠*)
2.  
   n = 6; (* n 是机械臂的自由度，后面还会用到*)
3.  
   partsName = { "1.stl", "2.stl", "3.stl", "4.stl", "5.stl", "6.stl", "7.stl", "8.stl", "9.stl"}; (*组成机械臂的9个零件*)
4.  
   robotPartsGraphics = Import[ #, "Graphics3D"] & /@ partsName; (*一次性导入全部零件，而且导入为直接能够显示的图形格式*)
5.  
   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 函数包裹着的数字，稍加分辨会发现这些数字又包含两部分：第一部分是零件全部顶点的三维坐标；第二部分是组成零件外形的三角形（构成每一个三角形的三个顶点是第一部分点的序号，而不是坐标）。咱们能够用如下代码将其分别显示出来：

1.  
   pts = robotParts[[ 1, 1]]; (*第一部分：顶点的三维坐标数据*)
2.  
   triangles = robotParts[[ 1, 2]]; (*第二部分：三角形面片*)
3.  
   trianglesB = triangles /. {EdgeForm[] -> EdgeForm[Blue]}; (*三角形的边显示为蓝色 Blue*)
4.  
   Graphics3D[ {Red, Point[pts], White, GraphicsComplex[pts, trianglesB]}]

• 1
• 2
• 3
• 4

• 1
• 2
• 3
• 4

 

　　机器人的全部零件都成功地导入了，并且它们的相对位置也是正确的。你可能会问：机械臂为何是处于“躺着”的姿式呢？这是因为零件是按照 SolidWorks 默认的坐标系（ 轴向上）绘制和装配的。而在 Mathematica 中默认的坐标系是  轴向上。那么咱们应该采用哪一个坐标系呢？ 
　　固然你能够任性而为，用哪一个均可以。不过根据国家标准《GBT 16977-2005 工业机器人 坐标系和运动命名原则》，基座坐标系的  轴应该垂直于机器人基座安装面（也就是地面）、朝向为重力加速度的反方向，  轴指向机器人工做空间中心点。制定国标的都是些经验丰富的专家老手，咱们最好跟国标保持一致（国标的做图水平就不能提升点吗？这图怎么感受像小学生画的）。 

　

　　为了让机器人变成国标规定的姿式，须要旋转各个零件。咱们先想一想应该怎么转：结合咱们以前导入的图形，能够先绕全局坐标系的  轴转 ，再绕全局坐标系的  轴转 。另外一种方法是：先绕全局坐标系的  轴转 （记这个旋转后的坐标系为 ），再绕  的  轴转 。两种方法的效果是同样的，可是注意合成矩阵时乘法的顺序（见如下代码），不懂的同窗能够看看文献中的3133页。固然，转动是有正负之分的：将你的右手握住某个坐标轴，竖起大拇哥，让大拇指和轴的正方向一致，这时四指所示的方向就是绕该轴转动的正方向。 
　　为此，定义旋转矩阵：

 

1.  
   Xaxis = {1, 0, 0}; Yaxis = {0, 1, 0}; Zaxis = {0, 0, 1}; (*定义旋转轴，更简洁的写法是： {Xaxis,Yaxis,Zaxis}=IdentityMatrix[3];*)
2.  
   rot = RotationMatrix[ 90 Degree, Zaxis].RotationMatrix[90 Degree, Xaxis]; (*注意第二次变换是在左边乘*)
3.  
   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个零件一个一个地设置颜色，而是把相同的元素（颜色）写在一块儿，这样作的好处就是代码比较简洁、清晰。之后咱们会常常这么作。

1.  
   colors = {Gray, Cyan, Orange, Yellow, Gray, Green, Magenta, Lighter[Green], Pink}; (*1~9 各零件的颜色*)
2.  
   robotPartsColored = Transpose[ {colors, robotParts}]; (*把颜色分配给各零件*)
3.  
   Graphics3D[robotPartsColored]

• 1
• 2
• 3

• 1
• 2
• 3

 

说明：如今的机器人姿式（大臂竖直、小臂前伸）是6自由度机械臂的“零位”状态，咱们将此时机械臂各关节的角度认为是0。通常机械臂上都有各关节的零点位置标记，用于指示各关节的零点。咱们用控制器控制机械臂时，发出的角度指令都是相对于这个零点位置的。零点位置不是必须遵照的，你能够将任意的角度设置为零位，不过为了统一，最好用机械臂固有的零位——也就是当前的姿式。

 

3. 运动学仿真

　　前面的工做只是让机械臂的模型显示出来，若是你想让机器人动起来，那就要考虑运动学了。机器人听起来高大上，可实际上如今大多数工业机器人的控制方式仍是比较低级的，它们只用到了运动学，高级一点的动力学不多用，更不要提智能了（它们要说本身有智能，咱们家的洗衣机和电视机都不服）。有的公司（例如倍福），更是将支持不一样类型的机械臂的运动学做为宣传的噱头。看来要使用机器人，运动学是必不可少的，因此咱们先来实现运动学。

　　在创建运动学模型以前咱们须要了解机器人的机械结构。前面提到，MOTOMAN-ES165D 是一个6自由度的串联机械臂。而6个自由度的机器人至少由7个连杆组成（其中要有一个连杆与大地固定，也就是基座）。但是咱们导入的零件有9个，多出来的2个零件是弹簧缸（基座上黄色的圆筒）的组成部分。MOTOMAN-ES165D 机器人可以抓持的最大负载是165公斤，弹簧缸的做用就是平衡掉一部分负载的重量，要否则前端的关节电机会有很大的负担。但是弹簧缸给咱们的建模形成了麻烦，由于它致使存在“闭链”，这不太好处理。为此，咱们先忽略掉弹簧缸。 
　　 
3.1 零件的局部坐标系

 

　　机器人的运动也就是其构成连杆（零件）的运动。而为了描述连杆的运动，咱们要描述每一个连杆的位置和姿态（合称为“位姿”）。一般的作法是在每一个连杆上固定一个坐标系（它跟随连杆一块儿运动），这个坐标系称为“局部坐标系”。经过描述局部坐标系的位姿咱们就能够描述每一个连杆的位姿。如何选择局部坐标系呢？理论上你能够任意选择，不过局部坐标系影响后续编程和计算的难易程度，因此咱们在选择时最好慎重。在运动学建模和动力学建模中，坐标系的选择一般是不一样的。 
　　●　运动学建模时，连杆的局部坐标系通常放置在关节处，这是由于经常使用的 D-H 参数是根据相邻关节轴定义的。 
　　●　动力学建模时，连杆的局部坐标系通常放置在质心处，这是由于牛顿方程是关于质心创建的，并且关于质心的转动惯量是常数，这方便了计算。 
　　咱们先考虑运动学，所以将局部坐标系设置在关节处。在 SolidWorks 中打开任何一个零件，都能看到它本身有一个坐标系。构成一个零件的每一条边、每个孔的数据都以这个坐标系为参考，咱们称它为“绘图坐标系”。绘图坐标系一般不在质心处，由于在你还没画以前你根本不知道质心在哪里。绘图坐标系一般在零件的对称中心或者关节处，咱们不妨将每一个零件的绘图坐标系当作它的局部坐标系。 
　　那么下一个问题是每一个零件的绘图坐标系在哪儿呢？咱们以第三个零件为例，以下图左所示。咱们点击左侧的“原点”标签，图中就会显示绘图坐标系的原点。（若是你想将绘图坐标系显示出来，能够先选中“原点”标签，而后点击上方菜单栏中的“参考几何体”，再选择“坐标系”，而后直接回车便可看到新建的绘图坐标系，如右图，可见它位于一个关节轴）

　　而后回到机器人的装配体中，在左侧的零件树中展开每一个零件找到并选中其绘图坐标系的原点，而后点击上方菜单栏“评估”中的“测量”便可看到图中出现了一组坐标值（以下图所示），这就是零件绘图坐标系的原点在全局坐标系（本文将全局坐标系定义为装配体的坐标系）中的位置。 

　　咱们记录下全部零件的绘图坐标系的原点位置（除去弹簧缸的2个，注意 SolidWorks 中默认的单位是毫米，这里除以 1000 是为了变换到 Mathematica 中使用的标准单位——米）：

 

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

　　还有印象吗？最开始导出和导入零件模型时，各零件的位置都已经按照装配关系肯定好了，因此它们的数据也是相对于全局坐标系描述的。但是如今咱们要让机械臂动起来（并且还要显示出来），这就要移动这些数据。为了方便起见，最好能将每一个零件的模型数据表示在本身的绘图坐标系中，由于这样咱们只须要移动绘图坐标系就好了，而各点的数据相对它们所属的绘图坐标系是不动的。应该怎么作呢？很简单，将零件模型的数据减去绘图坐标系的原点在全局坐标系中的坐标便可：

1.  
   partsName = { "1.stl", "2.stl", "3.stl", "6.stl", "7.stl", "8.stl", "9.stl"};　(*已经去除了弹簧缸的2个零件：4号和5号*)
2.  
   robotPartsGraphics = Import[ #, "Graphics3D"] & /@ partsName;
3.  
   robotParts = robotPartsGraphics [[;; , 1]];
4.  
   robotParts = Table[GraphicsComplex[rot\[CircleDot]robotParts [[i, 1]], robotParts[[i, 2]]], {i, 7}];
5.  
   robotParts = Table[GraphicsComplex[( # - drawInGlobal[[i]]) & /@ robotParts[[i, 1]], robotParts[[i, 2]]], {i, 7}];
6.  
   colors = {Gray, Cyan, Orange, Green, Magenta, Yellow, Pink}; (*从新定义零件的颜色*)
7.  
   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 上方菜单栏的“参考几何体”，选择“点”，而后在左侧面板选择“圆弧中心”，而后选择图中的关节轴周围的任意同心圆弧便可建立一个参考点，这个点就是咱们想要的。咱们能够在零件视图中测量这个点的坐标，也能够在机器人完整装配体中测量，这里我选择后者。（测量步骤参照前面测量“零件绘图坐标系的原点”）

　　定义关节旋量的代码以下。其中相对旋量  用于递归运动学计算，它的含义是当前连杆的转轴表示在前一个连杆坐标系中。

 

1.  
   axesPtInGlobal = rot\[CircleDot]{{ 0, 257, 0}, {-88, 650, 285}, {-280.86, 1800, 285}, {0, 2050, 1318}, {134, 2050, 1510}, {0, 2050, 1720.5}}/1000;
2.  
   axesPtInDraw = axesPtInGlobal - drawInGlobal [[1 ;; -2]];
3.  
   axesDirInDraw = axesDirInGlobal = {Zaxis, Yaxis, Yaxis, Xaxis, Yaxis, Xaxis};
4.  
   \[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 函数将旋转矩阵生成对应的齐次变换矩阵，代码以下：

1.  
   RToH[R_]:= RPToH[R, {0,0,0}]
2.  
   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 开头的变量）表示一个（或一组）齐次变换矩阵，这是约定俗成的。 
　　如今能够正式推导机械臂的运动学模型了。在使用机械臂时，你们通常只关心其最末端连杆的位姿，更确切的说，是最末端连杆的位姿与关节角度的关系。不过为了获得最末端连杆的位姿，咱们须要计算中间全部连杆的位姿。这里利用相邻连杆的递归关系——每一个连杆的位姿依赖前一个连杆的位姿——来提高计算效率。因此，能够定义机械臂全部连杆的运动学函数为：

1.  
   robotPartsKinematics[configuration _] := Module[{q, g2To7},
2.  
   　　 {g[I, L[ 1]], q} = configuration;
3.  
   　　 g2To7 = Table[g[L[i], L[i + 1]] = TwistExp[\[Xi]r[[i]], q[[i]]].g[L[i], L[i + 1], 0];
4.  
   　　 g[I, L[i + 1]] = g[I, L[i]].g[L[i], L[i + 1]], {i, n}];
5.  
   　　 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 函数，能够直接改变机械臂的各关节角度，并直观地查看机械臂姿式（应该叫构型了哦）的变化，如如下动画所示。能够看到，机械臂各连杆的运动符合咱们设置的关节值，这说明运动学模型是正确的。 

 

1.  
   Manipulate[qs = {##}[[;; , 1, 1]];
2.  
   gs = robotPartsKinematics[ {IdentityMatrix[4], qs}];
3.  
   Graphics3D[{MapThread[move3D, {robotPartsColored, gs}]}
4.  
   , 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

1.  
   object = cuboid[{0, 0, 0}, {0.3, 0.2, 0.05}];
2.  
   Manipulate[
3.  
   gt = RPToH[RollPitchYawMatrix[{\[Alpha], \[Beta], \[Gamma]}], {x, y, z}];
4.  
   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个）的个数，这是由于  旋转矩阵的各项不是独立的，所以要舍去一部分。该保留哪三项呢？只要不选择同一行或同一列的三项就能够了，这里我保留了三项。

 

1.  
   Manipulate[
2.  
   gts = Last@robotPartsKinematics[{IdentityMatrix[ 4], {q1, q2, q3, q4, q5, q6}}];
3.  
   gt = RPToH[RollPitchYawMatrix[{\[Alpha], \[Beta], \[Gamma]}], {x, y, z}];
4.  
   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}]];
5.  
   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}}]};
6.  
   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}}]};
7.  
   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}}],
8.  
   Grid[{{Control[{{ x, 1.3}, -2, 3, 0.1}], Control[{{y, 0}, -2, 2, 0.1}]},
9.  
   {Control[{{z, 2}, 0, 3, 0.1}], Control[{{\[Alpha], 0}, 0, Pi, 0.1}]},
10.  
    {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页），代码以下：

1.  
   forwardKinematicsJacobian[argList_, gst0_] :=
2.  
   Module[{g = IdentityMatrix[4], \[Xi], n = Length[argList]},
3.  
   Js = {}; (*注意空间雅可比矩阵Js是全局变量，后面会用*)
4.  
   Do[\[Xi] = Ad[g].argList[[i, 1]];
5.  
   Js = Join[Js, {\[Xi]}];
6.  
   g = g.TwistExp[argList[[i, 1]], argList[[i, 2]]]
7.  
   , {i, n}];
8.  
   Js = Transpose[Js];
9.  
   g.gst0 ]
10.  
    \[Xi]a = Table[\[Omega]a[i] = axesDirInGlobal[[i]]; la[i] = axesPtInGlobal[[i]]; Join[Cross[-\[Omega]a[i], la[i]], \[Omega]a[i]], {i, n}];
11.  
    (*forwardKinematicsJacobian函数是从 Screws.m 中抄的，它使用了表示在全局坐标系的旋量，所以定义\[Xi]a*)
12.  
    inverseKinematics[gt_, q0_, errorthreshold_: 0.0001] :=
13.  
    Module[{gReal, q = q0, Jb, Jg, F, error, theta, axis, positionerror, angleerror, maxIter = 20},(*输入指望的机械臂末端位姿 gt 和初始关节角 q0*)
14.  
    Do[gReal = forwardKinematicsJacobian[Transpose@{\[Xi]a, q}, g[I, L[7], 0]];
15.  
    Jb = Ad[Inverse[gReal]].Js;
16.  
    Jg = diagF[gToR[gReal], gToR[gReal]].Jb;
17.  
    positionerror = gToP[gt - gReal];
18.  
    angleerror = Reverse@RollPitchYawAngles[gToR[gt.Inverse[gReal]]]; (*注意Reverse函数*)
19.  
    error = Flatten[N[ {positionerror, angleerror}]]; (*偏差向量 error 包括位置和角度份量在全局坐标系中表示*)
20.  
    F = PseudoInverse[Jg].error;
21.  
    q = q + modToPiPi[F];
22.  
    If[Norm[error] < errorthreshold, Break[]]
23.  
    , {maxIter}];
24.  
    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 函数呢？由于角度是个小妖精，若是咱们不盯紧它，它可能会时不时的捣乱。从外部看，机械臂的一个转动关节位于角度  和角度  没什么区别。但是若是咱们听任角度这样随意跳变，会致使轨迹不连续，这样机械臂在跟踪轨迹时就会出现麻烦。

1.  
   modToPiPi[angle_]:= Module[{a = Mod[angle,2.0*Pi]}, If[Re[a]>=Pi, a-2.0*Pi, a]]
2.  
   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），代码以下。

1.  
   gToR[g_]:= Module[{n=(Dimensions[g]) [[1]]-1}, g[[1;;n,1;;n]]]
2.  
   gToP[g_]:= Module[{n=(Dimensions[g]) [[1]]-1}, g[[1;;n,n+1]]]

• 1
• 2

• 1
• 2

　　咱们之后会用到不少矩阵操做（好比转置、求逆）。而 Mathematica 的函数名太长，为了写起来方便，我定义了简写的转置和求逆函数，代码以下：

1.  
   T[g_]:= Transpose[g]
2.  
   Iv[g_]:= Inverse[g]

• 1
• 2

• 1
• 2

　　咱们想让机械臂（的末端）依次到达一些空间点（这些点多是机械臂运动时要通过的）。为此首先生成一些三维空间中的点：

1.  
   Clear[x,y];
2.  
   pts2D = Table[ {Sin[i], Cos[i]}/1.4, {i, 0, 4 Pi, Pi/400}]; (*先生成二维平面上的点，它们均匀地分布在一个圆上*)
3.  
   pts3D = pts2D /. {x_, y_} -> {1.721, x, y + 1.4}; (*再将二维坐标变换成三维坐标*)
4.  
   Graphics3D[Point[pts3D]]

• 1
• 2
• 3
• 4

• 1
• 2
• 3
• 4

　　而后调用逆运动学函数 inverseKinematics 挨个计算不一样点处的关节值，代码以下：

1.  
   gStars = PToH /@ pts3D; (*将三维点的坐标转换成齐次变换矩阵，转动部分始终不变*)
2.  
   q = ConstantArray[ 0, n]; (*inverseKinematics函数包含一个迭代过程，所以须要提供一个初始值*)
3.  
   g[I, L[ 7], 0] = (robotPartsKinematics[{IdentityMatrix[4], q}]; g[I, L[7]]); (*forwardKinematicsJacobian函数须要零位状态下的末端连杆位姿*)
4.  
   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个关节的插值函数求导从而获得关节速度和加速度函数：

1.  
   time = 10; (*time是本身定义的，表示机械臂运动通过全部点的总时间*)
2.  
   Do[qt[i] = T@{Range[0, time, time/(Length[(T@qs)[[i]]] - 1)], (T@qs)[[i]]}, {i, n}];
3.  
   Do[qfun[i] = Interpolation[qt[i]];
4.  
   dqfun[ i][x_] = D[qfun[i][x], x];
5.  
   ddqfun[ i][x_] = D[dqfun[i][x], x], {i, n}]

• 1
• 2
• 3
• 4
• 5

• 1
• 2
• 3
• 4
• 5

　　画出插值后各关节的角度、角速度、角加速度的变化趋势，以下图。能看到有两个关节角速度变化剧烈。理论上说，这个曲线不适合让机器人跟踪。

1.  
   pq = Plot[ Evaluate@Table[qfun[i][x], {i, 6}], {x, 0, time}, PlotRange -> All];
2.  
   pdq = Plot[ Evaluate@Table[dqfun[i][x], {i, n}], {x, 0, time}, PlotRange -> All];
3.  
   pddq = Plot[ Evaluate@Table[ddqfun[i][x], {i, n}], {x, 0, time}, PlotRange -> All];
4.  
   GraphicsGrid[{{pq, pdq, pddq}}]

• 1
• 2
• 3
• 4

• 1
• 2
• 3
• 4

 

4.3 雅克比矩阵的零空间

 

　　在上一节求解逆运动学问题时咱们使用了机械臂的雅克比矩阵。雅克比矩阵可以将关节速度映射到末端连杆的速度。因为末端连杆的速度有不止一种定义方式（例若有：空间速度、本体速度、全局速度，它们的定义见个人另外一篇博客），因此对应了不一样的雅克比形式（也就是逆运动学函数中的 js、Jb、Jg）。 
　　雅克比矩阵有一些有趣的性质，其中一个比较有意思的是它的零空间。只要关节速度在（雅克比矩阵的）零空间中，那末端连杆的速度老是零（零空间由此得名）。通俗的说就是：无论关节怎么动，末端连杆始终不动（就像被钉死了同样）。这个性质还挺有用的，由于有些场合要求机械臂在抓取东西的时候还能躲避障碍物。在其它领域，例如摄影，为了保证画面稳定须要摄像机能防抖动；在动物王国中，动物觅食时头部要紧盯猎物（被恶搞的稳定鸡）；在军事领域（例如坦克、武装直升机），要求炮口始终瞄准目标，无论车身如何移动和颠簸。 

 

　　对于本文中的 6 自由度机械臂，因为它不是冗余的，因此大多数时候计算零空间会获得空（说明不存在零空间）。我为了展现零空间的效果只用了平移的部分。如下代码展现了机械臂在（平移）零空间中的一种运动，以下图所示。无论机械臂如何运动，末端连杆的位置始终不动（可是姿式会改变，矩阵mask 的做用就是滤掉转动份量，只剩下沿  轴的平移运动）。

 

1.  
   q = ConstantArray[0, n]; dt = 0.05;
2.  
   g[ I, L[7], 0] = Last@robotPartsKinematics[{IdentityMatrix[4], q}];
3.  
   {xl, zl, yl, xr, zr, yr} = IdentityMatrix[6];
4.  
   mask = T[StackCols[xl, zl, yl]];
5.  
   Animate[Jb = BodyJacobian[T@{\[Xi]a, q}, g[I, L[7], 0]];
6.  
   gI7 = Last@robotPartsKinematics[{IdentityMatrix[4], q}];
7.  
   Jg = diagF[gToR[gI7], gToR[gI7]].Jb;
8.  
   Jgm = mask.Jg;
9.  
   dq = Total[NullSpace[Jgm]]; (*零空间的一种线性组合方式，能够改成其它线性组合*)
10.  
    q = q + dq*dt;
11.  
    Graphics3D[{MapThread[move3D, {robotPartsColored, robotPartsKinematics[{IdentityMatrix[4], q}]}]
12.  
    , 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 代替，不过前面要加一个取反操做。 

 

1.  
   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}};
2.  
   Manipulate[
3.  
   obstacle1 = Disk[pt1, 1];
4.  
   obstacle2 = Polygon[pt2 + # & /@ pts];
5.  
   color = If[RegionDisjoint[obstacle1, obstacle2], Green, Red];
6.  
   (*!Graphics`Mesh`IntersectQ[{obstacle1,obstacle2}]*)
7.  
   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 进一步简化，最后再导入。计算零件凸包及导出所需的代码以下。（注意：因为零件数据已是变换后的了，简化后的零件导入后不须要旋转等变换）

1.  
   robotPartsCH = Table[
2.  
   pts = robotParts [[i, 1]];
3.  
   poly = robotParts [[i, 2, 2, 1]];
4.  
   R = ConvexHullMesh[pts];
5.  
   pts = MeshCoordinates[R];
6.  
   poly = MeshCells[R, 2];
7.  
   R = MeshRegion[pts, poly];
8.  
   Export[ "D:\\MOTOMAN-ES165D-C" <> partsName[[i]], R];
9.  
   GraphicsComplex[pts, poly], {i, 7}];
10.  
    Graphics3D[robotPartsCH]

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10

　　咱们检验一下机械臂和外部障碍物的碰撞检测（至于机械臂连杆之间的碰撞咱们暂时不考虑），代码以下（效果以下图所示）。

1.  
   Robs = cuboid[ {1.3, 0, 0.5}, {0.5, 0.5, 1.0}]; (*障碍物，一个长方体*)
2.  
   Manipulate[
3.  
   gs = robotPartsKinematics[ {IdentityMatrix[4], {q1, q2, q3, q4, q5, q6}}];
4.  
   Rparts = Table[MeshRegion[ptTransform[gs[[i]]] /@ robotParts[[i, 1]], robotParts[[i, 2, 2]]], {i, 7}];
5.  
   bool = And @@ (RegionDisjoint[Robs, #] & /@ Rparts);
6.  
   color = If[bool, Black, Red]; txt = If[bool, "哈哈，没事", "啊...碰撞了！"];
7.  
   Graphics3D[{Gray, Robs, Text[Style[txt, FontSize -> 20, FontFamily -> "黑体", FontColor -> color], {-0.5, 1, 1.5}], {MapThread[move3D, {robotPartsColored, gs}]}}, plotOptions],
8.  
   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 作变换，代码以下：

1.  
   ptTransform[ g_][pt3D_]:=Module[{hPt3D,transfomredPt},
2.  
   hPt3D = Join[pt3D,{1.0}];
3.  
   transfomredPt = g.hPt3D;
4.  
   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 之间）：

1.  
   nb = Length[bfpts];
2.  
   ifunx=Interpolation[Transpose[{Range[ 0,1,1/(nb-1)],bfpts[[;;,1]]}]];
3.  
   ifuny=Interpolation[Transpose[{Range[ 0,1,1/(nb-1)],bfpts[[;;,2]]}]];
4.  
   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

　　●　既然能够计算弧长，就能用弧长对这条曲线从新参数化（我之前在学高等数学时，一直想不通怎么用弧长对一个曲线参数化，如今经过编程实践就很容易理解了）：

1.  
   arcLs = Table[ArcLength[Line[bfpts [[1 ;; i]]]], {i, Length[bfpts]}]/ArcLength[Line[bfpts]];
2.  
   ifunArcx = Interpolation[Transpose[{arcLs, bfpts [[;; , 1]]}]];
3.  
   ifunArcy = Interpolation[Transpose[{arcLs, bfpts [[;; , 2]]}]];
4.  
   ifunArcz = Interpolation[Transpose[{arcLs, bfpts [[;; , 3]]}]];
5.  
   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 标架想象成过山车，你坐在上面就能更直观地感觉曲率和挠率的含义。 

 

1.  
   basis = Last@FrenetSerretSystem[ifun[x],x];
2.  
   p1 = ParametricPlot3D[ifun[t],{t, 0,1},PlotRange->1];
3.  
   Manipulate[pt = ifun[t];
4.  
   tangent = Arrow[Tube[{pt,pt+(basis [[1]]/.x->t)/3}]];
5.  
   normal = Arrow[Tube[{pt,pt+(basis [[2]]/.x->t)/3}]];
6.  
   binormal= Arrow[Tube[{pt,pt+(basis [[3]]/.x->t)/3}]];
7.  
   p2 = Graphics3D[{Arrowheads[ 0.03],Red,tangent,Green,normal,Blue,binormal}];
8.  
   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 的一大优势。RRT 的思想是快速扩张一群像树同样的路径以探索（填充）空间的大部分区域，乘机找到可行的路径。之因此选择“树”是由于它可以探索空间。咱们知道，阳光是树木惟一的能量来源。为了最大程度地利用阳光，树木要尽可能用较少的树枝占据尽可能多的空间。固然能探索空间的不必定非得是树，好比Peano曲线也能够作到，如上图左所示的例子。虽然像Peano曲线这样的也能探索空间，可是它们太“肯定”了。在搜索轨迹的时候咱们可不知道出路应该在哪里，若是不在“肯定”的搜索方向上，咱们怎么找也找不到（找到的几率是0）。这时“随机”的好处就体现出来了，虽然不知道出路在哪里，可是经过随机的反复试探仍是能碰对的，并且碰对的几率随着试探次数的增多愈来愈大，就像买彩票同样，买的数量越多中奖的几率越大（RRT名字中“随机”的意思）。但是随机试探也讲究策略，若是咱们从树中随机取一个点，而后向着随机的方向生长，那么结果是什么样的呢？见上图右。能够看到，一样是随机树，可是这棵树并没很好地探索空间，它一直在起点（红点）附近打转。这可很差，咱们但愿树尽可能经济地、均匀地探索空间，尽可能不过分探索一个地方，也不能漏掉大部分地方。这样的一棵树怎么构造呢？ 
　　RRT 的基本步骤是： 
　　1. 起点做为一颗种子，从它开始生长枝丫； 
　　2. 在机器人的“构型”空间中，生成一个随机点 ； 
　　3. 在树上找到距离  最近的那个点，记为  吧； 
　　4.  朝着  的方向生长，若是没有碰到障碍物就把生长后的树枝和端点添加到树上，返回 2； 
　　随机点通常是均匀分布的，因此没有障碍物时树会均匀地向各个方向生长，这样能够快速探索空间（RRT名字中“快速探索”的意思），以下图所示。固然若是你事先掌握了最有可能发现路径的区域信息，能够集中兵力重点探索这个区域，这时就不宜用均匀分布了。 
　　RRT 的一个弱点是难以在有狭窄通道的环境找到路径。由于狭窄通道面积小，被碰到的几率低，找到路径须要的时间要看运气了。下图展现的例子是 RRT 应对一我的为制做的狭窄通道，有时RRT很快就找到了出路，有时则一直被困在障碍物里面。对应的代码以下（这段代码只用于演示 RRT 的原理，不是正式代码，但它有助于理解正式代码的运算过程）： 

　

 

1.  
   (*RRT示例：此段程序不依赖任何自定义函数，可独立运行。这是我能想到的最短的RRT演示代码了*)
2.  
   step = 3; maxIters = 2000; start = {50, 50}; range = {0, 100};
3.  
   pts = {start}; lines = {{start, start}};
4.  
   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;
5.  
   Do[randomPt = RandomReal[range, 2];
6.  
   {nearestPt} = Nearest[pts, randomPt, 1];
7.  
   grownPt = nearestPt + step* Normalize[randomPt - nearestPt];
8.  
   If[!Graphics`Mesh`IntersectQ[{Line[{nearestPt, grownPt}], Polygon[obstaclePts]}],
9.  
   　　　 AppendTo[lines, {nearestPt, grownPt}];
10.  
    　　　 AppendTo[pts, grownPt]], {maxIters}];
11.  
    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个三维空间，分别对应前三个关节和后三个关节： 

　　正式 RRT 的主程序代码以下。我将 RRT 树定义为由节点列表 nodes 和树枝列表 edges 组成，即 tree = {nodes, edges}。其中节点列表 nodes 存储树中全部节点（每一个节点都是一个 6 维向量，表示机械臂的关节值），树枝列表 edges 中存储全部树枝，树枝定义为两个节点的代号（节点的代号定义为节点被添加到树的顺序，好比添加新节点时树中已经有4个节点了，那么新节点的代号就是 5）。

 

1.  
   obsCenters = {{0,0,-0.15},{1.4,-0.8,0.5},{1,1,0.7},{0.5,0,2.3}};
2.  
   obsDims = {{8,8,0.2},{0.5,0.8,1.0},{0.7,0.3,1.4},{3,0.1,0.9}};
3.  
   Robs = MapThread[cuboid, {obsCenters, obsDims}]; (*定义4个长方体障碍物*)
4.  
   step = 0.2; (*树枝每次生长的长度，这里简单设置为固定值*)
5.  
   q0 = {-1.54, 0.76, 0.66, -1.14, -1.44, 0}; (*起点*)
6.  
   qt = {1.76, 0.76, 0.46, 0, 0.36, 0}; (*目标点*)
7.  
   nodes = {q0}; (*以起点q0做为种子*)
8.  
   edges = {}; (*树枝初始为空*)
9.  
   RRTtree = {nodes, edges}; (*树的初始化由节点和树枝组成，它是全局变量*)
10.  
    maxIters = 2000; (*最大迭代步数*)
11.  
    jointLims = {{-Pi, Pi}, {-Pi/2, Pi/2}, {-Pi, 1.45}, {-Pi, Pi}, {-2, 2}, {-Pi, Pi}}; (*关节极限范围，有些关节值不可取*)
12.  
    qRandList = RandomPoint[ Cuboid[ConstantArray[-Pi, n], ConstantArray[Pi, n]], maxIters, jointLims]; (*一次性生成全部的随机点*)
13.  
    Do[nodes = RRTtree[[1]];
14.  
    If[Min[Norm /@ ((-qt+#)&/@nodes)] < 0.01, Print["哈哈，到达目标了!"]; Break[]];
15.  
    qRand = RandomChoice[{qRandList[[i]], qt}]; (*以50%的几率贪婪地试探着朝目标生长*)
16.  
    grow[qRand,step], {maxIters}]; // AbsoluteTiming

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16

　　构造树用到了如下自定义的函数： 
　　1. 首先是碰撞检测函数 collisionDetection，若是机械臂没有碰到障碍物就返回True。为了节省时间，碰撞检测使用的是机械臂各零件的凸包，在最好的显示时才使用原始零件。

collisionDetection[Rparts_, Robs_]:= And @@ Flatten@Table[RegionDisjoint[i, #] & /@ Rparts, {i, Robs}]

• 1

• 1

　　2. 碰撞检测函数须要 Region 类型的变量，为此定义 toRegion 函数将几何体变换为 Region 类型，代码以下：

1.  
   toRegion[q_]:= Module[{gs, Rparts},
2.  
   gs = robotPartsKinematics[{IdentityMatrix[ 4], q}];
3.  
   Rparts = Table[MeshRegion[ptTransform[gs [[i]]] /@ robotParts[[i, 1]], robotParts[[i, 2, 2]]], {i, 7}]]

• 1
• 2
• 3

• 1
• 2
• 3

　　3. 向RRT树中添加节点和边的函数：

1.  
   addNode[node_]:= Module[{}, AppendTo[RRTtree [[1]], node]; Length[RRTtree[[1]]] ]
2.  
   addEdge[edge_]:= Module[{}, AppendTo[RRTtree [[2]], edge]; Length[RRTtree[[2]]] ]

• 1
• 2

• 1
• 2

　　4. 树枝朝着采样点生长（只检测一点的碰撞状况）：

1.  
   grow[qRand_,step_: 0.2]:= Module[{qNearestIdx, qNearest, growAngles},
2.  
   {qNearestIdx} = Nearest[nodes -> Automatic, qRand, 1];(*选择最近的节点*)
3.  
   qNearest = nodes[[qNearestIdx]];
4.  
   growDirection = Normalize[qRand - qNearest];
5.  
   qExpand = modToPiPi[qNearest + step * growDirection]; (*生长*)
6.  
   Rrobot = toRegion[qExpand];
7.  
   If[collisionDetection[Rrobot, Robs], qNewIdx = addNode[qExpand]; (*添加新节点，返回新节点的代号 Idx *)
8.  
   addEdge[ {qNearestIdx, qNewIdx}]] ]

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

　　5. 若是有树枝到达目标节点，backTrack 函数用于从树中抽取出链接起点和目标点的路径：

1.  
   backTrack[nodeIdx_]:=
2.  
   Module[ {searchNodeIdx = nodeIdx, nodes = RRTtree[[1]], edges = RRTtree[[2]], searchNodePos, preNodeIdx, pathIdx = {}, pathCoords},
3.  
   Do[{searchNodePos} = FirstPosition[edges[[All, 2]], searchNodeIdx];
4.  
   preNodeIdx = edges [[searchNodePos, 1]];
5.  
   AppendTo[pathIdx, preNodeIdx];
6.  
   If[preNodeIdx == 1, Break[], searchNodeIdx = preNodeIdx], {Length[edges]}];
7.  
   pathIdx = Reverse[pathIdx];
8.  
   pathCoords = nodes [[pathIdx]] ]

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

　　下面的代码能够显示搜索到的（关节空间中的）路径。这条路径质量不高，若是用于机器人的轨迹跟踪还须要通过后期的平滑处理。

1.  
   edges = RRTtree [[2]];
2.  
   targetIdx = edges [[-1, 2]];
3.  
   qPath = backTrack[targetIdx];
4.  
   anframe[i_] := Graphics3D[{q = qPath [[i]]; Robs, MapThread[move3D, {robotPartsColored, robotPartsKinematics[{IdentityMatrix[4], q}]}]}];
5.  
   Animate[anframe[i], {i, 1, Length[qPath], 1}]

• 1
• 2
• 3
• 4
• 5

• 1
• 2
• 3
• 4
• 5

7. 动力学仿真

　　你能够在淘宝花2块钱买个知网帐号，而后在其中搜索以“工业机器人控制”为关键词的学位论文并下载下来看看。在粗略地浏览了2030篇论文的目录以后，你就会像我同样总结出一个规律： 
　　●　硕士论文通常都创建了机器人的运动学模型。 
　　●　博士论文通常都创建了机器人的动力学模型。 
　　既然运动学已经可以帮助机器人动起来了，为何还须要费那么大劲创建动力学（以致于须要博士出马）？ 
　　在前面的运动学一节中，咱们能经过改变各个关节角度控制机械臂运动。可是在实际机械臂上，关节角度还不是直接控制的，它须要由电机驱动。那么电机应该输出多大的力才能驱动机械臂运动呢，所须要的电流又是多大呢？只有知道这些咱们才能真正实现对机械臂的控制。如今的工业机器人大多采用两层的控制方式，上层控制器直接输出角度信号给底层驱动器，底层驱动器负责控制电机的电流实现上层给出的运动。上层不须要知道机器人的动力学也能够，更不用管须要输出多大电流。若是你的机器人不须要过高的运动速度和精度，动力学没什么太大用处（运动学是必需的，动力学不是必需的）。但是若是你的机器人速度很快，动力学效应就很明显了，这时就要考虑动力学。在高级的机器人控制器中，都有力矩补偿功能（例如汇川、KEBA的控制器）。这个补偿的力矩是怎么来的呢？就是经过动力学方程计算获得的。补偿力矩用做前馈控制信号，将其添加到驱动器上能使机器人更好地跟踪一段轨迹。 

 
汇川控制器（动力学补偿使电流更小） 　　 KEBA控制器（动力学使跟踪精度更高）

　　咱们如何获得机器人的动力学模型呢？ 
　　宅男牛顿首开先河，在同时代的人还浑浑噩噩的时候初步搞明白了力、速度、惯性都是怎么回事，并用数学对其进行了定量描述，从而创建了物体作平移运动时的动力学方程。从牛顿的身上咱们知道，学好数学是有多重要。在那个遍地文盲的年代，牛顿偷偷地自学了欧几里得、笛卡尔、帕斯卡、韦达等大师的著做。 
　　勤奋的欧拉再接再砺，将牛顿的方程推广到转动的状况。哥俩的工做结合起来恰好能够完整地描述物体的运动，这就是牛顿-欧拉法。 
　　博学的拉格朗日发扬光大，又将牛顿和欧拉的工做总结提炼，提出了拉格朗日法。拉格朗日真聪明啊，只须要计算了物体的动能，而后再一微分就获得了动力学方程，这是多么简洁统一的方法啊。但是拉格朗日法的缺点是它的效率过低了。对于4自由度如下的机械臂，计算符号解的时间咱们还能忍受。至于6自由度以上的机械臂，大多数人都没这个耐心了（十几分钟到数小时）。并且计算出来的动力学是一大坨复杂的公式，很难分析利用。因此本文咱们采用牛顿-欧拉法创建机械臂的动力学模型（更准确的说是它的升级版——递归牛顿-欧拉法）。

 

7.1 惯性参数

　　早期工业机器人不使用动力学模型是有道理的，一个缘由是动力学的计算量太大，在高效的计算方法被发现以前，早年的老计算机吃不消；另外一个缘由就是动力学须要惯性参数。运动学只须要尺寸参数，这些相对好测量；但是动力学不只须要尺寸参数，还须要惯性参数。测量每一个连杆的质量、质心的位置、转动惯量很麻烦，尤为是当连杆具备不规则的形状时，精度很难保证。若是使用动力学带来的性能提高并不明显，谁也不想给本身找麻烦。 
　　要使用动力学模型，惯性参数必不可少。例如 Robotics Toolbox 工具箱中的 mdl_puma560.m 文件就存储了 PUMA-560 机器人的惯性参数。不一样型号的机器人具备不一样的惯性参数，并且机器人抓住负载运动时，也要把负载的惯性考虑进来。 
　　有些状况下，咱们不须要知道很精确的惯性参数，差很少够用就好了；但是有些场合对精度有要求，好比拖动示教就要求参数与实际值的偏差通常不能超过10%。对于精度要求不高的场合，能够使用一个近似值。大多数三维建模软件（例如 SolidWorks、CATIA）以及一些仿真软件（例如 Adams）都提供惯性计算功能，一些数学软件（Mathematica）也有用于计算惯性的函数（我没有对比过，因此不敢保证这些软件的计算结果都是同样的）。本文以 SolidWorks 为例介绍如何获取惯性参数。 
　　计算以前首先要设置零件的材质。在 SolidWorks 中打开一个零件，在左侧的“材质”上单击右键弹出“材料”对话框，以下图所示。在这里能够设置机器人本体的材质，MOTOMAN-ES165D 这款机器人的连杆是铸铝（铸造铝合金 Cast Aluminum）制造的。不过连杆没有把电机等部件包含进去，为此选择密度大一点的材料，本文选择钢铁。这里最重要的是材料的密度，钢铁的密度通常是7.8吨/立方米（在计算惯性时，软件假设零件的密度是均匀的，这明显是简化处理了）。设置好后点击应用便可。 

　　而后在上方“评估”选项卡中单击“质量属性”就会弹出以下图所示的对话框。 

　　SolidWorks 很快就计算出了这个零件的全部惯性参数。不过这里的信息量有点大，我逐个说明： 
　　　首先是零件的质量：172.28 千克。若是你显示的单位不是千克，能够在当前对话框中的“选项”中修改单位。 
　　　而后是零件的质心（或重心）坐标系，重心坐标系的原点也给出了：，注意它是相对于绘图坐标系的哦。重心坐标系的姿态下面会解释。 
　　　最后是零件的惯性张量，这个有些人可能不懂，我详细解释下。SolidWorks列出了3个惯性张量，它们之间的区别就在于分别相对于不一样的坐标系： 
　　①　相对于质心坐标系；其中的 Ix、Iy、Iz 三个向量表示质心坐标系相对于绘图坐标系的姿态（也就是质心坐标系的 x、y、z 三个轴向量在绘图坐标系中的表示），而 Px、Py、Pz 表示惯性主力矩（你要问我是怎么知道的，点“帮助”按钮）。惯性张量的形式是对角矩阵： 

 

　　②　相对于原点与质心坐标系重合，可是各轴与绘图坐标系一致的坐标系。SolidWorks只给出了惯性张量中各项的值。惯性张量的完整形式是对称矩阵（注意里面的负号）： 

 

　　③　相对于绘图坐标系（SolidWorks中称为输出坐标系），惯性张量的形式也是对称矩阵（一样注意里面的负号）： 

 

　　这三个惯性张量都反映了同一个零件的性质，所以应该是等价的。那么它们之间有什么关系吗？有的，它们之间能够转换。若是定义旋转矩阵 ，质心坐标向量 ，零件质量为 ，那么有

 

 

　　其中，表示向量的斜对称矩阵，这须要自定义函数（skew）实现，代码以下：

 

skew[v_] := {{0,-v[[3]],v[[2]]},{v[[3]],0,-v[[1]]},{-v[[2]],v[[1]],0}}

• 1

• 1

　　这组公式来自于，我已经验证过了，百分百正确，不信的话你也能够试试（要想结果比较接近，这些参数至少要取到小数点后5位，这依然是在“选项”页中设置）。 
　　咱们获得了三个惯性张量，在动力学方程中咱们应该使用哪一个呢？下面的程序使用了 ，由于它是相对于绘图坐标系的，而我创建运动学时选择的局部坐标系就是绘图坐标系。（我之后在这里补充个单刚体动力学的例子） 
　　 
7.2 正动力学仿真

　　若是给你一条轨迹，如何设计控制率让机械臂（末端）跟踪这条轨迹呢，控制率的跟踪效果怎么样呢？借助正动力学，咱们就能够检验所设计的控制率。 
　　因为后面的程序所依赖的动力学推导过程采用了相对自身的表示方法（也就是说每一个连杆的速度、惯性、受力这些量都是相对于这个连杆自身局部坐标系描述的），旋量也是如此，为此须要从新定义旋量（代码以下）。其实旋量轴的方向没变（由于局部坐标系的姿态与全局坐标系同样），只是改变了轴上点的相对位置。

1.  
   \[Xi]rb = Table[\[Omega]a[i] = axesDirInGlobal [[i]]; la[i] = axesPtInGlobal[[i]] - drawInGlobal[[i + 1]];
2.  
   Join[Cross[-\[Omega]a[i], la[i]], \[Omega]a[i]], {i, n}];

• 1
• 2

• 1
• 2

　　正动力学的 递归牛顿-欧拉算法 代码以下。能够看到，动力学模型比运动学模型复杂多了（动力学用到运动学，运动学却不须要动力学）。对于不少第一次接触机器人的同窗来讲，动力学是一只可怕的拦路虎，要搞明白十几个变量都是什么含义可不容易（在仿真的时候可能包含几十个变量，任何一个弄错了都会全盘皆输，动力学可比运动学难伺候多了）。由于动力学模型是一个微分方程，因此整个仿真过程就是个数值积分的过程。

1.  
   (*参数初始化*)
2.  
   endTime= 10.0; steps=2000; dt=endTime/steps; (*仿真时长、步数、步长*)
3.  
   gravityAcc= 9.80665; (*重力加速度*)
4.  
   Do[mass[i]=1.0; gForce[i]=gravityAcc*mass[i]*{0,0,-1,0,0,0},{i,n+1}]; (*mass[i]表示第i个连杆的质量，具体值本身设，重力是z轴的负方向*)
5.  
   Do[\[ScriptCapitalM][i]=IdentityMatrix[6],{i,n+1}]; (*\[ScriptCapitalM][i]表示第i个连杆的广义惯性矩阵，它包含质量和惯性张量*)
6.  
   g[L[n+ 1],L[n+2]]=g[I,L[1]]=IdentityMatrix[4];
7.  
   q=dq=ddq=ConstantArray[ 0,n]; (*关节角度、速度、加速度初始时刻假设都为0*)
8.  
   Table[V[i]=dV[i]=ConstantArray[ 0,6],{i,n+1}]; (*连杆i的广义速度V[i]包括线速度和角速度，也假设开始时刻都为0*)
9.  
   \[ CapitalPi][n+2]=ConstantArray[0,{6,6}]; (*中间变量，没啥具体物理意义，只是迭代初始值*)
10.  
    \[ Beta][n+2]=ConstantArray[0,6]; (*也是中间变量*) (*如下是计算过程*)
11.  
    qList=Table[
12.  
    　 dq=dq+ddq *dt; q=q+dq*dt;(*欧拉积分*)　
13.  
    For[i=2,i<=n+1,i++, (*速度前向递归，从第二个连杆开始到最后一个连杆*)
14.  
    　　 k=i-1; (*由于本文的连杆从1开始标记，因此第i个连杆依赖前一个关节（i-1）*)
15.  
    　　 g[L[i-1],L[i]]=TwistExp[\[Xi]r[[k]],q[[k]]].g[L[i-1],L[i],0];
16.  
    　　 g[I,L[i]]=g[I,L[i-1]].g[L[i-1],L[i]];
17.  
    　　 V[i]=Ad[Iv[g[L[i-1],L[i]]]].V[i-1]+\[Xi]rb[[k]]*dq[[k]];
18.  
    　　 \[ Eta][i]=ad[V[i]-\[Xi]rb[[k]]*dq[[k]]].\[Xi]rb[[k]]*dq[[k]];
19.  
    　　 ];
20.  
    For[i=n+1,i>=2,i--, (*力和惯量后向递归，从最后一个连杆开始到第二个连杆*)
21.  
    　　 k=i- 1;
22.  
    　　 \[ Tau][k] = 0.0; (*施加关节力矩*)
23.  
    　　 \!\(\*OverscriptBox[ \(\[ScriptCapitalM]\), \(^\)]\)[i]=\[ScriptCapitalM][i]+T[Ad[Iv[g[L[i],L[i+1]]]]].\[CapitalPi][i+1].Ad[Iv[g[L[i],L[i+1]]]];
24.  
    　　 Fex[i]=T[Ad[RToH[gToR[g[I,L[i]]]]]].gForce[i];
25.  
    　　 \[ ScriptCapitalB][i]=-T[ad[V[i]]].\[ScriptCapitalM][i].V[i]-Fex[i]+T[Ad[Iv[g[L[i],L[i+1]]]]].\[Beta][i+1];
26.  
    　　 \[ CapitalPsi][i]=1/(\[Xi]rb[[k]].\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].\[Xi]rb[[k]]);
27.  
    　　 \[ CapitalPi][i]=\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i]-\[CapitalPsi][i]*KroneckerProduct[\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].\[Xi]rb[[k]],\[Xi]rb[[k]].\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i]];
28.  
    　　 \[ Beta][i]=\[ScriptCapitalB][i]+\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].(\[Eta][i]+\[Xi]rb[[k]]*\[CapitalPsi][i]*(\[Tau][k]-\[Xi]rb[[k]].(\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].\[Eta][i]+\[ScriptCapitalB][i])));
29.  
    ];
30.  
    For[i=2,i<=n+1,i++, (*加速度前向递归，从第二个连杆开始到最后一个连杆*)
31.  
    　　 k=i-1;
32.  
    　　 ddq[[k]]=\[CapitalPsi][i]*(\[Tau][k]-\[Xi]rb[[k]].\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].(Ad[Iv[g[L[i-1],L[i]]]].dV[i-1]+\[Eta][i])-\[Xi]rb[[k]].\[ScriptCapitalB][i]);
33.  
    　　 dV[i]=Ad[Iv[g[L[i-1],L[i]]]].dV[i-1]+\[Xi]rb[[k]]*ddq[[k]]+\[Eta][i]
34.  
    　　 ];
35.  
    　　 q , {t, 0, endTime, dt}];//AbsoluteTiming (*显示计算耗时*)

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35

　　其中， ad 函数用于构造一个李代数的伴随表达形式，代码以下。（开始咱们定义的关节旋量是李代数，连杆的速度在自身局部坐标系下的表达也是一个李代数，可是加速度却不是）

1.  
   ad[\[Xi]_] := Module[{w, v},
2.  
   v = skew[ \[Xi][[1 ;; 3]]]; w = skew[\[Xi][[4 ;; 6]]];
3.  
   Join[Join[w, v, 2], Join[ConstantArray[0, {3, 3}], w, 2]] ]

• 1
• 2
• 3

• 1
• 2
• 3

　　正动力学的输入是关节力矩，下面咱们为关节力矩设置不一样的值，看看机械臂的表现： 
　　●　若是令关节力矩等于零（即 \[Tau][k] = 0.0），机械臂将在惟一的外力——重力做用下运动，以下图所示。 

　　只受重力的状况下，机械臂的总能量应该守恒。咱们能够动手计算一下机械臂的能量（由动能和重力势能组成，代码以下）。将仿真过程当中每一时刻的能量计算出来并保存在一个列表中，再将其画出图像以下图所示。可见能量几乎不变（轻微的变化是由积分偏差致使的，若是步长取的再小一些，就更接近一条直线），这说明机械臂的总能量保持恒定，也间接证明了正动力学代码的正确性。这个简单的事实让人很吃惊——虽然机械臂的运动看起来那么复杂，可是它的能量一直是不变的。从力和运动的角度看，机械臂的行为变化莫测，但是一旦切换到能量的角度，它竟然那么简洁。机械臂的运动方程和能量有什么关系呢？聪明的拉格朗日就是从能量的角度去推导动力学方程的。

 

1.  
   energyOfParts = Table[ 1/2*V[i].\[ScriptCapitalM][i].V[i] + mass[i]*gravityAcc*g[I, L[i]][[3, 4]], {i, n + 1}];
2.  
   totalEenergy = Total[energyOfParts];

• 1
• 2

• 1
• 2

 

　　●　咱们也可让机械臂跟踪一条给定的轨迹，此时给定力矩为 PD 控制率：

 

1.  
   Kp= 10000; Kd=50; (*PD 控制系数*)
2.  
   \[ Tau][k]=Kp(qfun[k][t]-q[[k]])+Kd(dqfun[k][t]-dq[[k]]);

• 1
• 2

• 1
• 2

　　其中，qfun 和dqfun 是 4.2 节中定义的插值函数，这里用做机械臂要跟踪的（关节空间中的）参考轨迹。跟踪一个圆的效果以下图所示。

　　●　机械臂实际工做时可能会受到干扰。PD控制率对于扰动的效果怎么样？咱们施加一个扰动信号试试。这里选择一个尖峰扰动，模拟的实际状况是机械臂忽然被踹了一脚。扰动函数的定义代码以下，你能够本身修改扰动的大小和尖峰出现的时间。

 

1.  
   Manipulate[disturb[t_] := peak Exp[-fat(t - tp)^2];
2.  
   Plot[disturb[t], {t, 0, 10}, PlotRange -> All]
3.  
   , {peak, 50, 300, 0.1}, {fat, 0.5, 40, 0.1}, {tp, 0, 10, 0.1}, TrackedSymbols :> True]

• 1
• 2
• 3

• 1
• 2
• 3

 

　　把扰动加到第二个关节的力矩上

 

\[Tau][k] = Kp (0 - q[[k]]) + Kd (0 - dq[[k]]) + If[k == 2, disturb[t], 0];

• 1

• 1

　　机械臂的响应以下图所示，可见机械臂仍是能回复零点姿式的。一开始机械臂有一个轻微的颤动，俗称“点头”。这是因为刚一开始机械臂的角度和角速度都为零，因此关节力矩也为零，致使机械臂缺乏可以平衡重力的驱动力。在第5秒左右扰动出现，致使机械臂偏离了零点姿式，但在反馈控制做用下很快又回到了零点姿式。 

 

7.3 逆动力学仿真

　　输入力矩后，借助正动力学能获得关节角加速度，积分后能够获得角速度和角度。就像运动学和逆运动学的关系同样，逆动力学与正动力学恰好相反，它的用处是：若是告诉你机械臂的运动（也就是关节角度、角速度、角加速度），计算所需的关节力矩。

1.  
   endTime= 10.0; steps=1000; dt=endTime/steps;(*最开始一样是参数初始化*)
2.  
   gravityAcc= 9.80665; (*重力加速度*)
3.  
   Do[mass[i]=1.0; gForce[i]=gravityAcc*mass[i]*{0,0,-1,0,0,0},{i,n+1}];
4.  
   Do[\[ScriptCapitalM][i]=IdentityMatrix[6],{i,n+1}];
5.  
   Do[V[i]=dV[i]=ConstantArray[0,6],{i,n+1}];
6.  
   Fin[n+ 2]=ConstantArray[0,6]; (*第n+1个连杆受到内力，为了迭代过程写起来方便定义的*)
7.  
   g[L[n+ 1],L[n+2]]=g[I,L[1]]=IdentityMatrix[4];
8.  
   q=dq=ddq=ConstantArray[ 0,n]; (*初始状态的关节角度、速度、加速度*)
9.  
   \[Tau]List=Table[
10.  
    　 For[i=2,i<=n+1,i++, (*速度前向递归，从第二个连杆开始到最后一个连杆*)
11.  
    　 k=i- 1;
12.  
    　　g[L[i- 1],L[i]]=TwistExp[\[Xi]r[[k]],q[[k]]].g[L[i-1],L[i],0];
13.  
    　　g[I,L[i]]=g[I,L[i- 1]].g[L[i-1],L[i]];
14.  
    　　V[i]=Ad[Iv[g[L[i- 1],L[i]]]].V[i-1]+\[Xi]rb[[k]]*dq[[k]];
15.  
    　　dV[i]=Ad[Iv[g[L[i- 1],L[i]]]].dV[i-1]+ad[V[i]-\[Xi]rb[[k]]*dq[[k]]].\[Xi]rb[[k]]*dq[[k]]+\[Xi]rb[[k]]*ddq[[k]]];
16.  
    　 For[i=n+1,i>=2,i--,　(*力后向递归，从最后一个连杆开始到第二个连杆*)
17.  
    　　k=i- 1;
18.  
    　　Fex[i]=T[Ad[RToH[gToR[g[I,L[i]]]]]].gForce[i]; (*连杆受到的重力*)
19.  
    　　Fin[i]=\[ScriptCapitalM][i].dV[i]-T[ad[V[i]]].\[ScriptCapitalM][i].V[i]+T[Ad[Iv[g[L[i],L[i+ 1]]]]].Fin[i+1]-Fex[i];
20.  
    　　\[Tau][k]=\[Xi]rb [[k]].Fin[i]];
21.  
    Array[\[Tau],n]
22.  
    , {t, 0, endTime, dt}];//AbsoluteTiming (*监视计算时间*)

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22

　　在重力做用下，机械臂保持“立正姿式”须要多大力矩呢？将初始状态设为 0，通过逆动力学计算获得的答案是{0,-38.1,-38.1,0,-2.06,0}。若是把这个力矩带入正动力学仿真就能看到机械臂保持不动，这证实咱们的模型基本是正确的。 
　　 
8. 结尾

　　本文咱们以 Mathematica 通用数学软件为平台，针对串联机械臂的建模、规划和控制中的基本问题进行了仿真，差很少该说再见了。不过新的篇章即将展开 —— 移动机器人是另外一个有趣的领域，将来咱们将加入移动机器人仿真的功能，支持地面移动机器人的运动控制、环境约束下的运动规划、移动机械臂、多机器人避障、多机器人编队运动等，并讨论环境建模、导航与定位、非完整约束、最优控制、轮式车辆和履带车辆的受力、群集协同运动等问题，敬请期待哟！ 

　　 
引用文献 

[1]　一种高效的开放式关节型机器人3D仿真环境构建方法，甘亚辉，机器人. 
[2]　Robotics, Vision and Control Fundamental Algorithms in MATLAB, Peter Corke. 
[3]　A Mathematical Introduction to Robotic Manipulation —— A Mathematica Package for Screw Calculus, Richard M. Murray, 435页. 
[4]　Robotica: A Mathematica Package for Robot Analysis, Mark W. Spong. 
[5]　机器人学导论，John J. Craig，中文第三版. 
[6]　PC-based Control for Robotics in Handling, Production and Assembly, Beckhoff. 
[7]　Robotics - Modelling, Planning and Control, Bruno Siciliano, 582页. 
[8]　Lie Group Formulation of Articulated Rigid Body Dynamics, Junggon Kim, 2012.

 

补充： Mathematica 的缺点

　　在笔者就读研究生期间，Matlab 的使用率颇高。每次参加答辩、报告，看着同窗或老师用 Matlab 制做的丑陋不堪的图表，心中就想把 Matlab 的界面设计师枪毙十分钟。再加上呆板的函数定义和使用方式、缺少对部分机器人仿真功能的支持，让笔者不得不寻找其它的替代软件。但是在网络发达的今天，我竟然找不到稍微像点样的介绍机器人仿真的博客以及原理性代码，要么过于简单和低级，要么则是东拼西凑。因而想把本身的经验写出来，并公开代码（若是你想要，我能够毫无保留地公开全部代码）。 
　　就像 Matlab 有不少让人不爽的地方同样，Mathematica 用于机器人仿真一样存在一些缺陷。咱们以前在碰撞检测部分已经提过，要想达到很快的检测速度就不得不使用简单的几何模型。虽然 Mathematica 的函数也通过了优化，可是只适用于须要较少计算次数的场合，在屡次处理大量数据时仍是比较慢。Mathematica 自己是用 C 语言写成的，若是某个函数被大量调用能够考虑用 C 语言写成动态连接库（dll），而后在 Mathematica 中调用，这就像 Matlab 中的 MEX 文件。 
　　Mathematica 支持设置中断，但使用起来至关不友好，它提供了一个专门用来开发调试的软件——Workbench，惋惜也很差用。 在调试时，我不得不使用 Print 函数打出中间计算结果来检查中间运算结果。Mathematica 缺乏像 Matlab 同样的变量监控窗口能够实时看到变量的值，这在调试时显得很不方便。

https://blog.csdn.net/weixin_39901416/article/details/79903999