基于9DOF IMU的AUV惯性导航技术研究

杨 磊，庞 硕，杨耀民，景春雷，高 伟，刘庆亮，马 俊

(1.国家深海基地管理中心，山东 青岛 266061; 2.美国Embry-Riddle大学，美国 佛罗里达 32114)



基于9DOF IMU的AUV惯性导航技术研究

杨 磊1，庞 硕2，杨耀民1，景春雷1，高 伟1，刘庆亮1，马 俊2

(1.国家深海基地管理中心，山东 青岛 266061; 2.美国Embry-Riddle大学，美国 佛罗里达 32114)

水下机器人(underwater vehicle)惯性导航技术是目前机器人控制技术的难点，通过加装不同类型的IMU组件，可以实现水下机器人姿态、方位等参数的测量，并将这些参数作为反馈输入来实现水下机器人的精确控制；针对低成本微惯性传感器的应用特性，采用Sparkfun、Arduino Mega和Raspberry Pi，设计了基于Raspberry Pi集中处理、Arduino控制板分布式控制的微小型捷联惯导系统，实现了水下机器人惯性导航系统的全部导航和制导参数读取，包含位置坐标、线速度、角速度、姿态角、方位角等信息，通过水池试验表明应用表明本系统满足Eco-dolphin的水下定航控制要求。

IMU; 惯性导航; PID算法

0 引言

惯性导航(inertial navigation)是上世纪中叶发展起来的完全自主式的导航技术。主要通过惯性测量组件(IMU)测量载体平台相对惯性空间的角速度率和加速度值，利用牛顿经典运动理论自动积分计算载体平台的瞬时速度和位置坐标信息，具有不需依赖外部信息、不向外界辐射能量、不受外界干扰、隐蔽性较好等特点，并且惯性导航系统能够连续的提供载体平台的全部导航和制导参数(位置坐标、线速度、角速度、姿态角、方位角)，故可以广泛应用于航空、航天、航海等领域，特别是在军事领域应用越来越普遍。

Arduino控制板是比较成熟的控制板卡，在小型无人机、舰船和水下机器人领域有着广泛的应用，其编程可以采用基于C语言的编程开发模式，且多数算法和程序包具备广泛的开源特点。美国Sparkfun公司生产的9 DOF IMU具备体积小、定位精度高、连接方便、后续开发容易等优点，本文选用该型导航元件，应用Arduino控制板通过串口读取IMU的各参数，编写Arduino板与IMU模块的通讯协议，定义IMU的数据读取格式，完成基于#OSCT指令的参数的读取、字符串辨识和数装载程序的开发和测试，并进行预处理后，发送到Raspberry Pi进行处理，并将处理后的运动参数发回Arduino控制板实现对Eco-dolphin的定航控制。

本文针对低成本微惯性传感器的应用特性，采用Sparkfun、Arduino Mega和Raspberry Pi，设计了基于Raspberry Pi集中处理、Arduino控制板分布式控制的微小型捷联惯导系统，应用表明本系统满足Eco-dolphin的水下定航控制要求。

1 捷联惯性导航的基本原理

捷联惯性导航通过惯性测量组件(IMU)测量载体平台相对惯性空间的角速度率和加速度值，利用牛顿经典运动理论自动积分计算载体平台的瞬时速度和位置坐标信息，具有不需依赖外部信息、不向外界辐射能量、不受外界干扰、隐蔽性较好等特点，其组成的基本原理如图1、图2所示。

图1 导航系统原理框图

图2 捷联惯性导航的计算框图

图3 捷联惯导系统框图

2 Eco-dolphin导航系统的组成

为满足eco-dolphin完成水下航行控制的要求，实现基于目标定位的航行控制，本文针对低成本微惯性传感器的应用特性，采用Sparkfun、Arduino Mega和Raspberry Pi，设计了基于Raspberry Pi集中处理、Arduino控制板分布式控制的微小型捷联惯导系统，其运行原理和构架如图4、图5所示。

图4 Eco-Dolphin惯性导航模块工作示意图

图5 Eco-Dolphin的惯性导航和控制组件结构框图

2.1 惯性导航测量模块

本文采用9DOF的SparkFun制作IMU包含3个传感器：一个ITG-3200(MEMS三轴陀螺仪)，ADXL345(三轴加速度计)，与HMC5883L(三轴磁力计)可以完成九自由度惯性测量。所有传感器的输出是由一个ATmega328板上的接口通过串行进行处理和输出。这使得9DOF惯性导航测量模块可以被应用到无人机、自动驾驶车辆和水下移动载体上。

该模块的编程可以通过Arduino的8 MHz引导程序(stk500v1)和板卡上的固件传感器的输出。只需连接串行TX、RX引脚和3.3 V供电，打开一个终端程序，波特率设置为57600bps完成测试传感器。也可以使用Arduino的IDE来编写代码到9DOF的IMU。

该9DOF的IMU测量模块工作电压为3.3 VDC；本文通过Arduino输出3.3 V电压接口提供IMU的工作电源电压需要，同时该模块可以使用蓝牙或者XBee资源管理器提供一个无线解决方案，可实现无线控制和传输。

