0引言

本文主要介紹一種適用于非結(jié)構(gòu)環(huán)境的四輪差速無人車,然后使用其實現(xiàn)了三維地圖的建立和同時 定位,并進(jìn)行了相應(yīng)的實驗。該無人車基于ROS(Robot Operating System)進(jìn)行開發(fā),ROS起源于斯坦福大學(xué)與機(jī)器人技術(shù)公司W(wǎng)illow Garage的個人機(jī)器人項目。它提供類似操作系統(tǒng)所提供的功能,包括硬件抽象描述、底層驅(qū)動程序管理、共用功能的執(zhí)行、程序間的消息傳遞、程序發(fā)行包管理,也提供一些工具程序和庫用于獲取、建立、編寫和運行多機(jī)整合的程序^[1]。車輛以NVIDIA Jetson TX2控制板作為上位機(jī)進(jìn)行控制和數(shù)據(jù)處理,使用 自制工控機(jī)作為下位機(jī)進(jìn)行數(shù)據(jù)采集和控制驅(qū)動器,裝備有編碼器、三維激光雷達(dá)、高精度IMU(Inertial Measurement Unit)、ZED雙目立體相機(jī)等多種傳感器。另外,提出了一種基于編碼器和IMU融合的六自由度狀態(tài)估計方法,然后使用改進(jìn)后的開源算法,實現(xiàn)了高精度三維地圖的建立和同時定位。

1四輪差速無人車

本文介紹的四輪差速無人車基于ROS開發(fā),硬件架構(gòu)如圖1所示,把具備多種接口的工控機(jī)作為下位機(jī)進(jìn)行數(shù)據(jù)采集,將計算能力強(qiáng)大的TX2控制板作為上位機(jī)。工控機(jī)和TX2都裝有通用操作系統(tǒng) Ubuntu 16.04和Kinetic版本的ROS系統(tǒng),工控機(jī)和TX2通過網(wǎng)線進(jìn)行通信。為提高下位機(jī)和驅(qū)動器之間的通信速率,提升車輛響應(yīng)速度,特采用CAN(Controller Area Network)總線通信。無人車裝備了多種傳感器,包括Velodyne 16線三維激光雷達(dá)、ZED雙目立體相機(jī)、Crossbow NAV440高精度IMU等。

圖2是四輪差速無人車實物圖,共分為三層,第一層放置通用外設(shè)、激光雷達(dá)、GPS天線和急停開關(guān);第二層放置IMU、ZED相機(jī)、工控機(jī)、TX2控制板、 CAN分析儀、供電端口和超聲波雷達(dá);第三層放置電池、銘朗MLDS4810型驅(qū)動器、伺服電機(jī)、ROTARY OVW2—10—2MD型編碼器。

2 里程計融合

里程計是車輛定位和建圖的重要依據(jù)之一,所以精確的里程計是高精度地圖和準(zhǔn)確狀態(tài)估計的重要保證。當(dāng)車輛行駛在非結(jié)構(gòu)路面上時,在世界坐標(biāo)下車輛不僅有x、y方向的位移,還有z方向的位移。基于視覺的里程計和基于雷達(dá)的里程計通過幀間匹配的方式都可得到六自由度的車輛狀態(tài)估計。近年來,專家學(xué)者提出了很多基于視覺的里程計算法:文獻(xiàn)[2]使用雙目相機(jī),通過基線計算車輛運動;文獻(xiàn)[3]使用深度相機(jī),通過視覺和深度計算里程計;文獻(xiàn)[4]通過使用三維激光雷達(dá)進(jìn)行了里程計計算。

但是使用單一的傳感器進(jìn)行里程計計算會有諸多缺點。例如,視覺里程計必須在中等且穩(wěn)定的光照條件下進(jìn)行匹配,而雷達(dá)里程計需要通過大量的變量對運動進(jìn)行解算,以保證運動不失真。為了得到高精度里程計,zhang Ji等人率先提出一種融合視覺和雷達(dá)的里程計計算方法^[5],但是這種方法需要大量的計算資源,實時性不好。

本文主要介紹一種基于編碼器和IMU融合的六自由度狀態(tài)估計方法,這種方法具有實時性好、精度高等特點。為簡化計算,本章在未特別注明使用世界坐標(biāo)系時,默認(rèn)使用車輛的自有坐標(biāo)系,如圖3所示。

3基于編碼器和IMU融合的里程計

四輪差速無人車采用四個編碼器分別采集四個車輪的轉(zhuǎn)速,忽略同側(cè)前后兩車輪轉(zhuǎn)速差,記左側(cè)兩車輪的轉(zhuǎn)速為n_L,右側(cè)兩車輪的轉(zhuǎn)速為n_R。由式(1)和(2)可得左側(cè)車輪線速度VL和右側(cè)車輪線速度V_R,其中R為車輪半徑,因左右兩側(cè)編碼器安裝方向相反,則測得的旋轉(zhuǎn)方向相反,所以V_R為速度的相反數(shù):

因同側(cè)前后車輪轉(zhuǎn)速近似相等,可將四輪差速模型簡化為如圖4所示的兩輪差分模型,d為輪距的一半,r為車輛中心點到轉(zhuǎn)彎中心點的距離,因左輪和右輪轉(zhuǎn)彎時繞轉(zhuǎn)彎中心點的角速度相同,可得式(3):

式中:W_L、W_R為左輪角速度和右輪角速度。

用V_L、V_R和d表示r,可得式(4):

則有式(5):

車輛x方向的速度Vx可由式(6)得出:

