基于捷聯(lián)慣導(dǎo)的變壓器內(nèi)微纜機(jī)器人定位研究

趙曉軍 田糧川 孫文博 劉陽(yáng)

關(guān)鍵詞： 捷聯(lián)慣導(dǎo); 四階龍格庫(kù)塔法; 變壓器; 微纜機(jī)器人; 卡爾曼濾波算法; 定位系統(tǒng)

中圖分類號(hào)： TN713?34; TP393 ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼： A ? ? ? ? ? ? ? ? ? ?文章編號(hào)： 1004?373X（2018）24?0035?04

Research on micro?cable robot positioning based on strapdown

inertial navigation for transformer

ZHAO Xiaojun， TIAN Liangchuan， SUN Wenbo， LIU Yang

（School of Electronic Information Engineering， Hebei University， Baoding 071000， China）

Abstract： A micro?cable robot positioning system based on strapdown inertial navigation is designed to reduce the cost of regular internal inspections of the large?scale transformer and resolve the positioning problem during the internal inspection of the large?scale transformer. The system is equipped with such inertial measurement units as acceleration sensor， gyro sensor and camera. The fourth?order Runge?Kutta method is used to solve the attitude matrix. The attitude matrix data is modified by combining the Kalman filtering algorithm to integrate accelerometer sensor data， so as to determine the moving speed and position of the robot. The robot positioning can be periodically corrected by using the camera， so as to guarantee the accuracy of the position. The experimental data shows that the system has strong autonomy and high navigation accuracy within a short period of time， which fully conforms to the positioning requirement of the detection system in the large?scale transformer.

Keywords： strapdown inertial navigation; fourth?order Runge?Kutta method; transformer; micro?cable robot; Kalman filtering algorithm; positioning system

在電力系統(tǒng)的運(yùn)行過(guò)程中，變壓器承擔(dān)著重要作用。同時(shí)，壓器故障也是整個(gè)電力系統(tǒng)穩(wěn)定性的重要隱患，對(duì)直流輸送功率和交流網(wǎng)架有很大的影響。及時(shí)監(jiān)測(cè)和發(fā)現(xiàn)變壓器內(nèi)部故障，迅速進(jìn)行故障定位、處理內(nèi)部故障，使大型變壓器安全生產(chǎn)運(yùn)行十分重要。

氣相色譜法是通過(guò)分析絕緣油中溶解氣體，進(jìn)而對(duì)變壓器內(nèi)部狀況進(jìn)行診斷定位。超聲波是通過(guò)監(jiān)測(cè)變壓器的局部放電異常狀況，進(jìn)行定位計(jì)算。變壓器繞組匝間短路故障定位根據(jù)匝間短路后對(duì)應(yīng)匝波阻抗改變引起的反射電壓突變，來(lái)實(shí)現(xiàn)繞組匝間短路故障定位計(jì)算。這些定位方法都是通過(guò)對(duì)變壓器內(nèi)某一變量的研究，從而檢測(cè)變壓器內(nèi)部運(yùn)行情況，進(jìn)行定位，這些系統(tǒng)檢測(cè)方法并沒(méi)有反映變壓器內(nèi)部的工作狀態(tài)和故障水平，檢測(cè)變壓器內(nèi)部綜合運(yùn)行情況不準(zhǔn)確，故障定位不精確。如果是通過(guò)人工進(jìn)行檢查的話，需要吊罩拆開箱體或?qū)z測(cè)技術(shù)人員送入變壓器內(nèi)部進(jìn)行檢查，這樣不僅需要將變壓器內(nèi)部絕緣油排空，浪費(fèi)大量人力，還極易造成變壓器內(nèi)部污染，變壓器安全運(yùn)行帶來(lái)二次隱患。因此尋求一種能夠系統(tǒng)檢測(cè)變壓器內(nèi)部狀態(tài)的潛測(cè)機(jī)器人定位系統(tǒng)或方法具有重要意義。

