基于UWB的智能召車導(dǎo)航定位系統(tǒng)研究

臧利國，唐一鳴，沈業(yè)輝，王孜洲

（南京工程學院 a.汽車與軌道交通學院；b.自動化學院，南京 211167）

隨著人工智能的高速發(fā)展和車輛技術(shù)的不斷提高，智能車的研究與應(yīng)用越來越成為生產(chǎn)生活的重要方面。作為現(xiàn)代社會的新產(chǎn)物，在智能車上開發(fā)出來的各種功能，如自動泊車、防碰撞預(yù)警、輔助駕駛系統(tǒng)等，可以保證行駛的安全性，進一步提高智能化水平。2019年9月，特斯拉推出了一項名為“Smart Summon（智能召車）”的新功能，通過該功能，用戶只需在車停放的任意位置周圍操控手機，車便可自動行駛到車主旁，無需人為干預(yù)。其結(jié)合了u-blox公司的NEO-M8L汽車慣性導(dǎo)航模塊定位［1］和自身強大的視覺處理能力，但此功能有60.96 m的距離限制和1 m的定位誤差。

實現(xiàn)智能召車，如何使車輛在非結(jié)構(gòu)化環(huán)境中獲取自身狀態(tài)是關(guān)鍵。在高精度定位方面，牛群峰等［2］提出了一種基于TW_TOF的UWB信號定位系統(tǒng)，利用三邊測量法實現(xiàn)定位，采用IMM卡爾曼濾波優(yōu)化定位數(shù)據(jù)，試驗顯示該系統(tǒng)有效減小了定位數(shù)據(jù)的誤差；王川陽等［3］提出基于雙向測距的TOA定位方法，利用Chan算法和高斯－牛頓迭代算法解算定位結(jié)果，提高了系統(tǒng)的測距和定位精度；張浩等［4］提出一種基于單基站六角單極定向天線陣列的到達角（AOA）估計方法，解決了多基站系統(tǒng)的部署困難問題，可提供較精確的定位精度。

針對定位精度問題，結(jié)合智能召車應(yīng)用場景，設(shè)計了一種基于超寬帶技術(shù)的導(dǎo)航定位系統(tǒng)，利用加權(quán)三邊測量法實現(xiàn)定位，通過改進無跡卡爾曼濾波算法減小噪聲干擾，有效提高了定位精度，保證了召車的安全性。

1 UWB定位

定位一般有如下幾種方法：GPS、超聲波、WIFI、UWB、RFID等。其中GPS易受天氣、地球自轉(zhuǎn)、衛(wèi)星運行、云層流動等因素的影響［5］，且在室內(nèi)信號強度較弱，精度無法滿足需求；超聲波易產(chǎn)生多徑效應(yīng)和NLOS誤差，且頻率受多普勒效應(yīng)和溫度影響；WIFI是通過無線接入點組成的無線局域網(wǎng)絡(luò)，但其定位誤差較大，且易受干擾［6］；RFID可以在幾毫秒內(nèi)實現(xiàn)厘米級定位，但其作用距離近，且不具通信能力。不同室內(nèi)定位方式的評價指標如表1所示。

表1 常見室內(nèi)定位方式的評價指標

UWB是近年來新興的一項無載波通信技術(shù)，其工作時間短、穿透能力強、定位精確［7］。利用納秒級的非正弦波窄脈沖傳輸數(shù)據(jù)，以較高的時間分辨力、抗多徑效應(yīng)等特點，成為目前室內(nèi)定位最佳物理層技術(shù)［8－17］。

1.1 基于UWB的智能召車定位模型

如圖1所示，將UWB基站A0、A1、A2分別固定于車身上，以A1A2中點O為原點建立空間坐標系。其中M為車主手持標簽，M′為M在基站平面的投影，定義OM′與Y軸夾角α為車輛姿態(tài)角。UWB測得標簽M距基站A0、A1、A2的距離分別為d0、d1、d2，根據(jù)基站位置和d0、d1、d2，實現(xiàn)對標簽位置的定位。

圖1 UWB定位車主示意圖

