李 昂， 韓 康， 霍 琦， 于慧莉

(1.中國科學(xué)院 長春光學(xué)精密機(jī)械與物理研究所,吉林 長春 130033；2.國網(wǎng)吉林省電力有限公司 長春供電公司,吉林 長春 130000)

基于OpenMP的Stewart并聯(lián)機(jī)器人控制系統(tǒng)*

李 昂1， 韓 康1， 霍 琦1， 于慧莉2

(1.中國科學(xué)院 長春光學(xué)精密機(jī)械與物理研究所,吉林 長春 130033；2.國網(wǎng)吉林省電力有限公司 長春供電公司,吉林 長春 130000)

針對Stewart并聯(lián)機(jī)器人控制難度大的問題，實(shí)現(xiàn)了一種基于OpenMP的Stewart并聯(lián)機(jī)器人上位機(jī)控制系統(tǒng)，對機(jī)器人實(shí)現(xiàn)了快速有效直觀的振動(dòng)控制。系統(tǒng)共包含了動(dòng)力學(xué)解算模塊、數(shù)據(jù)傳輸模塊和人機(jī)交互模塊，不僅能夠準(zhǔn)確地計(jì)算出振動(dòng)數(shù)據(jù)，控制機(jī)器人振動(dòng)，而且也設(shè)計(jì)了簡潔明了的用戶界面，提升用戶體驗(yàn)。為了提高軟件的執(zhí)行效率，還加入了OpenMP多線程并行計(jì)算技術(shù)加速控制算法，最高達(dá)到了2.18倍的加速比。驗(yàn)證了軟件計(jì)算的正確性、控制的穩(wěn)定性和執(zhí)行的高效性。

并聯(lián)機(jī)器人； OpenMP； 并行計(jì)算； 控制軟件； 動(dòng)力學(xué)

0 引 言

Stewart平臺(tái)并聯(lián)機(jī)器人是一種廣受關(guān)注的機(jī)器人構(gòu)型，它具有精度高、剛度大、動(dòng)態(tài)響應(yīng)好等優(yōu)點(diǎn)，因而在航空航天等領(lǐng)域應(yīng)用較為廣泛[1]。Stewart平臺(tái)由上下兩個(gè)平臺(tái)構(gòu)成，其中，上平臺(tái)為動(dòng)平臺(tái)，下平臺(tái)為定平臺(tái)，兩個(gè)平臺(tái)間由6條支腿相連，支腿的長度是可變化的，6條支腿長度變化合成實(shí)現(xiàn)動(dòng)平臺(tái)的運(yùn)動(dòng)[2]?；赟tewart構(gòu)型的振動(dòng)機(jī)器人在工程應(yīng)用中有很高的實(shí)用性和研究意義[3]。

傳統(tǒng)機(jī)器人的振動(dòng)控制大都直接靠硬件實(shí)現(xiàn)，學(xué)習(xí)成本高且靈活性較低，本文針對這個(gè)問題實(shí)現(xiàn)了一種基于OpenMP技術(shù)的Stewart并聯(lián)機(jī)器人控制軟件，既能正確控制機(jī)器人振動(dòng)，也降低了機(jī)器人控制的難度[4]。

1 控制系統(tǒng)的總體設(shè)計(jì)

機(jī)器人控制系統(tǒng)由上位機(jī)控制系統(tǒng)+下位機(jī)數(shù)字信號(hào)處理器(digital signal processor,DSP)構(gòu)成，其中，下位機(jī)DSP負(fù)責(zé)接收控制系統(tǒng)傳來的振動(dòng)數(shù)據(jù)，并按照這些數(shù)據(jù)控制機(jī)器人各支腿的電流輸出，從而實(shí)現(xiàn)振動(dòng)。控制系統(tǒng)的核心為上位機(jī)軟件，通過軟件計(jì)算出一定初始條件下的振動(dòng)數(shù)據(jù)，并通過PC串口傳送給DSP，同時(shí)，控制系統(tǒng)實(shí)時(shí)接收由下位機(jī)傳回的機(jī)器人傳感器數(shù)據(jù)，以實(shí)現(xiàn)閉環(huán)控制。除了數(shù)據(jù)傳輸外，命令傳輸也由控制系統(tǒng)執(zhí)行。其流程圖如圖1所示。其功能圖如圖2所示。

2 控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)

2.1 數(shù)據(jù)通信與傳輸模塊

考慮到機(jī)器人控制信息吞吐量較低，且對信息可靠性要求較高，軟件選擇基于RS—232標(biāo)準(zhǔn)的串口通信方式。數(shù)據(jù)通信傳輸模塊的主要功能見圖2。

圖1 控制系統(tǒng)的系統(tǒng)流程圖Fig 1 System flowchart of control system

