基于UWB/SINS組合的行人導航研究

陳 昊，張海華，陳瑋光，劉錫祥，汪宋兵

(1.國網江蘇省電力有限公司檢修分公司，南京 211102；2.東南大學儀器科學與工程學院，南京 210096)

0 引言

隨著我國電力事業(yè)的發(fā)展，人們對供電可靠性的要求日益提高。變電站作為電網的重要組成部分，其日常的運維與檢修工作對電網的安全穩(wěn)定運行至關重要[1-3]。變電站，尤其是500kV及以上的大型變電站，常有規(guī)模較大的工程施工工作和檢修工作，此時將有大量來自不同單位的工作人員進出變電站，長時間在一次帶電設備區(qū)作業(yè)。一方面，需要在管理措施層面加強現場管控，確保作業(yè)人員不誤入帶電間隔，與各類帶電體保持足夠的安全距離；另一方面，需要在技術措施層面積極采用新技術，實時掌握作業(yè)人員的位置動態(tài)，提高現場工作的安全性。此外，變電站設備區(qū)障礙物多，負荷波動特征多變[4-5]、電磁環(huán)境復雜[6-7]，局部區(qū)域衛(wèi)星信號衰減嚴重，全球定位系統(tǒng)(Global Positioning System，GPS)設備無法有效定位。與此同時，變電站內人員在這些區(qū)域又有著相同的定位精度需求，在此背景下，在全區(qū)域內無差別實現實時跟蹤個人的位置信息并提供位置服務，為施工和檢修人員提供更高級別的安全保障成為重要的研究課題。

目前，可用于行人導航定位的技術種類繁多，適用范圍和精度各有不同，其中慣性設備個人導航系統(tǒng)具有高穩(wěn)定性、連續(xù)性與自主性的優(yōu)點[8-11]，成為研究的熱點方向之一；然而低成本的慣性設備精度低，且其積分工作方式導致定位誤差會隨著時間累積，限制了其精確定位的時間長度[12]。利用無線技術實現行人定位的方案繁多[13-15]，其中超寬帶(Ultra Wideband，UWB)擁有獨特的定位優(yōu)勢，其具有時間分辨率高、穿透能力強、發(fā)射功率低的特點[16]；但UWB易受可視性和多徑效應影響，使得定位出現不連續(xù)與不穩(wěn)定的缺點[17]。本文結合慣性導航與UWB的優(yōu)點，在傳統(tǒng)的捷聯(lián)慣導算法的基礎上引入了零速檢測，并通過閾值法剔除了UWB錯誤信息，使用聯(lián)邦Kalman濾波將零速、UWB位置和航向信息進行融合，獲得了高精度、連續(xù)且穩(wěn)定的定位信息。通過現場實驗證明了該方法能夠大幅度提升系統(tǒng)定位精度，并進一步加強系統(tǒng)的穩(wěn)定性與可靠性。

1 UWB/SINS算法定位模型

初始對準階段，采用磁力計和加速度計輸出數據對捷聯(lián)慣性導航系統(tǒng)(Strapdown Inertial Navigation System，SINS)進行姿態(tài)角對準；行走過程中，利用陀螺儀輸出積分獲得姿態(tài)，加速度計積分獲得速度，速度積分獲得位置；在行走過程中，利用加速度計和陀螺儀輸出數據進行零速檢測，并利用UWB數據解算出位置與航向信息；采用聯(lián)邦Kalman濾波融合零速度信息、位置信息以及航向信息，校正系統(tǒng)狀態(tài)量，系統(tǒng)結構如圖1所示。

圖1 系統(tǒng)結構Fig.1 System structure

1.1 初始對準

(1)

(2)

根據式(2)可以獲得初始橫滾角與俯仰角R0和P0，P0∈(-π/2,π/2),R0∈(-π,π)。

(3)

(4)

表示載體坐標系b系到水平坐標系L系的姿態(tài)轉換矩陣。不考慮偏向磁分量，當規(guī)定航向角北偏東為正，北偏西為負時，初始航向角表示為

