梁鋒 黃政華

摘要：在無(wú)人駕駛技術(shù)的推廣和商業(yè)化過(guò)程中，SLAM（實(shí)時(shí)定位與建圖）技術(shù)在整個(gè)無(wú)人駕駛技術(shù)鏈已不可或缺。文章基于激光SLAM 技術(shù)，搭載激光雷達(dá)+ 組合慣導(dǎo)+ 控制器等硬件設(shè)備，在封閉廠區(qū)道路內(nèi)，研究無(wú)人物流車的多傳感器融合的實(shí)時(shí)定位與建圖技術(shù)，實(shí)現(xiàn)無(wú)人物流車采集和創(chuàng)建高精度點(diǎn)云地圖。

關(guān)鍵詞：無(wú)人駕駛技術(shù)；激光SLAM ；點(diǎn)云地圖

中圖分類號(hào)：TP391.7 文獻(xiàn)標(biāo)識(shí)碼：A

0 引言

近年來(lái)，隨著物聯(lián)網(wǎng)技術(shù)、無(wú)人駕駛、感知控制算法及傳感器不斷發(fā)展成熟，無(wú)人駕駛技術(shù)不斷涌入物流、公共交通、出租車和餐飲配送等行業(yè)，逐漸展現(xiàn)出智能移動(dòng)機(jī)器人替換傳統(tǒng)人工的新興景象[1]。為達(dá)到物流行業(yè)無(wú)人配送目的，本文對(duì)牽引車進(jìn)行改造升級(jí)，如增加感知定位傳感器、電子制動(dòng)、駐車、轉(zhuǎn)向控制器以及大算力的智駕控制器等，通過(guò)CAN 總線，智駕控制系統(tǒng)與車輛控制系統(tǒng)進(jìn)行交互。升級(jí)改造后的車輛，通過(guò)智駕感知傳感器，車輛可自行感知周圍環(huán)境和車輛當(dāng)前位置、位姿，通過(guò)循跡算法，自動(dòng)控制車輛轉(zhuǎn)向、驅(qū)動(dòng)電機(jī)工作，按既定點(diǎn)云地圖上的路線前往指定位置，最終實(shí)現(xiàn)無(wú)人物流配送功能[2]。

在整個(gè)智駕系統(tǒng)，車輛的感知定位及循跡算法均依賴于提前采集和創(chuàng)建的高精度地圖數(shù)據(jù)。地圖創(chuàng)建的可靠性，直接影響車輛運(yùn)行時(shí)的匹配準(zhǔn)確性，進(jìn)而決定車輛能否完成智駕功能[3]。

1 無(wú)人物流車硬件架構(gòu)

無(wú)人駕駛的快速發(fā)展離不開(kāi)控制器和智能傳感器的不斷發(fā)展，只有豐富的環(huán)境感知數(shù)據(jù)和高算力的控制器，才能支撐整個(gè)智駕系統(tǒng)所有算法模塊均在毫秒級(jí)內(nèi)完成相應(yīng)感知定位和規(guī)控任務(wù)[4]。本文硬件系統(tǒng)主要包括車輛載體、智駕控制器、組合慣導(dǎo)和3D 激光雷達(dá)（圖1）。

其中，車輛本體選用線控底盤(pán)技術(shù)較為成熟的E300 作為載體，激光雷達(dá)選用速騰聚創(chuàng)的16 線激光雷達(dá)，組合慣導(dǎo)選用導(dǎo)遠(yuǎn)INS570D 組合慣導(dǎo)，控制器采用米文Apex Xavier Ⅱ（圖2）?？刂破骱蛡鞲衅靼惭b在車頂，硬件之間通過(guò)網(wǎng)線或者CAN 總線連接。

由于生產(chǎn)制造工藝、物流運(yùn)輸、使用環(huán)境變化以及組裝過(guò)程影響，精密儀器的實(shí)際使用直接輸出值與理論值均存在一定偏差，為此，在組合慣導(dǎo)、激光雷達(dá)等傳感器使用之前，均需對(duì)所有傳感器進(jìn)行標(biāo)定。組合慣導(dǎo)標(biāo)定參考導(dǎo)遠(yuǎn)官方標(biāo)定手冊(cè)標(biāo)定即可，而激光雷達(dá)標(biāo)定，需借助開(kāi)源“IMU+Lidar”聯(lián)合標(biāo)定軟件完成標(biāo)定工作，最終得到激光雷達(dá)與慣導(dǎo)間旋轉(zhuǎn)平移關(guān)系矩陣以及載體當(dāng)前的橫滾角、俯仰角和偏航角，為建圖時(shí)的坐標(biāo)統(tǒng)一提供轉(zhuǎn)換支撐。

