1. 機(jī)器人操作系統(tǒng)核心架構(gòu)與SLAM導(dǎo)航的全局拆解機(jī)器人操作系統(tǒng)這個詞剛?cè)胄械呐笥崖牭饺菀装l(fā)怵覺得是某種高深莫測的黑科技。其實把它理解成一個“機(jī)器人專屬的安卓系統(tǒng)”就通了——它不直接控制電機(jī)轉(zhuǎn)動也不直接處理激光雷達(dá)的點(diǎn)云數(shù)據(jù)它干的事情是讓各個功能模塊能互相說上話并且按照統(tǒng)一的節(jié)奏協(xié)同工作。一個典型的移動機(jī)器人身上掛著激光雷達(dá)、深度相機(jī)、慣性測量單元、輪式編碼器、電機(jī)驅(qū)動板這些硬件各自跑各自的如果沒有一套通信中間件把它們串起來那就是一堆昂貴的電子垃圾。機(jī)器人操作系統(tǒng)解決的正是這個“串起來”的問題而SLAM導(dǎo)航則是讓機(jī)器人在這個框架下真正“活起來”的核心能力。我接觸過不少從零搭建移動機(jī)器人平臺的項目發(fā)現(xiàn)一個很普遍的現(xiàn)象很多人把大量精力花在調(diào)參和跑通demo上卻對底層的數(shù)據(jù)流走向和坐標(biāo)變換邏輯一知半解。結(jié)果就是demo跑得挺歡一旦換了個場地或者傳感器型號變了整個系統(tǒng)就崩了。所以這篇內(nèi)容我打算換個講法不按常規(guī)的“安裝-配置-運(yùn)行”三步走而是從架構(gòu)設(shè)計的角度切入把SLAM導(dǎo)航這件事拆開了揉碎了講清楚。不管你是剛買了一套開發(fā)套件準(zhǔn)備入門的學(xué)生還是公司里被派來做移動機(jī)器人原型的工程師只要跟著這個思路走一遍至少能少踩一半的坑。1.1 為什么選擇“通信中間件算法節(jié)點(diǎn)”的分層架構(gòu)機(jī)器人系統(tǒng)最忌諱的就是把所有邏輯寫在一個大循環(huán)里。我見過一個早期的項目主控程序里一個while循環(huán)先讀傳感器再算里程計然后跑一遍掃描匹配最后輸出速度指令整個流程串行執(zhí)行。這種寫法在傳感器少、算法簡單的時候勉強(qiáng)能跑但一旦加入新的傳感器或者想換一種定位算法代碼就變成了意大利面條改一處崩三處。分層架構(gòu)的核心價值在于解耦通信層只管數(shù)據(jù)怎么傳算法層只管數(shù)據(jù)怎么算硬件驅(qū)動層只管怎么跟板子打交道。這三層之間通過標(biāo)準(zhǔn)化的消息接口通信任何一層的改動都不會波及其他層。具體到SLAM導(dǎo)航這個場景分層帶來的好處更加明顯。激光雷達(dá)的驅(qū)動節(jié)點(diǎn)只負(fù)責(zé)把原始掃描數(shù)據(jù)打包成標(biāo)準(zhǔn)格式發(fā)出來它不關(guān)心這個數(shù)據(jù)是給建圖用還是給定位用。建圖節(jié)點(diǎn)訂閱掃描數(shù)據(jù)結(jié)合里程計信息構(gòu)建柵格地圖建完之后把地圖存下來。定位節(jié)點(diǎn)則訂閱掃描數(shù)據(jù)和已有地圖實時輸出機(jī)器人在地圖中的位姿。路徑規(guī)劃節(jié)點(diǎn)拿到位姿和目標(biāo)點(diǎn)算出一條可行路徑再把速度指令發(fā)給底盤驅(qū)動。整個數(shù)據(jù)流是單向的、清晰的每個節(jié)點(diǎn)只做一件事。這種設(shè)計還有一個隱藏優(yōu)勢調(diào)試的時候可以單獨(dú)把某個節(jié)點(diǎn)的數(shù)據(jù)錄下來離線反復(fù)回放分析不用每次都把真機(jī)跑起來。1.2 SLAM在導(dǎo)航棧中扮演的角色與邊界SLAM這三個字母拆開就是同步定位與建圖聽起來是一件事實際上是兩件互相依賴的事。定位需要地圖作為參照建圖又需要知道機(jī)器人當(dāng)前在哪這就成了一個雞生蛋蛋生雞的問題。早期的解決方案要么假設(shè)機(jī)器人位姿已知比如用高精度外部追蹤系統(tǒng)要么假設(shè)環(huán)境特征已知比如在墻上貼反光板但這些限制在實際場景中往往不成立。SLAM的價值就在于打破了這個循環(huán)依賴讓機(jī)器人在完全未知的環(huán)境中一邊走一邊構(gòu)建地圖同時用正在構(gòu)建的地圖來修正自己的位置估計。但這里有一個容易被忽視的邊界問題SLAM輸出的地圖和位姿精度是有限的而且會隨著時間漂移。很多新手會拿SLAM建出來的地圖直接去做高精度測量或者指望它在長走廊這種退化場景下也能保持厘米級定位這就不現(xiàn)實了。SLAM的本質(zhì)是一個概率估計過程它給出的是“最可能的位姿”和“最可能的地圖”而不是絕對真值。理解這一點(diǎn)才能在后續(xù)的導(dǎo)航調(diào)參中做出合理的取舍。比如在長直走廊里激光掃描匹配在垂直于走廊方向上的約束很弱位姿估計會沿著走廊方向漂移這時候就需要引入慣性測量單元或者輪式里程計來提供額外的約束。2. 核心算法模塊的深度解析與實操要點(diǎn)把架構(gòu)理清楚之后接下來要啃的是每個核心模塊的具體實現(xiàn)。這部分我會按照數(shù)據(jù)流動的順序從傳感器數(shù)據(jù)預(yù)處理開始一路講到路徑規(guī)劃的輸出把每個環(huán)節(jié)的關(guān)鍵算法、參數(shù)含義和實操中容易翻車的地方都過一遍。需要提前說明的是下面提到的具體參數(shù)值都是基于常見差速輪式底盤和二維激光雷達(dá)的配置如果你的平臺是阿克曼轉(zhuǎn)向或者三維激光雷達(dá)數(shù)值需要相應(yīng)調(diào)整但背后的邏輯是通用的。2.1 傳感器數(shù)據(jù)預(yù)處理與時間同步激光雷達(dá)和慣性測量單元的數(shù)據(jù)到達(dá)時間往往是不對齊的激光雷達(dá)可能每100毫秒出一幀慣性測量單元每5毫秒出一組數(shù)據(jù)而且兩者的時間戳基準(zhǔn)可能還不一樣。如果不做時間同步就直接把數(shù)據(jù)扔給SLAM算法建出來的地圖會出現(xiàn)明顯的重影和扭曲。我習(xí)慣的做法是在驅(qū)動層就把所有傳感器的時間戳統(tǒng)一到系統(tǒng)時鐘然后在SLAM節(jié)點(diǎn)里用一個時間窗口做近似同步。具體來說對于每一幀激光數(shù)據(jù)找到時間戳最接近的慣性測量單元數(shù)據(jù)如果時間差超過閾值通常設(shè)20到30毫秒就丟棄這組數(shù)據(jù)或者做插值補(bǔ)償。輪式里程計的處理也有講究。很多底盤驅(qū)動板輸出的里程計是累積脈沖數(shù)需要根據(jù)輪徑和編碼器分辨率換算成米和弧度。這里有一個容易忽略的細(xì)節(jié)輪距的測量誤差會直接導(dǎo)致角度估計的累積誤差。我實測過輪距標(biāo)定偏差5毫米機(jī)器人原地轉(zhuǎn)十圈之后角度誤差能到十幾度。標(biāo)定的方法很簡單讓機(jī)器人原地旋轉(zhuǎn)整整十圈對比里程計輸出的角度變化和實際角度變化按比例修正輪距參數(shù)。這個步驟花十分鐘做一次后面能省掉很多調(diào)試時間。注意慣性測量單元的零偏和尺度因子也需要標(biāo)定尤其是低成本的消費(fèi)級慣性測量單元零偏隨溫度變化很明顯。如果SLAM過程中發(fā)現(xiàn)機(jī)器人靜止時地圖還在緩慢漂移大概率是慣性測量單元的零偏沒有補(bǔ)償。2.2 掃描匹配與位姿圖優(yōu)化的參數(shù)調(diào)優(yōu)掃描匹配是二維SLAM的核心它的任務(wù)是找到當(dāng)前激光幀相對于已有地圖或上一幀的最佳位姿變換。常見的算法有基于相關(guān)性的暴力搜索和基于梯度的迭代最近點(diǎn)方法。暴力搜索對初值不敏感但計算量大迭代最近點(diǎn)速度快但容易陷入局部最優(yōu)。實際項目中我通常采用混合策略先用里程計提供的初值做一次迭代最近點(diǎn)匹配如果匹配得分低于閾值再退回到暴力搜索在小范圍內(nèi)重新找。參數(shù)調(diào)優(yōu)方面有幾個關(guān)鍵值需要重點(diǎn)關(guān)注。搜索窗口的大小決定了算法能容忍多大的里程計漂移設(shè)得太小會導(dǎo)致快速轉(zhuǎn)彎時匹配失敗設(shè)得太大則計算量飆升且容易匹配到錯誤的位置。我的經(jīng)驗值是對于室內(nèi)環(huán)境搜索窗口設(shè)為0.3米和0.3弧度左右比較均衡。匹配得分閾值需要根據(jù)激光雷達(dá)的噪聲水平來定噪聲大的雷達(dá)閾值要相應(yīng)放寬否則會頻繁觸發(fā)重定位。還有一個容易被忽視的參數(shù)是地圖更新頻率更新太快會把動態(tài)障礙物比如走動的人刻進(jìn)地圖里更新太慢則無法及時反映環(huán)境變化。通常設(shè)為1到2赫茲比較合適。位姿圖優(yōu)化是在掃描匹配的基礎(chǔ)上做全局一致性調(diào)整。當(dāng)機(jī)器人繞了一圈回到起點(diǎn)時如果直接拼接地圖起點(diǎn)和終點(diǎn)往往對不上這就是累積誤差。位姿圖優(yōu)化通過檢測回環(huán)識別出當(dāng)前場景之前來過在回環(huán)約束下重新分配誤差讓地圖閉合?；丨h(huán)檢測的準(zhǔn)確性至關(guān)重要誤檢一個回環(huán)會把整張地圖拉變形。我通常會在回環(huán)檢測后面加一道幾何驗證把當(dāng)前幀和候選回環(huán)幀做一次精細(xì)匹配如果匹配得分不夠高就拒絕這個回環(huán)。2.3 代價地圖構(gòu)建與路徑規(guī)劃算法選型代價地圖是導(dǎo)航棧里承上啟下的模塊它把SLAM輸出的柵格地圖轉(zhuǎn)換成規(guī)劃算法能用的形式。原始柵格地圖只有占用、空閑和未知三種狀態(tài)代價地圖在此基礎(chǔ)上疊加了膨脹層把障礙物周圍一定范圍內(nèi)的格子賦予較高的代價值這樣規(guī)劃出來的路徑會自然遠(yuǎn)離障礙物。膨脹半徑的設(shè)置需要匹配機(jī)器人的實際輪廓設(shè)得太小機(jī)器人會貼著墻走容易刮蹭設(shè)得太大則狹窄通道可能被完全堵死。我的做法是先按機(jī)器人外接圓半徑設(shè)置然后在實際測試中根據(jù)通過性微調(diào)。路徑規(guī)劃分全局和局部兩層。全局規(guī)劃負(fù)責(zé)從當(dāng)前位置到目標(biāo)點(diǎn)找一條粗略的路徑常用的算法有A星和迪杰斯特拉前者啟發(fā)式搜索效率更高后者在復(fù)雜環(huán)境下更穩(wěn)定。局部規(guī)劃負(fù)責(zé)在全局路徑的引導(dǎo)下結(jié)合實時障礙物信息輸出具體的速度指令常用的有動態(tài)窗口法和時間彈性帶。動態(tài)窗口法計算量小、響應(yīng)快但在狹窄空間容易震蕩時間彈性帶生成的軌跡更平滑但計算量較大。對于計算資源有限的嵌入式平臺我通常先用動態(tài)窗口法跑通如果發(fā)現(xiàn)震蕩嚴(yán)重再考慮換時間彈性帶。規(guī)劃算法計算量路徑平滑度狹窄通道通過性適用場景動態(tài)窗口法低一般容易震蕩開闊室內(nèi)環(huán)境時間彈性帶中高好較好復(fù)雜動態(tài)環(huán)境模型預(yù)測控制高很好好計算資源充足3. 從零搭建SLAM導(dǎo)航系統(tǒng)的完整實操流程理論講得再多不動手都是空的。這一部分我按照實際項目的推進(jìn)順序把從環(huán)境準(zhǔn)備到最終跑通導(dǎo)航的每一步都列出來包括具體的命令、配置文件的關(guān)鍵字段和調(diào)試方法。需要說明的是下面的流程假設(shè)你用的是常見的差速輪式底盤和二維激光雷達(dá)其他類型的平臺在標(biāo)定和參數(shù)上會有差異但整體框架是一致的。3.1 開發(fā)環(huán)境搭建與依賴管理第一步是確定運(yùn)行環(huán)境。機(jī)器人操作系統(tǒng)有多個發(fā)行版本每個版本對應(yīng)的系統(tǒng)依賴和軟件包版本都不同。我的建議是選擇長期支持版本社區(qū)維護(hù)周期長遇到問題更容易找到解決方案。安裝方式上如果只是做算法驗證用軟件源直接安裝最省事如果要做產(chǎn)品化部署建議從源碼編譯這樣可以針對特定硬件做優(yōu)化也方便裁剪不需要的模塊。依賴管理是新手最容易翻車的地方。機(jī)器人操作系統(tǒng)生態(tài)里的包依賴關(guān)系復(fù)雜不同包對同一個底層庫的版本要求可能沖突。我習(xí)慣用工作空間隔離的方式每個項目建一個獨(dú)立的工作空間用編譯工具管理依賴。編譯的時候注意并行度設(shè)置核心數(shù)少的老機(jī)器設(shè)太高反而會因為內(nèi)存不足而卡死。還有一個細(xì)節(jié)如果同時安裝了多個版本的Python編譯腳本可能會鏈接到錯誤的版本導(dǎo)致運(yùn)行時找不到模塊。解決辦法是在編譯前顯式指定Python路徑。# 創(chuàng)建工作空間 mkdir -p ~/robot_ws/src cd ~/robot_ws # 初始化工作空間具體命令取決于使用的構(gòu)建工具 # 編譯時指定并行任務(wù)數(shù)根據(jù)CPU核心數(shù)調(diào)整 # 如果內(nèi)存不足減少并行數(shù)提示編譯過程中如果遇到某個包反復(fù)失敗先單獨(dú)編譯這個包并查看完整錯誤輸出不要盲目重試整個工作空間。很多時候錯誤信息里已經(jīng)寫明了缺失的依賴或版本沖突。3.2 底盤與傳感器標(biāo)定實操底盤標(biāo)定是整個系統(tǒng)精度的基石。需要標(biāo)定的參數(shù)包括輪徑、輪距、編碼器分辨率和最大速度限制。輪徑的標(biāo)定方法是讓機(jī)器人直線行駛一段已知距離比如5米對比里程計輸出的距離和實際距離按比例修正輪徑參數(shù)。輪距的標(biāo)定前面提過原地旋轉(zhuǎn)十圈對比角度。這兩個參數(shù)標(biāo)定好之后里程計的精度能提升一個數(shù)量級。激光雷達(dá)的標(biāo)定主要是外參標(biāo)定也就是雷達(dá)安裝位置相對于機(jī)器人中心的平移和旋轉(zhuǎn)。對于二維雷達(dá)平移量影響不大但旋轉(zhuǎn)量尤其是偏航角對建圖影響很大。如果雷達(dá)安裝時沒有對準(zhǔn)正前方建出來的地圖會整體旋轉(zhuǎn)一個角度。標(biāo)定方法很簡單讓機(jī)器人正對一面平整的墻壁在雷達(dá)數(shù)據(jù)中找到墻壁對應(yīng)的直線計算這條直線與機(jī)器人正前方的夾角然后在配置文件中補(bǔ)償這個角度。慣性測量單元的標(biāo)定包括零偏和噪聲參數(shù)。零偏標(biāo)定就是讓機(jī)器人靜止放置一段時間采集數(shù)據(jù)取平均值。噪聲參數(shù)可以通過艾倫方差分析來估計不過對于大多數(shù)室內(nèi)導(dǎo)航場景用默認(rèn)參數(shù)也能跑只是精度稍差。如果發(fā)現(xiàn)機(jī)器人靜止時位姿估計還在緩慢漂移優(yōu)先檢查慣性測量單元的零偏。3.3 建圖與定位的完整操作流程建圖階段的目標(biāo)是構(gòu)建一張干凈、準(zhǔn)確的柵格地圖。操作流程是啟動底盤驅(qū)動和雷達(dá)驅(qū)動確認(rèn)數(shù)據(jù)正常發(fā)布啟動SLAM建圖節(jié)點(diǎn)在可視化工具中觀察地圖構(gòu)建過程手動遙控機(jī)器人遍歷整個環(huán)境注意控制速度不要太快轉(zhuǎn)彎時放慢遍歷完成后保存地圖。這里有幾個實操細(xì)節(jié)值得注意遍歷時盡量走閉合回路這樣回環(huán)檢測才能發(fā)揮作用避免在人多的時候建圖動態(tài)物體會被刻進(jìn)地圖如果發(fā)現(xiàn)地圖某處明顯扭曲不要勉強(qiáng)保存退回去重新走一遍那一段。定位階段是在已有地圖的基礎(chǔ)上實時估計機(jī)器人位姿。啟動定位節(jié)點(diǎn)后需要給機(jī)器人一個初始位姿估計通常是在可視化工具中用鼠標(biāo)拖拽一個箭頭指向機(jī)器人在地圖中的大概位置和朝向。初始位姿越準(zhǔn)定位收斂越快。如果初始位姿給得太離譜定位可能收斂到錯誤的位置。定位過程中可以通過觀察粒子云的分布來判斷定位質(zhì)量粒子云越集中說明定位越可信粒子云發(fā)散則說明定位不確定。階段關(guān)鍵操作常見問題排查方法建圖遙控遍歷環(huán)境地圖重影檢查時間同步和標(biāo)定建圖保存地圖地圖扭曲檢查回環(huán)檢測是否觸發(fā)定位設(shè)置初始位姿定位不收斂重新給更準(zhǔn)的初始位姿定位實時監(jiān)控粒子云發(fā)散檢查環(huán)境是否退化3.4 導(dǎo)航參數(shù)配置與實車調(diào)試導(dǎo)航參數(shù)配置是最后一步也是最考驗?zāi)托牡囊徊?。全局?guī)劃器的參數(shù)相對固定主要調(diào)整規(guī)劃頻率和路徑平滑權(quán)重。局部規(guī)劃器的參數(shù)需要根據(jù)機(jī)器人動力學(xué)特性來調(diào)包括最大速度、最大加速度、最大角速度等。這些參數(shù)設(shè)得太保守機(jī)器人行動遲緩設(shè)得太激進(jìn)則容易震蕩甚至失控。我的調(diào)參順序是先設(shè)一個保守值確保安全然后逐步提高速度上限直到機(jī)器人開始出現(xiàn)輕微震蕩再回調(diào)10%作為最終值。實車調(diào)試時建議先用小范圍測試比如讓機(jī)器人從一個房間走到另一個房間觀察它在直道、彎道和狹窄通道的表現(xiàn)。如果發(fā)現(xiàn)機(jī)器人在某個位置反復(fù)震蕩先檢查代價地圖的膨脹層是否把通道堵死了再檢查局部規(guī)劃器的軌跡評分權(quán)重是否合理。如果機(jī)器人貼著障礙物走說明膨脹半徑設(shè)小了。如果機(jī)器人走到一半停下來不動可能是全局路徑被動態(tài)障礙物擋住了需要檢查局部規(guī)劃器的避障邏輯。注意調(diào)試導(dǎo)航參數(shù)時一定要有人在場隨時準(zhǔn)備急停。我見過不止一次因為參數(shù)設(shè)錯導(dǎo)致機(jī)器人全速撞墻的情況輕則撞壞外殼重則燒毀電機(jī)驅(qū)動。4. 常見故障排查與性能優(yōu)化實戰(zhàn)記錄跑通基本流程只是第一步真正體現(xiàn)功力的是遇到問題能快速定位并解決。這一部分我整理了實際項目中遇到過的典型故障按照現(xiàn)象、可能原因、排查步驟和解決方案的結(jié)構(gòu)來組織方便你遇到類似問題時快速對照。4.1 建圖質(zhì)量問題的排查思路建圖重影是最常見的故障表現(xiàn)為地圖上同一面墻出現(xiàn)兩條或多條平行的線條。根本原因通常是位姿估計不準(zhǔn)導(dǎo)致不同時刻的激光幀沒有對齊。排查時先看時間同步確認(rèn)激光數(shù)據(jù)和里程計數(shù)據(jù)的時間戳是否對齊再看標(biāo)定檢查輪徑和輪距是否準(zhǔn)確最后看掃描匹配參數(shù)搜索窗口是否太小導(dǎo)致匹配失敗。我遇到過一次重影問題排查了半天發(fā)現(xiàn)是慣性測量單元的安裝方向配置錯了導(dǎo)致角速度符號反了機(jī)器人轉(zhuǎn)彎時位姿估計往反方向跑。地圖扭曲通常出現(xiàn)在長走廊或大房間場景表現(xiàn)為直線變彎、直角變銳角。這是累積誤差導(dǎo)致的如果回環(huán)檢測沒有觸發(fā)誤差就無法消除。排查時先確認(rèn)機(jī)器人是否走了閉合回路如果沒有閉合回路扭曲是正常的只能通過提高里程計精度來緩解。如果走了閉合回路但回環(huán)沒觸發(fā)檢查回環(huán)檢測的搜索半徑和匹配閾值是否設(shè)置得太嚴(yán)格。還有一種情況是地圖上出現(xiàn)大量散點(diǎn)噪聲這通常是動態(tài)障礙物或者雷達(dá)噪聲導(dǎo)致的。如果散點(diǎn)集中在某個區(qū)域可能是那個區(qū)域有玻璃或鏡面反射激光雷達(dá)在鏡面上會產(chǎn)生虛假回波。解決辦法是在建圖時避開這些區(qū)域或者在代價地圖中把這些區(qū)域標(biāo)記為不可通行。4.2 定位丟失與重定位策略定位丟失的表現(xiàn)是機(jī)器人在地圖中的位置突然跳變或者粒子云完全發(fā)散。常見原因有三個一是環(huán)境特征太少比如在空曠的走廊或大廳里激光掃描匹配沒有足夠的約束二是動態(tài)障礙物太多比如周圍全是走動的人激光數(shù)據(jù)被遮擋嚴(yán)重三是初始位姿給得太離譜定位算法收斂到了錯誤的位置。應(yīng)對定位丟失我通常采取分層策略。第一層是預(yù)防在建圖時盡量選擇特征豐富的環(huán)境避免在長走廊和空曠區(qū)域建圖。第二層是檢測實時監(jiān)控定位置信度指標(biāo)一旦低于閾值就觸發(fā)重定位流程。第三層是恢復(fù)重定位時先在全局范圍內(nèi)撒粒子然后根據(jù)激光匹配得分逐步收斂。如果全局重定位也失敗最后的辦法是讓機(jī)器人原地旋轉(zhuǎn)一圈用不同角度的激光數(shù)據(jù)來匹配。提示如果機(jī)器人經(jīng)常在同一個位置定位丟失說明那個位置的環(huán)境特征確實不足可以考慮在那里貼一些反光標(biāo)記或者放置一些特征明顯的物體來增強(qiáng)定位穩(wěn)定性。4.3 導(dǎo)航震蕩與路徑規(guī)劃異常處理導(dǎo)航震蕩表現(xiàn)為機(jī)器人在原地左右搖擺或者沿著路徑來回移動。根本原因是局部規(guī)劃器在多個候選軌跡之間猶豫不決每次選擇都偏向不同的方向。解決辦法是調(diào)整軌跡評分權(quán)重增加對路徑一致性的懲罰讓規(guī)劃器傾向于選擇與上一步方向相近的軌跡。另外降低最大角速度也能有效減少震蕩但會犧牲轉(zhuǎn)彎靈活性。路徑規(guī)劃異常還包括規(guī)劃失敗和路徑繞遠(yuǎn)。規(guī)劃失敗通常是目標(biāo)點(diǎn)被障礙物包圍或者代價地圖把目標(biāo)點(diǎn)標(biāo)記為不可通行檢查目標(biāo)點(diǎn)是否在膨脹層內(nèi)部即可。路徑繞遠(yuǎn)可能是全局規(guī)劃器的啟發(fā)式權(quán)重設(shè)得太低導(dǎo)致搜索范圍過大適當(dāng)提高啟發(fā)式權(quán)重可以讓規(guī)劃器更傾向于直奔目標(biāo)。故障現(xiàn)象可能原因排查步驟解決方案建圖重影時間不同步/標(biāo)定不準(zhǔn)檢查時間戳和輪距參數(shù)重新標(biāo)定/調(diào)整同步閾值地圖扭曲累積誤差/回環(huán)未觸發(fā)確認(rèn)是否閉合回路提高里程計精度/放寬回環(huán)閾值定位丟失環(huán)境特征少/動態(tài)干擾查看粒子云分布增強(qiáng)環(huán)境特征/分層重定位導(dǎo)航震蕩軌跡評分不合理觀察候選軌跡變化調(diào)整評分權(quán)重/降低角速度規(guī)劃失敗目標(biāo)點(diǎn)被障礙物包圍檢查代價地圖重新選擇目標(biāo)點(diǎn)/調(diào)整膨脹半徑4.4 計算資源優(yōu)化與實時性保障SLAM導(dǎo)航對計算資源的需求不低尤其是在嵌入式平臺上CPU和內(nèi)存都很緊張。我遇到過因為計算資源不足導(dǎo)致建圖頻率跟不上機(jī)器人移動速度地圖出現(xiàn)大片空白的情況。優(yōu)化思路有幾個方向降低激光雷達(dá)的采樣率比如從每秒40幀降到20幀對建圖精度影響不大但計算量減半減小地圖分辨率從5厘米降到10厘米內(nèi)存占用和計算量都能顯著下降關(guān)閉不必要的可視化模塊可視化工具本身很吃資源調(diào)試完成后應(yīng)該關(guān)掉。還有一個容易被忽視的點(diǎn)是消息隊列的長度。如果某個節(jié)點(diǎn)的處理速度跟不上數(shù)據(jù)發(fā)布速度消息隊列會不斷積壓最終導(dǎo)致內(nèi)存耗盡或者數(shù)據(jù)延遲越來越大。解決辦法是合理設(shè)置隊列長度對于實時性要求高的數(shù)據(jù)比如激光掃描隊列長度設(shè)為1到2即可舊數(shù)據(jù)直接丟棄對于不能丟的數(shù)據(jù)比如地圖更新隊列可以設(shè)長一些但要監(jiān)控積壓情況。5. 系統(tǒng)擴(kuò)展與多場景適配的進(jìn)階思路基本系統(tǒng)跑通之后下一步就是考慮怎么讓它適應(yīng)更多場景。室內(nèi)導(dǎo)航和室外導(dǎo)航的差異很大二維激光雷達(dá)和三維激光雷達(dá)的SLAM方案也完全不同。這一部分我分享一些擴(kuò)展思路包括多傳感器融合、多樓層地圖管理和動態(tài)環(huán)境適應(yīng)都是實際項目中驗證過可行的方向。5.1 多傳感器融合提升定位魯棒性單一傳感器總有局限性激光雷達(dá)在長走廊退化視覺在紋理缺失區(qū)域失效慣性測量單元有累積漂移。把多種傳感器融合起來取長補(bǔ)短是提升定位魯棒性的有效手段。最常見的融合方案是激光雷達(dá)加慣性測量單元激光提供絕對位置約束慣性測量單元提供高頻的姿態(tài)變化兩者通過擴(kuò)展卡爾曼濾波或者因子圖優(yōu)化融合。實測下來融合之后的定位在快速轉(zhuǎn)彎和短暫激光遮擋場景下明顯更穩(wěn)定。視覺和激光的融合是另一個方向。視覺可以提供豐富的紋理信息幫助在激光退化場景下維持定位。但視覺對光照變化敏感晚上或者光線劇烈變化時可能失效。我的做法是把視覺定位作為輔助只在激光定位置信度低的時候啟用并且用激光定位的結(jié)果來約束視覺的尺度漂移。這種松耦合的方式實現(xiàn)簡單效果也夠用。5.2 多樓層與大地圖管理方案單層地圖跑通之后多樓層場景是繞不開的坎。多樓層地圖管理的核心問題是樓層切換時的定位初始化。我的方案是每個樓層維護(hù)一張獨(dú)立的地圖樓層之間通過樓梯或電梯連接。機(jī)器人到達(dá)樓梯口時先保存當(dāng)前樓層的位姿切換到目標(biāo)樓層的地圖然后通過樓梯區(qū)域的激光匹配來初始化目標(biāo)樓層的位姿。如果樓梯區(qū)域特征不足可以在樓梯口貼一些人工標(biāo)記輔助定位。大地圖管理還有一個內(nèi)存問題。一張幾百米見方的地圖如果分辨率是5厘米柵格數(shù)量會非常龐大加載和查詢都很慢。解決辦法是用分層地圖結(jié)構(gòu)全局用低分辨率地圖做粗略定位局部用高分辨率地圖做精細(xì)匹配。這樣既保證了全局一致性又控制了內(nèi)存占用。5.3 動態(tài)環(huán)境下的長期建圖與更新靜態(tài)地圖在動態(tài)環(huán)境里用久了會越來越不準(zhǔn)比如家具挪了位置、墻上掛了新畫、走廊里多了個柜子。長期運(yùn)行的機(jī)器人需要具備地圖更新能力。我的做法是定期比如每周讓機(jī)器人重新遍歷一遍環(huán)境用新數(shù)據(jù)更新地圖。更新時不是簡單覆蓋而是用貝葉斯方法融合新舊數(shù)據(jù)對于多次觀測到變化的區(qū)域才真正修改地圖避免因為一次偶然的遮擋就誤改地圖。動態(tài)障礙物的處理是另一個層面。導(dǎo)航過程中遇到走動的人或移動的推車局部規(guī)劃器需要能夠?qū)崟r避讓。動態(tài)窗口法本身就有一定的動態(tài)避障能力但對于快速橫穿的障礙物反應(yīng)可能不夠及時。我通常會在代價地圖中增加一個動態(tài)層把檢測到的動態(tài)障礙物以較高代價標(biāo)記出來并且預(yù)測其短期運(yùn)動趨勢讓規(guī)劃器提前做出避讓動作。注意動態(tài)障礙物的預(yù)測不要做得太復(fù)雜簡單的勻速模型在大多數(shù)室內(nèi)場景下就夠用了。過于復(fù)雜的預(yù)測模型不僅計算量大而且一旦預(yù)測錯誤反而會導(dǎo)致更糟糕的避讓行為。5.4 從原型到產(chǎn)品的工程化考量原型跑通和產(chǎn)品落地之間還有很大的距離。工程化需要考慮的問題包括系統(tǒng)啟動的自動化不能每次都手動敲一堆命令異常狀態(tài)的監(jiān)控和恢復(fù)比如某個節(jié)點(diǎn)崩潰了要能自動重啟日志系統(tǒng)的完善出問題的時候能回溯分析還有硬件層面的防護(hù)比如電機(jī)堵轉(zhuǎn)保護(hù)、電池低壓報警等。我踩過的一個坑是忽略了系統(tǒng)啟動順序。底盤驅(qū)動還沒就緒導(dǎo)航節(jié)點(diǎn)就開始發(fā)速度指令結(jié)果指令被丟棄機(jī)器人不動。后來加了一個啟動狀態(tài)機(jī)確保所有節(jié)點(diǎn)按依賴順序啟動并且互相之間有心跳檢測任何一個節(jié)點(diǎn)異常都能被及時發(fā)現(xiàn)。這個改動看起來簡單但讓系統(tǒng)的穩(wěn)定性上了一個臺階。還有一個經(jīng)驗是版本管理。機(jī)器人操作系統(tǒng)生態(tài)更新很快今天能跑的配置明天可能就因為某個包升級而失效。我的做法是把整個工作空間用版本控制工具管理起來包括配置文件、啟動腳本和標(biāo)定參數(shù)。每次升級依賴之前先打標(biāo)簽出問題可以快速回滾。這個習(xí)慣在團(tuán)隊協(xié)作中尤其重要能避免很多“在我機(jī)器上能跑”的扯皮。最后分享一個關(guān)于參數(shù)配置的小技巧。所有可調(diào)參數(shù)不要寫死在代碼里而是放在獨(dú)立的配置文件中并且給每個參數(shù)加上注釋說明含義和取值范圍。這樣調(diào)參的時候不用翻代碼直接改配置文件重啟節(jié)點(diǎn)就行。更進(jìn)一步可以把常用場景的參數(shù)組合保存成不同的配置文件比如“狹窄通道模式”和“開闊區(qū)域模式”切換場景時直接加載對應(yīng)的配置省去反復(fù)調(diào)參的時間。