蔡 暢，陳建峰，劉 芬，周榮艷

（西北工業(yè)大學(xué) 航海學(xué)院，陜西 西安 710072）

0 引言

區(qū)域覆蓋路徑規(guī)劃（Coverage path planning，CPP）是指通過設(shè)計(jì)一條可行路徑達(dá)到完全覆蓋某一區(qū)域的目的，是機(jī)器人研究的基礎(chǔ)和關(guān)鍵領(lǐng)域[1-2]。CPP問題廣泛應(yīng)用于海底地貌測繪[3]、排雷（mine countermeasures，MCM）[4]、搜救（search and rescue，SAR）[5]、管道檢測[6]和海底考古等領(lǐng)域。隨著自動化和自主機(jī)器人設(shè)備的發(fā)展，已有將自主水下機(jī)器人（autonomous underwater vehicle，AUV）應(yīng)用到水下搜救任務(wù)中的案例[7]。本文針對使用配備側(cè)掃聲吶（side-scan sonar，SSS）的AUV進(jìn)行搜救任務(wù)的背景，解決完全覆蓋搜索區(qū)域的路徑規(guī)劃問題。

大量的覆蓋路徑規(guī)劃算法及其分類已被系統(tǒng)性地研究[8-9]，其研究重點(diǎn)集中于環(huán)境未知[10]、姿態(tài)不確定[11]、區(qū)域形狀不規(guī)則[12]、導(dǎo)航不可靠[13]等方面。但在大規(guī)模搜救任務(wù)中應(yīng)用CPP問題與傳統(tǒng)的CPP問題略有不同。在搜救任務(wù)中所使用的CPP問題有2個特點(diǎn)：一方面，海上搜救任務(wù)的覆蓋范圍可能非常大，容易出現(xiàn)任務(wù)失敗，而且操作成本高，因此，保證收集的數(shù)據(jù)質(zhì)量是至關(guān)重要的；另一方面，救援專家可提供專業(yè)協(xié)助[14]，如預(yù)測目標(biāo)信息等[15-16]，此類先驗(yàn)知識可對路徑規(guī)劃提供依據(jù)。

不同于被廣泛研究的以目標(biāo)搜索策略設(shè)計(jì)為重點(diǎn)的案例[17]，本文重點(diǎn)是完全覆蓋搜索區(qū)域，優(yōu)先搜索目標(biāo)可能出現(xiàn)區(qū)域的同時提高聲吶數(shù)據(jù)質(zhì)量以便后續(xù)數(shù)據(jù)處理。大多數(shù)目標(biāo)搜索問題致力于先驗(yàn)信息或傳感器數(shù)據(jù)如何指導(dǎo)機(jī)器人的路徑規(guī)劃，而不是機(jī)器人的行為是否有利于進(jìn)一步的目標(biāo)檢測。然而，目前采集的原始側(cè)掃聲吶數(shù)據(jù)主要用于形成聲吶圖像，主要是人工判讀來識別目標(biāo)[18]。因此，本文在路徑規(guī)劃方面，保持AUV航行穩(wěn)定，即保持直線運(yùn)動、穩(wěn)定速度以及恒定高度，來保證獲得較高質(zhì)量聲吶數(shù)據(jù)[19]。

雖然以往工作中的大部分覆蓋路徑規(guī)劃都可用于搜救任務(wù)，但是很少有研究考慮在可能出現(xiàn)失敗的大規(guī)模覆蓋任務(wù)中如何保證數(shù)據(jù)質(zhì)量的問題。針對上述問題，本文提出了一種適用于大規(guī)模搜救任務(wù)的概率覆蓋路徑規(guī)劃算法SAR- A *。該算法基于預(yù)測的目標(biāo)位置構(gòu)造了目標(biāo)存在概率圖，同時，考慮了環(huán)境、目標(biāo)、聲吶和平臺等影響側(cè)掃聲吶探測能力的因素，在作業(yè)區(qū)域分解為均勻分布的路徑點(diǎn)，通過遍歷所有路徑點(diǎn)達(dá)到全覆蓋。

1 問題的定義與模型

1.1 問題的定義

