Delta機(jī)器人運(yùn)動(dòng)控制及圖形軌跡的動(dòng)態(tài)仿真

閆林林，徐 方，賈 凱，鄒風(fēng)山

（1.中國科學(xué)院 沈陽自動(dòng)化研究所 機(jī)器人學(xué)國家重點(diǎn)實(shí)驗(yàn)室，遼寧 沈陽110016；2.中國科學(xué)院大學(xué)，北京100039；3.沈陽新松機(jī)器人自動(dòng)化有限公司，遼寧 沈陽110168）

0 引 言

Delta機(jī)器人作為一種最成功的并聯(lián)機(jī)器人已經(jīng)得到了許多專家學(xué)者的高度評(píng)價(jià)和熱衷研究［1－3］。針對(duì)并聯(lián)機(jī)器人軌跡的編程方式，傳統(tǒng)的機(jī)器人一般使用示教再現(xiàn)的方法，但此種方法不僅在控制策略上繁瑣復(fù)雜，而且在需要更改軌跡時(shí)，整個(gè)示教過程需要重新進(jìn)行，浪費(fèi)時(shí)間［4，5］。

機(jī)器人圖形化軌跡編程作為一種新的軌跡編程方法［6，7］，其在利用計(jì)算機(jī)圖形學(xué)對(duì)機(jī)器人進(jìn)行建模的基礎(chǔ)上，將機(jī)器人軌跡作為若干的圖形軌跡模塊的組合。在圖形化編程中，圖形軌跡算法是其重要和基本的研究內(nèi)容，而算法的研究必須需要一種平臺(tái)作為工具來進(jìn)行算法的分析和驗(yàn)證。傳統(tǒng)機(jī)器人一般使用MATLAB 及其Robotics工具箱來進(jìn)行算法研究［8］，而Robotics工具箱并沒有針對(duì)并聯(lián)機(jī)器人的相關(guān)模型庫，同時(shí)MATLAB 在圖形學(xué)方面對(duì)機(jī)器人的三維建模及動(dòng)態(tài)仿真的能力表現(xiàn)相對(duì)較差［9］。OpenGL作為一種優(yōu)秀的圖形編程接口，具有強(qiáng)大的三維建模能力，在機(jī)器人仿真等領(lǐng)域的研究受到了眾多學(xué)者的青睞［10］。

針對(duì)以上的情況，利用OpenGL建立了Delta機(jī)器人三維模型，設(shè)計(jì)了一套機(jī)器人動(dòng)態(tài)仿真系統(tǒng)。對(duì)Delta機(jī)器人的運(yùn)動(dòng)控制算法和圖形軌跡算法進(jìn)行了研究，并在仿真系統(tǒng)平臺(tái)上進(jìn)行了算法的驗(yàn)證及動(dòng)態(tài)仿真。

1 Delta機(jī)器人的OpenGL建模

OpenGL是一種獨(dú)立于硬件、可運(yùn)行于多操作系統(tǒng)的三維軟件包接口，其按照計(jì)算機(jī)圖形學(xué)原理設(shè)計(jì)而成，具有嚴(yán)格和卓越的三維圖形標(biāo)準(zhǔn)。其具有強(qiáng)大的三維建模能力，且仿真動(dòng)畫運(yùn)行效果逼真流暢，非常適用于機(jī)器人的仿真研究。下面根據(jù)Delta機(jī)器人的運(yùn)動(dòng)特性及相應(yīng)的構(gòu)件形狀，利用OpenGL相關(guān)技術(shù)對(duì)其構(gòu)建三維仿真模型。

如圖1所示，利用OpenGL 繪制兩個(gè)三角圖形來代替Delta機(jī)器人的靜平臺(tái)和動(dòng)平臺(tái)，根據(jù)三自由度Delta機(jī)器人的特點(diǎn)可知靜平臺(tái)和動(dòng)平臺(tái)所在的平面始終平行。鑲嵌在靜平臺(tái)3個(gè)邊中心點(diǎn)的圓柱體來表示3個(gè)電機(jī)驅(qū)動(dòng)裝置，與3個(gè)電機(jī)驅(qū)動(dòng)裝置相連接的為運(yùn)動(dòng)鏈的主動(dòng)桿，在動(dòng)平臺(tái)3個(gè)邊中心點(diǎn)的球體和主動(dòng)桿末端的球體分別表示機(jī)構(gòu)的球面副，而每條支鏈上連接兩個(gè)球面副的連桿為機(jī)構(gòu)的從動(dòng)桿。

圖1 Delta機(jī)器人三維建模

