改進遺傳算法的無人機路徑規(guī)劃

呂 倩，孫憲坤，熊玉潔

（上海工程技術(shù)大學(xué) 電子電氣工程學(xué)院，上海 201620）

0 引言

無人飛行器（unmanned aerial vehicle，UAV）的自主飛行與導(dǎo)航是1 個極具挑戰(zhàn)性的任務(wù)[1]，該任務(wù)可分為環(huán)境信息感知、路徑規(guī)劃和飛行控制3 個階段[2-3]。其中，路徑規(guī)劃階段上承環(huán)境感知，下接飛行控制，起到重要作用。在無人機機載傳感器準確探測周圍環(huán)境信息并建立地圖模型之后，規(guī)劃算法根據(jù)環(huán)境感知結(jié)果開始規(guī)劃從起點至終點的無碰撞軌跡。軌跡規(guī)劃完成后，飛行控制系統(tǒng)根據(jù)規(guī)劃結(jié)果指導(dǎo)無人機按線路飛行[4]。理想狀態(tài)下，只要路徑不接觸障礙物，無人機就可以安全到達終點。但實際飛行任務(wù)中，傳感器探測環(huán)境中的障礙物范圍會有所偏差，控制系統(tǒng)指導(dǎo)無人機飛行時也會出現(xiàn)控制誤差。既定路線距離障礙物越近，發(fā)生碰撞的可能性越大，所以本文采用使軌跡盡可能遠離障礙物的思想來減少碰撞可能。同時，由于無人機機載電池的續(xù)航時間極其有限，要無人機從起點至終點的路程不能過長。通常，解決路徑規(guī)劃問題有人工勢場法（artificial potential field method，APFM）、基于隨機采樣的方法、基于圖搜索的規(guī)劃方法、借鑒生物界進化規(guī)律的遺傳算法（genetic algorithm，GA）等幾類方法。

文獻[5]中采用人工勢場法，通過人工為飛行環(huán)境創(chuàng)造1 個包含斥力極與引力極的勢場來完成路徑規(guī)劃任務(wù)。但若環(huán)境中某些位置的引力與斥力達到平衡就會陷入局部最優(yōu)，導(dǎo)致無法完成規(guī)劃任務(wù)。基于隨機采樣的路徑規(guī)劃方法有快速擴展隨機樹（rapidly-exploring random tree， RRT）[6]、在 RRT 的基礎(chǔ)上加入了自起點與終點雙向搜索引導(dǎo)策略的雙向快速擴展隨機樹（rapidlyexploring random tree connect，RRTConnect）[7]、加入啟發(fā)式策略及貪心思想的RRT*[8]等。這些變體在原始 RRT 的基礎(chǔ)上加快了向終點的逼近速度。基于隨機采樣的算法理論上是可行的，但有時由于節(jié)點選擇具有隨機性，若進入環(huán)境死角會導(dǎo)致無法成功到達終點；即便到達終點，所得的路徑也具有不穩(wěn)定性，無法達到使無人機飛行路程較短以減少電池能量消耗的目的；同時，由于每階段的路徑與方向選擇存在隨機性，因此同樣無法滿足平滑易控制的條件。基于圖搜索的算法，如迪杰斯特拉（Dijkstra）算法[9]，加入了啟發(fā)式思想的A*算法[10]及各種變體，加入了修剪策略用于提高搜索速度的跳轉(zhuǎn)點搜索算法（jump point search，JPS）[1,11]及其變體等。這些算法雖然通常情況可以到達終點，能滿足飛行路程短的要求，但無法保證無人機在飛行過程中遠離障礙物，增加了發(fā)生碰撞的可能；而且路徑急轉(zhuǎn)彎情況較多，不夠平滑，造成飛行控制難度增加，同樣無法保證飛行安全。

