魏文斌，汪千強(qiáng)

（1.山東核電有限公司，山東 265116；2.江西工程學(xué)院，江西 338029）

0 引言

反應(yīng)堆壓力容器和壓力容器頂蓋是核電站的重要部件，是作為防止回路放射性物質(zhì)泄漏的屏障之一，承受著回路冷卻劑的高溫、高壓和高放射性的作用，壓力容器的密封面上通常會(huì)存留一些粉塵和劃痕，會(huì)影響壓力容器的密封性能，在核電站役前和在役期間均需要進(jìn)行必要的無(wú)損檢驗(yàn)。由于工況的特殊性，人員無(wú)法進(jìn)入壓力容器內(nèi)部或頂蓋下方進(jìn)行檢查，同時(shí)出于無(wú)損檢測(cè)重復(fù)性及穩(wěn)定性的需要，針對(duì)壓力容器及頂蓋的檢查，設(shè)計(jì)了一種在末端攜帶檢測(cè)探頭的六軸機(jī)械手，對(duì)反應(yīng)堆壓力容器和壓力容器頂蓋進(jìn)行周期性自動(dòng)檢查。

該機(jī)械手具有六個(gè)自由度，分別是沿壓力容器圓周方向的運(yùn)動(dòng)、軸向運(yùn)動(dòng)以及各個(gè)關(guān)節(jié)的旋轉(zhuǎn)運(yùn)動(dòng)。設(shè)計(jì)為長(zhǎng)度可伸展以適應(yīng)不同直徑的容器檢測(cè)。

圖1 機(jī)械手示意圖

圖2 機(jī)械手末端攜帶不同類(lèi)型掃查器檢測(cè)不同區(qū)域的狀態(tài)圖

1 機(jī)器人控制系統(tǒng)的設(shè)計(jì)

自主化控制系統(tǒng)主要用于六軸機(jī)械手的運(yùn)動(dòng)控制，以滿(mǎn)足反應(yīng)堆壓力容器及頂蓋的檢測(cè)需要。該系統(tǒng)分為三大模塊：上位機(jī)系統(tǒng)、下位機(jī)系統(tǒng)、軟硬件平臺(tái)接口系統(tǒng)[1～3]。

軟件運(yùn)行構(gòu)架如圖3所示，上位機(jī)系統(tǒng)主要根據(jù)鍵盤(pán)鼠標(biāo)的輸入，指定機(jī)械手要達(dá)到的位置，通過(guò)運(yùn)動(dòng)學(xué)分析模塊把各個(gè)關(guān)節(jié)的控制信號(hào)傳給機(jī)械手控制器，以此來(lái)控制機(jī)械手運(yùn)動(dòng)，同時(shí)通過(guò)仿真模擬模塊直接輸入機(jī)械各軸的位姿來(lái)模擬機(jī)械手的運(yùn)動(dòng)狀態(tài)。六軸機(jī)械手在指定空間范圍內(nèi)的連續(xù)動(dòng)作以實(shí)現(xiàn)各種掃查功能，上位機(jī)用戶(hù)界面提供實(shí)時(shí)狀態(tài)反饋和機(jī)械手三維姿態(tài)；下位機(jī)系統(tǒng)通過(guò)采集傳感器信號(hào)，傳輸給上位機(jī)系統(tǒng)，經(jīng)數(shù)據(jù)處理后得到輸出信號(hào)，由驅(qū)動(dòng)電機(jī)帶動(dòng)整個(gè)機(jī)械手。接口系統(tǒng)將機(jī)械手各個(gè)電機(jī)的位姿信息提供給運(yùn)動(dòng)控制模塊和仿真模塊，仿真模塊處理信息后將機(jī)械手的三維位姿情況顯示在人機(jī)交互界面[4～6]。

圖3 控制系統(tǒng)構(gòu)成示意圖

1.1 機(jī)器人上位機(jī)系統(tǒng)設(shè)計(jì)

