基于UWB和IMU融合的UWB弱信號環境下高精度定位算法

摘 要：針對全球導航衛星系統（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ，ＧＮＳＳ） 在受限空間內定位精度差，甚至無法定位和無線超寬帶（Ｕｌｔｒａ-Ｗｉｄｅ Ｂａｎｄ，ＵＷＢ） 脈沖室內定位技術在非視距（Ｎｏｎ Ｌｉｎｅ ｏｆ Ｓｉｇｈｔ，ＮＬｏＳ） 環境下定位精度差以及定位穩定性低等問題，提出以誤差狀態卡爾曼濾波（Ｅｒｒｏｒ Ｓｔａｔｅ Ｋａｌｍａｎ Ｆｉｌｔｅｒ，ＥＳＫＦ） 為基礎并將ＵＷＢ 定位技術和慣性傳感器（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ，ＩＭＵ） 技術相融合，設計一套ＵＷＢ 弱信號環境下的ＵＷＢ 定位算法模型，實現ＵＷＢ 弱信號環境下的厘米級定位。通過優化傳統ＥＳＫＦ 方法以及融合ＵＷＢ 和ＩＭＵ 的測量數據，解決傳統ＵＷＢ 定位在ＮＬｏＳ 環境下定位精度較差和定位結果容易發生偏移等問題。實驗研究結果表明，在導航實驗室內，系統在東－北－天（ＥＮＵ） 坐標系中東方向、北方向軸和ＯＸＹ 平面上的精度分別提高了２． ８７％ 、１２． ０２％ 和５． ７１％ ，方差分別降低了５． ８０％ 、１８． ０６％ 和５． ７１％ 。在地下通道ＵＷＢ 弱信號環境下，系統在東方向、北方向和ＯＸＹ 平面上的精度分別提高了１２． ０８％ 、２４． １０％ 和１６． ０８％ ；方差分別降低了８． １２％ 、３２． ７４％ 和１２． ２３％ 。所提出的算法模型有效改善了ＵＷＢ 室內定位技術在ＮＬＯＳ 情況下，定位精度低、系統定位穩定性差的問題，降低了定位成本，具有很強的實用性。

關鍵詞：超寬帶；慣性傳感器；誤差狀態卡爾曼濾波；非視距

中圖分類號：ＴＮ９６１ 文獻標志碼：Ａ 開放科學（資源服務）標識碼（ＯＳＩＤ）：

文章編號：１００３－３１０６（２０２４）０７－１７２１－１１

０ 引言

全球導航衛星系統（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ ＳａｔｅｌｌｉｔｅＳｙｓｔｅｍ，ＧＮＳＳ）已廣泛應用于人們的日常生活中，為人們的出行提供了諸多方便［１］。但不可否認的是，ＧＮＳＳ 受到衛星空間載荷限制、信號發射功率小等因素的影響，僅可以在衛星信號良好的室外提供較好的導航定位服務。遇到例如室內、峽谷和隧道等ＧＮＳＳ 信號較弱的環境區域時，經常會出現信號失鎖、周跳頻繁等情況［２］，嚴重時可能會出現無法提供定位服務的現象。然而，隨著現代城市化基礎建設和室內空間開發進程的不斷加快，城鎮地區的居民每天有９０％ 以上的時間都在密集市區和室內環境下生活。人們對城市中的交通運輸樞紐、大型購物商場、生產車間和圖書館等基礎設施中對于人員定位、物資運輸的監控等信息服務都有著很大的需求。同時，如突發災害時，救援人員在工作時需要準確而迅速地獲取導航定位信息。這些需求都對在ＧＮＳＳ 信號弱的環境內提供高精度位置服務提出了很高的要求。

近些年來，隨著無線通信技術、微控制技術的發展，越來越多的室內定位技術系統應用在日常的生活以及工業生產中。這些室內定位系統主要應用包括無線網絡（Ｗｉｒｅｌｅｓｓ Ｆｉｄｅｌｉｔｙ，ＷｉＦｉ）、無線超寬帶（Ｕｌｔｒａ-Ｗｉｄｅ Ｂａｎｄ，ＵＷＢ）脈沖技術、射頻識別（ＲａｄｉｏＦｒｅｑｕｅｎｃｙ Ｉｄｅｎｔｉｆｉｃａｔｉｏｎ，ＲＦＩＤ）、藍牙（Ｂｌｕｅｔｏｏｔｈ）等的無線通信技術以及慣性導航系統（ＩｎｅｒｔｉａｌＮａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ，ＩＮＳ ）實現準確定位。首先就ＷｉＦｉ 定位技術系統而言，因為室內ＷｉＦｉ 定位技術受室內設施設備以及室內環境等影響因素干擾較大，不同的室內環境需要根據實際情況重新鋪設無線接入點（Ｗｉｒｅｌｅｓｓ Ａｃｃｅｓｓ Ｐｏｉｎｔ，ＡＰ）設備，所以基于ＷｉＦｉ 的室內定位系統的遷移性較差［３］。同時，在實際使用過程中，基于ＷｉＦｉ 的室內定位系統環境搭建步驟較為復雜，實用性不夠理想。采用基于ＷｉＦｉ的室內定位技術系統需要在訓練階段構建指紋特征庫。在實施過程中，構建過程過于繁瑣，并且構建完成后若需要增刪定位設備時需要重新構建指紋特征庫，不便于在實際的生產和生活中使用［４］。其次，對于藍牙定位技術來說，藍牙定位技術雖然具有自身優勢，如不易受障礙物遮擋的影響、對環境要求較低等，但是由于藍牙的傳輸距離較短，所以基于藍牙的室內定位系統只能實現較小區域內的定位，不適合在環境復雜、有干擾的大型生產車間和大型購物商場（城）中使用，使用場景達不到理想化［５］。再次，ＲＦＩＤ 室內定位技術系統雖然可達到厘米級，但是由于該系統在實際應用過程中定位的距離較短，定位的范圍較小，且難以實現和其他設備之間進行通信，所以實際利用價值較低［６］。慣性導航技術雖可以短時間提供精度較高的定位服務，但隨著定位時間的增長，累計誤差會逐漸增加，定位精度會越來越低，所以ＩＮＳ 一般只能與其他定位技術融合使用，不能單獨使用［７］。ＵＷＢ 技術是以非常窄的帶寬以及超快的傳輸速度為主要特點的無線電傳輸技術，與傳統的帶通調制不同，使用完全不同類型的無線信號傳輸方式。以這種無線傳輸技術為主的定位技術的主要優點為傳輸速度快、傳輸信息量大和安全性較高，同時可以與許多其他設備聯合實時運行。所以在實際生產應用過程中使用無線ＵＷＢ 技術系統來實現定位是一種實用性高，并且是可靠的室內定位技術［８］。

