智能消殺機器人路徑規(guī)劃與滑模跟蹤算法研究

梁鳳強，陳曉杰，韓銘雪

（國網臨沂供電公司，山東 臨沂 270000）

0 引言

非完整移動機器人是集環(huán)境感知、動態(tài)決策和行為控制于一體的復雜系統(tǒng)。近年來，非完整移動機器人控制受到業(yè)界的密切關注，相關產品和技術廣泛應用于室內鳥瞰圖的構建、野外道路環(huán)境調查和自動貨物裝卸等場合[1-3]。本文開展了基于A-star 算法的機器人連續(xù)積分滑模路徑跟蹤控制研究。首先，采用A-star 路徑規(guī)劃算法，實現移動機器人的全局快速路徑規(guī)劃，從而得到最優(yōu)的機器人運動路徑。然后，根據輸出路徑，設計分段連續(xù)積分滑模跟蹤控制器，該控制器有效地提高了系統(tǒng)的趨近速率，實現跟蹤誤差在有限時間內趨近于零，保證了路徑跟蹤精度和控制系統(tǒng)的穩(wěn)定性。最后，通過仿真實驗驗證了方法的有效性。

1 現狀

文獻[4]在移動機器人中引入模糊控制方法，提高了系統(tǒng)的控制精度。文獻[5-7]提及滑?？刂凭哂辛己玫姆€(wěn)健性，需要解決滑模面設計過程中固有的抖動問題。文獻[8]采用A-star算法規(guī)劃機器人的室內運動。文獻[9-11]采用自適應模糊控制提高了機器人系統(tǒng)的穩(wěn)健性。文獻[12-14]采用了神經網絡自適應控制方法，實現移動機器人較小的跟蹤誤差和較強的抗干擾能力。

移動機器人普遍存在約束問題，文獻[15]提出的自適應控制方法成功地處理了約束。文獻[16]為機器人電機的安全性增加了電流飽和約束、路徑偏差和速度跟蹤約束，保證機器人的穩(wěn)定性。此外，文獻[17-19]設計了路徑跟蹤自適應控制算法，機器人跟蹤誤差收斂到足夠小的量。

2 路徑規(guī)劃

A-star 算法是在二維環(huán)境下進行路徑規(guī)劃的算法，具有簡單、快速的特點。在路徑搜索時，針對實際問題制定相應的啟發(fā)式函數進行引導性搜索，達到減少搜索節(jié)點的目的，提高路徑搜索效率。

A-star 算法結合了Dijkstra 算法和最優(yōu)快速搜索算法，機器人在找到最優(yōu)路徑的同時，計算量較小。算法設計如下：

式中，g(n) 為節(jié)點N到起始點的移動代價；f(n) 為第n個節(jié)點的綜合優(yōu)先級，算法根據節(jié)點的優(yōu)先級選擇下一個待遍歷的節(jié)點；h(n)為節(jié)點N到終點的期望代價，為快速優(yōu)先算法的啟發(fā)式函數。

啟發(fā)式函數h(n)在A-star 算法中至關重要。在極端情況下，如果h(n)為0，函數f(n)中僅有g(n)發(fā)揮作用，A-star 算法變化為Dijkstra算法。h(n)越小，A-star 算法遍歷節(jié)點越多，算法運行速度越慢。反之，h(n)越大，A-star就會成為快速優(yōu)先算法。

啟發(fā)式函數用以下方法計算：

該形式的啟發(fā)函數可以計算直線距離和對角線距離，完整的算法包含初始化開集和閉集；將初始點放入開集，設優(yōu)先級為最高；如果開集不為空，則從開集中選擇優(yōu)先級最高的節(jié)點為節(jié)點N。

如果節(jié)點N是目標節(jié)點，則完成路徑規(guī)劃，跟蹤父節(jié)點從終點到起點。如果節(jié)點N不是目標節(jié)點，將節(jié)點N從開集中移除，置于閉集中，遍歷節(jié)點N周圍的所有節(jié)點。

如果與N相鄰的節(jié)點M在閉集中，則跳過并檢測下一個節(jié)點。如果節(jié)點M不在開集中，將M的父節(jié)點設為N，計算M的優(yōu)先級，將節(jié)點M加入開集。

如果節(jié)點M在開集中，計算比較移動代價f(n)的值，如果f(n)更小，則該節(jié)點為父節(jié)點，重新計算該值。

3 跟蹤方法

3.1 運動學模型

移動機器人狀態(tài)示意圖如圖1 所示。機器人位姿（位置和方向）由向量P=[x y θ]Τ表示；運動速度矢量由向量q=[ν ω]Τ表示。其中[x y]為移動機器人的位置，θ為移動機器人前進方向與x軸的夾角，ν和ω分別為移動機器人的線速度和角速度，在運動學模型中作為控制輸入。

圖1 移動機器人狀態(tài)示意圖

考慮移動機器人的運動學方程為：

由運動學方程可以得知，共有2 個自由度，模型輸出為3 個變量，該模型為欠驅動系統(tǒng)，只能實現2 個變量的主動跟蹤，剩余變量為隨動或鎮(zhèn)定狀態(tài)。通過設計控制律q=[ν ω]實現移動機器人位置[x y]的跟蹤，實現夾角的隨動。得到移動機器人運動學模型為：

3.2 位置滑?？刂坡稍O計

通過設計位置控制律ν，實現x跟蹤xd，y跟蹤yd。取理想路徑為[xd yd]，則誤差跟蹤方程為：

控制律根據李雅普諾夫穩(wěn)定條件確定，構造如下 Lyapunov 函數：

