楊 磊，龐 碩，楊耀民，景春雷，高 偉，劉慶亮，馬 俊

(1.國家深?；毓芾碇行?，山東 青島 266061; 2.美國Embry-Riddle大學(xué)，美國 佛羅里達(dá) 32114)

?

基于9DOF IMU的AUV慣性導(dǎo)航技術(shù)研究

楊 磊1，龐 碩2，楊耀民1，景春雷1，高 偉1，劉慶亮1，馬 俊2

(1.國家深?；毓芾碇行?，山東 青島 266061; 2.美國Embry-Riddle大學(xué)，美國 佛羅里達(dá) 32114)

水下機(jī)器人(underwater vehicle)慣性導(dǎo)航技術(shù)是目前機(jī)器人控制技術(shù)的難點(diǎn)，通過加裝不同類型的IMU組件，可以實(shí)現(xiàn)水下機(jī)器人姿態(tài)、方位等參數(shù)的測量，并將這些參數(shù)作為反饋輸入來實(shí)現(xiàn)水下機(jī)器人的精確控制；針對低成本微慣性傳感器的應(yīng)用特性，采用Sparkfun、Arduino Mega和Raspberry Pi，設(shè)計(jì)了基于Raspberry Pi集中處理、Arduino控制板分布式控制的微小型捷聯(lián)慣導(dǎo)系統(tǒng)，實(shí)現(xiàn)了水下機(jī)器人慣性導(dǎo)航系統(tǒng)的全部導(dǎo)航和制導(dǎo)參數(shù)讀取，包含位置坐標(biāo)、線速度、角速度、姿態(tài)角、方位角等信息，通過水池試驗(yàn)表明應(yīng)用表明本系統(tǒng)滿足Eco-dolphin的水下定航控制要求。

IMU; 慣性導(dǎo)航; PID算法

0 引言

慣性導(dǎo)航(inertial navigation)是上世紀(jì)中葉發(fā)展起來的完全自主式的導(dǎo)航技術(shù)。主要通過慣性測量組件(IMU)測量載體平臺相對慣性空間的角速度率和加速度值，利用牛頓經(jīng)典運(yùn)動理論自動積分計(jì)算載體平臺的瞬時(shí)速度和位置坐標(biāo)信息，具有不需依賴外部信息、不向外界輻射能量、不受外界干擾、隱蔽性較好等特點(diǎn)，并且慣性導(dǎo)航系統(tǒng)能夠連續(xù)的提供載體平臺的全部導(dǎo)航和制導(dǎo)參數(shù)(位置坐標(biāo)、線速度、角速度、姿態(tài)角、方位角)，故可以廣泛應(yīng)用于航空、航天、航海等領(lǐng)域，特別是在軍事領(lǐng)域應(yīng)用越來越普遍。

Arduino控制板是比較成熟的控制板卡，在小型無人機(jī)、艦船和水下機(jī)器人領(lǐng)域有著廣泛的應(yīng)用，其編程可以采用基于C語言的編程開發(fā)模式，且多數(shù)算法和程序包具備廣泛的開源特點(diǎn)。美國Sparkfun公司生產(chǎn)的9 DOF IMU具備體積小、定位精度高、連接方便、后續(xù)開發(fā)容易等優(yōu)點(diǎn)，本文選用該型導(dǎo)航元件，應(yīng)用Arduino控制板通過串口讀取IMU的各參數(shù)，編寫Arduino板與IMU模塊的通訊協(xié)議，定義IMU的數(shù)據(jù)讀取格式，完成基于#OSCT指令的參數(shù)的讀取、字符串辨識和數(shù)裝載程序的開發(fā)和測試，并進(jìn)行預(yù)處理后，發(fā)送到Raspberry Pi進(jìn)行處理，并將處理后的運(yùn)動參數(shù)發(fā)回Arduino控制板實(shí)現(xiàn)對Eco-dolphin的定航控制。

本文針對低成本微慣性傳感器的應(yīng)用特性，采用Sparkfun、Arduino Mega和Raspberry Pi，設(shè)計(jì)了基于Raspberry Pi集中處理、Arduino控制板分布式控制的微小型捷聯(lián)慣導(dǎo)系統(tǒng)，應(yīng)用表明本系統(tǒng)滿足Eco-dolphin的水下定航控制要求。

