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

國核電站運行服務技術公司 王一帆 趙琛 袁夢揚 關光

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

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

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

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

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

1.2 總體設計說明

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

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

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

2.1 核壓力容器模型

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

圖1 核反應堆壓力容器結構圖Fig.1 Structural diagram of nuclear reactor pressure vessel

2.2 檢測機械手模型

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

圖2 六軸機械手結構圖Fig.2 Structure diagram of six axis manipulator

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

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

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

圖3 加載單元Fig.3 Loading unit

2.4 機械手仿真模型運動控制

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

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

3 自動檢測路徑規(guī)劃及控制模塊

3.1 自動檢測路徑規(guī)劃及控制模塊

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

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

3.2 軌跡規(guī)劃與運動控制模塊

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

3.2.1 直線路徑規(guī)劃

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

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

（1）加速時間

t

確定：

式中，

vel

是運動前預設的末端速度，，

acc

是運動前預設的末端加速度。

（2）插補運動直線長度確定：

假設

P

和

P

分別代表起始點和終止點，兩點間的距離則為：

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

（4）每次運動步長的確定：

采用插補周期

T

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

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

則下一步的位置為：

3.2.2 圓弧路徑規(guī)劃

在圓弧路徑規(guī)劃中，設定圓弧路徑的起始點

P

、終止點

P

和路經(jīng)點

P

，其中

P

與

P

和

P

不共線，通過這些點即可確定在仿真空間中的圓弧。設定以圓弧的圓心為原點，圓心相對起始點

P

的射線線為

X

軸，圓心相對終止點

P

的射線為

Y

軸，圓弧所在的

OXY

平面的法線為

Z

軸建立坐標系

O

-

X

Y

Z

，如圖7所示。

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

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

則圓心角

θ

為：

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

θ

的梯形速度曲線也將按此圓弧進行變化。在此條件下，第k次插補時的圓心角即為

θ

，則在

O

-

X

Y

Z

坐標系中，六軸機械手的末端坐標為：

六軸機械手在世界坐標系中的坐標還需進行轉換，轉換后的坐標為：

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

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

Q

={

?

，

η

}，其中：

式中，

θ

為轉動角度，

ε

為轉軸方向。其對應的旋轉矩陣為：

若給定旋轉矩陣：

其對應的四元數(shù)為：

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

ε

轉動

θ

角從一個姿態(tài)

Q

移動到另一個姿態(tài)

Q

。轉動角度

θ

和轉軸方向

ε

滿足：

由上式可以很方便地求出轉動角度和轉軸方向，然后進行插補。

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

4 系統(tǒng)調試與仿真測試

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

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

圖9 TCP調試Fig.9 TCP debugging

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

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

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

圖10 模型切換響應時間Fig.10 Model switching response time

5 結語

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