本發(fā)明涉及的是一種水下自主航行器(AUV)的三維路徑規(guī)劃方法。

背景技術(shù)：

AUV在水下進(jìn)行航路規(guī)劃時(shí)，除了要考慮路徑長度、障礙物信息、路徑的平滑程度之外，還需要考慮海流要素的影響。海流的速度和方向會(huì)對(duì)水下航行器的航行時(shí)間和能量消耗產(chǎn)生影響，尤其是當(dāng)航行器速度較慢時(shí)，對(duì)其進(jìn)行航路規(guī)劃時(shí)務(wù)必要考慮海流對(duì)航行器運(yùn)動(dòng)的影響。盡量使航行器順流航行，充分利用海流場(chǎng)的推動(dòng)作用，減小AUV逆流航行過程中帶來的能量損耗，規(guī)劃出一條能量損耗較小且時(shí)間最優(yōu)的路徑。近年來，國內(nèi)外很多學(xué)者對(duì)考慮海流影響的AUV航路規(guī)劃方法進(jìn)行研究。例如，王超，朱大奇等人在《基于人工勢(shì)場(chǎng)與速度合成的AUV路徑規(guī)劃》一文中提出了一種基于人工勢(shì)場(chǎng)法考慮海流影響的AUV路徑規(guī)劃方法。該方法的優(yōu)點(diǎn)是解決了海流對(duì)AUV航行時(shí)間的影響，減小了能量的損耗。缺點(diǎn)是算法在動(dòng)態(tài)的海流環(huán)境下會(huì)失效。Soulignac等人在《Feasible and Optimal Path Planning in Strong Current Fields》一文中提出了滑動(dòng)波前擴(kuò)展法，該方法的優(yōu)點(diǎn)是通過結(jié)合適當(dāng)?shù)拇鷥r(jià)函數(shù)與連續(xù)優(yōu)化技術(shù)，保證了路徑的有效性和全局最優(yōu)性。缺點(diǎn)是該方法沒有應(yīng)用到三維情況。

S.Osher和J.A.Sethian于1988年提出了水平集方法(Level Set)。水平集方法的主要思想是將移動(dòng)變形的曲線(面)作為零水平集嵌入到更高一維的函數(shù)中，由封閉超曲面的演化方程可以得到函數(shù)的演化方程，而嵌入的封閉曲線(面)總是保持為函數(shù)在零水平截面上的點(diǎn)集，最終只要獲得演化函數(shù)在零水平截面上點(diǎn)集的位置，即可得到移動(dòng)變形曲線(面)的演化結(jié)果。水平集方法提出以來已經(jīng)被成功地應(yīng)用于物理學(xué)、流體力學(xué)、材料科學(xué)、計(jì)算機(jī)圖形學(xué)等多個(gè)領(lǐng)域，尤其在圖像領(lǐng)域取得了巨大的發(fā)展。Ron Kimmel等人最早應(yīng)用水平集方法在三維空間中尋找兩點(diǎn)間的最短路徑，此后，水平集方法在路徑規(guī)劃中開始得到關(guān)注。以往的航路規(guī)劃算法大多數(shù)只考慮靜態(tài)海流的影響，在動(dòng)態(tài)的環(huán)境下效果不理想，使得這些算法的應(yīng)用范圍有限，而水平集方法能夠很容易的從二維空間擴(kuò)展到三維空間并很好的應(yīng)用于動(dòng)態(tài)流體運(yùn)動(dòng)，特別是在水下航路規(guī)劃中取得了很好的效果。因此，在水下航路規(guī)劃中，將水平集作為基礎(chǔ)的航路規(guī)劃算法具有良好的應(yīng)用前景。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種速度快、精度高，充分考慮了海流的影響，規(guī)劃出的路徑具有很強(qiáng)的實(shí)用性的基于改進(jìn)水平集算法的時(shí)間最優(yōu)航路規(guī)劃方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

步驟1.基于電子海圖和海流信息構(gòu)建三維航路規(guī)劃環(huán)境空間；

步驟2.初始化水平集函數(shù)為符號(hào)距離函數(shù)，將AUV航行的起點(diǎn)設(shè)置在零水平集上，并設(shè)置AUV的航行速度；

步驟3.構(gòu)建考慮海流對(duì)AUV航行的影響的水平集演化方程，并在起點(diǎn)處構(gòu)造窄帶，設(shè)置禁航區(qū)；

