基于激光與視覺融合的復合機器人高精度導航系統(tǒng)*

張智博,鄒喜華,鄧 果

(1.西南交通大學信息科學與技術(shù)學院,成都 611756;2.成都錦西數(shù)智科技有限公司,成都 611756)

0 引言

隨著工業(yè)和制造業(yè)等領(lǐng)域的工作量和復雜程度逐漸上升,用機器人代替人工來完成更加精確繁重的重復性任務(wù)的趨勢日益明顯,并且對于機器人自主化的需求也更加迫切。相比于AGV(automated guided vehicle)和機械臂的單一功能,集成了兩者特性的復合機器人則更具柔性化、自主化。此類機器人自主化和智能化的重要標志就是能夠進行自主穩(wěn)定的導航。導航涉及地圖建立、定位以及路徑規(guī)劃等技術(shù)。機器人導航技術(shù)的發(fā)展經(jīng)歷了固定軌跡的磁條導航[1-2]和基于機器視覺的色帶[3-5]、圖案導航[6]、反光板導航[7-8]、慣性導航[9-10]、視覺導航[11]、能夠靈活規(guī)劃路徑的激光導航[12-13],到目前的多傳感器融合的導航方式[14-15],每種導航方式都有其優(yōu)點。磁條、色帶和圖案等方式的導航軌跡固定,作業(yè)過程比較穩(wěn)定,只需要考慮停障或避障問題,且系統(tǒng)較為簡單,但是靈活性和工作效率相對較低,僅僅適用于少量特定的場合。目前應(yīng)用最廣泛的是SLAM技術(shù),即利用激光雷達等傳感器來構(gòu)建完整的環(huán)境地圖并用于自主導航。目前大多數(shù)成熟的機器人產(chǎn)品使用單線激光雷達導航的方式,但是由于信息源單一,隨著機器人被逐漸應(yīng)用在越來越復雜的場景,其定位精度不高且對于靈活多變的環(huán)境應(yīng)對能力不足的缺點就凸顯了出來,無法達到更加可靠、精準、穩(wěn)定的使用要求。

本文針對復合機器人高精度自主導航定位的需求,設(shè)計了激光雷達導航與視覺高精度定位融合的導航定位系統(tǒng),使最終的導航定位精度穩(wěn)定在了10 mm以內(nèi)。

1 復合機器人系統(tǒng)

1.1 軟硬件系統(tǒng)框架

基于Ubuntu系統(tǒng)和ROS(robot operating system)平臺,建立復合機器人系統(tǒng)框架。如圖1所示,系統(tǒng)分硬件層和軟件層兩個部分。

圖1 軟硬件系統(tǒng)框架

機器人硬件方面采用兩輪差速型底盤,英偉達NX工控機、倍加福單線激光雷達、覽沃固態(tài)激光雷達、Realsense深度相機、六軸機械臂、IMU。機器人實物如圖2所示。激光雷達、相機、IMU等傳感器的數(shù)據(jù)在工控機中處理,工控機通過CAN通信方式給機器人底盤下發(fā)速度指令,機器人底盤控制板將工控機發(fā)下來的速度指令通過運動學模型分解為左右輪電機的轉(zhuǎn)速,進而發(fā)送到相應(yīng)的電機驅(qū)動器,電機驅(qū)動器利用PID算法控制電機的轉(zhuǎn)速。同時,電機驅(qū)動器實時讀取編碼器的數(shù)據(jù),解算出機器人實際的運動速度和運動里程,并按照特定格式反饋給工控機。工控機通過TCP通信的方式給機械臂發(fā)送移動指令,并接收機械臂反饋的結(jié)果,最終實現(xiàn)整個系統(tǒng)的正常運行。

圖2 機器人實物圖

軟件部分主要包括激光雷達、IMU等傳感器驅(qū)動程序、激光雷達導航、視覺高精度定位與機械臂抓取部分。在啟動傳感器驅(qū)動后首先加載目標點進行激光雷達導航,在到達指定目標點后執(zhí)行視覺高精度定位,定位完成后導航系統(tǒng)控制機械臂模塊對物品進行識別抓取,抓取完成轉(zhuǎn)運至下一目標點并放置物品。

1.2 導航定位系統(tǒng)