對上式求導可得：

設計控制律為

因此，系統(tǒng)狀態(tài)將以有限時間到達滑模面，沿滑模面運動，跟蹤誤差收斂到零。

控制律可根據李雅普諾夫穩(wěn)定條件確定，構造如下 Lyapunov 函數：

對上式求導可得：

設計控制律為：

因此，系統(tǒng)狀態(tài)將以有限時間到達滑模面，沿滑模面運動， 跟蹤誤差收斂到0。

由式（6）可得：

如果θ的值域為則可得到滿足理想路徑跟蹤的θ為：

上式求得的θ為位置控制律式所要求的角度，如果θ與θd相等，則理想的路徑控制律可以實現。但是，實際模型式中的θ與θd不可能完全一致，尤其是控制的初始階段，這會造成閉環(huán)跟蹤系統(tǒng)式的不穩(wěn)定。

為此，需要將式（20）求得角度θ當成理想值，取

設計理想的位姿指令[xd yd]時，需要使θd的值域滿足

實際的θ與θd之間的差異會造成位置控制律式（11）和式（17）無法精確實現，從而造成閉環(huán)系統(tǒng)不穩(wěn)定。較簡單的解決方法是設計相比位置控制收斂更快的姿態(tài)控制算法，使θ盡快跟蹤θd。

由式（6），可得到實際的位置控制律為：

2.3 姿態(tài)滑?？刂坡稍O計

設計姿態(tài)控制律ω，實現角度θ跟蹤θd。

取θc=θ-θd，設計連續(xù)積分滑模函數為：

控制律可根據李雅普諾夫穩(wěn)定條件確定，構造如下 Lyapunov 函數：

對上式求導可得：

設計控制律為：

因此，系統(tǒng)狀態(tài)將以有限時間到達滑模面，并沿滑模面運動，跟蹤誤差將收斂到零。

上述的閉環(huán)系統(tǒng)屬于內外環(huán)構成的控制系統(tǒng)，位置子系統(tǒng)為外環(huán)，姿態(tài)子系統(tǒng)為內環(huán)，外環(huán)產生中間指令信號傳遞給內環(huán)系統(tǒng)，內環(huán)則通過滑?？刂坡蓪崿F對中間指令信號的跟蹤。

在控制律式（27）中，需要對外環(huán)產生的中間指令信號θd求導，求導較為復雜，為簡單起見，采用線性二階微分器實現。

式中，x(t)為待微分的輸入信號；m1為對信號的跟蹤變量；m2為信號一階導數的估計；微分器的初始值為m1(0)=0，m2(0)=0。

4 仿真實驗

4.1 路徑規(guī)劃算法

為了驗證A-star 算法在路徑規(guī)劃方面的優(yōu)勢和有效性，通過MATLAB 仿真軟件將A-star和Dijkstra 路徑算法進行了對比。對比實驗結果如圖2 所示。

圖2 對比實驗結果

從圖2 中可以看出，相比于Dijkstra 算法，A-star 搜索柵格更少，因此其搜索速度也更快。

4.2 路徑規(guī)劃仿真

為了驗證A-star 算法在路徑規(guī)劃上的實用性，采用智能消殺機器人作為驗證平臺。機器人上裝載了激光雷達掃描，樹莓派3 以及32位MCU。通過MCU 輸出PWM 控制驅動電機運動和轉彎，樹莓派處理數據進行地圖建模。A-star 算法設計路徑后，機器人通過設定的控制率循跡運行，即跟蹤控制系統(tǒng)的體現。

實驗中，機器人穩(wěn)定運行躲過墻體障礙物，20 s 后準確達到目標位置。A-star 算法循跡圖如圖3 所示。

圖3 A-star算法循跡圖

4.3 路徑跟蹤仿真

為了驗證設計的控制器使系統(tǒng)在滿足全狀態(tài)約束條件下，其各狀態(tài)能夠有效跟隨給定的參考信號變化，在MATLABSimulink 中搭建仿真實驗模型。選取控制參數：c1=c2=c3=5，γ1=γ2=γ3=2，ρ1=ρ2=ρ3=1。

A-star 算法計算的參考信號：yd=[sin(0.5x)+0.5x+1]，期望路徑的初始位姿為(-2, 2 0) ，進行系統(tǒng)仿真。機器人軌跡跟蹤曲線如圖4所示。

圖4 機器人軌跡跟蹤曲線

由圖4 可以看出，基于本文中所述的運動學模型和連續(xù)積分滑模路徑跟蹤控制器，移動機器人具備良好的跟蹤性能，可以在較大的初始誤差情況下，確保在較短的時間內實現對期望路徑的完全跟蹤。同時，系統(tǒng)的抖振現象顯著削弱，達到了預期的效果。從圖5、圖6、圖7 中可以看到，機器人位置和角度狀態(tài)較快地跟蹤到參考信號。

圖7 機器人位置和角度跟蹤曲線

圖8 微分器的輸入輸出

圖9 控制輸入信號

5 結語

本文主要開展了基于A-star 算法的機器人連續(xù)積分滑模路徑跟蹤控制研究。為了實現機器人的快速路徑規(guī)劃，采用了A-star 路徑規(guī)劃算法，得到移動機器人所需的最優(yōu)運動路徑。基于規(guī)劃輸出路徑，設計了分段連續(xù)積分滑模跟蹤控制器，實現了位置和姿態(tài)的跟蹤誤差在有限時間內趨近于零，有效地保證路徑跟蹤精度，通過李雅普諾夫函數證明了控制系統(tǒng)的穩(wěn)定性。