趙阿朋,王雪竹,張美玲, 康林清,樊繼壯

(1.中廣核研究院有限公司, 廣東 深圳 518000;2.哈爾濱工業(yè)大學(xué) 機(jī)電工程學(xué)院 哈爾濱 150008)

蒸汽發(fā)生器作為壓水堆核電站一回路設(shè)備中的核心裝置之一,在核電站發(fā)電過程中起到一、二回路間的能量傳輸作用.由于其內(nèi)部一回路側(cè)的傳熱管在工作中長(zhǎng)期承受高溫、高壓和硼酸水腐蝕,容易導(dǎo)致管板、傳熱管以及水室發(fā)生泄漏、破裂和生銹等異常狀況,給核電設(shè)備安全運(yùn)行帶來隱患[1-2].因此為減少安全風(fēng)險(xiǎn),避免非計(jì)劃停堆,本課題組研制了一種新型串聯(lián)作業(yè)機(jī)械臂,使用時(shí)可將其倒掛安裝在蒸汽發(fā)生器的管板上,在其末端可搭載多種作業(yè)工具對(duì)蒸汽發(fā)生器進(jìn)行視頻檢測(cè)、打磨、拋光等修復(fù)作業(yè).

目前,國(guó)內(nèi)外學(xué)者對(duì)于機(jī)械臂軌跡規(guī)劃算法進(jìn)行了深入研究并取得了眾多成果.Bailón W P[3]使用八次多項(xiàng)式插值法進(jìn)行機(jī)械臂末端軌跡規(guī)劃,雖然能夠得到更為光滑的運(yùn)動(dòng)參數(shù)曲線,但是計(jì)算過程較為復(fù)雜且軌跡精度難以保證.Altabey W A[4]利用五次多項(xiàng)式插值法得到了連續(xù)且光滑的運(yùn)動(dòng)參數(shù)曲線,在降低計(jì)算量的同時(shí)保證了軌跡精度.Gasparetto A[5]采用五次B樣條插值法進(jìn)行規(guī)劃,得到了加速度曲線不存在拐點(diǎn)的運(yùn)動(dòng)參數(shù)曲線,但該方法的計(jì)算量較大,可能會(huì)影響軌跡精度.喬亮[6]通過總結(jié)現(xiàn)有方法的不足,提出了三階參數(shù)樣條插值,通過參數(shù)選取解決軌跡曲率以及拐點(diǎn)問題,并進(jìn)行仿真驗(yàn)證了算法的可行性.

本文介紹了核電站蒸汽發(fā)生器的檢修任務(wù)工況和作業(yè)機(jī)械臂的作業(yè)過程,在此基礎(chǔ)上設(shè)計(jì)了作業(yè)機(jī)械臂的控制系統(tǒng),根據(jù)檢修任務(wù)需求,研究了作業(yè)機(jī)械臂在關(guān)節(jié)空間和笛卡爾坐標(biāo)系空間下的軌跡規(guī)劃算法,并基于霍夫變換圓原理實(shí)現(xiàn)了作業(yè)機(jī)械臂對(duì)管板圓孔的識(shí)別定位.通過實(shí)驗(yàn)驗(yàn)證了軌跡規(guī)劃算法和圓孔識(shí)別定位的有效性.

1 蒸汽發(fā)生器檢修工況

蒸汽發(fā)生器的整體結(jié)構(gòu)由下方的半球形水室和數(shù)千根倒置的U形傳熱管共同組成.半球形水室與一回路側(cè)U形傳熱管相連,中間有隔板隔開,形成兩個(gè)四分之一的水室,上方有約厚557 mm的管板,管板上有數(shù)千個(gè)直徑約為20 mm的通孔,倒置的U形管脹緊在通孔內(nèi),如圖1(A)所示.在蒸汽發(fā)生器長(zhǎng)期運(yùn)行的過程中,需要定期停堆對(duì)其內(nèi)部的管板、傳熱管以及水室進(jìn)行視頻檢測(cè)、打磨、拋光等修復(fù)作業(yè),由于蒸汽發(fā)生器內(nèi)部具有較強(qiáng)核輻照劑量,采用人工作業(yè)具有極高的安全風(fēng)險(xiǎn),因此課題組研制了一種作業(yè)機(jī)械臂,代替人工完成此項(xiàng)檢修任務(wù),提升作業(yè)安全性和自動(dòng)化作業(yè)水平.

