劉捷，黃燕民，陳宏軒，羅友高，孫 濤，唐國(guó)元

（1.武漢第二船舶設(shè)計(jì)研究所，湖北武漢 430060；2.華中科技大學(xué)船舶與海洋工程學(xué)院，湖北武漢 430074）

隨著海洋裝備技術(shù)的發(fā)展與應(yīng)用，許多具有特殊功能和用途的水下機(jī)器人被研制出來(lái)，并廣泛應(yīng)用在水下資源勘探、深水作業(yè)、救援打撈、漁業(yè)養(yǎng)殖等領(lǐng)域[1-2]。水下清洗機(jī)器人作為一種特殊的服務(wù)型機(jī)器人，主要用于對(duì)目標(biāo)作業(yè)物表面附著的臟物進(jìn)行清洗，常見(jiàn)的清洗方式有射流式、刷洗式以及混合式[3]。

設(shè)計(jì)一款交互性好、實(shí)時(shí)性高、功能完備的軟件控制系統(tǒng)對(duì)提高清洗機(jī)器人水下作業(yè)效率十分重要。該文結(jié)合實(shí)際使用場(chǎng)景，基于開(kāi)發(fā)高效的Python語(yǔ)言和梯形圖語(yǔ)言，采取模塊化設(shè)計(jì)方式，完成了水下清洗機(jī)器人控制系統(tǒng)軟件的整體設(shè)計(jì)開(kāi)發(fā)。

1 軟件系統(tǒng)結(jié)構(gòu)

清洗機(jī)器人軟件系統(tǒng)整體由上位機(jī)控制終端和下位機(jī)執(zhí)行終端兩部分組成，根據(jù)任務(wù)屬性不同，具體可分為應(yīng)用層、決策層和驅(qū)動(dòng)層[4]。應(yīng)用層主要負(fù)責(zé)人機(jī)交互功能，利用上位機(jī)軟件界面實(shí)時(shí)監(jiān)控從下位機(jī)采集并上傳的清洗機(jī)器人運(yùn)行狀態(tài)信息，如慣性導(dǎo)航模塊測(cè)量的姿態(tài)數(shù)據(jù)、深度計(jì)測(cè)量的當(dāng)前工作水深信息以及網(wǎng)絡(luò)攝像頭視頻畫(huà)面等，同時(shí)還負(fù)責(zé)將操作人員在軟件界面輸入的操作指令發(fā)送給決策層執(zhí)行；決策層負(fù)責(zé)解算下位機(jī)發(fā)送的數(shù)據(jù)信息和操作人員輸入的指令信息，將通過(guò)一系列運(yùn)算得到的控制指令發(fā)送給下位機(jī)執(zhí)行；驅(qū)動(dòng)層則負(fù)責(zé)執(zhí)行由上位機(jī)發(fā)出的控制指令，同時(shí)采集和上傳清洗機(jī)器人的水下運(yùn)行數(shù)據(jù)。清洗機(jī)器人軟件系統(tǒng)結(jié)構(gòu)如圖1 所示。

圖1 清洗機(jī)器人軟件系統(tǒng)結(jié)構(gòu)

清洗機(jī)器人控制系統(tǒng)軟件采用模塊化設(shè)計(jì)，將執(zhí)行不同功能的程序塊分隔開(kāi)來(lái)，以此降低彼此間的耦合，提高軟件開(kāi)發(fā)效率。軟件主要分為通信模塊、運(yùn)動(dòng)控制模塊、視頻畫(huà)面顯示模塊、運(yùn)行位姿顯示模塊、手柄、推進(jìn)器和傳感器數(shù)據(jù)顯示模塊。通信模塊用于上、下位機(jī)之間的數(shù)據(jù)交換，這也是軟件系統(tǒng)能夠控制清洗機(jī)器人水下位姿的前提條件；運(yùn)動(dòng)控制模塊會(huì)根據(jù)程序設(shè)定好的控制算法，按照自主模式或是手動(dòng)模式將控制指令發(fā)送給下位機(jī)執(zhí)行；視頻畫(huà)面顯示模塊則是將清洗機(jī)器人本體搭載的兩個(gè)網(wǎng)絡(luò)攝像頭視頻信號(hào)處理轉(zhuǎn)換為圖像信息，顯示在軟件界面，為操作人員提供決策參考；運(yùn)行位姿顯示模塊將根據(jù)下位機(jī)回傳的傳感器數(shù)據(jù)，在軟件界面繪制實(shí)時(shí)位姿圖，直觀反映清洗機(jī)器人水下運(yùn)行狀態(tài)；手柄、推進(jìn)器和傳感器數(shù)據(jù)顯示模塊則在軟件界面顯示了具體的數(shù)值信息。

