實(shí)驗(yàn)教學(xué)SCARA機(jī)械臂控制算法研究與實(shí)現(xiàn)

姚曉通 潘影麗 高 宇

(蘭州交通大學(xué)電子與信息工程學(xué)院 甘肅 蘭州 730070)

實(shí)驗(yàn)教學(xué)SCARA機(jī)械臂控制算法研究與實(shí)現(xiàn)

姚曉通 潘影麗 高 宇

(蘭州交通大學(xué)電子與信息工程學(xué)院 甘肅 蘭州 730070)

針對實(shí)驗(yàn)教學(xué)，設(shè)計一種開放式的SCARA教學(xué)機(jī)械臂，把對控制算法的研究作為教學(xué)重點(diǎn)。首次采用仿真設(shè)計和實(shí)物設(shè)計，作為示教的兩個主要部分。通過D-H參數(shù)法確定運(yùn)動學(xué)方程，根據(jù)運(yùn)動學(xué)方程對機(jī)械臂進(jìn)行了正逆運(yùn)動學(xué)分析，結(jié)合軌跡規(guī)劃完成機(jī)械臂運(yùn)動規(guī)劃。MATLAB建立機(jī)械臂模型，通過仿真模型運(yùn)動驗(yàn)證了前期控制算法分析的準(zhǔn)確性。利用Visual Studio 2013實(shí)現(xiàn)算法，完成上位機(jī)控制系統(tǒng)，通過上位機(jī)編程控制機(jī)械臂完成物體搬運(yùn)工作。機(jī)械臂能夠精準(zhǔn)地實(shí)現(xiàn)旋轉(zhuǎn)和搬運(yùn)功能，并驗(yàn)證了控制算法研究和仿真設(shè)計的正確性，能夠較好地達(dá)到實(shí)驗(yàn)教學(xué)的需求。

SCARA MATLAB 仿真 軌跡規(guī)劃 運(yùn)動控制

0 引 言

隨著科學(xué)技術(shù)水平的不斷提高，工業(yè)機(jī)器人(機(jī)械臂)已經(jīng)大量地應(yīng)用到生產(chǎn)生活中。機(jī)器人作為一個新興的技術(shù)，涉及到了多個領(lǐng)域、各個學(xué)科。隨著科技的發(fā)展，機(jī)器人技術(shù)也逐漸深入到教育領(lǐng)域。本文專門針對實(shí)驗(yàn)教學(xué)機(jī)器人，而設(shè)計制造SCARA機(jī)械臂，作為實(shí)驗(yàn)教學(xué)的研究模型。

MATLAB平臺下的Robotic-Tools工具是研究機(jī)械臂中應(yīng)用最廣的仿真平臺。文獻(xiàn)[1]采用MATLAB介紹了機(jī)器人在實(shí)驗(yàn)教學(xué)中的應(yīng)用，文獻(xiàn)[2-3]采用了MATLAB和ADAMS聯(lián)合仿真平臺。文獻(xiàn)[4]運(yùn)用OpenGL環(huán)境創(chuàng)建機(jī)械臂模型，ADAM、OpenGL都具有很好的三維模型，使仿真結(jié)果更直觀。本文通過MATLAB和實(shí)物機(jī)械臂聯(lián)合，采用PC+控制卡方法，通過Visual Studio 2013完成上位機(jī)控制，以真實(shí)的機(jī)械臂演示，能更好、直觀地驗(yàn)證實(shí)驗(yàn)結(jié)果的真實(shí)性。

1 機(jī)械臂的模型設(shè)計

SCARA機(jī)械臂，包括三個旋轉(zhuǎn)關(guān)節(jié)和一個移動關(guān)節(jié)。SCARA機(jī)械臂定位精度高、運(yùn)動速度快，適合于平面定位和垂直方向的裝配作業(yè)。SCARA型機(jī)械臂的三維模型如圖1所示。

圖1 SCARA機(jī)械臂結(jié)構(gòu)圖

對圖1的機(jī)械臂，建立連桿坐標(biāo)系如圖2所示。每個連桿都可以通過用于描述連桿本身和用于描述連桿之間的連接關(guān)系的運(yùn)動學(xué)參數(shù)描述[5]，其分別為：單根連桿參數(shù)：ai(連桿長度)、αi(連桿扭腳)；相鄰連桿之間的參數(shù)：di(連桿距離)、θi(連桿轉(zhuǎn)角)。

