宋孝成，劉曉培，陸疌

(上?？萍即髮W(xué)信息科學(xué)與技術(shù)學(xué)院， 上海 201210) (2020年3月13日收稿； 2020年6月10日收修改稿)

無人機(jī)(unmanned aerial vehicle)的自主導(dǎo)航(autonomous navigation)問題在近年來備受關(guān)注。要實現(xiàn)自主導(dǎo)航，關(guān)鍵是要進(jìn)行合理的運動規(guī)劃(motion planning)，這也是移動機(jī)器人的熱門研究領(lǐng)域。根據(jù)是否儲存并使用全局地圖進(jìn)行導(dǎo)航，運動規(guī)劃可以分為全局規(guī)劃和局部規(guī)劃。

全局規(guī)劃方法已經(jīng)被廣泛地研究，通常做法是通過感知全局環(huán)境建立一張地圖，然后尋找一條最優(yōu)路徑，該路徑可避免碰撞并到達(dá)目標(biāo)點，最后通過路徑跟蹤(path following)控制移動機(jī)器人沿路徑行至目標(biāo)點，這類方法也被稱之為路徑規(guī)劃(path planning)[1]。常見的路徑規(guī)劃算法包括基于節(jié)點搜索的方法[2]，基于快速搜索隨機(jī)樹的方法[3-4]，以及基于人工勢場的方法[5-6]。因為需要存儲并處理全局地圖以進(jìn)行導(dǎo)航，全局規(guī)劃方法一般只能工作在有限的空間范圍。根據(jù)是否預(yù)先已知全部障礙物信息，全局規(guī)劃可以大體上分為離線規(guī)劃和在線規(guī)劃。離線規(guī)劃在已知全局環(huán)境的情況下，直接預(yù)先計算出從起點到目標(biāo)點的路徑。而在線規(guī)劃在初始階段可能不知道或者僅知道部分環(huán)境信息，在機(jī)器運動過程中，當(dāng)檢測到新的環(huán)境信息或已知環(huán)境發(fā)生改變時，更新其儲存的全局地圖，并動態(tài)地重新規(guī)劃出新的路徑。很多研究是在傳統(tǒng)離線規(guī)劃算法的基礎(chǔ)上進(jìn)行改進(jìn)，提出在線規(guī)劃變體[7-8]。另外，還有一些研究在環(huán)境未知的條件下，采用一邊進(jìn)行構(gòu)建全局地圖，一邊根據(jù)已有地圖進(jìn)行導(dǎo)航的方法，進(jìn)而把問題轉(zhuǎn)化成為已知環(huán)境的路徑規(guī)劃問題。然而，這類方法不僅需要精度較高的傳感器，也需要較多的前期探索和地圖構(gòu)建時間，難以普適不同場景環(huán)境。

局部規(guī)劃方法則不使用通用的全局地圖，而是直接使用傳感器實時獲取的環(huán)境信息，或者一個活動窗口范圍內(nèi)的環(huán)境信息，進(jìn)行快速的避障反應(yīng)并指引機(jī)器向目標(biāo)點運動，有些研究者稱之為實時避障(real-time obstacle avoidance)。局部規(guī)劃不需要預(yù)先規(guī)劃出一條完整路徑，而是可以隨時改變導(dǎo)航的起點和目標(biāo)點。對環(huán)境信息的感知往往是依靠測距傳感器，比如聲吶、激光雷達(dá)、深度相機(jī)等，獲取其到周圍障礙物的距離。傳統(tǒng)的實時避障方法有虛擬力場法[9]、向量直方圖法[10]等。此外，一些模糊邏輯類的方法[11]、遺傳算法類的方法[12]和基于學(xué)習(xí)的方法[13]也被提出。因為只需要有限的信息進(jìn)行導(dǎo)航，不無限積累歷史環(huán)境信息，其工作的空間范圍理論上不受限制，而代價則是在某些特殊情況下可能無法找到可行的路徑。此外，有些研究使用多層規(guī)劃結(jié)合的方法實現(xiàn)自主導(dǎo)航，在頂層使用粗粒度的全局路徑規(guī)劃計算出路徑點以保證可達(dá)性，在底層則使用局部規(guī)劃從一個路徑點到下一個路徑點，從而得到更為平滑的路徑[14]。

