?

目的
　　本文手把手教你在 Mathematica 科學計算軟件中搭建機器人的仿真環(huán)境，具體包括以下內容：
　　 1　導入機械臂的三維模型
　　 2　正\逆運動學仿真
　　 3　碰撞檢測
　　 4　軌跡規(guī)劃
　　 5　正\逆動力學仿真
　　 6　運動控制
　　文中的代碼和模型文件點擊此處下載，或者此處：https://github.com/robinvista/Mathematica 。使用的軟件版本是 Mathematica 11.1，較早的版本可能缺少某些函數(shù)，所以最好使用最新版。進入正文之前不妨先看幾個例子：

?

　　　　　　　　　　　　　逆運動學　　　　　　　　　　　　　　　　　　　　雙臂協(xié)作搬運

　　　　　　　　　　顯示運動痕跡　　　　　　　　　　　　　　　（平移）零空間運動

　　無論你從事的是機器人研發(fā)還是教學科研，一款好用的仿真軟件都能對你的工作起到很大的幫助。那么應該選擇哪個軟件呢？最方便的選擇就是成熟的商業(yè)軟件，例如Adams、Webots。你的錢不是白花的，商業(yè)軟件功能強大又穩(wěn)定，而且性能一般都經過了優(yōu)化?？墒窃購姶蟮纳虡I(yè)軟件也有設計不合理的地方，它們的算法基本都是“黑箱”，你想做一點更改都不行。從學習和研究的角度出發(fā)，最好選擇代碼可修改的開源或半開源軟件。
　　在論文數(shù)據(jù)庫中檢索一下就會發(fā)現(xiàn)，很多人都選擇借助Matlab這個數(shù)學軟件平臺進行機器人的建模仿真[1] [1]。這并不奇怪，因為Matlab具有優(yōu)秀的數(shù)值計算和仿真能力，在它的基礎上開發(fā)會很便利。如果你非要舍近求遠，用 C 編寫一套仿真軟件，先不要說仿真結果如何顯示，光是矩陣計算的實現(xiàn)細節(jié)就能讓你焦頭爛額。

?

?

　　與大名鼎鼎的Matlab 相比，Mathematica在國內知名度并不高，但是不要小看它哦，一旦熟悉了你會刮目相看。我簡單對比了一下二者在機器人仿真方面的特點，見下表。由于Mathematica不俗的表現(xiàn)，我選擇在它的基礎上搭建仿真環(huán)境。如果你對Mathematica不熟悉，可以看網(wǎng)絡教程，也可以參考我的學習筆記。Mathematica有著陡峭的學習曲線，入門并不容易，其實初學者最快的入門方法就是照著大量的例子演練。本文面向Mathematica的初學者，所以不會使用過于高超的編程技巧。

?

1. 獲取機器人的外觀模型

?

　　制作逼真的仿真首先需要的是機器人的外觀模型。如果你有機器人的三維模型最好，沒有的話也不要緊，著名的機器人制造商都在官網(wǎng)提供其各種型號機器人的真實模型，例如 ABB、安川，供大家免費下載和使用。為了防止山寨，這些模型都是不可通過建模軟件直接修改的格式，例如 IGES 和 STEP 格式。但我們只用來顯示和碰撞檢測，所以并不影響仿真。

?

?

2. 導入機器人的外觀模型

?

　　獲得模型后要導入Mathematica中進行處理并顯示，下面我們借助一個例子說明具體的步驟。Motoman ES165D 是安川公司生產的一款6自由度點焊機器人，其最后三個關節(jié)軸線相交于一點，這是一種非常經典而且有代表性的設計，因此我們選擇以這款機器人為例進行講解（這個機器人的完整模型點擊此處下載）。

　　用SolidWorks 2014軟件打開機器人的裝配體文件，并選擇“另存為”STL 格式。然后打開當前頁面下方的“選項”對話框，如下圖。這里我們要設置三個地方：
　　1. “品質”表示對模型的簡化程度，如果你想實現(xiàn)非常逼真的效果，可以選擇“精細”，但缺點是數(shù)據(jù)點很多導致文件很大、處理起來比較慢。一般選擇“粗糙”就夠了；
　　2. 勾選“不要轉換 STL 輸出數(shù)據(jù)到正的坐標空間”；
　　3. 不要勾選“在單一文件中保存裝配體的所有連桿”。

?

?

?

小技巧　STL格式是一種三維圖形格式，被很多三維建模軟件支持（Mathematica也支持，所以我們要保存為這個格式）。STL格式只存儲三維圖形的表面信息，而且是用很多的三角形對圖形進行近似的表示。如果你的機器人外形比較簡單（規(guī)則的幾何體），那么得到的STL文件大小一般只有幾十KB ；可是如果外形很復雜（比如包含很多的孔洞、曲面），生成的STL文件會很大（幾MB～幾十MB）。對于一般配置的計算機，模型文件超過100KB用Mathematica處理起來就比較慢了。為了讓仿真顯示地更流暢，可以利用免費軟件MeshLab對其進行化簡，MeshLab通常能夠在基本不改變外形的前提下將文件大小縮減為原來的十分之一甚至更多。
　　MeshLab的安裝和操作都是傻瓜式的，打開后進入如下圖左所示的菜單中，出現(xiàn)右圖的頁面，這里的“Percentage reduction”表示縮減的百分比（1 表示不縮減，0.1 則表示縮減為原來的10%），設置好后點擊Apply并保存即可。

?

?

　　然后在 Mathematica中導入 STL 文件，使用的代碼如下（假設這些 STL 文件保存在 D:\MOTOMAN 文件夾下）：

?

SetDirectory["D:\\MOTOMAN"]; (*設置文件存放的地址，注意次級路徑用雙斜杠*) 
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]]; (*從三維圖形中提取出幾何數(shù)據(jù)：頂點的三維坐標和邊*)

?

?

　　這里我偷了個懶，為了少打些字，我把導出連桿的文件名改成了從1到9的數(shù)字（這個機械臂的裝配體一共包含9個零件）。我們把導入的模型顯示出來，效果如下圖。使用的代碼如下

?

Graphics3D[{frame3D, robotParts}]

?

?

說明：frame3D是三維坐標系的三個正交的軸（xyz xyz軸的顏色分別是RGB RGB）。在機器人領域會用到大量的坐標系及其變換，直接看數(shù)字總是不直觀，不如將坐標系顯示出來更方便。定義 frame3D 的代碼如下，這個坐標系默認原點的位置在(0,0,0) (0,0,0)，以后我們稱這個坐標系為“全局坐標系”。

?

