張 蒙，高 嵩，李 琛

（1.國(guó)網(wǎng)山東省電力公司電力科學(xué)研究院，山東 濟(jì)南 250003；2.國(guó)網(wǎng)山東省電力公司禹城市供電公司，山東 禹城 251200）

0 引言

傳統(tǒng)的變電站主要依靠值班員進(jìn)行人工巡檢，存在意外風(fēng)險(xiǎn)發(fā)生頻次高、巡視效率低等缺點(diǎn)［1-2］。隨著自動(dòng)化、物聯(lián)網(wǎng)和人工智能等技術(shù)的迅速發(fā)展，使用機(jī)器代替人工，進(jìn)行電力設(shè)備巡檢已經(jīng)成為數(shù)字化變電站的重要趨勢(shì)［3-5］。其中，實(shí)時(shí)定位系統(tǒng)為巡檢機(jī)器人的運(yùn)動(dòng)控制系統(tǒng)提供連續(xù)、實(shí)時(shí)、精確的位置、航向等導(dǎo)航信息，使其沿預(yù)定的路徑行駛并完成檢測(cè)任務(wù)［6-7］，是其實(shí)現(xiàn)高壓設(shè)備智能巡檢的關(guān)鍵技術(shù)之一［8］。

在變電站環(huán)境中，巡檢機(jī)器人近似在二維平面中運(yùn)動(dòng)［9］。因此，相對(duì)于俯仰角和橫滾角，對(duì)航向角的輸出精度和輸出頻率有更高的要求。全球定位系統(tǒng)（Global Positioning System，GPS）可以為用戶提供全球范圍內(nèi)的實(shí)時(shí)位置和速度信息［10］。GPS 定位基于衛(wèi)星測(cè)距原理，沒有累積誤差，其長(zhǎng)期性能優(yōu)異［11］。但是，在變電站多遮擋和復(fù)雜電磁環(huán)境下，衛(wèi)星信號(hào)容易受到干擾，使得定位性能下降甚至失效。另外，GPS 輸出的航向角信息是根據(jù)載體的速度方向推算的，靜止?fàn)顟B(tài)下航向角無(wú)輸出?；贕PS 存在隨機(jī)噪聲、易受到干擾、只能動(dòng)態(tài)輸出航向等特性，單依靠GPS 傳感器，無(wú)法滿足巡檢機(jī)器人對(duì)航向輸出的精度、穩(wěn)定性和連續(xù)性要求。光纖陀螺儀是一種可提供高精度高頻率角速度輸出的慣性器件［12］。通過對(duì)角速率進(jìn)行積分可獲得航向變化信息。陀螺儀的航向輸出幾乎不受外部干擾，具有良好的平滑度、較高的更新頻率和短時(shí)精度［13］。由于通過積分獲得航向變化，陀螺儀只能輸出相對(duì)的航向信息，而且誤差會(huì)隨著時(shí)間的推移而累積，并且趨向于無(wú)窮大。GPS 和陀螺儀傳感器單獨(dú)使用時(shí)，優(yōu)缺點(diǎn)鮮明；將兩者恰當(dāng)融合時(shí)，卻具有良好的互補(bǔ)性。短時(shí)間內(nèi)，陀螺儀的輸出具有較高高精度和高連續(xù)性，可以校正GPS 的隨機(jī)噪聲，并在信號(hào)丟失期間提供航向保持［14］。GPS 作為一種全局定位技術(shù)，其提供的航向誤差是有邊界的，可以糾正陀螺儀的累積誤差，改善其長(zhǎng)期輸出性能。

通過引入一種基于航向誤差的自適應(yīng)離散卡爾曼濾波器，融合GPS ∕陀螺儀傳感器輸出并獲得航向信息。試驗(yàn)結(jié)果表明，該融合算法在動(dòng)態(tài)和靜態(tài)條件下都能獲得高精度、高穩(wěn)定性、高刷新率的實(shí)時(shí)航向輸出，為巡檢機(jī)器人的高精度路徑規(guī)劃和運(yùn)動(dòng)控制提供可靠的傳感信息。

