?
目的
本文手把手教你在 Mathematica 科學(xué)計(jì)算軟件中搭建機(jī)器人的仿真環(huán)境，具體包括以下內(nèi)容：
1 導(dǎo)入機(jī)械臂的三維模型
2 正\逆運(yùn)動(dòng)學(xué)仿真
3 碰撞檢測(cè)
4 軌跡規(guī)劃
5 正\逆動(dòng)力學(xué)仿真
6 運(yùn)動(dòng)控制
文中的代碼和模型文件點(diǎn)擊此處下載，或者此處：https://github.com/robinvista/Mathematica 。使用的軟件版本是 Mathematica 11.1，較早的版本可能缺少某些函數(shù)，所以最好使用最新版。進(jìn)入正文之前不妨先看幾個(gè)例子：
?
逆運(yùn)動(dòng)學(xué) 雙臂協(xié)作搬運(yùn)
顯示運(yùn)動(dòng)痕跡 ?。ㄆ揭疲┝憧臻g運(yùn)動(dòng)
無論你從事的是機(jī)器人研發(fā)還是教學(xué)科研，一款好用的仿真軟件都能對(duì)你的工作起到很大的幫助。那么應(yīng)該選擇哪個(gè)軟件呢？最方便的選擇就是成熟的商業(yè)軟件，例如Adams、Webots。你的錢不是白花的，商業(yè)軟件功能強(qiáng)大又穩(wěn)定，而且性能一般都經(jīng)過了優(yōu)化。可是再強(qiáng)大的商業(yè)軟件也有設(shè)計(jì)不合理的地方，它們的算法基本都是“黑箱”，你想做一點(diǎn)更改都不行。從學(xué)習(xí)和研究的角度出發(fā)，最好選擇代碼可修改的開源或半開源軟件。
在論文數(shù)據(jù)庫中檢索一下就會(huì)發(fā)現(xiàn)，很多人都選擇借助Matlab這個(gè)數(shù)學(xué)軟件平臺(tái)進(jìn)行機(jī)器人的建模仿真[1] [1]。這并不奇怪，因?yàn)镸atlab具有優(yōu)秀的數(shù)值計(jì)算和仿真能力，在它的基礎(chǔ)上開發(fā)會(huì)很便利。如果你非要舍近求遠(yuǎn)，用 C 編寫一套仿真軟件，先不要說仿真結(jié)果如何顯示，光是矩陣計(jì)算的實(shí)現(xiàn)細(xì)節(jié)就能讓你焦頭爛額。
?
?
與大名鼎鼎的Matlab 相比，Mathematica在國內(nèi)知名度并不高，但是不要小看它哦，一旦熟悉了你會(huì)刮目相看。我簡單對(duì)比了一下二者在機(jī)器人仿真方面的特點(diǎn)，見下表。由于Mathematica不俗的表現(xiàn)，我選擇在它的基礎(chǔ)上搭建仿真環(huán)境。如果你對(duì)Mathematica不熟悉，可以看網(wǎng)絡(luò)教程，也可以參考我的學(xué)習(xí)筆記。Mathematica有著陡峭的學(xué)習(xí)曲線，入門并不容易，其實(shí)初學(xué)者最快的入門方法就是照著大量的例子演練。本文面向Mathematica的初學(xué)者，所以不會(huì)使用過于高超的編程技巧。
?
| ? | ? | ? | ? |
|---|---|---|---|
| ? | Matlab | Mathematica | ? |
| 可視化效果 | 一般 | 優(yōu)秀 | ? |
| 導(dǎo)入機(jī)器人模型 | 需要自己寫函數(shù)[1] [1] | 自帶函數(shù) | ? |
| 機(jī)器人工具箱/包 | Robotic Toolbox[2] [2]、SpaceLib等 | Screws[3] [3]、Robotica[4] [4]、ModernRobotics等 | ? |
| 調(diào)試功能 | 方便易用 | 目前還不太方便，有點(diǎn)繁瑣 | ? |
| 代碼長度（個(gè)人經(jīng)驗(yàn)） | 100 100 | 20～50 20～50 | ? |
1. 獲取機(jī)器人的外觀模型
?
制作逼真的仿真首先需要的是機(jī)器人的外觀模型。如果你有機(jī)器人的三維模型最好，沒有的話也不要緊，著名的機(jī)器人制造商都在官網(wǎng)提供其各種型號(hào)機(jī)器人的真實(shí)模型，例如 ABB、安川，供大家免費(fèi)下載和使用。為了防止山寨，這些模型都是不可通過建模軟件直接修改的格式，例如 IGES 和 STEP 格式。但我們只用來顯示和碰撞檢測(cè)，所以并不影響仿真。
?
?
2. 導(dǎo)入機(jī)器人的外觀模型
?
獲得模型后要導(dǎo)入Mathematica中進(jìn)行處理并顯示，下面我們借助一個(gè)例子說明具體的步驟。Motoman ES165D 是安川公司生產(chǎn)的一款6自由度點(diǎn)焊機(jī)器人，其最后三個(gè)關(guān)節(jié)軸線相交于一點(diǎn)，這是一種非常經(jīng)典而且有代表性的設(shè)計(jì)，因此我們選擇以這款機(jī)器人為例進(jìn)行講解（這個(gè)機(jī)器人的完整模型點(diǎn)擊此處下載）。
用SolidWorks 2014軟件打開機(jī)器人的裝配體文件，并選擇“另存為”STL 格式。然后打開當(dāng)前頁面下方的“選項(xiàng)”對(duì)話框，如下圖。這里我們要設(shè)置三個(gè)地方：
1. “品質(zhì)”表示對(duì)模型的簡化程度，如果你想實(shí)現(xiàn)非常逼真的效果，可以選擇“精細(xì)”，但缺點(diǎn)是數(shù)據(jù)點(diǎn)很多導(dǎo)致文件很大、處理起來比較慢。一般選擇“粗糙”就夠了；
2. 勾選“不要轉(zhuǎn)換 STL 輸出數(shù)據(jù)到正的坐標(biāo)空間”；
3. 不要勾選“在單一文件中保存裝配體的所有連桿”。
?
?
?
小技巧 STL格式是一種三維圖形格式，被很多三維建模軟件支持（Mathematica也支持，所以我們要保存為這個(gè)格式）。STL格式只存儲(chǔ)三維圖形的表面信息，而且是用很多的三角形對(duì)圖形進(jìn)行近似的表示。如果你的機(jī)器人外形比較簡單（規(guī)則的幾何體），那么得到的STL文件大小一般只有幾十KB ；可是如果外形很復(fù)雜（比如包含很多的孔洞、曲面），生成的STL文件會(huì)很大（幾MB～幾十MB）。對(duì)于一般配置的計(jì)算機(jī)，模型文件超過100KB用Mathematica處理起來就比較慢了。為了讓仿真顯示地更流暢，可以利用免費(fèi)軟件MeshLab對(duì)其進(jìn)行化簡，MeshLab通常能夠在基本不改變外形的前提下將文件大小縮減為原來的十分之一甚至更多。
MeshLab的安裝和操作都是傻瓜式的，打開后進(jìn)入如下圖左所示的菜單中，出現(xiàn)右圖的頁面，這里的“Percentage reduction”表示縮減的百分比（1 表示不縮減，0.1 則表示縮減為原來的10%），設(shè)置好后點(diǎn)擊Apply并保存即可。?
?
然后在 Mathematica中導(dǎo)入 STL 文件，使用的代碼如下（假設(shè)這些 STL 文件保存在 D:\MOTOMAN 文件夾下）：
?
SetDirectory["D:\\MOTOMAN"]; (*設(shè)置文件存放的地址，注意次級(jí)路徑用雙斜杠*)
n = 6; (*n是機(jī)械臂的自由度，文章后面還會(huì)用到*)
partsName = {"1.stl", "2.stl", "3.stl", "4.stl", "5.stl", "6.stl", "7.stl", "8.stl", "9.stl"}; (*分別是組成機(jī)械臂的9個(gè)連桿*)
robotPartsGraphics = Import[#, "Graphics3D"] & /@ partsName; (*一次性導(dǎo)入所有連桿，并且導(dǎo)入為可以直接顯示的圖形格式*)
robotParts = robotPartsGraphics[[;; , 1]]; (*從三維圖形中提取出幾何數(shù)據(jù)：頂點(diǎn)的三維坐標(biāo)和邊*)
?
?
這里我偷了個(gè)懶，為了少打些字，我把導(dǎo)出連桿的文件名改成了從1到9的數(shù)字（這個(gè)機(jī)械臂的裝配體一共包含9個(gè)零件）。我們把導(dǎo)入的模型顯示出來，效果如下圖。使用的代碼如下
?
Graphics3D[{frame3D, robotParts}]
?
?
說明：frame3D是三維坐標(biāo)系的三個(gè)正交的軸（xyz xyz軸的顏色分別是RGB RGB）。在機(jī)器人領(lǐng)域會(huì)用到大量的坐標(biāo)系及其變換，直接看數(shù)字總是不直觀，不如將坐標(biāo)系顯示出來更方便。定義 frame3D 的代碼如下，這個(gè)坐標(biāo)系默認(rèn)原點(diǎn)的位置在(0,0,0) (0,0,0)，以后我們稱這個(gè)坐標(biāo)系為“全局坐標(biāo)系”。
?
frame3D = {RGBColor[#], Arrowheads[0.05], Arrow[Tube[{{0, 0, 0}, #}, 0.01]]} & /@ IdentityMatrix[3];
?
?
?
你可能會(huì)好奇：導(dǎo)入的零件數(shù)據(jù)是以什么樣的格式儲(chǔ)存的呢？
用來存儲(chǔ)機(jī)器人外形數(shù)據(jù)的robotParts變量包含一共9個(gè)零件的數(shù)據(jù)，假如你想看第一個(gè)零件（對(duì)應(yīng)的是基座，它通常用來將機(jī)械臂固定在大地上），可以輸入：
?
robotParts[[1]] (*雙層方括號(hào)中的數(shù)字表示對(duì)應(yīng)第幾個(gè)零件*)
?
?
運(yùn)行后的輸出結(jié)果是一堆由 GraphicsComplex 函數(shù)包裹著的數(shù)字，主要可分為兩部分：第一部分是零件幾何體所有頂點(diǎn)的三維坐標(biāo)；第二部分是組成零件幾何體的三角形（注意：構(gòu)成每個(gè)三角形的三個(gè)頂點(diǎn)是第一部分點(diǎn)的序號(hào)，而不是坐標(biāo)值）。我們可以用以下代碼將其分別顯示出來：
?
pts = robotParts[[1]][[1]]; (*零件1的第一部分：頂點(diǎn)的三維坐標(biāo)數(shù)據(jù)*)
triangles = robotParts[[1]][[2]]; (*零件1的第二部分：三角形面片*)
trianglesBlue = triangles /. {EdgeForm[] -> EdgeForm[Blue]}; (*三角形的邊顯示為藍(lán)色*)
Graphics3D[{GraphicsComplex[pts, trianglesBlue], Red, Point[pts]}]
?
?
?
所有零件都成功地導(dǎo)入了，而且它們的相對(duì)位置也是正確的。你可能會(huì)問：機(jī)械臂為什么是處于“躺著”的姿勢(shì)呢？這是由于零件是按照 SolidWorks 默認(rèn)的坐標(biāo)系（y y 軸向上）繪制和裝配的。而在 Mathematica 中默認(rèn)的坐標(biāo)系是 z z 軸向上。那么我們采用哪個(gè)坐標(biāo)系呢？
當(dāng)然你可以任性而為，用哪個(gè)都可以。不過根據(jù)國家標(biāo)準(zhǔn)GBT 16977-2005 《工業(yè)機(jī)器人 坐標(biāo)系和運(yùn)動(dòng)命名原則》，機(jī)械臂基座坐標(biāo)系的 z z 軸應(yīng)該垂直于基座安裝面（一般是水平地面）、指向?yàn)橹亓铀俣鹊姆捶较颍ㄒ簿褪谴怪钡孛嫦蛏希?x x 軸指向機(jī)器人工作空間中心點(diǎn)的方向。制定國家標(biāo)準(zhǔn)的都是些經(jīng)驗(yàn)豐富的專家老手，我們最好跟國標(biāo)保持一致（國標(biāo)的作圖水平就不能提高點(diǎn)嗎？這圖怎么感覺像小學(xué)生畫的）。
?
?
為了讓機(jī)器人變成國標(biāo)規(guī)定的姿勢(shì)，需要旋轉(zhuǎn)各個(gè)連桿。我們先想想應(yīng)該怎么轉(zhuǎn)：結(jié)合我們之前導(dǎo)入的圖形，可以先繞全局坐標(biāo)系的 x x 軸轉(zhuǎn) 90° 90°，再繞全局坐標(biāo)系的 z z 軸轉(zhuǎn) 90° 90°。還有一種方法是：先繞全局坐標(biāo)系的 x x 軸轉(zhuǎn) 90° 90°（記這個(gè)旋轉(zhuǎn)后的坐標(biāo)系為 A A），再繞 A A 的 y y 軸轉(zhuǎn) 90° 90°。兩種方法的效果是一樣的，但是注意合成矩陣時(shí)乘法的順序（見以下代碼），不懂的同學(xué)可以看看文獻(xiàn)[5] [5]中的31 ～ ～ 33頁。當(dāng)然，轉(zhuǎn)動(dòng)是有正負(fù)之分的：將你的右手握住某個(gè)坐標(biāo)軸，豎起大拇指，讓大拇指和軸的正方向一致，這時(shí)四指所示的方向就是繞該軸轉(zhuǎn)動(dòng)的正方向。
為此，我們定義旋轉(zhuǎn)矩陣（兩種定義效果一樣）：
?
{Xaxis,Yaxis,Zaxis}=IdentityMatrix[3]; (*定義三個(gè)旋轉(zhuǎn)軸*)
rot = RotationMatrix[90 Degree, Zaxis].RotationMatrix[90 Degree, Xaxis]; (*注意第二次變換是在左邊乘*)
rot = RotationMatrix[90 Degree, Xaxis].RotationMatrix[90 Degree, Yaxis]; (*注意第二次變換是在右邊乘*)
?
?
然后用rot矩陣旋轉(zhuǎn)每個(gè)連桿（的坐標(biāo)，即保存在第一部分robotParts[[i, 1]]中的數(shù)據(jù)）：
?
robotParts=Table[GraphicsComplex[rot.# & /@ robotParts[[i, 1]], robotParts[[i, 2]]], {i, 9}];
?
?
經(jīng)過姿態(tài)變換后的機(jī)器人看起來舒服點(diǎn)了，只是有些蒼白。為了給它點(diǎn)個(gè)性（也方便區(qū)分不同的零件或者稱為連桿），我們給連桿設(shè)置一下顏色，代碼如下。你可能注意到了，這里我沒有使用循環(huán)去為9個(gè)連桿一個(gè)一個(gè)地設(shè)置顏色，而是把同類的元素（顏色）寫在一起，然后再和連桿列表一起轉(zhuǎn)置即可把顏色“分配”給各個(gè)連桿。這樣做的好處就是代碼比較簡潔、清晰，以后我們會(huì)經(jīng)常這么做。
?
colors = {Gray, Cyan, Orange, Yellow, Gray, Green, Magenta, LightGreen, Pink}; (*1~9 各連桿的顏色*)
robotPartsColored = Transpose[{colors, robotParts}]; (*把顏色分配給各連桿*)
Graphics3D[robotPartsColored]
?
?
?
說明：現(xiàn)在的機(jī)器人姿勢(shì)（大臂豎直、小臂前伸）是6自由度機(jī)械臂的“零位”狀態(tài)，我們將此時(shí)機(jī)械臂各關(guān)節(jié)的角度認(rèn)為是0。一般機(jī)械臂上都有各關(guān)節(jié)的零點(diǎn)位置標(biāo)記，用于指示各關(guān)節(jié)的零點(diǎn)。我們用控制器控制機(jī)械臂時(shí)，發(fā)出的角度指令都是相對(duì)于這個(gè)零點(diǎn)位置的。零點(diǎn)位置不是必須遵守的，你可以將任意的角度設(shè)置為零位，不過為了統(tǒng)一，最好用機(jī)械臂固有的零位——也就是當(dāng)前姿勢(shì)下各關(guān)節(jié)的角度。
?
3. 運(yùn)動(dòng)學(xué)仿真
?
前面的工作只是讓機(jī)械臂的模型顯示出來。如果想讓它動(dòng)起來，那就要考慮運(yùn)動(dòng)學(xué)了。機(jī)器人這個(gè)學(xué)科聽起來高大上（很多都停留在理論上），可實(shí)際上現(xiàn)在大多數(shù)工業(yè)機(jī)器人的控制方式還是比較低級(jí)的，它們只用到了運(yùn)動(dòng)學(xué)，高級(jí)一點(diǎn)的動(dòng)力學(xué)很少用，更不要提智能了（它們要說自己有智能，我們家的洗衣機(jī)和電視機(jī)都要笑掉大牙了）?？磥硪褂脵C(jī)器人，運(yùn)動(dòng)學(xué)是必不可少的，所以我們先來實(shí)現(xiàn)運(yùn)動(dòng)學(xué)。
在建立運(yùn)動(dòng)學(xué)模型之前我們需要了解機(jī)器人的機(jī)械結(jié)構(gòu)。前面提到，MOTOMAN-ES165D 是一個(gè)6自由度的串聯(lián)機(jī)械臂。而6個(gè)自由度的機(jī)器人至少由7個(gè)連桿組成（其中要有一個(gè)連桿與大地固定，也就是基座）?？墒俏覀儗?dǎo)入的連桿有9個(gè)，多出來的2個(gè)連桿是彈簧缸（基座上黃色的圓筒）的組成部分。MOTOMAN-ES165D 機(jī)器人能夠抓持的最大負(fù)載是165公斤，彈簧缸的作用就是作為配重平衡掉一部分負(fù)載的重量，要不然前端的關(guān)節(jié)電機(jī)會(huì)有很大的負(fù)擔(dān)?？墒菑椈筛捉o我們的建模造成了麻煩，因?yàn)樗鼘?dǎo)致“樹形拓?fù)洹币约按嬖凇伴]鏈”，這不太好處理。為此，我們先忽略掉彈簧缸。
3.1 連桿的局部坐標(biāo)系
?
機(jī)器人的運(yùn)動(dòng)也就是其構(gòu)成連桿的運(yùn)動(dòng)。為了表示連桿的運(yùn)動(dòng)，我們要描述每個(gè)連桿的位置和姿態(tài)（合稱為“位姿”）。通常的做法是在每個(gè)連桿上固定一個(gè)坐標(biāo)系（它跟隨連桿一起運(yùn)動(dòng)），這個(gè)坐標(biāo)系被稱為“局部坐標(biāo)系”。通過描述局部坐標(biāo)系的位姿我們就可以描述每個(gè)連桿的位姿。如何選擇局部坐標(biāo)系呢？理論上你可以任意選擇，不過局部坐標(biāo)系影響后續(xù)編程和計(jì)算的難易程度，所以我們?cè)谶x擇時(shí)最好慎重。在運(yùn)動(dòng)學(xué)建模和動(dòng)力學(xué)建模中，坐標(biāo)系的選擇通常是不同的：
● 運(yùn)動(dòng)學(xué)建模時(shí)，連桿的局部坐標(biāo)系一般放置在關(guān)節(jié)處，這是因?yàn)槌Ｓ玫?D-H 參數(shù)是根據(jù)相鄰關(guān)節(jié)軸定義的；
● 動(dòng)力學(xué)建模時(shí)，連桿的局部坐標(biāo)系一般放置在質(zhì)心處，這是因?yàn)榕ｎD方程是關(guān)于質(zhì)心建立的，而且關(guān)于質(zhì)心的轉(zhuǎn)動(dòng)慣量是常數(shù)，這方便了計(jì)算。
我們先考慮運(yùn)動(dòng)學(xué)，因此將局部坐標(biāo)系設(shè)置在關(guān)節(jié)處。在SolidWorks中打開任何一個(gè)連桿，都能看到它自己有一個(gè)坐標(biāo)系。描述一個(gè)連桿的每一條邊、每一個(gè)孔的坐標(biāo)都以這個(gè)坐標(biāo)系為參考，我們稱它為“繪圖坐標(biāo)系”。繪圖坐標(biāo)系通常不在質(zhì)心處，因?yàn)樵谀氵€沒畫完連桿之前你根本不知道它的質(zhì)心在哪里。繪圖坐標(biāo)系通常在連桿的對(duì)稱中心或者關(guān)節(jié)處，我們不妨將每個(gè)連桿的繪圖坐標(biāo)系當(dāng)做它的局部坐標(biāo)系。
那么下一個(gè)問題是每個(gè)連桿的繪圖坐標(biāo)系在哪兒呢？我們以第三個(gè)連桿為例說明，如下圖左所示，點(diǎn)擊SolidWorks左側(cè)的“原點(diǎn)”標(biāo)簽，圖中就會(huì)顯示繪圖坐標(biāo)系的原點(diǎn)。（如果你想將繪圖坐標(biāo)系顯示出來，可以先選中“原點(diǎn)”標(biāo)簽，然后點(diǎn)擊上方菜單欄中的“參考幾何體”，再選擇“坐標(biāo)系”，然后直接回車即可看到新建的繪圖坐標(biāo)系，如右圖，可見它位于上面的關(guān)節(jié)軸）
然后回到機(jī)器人的裝配體中，在左側(cè)的連桿樹中展開每個(gè)連桿找到并選中其繪圖坐標(biāo)系的原點(diǎn)，然后點(diǎn)擊上方菜單欄“評(píng)估”中的“測(cè)量”即可看到圖中出現(xiàn)了一組坐標(biāo)值（如下圖所示），這就是連桿繪圖坐標(biāo)系的原點(diǎn)在全局坐標(biāo)系（本文將全局坐標(biāo)系定義為裝配體的坐標(biāo)系）中的位置。
?
?
我們記錄下所有連桿的繪圖坐標(biāo)系的原點(diǎn)位置（除去彈簧缸的2個(gè)，注意 SolidWorks 中默認(rèn)的單位是毫米，這里除以 1000 1000 是為了變換到 Mathematica 中使用的國際標(biāo)準(zhǔn)單位——米）：
?
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;
?
?
因?yàn)槲覀兪窃?SolidWorks 中測(cè)量的位置，所以這些位置值還是相對(duì)于 SolidWorks 的坐標(biāo)系（y y 軸朝上），要變到 z z 軸朝上，方法仍然是乘以旋轉(zhuǎn)矩陣 rot：
?
drawInGlobal = Table[rot.i, {i, drawInGlobalSW}];
?
?
以后我們會(huì)經(jīng)常對(duì)點(diǎn)的坐標(biāo)進(jìn)行各種各樣的變換（平移、旋轉(zhuǎn)），而且很多時(shí)候是用一個(gè)矩陣同時(shí)對(duì)很多個(gè)點(diǎn)的坐標(biāo)進(jìn)行變換（例如上面的例子），不如定義一個(gè)算子以簡化代碼。我們可以定義算子（其實(shí)是一個(gè)函數(shù)）：
?
CircleDot[matrix_,vectors_]:=(matrix.#)&/@vectors;
?
?
所以前面的變換用我們自定義的算子表示如下（復(fù)制到 Mathematica中后 \[CircleDot] 會(huì)變成一個(gè)Mathematica內(nèi)部預(yù)留的圖形符號(hào)⊙ ⊙，這個(gè)符號(hào)沒有被占用，所以這里我征用了）：
?
drawInGlobal = rot\[CircleDot]drawInGlobalSW; (*哈哈！寫起來是不是簡單多了*)
?
?
說明：本文出現(xiàn)的所有自定義的函數(shù)都給出了實(shí)現(xiàn)代碼（Mathematica 自帶的函數(shù)首字母都是大寫。為了與官方函數(shù)區(qū)分，我自定義的函數(shù)有些采用小寫字母開頭，不過建議大家采用windows編程常用的命名法，即變量名首字母小寫，中間字母大寫：myVariable，而函數(shù)名首字母和中間字母都大寫：MyFunction）。為了方便，我將這些自定義函數(shù)打包成一個(gè)函數(shù)包，每次運(yùn)行程序時(shí)導(dǎo)入此函數(shù)包即可使用里面的函數(shù)。注意該函數(shù)包依賴另一個(gè)函數(shù)包 Screws.m[3] [3] ，該包基于旋量理論，可在此處下載：http://www.cds.caltech.edu/~murray/books/MLS/software.html （為了寫起來省事，我修改了其中部分函數(shù)的名字，為此重新定義了 myScrews.m）。在程序中導(dǎo)入函數(shù)包的代碼如下（如果函數(shù)包位于你的程序筆記本文件的同一目錄下）:
?
SetDirectory[NotebookDirectory[]]
<< myFunction.m
?
?
還記得嗎？最開始我們導(dǎo)入機(jī)器人模型時(shí)，各連桿的位置都已經(jīng)按照裝配關(guān)系確定好了，所以它們的坐標(biāo)也是相對(duì)于全局坐標(biāo)系描述的?？墒乾F(xiàn)在我們要讓機(jī)械臂動(dòng)起來（并且顯示出來），這就需要移動(dòng)這些坐標(biāo)。為了方便起見，最好能將每個(gè)連桿的坐標(biāo)表示在自己的繪圖坐標(biāo)系中，因?yàn)檫@樣我們只需要移動(dòng)繪圖坐標(biāo)系就行了，而各點(diǎn)的坐標(biāo)相對(duì)它們所屬的繪圖坐標(biāo)系是不動(dòng)的。應(yīng)該怎么做呢？很簡單，將連桿的坐標(biāo)減去繪圖坐標(biāo)系的原點(diǎn)在全局坐標(biāo)系中的坐標(biāo)即可：
?
partsName = {"1.stl", "2.stl", "3.stl", "6.stl", "7.stl", "8.stl", "9.stl"}; (*已經(jīng)去除了組成彈簧缸的2個(gè)零件：4號(hào)和5號(hào)*)
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};
?
?
坐標(biāo)移動(dòng)后的連桿如下圖所示（圖中的坐標(biāo)系是各個(gè)連桿自己的繪圖坐標(biāo)系，我沒有對(duì)坐標(biāo)轉(zhuǎn)動(dòng)，所以繪圖坐標(biāo)系和全局坐標(biāo)系的姿態(tài)相同）。我們一開始從 SolidWorks 導(dǎo)出文件時(shí)是一次性地導(dǎo)出整個(gè)裝配體的。其實(shí)，如果我們挨個(gè)打開各個(gè)連桿并且一個(gè)一個(gè)的導(dǎo)出這些連桿，那么得到數(shù)據(jù)就是相對(duì)于各自的繪圖坐標(biāo)系的，只不過這樣稍微麻煩一點(diǎn)。
?
?
3.2 利用旋量建立運(yùn)動(dòng)學(xué)模型
?
下面我們討論如何建立運(yùn)動(dòng)學(xué)模型。描述機(jī)器人連桿之間幾何關(guān)系的經(jīng)典方法是采用 D-H 參數(shù)（Denavit - Hartenberg parameters）。D-H 參數(shù)巧妙在什么地方呢？我們知道，完全確定兩個(gè)坐標(biāo)系（或者剛體）的位姿關(guān)系需要6個(gè)參數(shù)，因?yàn)槿S空間中的剛體有6個(gè)自由度。如果不考慮關(guān)節(jié)轉(zhuǎn)動(dòng)（平移）仍需要5個(gè)參數(shù)。然而 D-H 參數(shù)居然只用了4個(gè)參數(shù)就能夠確定相鄰連桿的位姿關(guān)系，可見 Denavit 和 Hartenberg 這哥倆確實(shí)動(dòng)了番腦筋。不過為了避免 D-H 參數(shù)的一些缺點(diǎn)，我們棄之不用而采用旋量的表示方法。剛接觸旋量的同學(xué)會(huì)覺得它很難理解。其實(shí)旋量有什么性質(zhì)、它和剛體運(yùn)動(dòng)的關(guān)系又是什么？這些問題數(shù)學(xué)家也是用了很長時(shí)間才搞清楚。在本文中你可以把旋量簡單想象成一個(gè)描述關(guān)節(jié)轉(zhuǎn)動(dòng)的向量。三維空間中的旋量是一個(gè)6維向量，要描述一個(gè)關(guān)節(jié)旋量需要確定一個(gè)關(guān)節(jié)軸線的方向向量（3個(gè)參數(shù)）和軸線上任意一點(diǎn)的坐標(biāo)（又要3個(gè)參數(shù)）。
旋量和向量相似的一個(gè)地方是，對(duì)它的描述也是相對(duì)于一個(gè)坐標(biāo)系的。我們選擇哪個(gè)坐標(biāo)系呢？這里我們要參考 D-H 參數(shù)，每一個(gè)連桿坐標(biāo)系在定義時(shí)都相對(duì)于前一個(gè)連桿的坐標(biāo)系。所以我們將每個(gè)關(guān)節(jié)軸的旋量表示在前一個(gè)連桿中。這次我們以2號(hào)連桿為例說明如何確定關(guān)節(jié)運(yùn)動(dòng)對(duì)應(yīng)的旋量：
1. 首先來看關(guān)節(jié)軸線的方向，這個(gè)要相對(duì)于2號(hào)連桿的繪圖坐標(biāo)系。（我們要確定關(guān)節(jié)2的旋量，至于關(guān)節(jié)1的旋量最好在連桿1中確定）。從下圖中看關(guān)節(jié)2的軸線方向似乎是 x x 軸，可是我們前面將繪圖坐標(biāo)系的姿態(tài)和全局坐標(biāo)系的姿態(tài)設(shè)定為一樣的，所以應(yīng)該在全局坐標(biāo)系中確定，也就是 y y 軸。
2. 關(guān)節(jié)軸線上任意一點(diǎn)的坐標(biāo)，這個(gè)同樣要相對(duì)于2號(hào)連桿的繪圖坐標(biāo)系。我們?cè)谳S線上任選一點(diǎn)即可。步驟是：點(diǎn)擊 SolidWorks 上方菜單欄的“參考幾何體”，選擇“點(diǎn)”，然后在左側(cè)面板選擇“圓弧中心”，然后選擇圖中的關(guān)節(jié)軸周圍的任意同心圓弧即可創(chuàng)建一個(gè)參考點(diǎn)，這個(gè)點(diǎn)就是我們想要的。我們可以在連桿視圖中測(cè)量這個(gè)點(diǎn)的坐標(biāo)，也可以在機(jī)器人完整裝配體中測(cè)量，這里我選擇后者。（測(cè)量步驟參照前面測(cè)量“連桿繪圖坐標(biāo)系的原點(diǎn)”）
定義關(guān)節(jié)旋量的代碼如下。其中相對(duì)旋量 ξr ξr 用于迭代運(yùn)動(dòng)學(xué)計(jì)算，它的含義是當(dāng)前連桿的轉(zhuǎn)軸表示在前一個(gè)連桿坐標(biāo)系中。
?
?
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}];
?
?
我們對(duì)關(guān)節(jié)的相對(duì)運(yùn)動(dòng)進(jìn)行了表示，然而要建立運(yùn)動(dòng)學(xué)還缺少一樣?xùn)|西：連桿間的初始相對(duì)位姿（初始的意思是機(jī)械臂處于“零位”的狀態(tài)下）。零位下，我們將所有連桿的姿態(tài)都認(rèn)為和全局坐標(biāo)系一樣，所以不用計(jì)算相對(duì)姿態(tài)了。至于它們的相對(duì)位置嘛，我們已經(jīng)知道了繪圖坐標(biāo)系原點(diǎn)在全局坐標(biāo)系中的坐標(biāo)，兩兩相減就可以得到它們的相對(duì)位置了，很簡單吧?。ㄒ娤旅娴拇a）
?
Do[g[L[i], L[i 1], 0] = PToH[drawInGlobal[[i 1]] - drawInGlobal[[i]]], {i, n}];
?
?
其中，PToH 函數(shù)能將位置向量轉(zhuǎn)換為一個(gè)4×4 4×4齊次變換矩陣，這是借助 RPToH 函數(shù)實(shí)現(xiàn)的（RPToH 函數(shù)就是 Screws 工具包[3] [3]中的 RPToHomogeneous 函數(shù)），它可以將一個(gè)3×3 3×3旋轉(zhuǎn)矩陣和3×1 3×1位移向量組合成一個(gè)4×4 4×4齊次變換矩陣。將旋轉(zhuǎn)矩陣和位移向量合成為齊次變換矩陣是我們以后會(huì)經(jīng)常用到的操作。類似的，也可以定義 RToH 函數(shù)將旋轉(zhuǎn)矩陣生成對(duì)應(yīng)的齊次變換矩陣，代碼如下：
?
RToH[R_]:= RPToH[R,{0,0,0}]
PToH[P_]:= RPToH[IdentityMatrix[3],P]
?
?
說明：本文中，用符號(hào)
I表示全局坐標(biāo)系（同時(shí)也是慣性坐標(biāo)系）；符號(hào)L[i]表示第 i i 個(gè)連桿，變量g[L[i], L[i 1]]表示第 i 1 i 1 個(gè)連桿相對(duì)于第 i i 個(gè)連桿的位姿矩陣（它是一個(gè)4×4 4×4齊次變換矩陣）；變量g[I, L[i]]表示什么你肯定猜到了，它表示第 i i 個(gè)連桿相對(duì)于全局坐標(biāo)系的位姿矩陣。如果不特別說明，本文總是用g（或者g開頭的變量）表示一個(gè)（或一組）齊次變換矩陣，這是約定俗成的。
?
現(xiàn)在可以正式推導(dǎo)機(jī)械臂的運(yùn)動(dòng)學(xué)模型了。在使用機(jī)械臂時(shí)，大家一般只關(guān)心其最末端連桿的位姿，更確切的說，是最末端連桿的位姿與關(guān)節(jié)角度的關(guān)系。不過為了得到最末端連桿的位姿，我們需要計(jì)算中間所有連桿的位姿。這里利用相鄰連桿的迭代關(guān)系——每個(gè)連桿的位姿依賴前一個(gè)連桿的位姿——來提升計(jì)算效率。所以，可以定義機(jī)械臂所有連桿的運(yùn)動(dòng)學(xué)函數(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]] 和所有關(guān)節(jié)的角度向量q∈R6 ∈R6，這組變量完整地描述了一個(gè)串聯(lián)機(jī)械臂的位置和姿勢(shì)（用機(jī)器人中的專業(yè)術(shù)語應(yīng)該叫“構(gòu)型”： configuration，注意不要翻譯為“配置”），而輸出則是所有連桿相對(duì)于全局坐標(biāo)系的4×4 4×4位姿矩陣（即 g[I, L[i]]，其中i = 1~7）。
其中，TwistExp 函數(shù)來自于 Screws 工具包[3] [3]，作用是構(gòu)造旋量的矩陣指數(shù)。
?
說明：在大多數(shù)的機(jī)器人教科書中，連桿的記號(hào)是從0開始的，也就是說將基座記為0號(hào)連桿，然后是1號(hào)連桿，最末端的連桿是n 1 n 1號(hào)（假設(shè)機(jī)械臂的自由度是n n）；而關(guān)節(jié)的記號(hào)是從1開始，也就是說1號(hào)關(guān)節(jié)連接0號(hào)連桿和1號(hào)連桿。這樣標(biāo)記的好處是記號(hào)一致，推導(dǎo)公式或編程時(shí)不容易出錯(cuò)：比如說我們計(jì)算第 i i 個(gè)連桿的速度時(shí)要利用第 i i 個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng)速度?？墒潜疚闹羞B桿的記號(hào)是從1開始的（基座標(biāo)記為1號(hào)連桿），我們保留0號(hào)標(biāo)記是為了以后將機(jī)械臂擴(kuò)展到裝在移動(dòng)基座（比如一個(gè)AGV小車）的情況，這時(shí)0號(hào)就用來表示移動(dòng)基座。
?
可以看到，只要定義好關(guān)節(jié)旋量，建立運(yùn)動(dòng)學(xué)模型非常簡單?？墒沁@樣得到的運(yùn)動(dòng)學(xué)模型對(duì)不對(duì)呢？我們來檢驗(yàn)一下。借助 Manipulate 函數(shù)，可以直接改變機(jī)械臂的各關(guān)節(jié)角度，并直觀地查看機(jī)械臂姿勢(shì)（應(yīng)該叫構(gòu)型了哦）的變化，如以下動(dòng)畫所示。可以看到，機(jī)械臂各連桿的運(yùn)動(dòng)符合我們?cè)O(shè)置的關(guān)節(jié)值，這說明運(yùn)動(dòng)學(xué)模型是正確的。
?
?
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]]
?
?
驗(yàn)證使用的代碼如上。其中，move3D 函數(shù)的功能是用一個(gè)齊次變換矩陣（g）移動(dòng)一個(gè)幾何圖形（shape）。這里還值得一提的是 MapThread 函數(shù)。雖然我們可以用 move3D 函數(shù)去一個(gè)一個(gè)地移動(dòng)連桿（寫起來就是：move3D[part1, g1], move3D[part2, g2], move3D[part3, g3]），這樣寫比較清楚也很容易讀懂，可就是太麻煩了。如果你的機(jī)械臂有一百個(gè)連桿，用這種方法豈不是要累死。當(dāng)然，我們可以使用循環(huán)，但是使用 MapThread 函數(shù)寫起來更簡單，即：MapThread[move3D, {{part1, part2, part3}, {g1, g2, g3}}]，而且得到的結(jié)果與前面完全一樣。這就是為什么我喜歡把同類型的元素都放到一起，因?yàn)椴僮鞯臅r(shí)候可以一起批量化進(jìn)行，使得代碼更簡潔。
可以看到，Mathematica 提供的控件類函數(shù) Manipulate 支持用戶使用鼠標(biāo)交互式地改變變量的值，同時(shí)動(dòng)態(tài)更新對(duì)應(yīng)的輸出。如果一段代碼運(yùn)行時(shí)間足夠快，就可以放在Manipulate 內(nèi)部，比如運(yùn)動(dòng)學(xué)函數(shù)robotPartsKinematics，它包含的計(jì)算并不復(fù)雜，但如果是后面要介紹的動(dòng)力學(xué)函數(shù)就不適合放在Manipulate里面了，因?yàn)閯?dòng)力學(xué)的計(jì)算比較耗時(shí)，窗口會(huì)顯得很“卡頓”。
隨著對(duì) Mathematica 越來越熟悉，你會(huì)逐漸體會(huì)到它的強(qiáng)大。比如說你可以隨心所欲地將已有的函數(shù)組合從而得到新的函數(shù)，如果一段代碼被頻繁地使用我們就可以這么干。舉個(gè)例子，我們可以借助robotPartsKinematics來定義一個(gè)新的函數(shù)robotMoveToQ，用來得到某個(gè)構(gòu)型下的機(jī)械臂的外觀：
?
robotMoveToQ[q_] := Module[{gs},
gs = robotPartsKinematics[{IdentityMatrix[4], q}];
MapThread[move3D, {robotPartsColored, gs}] ]
?
?
而且，函數(shù)的使用也很靈活。例如，我們可以將不同構(gòu)型下的機(jī)械臂同時(shí)顯示出來，只需要兩行代碼，如下：
?
qs = Table[{a - Pi/2, a/2, -a/2 Pi/6, 0, 0, 0}, {a, 0, Pi, 0.2}];(*生成關(guān)節(jié)變量列表，即不同構(gòu)型*)
Graphics3D[{robotMoveToQ /@ qs}]
?
?
?
4. 逆運(yùn)動(dòng)學(xué)仿真
?
借助運(yùn)動(dòng)學(xué)，我們成功地通過改變關(guān)節(jié)角度實(shí)現(xiàn)了對(duì)機(jī)械臂的控制。當(dāng)然這沒什么值得炫耀的，本質(zhì)上不過是矩陣相乘罷了。本節(jié)我們考慮一個(gè)更有挑戰(zhàn)性也更好玩的問題。如果告訴你所有連桿（局部坐標(biāo)系）的位姿，你能不能算出機(jī)械臂的各個(gè)關(guān)節(jié)角來？你一定會(huì)說這很簡單，求一下反三角函數(shù)就行了。但是實(shí)際應(yīng)用時(shí)經(jīng)常會(huì)遇到比這個(gè)難一些的問題：只告訴你機(jī)械臂最后一個(gè)連桿的位姿，如何得到各關(guān)節(jié)的角度？這個(gè)問題被稱為“逆運(yùn)動(dòng)學(xué)”。Robotic Toolbox工具箱[2] [2]中給出了兩個(gè)解逆運(yùn)動(dòng)學(xué)問題的函數(shù)：ikine 和 ikine6s，分別是數(shù)值解法和符號(hào)解析解法，本文我們也用兩種方式解決逆運(yùn)動(dòng)學(xué)問題。
?
4.1 數(shù)值解法之——解方程
?
上一節(jié)的運(yùn)動(dòng)學(xué)函數(shù) robotPartsKinematics 能得到所有連桿的位姿。大多數(shù)時(shí)候，人們只關(guān)心最后一個(gè)連桿的位姿（因?yàn)樗厦嬉b載操作工具），即 Last@robotPartsKinematics[{IdentityMatrix[4], q}]（注意q是一個(gè)六維向量，即q=(q1,q2,q3,q4,q5,q6 q1?,q2?,q3?,q4?,q5?,q6?)），結(jié)果如下圖所示（另存為可以看大圖）。這里關(guān)節(jié)角沒有設(shè)置數(shù)值，因此得到的是符號(hào)解，有些長哦。這也是為什么機(jī)器人領(lǐng)域經(jīng)常使用三角函數(shù)縮寫的原因：比如把 cos(q1) cos(q1?)記為c1 c1?。在[4] [4]中提供了一個(gè)函數(shù) SimplifyTrigNotation，可以用來對(duì)下式進(jìn)行縮寫。
?
?
如果我們想讓機(jī)械臂末端（連桿）到達(dá)某個(gè)（已知的）位姿 gt，也就是讓上面的矩陣等于這個(gè)位姿矩陣：
?
Last@robotPartsKinematics[{IdentityMatrix[4], {q1, q2, q3, q4, q5, q6}}] = gt (*逆運(yùn)動(dòng)學(xué)方程*)
?
?
通過解上面這個(gè)以6個(gè)關(guān)節(jié)角 (q1,q2,q3,q4,q5,q6) (q1?,q2?,q3?,q4?,q5?,q6?) 為未知量的方程組就能知道機(jī)械臂的構(gòu)型了。也就是說，逆運(yùn)動(dòng)學(xué)問題的本質(zhì)就是解方程。對(duì)于解方程我們一點(diǎn)也不陌生，從小到大我們解過無數(shù)的方程。甚至可以說數(shù)學(xué)這個(gè)學(xué)科本身有很大一部分就是在研究解方程、解各種各樣的方程：大規(guī)模的、小規(guī)模的，線性的、非線性的，代數(shù)的、微分的，常微分的、偏微分的。既然有這么多種方程，也就意味著存在很多種解法。Mathematica 提供的用來解方程的函數(shù)不止一個(gè)，例如有：Solve、NSolve、DSolve、LinearSolve、FindRoot 等等。面對(duì)這么多解方程的工具，我們應(yīng)該選哪個(gè)呢？你選擇的函數(shù)取決于方程的類型，所以我們先看看這個(gè)方程是什么類型呢？首先，它是個(gè)代數(shù)方程，所以不能使用求解微分方程的函數(shù)（DSolve）。其次，方程中包含未知量的三角函數(shù)，所以它是非線性代數(shù)方程，因此不能用求解線性方程的函數(shù)（LinearSolve）。再者，對(duì)于代數(shù)方程有數(shù)值解法和解析解法兩類方法。當(dāng)然，我們非常想得到用符號(hào)表示的解析解，因?yàn)橹恍枰庖淮我院笾苯訋霐?shù)值即可，計(jì)算速度非?？?。但是非線性方程一般很難得到符號(hào)解（對(duì)于這個(gè)機(jī)械臂，它存在符號(hào)解），所以我們只好退而求其次尋找數(shù)值解了，這樣就把范圍縮小到 NSolve、FindRoot 這兩個(gè)函數(shù)了。NSolve 會(huì)得到所有解（這個(gè)方程有不止一組解哦），而 FindRoot 會(huì)根據(jù)初始值得到最近的解。一番試驗(yàn)表明只有 FindRoot 能滿足要求。
?
說明：在求解逆運(yùn)動(dòng)學(xué)方程前還需要解決一個(gè)小問題：如何在 Mathematica 中表示一個(gè)期望的目標(biāo)位姿
gt呢？Mathematica 提供了RollPitchYawMatrix函數(shù)和EulerMatrix函數(shù)用來表示三維轉(zhuǎn)動(dòng)（你用哪個(gè)都可以），然后利用前面的RPToH函數(shù)合成為位姿矩陣gt即可，示例代碼如下。其中，cuboid函數(shù)用于繪制一個(gè)長方體。如果你使用 Matlab ，那我要可憐你了。因?yàn)?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]
?
?
?
不過這個(gè)方程組是由 4×4 4×4 齊次變換矩陣得到的，里面有 4×4=16 4×4=16 個(gè)方程，去掉最后一列(0,0,0,1) (0,0,0,1)對(duì)應(yīng)的4 4個(gè)恒等式還有12 12個(gè)方程，超過了未知量（6 6個(gè)）的個(gè)數(shù)，這是因?yàn)?3×3 3×3 旋轉(zhuǎn)矩陣的各項(xiàng)不是獨(dú)立的，因此要舍去一部分。該保留哪三項(xiàng)呢？只要不選擇同一行或同一列的三項(xiàng)就可以了，這里我保留了(1,2),(2,3),(3,3) (1,2),(2,3),(3,3)三項(xiàng)（但不是全都對(duì)，有時(shí)你需要試試其它項(xiàng)）。
?
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ù)改變值的大小，試驗(yàn)的效果見下圖。
?
4.2 數(shù)值解法之——迭代法
解方程的方法很多，下面我們換一種思路求解逆運(yùn)動(dòng)學(xué)方程，其思想來自于[2] [2]（英文版187頁），代碼如下：
?
forwardKinematicsJacobian[argList_, gst0_] :=
Module[{g = IdentityMatrix[4], \[Xi], n = Length[argList]},
Js = {}; (*注意空間雅可比矩陣Js是全局變量，后面會(huì)用*)
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 中抄的，它使用的旋量都要求表示在全局坐標(biāo)系中，因此需要定義絕對(duì)旋量\[Xi]a*)
inverseKinematics[gt_, q0_, errorthreshold_: 0.0001] :=
Module[{gReal, q = q0, Jb, Jg, F, error, theta, axis, positionerror, angleerror, maxIter = 20},(*輸入期望的機(jī)械臂末端位姿 gt 和初始關(guān)節(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 包括位置和角度分量在全局坐標(biāo)系中表示*)
F = PseudoInverse[Jg].error;
q = q modToPiPi[F];
If[Norm[error] < errorthreshold, Break[]]
,{maxIter}];
q]
?
?
forwardKinematicsJacobian 函數(shù)用于計(jì)算（空間）雅可比矩陣和最后一個(gè)連桿的位姿，它修改自 Screws 工具包[3] [3]。逆運(yùn)動(dòng)學(xué)計(jì)算函數(shù) inverseKinematics 的輸入是期望的末端連桿位姿 gt，迭代的初始角度 q0 ，以及誤差閾值 errorthreshold （默認(rèn)值為 0.0001 0.0001）。
其中的 modToPiPi 函數(shù)（實(shí)現(xiàn)代碼如下）用于將角度值轉(zhuǎn)換到 ?π～π ?π～π 的范圍之間。這里為什么需要 modToPiPi 函數(shù)呢？因?yàn)榻嵌仁莻€(gè)小妖精，如果我們不盯緊它，它可能會(huì)時(shí)不時(shí)的搗亂。從外部看，機(jī)械臂的一個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)位于角度 π/3 π/3 和角度 π/3 2π π/3 2π 沒什么區(qū)別?？墒侨绻覀兎湃谓嵌冗@樣隨意跳變，會(huì)導(dǎo)致軌跡不連續(xù)，這樣機(jī)械臂在跟蹤軌跡時(shí)就會(huì)出現(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ù)，它表示一個(gè)齊次變換矩陣的伴隨變換（Adjoint Transformation），diagF 函數(shù)用于將多個(gè)矩陣合成為塊對(duì)角矩陣，實(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ù)分別用于從一個(gè)齊次變換矩陣中取出旋轉(zhuǎn)矩陣（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]]]
?
?
我們以后會(huì)用到很多矩陣操作（比如轉(zhuǎn)置、求逆），而 Mathematica 的函數(shù)名太長，為了寫起來方便，我定義了簡寫的轉(zhuǎn)置和求逆函數(shù)，代碼如下：
?
T[g_]:=Transpose[g]
Iv[g_]:=Inverse[g]
?
?
我們想讓機(jī)械臂（的末端）依次到達(dá)一些空間點(diǎn)（這些點(diǎn)可能是機(jī)械臂運(yùn)動(dòng)時(shí)要經(jīng)過的）。為此首先生成一些三維空間中的點(diǎn)：
?
Clear[x,y];
pts2D = Table[{Sin[i], Cos[i]}/1.4, {i, 0, 4 Pi, Pi/400}]; (*先生成二維平面上的點(diǎn)，它們均勻地分布在一個(gè)圓上*)
pts3D = pts2D /. {x_, y_} -> {1.721, x, y 1.4}; (*再將二維坐標(biāo)變換成三維坐標(biāo)*)
Graphics3D[Point[pts3D]]
?
?
然后調(diào)用逆運(yùn)動(dòng)學(xué)函數(shù) inverseKinematics 挨個(gè)計(jì)算不同點(diǎn)處的關(guān)節(jié)值，代碼如下：
?
gStars = PToH /@ pts3D; (*將三維點(diǎn)的坐標(biāo)轉(zhuǎn)換成齊次變換矩陣，轉(zhuǎn)動(dòng)部分始終不變*)
q = ConstantArray[0, n]; (*inverseKinematics函數(shù)包含一個(gè)迭代過程，因此需要提供一個(gè)初始值*)
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}]; (*依次遍歷所有點(diǎn)，我們用每次計(jì)算得到的 q 作為下一次迭代的初始值，這樣迭代速度更快*)
?
?
計(jì)算結(jié)果 qs 中存儲(chǔ)了機(jī)械臂到達(dá)不同點(diǎn)處的關(guān)節(jié)向量，如果以后我們想讓機(jī)械臂跟蹤這個(gè)向量序列，可以對(duì)其插值得到連續(xù)的關(guān)節(jié)函數(shù)，這是靠 Interpolation 函數(shù)實(shí)現(xiàn)的，代碼如下。關(guān)于 Interpolation 函數(shù)我要啰嗦幾句，因?yàn)橐院笪覀兛赡軙?huì)經(jīng)常用到它。對(duì)于機(jī)械臂的每個(gè)關(guān)節(jié)來說， Interpolation 得到的是一個(gè)插值函數(shù)（InterpolatingFunction），更確切地說是“Hermite多項(xiàng)式” 或“Spline 樣條”插值函數(shù)。插值函數(shù)與其它的純函數(shù)沒什么區(qū)別，比如說我們可以對(duì)它求導(dǎo)、求積分。對(duì)這6個(gè)關(guān)節(jié)的插值函數(shù)求導(dǎo)就能得到關(guān)節(jié)速度和加速度函數(shù)：
?
time = 10; (*time是自己設(shè)置的，表示機(jī)械臂運(yùn)動(dòng)經(jīng)過所有點(diǎn)的總時(shí)間*)
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}]
?
?
畫出插值后各關(guān)節(jié)的角度、角速度、角加速度的變化趨勢(shì)，如下圖。能看到有兩個(gè)關(guān)節(jié)角速度變化劇烈，因此，理論上這個(gè)曲線不適合讓機(jī)器人跟蹤。
?
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é)求解逆運(yùn)動(dòng)學(xué)問題時(shí)我們使用了機(jī)械臂的雅克比矩陣，它能夠?qū)㈥P(guān)節(jié)速度映射到末端連桿的速度。由于末端連桿的速度有不止一種定義方式（例如有：空間速度、本體速度、全局速度，它們的定義見我的另一篇博客），所以對(duì)應(yīng)了不同的雅克比形式（也就是逆運(yùn)動(dòng)學(xué)函數(shù)中的 Js、Jb、Jg）。
雅克比矩陣是機(jī)器人學(xué)里的“紅人”，它出現(xiàn)在很多場(chǎng)合，在這個(gè)圈子混的時(shí)間長了你經(jīng)常能見到它。雅克比矩陣有一些有趣的性質(zhì)，比如它的零空間。只要機(jī)械臂的關(guān)節(jié)速度在其雅克比矩陣的零空間中，那么末端連桿的速度總是零，零空間由此得名。通俗的說就是：不管關(guān)節(jié)怎么動(dòng)，末端連桿始終不動(dòng)（就像被釘死了一樣）。這個(gè)性質(zhì)還挺有用的，因?yàn)橛行﹫?chǎng)合要求機(jī)械臂在抓取東西的時(shí)候還能躲避障礙物。在其它領(lǐng)域，例如攝影，為了保證畫面穩(wěn)定需要攝像機(jī)能防抖動(dòng)；在動(dòng)物王國中，動(dòng)物覓食時(shí)頭部要緊盯獵物（被惡搞的穩(wěn)定雞）；在軍事領(lǐng)域（例如坦克、武裝直升機(jī)），要求炮口始終瞄準(zhǔn)目標(biāo)，不管車身如何移動(dòng)和顛簸。
?
?
對(duì)于本文中的 6 自由度機(jī)械臂，由于它不是冗余的，所以大多數(shù)時(shí)候計(jì)算零空間會(huì)得到空（也就是說不存在零空間）。為了形象地展示零空間的效果，我不得已只用了平移的部分。以下代碼展示了機(jī)械臂在（平移）零空間中的一種運(yùn)動(dòng)，如下圖所示。可以看到，不管機(jī)械臂如何運(yùn)動(dòng)，末端連桿（局部坐標(biāo)系）的位置始終不動(dòng)（但是它的姿態(tài)會(huì)改變，矩陣mask 的作用就是濾掉轉(zhuǎn)動(dòng)分量，只剩下沿 x、y、z x、y、z 軸的平移運(yùn)動(dòng)）。BodyJacobian 函數(shù)用于計(jì)算本體雅可比矩陣，它也來自于 Screws 工具包[3] [3]。零空間是個(gè)線性空間，如果我們知道它的一組基向量，通過線性組合能夠得到任意的（速度）向量。NullSpace函數(shù)返回的剛好就是構(gòu)成矩陣的零空間的一組基向量。通過對(duì)這組基向量線性組合即可得到速度向量，其使機(jī)械臂末端始終不動(dòng)。下面的例子中使用的組合系數(shù)都是 1 ，也就是所有基向量相加（向量相加就是對(duì)應(yīng)元素相加，由Total函數(shù)完成）。由于雅可比矩陣是機(jī)械臂構(gòu)型q的函數(shù)，所以機(jī)械臂的構(gòu)型一旦改變了，我們就要重新計(jì)算它的雅可比矩陣。如果還不理解，可以隨時(shí)顯示出dq的值，然后計(jì)算Jgm.dq看看結(jié)果是不是零。如果是零就說明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. 碰撞檢測(cè)
?
我們生活的物質(zhì)世界有一個(gè)簡單的法則：兩個(gè)物體不能同時(shí)占據(jù)同一個(gè)空間位置，如果它們?cè)噲D那么做會(huì)有很大的力將它們分開。可是仿真是在虛擬的數(shù)字世界中進(jìn)行的，這個(gè)世界可不遵守物質(zhì)世界那套力的法則，因此仿真還不夠真實(shí)。為了讓機(jī)器人仿真更接近實(shí)際，我們需要考慮“碰撞檢測(cè)”（Collision Detection）功能。為了追求效率，工業(yè)機(jī)器人的運(yùn)動(dòng)速度通常比較快，而且抓著較重的負(fù)載，它一旦碰到障礙物或者人，結(jié)果一般是“物毀人傷”。所以在仿真時(shí)提前檢測(cè)是否有碰撞很有必要。在一些規(guī)劃算法中，碰撞檢測(cè)也是很重要的一部分。
值得一提的是，現(xiàn)在一些先進(jìn)的機(jī)器人控制器開始配備簡易的碰撞檢測(cè)功能，如果在機(jī)器人工作時(shí)有人突然擋住了它，它會(huì)自動(dòng)停止。這是通過檢測(cè)機(jī)械臂關(guān)節(jié)處電機(jī)的電流大小實(shí)現(xiàn)的。當(dāng)機(jī)械臂碰到人時(shí)，它相當(dāng)于受到了一個(gè)阻力，電機(jī)要想保持原來的速度運(yùn)行需要加大電流，靈敏的控制器會(huì)感知到電流的波動(dòng)，這樣我們就能通過監(jiān)視電流來判斷機(jī)械臂有沒有發(fā)生碰撞，如果電流超過一定范圍就認(rèn)為機(jī)械臂發(fā)生碰撞了，需要緊急剎車?？墒沁@種碰撞檢測(cè)方法只適用于小負(fù)載（<5kg）的機(jī)械臂。因?yàn)閷?duì)于重型機(jī)械臂，即便它也會(huì)停下來，可是它的慣性太大需要一段剎車距離，這足以對(duì)人造成傷害。
碰撞檢測(cè)是一個(gè)比較有難度的幾何問題，目前有很多成熟的算法（AABB、GJK）。我們的關(guān)注點(diǎn)在機(jī)器人，所以不想在碰撞檢測(cè)算法上浪費(fèi)太多時(shí)間。為此，我們使用 Mathematica 自帶的 RegionDisjoint 函數(shù)實(shí)現(xiàn)碰撞檢測(cè)。在幫助文檔中，可以了解到 RegionDisjoint 函數(shù)用于判斷多個(gè)幾何體是否相交，如果兩兩之間都不相交則返回 True ，而兩個(gè)幾何體出現(xiàn)了相交，就表示它們發(fā)生了碰撞（太好了，這簡直是為碰撞檢測(cè)量身定做的函數(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ù)并不意味著一帆風(fēng)順。“碰撞檢測(cè)”是出了名的吞噬者，它會(huì)霸占 CPU 大量的計(jì)算資源。如果不把它伺候好，你的計(jì)算機(jī)再先進(jìn)都會(huì)卡死。我們一般希望碰撞檢測(cè)越快越好，可是精度和速度是一對(duì)矛盾，追求速度只能犧牲一定的精度。機(jī)械臂的真實(shí)外形往往都是不規(guī)則的復(fù)雜幾何體，這使得精確的碰撞檢測(cè)很浪費(fèi)時(shí)間。多數(shù)情況下，我們沒有必要達(dá)到非常逼真的檢測(cè)效果。如果不追求很高的精度，碰撞檢測(cè)應(yīng)該保守一些。也就是說，在實(shí)際沒發(fā)生碰撞時(shí)允許誤報(bào)，但在發(fā)生碰撞時(shí)不能漏報(bào)——寧可錯(cuò)殺一千，不可放過一個(gè)。碰撞檢測(cè)的計(jì)算量與模型的復(fù)雜程度有關(guān)。我們導(dǎo)入的機(jī)器人模型雖然已經(jīng)經(jīng)過了“瘦身”，但對(duì)于RegionDisjoint函數(shù)來說還是有些復(fù)雜。為此，我們需要進(jìn)一步縮減簡化。為了保守一點(diǎn)，我們采用比真實(shí)機(jī)械臂連桿稍大些的模型，比如連桿的凸包（Convex Hull）。雖然 Meshlab 軟件可以制作凸包，但是我發(fā)現(xiàn)效果不太好。好在 Mathematica 自帶的 ConvexHullMesh 函數(shù)可以計(jì)算任意幾何體的凸包。我采用的方法是先用 ConvexHullMesh 分別計(jì)算各連桿的凸包，再導(dǎo)出凸包用 Meshlab 進(jìn)一步簡化，最后再導(dǎo)入回 Mathematica 中。計(jì)算連桿凸包及導(dǎo)出所需的代碼如下。（注意：由于連桿數(shù)據(jù)已經(jīng)是變換后的了，簡化后的連桿導(dǎo)入后不需要旋轉(zhuǎn)等變換）
?
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]
?
?
我們檢驗(yàn)一下機(jī)械臂和外部障礙物的碰撞檢測(cè)，至于機(jī)械臂連桿之間的碰撞我們暫時(shí)不考慮。代碼如下，效果如下圖所示。
?
Manipulate[
Robs = cuboid[{1.3, 0, 0.5}, {0.5, 0.5, 1.0}]; (*障礙物，一個(gè)長方體*)
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 對(duì)三維向量（點(diǎn)） pt3D 做變換，定義如下：
?
TransPt[g_][pt3D_]:=Module[{homogeneousPt},
homogeneousPt=Join[pt3D,{1.0}];
homogeneousPt=g.homogeneousPt;
homogeneousPt[[1;;3]] ]
?
?
6. 軌跡規(guī)劃
?
軌跡規(guī)劃的目的是得到機(jī)器人的參考運(yùn)動(dòng)軌跡，然后機(jī)器人再跟蹤這條軌跡完成最終的動(dòng)作。軌跡規(guī)劃是機(jī)器人研究領(lǐng)域非常重要的一部分。機(jī)器人要干活就離不開運(yùn)動(dòng)，可是該如何運(yùn)動(dòng)呢？像搭積木、疊衣服、擰螺釘這樣的動(dòng)作對(duì)人類來說輕而易舉，可要是讓機(jī)器人來實(shí)現(xiàn)就非常困難。工業(yè)機(jī)器人既沒有會(huì)思考的大腦，也缺少觀察世界的眼睛（又瞎又傻），要讓它們完全自主運(yùn)動(dòng)真是太難為它們了。它們所有的運(yùn)動(dòng)都是人教給它的。你可以把機(jī)器人想象成木偶，木偶看起來像是有靈魂的生物，但實(shí)際上它的運(yùn)動(dòng)都是人灌輸?shù)?，木偶只?huì)死板地按照人的控制運(yùn)動(dòng)，自己沒有任何主動(dòng)性，只是行尸走肉罷了。實(shí)際工廠中，是由工程師操作著控制面板，一點(diǎn)點(diǎn)調(diào)節(jié)機(jī)械臂的各個(gè)關(guān)節(jié)角度，讓它到達(dá)某個(gè)位置?？刂瞥绦驎?huì)記錄機(jī)械臂的角度變化，只要工程師示教一次，機(jī)械臂就能精確而忠實(shí)地重復(fù)無數(shù)次。不過這種不得已而為之的方法實(shí)在是太笨了，如果有一種方法能夠自動(dòng)根據(jù)任務(wù)生成機(jī)器人的參考軌跡多好，下面我們將介紹一種常用的軌跡規(guī)劃方法。
6.1 路徑、軌跡——傻傻分不清楚
?
“軌跡”是什么？要理解軌跡可離不開路徑。路徑（Path）和軌跡（Trajectory）是兩個(gè)相似的概念，它們的區(qū)別在于：
● 路徑只是一堆連續(xù)空間坐標(biāo)，它不隨時(shí)間變化。例如下圖左側(cè)的三維曲線就是一段路徑。
● 軌跡是運(yùn)動(dòng)的坐標(biāo)，它是時(shí)間的函數(shù)，一個(gè)時(shí)刻對(duì)應(yīng)一個(gè)空間坐標(biāo)點(diǎn)。軌跡包含的信息更多，我們可以對(duì)它微分得到速度、加速度等信息，而路徑是沒有這些的。下圖右側(cè)展示了兩條軌跡，它們雖然經(jīng)過相同的路徑，但卻具有不同的速度——黑色軌跡開始運(yùn)動(dòng)較快，隨后被紅色反超，最后二者又同時(shí)到達(dá)終點(diǎn)，所以它們是兩條軌跡（而不是一條）。
?
路徑 軌跡
如果我們畫出紅色和黑色軌跡的 x,y,z x,y,z 坐標(biāo)分量，就會(huì)看到它們從同一位置出發(fā)，又在另一個(gè)位置碰頭，卻經(jīng)歷了不同的過程，如下圖所示（注意紅黑兩組曲線的開始和結(jié)尾）。
?
制作上面的軌跡需要以下幾個(gè)步驟：
1. 首先隨機(jī)生成一些三維空間中的點(diǎn)。
?
pts = RandomReal[{-1,1},{6,3}]; (*6個(gè)三維坐標(biāo)點(diǎn)*)
?
?
2. 然后利用 BSplineFunction 函數(shù)對(duì)點(diǎn)插值。
?
bfun = BSplineFunction[pts];
?
?
所得到的 bfun 是一個(gè)（ B 樣條）插值函數(shù)，它的自變量的取值范圍是 0～1 0～1，你可以用 ParametricPlot3D[bfun[t], {t, 0, 1}] 畫出這條曲線。
?
?
3. 二次插值。我們雖然得到了插值函數(shù)，但它是一個(gè)向量值函數(shù)，難以進(jìn)一步處理（比如求積分、微分）。所以，我們需要在 bfun 函數(shù)的基礎(chǔ)上再處理。首先得到 bfun 函數(shù)圖像上若干離散點(diǎn)（按照 0.001 0.001 的間隔?。?/p>
?
bfpts = bfun /@ Range[0, 1, 0.001];
?
?
然后分別對(duì) xyz xyz 坐標(biāo)分量進(jìn)行單獨(dú)插值（這里我同樣將自變量的取值范圍設(shè)定在 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]]}]];
?
?
并定義一個(gè)新的插值函數(shù)為各分量的合成。這樣我們就人為生成了一段軌跡（或者說，是一個(gè)向量值函數(shù)）。
?
ifun[t_] := {ifunx[t], ifuny[t], ifunz[t]}
?
?
我們能對(duì)這段軌跡做什么呢？
● 可以計(jì)算它的弧長，代碼如下：
?
ArcLength[ifun[t], {t, 0, 1}]
?
?
● 既然可以計(jì)算弧長，就能用弧長對(duì)這條曲線重新參數(shù)化（我以前在學(xué)高等數(shù)學(xué)時(shí)，一直想不通怎么用弧長對(duì)一個(gè)曲線參數(shù)化，現(xiàn)在通過編程實(shí)踐就很容易理解了）：
?
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}]
?
?
我們說路徑攜帶的信息比軌跡少，那么從路徑中能提取出什么有價(jià)值的信息呢？
路徑只包含幾何信息：對(duì)于一個(gè)三維空間中的光滑路徑，我們能計(jì)算這條路徑上每一點(diǎn)處的切線和法線，它們剛好能唯一地確定一個(gè)右手直角坐標(biāo)系（這個(gè)坐標(biāo)系又被稱為 Frenet 標(biāo)架），如下圖所示。對(duì)應(yīng)的代碼如下。大家都知道，平面上的曲線可以用曲率描述它的彎曲程度，可是要描述三維空間曲線的彎曲程度還需要一個(gè)量，叫撓率，它是描述扭曲程度的。如果把Frenet 標(biāo)架想象成過山車，你坐在上面就能更直觀地感受曲率和撓率的含義。
?
?
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ù)值越小，點(diǎn)越密*)
pts = MeshCoordinates[tg];
pts = pts[[Last[FindShortestTour[pts]]]];
?
?
然后將其顯示出來。這里得到的是二維點(diǎn)的坐標(biāo)，要想讓我們的機(jī)械臂跟蹤，需要將其轉(zhuǎn)換為三維坐標(biāo)，這很簡單，你可以直接用替換命令：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，也翻譯為“計(jì)劃、打算”。你肯定知道“計(jì)劃”是什么意思。對(duì)于人來說，計(jì)劃就是在做事之前先想想應(yīng)該怎么做才好，比如沿著什么途徑、按照哪幾個(gè)步驟。而且，通常你有一個(gè)要到達(dá)的目標(biāo)，沒有目標(biāo)談不上計(jì)劃（當(dāng)然一般還得有一個(gè)出發(fā)點(diǎn)，但這不是必需的）。假如我想放假出去玩，在制定了詳細(xì)的開車路線后我連要去哪都不知道，那我是不是神經(jīng)病呢。正常人都是先決定去哪，然后才選擇交通線路。此外，計(jì)劃還有個(gè)評(píng)價(jià)的標(biāo)準(zhǔn)——怎么樣才算“好”呢？如果沒有標(biāo)準(zhǔn)，那我們還計(jì)劃個(gè)什么勁兒?。ǚ凑龥]有好壞之分）？把目標(biāo)和評(píng)價(jià)標(biāo)準(zhǔn)推廣到機(jī)器人的軌跡規(guī)劃領(lǐng)域就是：機(jī)器人怎么（運(yùn)動(dòng)）才能到達(dá)一個(gè)目標(biāo)位置（或者區(qū)域、構(gòu)型），而且不僅僅是到達(dá)目標(biāo)，有時(shí)我們還想以最好的方式（比如最快、消耗能量最少）到達(dá)，這就是軌跡規(guī)劃的任務(wù)?！败壽E規(guī)劃”的叫法挺多，有叫“軌跡生成”的，有叫“運(yùn)動(dòng)規(guī)劃”的，但不管怎么叫其實(shí)大概都是一個(gè)意思。
對(duì)于機(jī)械臂來說，軌跡規(guī)劃方法可以根據(jù)有沒有障礙物來劃分。如果沒有障礙物，那就簡單些了，我們可以直接規(guī)劃軌跡；如果有障礙物則一般先規(guī)劃路徑（因?yàn)槁窂桨畔⒏?，相?duì)更簡單），然后對(duì)路徑設(shè)置速度得到軌跡（因?yàn)橹饕墓ぷ鞫荚谝?guī)劃路徑，因此也可稱其為“路徑規(guī)劃”）。
路徑規(guī)劃都有哪些方法呢？比較流行的有：圖搜索、勢(shì)場(chǎng)法、RRT 等等。下面我們來實(shí)現(xiàn) RRT 方法。
RRT（快速探索隨機(jī)樹） 是一種通用的方法，不管什么機(jī)器人類型、不管自由度是多少、不管約束有多復(fù)雜都能用。而且它的原理很簡單，這是它在機(jī)器人領(lǐng)域流行的主要原因之一。不過它的缺點(diǎn)也很明顯，它得到的路徑一般質(zhì)量都不是很好，例如可能包含棱角，不夠光滑，通常也遠(yuǎn)離最優(yōu)路徑。
?
?
RRT 能在眾多的規(guī)劃方法中脫穎而出，它到底厲害在哪里呢？
天下武功唯快不破，“快”是 RRT 的一大優(yōu)點(diǎn)。RRT 的思想是快速擴(kuò)張一群像樹一樣的路徑以探索（填充）空間的大部分區(qū)域，伺機(jī)找到可行的路徑。之所以選擇“樹”是因?yàn)樗軌蛱剿骺臻g。我們知道，陽光幾乎是樹木唯一的能量來源。為了最大程度地利用陽光，樹木要用盡量少的樹枝占據(jù)盡量多的空間。當(dāng)然了，能探索空間的不一定非得是樹，比如Peano曲線也可以做到，而且要多密有多密，如上圖左所示的例子。雖然像Peano曲線這樣的單條連續(xù)曲線也能探索空間，但是它太“確定”了。在搜索軌跡的時(shí)候我們可不知道出路應(yīng)該在哪里，如果不在“確定”的搜索方向上，我們?cè)趺凑乙舱也坏剑ㄕ业降母怕适?）。這時(shí)“隨機(jī)”的好處就體現(xiàn)出來了，雖然不知道出路在哪里，但是通過隨機(jī)的反復(fù)試探還是能碰對(duì)的，而且碰對(duì)的概率隨著試探次數(shù)的增多越來越大，就像買彩票一樣，買的數(shù)量越多中獎(jiǎng)的概率越大（RRT名字中“隨機(jī)”的意思）?？墒请S機(jī)試探也講究策略，如果我們從樹中隨機(jī)取一個(gè)點(diǎn)，然后向著隨機(jī)的方向生長，那么結(jié)果是什么樣的呢？見上圖右?？梢钥吹?，同樣是隨機(jī)樹，但是這棵樹并沒很好地探索空間，它一直在起點(diǎn)（紅點(diǎn)）附近打轉(zhuǎn)。這可不好，我們希望樹盡量經(jīng)濟(jì)地、均勻地探索空間，不要過度探索一個(gè)地方，更不能漏掉大部分地方。這樣的一棵樹怎么構(gòu)造呢？
RRT 的基本步驟是：
1. 起點(diǎn)作為一顆種子，從它開始生長枝丫；
2. 在機(jī)器人的“構(gòu)型”空間中，生成一個(gè)隨機(jī)點(diǎn) A A；
3. 在樹上找到距離 A A 最近的那個(gè)點(diǎn)，記為 B B 吧；
4. B B 朝著 A A 的方向生長，如果沒有碰到障礙物就把生長后的樹枝和端點(diǎn)添加到樹上，返回 2；
隨機(jī)點(diǎn)一般是均勻分布的，所以沒有障礙物時(shí)樹會(huì)近似均勻地向各個(gè)方向生長，這樣可以快速探索空間（RRT名字中“快速探索”的意思）。當(dāng)然如果你事先掌握了最有可能發(fā)現(xiàn)路徑的區(qū)域信息，可以集中兵力重點(diǎn)探索這個(gè)區(qū)域，這時(shí)就不宜用均勻分布了。
RRT 的一個(gè)弱點(diǎn)是難以在有狹窄通道的環(huán)境找到路徑。因?yàn)楠M窄通道面積小，被碰到的概率低，找到路徑需要的時(shí)間要看運(yùn)氣了。下圖展示的例子是 RRT 應(yīng)對(duì)一個(gè)人為制作的很短的狹窄通道，有時(shí)RRT很快就找到了出路，有時(shí)則一直被困在障礙物里面。對(duì)應(yīng)的代碼如下（這段代碼只用于演示 RRT 的原理，不是正式代碼，但它有助于理解正式代碼的運(yùn)算過程）：
?
?
(*RRT示例：此段程序不依賴任何自定義函數(shù)，可獨(dú)立運(yùn)行。這是我能想到的最短的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探索空間的能力還是不錯(cuò)的，例如下圖左所示的例子，障礙物多而且雜亂（借助 Mathematica 本身具有的強(qiáng)大函數(shù)庫，實(shí)現(xiàn)這個(gè)例子所需的所有代碼應(yīng)該不會(huì)超過30行）。還有沒有環(huán)境能難住RRT呢？下圖右所示的迷宮對(duì)RRT就是個(gè)挑戰(zhàn)。這個(gè)時(shí)候空間被分割得非常嚴(yán)重，RRT顯得有些力不從心了，可見隨機(jī)策略不是什么時(shí)候都有效的。
“隨機(jī)”使得RRT有很強(qiáng)的探索能力。但是成也蕭何敗也蕭何，“隨機(jī)”也導(dǎo)致 RRT 很盲目，像個(gè)無頭蒼蠅一樣到處亂撞。如果機(jī)器人對(duì)環(huán)境一無所知，那么采用隨機(jī)的策略可以接受?？蓪?shí)際情況是，機(jī)器人對(duì)于它的工作環(huán)境多多少少是知道一些的（即使不是完全知道）。我的博客一直強(qiáng)調(diào)信息對(duì)于機(jī)器人的重要性。這些已知的信息就可以用來改進(jìn)算法。一個(gè)改進(jìn)的辦法就是給它一雙“慧眼”：在勢(shì)場(chǎng)法中，勢(shì)函數(shù)攜帶了障礙物和目標(biāo)的信息，如果能把這個(gè)信息告訴 RRT，讓它在探索空間時(shí)有傾向地沿著勢(shì)場(chǎng)的方向前進(jìn)會(huì)更好。這樣，RRT 出色的探索能力剛好可以彌補(bǔ)勢(shì)場(chǎng)法容易陷入局部極小值的缺點(diǎn)。
?
?
將RRT方法用在機(jī)械臂上的效果如下圖所示（綠色表示目標(biāo)狀態(tài)）。我設(shè)置了4個(gè)障礙物（其中一個(gè)是大地），這對(duì)機(jī)械臂是個(gè)小小的挑戰(zhàn)。由于我們生活在三維空間，沒辦法看到六維關(guān)節(jié)空間，所以我把六維關(guān)節(jié)空間拆成了兩個(gè)三維空間，分別對(duì)應(yīng)前三個(gè)關(guān)節(jié)和后三個(gè)關(guān)節(jié)（嚴(yán)格來說，六維轉(zhuǎn)動(dòng)關(guān)節(jié)空間不是一個(gè)歐式空間，它是個(gè)流形，不過這里我們不必糾結(jié)這個(gè)差別）：
?
?
正式 RRT 的主程序代碼如下。我將 RRT 樹定義為由節(jié)點(diǎn)列表 nodes 和樹枝列表 edges 組成，即 RRTtree = {nodes, edges}。其中節(jié)點(diǎn)列表 nodes 存儲(chǔ)樹中所有節(jié)點(diǎn)（每個(gè)節(jié)點(diǎn)都是一個(gè) 6 維向量，表示機(jī)械臂的關(guān)節(jié)值），樹枝列表 edges 中存儲(chǔ)所有樹枝，樹枝定義為兩個(gè)節(jié)點(diǎn)的代號(hào)（節(jié)點(diǎn)的代號(hào)定義為節(jié)點(diǎn)被添加到樹中的順序。例如，添加新節(jié)點(diǎn)時(shí)樹中已經(jīng)有4個(gè)節(jié)點(diǎn)了，那么新節(jié)點(diǎn)的代號(hào)就是 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個(gè)長方體障礙物*)
step = 0.2; (*樹枝每次生長的長度，這里簡單設(shè)置為固定值*)
q0 = {-1.54, 0.76, 0.66, -1.14, -1.44, 0}; (*起點(diǎn)*)
qt = {1.76, 0.76, 0.46, 0, 0.36, 0}; (*目標(biāo)點(diǎn)*)
nodes = {q0}; (*以起點(diǎn)q0作為種子*)
edges = {}; (*樹枝初始為空*)
RRTtree = {nodes, edges}; (*樹的初始化由節(jié)點(diǎn)和樹枝組成，它是全局變量*)
maxIters = 2000; (*最大迭代步數(shù)*)
jointLims = {{-Pi, Pi}, {-Pi/2, Pi/2}, {-Pi, 1.45}, {-Pi, Pi}, {-2, 2}, {-Pi, Pi}}; (*關(guān)節(jié)極限范圍，有些關(guān)節(jié)值不可取*)
qRandList = RandomPoint[Cuboid[ConstantArray[-Pi, n], ConstantArray[Pi, n]], maxIters, jointLims]; (*一次性生成所有的隨機(jī)點(diǎn)*)
Do[nodes = RRTtree[[1]];
If[Min[Norm /@ ((-qt #)&/@nodes)] < 0.01, Print["哈哈，到達(dá)目標(biāo)了!"]; Break[]];
qRand = RandomChoice[{qRandList[[i]], qt}]; (*以50%的概率貪婪地著朝目標(biāo)生長*)
grow[qRand,step], {maxIters}];
?
?
構(gòu)造 RRT 樹用到了以下自定義的函數(shù)：
1. 首先是碰撞檢測(cè)函數(shù) collisionDetection，如果機(jī)械臂沒有碰到障礙物就返回True。為了節(jié)省時(shí)間，碰撞檢測(cè)使用的是機(jī)械臂各連桿的凸包，在最后顯示的時(shí)候才使用原始連桿。
?
collisionDetection[Rparts_, Robs_]:= And @@ Flatten@Table[RegionDisjoint[i, #] & /@ Rparts, {i, Robs}]
?
?
2. 碰撞檢測(cè)函數(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é)點(diǎn)和邊的函數(shù)：
?
addNode[node_]:= Module[{}, AppendTo[RRTtree[[1]], node]; Length[RRTtree[[1]]] ]
addEdge[edge_]:= Module[{}, AppendTo[RRTtree[[2]], edge]; Length[RRTtree[[2]]] ]
?
?
4. 樹枝朝著采樣點(diǎn)生長（為了簡單，只檢測(cè)一點(diǎn)的碰撞情況）：
?
grow[qRand_,step_:0.2]:= Module[{qNearestIdx, qNearest, growAngles},
{qNearestIdx} = Nearest[nodes -> Automatic, qRand, 1];(*最近節(jié)點(diǎn)的代號(hào)*)
qNearest = nodes[[qNearestIdx]];
growDirection = Normalize[qRand - qNearest];
qExpand = modToPiPi[qNearest step * growDirection]; (*生長*)
Rrobot = toRegion[qExpand];
If[collisionDetection[Rrobot, Robs], qNewIdx = addNode[qExpand]; (*添加新節(jié)點(diǎn)，返回新節(jié)點(diǎn)的代號(hào) qNewIdx *)
addEdge[{qNearestIdx, qNewIdx}]] ]
?
?
下面的代碼可以顯示搜索到的（關(guān)節(jié)空間中的）路徑。這條路徑質(zhì)量不高，如果用于機(jī)器人的軌跡跟蹤還需要經(jīng)過后期的平滑處理。
?
edges = RRTtree[[2]];
targetIdx = edges[[-1, 2]];
qPath = backTrack[targetIdx];
Animate[Graphics3D[{Robs, robotMoveToQ[qPath[[i]]]}], {i, 1, Length[qPath], 1}]
?
?
其中，backTrack 函數(shù)用來從樹中抽取出連接起點(diǎn)和目標(biāo)點(diǎn)的路徑：
?
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. 動(dòng)力學(xué)仿真
?
如果在淘寶花2塊錢買個(gè)知網(wǎng)賬號(hào)，然后檢索以“工業(yè)機(jī)器人控制”為關(guān)鍵詞的學(xué)位論文，在粗略地瀏覽了20$\sim$30篇論文的目錄之后，你就會(huì)像我一樣總結(jié)出一個(gè)樸素的規(guī)律：
● 碩士論文一般都建立了機(jī)器人的運(yùn)動(dòng)學(xué)模型。
● 博士論文一般都建立了機(jī)器人的動(dòng)力學(xué)模型。
既然運(yùn)動(dòng)學(xué)已經(jīng)能夠幫助機(jī)器人動(dòng)起來了，為什么還需要費(fèi)那么大勁建立動(dòng)力學(xué)（以至于需要博士出馬）？
在前面的運(yùn)動(dòng)學(xué)一節(jié)中，我們能通過改變各個(gè)關(guān)節(jié)角度控制機(jī)械臂運(yùn)動(dòng)。但是在實(shí)際機(jī)械臂上，關(guān)節(jié)需要由電機(jī)驅(qū)動(dòng)才能改變角度。那么電機(jī)應(yīng)該輸出多大的力才能驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)呢，所需要的電流又是多大呢？只有知道這些我們才能真正實(shí)現(xiàn)對(duì)機(jī)械臂的控制。傳統(tǒng)的工業(yè)機(jī)器人大多采用兩層的控制方式，上層控制器直接輸出角度信號(hào)給底層驅(qū)動(dòng)器，底層驅(qū)動(dòng)器負(fù)責(zé)控制電機(jī)的電流實(shí)現(xiàn)上層給出的運(yùn)動(dòng)。上層不需要知道機(jī)器人的動(dòng)力學(xué)特性也可以，更不用管需要輸出多大電流。如果你的機(jī)器人不需要太高的運(yùn)動(dòng)速度和精度，動(dòng)力學(xué)沒什么太大用處（運(yùn)動(dòng)學(xué)是必需的，動(dòng)力學(xué)不是必需的）?？墒侨绻愕臋C(jī)器人速度很快，動(dòng)力學(xué)效應(yīng)就很明顯了，這時(shí)就要考慮動(dòng)力學(xué)，否則上層發(fā)出的指令底層驅(qū)動(dòng)器跟蹤不上，就會(huì)出現(xiàn)很大的誤差。要想把抓著很重的負(fù)載而且高速運(yùn)動(dòng)的六軸機(jī)器人控制好非常困難，中國好像還沒有人能做到。這也是為什么國產(chǎn)機(jī)器人大多應(yīng)用在精度不高的場(chǎng)合，比如搬運(yùn)、碼垛，而進(jìn)口機(jī)器人霸占高端應(yīng)用的原因。在高級(jí)的機(jī)器人控制器中，都有力矩補(bǔ)償功能（例如匯川、KEBA的控制器）。這個(gè)補(bǔ)償?shù)牧厥窃趺磥淼哪?？就是通過動(dòng)力學(xué)方程計(jì)算得到的。補(bǔ)償力矩用作前饋控制信號(hào)，將其添加到驅(qū)動(dòng)器上能使機(jī)器人更好地跟蹤一段軌跡?，F(xiàn)在機(jī)器人在中國變得火熱起來了，動(dòng)力學(xué)也成為一些人吹噓的噱頭了。
?
?
我們?nèi)绾蔚玫綑C(jī)器人的動(dòng)力學(xué)模型呢？
宅男牛頓首開先河，在同時(shí)代的人還渾渾噩噩的時(shí)候初步搞明白了力、速度、慣性都是怎么回事，并用數(shù)學(xué)對(duì)其進(jìn)行了定量描述，從而建立了物體做平移運(yùn)動(dòng)時(shí)的動(dòng)力學(xué)方程。從牛頓的身上我們知道，學(xué)好數(shù)學(xué)是有多重要。在那個(gè)遍地文盲的蠻荒年代，年輕的牛頓已經(jīng)偷偷地自學(xué)了歐幾里得、笛卡爾、帕斯卡、韋達(dá)等大師的著作，這為他日后發(fā)明微積分打下了基礎(chǔ)。牛頓謙虛地自稱站在巨人的肩上，事實(shí)的確如此。
勤奮的歐拉再接再厲，將牛頓的方程推廣到轉(zhuǎn)動(dòng)的情況。哥倆的工作結(jié)合起來剛好可以完整地描述物體的運(yùn)動(dòng)，這就是牛頓-歐拉法。
博學(xué)的拉格朗日發(fā)揚(yáng)光大，又將牛頓和歐拉的工作總結(jié)提煉，提出了拉格朗日法。拉格朗日真聰明啊，只需要計(jì)算物體的動(dòng)能，然后再一微分就得到了動(dòng)力學(xué)方程，這是多么簡潔統(tǒng)一的方法啊。可是拉格朗日法的缺點(diǎn)是它的效率太低了。對(duì)于4自由度以下的機(jī)械臂，計(jì)算符號(hào)解的時(shí)間我們還能忍受。至于6自由度以上的機(jī)械臂，大多數(shù)人都沒這個(gè)耐心了（十幾分鐘到數(shù)小時(shí)）。而且計(jì)算出來的動(dòng)力學(xué)是一大坨復(fù)雜的公式，很難分析利用。所以本文我們采用牛頓-歐拉法建立機(jī)械臂的動(dòng)力學(xué)模型（更準(zhǔn)確的說是它的升級(jí)版——迭代牛頓-歐拉法）。
?
7.1 慣性參數(shù)
?
早期工業(yè)機(jī)器人不使用動(dòng)力學(xué)模型是有原因的，一個(gè)是動(dòng)力學(xué)的計(jì)算量太大，在高效的計(jì)算方法被發(fā)現(xiàn)之前，早年的老計(jì)算機(jī)吃不消；另一個(gè)原因就是動(dòng)力學(xué)需要慣性參數(shù)。運(yùn)動(dòng)學(xué)只需要幾何參數(shù)，這些相對(duì)好測(cè)量；可是動(dòng)力學(xué)不僅需要幾何參數(shù)，還需要慣性參數(shù)。測(cè)量每個(gè)連桿的質(zhì)量、質(zhì)心的位置、轉(zhuǎn)動(dòng)慣量很麻煩，尤其是當(dāng)連桿具有不規(guī)則的形狀時(shí)，精度很難保證。如果使用動(dòng)力學(xué)帶來的性能提升并不明顯，誰也不想給自己找麻煩。
要使用動(dòng)力學(xué)模型，慣性參數(shù)必不可少。例如 Robotics Toolbox 工具箱中[2] [2]的 mdl_puma560.m 文件就存儲(chǔ)了 PUMA-560 機(jī)器人的慣性參數(shù)。不同型號(hào)的機(jī)器人具有不同的慣性參數(shù)，而且機(jī)器人抓住負(fù)載運(yùn)動(dòng)時(shí)，也要把負(fù)載的慣性考慮進(jìn)來。
有些情況下，我們不需要知道很精確的慣性參數(shù)，差不多夠用就行了；可是有些場(chǎng)合對(duì)精度有要求，比如拖動(dòng)示教就要求參數(shù)與實(shí)際值的誤差一般不能超過10%。對(duì)于精度要求不高的場(chǎng)合，可以使用一個(gè)近似值。大多數(shù)三維建模軟件（例如 SolidWorks、CATIA）以及一些仿真軟件（例如 Adams）都提供慣性計(jì)算功能，一些數(shù)學(xué)軟件（Mathematica）也有用于計(jì)算慣性的函數(shù)（我沒有對(duì)比過，所以不敢保證這些軟件的計(jì)算結(jié)果都是一樣的，我估計(jì)很有可能是不一樣的）。本文以 SolidWorks 為例介紹如何獲取慣性參數(shù)。
計(jì)算之前首先要設(shè)置連桿的材質(zhì)。在 SolidWorks 中打開一個(gè)連桿，在左側(cè)的“材質(zhì)”上單擊右鍵彈出“材料”對(duì)話框，如下圖所示。在這里可以設(shè)置機(jī)器人本體的材質(zhì)，MOTOMAN-ES165D 這款機(jī)器人的連桿是鑄鋁（鑄造鋁合金：Cast Aluminum）制造的。不過連桿不包含電機(jī)等部件，為此選擇密度大一點(diǎn)的材料，本文選擇鋼鐵。這里最重要的是材料的密度，鋼鐵的密度一般是7.8噸/立方米（在計(jì)算慣性時(shí)，軟件假設(shè)連桿的密度是均勻的，這明顯是簡化處理了）。設(shè)置好后點(diǎn)擊應(yīng)用即可。
?
然后在上方“評(píng)估”選項(xiàng)卡中單擊“質(zhì)量屬性”就會(huì)彈出如下圖所示的對(duì)話框。
?
SolidWorks 很快就計(jì)算出了這個(gè)連桿的所有慣性參數(shù)。不過這里的信息量有點(diǎn)大，我逐個(gè)說明：
■ ■ 首先是連桿的質(zhì)量：172.28 千克。如果你顯示的單位不是千克，可以在當(dāng)前對(duì)話框中的“選項(xiàng)”中修改單位。
■ ■ 然后是連桿的質(zhì)心（或重心）坐標(biāo)系，重心坐標(biāo)系的原點(diǎn)也給出了：(X,Y,Z)=(?0.73,?0.11,0) (X,Y,Z)=(?0.73,?0.11,0)，注意它是相對(duì)于繪圖坐標(biāo)系的哦。重心坐標(biāo)系的姿態(tài)下面會(huì)解釋。
■ ■ 最后是連桿的慣性張量，這個(gè)有些人可能不懂，我詳細(xì)解釋下。SolidWorks列出了3個(gè)慣性張量，它們之間的區(qū)別就在于分別相對(duì)于不同的坐標(biāo)系：
① 相對(duì)于質(zhì)心坐標(biāo)系；其中的 Ix、Iy、Iz 三個(gè)向量表示質(zhì)心坐標(biāo)系相對(duì)于繪圖坐標(biāo)系的姿態(tài)（也就是質(zhì)心坐標(biāo)系的 x、y、z 三個(gè)軸向量在繪圖坐標(biāo)系中的表示），而 Px、Py、Pz 表示慣性主力矩（你要問我是怎么知道的，點(diǎn)“幫助”按鈕）。慣性張量的形式是對(duì)角矩陣：
IA=[Px000Py000Pz] IA?=???Px00?0Py0?00Pz????(1) ?、凇∠鄬?duì)于原點(diǎn)與質(zhì)心坐標(biāo)系重合，但是各軸與繪圖坐標(biāo)系一致的坐標(biāo)系。SolidWorks只給出了慣性張量中各項(xiàng)的值。慣性張量的完整形式是對(duì)稱矩陣（注意里面的負(fù)號(hào)）：
IB=[Lxx?Lxy?Lxz?LyxLyy?Lyz?Lzx?LzyLzz] IB?=???Lxx?Lyx?Lzx??LxyLyy?Lzy??Lxz?LyzLzz????(2) ?、邸∠鄬?duì)于繪圖坐標(biāo)系（SolidWorks中稱為輸出坐標(biāo)系），慣性張量的形式也是對(duì)稱矩陣（同樣注意里面的負(fù)號(hào)）：
IC=[Ixx?Ixy?Ixz?IyxIyy?Iyz?Izx?IzyIzz] IC?=???Ixx?Iyx?Izx??IxyIyy?Izy??Ixz?IyzIzz????(3) 這三個(gè)慣性張量都反映了同一個(gè)連桿的性質(zhì)，因此應(yīng)該是等價(jià)的。那么它們之間有什么關(guān)系嗎？有的，它們之間可以轉(zhuǎn)換。如果定義旋轉(zhuǎn)矩陣 R=(Ix,Iy,Iz) R=(Ix,Iy,Iz)，質(zhì)心坐標(biāo)向量 p=(X,Y,Z) p=(X,Y,Z)，連桿質(zhì)量為 m m，那么有IB=RTIAR IB?=RTIA?RIC=IB mST(p)S(p) IC?=IB? mST(p)S(p) 其中，S(p) S(p)表示向量 p p 的斜對(duì)稱矩陣，這需要自定義函數(shù)（skew）實(shí)現(xiàn)，代碼如下：
?
skew[v_] := {{0,-v[[3]],v[[2]]},{v[[3]],0,-v[[1]]},{-v[[2]],v[[1]],0}}
?
?
這組公式來自于[6] [6]，我已經(jīng)驗(yàn)證過了，是正確的（要想結(jié)果比較接近，這些慣性參數(shù)至少要取到小數(shù)點(diǎn)后 5 位，這依然是在“選項(xiàng)”頁中設(shè)置）。
我們得到了三個(gè)慣性張量，在動(dòng)力學(xué)方程中我們應(yīng)該使用哪個(gè)呢？下面的程序使用了 IC IC?，因?yàn)樗窍鄬?duì)于繪圖坐標(biāo)系的，而我建立運(yùn)動(dòng)學(xué)時(shí)選擇的局部坐標(biāo)系就是繪圖坐標(biāo)系。（我以后在這里補(bǔ)充個(gè)單剛體動(dòng)力學(xué)的例子）
7.2 正動(dòng)力學(xué)仿真
?
如果給你一條軌跡，如何設(shè)計(jì)控制律讓機(jī)械臂（末端）跟蹤這條軌跡呢，控制律的跟蹤效果怎么樣呢？借助正動(dòng)力學(xué)，我們就可以檢驗(yàn)所設(shè)計(jì)的控制律。
由于后面的程序所依賴的動(dòng)力學(xué)推導(dǎo)過程[7] [7]采用了相對(duì)自身的表示方法（也就是說每個(gè)連桿的速度、慣性、受力這些量都是相對(duì)于這個(gè)連桿自身局部坐標(biāo)系描述的），旋量也是如此，為此需要重新定義旋量（代碼如下）。其實(shí)旋量軸的方向沒變（因?yàn)榫植孔鴺?biāo)系的姿態(tài)與全局坐標(biāo)系一樣），只是改變了軸上點(diǎn)的相對(duì)位置。
?
\[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}];
?
?
正動(dòng)力學(xué)的 迭代牛頓-歐拉算法 代碼如下，輸入是力矩，輸出是運(yùn)動(dòng)?？梢钥吹剑瑒?dòng)力學(xué)模型比運(yùn)動(dòng)學(xué)模型復(fù)雜多了（動(dòng)力學(xué)用到運(yùn)動(dòng)學(xué)，運(yùn)動(dòng)學(xué)卻不需要?jiǎng)恿W(xué)）。對(duì)于很多第一次接觸機(jī)器人的同學(xué)來說，動(dòng)力學(xué)是一只可怕的攔路虎，要搞明白十幾個(gè)變量都是什么含義可不容易（在仿真的時(shí)候可能包含幾十個(gè)變量，任何一個(gè)弄錯(cuò)了都會(huì)全盤皆輸，動(dòng)力學(xué)可比運(yùn)動(dòng)學(xué)難伺候多了）。因?yàn)閯?dòng)力學(xué)模型是一個(gè)微分方程，所以整個(gè)仿真過程就是個(gè)數(shù)值積分的過程。
?
endTime=10.0; steps=2000; dt=endTime/steps; (*參數(shù)初始化：仿真時(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個(gè)連桿的質(zhì)量，具體值自己設(shè)，重力加速度沿z軸的負(fù)方向，即{0,0,-1,0,0,0}*)
Do[\[ScriptCapitalM][i]=IdentityMatrix[6],{i,n 1}]; (*\[ScriptCapitalM][i]表示第i個(gè)連桿的廣義慣性矩陣，它包含質(zhì)量和慣性張量*)
g[L[n 1],L[n 2]]=g[I,L[1]]=IdentityMatrix[4];
q=dq=ddq=ConstantArray[0,n]; (*關(guān)節(jié)角度、速度、加速度初始時(shí)刻假設(shè)都為0*)
Table[V[i]=dV[i]=ConstantArray[0,6],{i,n 1}]; (*連桿i的廣義速度V[i]包括線速度和角速度，也假設(shè)開始時(shí)刻都為0*)
\[CapitalPi][n 2]=ConstantArray[0,{6,6}]; (*中間變量，沒啥具體物理意義，只是迭代初始值*)
\[Beta][n 2]=ConstantArray[0,6]; (*也是中間變量*) (*以下是計(jì)算過程*)
qList=Table[
dq=dq ddq*dt; q=q dq*dt;(*歐拉積分*)
For[i=2,i<=n 1,i , (*速度前向迭代，從第二個(gè)連桿開始到最后一個(gè)連桿*)
k=i-1; (*因?yàn)楸疚牡倪B桿從1開始標(biāo)記，所以第i個(gè)連桿依賴前一個(gè)（i-1）關(guān)節(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--, (*力和慣量后向迭代，從最后一個(gè)連桿開始到第二個(gè)連桿*)
k=i-1;
\[Tau][k] = 0.0; (*施加關(guān)節(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 , (*加速度前向迭代，從第二個(gè)連桿開始到最后一個(gè)連桿*)
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 (*顯示計(jì)算耗時(shí)*)
?
?
其中， ad 函數(shù)用于構(gòu)造一個(gè)李代數(shù)的伴隨表達(dá)形式，代碼如下。（開始我們定義的關(guān)節(jié)旋量是李代數(shù)，這里連桿的本體速度也是一個(gè)李代數(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]] ]
?
?
正動(dòng)力學(xué)的輸入是關(guān)節(jié)力矩，下面我們?yōu)殛P(guān)節(jié)力矩設(shè)置不同的值，看看機(jī)械臂的表現(xiàn)：
● 如果令關(guān)節(jié)力矩等于零（即 \[Tau][k] = 0.0），機(jī)械臂將在唯一的外力——重力作用下運(yùn)動(dòng)，如下圖所示。
?
?
只受重力的情況下，機(jī)械臂的總能量應(yīng)該守恒。我們可以動(dòng)手計(jì)算一下機(jī)械臂的能量（由動(dòng)能和重力勢(shì)能組成，代碼如下）。將仿真過程中每一時(shí)刻的能量計(jì)算出來并保存在一個(gè)列表中，再畫出圖像，如下圖所示?？梢娔芰繋缀醪蛔儯ㄝp微的變化是由積分誤差導(dǎo)致的，如果步長取的再小一些，就更接近一條直線），這說明機(jī)械臂的總能量確實(shí)保持恒定，這也間接證實(shí)了正動(dòng)力學(xué)代碼的正確性。這個(gè)簡單的事實(shí)讓人很吃驚——雖然機(jī)械臂的運(yùn)動(dòng)看起來那么復(fù)雜，但是它的能量一直是不變的。從力和運(yùn)動(dòng)的角度看，機(jī)械臂的行為變化莫測(cè)，可是一旦切換到能量的角度，它居然那么簡潔。機(jī)械臂的運(yùn)動(dòng)方程和能量有什么關(guān)系呢？聰明的拉格朗日就是從能量的角度去推導(dǎo)動(dòng)力學(xué)方程的。
?
totalEnergy = Total@Table[1/2*V[i].\[ScriptCapitalM][i].V[i] mass[i]*gravityAcc*g[I, L[i]][[3, 4]], {i, n 1}];
?
?
● 我們也可以讓機(jī)械臂跟蹤一條給定的軌跡，此時(shí)給定力矩為 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ù)，這里用作機(jī)械臂要跟蹤的（關(guān)節(jié)空間中的）參考軌跡。跟蹤一個(gè)圓的效果如下圖所示。
● 機(jī)械臂在實(shí)際工作時(shí)可能會(huì)受到干擾。PD控制律對(duì)于擾動(dòng)的效果怎么樣？我們施加一個(gè)擾動(dòng)信號(hào)試試。這里我選擇一個(gè)尖峰擾動(dòng)，模擬的實(shí)際情況是機(jī)械臂突然遭受了一個(gè)撞擊。擾動(dòng)函數(shù)的定義代碼如下，你可以自己修改擾動(dòng)的大小和尖峰出現(xiàn)的時(shí)間。
?
?
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]
?
?
?
把擾動(dòng)加到第二個(gè)關(guān)節(jié)的力矩上
?
\[Tau][k] = Kp (0 - q[[k]]) Kd (0 - dq[[k]]) If[k == 2, disturb[t], 0];
?
?
機(jī)械臂的響應(yīng)如上右圖所示，可見機(jī)械臂還是能回復(fù)零點(diǎn)姿勢(shì)的。一開始機(jī)械臂有一個(gè)輕微的顫動(dòng)，俗稱“點(diǎn)頭”。這是由于剛一開始機(jī)械臂的角度和角速度都為零，所以關(guān)節(jié)力矩也為零，導(dǎo)致機(jī)械臂缺少能夠平衡重力的驅(qū)動(dòng)力。在第5秒左右擾動(dòng)出現(xiàn)，導(dǎo)致機(jī)械臂偏離了零點(diǎn)姿勢(shì)，但在反饋控制作用下很快又回到了零點(diǎn)姿勢(shì)。
?
7.3 逆動(dòng)力學(xué)仿真
?
輸入力矩后，借助正動(dòng)力學(xué)能得到關(guān)節(jié)角加速度，積分后可以得到角速度和角度。就像運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)的關(guān)系一樣，逆動(dòng)力學(xué)與正動(dòng)力學(xué)剛好相反，它的用處是：如果告訴你機(jī)械臂的運(yùn)動(dòng)（也就是關(guān)節(jié)角度、角速度、角加速度），計(jì)算所需的關(guān)節(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è)連桿受到的內(nèi)力，為了迭代過程寫起來方便定義的*)
g[L[n 1],L[n 2]]=g[I,L[1]]=IdentityMatrix[4];
q=dq=ddq=ConstantArray[0,n]; (*初始狀態(tài)的關(guān)節(jié)角度、速度、加速度*)
\[Tau]List=Table[
For[i=2,i<=n 1,i , (*速度前向迭代：從第二個(gè)連桿開始到最后一個(gè)連桿*)
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--, (*力后向迭代：從最后一個(gè)連桿開始到第二個(gè)連桿*)
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
?
?
在重力作用下，機(jī)械臂保持“立正姿勢(shì)”需要多大力矩呢？將初始狀態(tài)設(shè)為 0，經(jīng)過逆動(dòng)力學(xué)計(jì)算得到的答案是 {0,?38.1,?38.1,0,?2.06,0} {0,?38.1,?38.1,0,?2.06,0}。如果把這個(gè)力矩再帶入正動(dòng)力學(xué)仿真就能看到機(jī)械臂保持靜止不動(dòng)，這證明我們的逆動(dòng)力學(xué)模型也是正確的。
8. 結(jié)尾
?
本文以 Mathematica 通用數(shù)學(xué)計(jì)算軟件為平臺(tái)，針對(duì)串聯(lián)機(jī)械臂的建模、規(guī)劃和控制中的基本問題進(jìn)行了仿真，差不多該說再見了。不過新的篇章即將展開 —— 移動(dòng)機(jī)器人是另一個(gè)有趣的領(lǐng)域。與固定的機(jī)器人相比，移動(dòng)機(jī)器人更有挑戰(zhàn)性，因此也會(huì)出現(xiàn)新的問題，比如信息的獲取和利用。未來將加入移動(dòng)機(jī)器人仿真的功能，支持地面移動(dòng)機(jī)器人的運(yùn)動(dòng)控制、環(huán)境約束下的運(yùn)動(dòng)規(guī)劃、移動(dòng)機(jī)械臂、多機(jī)器人避障、多機(jī)器人編隊(duì)運(yùn)動(dòng)等，并討論環(huán)境建模、導(dǎo)航與定位、SLAM、非完整約束、最優(yōu)控制、輪式車輛和履帶車輛的動(dòng)力學(xué)模型以及地面力學(xué)模型、群集協(xié)同運(yùn)動(dòng)等問題，敬請(qǐng)期待喲！
?
?
9. 補(bǔ)充： Mathematica 的缺點(diǎn)
?
在筆者就讀研究生期間，Matlab 的使用率頗高。每次參加答辯、聽報(bào)告，看著同學(xué)或老師用 Matlab 制作的丑陋不堪的圖表和動(dòng)畫，心中就想把 Matlab 的界面設(shè)計(jì)師槍斃十分鐘。再加上呆板的函數(shù)定義和使用方式、缺乏對(duì)部分機(jī)器人仿真功能的支持，讓我不得不尋找其它的替代軟件?？墒窃诰W(wǎng)絡(luò)發(fā)達(dá)的今天，我居然找不到稍微像點(diǎn)樣的介紹機(jī)器人仿真的文章以及原理性代碼，要么過于低端，要么是東拼西湊，于是想把自己的經(jīng)驗(yàn)寫出來，并公開代碼。
就像 Matlab 有很多讓人不爽的地方一樣，Mathematica 用于機(jī)器人仿真同樣存在一些缺陷。我們之前在碰撞檢測(cè)部分已經(jīng)提過，要想達(dá)到很快的檢測(cè)速度就不得不使用簡單的幾何模型。雖然 Mathematica 的函數(shù)也經(jīng)過了優(yōu)化，但是只適用于需要較少計(jì)算次數(shù)的場(chǎng)合，在多次處理大量數(shù)據(jù)時(shí)還是比較慢。Mathematica 本身是用 C 語言寫成的，如果某個(gè)函數(shù)被大量調(diào)用可以考慮用 C 語言寫成動(dòng)態(tài)鏈接庫（dll），然后在 Mathematica 中調(diào)用，這就像 Matlab 中的 MEX 文件。
調(diào)試找錯(cuò)是個(gè)痛苦的過程，在 Mathematica 中更是這樣。Mathematica 支持調(diào)試時(shí)設(shè)置中斷，可惜使用起來不太方便。Mathematica 提供了一個(gè)專門用來開發(fā)調(diào)試的軟件——Workbench，操作同樣繁瑣。 在調(diào)試時(shí)，我只好使用 Print 函數(shù)打出中間計(jì)算結(jié)果來檢查程序是否正確。Mathematica 缺少像 Matlab 一樣的變量監(jiān)控窗口可以實(shí)時(shí)看到變量的值，這在調(diào)試時(shí)顯得很不方便（雖然在堆棧中可以監(jiān)視局部變量）。所以，盡量不要在 Mathematica 中使用中斷[9] [9]。Matlab 中如果出現(xiàn)語法錯(cuò)誤，編譯會(huì)中止并顯示出錯(cuò)位置，但在 Mathematica 中卻不會(huì)自動(dòng)停止，它仿佛像推土機(jī)一樣停不下來。一旦出錯(cuò)你通常只會(huì)在計(jì)算結(jié)果中看到一堆冗長的代碼，卻很難發(fā)現(xiàn)錯(cuò)在哪了。對(duì)于包含許多步的計(jì)算過程，調(diào)試起來可能很浪費(fèi)時(shí)間，最好的方法是把代碼切割成功能簡單清晰的模塊，挨個(gè)檢驗(yàn)每個(gè)模塊要比檢驗(yàn)一堆糾纏不清的代碼更容易。
參考文獻(xiàn)
[1] 一種高效的開放式關(guān)節(jié)型機(jī)器人3D仿真環(huán)境構(gòu)建方法，甘亞輝，機(jī)器人，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] 機(jī)器人學(xué)導(dǎo)論，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
版權(quán)聲明：本文為作者原創(chuàng)文章，轉(zhuǎn)載請(qǐng)附上博文鏈接！
聯(lián)系客服
