顧育津，宋孝成，劉曉培，陸疌

(1 中國科學(xué)院上海微系統(tǒng)與信息技術(shù)研究所, 上海 200050； 2 中國科學(xué)院大學(xué), 北京 100049； 3 上?？萍即髮W(xué), 上海 201210)

近些年來，無人機(jī)的自主飛行問題備受研究者的關(guān)注。無人機(jī)想要完成這一任務(wù)，關(guān)鍵是運動規(guī)劃(motion planning)[1]。解決這一問題多數(shù)思路是預(yù)先規(guī)劃生成合理的避障路徑，再令無人機(jī)沿著路徑行至目標(biāo)點[2]。典型的路徑規(guī)劃算法包括如A*、D*、Dijkstra的最短路徑算法[3]，基于快速隨機(jī)搜索樹(rapidly-exploring random tree，RRT)的一系列隨機(jī)搜索算法，如DDRRT[4]，RRT*[5]以及人工勢場法(artificial potential field)。人工勢場法由Khatib于1986年提出[6]，其基本思想是建立一種目標(biāo)位置處的引力場和障礙物位置處的斥力場共同作用下的人工勢場，在進(jìn)行路徑規(guī)劃時沿著負(fù)梯度的方向進(jìn)行路徑搜索。然而傳統(tǒng)人工勢場法最大的缺陷在于勢場中可能存在局部最小值點。為解決這一問題，文獻(xiàn)[7]提出一種基于搜索的方法，但需要較高的計算量；文獻(xiàn)[8]針對局部最小點設(shè)置附加的虛擬勢場，文獻(xiàn)[9]針對特定場景制定特殊的導(dǎo)航策略，但都不夠簡潔直觀。

研究表明基于拉普拉斯方程的調(diào)和勢場函數(shù)(harmonic potential field) 不存在局部最小值[10]。本文基于傳感器探測生成的全局地圖建立調(diào)和勢場，工作步驟如下：

1)采取邊界元法(boundary element method，BEM)在歐式空間抽象出地圖中的邊界與障礙物，這種方法只需要利用地圖中障礙物的邊界單元，存儲要求低并且可以快速求解出勢場中任一點的梯度方向。

2)設(shè)計線性二次高斯(linear quadratic Gaussian，LQG)控制器進(jìn)行頂層的運動控制，在獲得相應(yīng)的參考速度方向后，控制器跟蹤調(diào)整無人機(jī)姿態(tài)使自身速度方向趨向參考方向，最終逐漸飛向目標(biāo)點。這種基于速度場的控制器，不需要機(jī)械地沿著預(yù)先規(guī)劃好的路徑行走，在應(yīng)對傳感器誤差、環(huán)境影響上更具魯棒性。

3)在仿真平臺Airsim 進(jìn)行試驗。實驗結(jié)果顯示面對數(shù)量較多且分布復(fù)雜的障礙物，本文提出的算法可以有效引導(dǎo)無人機(jī)進(jìn)行避障控制并最終到達(dá)目標(biāo)點。

本文設(shè)計的算法脫胎于傳統(tǒng)人工勢場方法的思想，著力尋找解決傳統(tǒng)方法固有缺陷的途徑，在實際應(yīng)用中更具備實用性與可靠性。拉普拉斯人工勢場可建立在由傳感器掃描重建的地圖上(如點云)，不同于傳統(tǒng)人工勢場，這種方式對于障礙物的邊界與位置描述更為精準(zhǔn)，有利于保障無人機(jī)在復(fù)雜環(huán)境中的路徑規(guī)劃與飛行安全，也更適合拓展到實時探測與規(guī)劃的層面。對比RRT、A*、遺傳算法等搜索算法，拉普拉斯人工勢場從本質(zhì)上避免了陷入“局部最優(yōu)解”的煩惱，保證收斂到全局最優(yōu)解。邊界元法往往應(yīng)用在力學(xué)等傳統(tǒng)工程問題上，本文設(shè)計的算法將邊界元法與地圖模型特性良好結(jié)合，在求解導(dǎo)航問題時無須設(shè)置調(diào)試眾多參數(shù)，在應(yīng)用實現(xiàn)上更為簡便可靠。

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

1.1 人工勢場的構(gòu)建

人工勢場法的基本思想是在無人機(jī)或者機(jī)器人的可行空間上建立合適的勢函數(shù)，使其沿著函數(shù)負(fù)梯度方向運動，直至最終到達(dá)目標(biāo)點。傳統(tǒng)的人工勢場法分別對障礙物跟目標(biāo)點建立引力斥力函數(shù)從而進(jìn)行代數(shù)疊加得到某一點合力函數(shù)。這種方法往往無法應(yīng)對實際應(yīng)用場景中各種復(fù)雜形態(tài)的障礙物，簡單的幾何替代會損失一定程度的規(guī)劃精度；當(dāng)?shù)貓D中障礙物的數(shù)量增加、相對位置更為復(fù)雜后，這樣產(chǎn)生的勢場也容易變得局部畸形，比如可能存在的局部最小點將無人機(jī)困住無法逃離，某些點梯度方向的驟變導(dǎo)致最終的飛行路徑不夠光滑，加大了姿態(tài)控制的難度。