針對(duì)這種現(xiàn)象設(shè)計(jì)了變壓器內(nèi)微纜機(jī)器人檢測(cè)系統(tǒng)，它是一種體積很小可以在變壓器內(nèi)自由游動(dòng)的裝置，一方面可以通過(guò)攝像頭觀察變壓器內(nèi)的運(yùn)行情況，同時(shí)定位;另一方面通過(guò)定位系統(tǒng)可以控制潛測(cè)機(jī)器人在變壓器的姿態(tài)，確定機(jī)器人在變壓器的三維定位信息。本文主要研究潛測(cè)機(jī)器人在變壓器的定位。對(duì)機(jī)器人定位技術(shù)來(lái)說(shuō)，因?yàn)镽FID射頻是利用無(wú)線射頻進(jìn)行非接觸位置定位的通信技術(shù)，能進(jìn)行區(qū)域定位，但不能在變壓器安放;WiFi定位技術(shù)是通過(guò)無(wú)線信號(hào)強(qiáng)弱進(jìn)行位置計(jì)算的定位技術(shù)，受變壓器內(nèi)部環(huán)境影響波動(dòng)較大;GPS定位技術(shù)由于變壓器的密閉性也不適用。而捷聯(lián)慣導(dǎo)系統(tǒng)是一種不依賴于任何外部信息、也不向外部輻射能量的自主式導(dǎo)航系統(tǒng)，可在空中、地面、水下等各種復(fù)雜環(huán)境下工作。綜上所述，本文采用捷聯(lián)慣導(dǎo)技術(shù)的方法對(duì)于機(jī)器人在變壓器的定位進(jìn)行了研究。

1 ?系統(tǒng)總體設(shè)計(jì)

本設(shè)計(jì)完成一種室內(nèi)機(jī)器人檢測(cè)定位系統(tǒng)設(shè)計(jì)，它可通過(guò)上位機(jī)控制其狀態(tài)，進(jìn)而控制檢測(cè)系統(tǒng)終端在室內(nèi)空間的來(lái)回自由走動(dòng)。室內(nèi)檢測(cè)機(jī)器人定位系統(tǒng)包括系統(tǒng)終端控制板、定位信息模塊、通信模塊、信息采集模塊以及供電模塊。檢測(cè)系統(tǒng)的設(shè)計(jì)總體框見圖1。

通過(guò)數(shù)字?jǐn)z像頭對(duì)室內(nèi)特殊部位進(jìn)行重點(diǎn)檢查，對(duì)室內(nèi)情況進(jìn)行有效監(jiān)控，實(shí)時(shí)傳輸室內(nèi)圖像，同時(shí)通過(guò)終端搭載的測(cè)距模塊和MEMS陀螺儀模塊測(cè)得檢測(cè)系統(tǒng)終端在室內(nèi)移動(dòng)的距離、仰角、偏航角等信息。然后將采集到的信息進(jìn)行處理存儲(chǔ)，同時(shí)把處理后的信息通過(guò)無(wú)線通信模塊傳送到上位機(jī)，數(shù)據(jù)傳送到上位機(jī)后再經(jīng)過(guò)一定的算法可以得到機(jī)器人檢測(cè)系統(tǒng)設(shè)計(jì)終端在室內(nèi)三維空間的定位情況。

2 ?檢測(cè)系統(tǒng)硬件設(shè)計(jì)

2.1 ?系統(tǒng)終端控制板

本系統(tǒng)終端控制板采用的主控芯片為STM32F4。該芯片具有低功耗、外設(shè)接口、信息處理能力快等優(yōu)良特性。同時(shí)，STM32F4的主頻也提高了很多，達(dá)到168 MHz，可獲得 210 DMIPS 的處理能力，并且自帶攝像頭接口，這使得其在圖像處理方面更加方便，具有非常廣泛的應(yīng)用前景。

2.2 ?信息采集模塊

系統(tǒng)終端控制板上配置有OV5640攝像頭圖像模塊，其主要作用是對(duì)室內(nèi)的情況進(jìn)行采集，將采集接收的數(shù)據(jù)傳送到上位機(jī)，然后實(shí)時(shí)顯示室內(nèi)的情況。OV5640是OmniVision 公司生產(chǎn)的高清像素圖像傳感器，其最高可輸出500萬(wàn)像素的彩色圖像，輸出圖像清晰。

2.3 ?定位信息模塊

陀螺儀模塊是由3軸加速度、3軸陀螺儀、3軸磁力計(jì)及其外圍電路組成。它采用的是君悅智控科技有限公司生產(chǎn)的MPU9250型號(hào)的復(fù)合型九軸陀螺儀模塊。動(dòng)態(tài)測(cè)角精度達(dá)到0.05°，最高達(dá)到200 Hz的數(shù)據(jù)輸出頻率，量程加速度為±16 g，角速度為2 000 （°）/s，穩(wěn)定性加速度為0.01 g，角速度為0.05 （°）/s。

