基于模糊PID的深海采礦機器人路徑跟蹤控制

毛競航，呂海寧，楊建民，劉 磊

(1. 上海交通大學 海洋工程國家重點實驗室，上海 200240； 2. 高新船舶與深海開發(fā)裝備協(xié)同創(chuàng)新中心，上海 200240)

深海多金屬結核等礦物資源的開采技術是海洋資源開發(fā)技術的最前沿，標志著一個國家開發(fā)海洋資源的綜合能力和技術水平，正越來越受到社會各界的關注。

深海采礦機器人技術是深海多金屬結核采礦系統(tǒng)的關鍵技術[1]，采礦作業(yè)中，采礦機器人需攜帶礦石收集裝置和礦石輸送裝置覆蓋式地走遍整個礦區(qū)。由于深海海泥含水率高，顆粒細小，其抗壓強度及抗剪切能力弱，故采礦機器人常采用履帶式行進系統(tǒng)。采礦機器人在海底行走時，由于海底沉積物稀軟，采礦機器人極易打滑，且存在海流、地形等擾動使得采礦機器人容易偏離預定行駛路線，影響行走精度，且履帶系統(tǒng)的非線性增加了其控制難度。

對于地面行走機器人，已經(jīng)提出了多種路徑跟蹤方法，如follow-the-carrot方法[2]、純跟蹤方法[3-4]、向量追蹤方法[5-6]等。Yeu等[7]和Yoon等[8]將跟蹤算法與深海采礦機器人定位相結合，提出了深海采礦機器人的路徑跟蹤算法。但是，地面路徑跟蹤算法通常忽略打滑的情況，而深海的環(huán)境相比于陸地環(huán)境，打滑是深海采礦機器人運動控制中不可忽略的一項。李力等[9]基于采礦機器人運動學建立采用PID控制履帶左右速度實現(xiàn)預定行走速度模型和采用模糊控制實現(xiàn)自動跟蹤預定開采路徑模型，并完成了在多種目標路徑下的仿真試驗，考慮了隨機打滑下海底機器人的行走性能?；谶\動學模型的控制算法，由于其未考慮系統(tǒng)輸入與打滑的相互關系，容易導致由于系統(tǒng)輸入引起的打滑問題。

隨著對深海泥土力學性質(zhì)研究[10-12]及軟土地上履帶車行駛性能研究的深入[13-15]，Hong等[16]提出了一種基于動力學模型的路徑跟蹤算法。Dai等[17]在RecurDyn/Track和Matlab/Simulink中建立了海底履帶車聯(lián)合仿真模型，提出了一種自適應模糊神經(jīng)控制算法。Yeu等[18]使用改進的向量追蹤路徑跟蹤控制算法，并通過數(shù)值模擬驗證了該算法。基于動力學方程的算法有效提高了控制精度，但是其使用的打滑模型是一個根據(jù)土壤剪切應力統(tǒng)計值建立的模型，其打滑率在機器人運動過程中不會發(fā)生變化；而在工程實際中，由于土壤并不均勻，機器人在運動過程中土壤牽引力與打滑率之間的關系并不恒定，且牽引力對打滑率的變化敏感，打滑率的微小變化會引起牽引力的巨大變化，這容易導致實際應用中控制效果較差。為了克服運動學模型未分析打滑及動力學模型只能反應統(tǒng)計值的缺點，采取了動力學模型獲得打滑關系指導在運動學模型中建立控制器的方式，既減弱了由于動力學模型不精確對系統(tǒng)的影響，又將打滑問題引入到控制算法中，實現(xiàn)了考慮打滑情況的采礦機器人路徑跟蹤。

1 采礦機器人模型

將采礦機器人的運動放在一個二維平面上以簡化采礦機器人模型，因此車輛的運動可以簡化為一個3自由度的運動(x,y,θ),其中，x，y表示采礦機器人的坐標。如圖1所示，δ1、δ2分別為大地坐標系和車體坐標系。大地坐標系δ1的原點Oδ1可設為采礦機器人運動平面內(nèi)任意點，x軸指向正東方向，y軸指向正北方向，θ以逆時針方向為正方向。車體坐標系δ2的原點Oδ2設于采礦機器人底盤形心處，x軸指向車體正前方，y軸指向車體的右側，θ以逆時針方向為正方向。