遺傳算法是從生物進化思想得到啟發(fā)而提出的優(yōu)化算法。文獻[12]提出的進化算法，可以有效地為給定障礙空間內(nèi)的起始位置到目標位置規(guī)劃出若干條可行路徑，并采用改進的遺傳算法解決靜態(tài)環(huán)境下的多目標路徑規(guī)劃問題。文獻[13]用遺傳算法與蟻群算法2 種仿生物學(xué)算法解決災(zāi)后應(yīng)急物資的路徑規(guī)劃問題，該研究適用于為可選擇路線已存在的情況下規(guī)劃最短路徑問題。文獻[14]在遺傳算法的種群更新階段，結(jié)合了模擬退火算法，令算法避免陷入局部最優(yōu)，從而得出最短的援救路線。但這些方法都只是從得出最短路徑的目的出發(fā)來規(guī)劃路徑，并未考慮機器人實際執(zhí)行時，在環(huán)境感知與飛行控制階段產(chǎn)生的誤差對路線的要求。若規(guī)劃路徑距離障礙物過近，在這2 個階段稍有偏差的情況下就會發(fā)生碰撞。無人機距障礙物越遠，因感知與控制誤差等因素導(dǎo)致的無人機與障礙物發(fā)生碰撞的概率也越小。本文通過規(guī)劃，使無人機盡可能遠離障礙物的路徑，以達到保證無人機飛行安全的目的[15]，同時也要考慮飛行路程問題。所以在設(shè)計適應(yīng)度函數(shù)時，通過為路程長度和路徑點距障礙物距離設(shè)置權(quán)重來達到平衡2 者的目的。在此基礎(chǔ)上，還加入精英個體保留策略，用于保證種群不會向更壞的方向繁衍；加入刪除操作，以杜絕冗余路徑節(jié)點的出現(xiàn)，以期有效縮短飛行路徑長度。最后通過起始點全部在寬闊區(qū)域與起點位于狹窄的障礙物內(nèi)部區(qū)域2 種場景的實驗，與人工勢場法、基于隨機采樣的RRT 算法、基于啟發(fā)式圖搜索的A*算法、原始遺傳算法相比較。

1 模型建立

1.1 環(huán)境表示

假設(shè)無人機所處環(huán)境如圖1 所示。四周的墻體表示環(huán)境邊界，無人機飛行不能越過此范圍。墻體內(nèi)的中空柱體與圓錐可表示樓房等障礙物，飛行過程中要避免與之發(fā)生碰撞。

圖1 無人機所處環(huán)境模型

在為無人機規(guī)劃路徑前，首先要對環(huán)境進行簡化，將其轉(zhuǎn)換為可進行信息處理的模型。由于3 維空間可表示為多個2 維平面的堆積，故本文針對 2 維環(huán)境進行處理。柵格法表示環(huán)境信息是常用的建模方式[3]，本文使用柵格表示2 維占用環(huán)境。模型表示如圖2 所示。

圖2 2 維環(huán)境柵格表示

以左下角為坐標原點建立直角坐標系，圖2 中黑色部分為障礙物，白色部分為可供無人機自由飛行的無障礙區(qū)域，左下角標識位置為無人機當前位置，右上角標識位置為期望到達的終點。在從起點到終點的過程中，根據(jù)無人機機載傳感器探測的范圍，可分階段為其規(guī)劃路徑。

1.2 遺傳模型建立

進化算法是計算科學(xué)的重要領(lǐng)域，它使用生物進化的思想來解決計算問題。遺傳算法是進化計算的最廣泛的使用形式之一，可以在不斷變化與復(fù)雜未知空間中尋找解決方案。本文采用遺傳算法分階段解決路徑規(guī)劃問題，其中，路徑 PI可表示為

式中： p0表示起點； pn表示終點；下標i 表示路徑節(jié)點的序號；pi表示整條路徑中的第i 個路徑節(jié)點；（ pi-1，pi）表示第i 段路徑。

圖3 為某條路徑的示意圖。在規(guī)劃過程中，每條自起點至終點的無碰撞路徑表示為1 個個體，每個個體中有1 條染色體，故每條路徑也可稱為1 條染色體。路徑中的每個階段（ pi， pi+1）表示為1 個基因。所有個體的集合即生成的所有從起點到終點無碰撞路徑表示為種群。適應(yīng)度函數(shù)的目的是將每個個體的屬性量化，通過設(shè)計適應(yīng)度函數(shù)來從種群中篩選出需要的個體。適應(yīng)度越高的個體就越是精英個體。

圖3 路徑表示

2 算法介紹

本文主要采用遺傳算法的思想為在復(fù)雜未知環(huán)境中的無人機規(guī)劃路徑，算法流程如圖4 所示。

圖4 算法流程

2.1 初始化種群

