核電站法蘭蓋開蓋機(jī)器人控制系統(tǒng)的設(shè)計(jì)

張浩文，劉 錚，樊紹勝，周宇霆

(長(zhǎng)沙理工大學(xué)電氣與信息工程學(xué)院，湖南長(zhǎng)沙 410114)

0 引言

核能是一種高效清潔的能源，核電站為人類的生產(chǎn)生活提供了大量的能源供應(yīng)，但核電站發(fā)展過(guò)程中的安全問(wèn)題也越來(lái)越受關(guān)注。核電站內(nèi)部有放射性污染且空間環(huán)境復(fù)雜等問(wèn)題，進(jìn)入核電站內(nèi)部通常需要諸多防護(hù)措施，這給操作人員帶來(lái)了不便和健康威脅[1-2]。

通過(guò)設(shè)計(jì)法蘭蓋開蓋機(jī)器人，代替人工進(jìn)行法蘭蓋螺栓擰松和法蘭蓋打開作業(yè)，可以減少人工更換RCV過(guò)濾器濾芯時(shí)操作人員暴露在輻射環(huán)境中的時(shí)間，對(duì)保障工作人員的健康、降低勞動(dòng)強(qiáng)度和提高工作效率都具有重要意義。本文設(shè)計(jì)的法蘭蓋開蓋機(jī)器人控制系統(tǒng)具有可靠性高、對(duì)法蘭蓋螺栓磨損小等特點(diǎn)。

1 控制系統(tǒng)總體結(jié)構(gòu)設(shè)計(jì)

法蘭蓋開蓋機(jī)器人總體結(jié)構(gòu)設(shè)計(jì)如圖1所示，包括可更換的移動(dòng)底盤、機(jī)器人控制柜體和五軸機(jī)械臂。控制柜包含機(jī)器人控制系統(tǒng)、電源模塊以及遠(yuǎn)程通信模塊；五軸機(jī)械臂的作業(yè)末端是柔順結(jié)構(gòu)設(shè)計(jì)的套筒[3]。

機(jī)器人控制系統(tǒng)總體結(jié)構(gòu)框圖如圖2所示。該系統(tǒng)主要由STM32單片機(jī)為核心組成的最小系統(tǒng)電路模塊、CAN總線通信模塊、串口通信模塊、網(wǎng)絡(luò)通信電路模塊、A/D模擬量采集電路模塊和供電模塊組成[4]。平臺(tái)移動(dòng)電機(jī)與機(jī)械臂伺服電機(jī)均采用基于單電機(jī)伺服控制的方法，即每個(gè)電機(jī)是一個(gè)完整的子伺服控制系統(tǒng)，經(jīng)CAN通信模塊接收中央運(yùn)動(dòng)控制器的目標(biāo)指令，獨(dú)立完成電流、速度和位置的閉環(huán)控制。主控單元通過(guò)網(wǎng)絡(luò)通信模塊接收上位機(jī)視覺(jué)識(shí)別及視覺(jué)伺服控制算法的處理結(jié)果；對(duì)各傳感器的數(shù)據(jù)采集、處理和分析，并根據(jù)分析結(jié)果執(zhí)行相應(yīng)的控制任務(wù)；對(duì)伺服電機(jī)驅(qū)動(dòng)控制器下發(fā)運(yùn)動(dòng)控制指令及電機(jī)運(yùn)動(dòng)參數(shù)采集并將參數(shù)上傳給上位機(jī)。

2 控制系統(tǒng)硬件設(shè)計(jì)

法蘭蓋開蓋機(jī)器人控制系統(tǒng)硬件部分設(shè)計(jì)包括主控芯片最小系統(tǒng)電路、串口通信電路、CAN總線通信電路、網(wǎng)絡(luò)通信電路、A/D采樣電路及電源系統(tǒng)電路的設(shè)計(jì)。

2.1 主控芯片及其最小系統(tǒng)電路