如圖4所示，上位機(jī)系統(tǒng)框架分為三個(gè)部分：機(jī)器人執(zhí)行與仿真環(huán)境生成子系統(tǒng)、機(jī)器人執(zhí)行業(yè)務(wù)邏輯子系統(tǒng)和基于ROBCAD的人機(jī)交互界面子系統(tǒng)。一般ROBCAD采用HP UX 10.10操作系統(tǒng)，軟件控制平臺(tái)為ROBCAD7.1，系統(tǒng)運(yùn)行界面是通過(guò)定制過(guò)的ROBCAD的用戶(hù)界面。但本系統(tǒng)中上位機(jī)軟件控制部分是基于ROBCAD平臺(tái)集成的，將現(xiàn)有的應(yīng)用移植到Windows環(huán)境中，復(fù)制現(xiàn)有的各種業(yè)務(wù)邏輯，并建立和下位機(jī)的通訊，對(duì)現(xiàn)有的設(shè)備控制進(jìn)行集成[7]。

圖4 上位機(jī)系統(tǒng)框架圖

1.1.1 機(jī)器人執(zhí)行與仿真環(huán)境生成子系統(tǒng)

1）設(shè)備幾何模型系統(tǒng)

包括核電設(shè)備和機(jī)器人幾何模型，導(dǎo)入為ROBCAD接受的格式IGES，STEP格式。建立幾何模型轉(zhuǎn)換器，用來(lái)轉(zhuǎn)換通用的幾何模型，設(shè)置模型的質(zhì)量屬性、關(guān)節(jié)、材質(zhì)等特征。

2）軌跡點(diǎn)生成系統(tǒng)

利用ROBCAD環(huán)境，在ROSE API環(huán)境下根據(jù)路徑規(guī)劃算法動(dòng)態(tài)生成機(jī)器人需要執(zhí)行的軌跡點(diǎn)。其中路徑規(guī)劃（path planning）算法是通過(guò)ROBCAD找到無(wú)干涉，并能完成任務(wù)的路徑點(diǎn)。生成一系列運(yùn)動(dòng)連續(xù)的參考點(diǎn)，以發(fā)送到控制器驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)。即通過(guò)路徑規(guī)劃找到一系列要經(jīng)過(guò)的路徑點(diǎn)，這些點(diǎn)只是空間中的一些位置，或者關(guān)節(jié)角度，通過(guò)ROBCAD軌跡規(guī)劃確定怎么走，走多快，它需要賦予這條路徑以時(shí)間信息。本項(xiàng)目對(duì)于路徑規(guī)劃，采用RRT、PRM以及他們的各種改進(jìn)算法。軌跡規(guī)劃采用ROBCAD的S型規(guī)劃等優(yōu)化算法，保證時(shí)間最優(yōu)，能量最小等方法規(guī)劃求解。

表1 機(jī)器人執(zhí)行業(yè)務(wù)邏輯子系統(tǒng)

1.1.2 人機(jī)交互界面子系統(tǒng)

ROBCAD軟件在Unix環(huán)境下開(kāi)發(fā)的，盡管有windows版本，ROBCAD系統(tǒng)仍然是Unix的Motif界面，在Exceed虛擬機(jī)上運(yùn)行?；谟脩?hù)接口語(yǔ)言ROBULL，采用OSF-Motif標(biāo)準(zhǔn)。

對(duì)ROBUIL提供的界面對(duì)象以及回調(diào)函數(shù)工具箱，用戶(hù)通過(guò)界面對(duì)象可以設(shè)計(jì)自定義的應(yīng)用程序。

1.2 軟硬件接口平臺(tái)設(shè)計(jì)

如圖5所示，機(jī)器人執(zhí)行命令接口分為4部分：

