一种用于仿生机器鱼的导航定位方法和系统.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113639744A(43)申请公布日2021.11.12(21)申请号202110767838.3(22)申请日2021.07.07(71)申请人武汉工程大学地址430000湖北省武汉市东湖新技术开发区光谷一路206号(72)发明人文小玲周勇李自成王佳魏涛杨颖(74)专利代理机构北京轻创知识产权代理有限公司11212代理人陈熙(51)Int.Cl.G01C21/16(2006.01)G01S19/47(2010.01)G01S19/39(2010.01)权利要求书5页说明书9页附图3页(54)发明名称一种用于仿生机器鱼的导航定位方法和系统(57)摘要本发明涉及一种用于仿生机器鱼的导航定位方法和系统，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块；惯性导航模块得到第一初始导航信息，北斗定位导航模块得到所述第二初始导航信息；控制模块对第一初始导航信息进行解码，得到处理导航信息；并对第二初始导航信息进行解码，得到第二目标导航信息；惯性导航模块根据处理导航信息进行解算，得到第一目标导航信息；控制模块基于卡尔曼滤波方法，根据第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到最优导航信息。本发明能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，克服了单一导航的缺陷。CN113639744ACN113639744A权利要求书1/5页1.一种用于仿生机器鱼的导航定位系统，其特征在于，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块，所述控制模块分别与所述惯性导航模块和所述北斗定位导航模块电连接；所述惯性导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；所述北斗定位导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；所述控制模块用于获取所述第一初始导航信息，对所述第一初始导航信息进行解码，得到处理导航信息；还用于获取所述第二初始导航信息，并对所述第二初始导航信息进行解码，得到第二目标导航信息；所述惯性导航模块还用于根据所述处理导航信息进行解算，得到第一目标导航信息；所述控制模块还用于基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息。2.根据权利要求1所述的用于仿生机器鱼的导航定位系统，其特征在于，所述处理导航信息包括所述仿生机器鱼在载体坐标系下的四元数导航信息和加速度导航信息，所述第一目标导航信息具体为所述仿生机器鱼在导航坐标系下的速度矢量；则所述惯性导航模块具体用于：按照第一公式，将所述四元数导航信息转换成欧拉角矩阵；所述第一公式具体为：其中，α、β和γ为分别所述欧拉角矩阵中的偏航角、俯仰角和翻滚角，q0、q1、q2和q3均为所述四元数导航信息中的元素，q0、q1、q2和q3均为实数且满足按照第二公式，根据所述欧拉角矩阵得到所述载体坐标系与所述导航坐标系之间的加速度转移矩阵；所述第二公式具体为：其中，为所述加速度转移矩阵，b表示所述载体坐标系，n表示所述导航坐标系；根据所述加速度转移矩阵和第三公式，将所述加速度导航信息进行坐标转换，得到所述仿生机器鱼在所述导航坐标系下的目标加速度信息；所述第三公式具体为：其中，an为所述目标加速度信息，ab为所述载体坐标系下的所述加速度导航信息；2CN113639744A权利要求书2/5页对所述目标加速度信息进行积分运算，得到所述仿生机器鱼在所述导航坐标系下的所述速度矢量。3.根据权利要求2所述的用于仿生机器鱼的导航定位系统，其特征在于，所述第二目标导航信息具体为所述仿生机器鱼在所述导航坐标系下的位移矢量，则所述控制模块具体用于：根据所述速度矢量和所述位移矢量构建卡尔曼滤波器的状态向量，根据状态向量得到所述卡尔曼滤波器的状态方程和测量方程；T任一时刻的状态向量的表达式具体为：X＝[X1X2]；所述状态方程的表达式具体为：TTXk＝[X1,kX2,k]＝A[X1,k‑1X2,k‑1]+ωk‑1；所述测量方程的表达式具体为：TTZk＝[Z1,kZ2,k]＝H[X1,kX2,k]+vk‑1；其中，X为任一时刻的状态向量，X1和X2分别为任一时刻的所述位移矢量和所述速度矢量，Xk为k时刻的状态向量，X1,k和X2,k分别为k时刻的所述位移矢量和所述速度矢量，[X1,k‑T1X2,k‑1]为k‑1时刻的状态向量，X1,k‑1和X2,k‑1分别为k‑1时刻的所述位移矢量和所述速度矢量，A为状态变换矩阵，ωk‑1为k‑1时刻的噪声误差矩阵；Zk为k时刻的导航信息观测向量，Z1,k和Z2,k分别为k时刻的位移观测值和速度观测值，H为观测变