圖1 蒸汽發(fā)生器和作業(yè)機(jī)械臂結(jié)構(gòu)示意圖

檢修作業(yè)機(jī)械臂結(jié)構(gòu)設(shè)計(jì)如圖1(B)所示,采用六自由度串聯(lián)結(jié)構(gòu)設(shè)計(jì)方案,各個(gè)自由度均采用驅(qū)控一體的模塊化關(guān)節(jié)驅(qū)動(dòng),以便于機(jī)械臂的維護(hù)和檢修.

作業(yè)機(jī)械臂在執(zhí)行檢修任務(wù)前,如圖2所示,先由地面小車抓持機(jī)械臂的末端,通過蒸汽發(fā)生器上的人孔送入水室,并將機(jī)械臂的底座吊掛安裝固定在管板上,然后通過遠(yuǎn)程控制軟件操控,配合多種檢修工具,完成對(duì)U形管、管板以及整個(gè)水室的打磨拋光、無損檢測(cè)、脹管、傳熱管清潔、傳熱管內(nèi)徑測(cè)量、襯管激光焊接等作業(yè).

圖2 作業(yè)機(jī)械安裝、檢修過程

2 作業(yè)機(jī)械臂控制系統(tǒng)設(shè)計(jì)

2.1 控制系統(tǒng)硬件設(shè)計(jì)

本文的控制系統(tǒng)采用分布式,將上位機(jī)和下位機(jī)分別進(jìn)行控制,作業(yè)機(jī)械臂控制器為主站,進(jìn)行運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)等算法計(jì)算,電機(jī)驅(qū)動(dòng)器為從站,安裝在各個(gè)模塊關(guān)節(jié)的內(nèi)部,以驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng),并實(shí)時(shí)反饋關(guān)節(jié)信息.采用EtherCAT總線來實(shí)現(xiàn)作業(yè)機(jī)械臂主站和從站之間的通訊,進(jìn)行實(shí)時(shí)的數(shù)據(jù)傳輸.作業(yè)機(jī)械臂的控制系統(tǒng),需要完成機(jī)械臂在蒸汽發(fā)生器水室內(nèi)的位置姿態(tài)、軌跡規(guī)劃以及視覺圓孔點(diǎn)位識(shí)別等計(jì)算,作業(yè)機(jī)械臂硬件系統(tǒng)框圖及作業(yè)機(jī)械臂控制系統(tǒng)方案如圖3、4所示.

圖3 作業(yè)機(jī)械臂硬件系統(tǒng)框圖

圖4 作業(yè)機(jī)械臂控制系統(tǒng)框圖

主站是整個(gè)作業(yè)機(jī)械臂控制系統(tǒng)的核心,需要進(jìn)行大量的數(shù)據(jù)計(jì)算,包括位姿、軌跡規(guī)劃、圓孔定位等一些重要的算法計(jì)算.其通過EtherCAT總線接受來自上位機(jī)傳遞的控制命令,進(jìn)行計(jì)算求解之后,通過EtherCAT總線將控制指令發(fā)送給各個(gè)模塊關(guān)節(jié)的驅(qū)動(dòng)器,從而驅(qū)動(dòng)各個(gè)關(guān)節(jié)完成相對(duì)應(yīng)的指令,同時(shí)將各個(gè)模塊關(guān)節(jié)所處的轉(zhuǎn)動(dòng)角度等狀態(tài)帶回給上位機(jī),得到一個(gè)實(shí)時(shí)的反饋.因此對(duì)主站的計(jì)算能力要求很高,同時(shí)還需要豐富的接口,來滿足實(shí)際的需求.本文采用納博特科技研發(fā)的NRC系列控制器,尺寸為45 mm×230 mm×165 mm.該控制器配備了Intel下的Bay Trail Celeron處理器,8GB系統(tǒng)內(nèi)存,以及豐富的外設(shè)接口.