2 軟件系統(tǒng)架構(gòu)

為了將所有精力集中到功能模塊的開(kāi)發(fā)上，提高開(kāi)發(fā)效率，本系統(tǒng)軟件設(shè)計(jì)基于Ubuntu18.04 操作系統(tǒng)，通訊架構(gòu)成熟穩(wěn)定；分布式的ROS（Robot Operating System，ROS）中間框架，利用ROS 通訊機(jī)制，實(shí)現(xiàn)各個(gè)算法模塊間的數(shù)據(jù)通訊。實(shí)時(shí)定位與建圖的實(shí)現(xiàn)主要需要激光點(diǎn)云、GPS 和IMU 數(shù)據(jù)[5]。

激光雷達(dá)采用UDP 協(xié)議通訊，通過(guò)網(wǎng)線與控制器連接，集成對(duì)應(yīng)型號(hào)的雷達(dá)驅(qū)動(dòng)后，可從系統(tǒng)發(fā)布的話題中獲取激光雷達(dá)的實(shí)時(shí)點(diǎn)云數(shù)據(jù)。如速騰16 線的雷達(dá)RS_LIDAR_16 對(duì)應(yīng)輸出的話題為/rslidar_points。對(duì)于GPS 位置、IMU 數(shù)據(jù)，通過(guò)集成CAN 驅(qū)動(dòng)，把解析后的CAN 報(bào)文轉(zhuǎn)換成ROS 系統(tǒng)Subscribe/Publish 通訊模式，發(fā)布到系統(tǒng)中，供其他功能模塊訂閱使用。本系統(tǒng)軟件架構(gòu)設(shè)計(jì)如圖3 所示。

系統(tǒng)主要由數(shù)據(jù)轉(zhuǎn)換、imu 預(yù)積分、點(diǎn)云映射、特征提取和地圖數(shù)據(jù)融合優(yōu)化等幾個(gè)模塊組成。數(shù)據(jù)轉(zhuǎn)換模塊car_driver從CAN 總線獲取imu、gps 數(shù)據(jù)，通過(guò)CAN 協(xié)議解析，轉(zhuǎn)換成ROS 通訊話題后發(fā)布到系統(tǒng)中，供其他算法模塊訂閱使用。imu預(yù)積分模塊imuPreintegration 訂閱/imu 數(shù)據(jù)進(jìn)行預(yù)積分后，發(fā)布imu 實(shí)時(shí)里程數(shù)據(jù)。點(diǎn)云映射模塊imageProjection 主要用于激光點(diǎn)云數(shù)據(jù)處理，發(fā)布矯正后的點(diǎn)云數(shù)據(jù)信息。特征提取模塊featureExtraction 主要是提取有效點(diǎn)云的特征數(shù)據(jù)。地圖優(yōu)化融合模塊訂閱點(diǎn)云信息及gps 數(shù)據(jù)進(jìn)行計(jì)算融合，并發(fā)布雷達(dá)里程計(jì)數(shù)據(jù)，最后將關(guān)鍵幀數(shù)據(jù)集合發(fā)布到地圖保存服務(wù)，通過(guò)PCL點(diǎn)云庫(kù)保存成我們需要的pcd 文件（圖4）。

3 算法模塊

3.1 數(shù)據(jù)轉(zhuǎn)換模塊

整個(gè)駕控系統(tǒng)中，與車輛CAN 總線進(jìn)行數(shù)據(jù)、命令交互的模塊是car_driver。該模塊的一個(gè)功能是通過(guò)集成第三方CAN 動(dòng)態(tài)庫(kù)，將特定波特率的CAN 數(shù)據(jù)讀取到駕控系統(tǒng)，并根據(jù)協(xié)議解析指定幀頭的幀數(shù)據(jù)，獲取到我們需要的實(shí)時(shí)數(shù)據(jù)；同時(shí)，將數(shù)據(jù)按ROS 系統(tǒng)通訊架構(gòu)，發(fā)布到駕控系統(tǒng)中，如imu 數(shù)據(jù)/imu，gps 數(shù)據(jù)/gps/fix。car_driver 的另一個(gè)功能就是解析需要下發(fā)到車輛控制執(zhí)行器的的話題，將話題轉(zhuǎn)換成CAN 報(bào)文，發(fā)布到CAN 總線（圖5）。

