李中道，劉元盛，常飛翔，張 軍，路 銘

1.北京聯(lián)合大學 智慧城市學院，北京 100101

2.北京聯(lián)合大學 機器人學院，北京 100101

3.北京聯(lián)合大學 北京市信息服務工程重點實驗室，北京 100101

4.北京聯(lián)合大學 應用科技學院，北京 100101

無人駕駛技術被廣泛認為是避免道路碰撞和改善交通狀況的一個有意義，且具有實用性價值的研究方向。為了使無人駕駛汽車在城市環(huán)境中安全行駛，汽車需要獲取自身的準確位姿。因此，定位技術在無人駕駛汽車的應用中起著關鍵的作用[1]。目前幾乎所有的無人駕駛車輛定位方案中都使用絕對定位（如Global Navigation Satellite System，GNSS）與相對定位（如帶里程表的Laser-Simultaneous Localization and Mapping，Laser-SLAM）相結合的方法來獲取車輛更精確的位置信息[2-3]。但GNSS系統(tǒng)僅適用于室外環(huán)境，在室內環(huán)境下由于衛(wèi)星信號遮擋導致定位失敗，然而95%的汽車大多時間都停在停車場或者室內環(huán)境[3]。由此可見，構建與GNSS相對應的室內定位系統(tǒng)從而實現(xiàn)無人駕駛車輛的室內精確定位已成為亟待解決的問題。

當前成熟的室內定位方法中，基于諸如超寬帶（Ultra-Wideband，UWB）、藍牙、Zigbee、可見光、超聲波、WiFi、射頻識別設備、激光雷達（Light Detection and Ranging，LiDAR）、視覺設備等傳感器；并采用到達角（Angle of Arrival，AOA）、飛行時間（Time of Flight ，TOF）、飛行返回時間（Roundtrip Time of Flight，RTOF）、往返飛行時間（Two Way-Time of Flight，TW-TOF）、接收信號強度（Received Signal Strength，RSS）等方法實現(xiàn)室內定位[4-5]。但目前成熟的室內定位方案中，由于不同的傳感器自身特性，沒有任何一個單一的傳感器能夠同時滿足無人駕駛車輛的定位精度、實時性、魯棒性需求。故需要采用多類型傳感器融合的方式實現(xiàn)無人駕駛車輛的定位與導航，如絕對定位類型的傳感器與相對定位類型的傳感器相融合的定位導航方式。

近年來，學者對于多傳感器數(shù)據(jù)融合定位算法的研究近年來趨向于基于濾波的數(shù)據(jù)融合方法，文獻[6]分析了LiDAR和UWB測距的定位問題，提出了一種將慣性測量單元（Inertial Measurement Unit，IMU）、LiDAR和UWB 相結合的基于卡爾曼濾波的傳感器融合方法，并在無人機設備連續(xù)定位狀態(tài)取得了較好的效果，然而在文中沒有涉及因動態(tài)障礙物干擾造成激光雷達失配后重新匹配的研究。文獻[7-8]利用UWB超高的時間分辨率、良好的抗多徑能力和較強的穿透能力，以及LiDAR在構圖與定位方面所表現(xiàn)出主動、高效、精確的特點，提出了基于擴展卡爾曼濾波的UWB/LiDAR 組合定位系統(tǒng)。該系統(tǒng)能夠穩(wěn)定維持較高的定位精度，然而在定位失效后無法迅速重定位，因此無法滿足無人駕駛定位穩(wěn)定性的需求。此外，文獻[9]中無人駕駛定位導航系統(tǒng)自適應地使用來自互補傳感器（如GNSS，LiDAR 和IMU）的信息，以在挑戰(zhàn)性場景（如市區(qū)、高速公路和隧道）中實現(xiàn)較高的定位精度和彈性，并采用誤差狀態(tài)卡爾曼濾波器將來自不同來源的定位測量結果與新的不確定性估計融合在一起，實現(xiàn)了5～10 cm 均方根精度。

