本發(fā)明涉及超寬帶(UWB：Ultra-wide band)通信領(lǐng)域，特別涉及基于超寬帶的室內(nèi)無線定位方法。

背景技術(shù)：

超寬帶技術(shù)是一種的無線通信技術(shù)，具有納秒級別的時(shí)間分辨能力、高速傳輸速率、對信道衰落不敏感、有良好的抗多徑干擾能力等優(yōu)點(diǎn)，特別適合于室內(nèi)定位系統(tǒng)的應(yīng)用。

現(xiàn)有的超寬帶定位方法主要有：基于接收信號強(qiáng)度(RSS：Received Signal Strength)、基于到達(dá)角(AOA：Angle of Arrival)、基于到達(dá)時(shí)間(TOA：Time of Arrival)、基于到達(dá)時(shí)間差(TDOA：Time Difference of Arrival)、以及基于它們的組合形式。RSS是用于估計(jì)節(jié)點(diǎn)間傳輸距離的一種有用的參量。該參量可以使用較低復(fù)雜度的電路在數(shù)據(jù)通信中測量獲得，因此RSS被廣泛應(yīng)用于室內(nèi)定位，但是對RSS和傳輸距離之間的關(guān)系建立精確的數(shù)學(xué)模型非常困難，因此基于RSS的定位系統(tǒng)具有有限的定位精度。AOA不適合用戶超寬帶定位系統(tǒng)，因?yàn)樾盘柭窂綌?shù)量可能非常大，導(dǎo)致精確的角度估計(jì)變得非常難。TOA和TDOA是超寬帶系統(tǒng)中最常用的設(shè)計(jì)方案，因?yàn)檫@種基于時(shí)間測距的方式最能體現(xiàn)超寬帶信號超高時(shí)間分辨率的特點(diǎn)。TOA可以進(jìn)一步分為單向測距法(OWR：One-Way TOA Ranging)和雙向測距法(TWR：Two-Way TOA Ranging)。OWR和TDOA都要求高精度的時(shí)鐘同步，這在實(shí)際工程中實(shí)現(xiàn)起來比較困難。TWR也叫做基于雙程時(shí)間(RTT：Round Trip Time)，是個(gè)很好的超寬帶室內(nèi)定位系統(tǒng)方案，因?yàn)樵摲绞娇梢员苊鈺r(shí)鐘同步問題。然而，基于RTT設(shè)計(jì)的超寬帶室內(nèi)定位系統(tǒng)有兩個(gè)缺陷：一是系統(tǒng)中的設(shè)備的時(shí)間分辨率沒有補(bǔ)償，時(shí)間同步問題不能完全解決；二是系統(tǒng)中的基站和定位設(shè)備采用雙向交流的，限制了用戶的數(shù)量。

技術(shù)實(shí)現(xiàn)要素：

為解決上述問題，本發(fā)明的目的在于提供一種有效解決設(shè)備時(shí)間同步問題且不限制用戶數(shù)量的超寬帶室內(nèi)定位方法。

本發(fā)明的實(shí)現(xiàn)包括以下步驟：

步驟一：在室內(nèi)建立空間直角坐標(biāo)系，將基站布置在室內(nèi)空間，測量并記錄各個(gè)基站的坐標(biāo)，選定其中一個(gè)基站為主基站，測量并記錄主基站到其它基站的距離；

步驟二：基站按照信號交流機(jī)制發(fā)射室內(nèi)定位信號，室內(nèi)定位設(shè)備接收室內(nèi)定位信號，利用卡爾曼濾波進(jìn)行時(shí)鐘偏差補(bǔ)償；

所述的信號交流機(jī)制具體描述為：以其中一個(gè)基站為主基站，用于協(xié)調(diào)系統(tǒng)信號交流機(jī)制，向其它所有基站以及用戶所持的室內(nèi)定位設(shè)備發(fā)送測距信號并提供其時(shí)間信息。其他基站為副站，在主基站的控制下，分別向定位設(shè)備和主基站發(fā)送測距信號并提供其時(shí)間信息。定位設(shè)備通過接收各個(gè)基站的測距信號，以及各個(gè)基站和自身的時(shí)間信息完成定位。