步驟4.依據(jù)步驟3建立的水平集方程，在窄帶內(nèi)沿著零水平集梯度方向和海流方向的矢量和方向演化水平集函數(shù)，并存儲(chǔ)每個(gè)時(shí)間步長的零水平集界面；

步驟5.判斷目標(biāo)點(diǎn)是否在當(dāng)前窄帶范圍內(nèi)，如果不在，檢測(cè)新的零水平集界面是否到達(dá)窄帶邊緣，如果未到達(dá)窄帶邊緣，則返回步驟4，否則執(zhí)行步驟6；如果目標(biāo)點(diǎn)在當(dāng)前窄帶范圍內(nèi)，檢測(cè)新的零水平集界面是否到達(dá)目標(biāo)點(diǎn)，如果未到達(dá)目標(biāo)點(diǎn)，則返回步驟4，否則轉(zhuǎn)步驟7；

步驟6.重構(gòu)窄帶，并且利用改進(jìn)快速行進(jìn)法重新初始化水平集函數(shù)為符號(hào)距離函數(shù)，返回步驟4；

步驟7.利用后向迭代方程，獲得AUV的時(shí)間最優(yōu)路徑，輸出最優(yōu)路徑，路徑規(guī)劃結(jié)束。

本發(fā)明還可以包括：

1、所述水平集演化方程為：

其中(x,y,z)代表三維空間中某點(diǎn)在X軸、Y軸和Z軸的坐標(biāo)，t代表水平集函數(shù)φ(x,y,z,t)到達(dá)坐標(biāo)(x,y,z)的時(shí)間，AUV的運(yùn)動(dòng)速度大小為F，航向?yàn)楹Ａ鲌?chǎng)為V(x,y,z,t)。

2、所述窄帶的寬度為22。

3、所述設(shè)置禁航區(qū)是在禁航區(qū)內(nèi)將AUV的航行速度和海流速度設(shè)置為0。

4、所述改進(jìn)快速行進(jìn)法是采用八鄰域的方式離散程函方程，同時(shí)采用雙快速行進(jìn)的方法，通過插值法對(duì)零水平集界面進(jìn)行插值得到兩條初始的零水平集同時(shí)行進(jìn)。

為了解決基于傳統(tǒng)水平集的路徑規(guī)劃算法中存在的速度較慢和精度不高的缺陷，本發(fā)明提出了一種基于改進(jìn)水平集算法的AUV航路規(guī)劃方法。本發(fā)明利用海流再分析數(shù)據(jù)庫生成海流場(chǎng)，在路徑規(guī)劃中充分考慮海流的影響，使得規(guī)劃出的路徑具有很強(qiáng)的實(shí)用性。

本發(fā)明的有益效果是：

1.本發(fā)明通過修改水平集方程將海流模型結(jié)合到水平集方程中，在海流模型的基礎(chǔ)上，規(guī)劃出的路徑以航行時(shí)間為最優(yōu)目標(biāo)且能夠充分地利用海流，即使在復(fù)雜海流存在的情況下，同樣能夠有效的找到一條可行路徑。

2.本發(fā)明提出的基于改進(jìn)水平集算法的AUV航路規(guī)劃方法，不僅能夠充分利用海流的影響，而且通過簡單的修改水平集方程能夠很好的避開禁航區(qū)，這也是該算法的主要優(yōu)勢(shì)。

3.本發(fā)明利用改進(jìn)水平集算法進(jìn)行AUV全局路徑規(guī)劃，相比于傳統(tǒng)水平集算法規(guī)劃速度更快，規(guī)劃精度更高，同時(shí)能夠很好的應(yīng)用于三維空間路徑規(guī)劃，實(shí)用性強(qiáng)，規(guī)劃出更接近實(shí)際航行路線的AUV航行路徑。

附圖說明

圖1為本發(fā)明所述考慮海流影響的AUV路徑規(guī)劃流程圖。

圖2為本發(fā)明所述改進(jìn)快速行進(jìn)法流程圖。

具體實(shí)施方式

下面舉例對(duì)本發(fā)明做進(jìn)一步說明。

