六自由度機(jī)械手控制系統(tǒng)設(shè)計(jì)與仿真*

國核電站運(yùn)行服務(wù)技術(shù)公司 王一帆 趙琛 袁夢(mèng)揚(yáng) 關(guān)光

針對(duì)常用的反應(yīng)堆壓力容器檢測(cè)用六自由度機(jī)械手進(jìn)行了運(yùn)動(dòng)學(xué)和控制研究。對(duì)機(jī)械手的控制需求進(jìn)行了系統(tǒng)介紹，通過系統(tǒng)模型導(dǎo)入實(shí)現(xiàn)機(jī)械手在核壓力容器內(nèi)的三維模型運(yùn)動(dòng)仿真和控制，并實(shí)現(xiàn)自動(dòng)檢測(cè)路徑的軌跡規(guī)劃和姿態(tài)規(guī)劃，通過系統(tǒng)通訊調(diào)試和仿真測(cè)試，驗(yàn)證了控制系統(tǒng)設(shè)計(jì)的合理性。

核反應(yīng)堆壓力容器是核電廠中保證核電設(shè)備安全的關(guān)鍵設(shè)備。在核電廠的檢驗(yàn)規(guī)范和大綱中，對(duì)反應(yīng)堆壓力容器和頂蓋的焊縫及其他部位提出了無損檢測(cè)的強(qiáng)制性要求。受核電環(huán)境的影響，檢測(cè)必須依靠多軸機(jī)械手來完成，采用多軸機(jī)械手首先是需要考慮安全。壓力堆反應(yīng)容器和頂蓋不允許出現(xiàn)碰撞、劃傷等，否則會(huì)對(duì)設(shè)備運(yùn)行帶來極大危險(xiǎn)。由于多軸機(jī)械手受空間限制和碰撞安全等要素影響，不僅需要多軸機(jī)械手具有高度靈活性，還需要在檢測(cè)前對(duì)多軸機(jī)械手的檢測(cè)路徑進(jìn)行規(guī)劃仿真。通過多軸機(jī)械手運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)仿真軟件對(duì)多軸機(jī)械手進(jìn)行檢測(cè)過程的運(yùn)動(dòng)仿真和路徑規(guī)劃，確保操作過程的高效性、安全性和有效性。

1 機(jī)械手系統(tǒng)控制及仿真模擬設(shè)計(jì)

1.1 系統(tǒng)控制和仿真需求

系統(tǒng)的控制與仿真即在導(dǎo)入反應(yīng)堆壓力容器/頂蓋模型、機(jī)械手、掃查器三維仿真模型后，根據(jù)機(jī)械手各個(gè)電機(jī)位置反饋的信息可使仿真模型與機(jī)械手系統(tǒng)在容器中的實(shí)際位置相匹配。根據(jù)人機(jī)界面的參數(shù)輸入，指定機(jī)械手進(jìn)行相應(yīng)的位姿變換，通過該六軸機(jī)械手運(yùn)動(dòng)學(xué)分析模塊，即可將機(jī)械手到達(dá)指定位置和姿態(tài)，同時(shí)人機(jī)界面上的仿真模型也會(huì)實(shí)時(shí)顯示實(shí)際機(jī)械手的運(yùn)動(dòng)狀態(tài)。同時(shí)，仿真操作和實(shí)際運(yùn)動(dòng)操作可根據(jù)需求進(jìn)行自由切換。

1.2 總體設(shè)計(jì)說明