在Delta機(jī)器人仿真機(jī)構(gòu)的基礎(chǔ)上，繪制了三維坐標(biāo)系。為了方便使用者進(jìn)行觀察和操作，首先選取坐標(biāo)系XOZ平面平行于靜平臺(tái)和動(dòng)平臺(tái)，x軸平行于靜平臺(tái)的一條三角邊，y軸的方向平行于靜平臺(tái)和動(dòng)平臺(tái)的公垂線，然后根據(jù)右手法則繪制出三維坐標(biāo)系。為使Delta機(jī)器人的運(yùn)動(dòng)范圍盡量保持在仿真系統(tǒng)平臺(tái)坐標(biāo)系的0 坐標(biāo)附近，選取當(dāng)3條主動(dòng)桿平行于動(dòng)平臺(tái)時(shí)，即3個(gè)電機(jī)驅(qū)動(dòng)的角度都為0 時(shí)，動(dòng)平臺(tái)所在的平面即為XOZ 所在的平面。

Delta機(jī)器人的機(jī)構(gòu)基本參數(shù)見表1。

表1 機(jī)器人基本參數(shù)

2 機(jī)器人運(yùn)動(dòng)學(xué)分析

如圖2所示，由點(diǎn)M1、M2、M3組成的Delta機(jī)器人的靜平臺(tái)，由N1、N2、N3組成的動(dòng)平臺(tái)，A1、A2、A3為靜平臺(tái)上驅(qū)動(dòng)機(jī)構(gòu)的中心點(diǎn)，同樣C1、C2、C3為連接動(dòng)平臺(tái)與從動(dòng)桿的球面副所在的中心點(diǎn)。R 為靜平臺(tái)內(nèi)切圓的半徑，r為動(dòng)平臺(tái)內(nèi)切圓的半徑，L1為主動(dòng)桿的長度，L2為從動(dòng)桿的長度，H 為靜平臺(tái)中心點(diǎn)O 到動(dòng)平臺(tái)中心點(diǎn)P的距離。根據(jù)Delta機(jī)器人的運(yùn)動(dòng)特性，將基坐標(biāo)系建立在靜平臺(tái)的中心點(diǎn)。

圖2 機(jī)器人運(yùn)動(dòng)學(xué)建模

2.1 運(yùn)動(dòng)學(xué)反解

根據(jù)機(jī)器人的末端位姿求解驅(qū)動(dòng)關(guān)節(jié)角度即為機(jī)器人運(yùn)動(dòng)學(xué)反解。Delta機(jī)器人的運(yùn)動(dòng)學(xué)反解即在已知?jiǎng)悠脚_(tái)中心點(diǎn)位置的情況下，求取3個(gè)驅(qū)動(dòng)桿轉(zhuǎn)動(dòng)角的問題。由于并聯(lián)機(jī)器人與串聯(lián)機(jī)器人本身結(jié)構(gòu)及運(yùn)動(dòng)特性的不同，傳統(tǒng)的D－H 分析法不能適用于對(duì)Delta機(jī)器人的運(yùn)動(dòng)學(xué)分析，所以用另一種方法對(duì)Delta機(jī)器人進(jìn)行了運(yùn)動(dòng)學(xué)算法的研究。

又

則根據(jù)式 （1）可知

由式 （2）可得到

設(shè)u1＝x ＋rcos－Rcos1，v1＝y(tǒng) ＋rsin－Rsin1，m1＝－2u1L1cos－2v1L1sin1，n1＝－2zL1，p1＝＋＋z2＋－，則

根據(jù)式 （4）可求得θ1的解。同理可求得θ2、θ3的解。

2.2 運(yùn)動(dòng)學(xué)正解

在已知機(jī)器人各關(guān)節(jié)角度的情況下，求解機(jī)器人的末端位姿即為機(jī)器人的正向運(yùn)動(dòng)學(xué)。在此Delta機(jī)器人模型中，已知3個(gè)驅(qū)動(dòng)桿的轉(zhuǎn)動(dòng)角分別為θ1、θ2、θ3，求解動(dòng)平臺(tái)的中心點(diǎn)的坐標(biāo)即是Delta機(jī)器人的運(yùn)動(dòng)學(xué)正解。

根據(jù)上面的式 （3），設(shè)φ1 ＝rcos1－Rcos1－L1cos1cosθ1，ω1＝rsin1－Rsin1－L1sin1cosθ1，ρ1 ＝－L1sinθ1，則

同理，可得

