逆运动学 双臂协作搬运
显示运动痕迹 （平移）零空间运动
无论你是从事机器人研发还是教学科研，一款好用的仿真软件能对你的工作起到很大的帮助。那么应该选择哪种仿真软件呢？最方便的选择就是现成的商业软件（例如 Webots、Adams）。你的钱不是白花的，商业软件功能完善又稳定，而且性能一般都经过了优化。可是再强大的商业软件也无法面面俱到。从学习和研究的角度出发，最好选择代码可修改的开源或半开源软件。
在论文数据库中简单检索一下就会发现，很多人都选择在 Matlab 的基础上搭建仿真环境。这并不奇怪，因为 Matlab 具有优秀的数值计算和仿真能力，使用它开发会很便利。如果你非要舍近求远，用 C++ 编写一套仿真软件，先不要说仿真结果如何显示，光是矩阵计算的实现细节就能让你焦头烂额（本来我们要造一辆汽车，可是制作车轮就耗费了大量的精力，而实际上车轮直接买现成的就行了）。
MatlabMathematica可视化效果一般优秀导入机器人模型需要自己写函数自带函数机器人工具箱（包）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]]; (*提取出三维图形的几何数据：顶点的三维坐标和边*)1234512345这里我偷了个懒，为了少打些字，我把导出零件的文件名改成了从1到9的数字（这个机械臂的装配体一共包含9个零件）。想要显示导入的机器人模型可以使用以下代码，显示效果如下图：
Graphics3D[{frame3D, robotParts}]11说明：frame3D是三维（右手）坐标系图形，因为我们会用到很多坐标系及其变换，将坐标系显示出来更直观。定义 frame3D 的代码如下。这个坐标系默认的位置在原点，我们称这个坐标系为全局坐标系。
frame3D = {RGBColor[#], Arrowheads[0.03], Arrow@Tube[{{0, 0, 0}, 0.5 #}, 0.01]} & /@ IdentityMatrix[3]; (*改变数值可以改变坐标系的长度、坐标轴的粗细等显示效果*)11 你可能会好奇：导入的零件是以什么样的格式存储的呢？
存储机器人外形数据的robotParts 变量包含9个零件的数据，假如你想看第一个零件（对应的是基座，它通常用来将机械臂固定在大地上），可以输入：
robotParts[[1]] (*双层方括 中的数字表示对应第几个零件*)11运行后的输出结果是一堆由 GraphicsComplex 函数包裹着的数字，稍加分辨会发现这些数字又包含两部分：第一部分是零件所有顶点的三维坐标；第二部分是组成零件外形的三角形（构成每个三角形的三个顶点是第一部分点的序 ，而不是坐标）。我们可以用以下代码将其分别显示出来：
pts = robotParts[[1, 1]]; (*第一部分：顶点的三维坐标数据*)triangles = robotParts[[1, 2]]; (*第二部分：三角形面片*)trianglesB = triangles /. {EdgeForm[] -> EdgeForm[Blue]}; (*三角形的边显示为蓝色 Blue*)Graphics3D[{Red, Point[pts], White, GraphicsComplex[pts, trianglesB]}]12341234 机器人的所有零件都成功地导入了，而且它们的相对位置也是正确的。你可能会问：机械臂为什么是处于“躺着”的姿势呢？这是由于零件是按照 SolidWorks 默认的坐标系（ 轴向上）绘制和装配的。而在 Mathematica 中默认的坐标系是 轴向上。那么我们应该采用哪个坐标系呢？
当然你可以任性而为，用哪个都可以。不过根据国家标准《GBT 16977-2005 工业机器人 坐标系和运动命名原则》，基座坐标系的 轴应该垂直于机器人基座安装面（也就是地面）、朝向为重力加速度的反方向， 轴指向机器人工作空间中心点。制定国标的都是些经验丰富的专家老手，我们最好跟国标保持一致（国标的作图水平就不能提高点吗？这图怎么感觉像小学生画的）。
为了让机器人变成国标规定的姿势，需要旋转各个零件。我们先想想应该怎么转：结合我们之前导入的图形，可以先绕全局坐标系的大 轴转 ，再绕全局坐标系的 轴转 。另一种方法是：先绕全局坐标系的 轴转 （记这个旋转后的坐标系为 ），再绕 的 轴转 。两种方法的效果是一样的，但是注意合成矩阵时乘法的顺序（见以下代码），不懂的同学可以看看文献中的3133页。当然，转动是有正负之分的：将你的右手握住某个坐标轴，竖起大拇指，让大拇指和轴的正方向一致，这时四指所示的方向就是绕该轴转动的正方向。
为此，定义旋转矩阵：
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]; (*注意第二次变换是在右边乘*)123123然后用 rot 矩阵旋转每个零件（的坐标，即保存在第一部分 robotParts[[i, 1]] 中的数据）：
robotParts=Table[GraphicsComplex[rot.# & /@ robotParts[[i, 1]], robotParts[[i, 2]]], {i, 9}];11经过姿态变换后的机器人看起来舒服点了，只是有些苍白。为了给它点个性（也方便区分零件），我们给机械臂设置一下颜色，代码如下。你可能注意到了，这里我没有使用循环去为9个零件一个一个地设置颜色，而是把相同的元素（颜色）写在一起，这样做的好处就是代码比较简洁、清晰。以后我们会经常这么做。
colors = {Gray, Cyan, Orange, Yellow, Gray, Green, Magenta, Lighter[Green], Pink}; (*1~9 各零件的颜色*)robotPartsColored = Transpose[{colors, robotParts}]; (*把颜色分配给各零件*)Graphics3D[robotPartsColored]123123说明：现在的机器人姿势（大臂竖直、小臂前伸）是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;11因为我们是在 SolidWorks 中测量的位置，所以这些位置值还是相对于 SolidWorks 的坐标系（ 轴朝上），要变到 轴朝上，方法仍然是乘以旋转矩阵 rot：
drawInGlobal = Table[rot.i, {i, drawInGlobalSW}];11以后会经常用到对坐标的旋转变换，而且多数时候是用一个旋转矩阵同时对很多坐标进行变换（例如上面的这个例子），我们不如定义一个算子以简化繁琐的代码。我们定义算子（其实是一个函数）：
CircleDot[Matrix_,Vectors_]:=(Matrix.#)&/@Vectors;11所以前面的变换用我们自定义的算子表示就是（复制到 Mathematica中后 [CircleDot] 会变成一个Mathematica内部预留的图形符 ，这个符 没有被占用，所以这里我征用了）：
drawInGlobal = rot[CircleDot]drawInGlobalSW; (*哈哈！写起来是不是简单多了*)11还有印象吗？最开始导出和导入零件模型时，各零件的位置都已经按照装配关系确定好了，所以它们的数据也是相对于全局坐标系描述的。可是现在我们要让机械臂动起来（而且还要显示出来），这就要移动这些数据。为了方便起见，最好能将每个零件的模型数据表示在自己的绘图坐标系中，因为这样我们只需要移动绘图坐标系就行了，而各点的数据相对它们所属的绘图坐标系是不同的。应该怎么做呢？很简单，将零件模型的数据减去绘图坐标系的原点在全局坐标系中的坐标即可：
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};12345671234567移动后的零件模型如下图所示（图中的坐标系是各个零件自己的绘图坐标系，我没有对数据转动，所以绘图坐标系和全局坐标系的姿态相同）。我们一开始从 SolidWorks 导出文件时是一次性地导出整个装配体的。其实，如果我们挨个打开各个零件并且一个一个的导出这些零件，那么得到数据就是相对于各自的绘图坐标系的，只不过这样稍微麻烦一点。
3.2 利用旋量建立运动学模型
定义关节旋量的代码如下。其中相对旋量 用于递归运动学计算，它的含义是当前连杆的转轴表示在前一个连杆坐标系中。
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}];12341234我们对关节的运动进行了建模，要建立运动学还缺少最后一样东西：零件间的初始相对位姿（初始的意思是机械臂处于“零位”的状态下）。零位下，我们将所有零件的姿态都认为和全局坐标系一样，所以不用计算相对姿态了。至于它们的相对位置嘛，我们已经知道了绘图坐标系原点在全局坐标系中的坐标，两两相减就可以得到它们的相对位置了，很简单吧！（见下面的代码）
Do[g[L[i], L[i + 1], 0] = PToH[drawInGlobal[[i + 1]] - drawInGlobal[[i]]], {i, n}];11其中，PToH 函数能将位置向量转换为一个齐次变换矩阵，这是借助 RPToH 函数实现的（RPToH 函数就是 Screws 工具包中的RPToHomogeneous 函数），它可以将一个旋转矩阵和位移向量组合成一个齐次变换矩阵。将旋转矩阵和位移向量合成为齐次变换矩阵是我们以后会经常用到的操作。类似的，也可以定义 RToH 函数将旋转矩阵生成对应的齐次变换矩阵，代码如下：
RToH[R_]:= RPToH[R,{0,0,0}]PToH[P_]:= RPToH[IdentityMatrix[3],P]1212robotPartsKinematics[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] ]1234512345Manipulate[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}]12341234move3D[shape_,g_]:=GeometricTransformation[shape,TransformationFunction[g]];11 验证使用的代码如上。其中，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. 逆运动学仿真
4.1 数值解法之——解方程
上一节的运动学函数 robotPartsKinematics 能得到所有连杆的位姿。大多数时候，人们只关心最后一个连杆的位姿（因为它上面要装载操作工具），即ULast@robotPartsKinematics[{IdentityMatrix[4], q}]（注意q是一个六维向量，即q=()），结果如下图所示（另存为可以看大图）。这里关节角没有设置数值，因此得到的是符 解，有些长哦。这也是为什么机器人领域经常使用缩写的原因：比如把 记为。在中提供了一个函数 SimplifyTrigNotation，可以用来对下式进行缩写。
如果我们想让机械臂末端（连杆）到达某个（已知的）位姿一gt，也就是让上面的矩阵等于这个位姿矩阵：
Last@robotPartsKinematics[{IdentityMatrix[4], {q1, q2, q3, q4, q5,
声明：本站部分文章及图片源自用户投稿，如本站任何资料有侵权请您尽早请联系jinwei@zod.com.cn进行处理,非常感谢！