系統(tǒng)信號交流機(jī)制以組為單位進(jìn)行信號交流：主基站發(fā)送一個(gè)測距信號，其他任意一個(gè)副站和定位設(shè)備分別接收來自主基站信號；接著，該副站經(jīng)過延遲發(fā)送另一個(gè)測距信號，然后主基站和定位設(shè)備分別接收來自該副站的信號，至此完成一組信號交流。其它組的信號交流類似。全部信號交流時(shí)序組成系統(tǒng)信號交流機(jī)制的一個(gè)周期。

所涉及時(shí)鐘偏差補(bǔ)償方法包含如下過程：

1)從信號交流機(jī)制中分離出一組信號交流過程，利用接收信號和發(fā)射信號模型建立時(shí)間的數(shù)學(xué)模型；

2)結(jié)合基站和定位設(shè)備的位置信息，建立時(shí)間和距離的數(shù)學(xué)模型；

3)計(jì)算時(shí)鐘偏差補(bǔ)償系數(shù)觀測值，即定位設(shè)備和基站的時(shí)間分辨率比值；

4)建立時(shí)鐘偏差補(bǔ)償?shù)臓顟B(tài)方程和觀測方程，進(jìn)行卡爾曼濾波解算，得到時(shí)鐘偏差補(bǔ)償系數(shù)的估計(jì)值；

5)通過判斷時(shí)鐘偏差補(bǔ)償系數(shù)觀測值相對于時(shí)鐘偏差補(bǔ)償系數(shù)估計(jì)值的差值是否超過門限值，去除觀測數(shù)據(jù)中的野值，得到時(shí)鐘偏差補(bǔ)償后的結(jié)果。

步驟三：利用泰勒迭代法估計(jì)定位設(shè)備空間坐標(biāo)；

步驟四：重復(fù)步驟二～步驟三，得到室內(nèi)定位結(jié)果。

本發(fā)明相比于現(xiàn)有超寬帶室內(nèi)定位方法具有以下優(yōu)點(diǎn)：

1.本發(fā)明設(shè)計(jì)的信號交流機(jī)制，使得定位設(shè)備只接收來自基站的信號，而不需要向基站發(fā)送信號，定位設(shè)備配置方式更加靈活，支持無限用戶同時(shí)三維定位。

2.利用信號交流機(jī)制使得基站、定位設(shè)備的時(shí)鐘起點(diǎn)進(jìn)行了同步；利用卡爾曼濾波算法對不同時(shí)鐘分辨率的基站、定位設(shè)備實(shí)現(xiàn)時(shí)鐘偏差補(bǔ)償。通過時(shí)鐘起點(diǎn)和時(shí)鐘分辨率兩方面同時(shí)進(jìn)行軟件算法層面的同步，不需要額外的同步硬件網(wǎng)絡(luò)，時(shí)鐘同步精度高，進(jìn)而獲取更高的定位精度。

3.在時(shí)鐘偏差補(bǔ)償方法中通過引入門限值去除觀測數(shù)據(jù)中的野值，有效抑制了在由視距場景切換至非視距場景、由非視距場景切換至視距場景過程中室內(nèi)定位誤差的跳變，提升了室內(nèi)定位的精度。

附圖說明

圖1為本發(fā)明方法的流程圖。

圖2為基站安裝布局圖。

圖3為系統(tǒng)發(fā)射信號過程示意圖。

圖4為系統(tǒng)接收信號過程示意圖。

圖5為系統(tǒng)信號交流機(jī)制示意圖。

圖6為時(shí)鐘偏差補(bǔ)償算法信號交流示意圖。

圖7為定位設(shè)備在實(shí)際中的位置關(guān)系示意圖。

