3K字機器人室內避障導航入門(挖坑!)

來自專欄機器人與CV實用技術6 人贊了文章

轉載必須留原文鏈接,做不到不要轉,君子協定.

作者正在研究室內無人機/無人車自動駕駛和一些機器學習結合的方向.寫這篇文章既是總結以往的閱讀學習成果梳理脈絡,也是為了自己進一步學習與幫助尚未入門的讀者快速把握這個方向的主幹內容.圖像,視頻上的神經網路方法作者較為了解,因此會加一些交叉方向的內容,希望能給讀者帶來一些有益的參考.

本文計劃用3K字左右(可能會改,作者認為這種形式可能不太好.老規矩,分割線往下開始算)講述機器人室內避障和導航演算法.包括:

1.避障軟硬體常用解決方案.

2.常用路徑規劃/導航演算法.

3.與圖像/視頻語義方法結合.

上一篇文章簡單的總結了SLAM演算法的必要組成和一些重要的技巧.這一篇的內容和上一篇有一定關聯性,如果對SLAM不很了解,可以看作者的上一篇文章.另外作者發現沒有圖看起來比較困難,因此未來可能會修改文章內容加一些圖,讓讀者理解起來輕鬆些.行文風格上,可能會傾向於靈活俏皮又不失沉穩一點,就像在北方城市冬夜凌晨12點打開窗戶呼吸室外冷又甜的空氣一樣的感覺,又像是喝到第一杯自己做的手沖黃波旁.(作者腦洞比較大,是鼠.)

作者本人背景是某工科專業畢業,計算機雙學位.正從事的工作是無人機室內室外自動駕駛開發.因此行文中可能更偏重介紹與我本人熟悉的方案相關內容,而忽略一些和從業方向關係不大的方案.

(第一次填坑.)

Part<1>.避障軟硬體常用解決方案.

Method<1>.超聲波測距避障.

超聲波雷達是常用的避障手段.最常見的是汽車的倒車雷達.發射模塊發射經編碼的超聲波信號,由接收模塊解碼.由於到不同弧形距離障礙物的反射時間不同,必然會產生多個回波,選擇最近的測算時間作為參考計算距離.其優點是簡單,廉價,對大型障礙物響應很可靠.缺點是無法確定障礙物的具體位置,而且對於一些小的障礙物反射的回波是沒有反應的(筆者買車後第二天倒車的時候直接懟到了小區地鎖,這種小目標超聲波雷達是沒有任何提示的.好吧我笨).

需要注意:超聲波測距的距離較短,一般可靠距離只有3米內.探測角度較寬,且一般不能在水面上使用.

Method<2>.紅外結構光深度攝像機/ToF深度攝像機避障.

紅外結構光深度攝像機的原理是由紅外發射機發射紅外條紋,並由兩部標定好的紅外攝像機對條紋進行拍攝.條紋對人眼是不可見的.紅外結構光恢復的結構是點雲,可以理解成對主攝像機來說每個像素對應的射線都有一個深度定義(雖然部分區域可能是超出最近和最遠量程的.).

與提取特徵點的雙目視覺不同,這種方法恢復的結構是稠密的,每一個點都有一個明確的深度定義.因此可以恢復出點雲,相比SfM方法基於三角剖分恢復具體結構,這種方法恢復的稠密結構更加細緻精確.

當然點雲的信息不可避免的會有一定的雜訊,可能需要一定的濾波演算法過濾掉極端值,避免因為偶然誤差導致誤報.

這裡要注意,由於紅外光信號是主動發出的,其發射功率是十分有限的,因此在室外白天幾乎不能用.

與之相似的還有一種ToF深度攝像頭.這種攝像頭的原理也是主動發射光源,但測算的是發射和返回時間差,因此抗外界光源干擾能力要好一些,但現在精度做不上去.

這兩種攝像頭測量範圍都不遠.紅外結構光一般在5m之內,ToF量程不超過10m.紅外結構光有一個大概60cm的死區,死區內的東西是看不見的(視差過大).

對於這兩種攝像機,由於返回的對象是點雲,因此需要進行後處理判斷是否有障礙物.可以在體素濾波之後維護一棵八叉樹,用於快速多尺度判斷某區域是否可以通過.

常用的有Kinect,Realsense和一些國內廠商仿製的東西.總的來說技術門檻不高,但現在處於初期階段,預計未來ToF會成為主流方法.

Method<3>.激光雷達.

激光雷達這東西作者沒實際用過.只能找一些資料補充.

激光雷達的價格現在有高有低.多線,機械式的高速旋轉的激光雷達價格仍然居高不下,VLP-16 Puck 16線激光雷達降價後仍然需要26k人民幣(無法用於汽車自動駕駛),64線需要約500k人民幣,128線的雷達就更是天價了.即使是汽車自動駕駛也接受不了如此高昂的成本.