2 上位機(jī)程序設(shè)計(jì)

上位機(jī)作為控制系統(tǒng)的核心，是實(shí)現(xiàn)水下清洗機(jī)器人能夠按照預(yù)期軌跡、姿態(tài)運(yùn)行的關(guān)鍵。該文水下清洗機(jī)器人控制系統(tǒng)上位機(jī)軟件使用Python 語(yǔ)言進(jìn)行編寫(xiě)，該語(yǔ)言開(kāi)發(fā)簡(jiǎn)潔、可擴(kuò)展性強(qiáng)[5]，用來(lái)編寫(xiě)清洗機(jī)器人上位機(jī)軟件十分合適。軟件開(kāi)發(fā)工具選擇PyCharm，用戶(hù)界面則采用跨平臺(tái)界面庫(kù)PyQt5作為開(kāi)發(fā)框架。

2.1 設(shè)計(jì)方案

清洗機(jī)器人上位機(jī)軟件具體需要實(shí)現(xiàn)的幾個(gè)功能分別是系統(tǒng)通信、運(yùn)動(dòng)控制、網(wǎng)絡(luò)視頻信號(hào)讀取和顯示、實(shí)時(shí)位姿展示等，因而需要對(duì)這幾個(gè)功能分別進(jìn)行程序設(shè)計(jì)。上位機(jī)軟件結(jié)構(gòu)如圖2 所示。

圖2 上位機(jī)軟件結(jié)構(gòu)

2.2 關(guān)鍵功能實(shí)現(xiàn)

1）系統(tǒng)通信：清洗機(jī)器人控制系統(tǒng)具備手動(dòng)控制和自動(dòng)控制兩種工作模式，而無(wú)論處在何種工作模式下，其水面系統(tǒng)和水下系統(tǒng)之間的數(shù)據(jù)交互都是以臍帶纜作為傳輸介質(zhì)并通過(guò)以太網(wǎng)通信連接實(shí)現(xiàn)的。清洗機(jī)器人手動(dòng)控制模式是通過(guò)使用兩臺(tái)操縱手柄來(lái)實(shí)現(xiàn)的，操縱手柄經(jīng)USB 串行線建立與工控機(jī)的物理連接。在Python 開(kāi)發(fā)環(huán)境中，使用第三方庫(kù)函數(shù)來(lái)實(shí)現(xiàn)這兩種通信。在PyCharm 開(kāi)發(fā)工具中安裝“pySerial”和“HslCommunication”兩個(gè)模塊，使用“from ** import **”語(yǔ)句獲取模塊中的調(diào)用方法。在建立的串口通信線程中，對(duì)串口參數(shù)進(jìn)行初始化，按照手柄通信協(xié)議配置相應(yīng)的串口號(hào)、波特率、奇偶校驗(yàn)等串口屬性，最后使用“serial.Serial()”方法對(duì)所配置的串口對(duì)象進(jìn)行實(shí)例化，在確認(rèn)串口正確打開(kāi)后，對(duì)實(shí)例化的串口對(duì)象使用“read()”方法便可以接收來(lái)自操縱手柄的數(shù)據(jù)，數(shù)據(jù)接收格式則可以根據(jù)需要自由設(shè)定。

清洗機(jī)器人使用西門(mén)子S7-1500PLC 作為水下控制器，該型控制器支持多種數(shù)據(jù)通信協(xié)議，其中較有特色的是西門(mén)子特有不開(kāi)放的S7 通信協(xié)議，這種通信協(xié)議不要求服務(wù)器方編寫(xiě)通信程序，使用便捷[6]。該文清洗機(jī)器人水面、水下控制系統(tǒng)通信使用支持S7 通信協(xié)議的開(kāi)源模塊“HslCommunication”來(lái)實(shí)現(xiàn)。在程序中導(dǎo)入該模塊的兩個(gè)類(lèi)“SiemensS7Net、SiemensPLCS”，并創(chuàng)建S7-1500PLC 連接對(duì)象類(lèi)，使用“SiemensS7Net()”方法對(duì)S7-1500PLC 實(shí)例化，在線程中建立上位機(jī)與S7-1500PLC 的長(zhǎng)連接?！癈onnect Server().IsSuccess”方法用于判斷系統(tǒng)連接狀態(tài)，當(dāng)在程序中調(diào)用“start()”方法開(kāi)啟通信連接線程后，前者將返回一個(gè)表征通信連接狀態(tài)的布爾型變量，當(dāng)該連接狀態(tài)為“TRUE”，則表示水面、水下控制系統(tǒng)已成功建立長(zhǎng)連接，此時(shí)上位機(jī)便可以通過(guò)建立的以太網(wǎng)通信連接對(duì)下位機(jī)進(jìn)行連續(xù)讀寫(xiě)數(shù)據(jù)操作。

