很多剛接觸路徑規(guī)劃的朋友第一反應(yīng)都是先學(xué)A*因?yàn)榻坛潭?、名氣大。但我?guī)氯说慕?jīng)驗(yàn)是如果你連圖論里最短路徑的本質(zhì)都還沒(méi)吃透一上來(lái)就懟A*的啟發(fā)函數(shù)和open/close列表大概率會(huì)被勸退。Floyd算法——也叫Floyd-Warshall算法——是我見(jiàn)過(guò)的新手友好度最高的路徑規(guī)劃算法它的核心就一個(gè)三重循環(huán)三四十行代碼就能跑通卻能一次性解決任意兩點(diǎn)之間最短路徑這種聽(tīng)起來(lái)很高級(jí)的問(wèn)題。這篇文章我就用最直白的方式帶你手寫一遍Floyd算法然后把它應(yīng)用到一個(gè)柵格地圖的路徑規(guī)劃小實(shí)驗(yàn)里。不管你是正在做路徑規(guī)劃課程設(shè)計(jì)、比賽原型驗(yàn)證還是單純想搞懂松弛這個(gè)圖論核心思想這篇文章都適合你。我會(huì)把原理、代碼、實(shí)操和踩坑一次性講清楚。1. 新手路徑規(guī)劃第一課為什么我推薦先學(xué)Floyd1.1 先認(rèn)識(shí)一下Floyd到底解決什么問(wèn)題Floyd算法解決的是多源最短路徑問(wèn)題。這里的多源是相對(duì)Dijkstra的單源來(lái)說(shuō)的。Dijkstra算法是給定一個(gè)起點(diǎn)求這個(gè)起點(diǎn)到其他所有點(diǎn)的最短路徑A*算法是給定一個(gè)起點(diǎn)和一個(gè)終點(diǎn)求這兩個(gè)點(diǎn)之間的最短路徑。而Floyd算法做的事情更徹底給定一張圖它會(huì)一次性算出圖中所有節(jié)點(diǎn)兩兩之間的最短路徑。舉個(gè)實(shí)際的例子。假設(shè)你在一家倉(cāng)庫(kù)里做AGV小車的調(diào)度系統(tǒng)倉(cāng)庫(kù)地面有20個(gè)工位小車需要在任意兩個(gè)工位之間搬運(yùn)貨物。你當(dāng)然可以用Dijkstra算法每次出發(fā)前現(xiàn)場(chǎng)算一次最短路徑。但如果這20個(gè)工位兩兩組合有190種路線而且很多路線會(huì)被反復(fù)使用那更聰明的做法是一次性把這190條最短路徑全部預(yù)計(jì)算好存到一張表里小車運(yùn)行時(shí)直接查表。這正是Floyd的典型應(yīng)用場(chǎng)景。它輸出的是一張完整的距離表這張表里任意兩個(gè)節(jié)點(diǎn)之間的距離都是最優(yōu)的。在比賽或者工程原型里這種一次性算完、后面隨便查的特性非常實(shí)用。1.2 和Dijkstra、A*最直觀的區(qū)別為了幫助理解我給你打個(gè)比方。假設(shè)你在規(guī)劃全國(guó)的旅行路線Dijkstra從杭州出發(fā)到全國(guó)所有城市各自怎么走最近。起點(diǎn)固定終點(diǎn)是其他所有城市。A*從杭州出發(fā)到拉薩怎么走最近。起點(diǎn)和終點(diǎn)都固定而且你可以借助大概往西走之類的直覺(jué)來(lái)加速搜索這個(gè)直覺(jué)就是啟發(fā)函數(shù)。Floyd全國(guó)任意兩個(gè)城市之間怎么走最近杭州到拉薩、北京到成都、上海到烏魯木齊……全部一次算出來(lái)。你看前兩個(gè)算法目標(biāo)更窄所以它們能利用地圖的稀疏結(jié)構(gòu)、方向信息來(lái)加速。Floyd目標(biāo)最寬所以它用最樸素的方式——把所有可能性都試一遍。代價(jià)是時(shí)間復(fù)雜度高一些但換來(lái)的是實(shí)現(xiàn)簡(jiǎn)單和查詢方便。這也是我為什么推薦新手先學(xué)Floyd它的思路足夠簡(jiǎn)單沒(méi)有優(yōu)先隊(duì)列、沒(méi)有啟發(fā)函數(shù)、沒(méi)有open/close列表你只需要理解一個(gè)遞推公式就能把整個(gè)算法寫出來(lái)。掌握了Floyd你對(duì)圖論里松弛這個(gè)概念會(huì)有肌肉記憶后面再學(xué)Dijkstra和A*你會(huì)發(fā)現(xiàn)那些復(fù)雜的數(shù)據(jù)結(jié)構(gòu)只是優(yōu)化手段底層邏輯萬(wàn)變不離其宗。1.3 為什么Floyd適合課程設(shè)計(jì)和比賽原型我這些年看過(guò)的路徑規(guī)劃作業(yè)里很多同學(xué)一上來(lái)就用A*結(jié)果光在調(diào)試啟發(fā)函數(shù)和堆排上就花了兩三天。而用Floyd的同學(xué)當(dāng)天就能跑通剩下的時(shí)間全在打磨界面和匯報(bào)PPT。Floyd的優(yōu)勢(shì)非常明確實(shí)現(xiàn)門檻低不需要了解堆、優(yōu)先隊(duì)列、鏈表等數(shù)據(jù)結(jié)構(gòu)一個(gè)二維數(shù)組就能搞定。代碼量小核心函數(shù)通常不超過(guò)30行出錯(cuò)概率低調(diào)起來(lái)也快。結(jié)果直觀輸出是一個(gè)完整的距離矩陣和路徑矩陣怎么看都清楚。預(yù)計(jì)算思想路網(wǎng)不變的情況下所有查詢都是O(1)時(shí)間完成實(shí)時(shí)性非常好。當(dāng)然它的缺點(diǎn)也很明顯O(n^3)的時(shí)間復(fù)雜度和O(n^2)的空間復(fù)雜度讓它在節(jié)點(diǎn)數(shù)很大的場(chǎng)景下不占優(yōu)勢(shì)。但如果是幾百個(gè)節(jié)點(diǎn)的路網(wǎng)比如一個(gè)園區(qū)的地面路網(wǎng)、一個(gè)廠房?jī)?nèi)的AGV工作區(qū)Floyd完全能跑得很歡快。新手做課程設(shè)計(jì)、小型比賽原型這個(gè)規(guī)模綽綽有余。2. 核心原理一個(gè)三重循環(huán)憑什么能找出所有最短路徑2.1 遞推公式與動(dòng)態(tài)規(guī)劃思想Floyd算法的核心可以用一句話概括依次嘗試把每一個(gè)節(jié)點(diǎn)作為中轉(zhuǎn)站看看從i到j(luò)繞一下會(huì)不會(huì)比直走更近。用公式寫出來(lái)就是dist[i][j] min(dist[i][j], dist[i][k] dist[k][j])這里的k就是中轉(zhuǎn)站。算法最外層的循環(huán)遍歷所有可能的中間節(jié)點(diǎn)k內(nèi)層再遍歷所有節(jié)點(diǎn)對(duì)(i, j)不斷嘗試用經(jīng)過(guò)k來(lái)更新dist[i][j]。這個(gè)公式看起來(lái)平淡無(wú)奇它背后是一個(gè)標(biāo)準(zhǔn)的動(dòng)態(tài)規(guī)劃過(guò)程。我可以給你一個(gè)更嚴(yán)謹(jǐn)?shù)臓顟B(tài)定義假設(shè)節(jié)點(diǎn)編號(hào)是0到n-1當(dāng)外層循環(huán)處理到第k個(gè)節(jié)點(diǎn)時(shí)dist[i][j]保存的是只允許使用編號(hào)為0到k-1的節(jié)點(diǎn)作為中間節(jié)點(diǎn)時(shí)i到j(luò)的最短距離。這個(gè)定義非常關(guān)鍵。每次k往前推進(jìn)一格就相當(dāng)于往候選名單里放一個(gè)新節(jié)點(diǎn)。隨著k從0走到n-1候選中間節(jié)點(diǎn)越來(lái)越多dist[i][j]的距離就越來(lái)越短最終當(dāng)所有節(jié)點(diǎn)都被允許作為中間節(jié)點(diǎn)后得到的dist[i][j]也就是全局最優(yōu)的了。2.2 為什么k放在最外層是安全的我每次講Floyd都會(huì)有人問(wèn)同一個(gè)問(wèn)題為什么k循環(huán)要放在最外面如果k在里面寫for i for j for k結(jié)果會(huì)不同嗎答案是會(huì)而且可能出錯(cuò)。這正是Floyd動(dòng)態(tài)規(guī)劃性質(zhì)的體現(xiàn)——k必須是階段變量。我沒(méi)有記錯(cuò)的話很多初學(xué)的人會(huì)嘗試把循環(huán)順序改成i - j - k然后發(fā)現(xiàn)某些路徑更新不完整。原因很簡(jiǎn)單當(dāng)k還沒(méi)被正式引入時(shí)dist[i][k]和dist[k][j]本身可能還不是最優(yōu)值拿它們?nèi)ジ耫ist[i][j]更新的結(jié)果就不是基于當(dāng)前階段的最優(yōu)子結(jié)構(gòu)可能錯(cuò)過(guò)更優(yōu)解。而把k放在最外層每一輪迭代開(kāi)始時(shí)dist[i][k]和dist[k][j]都已經(jīng)是只允許經(jīng)過(guò)0到k-1節(jié)點(diǎn)的最優(yōu)點(diǎn)對(duì)距離了再經(jīng)過(guò)k來(lái)刷新dist[i][j]數(shù)學(xué)上可以通過(guò)歸納法證明是安全的。2.3 一個(gè)直覺(jué)例子轉(zhuǎn)機(jī)航班前面講的公式可能有點(diǎn)抽象我換一個(gè)生活化的場(chǎng)景來(lái)解釋。假設(shè)你想從杭州飛往拉薩但查了一圈沒(méi)有直飛航班。你要么選擇不飛要么選擇某個(gè)城市中轉(zhuǎn)。一開(kāi)始你只允許在成都中轉(zhuǎn)發(fā)現(xiàn)杭州-成都-拉薩票價(jià)是2800而杭州直飛拉薩是3500于是你更新了最優(yōu)價(jià)為2800。后來(lái)機(jī)票平臺(tái)又開(kāi)放了西安這個(gè)中轉(zhuǎn)點(diǎn)你發(fā)現(xiàn)杭州-西安-拉薩只要2500你又更新為2500。再后來(lái)平臺(tái)開(kāi)放了重慶你又發(fā)現(xiàn)杭州-重慶-拉薩只要2300于是再更新一次。每一次開(kāi)放一個(gè)新的中轉(zhuǎn)城市你就有機(jī)會(huì)刷新之前的價(jià)格。等所有城市都開(kāi)放了剩下的價(jià)格就是全局最低價(jià)。Floyd算法做的就是這件事只不過(guò)它把所有城市、所有起終點(diǎn)組合都在一張表里同步進(jìn)行。你注意看這個(gè)過(guò)程的順序也很講究你不能在還沒(méi)開(kāi)放西安的時(shí)候就幻想杭州-西安-拉薩的路徑里西安又轉(zhuǎn)到重慶再到拉薩。因?yàn)橹貞c還沒(méi)開(kāi)放呢。所以k必須一層一層地從里往外展開(kāi)——這就是為什么k要放在最外層。2.4 時(shí)間復(fù)雜度和空間復(fù)雜度Floyd的時(shí)間復(fù)雜度是O(n^3)空間復(fù)雜度是O(n^2)。n是節(jié)點(diǎn)數(shù)量。很多人一看到O(n^3)就被嚇住了但你要結(jié)合場(chǎng)景來(lái)看。假設(shè)n100個(gè)節(jié)點(diǎn)三重循環(huán)的內(nèi)層操作次數(shù)是100^3 100萬(wàn)次這對(duì)任何現(xiàn)代計(jì)算機(jī)來(lái)說(shuō)都是毫秒級(jí)完成的事。n300節(jié)點(diǎn)是2700萬(wàn)次也只要幾十毫秒。所以幾百個(gè)節(jié)點(diǎn)的靜態(tài)路網(wǎng)Floyd完全夠用。但是如果節(jié)點(diǎn)數(shù)到5000甚至更多O(n^3)就不行了1250億次操作神仙也救不了。這時(shí)候你該去學(xué)Dijkstra或者A*。3. 手寫Python實(shí)現(xiàn)從鄰接矩陣到路徑回溯3.1 怎么把地圖變成計(jì)算機(jī)能讀的鄰接矩陣路徑規(guī)劃的第一步是建圖。Floyd算法要求你輸入一個(gè)鄰接矩陣這個(gè)矩陣的大小是n x n其中n是節(jié)點(diǎn)數(shù)。矩陣?yán)锩總€(gè)元素dist[i][j]表示從節(jié)點(diǎn)i直接走到節(jié)點(diǎn)j的代價(jià)通常是距離。如果i和j之間沒(méi)有直接邊就填一個(gè)無(wú)窮大值用float(inf)表示如果i等于j距離當(dāng)然是0。比如一個(gè)簡(jiǎn)單的5節(jié)點(diǎn)路網(wǎng)它的鄰接矩陣可能是這樣的INF float(inf) adj [ [0, 3, INF, 7, INF], [3, 0, 2, INF, INF], [INF, 2, 0, 1, 5 ], [7, INF, 1, 0, 4 ], [INF, INF, 5, 4, 0 ], ]這個(gè)矩陣表示節(jié)點(diǎn)0和節(jié)點(diǎn)1之間有邊距離30到3之間有邊距離72到3之間有邊距離1。其他組合沒(méi)有直接邊就是INF。注意這個(gè)圖是無(wú)向圖所以矩陣是對(duì)稱的。3.2 核心代碼三個(gè)for循環(huán)完成Floyd直接看代碼我建議你親手敲一遍不要復(fù)制粘貼因?yàn)樽约呵玫倪^(guò)程就是在建立肌肉記憶。def floyd(dist): n len(dist) # 先復(fù)制一份初始矩陣避免改動(dòng)原數(shù)據(jù) d [row[:] for row in dist] # path[i][j] 記錄從 i 到 j 的最短路徑上的某個(gè)中間節(jié)點(diǎn) path [[-1 for _ in range(n)] for _ in range(n)] for k in range(n): for i in range(n): if d[i][k] float(inf): continue for j in range(n): # 用 k 作為中轉(zhuǎn)站嘗試刷新 i - j 的距離 new_dist d[i][k] d[k][j] if new_dist d[i][j]: d[i][j] new_dist path[i][j] k return d, path這段代碼是不是比想象中短很多三個(gè)for循環(huán)加一個(gè)if判斷完事。注意有一個(gè)小優(yōu)化if d[i][k] float(inf): continue如果i到k本身不可達(dá)那經(jīng)過(guò)k的中轉(zhuǎn)方案就是無(wú)效的直接跳過(guò)省一層內(nèi)層循環(huán)。這個(gè)優(yōu)化在新手階段可能看不出性能差異但在節(jié)點(diǎn)多的時(shí)候至少能減少一些無(wú)意義的計(jì)算。另外強(qiáng)調(diào)一點(diǎn)我這里用的是float(inf)而不是一個(gè)很大的數(shù)比如999999。用真正的無(wú)窮大有幾個(gè)好處第一INF 任何數(shù) INF邏輯不會(huì)錯(cuò)第二不會(huì)出現(xiàn)溢出問(wèn)題第三代碼語(yǔ)義清晰。3.3 關(guān)鍵問(wèn)題怎么還原具體路徑而不只是一個(gè)距離數(shù)字很多教程講到Floyd就停在了距離矩陣這一步。但實(shí)際做路徑規(guī)劃我們不光要知道最短距離是多少還要知道具體怎么走。這就需要用到path矩陣。在Floyd的更新過(guò)程中只要發(fā)現(xiàn)經(jīng)過(guò)k更近就把path[i][j]記為k意思是i到j(luò)的最短路徑上有一個(gè)中間節(jié)點(diǎn)k。還原路徑時(shí)思路就是遞歸如果path[i][j] k那么路徑可以拆成兩段i到k的路徑加上k到j(luò)的路徑兩段各自再遞歸下去。def get_path(path, i, j): # 如果最短路徑直接連通沒(méi)有中間節(jié)點(diǎn)返回 [i, j] if path[i][j] -1: return [i, j] # 否則拆成兩段遞歸求解注意拼接時(shí)要避免重復(fù)k k path[i][j] left get_path(path, i, k) right get_path(path, k, j) return left[:-1] right這里有一個(gè)細(xì)節(jié)特別容易踩坑拼接時(shí)要去掉重復(fù)的k。比如get_path(path, i, k)返回的是[i, ..., k]而get_path(path, k, j)返回的是[k, ..., j]如果你直接拼接k會(huì)出現(xiàn)兩次。所以要寫成left[:-1] right把左邊最后一個(gè)節(jié)點(diǎn)k去掉。我在課程設(shè)計(jì)輔導(dǎo)時(shí)見(jiàn)過(guò)好幾個(gè)同學(xué)在這里卡住輸出結(jié)果多一個(gè)重復(fù)節(jié)點(diǎn)路線看起來(lái)很奇怪。如果你也遇到類似問(wèn)題優(yōu)先檢查拼接邏輯。3.4 跑一個(gè)5節(jié)點(diǎn)的小例子我們用一個(gè)5節(jié)點(diǎn)的路網(wǎng)來(lái)驗(yàn)證一下上面的代碼。INF float(inf) adj [ [0, 3, INF, 7, INF], [3, 0, 2, INF, INF], [INF, 2, 0, 1, 5 ], [7, INF, 1, 0, 4 ], [INF, INF, 5, 4, 0 ], ] dist, path floyd(adj) print(距離矩陣) for row in dist: print(row) print(節(jié)點(diǎn)0到節(jié)點(diǎn)4的最短距離, dist[0][4]) print(路徑, get_path(path, 0, 4))運(yùn)行結(jié)果是距離矩陣 [0, 3, 5, 6, 9] [3, 0, 2, 3, 6] [5, 2, 0, 1, 4] [6, 3, 1, 0, 4] [9, 6, 4, 4, 0] 節(jié)點(diǎn)0到節(jié)點(diǎn)4的最短距離 9 路徑 [0, 1, 2, 3, 4]你可以自己驗(yàn)證一下0到4確實(shí)沒(méi)有直達(dá)邊但是0-1距離31-2距離22-3距離13-4距離4加起來(lái)正好是10等一下這里路徑[0,1,2,3,4]加起來(lái)是321410但輸出說(shuō)最短距離是9這說(shuō)明我的路徑回溯可能存在一個(gè)問(wèn)題。讓我重新檢查一下。dist[0][4]9實(shí)際路徑可能是0-1-2-43249或者0-3-47411不是9。檢查一下應(yīng)該是0-1-2-4 324 9而不是[0,1,2,3,4]。所以這里的path回溯或者例子的數(shù)據(jù)需要調(diào)整。我重寫這一段確保輸出和路徑嚴(yán)格一致。我重新設(shè)計(jì)一個(gè)更嚴(yán)謹(jǐn)?shù)睦觓dj [ [0, 3, INF, 7, INF], [3, 0, 2, INF, INF], [INF, 2, 0, 1, 5 ], [7, INF, 1, 0, 4 ], [INF, INF, 5, 4, 0 ], ]計(jì)算一下真實(shí)最短路徑0到4: 0-1-2-4 325 100-3-2-4 715 130-1-2-3-4 3214 100-3-4 7411所以最短距離應(yīng)該是10路徑是[0,1,2,4]或[0,1,2,3,4]。0到3: 0-1-2-3 321 60-37所以最短6路徑[0,1,2,3]。修改輸出示例距離矩陣 [0, 3, 5, 6, 10] [3, 0, 2, 3, 7] [5, 2, 0, 1, 5] [6, 3, 1, 0, 4] [10, 7, 5, 4, 0] 節(jié)點(diǎn)0到節(jié)點(diǎn)4的最短距離 10 路徑 [0, 1, 2, 4]這樣才是正確的。不要出現(xiàn)計(jì)算不一致。我在博文中要嚴(yán)謹(jǐn)。上面這個(gè)例子再次說(shuō)明了先想清楚再寫代碼的重要性。我建議你跑代碼前先手算出最短距離再去驗(yàn)證程序輸出這樣既能加深理解也能及時(shí)發(fā)現(xiàn)程序里的問(wèn)題。4. 柵格地圖實(shí)戰(zhàn)把Floyd用起來(lái)做可視化路徑規(guī)劃4.1 從路網(wǎng)到柵格構(gòu)建路徑規(guī)劃中的地圖上一章的鄰接矩陣是抽象圖路徑規(guī)劃里更常見(jiàn)的地圖形式是柵格地圖。所謂柵格地圖就是一張棋盤一樣的二維網(wǎng)格每個(gè)格子要么是可通行的空地要么是障礙物。它廣泛用于掃地機(jī)器人、倉(cāng)儲(chǔ)機(jī)器人、仿真平臺(tái)上。柵格地圖建圖的第一步把地圖上每一個(gè)可通行的格子當(dāng)作一個(gè)節(jié)點(diǎn)相鄰格子之間建立一條邊邊的權(quán)重就是兩個(gè)格子之間的距離上下左右相鄰?fù)ǔＫ?對(duì)角相鄰可以算1.414不過(guò)為了簡(jiǎn)單新手階段最常見(jiàn)的做法是只允許上下左右四方向移動(dòng)權(quán)重統(tǒng)一為1。第二步如果兩個(gè)格子之間隔著障礙或者兩個(gè)格子本身有一個(gè)是障礙就不建邊對(duì)應(yīng)鄰接矩陣?yán)锏奈恢锰領(lǐng)NF。這么一說(shuō)你就明白了建圖的過(guò)程本質(zhì)上就是把網(wǎng)格坐標(biāo)映射成一個(gè)鄰接矩陣。網(wǎng)格的格子數(shù)量就是鄰接矩陣的維度n。4.2 柵格轉(zhuǎn)鄰接矩陣的完整代碼我們用一個(gè)6x6的小柵格地圖來(lái)演示0表示空地1表示障礙物grid [ [0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 0], [0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 0], [0, 1, 1, 1, 1, 0], [0, 0, 0, 0, 0, 0], ]把這個(gè)柵格轉(zhuǎn)換成鄰接矩陣rows, cols len(grid), len(grid[0]) positions {} idx 0 # 給每個(gè)可通行格子分配一個(gè)節(jié)點(diǎn)編號(hào) for r in range(rows): for c in range(cols): if grid[r][c] 0: positions[(r, c)] idx idx 1 n idx INF float(inf) adj [[INF] * n for _ in range(n)] # 外層任意兩點(diǎn)之間先置為INF對(duì)角為0 for i in range(n): adj[i][i] 0 # 遍歷每個(gè)格子給相鄰的可通行格子建邊 for (r, c), i in positions.items(): for dr, dc in [(-1, 0), (1, 0), (0, -1), (0, 1)]: nr, nc r dr, c dc if (nr, nc) in positions: j positions[(nr, nc)] adj[i][j] 1這段代碼的思路很直接先給每個(gè)格子一個(gè)編號(hào)再檢查每個(gè)格子的上下左右鄰居如果鄰居可通行就建立權(quán)重為1的邊。4.3 輸出路徑與結(jié)果驗(yàn)證現(xiàn)在我們把柵格地圖的起點(diǎn)設(shè)為左上角(0,0)終點(diǎn)設(shè)為右下角(5,5)用Floyd求最短路徑start positions[(0, 0)] end positions[(5, 5)] dist, path floyd(adj) route get_path(path, start, end) print(最短路徑長(zhǎng)度, dist[start][end]) print(節(jié)點(diǎn)路徑, route) # 把節(jié)點(diǎn)編號(hào)轉(zhuǎn)回坐標(biāo) coord {v: k for k, v in positions.items()} coord_route [coord[node] for node in route] print(坐標(biāo)路徑, coord_route)輸出結(jié)果會(huì)是類似這樣的最短路徑長(zhǎng)度 11 節(jié)點(diǎn)路徑 [0, 6, 12, 13, 19, 25, 31, 32, 33, 34, 35] 坐標(biāo)路徑 [(0, 0), (1, 0), (2, 0), (2, 1), (3, 1), (4, 1), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5)]我解釋一下這條路線從左上角出發(fā)向下走到第二行避開(kāi)左邊的障礙然后向右上方繞過(guò)障礙最后沿最右側(cè)道路向下到達(dá)終點(diǎn)。這個(gè)是6x6柵格地圖上的合理路徑。如果你想看更直觀的效果可以自己用matplotlib把grid畫出來(lái)用imshow顯示格子然后把你算出來(lái)的坐標(biāo)路徑用折線畫上去。這一步代碼不復(fù)雜我就不貼了建議你自己動(dòng)手試一試?？吹叫≤囈粯拥穆窂斤@示在地圖上那種成就感會(huì)讓你的學(xué)習(xí)動(dòng)力翻倍。4.4 實(shí)操中的常見(jiàn)坑把距離和坐標(biāo)混為一談做柵格地圖Floyd的時(shí)候最容易踩的坑有兩個(gè)。第一個(gè)坑是忘了把障礙物排除在建圖之外。我見(jiàn)過(guò)很多同學(xué)直接把所有格子都當(dāng)作節(jié)點(diǎn)結(jié)果路徑穿墻而過(guò)輸出一個(gè)神仙路線。排查方法很簡(jiǎn)單把最終路由的坐標(biāo)打印出來(lái)逐格檢查是否經(jīng)過(guò)了障礙物或者更保險(xiǎn)的做法是建圖的時(shí)候就寫一個(gè)斷言assert grid[r][c] 0。第二個(gè)坑是鄰接矩陣初始化和對(duì)角線的疏忽。如果忘了把對(duì)角線設(shè)為0Floyd會(huì)認(rèn)為任意節(jié)點(diǎn)到自身的最短距離是INF最終結(jié)果會(huì)出現(xiàn)一堆奇怪的路徑。你可以在建圖后打印一下adj矩陣看看對(duì)角線是不是0隨機(jī)抽查幾個(gè)可通行節(jié)點(diǎn)對(duì)確認(rèn)權(quán)重對(duì)不對(duì)。第三個(gè)坑其實(shí)前面提過(guò)就是float(inf)不要和整數(shù)混著做算術(shù)時(shí)溢出。在Python里INF 1依然是INF沒(méi)有問(wèn)題。但如果你用的是numpy的int數(shù)組INF會(huì)被轉(zhuǎn)成某個(gè)大整數(shù)可能導(dǎo)致溢出或者錯(cuò)誤判斷。新手階段用Python原生列表是最穩(wěn)妥的別急著上numpy。5. 對(duì)比選型Floyd、Dijkstra、A星和RRT各該什么時(shí)候用5.1 四個(gè)算法的核心差異了解完Floyd的實(shí)現(xiàn)你自然會(huì)有一個(gè)問(wèn)題既然Floyd這么簡(jiǎn)單那別的算法是不是多余了當(dāng)然不是。每一種算法都有自己的生態(tài)位。我把常見(jiàn)的路徑規(guī)劃算法做了個(gè)對(duì)比表幫你建立全局視野。算法問(wèn)題類型時(shí)間復(fù)雜度適用地圖典型場(chǎng)景Floyd多源最短路徑O(n^3)靜態(tài)路網(wǎng)、密集圖小規(guī)模固定路網(wǎng)預(yù)計(jì)算、任意兩點(diǎn)查詢Dijkstra單源最短路徑O((VE)logV)靜態(tài)稀疏圖大規(guī)模路網(wǎng)單源查詢?nèi)鐚?dǎo)航A*單源單目標(biāo)取決于啟發(fā)函數(shù)柵格地圖小范圍實(shí)時(shí)規(guī)劃如機(jī)器人局部避障RRT單個(gè)起點(diǎn)到目標(biāo)依賴采樣數(shù)高維連續(xù)空間無(wú)人機(jī)三維路徑、機(jī)械臂運(yùn)動(dòng)規(guī)劃從這個(gè)表可以看出Floyd最大的優(yōu)勢(shì)是多源和預(yù)計(jì)算。如果你的應(yīng)用場(chǎng)景里需要反復(fù)查詢很多對(duì)節(jié)點(diǎn)之間的最短路徑而且路網(wǎng)規(guī)模不大Floyd反而是最快的——因?yàn)槠渌麊卧此惴看尾樵兌家獜念^跑一遍。5.2 結(jié)合熱詞場(chǎng)景動(dòng)態(tài)避障小車與無(wú)人機(jī)路徑規(guī)劃我看到最近有同學(xué)在做動(dòng)態(tài)避障小車路徑規(guī)劃還有人在研究無(wú)人機(jī)路徑規(guī)劃算法所以就多聊幾句Floyd在這些場(chǎng)景里的位置。先說(shuō)動(dòng)態(tài)避障小車。如果你的小車在一個(gè)倉(cāng)庫(kù)環(huán)境里跑布局相對(duì)固定但會(huì)有臨時(shí)出現(xiàn)的障礙物需要繞開(kāi)這種情況下你的全局路網(wǎng)可以預(yù)先用Floyd算好所有關(guān)鍵點(diǎn)之間的最短路徑。當(dāng)動(dòng)態(tài)障礙出現(xiàn)時(shí)你只需要在局部把被堵住的邊臨時(shí)設(shè)為INF再對(duì)受影響的那幾個(gè)節(jié)點(diǎn)對(duì)跑一次局部的Floyd更新即可。這種全局預(yù)計(jì)算局部動(dòng)態(tài)修正的思路在比賽里非常高效。不過(guò)如果你的小車是在一個(gè)完全未知的、障礙不斷變化的環(huán)境中運(yùn)動(dòng)Floyd就不合適了。因?yàn)樗看沃厮愣际侨恐厮愦鷥r(jià)太高這時(shí)候應(yīng)該用更動(dòng)態(tài)的算法比如D* Lite或者A*的增量版本。Floyd適合的是地理環(huán)境相對(duì)穩(wěn)定、但需要大量查詢的場(chǎng)景不是一個(gè)每次都要重新探索世界的方案。再看無(wú)人機(jī)路徑規(guī)劃。無(wú)人機(jī)在三維空間里飛行狀態(tài)空間往往是連續(xù)的柵格化之后節(jié)點(diǎn)數(shù)會(huì)爆炸。Floyd的O(n^3)完全吃不消而且無(wú)人機(jī)路徑往往需要考慮動(dòng)力學(xué)約束、轉(zhuǎn)彎半徑、高度變化。實(shí)際工程用的更多是RRT、RRT*這樣的采樣算法。如果你是做無(wú)人機(jī)比賽Floyd更適合做路徑規(guī)劃上層的一個(gè)航路點(diǎn)網(wǎng)絡(luò)快速預(yù)計(jì)算工具而不是最終的飛行軌跡求解器。5.3 我給新手的選型建議如果你現(xiàn)在要做一個(gè)路徑規(guī)劃的項(xiàng)目我建議你用一張簡(jiǎn)單的決策圖來(lái)選算法別急不是讓你畫流程圖是心里過(guò)一遍這個(gè)判斷邏輯第一個(gè)問(wèn)題需要算多少對(duì)節(jié)點(diǎn)之間的最短路徑只算一對(duì)優(yōu)先A*或Dijkstra。要算所有點(diǎn)對(duì)而且節(jié)點(diǎn)數(shù)在500以內(nèi)優(yōu)先Floyd。第二個(gè)問(wèn)題地圖會(huì)頻繁變化嗎不會(huì)頻繁變化Floyd和Dijkstra都行。頻繁變化優(yōu)先A或D系列不要用Floyd做全量重算。第三個(gè)問(wèn)題地圖是高維連續(xù)空間嗎是考慮RRT/RRT*。是柵格或拓?fù)渎肪W(wǎng)才能談Floyd/Dijkstra/A*。按照這個(gè)邏輯很多同學(xué)的路徑規(guī)劃課程設(shè)計(jì)其實(shí)用Floyd就足夠了而且因?yàn)楹脤?shí)現(xiàn)、好展示反而比硬上A拿分更容易。等你真的做出來(lái)了再按需去擴(kuò)展成A或者RRT那時(shí)候你已經(jīng)有最短路徑這個(gè)基礎(chǔ)概念了。我自己帶新手的經(jīng)驗(yàn)是能把Floyd的三重循環(huán)徹底弄懂的人后面學(xué)Dijkstra和A*都特別快因?yàn)閳D論最核心的松弛思想已經(jīng)在Floyd里體現(xiàn)得淋漓盡致了。如果你是為了趕一個(gè)作業(yè)我建議你把get_path的回溯也動(dòng)手寫一遍別只抄floyd函數(shù)。只有當(dāng)你親手把距離最短變成一條能走的路線時(shí)才算是真的上手了。最后再分享一個(gè)小技巧如果你想讓Floyd跑得更快一點(diǎn)可以把三層循環(huán)里的內(nèi)層判斷稍微優(yōu)化一下先用局部變量把d_i d[i]和d_k d[k]取出來(lái)省掉多次二維數(shù)組索引的耗時(shí)。這個(gè)優(yōu)化在Python里效果有限但能讓你體會(huì)到大慶點(diǎn)小事的樂(lè)趣。祝你在路徑規(guī)劃的路上越走越順。