清淤機器人導航定位及控制系統的設計與實踐

摘 要：為了提高清淤機器人的工作效率，本文融合PLC技術、模糊算法與PID控制技術，設計了一套清淤機器人的導航定位及控制系統。該系統由硬件和軟件2個部分組成。硬件部分包括導航定位模塊、控制模塊、液壓動力模塊以及輔助控制模塊；軟件部分包括PLC與PC機的通信流程以及PID控制器的實現。軟硬件模塊相互配合，共同完成清淤機器人智能化控制。試驗結果表明，應用該系統后，清淤機器人偏航角穩定時間為10 s，基本達到預期要求。系統性能良好，可以應用于清淤機器人設計與制造中。

關鍵詞：清淤機器人；導航定位；控制系統

中圖分類號：TP 242" " " " 文獻標志碼：A

隨著現代科學技術快速發展，工業領域逐漸采用清淤機器人完成清淤工作，極大程度地提升了清淤效率，并且減少清淤對人員身體健康的危害。經過大量實踐可知，傳統的清淤機器人采用慣性導航模式，隨著機器人運行時間不斷增加，導航誤差逐漸增大，降低了機器人作業效率，不能滿足現代工業領域清淤工作的要求[1]。針對這種情況，本文設計了一種清淤機器人導航定位以及控制系統，進一步提升清淤機器人運行效果。

1 系統總體框架設計

本文針對清淤機器人控制需求設計一種導航定位以及控制系統，其總體結構框架如圖1所示。該系統主要工作原理如下：清淤機器人啟動后，利用機器人本體上安裝的電磁換向器、攝像頭、陀螺儀、編碼器和水槍泵等裝置自動采集機器人運行情況及其周邊環境相關數據，這些數據經過初步處理后，通過以太網傳輸至集中控制臺，進行進一步的計算與分析，生成機器人的運行軌跡控制指令，并對機器人本體進行控制，從而實現機器人的自動導航定位與運行。

2 硬件設計

2.1 導航定位模塊

導航定位是整個系統的重要功能之一。將機器人控制在作業區域后，發射聲吶對周邊環境進行掃描，以確定作業區域基本輪廓，并逐步進行吸污作業。每吸污一段時間后，機器人會再次利用聲吶掃描周邊環境，獲取新的作業區域輪廓，直至整個區域被完全清理干凈[2]。針對這種功能需求，導航定位模塊被設計為由以下3個部分組成。

2.1.1 聲吶機

清淤機器人導航定位精度要求＜0.3 m，轉彎半徑誤差＜0.5 m，本文選擇了Oculus M1200d聲吶機，其運行頻率為1.2 MHz/2.5 MHz，測量范圍為0.1 m～40.0 m，橫向開角為130°/80°，豎向開角為20°/12°，開角分辨率為1°/0.6°。

2.1.2 陀螺儀

機器人偏航角、俯仰角和橫滾角要求誤差＜1°，因此本文選擇了AHR717陀螺儀測量機器人俯仰角、航向角與橫滾角，其運行帶寬為133 Hz，檢測精度為±0.2%。

2.1.3 編碼器

本文使用編碼器對陀螺儀、聲吶機采集的數據進行轉換，得到符合傳輸以及集中控制模塊計算所需的信號。由于控制系統采集數據種類眾多，因此對編碼器性能要求較高。本文選擇了型號為DG60L XSR的編碼器，其最大轉速為6 000 r/min，主軸軸向負載為40 N，徑向負載為100 N，允許軸向竄動為±1.5 mm，允許徑向跳動為±0.2 mm，運行電壓為10 VDC～30 VDC，運行環境溫度為-40 ℃～85 ℃。

2.2 控制模塊

控制模塊是系統的核心部分，負責對采集的數據進行計算，并自動控制機器人本體，使機器人能夠快速、準確地完成清淤作業。控制模塊由3個部分組成。

2.2.1 集中控制平臺

集中控制平臺負責計算和分析采集的數據，生成相應的控制指令，并將該指令傳輸給機器人本體，進行機器人導航定位與控制。集中控制平臺設備如下：工控機負責控制機器人本體采集和傳輸數據，并對這些數據進行初步處理；顯示屏用于顯示機器人本體的運行狀態及其周邊環境情況；S7-1200 PLC負責計算和分析采集的數據，生成控制指令；模擬量轉換模塊將數字量信號轉換為模擬量信號，以滿足控制指令的傳輸要求；電源為集中控制平臺提供電能，驅動控制平臺運行；繼電器用于調節和保護控制平臺內部的電路，以確保整個控制平臺的安全、穩定運行[3]。針對控制平臺內部設備情況，本文選擇規格為1 200 mm×600 mm×1 288 mm的柜體進行封裝。