2）運(yùn)動(dòng)控制：清洗機(jī)器人水下定深、定艏向控制是通過(guò)運(yùn)行程序中編寫(xiě)的增量式PID 算法實(shí)現(xiàn)的。增量式PID 算法是一種遞推式算法，計(jì)算式如式（1）所示：

式中，u(k-1) 為控制系統(tǒng)上一次輸出，e(k)、e(k-1)、e(k-2)分別為最近三次偏差值，Kp為比例系數(shù)，Ki為積分系數(shù)，Kd為微分系數(shù)。該算法可有效減少偏差迭代次數(shù)，提高運(yùn)算速度的同時(shí)減少了對(duì)計(jì)算機(jī)資源的占用[7-9]。

在上位機(jī)程序中，建立一個(gè)PID 算法的類(lèi)，在類(lèi)的初始化方法中分別對(duì)比例、積分、微分系數(shù)以及三次偏差值進(jìn)行初始化，通過(guò)將操縱手柄輸入的航向角和深度值與下位機(jī)上傳的角度、深度數(shù)據(jù)進(jìn)行比較得到偏差值，每收到一次數(shù)據(jù)調(diào)用一次PID算法進(jìn)行數(shù)據(jù)更新，程序自動(dòng)將計(jì)算得到的輸出變化量與上一次系統(tǒng)輸出值進(jìn)行疊加，得到最終輸出控制量。程序?qū)⒏鶕?jù)設(shè)定好的控制循環(huán)周期，對(duì)下位機(jī)進(jìn)行數(shù)據(jù)寫(xiě)入操作。為驗(yàn)證程序中增量式PID算法的有效性，手動(dòng)設(shè)定艏向角為360°，程序控制周期T為100 ms，輸出響應(yīng)如圖3 所示?？梢?jiàn)所寫(xiě)代碼能夠起到實(shí)際控制作用，后續(xù)只需調(diào)整算法參數(shù)即可。

圖3 增量式PID算法程序輸出曲線

3）網(wǎng)絡(luò)視頻信號(hào)讀取和顯示：上位機(jī)使用跨平臺(tái)的計(jì)算機(jī)視覺(jué)庫(kù)OpenCV 來(lái)對(duì)網(wǎng)絡(luò)攝像頭視頻流信號(hào)進(jìn)行處理，提取出幀圖像并在軟件界面中使用“QLabel”控件來(lái)顯示。OpenCV-Python 庫(kù)豐富的函數(shù)和算法使得軟件開(kāi)發(fā)效率顯著提高[10]。需要注意的是OpenCV 是按照BGR 格式讀取圖像信息的，而在軟件界面，圖像信息是以RGB 格式顯示的，因而需要在程序中調(diào)用“cvtColor()”和“QImage()”兩個(gè)函數(shù)，將提取的幀圖片轉(zhuǎn)換為RGB 格式進(jìn)行顯示。通過(guò)在界面主線程引入定時(shí)器定時(shí)刷新界面或創(chuàng)建圖像處理線程的方式來(lái)解決界面顯示視頻畫(huà)面卡頓的問(wèn)題。

4）實(shí)時(shí)位姿展示：為了幫助操作人員更為直觀地了解清洗機(jī)器人在水下的運(yùn)動(dòng)姿態(tài)以及當(dāng)前工作水深情況，基于Matplotlib 庫(kù)在軟件界面建立兩個(gè)動(dòng)態(tài)圖表窗口，其中一個(gè)用來(lái)展示清洗機(jī)器人的三維姿態(tài)，另一個(gè)用于生成清洗機(jī)器人下潛深度隨時(shí)間變化的二維曲線圖。Matplotlib 是一個(gè)堪比MATLAB圖像繪制功能的強(qiáng)大圖形庫(kù)[11-12]，通過(guò)編寫(xiě)代碼可以十分輕松地進(jìn)行2D、3D 圖形的繪制。為了能夠在PyQt 界面實(shí)現(xiàn)動(dòng)態(tài)繪圖，首先需要在程序中建立一個(gè)畫(huà)布的類(lèi)，從Matplotlib 庫(kù)引入FigureCanvasQTAgg類(lèi)，并在新建畫(huà)布類(lèi)中繼承它。使用“figure()”生成圖像窗口，“add_subplot()”函數(shù)用于在該圖像窗口添加一個(gè)子圖，通過(guò)關(guān)鍵字“projection=‘3d’”將子圖設(shè)為3D 圖形屬性，之后便可以將該畫(huà)布類(lèi)作為控件添加到軟件界面。繪制圖形及更改視圖需要用到“plot()”和“view_init()”兩個(gè)函數(shù)。

