無(wú)人機(jī)路徑跟隨引導(dǎo)率參數(shù)優(yōu)化選擇方法

陳鴻宇，卜方玲，趙筱玥，汪志航

(武漢大學(xué) 電子信息學(xué)院，湖北 武漢 430072)

0 引 言

近年來(lái)，由于輕便、體積小、成本低等優(yōu)點(diǎn)，無(wú)人機(jī)在自然災(zāi)害地區(qū)調(diào)查以及交通和事故監(jiān)測(cè)等領(lǐng)域得到了廣泛的關(guān)注[1,2]。在上述任務(wù)中，無(wú)人機(jī)需要自動(dòng)沿著預(yù)先設(shè)定的軌跡飛行[3]，即實(shí)現(xiàn)路徑跟隨。預(yù)設(shè)的路徑常常使用一系列的路徑點(diǎn)來(lái)描述[4]。為了引導(dǎo)偏離航跡的無(wú)人機(jī)回到預(yù)設(shè)路徑上并沿路徑繼續(xù)飛行，無(wú)人機(jī)需要不斷改變其航向，航向改變所依據(jù)的規(guī)則被稱為引導(dǎo)率。研究者們?cè)O(shè)計(jì)了一系列的引導(dǎo)率來(lái)實(shí)現(xiàn)路徑跟隨，Kothari等結(jié)合了純追蹤法和視線法，提出了PLOS(pure pursuit and line-of-sight)引導(dǎo)率[5]。Sujit等總結(jié)了包括carrot chase和NLGL在內(nèi)的5種引導(dǎo)率[6]，通過(guò)仿真研究了不同參數(shù)選擇下路徑跟隨的結(jié)果，并制定了相應(yīng)的評(píng)價(jià)指標(biāo)，對(duì)引導(dǎo)結(jié)果進(jìn)行評(píng)價(jià)。通過(guò)Sujit的工作，可以看出參數(shù)的選擇對(duì)引導(dǎo)結(jié)果影響很大，但是上述方法中均缺乏參數(shù)選擇的策略。為此，本文選取了廣泛應(yīng)用于實(shí)際飛行中的carrot chase和路徑跟隨結(jié)果非常優(yōu)秀的NLGL引導(dǎo)率，用于無(wú)人機(jī)路徑跟隨算法中引導(dǎo)率最優(yōu)參數(shù)選擇問(wèn)題的研究。

選擇一個(gè)引導(dǎo)率參數(shù)，使得在相應(yīng)的指標(biāo)下路徑跟隨的結(jié)果達(dá)到最優(yōu)，是一個(gè)優(yōu)化問(wèn)題。優(yōu)化問(wèn)題有兩類解決辦法，一種是精確算法，另一種是啟發(fā)式算法[7]。精確算法在全局搜索域內(nèi)尋找最優(yōu)解，效率低，只適用于小規(guī)模問(wèn)題。啟發(fā)式算法易于實(shí)現(xiàn)，不需要梯度信息，能避免局部最優(yōu)，被廣泛應(yīng)用于求解優(yōu)化問(wèn)題[8]。常用的啟發(fā)式算法有模擬退火、遺傳算法、群體優(yōu)化算法等。模擬退火算法是一種隨機(jī)算法，非常易于實(shí)現(xiàn)[9]，然而為了接近全局最優(yōu)解，最終溫度會(huì)被設(shè)置為極低的值，使其收斂速度下降，也有可能陷在局部最優(yōu)解中[10]。群體優(yōu)化算法如粒子群算法，對(duì)初始種群的選擇具有很好的魯棒性，但是在接近全局最優(yōu)解時(shí)，搜索速度會(huì)下降且容易陷于局部最優(yōu)解中[11]。遺傳算法因其具有全局搜索的特性往往能取得出色的結(jié)果[12]，且搜索從群體出發(fā)，具有潛在的并行性和魯棒性。為了得到魯棒性的全局最優(yōu)解，本文選擇遺傳算法作為該問(wèn)題的解決方法。