無人機(jī)作為一種特殊的移動機(jī)器人，在通用的導(dǎo)航方法之上，又有一些特殊性。關(guān)鍵在于無人機(jī)不僅運動速度相對較快，且對碰撞更為敏感，一旦發(fā)生碰撞不僅很容易損壞機(jī)器，更可能失速墜落造成不必要的人身傷害和財產(chǎn)損失。除此之外，無人機(jī)的工作環(huán)境也往往不同于普通的地面機(jī)器人。上文提到的很多方法最早是基于地面機(jī)器人提出的，它們一般以路徑最短作為最優(yōu)指標(biāo)[1]。對無人機(jī)導(dǎo)航來說，更需要保證飛行中的安全，不應(yīng)僅僅為了路程更短以致于太靠近障礙物，而是需要在路徑長度和遠(yuǎn)離障礙上進(jìn)行均衡，人工勢場法天然具備這一個優(yōu)點。然而傳統(tǒng)人工勢場法可能會由于局部極小點的存在使無人機(jī)陷入其中。很多研究以不同的方式嘗試解決這個問題，其中，基于拉普拉斯方程(Laplace’s equation)和調(diào)和函數(shù)(harmonic function)的方法較為成功地解決了這個問題[15-16]。該方法利用拉普拉斯方程的解——調(diào)和函數(shù)不存在局部極小值的特性，通過構(gòu)建調(diào)和函數(shù)的人工勢場，避免陷入局部極小值。通過求解拉普拉斯方程建立的勢場叫做拉普拉斯人工勢場，或簡稱拉普拉斯勢場。

我們之前的工作也是基于拉普拉斯勢場，并使用邊界元法(boundary element method，BEM)進(jìn)行求解，實現(xiàn)了已知全局地圖的無人機(jī)避障導(dǎo)航[17]。對于大型勢場來說，邊界元法求解效率相對較高。與之前工作不同的是，本文主要針對未知環(huán)境，提出基于拉普拉斯勢場的局部規(guī)劃方法，因所求勢場較小，有限差分法(finite difference method，F(xiàn)DM)完全可以勝任，且其實現(xiàn)簡單，易于并行實現(xiàn)，故本文使用有限差分法求解勢場。該導(dǎo)航方法僅基于傳感器的實時探測數(shù)據(jù)，隨著無人機(jī)的移動，不斷建立新的局部拉普拉斯勢場，從而指引無人機(jī)向目標(biāo)點運動，實現(xiàn)實時避障導(dǎo)航：

1) 根據(jù)傳感器探測信息，建立一個網(wǎng)格化的局部地圖表示的邊值問題，其解即為拉普拉斯勢場。該方法可以靈活地應(yīng)對各種形狀大小各異的障礙物。

2) 采用有限差分法求解上述邊值問題，該方法是用于數(shù)值求解邊值問題的常用方法，通過迭代更新網(wǎng)格中的值，直至收斂。

3) 隨著傳感器數(shù)據(jù)的更新，隨時更換新的勢場并求解。而在更換勢場過程中仍然可以隨時提供場內(nèi)任意一點的參考速度。

4) 使用 MATLAB 實現(xiàn)并仿真了基于 PID的無人機(jī)控制器，根據(jù)機(jī)身所在位置獲得參考速度方向后，控制器控制無人機(jī)跟蹤參考方向，最終無人機(jī)在導(dǎo)航算法指引下飛向目標(biāo)點。