H0=-tan-1(myLmxL),H0∈(-π,π)

(5)

1.2 零速檢測算法

零速檢測是零速修正的前提，本文采用廣義似然比檢測，并通過判斷運動模式設置差異化的閾值來適應不同速度的步態(tài)。

1)判斷運動模式

這里將慣性測量單元(Inertial Measurement Unit，IMU)的x軸看作行走方向的前向軸，可以很大程度反映行走速度，通過計算x軸基于時間窗口的平均加速度來判斷運動快慢，設k時刻的x軸的平均加速度為

(6)

根據經驗值設置2個閾值Thv1和Thv2，劃分三種運動速度模式mod1、mod2和mod3，分別代表慢速模式、中速模式和快速模式，劃分標準如下

(7)

式中，modi表示運動模式i。

2)廣義似然比檢驗模型

定義k時刻IMU的輸出為

(8)

(9)

(10)

Thi為運動模式i下的經驗閾值，zero_flag表示零速標志，其值為1時表示零速，其值為0時表示非零速。根據不同的運動速度設置不同的閾值可以更加準確地判斷零速。

1.3 UWB定位

UWB解算位置會受到可視性誤差的影響出現跳變，而融合后的解算位置不出現跳變誤差；且行人行走屬于一個連續(xù)相對較慢的過程，本文利用式(11)進行UWB誤差數據剔除

(11)

ThL為閾值，uwb_flag表示UWB數據是否可用，其值為1時表示可用，否則不可用。

剔除誤差數據后，UWB提供的位置相對準確，其航向角相對磁力計將更加穩(wěn)定，利用兩點確定直線的方式確定航向角，計算方程如下

(12)

2 導航系統(tǒng)聯(lián)邦Kalman濾波器

組合導航系統(tǒng)聯(lián)邦Kalman濾波器由SINS、零速修正系統(tǒng)、UWB系統(tǒng)以及磁力計構成，以SINS作為公共的參考系統(tǒng)，聯(lián)邦Kalman濾波結構如圖2所示。

圖2中，Xi和Pi分別表示各個子濾波器的局部狀態(tài)估計值與協(xié)方差矩陣；Xm和Pm分別表示主濾波器的狀態(tài)估計值與協(xié)方差矩陣；Xg和Pg分別表示公共系統(tǒng)的狀態(tài)估計值與協(xié)方差矩陣；βi表示系數。

2.1 導航系統(tǒng)聯(lián)邦Kalman濾波算法

(13)

對子濾波器分別進行時間更新

(14)

對子濾波器分別進行量測更新

Pi,k=(I-Ki,kHi,k)Pi,k|k-1(I-Ki,kHi,k)T+

(15)

由于主濾波器沒有量測信息，所以只進行時間更新

(16)

將子濾波器與主濾波器的估計信息融合成為新的全局狀態(tài)估計信息，如下

(17)

最后按式(18)重置子濾波器與主濾波器

(18)

2.2 子濾波器系統(tǒng)模型

當選取支撐相位內的速度輸出作為偽觀測值時，傳感器誤差的可觀測性較差，利用Kalman濾波計算難以得到精確的傳感器誤差估計[18]，且磁力計與UWB無累計誤差，因此在聯(lián)邦Kalman濾波中各個子濾波器不考慮傳感器誤差，選擇速度、位置與姿態(tài)作為狀態(tài)量，即X[(δφ)T(δvn)T(δpn)T]T。

1)SINS/零速檢測系統(tǒng)子濾波器

SINS/零速檢測系統(tǒng)子濾波器模型如下

(19)

其中，Wk-1和V均視為彼此不相關的零均值白噪聲序列；狀態(tài)轉移矩陣Fk|k-1與噪聲輸入矩陣Gk|k-1具體如下

(20)

對SINS/零速檢測系統(tǒng)子濾波器進行速度融合。通過觀測值對系統(tǒng)的速度進行修正，觀測向量及觀測矩陣分別為

(21)