六軸機(jī)械手運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)仿真軟件系統(tǒng)適用于Windows7、Windows10等計(jì)算機(jī)系統(tǒng)，依托于Visual Studio2019為開發(fā)平臺(tái)，采用C#語言編寫，通過編程實(shí)現(xiàn)對(duì)模型的仿真。本文以六軸機(jī)械手運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)仿真軟件系統(tǒng)為立足點(diǎn)，以實(shí)現(xiàn)核反應(yīng)堆壓力容器的機(jī)械手智能化仿真檢測(cè)。通過建立（或兼容導(dǎo)入）壓力容器、頂蓋等被檢對(duì)象三維模型、機(jī)械手模型和端部效應(yīng)器模型，可在虛擬模式下實(shí)現(xiàn)機(jī)械手的模擬檢測(cè)并獲得可行的檢測(cè)方案，并可根據(jù)實(shí)際機(jī)械手關(guān)節(jié)臂長(zhǎng)度進(jìn)行不同機(jī)械手的選擇，生成機(jī)械手三維運(yùn)動(dòng)圖，模擬機(jī)械手控制的各種模式，提供仿真模擬運(yùn)動(dòng)軌跡規(guī)劃、驗(yàn)證。

2 仿真系統(tǒng)功能實(shí)現(xiàn)

系統(tǒng)仿真功能的實(shí)現(xiàn)通過對(duì)各個(gè)模塊開發(fā)來展開，采用UG、CATIA、Pro-E等三維建模軟件構(gòu)件模型，主要包括：機(jī)械手、壓力容器及檢測(cè)設(shè)備模型。在模型導(dǎo)入前建立模型的XML文檔，以此設(shè)置模型初始化空間坐標(biāo)及模型本身位姿。對(duì)于機(jī)械手模型參數(shù)的修改，在XML文件中可以修改機(jī)械手的名稱、質(zhì)量等信息，同時(shí)可以改變各關(guān)節(jié)可視化的顏色，設(shè)定D-H參數(shù)。

2.1 核壓力容器模型

核壓力容器常見于CAP1400、AP1000和CPR1000堆型。如圖1所示，為AP1000壓水堆壓力容器的大致結(jié)構(gòu)。

圖1 核反應(yīng)堆壓力容器結(jié)構(gòu)圖Fig.1 Structural diagram of nuclear reactor pressure vessel

2.2 檢測(cè)機(jī)械手模型

如圖2所示為所仿真的模型為六自由度機(jī)械手，該機(jī)械手由六個(gè)關(guān)節(jié)組成。其中，前四個(gè)關(guān)節(jié)決定機(jī)械手在空間中的位置，后兩關(guān)節(jié)則決定機(jī)械手末端的姿態(tài)，在機(jī)械手末端的法蘭盤與掃查器法蘭盤相接，用于安裝不同類型的掃查器。

圖2 六軸機(jī)械手結(jié)構(gòu)圖Fig.2 Structure diagram of six axis manipulator

從結(jié)構(gòu)上看，該機(jī)械手運(yùn)用開發(fā)式的運(yùn)動(dòng)鏈，通過多關(guān)節(jié)旋轉(zhuǎn)連桿的方式組成。機(jī)械手的每一個(gè)關(guān)節(jié)均裝有獨(dú)立的伺服驅(qū)動(dòng)器，通過關(guān)節(jié)的相對(duì)運(yùn)動(dòng)使得機(jī)械手以一定的運(yùn)動(dòng)軌跡和空間姿態(tài)到達(dá)預(yù)期位置，進(jìn)而進(jìn)行檢測(cè)工作。由于各關(guān)節(jié)存在線纜拉扯、限位等運(yùn)動(dòng)限制，各關(guān)節(jié)的轉(zhuǎn)角非360°的全周運(yùn)動(dòng)，而是通過多關(guān)節(jié)協(xié)調(diào)運(yùn)動(dòng)的方式進(jìn)行機(jī)械手控制姿態(tài)變換。

2.3 三維模型實(shí)現(xiàn)