全覆蓋路徑規(guī)劃通過遍歷工作區(qū)域中分布的所有路徑點(diǎn)來覆蓋整個區(qū)域。假設(shè)AUV是一個無動力學(xué)行為的質(zhì)點(diǎn)，以固定的速度和高度在x–y平面上運(yùn)動，忽略導(dǎo)航誤差。在AUV航行過程中，側(cè)掃聲吶沿軌跡生成聲吶圖像，帶有側(cè)掃聲吶的AUV及其覆蓋條帶如圖1所示，且設(shè)側(cè)掃聲吶盲點(diǎn)已被補(bǔ)償。針對搜救任務(wù)，我們假設(shè)由救援專家提供目標(biāo)位置、移動軌跡和搜索范圍的預(yù)測。該假設(shè)合理，且可能為航行提供有用的信息。

圖1 側(cè)掃聲吶系統(tǒng)的3D模型Fig.1 3D model of a SSS system

將Lw×Ww的工作區(qū)域W定義為一個障礙物的開放平坦海域，被分解為Nc個小網(wǎng)格單元，表示為Call={c1,c2,???,cNc}，即

式中，Ci表示第i個單元格。

為了快速有效地完成搜救任務(wù)，在考慮側(cè)掃聲吶技術(shù)指標(biāo)和先驗(yàn)知識的前提下，設(shè)計(jì)了目標(biāo)函數(shù)h。生成的路徑點(diǎn)用Pa={pa1,pa2,???,paNc}表示，其中pai表示第i個路徑點(diǎn)。每個單元格在生成的路徑中只能出現(xiàn)一次，直到所有網(wǎng)格單元都包含在Pa中：

1.2 目標(biāo)存在概率

根據(jù)救援專家的預(yù)測先驗(yàn)信息，目標(biāo)存在概率圖給出了目標(biāo)存在于任意單元Ci的概率，用表示。將專業(yè)的搜救仿真結(jié)果和路徑規(guī)劃方法相結(jié)合，采用二元高斯分布估計(jì)目標(biāo)在每個單元中存在的概率。圖2展示了一個典型的搜救范圍預(yù)測系統(tǒng)的仿真結(jié)果[20]。

圖2 典型的救援專家評估的目標(biāo)軌跡和搜索區(qū)域Fig.2 Classical target trajectory and search area evaluated by rescue specialists

式中，xi和yi是區(qū)域Ci的坐標(biāo)。

在該模型中，將N(L,μ,Σ)的參數(shù)選擇與目標(biāo)的預(yù)測位置和估計(jì)軌跡相結(jié)合，通過等值線的形狀來反映。均值μ被設(shè)定為預(yù)測目標(biāo)位置的橢圓的中心，Σ由下落軌跡決定。當(dāng)只提供預(yù)測的目標(biāo)位置時，Σ為對稱矩陣。

1.3 側(cè)掃聲吶探測能力

探測能力Pd是指系統(tǒng)能夠準(zhǔn)確地探測和識別目標(biāo)的綜合評價。對側(cè)掃聲吶探測能力進(jìn)行量化，從弱到強(qiáng)，取值0～1，例如Pd=0.8，表示當(dāng)前側(cè)掃聲吶的探測能力較好。在實(shí)際應(yīng)用中，聲吶數(shù)據(jù)質(zhì)量受多種因素影響，不同位置也會有不同的探測能力。

2 SAR-A＊路徑規(guī)劃算法

針對以上提出的問題和模型，本節(jié)詳細(xì)介紹了SAR-A*路徑規(guī)劃方法的定義和步驟，主要思想是不斷從未訪問的單元格中選出最優(yōu)下一路徑點(diǎn)，從而實(shí)現(xiàn)區(qū)域全覆蓋，同時保證聲吶數(shù)據(jù)質(zhì)量和重要區(qū)域優(yōu)先訪問。

2.1 覆蓋區(qū)域分解

本文將覆蓋區(qū)域分解為正六邊形，如圖3所示。六邊形分解可以獲得更多候選方向較少，且移動距離相等，較傳統(tǒng)的四邊形分解更有優(yōu)勢。因此，SAR-A*算法采用六邊形分解。

圖3 六邊形分解Fig.3 Hexagon decomposition

2.2 多目標(biāo)優(yōu)化

