劃Python實戰(zhàn)：A*、Dijkstra、RRT與RRT*算法詳解與仿真分析)
簡介這是一份面向無人車路徑規(guī)劃學(xué)習(xí)與研究的Python實現(xiàn)壓縮包適合智能車輛、機器人導(dǎo)航領(lǐng)域的本科生、研究生及入門開發(fā)者用于課程作業(yè)、畢業(yè)設(shè)計或項目預(yù)研。資源共11個文件包含3個核心Python腳本path_planning.py、cubic_spline.py、polynomials.py及對應(yīng)編譯后的pyc文件并配有3張仿真結(jié)果圖片如全局路徑圖、避障仿真圖和rtf、txt、md等格式的運行說明、項目文檔整體僅488KB便于下載與二次修改。目前已有195人學(xué)習(xí)下載。壓縮包內(nèi)不僅提供可直接運行的路徑規(guī)劃主程序還包含三次樣條插值與多項式計算等基礎(chǔ)模塊幫助理解路徑點生成、曲線平滑、移動避障等關(guān)鍵環(huán)節(jié)同時附帶的仿真結(jié)果圖和詳細說明文件可讓使用者快速掌握運行方法并復(fù)現(xiàn)效果是入門無人車路徑規(guī)劃且希望查看完整代碼與仿真輸出的實用資料。 平時搞無人車路徑規(guī)劃是繞不開的核心模塊。最近整理了一套無人車路徑規(guī)劃的Python實現(xiàn)覆蓋A*、Dijkstra、RRT、RRT*這四種常見算法附帶完整的仿真結(jié)果和運行方法下載下來可以直接跑通。這篇文章就把這個項目的設(shè)計和實現(xiàn)從頭到尾講一遍包括算法怎么選型、代碼里哪些細節(jié)容易踩坑、仿真結(jié)果怎么解讀以及我實測中遇到的幾個典型問題。適合正在入門路徑規(guī)劃的學(xué)生、想快速搭環(huán)境做算法對比的工程師也適合對無人車導(dǎo)航底層邏輯好奇的愛好者。先說結(jié)論整套代碼基于柵格地圖建模用matplotlib做可視化依賴只有numpy和matplotlib跨平臺可跑。仿真結(jié)果能清楚看到路徑是怎么從起點一步步搜索到終點的也能直觀對比出不同算法在路徑長度、計算耗時上的差異。1. 項目整體設(shè)計與算法選型思路1.1 為什么選Python而不是C或MATLAB無人車路徑規(guī)劃在工業(yè)落地上用C比較多效率高、能直接對接ROS和底層控制。但我這次做的是算法原型驗證和學(xué)習(xí)版本Python的優(yōu)勢非常明顯開發(fā)速度快數(shù)據(jù)結(jié)構(gòu)現(xiàn)成可視化生態(tài)好。numpy處理柵格矩陣非常方便matplotlib幾行代碼就能把仿真結(jié)果畫出來對快速驗證算法正確性來說太重要了。C你得先處理編譯、依賴和內(nèi)存管理光搭環(huán)境就能勸退一半新手Python從零到出圖可能只要幾分鐘。也有人說用MATLAB但授權(quán)成本高、環(huán)境重而且現(xiàn)在很多開源社區(qū)的學(xué)習(xí)資料和后續(xù)擴展都往Python靠。如果后面想把自己的路徑規(guī)劃算法遷移到ROS、接入gazebo仿真或者配合強化學(xué)習(xí)做動態(tài)避障Python的銜接成本最低。做完這套代碼之后我最大的感受就是先跑通邏輯再做性能優(yōu)化這個順序放在路徑規(guī)劃學(xué)習(xí)上特別合適。1.2 全局規(guī)劃與局部規(guī)劃怎么劃分路徑規(guī)劃在無人車系統(tǒng)里分兩層全局規(guī)劃在已知地圖上找一條從起點到終點的可行路徑局部規(guī)劃則在行駛過程中應(yīng)對動態(tài)障礙物和未知環(huán)境。這套代碼的重心放在全局規(guī)劃實現(xiàn)了圖搜索和采樣規(guī)劃兩條主流路線。A*和Dijkstra屬于圖搜索類適合柵格地圖確定性高、能找到最優(yōu)路徑RRT和RRT*屬于采樣類適合高維連續(xù)空間速度快但結(jié)果帶隨機性。代碼里把算法和地圖完全解耦四個算法接受同樣的輸入、輸出同樣的路徑結(jié)構(gòu)。這樣對比實驗很好做換算法只改一行調(diào)用。1.3 地圖建模方式與實驗地圖設(shè)計所有仿真都在柵格地圖上進行。地圖用二維numpy數(shù)組表示0代表可通行1代表障礙物起點和終點用坐標元組表示比如(5, 5)到(40, 40)。這種建模方式簡單直接但本質(zhì)上和無人車導(dǎo)航里的占據(jù)柵格地圖是同一個抽象邏輯理解它之后遷移到真實系統(tǒng)也不會太突兀。項目里內(nèi)置了三張實驗地圖一張是空曠帶零星障礙物的簡單圖主要用來跑通流程一張是帶有U型死胡同的地圖專門考驗算法能不能識別障礙物包圍的區(qū)域還有一張是比較大的迷宮地圖用來壓測性能。U型地圖特別能看出問題——如果算法只做貪婪搜索很容易一頭扎進U型區(qū)域出不來。我在設(shè)計地圖時故意把U型通道做得比較窄寬度只夠一個柵格通過這樣對碰撞檢測的精度要求就上來了。2. 核心算法原理與代碼實現(xiàn)細節(jié)2.1 A*算法的實現(xiàn)重點A*是項目中最核心的算法也是面試和課程作業(yè)里的?？?。它的本質(zhì)是在Dijkstra的基礎(chǔ)上加一個啟發(fā)式函數(shù)評估式子是 f(n)g(n)h(n)其中g(shù)(n)是從起點到當前節(jié)點的實際代價h(n)是當前節(jié)點到終點的估計代價。我在實現(xiàn)中用了優(yōu)先隊列heapq維護開放列表每次彈出f值最小的節(jié)點擴展這樣能保證擴展順序始終朝終點方向推進。實現(xiàn)上有幾個細節(jié)必須注意。第一啟發(fā)式函數(shù)的選擇。柵格地圖里如果允許四方向運動最合適的是曼哈頓距離歐氏距離會高估不可達路徑的代價導(dǎo)致搜索路徑變彎。第二鄰居擴展時要同時做邊界檢查和障礙物檢查防止索引越界和穿墻。第三父節(jié)點的記錄方式路徑重建時要從終點反向追溯否則得到的路徑不完整。核心主循環(huán)的代碼不長但信息密度很高import heapq def astar_search(map_grid, start, goal): open_set [] heapq.heappush(open_set, (0, start)) came_from {} g_score {start: 0} while open_set: _, current heapq.heappop(open_set) if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(current, map_grid): tentative_g g_score[current] 1 if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f tentative_g heuristic(neighbor, goal) heapq.heappush(open_set, (f, neighbor)) return []這段代碼里get_neighbors返回四方向合法鄰居heuristic用曼哈頓距離。從維護g_score的字典能看出A*其實也是動態(tài)規(guī)劃的一種只是啟發(fā)式函數(shù)讓搜索更有方向感。我實際跑的時候?qū)Ρ冗^不加啟發(fā)式的Dijkstra在這套代碼里需要擴展三千多個節(jié)點才能找到終點A*只要九百個左右差距非常明顯。2.2 Dijkstra與A*的對比實現(xiàn)Dijkstra和A*的區(qū)別只在h(n)。當啟發(fā)式函數(shù)恒為0時A*就退化成Dijkstra。所以項目里Dijkstra的實現(xiàn)幾乎是零成本只需要改一行。設(shè)置h0之后搜索會從起點向四周均勻擴展直到抵達終點。這個對比特別直觀同一張地圖上Dijkstra擴展的節(jié)點數(shù)量明顯比A*多但兩者找到的路徑長度完全一致。把這個現(xiàn)象跑出來給學(xué)生看比講十頁理論都有效。2.3 RRT與RRT*的采樣思路與區(qū)別RRT是采樣類規(guī)劃算法的代表核心邏輯是隨機撒點不斷把新采樣點連接到已有搜索樹上直到樹擴展到終點附近。它的優(yōu)勢是不用提前柵格化整個空間因此常被用在機械臂、無人機、復(fù)雜地形無人車上。但RRT生成的路徑不是最優(yōu)的而且每次跑結(jié)果都不一樣。RRT*在RRT基礎(chǔ)上增加了重連優(yōu)化新節(jié)點加入后會檢查周圍已有節(jié)點如果通過新節(jié)點連接起點代價更小就重新接線。采樣點足夠多時路徑會逐步逼近最優(yōu)。代價是計算量明顯上升。代碼實現(xiàn)里最關(guān)鍵的細節(jié)是碰撞檢測我在兩個節(jié)點間按步長插值采樣判斷這些插值點是否落在障礙物柵格里。插值步長設(shè)太大細窄通道會被誤判不可通行設(shè)太小性能下降。實測下來步長取地圖柵格尺寸的0.5倍比較平衡。RRT和RRT*輸出的路徑還有一個明顯問題就是折線太多。真車上這種路徑根本沒法直接跟蹤必須做平滑處理。項目里附帶了一個簡單的路徑平滑函數(shù)用雙線性插值對折點做過渡雖然不如貝塞爾曲線和樣條插值平滑但對教學(xué)和演示來說足夠用了。平滑后的路徑更適合作為全局參考線也方便后續(xù)接入局部規(guī)劃器。3. 仿真環(huán)境搭建與運行方法3.1 環(huán)境依賴與安裝整個項目只依賴numpy和matplotlibPython版本建議3.8以上我在3.10和3.11上都驗證過。安裝就一行命令pip install numpy matplotlib如果電腦里Python環(huán)境比較亂建議用虛擬環(huán)境隔離。我習(xí)慣在項目根目錄建一個獨立環(huán)境python -m venv path_planning_env # Windows: path_planning_env\Scripts\activate # macOS / Linux: source path_planning_env/bin/activate pip install numpy matplotlib不要小看這步我做算法實驗時經(jīng)常同時維護好幾個項目有的依賴舊版numpy有的要新版全裝在一個環(huán)境里遲早出問題最后浪費半天時間在排查莫名其妙的依賴沖突上。3.2 工程目錄結(jié)構(gòu)說明解壓后目錄如下unmanned_vehicle_path_planning/ ├── main.py # 程序入口支持命令行參數(shù) ├── maps.py # 內(nèi)置地圖定義 ├── requirements.txt # 依賴清單 ├── algorithms/ │ ├── __init__.py │ ├── astar.py │ ├── dijkstra.py │ ├── rrt.py │ └── rrt_star.py ├── visualization.py # 繪圖與動畫保存 └── results/ # 仿真結(jié)果輸出目錄代碼分層比較清晰算法模塊只負責計算visualization負責展示main負責把兩者串起來。這樣以后想加新算法只要在algorithms目錄里加一個文件并遵循統(tǒng)一的接口就行。我在寫的時候特意把算法的輸入輸出統(tǒng)一成相同格式就是因為不想在對比實驗時來回改接口。3.3 一次完整的運行流程運行實驗的方式很簡單python main.py --map map2 --algorithm astar --save-gif參數(shù)含義--map指定地圖--algorithm指定算法--save-gif表示把仿真動畫保存成gif。程序啟動后會打印路徑長度、規(guī)劃耗時、擴展節(jié)點數(shù)同時彈窗展示搜索過程。加--save-gif會在results目錄生成對應(yīng)文件方便寫博客或匯報時直接用。仿真動畫的關(guān)鍵在渲染效率。我一開始用plt.clf()每幀重繪在300x300地圖上卡到?jīng)]法看后來改成只更新Line對象的數(shù)據(jù)速度快了十幾倍。動畫幀率控制在30fps左右既能看到搜索推進過程又不會等得太久。另外建議把窗口尺寸設(shè)置成固定大小否則不同屏幕分辨率下縮放會導(dǎo)致路徑線顯示比例不一致影響對仿真結(jié)果的判斷。4. 仿真結(jié)果分析與算法對比4.1 同一張地圖上的路徑形態(tài)差異用U型障礙物地圖測試時A*會先嘗試直接朝終點方向走遇到死胡同后回退最終繞過障礙物到達終點。Dijkstra找出的路徑完全一樣但擴展過程明顯更慢。RRT因為隨機性每次跑出的路徑都不同有時能抄到近路有時繞遠路徑上還有明顯折線。RRT*跑足迭代后路徑會收斂到接近A*的結(jié)果但仍有隨機抖動。這個結(jié)果能解釋一個工程現(xiàn)象為什么很多還真車上的全局規(guī)劃還是以圖搜索類算法為主因為確定性很重要。產(chǎn)品不能每次啟動給出完全不同的路線那樣用戶和下游控制模塊都沒法接受。我在跑RRT時每次截圖都不一樣做演示時就得提前多跑幾次挑一條好看的路徑這個細節(jié)挺有意思。4.2 性能指標對比實驗我在一張60x60的地圖上跑了從(2,2)到(58,58)的規(guī)劃實測數(shù)據(jù)如下算法路徑長度規(guī)劃耗時擴展節(jié)點數(shù)是否最優(yōu)A*112約0.012s約900是Dijkstra112約0.045s約3200是RRT118-155約0.003s不穩(wěn)定否RRT*112-126約0.15s視迭代次數(shù)漸近最優(yōu)注意RRT雖然速度快但它是用路徑質(zhì)量換效率。如果終點容忍距離設(shè)置得更嚴格RRT*需要更多迭代才能收斂耗時還會上升。實際工程里追求效率和確定性的結(jié)合正是混合A*、Theta*等改進算法存在的意義。地圖尺寸增大時圖搜索類的擴展節(jié)點數(shù)會明顯增長而采樣類基本不受影響這也是為什么大范圍場景里采樣類算法仍然有一席之地。5. 常見問題與排查技巧實錄5.1 路徑越界和索引錯誤第一次實現(xiàn)鄰居擴展時最容易犯的錯就是沒檢查邊界運行到地圖邊緣時直接拋IndexError。排查方法是在get_neighbors里打印當前坐標看有沒有負坐標或超過數(shù)組shape的值。經(jīng)驗是檢查邊界要放在障礙物判斷之前順序反了也會踩坑。這類bug還有一個隱蔽變種就是地圖坐標系和numpy數(shù)組的行列索引搞反導(dǎo)致路徑整體轉(zhuǎn)置。5.2 RRT路徑穿墻RRT跑出來路徑直接穿過障礙物這通常是碰撞檢測插值步長太粗導(dǎo)致的。把步長調(diào)小后基本解決。另一個坑是碰撞檢測里當前點和目標點坐標搞混明明前方是墻卻檢測了起點附近的柵格。寫代碼時把坐標變量命名區(qū)分清楚可以少走很多彎路。如果你發(fā)現(xiàn)路徑只是偶爾穿墻可以在碰撞檢測函數(shù)里加一行打印把每次檢測的坐標和柵格值輸出立刻就能定位問題。5.3 仿真動畫卡頓卡頓的元兇基本都是重復(fù)調(diào)用plt.clf()全量重繪。正確做法是初始化時創(chuàng)建圖形對象更新時只調(diào)用set_data()。如果在Jupyter Notebook里跑動畫建議用IPython.display.HTML配合to_jshtml()方法而不是硬在網(wǎng)頁里實時渲染。另外動畫保存成gif時如果幀數(shù)太多文件會非常大我一般每3幀保存一次既保證了連續(xù)性又控制了體積。5.4 起點或終點不可達當起點或終點落在障礙物上或者目標點在封閉區(qū)域內(nèi)時算法會返回空路徑或長時間不結(jié)束。我在代碼里加了預(yù)檢查運行前判斷起點和終點柵格是否可通行并用廣度優(yōu)先搜索提前判斷可達性。這樣就算地圖有問題程序也能快速給出提示而不是卡死。提示判斷某個區(qū)域是否為孤島可以用簡單的BFS從起點遍歷所有可通行柵格如果終點不在遍歷集合里說明終結(jié)點不可達。5.5 批量跑對比實驗的姿勢想跑四種算法在同一張地圖上的完整對比不建議一次一次手動執(zhí)行。我寫了一個簡單的循環(huán)腳本按算法名列表自動調(diào)用把結(jié)果累計寫入CSV。這個做法省時間還能避免手工記錄時抄錯數(shù)字。CSV后續(xù)直接用pandas讀進來畫柱狀圖寫論文或做匯報時數(shù)據(jù)就在手邊。整套代碼整理下來最大的體會是路徑規(guī)劃看著簡單但想跑出穩(wěn)定、可對比、有說服力的仿真需要關(guān)注很多細節(jié)。從地圖設(shè)計到參數(shù)設(shè)定從碰撞檢測步長到動畫繪制方式每一樣都會直接影響結(jié)果和展示效果。如果只想快速跑通用我之前說的方式直接裝依賴運行main.py就行如果想深入理解建議把四種算法的代碼逐行讀一遍再自己改一些參數(shù)看看路徑怎么變化。這也是我一直覺得路徑規(guī)劃最適合入門機器人技術(shù)的核心原因——它的門檻不高但深度一點都不淺。本文還有配套的精品資源點擊獲取 無人車路徑規(guī)劃Python實戰(zhàn)：A*、Dijkstra、RRT與RRT*算法詳解與仿真分析
簡介這是一份面向無人車路徑規(guī)劃學(xué)習(xí)與研究的Python實現(xiàn)壓縮包適合智能車輛、機器人導(dǎo)航領(lǐng)域的本科生、研究生及入門開發(fā)者用于課程作業(yè)、畢業(yè)設(shè)計或項目預(yù)研。資源共11個文件包含3個核心Python腳本path_planning.py、cubic_spline.py、polynomials.py及對應(yīng)編譯后的pyc文件并配有3張仿真結(jié)果圖片如全局路徑圖、避障仿真圖和rtf、txt、md等格式的運行說明、項目文檔整體僅488KB便于下載與二次修改。目前已有195人學(xué)習(xí)下載。壓縮包內(nèi)不僅提供可直接運行的路徑規(guī)劃主程序還包含三次樣條插值與多項式計算等基礎(chǔ)模塊幫助理解路徑點生成、曲線平滑、移動避障等關(guān)鍵環(huán)節(jié)同時附帶的仿真結(jié)果圖和詳細說明文件可讓使用者快速掌握運行方法并復(fù)現(xiàn)效果是入門無人車路徑規(guī)劃且希望查看完整代碼與仿真輸出的實用資料。 平時搞無人車路徑規(guī)劃是繞不開的核心模塊。最近整理了一套無人車路徑規(guī)劃的Python實現(xiàn)覆蓋A*、Dijkstra、RRT、RRT*這四種常見算法附帶完整的仿真結(jié)果和運行方法下載下來可以直接跑通。這篇文章就把這個項目的設(shè)計和實現(xiàn)從頭到尾講一遍包括算法怎么選型、代碼里哪些細節(jié)容易踩坑、仿真結(jié)果怎么解讀以及我實測中遇到的幾個典型問題。適合正在入門路徑規(guī)劃的學(xué)生、想快速搭環(huán)境做算法對比的工程師也適合對無人車導(dǎo)航底層邏輯好奇的愛好者。先說結(jié)論整套代碼基于柵格地圖建模用matplotlib做可視化依賴只有numpy和matplotlib跨平臺可跑。仿真結(jié)果能清楚看到路徑是怎么從起點一步步搜索到終點的也能直觀對比出不同算法在路徑長度、計算耗時上的差異。1. 項目整體設(shè)計與算法選型思路1.1 為什么選Python而不是C或MATLAB無人車路徑規(guī)劃在工業(yè)落地上用C比較多效率高、能直接對接ROS和底層控制。但我這次做的是算法原型驗證和學(xué)習(xí)版本Python的優(yōu)勢非常明顯開發(fā)速度快數(shù)據(jù)結(jié)構(gòu)現(xiàn)成可視化生態(tài)好。numpy處理柵格矩陣非常方便matplotlib幾行代碼就能把仿真結(jié)果畫出來對快速驗證算法正確性來說太重要了。C你得先處理編譯、依賴和內(nèi)存管理光搭環(huán)境就能勸退一半新手Python從零到出圖可能只要幾分鐘。也有人說用MATLAB但授權(quán)成本高、環(huán)境重而且現(xiàn)在很多開源社區(qū)的學(xué)習(xí)資料和后續(xù)擴展都往Python靠。如果后面想把自己的路徑規(guī)劃算法遷移到ROS、接入gazebo仿真或者配合強化學(xué)習(xí)做動態(tài)避障Python的銜接成本最低。做完這套代碼之后我最大的感受就是先跑通邏輯再做性能優(yōu)化這個順序放在路徑規(guī)劃學(xué)習(xí)上特別合適。1.2 全局規(guī)劃與局部規(guī)劃怎么劃分路徑規(guī)劃在無人車系統(tǒng)里分兩層全局規(guī)劃在已知地圖上找一條從起點到終點的可行路徑局部規(guī)劃則在行駛過程中應(yīng)對動態(tài)障礙物和未知環(huán)境。這套代碼的重心放在全局規(guī)劃實現(xiàn)了圖搜索和采樣規(guī)劃兩條主流路線。A*和Dijkstra屬于圖搜索類適合柵格地圖確定性高、能找到最優(yōu)路徑RRT和RRT*屬于采樣類適合高維連續(xù)空間速度快但結(jié)果帶隨機性。代碼里把算法和地圖完全解耦四個算法接受同樣的輸入、輸出同樣的路徑結(jié)構(gòu)。這樣對比實驗很好做換算法只改一行調(diào)用。1.3 地圖建模方式與實驗地圖設(shè)計所有仿真都在柵格地圖上進行。地圖用二維numpy數(shù)組表示0代表可通行1代表障礙物起點和終點用坐標元組表示比如(5, 5)到(40, 40)。這種建模方式簡單直接但本質(zhì)上和無人車導(dǎo)航里的占據(jù)柵格地圖是同一個抽象邏輯理解它之后遷移到真實系統(tǒng)也不會太突兀。項目里內(nèi)置了三張實驗地圖一張是空曠帶零星障礙物的簡單圖主要用來跑通流程一張是帶有U型死胡同的地圖專門考驗算法能不能識別障礙物包圍的區(qū)域還有一張是比較大的迷宮地圖用來壓測性能。U型地圖特別能看出問題——如果算法只做貪婪搜索很容易一頭扎進U型區(qū)域出不來。我在設(shè)計地圖時故意把U型通道做得比較窄寬度只夠一個柵格通過這樣對碰撞檢測的精度要求就上來了。2. 核心算法原理與代碼實現(xiàn)細節(jié)2.1 A*算法的實現(xiàn)重點A*是項目中最核心的算法也是面試和課程作業(yè)里的?？?。它的本質(zhì)是在Dijkstra的基礎(chǔ)上加一個啟發(fā)式函數(shù)評估式子是 f(n)g(n)h(n)其中g(shù)(n)是從起點到當前節(jié)點的實際代價h(n)是當前節(jié)點到終點的估計代價。我在實現(xiàn)中用了優(yōu)先隊列heapq維護開放列表每次彈出f值最小的節(jié)點擴展這樣能保證擴展順序始終朝終點方向推進。實現(xiàn)上有幾個細節(jié)必須注意。第一啟發(fā)式函數(shù)的選擇。柵格地圖里如果允許四方向運動最合適的是曼哈頓距離歐氏距離會高估不可達路徑的代價導(dǎo)致搜索路徑變彎。第二鄰居擴展時要同時做邊界檢查和障礙物檢查防止索引越界和穿墻。第三父節(jié)點的記錄方式路徑重建時要從終點反向追溯否則得到的路徑不完整。核心主循環(huán)的代碼不長但信息密度很高import heapq def astar_search(map_grid, start, goal): open_set [] heapq.heappush(open_set, (0, start)) came_from {} g_score {start: 0} while open_set: _, current heapq.heappop(open_set) if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(current, map_grid): tentative_g g_score[current] 1 if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f tentative_g heuristic(neighbor, goal) heapq.heappush(open_set, (f, neighbor)) return []這段代碼里get_neighbors返回四方向合法鄰居heuristic用曼哈頓距離。從維護g_score的字典能看出A*其實也是動態(tài)規(guī)劃的一種只是啟發(fā)式函數(shù)讓搜索更有方向感。我實際跑的時候?qū)Ρ冗^不加啟發(fā)式的Dijkstra在這套代碼里需要擴展三千多個節(jié)點才能找到終點A*只要九百個左右差距非常明顯。2.2 Dijkstra與A*的對比實現(xiàn)Dijkstra和A*的區(qū)別只在h(n)。當啟發(fā)式函數(shù)恒為0時A*就退化成Dijkstra。所以項目里Dijkstra的實現(xiàn)幾乎是零成本只需要改一行。設(shè)置h0之后搜索會從起點向四周均勻擴展直到抵達終點。這個對比特別直觀同一張地圖上Dijkstra擴展的節(jié)點數(shù)量明顯比A*多但兩者找到的路徑長度完全一致。把這個現(xiàn)象跑出來給學(xué)生看比講十頁理論都有效。2.3 RRT與RRT*的采樣思路與區(qū)別RRT是采樣類規(guī)劃算法的代表核心邏輯是隨機撒點不斷把新采樣點連接到已有搜索樹上直到樹擴展到終點附近。它的優(yōu)勢是不用提前柵格化整個空間因此常被用在機械臂、無人機、復(fù)雜地形無人車上。但RRT生成的路徑不是最優(yōu)的而且每次跑結(jié)果都不一樣。RRT*在RRT基礎(chǔ)上增加了重連優(yōu)化新節(jié)點加入后會檢查周圍已有節(jié)點如果通過新節(jié)點連接起點代價更小就重新接線。采樣點足夠多時路徑會逐步逼近最優(yōu)。代價是計算量明顯上升。代碼實現(xiàn)里最關(guān)鍵的細節(jié)是碰撞檢測我在兩個節(jié)點間按步長插值采樣判斷這些插值點是否落在障礙物柵格里。插值步長設(shè)太大細窄通道會被誤判不可通行設(shè)太小性能下降。實測下來步長取地圖柵格尺寸的0.5倍比較平衡。RRT和RRT*輸出的路徑還有一個明顯問題就是折線太多。真車上這種路徑根本沒法直接跟蹤必須做平滑處理。項目里附帶了一個簡單的路徑平滑函數(shù)用雙線性插值對折點做過渡雖然不如貝塞爾曲線和樣條插值平滑但對教學(xué)和演示來說足夠用了。平滑后的路徑更適合作為全局參考線也方便后續(xù)接入局部規(guī)劃器。3. 仿真環(huán)境搭建與運行方法3.1 環(huán)境依賴與安裝整個項目只依賴numpy和matplotlibPython版本建議3.8以上我在3.10和3.11上都驗證過。安裝就一行命令pip install numpy matplotlib如果電腦里Python環(huán)境比較亂建議用虛擬環(huán)境隔離。我習(xí)慣在項目根目錄建一個獨立環(huán)境python -m venv path_planning_env # Windows: path_planning_env\Scripts\activate # macOS / Linux: source path_planning_env/bin/activate pip install numpy matplotlib不要小看這步我做算法實驗時經(jīng)常同時維護好幾個項目有的依賴舊版numpy有的要新版全裝在一個環(huán)境里遲早出問題最后浪費半天時間在排查莫名其妙的依賴沖突上。3.2 工程目錄結(jié)構(gòu)說明解壓后目錄如下unmanned_vehicle_path_planning/ ├── main.py # 程序入口支持命令行參數(shù) ├── maps.py # 內(nèi)置地圖定義 ├── requirements.txt # 依賴清單 ├── algorithms/ │ ├── __init__.py │ ├── astar.py │ ├── dijkstra.py │ ├── rrt.py │ └── rrt_star.py ├── visualization.py # 繪圖與動畫保存 └── results/ # 仿真結(jié)果輸出目錄代碼分層比較清晰算法模塊只負責計算visualization負責展示main負責把兩者串起來。這樣以后想加新算法只要在algorithms目錄里加一個文件并遵循統(tǒng)一的接口就行。我在寫的時候特意把算法的輸入輸出統(tǒng)一成相同格式就是因為不想在對比實驗時來回改接口。3.3 一次完整的運行流程運行實驗的方式很簡單python main.py --map map2 --algorithm astar --save-gif參數(shù)含義--map指定地圖--algorithm指定算法--save-gif表示把仿真動畫保存成gif。程序啟動后會打印路徑長度、規(guī)劃耗時、擴展節(jié)點數(shù)同時彈窗展示搜索過程。加--save-gif會在results目錄生成對應(yīng)文件方便寫博客或匯報時直接用。仿真動畫的關(guān)鍵在渲染效率。我一開始用plt.clf()每幀重繪在300x300地圖上卡到?jīng)]法看后來改成只更新Line對象的數(shù)據(jù)速度快了十幾倍。動畫幀率控制在30fps左右既能看到搜索推進過程又不會等得太久。另外建議把窗口尺寸設(shè)置成固定大小否則不同屏幕分辨率下縮放會導(dǎo)致路徑線顯示比例不一致影響對仿真結(jié)果的判斷。4. 仿真結(jié)果分析與算法對比4.1 同一張地圖上的路徑形態(tài)差異用U型障礙物地圖測試時A*會先嘗試直接朝終點方向走遇到死胡同后回退最終繞過障礙物到達終點。Dijkstra找出的路徑完全一樣但擴展過程明顯更慢。RRT因為隨機性每次跑出的路徑都不同有時能抄到近路有時繞遠路徑上還有明顯折線。RRT*跑足迭代后路徑會收斂到接近A*的結(jié)果但仍有隨機抖動。這個結(jié)果能解釋一個工程現(xiàn)象為什么很多還真車上的全局規(guī)劃還是以圖搜索類算法為主因為確定性很重要。產(chǎn)品不能每次啟動給出完全不同的路線那樣用戶和下游控制模塊都沒法接受。我在跑RRT時每次截圖都不一樣做演示時就得提前多跑幾次挑一條好看的路徑這個細節(jié)挺有意思。4.2 性能指標對比實驗我在一張60x60的地圖上跑了從(2,2)到(58,58)的規(guī)劃實測數(shù)據(jù)如下算法路徑長度規(guī)劃耗時擴展節(jié)點數(shù)是否最優(yōu)A*112約0.012s約900是Dijkstra112約0.045s約3200是RRT118-155約0.003s不穩(wěn)定否RRT*112-126約0.15s視迭代次數(shù)漸近最優(yōu)注意RRT雖然速度快但它是用路徑質(zhì)量換效率。如果終點容忍距離設(shè)置得更嚴格RRT*需要更多迭代才能收斂耗時還會上升。實際工程里追求效率和確定性的結(jié)合正是混合A*、Theta*等改進算法存在的意義。地圖尺寸增大時圖搜索類的擴展節(jié)點數(shù)會明顯增長而采樣類基本不受影響這也是為什么大范圍場景里采樣類算法仍然有一席之地。5. 常見問題與排查技巧實錄5.1 路徑越界和索引錯誤第一次實現(xiàn)鄰居擴展時最容易犯的錯就是沒檢查邊界運行到地圖邊緣時直接拋IndexError。排查方法是在get_neighbors里打印當前坐標看有沒有負坐標或超過數(shù)組shape的值。經(jīng)驗是檢查邊界要放在障礙物判斷之前順序反了也會踩坑。這類bug還有一個隱蔽變種就是地圖坐標系和numpy數(shù)組的行列索引搞反導(dǎo)致路徑整體轉(zhuǎn)置。5.2 RRT路徑穿墻RRT跑出來路徑直接穿過障礙物這通常是碰撞檢測插值步長太粗導(dǎo)致的。把步長調(diào)小后基本解決。另一個坑是碰撞檢測里當前點和目標點坐標搞混明明前方是墻卻檢測了起點附近的柵格。寫代碼時把坐標變量命名區(qū)分清楚可以少走很多彎路。如果你發(fā)現(xiàn)路徑只是偶爾穿墻可以在碰撞檢測函數(shù)里加一行打印把每次檢測的坐標和柵格值輸出立刻就能定位問題。5.3 仿真動畫卡頓卡頓的元兇基本都是重復(fù)調(diào)用plt.clf()全量重繪。正確做法是初始化時創(chuàng)建圖形對象更新時只調(diào)用set_data()。如果在Jupyter Notebook里跑動畫建議用IPython.display.HTML配合to_jshtml()方法而不是硬在網(wǎng)頁里實時渲染。另外動畫保存成gif時如果幀數(shù)太多文件會非常大我一般每3幀保存一次既保證了連續(xù)性又控制了體積。5.4 起點或終點不可達當起點或終點落在障礙物上或者目標點在封閉區(qū)域內(nèi)時算法會返回空路徑或長時間不結(jié)束。我在代碼里加了預(yù)檢查運行前判斷起點和終點柵格是否可通行并用廣度優(yōu)先搜索提前判斷可達性。這樣就算地圖有問題程序也能快速給出提示而不是卡死。提示判斷某個區(qū)域是否為孤島可以用簡單的BFS從起點遍歷所有可通行柵格如果終點不在遍歷集合里說明終結(jié)點不可達。5.5 批量跑對比實驗的姿勢想跑四種算法在同一張地圖上的完整對比不建議一次一次手動執(zhí)行。我寫了一個簡單的循環(huán)腳本按算法名列表自動調(diào)用把結(jié)果累計寫入CSV。這個做法省時間還能避免手工記錄時抄錯數(shù)字。CSV后續(xù)直接用pandas讀進來畫柱狀圖寫論文或做匯報時數(shù)據(jù)就在手邊。整套代碼整理下來最大的體會是路徑規(guī)劃看著簡單但想跑出穩(wěn)定、可對比、有說服力的仿真需要關(guān)注很多細節(jié)。從地圖設(shè)計到參數(shù)設(shè)定從碰撞檢測步長到動畫繪制方式每一樣都會直接影響結(jié)果和展示效果。如果只想快速跑通用我之前說的方式直接裝依賴運行main.py就行如果想深入理解建議把四種算法的代碼逐行讀一遍再自己改一些參數(shù)看看路徑怎么變化。這也是我一直覺得路徑規(guī)劃最適合入門機器人技術(shù)的核心原因——它的門檻不高但深度一點都不淺。本文還有配套的精品資源點擊獲取