圖2 控制系統(tǒng)功能圖Fig 2 Function chart of control system

2.1.1 串口自動(dòng)查詢和連接

實(shí)際應(yīng)用中，為了達(dá)到更好的用戶體驗(yàn)，系統(tǒng)中串口的查詢和連接無需用戶操作，而是由后臺(tái)自動(dòng)完成的。Windows操作系統(tǒng)會(huì)將可用的串口信息存儲(chǔ)在注冊表的“HKEY_LOCAL_MACHINE/HARDWA RE/DEVICEMAP/SERIALCOMM”這一表項(xiàng)中，在控制平臺(tái)創(chuàng)建成功后，后臺(tái)會(huì)自動(dòng)訪問這一表項(xiàng)，找出可以連接的串口并讀取相應(yīng)的參數(shù)，若存在這樣的串口，則軟件會(huì)創(chuàng)建一個(gè)新的串口變量，并將連接參數(shù)賦給這個(gè)串口變量，之后的通信操作都由這個(gè)變量完成；若不存在這樣的串口，則重復(fù)執(zhí)行上述操作直到用戶選擇退出程序或者找到可用串口[5]。

2.1.2 數(shù)據(jù)和命令幀的封裝

為了防止數(shù)據(jù)在傳輸過程中丟失或出錯(cuò)，系統(tǒng)將待傳輸?shù)臄?shù)據(jù)封裝成幀，為了方便解析，系統(tǒng)將數(shù)據(jù)和命令采用同一種幀格式封裝，幀格式如表1所示。

表1 幀格式與各標(biāo)識(shí)長度表Tab 1 Format of frame and length of each mark

每個(gè)幀的長度為9～255字節(jié)，幀的類型定義了此幀是命令幀還是數(shù)據(jù)幀；幀的長度定義了幀的有效內(nèi)容有多少個(gè)字節(jié)，只有當(dāng)幀為數(shù)據(jù)幀時(shí)，這個(gè)標(biāo)識(shí)才有意義，若此幀為命令幀，則長度固定位1；校驗(yàn)和為對幀內(nèi)容的校驗(yàn)，確保傳輸內(nèi)容沒有出錯(cuò)；幀頭和幀尾均為固定的兩字節(jié)。幀封裝結(jié)束后，系統(tǒng)就會(huì)將其通過串口進(jìn)行發(fā)送。

2.2 人機(jī)交互模塊

本系統(tǒng)設(shè)計(jì)了以繪圖功能和數(shù)據(jù)存取功能為核心的功能模塊，提升用戶體驗(yàn)。

2.2.1 力值曲線繪制功能

動(dòng)力學(xué)逆解算出每條支腿的振動(dòng)數(shù)據(jù)，包括加速度值和力值[6]。但是這些值若未經(jīng)驗(yàn)證直接發(fā)送至DSP控制器上，就可能發(fā)生因錯(cuò)點(diǎn)而導(dǎo)致的振動(dòng)異常，在振動(dòng)過程中這種異常是很難準(zhǔn)確定位的，因此，在數(shù)據(jù)通信之前，將力值曲線繪制在圖片上，就能直觀地觀測到各個(gè)曲線的極值和趨勢等，盡量避免上述異常的發(fā)生。

本系統(tǒng)基于C#.Net平臺(tái)來設(shè)計(jì)用戶界面，其中繪圖功能的開發(fā)基于ZedGraph控件，它能夠繪制曲線圖和柱狀圖[7]。ZedGraph控件繼承自FrameWork框架中的UserControl Interface，因而它可以像C#.Net提供的其他控件一樣進(jìn)行拖放操作。當(dāng)動(dòng)力學(xué)逆解結(jié)束后，就會(huì)啟動(dòng)繪圖流程，首先，在ZedGraph控件中生成圖層，再在圖層中聲明六條曲線，每條曲線綁定一個(gè)力值數(shù)組；然后，將每條曲線的力值按照振動(dòng)的先后次序插入到各自的力值數(shù)組中；最后，檢測是否有點(diǎn)超出當(dāng)前的顯示范圍，若有，則重繪坐標(biāo)軸使其能夠正常顯示。

2.2.2 數(shù)據(jù)存取功能

并聯(lián)平臺(tái)振動(dòng)形式的復(fù)雜性決定了初值的細(xì)微不同可能會(huì)造成結(jié)果的巨大差異，為了實(shí)現(xiàn)振動(dòng)的可重復(fù)性，系統(tǒng)提供了振動(dòng)初始條件的存取功能。系統(tǒng)采用FileStream類提供的文件流讀寫方法對初始條件進(jìn)行保存和讀取[8]。

2.3 動(dòng)力學(xué)逆解模塊

