基於 Mathematica 的機器人仿真環境（機械臂篇）

目的 

　　本文手把手教你在 Mathematica 軟件中搭建機器人的仿真環境，具體包括以下內容（所使用的版本是 Mathematica 11.1，更早的版本可能缺少某些函數，所以請使用最新版。[e-mail]）。 
　　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到9的數字（這個機械臂的裝配體一共包含9個零件）。想要顯示導入的機器人模型可以使用以下代碼，顯示效果如下圖：

Graphics3D[{frame3D, robotParts}]

說明：frame3D是三維（右手）坐標系圖形，因為我們會用到很多坐標系及其變換，將坐標系顯示出來更直觀。定義 frame3D 的代碼如下。這個坐標系默認的位置在原點，我們稱這個坐標系為全局坐標系。

frame3D = {RGBColor[#], Arrowheads[0.03], Arrow@Tube[{{0, 0, 0}, 0.5 #}, 0.01]} & /@ IdentityMatrix[3]; (*改變數值可以改變坐標系的長度、坐標軸的粗細等顯示效果*)

 

　　你可能會好奇：導入的零件是以什么樣的格式存儲的呢？ 
　　 存儲機器人外形數據的robotParts 變量包含9個零件的數據，假如你想看第一個零件（對應的是基座，它通常用來將機械臂固定在大地上），可以輸入：

 

robotParts[[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]}]

 

　　機器人的所有零件都成功地導入了，而且它們的相對位置也是正確的。你可能會問：機械臂為什么是處於“躺着”的姿勢呢？這是由於零件是按照 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]; (*注意第二次變換是在右邊乘*)

　　然后用 rot 矩陣旋轉每個零件（的坐標，即保存在第一部分 robotParts[[i, 1]] 中的數據）：

robotParts=Table[GraphicsComplex[rot.# & /@ robotParts[[i, 1]], robotParts[[i, 2]]], {i, 9}];

　　經過姿態變換后的機器人看起來舒服點了，只是有些蒼白。為了給它點個性（也方便區分零件），我們給機械臂設置一下顏色，代碼如下。你可能注意到了，這里我沒有使用循環去為9個零件一個一個地設置顏色，而是把相同的元素（顏色）寫在一起，這樣做的好處就是代碼比較簡潔、清晰。以后我們會經常這么做。

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

 

說明：現在的機器人姿勢（大臂豎直、小臂前伸）是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;

　　因為我們是在 SolidWorks 中測量的位置，所以這些位置值還是相對於 SolidWorks 的坐標系（ 軸朝上），要變到  軸朝上，方法仍然是乘以旋轉矩陣 rot：

drawInGlobal = Table[rot.i, {i, drawInGlobalSW}];

　　以后會經常用到對坐標的旋轉變換，而且多數時候是用一個旋轉矩陣同時對很多坐標進行變換（例如上面的這個例子），我們不如定義一個算子以簡化繁瑣的代碼。我們定義算子（其實是一個函數）：

CircleDot[Matrix_,Vectors_]:=(Matrix.#)&/@Vectors;

　　所以前面的變換用我們自定義的算子表示就是（復制到 Mathematica中后 \[CircleDot] 會變成一個Mathematica內部預留的圖形符號，這個符號沒有被占用，所以這里我征用了）：

drawInGlobal = rot\[CircleDot]drawInGlobalSW;　(*哈哈！寫起來是不是簡單多了*)

　　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};

　　移動后的零件模型如下圖所示（圖中的坐標系是各個零件自己的繪圖坐標系，我沒有對數據轉動，所以繪圖坐標系和全局坐標系的姿態相同）。我們一開始從 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}];

　　我們對關節的運動進行了建模，要建立運動學還缺少最后一樣東西：零件間的初始相對位姿（初始的意思是機械臂處於“零位”的狀態下）。零位下，我們將所有零件的姿態都認為和全局坐標系一樣，所以不用計算相對姿態了。至於它們的相對位置嘛，我們已經知道了繪圖坐標系原點在全局坐標系中的坐標，兩兩相減就可以得到它們的相對位置了，很簡單吧！（見下面的代碼）　　