圖2 D-H法建立的坐標(biāo)系

根據(jù)圖2坐標(biāo)系可得出D-H參數(shù)，如表1所示。

表1 D-H參數(shù)表

2 機(jī)械臂的控制算法

實(shí)現(xiàn)機(jī)械臂控制算法的整個過程是根據(jù)D-H法建立的坐標(biāo)系寫出變換矩陣，根據(jù)變換矩陣寫出每個坐標(biāo)系相對于基坐標(biāo)系的齊次變換矩陣；再根據(jù)每個坐標(biāo)系的齊次矩陣求出正、逆解，同時根據(jù)一定的任務(wù)尋求一條連接起點(diǎn)到終點(diǎn)的最優(yōu)或次優(yōu)有效路徑。

2.1 正運(yùn)動學(xué)分析

(1)

(2)

因此，對于本文SCARA教學(xué)機(jī)械臂，i=4時，確定末端執(zhí)行器坐標(biāo)系相當(dāng)于基坐標(biāo)系的變換矩陣為：

通過各連桿變換矩陣相乘，可以得到SCARA機(jī)械臂末端執(zhí)行的位姿方程：

(4)

通過式(4)得到末端位置矩陣：[pxpypz]T,

(5)

(6)

(7)

(8)

2.2 逆運(yùn)動學(xué)分析

逆運(yùn)動學(xué)的求解是指機(jī)械臂的末端執(zhí)行器的笛卡爾空間到各個關(guān)節(jié)空間的映射關(guān)系。逆解求解比正解復(fù)雜，需要考慮解的存在性、多重解等問題[7]。

(9)

對式(9)兩邊乘上0T1的逆矩陣，并根據(jù)矩陣兩邊對應(yīng)元素相等求解各個關(guān)節(jié)變量。

1) 求關(guān)節(jié)變量θ1為了分離變量，方程的兩邊同時左乘0T1(θ1)，得：

0T1-1·0T4=1T22T33T4

(10)

即：

根據(jù)式(10)矩陣對應(yīng)元素相等的：

L(1,4)=R(1,4)：px·cosθ1+py·sinθ1=cosθ2·l2+l1

(11)L(2,4)=R(2,4)：-px·sinθ1+py·cosθ1=sinθ2·l2

(12)

根據(jù)式(11)得：

2) 求關(guān)節(jié)變量θ2，根據(jù)式(12)得：

(13)

3) 求關(guān)節(jié)變量d3，根據(jù)左右兩邊第三行第四列對應(yīng)元素相等得：

L(3,4)=R(3,4)：d3=-pz

(14)

4) 求關(guān)節(jié)變量θ4，根據(jù)矩陣左右兩邊對應(yīng)元素相等得：

L(1,1)=R(1,1)：

nx.cosθ1+nysinθ1=cosθ2·cosθ4-sinθ2·sinθ4

(15)

L(2,1)=R(2,1)：nx·-sinθ1+ny·cosθ1=sinθ2·cosθ4-cosθ2·sinθ4

(16)

根據(jù)式(15)、式(16)得：

(17)

2.3 軌跡規(guī)劃

軌跡是指每個自由度的位置、速度和加速度的時間歷程[8]。軌跡規(guī)劃就是規(guī)劃出具有平滑性和連續(xù)型的軌跡，從而保證實(shí)際工作中的機(jī)械臂在運(yùn)動過程保持平穩(wěn)無沖擊。本文以關(guān)節(jié)空間規(guī)劃對機(jī)械臂軌跡進(jìn)行仿真并通過真實(shí)機(jī)械臂進(jìn)行驗(yàn)證。

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

(18)

(19)

(20)

把四個約束條件代入式(18)-式(20)中可以得到含有四個未知量的四個方程：

θ0=a0

(21)

(22)

0=a1

(23)

(24)

解方程可以得到：

a0=θ0

(25)

a1=0

(26)

(27)

(28)

通過式(25)-式(28)可以求出從任何起始關(guān)節(jié)角位置到期望終止位置的三次多項(xiàng)式。