1 捷聯(lián)慣性導(dǎo)航的基本原理

捷聯(lián)慣性導(dǎo)航通過慣性測量組件(IMU)測量載體平臺相對慣性空間的角速度率和加速度值，利用牛頓經(jīng)典運(yùn)動理論自動積分計(jì)算載體平臺的瞬時(shí)速度和位置坐標(biāo)信息，具有不需依賴外部信息、不向外界輻射能量、不受外界干擾、隱蔽性較好等特點(diǎn)，其組成的基本原理如圖1、圖2所示。

圖1 導(dǎo)航系統(tǒng)原理框圖

圖2 捷聯(lián)慣性導(dǎo)航的計(jì)算框圖

圖3 捷聯(lián)慣導(dǎo)系統(tǒng)框圖

2 Eco-dolphin導(dǎo)航系統(tǒng)的組成

為滿足eco-dolphin完成水下航行控制的要求，實(shí)現(xiàn)基于目標(biāo)定位的航行控制，本文針對低成本微慣性傳感器的應(yīng)用特性，采用Sparkfun、Arduino Mega和Raspberry Pi，設(shè)計(jì)了基于Raspberry Pi集中處理、Arduino控制板分布式控制的微小型捷聯(lián)慣導(dǎo)系統(tǒng)，其運(yùn)行原理和構(gòu)架如圖4、圖5所示。

圖4 Eco-Dolphin慣性導(dǎo)航模塊工作示意圖

圖5 Eco-Dolphin的慣性導(dǎo)航和控制組件結(jié)構(gòu)框圖

2.1 慣性導(dǎo)航測量模塊

本文采用9DOF的SparkFun制作IMU包含3個(gè)傳感器：一個(gè)ITG-3200(MEMS三軸陀螺儀)，ADXL345(三軸加速度計(jì))，與HMC5883L(三軸磁力計(jì))可以完成九自由度慣性測量。所有傳感器的輸出是由一個(gè)ATmega328板上的接口通過串行進(jìn)行處理和輸出。這使得9DOF慣性導(dǎo)航測量模塊可以被應(yīng)用到無人機(jī)、自動駕駛車輛和水下移動載體上。

該模塊的編程可以通過Arduino的8 MHz引導(dǎo)程序(stk500v1)和板卡上的固件傳感器的輸出。只需連接串行TX、RX引腳和3.3 V供電，打開一個(gè)終端程序，波特率設(shè)置為57600bps完成測試傳感器。也可以使用Arduino的IDE來編寫代碼到9DOF的IMU。

該9DOF的IMU測量模塊工作電壓為3.3 VDC；本文通過Arduino輸出3.3 V電壓接口提供IMU的工作電源電壓需要，同時(shí)該模塊可以使用藍(lán)牙或者XBee資源管理器提供一個(gè)無線解決方案，可實(shí)現(xiàn)無線控制和傳輸。

2.2 數(shù)據(jù)采集與預(yù)處理

由Arduino控制A/D轉(zhuǎn)換器完成對加速度計(jì)、陀螺儀以及磁場信號采集與預(yù)處理，并將數(shù)據(jù)輸送到上級處理器Raspberry Pi集中處理，并將數(shù)據(jù)傳輸?shù)酵七M(jìn)器的控制器ESC，完成對航行的控制。

考慮到陀螺儀的帶寬，系統(tǒng)選用美國maxim公司的max197芯片，完成對陀螺、加速度計(jì)信號的采樣。Arduino采用美國Atmel AVR單片機(jī)，采用基于開發(fā)原始代碼的軟硬件平臺，建構(gòu)基于簡單I/O的界面板，并采用基于C語言的編程環(huán)境。

2.3 數(shù)據(jù)通信模塊

數(shù)據(jù)通信模塊包括阿兩個(gè)部分：1)IMU與Arduino控制板之間；2)結(jié)算結(jié)果在Arduino Mega和Raspberry Pi之間的傳遞。IMU與Arduino之間信息的傳遞通過RS232串行通訊接口進(jìn)行數(shù)據(jù)通信。為提高通信的可靠性，采用取數(shù)據(jù)包進(jìn)行加權(quán)平均值的辦法，采用“$”和“#”分別作為幀頭和幀尾，每組數(shù)據(jù)的長度固定。