遺傳算法的種群初始化階段要求種群中個體具有隨機性與多樣性[3]，即在地圖中隨機生成自起點到終點的無碰撞路徑。路徑規(guī)劃可分為全局規(guī)劃和局部規(guī)劃。全局路徑規(guī)劃是指周圍環(huán)境信息已知，無人機只需要按照既定路徑飛行。但現(xiàn)實中的環(huán)境是復(fù)雜可變的，無人機無法在飛行任務(wù)開始之前得到周圍環(huán)境的先驗信息，所以通常需要依靠無人機機載傳感器探測到的環(huán)境信息的實時數(shù)據(jù)分階段規(guī)劃路徑。

局部路徑規(guī)劃的種群初始化可分為2 個階段，在第1 階段，終點位于機載傳感器最大感知范圍之外，在無人機當前所處位置ip 無法知道終點位于何處，只能采用隨機選擇的方式來生成下1 階段路徑點 ip+1。當從 ip 到 ip+1的途中會經(jīng)過障礙物時，則重新選擇1 個點作為 ip+1。

在第2 階段，當無人機此時位于 jp 點，假設(shè)此時終點位于傳感器感知范圍之內(nèi)，由于要規(guī)劃的是1 條距離較短的路徑，所以此時可以為下1 個候選路徑點pj+1限制范圍，即

如此可令無人機更快地接近終點。但為保證初始種群的多樣性與隨機性，本文設(shè)定限制在該范圍的個體占初始種群總數(shù)的50%，另外的50%依然采用隨機選擇的方式選取路徑節(jié)點。

2.2 個體適應(yīng)度函數(shù)

適應(yīng)度函數(shù)用于將種群中每個個體的屬性量化，不同的適應(yīng)度函數(shù)在同1 種群中篩選出的精英個體也不同。本文要求為無人機規(guī)劃出從起點到終點的無碰撞、路程短、平滑可行的路徑。保證無人機的安全行駛為路徑規(guī)劃的第1 要義。考慮到無人機機載傳感器對環(huán)境感知的誤差，以及路徑規(guī)劃完成之后，飛行控制系統(tǒng)按照規(guī)劃結(jié)果指導(dǎo)飛機實際飛行時可能出現(xiàn)的誤差，若規(guī)劃出的路徑距障礙物過近則會增加碰撞概率，故要通過設(shè)計適應(yīng)度函數(shù)選擇出盡可能遠離障礙物的軌跡，這樣在飛行過程中發(fā)生碰撞的概率也會更小。同時也要考慮路程短的要求，適應(yīng)度函數(shù)流程圖如圖5 所示。

圖5 適應(yīng)度函數(shù)流程

在有N個個體的種群中，F(xiàn)itness（PI）為個體 PI的適應(yīng)度函數(shù)，可表示為

式中：Length(PI)為路徑 PI的總長度的函數(shù)；w1為第1 部分Length( PI)在整體中所占比重； NSample( PI)為路徑 PI上的隨機采樣點個數(shù)函數(shù)；w2為第2 部分 NSample( PI)在整體中所占比重。

采樣點個數(shù)函數(shù) NSample( PI)可以表示為

式中δ 為正整數(shù)。

假設(shè)在路徑 PI上隨機選取的路徑點為 pk，點 pk與距其最近障礙點 Obstacle的距離 dpk_Obs可表示為

若

則令

式中：θ 為大于無人機半徑的定值； Number( PI)為在路徑 PI上選擇的采樣點中 dpk_Obs＜θ 的點的數(shù)量。

適應(yīng)度函數(shù)由路徑 PI的長度與路徑中距離障礙物過近的點的數(shù)量2 部分組成。函數(shù)值越大，則路徑性能越差，函數(shù)值越小的個體越是種群中的精英個體。

2.3 交叉與變異

交叉與變異操作是為了產(chǎn)生更多的新個體，交叉與變異概率的大小影響了新個體產(chǎn)生的速度。發(fā)生交叉與變異的概率越大，產(chǎn)生新的個體的速度也越快。本文選擇在種群中路徑的交點處以一定的概率進行交叉操作以保證路徑的連續(xù)性。

對于變異而言，本文采用隨機變異的方法，即在地圖的非障礙區(qū)域內(nèi)隨機選擇某個路徑點 Pi′用于替換原本的路徑點 Pi。若替換之后的路徑點 Pi′與 前 后點的 連 線即路 線（ Pi-1，Pi′）（ Pi，′ Pi+1）通過障礙物，則更換 Pi′為 Pi′，直到（ Pi-1，Pi′）( Pi′， Pi+1)中間無障礙物，針對路徑點 Pi的變異完成。變異完成后，若變異得到的路徑相較于變異前更優(yōu)，及適應(yīng)度函數(shù)值更小，則采用變異之后的路徑，否則，采用原有路徑。如此可保證變異朝著更優(yōu)的方向進行，不會變異出更差的個體。

