基于卫星/INS的仿生机器鱼组合导航系统

周 勇，文小玲，胡 慧，罗心睿

（1.武汉工程大学 电气信息学院，武汉430205；2.武汉工程大学 艺术与设计学院，武汉430205；3.武汉纺织大学 电子与电气工程学院，武汉430073）

导航定位和路径规划是仿生机器鱼的关键技术。由于红外、声纳、电磁信号在水下会严重衰减，使得一些适用于地面和天空的导航定位方法不适用于仿生机器鱼的导航定位，因此仿生机器鱼的导航与定位已成为仿生机器鱼研究的热点问题[1]。惯性导航系统（inertial navigation system，INS）具有短时间内精度高的特点，可用于水下机器鱼的定位。INS 通过对安装在仿生机器鱼体内的3 个相互垂直的加速度计和3 个相互垂直的陀螺仪的测量值进行积分来获得仿生机器鱼的瞬时速度和位置[2]。但是，由于加速度计和陀螺仪的漂移，仿生机器鱼的速度和姿态角误差随时间呈线性累积、位置误差随时间呈平方累积[3]，因此需要将INS 和其他导航方法组合来消除INS 的误差，提高机器鱼的定位精度。常见的组合导航方法有INS 和全球定位系统、INS 和声学定位系统、INS 和地球物理导航系统、INS 和多普勒测速仪系统、INS 和及时定位与地图重建系统[4]。

考虑到仿生机器鱼的体积和应用场景，本文选择INS 和卫星组合的导航方法，并且通过卡尔曼滤波算法得到比较精准的导航信息。

1 导航信息测量方法

ATGM332 测量位置和速度方法主要是测量卫星和仿生机器鱼之间的距离，通过距离计算出机器鱼的位置信息，再通过位移的导数求得瞬时速度[5]。距离方程可表示为

式中：x1，y1，z1为卫星1 的位置信息；x0，y0，z0为机器鱼的位置信息；r1为卫星1 和机器鱼之间的距离，可以通过计算卫星和机器鱼内ATGM332 信号的时间差乘以光速得出；Δr 为时间误差引起的距离误差。由于x0，z0，Δr 均未知，故最少需要4 颗卫星才能得到机器鱼位置信息。

卫星定位原理如图1所示，当芯片接收到多个卫星信号时，每4 个卫星为一组求解方程，从中选出误差最小的一组作为位置信息。在得到位置信息之后，可通过对短时间内的位移求导得到速度信息。机器鱼内部的ATGM332 遵循NMEA 协议，因此可以通过NMEA 协议将位置和速度信息解码成地理坐标系下的经纬高和速度[6]。

图1 卫星定位原理图Fig.1 Satellite positioning schematic diagram

INS 通过直接使用安装在仿生机器鱼上的加速度计和陀螺仪得到加速度和角速度，通过角速度得到载体参考坐标系下的四元数，通过载体参考坐标系四元数反求出载体参考坐标系的欧拉角以及姿态矩阵，使用角速度更新姿态矩阵，根据加速度和姿态矩阵计算导航参考坐标系下的加速度，再对加速度和角速度积分，即可得到瞬时速度和瞬时位置[7]。

首先引入载体参考系姿态四元数：

已知四元数的情况下，将ZYX 欧拉角绕Z 轴、Y 轴、X 轴分别旋转α，β，γ 角度，将旋转角度转换成四元数得：

根据公式（3）求逆解得载体坐标系下的欧拉角为

在组合导航系统中，需要把载体坐标系中的加速度、速度、角速度转换成导航坐标系中的加速度。

导航坐标系常选用地理坐标系，则根据ZYX 旋转坐标变换得到变换矩阵为[8]

式中：s 表示sin；c 表示cos。则导航坐标系下的加速度an为

式中：ab为载体坐标系下的加速度；为载体的姿态矩阵。

2 组合导航和数据融合

本文采取的定位方法为卫星和INS 组合导航定位方法。组合导航的总体结构如图2所示，图中IMU 测量得到的位置是相对ATGM332 测量的初始位置，将ATGM332 和MPU6050 测量的速度和位置输入到数据滤波器中，将滤波后的数据输出作为机器鱼的实际导航信息，可以实现卫星和INS 组合定位的功能[9]。

图2 组合导航原理图Fig.2 Combined navigation schematic

由于仿生机器鱼在水下情况多变，且控制芯片的数据存储量较小，符合卡尔曼滤波器在处理动态数据时的高效性以及求解时不需要存储上一时刻数据的特点，故图2中数据滤波器采用卡尔曼滤波器。通过卡尔曼滤波算法设计位置和速度融合算法，仿生机器鱼在水下可分为水平匀速和上浮下潜运动，则ATGM332 测量的位移为X1=［xx，xy，xz］，IMU测量的加速度积分为X2=［Δxx，Δxy，Δxz］，即系统状态量可变成：