frame3D = {RGBColor[#], Arrowheads[0.05], Arrow[Tube[{{0, 0, 0}, #}, 0.01]]} & /@ IdentityMatrix[3];

?

?

?

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

?

robotParts[[1]]  (*雙層方括號中的數(shù)字表示對應第幾個零件*)

?

?

　　運行后的輸出結果是一堆由 GraphicsComplex 函數(shù)包裹著的數(shù)字，主要可分為兩部分：第一部分是零件幾何體所有頂點的三維坐標；第二部分是組成零件幾何體的三角形（注意：構成每個三角形的三個頂點是第一部分點的序號，而不是坐標值）。我們可以用以下代碼將其分別顯示出來：

?

pts = robotParts[[1]][[1]]; (*零件1的第一部分：頂點的三維坐標數(shù)據(jù)*)
triangles = robotParts[[1]][[2]]; (*零件1的第二部分：三角形面片*)
trianglesBlue = triangles /. {EdgeForm[] -> EdgeForm[Blue]}; (*三角形的邊顯示為藍色*)
Graphics3D[{GraphicsComplex[pts, trianglesBlue], Red, Point[pts]}]

?

?

?

　　所有零件都成功地導入了，而且它們的相對位置也是正確的。你可能會問：機械臂為什么是處于“躺著”的姿勢呢？這是由于零件是按照 SolidWorks 默認的坐標系（y y 軸向上）繪制和裝配的。而在 Mathematica 中默認的坐標系是 z z 軸向上。那么我們采用哪個坐標系呢？
　　當然你可以任性而為，用哪個都可以。不過根據(jù)國家標準GBT 16977-2005 《工業(yè)機器人 坐標系和運動命名原則》，機械臂基座坐標系的 z z 軸應該垂直于基座安裝面（一般是水平地面）、指向為重力加速度的反方向（也就是垂直地面向上）， x x 軸指向機器人工作空間中心點的方向。制定國家標準的都是些經驗豐富的專家老手，我們最好跟國標保持一致（國標的作圖水平就不能提高點嗎？這圖怎么感覺像小學生畫的）。

?

?

　　為了讓機器人變成國標規(guī)定的姿勢，需要旋轉各個連桿。我們先想想應該怎么轉：結合我們之前導入的圖形，可以先繞全局坐標系的 x x 軸轉 90° 90°，再繞全局坐標系的 z z 軸轉 90° 90°。還有一種方法是：先繞全局坐標系的 x x 軸轉 90° 90°（記這個旋轉后的坐標系為 A A），再繞 A A 的 y y 軸轉 90° 90°。兩種方法的效果是一樣的，但是注意合成矩陣時乘法的順序（見以下代碼），不懂的同學可以看看文獻[5] [5]中的31 ～ ～ 33頁。當然，轉動是有正負之分的：將你的右手握住某個坐標軸，豎起大拇指，讓大拇指和軸的正方向一致，這時四指所示的方向就是繞該軸轉動的正方向。
　　為此，我們定義旋轉矩陣（兩種定義效果一樣）：

?

{Xaxis,Yaxis,Zaxis}=IdentityMatrix[3]; (*定義三個旋轉軸*)
rot = RotationMatrix[90 Degree, Zaxis].RotationMatrix[90 Degree, Xaxis]; (*注意第二次變換是在左邊乘*)
rot = RotationMatrix[90 Degree, Xaxis].RotationMatrix[90 Degree, Yaxis]; (*注意第二次變換是在右邊乘*)

?

?

　　然后用rot矩陣旋轉每個連桿（的坐標，即保存在第一部分robotParts[[i, 1]]中的數(shù)據(jù)）：

?

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

?

?

　　經過姿態(tài)變換后的機器人看起來舒服點了，只是有些蒼白。為了給它點個性（也方便區(qū)分不同的零件或者稱為連桿），我們給連桿設置一下顏色，代碼如下。你可能注意到了，這里我沒有使用循環(huán)去為9個連桿一個一個地設置顏色，而是把同類的元素（顏色）寫在一起，然后再和連桿列表一起轉置即可把顏色“分配”給各個連桿。這樣做的好處就是代碼比較簡潔、清晰，以后我們會經常這么做。

?

colors = {Gray, Cyan, Orange, Yellow, Gray, Green, Magenta, LightGreen, Pink}; (*1~9 各連桿的顏色*)
robotPartsColored = Transpose[{colors, robotParts}]; (*把顏色分配給各連桿*)
Graphics3D[robotPartsColored]

?

?

?

說明：現(xiàn)在的機器人姿勢（大臂豎直、小臂前伸）是6自由度機械臂的“零位”狀態(tài)，我們將此時機械臂各關節(jié)的角度認為是0。一般機械臂上都有各關節(jié)的零點位置標記，用于指示各關節(jié)的零點。我們用控制器控制機械臂時，發(fā)出的角度指令都是相對于這個零點位置的。零點位置不是必須遵守的，你可以將任意的角度設置為零位，不過為了統(tǒng)一，最好用機械臂固有的零位——也就是當前姿勢下各關節(jié)的角度。

?

3. 運動學仿真

?

　　前面的工作只是讓機械臂的模型顯示出來。如果想讓它動起來，那就要考慮運動學了。機器人這個學科聽起來高大上（很多都停留在理論上），可實際上現(xiàn)在大多數(shù)工業(yè)機器人的控制方式還是比較低級的，它們只用到了運動學，高級一點的動力學很少用，更不要提智能了（它們要說自己有智能，我們家的洗衣機和電視機都要笑掉大牙了）?？磥硪褂脵C器人，運動學是必不可少的，所以我們先來實現(xiàn)運動學。
　　在建立運動學模型之前我們需要了解機器人的機械結構。前面提到，MOTOMAN-ES165D 是一個6自由度的串聯(lián)機械臂。而6個自由度的機器人至少由7個連桿組成（其中要有一個連桿與大地固定，也就是基座）?？墒俏覀儗氲倪B桿有9個，多出來的2個連桿是彈簧缸（基座上黃色的圓筒）的組成部分。MOTOMAN-ES165D 機器人能夠抓持的最大負載是165公斤，彈簧缸的作用就是作為配重平衡掉一部分負載的重量，要不然前端的關節(jié)電機會有很大的負擔?？墒菑椈筛捉o我們的建模造成了麻煩，因為它導致“樹形拓撲”以及存在“閉鏈”，這不太好處理。為此，我們先忽略掉彈簧缸。
　　
3.1 連桿的局部坐標系

?

　　機器人的運動也就是其構成連桿的運動。為了表示連桿的運動，我們要描述每個連桿的位置和姿態(tài)（合稱為“位姿”）。通常的做法是在每個連桿上固定一個坐標系（它跟隨連桿一起運動），這個坐標系被稱為“局部坐標系”。通過描述局部坐標系的位姿我們就可以描述每個連桿的位姿。如何選擇局部坐標系呢？理論上你可以任意選擇，不過局部坐標系影響后續(xù)編程和計算的難易程度，所以我們在選擇時最好慎重。在運動學建模和動力學建模中，坐標系的選擇通常是不同的：
　　●　運動學建模時，連桿的局部坐標系一般放置在關節(jié)處，這是因為常用的 D-H 參數(shù)是根據(jù)相鄰關節(jié)軸定義的；
　　●　動力學建模時，連桿的局部坐標系一般放置在質心處，這是因為牛頓方程是關于質心建立的，而且關于質心的轉動慣量是常數(shù)，這方便了計算。
　　我們先考慮運動學，因此將局部坐標系設置在關節(jié)處。在SolidWorks中打開任何一個連桿，都能看到它自己有一個坐標系。描述一個連桿的每一條邊、每一個孔的坐標都以這個坐標系為參考，我們稱它為“繪圖坐標系”。繪圖坐標系通常不在質心處，因為在你還沒畫完連桿之前你根本不知道它的質心在哪里。繪圖坐標系通常在連桿的對稱中心或者關節(jié)處，我們不妨將每個連桿的繪圖坐標系當做它的局部坐標系。
　　那么下一個問題是每個連桿的繪圖坐標系在哪兒呢？我們以第三個連桿為例說明，如下圖左所示，點擊SolidWorks左側的“原點”標簽，圖中就會顯示繪圖坐標系的原點。（如果你想將繪圖坐標系顯示出來，可以先選中“原點”標簽，然后點擊上方菜單欄中的“參考幾何體”，再選擇“坐標系”，然后直接回車即可看到新建的繪圖坐標系，如右圖，可見它位于上面的關節(jié)軸）

　　然后回到機器人的裝配體中，在左側的連桿樹中展開每個連桿找到并選中其繪圖坐標系的原點，然后點擊上方菜單欄“評估”中的“測量”即可看到圖中出現(xiàn)了一組坐標值（如下圖所示），這就是連桿繪圖坐標系的原點在全局坐標系（本文將全局坐標系定義為裝配體的坐標系）中的位置。

?

?

　　我們記錄下所有連桿的繪圖坐標系的原點位置（除去彈簧缸的2個，注意 SolidWorks 中默認的單位是毫米，這里除以 1000 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 的坐標系（y y 軸朝上），要變到 z z 軸朝上，方法仍然是乘以旋轉矩陣 rot：

?

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

?

?

　　以后我們會經常對點的坐標進行各種各樣的變換（平移、旋轉），而且很多時候是用一個矩陣同時對很多個點的坐標進行變換（例如上面的例子），不如定義一個算子以簡化代碼。我們可以定義算子（其實是一個函數(shù)）：

?

CircleDot[matrix_,vectors_]:=(matrix.#)&/@vectors;

?

?

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

?

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

?

?

說明：本文出現(xiàn)的所有自定義的函數(shù)都給出了實現(xiàn)代碼（Mathematica 自帶的函數(shù)首字母都是大寫。為了與官方函數(shù)區(qū)分，我自定義的函數(shù)有些采用小寫字母開頭，不過建議大家采用windows編程常用的命名法，即變量名首字母小寫，中間字母大寫：myVariable，而函數(shù)名首字母和中間字母都大寫：MyFunction）。為了方便，我將這些自定義函數(shù)打包成一個函數(shù)包，每次運行程序時導入此函數(shù)包即可使用里面的函數(shù)。注意該函數(shù)包依賴另一個函數(shù)包 Screws.m[3] [3] ，該包基于旋量理論，可在此處下載：http://www.cds./~murray/books/MLS/software.html （為了寫起來省事，我修改了其中部分函數(shù)的名字，為此重新定義了 myScrews.m）。在程序中導入函數(shù)包的代碼如下（如果函數(shù)包位于你的程序筆記本文件的同一目錄下）:

?

SetDirectory[NotebookDirectory[]] 
<< myFunction.m

?

?

　　還記得嗎？最開始我們導入機器人模型時，各連桿的位置都已經按照裝配關系確定好了，所以它們的坐標也是相對于全局坐標系描述的?？墒乾F(xiàn)在我們要讓機械臂動起來（并且顯示出來），這就需要移動這些坐標。為了方便起見，最好能將每個連桿的坐標表示在自己的繪圖坐標系中，因為這樣我們只需要移動繪圖坐標系就行了，而各點的坐標相對它們所屬的繪圖坐標系是不動的。應該怎么做呢？很簡單，將連桿的坐標減去繪圖坐標系的原點在全局坐標系中的坐標即可：

?

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

?

?

　　坐標移動后的連桿如下圖所示（圖中的坐標系是各個連桿自己的繪圖坐標系，我沒有對坐標轉動，所以繪圖坐標系和全局坐標系的姿態(tài)相同）。我們一開始從 SolidWorks 導出文件時是一次性地導出整個裝配體的。其實，如果我們挨個打開各個連桿并且一個一個的導出這些連桿，那么得到數(shù)據(jù)就是相對于各自的繪圖坐標系的，只不過這樣稍微麻煩一點。

?

?

3.2 利用旋量建立運動學模型

?

　　下面我們討論如何建立運動學模型。描述機器人連桿之間幾何關系的經典方法是采用 D-H 參數(shù)（Denavit - Hartenberg parameters）。D-H 參數(shù)巧妙在什么地方呢？我們知道，完全確定兩個坐標系（或者剛體）的位姿關系需要6個參數(shù)，因為三維空間中的剛體有6個自由度。如果不考慮關節(jié)轉動（平移）仍需要5個參數(shù)。然而 D-H 參數(shù)居然只用了4個參數(shù)就能夠確定相鄰連桿的位姿關系，可見 Denavit 和 Hartenberg 這哥倆確實動了番腦筋。不過為了避免 D-H 參數(shù)的一些缺點，我們棄之不用而采用旋量的表示方法。剛接觸旋量的同學會覺得它很難理解。其實旋量有什么性質、它和剛體運動的關系又是什么？這些問題數(shù)學家也是用了很長時間才搞清楚。在本文中你可以把旋量簡單想象成一個描述關節(jié)轉動的向量。三維空間中的旋量是一個6維向量，要描述一個關節(jié)旋量需要確定一個關節(jié)軸線的方向向量（3個參數(shù)）和軸線上任意一點的坐標（又要3個參數(shù)）。
　　旋量和向量相似的一個地方是，對它的描述也是相對于一個坐標系的。我們選擇哪個坐標系呢？這里我們要參考 D-H 參數(shù)，每一個連桿坐標系在定義時都相對于前一個連桿的坐標系。所以我們將每個關節(jié)軸的旋量表示在前一個連桿中。這次我們以2號連桿為例說明如何確定關節(jié)運動對應的旋量：
　　1.　首先來看關節(jié)軸線的方向，這個要相對于2號連桿的繪圖坐標系。（我們要確定關節(jié)2的旋量，至于關節(jié)1的旋量最好在連桿1中確定）。從下圖中看關節(jié)2的軸線方向似乎是 x x 軸，可是我們前面將繪圖坐標系的姿態(tài)和全局坐標系的姿態(tài)設定為一樣的，所以應該在全局坐標系中確定，也就是 y y 軸。
　　2.　關節(jié)軸線上任意一點的坐標，這個同樣要相對于2號連桿的繪圖坐標系。我們在軸線上任選一點即可。步驟是：點擊 SolidWorks 上方菜單欄的“參考幾何體”，選擇“點”，然后在左側面板選擇“圓弧中心”，然后選擇圖中的關節(jié)軸周圍的任意同心圓弧即可創(chuàng)建一個參考點，這個點就是我們想要的。我們可以在連桿視圖中測量這個點的坐標，也可以在機器人完整裝配體中測量，這里我選擇后者。（測量步驟參照前面測量“連桿繪圖坐標系的原點”）

　　定義關節(jié)旋量的代碼如下。其中相對旋量 ξ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}];

?

?

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

?

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

?

?

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

?

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

?

?

說明：本文中，用符號 I 表示全局坐標系（同時也是慣性坐標系）；符號 L[i] 表示第 i i 個連桿，變量 g[L[i], L[i 1]] 表示第 i 1 i 1 個連桿相對于第 i i 個連桿的位姿矩陣（它是一個4×4 4×4齊次變換矩陣）；變量 g[I, L[i]] 表示什么你肯定猜到了，它表示第 i i 個連桿相對于全局坐標系的位姿矩陣。如果不特別說明，本文總是用 g （或者 g 開頭的變量）表示一個（或一組）齊次變換矩陣，這是約定俗成的。

?

　　現(xiàn)在可以正式推導機械臂的運動學模型了。在使用機械臂時，大家一般只關心其最末端連桿的位姿，更確切的說，是最末端連桿的位姿與關節(jié)角度的關系。不過為了得到最末端連桿的位姿，我們需要計算中間所有連桿的位姿。這里利用相鄰連桿的迭代關系——每個連桿的位姿依賴前一個連桿的位姿——來提升計算效率。所以，可以定義機械臂所有連桿的運動學函數(shù)為：

?

robotPartsKinematics[configuration_] := Module[{q, g2To7},
　　 {g[I, L[1]], q} = configuration;
　　 g2To7 = Table[g[L[i], L[i   1]] = TwistExp[\[Xi]r[[i]], q[[i]]].g[L[i], L[i   1], 0]; 
　　 g[I, L[i   1]] = g[I, L[i]].g[L[i], L[i   1]], {i, n}];
　　 Join[{g[I, L[1]]}, g2To7] ]

?

?

　　robotPartsKinematics函數(shù)的輸入是：基座的位姿矩陣 g[I, L[1]] 和所有關節(jié)的角度向量q∈R6 ∈R6，這組變量完整地描述了一個串聯(lián)機械臂的位置和姿勢（用機器人中的專業(yè)術語應該叫“構型”： configuration，注意不要翻譯為“配置”），而輸出則是所有連桿相對于全局坐標系的4×4 4×4位姿矩陣（即 g[I, L[i]]，其中i = 1~7）。
　　其中，TwistExp 函數(shù)來自于 Screws 工具包[3] [3]，作用是構造旋量的矩陣指數(shù)。

?

說明：在大多數(shù)的機器人教科書中，連桿的記號是從0開始的，也就是說將基座記為0號連桿，然后是1號連桿，最末端的連桿是n 1 n 1號（假設機械臂的自由度是n n）；而關節(jié)的記號是從1開始，也就是說1號關節(jié)連接0號連桿和1號連桿。這樣標記的好處是記號一致，推導公式或編程時不容易出錯：比如說我們計算第 i i 個連桿的速度時要利用第 i i 個關節(jié)的轉動速度。可是本文中連桿的記號是從1開始的（基座標記為1號連桿），我們保留0號標記是為了以后將機械臂擴展到裝在移動基座（比如一個AGV小車）的情況，這時0號就用來表示移動基座。

?

　　可以看到，只要定義好關節(jié)旋量，建立運動學模型非常簡單?？墒沁@樣得到的運動學模型對不對呢？我們來檢驗一下。借助 Manipulate 函數(shù)，可以直接改變機械臂的各關節(jié)角度，并直觀地查看機械臂姿勢（應該叫構型了哦）的變化，如以下動畫所示?？梢钥吹?，機械臂各連桿的運動符合我們設置的關節(jié)值，這說明運動學模型是正確的。

?

?

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

?

?

　　驗證使用的代碼如上。其中，move3D 函數(shù)的功能是用一個齊次變換矩陣（g）移動一個幾何圖形（shape）。這里還值得一提的是 MapThread 函數(shù)。雖然我們可以用 move3D 函數(shù)去一個一個地移動連桿（寫起來就是：move3D[part1, g1], move3D[part2, g2], move3D[part3, g3]），這樣寫比較清楚也很容易讀懂，可就是太麻煩了。如果你的機械臂有一百個連桿，用這種方法豈不是要累死。當然，我們可以使用循環(huán)，但是使用 MapThread 函數(shù)寫起來更簡單，即：MapThread[move3D, {{part1, part2, part3}, {g1, g2, g3}}]，而且得到的結果與前面完全一樣。這就是為什么我喜歡把同類型的元素都放到一起，因為操作的時候可以一起批量化進行，使得代碼更簡潔。
　　可以看到，Mathematica 提供的控件類函數(shù) Manipulate 支持用戶使用鼠標交互式地改變變量的值，同時動態(tài)更新對應的輸出。如果一段代碼運行時間足夠快，就可以放在Manipulate 內部，比如運動學函數(shù)robotPartsKinematics，它包含的計算并不復雜，但如果是后面要介紹的動力學函數(shù)就不適合放在Manipulate里面了，因為動力學的計算比較耗時，窗口會顯得很“卡頓”。
　　隨著對 Mathematica 越來越熟悉，你會逐漸體會到它的強大。比如說你可以隨心所欲地將已有的函數(shù)組合從而得到新的函數(shù)，如果一段代碼被頻繁地使用我們就可以這么干。舉個例子，我們可以借助robotPartsKinematics來定義一個新的函數(shù)robotMoveToQ，用來得到某個構型下的機械臂的外觀：

?

robotMoveToQ[q_] := Module[{gs},
  gs = robotPartsKinematics[{IdentityMatrix[4], q}];
  MapThread[move3D, {robotPartsColored, gs}] ]

?

?

　　而且，函數(shù)的使用也很靈活。例如，我們可以將不同構型下的機械臂同時顯示出來，只需要兩行代碼，如下：

?

qs = Table[{a - Pi/2, a/2, -a/2   Pi/6, 0, 0, 0}, {a, 0, Pi, 0.2}];(*生成關節(jié)變量列表，即不同構型*)
Graphics3D[{robotMoveToQ /@ qs}]

?

?

?

4. 逆運動學仿真

?

　　借助運動學，我們成功地通過改變關節(jié)角度實現(xiàn)了對機械臂的控制。當然這沒什么值得炫耀的，本質上不過是矩陣相乘罷了。本節(jié)我們考慮一個更有挑戰(zhàn)性也更好玩的問題。如果告訴你所有連桿（局部坐標系）的位姿，你能不能算出機械臂的各個關節(jié)角來？你一定會說這很簡單，求一下反三角函數(shù)就行了。但是實際應用時經常會遇到比這個難一些的問題：只告訴你機械臂最后一個連桿的位姿，如何得到各關節(jié)的角度？這個問題被稱為“逆運動學”。Robotic Toolbox工具箱[2] [2]中給出了兩個解逆運動學問題的函數(shù)：ikine 和 ikine6s，分別是數(shù)值解法和符號解析解法，本文我們也用兩種方式解決逆運動學問題。

?

4.1 數(shù)值解法之——解方程

?

　　上一節(jié)的運動學函數(shù) robotPartsKinematics 能得到所有連桿的位姿。大多數(shù)時候，人們只關心最后一個連桿的位姿（因為它上面要裝載操作工具），即 Last@robotPartsKinematics[{IdentityMatrix[4], q}]（注意q是一個六維向量，即q=(q1,q2,q3,q4,q5,q6 q1?,q2?,q3?,q4?,q5?,q6?)），結果如下圖所示（另存為可以看大圖）。這里關節(jié)角沒有設置數(shù)值，因此得到的是符號解，有些長哦。這也是為什么機器人領域經常使用三角函數(shù)縮寫的原因：比如把 cos(q1) cos(q1?)記為c1 c1?。在[4] [4]中提供了一個函數(shù) SimplifyTrigNotation，可以用來對下式進行縮寫。

?

?

　　如果我們想讓機械臂末端（連桿）到達某個（已知的）位姿 gt，也就是讓上面的矩陣等于這個位姿矩陣：

?

Last@robotPartsKinematics[{IdentityMatrix[4], {q1, q2, q3, q4, q5, q6}}] = gt　(*逆運動學方程*)

?

?

　　通過解上面這個以6個關節(jié)角 (q1,q2,q3,q4,q5,q6) (q1?,q2?,q3?,q4?,q5?,q6?) 為未知量的方程組就能知道機械臂的構型了。也就是說，逆運動學問題的本質就是解方程。對于解方程我們一點也不陌生，從小到大我們解過無數(shù)的方程。甚至可以說數(shù)學這個學科本身有很大一部分就是在研究解方程、解各種各樣的方程：大規(guī)模的、小規(guī)模的，線性的、非線性的，代數(shù)的、微分的，常微分的、偏微分的。既然有這么多種方程，也就意味著存在很多種解法。Mathematica 提供的用來解方程的函數(shù)不止一個，例如有：Solve、NSolve、DSolve、LinearSolve、FindRoot 等等。面對這么多解方程的工具，我們應該選哪個呢？你選擇的函數(shù)取決于方程的類型，所以我們先看看這個方程是什么類型呢？首先，它是個代數(shù)方程，所以不能使用求解微分方程的函數(shù)（DSolve）。其次，方程中包含未知量的三角函數(shù)，所以它是非線性代數(shù)方程，因此不能用求解線性方程的函數(shù)（LinearSolve）。再者，對于代數(shù)方程有數(shù)值解法和解析解法兩類方法。當然，我們非常想得到用符號表示的解析解，因為只需要解一次以后直接帶入數(shù)值即可，計算速度非?？?。但是非線性方程一般很難得到符號解（對于這個機械臂，它存在符號解），所以我們只好退而求其次尋找數(shù)值解了，這樣就把范圍縮小到 NSolve、FindRoot 這兩個函數(shù)了。NSolve 會得到所有解（這個方程有不止一組解哦），而 FindRoot 會根據(jù)初始值得到最近的解。一番試驗表明只有 FindRoot 能滿足要求。

?

說明：在求解逆運動學方程前還需要解決一個小問題：如何在 Mathematica 中表示一個期望的目標位姿 gt 呢？Mathematica 提供了 RollPitchYawMatrix 函數(shù)和 EulerMatrix 函數(shù)用來表示三維轉動（你用哪個都可以），然后利用前面的 RPToH 函數(shù)合成為位姿矩陣 gt 即可，示例代碼如下。其中，cuboid 函數(shù)用于繪制一個長方體。如果你使用 Matlab ，那我要可憐你了。因為 Matlab 沒有繪制長方體的函數(shù)，一切你都要自己畫。 而 Mathematica 定義了一些常用幾何圖形，可以直接用。

?

cuboid[center_, dim_]:= Cuboid[center - dim/2, center   dim/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×4 4×4 齊次變換矩陣得到的，里面有 4×4=16 4×4=16 個方程，去掉最后一列(0,0,0,1) (0,0,0,1)對應的4 4個恒等式還有12 12個方程，超過了未知量（6 6個）的個數(shù)，這是因為 3×3 3×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]] && gts[[2, 4]] == gt[[2, 4]] && gts[[3, 4]] == gt[[3, 4]] && gts[[1, 2]] == gt[[1, 2]] && gts[[2, 3]] == gt[[2, 3]] && gts[[3, 3]] == gt[[3, 3]], {q1, 0}, {q2, 0}, {q3, 0}, {q4, 0.3}, {q5, 0.3}, {q6, 0.3}]];
 planeXY = {FaceForm[], EdgeForm[Thickness[0.005]], InfinitePlane[{x, y, z}, {{0, 1, 0}, {1, 0, 0}}], InfinitePlane[{x, y, z}, {{0, 1, 0}, {0, 0, 1}}]};
 lines = {Red, Thickness[0.0012], Line[{{x, y, z}   {100, 0, 0}, {x, y, z}   {-100, 0, 0}}], Line[{{x, y, z}   {0, 100, 0}, {x, y, z}   {0, -100, 0}}], Line[{{x, y, z}   {0, 0, 100}, {x, y, z}   {0, 0, -100}}]};
 Graphics3D[{planeXY, lines, 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 函數(shù)改變值的大小，試驗的效果見下圖。

?

4.2 數(shù)值解法之——迭代法

　　解方程的方法很多，下面我們換一種思路求解逆運動學方程，其思想來自于[2] [2]（英文版187頁），代碼如下：

?

forwardKinematicsJacobian[argList_, gst0_] := 
  Module[{g = IdentityMatrix[4], \[Xi], n = Length[argList]},
     Js = {}; (*注意空間雅可比矩陣Js是全局變量，后面會用*)
     Do[\[Xi] = Ad[g].argList[[i, 1]];
      Js = Join[Js, {\[Xi]}];
      g = g.TwistExp[argList[[i, 1]], argList[[i, 2]]]
      , {i, n}];
     Js = Transpose[Js];
     g.gst0 ]
\[Xi]a = Table[\[Omega]a[i] = axesDirInGlobal[[i]]; la[i] = axesPtInGlobal[[i]]; Join[Cross[-\[Omega]a[i], la[i]], \[Omega]a[i]], {i, n}]; 
(*forwardKinematicsJacobian函數(shù)是從 Screws.m 中抄的，它使用的旋量都要求表示在全局坐標系中，因此需要定義絕對旋量\[Xi]a*)   
inverseKinematics[gt_, q0_, errorthreshold_: 0.0001] := 
  Module[{gReal, q = q0, Jb, Jg, F, error, theta, axis, positionerror, angleerror, maxIter = 20},(*輸入期望的機械臂末端位姿 gt 和初始關節(jié)角 q0*)
   Do[gReal = forwardKinematicsJacobian[Transpose@{\[Xi]a, q}, g[I, L[7], 0]];
    Jb = Ad[Inverse[gReal]].Js;
    Jg = diagF[HToR[gReal], HToR[gReal]].Jb;
    positionerror = HToP[gt - gReal];
    angleerror = Reverse@RollPitchYawAngles[HToR[gt.Inverse[gReal]]]; (*注意Reverse函數(shù)*)
    error = Flatten[N[{positionerror, angleerror}]]; (*誤差向量 error 包括位置和角度分量在全局坐標系中表示*)
    F = PseudoInverse[Jg].error;
    q = q   modToPiPi[F];
    If[Norm[error] < errorthreshold, Break[]]
    ,{maxIter}];
    q]

?

?

　　forwardKinematicsJacobian 函數(shù)用于計算（空間）雅可比矩陣和最后一個連桿的位姿，它修改自 Screws 工具包[3] [3]。逆運動學計算函數(shù) inverseKinematics 的輸入是期望的末端連桿位姿 gt，迭代的初始角度 q0 ，以及誤差閾值 errorthreshold （默認值為 0.0001 0.0001）。
　　其中的 modToPiPi 函數(shù)（實現(xiàn)代碼如下）用于將角度值轉換到 ?π～π ?π～π 的范圍之間。這里為什么需要 modToPiPi 函數(shù)呢？因為角度是個小妖精，如果我們不盯緊它，它可能會時不時的搗亂。從外部看，機械臂的一個轉動關節(jié)位于角度 π/3 π/3 和角度 π/3 2π π/3 2π 沒什么區(qū)別?？墒侨绻覀兎湃谓嵌冗@樣隨意跳變，會導致軌跡不連續(xù)，這樣機械臂在跟蹤軌跡時就會出現(xiàn)麻煩。

?

modToPiPi[angle_]:= Module[{a = Mod[angle,2.0*Pi]}, If[Re[a]>=Pi, a-2.0*Pi, a]]
SetAttributes[modToPiPi,Listable];

?

?

　　其中，Ad 函數(shù)就是 Screws 工具包[3] [3]中的 RigidAdjoint 函數(shù)，它表示一個齊次變換矩陣的伴隨變換（Adjoint Transformation），diagF 函數(shù)用于將多個矩陣合成為塊對角矩陣，實現(xiàn)代碼如下：

?

diagF=SparseArray[Band[{1,1}]->{##}]&  (*用法為 A = {{1,2},{3,4}}; B = {{5,6},{7,8}}; diagF[A,B]//MatrixForm *)

?

?

　　HToR 函數(shù)和 HToP 函數(shù)分別用于從一個齊次變換矩陣中取出旋轉矩陣（R R）和位移向量（P P），代碼如下。

?

HToR[g_]:= Module[{n=(Dimensions[g])[[1]]-1},  g[[1;;n,1;;n]]]
HToP[g_]:= Module[{n=(Dimensions[g])[[1]]-1},  g[[1;;n,n 1]]]

?

?

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

?

T[g_]:=Transpose[g]
Iv[g_]:=Inverse[g]

?

?

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

?

Clear[x,y];
pts2D = Table[{Sin[i], Cos[i]}/1.4, {i, 0, 4 Pi, Pi/400}]; (*先生成二維平面上的點，它們均勻地分布在一個圓上*)
pts3D = pts2D /. {x_, y_} -> {1.721, x, y   1.4}; (*再將二維坐標變換成三維坐標*)
Graphics3D[Point[pts3D]]

?

?

　　然后調用逆運動學函數(shù) inverseKinematics 挨個計算不同點處的關節(jié)值，代碼如下：

?

gStars = PToH /@ pts3D; (*將三維點的坐標轉換成齊次變換矩陣，轉動部分始終不變*)
q = ConstantArray[0, n]; (*inverseKinematics函數(shù)包含一個迭代過程，因此需要提供一個初始值*)
g[I, L[7], 0] = (robotPartsKinematics[{IdentityMatrix[4], q}]; g[I, L[7]]);  (*forwardKinematicsJacobian函數(shù)需要零位狀態(tài)下的末端連桿位姿*)
qs = Table[q = inverseKinematics[gt, q], {gt, gStars}]; (*依次遍歷所有點，我們用每次計算得到的 q 作為下一次迭代的初始值，這樣迭代速度更快*)

?

?

　　計算結果 qs 中存儲了機械臂到達不同點處的關節(jié)向量，如果以后我們想讓機械臂跟蹤這個向量序列，可以對其插值得到連續(xù)的關節(jié)函數(shù)，這是靠 Interpolation 函數(shù)實現(xiàn)的，代碼如下。關于 Interpolation 函數(shù)我要啰嗦幾句，因為以后我們可能會經常用到它。對于機械臂的每個關節(jié)來說， Interpolation 得到的是一個插值函數(shù)（InterpolatingFunction），更確切地說是“Hermite多項式” 或“Spline 樣條”插值函數(shù)。插值函數(shù)與其它的純函數(shù)沒什么區(qū)別，比如說我們可以對它求導、求積分。對這6個關節(jié)的插值函數(shù)求導就能得到關節(jié)速度和加速度函數(shù)：

?

time = 10; (*time是自己設置的，表示機械臂運動經過所有點的總時間*)
Do[qt[i] = T@{Range[0, time, time/(Length[(T@qs)[[i]]] - 1)], (T@qs)[[i]]}, {i, n}];
Do[qfun[i] = Interpolation[qt[i]];
   dqfun[i][x_] = D[qfun[i][x], x];
   ddqfun[i][x_] = D[dqfun[i][x], x], {i, n}]

?

?

　　畫出插值后各關節(jié)的角度、角速度、角加速度的變化趨勢，如下圖。能看到有兩個關節(jié)角速度變化劇烈，因此，理論上這個曲線不適合讓機器人跟蹤。

?

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 雅克比矩陣的零空間

　　在上一節(jié)求解逆運動學問題時我們使用了機械臂的雅克比矩陣，它能夠將關節(jié)速度映射到末端連桿的速度。由于末端連桿的速度有不止一種定義方式（例如有：空間速度、本體速度、全局速度，它們的定義見我的另一篇博客），所以對應了不同的雅克比形式（也就是逆運動學函數(shù)中的 Js、Jb、Jg）。
　　雅克比矩陣是機器人學里的“紅人”，它出現(xiàn)在很多場合，在這個圈子混的時間長了你經常能見到它。雅克比矩陣有一些有趣的性質，比如它的零空間。只要機械臂的關節(jié)速度在其雅克比矩陣的零空間中，那么末端連桿的速度總是零，零空間由此得名。通俗的說就是：不管關節(jié)怎么動，末端連桿始終不動（就像被釘死了一樣）。這個性質還挺有用的，因為有些場合要求機械臂在抓取東西的時候還能躲避障礙物。在其它領域，例如攝影，為了保證畫面穩(wěn)定需要攝像機能防抖動；在動物王國中，動物覓食時頭部要緊盯獵物（被惡搞的穩(wěn)定雞）；在軍事領域（例如坦克、武裝直升機），要求炮口始終瞄準目標，不管車身如何移動和顛簸。

?

?

　　對于本文中的 6 自由度機械臂，由于它不是冗余的，所以大多數(shù)時候計算零空間會得到空（也就是說不存在零空間）。為了形象地展示零空間的效果，我不得已只用了平移的部分。以下代碼展示了機械臂在（平移）零空間中的一種運動，如下圖所示?？梢钥吹?，不管機械臂如何運動，末端連桿（局部坐標系）的位置始終不動（但是它的姿態(tài)會改變，矩陣mask 的作用就是濾掉轉動分量，只剩下沿 x、y、z x、y、z 軸的平移運動）。BodyJacobian 函數(shù)用于計算本體雅可比矩陣，它也來自于 Screws 工具包[3] [3]。零空間是個線性空間，如果我們知道它的一組基向量，通過線性組合能夠得到任意的（速度）向量。NullSpace函數(shù)返回的剛好就是構成矩陣的零空間的一組基向量。通過對這組基向量線性組合即可得到速度向量，其使機械臂末端始終不動。下面的例子中使用的組合系數(shù)都是 1 ，也就是所有基向量相加（向量相加就是對應元素相加，由Total函數(shù)完成）。由于雅可比矩陣是機械臂構型q的函數(shù)，所以機械臂的構型一旦改變了，我們就要重新計算它的雅可比矩陣。如果還不理解，可以隨時顯示出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 = q   dq*dt;
 Graphics3D[{robotMoveToQ[q], move3D[frame3D, g[I, L[7], 0]]}], {i, 1, 1000, 1}]

?

?

?

5. 碰撞檢測

?

　　我們生活的物質世界有一個簡單的法則：兩個物體不能同時占據(jù)同一個空間位置，如果它們試圖那么做會有很大的力將它們分開?？墒欠抡媸窃谔摂M的數(shù)字世界中進行的，這個世界可不遵守物質世界那套力的法則，因此仿真還不夠真實。為了讓機器人仿真更接近實際，我們需要考慮“碰撞檢測”（Collision Detection）功能。為了追求效率，工業(yè)機器人的運動速度通常比較快，而且抓著較重的負載，它一旦碰到障礙物或者人，結果一般是“物毀人傷”。所以在仿真時提前檢測是否有碰撞很有必要。在一些規(guī)劃算法中，碰撞檢測也是很重要的一部分。
　　值得一提的是，現(xiàn)在一些先進的機器人控制器開始配備簡易的碰撞檢測功能，如果在機器人工作時有人突然擋住了它，它會自動停止。這是通過檢測機械臂關節(jié)處電機的電流大小實現(xiàn)的。當機械臂碰到人時，它相當于受到了一個阻力，電機要想保持原來的速度運行需要加大電流，靈敏的控制器會感知到電流的波動，這樣我們就能通過監(jiān)視電流來判斷機械臂有沒有發(fā)生碰撞，如果電流超過一定范圍就認為機械臂發(fā)生碰撞了，需要緊急剎車。可是這種碰撞檢測方法只適用于小負載（<5kg）的機械臂。因為對于重型機械臂，即便它也會停下來，可是它的慣性太大需要一段剎車距離，這足以對人造成傷害。
　　碰撞檢測是一個比較有難度的幾何問題，目前有很多成熟的算法（AABB、GJK）。我們的關注點在機器人，所以不想在碰撞檢測算法上浪費太多時間。為此，我們使用 Mathematica 自帶的 RegionDisjoint 函數(shù)實現(xiàn)碰撞檢測。在幫助文檔中，可以了解到 RegionDisjoint 函數(shù)用于判斷多個幾何體是否相交，如果兩兩之間都不相交則返回 True ，而兩個幾何體出現(xiàn)了相交，就表示它們發(fā)生了碰撞（太好了，這簡直是為碰撞檢測量身定做的函數(shù)）。

　　RegionDisjoint 函數(shù)可以用于二維幾何圖形，也可以用于三維幾何體，甚至可以用于非凸的幾何圖形或幾何體，如下面的例子所示。例子代碼如下，其中使用了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}]

?

?

　　不過有了 RegionDisjoint 函數(shù)并不意味著一帆風順?！芭鲎矙z測”是出了名的吞噬者，它會霸占 CPU 大量的計算資源。如果不把它伺候好，你的計算機再先進都會卡死。我們一般希望碰撞檢測越快越好，可是精度和速度是一對矛盾，追求速度只能犧牲一定的精度。機械臂的真實外形往往都是不規(guī)則的復雜幾何體，這使得精確的碰撞檢測很浪費時間。多數(shù)情況下，我們沒有必要達到非常逼真的檢測效果。如果不追求很高的精度，碰撞檢測應該保守一些。也就是說，在實際沒發(fā)生碰撞時允許誤報，但在發(fā)生碰撞時不能漏報——寧可錯殺一千，不可放過一個。碰撞檢測的計算量與模型的復雜程度有關。我們導入的機器人模型雖然已經經過了“瘦身”，但對于RegionDisjoint函數(shù)來說還是有些復雜。為此，我們需要進一步縮減簡化。為了保守一點，我們采用比真實機械臂連桿稍大些的模型，比如連桿的凸包（Convex Hull）。雖然 Meshlab 軟件可以制作凸包，但是我發(fā)現(xiàn)效果不太好。好在 Mathematica 自帶的 ConvexHullMesh 函數(shù)可以計算任意幾何體的凸包。我采用的方法是先用 ConvexHullMesh 分別計算各連桿的凸包，再導出凸包用 Meshlab 進一步簡化，最后再導入回 Mathematica 中。計算連桿凸包及導出所需的代碼如下。（注意：由于連桿數(shù)據(jù)已經是變換后的了，簡化后的連桿導入后不需要旋轉等變換）

?

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] 函數(shù)的功能是用齊次變換矩陣 g 對三維向量（點） pt3D 做變換，定義如下：

