MC-ROV导航系统研究

吴长进，刘慧婷

(1.扬子江船业集团公司，江苏 靖江 214500; 2.江苏科技大学 电子信息学院，江苏 镇江 212003)

MC-ROV导航系统研究

吴长进1，刘慧婷2

(1.扬子江船业集团公司，江苏 靖江 214500; 2.江苏科技大学 电子信息学院，江苏 镇江 212003)

针对研制的新型模态切换水下机器人(Mode-Converted ROV, MC-ROV)，设计了一套以MEMS器件为主的微惯性导航系统，包括三轴陀螺仪、三轴加速度计、三轴磁力计及高精度深度传感器；基于加速度计和陀螺仪的频域互补原理，系统首先运用互补滤波算法融合传感器数据，补偿陀螺漂移，提高系统动态性能；然后分析了四元数微分方程，以四元数估算水下机器人的角度大小和方向，可显著减小计算量；最后采用了改进的渐消记忆指数加权自适用卡尔曼滤波器，增大新近数据的作用，相应减小陈旧数据的作用，有效避免滤波发散，提高导航精度；水池实验表明，在持续震动和电磁干扰等恶劣环境下，利用互补滤波和改进的自适应卡尔曼滤波，水下机器人依然能够获得准确的、高可靠性的水下姿态信息。

水下机器人；微惯性导航；四元数；自适应卡尔曼

0 引言

开发海洋资源、进行渔业监测、对水下机构定期检测等等都对当今水下作业工具的功能和性能提出了更高的要求。由于人工作业危险性大、成本高，因此开发灵活、可靠的水下工具成为当今研究的一个热点。

有缆遥控水下机器人(remotely operated vehicle，ROV)是一种通过脐带电缆与水面支持平台联系的水下作业机器人，是海洋工程水下结构安全检测及维护的关键装备[1]。文中所述水下机器人，是模态切换水下机器人(mode-converted ROV, MC-ROV)，可在浮游和爬壁之间切换，是应用于科考船的特种水下机器人。

精确导航在水下机器人系统中占据重要地位，是其正常工作的前提和保证。现有的水下导航技术中，单一的方法都有各自固有的缺陷，而利用两种或以上技术组合成的组合导航可以互相弥补缺点，提高精度，成为当今导航首选，备受青睐，是未来导航技术的重要发展方向[2-4]。在组合导航中，利用各种低精度的微机电系统(MEMS)器件，可设计精巧的、高精度和动态性能的导航系统，因此被众多研究人员重视[5-6]。

系统基于MEMS传感器件，利用互补滤波和改进的自适应卡尔曼滤波对MC-ROV进行导航姿态更新和估算，降低轴间干扰，抑制姿态角漂移，获得良好的导航效果。

1 导航系统结构及原理

通常来讲，惯性导航是指测量运动载体的加速度和角速度，通过积分等方法推算出运动物体的导航信息，包括位置和角度。运动物体的坐标系称为载体坐标系，其参照的坐标系为参考坐标系。

对 MC-ROV，载体坐标系位于其几何中心，参考坐标系为常用的东北天坐标系，两坐标系遵循右手螺旋定则。通过测量载体坐标系的加速度和角速度，积分可获得位置和角度，再将其变换到参考坐标系，即可得到常规意义上的位置和角度。

MC-ROV导航系统由磁力计、陀螺仪、加速度计、深度传感器、导航微处理器组成。陀螺仪和加速度计作为惯性传感器，采集水下机器人的实时角速度和加速度，以估算位置和角度；磁力计测量磁场强度，对水下机器人的方位角进行矫正；深度计通过压力测算潜水深度。

MC-ROV导航系统主要用于实时估算姿态角和方位角，以实现各种复杂控制。各个传感器的重要参数，即灵敏度和量程等信息如表1所示。

表1 导航器件

2 MC-ROV微惯性导航算法分析

系统首先运用互补滤波降低陀螺漂移，再此基础上，采用渐消记忆指数加权的自适应卡尔曼滤波算法对MC-ROV导航姿态实时解算。导航算法如图1所示。

图1 导航算法框图

2.1 互补滤波

陀螺仪具有良好的动态响应特性，但是随着时间的增长累积误差越来越大；磁力计和加速度传感器虽然没有累积误差，但动态响应能力不足。在频域上它们互补，因此，可以采用互补滤波器融合传感器的数据，提高测量精度的同时也提高系统的动态性能[7-8]。