那么稱φ在Ω上是調(diào)和的，其中Δ為拉普拉斯算子。調(diào)和函數(shù)具有許多良好的性質(zhì)，由強(qiáng)最大最小定律[11]可知，φ在Ω上沒有局部最小點，且可推導(dǎo)出如下的結(jié)論：

命題1表明在有界區(qū)域上的調(diào)和函數(shù)值被邊界值所限定，且全局最大最小值只能出現(xiàn)在邊界上。

由定義可知，調(diào)和函數(shù)是拉普拉斯方程的解，因而可以通過求解該偏微分方程構(gòu)建調(diào)和勢場。當(dāng)合理設(shè)置邊界條件時，相應(yīng)的調(diào)和勢場函數(shù)將唯一確定[12]。常見的拉普拉斯方程邊界條件有兩種——Dirichlet與Neumann。前者是給勢場邊界設(shè)置固定的勢場值，后者則是設(shè)置邊界處的外法線方向?qū)?shù)。在二維空間下，上述初始邊界條件下的拉普拉斯方程可表示為

(1)

(2)

(3)

在可行空間上覆蓋的調(diào)和函數(shù)沒有局部極小點，二階可微的性質(zhì)令導(dǎo)出的梯度場仍是光滑的，非常適合后續(xù)的導(dǎo)航控制。將構(gòu)建可行空間上的調(diào)和函數(shù)轉(zhuǎn)化成式(1)～式(3)組成的邊界值問題(boundary value problem)便是簡單直觀的方式。

Γo=?Ω∪?,

目標(biāo)點的包圍邊界為目標(biāo)邊界Γg:

基于上述定義可構(gòu)建針對無人機(jī)運動規(guī)劃的拉普拉斯方程，在阻礙邊界與目標(biāo)邊界處分別設(shè)置Dirichlet條件，式(1)～式(3) 轉(zhuǎn)化為

(4)

φ(x)=M,?x∈Γo，

(5)

φ(x)=m,?x∈Γg.

(6)

式中：m與M均為實數(shù)，且M>m。

1.2 梯度場的求解

H(x,y)φ(y)]dS(y),

(7)

其中：

(8)

(9)

(10)

如圖1,S表示有界區(qū)域的邊界曲線，y是其上一點，r表示x與y兩點間的歐氏距離。G(x,y)為二維空間下的格林函數(shù)，n為外法線方向向量。區(qū)域內(nèi)任一點的梯度也可直接導(dǎo)出，即 ?x∈Ω,i=1,2，

圖1 邊界積分方程Fig.1 Boundary integration equation

(11)

結(jié)合式(7)～式(11)，一旦求解出邊界上的勢場值與外法線方向?qū)?shù)，就可以得到集合區(qū)域內(nèi)任一點的勢場值與梯度值。

由于邊界積分方程沒有解析解，采取邊界元法進(jìn)行離散數(shù)值求解。本文將邊界S分割成N個常量單元，每個邊界單元為直線段，表示為ΔSi,i=1,…,N。每個單元中心上有一節(jié)點pi,i=1,…,N，單元上的勢場值φi與 外法線方向?qū)?shù)qi等于節(jié)點上的值，從而將邊界積分方程轉(zhuǎn)化為分段線積分

(12)

其中

(13)

(14)

本文采用文獻(xiàn)[13]中提到的解析解求解式(13)、式(14)中的線積分。特別地，由于可達(dá)區(qū)域為多連通集，在總構(gòu)型邊界?Ω上的元素其線積分為逆時針方向，反之障礙物邊界與目標(biāo)邊界上的線積分方向為順時針。令向量Φ=(φ1,…,φN)，q=(q1,…,qN),并構(gòu)造矩陣H,G∈n×n使其第i行，第j列的元素分別為Gij與Hij，式(12)轉(zhuǎn)化為

(15)

本文中拉普拉斯方程的初始條件均為Dirichlet條件，無須進(jìn)行元素交換，式(15)即為形如Ax=b的標(biāo)準(zhǔn)線性方程組。一旦求解出方程的解，便可利用邊界元素上所有的φ與q求解可達(dá)區(qū)域中任意一點的負(fù)梯度值，并作為隨后運動控制的參考速度方向。