機(jī)械手在實(shí)際使用過程中，末端掃查器根據(jù)不同掃查對(duì)象會(huì)進(jìn)行更換，故在三維模型的顯示中，末端掃查器也需要隨著使用場(chǎng)景的變化進(jìn)行動(dòng)態(tài)替換。采取方案為：在主界面左側(cè)菜單欄中點(diǎn)擊“模型選擇”，調(diào)出不同的機(jī)械手模型和掃查器模型，選擇所采用的模型，并設(shè)置相應(yīng)的參數(shù)，點(diǎn)擊“加載機(jī)械手模型”/“加載末端掃查器”即可完成模型更換，加載單元如圖3所示。對(duì)模型的選擇包含了圖3中的單元模型選擇列表，導(dǎo)入所需的模型單元(cell)，步驟如下：（1）在“模型選擇”＞“選擇單元模型”模塊選擇需要加載的單元；（2）選擇好單元后點(diǎn)擊“加載單元”實(shí)現(xiàn)單元導(dǎo)入。

圖3 加載單元Fig.3 Loading unit

2.4 機(jī)械手仿真模型運(yùn)動(dòng)控制

六軸機(jī)械手仿真模型的運(yùn)動(dòng)選擇有單軸點(diǎn)動(dòng)和多軸聯(lián)動(dòng)兩種。單軸點(diǎn)動(dòng)，即各關(guān)節(jié)軸可相對(duì)于其他關(guān)節(jié)獨(dú)立運(yùn)動(dòng)控制，單一選擇關(guān)節(jié)以不同的速度進(jìn)行角度旋轉(zhuǎn)，實(shí)現(xiàn)機(jī)械手安裝、多步驟手動(dòng)調(diào)節(jié)機(jī)械手到達(dá)目標(biāo)位置等功能；多軸聯(lián)動(dòng)，即機(jī)械手的多個(gè)關(guān)節(jié)在到達(dá)預(yù)設(shè)位姿時(shí)需采用笛卡爾坐標(biāo)，同時(shí)驅(qū)動(dòng)機(jī)械手的多個(gè)關(guān)節(jié)來運(yùn)行控制，如圖4所示。在多軸聯(lián)動(dòng)時(shí)，全局坐標(biāo)分別對(duì)應(yīng)機(jī)械手的整體坐標(biāo)和壓力容器的整體坐標(biāo)，通過上下、前后、左右等全局坐標(biāo)方式實(shí)現(xiàn)位置和角度的任意控制，多軸聯(lián)動(dòng)的主界面采用的“笛卡爾”選項(xiàng)控制界面，具備手動(dòng)操作和數(shù)值輸入，可同時(shí)輸入速度和加速度來設(shè)置多軸聯(lián)動(dòng)的運(yùn)行方式，具體步驟為：（1）打開笛卡爾控制面板；（2）在控制面板中選擇平移維度“X Y Z”或者旋轉(zhuǎn)維度“A B C”中的任意一個(gè)，選中之后的按鈕顏色變暗；（3）“正向運(yùn)動(dòng)”或“負(fù)向運(yùn)動(dòng)”鍵中選擇其中一個(gè)。同時(shí)，在運(yùn)行控制時(shí)，鼠標(biāo)點(diǎn)擊不松時(shí)為連續(xù)運(yùn)動(dòng)方式，鼠標(biāo)單擊時(shí)則為單點(diǎn)運(yùn)動(dòng)方式。運(yùn)動(dòng)完畢后，六個(gè)關(guān)節(jié)軸的角度值將顯示在運(yùn)動(dòng)控制界面的下方供操作人員觀察，此外，為使機(jī)械手以到達(dá)指定位置并以指定姿態(tài)呈現(xiàn)，可通過數(shù)值輸入功能設(shè)置運(yùn)行參數(shù)精確控制，具體操作為：運(yùn)動(dòng)界面中選擇相對(duì)位置、絕對(duì)位置和從文件選項(xiàng)，在“X Y Z”“A B C”六個(gè)數(shù)值的輸入框中輸入數(shù)據(jù)，鍵盤按下Enter鍵開始運(yùn)動(dòng)。

圖4 多軸控制Fig.4 Multi axis control

