路徑確定方法、裝置和導(dǎo)航儀的制作方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種路徑確定方法、裝置和導(dǎo)航儀。該路徑確定方法包括獲取待確定路徑的起始點(diǎn)和終止點(diǎn)；獲取起始點(diǎn)和終止點(diǎn)對(duì)應(yīng)區(qū)域內(nèi)的障礙物數(shù)據(jù)，其中，每個(gè)障礙物采用一個(gè)多邊形表達(dá)，障礙物數(shù)據(jù)為各個(gè)多邊形對(duì)應(yīng)的數(shù)據(jù)；根據(jù)起始點(diǎn)、終止點(diǎn)和障礙物數(shù)據(jù)確定對(duì)應(yīng)區(qū)域的通視圖；計(jì)算通視圖的鄰接成本矩陣；以及根據(jù)鄰接成本矩陣確定從起始點(diǎn)到達(dá)終止點(diǎn)的最短路徑。通過(guò)本發(fā)明，基于通視圖確定路徑，將無(wú)路網(wǎng)區(qū)域的路徑確定過(guò)程轉(zhuǎn)化為有路網(wǎng)區(qū)域的路徑確定，路徑搜索準(zhǔn)確性高。
【專(zhuān)利說(shuō)明】路徑確定方法、裝置和導(dǎo)航儀

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及路徑規(guī)劃【技術(shù)領(lǐng)域】，具體而言，特別涉及一種路徑確定方法、裝置和導(dǎo) 航儀。

