淺論車輛路徑規(guī)劃的算法

黃淑彤

摘 要：隨著計(jì)算機(jī)技術(shù)、電信技術(shù)、智能化的不斷進(jìn)步發(fā)展，決定人們出行方式之一的汽車智能化發(fā)展得更為迅速，為了輔助駕駛員操控汽車，便利出行，且提升安全性能，研究人員開發(fā)了路徑規(guī)劃功能。路徑規(guī)劃屬于智能車領(lǐng)域的重要分支之一。本文首先介紹導(dǎo)航系統(tǒng)以路徑規(guī)劃為核心的部分工作原理，再核心介紹了路徑規(guī)劃的兩大基本算法，Dijkstra算法和Floyd算法， 最后簡(jiǎn)單展望了路徑規(guī)劃應(yīng)用于不同行業(yè)帶來(lái)的益處。

關(guān)鍵詞：路徑規(guī)劃;導(dǎo)航系統(tǒng);迪杰特斯拉算法;弗洛伊德算法

中圖分類號(hào)：TP273 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1671-2064（2019）03-0032-03

1 路徑規(guī)劃概述

路徑規(guī)劃即在周圍有障礙物的環(huán)境中，按照一定的評(píng)價(jià)標(biāo)準(zhǔn)，尋找一條從起始位置到終止位置的無(wú)碰撞路徑[1]。屬于運(yùn)動(dòng)規(guī)劃的一種，與軌跡規(guī)劃同屬于運(yùn)動(dòng)規(guī)劃。其中路徑規(guī)劃包括全局路徑規(guī)劃（也叫靜態(tài)規(guī)劃）和局部路徑規(guī)劃（也叫動(dòng)態(tài)規(guī)劃）[1]。靜態(tài)規(guī)劃要儲(chǔ)備一切環(huán)境信息，以地圖查詢?yōu)橹?，相?duì)簡(jiǎn)單。而動(dòng)態(tài)路徑規(guī)劃，即隨時(shí)根據(jù)環(huán)境變化更改推薦的路徑，再按需求提供最適路徑，需要傳感器實(shí)時(shí)傳達(dá)動(dòng)點(diǎn)當(dāng)前所在位置的相關(guān)信息[1，3]。

2 運(yùn)用路徑規(guī)劃的導(dǎo)航系統(tǒng)

車載式導(dǎo)航，即攜有全球定位系統(tǒng)功能的工具，能實(shí)時(shí)定位、檢測(cè)路況、自動(dòng)規(guī)劃路線、進(jìn)行語(yǔ)音服務(wù)通知的一種便捷的車載工具[1]。導(dǎo)航系統(tǒng)為車輛規(guī)劃路徑時(shí)離不開路徑規(guī)劃技術(shù)，車載導(dǎo)航中的路徑規(guī)劃實(shí)際是計(jì)算機(jī)領(lǐng)域與定位領(lǐng)域共同的產(chǎn)物。導(dǎo)航核心的技術(shù)之一就是衛(wèi)星定位，衛(wèi)星定位是進(jìn)行路徑規(guī)劃的基礎(chǔ)[1]。以北斗衛(wèi)星計(jì)劃的標(biāo)配為例，衛(wèi)星定位是基于三球交會(huì)原理來(lái)工作的，系統(tǒng)需要5顆靜止軌道衛(wèi)星、30顆非靜止軌道衛(wèi)星[2，5]。其中系統(tǒng)中的GEO（即任意兩個(gè)靜止衛(wèi)星）它們的坐標(biāo)是已知的，兩者各自將半徑定為與發(fā)出者的距離，通過(guò)畫第一個(gè)圓來(lái)形成球面，焦點(diǎn)處即發(fā)出者的位置，兩者的距離需要與地面進(jìn)行雙向測(cè)距，這是通過(guò)地面中心接收站與眾多衛(wèi)星的位置交互完成的[2，4]。中心站借助衛(wèi)星傳達(dá)過(guò)來(lái)的電子地圖，形成第二個(gè)以地球球心為圓心的圓，這時(shí)半徑是與地面的垂直距離，圓弧的第二個(gè)焦點(diǎn)就是用戶的二維位置[2，4，5]。這樣借助球半徑、圓弧焦點(diǎn)等訊息確定下來(lái)相應(yīng)距離，以便位置解算，進(jìn)而定下坐標(biāo)位置，最終發(fā)送回地面，由導(dǎo)航系統(tǒng)實(shí)時(shí)接收[4]。有了準(zhǔn)確位置后，系統(tǒng)再按照經(jīng)過(guò)衛(wèi)星覆蓋的路況數(shù)字地圖來(lái)分析路徑數(shù)據(jù)，按照一系列路徑算法為使用者規(guī)劃出最適路徑。