本發(fā)明是一種考慮海流影響的AUV三維路徑規(guī)劃方法，主要包括環(huán)境建模、水平集建模、重新初始化水平集函數(shù)、獲得最優(yōu)路徑等關(guān)鍵內(nèi)容，具體實(shí)現(xiàn)流程見圖1，包括以下：

步驟1.基于電子海圖和海流信息構(gòu)建三維航路規(guī)劃環(huán)境空間

為了真實(shí)模擬水下AUV的航行環(huán)境，對(duì)AUV航行的真實(shí)環(huán)境進(jìn)行建模。在三維空間中建立以左下角頂點(diǎn)為坐標(biāo)原點(diǎn)，經(jīng)度增加的方向?yàn)閄軸正方向，緯度增加的方向?yàn)閅軸正方向，水深增加的方向?yàn)閆軸負(fù)方向的三維空間坐標(biāo)系。將空間劃分為M×N×L個(gè)網(wǎng)格。網(wǎng)格劃分原則為：X軸和Y軸的網(wǎng)格間距不大于0.02°，Z軸的網(wǎng)格間距不大于10米。

從電子海圖中提取航行區(qū)域內(nèi)的水深數(shù)據(jù)，并利用克里金插值算法將離散水深數(shù)據(jù)處理成規(guī)則網(wǎng)格數(shù)據(jù)；從海洋環(huán)境數(shù)據(jù)庫中提取提取航行區(qū)域內(nèi)的的海流數(shù)據(jù)。

步驟2.初始化水平集函數(shù)為符號(hào)距離函數(shù)，將AUV航行的起點(diǎn)設(shè)置在零水平集上，并設(shè)置AUV的航行速度

在三維空間中，設(shè)定一個(gè)起點(diǎn)X和一個(gè)終點(diǎn)Y，X、Y為位置向量。假設(shè)AUV總是存在于零水平集界面上，初始化水平集函數(shù)φ(x,y,z,t＝0)為符號(hào)距離函數(shù)，其零水平集界面為以柵格間距為半徑的球面。

步驟3.構(gòu)建考慮海流對(duì)AUV航行的影響的水平集演化方程，并在起點(diǎn)處構(gòu)造窄帶，設(shè)置禁航區(qū)

令T(Y)代表AUV從起點(diǎn)X到達(dá)終點(diǎn)Y的時(shí)間，AUV的運(yùn)動(dòng)速度大小為F，航向?yàn)楹Ａ鲌?chǎng)為V(x,y,z,t)，初始化的水平集函數(shù)φ(x,y,z,t)由差分方程(1)演化:

其中(x,y,z)代表三維空間中某點(diǎn)在X軸、Y軸和Z軸的坐標(biāo)，t代表水平集函數(shù)φ(x,y,z,t)到達(dá)坐標(biāo)(x,y,z)的時(shí)間。

當(dāng)AUV到達(dá)終點(diǎn)Y時(shí)φ(Y,T(Y))＝0。為了保證為AUV從起點(diǎn)X到達(dá)終點(diǎn)Y的最短時(shí)間，從而得到一條時(shí)間最優(yōu)路徑，應(yīng)保證AUV的航向始終垂直于零水平集曲線的梯度方向。因此方程(1)可以改寫為：

其中x,y,z代表三維空間中某點(diǎn)在X軸、Y軸和Z軸的坐標(biāo)，t代表水平集函數(shù)φ(x,y,z,t)到達(dá)坐標(biāo)(x,y,z)的時(shí)間，F(xiàn)代表AUV的航行速度，V(x,y,z,t)代表三維海流場(chǎng)的速度，φ(x,y,z,t)代表水平集函數(shù)。

由方程(2)可知，當(dāng)AUV的航行速度始終垂直于水平集函數(shù)的梯度方向時(shí)，AUV的航行時(shí)間由水平集函數(shù)的演化速度決定，當(dāng)水平集函數(shù)演化至終點(diǎn)后，所有時(shí)間步Δt的總和即為AUV的最優(yōu)航行時(shí)間。

窄帶設(shè)置：根據(jù)窄帶寬度，以初始的零水平集曲線為基準(zhǔn)，將所有距離零水平集不超過λ的點(diǎn)加入零水平集的窄帶范圍，構(gòu)造寬度為2λ的窄帶。