2.2 控制系統(tǒng)上位機(jī)設(shè)計(jì)

由前文可知,作業(yè)機(jī)械臂由六個(gè)模塊關(guān)節(jié)、連接件、控制器和上位機(jī)組成,機(jī)械臂的運(yùn)動(dòng)模式由上位機(jī)軟件進(jìn)行控制,因此上位機(jī)軟件在作業(yè)機(jī)械臂控制系統(tǒng)中占重要位置.

本文上位機(jī)軟件系統(tǒng)采用Qt編寫,主要用于為操作者提供友好全面的作業(yè)機(jī)械臂軟件操作界面,用戶通過該軟件對(duì)作業(yè)機(jī)械臂進(jìn)行遠(yuǎn)程操作和實(shí)時(shí)控制,從而實(shí)現(xiàn)的對(duì)蒸發(fā)發(fā)生器管板及水室的檢查和修復(fù),如圖5所示.

圖5 上位機(jī)軟件控制界面

上位機(jī)軟件界面主要包括了通過IP連接機(jī)械臂、設(shè)置運(yùn)動(dòng)速度、點(diǎn)動(dòng)、多關(guān)節(jié)聯(lián)動(dòng)、圓孔視覺識(shí)別與定位、圓弧與直線規(guī)劃運(yùn)動(dòng)、獲取當(dāng)前關(guān)節(jié)位置與緊急下電等功能模塊.操作上位機(jī)軟件時(shí),首先通過EtherCAT連接到各個(gè)模塊關(guān)節(jié),點(diǎn)擊連接,等待響應(yīng),連接成功之后,上位機(jī)與主站建立了聯(lián)系,可以通過上位機(jī)發(fā)送指令,對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)控制.本文設(shè)計(jì)的上位機(jī)包含以下功能模塊:

1)回原點(diǎn)

回原點(diǎn)操作即是復(fù)位的操作,作業(yè)機(jī)械臂在搭建過程中,關(guān)節(jié)之間有一個(gè)相對(duì)位置.為了操作方便,在每次執(zhí)行完任務(wù)之后,可以執(zhí)行回原點(diǎn)的操作,方便下一次作業(yè),同時(shí)也方便觀察機(jī)械臂的狀態(tài),防止出現(xiàn)碰撞.

2)單關(guān)節(jié)點(diǎn)動(dòng)

單關(guān)節(jié)點(diǎn)動(dòng)是作業(yè)機(jī)械臂運(yùn)動(dòng)控制的基本功能.選擇點(diǎn)動(dòng)的關(guān)節(jié),輸入轉(zhuǎn)動(dòng)的關(guān)節(jié)角度,然后點(diǎn)擊動(dòng)作按鈕即可.通過修改速度,可以改變關(guān)節(jié)點(diǎn)動(dòng)的速度,速度變化范圍為0～10°/s.點(diǎn)擊獲取點(diǎn)位按鈕,可以實(shí)時(shí)得到關(guān)節(jié)的變化角度.

3)多關(guān)節(jié)聯(lián)動(dòng)