圖8Aa為第1組試驗(yàn)時(shí)鐘偏差補(bǔ)償濾波前仿真圖。

圖8Ab為第1組試驗(yàn)時(shí)鐘偏差補(bǔ)償濾波后仿真圖。

圖8Ba為第2組試驗(yàn)時(shí)鐘偏差補(bǔ)償濾波前仿真圖。

圖8Bb為第2組試驗(yàn)時(shí)鐘偏差補(bǔ)償濾波后仿真圖。

圖8Ca為第3組試驗(yàn)時(shí)鐘偏差補(bǔ)償濾波前仿真圖。

圖8Cb為第3組試驗(yàn)時(shí)鐘偏差補(bǔ)償濾波后仿真圖。

圖8Da為第4組試驗(yàn)時(shí)鐘偏差補(bǔ)償濾波前仿真圖。

圖8Db為第4組試驗(yàn)時(shí)鐘偏差補(bǔ)償濾波后仿真圖。

圖9a為第1個(gè)采樣點(diǎn)的時(shí)間數(shù)據(jù)示意圖。

圖9b為第2個(gè)采樣點(diǎn)的時(shí)間數(shù)據(jù)示意圖。

圖9c為第3個(gè)采樣點(diǎn)的時(shí)間數(shù)據(jù)示意圖。

圖10為時(shí)間數(shù)據(jù)和距離數(shù)據(jù)散點(diǎn)圖。

圖11為距離和真實(shí)距離的差值計(jì)算示意圖。

圖12為測試點(diǎn)平面圖。

圖13a為室內(nèi)靜態(tài)定位X軸坐標(biāo)值曲線。

圖13b為室內(nèi)靜態(tài)定位Y軸坐標(biāo)值曲線。

圖13c為室內(nèi)靜態(tài)定位Z軸坐標(biāo)值曲線。

圖14a為室內(nèi)靜態(tài)定位X軸定位誤差曲線。

圖14b為室內(nèi)靜態(tài)定位Y軸定位誤差曲線。

圖14c為室內(nèi)靜態(tài)定位Z軸定位誤差曲線。

圖15為室內(nèi)動態(tài)實(shí)驗(yàn)在XY平面的軌跡圖。

圖16a為室內(nèi)動態(tài)定位X軸坐標(biāo)值曲線。

圖16b為室內(nèi)動態(tài)定位Y軸坐標(biāo)值曲線。

圖16c為室內(nèi)動態(tài)定位Z軸坐標(biāo)值曲線。

具體實(shí)施方式

本發(fā)明方法的流程圖，如圖1所示。下面通過以下2個(gè)具體實(shí)施例，結(jié)合說明書附圖對本發(fā)明方法做進(jìn)一步描述。

具體實(shí)施例一.視距環(huán)境下的靜態(tài)實(shí)驗(yàn)

本發(fā)明優(yōu)選的超寬帶室內(nèi)定位硬件芯片為DWM1000，該裝置具有接收和發(fā)送超寬帶信號的功能，可以在信號交流過程中實(shí)現(xiàn)數(shù)據(jù)無線傳輸，并可以提供接收時(shí)間戳和發(fā)送時(shí)間戳，時(shí)間分辨率為1/(128×499.2×10⁶)秒。

基站數(shù)量優(yōu)選為4個(gè)及以上。

本發(fā)明方法的具體實(shí)施步驟如下：

步驟一：首先在室內(nèi)建立空間直角坐標(biāo)系。布置4個(gè)基站在室內(nèi)固定位置，使得4個(gè)基站非共面。選定靠近室內(nèi)中心位置的基站為主基站，記為基站1，其余基站分別記為基站2～基站4，測量并記錄各個(gè)基站的坐標(biāo)，測量并記錄基站1到其它基站的距離，具體的布局如圖2所示。