圖1 采礦機器人參考系Fig. 1 Reference frame of mining robot

在兩個坐標系間采礦機器人的運動轉換滿足：

(1)

式中：n1=[X1,Y1,θ1]為大地坐標系下采礦機器人的位置和艏向坐標，ν1=[u1,ν1,ω1]為車體坐標系下采礦機器人的運動速度角速度向量。Jθ(n1)為由坐標系δ2至坐標系δ1的轉換矩陣：

(2)

采礦機器人運動學模型如圖2所示，其運動學方程為：

圖2 采礦機器人運動學模型Fig. 2 Kinematic model of mining robot

(3)

采礦機器人動力學模型如圖3所示，其動力學方程如下[19]：

圖3 采礦機器人動力學模型Fig. 3 Dynamic model of mining robot

(4)

其中，M為采礦機器人總質(zhì)量，F(xiàn)L、FR分別為左、右履帶的牽引力，RL、RR為左、右履帶所受阻力，fy為側向阻力，d為重心和順時旋轉中心間的距離，D為采礦機器人兩側履帶中心距，L為采礦機器人履帶接地長度。

采礦機器人兩側履帶的牽引力FL、FR通過與土壤的剪切作用產(chǎn)生，采礦機器人運動過程中，履帶所受牽引力可視為履帶各點處所受剪切力的合力，牽引力與履帶運動的關系可用式(5)表示：

(5)

其中，b為單側履帶寬度；τL[jL(x)]，τR[jR(x)]為土壤剪切應力，jL(x)，jR(x)為距離履帶接地段最前端x處的剪切位移。

根據(jù)研究[20]，深海海泥的剪切應力—剪切位移特性曲線如圖4所示。

圖4 土壤剪切力—剪切位移關系Fig. 4 The relation of shear force-shear displacement

從圖4中可以看出，隨著剪切位移的增加，其剪切應力先快速增大至最大值τmax，隨后逐漸減小至殘余應力值τres并保持在τres不變。對于該種土壤剪切應力與剪切位移間的關系，Wong等[21]提出了如下關系式：

(6)

式中：Kr為殘余剪切應力τres與最大剪切應力τmax的比值。Kω為出現(xiàn)最大剪切應力τmax時的剪切位移，結合礦區(qū)土質(zhì)，Kr取0.28，Kω取0.035[20]。

采礦機器人運動時，采礦機器人履帶不可收縮，故履帶各點處的剪切位移為：

(7)

(8)

最終獲得履帶推力與打滑率的關系如圖5所示。

圖5 采礦機器人牽引力與打滑率關系Fig. 5 Relationship between traction force and slip rate of the mining robot

由圖5可知，打滑率約為2.5%時土壤提供采礦機器人最大牽引力，隨著打滑率上升，牽引力逐漸下降并穩(wěn)定。根據(jù)土壤提供牽引力隨打滑率的變化關系將其分為3個階段：正增益階段，牽引力隨打滑率上升增加至最大過程，該過程中，可以通過提高履帶轉速來增加牽引力；負增益階段，牽引力隨打滑率上升而下降至接近穩(wěn)定過程，該過程中，由于牽引力隨打滑率上升是下降的，此時提高轉速，采礦機器人受到的牽引力下降，打滑率將進一步提高導致控制效果反向；無增益階段，牽引力隨打滑率上升基本保持不變，該過程中無法通過改變履帶轉速來控制采礦機器人。因此將機器人打滑情況限制于正增益階段內(nèi)，機器人運動控制效果最好。

采礦機器人運動過程中，采礦機器人徑向所受阻力RL、RR主要由兩部分組成：壓實阻力和推土阻力。

壓實阻力[22]可表示為：

(9)

其中，M為采礦機器人質(zhì)量，b為采礦機器人單側履帶寬度，h為單側履帶承重輪數(shù)，p為履帶板節(jié)距。

推土阻力[20]可表示為：