?

TransPt[g_][pt3D_]:=Module[{homogeneousPt},
    homogeneousPt=Join[pt3D,{1.0}];
    homogeneousPt=g.homogeneousPt;
    homogeneousPt[[1;;3]]  ]

?

?

6. 軌跡規(guī)劃

?

　　軌跡規(guī)劃的目的是得到機器人的參考運動軌跡，然后機器人再跟蹤這條軌跡完成最終的動作。軌跡規(guī)劃是機器人研究領域非常重要的一部分。機器人要干活就離不開運動，可是該如何運動呢？像搭積木、疊衣服、擰螺釘這樣的動作對人類來說輕而易舉，可要是讓機器人來實現(xiàn)就非常困難。工業(yè)機器人既沒有會思考的大腦，也缺少觀察世界的眼睛（又瞎又傻），要讓它們完全自主運動真是太難為它們了。它們所有的運動都是人教給它的。你可以把機器人想象成木偶，木偶看起來像是有靈魂的生物，但實際上它的運動都是人灌輸?shù)模九贾粫腊宓匕凑杖说目刂七\動，自己沒有任何主動性，只是行尸走肉罷了。實際工廠中，是由工程師操作著控制面板，一點點調節(jié)機械臂的各個關節(jié)角度，讓它到達某個位置?？刂瞥绦驎涗洐C械臂的角度變化，只要工程師示教一次，機械臂就能精確而忠實地重復無數(shù)次。不過這種不得已而為之的方法實在是太笨了，如果有一種方法能夠自動根據(jù)任務生成機器人的參考軌跡多好，下面我們將介紹一種常用的軌跡規(guī)劃方法。
　　
6.1 路徑、軌跡——傻傻分不清楚