3 自動(dòng)檢測(cè)路徑規(guī)劃及控制模塊

3.1 自動(dòng)檢測(cè)路徑規(guī)劃及控制模塊

根據(jù)實(shí)際檢查需要，通過相應(yīng)參數(shù)選擇和設(shè)定實(shí)現(xiàn)壓力容器各檢查部位的掃查路徑生成。并可實(shí)現(xiàn)掃查路徑的單步、連續(xù)、中斷續(xù)檢等過程控制。自動(dòng)掃查的路徑是在輸入關(guān)鍵參數(shù)之后自動(dòng)生成的。機(jī)械手自動(dòng)掃查過程是自動(dòng)化控制中關(guān)鍵的一項(xiàng)內(nèi)容。其中，自動(dòng)掃查的路徑是在輸入關(guān)鍵參數(shù)之后自動(dòng)生成的。（1）點(diǎn)擊“Input parameters”按鈕，彈出路徑類型選擇項(xiàng)；（2）選擇需要生成路徑的類型，例如：在這里想要生成圓形路徑，點(diǎn)擊“Circular path”，此時(shí)會(huì)在一邊彈出提示輸入關(guān)鍵參數(shù)的輸入框；（3）輸入關(guān)鍵參數(shù)信息，包括“Radgap”“Gap”“Dmin”“Dmax”“Height”“Rs” “Re”“Rotate”“Startp”“Endp”。最終，將會(huì)在仿真界面上自動(dòng)顯示出所繪制的圓形路徑。如圖5所示，在路徑規(guī)劃中，通過輸入起始坐標(biāo)數(shù)據(jù)、離散點(diǎn)數(shù)、圓半徑，終點(diǎn)坐標(biāo)數(shù)據(jù)、圓半徑和中間間隔即可畫出路徑。其中圓半徑是指需要檢測(cè)的孔半徑，中間間隔是指起始環(huán)與終點(diǎn)環(huán)之間需要添加的路徑環(huán)數(shù)量。

圖5 不同孔形狀規(guī)劃路徑Fig.5 Planning paths for different hole shapes

3.2 軌跡規(guī)劃與運(yùn)動(dòng)控制模塊

軌跡規(guī)劃的目的是為了讓機(jī)械手以一定的方式進(jìn)行末端直線和圓弧運(yùn)動(dòng)，以滿足不同檢測(cè)需求，可有效保證檢測(cè)質(zhì)量。在運(yùn)行時(shí)，機(jī)械手的末端始終處于規(guī)劃軌跡上，且末端掃查器也始終與前進(jìn)的發(fā)現(xiàn)方向垂直，可防止機(jī)械手位姿混亂，同時(shí)節(jié)省大量的運(yùn)動(dòng)控制時(shí)間。因此，需要對(duì)機(jī)械手進(jìn)行六自由度路徑規(guī)劃，保證位置和姿態(tài)滿足任務(wù)需求。

3.2.1 直線路徑規(guī)劃

機(jī)械手的末端掃查器需要進(jìn)行精細(xì)的掃查運(yùn)動(dòng)，需要保證機(jī)械手末端運(yùn)動(dòng)的平滑性，在該運(yùn)動(dòng)系統(tǒng)中，末端運(yùn)動(dòng)特點(diǎn)為如圖6所示的梯形速度曲線方式進(jìn)行。

圖6 梯形速度曲線Fig.6 Trapezoidal velocity curve

（1）加速時(shí)間

t

確定：

式中，

vel

是運(yùn)動(dòng)前預(yù)設(shè)的末端速度，，

acc

是運(yùn)動(dòng)前預(yù)設(shè)的末端加速度。

（2）插補(bǔ)運(yùn)動(dòng)直線長(zhǎng)度確定：

假設(shè)

P

和

P

分別代表起始點(diǎn)和終止點(diǎn)，兩點(diǎn)間的距離則為：

