1、典型航路規劃方法的基本原理例題展示典型航路規劃方法的基本原理例題展示PAGE19典型航路規劃方法的基本原理例題展示簡要論述典型航路規劃方法的基本原理（任選方法之一，且選擇該方法中的一種具體方法。）并舉例說明。（1）路標圖法；（2）單元分解方法；（3）人工勢場法答：選擇（1）路標圖法。概略圖( Skeleton)也稱路標圖( Roadmap )。在基于概略圖空間的路徑規劃方法中，首先根據一定的規則將自由的C空間（Configuration Space)表示成一個由維的線段構成的網絡圖，然后采用某一搜索算法在該網絡圖上進行航跡搜索。這樣，規劃問題被轉化為一個網絡圖的搜索問題。概略圖必須表示出C空間

2、中的所有可能的路徑，否則該方法就是不完全的，即可能丟失最優解。常用的概略圖方法包括通視圖法( Visibility Graph)、Voronoi圖法、輪廓圖法( Silhouette)、 子目標網絡法(Subgoal Network )和隨機路線圖法( Probabilistic Roadmap, PRM)。 通視圖法通視圖由規劃空間中的障礙物的相互可見的頂點間的連線構成。圖1-1給出了包含三個多邊形障礙物的二維規劃空間的通視圖，其中較粗的線表示起始點S和目標點G之間的一條最短路徑。1989 年，Asano等證明在具有n個頂點的二維規劃空間中，其通視圖的邊數具有數量級0 (n2)，構造通視圖所

3、需時間的數量級也為0 (n2)。圖1-1通視圖 通視圖法可用于求解二維規劃空間中的最短路徑問題。盡管它也可用于高維規劃空間，但此時生成的路徑將不再是最短的。由于通視圖不能表達物體運動的方向性約束，除非運動物體可以按任何方向以任意角度轉彎，通常很少用通視圖法求解實際的路徑規劃問題。 Voronoi圖法如果運動物體要求與障礙(或威脅)的距離越遠越好，可以采用Voronoi圖方法。Voronoi 圖由與兩個或多個障礙(或威脅)的給定特征元素距離相等的點的集合構成。圖1-2 給出了以多邊形障礙物自身作為特征元素生成的Voronoi 圖。如果以多邊形的邊作為特征元素則可以得到不同的Voronoi 圖。對

4、于只包含有威脅的規劃空間來說，可以將威脅源的中心點作為特征元素。Voronoi圖將規劃空間分為多個區域，每個區域只包含一個特征元素。對于區域中的每一點，該區域的特征元素是所有特征元素中最近的。圖1-2 Voronoi 圖與通視圖比較，Voronoi 圖具有明顯的優點，Voronoi 圖的邊數只有數量級0 (n)， 構造Voronoi圖所需時間的數量級為0(nlogn),其中n為所選特征元素數目。如果將多邊形的邊作為特征元素，則Voronoi圖一般都包含有曲線段。1987 年，Canny 和Donald通過使用一種不同于歐氏距離的度量方法，構造出了一種只包含直線段的Voronoi 圖。對于維數大

5、于2的高維空間，通視圖與Voronoi圖將變得非常復雜，而且一般沒有確定的特征元素選擇方法。例如，多面體間的Voronoi 圖由二維曲面構成，它不再是一維的輪廓線。盡管通視圖仍然可以由多面體的各項點間的連線組成，但此時最短路徑不再存在于通視圖之中(如圖1-3 所示)。因此，Voronoi圖一般只應用于二維路徑規劃。圖1-3最短路徑不經過多面體的頂點 輪廓線法對于高維空間，Canny 于1987 年給出了另一種構造概略圖的方法。該方法先將高維空間中的物體投影到一個較低維的空間中，然后在低維空間中找出其投影的邊界曲線，稱為輪廓。該輪廓又投影到一個更低維的空間中，如此繼續下去，直到輪廓變成一維的輪廓

6、線。對于同一障礙物其輪廓不相連的部分，需用連接線將它們連接起來(如圖1-4 (b)所示)。這樣得到的一維輪廓線圖比原始的高維空間簡單得多，可以從中找到一條可行的路徑。該方法通常在理論上用于分析問題的復雜性，而很少用于實際中的路徑規劃。使用輪廓線方法得到的路徑，運動物體總是沿著障礙物邊緣移動。圖1-4輪廓線 子目標網絡法子目標網絡方法不直接構造明顯的概略圖，而是保存一個可以從起始點達到的節點列表。如果目標點出現在該表中，則規劃成功結束。規劃空間中兩點之間的可達性由一個簡單的局部規劃算法來判斷，該局部規劃算法稱為局部算子。局部算子的選擇一般根據具體問題確定，例如，可以簡單地在兩節點之間按直線運動。