?

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

?

　　　　　　　　　　　路徑　　　　　　　　　　　　　　軌跡

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

?

　　制作上面的軌跡需要以下幾個步驟：

　　1. 首先隨機生成一些三維空間中的點。

?

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

?

?

　　2. 然后利用 BSplineFunction 函數(shù)對點插值。

?

bfun = BSplineFunction[pts];

?

?

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

?

?

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

?

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

?

?

　　然后分別對 xyz xyz 坐標分量進行單獨插值（這里我同樣將自變量的取值范圍設定在 0～1 0～1 之間）：

?

nb = Length[bfpts];
ifunx=Interpolation[Transpose[{Range[0,1,1/(nb-1)],bfpts[[;;,1]]}]];
ifuny=Interpolation[Transpose[{Range[0,1,1/(nb-1)],bfpts[[;;,2]]}]];
ifunz=Interpolation[Transpose[{Range[0,1,1/(nb-1)],bfpts[[;;,3]]}]];

?

?

　　并定義一個新的插值函數(shù)為各分量的合成。這樣我們就人為生成了一段軌跡（或者說，是一個向量值函數(shù)）。

?

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

?

?

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

?

ArcLength[ifun[t], {t, 0, 1}]

?

?

　　●　既然可以計算弧長，就能用弧長對這條曲線重新參數(shù)化（我以前在學高等數(shù)學時，一直想不通怎么用弧長對一個曲線參數(shù)化，現(xiàn)在通過編程實踐就很容易理解了）：