多關(guān)節(jié)聯(lián)動(dòng)功能是同時(shí)控制六個(gè)模塊關(guān)節(jié)運(yùn)動(dòng),可以實(shí)現(xiàn)作業(yè)機(jī)械臂末端點(diǎn)到點(diǎn)的運(yùn)動(dòng).首先給定各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的角度,并設(shè)置目標(biāo)速度,然后點(diǎn)擊運(yùn)動(dòng)按鈕,即可實(shí)現(xiàn)關(guān)節(jié)聯(lián)動(dòng)功能.同時(shí),點(diǎn)擊當(dāng)前位置按鈕,上位機(jī)軟件還能通過讀取絕對(duì)值編碼器值,反饋出各個(gè)關(guān)節(jié)目前所處的位置狀態(tài).

4)逆運(yùn)動(dòng)學(xué)求解

在該功能模塊中,輸入作業(yè)機(jī)械臂末端在基坐標(biāo)系下的位置坐標(biāo)和姿態(tài)角度,即橫滾、俯仰與偏轉(zhuǎn)角度,運(yùn)用機(jī)器人的逆運(yùn)動(dòng)學(xué)公式求解出各個(gè)關(guān)節(jié)需要轉(zhuǎn)動(dòng)的角度,在軟件中顯示出來.點(diǎn)擊開始運(yùn)動(dòng)按鈕之后,作業(yè)機(jī)械臂即從當(dāng)前位置運(yùn)動(dòng)到所給定的位姿.

5)圓孔識(shí)別定位

作業(yè)機(jī)械臂末端運(yùn)動(dòng)到管板正下方,通過相機(jī)抓拍一張管板管孔深度彩色圖片,基于霍夫變換圓檢測(cè)技術(shù),可得到各個(gè)圓孔圓心在相機(jī)坐標(biāo)系下的位置,通過位姿矩陣變換,得到管孔圓心在作業(yè)機(jī)械臂基坐標(biāo)系下的位置坐標(biāo)和姿態(tài),運(yùn)用所推導(dǎo)的逆運(yùn)動(dòng)學(xué)求解公式得到機(jī)械臂末端工具運(yùn)動(dòng)到管孔圓心的各個(gè)關(guān)節(jié)角度變量,然后點(diǎn)擊圓孔定位.

6)軌跡規(guī)劃運(yùn)動(dòng)

在使用前需要轉(zhuǎn)到連續(xù)運(yùn)動(dòng)模式,該功能模塊分成兩部分,一部分是機(jī)械臂軌跡規(guī)劃仿真,另一部分是樣機(jī)軌跡規(guī)劃運(yùn)動(dòng).首先,通過RemoteAPI將Qt與仿真軟件Vrep進(jìn)行通訊,將求解得到軌跡規(guī)劃數(shù)據(jù)傳給Vrep,控制作業(yè)機(jī)械臂模型進(jìn)行運(yùn)動(dòng),通過觀察機(jī)械臂在水室中的運(yùn)動(dòng)狀態(tài),主要觀察是否發(fā)生碰撞、關(guān)節(jié)運(yùn)動(dòng)是否異常、末端狀態(tài)等.在保證仿真驗(yàn)證不出問題的前提下,再將軌跡規(guī)劃數(shù)據(jù)通過EtherCAT總線傳給機(jī)械臂進(jìn)行運(yùn)動(dòng)控制.

3 作業(yè)機(jī)械臂運(yùn)動(dòng)軌跡規(guī)劃

根據(jù)檢修任務(wù)需求,可以將作業(yè)機(jī)械臂軌跡運(yùn)動(dòng)形式分為點(diǎn)對(duì)點(diǎn)運(yùn)動(dòng)和連續(xù)路徑運(yùn)動(dòng).點(diǎn)對(duì)點(diǎn)運(yùn)動(dòng)用于管板圓孔的檢修作業(yè),通過關(guān)節(jié)空間軌跡規(guī)劃算法實(shí)現(xiàn);連續(xù)路徑運(yùn)動(dòng)用于管板和水室內(nèi)壁加工面的檢測(cè)、焊接等作業(yè),通過笛卡爾坐標(biāo)系空間軌跡規(guī)劃算法實(shí)現(xiàn)[7-9].