然而在復雜的室內環(huán)境中，如動態(tài)障礙物較多，以及由于長時間停車造成的LiDAR SLAM時間累積誤差導致定位失效問題，在該研究中并沒有給出解決方案。

為了更好地解決室內定位的問題，本文采用UWB絕對定位技術和LiDAR SLAM相對定位技術相融合的定位方法。UWB 技術是一種基于信號的定位方式，類比于GNSS的定位方式，在視距（Line of Sight，LOS）環(huán)境的情況下可以提供可靠的絕對定位，但也會受到室內環(huán)境復雜性的制約，在非視距（Non Line of Sight，NLOS）環(huán)境下以及多路徑效應會使得定位精度下降。另外LiDAR SLAM 定位技術可以提供可靠的空間特性，而且其定位精度較高，但單獨使用LiDAR SLAM 定位技術存在起始點定位搜索時間長，以及激光點云匹配失效后無法快速重定位問題。為此提出基于粒子濾波的多傳感器融合算法，將UWB和LiDAR量測數(shù)據(jù)互補融合解算。一方面，LiDAR 給定平面圖的精確性可以補償UWB 在非視距環(huán)境下定位誤差，另一方面，UWB 定位高實時性可以彌補LiDAR SLAM 定位速率低的問題。通過這種方法可以滿足無人駕駛車輛定位系統(tǒng)的低延時，定位精度高以及穩(wěn)定性的需求。

1 定位技術原理

1.1 UWB定位原理

UWB 定位通常使用TOF 測距技術，測距至少需要三個錨點（空間基站）和一個標簽（車載標簽）兩種設備，通過計算信號在錨點和標簽間的傳輸時間差來確定二者的相對距離，這與全球定位系統(tǒng)中的偽距定位技術類似。其優(yōu)點是TOF 測距技術不依賴基站與標簽的時間同步，故沒有時鐘同步偏差帶來的誤差，但TOF測距方法的時間取決于時鐘精度。為此本文采用異步TW-TOF方法，其測量模型如圖1所示。該方法不需要錨點和標簽通信雙方的時間同步，根據(jù)數(shù)據(jù)交互產(chǎn)生的時間差，可以計算出錨點和標簽之間的距離，從而消除TOF測距方式時鐘精度偏移引起的誤差[10-11]。

圖1 TW-TOF測量模型

根據(jù)TW-TOF測量原理，可以求出錨點與標簽之間的距離dTW-TOF如式（1）所示：

其中，c是電磁波在真空中的傳播速度的近似常數(shù)3×108m/s；t1和t2分別表示請求時鐘起始時刻和接收應答終止時刻；δ為響應時鐘周期，一般為固定參數(shù)；δe表示總的系統(tǒng)時延（多由溫度，傳播介質等因素決定），其誤差分布符合高斯白噪聲N(0,Q)。

由于環(huán)境的復雜性，UWB 在測距過程中經(jīng)常由于障礙物遮擋等原因引起的NLOS 誤差dNLOS，NLOS 誤差呈現(xiàn)非高斯分布。故可得出基于TW-TOF 測距模型的表達式為式（2）：

其中，di表示標簽距第i個基站的距離。

由上所述，本文采用三個坐標已知的UWB 基站和一個UWB 車載標簽實現(xiàn)車輛的定位，通過測得的標簽到基站距離di,(i=1,2,3)后，通過三點定位法畫三個圓，所得的交點即為標簽的位置。

其原理如圖2 所示，UWB 基站的坐標分別為A1(x1,y1,z1)、A2(x2,y2,z2) 和A3(x3,y3,z3) ，基站在安裝部署時位置固定且坐標已知，所求UWB 車載標簽的坐標為T0(x0,y0,z0)，其中z0為標簽高度且已知。設r1、r2和r3分別為三個基站與標簽之間的相對距離，計算公式如式（3）：

圖2 UWB定位原理示意圖

每個基站以相對距離為半徑畫一個圓形軌跡。利用三個圓形方程能夠計算出唯一的交點，計算公式如式（4）：