7、開始的時候，該算法在起始節點與目標點之間選取一個由稱為子目標的中間節點組成的候選序列，并運用局部算子依次連接這些子目標。在選取子目標候選序列時可以采用某些啟發式信息，也可以隨機選取。如果連接過程不能到達目標點，則將已經連接的子目標保存在列表中。然后任取一個已到達的子目標，并在該子目標與目標節點之間選取一個候選序列，如此反復，直至到達目標節點。在該算法中，可到達的節點間的運動路徑可以由局部算子非常容易地重新得到，因此不用保存。該算法的一個主要優點是節省內存空間。通視圖可以看作是一個子目標網絡，其子目標為障礙物的頂點，局部算子為“直線運動”。圖1-5顯示了一個“沿對角線方向運動”的局部算子生成的子

8、目標網絡。圖1-5 子目標網絡局部算子的選擇確定了規劃算法的實現。一種極端的情形是采用“直線運動”，但當兩個節點之間的距離很遠時，該方法通常很難找到可行的路徑。因此，相鄰的兩個子目標間的距離一般很近，這勢必增加子目標的數目，從而增加內存空間。另一個極端就是采用一種精確的全局規劃算法作為局部算子，此時僅有一個包含有起始點和目標點的候選序列需要連接。這種方法將一個全局規劃問題分解成若千個比較簡單的局部規劃問題。 隨機路線圖法隨機路線圖法是近年發展起來的一種針對多自由度問題的路徑規劃方法，由Overmars等于1992年率先提出。該方法通過在規劃空間中隨機進行采樣生成路線圖，然后在該路線圖中搜索路徑

9、。隨機路線圖的構造如下:如果最新的采樣點與路線圖中的某節點間存在可行路徑，則將該采樣點加入到路線圖中，同時找出該節點與路線圖中的近鄰節點間可能存在的路徑，并將可行路徑作為節點間的邊加入到路線圖中。該方法的優點之一就是可以在規劃時間和路徑的質量之間進行權衡，構造隨機路線圖的時間越長，得到最優路徑的可能性就越大。在確定環境下，隨機路線圖一般可以事先構造。然而，如果規劃環境一日發生變化，隨機路線圖并不能通過局部更新以適應新的環境，因此，該算法-般不適于在線實時應用。簡要論述航路規劃啟發式搜索算法A*， .D*，anytime algorithms (ARA*)，anytime re-planning

10、 algorithms (AD*)的特點。A*A*（A-Star)算法是一種靜態路網中求解最短路最有效的方法。和Dijkstra一樣，A*能用于搜索最短路徑。和BFS一樣，A*能用啟發式函數引導它自己。它把Dijkstra算法（靠近初始點的結點）和BFS算法（靠近目標點的結點）的信息塊結合起來。有點不同的是，類似BFS的啟發式方法經常給出一個近似解而不是保證最佳解。然而，盡管A*基于無法保證最佳解的啟發式方法，A*卻能保證找到一條最短路徑。在討論A*的標準術語中，g(n)表示從初始結點到任意結點n的代價，h(n)表示從結點n到目標點的啟發式評估代價（heuristic estimated co

11、st）。當從初始點向目標點移動時，A*權衡這兩者。每次進行主循環時，它檢查f(n)最小的結點n，其中f(n) = g(n) + h(n)。保證找到最短路徑（最優解的）條件，關鍵在于估價函數h(n)的選取：估價值h(n)n到目標節點的距離實際值，這種情況下，搜索的點數多，搜索范圍大，效率低。但能得到最優解。如果估價值實際值,搜索的點數少，搜索范圍小，效率高，但不能保證得到最優解。估價值與實際值越接近，估價函數取得就越好。A*算法的搜索過程實際上是被選節點擴展的過程，它存在一種潛能，可以采用最少的估價源找到最近的優化路徑。在確定優化路徑后，要進行航路回朔，計算是否滿足任務系統中設定的燃油、時間、速