式中:W為角速度。

四輪差速無人車裝備了Crossbow NAV440系列的高精度IMU,Crossbow NAV440是一款GPS/INS組合導(dǎo)航的產(chǎn)品,它綜合利用了基于MEMS的慣性器件和GPS技術(shù),即使在沒有GPS的狀態(tài)下,角速度測量精度也小于0.02°，加速度測量精度在運動過程中小于1 mGal。通過IMU測得車輛繞xyz旋轉(zhuǎn)的角速度,對三個角速度進(jìn)行積分,得到車輛的姿態(tài),以歐拉角進(jìn)行表示,具體表示為滾動角θ_R、俯仰角θ_P和航向角θ_Y。則在世界坐標(biāo)系下,車輛的位移Lx、Ly和Lz分別為:

4 三維地圖構(gòu)建及狀態(tài)估計實驗

目前,有許多開源三維地圖構(gòu)建及同時定位的算法,比如LOAM、hdl_graph_slam等。筆者對hdl_graph_slam算法進(jìn)行了改進(jìn),原有算法使用單一的三維激光雷達(dá)點云進(jìn)行抽稀(Downsample)后再進(jìn)行幀間匹 配,得出里程計。但這種方法需要大量的特征點進(jìn)行匹配,還需要消耗大量的計算資源,而且在車速較高時會產(chǎn)生畸變;另外,經(jīng)多次實驗,用幀間匹配得到的里程計在z方向上的位移計算不準(zhǔn)確。針對這種情況,改進(jìn)為采用編碼器和IMU融合計算里程計。

hdl_graph_slam算法主要分為四個部分:

1)濾波,就是通過PCL(Point Cloud Library)對原始激光點云數(shù)據(jù)進(jìn)行抽稀,對不在有效范圍內(nèi)的無效激光進(jìn)行去除(Outlier Removal)。

2)幀間匹配,每隔一段預(yù)先設(shè)置好的時間提取一幀點云作為關(guān)鍵幀(keyframe),在預(yù)先設(shè)置好的范圍內(nèi)提取特征與關(guān)鍵幀進(jìn)行匹配,計算出有六自由度的里程計。

3)地面探測,設(shè)置激光高度閾值,將高于閾值的激光濾除,得到地面的點云數(shù)據(jù)。

4)建圖,將所有匹配過的點云幀保存,形成點云地圖,使用g2o圖優(yōu)化算法對全局地圖進(jìn)行優(yōu)化。g2o是一個通用圖優(yōu)化函數(shù)庫,帶有多種求解器,其將優(yōu)化問題以圖的形式表達(dá)。把關(guān)鍵幀作為g2o中的“node”,將與關(guān)鍵幀進(jìn)行匹配的點云幀作為g2o的“edge”。

具體實驗步驟如下:

步驟一:使用手柄向上位機(jī)發(fā)送速度指令,使車輛繞某工廠大樓行駛一周,速度指令以Twist消息格式傳遞。

步驟二:工控機(jī)接收到速度指令后執(zhí)行相應(yīng)運動,同時采集三維環(huán)境點云數(shù)據(jù),消息以點Pointcloud2格式傳遞;與此同時,根據(jù)周圍環(huán)境及里程計信息,估計當(dāng)前位姿信息,如圖5所示。

步驟三:上位機(jī)對點云數(shù)據(jù)進(jìn)行處理。

步驟四:以PCD格式保存三維點云地圖,如圖6所示。

本次實驗建立了高精度稠密點云地圖,在車輛圍繞工廠大樓行駛一周后,地圖形成了閉環(huán),沒有發(fā)生偏移,這說明使用編碼器和IMU融合的里程計表現(xiàn)出很高的精度。

5結(jié)論

本文提出了一種適用于室外環(huán)境的四輪差速無人車,車輛采用ROS系統(tǒng),很好地解決了機(jī)器人領(lǐng)域代碼可移植性和復(fù)用性的問題;在硬件方面,裝備了機(jī)器人行走領(lǐng)域所需傳感器;另外,還提出了一種融合編碼器和IMU狀態(tài)估計的方法,并成功應(yīng)用到3D SLAM (Simultaneous Localization and Mapping)算法中,然后進(jìn)行了相應(yīng)實驗,在該實驗中,車輛表現(xiàn)出了精度高、響應(yīng)速度快的特點,可作為室外移動機(jī)器人同時定位與建圖的參考。

[參考文獻(xiàn)]

[1]胡春旭,熊梟,任慰,等.基于嵌入式系統(tǒng)的室內(nèi)移動機(jī)器人定位與導(dǎo)航[J].華中科技大學(xué)學(xué)報(自然科學(xué)版), 2013,41(增刊1):254—257.

[2]NISTERD,NAR0DITSKY0,BERGENJ.visual odometryforgroundvehicleapplications [J].Journalof FieldRobotics,2006,23(1): 3—20.

[3] Huang A S,Bachrach A,Henry P,et al.visual odometry and  mapping  for  autonomous  flight  using  an  RGB—D
camera[c]//Robotics Research,The 15th International Symposium  ISRR  2017 : 235—252.

[4]ZHANG  J , SINGH   S. Low—drift   and  real—time   lidar odometry  and  mapping[J].Autonomous  Robots,2017,41
(2) :401—416.

[5] ZHANG  J,SINGH  S.visual—lidar odometry and mapping :  low—drift,robust,and  fast[c]//2015  IEEE  Interna— tional   conference   on   Robotics   and   Automation (IcRA),2015 : 2174—2181.

2024年第19期第12篇