?

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

?

?

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

?

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 標架想象成過山車，你坐在上面就能更直觀地感受曲率和撓率的含義。

?

?

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]; (*數(shù)值越小，點越密*)
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 軌跡規(guī)劃方法

?

　　“軌跡規(guī)劃”中的“規(guī)劃”又是什么意思呢？
　　規(guī)劃的英文是 plan，也翻譯為“計劃、打算”。你肯定知道“計劃”是什么意思。對于人來說，計劃就是在做事之前先想想應該怎么做才好，比如沿著什么途徑、按照哪幾個步驟。而且，通常你有一個要到達的目標，沒有目標談不上計劃（當然一般還得有一個出發(fā)點，但這不是必需的）。假如我想放假出去玩，在制定了詳細的開車路線后我連要去哪都不知道，那我是不是神經病呢。正常人都是先決定去哪，然后才選擇交通線路。此外，計劃還有個評價的標準——怎么樣才算“好”呢？如果沒有標準，那我們還計劃個什么勁兒?。ǚ凑龥]有好壞之分）？把目標和評價標準推廣到機器人的軌跡規(guī)劃領域就是：機器人怎么（運動）才能到達一個目標位置（或者區(qū)域、構型），而且不僅僅是到達目標，有時我們還想以最好的方式（比如最快、消耗能量最少）到達，這就是軌跡規(guī)劃的任務?！败壽E規(guī)劃”的叫法挺多，有叫“軌跡生成”的，有叫“運動規(guī)劃”的，但不管怎么叫其實大概都是一個意思。
　　對于機械臂來說，軌跡規(guī)劃方法可以根據(jù)有沒有障礙物來劃分。如果沒有障礙物，那就簡單些了，我們可以直接規(guī)劃軌跡；如果有障礙物則一般先規(guī)劃路徑（因為路徑包含信息更少，相對更簡單），然后對路徑設置速度得到軌跡（因為主要的工作都在規(guī)劃路徑，因此也可稱其為“路徑規(guī)劃”）。
　　路徑規(guī)劃都有哪些方法呢？比較流行的有：圖搜索、勢場法、RRT 等等。下面我們來實現(xiàn) RRT 方法。
　　RRT（快速探索隨機樹） 是一種通用的方法，不管什么機器人類型、不管自由度是多少、不管約束有多復雜都能用。而且它的原理很簡單，這是它在機器人領域流行的主要原因之一。不過它的缺點也很明顯，它得到的路徑一般質量都不是很好，例如可能包含棱角，不夠光滑，通常也遠離最優(yōu)路徑。

?

?

