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

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

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

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

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

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

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

本文設(shè)計的算法脫胎于傳統(tǒng)人工勢場方法的思想，著力尋找解決傳統(tǒng)方法固有缺陷的途徑，在實際應(yīng)用中更具備實用性與可靠性。拉普拉斯人工勢場可建立在由傳感器掃描重建的地圖上(如點云)，不同于傳統(tǒng)人工勢場，這種方式對于障礙物的邊界與位置描述更為精準，有利于保障無人機在復(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)建

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

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

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

由定義可知，調(diào)和函數(shù)是拉普拉斯方程的解，因而可以通過求解該偏微分方程構(gòu)建調(diào)和勢場。當合理設(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=?Ω∪?,

目標點的包圍邊界為目標邊界Γg:

基于上述定義可構(gòu)建針對無人機運動規(guī)劃的拉普拉斯方程，在阻礙邊界與目標邊界處分別設(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)任一點的勢場值與梯度值。

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

(12)

其中

(13)

(14)

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

(15)

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

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

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

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

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

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

旋轉(zhuǎn)矩陣R描述輸入分量u到無人機姿態(tài)的映射，同時將姿態(tài)控制分解為對u的控制?？紤]無人機飛行時的運動學(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)的目標函數(shù)為

3 實驗仿真與分析

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

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

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

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

實驗2中，地圖中障礙物的數(shù)量增多，且分布更為凌亂。圖5(a)～5(c)分別展示不同起點下無人機的飛行軌跡。結(jié)果顯示，無論起點在何處，無人機都可以自行選擇合適的飛行方向達到目標點，且不需要再進行重復(fù)的規(guī)劃計算。傳統(tǒng)的路徑規(guī)劃與導(dǎo)航控制算法大都采取預(yù)先根據(jù)地圖規(guī)劃并生成路徑，再利用控制器控制無人機進行路徑跟蹤的理念。在實際應(yīng)用中，一旦遭遇強風、信號干擾等意外，無人機的底層控制器超出穩(wěn)態(tài)，其位置發(fā)生偏移，路徑跟蹤器將逐漸引導(dǎo)無人機回到之前的路徑或進行重新規(guī)劃。本文提出的算法在應(yīng)對這種情況時有明顯的優(yōu)勢，圖5(d)展示強烈外界干擾后，無人機自行調(diào)整的飛行結(jié)果。在Airsim中模擬無人機遭遇瞬間大風后發(fā)生短暫失控的情形，其中折線段表示無人機的位置瞬間偏離，圖5(d)中的無人機起點與圖5(c)中一致，但由于遭遇意外干擾，在岔路口處路徑選擇與圖5(c)并不一致，從而使得整個飛機軌跡都截然不同。這種調(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。這一步驟可由無人機自帶的處理器進行計算，也可線下計算通信傳輸，通信負擔僅為每個邊界元的位置坐標與H,G兩個n×n矩陣。模擬飛行中，將無人機的底層控制頻率設(shè)為100 Hz，在每個控制周期內(nèi)，無人機根據(jù)自身位置實時計算出當前的參考速度，平均耗時僅在3 ms左右。圖6展示整個飛行過程中無人機的飛行速度變化，其中實線為參考速度值，虛線為實際值。 即便當參考速度發(fā)生突變時，控制器也能及時響應(yīng)，迅速改變調(diào)整無人機的速度達到理想狀態(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ù)光滑，有利于無人機等移動機器人的控制；

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

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