蒸汽發(fā)生器二次側(cè)檢測用爬壁式移動機械臂控制系統(tǒng)設(shè)計

張 碩 吳洪明 孫健銓

1 武漢理工大學(xué)交通與物流工程學(xué)院 武漢 430063 2 廣東省機器人與智能系統(tǒng)重點實驗室 深圳 518055

3 中國科學(xué)院人機智能協(xié)同系統(tǒng)重點實驗室 深圳 518055

0 引言

蒸汽發(fā)生器在核電站發(fā)電系統(tǒng)發(fā)揮著極其重要的作用。蒸汽發(fā)生器的下半部分為二次側(cè)，其中排布的傳熱管在一回路壓力邊界處與許多零部件相連接，受到多種惡劣環(huán)境因素的影響（包括但不限于高溫、高壓、振動、應(yīng)力和水化學(xué)等）。因此，易產(chǎn)生零部件的腐蝕損傷（如機械零件脫落和傳熱管破裂等），成為熱交換設(shè)備中的薄弱環(huán)節(jié)。對蒸汽發(fā)生器進行預(yù)防檢查是降低核電站事故發(fā)生概率的重要手段[1]。在蒸汽發(fā)生器二次側(cè)的結(jié)構(gòu)中，內(nèi)部容許操作的空間極其有限，應(yīng)充分考慮限制條件，選擇爬壁機器人處理該類工程問題，可具備良好的可操作性。由于工作環(huán)境一般不會發(fā)生較大差異與變化，設(shè)計人員在充分了解作業(yè)環(huán)境后便可提出可行的機械方案，以解決機構(gòu)設(shè)計問題，而在結(jié)構(gòu)設(shè)計基礎(chǔ)上延伸出的控制系統(tǒng)設(shè)計問題才是爬壁機器人作業(yè)的中心問題。

在爬壁機器人的控制系統(tǒng)設(shè)計研究中，國內(nèi)外學(xué)者研究的側(cè)重方向不同，提供了不同的研究內(nèi)容。在移動機器人定位方面，針對機器人的內(nèi)部傳感器存在使用受限的影響，王子潤等[2]采用了激光雷達與改進AMCL相結(jié)合的全局定位方法；路晨曦等[3]采用視覺信息與車體IMU 的定位方法，提升機器人在運動過程中的定位精度。在軌跡跟蹤方面，滕昊等[4]在爬壁機器人運動學(xué)模型的基礎(chǔ)上構(gòu)建了雙環(huán)軌跡跟蹤控制器，提升機器人在跟蹤過程中的動態(tài)品質(zhì)；Boomeri V 等[5]采用拉格朗日方法對爬壁機器人進行了動力學(xué)分析，其中涵蓋了負載加速力矩、科里奧利力、離心力、重力等因素；劉佳等[6]利用U-K 方程建立了爬壁機器人在預(yù)定軌跡下的解析動力學(xué)方程，消除了采用拉格朗日方法引入的拉格朗日算子。在軟硬件設(shè)計方面，楊東宇等[7]介紹了以ROS 為上層軟件控制器、以BeagleBone Black 嵌入式單板計算機為底層驅(qū)動控制器的設(shè)計方案；宋容[8]則在軟硬件控制系統(tǒng)設(shè)計的基礎(chǔ)之上提出了基于大數(shù)據(jù)聚類分析的設(shè)計方案，以期解決機器人的運動定位控制問題。

目前，國內(nèi)外針對爬壁機器人的相關(guān)研究將爬壁機器人視作移動機器人，并從該角度入手進行定位、軌跡跟蹤和軟硬件的設(shè)計，但未形成從作業(yè)角度出發(fā)的控制系統(tǒng)細分設(shè)計思路。本文將結(jié)合蒸汽發(fā)生器二次側(cè)檢測用爬壁式移動機械臂的作業(yè)需求，總結(jié)出較全面地爬壁機器人控制系統(tǒng)設(shè)計路線，對爬壁式移動機械臂的控制系統(tǒng)設(shè)計方案進行介紹，為相關(guān)研究提供參考。

1 爬壁式移動機械臂運動學(xué)模型

