專利名稱：一種車載導(dǎo)航系統(tǒng)通過性尋路算法的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種最優(yōu)路徑規(guī)劃方法，特別是關(guān)于一種基于用戶意圖通過某個指定區(qū)域的車載導(dǎo)航系統(tǒng)通過性尋路算法。
背景技術(shù)：
最優(yōu)路徑規(guī)劃可以在如代價最低、路段規(guī)避、通過特定區(qū)域等多種準(zhǔn)則下進(jìn)行?，F(xiàn)有的路徑規(guī)劃算法大多是系統(tǒng)基于代價(距離、時間、花費(fèi)等)最低準(zhǔn)則進(jìn)行從起點(diǎn)到終點(diǎn)的尋路，為用戶選擇一條代價最優(yōu)路線。然而每個人都希望有參與選擇的權(quán)利，車載導(dǎo)航系統(tǒng)的用戶往往希望能夠根據(jù)自己的需求選擇一條合適的道路。例如一些用戶希望經(jīng)過某個特定區(qū)域，但又不確定是否要通過某一特定點(diǎn)。諸如此類的問題都屬于用戶自定義地理準(zhǔn)則下的系統(tǒng)模糊最優(yōu)化尋路問題，目前為止還沒有一個成熟的算法能夠有效地解決這類問題。另一方面，目前由用戶參與的最優(yōu)路徑規(guī)劃算法大多以K最短路徑算法為核心。這種K最短路徑算法是系統(tǒng)在路網(wǎng)中搜索多條最優(yōu)路徑推薦給用戶，由用戶在其中選擇最合適的路徑。當(dāng)K數(shù)值較大時，K最短路徑算法的路徑計算效率會降低很多，而且由于這種方法對機(jī)器的硬件性能要求較高，導(dǎo)致當(dāng)前硬件配置較弱的嵌入式設(shè)備在實際執(zhí)行K最短路徑算法時會有一定的困難。

