基于四元数的机械海流计姿态解算

刘庆鹏，于 雨，惠 力，鲁成杰

(1.齐鲁工业大学(山东省科学院)，济南 250353；2.齐鲁工业大学(山东省科学院)，山东省科学院海洋仪器仪表研究所，青岛 266100)

0 引言

机械式海流仪经历了几代发展[1]，在其姿态角数据测量中，传感器测量的数据经算法处理后，变为实时的角度显示。而角度融合算法在工程上有几种不同的方法[2]，采用欧拉角微分方程式需要进行三角函数的运算，且方程会出现“奇点”，方程式退化；用方向余弦法计算姿态矩阵，没有方程退化问题，但需要求解6个微分方程，计算量较大，影响嵌入式系统的响应时间。文章采用四元数微分方程式，计算量不大且没有“奇点”问题，并在实验基础上对算法可行性进行了验证。

1 姿态解算原理

1.1 常用的坐标系

为了求解机械海流计的姿态角信息，引入北东地坐标系作为地理坐标系[3]。具体来说，OXg指向北方，OYg指向东方，OZg垂直指向地面。由机械海流计的纵轴OYb、横轴OXb和竖轴OZb所构成的右手系作为载体坐标系，将两个坐标系的原点定在机械海流计的质心，解算出地理坐标系和载体坐标系之间的角度关系即可求得机械海流计的姿态信息。

1.2 四元数解算原理

四元数是一种超复数，其形式可以表示为a+bi+cj+dk，有1个实部和3个虚部，其中a，b，c，d都是实数[4]。四元数可以用来描述机械海流计相对于地理坐标系的方向矢量，其中实部表示转角大小，虚部表示转动方向。通过四元数可以求出一个物体相对参考坐标系旋转之后的坐标信息，参考坐标系和旋转后的坐标系的转换关系用变换矩阵式(1)来解算。

(1)

其中，q0代表实部；q1、q2、q3代表虚部。

物体旋转后相对参考坐标系的角度变化信息也可以通过绕X轴旋转一个角度(横滚角)θ，绕Y轴旋转一个角度(俯仰角)γ和绕Z轴旋转一个角度(偏航角)φ来表示。3次旋转过程可由姿态角变换矩阵来表示，姿态变换矩阵如式(2)所示。

(2)

式(1)和式(2)均为物体从参考坐标系变换到载体坐标系，所以两个变换矩阵相等。由此可建立起四元数组成的矩阵与方向余弦矩阵之间的关系，可以通过反三角函数求取3个欧拉角[5]，公式如下：

横滚角：

(3)

俯仰角：

γ=-arctan(2(q1q3+q0q2))

(4)

偏航角：

(5)

只需要知道四元数的4个参数，就能解算出物体的姿态角。

海流计处于动态之中，故四元数是一个随时间变化的函数。建立四元数微分方程如下：

(6)

式(6)中，d0、d1、d2、d3表示更新后的四元数；ωx、ωy、ωz是由陀螺仪测量得到的机械海流计绕载体坐标系三轴旋转的角速率。实际计算中将四元数微分方程离散化，使用一阶龙格-库塔法得到更新后的四元数，则有：

(7)

式中，T表示姿态解算当前时刻，T+ΔT表示姿态解算下一时刻。从式(7)可看出，理论上只需要得到3个绕轴角速率便可以通过四元数法求出物体的欧拉角。而MEMS陀螺仪根据角动量守恒原理可以精确地测量角速率，但由于温度变化、摩擦力、不稳定力矩等因素，陀螺仪会产生漂移误差，通过积分会使误差变大。该误差可以通过加速度计进行矫正，当加速度计在没有加速度的状态下，加速度计的输出是一个幅度为1 G地球表面的法向量。通过该法向量，可以计算出罗盘相对于地球表面的俯仰角和横滚角，利用这两个角可以在机械海流计近乎匀速的情况下修正陀螺仪产生的累计误差。文章采用六轴的处理组件MPU6050(包括三轴加速度计和三轴陀螺仪)，通过四元数算法对HMC5883L得到的流向进行倾角误差补偿。

2 测量电路

测量电路采用InvenSence公司的MPU-6050六轴传感器芯片和Honeywell公司的HMC5883L磁阻传感器芯片[6]，将二者组合相连用以测量加速度数据A、陀螺仪数据G和地磁场数据M。姿态解算电路如图1所示。

图1 姿态解算电路

陀螺仪和加速度计都集成在MPU6050传感器芯片上，其测量范围达到±16 g。芯片正常工作时，MPU6050采用I2C接口与控制芯片连接，SDA和SCL信号线需要接入4.7 kΩ的上拉电阻以增加驱动能力。读取芯片数据首先要进行初始化，需要配置电源管理，陀螺仪采样频率、低通滤波频率、陀螺仪自检及测量范围和加速度计自检、测量范围及高通滤波频率。初始化后，陀螺仪采集科里奥利力在3个轴上形成的电压值，加速度计采集1 G的地球表面法向量在各轴的分量形成的电压值，经过16位A/D转换器转换为数字信号，利用预设的比例关系将其换算成实际的角速度值和角度值。