2.3 界面設(shè)計(jì)

在Python 中使用PyQt 庫(kù)開(kāi)發(fā)GUI（圖形用戶(hù)界面）可選擇使用手寫(xiě)代碼的方式設(shè)計(jì)軟件界面，也可使用QtDesigner 這一界面輔助工具對(duì)界面進(jìn)行設(shè)計(jì)。相比前者要手寫(xiě)大量代碼，后者的引入則只需拖動(dòng)控件來(lái)設(shè)計(jì)界面，再用軟件生成代碼便可完成界面設(shè)計(jì)，減少了程序設(shè)計(jì)的工作量[13]，故而該文軟件界面設(shè)計(jì)采用這一方法。

3 下位機(jī)程序設(shè)計(jì)

采用了PLC 作為下位機(jī),其軟件不涉及復(fù)雜的邏輯運(yùn)算，只是接收并執(zhí)行來(lái)自上位機(jī)的指令以及上傳水下運(yùn)行數(shù)據(jù)，該文主要針對(duì)一些開(kāi)發(fā)過(guò)程中的實(shí)際問(wèn)題分析其解決辦法。

3.1 問(wèn)題描述

1）連接保護(hù)：由于上位機(jī)軟件中使用西門(mén)子專(zhuān)門(mén)的S7 協(xié)議來(lái)與下位機(jī)通信，而這種通信機(jī)制是單向連接的[6]，清洗機(jī)器人水下運(yùn)行過(guò)程中一旦失去與上位機(jī)的連接，可能會(huì)陷入失控運(yùn)行狀態(tài)，故需要在程序中設(shè)置通信連接保護(hù)機(jī)制。

2）模擬量信號(hào)干擾和噪聲：清洗機(jī)器人以七臺(tái)推進(jìn)器作為水下動(dòng)力，為達(dá)到良好的控制效果，需要對(duì)推進(jìn)器進(jìn)行轉(zhuǎn)速的閉環(huán)控制。該文水下控制器S7-1500PLC 擴(kuò)展了一個(gè)模擬量采集模塊，最多支持八個(gè)模擬量信號(hào)同時(shí)采集。但模擬量信號(hào)采集易受信號(hào)源以及傳輸過(guò)程干擾等因素影響，準(zhǔn)確性有所下降，因而有必要對(duì)信號(hào)干擾進(jìn)行去除，獲得可信度較高的采集數(shù)據(jù)。

3）推進(jìn)器正反轉(zhuǎn)狀態(tài)：該文清洗機(jī)器人所選配的推進(jìn)器，在未知轉(zhuǎn)速控制量的前提下，無(wú)法通過(guò)模擬量反饋信號(hào)直接判斷推進(jìn)器當(dāng)前轉(zhuǎn)向，只能根據(jù)反饋的模擬量信號(hào)計(jì)算得到轉(zhuǎn)速信息。在水下運(yùn)行過(guò)程中，清洗機(jī)器人兩種工作模式下的水平姿態(tài)都是自動(dòng)控制的，假設(shè)某一時(shí)刻推進(jìn)器轉(zhuǎn)速為反轉(zhuǎn)400 rpm，下一時(shí)刻控制量為正轉(zhuǎn)400 rpm，此時(shí)控制偏差應(yīng)為800 rpm，但實(shí)際計(jì)算偏差卻為零，導(dǎo)致推進(jìn)器保持原輸出不變，清洗機(jī)器人水下運(yùn)動(dòng)“失控”。因此需要在軟件層解決這一問(wèn)題。

3.2 解決方案