3 仿真設(shè)計

利用MATLAB平臺的機(jī)器人工具箱——Robotics Toolbox[9]，采用改進(jìn)D-H建模方法，改進(jìn)的D-H建模方法可有效的彌補(bǔ)標(biāo)準(zhǔn)D-H建模與實(shí)物的偏差[10]。建立機(jī)械手運(yùn)動學(xué)模型，代碼如下：

L1=link([0 0 0 0 0],′modified′);

L2=link([0 95 0 0 0],′modified′);

L3=link([0 100 0 0 1],′modified′);

L4=link([0 0 0 -40 0],′modified′);

r = robot({L1 L2 L3 L4});

r.name = ′SCARA′;

drivebot(r);

仿真結(jié)果如圖3所示。

圖3 SCARA機(jī)械臂模型圖

根據(jù)上面軌跡規(guī)劃計算，應(yīng)用MATLAB仿真[11]，設(shè)置其由qa=[-0.000 062 0.000 178 0.000 000 -0.000 117]運(yùn)動到qb=[-0.522 315 1.504 080 0.000 000 -0.981 765]。設(shè)置時間向量t=[0∶0.015∶1]，可以觀察各個關(guān)節(jié)的運(yùn)動情況如圖4-圖6所示。

圖4 關(guān)節(jié)角度與時間的關(guān)系

圖5 關(guān)節(jié)速度與時間的關(guān)系

圖6 關(guān)節(jié)加速度與時間的關(guān)系

通過以上仿真結(jié)果，觀察每個關(guān)節(jié)的運(yùn)動。通過觀察機(jī)械臂從qa運(yùn)動qb過程中各個關(guān)節(jié)角度、速度、加速度隨時間的變化仿真結(jié)果，可以確定各連桿沒有運(yùn)動錯位的情況，并且各個關(guān)節(jié)可以平穩(wěn)地由起點(diǎn)運(yùn)動到終點(diǎn)，從而確定規(guī)劃可以滿足運(yùn)動要求。

4 機(jī)械臂實(shí)物設(shè)計

本文采用PC+控制卡方法，PC機(jī)完成運(yùn)動規(guī)劃任務(wù)，運(yùn)動控制卡完成運(yùn)動控制功能，其成本較低，適合研究討論，驗(yàn)證理論結(jié)果。

4.1 SCARA機(jī)械臂控制系統(tǒng)的組成

SCARA四自由度機(jī)械臂控制系統(tǒng)由工業(yè)機(jī)械臂、控制箱、計算機(jī)組成。由于平面機(jī)械臂主要實(shí)現(xiàn)速度快、精度高等特性，因此采用基于FPGA實(shí)現(xiàn)的多路步進(jìn)電機(jī)控制[12]。FPGA擁有豐富的內(nèi)部資源和大量的IP核，具有速度快、精度高、指令并行執(zhí)行、硬件擴(kuò)展方便等優(yōu)點(diǎn)。實(shí)物如圖7、圖8所示。

圖7 機(jī)械臂實(shí)物圖 圖8 控制箱實(shí)物圖

4.2 控制系統(tǒng)的軟件設(shè)計

本文設(shè)計了基于PC機(jī)USB總線的運(yùn)動控制器，該控制器可以同時控制多個步進(jìn)電機(jī)。開發(fā)了一套基于Windows具有交互式界面及開放式結(jié)構(gòu)的控制軟件，該控制軟件利用Visual Studio 2013并且通過動態(tài)鏈接庫編寫運(yùn)動控制函數(shù)，可以直接對硬件接口進(jìn)行訪問，便于底層訪問控制。四自由度機(jī)械臂控制界面分為參數(shù)設(shè)置模塊、正運(yùn)動學(xué)分析模塊、逆運(yùn)動學(xué)分析模塊，運(yùn)動控制模塊、結(jié)果顯示模塊。