3.1 關(guān)節(jié)空間軌跡規(guī)劃算法

關(guān)節(jié)空間軌跡規(guī)劃算法需要給定作業(yè)機(jī)械臂末端作業(yè)工具的起始位姿和目標(biāo)位姿,利用逆運(yùn)動(dòng)學(xué)求解公式得到各個(gè)關(guān)節(jié)起點(diǎn)和終點(diǎn)的角度,對(duì)各個(gè)關(guān)節(jié)起點(diǎn)和終點(diǎn)的角度進(jìn)行插補(bǔ),得到關(guān)節(jié)變化的連續(xù)軌跡.由于需要對(duì)速度和加速度進(jìn)行約束,本文選擇五次多項(xiàng)式插值規(guī)劃算法.

五次多項(xiàng)式的通式為式(1),有6個(gè)待定系數(shù).給定作業(yè)機(jī)械臂末端工具在起始點(diǎn)和目標(biāo)點(diǎn)下各個(gè)關(guān)節(jié)的角度、速度以及加速度,根據(jù)五次多項(xiàng)式的一階導(dǎo)、二階導(dǎo),如式(2)所示,可列出六個(gè)方程,如式(3)所示,從而求解得到五次多項(xiàng)式.

q(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(1)

(2)

(3)

聯(lián)立式(1)～(3)可求解待定系數(shù)分別為:

同樣的,得到各個(gè)關(guān)節(jié)的角度、速度和加速度對(duì)時(shí)間的函數(shù)之后,通過仿真驗(yàn)證五次多項(xiàng)式插值軌跡規(guī)劃在關(guān)節(jié)空間的結(jié)果,如圖6所示.

圖6 五次多項(xiàng)式插值關(guān)節(jié)角度、速度、加速度變化曲線

從五次多項(xiàng)式插值的結(jié)果來看,作業(yè)機(jī)械臂的關(guān)節(jié)角度變化較為平滑,速度與加速度的變化也較為連續(xù),沒有出現(xiàn)突變現(xiàn)象,有利于作業(yè)機(jī)械臂的運(yùn)動(dòng)平穩(wěn)進(jìn)行.

3.2 笛卡爾坐標(biāo)系空間軌跡規(guī)劃算法

在關(guān)節(jié)空間下的軌跡規(guī)劃無法保證作業(yè)機(jī)械臂末端作業(yè)工具按照特定路徑進(jìn)行運(yùn)動(dòng),因此,若要對(duì)作業(yè)機(jī)械臂末端作業(yè)工具的位姿進(jìn)行絕對(duì)的約束,需要在笛卡爾坐標(biāo)系空間內(nèi)實(shí)現(xiàn)軌跡規(guī)劃.

3.2.1 直線軌跡規(guī)劃

圖7 空間直線軌跡

設(shè)作業(yè)機(jī)械臂末端工具沿直線插補(bǔ)的速度為v,直線插補(bǔ)周期為Ts,總運(yùn)動(dòng)時(shí)長(zhǎng)為T,根據(jù)直線長(zhǎng)度L,求解總的直線插補(bǔ)步數(shù)N,如式(4)～(6)所示:

(4)

T=L/v

(5)

N=round(T/Ts)+1

(6)

其中:round表示對(duì)內(nèi)部的數(shù)值進(jìn)行取整.

規(guī)定λ為歸一化位置變量,取值范圍為[0,1],當(dāng)λ取0和1時(shí),分別對(duì)應(yīng)為作業(yè)機(jī)械臂末端工具在起始點(diǎn)和目標(biāo)點(diǎn)位置.F函數(shù)表示λ在時(shí)間上的映射,如式(7)所示,對(duì)應(yīng)的變化曲線如圖8所示.

圖8 歸一化變化曲線

(7)

3.2.2 圓弧軌跡規(guī)劃

在一個(gè)三維空間內(nèi),已知三個(gè)不共線的坐標(biāo)點(diǎn),可以求得經(jīng)過這三個(gè)坐標(biāo)點(diǎn)的圓弧軌跡,并采用插補(bǔ)方法計(jì)算出在圓弧上各個(gè)插值點(diǎn)的三維坐標(biāo)值.給定空間中的三個(gè)坐標(biāo)點(diǎn)分別為Q1=[x1y1z1]、Q2=[x2y2z2]、Q3=[x3y3z3],其在空間中的位置如圖9所示.

圖9 空間圓弧軌跡

給定的三個(gè)坐標(biāo)點(diǎn)在平面o1x1y1內(nèi)確定了一個(gè)外接圓,根據(jù)外接圓公式,得到圓的方程:

(8)

圓心的位置可以通過平面o1x1y1與線段Q1Q2的垂直平分面和與線段Q1Q3的垂直平分面的交點(diǎn)確定,線段Q1Q2的垂直平分面的方程如式(9).

(9)

線段Q1Q3的垂直平分面的方程如式(10):

(10)

聯(lián)立式(8)～(10)可求解圓弧圓心坐標(biāo)和半徑為:

(11)

(12)

根據(jù)圓弧圓心坐標(biāo)及其半徑值,運(yùn)用插值算法,可計(jì)算出圓弧上各個(gè)插值點(diǎn)在o1x1y1z1坐標(biāo)系中的坐標(biāo)P(1).通過齊次坐標(biāo)變換,得出插值點(diǎn)相對(duì)于基坐標(biāo)系下的三維坐標(biāo)值P(0),如式(13).

P(0)=0T1P(1)

(13)

其中:0T1為o1x1y1z1坐標(biāo)系相對(duì)于o0x0y0z0坐標(biāo)系的變換矩陣.

4 管板圓孔識(shí)別定位方法

管板位于水室上方,表面均布有上千個(gè)直徑約為20 mm的通孔.根據(jù)檢修任務(wù)要求,需要作業(yè)機(jī)械臂對(duì)管板圓孔進(jìn)行識(shí)別定位,進(jìn)而執(zhí)行檢修作業(yè).首先利用深度相機(jī)對(duì)管板進(jìn)行拍攝,根據(jù)霍夫圓環(huán)變換識(shí)別圖像中的圓孔,通過坐標(biāo)變換得到基坐標(biāo)系下的圓孔圓心坐標(biāo).

4.1 霍夫變換圓環(huán)檢測(cè)

圖像中的圓孔識(shí)別是利用霍夫變換圓環(huán)檢測(cè)實(shí)現(xiàn)的,該算法是將識(shí)別到圖像中的每一個(gè)像素點(diǎn)作為圓上的一個(gè)點(diǎn),進(jìn)行投票方式來得到該圖像中的形狀集合,并根據(jù)實(shí)際情況設(shè)定一個(gè)權(quán)重從而定位圓的位置[10],如圖10所示.

圖10 霍夫變換圓環(huán)檢測(cè)原理

假設(shè)在X-Y平面內(nèi)有一個(gè)圓,其圓心為(a,b),半徑為r,則該圓可用(x-a)2+(y-b)2=r2方程來表示,即為二元一次函數(shù)f(x,y)=r2.圓方程也可寫成為(a-x)2+(b-y)2-r2=0,其表示a-b-r空間中函數(shù),此時(shí)x、y為常量,也可以寫成三元一次函數(shù)f(a,b,r)=0.此變換的含義就是將X-Y平面內(nèi)圓環(huán)上的一個(gè)點(diǎn)對(duì)應(yīng)為a-b-r三維空間中的一條曲線,如圖8所示.圓上各點(diǎn)對(duì)應(yīng)的曲線在a-b-r三維空間中會(huì)相交于一點(diǎn)(a,b,r),該點(diǎn)就是X-Y平面上所定義的圓的圓心(a,b)和半徑r.前面提到的權(quán)重就是在a-b-r三維空間內(nèi)相交曲線的數(shù)量,當(dāng)累加的相交曲線數(shù)量達(dá)到該權(quán)重的比例時(shí),就認(rèn)為相交的那個(gè)點(diǎn)對(duì)應(yīng)X-Y平面上的一個(gè)圓.

4.2 圓心三維坐標(biāo)獲取

使用深度相機(jī)拍取一張管板的深度/彩色圖并進(jìn)行灰度處理,利用霍夫變換圓檢測(cè)算法得到圓孔的位置信息.由于此位置信息是相對(duì)于像素坐標(biāo)系而言的,需要通過坐標(biāo)變換,得到圓孔圓心在基坐標(biāo)系下的位置坐標(biāo)[11].

由深度相機(jī)得到的深度/彩色圖,因?yàn)椴噬珗D像數(shù)據(jù)和深度圖像數(shù)據(jù)是相對(duì)于不同的坐標(biāo)系生成的,彩色圖像數(shù)據(jù)的原點(diǎn)在RGB攝像頭中心,深度圖像數(shù)據(jù)的原點(diǎn)在紅外攝像頭中心,兩攝像頭存在一定距離,兩者的坐標(biāo)系會(huì)有一定的誤差.因此需要進(jìn)行深度圖像的匹配,將彩色圖像與深度圖像進(jìn)行對(duì)齊,使其在同一個(gè)坐標(biāo)系下進(jìn)行位置計(jì)算.

