這幾年做水面無人艇導(dǎo)航系統(tǒng)繞不開的一個硬骨頭就是全局路徑規(guī)劃。很多人一上來就盯著局部避碰、動態(tài)窗口、模型預(yù)測控制這些看起來高級的東西但真到湖上、海上跑起來就發(fā)現(xiàn)如果全局規(guī)劃沒做好艇連港口都出不去更別提什么避碰了。我自己的經(jīng)驗是全局路徑規(guī)劃才是整個導(dǎo)航鏈路里真正決定任務(wù)成敗的地基而在地基之上鋪的第一塊磚就是電子海圖數(shù)據(jù)。這個項目做的就是一件事基于電子海圖數(shù)據(jù)為水面無人艇在已知靜態(tài)環(huán)境中搜索一條從起點到終點的安全、可行、符合船舶操縱習(xí)慣的全局航線。它解決的核心問題是路在哪里、哪里不能去、怎么走最穩(wěn)妥輸出的不是逐幀軌跡而是一串帶語義信息的航路點給后續(xù)局部避碰模塊當(dāng)上游參考線。適合正在做無人艇導(dǎo)航、內(nèi)河/近海測繪任務(wù)規(guī)劃、海事仿真系統(tǒng)以及剛?cè)胄邢敫愣D數(shù)據(jù)到底怎么用的工程師參考。下面我把整個方案的思路、數(shù)據(jù)解析、算法實現(xiàn)和踩坑經(jīng)驗完整拆開講。1. 從海圖到航線無人艇全局規(guī)劃到底在解決什么問題1.1 為什么電子海圖是全局規(guī)劃的第一優(yōu)先級數(shù)據(jù)源陸地上的機器人導(dǎo)航可以用激光雷達建圖無人機可以用航測影像建模但水面無人艇的傳感器視角天然吃虧——雷達對小目標(biāo)不敏感攝像頭在開闊水面容易丟特征測深儀只能看正下方一條線。也就是說無人艇對環(huán)境的感知是嚴(yán)重受限的。全局規(guī)劃如果還依賴艇上傳感器現(xiàn)場建圖安全性完全沒有保障。電子海圖的優(yōu)勢在于它是一個先驗的、已經(jīng)經(jīng)過官方測量和審核的靜態(tài)環(huán)境模型。陸地在哪里、港口航道怎么走、哪里有沉船、哪里有暗礁、水深是多少這些信息在出航之前就已經(jīng)按照統(tǒng)一標(biāo)準(zhǔn)編碼好了。拿它做全局規(guī)劃相當(dāng)于先給無人艇一份城市地圖艇只需要在地圖基礎(chǔ)上找一條安全路線剩下的動態(tài)障礙交給局部規(guī)劃去處理。用海圖做全局規(guī)劃的另一個好處是語義豐富——它不是一張簡單圖片而是帶屬性的矢量數(shù)據(jù)你可以知道某個區(qū)域是干出灘還是深度足夠的航道這對安全評估至關(guān)重要。1.2 全局規(guī)劃在導(dǎo)航體系中的定位與任務(wù)邊界先明確一個容易混淆的概念全局規(guī)劃和局部避碰不是一回事。在我的項目實踐中導(dǎo)航系統(tǒng)通常分三層任務(wù)層負(fù)責(zé)任務(wù)點拆分全局規(guī)劃層基于靜態(tài)海圖生成宏觀航線局部避碰層用雷達、AIS等實時數(shù)據(jù)做短時避讓繞開臨時出現(xiàn)的漁船、浮標(biāo)和移動船只。全局規(guī)劃管的是分鐘級到小時級的路徑局部避碰管的是秒級的機動。所以這個項目的任務(wù)邊界很清晰輸入是電子海圖、起點、終點、艇的吃水深度和安全參數(shù)輸出是一串結(jié)構(gòu)化的航路點序列每個航路點包含經(jīng)緯度、航向建議、航段距離等屬性。全局規(guī)劃不需要去管前面100米有條漁船怎么辦但必須保證這條航線上的每一個點水深足夠、無障礙物、不在禁航區(qū)里并且從船舶操縱的角度能走通。這個邊界一旦定了后面所有算法選型和數(shù)據(jù)處理的思路都會變得清爽。2. 電子海圖數(shù)據(jù)解析S-57里到底藏了什么2.1 為什么必須是S-57矢量海圖而不是光柵圖市面上常見的電子海圖分兩大類光柵海圖和矢量海圖。光柵圖本質(zhì)上是掃描后的圖片雖然人眼看得清楚但計算機沒法直接從里面提取水深、物標(biāo)類別這些結(jié)構(gòu)化信息。你沒法問一張圖片這里水深多少除非上OCR或者圖像識別這在工程上又慢又不穩(wěn)定。S-57是國際海道測量組織IHO發(fā)布的矢量海圖數(shù)據(jù)交換標(biāo)準(zhǔn)所有官方海道測量機構(gòu)發(fā)布的ENCElectronic Navigational Chart電子導(dǎo)航海圖數(shù)據(jù)都遵循這個標(biāo)準(zhǔn)。S-57把現(xiàn)實世界中的航標(biāo)、沉船、航道、水深等都抽象為帶屬性、帶幾何形狀的物標(biāo)計算機可以直接查詢、計算和分析。這個項目我正是用S-57矢量數(shù)據(jù)作為數(shù)據(jù)源它天然適合做全局路徑規(guī)劃——因為你需要的環(huán)境要素全部是現(xiàn)成的結(jié)構(gòu)化信息。2.2 看懂S-57的物標(biāo)模型從現(xiàn)實地理到數(shù)據(jù)編碼S-57數(shù)據(jù)模型的核心是物標(biāo)Feature。一個物標(biāo)包含兩部分屬性描述這是什么、有什么特征幾何描述它在哪里、長什么樣。舉個例子一個港口水域的水深區(qū)域DEPARE物標(biāo)屬性里有一個深度范圍DRVAL1最淺水深、DRVAL2最深水深幾何上是一塊多邊形。再比如沉船WRECKS物標(biāo)幾何可能是一個點或者一個面屬性里會標(biāo)出沉船類型、是否危險。物標(biāo)按照幾何類型分成點物標(biāo)、線物標(biāo)和面物標(biāo)讀取數(shù)據(jù)的時候要用不同的方式處理。這里有個新手容易忽略的點S-57不是把物體坐標(biāo)直接存在物標(biāo)里而是通過空間物標(biāo)Spatial Object間接引用也就是物標(biāo)通過指針指向空間物標(biāo)空間物標(biāo)才包含坐標(biāo)序列。代碼里處理這種拓?fù)潢P(guān)系時需要格外小心尤其要處理空間物標(biāo)被多個物標(biāo)共享、坐標(biāo)鏈斷裂等情況。我自己第一次解析S-57時就被這個繞暈過后來干脆封裝了一層數(shù)據(jù)訪問接口統(tǒng)一做好拓?fù)渲貥?gòu)。2.3 構(gòu)建路徑規(guī)劃可用的物標(biāo)分類體系拿到海圖數(shù)據(jù)后不能一股腦全用必須先做合理的物標(biāo)分類。海圖物標(biāo)有上百種但真正影響水面無人艇路徑規(guī)劃的就那么幾類。按我的經(jīng)驗可以劃分成四個大類用表格列出來類別代表物標(biāo)路徑規(guī)劃語義陸地區(qū)域LNDARE陸地、DEPARE水深為負(fù)的區(qū)域不可通行硬障礙礙航物WRECKS沉船、OBSTRN障礙物、UWTROC暗礁不可通行需安全距離避讓水下地形DEPCNT等深線、DEPARE水深區(qū)域、SOUNDG水深點決定可通行性需結(jié)合吃水判斷航行限制區(qū)航道邊界、錨地、禁航區(qū)、港區(qū)范圍約束通行規(guī)則優(yōu)先/禁止通行劃分好類別后我還會做一層語義抽象把物標(biāo)屬性映射成規(guī)劃用的統(tǒng)一字段比如是否硬障礙最小安全水深禁止通行標(biāo)志建議通行方向等。這層抽象的價值在于后續(xù)做柵格化或搜索時不需要反復(fù)去查S-57原始屬性表直接在統(tǒng)一模型上計算即可。如果項目中需要支持更多數(shù)據(jù)源比如本地實測水深插值網(wǎng)格也只要轉(zhuǎn)換成這一套統(tǒng)一字段就行。3. 全局路徑規(guī)劃算法選型與實現(xiàn)要點3.1 從全局規(guī)劃算法譜系中做取舍路徑規(guī)劃算法很多但在水面無人艇場景里常用的全局規(guī)劃算法我梳理下來主要就這幾個方向基于圖搜索的Dijkstra、A*基于采樣的RRT系列以及基于智能優(yōu)化的遺傳算法、蟻群算法。每個都有自己的脾氣我整理了一個對比表格方便直觀感受差異。算法完備性最優(yōu)性計算開銷適用場景Dijkstra完備最短路徑高小地圖、需要嚴(yán)格最優(yōu)解A*完備有啟發(fā)函數(shù)時最優(yōu)中中等大小柵格地圖最常用RRT/RRT*概率完備漸進最優(yōu)低高維空間、快速粗糙路徑遺傳算法不保證近似最優(yōu)高多目標(biāo)、航路點優(yōu)化在這個項目里我最終選了A作為主搜索算法原因有三。第一海圖柵格化后的地圖規(guī)模通常在千萬像素量級以內(nèi)A在合理的數(shù)據(jù)結(jié)構(gòu)下能在數(shù)百毫秒到數(shù)秒內(nèi)完成搜索實時性完全夠。第二A在柵格地圖上的完備性和最優(yōu)性都有數(shù)學(xué)保證這一點對安全敏感的無人艇場景很重要——我不希望某次規(guī)劃因為算法本身的隨機性給出一個漏掉障礙物的路徑。第三A的邏輯直觀后續(xù)加工程約束、代價函數(shù)調(diào)優(yōu)都好改。RRT不是不能用但它的路徑通常比較糙轉(zhuǎn)折多、不平滑而且在窄航道場景下采樣效率低后處理成本高。遺傳算法這種隨機優(yōu)化方法更適合在已有航路點序列上做多目標(biāo)優(yōu)化比如同時權(quán)衡路程、油耗、風(fēng)險直接拿來搜全局路徑反而效率低。這些算法不是誰替代誰的關(guān)系而是放在不同層級配合使用。3.2 A*搜索的工程實現(xiàn)狀態(tài)空間、鄰域和啟發(fā)函數(shù)A*算法雖然是經(jīng)典算法但工程實現(xiàn)里有很多細節(jié)決定成敗。首先是狀態(tài)空間的定義。我做柵格化時把海圖切成了均勻網(wǎng)格每個網(wǎng)格是一個狀態(tài)節(jié)點網(wǎng)格的值記錄了該位置是否可通行、最小安全水深、物標(biāo)語義等信息。鄰居擴展我采用的是8鄰域也就是當(dāng)前格子周圍8個方向的格子都可以進入。8鄰域的好處是允許斜向航行更符合船舶實際航向的連續(xù)性但代價函數(shù)里必須區(qū)分直行和對角移動的成本差異。按照我項目的做法直行成本設(shè)為1.0對角移動成本設(shè)為√2約1.414這樣搜索出來的路徑不會因為鄰域定義產(chǎn)生畸變。啟發(fā)函數(shù)的選擇直接關(guān)系到搜索效率。我采用的啟發(fā)函數(shù)是歐氏距離實際是大圓距離的平面近似即當(dāng)前節(jié)點到目標(biāo)節(jié)點的直線距離。這個啟發(fā)函數(shù)是可采納的——它永遠不大于實際從當(dāng)前節(jié)點到終點的真實最短距離因此A*保證能找到最優(yōu)路徑。我試過曼哈頓距離做啟發(fā)函數(shù)在允許斜向移動的8鄰域地圖上會明顯高估距離導(dǎo)致搜索擴展的節(jié)點變多、路徑質(zhì)量下降。關(guān)于這一點建議做水面無人艇規(guī)劃時直接采用歐氏距離匹配8鄰域模型效果最自然。3.3 代價函數(shù)里怎么融合海圖語義安全、吃水與航行規(guī)則A*的搜索效率再高如果代價函數(shù)設(shè)計不合理找出來的也只是一條幾何上最短但航海上不靠譜的路。在水面無人艇場景里代價函數(shù)的設(shè)計必須融合海圖語義。我把代價函數(shù)拆成幾個部分來設(shè)計硬約束速度設(shè)置為無窮大代價直接排除不可通行區(qū)域。包括陸地、干出灘水深為負(fù)值的區(qū)域、沉船和暗礁外側(cè)緩沖區(qū)、禁航區(qū)、已標(biāo)識的軍事演習(xí)區(qū)如果有該物標(biāo)等。硬約束是安全底線哪怕路徑繞遠也必須避開。水深可通行判斷這部分是整個海圖路徑規(guī)劃最核心的工程邏輯。判斷一個柵格能不能走不是簡單看有沒有標(biāo)沉船而是要結(jié)合無人艇自身吃水信息。我設(shè)定的公式是安全水深 靜態(tài)吃水 富余水深(通常取0.5~1米) 浪高影響量(有海況預(yù)報時加上)當(dāng)柵格的水深值小于這個安全水深時該柵格標(biāo)記為不可通行。這里的水深都是基于理論最低潮面的基準(zhǔn)水深實際規(guī)劃時如果需要精確到某個出海時刻還得疊加當(dāng)時的潮高修正這部分后面在問題排查里我會再展開。軟懲罰有些區(qū)域不是不能走而是走了不太好或者是盡量別走。比如漁柵區(qū)、推薦航道外的淺水區(qū)、錨地邊緣、生態(tài)保護區(qū)等。我把這些區(qū)域的額外代價值設(shè)置為基礎(chǔ)通行代價的3~8倍A*搜索時會自動避開它們但實在沒有別的路時也會選擇穿過這樣規(guī)劃結(jié)果更靈活不會因為某個軟約束把所有可走的路都堵死。這樣設(shè)計出來的代價函數(shù)搜索出來的路徑不僅滿足幾何可達還能自動貼合深水大路規(guī)避風(fēng)險區(qū)域。我曾見過有人直接把水深小于3米就標(biāo)成不可通行結(jié)果在淺水港口里規(guī)劃出一條貼著岸邊礁石走的自殺航線就是因為沒把艇的吃水和安全余量做參數(shù)化建模。4. 實操完整搭建一個海圖數(shù)據(jù)驅(qū)動的全局規(guī)劃流程4.1 環(huán)境準(zhǔn)備與數(shù)據(jù)接入這一節(jié)我完整寫一下我實際項目的搭建過程每一步都可以直接參考。先說環(huán)境我用的主力語言是Python主要依賴庫是GDAL/OGR讀取S-57數(shù)據(jù)、NumPy柵格矩陣計算、Pyproj坐標(biāo)投影轉(zhuǎn)換、heapqA*優(yōu)先隊列可視化用的是Matplotlib和QGIS做輔助驗證。數(shù)據(jù)源方面我建議優(yōu)先接入官方或授權(quán)機構(gòu)發(fā)布的S-57格式ENC數(shù)據(jù)。不同的海道測量機構(gòu)會發(fā)布不同區(qū)域的ENC數(shù)據(jù)包拿到以后注意數(shù)據(jù)版本。S-57有3.1版和4.0版也就是S-101前的過渡版物標(biāo)屬性名會有些微不同代碼里要做兼容處理。對于做算法驗證的場景也可以用開放街區(qū)和公開航道數(shù)據(jù)做替代但工程落地一定要用合法授權(quán)的ENC數(shù)據(jù)。實測下來GDAL的OGR模塊讀取S-57非常方便。from osgeo import ogr ds ogr.Open(enc_folder/, 0) # 0表示只讀S-57目錄級打開 layer ds.GetLayerByName(DEPARE) # 水深區(qū)域物標(biāo)層 print(水深區(qū)域物標(biāo)數(shù)量:, layer.GetFeatureCount()) feature layer.GetNextFeature() while feature: geom feature.GetGeometryRef() drval1 feature.GetField(DRVAL1) drval2 feature.GetField(DRVAL2) # 處理每個面的幾何、水深屬性 ... feature layer.GetNextFeature()這里有一個關(guān)鍵點OGR打開S-57時數(shù)據(jù)源路徑指向的是包含海圖單元格擴展名.000等的目錄不是直接指向某個文件。不同版本的GDAL對S-57的支持程度也有差異我建議使用2.4以上版本物標(biāo)識別和拓?fù)錁?gòu)建會更穩(wěn)定。4.2 經(jīng)緯度到平面坐標(biāo)投影與柵格化S-57的原始坐標(biāo)是經(jīng)緯度經(jīng)緯度WGS84做柵格搜索之前必須轉(zhuǎn)成平面坐標(biāo)不然距離計算和柵格劃分在緯度高一點的地方會產(chǎn)生不可接受的畸變。我的做法是根據(jù)任務(wù)區(qū)域所在的經(jīng)度帶選取對應(yīng)的UTM投影帶作為工作坐標(biāo)系。例如在中國沿海某區(qū)域可能用UTM 51N帶中央經(jīng)線在123°E附近投影后的單位是米。投影轉(zhuǎn)換用Pyproj庫很輕松from pyproj import Transformer transformer Transformer.from_crs(EPSG:4326, EPSG:32651, always_xyTrue) x, y transformer.transform(lon, lat)投影做好后確定柵格地圖的邊界和分辨率。柵格分辨率的選擇是個權(quán)衡分辨率太粗窄航道和小障礙物會被糊掉規(guī)劃容易出現(xiàn)穿島或擦碰危險分辨率太細柵格矩陣巨大A*搜索時間和內(nèi)存都會急劇上升。我的經(jīng)驗是對于一艘?guī)酌准壍男⌒蜔o人艇柵格分辨率取5~10米足夠了再結(jié)合船本身的尺寸設(shè)置膨脹量。具體來說柵格尺寸小于等于船寬的一半時路徑保真度會好一些但地圖像素數(shù)會多好幾倍建議先用10米分辨率做快測確認(rèn)航路大體合理后再用5米分辨率精算。柵格化時所有面狀礙航物和陸地區(qū)域在柵格上標(biāo)記為不可通行同時把區(qū)域內(nèi)最淺水深記錄到柵格的值中。這樣A*在判斷一個格子能不能走時直接查柵格的水深值和障礙標(biāo)志高效而且內(nèi)存友好。柵格化后的數(shù)據(jù)我用NumPy矩陣存一個矩陣存障礙標(biāo)志一個矩陣存水深值一個矩陣存語義類別編碼。4.3 關(guān)鍵環(huán)節(jié)安全邊界膨脹與增益處理柵格化完成之后如果直接拿原始障礙物柵格去跑A*出來的路徑會緊貼著障礙物邊緣走。這在陸地上也許還能接受但對無人艇來說非常危險——海圖誤差、水流漂移、定位噪聲都會讓實際船位偏離規(guī)劃線一旦偏離就非常容易發(fā)生碰撞。所以我必須對障礙物做膨脹處理。膨脹半徑的計算不是拍腦袋而是結(jié)合多個因素膨脹半徑 船體半寬 定位誤差(通常取1~2米) 海圖誤差(不同比例尺海圖精度不同取2~5米) 安全余量(至少1米)比如一艘船寬2米的無人艇定位誤差取2米海圖誤差取3米安全余量取1米那么膨脹半徑就是1 2 3 1 7米。在柵格分辨率10米的地圖上約相當(dāng)于對每個障礙物格子向外擴1個格子。這個膨脹過程如果用柵格形態(tài)學(xué)膨脹來做開銷很小如果沒有現(xiàn)成庫也可以對每個障礙物格子周圍半徑內(nèi)的格子做標(biāo)記。但注意膨脹必須只在障礙物可通行邊界方向擴散不能覆蓋起點和終點區(qū)域。膨脹后還應(yīng)該做一次連通性檢查——判斷起點和終點是不是被障礙物隔開了。如果隔開說明在當(dāng)前參數(shù)下根本沒有可行航線需要提示操作員調(diào)整參數(shù)或者確認(rèn)任務(wù)可行性。這個檢查用泛洪填充或者BFS從起點開始擴展一遍就行成本很低但能在算法運行前就避免一次無意義的搜索。4.4 A*搜索與航路點序列生成膨脹完成后進入核心搜索環(huán)節(jié)。我實現(xiàn)的A*用的是Python的heapq作為優(yōu)先隊列每個節(jié)點記錄坐標(biāo)、g值、啟發(fā)值、父節(jié)點指針。搜索的偽代碼邏輯如下import heapq def a_star_search(grid, start, goal): open_heap [] heapq.heappush(open_heap, (0.0, start)) came_from {} g_score {start: 0.0} while open_heap: _, current heapq.heappop(open_heap) if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(grid, current): # 跳過不可通行格子和地圖外 if not is_passable(grid, neighbor): continue tentative_g g_score[current] cost_between(current, neighbor) if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f_score tentative_g heuristic(neighbor, goal) heapq.heappush(open_heap, (f_score, neighbor)) return None # 無可行路徑搜索完成后得到的是一個密集柵格點序列直接拿去用肯定不行因為點數(shù)太多、轉(zhuǎn)折太碎。我會做兩步后處理第一步用路徑壓縮去掉不必要的中間點——從起點開始嘗試盡可能遠的點能否直線通過且整段都在可通行區(qū)域能就跳過中間點第二步用轉(zhuǎn)折點簡化去除共線點只保留必要轉(zhuǎn)向點。經(jīng)過這兩步最終輸出給下游的通常是幾個到十幾個航路點的序列每個航路點包含經(jīng)緯度、預(yù)計航向、安全半徑等屬性作為無人艇航跡跟蹤層的參考航線。4.5 路徑平滑與操縱性約束的引入A*和路徑壓縮出來的航路點仍然是折線直接讓無人艇跟蹤折線每個轉(zhuǎn)角處都需要急轉(zhuǎn)對于舵角有限、慣性大的水面艇來說很難受。因此我在后處理里加了一步平滑。這里我沒有直接上復(fù)雜的B樣條或Dubins曲線而是先采用了更工程化的做法轉(zhuǎn)彎半徑約束檢查。具體做法是遍歷壓縮后的航路點計算每個轉(zhuǎn)向點的轉(zhuǎn)角大小如果轉(zhuǎn)彎角度超過了無人艇的最大轉(zhuǎn)向能力比如最大轉(zhuǎn)向角30度就在轉(zhuǎn)角處插入過渡弧線點把一次急轉(zhuǎn)彎拆成幾段緩轉(zhuǎn)彎。這個做法的好處是邏輯清晰、不會產(chǎn)生偏離安全區(qū)域的路徑。對于更高級的平滑需求B樣條或者貝塞爾曲線也是好方案但務(wù)必要確保平滑后的曲線仍然在安全區(qū)域內(nèi)——我曾經(jīng)見過有人用B樣條平滑結(jié)果曲線直接穿過小島邊緣就是平滑時沒做碰撞檢測。平滑完成后把航路點序列按等間距內(nèi)插成更密集的航跡點提供給底層航跡跟蹤控制器使用。到這里基于海圖的全局路徑規(guī)劃流程就走通了。5. 常見問題與排查技巧實錄5.1 海圖坐標(biāo)錯位、物標(biāo)丟失怎么辦做海圖數(shù)據(jù)解析時最常見的故障原因大多出在坐標(biāo)基準(zhǔn)和拓?fù)涮幚砩?。坐?biāo)錯位所有人都會遇到一次。S-57原始坐標(biāo)是經(jīng)緯度如果你在柵格化之前忘了做投影轉(zhuǎn)換直接拿經(jīng)緯度當(dāng)平面坐標(biāo)用在低緯度也許看不出來但在北方港口會偏出好幾公里。排查辦法很簡單——把解析出來的物標(biāo)疊加到已知的岸線底圖上如果物標(biāo)和底圖上的陸地輪廓對不上先查投影轉(zhuǎn)換再查坐標(biāo)系基準(zhǔn)尤其注意CGCS2000和WGS84混用的問題。物標(biāo)丟失S-57數(shù)據(jù)里部分面物標(biāo)的空間幾何非常復(fù)雜可能包含島中湖、多邊形帶洞等情況。有些解析庫處理這類帶洞多邊形時會把內(nèi)環(huán)丟到下層數(shù)據(jù)里導(dǎo)致渲染或者柵格化時把島嶼內(nèi)部識別成陸地。這個問題的排查方法是柵格化后隨機挑幾個已知的狹長水道驗證一下通航性再結(jié)合可視化層把物標(biāo)輪廓畫出來比對。我用QGIS疊加驗證過之后就再沒出過這種問題。5.2 路徑穿島、穿淺灘但明明避開了標(biāo)注障礙物我在實際測試中碰到過一個十分隱蔽的坑A*給出的路徑從紙面看完全繞開了沉船、暗礁等點狀障礙物但疊加到海圖上發(fā)現(xiàn)路徑直接橫穿了一塊淺灘區(qū)域。原因是那塊淺灘在水深數(shù)據(jù)集里是沒有物標(biāo)面標(biāo)識的只有一些離散的水深點SOUNDG物標(biāo)而我在物標(biāo)分類時沒有把離散水深點納入柵格水深計算。解決方法很直接把所有水深點數(shù)據(jù)也參與柵格化插值。我的做法是用反距離加權(quán)IDW插值把這些離散水深點加密到柵格矩陣?yán)?。更穩(wěn)妥的方案是在柵格化之前建立水深網(wǎng)格模型比如用線性插值或Kriging把所有離散水深數(shù)據(jù)組織成連續(xù)表面再和面狀水深數(shù)據(jù)融合取保守值取淺的那個。做完這一步之后淺灘區(qū)域在柵格地圖上就有了真實的水深記錄A*結(jié)合吃水判斷后自然會繞開。5.3 A*搜索耗時太長怎么優(yōu)化柵格地圖一大A*的耗時和耗內(nèi)存就會起飛。我遇到過一張上萬乘上萬像素的海圖單次搜索耗時好幾十秒這在任務(wù)規(guī)劃場景下完全不能接受。優(yōu)化思路有四個按投入產(chǎn)出比從高到低列出來優(yōu)先隊列用heapq而不是list最基本的性能漏洞。用list加排序函數(shù)的A*在大地圖下時間會從秒級變成分鐘級。把開放列表換成heapq后時間通常能下降一個量級。雙向A*搜索從起點和終點同時向中間搜搜索空間從指數(shù)級縮小到接近兩棵更小的樹實踐下來時間大約能再降40%。地圖分層/縮采樣預(yù)搜索先用低分辨率柵格跑一次粗糙路徑得到一條走廊然后在走廊區(qū)域的精細柵格上再精確搜索。這個方案特別適合超大面積海圖但工程上稍復(fù)雜我是在確定需要時再啟用?？紤]在柵格化和搜索參數(shù)上做保守將柵格做粗一些如20米而不是10米搜索節(jié)點少4倍速度提升非常明顯代價是路徑精細度下降。適合遠程快速偵察場景最終接近目標(biāo)區(qū)域再切換高分辨率地圖。我最終的方案是heapq加雙向搜索10米分辨率、5000×8000像素的地圖單次搜索可以穩(wěn)定在2秒內(nèi)完成任務(wù)規(guī)劃完全夠用。5.4 吃水判斷不準(zhǔn)導(dǎo)致近岸擦底風(fēng)險吃水判斷這塊我前面提過基準(zhǔn)面的問題這里展開講一下。海圖上的水深標(biāo)注是基于理論最低潮面的真實水深 海圖水深 當(dāng)前潮高。如果你的無人艇吃水0.8米安全余量0.5米安全水深是1.3米海圖上一個區(qū)域標(biāo)成1.5米按靜態(tài)判斷可以走。但如果當(dāng)天低潮時潮高為負(fù)值比理論最低潮面還低實際水深可能只有1.2米就會擦底。處理方式是引入外部潮汐數(shù)據(jù)在規(guī)劃時為安全水深增加一個潮汐修正量。簡單做法是加載該海域的潮汐預(yù)報站數(shù)據(jù)在規(guī)劃時間段內(nèi)查詢預(yù)測潮高把最小預(yù)期潮高疊加進安全水深公式。如果沒有實時潮汐數(shù)據(jù)我還有一個保守做法——安全水深中額外增加0.3~0.5米的潮汐不確定余量雖然會讓路徑略微遠離淺水區(qū)但把擦底風(fēng)險壓到一個極低水平。工程上寧可航線繞遠一點也不能讓艇去賭水深。5.5 物標(biāo)語義與航行規(guī)則的結(jié)合最后一個經(jīng)驗關(guān)于海圖物標(biāo)里那些隱含規(guī)則。有些區(qū)域不是物理障礙而是規(guī)則障礙。比如航道FAIRWY有中心線和邊界船舶應(yīng)該沿著航道方向走而不是橫穿有些區(qū)域是海底管道區(qū)PIPELINE雖然水深夠、表面沒有任何障礙物但拋錨會砸壞管道應(yīng)該盡量避開。全局規(guī)劃如果不把這些語義加進去出來的路徑雖然物理上是通的但航海上可能違規(guī)或者有隱患。我的做法是在代價函數(shù)中加入航行規(guī)則碼字段按照海圖物標(biāo)類別預(yù)置規(guī)則等級。比如航道邊界內(nèi)的代價基礎(chǔ)值較低航道外的近岸區(qū)代價高一點管道區(qū)設(shè)為極高代價軍事禁區(qū)和生態(tài)保護區(qū)直接設(shè)為硬約束。這樣規(guī)劃出來的路徑就會自然地順著航道走、遠離管道區(qū)、貼著深水區(qū)整體看起來就像一條有經(jīng)驗的船長會走的航線而不是純粹幾何搜索的結(jié)果。這個語義層是區(qū)分能跑和敢用的關(guān)鍵也是我們從原理樣機走向?qū)嶋H作業(yè)時收獲最大的一塊。我在實際項目中體會最深的一點是海圖數(shù)據(jù)是參考不是絕對真實。海圖的測量時間、測區(qū)覆蓋、比例尺精度都會有局限水面環(huán)境還會因疏浚、淤積而持續(xù)變化。因此全局路徑規(guī)劃只是第一步規(guī)劃出來的航線在下發(fā)執(zhí)行前最好再結(jié)合當(dāng)時當(dāng)?shù)氐乃臍庀髼l件、最新的航行通告和AIS船舶動態(tài)做一次人工復(fù)核。這也提醒我們?nèi)魏嗡惴ǖ妮敵龆疾荒芴娲踩哂嘣跓o人艇真正大規(guī)模投入運營之前人類在環(huán)上的監(jiān)督驗證依舊是必不可少的安全閥。最后分享一個實用小習(xí)慣每次規(guī)劃結(jié)束后把輸入海圖、障礙物圖層、規(guī)劃路徑、膨脹區(qū)域、安全參數(shù)一起導(dǎo)出成一張疊加圖存檔既方便審計也是調(diào)試新參數(shù)時最快捷的前后對照工具。