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

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

中圖分類號：TP242.3 文獻標志碼：A 文章編號：1671-0797（2024）19-0053-03

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

0 引言

本文主要介紹一種適用于非結(jié)構(gòu)環(huán)境的四輪差速無人車，然后使用其實現(xiàn)了三維地圖的建立和同時定位，并進行了相應的實驗。該無人車基于ROS（Robot

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

1 四輪差速無人車

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

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

2 里程計融合

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

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

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

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

四輪差速無人車采用四個編碼器分別采集四個車輪的轉(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為車輛中心點到轉(zhuǎn)彎中心點的距離，因左輪和右輪轉(zhuǎn)彎時繞轉(zhuǎ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組合導航的產(chǎn)品，它綜合利用了基于MEMS的慣性器件和GPS技術(shù)，即使在沒有GPS的狀態(tài)下，角速度測量精度也小于0.02°，加速度測量精度在運動過程中小于1 mGal。通過IMU測得車輛繞xyz旋轉(zhuǎn)的角速度，對三個角速度進行積分，得到車輛的姿態(tài)，以歐拉角進行表示，具體表示為滾動角θR、俯仰角θP和航向角θY。則在世界坐標系下，車輛的位移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)估計實驗

目前，有許多開源三維地圖構(gòu)建及同時定位的算法，比如LOAM、hdl_graph_slam等。筆者對hdl_graph_

slam算法進行了改進，原有算法使用單一的三維激光雷達點云進行抽?。―ownsample）后再進行幀間匹配，得出里程計。但這種方法需要大量的特征點進行匹配，還需要消耗大量的計算資源，而且在車速較高時會產(chǎn)生畸變;另外，經(jīng)多次實驗，用幀間匹配得到的里程計在z方向上的位移計算不準確。針對這種情況，改進為采用編碼器和IMU融合計算里程計。

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

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

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

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

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

具體實驗步驟如下：

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

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

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

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

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

5 結(jié)論

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

[參考文獻]

[1] 胡春旭，熊梟，任慰，等.基于嵌入式系統(tǒng)的室內(nèi)移動機器人定位與導航[J].華中科技大學學報（自然科學版），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è)計。