TDOA／DR組合系統定位算法及仿真研究＊

黃曉冬 周 旻

（1.海軍航空工程學院科研部 煙臺 264001）（2.海軍航空工程學院電子信息工程系 煙臺 264001）

1 引言

電磁導航、紅外導航、激光導航、視覺導航、超聲波檢測等手段都是目前常用的室內導航和定位手段。而其中超聲波定位，由于成本低，結構簡單，精度高，環境適應性強，易于實現等優點，成為室內自主導航的主要手段［1］，此外，超聲波已被證明對人體并無傷害，因此作為行人導航定位系統也更具優勢。

超聲波是一種人耳無法聽到，頻率超過20kHz的聲音，由于其傳播速度慢，因此具有較高的時間分辨率，能夠提高測距、定位精度。但聲波在空氣中衰減較大，傳播距離往往較短，只有幾米或十幾米，因而比較適用于室內定位，并得到了廣泛的應用。

超聲波定位原理與無線電定位原理類似，通過接收位置已知的發射點（反射點）的超聲波，來獲得移動體距離固定點的相對距離，再計算得到其具體位置。常用的定位方法有渡越時間法（Time of Flight，TOF），到達時間法（Time of Arrival，TOA），到達時間差法（Time Difference Of Arrival，TDOA）［2～3］，其中 TOF是最為傳統的超聲測距定位方法，也最為常用，TDOA是近幾年才開始從無線電定位引入超聲波定位中［4］。由于TOF的實質為回波測距，超聲波本身在空氣中衰減嚴重，因此這種方法會大大限制測量距離，制約了超聲定位的范圍；TOA算法可以避免上述問題，但對各基站時間同步性要求較為苛刻。TDOA與TOA相比，由于是檢測信號到達的時間差而非絕對時間，因而降低了對時間同步性的要求［5～6］。

2 基于超聲傳感器的TDOA室內行人定位

在行人導航系統中增加超聲波接收器，在室內天花板上固定超聲波發射器。則當行人佩戴著PNS在布有超聲波發射器（基站）的室內行走時，通過接收各基站發射的超聲波信號，測量接收到信號時的時間差，從而獲得行人到各個基站的距離差［7～8］，如圖1所示。

圖1 TDOA室內定位示意圖

圖中，R1、R2、R3和R4代表室內固定的超聲發射點，為了避免外物的遮擋，將超聲發射器固定于天花板上。H表示室內空間高度，h表示佩戴超聲傳感器的高度，約為身高的1／2。則發射點與接收器的高度差Δh＝H－h。d1、d2和d3表示實際測量計算得到的對應基站到接收器的距離，r1為d1在水平面的投影長度。TDOA定位原理如圖2所示。

圖2 TDOA定位原理圖

圖2中，D表示待定位的行人位置在基站水平面的投影位置。當行人行走過程中，佩戴在人體的超聲波接收裝置接收各基站發射超聲波信號，并測量接收到信號時的時間。假設基站R1、R2和R3接收到超聲波信號的時間分別是t1、t2和t，則基站R1、R2到行人的距離為

接收信號的時間差Δt12＝t1－t2，可計算得到行人距兩基站的距離差：

其中，c為超聲波傳播速度，將式（1）帶入式（2），可得方程：

測得基站R3、R2接收信號的時間差Δt32＝t3－t2，即可得到行人距R3、R2的距離差Δd32＝cΔt32，得：

通過解算式（3）、（4）便可確定行人所處位置。

由于超聲波的傳播速度受周圍溫度的影響，會發生變化，因而還必須增加溫補模塊，對溫度進行測量，再根據補償公式：c＝331.5＋0.607T（m／s），T 為溫度（℃），進行計算從而獲得較準確的傳播速度。

3 TDOA／DR組合導航算法

3.1 TDOA／DR組合導航方案

由于利用TDOA進行定位導航時基站附近定位誤差較大，為了提高導航參數的可靠性和連續性，本文采用TDOA／DR組合導航定位。具體工作原理如圖3所示。

圖3 TDOA／DR組合導航原理框圖