然而，雖然ＵＷＢ 室內定位技術具有功耗低，傳輸可靠性高、安全性強和傳輸距離遠等優點，但當遇到非視距（Ｎｏｎ Ｌｉｎｅ ｏｆ Ｓｉｇｈｔ，ＮＬｏＳ）環境時，ＵＷＢ 信號會產生較大的偏移甚至丟失，ＵＷＢ 定位技術的定位精度以及定位穩定性都會大大下降。針對這一問題，研究人員對室內ＵＷＢ 定位技術在ＮＬｏＳ 情況下的定位問題進行了大量的研究。徐曉蘇等［９］提出一種通過對坐標系統進行２ 次解算從而提高ＵＷＢ定位精度的定位方法，減少了基站布局過程中高度對定位結果的影響。

宰昶豐等［１０］將采集到的定位數據通過ＵＷＢ 穿越障礙物的公式推導結合ＵＷＢ 無線載波通信信號減弱模型，進行衰減能量測算，減小定位時的信號能量衰減誤差，提高測距和定位精度。宋波等［１１］提出了一種新型識別ＮＬｏＳ 信號的方法。該方法用視距的均值以及協方差矩陣表示ＮＬｏＳ 環境中障礙物的概率分布，并且通過收集來的樣本建立模型矩陣，從而識別ＮＬｏＳ 信號。李旭等［１２］提出了一種ＵＷＢ 和慣性測量單元緊組合而成的導航定位方法，該定位方法的主要特點是人機交互方式多、模型之間轉化較為快速平滑。隋心等［１３］提出了一種在間斷ＮＬｏＳ環境下定位效果較好的誤差檢測方法，該方法的主要特點是利用三角不等式法判斷ＮＬｏＳ 環境中不同障礙物之間的距離值，通過相近的定位基站和標簽解算出來的距離值來判斷ＮＬｏＳ 環境中障礙物所在的具體位置。然而，上述文獻提出的方法僅提高了ＵＷＢ 在ＮＬｏＳ 情況下的定位精度，沒有對系統定位的穩定性做進一步討論。上述文獻提出的方法計算較復雜，在實用性方面需要進一步加強。

為解決ＵＷＢ 室內定位技術在ＮＬｏＳ 環境下的高精度、強穩定性的定位問題，本文設計了一個基于無線ＵＷＢ 脈沖技術和慣性測量單元的室內高精度定位算法模型。該算法模型以誤差狀態卡爾曼濾波（Ｅｒｒｏｒ Ｓｔａｔｅ Ｋａｌｍａｎ Ｆｉｌｔｅｒ，ＥＳＫＦ）為基礎框架，通過融合慣性傳感器（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ，ＩＭＵ）和ＵＷＢ 的測量數據，緩解和修正ＵＷＢ 在ＮＬｏＳ 環境下產生的定位誤差，在一定程度上提高定位系統在ＮＬｏＳ 環境下的定位精度和定位穩定性。經實驗驗證，本文算法具有抗干擾能力強、精度較高等優點，能很好地解決實際導航定位過程中出現的ＮＬｏＳ 以及多徑效應等問題，可以滿足復雜環境下的長時間、高精度的定位需求。

１ ＥＳＫＦ 融合濾波模型與仿真研究

為解決ＵＷＢ 室內定位技術在ＮＬｏＳ 環境下的定位精度下降，定位發生較大偏移等問題，選擇使用多傳感器融合的方式提高ＵＷＢ 在ＮＬｏＳ 環境下的定位精度，選擇使用ＩＭＵ 和ＵＷＢ 設備進行融合定位。

１． １ 坐標系統

當使用多傳感器融合實現導航定位時，導航參考坐標的選擇是一個重要的問題。本算法中使用到的ＩＭＵ 主要由三軸加速度計測量單元和三軸陀螺儀測量單元這兩部分構成，通過這兩部分的測量可以首先得出定位標簽在載體坐標系下的加速度和角速度的信息。然后，將所得到的信息通過積分運算得出定位標簽的位置、速度和姿態等信息。最后，將其轉化到導航坐標系下，導航坐標系的選擇對整個定位系統的實現十分關鍵。

