基于GPS與激光雷達(dá)的無人駕駛組合導(dǎo)航研究

摘" 要：為進(jìn)一步解決無人駕駛小車在行駛過程中的GPS定位信號容易受建筑物和天氣等因素影響而導(dǎo)致定位精度不準(zhǔn)問題，該文提出基于GPS和激光雷達(dá)的無人駕駛組合導(dǎo)航系統(tǒng)。該系統(tǒng)采用卡爾曼濾波與低通濾波融合算法提高GPS定位，以及結(jié)合激光雷達(dá)的實地建模及精準(zhǔn)定位，系統(tǒng)根據(jù)設(shè)定的目的地結(jié)合地圖規(guī)劃出一條安全且可執(zhí)行的最佳路線，并采用PID算法和卡爾曼濾波算法控制電機(jī)來實現(xiàn)無人駕駛小車勻速行駛和里程計的穩(wěn)定運行。測試結(jié)果表明，基于GPS和激光雷達(dá)的無人駕駛組合導(dǎo)航系統(tǒng)可提高小車的定位精度和行駛靈敏度，實現(xiàn)精準(zhǔn)定位和導(dǎo)航。

關(guān)鍵詞：無人駕駛；GPS；激光雷達(dá)；定位；導(dǎo)航

中圖分類號：U463.6" " "文獻(xiàn)標(biāo)志碼：A" " " " " 文章編號：2095-2945（2023）16-0031-04

Abstract： In order to further solve the problem that the positioning accuracy of the GPS positioning signal of a driverless car is easily affected by buildings， weather and other factors， a driverless integrated navigation system based on GPS and LiDAR is proposed in this paper. The system uses Kalman filter and low-pass filter fusion algorithm to improve GPS positioning， as well as LiDAR field modeling and accurate positioning. The system plans a safe and executable best route according to the set destination combined with the map， and uses PID algorithm and Kalman filter algorithm to control the motor to realize the driverless car driving at a uniform speed and the stable operation of the odometer. The test results show that the driverless integrated navigation system based on GPS and LiDAR can improve the positioning accuracy and driving sensitivity of the car and realize accurate positioning and navigation.

Keywords： self-driving; GPS; LiDAR; positioning; navigation

GPS和即時定位與地圖構(gòu)建（SLAM）是當(dāng)前使用最多的2種基于未知地圖的高精度導(dǎo)航。GPS依靠導(dǎo)航終端接收并處理衛(wèi)星的導(dǎo)航電文信息并根據(jù)定位幾何關(guān)系確定無人車的絕對位置，GPS具有不受時間限制、氣候影響，且在空曠的室外環(huán)境中實時定位較精準(zhǔn)的特點，但易受高大建筑物、樹木和隧道等的影響，且長時間的GPS信號缺失會導(dǎo)致航跡推算的累積誤差增大，增加駕駛的危險性[1-2]。

SLAM定位以激光和光學(xué)SLAM為主，激光雷達(dá)具有不受外界信號干擾和受環(huán)境干擾影響較小的特點，適用于定位精度要求高的應(yīng)用場景。本文采用二維激光雷達(dá)對無人駕駛障礙物進(jìn)行精準(zhǔn)測量，激光雷達(dá)通過激光從發(fā)射到經(jīng)過障礙目標(biāo)反射后接收的時間計算距離，并根據(jù)特征點匹配和環(huán)境的相對運動結(jié)合匹配確定小車的相對運動距離，受環(huán)境影響較?。煌瑫r，二維激光雷達(dá)可對周圍環(huán)境進(jìn)行實時地圖構(gòu)建，壓縮所構(gòu)建地圖大小，提高定位搜索算法的運行速度，提高計算機(jī)的運行性能，相對于三維激光雷達(dá)，二維激光雷達(dá)成本較低[1，3]。為進(jìn)一步解決無人駕駛小車在行駛過程中GPS定位信號容易受建筑物和天氣等因素影響而導(dǎo)致定位精度不準(zhǔn)的問題，本文結(jié)合GPS的粗定位和二維激光雷達(dá)的精定位來實現(xiàn)無人駕駛的自主導(dǎo)航。其中，采用卡爾曼濾波與低通濾波融合算法對GPS定位采集的信息進(jìn)行糾正，采用二維激光雷達(dá)實現(xiàn)實地建模及精準(zhǔn)定位，進(jìn)而提高小車的定位精度，實現(xiàn)無人駕駛的精準(zhǔn)定位和自主導(dǎo)航。