3 經(jīng)典的最短路徑算法

3.1 Dijkstra算法

在上述的導(dǎo)航系統(tǒng)中，選擇出發(fā)地和目的地之后，兩地之間有許多條可選擇的行程路徑，但是，由于選擇及要求的不同，對(duì)應(yīng)的最佳路徑也不同。所以，為了計(jì)算出最短路徑，前人提出了dijkstra和弗洛伊德基本算法。

dijkstra算法，即通過(guò)使路徑長(zhǎng)度按次序累計(jì)從而產(chǎn)生最短路徑，路徑規(guī)劃中運(yùn)用該算法，逐步計(jì)算由起點(diǎn)到終點(diǎn)的路途中每個(gè)分岔口的之間的最短距離，之后不斷累加距離值，且每進(jìn)行一步都要依賴前一步的最短路徑的基礎(chǔ)，環(huán)環(huán)相扣，直到最終將每個(gè)岔口的距離都分析全，完整比較后求解得最短路徑。再加上其他的考慮因素之后得到的最優(yōu)路徑被提供給駕駛員，這能讓使用者在運(yùn)用路徑規(guī)劃時(shí)很大程度享用便利[6，8]。

下面簡(jiǎn)述dijkstra算法實(shí)例：

傳統(tǒng)dijkstra算法中要引入兩個(gè)數(shù)組，分別是簡(jiǎn)單一維數(shù)組和二維數(shù)組以及一個(gè)鄰接矩陣如圖1。下面給出了一簡(jiǎn)單的有向圖算法工作原理如下：本圖共有4個(gè)節(jié)點(diǎn)v1，v2，v3，v4.現(xiàn)在要尋找到從v1到其他各點(diǎn)的最短距離。在四個(gè)點(diǎn)間各自有不同的權(quán)值對(duì)應(yīng)不同的距離，運(yùn)算時(shí)需要全部歷遍每個(gè)點(diǎn)及每個(gè)距離。首先由起點(diǎn)v1開始，算法規(guī)定在有向圖中，若某一點(diǎn)到本位點(diǎn)，則用0表示，比如該有向圖中v1到v1點(diǎn)即本位到本位，用坐標(biāo)表示（v1，0）。若某一點(diǎn)沒有指向另一點(diǎn)的有向線段則用+∞表示比如v1沒有指向到v4的有向線段，故在v1的迭代中用坐標(biāo)表示即（v1，+∞）。從v1到v4每次迭代，都是先找到最小的權(quán)值，這個(gè)過(guò)程需要進(jìn)行比較，v1到v1為0，一定比正無(wú)窮及任何大于零的值小于是迭代1的min值就是0，用雙*表示，在下次迭代時(shí)就不再改變記錄下的坐標(biāo)了，但下次迭代仍然需要在此基礎(chǔ)上加上本值。如圖中明顯v1只能到v2，第二個(gè)min值是5加0，在用雙*記錄下后，繼續(xù)查尋第三個(gè)min值，由于指向已確定是v1到v4，此時(shí)v3到v1是無(wú)意義的計(jì)算，直接略去，故在權(quán)值5的基礎(chǔ)上加上3得到的距離值8即為所求值，加*再次如上。按照上述規(guī)則我們易知接下來(lái)的操作步驟了。等到結(jié)束時(shí)，為確保每個(gè)點(diǎn)都已經(jīng)檢查過(guò)了，直接檢查集合T即可，若集合T包含了所有需要遍歷的元素，那么就可直接看最后一次迭代的值，此例中即為23，最終得出結(jié)論：從v1到v4的最短路徑值為23，其中經(jīng)過(guò)了v2、v3兩個(gè)點(diǎn)。該算法完成[6]。

