趙曉軍 田糧川 孫文博 劉陽

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

中圖分類號： TN713?34; TP393 ? ? ? ? ? ? ? ? 文獻標識碼： A ? ? ? ? ? ? ? ? ? ?文章編號： 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)的運行過程中，變壓器承擔著重要作用。同時，壓器故障也是整個電力系統(tǒng)穩(wěn)定性的重要隱患，對直流輸送功率和交流網(wǎng)架有很大的影響。及時監(jiān)測和發(fā)現(xiàn)變壓器內(nèi)部故障，迅速進行故障定位、處理內(nèi)部故障，使大型變壓器安全生產(chǎn)運行十分重要。

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

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

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

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

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

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

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

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

2.2 ?信息采集模塊

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

2.3 ?定位信息模塊

陀螺儀模塊是由3軸加速度、3軸陀螺儀、3軸磁力計及其外圍電路組成。它采用的是君悅智控科技有限公司生產(chǎn)的MPU9250型號的復(fù)合型九軸陀螺儀模塊。動態(tài)測角精度達到0.05°，最高達到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)過坐標變化，然后求解得到速度和位置導(dǎo)航信息。在捷聯(lián)式慣導(dǎo)定位系統(tǒng)中為了得到速度和位置，需要將比力加速度通過加速度計測量出來，然后使用導(dǎo)航處理器將其轉(zhuǎn)換到導(dǎo)航坐標系。為了實現(xiàn)這個矢量變換，導(dǎo)航處理器需要知道安裝在檢測機器人上的捷聯(lián)慣導(dǎo)傳感器和慣性坐標系之間的角度方位，它可以計算出來。

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

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

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

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

式中：[Vn]表示檢測機器人運動的速度;g是重力加速度;[fb]表示比力加速度，其值可由加速度計測量。式（1）積分得到檢測機器人在[tk]時刻的速度：[ΔVlsfm]表示由比力加速度引起的速度增量，也就是比力增量;[ΔVng/corm]用來表示在更新周期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è)地理坐標系為解算的導(dǎo)航坐標系，檢測機器人位置定位計算就是采煤機所處高度h和經(jīng)度λ、緯度L的計算。南北方向的速度引起緯度的變化，東西方向的速度引起經(jīng)度的影響，將加速度計測得的比力信息變換到導(dǎo)航坐標系，得到解算坐標系的加速度信息。檢測機器人定位微分方程為：

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

因為加速度傳感器輸出的是離散數(shù)據(jù)且采樣頻率高，根據(jù)高等數(shù)學和數(shù)值分析可將檢測機器人在傳感器單個采樣間隔內(nèi)的運動近似視為勻加速直線運動。其速度采用平均值法，結(jié)合式（2）和式（5）可以求得機器人的位置信息：

[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]為地球長度半徑;L為緯度。

5 ?實驗結(jié)果與分析

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

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

6 ?結(jié) ?論

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

參考文獻

[1] 王磊，汪洲，任元，等.捷聯(lián)慣導(dǎo)系統(tǒng)中卡爾曼濾波的應(yīng)用研究[J].計算機測量與控制，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)部檢測機器人研究[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)部檢測機器人結(jié)構(gòu)設(shè)計與分析[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] 吳哲明，孫振國，張文增，等.基于慣性測量單元旋轉(zhuǎn)的陀螺漂移估計和補償方法[J].清華大學學報（自然科學版），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)的采煤機位姿解算算法研究[D].西安：西安科技大學，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] 謝鵬程.基于單攝像頭的運動目標跟蹤定位技術(shù)研究[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].航空科學技術(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] 鄭新芳.視覺慣導(dǎo)融合實時6DOF機器人定位方法研究[D].北京：北京交通大學，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].西南石油大學學報（自然科學版），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è)計[D].南京：南京理工大學，2017.

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

[11] 黃東，楊凌輝，羅文，等.基于視覺/慣導(dǎo)的掘進機實時位姿測量方法研究[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].電子產(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è)計與實現(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.