針對(duì)路徑跟隨算法中引導(dǎo)率參數(shù)選擇非優(yōu)化的問(wèn)題，本文提出了一種基于遺傳算的引導(dǎo)率參數(shù)優(yōu)化選擇方法，構(gòu)建無(wú)人機(jī)自適應(yīng)路徑跟隨系統(tǒng)，實(shí)現(xiàn)了無(wú)人機(jī)自適應(yīng)路徑跟隨。首先根據(jù)路徑的信息、無(wú)人機(jī)的初始姿態(tài)以及引導(dǎo)率，用遺傳算法計(jì)算出代價(jià)最小的引導(dǎo)率參數(shù)，然后再將該參數(shù)引入相應(yīng)引導(dǎo)率中，引導(dǎo)無(wú)人機(jī)進(jìn)行朝著預(yù)設(shè)的路徑飛行。

1 無(wú)人機(jī)路徑跟隨算法

1.1 路徑跟隨問(wèn)題描述

無(wú)人機(jī)路徑跟隨的目的是為了讓無(wú)人機(jī)能夠準(zhǔn)確地、自動(dòng)地沿著預(yù)設(shè)的路徑飛行[13]。用數(shù)學(xué)模型[6]來(lái)描述這一問(wèn)題，如圖1所示，給定一條路徑(由路徑點(diǎn)Wi和Wi+1來(lái)描述)，無(wú)人機(jī)的初始位置P、速度v和航向角，路徑跟隨算法的目標(biāo)是，在無(wú)人機(jī)飛行過(guò)程中，最小化無(wú)人機(jī)到路徑的距離d，以及航向角與路徑角度θ之間的差值，即隨著時(shí)間t增大，使d→0，|-θ|→0。通過(guò)路徑跟隨算法中的引導(dǎo)率，計(jì)算出無(wú)人機(jī)下一時(shí)間點(diǎn)所要求的航向角，從而不斷調(diào)整無(wú)人機(jī)航向角，使其在任務(wù)進(jìn)行過(guò)程中能夠精確回到預(yù)設(shè)路徑上，并沿著路徑飛行。

圖1 路徑跟隨

1.2 路徑跟隨算法

本文聚焦于無(wú)人機(jī)在二維空間的路徑跟隨，因此算法只需要控制無(wú)人機(jī)的航向角，同時(shí)將無(wú)人機(jī)速度方向控制為機(jī)頭方向。路徑跟隨算法中引導(dǎo)率的輸出往往為徑向加速度控制量u，可根據(jù)如下式(1)計(jì)算得到航向角

(1)

路徑跟隨算法中的核心規(guī)則為引導(dǎo)率，它根據(jù)輸入的預(yù)設(shè)路徑和無(wú)人機(jī)位置、姿態(tài)信息，生成相應(yīng)的控制指令，即航向角的控制信息。本文選取了廣泛應(yīng)用于實(shí)際飛行的carrot chase以及路徑跟隨效果非常優(yōu)秀NLGL引導(dǎo)率，在以下兩小節(jié)中進(jìn)行簡(jiǎn)單的介紹。

1.2.1 Carrot chase引導(dǎo)率

Carrot chase是路徑跟隨算法中最為廣泛應(yīng)用的引導(dǎo)率之一，該算法通過(guò)在路徑上設(shè)置虛擬目標(biāo)，使無(wú)人機(jī)不斷追逐虛擬目標(biāo)，從而實(shí)現(xiàn)路徑跟隨。虛擬目標(biāo)的設(shè)置如圖2所示，根據(jù)無(wú)人機(jī)的位置和預(yù)設(shè)路徑，計(jì)算出無(wú)人機(jī)在路徑上的投影P′，再將其加上一個(gè)參數(shù)δ，得到虛擬目標(biāo)S。

圖2 Carrot chase引導(dǎo)率虛擬點(diǎn)設(shè)置

引導(dǎo)率的輸出，即徑向加速度控制量u的定義為

u=k(d-)

(2)

其中，ψd為無(wú)人機(jī)位置P和虛擬目標(biāo)S連線與水平軸之間的夾角，k為控制增益，參照文獻(xiàn)[6]取k=0.5。改變參數(shù)δ的值，虛擬目標(biāo)的位置會(huì)產(chǎn)生變化，路徑跟隨結(jié)果也會(huì)有所不同。

1.2.2 NLGL引導(dǎo)率

NLGL引導(dǎo)率也用到了虛擬目標(biāo)的概念，如圖3所示，以無(wú)人機(jī)位置為圓心，參數(shù)L為半徑畫(huà)圓，與路徑相交于S和S′，取前進(jìn)方向與該圓的交點(diǎn)為虛擬目標(biāo)。

圖3 NILG引導(dǎo)率虛擬點(diǎn)設(shè)置