2.2.2 動力柜

動力柜為整個系統的運行提供動力，主要設備如下：軟啟動器控制電機軟啟動，防止啟動電流過大導致電機損傷；斷路器保護電機電路，避免出現電流過載、短路和欠壓等故障；當電機出現異常狀況時，空氣開關自動切斷電源，避免造成更大危害。針對動力柜內部設備情況，本文采用規格為812 mm×500 mm×1 428 mm的柜體進行封裝。

2.2.3 水下控制箱

水下控制箱控制水下機器人本體，并與集中控制平臺進行數據傳輸，其內部結構除陀螺儀、聲吶機和編碼器外，還包括網絡交換機，用于轉發電信號；PLC對本體采集的數據進行初步處理；串口服務器將RS232、RS485或RS422等串行數據接口轉換為具有TCP/IP協議的網絡數據接口；穩壓模塊調節控制箱內部輸出壓力。針對控制箱內部設備情況，本文采用規格為350 mm×390 mm×180 mm的柜體進行封裝。

2.3 液壓動力模塊

液壓動力模塊負責驅動機器人行走。觀察機器人液壓模塊可知，其標準壓力為25 MPa，標準流量為14.5 L。為了符合這種要求，選用了功率為11 kW、標準電壓為380 V的三相異步電動機，液壓系統的液體工作溫度設定在25 ℃～70 ℃，箱體總容積為400 L。液壓動力模塊布置在地表，通過液壓管與機器人本體相連，管道最長為60 m。在液壓泵上安裝1個電磁閥，電機啟動后，電磁閥不動作，液壓泵自動加載負荷。系統出現故障后，電磁閥調節為卸油狀態，逐漸卸除泵內的油液，降低系統壓力，提升系統運行的安全性。在機器人本體上安裝1條液壓履帶底盤，其結構如圖2所示。

底盤總質量為180 kg，長度為820 mm，寬度為800 mm，高度為340 mm，能夠驅動機器人本體行走，行走速度峰值為1.8 m/min。在底盤安裝2臺液壓電機與減速機，液壓電機在液壓作用下驅動機器人本體移動，減速機控制機器人本體減速。2臺液壓電機型號均為ITM02，輸入流量為25 L/min，標準運行壓力為25 MPa，標準輸出扭矩為2 503 N·m，減速比為41.929，標準輸出轉速為70.94 r/min。

2.4 輔助控制模塊

輔助控制模塊主要由4個部分構成。1）吸污泵。用于吸收水中污染物。由于部分淤泥附著在池底，在一定程度上影響清淤效果，因此在清淤過程中，應當攪拌池底淤泥，使其變為懸浮狀態，便于清除。本文選用WQ65-30-40-7.5型吸污泵，其吸污能力良好，標準流量為30 m3/h，標準揚程為40 m，標準功率為7.5 kW，標準電壓為380 V，轉速為2 860 r/min，內部自帶攪拌元件，能自動攪拌池底淤泥。2）電動切換閥。用于調節電機的運轉狀態。淤機器人有開啟和關閉2種工作狀態，本文選用TOP-140型電動切換閥，其運行電壓為24 VDC，電機功率為70 W，標準電流為5.13 A，運行時間為12 s，輸出力矩為400 N·m。3）排污泵。用于排出吸收的污染物。本文選用150DL545-C型排污汞，其標準流量為150 m3/h，標準揚程為40 m，標準功率為45 kW。4）攝像頭。用于拍攝水下環境信息，型號為LBF-C50HD3P。

3 軟件設計

3.1 PLC與PC機通信流程

根據清淤機器人導航定位以及控制系統運行需求，設計了水下機器人本體PLC與水上集中控制平臺PC機之間的通信流程。PLC初始化后，嘗試與PC機進行連接，并判斷是否連接成功，如果連接成功，則流程結束；如果連接不成功，則繼續等待并判斷是否收到上位機指令。如果未收到指令，則繼續等待；如果收到指令，則將該指令直接傳輸給水下PLC[4]。通信流程控制算法如下。

S7.Net.Plc-PLC1214_2：

Public void Initialize_FrmMain（）