本文將拉普拉斯人工勢場的思想應(yīng)用到局部地圖上。仿真實驗表明，該方法計算簡潔快速，沒有復(fù)雜的參數(shù)調(diào)試，可以自如應(yīng)對各種不同的環(huán)境和障礙物，能夠適用于未知環(huán)境中的實時導(dǎo)航。

1 基于拉普拉斯方程的人工勢場

1.1 拉普拉斯方程與邊值問題

人工勢場法[18]最早由 Khatib 于 1986 年提出，其基本思想是在可行的工作空間上建立合適的勢函數(shù)，使移動機(jī)器人沿著勢函數(shù)的負(fù)梯度方向進(jìn)行運動，也可以近似看作是沿著勢函數(shù)的流線(streamline)進(jìn)行運動。傳統(tǒng)的人工勢場法對障礙物建立排斥勢函數(shù)，對目標(biāo)點建立吸引勢函數(shù)，然后進(jìn)行代數(shù)疊加得到合勢函數(shù)。這種勢函數(shù)可能產(chǎn)生局部極小值，使得無人機(jī)被困住。由拉普拉斯方程定義的調(diào)和函數(shù)因具有無局部極值的良好特性，可以巧妙地解決這個問題。

拉普拉斯方程是有著如下形式的二階偏微分方程

(1)

其中：n是自變量的維度，Δ稱作拉普拉斯算子。該方程的解φ被稱作調(diào)和函數(shù)。

調(diào)和函數(shù)有很多良好的性質(zhì)，比如調(diào)和函數(shù)是實解析的[19]，因此也是無窮階可導(dǎo)(光滑)的。另外一個良好性質(zhì)，被稱作強(qiáng)最大值/最小值原理[19]，表述如下：

定理1(強(qiáng)最大值/最小值原理)設(shè)Ω是非空區(qū)域(連通的開集稱為區(qū)域或開區(qū)域),φ是在Ω上調(diào)和的實函數(shù),若φ在Ω上有最大值/最小值，則φ為常數(shù)函數(shù)。

根據(jù)上述定理和調(diào)和函數(shù)的實解析性質(zhì)，可以推導(dǎo)出局部極大值/極小值原理：

定理2 (局部極大值/極小值原理)設(shè)Ω是非空區(qū)域，φ是在Ω上調(diào)和的實函數(shù)，若φ在Ω上有局部極大值/極小值，則φ為常數(shù)函數(shù)。

也就是說，若Ω有界，則非常數(shù)的調(diào)和函數(shù)φ，其全局的最大值與最小值只能出現(xiàn)在Ω的邊界上，且在Ω內(nèi)部沒有局部極值。

僅通過上述偏微分方程是無法唯一確定一個解的。一個微分方程及其邊界上的約束條件就構(gòu)成了邊值問題。最早研究的以及最為人熟知的邊值問題是狄利克雷問題(Dirichlet problem)，就是要找出滿足狄利克雷邊界條件的拉普拉斯方程的解，其定義如下：

定義 (狄利克雷問題)考慮定義在n上的一個區(qū)域Ω，其邊界表示為?Ω，其閉包表示為給出定義在邊界?Ω上的一個函數(shù)f，是否存在惟一函數(shù)φ在區(qū)域Ω內(nèi)部二次連續(xù)可微，且在邊界?Ω上連續(xù)，使得φ在Ω內(nèi)部調(diào)和并在邊界上φ=f。

二維的狄利克雷問題可以如下表示

(2)

φ(x,y)=f(x,y), ?(x,y)∈?Ω.

(3)

根據(jù)最大值/最小值原理，可以證明狄利克雷問題的解的唯一性[19]。研究表明，當(dāng)邊界足夠光滑時，狄利克雷問題總有解[20]。而本文中對于整個定義域包括邊界，都進(jìn)行了網(wǎng)格化表示，可以將其看作另一個有著足夠光滑邊界的狄利克雷問題的離散近似，并且通過數(shù)值求解方法，總是可以求得一個數(shù)值解。