禁航區(qū)的設(shè)置：出于安全性的考慮，AUV在航行過程中需要避開某些區(qū)域，比如障礙物，此區(qū)域即為禁航區(qū)，禁航區(qū)內(nèi)AUV的速度為0，此時(shí)可以修改水平集演化方程(2)，在規(guī)劃空間中的禁航區(qū)內(nèi)令A(yù)UV的航行速度和海流速度為零，因此修改后的水平集方程在演化過程中順著水平集函數(shù)的梯度演化即可避開禁航區(qū)。

步驟4.依據(jù)步驟3建立的水平集方程，在窄帶內(nèi)沿著零水平集梯度方向和海流方向的矢量和方向演化水平集函數(shù)，存儲(chǔ)每個(gè)時(shí)間步長的零水平集界面。界面由點(diǎn)集組成，此點(diǎn)集是當(dāng)前時(shí)刻形成的一個(gè)可達(dá)集輪廓。初始條件下，零水平集是以柵格邊長為半徑的球面

某一時(shí)刻AUV能夠到達(dá)的點(diǎn)集稱為一個(gè)可達(dá)集。AUV從起點(diǎn)開始航行，在t時(shí)刻存在一個(gè)可達(dá)集，AUV以垂直于零水平集曲線的梯度方向繼續(xù)航行到達(dá)下一個(gè)可達(dá)集，通過可達(dá)集的不斷演化直至到達(dá)終點(diǎn)。

AUV在航行過程中的速度與零水平集界面演化的速度相同，AUV運(yùn)動(dòng)時(shí)所受的和速度由水平集函數(shù)梯度方向的速度和海流速度組成，令X(t)代表AUV的運(yùn)動(dòng)軌跡，因此有下式：

其中x,y,z代表三維空間中某點(diǎn)在X軸、Y軸和Z軸的坐標(biāo)，t代表水平集函數(shù)φ(x,y,z,t)到達(dá)坐標(biāo)(x,y,z)的時(shí)間，代表零水平集上任意一點(diǎn)X的速度，F(xiàn)代表AUV的航行速度，代表航向，V(x,y,z,t)代表三維海流場(chǎng)的速度，φ(x,y,z,t)代表水平集函數(shù)。

最后，AUV的時(shí)間最優(yōu)航行路徑可通過式(4)從終點(diǎn)開始后向迭代求得。

其中x,y,z代表三維空間中某點(diǎn)在X軸、Y軸和Z軸的坐標(biāo)，t代表水平集函數(shù)φ(x,y,z,t)到達(dá)坐標(biāo)(x,y,z)的時(shí)間，代表零水平集上任意一點(diǎn)X的速度，F(xiàn)代表AUV的航行速度，代表航向，V(x,y,z,t)代表三維海流場(chǎng)的速度，φ(x,y,z,t)代表水平集函數(shù)。

步驟5.判斷目標(biāo)點(diǎn)是否在當(dāng)前窄帶范圍內(nèi)。如果不在，檢測(cè)新的零水平集界面是否到達(dá)窄帶邊緣，如果未到達(dá)窄帶邊緣，則返回步驟4，否則進(jìn)行步驟6；如果目標(biāo)點(diǎn)在當(dāng)前窄帶范圍內(nèi)，檢測(cè)新的零水平集界面是否到達(dá)目標(biāo)點(diǎn)，如果未到達(dá)目標(biāo)點(diǎn)，則返回步驟4，否則進(jìn)行步驟7

窄帶邊緣檢測(cè)方法如下：計(jì)算出距離窄帶邊緣σ的一組點(diǎn)，將這組點(diǎn)和窄帶邊緣點(diǎn)一起加入到一個(gè)數(shù)組中，時(shí)刻監(jiān)視這些點(diǎn)的符號(hào)。因?yàn)槲挥诹闼郊獾狞c(diǎn)的符號(hào)為正，位于零水平集內(nèi)的點(diǎn)的符號(hào)為負(fù)，所以初始時(shí)數(shù)組內(nèi)點(diǎn)的符號(hào)均為正。當(dāng)零水平集距離窄帶的邊緣小于σ，甚至超出窄帶時(shí)，數(shù)組內(nèi)的點(diǎn)至少存在一個(gè)點(diǎn)的符號(hào)為負(fù)，此時(shí)表示零水平集已經(jīng)接近窄帶邊緣。

步驟6.重構(gòu)窄帶，并且利用改進(jìn)快速行進(jìn)法重新初始化水平集函數(shù)為符號(hào)距離函數(shù)，返回步驟4