如圖1 所示，爬壁式移動機械臂由移動平臺和二自由度機械臂組合而成。移動平臺通過永磁式車輪吸附于蒸汽發(fā)生器二次側(cè)結(jié)構(gòu)的內(nèi)壁，移動機構(gòu)采用輔助支撐輪與雙驅(qū)動輪相結(jié)合的方式，需要注意的是2 車輪并不同軸，輪距與軸距之比約等于5。在設(shè)計時采用了吸附力十分強勁的永磁鐵，車輪與接觸平面之間的摩擦極大，運動時產(chǎn)生的軸向滑移量較小。另外，考慮到輪距與軸距之比較大，為了降低分析的復(fù)雜度、簡化運動模型，將圖1 所示移動平臺視為受到了圖2 所示情況的非完整約束。

圖1 爬壁式移動機械臂參數(shù)示意圖

圖2 移動平臺非完整約束簡化模型

在執(zhí)行分析時，按照圖3 所示方法建立機器人的運動坐標(biāo)系，坐標(biāo)系的原點建立在移動平臺的運動中心處。具體的運動控制參數(shù)見圖1，其運動控制變量有航向角θ、末端姿態(tài)φ、機械臂1θ1、機械臂2θ2、車輪角速度θl和θr等；操作空間坐標(biāo)系下的特征點坐標(biāo)有移動平臺運動中心坐標(biāo)為（x0，0，z0）、機械臂2 終點坐標(biāo)為（x2，y2，z2）。另外，為了簡化模型，將二自由度機械臂中關(guān)節(jié)2 的最遠端點視為機械臂控制末端。

圖3 爬壁式移動機械臂坐標(biāo)系示意圖

1）移動平臺的非完整約束

移動平臺的運動中心受到的非完整約束條件可表示為

由式（1）可知，在非完整約束下，移動平臺的受控變量僅為2 個。將驅(qū)動車輪的控制角度θl和θr轉(zhuǎn)化后，即可得到沿移動平臺前進方向的線速度v和改變前進方向的航向角θ。

2）二自由度機械臂的完整約束

在二自由度機械臂中，轉(zhuǎn)動關(guān)節(jié)僅具備單一可運動的自由度，故屬于完整約束。機械臂末端相對于運動中心的坐標(biāo)為

2 機器人系統(tǒng)總體控制框架設(shè)計

爬壁式移動機械臂是復(fù)合型機器人，由移動平臺和二自由度機械臂復(fù)合而成，且增設(shè)了吸附功能。相較于傳統(tǒng)的平面移動機器人、爬壁機器人、二自由度機械臂，爬壁式移動機械臂的控制系統(tǒng)的設(shè)計難度更大。為了降低系統(tǒng)設(shè)計難度，本文設(shè)計了圖4 所示機器人控制系統(tǒng)，從物理連接的角度分為移動平臺和二自由度機械臂2 個子系統(tǒng)；從控制層級的角度劃分為上位機控制軟件、定位系統(tǒng)和軌跡跟蹤控制系統(tǒng)3 部分。上位機控制軟件向機器人發(fā)送通信控制指令，部署于機器人的定位系統(tǒng)為機器人提供運動定位信息、軌跡跟蹤控制系統(tǒng)，在運動定位信息的基礎(chǔ)上進行閉環(huán)控制，實現(xiàn)了未知狹窄環(huán)境下的高精度作業(yè)。

圖4 爬壁式移動機械臂控制系統(tǒng)總體框架

本文以單片機控制器為基礎(chǔ)，搭建了爬壁式移動機械臂的基礎(chǔ)功能。機器人控制系統(tǒng)采用分層設(shè)計方法，上層系統(tǒng)以機器人的運動學(xué)模型為基礎(chǔ)設(shè)計上位機軟件，底層控制子系統(tǒng)以機器人的閉環(huán)控制和定位功能為目標(biāo)進行設(shè)計。二自由度機械臂和移動平臺均選用STM32F103VET6 型單片機作為主控制器，采用串口通信方式與上位機軟件進行實時通信。各子系統(tǒng)的單片機控制均采用中斷處理方式，高頻定時處理子系統(tǒng)內(nèi)的傳感器對應(yīng)信息和閉環(huán)驅(qū)動控制的輸出更新。