機(jī)器人控制系統(tǒng)的主控芯片選用STM32F407ZGT6單片機(jī)，該芯片采用ARM的32位Cortex-M4內(nèi)核處理器，其主頻可達(dá)168 MHz，具有豐富的片上外設(shè)資源和通用I/O接口[5]，且具有符合IEEE 1588 v2標(biāo)準(zhǔn)要求的以太網(wǎng)MAC10/100。在本控制系統(tǒng)中所用到的外設(shè)包括：RS232通用異步收發(fā)傳輸器(讀取激光測(cè)距傳感器的測(cè)量結(jié)果)、CAN通信控制器(對(duì)伺服電機(jī)驅(qū)動(dòng)控制器下發(fā)運(yùn)動(dòng)控制指令，讀取電機(jī)及編碼器當(dāng)前狀態(tài)和參數(shù))和ADC模數(shù)轉(zhuǎn)換器 (對(duì)三維力傳感器以及電源的電壓電流信息進(jìn)行采集轉(zhuǎn)換)。

為了滿足控制系統(tǒng)的基本要求，設(shè)計(jì)了STM32F407ZGT6的最小系統(tǒng)，包括電源、時(shí)鐘、復(fù)位和調(diào)試接口電路，如圖3所示。

其中最小系統(tǒng)電源電路采用LM2576HV DC-DC芯片與LD1086D2M33 DC-DC芯片配合，LM2576HV為BUCK型降壓轉(zhuǎn)換電路，具有較寬的輸入電壓范圍(7～60 V)，很好地適應(yīng)了機(jī)器人鋰電池電源在滿電與虧電情況下不同的電池電壓。由LM2576HV輸出的5 V穩(wěn)壓最高可達(dá)4 A電流，滿足了主控電路板其他元器件的供電需求，LD1086D2M33為3.3 V固定電壓輸出的BUCK型DC-DC芯片，由LD1086D2M33的BUCK型降壓轉(zhuǎn)換芯片實(shí)現(xiàn)STM32控制器的3.3 V供電。單片機(jī)的復(fù)位電路采用低電平上電復(fù)位設(shè)計(jì)，在上電瞬間，接地電容充電，單片機(jī)RESET管腳出現(xiàn)短暫低電平，以此產(chǎn)生復(fù)位信號(hào)。時(shí)鐘電路外部高速時(shí)鐘選擇為DSX321G的8 MHz晶振，精度為20 ppm，經(jīng)倍頻后可達(dá)168 MHz；下載調(diào)試接口采用SWD模式，相比JTAG接口在高速模式下具有更高的可靠性并且占用更少的I/O口。主控板電源電路設(shè)計(jì)如圖4所示。其中C1為鋁電解電容，有效防止輸入端出現(xiàn)較大的瞬間電壓和輸入電壓波動(dòng)；D1為肖特基二極管，其開關(guān)速度快，正向壓降低，反向恢復(fù)時(shí)間短；C2為鉭電容，用來(lái)輸出濾波以及提高反饋環(huán)路的穩(wěn)定性。

2.2 數(shù)據(jù)采集電路硬件設(shè)計(jì)

2.2.1 A/D采樣電路

三維力傳感器輸出為與力值大小成正比的0～5 V的模擬量信號(hào)，STM32單片機(jī)片上ADC是12位逐次逼近型模數(shù)轉(zhuǎn)換器，其I/O允許輸入電壓為0～3.3 V，故采用電阻分壓法對(duì)輸出電壓進(jìn)行處理轉(zhuǎn)換。為了提高輸入阻抗，以提高測(cè)量準(zhǔn)確性，同時(shí)在單片機(jī)與被測(cè)電路之間起到隔離緩沖的作用，選擇輸入阻抗較高的集成運(yùn)放TLC2262搭建電壓跟隨器，如圖5所示；TLC2262為滿幅電壓運(yùn)放，當(dāng)采用5 V單電源時(shí)，其輸出電壓幅值可達(dá)5 V不失真，在保證電路安全且不失真的前提下可采用先進(jìn)行電壓跟隨再分壓的形式，可避免分壓采樣電阻對(duì)前級(jí)電路的影響。R4與C2構(gòu)成低通濾波器，濾除高頻尖峰干擾。