如圖9所示上位機(jī)部分界面控制，包含對設(shè)備的打開，讓設(shè)備處于開始工作狀態(tài)；機(jī)械臂參數(shù)的設(shè)置，包含機(jī)械各個臂的長度。正運(yùn)動學(xué)仿真，輸入兩點(diǎn)參數(shù)后，會把運(yùn)動停止后的關(guān)節(jié)參數(shù)顯示出來。逆運(yùn)動學(xué)仿真，當(dāng)在末端抓持器的位姿編輯框中輸入位置時，關(guān)節(jié)角編輯框中會顯示對應(yīng)角度的變化。另外為了更好實(shí)現(xiàn)對機(jī)械臂運(yùn)動控制設(shè)置了控制末端按鈕，控制其對物體的“拿起”和“放下”，對突發(fā)事件發(fā)生設(shè)置“急?！薄C(jī)械臂的運(yùn)動的次數(shù)設(shè)置，并可以隨時控制其“暫停”“開始”。下半部分界面主要是實(shí)現(xiàn)運(yùn)動結(jié)果顯示，對機(jī)械臂運(yùn)動的一下微調(diào)控制，可以設(shè)置機(jī)械臂的運(yùn)動的方向，對每個軸分開控制。

圖9 界面整體圖

實(shí)驗(yàn)測試結(jié)果：運(yùn)行后設(shè)備打開，調(diào)用相應(yīng)函數(shù)設(shè)置機(jī)械臂參數(shù)和機(jī)械臂算法參數(shù)，在測試時，起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角速度、加速度都默認(rèn)設(shè)為零。上位機(jī)界面給出機(jī)械臂運(yùn)動軌跡，實(shí)物機(jī)械臂可以到達(dá)預(yù)設(shè)的位置點(diǎn)并完成搬運(yùn)動作。結(jié)果如圖10所示。

圖10 運(yùn)行結(jié)果圖

算法實(shí)現(xiàn)如下：

1) 正解過程：

輸入?yún)?shù)：r_theta_move關(guān)節(jié)坐標(biāo)(θ1，θ2，d3，θ4)；

輸出參數(shù)：p_clamp_point機(jī)械臂夾具坐標(biāo)。

bool flag;

flag=bool robot_forward(Rotational_struct

r_theta_move,

HPosition *p_clamp_point_ptr)

If(flag==TRUE)

{計算出p_clamp_point坐標(biāo)}

2) 反解過程

輸入?yún)?shù)：HPosition p_clamp_point機(jī)械臂夾具末端坐標(biāo);

輸出參數(shù)：float clamp_theta夾具姿態(tài)角度θ。

bool flag;

Rotational_struct left_theta_move;左臂關(guān)節(jié)坐標(biāo)(θ1，θ2，d3，θ4)；

bool left_workspace; 左臂反解是否成功

Rotational_struct right_theta_move;左臂關(guān)節(jié)坐標(biāo)(θ1，θ2，d3，θ4)；

bool right_workspace;左臂反解是否成功

flag=bool robot _inverse (p_clamp_point,

clamp_theta,

r_left_theta_move,

left_workspace,

r_right_theta_move,

right_workspace)

if(left_workspace==TRUE)

{解出r_left_theta_move左臂姿態(tài)時的關(guān)節(jié)角}

else

{由p_clamp_point、clamp_theta確定夾具姿態(tài)不在機(jī)械臂左臂工作空間}

if(right_workspace==TRUE)

{解出r_right_theta_move右臂姿態(tài)時的關(guān)節(jié)角}

else

{由p_clamp_point、clamp_theta確定夾具姿態(tài)不在機(jī)械臂右臂工作空間}

5 結(jié) 語

通過利用D-H參數(shù)法建立了SCARA機(jī)械臂的模型，對SCARA教學(xué)機(jī)械臂的整體設(shè)計規(guī)劃與實(shí)現(xiàn)，能較好地完成預(yù)計的控制目標(biāo)，完成動作和達(dá)到控制要求。通過不斷地對機(jī)械臂的調(diào)試和與對硬件的修改，實(shí)現(xiàn)了靈活的控制平臺。同時運(yùn)用各種運(yùn)行信息的實(shí)時傳輸，再與上位機(jī)的聯(lián)合調(diào)試，能夠很好地完成對各個關(guān)節(jié)的精確定位、各項(xiàng)功能都能較好地實(shí)現(xiàn)，并且可以支持遙控功能。此實(shí)驗(yàn)教學(xué)機(jī)械臂控制平臺的設(shè)計與研究，能很好地滿足教學(xué)實(shí)驗(yàn)需求，便于學(xué)生自主的安裝和調(diào)試，不僅鞏固了學(xué)生的理論知識，而且學(xué)生的創(chuàng)新意識和動手能力得到很好的培養(yǎng)和提高。

