水下清洗機器人控制系統(tǒng)軟件設計

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

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

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

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

1 軟件系統(tǒng)結構

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

圖1 清洗機器人軟件系統(tǒng)結構

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

2 上位機程序設計

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

2.1 設計方案

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

圖2 上位機軟件結構

2.2 關鍵功能實現(xiàn)

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

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

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

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

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

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

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

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

2.3 界面設計

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

3 下位機程序設計

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

3.1 問題描述

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

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

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

3.2 解決方案

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

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

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

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

圖4 推進器正反轉控制程序流程圖

4 軟件測試

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

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

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

5 結論

該文對一款水下清洗機器人整體軟件設計進行了詳細論述，基于Python 語言和PLC 梯形圖、SCL 結構化語言對軟件關鍵功能模塊進行了設計和代碼實現(xiàn)，最后在水池測試了軟件功能。現(xiàn)場測試表明，軟件整體達到預期設計目標，下一步將進一步優(yōu)化程序和完善軟件功能。