2.3.1 動(dòng)力學(xué)逆解

動(dòng)力學(xué)逆解是指已知某一時(shí)刻并聯(lián)平臺(tái)的位姿，平臺(tái)速度和加速度，求解施加在每條支腿上的驅(qū)動(dòng)力大小[9]。設(shè)施加在鉸鏈上廣義力為

(1)

引入拉格朗日系數(shù)λ后，可推出廣義力以及相應(yīng)的廣義驅(qū)動(dòng)力之間關(guān)系的規(guī)范形式為

(2)

式中τ代表廣義驅(qū)動(dòng)力，m代表主動(dòng)關(guān)節(jié)的數(shù)目。在得到各個(gè)支腿的動(dòng)力學(xué)模型后，整理可得動(dòng)力學(xué)逆解方程

(3)

式中Qe為所求的廣義力[10]。

2.3.2 OpenMP并行計(jì)算

并聯(lián)平臺(tái)的一次振動(dòng)需要求多個(gè)振動(dòng)點(diǎn)的力值以形成至少一個(gè)周期的循環(huán)振動(dòng)。而每次求廣義力都要進(jìn)行一次動(dòng)力學(xué)逆解運(yùn)算，這個(gè)循環(huán)求解過程是極為耗時(shí)的。對這個(gè)循環(huán)進(jìn)行進(jìn)一步分析發(fā)現(xiàn)每次循環(huán)求解之間是相互獨(dú)立的，因此，考慮將這個(gè)循環(huán)進(jìn)行OpenMP并行分解加速程序執(zhí)行[11]。

OpenMP并行計(jì)算的原理是將循環(huán)體拆分成多個(gè)獨(dú)立的任務(wù)，這些任務(wù)由CPU或者GPU上的多個(gè)計(jì)算核心同時(shí)計(jì)算完成，其原理圖如圖3所示[12]。

圖3 并行計(jì)算原理圖Fig 3 Principle chart of parallel computing

從上圖中可以看出，串行計(jì)算時(shí)，設(shè)共有m×n個(gè)任務(wù)，每個(gè)任務(wù)的執(zhí)行時(shí)間為1，則串行計(jì)算總的計(jì)算時(shí)間為

Ts=m×n

(4)

采用并行計(jì)算后，將這些任務(wù)分配到m個(gè)線程上，每個(gè)線程執(zhí)行n個(gè)任務(wù)，則并行計(jì)算總的計(jì)算時(shí)間為

Tp=T1=T=…=Tm=Ts/m=n

(5)

可以看出：采用并行計(jì)算后，理論上總的計(jì)算時(shí)間縮短為原來的1/m，其中，m為參與并行計(jì)算的線程數(shù)目。這就大大提高了計(jì)算效率，縮短計(jì)算時(shí)間[13]。

3 并聯(lián)機(jī)器人控制軟件實(shí)驗(yàn)結(jié)果分析

為了驗(yàn)證力值運(yùn)算結(jié)果的正確性，軟件運(yùn)行了與Matlab軟件相同的一組算例，并將計(jì)算結(jié)果作對比，其結(jié)果如圖4、圖5所示[14]。

圖4 并聯(lián)機(jī)器人控制軟件運(yùn)算結(jié)果Fig 4 Computing result of parallel robots control software

圖5 Matlab運(yùn)算結(jié)果Fig 5 Computing result of Matlab

從圖中可以看出，每條支腿的曲線走勢，極值完全一致，進(jìn)一步，采用ultraCompare軟件對結(jié)果進(jìn)行對比，發(fā)現(xiàn)其極差小于等于10-6，證明系統(tǒng)的運(yùn)算結(jié)果是完全正確的。

最后，驗(yàn)證了OpenMP并行計(jì)算對于效率的提升，實(shí)驗(yàn)采用的計(jì)算機(jī)處理器為IntelCoreI7 2670QM處理器，其硬件規(guī)格為四核心八線程，實(shí)驗(yàn)結(jié)果如表2所示。

表2 并行計(jì)算時(shí)間表格Tab 2 Time of parallel computing

由表2中可以看出：在保證計(jì)算準(zhǔn)確性的前提下，開啟多線程并行計(jì)算后，計(jì)算時(shí)間會(huì)隨著參與并行計(jì)算線程數(shù)目的增加而縮短，其加速比也會(huì)隨之大幅增加?？梢?，并行計(jì)算對提升計(jì)算效率起到了很大作用。

4 結(jié)束語

