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

時間  2019-11-05
標籤 基於 mathematica 機器人 仿真 環境 機械 简体版
原文   原文鏈接

完美的教程，沒有之一，收藏學習。javascript

目的 php

　　本文手把手教你在 Mathematica 軟件中搭建機器人的仿真環境，具體包括如下內容（所使用的版本是 Mathematica 11.1，更早的版本可能缺乏某些函數，因此請使用最新版。robinvista2@gmail.com）。 
　　1　導入機械臂的三維模型 
　　2　（正/逆）運動學仿真 
　　3　碰撞檢測 
　　4　軌跡規劃 
　　5　（正/逆）動力學仿真 
　　6　控制方法的驗證 
　　不妨先看幾個例子： css

　　　　　　　逆運動學　　　　　　　　　　　　　　　　雙臂協做搬運 html

　　　　　　　顯示運動痕跡　　　　　　　　　　　（平移）零空間運動 
　　不管你是從事機器人研發仍是教學科研，一款好用的仿真軟件能對你的工做起到很大的幫助。那麼應該選擇哪一種仿真軟件呢？最方便的選擇就是現成的商業軟件（例如 Webots、Adams）。你的錢不是白花的，商業軟件功能完善又穩定，並且性能通常都通過了優化。但是再強大的商業軟件也沒法面面俱到。從學習和研究的角度出發，最好選擇代碼可修改的開源或半開源軟件。 
　　在論文數據庫中簡單檢索一下就會發現，不少人都選擇在 Matlab 的基礎上搭建仿真環境。這並不奇怪，由於 Matlab 具備優秀的數值計算和仿真能力，使用它開發會很便利。若是你非要捨近求遠，用 C++ 編寫一套仿真軟件，先不要說仿真結果如何顯示，光是矩陣計算的實現細節就能讓你焦頭爛額（原本咱們要造一輛汽車，但是製做車輪就耗費了大量的精力，而實際上車輪直接買現成的就好了）。 前端

　　與大名鼎鼎的 Matlab 相比，Mathematica 是一款知名度不高的軟件。可是不要小看它哦，我簡單對比了一下兩者在機器人仿真方面的特色，見下表。因爲 Mathematica 不俗的表現，我選擇在它的基礎上搭建仿真環境。若是你對 Mathematica 不熟悉，能夠看網絡教程，也能夠參考個人學習筆記入門（點擊這裏查看）。本文面向 Mathematica 的初學者，因此避免使用過於高超的編程技巧。java

 

     
  Matlab Mathematica
可視化效果 通常 優秀
導入機器人模型 須要本身寫函數 自帶函數
機器人工具箱（包） Robotic Toolbox、SpaceLib等 Screws、Robotica等
調試功能 方便易用 目前還不太方便，有點繁瑣
代碼長度（以Matlab爲標準）   左右

　　 
1. 準備工做：獲取機器人的真實外觀模型node

　　製做逼真的仿真須要機器人的外觀模型。若是你有機器人的三維模型最好，沒有的話也沒關係。著名的機器人制造商都在其官網提供其各類型號機器人的逼真或者真實模型，例如 ABB、安川，供你們免費下載和使用。 
說明：爲了防止抄襲，這些模型都是不可經過建模軟件修改的格式，例如 IGES 和 STEP 格式，但咱們只用來顯示和碰撞檢測，因此並不影響仿真。 python

 

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

　　得到模型後要導入 Mathematica 中進行處理並顯示，下面咱們藉助一個例子說明具體的步驟。Motoman ES165D 是安川公司生產的一款6自由度點焊機器人，其最後三個關節軸線相交於1點，這是一種很是經典並且有表明性的設計，所以咱們選擇以這款機器人爲例進行講解（這個機器人的模型點擊此處下載）。sql


　　用 SolidWorks 2014 軟件打開機器人的裝配體文件，並選擇「另存爲」STL 格式。而後打開當前頁面下方的「選項」對話框，以下圖。這裏咱們要設置三個地方： 
　　1. 「品質」表示對模型的簡化程度，咱們若是想很是逼真的效果，能夠選擇「精細」，但缺點是數據點不少致使文件很大、處理起來比較慢。通常選擇「粗糙」就夠了； 
　　2. 勾選「不要轉換 STL 輸出數據到正的座標空間」； 
　　3. 不要勾選「在單一文件中保存裝配體的全部零件」。 

 

　　小技巧：STL格式是一種三維圖形格式，被不少三維建模軟件支持（Mathematica也支持，因此咱們要保存爲這個格式）。STL格式只存儲三維圖形的表面信息，並且是用不少的三角形對圖形進行近似的表示。若是你的機器人外形比較簡單（規則的幾何體），那麼獲得的STL文件大小通常只有幾十KB ；但是若是外形很複雜（好比包含不少的孔洞、曲面），生成的STL文件會很大（幾MB∼幾十MB）。對於通常配置的計算機，模型文件超過100KB用Mathematica處理起來就比較慢了。這時能夠利用免費軟件MeshLab對其進行化簡，MeshLab一般可以在基本不改變外形的前提下將尺寸縮減爲原來的十分之一甚至更多。 
　　MeshLab的安裝和操做都是傻瓜式的，打開後進入以下圖左所示的菜單中，出現右圖的頁面，這裏的「Percentage reduction」表示縮減的百分比（1 表示不縮減，0.1 則表示縮減爲原來的10%），設置好後點擊Apply並保存便可。

 