1）控制指令集定義與生成：采用標(biāo)準(zhǔn)機(jī)器人語(yǔ)言來(lái)定義，以滿(mǎn)足擴(kuò)展，包括運(yùn)動(dòng)、信號(hào)處理、流程控制、數(shù)學(xué)運(yùn)算、邏輯運(yùn)算、文件管理、數(shù)據(jù)編輯等指令；

2）通信協(xié)議：采用標(biāo)準(zhǔn)TCP/IP接口來(lái)實(shí)現(xiàn)；

3）控制指令生成：根據(jù)軌跡路徑、機(jī)器人運(yùn)動(dòng)學(xué)對(duì)軌跡進(jìn)行離散，生成控制指令集；

4）生成的指令校驗(yàn)：根據(jù)設(shè)置的校驗(yàn)碼進(jìn)行指令校驗(yàn)，以防功能性故障。

圖5 機(jī)器人執(zhí)行命令接口

如圖6所示，機(jī)器人檢測(cè)監(jiān)控接口分為4部分：

1）控制原始目標(biāo)定義：對(duì)待檢測(cè)目標(biāo)進(jìn)行空間建模，轉(zhuǎn)換到實(shí)際坐標(biāo)系進(jìn)行融合。并繪制目標(biāo)預(yù)計(jì)軌跡和控制精度；

2）通信協(xié)議：標(biāo)準(zhǔn)TCP/IP協(xié)議；

3）機(jī)器人位姿讀取同步器：基于固定幀，實(shí)現(xiàn)定間隔對(duì)機(jī)器人裝置的各種位姿信號(hào)進(jìn)行同步掃描，滿(mǎn)足實(shí)時(shí)可控?cái)?shù)據(jù)同步；

4）狀態(tài)比對(duì)：根據(jù)讀取的信息，通過(guò)ROBCAD中建立的仿真模型實(shí)時(shí)復(fù)演比對(duì)，實(shí)現(xiàn)誤差顯示，可以完成實(shí)時(shí)精度補(bǔ)償。

圖6 機(jī)器人檢測(cè)監(jiān)控接口

2 六軸機(jī)器人笛卡爾路徑規(guī)劃

為實(shí)現(xiàn)對(duì)六軸機(jī)器人的運(yùn)動(dòng)控制系統(tǒng)的監(jiān)測(cè)，實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)軌跡按預(yù)設(shè)軌跡動(dòng)作，需要對(duì)機(jī)器人進(jìn)行末端直線(xiàn)和圓弧路徑規(guī)劃[8,9]。為保證檢測(cè)質(zhì)量，機(jī)器人的末端除了沿軌跡線(xiàn)運(yùn)動(dòng)外，還需確保檢測(cè)頭始終垂直于當(dāng)前點(diǎn)的法線(xiàn)方向。為此需要對(duì)機(jī)器人進(jìn)行六自由度路徑規(guī)劃，確保機(jī)器人的位置和姿態(tài)滿(mǎn)足任務(wù)需求。

2.1 位置規(guī)劃

2.1.1 直線(xiàn)路徑規(guī)劃

為保證機(jī)械臂末端運(yùn)動(dòng)的平滑性，機(jī)械臂末端按圖7所示的梯形速度曲線(xiàn)運(yùn)動(dòng)。

圖7 梯形速度曲線(xiàn)

1）確定加速時(shí)間tA：

式中，vel是預(yù)定的末端速度，acc是預(yù)定的末端加速度。

2）求插補(bǔ)直線(xiàn)長(zhǎng)度：

如果起始點(diǎn)和終止點(diǎn)分別用Ps和Pf表示，則兩點(diǎn)間的距離為：

（1）修訂速度軌跡曲線(xiàn)：

（2）確定每次運(yùn)動(dòng)的步長(zhǎng)

采用定時(shí)插補(bǔ)，插補(bǔ)周期為T(mén)，則插補(bǔ)次數(shù)為：

根據(jù)圖6中的梯形速度曲線(xiàn)，可以計(jì)算出第k次插補(bǔ)時(shí)走過(guò)的位移：

則下一步的位置為：