SAR-A*算法的核心是目標(biāo)函數(shù)，其決定了路徑規(guī)劃算法質(zhì)量。為了減少計(jì)算負(fù)擔(dān)，預(yù)計(jì)算的availableList是當(dāng)前單元格的鄰居neighbors和未訪問單元格集合openList的交集。

第1個目標(biāo)函數(shù)f1是避免不必要路程，選擇距離較短、距目標(biāo)最可能出現(xiàn)位置最近的單元格

式中：cc為當(dāng)前位置；ci為Cava中的候選單元格；ch為pe最大值所在單元格；d(ccci)和d(chci)分別表示ci到點(diǎn)cc和點(diǎn)ch的歐氏距離；d0表示與初始單元格c1的最大距離；dh表示含有最小值pe的單元格ck與含有最大值pe的單元格ch之間的最大距離；ωd0和ωdh是滿足ωd0+ωdh=1的權(quán)重。以圖4（a）為例，如果點(diǎn)C和點(diǎn)D沒有被訪問，那么在A點(diǎn)的AUV不應(yīng)將B點(diǎn)作為下一個路徑點(diǎn)。

圖4 選擇下一個路徑點(diǎn)的標(biāo)準(zhǔn)Fig.4 Criteria for selecting the next waypoint

第2個目標(biāo)函數(shù)f2是為了避免轉(zhuǎn)彎，因?yàn)檗D(zhuǎn)彎會導(dǎo)致航行不穩(wěn)定及聲吶圖像失真[9]。因此，更少的轉(zhuǎn)彎能在路徑規(guī)劃層次上提高聲吶數(shù)據(jù)質(zhì)量。在圖4（b）中，當(dāng)最后一次移動是從E～F時，下一個路徑點(diǎn)優(yōu)先選擇點(diǎn)G，因?yàn)镋F和FG在同一直線上，H和I是次優(yōu)的。

式中：θa(ccci)和θa(cparcc)分別為ci到點(diǎn)cc和點(diǎn)cpar的候選方向和最后一個方向之間的夾角，cpar是cc的父單元格。

目標(biāo)函數(shù)f3強(qiáng)調(diào)了更高目標(biāo)定位概率。搜救任務(wù)的最終目的是在最短的時間內(nèi)探測到目標(biāo)，即確定目標(biāo)位置。f3將目標(biāo)定位概率pl（如公式（6）所示）與探測能力pd和目標(biāo)存在概率pe相結(jié)合，即

圖4（c）中，從點(diǎn)J開始的下一步最可取的點(diǎn)是點(diǎn)K，因?yàn)辄c(diǎn)K目標(biāo)定位概率最大

式中：plmin和plmax為pl的最小值和最大值。

在考慮上述3個準(zhǔn)則的基礎(chǔ)上，將區(qū)域選擇策略表述為一個多目標(biāo)優(yōu)化問題。本文采用加權(quán)度量方法對單元格進(jìn)行評估。單個目標(biāo)函數(shù)的理想解為：f1*=1，f2*=0，f3*=1。最終多目標(biāo)函數(shù)表示為

式中：h(ci)為候選單元格ci的最終評估得分；M=3；ωm為對應(yīng)的權(quán)重。

2.3 基于A＊算法的覆蓋路徑規(guī)劃

SAR-A*路徑規(guī)劃算法是為完全覆蓋大規(guī)模搜救區(qū)域而設(shè)計(jì)的，其主要步驟如圖5所示。該算法首先將搜索區(qū)域分解為大量的六邊形單元格，通過遍歷所有單元格來實(shí)現(xiàn)搜索區(qū)域全覆蓋。然后，建立目標(biāo)存在概率圖和探測能力模型，作為搜索的基礎(chǔ)。該方法的主循環(huán)是基于多目標(biāo)函數(shù)的 A*算法框架，將模型和目標(biāo)函數(shù)結(jié)合到AUV路徑規(guī)劃中，直到實(shí)現(xiàn)全覆蓋。

圖5 SAR-A＊算法路徑規(guī)劃過程的流程圖Fig.5 Flow diagram describing the path planning process with the SAR-A* algorithm

A*算法是一種常見的圖遍歷和路徑搜索算法[21-22]。被應(yīng)用的A*算法體系結(jié)構(gòu)可以保證生成的路徑中沒有重復(fù)的單元格。下一個路徑點(diǎn)選擇策略的偽代碼在圖6中說明。