步驟二：將定位設(shè)備固定在一個(gè)測試點(diǎn)，以構(gòu)建靜態(tài)實(shí)驗(yàn)條件，測量并記錄定位設(shè)備到每個(gè)基站的距離；讓基站和定位設(shè)備工作一個(gè)小時(shí)，從而使系統(tǒng)充分穩(wěn)定。

步驟三：基站按照信號交流機(jī)制發(fā)射室內(nèi)定位信號，室內(nèi)定位設(shè)備接收室內(nèi)定位信號，利用卡爾曼濾波結(jié)合室內(nèi)定位模型進(jìn)行時(shí)鐘偏差補(bǔ)償。

理想的時(shí)鐘同步是指基站和定位設(shè)備使用同一個(gè)時(shí)鐘信號作為時(shí)鐘源，但這種方式很難實(shí)現(xiàn)，在實(shí)際應(yīng)用中，系統(tǒng)中的每個(gè)設(shè)備都有自己的一套時(shí)鐘系統(tǒng)，彼此相互獨(dú)立。為了建立統(tǒng)一的時(shí)鐘系統(tǒng)，本發(fā)明將現(xiàn)實(shí)中均勻流逝的真實(shí)時(shí)間作為系統(tǒng)時(shí)間，而每個(gè)設(shè)備的時(shí)鐘都是對現(xiàn)實(shí)中每個(gè)時(shí)刻的反映，將其定義為設(shè)備時(shí)間，因此設(shè)備時(shí)間和真實(shí)時(shí)間具有一一對應(yīng)的關(guān)系，設(shè)4個(gè)基站和定位設(shè)備的時(shí)鐘起點(diǎn)時(shí)刻分別為t₀₁、t₀₂、t₀₃、t₀₄、t₀₅，時(shí)鐘頻率分別為λ₁(t)、λ₂(t)、λ₃(t)、λ₄(t)、λ₅(t)。因此對于系統(tǒng)中的任一設(shè)備i(i＝1,2,...,5)，該設(shè)備時(shí)間t_device和與之對應(yīng)的真實(shí)時(shí)間t′_real滿足如下關(guān)系：

系統(tǒng)發(fā)射信號過程如圖3所示，假設(shè)設(shè)備i在該設(shè)備時(shí)間t_send發(fā)送一個(gè)測距信號，并且該設(shè)備對t_send的測量值為經(jīng)過固定系統(tǒng)時(shí)間偏移Δt_i1后，信號在真實(shí)時(shí)間t′_send離開天線，發(fā)射信號過程滿足如下關(guān)系：

在式(3)中，是設(shè)備i測量發(fā)送時(shí)間戳的測量誤差，為零均值高斯白噪聲。

系統(tǒng)接收信號過程如圖4所示，假設(shè)一個(gè)測距信號在真實(shí)時(shí)間t′_receive到達(dá)設(shè)備i的天線，經(jīng)過固定系統(tǒng)時(shí)間偏移Δt_i2后，在該設(shè)備時(shí)間t_receive接收該信號，并且該設(shè)備對t_receive的測量值為接收信號過程滿足如下關(guān)系：

在式(5)中，φ_i為設(shè)備i測量發(fā)送時(shí)間戳的測量誤差，為零均值高斯白噪聲。

