考慮最短轉(zhuǎn)彎半徑的無(wú)人機(jī)區(qū)域覆蓋航跡規(guī)劃

宋志強(qiáng)， 陳少博

（1.無(wú)錫學(xué)院自動(dòng)化學(xué)院，江蘇 無(wú)錫 214105；2.南京信息工程大學(xué)濱江學(xué)院自動(dòng)化學(xué)院，江蘇 無(wú)錫 214105）

0 引言

無(wú)人機(jī)應(yīng)用越來(lái)越廣泛，將無(wú)人機(jī)應(yīng)用于測(cè)繪［1］、環(huán)境監(jiān)測(cè)［2］、搜救［3］、精確農(nóng)業(yè)［4］等，得益于無(wú)人機(jī)機(jī)載傳感器獲取地面信息并實(shí)現(xiàn)區(qū)域覆蓋。此類應(yīng)用中，高效的無(wú)人機(jī)航跡規(guī)劃算法非常重要，從圖像提取信息的時(shí)間、成本和質(zhì)量直接關(guān)系到規(guī)劃質(zhì)量。在各種應(yīng)用中，二維覆蓋尤為成功。覆蓋任務(wù)分兩步驟進(jìn)行，一是準(zhǔn)備階段，選擇運(yùn)載工具、相機(jī)配置和任務(wù)規(guī)劃，二是執(zhí)行階段，無(wú)人機(jī)自主飛行并收集數(shù)據(jù)。要實(shí)現(xiàn)任務(wù)的完全自動(dòng)化，就必須解決任務(wù)規(guī)劃問(wèn)題，其核心是覆蓋航跡規(guī)劃，定義為計(jì)算無(wú)人機(jī)航跡的任務(wù)，使得感興趣區(qū)域（Region of Interest，ROI）的所有點(diǎn)都能被監(jiān)測(cè)到［5］?，F(xiàn)有的規(guī)劃器沒(méi)有考慮連接路徑，即無(wú)人機(jī)起飛點(diǎn)到ROI 的路徑以及從ROI 到無(wú)人機(jī)著陸點(diǎn)的航跡，不考慮連接路徑通常并不是無(wú)人機(jī)執(zhí)行任務(wù)的完整路徑，且當(dāng)任務(wù)由多個(gè)ROI 和多個(gè)連接路徑組成時(shí)，路徑長(zhǎng)度增量更大。因此，有必要規(guī)劃一條考慮任務(wù)起點(diǎn)和終點(diǎn)的路徑。此外，固定翼無(wú)人機(jī)的最小轉(zhuǎn)彎半徑也是需要考慮的因素。

直線飛行形成來(lái)回航跡（Back and Forth Path，BFP）需要計(jì)算路線的方向，以往的研究?jī)H在舍棄起點(diǎn)和終點(diǎn)的航線上定義路徑最優(yōu)性。一般來(lái)說(shuō)，如果想保持BFP作為覆蓋模式，必須結(jié)合起點(diǎn)、終點(diǎn)提供最小成本的BFP 執(zhí)行搜索。一個(gè)簡(jiǎn)單的策略是循環(huán)BFP直到出現(xiàn)最小值，但這不是有效策略。本文提出一種改進(jìn)的旋轉(zhuǎn)卡尺路徑規(guī)劃算法，考慮無(wú)人機(jī)的起飛點(diǎn)和著陸點(diǎn)，并考慮固定翼無(wú)人機(jī)的最小轉(zhuǎn)彎半徑，使算法更具實(shí)用性。

