孫凌云，羅福源，劉鵬

(南京航空航天大學(xué) 機(jī)電學(xué)院，江蘇 南京 210016)

0 引言

工業(yè)機(jī)器人是具有編程能力并可在自動控制下實(shí)現(xiàn)預(yù)期功能的多關(guān)節(jié)、多自由度機(jī)械裝置，其主要機(jī)構(gòu)類型包括關(guān)節(jié)型、笛卡兒型SCARA、球面坐標(biāo)型、圓柱面坐標(biāo)型等。而其中關(guān)節(jié)型機(jī)械臂具有結(jié)構(gòu)緊湊、空間施展范圍大等優(yōu)點(diǎn)，應(yīng)用最為廣泛。但因其連桿數(shù)量較多，運(yùn)動學(xué)中正逆解求解、奇異點(diǎn)解算以及三維空間中碰撞檢測較為復(fù)雜，動力學(xué)系統(tǒng)也因其存在多輸入輸出耦合關(guān)系而存在求解復(fù)雜的問題[1-2]，導(dǎo)致關(guān)節(jié)型工業(yè)機(jī)器人開發(fā)周期長、開發(fā)費(fèi)用高。隨著工業(yè)機(jī)器人在工業(yè)、航天等多個領(lǐng)域的廣泛應(yīng)用[3-4]，如何提升控制系統(tǒng)精度、縮短研發(fā)周期、減少開發(fā)費(fèi)用等引起了相關(guān)研究人員的廣泛關(guān)注。

為了解決上述問題，虛擬樣機(jī)技術(shù)應(yīng)運(yùn)而生，其興起于20世紀(jì)80年代，現(xiàn)已成功運(yùn)用于汽車制造和工程機(jī)械等多個領(lǐng)域，成為新一代數(shù)字化設(shè)計(jì)方法[5-6]。本文以6自由度串聯(lián)結(jié)構(gòu)AUBO-i5機(jī)械臂為研究對象，分析推導(dǎo)其正逆運(yùn)動學(xué)公式，并在MATLAB中采用Robotics Toolbox工具箱搭建該6軸機(jī)械臂運(yùn)動學(xué)模型，通過導(dǎo)入UG三維模型的方式在V-REP中建立其虛擬樣機(jī)模型，使用MATLAB完成運(yùn)動軌跡規(guī)劃并通過遠(yuǎn)程API同步模式控制V-REP，完成機(jī)械臂向壓力機(jī)內(nèi)部裝卸工件過程的運(yùn)動學(xué)仿真和碰撞檢測。

1 機(jī)械臂三維結(jié)構(gòu)建模

本文選取實(shí)驗(yàn)室現(xiàn)有的某公司生產(chǎn)的AUBO-i5協(xié)作機(jī)器人為研究對象。該工業(yè)機(jī)器人具有6個自由度，即3個轉(zhuǎn)動自由度和3個平動自由度，由基座、肩部、肘部和腕部構(gòu)成，其關(guān)節(jié)示意圖如圖1所示。依據(jù)公司提供的官方資料，結(jié)合對AUBO-i5協(xié)作機(jī)器人測繪來獲取形狀和尺寸參數(shù)，完成三維結(jié)構(gòu)建模。三維建模選用UG軟件，其建模步驟包括草圖繪制、各零件特征實(shí)體建模以及各關(guān)節(jié)連桿零件的裝配等。利用UG軟件建立的6軸機(jī)器人三維結(jié)構(gòu)模型圖如圖2所示。

圖1 AUBO-i5協(xié)作機(jī)器人關(guān)節(jié)示意圖

圖2 AUBO-i5協(xié)作機(jī)器人三維結(jié)構(gòu)模型

2 機(jī)械臂運(yùn)動學(xué)建模

2.1 AUBO-i5正運(yùn)動學(xué)的建立及公式推導(dǎo)

本節(jié)以某公司的AUBO-i5機(jī)器人作為研究對象，分析一個6自由度機(jī)器人的正運(yùn)動學(xué)問題。對機(jī)械臂運(yùn)動學(xué)建模時通常主流采用的是1965年HARTENBERG R S在文獻(xiàn)[7]中提出的DH系，后人對DH系又做了許多改進(jìn)，現(xiàn)在基本分為SDH和MDH[8]兩大類。AUBO-i5機(jī)械結(jié)構(gòu)基本類似于UR機(jī)器人，但其第3關(guān)節(jié)軸方向相反，采用SDH參數(shù)法分析并建立AUBO-i5機(jī)械臂的連桿固連坐標(biāo)系，連桿參數(shù)如表1所示，坐標(biāo)系分布如圖3所示。

