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

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

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

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

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

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

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

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

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

2.2" GPS定位

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

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

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

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

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

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

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

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

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

2.6" 交通信號(hào)燈和障礙物的識(shí)別

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

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

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

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

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

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

4" 結(jié)束語

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

參考文獻(xiàn)：

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

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

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

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

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