Rb=b(0.67czKc+0.5z2γKγ)

(10)

其中，γ為沉積物比重，z為土壤壓實深度，c為黏聚系數(shù)。Kc、Kγ為被動土壓系數(shù)，可由式(11)、(12)獲得：

Kc=(Nc-tan)cos2

(11)

(12)

采礦機器人旋轉阻力fy可用式(13)計算[23]：

(13)

式中：μ為履帶與地面摩擦系數(shù)，取μ=0.08；eG為重心偏心距，假設機器人重心位于形心，即取e=0。

2 路徑跟蹤算法

采礦機器人跟蹤的路徑通過一系列帶艏向的三維坐標向量表示，如式(14)所示：

(14)

其中，xn，yn，θn分別為路徑點的橫坐標、縱坐標及艏向坐標。n為當前路徑點的編號。

采礦機器人路徑跟蹤目標艏向角定義如圖6所示，圖中P1=(xn,yn,θn)為機器人當前目標點，P0=(X,Y,θ)為當前機器人位置。虛線OP1為目標點P1按其艏向坐標θn方向的延長線，O點為采礦機器人位置P0到線OP1的垂足。為采礦機器人設計在當前點的目標艏向，其方程為：

圖6 路徑跟蹤目標艏向角定義Fig. 6 Definition of path tracking target heading angle

θt=arctan(|D1|)×jud(P0,P1)

(15)

式中：D1為P0至虛線OP1的距離。根據(jù)P0、P1坐標可由式(16)計算D1：

(16)

其中，dot(a,b)為向量a在b上的投影。Δθ為采礦機器人目標艏向與θn的偏差絕對值，jud(P0,P1)為判斷系數(shù)用于表示θt的方向，由式(17)獲得：

(17)

以設計目標艏向為切線方向，從P0開始作曲線l。沿曲線l運動采礦機器人最終將運動至虛線OP1。D1越大，θt越大，采礦機器人趨近虛線OP1速度越快，系統(tǒng)有更好的快速性；D1越小，θt越小，采礦機器人趨近虛線OP1速度越慢，系統(tǒng)有更好的穩(wěn)定性。

3 模糊PID控制器

由于采用運動學模型建立控制器未考慮打滑這一重要因素，而采用動力學模型設計艏向控制器存在對模型的精度要求高而打滑模型必然不準確的矛盾，故采用通過運動學模型建立控制器和通過動力學模型分析控制器輸入限制相結合的方式設計采礦機器人艏向控制器。

傳統(tǒng)的PID控制器由于其結構簡單，魯棒性和適應性強且不依賴于具體模型的特點被廣泛用于工業(yè)控制中[24]。然而，傳統(tǒng)的PID控制器對于強非線性系統(tǒng)，特別是復雜的沒有精確模型的系統(tǒng)無法起到良好地控制效果。模糊PID控制器通過增加模糊模塊，根據(jù)誤差及誤差變化動態(tài)調(diào)節(jié)PID的控制系數(shù)，提高了控制器的精度和響應速度，同時增加了其對沒有精確模型的系統(tǒng)控制能力[25]。

采礦機器人艏向角的偏差模型為：

e=θt+θn-θ

(18)

模糊PID控制框圖如圖7所示，艏向偏差e及其變化率Δe為控制系統(tǒng)輸入，模糊控制器根據(jù)e及其變化率Δe動態(tài)調(diào)節(jié)PID控制器的控制參數(shù)kp、ki、kd，再通過PID控制器計算所需角度差。

圖7 模糊PID控制器Fig. 7 Fuzzy PID controller

E及ΔE為艏向偏差e及其變化率Δe的模糊變量，其論域為{-3,-2,-1,0,1,2,3}。Kp、Ki、Kd為PID控制參數(shù)kp、ki、kd的模糊變量，其論域為{0,1,2,3,4,5,6}。

其具體流程如下：系統(tǒng)首先對輸入量進行模糊處理，將系統(tǒng)輸入e、Δe轉換為模糊變量E及ΔE,再通過模糊控制規(guī)則表，獲得Kp、Ki、Kd，后采用重心法進行解模糊處理得到控制參數(shù)kp、ki、kd。模糊控制器的輸入輸出關系如圖8～10所示。