（3）速度軌跡曲線的修訂：

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

采用插補(bǔ)周期

T

作為定時(shí)插補(bǔ)，插補(bǔ)次數(shù)則為

根據(jù)圖6中所呈現(xiàn)的機(jī)械手末端梯形速度曲線，可以得出插補(bǔ)運(yùn)動(dòng)至第k次是所得出的位移：

則下一步的位置為：

3.2.2 圓弧路徑規(guī)劃

在圓弧路徑規(guī)劃中，設(shè)定圓弧路徑的起始點(diǎn)

P

、終止點(diǎn)

P

和路經(jīng)點(diǎn)

P

，其中

P

與

P

和

P

不共線，通過這些點(diǎn)即可確定在仿真空間中的圓弧。設(shè)定以圓弧的圓心為原點(diǎn)，圓心相對(duì)起始點(diǎn)

P

的射線線為

X

軸，圓心相對(duì)終止點(diǎn)

P

的射線為

Y

軸，圓弧所在的

OXY

平面的法線為

Z

軸建立坐標(biāo)系

O

-

X

Y

Z

，如圖7所示。

圖7 圓弧路徑規(guī)劃Fig.7 Arc path planning

圖8 平面上的圓弧Fig.8 Arc on plane

則圓心角

θ

為：

圖8作為平面上的圓弧，圓心角

θ

的梯形速度曲線也將按此圓弧進(jìn)行變化。在此條件下，第k次插補(bǔ)時(shí)的圓心角即為

θ

，則在

O

-

X

Y

Z

坐標(biāo)系中，六軸機(jī)械手的末端坐標(biāo)為：

六軸機(jī)械手在世界坐標(biāo)系中的坐標(biāo)還需進(jìn)行轉(zhuǎn)換，轉(zhuǎn)換后的坐標(biāo)為：

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

六軸機(jī)械手姿態(tài)規(guī)劃的進(jìn)行采用單位四元數(shù)來表征，單位四元數(shù)的形式為

Q

={

?

，

η

}，其中：

式中，

θ

為轉(zhuǎn)動(dòng)角度，

ε

為轉(zhuǎn)軸方向。其對(duì)應(yīng)的旋轉(zhuǎn)矩陣為：

若給定旋轉(zhuǎn)矩陣：

其對(duì)應(yīng)的四元數(shù)為：

對(duì)于任意2個(gè)姿態(tài)，可以繞一軸線

ε

轉(zhuǎn)動(dòng)

θ

角從一個(gè)姿態(tài)

Q

移動(dòng)到另一個(gè)姿態(tài)

Q

。轉(zhuǎn)動(dòng)角度

θ

和轉(zhuǎn)軸方向

ε

滿足：

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

自動(dòng)檢測(cè)的路徑采用自主開發(fā)的平臺(tái)來實(shí)現(xiàn)，機(jī)械手根據(jù)RRT、PRM以及他們的各種改進(jìn)算法形成姿態(tài)規(guī)劃的軌跡點(diǎn)，根據(jù)軌跡點(diǎn)即可生成一系列待執(zhí)行的運(yùn)動(dòng)指令，以發(fā)送至機(jī)械手驅(qū)動(dòng)控制器按照指定路徑點(diǎn)運(yùn)行，這些點(diǎn)明確了機(jī)械手在空間坐標(biāo)系中的一些位置或關(guān)節(jié)角度，同時(shí)在自主開發(fā)平臺(tái)軌跡規(guī)劃中確定每個(gè)關(guān)節(jié)軸運(yùn)行的速度和加速度，則該挑路徑的時(shí)間信息也相對(duì)確定。自主開發(fā)平臺(tái)的軌跡規(guī)劃以能量最小等原則規(guī)劃求解，通過單軸運(yùn)動(dòng)和多軸聯(lián)動(dòng)相互配合的方式，提供軌跡運(yùn)動(dòng)時(shí)各軸實(shí)時(shí)位置、角度、扭矩等相關(guān)參數(shù)的顯示。