1" 系統(tǒng)總體結(jié)構(gòu)設(shè)計

系統(tǒng)以微型計算機(jī)控制為核心，結(jié)合圖像采集模塊、激光雷達(dá)模塊、IMU 姿態(tài)傳感器、動力驅(qū)動模塊、GPS模塊、5G模塊和STM32微控制器來實現(xiàn)自主導(dǎo)航。系統(tǒng)利用圖像采集模塊對周圍環(huán)境進(jìn)行圖像采集與識別，通過GPS和激光雷達(dá)模塊進(jìn)行粗/精定位，利用雷達(dá)感應(yīng)模塊感知周圍障礙物并采集障礙物的數(shù)據(jù)信息；微型計算機(jī)將采集到的周圍環(huán)境、障礙物信息進(jìn)行SLAM算法處理，進(jìn)而給單片機(jī)下達(dá)行駛指令，單片機(jī)控制動力模塊調(diào)節(jié)車速以及控制IMU姿態(tài)傳感器精準(zhǔn)控制小車行駛方向，單片機(jī)根據(jù)微型計算機(jī)的ROS系統(tǒng)所提供的路況信息與實時行駛反饋數(shù)據(jù)微調(diào)行駛路線，進(jìn)而實現(xiàn)自主導(dǎo)航功能。同時，微型計算機(jī)通過5G模塊實現(xiàn)通信傳輸網(wǎng)絡(luò)的連接，并將采集到的圖像信息傳輸至ROS master（節(jié)點管理中心），通過無線網(wǎng)絡(luò)傳輸至手機(jī)端進(jìn)行圖像實時監(jiān)控，系統(tǒng)結(jié)構(gòu)框圖如圖1所示。

2" 系統(tǒng)軟件設(shè)計

2.1" 軟件總體框架設(shè)計

系統(tǒng)軟件總體設(shè)計流程如圖2所示，系統(tǒng)上電開始運行，程序進(jìn)入初始化狀態(tài)，等待GPS信號正常后，系統(tǒng)激活激光雷達(dá)開始工作，等檢測正常后，多種采集設(shè)備采集環(huán)境數(shù)據(jù)并將其傳輸至單片機(jī)，單片機(jī)設(shè)定好目的地后將數(shù)據(jù)傳輸給樹莓派，樹莓派將目的點和小車目前位置進(jìn)行數(shù)據(jù)整合，規(guī)劃最合理路線，并結(jié)合角速度傳感器將最終小車前進(jìn)方向指令下達(dá)給單片機(jī)。在行駛過程中，小車使用視覺模塊實時識別行人、車輛、車道線和道路標(biāo)識等，使用激光雷達(dá)進(jìn)行測量，明確小車與障礙物之間的距離從而達(dá)到避障效果，使用角度傳感器進(jìn)行小車姿態(tài)檢測，從而避免小車出現(xiàn)偏離車道線、拐彎時偏幅過大等情況。

2.2" GPS定位

GPS模塊采用NMEA協(xié)議，對GPS發(fā)送回來的數(shù)據(jù)進(jìn)行解析，GPS在定位成功時發(fā)送的數(shù)據(jù)之間存在明顯的時間間隔問題，需采用該時間隔作為判斷當(dāng)前接收的數(shù)據(jù)是否是一個完整數(shù)據(jù)流。為了保證數(shù)據(jù)接收的完整性和減少系統(tǒng)的壓力，在連接GPS模塊的串口時要采用DMA加空閑中斷，當(dāng)串口觸發(fā)空閑中斷時先對數(shù)據(jù)進(jìn)行緩存再打開DMA中斷，再對緩存中的經(jīng)緯度和速度數(shù)據(jù)進(jìn)行原始數(shù)據(jù)處理。GPS模塊獲得的經(jīng)緯度坐標(biāo)只是原始坐標(biāo)系，會受到大氣層中電離層和對流層對GPS信號的延遲、多徑效應(yīng)等影響，導(dǎo)致出現(xiàn)位置漂移和位置偏差現(xiàn)象。在實際定位過程中，不可避免地會出現(xiàn)誤差過大現(xiàn)象，從而影響定位的精度，使得定位位置產(chǎn)生漂移現(xiàn)象。為減少上述問題出現(xiàn)的次數(shù)，引入卡爾曼濾波算法與低通濾波算法的融合算法，在原始數(shù)據(jù)上采用卡爾曼濾波算法進(jìn)行糾正，糾正后的數(shù)據(jù)相對穩(wěn)定些，但難免也會出現(xiàn)一些位置坐標(biāo)與實際偏差較大的情況，因此要在卡爾曼濾波算法的后面加上去噪處理，把突變的數(shù)據(jù)進(jìn)行去除。

