基于MATLAB 的工業機器人碼垛單元軌跡規劃

王魯平，朱華炳，秦 磊

(合肥工業大學 機械與汽車工程學院，合肥 230009)

0 引言

碼垛機器人代替人工可以進行高強度高精度重復性的物料搬運工作，具有廣闊的應用與發展前景［1］。在對碼垛機器人的研究中，軌跡規劃問題是一個重要的研究方向。目前實際生產中工業機器人編程主要使用在線示教再現編程。這一編程方法操作直觀簡便，操作者可根據現場實際情況靈活進行編程，但面對小批量、復雜多變的生產情況，這一編程方式效率較低，并受操作者素質、經驗等復雜因素影響，易于出現錯誤［2］。通過離線仿真實驗，可以根據不同實際生產情況建立機器人工作單元模型，在綜合考慮機器人重復定位精度等復雜因素的條件下，對末端執行器路徑進行設計，進而對機器人進行軌跡規劃，模擬機器人動態特性，直觀地顯示機器人運動情況，對生產過程中的機器人單元編程起到指導作用。本文以合肥工業大學工程認知博物館FANUC M-10iA 機器人碼垛工作單元實物為例，通過實驗獲得實際工況下機器人重復定位精度參考值，基于MATLAB Robotics 對機器人工作單元進行了運動學仿真，對機器人碼垛過程進行了軌跡規劃。

1 FANAC M-10iA 機器人運動學模型

1.1 FANAC M-10iA 機器人運動學正解模型

FANUC M-10iA 機器人是一種精密多關節工業機器人，具有六個轉動關節，即六個自由度。各轉動關節由交流伺服電機驅動，可以滿足驅動大慣性力矩負載和快速運動精確定位的要求。機器人本體由基座，機身，臂部，手腕及手部組成。其中基座主要起支撐作用，鏈接機身與基座的J1 關節產生回轉運動，大臂與機身構成的J2 關節驅動大臂的仰俯運動，大臂與小臂構成的J3 關節驅動小臂的仰俯運動，J4，J5，J6 關節驅動手臂的橫擺，手腕的仰俯及旋轉運動(圖1)。機器人手臂末端可安裝不同的末端執行器，以滿足不同工況的需求［3］。

圖1 FUNAC M-10iA 機器人三維結構

根據D-H 參數法［4-5］，確定了FANUC M-10iA 機器人各關節坐標系，將連桿坐標系按坐標系編號依次變換，建立4 ×4 變換矩陣表征其與前一桿件坐標系的變換關系。x0y0z0為固定參考坐標系，稱為基座坐標系，xiyizi與機器人第i個桿件固連，坐標原點在第i+1 個關節的中心點處。桿件長度ai-1為沿xi軸從zi-1到zi的距離;扭轉角αi-1為繞xi-1從zi-1到zi旋轉的角度;偏移距di為沿zi從xi-1到xi的距離;關節角θi為繞zi從xi-1到xi旋轉的角度;建立的連桿坐標系見圖2，連桿參數與關節變量的D-H 參數表見表1。

圖2 M-10iA 機器人D-H 連桿坐標系

表1 FANUC M10-iA 機器人D-H 參數表

由D-H 參數和齊次變換公式可求得兩桿之間的位姿矩陣Ti:

由上式得出機器人從基座到末端執行器的總齊次變換公式:

式中:nx、ny、nz為末端執行器法向矢量;ox、oy、oz為方向矢量;ax、ay、az為接近矢量;px、py、pz為位置坐標。

由式(7)可得出末端執行器位置坐標與各關節轉角關系:

1.2 機器人逆運動學分析

在軌跡規劃過程中，需要在已知末端執行器到達點位姿的情況下求解對應關節變量值，即求運動學逆解。運動學逆解需要考慮機器人可達空間的制約，并存在多解的問題。對于本文研究的碼垛任務，預先在笛卡爾空間內對軌跡途徑的關鍵點進行選定，將較復雜的運動路徑分解成在關鍵點間的分段運動，進而采用代數法求逆解，將逆解問題簡化［6］。

將(8)式兩邊展開，根據等式兩邊矩陣含θ1部分對應相等，解出θ1。同理，可依次解出其它各關節角θ2-6。

2 FANUC M-10iA 機器人工作單元建模

本文討論的機器人工作單元包括機器人本體，物料托架以及16 個250mm ×250mm ×250mm 的物塊。機器人水平安裝于料臺前的地面，料臺上擺放著16 個正方體物塊。機器人碼垛工作即為將這些擺放在料臺一側的正方體物塊依次搬運到料臺的另一側。為了實現搬運工作，機器人腕部附加了吸盤作為末端執行器，見圖3。

圖3 FANUC M-10iA 碼垛單元實物