本發(fā)明采用改進(jìn)快速行進(jìn)法作為重新初始化方法，改進(jìn)的快速行進(jìn)法采用八鄰域的方式代替?zhèn)鹘y(tǒng)的四鄰域方式離散程函方程|▽T|F＝1，提高了算法精度。離散后的程函方程為：

其中DT代表差分格式，T稱為到達(dá)時(shí)間函數(shù)，h代表網(wǎng)格間距，F(xiàn)代表行進(jìn)速度。

同時(shí)，由于傳統(tǒng)快速行進(jìn)法只能單向傳播，重新初始化水平集函數(shù)的時(shí)間較長。為了加快窄帶內(nèi)水平集函數(shù)的快速重構(gòu)，采用雙快速行進(jìn)的方法，通過插值法對(duì)零水平集界面進(jìn)行插值得到兩條初始的波前同時(shí)行進(jìn)，進(jìn)一步提高算法的效率，具體處理流程見圖2。

應(yīng)用八鄰域求解中心點(diǎn)的前提是每一維度的網(wǎng)格間距相等，設(shè)h代表網(wǎng)格間距，則利用八個(gè)鄰近點(diǎn)方式求解程函方程的方法如下：

設(shè)存在一階算子和將其帶入程函方程(5)中可得：

同樣存在一階算子和將其帶入程函方程(5)中可得：

當(dāng)點(diǎn)(i,j)周圍鄰域點(diǎn)只有點(diǎn)(i,j-1)或者點(diǎn)(i-1,j)單一鄰接點(diǎn)，且對(duì)角點(diǎn)存在時(shí)，則利用式(6)或者式(7)更新點(diǎn)(i,j)水平集函數(shù)值。當(dāng)點(diǎn)(i,j)周圍鄰域點(diǎn)同時(shí)存在點(diǎn)(i,j-1)和點(diǎn)(i-1,j)兩個(gè)鄰接點(diǎn)，且兩鄰接點(diǎn)相鄰的對(duì)角點(diǎn)存在時(shí)，則利用式(8)更新點(diǎn)(i,j)水平集函數(shù)值。

改進(jìn)快速行進(jìn)法中采用的數(shù)據(jù)結(jié)構(gòu)定義如下：

結(jié)構(gòu)體Coordinate：主要存儲(chǔ)算法行進(jìn)過程中，點(diǎn)的位置坐標(biāo)(橫坐標(biāo)和縱坐標(biāo))以及標(biāo)志位(標(biāo)識(shí)點(diǎn)是否處于零水平集內(nèi)部)。

結(jié)構(gòu)體Node：存儲(chǔ)待處理的更新點(diǎn)的位置信息和水平集函數(shù)值。該結(jié)構(gòu)體包含位置坐標(biāo)和走時(shí)值兩個(gè)成員變量。位置坐標(biāo)類型為Coordinate，走時(shí)值代表當(dāng)前更新點(diǎn)的距離值。

C++標(biāo)準(zhǔn)模板類Vector<Node>：代表一個(gè)優(yōu)先隊(duì)列，存儲(chǔ)待處理的更新點(diǎn)。

已知點(diǎn)Accepted：類型為map<Coordinate，double>。該變量存儲(chǔ)對(duì)零水平集插值后的初始外部波前點(diǎn)和初始內(nèi)部波前點(diǎn)。其中鍵存儲(chǔ)的是初始波前點(diǎn)的坐標(biāo)和標(biāo)識(shí)信息，值存儲(chǔ)的是當(dāng)前點(diǎn)的水平集函數(shù)值。

更新點(diǎn)tentative：類型為map<Coordinate，Node*>。該變量存儲(chǔ)零水平集外部和內(nèi)部的待更新點(diǎn)。其中鍵存儲(chǔ)的待更新點(diǎn)的坐標(biāo)和標(biāo)識(shí)信息，值存儲(chǔ)的是當(dāng)前點(diǎn)的位置信息和水平集函數(shù)值。

窄帶內(nèi)改進(jìn)快速行進(jìn)法重構(gòu)水平集函數(shù)的具體步驟為：

步驟6.1初始化：