本文基于二維的拉普拉斯方程，通過設(shè)置有界區(qū)域的邊界值，構(gòu)建出一個狄利克雷問題，其區(qū)域邊界上的值可以唯一確定一個滿足約束的調(diào)和函數(shù)。該調(diào)和函數(shù)表示的人工勢場，即拉普拉斯勢場，其負(fù)梯度方向被用來作為避障導(dǎo)航的參考方向。

1.2 狄利克雷問題的構(gòu)建

本文所述的測距傳感器并不特定于某種具體的傳感器，只要可以實時探測到自身到周邊障礙物的距離即可，其偵測范圍也被設(shè)定在水平面上。在仿真中，直接使用射線進(jìn)行交點計算：以自身為中心向四周發(fā)射射線，然后返回到命中點(射線與障礙物的交點)的距離。設(shè)傳感器對一圓周進(jìn)行n等角抽樣，則抽樣間隔為2π/n弧度。傳感器安裝在無人機(jī)機(jī)身上，本文將傳感器和無人機(jī)作為同一質(zhì)點。設(shè)傳感器的最大可探測距離為R，則無人機(jī)的可探測范圍是一個圓面={(x,y)|(x-x0)2+(y-y0)2≤R2},其中(x0,y0)為無人機(jī)當(dāng)前點。

(4)

局部地圖范圍應(yīng)該覆蓋所有探測點和臨時目標(biāo)點。因此，以當(dāng)前點P為中心，以a=2λ2R為邊長，取λ2>λ1，形成的正方形區(qū)域

(5)

即為局部地圖范圍，如圖 1所示。

圖1 狄利克雷問題的構(gòu)建Fig.1 The construction of Dirichlet problems

下一步則是確定勢場函數(shù)的邊界及其邊界值。為實現(xiàn)局部勢場的導(dǎo)航，有如下約束條件：

1) 避免與障礙物發(fā)生碰撞。因此，障礙物應(yīng)該作為勢場邊界且固定為最大值M，否則勢場中的勢值可能大于障礙物的勢值，使無人機(jī)與障礙物發(fā)生碰撞。

2) 避免無人機(jī)飛出局部地圖。因此，局部地圖的邊界需要作為勢場的邊界且固定為最大值M，否則，無人機(jī)很可能在負(fù)梯度方向的指引下傾向于飛出局部地圖邊界。

3) 引導(dǎo)無人機(jī)趨近臨時目標(biāo)點。因此，臨時目標(biāo)點應(yīng)該作為勢場邊界并固定為最小值m，只有這樣，勢場函數(shù)的流線才會匯聚于臨時目標(biāo)點。

最終，勢場函數(shù)的邊界值由以上3部分組成。其中傳感器探測的結(jié)果是很多個點，這些障礙點可以根據(jù)實際情況、離散間隔等適當(dāng)擴(kuò)充為稍大的區(qū)間或者圓面。本文因為進(jìn)行網(wǎng)格化時的分辨率較低，并未進(jìn)行擴(kuò)充。

本文選取λ1=1.1，λ2=1.2，M=1，m=0。值得注意的是，因為計算機(jī)求解的時候是網(wǎng)格化進(jìn)行的，如果格數(shù)太少，可能使得目標(biāo)方向的最遠(yuǎn)探測點、臨時目標(biāo)點和局部地圖邊界點出現(xiàn)重合，這時候應(yīng)該調(diào)整λ1和λ2使之落在不同的格子里，并且最好有些間隔。

1.3 拉普拉斯人工勢場的數(shù)值求解

拉普拉斯方程是最簡單的橢圓微分方程之一，其數(shù)值求解方法主要包括：有限差分法、有限元法、邊界元法。本文采用有限差分法進(jìn)行數(shù)值求解，因其實現(xiàn)簡單，且網(wǎng)格化的方式非常有利于將探測到的障礙點以及目標(biāo)點等直接放置在局部地圖中。

