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

摘要：非結(jié)構(gòu)環(huán)境中的車輛因環(huán)境復(fù)雜，實(shí)時(shí)建圖與定位實(shí)現(xiàn)困難。四輪驅(qū)動(dòng)的實(shí)驗(yàn)車輛具有行駛穩(wěn)定、轉(zhuǎn)向方便等優(yōu)勢，鑒于此，利用車輛裝備的三維激光雷達(dá)和開源程序進(jìn)行實(shí)時(shí)定位與建圖的研究，對(duì)搜索和救援機(jī)器人，林業(yè)和采礦應(yīng)用中的自主作業(yè)裝備在進(jìn)行搜救、采集等作業(yè)時(shí)應(yīng)對(duì)復(fù)雜的非結(jié)構(gòu)化室外環(huán)境具有良好的指導(dǎo)意義。

關(guān)鍵詞：無人化;移動(dòng)機(jī)器人;三維重建

中圖分類號(hào)：TP242.3 文獻(xiàn)標(biāo)志碼：A 文章編號(hào)：1671-0797（2024）19-0053-03

DOI：10.19514/j.cnki.cn32-1628/tm.2024.19.012

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)Ubuntu16.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)。為簡化計(jì)算，本章在未特別注明使用世界坐標(biāo)系時(shí)，默認(rèn)使用車輛的自有坐標(biāo)系，如圖3所示。

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

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

vR=2πRnR（1）

vL=2πRnL（2）

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

ωL=ωR==（3）

式中：ωL、ωR為左輪角速度和右輪角速度。

用vL、vR和d表示r，可得式（4）：

r=（4）

則有式（5）：

ωR==（5）

車輛x方向的速度vx可由式（6）得出：

vx=ω×r=×=（6）

式中：ω為角速度。

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

Lx=

vx（cos θP+cos θY）dt，

Ly=

vx（sin θY+cos θR）dt，

Lz=

vx（sin θP+sin θR）dt（7）

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)行抽?。―ownsample）后再進(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）地面探測，設(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] NISTER D，NARODITSKY O，BERGEN J.Visual odometry for ground vehicle applications[J].Journal of Field Robotics，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-05-29

作者簡介：任衛(wèi)東（1982—），男，內(nèi)蒙古人，工程師，研究方向：汽車底盤開發(fā)設(shè)計(jì)。