由像素坐標(biāo)系下的位置轉(zhuǎn)換成相機(jī)坐標(biāo)系下的位置這個(gè)過程,運(yùn)用小孔成像原理,如圖11所示,為了理解清晰,將實(shí)際點(diǎn)與成像點(diǎn)放置在同一方向上,實(shí)際應(yīng)是關(guān)于Oc對(duì)稱的.f為相機(jī)坐標(biāo)系下的焦距,m為在焦平面上的成像點(diǎn),其坐標(biāo)為Px(-x,-y,-z),M為相機(jī)坐標(biāo)系下的一個(gè)點(diǎn),坐標(biāo)為Pc(xc,yc,zc).由相似三角形關(guān)系可得:

圖11 小孔成像模型

(14)

一般像素坐標(biāo)系的原點(diǎn)位于圖像左上角,為了計(jì)算方便,將像素坐標(biāo)系(Px)相對(duì)于相機(jī)坐標(biāo)系(Pc)進(jìn)行了縮放和原點(diǎn)平移,像素坐標(biāo)系為Px(u,v):

(15)

其中:fx,fy,cx,cy的單位為像素,cx,cy是像素坐標(biāo)系平移的尺寸.f為像距,dX,dY分別表示X和Y方向上的一個(gè)像素點(diǎn)在相機(jī)感光板上的物理長(zhǎng)度,即fx=f/dX,fy=f/dY.則(u,v)可以寫成矩陣方程為:

(16)

通過矩陣運(yùn)算,點(diǎn)在相機(jī)坐標(biāo)系下的位置為:

(17)

同樣的,該點(diǎn)位置只是相對(duì)于相機(jī)坐標(biāo)系而言的,需要將相機(jī)坐標(biāo)系(Pc)變換到基坐標(biāo)系(P0)下,從而得到該點(diǎn)在基坐標(biāo)系下的位置為:

(18)

其中:R為一個(gè)3×3的旋轉(zhuǎn)矩陣,T是一個(gè)3×1的平移矩陣.聯(lián)立兩式即可得到像素坐標(biāo)系下m點(diǎn)的位置相對(duì)于基坐標(biāo)系下的位置為:

(19)

5 實(shí)驗(yàn)測(cè)試

為了驗(yàn)證軌跡規(guī)劃算法和圓孔識(shí)別定位方法的有效性, 搭建了實(shí)驗(yàn)平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證. 利用EtherCAT信號(hào)線借口串聯(lián)各個(gè)模塊,電纜及信號(hào)線均為中空走線,作業(yè)機(jī)械臂樣機(jī)如圖12所示.

圖12 作業(yè)機(jī)械臂樣機(jī)

5.1 軌跡規(guī)劃運(yùn)動(dòng)實(shí)驗(yàn)