圖8 控制參數(shù)kpFig. 8 Control parameterkp

圖9 控制參數(shù)kiFig. 9 Control parameter ki

圖10 控制參數(shù)kdFig. 10 Control parameter kd

對于kp，當e為0時，kp取最小值，當e較大時選用較大的kp使系統(tǒng)有較大的調(diào)節(jié)速度，當Δe增大，kp也適當增大從而保證調(diào)節(jié)速度。對于ki，當e及Δe較小時，選用較大的ki以增加系統(tǒng)穩(wěn)定性，當e及Δe較大時，選用較小的ki以減小系統(tǒng)的超調(diào)量。對于kd，當e較小且Δe較大時，選用較大的kd以減小系統(tǒng)超調(diào)量，當e及Δe為同向且為大值時，選用較大的kd增加系統(tǒng)調(diào)節(jié)速度，當e及Δe為反向且較大時，選用小的kd減小對比例環(huán)節(jié)的影響，增加調(diào)節(jié)速度。

PID控制器獲得控制參數(shù)后輸出兩側主動輪轉速差值Δω：

(19)

此時，采礦機器人左右履帶主動輪的目標轉速為:

(20)

其中，ω0為采礦機器人左右履帶主動輪初始轉速，ωL、ωR為采礦機器人左右履帶主動輪的目標轉速。

(21)

式中：ωLs、ωRs為采礦機器人左右履帶主動輪實際轉速，kω為設定的履帶主動輪角加速度。

通過前文對打滑階段的分析，將打滑率限制于正增益階段實際控制效果最好，而由于打滑問題非線性且較為復雜，難以直接通過數(shù)值求解的方式獲得合適的履帶主動輪角加速度kω，因此采用數(shù)值模擬的方式，模擬不同的履帶角加速度下打滑率變化情況及最大打滑，據(jù)此選擇合適的履帶主動輪角加速度值。

首先，為了保證獲得的最大打滑率大于等于實際中的最大打滑率，將模擬輸入設置為模糊PID控制器輸出的最大值，即Δω=4，其后選擇不同的履帶主動輪角加速度數(shù)值仿真獲得不同主動輪角加速度下的打滑率，如圖11展示了部分kω下打滑率的數(shù)值仿真結果。

圖11 不同kω下打滑率的數(shù)值仿真結果Fig. 11 Numerical simulation results of slip rate under different kω

以不同主動輪角加速度為橫軸，其對應的加速過程中履帶最大打滑率為縱軸作圖，如圖12所示，從圖中可以看出，主動輪角加速度與履帶最大打滑率近似為線性關系，為了避免控制輸入引起的過度打滑，且使系統(tǒng)有較快反應和一定的打滑余量，最終選擇履帶主動輪角加速度kω=2 rad/s2。

圖12 主動輪角加速度與履帶最大打滑率關系Fig. 12 Relationship between angular acceleration of driving wheel and maximum skid rate of track

4 仿真及結果分析

為驗證該算法路徑跟蹤效果，使用Matlab/Simulink軟件基于動力學方程建立采礦機器人的動力學仿真模型，并基于該模型對算法進行了數(shù)值仿真分析，表1給出了采礦機器人參數(shù)及土壤參數(shù)。分別針對系統(tǒng)在直線變道、圓周曲線路徑和采礦工況下的路徑跟蹤效果進行驗證。

表1 采礦機器人參數(shù)及土壤參數(shù)Tab. 1 Mining robot parameters and soil parameters

4.1 直線變道

采礦機器人從當前路徑變道至相隔一定距離的路徑并保持艏向不變。圖13為直線變道的仿真結果，兩條路徑間間隔為8 m。

圖13 直線變道仿真結果Fig. 13 Simulation results of linear lane change