3.2 imu 預(yù)積分模塊

預(yù)積分模塊主要由兩部分構(gòu)成：一部分是訂閱及處理i m u 數(shù)據(jù)，另一部分是訂閱及處理激光里程計(jì)數(shù)據(jù)。

imu 處理過(guò)程主要是訂閱實(shí)時(shí)imu 數(shù)據(jù)，通過(guò)雷達(dá)與imu的外參矩陣把imu 數(shù)據(jù)轉(zhuǎn)換到雷達(dá)坐標(biāo)系后保存到消息隊(duì)列中。同時(shí)，利用一個(gè)全局標(biāo)志，控制系統(tǒng)是否繼續(xù)將imu 數(shù)據(jù)添加到積分器中進(jìn)行預(yù)積分，并結(jié)合上一幀優(yōu)化后的數(shù)據(jù)完成當(dāng)前狀態(tài)的預(yù)測(cè)，發(fā)布當(dāng)前里程計(jì)狀態(tài)數(shù)據(jù)。

文中雷達(dá)數(shù)據(jù)頻率為10 Hz，imu 數(shù)據(jù)頻率是200 Hz，在雷達(dá)里程計(jì)數(shù)據(jù)還未完成優(yōu)化并預(yù)測(cè)最新位置時(shí)，默認(rèn)imu 數(shù)據(jù)直接添加到積分器中進(jìn)行預(yù)積分。當(dāng)完成當(dāng)前狀態(tài)優(yōu)化后，記錄優(yōu)化后的位置、狀態(tài)及偏差值。優(yōu)化的過(guò)程主要是借助GTSAM 因子圖庫(kù)，將所有約束都添加到因子圖中，執(zhí)行優(yōu)化操作（圖6）。

3.3 圖像映射模塊

圖像映射模塊主要功能是處理激光點(diǎn)云數(shù)據(jù)，通過(guò)訂閱imu數(shù)據(jù)以及imu 里程計(jì)數(shù)據(jù)，根據(jù)激光點(diǎn)云當(dāng)前幀的起止時(shí)間，對(duì)imu 及imu 里程計(jì)數(shù)據(jù)計(jì)算起止時(shí)間的狀態(tài)變化量。

點(diǎn)云處理過(guò)程是遍歷當(dāng)前點(diǎn)云幀數(shù)據(jù)，利用每個(gè)點(diǎn)的位置（x、y、z 值）計(jì)算出所有點(diǎn)與原點(diǎn)的的距離，過(guò)濾部分有問(wèn)題或距離不符合需求的點(diǎn)云。同時(shí)，通過(guò)imu 數(shù)據(jù)計(jì)算得到的旋轉(zhuǎn)矩陣和imu 里程計(jì)數(shù)據(jù)計(jì)算平移矩陣，將所有過(guò)濾后的點(diǎn)云都轉(zhuǎn)換到第一個(gè)激光點(diǎn)的坐標(biāo)系下，進(jìn)行運(yùn)動(dòng)補(bǔ)償。最后將具有距離的所有激光點(diǎn)云都統(tǒng)一保存到點(diǎn)云信息結(jié)構(gòu)中，發(fā)布經(jīng)過(guò)矯正及過(guò)濾后的云信息（圖7）。

3.4 特征提取模塊

特征提取模塊主要是處理經(jīng)過(guò)運(yùn)動(dòng)畸變矯正、過(guò)濾無(wú)效點(diǎn)云后的點(diǎn)云數(shù)據(jù)。首先是訂閱去畸變后的有效點(diǎn)云信息，通過(guò)計(jì)算點(diǎn)云曲率，并以曲率為基礎(chǔ)再次過(guò)濾掉與激光線曲率或者被遮擋的點(diǎn)。之后，再對(duì)剩余的點(diǎn)云進(jìn)行分割提取平面和角點(diǎn)，保存到點(diǎn)云信息中。最后發(fā)布角點(diǎn)、面點(diǎn)點(diǎn)云信息（圖8）。

3.5 地圖優(yōu)化模塊