1 傳感器輸出及數(shù)據(jù)處理

1.1 GPS接收機(jī)

采用NovAtel 公司的ProPak-G2 GPS 接收機(jī)，其內(nèi)置的OEMV-2板塊能夠捕獲L1∕L2雙頻GPS信號(hào)，而且支持差分修正。兩個(gè)雙向的COM 端口，最高可支持230 400 bit∕s 串口通信速率。初始化完成后，能以最大20 Hz 的更新頻率輸出定位信息。該接收機(jī)的實(shí)體和具體接口如圖1所示。

圖1 NovAtel GPS接收機(jī)

ProPak-G2接收機(jī)有多種輸出模式，可以通過串口向接收機(jī)發(fā)送不同的指令來(lái)完成不同的輸出。本文采用它的BESTXYZA 的輸出格式。該格式可以輸出Wgs84 地心坐標(biāo)系下載體的實(shí)時(shí)三維位置信息、載體的三維速度信息和位置精度因子的信息。圖2展示了BESTXYZA格式下一幀典型的輸出數(shù)據(jù)。

圖2 1幀完整的BESTXYZA格式數(shù)據(jù)

在該模式下，GPS接收機(jī)可以輸出載體的三維多普勒速度信息，即圖2 中字母DOPPLER_VELOCITY后面的連續(xù)3 幀數(shù)據(jù)，其分別表示W(wǎng)GS84 地心坐標(biāo)系下三維速度分量。

變電站機(jī)器人在地面近似二維平面運(yùn)動(dòng)，為便于計(jì)算，一般地，將GPS 接收機(jī)獲得的地心坐標(biāo)系WGS84 下的位置和速度信息轉(zhuǎn)換到東北天ENU 坐標(biāo)系中，航向信息可以通過速度分量進(jìn)行反推［15］。公式為：航向=arctan（Vy∕Vx）。其中，Vy、Vx分別為由WGS84 坐標(biāo)轉(zhuǎn)換到ENU 坐標(biāo)后，載體沿著Y軸（北向）、X軸（東向）的速度分量。

1.2 陀螺儀

陀螺儀能夠測(cè)量車體的實(shí)時(shí)角速度。采用KVH公司生產(chǎn)的DSP3000 陀螺儀，如圖3 所示。該陀螺儀采用成熟的光纖電路和全數(shù)字信號(hào)處理系統(tǒng)，能夠有效消除零偏不穩(wěn)定性和震動(dòng)噪聲。實(shí)際工程測(cè)試中，將KVH DSP3000 水平安裝在機(jī)器人系統(tǒng)上，同時(shí)打開電源，在靜止?fàn)顟B(tài)下，經(jīng)過標(biāo)定修正后每小時(shí)的零漂累積誤差可控制在5°以內(nèi)。

圖3 DSP 3000陀螺儀

DSP3000 可以提供角度輸出模式，直接為載體提供相對(duì)航向信息。上電后，陀螺默認(rèn)輸出的是角速率信息。只要持續(xù)向串口發(fā)送‘P’指令，即可切換到角度輸出模式。陀螺儀的輸出可選100 Hz或高達(dá)1 000 Hz，高更新頻率保證精確捕捉短時(shí)間內(nèi)的航向變化信息。

2 離散卡爾曼濾波器

卡爾曼濾波器是一種線性濾波器，它通過一系列遞推數(shù)學(xué)公式描述，給出一種高效可靠的狀態(tài)估計(jì)方法，而且使估計(jì)均方差最小。卡爾曼濾波器及其衍生算法在多傳感信息融合中有著廣泛的應(yīng)用［16］。記系統(tǒng)的狀態(tài)變量x∈?n，?n即n維的線性空間，其時(shí)間更新過程可以用離散差分方程表示，即：

同時(shí)定義觀測(cè)變量z∈?m。?m表示m維的線性空間。系統(tǒng)的量測(cè)方程可寫為