表1 AUBO-i5的連桿參數(shù)表

圖3 AUBO-i5機(jī)械臂運(yùn)動參數(shù)和坐標(biāo)系分布

正運(yùn)動學(xué)方程推導(dǎo)即研究從關(guān)節(jié)空間到笛卡兒空間的運(yùn)動解過程如下：

求出SDH參數(shù)法下每一個連桿變換矩陣，得到6個連桿坐標(biāo)矩陣的乘積，可以簡寫為

該方程構(gòu)成了AUBO-i5的正運(yùn)動學(xué)方程。給定一組各個關(guān)節(jié)角的值，通過正運(yùn)動學(xué)方程可以計(jì)算工具坐標(biāo)系相對于基坐標(biāo)系的位置和姿態(tài)，從而完成從關(guān)節(jié)空間描述到笛卡兒空間描述的操作臂位姿轉(zhuǎn)化。

2.2 AUBO-i5逆運(yùn)動學(xué)方程求解

逆運(yùn)動學(xué)問題，即已知工具坐標(biāo)系相對于固定坐標(biāo)系的期望位置和姿態(tài)，求解一系列滿足期望要求的關(guān)節(jié)角。對于機(jī)械臂的非線性方程求解方法一般分為兩大類：數(shù)值解和封閉(解析)解。由于數(shù)值解一般通過迭代法求解，比相應(yīng)封閉解的求解速度要慢很多，且不能保證求出所有解，在篩選解的時候會造成一定問題，故在此不做詳細(xì)討論。下面主要討論封閉(解析)解方法，具有6個旋轉(zhuǎn)關(guān)節(jié)機(jī)器人存在封閉解的充分條件是相鄰的3根關(guān)節(jié)軸相交于一點(diǎn)或者平行。

(1)

求解θi，滿足θi∈[-175°,175°]。經(jīng)推導(dǎo)各個關(guān)節(jié)解如下，θ1的解。

(2)

式中：

(3)

θ5的解：

(4)

當(dāng)θ5≠0時，θ6的解為

θ6=arctan2(A6,B6)

(5)

式中：

(6)

當(dāng)θ5=0時，機(jī)械臂處于奇異位形，此時2關(guān)節(jié)、3關(guān)節(jié)、4關(guān)節(jié)和6關(guān)節(jié)軸平行，機(jī)器人存在無數(shù)種解，按情況選取θ6的解，通常選取關(guān)節(jié)6的當(dāng)前值。

當(dāng)θ5≠0時，得

(7)

θ2的解為

(8)

式中：

(9)

θ3的解為

(10)

θ4的值為

θ4=arctan2(s234,c234)-arctan2(s23,c23)

(11)

上述式中：s234=sin(θ2-θ3+θ4)；c234=cos(θ2-θ3+θ4)；s23=sin(θ2-θ3)；c23=cos(θ2-θ3)；si=sin(θi)；ci=cos(θi)。

根據(jù)上述推導(dǎo)方程在MATLAB中編寫逆運(yùn)動學(xué)求解函數(shù)，對于AUBO-i5機(jī)械臂逆運(yùn)動學(xué)求解可得到8組解。由于關(guān)節(jié)運(yùn)動范圍、機(jī)械構(gòu)型干擾等的限制，在實(shí)際篩選時需要去除一部分，在最后剩下的解中，一般選取最接近當(dāng)前機(jī)械臂位形的解。

3 基于V-REP和MATLAB的運(yùn)動學(xué)仿真

3.1 V-REP簡介

V-REP(virtual robot experimentation platform)，現(xiàn)在已改名為CoppeliaSim，是一款跨平臺的開源機(jī)器人仿真軟件，其內(nèi)部含有多種類型的功能與特性、豐富的應(yīng)用編程接口，可滿足機(jī)器人在各種環(huán)境下的仿真。其使用和搭建機(jī)器人仿真環(huán)境等操作相比ADAMS對于初學(xué)者更加友好且更加高效，而MATLAB軟件具有強(qiáng)大的矩陣計(jì)算功能，能夠根據(jù)機(jī)械臂的運(yùn)動學(xué)方程進(jìn)行快速仿真實(shí)驗(yàn)和驗(yàn)證，故本文聯(lián)合V-REP和MATLAB兩種平臺以實(shí)現(xiàn)機(jī)械臂的運(yùn)動學(xué)驗(yàn)證仿真。