Huang［6］提出一種移動(dòng)機(jī)器人排雷作業(yè)的覆蓋方法，使用無(wú)人機(jī)覆蓋航跡規(guī)劃大多遵循Huang 定義的最優(yōu)準(zhǔn)則，即航線數(shù)量最少的路徑為最優(yōu)路徑。Huang提出機(jī)器人來(lái)回運(yùn)動(dòng)的掃描方向必須垂直于感興趣區(qū)域多邊形的最小寬度，方法沒(méi)有考慮從起點(diǎn)到感興趣區(qū)域的距離。一些文獻(xiàn)研究非凸區(qū)域的覆蓋問(wèn)題，涉及凹多邊形凸分解［7］，Li 等［8］將非凸ROI 分解為凸單元，通過(guò)設(shè)置垂直于單元最小寬度的飛行線來(lái)計(jì)算路徑的最優(yōu)方向。王自亮等［9］采用對(duì)凹多形區(qū)域直接遍歷的方法，實(shí)現(xiàn)對(duì)區(qū)域的覆蓋，但凹多邊形區(qū)域存在狹長(zhǎng)區(qū)域時(shí)，容易產(chǎn)生多個(gè)轉(zhuǎn)彎路徑。王紅星等［10］針對(duì)凹多邊形區(qū)域進(jìn)行凸分解，研究區(qū)域覆蓋算法，使無(wú)人機(jī)對(duì)指定區(qū)域進(jìn)行覆蓋搜索。劉旭林等［11］選取凹多邊形及其凸包的邊所在的方向分別為主方向，得到全局最優(yōu)解。Coombes 等［12］考慮風(fēng)速對(duì)無(wú)人機(jī)航跡的影響，旨在減少固定翼飛機(jī)測(cè)量飛行時(shí)間。多無(wú)人機(jī)協(xié)同搜索［13-15］可提高任務(wù)的完成速度，一般的方法是將其分為區(qū)域劃分和區(qū)域覆蓋搜索路徑規(guī)劃兩個(gè)子問(wèn)題處理。Fevgas 等［16］通過(guò)無(wú)人機(jī)的合作策略以實(shí)現(xiàn)節(jié)能的覆蓋航跡規(guī)劃。對(duì)無(wú)人機(jī)區(qū)域覆蓋中的轉(zhuǎn)彎控制［17-19］，也是值得研究的問(wèn)題。文獻(xiàn)［20］中提出一種包含無(wú)人機(jī)飛行起點(diǎn)和終點(diǎn)的邊-點(diǎn)來(lái)回掃描覆蓋航跡規(guī)劃，但主要針對(duì)凸多邊形區(qū)域，且沒(méi)有考慮固定翼無(wú)人機(jī)的最小轉(zhuǎn)彎半徑。

綜上，以前的方法很少考慮無(wú)人機(jī)起飛點(diǎn)到達(dá)ROI的連接路徑，考慮連接路徑和ROI凹凸性以及固定翼無(wú)人機(jī)最小轉(zhuǎn)彎半徑的覆蓋航跡規(guī)劃，更具實(shí)用價(jià)值。

1 覆蓋問(wèn)題描述

1.1 感興趣區(qū)域

假設(shè)ROI 為二維平面圖形Q=｛V，E｝，其中V=｛1，2，…，n｝為頂點(diǎn)集，E=｛（1，2），…，（n-1，n），（n，1）｝為邊集。

對(duì)于凸多邊形區(qū)域，第一步是尋找覆蓋最優(yōu)方向，其垂直于多邊型區(qū)域的最小高度，在這一方向上，區(qū)域可被最少行覆蓋，即無(wú)人機(jī)的轉(zhuǎn)彎次數(shù)也最少，完全覆蓋航跡如圖1 所示。轉(zhuǎn)彎次數(shù)和覆蓋指定區(qū)域的時(shí)間直接相關(guān)，具有最少轉(zhuǎn)彎次數(shù)的航線在航線長(zhǎng)度、無(wú)人機(jī)續(xù)航時(shí)間、能量消耗等方面更優(yōu)。

圖1 完全覆蓋航跡示例

1.2 航線方向設(shè)置

假設(shè)無(wú)人機(jī)將飛越要覆蓋的區(qū)域，并在垂直于給定掃描方向的行中來(lái)回運(yùn)動(dòng)。為找到給定多邊形的最佳覆蓋方向，可使多邊形在平面內(nèi)繞水平軸旋轉(zhuǎn)，并測(cè)量其高度。最佳方向是產(chǎn)生最小高度hmin的方向。一旦找到最佳掃描方向，就可在該區(qū)域規(guī)劃航跡。假設(shè)圖像傳感器平行于地平面，已知圖像傳感器的寬度l，相機(jī)鏡頭的焦距為f，均以mm 為單位，相機(jī)到地面的距離H（飛行高度），單位為m，則相機(jī)在地面上的覆蓋寬度（m）