3 機器人定位系統(tǒng)設(shè)計

定位系統(tǒng)內(nèi)對應(yīng)的傳感器安裝示意圖以及對應(yīng)的測量參數(shù)如圖5 所示。二自由度機械臂關(guān)節(jié)定位系統(tǒng)受限于機械結(jié)構(gòu)的設(shè)計，在不同關(guān)節(jié)處采用不同的傳感器定位方案。在車體一側(cè)的機械臂關(guān)節(jié)1 處，采用電位器進行角度反饋；在靠近機器人末端的一側(cè)，采用磁環(huán)＋ES49ESUA 的組合形式，以霍爾傳感器檢測磁環(huán)的磁場變化反饋實際電壓輸出。

圖5 傳感器布置與參數(shù)示意圖

移動類機器人的運動定位方法有內(nèi)部信息定位、外部信息定位、內(nèi)外部信息結(jié)合定位3 種?？紤]到永磁式車輪對車輪電動機編碼器磁輪的影響、內(nèi)部信息定位方法在運動距離較長時會場生較大的累計誤差等因素，爬壁式移動機械臂選用外部信息定位方法處理。移動平臺定位系統(tǒng)使用PAT9125EL 型光流傳感器，通過對不銹鋼材質(zhì)的光滑工作平面進行表面圖像信息采集，測量移動平臺的運動信息；采用VL53-400S 型激光測距傳感器測量移動平臺兩側(cè)到容器的上下底面的距離；選用HWT31型傳感器對移動平臺的實時運動位姿進行檢測。其中，光流傳感器的測量數(shù)據(jù)受到諸多因素的影響，如傳感器距離工作平面的距離、工作平面的表面特性等。經(jīng)過多次數(shù)據(jù)采集實驗發(fā)現(xiàn)，在運動一定距離后會產(chǎn)生誤差變化，需要進行補足。

移動平臺的工作平面上的坐標(biāo)計算式為

式中：Δl為運動中心到激光測距傳感器在機器人運動方向上的距離，取值為42 mm。

4 機器人軌跡跟蹤控制系統(tǒng)設(shè)計

機器人的軌跡跟蹤控制是實現(xiàn)機器人在作業(yè)空間內(nèi)精準(zhǔn)作業(yè)的核心內(nèi)容，需將驅(qū)動關(guān)節(jié)的電動機與定位系統(tǒng)的反饋信息進行組合，從而實現(xiàn)機器人的位姿閉環(huán)控制。移動平臺和機械臂關(guān)節(jié)1 的驅(qū)動電動機均采用Maxon 電動機，機械臂關(guān)節(jié)2 則采用微型直流減速電動機。

為了增強電動機力矩的響應(yīng)特性，采用無需電動機模型的增量式（Proportional Integral Derivative，PID）控制算法，設(shè)計得出機器人驅(qū)動電動機的電流閉環(huán)控制控制器（見圖6）。本文將各電動機的電流環(huán)驅(qū)動視為驅(qū)動控制系統(tǒng)，結(jié)合前述建立的2 個機器人定位子系統(tǒng)，綜合成為2 個采用級聯(lián)形式的底層軌跡跟蹤控制子系統(tǒng)。

圖6 各電動機的電流環(huán)控制框圖

對于二自由度機械臂而言，在執(zhí)行作業(yè)任務(wù)時需進行準(zhǔn)確的位姿控制，選用位置式PID 控制算法設(shè)計位置環(huán)，并將其與電流環(huán)直接進行級聯(lián)處理，如圖7所示。

圖7 二自由度機械臂位置環(huán)控制框圖

移動平臺的作業(yè)應(yīng)從移動機器人的角度出發(fā)，由于非完整約束的存在，機器人的位形空間可達性將由移動平臺的線速度和航向角共同完成?；诒狙芯繉ο?，移動平臺在執(zhí)行巡檢任務(wù)時的軌跡跟蹤控制器需要對線速度和航向角進行分別的閉環(huán)控制，并綜合成為一個控制器。其控制框圖如圖8 所示。