[1] 謝斌，蔡自興.基于MATLAB Robotics Toolbox的機(jī)器人學(xué)仿真實(shí)驗(yàn)教學(xué)[J].計算機(jī)教育，2010(19):140-143.

[2] 馬如奇，郝雙輝，鄭偉峰，等.基于MATLAB與ADAMS的機(jī)械臂聯(lián)合仿真研究[J].機(jī)械設(shè)計與制造，2010(4):93-95.

[3] 高東強(qiáng)，楊磊，韓昆，等.SCARA機(jī)械手的軌跡規(guī)劃及運(yùn)動學(xué)分析[J].機(jī)械設(shè)計與制造，2015(1):269-272S.

[4] 宋西平，李國琴，羅陸鋒.基于OpenGL的五自由度機(jī)械臂的運(yùn)動學(xué)仿真[J].機(jī)械設(shè)計與制造工程，2015,44(12):18-21.

[5] Craig J J.機(jī)器人學(xué)導(dǎo)論[M].機(jī)械工業(yè)出版社,2006.

[6] 潘磊，錢煒，張志艷，等.四自由度機(jī)械臂運(yùn)動學(xué)分析及Matlab仿真[J].機(jī)械科學(xué)與技術(shù)，2013,32(3):421-425.

[7] 郭發(fā)勇，梅濤，趙江梅.D-H法建立連桿坐標(biāo)系存在的問題及改進(jìn)[J].中國機(jī)械工程,2014,25(20):2710-2714.

[8] 劉磊.SCARA型四自由度機(jī)械臂軌跡規(guī)劃算法的設(shè)計與實(shí)現(xiàn)[D].安徽：安徽工程大學(xué),2013.

[9] Corke P I.A robotics toolbox for Matlab[J].IEEE Robotics and Automation Magazine,1996,3(1):24-32.

[10] 臧慶凱，春貴，閏向磊.MATLAB分析與軌跡規(guī)劃仿真[J].工業(yè)控制計算機(jī),2014,27(8):12-14.

[11] 張舒曼.基于Matlab的機(jī)器人運(yùn)動學(xué)械臂控制系統(tǒng)研究[D].湖北：湖北工業(yè)大學(xué),2014.

[12] 李遠(yuǎn)重.基于FPGA的5自由度工業(yè)機(jī)的PUMA560機(jī)器人運(yùn)動仿真研究[J].廣西科學(xué)院學(xué)報，2010,26(4)：397-400.

RESEARCH AND IMPLEMENTATION OF SCARA MANIPULATOR CONTROL ALGORITHM IN EXPERIMENTAL TEACHING

Yao Xiaotong Pan Yingli Gao Yu

(CollegeofElectronicalandInformationEngineering,LanzhouJiaotongUniversity,Lanzhou730070,Gansu,China)

Aiming at the experimental teaching, an open SCARA teaching manipulator is designed, and the research on the control algorithm is the teaching key points. Simulation design and physical design are used for the first time, as the teaching of two main parts. According to the D-H parameter method, the kinematics equation is determined. The kinematics equation is used to analyse the forward and inverse kinematics of the manipulator, and the motion planning of manipulator is completed according to the trajectory planning. The model of the manipulator is established by MATLAB, and the accuracy of the pre control algorithm is validated by simulation model movement. The algorithm completes PC control system by using Visual Studio 2013, finally through the PC programming control the manipulator to complete object handling work. The manipulator can accurately realize the rotation and handling function to verify the correctness of the control algorithm and simulation design, and can better meet the needs of experimental teaching.

SCARA MATLAB Simulation Trajectory planning Motion control

2016-09-08。甘肅省蘭州市科技局資助項(xiàng)目(213188)。姚曉通，副教授，主研領(lǐng)域：計算機(jī)測控，智能識別應(yīng)用開發(fā)，物聯(lián)網(wǎng)應(yīng)用開發(fā)。潘影麗，碩士生。高宇，碩士生。

TP39

A

10.3969/j.issn.1000-386x.2017.06.019