4 系統(tǒng)調(diào)試與仿真測(cè)試

4.1 系統(tǒng)通訊調(diào)試

在自主開發(fā)系統(tǒng)中，為方便后續(xù)進(jìn)一步開發(fā)，對(duì)于下位機(jī)通訊系統(tǒng)，采用TCP/IP實(shí)現(xiàn)與下位機(jī)連接，在IP處輸入IP地址，Port處輸入端口號(hào)。該功能模塊包括校零功能、數(shù)據(jù)同步功能以及上下位機(jī)數(shù)據(jù)傳輸可視化功能。實(shí)時(shí)提供各信號(hào)的傳輸狀態(tài)，提供通訊及通訊保障，TCP通訊如圖9所示。點(diǎn)擊發(fā)送數(shù)據(jù)可以發(fā)送下位機(jī)所需各項(xiàng)數(shù)據(jù)，發(fā)送數(shù)據(jù)時(shí)間間隔通過定時(shí)器調(diào)節(jié)，最大程度實(shí)現(xiàn)虛擬與現(xiàn)實(shí)的實(shí)時(shí)運(yùn)動(dòng)，同時(shí)上位機(jī)可以接受下位機(jī)發(fā)送過來的指令，斷開/重新連接功能按鈕實(shí)現(xiàn)與下位機(jī)通訊斷開與連接。

圖9 TCP調(diào)試Fig.9 TCP debugging

4.2 系統(tǒng)仿真測(cè)試

本次測(cè)試覆蓋的范圍包含開發(fā)需求各大功能。測(cè)試方案：在UG、CATIA、Pro-E等三維建模軟件構(gòu)建三維幾何模型，設(shè)置用于自由切換的單元模型共12個(gè)（對(duì)應(yīng)于12個(gè)不同的組合：機(jī)械手本體+14種不同的末端掃查器+筒體）。設(shè)置模型導(dǎo)入的測(cè)試組主要為單元模型導(dǎo)入和機(jī)械手模型導(dǎo)入，并記錄模型導(dǎo)入的空間位姿坐標(biāo)變化情況以及響應(yīng)時(shí)間等數(shù)據(jù)。

通過測(cè)試結(jié)果發(fā)現(xiàn)，且系統(tǒng)在任何情況下都能夠快速地實(shí)現(xiàn)模型的導(dǎo)入，較少出現(xiàn)系統(tǒng)奔潰不成功等現(xiàn)象，對(duì)于不同的測(cè)試用模型，都能夠具有100%的切換成功率，且在在高負(fù)荷運(yùn)行等多種情況下，都能夠?qū)崿F(xiàn)正常的模型切換過程。如圖10所示，機(jī)械手模型導(dǎo)切換延遲1s左右，單元模型切換延遲1min左右，符合操作運(yùn)行狀態(tài)下切換的要求。

圖10 模型切換響應(yīng)時(shí)間Fig.10 Model switching response time

5 結(jié)語

本文根據(jù)核電站換料大修期間反應(yīng)堆壓力容器無損檢測(cè)六軸機(jī)械手的控制需求進(jìn)行了系統(tǒng)總體設(shè)計(jì)，通過對(duì)核電站壓力容器的導(dǎo)入和運(yùn)用運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的方法推導(dǎo)出的六軸機(jī)械手模型，并運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程的理論解析，規(guī)劃自動(dòng)檢測(cè)路徑的姿態(tài)和軌跡，以此設(shè)計(jì)出機(jī)械自動(dòng)檢測(cè)手直線、弧形規(guī)劃路徑和姿態(tài)規(guī)劃策略。最后采用系統(tǒng)通訊和仿真測(cè)試的方式驗(yàn)證了系統(tǒng)設(shè)計(jì)的合理性，滿足系統(tǒng)仿真運(yùn)行要求。