曾慶喜， 金 宇， 林雅潔， 修巧艷， 張 琪， 藺筱宣

（南京航空航天大學(xué) a.自動(dòng)化學(xué)院；b.教師發(fā)展與教學(xué)評(píng)估中心／高等教育研究所，南京 211106）

0 引 言

隨著汽車工業(yè)的發(fā)展以及人們生活水平的提高，城市機(jī)動(dòng)車輛迅速增加，這給城市交通帶來了壓力的同時(shí)也對(duì)停車場(chǎng)的管理技術(shù)提出了要求［1-2］。目前，對(duì)于傳統(tǒng)停車場(chǎng)，車主進(jìn)入停車場(chǎng)后，不能迅速、準(zhǔn)確地找到自己滿意的停車位，這浪費(fèi)了車主的時(shí)間，同時(shí)也降低了停車場(chǎng)的運(yùn)行效率。隨著計(jì)算機(jī)技術(shù)逐漸被應(yīng)用到停車場(chǎng)中，停車場(chǎng)的智能化和自動(dòng)化程度也在不斷地提升［3］。停車場(chǎng)有室外停車場(chǎng)與室內(nèi)停車場(chǎng)，在對(duì)車輛進(jìn)行誘導(dǎo)停車時(shí)，室外停車場(chǎng)可使用GPS 對(duì)車輛進(jìn)行定位，但對(duì)于室內(nèi)停車場(chǎng)，GPS 信號(hào)受到遮擋，會(huì)出現(xiàn)GPS 拒止的情況，無法對(duì)車輛進(jìn)行定位，UWB定位需要預(yù)先布置基站，適用于室內(nèi)定位［4-5］。

單一的GPS或UWB的定位方法不足以實(shí)現(xiàn)大范圍的室內(nèi)外定位導(dǎo)航，故引入U(xiǎn)WB 模塊與GPS 進(jìn)行聯(lián)合定位［6］。本文設(shè)計(jì)的實(shí)驗(yàn)裝置可應(yīng)用于智能停車場(chǎng)等環(huán)境，滿足車輛誘導(dǎo)等定位需求。本文將從實(shí)驗(yàn)的角度分析單一GPS／UWB 模塊的具體定位性能，給出相對(duì)坐標(biāo)解算算法以及GPS與UWB 在不同定位環(huán)境下的切換算法。

1 GPS模塊定位實(shí)驗(yàn)

1.1 單頻GPS定位原理

GPS定位方式可以分為單點(diǎn)定位與差分定位。所謂單點(diǎn)定位，即根據(jù)單一的地面接收機(jī)得到的觀測(cè)量來確定接收機(jī)具體坐標(biāo)位置的方式。它僅僅采取偽距觀測(cè)量來進(jìn)行定位和坐標(biāo)解算，用于車輛、船舶、無人機(jī)等的初步定位［7-8］。

對(duì)于單點(diǎn)定位來說，其基本原理是偽距測(cè)量。在利用偽距法進(jìn)行絕對(duì)定位過程中，偽距的觀測(cè)方程如下：

式中：c是光速；tR是信號(hào)到達(dá)接收機(jī)的時(shí)刻；tS是信號(hào)離開衛(wèi)星的時(shí)刻~為衛(wèi)星與地面接收機(jī)的距離。

依據(jù)平面建立坐標(biāo)系的方式，接收機(jī)坐標(biāo)未知數(shù)有L（經(jīng)度）、B（緯度）、H（高度）以及t（時(shí)鐘同步）。所以至少需要4 顆可見衛(wèi)星，聯(lián)立4 個(gè)偽距方程，才可以解算出地面接收機(jī)的坐標(biāo)信息。

1.2 GPS硬件搭建及數(shù)據(jù)傳輸

本文采用單頻GPS模塊，如圖1 所示，將U-BLOX芯片與GPS 接收機(jī)，TTL 轉(zhuǎn)USB 串口相連，制得一個(gè)簡(jiǎn)易的單頻GPS模塊，該模塊可以直接向單片機(jī)傳輸經(jīng)度、緯度和高度等信息，波特率為9.6 Kb／s，以字符型數(shù)據(jù)格式傳輸，1 幀數(shù)據(jù)的數(shù)據(jù)長(zhǎng)度為71 bit。

圖1 單頻GPS模塊

由于GPS模塊以幀為單位發(fā)送數(shù)據(jù)，故使用GPS模塊首先需要判斷接收數(shù)據(jù)是否幀格式正確。本文采用對(duì)接收到的每1 幀GPS 數(shù)據(jù)的多位標(biāo)志位進(jìn)行校驗(yàn)，從而保證數(shù)據(jù)接收的完整性和準(zhǔn)確性。

