用于水下ROV控制的姿态融合技术研究

裴国栋，温亚楠，朱莉莉

（青岛市光电工程技术研究院，山东青岛 266019）

0 引言

利用微机械结构来测量载体相对于空间运动数据的传感器成为惯性传感器[1]，通过空间距离的变化将运动数据转化为模拟电子数据，然后通过相关算法计算后最终得到导航数据。惯性导航近年来被广泛应用于航海、航空、各种平台的稳定与控制以及无人机的的姿态以及导航控制、军工产业等各个领域。由于该技术应用广泛，所以对国民经济的发展有着突出的贡献。

惯性导航主要是通过惯性传感器来实现的,现如今惯性传感器主要包括加速度计与陀螺仪，加速度计可以测量沿某个轴向的加速度值，陀螺仪可以测量沿某个轴向的角速度值，因为这两种传感器的工作原理的不同，所以它们数据的应用方式也不同[2]。由于陀螺仪数据得到姿态角需要做积分运算，所以陀螺仪数据中的误差会随时间的增加而逐渐增大；加速度计由于内部微机械结构的限制较适合静态的姿态检测，动态时数据不稳定。因此需要一种合适的算法将两种数据融合在一起，使融合后的数据更接近真实姿态数据[3]。

1 系统硬件设计

根据系统需求以及各方面综合考量本系统选用 ST的增强型处理器 STM32f103C8T6芯片作为主处理器芯片，负责从惯性传感器采集、处理、合成数据，芯片内置 64KB 的闪存存储器，最高时钟频率可72 MHz。惯性传感器选择MPU6050，该传感器是 InvenSense 公司生产的全球首列整合性六轴运动处理组件，整合了三轴陀螺仪、三轴加速度计，内置的数字运动处理(DMP: Digital Motion Processor)。使用非常方便[4]。其使用原理图如图1所示。

图1 MPU6050原理图

2 数据融合与姿态解算

2.1 四元数与欧拉角

四元数的组成是实数加上三个复数元素i、j、k，各个复数之间的关系如下：

每个四元数都是1、i、j、k的线性组合，所以四元数可表示如下：

欧拉角由章动角θ、进动角ψ和自转角φ组成，是用来表示定点转动的物体位置的三个参数。欧拉角最先由L.欧拉首先提出，一个绕点转动的物体欧拉角有多种取法，以下是其中常用的一种表示法[5]。

图2

图2所示，为欧拉角按照z-y-x旋转次序的一种表示法，ψ、θ、φ分别为物体绕 z轴、y轴以及x轴的角度，若用Tait-bryan angle来表示为：Yaw（偏航角），Roll（横滚角），Pitch（俯仰角）。

2.2 数据的采集处理

采集的数据在进行 Kalman滤波时，需要采用陀螺仪以及加速度计的输出数据来构造合适的状态方程以及测量方程。Kalman是一种递归的估计滤波算法，只需要记录上一时刻状态的估计值以及解算出当前状态的观测值就可以解算出当前状态的估计值。公式（3）是卡尔曼滤波器的一个核心公式，计算得到的K(n)是估计误差占总误差的权重，H(n)为观测矢量[6]。

2.3 姿态融合算法

第一步、首先对加速度计数值进行归一化：(注：Aac_x、Aac_y、Aac_z分别为 X、y、z轴加速度计数值;)

第二步、根据四元数计算法计算出重力在当前三个轴上的加速度分量：

第三步、根据向量积计算测得的重力与计算的重力间的误差：

第四步、对误差进行积分计算：

第五步、将积累的误差经过PI算法后补偿到陀螺仪，即补偿零点漂移，由于z轴没有观测者进行矫正会产生持续漂移[7][8]：

第六步、采用一介毕卡解法解算四元数，并保存历史值待用（q0temp为q0历史值，其他类似）：

第七部、单位化四元数，使其在空间旋转时不会被拉伸变换：

第八步、四元数转换到欧拉角完成数据融合：

3 结论

融合后的角度曲线上传至上位机显示如图 3，其中包含9个数据，分别是三轴角速度值，三轴角加速度值以及融合后的三轴角度，其中曲线1为X轴角度曲线，曲线2为X轴陀螺仪曲线，由于加速度计数据太小图中无法展示出来，在周期性摇晃X轴的过程中，角度曲线紧跟灵敏度最高的角速度曲线，合成后的角度曲线无过冲现象，能紧跟平台的角度变化。根据角度曲线所示水下姿态检测系统已基本实现。

图3 数据融合角度曲线

本课题所研制的水下 ROV姿态检测系统，系统简单实用，可以达到水下 ROV姿态控制对相关数据的要求。

参考文献：

[1]陈哲. 捷联惯导系统原理[M]. 北京: 宇航出版社,1996: 20-43.

[2]陈永冰, 钟斌. 惯性导航原理[M]. 北京: 国防工业出版社, 1990: 14-21.

[3]于波, 陈云相, 郭秀中. 惯性技术[M]. 北京: 北京航空航天大学出版社, 1994: 33-52

[4]杨艳娟. 捷联惯导系统关键技术研究[D]. 哈尔滨:哈尔滨工程大学, 2001: 19-54.

[5]卢大伟. 多传感器数据融合方法在新型组合导航系统中的应用[D]. 南京: 东南大学, 2001.

[6]曾庆华, 张为华. IMEMS 速率陀螺芯片在MAV飞行控制系统中应用研究[J]. 测控技术, 2004, 23(2).

[7]张静, 金志华. 空中平台航姿参考系统的设计[J].中国惯性技术学报, 2004, 12(2): 47-52.

[8]秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安: 西北工业大学出版社, 1999, 7(3):17-21.