在采集電池電壓信號(hào)時(shí)，將R2和R3構(gòu)成的分壓電路前置以保護(hù)后級(jí)電路。

2.2.2 RS232通信電路

RS232C是常用的串行通信接口標(biāo)準(zhǔn)之一，采用負(fù)邏輯電平，即DC-15～-3 V表示邏輯1，DC3～15 V表示邏輯0，實(shí)際使用中-9～-7 V為合理的驅(qū)動(dòng)電平[6]。而STM32控制器的信號(hào)電平為TTL電平，激光距離傳感器與控制器之間的RS232通信需要進(jìn)行電平轉(zhuǎn)換，因此采用MAX3232電平轉(zhuǎn)換IC進(jìn)行設(shè)計(jì)，MAX3232采用3.3 V單電源供電，配備高精度鉭電容完成電平轉(zhuǎn)換，設(shè)計(jì)電路如圖6所示。

3 控制系統(tǒng)軟件設(shè)計(jì)

本控制系統(tǒng)的軟件包括下位機(jī)底層控制軟件和上位機(jī)監(jiān)測(cè)軟件[7]。其中系統(tǒng)底層采用C語(yǔ)言在KEIL MDK-ARM集成開發(fā)環(huán)境進(jìn)行程序設(shè)計(jì)。上位機(jī)在Visual Studio IDE中使用C++語(yǔ)言開發(fā)。

3.1 下位機(jī)底層軟件設(shè)計(jì)

底層控制軟件在STM32微控制器上移植的μcos-Ⅲ實(shí)時(shí)操作系統(tǒng)，它是Micrium公司推出的第三代產(chǎn)品，相比于前代產(chǎn)品，專門針對(duì)Cortex-arm4進(jìn)行了優(yōu)化，具有更短的中斷切入時(shí)間和任務(wù)切換時(shí)間[8]，能夠滿足機(jī)器人的控制任務(wù)。

在μcos-Ⅲ實(shí)時(shí)操作系統(tǒng)中，以多線程的方式處理不同模塊的任務(wù)，任務(wù)模塊包括用于上位機(jī)監(jiān)控的網(wǎng)絡(luò)通信模塊、用于機(jī)械臂控制的CAN總線模塊以及傳感器信號(hào)采集模塊。在主線程中實(shí)現(xiàn)對(duì)以上模塊的初始化以及創(chuàng)建默認(rèn)子線程。

根據(jù)不同的任務(wù)模塊分別創(chuàng)建對(duì)應(yīng)的子線程，其中網(wǎng)絡(luò)通信子線程用于與上位機(jī)建立TCP通信，將機(jī)器人當(dāng)前狀態(tài)實(shí)時(shí)反饋給上位機(jī)，對(duì)接收到的上位機(jī)通信指令進(jìn)行解析并執(zhí)行相應(yīng)功能的子線程；CAN總線子線程用于對(duì)機(jī)械臂及平臺(tái)進(jìn)行運(yùn)動(dòng)控制并讀取電機(jī)參數(shù)；傳感器信號(hào)采集子線程包括激光距離傳感器和三維力傳感器的數(shù)據(jù)采集及預(yù)處理。下位機(jī)程序流程圖如圖7所示。

3.2 上位機(jī)監(jiān)測(cè)與控制軟件

