清淤機器人導(dǎo)航定位及控制系統(tǒng)的設(shè)計與實踐

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

關(guān)鍵詞：清淤機器人；導(dǎo)航定位；控制系統(tǒng)

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

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

1 系統(tǒng)總體框架設(shè)計

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

2 硬件設(shè)計

2.1 導(dǎo)航定位模塊

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

2.1.1 聲吶機

清淤機器人導(dǎo)航定位精度要求＜0.3 m，轉(zhuǎn)彎半徑誤差＜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 編碼器

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

2.2 控制模塊

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

2.2.1 集中控制平臺

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

2.2.2 動力柜

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

2.2.3 水下控制箱

水下控制箱控制水下機器人本體，并與集中控制平臺進行數(shù)據(jù)傳輸，其內(nèi)部結(jié)構(gòu)除陀螺儀、聲吶機和編碼器外，還包括網(wǎng)絡(luò)交換機，用于轉(zhuǎn)發(fā)電信號；PLC對本體采集的數(shù)據(jù)進行初步處理；串口服務(wù)器將RS232、RS485或RS422等串行數(shù)據(jù)接口轉(zhuǎn)換為具有TCP/IP協(xié)議的網(wǎng)絡(luò)數(shù)據(jù)接口；穩(wěn)壓模塊調(diào)節(jié)控制箱內(nèi)部輸出壓力。針對控制箱內(nèi)部設(shè)備情況，本文采用規(guī)格為350 mm×390 mm×180 mm的柜體進行封裝。

2.3 液壓動力模塊

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

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

2.4 輔助控制模塊

輔助控制模塊主要由4個部分構(gòu)成。1）吸污泵。用于吸收水中污染物。由于部分淤泥附著在池底，在一定程度上影響清淤效果，因此在清淤過程中，應(yīng)當攪拌池底淤泥，使其變?yōu)閼腋顟B(tài)，便于清除。本文選用WQ65-30-40-7.5型吸污泵，其吸污能力良好，標準流量為30 m3/h，標準揚程為40 m，標準功率為7.5 kW，標準電壓為380 V，轉(zhuǎn)速為2 860 r/min，內(nèi)部自帶攪拌元件，能自動攪拌池底淤泥。2）電動切換閥。用于調(diào)節(jié)電機的運轉(zhuǎn)狀態(tài)。淤機器人有開啟和關(guān)閉2種工作狀態(tài)，本文選用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）攝像頭。用于拍攝水下環(huán)境信息，型號為LBF-C50HD3P。

3 軟件設(shè)計

3.1 PLC與PC機通信流程

根據(jù)清淤機器人導(dǎo)航定位以及控制系統(tǒng)運行需求，設(shè)計了水下機器人本體PLC與水上集中控制平臺PC機之間的通信流程。PLC初始化后，嘗試與PC機進行連接，并判斷是否連接成功，如果連接成功，則流程結(jié)束；如果連接不成功，則繼續(xù)等待并判斷是否收到上位機指令。如果未收到指令，則繼續(xù)等待；如果收到指令，則將該指令直接傳輸給水下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控制器

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

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

（2）

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

在這個基礎(chǔ)上進行模糊推理，確定PID控制器的比例增益系數(shù)Kp、積分系數(shù)Ki和微分系數(shù)Kd，作為PID控制器的輸入?yún)?shù)。將Kp設(shè)為0，模擬系統(tǒng)圖形，利用圖形觀察并判斷Kp是否合理，如果不合理，那么增加Kp，直至系統(tǒng)圖形區(qū)域穩(wěn)定，得到Kp值。采用經(jīng)驗法確定Kd，通常為Kp的1/2。計算Ki，Ki=Kd×1%。

利用PID控制器進行計算，得到2個速度值，分別將其作為不同速度環(huán)的輸入?yún)?shù)，并再次使用模糊PID控制器進行計算，得到左側(cè)履帶與右側(cè)履帶的速度值。模糊控制運算計算過程如公式（3）～公式（5）所示。

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

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

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

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

根據(jù)2個速度環(huán)計算結(jié)果，完成機器人本體姿態(tài)解析作業(yè)，達到機器人姿態(tài)智能控制的目的[5]。

4 試驗分析

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

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

5 結(jié)語

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

參考文獻

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

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

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

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

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