根據(jù)式（5）、式 （6）、式 （7）聯(lián)立組合成三元二次方程組，則求解此三元二次方程組即求得Delta機(jī)器人的運(yùn)動(dòng)學(xué)正解。由于機(jī)器人運(yùn)動(dòng)學(xué)建模中坐標(biāo)系的建立方法與Open－GL三維模型中的不同，系統(tǒng)將機(jī)器人在運(yùn)動(dòng)學(xué)模型坐標(biāo)系下的控制算法向OpenGL三維模型的坐標(biāo)系進(jìn)行了轉(zhuǎn)換。

3 系統(tǒng)平臺(tái)設(shè)計(jì)

系統(tǒng)平臺(tái)選擇在Windows系統(tǒng)上進(jìn)行開發(fā)，鑒于仿真平臺(tái)需要強(qiáng)有力的圖形工具做支撐，同時(shí)OpenGL 作為一種優(yōu)秀的圖形開發(fā)接口可以與Windows緊密結(jié)合。在VC平臺(tái)下使用MFC與OpenGL的混合編程進(jìn)行系統(tǒng)平臺(tái)的設(shè)計(jì)開發(fā)。

3.1 系統(tǒng)平臺(tái)結(jié)構(gòu)

如圖3所示，系統(tǒng)平臺(tái)分為基本菜單、機(jī)器人仿真主窗口、多視角窗口、功能操作窗口4 個(gè)部分?；静藛沃饕獞?yīng)用于仿真軌跡的切換等。功能操作窗口則完成對(duì)機(jī)器人軌跡參數(shù)的輸入和狀態(tài)控制。機(jī)器人主仿真窗口是操作者對(duì)機(jī)器人模型的主要觀察窗口，而多視角窗口則是為了滿足操作者可以從其它視角來對(duì)機(jī)器人進(jìn)行了全面的觀察分析。

圖3 系統(tǒng)仿真平臺(tái)

3.2 系統(tǒng)設(shè)計(jì)流程

如圖4所示，系統(tǒng)共有4個(gè)線程，其中主線程進(jìn)行系統(tǒng)任務(wù)的監(jiān)測，同時(shí)將分析后的任務(wù)消息傳送給子線程1。子線程1負(fù)責(zé)機(jī)器人運(yùn)動(dòng)控制算法的處理和主視口的刷新，同時(shí)向子線程2、子線程3發(fā)送刷新命令。線程2和線程3主要完成兩個(gè)不同視角的刷新任務(wù)。

圖4 系統(tǒng)控制流程

4 圖形軌跡算法及仿真驗(yàn)證

機(jī)器人軌跡是機(jī)器人研究領(lǐng)域中的重要內(nèi)容，是機(jī)器人圖形化編程的基礎(chǔ)。在對(duì)機(jī)器人進(jìn)行了OpenGL 三維建模和運(yùn)動(dòng)學(xué)建模之后，系統(tǒng)實(shí)現(xiàn)了機(jī)器人兩種類型圖形軌跡，并進(jìn)行了驗(yàn)證。

4.1 基本圖形軌跡

包括直線、圓／橢圓、正弦／余弦曲線3 種基本圖形軌跡。下面以圓形軌跡為例說明軌跡的相關(guān)算法。

假設(shè)運(yùn)動(dòng)前機(jī)器人末端位置為A（xA，yA，zA），圓形軌跡起點(diǎn)為B（xB，yB，zB）。令A(yù)B段軌跡速度為v，運(yùn)行時(shí)間為t，長度為，則AB段軌跡

用圓的參數(shù)方程表示更易于此軌跡的設(shè)計(jì)實(shí)現(xiàn)，假定當(dāng)前軌跡點(diǎn)對(duì)應(yīng)的角度為rad0，圓半徑為r，起始角度rad1，終止角度rad2，軌跡所在平面是繞Y 軸旋轉(zhuǎn)角度βy，繞Z軸旋轉(zhuǎn)角度為βz 所形成的平面。令軌跡起始點(diǎn)為x0y0z0，有以下方程

且

當(dāng)系統(tǒng)運(yùn)行時(shí)間為t，角速度為w 時(shí)，則rad0＝w·t，由此可知旋轉(zhuǎn)前的軌跡方程為

根據(jù)點(diǎn)在空間坐標(biāo)系中的映射算法，可以得到機(jī)器人末端的位置點(diǎn)，即圓形軌跡的表達(dá)式如下

4.2 復(fù)合圖形軌跡