在導航定位系統中，ＩＭＵ 通常被用于追蹤確定定位標簽的運動狀態和姿態信息，因此需要使用導航坐標系的概念。導航坐標系一般固定在定位標簽起始時刻的位置。導航坐標系需要進行重力對齊，即導航坐標系的Ｚ 軸負方向與當地重力方向對齊。在多數情況下定位過程都是在地面平整的區域進行的，如倉庫、地下室和地下人行通道等。但在實際應用過程，不可忽略地會出現定位標簽處于斜面中的情況，如峽谷、山區等。此時導航坐標系的重力對齊十分重要。如果不進行重力對齊，因為導航坐標系為定位標簽的起始位置，Ｚ 軸正方向為當前路面的法線方向。而當定位標簽的起始位置處于斜面時，導航坐標系會整體存在一個坡面的傾角。此時，定位標簽的位姿與運動狀態是基于傾斜面表示的，會對后續導航定位造成一定的誤差。

因此，通常在使用ＩＭＵ 時，在初始化階段要進行重力對齊，估計當前位置的重力方向，求得初始位置的三軸姿態角，定義為初始姿態，此時導航坐標系的Ｚ 軸方向與當前重力方向平行。在實際定位過程中，ＯＸＹ 平面為切平面，因此重力對齊后的導航坐標系稱為當地切平面坐標系。在實際實驗測量過程中，由于定位時接收機與定位基站高度相對統一，所以高度對測量的結果的影響很小，本實驗將各個基站與接收機視作統一平面，在定位時視作二維定位。在后續討論中，Ｘ 軸方向對應東－北－天（ＥＮＵ）坐標系中的東方向，Ｙ 軸方向對應ＥＮＵ 坐標系中的北方向。

在對齊重力的情況下，一般存在２ 種方向的定義方式：東－北－天（ＥＮＵ）、北－東－地（ＮＥＤ）或者使用簡化后的二維投影坐標例如通用橫墨卡托格網系統（Ｕｎｉｖｅｒｓａｌ Ｆｒａｎｓｖｅｒｓｅ Ｍｅｒｃａｔｏｒ Ｇｒｉｄ Ｓｙｓｔｅｍ，ＵＴＭ）坐標等。其中，ＥＮＵ 局部坐標系采用三維直角坐標系來描述地球表面，在小范圍內固定了原點。但是此類方法忽略了地球曲率的因素，會造成誤差，只適用于小范圍場景。但是本文算法模型主要應用場景為室內以及密閉空間的小范圍區域，所以使用ＥＮＵ坐標系也很合適。

基于上述研究，若要將載體坐標系下的矢量旋轉變化到ＥＮＵ 導航坐標系下可以使用歐拉角法、方向余弦陣法和四元數法等方法。

歐拉角法雖簡單直觀，但會出現萬向節死鎖現象。方向余弦陣法雖然有一定的應用場景，但該方法存在計算量較大的問題，在定位系統中應用會導致定位結果冗余量過大的問題。故本文考慮到計算量和算法的復雜度問題以及實際應用情況下需要響應速度快的要求，最終選用了四元數法。根據文獻［１４］，四元數微分方程如下：

式中：ω ＝ ［ωｘ，ωｙ，ωｚ］ Ｔ ，ωｘ、ωｙ、ωｚ 分別為定位標簽中ＩＭＵ 里的陀螺儀在ＸＹＺ 坐標系中測量出的定位標簽在定位過程中的各方向的角速度；ｑ ＝ ［ｑ０ ｑ１ｑ２ ｑ３］ Ｔ 為將載體坐標系轉化為導航坐標系時采用的四元數法中的轉移矩陣，且此矩陣的形式是固定的，即ｑ２０＋ｑ２１＋ｑ２２＋ｑ２３＝ １；

載體坐標系（ｂ 系）過渡到導航坐標系（ｅ 系）的旋轉矩陣［１４］通過四元數法推導得到：

定位標簽中ＩＭＵ 的陀螺儀的測量值通過算法變化到導航坐標系下，姿態信息可用橫滾角γ、俯仰角θ 和偏航角ψ 來表示，具體如下：

γ ＝ ａｔａｎ２（２（ｑ２ ｑ３ ＋ ｑ０ ｑ１ ），ｑ２０－ ｑ２１－ ｑ２２＋ ｑ２３）， （３）

θ ＝ ａｓｉｎ（２（ｑ０ ｑ２ － ｑ１ ｑ３ ））， （４）

ψ ＝ ａｔａｎ２（２（ｑ１ ｑ２ ＋ ｑ０ ｑ３ ）（ｑ２０＋ ｑ２１－ ｑ２２－ ｑ２３）），（５）

式中：ａｔａｎ２（）為反正切值函數，表示角度為（－π，π）。

通過旋轉變化，可以將ｂ 系下的加速度計測量值轉化到ｅ 系：

ａ ｅ ＝ Ｃｅｂ ａｂ － ［０ ０ ｇ］ Ｔ ， （６）

式中：ａｅ ＝ ［ａｅｘ ａｅｙ ａｅｚ］Ｔ表示ｅ 系下的三軸加速度，ａｂ ＝ ［ａｂｘ ａｂｙ ａｂｚ］ Ｔ表示ｂ 系下的三軸加速度，ｇ 表示當地重力加速度。對加速度ａｅ 積分，即得到ｅ 系下的速度和位置。

１． ２ ＵＷＢ 和ＩＭＵ 融合濾波算法研究

１． ２． １ 基于ＵＷＢ 無線測距定位原理