信號交流機(jī)制如圖5所示，系統(tǒng)中包含4個(gè)基站?；?為主站，其作用是協(xié)調(diào)系統(tǒng)信號交流機(jī)制，同時(shí)向其它所有基站以及定位設(shè)備發(fā)送測距信號并提供其時(shí)間信息。基站2～基站4為副站，在基站1的控制下，分別向定位設(shè)備和基站1發(fā)送測距信號并提供其時(shí)間信息。定位設(shè)備為實(shí)際中需要被定位的用戶，通過接收基站1～基站4的測距信號并利用基站1～基站4和自身的時(shí)間信息完成定位。系統(tǒng)信號交流機(jī)制以組為單位進(jìn)行信號交流?；?發(fā)送一個(gè)測距信號，基站2和定位設(shè)備分別接收來自基站1的信號后，基站2經(jīng)過一定延遲發(fā)送另一個(gè)測距信號，然后基站1和定位設(shè)備分別接收來自基站2的信號，至此完成一組信號交流，如圖6所示。其它兩組信號交流類似。三組信號交流時(shí)序組成系統(tǒng)信號交流機(jī)制的一個(gè)周期。從系統(tǒng)信號交流機(jī)制中可以看出，定位設(shè)備只是被動接收來自基站的測距信號，不與基站進(jìn)行雙向信息交流，當(dāng)定位設(shè)備接收所有基站在一個(gè)周期內(nèi)的測距信號后，便可利用基站以及自己的時(shí)間信息完成定位，因此系統(tǒng)打破了系統(tǒng)用戶容量的限制，理論上可以同時(shí)定位無限數(shù)量的用戶。此外，用戶數(shù)量的改變不影響系統(tǒng)基站間的信號交流時(shí)序，從而系統(tǒng)具有高度的靈活性和實(shí)用性。

根據(jù)式(2)、式(4)，信號交流機(jī)制的一個(gè)周期的第一組信號交流時(shí)序可以建立如下數(shù)學(xué)關(guān)系：

在式(6)(7)(8)(9)中，t₁₁表示基站1的發(fā)送時(shí)間戳，并且測距信號在真實(shí)時(shí)間t′₁₁離開天線；t₁₂表示基站1的接收時(shí)間戳，并且測距信號在真實(shí)時(shí)間t′₁₂到達(dá)天線；t₅₁和t₅₂表示定位設(shè)備的接收時(shí)間戳，并且測距信號分別在真實(shí)時(shí)間t′₅₁和t′₅₂到達(dá)天線；Δt₁₁和Δt₁₂分別表示基站1發(fā)射信號和接收信號過程的固定時(shí)間延遲；Δt₅₂表示定位設(shè)備接收信號過程中的固定時(shí)間延遲。

在實(shí)際中，信號交流時(shí)序的一個(gè)周期持續(xù)時(shí)間相當(dāng)短，通常在毫秒級，而室內(nèi)用戶的移動速度通常比較慢，所以在一個(gè)周期內(nèi)可以認(rèn)為用戶是靜止不動的。在非視距環(huán)境下，信號在節(jié)點(diǎn)之間傳播的時(shí)間相對于視距環(huán)境下會偏大。由于基站1～基站4被安裝在固定位置，因此環(huán)境對基站間的信號交流的影響是固定的，而定位設(shè)備的位置是移動的，因此環(huán)境對基站和定位設(shè)備間的信號交流的影響隨著定位設(shè)備位置的改變而改變。因此，信號傳播時(shí)間和實(shí)際節(jié)點(diǎn)間距離滿足如下關(guān)系：

d₁₅＝c(t′₅₁-t′₁₁-Δt′₁₅(P)) (10)

d₁₂＝c(t₁₂′-t₂₂′-Δt′₂₁) (11)

d₂₅＝c(t′₅₂-t′₂₂-Δt′₂₅(P)) (12)

在式(10)(11)(12)中，P表示定位設(shè)備的位置；Δt′₂₁表示環(huán)境對信號由基站2傳播到基站1所引起的傳播時(shí)間誤差；Δt′₁₅(P)和Δt′₂₅(P)分別表示環(huán)境對信號由基站1傳播到定位設(shè)備和基站2傳播到定位設(shè)備所引起的傳播時(shí)間誤差；d₁₂、d₁₅、d₂₅分別表示基站1到基站2的距離、基站1到定位設(shè)備的距離、基站2到定位設(shè)備的距離；c表示光速。

由式(10)(11)(12)可得：

c(t′₁₂-t′₁₁)-c(t′₅₂-t′₅₁)＝d₁-Δd₁ (13)

其中：