Do[g[L[i], L[i + 1], 0] = PToH[drawInGlobal[[i + 1]] - drawInGlobal[[i]]], {i, n}];

　　其中，PToH 函數能將位置向量轉換為一個齊次變換矩陣，這是借助 RPToH 函數實現的（RPToH 函數就是 Screws 工具包中的RPToHomogeneous 函數），它可以將一個旋轉矩陣和位移向量組合成一個齊次變換矩陣。將旋轉矩陣和位移向量合成為齊次變換矩陣是我們以后會經常用到的操作。類似的，也可以定義 RToH 函數將旋轉矩陣生成對應的齊次變換矩陣，代碼如下：

1.  
   RToH[R_]:= RPToH[R, {0,0,0}]
2.  
   PToH[P_]:= RPToH[ IdentityMatrix[3],P]

說明：本文中，用符號 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] ]

　　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}]

move3D[shape_,g_]:=GeometricTransformation[shape,TransformationFunction[g]];

　　驗證使用的代碼如上。其中，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　(*逆運動學方程*)

　　通過解上面這個以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.  
   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]

 

　　 不過這個方程組是由  齊次變換矩陣得到的，里面有  個方程，去掉最后一列對應的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]

　　同樣借助 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]

　　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];

　　其中，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 *)

　　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]]]

　　我們以后會用到很多矩陣操作（比如轉置、求逆）。而 Mathematica 的函數名太長，為了寫起來方便，我定義了簡寫的轉置和求逆函數，代碼如下：

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

　　我們想讓機械臂（的末端）依次到達一些空間點（這些點可能是機械臂運動時要經過的）。為此首先生成一些三維空間中的點：

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]]

　　然后調用逆運動學函數 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 作為下一次迭代的初始值，這樣迭代速度更快*)

　　計算結果 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.  
   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}}]

 

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}]

 

 

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]

　　不過有了 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.  
   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]

　　其中，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]] ]

 

 

6. 軌跡規划

　　軌跡規划的目的是得到機器人期望的參考運動軌跡，然后機器人控制器再跟蹤這條參考軌跡完成最終的動作，它是機器人領域非常重要的一部分。機器人要干活就離不開運動，可是該如何運動呢？像搭積木、疊衣服、擰螺釘這樣的動作對人類來說輕而易舉，可要是讓機器人來實現就非常困難。工業機器人既沒有會思考的大腦，也缺少觀察世界的眼睛（又瞎又傻），要讓它們自己運動真是太難為它們了。它們所有的運動都是人教給它的。你可以把機器人想象成木偶，它的運動都是人灌輸的。實際工廠中，是由工程師操作着控制面板，一點點調節機械臂的各個關節角度，讓它到達某個位置。控制程序會記錄機械臂的角度變化，只要工程師示教一次，機械臂就能精確而忠實地重復無數次。不過這種不得已而為之的方法實在是太笨了。如果有一種方法能夠自動根據任務生成機器人的參考軌跡多好，下面我們將介紹一種常用的軌跡規划方法。 
　　 
6.1 路徑、軌跡——傻傻分不清楚

　　“軌跡”是什么？要理解軌跡可離不開路徑。路徑（Path）和軌跡（Trajectory）是兩個相似的概念，它們的區別在於： 
　　●　路徑只是一堆連續空間坐標，它不隨時間變化。例如下圖左側的三維曲線就是一段路徑。 
　　●　軌跡是運動的坐標，它是時間的函數，一個時刻對應一個空間坐標點。軌跡包含的信息更多，我們可以對它微分得到速度、加速度等等信息，而路徑是沒有這些的。下圖右側展示了兩條軌跡，它們雖然經過相同的路徑，但卻具有不同的速度——黑色軌跡開始運動較快，隨后被紅色反超，最后二者又同時到達終點。 