以標簽M的位置和速度作為狀態(tài)向量Xk＝［xkykvx，kvy，k］，其中xk、yk分別為標簽的平面坐標，vx、vy分別表示標簽在方向上的速度。由于應(yīng)用場景中只需考慮標簽在平面中的移動情況，故只考慮二維位置信息。

狀態(tài)方程

式中：F為狀態(tài)轉(zhuǎn)移矩陣；ΔT為UWB采樣間隔；ωk為k時刻系統(tǒng)噪聲向量。

UWB量測方程

式中：Yk＝［r1，kr2，kr3，k］T為量測信息；h（Xk）＝［d1，kd2，kd3，k］T，dn，k為第n個基站與標簽的實際距離；Vk為k時刻量測噪聲。

為衡量車輛的位置與姿態(tài)，引入機器人正運動學概念。以基站A0、A1、A2為基準建立如圖1空間坐標系，設(shè)A0（0，－b，0），A1（－a，0，0），A2（a，0，0），M（x，y，z），已知基站A0、A1、A2距目標M的距離分別為d0、d1、d2，列出如下方程，

解標簽M坐標（x，y，z），對坐標軸XYZ進行變換，使得

1）O點到達目標點M′（x，y，0）

2）變換后的X軸垂直于線段OA變換矩陣

令

式中，矩陣P1、P2、…、Pn為召車過程中車輛經(jīng)歷的所有位姿變換；P為總變換矩陣，P的確定為智能召車的路徑規(guī)劃奠定了基礎(chǔ)。

1.2 測距方式

目前室內(nèi)定位大多使用飛行時間（TOF）測距原理，但其忽略了設(shè)備的響應(yīng)時間（1 ns會導(dǎo)致0.3 m的測距誤差），且現(xiàn)實中不可避免地存在因時鐘振蕩器不同引起的時鐘漂移問題。為提高測距精度，本研究采用雙向測距方法（TW_TOF）。TW_TOF工作示意圖見圖2。

圖2 TW_TOF工作示意圖

標簽與基站分別生成2條獨立的時間戳，TSP時刻標簽向基站發(fā)送一條請求性質(zhì)的信號，基站開始接收，并記下此時的時刻TRP。TSR時刻基站回復(fù)信息，標簽開始接收，記錄此時的時刻，并在TSF時刻將數(shù)據(jù)包P3（TSP，TRR，TSF）發(fā)送給基站，進行如下計算：

因此，TW_TOF測距法因時鐘漂移帶來的誤差僅為毫米級。

圖3為標簽與基站的TW_TOF法測距試驗數(shù)據(jù)，其真實距離為5 m，可看出誤差均在0.1 m之內(nèi)。

圖3 標簽與基站測距試驗數(shù)據(jù)

1.3 改進Trilateration三邊測量法

標準三邊測量法根據(jù)標簽與基站距離，以基站為圓心、距離為半徑作圓，圓的交點即為標簽位置。設(shè)3個基站的坐標分別為（x0，y0），（x1，y1），（x2，y2），標簽距3個基站的距離分別為d0，d1，d2，交點坐標為（x，y），根據(jù)畢達哥拉斯定理［10］：

求解上述公式得交點坐標

但由于UWB基站功耗的不同，現(xiàn)實情況中測距不可避免地存在誤差，三邊法中3個圓不可能理想化地交于同一點，且多徑效應(yīng)的存在大大提高了定位的不確定性，為此對標準三邊測量法進行了改進。

絕大多數(shù)情況下三邊法會相交于如圖4所示的一片區(qū)域，設(shè)3個圓交點分別為（xa，ya），（xb，yb），（xc，yc），選擇3個點所圍成的三角形的顯然3個圓所圍成的區(qū)域面積大小對標簽的位置準確性具有較大影響，且所圍區(qū)域面積越小，準確度越高［11］。

圖4 改進的三邊測量示意圖

所以面積可以作為權(quán)值衡量定位點的可信程度。但由于所圍區(qū)域面積的求解過于繁瑣，若直接使用面積計算權(quán)值，則會占用大量的CPU資源，導(dǎo)致定位的滯后。

