基于目標方向約束的路徑規(guī)劃算法
【技術(shù)領域】
[0001] 本發(fā)明屬于室內(nèi)導引技術(shù)領域，具體涉及室內(nèi)（如交通樞紐、大型商場等）路徑導 引技術(shù)的基于目標方向約束的路徑規(guī)劃算法。
【背景技術(shù)】
[0002] 目前，基于室內(nèi)導引的最短路徑問題已經(jīng)有很多成熟的算法，常見的路徑規(guī)劃算 法有Di jkstra算法、Floyd算法、啟發(fā)式搜索算法等。
[0003] Di jkstra算法是最經(jīng)典的最短路徑搜索算法，亦是一種相對耗時的算法。它按權(quán) 值遞增順序求最短路徑，具有簡潔清晰的特點，算法搜索所得的結(jié)果也比較準確。從另一方 面看，Dijkstra對于輸入節(jié)點數(shù)繁多的稀疏圖，無論是計算指定兩點還是空間任意兩點都 是計算整個稀疏圖的最短路徑，具有效率低、占用空間大的缺點。
[0004] Floyd算法又稱弗洛伊德算法，是一種用于尋找給定加權(quán)路徑拓撲網(wǎng)絡中頂點間 最短路算法，其基本原理是動態(tài)規(guī)劃，它先把路徑網(wǎng)絡轉(zhuǎn)化為權(quán)值矩陣，而后在權(quán)值矩陣中 求任意兩點的最短路徑，它相較于Di jkstra算法有了很大的改進，稠密圖效果最佳，起始 點和終點的變化對算法影響不大，簡單有效，效率高于Dijkstra算法，但也存在時間復雜 度高，不適合計算大量數(shù)據(jù)的缺點。
[0005] 以Di jkstra算法、Floyd算法為代表的最短路徑算法屬于盲目搜索算法，雖然能 夠求得最短路徑，但是計算量卻非常大，適用于節(jié)點數(shù)較少的圖結(jié)構(gòu)，而對于節(jié)點數(shù)量非常 龐大的圖結(jié)構(gòu)卻不適用。
[0006] 啟發(fā)式搜索算法是基于具體領域的知識的搜索算法，在搜索的過程中，算法不僅 考慮節(jié)點的當前代價，而且考慮到了擴展該節(jié)點所需要的估計代價，使搜索過程向著最有 希望的方向前進，進而加速整個計算過程。啟發(fā)性信息主要反映在估價函數(shù)上，在搜索過程 中估價函數(shù)的任務就是估計待搜索節(jié)點處于最佳路徑上的可能性，從而優(yōu)先搜索可能性比 較大的節(jié)點，從而達到提高搜索速度的目的。
[0007] 基于啟發(fā)式搜索的最短路徑算法有局部擇優(yōu)搜索算法、最好優(yōu)先算法，以及常見 的Y算法，局部擇優(yōu)搜索算法是最簡單的啟發(fā)式搜索算法，在搜索的過程中，當某個節(jié)點被 擴展以后，"最優(yōu)"的那個節(jié)點將被進一步擴展，舍棄掉了這個子節(jié)點的父節(jié)點和其他的擴 展子節(jié)點。如果搜索過程一直繼續(xù)下去，由于舍棄許多的擴展子節(jié)點，就有可能把真正的最 佳節(jié)點都舍棄了，所以在一段過程中的最佳節(jié)點并不是全局的最佳節(jié)點，所以這種算法搜 索到的路徑不一定是真正的最佳路徑。
[0008] Y算法是啟發(fā)式搜索算法中重要的方法之一。它是一種最好優(yōu)先搜索算法，在搜 索過程中，沒有舍棄掉節(jié)點，在每一步的估價中都把當前的節(jié)點和以前的節(jié)點的估價值比 較得到一個"最佳節(jié)點"。這樣可以有效地防止"最佳節(jié)點"的丟失，提高了算法搜索結(jié)果的 準確性。在比較簡單的地圖上它的速度非?？欤芎芸煺业阶疃搪窂?。在比較復雜的地圖 中，由于在每一步擴展子節(jié)點時，把當前節(jié)點的所有可擴展的子節(jié)點都保留下來，隨著搜索 的進行，需要保留的節(jié)點數(shù)量越來越多，保留節(jié)點的數(shù)量過于龐大，導致搜索效率不高，占 用內(nèi)存資源較大，所以在一些實時性要求高的搜索中并不適用。

【發(fā)明內(nèi)容】

