梁 鋒、黃政華

（上汽通用五菱汽車股份有限公司，柳州 545007）

0 引言

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

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

1 無人物流車硬件架構

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

圖1 無人駕駛硬件系統(tǒng)架構圖

其中，車輛本體選用線控底盤技術較為成熟的E300 作為載體，激光雷達選用速騰聚創(chuàng)的16 線激光雷達，組合慣導選用導遠INS570D 組合慣導，控制器采用米文Apex Xavier Ⅱ（圖2）。控制器和傳感器安裝在車頂，硬件之間通過網(wǎng)線或者CAN 總線連接。

圖2 硬件組裝效果圖

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

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

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

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

圖3 建圖軟件系統(tǒng)架構圖

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

圖4 3D 點云圖

3 算法模塊

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

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

圖5 數(shù)據(jù)模塊

3.2 imu 預積分模塊

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

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

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

圖6 imu 預積分模塊

3.3 圖像映射模塊

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

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

圖7 圖像映射模塊

3.4 特征提取模塊

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

圖8 特征提取模塊

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

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

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

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

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

4 結束語

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