導航定位系統(tǒng)包括激光雷達導航、視覺高精度定位部分,具體結(jié)構(gòu)如圖3所示。這里由于里程計的準確度密切影響激光雷達導航的重定位精度,且室內(nèi)輪式機器人容易出現(xiàn)打滑現(xiàn)象,單獨依靠編碼器解算的輪式里程計容易引起較大誤差,所以使用擴展卡爾曼濾波算法對輪式里程計與IMU進行融合來保證里程計的準確性[16]。另外復合機器人載有機械臂,比普通搬運機器人高出許多,單獨使用二維激光雷達不足以躲避不規(guī)則動態(tài)障礙物。如圖3所示,由于二維激光雷達的安裝高度較低,只能檢測到桌子的四條桌腿,而在桌面高度很容易與桌面發(fā)生碰撞。三維雷達就能夠檢測到桌子的全貌,所以增加固態(tài)三維雷達來提高避障的準確性。整個融合式導航定位系統(tǒng)的工作原理詳見后文。

圖3 導航定位系統(tǒng)框架

2 高精度自主導航定位系統(tǒng)

本文技術(shù)方案中的高精度自主導航定位系統(tǒng)分為兩個階段,第一階段為激光雷達導航,主要作用為構(gòu)建環(huán)境地圖并根據(jù)實時定位以及掃描的障礙物信息自主規(guī)劃路徑并移動到目標點,且能夠?qū)崟r避障;第二階段為視覺高精度定位,針對激光雷達導航精度不足的問題進行位姿調(diào)整,使最終定位精度達到10 mm以內(nèi)。

2.1 激光雷達導航

激光雷達導航部分包括SLAM建圖、重定位、路徑規(guī)劃與運動控制,根據(jù)ROS分布式架構(gòu),對激光雷達導航系統(tǒng)進行模塊化開發(fā)。

2.1.1 SLAM建圖

室內(nèi)機器人執(zhí)行導航任務(wù)的前提是有一個準確的地圖,這里使用Cartographer來構(gòu)建環(huán)境地圖。Cartographer是一套基于圖優(yōu)化的算法,分前端和后端兩個部分。算法以機器人各時刻的位姿為節(jié)點,以節(jié)點間的運動作為約束邊來進行圖優(yōu)化,并且能夠進行回環(huán)檢測。

Cartographer前端引入子圖(Submap)概念,負責子圖的生成,并通過Scan-to-map激光幀與子圖匹配代替常用的Scan-to-scan幀間匹配的方法,不斷執(zhí)行激光與子圖匹配,當新的激光幀觀測不到當前子圖之外的新信息時再開始創(chuàng)建新的子圖,然后由所有子圖構(gòu)成全局地圖,過程如圖4所示。所有激光幀對應(yīng)的機器人全局位姿和所有子圖對應(yīng)的全局位姿通過Scan-to-map的匹配方式進行關(guān)聯(lián),從而構(gòu)成位姿圖[17]。

圖4 Cartographer地圖結(jié)構(gòu)

隨著地圖規(guī)模逐漸增大,累積誤差就會越來越大,這時引入回環(huán)檢測,可以檢測到機器人在此之前是否到過此地,即位姿閉環(huán),能夠在一定程度上消除累積誤差。具體方式為將當前激光數(shù)據(jù)與所有子圖進行匹配,若當前激光幀位姿和某個子圖中的一個位姿距離較近,則將當前激光幀與這個子圖進行匹配,若匹配程度復合要求,則認為成功檢測到了回環(huán)。并且為了提高搜索效率,Cartographer采用了分支定界的方法,每來一個新的激光幀,都對其進行回環(huán)檢測,若激光幀被確定為閉環(huán),則將此閉環(huán)約束加到全局建圖的約束中,對子圖在世界坐標系下的位姿和激光幀在世界坐標系下的位姿和進行一次全局優(yōu)化,最終得到優(yōu)化后的全局地圖。

2.1.2 重定位

基于已知地圖的重定位使用AMCL(自適應(yīng)蒙特卡洛)算法實現(xiàn),其主要負責根據(jù)里程計、激光雷達信息來解算機器人在環(huán)境地圖中的實時位姿。AMCL是一種基于粒子濾波并且增加自適應(yīng)方法來對粒子進行采樣的重定位算法,考慮到機器人在地圖中的初始位姿是未知的,首先通過高斯隨機采樣的方式將粒子分布在整個地圖中,然后通過狀態(tài)轉(zhuǎn)移方程即機器人運動學方程得到下一時刻粒子的建議分布,并將激光雷達觀測信息帶入粒子狀態(tài),從而得到每個粒子位置的觀測信息。然后將觀測信息與機器人所處的地圖進行比較,并根據(jù)二者的匹配程度給每個粒子打分,添加權(quán)重。然后進行迭代,將匹配得分小于設(shè)定閾值的粒子丟棄,為保持粒子數(shù)量并將注意力轉(zhuǎn)移到粒子得分較高的地方,按高斯分布在剩余粒子周圍重新生成部分新的粒子,迭代完成后用粒子的權(quán)值表示狀態(tài)變量的概率分布,即機器人最終的位姿。