法蘭蓋開蓋機(jī)器人上位機(jī)監(jiān)測(cè)與控制軟件在Windows系統(tǒng)中使用Visual Studio集成開發(fā)環(huán)境完成設(shè)計(jì)，其界面如圖8所示：界面上方為監(jiān)控視頻顯示區(qū)，第一個(gè)視頻窗口為法蘭蓋及螺栓的視覺(jué)檢測(cè)結(jié)果顯示、機(jī)械臂視覺(jué)伺服初步定位結(jié)果顯示；第二個(gè)視頻窗口為螺栓對(duì)中控制結(jié)果顯示區(qū)。向下依次為狀態(tài)顯示區(qū)(關(guān)節(jié)電機(jī)狀態(tài)參數(shù)、傳感器參數(shù)以及電源狀態(tài)參數(shù))、系統(tǒng)控制區(qū)(啟停控制)、自動(dòng)控制區(qū)和手動(dòng)操作區(qū)。

4 基于雙傳感器的螺栓柔順對(duì)中控制

傳統(tǒng)的擰螺栓作業(yè)末端通常采用具有被動(dòng)柔順結(jié)構(gòu)的機(jī)械末端來(lái)被動(dòng)調(diào)整套筒與螺栓對(duì)準(zhǔn)時(shí)的微小誤差。被動(dòng)柔順擰螺栓時(shí)，由于套筒相對(duì)于螺栓有一定偏角，電機(jī)帶動(dòng)套筒的擰緊力被分解，電機(jī)需要更大的功率，其次也會(huì)對(duì)螺栓和套筒造成較大的磨損。基于以上缺點(diǎn)和法蘭蓋開蓋作業(yè)特定環(huán)境，提出了基于激光距離傳感器和三維力傳感器的雙傳感器末端柔順控制。

導(dǎo)納控制算法是根據(jù)機(jī)械臂與環(huán)境的作用力來(lái)調(diào)整末端的位姿來(lái)達(dá)到柔順控制的效果。位姿的調(diào)整分為末端姿態(tài)的調(diào)整與位置的調(diào)整。為簡(jiǎn)化控制算法，便于算法的實(shí)現(xiàn)，本文采用激光距離傳感器和三維力傳感器分別對(duì)姿態(tài)和位置進(jìn)行調(diào)整。

4.1 五自由度機(jī)械臂作業(yè)平臺(tái)的簡(jiǎn)化

機(jī)械臂平臺(tái)簡(jiǎn)化示意圖在Matlab中進(jìn)行運(yùn)動(dòng)學(xué)建模，如圖9所示。其中前移平臺(tái)可以為套筒提供向螺栓進(jìn)給的距離，關(guān)節(jié)5為旋轉(zhuǎn)電機(jī)，依靠旋轉(zhuǎn)驅(qū)動(dòng)套筒擰松螺栓。關(guān)節(jié)1用來(lái)調(diào)整末端與法蘭蓋的左右夾角，關(guān)節(jié)4用來(lái)調(diào)整與法蘭蓋的上下夾角，故調(diào)節(jié)關(guān)節(jié)1和關(guān)節(jié)4即完成對(duì)末端姿態(tài)的調(diào)節(jié)；關(guān)節(jié)2和關(guān)節(jié)3聯(lián)動(dòng)可完成平面內(nèi)的位置調(diào)整。

4.2 基于激光距離傳感器的末端姿態(tài)調(diào)節(jié)

法蘭蓋可視為豎直平面，螺栓與法蘭蓋為平面垂直關(guān)系，末端姿態(tài)調(diào)整時(shí)需將末端套筒截面調(diào)整為與法蘭蓋平面平行(套筒可無(wú)偏角套入螺栓)。采用套筒側(cè)面的激光測(cè)距儀隨套筒旋轉(zhuǎn)一周，測(cè)量圓周各點(diǎn)與法蘭盤的距離，即可測(cè)算末端與法蘭蓋的姿態(tài)偏差。

旋轉(zhuǎn)電機(jī)帶動(dòng)套筒轉(zhuǎn)動(dòng)一周為360°，轉(zhuǎn)動(dòng)套筒，調(diào)整激光測(cè)距的起點(diǎn)在套筒圓周的豎直方向，記此點(diǎn)為0°。測(cè)距圖像如圖10所示，當(dāng)測(cè)得距離最小值時(shí)套筒轉(zhuǎn)動(dòng)的角度為θZ，則測(cè)得最大值時(shí)轉(zhuǎn)動(dòng)角度為θZ+180°。