1）針對(duì)連接保護(hù)問(wèn)題，采取在下位機(jī)主程序塊OB1中，利用系統(tǒng)時(shí)鐘存儲(chǔ)器與加計(jì)數(shù)器（CTU）組成一個(gè)接通延時(shí)時(shí)長(zhǎng)為2 s 的定時(shí)器。系統(tǒng)通信已建立期間，上位機(jī)發(fā)出指令，保持加計(jì)數(shù)器始終處在復(fù)位狀態(tài)，由于上位機(jī)指令輸出頻率遠(yuǎn)大于定時(shí)時(shí)長(zhǎng)，因而在系統(tǒng)保持連接期間，下位機(jī)程序中這一組合定時(shí)器始終不能接通輸出。一旦系統(tǒng)通信斷開(kāi)，2 s 時(shí)間一到，組合定時(shí)器接通輸出，推進(jìn)器輸出復(fù)位和急停程序立即響應(yīng)，從而保證清洗機(jī)器人水下運(yùn)行安全。

2）針對(duì)模擬量信號(hào)去干擾和噪聲的問(wèn)題，常用方法為硬件濾波和軟件濾波。所使用的S7-1500PLC擴(kuò)展模擬量模塊自身具備一定的濾波能力，使用方便，只需在組態(tài)下載設(shè)備時(shí)開(kāi)啟這一功能即可。而單純依靠PLC 有限的硬件濾波功能并不能解決推進(jìn)器轉(zhuǎn)速模擬量信號(hào)波動(dòng)的問(wèn)題，所以還需要考慮使用軟件濾波。但是由于控制器本身的局限性，PLC無(wú)法運(yùn)行復(fù)雜的濾波算法，文獻(xiàn)[14]則提出了一種用于PLC 軟件濾波的改進(jìn)型加權(quán)平均濾波算法，計(jì)算公式如式（2）所示。該算法使用結(jié)構(gòu)化控制語(yǔ)言（SCL）進(jìn)行編寫(xiě)，相比使用廣泛的梯形圖語(yǔ)言（LAD），更適合處理復(fù)雜運(yùn)算，且編程條理清晰[15-16]。

式中，X1(k)為采樣數(shù)據(jù)去除峰值后新的數(shù)組第k項(xiàng)數(shù)據(jù)，Ck為權(quán)系數(shù)，I(k)為X1(k)在新數(shù)組中出現(xiàn)的頻次，Y為濾波輸出。

3）針對(duì)推進(jìn)器正反轉(zhuǎn)狀態(tài)問(wèn)題，因推進(jìn)器正、反轉(zhuǎn)運(yùn)行是由上位機(jī)發(fā)出的控制指令決定的，雖然無(wú)法從反饋的轉(zhuǎn)速信息了解推進(jìn)器某一時(shí)刻的旋轉(zhuǎn)方向，但可以通過(guò)判斷上位機(jī)控制指令狀態(tài)來(lái)使推進(jìn)器變得“可控”。在PLC 程序中根據(jù)控制流程圖4 編寫(xiě)軟件即可解決問(wèn)題。

圖4 推進(jìn)器正反轉(zhuǎn)控制程序流程圖

4 軟件測(cè)試

為檢驗(yàn)該文所設(shè)計(jì)軟件的實(shí)際使用效果，從而發(fā)現(xiàn)和解決軟件設(shè)計(jì)中存在的問(wèn)題，選擇在人工搭建的水池進(jìn)行測(cè)試[17-18]。

截取了上位機(jī)軟件界面部分功能區(qū)域進(jìn)行展示。圖5 為軟件界面中清洗機(jī)器人水下運(yùn)動(dòng)姿態(tài)顯示區(qū)，底部欄為實(shí)時(shí)姿態(tài)數(shù)據(jù)顯示區(qū)域。這種設(shè)計(jì)為實(shí)現(xiàn)良好的人機(jī)交互性提供了便利。軟件測(cè)試過(guò)程中各功能模塊使用正常，軟件工作狀態(tài)良好，達(dá)到了設(shè)計(jì)要求。

圖5 上位機(jī)軟件姿態(tài)界面

5 結(jié)論

該文對(duì)一款水下清洗機(jī)器人整體軟件設(shè)計(jì)進(jìn)行了詳細(xì)論述，基于Python 語(yǔ)言和PLC 梯形圖、SCL 結(jié)構(gòu)化語(yǔ)言對(duì)軟件關(guān)鍵功能模塊進(jìn)行了設(shè)計(jì)和代碼實(shí)現(xiàn)，最后在水池測(cè)試了軟件功能?，F(xiàn)場(chǎng)測(cè)試表明，軟件整體達(dá)到預(yù)期設(shè)計(jì)目標(biāo)，下一步將進(jìn)一步優(yōu)化程序和完善軟件功能。