通過上位機(jī)發(fā)送指令,機(jī)械臂各個(gè)關(guān)節(jié)之間進(jìn)行配合實(shí)現(xiàn)聯(lián)動(dòng)控制,分別完成在笛卡爾坐標(biāo)系下的直線和圓弧軌跡規(guī)劃,如圖13所示.

圖13 作業(yè)機(jī)械臂軌跡規(guī)劃實(shí)驗(yàn)

作業(yè)機(jī)械臂末端繪制的軌跡如圖14所示.繪制結(jié)果沒有明顯的抖動(dòng)痕跡,且運(yùn)動(dòng)精度較為精確,運(yùn)動(dòng)過程比較穩(wěn)定.給了確定作業(yè)機(jī)械臂末端軌跡規(guī)劃的精度,直線與圓弧進(jìn)行了多組實(shí)驗(yàn),每組進(jìn)行十次測(cè)試,求取平均值,結(jié)果如表1、2所示.經(jīng)過計(jì)算,作業(yè)機(jī)械臂末端直線軌跡規(guī)劃誤差不超過0.5%,圓弧軌跡規(guī)劃誤差不超過1%.

表1 直線理論尺寸與實(shí)際尺寸

表2 圓弧理論尺寸與實(shí)際尺寸

圖14 機(jī)械臂末端軌跡

5.2 管板圓孔識(shí)別定位實(shí)驗(yàn)

實(shí)驗(yàn)所用的管板模型如圖15(A),圓孔直徑為19.28 mm.相機(jī)位于管板平面的正上方,相機(jī)到管板的垂直距離為300 mm.經(jīng)過霍夫變換圓檢測(cè)處理,得到如圖15(B)所示的圓孔處理圖像.從霍夫圓檢測(cè)識(shí)別到的圓心位置與實(shí)際的圓心位置進(jìn)行對(duì)比發(fā)現(xiàn),識(shí)別得到的圓心位置誤差不超過1.38 mm,并能夠準(zhǔn)確將末端作業(yè)工具插入到管板圓孔中,如圖15(C)所示.

圖15 管板圓孔識(shí)別定位實(shí)驗(yàn)

在實(shí)驗(yàn)的過程中,誤差有多個(gè)方面原因:1)光源與識(shí)別背景,光線太強(qiáng)或背景過于復(fù)雜,會(huì)造成較大的識(shí)別誤差,甚至導(dǎo)致識(shí)別失敗;2)相機(jī)視平面應(yīng)與管板平行,若成傾斜狀態(tài),會(huì)導(dǎo)致識(shí)別到的圓孔為橢圓形,降低識(shí)別到圓心個(gè)數(shù).

6 結(jié) 論

1)根據(jù)核電站蒸汽發(fā)生器內(nèi)部結(jié)構(gòu)及檢修任務(wù)要求,分別在關(guān)節(jié)空間和笛卡爾坐標(biāo)系空間下對(duì)自研的六自由度機(jī)械臂進(jìn)行了軌跡規(guī)劃,并完成了機(jī)械臂末端工具對(duì)管板圓孔的識(shí)別定位.

2)搭建了作業(yè)機(jī)械臂樣機(jī)并進(jìn)行性能測(cè)試,機(jī)械臂能夠完成直線和圓弧軌跡規(guī)劃,誤差分別不超過0.5%和1%;能夠識(shí)別管板圓孔坐標(biāo)并定位,識(shí)別的圓心誤差不超過1.38 mm.