縫合機(jī)器人自主規(guī)劃與協(xié)調(diào)控制系統(tǒng)設(shè)計(jì)

何俊杰，李金鐘，王天琪，武永盛，侯仰強(qiáng)

（天津工業(yè)大學(xué) 天津市現(xiàn)代機(jī)電裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，天津 300387）

復(fù)合材料因其耐腐蝕、強(qiáng)度高、熱穩(wěn)定性強(qiáng)等特點(diǎn)[1]，已廣泛應(yīng)用于航空航天、汽車等行業(yè)[2].同時(shí)，隨著機(jī)器人技術(shù)的發(fā)展[3]，機(jī)器人逐步取代了人工操作，特別是在惡劣的工作環(huán)境下不僅安全性、穩(wěn)定性得到了很大的提升，更使得勞動(dòng)效率及勞動(dòng)成本得到了優(yōu)化，因此機(jī)器人縫合技術(shù)成為復(fù)合材料研究領(lǐng)域的焦點(diǎn).德國(guó)的KSL公司對(duì)基于工業(yè)機(jī)器人的三維復(fù)合材料機(jī)構(gòu)件的單邊縫合技術(shù)進(jìn)行了一定的研究[4-5]，該技術(shù)日趨成熟，已經(jīng)應(yīng)用到機(jī)翼、擋板等飛機(jī)部件的生產(chǎn)當(dāng)中，由于航空領(lǐng)域的敏感性，該技術(shù)已經(jīng)被封鎖；國(guó)內(nèi)關(guān)于單邊自動(dòng)化縫合的技術(shù)發(fā)展較晚，目前的縫合機(jī)器人為了提高其適用性已裝備了一些傳感器來感知周邊工作環(huán)境[6]，可以滿足一些簡(jiǎn)單平面形狀縫合預(yù)制件的自主縫合，但是對(duì)于一些曲面造型的縫合預(yù)制件工作方式仍多為人工示教再現(xiàn)的形式，其靈活性與柔性受到了很大限制.

本文針對(duì)空間曲面形狀的縫合預(yù)制件的自主縫合問題，通過三維掃描技術(shù)對(duì)縫合預(yù)制件進(jìn)行三維信息采集與處理得到接縫特征信息；并提出一種縫合機(jī)器人位姿自主規(guī)劃算法，對(duì)縫針位姿進(jìn)行規(guī)劃，進(jìn)而對(duì)三維織物進(jìn)行自動(dòng)化縫合.

1 系統(tǒng)的構(gòu)成及原理

縫合系統(tǒng)硬件主要由工控機(jī)、三維光柵掃描儀、ABB機(jī)器人和單邊縫合裝置組成，如圖1所示.其中視覺系統(tǒng)采用Eye-to-Hand[7]方案采集縫合預(yù)制件三維信息，通過對(duì)三維光柵掃描儀獲得的預(yù)制件整體點(diǎn)云數(shù)據(jù)進(jìn)行處理得到接縫特征點(diǎn)信息.

圖1 系統(tǒng)結(jié)構(gòu)圖Fig.1 Diagram of system structural

1.1 基于雙目視覺的三維光柵掃描儀

雙目視覺是三維掃描技術(shù)中最常用的技術(shù)之一，具有掃描效果好，精度高等優(yōu)點(diǎn)[8-9].其工作原理如圖2所示，通過計(jì)算空間中的一點(diǎn)P在2個(gè)攝像機(jī)上所成像的位置分別得出2條直線O1P、O2P方程，通過求解直線的交點(diǎn)，即可求得世界坐標(biāo)系中點(diǎn)的空間坐標(biāo).

圖2 雙目系統(tǒng)成像原理Fig.2 Imaging principle of binocular system

本文采用Cposmini三維光柵掃描儀對(duì)縫合預(yù)制件進(jìn)行掃描，在攝像機(jī)中間添加光柵設(shè)備，可以顯著提高系統(tǒng)掃描的精度與速度.通過固定位置的三維掃描設(shè)備與旋轉(zhuǎn)轉(zhuǎn)臺(tái)相互配合即可對(duì)整個(gè)縫合預(yù)制件的全部表面進(jìn)行掃描采集.

