劃實(shí)戰(zhàn))
很多剛接觸路徑規(guī)劃的朋友第一反應(yīng)都是先學(xué)A*因?yàn)榻坛潭唷⒚麣獯蟆５規(guī)氯说慕?jīng)驗(yàn)是如果你連圖論里最短路徑的本質(zhì)都還沒吃透一上來就懟A*的啟發(fā)函數(shù)和open/close列表大概率會(huì)被勸退。Floyd算法——也叫Floyd-Warshall算法——是我見過的新手友好度最高的路徑規(guī)劃算法它的核心就一個(gè)三重循環(huán)三四十行代碼就能跑通卻能一次性解決任意兩點(diǎn)之間最短路徑這種聽起來很高級(jí)的問題。這篇文章我就用最直白的方式帶你手寫一遍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到底解決什么問題Floyd算法解決的是多源最短路徑問題。這里的多源是相對(duì)Dijkstra的單源來說的。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è)你在一家倉庫里做AGV小車的調(diào)度系統(tǒng)倉庫地面有20個(gè)工位小車需要在任意兩個(gè)工位之間搬運(yùn)貨物。你當(dāng)然可以用Dijkstra算法每次出發(fā)前現(xiàn)場算一次最短路徑。但如果這20個(gè)工位兩兩組合有190種路線而且很多路線會(huì)被反復(fù)使用那更聰明的做法是一次性把這190條最短路徑全部預(yù)計(jì)算好存到一張表里小車運(yùn)行時(shí)直接查表。這正是Floyd的典型應(yīng)用場景。它輸出的是一張完整的距離表這張表里任意兩個(gè)節(jié)點(diǎn)之間的距離都是最優(yōu)的。在比賽或者工程原型里這種一次性算完、后面隨便查的特性非常實(shí)用。1.2 和Dijkstra、A*最直觀的區(qū)別為了幫助理解我給你打個(gè)比方。假設(shè)你在規(guī)劃全國的旅行路線Dijkstra從杭州出發(fā)到全國所有城市各自怎么走最近。起點(diǎn)固定終點(diǎn)是其他所有城市。A*從杭州出發(fā)到拉薩怎么走最近。起點(diǎn)和終點(diǎn)都固定而且你可以借助大概往西走之類的直覺來加速搜索這個(gè)直覺就是啟發(fā)函數(shù)。Floyd全國任意兩個(gè)城市之間怎么走最近杭州到拉薩、北京到成都、上海到烏魯木齊……全部一次算出來。你看前兩個(gè)算法目標(biāo)更窄所以它們能利用地圖的稀疏結(jié)構(gòu)、方向信息來加速。Floyd目標(biāo)最寬所以它用最樸素的方式——把所有可能性都試一遍。代價(jià)是時(shí)間復(fù)雜度高一些但換來的是實(shí)現(xiàn)簡單和查詢方便。這也是我為什么推薦新手先學(xué)Floyd它的思路足夠簡單沒有優(yōu)先隊(duì)列、沒有啟發(fā)函數(shù)、沒有open/close列表你只需要理解一個(gè)遞推公式就能把整個(gè)算法寫出來。掌握了Floyd你對(duì)圖論里松弛這個(gè)概念會(huì)有肌肉記憶后面再學(xué)Dijkstra和A*你會(huì)發(fā)現(xiàn)那些復(fù)雜的數(shù)據(jù)結(jié)構(gòu)只是優(yōu)化手段底層邏輯萬變不離其宗。1.3 為什么Floyd適合課程設(shè)計(jì)和比賽原型我這些年看過的路徑規(guī)劃作業(yè)里很多同學(xué)一上來就用A*結(jié)果光在調(diào)試啟發(fā)函數(shù)和堆排上就花了兩三天。而用Floyd的同學(xué)當(dāng)天就能跑通剩下的時(shí)間全在打磨界面和匯報(bào)PPT。Floyd的優(yōu)勢非常明確實(shí)現(xiàn)門檻低不需要了解堆、優(yōu)先隊(duì)列、鏈表等數(shù)據(jù)結(jié)構(gòu)一個(gè)二維數(shù)組就能搞定。代碼量小核心函數(shù)通常不超過30行出錯(cuò)概率低調(diào)起來也快。結(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ù)很大的場景下不占優(yōu)勢。但如果是幾百個(gè)節(jié)點(diǎn)的路網(wǎng)比如一個(gè)園區(qū)的地面路網(wǎng)、一個(gè)廠房內(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ì)比直走更近。用公式寫出來就是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)過k來更新dist[i][j]。這個(gè)公式看起來平淡無奇它背后是一個(gè)標(biāo)準(zhǔn)的動(dòng)態(tài)規(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)越來越多dist[i][j]的距離就越來越短最終當(dāng)所有節(jié)點(diǎn)都被允許作為中間節(jié)點(diǎn)后得到的dist[i][j]也就是全局最優(yōu)的了。2.2 為什么k放在最外層是安全的我每次講Floyd都會(huì)有人問同一個(gè)問題為什么k循環(huán)要放在最外面如果k在里面寫for i for j for k結(jié)果會(huì)不同嗎答案是會(huì)而且可能出錯(cuò)。這正是Floyd動(dòng)態(tài)規(guī)劃性質(zhì)的體現(xiàn)——k必須是階段變量。我沒有記錯(cuò)的話很多初學(xué)的人會(huì)嘗試把循環(huán)順序改成i - j - k然后發(fā)現(xiàn)某些路徑更新不完整。原因很簡單當(dāng)k還沒被正式引入時(shí)dist[i][k]和dist[k][j]本身可能還不是最優(yōu)值拿它們?nèi)ジ耫ist[i][j]更新的結(jié)果就不是基于當(dāng)前階段的最優(yōu)子結(jié)構(gòu)可能錯(cuò)過更優(yōu)解。而把k放在最外層每一輪迭代開始時(shí)dist[i][k]和dist[k][j]都已經(jīng)是只允許經(jīng)過0到k-1節(jié)點(diǎn)的最優(yōu)點(diǎn)對(duì)距離了再經(jīng)過k來刷新dist[i][j]數(shù)學(xué)上可以通過歸納法證明是安全的。2.3 一個(gè)直覺例子轉(zhuǎn)機(jī)航班前面講的公式可能有點(diǎn)抽象我換一個(gè)生活化的場景來解釋。假設(shè)你想從杭州飛往拉薩但查了一圈沒有直飛航班。你要么選擇不飛要么選擇某個(gè)城市中轉(zhuǎn)。一開始你只允許在成都中轉(zhuǎn)發(fā)現(xiàn)杭州-成都-拉薩票價(jià)是2800而杭州直飛拉薩是3500于是你更新了最優(yōu)價(jià)為2800。后來機(jī)票平臺(tái)又開放了西安這個(gè)中轉(zhuǎn)點(diǎn)你發(fā)現(xiàn)杭州-西安-拉薩只要2500你又更新為2500。再后來平臺(tái)開放了重慶你又發(fā)現(xiàn)杭州-重慶-拉薩只要2300于是再更新一次。每一次開放一個(gè)新的中轉(zhuǎn)城市你就有機(jī)會(huì)刷新之前的價(jià)格。等所有城市都開放了剩下的價(jià)格就是全局最低價(jià)。Floyd算法做的就是這件事只不過它把所有城市、所有起終點(diǎn)組合都在一張表里同步進(jìn)行。你注意看這個(gè)過程的順序也很講究你不能在還沒開放西安的時(shí)候就幻想杭州-西安-拉薩的路徑里西安又轉(zhuǎn)到重慶再到拉薩。因?yàn)橹貞c還沒開放呢。所以k必須一層一層地從里往外展開——這就是為什么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é)合場景來看。假設(shè)n100個(gè)節(jié)點(diǎn)三重循環(huán)的內(nèi)層操作次數(shù)是100^3 100萬次這對(duì)任何現(xiàn)代計(jì)算機(jī)來說都是毫秒級(jí)完成的事。n300節(jié)點(diǎn)是2700萬次也只要幾十毫秒。所以幾百個(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之間沒有直接邊就填一個(gè)無窮大值用float(inf)表示如果i等于j距離當(dāng)然是0。比如一個(gè)簡單的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。其他組合沒有直接邊就是INF。注意這個(gè)圖是無向圖所以矩陣是對(duì)稱的。3.2 核心代碼三個(gè)for循環(huán)完成Floyd直接看代碼我建議你親手敲一遍不要復(fù)制粘貼因?yàn)樽约呵玫倪^程就是在建立肌肉記憶。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)過k的中轉(zhuǎn)方案就是無效的直接跳過省一層內(nèi)層循環(huán)。這個(gè)優(yōu)化在新手階段可能看不出性能差異但在節(jié)點(diǎn)多的時(shí)候至少能減少一些無意義的計(jì)算。另外強(qiáng)調(diào)一點(diǎn)我這里用的是float(inf)而不是一個(gè)很大的數(shù)比如999999。用真正的無窮大有幾個(gè)好處第一INF 任何數(shù) INF邏輯不會(huì)錯(cuò)第二不會(huì)出現(xiàn)溢出問題第三代碼語義清晰。3.3 關(guān)鍵問題怎么還原具體路徑而不只是一個(gè)距離數(shù)字很多教程講到Floyd就停在了距離矩陣這一步。但實(shí)際做路徑規(guī)劃我們不光要知道最短距離是多少還要知道具體怎么走。這就需要用到path矩陣。在Floyd的更新過程中只要發(fā)現(xiàn)經(jīng)過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): # 如果最短路徑直接連通沒有中間節(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í)見過好幾個(gè)同學(xué)在這里卡住輸出結(jié)果多一個(gè)重復(fù)節(jié)點(diǎn)路線看起來很奇怪。如果你也遇到類似問題優(yōu)先檢查拼接邏輯。3.4 跑一個(gè)5節(jié)點(diǎn)的小例子我們用一個(gè)5節(jié)點(diǎn)的路網(wǎng)來驗(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í)沒有直達(dá)邊但是0-1距離31-2距離22-3距離13-4距離4加起來正好是10等一下這里路徑[0,1,2,3,4]加起來是321410但輸出說最短距離是9這說明我的路徑回溯可能存在一個(gè)問題。讓我重新檢查一下。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è)例子再次說明了先想清楚再寫代碼的重要性。我建議你跑代碼前先手算出最短距離再去驗(yàn)證程序輸出這樣既能加深理解也能及時(shí)發(fā)現(xiàn)程序里的問題。4. 柵格地圖實(shí)戰(zhàn)把Floyd用起來做可視化路徑規(guī)劃4.1 從路網(wǎng)到柵格構(gòu)建路徑規(guī)劃中的地圖上一章的鄰接矩陣是抽象圖路徑規(guī)劃里更常見的地圖形式是柵格地圖。所謂柵格地圖就是一張棋盤一樣的二維網(wǎng)格每個(gè)格子要么是可通行的空地要么是障礙物。它廣泛用于掃地機(jī)器人、倉儲(chǔ)機(jī)器人、仿真平臺(tái)上。柵格地圖建圖的第一步把地圖上每一個(gè)可通行的格子當(dāng)作一個(gè)節(jié)點(diǎn)相鄰格子之間建立一條邊邊的權(quán)重就是兩個(gè)格子之間的距離上下左右相鄰?fù)ǔＫ?對(duì)角相鄰可以算1.414不過為了簡單新手階段最常見的做法是只允許上下左右四方向移動(dòng)權(quán)重統(tǒng)一為1。第二步如果兩個(gè)格子之間隔著障礙或者兩個(gè)格子本身有一個(gè)是障礙就不建邊對(duì)應(yīng)鄰接矩陣?yán)锏奈恢锰領(lǐng)NF。這么一說你就明白了建圖的過程本質(zhì)上就是把網(wǎng)格坐標(biāo)映射成一個(gè)鄰接矩陣。網(wǎng)格的格子數(shù)量就是鄰接矩陣的維度n。4.2 柵格轉(zhuǎn)鄰接矩陣的完整代碼我們用一個(gè)6x6的小柵格地圖來演示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(最短路徑長度, 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ì)是類似這樣的最短路徑長度 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ā)向下走到第二行避開左邊的障礙然后向右上方繞過障礙最后沿最右側(cè)道路向下到達(dá)終點(diǎn)。這個(gè)是6x6柵格地圖上的合理路徑。如果你想看更直觀的效果可以自己用matplotlib把grid畫出來用imshow顯示格子然后把你算出來的坐標(biāo)路徑用折線畫上去。這一步代碼不復(fù)雜我就不貼了建議你自己動(dòng)手試一試。看到小車一樣的路徑顯示在地圖上那種成就感會(huì)讓你的學(xué)習(xí)動(dòng)力翻倍。4.4 實(shí)操中的常見坑把距離和坐標(biāo)混為一談做柵格地圖Floyd的時(shí)候最容易踩的坑有兩個(gè)。第一個(gè)坑是忘了把障礙物排除在建圖之外。我見過很多同學(xué)直接把所有格子都當(dāng)作節(jié)點(diǎn)結(jié)果路徑穿墻而過輸出一個(gè)神仙路線。排查方法很簡單把最終路由的坐標(biāo)打印出來逐格檢查是否經(jīng)過了障礙物或者更保險(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í)前面提過就是float(inf)不要和整數(shù)混著做算術(shù)時(shí)溢出。在Python里INF 1依然是INF沒有問題。但如果你用的是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è)問題既然Floyd這么簡單那別的算法是不是多余了當(dāng)然不是。每一種算法都有自己的生態(tài)位。我把常見的路徑規(guī)劃算法做了個(gè)對(duì)比表幫你建立全局視野。算法問題類型時(shí)間復(fù)雜度適用地圖典型場景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ù)空間無人機(jī)三維路徑、機(jī)械臂運(yùn)動(dòng)規(guī)劃從這個(gè)表可以看出Floyd最大的優(yōu)勢是多源和預(yù)計(jì)算。如果你的應(yīng)用場景里需要反復(fù)查詢很多對(duì)節(jié)點(diǎn)之間的最短路徑而且路網(wǎng)規(guī)模不大Floyd反而是最快的——因?yàn)槠渌麊卧此惴看尾樵兌家獜念^跑一遍。5.2 結(jié)合熱詞場景動(dòng)態(tài)避障小車與無人機(jī)路徑規(guī)劃我看到最近有同學(xué)在做動(dòng)態(tài)避障小車路徑規(guī)劃還有人在研究無人機(jī)路徑規(guī)劃算法所以就多聊幾句Floyd在這些場景里的位置。先說動(dòng)態(tài)避障小車。如果你的小車在一個(gè)倉庫環(huán)境里跑布局相對(duì)固定但會(huì)有臨時(shí)出現(xiàn)的障礙物需要繞開這種情況下你的全局路網(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)修正的思路在比賽里非常高效。不過如果你的小車是在一個(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)定、但需要大量查詢的場景不是一個(gè)每次都要重新探索世界的方案。再看無人機(jī)路徑規(guī)劃。無人機(jī)在三維空間里飛行狀態(tài)空間往往是連續(xù)的柵格化之后節(jié)點(diǎn)數(shù)會(huì)爆炸。Floyd的O(n^3)完全吃不消而且無人機(jī)路徑往往需要考慮動(dòng)力學(xué)約束、轉(zhuǎn)彎半徑、高度變化。實(shí)際工程用的更多是RRT、RRT*這樣的采樣算法。如果你是做無人機(jī)比賽Floyd更適合做路徑規(guī)劃上層的一個(gè)航路點(diǎn)網(wǎng)絡(luò)快速預(yù)計(jì)算工具而不是最終的飛行軌跡求解器。5.3 我給新手的選型建議如果你現(xiàn)在要做一個(gè)路徑規(guī)劃的項(xiàng)目我建議你用一張簡單的決策圖來選算法別急不是讓你畫流程圖是心里過一遍這個(gè)判斷邏輯第一個(gè)問題需要算多少對(duì)節(jié)點(diǎn)之間的最短路徑只算一對(duì)優(yōu)先A*或Dijkstra。要算所有點(diǎn)對(duì)而且節(jié)點(diǎn)數(shù)在500以內(nèi)優(yōu)先Floyd。第二個(gè)問題地圖會(huì)頻繁變化嗎不會(huì)頻繁變化Floyd和Dijkstra都行。頻繁變化優(yōu)先A或D系列不要用Floyd做全量重算。第三個(gè)問題地圖是高維連續(xù)空間嗎是考慮RRT/RRT*。是柵格或拓?fù)渎肪W(wǎng)才能談Floyd/Dijkstra/A*。按照這個(gè)邏輯很多同學(xué)的路徑規(guī)劃課程設(shè)計(jì)其實(shí)用Floyd就足夠了而且因?yàn)楹脤?shí)現(xiàn)、好展示反而比硬上A拿分更容易。等你真的做出來了再按需去擴(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]取出來省掉多次二維數(shù)組索引的耗時(shí)。這個(gè)優(yōu)化在Python里效果有限但能讓你體會(huì)到大慶點(diǎn)小事的樂趣。祝你在路徑規(guī)劃的路上越走越順。 Floyd算法入門：從最短路徑原理到柵格地圖路徑規(guī)劃實(shí)戰(zhàn)
很多剛接觸路徑規(guī)劃的朋友第一反應(yīng)都是先學(xué)A*因?yàn)榻坛潭唷⒚麣獯蟆５規(guī)氯说慕?jīng)驗(yàn)是如果你連圖論里最短路徑的本質(zhì)都還沒吃透一上來就懟A*的啟發(fā)函數(shù)和open/close列表大概率會(huì)被勸退。Floyd算法——也叫Floyd-Warshall算法——是我見過的新手友好度最高的路徑規(guī)劃算法它的核心就一個(gè)三重循環(huán)三四十行代碼就能跑通卻能一次性解決任意兩點(diǎn)之間最短路徑這種聽起來很高級(jí)的問題。這篇文章我就用最直白的方式帶你手寫一遍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到底解決什么問題Floyd算法解決的是多源最短路徑問題。這里的多源是相對(duì)Dijkstra的單源來說的。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è)你在一家倉庫里做AGV小車的調(diào)度系統(tǒng)倉庫地面有20個(gè)工位小車需要在任意兩個(gè)工位之間搬運(yùn)貨物。你當(dāng)然可以用Dijkstra算法每次出發(fā)前現(xiàn)場算一次最短路徑。但如果這20個(gè)工位兩兩組合有190種路線而且很多路線會(huì)被反復(fù)使用那更聰明的做法是一次性把這190條最短路徑全部預(yù)計(jì)算好存到一張表里小車運(yùn)行時(shí)直接查表。這正是Floyd的典型應(yīng)用場景。它輸出的是一張完整的距離表這張表里任意兩個(gè)節(jié)點(diǎn)之間的距離都是最優(yōu)的。在比賽或者工程原型里這種一次性算完、后面隨便查的特性非常實(shí)用。1.2 和Dijkstra、A*最直觀的區(qū)別為了幫助理解我給你打個(gè)比方。假設(shè)你在規(guī)劃全國的旅行路線Dijkstra從杭州出發(fā)到全國所有城市各自怎么走最近。起點(diǎn)固定終點(diǎn)是其他所有城市。A*從杭州出發(fā)到拉薩怎么走最近。起點(diǎn)和終點(diǎn)都固定而且你可以借助大概往西走之類的直覺來加速搜索這個(gè)直覺就是啟發(fā)函數(shù)。Floyd全國任意兩個(gè)城市之間怎么走最近杭州到拉薩、北京到成都、上海到烏魯木齊……全部一次算出來。你看前兩個(gè)算法目標(biāo)更窄所以它們能利用地圖的稀疏結(jié)構(gòu)、方向信息來加速。Floyd目標(biāo)最寬所以它用最樸素的方式——把所有可能性都試一遍。代價(jià)是時(shí)間復(fù)雜度高一些但換來的是實(shí)現(xiàn)簡單和查詢方便。這也是我為什么推薦新手先學(xué)Floyd它的思路足夠簡單沒有優(yōu)先隊(duì)列、沒有啟發(fā)函數(shù)、沒有open/close列表你只需要理解一個(gè)遞推公式就能把整個(gè)算法寫出來。掌握了Floyd你對(duì)圖論里松弛這個(gè)概念會(huì)有肌肉記憶后面再學(xué)Dijkstra和A*你會(huì)發(fā)現(xiàn)那些復(fù)雜的數(shù)據(jù)結(jié)構(gòu)只是優(yōu)化手段底層邏輯萬變不離其宗。1.3 為什么Floyd適合課程設(shè)計(jì)和比賽原型我這些年看過的路徑規(guī)劃作業(yè)里很多同學(xué)一上來就用A*結(jié)果光在調(diào)試啟發(fā)函數(shù)和堆排上就花了兩三天。而用Floyd的同學(xué)當(dāng)天就能跑通剩下的時(shí)間全在打磨界面和匯報(bào)PPT。Floyd的優(yōu)勢非常明確實(shí)現(xiàn)門檻低不需要了解堆、優(yōu)先隊(duì)列、鏈表等數(shù)據(jù)結(jié)構(gòu)一個(gè)二維數(shù)組就能搞定。代碼量小核心函數(shù)通常不超過30行出錯(cuò)概率低調(diào)起來也快。結(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ù)很大的場景下不占優(yōu)勢。但如果是幾百個(gè)節(jié)點(diǎn)的路網(wǎng)比如一個(gè)園區(qū)的地面路網(wǎng)、一個(gè)廠房內(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ì)比直走更近。用公式寫出來就是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)過k來更新dist[i][j]。這個(gè)公式看起來平淡無奇它背后是一個(gè)標(biāo)準(zhǔn)的動(dòng)態(tài)規(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)越來越多dist[i][j]的距離就越來越短最終當(dāng)所有節(jié)點(diǎn)都被允許作為中間節(jié)點(diǎn)后得到的dist[i][j]也就是全局最優(yōu)的了。2.2 為什么k放在最外層是安全的我每次講Floyd都會(huì)有人問同一個(gè)問題為什么k循環(huán)要放在最外面如果k在里面寫for i for j for k結(jié)果會(huì)不同嗎答案是會(huì)而且可能出錯(cuò)。這正是Floyd動(dòng)態(tài)規(guī)劃性質(zhì)的體現(xiàn)——k必須是階段變量。我沒有記錯(cuò)的話很多初學(xué)的人會(huì)嘗試把循環(huán)順序改成i - j - k然后發(fā)現(xiàn)某些路徑更新不完整。原因很簡單當(dāng)k還沒被正式引入時(shí)dist[i][k]和dist[k][j]本身可能還不是最優(yōu)值拿它們?nèi)ジ耫ist[i][j]更新的結(jié)果就不是基于當(dāng)前階段的最優(yōu)子結(jié)構(gòu)可能錯(cuò)過更優(yōu)解。而把k放在最外層每一輪迭代開始時(shí)dist[i][k]和dist[k][j]都已經(jīng)是只允許經(jīng)過0到k-1節(jié)點(diǎn)的最優(yōu)點(diǎn)對(duì)距離了再經(jīng)過k來刷新dist[i][j]數(shù)學(xué)上可以通過歸納法證明是安全的。2.3 一個(gè)直覺例子轉(zhuǎn)機(jī)航班前面講的公式可能有點(diǎn)抽象我換一個(gè)生活化的場景來解釋。假設(shè)你想從杭州飛往拉薩但查了一圈沒有直飛航班。你要么選擇不飛要么選擇某個(gè)城市中轉(zhuǎn)。一開始你只允許在成都中轉(zhuǎn)發(fā)現(xiàn)杭州-成都-拉薩票價(jià)是2800而杭州直飛拉薩是3500于是你更新了最優(yōu)價(jià)為2800。后來機(jī)票平臺(tái)又開放了西安這個(gè)中轉(zhuǎn)點(diǎn)你發(fā)現(xiàn)杭州-西安-拉薩只要2500你又更新為2500。再后來平臺(tái)開放了重慶你又發(fā)現(xiàn)杭州-重慶-拉薩只要2300于是再更新一次。每一次開放一個(gè)新的中轉(zhuǎn)城市你就有機(jī)會(huì)刷新之前的價(jià)格。等所有城市都開放了剩下的價(jià)格就是全局最低價(jià)。Floyd算法做的就是這件事只不過它把所有城市、所有起終點(diǎn)組合都在一張表里同步進(jìn)行。你注意看這個(gè)過程的順序也很講究你不能在還沒開放西安的時(shí)候就幻想杭州-西安-拉薩的路徑里西安又轉(zhuǎn)到重慶再到拉薩。因?yàn)橹貞c還沒開放呢。所以k必須一層一層地從里往外展開——這就是為什么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é)合場景來看。假設(shè)n100個(gè)節(jié)點(diǎn)三重循環(huán)的內(nèi)層操作次數(shù)是100^3 100萬次這對(duì)任何現(xiàn)代計(jì)算機(jī)來說都是毫秒級(jí)完成的事。n300節(jié)點(diǎn)是2700萬次也只要幾十毫秒。所以幾百個(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之間沒有直接邊就填一個(gè)無窮大值用float(inf)表示如果i等于j距離當(dāng)然是0。比如一個(gè)簡單的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。其他組合沒有直接邊就是INF。注意這個(gè)圖是無向圖所以矩陣是對(duì)稱的。3.2 核心代碼三個(gè)for循環(huán)完成Floyd直接看代碼我建議你親手敲一遍不要復(fù)制粘貼因?yàn)樽约呵玫倪^程就是在建立肌肉記憶。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)過k的中轉(zhuǎn)方案就是無效的直接跳過省一層內(nèi)層循環(huán)。這個(gè)優(yōu)化在新手階段可能看不出性能差異但在節(jié)點(diǎn)多的時(shí)候至少能減少一些無意義的計(jì)算。另外強(qiáng)調(diào)一點(diǎn)我這里用的是float(inf)而不是一個(gè)很大的數(shù)比如999999。用真正的無窮大有幾個(gè)好處第一INF 任何數(shù) INF邏輯不會(huì)錯(cuò)第二不會(huì)出現(xiàn)溢出問題第三代碼語義清晰。3.3 關(guān)鍵問題怎么還原具體路徑而不只是一個(gè)距離數(shù)字很多教程講到Floyd就停在了距離矩陣這一步。但實(shí)際做路徑規(guī)劃我們不光要知道最短距離是多少還要知道具體怎么走。這就需要用到path矩陣。在Floyd的更新過程中只要發(fā)現(xiàn)經(jīng)過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): # 如果最短路徑直接連通沒有中間節(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í)見過好幾個(gè)同學(xué)在這里卡住輸出結(jié)果多一個(gè)重復(fù)節(jié)點(diǎn)路線看起來很奇怪。如果你也遇到類似問題優(yōu)先檢查拼接邏輯。3.4 跑一個(gè)5節(jié)點(diǎn)的小例子我們用一個(gè)5節(jié)點(diǎn)的路網(wǎng)來驗(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í)沒有直達(dá)邊但是0-1距離31-2距離22-3距離13-4距離4加起來正好是10等一下這里路徑[0,1,2,3,4]加起來是321410但輸出說最短距離是9這說明我的路徑回溯可能存在一個(gè)問題。讓我重新檢查一下。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è)例子再次說明了先想清楚再寫代碼的重要性。我建議你跑代碼前先手算出最短距離再去驗(yàn)證程序輸出這樣既能加深理解也能及時(shí)發(fā)現(xiàn)程序里的問題。4. 柵格地圖實(shí)戰(zhàn)把Floyd用起來做可視化路徑規(guī)劃4.1 從路網(wǎng)到柵格構(gòu)建路徑規(guī)劃中的地圖上一章的鄰接矩陣是抽象圖路徑規(guī)劃里更常見的地圖形式是柵格地圖。所謂柵格地圖就是一張棋盤一樣的二維網(wǎng)格每個(gè)格子要么是可通行的空地要么是障礙物。它廣泛用于掃地機(jī)器人、倉儲(chǔ)機(jī)器人、仿真平臺(tái)上。柵格地圖建圖的第一步把地圖上每一個(gè)可通行的格子當(dāng)作一個(gè)節(jié)點(diǎn)相鄰格子之間建立一條邊邊的權(quán)重就是兩個(gè)格子之間的距離上下左右相鄰?fù)ǔＫ?對(duì)角相鄰可以算1.414不過為了簡單新手階段最常見的做法是只允許上下左右四方向移動(dòng)權(quán)重統(tǒng)一為1。第二步如果兩個(gè)格子之間隔著障礙或者兩個(gè)格子本身有一個(gè)是障礙就不建邊對(duì)應(yīng)鄰接矩陣?yán)锏奈恢锰領(lǐng)NF。這么一說你就明白了建圖的過程本質(zhì)上就是把網(wǎng)格坐標(biāo)映射成一個(gè)鄰接矩陣。網(wǎng)格的格子數(shù)量就是鄰接矩陣的維度n。4.2 柵格轉(zhuǎn)鄰接矩陣的完整代碼我們用一個(gè)6x6的小柵格地圖來演示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(最短路徑長度, 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ì)是類似這樣的最短路徑長度 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ā)向下走到第二行避開左邊的障礙然后向右上方繞過障礙最后沿最右側(cè)道路向下到達(dá)終點(diǎn)。這個(gè)是6x6柵格地圖上的合理路徑。如果你想看更直觀的效果可以自己用matplotlib把grid畫出來用imshow顯示格子然后把你算出來的坐標(biāo)路徑用折線畫上去。這一步代碼不復(fù)雜我就不貼了建議你自己動(dòng)手試一試。看到小車一樣的路徑顯示在地圖上那種成就感會(huì)讓你的學(xué)習(xí)動(dòng)力翻倍。4.4 實(shí)操中的常見坑把距離和坐標(biāo)混為一談做柵格地圖Floyd的時(shí)候最容易踩的坑有兩個(gè)。第一個(gè)坑是忘了把障礙物排除在建圖之外。我見過很多同學(xué)直接把所有格子都當(dāng)作節(jié)點(diǎn)結(jié)果路徑穿墻而過輸出一個(gè)神仙路線。排查方法很簡單把最終路由的坐標(biāo)打印出來逐格檢查是否經(jīng)過了障礙物或者更保險(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í)前面提過就是float(inf)不要和整數(shù)混著做算術(shù)時(shí)溢出。在Python里INF 1依然是INF沒有問題。但如果你用的是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è)問題既然Floyd這么簡單那別的算法是不是多余了當(dāng)然不是。每一種算法都有自己的生態(tài)位。我把常見的路徑規(guī)劃算法做了個(gè)對(duì)比表幫你建立全局視野。算法問題類型時(shí)間復(fù)雜度適用地圖典型場景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ù)空間無人機(jī)三維路徑、機(jī)械臂運(yùn)動(dòng)規(guī)劃從這個(gè)表可以看出Floyd最大的優(yōu)勢是多源和預(yù)計(jì)算。如果你的應(yīng)用場景里需要反復(fù)查詢很多對(duì)節(jié)點(diǎn)之間的最短路徑而且路網(wǎng)規(guī)模不大Floyd反而是最快的——因?yàn)槠渌麊卧此惴看尾樵兌家獜念^跑一遍。5.2 結(jié)合熱詞場景動(dòng)態(tài)避障小車與無人機(jī)路徑規(guī)劃我看到最近有同學(xué)在做動(dòng)態(tài)避障小車路徑規(guī)劃還有人在研究無人機(jī)路徑規(guī)劃算法所以就多聊幾句Floyd在這些場景里的位置。先說動(dòng)態(tài)避障小車。如果你的小車在一個(gè)倉庫環(huán)境里跑布局相對(duì)固定但會(huì)有臨時(shí)出現(xiàn)的障礙物需要繞開這種情況下你的全局路網(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)修正的思路在比賽里非常高效。不過如果你的小車是在一個(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)定、但需要大量查詢的場景不是一個(gè)每次都要重新探索世界的方案。再看無人機(jī)路徑規(guī)劃。無人機(jī)在三維空間里飛行狀態(tài)空間往往是連續(xù)的柵格化之后節(jié)點(diǎn)數(shù)會(huì)爆炸。Floyd的O(n^3)完全吃不消而且無人機(jī)路徑往往需要考慮動(dòng)力學(xué)約束、轉(zhuǎn)彎半徑、高度變化。實(shí)際工程用的更多是RRT、RRT*這樣的采樣算法。如果你是做無人機(jī)比賽Floyd更適合做路徑規(guī)劃上層的一個(gè)航路點(diǎn)網(wǎng)絡(luò)快速預(yù)計(jì)算工具而不是最終的飛行軌跡求解器。5.3 我給新手的選型建議如果你現(xiàn)在要做一個(gè)路徑規(guī)劃的項(xiàng)目我建議你用一張簡單的決策圖來選算法別急不是讓你畫流程圖是心里過一遍這個(gè)判斷邏輯第一個(gè)問題需要算多少對(duì)節(jié)點(diǎn)之間的最短路徑只算一對(duì)優(yōu)先A*或Dijkstra。要算所有點(diǎn)對(duì)而且節(jié)點(diǎn)數(shù)在500以內(nèi)優(yōu)先Floyd。第二個(gè)問題地圖會(huì)頻繁變化嗎不會(huì)頻繁變化Floyd和Dijkstra都行。頻繁變化優(yōu)先A或D系列不要用Floyd做全量重算。第三個(gè)問題地圖是高維連續(xù)空間嗎是考慮RRT/RRT*。是柵格或拓?fù)渎肪W(wǎng)才能談Floyd/Dijkstra/A*。按照這個(gè)邏輯很多同學(xué)的路徑規(guī)劃課程設(shè)計(jì)其實(shí)用Floyd就足夠了而且因?yàn)楹脤?shí)現(xiàn)、好展示反而比硬上A拿分更容易。等你真的做出來了再按需去擴(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]取出來省掉多次二維數(shù)組索引的耗時(shí)。這個(gè)優(yōu)化在Python里效果有限但能讓你體會(huì)到大慶點(diǎn)小事的樂趣。祝你在路徑規(guī)劃的路上越走越順。