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

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

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