設(shè)距離最小值為L(zhǎng)1，最大值為L(zhǎng)2，則根據(jù)幾何關(guān)系可得套筒與法蘭蓋平面夾角大小為θX：

(1)

式中R為激光繞套筒中心軸的旋轉(zhuǎn)半徑。

在基座標(biāo)下，已知機(jī)械臂末端姿態(tài)為

(2)

將末端調(diào)整至與法蘭蓋平面平行需調(diào)整角度按Z-Y-X型動(dòng)態(tài)歐拉角可記為Euler(θZ、0，θX)，可得旋轉(zhuǎn)矩陣：

(3)

式中:cZ、cX、sZ、sX分別表示cosθZ、cosθX、sinθZ、sinθX。

可求得待調(diào)整的機(jī)械臂末端姿態(tài)為T′=TR，再由逆運(yùn)動(dòng)學(xué)可解出關(guān)節(jié)1和關(guān)節(jié)4角度。

4.3 基于三維力傳感器的末端套筒位置調(diào)節(jié)

在末端調(diào)平與初步對(duì)準(zhǔn)螺栓后，平臺(tái)前移使機(jī)械臂整體向螺栓進(jìn)給，末端套筒在其X軸(左右方向)與Y軸(上下方向)的期望受力為0，僅在Z軸的進(jìn)給方向受力FR。所以期望受力F0=(0,0,FR)。若三維力傳感器測(cè)得套筒受力為F=(FX,FY,FZ)，則采用直接力反饋控制算法對(duì)套筒位置調(diào)整，增加末端套筒柔順性。

4.3.1 位置環(huán)PD控制

在力反饋控制前，需要有獨(dú)立的位置控制器作為內(nèi)環(huán)控制，本文采用目前實(shí)際應(yīng)用最為廣泛的PD位置控制器：

(4)

式中：τ為關(guān)節(jié)力矩；e為關(guān)節(jié)角位移偏差Δθ。

由于調(diào)節(jié)第二、三關(guān)節(jié)電機(jī)即可調(diào)整位置，故機(jī)械臂模型可簡(jiǎn)化為二連桿結(jié)構(gòu)，示意圖如圖11所示，關(guān)節(jié)角位移分別為θ1、θ2，兩關(guān)節(jié)轉(zhuǎn)矩分別為τ1、τ2，質(zhì)量分別為m1、m2，桿長(zhǎng)度分別為l1、l2。

利用拉格朗日法對(duì)機(jī)械臂動(dòng)力學(xué)建模[9]：

(m1+m2)l1gcosθ1

(5)

改寫為矩陣形式，則有：

(6)

可以解得：

(7)

建立PD位置控制的算法框圖如圖12所示。

4.3.2 直接力反饋控制

末端套筒除了在進(jìn)給方向的受力外，在平面內(nèi)受力為F=(FX，F(xiàn)Y)。偏差越大，受力越大，故設(shè)定末端套筒受力與位置偏差成正比。力反饋控制器為

τ=KθC

(8)

式中：θC為位置修正量；K為力矩反饋系數(shù)。

建立直接力反饋控制算法框圖如圖13所示。

力反饋控制器的輸入為

(9)

式中J為力雅可比矩陣，表示為

以二連桿機(jī)構(gòu)為對(duì)象在Simulink仿真平臺(tái)進(jìn)行直接力反饋控制仿真，如圖14所示。

設(shè)關(guān)節(jié)1和關(guān)節(jié)2的起點(diǎn)位置速度均為0，在t=0時(shí)刻，給定位置為幅值0.98的階躍信號(hào)，實(shí)際位置為1。通過(guò)給定不同的力矩反饋系數(shù)K，來(lái)調(diào)節(jié)力反饋控制達(dá)到最佳效果，圖15為不同K值情況下的位置輸出結(jié)果。