　　RRT 能在眾多的規(guī)劃方法中脫穎而出，它到底厲害在哪里呢？
　　天下武功唯快不破，“快”是 RRT 的一大優(yōu)點。RRT 的思想是快速擴張一群像樹一樣的路徑以探索（填充）空間的大部分區(qū)域，伺機找到可行的路徑。之所以選擇“樹”是因為它能夠探索空間。我們知道，陽光幾乎是樹木唯一的能量來源。為了最大程度地利用陽光，樹木要用盡量少的樹枝占據(jù)盡量多的空間。當然了，能探索空間的不一定非得是樹，比如Peano曲線也可以做到，而且要多密有多密，如上圖左所示的例子。雖然像Peano曲線這樣的單條連續(xù)曲線也能探索空間，但是它太“確定”了。在搜索軌跡的時候我們可不知道出路應該在哪里，如果不在“確定”的搜索方向上，我們怎么找也找不到（找到的概率是0）。這時“隨機”的好處就體現(xiàn)出來了，雖然不知道出路在哪里，但是通過隨機的反復試探還是能碰對的，而且碰對的概率隨著試探次數(shù)的增多越來越大，就像買彩票一樣，買的數(shù)量越多中獎的概率越大（RRT名字中“隨機”的意思）?？墒请S機試探也講究策略，如果我們從樹中隨機取一個點，然后向著隨機的方向生長，那么結果是什么樣的呢？見上圖右?？梢钥吹?，同樣是隨機樹，但是這棵樹并沒很好地探索空間，它一直在起點（紅點）附近打轉。這可不好，我們希望樹盡量經濟地、均勻地探索空間，不要過度探索一個地方，更不能漏掉大部分地方。這樣的一棵樹怎么構造呢？
　　RRT 的基本步驟是：
　　1. 起點作為一顆種子，從它開始生長枝丫；
　　2. 在機器人的“構型”空間中，生成一個隨機點 A A；
　　3. 在樹上找到距離 A A 最近的那個點，記為 B B 吧；
　　4. B B 朝著 A A 的方向生長，如果沒有碰到障礙物就把生長后的樹枝和端點添加到樹上，返回 2；
　　隨機點一般是均勻分布的，所以沒有障礙物時樹會近似均勻地向各個方向生長，這樣可以快速探索空間（RRT名字中“快速探索”的意思）。當然如果你事先掌握了最有可能發(fā)現(xiàn)路徑的區(qū)域信息，可以集中兵力重點探索這個區(qū)域，這時就不宜用均勻分布了。
　　RRT 的一個弱點是難以在有狹窄通道的環(huán)境找到路徑。因為狹窄通道面積小，被碰到的概率低，找到路徑需要的時間要看運氣了。下圖展示的例子是 RRT 應對一個人為制作的很短的狹窄通道，有時RRT很快就找到了出路，有時則一直被困在障礙物里面。對應的代碼如下（這段代碼只用于演示 RRT 的原理，不是正式代碼，但它有助于理解正式代碼的運算過程）：

?

?

(*RRT示例：此段程序不依賴任何自定義函數(shù)，可獨立運行。這是我能想到的最短的RRT演示代碼了*)
step = 3; maxIters = 2000; start = {50, 50}; range = {0, 100};
pts = {start}; lines = {{start, start}};
obstaclePts = {{20, 1}, {25, 1}, {25, 25}, {-25, 25}, {-25, -25}, {25, -25}, {25, -1}, {20, -1}, {20, -20}, {-20, -20}, {-20, 20}, {20, 20}}   50; 
Do[randomPt = RandomReal[range, 2];
   {nearestPt} = Nearest[pts, randomPt, 1];
   grownPt = nearestPt   step*Normalize[randomPt - nearestPt];
   If[!Graphics`Mesh`IntersectQ[{Line[{nearestPt, grownPt}], Polygon[obstaclePts]}],
　　　AppendTo[lines, {nearestPt, grownPt}];
　　　AppendTo[pts, grownPt]], {maxIters}];
Animate[Graphics[{Polygon[obstaclePts], Line[lines[[1 ;; i]]], Blue, PointSize[0.004], Point[pts[[1 ;; i]]], Red, Disk[start, 0.7]}, PlotRange -> {range, range}], {i, 1, Length[pts] - 1, 1}]

?

?

　　RRT探索空間的能力還是不錯的，例如下圖左所示的例子，障礙物多而且雜亂（借助 Mathematica 本身具有的強大函數(shù)庫，實現(xiàn)這個例子所需的所有代碼應該不會超過30行）。還有沒有環(huán)境能難住RRT呢？下圖右所示的迷宮對RRT就是個挑戰(zhàn)。這個時候空間被分割得非常嚴重，RRT顯得有些力不從心了，可見隨機策略不是什么時候都有效的。
　　“隨機”使得RRT有很強的探索能力。但是成也蕭何敗也蕭何，“隨機”也導致 RRT 很盲目，像個無頭蒼蠅一樣到處亂撞。如果機器人對環(huán)境一無所知，那么采用隨機的策略可以接受?？蓪嶋H情況是，機器人對于它的工作環(huán)境多多少少是知道一些的（即使不是完全知道）。我的博客一直強調信息對于機器人的重要性。這些已知的信息就可以用來改進算法。一個改進的辦法就是給它一雙“慧眼”：在勢場法中，勢函數(shù)攜帶了障礙物和目標的信息，如果能把這個信息告訴 RRT，讓它在探索空間時有傾向地沿著勢場的方向前進會更好。這樣，RRT 出色的探索能力剛好可以彌補勢場法容易陷入局部極小值的缺點。

?

?

　　將RRT方法用在機械臂上的效果如下圖所示（綠色表示目標狀態(tài)）。我設置了4個障礙物（其中一個是大地），這對機械臂是個小小的挑戰(zhàn)。由于我們生活在三維空間，沒辦法看到六維關節(jié)空間，所以我把六維關節(jié)空間拆成了兩個三維空間，分別對應前三個關節(jié)和后三個關節(jié)（嚴格來說，六維轉動關節(jié)空間不是一個歐式空間，它是個流形，不過這里我們不必糾結這個差別）：

?

?

　　正式 RRT 的主程序代碼如下。我將 RRT 樹定義為由節(jié)點列表 nodes 和樹枝列表 edges 組成，即 RRTtree = {nodes, edges}。其中節(jié)點列表 nodes 存儲樹中所有節(jié)點（每個節(jié)點都是一個 6 維向量，表示機械臂的關節(jié)值），樹枝列表 edges 中存儲所有樹枝，樹枝定義為兩個節(jié)點的代號（節(jié)點的代號定義為節(jié)點被添加到樹中的順序。例如，添加新節(jié)點時樹中已經有4個節(jié)點了，那么新節(jié)點的代號就是 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};  (*樹的初始化由節(jié)點和樹枝組成，它是全局變量*)
maxIters = 2000; (*最大迭代步數(shù)*)
jointLims = {{-Pi, Pi}, {-Pi/2, Pi/2}, {-Pi, 1.45}, {-Pi, Pi}, {-2, 2}, {-Pi, Pi}}; (*關節(jié)極限范圍，有些關節(jié)值不可取*)
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}];

?

?

　　構造 RRT 樹用到了以下自定義的函數(shù)：
　　1. 首先是碰撞檢測函數(shù) collisionDetection，如果機械臂沒有碰到障礙物就返回True。為了節(jié)省時間，碰撞檢測使用的是機械臂各連桿的凸包，在最后顯示的時候才使用原始連桿。

?

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

?

?

　　2. 碰撞檢測函數(shù)需要 Region 類型的變量，為此定義 toRegion 函數(shù)將幾何體變換為 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樹中添加節(jié)點和邊的函數(shù)：

?

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];(*最近節(jié)點的代號*)
  qNearest = nodes[[qNearestIdx]];
  growDirection = Normalize[qRand - qNearest];
  qExpand = modToPiPi[qNearest   step * growDirection]; (*生長*)
  Rrobot = toRegion[qExpand];
  If[collisionDetection[Rrobot, Robs], qNewIdx = addNode[qExpand]; (*添加新節(jié)點，返回新節(jié)點的代號 qNewIdx *)
  addEdge[{qNearestIdx, qNewIdx}]] ]

?

?

　　下面的代碼可以顯示搜索到的（關節(jié)空間中的）路徑。這條路徑質量不高，如果用于機器人的軌跡跟蹤還需要經過后期的平滑處理。

?

edges = RRTtree[[2]];
targetIdx = edges[[-1, 2]];
qPath = backTrack[targetIdx];
Animate[Graphics3D[{Robs, robotMoveToQ[qPath[[i]]]}], {i, 1, Length[qPath], 1}]

?

?

　　其中，backTrack 函數(shù)用來從樹中抽取出連接起點和目標點的路徑：

?

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. 動力學仿真

?

　　如果在淘寶花2塊錢買個知網(wǎng)賬號，然后檢索以“工業(yè)機器人控制”為關鍵詞的學位論文，在粗略地瀏覽了20$\sim$30篇論文的目錄之后，你就會像我一樣總結出一個樸素的規(guī)律：
　　●　碩士論文一般都建立了機器人的運動學模型。
　　●　博士論文一般都建立了機器人的動力學模型。
　　既然運動學已經能夠幫助機器人動起來了，為什么還需要費那么大勁建立動力學（以至于需要博士出馬）？
　　在前面的運動學一節(jié)中，我們能通過改變各個關節(jié)角度控制機械臂運動。但是在實際機械臂上，關節(jié)需要由電機驅動才能改變角度。那么電機應該輸出多大的力才能驅動機械臂運動呢，所需要的電流又是多大呢？只有知道這些我們才能真正實現(xiàn)對機械臂的控制。傳統(tǒng)的工業(yè)機器人大多采用兩層的控制方式，上層控制器直接輸出角度信號給底層驅動器，底層驅動器負責控制電機的電流實現(xiàn)上層給出的運動。上層不需要知道機器人的動力學特性也可以，更不用管需要輸出多大電流。如果你的機器人不需要太高的運動速度和精度，動力學沒什么太大用處（運動學是必需的，動力學不是必需的）。可是如果你的機器人速度很快，動力學效應就很明顯了，這時就要考慮動力學，否則上層發(fā)出的指令底層驅動器跟蹤不上，就會出現(xiàn)很大的誤差。要想把抓著很重的負載而且高速運動的六軸機器人控制好非常困難，中國好像還沒有人能做到。這也是為什么國產機器人大多應用在精度不高的場合，比如搬運、碼垛，而進口機器人霸占高端應用的原因。在高級的機器人控制器中，都有力矩補償功能（例如匯川、KEBA的控制器）。這個補償?shù)牧厥窃趺磥淼哪兀烤褪峭ㄟ^動力學方程計算得到的。補償力矩用作前饋控制信號，將其添加到驅動器上能使機器人更好地跟蹤一段軌跡?，F(xiàn)在機器人在中國變得火熱起來了，動力學也成為一些人吹噓的噱頭了。

?

　　匯川控制器（動力學補償使電流更?。?　　　　　 KEBA控制器（動力學使跟蹤精度更高）

?

