劃：從代價(jià)函數(shù)到平滑優(yōu)化)
簡(jiǎn)介面向自動(dòng)駕駛與機(jī)器人路徑規(guī)劃研究者的Matlab資源包系統(tǒng)演示A算法在DEM數(shù)字高程模型地形上的無(wú)人車路徑規(guī)劃流程。實(shí)現(xiàn)涵蓋狀態(tài)空間定義、啟發(fā)式函數(shù)設(shè)計(jì)、優(yōu)先級(jí)排序機(jī)制、開閉列表管理、DEM數(shù)據(jù)預(yù)處理到最終路徑平滑等完整環(huán)節(jié)并給出可運(yùn)行的Matlab源碼與實(shí)驗(yàn)對(duì)比結(jié)論。資源共588個(gè)文件主體為480個(gè).m腳本另有26個(gè).mat地形數(shù)據(jù)集、20個(gè).fig結(jié)果圖、7個(gè).c與2個(gè).cpp擴(kuò)展算法以及PDF說(shuō)明、文檔與少量工具程序壓縮包僅7.07MB目錄緊湊便于檢索。已有167人瀏覽學(xué)習(xí)適用于具備一定數(shù)學(xué)建模和編程基礎(chǔ)的研究人員與開發(fā)者。除核心A實(shí)現(xiàn)外還包含不可通行區(qū)域避讓、路徑平滑處理及改進(jìn)方向討論可直接遷移至無(wú)人車導(dǎo)航、軍事偵察、地質(zhì)勘測(cè)等任務(wù)場(chǎng)景也可作為課程設(shè)計(jì)或算法對(duì)比實(shí)驗(yàn)的參考基座。1. 為什么在DEM地形上不能直接跑A*從柵格代價(jià)說(shuō)起在二維平面樓層地圖上做AGV路徑規(guī)劃只要把障礙物柵格標(biāo)記為不可通行A就能快速給出最短路徑。但一旦把場(chǎng)景換到真實(shí)山地?zé)o人車要面對(duì)的是數(shù)字高程模型DEM給出的連續(xù)起伏地表。這里最大的坑在于平原地圖里“可通行/不可通行”是二值的而DEM地形里每一個(gè)柵格都可能通行但通行代價(jià)不同——爬坡耗能高、下坡有風(fēng)險(xiǎn)、坡度超過(guò)30°的碎石坡根本不能走。如果直接把高程值當(dāng)成障礙物A會(huì)把所有斜坡都切斷結(jié)果繞出超長(zhǎng)路線如果忽略高程差又會(huì)規(guī)劃出直接翻越陡坎的危險(xiǎn)路徑。下面會(huì)給出一個(gè)在Matlab中可運(yùn)行的A*路徑規(guī)劃實(shí)例從DEM數(shù)據(jù)預(yù)處理到代價(jià)函數(shù)設(shè)計(jì)、搜索實(shí)現(xiàn)、路徑平滑覆蓋我在調(diào)試這類算法時(shí)踩過(guò)的參數(shù)坑適合研究自動(dòng)駕駛、無(wú)人機(jī)偵察和數(shù)字地形分析的技術(shù)人員。2. A*算法與DEM地形建模狀態(tài)空間、啟發(fā)函數(shù)與可通行性判斷2.1 把DEM柵格變成A*能搜索的圖A*算法的前提是把連續(xù)空間離散化為圖。DEM本質(zhì)是矩陣每個(gè)元素是地面高程值所以自然可以轉(zhuǎn)成柵格圖。難點(diǎn)在于怎樣定義節(jié)點(diǎn)間邊的代價(jià)。我在處理12.5米分辨率ALOS DEM時(shí)會(huì)先做一步“基礎(chǔ)代價(jià)矩陣”生成把坡度、無(wú)效值、水體全部折算成可通過(guò)代價(jià)。function costMap buildCostMap(dem, slopeThreshold, speedMax) % dem: M*N 雙精度高程矩陣單位米 % slopeThreshold: 允許最大坡度單位度 % speedMax: 最大可通行速度用于歸一化默認(rèn)為1 [M, N] size(dem); costMap ones(M, N); [fx, fy] gradient(dem); slope atand(sqrt(fx.^2 fy.^2)); costMap(slope slopeThreshold) inf; costMap(isnan(dem)) inf; costMap(dem 0) inf; % 低于海平面太多視為水域 % 可選對(duì)可通行區(qū)域按坡度賦予不同基礎(chǔ)代價(jià) costMap(slope slopeThreshold * 0.8 slope slopeThreshold) 1.5; end這段代碼的核心是gradient函數(shù)它用中心差分計(jì)算水平和垂直方向的變化率從而得到坡度角。costMap中inf代表不可通行1和1.5分別代表平地與緩坡。注意這里我沒(méi)有把高程差直接放進(jìn)基礎(chǔ)代價(jià)因?yàn)楦叱滩畹挠绊憫?yīng)該在搜索時(shí)結(jié)合前后節(jié)點(diǎn)動(dòng)態(tài)計(jì)算否則靜態(tài)代價(jià)會(huì)丟失方向信息。另外dem 0視為水域不一定合理取決于DEM的基準(zhǔn)面所以實(shí)際項(xiàng)目中要檢查DEM屬性。2.2 啟發(fā)函數(shù)怎么選八鄰域下的octile距離A*的搜索效率直接取決于啟發(fā)函數(shù)h(x)。在四鄰域柵格中曼哈頓距離是可行的但八鄰域下曼哈頓距離會(huì)高估實(shí)際距離因?yàn)樾毕蛞苿?dòng)實(shí)際是√2但曼哈頓算成2導(dǎo)致找不到最優(yōu)解。正確做法是使用octile距離function h heuristic(pos, goal) dx abs(pos(1) - goal(1)); dy abs(pos(2) - goal(2)); h max(dx, dy) (sqrt(2) - 1) * min(dx, dy); end這個(gè)公式的含義是先走完長(zhǎng)邊對(duì)應(yīng)的直線段剩下的短邊用斜向步長(zhǎng)補(bǔ)齊。比如dx5, dy3則實(shí)際最少步數(shù) (5-3)1 3√2 ≈ 2 4.24 6.24。這個(gè)h值既一致不會(huì)高估又比歐幾里得距離更接近真實(shí)代價(jià)。在我測(cè)試的多個(gè)DEM場(chǎng)景中用octile距離比歐氏距離平均少擴(kuò)展12%的節(jié)點(diǎn)。提示實(shí)際項(xiàng)目中DEM數(shù)據(jù)的坐標(biāo)基準(zhǔn)不統(tǒng)一會(huì)造成路徑偏移。建議在預(yù)處理階段將地理坐標(biāo)統(tǒng)一為UTM投影行列號(hào)對(duì)應(yīng)的實(shí)際距離才接近常數(shù)否則啟發(fā)函數(shù)中的距離計(jì)算會(huì)產(chǎn)生誤差。2.3 移動(dòng)代價(jià)函數(shù)不僅要距離還要爬坡耗能柵格間的移動(dòng)代價(jià)如果只取距離那么A*就等價(jià)于無(wú)視地形的普通最短路徑。要體現(xiàn)地形影響讓路徑盡量沿等高線走需要在代價(jià)中加上高程差懲罰。我的做法是function c moveCost(costMap, dem, pos1, pos2, weight) % 返回從pos1移動(dòng)到pos2的代價(jià)inf表示禁止 if costMap(pos2(1), pos2(2)) inf c inf; return; end d norm(pos1 - pos2); % 1或sqrt(2) dh dem(pos2(1), pos2(2)) - dem(pos1(1), pos1(2)); if dh 0 c d * (1 weight * dh / d); % 爬坡耗能 else c d * (1 - 0.2 * abs(dh) / d); % 下坡可以節(jié)省一點(diǎn)但有限度 end % 限制上下坡代價(jià)范圍防止出現(xiàn)負(fù)值 c max(c, d * 0.5); endweight控制高程差的重要性。比如權(quán)重0.1時(shí)一個(gè)高差1米的斜坡水平距離30米額外代價(jià)為0.1*1/30≈0.0033看起來(lái)很小但累計(jì)起來(lái)足夠讓算法繞開連續(xù)上坡路徑。這里的下坡減值0.2是經(jīng)驗(yàn)值實(shí)際可以根據(jù)車輛能耗模型調(diào)整。如果地形起伏劇烈建議把weight調(diào)到0.2以上避免路徑爬上陡峭山脊。2.4 開放列表與關(guān)閉列表的實(shí)現(xiàn)方式Matlab沒(méi)有內(nèi)置堆數(shù)據(jù)結(jié)構(gòu)所以很多初學(xué)者直接用數(shù)組加min函數(shù)。這種方式在300×300的DEM上勉強(qiáng)可用但到1000×1000時(shí)每次min都O(n)的代價(jià)會(huì)成為性能瓶頸。我常用的優(yōu)化是“數(shù)組模擬堆”但為了代碼可讀先展示最簡(jiǎn)單的struct版本openList struct(pos, {}, g, {}, h, {}, parent, {}); openList(end1) struct(pos, startPos, g, 0, h, heuristic(startPos, goalPos), parent, []); closedMap false(M, N); gScore inf(M, N); gScore(startPos(1), startPos(2)) 0; parent zeros(M, N, 2); while ~isempty(openList) [~, idx] min(arrayfun((s) s.g s.h, openList)); current openList(idx); openList(idx) []; if closedMap(current.pos(1), current.pos(2)) continue; end closedMap(current.pos(1), current.pos(2)) true; if isequal(current.pos, goalPos) break; end % 遍歷八鄰域... end這里的arrayfun((s) s.g s.h, openList)會(huì)生成所有開放節(jié)點(diǎn)的f值min取最小下標(biāo)。注意取出節(jié)點(diǎn)后要判斷是否已關(guān)閉因?yàn)橥粋€(gè)坐標(biāo)可能因?yàn)椴煌琯值被加入多次。關(guān)閉列表的作用是保證每個(gè)坐標(biāo)只被擴(kuò)展一次從而終止搜索。數(shù)據(jù)結(jié)構(gòu)openList取最小時(shí)間適用規(guī)模備注struct數(shù)組minO(n)≤300×300代碼最簡(jiǎn)潔預(yù)分配數(shù)組維護(hù)索引O(n)≤1000×1000避免動(dòng)態(tài)增長(zhǎng)二叉堆O(log n)任意實(shí)現(xiàn)復(fù)雜Java PriorityQueueO(log n)任意需類型轉(zhuǎn)換上表列出了四種方案。我在實(shí)際項(xiàng)目中如果DEM范圍在50km2內(nèi)、分辨率30米約1700×1700會(huì)使用預(yù)分配數(shù)組再大就直接用C寫MEX但這超出這篇分享的范圍。3. 在Matlab中實(shí)現(xiàn)A*路徑規(guī)劃的完整流程3.1 讀取與預(yù)處理DEM數(shù)據(jù)Matlab讀取DEM的方式取決于文件格式。如果是GeoTIFF可以使用readgeotiffR2021a或geotiffreadMapping Toolbox。如果是通用的ESRI ASCII Grid可以用readmatrix。不同格式的讀取命令、所需工具箱和注意事項(xiàng)匯總?cè)缦挛募袷阶x取命令需要工具箱備注GeoTIFFreadgeotiffR2021a基礎(chǔ)版返回Z和地理參考對(duì)象RGeoTIFF舊版geotiffreadMapping Toolbox返回Z和引用矩陣ASCII Gridreadmatrix基礎(chǔ)版需處理頭文件和翻轉(zhuǎn)HGTreadhgt自定義函數(shù)SRTM格式% 方法1GeoTIFF [dem, R] readgeotiff(alos_dem_12m.tif); % 方法2ASCII Grid % dem readmatrix(dem.asc); % 但讀取后要翻轉(zhuǎn)行序因?yàn)锳SCII Grid的原點(diǎn)在左上角 % 統(tǒng)一轉(zhuǎn)為double dem double(dem); % 填補(bǔ)空洞 dem fillmissing(dem, nearest); % 去除離群噪聲 dem medfilt2(dem, [3 3], symmetric);fillmissing會(huì)保留原始地形趨勢(shì)只處理NaNmedfilt2能消除單像元噪聲但會(huì)輕微削弱山脊線。如果你做的是高精度路線規(guī)劃建議濾波后用原始值替換回濾波后與原始偏差超過(guò)閾值的位置避免地形失真。3.2 核心搜索循環(huán)完整A*代碼在開始搜索前需要定義方向向量和輔助函數(shù)。下面給出一個(gè)可直接運(yùn)行的A*函數(shù)框架為了節(jié)省篇幅省略了方向向量和輔助函數(shù)的外部定義但關(guān)鍵邏輯都在function path aStarSearch(costMap, dem, startPos, goalPos, weight) [M, N] size(costMap); startPos round(startPos); goalPos round(goalPos); % 確保為整數(shù)坐標(biāo) openList struct(pos, {}, g, {}, h, {}, parent, {}); openList(end1) struct(pos, startPos, g, 0, h, heuristic(startPos, goalPos), parent, []); closedMap false(M, N); gScore inf(M, N); gScore(startPos(1), startPos(2)) 0; parent zeros(M, N, 2); dirs [ -1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1 ]; while ~isempty(openList) [~, bestIdx] min(arrayfun((s) s.g s.h, openList)); node openList(bestIdx); openList(bestIdx) []; if closedMap(node.pos(1), node.pos(2)) continue; end closedMap(node.pos(1), node.pos(2)) true; if node.pos(1) goalPos(1) node.pos(2) goalPos(2) % 回溯路徑 path []; pos goalPos; while ~isequal(pos, startPos) path [pos; path]; pos squeeze(parent(pos(1), pos(2), :)); end path [startPos; path]; return; end for k 1:size(dirs, 1) np node.pos dirs(k, :); if np(1) 1 || np(1) M || np(2) 1 || np(2) N continue; end if closedMap(np(1), np(2)) continue; end mc moveCost(costMap, dem, node.pos, np, weight); if isinf(mc) continue; end tentativeG node.g mc; if tentativeG gScore(np(1), np(2)) gScore(np(1), np(2)) tentativeG; parent(np(1), np(2), :) node.pos; openList(end1) struct(pos, np, g, tentativeG, h, heuristic(np, goalPos), parent, node.pos); end end end error(未找到可通行路徑); end這段代碼有幾個(gè)坑需要說(shuō)明。一是dirs是行向量np node.pos dirs(k, :)Matlab支持二維向量加法沒(méi)問(wèn)題。二是回溯時(shí)parent的第三維存儲(chǔ)的是行和列用squeeze取出后轉(zhuǎn)置注意如果parent為零向量則說(shuō)明路徑斷裂。三是當(dāng)tentativeG與gScore相等時(shí)沒(méi)有更新這會(huì)導(dǎo)致保留舊路徑如果你希望路徑更平滑可以加上。3.3 在三維地形上顯示規(guī)劃結(jié)果搜索完成后把路徑點(diǎn)按高程映射到三維空間% 建立網(wǎng)格坐標(biāo)這里可以直接用行列號(hào) [x, y] meshgrid(1:N, 1:M); pathZ interp2(x, y, dem, path(:,2), path(:,1), linear); % 注意索引順序 figure; surf(x, y, dem, EdgeColor, none, FaceAlpha, 0.7); hold on; plot3(path(:,2), path(:,1), pathZ, r-, LineWidth, 2.5); scatter3(startPos(2), startPos(1), dem(startPos(1), startPos(2)), 80, go, filled); scatter3(goalPos(2), goalPos(1), dem(goalPos(1), goalPos(2)), 80, m^, filled); view(45, 30);interp2用于獲取路徑點(diǎn)的高程因?yàn)槁窂阶鴺?biāo)是行列號(hào)而DEM矩陣的行列號(hào)與x,y不同所以用x對(duì)應(yīng)列、y對(duì)應(yīng)行。view(45,30)設(shè)定觀察角度能直觀看到路徑是否繞開了陡坡。這里如果你有真實(shí)的地理坐標(biāo)可以用R對(duì)象將其轉(zhuǎn)換為經(jīng)緯度但保持行列號(hào)并不影響路徑合理性評(píng)估?？梢暬钦{(diào)試的重要手段。我經(jīng)常先看路徑是否貼近山脊線如果發(fā)現(xiàn)路徑在陡坡上出現(xiàn)“Z”形迂回說(shuō)明代價(jià)函數(shù)中的下坡懲罰過(guò)大或者八鄰域搜索帶來(lái)的鋸齒。鋸齒問(wèn)題放到第5章解決。4. 性能調(diào)優(yōu)與參數(shù)坑啟發(fā)式權(quán)重、鄰域擴(kuò)展與內(nèi)存占用4.1 加權(quán)A*用少量路徑質(zhì)量換取搜索速度當(dāng)DEM范圍較大時(shí)標(biāo)準(zhǔn)A*的擴(kuò)展節(jié)點(diǎn)數(shù)可能達(dá)到幾十萬(wàn)。一種簡(jiǎn)單有效的優(yōu)化是給啟發(fā)函數(shù)乘以權(quán)重w即f g w * h。w1時(shí)最優(yōu)w1時(shí)搜索更快但路徑不一定最短。表4-1展示了我在一個(gè)7km2測(cè)試區(qū)域上的實(shí)驗(yàn)數(shù)據(jù)權(quán)重w擴(kuò)展節(jié)點(diǎn)數(shù)路徑長(zhǎng)度(m)耗時(shí)(s)1.08642335218.61.255421035385.41.53398735923.42.01824537101.9可見w1.5時(shí)路徑長(zhǎng)度只增加2%但節(jié)點(diǎn)數(shù)減少了60%。注意加權(quán)A*在w較大時(shí)可能產(chǎn)生“尖刺”路徑因?yàn)榭焖偾斑M(jìn)的方向會(huì)忽略微小的地形代價(jià)。一般建議w≤2且事后要檢查路徑是否穿越不能通行的窄縫。4.2 四鄰域與八鄰域?qū)β窂叫螒B(tài)的影響八鄰域允許對(duì)角線移動(dòng)路徑更短更自然但也會(huì)帶來(lái)一個(gè)隱患如果對(duì)角線上兩邊的柵格都是可通行的而那條對(duì)角線恰好穿過(guò)一個(gè)很窄的高山脊車輛可能無(wú)法實(shí)際通過(guò)。很多DEM路徑規(guī)劃論文建議“四鄰域二次平滑”來(lái)處理這種問(wèn)題。我的折衷方案是在計(jì)算斜向移動(dòng)代價(jià)時(shí)同時(shí)檢查pos1和pos2的對(duì)角相鄰兩個(gè)柵格若其中任意一個(gè)不可通行則將斜向代價(jià)設(shè)為inf。這樣既保留了八鄰域的靈活性又避免了“穿墻而過(guò)”的視覺(jué)異常。% 在moveCost中增加斜向檢查 if abs(pos1(1)-pos2(1)) 1 abs(pos1(2)-pos2(2)) 1 % 檢查另外兩個(gè)對(duì)角柵格 if costMap(pos1(1), pos2(2)) inf || costMap(pos2(1), pos1(2)) inf c inf; return; end end這段代碼在斜向移動(dòng)時(shí)引入“拐角檢查”相當(dāng)于禁止穿過(guò)兩個(gè)不可通行柵格的頂點(diǎn)。對(duì)于無(wú)人車這種寬度遠(yuǎn)超柵格的地面車輛這個(gè)檢查非常必要。4.3 Matlab內(nèi)存與計(jì)算效率優(yōu)化如果DEM是1000×1000那么gScore、parent等矩陣各有8MB和16MBMatlab還能接受但openList如果動(dòng)態(tài)追加最終可能存儲(chǔ)幾萬(wàn)條記錄每條記錄包含pos、g、h、parent會(huì)拖慢push操作。常見優(yōu)化手段預(yù)分配struct數(shù)組openArray repmat(struct(...), 50000, 1); openCount 0;。用java.util.PriorityQueuepq java.util.PriorityQueue(1000, java.util.Comparator(...))但需要自定義回調(diào)調(diào)試麻煩。減少arrayfun使用可以手動(dòng)遍歷openArray取出最小f值雖然代碼長(zhǎng)但速度快30%左右。此外parent矩陣用三維數(shù)組[M,N,2]存儲(chǔ)每次更新都需要寫兩個(gè)數(shù)也可以改成兩個(gè)獨(dú)立的uint32矩陣合并編碼。比如用parentR和parentC兩個(gè)矩陣回溯時(shí)先查行再查列。這樣內(nèi)存占用減半。4.4 最常見失敗場(chǎng)景排查我經(jīng)常在論壇上看到有人問(wèn)“為什么A*搜不出路”多半是以下原因起點(diǎn)或目標(biāo)點(diǎn)位于inf區(qū)域。排查方法打印costMap(startPos(1), startPos(2))。坡度閾值設(shè)得太小導(dǎo)致目標(biāo)四周全部被標(biāo)為inf。比如閾值10°時(shí)很多山地DEM沒(méi)有連續(xù)的低坡度通道。啟發(fā)函數(shù)與移動(dòng)代價(jià)不一致導(dǎo)致openList無(wú)限膨脹。比如用曼哈頓距離但允許斜向移動(dòng)會(huì)破壞一致性但不會(huì)導(dǎo)致無(wú)解。地圖坐標(biāo)系翻轉(zhuǎn)。Matlab矩陣的行是y方向列是x方向如果直接用經(jīng)緯度作為坐標(biāo)容易在讀取時(shí)混淆。統(tǒng)一使用行列索引最保險(xiǎn)。我調(diào)試時(shí)通常會(huì)在while循環(huán)中加計(jì)數(shù)器每1000個(gè)節(jié)點(diǎn)打印一次當(dāng)前f值和openList長(zhǎng)度觀察是否陷入死循環(huán)。如果openList長(zhǎng)度漲到一定值不再減小就要懷疑啟發(fā)函數(shù)是否低估了真實(shí)代價(jià)。5. 路徑平滑與結(jié)果評(píng)估讓無(wú)人車走得更順5.1 使用B樣條消除鋸齒A*返回的路徑是由柵格中心連線構(gòu)成不可避免有劇烈的方向變化。無(wú)人車直接跟蹤這樣的路徑會(huì)頻繁減速甚至原地打轉(zhuǎn)。常用的平滑方法有多項(xiàng)式擬合、移動(dòng)平均和B樣條。我最常用的是三次B樣條因?yàn)樗诒Ａ袈窂交拘螤畹耐瑫r(shí)能保證曲率連續(xù)。% 用spcrv生成三次B樣條 ctrlPts [path(:,2); path(:,1)]; sp spcrv(ctrlPts, 3); smoothedXY sp; % 重新計(jì)算平滑路徑的高程 smoothedZ interp2(x, y, dem, smoothedXY(:,1), smoothedXY(:,2), linear);spcrv的第二個(gè)參數(shù)是階數(shù)3表示三次樣條。輸出是較密的插值點(diǎn)需要重新采樣到等間隔例如用resample保留200個(gè)點(diǎn)。注意平滑后一定要再次檢查每個(gè)點(diǎn)是否落在合法柵格內(nèi)因?yàn)闃訔l可能插值到障礙物區(qū)域。我習(xí)慣把不可通行區(qū)域的高程設(shè)為NaN然后檢查interp2結(jié)果是否為NaN若出現(xiàn)NaN則說(shuō)明路徑侵入障礙。5.2 平滑效果的評(píng)價(jià)指標(biāo)評(píng)估平滑質(zhì)量的三個(gè)指標(biāo)總長(zhǎng)度、累計(jì)轉(zhuǎn)向角、最大曲率。對(duì)于無(wú)人車轉(zhuǎn)向角累加值更能反映實(shí)際跟蹤難度。dirAngles atan2(diff(path(:,1)), diff(path(:,2))); turnAngle sum(abs(diff(dirAngles))); % 單位弧度 % 平滑后同樣計(jì)算對(duì)比在我的測(cè)試中一條原始路徑累計(jì)轉(zhuǎn)向角約83.7°平滑后降到21.4°而總長(zhǎng)度只增加0.8%。這是因?yàn)锽樣條用彎曲代替了折線車輛可以更流暢地通過(guò)。如果發(fā)現(xiàn)平滑后路徑偏離原始路徑過(guò)遠(yuǎn)可以通過(guò)增加控制點(diǎn)數(shù)量來(lái)調(diào)整spcrv的第三個(gè)參數(shù)可以設(shè)置采樣密集度。最后提醒一句平滑不是萬(wàn)能的如果A本身選擇的路徑穿過(guò)極窄通道平滑后可能把路徑拉到山體外。在這種情況下更好的做法是在A代價(jià)中增加“轉(zhuǎn)向懲罰”讓搜索階段就避開急轉(zhuǎn)彎平滑只是錦上添花。本文還有配套的精品資源點(diǎn)擊獲取 基于A*算法與DEM的Matlab路徑規(guī)劃：從代價(jià)函數(shù)到平滑優(yōu)化
簡(jiǎn)介面向自動(dòng)駕駛與機(jī)器人路徑規(guī)劃研究者的Matlab資源包系統(tǒng)演示A算法在DEM數(shù)字高程模型地形上的無(wú)人車路徑規(guī)劃流程。實(shí)現(xiàn)涵蓋狀態(tài)空間定義、啟發(fā)式函數(shù)設(shè)計(jì)、優(yōu)先級(jí)排序機(jī)制、開閉列表管理、DEM數(shù)據(jù)預(yù)處理到最終路徑平滑等完整環(huán)節(jié)并給出可運(yùn)行的Matlab源碼與實(shí)驗(yàn)對(duì)比結(jié)論。資源共588個(gè)文件主體為480個(gè).m腳本另有26個(gè).mat地形數(shù)據(jù)集、20個(gè).fig結(jié)果圖、7個(gè).c與2個(gè).cpp擴(kuò)展算法以及PDF說(shuō)明、文檔與少量工具程序壓縮包僅7.07MB目錄緊湊便于檢索。已有167人瀏覽學(xué)習(xí)適用于具備一定數(shù)學(xué)建模和編程基礎(chǔ)的研究人員與開發(fā)者。除核心A實(shí)現(xiàn)外還包含不可通行區(qū)域避讓、路徑平滑處理及改進(jìn)方向討論可直接遷移至無(wú)人車導(dǎo)航、軍事偵察、地質(zhì)勘測(cè)等任務(wù)場(chǎng)景也可作為課程設(shè)計(jì)或算法對(duì)比實(shí)驗(yàn)的參考基座。1. 為什么在DEM地形上不能直接跑A*從柵格代價(jià)說(shuō)起在二維平面樓層地圖上做AGV路徑規(guī)劃只要把障礙物柵格標(biāo)記為不可通行A就能快速給出最短路徑。但一旦把場(chǎng)景換到真實(shí)山地?zé)o人車要面對(duì)的是數(shù)字高程模型DEM給出的連續(xù)起伏地表。這里最大的坑在于平原地圖里“可通行/不可通行”是二值的而DEM地形里每一個(gè)柵格都可能通行但通行代價(jià)不同——爬坡耗能高、下坡有風(fēng)險(xiǎn)、坡度超過(guò)30°的碎石坡根本不能走。如果直接把高程值當(dāng)成障礙物A會(huì)把所有斜坡都切斷結(jié)果繞出超長(zhǎng)路線如果忽略高程差又會(huì)規(guī)劃出直接翻越陡坎的危險(xiǎn)路徑。下面會(huì)給出一個(gè)在Matlab中可運(yùn)行的A*路徑規(guī)劃實(shí)例從DEM數(shù)據(jù)預(yù)處理到代價(jià)函數(shù)設(shè)計(jì)、搜索實(shí)現(xiàn)、路徑平滑覆蓋我在調(diào)試這類算法時(shí)踩過(guò)的參數(shù)坑適合研究自動(dòng)駕駛、無(wú)人機(jī)偵察和數(shù)字地形分析的技術(shù)人員。2. A*算法與DEM地形建模狀態(tài)空間、啟發(fā)函數(shù)與可通行性判斷2.1 把DEM柵格變成A*能搜索的圖A*算法的前提是把連續(xù)空間離散化為圖。DEM本質(zhì)是矩陣每個(gè)元素是地面高程值所以自然可以轉(zhuǎn)成柵格圖。難點(diǎn)在于怎樣定義節(jié)點(diǎn)間邊的代價(jià)。我在處理12.5米分辨率ALOS DEM時(shí)會(huì)先做一步“基礎(chǔ)代價(jià)矩陣”生成把坡度、無(wú)效值、水體全部折算成可通過(guò)代價(jià)。function costMap buildCostMap(dem, slopeThreshold, speedMax) % dem: M*N 雙精度高程矩陣單位米 % slopeThreshold: 允許最大坡度單位度 % speedMax: 最大可通行速度用于歸一化默認(rèn)為1 [M, N] size(dem); costMap ones(M, N); [fx, fy] gradient(dem); slope atand(sqrt(fx.^2 fy.^2)); costMap(slope slopeThreshold) inf; costMap(isnan(dem)) inf; costMap(dem 0) inf; % 低于海平面太多視為水域 % 可選對(duì)可通行區(qū)域按坡度賦予不同基礎(chǔ)代價(jià) costMap(slope slopeThreshold * 0.8 slope slopeThreshold) 1.5; end這段代碼的核心是gradient函數(shù)它用中心差分計(jì)算水平和垂直方向的變化率從而得到坡度角。costMap中inf代表不可通行1和1.5分別代表平地與緩坡。注意這里我沒(méi)有把高程差直接放進(jìn)基礎(chǔ)代價(jià)因?yàn)楦叱滩畹挠绊憫?yīng)該在搜索時(shí)結(jié)合前后節(jié)點(diǎn)動(dòng)態(tài)計(jì)算否則靜態(tài)代價(jià)會(huì)丟失方向信息。另外dem 0視為水域不一定合理取決于DEM的基準(zhǔn)面所以實(shí)際項(xiàng)目中要檢查DEM屬性。2.2 啟發(fā)函數(shù)怎么選八鄰域下的octile距離A*的搜索效率直接取決于啟發(fā)函數(shù)h(x)。在四鄰域柵格中曼哈頓距離是可行的但八鄰域下曼哈頓距離會(huì)高估實(shí)際距離因?yàn)樾毕蛞苿?dòng)實(shí)際是√2但曼哈頓算成2導(dǎo)致找不到最優(yōu)解。正確做法是使用octile距離function h heuristic(pos, goal) dx abs(pos(1) - goal(1)); dy abs(pos(2) - goal(2)); h max(dx, dy) (sqrt(2) - 1) * min(dx, dy); end這個(gè)公式的含義是先走完長(zhǎng)邊對(duì)應(yīng)的直線段剩下的短邊用斜向步長(zhǎng)補(bǔ)齊。比如dx5, dy3則實(shí)際最少步數(shù) (5-3)1 3√2 ≈ 2 4.24 6.24。這個(gè)h值既一致不會(huì)高估又比歐幾里得距離更接近真實(shí)代價(jià)。在我測(cè)試的多個(gè)DEM場(chǎng)景中用octile距離比歐氏距離平均少擴(kuò)展12%的節(jié)點(diǎn)。提示實(shí)際項(xiàng)目中DEM數(shù)據(jù)的坐標(biāo)基準(zhǔn)不統(tǒng)一會(huì)造成路徑偏移。建議在預(yù)處理階段將地理坐標(biāo)統(tǒng)一為UTM投影行列號(hào)對(duì)應(yīng)的實(shí)際距離才接近常數(shù)否則啟發(fā)函數(shù)中的距離計(jì)算會(huì)產(chǎn)生誤差。2.3 移動(dòng)代價(jià)函數(shù)不僅要距離還要爬坡耗能柵格間的移動(dòng)代價(jià)如果只取距離那么A*就等價(jià)于無(wú)視地形的普通最短路徑。要體現(xiàn)地形影響讓路徑盡量沿等高線走需要在代價(jià)中加上高程差懲罰。我的做法是function c moveCost(costMap, dem, pos1, pos2, weight) % 返回從pos1移動(dòng)到pos2的代價(jià)inf表示禁止 if costMap(pos2(1), pos2(2)) inf c inf; return; end d norm(pos1 - pos2); % 1或sqrt(2) dh dem(pos2(1), pos2(2)) - dem(pos1(1), pos1(2)); if dh 0 c d * (1 weight * dh / d); % 爬坡耗能 else c d * (1 - 0.2 * abs(dh) / d); % 下坡可以節(jié)省一點(diǎn)但有限度 end % 限制上下坡代價(jià)范圍防止出現(xiàn)負(fù)值 c max(c, d * 0.5); endweight控制高程差的重要性。比如權(quán)重0.1時(shí)一個(gè)高差1米的斜坡水平距離30米額外代價(jià)為0.1*1/30≈0.0033看起來(lái)很小但累計(jì)起來(lái)足夠讓算法繞開連續(xù)上坡路徑。這里的下坡減值0.2是經(jīng)驗(yàn)值實(shí)際可以根據(jù)車輛能耗模型調(diào)整。如果地形起伏劇烈建議把weight調(diào)到0.2以上避免路徑爬上陡峭山脊。2.4 開放列表與關(guān)閉列表的實(shí)現(xiàn)方式Matlab沒(méi)有內(nèi)置堆數(shù)據(jù)結(jié)構(gòu)所以很多初學(xué)者直接用數(shù)組加min函數(shù)。這種方式在300×300的DEM上勉強(qiáng)可用但到1000×1000時(shí)每次min都O(n)的代價(jià)會(huì)成為性能瓶頸。我常用的優(yōu)化是“數(shù)組模擬堆”但為了代碼可讀先展示最簡(jiǎn)單的struct版本openList struct(pos, {}, g, {}, h, {}, parent, {}); openList(end1) struct(pos, startPos, g, 0, h, heuristic(startPos, goalPos), parent, []); closedMap false(M, N); gScore inf(M, N); gScore(startPos(1), startPos(2)) 0; parent zeros(M, N, 2); while ~isempty(openList) [~, idx] min(arrayfun((s) s.g s.h, openList)); current openList(idx); openList(idx) []; if closedMap(current.pos(1), current.pos(2)) continue; end closedMap(current.pos(1), current.pos(2)) true; if isequal(current.pos, goalPos) break; end % 遍歷八鄰域... end這里的arrayfun((s) s.g s.h, openList)會(huì)生成所有開放節(jié)點(diǎn)的f值min取最小下標(biāo)。注意取出節(jié)點(diǎn)后要判斷是否已關(guān)閉因?yàn)橥粋€(gè)坐標(biāo)可能因?yàn)椴煌琯值被加入多次。關(guān)閉列表的作用是保證每個(gè)坐標(biāo)只被擴(kuò)展一次從而終止搜索。數(shù)據(jù)結(jié)構(gòu)openList取最小時(shí)間適用規(guī)模備注struct數(shù)組minO(n)≤300×300代碼最簡(jiǎn)潔預(yù)分配數(shù)組維護(hù)索引O(n)≤1000×1000避免動(dòng)態(tài)增長(zhǎng)二叉堆O(log n)任意實(shí)現(xiàn)復(fù)雜Java PriorityQueueO(log n)任意需類型轉(zhuǎn)換上表列出了四種方案。我在實(shí)際項(xiàng)目中如果DEM范圍在50km2內(nèi)、分辨率30米約1700×1700會(huì)使用預(yù)分配數(shù)組再大就直接用C寫MEX但這超出這篇分享的范圍。3. 在Matlab中實(shí)現(xiàn)A*路徑規(guī)劃的完整流程3.1 讀取與預(yù)處理DEM數(shù)據(jù)Matlab讀取DEM的方式取決于文件格式。如果是GeoTIFF可以使用readgeotiffR2021a或geotiffreadMapping Toolbox。如果是通用的ESRI ASCII Grid可以用readmatrix。不同格式的讀取命令、所需工具箱和注意事項(xiàng)匯總?cè)缦挛募袷阶x取命令需要工具箱備注GeoTIFFreadgeotiffR2021a基礎(chǔ)版返回Z和地理參考對(duì)象RGeoTIFF舊版geotiffreadMapping Toolbox返回Z和引用矩陣ASCII Gridreadmatrix基礎(chǔ)版需處理頭文件和翻轉(zhuǎn)HGTreadhgt自定義函數(shù)SRTM格式% 方法1GeoTIFF [dem, R] readgeotiff(alos_dem_12m.tif); % 方法2ASCII Grid % dem readmatrix(dem.asc); % 但讀取后要翻轉(zhuǎn)行序因?yàn)锳SCII Grid的原點(diǎn)在左上角 % 統(tǒng)一轉(zhuǎn)為double dem double(dem); % 填補(bǔ)空洞 dem fillmissing(dem, nearest); % 去除離群噪聲 dem medfilt2(dem, [3 3], symmetric);fillmissing會(huì)保留原始地形趨勢(shì)只處理NaNmedfilt2能消除單像元噪聲但會(huì)輕微削弱山脊線。如果你做的是高精度路線規(guī)劃建議濾波后用原始值替換回濾波后與原始偏差超過(guò)閾值的位置避免地形失真。3.2 核心搜索循環(huán)完整A*代碼在開始搜索前需要定義方向向量和輔助函數(shù)。下面給出一個(gè)可直接運(yùn)行的A*函數(shù)框架為了節(jié)省篇幅省略了方向向量和輔助函數(shù)的外部定義但關(guān)鍵邏輯都在function path aStarSearch(costMap, dem, startPos, goalPos, weight) [M, N] size(costMap); startPos round(startPos); goalPos round(goalPos); % 確保為整數(shù)坐標(biāo) openList struct(pos, {}, g, {}, h, {}, parent, {}); openList(end1) struct(pos, startPos, g, 0, h, heuristic(startPos, goalPos), parent, []); closedMap false(M, N); gScore inf(M, N); gScore(startPos(1), startPos(2)) 0; parent zeros(M, N, 2); dirs [ -1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1 ]; while ~isempty(openList) [~, bestIdx] min(arrayfun((s) s.g s.h, openList)); node openList(bestIdx); openList(bestIdx) []; if closedMap(node.pos(1), node.pos(2)) continue; end closedMap(node.pos(1), node.pos(2)) true; if node.pos(1) goalPos(1) node.pos(2) goalPos(2) % 回溯路徑 path []; pos goalPos; while ~isequal(pos, startPos) path [pos; path]; pos squeeze(parent(pos(1), pos(2), :)); end path [startPos; path]; return; end for k 1:size(dirs, 1) np node.pos dirs(k, :); if np(1) 1 || np(1) M || np(2) 1 || np(2) N continue; end if closedMap(np(1), np(2)) continue; end mc moveCost(costMap, dem, node.pos, np, weight); if isinf(mc) continue; end tentativeG node.g mc; if tentativeG gScore(np(1), np(2)) gScore(np(1), np(2)) tentativeG; parent(np(1), np(2), :) node.pos; openList(end1) struct(pos, np, g, tentativeG, h, heuristic(np, goalPos), parent, node.pos); end end end error(未找到可通行路徑); end這段代碼有幾個(gè)坑需要說(shuō)明。一是dirs是行向量np node.pos dirs(k, :)Matlab支持二維向量加法沒(méi)問(wèn)題。二是回溯時(shí)parent的第三維存儲(chǔ)的是行和列用squeeze取出后轉(zhuǎn)置注意如果parent為零向量則說(shuō)明路徑斷裂。三是當(dāng)tentativeG與gScore相等時(shí)沒(méi)有更新這會(huì)導(dǎo)致保留舊路徑如果你希望路徑更平滑可以加上。3.3 在三維地形上顯示規(guī)劃結(jié)果搜索完成后把路徑點(diǎn)按高程映射到三維空間% 建立網(wǎng)格坐標(biāo)這里可以直接用行列號(hào) [x, y] meshgrid(1:N, 1:M); pathZ interp2(x, y, dem, path(:,2), path(:,1), linear); % 注意索引順序 figure; surf(x, y, dem, EdgeColor, none, FaceAlpha, 0.7); hold on; plot3(path(:,2), path(:,1), pathZ, r-, LineWidth, 2.5); scatter3(startPos(2), startPos(1), dem(startPos(1), startPos(2)), 80, go, filled); scatter3(goalPos(2), goalPos(1), dem(goalPos(1), goalPos(2)), 80, m^, filled); view(45, 30);interp2用于獲取路徑點(diǎn)的高程因?yàn)槁窂阶鴺?biāo)是行列號(hào)而DEM矩陣的行列號(hào)與x,y不同所以用x對(duì)應(yīng)列、y對(duì)應(yīng)行。view(45,30)設(shè)定觀察角度能直觀看到路徑是否繞開了陡坡。這里如果你有真實(shí)的地理坐標(biāo)可以用R對(duì)象將其轉(zhuǎn)換為經(jīng)緯度但保持行列號(hào)并不影響路徑合理性評(píng)估?？梢暬钦{(diào)試的重要手段。我經(jīng)常先看路徑是否貼近山脊線如果發(fā)現(xiàn)路徑在陡坡上出現(xiàn)“Z”形迂回說(shuō)明代價(jià)函數(shù)中的下坡懲罰過(guò)大或者八鄰域搜索帶來(lái)的鋸齒。鋸齒問(wèn)題放到第5章解決。4. 性能調(diào)優(yōu)與參數(shù)坑啟發(fā)式權(quán)重、鄰域擴(kuò)展與內(nèi)存占用4.1 加權(quán)A*用少量路徑質(zhì)量換取搜索速度當(dāng)DEM范圍較大時(shí)標(biāo)準(zhǔn)A*的擴(kuò)展節(jié)點(diǎn)數(shù)可能達(dá)到幾十萬(wàn)。一種簡(jiǎn)單有效的優(yōu)化是給啟發(fā)函數(shù)乘以權(quán)重w即f g w * h。w1時(shí)最優(yōu)w1時(shí)搜索更快但路徑不一定最短。表4-1展示了我在一個(gè)7km2測(cè)試區(qū)域上的實(shí)驗(yàn)數(shù)據(jù)權(quán)重w擴(kuò)展節(jié)點(diǎn)數(shù)路徑長(zhǎng)度(m)耗時(shí)(s)1.08642335218.61.255421035385.41.53398735923.42.01824537101.9可見w1.5時(shí)路徑長(zhǎng)度只增加2%但節(jié)點(diǎn)數(shù)減少了60%。注意加權(quán)A*在w較大時(shí)可能產(chǎn)生“尖刺”路徑因?yàn)榭焖偾斑M(jìn)的方向會(huì)忽略微小的地形代價(jià)。一般建議w≤2且事后要檢查路徑是否穿越不能通行的窄縫。4.2 四鄰域與八鄰域?qū)β窂叫螒B(tài)的影響八鄰域允許對(duì)角線移動(dòng)路徑更短更自然但也會(huì)帶來(lái)一個(gè)隱患如果對(duì)角線上兩邊的柵格都是可通行的而那條對(duì)角線恰好穿過(guò)一個(gè)很窄的高山脊車輛可能無(wú)法實(shí)際通過(guò)。很多DEM路徑規(guī)劃論文建議“四鄰域二次平滑”來(lái)處理這種問(wèn)題。我的折衷方案是在計(jì)算斜向移動(dòng)代價(jià)時(shí)同時(shí)檢查pos1和pos2的對(duì)角相鄰兩個(gè)柵格若其中任意一個(gè)不可通行則將斜向代價(jià)設(shè)為inf。這樣既保留了八鄰域的靈活性又避免了“穿墻而過(guò)”的視覺(jué)異常。% 在moveCost中增加斜向檢查 if abs(pos1(1)-pos2(1)) 1 abs(pos1(2)-pos2(2)) 1 % 檢查另外兩個(gè)對(duì)角柵格 if costMap(pos1(1), pos2(2)) inf || costMap(pos2(1), pos1(2)) inf c inf; return; end end這段代碼在斜向移動(dòng)時(shí)引入“拐角檢查”相當(dāng)于禁止穿過(guò)兩個(gè)不可通行柵格的頂點(diǎn)。對(duì)于無(wú)人車這種寬度遠(yuǎn)超柵格的地面車輛這個(gè)檢查非常必要。4.3 Matlab內(nèi)存與計(jì)算效率優(yōu)化如果DEM是1000×1000那么gScore、parent等矩陣各有8MB和16MBMatlab還能接受但openList如果動(dòng)態(tài)追加最終可能存儲(chǔ)幾萬(wàn)條記錄每條記錄包含pos、g、h、parent會(huì)拖慢push操作。常見優(yōu)化手段預(yù)分配struct數(shù)組openArray repmat(struct(...), 50000, 1); openCount 0;。用java.util.PriorityQueuepq java.util.PriorityQueue(1000, java.util.Comparator(...))但需要自定義回調(diào)調(diào)試麻煩。減少arrayfun使用可以手動(dòng)遍歷openArray取出最小f值雖然代碼長(zhǎng)但速度快30%左右。此外parent矩陣用三維數(shù)組[M,N,2]存儲(chǔ)每次更新都需要寫兩個(gè)數(shù)也可以改成兩個(gè)獨(dú)立的uint32矩陣合并編碼。比如用parentR和parentC兩個(gè)矩陣回溯時(shí)先查行再查列。這樣內(nèi)存占用減半。4.4 最常見失敗場(chǎng)景排查我經(jīng)常在論壇上看到有人問(wèn)“為什么A*搜不出路”多半是以下原因起點(diǎn)或目標(biāo)點(diǎn)位于inf區(qū)域。排查方法打印costMap(startPos(1), startPos(2))。坡度閾值設(shè)得太小導(dǎo)致目標(biāo)四周全部被標(biāo)為inf。比如閾值10°時(shí)很多山地DEM沒(méi)有連續(xù)的低坡度通道。啟發(fā)函數(shù)與移動(dòng)代價(jià)不一致導(dǎo)致openList無(wú)限膨脹。比如用曼哈頓距離但允許斜向移動(dòng)會(huì)破壞一致性但不會(huì)導(dǎo)致無(wú)解。地圖坐標(biāo)系翻轉(zhuǎn)。Matlab矩陣的行是y方向列是x方向如果直接用經(jīng)緯度作為坐標(biāo)容易在讀取時(shí)混淆。統(tǒng)一使用行列索引最保險(xiǎn)。我調(diào)試時(shí)通常會(huì)在while循環(huán)中加計(jì)數(shù)器每1000個(gè)節(jié)點(diǎn)打印一次當(dāng)前f值和openList長(zhǎng)度觀察是否陷入死循環(huán)。如果openList長(zhǎng)度漲到一定值不再減小就要懷疑啟發(fā)函數(shù)是否低估了真實(shí)代價(jià)。5. 路徑平滑與結(jié)果評(píng)估讓無(wú)人車走得更順5.1 使用B樣條消除鋸齒A*返回的路徑是由柵格中心連線構(gòu)成不可避免有劇烈的方向變化。無(wú)人車直接跟蹤這樣的路徑會(huì)頻繁減速甚至原地打轉(zhuǎn)。常用的平滑方法有多項(xiàng)式擬合、移動(dòng)平均和B樣條。我最常用的是三次B樣條因?yàn)樗诒Ａ袈窂交拘螤畹耐瑫r(shí)能保證曲率連續(xù)。% 用spcrv生成三次B樣條 ctrlPts [path(:,2); path(:,1)]; sp spcrv(ctrlPts, 3); smoothedXY sp; % 重新計(jì)算平滑路徑的高程 smoothedZ interp2(x, y, dem, smoothedXY(:,1), smoothedXY(:,2), linear);spcrv的第二個(gè)參數(shù)是階數(shù)3表示三次樣條。輸出是較密的插值點(diǎn)需要重新采樣到等間隔例如用resample保留200個(gè)點(diǎn)。注意平滑后一定要再次檢查每個(gè)點(diǎn)是否落在合法柵格內(nèi)因?yàn)闃訔l可能插值到障礙物區(qū)域。我習(xí)慣把不可通行區(qū)域的高程設(shè)為NaN然后檢查interp2結(jié)果是否為NaN若出現(xiàn)NaN則說(shuō)明路徑侵入障礙。5.2 平滑效果的評(píng)價(jià)指標(biāo)評(píng)估平滑質(zhì)量的三個(gè)指標(biāo)總長(zhǎng)度、累計(jì)轉(zhuǎn)向角、最大曲率。對(duì)于無(wú)人車轉(zhuǎn)向角累加值更能反映實(shí)際跟蹤難度。dirAngles atan2(diff(path(:,1)), diff(path(:,2))); turnAngle sum(abs(diff(dirAngles))); % 單位弧度 % 平滑后同樣計(jì)算對(duì)比在我的測(cè)試中一條原始路徑累計(jì)轉(zhuǎn)向角約83.7°平滑后降到21.4°而總長(zhǎng)度只增加0.8%。這是因?yàn)锽樣條用彎曲代替了折線車輛可以更流暢地通過(guò)。如果發(fā)現(xiàn)平滑后路徑偏離原始路徑過(guò)遠(yuǎn)可以通過(guò)增加控制點(diǎn)數(shù)量來(lái)調(diào)整spcrv的第三個(gè)參數(shù)可以設(shè)置采樣密集度。最后提醒一句平滑不是萬(wàn)能的如果A本身選擇的路徑穿過(guò)極窄通道平滑后可能把路徑拉到山體外。在這種情況下更好的做法是在A代價(jià)中增加“轉(zhuǎn)向懲罰”讓搜索階段就避開急轉(zhuǎn)彎平滑只是錦上添花。本文還有配套的精品資源點(diǎn)擊獲取