地圖優(yōu)化模塊主要功能是根據(jù)當(dāng)前幀和當(dāng)前幀時(shí)空上較近的全局地圖匹配，獲取到當(dāng)前幀一個(gè)初步的位姿。最終通過(guò)融合lidar 里程計(jì)的因子、GPS 因子和閉環(huán)因子，獲得一個(gè)準(zhǔn)確的位姿，同時(shí)更新所有歷史關(guān)鍵幀的位姿，最后把優(yōu)化后的數(shù)據(jù)集發(fā)布出去（圖9）。

要完成當(dāng)前幀到附近全局地圖的匹配并獲取到較好的位姿，首先要根據(jù)當(dāng)前關(guān)鍵幀位置搜索最近的地圖關(guān)鍵幀集合，提取角點(diǎn)、平面點(diǎn)等特征，加入局部地圖。其次，對(duì)當(dāng)前幀角點(diǎn)、平面點(diǎn)進(jìn)行降采樣。最后，執(zhí)行迭代優(yōu)化，即對(duì)關(guān)鍵幀特征點(diǎn)進(jìn)行匹配，并將匹配上了的點(diǎn)集都加入同一集合。同時(shí)構(gòu)建高斯牛頓方程，迭代優(yōu)化并同步更新當(dāng)前位姿，從而完成幀到地圖的匹配過(guò)程，獲取當(dāng)前較好的位姿[6]。

在獲取到當(dāng)前幀位姿后，計(jì)算與上一關(guān)鍵幀的變化量，以確定是否添加當(dāng)前幀為關(guān)鍵幀。同時(shí)，將約束條件都加到因子圖優(yōu)化中，獲取當(dāng)前幀優(yōu)化后姿態(tài)數(shù)據(jù)并執(zhí)行更新操作。最后將優(yōu)化后的數(shù)據(jù)集向外發(fā)布，并利用點(diǎn)云庫(kù)保存全局點(diǎn)云地圖，即可完成數(shù)據(jù)集保存及地圖創(chuàng)建。

4 結(jié)束語(yǔ)

本文基于車載駕控硬件及建圖算法，研究無(wú)人物流車的多傳感器融合的實(shí)時(shí)定位與建圖技術(shù)，實(shí)現(xiàn)高精度點(diǎn)云地圖創(chuàng)建。在整個(gè)系統(tǒng)運(yùn)行中，由于點(diǎn)云數(shù)據(jù)量大且有實(shí)時(shí)性的要求，對(duì)控制器配置要求較高，配置過(guò)低，有出現(xiàn)卡滯或者宕機(jī)可能。在點(diǎn)云關(guān)鍵幀與地圖配準(zhǔn)過(guò)程中，需要較多特征點(diǎn)進(jìn)行比對(duì)，故針對(duì)空曠且周圍缺少固定特征的場(chǎng)景，定位結(jié)果容易有偏差。為解決該問(wèn)題，可在此基礎(chǔ)上增加視覺(jué)傳感器，融合圖像數(shù)據(jù)，使得提取到的關(guān)鍵特征數(shù)據(jù)有所增加，提高系統(tǒng)冗余性與穩(wěn)定性。

【參考文獻(xiàn)】

[1] 高翔， 張濤， 劉毅， 等. 視覺(jué)SLAM 十四講： 從理論到實(shí)踐[M]. 北京： 電子工業(yè)出版社，2019.

[2] 曾俊皓. 基于ROS 的移動(dòng)機(jī)器人系統(tǒng)平臺(tái)的設(shè)計(jì)與實(shí)現(xiàn)[D]. 廣州： 華南理工大學(xué)，2018.

[3] Frank DellaertF， Michael Kaess. 機(jī)器人感知- 因子圖在SLAM 中的應(yīng)用[M]. 劉富強(qiáng)， 董靖， 譯. 北京： 電子工業(yè)出版社.2018

[4] 蔣燕. 基于多傳感器信息融合的同步定位與地圖創(chuàng)建研究[D]. 湘潭： 湘潭大學(xué)，2011.

[5] 許輝. 基于多傳感器融合的機(jī)器人建圖與導(dǎo)航定位技術(shù)研究[D]. 西安：長(zhǎng)安大學(xué)，2021.

[6] 張鐘元. 室外物流配送機(jī)器人的多傳感器融合同步定位與建圖技術(shù)[D].上海： 上海交通大學(xué).2019.

作者簡(jiǎn)介：

梁鋒，本科，工程師，研究方向?yàn)橹悄荞{駛、無(wú)人物流。