卡爾曼濾波算法通過建立狀態(tài)空間模型，根據(jù)上一時刻預(yù)測的當(dāng)前時刻的理想值和當(dāng)前時刻所測量到的值進(jìn)行對比分析，以最小均方誤差作為最佳值對這2個值進(jìn)行估計，進(jìn)而更快修正位置誤差并使得誤差趨于穩(wěn)定收斂狀態(tài)。對卡爾曼濾波輸出的數(shù)據(jù)進(jìn)行低通濾波處理，通過設(shè)定一個變化量，當(dāng)采集到的坐標(biāo)變化量高于這個變化量時則不能通過，在實際的GPS坐標(biāo)變化過程中，變化量超過額定的變化量時則被阻隔、減弱。低通濾波算法可以解決長期可靠、短期噪聲大的數(shù)據(jù)，并可有效地實現(xiàn)噪聲的濾除。在原始數(shù)據(jù)上采用卡爾曼濾波與低通濾波融合算法進(jìn)行糾正和濾波，可減少位置漂移和偏差對定位精度的影響，糾正后的數(shù)據(jù)即為最終定位信息，提高了數(shù)據(jù)的精準(zhǔn)性。

2.3" 自主導(dǎo)航區(qū)域信息與路徑采集

導(dǎo)航區(qū)域選擇一個開闊的地點，有利于GPS的定位。如圖3所示，在開始定位時要先對導(dǎo)航區(qū)域的地圖信息進(jìn)行存儲，即先用經(jīng)緯度信息把可以前進(jìn)區(qū)域的經(jīng)緯度進(jìn)行畫線標(biāo)記，然后再在可前進(jìn)的區(qū)域里規(guī)劃出一些節(jié)點。

得到小車的初始位置與目的點后分別連接最近節(jié)點，2節(jié)點之間連接進(jìn)而規(guī)劃出一條安全且路徑最短的前進(jìn)路線。當(dāng)小車行駛在某一節(jié)點上時則可以向下一個節(jié)點行駛；當(dāng)小車不在節(jié)點上行駛時，小車先行駛到規(guī)劃好的路線節(jié)點上，然后再向下個節(jié)點行駛；當(dāng)小車行駛因躲避障礙物而脫離節(jié)點時，樹莓派可以根據(jù)實時定位信息使小車調(diào)整行駛方向確保行駛到下一個節(jié)點，進(jìn)而按照規(guī)劃路線到達(dá)目的地。

2.4" 激光雷達(dá)圖的創(chuàng)建

激光雷達(dá)正常啟動后，使用ROS操作系統(tǒng)自帶功能包可以直接對激光雷達(dá)傳回來的數(shù)據(jù)進(jìn)行運算，可以得到物體與激光雷達(dá)的角度和距離。先將激光雷達(dá)的周圍劃分為前、后、左和右4個區(qū)域，當(dāng)激光雷達(dá)旋轉(zhuǎn)完一圈后分別對4個區(qū)域的點進(jìn)行遍歷，當(dāng)在區(qū)域內(nèi)遍歷到最近點時則將數(shù)據(jù)發(fā)送給STM32單片機(jī)，STM32單片機(jī)將回傳的數(shù)據(jù)按照區(qū)域和距離進(jìn)行劃分判讀，分別在顯示屏上顯示出來，并根據(jù)距離判斷遇到障礙物時是否進(jìn)行躲避，增大自動避障的準(zhǔn)確度。二維激光雷達(dá)圖如圖4所示。

2.5" 數(shù)據(jù)采集與發(fā)送