2.2 数据采集与预处理

由Arduino控制A/D转换器完成对加速度计、陀螺仪以及磁场信号采集与预处理，并将数据输送到上级处理器Raspberry Pi集中处理，并将数据传输到推进器的控制器ESC，完成对航行的控制。

考虑到陀螺仪的带宽，系统选用美国maxim公司的max197芯片，完成对陀螺、加速度计信号的采样。Arduino采用美国Atmel AVR单片机，采用基于开发原始代码的软硬件平台，建构基于简单I/O的界面板，并采用基于C语言的编程环境。

2.3 数据通信模块

数据通信模块包括阿两个部分：1)IMU与Arduino控制板之间；2)结算结果在Arduino Mega和Raspberry Pi之间的传递。IMU与Arduino之间信息的传递通过RS232串行通讯接口进行数据通信。为提高通信的可靠性，采用取数据包进行加权平均值的办法，采用“$”和“#”分别作为帧头和帧尾，每组数据的长度固定。

导航参数通过Arduino读取IMU各传感器数值进行A/D转化，转载到数据段中，然后传送给Raspberry Pi进行对比和计算，并将计算结果回传给Arduino，由Arduino控制ESC实现对推进器的推力分配和控制。

2.4 捷联惯性测量单元的静态模型

导航计算模块主要完成对IMU数据信息的读取初始校准、捷联结算以及导航结果的回传等。本系统采用Raspberry Pi进行对比和计算，并将计算结果回传给Arduino mega。

本文研究的IMU是有3个挠性加速度计和3个光纤陀螺仪组成的惯性测量单元。陀螺仪用来敏感IMU3个轴的角速度，加速度计用来敏感3个轴的比力，通过6路脉冲输出，给出导航初始参数。在常温下，IMU静态数学模型为：

(1)

(2)

(3)

(4)

(5)

(6)

3 PID控制算法及控制方案

按照控制理论中对PID控制算法的描述，项目所采用离散的PID算法数学表达式：

(7)

其中：Δt=T为采样周期，保证T足够的小，系统才有一定的精度；E(k)为第k次采样信号偏差值：E(k-1)为第k-1次采样信号偏差值：k为采样序号，k=0，1，2…；P(k)为第k次采样时PID调节器的输出[3-5]。

由式(7) 可知，要计算输出值P(k)，不仅需要知道偏差信号E(k)和E(k-1)，而且还要在积分项中把往次的偏差信号E(j)进行累加求和，这样不仅计算过程繁琐，而且保存E(j)要占用很多的内存。为此，做如下优化。

根据递推原理，第k-1次PID输出的表达式：

(8)

则得到：

(9)

由式(9)可知，要计算第k次的PID输出值P(k)，只需知道P(k-1)，E(k)，E(k-1)和E(k-2)即可，因此计算要简单许多。

在水下机器人控制系统中，由于执行机构是采用由电机驱动的推进器进行控制的，所以，只要输出一个增量信号即可。则，由式(9)得到：

(10)

式中，PK、DK表示第k次输出的增量值ΔP(k)，等于第k次与第k-1次调节器输出的偏差值，式(10)称作增量型PID控制计算式。用计算机实现增量式数字PID控制的原理方框图和程序框图，分别如图6、图7所示。

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

图7 OpenRov水池测试试验

4 试验测试

采用设计的数字PID算法分别对Eco-Dophin进行定深度和航向角进行控制试验，本文对Eco-Dophin的深度设定为5米进行定深航行试验。图8中为Depth Sensor测量得到的定深测试的试验结果曲线。

图8 定深测试的试验结果曲线

在完成Eco-Dophin的定深航行试验后，开始定航向航行试验。图9为定航向航行时的航向/时间曲线，设定航向为北向零度，通过IMU惯导测得的航向角如图中线所示。

图9 定航向航行试验航向角测试曲线

5 结论

本文针对低成本微惯性传感器的应用特性，采用Sparkfun、Arduino Mega和Raspberry Pi，设计了基于Raspberry Pi集中处理、Arduino控制板分布式控制的微小型捷联惯导系统，应用表明本系统满足Eco-dolphin的水下定航控制要求。同时，我们发现Raspberry Pi集中处理+Arduino底层控制办卡控制模式，在AUV长时序复杂轨迹运动控制时可靠性和实效性还存在处理速度滞后、数据容易溢出和报错的问题，如何提高该系统的应用可靠性，还需要进一步研究。

[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安装及动态标注误差在线辨识方法研究[J].中国空间科学技术. 2014, 42(1): 42-51.

[3] 曾占魁，曹喜滨，张世杰,等.航天器相对视觉/IMU导航量测修正多速率滤波[J].哈尔滨工业大学学报．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] 张伦东，练军想，胡小平.载体角运动对旋转式惯导系统旋转调制效果的影响[J]. 国防科技大学学报,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。

海洋系统公派留学项目(留金法[2014]5042号)。

杨 磊(1982-)，男，山东青岛人，博士，主要从事深海装备研发、机械设计、水下运载技术方向的研究。

1671-4598(2016)03-0133-03

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

TB69

A