基于V-REP和MATLAB的機器人建模及軌跡規劃仿真驗證

孫凌云，羅福源，劉鵬

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

0 引言

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

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

1 機械臂三維結構建模

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

圖1 AUBO-i5協作機器人關節示意圖

圖2 AUBO-i5協作機器人三維結構模型

2 機械臂運動學建模

2.1 AUBO-i5正運動學的建立及公式推導

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

表1 AUBO-i5的連桿參數表

圖3 AUBO-i5機械臂運動參數和坐標系分布

正運動學方程推導即研究從關節空間到笛卡兒空間的運動解過程如下：

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

該方程構成了AUBO-i5的正運動學方程。給定一組各個關節角的值，通過正運動學方程可以計算工具坐標系相對于基坐標系的位置和姿態，從而完成從關節空間描述到笛卡兒空間描述的操作臂位姿轉化。

2.2 AUBO-i5逆運動學方程求解

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

(1)

求解θi，滿足θi∈[-175°,175°]。經推導各個關節解如下，θ1的解。

(2)

式中：

(3)

θ5的解：

(4)

當θ5≠0時，θ6的解為

θ6=arctan2(A6,B6)

(5)

式中：

(6)

當θ5=0時，機械臂處于奇異位形，此時2關節、3關節、4關節和6關節軸平行，機器人存在無數種解，按情況選取θ6的解，通常選取關節6的當前值。

當θ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)。

根據上述推導方程在MATLAB中編寫逆運動學求解函數，對于AUBO-i5機械臂逆運動學求解可得到8組解。由于關節運動范圍、機械構型干擾等的限制，在實際篩選時需要去除一部分，在最后剩下的解中，一般選取最接近當前機械臂位形的解。

3 基于V-REP和MATLAB的運動學仿真

3.1 V-REP簡介

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

3.2 V-REP中建立虛擬樣機模型

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

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

3.3 軌跡規劃與運動學仿真

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

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

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

圖5 AUBO-i5裝卸工件過程的運動仿真

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

圖6 關節角度隨時間變化圖

圖7 關節角速度隨時間變化圖

圖8 關節角加速度隨時間變化圖

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

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

圖9 壓力機內部沿各主軸投影圖

3.4 機器人軌跡運動實驗

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

圖10 真實運動與仿真關節角度誤差圖

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

4 結語

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