導(dǎo)航參數(shù)通過Arduino讀取IMU各傳感器數(shù)值進(jìn)行A/D轉(zhuǎn)化，轉(zhuǎn)載到數(shù)據(jù)段中，然后傳送給Raspberry Pi進(jìn)行對比和計(jì)算，并將計(jì)算結(jié)果回傳給Arduino，由Arduino控制ESC實(shí)現(xiàn)對推進(jìn)器的推力分配和控制。

2.4 捷聯(lián)慣性測量單元的靜態(tài)模型

導(dǎo)航計(jì)算模塊主要完成對IMU數(shù)據(jù)信息的讀取初始校準(zhǔn)、捷聯(lián)結(jié)算以及導(dǎo)航結(jié)果的回傳等。本系統(tǒng)采用Raspberry Pi進(jìn)行對比和計(jì)算，并將計(jì)算結(jié)果回傳給Arduino mega。

本文研究的IMU是有3個(gè)撓性加速度計(jì)和3個(gè)光纖陀螺儀組成的慣性測量單元。陀螺儀用來敏感IMU3個(gè)軸的角速度，加速度計(jì)用來敏感3個(gè)軸的比力，通過6路脈沖輸出，給出導(dǎo)航初始參數(shù)。在常溫下，IMU靜態(tài)數(shù)學(xué)模型為：

(1)

(2)

(3)

(4)

(5)

(6)

3 PID控制算法及控制方案

按照控制理論中對PID控制算法的描述，項(xiàng)目所采用離散的PID算法數(shù)學(xué)表達(dá)式：

(7)

其中：Δt=T為采樣周期，保證T足夠的小，系統(tǒng)才有一定的精度；E(k)為第k次采樣信號偏差值：E(k-1)為第k-1次采樣信號偏差值：k為采樣序號，k=0，1，2…；P(k)為第k次采樣時(shí)PID調(diào)節(jié)器的輸出[3-5]。

由式(7) 可知，要計(jì)算輸出值P(k)，不僅需要知道偏差信號E(k)和E(k-1)，而且還要在積分項(xiàng)中把往次的偏差信號E(j)進(jìn)行累加求和，這樣不僅計(jì)算過程繁瑣，而且保存E(j)要占用很多的內(nèi)存。為此，做如下優(yōu)化。

根據(jù)遞推原理，第k-1次PID輸出的表達(dá)式：

(8)

則得到：

(9)

由式(9)可知，要計(jì)算第k次的PID輸出值P(k)，只需知道P(k-1)，E(k)，E(k-1)和E(k-2)即可，因此計(jì)算要簡單許多。

在水下機(jī)器人控制系統(tǒng)中，由于執(zhí)行機(jī)構(gòu)是采用由電機(jī)驅(qū)動的推進(jìn)器進(jìn)行控制的，所以，只要輸出一個(gè)增量信號即可。則，由式(9)得到：

(10)

式中，PK、DK表示第k次輸出的增量值ΔP(k)，等于第k次與第k-1次調(diào)節(jié)器輸出的偏差值，式(10)稱作增量型PID控制計(jì)算式。用計(jì)算機(jī)實(shí)現(xiàn)增量式數(shù)字PID控制的原理方框圖和程序框圖，分別如圖6、圖7所示。

圖6 增量式PID控制算法流程框圖

圖7 OpenRov水池測試試驗(yàn)

4 試驗(yàn)測試

采用設(shè)計(jì)的數(shù)字PID算法分別對Eco-Dophin進(jìn)行定深度和航向角進(jìn)行控制試驗(yàn)，本文對Eco-Dophin的深度設(shè)定為5米進(jìn)行定深航行試驗(yàn)。圖8中為Depth Sensor測量得到的定深測試的試驗(yàn)結(jié)果曲線。

圖8 定深測試的試驗(yàn)結(jié)果曲線

