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

張 碩 吳洪明 孫健銓

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

3 中國(guó)科學(xué)院人機(jī)智能協(xié)同系統(tǒng)重點(diǎn)實(shí)驗(yàn)室 深圳 518055

0 引言

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

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

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

1 爬壁式移動(dòng)機(jī)械臂運(yùn)動(dòng)學(xué)模型

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

圖1 爬壁式移動(dòng)機(jī)械臂參數(shù)示意圖

圖2 移動(dòng)平臺(tái)非完整約束簡(jiǎn)化模型

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

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

1）移動(dòng)平臺(tái)的非完整約束

移動(dòng)平臺(tái)的運(yùn)動(dòng)中心受到的非完整約束條件可表示為

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

2）二自由度機(jī)械臂的完整約束

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

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

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

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

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

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

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

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

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

移動(dòng)平臺(tái)的工作平面上的坐標(biāo)計(jì)算式為

式中：Δl為運(yùn)動(dòng)中心到激光測(cè)距傳感器在機(jī)器人運(yùn)動(dòng)方向上的距離，取值為42 mm。

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

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

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

圖6 各電動(dòng)機(jī)的電流環(huán)控制框圖

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

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

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

圖8 移動(dòng)平臺(tái)巡檢軌跡跟蹤控制框圖

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

圖9 移動(dòng)平臺(tái)航向角軌跡跟蹤控制框圖

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

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

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

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

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

狀態(tài)4（Mobile Platform Angle Control Mode）：移動(dòng)平臺(tái)的航向角調(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)在自動(dòng)執(zhí)行控制指令后自動(dòng)跳轉(zhuǎn)至狀態(tài)5，接受外部通信指令的觸發(fā)。其中，外部通信指令優(yōu)先級(jí)最高。

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

6 總結(jié)

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