小車啟動后，系統(tǒng)對各電路模塊進(jìn)行檢測，檢測一切正常后，單片機(jī)向樹莓派發(fā)送開始信號。接收到開始信號的樹莓派向各設(shè)備發(fā)出檢測周圍環(huán)境的信號。GPS、激光雷達(dá)、方向傳感器等設(shè)備接收到信號后不停地對周圍的環(huán)境進(jìn)行數(shù)據(jù)采集，其采集到數(shù)據(jù)后，在一定的時間間隔內(nèi)不停地將采集的環(huán)境數(shù)據(jù)傳給樹莓派。樹莓派接收到數(shù)據(jù)后，將數(shù)據(jù)整合構(gòu)建出二維雷達(dá)地圖，并將其上傳至單片機(jī)。單片機(jī)接收到樹莓派上傳的地圖后，根據(jù)設(shè)定的目的地結(jié)合地圖規(guī)劃出一條安全且可執(zhí)行的最佳路線，并采用PID算法和卡爾曼濾波算法控制電機(jī)來實現(xiàn)無人駕駛小車勻速行駛和里程計的穩(wěn)定運行。

2.6" 交通信號燈和障礙物的識別

實際道路中的信號燈場景往往比較復(fù)雜，小車在行駛中對信號燈進(jìn)行識別時容易出現(xiàn)誤判現(xiàn)象，針對該誤判現(xiàn)象，系統(tǒng)采用GPS結(jié)合攝像頭的方案對交通信號燈進(jìn)行識別。攝像頭運行后，對每一幀圖片讀取分析，如果發(fā)現(xiàn)圖片有紅色信號燈時，會發(fā)送停止指令給STM32單片機(jī)；如果發(fā)現(xiàn)有車輛時開始標(biāo)記；如果發(fā)現(xiàn)有車道線時開始跟著車道線前進(jìn)，并等待紅綠燈。攝像頭識別流程圖如圖5所示。

系統(tǒng)先對所構(gòu)建的地圖中的所有交通燈區(qū)域進(jìn)行規(guī)劃，當(dāng)小車行駛到交通信號燈的區(qū)域范圍內(nèi)時，系統(tǒng)啟動交通燈識別功能并降低小車行駛速度，使其行駛到人行橫道標(biāo)線前，然后通過攝像頭對前方信號燈的色彩特點、圖形學(xué)特點及彩色視覺RGB顏色空間等特征進(jìn)行判定，當(dāng)系統(tǒng)在圖像中尋找到興趣區(qū)域后，為消除噪聲、光照等因素的干擾，會先對采集的興趣區(qū)域進(jìn)行直方圖均衡化，接著對興趣區(qū)域的長度、寬度、長寬比等基本幾何特征進(jìn)行提取，最后對信號燈區(qū)域判定與識別，如果識別出來的是綠色，小車則結(jié)合雷達(dá)判斷前面的人行道是否存在過往行人，在綠燈及無行人的安全情況下，小車緩慢行駛出路口；否則小車將停在人行橫道標(biāo)線前，等待判定與識別到興趣區(qū)域中的綠色信號后才可以正常行駛通過路口。

小車在行駛過程中，需要對障礙物進(jìn)行檢測實現(xiàn)避障功能，對前方路況進(jìn)行識別主要分為車類障礙物和非車類障礙物識別，系統(tǒng)通過激光雷達(dá)獲取障礙物的基本形貌、距離、位置等信息[4-5]，先對獲取的RGB圖片進(jìn)行二值化處理，然后對一些比較小的白色像素塊進(jìn)行腐蝕處理，進(jìn)而去除一些小的噪點；為解決運動中的物體容易被腐蝕處理的問題，需對圖面進(jìn)行膨脹處理，把目標(biāo)圖像增大至一定比例后用于邊緣檢測。如果檢測出的目標(biāo)為車輛，并且對2次采集的車輛速度進(jìn)行結(jié)算，如果對方車輛的速度低于本小車并且對面車道無小車時，系統(tǒng)控制小車加速超越；如果是非車輛障礙物則進(jìn)行變道處理。

3" 系統(tǒng)測試分析