1.2 控制系統(tǒng)設(shè)計(jì)

采用具有中央處理單元功能的工控機(jī)作為系統(tǒng)控制的主要設(shè)備；中央處理單元用于處理獲得的預(yù)制件點(diǎn)云并在離線編程系統(tǒng)[10]中運(yùn)用自主規(guī)劃算法實(shí)現(xiàn)對(duì)縫針位姿的計(jì)算.單邊縫合設(shè)備作為執(zhí)行機(jī)構(gòu)裝載于ABB機(jī)器人末端，縫合工位控制采用Profibus現(xiàn)場(chǎng)總線技術(shù)組態(tài)S7-200xpsi與S7-300cn，由S7-300cn實(shí)現(xiàn)整個(gè)系統(tǒng)的協(xié)調(diào)，采用S7-200xpsi完成縫針運(yùn)動(dòng)控制，進(jìn)而通過對(duì)整個(gè)網(wǎng)絡(luò)的監(jiān)控完成縫針運(yùn)動(dòng)與機(jī)器人運(yùn)動(dòng)的協(xié)調(diào)控制.系統(tǒng)工作原理圖如圖3所示.

圖3 系統(tǒng)工作原理圖Fig.3 Principle of system

2 數(shù)據(jù)采集及自主規(guī)劃

2.1 接縫特征采集

本文通過cposmini三維光柵掃描儀對(duì)玻璃纖維縫合預(yù)制件點(diǎn)云數(shù)據(jù)進(jìn)行采集.預(yù)制件點(diǎn)云如圖4所示.

圖4 預(yù)制件點(diǎn)云Fig.4 Point cloud of preform

由圖4可以看出，由于所提取的預(yù)制件點(diǎn)云數(shù)據(jù)接縫處存在較大的曲率變化值，本文運(yùn)用高斯曲率極值點(diǎn)對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行處理[11]；設(shè)j1、j2為點(diǎn)云數(shù)據(jù)上一點(diǎn)的2個(gè)主曲率，j=j1×j2即為該點(diǎn)的高斯曲率，高斯曲率體現(xiàn)的是曲面的彎曲程度，對(duì)于散亂點(diǎn)云特征點(diǎn)的提取具有較現(xiàn)實(shí)的意義；由于織物厚度的關(guān)系，織物搭接處存在2條曲率存在較大變化的位置；在求得2處點(diǎn)云特征點(diǎn)的情況下，通過求取2處特征點(diǎn)的坐標(biāo)平均值作為接縫實(shí)際位置，并通過一定的偏移技巧求出實(shí)際的縫合位置的點(diǎn)云數(shù)據(jù)，對(duì)獲得的點(diǎn)云進(jìn)行處理，結(jié)果如圖5所示.

圖5 縫合處點(diǎn)云坐標(biāo)Fig.5 Coordinates of Stitching

2.2 縫合機(jī)器人縫合軌跡規(guī)劃

得到的接縫中心處點(diǎn)云后，在所獲得的預(yù)制件點(diǎn)云的基礎(chǔ)上分別求取與接縫處中心點(diǎn)云相距不同距離的3處點(diǎn)云，以中間位置的點(diǎn)云位置作為實(shí)際縫合位置，兩邊點(diǎn)云位置作為輔助縫合位置，根據(jù)3處點(diǎn)云數(shù)據(jù)，以中間點(diǎn)云處數(shù)據(jù)為主，如圖6所示.

圖6 鄰近點(diǎn)分布圖Fig.6 Distribution of adjacent points

根據(jù)最鄰近點(diǎn)算法分別取其周圍8個(gè)鄰近點(diǎn)進(jìn)行平面擬合，求取所擬合平面的垂向量作為位姿坐標(biāo)系的Z軸，該點(diǎn)到下一點(diǎn)的連線作為位姿坐標(biāo)系的X軸方向，依據(jù)右手定則即可求出位姿坐標(biāo)系的Y軸方向矢量；求取后的位姿坐標(biāo)系用矩陣的方式表示為T.