　　我們如何得到機器人的動力學模型呢？
　　宅男牛頓首開先河，在同時代的人還渾渾噩噩的時候初步搞明白了力、速度、慣性都是怎么回事，并用數(shù)學對其進行了定量描述，從而建立了物體做平移運動時的動力學方程。從牛頓的身上我們知道，學好數(shù)學是有多重要。在那個遍地文盲的蠻荒年代，年輕的牛頓已經偷偷地自學了歐幾里得、笛卡爾、帕斯卡、韋達等大師的著作，這為他日后發(fā)明微積分打下了基礎。牛頓謙虛地自稱站在巨人的肩上，事實的確如此。
　　勤奮的歐拉再接再厲，將牛頓的方程推廣到轉動的情況。哥倆的工作結合起來剛好可以完整地描述物體的運動，這就是牛頓-歐拉法。
　　博學的拉格朗日發(fā)揚光大，又將牛頓和歐拉的工作總結提煉，提出了拉格朗日法。拉格朗日真聰明啊，只需要計算物體的動能，然后再一微分就得到了動力學方程，這是多么簡潔統(tǒng)一的方法啊?？墒抢窭嗜辗ǖ娜秉c是它的效率太低了。對于4自由度以下的機械臂，計算符號解的時間我們還能忍受。至于6自由度以上的機械臂，大多數(shù)人都沒這個耐心了（十幾分鐘到數(shù)小時）。而且計算出來的動力學是一大坨復雜的公式，很難分析利用。所以本文我們采用牛頓-歐拉法建立機械臂的動力學模型（更準確的說是它的升級版——迭代牛頓-歐拉法）。

?

7.1 慣性參數(shù)

?

　　早期工業(yè)機器人不使用動力學模型是有原因的，一個是動力學的計算量太大，在高效的計算方法被發(fā)現(xiàn)之前，早年的老計算機吃不消；另一個原因就是動力學需要慣性參數(shù)。運動學只需要幾何參數(shù)，這些相對好測量；可是動力學不僅需要幾何參數(shù)，還需要慣性參數(shù)。測量每個連桿的質量、質心的位置、轉動慣量很麻煩，尤其是當連桿具有不規(guī)則的形狀時，精度很難保證。如果使用動力學帶來的性能提升并不明顯，誰也不想給自己找麻煩。
　　要使用動力學模型，慣性參數(shù)必不可少。例如 Robotics Toolbox 工具箱中[2] [2]的 mdl_puma560.m 文件就存儲了 PUMA-560 機器人的慣性參數(shù)。不同型號的機器人具有不同的慣性參數(shù)，而且機器人抓住負載運動時，也要把負載的慣性考慮進來。
　　有些情況下，我們不需要知道很精確的慣性參數(shù)，差不多夠用就行了；可是有些場合對精度有要求，比如拖動示教就要求參數(shù)與實際值的誤差一般不能超過10%。對于精度要求不高的場合，可以使用一個近似值。大多數(shù)三維建模軟件（例如 SolidWorks、CATIA）以及一些仿真軟件（例如 Adams）都提供慣性計算功能，一些數(shù)學軟件（Mathematica）也有用于計算慣性的函數(shù)（我沒有對比過，所以不敢保證這些軟件的計算結果都是一樣的，我估計很有可能是不一樣的）。本文以 SolidWorks 為例介紹如何獲取慣性參數(shù)。
　　計算之前首先要設置連桿的材質。在 SolidWorks 中打開一個連桿，在左側的“材質”上單擊右鍵彈出“材料”對話框，如下圖所示。在這里可以設置機器人本體的材質，MOTOMAN-ES165D 這款機器人的連桿是鑄鋁（鑄造鋁合金：Cast Aluminum）制造的。不過連桿不包含電機等部件，為此選擇密度大一點的材料，本文選擇鋼鐵。這里最重要的是材料的密度，鋼鐵的密度一般是7.8噸/立方米（在計算慣性時，軟件假設連桿的密度是均勻的，這明顯是簡化處理了）。設置好后點擊應用即可。

?

　　然后在上方“評估”選項卡中單擊“質量屬性”就會彈出如下圖所示的對話框。

?

　　SolidWorks 很快就計算出了這個連桿的所有慣性參數(shù)。不過這里的信息量有點大，我逐個說明：
　　■ ■　首先是連桿的質量：172.28 千克。如果你顯示的單位不是千克，可以在當前對話框中的“選項”中修改單位。
　　■ ■　然后是連桿的質心（或重心）坐標系，重心坐標系的原點也給出了：(X,Y,Z)=(?0.73,?0.11,0) (X,Y,Z)=(?0.73,?0.11,0)，注意它是相對于繪圖坐標系的哦。重心坐標系的姿態(tài)下面會解釋。
　　■ ■　最后是連桿的慣性張量，這個有些人可能不懂，我詳細解釋下。SolidWorks列出了3個慣性張量，它們之間的區(qū)別就在于分別相對于不同的坐標系：
　?、佟∠鄬τ谫|心坐標系；其中的 Ix、Iy、Iz 三個向量表示質心坐標系相對于繪圖坐標系的姿態(tài)（也就是質心坐標系的 x、y、z 三個軸向量在繪圖坐標系中的表示），而 Px、Py、Pz 表示慣性主力矩（你要問我是怎么知道的，點“幫助”按鈕）。慣性張量的形式是對角矩陣：
IA=[Px000Py000Pz] IA?=???Px00?0Py0?00Pz????(1)　　②　相對于原點與質心坐標系重合，但是各軸與繪圖坐標系一致的坐標系。SolidWorks只給出了慣性張量中各項的值。慣性張量的完整形式是對稱矩陣（注意里面的負號）：
IB=[Lxx?Lxy?Lxz?LyxLyy?Lyz?Lzx?LzyLzz] IB?=???Lxx?Lyx?Lzx??LxyLyy?Lzy??Lxz?LyzLzz????(2)　?、邸∠鄬τ诶L圖坐標系（SolidWorks中稱為輸出坐標系），慣性張量的形式也是對稱矩陣（同樣注意里面的負號）：
IC=[Ixx?Ixy?Ixz?IyxIyy?Iyz?Izx?IzyIzz] IC?=???Ixx?Iyx?Izx??IxyIyy?Izy??Ixz?IyzIzz????(3)　　這三個慣性張量都反映了同一個連桿的性質，因此應該是等價的。那么它們之間有什么關系嗎？有的，它們之間可以轉換。如果定義旋轉矩陣 R=(Ix,Iy,Iz) R=(Ix,Iy,Iz)，質心坐標向量 p=(X,Y,Z) p=(X,Y,Z)，連桿質量為 m m，那么有IB=RTIAR IB?=RTIA?RIC=IB mST(p)S(p) IC?=IB? mST(p)S(p)　　其中，S(p) S(p)表示向量 p p 的斜對稱矩陣，這需要自定義函數(shù)（skew）實現(xiàn)，代碼如下：

?

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

?

?

　　這組公式來自于[6] [6]，我已經驗證過了，是正確的（要想結果比較接近，這些慣性參數(shù)至少要取到小數(shù)點后 5 位，這依然是在“選項”頁中設置）。
　　我們得到了三個慣性張量，在動力學方程中我們應該使用哪個呢？下面的程序使用了 IC IC?，因為它是相對于繪圖坐標系的，而我建立運動學時選擇的局部坐標系就是繪圖坐標系。（我以后在這里補充個單剛體動力學的例子）
　　
7.2 正動力學仿真

?

　　如果給你一條軌跡，如何設計控制律讓機械臂（末端）跟蹤這條軌跡呢，控制律的跟蹤效果怎么樣呢？借助正動力學，我們就可以檢驗所設計的控制律。
　　由于后面的程序所依賴的動力學推導過程[7] [7]采用了相對自身的表示方法（也就是說每個連桿的速度、慣性、受力這些量都是相對于這個連桿自身局部坐標系描述的），旋量也是如此，為此需要重新定義旋量（代碼如下）。其實旋量軸的方向沒變（因為局部坐標系的姿態(tài)與全局坐標系一樣），只是改變了軸上點的相對位置。

?

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

?

?

　　正動力學的 迭代牛頓-歐拉算法 代碼如下，輸入是力矩，輸出是運動。可以看到，動力學模型比運動學模型復雜多了（動力學用到運動學，運動學卻不需要動力學）。對于很多第一次接觸機器人的同學來說，動力學是一只可怕的攔路虎，要搞明白十幾個變量都是什么含義可不容易（在仿真的時候可能包含幾十個變量，任何一個弄錯了都會全盤皆輸，動力學可比運動學難伺候多了）。因為動力學模型是一個微分方程，所以整個仿真過程就是個數(shù)值積分的過程。

?

endTime=10.0; steps=2000; dt=endTime/steps; (*參數(shù)初始化：仿真時長、步數(shù)、步長*)
gravityAcc=9.807; (*重力加速度大小*)
Do[mass[i]=1.0; gravity[i]=gravityAcc*mass[i]*{0,0,-1,0,0,0},{i,n 1}]; (*mass[i]表示第i個連桿的質量，具體值自己設，重力加速度沿z軸的負方向，即{0,0,-1,0,0,0}*)
Do[\[ScriptCapitalM][i]=IdentityMatrix[6],{i,n 1}]; (*\[ScriptCapitalM][i]表示第i個連桿的廣義慣性矩陣，它包含質量和慣性張量*)
g[L[n 1],L[n 2]]=g[I,L[1]]=IdentityMatrix[4];
q=dq=ddq=ConstantArray[0,n]; (*關節(jié)角度、速度、加速度初始時刻假設都為0*)
Table[V[i]=dV[i]=ConstantArray[0,6],{i,n 1}]; (*連桿i的廣義速度V[i]包括線速度和角速度，也假設開始時刻都為0*)
\[CapitalPi][n 2]=ConstantArray[0,{6,6}]; (*中間變量，沒啥具體物理意義，只是迭代初始值*)
\[Beta][n 2]=ConstantArray[0,6]; (*也是中間變量*) (*以下是計算過程*)
qList=Table[
　 dq=dq ddq*dt; q=q dq*dt;(*歐拉積分*)　 
 For[i=2,i<=n 1,i  , (*速度前向迭代，從第二個連桿開始到最后一個連桿*)
　　 k=i-1;  (*因為本文的連桿從1開始標記，所以第i個連桿依賴前一個（i-1）關節(jié)*)
　　 g[L[i-1],L[i]]=TwistExp[\[Xi]r[[k]],q[[k]]].g[L[i-1],L[i],0];
　　 g[I,L[i]]=g[I,L[i-1]].g[L[i-1],L[i]];
　　 V[i]=Ad[Iv[g[L[i-1],L[i]]]].V[i-1] \[Xi]rb[[k]]*dq[[k]];
　　 \[Eta][i]=ad[V[i]-\[Xi]rb[[k]]*dq[[k]]].\[Xi]rb[[k]]*dq[[k]];
　　 ];
 For[i=n 1,i>=2,i--, (*力和慣量后向迭代，從最后一個連桿開始到第二個連桿*)
　　 k=i-1;
　　 \[Tau][k] = 0.0; (*施加關節(jié)力矩*)
　　 \!\(\*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]]]];
　　 Fex[i]=T[Ad[RToH[HToR[g[I,L[i]]]]]].gravity[i];
　　 \[ScriptCapitalB][i]=-T[ad[V[i]]].\[ScriptCapitalM][i].V[i]-Fex[i] T[Ad[Iv[g[L[i],L[i 1]]]]].\[Beta][i 1];
　　 \[CapitalPsi][i]=1/(\[Xi]rb[[k]].\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].\[Xi]rb[[k]]);
　　 \[CapitalPi][i]=\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i]-\[CapitalPsi][i]*KroneckerProduct[\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].\[Xi]rb[[k]],\[Xi]rb[[k]].\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i]];
　　 \[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])));
    ];
 For[i=2,i<=n 1,i  , (*加速度前向迭代，從第二個連桿開始到最后一個連桿*)
　　 k=i-1;
　　 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]);
　　 dV[i]=Ad[Iv[g[L[i-1],L[i]]]].dV[i-1] \[Xi]rb[[k]]*ddq[[k]] \[Eta][i]
　　 ];
　　 q , {t, 0, endTime, dt}];//AbsoluteTiming (*顯示計算耗時*)