3.2 V-REP中建立虛擬樣機(jī)模型

V-REP軟件自身三維建模效率較低，只能建立已知空間三維坐標(biāo)的一些簡單形狀，故采用UG三維模型導(dǎo)入的方法，同時為了保證三維模型導(dǎo)入V-REP后模型方便處理，需要先將UG中建立的AUBO-i5模型各個關(guān)節(jié)位置調(diào)整至關(guān)節(jié)零位處，并將原點(diǎn)調(diào)整與基座的中心點(diǎn)重合。詳細(xì)步驟如下：將UG模型文件保存為STL文件，導(dǎo)入V-REP場景中；分解模型并刪除大量多余的形狀，以此減輕運(yùn)動學(xué)以及動力學(xué)仿真的計(jì)算量；組合各個形狀,添加機(jī)械臂動力學(xué)參數(shù)，調(diào)整各個形狀之間的繼承關(guān)系并使用關(guān)節(jié)連接各個連桿；最后添加裝卸物件的末端夾具，完成機(jī)械臂在V-REP中的三維建模，如圖4所示。圖中為了方便理解，顯示了關(guān)節(jié)模型以及場景中各個部件的繼承關(guān)系。

圖4 V-REP中建立AUBO-i5虛擬樣機(jī)模型

3.3 軌跡規(guī)劃與運(yùn)動學(xué)仿真

運(yùn)動學(xué)仿真以AUBO-i5為研究對象，使用MATLAB通過遠(yuǎn)程API客戶端模式同步控制V-REP中虛擬樣機(jī)的運(yùn)動，完成機(jī)器人向壓力機(jī)內(nèi)部裝卸工件過程的仿真。雖然三次多項(xiàng)式軌跡確定的軌跡具有一定的連續(xù)性，但是對于一些應(yīng)用的動力學(xué)和慣性載荷會產(chǎn)生一些不期望的影響。為了獲得連續(xù)的加速度軌跡，位置和速度需要有合適的初始和終止條件，也需要合適的初始和終止加速度值。故此次仿真為保證高平穩(wěn)的運(yùn)動控制，路徑規(guī)劃采用基于五次多項(xiàng)式插值過渡的線性路徑規(guī)劃函數(shù)。五次多項(xiàng)式函數(shù)如下所示：

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

利用MATLAB Robotics Toolbox工具箱以及自編寫的正逆運(yùn)動學(xué)解算函數(shù)對裝卸工件過程進(jìn)行插補(bǔ)：機(jī)械臂零位狀態(tài)—準(zhǔn)備裝夾—裝夾工件—準(zhǔn)備移動到壓力機(jī)內(nèi)部—完成工件放置。空間中添加6個路徑中間點(diǎn)，通過雅可比矩陣將關(guān)節(jié)速度和機(jī)械臂末端的笛卡兒速度聯(lián)系，以此平滑銜接笛卡兒空間直線段與關(guān)節(jié)空間曲線段，完成對機(jī)器人末端執(zhí)行器在多位姿之間移動軌跡的路徑插補(bǔ)，以實(shí)現(xiàn)無干涉的工件裝卸過程，V-REP虛擬樣機(jī)仿真過程中4個時刻位姿如圖5所示。

圖5 AUBO-i5裝卸工件過程的運(yùn)動仿真

對機(jī)械臂裝卸工件于壓力機(jī)內(nèi)部作業(yè)過程進(jìn)行無干涉路徑規(guī)劃后，以0.005s為間隔生成總時長25 s并包含5 000路徑點(diǎn)的運(yùn)動軌跡，其中關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度隨時間變化如圖6-圖8所示(本刊黑白印刷，相關(guān)疑問咨詢作者)。

圖6 關(guān)節(jié)角度隨時間變化圖

圖7 關(guān)節(jié)角速度隨時間變化圖

圖8 關(guān)節(jié)角加速度隨時間變化圖

通過關(guān)節(jié)角加速度隨時間變化圖分析，可得知基于該運(yùn)動軌跡規(guī)劃的機(jī)械臂裝卸工件過程平滑無沖擊，滿足高平穩(wěn)運(yùn)動的控制要求。