d₁＝d₁₂+d₁₅-d₂₅ (14)

Δd₁＝c(Δt′₂₅(P)-Δt′₁₅(P)-Δt′₂₁) (15)

式中，d₁和Δd₁分別表示實(shí)際節(jié)點(diǎn)間的距離和環(huán)境造成的距離誤差。由式(6)(7)(8)(9)和中值定理可得：

式中，ξ₅和ξ₁分別表示區(qū)間(t′₅₁+Δt₅₂,t′₅₂+Δt₅₂)和(t′₁₁-Δt₁₁,t′₁₂+Δt₁₂)中的某個(gè)常量。由式(13)(16)(17)可得：

c[k₁(t₁₂-t₁₁)-k₅(t₅₂-t₅₁)]-Δd＝d₁-Δd₁ (18)

其中：

Δd＝c(Δt₁₂+Δt₁₁) (21)

式中，k₁和k₅分別表示基站1和定位設(shè)備的時(shí)間分辨率；Δd表示由基站1在發(fā)射信號過程和接收信號過程中的固定時(shí)間偏移引起的距離常量。一個(gè)周期內(nèi)的其它兩組推到過程類似，本發(fā)明便不進(jìn)行贅述。由于設(shè)備的時(shí)間分辨率在短時(shí)間內(nèi)不變或變化極小，故在一個(gè)周期內(nèi)認(rèn)為設(shè)備的時(shí)間分辨率為常數(shù)。通過推導(dǎo)其它兩組方程可得系統(tǒng)定位模型為：

時(shí)鐘偏差補(bǔ)償方法主要包含了如下5個(gè)步驟：

1)從信號交流機(jī)制中分離出一組信號交流過程，利用接收信號和發(fā)射信號模型建立時(shí)間的數(shù)學(xué)模型；

2)結(jié)合基站和定位設(shè)備的位置信息，建立時(shí)間和距離的數(shù)學(xué)模型；

3)計(jì)算時(shí)鐘偏差補(bǔ)償系數(shù)觀測值，即定位設(shè)備和基站的時(shí)間分辨率比值；

4)建立時(shí)鐘偏差補(bǔ)償?shù)臓顟B(tài)方程和觀測方程，進(jìn)行卡爾曼濾波解算，得到時(shí)鐘偏差補(bǔ)償系數(shù)的估計(jì)值；

5)通過判斷時(shí)鐘偏差補(bǔ)償系數(shù)觀測值相對于時(shí)鐘偏差補(bǔ)償系數(shù)估計(jì)值的差值是否超過門限值，去除觀測數(shù)據(jù)中的野值，得到時(shí)鐘偏差補(bǔ)償后的結(jié)果。

圖5中給出的信號交流機(jī)制滿足如下關(guān)系：

定位設(shè)備在t′_5,n時(shí)刻所在位置為P_n，定位設(shè)備在t′_5,n+1時(shí)刻所在位置為P_n+1，兩個(gè)位置的距離為定位設(shè)備在兩個(gè)時(shí)刻間的平均速度為v_n，實(shí)際中的位置關(guān)系，如圖7所示。得：

根據(jù)式(23)(24)(25)(26)(27)(28)(29)可得：

在(30)中，

在式(31)中，和分別表示定位設(shè)備和基站1當(dāng)前時(shí)刻的時(shí)間分辨率；x_n表示時(shí)鐘偏差補(bǔ)償系數(shù)。

當(dāng)定位設(shè)備處于靜止?fàn)顟B(tài)時(shí)，參數(shù)a和b的值為0；當(dāng)定位設(shè)備處于運(yùn)動狀態(tài)時(shí)，由于參數(shù)a的值極小，因此a的值仍然當(dāng)成0處理。對于定位設(shè)備短距離移動，信號從基站1傳播到定位設(shè)備的信道通常不會發(fā)生驟變，因此環(huán)境對信號的影響基本不變，在這種情況下，b的值也可以當(dāng)成0處理。但若信號傳播的信道發(fā)生改變，則b將會顯著增大或減小，可以通過判斷測量值和估計(jì)的偏差是否超過某一設(shè)定門限值將該組數(shù)據(jù)舍棄掉。