無線通信定位原理主要是首先通過無線電信號在定位標簽以及定位基站之間來回通信，從而獲得二者之間的距離等信息。然后根據得到的距離等信息。最后通過諸如最小二乘法等的定位算法解算出標簽節點的坐標。但是在實際測量過程中，由于存在誤差的影響，所以通常需要進行算法優化和估計。

ＵＷＢ 技術早期源于２０ 世紀６０ 年代。與傳統通信技術中運用的一般載波不同的是，ＵＷＢ 中使用的是能量較短且帶寬較窄的脈沖。ＵＷＢ 信號僅需要一個納秒級以下的脈沖便可通過天線進行發送，通過改變脈沖的幅度、時間或相位來加載所需要傳輸的信息?；冢眨祝?信號的定位系統不僅具有定位準確度高以及穿透力強的特點，而且在多傳感、多信號的復雜環境下，ＵＷＢ 信號不易干擾其他無線信號，非常適合在多傳感器融合的環境下使用。ＵＷＢ技術使用超窄脈沖精確測量信號到接收位置的時間，解算基站到接收機的距離，通過位置解算方程完成室內定位需求。近年來ＵＷＢ 的使用方案在數據發送領域以及室內定位領域快速推廣，尤其是基于ＵＷＢ 空間定位方案目前廣泛應用于地下停車場、大型倉庫和展覽廳等密閉空間內，定位精度可達厘米級。

本文使用基于到達時間差（Ｔｉｍｅ Ｄｉｆｆｅｒｅｎｃｅ ｏｆＡｒｒｉｖａｌ，ＴＤＯＡ）的傳統定位模型。ＴＤＯＡ 的定位原理是通過測量定位標簽和定位基站之間的時間差來計算定位標簽和各個基站之間的距離值，進而求解出最終的坐標。具體而言，通過將時間差乘以無線傳輸信號在環境中的傳輸速度，可以計算出定位標簽與各個基站之間的距離值，并利用這些距離值求解方程組則得出定位標簽的最終坐標。ＴＤＯＡ 定位原理如圖１ 所示?？梢钥闯觯趯嶋H定位過程中，若要完成通過ＴＤＯＡ 定位原理的定位，首先至少需要布置３ 個基站，定位原理可以理解為以２ 個基站為圓心畫２ 條雙曲線，雙曲線的交點則為定位過程中定位標簽所處的位置。具體為，在ＴＤＯＡ 定位系統的布置方式中點Ａｎｃｈｏｒ１ 是主基站，坐標值為（Ｘ１，Ｙ１），點Ａｎｃｈｏｒ２、Ａｎｃｈｏｒ３ 是副基站，坐標值分別為（Ｘ２，Ｙ２），（Ｘ３，Ｙ３），ＴＡＧ 是標簽，坐標值為（Ｘ，Ｙ）。假定電子標簽信號到達３ 個基站的時間為ｔ１、ｔ２、ｔ３，可以得到：

１． ２． ２ 濾波方法的選擇

為實現ＵＷＢ 和ＩＭＵ 的數據融合，使用濾波的方式將二者融合。選擇以貝葉斯公式作為理論框架的濾波方法，例如卡爾曼濾波（Ｋａｌｍａｎ Ｆｉｌｔｅｒ，ＫＦ）、擴展卡爾曼濾波（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ，ＥＫＦ）和ＥＳＫＦ。其中ＫＦ 模型和ＥＫＦ 模型屬于直接濾波模型，ＥＳＫＦ 屬于間接濾波模型，都是貝葉斯濾波的具化形式之一，遵循貝葉斯濾波的基礎理論。然而，由于ＫＦ 只適用于線性系統，所以在處理復雜的ＮＬｏＳ環境時，僅用ＫＦ 是不合適的。ＥＫＦ 和ＥＳＫＦ 可用于解決非線性問題。但與ＥＫＦ 相比，ＥＳＫＦ 的主要特點是濾波過程中的工作點坐標更接近原點（０，０），并且能夠進一步有效地減少奇異值情況的產生，能夠更好地確保線性化過程的合理和有效。所以本文選擇ＥＳＫＦ 為基礎框架并加以改進作為本系統中融合ＩＭＵ 和ＵＷＢ 的方法［１５］。

１． ２． ３ 融合濾波思路

通過測量得到ＩＭＵ 和ＵＷＢ 的初始數據之后，首先將ＩＭＵ 的初始測量數據在ＥＳＫＦ 預測環節內進行預處理和位置預測，通過預處理和位置預測的結果得到預測誤差狀態和名義狀態。接著將ＵＷＢ 各個定位基站測量得到的偽距值與名義狀態中各個基站到估計位置的距離的值作差進行對比，從而計算得出ＵＷＢ 的各個基站在定位中的權重占比。然后將定位過程中的每個定位基站相對于定位標簽的權重占比作為初始值進行最小二乘處理，反復迭代這個過程。最后對誤差狀態進行修正從而獲得最終狀態［１５］。整體融合思路框圖如圖２ 所示。

１． ２． ４ ＥＳＫＦ 融合濾波融合ＩＭＵ 和ＵＷＢ

ＥＳＫＦ 在使用過程中將傳統的狀態量分為三部分，分別為真實狀態、名義狀態和誤差狀態。ＥＳＫＦ的核心思想為對系統的誤差狀態求解，再與名義狀態合并，最終獲得系統的實際狀態。ＥＳＫＦ 的步驟主要包括名義狀態方程的確定、誤差狀態方程的確定、修正ＮＬｏＳ 數據以及誤差狀態量測更新［１６］。