圖13(a)為路徑跟蹤效果，可以看出，在跟蹤變道路徑時，采礦機器人跟蹤效果優(yōu)秀。圖13(b)為變道工況下采礦機器人的艏向跟蹤情況，從圖中可以看出，在變道過程中，存在兩次目標艏向角的突變，這是由目標點變化造成的，這種突變可以通過改變采礦機器人的提前轉向點進行削弱。采礦機器人的目標艏向有一定的滯后但控制器保持良好的控制效果，這同時也解釋了采礦機器人運動小幅振蕩的原因。圖13(c)為采礦機器人兩側履帶的打滑情況，在t為13 s時，左側履帶打滑率達到了最大值，在t為10 s時，右側履帶打滑率達到了最大值，兩者均在打滑限制范圍內(nèi)，限制打滑效果良好。圖13(d)為跟蹤誤差曲線，從圖中可以看出跟蹤誤差最大值僅有0.3 m且能迅速恢復。

4.2 圓周曲線路徑

圖14展示了半徑為5 m的圓周曲線路徑的跟蹤效果。從圖14(a)中可以看出，跟蹤5 m半徑的路徑時，系統(tǒng)有出色的跟蹤效果。圖14(b)為采礦機器人艏向跟蹤效果，圖中目標艏向的每一次跳躍即為一次目標點切換，結合艏向誤差，可以看到在初始兩個目標點跟蹤過程中，艏向誤差相對較大，為0.06 rad，隨后的目標點誤差減小，這說明該控制器具有一定的適應能力。圖14(c)為采礦機器人兩側履帶打滑率，可以看出打滑率最大值約為0.3%。圖14(d)為跟蹤誤差曲線。

圖14 曲線跟蹤仿真結果Fig. 14 Simulation results of circular path

4.3 采礦工況

圖15展示了采礦工況下的路徑跟蹤結果，采礦工況的路徑由直線及半徑為5 m的圓弧組成，并在兩側履帶增加了額外的干擾阻力。從圖15(a)中可以看出，在直線段，采礦機器人的位置誤差僅為4 cm，在全程，采礦機器人均有良好的跟蹤效果。圖15(b)為采礦機器人艏向跟蹤效果。圖15(c)為采礦機器人兩側履帶打滑率，可以看出打滑率最大值約為0.5%。圖15(d)為采礦機器人兩側額外干擾阻力。在前50 s，采礦機器人做直線運動，此階段采礦機器人運動較穩(wěn)定，從50 s開始，采礦機器人進入曲線階段，兩側履帶打滑率開始增加。在每次進入及離開曲線段后至穩(wěn)定前的一段時間內(nèi)，采礦機器人打滑率均有一定程度的增加，但依舊在2.5%的限度內(nèi)，說明防止過度打滑效果良好。圖15(e)為跟蹤誤差曲線，對比圖15(b)可以看出，在艏向角切換時，機器人的運動偏差較大。

圖15 作業(yè)工況仿真結果Fig. 15 Simulation results of working path

5 結 語

針對深海采礦機器人路徑跟蹤問題，經(jīng)過分析得到如下結論：

1) 提出了一種基于模糊PID的深海采礦機器人路徑跟蹤控制器。根據(jù)采礦機器人與當前目標點相對位置采用反正切函數(shù)計算采礦機器人的當前位置目標艏向角，通過采礦機器人按目標艏向角方向運動從而實現(xiàn)路徑跟蹤。

2) 通過運動學模型建立了采礦機器人艏向模糊PID控制算法，同時為避免采礦機器人過度打滑，通過動力學模型數(shù)值模擬獲得了最大打滑率和履帶主動輪角加速度的關系，并據(jù)此限制履帶主動輪的角加速度來防止嚴重打滑。

3) 根據(jù)深海采礦機器人動力學模型及海底軟泥性質(zhì)在Matlab/Simulink軟件建立了采礦機器人運動仿真模型。對幾個典型路徑跟蹤問題進行了仿真分析，仿真結果表明，基于艏向控制的控制器能夠良好地保證采礦機器人按目標路徑行走。

雖然該路徑跟蹤控制器有良好的路徑跟蹤效果，但其仍存在如下問題需要進一步解決：1)未解決因外界因素導致打滑進入負增益階段時的控制；2)控制器未達最優(yōu)狀態(tài)，可采用深度學習等方法對模糊控制器進行進一步的優(yōu)化。