3.2 弗洛伊德算法

前面表1所述的Dijkstra算法是求單源最短路徑，即起點(diǎn)已經(jīng)定下比如v1到其他任意Vx的最短路徑，而弗洛伊德算法是可以解決任意兩個(gè)點(diǎn)的最短路徑的一種更方便有利的算法，譬如四個(gè)點(diǎn)中可以直接得到V2到V3的最短路徑。過(guò)程更復(fù)雜一點(diǎn)，如下：首先我們需要引入兩個(gè)鄰接矩陣，D矩陣（distance）和一個(gè)P矩陣，D這個(gè)矩陣能夠表示任意兩點(diǎn)的距離，另一個(gè)P矩陣則表示的是任意兩點(diǎn)的最短路徑確定下來(lái)之后，用于儲(chǔ)存路程中間所經(jīng)過(guò)的那些節(jié)點(diǎn)。這樣不僅距離最短可知，中間經(jīng)過(guò)點(diǎn)也可知了。兩個(gè)矩陣都采用的（N×N）型，這里的n表示的是節(jié)點(diǎn)的個(gè)數(shù)，即Vx的x的數(shù)值。N×N表示的N行×N列將在下面的矩陣中用到。D矩陣中的元素用a[i][j]表示，此處的i和j表示任意值，而i、j的范圍都是0到（n-1），因?yàn)橛?jì)算機(jī)是從0開始共n項(xiàng)到n-1。故i等于0其實(shí)是第一個(gè)節(jié)點(diǎn)，所以a[0][1]表示的是從節(jié)點(diǎn)V1到節(jié)點(diǎn)V2的最短距離，在矩陣中則表示第一行第二列對(duì)應(yīng)的坐標(biāo)值。借鑒上面算法，若兩個(gè)節(jié)點(diǎn)沒有指向路徑，那么D矩陣中表示為∞而a[m][m]對(duì)應(yīng)的值就是0。為了便于理解，矩陣計(jì)算過(guò)程用圖2表示。首先第一個(gè)表格展示了任意兩個(gè)點(diǎn)的距離值，但是之后的距離值發(fā)生了變化，被替代成了最短路徑距離，其中計(jì)算采用的公式如下：a[i][k-1]+a[k-1][j]<a[i][j]。定義更新次數(shù)為k時(shí)，k的取值范圍是1到n，一共有多少個(gè)節(jié)點(diǎn)則要更新多少次。由給定的有向圖可知當(dāng)k=4就更新完了，每次變化的其實(shí)是k值。下面以第一列為例，當(dāng)k=1時(shí)，進(jìn)行第一次遍歷，要將第一列的每個(gè)數(shù)與與第一行的每個(gè)數(shù)都相加一遍看是否比該行該列的坐標(biāo)值更小，若更小就替換。而i及k的值是范圍內(nèi)的任意值。