{

PLC1214_2=new Plc（CPU Type.S71200，”192.168.0.1”，0，0）;

System.Timers.Timer a =new System.Timers.Timer（）;

a.Interval=100;

a.Start（）：

a.Elapsed +=A Elapsed;

Private void" A_Elapsed（object sender， ElapsedEventArgs e）

yxxTitlel.TextRight=DateTime.Now. ToString0：

try

{

if （PLC1214 2.1sConnected）.

{

readFloatDB200=PLC1214_4=Read（DataType. Datablock， 200， 0， VarType.

Real，4）as float[];

readFloatDB200=-PLC1214_4=Read（DataType. Datablock， 200， 16， VarType.

Int，25）as Int16[];

readFloatDB300=PLC1214_4=Read（DataType. Datablock， 300， 14， VarType.

Real.53）as float[];

readFloatDB300=PLC1214_4=Read（DataType. Datablock， 300， 238， VarTyp

e.Int， 6）as Int16[];

readFloatDB310=PLC1214_4=Read（DataType. Datablock， 310， 8， VarType.

Real，2）as float[];

readFloatDB310=PLC1214_4=Read（DataType. Datablock， 310， 32， VarType.

Int， 9）as”Int16[];

}

Elser

{

PLC1214 2.0pen（）;

}

}

Catch {}.

}

3.2 PID控制器

為了達到清淤機器人智能控制的目的，本文基于交叉耦合原理設計了一種模糊比例-積分-微分（Proportion Integration Differentiation，PID）控制器。該控制器由姿態控制器與速度控制器2個部分構成，前者調控機器人姿態，后者調控機器人行走速度，2個部分控制原理基本相同。以姿態PID控制器為例，其框架如圖3所示。PID控制器運行原理如下：將水下機器人本體姿態信息傳輸至控制系統后，系統分別計算姿態偏差e與偏差變化率ce，作為控制器模糊推理的輸入參數，計算過程如公式（1）、公式（2）所示。

e=Aj/As" " " " （1）

（2）

式中：Aj為最佳姿態；As為實際測量姿態；t為測量時間。

在這個基礎上進行模糊推理，確定PID控制器的比例增益系數Kp、積分系數Ki和微分系數Kd，作為PID控制器的輸入參數。將Kp設為0，模擬系統圖形，利用圖形觀察并判斷Kp是否合理，如果不合理，那么增加Kp，直至系統圖形區域穩定，得到Kp值。采用經驗法確定Kd，通常為Kp的1/2。計算Ki，Ki=Kd×1%。

利用PID控制器進行計算，得到2個速度值，分別將其作為不同速度環的輸入參數，并再次使用模糊PID控制器進行計算，得到左側履帶與右側履帶的速度值。模糊控制運算計算過程如公式（3）～公式（5）所示。

^=min（（x），（x）） " " （3）

^=max（（x），（x）） " "（4）

（x）=1-（x） " " " " " " "（5）

式中：為左側履帶的模糊集合；為右側履帶的模糊集合；為的隸屬度函數；為的隸屬度函數，、取值范圍均為[0，1]；為的補集。

根據2個速度環計算結果，完成機器人本體姿態解析作業，達到機器人姿態智能控制的目的[5]。

4 試驗分析

按照上述內容開發清淤機器人的導航定位及控制系統，并通過模擬試驗對該系統的應用效果進行分析。在試驗過程中，構建1個規格為5.0 m×2.0 m×1.5 m的水池，在水池底部平鋪一層黃土并灌滿清水，在水池周邊建立電壓為380 V 與 220 V 的電源系統。將控制機器人移動至水池中，研究人員在水池旁控制機器人運行狀態，使機器人向3.5°方向行走，利用機器人本體上安裝的陀螺儀實時采集機器人的偏航角，并繪制相應的姿態調節圖像，如圖4所示。

由圖4可知，向機器人發送控制指令后，機器人偏航角呈波動上升的趨勢，當時間約為10 s時，偏航角約為3.5°，并保持波動穩定狀態。說明系統控制效果較好，提升了清淤機器人作業效率，可以大規模推廣。

5 結語

綜上所述，本文設計了一種清淤機器人的導航定位及控制系統，該系統由導航定位模塊、控制系統模塊、液壓動力模塊和輔助模塊等組成。在各模塊以及PID控制器的共同作用下，實現了機器人的導航定位功能，保證了機器人按照預設軌跡行走，為機器人的清淤作業打下了良好基礎。

參考文獻

[1]馮業寧，李體錦，周圍，等.基于Recurdyn的清淤機器人水下爬行運動學仿真[J].上海工程技術大學學報，2024，38（2）：171-178.

[2]楊偉，馬惠民，宋朝英，等.礦用淤泥自適應水倉清淤機器人研究與應用[J].中國煤炭，2024，50（3）：91-96.

[3]康向南.礦用水倉清淤機器人液壓越障自適應控制技術[J].當代化工研究，2023（24）：171-173.

[4]張強，郭高陽，胡宴才，等.狀態受限下的水下履帶式清淤機器人軌跡跟蹤控制[J].中國艦船研究，2023，18（3）：57-65.

[5]嵇鵬程，嵇鑫宇.城市管道清淤機器人數學建模與MATLAB仿真[J].陜西水利，2021（5）：10-13，17.