履帶移動平臺帶邊界層bang-bang路徑跟蹤控制

吳明陽， 李小波， 代嘉惠， 雷世威， 潘長松， 薛春榮

(中煤科工集團重慶研究院有限公司， 重慶 400037)

0 引言

煤礦機器人是煤礦智能化的重要載體，研發和應用煤礦機器人是實現煤礦無人化的重要途徑[1]。國家煤礦安全監察局發布的《煤礦機器人重點研發目錄》建議將機器人納入煤礦重點科技專項范圍[2]，以提升煤礦智能化水平，逐步實現煤礦少人及無人化。

路徑跟蹤控制是實現煤礦機器人自主導航行走的關鍵技術之一。當完成規劃任務后，需要利用跟蹤算法計算機器人執行器目標輸入，以保證機器人跟蹤預定的路徑[3]。現有的機器人路徑跟蹤算法包括純追蹤算法[4]、滑模控制算法[5-6]、模型預測控制算法[7]等。其中純追蹤算法簡單、直觀，控制參數少，在低速場合跟蹤效果良好，因此，成為當前移動機器人導航和控制中廣泛應用的路徑跟蹤算法之一[8-9]。前視距離是影響純追蹤算法跟蹤精度的重要因素，為實現前視距離的自適應調整，李逃昌等[10]提出基于模糊理論調整純追蹤算法的前視距離；李革等[11]通過定義以速度和彎度為自變量的函數，實現前視距離調整，平均跟蹤誤差為0.077 m。

已有的路徑跟蹤算法均需采集機器人兩側驅動輪轉速作為反饋量進行轉速閉環控制，對于開關閥控制不可閉環調速的液壓履帶移動平臺并不完全適用。由于成本、加工難易度及用戶喜好等因素，開關閥控液壓履帶移動平臺在礦業、農業、軍工等領域應用十分廣泛。目前，在掘進機[12]、鉆孔機[13]、噴漿機[14]等大中型煤礦機械中均大量使用該類型底盤。因此，研究開關閥控履帶移動平臺的路徑跟蹤控制對于提高煤礦機械智能化水平具有重要意義。

針對開關閥控履帶移動平臺路徑跟蹤問題，本文借鑒純追蹤算法的思想，提出一種基于航向角偏差帶邊界層的bang-bang路徑跟蹤控制算法，并通過李雅普諾夫穩定性定理分析了邊界層內外系統狀態的穩定性；通過仿真驗證算法在不同前視距離下對矩形目標路徑的跟蹤效果，并與純追蹤算法進行對比，分析兩者的路徑跟蹤性能差異；最后，在試驗樣機上對bang-bang控制算法進行了地面試驗。

1 履帶移動平臺路徑跟蹤控制模型

定義導航坐標系為xoy，由于平臺速度較低，忽略縱向滑移和側向滑移，假設重心與幾何中心重合，則履帶移動平臺點追蹤簡化運動學模型如圖1所示。圖1中，(x,y)為平臺實時位置；(xr,yr)為當前時刻追蹤的目標路徑點；θ為航向角；θerr為航向角偏差；ρ為當前位置和目標路徑上目標點的距離；r為純追蹤算法轉彎半徑；l為純追蹤算法前視距離；a，b分別為機器人局部坐標系下前視點到機器人幾何中心的側向距離和縱向距離；d=r-a;Δx,Δy分別為全局坐標系下目標路徑點到平臺當前位置的距離在x，y方向的分量。

圖1 履帶移動平臺點追蹤簡化運動學模型Fig.1 Simplified kinematics model of point tracking for crawler mobile platform

履帶移動平臺的動力學響應可近似為一階環節，定義ρ、θerr、速度v及航向角速度ω為狀態變量，根據幾何及運動學關系得到點追蹤模型[15]：

(1)

式中：Tv和Tω為一階系統時間常數；uv和uω分別為速度及航向角速度的虛擬控制輸入。

定義左右兩側履帶線速度為vl和vr，則其與虛擬控制輸入的關系為

(2)

式中h為履帶間距。

2 履帶移動平臺路徑跟蹤控制算法

2.1 純追蹤算法

純追蹤算法是一種幾何控制算法，可模擬人駕駛車輛行為對平臺進行控制。純追蹤算法僅包含前視距離l一個可調參數，在平臺運動過程中，平臺將依據自身位置和目標路徑搜尋目標路徑前進方向上不小于前視距離且與前視距離最接近的離散路徑點作為目標點，然后基于幾何關系獲得目標曲率。設目標曲率為γ，根據圖1可得