發(fā)明內(nèi)容
針對上述問題，本發(fā)明的目的是提供一種能夠基于用戶意圖通過某個指定區(qū)域的車載導(dǎo)航系統(tǒng)通過性尋路算法。該算法根據(jù)用戶模糊指定的一個控制點(diǎn)，自動規(guī)劃一條經(jīng)過該控制點(diǎn)附近區(qū)域的最優(yōu)路徑，使規(guī)劃出的路徑既能滿足用戶意圖通過指定區(qū)域又能滿足代價最低準(zhǔn)則，使用戶以較快的方式到達(dá)目的地。為實現(xiàn)上述目的，本發(fā)明采取以下技術(shù)方案1、一種車載導(dǎo)航系統(tǒng)通過性尋路算法，它包括以下步驟I)設(shè)置一包括有一起點(diǎn)路網(wǎng)、一終點(diǎn)路網(wǎng)和一隔離面的通過性路網(wǎng)模型；所述起點(diǎn)路網(wǎng)和終點(diǎn)路網(wǎng)分別設(shè)置一規(guī)劃路徑的起點(diǎn)和終點(diǎn)，除所述起點(diǎn)和終點(diǎn)以及它們的鄰接路段以外，所述起點(diǎn)路網(wǎng)和終點(diǎn)路網(wǎng)內(nèi)的元素相同，均設(shè)置有若干節(jié)點(diǎn)和一半徑為r的圓形通過區(qū)域，所述通過區(qū)域的圓心為用戶指定的控制點(diǎn)；所述起點(diǎn)路網(wǎng)和終點(diǎn)路網(wǎng)通過所述隔離面隔離，僅通過所述通過區(qū)域內(nèi)的節(jié)點(diǎn)相互連通；2)在由步驟I)建立的通過性路網(wǎng)模型中分兩步虛終點(diǎn)尋路和變方向?qū)ぢ愤M(jìn)行通過性尋路①在起點(diǎn)路網(wǎng)中設(shè)置一虛終點(diǎn)，以起點(diǎn)路網(wǎng)中節(jié)點(diǎn)到虛終點(diǎn)的代價為引導(dǎo)代價，利用A*算法引導(dǎo)路徑向通過區(qū)域規(guī)劃拓展，在路徑拓展區(qū)域完全覆蓋通過區(qū)域后，停止虛終點(diǎn)尋路；②以終點(diǎn)路網(wǎng)中通過區(qū)域內(nèi)經(jīng)過步驟①拓展到的節(jié)點(diǎn)到終點(diǎn)的代價為引導(dǎo)代價，利用A*算法引導(dǎo)路徑向終點(diǎn)規(guī)劃拓展，在到達(dá)終點(diǎn)后停止變方向?qū)ぢ罚?)回溯步驟2)的尋路路徑，并將回溯結(jié)果確定為經(jīng)過通過區(qū)域的最優(yōu)路徑。上述步驟2)的虛終點(diǎn)尋路和變方向?qū)ぢ钒ㄒ韵虏襟E2. I)在起點(diǎn)路網(wǎng)中起點(diǎn)與控制點(diǎn)的連線的延長線上，于靠近控制點(diǎn)的一端設(shè)置一虛終點(diǎn)，使控制點(diǎn)與虛終點(diǎn)之間的距離等于控制點(diǎn)與終點(diǎn)之間的距離；設(shè)置一優(yōu)先隊列Q，將其初始化僅含規(guī)劃路徑的起點(diǎn)；將優(yōu)先隊列Q外的所有節(jié)點(diǎn)初始化為未拓展節(jié)點(diǎn)，將所 有未拓展節(jié)點(diǎn)到起點(diǎn)的已有代價Ug初始化為c ，將路徑拓展區(qū)域完全覆蓋通過區(qū)域的標(biāo)志Fp初始化為0，將一計數(shù)器Nq初始化為O ;2. 2)從優(yōu)先隊列Q中取出當(dāng)前總代價Uf = Ug+Uh最小的節(jié)點(diǎn)V，Uh為節(jié)點(diǎn)到虛終點(diǎn)的啟發(fā)代價；2. 3)判斷節(jié)點(diǎn)V是否在通過區(qū)域內(nèi)若在通過區(qū)域內(nèi)，計數(shù)器Nq減1，進(jìn)入步驟2. 9)；若未在通過區(qū)域內(nèi)，進(jìn)入步驟2.4);2. 4)判斷節(jié)點(diǎn)V的相鄰節(jié)點(diǎn)是否均已訪問到若均已訪問到，將節(jié)點(diǎn)V標(biāo)記為已拓展節(jié)點(diǎn)，返回步驟2. 2)；若尚未全部訪問到,進(jìn)入步驟2. 5);2. 5)訪問節(jié)點(diǎn)V的一尚未訪問到的相鄰節(jié)點(diǎn)U，判斷節(jié)點(diǎn)U的拓展?fàn)顟B(tài) ①若節(jié)點(diǎn)U為未拓展節(jié)點(diǎn)，計算節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug和到虛終點(diǎn)的啟發(fā)代價Uh,將節(jié)點(diǎn)U加入優(yōu)先隊列Q變?yōu)樵谕卣构?jié)點(diǎn)，進(jìn)入步驟2. 6)；②若節(jié)點(diǎn)U為在拓展節(jié)點(diǎn)，計算節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug'，進(jìn)入步驟2. 7)；③若節(jié)點(diǎn)U為已拓展節(jié)點(diǎn)，返回步驟2. 4)；2. 6)判斷節(jié)點(diǎn)U是否在通過區(qū)域內(nèi)若在通過區(qū)域內(nèi)，計數(shù)器Nq加1，將節(jié)點(diǎn)U加入復(fù)活隊列R，進(jìn)入步驟2. 8)；若未在通過區(qū)域內(nèi)，返回步驟2.4);2. 7)判斷Ug' < Ug是否成立若成立，更新優(yōu)先隊列Q中節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug = U/，返回步驟2. 4)；若未成立,直接返回步驟2. 4);2. 8)比較Nq與預(yù)先設(shè)定的復(fù)活節(jié)點(diǎn)個數(shù)閾值Nt 若Nq 彡 Nt, Fp = I，進(jìn)入步驟 2. 9)；若Nq < Nt,返回步驟 2. 4)；2. 9)判斷Nq = O且Fp = I是否成立若成立，結(jié)束虛終點(diǎn)尋路，進(jìn)入步驟2. 10)；若未成立，返回步驟2. 4)；2. 10)初始化優(yōu)先隊列Q = R，將優(yōu)先隊列Q中所有節(jié)點(diǎn)的啟發(fā)代價Uh更改為節(jié)點(diǎn)到終點(diǎn)的啟發(fā)代價，并將優(yōu)先隊列Q外的所有節(jié)點(diǎn)初始化為未拓展節(jié)點(diǎn)；2. 11)從優(yōu)先隊列Q中取出當(dāng)前總代價Uf = Ug+Uh最小的節(jié)點(diǎn)V ；2. 12)判斷節(jié)點(diǎn)V是否為終點(diǎn)若節(jié)點(diǎn)V非終點(diǎn)，進(jìn)入步驟2. 13)；若節(jié)點(diǎn)V為終點(diǎn)，進(jìn)入步驟2. 16)；
2. 13)判斷節(jié)點(diǎn)V的相鄰節(jié)點(diǎn)是否均已訪問到若均已拓展到，將節(jié)點(diǎn)V變?yōu)橐淹卣构?jié)點(diǎn)，返回步驟2. 11)；若未全部拓展到，進(jìn)入步驟2. 14);2. 14)訪問節(jié)點(diǎn)V的一尚未訪問到的相鄰節(jié)點(diǎn)U，判斷節(jié)點(diǎn)U的拓展?fàn)顟B(tài)①若節(jié)點(diǎn)U為未拓展節(jié)點(diǎn)，計算節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug和到終點(diǎn)的啟發(fā)代價uh，將節(jié)點(diǎn)U加入優(yōu)先隊列Q變?yōu)樵谕卣构?jié)點(diǎn)，返回步驟2. 13)；②若節(jié)點(diǎn)U為在拓展節(jié)點(diǎn)，計算節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug'，進(jìn)入步驟2. 15)；③若節(jié)點(diǎn)U為已拓展節(jié)點(diǎn)，返回步驟2. 13)；2. 15)判斷Ug' < Ug是否成立若成立，更新優(yōu)先隊列Q中節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug = Ug,，返回步驟2. 13)；若未成立，直接返回步驟2. 13);2. 16)結(jié)束變方向?qū)ぢ?。上述步驟2. 8)中，復(fù)活節(jié)點(diǎn)個數(shù)閾值Nt設(shè)定為5。上述步驟I)中，通過區(qū)域半徑r為r = min Cr1, rd), rx為距離半徑，rd為密度半徑；所述距離半徑^根據(jù)起點(diǎn)或終點(diǎn)到控制點(diǎn)的距離確定，其計算表達(dá)式如下Y1 = min ( a *1ST，^ *min (1SM, 1MT)),上式中，1ST、1SM、1MT分別為起點(diǎn)到終點(diǎn)、起點(diǎn)到控制點(diǎn)、控制點(diǎn)到終點(diǎn)的距離；a和^分別為全路徑距離影響因子和子路徑距離影響因子；所述密度半徑rd根據(jù)路網(wǎng)密度確
定，其計算表達(dá)式如下