　　　　　　　　　　　　路徑　　　　　　　　　　　　　　　軌跡 
　　如果我們畫出紅色和黑色軌跡的  坐標分量，就會看到它們從同一位置出發，又在另一個位置碰頭，卻經歷了不同的過程，如下圖所示（注意紅黑兩組曲線的開始和結尾）。 

　　制作上面的軌跡需要以下幾個步驟： 
　　1. 首先隨機生成一些三維空間中的點。

 

pts = RandomReal[{-1,1},{6,3}]; (*6個三維坐標點*)

　　2. 然后利用 BSplineFunction 函數對點插值。

bfun = BSplineFunction[pts];

　　所得到的 bfun 是一個（ B 樣條曲線）插值函數，它的自變量的取值范圍是 0∼1，你可以用ParametricPlot3D[bfun[t], {t, 0, 1}] 畫出這條曲線。 

 

　　3. 二次插值。我們雖然得到了插值函數，但它是一個向量值函數，難以進一步處理（比如求積分、微分）。所以，我們需要在bfun 函數的基礎上再處理。首先得到 bfun 函數圖像上若干離散點（按照 0.001的間隔取）：

bfpts = bfun /@ Range[0, 1, 0.001]; 

　　然后分別對各坐標軸進行單獨插值（這里我同樣將自變量的取值范圍設定在 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]]}]];

　　並定義一個新的插值函數為各分量的合成。這樣我們就人工制作了一段軌跡（或者說，是一個向量值函數）。

ifun[t_] := {ifunx[t], ifuny[t], ifunz[t]}

　　我們能對這段軌跡做什么呢？ 
　　●　可以計算它的弧長：

ArcLength[ifun[t], {t, 0, 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]}

　　我們可以觀察兩種參數化的軌跡的圖像：

Animate[ParametricPlot3D[{ifun[t], ifunArc[t]}, {t, 0, end}, PlotStyle -> {{Thick, Black}, {Thin, Dashed, Red}}, PlotRange -> 1], {end, 0.01, 1, 0.01}]

　　我們說軌跡比路徑包含更多的信息，可是如果單看路徑，我們能提取出什么信息呢？ 
　　路徑只包含幾何信息：對於一個三維空間中的路徑（曲線），我們能計算路徑上每一點的切線和法線，它們剛好能唯一地確定一個直角坐標系（這個坐標系又被稱為 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"}}]

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}]

　　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. 首先是碰撞檢測函數 collisionDetection，如果機械臂沒有碰到障礙物就返回True。為了節省時間，碰撞檢測使用的是機械臂各零件的凸包，在最好的顯示時才使用原始零件。

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

　　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}]]

　　3. 向RRT樹中添加節點和邊的函數：

1.  
   addNode[node_]:= Module[{}, AppendTo[RRTtree [[1]], node]; Length[RRTtree[[1]]] ]
2.  
   addEdge[edge_]:= Module[{}, AppendTo[RRTtree [[2]], edge]; Length[RRTtree[[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}]] ]

　　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.  
   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}]

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}}

　　這組公式來自於，我已經驗證過了，百分百正確，不信的話你也可以試試（要想結果比較接近，這些參數至少要取到小數點后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.  
   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 (*顯示計算耗時*)

　　其中， 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]] ]

　　正動力學的輸入是關節力矩，下面我們為關節力矩設置不同的值，看看機械臂的表現： 
　　●　如果令關節力矩等於零（即 \[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];

 

　　●　我們也可以讓機械臂跟蹤一條給定的軌跡，此時給定力矩為 PD 控制率：

 

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

　　其中，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]

 

　　把擾動加到第二個關節的力矩上

 

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

　　機械臂的響應如下圖所示，可見機械臂還是能回復零點姿勢的。一開始機械臂有一個輕微的顫動，俗稱“點頭”。這是由於剛一開始機械臂的角度和角速度都為零，所以關節力矩也為零，導致機械臂缺少能夠平衡重力的驅動力。在第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 (*監視計算時間*)

　　在重力作用下，機械臂保持“立正姿勢”需要多大力矩呢？將初始狀態設為 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 一樣的變量監控窗口可以實時看到變量的值，這在調試時顯得很不方便。