圖6 下一個路徑點(diǎn)選擇策略Fig.6 The next waypoint selection strategy

3 仿真結(jié)果

本文提出的路徑規(guī)劃方法在探測能力不均勻場景下進(jìn)行了仿真。假設(shè)存在有利于探測的區(qū)域（見圖7（a）的淺藍(lán)色矩形）和不利于探測的區(qū)域（見圖7（a）的黃色帶狀區(qū)域）。其對應(yīng)的探測能力的準(zhǔn)確分布如圖7（b）所示，其中凸起區(qū)域和凹陷區(qū)域分別對應(yīng)圖7（b）中的淺藍(lán)色區(qū)域和黃色區(qū)域。目標(biāo)存在概率模型如圖 8所示，基本參數(shù)列于表1。仿真結(jié)果與幾種同類型算法進(jìn)行了對比，包括割草機(jī)算法（LM）、隨機(jī)規(guī)劃算法（Rnd）、距離優(yōu)先算法（Dist）、目標(biāo)搜索概率優(yōu)先算法（Pl）。

表1 默認(rèn)參數(shù)Tab.1 Default Parameters

圖7 工作區(qū)域和非均勻探測能力Fig.7 Workspace and nonuniform detective ability map

圖8 目標(biāo)存在概率分布和預(yù)測目標(biāo)位置Fig.8 Target presence probability distributionand predicted target position

提出的方法所生成的路徑如圖9所示?？梢钥闯?，AUV首先對目標(biāo)定位概率高、探測能力強(qiáng)的區(qū)域進(jìn)行探測?？紤]非均勻探測能力下，5種不同方法的累積目標(biāo)定位概率（置信度）如圖10所示?？梢?，目標(biāo)定位概率隨著步數(shù)的增加而迅速增加，SAR-A*算法（綠線）和Pl算法可以相對較少的步數(shù)快速提高置信度。但 Pl算法只考慮目標(biāo)定位的概率，在整體目標(biāo)函數(shù)上有明顯不足。圖11給出了5種方法的目標(biāo)函數(shù)值。從圖中可以看出，SAR-A*算法的目標(biāo)函數(shù)值整體保持低于其他方法。圖11（b）的柱狀圖說明了5種方法的累計(jì)目標(biāo)函數(shù)值。SAR-A*算法的目標(biāo)函數(shù)值比LM算法小43%，證明SAR-A*達(dá)到了預(yù)期效果。

圖9 由SAR-A＊路徑規(guī)劃算法在非均勻探測能力場景中生成的跟蹤Fig.9 Trace generated by the SAR-A* path planner in the nonuniform detective ability scenario

圖10 非均勻探測能力場景下5種不同算法定位目標(biāo)的置信度Fig.10 Confidence of locating the target with five different methods in the nonuniform detective ability scenario

圖11 5種算法目標(biāo)函數(shù)值性能Fig.11 Objective function value performance of five algorithms

由以上討論可知，SAR-A*算法在目標(biāo)定位概率及整體目標(biāo)函數(shù)值上都有很好的性能。使用SAR-A*算法可快速搜索更高的目標(biāo)定位概率。

4 結(jié)束語

本文針對AUV進(jìn)行搜救任務(wù)的場景，提出了一種全覆蓋路徑規(guī)劃方法SAR-A*。該方法將工作區(qū)域進(jìn)行六邊形分解，并充分利用了先驗(yàn)?zāi)繕?biāo)信息，建立目標(biāo)存在概率圖，同時對側(cè)掃聲吶探測能力進(jìn)行量化。利用多目標(biāo)函數(shù)不斷對候選單元格進(jìn)行評估，不斷確定最優(yōu)下一路徑點(diǎn)，引導(dǎo)AUV進(jìn)行區(qū)域的覆蓋。仿真結(jié)果表明：所設(shè)計(jì)的SAR-A*路徑規(guī)劃算法能夠完全覆蓋搜索區(qū)域，并能快速覆蓋目標(biāo)定位置信度高的區(qū)域，從傳感器的探測能力和先驗(yàn)?zāi)繕?biāo)信息2方面獲取高質(zhì)量的數(shù)據(jù)。下一步擬將AUV集群應(yīng)用到大規(guī)模搜救任務(wù)中來提高任務(wù)效率。