引導(dǎo)率輸出定義為

u=2v2sin(η)/L

(3)

其中，v為無(wú)人機(jī)速度，η為虛擬目標(biāo)和位置連線與無(wú)人機(jī)飛行方向之間的夾角。若L值小于無(wú)人機(jī)到路徑的距離，則將u確定為一個(gè)固定值，直到無(wú)人機(jī)到路徑距離小于L的值。改變參數(shù)L的值，將改變虛擬目標(biāo)的位置，同時(shí)由式(3)可以看出，會(huì)對(duì)控制量u產(chǎn)生影響，從而改變路徑跟隨的結(jié)果。由此可見(jiàn)，引導(dǎo)率參數(shù)的選擇對(duì)路徑跟隨算法至關(guān)重要。

2 自適應(yīng)路徑跟隨方法

2.1 基于遺傳算法的引導(dǎo)率參數(shù)計(jì)算方法

遺傳算法是一種基于生物遺傳過(guò)程的自然優(yōu)化機(jī)制的啟發(fā)式算法[14]，算法受到達(dá)爾文進(jìn)化生物學(xué)的啟發(fā)，使用遺傳、選擇、變異和交叉技術(shù)來(lái)尋找最優(yōu)解[15]。遺傳算法首先隨機(jī)初始化一個(gè)種群，然后對(duì)種群進(jìn)行選擇、變異和交叉操作，以產(chǎn)生下一代種群，直到滿足終止條件(通常為確定一個(gè)迭代次數(shù))，輸出最優(yōu)個(gè)體。迭代過(guò)程中，使用適應(yīng)度函數(shù)來(lái)評(píng)價(jià)個(gè)體，適應(yīng)度函數(shù)得分越高，產(chǎn)生的個(gè)體越接近最優(yōu)解。

將遺傳算法用于引導(dǎo)率的參數(shù)選擇，以carrot chase引導(dǎo)率為例，將參數(shù)δ作為遺傳算法中的個(gè)體，用十進(jìn)制的方式進(jìn)行參數(shù)編碼，隨機(jī)生成一個(gè)初始種群。選擇操作基于輪盤賭的思想，適應(yīng)度值越大，該個(gè)體被選擇為父代的概率越大。被選擇為父代的個(gè)體將進(jìn)行之后的交叉、變異以及精英選擇操作。交叉則是隨機(jī)選取兩個(gè)父代個(gè)體，進(jìn)行基因重組，對(duì)于十進(jìn)制編碼的個(gè)體，即以一個(gè)隨機(jī)的權(quán)重，求得兩個(gè)父代的加權(quán)和作為下一代個(gè)體。變異保證了遺傳的多樣性，對(duì)于十進(jìn)制編碼的個(gè)體，隨機(jī)選取一個(gè)父代，使它的值隨機(jī)上下浮動(dòng)，產(chǎn)生的新個(gè)體作為下一代子代。為了使得遺傳算法能更快的收斂，將會(huì)在每次迭代中選擇最佳的個(gè)體作為精英個(gè)體，該精英個(gè)體直接保留到下一代中，該操作稱之為精英選擇。為了完成上述操作，最為關(guān)鍵的是定義一個(gè)適應(yīng)度函數(shù)。我們定義了路徑跟隨評(píng)價(jià)指標(biāo)作為適應(yīng)度函數(shù)，以對(duì)結(jié)果進(jìn)行定量的比較。

常用的路徑跟隨評(píng)價(jià)指標(biāo)有航跡誤差和控制效益。航跡誤差是指無(wú)人機(jī)到路徑的距離d，而控制效益則為式(2)、式(3)中的控制量u。綜合考慮航跡誤差和控制效益，參考文獻(xiàn)[6]，我們定義評(píng)價(jià)指標(biāo)ξ為

(4)

(5)

(6)

Γ為權(quán)重，下文中選擇為0.5。無(wú)人機(jī)是否回到路徑上的判斷準(zhǔn)則為：若最后時(shí)刻無(wú)人機(jī)位置距離路徑不超過(guò)1 m且此時(shí)航向角與路徑方向差值不超過(guò)0.1弧度，則判定為回到路徑上，否則，判定為未回到路徑上。

由于路徑跟隨指標(biāo)ξ越小說(shuō)明路徑跟隨結(jié)果越佳，而遺傳算法會(huì)選擇適應(yīng)度更高的個(gè)體，因此我們將適應(yīng)度函數(shù)定義為

