无论你从事的是机器人研发还是教学科研，一款好用的仿真软件都能对你的工作起到很大的帮助。那么应该选择哪个软件呢？最方便的选择就是成熟的商业软件，例如Adams、Webots。你的钱不是白花的，商业软件功能强大又稳定，而且性能一般都经过了优化。可是再强大的商业软件也有设计不合理的地方，它们的算法基本都是“黑箱”，你想做一点更改都不行。从学习和研究的角度出发，最好选择代码可修改的开源或半开源软件。在论文数据库中检索一下就会发现，很多人都选择借助Matlab这个数学软件平台进行机器人的建模仿真[1][1]。这并不奇怪，因为Matlab具有优秀的数值计算和仿真能力，在它的基础上开发会很便利。如果你非要舍近求远，用C编写一套仿真软件，先不要说仿真结果如何显示，光是矩阵计算的实现细节就能让你焦头烂额。
1.获取机器人的外观模型
2.导入机器人的外观模型
用SolidWorks2014软件打开机器人的装配体文件，并选择“另存为”STL格式。然后打开当前页面下方的“选项”对话框，如下图。这里我们要设置三个地方：1.“品质”表示对模型的简化程度，如果你想实现非常逼真的效果，可以选择“精细”，但缺点是数据点很多导致文件很大、处理起来比较慢。一般选择“粗糙”就够了；2.勾选“不要转换STL输出数据到正的坐标空间”；3.不要勾选“在单一文件中保存装配体的所有连杆”。
小技巧STL格式是一种三维图形格式，被很多三维建模软件支持（Mathematica也支持，所以我们要保存为这个格式）。STL格式只存储三维图形的表面信息，而且是用很多的三角形对图形进行近似的表示。如果你的机器人外形比较简单（规则的几何体），那么得到的STL文件大小一般只有几十KB；可是如果外形很复杂（比如包含很多的孔洞、曲面），生成的STL文件会很大（几MB～几十MB）。对于一般配置的计算机，模型文件超过100KB用Mathematica处理起来就比较慢了。为了让仿真显示地更流畅，可以利用免费软件MeshLab对其进行化简，MeshLab通常能够在基本不改变外形的前提下将文件大小缩减为原来的十分之一甚至更多。MeshLab的安装和操作都是傻瓜式的，打开后进入如下图左所示的菜单中，出现右图的页面，这里的“Percentagereduction”表示缩减的百分比（1表示不缩减，0.1则表示缩减为原来的10%），设置好后点击Apply并保存即可。
然后在Mathematica中导入STL文件，使用的代码如下（假设这些STL文件保存在D:\MOTOMAN文件夹下）：
这里我偷了个懒，为了少打些字，我把导出连杆的文件名改成了从1到9的数字（这个机械臂的装配体一共包含9个零件）。我们把导入的模型显示出来，效果如下图。使用的代码如下
Graphics3D[{frame3D,robotParts}]
说明：frame3D是三维坐标系的三个正交的轴（xyzxyz轴的颜色分别是RGBRGB）。在机器人领域会用到大量的坐标系及其变换，直接看数字总是不直观，不如将坐标系显示出来更方便。定义frame3D的代码如下，这个坐标系默认原点的位置在(0,0,0)(0,0,0)，以后我们称这个坐标系为“全局坐标系”。
frame3D={RGBColor[#],Arrowheads[0.05],Arrow[Tube[{{0,0,0},#},0.01]]}&/@IdentityMatrix[3];
你可能会好奇：导入的零件数据是以什么样的格式储存的呢？用来存储机器人外形数据的robotParts变量包含一共9个零件的数据，假如你想看第一个零件（对应的是基座，它通常用来将机械臂固定在大地上），可以输入：
robotParts[[1]](*双层方括号中的数字表示对应第几个零件*)
运行后的输出结果是一堆由GraphicsComplex函数包裹着的数字，主要可分为两部分：第一部分是零件几何体所有顶点的三维坐标；第二部分是组成零件几何体的三角形（注意：构成每个三角形的三个顶点是第一部分点的序号，而不是坐标值）。我们可以用以下代码将其分别显示出来：
pts=robotParts[[1]][[1]];(*零件1的第一部分：顶点的三维坐标数据*)triangles=robotParts[[1]][[2]];(*零件1的第二部分：三角形面片*)trianglesBlue=triangles/.{EdgeForm[]->EdgeForm[Blue]};(*三角形的边显示为蓝色*)Graphics3D[{GraphicsComplex[pts,trianglesBlue],Red,Point[pts]}]
所有零件都成功地导入了，而且它们的相对位置也是正确的。你可能会问：机械臂为什么是处于“躺着”的姿势呢？这是由于零件是按照SolidWorks默认的坐标系（yy轴向上）绘制和装配的。而在Mathematica中默认的坐标系是zz轴向上。那么我们采用哪个坐标系呢？当然你可以任性而为，用哪个都可以。不过根据国家标准GBT16977-2005《工业机器人坐标系和运动命名原则》，机械臂基座坐标系的zz轴应该垂直于基座安装面（一般是水平地面）、指向为重力加速度的反方向（也就是垂直地面向上），xx轴指向机器人工作空间中心点的方向。制定国家标准的都是些经验丰富的专家老手，我们最好跟国标保持一致（国标的作图水平就不能提高点吗？这图怎么感觉像小学生画的）。
为了让机器人变成国标规定的姿势，需要旋转各个连杆。我们先想想应该怎么转：结合我们之前导入的图形，可以先绕全局坐标系的xx轴转90°90°，再绕全局坐标系的zz轴转90°90°。还有一种方法是：先绕全局坐标系的xx轴转90°90°（记这个旋转后的坐标系为AA），再绕AA的yy轴转90°90°。两种方法的效果是一样的，但是注意合成矩阵时乘法的顺序（见以下代码），不懂的同学可以看看文献[5][5]中的31～～33页。当然，转动是有正负之分的：将你的右手握住某个坐标轴，竖起大拇指，让大拇指和轴的正方向一致，这时四指所示的方向就是绕该轴转动的正方向。为此，我们定义旋转矩阵（两种定义效果一样）：
{Xaxis,Yaxis,Zaxis}=IdentityMatrix[3];(*定义三个旋转轴*)rot=RotationMatrix[90Degree,Zaxis].RotationMatrix[90Degree,Xaxis];(*注意第二次变换是在左边乘*)rot=RotationMatrix[90Degree,Xaxis].RotationMatrix[90Degree,Yaxis];(*注意第二次变换是在右边乘*)
然后用rot矩阵旋转每个连杆（的坐标，即保存在第一部分robotParts[[i,1]]中的数据）：
robotParts=Table[GraphicsComplex[rot.#&/@robotParts[[i,1]],robotParts[[i,2]]],{i,9}];
经过姿态变换后的机器人看起来舒服点了，只是有些苍白。为了给它点个性（也方便区分不同的零件或者称为连杆），我们给连杆设置一下颜色，代码如下。你可能注意到了，这里我没有使用循环去为9个连杆一个一个地设置颜色，而是把同类的元素（颜色）写在一起，然后再和连杆列表一起转置即可把颜色“分配”给各个连杆。这样做的好处就是代码比较简洁、清晰，以后我们会经常这么做。
colors={Gray,Cyan,Orange,Yellow,Gray,Green,Magenta,LightGreen,Pink};(*1~9各连杆的颜色*)robotPartsColored=Transpose[{colors,robotParts}];(*把颜色分配给各连杆*)Graphics3D[robotPartsColored]
说明：现在的机器人姿势（大臂竖直、小臂前伸）是6自由度机械臂的“零位”状态，我们将此时机械臂各关节的角度认为是0。一般机械臂上都有各关节的零点位置标记，用于指示各关节的零点。我们用控制器控制机械臂时，发出的角度指令都是相对于这个零点位置的。零点位置不是必须遵守的，你可以将任意的角度设置为零位，不过为了统一，最好用机械臂固有的零位——也就是当前姿势下各关节的角度。
3.运动学仿真
前面的工作只是让机械臂的模型显示出来。如果想让它动起来，那就要考虑运动学了。机器人这个学科听起来高大上（很多都停留在理论上），可实际上现在大多数工业机器人的控制方式还是比较低级的，它们只用到了运动学，高级一点的动力学很少用，更不要提智能了（它们要说自己有智能，我们家的洗衣机和电视机都要笑掉大牙了）。看来要使用机器人，运动学是必不可少的，所以我们先来实现运动学。在建立运动学模型之前我们需要了解机器人的机械结构。前面提到，MOTOMAN-ES165D是一个6自由度的串联机械臂。而6个自由度的机器人至少由7个连杆组成（其中要有一个连杆与大地固定，也就是基座）。可是我们导入的连杆有9个，多出来的2个连杆是弹簧缸（基座上黄色的圆筒）的组成部分。MOTOMAN-ES165D机器人能够抓持的最大负载是165公斤，弹簧缸的作用就是作为配重平衡掉一部分负载的重量，要不然前端的关节电机会有很大的负担。可是弹簧缸给我们的建模造成了麻烦，因为它导致“树形拓扑”以及存在“闭链”，这不太好处理。为此，我们先忽略掉弹簧缸。3.1连杆的局部坐标系
然后回到机器人的装配体中，在左侧的连杆树中展开每个连杆找到并选中其绘图坐标系的原点，然后点击上方菜单栏“评估”中的“测量”即可看到图中出现了一组坐标值（如下图所示），这就是连杆绘图坐标系的原点在全局坐标系（本文将全局坐标系定义为装配体的坐标系）中的位置。
我们记录下所有连杆的绘图坐标系的原点位置（除去弹簧缸的2个，注意SolidWorks中默认的单位是毫米，这里除以10001000是为了变换到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;
因为我们是在SolidWorks中测量的位置，所以这些位置值还是相对于SolidWorks的坐标系（yy轴朝上），要变到zz轴朝上，方法仍然是乘以旋转矩阵rot：
drawInGlobal=Table[rot.i,{i,drawInGlobalSW}];
以后我们会经常对点的坐标进行各种各样的变换（平移、旋转），而且很多时候是用一个矩阵同时对很多个点的坐标进行变换（例如上面的例子），不如定义一个算子以简化代码。我们可以定义算子（其实是一个函数）：
CircleDot[matrix_,vectors_]:=(matrix.#)&/@vectors;
所以前面的变换用我们自定义的算子表示如下（复制到Mathematica中后\[CircleDot]会变成一个Mathematica内部预留的图形符号⊙⊙，这个符号没有被占用，所以这里我征用了）：
drawInGlobal=rot\[CircleDot]drawInGlobalSW;(*哈哈！写起来是不是简单多了*)
SetDirectory[NotebookDirectory[]]< 还记得吗？最开始我们导入机器人模型时，各连杆的位置都已经按照装配关系确定好了，所以它们的坐标也是相对于全局坐标系描述的。可是现在我们要让机械臂动起来（并且显示出来），这就需要移动这些坐标。为了方便起见，最好能将每个连杆的坐标表示在自己的绘图坐标系中，因为这样我们只需要移动绘图坐标系就行了，而各点的坐标相对它们所属的绘图坐标系是不动的。应该怎么做呢？很简单，将连杆的坐标减去绘图坐标系的原点在全局坐标系中的坐标即可： 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}; 坐标移动后的连杆如下图所示（图中的坐标系是各个连杆自己的绘图坐标系，我没有对坐标转动，所以绘图坐标系和全局坐标系的姿态相同）。我们一开始从SolidWorks导出文件时是一次性地导出整个装配体的。其实，如果我们挨个打开各个连杆并且一个一个的导出这些连杆，那么得到数据就是相对于各自的绘图坐标系的，只不过这样稍微麻烦一点。 3.2利用旋量建立运动学模型 定义关节旋量的代码如下。其中相对旋量ξrξr用于迭代运动学计算，它的含义是当前连杆的转轴表示在前一个连杆坐标系中。 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}]; 我们对关节的相对运动进行了表示，然而要建立运动学还缺少一样东西：连杆间的初始相对位姿（初始的意思是机械臂处于“零位”的状态下）。零位下，我们将所有连杆的姿态都认为和全局坐标系一样，所以不用计算相对姿态了。至于它们的相对位置嘛，我们已经知道了绘图坐标系原点在全局坐标系中的坐标，两两相减就可以得到它们的相对位置了，很简单吧！（见下面的代码） Do[g[L[i],L[i1],0]=PToH[drawInGlobal[[i1]]-drawInGlobal[[i]]],{i,n}]; 其中，PToH函数能将位置向量转换为一个4×44×4齐次变换矩阵，这是借助RPToH函数实现的（RPToH函数就是Screws工具包[3][3]中的RPToHomogeneous函数），它可以将一个3×33×3旋转矩阵和3×13×1位移向量组合成一个4×44×4齐次变换矩阵。将旋转矩阵和位移向量合成为齐次变换矩阵是我们以后会经常用到的操作。类似的，也可以定义RToH函数将旋转矩阵生成对应的齐次变换矩阵，代码如下： RToH[R_]:=RPToH[R,{0,0,0}]PToH[P_]:=RPToH[IdentityMatrix[3],P] 说明：本文中，用符号I表示全局坐标系（同时也是惯性坐标系）；符号L[i]表示第ii个连杆，变量g[L[i],L[i1]]表示第i1i1个连杆相对于第ii个连杆的位姿矩阵（它是一个4×44×4齐次变换矩阵）；变量g[I,L[i]]表示什么你肯定猜到了，它表示第ii个连杆相对于全局坐标系的位姿矩阵。如果不特别说明，本文总是用g（或者g开头的变量）表示一个（或一组）齐次变换矩阵，这是约定俗成的。 现在可以正式推导机械臂的运动学模型了。在使用机械臂时，大家一般只关心其最末端连杆的位姿，更确切的说，是最末端连杆的位姿与关节角度的关系。不过为了得到最末端连杆的位姿，我们需要计算中间所有连杆的位姿。这里利用相邻连杆的迭代关系——每个连杆的位姿依赖前一个连杆的位姿——来提升计算效率。所以，可以定义机械臂所有连杆的运动学函数为： robotPartsKinematics[configuration_]:=Module[{q,g2To7},{g[I,L[1]],q}=configuration;g2To7=Table[g[L[i],L[i1]]=TwistExp[\[Xi]r[[i]],q[[i]]].g[L[i],L[i1],0];g[I,L[i1]]=g[I,L[i]].g[L[i],L[i1]],{i,n}];Join[{g[I,L[1]]},g2To7]] robotPartsKinematics函数的输入是：基座的位姿矩阵g[I,L[1]]和所有关节的角度向量q∈R6∈R6，这组变量完整地描述了一个串联机械臂的位置和姿势（用机器人中的专业术语应该叫“构型”：configuration，注意不要翻译为“配置”），而输出则是所有连杆相对于全局坐标系的4×44×4位姿矩阵（即g[I,L[i]]，其中i=1~7）。其中，TwistExp函数来自于Screws工具包[3][3]，作用是构造旋量的矩阵指数。 说明：在大多数的机器人教科书中，连杆的记号是从0开始的，也就是说将基座记为0号连杆，然后是1号连杆，最末端的连杆是n1n1号（假设机械臂的自由度是nn）；而关节的记号是从1开始，也就是说1号关节连接0号连杆和1号连杆。这样标记的好处是记号一致，推导公式或编程时不容易出错：比如说我们计算第ii个连杆的速度时要利用第ii个关节的转动速度。可是本文中连杆的记号是从1开始的（基座标记为1号连杆），我们保留0号标记是为了以后将机械臂扩展到装在移动基座（比如一个AGV小车）的情况，这时0号就用来表示移动基座。 可以看到，只要定义好关节旋量，建立运动学模型非常简单。可是这样得到的运动学模型对不对呢？我们来检验一下。借助Manipulate函数，可以直接改变机械臂的各关节角度，并直观地查看机械臂姿势（应该叫构型了哦）的变化，如以下动画所示。可以看到，机械臂各连杆的运动符合我们设置的关节值，这说明运动学模型是正确的。 Manipulate[q={##}[[;;,1,1]];gs=robotPartsKinematics[{IdentityMatrix[4],q}];Graphics3D[{MapThread[move3D,{robotPartsColored,gs}]},PlotRange->{{-2,3},{-2,3},{0,3}}],##,ControlPlacement->Up]&@@Table[{{qt[i],0},-Pi,Pi,0.1},{i,n}] move3D[shape_,g_]:=GeometricTransformation[shape,TransformationFunction[g]] robotMoveToQ[q_]:=Module[{gs},gs=robotPartsKinematics[{IdentityMatrix[4],q}];MapThread[move3D,{robotPartsColored,gs}]] 而且，函数的使用也很灵活。例如，我们可以将不同构型下的机械臂同时显示出来，只需要两行代码，如下： qs=Table[{a-Pi/2,a/2,-a/2Pi/6,0,0,0},{a,0,Pi,0.2}];(*生成关节变量列表，即不同构型*)Graphics3D[{robotMoveToQ/@qs}] 4.逆运动学仿真 借助运动学，我们成功地通过改变关节角度实现了对机械臂的控制。当然这没什么值得炫耀的，本质上不过是矩阵相乘罢了。本节我们考虑一个更有挑战性也更好玩的问题。如果告诉你所有连杆（局部坐标系）的位姿，你能不能算出机械臂的各个关节角来？你一定会说这很简单，求一下反三角函数就行了。但是实际应用时经常会遇到比这个难一些的问题：只告诉你机械臂最后一个连杆的位姿，如何得到各关节的角度？这个问题被称为“逆运动学”。RoboticToolbox工具箱[2][2]中给出了两个解逆运动学问题的函数：ikine和ikine6s，分别是数值解法和符号解析解法，本文我们也用两种方式解决逆运动学问题。 4.1数值解法之——解方程 上一节的运动学函数robotPartsKinematics能得到所有连杆的位姿。大多数时候，人们只关心最后一个连杆的位姿（因为它上面要装载操作工具），即Last@robotPartsKinematics[{IdentityMatrix[4],q}]（注意q是一个六维向量，即q=(q1,q2,q3,q4,q5,q6q1,q2,q3,q4,q5,q6)），结果如下图所示（另存为可以看大图）。这里关节角没有设置数值，因此得到的是符号解，有些长哦。这也是为什么机器人领域经常使用三角函数缩写的原因：比如把cos(q1)cos(q1)记为c1c1。在[4][4]中提供了一个函数SimplifyTrigNotation，可以用来对下式进行缩写。 如果我们想让机械臂末端（连杆）到达某个（已知的）位姿gt，也就是让上面的矩阵等于这个位姿矩阵： Last@robotPartsKinematics[{IdentityMatrix[4],{q1,q2,q3,q4,q5,q6}}]=gt(*逆运动学方程*) 说明：在求解逆运动学方程前还需要解决一个小问题：如何在Mathematica中表示一个期望的目标位姿gt呢？Mathematica提供了RollPitchYawMatrix函数和EulerMatrix函数用来表示三维转动（你用哪个都可以），然后利用前面的RPToH函数合成为位姿矩阵gt即可，示例代码如下。其中，cuboid函数用于绘制一个长方体。如果你使用Matlab，那我要可怜你了。因为Matlab没有绘制长方体的函数，一切你都要自己画。而Mathematica定义了一些常用几何图形，可以直接用。 cuboid[center_,dim_]:=Cuboid[center-dim/2,centerdim/2];(*输入center是长方体的几何中心，dim是长方体的长宽高*) object=cuboid[{0,0,0},{0.3,0.2,0.05}];Manipulate[gt=RPToH[EulerMatrix[{\[Alpha],\[Beta],\[Gamma]}],{x,y,z}];Graphics3D[{Yellow,move3D[{frame3D,object},gt]},PlotRange->0.5],Grid[{{Control[{{x,0},-0.5,0.5,0.01}],Control[{{\[Alpha],0},-Pi,Pi,0.1}]},{Control[{{y,0},-0.5,0.5,0.01}],Control[{{\[Beta],0},-Pi,Pi,0.1}]},{Control[{{z,0},-0.5,0.5,0.01}],Control[{{\[Gamma],0},-Pi,Pi,0.1}]}}],TrackedSymbols:>True] 不过这个方程组是由4×44×4齐次变换矩阵得到的，里面有4×4=164×4=16个方程，去掉最后一列(0,0,0,1)(0,0,0,1)对应的44个恒等式还有1212个方程，超过了未知量（66个）的个数，这是因为3×33×3旋转矩阵的各项不是独立的，因此要舍去一部分。该保留哪三项呢？只要不选择同一行或同一列的三项就可以了，这里我保留了(1,2),(2,3),(3,3)(1,2),(2,3),(3,3)三项（但不是全都对，有时你需要试试其它项）。 Manipulate[gts=Last@robotPartsKinematics[{IdentityMatrix[4],{q1,q2,q3,q4,q5,q6}}];gt=RPToH[RollPitchYawMatrix[{\[Alpha],\[Beta],\[Gamma]}],{x,y,z}];Quiet[q={q1,q2,q3,q4,q5,q6}/.FindRoot[gts[[1,4]]==gt[[1,4]]&>s[[2,4]]==gt[[2,4]]&>s[[3,4]]==gt[[3,4]]&>s[[1,2]]==gt[[1,2]]&>s[[2,3]]==gt[[2,3]]&>s[[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,robotMoveToQ[q],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] 同样借助Manipulate函数改变值的大小，试验的效果见下图。 4.2数值解法之——迭代法 解方程的方法很多，下面我们换一种思路求解逆运动学方程，其思想来自于[2][2]（英文版187页），代码如下： forwardKinematicsJacobian函数用于计算（空间）雅可比矩阵和最后一个连杆的位姿，它修改自Screws工具包[3][3]。逆运动学计算函数inverseKinematics的输入是期望的末端连杆位姿gt，迭代的初始角度q0，以及误差阈值errorthreshold（默认值为0.00010.0001）。其中的modToPiPi函数（实现代码如下）用于将角度值转换到π～ππ～π的范围之间。这里为什么需要modToPiPi函数呢？因为角度是个小妖精，如果我们不盯紧它，它可能会时不时的捣乱。从外部看，机械臂的一个转动关节位于角度π/3π/3和角度π/32ππ/32π没什么区别。可是如果我们放任角度这样随意跳变，会导致轨迹不连续，这样机械臂在跟踪轨迹时就会出现麻烦。 modToPiPi[angle_]:=Module[{a=Mod[angle,2.0*Pi]},If[Re[a]>=Pi,a-2.0*Pi,a]]SetAttributes[modToPiPi,Listable]; 其中，Ad函数就是Screws工具包[3][3]中的RigidAdjoint函数，它表示一个齐次变换矩阵的伴随变换（AdjointTransformation），diagF函数用于将多个矩阵合成为块对角矩阵，实现代码如下： diagF=SparseArray[Band[{1,1}]->{##}]&(*用法为A={{1,2},{3,4}};B={{5,6},{7,8}};diagF[A,B]//MatrixForm*) HToR函数和HToP函数分别用于从一个齐次变换矩阵中取出旋转矩阵（RR）和位移向量（PP），代码如下。 HToR[g_]:=Module[{n=(Dimensions[g])[[1]]-1},g[[1;;n,1;;n]]]HToP[g_]:=Module[{n=(Dimensions[g])[[1]]-1},g[[1;;n,n1]]] 我们以后会用到很多矩阵操作（比如转置、求逆），而Mathematica的函数名太长，为了写起来方便，我定义了简写的转置和求逆函数，代码如下： T[g_]:=Transpose[g]Iv[g_]:=Inverse[g] 我们想让机械臂（的末端）依次到达一些空间点（这些点可能是机械臂运动时要经过的）。为此首先生成一些三维空间中的点： Clear[x,y];pts2D=Table[{Sin[i],Cos[i]}/1.4,{i,0,4Pi,Pi/400}];(*先生成二维平面上的点，它们均匀地分布在一个圆上*)pts3D=pts2D/.{x_,y_}->{1.721,x,y1.4};(*再将二维坐标变换成三维坐标*)Graphics3D[Point[pts3D]] 然后调用逆运动学函数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[gt,q],{gt,gStars}];(*依次遍历所有点，我们用每次计算得到的q作为下一次迭代的初始值，这样迭代速度更快*) 计算结果qs中存储了机械臂到达不同点处的关节向量，如果以后我们想让机械臂跟踪这个向量序列，可以对其插值得到连续的关节函数，这是靠Interpolation函数实现的，代码如下。关于Interpolation函数我要啰嗦几句，因为以后我们可能会经常用到它。对于机械臂的每个关节来说，Interpolation得到的是一个插值函数（InterpolatingFunction），更确切地说是“Hermite多项式”或“Spline样条”插值函数。插值函数与其它的纯函数没什么区别，比如说我们可以对它求导、求积分。对这6个关节的插值函数求导就能得到关节速度和加速度函数： 画出插值后各关节的角度、角速度、角加速度的变化趋势，如下图。能看到有两个关节角速度变化剧烈，因此，理论上这个曲线不适合让机器人跟踪。 pq=Plot[Evaluate@Table[qfun[i][x],{i,n}],{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}}] 4.3雅克比矩阵的零空间 对于本文中的6自由度机械臂，由于它不是冗余的，所以大多数时候计算零空间会得到空（也就是说不存在零空间）。为了形象地展示零空间的效果，我不得已只用了平移的部分。以下代码展示了机械臂在（平移）零空间中的一种运动，如下图所示。可以看到，不管机械臂如何运动，末端连杆（局部坐标系）的位置始终不动（但是它的姿态会改变，矩阵mask的作用就是滤掉转动分量，只剩下沿x、y、zx、y、z轴的平移运动）。BodyJacobian函数用于计算本体雅可比矩阵，它也来自于Screws工具包[3][3]。零空间是个线性空间，如果我们知道它的一组基向量，通过线性组合能够得到任意的（速度）向量。NullSpace函数返回的刚好就是构成矩阵的零空间的一组基向量。通过对这组基向量线性组合即可得到速度向量，其使机械臂末端始终不动。下面的例子中使用的组合系数都是1，也就是所有基向量相加（向量相加就是对应元素相加，由Total函数完成）。由于雅可比矩阵是机械臂构型q的函数，所以机械臂的构型一旦改变了，我们就要重新计算它的雅可比矩阵。如果还不理解，可以随时显示出dq的值，然后计算Jgm.dq看看结果是不是零。如果是零就说明dq在零空间里。 q=ConstantArray[0,n];dt=0.05;g[I,L[7],0]=Last@robotPartsKinematics[{IdentityMatrix[4],q}];{xl,yl,zl,xr,yr,zr}=IdentityMatrix[6];mask={xl,yl,zl};Animate[Jb=BodyJacobian[T@{\[Xi]a,q},g[I,L[7],0]];gIL7=Last@robotPartsKinematics[{IdentityMatrix[4],q}];Jg=diagF[HToR[gIL7],HToR[gIL7]].Jb;Jgm=mask.Jg;dq=Total[NullSpace[Jgm]];(*速度定义为零空间的一种线性组合方式，你可以试试其它线性组合*)q=qdq*dt;Graphics3D[{robotMoveToQ[q],move3D[frame3D,g[I,L[7],0]]}],{i,1,1000,1}] 5.碰撞检测 RegionDisjoint函数可以用于二维几何图形，也可以用于三维几何体，甚至可以用于非凸的几何图形或几何体，如下面的例子所示。例子代码如下，其中使用了Locator控件。 Manipulate[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}};obstacle1=Disk[pt1,1];obstacle2=Polygon[pt2#&/@pts];color=If[RegionDisjoint[obstacle1,obstacle2],Green,Red];Graphics[{EdgeForm[Black],color,obstacle1,obstacle2},PlotRange->3],{{pt1,{-1,-1}},Locator},{{pt2,{1,1}},Locator}] 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:\\MOTOMANCH\\"<>partsName[[i]],R];GraphicsComplex[pts,poly],{i,7}];Graphics3D[robotPartsCH] 我们检验一下机械臂和外部障碍物的碰撞检测，至于机械臂连杆之间的碰撞我们暂时不考虑。代码如下，效果如下图所示。 Manipulate[Robs=cuboid[{1.3,0,0.5},{0.5,0.5,1.0}];(*障碍物，一个长方体*)gs=robotPartsKinematics[{IdentityMatrix[4],{q1,q2,q3,q4,q5,q6}}];Rparts=Table[MeshRegion[TransPt[gs[[i]]]/@robotParts[[i,1]],robotParts[[i,2,2]]],{i,7}];collisionQ=And@@(RegionDisjoint[Robs,#]&/@Rparts);color=If[collisionQ,Black,Red];text=If[collisionQ,"哈哈，没事","啊...碰撞了！"];Graphics3D[{Gray,Robs,Text[Style[text,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] 其中，TransPt[g][pt3D]函数的功能是用齐次变换矩阵g对三维向量（点）pt3D做变换，定义如下： TransPt[g_][pt3D_]:=Module[{homogeneousPt},homogeneousPt=Join[pt3D,{1.0}];homogeneousPt=g.homogeneousPt;homogeneousPt[[1;;3]]] 6.轨迹规划 轨迹规划的目的是得到机器人的参考运动轨迹，然后机器人再跟踪这条轨迹完成最终的动作。轨迹规划是机器人研究领域非常重要的一部分。机器人要干活就离不开运动，可是该如何运动呢？像搭积木、叠衣服、拧螺钉这样的动作对人类来说轻而易举，可要是让机器人来实现就非常困难。工业机器人既没有会思考的大脑，也缺少观察世界的眼睛（又瞎又傻），要让它们完全自主运动真是太难为它们了。它们所有的运动都是人教给它的。你可以把机器人想象成木偶，木偶看起来像是有灵魂的生物，但实际上它的运动都是人灌输的，木偶只会死板地按照人的控制运动，自己没有任何主动性，只是行尸走肉罢了。实际工厂中，是由工程师操作着控制面板，一点点调节机械臂的各个关节角度，让它到达某个位置。控制程序会记录机械臂的角度变化，只要工程师示教一次，机械臂就能精确而忠实地重复无数次。不过这种不得已而为之的方法实在是太笨了，如果有一种方法能够自动根据任务生成机器人的参考轨迹多好，下面我们将介绍一种常用的轨迹规划方法。6.1路径、轨迹——傻傻分不清楚 路径轨迹 如果我们画出红色和黑色轨迹的x,y,zx,y,z坐标分量，就会看到它们从同一位置出发，又在另一个位置碰头，却经历了不同的过程，如下图所示（注意红黑两组曲线的开始和结尾）。 制作上面的轨迹需要以下几个步骤： 1.首先随机生成一些三维空间中的点。 pts=RandomReal[{-1,1},{6,3}];(*6个三维坐标点*) 2.然后利用BSplineFunction函数对点插值。 bfun=BSplineFunction[pts]; 所得到的bfun是一个（B样条）插值函数，它的自变量的取值范围是0～10～1，你可以用ParametricPlot3D[bfun[t],{t,0,1}]画出这条曲线。 3.二次插值。我们虽然得到了插值函数，但它是一个向量值函数，难以进一步处理（比如求积分、微分）。所以，我们需要在bfun函数的基础上再处理。首先得到bfun函数图像上若干离散点（按照0.0010.001的间隔取）： bfpts=bfun/@Range[0,1,0.001]; 然后分别对xyzxyz坐标分量进行单独插值（这里我同样将自变量的取值范围设定在0～10～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]]}]]; 并定义一个新的插值函数为各分量的合成。这样我们就人为生成了一段轨迹（或者说，是一个向量值函数）。 ifun[t_]:={ifunx[t],ifuny[t],ifunz[t]} 我们能对这段轨迹做什么呢？●可以计算它的弧长，代码如下： ArcLength[ifun[t],{t,0,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]} 我们可以观察两种参数化的轨迹的图像： Animate[ParametricPlot3D[{ifun[t],ifunArc[t]},{t,0,end},PlotStyle->{{Thick,Black},{Thin,Dashed,Red}},PlotRange->1],{end,0.01,1,0.01}] 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"}}] 还可以制作任意你想要的路径，例如制作一条“文字轮廓”路径的代码如下： text=Text[Style["欢",Bold]];tg=DiscretizeGraphics[text,_Text,MaxCellMeasure->0.005];(*数值越小，点越密*)pts=MeshCoordinates[tg];pts=pts[[Last[FindShortestTour[pts]]]]; 然后将其显示出来。这里得到的是二维点的坐标，要想让我们的机械臂跟踪，需要将其转换为三维坐标，这很简单，你可以直接用替换命令：pts3D=pts/.{x_,y_}->{x,y,1.5}。 Manipulate[Graphics[{Line[pts[[1;;i]]]},PlotRange->10,GridLines->Automatic],{i,1,Length[pts],1}] 6.2轨迹规划方法 (*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=nearestPtstep*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}] RRT探索空间的能力还是不错的，例如下图左所示的例子，障碍物多而且杂乱（借助Mathematica本身具有的强大函数库，实现这个例子所需的所有代码应该不会超过30行）。还有没有环境能难住RRT呢？下图右所示的迷宫对RRT就是个挑战。这个时候空间被分割得非常严重，RRT显得有些力不从心了，可见随机策略不是什么时候都有效的。“随机”使得RRT有很强的探索能力。但是成也萧何败也萧何，“随机”也导致RRT很盲目，像个无头苍蝇一样到处乱撞。如果机器人对环境一无所知，那么采用随机的策略可以接受。可实际情况是，机器人对于它的工作环境多多少少是知道一些的（即使不是完全知道）。我的博客一直强调信息对于机器人的重要性。这些已知的信息就可以用来改进算法。一个改进的办法就是给它一双“慧眼”：在势场法中，势函数携带了障碍物和目标的信息，如果能把这个信息告诉RRT，让它在探索空间时有倾向地沿着势场的方向前进会更好。这样，RRT出色的探索能力刚好可以弥补势场法容易陷入局部极小值的缺点。 将RRT方法用在机械臂上的效果如下图所示（绿色表示目标状态）。我设置了4个障碍物（其中一个是大地），这对机械臂是个小小的挑战。由于我们生活在三维空间，没办法看到六维关节空间，所以我把六维关节空间拆成了两个三维空间，分别对应前三个关节和后三个关节（严格来说，六维转动关节空间不是一个欧式空间，它是个流形，不过这里我们不必纠结这个差别）： 正式RRT的主程序代码如下。我将RRT树定义为由节点列表nodes和树枝列表edges组成，即RRTtree={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={q0};(*以起点q0作为种子*)edges={};(*树枝初始为空*)RRTtree={nodes,edges};(*树的初始化由节点和树枝组成，它是全局变量*)maxIters=2000;(*最大迭代步数*)jointLims={{-Pi,Pi},{-Pi/2,Pi/2},{-Pi,1.45},{-Pi,Pi},{-2,2},{-Pi,Pi}};(*关节极限范围，有些关节值不可取*)qRandList=RandomPoint[Cuboid[ConstantArray[-Pi,n],ConstantArray[Pi,n]],maxIters,jointLims];(*一次性生成所有的随机点*)Do[nodes=RRTtree[[1]];If[Min[Norm/@((-qt#)&/@nodes)]<0.01,Print["哈哈，到达目标了!"];Break[]];qRand=RandomChoice[{qRandList[[i]],qt}];(*以50%的概率贪婪地着朝目标生长*)grow[qRand,step],{maxIters}]; collisionDetection[Rparts_,Robs_]:=And@@Flatten@Table[RegionDisjoint[i,#]&/@Rparts,{i,Robs}] 2.碰撞检测函数需要Region类型的变量，为此定义toRegion函数将几何体变换为Region类型，代码如下： toRegion[q_]:=Module[{gs,Rparts},gs=robotPartsKinematics[{IdentityMatrix[4],q}];Rparts=Table[MeshRegion[TransPt[gs[[i]]]/@robotParts[[i,1]],robotParts[[i,2,2]]],{i,7}]] 3.向RRT树中添加节点和边的函数： addNode[node_]:=Module[{},AppendTo[RRTtree[[1]],node];Length[RRTtree[[1]]]]addEdge[edge_]:=Module[{},AppendTo[RRTtree[[2]],edge];Length[RRTtree[[2]]]] 4.树枝朝着采样点生长（为了简单，只检测一点的碰撞情况）： grow[qRand_,step_:0.2]:=Module[{qNearestIdx,qNearest,growAngles},{qNearestIdx}=Nearest[nodes->Automatic,qRand,1];(*最近节点的代号*)qNearest=nodes[[qNearestIdx]];growDirection=Normalize[qRand-qNearest];qExpand=modToPiPi[qNeareststep*growDirection];(*生长*)Rrobot=toRegion[qExpand];If[collisionDetection[Rrobot,Robs],qNewIdx=addNode[qExpand];(*添加新节点，返回新节点的代号qNewIdx*)addEdge[{qNearestIdx,qNewIdx}]]] 下面的代码可以显示搜索到的（关节空间中的）路径。这条路径质量不高，如果用于机器人的轨迹跟踪还需要经过后期的平滑处理。 edges=RRTtree[[2]];targetIdx=edges[[-1,2]];qPath=backTrack[targetIdx];Animate[Graphics3D[{Robs,robotMoveToQ[qPath[[i]]]}],{i,1,Length[qPath],1}] 其中，backTrack函数用来从树中抽取出连接起点和目标点的路径： backTrack[nodeIdx_]:=Module[{searchNodeIdx=nodeIdx,nodes=RRTtree[[1]],edges=RRTtree[[2]],searchNodePos,preNodeIdx,pathIdx={},pathCoords},Do[{searchNodePos}=FirstPosition[edges[[All,2]],searchNodeIdx];preNodeIdx=edges[[searchNodePos,1]];AppendTo[pathIdx,preNodeIdx];If[preNodeIdx==1,Break[],searchNodeIdx=preNodeIdx],{Length[edges]}];pathIdx=Reverse[pathIdx];pathCoords=nodes[[pathIdx]]] 7.动力学仿真 7.1惯性参数 然后在上方“评估”选项卡中单击“质量属性”就会弹出如下图所示的对话框。 skew[v_]:={{0,-v[[3]],v[[2]]},{v[[3]],0,-v[[1]]},{-v[[2]],v[[1]],0}} 这组公式来自于[6][6]，我已经验证过了，是正确的（要想结果比较接近，这些惯性参数至少要取到小数点后5位，这依然是在“选项”页中设置）。我们得到了三个惯性张量，在动力学方程中我们应该使用哪个呢？下面的程序使用了ICIC，因为它是相对于绘图坐标系的，而我建立运动学时选择的局部坐标系就是绘图坐标系。（我以后在这里补充个单刚体动力学的例子）7.2正动力学仿真 如果给你一条轨迹，如何设计控制律让机械臂（末端）跟踪这条轨迹呢，控制律的跟踪效果怎么样呢？借助正动力学，我们就可以检验所设计的控制律。由于后面的程序所依赖的动力学推导过程[7][7]采用了相对自身的表示方法（也就是说每个连杆的速度、惯性、受力这些量都是相对于这个连杆自身局部坐标系描述的），旋量也是如此，为此需要重新定义旋量（代码如下）。其实旋量轴的方向没变（因为局部坐标系的姿态与全局坐标系一样），只是改变了轴上点的相对位置。 \[Xi]rb=Table[\[Omega]a[i]=axesDirInGlobal[[i]];la[i]=axesPtInGlobal[[i]]-drawInGlobal[[i1]];Join[Cross[-\[Omega]a[i],la[i]],\[Omega]a[i]],{i,n}]; 其中，ad函数用于构造一个李代数的伴随表达形式，代码如下。（开始我们定义的关节旋量是李代数，这里连杆的本体速度也是一个李代数，但加速度却不是。实际上，要想把加速度搞明白可不是那么容易的事[8][8]） ad[\[Xi]_]:=Module[{w,v},v=skew[\[Xi][[1;;3]]];w=skew[\[Xi][[4;;6]]];Join[Join[w,v,2],Join[ConstantArray[0,{3,3}],w,2]]] 正动力学的输入是关节力矩，下面我们为关节力矩设置不同的值，看看机械臂的表现：●如果令关节力矩等于零（即\[Tau][k]=0.0），机械臂将在唯一的外力——重力作用下运动，如下图所示。 只受重力的情况下，机械臂的总能量应该守恒。我们可以动手计算一下机械臂的能量（由动能和重力势能组成，代码如下）。将仿真过程中每一时刻的能量计算出来并保存在一个列表中，再画出图像，如下图所示。可见能量几乎不变（轻微的变化是由积分误差导致的，如果步长取的再小一些，就更接近一条直线），这说明机械臂的总能量确实保持恒定，这也间接证实了正动力学代码的正确性。这个简单的事实让人很吃惊——虽然机械臂的运动看起来那么复杂，但是它的能量一直是不变的。从力和运动的角度看，机械臂的行为变化莫测，可是一旦切换到能量的角度，它居然那么简洁。机械臂的运动方程和能量有什么关系呢？聪明的拉格朗日就是从能量的角度去推导动力学方程的。 totalEnergy=Total@Table[1/2*V[i].\[ScriptCapitalM][i].V[i]mass[i]*gravityAcc*g[I,L[i]][[3,4]],{i,n1}]; ●我们也可以让机械臂跟踪一条给定的轨迹，此时给定力矩为PD控制律： Kp=10000;Kd=50;(*PD控制系数，需要自己试探选取*)\[Tau][k]=Kp(qfun[k][t]-q[[k]])Kd(dqfun[k][t]-dq[[k]]); 其中，qfun和dqfun是4.2节中定义的插值函数，这里用作机械臂要跟踪的（关节空间中的）参考轨迹。跟踪一个圆的效果如下图所示。 Manipulate[disturb[t_]:=peakExp[-fat(t-tp)^2];Plot[disturb[t],{t,0,10},PlotRange->All],{peak,50,300,0.1},{fat,0.5,40,0.1},{tp,0,10,0.1},TrackedSymbols:>True] 把扰动加到第二个关节的力矩上 \[Tau][k]=Kp(0-q[[k]])Kd(0-dq[[k]])If[k==2,disturb[t],0]; 机械臂的响应如上右图所示，可见机械臂还是能回复零点姿势的。一开始机械臂有一个轻微的颤动，俗称“点头”。这是由于刚一开始机械臂的角度和角速度都为零，所以关节力矩也为零，导致机械臂缺少能够平衡重力的驱动力。在第5秒左右扰动出现，导致机械臂偏离了零点姿势，但在反馈控制作用下很快又回到了零点姿势。 7.3逆动力学仿真 输入力矩后，借助正动力学能得到关节角加速度，积分后可以得到角速度和角度。就像运动学和逆运动学的关系一样，逆动力学与正动力学刚好相反，它的用处是：如果告诉你机械臂的运动（也就是关节角度、角速度、角加速度），计算所需的关节力矩。 在重力作用下，机械臂保持“立正姿势”需要多大力矩呢？将初始状态设为0，经过逆动力学计算得到的答案是{0,38.1,38.1,0,2.06,0}{0,38.1,38.1,0,2.06,0}。如果把这个力矩再带入正动力学仿真就能看到机械臂保持静止不动，这证明我们的逆动力学模型也是正确的。8.结尾 本文以Mathematica通用数学计算软件为平台，针对串联机械臂的建模、规划和控制中的基本问题进行了仿真，差不多该说再见了。不过新的篇章即将展开——移动机器人是另一个有趣的领域。与固定的机器人相比，移动机器人更有挑战性，因此也会出现新的问题，比如信息的获取和利用。未来将加入移动机器人仿真的功能，支持地面移动机器人的运动控制、环境约束下的运动规划、移动机械臂、多机器人避障、多机器人编队运动等，并讨论环境建模、导航与定位、SLAM、非完整约束、最优控制、轮式车辆和履带车辆的动力学模型以及地面力学模型、群集协同运动等问题，敬请期待哟！