12、度等約束條件(這些約束條件有一定的次序)。如果不能滿足所有的約束條件，則計劃就失敗，必須重新計劃并修改有關參數。啟發式函數h(n)告訴A*從任意結點n到目標結點的最小代價評估值。例如對于幾何路網來說，可以取兩節點間歐幾理德距離（直線距離）做為估價值，即f=g(n)+sqrt(dx-nx)*(dx-nx)+(dy-ny)*(dy-ny)；這樣估價函數f在g值一定的情況下，會或多或少的受估價值h的制約，節點距目標點近，h值小，f值相對就小，能保證最短路的搜索向終點的方向進行。A*算法的關鍵是對評價函數的定義，從找到一條最小代價路徑的思路出發，在計算時可以把評價函數值分為從初始節點到節點n的代價，和

13、從節點n到達目標節點的代價兩個部分來進行計算和分析。估價函數的正確選取將直接關系到A*算法的成功與否，而函數的確定卻與實際情形有著密切的關系。所選擇的啟發式函數的好壞是很重要的。一個不恰當的啟發式函數反而會減慢A*算法，導致其產生出不好的路徑來。如果啟發式估值是精確的，則A*將不會偏離最佳路徑。當然，很難得到一個精確的啟發式估值，但知道當給A*一個正確的信息，它會正確地進行操作，這是非常有用的。另外，如果啟發式估值趨向于精確值，則A*搜索操作就會越少。因此，f值的增加是與搜索的多少相關。當啟發式估值是精確的，則f將不會變化。當啟發式估值低于實際值太多，f值就會迅速地增加。啟發式越好，搜索的地方

14、就越少。當探測器檢測到實際的環境和已知的環境信息存在差異時,則更新原有的環境地圖信息,那么原先規劃的路徑也許就是不可通的或者不是最優的。此時,可以根據更新的環境信息重新規劃一條從當前所在位置到目標點的新的路徑,但是代價很大。據于此,Anthony Stentz提出的動態A*算法或者叫D*算法。D*A* 在靜態路網中非常有效，但不適于在動態路網，環境如權重等不斷變化的動態環境下。D*是動態A*（D-Star,Dynamic A Star）。相對于A*算法，D*算法的主要特點是可以應用于環境僅為部分已知或環境不斷變化的情況下的路徑尋優。該算法根據當前已知環境狀況沿最有啟發性的節點搜索，如探測到下一

15、節點將會發生阻塞，則適時調整估價函數，改變方向繼續搜索，從而最終得到整個路徑。D*算法彌補了A*算法必須事先知道全部環境信息的缺點，且具有與A*算法一樣的高效特征。在環境信息是靜態的時候，D*的搜索方法和Dijkstra算法的搜索過程是一樣的。D*算法的主要過程分為離線和在線兩個部分。離線部分主要是指在已有的部分環境信息下規劃出一條機器人的行進路徑;而在線部分主要指移動機器人在沿著離線部分規劃出的路徑行進，當遇到和己知的部分環境不相同的情況或者說接受到新的環境信息的時候，重新規劃一條從機器人當前位置到目標的路徑的過程。D*算法的作用相當于靜態情況下，信息完全己知的情況下的路徑規劃算法。此時它的

16、執行過程和Dijksart搜索算法相同，在離線情況下，基本的D*算法的效能是不及A*算法的。但是，A*算法利用啟發信息限制了搜索擴展到的狀態，而其中一些沒有擴展到的狀態很有可能是在后面的在線規劃中所需要用到的，因而D*必須使用這種離線的規劃方法。對于在線規劃部分，此時算法的執行時間和很多因素直接相關，比如，預先己知的環境信息量，實際環境中障礙物密度，機器人環境傳感器感知范圍等。因此根前面的一般的重規劃方法相比，在相同的障礙物密度，相同的已知部分信息量，相同的感知范圍情況下，D*算法所重新規劃的環境中的狀態只是環境中的一部分，而一般的重規劃策略則需要在整個更新的環境中重新規劃。從這一點來說，該算

