非結(jié)構(gòu)環(huán)境中四輪差速無人車的三維重建研究

0引言

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

1四輪差速無人車

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

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

2 里程計(jì)融合

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

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

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

3基于編碼器和IMU融合的里程計(jì)

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

因同側(cè)前后車輪轉(zhuǎn)速近似相等,可將四輪差速模型簡(jiǎn)化為如圖4所示的兩輪差分模型,d為輪距的一半,r為車輛中心點(diǎn)到轉(zhuǎn)彎中心點(diǎn)的距離,因左輪和右輪轉(zhuǎn)彎時(shí)繞轉(zhuǎn)彎中心點(diǎ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)下,角速度測(cè)量精度也小于0.02°，加速度測(cè)量精度在運(yùn)動(dòng)過程中小于1 mGal。通過IMU測(cè)得車輛繞xyz旋轉(zhuǎn)的角速度,對(duì)三個(gè)角速度進(jìn)行積分,得到車輛的姿態(tài),以歐拉角進(jìn)行表示,具體表示為滾動(dòng)角θ_R、俯仰角θ_P和航向角θ_Y。則在世界坐標(biāo)系下,車輛的位移Lx、Ly和Lz分別為:

4 三維地圖構(gòu)建及狀態(tài)估計(jì)實(shí)驗(yàn)

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

hdl_graph_slam算法主要分為四個(gè)部分:

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

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

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

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

具體實(shí)驗(yàn)步驟如下:

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

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

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

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

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

5結(jié)論

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

[參考文獻(xiàn)]

[1]胡春旭,熊梟,任慰,等.基于嵌入式系統(tǒng)的室內(nèi)移動(dòng)機(jī)器人定位與導(dǎo)航[J].華中科技大學(xué)學(xué)報(bào)(自然科學(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篇