由仿真結(jié)果可知，當(dāng)1/K值太小時(shí)調(diào)節(jié)時(shí)間過(guò)長(zhǎng)，不具有實(shí)用性；當(dāng)1/K值太大時(shí)超調(diào)量較大，不能用于擰松作業(yè)。本文取1/K為3.7。

5 試驗(yàn)結(jié)果分析與樣機(jī)測(cè)試

為測(cè)試法蘭蓋開蓋機(jī)器人實(shí)際作業(yè)效果，按核電站實(shí)際環(huán)境搭建了法蘭盤模型。法蘭盤中心距地面1.5 m，螺栓預(yù)緊力矩為150 N·m。以此為作業(yè)對(duì)象進(jìn)行試驗(yàn)。

5.1 螺栓柔順對(duì)中控制試驗(yàn)

僅靠末端套筒的被動(dòng)柔順結(jié)構(gòu)裹覆螺栓進(jìn)行螺栓對(duì)中和擰松時(shí)，由于套筒中心軸未與螺栓中心軸重合，套筒擰松力矩被分解，套筒驅(qū)動(dòng)電機(jī)輸出扭矩需大于150 N·m才可擰松螺栓，且中心軸偏離越遠(yuǎn)，所需輸出扭矩越大。驅(qū)動(dòng)電機(jī)轉(zhuǎn)矩系數(shù)為10.1 N·m/A，實(shí)驗(yàn)中末端套筒從同一位姿開始進(jìn)行主動(dòng)/被動(dòng)柔順對(duì)中控制對(duì)比實(shí)驗(yàn)，通過(guò)監(jiān)測(cè)螺栓擰松時(shí)峰值電流的大小來(lái)評(píng)價(jià)主動(dòng)柔順對(duì)中控制的結(jié)果。實(shí)驗(yàn)數(shù)據(jù)如表1所示。

表1 主動(dòng)/被動(dòng)柔順對(duì)中控制電機(jī)峰值電流

由表1數(shù)據(jù)可知，主動(dòng)柔順控制減小了驅(qū)動(dòng)電流，減少了套筒與螺栓之間的磨損。

5.2 機(jī)器人實(shí)際作業(yè)效果

在完成螺栓柔順對(duì)中控制試驗(yàn)，取得了更好的柔順對(duì)中效果后，再對(duì)機(jī)器人整機(jī)作業(yè)效果進(jìn)行試驗(yàn)。檢測(cè)上下位機(jī)通信的穩(wěn)定性、傳感器及電機(jī)數(shù)據(jù)能否及時(shí)上傳、機(jī)械臂作業(yè)空間是否與周圍環(huán)境存在干涉。試驗(yàn)結(jié)果表明，法蘭蓋開蓋機(jī)器人能安全有效地完成法蘭蓋螺栓擰松及開蓋任務(wù)。實(shí)際作業(yè)效果如圖16所示。

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

設(shè)計(jì)了基于STM32微處理器的法蘭蓋開蓋機(jī)器人控制系統(tǒng)并進(jìn)行了測(cè)試，采用上下位機(jī)結(jié)合的結(jié)構(gòu)，完成傳感器數(shù)據(jù)采集及分析、機(jī)械臂運(yùn)動(dòng)控制及作業(yè)過(guò)程的實(shí)時(shí)監(jiān)控。提出了基于雙傳感器的螺栓柔順對(duì)中控制，簡(jiǎn)化了柔順控制算法的設(shè)計(jì)，且相較于被動(dòng)柔順結(jié)構(gòu)具有更好的柔順效果。實(shí)驗(yàn)結(jié)果表明該機(jī)器人控制系統(tǒng)能有效完成法蘭蓋開蓋任務(wù)，降低工作人員的勞動(dòng)強(qiáng)度和輻射風(fēng)險(xiǎn)。