根據前述D-H 參數，在Matlab 環境下使用Robotics 工具箱Link 函數L= link(［alpha A theta D sigma］)對機器人本體完成建模［7］。完成的機器人本體運動學模型見圖4。

圖4 Funac M-10iA 機器人運動學模型

在Matlab 軟件中有多種方式進行三維形體的建模，本文采用了voxel 函數，可以對三維空間中特定位置的長方體物塊進行繪圖［8］。voxel 函數可以對對象的位置、尺寸及透明度進行定義，多個voxel 繪制出的圖形可以疊加在一起形成形狀較為復雜的對象，能較好地模擬工作空間內機床、物塊、料臺等對象的幾何特征與空間位置，可以滿足多變的工作空間的建模需求。在Matlab 環境下對工作單元的建模簡便快捷，可以很好地適應多變的實際工況，能直觀地為機器人軌跡規劃提供參照［9］。本文以M-10iA 機器人碼垛任務單元為例，其初始狀態見圖5。

圖5 M-10iA 機器人工作單元初始狀態圖

3 工業機器人末端執行器的軌跡規劃

3.1 重復定位精度測量實驗

工業機器人的重復定位精度指在同一工況下機器人以相同位姿重復運動至一點時的偏差情況。作為示教編程條件下一項重要指標，機器人重復定位精度受到具體工況下溫度、濕度、載荷等復雜因素的影響［10］。為了提高機器人路徑規劃的精確性，需要考慮具體工況下機器人重復定位精度。通過固定在測量平臺上的三個位移傳感器，可以獲得機器人末端執行器搬運物塊到達指定點時在x、y、z三方向上的偏差值。通過重復試驗獲取偏差值數據，可以得出機器人實際碼垛任務工況下重復定位精度參考值。

本實驗平臺選用SGC-W10 微位移光柵傳感器，有效量程10mm，分辨率1 μm。鑒于機器人說明書給出重復定位精度參考值為±0.08mm，本實驗平臺精度可以滿足需求。傳感器輸出TTL 方波，通過RS235 串口與PC 通訊，在Matlab 環境下調用Data Acquisition 數據采集工具箱完成對傳感器信號的采集和處理，如圖6 所示。

圖6 重復定位精度實驗平臺實物圖片

通過100 次重復試驗，獲取末端執行器三維空間內偏移情況，見圖7。對實驗數據分析可得出實際工況下機器人重復定位精度為±0.1mm，這一結果與機器人說明書給出的參考值大致相符。

圖7 機器人末端執行器空間偏移量散點圖

3.2 碼垛路徑關鍵點的選取

軌跡規劃既可以在笛卡爾空間進行也可以在關節空間進行。關節空間的軌跡規劃是以關節角度的函數來描述機器人的軌跡的，也就是說機器人末端執行器的運動軌跡是由關節變量直接確定的，所以在關節空間中進行軌跡規劃容易簡單，在滿足運動學和動力學約束的條件下通過插補算法可以得到關節的軌跡。并且，在關節空間進行軌跡規劃可以避免運動中出現奇異點和機器人冗余問題［11］。但是，關節空間內軌跡規劃由于對機器人末端執行器的實際運動位置沒有直觀明顯的顯示，很難清楚的獲取末端執行器的實際位置和運動路徑，對于本文研究的碼垛作業，由于工作單元內存在障礙物，對末端執行器任務過程中瞬時位姿要求較高，首先需要在笛卡爾坐標空間進行路徑設計，進而將設計期望軌跡中的關鍵點返回到關節空間，實現對關節變量的控制，最終在關節空間內完成軌跡規劃。

在整個碼垛任務實現過程中，在工作單元內有4 個工作點Q1、Q2、Q3、Q4。機器人末端執行器從初始位置Q0出發，經過趨近點Q1到達Q2，抓取物塊后退回Q1，再由Q1經趨近點Q3到達終點Q4，最終完成對物塊的碼垛。由于機器人末端執行器采用吸盤完成抓取，具有一定的柔性，結合試驗獲取的重復定位精度參考值，在路徑設計中將抓取點設定低于物塊上表面0.5mm，使吸盤與物塊緊密接觸，確保抓取成功。在碼垛過程不與工作單元內其他物塊發生碰撞的前提下，給定末端執行器初始位置及依次到達的位置坐標如下:

圖8 碼垛任務關鍵點選取示意圖

上述位置坐標在基坐標系x0y0z0中的矢量為qi，末端執行器在運動過程中位移矢量分別為

設定末端執行器運動時間為50s，對末端執行器在笛卡爾空間內運軌跡進行仿真。依據選定的關鍵點，調用transl 函數將坐標返回為齊次變換矩陣，再調用ctraj 函數計算笛卡爾空間內軌跡。末端執行器完成碼垛任務到達軌跡末端見圖9。

末端執行器依次通過選取關鍵點過程中在x、y、z方向位移變化情況見圖10。