2.1.3 路徑規(guī)劃與運動控制

機器人路徑規(guī)劃通常使用A*算法,其在地圖中對于每個搜索到的節(jié)點通過式(1)所示的總代價估計函數(shù)式進行估計來得到最優(yōu)位置,再從這個位置繼續(xù)搜索,直到到達目標點[18]?？偞鷥r估計函數(shù)表達式為:

f(n)=g(n)+h(n)

(1)

式中:f(n)是節(jié)點n從起點到目標點的代價估計函數(shù),g(n)是狀態(tài)空間中從起始節(jié)點到當前節(jié)點的實際代價,h(n)是從當前節(jié)點到目標點的最佳路徑的代價。

但是傳統(tǒng)A*算法規(guī)劃的路徑不夠平滑,會導致機器人頻繁轉(zhuǎn)向,所以這里使用三階貝塞爾曲線對其進行改進。

由選取的n+1個控制點構(gòu)成的n階貝塞爾曲線的表達式可由式(2)表示:

(2)

式中:j為控制參數(shù),Pi為控制點坐標,Bi,n(j)為伯恩斯坦多項式,其表達式為:

(3)

那么三階貝塞爾曲線的參數(shù)方程可表示為:

P(j)=P0(1-j)3+3P1(1-j)2j+3P2(1-j)j2+P3j3

(4)

式中:Pi=(xi,yi)為控制點的坐標。

局部路徑規(guī)劃模塊負責根據(jù)已有的全局路徑規(guī)劃出一段在機器人一定范圍內(nèi)的路徑,計算并下發(fā)速度指令,即運動控制。使得機器人能夠按照規(guī)劃的路徑運行,并實時地進行避障。本文采用DWA算法,其主要通過在速度空間(v,ω)中采樣多組速度值,并模擬機器人在這些速度下一定時間內(nèi)的軌跡[19],如圖5所示,在得到多組軌跡后,使用式(5)所示的評價函數(shù)對這些軌跡進行評價,選取最優(yōu)軌跡所對應(yīng)的速度來驅(qū)動機器人運動。

圖5 動態(tài)窗口采樣軌跡

F(v,ω)=α·velocity(v,ω)+β·dist(v,ω)+γ·angle(v,ω)

(5)

式中:v為機器人的線速度,ω為機器人的角速度,velocity(v,ω)為用來評價模擬軌跡的機器人速度,dist(v,ω)用來評價此條模擬軌跡上與障礙物的距離,angle(v,ω)用來評價若機器人在以該速度行駛的情況下,模擬軌跡終點與目標點相差的角度,α、β、γ分別為3個函數(shù)對應(yīng)的權(quán)重。

2.2 視覺高精度定位

本文選擇二維碼作為視覺標志物,根據(jù)二維碼大小可得到二維碼在相機坐標系下的實時位姿,經(jīng)測試其測量誤差在1 mm以內(nèi)。二維碼視覺高精度定位流程如圖6所示,主要過程為通過相機實時檢測二維碼,并獲取其姿態(tài)信息,然后轉(zhuǎn)換為兩坐標系之間的偏移旋轉(zhuǎn)量,最后通過相機運動使二者重合,達到高精度定位的目的。

圖6 二維碼視覺高精度定位流程

相機調(diào)整完成后與二維碼坐標系的相對位置關(guān)系如圖7所示,上下分別為相機坐標系與二維碼坐標系,二者坐標軸重合,表明定位成功。

圖7 相機與二維碼相對位置關(guān)系

2.3 激光雷達導航與視覺高精度定位融合

激光雷達導航受限于激光雷達精度、地圖質(zhì)量和機器人底盤硬件精度等多種因素,其定位精度一般在2～5 cm,有些甚至在5～10 cm范圍,所以單獨依賴激光雷達導航無法達到有些場景下10 mm的定位需求。本文將激光導航與二維碼視覺高精度定位融合,第一階段采用激光雷達導航,在自主導航至目標點附近后通過在目標點附近設(shè)置標志物再進行第二階段的二維碼視覺高精度定位,使定位精度達到10 mm以內(nèi)。