式中：wk、vk分別為過程噪聲和觀測(cè)噪聲；A，B，H表示轉(zhuǎn)移矩陣；uk-1代表k-1時(shí)刻的系統(tǒng)輸入。一般地，過程噪聲wk和觀測(cè)噪聲vk是相互獨(dú)立，并且服從正態(tài)分布的高斯白噪聲，即：

式中：Q和R分別是過程噪聲和觀測(cè)噪聲的協(xié)方差矩陣。

卡爾曼濾波器通過反饋控制的方法來(lái)估計(jì)過程狀態(tài)。具體來(lái)說(shuō)，可以分為時(shí)間更新和量測(cè)更新兩部分。時(shí)間更新方程通過前一時(shí)刻的系統(tǒng)狀態(tài)和誤差協(xié)方差，利用狀態(tài)更新方程推算當(dāng)前時(shí)刻的系統(tǒng)狀態(tài)和誤差協(xié)方差，完成對(duì)當(dāng)前時(shí)刻狀態(tài)的先驗(yàn)估計(jì)。量測(cè)更新則將先驗(yàn)估計(jì)與當(dāng)前時(shí)刻的量測(cè)值結(jié)合，得到改進(jìn)的后驗(yàn)估計(jì)。經(jīng)過量測(cè)反饋校正得到的后驗(yàn)估計(jì)又會(huì)被用來(lái)推算下一時(shí)刻的先驗(yàn)估計(jì)，從而實(shí)現(xiàn)卡爾曼濾波器的反饋遞歸估計(jì)過程。

具體應(yīng)用中，卡爾曼濾波一般由初始化、時(shí)間更新和量測(cè)更新3部分組成，估計(jì)過程如圖4所示。其中表示k時(shí)刻狀態(tài)變量估計(jì)的協(xié)方差矩陣，Kk表示k時(shí)刻的濾波器增益。x0、P0分別表示狀態(tài)變量的初始估計(jì)值和初始估計(jì)協(xié)方差矩陣。

圖4 卡爾曼濾波流程

要使用卡爾曼濾波器，首先建立估計(jì)變量的狀態(tài)方程和量測(cè)方程。只有對(duì)系統(tǒng)狀態(tài)建模準(zhǔn)確，并對(duì)過程噪聲協(xié)方差和測(cè)量噪聲協(xié)方差進(jìn)行正確的估計(jì)，濾波器才能準(zhǔn)確估計(jì)狀態(tài)變量，消除累積誤差和隨機(jī)噪聲。

3 航向融合算法

GPS 接收機(jī)獲得的航向估計(jì)只能在動(dòng)態(tài)情況下使用，存在隨機(jī)噪聲且易受外界環(huán)境干擾。陀螺儀提供的相對(duì)航向信息輸出穩(wěn)定，短時(shí)間內(nèi)精度很高，但存在累積誤差，且在全局定位時(shí)需要初始化。通過建立恰當(dāng)?shù)碾x散卡爾曼濾波器，融合兩系統(tǒng)的航向輸出，使巡檢機(jī)器人在動(dòng)態(tài)和靜態(tài)情況下都能獲得精度高、穩(wěn)定性好的航向輸出。

3.1 濾波器建模

陀螺儀的航向輸出可以看成由真實(shí)的航向和航向誤差組成。采用間接卡爾曼濾波進(jìn)行數(shù)據(jù)融合，將航向輸出的系統(tǒng)誤差作為估計(jì)量，通過估計(jì)系統(tǒng)誤差間接獲得對(duì)航向的融合估計(jì)。設(shè)GPS 和陀螺儀得到的航向信息分別為θgps和θgyro，則有：

式中：θ為真實(shí)航向；δθ為航向誤差。將陀螺儀的航向誤差作為系統(tǒng)估計(jì)變量，其航向誤差主要由漂移累積誤差和隨機(jī)噪聲兩部分組成，離散化的系統(tǒng)狀態(tài)方程可表示為