以R表示MC-ROV的真实姿态的方向余弦矩阵，R0代表由加速度传感器观测到的姿态方向余弦矩阵，uH表示高频观测噪声，则R0=R+uH。Rc代表由陀螺仪计算得到的姿态，uL表示Rc中的低频累积误差，则有Rc=R+uL。

GL(s)uH(s)+GH(s)uL(s)≈R(s)

(1)

由上式可知，信号中一部分的低频和高频干扰可同时被互补滤波滤掉。设Ω为陀螺仪测量值构成的反对称矩阵，则有:

(2)

2.2 四元数微分方程

设ω=[ωxωyωz]Τ，表示经过归一化的水下机器人的三轴角速度。令Q=[q0q1q2q3]Τ，四元数q0,q1,q2和q3与角加速度有如下关系[9-10]:

(3)

其矩阵形式为:

(4)

由此可知四元数更新方程为:

(5)

2.3 渐消记忆指数加权的自适应卡尔曼滤波

最初的卡尔曼滤波是一种线性最小方差估计方法，可对目标状态进行有效估计。而在实际环境中，系统的噪声统计特性是时变的、非线性的，大大的降低了滤波的精度，甚至可能引起滤波发散。

为了解决这种缺陷，应加强新近数据的比重，减弱陈旧数据的比重，这可以通过渐消记忆指数加权方法来实现。传统的衰减记忆卡尔曼滤波就是基于此来防止滤波发散的一种滤波方法[11]。

与常规卡尔曼滤波类似，渐消记忆指数加权自适应卡尔曼滤波过程如式(6)～(10)所示：

(6)

(7)

(8)

Pk/k=(I-KkHk)Pk/k-1

(9)

Xk/k=(I-KkHk)Φk,k-1+KkZk

(10)

将上式中的(6)和(9)相结合得到：

在料液浓度、料液体积流量、浓缩倍数等相同的条件下，研究了超滤时间对膜通量及压力的影响，结果如图5所示。

Xk/k=(I-KkHk)Φk,k-1Xk-1/k-1+KkZk

(11)

从上式可以看出，λ的引入使得滤波增益Kk增大，即加强了新近测量值的权重。而对于Xk-1/k-1，其系数减小，即以前时刻的状态对当前时刻的状态影响减小。为了体现新近数据对滤波的作用，文章设计了一个自适应计算衰减因子λk对传统算法进行改进，即用一个指数型系数替换传统衰减记忆卡尔曼的常值系数，用公式中的每项乘以不同的加权系数。

,...

(12)

式中的衰减因子λk为指数型函数，表示新近测量值权重逐渐增大，体现了自适应性。将λk代入式(6)～(10)得到改进后的滤波方程组。其中式(7)变形为:

(13)

3 MC-ROV水池实验和仿真

MC-ROV水下机器人长宽高为80cm、50cm和50cm，重约90kg，属于开架式小型水下机器人。为验证本系统算法的有效性，在水池分别进行了静止、定航和旋转实验。

将水下机器人静置于水平地面，运用互补滤波、四元数更新算法及自适应卡尔曼滤波计算其姿态角，结果如图2。

图2 MC-ROV静置时的姿态角

由图2可知，MC-ROV在静态时，其航向角(yaw)、俯仰角(pitch)、横滚角(roll)误差绝对值基本小于0.5度，且没有发生漂移，表现出良好的稳定性和精度。

对水下机器人进行定航实验，分别应用了无味卡尔曼和改进的自适应卡尔曼，结果如图3所示。从图中我们可以看出，MC-ROV在18s时都达到了规定的角度，但自适应卡尔曼的收敛速度要明显快于标准尔曼。

图3 MC-ROV定航

如图4所示，为水下机器人在水中的小幅度旋转运动示意图，控制其先正转再反转，得到上述姿态角变化曲线。图(a)中，俯仰角(pitch)、横滚角(roll)发生漂移，这是由机械安装误差和算法固有特性造成的。通过添加自适应卡尔曼滤波，使得MC-ROV不再沿着一个方向持续漂移，并使俯仰角(pitch)、横滚角(roll)误差绝对值基本小于0.2°。

图4 MC-ROV旋转