2.1.2 圓弧路徑規(guī)劃

給定圓弧起始點(diǎn)Ps、終止點(diǎn)Pf以及與Ps和Pf不共線(xiàn)的路經(jīng)點(diǎn)Pm，可以確定空間上的一條圓弧。以圓弧的圓心為原點(diǎn)，圓心指向起始點(diǎn)Pf為X軸，圓弧所在平面的法線(xiàn)為Z軸建立坐標(biāo)系O*-X*Y*Z*，如圖8所示。

圖8 圓弧路徑規(guī)劃

將Ps、Pf和Pm轉(zhuǎn)換到坐標(biāo)系O*-X*Y*Z*中，得到平面上的點(diǎn)Ps*、Pf*和Pm*，求出此時(shí)的圓心角，如圖9所示。

圖9 平面上的圓弧

則圓心角θ為：

圓心角θ按圖7中的梯形速度曲線(xiàn)變化，根據(jù)上面式(6)計(jì)算第k次插補(bǔ)時(shí)的圓心角，進(jìn)而可得到坐標(biāo)系O*-X*Y*Z*中機(jī)器人末端的坐標(biāo)。將此坐標(biāo)轉(zhuǎn)換到世界坐標(biāo)系中，得到機(jī)械臂末端在世界坐標(biāo)系中的坐標(biāo)：

2.2 姿態(tài)規(guī)劃

采用單位四元數(shù)來(lái)表征機(jī)器人末端姿態(tài)并進(jìn)行姿態(tài)的規(guī)劃。單位四元數(shù)的形式為，其中：

由式(9)可以很方便地求出轉(zhuǎn)動(dòng)角度和轉(zhuǎn)軸方向，然后就可以根據(jù)上面式(1)進(jìn)行插補(bǔ)計(jì)算。

3 系統(tǒng)應(yīng)用

系統(tǒng)已經(jīng)在山東海陽(yáng)核電站的“核電站反應(yīng)容器機(jī)器人檢測(cè)”的項(xiàng)目上得到了應(yīng)用。系統(tǒng)最大掃描速度為120mm/S，機(jī)械手末端可以驅(qū)動(dòng)15KG的端部效應(yīng)器，負(fù)載為16kg，絕對(duì)位置重復(fù)精度為±0.95mm，累計(jì)位置精度±5.5mm，可以根據(jù)法規(guī)及實(shí)際檢查需要，通過(guò)相應(yīng)參數(shù)選擇和設(shè)定實(shí)現(xiàn)壓力容器各檢查部位的掃查路徑生成，并可實(shí)現(xiàn)掃查路徑的單步、連續(xù)、中斷續(xù)檢等過(guò)程控制。核電站的應(yīng)用，充分通過(guò)實(shí)踐驗(yàn)證了該系統(tǒng)的可靠性及實(shí)用性。

圖10 反應(yīng)容器內(nèi)機(jī)械手模型

圖11 系統(tǒng)掃描軌跡生成

4 結(jié)束語(yǔ)

本文綜合考慮核電站反應(yīng)堆壓力容器檢測(cè)機(jī)器人在機(jī)械部分、電氣控制和探測(cè)儀器等諸多因素，設(shè)計(jì)實(shí)現(xiàn)了反應(yīng)堆壓力容器六軸機(jī)器人檢測(cè)控制系統(tǒng)。該檢測(cè)控制系統(tǒng)的基本功能、可靠性、安全性和技術(shù)指標(biāo)已滿(mǎn)足核電站工程應(yīng)用現(xiàn)場(chǎng)的實(shí)際檢測(cè)要求，提高了反應(yīng)堆壓力容器無(wú)損檢測(cè)的重復(fù)性以及穩(wěn)定性，達(dá)到了良好的效果，由此可見(jiàn)，該系統(tǒng)可以進(jìn)一步的適應(yīng)于各種特殊的核環(huán)境，有較大的普適性跟推廣價(jià)值。