由于系統(tǒng)中的設(shè)備的時(shí)鐘源都有較高的穩(wěn)定性，時(shí)鐘偏差補(bǔ)償系數(shù)是穩(wěn)定的，受到零均值高斯白噪聲的影響，時(shí)鐘偏差補(bǔ)償模型的系統(tǒng)狀態(tài)方程為：

x_n+1＝x_n+η (34)

式中，η為零均值高斯白噪聲序列，表示系統(tǒng)時(shí)鐘本身漂移導(dǎo)致的偏差。由式(30)，并使a和b的值為0可得：

系統(tǒng)的測量值用w表示，則?。?/p>

由于系統(tǒng)對時(shí)間戳的測量值具有隨機(jī)噪聲偏差，故：

其中ε可以近似為零均值的高斯白噪聲。

由式(35)(36)(37)可得時(shí)鐘偏差補(bǔ)償模型的量測方程為：

w_n＝x_n+ε (38)

根據(jù)式(34)(38)，卡爾曼濾波算法可以表示為：

該系統(tǒng)為線性時(shí)不變系統(tǒng)，卡爾曼濾波算法可優(yōu)選為極限卡爾曼濾波算法，因?yàn)樵诰€性系統(tǒng)變化緩慢的情況下，可證明{G_n}是收斂的。

?。?/p>

極限卡爾曼濾波算法為：

綜上，時(shí)鐘偏差補(bǔ)償表達(dá)式為，

步驟四：利用泰勒迭代法估計(jì)設(shè)備空間坐標(biāo)；

記4個(gè)基站坐標(biāo)分別為BS1(x₁,y₁,z₁)、BS2(x₂,y₂,z₂)、BS3(x₃,y₃,z₃)、BS4(x₄,y₄,z₄)；設(shè)需要被定位的定位設(shè)備的坐標(biāo)為(x,y,z)；設(shè)定位定位設(shè)備和4個(gè)基站的距離分別為r₁、r₂、r₃、r₄，則有：

上述方程直接求解定位設(shè)備的坐標(biāo)(x,y,z)比較困難，可以將方程式進(jìn)行泰勒二項(xiàng)式展開，然后省略前兩項(xiàng)以外的所有項(xiàng)。泰勒展開式就變成一個(gè)線性的公式，可以通過求解這個(gè)線性公式獲得最終的解。

泰勒迭代法需要一個(gè)估計(jì)值，假設(shè)(x_R,y_R,z_R)表示定位設(shè)備的估計(jì)位置，則可得一個(gè)偏差方程：

式中，Δx、Δy、Δz表示估計(jì)和實(shí)際位置的偏差。

泰勒迭代法利用初值進(jìn)行估計(jì)運(yùn)算，每次運(yùn)算都能夠得到一個(gè)更優(yōu)化的值，以此迭代下去直到獲取到最優(yōu)值。計(jì)算定位節(jié)點(diǎn)的偏差(Δx,Δy,Δz)可得：

式(45)中，Q表示基于到達(dá)時(shí)間差定位方法測量值的協(xié)方差。

式中，r_1R、r_2R、r_3R、r_4R分別代表定位設(shè)備估計(jì)位置到基站1～基站4的距離。計(jì)算得出一組(Δx,Δy,Δz)，利用式(44)，求解出一個(gè)新的坐標(biāo)點(diǎn)，將該坐標(biāo)點(diǎn)作為新的定位設(shè)備估計(jì)點(diǎn)進(jìn)行下次迭代。

