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