基于UWB的智能召車導航定位系統研究

臧利國，唐一鳴，沈業輝，王孜洲

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

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

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

針對定位精度問題，結合智能召車應用場景，設計了一種基于超寬帶技術的導航定位系統，利用加權三邊測量法實現定位，通過改進無跡卡爾曼濾波算法減小噪聲干擾，有效提高了定位精度，保證了召車的安全性。

1 UWB定位

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

表1 常見室內定位方式的評價指標

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

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

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

圖1 UWB定位車主示意圖

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

狀態方程

式中：F為狀態轉移矩陣；ΔT為UWB采樣間隔；ωk為k時刻系統噪聲向量。

UWB量測方程

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

為衡量車輛的位置與姿態，引入機器人正運動學概念。以基站A0、A1、A2為基準建立如圖1空間坐標系，設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為召車過程中車輛經歷的所有位姿變換；P為總變換矩陣，P的確定為智能召車的路徑規劃奠定了基礎。

1.2 測距方式

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

圖2 TW_TOF工作示意圖

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

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

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

圖3 標簽與基站測距試驗數據

1.3 改進Trilateration三邊測量法

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

求解上述公式得交點坐標

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

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

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

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

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

圖5 三圓相交面積和交點圍成的三角形面積關系

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

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

標準化處理，求得位置最優值：

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

2 誤差分析與修正

2.1 理論模型誤差

2.1.1 標準時間偏差

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

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

2.1.2 信號傳播誤差

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

2.2 實際測量誤差

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

圖6 誤差分布

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

2.3 UKF算法

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

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

步驟1設置濾波初始值

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

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

步驟3計算均值和方差

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

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

步驟5測量更新

計算更新后的增益矩陣，求出狀態向量、協方差矩陣最優值

2.4 算法簡化

標準UKF算法需要計算2次Sigma點，預測k時刻的狀態值時需進行如下計算，

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

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

3 試驗與分析

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

3.1 靜態試驗

試驗模擬智能召車過程中的UWB定位，將車上的3個基站分別設置于（500，0，1 000），（0，－300，1 000），（－500，0，1 000），標簽固定于（3 000，4 000，1 000）處，試驗中標簽與基站使用TW_TOF法測距，利用加權優化后的三邊測量法初步定位標簽位置，其位置再經改進的無跡卡爾曼濾波處理得到最終定位。試驗共分為2組，圖7為對標簽的定位結果，其中改進組（紅點）使用本文算法，未改進組（藍點）使用標準三邊測量法。

圖7 靜態定位

與理想點（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 試驗結果

3.2 動態試驗

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

圖8 動態定位軌跡

4 結論

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