【背景技術(shù)】
[0002] 隨著汽車(chē)的普及和道路的建設(shè)，車(chē)載GPS導(dǎo)航儀成為汽車(chē)的基本裝備，這類(lèi)導(dǎo)航 儀確定的都是有路狀態(tài)下起始點(diǎn)至終止點(diǎn)的可達(dá)路徑。然而，很多情況下，在一些無(wú)路網(wǎng)的 區(qū)域，也需要尋求最優(yōu)的可達(dá)路徑，此時(shí)主要考慮路徑如何繞開(kāi)障礙物，如湖泊，懸崖，樓房 等等。
[0003] 目前，常用的無(wú)路網(wǎng)區(qū)域避障方法主要包括以下兩種：
[0004] 第一，基于李氏迷宮算法的避障方法。其基本思想可以描述為波的傳播過(guò)程的模 擬。在一個(gè)存在障礙的湖面上，若需尋找連接A，B兩點(diǎn)的最小路徑，可以在A點(diǎn)投下一枚 石子，然后觀察所引起的水波傳播情況。假定"水波"傳播時(shí)能量無(wú)損失，當(dāng)遇到障礙時(shí)， 波產(chǎn)生反射，最先到達(dá)的終止點(diǎn)波前所經(jīng)過(guò)的路徑必定是一條最短距離。而且只要兩點(diǎn)間 有通路存在，則自A點(diǎn)擴(kuò)散出去的波一定能傳播到B點(diǎn)。
[0005] 這種基于李氏迷宮算法的避障方法類(lèi)似枚舉，其搜索效率比較低，而且隨著搜索 的深入，算法復(fù)雜度越來(lái)越高，從而數(shù)據(jù)處理速度慢，資源消耗大。
[0006] 第二，基于線探索算法的避障方法。該方法首先將給定的起始點(diǎn)和終止點(diǎn)連起來(lái)， 得到線段SE，從S指向E的射線SE為探索線，沿探索線探索前方是否有障礙物，具體地，探 索是否存在障礙物的過(guò)程如下：找出與SE相交的第一個(gè)障礙物集合0,如果O不存在，則 SE即為所求的解，否則在O上找到登陸點(diǎn)，繞過(guò)0。如果O的分離點(diǎn)D不能直達(dá)終止點(diǎn) E，中間存在障礙物(V，則求出以D為出發(fā)點(diǎn)，E為終止點(diǎn)時(shí)(V上的登陸點(diǎn)VC。然后 用與上述同樣的方法求出D到VC'的通路和VC'到E的通路。如圖1所示，該區(qū)域內(nèi)存在 a、b、c、d四個(gè)障礙物，沿探索線SEl探索前方無(wú)障礙物，沿探索線SE2探索前方具有障礙物 b、c、d〇
[0007] 這種基于線探索算法的避障方法，類(lèi)似于一種啟發(fā)式深度搜索，該方法搜索效率 比較高，但準(zhǔn)確性比較差，尋找到的路徑并不一定是最優(yōu)的路徑。
[0008] 綜上所述，現(xiàn)有的用于無(wú)路網(wǎng)區(qū)域的路徑確定方法準(zhǔn)確性差的問(wèn)題，目前尚未提 出有效的解決方法。


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

[0009] 本發(fā)明的主要目的在于提供一種路徑確定方法、裝置和導(dǎo)航儀，以解決現(xiàn)有技術(shù) 中用于無(wú)路網(wǎng)區(qū)域的路徑確定方法準(zhǔn)確性差的問(wèn)題。
[0010] 依據(jù)本發(fā)明的一個(gè)方面，提供了一種路徑確定方法。
[0011] 根據(jù)本發(fā)明的路徑確定方法包括：獲取待確定路徑的起始點(diǎn)和終止點(diǎn)；獲取起始 點(diǎn)和終止點(diǎn)對(duì)應(yīng)區(qū)域內(nèi)的障礙物數(shù)據(jù)，其中，每個(gè)障礙物采用一個(gè)多邊形表達(dá)，障礙物數(shù)據(jù) 為各個(gè)多邊形對(duì)應(yīng)的數(shù)據(jù)；根據(jù)起始點(diǎn)、終止點(diǎn)和障礙物數(shù)據(jù)確定對(duì)應(yīng)區(qū)域的通視圖；計(jì) 算通視圖的鄰接成本矩陣；以及根據(jù)鄰接成本矩陣確定從起始點(diǎn)到達(dá)終止點(diǎn)的最短路徑。
[0012] 進(jìn)一步地，設(shè)通視圖G= (V，E)，E為與任何多邊形均不相交的邊的全體， V=IviIi= 1，2,…，n}，V1S起始點(diǎn)，Vn為終止點(diǎn)，V2、VfVlri為各個(gè)多邊形的 頂點(diǎn)，Vi的坐標(biāo)為（Xi，yi)，Vj的坐標(biāo)為（Xj，yP，則計(jì)算通視圖的鄰接成本矩陣為： cost[i] [j] = (!(Vi,Vj)，其中，若VpVj屬于同一個(gè)多邊形，則當(dāng)Vi、Vj為相鄰頂點(diǎn)時(shí)， ?'卜:.，'.)=#'-.r;)2 +(乃―.匕.)2 ;當(dāng)Vi、vJ為同一頂點(diǎn)時(shí)，d(v^^ 若Vi、\不屬于同一個(gè)多邊形，則當(dāng)Vi^兩點(diǎn)構(gòu)成的線段與任意一個(gè)多邊形的邊均不相交 時(shí)，?/(ν,.,V,) = - ..Vy)2 +(>·,. -V7-)2 ;否則d(Vi,Vj) = ，其中，cost[i][j]為鄰接成本矩陣 的第i行第j列個(gè)元素。
[0013] 進(jìn)一步地，若VpVj均屬于第一多邊形，其中，第一多邊形為對(duì)應(yīng)區(qū)域內(nèi)任意一個(gè) 障礙物對(duì)應(yīng)的多邊形，在計(jì)算d(Vi，Vj)之前，方法還包括：判斷第一多邊形是否為凹多邊 形；若第一多邊形為凹多邊形，則將凹多邊形轉(zhuǎn)化為凸多邊形。
[0014] 進(jìn)一步地，若VpVj均屬于第二多邊形，其中，第二多邊形為對(duì)應(yīng)區(qū)域內(nèi)任意一個(gè) 障礙物對(duì)應(yīng)的多邊形，在計(jì)算d(Vi，Vj)之前，該方法還包括：判斷第二多邊形是否為凹多邊 形；若第二多邊形為凹多邊形，則當(dāng)Vp'為相鄰頂點(diǎn)時(shí)或Vi、'為不相同不相鄰的頂點(diǎn)且 Vi、Vj構(gòu)成的線段與任意一個(gè)多邊形的各邊均不相交時(shí)，?/(V,.,Vj)= -.XiY+(V·. - r)2， 當(dāng)Vi、Vj為同一頂點(diǎn)時(shí)，d(VpVj) =0,否則=〇°。
[0015] 進(jìn)一步地，每個(gè)障礙物采用凸多邊形最小面積四邊形包圍盒算法表達(dá)。
[0016] 進(jìn)一步地，根據(jù)鄰接成本矩陣確定從起始點(diǎn)到達(dá)終止點(diǎn)的最短路徑包括：根據(jù)鄰 接成本矩陣通過(guò)弗洛伊德算法、迪杰斯特拉算法或基于堆的雙向路徑搜索算法確定從起始 點(diǎn)到達(dá)終止點(diǎn)的最短路徑。
[0017] 進(jìn)一步地，根據(jù)鄰接成本矩陣通過(guò)基于堆的雙向路徑搜索算法確定最短路徑包括 以下步驟：步驟Sl:去除鄰接成本矩陣中值為無(wú)窮大的元素得到第一弧段集，將正堆、反堆 初始化為空；步驟S2 :將第一弧段集中與起始點(diǎn)相關(guān)的弧段置入正堆，將第一弧段集中與 終止點(diǎn)相關(guān)的弧段置入反堆；步驟S3 :判斷正堆是否為空，若正堆為空時(shí)，輸出查找結(jié)束的 信息，若正堆不為空時(shí)，執(zhí)行步驟S4;步驟S4:將當(dāng)前正堆得到的第一弧段出堆，其中，任一 個(gè)弧段入正堆后，均會(huì)得到由起始點(diǎn)出發(fā)連接這一弧段的路徑，第一弧段入正堆后得到的 路徑為當(dāng)前正堆中各弧段得到的路徑中的最小路徑；步驟S5 :查找反堆判斷是否找到最短 路徑，若找到，則輸出查找成功的信息，若沒(méi)有找到，則執(zhí)行步驟S6 ;步驟S6 :在第一弧段集 中獲取根據(jù)第一弧段擴(kuò)展得到的第一弧段子集，其中，第一弧段子集中的弧段均與第一弧 段的終點(diǎn)相關(guān)；步驟S7 :將第一弧段子集中的各弧段置入正堆，并執(zhí)行步驟S8 ;步驟S8 :判 斷反堆是否為空，若反堆為空時(shí)，輸出查找結(jié)束的信息，若反堆不為空時(shí)，執(zhí)行步驟S9;步 驟S9 :將當(dāng)前反堆得到的第二弧段出堆，其中，任一個(gè)弧段入反堆后，均會(huì)得到由終止點(diǎn)出 發(fā)連接這一弧段的路徑，第二弧段入反堆后得到的路徑為當(dāng)前反堆中各弧段得到的路徑中 的最小路徑；步驟SlO:查找正堆判斷是否找到最短路徑，若找到，則輸出查找成功的信息， 若沒(méi)有找到，則執(zhí)行步驟Sll;步驟Sll:在第一弧段集中獲取由第二弧段擴(kuò)展得到的第二 弧段子集，其中，第二弧段子集中的弧段均與第二弧段對(duì)應(yīng)的終點(diǎn)相關(guān)；步驟S12 :將第二 弧段子集中的各弧段置入反堆，并執(zhí)行步驟S3。
[0018] 進(jìn)一步地，若對(duì)應(yīng)區(qū)域?yàn)榉忾]區(qū)域時(shí)，該方法還包括：獲取封閉區(qū)域的邊界；根據(jù) 起始點(diǎn)、終止點(diǎn)和障礙物數(shù)據(jù)確定對(duì)應(yīng)區(qū)域的通視圖包括：根據(jù)起始點(diǎn)、終止點(diǎn)、障礙物數(shù) 據(jù)和邊界確定對(duì)應(yīng)區(qū)域的通視圖。
[0019] 依據(jù)本發(fā)明的另一個(gè)方面，提供了一種路徑確定裝置。該裝置用于執(zhí)行本發(fā)明提 供的任意一種路徑確定方法。
[0020] 根據(jù)本發(fā)明的路徑確定裝置包括：第一獲取模塊，用于獲取待確定路徑的起始點(diǎn) 和終止點(diǎn)；第二獲取模塊，用于獲取起始點(diǎn)和終止點(diǎn)對(duì)應(yīng)區(qū)域內(nèi)的障礙物數(shù)據(jù)，其中，每個(gè) 障礙物采用一個(gè)多邊形表達(dá)，障礙物數(shù)據(jù)為各個(gè)多邊形對(duì)應(yīng)的數(shù)據(jù)；第一確定模塊，用于根 據(jù)起始點(diǎn)、終止點(diǎn)和障礙物數(shù)據(jù)確定對(duì)應(yīng)區(qū)域的通視圖；計(jì)算模塊，用于計(jì)算通視圖的鄰接 成本矩陣；以及第二確定模塊，用于根據(jù)鄰接成本矩陣確定從起始點(diǎn)到達(dá)終止點(diǎn)的最短路 徑。
[0021] 進(jìn)一步地，設(shè)通視圖G= (V，E)，E為與任何多邊形均不相交的邊的全體，V= IviIi=1，2,…，n}，V1為起始點(diǎn)，Vn為終止點(diǎn)，V2、VfVlri為各個(gè)多邊形的頂點(diǎn)，Vi的 坐標(biāo)為（Xpyi)，Vj的坐標(biāo)為（Xj，yj)，則計(jì)算模塊采用以下方法計(jì)算通視圖的鄰接成本矩 陣：cost[i] [j] = (!(Vi,Vj)，其中，若Vi、Vj屬于同一個(gè)多邊開(kāi)多，則當(dāng)Vi、Vj為相鄰頂點(diǎn)時(shí)， d(I"'/,V；) ^yj(x；-Xi)2+(.Vv-Vy)";當(dāng)VpVj為同一頂點(diǎn)時(shí)，(!(Vi,Vj) = 0 ;否則(!(Vi,Vj) =〇°， 若Vi、t不屬于同一個(gè)多邊形，則當(dāng)Vi^兩點(diǎn)構(gòu)成的線段與任意一個(gè)多邊形的邊均不相交 時(shí)，=A' )2 + (.U，,)：；否則d(Vi,Vj) =°°，其中，cost[i] [j]為鄰接成本矩陣 的第i行第j列個(gè)元素。
[0022]進(jìn)一步地，若Vi、Vj均屬于第一多邊形，其中，第一多邊形為對(duì)應(yīng)區(qū)域內(nèi)任意一個(gè) 障礙物對(duì)應(yīng)的多邊形，計(jì)算模塊還包括：第一判斷子模塊，用于在計(jì)算(Kvi,Vj)之前，判斷 第一多邊形是否為凹多邊形；轉(zhuǎn)化子模塊，用于當(dāng)?shù)谝欢噙呅螢榘级噙呅危瑢级噙呅无D(zhuǎn)化 為凸多邊形。
[0023] 進(jìn)一步地，若VpVj均屬于第二多邊形，其中，第二多邊形為對(duì)應(yīng)區(qū)域內(nèi)任意一個(gè) 障礙物對(duì)應(yīng)的多邊形，計(jì)算模塊還包括：第二判斷子模塊，用于在計(jì)算(Kvi,Vj)之前，判斷 第二多邊形是否為凹多邊形；計(jì)算子模塊，用于采用以下方法計(jì)算d(Vi，'):若第二多邊形 為凹多邊形，則當(dāng)Vp'為相鄰頂點(diǎn)時(shí)或Vi、'為不相同不相鄰的頂點(diǎn)且Vi、'構(gòu)成的線段 與任意一個(gè)多邊形的各邊均不相交時(shí)，d(vt,Vj)=^j(xt-Xj)2+ (Vi -y})2，當(dāng)Vi、Vj為同一頂 點(diǎn)時(shí)，(I(VDVj) =0,否則(Kvi,Vj) =〇°。
[0024] 進(jìn)一步地，每個(gè)障礙物采用凸多邊形最小面積四邊形包圍盒算法表達(dá)。
[0025] 進(jìn)一步地，第二確定模塊還用于根據(jù)鄰接成本矩陣通過(guò)弗洛伊德算法、迪杰斯特 拉算法或基于堆的雙向路徑搜索算法確定從起始點(diǎn)到達(dá)終止點(diǎn)的最短路徑。
[0026] 進(jìn)一步地，第二確定模塊根據(jù)鄰接成本矩陣通過(guò)基于堆的雙向路徑搜索算法確定 最短路徑時(shí)，執(zhí)行以下步驟：步驟Sl:去除鄰接成本矩陣中值為無(wú)窮大的元素得到第一弧 段集，將正堆、反堆初始化為空；步驟S2 :將第一弧段集中與起始點(diǎn)相關(guān)的弧段置入正堆， 將第一弧段集中與終止點(diǎn)相關(guān)的弧段置入反堆；步驟S3 :判斷正堆是否為空，若正堆為空 時(shí)，輸出查找結(jié)束的信息，若正堆不為空時(shí)，執(zhí)行步驟S4;步驟S4:將當(dāng)前正堆得到的第一 弧段出堆，其中，任一個(gè)弧段入正堆后，均會(huì)得到由起始點(diǎn)出發(fā)連接這一弧段的路徑，第一 弧段入正堆后得到的路徑為當(dāng)前正堆中各弧段得到的路徑中的最小路徑；步驟S5 :查找反 堆判斷是否找到最短路徑，若找到，則輸出查找成功的信息，若沒(méi)有找到，則執(zhí)行步驟S6; 步驟S6:在第一弧段集中獲取根據(jù)第一弧段擴(kuò)展得到的第一弧段子集，其中，第一弧段子 集中的弧段均與第一弧段的終點(diǎn)相關(guān)；步驟S7 :將第一弧段子集中的各弧段置入正堆，并 執(zhí)行步驟S8 ;步驟S8 :判斷反堆是否為空，若反堆為空時(shí)，輸出查找結(jié)束的信息，若反堆不 為空時(shí)，執(zhí)行步驟S9 ;步驟S9 :將當(dāng)前反堆得到的第二弧段出堆，其中，任一個(gè)弧段入反堆 后，均會(huì)得到由終止點(diǎn)出發(fā)連接這一弧段的路徑，第二弧段入反堆后得到的路徑為當(dāng)前反 堆中各弧段得到的路徑中的最小路徑；步驟SlO:查找正堆判斷是否找到最短路徑，若找 到，則輸出查找成功的信息，若沒(méi)有找到，則執(zhí)行步驟Sll;步驟Sll:在第一弧段集中獲取 由第二弧段擴(kuò)展得到的第二弧段子集，其中，第二弧段子集中的弧段均與第二弧段對(duì)應(yīng)的 終點(diǎn)相關(guān)；步驟S12:將第二弧段子集中的各弧段置入反堆，并執(zhí)行步驟S3。
[0027] 進(jìn)一步地，若對(duì)應(yīng)區(qū)域?yàn)榉忾]區(qū)域時(shí)，該裝置還包括：第三獲取模塊，用于獲取封 閉區(qū)域的邊界；第二確定模塊還用于根據(jù)起始點(diǎn)、終止點(diǎn)、障礙物數(shù)據(jù)和邊界確定對(duì)應(yīng)區(qū)域 的通視圖。
[0028] 依據(jù)本發(fā)明的再一個(gè)方面，提供了一種導(dǎo)航儀，該導(dǎo)航儀包括本發(fā)明提供的任意 一種路徑確定裝置。
[0029] 通過(guò)本發(fā)明，在確定起始點(diǎn)和終止點(diǎn)之間最短路徑時(shí)，將起始點(diǎn)和終止點(diǎn)對(duì)應(yīng)區(qū) 域內(nèi)的障礙物用多邊形表達(dá)，然后根據(jù)起始點(diǎn)、終止點(diǎn)和多邊形確定這一區(qū)域的通視圖， 最后根據(jù)通視圖的鄰接成本矩陣確定從起始點(diǎn)到達(dá)終止點(diǎn)的最短路徑，這種基于通視圖的 路徑確定方法，將無(wú)路網(wǎng)區(qū)域的路徑確定過(guò)程轉(zhuǎn)化為有路網(wǎng)區(qū)域的路徑確定，從而能夠準(zhǔn) 確的搜索到從起始點(diǎn)到達(dá)終止點(diǎn)的最短路徑，解決了現(xiàn)有技術(shù)中用于無(wú)路網(wǎng)區(qū)域的路徑確 定方法準(zhǔn)確性差的問(wèn)題。
[0030] 上述說(shuō)明僅是本發(fā)明技術(shù)方案的概述，為了能夠更清楚了解本發(fā)明的技術(shù)手段， 而可依照說(shuō)明書(shū)的內(nèi)容予以實(shí)施，并且為了讓本發(fā)明的上述和其它目的、特征和優(yōu)點(diǎn)能夠 更明顯易懂，以下特舉本發(fā)明的【具體實(shí)施方式】。

【專(zhuān)利附圖】

【附圖說(shuō)明】
[0031] 通過(guò)閱讀下文優(yōu)選實(shí)施方式的詳細(xì)描述，各種其他的優(yōu)點(diǎn)和益處對(duì)于本領(lǐng)域普通 技術(shù)人員將變得清楚明了。附圖僅用于示出優(yōu)選實(shí)施方式的目的，而并不認(rèn)為是對(duì)本發(fā)明 的限制。而且在整個(gè)附圖中，用相同的參考符號(hào)表示相同的部件。在附圖中：
[0032] 圖1是根據(jù)現(xiàn)有技術(shù)的基于線探索算法的避障方法的示意圖；
[0033] 圖2是根據(jù)本發(fā)明實(shí)施例的路徑確定方法的流程圖；
[0034] 圖3是根據(jù)本發(fā)明實(shí)施例的采用多邊形表達(dá)障礙物的示意圖；
[0035] 圖4是根據(jù)本發(fā)明實(shí)施例的通視圖一的示意圖；
[0036] 圖5是根據(jù)本發(fā)明實(shí)施例的凸多邊形最小面積四邊形包圍盒的示意圖；
[0037] 圖6是根據(jù)本發(fā)明實(shí)施例的封閉區(qū)域內(nèi)路徑確定的示意圖
[0038] 圖7是根據(jù)本發(fā)明實(shí)施例的路徑確定方法中基于堆的雙向路徑搜索算法的流程 圖；
[0039] 圖8是根據(jù)本發(fā)明實(shí)施例的通視圖二的示意圖；
[0040] 圖9是根據(jù)本發(fā)明實(shí)施例的最短路徑的示意圖。
[0041]
[0042]

【具體實(shí)施方式】
[0043] 下面結(jié)合附圖和【具體實(shí)施方式】對(duì)本發(fā)明做進(jìn)一步說(shuō)明。需要指出的是，在不沖突 的情況下，本申請(qǐng)中的實(shí)施例及實(shí)施例中的特征可以相互組合。
[0044] 在對(duì)描述本申請(qǐng)的實(shí)施例之前，首先進(jìn)行如下的定義，以下定義適用于描述本申 請(qǐng)的所有實(shí)施例。
[0045] 定義 3. 1 設(shè)Vi=(Xi,yj(i= 1，2, -",m),V1=Vm+1 是平面R2上構(gòu)成某多邊形Pi 的m個(gè)頂點(diǎn)，如果對(duì)任意i，j，i乒j且i,j= 1，2,…，m，線段ViVw與線段V%+1相鄰且相 交于一端點(diǎn)或不相交時(shí)，則稱(chēng)多邊形Pi為簡(jiǎn)單多邊形。簡(jiǎn)單多邊形集合記為P= {P」i= 1，2, "·Κ}，K為簡(jiǎn)單多邊形的個(gè)數(shù)，簡(jiǎn)單多邊形？1可表示為Pi= {v」j= 1，2,…m}。
[0046] 沿力一v2―…一Vni-V1方向，簡(jiǎn)單多邊形Pi的有界區(qū)域總在左邊，稱(chēng)此方向?yàn)?逆時(shí)針?lè)较颍ㄕ颍?br> [0047] 定義3. 2平面R2內(nèi)障礙物集合O是簡(jiǎn)單多邊形集合P的子集，即OeP。每個(gè)障 礙物OiG〇,i= 1，2,…K，每個(gè)障礙物對(duì)應(yīng)的各簡(jiǎn)單多邊形的頂點(diǎn)數(shù)^可以不同，且i辛j 時(shí)，QnO, = 0，其中i，j=u，···，!^
[0048] 定義3. 3s，geR2分別稱(chēng)為起始點(diǎn)和終止點(diǎn)。點(diǎn)集V= {s，g}UV。，其中V。表示 全體障礙物頂點(diǎn)。為了敘述方便，設(shè)V=IviIi= 1，2,…，η}，其中V1=s,Vn=g，而V2、 vIri對(duì)應(yīng)全體障礙物頂點(diǎn)V。的一個(gè)序列；設(shè)〇i=is}，〇k= {g}。
[0049] 定義3· 4在有障礙物的平面R2上，定義G是由V構(gòu)成的有權(quán)圖。邊的權(quán)（成本） 值定義為V集兩頂點(diǎn)間的擴(kuò)展歐幾里得（Euclid)距離，表示為d(Vi，Vj)，其中Vi，VjeV。 定義cost[η] [η]為圖G的鄰接成本矩陣，其中cost[i] [j] = (!(Vi,Vj)。
[0050] 定義3. 5設(shè)G= (V，E)，V如定義3. 3，E為與任何多邊形均不相交的邊的全體。圖 G稱(chēng)為通視圖，因?yàn)閳D中相鄰的頂點(diǎn)能相互看到。
[0051] 定義3. 6使H1Vi (其中s=vdl,g=vd2;vieV，以{孔舊})為最小值的路線 上全體點(diǎn)構(gòu)成由s-g的最短避障路徑。
[0052] 在以上定義的基礎(chǔ)上，下面將具體描述本發(fā)明提供的實(shí)施例。
[0053] 圖2是根據(jù)第一實(shí)施例的路徑確定方法的流程圖，該方法用于確定無(wú)路網(wǎng)區(qū)域內(nèi) 的路徑，如圖2所示，該方法包括如下的步驟S102至步驟S110。
[0054] 步驟S102 :獲取待確定路徑的起始點(diǎn)和終止點(diǎn)。
[0055] 步驟S104 :獲取起始點(diǎn)和終止點(diǎn)對(duì)應(yīng)區(qū)域內(nèi)的障礙物數(shù)據(jù)。
[0056] 其中，將起始點(diǎn)和終止點(diǎn)對(duì)應(yīng)的區(qū)域內(nèi)的障礙物采用多邊形包圍盒來(lái)表達(dá)，將每 個(gè)障礙物表達(dá)為一個(gè)多邊形，障礙物數(shù)據(jù)為各個(gè)多邊形對(duì)應(yīng)的數(shù)據(jù)，也即各個(gè)多邊形的頂 點(diǎn)和邊。在起始點(diǎn)S和終止點(diǎn)g對(duì)應(yīng)區(qū)域內(nèi)存在兩個(gè)障礙物OdPO2，分別用多邊形表達(dá)后 如圖3所示。
[0057] 步驟S106 :根據(jù)起始點(diǎn)、終止點(diǎn)和障礙物數(shù)據(jù)確定對(duì)應(yīng)區(qū)域的通視圖。
[0058] 通視圖G= (V，E)如上文中定義3. 5,以圖3所示的起始點(diǎn)、終止點(diǎn)和障礙物為例， 根據(jù)起始點(diǎn)、終止點(diǎn)和障礙物數(shù)據(jù)確定對(duì)應(yīng)區(qū)域的通視圖如圖4所示。
[0059] 其中，可根據(jù)某一固定區(qū)域內(nèi)的障礙物數(shù)據(jù)預(yù)先建立一個(gè)通視圖，該通視圖是唯 一的，當(dāng)獲取到不同的起始點(diǎn)、終止點(diǎn)后，在該已建立的通視圖的基礎(chǔ)上加入起始點(diǎn)和終止 點(diǎn)得到一個(gè)臨時(shí)的通視圖用于路徑的確定，從而對(duì)于固定障礙物的區(qū)域，能夠減少數(shù)據(jù)處 理量。
[0060] 或者，在獲取到起始點(diǎn)、終止點(diǎn)之后，再實(shí)時(shí)獲取當(dāng)前的障礙物數(shù)據(jù)建立通視圖， 能夠?qū)崟r(shí)將障礙物變化反映到路徑規(guī)劃過(guò)程中。
[0061] 步驟S108 :計(jì)算通視圖的鄰接成本矩陣。
[0062] 得到通視圖G= (V，E)后，構(gòu)建G= (V，E)的鄰接成本矩陣（也即鄰接矩陣）。
[0063] 步驟SllO:根據(jù)鄰接成本矩陣確定從起始點(diǎn)到達(dá)終止點(diǎn)的最短路徑。
[0064] 具體地，可采用弗洛伊德算法或迪杰斯特拉算法根據(jù)鄰接成本矩陣確定從起始點(diǎn) 到達(dá)終止點(diǎn)的最短路徑。
[0065] 弗洛伊德算法（Floyd算法）又稱(chēng)為插點(diǎn)法，是一種用于尋找給定的加權(quán)圖中多源 點(diǎn)之間最短路徑的算法。該算法能夠通過(guò)一個(gè)圖的權(quán)值矩陣求出它的每?jī)牲c(diǎn)間的最短路徑 矩陣，但是時(shí)間復(fù)雜度比較高，不適合計(jì)算大量數(shù)據(jù)。
[0066] 迪杰斯特拉算法是計(jì)算從一個(gè)頂點(diǎn)到其余各頂點(diǎn)的最短路徑算法，解決的是有向 圖中最短路徑問(wèn)題，其主要特點(diǎn)是以起始點(diǎn)為中心向外層層擴(kuò)展，直到擴(kuò)展到終點(diǎn)為止，但 是這種算法的效率低、運(yùn)算中占用空間大。
[0067] 基于堆的雙向路徑分析算法是基于迪杰斯特拉算法的變種，它能在不影響精確度 的前提下提高運(yùn)算效率。由于該算法在搜尋最短路徑時(shí)，采用正向和反向，也即雙向擴(kuò)展的 方式，因而，該算法的運(yùn)算效率高于基于迪杰斯特拉算法。
[0068] 米用該實(shí)施例提供的路徑的確定方法，在確定起始點(diǎn)和終止點(diǎn)之間最短路徑時(shí)， 將起始點(diǎn)和終止點(diǎn)對(duì)應(yīng)區(qū)域內(nèi)的障礙物用多邊形表達(dá)，然后根據(jù)起始點(diǎn)、終止點(diǎn)和多邊形 確定這一區(qū)域的通視圖，最后根據(jù)通視圖的鄰接成本矩陣確定從起始點(diǎn)到達(dá)終止點(diǎn)的最短 路徑，這種基于通視圖的路徑確定方法，將無(wú)路網(wǎng)區(qū)域的路徑確定過(guò)程轉(zhuǎn)化為有路網(wǎng)區(qū)域 的路徑確定，從而能夠準(zhǔn)確的搜索到從起始點(diǎn)到達(dá)終止點(diǎn)的最短路徑。
[0069] 優(yōu)選地，在步驟S108中，采用以下方法計(jì)算通視圖的鄰接成本矩陣。設(shè)通視圖G =(V，E),E為與任何多邊形均不相交的邊的全體，V=IviIi= 1，2,…，n}，起始點(diǎn)，Vn 為終止點(diǎn)，VyVfvIri為各個(gè)多邊形的頂點(diǎn)，Vi的坐標(biāo)為（Xi，Yi)，vj的坐標(biāo)為（Xj，yp，則計(jì) 算通視圖的鄰接成本矩陣為：cost[i] [j] = (!(Vi,Vj)。
[0070] 具體地，在計(jì)算d(Vi，')時(shí)，需要解決兩類(lèi)鄰接關(guān)系，也即同一障礙物多邊形頂點(diǎn) 間的鄰接關(guān)系和不同障礙物多邊形頂點(diǎn)間的鄰接關(guān)系，在該優(yōu)選實(shí)施例中，這兩類(lèi)鄰接關(guān) 系均通過(guò)判斷V中頂點(diǎn)連線，即邊（Vi，Vj)，與障礙物多邊形Opi= 1，2，···Κ是否相交來(lái)解 決，具體計(jì)算方法如下：
[0071]第一，若VpVj屬于同一個(gè)多邊開(kāi)多，也即V#V且Vi, 〇k，k=1，2,…，Κ,采用 以下公式計(jì)算邊（Vi,Vj) (j=i,i+1,…mk,mk+1)的權(quán)值d(Vi,Vj):
[0072]

【權(quán)利要求】
1. 一種路徑確定方法，其特征在于，包括： 獲取待確定路徑的起始點(diǎn)和終止點(diǎn)； 獲取所述起始點(diǎn)和所述終止點(diǎn)對(duì)應(yīng)區(qū)域內(nèi)的障礙物數(shù)據(jù)，其中，每個(gè)障礙物采用一個(gè) 多邊形表達(dá)，所述障礙物數(shù)據(jù)為各個(gè)多邊形對(duì)應(yīng)的數(shù)據(jù)； 根據(jù)所述起始點(diǎn)、所述終止點(diǎn)和所述障礙物數(shù)據(jù)確定所述對(duì)應(yīng)區(qū)域的通視圖； 計(jì)算所述通視圖的鄰接成本矩陣；以及 根據(jù)所述鄰接成本矩陣確定從所述起始點(diǎn)到達(dá)所述終止點(diǎn)的最短路徑。
2. 根據(jù)權(quán)利要求1所述的路徑確定方法，其特征在于， 設(shè)所述通視圖G= (V，E),E為與任何多邊形均不相交的邊的全體，V=IviIi= 1，2,…，n}，V1為所述起始點(diǎn)，vn為所述終止點(diǎn)，VPVfVlri為所述各個(gè)多邊形的頂點(diǎn)，V滿(mǎn) 坐標(biāo)為（Xi,yi)，Vj的坐標(biāo)為（X』，y』)，則計(jì)算所述通視圖的鄰接成本矩陣為：cost[i] [j]= d (Vi, Vj-)， 其中，若Vi、Vj屬于同一個(gè)多邊形，則當(dāng)vpVj為相鄰頂點(diǎn)時(shí)， d(1；/^1'/) ^+〇·,--Vi)2 ;當(dāng)VpVj為同一頂點(diǎn)時(shí)，(!(Vi,Vj) = 0 ;否則(!(Vi,Vj) =〇°， 若?不屬于同一個(gè)多邊形，則當(dāng)Vi、'兩點(diǎn)構(gòu)成的線段與任意一個(gè)多邊形的邊均不 相交時(shí)，J(V),V·:,.) = -Λ；r+(Vi-Vj)2，·否則d(Vi,Vj) = ， 其中，cost[i] [j]為所述鄰接成本矩陣的第i行第j列個(gè)元素。
3. 根據(jù)權(quán)利要求2所述的路徑確定方法，其特征在于，若Vi、'均屬于第一多邊形，其 中，所述第一多邊形為所述對(duì)應(yīng)區(qū)域內(nèi)任意一個(gè)障礙物對(duì)應(yīng)的多邊形，在計(jì)算d(Vi，Vj)之 前，所述方法還包括： 判斷所述第一多邊形是否為凹多邊形； 若所述第一多邊形為凹多邊形，則將所述凹多邊形轉(zhuǎn)化為凸多邊形。
4. 根據(jù)權(quán)利要求2所述的路徑確定方法，其特征在于，若Vi、'均屬于第二多邊形，其 中，所述第二多邊形為所述對(duì)應(yīng)區(qū)域內(nèi)任意一個(gè)障礙物對(duì)應(yīng)的多邊形，在計(jì)算d(Vi，Vj)之 前，所述方法還包括： 判斷所述第二多邊形是否為凹多邊形； 若所述第二多邊形為凹多邊形，則當(dāng)Vi、'為相鄰頂點(diǎn)時(shí)或Vi、'為不相 同不相鄰的頂點(diǎn)且Vi、'構(gòu)成的線段與任意一個(gè)多邊形的各邊均不相交時(shí)， d('"；,V·,.) ^- .V,.)-+(Vj-Vj)1，當(dāng)ViJj為同一頂點(diǎn)時(shí)，(!(Vi,Vj) = 0,否則(!(Vi,Vj) =〇°。
5. 根據(jù)權(quán)利要求1至4中任一項(xiàng)所述的路徑確定方法，其特征在于，每個(gè)障礙物采用凸 多邊形最小面積四邊形包圍盒算法表達(dá)。
6. 根據(jù)權(quán)利要求2至4中任一項(xiàng)所述的路徑確定方法，其特征在于，根據(jù)所述鄰接成本 矩陣確定從所述起始點(diǎn)到達(dá)所述終止點(diǎn)的最短路徑包括： 根據(jù)所述鄰接成本矩陣通過(guò)弗洛伊德算法、迪杰斯特拉算法或基于堆的雙向路徑搜索 算法確定從所述起始點(diǎn)到達(dá)所述終止點(diǎn)的最短路徑。
7. 根據(jù)權(quán)利要求6所述的路徑確定方法，其特征在于，根據(jù)所述鄰接成本矩陣通過(guò)基 于堆的雙向路徑搜索算法確定最短路徑包括以下步驟： 步驟SI:去除所述鄰接成本矩陣中值為無(wú)窮大的元素得到第一弧段集，將正堆、反堆 初始化為空； 步驟S2 :將所述第一弧段集中與所述起始點(diǎn)相關(guān)的弧段置入所述正堆，將所述第一弧 段集中與所述終止點(diǎn)相關(guān)的弧段置入所述反堆； 步驟S3:判斷所述正堆是否為空，若所述正堆為空時(shí)，輸出查找結(jié)束的信息，若所述正 堆不為空時(shí)，執(zhí)行步驟S4; 步驟S4:將當(dāng)前所述正堆得到的第一弧段出堆，其中，任一個(gè)弧段入所述正堆后，均會(huì) 得到由所述起始點(diǎn)出發(fā)連接這一弧段的路徑，所述第一弧段入所述正堆后得到的路徑為當(dāng) 前所述正堆中各弧段得到的路徑中的最小路徑； 步驟S5 :查找所述反堆判斷是否找到所述最短路徑，若找到，則輸出查找成功的信息， 若沒(méi)有找到，則執(zhí)行步驟S6; 步驟S6 :在所述第一弧段集中獲取根據(jù)所述第一弧段擴(kuò)展得到的第一弧段子集，其 中，所述第一弧段子集中的弧段均與所述第一弧段的終點(diǎn)相關(guān)； 步驟S7 :將所述第一弧段子集中的各弧段置入所述正堆，并執(zhí)行步驟S8 ; 步驟S8:判斷所述反堆是否為空，若所述反堆為空時(shí)，輸出查找結(jié)束的信息，若所述反 堆不為空時(shí)，執(zhí)行步驟S9; 步驟S9 :將當(dāng)前所述反堆得到的第二弧段出堆，其中，任一個(gè)弧段入所述反堆后，均會(huì) 得到由所述終止點(diǎn)出發(fā)連接這一弧段的路徑，所述第二弧段入所述反堆后得到的路徑為當(dāng) 前所述反堆中各弧段得到的路徑中的最小路徑； 步驟SlO:查找所述正堆判斷是否找到所述最短路徑，若找到，則輸出查找成功的信 息，若沒(méi)有找到，則執(zhí)行步驟Sll; 步驟Sll:在所述第一弧段集中獲取由所述第二弧段擴(kuò)展得到的第二弧段子集，其中， 所述第二弧段子集中的弧段均與所述第二弧段對(duì)應(yīng)的終點(diǎn)相關(guān)； 步驟S12 :將所述第二弧段子集中的各弧段置入所述反堆，并執(zhí)行步驟S3。
8. 根據(jù)權(quán)利要求1所述的路徑確定方法，其特征在于， 若所述對(duì)應(yīng)區(qū)域?yàn)榉忾]區(qū)域時(shí)，所述方法還包括：獲取所述封閉區(qū)域的邊界； 根據(jù)所述起始點(diǎn)、所述終止點(diǎn)和所述障礙物數(shù)據(jù)確定所述對(duì)應(yīng)區(qū)域的通視圖包括：根 據(jù)所述起始點(diǎn)、所述終止點(diǎn)、所述障礙物數(shù)據(jù)和所述邊界確定所述對(duì)應(yīng)區(qū)域的通視圖。
9. 一種路徑確定裝置，其特征在于，包括： 第一獲取模塊，用于獲取待確定路徑的起始點(diǎn)和終止點(diǎn)； 第二獲取模塊，用于獲取所述起始點(diǎn)和所述終止點(diǎn)對(duì)應(yīng)區(qū)域內(nèi)的障礙物數(shù)據(jù)，其中，每 個(gè)障礙物采用一個(gè)多邊形表達(dá)，所述障礙物數(shù)據(jù)為各個(gè)多邊形對(duì)應(yīng)的數(shù)據(jù)； 第一確定模塊，用于根據(jù)所述起始點(diǎn)、所述終止點(diǎn)和所述障礙物數(shù)據(jù)確定所述對(duì)應(yīng)區(qū) 域的通視圖； 計(jì)算模塊，用于計(jì)算所述通視圖的鄰接成本矩陣；以及 第二確定模塊，用于根據(jù)所述鄰接成本矩陣確定從所述起始點(diǎn)到達(dá)所述終止點(diǎn)的最短 路徑。
10. -種導(dǎo)航儀，其特征在于，包括權(quán)利要求9所述的路徑確定裝置。
【文檔編號(hào)】G01C21/34GK104457775SQ201410773311
【公開(kāi)日】2015年3月25日 申請(qǐng)日期:2014年12月12日 優(yōu)先權(quán)日:2014年12月12日
【發(fā)明者】肖軼圣, 馬海波, 劉富乾, 何洪洲 申請(qǐng)人:北京航天宏圖信息技術(shù)有限責(zé)任公司