通過以上研究，本文認(rèn)為當(dāng)前正處于轉(zhuǎn)型期的中國不應(yīng)只將經(jīng)濟(jì)增長、收入增加作為國家追求的終極目標(biāo)，而應(yīng)該在經(jīng)濟(jì)取得一定成就后，逐漸將視角轉(zhuǎn)移到增強(qiáng)國民實際幸福感這一問題上。而提高國民幸福感除了采取措施提高收入外，改善醫(yī)療、教育、就業(yè)等民生問題顯得十分重要。另外，由于個體之間對于幸福的感受存在異質(zhì)性，政策的制定和實施應(yīng)該做到有差別且具針對性，應(yīng)該更多關(guān)注弱勢群體和幸福感低的群體，提升他們的幸福度，以維護(hù)社會的穩(wěn)定和經(jīng)濟(jì)的可持續(xù)性發(fā)展。

2 基于梯度場的姿態(tài)控制

實際飛行過程中，無人機(jī)的建模不是單純的質(zhì)點，需要面對空氣阻力、自身傳感器誤差等諸多問題。本節(jié)設(shè)計的頂層控制算法將連接路徑規(guī)劃與底層的姿態(tài)控制，當(dāng)無人機(jī)到達(dá)某一位置時，上一節(jié)構(gòu)建的人工勢場能夠?qū)崟r導(dǎo)出其下一時刻的參考速度方向，控制算法將對應(yīng)調(diào)整姿態(tài)使其逐漸趨向所設(shè)計的運動狀態(tài)。

慣性坐標(biāo)系采用NED 坐標(biāo)系，即x軸指向東，y軸指向北，z軸指向地面，令機(jī)身坐標(biāo)系基底方向為 [x′y′z′],相關(guān)符號的詳細(xì)含義參見表1。

表1 建立模型所用變量Table 1 Variables used in dynamic model

旋轉(zhuǎn)矩陣R描述輸入分量u到無人機(jī)姿態(tài)的映射，同時將姿態(tài)控制分解為對u的控制?？紤]無人機(jī)飛行時的運動學(xué)方程，它被描述成如下的線性系統(tǒng)：

(16)

(17)

其中Q為二階半正定矩陣，Qe與R為二階正定矩陣。

圖2 LQG伺服控制Fig.2 Linear quadratic Gaussian servo controller

其中Kf為卡爾曼系數(shù)。求解對應(yīng)的 Riccati 方程

K∈2×4為對應(yīng)增廣狀態(tài)方程下LQR調(diào)節(jié)器的反饋系數(shù)。在忽略系統(tǒng)擾動與觀測噪聲的情況下，增廣系統(tǒng)表示為

相應(yīng)的目標(biāo)函數(shù)為

3 實驗仿真與分析

本文采用 Airsim[16]作為實驗仿真平臺。Airsim 是微軟開發(fā)的一款開源自動駕駛仿真平臺，基于虛幻引擎打造，能最大限度地模擬現(xiàn)實環(huán)境中的物理表現(xiàn)。Airsim 為無人機(jī)模型提供基于PX4的底層控制，將上節(jié)提到的旋轉(zhuǎn)矩陣R轉(zhuǎn)換為四元數(shù)即可直接控制無人機(jī)的運動。

本文的Airsim仿真程序基于 C++ 編寫，CPU為Xeon E5-2650，主頻2.3 GHz。為檢驗算法的有效性跟可靠性，建立多種不同類型的全局地圖。地圖同樣采用NED坐標(biāo)系，無人機(jī)的高度限定在5 m。實驗1中，底邊長為20 m的立方柱緊密排列，構(gòu)成一個類似走廊的地圖環(huán)境。地圖的總區(qū)域為一個140 m×110 m的截面，目標(biāo)點位于(40 m,60 m)，以邊長為1的正方形包圍構(gòu)成目標(biāo)邊界。設(shè)阻礙邊界Γo初值為100，目標(biāo)邊界Γg初值為0，使用BEM算法求解對應(yīng)的拉普拉斯方程，可求得如圖3的人工勢場。圖3(a)、3(b)分別展示人工勢場的等高線勢場與流線圖。

圖4展示Airsim中的實際仿真結(jié)果。無人機(jī)的質(zhì)量設(shè)定為1 kg，起點位于(-20 m,-15 m)，LQG控制器中的相應(yīng)參數(shù)飛行速度大小‖v‖維持在3 m/s。當(dāng)程序獲取到地圖全局信息后，通過BEM求解建立人工勢場。求解過程一旦完成，無人機(jī)可實時計算出下一時刻的參考速度方向，并在控制器的引導(dǎo)下調(diào)整姿態(tài)從而逐步達(dá)到目標(biāo)點。仿真得到的無人機(jī)實際軌跡與圖3中的流線圖幾乎一致。可以看到，運動軌跡的光滑性體現(xiàn)了拉普拉斯勢場無限可微的優(yōu)勢，這一特點也更好地適應(yīng)對無人機(jī)的動力控制；另一方面路徑選擇也達(dá)到了路徑長度最優(yōu)與碰撞概率最小的某種平衡。