在求得預(yù)制件表面處的位姿矩陣后，依據(jù)機(jī)器人手眼關(guān)系將位姿矩陣轉(zhuǎn)化為機(jī)器人坐標(biāo)系下的矩陣；手眼關(guān)系的標(biāo)定可通過對(duì)攝像機(jī)內(nèi)外參數(shù)及手眼關(guān)系[12-13]的標(biāo)定建立.令求得的手眼關(guān)系矩陣為M，則機(jī)器人坐標(biāo)系下位姿矩陣

式中：

A為相機(jī)坐標(biāo)系相對(duì)于機(jī)器人基坐標(biāo)系之間的旋轉(zhuǎn)矩陣；T 為平移矩陣；Px、Py、Pz為該點(diǎn)在機(jī)器人坐標(biāo)系下的坐標(biāo)；通過計(jì)算中心處點(diǎn)云上每一點(diǎn)在機(jī)器人坐標(biāo)系下的位姿矩陣并將其傳輸?shù)綑C(jī)器人控制系統(tǒng)中，進(jìn)而對(duì)機(jī)器人的縫合路徑進(jìn)行規(guī)劃.

3 協(xié)調(diào)控制系統(tǒng)設(shè)計(jì)

3.1 控制系統(tǒng)硬件的選取

由工控機(jī)將規(guī)劃所得位姿信息以離線編程的形式傳遞給機(jī)器人，使機(jī)器人獲得在各縫合工位處的路徑點(diǎn)信息及縫針姿態(tài)信息，進(jìn)而將機(jī)器人運(yùn)動(dòng)與縫針運(yùn)動(dòng)相結(jié)合即可完成縫合工作.

為了保證系統(tǒng)穩(wěn)定性，采用具有高穩(wěn)定性以及強(qiáng)抗干擾能力的西門子PLC作為核心控制單元.選用西門子公司S7-300CN作為機(jī)器人運(yùn)動(dòng)控制單元，通過Profibus現(xiàn)場(chǎng)總線技術(shù)建立機(jī)器人與PLC之間的通訊.伺服放大器采用三菱公司生產(chǎn)的MR-J2S-70A型伺服電機(jī)驅(qū)動(dòng)器，S7-200xpsi作為控制單邊縫合裝置縫針運(yùn)動(dòng)的控制單元，通過上位控制器S7-200xpsi發(fā)送的脈沖序列來控制伺服電機(jī)轉(zhuǎn)速及位置[14].S7-300cn與S7-200xpsi的Profibus-DP通訊[15]由EM277擴(kuò)展模塊完成.

3.2 組態(tài)對(duì)象分析

針對(duì)本系統(tǒng)的硬件配置及保證控制效率與響應(yīng)效率，采用單主站結(jié)構(gòu)形式，“一主多從”網(wǎng)絡(luò)控制策略.將S7-300CN作為控制系統(tǒng)主站，并通過Profibus-DP現(xiàn)場(chǎng)總線依次串行連接S7-200xpsi、MP277觸摸屏以及ABB機(jī)器人，即可將系統(tǒng)中各離散的分布式I/O與智能設(shè)備連接到一起，從而作為網(wǎng)絡(luò)通信主站的S7-300 PLC通過Profibus-DP網(wǎng)絡(luò)管理各個(gè)設(shè)備之間的數(shù)據(jù)交換，實(shí)現(xiàn)對(duì)整體系統(tǒng)的協(xié)調(diào)控制.系統(tǒng)結(jié)構(gòu)如圖7所示.

圖7 PLC與觸摸屏通訊連接Fig.7 Connection between touch panel and PLC

3.3 系統(tǒng)硬件電路圖

本文設(shè)計(jì)的縫合機(jī)器人協(xié)調(diào)控制系統(tǒng)選用1臺(tái)S7-300CPU，1臺(tái) S7-200CPU，1個(gè) MP277觸摸屏，1臺(tái)伺服驅(qū)動(dòng)器，1個(gè)驅(qū)動(dòng)單邊縫合裝置的伺服電機(jī)，1臺(tái)ABB機(jī)器人.控制系統(tǒng)電源輸入端為單相220 V，輸出電源共分為兩部分，一部分通過交流接觸器，用于供給伺服放大器，驅(qū)動(dòng)伺服電機(jī)運(yùn)轉(zhuǎn)，另一部分通過AC-DC電源，輸出直流24 V，用于給PLC核心控制單元進(jìn)行供電，控制系統(tǒng)硬件構(gòu)成如圖8所示.圖8中L1、L2、L11、L12均為交流接觸器接口.