① 確定名義狀態方程以及信號的預處理

名義狀態是指系統在不考慮噪聲影響下的狀態，主要作用是確定定位標簽的運動趨勢。在定位初始階段確定名義狀態的主要作用是幫助系統估計初步位置，對ＵＷＢ 測量的偽距進行ＮＬｏＳ 環境下的判斷和修正，使用ＩＭＵ 測量的運動狀態進行更新。在ＩＭＵ 運動方程的基礎上，假設噪聲項全為０，選取位置ｐ、速度ｖ 及四元數ｑ 作為名義狀態。為使四元數名義狀態始終為單位四元數，保證迭代收斂，定義：

ｑ｛ｖ｝ ＝ ｅ ｖ/２。（９）

根據標簽運動方程與四元數姿態方程［１４］，有：

式中：ａｍ 、ａｂ 分別為定位標簽運動時，ＩＭＵ 中三軸加速度計測量的加速度的測量值和名義值；ωｍ 、ωｂ 分別為ＩＭＵ 中三軸陀螺儀測量角速度的測量值和名義值，Ｒ 為ＩＭＵ 到慣性系旋轉的轉移矩陣，ｐｋ＋１ 為ｋ＋１ 時刻名義位置矩陣，ｐｋ 為ｋ 時刻名義位置矩陣，ｖｋ＋１ 為ｋ＋１ 時刻定位標簽中ＩＭＵ 加速度計的名義矩陣，ｖｋ 為ｋ 時刻定位標簽中ＩＭＵ 加速度計的名義矩陣，ｑｋ＋１ 為ｋ＋１ 時刻定位標簽中ＩＭＵ 陀螺儀的姿態四元數矩陣，ｑｋ 為ｋ 時刻姿態四元數矩陣，ｇ 為定位標簽所處位置的加速度矩陣。

將式（９）帶入式（１０），有：

式中：θ ＝ ||（ωｍ －ωｂ）Δｔ|| ，υ ＝ （ωｍ －ωｂ）Δｔ/θ 。利用２ 個單位四元數的乘積仍為單位四元數的特性，保證迭代收斂。

② 通過名義狀態和噪聲誤差確立誤差狀態方程

在步驟①中通過ＩＭＵ 的測量信息計算出的名義狀態未考慮到噪聲的影響，如果使用該名義狀態繼續計算下去會使得系統的定位誤差越來越大。所以需要通過確立誤差狀態，再利用誤差狀態對步驟①中的名義狀態中的誤差進行修改，從而確保定位結果不受誤差影響。所以，選取位移誤差δｐＴ、速度誤差δνＴ、姿態誤差δθＴ、陀螺儀零偏誤差δωＴｂ及加速度計零偏誤差δａＴｂ作為誤差狀態，記作：

式中：Ｋｋ＋１ 為定位標簽在ｋ＋１ 時刻的ＫＦ 中的卡爾曼增益，Ｐｋ＋１ ｜ ｋ 為定位標簽在ｋ＋１ 時刻的先驗估計協方差，Ｈ 為定位標簽中ＵＷＢ 的狀態量到實際測量的轉移矩陣，Ｖ 為定位標簽在定會過程中的測量噪聲矩陣，Ｐｋ＋１ ｜ ｋ＋１ 為ｋ＋１ 時刻的后驗估計協方差，Ｙｋ為觀測矩陣，Ｘｋ＋１ ｜ ｋ＋１ 為ｋ ＋１ 時刻后驗估計狀態矩陣。這些符號是ＥＳＫＦ 中量測更新方程的關鍵組成部分，用于計算系統狀態的后驗估計值。

為了得到定位標簽的定位狀態信息，本文融合了量測更新后的２ 個狀態公式：誤差狀態式（１２）和名義狀態式（１０），從而完成整個算法模型的建立。

２ 算法驗證與測試

２． １ 視距以及ＮＬｏＳ 環境對ＵＷＢ 定位精度的影響

為驗證ＮＬｏＳ 環境是否一定會對定位精度產生影響以及不同障礙物對視距的影響程度，本文首先設計了視距條件良好和視距環境較差的２ 種情況下的對照實驗。根據文獻［２０］可知，在定位標簽靜態的情況下，ＵＷＢ 定位設備確實會受到ＮＬｏＳ 問題的影響。所以本文所做的對照實驗主要是從動態方面考慮ＮＬｏＳ 環境對定位精度產生的影響。

如視距實驗場景圖３ 所示，在實驗樓內選擇一塊空曠區域，搭建實驗環境。實驗中選擇可以遠程控制的智能小車將定位標簽放置在小車上，此時實驗人員可以離開定位區域，不會對實驗造成干擾。小車按照預先規定好的軌跡行駛，將障礙物隨機放置在小車行駛軌跡周圍。接著，在這樣的環境下分別進行４ 組實驗，第一組為視距條件非常良好且完全沒有任何遮擋的情況，第二、三、四組分別使用Ａ４白紙、厚度約為８ ｃｍ 紙盒以及厚度為５ ｃｍ 左右的水泥磚頭做遮擋。４ 組實驗分別統計平均誤差，實驗結果如表１ 所示。

由實驗結果可以看出，障礙物造成的ＮＬｏＳ 影響確實會降低ＵＷＢ 的定位精度，且根據障礙物的材質不同，對定位精度的影響程度也會不同。

２． ２ ＩＭＵ ／ ＵＷＢ 融合定位算法改善ＮＬｏＳ 對定位精度影響實驗