其次就是在1 幀數(shù)據(jù)中解析出所需要的經(jīng)度、緯度、高度，以及可見衛(wèi)星數(shù)量信息。由于該模塊在傳輸過程中是以字符型數(shù)據(jù)傳輸，故在解析過程中需要逐位編寫程序進(jìn)行解析得到經(jīng)緯度信息。

1.3 GPS坐標(biāo)轉(zhuǎn)換算法與誤差分析

由于GPS模塊傳輸?shù)慕?jīng)緯度信息不能直接用于導(dǎo)航定位分析，故本文采用高斯-克呂格投影法［9］，將經(jīng)緯度信息通過投影方式轉(zhuǎn)換到大地平面坐標(biāo)系X、Y、Z三軸正交坐標(biāo)系上，具體方法如下：

數(shù)據(jù)解析與坐標(biāo)轉(zhuǎn)換完成后，即可通過單片機(jī)讀取GPS定位信息并分析其定位質(zhì)量。

單點(diǎn)靜態(tài)定位效果如圖2 所示，圖中橫軸為單點(diǎn)靜態(tài)定位的采樣點(diǎn)序號(hào)；縱軸為單點(diǎn)靜態(tài)定位的Y 軸坐標(biāo)值。在Y軸方向，GPS原始定位數(shù)據(jù)有毛刺誤差，最大絕對(duì)誤差3 m左右。經(jīng)Kalman濾波之后，定位效果更加平穩(wěn)，誤差相對(duì)減小［10-11］。

圖2 GPS單點(diǎn)靜態(tài)定位效果圖

多點(diǎn)動(dòng)態(tài)軌跡定位效果如圖3 所示，實(shí)驗(yàn)地點(diǎn)在南航東區(qū)操場(chǎng)跑道，自坐標(biāo)系左下方點(diǎn)（－132.250 000，－273.835 600）開始。由圖可見，在開始位置準(zhǔn)備階段的停留時(shí)間過長(zhǎng)等原因，紅色原始軌跡波動(dòng)比較劇烈，但經(jīng)過Kalman 濾波之后的藍(lán)色曲線雖然與RTK系統(tǒng)相比有差距，但是已經(jīng)使得GPS數(shù)據(jù)平滑并減小了大部分毛刺和階梯狀誤差。

圖3 GPS操場(chǎng)動(dòng)態(tài)定位軌跡

由上述GPS 靜態(tài)及動(dòng)態(tài)定位實(shí)驗(yàn)軌跡圖可得，GPS在10 m級(jí)的定位是可以保障穩(wěn)定的，在解析定位的過程中采用參數(shù)合理的Kalman 濾波程序可以保證軌跡的平滑性和準(zhǔn)確性。

2 UWB模塊定位實(shí)驗(yàn)

2.1 TOF測(cè)距原理

本文使用研創(chuàng)物聯(lián)UWB Mini 3S Plus系列超寬帶測(cè)距模塊。該模塊的測(cè)距原理是雙向時(shí)間飛行法（TW-TOF，two way-time of flight）。在啟動(dòng)開始時(shí)，UWB系統(tǒng)中的每一個(gè)模塊（基站或者標(biāo)簽）都會(huì)生成一條互不干擾的時(shí)間戳。如圖4 所示，基站1 在其時(shí)間戳的t11上發(fā)送信息1，基站2 在t21時(shí)刻接收，并在t22時(shí)刻發(fā)射一個(gè)響應(yīng)信息1 的信息2，基站1 在t12時(shí)刻接收該響應(yīng)信息2。根據(jù)上面闡述的發(fā)送與接收響應(yīng)過程，可以計(jì)算出信息在兩個(gè)模塊之間的飛行傳遞時(shí)間，并通過計(jì)算得到基站1 與基站2 之間的距離。設(shè)該距離為S，光速為c，則

圖4 雙向時(shí)間飛行法（TOF）

式中：T1＝t12－t11；T2＝t22－t21。

2.2 UWB硬件搭建

本文中使用的UWB模塊主控芯片是STM32 單片機(jī)，測(cè)距芯片為UWB DW1000。該模塊支持虛擬USB串口，通信距離可達(dá)300 m。

打開單片機(jī)程序軟件以及UWB 的基站與標(biāo)簽，能夠觀察到UWB的基站0 通過USB虛擬串口傳送到電腦端的數(shù)據(jù)傳送格式如下：

mc 0f 00000663 000005a3 00000512 000004cb 095f c1 00023c24 a0：0 mc表示基站-標(biāo)簽（修正數(shù)據(jù)），后面的4 組數(shù)據(jù)是各個(gè)基站到標(biāo)簽之間的距離。