有限差分法的基本思想是把連續(xù)問題離散化，以差分替代微分，將微分方程及其約束條件轉(zhuǎn)化為只包含有限個未知量的差分方程組，并將該差分方程組的解作為原問題的近似離散解。首先，將整個局部地圖網(wǎng)格化為等間隔的網(wǎng)格節(jié)點。本文將正方形地圖的每個方向都等分為n個區(qū)間，則區(qū)間長度為h=a/n，得到縱向網(wǎng)格線x=xi=x0+ih和橫向網(wǎng)格線y=yj=y0+jh，其中i,j=0,1,…,n。網(wǎng)格線的交點(xi,yj)被稱為網(wǎng)格點，其勢函數(shù)值表示為φi, j。然后，將拉普拉斯算子離散化，以中心差分公式來近似偏微分，可得

(6)

(7)

根據(jù)拉普拉斯方程的定義，可以得到

(8)

化簡為

4φi, j-(φi+1, j+φi-1, j+φi, j+1+φi, j-1)=0.

(9)

將區(qū)域中的每個非邊界網(wǎng)格點的函數(shù)值φi, j作為未知變量，對它們分別應(yīng)用式(9)，可以得到一個大型的稀疏線性方程組。該方程組的系數(shù)矩陣對角線值均為4，且每一行都有不超過4個-1，其余元素均為0。該線性方程組的解構(gòu)成了以網(wǎng)格值表示的拉普拉斯勢場。

求解線性方程組的迭代解法有雅克比法和高斯賽德爾法等。通過運用雅克比法求解該線性方程組，則求解過程實際上等同于將每一個非邊界網(wǎng)格點更新為其上、下、左、右4個網(wǎng)格點的平均值，即

(10)

其中k為迭代的循環(huán)變量。如此循環(huán)往復(fù)，直至收斂。運用高斯賽德爾法求解相當(dāng)于類似的過程，不同的是其對每一個點的更新都當(dāng)場生效，更新所需的4個相鄰點的值均使用目前的最新值。假設(shè)i和j的遍歷順序都是從小到大，高斯賽德爾法可以表示為

(11)

可以證明，對于連通區(qū)域的有限差分方法，雅克比法和高斯賽德爾法可以保證收斂[21]。高斯賽德爾法收斂速度是雅克比法的2倍，但是雅克比法的優(yōu)點是在一次迭代中各點更新互相獨立，更容易在GPU上并行實現(xiàn)。本文的仿真部分采用高斯賽德爾法進(jìn)行數(shù)值求解。

1.4 勢場梯度的數(shù)值求解

因為對局部地圖進(jìn)行網(wǎng)格化，可以直接使用周圍相鄰網(wǎng)格點進(jìn)行數(shù)值微分求得梯度。對于任一網(wǎng)格點(xi,yj)，由中心差分公式得到該點的近似梯度為

(12)

對于在網(wǎng)格線但不是網(wǎng)格點的坐標(biāo)點，可以進(jìn)行線性插值來近似該點的梯度。例如，對于橫向網(wǎng)格線y=yj上的點(x,yj)，計算出h整除x-x0的商i∈和余數(shù)α，根據(jù)線性插值公式

(13)

得出該點的近似梯度，縱向網(wǎng)格線上的點同理。對于不在網(wǎng)格線上的點，可以進(jìn)行雙線性插值來近似該點的梯度。首先，計算h整除x-x0的商i∈和余數(shù)α，以及h整除y-y0的商j∈和余數(shù)β，然后，根據(jù)雙線性插值公式

(14)

求得該點的近似梯度。網(wǎng)格點和網(wǎng)格線的情況均可看作雙線性插值的特例，可以直接使用式(14)實現(xiàn)。

2 無人機(jī)控制器設(shè)計與仿真