17、法是很有優勢的。并且隨著環境大小的增加，算法比普通的最優重規劃方法所節約的時間也成倍的增加。環境信息部分未知的情況下全局規劃部分很明顯需要一個重規劃的過程。這里的重規劃是指當遇到已知環境與原有實際環境存在差異時，使得原有的全局規劃路徑無法通過而必須重新進行的規劃。按照重規劃與原規劃的起點相同與否可以劃分為完全重規劃與不完全重規劃。所謂完全重規劃，是指重規劃路徑的起點與原規劃的起點相同，而不完全重規劃或部分重規劃則是指重規劃路徑的起點與原規劃的起點不同。動態的D*算法是不完全的重規劃，但是利用了原有的規劃信息，在一定程度上實現了最優性和實時性的結合。D*算法做到全局規劃和局部信息相結合、離線的規

18、劃和在線的規劃相結合。D*的搜索步驟如下：1.先用Dijstra算法從目標節點G向起始節點搜索。儲存路網中目標點到各個節點的最短路和該位置到目標點的實際值h,k（k為所有變化h之中最小的值,當前為k=h。每個節點包含上一節點到目標點的最短路信息1(2),2(5),5(4)，4（7）。則1到4的最短路為1-2-5-4。原OPEN和CLOSE中節點信息保存。2.機器人沿最短路開始移動，在移動的下一節點沒有變化時，無需計算，利用上一步Dijstra計算出的最短路信息從出發點向后追述即可，當在Y點探測到下一節點X狀態發生改變，如堵塞。機器人首先調整自己在當前位置Y到目標點G的實際值h(Y)，h(Y)=