覆蓋行數(shù)

式中，α∈（0，1）為兩幅圖像之間的重疊比例，這種重疊通常是連接圖像以組成航空地圖所必需的。

飛行線之間的距離

對(duì)于固定翼無(wú)人機(jī)，還須考慮其最小轉(zhuǎn)彎半徑

式中：v為無(wú)人機(jī)飛行速度；g為重力加速度；φmax為無(wú)人機(jī)最大滾轉(zhuǎn)角。

1.3 邊-點(diǎn)路徑構(gòu)建

為構(gòu)建邊-點(diǎn)路徑，通過(guò)迭代將線Lflight與多邊形相交將航點(diǎn)添加到航跡中（見(jiàn)算法1）。該算法需要輸入多邊形、初始頂點(diǎn)b，相鄰頂點(diǎn)bmate，頂點(diǎn)b的對(duì)映頂點(diǎn)a以及飛行線之間的距離dx。對(duì)于固定翼無(wú)人機(jī)，取dx=2Rmin。首先，創(chuàng)建一條Lflight，平行于邊（b，bmate），其垂直于掃描方向且位移一個(gè)偏移量Δinit=d/2（見(jiàn)圖2）。Lflight與多邊形相交，得到點(diǎn)ip1和ip2，通過(guò)CheckAndConnect函數(shù)建立垂直約束，并按照正確的順序連接起來(lái)以形成無(wú)人機(jī)的航線，對(duì)于固定翼無(wú)人機(jī)來(lái)說(shuō)，這有利于杜賓曲線的構(gòu)建。不斷將直線向a方向移動(dòng)并與多邊形相交，最后算法返回航跡ρ=｛p0，…，pm｝。構(gòu)造的邊-點(diǎn)路徑從b開(kāi)始到bmate，并掃向a。如果航跡包含起飛點(diǎn)和著陸點(diǎn)，則可表示為：τ=｛ps，p0，…，pm，pe｝。

算法1獲取多邊形的邊-頂點(diǎn)路徑（GetPath（b，bmate，a））。該算法計(jì)算多邊形Q=｛V，E｝的來(lái)回路徑（ρ=｛p0，p1，…，pm｝）。路徑從初始頂點(diǎn)b開(kāi)始，指向bmate，然后掃向a。

輸入：Q，d，b，bmate，a

輸出：ρ

1. Δinit= d/2；

2. Lflight←GenerateLine（b，bmate）；

3. Lflight←OffSet（Lflight，Δinit）；

4. ρ ←?；

5. while Intersects（C（Lflight），Q）do

6. ip1，ip2←IntersectEdges（Lflight，E）；

7. ρ ←CheckAndConnect（ρ，ip1，ip2）；

8. Lflight←OffSet（Lflight，d）；

9. return ρ；

2 改進(jìn)的旋轉(zhuǎn)卡尺航跡規(guī)劃

2.1 算法簡(jiǎn)介

改進(jìn)的旋轉(zhuǎn)卡尺航跡規(guī)劃總體流程如圖3 所示，該算法對(duì)固定翼無(wú)人機(jī)和多旋翼無(wú)人機(jī)均適用，路徑規(guī)劃開(kāi)始時(shí)，輸入RoI的頂點(diǎn)坐標(biāo)，算法根據(jù)RoI是否為凸多邊形而作相應(yīng)處理，如果是凸多邊形，則對(duì)凸多邊形作旋轉(zhuǎn)卡尺航跡規(guī)劃。如果是凹多邊形則先將RoI分割成最少個(gè)數(shù)的凸多邊形［21］，再對(duì)子區(qū)域做旋轉(zhuǎn)卡尺航跡規(guī)劃。在得到航點(diǎn)后，如果無(wú)人機(jī)是固定翼的，則標(biāo)記相應(yīng)轉(zhuǎn)彎航點(diǎn)，以便將算法應(yīng)用于實(shí)際時(shí)，對(duì)轉(zhuǎn)彎航跡進(jìn)行離散化處理后上傳至飛控。