2.1 基于梯度場的速度跟蹤

根據(jù)求解得到的拉普拉斯勢場可以計算出一條到目標(biāo)點的流線。雖然可以直接對該流線進(jìn)行路徑跟蹤，但是在真實環(huán)境中的誤差和干擾下，該方式對于實時避障不太友好。本文采用拉普拉斯勢場的負(fù)梯度方向作為參考速度方向。結(jié)合設(shè)定的速率值s，得到參考水平速度vd，然后通過速度跟蹤控制(tracking control)，使無人機(jī)調(diào)整自身狀態(tài)，并將其水平速度收斂于參考值。之所以僅使用負(fù)梯度的方向，是因為拉普拉斯勢場是由邊值問題解出來的，而不是手動定義出來的，其梯度的大小對導(dǎo)航而言無意義，只需要按照負(fù)梯度的方向飛行即可實現(xiàn)避障與趨近目標(biāo)點。根據(jù)求得的梯度值，無人機(jī)處于點P=(xP,yP)時的參考水平速度即為

(15)

本文僅考慮水平面的二維導(dǎo)航，在仿真實驗中采用恒定速率的定高飛行。需要指出的是，根據(jù)本文控制器的設(shè)計，速率s和參考高度zd均可以在飛行過程中隨時進(jìn)行更改設(shè)置。由于本文提出的局部導(dǎo)航方法僅使用由實時傳感器數(shù)據(jù)構(gòu)建的局部地圖，并不需要一張全局地圖，因此高度的變化并不影響導(dǎo)航。而水平速率的設(shè)置也不影響導(dǎo)航本身，只要無人機(jī)可以保持穩(wěn)定和安全即可。另外旋翼的物理限制也可能導(dǎo)致在過大傾角的情況下難以維持高度。但是這些和導(dǎo)航算法本身無關(guān)，總而言之，參考速率和參考高度的變化對導(dǎo)航本身影響不大。

2.2 控制器設(shè)計與仿真實現(xiàn)

本文使用基于PID的控制器對無人機(jī)進(jìn)行導(dǎo)航控制仿真實驗。下文使用的相關(guān)符號如表 1所示。需要注意的是，歐拉角是在旋轉(zhuǎn)過程中以中間坐標(biāo)系定義的，而角速度是統(tǒng)一以機(jī)身坐標(biāo)系衡量的，因此，歐拉角的導(dǎo)數(shù)并不等于角速度。

表1 控制系統(tǒng)符號表Table 1 Symbols of the control system

圖2 無人機(jī)控制系統(tǒng)Fig.2 The UAV control system

無人機(jī)的全局坐標(biāo)系采用NED坐標(biāo)系，即3個坐標(biāo)軸正方向依次指向北、東、下。本文使用下列經(jīng)典的無人機(jī)運動學(xué)及動力學(xué)模型[22-23]進(jìn)行仿真計算：

(16)

(17)

(18)

其中:C、S、T為三角函數(shù)的簡寫。該模型考慮了慣性和空氣阻力，并進(jìn)行部分簡化。因為我們是離散地實現(xiàn)控制系統(tǒng)，在一個時間片長度內(nèi)，可以認(rèn)為這些變量是常數(shù)，由此計算出其他狀態(tài)量。為簡明起見，沒有模擬狀態(tài)觀測，而是直接將狀態(tài)的真值反饋給各控制器。

3 實驗仿真與分析

3.1 勢場求解

本文提出的局部導(dǎo)航方法主要分為勢場求解和勢場中任意點的梯度求解。其中梯度求解使用固定數(shù)量的節(jié)點進(jìn)行中心差分和雙線性插值，其時間復(fù)雜度為常數(shù)。而求解勢場的計算時間主要在于使用高斯賽德爾方法求解FDM。高斯賽德爾方法的時間復(fù)雜度為O(ln2)，其中n為未知數(shù)的數(shù)量，l為迭代次數(shù)。不過根據(jù)上文所述，由于待求解線性方程組是稀疏的，每個待求解變量的單次更新都是常數(shù)時間，因此求解勢場的時間復(fù)雜度為O(ln)。本文將求解區(qū)域離散化為N×N的格子，則總的未知數(shù)數(shù)量約為n=O(N2)。故總的時間復(fù)雜度為O(lN2)，其中N為局部地圖一個維度上的離散格子數(shù)量。