[0009] 本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，提供在路徑搜索過程中，只保留當前節(jié) 點與目標節(jié)點在同一方向上的可擴展節(jié)點，減少了當前節(jié)點的擴展節(jié)點的狀態(tài)空間中的節(jié) 點數(shù)量，降低了算法的搜索規(guī)模，提高了算法的搜索效率的基于目標方向約束的路徑規(guī)劃 算法。
[0010] 本發(fā)明的目的是通過以下技術(shù)方案來實現(xiàn)的：基于目標方向約束的路徑規(guī)劃算 法，其原理為：在路徑搜索過程中，只保留當前節(jié)點與目標節(jié)點在同一方向上的可擴展節(jié) 點，并將這些可擴展節(jié)點加入可擴展節(jié)點的狀態(tài)空間，對狀態(tài)空間中的每一個可擴展節(jié)點 進行評估，得到估價函數(shù)值最小的可擴展節(jié)點作為下一個當前節(jié)點，重復路徑搜索，直到狀 態(tài)空間中估價函數(shù)值最小的可擴展節(jié)點為目標節(jié)點，得到最優(yōu)路徑。
[0011] 進一步地，所述的步驟Sl中狀態(tài)空間的確定方法為：設當前節(jié)點為S，當前節(jié)點S 的可擴展節(jié)點為N個，連線當前節(jié)點S與目標節(jié)點D，分別作該N個可擴展節(jié)點在連線方向 上的投影，選取投影恰好落在連線上方向上的節(jié)點加入狀態(tài)空間。
[0012] 進一步地，所述的狀態(tài)空間的確定的具體操作方法為：設當前節(jié)點S的坐標為 (X1, Y1, Z1)，其可擴展節(jié)點X的坐標為（x2, y2, Z2)，目標節(jié)點D的坐標為（x3, y3, Z3)，分別連 線 SX、SDU
[0013]
[0014]
[0015]
[0016]
[0017] 判斷cosXSD值的大?。?br>[0018] (1)如果cosXSD <0,則可擴展節(jié)點X在連線SD上的投影在有向線段SD的反向 延長線上，則舍棄該可擴展節(jié)點X ;
[0019] (2)如果cosXSD彡0,則可擴展節(jié)點X在連線SD上的投影恰好在有向線段SD上， 保留符合該條件的可擴展節(jié)點X，并將所有的可擴展節(jié)點X形成當前節(jié)點S的可擴展節(jié)點的 狀態(tài)空間。
[0020] 進一步地，所述的步驟S2的具體實現(xiàn)方法為：針對每個可擴展節(jié)點的評估，采用 該可擴展節(jié)點的啟發(fā)式估價函數(shù)進行計算，f'（X)函數(shù)定義式為：
[0021] ?' (x) = g(x)+hr (X)
[0022] 式中，f'（X)是f (X)的估價函數(shù)，其中f (X)是從當前節(jié)點S到達目標節(jié)點D的實 際代價值，g(x)是從當前節(jié)點S到可擴展節(jié)點X的實際代價值；h'（X)是啟發(fā)函數(shù)，是h(x) 的估計函數(shù)，其中h(x)是從可擴展節(jié)點X到目標節(jié)點D的實際的最小代價值，h'（X)小于 可擴展節(jié)點X到目標節(jié)點D的實際的最小代價；
[0023] 利用上述估價函數(shù)f'（X)來衡量狀態(tài)空間中所有可擴展節(jié)點的重要程度，可擴展 節(jié)點的估價函數(shù)的值越小，該可擴展節(jié)點對尋路來講就越重要，因此最終即選取估價函數(shù) 值最小的可擴展節(jié)點，將其作為下一步待擴展的當前節(jié)點。
[0024] 具體地，所述的路徑規(guī)劃算法的具體操作方法包括以下步驟：
[0025] 步驟1、生成空的open、close表，將起始節(jié)點放入open表中；
[0026] 步驟2、判斷open表是否為空，如果open表為空，則表示沒找到路徑，失敗退出，否 貝IJ，執(zhí)行步驟3;
[0027] 步驟3、從open表中找出頭節(jié)點作為當前節(jié)點，并將其從open表中移除，存入 close表中；
[0028] 步驟4、判斷該頭結(jié)點是否是目標節(jié)點，如果是，則頭節(jié)點是終點，判斷其是否存在 父節(jié)點；若存在父節(jié)點，則在close表中找到該節(jié)點的父節(jié)點，遍歷close表直到起始節(jié)點， 找到最優(yōu)路徑，算法結(jié)束；若不存在父節(jié)點，則算法結(jié)束；若該頭結(jié)點不是目標節(jié)點則執(zhí)行 步驟5 ;
[0029] 步驟5、判斷該頭節(jié)點是否為可擴展節(jié)點，若是則進行步驟6,若不是則返回步驟 2 ；
[0030] 步驟6、將該頭節(jié)點進行擴展，找到其可擴展節(jié)點，連線該頭節(jié)點與目標節(jié)點，作可 擴展節(jié)點在連線方向上的投影，選擇投影在連線上的可擴展節(jié)點構(gòu)成集合V;
[0031] 步驟7、遍歷集合V中的可擴展節(jié)點，如果可擴展節(jié)點既不在open表中，又不在 close表中，將該可擴展節(jié)點加入open表中，并計算該可擴展節(jié)點的估價函數(shù)，將擴展出這 些可擴展節(jié)點的頭節(jié)點定義為其可擴展節(jié)點的父節(jié)點；
[0032] 步驟8、如果可擴展節(jié)點在open表中，則該可擴展節(jié)點在open表中原本已經(jīng)存在 一個估價函數(shù)和一個父節(jié)點，比較該可擴展節(jié)點的估價函數(shù)和open表中原有的估價函數(shù) 大小，如果可擴展節(jié)點的估價函數(shù)小于open表中原有的估價函數(shù)，就更新該可擴展節(jié)點在 open表中的估價函數(shù)和父節(jié)點，若不小于則不操作；如果可擴展節(jié)點在close表中，則不做 處理，繼續(xù)判斷其它可擴展節(jié)點；
[0033] 步驟9、按照估價函數(shù)值遞增的順序，對open表中的所有節(jié)點進行排序，返回步驟 3〇
[0034] 進一步地，所述的步驟3中open表中的第一個頭節(jié)點為起始節(jié)點，以后每一個頭 節(jié)點即為open表中估價函數(shù)值最小的節(jié)點。
[0035] 本發(fā)明的有益效果是：與現(xiàn)有的算法相比，本發(fā)明在路徑搜索過程中，不保留當前 節(jié)點的所有可擴展節(jié)點，只保留當前節(jié)點與目標節(jié)點在同一方向上的可擴展節(jié)點，減少了 當前節(jié)點的擴展節(jié)點的狀態(tài)空間中的節(jié)點數(shù)量，降低了算法的搜索規(guī)模，減少了內(nèi)存資源 的占用，提高了算法的搜索效率，適用于各種場景的路徑搜索，特別適用于實時性要求高的 場景的路徑搜索。
【附圖說明】
[0036] 圖1為本發(fā)明的本發(fā)明的路徑搜索示意圖；
[0037] 圖2為本發(fā)明的具體實施例的算法流程圖。
【具體實施方式】
[0038] 下面結(jié)合附圖進一步說明本發(fā)明的技術(shù)方案。
[0039] 基于目標方向約束的路徑規(guī)劃算法，基于目標方向約束的路徑規(guī)劃算法，其算法 原理為：在路徑搜索過程中，只保留當前節(jié)點（起始節(jié)點為第一個當前節(jié)點）與目標節(jié)點在 同一方向上的可擴展節(jié)點，并將這些可擴展節(jié)點加入可擴展節(jié)點的狀態(tài)空間，對狀態(tài)空間 中的每一個可擴展節(jié)點進行評估，得到估價函數(shù)值最小的可擴展節(jié)點作為下一個當前節(jié)點 (作為當前節(jié)點的節(jié)點已經(jīng)不在狀態(tài)空間中了），重復路徑搜索，直到狀態(tài)空間中估價函數(shù) 值最小的可擴展節(jié)點為目標節(jié)點，得到最優(yōu)路徑。
[0040] 根據(jù)幾何學可知，兩點之間直線距離最短，所以在道路網(wǎng)絡拓撲中對給定兩點進 行路徑規(guī)劃時，從起始節(jié)點到目標節(jié)點的連線方向，基本上代表了最短路徑的大致走向。也 就是說，最終的最短路徑基本是在兩節(jié)點連線的兩側(cè)，而且通常在其附近，所以對算法搜索 的范圍在連線兩側(cè)進行目標方向約束，即對每一個節(jié)點擴展的時候，連接當前節(jié)點與目標 節(jié)點的連線，對當前節(jié)點的可擴展節(jié)點在連線方向上的作投影，若可擴展節(jié)點的投影恰好 落在連線上，保留該可擴展節(jié)點：設當前節(jié)點為S，當前節(jié)點S的可擴展節(jié)點為N個，連線當 前節(jié)點S與目標節(jié)點D，分別作該N個可擴展節(jié)點在連線方向上的投影，選取投影恰好落在 連線上方向上的節(jié)點加入狀態(tài)空間。如圖1所示，比如在當前節(jié)點S時，它的可擴展的節(jié)點 有六個1、2、3、6、7、8。連線當前節(jié)點S與目標節(jié)點D，然后分別作該六個節(jié)點在連線方向上 的投影，則節(jié)點6、7、8的投影恰好落在連線上，而節(jié)點1、2、3的投影落在了連線的反向延長 線上，選取投影恰好落在連線上的節(jié)點。具體操作方法為：設當前節(jié)點S的坐標為（Xl，yi， Z1)，其可擴展節(jié)點X的坐標為（x2, y2, z2)