即可求解出T0在平面坐標系上的坐標(x0,y0)。

由于非視距誤差為非高斯模型，涉及到基站與標簽的位置分布、障礙物的物質介質因素等難以估計，故用LiDAR 與UWB 數(shù)據(jù)融合算法消除。該算法適用于室內NLOS環(huán)境下運動目標的精確定位和動態(tài)跟蹤。

1.2 LiDAR SLAM定位原理

即時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）算法是目前解決無人駕駛車輛LiDAR或者視覺定位所常用的方法[12]。

本文采用Google Cartographer[13]算法地圖構建功能，生成車輛可行駛的局部點云地圖。其可以生成分辨率為r=5 cm 的二維柵格地圖，系統(tǒng)結合了單獨的局部和全局方法來實現(xiàn)二維SLAM。并且兩種方法都優(yōu)化了由激光雷達觀測值（又稱為掃描）組成的由(x,y)平移和旋轉γ組成的姿態(tài)δ=( )δx δy δγ。其中激光點云的坐標由激光雷達以固定的頻率與角度旋轉掃描所得，如其中一個掃描點數(shù)據(jù)為Pi=(ρi,θi)，其中ρi為當前激光點到原點的距離，θi為當前激光束與起始激光束之間的夾角。則可得其i點坐標如式（5）所示：

其中，xi和yi分別表示第i個激光點距離激光雷達原點的坐標，δγi為當前激光束與上一時刻激光束的轉角增量。

其主要建圖與定位步驟如下：

（1）把空間劃分成邊長r為5 cm的二維柵格。

（2）根據(jù)每個柵格內t時刻是否有激光點，給每個柵格分配一個占柵格值p(mi)：

式（6）中，p(mi)為第i個柵格占用的概率，柵格值為1代表被占用，柵格值為0代表空閑。

（3）計算當前幀和局部地圖的占柵格值。

（4）對當前一幀設置一個搜索區(qū)間，即一個平移(δx,δy)和旋轉δγ的區(qū)間。

（5）把搜索區(qū)間劃分成多個選擇項，每一個選擇項對應一個位姿，即(δx δy δγ)，其中δx,δy為當前激光雷達對上一時刻的平移量，δγ為航向角旋轉增量。

（6）用當前幀備選位姿計算相應的指標，判斷其位姿相對局部地圖的匹配程度，匹配度最高的位姿，即為當前位姿的最優(yōu)估計。

但LiDAR SLAM 技術在實際應用中，需要提供初始位姿以提高匹配速率，在復雜環(huán)境中由于動態(tài)障礙物等因素的影響導致匹配失敗以至于無法重定位。

2 組合定位系統(tǒng)模型

2.1 UWB與LiDAR組合定位模型

通過第1 章分別對UWB 和LiDAR SLAM 定位技術的介紹，可以得知每一種傳感器在室內獨立定位時都存在自身固有的技術壁壘，例如：UWB定位技術存在的非視距環(huán)境下誤差，以及LiDAR SLAM 技術存在的誤差累積和時延問題。這些問題都會影響到無人駕駛車輛在室內環(huán)境下的定位精度以及穩(wěn)定性。為此有效的數(shù)據(jù)融合可以對兩者的劣勢進行互補，從而獲得更加準確和穩(wěn)定的定位結果。

無人駕駛車輛室內環(huán)境下UWB 和LiDAR 融合定位算法框架主要分為兩大部分，第一步對UWB與LiDAR組合定位系統(tǒng)在時間和空間系統(tǒng)上進行統(tǒng)一。首先在待測地圖中根據(jù)合理布局對UWB基站進行布局以及測量地圖，并利用LiDAR SLAM 算法對待測地圖進行地圖構建，最終將地圖坐標系通過坐標變換進行統(tǒng)一；對于融合系統(tǒng)的時間輸出，使用解算設備的系統(tǒng)時間作為統(tǒng)一標準，從而達到組合定位模型在時間和空間上的統(tǒng)一。對于超寬帶和激光雷達組合定位算法模型，將UWB和LIDAR量測信息用于基于粒子濾波的多傳感器融合算法中。該算法是基于蒙特卡洛積分的最佳非線性濾波器的估計算法，其適用于解決非線性非高斯的動態(tài)模型問題[14-16]；針對UWB 與LiDAR 融合定位系統(tǒng)的非線性非高斯的動態(tài)模型問題，可以發(fā)揮不錯的效果。如圖3為無人駕駛車輛室內環(huán)境下UWB 和LiDAR 融合定位算法框架。