本文設(shè)定無人機(jī)傳感器的最大探測距離R為20 m，按照1 m間隔進(jìn)行網(wǎng)格化，結(jié)合前文所述的λ2=1.2，最終得到48×48的局部地圖網(wǎng)格。某單個局部勢場的等高線圖和流線圖如圖 3所示，其中灰色格子為勢場的邊界點，包括地圖邊界、探測到的障礙點和目標(biāo)點。

圖3 拉普拉斯人工勢場Fig.3 Laplacian potential field

這里等高線和流線均采用雙線性插值進(jìn)行計算，且圖中等高線的值并不是均勻的。從圖中可以看出，障礙物和局部地圖邊界就是最高值的等高線，而目標(biāo)點處為最低值的等高線，各個起點的流線均流向目標(biāo)點。

3.2 避障導(dǎo)航

前文分別介紹了拉普拉斯勢場的構(gòu)建和控制器的設(shè)計與實現(xiàn)。事實上，實際應(yīng)用中的這2個模塊可以是同時進(jìn)行、互不干擾的。導(dǎo)航模塊負(fù)責(zé)根據(jù)接收到的最新傳感器探測信息求解勢場，而控制模塊從導(dǎo)航模塊獲取參考方向，生成參考速度，從而控制無人機(jī)飛行。在求解新勢場的時間內(nèi)，參考方向仍然從之前的勢場中獲取，一旦新的勢場求解出來則立即更換，之后參考方向就從新的勢場中獲取了。勢場的更新頻率主要取決于傳感器探測效率和解算勢場時的計算效率。本文使用MATLAB仿真時，設(shè)定勢場的更新頻率為1 Hz，而控制頻率為100 Hz，無人機(jī)質(zhì)量m=1.4 kg，測距傳感器在水平面對圓周進(jìn)行60°等角抽樣探測。進(jìn)行仿真實驗的計算機(jī)使用Intel(R) Core(TM) i7-8850 H CPU，主頻2.60 GHz。

本文采用3個不同的場景進(jìn)行實驗，圖 4展示了這些場景下的仿真實驗結(jié)果。3個場景中障礙物的位置均為隨機(jī)生成，起點均設(shè)為 (0, 0)。其中場景1是較多的小障礙物，目標(biāo)點設(shè)為 (80, 80)；場景2是面對幾個大障礙物的情況，目標(biāo)點設(shè)為 (90, 90)；場景3則是更長距離的飛行，目標(biāo)點設(shè)為 (300, 0)，該場景中障礙物大小不一、形狀各異、疏密不同，且通過重疊黏連形成了不規(guī)則的障礙物。在這3個場景中，我們統(tǒng)一設(shè)定水平速率為1 m/s。從圖中可以看到，無人機(jī)在躲避障礙物的情況下，順利到達(dá)了圖中以五角星標(biāo)記的目標(biāo)點。無人機(jī)的運動路徑既沒有特別繞遠(yuǎn)，也沒有為趨近目標(biāo)點而與障礙物過近或擦邊，而是在趨近目標(biāo)點的同時，盡可能在障礙物間隙的中間走，即到周邊各障礙物的距離較為均衡。

本文控制器部分直接對水平速度的2個正交分量進(jìn)行跟蹤控制，為觀察控制器的跟蹤效果，以圖 4中的場景1為例，整個飛行過程中無人機(jī)水平線速度的參考值和實際值如圖 5所示，其中深灰色實線為參考值，淺灰色虛線為實際值(下同)。從圖中可以看到，控制器能及時響應(yīng)參考速度的變化。