在完成Eco-Dophin的定深航行試驗(yàn)后，開始定航向航行試驗(yàn)。圖9為定航向航行時(shí)的航向/時(shí)間曲線，設(shè)定航向?yàn)楸毕蛄愣?，通過IMU慣導(dǎo)測得的航向角如圖中線所示。

圖9 定航向航行試驗(yàn)航向角測試曲線

5 結(jié)論

本文針對低成本微慣性傳感器的應(yīng)用特性，采用Sparkfun、Arduino Mega和Raspberry Pi，設(shè)計(jì)了基于Raspberry Pi集中處理、Arduino控制板分布式控制的微小型捷聯(lián)慣導(dǎo)系統(tǒng)，應(yīng)用表明本系統(tǒng)滿足Eco-dolphin的水下定航控制要求。同時(shí)，我們發(fā)現(xiàn)Raspberry Pi集中處理+Arduino底層控制辦卡控制模式，在AUV長時(shí)序復(fù)雜軌跡運(yùn)動控制時(shí)可靠性和實(shí)效性還存在處理速度滯后、數(shù)據(jù)容易溢出和報(bào)錯的問題，如何提高該系統(tǒng)的應(yīng)用可靠性，還需要進(jìn)一步研究。

[1] T Fujii , T Ura. Control with neural network for autonomous underwater vehicle[J]. J. Soe. Naval Arch. Japan,1989.166:503-511.

[2] 彭慧惠，熊 智，王 融,等. IMU安裝及動態(tài)標(biāo)注誤差在線辨識方法研究[J].中國空間科學(xué)技術(shù). 2014, 42(1): 42-51.

[3] 曾占魁，曹喜濱，張世杰,等.航天器相對視覺/IMU導(dǎo)航量測修正多速率濾波[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào)．2015,47(3):1-7.

[4] Gu Yu，Liu Yong，Towsley Don. On integrating fluid models with packet simulation[A].Proceedings of IEEE Conference on Computer and Communications(INFOCOM)2004[C].Hong Kong, China.

[5] Kang—Ping, Wang Lan Huang, Chun—Guang Zhou etal. Particle Swarm Optimization for Traveling Sales mail Problem[A]．Proceedings of the 2ndInternational Conference on Machine Learning and Cybernetics[C]. Xian.2003:1583-1587.

[6] 張倫東，練軍想，胡小平.載體角運(yùn)動對旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)旋轉(zhuǎn)調(diào)制效果的影響[J]. 國防科技大學(xué)學(xué)報(bào),2011,33(4)：152-156.

[7] Zhang H X, Yuan O F, et al. Research of DFT. OFDM on different transmission scenarios[J]. ICITA, 2004: 31-33.

Study of Navigation for AUV Based on 9 DOF IMU

Yang Lei1, Pang Shuo2, Yang Yaomin1, Lei Jingchun1, Gao Wei1, Liu Qingliang1, Ma Jun2

(1.China National Deep Sea Center, Qingdao 266061,China;2.Embry-Riddle Aeronautical University, Daytona Beach, FL, United States, 32114)

The inertial navigation technology of underwater vehicle is the difficulty robot control technology, through the installation of different types of IMU components we can measure parameters underwater vehicle posture, orientation, etc., and achieve the underwater vehicle precise control of the robot using these parameters as feedback. In this paper, we designed the basic AUV control system based on the low-cost micro inertial sensor applications including Spark fun, Arduino Mega and Raspberry Pi and so on. The designed navigation and guidance systems focused on Raspberry Pi, micro SINS distributed control Arduino control board can achieve the AUV inertial navigation requirements, which can read and process these parameters, including location coordinates, linear velocity, angular velocity, attitude angle, azimuth and other information. The pool test application results show that the system meets the Eco-dolphin underwater scheduled flight control requirements.

IMU; Navigation Control; PID Algorithm

2015-07-21；

2015-10-26。

海洋系統(tǒng)公派留學(xué)項(xiàng)目(留金法[2014]5042號)。

楊 磊(1982-)，男，山東青島人，博士，主要從事深海裝備研發(fā)、機(jī)械設(shè)計(jì)、水下運(yùn)載技術(shù)方向的研究。

1671-4598(2016)03-0133-03

10.16526/j.cnki.11-4762/tp.2016.03.036

TB69

A