19、X到Y的新權值c(X,Y)+X的原實際值h(X).X為下一節點(到目標點方向Y-X-G），Y是當前點。k值取h值變化前后的最小。3.用A*或其它算法計算，這里假設用A*算法,遍歷Y的子節點，點放入CLOSE,調整Y的子節點a的h值，h(a)=h(Y)+Y到子節點a的權重C(Y,a),比較a點是否存在于OPEN和CLOSE中。D*算法在動態環境中尋路非常有效，向目標點移動中，只檢查最短路徑上下一節點或臨近節點的變化情況，如機器人尋路等情況。對于距離遠的最短路徑上發生的變化，則感覺不太適用。anytime algorithms (ARA*)在很多情況下，最短路徑不一定就是我們想要的，還需要考慮時間

20、等因素。在有限的時間內得到好的、可行的結果才是我們想要的。ARA*算法就是解決這類問題的。Anytime算法是80年代末期Dean和Boddy在關于時間依賴規劃(Time-dependent planning)研究中提出的，其核心思想是使解的質量隨計算時間的增加而不斷提高。它具有在求解質量和求解時間之間折中的能力，被廣泛應用于時間約束條件下復雜問題的近寸以求解。同時也提出了一種問題元級控制的方法，使得智能系統具有高層調度的能力，且擴充了傳統算法的輸入一處理一輸出的計算過程，提供了運行中動態輸出結果的功能。Anytime算法把每一組輸入和特定的計算時間映射為一組輸出結果，這一特征使得該算法可以在

21、任何時候被中斷，返回特定質量的解，因此被稱為Anytime算法。從某種意義上說，具有如下特征的近以算法都可以稱為Anytime算法: 1.在算法執行的任何時候都能給出問題的一個解; 2.解質量隨計算時間的增加而提高。可以看出Anytime算法是一個逐步求精的過程。在這一點上，許多現有的算法都可視為Anytime算法。如:計算方法中的迭代算法、迭代加深的啟發式搜索算法、各種貪心算法、利用隨機原理的蒙特卡歲方法、遺傳算法。這些傳統的Anytime算法是被動式的，缺乏完善的元級調度機制。研究者公認，作為完善的Anytime算法，還應該具有如下特征:1.質量的度量Anytime算法用多值度量模型代替了

22、傳統的二值度量模型，因此解的質量是可以度量的;它反映了近似解和真實解的質量差距。 2.可預言性Anytime算法本身包含許多關于輸入、執行時間和解質量映射關系的統計信息，俠得算法可預言在一定計算時間內輸出解的質量，以便做高層的調度和控制。3.單調性Anytime算法具有解質量隨執行時間增加而單調增加的特性。這一點是以解質量可以度量為前提的。只有這樣，Anytime算法才能隨計算時間的增加返回一個質量更好的解，而不僅是最新產生的解。4.遞減性Anytime算法的解質量在運行的早期提高較快，隨著計算時間的增加，解質量提高的速率會逐漸減小。中斷及可恢復性是Anytime算法最本質的特性。它能在運洲于

23、的任何時候停止，同時也能夠恢復執行，且利用Anytime算法的系統能夠在運行中重新分配計算資源。anytime algorithms (ARA*)算法在每次搜索中,根據新的因子的值,只處理在上次處理中可能不是有效的節點。首先,根據因子0的值通過A*算法來搜索路徑,只不過每個節點最多處理一次。在某次搜索過程中,如果某個節點已經擴展過,但是由于周圍于周圍節點之間邊的代價的改變需要再次處理時,不是將其再放入OPEN表中處理,而是放入INCONS表中,當本次搜索結束后,再將INCONS表中的節點全部插入OPEN表中用于下次搜索。這里INCONS表是用來保存這類已擴展過但是需要再次處理的節點。這個算法在

24、兩個方面提高了每次搜索效率。首先,由于對每個節點最多只擴展一次,所以路徑可以很快產生。其次,由于每次處理中,只處理在ICONS表中的節點,所以以前搜索的結果可以重用。因此,當因子的值在每次連續的搜索中減少時,一個相對較少計算量就可以產生新結果。在A*算法時我們了解到：f(n)=g(n)+h(n)，用d(n)表達狀態n到目標狀態的距離，那么h(n)的選取大致有如下三種情況：如果h(n)d(n)，搜索的點數少，搜索范圍小，效率高，但不能保證得到最優解。當h(n)d(n)的時候，會搜索較少的點，快速的產生一個解。ARA*算法就是用到了這一點。anytime re-planning algorithm

25、s (AD*)AD*算法就是結合了D*算法中的一升級版D*Lite算法和ARA*算法提出的一種新算法，兼顧D*Lite算法和ARA*算法的優點。在AD*算法之前已經存在了很多算法，如A*，D*，稀疏D*。由于它們有效的啟發函數和動態數據更新而被廣泛應用。D*已經被證明效率更高于A*，而稀疏D*在某些方面效率要高于D*。D*和稀疏D*都保證了最優路徑并且是動態的算法，并且都適用于導航規劃。但總體說來，稀疏D*效率要高于D*而且容易理解。但是當同時面對復雜規劃和動態環境時，將稀疏D*和ARA*結合起來，得到一種更高效的算法，即AD*算法。當邊緣代價和膨脹因子發生變化時， AD*都能處理。當出發點也

26、不斷變化時，AD*略作修改即可適應這種情況。AD*比稀疏D*和ARA*更有優勢，因為AD*僅僅在當前搜索路徑或即將導致搜索路徑矛盾時作出修改，這使得它效率更高。AD*在計算過程中盡可能的使用已經計算好的路徑，當遇到環境改變時，適當的選擇次優解，以此來提高計算速度。當環境的改變不可避免時，AD*有能力修改以往的路徑。最新的試驗結果證明，在啟發式搜索算法這個大家庭中，AD*是一種在工程應用中十分有效的算法。在動態路網，環境如權重等不斷變化的動態環境下，考慮時間等因素，在有限的時間內得到好的、可行的結果。對下面的路標圖，采用Greedy Best-First 或 A*搜索從Arad到Buchares

27、t的路徑，給出搜索過程結果。采用A*搜索搜索結果AradSibiuRimnicu VilceaPitestiBucharest總長度418實驗代碼:class A_Starprivate:int MaxWeight=10000;stack s,ss;public:void A_Search(Graph g,int v0,int vg,int H)bool flag=true;int x;:2);f=x1.2+x2.2;%優化目標函數figure(1);contour3(x1,x2,f,15);figure(2);contour(x1,x2,f,15);x2=-x1-2;%約束條件hold on

28、;plot(x1,x2,*);結果：分析：最優點為圖2中等高線與約束條件投影曲線相切點，此時約束條件滿足x1=-1,x2=-1;目標函數最優值為2，也即目標函數的最小值。設函數，函數的定義域為。（1）利用牛頓法計算初值分別為和的疊代序列。（2）給出牛頓法疊代的準確公式。答：（1）序列 （舍去）序列 （舍去）（2）XK+1=XK-9Xk-4ln(Xk-7).對圖1最短路徑問題，利用動態規劃原理求從點A到點B的最短路線。圖1 最短路徑問題程序代碼：ab=1 1 2 2 3 3 4 4 5 5 6 6 7 8 8 9 9 10 11 12 12 13 14 15;bb=2 3 4 5 5 6 7 8 8 9 9 10 11 11 12 12 13 13 14 14 15 15