圖5為三圓相交面積與3個交點所圍成三角形面積關(guān)系的Matlab仿真結(jié)果。由圖5可知，三圓相交面積和三角形面積呈正相關(guān)，即可用三角形面積與定位精確程度的關(guān)系近似3個圓相交面積與定位精確程度的關(guān)系。圍成的三角形面積越小，所求取到的定位點更準確，其權(quán)值也更大。

圖5 三圓相交面積和交點圍成的三角形面積關(guān)系

設(shè)3個圓分別為O1，O2，O3，其方程為：三角形面積：

假設(shè)UWB每0.2 s獲取n組有效數(shù)據(jù)，每組數(shù)據(jù)下所求取的交點圍成的三角形面積為S，可以定義以下的權(quán)重函數(shù)，表示位置的可信程度

標準化處理，求得位置最優(yōu)值：

這樣，考慮多次估算未知點位置，利用各個位置的權(quán)值確定最終定位。不僅解決了標準三邊測量法無法交于一點的問題，也一定程度上克服了多徑效應(yīng)、NLOS誤差對定位的影響，增加了定位的可信度。

2 誤差分析與修正

2.1 理論模型誤差

2.1.1 標準時間偏差

標準時間偏差en包括UWB信號在標簽與基站之間傳播的固定時延誤差、器件誤差、時鐘漂移誤差，還有溫度、氣壓等外界環(huán)境引起的誤差，可表示為：

式中：en為標準時間偏差引起的測距誤差；cn為器件誤差、固定時延誤差、時鐘漂移誤差和外界環(huán)境所引起的常值誤差；φ（d）為不同UWB基站測距形成的誤差，與距離d有關(guān)［12］；Dn為量測噪聲。對UWB進行數(shù)據(jù)標定，通過大量測距數(shù)據(jù)進行線性擬合可確定標準時間偏差，從而減小誤差。

2.1.2 信號傳播誤差

UWB傳播誤差主要來源于2個方面：一是多徑效應(yīng)，多徑傳播是UWB的典型信道環(huán)境，多徑信號受信道干擾和噪聲等影響，會引起直達單徑分量DP被誤檢或延遲檢測，帶來多徑誤差；二是NLOS誤差，NLOS環(huán)境中存在障礙物遮擋，造成NDDP（non-dominant direct path）和UDP（undetectable direct path）情形［13］，導(dǎo)致DP延遲或無法檢測。當誤差較大時，會導(dǎo)致量測信息偏離真實狀態(tài)，甚至引起濾波發(fā)散。

2.2 實際測量誤差

為分析測距誤差特性，建立誤差模型，隨機選擇距離進行了10 000次測距試驗（已進行過數(shù)據(jù)標定，剔除標準時間偏差），其誤差分布情況如圖6所示。

圖6 誤差分布

此試驗表明，UWB測距誤差集中分布在－0.015～0.015 m，隨著誤差絕對值的增大概率明顯減小，且其分布圖形與高斯分布的鐘形較為貼合，在濾波處理時可將誤差模型近似為高斯分布。

2.3 UKF算法

在數(shù)字濾波方面，卡爾曼濾波作為經(jīng)典的算法，其可以利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行最優(yōu)估計［14］。但其只適用于高斯線性模型，為此有學者提出了擴展卡爾曼濾波（EKF）和無跡卡爾曼濾波（UKF）。

對于EKF，首先必須求出非線性函數(shù)的Jacobi矩陣，對于模型復(fù)雜的系統(tǒng)，計算復(fù)雜且容易出錯；其次，必須滿足小擾動假設(shè)，只適用于弱非線性系統(tǒng)，對于強非線性系統(tǒng)容易導(dǎo)致精度下降［15］。UKF使用確定性采樣方式，通過一種非線性變換——UT變換來進行模型狀態(tài)與協(xié)方差的更新，其估計均值與協(xié)方差均達到了Taylor級數(shù)的4階精度，過程如下：

步驟1設(shè)置濾波初始值

步驟2計算k－1時刻的Sigma樣本點，對Sigma點進行非線性變換

式中：n為狀態(tài)維數(shù)（本文研究對象為UWB定位模型，考慮取n＝2）；κ為尺度參數(shù)，對其進行調(diào)整可以提高逼近的精度。用這組采樣點χik－1為2n＋1個Sigma點構(gòu)成的列向量，可以近似表示狀態(tài)的高斯分布。