圖3 算法總體流程圖

2.2 航跡規(guī)劃

2.2.1 對(duì)映點(diǎn)對(duì)計(jì)算

考慮任務(wù)起止點(diǎn)，采用旋轉(zhuǎn)卡尺航跡規(guī)劃［20］來(lái)得到覆蓋RoI的航跡。建立覆蓋RoI的來(lái)回搜索模式，以找到結(jié)合起點(diǎn)和終點(diǎn)的最少路徑。如果不考慮任務(wù)的起點(diǎn)和終點(diǎn)，那么最優(yōu)路徑是飛行線垂直于最小寬度的路徑。一般來(lái)說(shuō)，為解決這個(gè)問(wèn)題，必須測(cè)試具有起點(diǎn)和終點(diǎn)的來(lái)回路徑的所有可能組合。發(fā)現(xiàn)所有來(lái)回路徑的覆蓋都始于和終于對(duì)映點(diǎn)對(duì)，對(duì)映點(diǎn)對(duì)的數(shù)量被限制在x=（3/2）·n。若已知一組給定對(duì)映點(diǎn)對(duì)的最優(yōu)路徑，則與起點(diǎn)和終點(diǎn)的組合也能計(jì)算出來(lái)。算法2 為旋轉(zhuǎn)卡尺航跡規(guī)劃，第1 行ComputeAntipodalPairs函數(shù)用于獲得對(duì)映點(diǎn)對(duì)集，輸入為頂點(diǎn)集V，返回凸多邊形的對(duì)映點(diǎn)對(duì)集，即A=｛（i1，j1），（i2，j2），…，（in，jn）｝，在O（n）時(shí)間內(nèi)計(jì)算所有對(duì)映點(diǎn)對(duì)，其中n是多邊形的頂點(diǎn)數(shù)。對(duì)于每組對(duì)映點(diǎn)對(duì)（第3 行），計(jì)算最優(yōu)路徑（第4 行），并將其附加到起點(diǎn)和終點(diǎn)（第5 行）。BestPath（）函數(shù)將在2.2.2 節(jié)中解釋。由于路徑的順序很重要，按照正序和逆序進(jìn)行測(cè)試，并選擇距離最小的路徑（第5 行）。如果當(dāng)前路徑的代價(jià)更小，則將其保留為最優(yōu)完整航跡（τ）。最后，一旦測(cè)試完所有的對(duì)映點(diǎn)對(duì)，返回τ。

算法2旋轉(zhuǎn)卡尺航跡規(guī)劃。得到包含起點(diǎn)ps，邊-點(diǎn)路徑ρ和終點(diǎn)pe的完全航跡τ。

輸入：V，ps，pe

輸出：τ

1. A ←ComputeAntipodalPairs（V）；

2. c*←∞

3. foreach（i，j）∈A do

4. ρ ←BestPath（V，i，j）；

5. τaux←minCost（｛ps，ρ，pe｝，｛pe，ρ，ps｝）；

6. if cost（τaux）＜c*then

7. τ ←τaux；

8. c*←cost（τ）；

9. return τ；

2.2.2 計(jì)算每組對(duì)映點(diǎn)對(duì)的最優(yōu)航跡