从上述实验可知，利用互补滤波和改进的渐消记忆指数加权自适应卡尔曼滤波能够提高动态响应速度、改善姿态漂移特性。对比图4中两图可知，与标准卡尔曼相比，改进算法后，俯仰角误差降低5倍，并且抑制了角度漂移；横滚角的误差降低近一倍，波动更小。当然，采用渐消记忆指数加权自适应卡尔曼滤波后，算法对干扰更加敏感，表现出微小的波动，后续研究可以考虑改善权值或进行硬件滤波等方法处理。

4 结论

基于MEMS的导航系统应用越来越广，已成为水下导航领域的重要研究方向。MEMS微惯性组合导航，具有体积小、易实现、干扰低等优点，是一种极佳的水下导航方法。本文结合研制的MC-ROV，在微惯性导航基础上，采用互补滤波、四元数法、渐消记忆指数加权的自适应卡尔曼等算法得到了较高精度的姿态信息，解决了姿态漂移等问题，具有良好的稳定性和实时性。

[1] 封锡盛. 从有缆遥控水下机器人到自治水下机器人[J]. 中国工程科学, 2000, 2(12):29-33.

[2]WendelJ,MeisterO,SchlaileC,etal.AnintegratedGPS/MEMS-IMUnavigationsystemforanautonomoushelicopter[J].AerospaceScience&Technology, 2006, 10(6):527-533.

[3] 孙玉山, 代天娇, 赵志平. 水下机器人航位推算导航系统及误差分析[J]. 船舶工程, 2010, 32 (5): 67-72.

[4] 唐国元, 徐正武, 黄道敏,等. 基于姿态控制力矩陀螺的水下航行体运动建模方法研究[J]. 中国造船, 2014(2):18-27.

[5] 丁 君. 基于微惯性传感器的姿态算法研究[D]. 上海：上海交通大学, 2013.

[6] 杨 磊, 庞 硕, 杨耀民,等. 基于9DOFIMU的AUV惯性导航技术研究[J]. 计算机测量与控制, 2016, 24(3):133-135.

[7] 梁延德, 程 敏, 何福本,等. 基于互补滤波器的四旋翼飞行器姿态解算[J]. 传感器与微系统, 2011, 30(11):56-58.

[8] 张 明. 水下结构检测ROV的研制及导航定位研究[D]. 江苏科技大学, 2015.

[9] 刘建业，曾庆化，赵 伟,等. 导航系统理论与应用[M]. 西北工业大学出版社, 2010,20-23.

[10] 张 明, 刘慧婷, 眭 翔,等. 模态切换水下机器人微惯性导航分析[J]. 计算机辅助工程, 2015, 24(3):88-94.

[11] 张朝飞, 罗建军, 龚柏春,等. 一种复杂多介质环境下的视觉/惯性自适应组合导航方法[J]. 中国惯性技术学报, 2016, 24(2):190-195.

Research of MC-ROV Navigation System

Wu Changjin1，Liu Huiting2

(1.Yangzijiang Shipbuilding (Holidings) Co.Ltd, Jingjiang 214500, China;
2.School of Electronic and Information, Jiangsu University of Science and Technology, Zhenjiang 212003, China)

For the novelty Mode-Converted ROV(MC-ROV), a set of MEMS-based integrated inertial navigation system has been developed, including 3-axis gyroscope, 3-axis accelerometer, 3-axis magnetic compass and high-precision depth sensor. In terms of the compensation infrequency domain between accelerometer and gyroscope, the system firstly utilizes complementary filter to merge the sensors in order to restrain the drifts of gyroscope and improve the dynamic performence. Then quaternions‘ differential equation is analised and further to be the estimated values of MC-ROV’s position and angles, which apparentely reduce the computing. This paper applies an improved fading memory index-weighted adaptive Kalman filter that stresses the effects of new data while weakening that of old data to avoid divergence and further enhance the navigation precision. The pool experimental results present that using complementary filter and the improved Kalman filter can aquire underwater angles with high precision and reliability, though in harsh ambience such as continuous vibration and electromagnet interference.

underwater vehicle; micro-inertial navigation; quaternion; AKF

2016-11-01；

2016-12-01。

吴长进(1990-)，男，江苏靖江人，大学，主要从事船舶设计与导航方向的研究。

1671-4598(2017)04-0159-03

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

TP242

A