一种用于仿生机器鱼的导航定位方法和系统技术方案

本发明专利技术涉及一种用于仿生机器鱼的导航定位方法和系统，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块；惯性导航模块得到第一初始导航信息，北斗定位导航模块得到所述第二初始导航信息；控制模块对第一初始导航信息进行解码，得到处理导航信息；并对第二初始导航信息进行解码，得到第二目标导航信息；惯性导航模块根据处理导航信息进行解算，得到第一目标导航信息；控制模块基于卡尔曼滤波方法，根据第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到最优导航信息。本发明专利技术能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，克服了单一导航的缺陷。导航的缺陷。导航的缺陷。

【技术实现步骤摘要】
一种用于仿生机器鱼的导航定位方法和系统


[0001]本专利技术涉及仿生机器鱼导航领域，尤其涉及一种用于仿生机器鱼的导航定位方法和系统。

技术介绍

[0002]在仿生机器鱼的控制系统中，导航定位和路径规划都基于精确的位置参数，所以导航定位是仿生机器鱼控制系统的关键技术之一。由于红外、声纳、电磁信号在水下会严重衰减，使得一些适用于地面和天空的导航定位方法不适用于仿生机器鱼的导航定位，所以仿生机器鱼的定位和导航已成为仿生机器鱼研究中的热点问题。
[0003]由于惯性导航(Inertial Navigation System，INS)在短时间内精度高的特点，故常用于水下定位中，INS的工作原理是使用安装在仿生机器鱼上的3个相互垂直的加速度计和3个相互垂直的陀螺仪，得到加速度计和陀螺仪的测量值，通过对测量值进行积分来获得仿生机器鱼的瞬时速度和位置。
[0004]然而，由于陀螺仪的温漂以及速度和位置是通过积分求出，故仿生机器鱼的速度和航向误差随时间呈线性累积，而位置误差随时间呈指数累积。同时，由于仿生机器鱼在水下信号差，扰动大，所以目前基于惯性导航的单一导航定位方法已经无法实现对仿生机器鱼实时导航定位，导航定位误差较大；另外，由于仿生机器鱼的体积较小，且工作环境在深度较小的水域或鱼塘，所以一旦仿生机器鱼定位误差很大，容易出现无法避障以及丢失等问题。

技术实现思路