選取車流量較少的公路主干道作為測試場地，小車可以正常識別出發(fā)的經(jīng)緯度，但定位的時間需要很長，其原因是對定位坐標(biāo)進(jìn)行卡爾曼濾波算法和去噪算法處理所帶來的影響。系統(tǒng)定位當(dāng)前小車所在的經(jīng)緯度作為出發(fā)點，通過小車上自帶的搖桿控制設(shè)置目的點，當(dāng)移動到目的點后，按下?lián)u桿則目的點設(shè)置成功。當(dāng)設(shè)置成功時，地圖上會顯示目的點和出發(fā)點的經(jīng)緯度，并有聲音播報提示。

按下出發(fā)按鍵后，系統(tǒng)采用PID算法和卡爾曼濾波算法控制電機(jī)，使小車不受外界因素的影響可以在不同天氣環(huán)境下，實現(xiàn)勻速行駛和里程計的穩(wěn)定運行。同時，為了實現(xiàn)自動將數(shù)據(jù)完整和快速發(fā)送出去的目的，微型計算機(jī)通過5G模塊，連接移動通信5G網(wǎng)絡(luò)，打開自動撥號和后臺自啟動功能，正常連接后，微型計算機(jī)將image話題通過ROS master發(fā)布到公網(wǎng)上，手機(jī)監(jiān)控端通過連接公網(wǎng)后向服務(wù)器訂閱image話題，小車將攝像頭采集到的視頻信息通過NBIOT發(fā)送到運營商，由運營商發(fā)送到互聯(lián)網(wǎng)上，最后遠(yuǎn)程端通過IP地址獲取小車發(fā)送的數(shù)據(jù)。同時，小車根據(jù)節(jié)點和定位信息確定行駛狀態(tài)并把圖像發(fā)送到手機(jī)端。經(jīng)過GPS的粗定位和激光雷達(dá)的精定位及系統(tǒng)最佳路線的規(guī)劃后，小車能自動駕駛到目的地，但跟設(shè)置的目的點誤差為1～2 m，該誤差較小進(jìn)而可以忽略，這樣就基本上實現(xiàn)了無人小車的精準(zhǔn)導(dǎo)航和定位，說明本次系統(tǒng)測試是成功的。

4" 結(jié)束語

為解決GPS信號會受建筑物及天氣等因素影響和提高自動導(dǎo)航系統(tǒng)定位的精度，本研究提出了基于GPS和激光雷達(dá)的無人駕駛組合導(dǎo)航設(shè)計方案。系統(tǒng)結(jié)合GPS和激光雷達(dá)定位技術(shù)，利用GPS模塊獲取經(jīng)緯度原始數(shù)據(jù)，并采用卡爾曼濾波與低通濾波融合算法來提高數(shù)據(jù)的精準(zhǔn)性，再結(jié)合激光雷達(dá)精準(zhǔn)定位和障礙物的檢測可以實現(xiàn)避障功能，樹莓派將接收到的數(shù)據(jù)整合構(gòu)建出二維雷達(dá)地圖，單片機(jī)根據(jù)設(shè)定的目的地結(jié)合地圖規(guī)劃出一條安全且可執(zhí)行的最佳路線，使小車在導(dǎo)航時可以根據(jù)規(guī)劃路線自動行駛。確定初始和目標(biāo)位置后，小車可以在無人工參與的情況下自動駕駛到目的地，可大大減少因駕駛員的疏忽大意和操作不當(dāng)所引起的交通事故次數(shù)。

參考文獻(xiàn)：

[1] 陳麗.基于激光雷達(dá)與GPS多傳感器融合的無人駕駛重定位研究與實現(xiàn)[D].成都：西南交通大學(xué)，2021.

[2] 李晨曦，張軍，靳欣宇，等.激光雷達(dá)SLAM技術(shù)及其在無人車中的應(yīng)用研究進(jìn)展[J].北京聯(lián)合大學(xué)學(xué)報，2017，31（4）：61-69.

[3] 陳鋼.基于激光雷達(dá)的無人駕駛障礙物檢測與辨識探析[J].華東科技，2022（6）：128-130.

[4] 龐辰耀，吳平，翁德華，等.基于ROS與激光SLAM自主導(dǎo)航與避障系統(tǒng)的設(shè)計[J].數(shù)字技術(shù)與應(yīng)用，2021，39（11）：214-218.

[5] 陶倩文.基于車載激光雷達(dá)與GPS數(shù)據(jù)融合的智能車高精度定位方法[D].武漢：武漢理工大學(xué)，2019.