2.4 刪除

本文采用文獻[3]中增加的刪除操作，目的是為了排除最優(yōu)路徑出現(xiàn)冗余路徑點的情況，增加此步驟可以有效地縮短路徑長度，防止節(jié)點冗余情況的發(fā)生。主要思想為：從終點開始遍歷每個路徑節(jié)點，若某節(jié)點 ip 可以與起點無障礙相連，則起點與節(jié)點 ip 中間的節(jié)點就是冗余節(jié)點，刪除操作就是要刪除這些冗余節(jié)點并重新計算路徑的適應(yīng)度函數(shù)。

2.5 精英個體選擇與保留

遺傳算法通過篩選出種群中優(yōu)秀的子集用 于繁衍后代，遺傳算法認為，越是優(yōu)秀的個體繁衍出的后代也會越優(yōu)秀，采用該思想僅使用每次選出的優(yōu)秀個體集來繁衍后代，如此不斷迭代即可令普通種群向精英種群進化，直到到達終止條件。

通過計算適應(yīng)度函數(shù)，選取每代種群中的精英子種群，并選出當代種群中適應(yīng)度函數(shù)值最小的個體Min(Fitness(IP ))，記IP 為最優(yōu)路徑POptimalpath，將其保留下來用于與下代種群中的最精英個體相比較，若后代精英個體的適應(yīng)度函數(shù)小于 Fitness（POptimalpath），則更新最優(yōu)路徑，否則，依然保留上代最精英個體為POptimalpath，如此可以保證將適應(yīng)度函數(shù)最低的個體保留下來。

3 實驗

針對為無人機規(guī)劃路徑，保證無人機的飛行安全是首要目的。由于在環(huán)境感知與飛行控制過程中可能出現(xiàn)的誤差，無人機離障礙物越遠，那么因不可控誤差因素導(dǎo)致的無人機與障礙物發(fā)生碰撞的概率也就越小。由于無人機機載電池的續(xù)航時間有限，要令無人機在短時間內(nèi)到達指定目的地并且完成某種任務(wù)，則應(yīng)使無人機飛行路程盡可能短；另外，為便于飛行控制系統(tǒng)根據(jù)路徑規(guī)劃結(jié)果指導(dǎo)飛行，應(yīng)使路徑盡可能平滑而易于控制。

本文設(shè)計實驗為：在不同的起始點場景，對不同算法的規(guī)劃結(jié)果進行比較：場景1 為起始點全部位于環(huán)境開闊的空曠地帶；場景2 中，起點位于較狹窄區(qū)域，目的是模擬無人機位于中空障礙物內(nèi)部起飛的路徑規(guī)劃情況。

3.1 不同算法規(guī)劃結(jié)果比較

圖6 為起始點都位于障礙物外部的開闊地帶即場景1 的規(guī)劃結(jié)果。圖7 為起點位于左上角障礙物內(nèi)部的狹窄地區(qū)即場景2 的規(guī)劃結(jié)果。圖6、圖7 中的子圖圖6(a)、圖6(b)、圖6(c)、圖7(a)、圖7(b)及圖7(c)分別為使用人工勢場法、快速隨機搜索樹法、A*算法規(guī)劃得出的結(jié)果。

圖6 場景1 中3 種算法規(guī)劃結(jié)果

圖7 場景2 中3 種算法規(guī)劃結(jié)果

3.1.1 人工勢場法

APFM 是常見的路徑規(guī)劃方法，即人工為無人機運動的環(huán)境設(shè)置了1 個包含引力場和斥力場的虛擬力場，通過求合力的方法來控制運動。但若在某些位置的引力與斥力相同，則依靠規(guī)定的虛擬立場無法指導(dǎo)下一步路徑節(jié)點的選擇，也意味著規(guī)劃失敗。圖6(a)與圖7(a)為采用人工勢場法分別為2 種不同的起始點位置規(guī)劃路徑的結(jié)果。實驗表明該方法無論是在場景1 還是在場景2 中都未成功規(guī)劃出從起點到終點的路徑。

3.1.2 基于采樣的路徑規(guī)劃算法