為驗證算法的可行性，選擇學校導航定位實驗室內現有實驗平臺進行驗證。實驗平臺由型號為ＤＷＭ１０００ 的ＵＷＢ 模塊和型號為ＩＣＭ４２６０５ 的ＩＭＵ模塊以及相關外圍電路組成。ＩＣＭ４２６０５ＩＭＵ 由三軸加速度計和三軸陀螺儀組成且具有體積小、可低功耗運行等特點。為考慮實驗完整性，充分驗證算法在各個環境下的可行性，設計了２ 種實驗場景：一種是在導航定位實驗室內進行靜態實驗；另一種是在地下人行通道內進行動態實驗。

（１）室內定點定位靜態實驗

為達到模擬ＵＷＢ 弱信號以及ＮＬｏＳ 環境的目的，實驗選擇在導航定位實驗室（江蘇省常州市）內進行。實驗室內使用ＵＷＢ 基站以及標簽搭建實驗環境進行定位，實驗過程中隨機利用實驗室中的桌椅以及實驗人員的身體對ＵＷＢ 信號進行遮擋，創造視距情況較差的環境。室內定位場景實驗環境如圖４ 所示。實驗選擇長為６ ｍ、寬為４ ｍ 的矩形作為定位區域，以Ａｎｃｈｏｒ０ 基站為原點建立直角坐標系，另外３ 個基站的坐標分別為Ａｎｃｈｏｒ１ （４，０）ｍ、Ａｎｃｈｏｒ２（４，６）ｍ、Ａｎｃｈｏｒ３（０，６）ｍ。實驗人員手臂處固定標簽，站在（３，３）ｍ 處進行定位。室內定位場景實驗如圖５ 所示。實驗結果及誤差分析如圖６、表２ 和表３ 所示。為驗證本文所提出的算法比現在實際生產中常用的通過ＥＫＦ 融合濾波的定位方法精度高，本文又做了對比試驗。由實驗結果可以看出，該算法在ＮＬｏＳ 情況下確實可以提高定位精度以及定位的穩定性，且比現階段常使用的解決辦法效果更好。

由表２ 可知，在導航定位實驗室的定點實驗情況下，本文提出的算法模型在東方向上的方差降低了５． ８０％ ；在北方向上的方差降低了１８． ０６％ ；在ＯＸＹ 平面上的方差降低了５． ７１％ 。由此可以看出，通過應用本文的算法，在ＮＬｏＳ 的情況下，可以在一定程度上減少ＵＷＢ 系統在面對ＮＬｏＳ 影響下發生偏移的情況，提高定位的穩定性。

由表３ 可知，在導航定位實驗室內，本文提出的算法模型在東方向上的平均誤差降低了０． ６８ ｃｍ，精度提高了２． ８７％ ；在北方向上的平均誤差降低了０． １８ ｃｍ，精度提高了１２． ０２％ ；在ＯＸＹ 平面上的平均誤差降低了０． ８８ ｃｍ，精度提高了３． ４４％ 。由此可以看出，本文算法可以提高ＵＷＢ 室內定位技術在ＮＬｏＳ 環境下的定位精度。

（２）地下人行通道內動態定位實驗

為驗證系統的動態性能，以及測試定位算法系統在實際生產以及應用中的效果。本文設計了一個動態定位測試實驗。與室內定點定位實驗相比不同的是，實驗更加側重測試算法的動態性能。因為在實際應用過程中，人們更多地是將定位標簽攜帶在身上，在定位區域內來回移動，所以測量整個算法的動態性能很有必要。實驗選擇在江蘇省常州市某地下人行通道內進行。此地下通道范圍較大，長為２５ ｍ，寬為６． ８ ｍ，且ＧＮＳＳ 信號非常微弱，不適合使用傳統的ＧＮＳＳ 定位方法，易使用基于ＵＷＢ 信號的室內定位方法進行定位，適合本實驗的環境需求。實驗中選取長為１９ ｍ、寬為５． ７ ｍ 的矩形范圍布置ＵＷＢ 定位基站。地下通道實驗環境如圖７ 所示。實驗人員在基站范圍內手持定位標簽順時針方向行走一圈并進行定位，另外的實驗人員隨機在定位范圍內用身體對ＵＷＢ 信號進行遮擋，制造ＮＬｏＳ 環境。地下通道實驗場景如圖８ 所示。實驗結果和誤差分析如圖９、表４ 和表５ 所示。

由表４ 可知，在地下人行通道動態實驗中，系統在東方向上的方差降低了８． １２％ ；在北方向上的方差降低了３２． ７４％ ；在ＯＸＹ 平面上的方差降低了１２． ２３％ 。由此可以看出，通過應用本文所示的算法，在動態環境下面對ＮＬｏＳ 的情況，同樣可以在一定程度上提高系統定位的穩定性。

由表５ 可知，在地下人行通道動態實驗中，系統在東方向上平均誤差降低了１． ６４ ｃｍ，精度提高了１２． ０８％ ；在北方向上平均誤差降低了２． １２ ｃｍ，精度提高了２４． １０％ ；在ＯＸＹ 平面上平均誤差降低了３． ７７ ｃｍ，精度提高了１６． ８０％ 。由此可以看出，通過應用本文所示的算法，在動態環境下面對ＮＬｏＳ的情況，定位精度也有了一定的提升。

３ 結束語