如圖a[0][0]=0，a[1][0]=∞，a[2][0]=4，a[3][0]=10，各距離值填完表格1之后，我們按照第一行與第一列、第二行與第二列、第三行與第三列、第四行與第四列形成十字交叉結(jié)構(gòu)，交叉點(diǎn)為a[0][0]、a[1][1]、a[2][2]、a[3][3]。定下來(lái)值[k-1]之后，再按照公式，比較a[0][1]+a[1][0]=5+∞，而a[1][1]=0。故可知a[0][1]+a[1][0]>a[1][1].經(jīng)過(guò)比較發(fā)現(xiàn)還是原來(lái)的a[1][1]=0更加小，所以不再改動(dòng)該數(shù)值接著進(jìn)行a[2][0]+a[0][1]=4+5=9，而原來(lái)的a[2][1]=∞，明顯9<∞，故求得了新的v3到v2的最短路徑9，于是我們將9作為新的a[2][1]的值取代了原來(lái)的∞，做了加粗符號(hào)，這個(gè)新的9也跟著延續(xù)用在下一個(gè)表格中，這樣按照同樣的規(guī)律相加后比較，依次遍歷比較每一個(gè)a[i][j-1]+a[i-1][j]的值和a[i][j]的值大小，選擇更小的值。在本例中，以a[0][0]為交叉點(diǎn)時(shí)，有a[2][1]從∞變成了9，a[3][1]從∞變成了15。K=2，以a[1][1]為交叉點(diǎn)時(shí)，有a[0][2]從∞變成了8，有a[3][2]從∞變成了18.k=3，以a[2][2]為交叉點(diǎn)時(shí)，有a[1][0]從∞變成了7，a[1][3]從∞變成了18.最終以a[3][3]作為交叉點(diǎn)時(shí)，即k取最后的4時(shí)，代表最后一次遍歷，發(fā)現(xiàn)并無(wú)交換項(xiàng)，不替換任何值，所以得到的最后一個(gè)表格即為最終D矩陣的結(jié)果。D矩陣完成后，已經(jīng)得到各點(diǎn)間的最短距離值了。

接著開始P矩陣，引入b[i][j]作為距離坐標(biāo)值，其實(shí)是a[i][j]在p矩陣中的另一種表示。其對(duì)應(yīng)的公式如下：b[i][j]=b[i][k-1]，這時(shí)變化的值是b[i][k-1]，如上述知k仍是變值，仍然是1到n（本例只取到4）的范圍?；贒矩陣變化的情況，相應(yīng)的在P矩陣中做出調(diào)整。如上例原始矩陣（即表格1），因?yàn)樵诘谝涣袝r(shí)j=0，所以定為第一列所有行定位的值都為0，第二列所有行定位的值都為2，第三列所有行定位的值都為3，一直到第N列所有行是N終止（本例只取到N=4）。接下來(lái)根據(jù)D矩陣中的變化，開始第一次更替，K=1時(shí)，對(duì)應(yīng)表格2，由a[2][1]從∞變成了9，故要變化b[2][1]這時(shí)由公式知，i=2，b[2][1]就被b[2][1-1]=b[2][0]=0取代了。接著發(fā)現(xiàn)K=1時(shí)a[3][1]從∞變成了15，相應(yīng)的b[3][1]就被b[3][1-1]=b[3][0]=0取代了。表格2中已經(jīng)加粗標(biāo)示出來(lái)了。結(jié)束了k=1之后，繼續(xù)進(jìn)行k=2，發(fā)現(xiàn)K=2時(shí)a[0][2]從∞變成了8，相應(yīng)的b[0][2]就被b[0][2-1]=b[0][1]=1取代了，同時(shí)發(fā)現(xiàn)K=2時(shí)a[3][2]從∞變成了18，相應(yīng)的b[3][2]就被b[3][2-1]=b[3][1]=0取代了，表格3完成，繼續(xù)k=3，發(fā)現(xiàn)K=3時(shí)a[1][0]從∞變成了7，相應(yīng)的b[1][0]就被b[1][3-1]=b[1][2]=2取代了，且發(fā)現(xiàn)K=3時(shí)a[1][3]從∞變成了18，相應(yīng)的b[1][3]就被b[1][3-1]=b[1][2]=2取代了.結(jié)束了表格4之后，由于k取最后的值4時(shí)，對(duì)應(yīng)在D矩陣中無(wú)替換項(xiàng)，所以直接由表格4復(fù)制數(shù)據(jù)之后得到表格5。遍歷了所有值之后P矩陣就完成了。