因此卡尔曼滤波器的状态方程和测量方程为

式中：Xk表示k 时刻的状态；Zk表示k 时刻的测量状态；A 为状态变化矩阵。根据速度和位移的关系，得到，I3表示三维单位矩阵；Δt 表示采样时间，根据仿生机器鱼的运行流程可设定采样时间为1 s，H 为观测变换矩阵，根据测量值和实际值的关系，得到H=I6，I6表示六维单位矩阵。p（w），p（v）为估计误差和测量误差，都是零均值白噪声，服从正态分布p（w）～N（0，Q），p（v）～N（0，R）。

卡尔曼滤波算法如下[10]：

3 仿真与水下实验

3.1 算法仿真

仿生机器鱼的游动可分为水平游动和上浮下潜游动。当机器鱼在水下水平匀速游动时，有白噪声误差，设定初始状态为［0 0 0.2 0.2］，由于估计误差相对于测量误差较小，因此设定测量误差ν=［2.5 2.5 0.1 0.1 ］T，估计误差ω=0.01 ［1 1 1 1 ］T，则Q=0.0001·I4，R=］，P0=I4，将上述参数输入到卡尔曼滤波器，取前500 个点，仿真结果如图3所示。

图3 xy 轴数据融合仿真图Fig.3 XY axis data fusion simulation diagram

当机器鱼上浮下潜游动时，有白噪声误差，设定初始状态为［0 0.05］，由于估计误差相对于测量误差较小，故设定测量误差ν= ［2.5 0.1 ］T，估计误差ω=0.01 ［1 1］T，则Q=0.01·0.01·I2，R=P0=I2，将上述参数输入到卡尔曼滤波器，取前500 个点，仿真结果如图4所示。

图4 z 轴数据融合仿真图Fig.4 Z axis data fusion simulation diagram

从图3和图4可得出，在测量值误差较大的情况下，滤波后的速度和位移也能接近真实值，说明在ATGM332 信号较弱或者INS 累计误差较大的情况下，依靠卡尔曼滤波算法也能得到较为精确的值。

3.2 水下实验

本文使用的ATGM332 采用GPS 和北斗双定位系统，外接一根有源天线。ATGM332 的性能指标如表1所示。ATGM332 和单片机STM32 相连，通过单片机使用NMEA 协议解码ATGM332 接收的信息，得到ATGM332 所在的位置、速度等导航信息。

表1 ATGM332 性能指标Tab.1 ATGM332 performance indicator

IMU 使用的是MPU6050 模块，该模块能够同时测量芯片的三轴加速度、角速度等运动数据。单片机可以获得由芯片内部DMP 模块数据融合滤波之后的值，根据滤波之后的值可以计算出姿态角、导航坐标系下的加速度以及姿态转移矩阵。MPU6050的性能指标如表2所示。

表2 MPU6050 性能指标Tab.2 MPU6050 performance indicator

仿生机器鱼的控制模式可分为手动控制模式和自动巡航避障模式，在实验中使用手动控制模式，内部搭载无线LORA 模块，用于接收控制信号以及向上位机传输相关的信息，通过控制舵机的转动以及偏角可以实现前进和转向，机器鱼的直行速度最大为0.2 m/s，机器鱼底部搭载大容量锂电池，工作时间最长可达30 min。ATGM332，MPU6050 及仿生机器鱼的实物如图5所示。

图5 实物图Fig.5 Picture of real products

实验地点选择武汉工程大学静心湖水域，控制机器鱼在水下游动，将机器鱼的导航信息通过LORA 模块发送到上位机保存，进行仿真测试和水中定位实验，仿真图如图6所示。从仿真图可以看出，加速度积分的累计误差越来越大，通过组合导航和卡尔曼滤波器能够得到较为精准的导航信息。

图6 水下实验仿真图Fig.6 Underwater experiment simulation diagram

4 结语

为了解决仿生机器鱼水下定位困难的问题，本文使用ATGM332 和INS 组合导航的方法，并将测量的速度和位移数据进行卡尔曼滤波，以提高测量精度。仿真实验以及水下实验的结果表明，在因ATGM332 信号弱或INS 累计误差较大而导致定位精度不准时，可以通过卡尔曼滤波算法，得到较为精准的值，提高了机器鱼的定位精度。本方法虽然存在一些问题，但通过改进滤波器算法，可以实现更高精度的定位功能。