雙手互換機械臂的運動學建模及控制研究

田志彬，崔順風

(蘇州大學 機電工程學院，江蘇 蘇州 215100)

0 引言

目前越來越多的機器人正在取代人工，機械手臂作為現代化生產線的重要組成部分，已被廣泛應用于工件加工、貨物搬運和產品組裝等環節[1-2]。汪滿新等[3]提出了一種3-SPR并聯機械手結構，基于量綱-尺寸參數相對應的方法對機構的運動學及性能做了相關研究。梁輝等[4]提出一種具有9自由度的上肢康復機構，通過運動仿真，驗證了該康復機構運行的穩定性和可行性。當前在簡單工件的正反沖壓作業中，大部分采用人工翻面處理為主，如果將這部分工作交給機器人完成，不僅會解放更多的勞動力，還會大大提高生產效率。因此，設計了4自由度的雙手機械臂，通過兩個手爪的交替使用，實現沖壓工件的翻面處理。通過CODESYS軟控制器對運行軌跡進行規劃，在三維空間上實現了工件的自動正反面沖壓處理，且具有較快的速度和平滑的軌跡。

1 機械系統及運動學建模

1)三維模型

雙手機械臂的三維模型如圖1所示，1關節、2關節為旋轉副，實現二維空間的定位；3關節為上下移動的模組，實現上下運動；4關節為活動手爪8的旋轉運動，實現兩手的轉換，固定手爪7固定在模組上面；手爪的末端固定吸盤，實現對工件的吸取搬運。

圖1 雙手機械臂三維實體模型

2)運動學建模

運用D-H參數方法建立機械臂的連桿坐標系如圖2所示。其中，x0z0為其基坐標系，基坐標系原點位于第一關節的軸線與底座的交點處。兩個連桿參數分別為a和d,a為兩個軸關節之間的連桿的長度，d為相鄰者兩個連桿之間的偏移距離。另外還有兩個變量用來描述連桿本身或連桿之間的相互關系，其中一個為連桿連接兩端軸線的夾角度數，另一個則是兩個相鄰連桿之間的夾角度數。不過對于本機器人來說，它有3個旋轉軸以及1個移動軸，軸線之間的夾角都是90°或者0°。

圖2 雙手機械臂的連桿坐標系

當確定了各個連桿參數之后就可以畫出連桿參數表。關節4作為附加軸，獨立于軸組之外，單獨控制，工件末端的位置由前3個關節決定。設θi為桿件i相對于桿件i-1的角度，di為桿件i相對于桿件i-1的偏移量，ai為桿件i的長度，αi為桿件i的扭轉角度。

表1 機械臂的連桿參數

依據參數表，根據D-H法，求出坐標系i相對于坐標系i-1的齊次變換矩陣i-1Ti，可得

0T3=0T11T22T3=

(1)

式(1)中矩陣對應元素相等，得出

(2)

上式為機器人運動學正解，可將關節角度轉變成機器人末端位置。

由于本機械臂沒有末端姿態的旋轉，由末端位置即可反解出θ1、θ2和d3。

(3)

3)運動學模型初步驗證

利用MATLAB的Robotics工具，進行運動學建模的驗證。機械臂的規格為l1=200，l2=200。利用Link函數指定連桿，SerialLink函數連接連桿，bot. fkine和bot. ikine兩個函數分別實現正運動學和逆運動學的計算和驗證。輸入2個軌跡點的軸坐標(-41.234,75.829,40,0),(-33.563,73.472,60,180)，正解出的位置坐標分別為(315.042，-18.277，-40)、(320.069,17.742，-60)，手爪分別為閉合狀態和分離狀態。進而利用得到的位置坐標，進行運動學逆解，結果與兩個點的軸坐標吻合，運動學模型初步得以驗證。

4)運動模擬

借助SolidWorks實現對雙手機械臂的作業機制的模擬仿真。

圖3 運動模擬圖

作業機制如下：首先機械臂兩手爪呈分離狀態，固定手爪抓取正面已經加工完畢的工件；機械臂改變位置，固定手爪將工件轉移到活動手爪；活動手爪轉動180°，即實現了工件的翻面處理，通過位置控制將工件放到工位上。

2 控制系統搭建及運動學仿真

1)控制系統搭建