?

?

　　其中， ad 函數(shù)用于構造一個李代數(shù)的伴隨表達形式，代碼如下。（開始我們定義的關節(jié)旋量是李代數(shù)，這里連桿的本體速度也是一個李代數(shù)，但加速度卻不是。實際上，要想把加速度搞明白可不是那么容易的事[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]] ]

?

?

　　正動力學的輸入是關節(jié)力矩，下面我們?yōu)殛P節(jié)力矩設置不同的值，看看機械臂的表現(xiàn)：
　　●　如果令關節(jié)力矩等于零（即 \[Tau][k] = 0.0），機械臂將在唯一的外力——重力作用下運動，如下圖所示。

?

?

　　只受重力的情況下，機械臂的總能量應該守恒。我們可以動手計算一下機械臂的能量（由動能和重力勢能組成，代碼如下）。將仿真過程中每一時刻的能量計算出來并保存在一個列表中，再畫出圖像，如下圖所示?？梢娔芰繋缀醪蛔儯ㄝp微的變化是由積分誤差導致的，如果步長取的再小一些，就更接近一條直線），這說明機械臂的總能量確實保持恒定，這也間接證實了正動力學代碼的正確性。這個簡單的事實讓人很吃驚——雖然機械臂的運動看起來那么復雜，但是它的能量一直是不變的。從力和運動的角度看，機械臂的行為變化莫測，可是一旦切換到能量的角度，它居然那么簡潔。機械臂的運動方程和能量有什么關系呢？聰明的拉格朗日就是從能量的角度去推導動力學方程的。

?

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

?

?

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

Kp=10000; Kd=50; (*PD 控制系數(shù)，需要自己試探選取*)
\[Tau][k]=Kp(qfun[k][t]-q[[k]]) Kd(dqfun[k][t]-dq[[k]]);

?

?

其中，qfun 和dqfun 是 4.2 節(jié)中定義的插值函數(shù)，這里用作機械臂要跟蹤的（關節(jié)空間中的）參考軌跡。跟蹤一個圓的效果如下圖所示。

　　●　機械臂在實際工作時可能會受到干擾。PD控制律對于擾動的效果怎么樣？我們施加一個擾動信號試試。這里我選擇一個尖峰擾動，模擬的實際情況是機械臂突然遭受了一個撞擊。擾動函數(shù)的定義代碼如下，你可以自己修改擾動的大小和尖峰出現(xiàn)的時間。

?

?

Manipulate[disturb[t_] := peak Exp[-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]

?

?

?

把擾動加到第二個關節(jié)的力矩上

?

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

?

?

　　機械臂的響應如上右圖所示，可見機械臂還是能回復零點姿勢的。一開始機械臂有一個輕微的顫動，俗稱“點頭”。這是由于剛一開始機械臂的角度和角速度都為零，所以關節(jié)力矩也為零，導致機械臂缺少能夠平衡重力的驅動力。在第5秒左右擾動出現(xiàn)，導致機械臂偏離了零點姿勢，但在反饋控制作用下很快又回到了零點姿勢。

?

7.3 逆動力學仿真

?

　　輸入力矩后，借助正動力學能得到關節(jié)角加速度，積分后可以得到角速度和角度。就像運動學和逆運動學的關系一樣，逆動力學與正動力學剛好相反，它的用處是：如果告訴你機械臂的運動（也就是關節(jié)角度、角速度、角加速度），計算所需的關節(jié)力矩。

?

{endTime,steps}={10.0,1000};
dt=endTime/steps; (*還是參數(shù)初始化*)
gravityAcc=9.807; (*重力加速度*)
Do[mass[i]=1.0; gravity[i]=gravityAcc*mass[i]*{0,0,-1,0,0,0},{i,n 1}];
Do[\[ScriptCapitalM][i]=IdentityMatrix[6];  V[i]=dV[i]=ConstantArray[0,6],{i,n 1}];
Fin[n 2]=ConstantArray[0,6]; (*第n 1個連桿受到的內力，為了迭代過程寫起來方便定義的*)
g[L[n 1],L[n 2]]=g[I,L[1]]=IdentityMatrix[4];
q=dq=ddq=ConstantArray[0,n]; (*初始狀態(tài)的關節(jié)角度、速度、加速度*)
\[Tau]List=Table[
　For[i=2,i<=n 1,i  , (*速度前向迭代：從第二個連桿開始到最后一個連桿*)
　  k=i-1;
　　g[L[i-1],L[i]]=TwistExp[\[Xi]r[[k]],q[[k]]].g[L[i-1],L[i],0];
　　g[I,L[i]]=g[I,L[i-1]].g[L[i-1],L[i]];
　　V[i]=Ad[Iv[g[L[i-1],L[i]]]].V[i-1] \[Xi]rb[[k]]*dq[[k]];
　　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]]];
　For[i=n 1,i>=2,i--,　(*力后向迭代：從最后一個連桿開始到第二個連桿*)
　　k=i-1;
　　Fex[i]=T[Ad[RToH[HToR[g[I,L[i]]]]]].gravity[i]; (*連桿受到的重力*)
　　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];
　　\[Tau][k]=\[Xi]rb[[k]].Fin[i]];
  Array[\[Tau],n]
, {t, 0, endTime, dt}];//AbsoluteTiming

?

?

　　在重力作用下，機械臂保持“立正姿勢”需要多大力矩呢？將初始狀態(tài)設為 0，經過逆動力學計算得到的答案是 {0,?38.1,?38.1,0,?2.06,0} {0,?38.1,?38.1,0,?2.06,0}。如果把這個力矩再帶入正動力學仿真就能看到機械臂保持靜止不動，這證明我們的逆動力學模型也是正確的。
　　
8. 結尾

?

　　本文以 Mathematica 通用數(shù)學計算軟件為平臺，針對串聯(lián)機械臂的建模、規(guī)劃和控制中的基本問題進行了仿真，差不多該說再見了。不過新的篇章即將展開 —— 移動機器人是另一個有趣的領域。與固定的機器人相比，移動機器人更有挑戰(zhàn)性，因此也會出現(xiàn)新的問題，比如信息的獲取和利用。未來將加入移動機器人仿真的功能，支持地面移動機器人的運動控制、環(huán)境約束下的運動規(guī)劃、移動機械臂、多機器人避障、多機器人編隊運動等，并討論環(huán)境建模、導航與定位、SLAM、非完整約束、最優(yōu)控制、輪式車輛和履帶車輛的動力學模型以及地面力學模型、群集協(xié)同運動等問題，敬請期待喲！

?

?

9. 補充： Mathematica 的缺點

?

　　在筆者就讀研究生期間，Matlab 的使用率頗高。每次參加答辯、聽報告，看著同學或老師用 Matlab 制作的丑陋不堪的圖表和動畫，心中就想把 Matlab 的界面設計師槍斃十分鐘。再加上呆板的函數(shù)定義和使用方式、缺乏對部分機器人仿真功能的支持，讓我不得不尋找其它的替代軟件?？墒窃诰W(wǎng)絡發(fā)達的今天，我居然找不到稍微像點樣的介紹機器人仿真的文章以及原理性代碼，要么過于低端，要么是東拼西湊，于是想把自己的經驗寫出來，并公開代碼。
　　就像 Matlab 有很多讓人不爽的地方一樣，Mathematica 用于機器人仿真同樣存在一些缺陷。我們之前在碰撞檢測部分已經提過，要想達到很快的檢測速度就不得不使用簡單的幾何模型。雖然 Mathematica 的函數(shù)也經過了優(yōu)化，但是只適用于需要較少計算次數(shù)的場合，在多次處理大量數(shù)據(jù)時還是比較慢。Mathematica 本身是用 C 語言寫成的，如果某個函數(shù)被大量調用可以考慮用 C 語言寫成動態(tài)鏈接庫（dll），然后在 Mathematica 中調用，這就像 Matlab 中的 MEX 文件。
　　調試找錯是個痛苦的過程，在 Mathematica 中更是這樣。Mathematica 支持調試時設置中斷，可惜使用起來不太方便。Mathematica 提供了一個專門用來開發(fā)調試的軟件——Workbench，操作同樣繁瑣。 在調試時，我只好使用 Print 函數(shù)打出中間計算結果來檢查程序是否正確。Mathematica 缺少像 Matlab 一樣的變量監(jiān)控窗口可以實時看到變量的值，這在調試時顯得很不方便（雖然在堆棧中可以監(jiān)視局部變量）。所以，盡量不要在 Mathematica 中使用中斷[9] [9]。Matlab 中如果出現(xiàn)語法錯誤，編譯會中止并顯示出錯位置，但在 Mathematica 中卻不會自動停止，它仿佛像推土機一樣停不下來。一旦出錯你通常只會在計算結果中看到一堆冗長的代碼，卻很難發(fā)現(xiàn)錯在哪了。對于包含許多步的計算過程，調試起來可能很浪費時間，最好的方法是把代碼切割成功能簡單清晰的模塊，挨個檢驗每個模塊要比檢驗一堆糾纏不清的代碼更容易。
　　
參考文獻
[1]　一種高效的開放式關節(jié)型機器人3D仿真環(huán)境構建方法，甘亞輝，機器人，2012.
[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, IEEE Robotics & Automation Magazine, 1994.
[5]　機器人學導論，John J. Craig，中文第三版.
[6]　Robotics - Modelling, Planning and Control, Bruno Siciliano, 582頁.
[7]　Lie Group Formulation of Articulated Rigid Body Dynamics, Junggon Kim, 2012.
[8]　The Acceleration Vector of a Rigid Body, Roy Featherstone, The International Journal of Robotics Research, 2001.
[9]　Automatically Add Debug Contents to a Program, StackExchange, 2016.

---------------------
作者：robinvista
來源：CSDN
原文：https://blog.csdn.net/robinvista/article/details/70231205
版權聲明：本文為作者原創(chuàng)文章，轉載請附上博文鏈接！