器人坐標(biāo)變換矩陣原理與實(shí)操指南)
1. 坐標(biāo)變換矩陣的本質(zhì)為什么玩Fanuc必須先啃這塊硬骨頭做工業(yè)機(jī)器人這行久了你會(huì)發(fā)現(xiàn)一個(gè)現(xiàn)象很多工程師操作Fanuc機(jī)器人溜得很示教器上的按鍵閉著眼睛都能摸到但一碰到“用戶坐標(biāo)系偏移”“工具坐標(biāo)系旋轉(zhuǎn)”“視覺引導(dǎo)抓取”這類需要?jiǎng)幽X子算坐標(biāo)的場景立馬卡殼。歸根結(jié)底就是對坐標(biāo)變換矩陣的理解停留在“會(huì)用”而不是“懂原理”的層面。不管你是搞焊接、搬運(yùn)、涂膠還是視覺引導(dǎo)機(jī)器人所有運(yùn)動(dòng)最終都得落到一個(gè)數(shù)學(xué)問題上工具末端在空間里到底在哪兒、朝哪個(gè)方向。而回答這個(gè)問題靠的就是坐標(biāo)變換矩陣。Fanuc機(jī)器人內(nèi)部把每一個(gè)運(yùn)動(dòng)指令都翻譯成4×4的齊次變換矩陣由旋轉(zhuǎn)矩陣和平移向量組成你看到的XYZ坐標(biāo)、WPR姿態(tài)角本質(zhì)上都是這個(gè)矩陣的“人類可讀版本”。所以這篇博文我就把Fanuc機(jī)器人坐標(biāo)變換矩陣這件事徹底掰開揉碎講清楚它是什么、怎么算、怎么用再附上實(shí)際調(diào)試中的避坑經(jīng)驗(yàn)。這可能是你在中文社區(qū)找到的最貼近現(xiàn)場的一篇了。2. 整體設(shè)計(jì)與思路拆解2.1 從點(diǎn)、向量到矩陣基礎(chǔ)數(shù)學(xué)鋪墊要理解坐標(biāo)變換矩陣首先你得把機(jī)器人運(yùn)動(dòng)學(xué)里幾個(gè)基礎(chǔ)概念過一遍?？臻g中的任意一個(gè)點(diǎn)用坐標(biāo)(Px, Py, Pz)表示這誰都知道。但機(jī)器人要運(yùn)動(dòng)光有坐標(biāo)不夠還得知道末端朝向于是有了姿態(tài)的概念。一個(gè)剛體在空間里有6個(gè)自由度3個(gè)位置加3個(gè)姿態(tài)通常我們用“平移向量旋轉(zhuǎn)矩陣”來描述。Fanuc的坐標(biāo)變換矩陣是4×4的齊次矩陣長這樣| R11 R12 R13 Tx | | R21 R22 R23 Ty | | R31 R32 R33 Tz | | 0 0 0 1 |左上角3×3的R是旋轉(zhuǎn)矩陣右上角3×1的(Tx, Ty, Tz)是平移向量。為什么非要用4×4而不是3×4因?yàn)榧恿俗詈笠恍?0, 0, 0, 1)之后平移和旋轉(zhuǎn)可以統(tǒng)一用一次矩陣乘法完成省掉來回切換的麻煩這在做坐標(biāo)系嵌套變換比如工具裝在法蘭上、相機(jī)裝在工具上時(shí)特別方便。很多新手卡在這一步總覺得矩陣相乘抽象得不行。我的經(jīng)驗(yàn)是別去死記硬背公式把它當(dāng)“完成一次空間搬運(yùn)動(dòng)作”來理解先旋轉(zhuǎn)到目標(biāo)姿態(tài)再平移到目標(biāo)位置。矩陣乘法就是把這個(gè)動(dòng)作連續(xù)疊加上去跟你在CAD里先旋轉(zhuǎn)零件再移動(dòng)零件是一個(gè)道理。2.2 為什么Fanuc用WPR而非歐拉角示教器上你看到的是WPR值分別代表繞X軸、Y軸、Z軸的旋轉(zhuǎn)角度。這里有個(gè)關(guān)鍵坑WPR并不是標(biāo)準(zhǔn)的歐拉角它更像是RPYRoll-Pitch-Yaw橫滾-俯仰-偏航的變種旋轉(zhuǎn)順序是繞X→繞Y→繞Z而且是相對固定坐標(biāo)系的旋轉(zhuǎn)。我見過太多人把Fanuc的WPR當(dāng)成XYZ歐拉角去算結(jié)果算出來的姿態(tài)死活不對。實(shí)際上Fanuc內(nèi)部計(jì)算旋轉(zhuǎn)矩陣時(shí)是按照R Rz(WR) * Ry(PR) * Rx(WPR)這個(gè)順序組合的注意這里的符號命名容易混淆示教器上的W、P、R分別對應(yīng)繞Z、繞Y、繞X的旋轉(zhuǎn)。如果你自己寫外部程序做坐標(biāo)變換必須嚴(yán)格按這個(gè)順序構(gòu)造旋轉(zhuǎn)矩陣否則視覺引導(dǎo)或者其他外部計(jì)算的坐標(biāo)一進(jìn)去就偏。2.3 外部工具與矩陣計(jì)算的典型場景在實(shí)際項(xiàng)目中坐標(biāo)變換矩陣最廣泛的用途有四個(gè)場景第一個(gè)是工具坐標(biāo)系TCP標(biāo)定第二個(gè)是用戶坐標(biāo)系UFrame定義第三個(gè)是視覺引導(dǎo)中相機(jī)坐標(biāo)到機(jī)器人坐標(biāo)的轉(zhuǎn)換第四個(gè)是離線編程或上位機(jī)做軌跡規(guī)劃時(shí)把目標(biāo)點(diǎn)從工件坐標(biāo)系映射到機(jī)器人基坐標(biāo)系。這四個(gè)場景本質(zhì)上都是同一個(gè)數(shù)學(xué)操作已知一個(gè)點(diǎn)在某坐標(biāo)系下的坐標(biāo)通過變換矩陣求出在另一個(gè)坐標(biāo)系下的坐標(biāo)。區(qū)別只是變換矩陣的構(gòu)造方式不同。所以當(dāng)你掌握了矩陣乘法、逆變換、相對變換這三板斧上述場景基本都能應(yīng)付。3. 核心細(xì)節(jié)解析與實(shí)操要點(diǎn)3.1 工具坐標(biāo)系標(biāo)定背后的矩陣邏輯先講相機(jī)引導(dǎo)里最典型的問題。我常遇到有人問為什么我在機(jī)器人上設(shè)好了TCP視覺算出來的坐標(biāo)還是偏答案多半是TCP標(biāo)定時(shí)矩陣算錯(cuò)或者驗(yàn)證不規(guī)范。Fanuc的TCP自動(dòng)標(biāo)定原理是讓機(jī)器人以不同姿態(tài)逼近同一個(gè)固定點(diǎn)然后根據(jù)多個(gè)姿態(tài)下的法蘭坐標(biāo)系位置反解出工具末端相對法蘭坐標(biāo)系的固定偏移。用坐標(biāo)變換的語言說就是要找到一個(gè)常量變換矩陣 T_tool_flange使得在所有標(biāo)定姿態(tài)下工具末端在世界坐標(biāo)系里的位置保持一致。這個(gè)過程的數(shù)學(xué)實(shí)現(xiàn)Fanuc用的是最小二乘法擬合但你沒必要自己寫示教器上的“TCP自動(dòng)設(shè)定”功能就是干這個(gè)的。實(shí)操時(shí)我建議用尖錐標(biāo)定法選6到8個(gè)差異大的姿態(tài)范圍盡量拉大避免所有姿態(tài)都聚集在一個(gè)小錐角內(nèi)——那樣矩陣接近奇異解出來極不穩(wěn)定但示教器不會(huì)給你任何提示。3.2 用戶坐標(biāo)系與工件偏移的計(jì)算用戶坐標(biāo)系UFrame的定義同樣可以用矩陣思維理解。三點(diǎn)法設(shè)定用戶坐標(biāo)系時(shí)示教器要你記錄三個(gè)點(diǎn)原點(diǎn)O、X方向上的點(diǎn)X、XY平面上的點(diǎn)Y。背后做的事就是根據(jù)這三個(gè)點(diǎn)構(gòu)造一個(gè)正交坐標(biāo)系算出這個(gè)世界坐標(biāo)系到用戶坐標(biāo)系的變換矩陣。有一點(diǎn)值得注意利用這三個(gè)點(diǎn)構(gòu)造出的X軸和Y軸可能并不嚴(yán)格正交Fanuc內(nèi)部會(huì)做一次施密特正交化處理。這意味著你示教三個(gè)點(diǎn)的時(shí)候第三個(gè)點(diǎn)Y方向點(diǎn)只要大致在平面上就行不需要精確垂直。但如果你自己寫上位機(jī)程序去算用戶坐標(biāo)系這一條一定要處理否則你算出來的變換矩陣和機(jī)器人內(nèi)部用的是兩套東西對不齊。3.3 矩陣求逆與坐標(biāo)系回推有時(shí)候你需要反過來算已知目標(biāo)點(diǎn)在工具坐標(biāo)系下的坐標(biāo)想求它在基坐標(biāo)系下的坐標(biāo)或者反過來。這時(shí)候就用得到矩陣求逆。4×4齊次變換矩陣的逆矩陣結(jié)構(gòu)上是先對旋轉(zhuǎn)矩陣轉(zhuǎn)置正交矩陣的逆就是轉(zhuǎn)置再計(jì)算平移部分的變換公式為T_inv | R^T -R^T * T | | 0 1 |這個(gè)操作看起來簡單但據(jù)我觀察有相當(dāng)一部分工程師在這個(gè)地方翻車原因是平移部分忘了乘以R的轉(zhuǎn)置。Fanuc系統(tǒng)內(nèi)部計(jì)算坐標(biāo)變換時(shí)不會(huì)出錯(cuò)但很多人在自己寫外部程序比如Python、C#里做路徑規(guī)劃時(shí)會(huì)在這里踩坑。建議寫代碼的時(shí)候用兩個(gè)已知點(diǎn)驗(yàn)證一下先正變換再逆變換看能不能回到原點(diǎn)。3.4 視覺引導(dǎo)中的矩陣疊加視覺引導(dǎo)抓取是坐標(biāo)變換矩陣應(yīng)用得最復(fù)雜的場景因?yàn)樯婕八膫€(gè)坐標(biāo)系像素坐標(biāo)系、相機(jī)坐標(biāo)系、機(jī)器人工具坐標(biāo)系、機(jī)器人基坐標(biāo)系。相機(jī)標(biāo)定求出的那個(gè)矩陣本質(zhì)就是從像素坐標(biāo)到某個(gè)機(jī)械坐標(biāo)系的變換矩陣但標(biāo)定板放的位置不同求出的矩陣含義完全不同。如果你的標(biāo)定板放在工作臺上那么標(biāo)定結(jié)果直接就是“像素→用戶坐標(biāo)系”的映射機(jī)器人可以直接用但如果標(biāo)定板固定在相機(jī)視野內(nèi)且與機(jī)器人基座無固定關(guān)系那標(biāo)定結(jié)果只是“像素→標(biāo)定板自身坐標(biāo)”的映射你還得額外測量標(biāo)定板相對機(jī)器人基座的位姿再做一次矩陣乘法才能用。這個(gè)坑是我在項(xiàng)目里踩過的。當(dāng)時(shí)一套視覺引導(dǎo)系統(tǒng)標(biāo)定完測試單點(diǎn)精度沒問題一換工件放置區(qū)域就偏得離譜。后來排查半天發(fā)現(xiàn)就是標(biāo)定板坐標(biāo)系到機(jī)器人坐標(biāo)系這個(gè)中間變換忘了疊加上去。所以看標(biāo)定模塊代碼時(shí)第一件事就是搞清它輸出的矩陣到底是相對于哪個(gè)坐標(biāo)系的。4. 實(shí)操過程與核心環(huán)節(jié)實(shí)現(xiàn)4.1 計(jì)算一個(gè)坐標(biāo)變換的完整流程下面我用一個(gè)搬運(yùn)場景從頭到尾走一遍坐標(biāo)變換矩陣的完整計(jì)算流程。假設(shè)機(jī)器人要從傳送帶上的A點(diǎn)抓取工件放到B點(diǎn)但B點(diǎn)的位置是隨傳送帶編碼器動(dòng)態(tài)變化的。第一步從PLC拿到傳送帶編碼器的累積值Encoder。第二步根據(jù)編碼器值與物理距離的線性關(guān)系算出傳送帶相對基準(zhǔn)位置的偏移量ΔX如果傳送帶方向與機(jī)器人基坐標(biāo)X軸平行。第三步構(gòu)造一個(gè)平移矩陣T_move第二列設(shè)ΔX其他平移量為0。第四步讀取目標(biāo)基準(zhǔn)點(diǎn)的位姿矩陣P_base這個(gè)基準(zhǔn)點(diǎn)可以是工件在傳送帶基準(zhǔn)位置時(shí)示教的抓取點(diǎn)。第五步實(shí)際抓取點(diǎn)姿態(tài)矩陣P_current T_move × P_base。這里有個(gè)細(xì)節(jié)值得強(qiáng)調(diào)矩陣乘法不滿足交換律T_move必須寫在左邊而不是右邊。很多剛開始接觸的人會(huì)搞反結(jié)果就是工件位置始終差一個(gè)偏移量,還找不出原因。4.2 用宏程序?qū)崿F(xiàn)實(shí)時(shí)坐標(biāo)變換Fanuc系統(tǒng)內(nèi)不借助外部上位機(jī)也可以做坐標(biāo)變換運(yùn)算通過TPTeach Pendant程序里的PRPosition Register位置寄存器和矩陣運(yùn)算指令就能實(shí)現(xiàn)。這里寫一個(gè)簡單的宏程序邏輯大家可以直接套用1: ! 讀取傳送帶編碼器值到整數(shù)寄存器 2: JMP LBL[1] 3: ... 4: LBL[1] 5: GET_REG INT[1] ENC[1] 6: ! 將編碼值轉(zhuǎn)換為偏移量并存入位置寄存器PR[10] 7: PR[10,1] INT[1] * 0.01 8: PR[10,2] 0 9: PR[10,3] 0 10: ! 基準(zhǔn)點(diǎn)存放在PR[20] 11: ! 目標(biāo)點(diǎn) PR[10] 變換 PR[20] 12: PR[30] PR[10] : PR[20] 13: ! 移動(dòng)至目標(biāo)點(diǎn) 14: L PR[30] 1000mm/sec FINE第12行那個(gè)冒號就是Fanuc TP語言里做矩陣相乘的指令實(shí)現(xiàn)的是PR[30] PR[10] × PR[20]的坐標(biāo)變換。這里PR[10]是一個(gè)純平移矩陣PR[20]是基準(zhǔn)點(diǎn)兩個(gè)矩陣復(fù)合之后就是實(shí)際抓取點(diǎn)。如果你需要對姿態(tài)也做變換那么把PR[10]中對應(yīng)的WPR分量也設(shè)置上就行。4.3 手寫矩陣計(jì)算驗(yàn)證程序有時(shí)候項(xiàng)目現(xiàn)場沒有電腦或者你只是想快速驗(yàn)證一下機(jī)器人示教的位姿數(shù)據(jù)是否正確我習(xí)慣在手機(jī)上寫一段簡單的Python代碼做矩陣驗(yàn)算。核心代碼給大家參考import numpy as np import math def wpr_to_matrix(x, y, z, w, p, r): # 注意Fanuc的WPR旋轉(zhuǎn)順序?yàn)?R Rz(w) * Ry(p) * Rx(r) wx, wy, wz math.radians(w), math.radians(p), math.radians(r) Rx np.array([[1,0,0], [0,math.cos(wx),-math.sin(wx)], [0,math.sin(wx),math.cos(wx)]]) Ry np.array([[math.cos(wy),0,math.sin(wy)], [0,1,0], [-math.sin(wy),0,math.cos(wy)]]) Rz np.array([[math.cos(wz),-math.sin(wz),0], [math.sin(wz),math.cos(wz),0], [0,0,1]]) R Rz Ry Rx T np.eye(4) T[0:3,0:3] R T[0:3,3] [x, y, z] return T def matrix_to_wpr(T): x, y, z T[0,3], T[1,3], T[2,3] r math.degrees(math.atan2(T[2,1], T[2,2])) p math.degrees(math.atan2(-T[2,0], math.sqrt(T[2,1]**2 T[2,2]**2))) w math.degrees(math.atan2(T[1,0], T[0,0])) return x, y, z, w, p, r # 示例基準(zhǔn)點(diǎn) T_base wpr_to_matrix(100, 200, 300, 0, 0, 0) # 傳送帶偏移 T_offset wpr_to_matrix(50, 0, 0, 0, 0, 0) # 實(shí)際目標(biāo)點(diǎn) T_target T_offset T_base print(matrix_to_wpr(T_target))這個(gè)程序我平時(shí)隨身帶著輸出結(jié)果跟示教器上顯示的數(shù)據(jù)對比一致后我才放心把數(shù)據(jù)發(fā)給機(jī)械或電氣工程師。注意上面計(jì)算WPR時(shí)的分解順序因?yàn)镕anuc的旋轉(zhuǎn)順序是固定的不能隨意用通用的歐拉角分解公式否則會(huì)得到一組看起來正確但實(shí)際上姿態(tài)完全不對的角度。4.4 調(diào)試中的參數(shù)驗(yàn)證方法坐標(biāo)變換做完之后不要急著跑自動(dòng)流程先在手動(dòng)模式下驗(yàn)證。我的習(xí)慣是三步走第一步是單點(diǎn)驗(yàn)證把計(jì)算出來的目標(biāo)點(diǎn)發(fā)給機(jī)器人手動(dòng)移動(dòng)過去觀察工具末端是否和期望位置重合。這步誤差應(yīng)該在1毫米以內(nèi)超過1毫米就要回頭查變換矩陣。第二步是姿態(tài)驗(yàn)證用角度尺或用示教器上的位置顯示頁面對比工具朝向重點(diǎn)檢查繞重力方向的W值也就是水平旋轉(zhuǎn)角。如果你的工具是吸盤或者焊槍這種有明確方向性的末端這一步尤其重要。第三步是連續(xù)軌跡驗(yàn)證讓機(jī)器人以不同速度走一遍完整路徑觀察中間點(diǎn)是否有抖動(dòng)或者突然的位姿跳變。出現(xiàn)跳變往往意味著在某個(gè)路徑點(diǎn)上變換矩陣的連續(xù)性出了問題最常見的原因是角度從179度跳到-179度處理這類問題時(shí)需要額外做好角度卷繞的判斷。5. 常見問題與排查技巧實(shí)錄5.1 報(bào)警Syst212的坐標(biāo)變換原理分析熱詞里有人提到syst212報(bào)警問題這個(gè)報(bào)警在Fanuc系統(tǒng)中經(jīng)常出現(xiàn)英文全稱是“Invalid position data”或類似含義實(shí)際上在多數(shù)現(xiàn)場表現(xiàn)為“Soft limit error”或“Position not reachable”。從坐標(biāo)變換的角度看本質(zhì)就是機(jī)器人逆解算出的關(guān)節(jié)角度超出軟限位。排查思路如下先查目標(biāo)點(diǎn)是否超出機(jī)器人可達(dá)范圍再查是否是用戶坐標(biāo)系或工具坐標(biāo)系設(shè)置錯(cuò)誤導(dǎo)致機(jī)器人實(shí)際運(yùn)動(dòng)目標(biāo)偏離預(yù)期最后查是否有奇異點(diǎn)。這里我強(qiáng)調(diào)一個(gè)容易忽略的點(diǎn)如果你在用戶坐標(biāo)系UFrame下執(zhí)行運(yùn)動(dòng)指令機(jī)器人內(nèi)部會(huì)先把UFrame坐標(biāo)系下的目標(biāo)點(diǎn)變換到世界坐標(biāo)系再做逆解。如果你的UFrame定義有誤即使目標(biāo)點(diǎn)看起來合理逆解結(jié)果也可能超出軟限位。5.2 角度數(shù)據(jù)跳變與姿態(tài)異常姿態(tài)異常是坐標(biāo)變換中最讓人頭疼的問題。具體表現(xiàn)是機(jī)器人移動(dòng)到目標(biāo)點(diǎn)的位置完全正確但姿態(tài)在接近目標(biāo)點(diǎn)時(shí)突然出現(xiàn)旋轉(zhuǎn)軸翻轉(zhuǎn)看起來像猛地甩了一下。出現(xiàn)這個(gè)問題的根源通常是姿態(tài)矩陣在不同表達(dá)方式之間轉(zhuǎn)換時(shí)發(fā)生了不連續(xù)。比如你從外部視覺系統(tǒng)拿到的姿態(tài)是用四元數(shù)表示的轉(zhuǎn)到Fanuc的WPR后可能因?yàn)樾D(zhuǎn)順序不匹配出現(xiàn)鏡像姿態(tài)。這時(shí)候我建議采用“位置和姿態(tài)分開處理”的策略先只驗(yàn)證位置是否準(zhǔn)確姿態(tài)單獨(dú)用直線運(yùn)動(dòng)逐步逼近避免多軸聯(lián)動(dòng)時(shí)姿態(tài)突變造成的意外。另一個(gè)常見現(xiàn)象是WPR角度接近±180度時(shí)示教器顯示值會(huì)在正負(fù)之間跳動(dòng)但機(jī)器人實(shí)際運(yùn)動(dòng)是連續(xù)的。這屬于正常的數(shù)學(xué)表達(dá)切換處理方法是不要直接對角度值做數(shù)值比較而是先轉(zhuǎn)換成旋轉(zhuǎn)矩陣再對比。5.3 現(xiàn)場坐標(biāo)系抖動(dòng)的排查思路如果機(jī)器人運(yùn)行中坐標(biāo)值沒有變化但實(shí)際位置在漂移這就要從機(jī)械和電氣兩個(gè)方向排查。矩陣計(jì)算本身是不會(huì)出錯(cuò)的但機(jī)械間隙大、傳動(dòng)帶磨損、零點(diǎn)丟失都會(huì)讓實(shí)際位姿偏離理論值。有一種典型情況是工具較重或較長時(shí)不同姿態(tài)下的重力變形會(huì)導(dǎo)致偏移量不同。這時(shí)如果你只做了一次TCP標(biāo)定換姿態(tài)后精度就會(huì)下降。對策是做多姿態(tài)驗(yàn)證或者選用更高質(zhì)量的標(biāo)定工具配合激光測量儀來做標(biāo)定。從矩陣的角度來說就是工具變形相當(dāng)于在末端執(zhí)行器上疊加了一個(gè)姿態(tài)相關(guān)的微小平移擾動(dòng)屬于矩陣模型與實(shí)際機(jī)械結(jié)構(gòu)的偏差不屬于計(jì)算錯(cuò)誤。5.4 近似奇異姿態(tài)的規(guī)避策略任何六軸機(jī)器人在某些特定姿態(tài)下都會(huì)出現(xiàn)關(guān)節(jié)速度接近無窮大的現(xiàn)象也就是奇異點(diǎn)。從坐標(biāo)變換矩陣的角度來解釋當(dāng)機(jī)器人處于奇異姿態(tài)時(shí)末端執(zhí)行器的微小位置變化需要某個(gè)關(guān)節(jié)的高速旋轉(zhuǎn)才能實(shí)現(xiàn)這個(gè)關(guān)節(jié)的速度指令會(huì)趨向極限。規(guī)避策略有兩種。一是在做軌跡規(guī)劃時(shí)預(yù)先用矩陣條件數(shù)判斷目標(biāo)姿態(tài)附近是否存在奇異點(diǎn)如果條件數(shù)過大就對目標(biāo)姿態(tài)做微調(diào)。二是把運(yùn)動(dòng)指令從關(guān)節(jié)運(yùn)動(dòng)改成直線運(yùn)動(dòng)Fanuc的直線插補(bǔ)在遇到奇異姿態(tài)時(shí)會(huì)自動(dòng)降低速度并盡量平滑過渡但無法完全消除問題。項(xiàng)目經(jīng)驗(yàn)表明焊接和涂膠軌跡最容易踩這個(gè)坑因?yàn)檐壽E貼近工件表面可調(diào)整空間非常有限。5.5 坐標(biāo)系標(biāo)定精度驗(yàn)證的獨(dú)門技巧分享一個(gè)比較小眾但很實(shí)用的驗(yàn)證方法找一個(gè)帶精密刻線的標(biāo)定板固定在臺面上用機(jī)器人工具尖端沿著刻線方向移動(dòng)分別記錄起點(diǎn)和終點(diǎn)的位姿反算工具運(yùn)動(dòng)方向向量再用千分表或激光位移傳感器測量實(shí)際位移兩者一對比就能判斷出TCP標(biāo)定的精度。這個(gè)方法比起單純用對尖錐驗(yàn)證優(yōu)勢在于可以一次性覆蓋多個(gè)姿態(tài)下的精度而且能定量看出誤差是平移誤差還是姿態(tài)誤差。如果工具距離法蘭中心越遠(yuǎn)誤差越大大概率是姿態(tài)矩陣標(biāo)定不準(zhǔn)確而不是平移向量的問題。此時(shí)重新做TCP標(biāo)定時(shí)記得在X、X-、Y、Y-、Z、Z-六個(gè)方向各選幾個(gè)姿態(tài)分散分布效果往往好過只在一個(gè)錐面上旋轉(zhuǎn)。6. 個(gè)人的一點(diǎn)經(jīng)驗(yàn)總結(jié)做Fanuc機(jī)器人項(xiàng)目這么多年我的體會(huì)是坐標(biāo)變換矩陣這個(gè)東西不是靠看一遍書就能真正掌握的一定要多動(dòng)手算、多在現(xiàn)場驗(yàn)證。最好找?guī)讉€(gè)不同類型的項(xiàng)目比如視覺引導(dǎo)、傳送帶跟蹤、多機(jī)器人協(xié)同把矩陣運(yùn)算在里面的應(yīng)用徹底過一遍才能真正融會(huì)貫通。遇到問題的時(shí)候先別急著懷疑機(jī)器人系統(tǒng)用紙筆把整個(gè)坐標(biāo)變換鏈條列出來誰相對誰、誰是基準(zhǔn)、誰是偏移量畫出來之后80%的問題都能自己解決。原理清楚了示教器上那些設(shè)置就不會(huì)是玄學(xué)報(bào)警信息也看得懂了。最后強(qiáng)調(diào)一遍所有自己做矩陣運(yùn)算的場景務(wù)必先用最簡單的例子驗(yàn)證代碼的正確性再上現(xiàn)場否則排查起來真能把人搞到崩潰。 Fanuc機(jī)器人坐標(biāo)變換矩陣原理與實(shí)操指南
1. 坐標(biāo)變換矩陣的本質(zhì)為什么玩Fanuc必須先啃這塊硬骨頭做工業(yè)機(jī)器人這行久了你會(huì)發(fā)現(xiàn)一個(gè)現(xiàn)象很多工程師操作Fanuc機(jī)器人溜得很示教器上的按鍵閉著眼睛都能摸到但一碰到“用戶坐標(biāo)系偏移”“工具坐標(biāo)系旋轉(zhuǎn)”“視覺引導(dǎo)抓取”這類需要?jiǎng)幽X子算坐標(biāo)的場景立馬卡殼。歸根結(jié)底就是對坐標(biāo)變換矩陣的理解停留在“會(huì)用”而不是“懂原理”的層面。不管你是搞焊接、搬運(yùn)、涂膠還是視覺引導(dǎo)機(jī)器人所有運(yùn)動(dòng)最終都得落到一個(gè)數(shù)學(xué)問題上工具末端在空間里到底在哪兒、朝哪個(gè)方向。而回答這個(gè)問題靠的就是坐標(biāo)變換矩陣。Fanuc機(jī)器人內(nèi)部把每一個(gè)運(yùn)動(dòng)指令都翻譯成4×4的齊次變換矩陣由旋轉(zhuǎn)矩陣和平移向量組成你看到的XYZ坐標(biāo)、WPR姿態(tài)角本質(zhì)上都是這個(gè)矩陣的“人類可讀版本”。所以這篇博文我就把Fanuc機(jī)器人坐標(biāo)變換矩陣這件事徹底掰開揉碎講清楚它是什么、怎么算、怎么用再附上實(shí)際調(diào)試中的避坑經(jīng)驗(yàn)。這可能是你在中文社區(qū)找到的最貼近現(xiàn)場的一篇了。2. 整體設(shè)計(jì)與思路拆解2.1 從點(diǎn)、向量到矩陣基礎(chǔ)數(shù)學(xué)鋪墊要理解坐標(biāo)變換矩陣首先你得把機(jī)器人運(yùn)動(dòng)學(xué)里幾個(gè)基礎(chǔ)概念過一遍?？臻g中的任意一個(gè)點(diǎn)用坐標(biāo)(Px, Py, Pz)表示這誰都知道。但機(jī)器人要運(yùn)動(dòng)光有坐標(biāo)不夠還得知道末端朝向于是有了姿態(tài)的概念。一個(gè)剛體在空間里有6個(gè)自由度3個(gè)位置加3個(gè)姿態(tài)通常我們用“平移向量旋轉(zhuǎn)矩陣”來描述。Fanuc的坐標(biāo)變換矩陣是4×4的齊次矩陣長這樣| R11 R12 R13 Tx | | R21 R22 R23 Ty | | R31 R32 R33 Tz | | 0 0 0 1 |左上角3×3的R是旋轉(zhuǎn)矩陣右上角3×1的(Tx, Ty, Tz)是平移向量。為什么非要用4×4而不是3×4因?yàn)榧恿俗詈笠恍?0, 0, 0, 1)之后平移和旋轉(zhuǎn)可以統(tǒng)一用一次矩陣乘法完成省掉來回切換的麻煩這在做坐標(biāo)系嵌套變換比如工具裝在法蘭上、相機(jī)裝在工具上時(shí)特別方便。很多新手卡在這一步總覺得矩陣相乘抽象得不行。我的經(jīng)驗(yàn)是別去死記硬背公式把它當(dāng)“完成一次空間搬運(yùn)動(dòng)作”來理解先旋轉(zhuǎn)到目標(biāo)姿態(tài)再平移到目標(biāo)位置。矩陣乘法就是把這個(gè)動(dòng)作連續(xù)疊加上去跟你在CAD里先旋轉(zhuǎn)零件再移動(dòng)零件是一個(gè)道理。2.2 為什么Fanuc用WPR而非歐拉角示教器上你看到的是WPR值分別代表繞X軸、Y軸、Z軸的旋轉(zhuǎn)角度。這里有個(gè)關(guān)鍵坑WPR并不是標(biāo)準(zhǔn)的歐拉角它更像是RPYRoll-Pitch-Yaw橫滾-俯仰-偏航的變種旋轉(zhuǎn)順序是繞X→繞Y→繞Z而且是相對固定坐標(biāo)系的旋轉(zhuǎn)。我見過太多人把Fanuc的WPR當(dāng)成XYZ歐拉角去算結(jié)果算出來的姿態(tài)死活不對。實(shí)際上Fanuc內(nèi)部計(jì)算旋轉(zhuǎn)矩陣時(shí)是按照R Rz(WR) * Ry(PR) * Rx(WPR)這個(gè)順序組合的注意這里的符號命名容易混淆示教器上的W、P、R分別對應(yīng)繞Z、繞Y、繞X的旋轉(zhuǎn)。如果你自己寫外部程序做坐標(biāo)變換必須嚴(yán)格按這個(gè)順序構(gòu)造旋轉(zhuǎn)矩陣否則視覺引導(dǎo)或者其他外部計(jì)算的坐標(biāo)一進(jìn)去就偏。2.3 外部工具與矩陣計(jì)算的典型場景在實(shí)際項(xiàng)目中坐標(biāo)變換矩陣最廣泛的用途有四個(gè)場景第一個(gè)是工具坐標(biāo)系TCP標(biāo)定第二個(gè)是用戶坐標(biāo)系UFrame定義第三個(gè)是視覺引導(dǎo)中相機(jī)坐標(biāo)到機(jī)器人坐標(biāo)的轉(zhuǎn)換第四個(gè)是離線編程或上位機(jī)做軌跡規(guī)劃時(shí)把目標(biāo)點(diǎn)從工件坐標(biāo)系映射到機(jī)器人基坐標(biāo)系。這四個(gè)場景本質(zhì)上都是同一個(gè)數(shù)學(xué)操作已知一個(gè)點(diǎn)在某坐標(biāo)系下的坐標(biāo)通過變換矩陣求出在另一個(gè)坐標(biāo)系下的坐標(biāo)。區(qū)別只是變換矩陣的構(gòu)造方式不同。所以當(dāng)你掌握了矩陣乘法、逆變換、相對變換這三板斧上述場景基本都能應(yīng)付。3. 核心細(xì)節(jié)解析與實(shí)操要點(diǎn)3.1 工具坐標(biāo)系標(biāo)定背后的矩陣邏輯先講相機(jī)引導(dǎo)里最典型的問題。我常遇到有人問為什么我在機(jī)器人上設(shè)好了TCP視覺算出來的坐標(biāo)還是偏答案多半是TCP標(biāo)定時(shí)矩陣算錯(cuò)或者驗(yàn)證不規(guī)范。Fanuc的TCP自動(dòng)標(biāo)定原理是讓機(jī)器人以不同姿態(tài)逼近同一個(gè)固定點(diǎn)然后根據(jù)多個(gè)姿態(tài)下的法蘭坐標(biāo)系位置反解出工具末端相對法蘭坐標(biāo)系的固定偏移。用坐標(biāo)變換的語言說就是要找到一個(gè)常量變換矩陣 T_tool_flange使得在所有標(biāo)定姿態(tài)下工具末端在世界坐標(biāo)系里的位置保持一致。這個(gè)過程的數(shù)學(xué)實(shí)現(xiàn)Fanuc用的是最小二乘法擬合但你沒必要自己寫示教器上的“TCP自動(dòng)設(shè)定”功能就是干這個(gè)的。實(shí)操時(shí)我建議用尖錐標(biāo)定法選6到8個(gè)差異大的姿態(tài)范圍盡量拉大避免所有姿態(tài)都聚集在一個(gè)小錐角內(nèi)——那樣矩陣接近奇異解出來極不穩(wěn)定但示教器不會(huì)給你任何提示。3.2 用戶坐標(biāo)系與工件偏移的計(jì)算用戶坐標(biāo)系UFrame的定義同樣可以用矩陣思維理解。三點(diǎn)法設(shè)定用戶坐標(biāo)系時(shí)示教器要你記錄三個(gè)點(diǎn)原點(diǎn)O、X方向上的點(diǎn)X、XY平面上的點(diǎn)Y。背后做的事就是根據(jù)這三個(gè)點(diǎn)構(gòu)造一個(gè)正交坐標(biāo)系算出這個(gè)世界坐標(biāo)系到用戶坐標(biāo)系的變換矩陣。有一點(diǎn)值得注意利用這三個(gè)點(diǎn)構(gòu)造出的X軸和Y軸可能并不嚴(yán)格正交Fanuc內(nèi)部會(huì)做一次施密特正交化處理。這意味著你示教三個(gè)點(diǎn)的時(shí)候第三個(gè)點(diǎn)Y方向點(diǎn)只要大致在平面上就行不需要精確垂直。但如果你自己寫上位機(jī)程序去算用戶坐標(biāo)系這一條一定要處理否則你算出來的變換矩陣和機(jī)器人內(nèi)部用的是兩套東西對不齊。3.3 矩陣求逆與坐標(biāo)系回推有時(shí)候你需要反過來算已知目標(biāo)點(diǎn)在工具坐標(biāo)系下的坐標(biāo)想求它在基坐標(biāo)系下的坐標(biāo)或者反過來。這時(shí)候就用得到矩陣求逆。4×4齊次變換矩陣的逆矩陣結(jié)構(gòu)上是先對旋轉(zhuǎn)矩陣轉(zhuǎn)置正交矩陣的逆就是轉(zhuǎn)置再計(jì)算平移部分的變換公式為T_inv | R^T -R^T * T | | 0 1 |這個(gè)操作看起來簡單但據(jù)我觀察有相當(dāng)一部分工程師在這個(gè)地方翻車原因是平移部分忘了乘以R的轉(zhuǎn)置。Fanuc系統(tǒng)內(nèi)部計(jì)算坐標(biāo)變換時(shí)不會(huì)出錯(cuò)但很多人在自己寫外部程序比如Python、C#里做路徑規(guī)劃時(shí)會(huì)在這里踩坑。建議寫代碼的時(shí)候用兩個(gè)已知點(diǎn)驗(yàn)證一下先正變換再逆變換看能不能回到原點(diǎn)。3.4 視覺引導(dǎo)中的矩陣疊加視覺引導(dǎo)抓取是坐標(biāo)變換矩陣應(yīng)用得最復(fù)雜的場景因?yàn)樯婕八膫€(gè)坐標(biāo)系像素坐標(biāo)系、相機(jī)坐標(biāo)系、機(jī)器人工具坐標(biāo)系、機(jī)器人基坐標(biāo)系。相機(jī)標(biāo)定求出的那個(gè)矩陣本質(zhì)就是從像素坐標(biāo)到某個(gè)機(jī)械坐標(biāo)系的變換矩陣但標(biāo)定板放的位置不同求出的矩陣含義完全不同。如果你的標(biāo)定板放在工作臺上那么標(biāo)定結(jié)果直接就是“像素→用戶坐標(biāo)系”的映射機(jī)器人可以直接用但如果標(biāo)定板固定在相機(jī)視野內(nèi)且與機(jī)器人基座無固定關(guān)系那標(biāo)定結(jié)果只是“像素→標(biāo)定板自身坐標(biāo)”的映射你還得額外測量標(biāo)定板相對機(jī)器人基座的位姿再做一次矩陣乘法才能用。這個(gè)坑是我在項(xiàng)目里踩過的。當(dāng)時(shí)一套視覺引導(dǎo)系統(tǒng)標(biāo)定完測試單點(diǎn)精度沒問題一換工件放置區(qū)域就偏得離譜。后來排查半天發(fā)現(xiàn)就是標(biāo)定板坐標(biāo)系到機(jī)器人坐標(biāo)系這個(gè)中間變換忘了疊加上去。所以看標(biāo)定模塊代碼時(shí)第一件事就是搞清它輸出的矩陣到底是相對于哪個(gè)坐標(biāo)系的。4. 實(shí)操過程與核心環(huán)節(jié)實(shí)現(xiàn)4.1 計(jì)算一個(gè)坐標(biāo)變換的完整流程下面我用一個(gè)搬運(yùn)場景從頭到尾走一遍坐標(biāo)變換矩陣的完整計(jì)算流程。假設(shè)機(jī)器人要從傳送帶上的A點(diǎn)抓取工件放到B點(diǎn)但B點(diǎn)的位置是隨傳送帶編碼器動(dòng)態(tài)變化的。第一步從PLC拿到傳送帶編碼器的累積值Encoder。第二步根據(jù)編碼器值與物理距離的線性關(guān)系算出傳送帶相對基準(zhǔn)位置的偏移量ΔX如果傳送帶方向與機(jī)器人基坐標(biāo)X軸平行。第三步構(gòu)造一個(gè)平移矩陣T_move第二列設(shè)ΔX其他平移量為0。第四步讀取目標(biāo)基準(zhǔn)點(diǎn)的位姿矩陣P_base這個(gè)基準(zhǔn)點(diǎn)可以是工件在傳送帶基準(zhǔn)位置時(shí)示教的抓取點(diǎn)。第五步實(shí)際抓取點(diǎn)姿態(tài)矩陣P_current T_move × P_base。這里有個(gè)細(xì)節(jié)值得強(qiáng)調(diào)矩陣乘法不滿足交換律T_move必須寫在左邊而不是右邊。很多剛開始接觸的人會(huì)搞反結(jié)果就是工件位置始終差一個(gè)偏移量,還找不出原因。4.2 用宏程序?qū)崿F(xiàn)實(shí)時(shí)坐標(biāo)變換Fanuc系統(tǒng)內(nèi)不借助外部上位機(jī)也可以做坐標(biāo)變換運(yùn)算通過TPTeach Pendant程序里的PRPosition Register位置寄存器和矩陣運(yùn)算指令就能實(shí)現(xiàn)。這里寫一個(gè)簡單的宏程序邏輯大家可以直接套用1: ! 讀取傳送帶編碼器值到整數(shù)寄存器 2: JMP LBL[1] 3: ... 4: LBL[1] 5: GET_REG INT[1] ENC[1] 6: ! 將編碼值轉(zhuǎn)換為偏移量并存入位置寄存器PR[10] 7: PR[10,1] INT[1] * 0.01 8: PR[10,2] 0 9: PR[10,3] 0 10: ! 基準(zhǔn)點(diǎn)存放在PR[20] 11: ! 目標(biāo)點(diǎn) PR[10] 變換 PR[20] 12: PR[30] PR[10] : PR[20] 13: ! 移動(dòng)至目標(biāo)點(diǎn) 14: L PR[30] 1000mm/sec FINE第12行那個(gè)冒號就是Fanuc TP語言里做矩陣相乘的指令實(shí)現(xiàn)的是PR[30] PR[10] × PR[20]的坐標(biāo)變換。這里PR[10]是一個(gè)純平移矩陣PR[20]是基準(zhǔn)點(diǎn)兩個(gè)矩陣復(fù)合之后就是實(shí)際抓取點(diǎn)。如果你需要對姿態(tài)也做變換那么把PR[10]中對應(yīng)的WPR分量也設(shè)置上就行。4.3 手寫矩陣計(jì)算驗(yàn)證程序有時(shí)候項(xiàng)目現(xiàn)場沒有電腦或者你只是想快速驗(yàn)證一下機(jī)器人示教的位姿數(shù)據(jù)是否正確我習(xí)慣在手機(jī)上寫一段簡單的Python代碼做矩陣驗(yàn)算。核心代碼給大家參考import numpy as np import math def wpr_to_matrix(x, y, z, w, p, r): # 注意Fanuc的WPR旋轉(zhuǎn)順序?yàn)?R Rz(w) * Ry(p) * Rx(r) wx, wy, wz math.radians(w), math.radians(p), math.radians(r) Rx np.array([[1,0,0], [0,math.cos(wx),-math.sin(wx)], [0,math.sin(wx),math.cos(wx)]]) Ry np.array([[math.cos(wy),0,math.sin(wy)], [0,1,0], [-math.sin(wy),0,math.cos(wy)]]) Rz np.array([[math.cos(wz),-math.sin(wz),0], [math.sin(wz),math.cos(wz),0], [0,0,1]]) R Rz Ry Rx T np.eye(4) T[0:3,0:3] R T[0:3,3] [x, y, z] return T def matrix_to_wpr(T): x, y, z T[0,3], T[1,3], T[2,3] r math.degrees(math.atan2(T[2,1], T[2,2])) p math.degrees(math.atan2(-T[2,0], math.sqrt(T[2,1]**2 T[2,2]**2))) w math.degrees(math.atan2(T[1,0], T[0,0])) return x, y, z, w, p, r # 示例基準(zhǔn)點(diǎn) T_base wpr_to_matrix(100, 200, 300, 0, 0, 0) # 傳送帶偏移 T_offset wpr_to_matrix(50, 0, 0, 0, 0, 0) # 實(shí)際目標(biāo)點(diǎn) T_target T_offset T_base print(matrix_to_wpr(T_target))這個(gè)程序我平時(shí)隨身帶著輸出結(jié)果跟示教器上顯示的數(shù)據(jù)對比一致后我才放心把數(shù)據(jù)發(fā)給機(jī)械或電氣工程師。注意上面計(jì)算WPR時(shí)的分解順序因?yàn)镕anuc的旋轉(zhuǎn)順序是固定的不能隨意用通用的歐拉角分解公式否則會(huì)得到一組看起來正確但實(shí)際上姿態(tài)完全不對的角度。4.4 調(diào)試中的參數(shù)驗(yàn)證方法坐標(biāo)變換做完之后不要急著跑自動(dòng)流程先在手動(dòng)模式下驗(yàn)證。我的習(xí)慣是三步走第一步是單點(diǎn)驗(yàn)證把計(jì)算出來的目標(biāo)點(diǎn)發(fā)給機(jī)器人手動(dòng)移動(dòng)過去觀察工具末端是否和期望位置重合。這步誤差應(yīng)該在1毫米以內(nèi)超過1毫米就要回頭查變換矩陣。第二步是姿態(tài)驗(yàn)證用角度尺或用示教器上的位置顯示頁面對比工具朝向重點(diǎn)檢查繞重力方向的W值也就是水平旋轉(zhuǎn)角。如果你的工具是吸盤或者焊槍這種有明確方向性的末端這一步尤其重要。第三步是連續(xù)軌跡驗(yàn)證讓機(jī)器人以不同速度走一遍完整路徑觀察中間點(diǎn)是否有抖動(dòng)或者突然的位姿跳變。出現(xiàn)跳變往往意味著在某個(gè)路徑點(diǎn)上變換矩陣的連續(xù)性出了問題最常見的原因是角度從179度跳到-179度處理這類問題時(shí)需要額外做好角度卷繞的判斷。5. 常見問題與排查技巧實(shí)錄5.1 報(bào)警Syst212的坐標(biāo)變換原理分析熱詞里有人提到syst212報(bào)警問題這個(gè)報(bào)警在Fanuc系統(tǒng)中經(jīng)常出現(xiàn)英文全稱是“Invalid position data”或類似含義實(shí)際上在多數(shù)現(xiàn)場表現(xiàn)為“Soft limit error”或“Position not reachable”。從坐標(biāo)變換的角度看本質(zhì)就是機(jī)器人逆解算出的關(guān)節(jié)角度超出軟限位。排查思路如下先查目標(biāo)點(diǎn)是否超出機(jī)器人可達(dá)范圍再查是否是用戶坐標(biāo)系或工具坐標(biāo)系設(shè)置錯(cuò)誤導(dǎo)致機(jī)器人實(shí)際運(yùn)動(dòng)目標(biāo)偏離預(yù)期最后查是否有奇異點(diǎn)。這里我強(qiáng)調(diào)一個(gè)容易忽略的點(diǎn)如果你在用戶坐標(biāo)系UFrame下執(zhí)行運(yùn)動(dòng)指令機(jī)器人內(nèi)部會(huì)先把UFrame坐標(biāo)系下的目標(biāo)點(diǎn)變換到世界坐標(biāo)系再做逆解。如果你的UFrame定義有誤即使目標(biāo)點(diǎn)看起來合理逆解結(jié)果也可能超出軟限位。5.2 角度數(shù)據(jù)跳變與姿態(tài)異常姿態(tài)異常是坐標(biāo)變換中最讓人頭疼的問題。具體表現(xiàn)是機(jī)器人移動(dòng)到目標(biāo)點(diǎn)的位置完全正確但姿態(tài)在接近目標(biāo)點(diǎn)時(shí)突然出現(xiàn)旋轉(zhuǎn)軸翻轉(zhuǎn)看起來像猛地甩了一下。出現(xiàn)這個(gè)問題的根源通常是姿態(tài)矩陣在不同表達(dá)方式之間轉(zhuǎn)換時(shí)發(fā)生了不連續(xù)。比如你從外部視覺系統(tǒng)拿到的姿態(tài)是用四元數(shù)表示的轉(zhuǎn)到Fanuc的WPR后可能因?yàn)樾D(zhuǎn)順序不匹配出現(xiàn)鏡像姿態(tài)。這時(shí)候我建議采用“位置和姿態(tài)分開處理”的策略先只驗(yàn)證位置是否準(zhǔn)確姿態(tài)單獨(dú)用直線運(yùn)動(dòng)逐步逼近避免多軸聯(lián)動(dòng)時(shí)姿態(tài)突變造成的意外。另一個(gè)常見現(xiàn)象是WPR角度接近±180度時(shí)示教器顯示值會(huì)在正負(fù)之間跳動(dòng)但機(jī)器人實(shí)際運(yùn)動(dòng)是連續(xù)的。這屬于正常的數(shù)學(xué)表達(dá)切換處理方法是不要直接對角度值做數(shù)值比較而是先轉(zhuǎn)換成旋轉(zhuǎn)矩陣再對比。5.3 現(xiàn)場坐標(biāo)系抖動(dòng)的排查思路如果機(jī)器人運(yùn)行中坐標(biāo)值沒有變化但實(shí)際位置在漂移這就要從機(jī)械和電氣兩個(gè)方向排查。矩陣計(jì)算本身是不會(huì)出錯(cuò)的但機(jī)械間隙大、傳動(dòng)帶磨損、零點(diǎn)丟失都會(huì)讓實(shí)際位姿偏離理論值。有一種典型情況是工具較重或較長時(shí)不同姿態(tài)下的重力變形會(huì)導(dǎo)致偏移量不同。這時(shí)如果你只做了一次TCP標(biāo)定換姿態(tài)后精度就會(huì)下降。對策是做多姿態(tài)驗(yàn)證或者選用更高質(zhì)量的標(biāo)定工具配合激光測量儀來做標(biāo)定。從矩陣的角度來說就是工具變形相當(dāng)于在末端執(zhí)行器上疊加了一個(gè)姿態(tài)相關(guān)的微小平移擾動(dòng)屬于矩陣模型與實(shí)際機(jī)械結(jié)構(gòu)的偏差不屬于計(jì)算錯(cuò)誤。5.4 近似奇異姿態(tài)的規(guī)避策略任何六軸機(jī)器人在某些特定姿態(tài)下都會(huì)出現(xiàn)關(guān)節(jié)速度接近無窮大的現(xiàn)象也就是奇異點(diǎn)。從坐標(biāo)變換矩陣的角度來解釋當(dāng)機(jī)器人處于奇異姿態(tài)時(shí)末端執(zhí)行器的微小位置變化需要某個(gè)關(guān)節(jié)的高速旋轉(zhuǎn)才能實(shí)現(xiàn)這個(gè)關(guān)節(jié)的速度指令會(huì)趨向極限。規(guī)避策略有兩種。一是在做軌跡規(guī)劃時(shí)預(yù)先用矩陣條件數(shù)判斷目標(biāo)姿態(tài)附近是否存在奇異點(diǎn)如果條件數(shù)過大就對目標(biāo)姿態(tài)做微調(diào)。二是把運(yùn)動(dòng)指令從關(guān)節(jié)運(yùn)動(dòng)改成直線運(yùn)動(dòng)Fanuc的直線插補(bǔ)在遇到奇異姿態(tài)時(shí)會(huì)自動(dòng)降低速度并盡量平滑過渡但無法完全消除問題。項(xiàng)目經(jīng)驗(yàn)表明焊接和涂膠軌跡最容易踩這個(gè)坑因?yàn)檐壽E貼近工件表面可調(diào)整空間非常有限。5.5 坐標(biāo)系標(biāo)定精度驗(yàn)證的獨(dú)門技巧分享一個(gè)比較小眾但很實(shí)用的驗(yàn)證方法找一個(gè)帶精密刻線的標(biāo)定板固定在臺面上用機(jī)器人工具尖端沿著刻線方向移動(dòng)分別記錄起點(diǎn)和終點(diǎn)的位姿反算工具運(yùn)動(dòng)方向向量再用千分表或激光位移傳感器測量實(shí)際位移兩者一對比就能判斷出TCP標(biāo)定的精度。這個(gè)方法比起單純用對尖錐驗(yàn)證優(yōu)勢在于可以一次性覆蓋多個(gè)姿態(tài)下的精度而且能定量看出誤差是平移誤差還是姿態(tài)誤差。如果工具距離法蘭中心越遠(yuǎn)誤差越大大概率是姿態(tài)矩陣標(biāo)定不準(zhǔn)確而不是平移向量的問題。此時(shí)重新做TCP標(biāo)定時(shí)記得在X、X-、Y、Y-、Z、Z-六個(gè)方向各選幾個(gè)姿態(tài)分散分布效果往往好過只在一個(gè)錐面上旋轉(zhuǎn)。6. 個(gè)人的一點(diǎn)經(jīng)驗(yàn)總結(jié)做Fanuc機(jī)器人項(xiàng)目這么多年我的體會(huì)是坐標(biāo)變換矩陣這個(gè)東西不是靠看一遍書就能真正掌握的一定要多動(dòng)手算、多在現(xiàn)場驗(yàn)證。最好找?guī)讉€(gè)不同類型的項(xiàng)目比如視覺引導(dǎo)、傳送帶跟蹤、多機(jī)器人協(xié)同把矩陣運(yùn)算在里面的應(yīng)用徹底過一遍才能真正融會(huì)貫通。遇到問題的時(shí)候先別急著懷疑機(jī)器人系統(tǒng)用紙筆把整個(gè)坐標(biāo)變換鏈條列出來誰相對誰、誰是基準(zhǔn)、誰是偏移量畫出來之后80%的問題都能自己解決。原理清楚了示教器上那些設(shè)置就不會(huì)是玄學(xué)報(bào)警信息也看得懂了。最后強(qiáng)調(diào)一遍所有自己做矩陣運(yùn)算的場景務(wù)必先用最簡單的例子驗(yàn)證代碼的正確性再上現(xiàn)場否則排查起來真能把人搞到崩潰。