2.2 基于粒子濾波的數(shù)據(jù)融合算法

根據(jù)第1 章中UWB 和LiDAR 定位算法，為了得到高精度、高穩(wěn)定性的定位結果，本文采用基于粒子濾波的超寬帶和激光雷達融合定位誤差模型，將UWB 和LiDAR 量測信息互補融合，其中使用Xt表示移動車輛在t時刻的位置與姿態(tài)，使用Zt表示UWB和LiDAR傳感器在t時刻的觀測值，則可以構建基于粒子濾波的融合算法模型。公式（7）和公式（8）分別表示該模型的狀態(tài)方程和量測方程。

狀態(tài)方程：

量測方程：

式（7）、（8）中，函數(shù)f(?)為基于時間t的狀態(tài)函數(shù)，g(?)為傳感器非線性量測函數(shù)。Wt-1表示離散時域中的隨機白噪聲序列，Vt表示測量噪聲序列。在該模型中，將Xt=[ ]

xt yt θtT 作為狀態(tài)信息向量，xt和yt分別表示UWB 和LiDAR 組合系統(tǒng)在t時刻的橫縱坐標，θt表示在t時刻的航向角增量，且有：

式（9）中，γt-1為系統(tǒng)在t-1 時刻的航向角。將Zt=作為量測信息輸入向量，其中分別表示UWB 設備在t時刻橫縱坐標的量測值，分別表示LiDAR 設備在t時刻的橫縱坐標和航向角增量的量測值。

本文基于粒子濾波的UWB 和LiDAR 定位算法的主要步驟如下，如圖4 所示為基于粒子濾波的UWB 和LiDAR融合定位算法流程圖。

圖3 室內環(huán)境下UWB和LiDAR融合定位算法框架圖

圖4 基于粒子濾波的UWB和LiDAR融合定位算法

（1）Initialization Step：在初始化步驟中，根據(jù)UWB坐標輸入估算位置，估算位置是存在噪聲的，但是可以提供一個范圍約束。通過UWB 提供的初始位置，提高粒子濾波運算效率。

公式（10）中，和分別表示UWB 定位系統(tǒng)在t時刻的測量值，ε為 UWB 定系統(tǒng)最大測量誤差，rand(?)為生成(0,1)區(qū)間的隨機函數(shù)，xi和yi分別表示生成第i個粒子的橫縱坐標，N為隨機粒子樣本數(shù)，對應每個粒子的初始化權值為ωi，設為1/N。

（2）Prediction Step：在預測過程中，對所有粒子增加車輛的控制輸入（速度、角速度等），預測所有粒子的下一步位置。

公式（11）中，xt和yt分別為當前粒子狀態(tài)預測橫縱坐標，xt-1和yt-1分別為上一時刻橫縱狀態(tài)坐標，ΔT為系統(tǒng)運行時間增量，vt為當前時刻車輛速度，γt-1為上一時刻車輛航向角，θt為當前時刻航向角增量，和為系統(tǒng)狀態(tài)噪聲，由車輛運動的橫縱向標準偏差確定。

（3）Update Step：在更新過程中，計算此時由UWB觀測生成的每個粒子的位置和LiDAR量測得到的車輛位置之間的幾何距離，來更新所有粒子的權重。從而通過超寬帶和激光雷達交互更新粒子權重，消除UWB 引起的NLOS誤差，同時消除LiDAR時間累積誤差。

其權值的更新公式如式（12）所示：