3 ?系統(tǒng)終端定位原理與分析

捷聯(lián)慣導(dǎo)定位系統(tǒng)將輸出的比例信息經(jīng)過(guò)坐標(biāo)變化，然后求解得到速度和位置導(dǎo)航信息。在捷聯(lián)式慣導(dǎo)定位系統(tǒng)中為了得到速度和位置，需要將比力加速度通過(guò)加速度計(jì)測(cè)量出來(lái)，然后使用導(dǎo)航處理器將其轉(zhuǎn)換到導(dǎo)航坐標(biāo)系。為了實(shí)現(xiàn)這個(gè)矢量變換，導(dǎo)航處理器需要知道安裝在檢測(cè)機(jī)器人上的捷聯(lián)慣導(dǎo)傳感器和慣性坐標(biāo)系之間的角度方位，它可以計(jì)算出來(lái)。

在捷聯(lián)慣導(dǎo)系統(tǒng)中，導(dǎo)航處理器解算出導(dǎo)航平臺(tái)。本文設(shè)地理坐標(biāo)系為導(dǎo)航系，需先用慣性測(cè)量元件陀螺儀測(cè)量的角速度[ωbib]，解算角速度[ωnin]，兩者算出姿態(tài)變化角速率，繼而計(jì)算檢測(cè)機(jī)器人姿態(tài)矩陣[Cbn]，然后將檢測(cè)機(jī)器人的姿態(tài)角信息從姿態(tài)矩陣的元素中提取出來(lái)。慣性元件加速度計(jì)測(cè)量比力信息[fb]，結(jié)合[Cbn]變換到導(dǎo)航坐標(biāo)系得到比力[fn]，經(jīng)過(guò)積分解算便可得導(dǎo)航坐標(biāo)系中檢測(cè)機(jī)器人的速度和位置信息。其系統(tǒng)工作原理見圖2。

4 ?系統(tǒng)軟件設(shè)計(jì)

由慣導(dǎo)定位系統(tǒng)基本原理可知速度變化率微分方程為：

[Vn=CnlClbfb-ωnen+2ωnie·Vn+gn] （1）

式中：[Vn]表示檢測(cè)機(jī)器人運(yùn)動(dòng)的速度;g是重力加速度;[fb]表示比力加速度，其值可由加速度計(jì)測(cè)量。式（1）積分得到檢測(cè)機(jī)器人在[tk]時(shí)刻的速度：[ΔVlsfm]表示由比力加速度引起的速度增量，也就是比力增量;[ΔVng/corm]用來(lái)表示在更新周期T內(nèi)總的速度增量。

[Vnk=Vnk-1+CnlΔVlsfm+ΔVng/corm] ? ?（2）

[ΔVlsfm=tk-1tkClbfbdt] ?（3）

[ΔVng/corm=tk-1tkgnp-ωnen+2ωnie·Vndt] ? ? （4）

設(shè)地理坐標(biāo)系為解算的導(dǎo)航坐標(biāo)系，檢測(cè)機(jī)器人位置定位計(jì)算就是采煤機(jī)所處高度h和經(jīng)度λ、緯度L的計(jì)算。南北方向的速度引起緯度的變化，東西方向的速度引起經(jīng)度的影響，將加速度計(jì)測(cè)得的比力信息變換到導(dǎo)航坐標(biāo)系，得到解算坐標(biāo)系的加速度信息。檢測(cè)機(jī)器人定位微分方程為：

[L=VnRy+h，λ=VneRx+hcos L，h=Vnu] （5）

因?yàn)榧铀俣葌鞲衅鬏敵龅氖请x散數(shù)據(jù)且采樣頻率高，根據(jù)高等數(shù)學(xué)和數(shù)值分析可將檢測(cè)機(jī)器人在傳感器單個(gè)采樣間隔內(nèi)的運(yùn)動(dòng)近似視為勻加速直線運(yùn)動(dòng)。其速度采用平均值法，結(jié)合式（2）和式（5）可以求得機(jī)器人的位置信息：

[Lk=Lk-1+Vnk-1+Vnk2Rmk-1+hk-1λk=λk-1+Vnek-1+Vnek2Rnk-1+hk-1cos Lk-1hk=hk-1+12Vnuk-1+Vnuk] ? ? （6）