因壓力機(jī)內(nèi)部空間狹小，機(jī)械臂及末端夾具在裝卸工件過程中可能會與壓力機(jī)上下工作面發(fā)生干涉，因最后裝卸工件過程為直線軌跡，故只截取末端夾具最后接近壓力機(jī)的位姿圖，沿各主軸作投影圖，如圖9所示。投影圖表明該軌跡規(guī)劃正確無干涉，且V-REP仿真過程中同時開啟碰撞檢測和最小距離模塊，實(shí)時仿真AUBO-i5虛擬樣機(jī)及末端夾具和壓力機(jī)及其他工件之間的干涉以及最小距離，在25s完整仿真過程中，經(jīng)軟件檢測，無碰撞且最小距離>0.004m。

圖9 壓力機(jī)內(nèi)部沿各主軸投影圖

3.4 機(jī)器人軌跡運(yùn)動實(shí)驗(yàn)

為驗(yàn)證上述生成的運(yùn)動軌跡在真實(shí)機(jī)器人裝卸工件作業(yè)中平穩(wěn)且無干涉，搭建實(shí)驗(yàn)平臺，通過機(jī)器人內(nèi)部自帶的C語言SDK庫直接對機(jī)器人進(jìn)行點(diǎn)位控制，通過rs_enter_tcp2canbus_mode函數(shù)進(jìn)入驅(qū)動器CAN直接通信模式，再使用rs_set_waypoints_to_canbus函數(shù)將上節(jié)在MATLAB中生成的平穩(wěn)無干涉的裝卸路徑加載到機(jī)器人路徑緩沖區(qū)，驅(qū)動機(jī)器人沿規(guī)劃路徑運(yùn)行，完成全部裝卸工件過程。通過rs_setcallback_realtime_joint_status實(shí)時反饋各個關(guān)節(jié)速度，最終獲得真實(shí)機(jī)器人和仿真關(guān)節(jié)角度兩者誤差隨時間變化，如圖10所示。

圖10 真實(shí)運(yùn)動與仿真關(guān)節(jié)角度誤差圖

經(jīng)過對圖8和圖10進(jìn)行聯(lián)合分析，可以看出真實(shí)機(jī)器人運(yùn)行和V-REP軟件仿真關(guān)節(jié)誤差值與各個關(guān)節(jié)角瞬時角加速度呈現(xiàn)正相關(guān)性。對以上25s運(yùn)動路徑誤差數(shù)據(jù)進(jìn)行處理，各個關(guān)節(jié)軸誤差最大分別為{0.165，0.094，0.346，0.226，0.205，0.000}(°)?？梢娫赩-REP仿真中高度還原了真實(shí)機(jī)器人各個關(guān)節(jié)在實(shí)際運(yùn)行過程中的運(yùn)動情況，驗(yàn)證了仿真方法的正確、有效。

4 結(jié)語

本文選取某公司生產(chǎn)的AUBO-i5協(xié)作機(jī)械臂為研究對象，采用UG對機(jī)械臂以及末端夾具進(jìn)行三維建模，通過對該6軸機(jī)械臂的運(yùn)動學(xué)分析，建立正逆運(yùn)動學(xué)公式，通過MATLAB搭建了運(yùn)動學(xué)仿真模型以及編寫其正逆解函數(shù)，并在V-REP中建立了虛擬樣機(jī)，以用于工件裝卸過程的仿真。采用MATLAB編寫基于五次多項(xiàng)式插值過渡的線性函數(shù)對裝卸工件過程進(jìn)行軌跡規(guī)劃，以此達(dá)到高平穩(wěn)的運(yùn)動控制，并通過遠(yuǎn)程API方式與V-REP建立通信進(jìn)行運(yùn)動仿真和碰撞檢測。仿真結(jié)果表明：以上方法能夠生成平滑無干涉的機(jī)器人工件裝卸軌跡，并通過仿真的無干涉軌跡驅(qū)動實(shí)體機(jī)器人。經(jīng)過對比實(shí)物機(jī)器人和軟件仿真兩者關(guān)節(jié)角度的實(shí)時誤差，結(jié)果顯示各個關(guān)節(jié)角度誤差均<0.35°，說明軟件仿真能高度還原真實(shí)機(jī)器人的實(shí)際運(yùn)動情況，最終驗(yàn)證了MATLAB和V-REP聯(lián)合仿真方法有效。