(3)

因為sinθerr=a/l，所以，式(3)可寫為

(4)

從式(4)可看出，純追蹤算法本質上是依據航向角偏差進行控制，算法以前視距離l作為調整參數，選擇較大的l可使跟蹤平穩，但收斂時間變長；選擇較小的l可提高跟蹤精度，但l過小可能造成振蕩甚至不穩定。

定義速度控制輸入為uv=v2，結合式(4)可得到航向角速度輸入量：

uω=v2γ

(5)

在獲得虛擬控制量uv和uω后，利用式(2)可得到履帶的目標速度。根據式(2)，純追蹤算法需要通過控制兩側履帶速度得到虛擬控制量uv和uω，但是開關閥控履帶移動平臺兩側履帶不可閉環調速，因此，純追蹤算法無法直接在開關閥控履帶移動平臺上應用。

2.2 bang-bang控制算法

bang-bang控制又稱繼電控制，適用于開關控制輸入系統，便于編程實現及調試，參數校準及調整簡單，在航天、液壓等領域均有應用。bang-bang控制屬于變結構控制，可在2種狀態之間切換，開關閥控履帶移動平臺的控制信號包含開、關2種狀態，因此，本文基于航向角偏差，利用bang-bang控制實現平臺的路徑跟蹤。

(6)

(7)

根據式(5)，uω的符號與v2和γ有關，由于vr=uv>0，故sign(uω)=sign(γ)。根據式(4)，由于前視距離l>0，故sign(γ)=sign(θerr)。綜上可得，sign(uω)=sign(θerr)，故式(6)、式(7)中uω可以用θerr替換：

(8)

(9)

由于存在擾動或傳感器噪聲，式(8)、式(9)會造成電磁閥頻繁切換。為減小擾動對系統的影響及電磁閥切換頻率，借鑒滑模控制中減小抖振的方法，引入邊界層，則控制律修改為

(10)

(11)

式中ε為邊界層厚度，rad，0<ε?π/2，ε越大，電磁閥切換頻率越低，但跟蹤精度也會下降。

在實際應用中，應在保證滿足跟蹤精度要求的前提下，選擇較大的ε，以保護執行器。

對于開關閥控制履帶移動平臺，其兩側履帶的線速度大小近似相等，即vl≈vr。因此，根據式(2)可得到在式(10)、式(11)作用下的虛擬控制輸入：

(12)

(13)

式中：v3=(vr+vl)/2；ω0=2(vr+vl)/h。

從式(12)、式(13)可知，當航向角偏差超過邊界層時，移動平臺通過原地轉向調整姿態，從而減小航向角偏差；當航向角偏差在邊界層之內時，平臺朝目標點直線行駛。因此，該算法適用于直線或折線目標路徑。

3 穩定性分析

穩定是控制系統的重要性能，也是系統能夠正常運行的首要條件。控制系統在實際運行過程中，會受到內外因素的擾動。如果系統不穩定，任何微小的擾動作用都將導致系統偏離原來的平衡狀態，并隨著時間發散。因而，分析閉環系統的穩定性，是控制理論的基本任務之一。李雅普諾夫穩定性理論[16]是控制領域判斷系統穩定性的普遍理論，不僅適用于單變量、線性、定常系統，對于多變量、非線性、時變系統同樣適用。由于本文中的閉環控制系統為非線性系統，證明其李雅普諾夫穩定性，可判斷當系統狀態在平衡點鄰域內不會發散，即航向角偏差、到目標點的距離等關鍵狀態會收斂至原點附近的鄰域內，從而保證路徑跟蹤正常運行。

根據式(12)、式(13)，在路徑跟蹤過程中存在原地姿態調整和直線行駛2種動態過程，2種動態過程根據θerr的值進行切換。下面分別討論2種動態過程中閉環系統的穩定性。

3.1 原地姿態調整過程中的穩定性

移動平臺在原地調整姿態下，|θerr|≥ε，將式(12)、式(13)代入式(1)，得

(14)

定義該過程初始時刻速度v(0)=v0，v在原點漸進穩定，其解為

v=v0exp(-t/Tv)

(15)

式中t為時間。

將式(15)代入式(14)，消去v，得

(16)

(17)