　　而後在 Mathematica中導入生成的STL 文件，使用的代碼以下（假設 STL 文件保存在 D:\MOTOMAN-ES165D 文件夾下）：

  1.  
    SetDirectory[ "D:\\MOTOMAN-ES165D"]; (*設置文件的存儲位置，注意雙斜槓*)
  2.  
    n = 6; (* n 是機械臂的自由度，後面還會用到*)
  3.  
    partsName = { "1.stl", "2.stl", "3.stl", "4.stl", "5.stl", "6.stl", "7.stl", "8.stl", "9.stl"}; (*組成機械臂的9個零件*)
  4.  
    robotPartsGraphics = Import[ #, "Graphics3D"] & /@ partsName; (*一次性導入全部零件，而且導入爲直接能夠顯示的圖形格式*)
  5.  
    robotParts = robotPartsGraphics [[;; , 1]]; (*提取出三維圖形的幾何數據：頂點的三維座標和邊*)
  • 1
  • 2
  • 3
  • 4
  • 5
  • 1
  • 2
  • 3
  • 4
  • 5

　　這裏我偷了個懶，爲了少打些字，我把導出零件的文件名改爲了從1到9的數字（這個機械臂的裝配體一共包含9個零件）。想要顯示導入的機器人模型能夠使用如下代碼，顯示效果以下圖：

Graphics3D[{frame3D, robotParts}]
  • 1
  • 1

說明：frame3D是三維（右手）座標系圖形，由於咱們會用到不少座標系及其變換，將座標系顯示出來更直觀。定義 frame3D 的代碼以下。這個座標系默認的位置在原點，咱們稱這個座標系爲全局座標系。

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

 


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

 

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

　　運行後的輸出結果是一堆由 GraphicsComplex 函數包裹着的數字，稍加分辨會發現這些數字又包含兩部分：第一部分是零件全部頂點的三維座標；第二部分是組成零件外形的三角形（構成每一個三角形的三個頂點是第一部分點的序號，而不是座標）。咱們能夠用如下代碼將其分別顯示出來：

  1.  
    pts = robotParts[[ 1, 1]]; (*第一部分：頂點的三維座標數據*)
  2.  
    triangles = robotParts[[ 1, 2]]; (*第二部分：三角形面片*)
  3.  
    trianglesB = triangles /. {EdgeForm[] -> EdgeForm[Blue]}; (*三角形的邊顯示爲藍色 Blue*)
  4.  
    Graphics3D[ {Red, Point[pts], White, GraphicsComplex[pts, trianglesB]}]
  • 1
  • 2
  • 3
  • 4
  • 1
  • 2
  • 3
  • 4

 

　　機器人的全部零件都成功地導入了，並且它們的相對位置也是正確的。你可能會問：機械臂爲何是處於「躺着」的姿式呢？這是因爲零件是按照 SolidWorks 默認的座標系（ 軸向上）繪製和裝配的。而在 Mathematica 中默認的座標系是  軸向上。那麼咱們應該採用哪一個座標系呢？ 
　　固然你能夠任性而爲，用哪一個均可以。不過根據國家標準《GBT 16977-2005 工業機器人 座標系和運動命名原則》，基座座標系的  軸應該垂直於機器人基座安裝面（也就是地面）、朝向爲重力加速度的反方向，  軸指向機器人工做空間中心點。制定國標的都是些經驗豐富的專家老手，咱們最好跟國標保持一致（國標的做圖水平就不能提升點嗎？這圖怎麼感受像小學生畫的）。 

　

　　爲了讓機器人變成國標規定的姿式，須要旋轉各個零件。咱們先想一想應該怎麼轉：結合咱們以前導入的圖形，能夠先繞全局座標系的  軸轉 ，再繞全局座標系的  軸轉 。另外一種方法是：先繞全局座標系的  軸轉 （記這個旋轉後的座標系爲 ），再繞  的  軸轉 。兩種方法的效果是同樣的，可是注意合成矩陣時乘法的順序（見如下代碼），不懂的同窗能夠看看文獻中的3133頁。固然，轉動是有正負之分的：將你的右手握住某個座標軸，豎起大拇哥，讓大拇指和軸的正方向一致，這時四指所示的方向就是繞該軸轉動的正方向。 
　　爲此，定義旋轉矩陣：

 

  1.  
    Xaxis = {1, 0, 0}; Yaxis = {0, 1, 0}; Zaxis = {0, 0, 1}; (*定義旋轉軸，更簡潔的寫法是： {Xaxis,Yaxis,Zaxis}=IdentityMatrix[3];*)
  2.  
    rot = RotationMatrix[ 90 Degree, Zaxis].RotationMatrix[90 Degree, Xaxis]; (*注意第二次變換是在左邊乘*)
  3.  
    rot = RotationMatrix[ 90 Degree, Xaxis].RotationMatrix[90 Degree, Yaxis]; (*注意第二次變換是在右邊乘*)
  • 1
  • 2
  • 3
  • 1
  • 2
  • 3

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

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

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

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

 


說明：如今的機器人姿式（大臂豎直、小臂前伸）是6自由度機械臂的「零位」狀態，咱們將此時機械臂各關節的角度認爲是0。通常機械臂上都有各關節的零點位置標記，用於指示各關節的零點。咱們用控制器控制機械臂時，發出的角度指令都是相對於這個零點位置的。零點位置不是必須遵照的，你能夠將任意的角度設置爲零位，不過爲了統一，最好用機械臂固有的零位——也就是當前的姿式。

 

3. 運動學仿真

　　前面的工做只是讓機械臂的模型顯示出來，若是你想讓機器人動起來，那就要考慮運動學了。機器人聽起來高大上，可實際上如今大多數工業機器人的控制方式仍是比較低級的，它們只用到了運動學，高級一點的動力學不多用，更不要提智能了（它們要說本身有智能，咱們家的洗衣機和電視機都不服）。有的公司（例如倍福），更是將支持不一樣類型的機械臂的運動學做爲宣傳的噱頭。看來要使用機器人，運動學是必不可少的，因此咱們先來實現運動學。

　　在創建運動學模型以前咱們須要瞭解機器人的機械結構。前面提到，MOTOMAN-ES165D 是一個6自由度的串聯機械臂。而6個自由度的機器人至少由7個連桿組成（其中要有一個連桿與大地固定，也就是基座）。但是咱們導入的零件有9個，多出來的2個零件是彈簧缸（基座上黃色的圓筒）的組成部分。MOTOMAN-ES165D 機器人可以抓持的最大負載是165公斤，彈簧缸的做用就是平衡掉一部分負載的重量，要否則前端的關節電機會有很大的負擔。但是彈簧缸給咱們的建模形成了麻煩，由於它致使存在「閉鏈」，這不太好處理。爲此，咱們先忽略掉彈簧缸。 
　　 
3.1 零件的局部座標系

 

　　機器人的運動也就是其構成連桿（零件）的運動。而爲了描述連桿的運動，咱們要描述每一個連桿的位置和姿態（合稱爲「位姿」）。一般的作法是在每一個連桿上固定一個座標系（它跟隨連桿一塊兒運動），這個座標系稱爲「局部座標系」。經過描述局部座標系的位姿咱們就能夠描述每一個連桿的位姿。如何選擇局部座標系呢？理論上你能夠任意選擇，不過局部座標系影響後續編程和計算的難易程度，因此咱們在選擇時最好慎重。在運動學建模和動力學建模中，座標系的選擇一般是不一樣的。 
　　●　運動學建模時，連桿的局部座標系通常放置在關節處，這是由於經常使用的 D-H 參數是根據相鄰關節軸定義的。 
　　●　動力學建模時，連桿的局部座標系通常放置在質心處，這是由於牛頓方程是關於質心創建的，並且關於質心的轉動慣量是常數，這方便了計算。 
　　咱們先考慮運動學，所以將局部座標系設置在關節處。在 SolidWorks 中打開任何一個零件，都能看到它本身有一個座標系。構成一個零件的每一條邊、每個孔的數據都以這個座標系爲參考，咱們稱它爲「繪圖座標系」。繪圖座標系一般不在質心處，由於在你還沒畫以前你根本不知道質心在哪裏。繪圖座標系一般在零件的對稱中心或者關節處，咱們不妨將每一個零件的繪圖座標系當作它的局部座標系。 
　　那麼下一個問題是每一個零件的繪圖座標系在哪兒呢？咱們以第三個零件爲例，以下圖左所示。咱們點擊左側的「原點」標籤，圖中就會顯示繪圖座標系的原點。（若是你想將繪圖座標系顯示出來，能夠先選中「原點」標籤，而後點擊上方菜單欄中的「參考幾何體」，再選擇「座標系」，而後直接回車便可看到新建的繪圖座標系，如右圖，可見它位於一個關節軸）

　　而後回到機器人的裝配體中，在左側的零件樹中展開每一個零件找到並選中其繪圖座標系的原點，而後點擊上方菜單欄「評估」中的「測量」便可看到圖中出現了一組座標值（以下圖所示），這就是零件繪圖座標系的原點在全局座標系（本文將全局座標系定義爲裝配體的座標系）中的位置。 

　　咱們記錄下全部零件的繪圖座標系的原點位置（除去彈簧缸的2個，注意 SolidWorks 中默認的單位是毫米，這裏除以 1000 是爲了變換到 Mathematica 中使用的標準單位——米）：

 

drawInGlobalSW = {{0, 0, 0}, {0, 650, 0}, {-315, 1800, 285}, {-53.7, 1800, 285}, {0, 2050, 1510}, {0, 2050, 1510}, {0, 2050, 1720.5}}/1000;
  • 1
  • 1

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

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

　　之後會常常用到對座標的旋轉變換，並且多數時候是用一個旋轉矩陣同時對不少座標進行變換（例如上面的這個例子），咱們不如定義一個算子以簡化繁瑣的代碼。咱們定義算子（實際上是一個函數）：

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

　　因此前面的變換用咱們自定義的算子表示就是（複製到 Mathematica中後 \[CircleDot] 會變成一個Mathematica內部預留的圖形符號，這個符號沒有被佔用，因此這裏我徵用了）：

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

　　Mathematica 自帶的函數首字母都是大寫。爲了與官方函數區分，我自定義的函數通常採用小寫字母開頭。本文使用的自定義的函數都會給出實現代碼，並且爲了方便，我將經常使用的自定義函數打包成一個函數包，每次運行程序時導入此函數包便可使用裏面的函數。該函數包依賴另外一個函數包 Screws.m （我修改了部分函數的名字，爲此從新定義了 myScrews.m）。兩個函數包點擊此處下載。在程序中導入函數包的代碼以下（假設函數包位於你的程序筆記本文件的同一目錄下）： 
SetDirectory[NotebookDirectory[]] 
<< myFunction.m

　　還有印象嗎？最開始導出和導入零件模型時，各零件的位置都已經按照裝配關係肯定好了，因此它們的數據也是相對於全局座標系描述的。但是如今咱們要讓機械臂動起來（並且還要顯示出來），這就要移動這些數據。爲了方便起見，最好能將每一個零件的模型數據表示在本身的繪圖座標系中，由於這樣咱們只須要移動繪圖座標系就好了，而各點的數據相對它們所屬的繪圖座標系是不動的。應該怎麼作呢？很簡單，將零件模型的數據減去繪圖座標系的原點在全局座標系中的座標便可：

  1.  
    partsName = { "1.stl", "2.stl", "3.stl", "6.stl", "7.stl", "8.stl", "9.stl"};　(*已經去除了彈簧缸的2個零件：4號和5號*)
  2.  
    robotPartsGraphics = Import[ #, "Graphics3D"] & /@ partsName;
  3.  
    robotParts = robotPartsGraphics [[;; , 1]];
  4.  
    robotParts = Table[GraphicsComplex[rot\[CircleDot]robotParts [[i, 1]], robotParts[[i, 2]]], {i, 7}];
  5.  
    robotParts = Table[GraphicsComplex[( # - drawInGlobal[[i]]) & /@ robotParts[[i, 1]], robotParts[[i, 2]]], {i, 7}];
  6.  
    colors = {Gray, Cyan, Orange, Green, Magenta, Yellow, Pink}; (*從新定義零件的顏色*)
  7.  
    robotPartsColored = Transpose@{colors, robotParts};
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

　　移動後的零件模型以下圖所示（圖中的座標系是各個零件本身的繪圖座標系，我沒有對數據轉動，因此繪圖座標系和全局座標系的姿態相同）。咱們一開始從 SolidWorks 導出文件時是一次性地導出整個裝配體的。其實，若是咱們挨個打開各個零件而且一個一個的導出這些零件，那麼獲得數據就是相對於各自的繪圖座標系的，只不過這樣稍微麻煩一點。 


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

 

　　下面咱們討論如何創建運動學模型。描述機器人連桿之間幾何關係的經典方法是採用 D-H 參數（Denavit - Hartenberg parameters）。能留下本身名字的人都不是通常人，那麼 D-H 參數巧妙在什麼地方呢？咱們知道，徹底肯定兩個座標系（或者剛體）的位姿關係須要6個參數，由於有6個自由度。若是不考慮關節轉動（平移）仍須要5個參數。然而 D-H 參數竟然只用了4個參數就可以肯定相鄰連桿的位姿關係，可見 Denavit 和 Hartenberg 這哥倆確實動了番腦筋。不過爲了不 D-H 參數的一些缺點，咱們棄之不用而採用旋量的表示方法。旋量有什麼性質、它和剛體運動的關係是什麼、這些問題數學家用了很長時間才搞清楚。在本文中你能夠把旋量簡單想象成一個表示關節轉動的量。表示一個關節旋量須要肯定一個關節軸線的方向向量（3個參數）和軸線上任意一點的座標（又要3個參數）。 
　　旋量和向量類似的地方是，它也要相對於一個座標系來描述。咱們選擇哪一個座標系呢？這裏咱們要參考 D-H 參數，每個連桿座標系在定義時都相對於前一個連桿的座標系。因此咱們將每一個關節軸的旋量表示在前一個連桿中。此次咱們以2號零件爲例說明如何肯定關節軸的旋量： 
　　1.　首先來看關節軸線的方向，這個要相對於2號零件的繪圖座標系。（咱們要肯定關節2的旋量，至於關節1的旋量最好在零件1中肯定）。從下圖中看關節2的軸線方向彷佛是  軸，但是咱們前面將繪圖座標系的姿態和全局座標系的姿態設定爲同樣的，因此應該在全局座標系（基座座標系）中肯定，也就是  軸。 
　　２.　關節軸線上任意一點的座標，這個一樣要相對於2號零件的繪圖座標系。咱們在軸線上任選一點便可。步驟是：點擊 SolidWorks 上方菜單欄的「參考幾何體」，選擇「點」，而後在左側面板選擇「圓弧中心」，而後選擇圖中的關節軸周圍的任意同心圓弧便可建立一個參考點，這個點就是咱們想要的。咱們能夠在零件視圖中測量這個點的座標，也能夠在機器人完整裝配體中測量，這裏我選擇後者。（測量步驟參照前面測量「零件繪圖座標系的原點」）

　　定義關節旋量的代碼以下。其中相對旋量  用於遞歸運動學計算，它的含義是當前連桿的轉軸表示在前一個連桿座標系中。

 

  1.  
    axesPtInGlobal = rot\[CircleDot]{{ 0, 257, 0}, {-88, 650, 285}, {-280.86, 1800, 285}, {0, 2050, 1318}, {134, 2050, 1510}, {0, 2050, 1720.5}}/1000;
  2.  
    axesPtInDraw = axesPtInGlobal - drawInGlobal [[1 ;; -2]];
  3.  
    axesDirInDraw = axesDirInGlobal = {Zaxis, Yaxis, Yaxis, Xaxis, Yaxis, Xaxis};
  4.  
    \[Xi]r = Table[\[Omega]r[i] = axesDirInDraw [[i]]; lr[i] = axesPtInDraw[[i]]; Join[Cross[-\[Omega]r[i], lr[i]], \[Omega]r[i]], {i, n}];
  • 1
  • 2
  • 3
  • 4
  • 1
  • 2
  • 3
  • 4

　　咱們對關節的運動進行了建模，要創建運動學還缺乏最後同樣東西：零件間的初始相對位姿（初始的意思是機械臂處於「零位」的狀態下）。零位下，咱們將全部零件的姿態都認爲和全局座標系同樣，因此不用計算相對姿態了。至於它們的相對位置嘛，咱們已經知道了繪圖座標系原點在全局座標系中的座標，兩兩相減就能夠獲得它們的相對位置了，很簡單吧！（見下面的代碼）　　

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

　　其中，PToH 函數能將位置向量轉換爲一個齊次變換矩陣，這是藉助 RPToH 函數實現的（RPToH 函數就是 Screws 工具包中的RPToHomogeneous 函數），它能夠將一個旋轉矩陣和位移向量組合成一個齊次變換矩陣。將旋轉矩陣和位移向量合成爲齊次變換矩陣是咱們之後會常常用到的操做。相似的，也能夠定義 RToH 函數將旋轉矩陣生成對應的齊次變換矩陣，代碼以下：

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

說明：本文中，用符號 I 表示全局座標系（同時也是慣性座標系）；符號 L[i] 表示第 i 個連桿，變量 g[L[i], L[i+1]] 表示第 i+1 個連桿相對於第 i 個連桿的位姿矩陣（它是一個的齊次變換矩陣）；變量 g[I, L[i]] 表示什麼你確定猜到了，它表示第 i 個連桿相對於全局座標系的位姿矩陣。若是不特別說明，本文老是用 g （或者 g 開頭的變量）表示一個（或一組）齊次變換矩陣，這是約定俗成的。 
　　如今能夠正式推導機械臂的運動學模型了。在使用機械臂時，你們通常只關心其最末端連桿的位姿，更確切的說，是最末端連桿的位姿與關節角度的關係。不過爲了獲得最末端連桿的位姿，咱們須要計算中間全部連桿的位姿。這裏利用相鄰連桿的遞歸關係——每一個連桿的位姿依賴前一個連桿的位姿——來提高計算效率。因此，能夠定義機械臂全部連桿的運動學函數爲：

  1.  
    robotPartsKinematics[configuration _] := Module[{q, g2To7},
  2.  
    　　 {g[I, L[ 1]], q} = configuration;
  3.  
    　　 g2To7 = Table[g[L[i], L[i + 1]] = TwistExp[\[Xi]r[[i]], q[[i]]].g[L[i], L[i + 1], 0];
  4.  
    　　 g[I, L[i + 1]] = g[I, L[i]].g[L[i], L[i + 1]], {i, n}];
  5.  
    　　 Join[{g[I, L[ 1]]}, g2To7] ]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 1
  • 2
  • 3
  • 4
  • 5

　　robotPartsKinematics函數的輸入是：基座的位姿矩陣 g[I, L[1]] 和全部關節的角度向量q，這組變量完整地描述了一個串聯機械臂的位置和姿式（用機器人中的專業術語應該叫「構型」： configuration，注意不要翻譯爲「配置」），而輸出則是全部連桿相對於全局座標系的位姿（即 g[I, L[i]]，其中i = 1~7）。 
　　其中，TwistExp 函數來自於 Screws 工具包，做用是構造旋量的矩陣指數。 
說明：在大多數的機器人教科書中，連桿的記號是從0開始的，也就是說將基座記爲0號連桿，而後是1號連桿，最末端的連桿是號（假設機械臂的自由度是）；而關節的記號是從1開始，也就是說1號關節鏈接0號連桿和1號連桿。這樣標記的好處是記號一致，推導公式或編程時不容易出錯：好比說咱們計算第  個連桿的速度時要利用第  個關節的轉動速度。但是本文中連桿的記號是從1開始的（基座標記爲1號連桿），咱們保留0號標記是爲了之後將機械臂擴展到裝在移動基座的狀況，這時0號就用來表示移動基座（好比一個AGV小車）。 
　　能夠看到，只要定義好關節旋量，創建運動學模型很是簡單。但是這樣獲得的運動學模型對不對呢？咱們來檢驗一下。藉助Manipulate 函數，能夠直接改變機械臂的各關節角度，並直觀地查看機械臂姿式（應該叫構型了哦）的變化，如如下動畫所示。能夠看到，機械臂各連桿的運動符合咱們設置的關節值，這說明運動學模型是正確的。 

 

  1.  
    Manipulate[qs = {##}[[;; , 1, 1]];
  2.  
    gs = robotPartsKinematics[ {IdentityMatrix[4], qs}];
  3.  
    Graphics3D[{MapThread[move3D, {robotPartsColored, gs}]}
  4.  
    , PlotRange -> {{-2, 3}, {-2, 3}, {0, 3}}], ##, ControlPlacement -> Up] & @@ Table[{{q[i], 0}, -Pi, Pi, 0.1}, {i, n}]
  • 1
  • 2
  • 3
  • 4
  • 1
  • 2
  • 3
  • 4
move3D[shape_,g_]:=GeometricTransformation[shape,TransformationFunction[g]];
  • 1
  • 1

　　驗證使用的代碼如上。其中，move3D 函數的功能是用一個齊次變換矩陣（g）移動一個幾何圖形（shape）。這裏還值得一提的是 MapThread 函數。雖然咱們能夠用 move3D 函數去一個一個地移動連桿（寫起來就是：move3D[part1, g1], move3D[part2, g2], move3D[part3, g3]……），這樣寫比較清楚也很容易讀懂，可就是太麻煩了，想象你的機械臂有一百個連桿就不得不用循環了。可是使用 MapThread 函數寫起來就很是簡單了，並且獲得的結果與前面徹底同樣（MapThread[move3D, {{part1, part2, part3}, {g1, g2, g3}}]）。這就是爲何我一直強調最好把同類型的元素放到一塊兒，由於操做的時候能夠一塊兒批量化進行。 
　　能夠看到，Mathematica 提供的控件類函數 Manipulate 支持用戶使用鼠標交互式地改變變量的值，同時動態更新對應的輸出。若是一段代碼運行時間足夠快，就能夠放在Manipulate 內部，好比運動學函數robotPartsKinematics，它包含的計算並不複雜，但若是是後面要介紹的動力學函數就不適合放在Manipulate裏面了，由於動力學的計算比較耗時，窗口會顯得很「卡頓」。

4. 逆運動學仿真

　　藉助運動學，咱們成功地經過改變關節角度實現了對機械臂的控制。固然這沒什麼值得炫耀的，本質上不過是矩陣相乘罷了。本節咱們考慮一個更好玩的問題。若是告訴你全部連桿（局部座標系）的位姿，你能不能算出機械臂的各個關節角來？你必定會說這很簡單，求一下反三角函數就好了。可是實際應用時常常會遇到比這個稍難些的問題：只告訴你機械臂最後一個連桿的位姿，如何獲得各關節的角度？這個問題被稱爲逆運動學。Robotic Toolbox工具箱中給出了兩個解逆運動學問題的函數：ikine 和 ikine6s，分別是數值解法和符號解析解法，本文咱們也用兩種方式解決逆運動學問題。 
說明：其它求解逆運動學的軟件工具還有 IKFast——適用於6自由度機械臂，求得的是解析解，求解速度賊快；Kinematics and Dynamics Library（KDL）——適用於任意自由度，求得的是數值解。這些代碼都是開源的，你能夠研究研究。

4.1 數值解法之——解方程

　　上一節的運動學函數 robotPartsKinematics 能獲得全部連桿的位姿。大多數時候，人們只關心最後一個連桿的位姿（由於它上面要裝載操做工具），即 Last@robotPartsKinematics[{IdentityMatrix[4], q}]（注意q是一個六維向量，即q=()），結果以下圖所示（另存爲能夠看大圖）。這裏關節角沒有設置數值，所以獲得的是符號解，有些長哦。這也是爲何機器人領域常用縮寫的緣由：好比把 記爲。在中提供了一個函數 SimplifyTrigNotation，能夠用來對下式進行縮寫。 


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

 

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

　　經過解上面這個以6個關節角  爲未知量的方程組就能知道機械臂的構型了。也就是說，逆運動學問題的本質就是解方程。從小到大咱們解過無數的方程。數學有很大一部分就是在研究怎麼解方程、解各類各樣的方程：大的小的、線性的非線性的、代數的微分的。Mathematica 提供了不止一個函數用來解方程：Solve、NSolve、DSolve、LinearSolve、FindRoot 等等。面對這麼多工具，咱們應該用哪一個好呢？你選用的求解方法取決於方程的類型，咱們看看這個方程是什麼類型呢？首先它是個代數方程，其次裏面含有三角函數，因此是非線性代數方程。代數方程有數值解法和解析解法。咱們很是想獲得用符號表示的解析解，由於只須要解一次之後直接帶入數值便可，計算速度很是快。可是非線性方程通常很可貴到符號解，因此咱們只好退而求其次找數值解了，這樣就把範圍縮小到 NSolve、FindRoot 這兩個函數了。NSolve 會獲得全部解（這個方程有不止一個解哦），而 FindRoot 會根據初始值獲得最近的解。一番試驗代表只有 FindRoot 函數能知足咱們的需求。 
說明：在求解逆運動學方程前還須要解決一個小問題：如何在 Mathematica 中表示一個指望的目標位姿 gt 呢？Mathematica 提供了 RollPitchYawMatrix 函數和 EulerMatrix 函數用來表示三維轉動（你用哪一個均可以），而後利用前面的 RPToH 函數合成爲位姿矩陣 gt 便可，示例代碼以下。其中，cuboid 函數用於繪製一個長方體。若是你使用 Matlab ，那我要可憐你了。由於 Matlab 沒有繪製長方體的函數，一切你都要本身畫。 而 Mathematica 定義了一些經常使用幾何圖形，能夠直接用。

cuboid[center_, dim_]:= Cuboid[center - dim/2, center + dim/2]
  • 1
  • 1
  1.  
    object = cuboid[{0, 0, 0}, {0.3, 0.2, 0.05}];
  2.  
    Manipulate[
  3.  
    gt = RPToH[RollPitchYawMatrix[{\[Alpha], \[Beta], \[Gamma]}], {x, y, z}];
  4.  
    Graphics3D[{Yellow, move3D[{frame3D, object}, gt]}, PlotRange -> {{-0.5, 0.5}, {-0.5, 0.5}, {-0.5, 0.5}}], Grid[{{Control[{{x, 0}, -0.5, 0.5, 0.1}], Control[{{\[Alpha], 0}, 0, 2 Pi, 0.1}]}, {Control[{{y, 0}, -0.5, 0.5, 0.1}],Control[{{\[Beta], 0}, 0, 2 Pi, 0.1}]}, {Control[{{z, 0}, -0.5, 0.5, 0.1}], Control[{{\[Gamma], 0}, 0, 2 Pi, 0.1}]}}], TrackedSymbols :> True]
  • 1
  • 2
  • 3
  • 4
  • 1
  • 2
  • 3
  • 4

 


　　 不過這個方程組是由  齊次變換矩陣獲得的，裏面有  個方程，去掉最後一列對應的4個恆等式還有12個方程，超過了未知量（6個）的個數，這是由於  旋轉矩陣的各項不是獨立的，所以要捨去一部分。該保留哪三項呢？只要不選擇同一行或同一列的三項就能夠了，這裏我保留了三項。

 

  1.  
    Manipulate[
  2.  
    gts = Last@robotPartsKinematics[{IdentityMatrix[ 4], {q1, q2, q3, q4, q5, q6}}];
  3.  
    gt = RPToH[RollPitchYawMatrix[{\[Alpha], \[Beta], \[Gamma]}], {x, y, z}];
  4.  
    Quiet[qts = {q1, q2, q3, q4, q5, q6}/.FindRoot[gts [[1, 4]] == gt[[1, 4]] && gts[[2, 4]] == gt[[2, 4]] && gts[[3, 4]] == gt[[3, 4]] && gts[[1, 2]] == gt[[1, 2]] && gts[[2, 3]] == gt[[2, 3]] && gts[[3, 3]] == gt[[3, 3]], {q1, 0}, {q2, 0}, {q3, 0}, {q4, 0.3}, {q5, 0.3}, {q6, 0.3}]];
  5.  
    planeXY = {FaceForm[], EdgeForm[Thickness[ 0.005]], InfinitePlane[{x, y, z}, {{0, 1, 0}, {1, 0, 0}}], InfinitePlane[{x, y, z}, {{0, 1, 0}, {0, 0, 1}}]};
  6.  
    lines = {Red, Thickness[ 0.0012], Line[{{x, y, z} + {100, 0, 0}, {x, y, z} + {-100, 0, 0}}], Line[{{x, y, z} + {0, 100, 0}, {x, y, z} + {0, -100, 0}}], Line[{{x, y, z} + {0, 0, 100}, {x, y, z} + {0, 0, -100}}]};
  7.  
    Graphics3D[{planeXY, lines, MapThread[move3D, {robotPartsColored, robotPartsKinematics[{IdentityMatrix[ 4], qts}]}], move3D[frame3D, g[I, L[7]]]}, PlotRange -> {{-1.5, 2.5}, {-2.5, 2.5}, {0, 3}}],
  8.  
    Grid[{{Control[{{ x, 1.3}, -2, 3, 0.1}], Control[{{y, 0}, -2, 2, 0.1}]},
  9.  
    {Control[{{z, 2}, 0, 3, 0.1}], Control[{{\[Alpha], 0}, 0, Pi, 0.1}]},
  10.  
    {Control[{{\[Beta], 0}, 0, Pi, 0.1}], Control[{{\[Gamma], 0}, 0, Pi, 0.1}]}}], TrackedSymbols :> True]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

　　一樣藉助 Manipulate 函數改變值的大小，試驗的效果以下圖。 


4.2 數值解法之——迭代法

 

　　解方程的方法不少，下面咱們換一種思路求解逆運動學方程，其思想來自於（英文版187頁），代碼以下：

  1.  
    forwardKinematicsJacobian[argList_, gst0_] :=
  2.  
    Module[{g = IdentityMatrix[4], \[Xi], n = Length[argList]},
  3.  
    Js = {}; (*注意空間雅可比矩陣Js是全局變量，後面會用*)
  4.  
    Do[\[Xi] = Ad[g].argList[[i, 1]];
  5.  
    Js = Join[Js, {\[Xi]}];
  6.  
    g = g.TwistExp[argList[[i, 1]], argList[[i, 2]]]
  7.  
    , {i, n}];
  8.  
    Js = Transpose[Js];
  9.  
    g.gst0 ]
  10.  
    \[Xi]a = Table[\[Omega]a[i] = axesDirInGlobal[[i]]; la[i] = axesPtInGlobal[[i]]; Join[Cross[-\[Omega]a[i], la[i]], \[Omega]a[i]], {i, n}];
  11.  
    (*forwardKinematicsJacobian函數是從 Screws.m 中抄的，它使用了表示在全局座標系的旋量，所以定義\[Xi]a*)
  12.  
    inverseKinematics[gt_, q0_, errorthreshold_: 0.0001] :=
  13.  
    Module[{gReal, q = q0, Jb, Jg, F, error, theta, axis, positionerror, angleerror, maxIter = 20},(*輸入指望的機械臂末端位姿 gt 和初始關節角 q0*)
  14.  
    Do[gReal = forwardKinematicsJacobian[Transpose@{\[Xi]a, q}, g[I, L[7], 0]];
  15.  
    Jb = Ad[Inverse[gReal]].Js;
  16.  
    Jg = diagF[gToR[gReal], gToR[gReal]].Jb;
  17.  
    positionerror = gToP[gt - gReal];
  18.  
    angleerror = Reverse@RollPitchYawAngles[gToR[gt.Inverse[gReal]]]; (*注意Reverse函數*)
  19.  
    error = Flatten[N[ {positionerror, angleerror}]]; (*偏差向量 error 包括位置和角度份量在全局座標系中表示*)
  20.  
    F = PseudoInverse[Jg].error;
  21.  
    q = q + modToPiPi[F];
  22.  
    If[Norm[error] < errorthreshold, Break[]]
  23.  
    , {maxIter}];
  24.  
    q]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

　　forwardKinematicsJacobian 函數用於計算（空間）雅可比矩陣和最後一個連桿的位姿，它修改自 Screws 工具包。逆運動學計算函數 inverseKinematics 的輸入是指望的末端連桿位姿 gt，迭代的初始角度 q0 ，以及偏差閾值 errorthreshold （默認值爲 0.0001）。 
　　其中的 modToPiPi 函數（實現代碼以下）用於將角度值轉換到  的範圍之間。這裏爲何須要 modToPiPi 函數呢？由於角度是個小妖精，若是咱們不盯緊它，它可能會時不時的搗亂。從外部看，機械臂的一個轉動關節位於角度  和角度  沒什麼區別。但是若是咱們聽任角度這樣隨意跳變，會致使軌跡不連續，這樣機械臂在跟蹤軌跡時就會出現麻煩。

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

　　其中，Ad 函數就是 Screws 工具包中的 RigidAdjoint 函數，它表示一個齊次變換矩陣的伴隨變換（Adjoint Transformation），diagF 函數用於將多個矩陣合成爲塊對角矩陣，實現代碼以下：

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

　　gToR 函數和 gToP 函數分別用於提取一個齊次變換矩陣中的旋轉矩陣（R）和位移向量（P），代碼以下。

  1.  
    gToR[g_]:= Module[{n=(Dimensions[g]) [[1]]-1}, g[[1;;n,1;;n]]]
  2.  
    gToP[g_]:= Module[{n=(Dimensions[g]) [[1]]-1}, g[[1;;n,n+1]]]
  • 1
  • 2
  • 1
  • 2

　　咱們之後會用到不少矩陣操做（好比轉置、求逆）。而 Mathematica 的函數名太長，爲了寫起來方便，我定義了簡寫的轉置和求逆函數，代碼以下：

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

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

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

　　而後調用逆運動學函數 inverseKinematics 挨個計算不一樣點處的關節值，代碼以下：

  1.  
    gStars = PToH /@ pts3D; (*將三維點的座標轉換成齊次變換矩陣，轉動部分始終不變*)
  2.  
    q = ConstantArray[ 0, n]; (*inverseKinematics函數包含一個迭代過程，所以須要提供一個初始值*)
  3.  
    g[I, L[ 7], 0] = (robotPartsKinematics[{IdentityMatrix[4], q}]; g[I, L[7]]); (*forwardKinematicsJacobian函數須要零位狀態下的末端連桿位姿*)
  4.  
    qs = Table[q = inverseKinematics[i, q], {i, gStars}];//AbsoluteTiming (*依次遍歷全部點，咱們用每次計算獲得的 q 做爲下一次迭代的初始值，這樣迭代速度更快*)
  • 1
  • 2
  • 3
  • 4
  • 1
  • 2
  • 3
  • 4

　　計算結果 qs 中存儲了機械臂到達不一樣點處的關節向量，若是之後咱們想讓機械臂跟蹤這個向量序列，能夠對其插值獲得連續的關節函數，這是靠 Interpolation 函數實現的，代碼以下。關於 Interpolation 函數我要囉嗦幾句，由於之後咱們可能會常常用到它。對於每一個關節來講， Interpolation 獲得的是一個插值函數（InterpolatingFunction），更確切地說是「Hermite多項式」 或「Spline 樣條」插值函數。它與其它的純函數沒什麼區別，能夠對它求導、求積分。例如，咱們能夠對這6個關節的插值函數求導從而獲得關節速度和加速度函數：

  1.  
    time = 10; (*time是本身定義的，表示機械臂運動通過全部點的總時間*)
  2.  
    Do[qt[i] = T@{Range[0, time, time/(Length[(T@qs)[[i]]] - 1)], (T@qs)[[i]]}, {i, n}];
  3.  
    Do[qfun[i] = Interpolation[qt[i]];
  4.  
    dqfun[ i][x_] = D[qfun[i][x], x];
  5.  
    ddqfun[ i][x_] = D[dqfun[i][x], x], {i, n}]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 1
  • 2
  • 3
  • 4
  • 5

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

  1.  
    pq = Plot[ Evaluate@Table[qfun[i][x], {i, 6}], {x, 0, time}, PlotRange -> All];
  2.  
    pdq = Plot[ Evaluate@Table[dqfun[i][x], {i, n}], {x, 0, time}, PlotRange -> All];
  3.  
    pddq = Plot[ Evaluate@Table[ddqfun[i][x], {i, n}], {x, 0, time}, PlotRange -> All];
  4.  
    GraphicsGrid[{{pq, pdq, pddq}}]
  • 1
  • 2
  • 3
  • 4
  • 1
  • 2
  • 3
  • 4

 


4.3 雅克比矩陣的零空間

 

　　在上一節求解逆運動學問題時咱們使用了機械臂的雅克比矩陣。雅克比矩陣可以將關節速度映射到末端連桿的速度。因爲末端連桿的速度有不止一種定義方式（例若有：空間速度、本體速度、全局速度，它們的定義見個人另外一篇博客），因此對應了不一樣的雅克比形式（也就是逆運動學函數中的 js、Jb、Jg）。 
　　雅克比矩陣有一些有趣的性質，其中一個比較有意思的是它的零空間。只要關節速度在（雅克比矩陣的）零空間中，那末端連桿的速度老是零（零空間由此得名）。通俗的說就是：無論關節怎麼動，末端連桿始終不動（就像被釘死了同樣）。這個性質還挺有用的，由於有些場合要求機械臂在抓取東西的時候還能躲避障礙物。在其它領域，例如攝影，爲了保證畫面穩定須要攝像機能防抖動；在動物王國中，動物覓食時頭部要緊盯獵物（被惡搞的穩定雞）；在軍事領域（例如坦克、武裝直升機），要求炮口始終瞄準目標，無論車身如何移動和顛簸。 

 


　　對於本文中的 6 自由度機械臂，因爲它不是冗餘的，因此大多數時候計算零空間會獲得空（說明不存在零空間）。我爲了展現零空間的效果只用了平移的部分。如下代碼展現了機械臂在（平移）零空間中的一種運動，以下圖所示。無論機械臂如何運動，末端連桿的位置始終不動（可是姿式會改變，矩陣mask 的做用就是濾掉轉動份量，只剩下沿  軸的平移運動）。

 

  1.  
    q = ConstantArray[0, n]; dt = 0.05;
  2.  
    g[ I, L[7], 0] = Last@robotPartsKinematics[{IdentityMatrix[4], q}];
  3.  
    {xl, zl, yl, xr, zr, yr} = IdentityMatrix[6];
  4.  
    mask = T[StackCols[xl, zl, yl]];
  5.  
    Animate[Jb = BodyJacobian[T@{\[Xi]a, q}, g[I, L[7], 0]];
  6.  
    gI7 = Last@robotPartsKinematics[{IdentityMatrix[4], q}];
  7.  
    Jg = diagF[gToR[gI7], gToR[gI7]].Jb;
  8.  
    Jgm = mask.Jg;
  9.  
    dq = Total[NullSpace[Jgm]]; (*零空間的一種線性組合方式，能夠改成其它線性組合*)
  10.  
    q = q + dq*dt;
  11.  
    Graphics3D[{MapThread[move3D, {robotPartsColored, robotPartsKinematics[{IdentityMatrix[4], q}]}]
  12.  
    , move3D[frame3D, g[ I, L[7], 0]]}], {i, 1, 1000, 1}]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12

 

 

5. 碰撞檢測

　　咱們生活的物質世界有一個法則：兩個物體不能同時佔據同一個空間位置，不然會有很大的力將它們分開。但是仿真是在虛擬的數字世界中進行的，這個世界可不遵照物質世界的那套力的法則，所以不夠真實。爲了讓機器人仿真更真實，咱們須要考慮「碰撞檢測」（Collision Detection）。爲了追求效率，工業機器人的運動速度一般比較快，並且抓着較重的負載，它一旦碰到障礙物或者人類，結果通常是「物毀人傷」。並且在一些用到規劃算法中，碰撞檢測也是很重要的一部分。因此在仿真時提早檢測是否有碰撞頗有必要。 
　　值得一提的是，如今一些先進的機器人控制器開始配備簡易的碰撞檢測功能，若是在機器人工做時有人忽然擋住了它，它會自動中止。這是經過檢測機械臂關節處電機的電流大小實現的。當機械臂碰到人時，它至關於受到了一個阻力，電機要想保持原來的速度運行須要加大電流，靈敏的控制器會感知到電流的波動，這樣咱們就能經過監視電流來判斷機械臂有沒有發生碰撞，若是電流超過必定範圍就認爲機械臂發生碰撞了，須要緊急剎車。但是這種碰撞檢測方法只適用於小負載（<5kg）的機械臂。由於對於重型機械臂，即使它也會停下來，但是它的慣性太大須要一段剎車距離，這足以對人形成傷害。 
　　碰撞檢測是一個比較有難度的幾何問題，目前有不少成熟的算法（AABB、GJK）。咱們的關注點在機器人，因此不想在碰撞檢測上浪費太多時間。爲此，咱們使用 Mathematica 自帶的 RegionDisjoint 函數實現碰撞檢測。在幫助文檔中，咱們瞭解到RegionDisjoint 函數用於判斷多個幾何體是否相交，若是兩兩之間都不相交則返回 True ，而兩個幾何體出現了相交，就表示它們發生了碰撞（太好了，這簡直是爲碰撞檢測量身定作的函數）。 


　　RegionDisjoint 函數能夠用於二維圖形，也能夠用於三維圖形，甚至能夠用於非凸的圖形，以下面的例子所示，其中使用了Locator 控件。若是你使用了較早的軟件版本，可能沒有RegionDisjoint 函數，這時能夠用 Graphics`Mesh`IntersectQ 代替，不過前面要加一個取反操做。 

 

  1.  
    pts = {{0.95, 0.31}, {0.36, -0.12}, {0.59, -0.81}, {0., -0.38}, {-0.59, -0.81}, {-0.36, -0.12}, {-0.95, 0.31}, {-0.22, 0.31}, {0., 1.}, {0.22, 0.31}, {0.95, 0.31}};
  2.  
    Manipulate[
  3.  
    obstacle1 = Disk[pt1, 1];
  4.  
    obstacle2 = Polygon[pt2 + # & /@ pts];
  5.  
    color = If[RegionDisjoint[obstacle1, obstacle2], Green, Red];
  6.  
    (*!Graphics`Mesh`IntersectQ[{obstacle1,obstacle2}]*)
  7.  
    Graphics[ {EdgeForm[Black], color, obstacle1, obstacle2}, PlotRange -> 3], {{pt1, {-1, -1}}, Locator}, {{pt2, {1, 1}}, Locator}, TrackedSymbols :> True]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

　　不過有了 RegionDisjoint 函數並不意味着一勞永逸。「碰撞檢測」是有名的計算資源吞噬者，它會佔用大量CPU資源。咱們通常但願碰撞檢測越快越好，但是精度和速度是一對矛盾，追求速度只能犧牲必定的精度。若是不追求很高的精度，碰撞檢測應該保守一些。也就是說，在實際沒發生碰撞時容許誤報，但在發生碰撞時不能漏報——寧肯錯殺一千，不可放過一個。碰撞檢測的計算量與模型的複雜程度有關。咱們導入的機器人模型雖然已經通過了「瘦身」，但用於碰撞檢測仍是有些複雜。爲此，咱們須要進一步縮減。爲了保守一點，咱們採用比真實機械臂零件稍大些的模型，好比零件的凸包（Convex Hull）。雖然 Meshlab 軟件能夠製做凸包，可是效果不太好。好在 Mathematica 自帶的 ConvexHullMesh 函數能夠計算任意幾何體的凸包。我採用的方法是先用 ConvexHullMesh 分別計算各零件的凸包，再導出零件用 Meshlab 進一步簡化，最後再導入。計算零件凸包及導出所需的代碼以下。（注意：因爲零件數據已是變換後的了，簡化後的零件導入後不須要旋轉等變換）

  1.  
    robotPartsCH = Table[
  2.  
    pts = robotParts [[i, 1]];
  3.  
    poly = robotParts [[i, 2, 2, 1]];
  4.  
    R = ConvexHullMesh[pts];
  5.  
    pts = MeshCoordinates[R];
  6.  
    poly = MeshCells[R, 2];
  7.  
    R = MeshRegion[pts, poly];
  8.  
    Export[ "D:\\MOTOMAN-ES165D-C" <> partsName[[i]], R];
  9.  
    GraphicsComplex[pts, poly], {i, 7}];
  10.  
    Graphics3D[robotPartsCH]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

　　咱們檢驗一下機械臂和外部障礙物的碰撞檢測（至於機械臂連桿之間的碰撞咱們暫時不考慮），代碼以下（效果以下圖所示）。

  1.  
    Robs = cuboid[ {1.3, 0, 0.5}, {0.5, 0.5, 1.0}]; (*障礙物，一個長方體*)
  2.  
    Manipulate[
  3.  
    gs = robotPartsKinematics[ {IdentityMatrix[4], {q1, q2, q3, q4, q5, q6}}];
  4.  
    Rparts = Table[MeshRegion[ptTransform[gs[[i]]] /@ robotParts[[i, 1]], robotParts[[i, 2, 2]]], {i, 7}];
  5.  
    bool = And @@ (RegionDisjoint[Robs, #] & /@ Rparts);
  6.  
    color = If[bool, Black, Red]; txt = If[bool, "哈哈，沒事", "啊...碰撞了！"];
  7.  
    Graphics3D[{Gray, Robs, Text[Style[txt, FontSize -> 20, FontFamily -> "黑體", FontColor -> color], {-0.5, 1, 1.5}], {MapThread[move3D, {robotPartsColored, gs}]}}, plotOptions],
  8.  
    Grid[{{Control[{{q1, 0}, -Pi, Pi, 0.1}],Control[{{q2, 0}, -Pi, Pi, 0.1}]}, {Control[{{q3, 0}, -Pi, Pi, 0.1}], Control[{{q4, 0}, -Pi, Pi, 0.1}]}, {Control[{{q5, 0}, -Pi, Pi, 0.1}], Control[{{q6, 0}, -Pi, Pi, 0.1}]}}], TrackedSymbols :> True]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

　　其中，ptTransform[g][pt3D] 函數的功能是用齊次變換矩陣 g 對三維座標 pt3D 作變換，代碼以下：

  1.  
    ptTransform[ g_][pt3D_]:=Module[{hPt3D,transfomredPt},
  2.  
    hPt3D = Join[pt3D,{1.0}];
  3.  
    transfomredPt = g.hPt3D;
  4.  
    transfomredPt[[ 1;;3]] ]
  • 1
  • 2
  • 3
  • 4
  • 1
  • 2
  • 3
  • 4

 

 

6. 軌跡規劃

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

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

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


　　製做上面的軌跡須要如下幾個步驟： 
　　1. 首先隨機生成一些三維空間中的點。

 

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

　　2. 而後利用 BSplineFunction 函數對點插值。

bfun = BSplineFunction[pts];
  • 1
  • 1

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

 

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

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

　　而後分別對各座標軸進行單獨插值（這裏我一樣將自變量的取值範圍設定在 0∼1 之間）：

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

　　並定義一個新的插值函數爲各份量的合成。這樣咱們就人工製做了一段軌跡（或者說，是一個向量值函數）。

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

　　咱們能對這段軌跡作什麼呢？ 
　　●　能夠計算它的弧長：

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

　　●　既然能夠計算弧長，就能用弧長對這條曲線從新參數化（我之前在學高等數學時，一直想不通怎麼用弧長對一個曲線參數化，如今經過編程實踐就很容易理解了）：

  1.  
    arcLs = Table[ArcLength[Line[bfpts [[1 ;; i]]]], {i, Length[bfpts]}]/ArcLength[Line[bfpts]];
  2.  
    ifunArcx = Interpolation[Transpose[{arcLs, bfpts [[;; , 1]]}]];
  3.  
    ifunArcy = Interpolation[Transpose[{arcLs, bfpts [[;; , 2]]}]];
  4.  
    ifunArcz = Interpolation[Transpose[{arcLs, bfpts [[;; , 3]]}]];
  5.  
    ifunArc[t_]:= {ifunArcx[t], ifunArcy[t], ifunArcz[t]}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 1
  • 2
  • 3
  • 4
  • 5

　　咱們能夠觀察兩種參數化的軌跡的圖像：

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

　　咱們說軌跡比路徑包含更多的信息，但是若是單看路徑，咱們能提取出什麼信息呢？ 
　　路徑只包含幾何信息：對於一個三維空間中的路徑（曲線），咱們能計算路徑上每一點的切線和法線，它們恰好能惟一地肯定一個直角座標系（這個座標系又被稱爲 Frenet 標架），以下圖所示（對應的代碼以下）。你們都知道，平面上的曲線能夠用曲率描述它的彎曲程度，但是要描述三維空間曲線的彎曲程度還須要一個量，叫撓率（它是描述扭曲程度的）。若是把Frenet 標架想象成過山車，你坐在上面就能更直觀地感覺曲率和撓率的含義。 

 

  1.  
    basis = Last@FrenetSerretSystem[ifun[x],x];
  2.  
    p1 = ParametricPlot3D[ifun[t],{t, 0,1},PlotRange->1];
  3.  
    Manipulate[pt = ifun[t];
  4.  
    tangent = Arrow[Tube[{pt,pt+(basis [[1]]/.x->t)/3}]];
  5.  
    normal = Arrow[Tube[{pt,pt+(basis [[2]]/.x->t)/3}]];
  6.  
    binormal= Arrow[Tube[{pt,pt+(basis [[3]]/.x->t)/3}]];
  7.  
    p2 = Graphics3D[{Arrowheads[ 0.03],Red,tangent,Green,normal,Blue,binormal}];
  8.  
    Show[p1,p2],{t, 0,1,Appearance->{"Open"}}]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

6.2 軌跡規劃方法

　　「軌跡規劃」中的「規劃」又是什麼意思呢？ 
　　規劃的英文是 plan，也翻譯爲「計劃、打算」。你確定知道「計劃」是什麼意思，計劃就是在作事以前先想一想應該怎麼作纔好。並且，一般你有一個要到達的目標，沒有目標談不上計劃（固然通常還得有一個出發點，但這不是必需的）。假如我想放假出去玩，在制定了詳細的開車路線後我連要去哪都不知道，那我是否是神經病呢。正常人都是先決定去哪，而後才選擇交通線路。此外，計劃還有個評價的標準——怎麼樣纔算「好」呢？若是沒有標準，那咱們還計劃個什麼勁兒啊（反正沒有好壞之分）？把目標和評價標準推廣到機器人的軌跡規劃領域就是：機器人怎麼（運動）才能到達一個目標，並且不只僅是到達目標，有時咱們還想以最好的方式（好比最快、消耗能量最少）到達，這就是軌跡規劃的任務。「軌跡規劃」的叫法挺多，有叫「軌跡生成」的，有叫「運動規劃」的，但無論怎麼叫其實大概都是一個意思。 
　　對於機械臂來講，軌跡規劃方法能夠根據有沒有障礙物來劃分。若是沒有障礙物，那就簡單些了，咱們能夠直接規劃軌跡；若是有障礙物則通常先規劃路徑（由於路徑包含信息更少，相對更簡單），而後對路徑設置速度獲得軌跡（由於主要的工做都在規劃路徑，所以也可稱其爲「路徑規劃」）。 
　　路徑規劃都有哪些方法呢？比較流行的有：圖搜索、勢場法、RRT 等等。下面咱們來實現 RRT 方法。 
　　RRT（快速探索隨機樹） 是一種通用的方法，無論什麼機器人類型、無論自由度是多少、無論約束有多複雜都能用。並且它的原理很簡單，這是它在機器人領域流行的主要緣由之一。不過它的缺點也很明顯，它獲得的路徑通常質量都不是很好，可能包含棱角，不夠光滑，也可能遠離最優。

　

　　RRT 能在衆多的規劃方法中脫穎而出，它到底厲害在哪裏呢？ 
　　天下武功惟快不破，「快」是 RRT 的一大優勢。RRT 的思想是快速擴張一羣像樹同樣的路徑以探索（填充）空間的大部分區域，乘機找到可行的路徑。之因此選擇「樹」是由於它可以探索空間。咱們知道，陽光是樹木惟一的能量來源。爲了最大程度地利用陽光，樹木要儘可能用較少的樹枝佔據儘可能多的空間。固然能探索空間的不必定非得是樹，好比Peano曲線也能夠作到，如上圖左所示的例子。雖然像Peano曲線這樣的也能探索空間，可是它們太「肯定」了。在搜索軌跡的時候咱們可不知道出路應該在哪裏，若是不在「肯定」的搜索方向上，咱們怎麼找也找不到（找到的機率是0）。這時「隨機」的好處就體現出來了，雖然不知道出路在哪裏，可是經過隨機的反覆試探仍是能碰對的，並且碰對的機率隨着試探次數的增多愈來愈大，就像買彩票同樣，買的數量越多中獎的機率越大（RRT名字中「隨機」的意思）。但是隨機試探也講究策略，若是咱們從樹中隨機取一個點，而後向着隨機的方向生長，那麼結果是什麼樣的呢？見上圖右。能夠看到，一樣是隨機樹，可是這棵樹並沒很好地探索空間，它一直在起點（紅點）附近打轉。這可很差，咱們但願樹儘可能經濟地、均勻地探索空間，儘可能不過分探索一個地方，也不能漏掉大部分地方。這樣的一棵樹怎麼構造呢？ 
　　RRT 的基本步驟是： 
　　1. 起點做爲一顆種子，從它開始生長枝丫； 
　　2. 在機器人的「構型」空間中，生成一個隨機點 ； 
　　3. 在樹上找到距離  最近的那個點，記爲  吧； 
　　4.  朝着  的方向生長，若是沒有碰到障礙物就把生長後的樹枝和端點添加到樹上，返回 2； 
　　隨機點通常是均勻分佈的，因此沒有障礙物時樹會均勻地向各個方向生長，這樣能夠快速探索空間（RRT名字中「快速探索」的意思），以下圖所示。固然若是你事先掌握了最有可能發現路徑的區域信息，能夠集中兵力重點探索這個區域，這時就不宜用均勻分佈了。 
　　RRT 的一個弱點是難以在有狹窄通道的環境找到路徑。由於狹窄通道面積小，被碰到的機率低，找到路徑須要的時間要看運氣了。下圖展現的例子是 RRT 應對一我的爲製做的狹窄通道，有時RRT很快就找到了出路，有時則一直被困在障礙物裏面。對應的代碼以下（這段代碼只用於演示 RRT 的原理，不是正式代碼，但它有助於理解正式代碼的運算過程）： 

　

 

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

　　RRT探索空間的能力仍是不錯的，例以下圖左所示的例子，障礙物多並且雜亂（實現這個例子所需的全部代碼不會超過30行）。還有沒有環境能難住RRT呢？下圖右所示的迷宮對RRT就是個挑戰。這個時候空間被分割得很是嚴重，RRT顯得有些力不從心了，可見隨機策略不是何時都有效的。 
　　「隨機」使得RRT有很強的探索能力。可是成也蕭何敗也蕭何，「隨機」也致使 RRT 很盲目，像個無頭蒼蠅同樣處處亂撞。一個改進的辦法就是給它一雙「慧眼」（慧眼表明信息）。在勢場法中，勢函數攜帶了障礙物和目標的信息，若是能把這個信息告訴 RRT ，讓它在探索空間時有傾向地沿着勢場的方向前進會更好。這樣，RRT 出色的探索能力恰好能夠彌補勢場法容易陷入局部極小值的缺點。 

　

　　 
　　將RRT方法用在機械臂上的效果以下圖所示（綠色表示目標狀態）。我設置了4個障礙物（其中一個是大地），這對機械臂是個小小的挑戰。因爲咱們生活在三維空間，沒辦法看到6維關節空間，因此我把6維關節空間拆成了2個三維空間，分別對應前三個關節和後三個關節： 

　　正式 RRT 的主程序代碼以下。我將 RRT 樹定義爲由節點列表 nodes 和樹枝列表 edges 組成，即 tree = {nodes, edges}。其中節點列表 nodes 存儲樹中全部節點（每一個節點都是一個 6 維向量，表示機械臂的關節值），樹枝列表 edges 中存儲全部樹枝，樹枝定義爲兩個節點的代號（節點的代號定義爲節點被添加到樹的順序，好比添加新節點時樹中已經有4個節點了，那麼新節點的代號就是 5）。

 

  1.  
    obsCenters = {{0,0,-0.15},{1.4,-0.8,0.5},{1,1,0.7},{0.5,0,2.3}};
  2.  
    obsDims = {{8,8,0.2},{0.5,0.8,1.0},{0.7,0.3,1.4},{3,0.1,0.9}};
  3.  
    Robs = MapThread[cuboid, {obsCenters, obsDims}]; (*定義4個長方體障礙物*)
  4.  
    step = 0.2; (*樹枝每次生長的長度，這裏簡單設置爲固定值*)
  5.  
    q0 = {-1.54, 0.76, 0.66, -1.14, -1.44, 0}; (*起點*)
  6.  
    qt = {1.76, 0.76, 0.46, 0, 0.36, 0}; (*目標點*)
  7.  
    nodes = {q0}; (*以起點q0做爲種子*)
  8.  
    edges = {}; (*樹枝初始爲空*)
  9.  
    RRTtree = {nodes, edges}; (*樹的初始化由節點和樹枝組成，它是全局變量*)
  10.  
    maxIters = 2000; (*最大迭代步數*)
  11.  
    jointLims = {{-Pi, Pi}, {-Pi/2, Pi/2}, {-Pi, 1.45}, {-Pi, Pi}, {-2, 2}, {-Pi, Pi}}; (*關節極限範圍，有些關節值不可取*)
  12.  
    qRandList = RandomPoint[ Cuboid[ConstantArray[-Pi, n], ConstantArray[Pi, n]], maxIters, jointLims]; (*一次性生成全部的隨機點*)
  13.  
    Do[nodes = RRTtree[[1]];
  14.  
    If[Min[Norm /@ ((-qt+#)&/@nodes)] < 0.01, Print["哈哈，到達目標了!"]; Break[]];
  15.  
    qRand = RandomChoice[{qRandList[[i]], qt}]; (*以50%的機率貪婪地試探着朝目標生長*)
  16.  
    grow[qRand,step], {maxIters}]; // AbsoluteTiming
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

　　構造樹用到了如下自定義的函數： 
　　1. 首先是碰撞檢測函數 collisionDetection，若是機械臂沒有碰到障礙物就返回True。爲了節省時間，碰撞檢測使用的是機械臂各零件的凸包，在最好的顯示時才使用原始零件。

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

　　2. 碰撞檢測函數須要 Region 類型的變量，爲此定義 toRegion 函數將幾何體變換爲 Region 類型，代碼以下：

  1.  
    toRegion[q_]:= Module[{gs, Rparts},
  2.  
    gs = robotPartsKinematics[{IdentityMatrix[ 4], q}];
  3.  
    Rparts = Table[MeshRegion[ptTransform[gs [[i]]] /@ robotParts[[i, 1]], robotParts[[i, 2, 2]]], {i, 7}]]
  • 1
  • 2
  • 3
  • 1
  • 2
  • 3

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

  1.  
    addNode[node_]:= Module[{}, AppendTo[RRTtree [[1]], node]; Length[RRTtree[[1]]] ]
  2.  
    addEdge[edge_]:= Module[{}, AppendTo[RRTtree [[2]], edge]; Length[RRTtree[[2]]] ]
  • 1
  • 2
  • 1
  • 2

　　4. 樹枝朝着採樣點生長（只檢測一點的碰撞狀況）：

  1.  
    grow[qRand_,step_: 0.2]:= Module[{qNearestIdx, qNearest, growAngles},
  2.  
    {qNearestIdx} = Nearest[nodes -> Automatic, qRand, 1];(*選擇最近的節點*)
  3.  
    qNearest = nodes[[qNearestIdx]];
  4.  
    growDirection = Normalize[qRand - qNearest];
  5.  
    qExpand = modToPiPi[qNearest + step * growDirection]; (*生長*)
  6.  
    Rrobot = toRegion[qExpand];
  7.  
    If[collisionDetection[Rrobot, Robs], qNewIdx = addNode[qExpand]; (*添加新節點，返回新節點的代號 Idx *)
  8.  
    addEdge[ {qNearestIdx, qNewIdx}]] ]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

　　5. 若是有樹枝到達目標節點，backTrack 函數用於從樹中抽取出鏈接起點和目標點的路徑：

  1.  
    backTrack[nodeIdx_]:=
  2.  
    Module[ {searchNodeIdx = nodeIdx, nodes = RRTtree[[1]], edges = RRTtree[[2]], searchNodePos, preNodeIdx, pathIdx = {}, pathCoords},
  3.  
    Do[{searchNodePos} = FirstPosition[edges[[All, 2]], searchNodeIdx];
  4.  
    preNodeIdx = edges [[searchNodePos, 1]];
  5.  
    AppendTo[pathIdx, preNodeIdx];
  6.  
    If[preNodeIdx == 1, Break[], searchNodeIdx = preNodeIdx], {Length[edges]}];
  7.  
    pathIdx = Reverse[pathIdx];
  8.  
    pathCoords = nodes [[pathIdx]] ]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

　　下面的代碼能夠顯示搜索到的（關節空間中的）路徑。這條路徑質量不高，若是用於機器人的軌跡跟蹤還須要通過後期的平滑處理。

  1.  
    edges = RRTtree [[2]];
  2.  
    targetIdx = edges [[-1, 2]];
  3.  
    qPath = backTrack[targetIdx];
  4.  
    anframe[i_] := Graphics3D[{q = qPath [[i]]; Robs, MapThread[move3D, {robotPartsColored, robotPartsKinematics[{IdentityMatrix[4], q}]}]}];
  5.  
    Animate[anframe[i], {i, 1, Length[qPath], 1}]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 1
  • 2
  • 3
  • 4
  • 5

7. 動力學仿真

　　你能夠在淘寶花2塊錢買個知網帳號，而後在其中搜索以「工業機器人控制」爲關鍵詞的學位論文並下載下來看看。在粗略地瀏覽了2030篇論文的目錄以後，你就會像我同樣總結出一個規律： 
　　●　碩士論文通常都創建了機器人的運動學模型。 
　　●　博士論文通常都創建了機器人的動力學模型。 
　　既然運動學已經可以幫助機器人動起來了，爲何還須要費那麼大勁創建動力學（以致於須要博士出馬）？ 
　　在前面的運動學一節中，咱們能經過改變各個關節角度控制機械臂運動。可是在實際機械臂上，關節角度還不是直接控制的，它須要由電機驅動。那麼電機應該輸出多大的力才能驅動機械臂運動呢，所須要的電流又是多大呢？只有知道這些咱們才能真正實現對機械臂的控制。如今的工業機器人大多采用兩層的控制方式，上層控制器直接輸出角度信號給底層驅動器，底層驅動器負責控制電機的電流實現上層給出的運動。上層不須要知道機器人的動力學也能夠，更不用管須要輸出多大電流。若是你的機器人不須要過高的運動速度和精度，動力學沒什麼太大用處（運動學是必需的，動力學不是必需的）。但是若是你的機器人速度很快，動力學效應就很明顯了，這時就要考慮動力學。在高級的機器人控制器中，都有力矩補償功能（例如匯川、KEBA的控制器）。這個補償的力矩是怎麼來的呢？就是經過動力學方程計算獲得的。補償力矩用做前饋控制信號，將其添加到驅動器上能使機器人更好地跟蹤一段軌跡。 

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


　　咱們如何獲得機器人的動力學模型呢？ 
　　宅男牛頓首開先河，在同時代的人還渾渾噩噩的時候初步搞明白了力、速度、慣性都是怎麼回事，並用數學對其進行了定量描述，從而創建了物體作平移運動時的動力學方程。從牛頓的身上咱們知道，學好數學是有多重要。在那個遍地文盲的年代，牛頓偷偷地自學了歐幾里得、笛卡爾、帕斯卡、韋達等大師的著做。 
　　勤奮的歐拉再接再礪，將牛頓的方程推廣到轉動的狀況。哥倆的工做結合起來恰好能夠完整地描述物體的運動，這就是牛頓-歐拉法。 
　　博學的拉格朗日發揚光大，又將牛頓和歐拉的工做總結提煉，提出了拉格朗日法。拉格朗日真聰明啊，只須要計算了物體的動能，而後再一微分就獲得了動力學方程，這是多麼簡潔統一的方法啊。但是拉格朗日法的缺點是它的效率過低了。對於4自由度如下的機械臂，計算符號解的時間咱們還能忍受。至於6自由度以上的機械臂，大多數人都沒這個耐心了（十幾分鍾到數小時）。並且計算出來的動力學是一大坨複雜的公式，很難分析利用。因此本文咱們採用牛頓-歐拉法創建機械臂的動力學模型（更準確的說是它的升級版——遞歸牛頓-歐拉法）。

 

7.1 慣性參數

　　早期工業機器人不使用動力學模型是有道理的，一個緣由是動力學的計算量太大，在高效的計算方法被發現以前，早年的老計算機吃不消；另外一個緣由就是動力學須要慣性參數。運動學只須要尺寸參數，這些相對好測量；但是動力學不只須要尺寸參數，還須要慣性參數。測量每一個連桿的質量、質心的位置、轉動慣量很麻煩，尤爲是當連桿具備不規則的形狀時，精度很難保證。若是使用動力學帶來的性能提高並不明顯，誰也不想給本身找麻煩。 
　　要使用動力學模型，慣性參數必不可少。例如 Robotics Toolbox 工具箱中的 mdl_puma560.m 文件就存儲了 PUMA-560 機器人的慣性參數。不一樣型號的機器人具備不一樣的慣性參數，並且機器人抓住負載運動時，也要把負載的慣性考慮進來。 
　　有些狀況下，咱們不須要知道很精確的慣性參數，差很少夠用就好了；但是有些場合對精度有要求，好比拖動示教就要求參數與實際值的偏差通常不能超過10%。對於精度要求不高的場合，能夠使用一個近似值。大多數三維建模軟件（例如 SolidWorks、CATIA）以及一些仿真軟件（例如 Adams）都提供慣性計算功能，一些數學軟件（Mathematica）也有用於計算慣性的函數（我沒有對比過，因此不敢保證這些軟件的計算結果都是同樣的）。本文以 SolidWorks 爲例介紹如何獲取慣性參數。 
　　計算以前首先要設置零件的材質。在 SolidWorks 中打開一個零件，在左側的「材質」上單擊右鍵彈出「材料」對話框，以下圖所示。在這裏能夠設置機器人本體的材質，MOTOMAN-ES165D 這款機器人的連桿是鑄鋁（鑄造鋁合金 Cast Aluminum）製造的。不過連桿沒有把電機等部件包含進去，爲此選擇密度大一點的材料，本文選擇鋼鐵。這裏最重要的是材料的密度，鋼鐵的密度通常是7.8噸/立方米（在計算慣性時，軟件假設零件的密度是均勻的，這明顯是簡化處理了）。設置好後點擊應用便可。 

　　而後在上方「評估」選項卡中單擊「質量屬性」就會彈出以下圖所示的對話框。 


　　SolidWorks 很快就計算出了這個零件的全部慣性參數。不過這裏的信息量有點大，我逐個說明： 
　　　首先是零件的質量：172.28 千克。若是你顯示的單位不是千克，能夠在當前對話框中的「選項」中修改單位。 
　　　而後是零件的質心（或重心）座標系，重心座標系的原點也給出了：，注意它是相對於繪圖座標系的哦。重心座標系的姿態下面會解釋。 
　　　最後是零件的慣性張量，這個有些人可能不懂，我詳細解釋下。SolidWorks列出了3個慣性張量，它們之間的區別就在於分別相對於不一樣的座標系： 
　　①　相對於質心座標系；其中的 Ix、Iy、Iz 三個向量表示質心座標系相對於繪圖座標系的姿態（也就是質心座標系的 x、y、z 三個軸向量在繪圖座標系中的表示），而 Px、Py、Pz 表示慣性主力矩（你要問我是怎麼知道的，點「幫助」按鈕）。慣性張量的形式是對角矩陣： 

 

　　②　相對於原點與質心座標系重合，可是各軸與繪圖座標系一致的座標系。SolidWorks只給出了慣性張量中各項的值。慣性張量的完整形式是對稱矩陣（注意裏面的負號）： 

 

　　③　相對於繪圖座標系（SolidWorks中稱爲輸出座標系），慣性張量的形式也是對稱矩陣（一樣注意裏面的負號）： 

 

　　這三個慣性張量都反映了同一個零件的性質，所以應該是等價的。那麼它們之間有什麼關係嗎？有的，它們之間能夠轉換。若是定義旋轉矩陣 ，質心座標向量 ，零件質量爲 ，那麼有

 
 

　　其中，表示向量的斜對稱矩陣，這須要自定義函數（skew）實現，代碼以下：

 

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

　　這組公式來自於，我已經驗證過了，百分百正確，不信的話你也能夠試試（要想結果比較接近，這些參數至少要取到小數點後5位，這依然是在「選項」頁中設置）。 
　　咱們獲得了三個慣性張量，在動力學方程中咱們應該使用哪一個呢？下面的程序使用了 ，由於它是相對於繪圖座標系的，而我創建運動學時選擇的局部座標系就是繪圖座標系。（我之後在這裏補充個單剛體動力學的例子） 
　　 
7.2 正動力學仿真

　　若是給你一條軌跡，如何設計控制率讓機械臂（末端）跟蹤這條軌跡呢，控制率的跟蹤效果怎麼樣呢？藉助正動力學，咱們就能夠檢驗所設計的控制率。 
　　因爲後面的程序所依賴的動力學推導過程採用了相對自身的表示方法（也就是說每一個連桿的速度、慣性、受力這些量都是相對於這個連桿自身局部座標系描述的），旋量也是如此，爲此須要從新定義旋量（代碼以下）。其實旋量軸的方向沒變（由於局部座標系的姿態與全局座標系同樣），只是改變了軸上點的相對位置。

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

　　正動力學的 遞歸牛頓-歐拉算法 代碼以下。能夠看到，動力學模型比運動學模型複雜多了（動力學用到運動學，運動學卻不須要動力學）。對於不少第一次接觸機器人的同窗來講，動力學是一隻可怕的攔路虎，要搞明白十幾個變量都是什麼含義可不容易（在仿真的時候可能包含幾十個變量，任何一個弄錯了都會全盤皆輸，動力學可比運動學難伺候多了）。由於動力學模型是一個微分方程，因此整個仿真過程就是個數值積分的過程。

  1.  
    (*參數初始化*)
  2.  
    endTime= 10.0; steps=2000; dt=endTime/steps; (*仿真時長、步數、步長*)
  3.  
    gravityAcc= 9.80665; (*重力加速度*)
  4.  
    Do[mass[i]=1.0; gForce[i]=gravityAcc*mass[i]*{0,0,-1,0,0,0},{i,n+1}]; (*mass[i]表示第i個連桿的質量，具體值本身設，重力是z軸的負方向*)
  5.  
    Do[\[ScriptCapitalM][i]=IdentityMatrix[6],{i,n+1}]; (*\[ScriptCapitalM][i]表示第i個連桿的廣義慣性矩陣，它包含質量和慣性張量*)
  6.  
    g[L[n+ 1],L[n+2]]=g[I,L[1]]=IdentityMatrix[4];
  7.  
    q=dq=ddq=ConstantArray[ 0,n]; (*關節角度、速度、加速度初始時刻假設都爲0*)
  8.  
    Table[V[i]=dV[i]=ConstantArray[ 0,6],{i,n+1}]; (*連桿i的廣義速度V[i]包括線速度和角速度，也假設開始時刻都爲0*)
  9.  
    \[ CapitalPi][n+2]=ConstantArray[0,{6,6}]; (*中間變量，沒啥具體物理意義，只是迭代初始值*)
  10.  
    \[ Beta][n+2]=ConstantArray[0,6]; (*也是中間變量*) (*如下是計算過程*)
  11.  
    qList=Table[
  12.  
    　 dq=dq+ddq *dt; q=q+dq*dt;(*歐拉積分*)　
  13.  
    For[i=2,i<=n+1,i++, (*速度前向遞歸，從第二個連桿開始到最後一個連桿*)
  14.  
    　　 k=i-1; (*由於本文的連桿從1開始標記，因此第i個連桿依賴前一個關節（i-1）*)
  15.  
    　　 g[L[i-1],L[i]]=TwistExp[\[Xi]r[[k]],q[[k]]].g[L[i-1],L[i],0];
  16.  
    　　 g[I,L[i]]=g[I,L[i-1]].g[L[i-1],L[i]];
  17.  
    　　 V[i]=Ad[Iv[g[L[i-1],L[i]]]].V[i-1]+\[Xi]rb[[k]]*dq[[k]];
  18.  
    　　 \[ Eta][i]=ad[V[i]-\[Xi]rb[[k]]*dq[[k]]].\[Xi]rb[[k]]*dq[[k]];
  19.  
    　　 ];
  20.  
    For[i=n+1,i>=2,i--, (*力和慣量後向遞歸，從最後一個連桿開始到第二個連桿*)
  21.  
    　　 k=i- 1;
  22.  
    　　 \[ Tau][k] = 0.0; (*施加關節力矩*)
  23.  
    　　 \!\(\*OverscriptBox[ \(\[ScriptCapitalM]\), \(^\)]\)[i]=\[ScriptCapitalM][i]+T[Ad[Iv[g[L[i],L[i+1]]]]].\[CapitalPi][i+1].Ad[Iv[g[L[i],L[i+1]]]];
  24.  
    　　 Fex[i]=T[Ad[RToH[gToR[g[I,L[i]]]]]].gForce[i];
  25.  
    　　 \[ ScriptCapitalB][i]=-T[ad[V[i]]].\[ScriptCapitalM][i].V[i]-Fex[i]+T[Ad[Iv[g[L[i],L[i+1]]]]].\[Beta][i+1];
  26.  
    　　 \[ CapitalPsi][i]=1/(\[Xi]rb[[k]].\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].\[Xi]rb[[k]]);
  27.  
    　　 \[ CapitalPi][i]=\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i]-\[CapitalPsi][i]*KroneckerProduct[\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].\[Xi]rb[[k]],\[Xi]rb[[k]].\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i]];
  28.  
    　　 \[ Beta][i]=\[ScriptCapitalB][i]+\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].(\[Eta][i]+\[Xi]rb[[k]]*\[CapitalPsi][i]*(\[Tau][k]-\[Xi]rb[[k]].(\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].\[Eta][i]+\[ScriptCapitalB][i])));
  29.  
    ];
  30.  
    For[i=2,i<=n+1,i++, (*加速度前向遞歸，從第二個連桿開始到最後一個連桿*)
  31.  
    　　 k=i-1;
  32.  
    　　 ddq[[k]]=\[CapitalPsi][i]*(\[Tau][k]-\[Xi]rb[[k]].\!\(\*OverscriptBox[\(\[ScriptCapitalM]\), \(^\)]\)[i].(Ad[Iv[g[L[i-1],L[i]]]].dV[i-1]+\[Eta][i])-\[Xi]rb[[k]].\[ScriptCapitalB][i]);
  33.  
    　　 dV[i]=Ad[Iv[g[L[i-1],L[i]]]].dV[i-1]+\[Xi]rb[[k]]*ddq[[k]]+\[Eta][i]
  34.  
    　　 ];
  35.  
    　　 q , {t, 0, endTime, dt}];//AbsoluteTiming (*顯示計算耗時*)
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35

　　其中， ad 函數用於構造一個李代數的伴隨表達形式，代碼以下。（開始咱們定義的關節旋量是李代數，連桿的速度在自身局部座標系下的表達也是一個李代數，可是加速度卻不是）

  1.  
    ad[\[Xi]_] := Module[{w, v},
  2.  
    v = skew[ \[Xi][[1 ;; 3]]]; w = skew[\[Xi][[4 ;; 6]]];
  3.  
    Join[Join[w, v, 2], Join[ConstantArray[0, {3, 3}], w, 2]] ]
  • 1
  • 2
  • 3
  • 1
  • 2
  • 3

　　正動力學的輸入是關節力矩，下面咱們爲關節力矩設置不一樣的值，看看機械臂的表現： 
　　●　若是令關節力矩等於零（即 \[Tau][k] = 0.0），機械臂將在惟一的外力——重力做用下運動，以下圖所示。 


　　只受重力的狀況下，機械臂的總能量應該守恆。咱們能夠動手計算一下機械臂的能量（由動能和重力勢能組成，代碼以下）。將仿真過程當中每一時刻的能量計算出來並保存在一個列表中，再將其畫出圖像以下圖所示。可見能量幾乎不變（輕微的變化是由積分偏差致使的，若是步長取的再小一些，就更接近一條直線），這說明機械臂的總能量保持恆定，也間接證明了正動力學代碼的正確性。這個簡單的事實讓人很吃驚——雖然機械臂的運動看起來那麼複雜，可是它的能量一直是不變的。從力和運動的角度看，機械臂的行爲變化莫測，但是一旦切換到能量的角度，它竟然那麼簡潔。機械臂的運動方程和能量有什麼關係呢？聰明的拉格朗日就是從能量的角度去推導動力學方程的。

 

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

 


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

 

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

　　其中，qfun 和dqfun 是 4.2 節中定義的插值函數，這裏用做機械臂要跟蹤的（關節空間中的）參考軌跡。跟蹤一個圓的效果以下圖所示。


　　●　機械臂實際工做時可能會受到干擾。PD控制率對於擾動的效果怎麼樣？咱們施加一個擾動信號試試。這裏選擇一個尖峯擾動，模擬的實際狀況是機械臂忽然被踹了一腳。擾動函數的定義代碼以下，你能夠本身修改擾動的大小和尖峯出現的時間。

 

  1.  
    Manipulate[disturb[t_] := peak Exp[-fat(t - tp)^2];
  2.  
    Plot[disturb[t], {t, 0, 10}, PlotRange -> All]
  3.  
    , {peak, 50, 300, 0.1}, {fat, 0.5, 40, 0.1}, {tp, 0, 10, 0.1}, TrackedSymbols :> True]
  • 1
  • 2
  • 3
  • 1
  • 2
  • 3

 


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

 

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

　　機械臂的響應以下圖所示，可見機械臂仍是能回覆零點姿式的。一開始機械臂有一個輕微的顫動，俗稱「點頭」。這是因爲剛一開始機械臂的角度和角速度都爲零，因此關節力矩也爲零，致使機械臂缺乏可以平衡重力的驅動力。在第5秒左右擾動出現，致使機械臂偏離了零點姿式，但在反饋控制做用下很快又回到了零點姿式。 

 

7.3 逆動力學仿真

　　輸入力矩後，藉助正動力學能獲得關節角加速度，積分後能夠獲得角速度和角度。就像運動學和逆運動學的關係同樣，逆動力學與正動力學恰好相反，它的用處是：若是告訴你機械臂的運動（也就是關節角度、角速度、角加速度），計算所需的關節力矩。

  1.  
    endTime= 10.0; steps=1000; dt=endTime/steps;(*最開始一樣是參數初始化*)
  2.  
    gravityAcc= 9.80665; (*重力加速度*)
  3.  
    Do[mass[i]=1.0; gForce[i]=gravityAcc*mass[i]*{0,0,-1,0,0,0},{i,n+1}];
  4.  
    Do[\[ScriptCapitalM][i]=IdentityMatrix[6],{i,n+1}];
  5.  
    Do[V[i]=dV[i]=ConstantArray[0,6],{i,n+1}];
  6.  
    Fin[n+ 2]=ConstantArray[0,6]; (*第n+1個連桿受到內力，爲了迭代過程寫起來方便定義的*)
  7.  
    g[L[n+ 1],L[n+2]]=g[I,L[1]]=IdentityMatrix[4];
  8.  
    q=dq=ddq=ConstantArray[ 0,n]; (*初始狀態的關節角度、速度、加速度*)
  9.  
    \[Tau]List=Table[
  10.  
    　 For[i=2,i<=n+1,i++, (*速度前向遞歸，從第二個連桿開始到最後一個連桿*)
  11.  
    　 k=i- 1;
  12.  
    　　g[L[i- 1],L[i]]=TwistExp[\[Xi]r[[k]],q[[k]]].g[L[i-1],L[i],0];
  13.  
    　　g[I,L[i]]=g[I,L[i- 1]].g[L[i-1],L[i]];
  14.  
    　　V[i]=Ad[Iv[g[L[i- 1],L[i]]]].V[i-1]+\[Xi]rb[[k]]*dq[[k]];
  15.  
    　　dV[i]=Ad[Iv[g[L[i- 1],L[i]]]].dV[i-1]+ad[V[i]-\[Xi]rb[[k]]*dq[[k]]].\[Xi]rb[[k]]*dq[[k]]+\[Xi]rb[[k]]*ddq[[k]]];
  16.  
    　 For[i=n+1,i>=2,i--,　(*力後向遞歸，從最後一個連桿開始到第二個連桿*)
  17.  
    　　k=i- 1;
  18.  
    　　Fex[i]=T[Ad[RToH[gToR[g[I,L[i]]]]]].gForce[i]; (*連桿受到的重力*)
  19.  
    　　Fin[i]=\[ScriptCapitalM][i].dV[i]-T[ad[V[i]]].\[ScriptCapitalM][i].V[i]+T[Ad[Iv[g[L[i],L[i+ 1]]]]].Fin[i+1]-Fex[i];
  20.  
    　　\[Tau][k]=\[Xi]rb [[k]].Fin[i]];
  21.  
    Array[\[Tau],n]
  22.  
    , {t, 0, endTime, dt}];//AbsoluteTiming (*監視計算時間*)
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22

　　在重力做用下，機械臂保持「立正姿式」須要多大力矩呢？將初始狀態設爲 0，通過逆動力學計算獲得的答案是{0,-38.1,-38.1,0,-2.06,0}。若是把這個力矩帶入正動力學仿真就能看到機械臂保持不動，這證實咱們的模型基本是正確的。 
　　 
8. 結尾

　　本文咱們以 Mathematica 通用數學軟件爲平臺，針對串聯機械臂的建模、規劃和控制中的基本問題進行了仿真，差很少該說再見了。不過新的篇章即將展開 —— 移動機器人是另外一個有趣的領域，將來咱們將加入移動機器人仿真的功能，支持地面移動機器人的運動控制、環境約束下的運動規劃、移動機械臂、多機器人避障、多機器人編隊運動等，並討論環境建模、導航與定位、非完整約束、最優控制、輪式車輛和履帶車輛的受力、羣集協同運動等問題，敬請期待喲！ 


　　 
引用文獻 

[1]　一種高效的開放式關節型機器人3D仿真環境構建方法，甘亞輝，機器人. 
[2]　Robotics, Vision and Control Fundamental Algorithms in MATLAB, Peter Corke. 
[3]　A Mathematical Introduction to Robotic Manipulation —— A Mathematica Package for Screw Calculus, Richard M. Murray, 435頁. 
[4]　Robotica: A Mathematica Package for Robot Analysis, Mark W. Spong. 
[5]　機器人學導論，John J. Craig，中文第三版. 
[6]　PC-based Control for Robotics in Handling, Production and Assembly, Beckhoff. 
[7]　Robotics - Modelling, Planning and Control, Bruno Siciliano, 582頁. 
[8]　Lie Group Formulation of Articulated Rigid Body Dynamics, Junggon Kim, 2012.

 

補充： Mathematica 的缺點

　　在筆者就讀研究生期間，Matlab 的使用率頗高。每次參加答辯、報告，看着同窗或老師用 Matlab 製做的醜陋不堪的圖表，心中就想把 Matlab 的界面設計師槍斃十分鐘。再加上呆板的函數定義和使用方式、缺少對部分機器人仿真功能的支持，讓筆者不得不尋找其它的替代軟件。但是在網絡發達的今天，我竟然找不到稍微像點樣的介紹機器人仿真的博客以及原理性代碼，要麼過於簡單和低級，要麼則是東拼西湊。因而想把本身的經驗寫出來，並公開代碼（若是你想要，我能夠毫無保留地公開全部代碼）。 
　　就像 Matlab 有不少讓人不爽的地方同樣，Mathematica 用於機器人仿真一樣存在一些缺陷。咱們以前在碰撞檢測部分已經提過，要想達到很快的檢測速度就不得不使用簡單的幾何模型。雖然 Mathematica 的函數也通過了優化，可是隻適用於須要較少計算次數的場合，在屢次處理大量數據時仍是比較慢。Mathematica 自己是用 C 語言寫成的，若是某個函數被大量調用能夠考慮用 C 語言寫成動態連接庫（dll），而後在 Mathematica 中調用，這就像 Matlab 中的 MEX 文件。 
　　Mathematica 支持設置中斷，但使用起來至關不友好，它提供了一個專門用來開發調試的軟件——Workbench，惋惜也很差用。 在調試時，我不得不使用 Print 函數打出中間計算結果來檢查中間運算結果。Mathematica 缺乏像 Matlab 同樣的變量監控窗口能夠實時看到變量的值，這在調試時顯得很不方便。

https://blog.csdn.net/weixin_39901416/article/details/79903999

相關文章
相關標籤/搜索
每日一句
    每一个你不满意的现在，都有一个你没有努力的曾经。
本站公眾號
   歡迎關注本站公眾號,獲取更多信息
聯繫我們 最近搜索 最新文章 沪ICP备13005482号-10 MyBatis教程 SQL 教程 MySQL教程 Java 教程 Thymeleaf 教程 Hibernate教程 Spring教程 Redis教程