式中：

[Rn=Re1+esin2L，Rm=Re1-2e+3esin2L]

e為地球的曲率;[Re]為地球長(zhǎng)度半徑;L為緯度。

5 ?實(shí)驗(yàn)結(jié)果與分析

本文是研究捷聯(lián)慣導(dǎo)系統(tǒng)在大型變壓器內(nèi)定位的應(yīng)用，對(duì)其算法進(jìn)行理論分析和實(shí)驗(yàn)研究，主要針對(duì)應(yīng)用捷聯(lián)慣導(dǎo)定位檢測(cè)機(jī)器人位姿的算法進(jìn)行模擬實(shí)驗(yàn)研究，目前還屬于理論研究和系統(tǒng)方案設(shè)計(jì)階段，同時(shí)考慮到其他因素，文中用手驅(qū)動(dòng)檢測(cè)機(jī)器人在空的大型變壓器內(nèi)運(yùn)動(dòng)進(jìn)行數(shù)據(jù)采集，其實(shí)驗(yàn)的效果和原理是一致的，不影響實(shí)驗(yàn)對(duì)算法有效行的估計(jì)。該次實(shí)驗(yàn)測(cè)試由被定位人員攜帶檢測(cè)機(jī)器人在大型變壓器內(nèi)三維空間中任意行走，包括大型變壓器內(nèi)同一水平線的行走和豎直方向的行走，最終再回到出發(fā)點(diǎn)，記錄最終的坐標(biāo)值，該值即為行走定位誤差。每次行走測(cè)試總距離大于200 m，進(jìn)行5次測(cè)試。測(cè)試結(jié)果如表1所示。

從表1可以看出，平均平面相對(duì)誤差為2.61%，平均高度相對(duì)誤差為1.02%，可以滿足大型變壓器內(nèi)定位要求。本系統(tǒng)因其可短時(shí)間內(nèi)進(jìn)行高精度自主定位的特性，可在復(fù)雜環(huán)境進(jìn)行目標(biāo)定位，從而具有較高的應(yīng)用價(jià)值。

6 ?結(jié) ?論

本文提出一種通過(guò)三軸加速度、陀螺儀傳感器的慣性測(cè)量單元估計(jì)大型變壓器內(nèi)檢測(cè)機(jī)器人的運(yùn)動(dòng)狀態(tài)，實(shí)現(xiàn)對(duì)其動(dòng)態(tài)定位跟蹤的方法。實(shí)驗(yàn)結(jié)果表明，該系統(tǒng)短時(shí)間內(nèi)可以對(duì)檢測(cè)機(jī)器人進(jìn)行高精度的定位，完全符合檢測(cè)系統(tǒng)在大型變壓器內(nèi)的定位需求，而且本文所提出的方法能夠?yàn)榇笮妥儔浩鲀?nèi)檢測(cè)機(jī)器人定位跟蹤在工業(yè)中的應(yīng)用提供借鑒，可以為變壓器現(xiàn)場(chǎng)內(nèi)部檢查發(fā)生革命性的變化，大幅降低檢查成本，避免吊罩拆開箱體而造成影響變壓器原有的設(shè)計(jì)性及可靠性的弊端，具有廣闊的市場(chǎng)前景和巨大的經(jīng)濟(jì)效益。

參考文獻(xiàn)

[1] 王磊，汪洲，任元，等.捷聯(lián)慣導(dǎo)系統(tǒng)中卡爾曼濾波的應(yīng)用研究[J].計(jì)算機(jī)測(cè)量與控制，2017，25（6）：139?141.

WANG Lei， WANG Zhou， REN Yuan， et al. Application of Kalman filter in strapdown inertial navigation system [J]. Computer measurement & control， 2017， 25（6）： 139?141.

[2] 黃榮輝，馮迎賓，李勛，等.基于WiFi遙控的油浸式變壓器內(nèi)部檢測(cè)機(jī)器人研究[J].機(jī)械工程師，2017（6）：73?76.

HUANG Ronghui， FENG Yingbin， LI Xun， et al. Study on fault detection robot for oil?immersed transformer based on WiFi control [J]. Mechanical engineer， 2017（6）： 73?76.

[3] 李勛，王亞彪，黃榮輝，等.油浸式變壓器內(nèi)部檢測(cè)機(jī)器人結(jié)構(gòu)設(shè)計(jì)與分析[J].機(jī)械工程師，2017（3）：35?38.