式中：δθk和δθk-1分別為k時(shí)刻和k-1 時(shí)刻航向誤差的估計(jì)；δdrift為k-1 時(shí)刻到k時(shí)刻陀螺的累積誤差；δnoise為k時(shí)刻的隨機(jī)噪聲。

由式（5）和式（6）可得系統(tǒng)的量測(cè)方程為

式中：v為量測(cè)噪聲，即GPS輸出航向的白噪聲。

3.2 融合算法

建立估計(jì)系統(tǒng)誤差的離散化濾波器模型后，可以按照卡爾曼濾波的一般過程，融合輸出實(shí)時(shí)航向信息。

3.2.1 初始化

初始化即確定卡爾曼濾波狀態(tài)變量和狀態(tài)協(xié)方差的初始估計(jì)，為濾波遞推過程提供初始值。根據(jù)濾波器建模，本文的初始化變量為陀螺儀輸出的相對(duì)航向與Wgs84東北天坐標(biāo)系下絕對(duì)航向的誤差X0（δθ），其初始協(xié)方差矩陣P0。由于X0（δθ）和P0初始化后均為1×1階矩陣，記為X0（δθ）和P0。

系統(tǒng)的真實(shí)航向?yàn)闄C(jī)器人相對(duì)于ENU東北天坐標(biāo)系下的指北絕對(duì)航向。慣性器件獲得航向的原理是依靠角速率積分，故只能獲得航向的變化信息，即相對(duì)航向。要獲得指北航向，只能依靠全球定位系統(tǒng)GPS的信號(hào)輸出。

GPS 輸出的實(shí)際上是位置信息，航向推算依靠的是位置變化帶來(lái)的速度方向。當(dāng)載體處在靜止?fàn)顟B(tài)時(shí)，速度信息為0，只有在運(yùn)動(dòng)的情況下，才能通過GPS得到航向信息。

為盡可能保證初始航向的精確性，需要載體以一定速度運(yùn)動(dòng)，并且該段時(shí)間內(nèi)GPS 信號(hào)良好。本文通過連續(xù)幀的自適應(yīng)算法，實(shí)時(shí)估計(jì)GPS 航向信息的可靠性，確保航向誤差的初始化的準(zhǔn)確性，具體的算法如下。

航向?yàn)V波器初始化過程中，實(shí)時(shí)連續(xù)采集20 幀陀螺儀和GPS 得到的航向信息并儲(chǔ)存在序列中，分別記為：

式（9）和式（10）相減，得到一組航向差值的量測(cè)序列為

求取公式（11）中差值序列的均值和方差，作為濾波器的狀態(tài)變量和誤差協(xié)方差的初值。初始化過程中，實(shí)時(shí)存儲(chǔ)連續(xù)n幀（本初始化算法中，取n=20）航向差值序列ΔΘ，并求取序列的均方差σΔθ。當(dāng)方差小于一定閾值（2°），說(shuō)明GPS 的航向估計(jì)已經(jīng)較為精確，系統(tǒng)初始化完畢。將滿足閾值條件的航向差值序列平均值作為濾波狀態(tài)變量的初值，并將序列的方差作為誤差協(xié)方差的初值，即：

求取式（11）中差值序列的均值和方差，作為濾波器的狀態(tài)變量和誤差協(xié)方差的初值。初始化算法的流程如圖5所示。

圖5 濾波器初始化算法

為保證初始化算法的準(zhǔn)確性，必須保證同步采集到GPS 和陀螺儀的每幀數(shù)據(jù)進(jìn)行比對(duì)。在實(shí)際的工程實(shí)踐中，由于GPS 和陀螺儀的數(shù)據(jù)更新頻率存在差異，“同步采集”只能是一種離散化的近似。