圖8 移動平臺巡檢軌跡跟蹤控制框圖

爬壁式移動機械臂在進行特殊作業(yè)和檢測任務(wù)時需對位姿進行微調(diào)，此時移動平臺圍繞運動中心進行旋轉(zhuǎn)運動，改變機器人的航向角。本文直接選擇在電流環(huán)控制的基礎(chǔ)上進行航向角的位置環(huán)控制，控制框圖如圖9所示。

圖9 移動平臺航向角軌跡跟蹤控制框圖

5 基于爬壁式移動機械臂控制狀態(tài)劃分的上位機軟件設(shè)計

前述分析給出了爬壁式移動機械臂在軌跡跟蹤控制方面的控制算法設(shè)計思路。為了提升機器人的控制水平，增強在未知環(huán)境下的自動化控制適應(yīng)能力，采用基于有限狀態(tài)機的控制狀態(tài)切換方法，對機器人進行控制狀態(tài)劃分。機器人在進入蒸汽發(fā)生器二次側(cè)過程中有5 個狀態(tài)。

狀態(tài)1（Torque/Current Control Mode）：機器人在由手孔進入時，由于機械結(jié)構(gòu)的限制，機械臂關(guān)節(jié)1 需要根據(jù)操作人員對關(guān)節(jié)施加力的方向進行隨動，遇到極限位置即自動停止；機器人在進入蒸汽發(fā)生器二次側(cè)并穩(wěn)定吸附在內(nèi)壁上后、等待作業(yè)的狀態(tài)：

狀態(tài)2（Manipulator Position Control Mode）：機械臂的位置控制；

狀態(tài)3（Mobile Platform Position Control Mode）：移動平臺的巡檢控制；

狀態(tài)4（Mobile Platform Angle Control Mode）：移動平臺的航向角調(diào)整；

狀態(tài)5（Stop Mode）：停止?fàn)顟B(tài)，等待外部通信指令。

狀態(tài)1 執(zhí)行結(jié)束后由外部通信指令觸發(fā)后轉(zhuǎn)至狀態(tài)5，其余狀態(tài)在自動執(zhí)行控制指令后自動跳轉(zhuǎn)至狀態(tài)5，接受外部通信指令的觸發(fā)。其中，外部通信指令優(yōu)先級最高。

在使用上位機軟件時，可進行機器人控制測試，可選擇機器人的控制模式分別為自動巡檢和手動測試。其中，選擇手動測試時機器人執(zhí)行過所設(shè)置參數(shù)的對應(yīng)功能后將自動進入狀態(tài)5。上位機軟件與軌跡跟蹤控制系統(tǒng)間采用RS485 通訊方式進行實時通信，以完成指令傳達和傳感器信息反饋。上位機軟件通過傳感器信息反饋實時監(jiān)測機器人的位姿，結(jié)合運動學(xué)模型對機器人的進行狀態(tài)監(jiān)督，避免與工作環(huán)境之間發(fā)生碰撞。

6 總結(jié)

爬壁式移動機械臂控制系統(tǒng)的組成涵蓋了大量軟硬件設(shè)計。本文針對蒸汽發(fā)生器二次側(cè)視頻檢查的需要，對爬壁式移動機械臂進行了運動定位、軌跡跟蹤控制系統(tǒng)的設(shè)計，設(shè)計過程可歸納為爬壁類機器人的一般設(shè)計方法，包括運動學(xué)模型分析、控制系統(tǒng)總體框架設(shè)計、定位系統(tǒng)設(shè)計、軌跡跟蹤控制系統(tǒng)設(shè)計等，并結(jié)合機器人的實際工況進行控制狀態(tài)劃分，設(shè)計了上位機控制軟件。機器人定位技術(shù)、軌跡跟蹤控制技術(shù)等機器人相關(guān)技術(shù)經(jīng)過融合后，為爬壁式移動機械臂在狹小空間內(nèi)的準(zhǔn)確運動控制提供了有力保障，有助于擴大機器人的安全運行范圍，提升綜合作業(yè)效率，并為今后無人化自動視頻檢查建立實現(xiàn)基礎(chǔ)。