HMC5883L三轴磁场强度敏感芯片内置ASIC放大器，工作频率达160 Hz，罗盘航行精度精确至1°～2°，同样使用I2C总线输出。芯片的SCL和SDA分别接到MPU6050的AUX_CL和AUX_DA，同时接入4.7 kΩ的电阻来增加驱动能力。磁阻传感器主要用于测量地磁场矢量，定位海流计的方向(定位流向)。

3 具体姿态解算过程

3.1 三轴磁罗盘组件HMC5883L测流向

带有磁阻传感器的线路板封装在机械海流计旋桨壳体内，安装时将X轴指向旋桨方向，Y轴指向旋桨右侧，Z轴垂直于其他两轴指向海底。海流计尾翼随海流方向的变化而变化，旋桨相应调整方向并指向海流方向。磁阻传感器感应地磁场变化，采集各轴方向上的磁场强度。

磁场水平分量HN与磁场垂直分量Hz的矢量和表征地球磁场强度[7]。磁场水平分量HN总是指向地球磁场北极，利用该特性，若想测出流向，只需要测出旋桨方向(载体坐标系X轴和磁场水平分量之间的夹角)即可。磁阻传感器感应到X轴的磁场强度为Hx，Y轴的磁场强度为Hy，其矢量和为地磁场水平分量HN。地磁场水平分量HN与X轴之间的夹角即为方位角α，已知方位角即可知流向[8](图2)。

图2 磁阻传感器处于水平状态的坐标系

根据图2，由三角函数可求得方位角为[9]：

(8)

3.2 MPU6050修正流向

在实际应用中，磁阻传感器坐标系和海流计载体坐标系重合，假设载体坐标系和地理坐标系重合，则海流计测得的磁场强度HB为：

(9)

式(9)中，HN为地磁场水平分量，其方向与载体坐标系X轴重合，在Y轴上的分量为0；Hz为地磁场垂直分量，其方向与Z轴重合。

(10)

(11)

式(11)中，q0、q1、q2、q3是MPU6050所测数据更新后的四元数(标量)；Hbx、Hby和Hbz分别为当地地磁强度在3个轴上的磁场分量。通过式(3)和式(4)可得X轴与水平面之间的俯仰角γ，Z轴与过X轴的铅垂平面之间的横滚角θ，结合磁阻传感器测出的地磁场在三轴的方向分量，重新计算得到水平方向分量Hx2和Hy2计算其方位角。由坐标关系得水平方向磁分量：

(12)

将Hx2，Hy2带入式(8)计算即可得到方位角。

由于磁力线在地球表面分布的大小和方向存在差异，所以地球南北极和地磁南北极之间的磁偏角也是因地而异。要得到真方位角，只需在计算出的方位角基础上加上或减去当地的磁偏角。查出所在海域的磁偏角β，则真方位角ρ为：

ρ=α+β

(13)

3.3 软件实现

文章利用C语言编写软件程序，且采用调试更为方便的模块化设计，以便快速获取海流流向信息。软件功能主要包括初始化系统参数、磁场数据和姿态角数据的采集、方位角的解算、磁偏角矫正以及真北显示输出等，软件处理流程如图3所示。

图3 软件处理流程

4 调试与实验

为验证文章所采用的姿态解算算法的有效性，对以姿态算法为原理搭建的机械海流计进行测试。实验的主控制器选用STM32F103RCT6，姿态检测单元选用MPU6050和磁阻传感器HMC5883L，配合CAN总线收发模块及最小系统模块完成对姿态数据的采集、处理、通讯以及显示。

实验步骤如下：

1)在保持海流计水平状态下，使其做原地旋转运动，取定点测量，并进行数据的采集和解算；

2)对海流计进行人为扰动，在海流计不能保持水平状态的情况下，使其做原地旋转运动，取定点测量，并进行数据的采集和解算。

经多次测量，发现在定点处测量的数据在两种状态下得到的流向基本一致。说明使用该算法能够很好地处理海流计在不平衡状态下的倾角补偿问题，能够测出准确的流向数据。

5 结束语

文章利用磁阻传感器HMC5883L测出海流计的流向，分析了方向余弦与四元数之间一一对应的关系，并用MPU6050结合四元数算法解算出机械海流计的姿态角，用姿态角对海流计流向进行修正。文章结合实验验证了四元数法在姿态解算中的应用，通过设计海流计的姿态解算模块，给出了四元数具体的应用过程。为海流计在任意姿态下进行流向测量提供了保障。