文中提到UWB 與GPS 數(shù)據(jù)判定、解析相似，在UWB的處理過程中，首先對(duì)其進(jìn)行格式檢驗(yàn)，并逐位解析；UWB模塊數(shù)據(jù)以16 進(jìn)制數(shù)據(jù)格式傳輸，在逐位解析過程中，需要對(duì)UWB 數(shù)據(jù)進(jìn)行進(jìn)制轉(zhuǎn)換，得到基站和標(biāo)簽之間的距離。

要實(shí)現(xiàn)三維定位，則該定位系統(tǒng)需要4 個(gè)基站和1 個(gè)可移動(dòng)標(biāo)簽組成，三維定位基站搭建時(shí)，各個(gè)基站之間距離大于1 m，至少有1 個(gè)基站與其他3 個(gè)基站高度差50 cm以上。如圖5 所示，在建立的坐標(biāo)系中，基站0 坐標(biāo)為（0，0，－928），基站1 坐標(biāo)為（1 213，0，0），基站2 坐標(biāo)為（－1 428，0，0），基站3 坐標(biāo)為（0，－1 130，0），單位mm。

圖5 UWB室內(nèi)定位基站布置

2.3 UWB相對(duì)坐標(biāo)解算算法與數(shù)據(jù)分析

根據(jù)TOF雙向飛行時(shí)間法可以獲取基站與標(biāo)簽之間的距離R［12-13］。由圖6 可知，當(dāng)求解標(biāo)簽二維平面坐標(biāo)時(shí)，可通過提前標(biāo)定并建立坐標(biāo)系獲取3 個(gè)基站的X、Y兩軸坐標(biāo)，再通過3 個(gè)圓交于一點(diǎn)解算出標(biāo)簽的X、Y二維坐標(biāo)。

圖6 TOF算法原理

因?yàn)樾枰S定位信息，故可根據(jù)上述原理，4 個(gè)基站建立坐標(biāo)系。Ra，Rb，Rc，Rd為UWB 模塊測(cè)距結(jié)果，X1—X4，Y1—Y4，Z1—Z4為建立坐標(biāo)系時(shí)標(biāo)定的坐標(biāo)值，為已知值。設(shè)標(biāo)簽坐標(biāo)為X，Y，Z?？闪谐鲆韵路匠蹋?/p>

本文采用線性方程解法解算出在UWB 基站建立坐標(biāo)系下的坐標(biāo)。在數(shù)據(jù)解析與坐標(biāo)轉(zhuǎn)換完成后，即可通過單片機(jī)讀取UWB 定位信息并分析其定位質(zhì)量。

單點(diǎn)靜態(tài)定位效果如圖7 所示，在X 軸方向，當(dāng)有障礙物遮擋基站或標(biāo)簽時(shí)，UWB 定位有最大可達(dá)50 cm的靜態(tài)定點(diǎn)誤差。經(jīng)Kalman 濾波之后，可將最大絕對(duì)誤差降至15 cm 以內(nèi)；當(dāng)無障礙物遮擋時(shí)，UWB定位處于相對(duì)理想狀態(tài)，定位精度可達(dá)到cm級(jí)。

圖7 UWB定點(diǎn)靜態(tài)定位效果

動(dòng)態(tài)軌跡效果如圖8 所示，在基站無遮擋慢速前進(jìn)的情況下，原始數(shù)據(jù)軌跡在轉(zhuǎn)向等區(qū)域波動(dòng)較大，絕對(duì)誤差在15 cm上下起伏，通過Kalman濾波之后軌跡相對(duì)平滑，絕對(duì)誤差值降低，符合預(yù)期。

圖8 UWB動(dòng)態(tài)軌跡

3 GPS與UWB 雙定位系統(tǒng)切換算法設(shè)計(jì)與實(shí)驗(yàn)

3.1 GPS與UWB雙定位系統(tǒng)切換算法設(shè)計(jì)

定位方法如圖9 所示，在距離目標(biāo)位置較遠(yuǎn)時(shí)，通過GPS模塊獲取經(jīng)緯度坐標(biāo)，并通過高斯-克呂格算法將其轉(zhuǎn)化為大地坐標(biāo)系坐標(biāo)，通過兩者大地坐標(biāo)系坐標(biāo)計(jì)算得到相對(duì)坐標(biāo)［14］。

圖9 GPS／UWB定位系統(tǒng)整體設(shè)計(jì)

通過相對(duì)坐標(biāo)計(jì)算并判斷距離遠(yuǎn)近，到達(dá)近距離范圍時(shí)切換定位方式，通過UWB模塊與事先布置的4個(gè)基站來計(jì)算相對(duì)位置實(shí)現(xiàn)近距離精準(zhǔn)定位。