計(jì)算對(duì)映點(diǎn)對(duì)i和j的最優(yōu)來(lái)回航跡，參見(jiàn)算法3。其主要思想是找到通過(guò)頂點(diǎn)i和j的一組平行線，它們之間的距離最小，然后得到與這些線匹配的來(lái)回航跡。該過(guò)程給出一組對(duì)映點(diǎn)對(duì)和一個(gè)接觸該對(duì)的卡尺，順時(shí)針旋轉(zhuǎn)卡尺，直到和邊接觸并測(cè)量多邊形的高度，然后逆時(shí)針旋轉(zhuǎn)卡尺，直到第2 條邊接觸并測(cè)量高度，最后，比較2 個(gè)高度以找到最小高度。假設(shè)n個(gè)頂點(diǎn)從0 到n-1 標(biāo)號(hào)，angle（m，n）函數(shù)用于計(jì)算一條直線從平行于邊（m，m+1）的位置順時(shí)針旋轉(zhuǎn)到平行于邊（n，n+1）的位置時(shí)所掠出的角度。

算法3BestPath（V，i，j）函數(shù)計(jì)算對(duì)映點(diǎn)對(duì)的最優(yōu)航跡ρ，輸入為邊集V和對(duì)映點(diǎn)對(duì)（i，j）。

輸入：V=｛1，2，…，n｝，（i，j）

輸出：ρ

/*順時(shí)針旋轉(zhuǎn)卡尺*/

1.if angle（i，j）-π ＜0 then

2. b ←j；

3. a ←i；

4. else

5. b ←i；

6. a ←j；

/*逆時(shí)針旋轉(zhuǎn)卡尺*/

7. φ←angle（b，a）-π；

8. γb←angle（b-1，b）；

9. γa←angle（a-1，a）-φ；

10. if γb＜γathen

11. b2←b-1；

12. a2←a；

13. else

14. b2←a-1；

15. a2←b；

/*尋找航線最少的路徑*/

16. if dist（b，a）＜dist（b2，a2）then

17. ρ ←GetPath（b，b+1，a）；

18. else

19. ρ ←GetPath（b2+1，b2，a2）；

首先，順時(shí)針旋轉(zhuǎn)卡尺，找到接觸的第一條邊，以（b，b+1）表示并且它將是一條可能的BFP 的基邊。以圖4 為例，如果在對(duì)映點(diǎn)i和j上旋轉(zhuǎn)直線L和M，第一條接觸邊將是（j，j+1），其與M′相重合。找到邊（b，b+1）的一種方法是找到αi和αj兩者的較小角度，即從假想的支撐線順時(shí)針?lè)较蛐D(zhuǎn)到多邊形最近邊的角度。通過(guò)測(cè)量從線L′到線M′旋轉(zhuǎn)的差值而非測(cè)量αi和αj確定基邊，參見(jiàn)算法3 的第1 ～6 行。如果角度減去π的差小于零，則邊（j，j+1）被視為航跡的基邊。否則，將邊（i，i+1）作為基邊。一種特殊情況是兩個(gè)角度相等，在這種情況下，航線的數(shù)量是相同的，就航線數(shù)量而言，選擇哪個(gè)基邊并不重要。為使算法簡(jiǎn)單，在角度相等的情況下，選擇邊（i，i+1）作為基邊。一旦找到邊（b，b+1），就將其存儲(chǔ)為第一個(gè)可能的基邊。

圖4 卡尺順時(shí)針旋轉(zhuǎn)示意圖

逆時(shí)針?lè)较蛐D(zhuǎn)卡尺以找到另一條可能的航跡，基邊為（b2，b2+1）。從嵌入式系統(tǒng)角度考慮，采用同一個(gè)angle（）函數(shù)，基于此，測(cè)量相對(duì)于直線B的角度，直線B與（b，b+1）重合（見(jiàn)圖5）。角γb即邊（b-1，b）和直線B之間的角度。角γa為邊（a-1，a）和直線A之間的角度，直線A為經(jīng)過(guò)a點(diǎn)的直線B的平行線。角φ與角γa互補(bǔ)以到達(dá)邊（a，a+1）、邊（b，b+1）在邊（a，a+1）之前接觸卡尺，因此角φ大于或等于0。角γb和γa的較小值決定兩種可能性中的邊：（b-1，b）或（a-1，a）。一旦確定則第2 個(gè)基邊（b2，b2+1）就找到了，則最遠(yuǎn)的頂點(diǎn)就是對(duì)映點(diǎn)對(duì)，參見(jiàn)算法3 第7 ～15 行。

