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