由于GPS 的數(shù)據(jù)更新頻率一般較低（采用的Novtel Propak-G2 GPS接收機(jī)，最大輸出頻率20 Hz），基于慣性器件的陀螺儀更新頻率較高（采用的KVH DSP3000 陀螺儀可選100 Hz 或1 000 Hz 輸出）?？梢越频卣J(rèn)為：GPS 的輸出是相對(duì)離散的，而陀螺儀的輸出是近似連續(xù)的。數(shù)據(jù)同步過程將以GPS 輸出為基準(zhǔn)，采用多線程同步采集GPS 和陀螺儀的輸出信號(hào)，每獲得一幀GPS 數(shù)據(jù)，便抓取附近時(shí)刻的陀螺儀輸出，作為近似的同步數(shù)據(jù)，以此類推，獲得一組連續(xù)的數(shù)據(jù)比對(duì)幀。

實(shí)際上，本算法取航向差序列的幀數(shù)n=20，與GPS 接收機(jī)的最大輸出頻率20 Hz是一致的，即實(shí)際連續(xù)取得1 s內(nèi)的GPS和陀螺儀近似同步序列。

3.2.2 濾波參數(shù)及濾波過程

因?yàn)椴捎靡痪S估計(jì)，濾波器的系統(tǒng)方程和量測(cè)方程的轉(zhuǎn)移矩陣為1×1的單位矩陣，即

系統(tǒng)的預(yù)測(cè)誤差主要由累積誤差和隨機(jī)噪聲構(gòu)成。相比累積誤差，實(shí)時(shí)的隨機(jī)噪聲是小量，可以略去。航跡推算系統(tǒng)的航向信息來(lái)自光纖陀螺的相對(duì)航向輸出，累積誤差δdrift以近似線性方式增長(zhǎng)。經(jīng)過靜態(tài)試驗(yàn)標(biāo)定后，陀螺每小時(shí)的零漂在7.2°，過程噪聲協(xié)方差1×1矩陣Qk可以按式（3）和圖4所示流程推導(dǎo)，即為

式中：dtk為陀螺儀更新的時(shí)間間隔。如果選取100 Hz的角度更新輸出模式，時(shí)間間隔dtk=0.01 s。

為達(dá)到更好的濾波效果，采用統(tǒng)計(jì)的方法，較為精確的估計(jì)量測(cè)噪聲。量測(cè)噪聲的估計(jì)方法與濾波器初始化時(shí)所用方法類似。同時(shí)采集連續(xù)20 幀的航跡推算系統(tǒng)和GPS 測(cè)量得到的航向序列并做差，得到航向差序列，求取均值和均方差。短時(shí)間內(nèi)，陀螺的零漂誤差可以忽略不計(jì)，航相差序列的均方差可以近似作為系統(tǒng)的量測(cè)噪聲，序列的均值則作為量測(cè)變量，即

濾波器估計(jì)的是航跡推算系統(tǒng)的航向誤差，航向誤差主要是由光纖陀螺零漂造成的累積誤差。相比GPS 航向測(cè)量的噪聲，短時(shí)間內(nèi)陀螺的累積誤差很??；而且，GPS 對(duì)航向的測(cè)量受車速和GPS 接收信號(hào)的影響，測(cè)量精度并不穩(wěn)定。因此在濾波時(shí)，沒必要對(duì)每幀數(shù)據(jù)都進(jìn)行量測(cè)更新。設(shè)定閾值為5°，只有當(dāng)航相差序列標(biāo)準(zhǔn)差σΔθk＜5°時(shí)，才進(jìn)行量測(cè)更新，校正系統(tǒng)變量和狀態(tài)協(xié)方差矩陣。否則，只進(jìn)行一步預(yù)測(cè)過程，維持航向誤差不變，同時(shí)向前推算系統(tǒng)誤差協(xié)方差。實(shí)時(shí)濾波流程如圖6 所示。圖中，Δθf(wàn)ilter表示經(jīng)過卡爾曼濾波器融合后輸出的航向角誤差數(shù)據(jù)，θf(wàn)表示最終輸出的融合航向角信息。

圖6 實(shí)時(shí)濾波過程

4 試驗(yàn)結(jié)果分析