3.4 PLC之間的通訊

為了對(duì)PLC的工作進(jìn)行監(jiān)控，基于通訊方便、穩(wěn)定性的考量，本文采用prifibus-DP通訊來實(shí)現(xiàn)PLC之間的通訊；該通信協(xié)議是一種主—從協(xié)議；工作過程中主站可以向從站發(fā)出命令信息，從站對(duì)主站發(fā)出的信息作出響應(yīng)；基于該特點(diǎn)，在通訊過程中，主站可與較多數(shù)目的從站之間進(jìn)行通訊，是一種較為經(jīng)濟(jì)的總線技術(shù).

采用STEP7-Micro/WIN V4.0 SP7與STEP7 V5.4 SP4 HF3軟件對(duì)PLC進(jìn)行編程，通過PC/MPI適配器及PC/PPI通訊線纜將程序由工控機(jī)下載到PLC中，實(shí)現(xiàn)對(duì)縫合過程中各環(huán)節(jié)工藝的精準(zhǔn)控制及縫針與機(jī)器人運(yùn)動(dòng)的協(xié)調(diào)控制，主程序流程圖如圖9所示.

圖9 主程序流程圖Fig.9 Chart of main program

4 實(shí)驗(yàn)結(jié)果及分析

實(shí)驗(yàn)所采用預(yù)制件材料為厚度3 mm的玻璃纖維，采集并處理得到的預(yù)制件點(diǎn)云數(shù)據(jù)，規(guī)劃得到機(jī)器人在13個(gè)工位處的路徑點(diǎn)坐標(biāo)，結(jié)果如表1所示.

表1 縫合路徑點(diǎn)Tab.1 Points of suture path

對(duì)表1中各點(diǎn)進(jìn)行位姿矩陣求解，將結(jié)果以傳輸給機(jī)器人并對(duì)預(yù)制件進(jìn)行縫合實(shí)驗(yàn)，縫合結(jié)果如圖10所示；實(shí)驗(yàn)結(jié)束后沿縫合中心線對(duì)縫合機(jī)器人進(jìn)行示教，將實(shí)際縫合點(diǎn)與計(jì)算點(diǎn)導(dǎo)入三維逆向軟件Imageware進(jìn)行線性擬合并進(jìn)行軌跡誤差比對(duì)如圖11所示，軌跡偏差總體小于1.5 mm，滿足精度要求.

圖10 縫合實(shí)驗(yàn)Fig.10 Experimental of stitching

圖11 軌跡偏差對(duì)比Fig.11 Result of trajectory deviation

5 結(jié)語(yǔ)

（1）本文采用三維掃描技術(shù)，對(duì)縫合預(yù)制件的三維信息進(jìn)行采集并對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行處理，提出了一種縫合機(jī)器人自主規(guī)劃算法，實(shí)現(xiàn)了對(duì)縫合機(jī)器人工作路徑位姿矩陣的求解.

（2）基于Profibus現(xiàn)場(chǎng)總線通信技術(shù)，采用“一主多從”網(wǎng)絡(luò)控制策略，應(yīng)用西門子PLC設(shè)計(jì)縫針運(yùn)動(dòng)與機(jī)器人運(yùn)動(dòng)協(xié)調(diào)控制系統(tǒng)，解決了每一縫合工位縫針運(yùn)動(dòng)與機(jī)器人運(yùn)動(dòng)的協(xié)調(diào)控制問題.

參考文獻(xiàn)：

[1]匡寧，陳同海，錢育勝.中空復(fù)合材料的成型工藝及應(yīng)用進(jìn)展[J].工程塑料應(yīng)用，2015，43（1）：120-123.KUANG N，CHEN T H，QIAN Y S.Progress in molding technology and application of hollow composites[J].Engineering Plasitcs Application，2015，43（1）：120-123（in Chinese）.