a)通過最鄰近插值法對(duì)零水平集曲線插值，同時(shí)得到零水平集外部的初始波前點(diǎn)和零水平集內(nèi)部的初始波前點(diǎn)，此時(shí)這些點(diǎn)的水平集函數(shù)值已經(jīng)得到，標(biāo)記這些點(diǎn)為已知點(diǎn)，同時(shí)設(shè)置標(biāo)志位，標(biāo)志已知點(diǎn)是否為零水平集內(nèi)部點(diǎn)。

b)遍歷檢查每個(gè)已知點(diǎn)的四個(gè)鄰域點(diǎn)，如果此點(diǎn)標(biāo)記不是已知點(diǎn)，則記此點(diǎn)為更新點(diǎn)，通過方程(8)計(jì)算更新點(diǎn)此時(shí)的水平集函數(shù)值，同時(shí)通過已知點(diǎn)標(biāo)志位，確認(rèn)更新點(diǎn)是否位于零水平集內(nèi)部。將所有的更新點(diǎn)加入一個(gè)優(yōu)先隊(duì)列中，并依據(jù)更新點(diǎn)的水平集值升序排列；

步驟6.2外部和內(nèi)部同時(shí)推進(jìn)：

a)輸出優(yōu)先隊(duì)列中的首元素φ_min(i,j)，標(biāo)記φ_min(i,j)為已知點(diǎn)，并從優(yōu)先隊(duì)列中刪除；

b)依次檢查φ_min(i,j)的四鄰域點(diǎn)(i-1,j)，(i+1,j)，(i,j-1)，(i,j+1)。如果原來標(biāo)記為已知點(diǎn)或者更新點(diǎn)則忽略此鄰域點(diǎn)，維持水平集函數(shù)值不變。否則，若該鄰域點(diǎn)的水平集函數(shù)值不大于窄帶寬度，將該鄰域點(diǎn)記為更新點(diǎn)，根據(jù)其周圍鄰點(diǎn)的情況，由方程(8)求得該鄰域點(diǎn)的水平集函數(shù)值，并將該點(diǎn)加入到優(yōu)先隊(duì)列的尾部；若該鄰域點(diǎn)的水平集函數(shù)值大于窄帶寬度，則忽略該鄰域點(diǎn)，繼續(xù)檢查下一個(gè)鄰域點(diǎn)，當(dāng)φ_min(i,j)的四個(gè)鄰域點(diǎn)均檢查完，檢查優(yōu)先隊(duì)列是否為空，若為空，停止推進(jìn)，轉(zhuǎn)到步驟3.3，否則轉(zhuǎn)到a)步驟。

步驟6.3此時(shí)，窄帶內(nèi)所有點(diǎn)均為已知點(diǎn)，根據(jù)已知點(diǎn)的標(biāo)志位輸出零水平集內(nèi)外點(diǎn)新的水平集函數(shù)值。

步驟7.利用后向迭代方程(4)，獲得AUV的時(shí)間最優(yōu)路徑，輸出最優(yōu)路徑，路徑規(guī)劃結(jié)束

當(dāng)零水平集界面第一次到達(dá)終點(diǎn)Y時(shí)，首先確定終點(diǎn)Y是否在此時(shí)刻的零水平集界面上，若Y不在零水平集界面上，則確定終點(diǎn)Y在零水平集曲線上的投影，記為Y₁，從點(diǎn)Y₁開始以式(9)反向計(jì)算最優(yōu)路徑。

具體步驟如下：

步驟7.1.在t時(shí)刻，尋找距離終點(diǎn)Y最近的三個(gè)點(diǎn)構(gòu)成一個(gè)三角平面，根據(jù)點(diǎn)和平面的位置關(guān)系，確定終點(diǎn)Y在零水平集界面上的投影Y₁。

步驟7.2.求取投影Y₁垂直于零水平集界面的垂向如果Y₁在零水平集界面上，則垂向?yàn)槿瞧矫娴拇怪狈较?；如果Y₁不再零水平集平面上，則垂向?yàn)榫嚯xY₁最近的點(diǎn)周圍三角平面垂向的平均值。

步驟7.3.計(jì)算

步驟7.4重復(fù)步驟7.1到步驟7.3直至回溯到起點(diǎn)，這樣從終點(diǎn)到起點(diǎn)能夠回溯出一系列路徑點(diǎn)，將這些路徑點(diǎn)連接起來，輸出最優(yōu)路徑。