權(quán)利要求
1.一種車載導(dǎo)航系統(tǒng)通過性尋路算法，它包括以下步驟 1)設(shè)置一包括有一起點(diǎn)路網(wǎng)、一終點(diǎn)路網(wǎng)和一隔離面的通過性路網(wǎng)模型；所述起點(diǎn)路網(wǎng)和終點(diǎn)路網(wǎng)分別設(shè)置一規(guī)劃路徑的起點(diǎn)和終點(diǎn)，除所述起點(diǎn)和終點(diǎn)以及它們的鄰接路段以外，所述起點(diǎn)路網(wǎng)和終點(diǎn)路網(wǎng)內(nèi)的元素相同，均設(shè)置有若干節(jié)點(diǎn)和一半徑為r的圓形通過區(qū)域，所述通過區(qū)域的圓心為用戶指定的控制點(diǎn)；所述起點(diǎn)路網(wǎng)和終點(diǎn)路網(wǎng)通過所述隔離面隔離，僅通過所述通過區(qū)域內(nèi)的節(jié)點(diǎn)相互連通； 2)在由步驟I)建立的通過性路網(wǎng)模型中分兩步虛終點(diǎn)尋路和變方向?qū)ぢ愤M(jìn)行通過性尋路 ①在起點(diǎn)路網(wǎng)中設(shè)置一虛終點(diǎn)，以起點(diǎn)路網(wǎng)中節(jié)點(diǎn)到虛終點(diǎn)的代價為引導(dǎo)代價，利用A*算法引導(dǎo)路徑向通過區(qū)域規(guī)劃拓展，在路徑拓展區(qū)域完全覆蓋通過區(qū)域后，停止虛終點(diǎn)尋路； ②以終點(diǎn)路網(wǎng)中通過區(qū)域內(nèi)經(jīng)過步驟①拓展到的節(jié)點(diǎn)到終點(diǎn)的代價為引導(dǎo)代價，利用A*算法引導(dǎo)路徑向終點(diǎn)規(guī)劃拓展，在到達(dá)終點(diǎn)后停止變方向?qū)ぢ罚? 3)回溯步驟2)的尋路路徑，并將回溯結(jié)果確定為經(jīng)過通過區(qū)域的最優(yōu)路徑。
2.如權(quán)利要求I所述的一種車載導(dǎo)航系統(tǒng)通過性尋路算法，其特征在于所述步驟2)的虛終點(diǎn)尋路和變方向?qū)ぢ钒ㄒ韵虏襟E . 2. I)在起點(diǎn)路網(wǎng)中起點(diǎn)與控制點(diǎn)的連線的延長線上，于靠近控制點(diǎn)的一端設(shè)置一虛終點(diǎn)，使控制點(diǎn)與虛終點(diǎn)之間的距離等于控制點(diǎn)與終點(diǎn)之間的距離；設(shè)置一優(yōu)先隊列Q，將其初始化僅含規(guī)劃路徑的起點(diǎn)；將優(yōu)先隊列Q外的所有節(jié)點(diǎn)初始化為未拓展節(jié)點(diǎn)，將所有未拓展節(jié)點(diǎn)到起點(diǎn)的已有代價Ug初始化為①，將路徑拓展區(qū)域完全覆蓋通過區(qū)域的標(biāo)志Fp初始化為O，將一計數(shù)器Nq初始化為O ; . 2. 2)從優(yōu)先隊列Q中取出當(dāng)前總代價Uf = Ug+Uh最小的節(jié)點(diǎn)V，Uh為節(jié)點(diǎn)到虛終點(diǎn)的啟發(fā)代價； . 2. 3)判斷節(jié)點(diǎn)V是否在通過區(qū)域內(nèi) 若在通過區(qū)域內(nèi)，計數(shù)器Nq減1，進(jìn)入步驟2. 9)； 若未在通過區(qū)域內(nèi)，進(jìn)入步驟2. 4)； . 2. 4)判斷節(jié)點(diǎn)V的相鄰節(jié)點(diǎn)是否均已訪問到 若均已訪問到，將節(jié)點(diǎn)V標(biāo)記為已拓展節(jié)點(diǎn)，返回步驟2. 2)； 若尚未全部訪問到，進(jìn)入步驟2. 5)； . 2. 5)訪問節(jié)點(diǎn)V的一尚未訪問到的相鄰節(jié)點(diǎn)U，判斷節(jié)點(diǎn)U的拓展?fàn)顟B(tài) ①若節(jié)點(diǎn)U為未拓展節(jié)點(diǎn)，計算節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug和到虛終點(diǎn)的啟發(fā)代價Uh,將節(jié)點(diǎn)U加入優(yōu)先隊列Q變?yōu)樵谕卣构?jié)點(diǎn)，進(jìn)入步驟2. 6)； ②若節(jié)點(diǎn)U為在拓展節(jié)點(diǎn)，計算節(jié)點(diǎn)U到起點(diǎn)的已有代價U/，進(jìn)入步驟2.7)； ③若節(jié)點(diǎn)U為已拓展節(jié)點(diǎn)，返回步驟2.4)； 2.6)判斷節(jié)點(diǎn)U是否在通過區(qū)域內(nèi) 若在通過區(qū)域內(nèi)，計數(shù)器Nq加1，將節(jié)點(diǎn)U加入復(fù)活隊列R，進(jìn)入步驟2. 8)； 若未在通過區(qū)域內(nèi)，返回步驟2. 4)； 2.7)判斷Ug' < Ug是否成立 若成立，更新優(yōu)先隊列Q中節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug = Ug,，返回步驟2. 4)；若未成立，直接返回步驟2. 4)； 、 2. 8)比較Nq與預(yù)先設(shè)定的復(fù)活節(jié)點(diǎn)個數(shù)閾值Nt 若Nq彡Nt, Fp = 1，進(jìn)入步驟2. 9)； 若Nq < Nt,返回步驟2. 4)； 、2. 9)判斷Nq = O且Fp = I是否成立 若成立，結(jié)束虛終點(diǎn)尋路，進(jìn)入步驟2. 10)； 若未成立，返回步驟2. 4)； 、2. 10)初始化優(yōu)先隊列Q = R，將優(yōu)先隊列Q中所有節(jié)點(diǎn)的啟發(fā)代價Uh更改為節(jié)點(diǎn)到終點(diǎn)的啟發(fā)代價，并將優(yōu)先隊列Q外的所有節(jié)點(diǎn)初始化為未拓展節(jié)點(diǎn)； 、2. 11)從優(yōu)先隊列Q中取出當(dāng)前總代價Uf = Ug+Uh最小的節(jié)點(diǎn)V ；、 、2. 12)判斷節(jié)點(diǎn)V是否為終點(diǎn) 若節(jié)點(diǎn)V非終點(diǎn)，進(jìn)入步驟2. 13)； 若節(jié)點(diǎn)V為終點(diǎn)，進(jìn)入步驟2. 16)； 、2. 13)判斷節(jié)點(diǎn)V的相鄰節(jié)點(diǎn)是否均已訪問到 若均已拓展到，將節(jié)點(diǎn)V變?yōu)橐淹卣构?jié)點(diǎn)，返回步驟2. 11)； 若未全部拓展到，進(jìn)入步驟2. 14)； 、2. 14)訪問節(jié)點(diǎn)V的一尚未訪問到的相鄰節(jié)點(diǎn)U，判斷節(jié)點(diǎn)U的拓展?fàn)顟B(tài) ①若節(jié)點(diǎn)U為未拓展節(jié)點(diǎn)，計算節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug和到終點(diǎn)的啟發(fā)代價Uh,將節(jié)點(diǎn)U加入優(yōu)先隊列Q變?yōu)樵谕卣构?jié)點(diǎn)，返回步驟2. 13)； ②若節(jié)點(diǎn)U為在拓展節(jié)點(diǎn)，計算節(jié)點(diǎn)U到起點(diǎn)的已有代價U/，進(jìn)入步驟2.15)； ③若節(jié)點(diǎn)U為已拓展節(jié)點(diǎn)，返回步驟2.13)； 、 2. 15)判斷Ug' < Ug是否成立 若成立，更新優(yōu)先隊列Q中節(jié)點(diǎn)U到起點(diǎn)的已有代價Ug = Ug,，返回步驟2. 13)； 若未成立，直接返回步驟2. 13)； 、2.16)結(jié)束變方向?qū)ぢ贰?br> 3.如權(quán)利要求2所述的一種車載導(dǎo)航系統(tǒng)通過性尋路算法，其特征在于所述步驟、2.8)中，復(fù)活節(jié)點(diǎn)個數(shù)閾值Nt設(shè)定為5。
4.如權(quán)利要求I或2或3所述的一種車載導(dǎo)航系統(tǒng)通過性尋路算法，其特征在于所述步驟I)中，通過區(qū)域半徑r為r = min (rx, rd)，rx為距離半徑，rd為密度半徑；所述距離半徑T1根據(jù)起點(diǎn)或終點(diǎn)到控制點(diǎn)的距離確定，其計算表達(dá)式如下 T1 = min ( a *1ST，^ *min (1SM, 1MT)), 上式中，1ST、1SM、Imt分別為起點(diǎn)到終點(diǎn)、起點(diǎn)到控制點(diǎn)、控制點(diǎn)到終點(diǎn)的距離；a和3分別為全路徑距離影響因子和子路徑距離影響因子；所述密度半徑rd根據(jù)路網(wǎng)密度確定，其計算表達(dá)式如下
5.如權(quán)利要求4所述的一種車載導(dǎo)航系統(tǒng)通過性尋路算法，其特征在于所述路段框選模型為當(dāng)一路段的外接矩形框與半徑為4的圓形目標(biāo)區(qū)域相交或內(nèi)含時，認(rèn)定該路段在目標(biāo)區(qū)域內(nèi)，否則認(rèn)定該路段在目標(biāo)區(qū)域外。
6.如權(quán)利要求4所述的一種車載導(dǎo)航系統(tǒng)通過性尋路算法，其特征在于所述全路徑距離影響因子α設(shè)定為1/35，子路徑距離影響因子β設(shè)定為1/20，路段基數(shù)Nb設(shè)定為.600。
7.如權(quán)利要求5所述的一種車載導(dǎo)航系統(tǒng)通過性尋路算法，其特征在于所述全路徑距離影響因子α設(shè)定為1/35，子路徑距離影響因子β設(shè)定為1/20，路段基數(shù)Nb設(shè)定為.600。
全文摘要
一種車載導(dǎo)航系統(tǒng)通過性尋路算法，其特征在于，它采用通過性路網(wǎng)模型，將路徑的起點(diǎn)和終點(diǎn)分設(shè)在兩個路網(wǎng)中，兩路網(wǎng)只在通過區(qū)域范圍內(nèi)才能相互連通，從而確保由起點(diǎn)到達(dá)終點(diǎn)的尋路過程中一定經(jīng)過指定的通過區(qū)域；采用通過區(qū)域半徑調(diào)節(jié)方案，在起點(diǎn)和終點(diǎn)距離較遠(yuǎn)時設(shè)置較大的通過區(qū)域半徑，路網(wǎng)密度較小時設(shè)置較大的通過區(qū)域半徑，從而確保規(guī)劃路徑有效經(jīng)過控制點(diǎn)附近，但并不一定經(jīng)過控制點(diǎn)；采用虛終點(diǎn)尋路通過設(shè)置虛終點(diǎn)并利用A*算法引導(dǎo)路徑通過指定區(qū)域，采用變方向?qū)ぢ吠ㄟ^更改尋路引導(dǎo)代價引導(dǎo)路徑從指定區(qū)域向終點(diǎn)規(guī)劃，從而確保結(jié)果的準(zhǔn)確性和可靠性。本發(fā)明規(guī)劃出的路線既能滿足用戶意圖經(jīng)過通過區(qū)域又能滿足代價最低準(zhǔn)則，可以用于各種嵌入式車載導(dǎo)航系統(tǒng)。
文檔編號G01C21/34GK102645228SQ20121010731
公開日2012年8月22日 申請日期2012年4月12日 優(yōu)先權(quán)日2012年4月12日
發(fā)明者張德鑫, 張照生, 彭應(yīng)亮, 李江濤, 楊殿閣, 王釗, 連小珉 申請人:清華大學(xué)