這樣經(jīng)過(guò)P矩陣及D矩陣的計(jì)算，就直接可求得任意兩個(gè)點(diǎn)的間距最短是多少，且這個(gè)過(guò)程中所經(jīng)歷的點(diǎn)位數(shù)都很明晰清楚。如圖3所示。

4 路徑規(guī)劃的應(yīng)用

路徑規(guī)劃目前在科技前端頗受重視，現(xiàn)在的主要應(yīng)用如下：（1）在機(jī)器人領(lǐng)域，能保證機(jī)器人行動(dòng)無(wú)錯(cuò)，運(yùn)用路徑規(guī)劃對(duì)周圍環(huán)境進(jìn)行抽象建模和形象還原，進(jìn)而確保路徑最佳。機(jī)器人的伸展機(jī)械擺臂運(yùn)動(dòng)同樣要事先構(gòu)造運(yùn)動(dòng)軌跡，借助路徑規(guī)劃更加精準(zhǔn)有序。（2）無(wú)人機(jī)在空中飛行時(shí)，借助路徑規(guī)劃方案的導(dǎo)向在空域中無(wú)人機(jī)能更高效地適應(yīng)陌生環(huán)境。（3）城市道路網(wǎng)借助路徑規(guī)劃在智能領(lǐng)域的涉獵，同樣也收益匪淺，目前城市交通大多數(shù)將地標(biāo)從以前的靜止圖像換成了如今的電子實(shí)時(shí)變換屏。（4）在刑事辦案方面對(duì)于跨省捕獲罪犯、排查目標(biāo)犯罪車輛運(yùn)動(dòng)行徑等需要耗費(fèi)大量的工作量，而路徑規(guī)劃幫助警方節(jié)約了大量人力物力，成效快，節(jié)約了寶貴時(shí)間，也帶來(lái)了巨大便利，給公共治安、警方辦案幫助頗多，是真正意義上的便利社會(huì)。

參考文獻(xiàn)

[1] 張青.導(dǎo)航系統(tǒng)中路徑規(guī)劃的研究[D].武漢科技大學(xué)，2008.

[2] 嚴(yán)勇，姚亮亮，李朝輝.北斗衛(wèi)星導(dǎo)航系統(tǒng)現(xiàn)狀及測(cè)量中的應(yīng)用[J].經(jīng)緯天地，2018（05）：29-32.

[3] 趙學(xué)青.導(dǎo)航技術(shù)的發(fā)展與展望[A].中國(guó)指揮與控制學(xué)會(huì).2013第一屆中國(guó)指揮控制大會(huì)論文集[C].中國(guó)指揮與控制學(xué)會(huì)：中國(guó)指揮與控制學(xué)會(huì)，2013：3.

[4] 朱照榮.北斗衛(wèi)星導(dǎo)航系統(tǒng)的發(fā)展與應(yīng)用[A].衛(wèi)星導(dǎo)航系統(tǒng)應(yīng)用與繁榮2011[C].中國(guó)衛(wèi)星導(dǎo)航定位協(xié)會(huì)，2011：3.

[5] 陳軍.北斗衛(wèi)星導(dǎo)航定位系統(tǒng)應(yīng)用綜述[A].中國(guó)高科技產(chǎn)業(yè)化研究會(huì)智能信息處理產(chǎn)業(yè)化分會(huì).第九屆全國(guó)信號(hào)和智能信息處理與應(yīng)用學(xué)術(shù)會(huì)議?？痆C].中國(guó)高科技產(chǎn)業(yè)化研究會(huì)智能信息處理產(chǎn)業(yè)化分會(huì)：中國(guó)高科技產(chǎn)業(yè)化研究會(huì)，2015：4.

[6] 王運(yùn).汽車導(dǎo)航路徑規(guī)劃算法研究[J].汽車實(shí)用技術(shù)，2017（21）：202-204.