基于拉普拉斯人工勢(shì)場(chǎng)的無(wú)人機(jī)避障控制*

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

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

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

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

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

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

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

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

1 基于拉普拉斯方程的人工勢(shì)場(chǎng)

1.1 人工勢(shì)場(chǎng)的構(gòu)建

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

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

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

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

(1)

(2)

(3)

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

Γo=?Ω∪?,

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

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

(4)

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

(5)

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

(6)

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

1.2 梯度場(chǎng)的求解

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

(7)

其中：

(8)

(9)

(10)

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

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

(11)

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

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

(12)

其中

(13)

(14)

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

(15)

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

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

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

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

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

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

旋轉(zhuǎn)矩陣R描述輸入分量u到無(wú)人機(jī)姿態(tài)的映射，同時(shí)將姿態(tài)控制分解為對(duì)u的控制。考慮無(wú)人機(jī)飛行時(shí)的運(yùn)動(dòng)學(xué)方程，它被描述成如下的線性系統(tǒng)：

(16)

(17)

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

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

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

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

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

3 實(shí)驗(yàn)仿真與分析

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

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

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

圖4 實(shí)驗(yàn)1：Airsim仿真圖Fig.4 Experiment 1: Airsim simulation

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

圖5 實(shí)驗(yàn)2：Airsim仿真圖Fig.5 Experiment 2: Airsim simulation

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

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

4 結(jié)論

本文提出基于拉普拉斯方程的人工勢(shì)場(chǎng)，

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

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

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