采用該融合算法進(jìn)行實(shí)際測(cè)試。融合算法軟件基于linux C++平臺(tái)開發(fā)，分為數(shù)據(jù)解析、數(shù)據(jù)處理和數(shù)據(jù)發(fā)送三個(gè)模塊。數(shù)據(jù)解析模塊主要負(fù)責(zé)讀取來(lái)自GPS、陀螺儀的數(shù)據(jù)包信息。數(shù)據(jù)處理模塊則負(fù)責(zé)融合處理來(lái)自各個(gè)傳感器的測(cè)量數(shù)據(jù)，數(shù)據(jù)發(fā)送模塊負(fù)責(zé)將經(jīng)過融合濾波處理的航向數(shù)據(jù)打包發(fā)送到機(jī)器人導(dǎo)航控制模塊。因?yàn)橐瑫r(shí)處理來(lái)自不同傳感器的數(shù)據(jù)，為保證系統(tǒng)的實(shí)時(shí)性，滿足多傳感器和多任務(wù)的要求，結(jié)算軟件采用多線程方式設(shè)計(jì)，利用linux 平臺(tái)的并發(fā)性，采用多線程同時(shí)解析和處理多傳感器的信息并進(jìn)行數(shù)據(jù)融合。

在戶外模擬變電站復(fù)雜環(huán)境進(jìn)行實(shí)際運(yùn)行試驗(yàn)，機(jī)器人最高運(yùn)行速度控制在5 m∕s 以內(nèi)。在濾波器初始化完畢后，得到單純依靠GPS 測(cè)得航向和數(shù)據(jù)融合得到的航向結(jié)果如圖7所示。

圖7 航向?qū)Ρ?/p>

為了更全面的分析融合算法獲得航向的效果，分別選取機(jī)器人從靜止到啟動(dòng)、GPS信號(hào)較差和GPS信號(hào)較好3 種情況，對(duì)比融合航向輸出和GPS 航向輸出，結(jié)果如圖8—圖10所示。

圖8 從靜止到啟動(dòng)狀態(tài)下航向輸出對(duì)比

圖9 GPS信號(hào)差情況下航向輸出對(duì)比

圖10 從靜止到啟動(dòng)狀態(tài)下航向輸出對(duì)比

由圖8 結(jié)果可知，在靜止?fàn)顟B(tài)下，單純靠GPS 無(wú)法獲得航向信息。然而，濾波算法融合GPS 和航跡推算的航向輸出，即使在靜止?fàn)顟B(tài)下也可以對(duì)車體的航向信息進(jìn)行保持。車啟動(dòng)后，濾波算法得到的航向信息與GPS 測(cè)得的航向能夠很好吻合，說(shuō)明靜態(tài)下航向保持的效果很好。

由圖9 結(jié)果可知，在GPS 信號(hào)受到干擾情況下，依靠GPS獲得的航向信息有很大的誤差。依靠融合算法，可以有效消除抑制GPS的誤差，即使在GPS性能很差的情況下，融合航向仍能保持較高的輸出精度。

由圖10 結(jié)果可知，在GPS 信號(hào)較好的情況下，能夠輸出較高精度的航向信息，但是仍存在隨機(jī)噪聲。通過融合算法得到的航向信息相對(duì)連續(xù)，能夠有效消除隨機(jī)噪聲的干擾。

5 結(jié)語(yǔ)

提出了一種基于卡爾曼濾波的地面巡檢機(jī)器人實(shí)時(shí)航向估計(jì)融合算法。通過GPS 信號(hào)修正陀螺儀的累積誤差；同時(shí)，通過陀螺儀的穩(wěn)定輸出消除GPS輸出的隨機(jī)噪聲和信號(hào)不穩(wěn)定性。試驗(yàn)結(jié)果表明，該融合算法可以提高輸出航向的準(zhǔn)確性，魯棒性，并在GPS丟失或靜止?fàn)顟B(tài)下保持高精度的航向輸出。