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

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

0 引言

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

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

1 UWB/SINS算法定位模型

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

圖1 系統(tǒng)結(jié)構(gòu)Fig.1 System structure

1.1 初始對(duì)準(zhǔn)

(1)

(2)

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

(3)

(4)

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

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

(5)

1.2 零速檢測(cè)算法

零速檢測(cè)是零速修正的前提，本文采用廣義似然比檢測(cè)，并通過(guò)判斷運(yùn)動(dòng)模式設(shè)置差異化的閾值來(lái)適應(yīng)不同速度的步態(tài)。

1)判斷運(yùn)動(dòng)模式

這里將慣性測(cè)量單元(Inertial Measurement Unit，IMU)的x軸看作行走方向的前向軸，可以很大程度反映行走速度，通過(guò)計(jì)算x軸基于時(shí)間窗口的平均加速度來(lái)判斷運(yùn)動(dòng)快慢，設(shè)k時(shí)刻的x軸的平均加速度為

(6)

根據(jù)經(jīng)驗(yàn)值設(shè)置2個(gè)閾值Thv1和Thv2，劃分三種運(yùn)動(dòng)速度模式mod1、mod2和mod3，分別代表慢速模式、中速模式和快速模式，劃分標(biāo)準(zhǔn)如下

(7)

式中，modi表示運(yùn)動(dòng)模式i。

2)廣義似然比檢驗(yàn)?zāi)Ｐ?/p>

定義k時(shí)刻IMU的輸出為

(8)

(9)

(10)

Thi為運(yùn)動(dòng)模式i下的經(jīng)驗(yàn)閾值，zero_flag表示零速標(biāo)志，其值為1時(shí)表示零速，其值為0時(shí)表示非零速。根據(jù)不同的運(yùn)動(dòng)速度設(shè)置不同的閾值可以更加準(zhǔn)確地判斷零速。

1.3 UWB定位

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

(11)

ThL為閾值，uwb_flag表示UWB數(shù)據(jù)是否可用，其值為1時(shí)表示可用，否則不可用。

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

(12)

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

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

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

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

(13)

對(duì)子濾波器分別進(jìn)行時(shí)間更新

(14)

對(duì)子濾波器分別進(jìn)行量測(cè)更新

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

(15)

由于主濾波器沒(méi)有量測(cè)信息，所以只進(jìn)行時(shí)間更新

(16)

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

(17)

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

(18)

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

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

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

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

(19)

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

(20)

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

(21)

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

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

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

(22)

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

3)SINS/磁力計(jì)子濾波器

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

(23)

式中，Hs是SINS解算出來(lái)的航向角，Hc是磁力計(jì)解算出來(lái)的航向角。

3 現(xiàn)場(chǎng)應(yīng)用

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

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

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

現(xiàn)場(chǎng)試驗(yàn)區(qū)域?yàn)槟暇┑貐^(qū)某500kV變電站設(shè)備區(qū)14m×16m的矩形區(qū)域，圍繞該區(qū)域行走1圈，總路程60m。當(dāng)可用基站數(shù)據(jù)少于3時(shí)，就不使用UWB數(shù)據(jù)。圖5中，(0,0)是起點(diǎn)也是終點(diǎn)，圍繞該矩形區(qū)域逆時(shí)針旋轉(zhuǎn)；從(0,0)出發(fā)依次經(jīng)過(guò)(-14,0)(-14,16)(0,16)回到(0,0)；點(diǎn)表示W(wǎng)UB解算位置；虛線表示SINS解算獲得的軌跡，實(shí)線表示SINS-UWB組合導(dǎo)航軌跡，圖中五角星的位置為4個(gè)基站的位置。

圖5 定位軌跡Fig.5 Location trajectory

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

圖6 定位誤差Fig.6 Location error

表1 定位誤差Tab.1 Location error

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

4 結(jié)論

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