除了基本圖形軌跡之外，實(shí)現(xiàn)了幾種復(fù)合軌跡，在基本圖形軌跡的基礎(chǔ)上，通過一種或者幾種軌跡的多次耦合而成。實(shí)現(xiàn)了包括平面復(fù)合軌跡 （扇形）和空間復(fù)合軌跡 （多邊形、六面體、球／橢球）。具體的實(shí)現(xiàn)流程如圖5所示。

圖5 復(fù)合軌跡控制算法

4.3 仿真驗(yàn)證

為了驗(yàn)證系統(tǒng)的運(yùn)動(dòng)控制和圖形軌跡算法，進(jìn)行了正弦曲線軌跡、橢圓曲線軌跡、扇形軌跡、六面體軌跡的仿真演示。根據(jù)如圖6所示仿真軌跡，可以驗(yàn)證仿真系統(tǒng)平臺(tái)的可靠性及運(yùn)動(dòng)控制算法和軌跡算法的正確性。

圖6 軌跡仿真結(jié)果

5 結(jié)束語

在Delta機(jī)器人的三維建模的基礎(chǔ)上，分析研究了其運(yùn)動(dòng)學(xué)算法，并在運(yùn)動(dòng)控制基礎(chǔ)上，結(jié)合機(jī)器人圖形軌跡的控制算法，建立了一套機(jī)器人軌跡仿真系統(tǒng)。仿真結(jié)果驗(yàn)證了運(yùn)動(dòng)學(xué)和軌跡算法的可靠性，直觀地展現(xiàn)了機(jī)器人運(yùn)行的圖形軌跡，仿真系統(tǒng)達(dá)到了對(duì)Delta機(jī)器人控制算法檢驗(yàn)和分析目的。圖形軌跡算法對(duì)Delta機(jī)器人的圖形化編程起到可靠的算法保證，仿真系統(tǒng)操作簡單，多視角仿真使操作者對(duì)機(jī)器人整體的運(yùn)動(dòng)情況有全方位的掌握，作為一種Delta機(jī)器人控制算法的驗(yàn)證工具，對(duì)圖形化編程的研究具較高的科研價(jià)值。

［1］GAO Mingwang，ZHANG Xianmin，LIU Han.3－RRR highspeed parallel robot kinematics design and experiment［J］.Robot，2013 （6）：716－722 （in Chinese）.［高名旺，張憲民，劉晗.3－RRR高速并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)設(shè)計(jì)與實(shí)驗(yàn) ［J］.機(jī)器人，2013 （6）：716－722.］

［2］Angel L，Bermudez J，Munoz O.Dynamic optimization and building of a parallel delta－type robot［C］／／IEEE International Conference on Proceedings of the Robotics and Biomimetics，2013.

［3］FENG Lihang，ZHANG Weigong，GONG Zongyang，et al.Progress and status of Delta series parallel robot research ［J］.Robot，2014，36 （3）：375－384 （in Chinese）. ［馮李航，張為公，龔宗洋，等.Delta 系列并聯(lián)機(jī)器人研究進(jìn)展與現(xiàn)狀［J］.機(jī)器人，2014，36 （3）：375－384.］

［4］Neto Pedro，Pires JN，Moreira AP.CAD－based off－line robot programming ［C］／／IEEE Conference on Robotics Automation and Mechatronics，2010：516－521.

［5］Neto P.Off－line programming and simulation from CAD drawings：Robot－assisted sheet metal bending ［C］／／39th Annual Conference of the IEEE Industrial Electronics Society，2013：4235－4240.

［6］Silke Lampe.A practical graphical based robot programming model in machine tending ［C］／／39th International Symposium on Robotics，2008：259－264.

［7］Li J，Xu A，Dudek G.Graphical state space programming：A visual programming paradigm for robot task specification［C］／／IEEE International Conference on Proceedings of the Robotics and Automation.IEEE，2011：4846－4853.

［8］Corke P. MATLAB toolboxes：Robotics and vision for students and teachers ［J］.IEEE Robotics ＆ Automation Magazine，2007，14 （4）：16－17.

［9］CHEN Wei，CHEN Yimin，WANG Di.Research on virtual simulation and control of robot［J］.Computer Engineering and Design，2010，31 （20）：4455－4458 （in Chinese）.［陳偉，陳一民，汪地，等.機(jī)器人虛擬仿真及控制技術(shù)研究 ［J］.計(jì)算機(jī)工程與設(shè)計(jì)，2010，31 （20）：4455－4458.］

［10］Xiang G，Kai Z，Xijuan G.Analysis of dynamic performance indices of 3－RPC parallel mechanism ［C］／／International Conference on Proceedings of the Environmental Science and Information Application Technology.IEEE，2010：78－82.