針對ＧＮＳＳ 信號易受障礙物遮擋以及在峽谷和城市內等ＧＮＳＳ 信號弱的環境中定位精度差以及傳統ＵＷＢ 室內定位技術易受到ＮＬｏＳ 影響等問題，提出了一種基于ＵＷＢ 和ＩＭＵ 的ＵＷＢ 弱信號環境下空間定位算法模型。將ＩＭＵ 與傳統的ＵＷＢ 室內定位技術相結合，并且與三邊定位法、ＴＤＯＡ 定位方法以及坐標轉換算法相融合，從而實現在ＵＷＢ 弱信號環境下的高精度定位。將該算法模型應用在定位系統中后，系統不易受ＮＬｏＳ 的影響且定位精度有了一定提高。經過反復在不同環境下的實驗對比，證明所設計算法模型可以在一定程度上提高定位精度，可以提高系統在ＮＬｏＳ 環境下的抗干擾能力。與現階段常用的方法做對比可以看出，本文提出的算法在精度和穩定性方面也有更強的優越性。綜上所述，本文所設計的算法模型實現了在ＵＷＢ 弱信號環境下的定位功能，提高了傳統ＵＷＢ 室內定位技術在ＮＬｏＳ 環境下的定位精度和定位的抗干擾性，并且使用的ＩＭＵ 為消費級ＩＭＵ，成本較低，算法模型具有很強的實用性。

參考文獻

［１］ ＧＡＯ Ｙ Ｔ，ＪＩＡＮＧ Ｙ，ＧＡＯ Ｙ，ｅｔ ａｌ． Ｓｏｌｕｔｉｏｎ Ｓｅｐａｒａｔｉｏｎｂａｓｅｄ Ｉｎｔｅｇｒｉｔｙ Ｍｏｎｉｔｏｒｉｎｇ ｆｏｒ ＲＴＫ Ｐｏｓｉｔｉｏｎｉｎｇ ｗｉｔｈ ＦａｕｌｔｙＡｍｂｉｇｕｉｔｙ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｐｒｏｔｅｃｔｉｏｎ Ｌｅｖｅｌ［Ｊ］． ＧＰＳ Ｓｏｌｕｔｉｏｎｓ，２０２３，２７：１４０．

［２］ ＳＵ Ｊ Ｘ，ＹＡＯ Ｚ，ＬＵ Ｍ Ｑ． Ａｎ Ｉｍｐｒｏｖｅｄ Ｐｏｓｉｔｉｏｎ Ｄｅｔｅｒｍｉｎａｔｉｏｎ Ａｌｇｏｒｉｔｈｍ Ｂａｓｅｄ ｏｎ Ｎｏｎｌｉｎｅａｒ Ｃｏｍｐｅｎｓａｔｉｏｎ ｆｏｒＧｒｏｕｎｄｂａｓｅｄ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍｓ ［Ｊ ］． ＩＥＥＥ Ａｃｃｅｓｓ，２０１９，７：２３６７５－２３６８９．

［３］ 岳喜慶． 基于Ａｎｄｒｏｉｄ 的ＷｉＦｉ 室內定位技術的研究與實現［Ｄ］． 武漢：華中師范大學，２０１８．

［４］ ＭＡＬＩＫ Ａ，ＫＵＳＨＷＡＨ Ｒ． Ｅｎｅｒｇｙｅｆｆｉｃｉｅｎｔ Ｓｃｈｅｄｕｌｉｎｇ ｉｎＩｏＩ Ｕｓｉｎｇ ＷｉＦｉ ａｎｄ Ｚｉｇｂｅｅ Ｃｒｏｓｓｔｅｃｈｎｏｌｏｇｙ ［Ｊ］． ＴｈｅＪｏｕｒｎａｌ ｏｆ Ｓｕｐｅｒｃｏｍｐｕｔｉｎｇ，２０２３，７９（１０）：１０９７７－１１００６．

［５］ ＹＯＮＧ Ｚ． Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍ Ｓｉｍｕｌａｔｉｏｎ ａｎｄ Ｄａｔａ Ａｃｃｕｒａｃｙｏｆ Ｐｈｙｓｉｃａｌ Ｆｉｔｎｅｓｓ Ｔｒａｉｎｉｎｇ ｆｏｒ Ｓｐｏｒｔｓ Ｍａｊｏｒｓ Ｂａｓｅｄ ｏｎＲｅａｌｔｉｍｅ Ｓｔａｔｕｓ Ｕｐｄａｔｅ ｏｆ Ｗｅａｒａｂｌｅ Ｉｎｔｅｒｎｅｔ ｏｆ Ｔｈｉｎｇｓ［Ｊ］． Ｓｏｆｔ Ｃｏｍｐｕｔｉｎｇ，２０２３，２７（１４）：１０１４５－１０１５４．

［６］ ＸＵ Ｈ，ＤＩＮＧ Ｙ，ＬＩ Ｐ，ｅｔ ａｌ． Ａｎ ＲＦＩＤ Ｉｎｄｏｏｒ ＰｏｓｉｔｉｏｎｉｎｇＡｌｇｏｒｉｔｈｍ Ｂａｓｅｄ ｏｎ Ｂａｙｅｓｉａｎ Ｐｒｏｂａｂｉｌｉｔｙ ａｎｄ ＫｎｅａｒｅｓｔＮｅｉｇｈｂｏｒ［Ｊ］． Ｓｅｎｓｏｒｓ，２０１７，１７（８）：１８０６．

［７］ 彭俊杰，李英華，王春琦，等． 非視距環境下的ＩＭＵ 和ＵＷＢ 融合定位系統［Ｊ］． 無線電工程，２０２２，５２ （６）：９３２－９３９．