3個場景中無人機(jī)的水平速率和偏航角如圖 6所示。從圖中可見，3個場景中無人機(jī)速率基本維持設(shè)定值，偏航角整體上變化平穩(wěn)。在前2個場景中，水平速率幾乎沒有變化，偏航角也只有局部輕微抖動，而場景3因為障礙物較多，頻繁改變方向，使得維持恒定速率相對更加困難，偏航角的局部抖動較前2個場景稍多稍大，但整體上也沒有產(chǎn)生巨大突變。偏航角的抖動主要來自于勢場的更換，由于探測信息的變動和無人機(jī)的運動，使得勢場的構(gòu)成和目標(biāo)點的選取發(fā)生變化，求解得到的新的勢場和原勢場在同一點就可能產(chǎn)生不同的參考方向。這種輕微的抖動是可以接受的，也是無人機(jī)根據(jù)新的信息調(diào)整方向的表現(xiàn)。另外，從圖中可以看出，控制器通過對速度分量的跟蹤，使得偏航角也緊緊跟隨著參考值，可以快速響應(yīng)參考方向的變化。

圖4 MATLAB仿真實驗路徑圖Fig.4 The path results in MATLAB simulations

圖5 速度的跟蹤控制Fig.5 The tracking control of velocity

經(jīng)過統(tǒng)計，場景1從起點到目標(biāo)點，總共運行了11 737個控制周期，根據(jù)本文控制頻率100 Hz的設(shè)定，這相當(dāng)于飛行了117.37 s。其中總共解算了118個局部拉普拉斯勢場，使用高斯賽德爾法求解的平均迭代次數(shù)為897.559 3(計整個局部地圖迭代一遍為1次)，且迭代收斂次數(shù)的標(biāo)準(zhǔn)差為155.440 7，從拿到傳感器數(shù)據(jù)到求解出拉普拉斯勢場的平均時間為0.020 7 s，標(biāo)準(zhǔn)差為0.003 3 s。從該結(jié)果可以看出，達(dá)到收斂所需的迭代次數(shù)比較一致，且求解時間較快，完全滿足實時要求，可以接受最快約48.3 Hz的傳感器信息更新頻率。至于獲取梯度，只需對周圍幾個點進(jìn)行差分和插值，所用時間可以忽略不計。

4 結(jié)論

本文提出基于局部拉普拉斯人工勢場的無人機(jī)實時避障導(dǎo)航，該方法有以下特點：

1) 僅使用實時傳感器信息構(gòu)建局部邊值問題，采用有限差分法求解拉普拉斯人工勢場，該勢場可以適應(yīng)各種形態(tài)大小各異的障礙物。調(diào)和函數(shù)的良好特性使得勢場內(nèi)部不存在局部極小點，且勢場梯度連續(xù)光滑使導(dǎo)航軌跡平穩(wěn)。

圖6 水平速率和偏航角Fig.6 Horizontal speeds and yaw angles

2) 隨著傳感器信息的更新，重新計算并替換拉普拉斯勢場，控制部分始終使用最新解出的勢場獲取無人機(jī)當(dāng)前點的參考方向，并將其結(jié)合設(shè)定的速率計算出參考速度，以進(jìn)行速度的跟蹤控制。

3) 設(shè)計并實現(xiàn)基于PID的控制系統(tǒng)，并使用MATLAB進(jìn)行仿真實驗以驗證該導(dǎo)航方法。

仿真實驗顯示，該方法在不同外界環(huán)境下，可以較為平滑地避開障礙物并趨向目標(biāo)點。最終的軌跡不僅總體較短，而且盡可能地保持了到各障礙物距離的均衡，導(dǎo)航更為安全。該方法具有較好的實時性，可以應(yīng)用到現(xiàn)實場景中。