水下清洗機器人控制系統軟件設計

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

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

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

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

1 軟件系統結構

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

圖1 清洗機器人軟件系統結構

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

2 上位機程序設計

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

2.1 設計方案

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

圖2 上位機軟件結構

2.2 關鍵功能實現

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

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

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

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

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

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

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

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

2.3 界面設計

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

3 下位機程序設計

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

3.1 問題描述

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

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

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

3.2 解決方案

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

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

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

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

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

4 軟件測試

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

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

圖5 上位機軟件姿態界面

5 結論

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