本文設(shè)計(jì)的切換算法是基于距離及GPS 信號(hào)的質(zhì)量，具體切換算法如圖10 所示。

圖10 切換算法流程

首先，明確切換的閾值。GPS 定位信號(hào)質(zhì)量較好（可見衛(wèi)星數(shù)量＞4）時(shí)距離目標(biāo)距離較遠(yuǎn)（歐氏距離＞100 m）時(shí)，宜進(jìn)行GPS 定位；GPS 定位信號(hào)質(zhì)量過差（可見衛(wèi)星數(shù)量＜4）時(shí)或者距離目標(biāo)較近（歐氏距離＜100 m）時(shí)，且UWB 可正常接收數(shù)據(jù)時(shí)，宜進(jìn)行UWB定位，當(dāng)可見衛(wèi)星數(shù)量＞4 且UWB 無法定位時(shí)用GPS定位，其他情況輸出無法定位。

實(shí)驗(yàn)對(duì)切換的觸發(fā)條件做了部分更改，在室外GPS信號(hào)良好，可見衛(wèi)星數(shù)量＞4 時(shí)系統(tǒng)默認(rèn)采用GPS定位，在GPS 信號(hào)不良且可接收到UWB 信號(hào)時(shí)采用UWB進(jìn)行定位。

3.2 定位系統(tǒng)數(shù)據(jù)處理

在實(shí)際定位時(shí)，由于室內(nèi)外環(huán)境的復(fù)雜多變和信號(hào)傳播有多路徑性，測(cè)得的信號(hào)中有粗差和噪聲。為了減少粗差、降低噪聲，使數(shù)據(jù)輸出更加平滑，從而進(jìn)一步提高測(cè)試數(shù)據(jù)的穩(wěn)定性和精度，可以加入卡爾曼濾波［15］。本文將GPS的定位模塊經(jīng)過高斯-克呂格投影坐標(biāo)轉(zhuǎn)換后的坐標(biāo)位置和UWB 標(biāo)簽的位置作為狀態(tài)向量?？柭鼮V波方程：

增益矩陣Jk是在每次更新之前都能被確定的矩陣。當(dāng)觀測(cè)噪聲R 越小時(shí)，Jk就越大，通過第1 個(gè)式子可以看出觀測(cè)量對(duì)濾波過程的貢獻(xiàn)就越大；當(dāng)上一步狀態(tài)向量的協(xié)方差陣DX（k－1／k－1）變小時(shí)，預(yù)測(cè)的協(xié)方差陣DX（k／k－1）也變小，進(jìn)而Jk也變小，即增益效果不明顯，觀測(cè)量對(duì)濾波過程的貢獻(xiàn)較小。

3.3 實(shí)驗(yàn)驗(yàn)證

系統(tǒng)實(shí)驗(yàn)裝置的實(shí)驗(yàn)軌跡如圖11 所示，雙系統(tǒng)整體切換流暢，Kalman 濾波起到了較好的濾波效果，減小了GPS模塊部分階梯狀跳躍軌跡。在室內(nèi)外切換點(diǎn)附近會(huì)有4 或5 個(gè)采樣點(diǎn)左右的延遲。總體來說，本實(shí)驗(yàn)裝置滿足智能停車場(chǎng)等環(huán)境的整體定位需求。

圖11 GPS／UWB切換定位實(shí)驗(yàn)軌跡

4 結(jié) 語

本文基于STC8H8K64U 單片機(jī)實(shí)現(xiàn)了單頻GPS模塊和UWB模塊的數(shù)據(jù)解析和相對(duì)坐標(biāo)解算，并對(duì)兩者單獨(dú)定位的動(dòng)態(tài)軌跡和靜態(tài)定位效果進(jìn)行了分析，得出任一定位系統(tǒng)的定位精度滿足車輛定位所需，但是無法滿足智能停車場(chǎng)室內(nèi)外全場(chǎng)景定位要求的結(jié)論。調(diào)試卡爾曼濾波的相關(guān)參數(shù)，進(jìn)一步提高了定位質(zhì)量。設(shè)計(jì)了基于GPS／UWB的室內(nèi)外無縫組合定位系統(tǒng)的切換算法并對(duì)實(shí)驗(yàn)結(jié)果進(jìn)行分析，組合定位系統(tǒng)的定位效果基本滿足智能停車場(chǎng)的車輛定位所需。在實(shí)際搭建基于STC單片機(jī)的GPS／UWB室內(nèi)外無縫定位實(shí)驗(yàn)裝置的過程中，可使學(xué)生將所學(xué)的理論知識(shí)與實(shí)踐相結(jié)合，更好地理解理論知識(shí)，同時(shí)可使學(xué)生掌握工程實(shí)踐知識(shí)，提高學(xué)生的工程實(shí)踐能力。