［８］ 郁劍，郝哲軒． 一種新型超寬帶天線的設計［Ｊ］． 無線電工程，２０２２，５２（５）：８５９－８６３．

［９］ 徐曉蘇，劉興華，楊博，等． 基于二次解析的ＵＷＢ 室內定位高度方向優化方法［Ｊ］． 中國慣性技術學報，２０１９，２７（５）：５８９－５９７．

［１０］ 宰昶豐，郭英，葉瑾，等． 基于ＵＷＢ 穿墻定位中ＮＬｏＳ誤差的優化方法［Ｊ］． 電子測量技術，２０２１，４４ （１９）：１２８－１３３．

［１１］ 宋波，李生林，劉恩啟． 基于類別不平衡分類的超寬帶非視距信號識別方法研究［Ｊ］． 信息工程大學學報，２０１９，２０（２）：１４７－１５３．

［１２］ 李旭，孔鑫，劉錫祥，等． 基于交互多模型距離平滑的ＵＷＢ ／ ＩＭＵ 因子圖組合導航方法［Ｊ］． 中國慣性技術學報，２０２２，３０（３）：３２２－３２７．

［１３］ 隋心，高嵩，黃俊堯，等． ＵＷＢ 輔助ＩＮＳ 的室內組合定位方法［Ｊ］． 無線電工程，２０２０，５０（８）：６２４－６３０．

［１４］ ＺＨＡＮＧ Ｙ Ｌ，ＬＩＡＮＧ Ｗ，ＺＨＡＮＧ Ｓ Ｃ，ｅｔ ａｌ． Ｈｉｇｈｐｒｅｃｉｓｉｏｎ Ｃａｌｉｂｒａｔｉｏｎ ｏｆ Ｃａｍｅｒａ ａｎｄ ＩＭＵ ｏｎ Ｍａｎｉｐｕｌａｔｏｒｆｏｒ Ｂｉｏｉｎｓｐｉｒｅｄ Ｒｏｂｏｔｉｃ Ｓｙｓｔｅｍ［Ｊ］． Ｊｏｕｒｎａｌ ｏｆ Ｂｉｏｎｉｃ Ｅｎｇｉｎｅｅｒｉｎｇ，２０２２，１９（２）：２９９－３１３．

［１５］ 陳荊宇，徐琳，黃斌，等． 基于室內無人車的ＵＷＢ 和ＩＭＵ 新型融合定位方法［Ｊ］． 工程機械，２０２２，５３（１１）：１－７．

［１６］ ＫＩＭ Ｍ Ｊ，ＫＩＭ Ｍ，ＬＩＭ Ｄ，ｅｔ ａｌ． Ｄｉｓｔｕｒｂａｎｃｅ ＡｄａｐｔｉｎｇＷａｌｋｉｎｇ Ｐａｔｔｅｒｎ Ｇｅｎｅｒａｔｉｏｎ Ｕｓｉｎｇ Ｃａｐｔｕｒｅ Ｐｏｉｎｔ ＦｅｅｄｂａｃｋＣｏｎｓｉｄｅｒｉｎｇ ＣｏＭ Ｃｏｎｔｒｏｌ Ｐｅｒｆｏｒｍａｎｃｅ［Ｊ］． Ｊｏｕｒｎａｌ ｏｆ Ｉｎｔｅｌｌｉｇｅｎｔ ａｎｄ Ｒｏｂｏｔｉｃ Ｓｙｓｔｅｍｓ，２０２３，１０８（２）：２２．

［１７］ 張舸，謝陽光，伊國興，等． 基于羅經／ ＤＶＬ ／ ＵＳＢＬ 組合導航系統的ＤＶＬ 在線標定算法［Ｊ］． 中國慣性技術學報，２０２２，３０（５）：５８９－５９６．

［１８］ 徐博，趙曉偉，王連釗． 基于ＩＥＫＦ 的ＳＩＮＳ ／ ＵＳＢＬ 組合導航系統安裝偏差標定算法［Ｊ］． 中國慣性技術學報，２０２２，３０（４）：５０１－５０７．

［１９］ 司明豪，汪云甲，徐生磊，等． 基于ＬＯＳ ／ ＮＬＯＳ 識別的ＷｉＦｉ ＦＴＭ 定位［Ｊ ］． 無線電工程，２０２１，５１ （８ ）：８０２－８０９．

［２０］ 顧慧東． 基于ＵＷＢ 的室內測距與定位系統［Ｄ］． 南京：南京郵電大學，２０２０．

作者簡介

趙 陽 男，（１９９８—），碩士研究生。主要研究方向：嵌入式系統與智能控制。

（*通信作者）王田虎 男，（１９８０—），博士，教授，碩士生導師。主要研究方向：嵌入式系統與智能控制。

李文杰 男，（１９９７—），碩士研究生。主要研究方向：集成電路與系統設計。

繆千年 男，（１９９７—），碩士研究生。主要研究方向：嵌入式系統與智能控制。

沈運哲 男，（１９９７—），碩士研究生。主要研究方向：嵌入式系統與智能控制。

黃 濤 男，（１９８１—），正高級工程師。主要研究方向：智能產品開發。

基金項目：江蘇省自然科學基金（ＢＫ２０１５０２４７）；江蘇省實踐創新項目（ＸＳＪＣＸ２２＿４４）；中車南京浦鎮車輛有限公司委托課題（ＫＹ３０７２０２１０００１）