RRT 及其變體是典型的基于隨機采樣的路徑規(guī)劃算法，但隨機算法規(guī)劃出的路徑具有不確定性，不能夠保證成功率，在某些環(huán)境下會陷入死胡同，根本無法到達目的地。圖6(b)與圖7(b)為使用RRT 算法分別為2 種場景規(guī)劃得出的路徑。在2 種場景下，該方法雖然得出了從起點到達目標點的路徑，但既不滿足最短路程也不滿足路徑最優(yōu)，距障礙物較遠且蜿蜒曲折，如若無人機按此路徑飛行既耗時耗能又極度不安全且難以控制。

3.1.3 基于圖搜索的算法

基于圖搜索的算法多種多樣，如深度優(yōu)先搜索，廣度優(yōu)先搜索、迪杰斯特拉算法、A*算法等，其中A*算法中加入了啟發(fā)式策略，是可以得到最短路徑的算法。圖 6(c)與圖7(c)為使用A*算法得出的路徑。但A*算法有極大的局限性，不具備智能性，相同的地圖使用A*算法得出的結(jié)果是唯一的，無法產(chǎn)生更多的路徑來滿足除路程最短之外的其他目的，如本文所需要的離障礙物足夠遠與平滑易控的需求。

3.2 原始遺傳算法

圖8、圖9 為原始遺傳算法的運行結(jié)果。圖8 為場景1 情況下原始遺傳算法的運行結(jié)果。在圖8（a）中圈出的地方明顯有1 段路程距離障礙物極近，若傳感器在探測環(huán)境信息或者飛行控制方面稍有偏差，必將發(fā)生碰撞。在圖8(b)中，路徑明顯過長，未達到飛行路程短的目的。在圖8(c)中，出現(xiàn)了冗余路徑點的情況，本文所加入的刪除操作可以避 免該情況的發(fā)生。

圖8 場景1 中原始遺傳算法規(guī)劃結(jié)果

圖9 為場景2 情況下原始遺傳算法的運行結(jié)果。在圖9(a)中右下角障礙物附近出現(xiàn)路線距離障礙物較近的情況。圖9(b)中在起點附近位置出現(xiàn)了冗余節(jié)點，同時規(guī)劃路程過長，不利于節(jié)約機載電池耗電量。圖9(c)中出現(xiàn)冗余路徑點情況，加入刪除操作就是為了避免此類情況的發(fā)生。

圖9 場景2 中原始遺傳算法規(guī)劃結(jié)果

3.3 本文算法規(guī)劃結(jié)果

圖10、圖11 為本文所用算法的規(guī)劃結(jié)果，分別為場景1 與場景2 中本文所述算法運行的結(jié)果，起始點位置與3.1 節(jié)、3.2 節(jié)所列算法相同的。實驗結(jié)果表明，此方法相較于圖6(a)、圖7(a)所示的人工勢場法，可以完成路徑規(guī)劃的任務(wù)；與圖6(b)、圖7(b)所示的基于隨機采樣的RRT 算法相比，此方法可得出路程更短、離障礙物更遠、更加平滑可控的路徑；與圖6(c)、圖7(c)所示的基于圖搜索的A*算法相比，此方法有更大的靈活性，可按照不同的需求規(guī)劃不同路徑，得出的路徑更加光滑易控；相較于圖10 所示的原始遺傳算法，本方法在保證飛行安全（見圖8(a)、圖9(a)），保證路程較短（見圖8(b)、圖9(b)）與避免冗余節(jié)點(見圖8(c)、圖9(c))等方面有了明顯提高。

圖10 場景1 中本文算法規(guī)劃結(jié)果

圖11 場景2 中本文算法規(guī)劃結(jié)果

4 結(jié)束語

隨著機器人技術(shù)的快速發(fā)展與飛行控制技術(shù)的 日趨成熟，無人機在越來越多的領(lǐng)域中得到了廣泛應(yīng)用，路徑規(guī)劃是無人機自主飛行中的重要組成部分，規(guī)劃出優(yōu)質(zhì)的路徑是保證無人機在執(zhí)行任務(wù)時，可以安全、快速到達終點的關(guān)鍵。因此，本文采用改進遺傳算法的方法，通過規(guī)劃出遠離障礙物的平滑短距路徑來保障無人機能夠快速、安全地到達目的地。在無人機實際自主飛行與應(yīng)用方面還需要考慮諸多因素，需要與環(huán)境感知與飛行控制等多個階段密切配合才能夠?qū)⒃撍惴▽嶋H應(yīng)用于無人機飛行中。