其中，為根據(jù)UWB 量測值生成的粒子中第i個粒子的橫縱坐標集合為時刻t+1 的 LiDAR橫縱坐標量測集合為兩者集合的2范數(shù)求解，意為求解兩個集合之間的距離。R為定位系統(tǒng)測量誤差。

再利用式（13）對權值進行歸一化：

（4）Resample Step：根據(jù)粒子的權重，對所有粒子進行重采樣，權重越高的粒子有更大的概率生存下來，權重越小的例子生存下來的概率就越低，從而達到優(yōu)勝劣汰的目的。利用后驗概率密度P(Xt|Zt)（式（14））產(chǎn)生N個新粒子解決粒子退化問題。

即對不同粒子進行加權后的概率密度，同時，t時刻狀態(tài)Xt的最終估計值如式（15）所示：

式（13）中，Xt表示粒子在t時刻的車輛狀態(tài)，Zt表示車輛在t時刻的觀測車輛位置，式（15）中，為車輛t時刻的車輛狀態(tài)最優(yōu)估計值，表示t時刻第i個粒子的狀態(tài)函數(shù)，表示t時刻重采樣之后的第i個粒子權重值。

（5）再回到（2），進入下一次循環(huán)，在此過程中，遇到車輛狀態(tài)突變，以當前UWB 定位結果作為初始位置回到（1），重新搜索車輛位置。

3 實驗分析

3.1 實驗平臺搭建及預處理

為了驗證UWB 與LiDAR 的室內組合定位算法的效果，在地下車庫內搭建測試場景，并選用長530 mm，寬310 mm 的智能駕駛小車作為無人駕駛車輛。同時，該車還配有車載處理器NVIDIA TX2，并運行ROS機器人操作系統(tǒng)。定位傳感器UWB 和LiDAR 分別使用布科思科技有限公司的BooStar-T和思嵐科技有限公司的RPLIDAR-A2。其中，UWB的工作頻率為6.2～6.7 GHz，無遮擋通信距離為130 m；LIDAR的測距范圍為0.2～8 m，掃描角度為360°，單次測距時間0.125 ms。為保證UWB 和LiDAR 在空間解算上的統(tǒng)一性，將UWB 標簽和LiDAR 安裝在智能駕駛小車平臺的同一軸線上，分別通過RS232串口協(xié)議將數(shù)據(jù)傳輸至TX2平臺。

此外，為了保證定位精度，使用激光水平儀與激光測距儀測量車庫的空間距離以及UWB 基站的相對位置，如圖5 所示。測量結果為：長7.0 m，寬7.0 m，環(huán)境左下角為坐標原點；其中UWB 基站的坐標分別為(0.0,1.0,2.0),(7.0,1.0,2.0)和(3.5,7.0,2.0)（坐標單位：m）。

圖5 實驗環(huán)境及其參數(shù)

在地庫測試環(huán)境中，智能駕駛小車平臺的運行路徑為A、B、C、D 圍成的矩形區(qū)域（圖5 所示），其坐標分別為(1.0,1.0),(6.0,1.0),(6.0,6.0),(1.0,6.0)（坐標單位：m），其中A→B→C路段為LOS和無動態(tài)障礙物路段，C→D→A路段為NLOS和有動態(tài)障礙物路段。

圖6 為智能駕駛小車平臺使用cartographer 激光雷達SLAM算法建立的點云地圖，采集地圖運行路徑如圖5 所示，從A 點出發(fā)，按照既定路線（A→B→C→D→A）運行智能駕駛小車平臺，回到A點。采集所得的地圖通過坐標變換后用于匹配激光雷達定位。

圖6 激光雷達點云地圖

3.2 實驗結果及分析

實驗1單獨使用UWB 定位方法在LOS 路段A→B→C和NLOS路段C→D→A進行定位實驗。圖7所示為UWB 在室內環(huán)境下定位實驗結果?？梢缘贸鯴軸（A→B）LOS 最大誤差為30.5 cm，Y軸為（B→C）LOS最大誤差為24.5 cm，X軸（C→D）NLOS 最大誤差為33.0 cm，Y軸（D→A）NLOS最大誤差為93.5 cm。