工控機選用海康威視視覺工控機MV-VC2210，其中一個網卡安裝Ethercat驅動，用于和驅動器通信；驅動器采用新時達MS-R四軸驅動器，支持Ethercat通信；驅動器連接4個帶有絕對編碼器的伺服電機。配置工控機為軟控制器，在CODESYS軟控制器上面建立雙手機械臂模型，實現對機械臂的運動學仿真。控制系統如圖4所示。

圖4 控制系統構成

2)運動學仿真

根據正逆運動學，前3個軸構建為軸組，為其構造正逆解模塊用于運動控制，第4個軸單獨控制。CODESYS中的虛擬模型為雙手機械臂，仿真的實體為SCARA機器人。因為SCARA機器人的大臂和小臂以及上下軸與本機械臂的結構大致相同。CODESYS可視化只顯示二維位置模型，另外兩個軸的位置信息用Ethercat捕捉，顯示其運動軌跡即可。dArmLength1、dArmLength2分別為200mm、200mm。

利用雙手機械臂的正逆解模塊進行編程，通過梯形速度規劃實現對雙手機械臂的運動仿真，工控機作為Ethercat主站，驅動器作為從站，利用直線插補將機器人從(300,-200)運動到(300,50)，跟蹤4個軸的軌跡，每隔4ms捕捉一次位置、速度、加速度信息及所有點構成平穩的位置和速度曲線。雖然實體機器人運行平穩，但是一軸、二軸的加速度有突變，會對機器人壽命產生影響。運動學逆解的關節曲線和對應的運動學正解的空間位置曲線如圖5、圖6所示。

圖5 雙手機械臂一軸、二軸的位置和速度軌跡曲線

圖6 雙手機械臂空間位置xy變化曲線

3 軌跡規劃

由于梯形速度規劃在實際中會引起加速的突變，于是在關節空間進行軌跡規劃，利用機械臂工作時必經的5個點，反解出每個軸的參數，對每個軸進行7次多項式插值。給定n+1個點位(ti,pi),利用分段7次多項式插值，使得分段多項式經過所有點序列。

起點處一階導數估計：

v0=(p1-p0)/(t1-t0)

(4)

終點處一階導數估計:

vn(pn-pn-1)/(tn-tn-1)

(5)

中間點處一階導數估計：

vk=[(pk+1-pk)/(tk+1-tk)+(pk-pk-1)/(tk-tk-1)]/2

(6)

二階導數和三階導數同理。

設每段的7次多項式方程為

(7)

利用7次多項式方程、一階導數、二階導數、三階導數的起點和終點，構成8個約束方程：p(ts)=ps；p′(ts)=vs；p″(ts)=as；p?(ts)=js；p(te)=pe；p′(te)=ve；p″(te)=ae；p?(te)=je。

根據約束方程即可求解每一時間段的7次多項式系數a0，…，a7。

(8)

對機械臂的一軸進行軌跡規劃，通過機器臂的5個位置反解出一軸的5個角度[-41.234, -33.563, -15.680, 2.566, -45.785]，每個節點對應的時間點為[0, 2, 4, 8, 10]，dt設為 0.001，利用7次多項式分段插值得到每個時間點的位置、速度、加速度和加加速度，繪制出第一個軸的位置、速度、加速度、加加速度曲線，如圖7所示。與梯形速度規劃的加速度曲線相比，7次多項式分段插值軌跡規劃加速度連續平滑，解決了加速度突變的問題。

圖7 一軸的位置、速度、加速度、加加速度曲線

4 結語

設計了一種雙手互換實現工件正反面加工的搬運機械臂，首先推導出運動學方程并通過D-H法建立機械臂的運動學模型，解決了在工作空間內從 A位置精準運動到 B位置的問題，誤差在0.1mm以內；然后通過Ethercat通信協議搭建機械臂的控制平臺，在CODESYS上建立機械臂模型，仿真得到機械臂在 0～30s內各關節角度和末端執行機構質心的變化曲線以及速度曲線。通過MATLAB進行軌跡研究，利用7次多項式插值實現更平滑的軌跡曲線，改善了加速度突變的問題，得到了良好的控制效果。仿真結果表明：機械臂的結構設計十分可靠，推導出的運動學理論方程正確，求解出的工作空間直觀、準確。