步驟3計算均值和方差

α、λ和β是計算過程中所需的3個參數(shù)，α通常取e－4≤α＜1；λ為比例因子，設(shè)置為0或3－n；β表示狀態(tài)分布，對于高斯分布其最優(yōu)值為2；κ＝α2（n＋λ）－n；適當調(diào)節(jié)α、λ可以提高估計均值的精度，調(diào)節(jié)β可以提高所求方差的精度。

步驟4計算Sigma樣本點，預(yù)測k時刻的測量信息。

步驟5測量更新

計算更新后的增益矩陣，求出狀態(tài)向量、協(xié)方差矩陣最優(yōu)值

2.4 算法簡化

標準UKF算法需要計算2次Sigma點，預(yù)測k時刻的狀態(tài)值時需進行如下計算，

需要以xk和Pk作為輸出值，計算5個Sigma點，從而預(yù)測狀態(tài)和協(xié)方差值。但式（1）觀測模型為線性方程，預(yù)測時使用UT變換不一定能提高精度，反而一定會降低計算效率［16］，因此將式（35）～（37）的預(yù)測方程改為KF的預(yù)測方程：

這樣，不僅保證了模型的定位精度，也減少了算法的運算量，提高了近40%的計算效率，使其可以應(yīng)用于UWB定位的數(shù)據(jù)處理。

3 試驗與分析

為驗證定位與算法的合理性，分別進行了靜態(tài)試驗和動態(tài)試驗。試驗在10 m×10 m的LOS環(huán)境中進行，采用Decawave公司生產(chǎn)的DWM1000射頻模塊。其典型帶寬500 MHz，發(fā)射功率調(diào)整范圍為－62～－35 dBm／MHz，選擇110 kb／s的通信速率。

3.1 靜態(tài)試驗

試驗?zāi)M智能召車過程中的UWB定位，將車上的3個基站分別設(shè)置于（500，0，1 000），（0，－300，1 000），（－500，0，1 000），標簽固定于（3 000，4 000，1 000）處，試驗中標簽與基站使用TW_TOF法測距，利用加權(quán)優(yōu)化后的三邊測量法初步定位標簽位置，其位置再經(jīng)改進的無跡卡爾曼濾波處理得到最終定位。試驗共分為2組，圖7為對標簽的定位結(jié)果，其中改進組（紅點）使用本文算法，未改進組（藍點）使用標準三邊測量法。

圖7 靜態(tài)定位

與理想點（3 000，4 000）對比，未改進組中x方向平均誤差70.91 mm，最大誤差172.08 mm，y方向平均誤差89.47 mm，最大誤差239.87 mm；改進組中x方向平均誤差55.66 mm，最大誤差102.25 mm，y方向平均誤差68.17 mm，最大誤差180.38 mm，其RMSE（均方根誤差）與MaE（平均絕對誤差）值如表2所示。

表2 試驗結(jié)果

3.2 動態(tài)試驗

為驗證此導(dǎo)航系統(tǒng)對動態(tài)目標的定位精度，進行了動態(tài)試驗。試驗中，車主手持標簽進行圖8紅線所示的S型移動，結(jié)果表明，動態(tài)定位中x方向最大偏離123.65 mm，y方向最大偏離150.27 mm，絕大多數(shù)點誤差在100 mm之內(nèi)，且軌跡波動不大，與理想軌跡基本一致。

圖8 動態(tài)定位軌跡

4 結(jié)論

分析了TW_TOF的測距原理與精度，構(gòu)建UWB定位模型，并對三邊測量的結(jié)果進行加權(quán)優(yōu)化，有效抑制了多徑效應(yīng)和NLOS誤差對定位的影響，提高了定位可信度；根據(jù)誤差分析，改進無跡卡爾曼濾波算法，在保證精度的同時提高了計算效率；最后，根據(jù)定位數(shù)據(jù)計算車輛位姿變換矩陣，為智能召車路徑規(guī)劃奠定了研究基礎(chǔ)。試驗結(jié)果表明：該導(dǎo)航定位系統(tǒng)能有效地對動、靜態(tài)目標進行高精度定位，為智能召車定位提供了一種可行的解決方案。