(7)

不同的參數(shù)δ將會(huì)計(jì)算出一條不同的路徑，其航跡誤差和控制效益也會(huì)有所差異，根據(jù)式(7)中的適應(yīng)度函數(shù)，將會(huì)產(chǎn)生不同適應(yīng)度值，在進(jìn)行一定代數(shù)的選擇、交叉、變異及精英選擇操作后，輸出一個(gè)最優(yōu)的個(gè)體δ。整個(gè)過(guò)程見(jiàn)表1。

表1 總體框架

NLGL引導(dǎo)率的參數(shù)選擇也與上述方法類似，只是將遺傳算法的個(gè)體修改為參數(shù)L，其它步驟類似。

2.2 基于遺傳算法的路徑跟隨

針對(duì)路徑跟隨算法中忽略引導(dǎo)率參數(shù)選擇的問(wèn)題，提出了一種基于遺傳算法的引導(dǎo)率參數(shù)優(yōu)化選擇方法。方法流程如圖4所示，首先將初始信息，包括路徑點(diǎn)坐標(biāo)，無(wú)人機(jī)飛行速度和方向以及選用的引導(dǎo)率傳入遺傳算法中，遺傳算法根據(jù)適應(yīng)度函數(shù)計(jì)算出一個(gè)最優(yōu)的引導(dǎo)率參數(shù)，然后每隔一段時(shí)間獲取無(wú)人機(jī)實(shí)時(shí)的位置信息和姿態(tài)信息(主要是航向角)，輸入到引導(dǎo)率中，計(jì)算出無(wú)人機(jī)應(yīng)該調(diào)整的角度，控制無(wú)人機(jī)飛行，從而實(shí)現(xiàn)路徑跟隨。

圖4 無(wú)人機(jī)路徑跟隨流程

3 實(shí)驗(yàn)結(jié)果與分析

3.1 實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)分2步進(jìn)行，第一步計(jì)算路徑跟隨引導(dǎo)率最優(yōu)參數(shù)，第二步，根據(jù)計(jì)算出的引導(dǎo)率和無(wú)人機(jī)路徑跟隨流程，編制軟件，在大疆無(wú)人機(jī)上實(shí)現(xiàn)路徑跟隨。遺傳算法計(jì)算最優(yōu)參數(shù)基于Matlab實(shí)現(xiàn)，計(jì)算環(huán)境是在處理器為i5-4460，CPU為3.2 GHz的臺(tái)式電腦和64位Windows系統(tǒng)。計(jì)算得到了最優(yōu)參數(shù)后，實(shí)際的飛行測(cè)試是在大疆M100上實(shí)現(xiàn)的，大疆M100上搭載了Manifold開(kāi)發(fā)板，作為路徑跟隨算法的運(yùn)行軟硬件平臺(tái)，無(wú)人機(jī)的定位使用的是GPS。整個(gè)無(wú)人機(jī)平臺(tái)如圖5所示。

圖5 無(wú)人機(jī)平臺(tái)

3.2 實(shí)驗(yàn)結(jié)果與分析

本文選取了武漢大學(xué)學(xué)校操場(chǎng)的一條直線路徑，如圖6所示，用兩個(gè)路徑點(diǎn)來(lái)描述，以第一個(gè)路徑點(diǎn)W1作為坐標(biāo)原點(diǎn)，正北方向?yàn)閅軸正方向，正東方向?yàn)閄軸正方向建立坐標(biāo)系，則W1=(0,0)，第二個(gè)路徑點(diǎn)W2=(-30.0,192.1)，無(wú)人機(jī)的初始坐標(biāo)為Puav=(18.9,14.3)，坐標(biāo)點(diǎn)的單位為m，無(wú)人機(jī)速度設(shè)定為3 m/s，初始航向?yàn)?°，即朝著正北方向。路徑跟隨算法的控制周期會(huì)影響路徑跟隨的結(jié)果，周期設(shè)置短，會(huì)增大控制效益；周期設(shè)置過(guò)長(zhǎng)，會(huì)使跟隨結(jié)果呈折線狀態(tài)，增加航跡誤差。同時(shí)，控制周期的選取也要考慮無(wú)人機(jī)的速度，若無(wú)人機(jī)速度小，無(wú)人機(jī)位置變化慢，此時(shí)根據(jù)路徑跟隨算法計(jì)算所得的控制量u變化也就不大，過(guò)小的控制周期會(huì)導(dǎo)致計(jì)算資源浪費(fèi)；相反的，速度較大時(shí)由于位置變化快則需要較短的控制周期。綜合考慮兩者影響，本文選取控制周期為1 s。然后分別在carrot chase和NIGL引導(dǎo)率下進(jìn)行實(shí)驗(yàn)驗(yàn)證，結(jié)果如圖7-圖10所示。