基于李雅普諾夫穩定性理論分析簡化后狀態空間模型的穩定性，得出如下結論：當|θerr|≥ε時，在式(12)、式(13)的作用下，Tω>0且ω0>0時，系統在原點是李雅普諾夫穩定的。下面對該結論進行證明。

選取備選李雅普諾夫函數：

(18)

當θerr≠0，ω≠0時，因為ω0>0，Tω>0，得到2Tωω0θerrsign(θerr)>0。

(19)

當θerr=0，ω=0時，V=0。

(20)

3.2 沿直線行駛過程中的穩定性

移動平臺在沿直線行駛狀態下，|θerr|<ε，將式(12)、式(13)代入式(1)，得

(21)

分析ρ和v的穩定性，得出如下結論：當|θerr|<ε時，在式(12)、式(13)的作用下，v在平衡點v3處是漸進穩定的，ρ在原點處是李雅普諾夫穩定的。下面對該結論進行證明。

先證明v在平衡點v3處漸進穩定。在該過程的初始時刻，有v(0)=0，根據式(21)可得到v(t)=v3-v3exp(-t/Tv)。因此，v是漸進穩定的，其平衡點為v3。當t>0時，v>0且最終收斂至v3。

下面證明ρ在原點處李雅普諾夫穩定。當t>0時，選擇備選李雅普諾夫函數：

(22)

對式(22)，當且僅當ρ=0時，V=0，否則，V>0。對V求導，并代入v(t)可得

(23)

上述證明過程說明，當|θerr|<ε且ε?π/2時，狀態θerr和ω并不影響ρ和v的穩定性。但是，狀態θerr在原點是不穩定的。為便于分析，僅考慮ρ和v到達穩態的情況，此時，ρ=l，v=v3。在θerr=0及ω=0附近線性化，sinθerr≈θerr，從而可得

(24)

線性化系統的狀態矩陣存在特征值v3/l，因此，當θerr≠0時，θerr將發散，直到θerr超過邊界層，控制律使其重新收斂到ε內。θerr的發散速度取決于v3/l，選擇較大的前視距離可減小θerr的發散速度，避免平臺頻繁調整姿態。

4 仿真分析

為驗證上述控制算法的路徑跟蹤性能，以中煤科工集團重慶研究院的履帶移動平臺為原型，在Recurdyn中搭建動力學仿真模型，同時在Matlab/Simulink平臺建立路徑跟蹤控制算法，通過Recurdyn和Matlab/Simulink聯合仿真驗證算法的有效性，分析前視距離對控制算法的影響，并與純追蹤算法進行對比。

4.1 不同前視距離下路徑跟蹤仿真

采用矩形路徑作為平臺的目標路徑，設置ε=0.087 rad，初始平臺位置在(0,0)附近，初始航向角為0，平臺直線行駛速度為0.15 m/s，采樣頻率為10 Hz，前視距離l分別為0.4 m和0.8 m，仿真結果如圖2所示。路徑跟蹤誤差有多種定義方式，本文基于文獻[17]中的方法，定義路徑跟蹤誤差e為平臺幾何中心到目標路徑的最短距離。

從圖2(a)可看出，當前視距離為0.4 m和0.8 m時，算法對直線目標路徑均具有較高的跟蹤精度，但在轉角處都出現了“抄近道”的現象。這是由于算法總是根據前視距離的大小預瞄目標路徑點，根據當前位置和目標位置提前做出轉向動作。從圖2(b)可看出，因為初始點與目標路徑相距較遠，所以最大跟蹤誤差出現在初始點處，當平臺開始運動后，誤差逐漸減小。在穩態跟蹤階段，路徑跟蹤誤差峰值出現在轉角處，當前視距離為0.4 m時，轉角處最大誤差為0.180 9 m，前視距離0.8 m時，轉角處最大誤差為0.371 5 m。

(a) 運動路徑

(b) 跟蹤誤差

采用平均誤差和穩態誤差作為統計指標衡量路徑跟蹤質量[18]，平均誤差為整個跟蹤過程路徑跟蹤誤差的平均值，穩態誤差為路徑跟蹤誤差首次到達0之后路徑跟蹤誤差平均值，統計結果見表1。從表1可看出，在保證穩定的前提下，選擇較小的前視距離可提高路徑跟蹤精度。

表1 不同前視距離下路徑跟蹤仿真統計結果Table 1 Statistical results of path tracking simulation with different look-ahead distance m

4.2 對比仿真分析