圖4 實驗1：Airsim仿真圖Fig.4 Experiment 1: Airsim simulation

實驗2中，地圖中障礙物的數(shù)量增多，且分布更為凌亂。圖5(a)～5(c)分別展示不同起點下無人機(jī)的飛行軌跡。結(jié)果顯示，無論起點在何處，無人機(jī)都可以自行選擇合適的飛行方向達(dá)到目標(biāo)點，且不需要再進(jìn)行重復(fù)的規(guī)劃計算。傳統(tǒng)的路徑規(guī)劃與導(dǎo)航控制算法大都采取預(yù)先根據(jù)地圖規(guī)劃并生成路徑，再利用控制器控制無人機(jī)進(jìn)行路徑跟蹤的理念。在實際應(yīng)用中，一旦遭遇強(qiáng)風(fēng)、信號干擾等意外，無人機(jī)的底層控制器超出穩(wěn)態(tài)，其位置發(fā)生偏移，路徑跟蹤器將逐漸引導(dǎo)無人機(jī)回到之前的路徑或進(jìn)行重新規(guī)劃。本文提出的算法在應(yīng)對這種情況時有明顯的優(yōu)勢，圖5(d)展示強(qiáng)烈外界干擾后，無人機(jī)自行調(diào)整的飛行結(jié)果。在Airsim中模擬無人機(jī)遭遇瞬間大風(fēng)后發(fā)生短暫失控的情形，其中折線段表示無人機(jī)的位置瞬間偏離，圖5(d)中的無人機(jī)起點與圖5(c)中一致，但由于遭遇意外干擾，在岔路口處路徑選擇與圖5(c)并不一致，從而使得整個飛機(jī)軌跡都截然不同。這種調(diào)整并不需要額外的程序設(shè)計與線程來應(yīng)對，節(jié)約了計算資源，實時性高，非常適合實際場景的應(yīng)用。

圖5 實驗2：Airsim仿真圖Fig.5 Experiment 2: Airsim simulation

仿真實驗結(jié)果表明本文設(shè)計的算法非常適合遷移到實際飛行的應(yīng)用中。實驗2中的全局地圖抽象得到的476個邊界元，采用單線程求解時間僅需2.01 s。這一步驟可由無人機(jī)自帶的處理器進(jìn)行計算，也可線下計算通信傳輸，通信負(fù)擔(dān)僅為每個邊界元的位置坐標(biāo)與H,G兩個n×n矩陣。模擬飛行中，將無人機(jī)的底層控制頻率設(shè)為100 Hz，在每個控制周期內(nèi)，無人機(jī)根據(jù)自身位置實時計算出當(dāng)前的參考速度，平均耗時僅在3 ms左右。圖6展示整個飛行過程中無人機(jī)的飛行速度變化，其中實線為參考速度值，虛線為實際值。 即便當(dāng)參考速度發(fā)生突變時，控制器也能及時響應(yīng)，迅速改變調(diào)整無人機(jī)的速度達(dá)到理想狀態(tài)。

圖6 速度方向的追蹤控制Fig.6 Velocity direction tracking control

4 結(jié)論

本文提出基于拉普拉斯方程的人工勢場，

1)根據(jù)全局地圖抽象出可行區(qū)域邊界，合理設(shè)置初始邊界條件，采用邊界元法求解相應(yīng)方程。求解得出的調(diào)和勢場相較傳統(tǒng)人工勢場可以適應(yīng)各種形態(tài)復(fù)雜的障礙物，內(nèi)部不存在局部極小點，二階可微使得導(dǎo)出的梯度場連續(xù)光滑，有利于無人機(jī)等移動機(jī)器人的控制；

2)基于人工勢場導(dǎo)出的速度場，設(shè)計關(guān)于無人機(jī)姿態(tài)的控制器?？刂破魇篃o人機(jī)合理調(diào)整姿態(tài)，不斷追蹤更新中的參考速度，直至最終達(dá)到目標(biāo)點；

3) 基于Airsim的擬真環(huán)境的仿真實驗，驗證了算法的有效性與魯棒性。在應(yīng)對外界環(huán)境的意外干擾下，算法保證了無人機(jī)路徑與運動狀態(tài)的合理選擇，具有較好的實時性與較廣泛的實際應(yīng)用前景。鑒于本文提出的人工勢場依舊構(gòu)建在二維空間，對于三維空間下的應(yīng)用擴(kuò)展將是后續(xù)研究的方向。