TDOA與DR系統仍然獨立工作，當TDOA信號穩定有效時，將TDOA輸出的位置信息與DR輸出的位置信息的差值作為量測值，進行最優卡爾曼濾波組合，濾波結果作為導航數據輸出，同時作為反饋信息修正DR系統；當車輛進入TDOA系統盲區，基站無法解算車輛位置時，組合導航系統將直接輸出DR系統的定位結果［9］。

3.2 TDOA／DR組合導航算法

假設TDOA觀測噪聲為白噪聲，因此在GPS／DR組合導航算法的研究基礎上，TDOA／DR組合導航濾波狀態量依然采用誤差量，狀態方程為

由于TDOA定位系統無航向角輸出信息，因此觀測量為DR與TDOA定位系統獲得的東向，北向位置差，即：

觀測方程為：

式中，Hk表示觀測矩陣，Vk為觀測噪聲。

由于在該系統中狀態方程與觀測方程均為線性，因此采用線性卡爾曼濾波進行解算。

4 TDOA／DR組合導航算法

假設行人以2steps／s的步頻，0.6m／step的步長在矩形區域內勻速繞行，每秒鐘獲得一個TDOA定位值，進行卡爾曼濾波［10］。TDOA系統定位誤差由時差為100ms白噪聲引起。DR誤差由步長誤差—方差為（0.05m／step）2的一階馬爾可夫過程噪聲與航向角偏差—方差為（2°／s）2的一階馬爾可夫過程引起。定位誤差仿真曲線如圖4所示。

圖4 TDOA／DR組合導航仿真誤差曲線圖

圖4中，虛線為TDOA導航系統定位誤差，粗實線為組合導航系統的定位誤差，從圖中可以看出，TDOA／DR組合導航系統能夠在一定程度上提高系統的定位精度，將定位誤差控制在10cm內，有效抑制DR系統的誤差隨時間發散的問題，可以為行人導航室內定位提供可靠導航數據。

5 結語

本論文主要針對行人導航室內定位問題展開研究。針對超聲傳感器在室內定位中的廣泛應用，將其與TDOA定位算法進行有效地結合，并將其應用到室內區域定位中。并對TDOA定位系統與DR系統的組合導航算法進行了研究及仿真試驗。通過仿真實驗，驗證了TDOA／DR組合導航算法能夠有效抑制DR系統誤差發散問題，并在一定程度上提高系統的定位精度，獲得精度為10cm的位置信息，能夠為行人導航的室內區域定位提供連續的高精度導航參數。

［1］王松，鄭正奇，鄒晨祎.超聲定位車輛路徑監測系統的設計［J］.計算機測量與控制，2006，14（10）：1310-1311，1325.

［2］J.Manuel Villadangos，J.Urena，M.Mazo，et al.Improvement of Ultrasonic Beacon-based Local Position System Using Multi-Access Techniques［J］.Workshop on Intelligent Signal Processing 2005，Faro，Portugal：University of Algarve Portugal，2005：352-357.

［3］Wenfeng Jiang，Yu Chen，Yuanchun Shi，et al.The Design and Implementation of the Cicada Wireless Sensor Network Indoor Localization System［J］.Proceedings of the 16th International Conference on Artificial Reality and Telexistence—Workshops（ICAT'06），Los Alamitos，USA，IEEE Computer Society Press，2006：536-541.

［4］梁延德，劉大維.基于到達時間差異的超聲波AGV定位［J］.控制與檢測，2007（4）：66-69.

［5］孫德波，楊功流，張玉堂.SINS／GPS組合導航序貫濾波算法［J］.中國慣性技術學報，2005，13（1）：59-46.

［6］林雪原，徐進，石曉輝.GPS小波去噪的誤差方差建模技術與試驗［J］.海軍航空工程學院學報，2009，24（4）：361-364.

［7］Long Hong.Distributed filtering using set models［J］.IEEE Transactions Aerospace and Electronics System，1992，28（4）：1144-1153.

［8］康國華，劉建業，熊智，等.彈道導彈的GNSS／SST／SINS組合導航系統研究［J］.應用科學學報，2006，24（3）：293-297.

［9］文成林，周東華，潘泉，等.多尺度動態模型單傳感器動態系統分布式信息融合［J］.自動化學報，2001，27（2）：176-179.

［10］吳富海，楊元喜.基于小波閾值自適應濾波的GPS／SINS組合導航［J］.測繪學報，2007，36（2）：5-9.