設置純追蹤算法前視距離為0.4 m，且履帶驅動輪可調速，其余條件與上文相同，對純追蹤算法和bang-bang控制算法進行對比仿真分析，結果如圖3所示。

從圖3可看出，直線跟蹤階段，純追蹤算法的跟蹤誤差略小于bang-bang控制算法；在整個跟蹤過程中，純追蹤算法的平均跟蹤誤差為0.063 8 m，穩態跟蹤誤差為0.030 4 m，也略優于bang-bang控制算法；但在轉角處，純追蹤算法出現了明顯的超調，跟蹤誤差峰值為0.348 9 m，高于bang-bang控制算法。

對比2種算法的跟蹤效率，純追蹤算法跟蹤至最終目標點耗時約為208.8 s，bang-bang控制算法的耗時為284.8 s，純追蹤算法效率高于bang-bang控制算法，其主要原因在于帶調速的純追蹤算法可以在速度為0.1 m/s的前提下持續跟蹤目標點。

(a) 平臺運動路徑

(b) 跟蹤誤差

5 樣機試驗

5.1 試驗樣機

本文試驗樣機由中煤科工集團重慶研究院履帶式移動平臺進行改裝得到，如圖4所示。樣機質量約為8 t，采用液壓驅動的履帶式摩擦底盤，履帶間距為0.93 m，最大線速度約為0.15 m/s。移動平臺電控箱上方安裝有1個激光雷達和1個六軸慣性測量單元(Inertial Measurement Unit，IMU)，用于同步定位與建圖(Simultaneous Localization and Mapping，SLAM)。激光雷達型號為Velodyne VLP-16，測量距離為100 m，檢測精度為±0.03 m。IMU型號為LPMS-BE1，其歐拉角測量分辨率小于0.1°，靜態、動態測量精度小于2°。工控機用于SLAM和路徑跟蹤控制，PLC用于底盤電磁閥控制，工控機與PLC之間通過以太網Modbus協議通信。

自主導航行走系統架構如圖5所示。

圖5 自主導航行走系統架構Fig.5 Architecture of autonomous navigation system

5.2 路徑跟蹤試驗

采用激光雷達及IMU建立測試場地的二維地圖，然后基于地圖坐標系規劃目標路徑。路徑函數為y=-0.051 2x-0.143 5，起始目標點坐標為(2.4，-0.266 5)。履帶移動平臺以約0.05 m/s的速度跟蹤目標路徑，SLAM以10 Hz的頻率實時輸出激光雷達位置，通過坐標變換得到履帶移動平臺幾何中心的位置。設控制算法前視距離l=0.5 m，ε=0.087 rad。進行2次試驗，試驗1中履帶移動平臺初始位置為(0.729 1，-0.262 7)，試驗2中履帶移動平臺初始位置為(1.520 7，-0.232 8)，試驗結果如圖6所示。

(a) 平臺運動路徑

(b) 跟蹤誤差

從圖6(a)可看出，履帶移動平臺啟動后迅速向目標路徑運動，在行駛過程中，當其偏離目標路徑時，會調整前進方向，回到目標路徑上。從圖6(b)可看出，初始誤差為履帶移動平臺與起始目標點的距離，當履帶移動平臺開始運動后，誤差逐漸收斂至0附近。

2次試驗的統計結果見表2。因為平均誤差與初始位置有關，所以試驗1的平均誤差大于試驗2；2次試驗穩態誤差均為0.02 m左右，說明系統路徑跟蹤穩態誤差小且一致性較好。

表2 路徑跟蹤試驗統計結果Table 2 Statistical results of path tracking experiments

6 結論

(1) bang-bang控制算法的平均跟蹤誤差及穩態跟蹤誤差略高于帶閉環調速的純追蹤算法，但在轉角處的跟蹤誤差小于純追蹤算法。

(2) 仿真分析和樣機試驗結果表明，bang-bang控制算法可用于無閉環調速履帶移動平臺的直線或折線類型路徑跟蹤，且穩態誤差均小于9 cm。

(3) 在保證穩定的前提下，bang-bang控制算法可選擇較小的前視距離，以獲得更高的跟蹤精度。

(4) 純追蹤算法僅包含前視距離1個調整參數，而bang-bang控制算法引入了邊界層厚度，該參數對路徑跟蹤精度及跟蹤效率有重要影響。邊界層厚度越大，則跟蹤精度越低，跟蹤效率越高。下一步將對前視距離和邊界層厚度的自適應調整進行研究。