[0005]本专利技术所要解决的技术问题是针对上述现有技术的不足，提供一种用于仿生机器鱼的导航定位方法和系统，能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，极其适用于环境多变、体积较小的仿生机器鱼的定位导航。
[0006]本专利技术解决上述技术问题的技术方案如下：
[0007]一种用于仿生机器鱼的导航定位系统，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块，所述控制模块分别与所述惯性导航模块和所述北斗定位导航模块电连接；
[0008]所述惯性导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；
[0009]所述北斗定位导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；
[0010]所述控制模块用于获取所述第一初始导航信息，对所述第一初始导航信息进行解码，得到处理导航信息；还用于获取所述第二初始导航信息，并对所述第二初始导航信息进行解码，得到第二目标导航信息；
[0011]所述惯性导航模块还用于根据所述处理导航信息进行解算，得到第一目标导航信息；
[0012]所述控制模块还用于基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息。
[0013]依据本专利技术的另一方面，还提供了一种用于仿生机器鱼的导航定位方法，采用本专利技术中的用于仿生机器鱼的导航定位系统进行登机导航，包括以下步骤：
[0014]利用惯性导航模块对仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；
[0015]利用北斗定位导航模块对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；
[0016]利用控制模块，对所述第一初始导航信息进行解码，得到处理导航信息；并对所述第二初始导航信息进行解码，得到第二目标导航信息；
[0017]利用所述惯性导航模块，根据所述处理导航信息进行解算，得到第一目标导航信息；
[0018]利用所述控制模块，基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息。
[0019]本专利技术的用于仿生机器鱼的导航定位方法和系统的有益效果是：通过惯性导航模块获取第一初始导航信息，并通过控制模块进行解码，便于后续再通过惯性导航模块根据解码得到的处理导航信息进行解算，得到卡尔曼滤波的其中一个输入变量；通过北斗定位导航模块获取第二初始导航信息，并通过控制模块进行解码，得到的第二目标导航信息作为卡尔曼滤波的另一个输入变量；基于卡尔曼滤波方法，依据两种定位导航模块得到的第一目标导航信息和第二目标导航信息来进行卡尔曼滤波，得到的最优估计的最优导航信息
[0020]本专利技术的用于仿生机器鱼的导航定位方法和系统，克服了单一导航的缺陷，避免了单一惯性导航模块中温漂、加速度计的误差以及速度和位移随时间的累计误差等各种误差，也避免了单一北斗定位导航模块因接收到的信号，质量较差而出现较大的导航误差；能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，极其适用于环境多变、体积较小的仿生机器鱼的定位导航。
附图说明
[0021]图1为本专利技术实施例一中一种用于仿生机器鱼的导航定位系统的结构示意图；
[0022]图2为本专利技术实施例一中导航定位系统各模块执行功能的流程示意图；
[0023]图3为本专利技术实施例二中一种用于仿生机器鱼的导航定位方法的流程示意图；
[0024]图4为本专利技术实施例二中导航定位方法的完整流程示意图。
具体实施方式
[0025]以下结合附图对本专利技术的原理和特征进行描述，所举实例只用于解释本专利技术，并非用于限定本专利技术的范围。
[0026]下面结合附图，对本专利技术进行说明。
[0027]实施例一、如图1所示，一种用于仿生机器鱼的导航定位系统，包括均设置在仿生
均为所述四元数导航信息中的元素，q0、q1、q2和q3均为实数且满足
[0042]按照第二公式，根据所述欧拉角矩阵得到所述载体坐标系与所述导航坐标系之间的加速度转移矩阵；
[0043]所述第二公式具体为：
[0044][0045]其中，为所述加速度转移矩阵，b表示所述载体坐标系，n表示所述导航坐标系；
[0046]根据所述加速度转移矩阵和第三公式，将所述加速度导航信息进行坐标转换，得到所述仿生机器鱼在所述导航坐标系下的目标加速度信息；
[0047]所述第三公式具体为：
[0048]其中，a
n
为所述目标加速度信息，a
b
为所述载体坐标系下的所述加速度导航信息；
[0049]对所述目标加速度信息进行积分运算，得到所述仿生机器鱼在所述导航坐标系下的所述速度矢量。
[0050]由于惯性导航模块得到的导航信息是载体坐标系相对于惯性坐标系的信息，因此通过控制模块的解码得到四元数导航信息，并分别依据第一公式和第二公式对四元数导航信息进行转换，以及依据第三公式对加速度导航信息进行转换，便于将惯性导航模块所得到的导航信息和北斗定位导航模块所得到的导航信息统一到同一个坐标系下，即导航坐标系，进而便于后续基于同一个坐标系下的导航信息来构建卡尔曼滤波器，得到最优的导航信息；依据速度和加速度之间的物理关系，通过控制模块的解码得到的加速度导航信息，便于后续得到依据该加速度导航信息进行积分运算，得到速度矢量本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种用于仿生机器鱼的导航定位系统，其特征在于，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块，所述控制模块分别与所述惯性导航模块和所述北斗定位导航模块电连接；所述惯性导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；所述北斗定位导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；所述控制模块用于获取所述第一初始导航信息，对所述第一初始导航信息进行解码，得到处理导航信息；还用于获取所述第二初始导航信息，并对所述第二初始导航信息进行解码，得到第二目标导航信息；所述惯性导航模块还用于根据所述处理导航信息进行解算，得到第一目标导航信息；所述控制模块还用于基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息。2.根据权利要求1所述的用于仿生机器鱼的导航定位系统，其特征在于，所述处理导航信息包括所述仿生机器鱼在载体坐标系下的四元数导航信息和加速度导航信息，所述第一目标导航信息具体为所述仿生机器鱼在导航坐标系下的速度矢量；则所述惯性导航模块具体用于：按照第一公式，将所述四元数导航信息转换成欧拉角矩阵；所述第一公式具体为：其中，α、β和γ为分别所述欧拉角矩阵中的偏航角、俯仰角和翻滚角，q0、q1、q2和q3均为所述四元数导航信息中的元素，q0、q1、q2和q3均为实数且满足按照第二公式，根据所述欧拉角矩阵得到所述载体坐标系与所述导航坐标系之间的加速度转移矩阵；所述第二公式具体为：其中，为所述加速度转移矩阵，b表示所述载体坐标系，n表示所述导航坐标系；根据所述加速度转移矩阵和第三公式，将所述加速度导航信息进行坐标转换，得到所述仿生机器鱼在所述导航坐标系下的目标加速度信息；所述第三公式具体为：其中，a
n
为所述目标加速度信息，a
b
为所述载体坐标系下的所述加速度导航信息；
对所述目标加速度信息进行积分运算，得到所述仿生机器鱼在所述导航坐标系下的所述速度矢量。3.根据权利要求2所述的用于仿生机器鱼的导航定位系统，其特征在于，所述第二目标导航信息具体为所述仿生机器鱼在所述导航坐标系下的位移矢量，则所述控制模块具体用于：根据所述速度矢量和所述位移矢量构建卡尔曼滤波器的状态向量，根据状态向量得到所述卡尔曼滤波器的状态方程和测量方程；任一时刻的状态向量的表达式具体为：X＝[X
1 X2]
T
；所述状态方程的表达式具体为：X
k
＝[X
1,k X
2,k
]
T
＝A[X
1,k
‑
1 X
2,k
‑1]
T
+ω
k
‑1；所述测量方程的表达式具体为：Z
k
＝[Z
1,k Z
2,k
]
T
＝H[X
1,k X
2,k
]
T
+v
k
‑1；其中，X为任一时刻的状态向量，X1和X2分别为任一时刻的所述位移矢量和所述速度矢量，X
k
为k时刻的状态向量，X
1,k
和X
2,k
分别为k时刻的所述位移矢量和所述速度矢量，[X
1,k
‑1X
2,k
‑1]
T
为k
‑
1时刻的状态向量，X
1,k
‑1和X
2,k
‑1分别为k
‑
1时刻的所述位移矢量和所述速度矢量，A为状态变换矩阵，ω
k
‑1为k
‑
1时刻的噪声误差矩阵；Z
k
为k时刻的导航信息观测向量，Z
1,k
和Z
2,k
分别为k时刻的位移观测值和速度观测值，H为观测变换矩阵，v
k
‑1为噪声测量误差矩阵；根据状态向量、所述状态方程和所述测量方程，得到所述卡尔曼滤波器的预测方程；所述预测方程的表达式具体为：其中，为k时刻对下一时刻的状态估计值，为k
‑
1时刻对k时刻的状态估计值，为k时刻状态估计值的协方差矩阵，P
k
‑1为k
‑
1时刻状态估计值经过校正后的协方差矩阵，Q
k
‑1为k
‑
1时刻的噪声误差矩阵ω
k
‑1的协方差矩阵，A
T
为状态变换矩阵A的转置矩阵；根据所述状态方程、所述测量方程和所述预测方程，得到所述卡尔曼滤波器的更新方程：所述更新方程的表达式具体为：其中，为k时刻经过校正后的状态估计值，具体为卡尔曼滤波处理之后的所述最优导航信息；K
k
为k时刻的卡尔曼增益，P
k
为k时刻状态估计值经过校正后的协方差矩阵，H
T
为观测变换矩阵H的转置矩阵，R
k
为k时刻的噪声测量误差矩阵v
k
‑1的协方差矩阵；根据预测方程和所述更新方程，构建所述卡尔曼滤波器；并根据所述卡尔曼滤波器，得到所述最优导航信息。
4.根据权利要求1至3任一项所述的用于仿生机器鱼的导航定位系统，其特征在于，所述惯性导航模块具体为MPU6050型号的导航模块，所述北斗定位导航模块具体为ATGM332型号的导航模块。5.根据权利要求1至3任一项所述的用于仿生机器鱼的导航定位系统，其特征在于，所述系统还包括均设置在所述仿生机器鱼内的电源模块和无线LORA模块，所述电源模块分别与所述控制模块、所述惯性导航模块、所述北...

【专利技术属性】
技术研发人员：文小玲，周勇，李自成，王佳，魏涛，杨颖，
申请(专利权)人：武汉工程大学，
类型：发明
国别省市：