器人建模：DH參數(shù)法原理、實(shí)戰(zhàn)與避坑指南)
1. 從零開始為什么串聯(lián)機(jī)器人建模繞不開DH參數(shù)法如果你剛接觸機(jī)器人學(xué)或者正在用Matlab做機(jī)器人相關(guān)的畢設(shè)、項(xiàng)目大概率會(huì)聽到一個(gè)詞DH參數(shù)法。這幾乎是所有串聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)建模的“標(biāo)準(zhǔn)答案”就像學(xué)微積分繞不開牛頓-萊布尼茨公式一樣。但很多人只是機(jī)械地記住了DH表的四個(gè)參數(shù)連桿長度a、連桿轉(zhuǎn)角α、連桿偏距d、關(guān)節(jié)角θ然后對(duì)著公式硬套結(jié)果要么是模型動(dòng)不起來要么是正運(yùn)動(dòng)學(xué)算出來的位姿和實(shí)際對(duì)不上最后只能對(duì)著Matlab里一堆報(bào)錯(cuò)的代碼干瞪眼。我剛開始做機(jī)器人仿真的時(shí)候也踩過這個(gè)坑。當(dāng)時(shí)導(dǎo)師扔給我一個(gè)六軸機(jī)械臂的圖紙讓我在Matlab里把它的運(yùn)動(dòng)模型建出來。我心想這不就是套公式嘛簡(jiǎn)單。結(jié)果光是確定每個(gè)關(guān)節(jié)的坐標(biāo)系原點(diǎn)就折騰了我整整兩天。更別提后來發(fā)現(xiàn)不同教材、不同機(jī)器人廠商對(duì)DH參數(shù)的定義還有“標(biāo)準(zhǔn)DH”和“改進(jìn)DH”之分直接把我給整懵了。后來在項(xiàng)目里反復(fù)折騰才算是把這里面的門道摸清楚了。今天我就以一個(gè)過來人的身份跟你聊聊怎么在Matlab里用DH參數(shù)法給串聯(lián)機(jī)器人建模并且一次就把它搞對(duì)。簡(jiǎn)單來說DH參數(shù)法是一套“約定俗成”的規(guī)則它用四個(gè)參數(shù)就能清晰地描述出機(jī)器人相鄰兩個(gè)連桿之間的空間關(guān)系。它的核心價(jià)值在于標(biāo)準(zhǔn)化和可計(jì)算性。通過這套規(guī)則我們可以把復(fù)雜的、三維空間中的機(jī)械結(jié)構(gòu)轉(zhuǎn)化成一個(gè)整齊的表格DH表和一系列齊次變換矩陣。在Matlab里我們就能利用這些矩陣輕松地計(jì)算機(jī)械臂末端執(zhí)行器比如夾爪、焊槍相對(duì)于基座的位置和姿態(tài)這就是正運(yùn)動(dòng)學(xué)。反過來如果知道了末端想要到達(dá)的位置和姿態(tài)去反推每個(gè)關(guān)節(jié)需要轉(zhuǎn)動(dòng)的角度這就是逆運(yùn)動(dòng)學(xué)。而這一切的起點(diǎn)就是建立一個(gè)正確無誤的DH模型。所以這篇文章適合所有需要用Matlab進(jìn)行機(jī)器人仿真、算法開發(fā)或者僅僅是完成課程作業(yè)的朋友。無論你是想驗(yàn)證自己設(shè)計(jì)的機(jī)械臂結(jié)構(gòu)是否合理還是為后續(xù)的軌跡規(guī)劃、控制算法提供一個(gè)精確的“數(shù)字孿生”模型一個(gè)正確的DH模型都是你必須邁過去的第一道坎。接下來我會(huì)手把手帶你走完從理解原理、建立參數(shù)表到在Matlab中實(shí)現(xiàn)和驗(yàn)證的完整流程并分享幾個(gè)我踩過的大坑和避坑技巧。2. 庖丁解牛徹底搞懂DH參數(shù)法的“潛規(guī)則”與兩種慣例在動(dòng)手寫代碼之前我們必須把理論基礎(chǔ)打牢。DH參數(shù)法的核心思想是在機(jī)器人的每個(gè)連桿上“安裝”一個(gè)坐標(biāo)系然后用四個(gè)參數(shù)來描述相鄰兩個(gè)坐標(biāo)系之間的變換關(guān)系。聽起來簡(jiǎn)單但魔鬼藏在細(xì)節(jié)里。最大的 confusion 往往來自于坐標(biāo)系到底應(yīng)該怎么裝參數(shù)到底按什么順序來2.1 坐標(biāo)系建立的“黃金法則”給串聯(lián)機(jī)器人的每個(gè)連桿建立坐標(biāo)系遵循一套固定的步驟。這里以最常用的標(biāo)準(zhǔn)DH參數(shù)法為例確定關(guān)節(jié)軸Z軸第 i 個(gè)關(guān)節(jié)的旋轉(zhuǎn)或移動(dòng)軸線就是連桿 i 的 Z 軸記為 Z_i。對(duì)于旋轉(zhuǎn)關(guān)節(jié)Z軸就是關(guān)節(jié)的旋轉(zhuǎn)軸對(duì)于移動(dòng)關(guān)節(jié) prismatic jointZ軸就是關(guān)節(jié)的移動(dòng)方向。確定坐標(biāo)系原點(diǎn)O_i原點(diǎn) O_i 是 Z_i 軸和 Z_{i-1} 軸的公垂線common perpendicular與 Z_i 軸的交點(diǎn)。如果兩軸平行則公垂線有無數(shù)條通常選擇使連桿偏距 d_i 為0的那一條。如果兩軸相交則原點(diǎn)就是交點(diǎn)。確定X軸X_iX_i 軸的方向沿 Z_{i-1} 軸和 Z_i 軸的公垂線由 Z_{i-1} 軸指向 Z_i 軸。確定Y軸Y_i根據(jù)右手定則由 Z_i 軸和 X_i 軸確定 Y_i 軸Y_i Z_i × X_i。這套規(guī)則必須嚴(yán)格執(zhí)行不能憑感覺亂放。一個(gè)常見的錯(cuò)誤是把坐標(biāo)系原點(diǎn)直接放在關(guān)節(jié)的物理中心上這對(duì)于某些特定結(jié)構(gòu)的關(guān)節(jié)比如兩個(gè)旋轉(zhuǎn)關(guān)節(jié)軸線相交于一點(diǎn)是成立的但對(duì)于大多數(shù)情況尤其是軸線異面的情況就會(huì)導(dǎo)致后續(xù)的變換矩陣出錯(cuò)。2.2 四個(gè)參數(shù)的物理意義與“標(biāo)準(zhǔn)DH”流程坐標(biāo)系建好后從坐標(biāo)系 {i-1} 變換到坐標(biāo)系 {i}可以通過以下四個(gè)有序的變換來實(shí)現(xiàn)對(duì)應(yīng)四個(gè)DH參數(shù)繞 Z_{i-1} 軸旋轉(zhuǎn) θ_i 角將 X_{i-1} 軸旋轉(zhuǎn)到與 X_i 軸平行的方向。θ_i 是關(guān)節(jié)變量對(duì)于旋轉(zhuǎn)關(guān)節(jié)它是變化的對(duì)于移動(dòng)關(guān)節(jié)它是固定的。沿 Z_{i-1} 軸平移 d_i將原點(diǎn) O_{i-1} 移動(dòng)到 O_i 在 Z_{i-1} 軸上的投影點(diǎn)。d_i 是連桿偏距。沿 X_i 軸平移 a_i將原點(diǎn)移動(dòng)到 O_i 點(diǎn)。a_i 是連桿長度即從 Z_{i-1} 軸到 Z_i 軸沿 X_i 軸方向的距離。繞 X_i 軸旋轉(zhuǎn) α_i 角將 Z_{i-1} 軸旋轉(zhuǎn)到與 Z_i 軸平行的方向。α_i 是連桿轉(zhuǎn)角即從 Z_{i-1} 軸到 Z_i 軸繞 X_i 軸旋轉(zhuǎn)的角度。記住這個(gè)順序旋轉(zhuǎn)θ → 平移d → 平移a → 旋轉(zhuǎn)α。每一步變換都對(duì)應(yīng)一個(gè)齊次變換矩陣連乘起來就得到了從 {i-1} 到 {i} 的總變換矩陣A_i。A_i Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)把這個(gè)矩陣寫開就是教科書上那個(gè)經(jīng)典的DH變換矩陣。在Matlab里我們不需要手動(dòng)輸入這個(gè)矩陣的每一個(gè)元素后面會(huì)用到現(xiàn)成的函數(shù)來生成它。2.3 “標(biāo)準(zhǔn)DH” vs “改進(jìn)DH”你必須做的選擇題這是新手最容易栽跟頭的地方。除了上面講的標(biāo)準(zhǔn)DHStandard DH還有一種廣泛使用的改進(jìn)DHModified DH也叫Craig版本。它們的主要區(qū)別在于坐標(biāo)系附著在連桿的位置不同標(biāo)準(zhǔn)DH本文主要采用坐標(biāo)系 {i} 固連在連桿 i 的遠(yuǎn)端靠近關(guān)節(jié) i1 的一端。參數(shù)a_i和α_i描述了連桿 i 本身的幾何形狀。改進(jìn)DH坐標(biāo)系 {i} 固連在連桿 i 的近端靠近關(guān)節(jié) i 的一端。參數(shù)a_{i-1}和α_{i-1}描述了連桿 i-1 的幾何形狀。為什么會(huì)有兩種改進(jìn)DH在描述樹狀結(jié)構(gòu)或閉鏈機(jī)器人時(shí)更清晰且其變換矩陣連乘順序從基座到末端更直觀。而標(biāo)準(zhǔn)DH在傳統(tǒng)工業(yè)機(jī)器人教材中更常見。怎么選注意絕對(duì)不要混用一旦你為某個(gè)機(jī)器人選擇了其中一種慣例所有的參數(shù)定義、坐標(biāo)系建立、變換矩陣計(jì)算都必須遵循這一種。通常你參考的機(jī)器人數(shù)據(jù)手冊(cè)或論文會(huì)指明它用的是哪種。如果沒指明你可以根據(jù)變換矩陣的連乘順序來判斷或者選擇一種并始終堅(jiān)持。在Matlab的Robotics System Toolbox中兩種都支持但創(chuàng)建機(jī)器人對(duì)象時(shí)需要明確指定。我個(gè)人的經(jīng)驗(yàn)是對(duì)于大多數(shù)教科書上的經(jīng)典六軸串聯(lián)機(jī)械臂如PUMA560標(biāo)準(zhǔn)DH資料更多更容易上手。在本文的后續(xù)Matlab實(shí)操中我們將使用標(biāo)準(zhǔn)DH。3. 實(shí)戰(zhàn)演練以經(jīng)典六軸機(jī)械臂為例構(gòu)建DH參數(shù)表理論說再多不如動(dòng)手算一遍。我們以一個(gè)簡(jiǎn)化版的經(jīng)典六軸旋轉(zhuǎn)關(guān)節(jié)機(jī)械臂為例來推導(dǎo)它的DH參數(shù)表。假設(shè)它的結(jié)構(gòu)類似于PUMA560但尺寸簡(jiǎn)化以便理解。我們的目標(biāo)是得到下面這張表連桿 iθ_i (變量)d_ia_iα_i1θ1L10-90°2θ20L203θ30L3-90°4θ4L4090°5θ500-90°6θ6L600注L1, L2, L3, L4, L6為具體的長度值例如可以設(shè)為0.5, 0.4, 0.3, 0.2, 0.1 (米)。3.1 步驟一分析機(jī)械臂結(jié)構(gòu)草圖首先你需要一張機(jī)械臂的示意圖或三維模型。明確以下幾點(diǎn)所有關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)Revolute Joint。關(guān)節(jié)1基座關(guān)節(jié)繞垂直軸Z0旋轉(zhuǎn)。關(guān)節(jié)2和關(guān)節(jié)3的軸線平行。關(guān)節(jié)4、5、6的軸線相交于一點(diǎn)腕部中心點(diǎn)。3.2 步驟二按標(biāo)準(zhǔn)DH規(guī)則建立坐標(biāo)系我們從基座{0}開始一步步建立到末端工具坐標(biāo)系{6}。坐標(biāo)系 {0}固定在基座上。Z0沿關(guān)節(jié)1的旋轉(zhuǎn)軸垂直向上。通常將原點(diǎn)O0放在基座中心X0方向任意一般向前根據(jù)右手定則確定Y0。坐標(biāo)系 {1}Z1軸沿關(guān)節(jié)2的旋轉(zhuǎn)軸假設(shè)水平向前。原點(diǎn)O1是Z0和Z1公垂線與Z1的交點(diǎn)。因?yàn)閆0垂直Z1水平公垂線就是它們之間最短的垂直線段。這個(gè)線段的長度就是d1 L1基座高度。X1軸沿Z0到Z1的公垂線方向由Z0指向Z1水平向前。此時(shí)從{0}到{1}需要繞Z0轉(zhuǎn)θ1再沿Z0平移d1L1就使Z0與Z1共線但原點(diǎn)還差一截。接著沿X1平移a1但此時(shí)Z0和Z1已經(jīng)相交了嗎不它們異面。實(shí)際上平移a1是沿著X1軸移動(dòng)但在這個(gè)例子中經(jīng)過旋轉(zhuǎn)和平移d后原點(diǎn)O0已經(jīng)移動(dòng)到O1在Z0上的投影點(diǎn)而O1在X1軸上。因?yàn)閆0和Z1是垂直異面公垂線就是X1本身所以從投影點(diǎn)到O1的距離就是a1 0。最后需要繞X1軸旋轉(zhuǎn)使Z0與Z1同向。Z0原來是垂直的要變成水平的Z1需要繞X1軸旋轉(zhuǎn)α1 -90°(順時(shí)針90度)。坐標(biāo)系 {2}Z2軸沿關(guān)節(jié)3的旋轉(zhuǎn)軸與Z1平行水平向前。原點(diǎn)O2是Z1和Z2公垂線與Z2的交點(diǎn)。因?yàn)閆1和Z2平行公垂線是它們之間最短的連線。這段距離就是a2 L2大臂長度。X2軸沿Z1到Z2的公垂線方向。因?yàn)閮奢S平行方向可以選為從Z1指向Z2水平向前與X1同向或反向這里需要根據(jù)實(shí)際幾何判斷通常向前。從{1}到{2}繞Z1轉(zhuǎn)θ2沿Z1平移d20因?yàn)樵c(diǎn)O1已經(jīng)在公垂線與Z1的交點(diǎn)沿X2平移a2L2繞X2旋轉(zhuǎn)α20因?yàn)閆1和Z2平行。坐標(biāo)系 {3}Z3軸沿關(guān)節(jié)4的旋轉(zhuǎn)軸假設(shè)水平與Z2垂直。原點(diǎn)O3是Z2和Z3公垂線與Z3的交點(diǎn)。這段公垂線長度是a3 L3小臂長度。X3軸沿Z2到Z3的公垂線方向。從{2}到{3}繞Z2轉(zhuǎn)θ3沿Z2平移d30沿X3平移a3L3繞X3旋轉(zhuǎn)α3-90°使水平的Z2變?yōu)樗降腪3但方向垂直了所以需要轉(zhuǎn)90度具體符號(hào)根據(jù)右手定則判斷。坐標(biāo)系 {4}, {5}, {6}腕部三個(gè)關(guān)節(jié)軸線相交于一點(diǎn)腕部中心。這意味著從坐標(biāo)系{3}的原點(diǎn)O3到腕部中心點(diǎn)需要沿Z3平移d4 L4。然后因?yàn)楹罄m(xù)關(guān)節(jié)軸線相交a4, a5, a6 都為0。α4, α5, α6 則描述腕部軸線之間的夾角通常為90°或-90°。具體參數(shù)如上面表格所示。這個(gè)過程需要極強(qiáng)的空間想象力。一個(gè)極其重要的技巧是畫圖在紙上或繪圖軟件里把每個(gè)坐標(biāo)系的三根軸清清楚楚地畫出來標(biāo)上XYZ和原點(diǎn)O。這是確保參數(shù)正確的唯一可靠方法。3.3 步驟三填寫DH參數(shù)表并復(fù)核將上面分析得到的參數(shù)整理成表。填寫時(shí)務(wù)必檢查對(duì)于旋轉(zhuǎn)關(guān)節(jié)θ是變量用θ1, θ2...表示d是常數(shù)。對(duì)于移動(dòng)關(guān)節(jié)d是變量θ是常數(shù)。a和α總是常數(shù)描述連桿的固定幾何形狀。符號(hào)旋轉(zhuǎn)角α和θ遵循右手定則拇指指向旋轉(zhuǎn)軸正方向四指彎曲方向?yàn)檎?. 在Matlab中“復(fù)活”你的機(jī)器人模型參數(shù)表有了我們就可以在Matlab里把它變成一個(gè)可以驅(qū)動(dòng)、可以計(jì)算的模型了。這里主要依賴Robotics System Toolbox它是Matlab中機(jī)器人學(xué)的“瑞士軍刀”。如果你沒有這個(gè)工具箱需要額外安裝。4.1 環(huán)境準(zhǔn)備與工具箱核心函數(shù)簡(jiǎn)介首先在Matlab命令窗口輸入ver查看是否已安裝‘Robotics System Toolbox’。如果沒有需要通過Matlab的“附加功能”管理器安裝。這個(gè)工具箱提供了兩種主要的機(jī)器人對(duì)象表示方式rigidBodyTree更現(xiàn)代、更靈活的表示支持標(biāo)準(zhǔn)DH和改進(jìn)DH是官方推薦的方式。SerialLink(通過Robotics Toolbox Peter Corke版本)一個(gè)非常流行且強(qiáng)大的第三方工具箱語法更簡(jiǎn)潔。但Matlab官方已不再維護(hù)其內(nèi)置的類似功能更推薦使用rigidBodyTree。不過Peter Corke的Toolbox仍然被廣泛使用。為了覆蓋更廣我先介紹官方的rigidBodyTree方法。4.2 使用 rigidBodyTree 構(gòu)建機(jī)器人對(duì)象我們按照前面得到的DH表來創(chuàng)建機(jī)器人。% 清除工作區(qū)關(guān)閉所有圖形 clear; close all; clc; % 1. 創(chuàng)建空的剛性體樹對(duì)象 robot rigidBodyTree(DataFormat, row); % row 表示使用行向量表示變換更常見 % 2. 定義連桿的幾何參數(shù)單位米 L1 0.5; L2 0.4; L3 0.3; L4 0.2; L6 0.1; % 3. 創(chuàng)建第一個(gè)連桿Base to Joint1 % 注意基座‘world’是默認(rèn)存在的。第一個(gè)物體是連接在基座上的。 body1 rigidBody(body1); joint1 rigidBodyJoint(joint1, revolute); % 旋轉(zhuǎn)關(guān)節(jié) % 設(shè)置關(guān)節(jié)的DH參數(shù)使用標(biāo)準(zhǔn)DH慣例 % setFixedTransform(joint, dhparams, dh) 其中dhparams [a alpha d theta] % 注意這里的theta對(duì)應(yīng)的是關(guān)節(jié)變量在創(chuàng)建時(shí)先設(shè)為0后續(xù)通過關(guān)節(jié)位置控制。 dhparams1 [0, -pi/2, L1, 0]; % [a1, alpha1, d1, theta1] setFixedTransform(joint1, dhparams1, dh); body1.Joint joint1; addBody(robot, body1, base); % 將body1添加到基座上 % 4. 創(chuàng)建第二個(gè)連桿 (Body1 to Body2) body2 rigidBody(body2); joint2 rigidBodyJoint(joint2, revolute); dhparams2 [L2, 0, 0, 0]; % [a2, alpha2, d2, theta2] setFixedTransform(joint2, dhparams2, dh); body2.Joint joint2; addBody(robot, body2, body1); % 將body2連接到body1上 % 5. 創(chuàng)建第三個(gè)連桿 (Body2 to Body3) body3 rigidBody(body3); joint3 rigidBodyJoint(joint3, revolute); dhparams3 [L3, -pi/2, 0, 0]; % [a3, alpha3, d3, theta3] setFixedTransform(joint3, dhparams3, dh); body3.Joint joint3; addBody(robot, body3, body2); % 6. 創(chuàng)建第四、五、六個(gè)連桿腕部 body4 rigidBody(body4); joint4 rigidBodyJoint(joint4, revolute); dhparams4 [0, pi/2, L4, 0]; % [a4, alpha4, d4, theta4] setFixedTransform(joint4, dhparams4, dh); body4.Joint joint4; addBody(robot, body4, body3); body5 rigidBody(body5); joint5 rigidBodyJoint(joint5, revolute); dhparams5 [0, -pi/2, 0, 0]; % [a5, alpha5, d5, theta5] setFixedTransform(joint5, dhparams5, dh); body5.Joint joint5; addBody(robot, body5, body4); % 末端執(zhí)行器通常作為一個(gè)沒有關(guān)節(jié)的固定連桿 body6 rigidBody(tool); joint6 rigidBodyJoint(fix6, fixed); % 固定關(guān)節(jié)將工具固定到body5 % 從body5的坐標(biāo)系到工具坐標(biāo)系還有一個(gè)偏移 d6 dhparams6 [0, 0, L6, 0]; % [a6, alpha6, d6, theta6] setFixedTransform(joint6, dhparams6, dh); body6.Joint joint6; addBody(robot, body6, body5); % 7. 顯示機(jī)器人詳細(xì)信息 showdetails(robot)運(yùn)行showdetails(robot)你會(huì)看到機(jī)器人每個(gè)關(guān)節(jié)的類型、父物體、子物體以及變換關(guān)系這是驗(yàn)證模型結(jié)構(gòu)是否正確的重要一步。4.3 可視化與正運(yùn)動(dòng)學(xué)驗(yàn)證模型建好了我們讓它動(dòng)起來看看。% 8. 可視化機(jī)器人初始狀態(tài)所有關(guān)節(jié)角為0 figure(Name, Robot Visualization, Position, [100 100 800 600]); show(robot); title(六軸機(jī)械臂模型 (初始零位)); xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); view(135, 30); % 設(shè)置一個(gè)較好的觀察視角 grid on; axis equal; % 9. 正運(yùn)動(dòng)學(xué)計(jì)算示例給定一組關(guān)節(jié)角度計(jì)算末端位姿 config [pi/6, -pi/4, pi/3, -pi/6, pi/2, 0]; % 六個(gè)關(guān)節(jié)的角度 [θ1, θ2, θ3, θ4, θ5, θ6] % 注意config的順序必須和添加body的順序一致即對(duì)應(yīng)關(guān)節(jié)1到關(guān)節(jié)6。 % 設(shè)置機(jī)器人到這個(gè)構(gòu)型 robot.DataFormat row; % 確保數(shù)據(jù)格式 config deg2rad([30, -45, 60, -30, 90, 0]); % 也可以用角度制但記得轉(zhuǎn)換 % 獲取正運(yùn)動(dòng)學(xué)變換矩陣 tform getTransform(robot, config, tool); % ‘tool’是我們定義的末端執(zhí)行器名稱 disp(末端執(zhí)行器相對(duì)于基座的變換矩陣 T_0_tool:); disp(tform); % 從變換矩陣中提取位置和歐拉角 position tform(1:3, 4); % 位置向量 orientation tform(1:3, 1:3); % 旋轉(zhuǎn)矩陣 % 將旋轉(zhuǎn)矩陣轉(zhuǎn)換為ZYX歐拉角更直觀 eulZYX rotm2eul(orientation, ZYX); disp([末端位置: [, num2str(position), ] m]); disp([末端姿態(tài)(ZYX歐拉角): [, num2str(rad2deg(eulZYX)), ] deg]); % 10. 可視化這個(gè)特定構(gòu)型 figure; show(robot, config); title([機(jī)器人構(gòu)型: θ [, num2str(rad2deg(config)), ] deg]); view(135, 30); grid on; axis equal;如果一切正確你將看到機(jī)械臂從一個(gè)“折疊”的初始狀態(tài)運(yùn)動(dòng)到你指定關(guān)節(jié)角度的姿態(tài)。getTransform函數(shù)計(jì)算出的變換矩陣T_0_tool就是通過我們輸入的DH參數(shù)和關(guān)節(jié)角度一步步從基座坐標(biāo)系{0}變換到工具坐標(biāo)系{tool}的結(jié)果。這就是正運(yùn)動(dòng)學(xué)的核心。5. 避坑指南與進(jìn)階思考從“能用”到“好用”把模型跑通只是第一步。在實(shí)際項(xiàng)目中你會(huì)遇到更多問題。下面是我總結(jié)的幾個(gè)關(guān)鍵點(diǎn)和進(jìn)階方向。5.1 常見錯(cuò)誤排查清單如果你的模型報(bào)錯(cuò)或者運(yùn)動(dòng)姿態(tài)很奇怪請(qǐng)按以下順序檢查DH參數(shù)表錯(cuò)誤這是最最常見的問題。反復(fù)對(duì)照第二節(jié)的規(guī)則檢查每個(gè)坐標(biāo)系的Z軸方向、原點(diǎn)位置、X軸方向。特別是α角的符號(hào)很容易搞反。一個(gè)快速驗(yàn)證的方法是令所有關(guān)節(jié)角為0看看機(jī)器人的初始姿態(tài)是否符合你的預(yù)期比如是否站立、手臂是否向前伸展。如果初始姿態(tài)就歪了肯定是DH參數(shù)錯(cuò)了。Matlab代碼中的參數(shù)順序setFixedTransform(joint, dhparams, dh)中的dhparams向量順序是[a, alpha, d, theta]。千萬注意這個(gè)順序和你教科書上常見的[theta, d, a, alpha]或[d, theta, a, alpha]可能不同一定要看Matlab官方文檔。本文使用的順序是[a, alpha, d, theta]。“標(biāo)準(zhǔn)DH”與“改進(jìn)DH”混淆在setFixedTransform中第三個(gè)參數(shù)指定了慣例。我們用的是dh代表標(biāo)準(zhǔn)DH。如果你參考的模型是改進(jìn)DH則需要使用mdh并且輸入的參數(shù)意義完全不同。再次強(qiáng)調(diào)不要混用關(guān)節(jié)類型設(shè)置錯(cuò)誤旋轉(zhuǎn)關(guān)節(jié)是revolute移動(dòng)關(guān)節(jié)是prismatic。如果設(shè)錯(cuò)后續(xù)控制會(huì)出問題。單位不一致DH參數(shù)中的長度單位a, d要一致通常用米(m)。角度單位在輸入函數(shù)時(shí)Matlab默認(rèn)使用弧度(rad)。如果你習(xí)慣用度(deg)記得用deg2rad()函數(shù)轉(zhuǎn)換。末端執(zhí)行器定義我們通常關(guān)心工具末端點(diǎn)TCP的位姿。在上面的代碼中我們通過一個(gè)固定的連桿body6和固定關(guān)節(jié)‘fix6’來定義了一個(gè)工具坐標(biāo)系它距離最后一個(gè)運(yùn)動(dòng)關(guān)節(jié)body5的關(guān)節(jié)有一個(gè)d6L6的偏移。確保這個(gè)偏移是你想要的TCP位置。5.2 模型驗(yàn)證與已知模型或?qū)嵨飳?duì)比如何確信你的模型是正確的有幾個(gè)方法與經(jīng)典模型對(duì)比比如PUMA560機(jī)器人有公開的、經(jīng)過驗(yàn)證的DH參數(shù)。你可以用你的代碼實(shí)現(xiàn)PUMA560然后讓機(jī)器人移動(dòng)到某個(gè)特定姿態(tài)如所有關(guān)節(jié)為0看末端位置是否與文獻(xiàn)中一致。正逆運(yùn)動(dòng)學(xué)互驗(yàn)這是最有力的驗(yàn)證。編寫逆運(yùn)動(dòng)學(xué)算法或使用工具箱的inverseKinematics函數(shù)對(duì)正運(yùn)動(dòng)學(xué)計(jì)算出的一個(gè)末端位姿進(jìn)行逆解得到的關(guān)節(jié)角再代回正運(yùn)動(dòng)學(xué)看是否能回到原來的位姿允許微小誤差。軌跡驗(yàn)證讓末端走一個(gè)簡(jiǎn)單的已知軌跡如直線、圓觀察關(guān)節(jié)角變化是否平滑、合理。5.3 超越基礎(chǔ)將模型用于仿真與控制一個(gè)正確的DH模型是你的“數(shù)字孿生”機(jī)器人它是后續(xù)所有高級(jí)應(yīng)用的基礎(chǔ)逆運(yùn)動(dòng)學(xué)求解使用inverseKinematics對(duì)象。你可以指定末端位姿的約束位置、方向并設(shè)置權(quán)重讓求解器為你計(jì)算所需的關(guān)節(jié)角度。這對(duì)于軌跡規(guī)劃至關(guān)重要。ik inverseKinematics(RigidBodyTree, robot); weights [0.1, 0.1, 0.1, 1, 1, 1]; % 位置權(quán)重低姿態(tài)權(quán)重高 initialGuess config; % 初始猜測(cè)關(guān)節(jié)角 [qSol, solInfo] ik(tool, tform, weights, initialGuess);軌跡規(guī)劃在關(guān)節(jié)空間或笛卡爾空間生成一系列路徑點(diǎn)。例如使用trapveltraj或polynomialtraj生成平滑的關(guān)節(jié)角度軌跡然后用show函數(shù)以動(dòng)畫形式播放出來檢查是否有奇異點(diǎn)或碰撞。與Simulink集成rigidBodyTree對(duì)象可以直接導(dǎo)入Simulink的Simscape Multibody環(huán)境進(jìn)行物理仿真或者與控制系統(tǒng)模塊結(jié)合實(shí)現(xiàn)更真實(shí)的帶動(dòng)力學(xué)和控制的仿真。導(dǎo)出與共享你可以將建好的rigidBodyTree機(jī)器人模型保存為.mat文件或URDFUnified Robot Description Format文件方便在其他項(xiàng)目或ROS機(jī)器人操作系統(tǒng)中使用。5.4 一個(gè)關(guān)鍵技巧使用可視化工具輔助建模對(duì)于復(fù)雜的機(jī)器人純靠腦子想坐標(biāo)系非常困難。我強(qiáng)烈推薦一個(gè)流程在SolidWorks、Fusion 360或OnShape等CAD軟件中畫出機(jī)器人的簡(jiǎn)化模型只需要關(guān)節(jié)軸線和連桿輪廓。在CAD環(huán)境中按照標(biāo)準(zhǔn)DH規(guī)則在每個(gè)連桿上創(chuàng)建坐標(biāo)系。很多CAD軟件可以顯示坐標(biāo)系的變換矩陣。直接從CAD環(huán)境中讀取相鄰坐標(biāo)系之間的變換關(guān)系然后反推出DH參數(shù)。這比正向推導(dǎo)更不容易出錯(cuò)。 具體方法是已知從坐標(biāo)系{i-1}到{i}的變換矩陣T可以通過公式反解出(θ_i, d_i, a_i, α_i)。Matlab也有函數(shù)可以幫助完成這個(gè)計(jì)算。建立機(jī)器人運(yùn)動(dòng)學(xué)模型是機(jī)器人編程的基石這個(gè)過程充滿了對(duì)空間幾何的理解和嚴(yán)謹(jǐn)?shù)倪壿嬐茖?dǎo)。第一次做可能會(huì)覺得繁瑣但一旦你成功地把一個(gè)物理機(jī)器人用一組參數(shù)和矩陣在電腦里“復(fù)活”過來并且能精確控制它運(yùn)動(dòng)時(shí)那種成就感是無與倫比的。希望這篇詳細(xì)的指南能幫你避開我當(dāng)年踩過的那些坑順利搭建起屬于你自己的機(jī)器人仿真世界。記住多畫圖多驗(yàn)證從簡(jiǎn)單的二連桿、三連桿開始練習(xí)手感自然就來了。 Matlab機(jī)器人建模：DH參數(shù)法原理、實(shí)戰(zhàn)與避坑指南
1. 從零開始為什么串聯(lián)機(jī)器人建模繞不開DH參數(shù)法如果你剛接觸機(jī)器人學(xué)或者正在用Matlab做機(jī)器人相關(guān)的畢設(shè)、項(xiàng)目大概率會(huì)聽到一個(gè)詞DH參數(shù)法。這幾乎是所有串聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)建模的“標(biāo)準(zhǔn)答案”就像學(xué)微積分繞不開牛頓-萊布尼茨公式一樣。但很多人只是機(jī)械地記住了DH表的四個(gè)參數(shù)連桿長度a、連桿轉(zhuǎn)角α、連桿偏距d、關(guān)節(jié)角θ然后對(duì)著公式硬套結(jié)果要么是模型動(dòng)不起來要么是正運(yùn)動(dòng)學(xué)算出來的位姿和實(shí)際對(duì)不上最后只能對(duì)著Matlab里一堆報(bào)錯(cuò)的代碼干瞪眼。我剛開始做機(jī)器人仿真的時(shí)候也踩過這個(gè)坑。當(dāng)時(shí)導(dǎo)師扔給我一個(gè)六軸機(jī)械臂的圖紙讓我在Matlab里把它的運(yùn)動(dòng)模型建出來。我心想這不就是套公式嘛簡(jiǎn)單。結(jié)果光是確定每個(gè)關(guān)節(jié)的坐標(biāo)系原點(diǎn)就折騰了我整整兩天。更別提后來發(fā)現(xiàn)不同教材、不同機(jī)器人廠商對(duì)DH參數(shù)的定義還有“標(biāo)準(zhǔn)DH”和“改進(jìn)DH”之分直接把我給整懵了。后來在項(xiàng)目里反復(fù)折騰才算是把這里面的門道摸清楚了。今天我就以一個(gè)過來人的身份跟你聊聊怎么在Matlab里用DH參數(shù)法給串聯(lián)機(jī)器人建模并且一次就把它搞對(duì)。簡(jiǎn)單來說DH參數(shù)法是一套“約定俗成”的規(guī)則它用四個(gè)參數(shù)就能清晰地描述出機(jī)器人相鄰兩個(gè)連桿之間的空間關(guān)系。它的核心價(jià)值在于標(biāo)準(zhǔn)化和可計(jì)算性。通過這套規(guī)則我們可以把復(fù)雜的、三維空間中的機(jī)械結(jié)構(gòu)轉(zhuǎn)化成一個(gè)整齊的表格DH表和一系列齊次變換矩陣。在Matlab里我們就能利用這些矩陣輕松地計(jì)算機(jī)械臂末端執(zhí)行器比如夾爪、焊槍相對(duì)于基座的位置和姿態(tài)這就是正運(yùn)動(dòng)學(xué)。反過來如果知道了末端想要到達(dá)的位置和姿態(tài)去反推每個(gè)關(guān)節(jié)需要轉(zhuǎn)動(dòng)的角度這就是逆運(yùn)動(dòng)學(xué)。而這一切的起點(diǎn)就是建立一個(gè)正確無誤的DH模型。所以這篇文章適合所有需要用Matlab進(jìn)行機(jī)器人仿真、算法開發(fā)或者僅僅是完成課程作業(yè)的朋友。無論你是想驗(yàn)證自己設(shè)計(jì)的機(jī)械臂結(jié)構(gòu)是否合理還是為后續(xù)的軌跡規(guī)劃、控制算法提供一個(gè)精確的“數(shù)字孿生”模型一個(gè)正確的DH模型都是你必須邁過去的第一道坎。接下來我會(huì)手把手帶你走完從理解原理、建立參數(shù)表到在Matlab中實(shí)現(xiàn)和驗(yàn)證的完整流程并分享幾個(gè)我踩過的大坑和避坑技巧。2. 庖丁解牛徹底搞懂DH參數(shù)法的“潛規(guī)則”與兩種慣例在動(dòng)手寫代碼之前我們必須把理論基礎(chǔ)打牢。DH參數(shù)法的核心思想是在機(jī)器人的每個(gè)連桿上“安裝”一個(gè)坐標(biāo)系然后用四個(gè)參數(shù)來描述相鄰兩個(gè)坐標(biāo)系之間的變換關(guān)系。聽起來簡(jiǎn)單但魔鬼藏在細(xì)節(jié)里。最大的 confusion 往往來自于坐標(biāo)系到底應(yīng)該怎么裝參數(shù)到底按什么順序來2.1 坐標(biāo)系建立的“黃金法則”給串聯(lián)機(jī)器人的每個(gè)連桿建立坐標(biāo)系遵循一套固定的步驟。這里以最常用的標(biāo)準(zhǔn)DH參數(shù)法為例確定關(guān)節(jié)軸Z軸第 i 個(gè)關(guān)節(jié)的旋轉(zhuǎn)或移動(dòng)軸線就是連桿 i 的 Z 軸記為 Z_i。對(duì)于旋轉(zhuǎn)關(guān)節(jié)Z軸就是關(guān)節(jié)的旋轉(zhuǎn)軸對(duì)于移動(dòng)關(guān)節(jié) prismatic jointZ軸就是關(guān)節(jié)的移動(dòng)方向。確定坐標(biāo)系原點(diǎn)O_i原點(diǎn) O_i 是 Z_i 軸和 Z_{i-1} 軸的公垂線common perpendicular與 Z_i 軸的交點(diǎn)。如果兩軸平行則公垂線有無數(shù)條通常選擇使連桿偏距 d_i 為0的那一條。如果兩軸相交則原點(diǎn)就是交點(diǎn)。確定X軸X_iX_i 軸的方向沿 Z_{i-1} 軸和 Z_i 軸的公垂線由 Z_{i-1} 軸指向 Z_i 軸。確定Y軸Y_i根據(jù)右手定則由 Z_i 軸和 X_i 軸確定 Y_i 軸Y_i Z_i × X_i。這套規(guī)則必須嚴(yán)格執(zhí)行不能憑感覺亂放。一個(gè)常見的錯(cuò)誤是把坐標(biāo)系原點(diǎn)直接放在關(guān)節(jié)的物理中心上這對(duì)于某些特定結(jié)構(gòu)的關(guān)節(jié)比如兩個(gè)旋轉(zhuǎn)關(guān)節(jié)軸線相交于一點(diǎn)是成立的但對(duì)于大多數(shù)情況尤其是軸線異面的情況就會(huì)導(dǎo)致后續(xù)的變換矩陣出錯(cuò)。2.2 四個(gè)參數(shù)的物理意義與“標(biāo)準(zhǔn)DH”流程坐標(biāo)系建好后從坐標(biāo)系 {i-1} 變換到坐標(biāo)系 {i}可以通過以下四個(gè)有序的變換來實(shí)現(xiàn)對(duì)應(yīng)四個(gè)DH參數(shù)繞 Z_{i-1} 軸旋轉(zhuǎn) θ_i 角將 X_{i-1} 軸旋轉(zhuǎn)到與 X_i 軸平行的方向。θ_i 是關(guān)節(jié)變量對(duì)于旋轉(zhuǎn)關(guān)節(jié)它是變化的對(duì)于移動(dòng)關(guān)節(jié)它是固定的。沿 Z_{i-1} 軸平移 d_i將原點(diǎn) O_{i-1} 移動(dòng)到 O_i 在 Z_{i-1} 軸上的投影點(diǎn)。d_i 是連桿偏距。沿 X_i 軸平移 a_i將原點(diǎn)移動(dòng)到 O_i 點(diǎn)。a_i 是連桿長度即從 Z_{i-1} 軸到 Z_i 軸沿 X_i 軸方向的距離。繞 X_i 軸旋轉(zhuǎn) α_i 角將 Z_{i-1} 軸旋轉(zhuǎn)到與 Z_i 軸平行的方向。α_i 是連桿轉(zhuǎn)角即從 Z_{i-1} 軸到 Z_i 軸繞 X_i 軸旋轉(zhuǎn)的角度。記住這個(gè)順序旋轉(zhuǎn)θ → 平移d → 平移a → 旋轉(zhuǎn)α。每一步變換都對(duì)應(yīng)一個(gè)齊次變換矩陣連乘起來就得到了從 {i-1} 到 {i} 的總變換矩陣A_i。A_i Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)把這個(gè)矩陣寫開就是教科書上那個(gè)經(jīng)典的DH變換矩陣。在Matlab里我們不需要手動(dòng)輸入這個(gè)矩陣的每一個(gè)元素后面會(huì)用到現(xiàn)成的函數(shù)來生成它。2.3 “標(biāo)準(zhǔn)DH” vs “改進(jìn)DH”你必須做的選擇題這是新手最容易栽跟頭的地方。除了上面講的標(biāo)準(zhǔn)DHStandard DH還有一種廣泛使用的改進(jìn)DHModified DH也叫Craig版本。它們的主要區(qū)別在于坐標(biāo)系附著在連桿的位置不同標(biāo)準(zhǔn)DH本文主要采用坐標(biāo)系 {i} 固連在連桿 i 的遠(yuǎn)端靠近關(guān)節(jié) i1 的一端。參數(shù)a_i和α_i描述了連桿 i 本身的幾何形狀。改進(jìn)DH坐標(biāo)系 {i} 固連在連桿 i 的近端靠近關(guān)節(jié) i 的一端。參數(shù)a_{i-1}和α_{i-1}描述了連桿 i-1 的幾何形狀。為什么會(huì)有兩種改進(jìn)DH在描述樹狀結(jié)構(gòu)或閉鏈機(jī)器人時(shí)更清晰且其變換矩陣連乘順序從基座到末端更直觀。而標(biāo)準(zhǔn)DH在傳統(tǒng)工業(yè)機(jī)器人教材中更常見。怎么選注意絕對(duì)不要混用一旦你為某個(gè)機(jī)器人選擇了其中一種慣例所有的參數(shù)定義、坐標(biāo)系建立、變換矩陣計(jì)算都必須遵循這一種。通常你參考的機(jī)器人數(shù)據(jù)手冊(cè)或論文會(huì)指明它用的是哪種。如果沒指明你可以根據(jù)變換矩陣的連乘順序來判斷或者選擇一種并始終堅(jiān)持。在Matlab的Robotics System Toolbox中兩種都支持但創(chuàng)建機(jī)器人對(duì)象時(shí)需要明確指定。我個(gè)人的經(jīng)驗(yàn)是對(duì)于大多數(shù)教科書上的經(jīng)典六軸串聯(lián)機(jī)械臂如PUMA560標(biāo)準(zhǔn)DH資料更多更容易上手。在本文的后續(xù)Matlab實(shí)操中我們將使用標(biāo)準(zhǔn)DH。3. 實(shí)戰(zhàn)演練以經(jīng)典六軸機(jī)械臂為例構(gòu)建DH參數(shù)表理論說再多不如動(dòng)手算一遍。我們以一個(gè)簡(jiǎn)化版的經(jīng)典六軸旋轉(zhuǎn)關(guān)節(jié)機(jī)械臂為例來推導(dǎo)它的DH參數(shù)表。假設(shè)它的結(jié)構(gòu)類似于PUMA560但尺寸簡(jiǎn)化以便理解。我們的目標(biāo)是得到下面這張表連桿 iθ_i (變量)d_ia_iα_i1θ1L10-90°2θ20L203θ30L3-90°4θ4L4090°5θ500-90°6θ6L600注L1, L2, L3, L4, L6為具體的長度值例如可以設(shè)為0.5, 0.4, 0.3, 0.2, 0.1 (米)。3.1 步驟一分析機(jī)械臂結(jié)構(gòu)草圖首先你需要一張機(jī)械臂的示意圖或三維模型。明確以下幾點(diǎn)所有關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)Revolute Joint。關(guān)節(jié)1基座關(guān)節(jié)繞垂直軸Z0旋轉(zhuǎn)。關(guān)節(jié)2和關(guān)節(jié)3的軸線平行。關(guān)節(jié)4、5、6的軸線相交于一點(diǎn)腕部中心點(diǎn)。3.2 步驟二按標(biāo)準(zhǔn)DH規(guī)則建立坐標(biāo)系我們從基座{0}開始一步步建立到末端工具坐標(biāo)系{6}。坐標(biāo)系 {0}固定在基座上。Z0沿關(guān)節(jié)1的旋轉(zhuǎn)軸垂直向上。通常將原點(diǎn)O0放在基座中心X0方向任意一般向前根據(jù)右手定則確定Y0。坐標(biāo)系 {1}Z1軸沿關(guān)節(jié)2的旋轉(zhuǎn)軸假設(shè)水平向前。原點(diǎn)O1是Z0和Z1公垂線與Z1的交點(diǎn)。因?yàn)閆0垂直Z1水平公垂線就是它們之間最短的垂直線段。這個(gè)線段的長度就是d1 L1基座高度。X1軸沿Z0到Z1的公垂線方向由Z0指向Z1水平向前。此時(shí)從{0}到{1}需要繞Z0轉(zhuǎn)θ1再沿Z0平移d1L1就使Z0與Z1共線但原點(diǎn)還差一截。接著沿X1平移a1但此時(shí)Z0和Z1已經(jīng)相交了嗎不它們異面。實(shí)際上平移a1是沿著X1軸移動(dòng)但在這個(gè)例子中經(jīng)過旋轉(zhuǎn)和平移d后原點(diǎn)O0已經(jīng)移動(dòng)到O1在Z0上的投影點(diǎn)而O1在X1軸上。因?yàn)閆0和Z1是垂直異面公垂線就是X1本身所以從投影點(diǎn)到O1的距離就是a1 0。最后需要繞X1軸旋轉(zhuǎn)使Z0與Z1同向。Z0原來是垂直的要變成水平的Z1需要繞X1軸旋轉(zhuǎn)α1 -90°(順時(shí)針90度)。坐標(biāo)系 {2}Z2軸沿關(guān)節(jié)3的旋轉(zhuǎn)軸與Z1平行水平向前。原點(diǎn)O2是Z1和Z2公垂線與Z2的交點(diǎn)。因?yàn)閆1和Z2平行公垂線是它們之間最短的連線。這段距離就是a2 L2大臂長度。X2軸沿Z1到Z2的公垂線方向。因?yàn)閮奢S平行方向可以選為從Z1指向Z2水平向前與X1同向或反向這里需要根據(jù)實(shí)際幾何判斷通常向前。從{1}到{2}繞Z1轉(zhuǎn)θ2沿Z1平移d20因?yàn)樵c(diǎn)O1已經(jīng)在公垂線與Z1的交點(diǎn)沿X2平移a2L2繞X2旋轉(zhuǎn)α20因?yàn)閆1和Z2平行。坐標(biāo)系 {3}Z3軸沿關(guān)節(jié)4的旋轉(zhuǎn)軸假設(shè)水平與Z2垂直。原點(diǎn)O3是Z2和Z3公垂線與Z3的交點(diǎn)。這段公垂線長度是a3 L3小臂長度。X3軸沿Z2到Z3的公垂線方向。從{2}到{3}繞Z2轉(zhuǎn)θ3沿Z2平移d30沿X3平移a3L3繞X3旋轉(zhuǎn)α3-90°使水平的Z2變?yōu)樗降腪3但方向垂直了所以需要轉(zhuǎn)90度具體符號(hào)根據(jù)右手定則判斷。坐標(biāo)系 {4}, {5}, {6}腕部三個(gè)關(guān)節(jié)軸線相交于一點(diǎn)腕部中心。這意味著從坐標(biāo)系{3}的原點(diǎn)O3到腕部中心點(diǎn)需要沿Z3平移d4 L4。然后因?yàn)楹罄m(xù)關(guān)節(jié)軸線相交a4, a5, a6 都為0。α4, α5, α6 則描述腕部軸線之間的夾角通常為90°或-90°。具體參數(shù)如上面表格所示。這個(gè)過程需要極強(qiáng)的空間想象力。一個(gè)極其重要的技巧是畫圖在紙上或繪圖軟件里把每個(gè)坐標(biāo)系的三根軸清清楚楚地畫出來標(biāo)上XYZ和原點(diǎn)O。這是確保參數(shù)正確的唯一可靠方法。3.3 步驟三填寫DH參數(shù)表并復(fù)核將上面分析得到的參數(shù)整理成表。填寫時(shí)務(wù)必檢查對(duì)于旋轉(zhuǎn)關(guān)節(jié)θ是變量用θ1, θ2...表示d是常數(shù)。對(duì)于移動(dòng)關(guān)節(jié)d是變量θ是常數(shù)。a和α總是常數(shù)描述連桿的固定幾何形狀。符號(hào)旋轉(zhuǎn)角α和θ遵循右手定則拇指指向旋轉(zhuǎn)軸正方向四指彎曲方向?yàn)檎?. 在Matlab中“復(fù)活”你的機(jī)器人模型參數(shù)表有了我們就可以在Matlab里把它變成一個(gè)可以驅(qū)動(dòng)、可以計(jì)算的模型了。這里主要依賴Robotics System Toolbox它是Matlab中機(jī)器人學(xué)的“瑞士軍刀”。如果你沒有這個(gè)工具箱需要額外安裝。4.1 環(huán)境準(zhǔn)備與工具箱核心函數(shù)簡(jiǎn)介首先在Matlab命令窗口輸入ver查看是否已安裝‘Robotics System Toolbox’。如果沒有需要通過Matlab的“附加功能”管理器安裝。這個(gè)工具箱提供了兩種主要的機(jī)器人對(duì)象表示方式rigidBodyTree更現(xiàn)代、更靈活的表示支持標(biāo)準(zhǔn)DH和改進(jìn)DH是官方推薦的方式。SerialLink(通過Robotics Toolbox Peter Corke版本)一個(gè)非常流行且強(qiáng)大的第三方工具箱語法更簡(jiǎn)潔。但Matlab官方已不再維護(hù)其內(nèi)置的類似功能更推薦使用rigidBodyTree。不過Peter Corke的Toolbox仍然被廣泛使用。為了覆蓋更廣我先介紹官方的rigidBodyTree方法。4.2 使用 rigidBodyTree 構(gòu)建機(jī)器人對(duì)象我們按照前面得到的DH表來創(chuàng)建機(jī)器人。% 清除工作區(qū)關(guān)閉所有圖形 clear; close all; clc; % 1. 創(chuàng)建空的剛性體樹對(duì)象 robot rigidBodyTree(DataFormat, row); % row 表示使用行向量表示變換更常見 % 2. 定義連桿的幾何參數(shù)單位米 L1 0.5; L2 0.4; L3 0.3; L4 0.2; L6 0.1; % 3. 創(chuàng)建第一個(gè)連桿Base to Joint1 % 注意基座‘world’是默認(rèn)存在的。第一個(gè)物體是連接在基座上的。 body1 rigidBody(body1); joint1 rigidBodyJoint(joint1, revolute); % 旋轉(zhuǎn)關(guān)節(jié) % 設(shè)置關(guān)節(jié)的DH參數(shù)使用標(biāo)準(zhǔn)DH慣例 % setFixedTransform(joint, dhparams, dh) 其中dhparams [a alpha d theta] % 注意這里的theta對(duì)應(yīng)的是關(guān)節(jié)變量在創(chuàng)建時(shí)先設(shè)為0后續(xù)通過關(guān)節(jié)位置控制。 dhparams1 [0, -pi/2, L1, 0]; % [a1, alpha1, d1, theta1] setFixedTransform(joint1, dhparams1, dh); body1.Joint joint1; addBody(robot, body1, base); % 將body1添加到基座上 % 4. 創(chuàng)建第二個(gè)連桿 (Body1 to Body2) body2 rigidBody(body2); joint2 rigidBodyJoint(joint2, revolute); dhparams2 [L2, 0, 0, 0]; % [a2, alpha2, d2, theta2] setFixedTransform(joint2, dhparams2, dh); body2.Joint joint2; addBody(robot, body2, body1); % 將body2連接到body1上 % 5. 創(chuàng)建第三個(gè)連桿 (Body2 to Body3) body3 rigidBody(body3); joint3 rigidBodyJoint(joint3, revolute); dhparams3 [L3, -pi/2, 0, 0]; % [a3, alpha3, d3, theta3] setFixedTransform(joint3, dhparams3, dh); body3.Joint joint3; addBody(robot, body3, body2); % 6. 創(chuàng)建第四、五、六個(gè)連桿腕部 body4 rigidBody(body4); joint4 rigidBodyJoint(joint4, revolute); dhparams4 [0, pi/2, L4, 0]; % [a4, alpha4, d4, theta4] setFixedTransform(joint4, dhparams4, dh); body4.Joint joint4; addBody(robot, body4, body3); body5 rigidBody(body5); joint5 rigidBodyJoint(joint5, revolute); dhparams5 [0, -pi/2, 0, 0]; % [a5, alpha5, d5, theta5] setFixedTransform(joint5, dhparams5, dh); body5.Joint joint5; addBody(robot, body5, body4); % 末端執(zhí)行器通常作為一個(gè)沒有關(guān)節(jié)的固定連桿 body6 rigidBody(tool); joint6 rigidBodyJoint(fix6, fixed); % 固定關(guān)節(jié)將工具固定到body5 % 從body5的坐標(biāo)系到工具坐標(biāo)系還有一個(gè)偏移 d6 dhparams6 [0, 0, L6, 0]; % [a6, alpha6, d6, theta6] setFixedTransform(joint6, dhparams6, dh); body6.Joint joint6; addBody(robot, body6, body5); % 7. 顯示機(jī)器人詳細(xì)信息 showdetails(robot)運(yùn)行showdetails(robot)你會(huì)看到機(jī)器人每個(gè)關(guān)節(jié)的類型、父物體、子物體以及變換關(guān)系這是驗(yàn)證模型結(jié)構(gòu)是否正確的重要一步。4.3 可視化與正運(yùn)動(dòng)學(xué)驗(yàn)證模型建好了我們讓它動(dòng)起來看看。% 8. 可視化機(jī)器人初始狀態(tài)所有關(guān)節(jié)角為0 figure(Name, Robot Visualization, Position, [100 100 800 600]); show(robot); title(六軸機(jī)械臂模型 (初始零位)); xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); view(135, 30); % 設(shè)置一個(gè)較好的觀察視角 grid on; axis equal; % 9. 正運(yùn)動(dòng)學(xué)計(jì)算示例給定一組關(guān)節(jié)角度計(jì)算末端位姿 config [pi/6, -pi/4, pi/3, -pi/6, pi/2, 0]; % 六個(gè)關(guān)節(jié)的角度 [θ1, θ2, θ3, θ4, θ5, θ6] % 注意config的順序必須和添加body的順序一致即對(duì)應(yīng)關(guān)節(jié)1到關(guān)節(jié)6。 % 設(shè)置機(jī)器人到這個(gè)構(gòu)型 robot.DataFormat row; % 確保數(shù)據(jù)格式 config deg2rad([30, -45, 60, -30, 90, 0]); % 也可以用角度制但記得轉(zhuǎn)換 % 獲取正運(yùn)動(dòng)學(xué)變換矩陣 tform getTransform(robot, config, tool); % ‘tool’是我們定義的末端執(zhí)行器名稱 disp(末端執(zhí)行器相對(duì)于基座的變換矩陣 T_0_tool:); disp(tform); % 從變換矩陣中提取位置和歐拉角 position tform(1:3, 4); % 位置向量 orientation tform(1:3, 1:3); % 旋轉(zhuǎn)矩陣 % 將旋轉(zhuǎn)矩陣轉(zhuǎn)換為ZYX歐拉角更直觀 eulZYX rotm2eul(orientation, ZYX); disp([末端位置: [, num2str(position), ] m]); disp([末端姿態(tài)(ZYX歐拉角): [, num2str(rad2deg(eulZYX)), ] deg]); % 10. 可視化這個(gè)特定構(gòu)型 figure; show(robot, config); title([機(jī)器人構(gòu)型: θ [, num2str(rad2deg(config)), ] deg]); view(135, 30); grid on; axis equal;如果一切正確你將看到機(jī)械臂從一個(gè)“折疊”的初始狀態(tài)運(yùn)動(dòng)到你指定關(guān)節(jié)角度的姿態(tài)。getTransform函數(shù)計(jì)算出的變換矩陣T_0_tool就是通過我們輸入的DH參數(shù)和關(guān)節(jié)角度一步步從基座坐標(biāo)系{0}變換到工具坐標(biāo)系{tool}的結(jié)果。這就是正運(yùn)動(dòng)學(xué)的核心。5. 避坑指南與進(jìn)階思考從“能用”到“好用”把模型跑通只是第一步。在實(shí)際項(xiàng)目中你會(huì)遇到更多問題。下面是我總結(jié)的幾個(gè)關(guān)鍵點(diǎn)和進(jìn)階方向。5.1 常見錯(cuò)誤排查清單如果你的模型報(bào)錯(cuò)或者運(yùn)動(dòng)姿態(tài)很奇怪請(qǐng)按以下順序檢查DH參數(shù)表錯(cuò)誤這是最最常見的問題。反復(fù)對(duì)照第二節(jié)的規(guī)則檢查每個(gè)坐標(biāo)系的Z軸方向、原點(diǎn)位置、X軸方向。特別是α角的符號(hào)很容易搞反。一個(gè)快速驗(yàn)證的方法是令所有關(guān)節(jié)角為0看看機(jī)器人的初始姿態(tài)是否符合你的預(yù)期比如是否站立、手臂是否向前伸展。如果初始姿態(tài)就歪了肯定是DH參數(shù)錯(cuò)了。Matlab代碼中的參數(shù)順序setFixedTransform(joint, dhparams, dh)中的dhparams向量順序是[a, alpha, d, theta]。千萬注意這個(gè)順序和你教科書上常見的[theta, d, a, alpha]或[d, theta, a, alpha]可能不同一定要看Matlab官方文檔。本文使用的順序是[a, alpha, d, theta]。“標(biāo)準(zhǔn)DH”與“改進(jìn)DH”混淆在setFixedTransform中第三個(gè)參數(shù)指定了慣例。我們用的是dh代表標(biāo)準(zhǔn)DH。如果你參考的模型是改進(jìn)DH則需要使用mdh并且輸入的參數(shù)意義完全不同。再次強(qiáng)調(diào)不要混用關(guān)節(jié)類型設(shè)置錯(cuò)誤旋轉(zhuǎn)關(guān)節(jié)是revolute移動(dòng)關(guān)節(jié)是prismatic。如果設(shè)錯(cuò)后續(xù)控制會(huì)出問題。單位不一致DH參數(shù)中的長度單位a, d要一致通常用米(m)。角度單位在輸入函數(shù)時(shí)Matlab默認(rèn)使用弧度(rad)。如果你習(xí)慣用度(deg)記得用deg2rad()函數(shù)轉(zhuǎn)換。末端執(zhí)行器定義我們通常關(guān)心工具末端點(diǎn)TCP的位姿。在上面的代碼中我們通過一個(gè)固定的連桿body6和固定關(guān)節(jié)‘fix6’來定義了一個(gè)工具坐標(biāo)系它距離最后一個(gè)運(yùn)動(dòng)關(guān)節(jié)body5的關(guān)節(jié)有一個(gè)d6L6的偏移。確保這個(gè)偏移是你想要的TCP位置。5.2 模型驗(yàn)證與已知模型或?qū)嵨飳?duì)比如何確信你的模型是正確的有幾個(gè)方法與經(jīng)典模型對(duì)比比如PUMA560機(jī)器人有公開的、經(jīng)過驗(yàn)證的DH參數(shù)。你可以用你的代碼實(shí)現(xiàn)PUMA560然后讓機(jī)器人移動(dòng)到某個(gè)特定姿態(tài)如所有關(guān)節(jié)為0看末端位置是否與文獻(xiàn)中一致。正逆運(yùn)動(dòng)學(xué)互驗(yàn)這是最有力的驗(yàn)證。編寫逆運(yùn)動(dòng)學(xué)算法或使用工具箱的inverseKinematics函數(shù)對(duì)正運(yùn)動(dòng)學(xué)計(jì)算出的一個(gè)末端位姿進(jìn)行逆解得到的關(guān)節(jié)角再代回正運(yùn)動(dòng)學(xué)看是否能回到原來的位姿允許微小誤差。軌跡驗(yàn)證讓末端走一個(gè)簡(jiǎn)單的已知軌跡如直線、圓觀察關(guān)節(jié)角變化是否平滑、合理。5.3 超越基礎(chǔ)將模型用于仿真與控制一個(gè)正確的DH模型是你的“數(shù)字孿生”機(jī)器人它是后續(xù)所有高級(jí)應(yīng)用的基礎(chǔ)逆運(yùn)動(dòng)學(xué)求解使用inverseKinematics對(duì)象。你可以指定末端位姿的約束位置、方向并設(shè)置權(quán)重讓求解器為你計(jì)算所需的關(guān)節(jié)角度。這對(duì)于軌跡規(guī)劃至關(guān)重要。ik inverseKinematics(RigidBodyTree, robot); weights [0.1, 0.1, 0.1, 1, 1, 1]; % 位置權(quán)重低姿態(tài)權(quán)重高 initialGuess config; % 初始猜測(cè)關(guān)節(jié)角 [qSol, solInfo] ik(tool, tform, weights, initialGuess);軌跡規(guī)劃在關(guān)節(jié)空間或笛卡爾空間生成一系列路徑點(diǎn)。例如使用trapveltraj或polynomialtraj生成平滑的關(guān)節(jié)角度軌跡然后用show函數(shù)以動(dòng)畫形式播放出來檢查是否有奇異點(diǎn)或碰撞。與Simulink集成rigidBodyTree對(duì)象可以直接導(dǎo)入Simulink的Simscape Multibody環(huán)境進(jìn)行物理仿真或者與控制系統(tǒng)模塊結(jié)合實(shí)現(xiàn)更真實(shí)的帶動(dòng)力學(xué)和控制的仿真。導(dǎo)出與共享你可以將建好的rigidBodyTree機(jī)器人模型保存為.mat文件或URDFUnified Robot Description Format文件方便在其他項(xiàng)目或ROS機(jī)器人操作系統(tǒng)中使用。5.4 一個(gè)關(guān)鍵技巧使用可視化工具輔助建模對(duì)于復(fù)雜的機(jī)器人純靠腦子想坐標(biāo)系非常困難。我強(qiáng)烈推薦一個(gè)流程在SolidWorks、Fusion 360或OnShape等CAD軟件中畫出機(jī)器人的簡(jiǎn)化模型只需要關(guān)節(jié)軸線和連桿輪廓。在CAD環(huán)境中按照標(biāo)準(zhǔn)DH規(guī)則在每個(gè)連桿上創(chuàng)建坐標(biāo)系。很多CAD軟件可以顯示坐標(biāo)系的變換矩陣。直接從CAD環(huán)境中讀取相鄰坐標(biāo)系之間的變換關(guān)系然后反推出DH參數(shù)。這比正向推導(dǎo)更不容易出錯(cuò)。 具體方法是已知從坐標(biāo)系{i-1}到{i}的變換矩陣T可以通過公式反解出(θ_i, d_i, a_i, α_i)。Matlab也有函數(shù)可以幫助完成這個(gè)計(jì)算。建立機(jī)器人運(yùn)動(dòng)學(xué)模型是機(jī)器人編程的基石這個(gè)過程充滿了對(duì)空間幾何的理解和嚴(yán)謹(jǐn)?shù)倪壿嬐茖?dǎo)。第一次做可能會(huì)覺得繁瑣但一旦你成功地把一個(gè)物理機(jī)器人用一組參數(shù)和矩陣在電腦里“復(fù)活”過來并且能精確控制它運(yùn)動(dòng)時(shí)那種成就感是無與倫比的。希望這篇詳細(xì)的指南能幫你避開我當(dāng)年踩過的那些坑順利搭建起屬于你自己的機(jī)器人仿真世界。記住多畫圖多驗(yàn)證從簡(jiǎn)單的二連桿、三連桿開始練習(xí)手感自然就來了。