步驟五：重復(fù)步驟三、步驟四，進(jìn)行迭代計(jì)算，直至Δx、Δy、Δz足夠小后，到達(dá)了門限ε，即Δx²+Δy²+Δz²＜ε²，系統(tǒng)工作一個(gè)小時(shí)，采集系統(tǒng)對時(shí)間戳的測量數(shù)據(jù)，后停止迭代，得到定位節(jié)點(diǎn)坐標(biāo)值(x,y,z)，完成單點(diǎn)的室內(nèi)定位。

為了驗(yàn)證靜態(tài)條件下的室內(nèi)定位精度，變換定位設(shè)備位置，共測試15個(gè)點(diǎn)。

用MATLAB軟件將測量數(shù)據(jù)對時(shí)鐘偏差補(bǔ)償算法進(jìn)行仿真，15組實(shí)驗(yàn)中4組仿真結(jié)果如圖8Aa、圖8Ab、圖8Ba、圖8Bb、圖8Ca、圖8Cb、圖8Da、圖8Db、所示。結(jié)果表明時(shí)鐘偏差補(bǔ)償算法可以有效的濾除噪聲，實(shí)時(shí)從觀測數(shù)據(jù)中估計(jì)出時(shí)鐘偏差補(bǔ)償系數(shù)。

用MATLAB軟件仿真系統(tǒng)輸出時(shí)間數(shù)據(jù)，仿真結(jié)果如圖9a、圖9b所示，結(jié)果表明每一個(gè)階梯表示一個(gè)采樣點(diǎn)的一組時(shí)間數(shù)據(jù)，系統(tǒng)輸出的時(shí)間數(shù)據(jù)在具有極好的穩(wěn)定性。

用MATLAB軟件仿真由時(shí)間數(shù)據(jù)和記錄的距離數(shù)據(jù)組成的45個(gè)離散點(diǎn)，仿真結(jié)果如圖10所示，結(jié)果表明時(shí)間數(shù)據(jù)和距離數(shù)據(jù)有較好的線性關(guān)系。

用最小二乘法估計(jì)系數(shù)確定線性模型，通過系統(tǒng)模型計(jì)算距離，用MATLAB軟件仿真計(jì)算的距離和實(shí)驗(yàn)記錄的距離差，仿真結(jié)果如圖11所示，結(jié)果表明通過模型計(jì)算出的距離相對于真實(shí)距離的偏差為229mm(RMS)。

在室內(nèi)靜態(tài)定位實(shí)驗(yàn)中，在距地面1500mm的平面內(nèi)，選擇15個(gè)合適的測試點(diǎn)用來固定定位設(shè)備進(jìn)行定位。測試點(diǎn)位置平面圖如圖12所示。室內(nèi)靜態(tài)實(shí)驗(yàn)定位結(jié)果如圖13a、圖13b、圖13c所示。結(jié)果表明當(dāng)定位設(shè)備處于靜止?fàn)顟B(tài)時(shí)，系統(tǒng)定位坐標(biāo)值具有較好的穩(wěn)定性。定位誤差如圖14a、圖14b、圖14c所示。結(jié)果表明系統(tǒng)在X軸和Y軸的定位誤差分別為166mm(RMS)和119mm(RMS)，Z軸的定位誤差為483mm(RMS)，具有較高的定位精度。

具體實(shí)施例二.室內(nèi)動態(tài)定位

在室內(nèi)動態(tài)定位實(shí)驗(yàn)中，用戶持定位設(shè)備在室內(nèi)走動，其他條件和步驟與具體實(shí)施例一中的靜態(tài)試驗(yàn)一致。

系統(tǒng)在水平面內(nèi)的輸出軌跡如圖15所示，系統(tǒng)輸出的坐標(biāo)值如圖16a、圖16b、圖16c所示。系統(tǒng)可以實(shí)現(xiàn)精確的室內(nèi)三維定位。

通過室內(nèi)靜態(tài)定位實(shí)驗(yàn)和室內(nèi)動態(tài)定位實(shí)驗(yàn)，驗(yàn)證了本發(fā)明方法的有效性。