圖9 M-10iA 機器人碼垛任務完成效果圖

圖10 末端執行器各向位移變化隨時間曲線

仿真結果顯示，末端執行器在笛卡爾坐標系內依次通過選定的關鍵點，軌跡平滑，所設計的這一碼垛路徑可行。

3.3 關節空間內的軌跡規劃

關節空間內的軌跡規劃關鍵在于對運動學模型逆解的求解，根據笛卡爾空間內標定的末端執行器期望關鍵點，將整體軌跡分段分別反求出機器人各關節轉角位移、速度與加速度。在Matlab 環境下使用Robotics 工具箱進行逆運動學求解，得到各關節運動學變量。由關節空間內變量變化情況可以對末端執行器軌跡合理性進行驗證。各關節角位移、角速度及角加速度分別見圖11、圖12、圖13。

對6 個關節角運動學參數進行分析，可見根據在笛卡爾空間內規劃的軌跡完成碼垛任務過程中，各關節角位移平滑，角速度變化幅度較小，參照M-10iA 機器人說明書參數，可以確定機器人關節角處于安全范圍以內。由于機器人逆運動學中逆解的多重性問題，實現到達某一特定位置的方式很多，本文規劃的這一軌跡整個運動過程平穩，可以滿足碼垛作業的實際需求。末端執行器返回初始位置的軌跡規劃方法與前述相同，不再贅述。

圖11 碼垛任務中各關節(Joint1—6)角位移隨時間變化曲線

圖12 碼垛任務中各關節(Joint1—6)角速度隨時間變化曲線

圖13 碼垛任務中各關節(Joint1—6)角加速度隨時間變化曲線

3.4 后續碼垛任務的軌跡遞推

當完成首個物塊碼垛任務后，本工作空間內物塊布局發生變化。參照碼垛單元模型，在笛卡爾空間內對前述碼垛任務關鍵點進行平移，進而對后續軌跡完成規劃。為了避免干涉，后續物塊抓取結束后均需返回Q1點。后續碼垛任務規劃示意見圖14。

圖14 后續碼垛任務規劃示意

依照模塊化設計思路，在機器人編程過程中，可以將每一個物塊的碼垛過程獨立保存為一個子程序，通過順序調用這些子程序完成全部碼垛任務。

4 結論

(1)在MATlAB 軟件環境下依據D-H 參數法，結合實際工業機器人連桿及轉角參數建立的運動學模型能達到很好的動態仿真效果。本文在此基礎上對碼垛工作單元整體進行的運動學仿真，具有較大的柔性，可以適應多變的實際工況。依據碼垛單元整體運動學模型，可以便捷地在笛卡爾空間內直觀地完成末端執行器路徑設計，并返回關節空間控制各關節轉角、角速度、角加速度，最終完成軌跡規劃。

(2)相對于通過機器人示教盒在線示教再現編程方法完成碼垛任務軌跡規劃，這一動態仿真基礎上的軌跡規劃實現了對操作方案的預先設計，較好地消除了實際操作中操作者素質、經驗等復雜因素對編程準確性的影響，降低了發生碰撞事故的危險，提高了復雜任務軌跡規劃的效率，為機器人實際應用中的編程提供了有益的參考。

［1］蔡自興.機器人學［M］. 北京:清華大學出版社，2009.

［2］孟國軍.工業機器人離線編程系統關鍵技術的研究［D］.武漢:華中科技大學，2011.

［3］張衛榮.M-10iA 型FANUC 機器人控制系統研究與應用［D］.合肥:合肥工業大學，2012.

［4］Richard S. Hartenberg and Jacques Denavit. A Kinematic Notation for Lower Pair Mechanisms Based on Matrices［J］.Journal of Applied Mechanics，1955，77:215 -221.

［5］楊麗紅，秦緒祥，蔡錦達，等.工業機器人定位精度標定技術的研究［J］.控制工程，2013，20(4):785 -788.

［6］韓建海，吳斌芳，楊萍，等.工業機器人［M］.武漢:華中科技大學出版社，2009.

［7］Corke P. I.A Robotics Toolbox for MATLAB［J］.IEEE Robotics and Automation Magazine，1996，3:24 -32.

［8］Joel S. Voxel function ［EB/OL］. MATLAB Central，file exchange，2003.

［9］XIAOYUN DENG. Robot Workcell Modeling and Collision Detection with MATLAB Robotics Toolbox ［D］. Master of Science Thesis，Tampere University of Technology，2012.

［10］胡毅，費業泰，程文濤.關節式坐標測量機熱變形誤差及修正［J］. 機械工程學報，2011，47(24):15 -19.

［11］劉亞軍，黃田.6R 操作臂逆運動學分析與軌跡規劃［J］.機械工程學報，2012，48(3):9 -15.