梁鳳強(qiáng)，陳曉杰，韓銘雪

（國網(wǎng)臨沂供電公司，山東 臨沂 270000）

0 引言

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

1 現(xiàn)狀

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

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

2 路徑規(guī)劃

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

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

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

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

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

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

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

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

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

3 跟蹤方法

3.1 運(yùn)動(dòng)學(xué)模型

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

圖1 移動(dòng)機(jī)器人狀態(tài)示意圖

考慮移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)方程為：

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

3.2 位置滑模控制律設(shè)計(jì)

通過設(shè)計(jì)位置控制律ν，實(shí)現(xiàn)x跟蹤xd，y跟蹤yd。取理想路徑為[xd yd]，則誤差跟蹤方程為：

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

對(duì)上式求導(dǎo)可得：

設(shè)計(jì)控制律為

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

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

對(duì)上式求導(dǎo)可得：

設(shè)計(jì)控制律為：

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

由式（6）可得：

如果θ的值域?yàn)閯t可得到滿足理想路徑跟蹤的θ為：

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

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

設(shè)計(jì)理想的位姿指令[xd yd]時(shí)，需要使θd的值域滿足

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

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

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

設(shè)計(jì)姿態(tài)控制律ω，實(shí)現(xiàn)角度θ跟蹤θd。

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

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

對(duì)上式求導(dǎo)可得：

設(shè)計(jì)控制律為：

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

上述的閉環(huán)系統(tǒng)屬于內(nèi)外環(huán)構(gòu)成的控制系統(tǒng)，位置子系統(tǒng)為外環(huán)，姿態(tài)子系統(tǒng)為內(nèi)環(huán)，外環(huán)產(chǎn)生中間指令信號(hào)傳遞給內(nèi)環(huán)系統(tǒng)，內(nèi)環(huán)則通過滑?？刂坡蓪?shí)現(xiàn)對(duì)中間指令信號(hào)的跟蹤。

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

式中，x(t)為待微分的輸入信號(hào)；m1為對(duì)信號(hào)的跟蹤變量；m2為信號(hào)一階導(dǎo)數(shù)的估計(jì)；微分器的初始值為m1(0)=0，m2(0)=0。

4 仿真實(shí)驗(yàn)

4.1 路徑規(guī)劃算法

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

圖2 對(duì)比實(shí)驗(yàn)結(jié)果

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

4.2 路徑規(guī)劃仿真

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

實(shí)驗(yàn)中，機(jī)器人穩(wěn)定運(yùn)行躲過墻體障礙物，20 s 后準(zhǔn)確達(dá)到目標(biāo)位置。A-star 算法循跡圖如圖3 所示。

圖3 A-star算法循跡圖

4.3 路徑跟蹤仿真

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

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

圖4 機(jī)器人軌跡跟蹤曲線

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

圖7 機(jī)器人位置和角度跟蹤曲線

圖8 微分器的輸入輸出

圖9 控制輸入信號(hào)

5 結(jié)語

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