LI Xun， WANG Yabiao， HUANG Ronghui， et al. Structure design and analysis of oil?immersed transformer internal inspection robot [J]. Mechanical engineer， 2017（3）： 35?38.

[4] 吳哲明，孫振國(guó)，張文增，等.基于慣性測(cè)量單元旋轉(zhuǎn)的陀螺漂移估計(jì)和補(bǔ)償方法[J].清華大學(xué)學(xué)報(bào)（自然科學(xué)版），2014，54（9）：1143?1147.

WU Zheming， SUN Zhenguo， ZHANG Wenzeng， et al. Gyroscope bias estimation and compensation by rotation of the inertial measurement [J]. Journal of Tsinghua University （Science and technology）， 2014， 54（9）： 1143?1147.

[5] 張露.基于捷聯(lián)慣導(dǎo)的采煤機(jī)位姿解算算法研究[D].西安：西安科技大學(xué)，2014.

ZHANG Lu. The research based on strapdown inertial navigation of shearer position and posture calculate algorithm [D]. Xian： Xian University of Science and Technology， 2014.

[6] 謝鵬程.基于單攝像頭的運(yùn)動(dòng)目標(biāo)跟蹤定位技術(shù)研究[J].計(jì)算機(jī)光盤軟件與應(yīng)用，2012，15（18）：82?84.

XIE Pengcheng. Research on moving target tracking and positioning technology based on single camera [J]. Computer CD software and applications， 2012， 15（18）： 82?84.

[7] 曲東才.捷聯(lián)慣導(dǎo)系統(tǒng)發(fā)展及其軍事應(yīng)用[J].航空科學(xué)技術(shù)，2004（6）：27?30.

QU Dongcai. Development of strapdown inertial navigation system and its military application [J]. Aeronautical science and technology， 2004（6）： 27?30.

[8] 鄭新芳.視覺(jué)慣導(dǎo)融合實(shí)時(shí)6DOF機(jī)器人定位方法研究[D].北京：北京交通大學(xué)，2017.

ZHENG Xinfang. Research on visual inertial navigation integrated real?time 6DOF robot positioning method [D]. Beijing： Beijing Jiaotong University， 2017.

[9] 王澤根，譚靜，王金柱.基于組合導(dǎo)航的管道內(nèi)部定位仿真與誤差分析[J].西南石油大學(xué)學(xué)報(bào)（自然科學(xué)版），2017，39（2）：163?171.

WANG Zegen， TAN Jing， WANG Jinzhu. Simulation and error analysis for pipeline internal positioning based on integrated navigation [J]. Journal of Southwest Petroleum University （Science & technology edition）， 2017， 39（2）： 163?171.

[10] 劉宇倫.基于地磁輔助系統(tǒng)的捷聯(lián)慣導(dǎo)系統(tǒng)設(shè)計(jì)[D].南京：南京理工大學(xué)，2017.

LIU Yulun. Design of strapdown inertial navigation system based on geomagnetic auxiliary system [D]. Nanjing： Nanjing University of Science and Technology， 2017.

[11] 黃東，楊凌輝，羅文，等.基于視覺(jué)/慣導(dǎo)的掘進(jìn)機(jī)實(shí)時(shí)位姿測(cè)量方法研究[J].激光技術(shù)，2017，41（1）：19?23.

HUANG Dong， YANG Linghui， LUO Wen， et al. Study on measurement method of real?time position and attitude of roadheader based on vision/inertial navigation system [J]. Laser technology， 2017， 41（1）： 19?23.

[12] 王赟赟，李建民，侯文.基于組合導(dǎo)航的汽車姿態(tài)數(shù)據(jù)采集系統(tǒng)設(shè)計(jì)[J].電子產(chǎn)品世界，2017，24（1）：44?47.

WANG Yunyun， LI Jianmin， HOU Wen. Design of automobile attitude data acquisition system based on integrated navigation [J]. Electronic engineering product world， 2017， 24（1）： 44?47.

[13] 趙廣杰，許忠信，楊煉，等.基于FPGA的雙攝像頭定位系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].電視技術(shù)，2009，33（z2）：220?222.

ZHAO Guangjie， XU Zhongxin， YANG Lian， et al. Implementation of positioning system using two cameras based on FPGA [J]. Video engineering， 2009， 33（S2）： 220?222.