[2]程小全，酈正能，趙龍.縫合復(fù)合材料制備工藝和力學(xué)性能研究[J].力學(xué)進(jìn)展，2009，39（1）：89-102.CHENG X Q，LI Z N，ZHAO L.Fabricating process and mechanical property research of stitched composite structures[J].Advances in Mechanics，2009，39（1）：89-102（in Chinese）.

[3]王田苗，陶永.我國(guó)工業(yè)機(jī)器人技術(shù)現(xiàn)狀與產(chǎn)業(yè)化發(fā)展戰(zhàn)略[J].機(jī)械工程學(xué)報(bào)，2014，50（9）：1-13.WANG T M，TAO Y.Research status and industrialization development strategy of Chinese industrial robot[J].Journal of Mechanical Engineering，2014，50（9）：1-13（in Chinese）.

[4]DREEHSLER Klaus.Latest development in stitching and braiding technologies for textile prefroming[J].Materials and Processing Technology，2004，5：2073-2085.

[5]IINOM.Electroniccomputer：US，USD396849[P].1998-07-13.

[6]姜海珍，李亮玉，范芳蕾，等.三維預(yù)制件縫合機(jī)器人視覺跟蹤系統(tǒng)[J].紡織學(xué)報(bào)，2008，29（10）：113-116.

JIANG H Z，LI L Y，F(xiàn)AN F L，et al.Vision tracking system of 3-D preform stitching robot[J].Journal of Textile Research，2008，29（10）：113-116（in Chinese）.

[7]PAN Hui，WANG Nali，QIN Yinshi.A closed-form solution to eye-to-hand calibration towards visual grasping[J].Industrial Robot，2014，41（6）：567-574.

[8]KROTKOV E，BAJCSY R.Active vision for reliable ranging：Cooperating focus，stereo，and vergence[J].International Journal of Computer Vision，1993，11（2）：187-203.

[9]MATT C，JASNA M，REBECCA L.An advantage for active versus passive aperture-viewing in visual object recognition[J].Perception，2011，40（10）：1154-1163.

[10]秦濤.基于結(jié)構(gòu)光視覺傳感器的焊縫實(shí)時(shí)跟蹤控制[D].上海：上海交通大學(xué)，2012 QIN T.Real-time control of welding seam tracking based on vision sensor with structural stripe[D].Shanghai：Shanghai Jiaotong University，2012（in Chinese）.

[11]GUMHOLD S.Feature extraction from point clouds[J].Proc Ofimr，2001，25（7）：293-305.

[12]邱茂林，馬頌德，李毅.計(jì)算機(jī)視覺中攝像機(jī)定標(biāo)綜述[J].自動(dòng)化學(xué)報(bào)，2000，26（1）：47-59.QIU M L，MA S D，LI Y.The review of the camera calibration in computer vision[J].Acta Automatic Sinica，2000，26（1）：47-59（in Chinese）.

[13]沈愛華，羅印升，宋偉.一種基于雙目視覺的手眼標(biāo)定方法[J].機(jī)械設(shè)計(jì)與制造工程，2016，45（5）：42-46.SHEN A H，LUO Y S，SONG W.An approach to hand-eye calibration based on stereo vision[J].Machine Design and Manufacturing Engineering，2016，45（5）：42-46（in Chinese）.

[14]董明，邵偉，陸藺輝.基于S7-200和V80伺服定位系統(tǒng)設(shè)計(jì)[J].制造業(yè)自動(dòng)化，2014，36（7）：105-116.DONG M，SHAO W，LU L H.The design of position servo control system based on S7-200 and V80[J].Manufacturing Automation，2014，36（7）：105-116（in Chinese）.

[15]王勇.基于PLC與PROFIBUS的柔性制造生產(chǎn)線控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].蘇州：蘇州大學(xué)，2009.WANG Y.Design and realization of controlling system of flexible manufacturing line based on PLC and PROFIBUS[D].Suzhou：Soochow University，2009（in Chinese）.