現在市面上賣的比較多的使用"激光雷達"的掃地機器人,用的多半是單線的激光雷達,掃描一圈的頻率一般10Hz以內,距離最遠10-20m,價格500人民幣左右.配合這種激光雷達的SLAM演算法和上一篇文章中的視覺SLAM不同,主要是基於網格估計和粒子濾波進行定位,其演算法較為簡單成熟,精度和可優化空間一般(不過Google的Cartographer使用了Ceres進行圖優化而不是簡單濾波融合,使得其精度比經典的演算法好了很多).這種廉價激光雷達用於避障相對局限性較大,尤其是遇到機器人的高度較為顯著或者不是在地面上(如無人機)的情況,其局限性更為明顯.激光雷達的返回結果是角度和距離的元組,可以結合一些概率方法進行障礙物位置的估計,推薦參考tinySLAM的代碼實現(鏈接:OSLL/tiny-slam-ros-cpp).

Part<2>.常用路徑規劃和導航演算法.

設現在機器人已有(至少是部分)室內地圖和目標點的位置.如何搜索路徑到達想去的點?

如果了解一些常用演算法,可能想到先將空間離散化,然後使用圖論里的Djikstra搜索.但這種方法會向各個方向移動,不會考慮到幾何上的關係,因為圖論中的圖是沒有笛卡爾坐標系下的約束的,各個節點和邊的關係是離散的.表現在機器人上,會浪費很多時間嘗試捨近求遠的道路.

分析原因.Djikstra演算法將空間離散化,而在搜索的過程中只考慮到離散點離起點距離這一個因素,這就限制了演算法的性能.

如果加一些啟發式的信息呢?可以使用A*演算法.它考慮探索一個點的優先順序不是離起點多近,而是離起點距離+到終點的,不考慮障礙的直連距離(注意:這裡的距離可以是歐氏距離,也可以是曼哈頓距離,具體怎麼取取決於你的機器人類型.).

使用A*演算法時可以加入一個權重,權衡啟發距離和起點距離的比值.其中點和點離散化時候距離的選擇可以自己權衡.比如做小車控制,崎嶇的路線(地毯等)上點的連線可以設的損失權重大一些;用在無人機上時,要考慮到上升下降的損失權重比平移大.

但只用遊戲里常用的普通A*演算法行不行呢?如果做一個模擬實驗,會發現有一些問題.比如面對一面比較寬的牆的時候,演算法會讓機器人左走一格,右走一格,左右兩格,右走兩格...這種走法效率是很低的.如果障礙需要左走N格即可離開,那用這種搜索方法,需要2*(1+2+...N),時間複雜度是N^2的.

為此,可以設置一個忍耐閾值.在閾值內即使探索地圖有新的障礙物發現,也不更改方向.比如設置成左走10格以內,都不改為右走,即可大大減少這種情況的複雜度,時間消耗仍然是N^2的,但常數項削減不少.

更進一步的看這個問題.如果運動的維度不是2或者3呢?比如,機械臂的運動.機械臂可能有多個關節,每個關節上都有一定的自由度,組合後自由度可以很高.且機械臂的任何部件都可能和環境中的剛體發生運動干涉(說通俗點,就是兩個東西碰上了).這部分運動規劃如果是較為簡單的情況,超靜定次數不大且沒什麼障礙物,直接使用IK解算反解關鍵幾個關節的運動即可.如:將手心移動到球體的上方準備抓取,直接對手臂上的兩個關節做IK解算即可求解目標運動角度.

如果遇到一些複雜的情況,可能要考慮採用一些隨機的演算法.為什麼不能繼續在高維空間里用A*呢?

這裡要考慮一件事.如果離散化的時候0.05m作為一個格,那麼1m^3的空間里有8k個格,格與格之間的邊可以稀疏存儲(即只有連接的才存儲).總體代價為O(N^3).

如果某關節結構自由度是7呢?存儲,搜索的代價都是O(N^7),幾乎是不可能實現了.而且這種演算法也不符合直覺.人類有幾百塊不同的骨骼肌,計算運動目標的時候也沒有用到這麼大範圍的啟發式搜索.這裡考慮引入一定的隨機性,使用RRT(Rapidly exploring Random Tree)演算法.這篇文章介紹了RRT和SRT的實現,可作為參考.鏈接:未知環境探索（三）RRT 與SRT.

這類演算法最大的好處是沒有保存那麼多的狀態,這樣對於高維解空間的搜索就很有利.如果像A*一樣保存很多的點,就經常需要更新這些狀態.尤其是處於探索狀態(地圖不完全已知)時這種反覆搜索路徑困難更大.

還有一類尋路方法,勢場法.不過不太常用就是了.挖個坑,回來填.

Part<3>.圖像/視頻語義方法在導航避障中的應用.

首先講一下運動結構恢復(Structure from Motion,SfM):

和SLAM類似,這種演算法的目標是從多張圖像中恢復一張全局地圖.不過這種地圖更加稠密,且較為強調精度.一般的SfM不太要求對攝像機進行預先標定,而SLAM一般都要求有一個不錯的相機參數初值.

挖坑,計劃介紹一下語義地圖,SfM和3D物體檢測(ssd-6d).