其中，Z=03×1-vn中03×1表示零速檢測獲得的零速矢量，vn表示慣性導航解算獲得的速度矢量。

2)SINS/UWB系統(tǒng)子濾波器

其狀態(tài)方程與SINS/零速檢測系統(tǒng)子濾波器相同，觀測方程不同；對SINS/UWB子濾波器進行位置、航向融合。通過觀測值對位置、航向進行修正，觀測向量及觀測矩陣分別為

(22)

式中，Hs是SINS解算出來的航向角，Hu是UWB解算出來的航向角，Ps是SINS解算出的位置，Pu是UWB解算得到的位置。

3)SINS/磁力計子濾波器

其狀態(tài)方程與SINS/零速檢測系統(tǒng)組合子濾波器相同，對SINS/磁力計子濾波器進行航向融合。通過觀測值對航向進行修正，觀測向量及觀測矩陣分別為

(23)

式中，Hs是SINS解算出來的航向角，Hc是磁力計解算出來的航向角。

3 現場應用

捷聯(lián)慣性導航實驗設備采用法國SBG SYSTEMS公司的Ellipse2-A系列，產品內部包含3個正交的加速度計和3個正交的陀螺儀以及1個三軸磁力計。SBG安裝于腳部(建議安裝處為試驗中采用的右腳腳踝)，對行人行走數據進行采集，實驗中坐標系采用的是前右下坐標系，系統(tǒng)的采集頻率為100Hz(見圖3)。UWB采用的是研創(chuàng)物聯(lián)的Mini3s系列，其最大測量距離為50m，定位誤差為35cm，頻率為3.5Hz，UWB數據包含可用基站數目，當可用基站數小于3時，棄用UWB數據信息(見圖4)。

圖3 SBGFig.3 SBG Ellipse 2-A AHRS used in the experiment

圖4 超寬帶Fig.4 UWB Mini 3s and in the experiment

現場試驗區(qū)域為南京地區(qū)某500kV變電站設備區(qū)14m×16m的矩形區(qū)域，圍繞該區(qū)域行走1圈，總路程60m。當可用基站數據少于3時，就不使用UWB數據。圖5中，(0,0)是起點也是終點，圍繞該矩形區(qū)域逆時針旋轉；從(0,0)出發(fā)依次經過(-14,0)(-14,16)(0,16)回到(0,0)；點表示WUB解算位置；虛線表示SINS解算獲得的軌跡，實線表示SINS-UWB組合導航軌跡，圖中五角星的位置為4個基站的位置。

圖5 定位軌跡Fig.5 Location trajectory

誤差曲線如圖6所示。圖6中，虛線為SINS軌跡誤差，點劃線為UWB解算估計誤差，實線為SINS-UWB聯(lián)合解算誤差。統(tǒng)計3條曲線定位誤差，如表1所示。

圖6 定位誤差Fig.6 Location error

表1 定位誤差Tab.1 Location error

從表1中可知，SINS與UWB定位誤差均值與最大值均大于SINS-UWB組合導航，其中SINS是由于誤差累計導致位置與航向發(fā)生漂移，而UWB主要由于可視性誤差與多徑效應導致定位誤差；組合導航方法可以平滑軌跡曲線，其軌跡更加接近于真實軌跡，誤差未明顯受到變電站復雜電磁環(huán)境的干擾。

4 結論

采用UWB/SINS組合系統(tǒng)實現了變電站現場區(qū)域的定位功能，在傳統(tǒng)的捷聯(lián)解算基礎上引入了零速檢測，并通過閾值法剔除了UWB錯誤信息，最后利用聯(lián)邦Kalman濾波融合了UWB、磁力計、零速等信息，獲得了較高的系統(tǒng)定位精度?，F場試驗表明，組合系統(tǒng)的定位誤差小于UWB系統(tǒng)與SINS，能夠消除UWB系統(tǒng)中的可視性誤差以及SINS的累計誤差，穩(wěn)定性與可靠性都有進一步提升，適用于高精度變電站現場區(qū)域無差別導航。