本文設(shè)計(jì)并實(shí)現(xiàn)了基于OpenMP的Stewart并聯(lián)機(jī)器人控制系統(tǒng)。對系統(tǒng)中的各個(gè)重要功能模塊進(jìn)行了分析和說明。進(jìn)行了一系列實(shí)驗(yàn)驗(yàn)證了并行計(jì)算的正確性和高效性。機(jī)器人能夠在控制系統(tǒng)的操作下實(shí)現(xiàn)較為精確，靈活的振動(dòng)，為并聯(lián)機(jī)器人振動(dòng)的研究和發(fā)展奠定了良好的基礎(chǔ)。

[1] 高 洪，趙 韓.并聯(lián)機(jī)器人機(jī)構(gòu)學(xué)理論研究綜述[J].安徽工程科技學(xué)院學(xué)報(bào)：自然科學(xué)版, 2006(1)：73-79.

[2] Su Y X,Duan B Y,Zheng C H,et al.Disturbance-rejection high-precision motion control of a Stewart platform[J].Control Systems Technology,2004,12(3):364-374.

[3] 王 憲,楊國梁,張方生,等.基于牛頓—拉夫遜迭代法的6自由度機(jī)器人逆解算法[J].傳感器與微系統(tǒng),2010,29(10):116-118.

[4] 劉瑞梅，劉鐵英，韓曉毅.注冊表管理的C#方法[J].河北建筑工程學(xué)院學(xué)報(bào)，2009,27(4)：108-111.

[5] 翟希述，王寶興，范 淼.基于Visual C#的串口通信程序設(shè)計(jì)[J].電子科技，2011,24(2)：24-26.

[6] Aleksander G,Krzysztof H,Gabriel K,et al.Motion analysis of mechatronic equipment considering the example of the Stewart platform[J].Solid State Phenomena,2015,220-221:479-484.

[7] 朱亦鋼.應(yīng)用Zedgraph高效開發(fā)數(shù)據(jù)圖表[J].電腦編程技巧與維護(hù)，2009(12)：59-61.

[8] 曾 輝.基于C#的文件處理[J].軟件導(dǎo)刊，2006(9):44-45.

[9] 徐 鵬.六自由度并聯(lián)機(jī)構(gòu)Stewart平臺(tái)的動(dòng)力學(xué)建模與仿真[D].重慶：重慶大學(xué),2005.

[10] 單 鵬，謝里陽，田萬祿，等.基于D—H矩陣的Stewart型并聯(lián)機(jī)床位姿誤差計(jì)算模型[J].機(jī)械工程學(xué)報(bào)， 2010，46(17)：186-191.

[11] LaSalle D,Karypis G.Multi-threaded graph partitioning[C]∥IEEE the 27th International Symposium on Parallel & Distributed Processing(ISPDP),2013:225-223.

[12] 方先康.多核多線程處理器環(huán)境下軟件系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].北京：北京郵電大學(xué)，2008.

[13] 陳 昊.基于OpenMP的并行蟻群算法求解協(xié)同空戰(zhàn)火力分配[J].傳感器與微系統(tǒng),2013,32(1):20-24.

[14] 賈振元，褚宏飛，劉 巍，等.基于Stewart結(jié)構(gòu)六維大力傳感器的性能分析及結(jié)構(gòu)優(yōu)化[J].儀器儀表學(xué)報(bào)，2010,31(2)：341-346.

Control system of Stewart parallel robot based on OpenMP*

LI Ang1, HAN Kang1, HUO Qi1, YU Hui-li2

(1.Changchun Institute of Optics,Fine Mechanics and Physics,Chinese Academy of Science,Changchun 130033,China; 2.State Grid Electric Power Co in Changchun,Jilin Province Power Company,Changchun 130000,China)

Aiming at problem of great control difficulty of Stewart parallel robot,implement an upper PC control system of Stewart parallel robot based on openMP,which achieves fast,efficient and intuitive vibration control on robot.This system contains solution module of dynamics,data transmission module and Human-Computer Interaction module,it not only can accurately calculate vibration data,control robot vibration but also contains a concise User Interface to improve user’s experience.Multi-thread parallel computation technology acceleration control algorithm is also used to improve software efficiency based on OpenMP which achieves 2.18 speedups.A series of tests are executed to prove the correctness of software computation,stability of control and high efficiency of execution.

parallel robot; OpenMP; parallel computation; control software; dynamics

10.13873/J.1000—9787(2016)07—0067—03

2016—06—19

國家自然科學(xué)基金資助項(xiàng)目(11302222)；中國科學(xué)院長春光學(xué)精密機(jī)械與物理研究所創(chuàng)新基金資助項(xiàng)目(Y4CX1SS141)

TP 319

A

1000—9787(2016)07—0067—03

李 昂(1988-),男，吉林省白城人, 碩士,實(shí)習(xí)研究員，從事計(jì)算機(jī)科學(xué)與技術(shù)、高性能計(jì)算方向研究。