圖7 UWB在室內環(huán)境下定位實驗結果

實驗2使用LiDAR-SLAM方法在無動態(tài)障礙物路段A→B→C 和有動態(tài)障礙物路段C→D→A 進行定位實驗。圖8 所示為LiDAR 在室內環(huán)境下定位實驗結果。結果表明，在無動態(tài)障礙物路段LiDAR 的X軸（A→B）最大誤差為33.2 cm，Y軸（B→C）最大誤差為31.4 cm；在有動態(tài)障礙物路段LiDAR的X軸（C→D）最大誤差為42.8 cm，Y軸（D→A）最大誤差為77.9 cm。同時可以看出LiDAR單一傳感器SLAM算法容易產(chǎn)生較大的累積誤差。

圖8 LiDAR在室內環(huán)境下定位實驗結果

實驗3使用本文提出的UWB 和LiDAR 融合定位算法在LOS 和無動態(tài)障礙物路段A→B→C 和在NLOS和有動態(tài)障礙物的路段C→D→A，進行實驗。圖9所示為本文算法在室內環(huán)境下定位實驗結果。實驗表明，本文算法在LOS和無動態(tài)障礙物環(huán)境下，X軸（A→B）最大誤差為12.8 cm，Y軸（B→C）最大誤差為18.5 cm；在NLOS 和有動態(tài)障礙物環(huán)境下，X軸（C→D）最大誤差為22.1 cm，Y軸（D→A）最大誤差為37.9 cm。

圖9 本文算法在室內環(huán)境下定位實驗結果

圖10 給出了 UWB 定位、LiDAR SLAM 定位以及本文算法的定位對比結果，圖中實線軌跡為本文算法，可以看出本文算法定位結果在最大定位誤差上得到了很大的改善，其均方根誤差實驗結果如表1 所示，結果表明UWB 和LiDAR 組合定位的方式在NLOS 和有動態(tài)障礙物環(huán)境下仍然可以提供良好的定位精度。

表1 給出了三次動態(tài)實驗的數(shù)據(jù)統(tǒng)計，分別為UWB定位、LiDAR定位和融合定位的均方根誤差對比。

實驗4分別對LiDAR 定位實驗和融合定位實驗中，添加人為遮擋因素，使其LiDAR傳感器點云匹配失效，并記錄重新匹配定位時長。表2中給出了本文融合定位算法與室內LiDAR定位算法在點云地圖失配后重定位時長，結果表明，本文融合算法解決了LiDAR SLAM失配后的重定位時間長的問題，重定位平均時長較LiDAR SLAM算法降低了15.7倍。

圖10 本文算法與單一傳感器定位實驗對比

表1 定位結果實驗對比 m

表2 重定位結果實驗對比 ms

4 結束語

針對現(xiàn)有無人駕駛車輛在室內環(huán)境（如地下車庫，倉庫）中由于衛(wèi)星信號的丟失導致車輛定位精度低甚至無法定位問題，本文采用超寬帶（UWB）定位技術與激光雷達（LiDAR）即時定位與地圖構建（SLAM）技術并提出一種基于粒子濾波的多傳感器互補誤差消除融合定位算法。通過將UWB 和LiDAR 的量測數(shù)據(jù)進行基于粒子濾波的互補融合解算，提升了多傳感器融合定位系統(tǒng)的定位精度以及穩(wěn)定性，并提供良好的定位實時性。在室內環(huán)境下實驗結果表明：相比單獨使用UWB或LiDAR的定位技術來說，其定位精度被全面提升，平均誤差控制在15 cm以內，同時該方法還解決了無人駕駛車輛起始點搜索以及點云匹配失效后的快速匹配的問題，從而提高了無人駕駛車輛定位的實時性和穩(wěn)定性。綜上，融合定位系統(tǒng)能夠同時滿足無人駕駛車輛需求的定位精度，實時性以及穩(wěn)定性。