圖6 實(shí)驗(yàn)路徑及無(wú)人機(jī)初始位置

圖7 適應(yīng)度變化曲線

圖8 不同δ下無(wú)人機(jī)路徑跟隨結(jié)果

表2 不同δ下路徑跟隨結(jié)果評(píng)價(jià)

3.2.1 Carrot chase引導(dǎo)率

首先進(jìn)行仿真確定最優(yōu)的δ參數(shù)，迭代次數(shù)設(shè)置為100，遺傳算法的適應(yīng)度變化曲線如圖7所示，算法到達(dá)40代左右時(shí)收斂，輸出的最優(yōu)參數(shù)為11.45，執(zhí)行時(shí)間約為1 min。

然后分別選取δ為5，11.45，20和30進(jìn)行實(shí)際飛行驗(yàn)證，飛行的軌跡如圖8所示。

用式(4)中的指標(biāo)ξ，對(duì)上述4條路徑進(jìn)行計(jì)算，結(jié)果見(jiàn)表2。

其中，δ為20和30時(shí)指標(biāo)為1，這是因?yàn)榇藭r(shí)無(wú)人機(jī)并未回到路徑上，仍在路徑上左右震蕩。從表2中可以看出，基于遺傳算法選擇的最優(yōu)參數(shù)，路徑跟隨的結(jié)果能夠達(dá)到代價(jià)最小。

3.2.2 NLGL引導(dǎo)率

首先進(jìn)行仿真確定最優(yōu)的L參數(shù)，迭代次數(shù)設(shè)置為100，遺傳算法的適應(yīng)度變化曲線如圖9所示，算法在20代左右收斂，輸出的最優(yōu)參數(shù)為18.44，執(zhí)行時(shí)間約為1 min。

圖9 適應(yīng)度變化曲線

圖10 不同L下無(wú)人機(jī)路徑跟隨結(jié)果

然后分別選取L為10，18.44，30，40進(jìn)行實(shí)際飛行驗(yàn)證，飛行結(jié)果如圖10所示。

用式(4)中的指標(biāo)ξ，對(duì)上述4條路徑進(jìn)行計(jì)算，結(jié)果見(jiàn)表3。

從表3中可以看出，基于遺傳算法選擇的最優(yōu)參數(shù)，路徑跟隨的結(jié)果能夠達(dá)到代價(jià)最小。

表3 不同L下路徑跟隨結(jié)果評(píng)價(jià)

上述兩個(gè)實(shí)驗(yàn)結(jié)果表明，在Carrot chase和NLGL(nonlinear guidance law)這兩種引導(dǎo)率下，遺傳算法均能夠選擇出一個(gè)最優(yōu)參數(shù)；使用該最優(yōu)參數(shù)能夠使得評(píng)價(jià)指標(biāo)ξ值最小，即路徑跟隨的代價(jià)最小。

4 結(jié)束語(yǔ)

本文針對(duì)傳統(tǒng)路徑跟隨算法缺引導(dǎo)率參數(shù)優(yōu)化選擇的問(wèn)題，基于遺傳算法提出了一種引導(dǎo)率參數(shù)優(yōu)化選擇方法，構(gòu)建了無(wú)人機(jī)自適應(yīng)路徑跟隨系統(tǒng)。給定路徑信息以及無(wú)人機(jī)初始位姿信息，能夠計(jì)算出一個(gè)最優(yōu)的引導(dǎo)率參數(shù)，用于引導(dǎo)無(wú)人機(jī)回到預(yù)設(shè)路徑上。在大疆M100無(wú)人機(jī)平臺(tái)上進(jìn)行了實(shí)際飛行測(cè)試，實(shí)驗(yàn)結(jié)果表明，在carrot chase和NLGL兩種引導(dǎo)率下，遺傳算法選擇的參數(shù)是最優(yōu)的；且基于該參數(shù)實(shí)現(xiàn)的路徑跟隨代價(jià)最小。