具體實現(xiàn)方式如圖8所示,由于機器人底盤高度原因,選擇將相機安裝到底盤正后方,使用相機檢測二維碼只能獲取二維碼與相機坐標系的相對位置,所以目標是在激光雷達導航完成后將相機坐標系與二維碼坐標系盡量重合。其中XOY為二維碼坐標系,L為機器人底盤坐標系與相機坐標系之間的距離,為固定值,相機坐標系原點在二維碼坐標系下的坐標x,y和偏角θ已知,調(diào)整流程分旋轉(zhuǎn)-平移-旋轉(zhuǎn)3步,那么根據(jù)式(6)～式(8)即可求出機器人需要移動的距離和旋轉(zhuǎn)角度。且為了便于控制旋轉(zhuǎn)和平移量,使用IMU數(shù)據(jù)積分量作為反饋,從而構(gòu)成閉環(huán),待精度達到要求范圍內(nèi)結(jié)束調(diào)整,激光雷達導航與二維碼視覺融合高精度導航定位的整個流程完成。

圖8 融合定位流程

第一次旋轉(zhuǎn)角度:

(6)

平移距離:

(7)

第二次旋轉(zhuǎn)角度:

(8)

3 系統(tǒng)實現(xiàn)與功能測試

3.1 激光雷達導航系統(tǒng)測試

機器人硬件搭建完成后在ROS下進行軟件部分的開發(fā),ROS能夠很好地支持Python、C++等編程語言進行開發(fā),且提供了RVIZ等可視化工具,為機器人研究提供了很大便利。

使用Cartographer算法構(gòu)建的實驗所用環(huán)境地圖與真實環(huán)境如圖9所示。

(a) 環(huán)境地圖 (b) 真實場景圖9 環(huán)境地圖與真實場景

進而,通過控制機器人運動記錄下其在地圖中幾個點的坐標作為目標點,編寫程序使其能夠在目標點之間循環(huán)導航,通過ROS下的可視化工具RVIZ觀察機器人運行情況,靜態(tài)導航過程如圖10所示。

圖10 導航示意圖

在兩個導航點之間放置一把椅子作為動態(tài)障礙物,觀察機器人的動態(tài)避障情況,如圖11所示。

(a) 二維雷達下的障礙 (b) 三維雷達下的障礙

圖11a中二維激光雷達僅能檢測到椅子的四條腿,而圖11b顯示三維激光雷達點云能夠檢測到椅子全貌。機器人成功規(guī)劃出了一條繞過障礙物的路徑(彎曲線條),且機器人在運動過程中一直與障礙物保持著一定的距離。如圖11d所示,到達目標點后激光輪廓依然與地圖輪廓保持著較好的重合度,表明機器人定位準確。

3.2 激光雷達與二維碼視覺融合導航定位測試

通過在一個目標點處地面上放置二維碼,并在一階段激光雷達導航完成后執(zhí)行二維碼視覺高精度定位模塊進行位姿調(diào)整,測試定位精度。激光雷達導航結(jié)束與二維碼視覺高精度定位結(jié)束后由相機檢測數(shù)據(jù)計算得到的機器人定位精度對比如表1所示。由這些實驗數(shù)據(jù)可得二維碼視覺高精度定位后的平均精度為5.1 mm,且相較于一階段激光雷達導航的精度提升了82.7%,達到了預期效果。

表1 二維碼視覺高精度定位前后精度對比 (mm)

機器人導航至目標點并執(zhí)行完視覺高精度定位后,既可執(zhí)行對接轉(zhuǎn)運,也可進行機械臂識別抓取。對自動抓取轉(zhuǎn)運進行測試,如圖12a所示,機械臂抓取成功并調(diào)整好姿態(tài)后,導航系統(tǒng)繼續(xù)前往下一個目標點進行放置,如圖12b所示。

(a) 抓取物品 (b) 放置物品圖12 機械臂抓取與轉(zhuǎn)運放置

4 結(jié)束語

本文主要研究了室內(nèi)場景下復合機器人高精度導航定位問題,將激光雷達與視覺高精度定位融合形成兩階段導航系統(tǒng),同時增加了三維激光雷達進行避障,有效提升了定位精度,并兼顧了穩(wěn)定性與精確性。經(jīng)實驗證明設(shè)計的導航定位系統(tǒng)定位精度更高,且能夠安全躲避不規(guī)則障礙物,為復合機器人自動搬運等應(yīng)用場景提供了精確穩(wěn)定的方案。