圖5 卡尺最后位置與之前多邊形邊之間的角度

現(xiàn)有2 種可能的航跡：一條以（b，b+1）為基邊的航跡，在頂點(diǎn)a處結(jié)束；另一條以（b2，b2+1）為基邊的航跡，在頂點(diǎn)a2處結(jié)束。選擇飛行路線最少的路徑，參見(jiàn)算法3 的第16 ～19 行，其中函數(shù)dist（b，a）計(jì)算與邊（b，b+1）重合的直線與頂點(diǎn)a之間的距離，函數(shù)getPath（b，bmate，a）計(jì)算原點(diǎn)位于頂點(diǎn)b的BFP，掃描方向垂直于（b，bmate）。

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

對(duì)本文所提改進(jìn)的旋轉(zhuǎn)卡尺路徑規(guī)劃（IRCPP）用Matlab仿真，并和文獻(xiàn)［20］中的RCPP對(duì)比，無(wú)人機(jī)飛行參數(shù)如下：無(wú)人機(jī)飛行速度，v=5 m·s-1，航距，dx=20 m，無(wú)人機(jī)飛行高度，H=50 m，無(wú)人機(jī)最小轉(zhuǎn)彎半徑，Rmin=10 m。

IRCPP與RCPP比較結(jié)果如圖6 所示。圖6（a）、（b）分別為IRCPP 和RCPP 對(duì)于凸多邊形ROI 時(shí)，無(wú)人機(jī)分別為固定翼和多旋翼時(shí)的仿真。當(dāng)無(wú)人機(jī)為固定翼時(shí)，RCPP并不適用，而IRCPP可根據(jù)無(wú)人機(jī)的類型作出相應(yīng)的航跡規(guī)劃。圖6（c）、（d）分別為IRCPP和RCPP對(duì)于凹多邊形RoI 時(shí)，無(wú)人機(jī)分別為固定翼和多旋翼時(shí)的仿真。IRCPP首先進(jìn)行凹多邊形區(qū)域凸分解，然后再做規(guī)劃，而RCPP 對(duì)于凹多邊形ROI，雖然也能完成航跡規(guī)劃，但對(duì)于具有狹長(zhǎng)區(qū)域的地形，其轉(zhuǎn)彎次數(shù)多于IRCPP，如圖6（c）、（d）所示，IRCPP 轉(zhuǎn)彎17 次，RCPP轉(zhuǎn)彎21 次。

圖6 兩種算法在固定翼和多旋翼無(wú)人機(jī)路徑規(guī)劃中的比較

綜上，RCPP 只適合于ROI 是凸多邊形的多旋翼無(wú)人機(jī)航跡規(guī)劃，而IRCPP可根據(jù)ROI的形狀和無(wú)人機(jī)類型作出相應(yīng)的航跡規(guī)劃，更具實(shí)用性。

4 結(jié)語(yǔ)

改進(jìn)的旋轉(zhuǎn)卡尺路徑規(guī)劃算法考慮被測(cè)區(qū)域的凹凸性，如果被測(cè)區(qū)域是凹多邊形，則先將其分割為具有最少個(gè)數(shù)的凸邊形，使算法更具實(shí)用性。將無(wú)人機(jī)飛行起點(diǎn)和著陸點(diǎn)包括在內(nèi)，計(jì)算最優(yōu)邊-點(diǎn)來(lái)回航跡，構(gòu)成完整覆蓋航跡，更符合實(shí)際應(yīng)用場(chǎng)景，算法復(fù)雜度為O（n），其中n為感興趣區(qū)域的邊數(shù)?？紤]無(wú)人機(jī)類型，如果是固定翼無(wú)人機(jī)，則需考慮其最小轉(zhuǎn)彎半徑，使算法更具適用性。實(shí)驗(yàn)仿真表明了算法的實(shí)用性和適用性。

·名人名言·

我們應(yīng)該不虛度一生，應(yīng)該能夠說(shuō)，“我已經(jīng)做了我能做的事。”

——居里夫人