CN113639744B - 一种用于仿生机器鱼的导航定位方法和*** - Google Patents
一种用于仿生机器鱼的导航定位方法和*** Download PDFInfo
- Publication number
- CN113639744B CN113639744B CN202110767838.3A CN202110767838A CN113639744B CN 113639744 B CN113639744 B CN 113639744B CN 202110767838 A CN202110767838 A CN 202110767838A CN 113639744 B CN113639744 B CN 113639744B
- Authority
- CN
- China
- Prior art keywords
- navigation
- navigation information
- vector
- state
- module
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 241000251468 Actinopterygii Species 0.000 title claims abstract description 116
- 239000011664 nicotinic acid Substances 0.000 title claims abstract description 90
- 238000000034 method Methods 0.000 title claims abstract description 47
- 238000001914 filtration Methods 0.000 claims abstract description 39
- 238000012545 processing Methods 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims description 89
- 230000001133 acceleration Effects 0.000 claims description 45
- 238000006073 displacement reaction Methods 0.000 claims description 44
- 230000003592 biomimetic effect Effects 0.000 claims description 25
- 238000005259 measurement Methods 0.000 claims description 24
- 230000009466 transformation Effects 0.000 claims description 24
- QVFWZNCVPCJQOP-UHFFFAOYSA-N chloralodol Chemical compound CC(O)(C)CC(C)OC(O)C(Cl)(Cl)Cl QVFWZNCVPCJQOP-UHFFFAOYSA-N 0.000 claims description 21
- 238000012546 transfer Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 5
- 230000007547 defect Effects 0.000 abstract description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000002238 attenuated effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000009182 swimming Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种用于仿生机器鱼的导航定位方法和***，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块；惯性导航模块得到第一初始导航信息，北斗定位导航模块得到所述第二初始导航信息；控制模块对第一初始导航信息进行解码，得到处理导航信息；并对第二初始导航信息进行解码，得到第二目标导航信息；惯性导航模块根据处理导航信息进行解算，得到第一目标导航信息；控制模块基于卡尔曼滤波方法，根据第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到最优导航信息。本发明能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，克服了单一导航的缺陷。
Description
技术领域
本发明涉及仿生机器鱼导航领域，尤其涉及一种用于仿生机器鱼的导航定位方法和***。
背景技术
在仿生机器鱼的控制***中，导航定位和路径规划都基于精确的位置参数，所以导航定位是仿生机器鱼控制***的关键技术之一。由于红外、声纳、电磁信号在水下会严重衰减，使得一些适用于地面和天空的导航定位方法不适用于仿生机器鱼的导航定位，所以仿生机器鱼的定位和导航已成为仿生机器鱼研究中的热点问题。
由于惯性导航(Inertial Navigation System，INS)在短时间内精度高的特点，故常用于水下定位中，INS的工作原理是使用安装在仿生机器鱼上的3个相互垂直的加速度计和3个相互垂直的陀螺仪，得到加速度计和陀螺仪的测量值，通过对测量值进行积分来获得仿生机器鱼的瞬时速度和位置。
然而，由于陀螺仪的温漂以及速度和位置是通过积分求出，故仿生机器鱼的速度和航向误差随时间呈线性累积，而位置误差随时间呈指数累积。同时，由于仿生机器鱼在水下信号差，扰动大，所以目前基于惯性导航的单一导航定位方法已经无法实现对仿生机器鱼实时导航定位，导航定位误差较大；另外，由于仿生机器鱼的体积较小，且工作环境在深度较小的水域或鱼塘，所以一旦仿生机器鱼定位误差很大，容易出现无法避障以及丢失等问题。
发明内容
本发明所要解决的技术问题是针对上述现有技术的不足，提供一种用于仿生机器鱼的导航定位方法和***，能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，极其适用于环境多变、体积较小的仿生机器鱼的定位导航。
本发明解决上述技术问题的技术方案如下：
一种用于仿生机器鱼的导航定位***，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块，所述控制模块分别与所述惯性导航模块和所述北斗定位导航模块电连接；
所述惯性导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；
所述北斗定位导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；
所述控制模块用于获取所述第一初始导航信息，对所述第一初始导航信息进行解码，得到处理导航信息；还用于获取所述第二初始导航信息，并对所述第二初始导航信息进行解码，得到第二目标导航信息；
所述惯性导航模块还用于根据所述处理导航信息进行解算，得到第一目标导航信息；
所述控制模块还用于基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息。
依据本发明的另一方面，还提供了一种用于仿生机器鱼的导航定位方法，采用本发明中的用于仿生机器鱼的导航定位***进行登机导航，包括以下步骤：
利用惯性导航模块对仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；
利用北斗定位导航模块对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；
利用控制模块，对所述第一初始导航信息进行解码，得到处理导航信息；并对所述第二初始导航信息进行解码，得到第二目标导航信息；
利用所述惯性导航模块，根据所述处理导航信息进行解算，得到第一目标导航信息；
利用所述控制模块，基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息。
本发明的用于仿生机器鱼的导航定位方法和***的有益效果是：通过惯性导航模块获取第一初始导航信息，并通过控制模块进行解码，便于后续再通过惯性导航模块根据解码得到的处理导航信息进行解算，得到卡尔曼滤波的其中一个输入变量；通过北斗定位导航模块获取第二初始导航信息，并通过控制模块进行解码，得到的第二目标导航信息作为卡尔曼滤波的另一个输入变量；基于卡尔曼滤波方法，依据两种定位导航模块得到的第一目标导航信息和第二目标导航信息来进行卡尔曼滤波，得到的最优估计的最优导航信息
本发明的用于仿生机器鱼的导航定位方法和***，克服了单一导航的缺陷，避免了单一惯性导航模块中温漂、加速度计的误差以及速度和位移随时间的累计误差等各种误差，也避免了单一北斗定位导航模块因接收到的信号，质量较差而出现较大的导航误差；能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，极其适用于环境多变、体积较小的仿生机器鱼的定位导航。
附图说明
图1为本发明实施例一中一种用于仿生机器鱼的导航定位***的结构示意图；
图2为本发明实施例一中导航定位***各模块执行功能的流程示意图；
图3为本发明实施例二中一种用于仿生机器鱼的导航定位方法的流程示意图；
图4为本发明实施例二中导航定位方法的完整流程示意图。
具体实施方式
以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
下面结合附图，对本发明进行说明。
实施例一、如图1所示，一种用于仿生机器鱼的导航定位***，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块，所述控制模块分别与所述惯性导航模块和所述北斗定位导航模块电连接；
所述惯性导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；
所述北斗定位导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；
所述控制模块用于获取所述第一初始导航信息，对所述第一初始导航信息进行解码，得到处理导航信息；还用于获取所述第二初始导航信息，并对所述第二初始导航信息进行解码，得到第二目标导航信息；
所述惯性导航模块还用于根据所述处理导航信息进行解算，得到第一目标导航信息；
所述控制模块还用于基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息。
通过惯性导航模块获取第一初始导航信息，并通过控制模块进行解码，便于后续再通过惯性导航模块根据解码得到的处理导航信息进行解算，得到卡尔曼滤波的其中一个输入变量；通过北斗定位导航模块获取第二初始导航信息，并通过控制模块进行解码，得到的第二目标导航信息作为卡尔曼滤波的另一个输入变量；基于卡尔曼滤波方法，依据两种定位导航模块得到的第一目标导航信息和第二目标导航信息来进行卡尔曼滤波，得到的最优估计的最优导航信息
本实施例的用于仿生机器鱼的导航定位***，克服了单一导航的缺陷，避免了单一惯性导航模块中温漂、加速度计的误差以及速度和位移随时间的累计误差等各种误差，也避免了单一北斗定位导航模块因接收到的信号，质量较差而出现较大的导航误差；能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，极其适用于环境多变、体积较小的仿生机器鱼的定位导航。
具体地，本实施例中所述惯性导航模块具体为MPU6050型号的导航模块，所述北斗定位导航模块具体为ATGM332型号的导航模块。其中，第二初始导航信息通过ATGM332导航模块中的信号处理单元和载波跟踪单元依次对卫星导航信息进行处理而得到。
具体地，本实施例中所述控制模块具体为STM32系列的单片机。
优选地，所述处理导航信息包括所述仿生机器鱼在载体坐标系下的四元数导航信息和加速度导航信息，所述第一目标导航信息具体为所述仿生机器鱼在导航坐标系下的速度矢量；则所述惯性导航模块具体用于：
按照第一公式，将所述四元数导航信息转换成欧拉角矩阵；
所述第一公式具体为：
其中，α、β和γ为分别所述欧拉角矩阵中的偏航角、俯仰角和翻滚角，q0、q1、q2和q3均为所述四元数导航信息中的元素，q0、q1、q2和q3均为实数且满足
按照第二公式，根据所述欧拉角矩阵得到所述载体坐标系与所述导航坐标系之间的加速度转移矩阵；
所述第二公式具体为：
其中，为所述加速度转移矩阵，b表示所述载体坐标系，n表示所述导航坐标系；
根据所述加速度转移矩阵和第三公式，将所述加速度导航信息进行坐标转换，得到所述仿生机器鱼在所述导航坐标系下的目标加速度信息；
所述第三公式具体为：
其中，an为所述目标加速度信息，ab为所述载体坐标系下的所述加速度导航信息；
对所述目标加速度信息进行积分运算，得到所述仿生机器鱼在所述导航坐标系下的所述速度矢量。
由于惯性导航模块得到的导航信息是载体坐标系相对于惯性坐标系的信息，因此通过控制模块的解码得到四元数导航信息，并分别依据第一公式和第二公式对四元数导航信息进行转换，以及依据第三公式对加速度导航信息进行转换，便于将惯性导航模块所得到的导航信息和北斗定位导航模块所得到的导航信息统一到同一个坐标系下，即导航坐标系，进而便于后续基于同一个坐标系下的导航信息来构建卡尔曼滤波器，得到最优的导航信息；依据速度和加速度之间的物理关系，通过控制模块的解码得到的加速度导航信息，便于后续得到依据该加速度导航信息进行积分运算，得到速度矢量，进而便于后续的卡尔曼滤波。
优选地，所述第二目标导航信息具体为所述仿生机器鱼在所述导航坐标系下的位移矢量，则所述控制模块具体用于：
根据所述速度矢量和所述位移矢量构建卡尔曼滤波器的状态向量，根据状态向量得到所述卡尔曼滤波器的状态方程和测量方程；
任一时刻的状态向量的表达式具体为：X＝[X1 X2]T；
所述状态方程的表达式具体为：
Xk＝[X1,k X2,k]T＝A[X1,k-1 X2,k-1]T+ωk-1；
所述测量方程的表达式具体为：
Zk＝[Z1,k Z2,k]T＝H[X1,k X2,k]T+vk-1；
其中，X为任一时刻的状态向量，X1和X2分别为任一时刻的所述位移矢量和所述速度矢量，Xk为k时刻的状态向量，X1,k和X2,k分别为k时刻的所述位移矢量和所述速度矢量，[X1,k-1 X2,k-1]T为k-1时刻的状态向量，X1,k-1和X2,k-1分别为k-1时刻的所述位移矢量和所述速度矢量，A为状态变换矩阵，ωk-1为k-1时刻的噪声误差矩阵；Zk为k时刻的导航信息观测向量，Z1,k和Z2,k分别为k时刻的位移观测值和速度观测值，H为观测变换矩阵，vk-1为噪声测量误差矩阵；
根据状态向量、所述状态方程和所述测量方程，得到所述卡尔曼滤波器的预测方程；
所述预测方程的表达式具体为：
其中，为k时刻对下一时刻的状态估计值，/>为k-1时刻对k时刻的状态估计值，/>为k时刻状态估计值/>的协方差矩阵，Pk-1为k-1时刻状态估计值经过校正后的协方差矩阵，Qk-1为k-1时刻的噪声误差矩阵ωk-1的协方差矩阵，AT为状态变换矩阵A的转置矩阵；
根据所述状态方程、所述测量方程和所述预测方程，得到所述卡尔曼滤波器的更新方程：
所述更新方程的表达式具体为：
其中，为k时刻经过校正后的状态估计值，具体为卡尔曼滤波处理之后的所述最优导航信息；Kk为k时刻的卡尔曼增益，Pk为k时刻状态估计值经过校正后的协方差矩阵，HT为观测变换矩阵H的转置矩阵，Rk为k时刻的噪声测量误差矩阵vk-1的协方差矩阵；
根据预测方程和所述更新方程，构建所述卡尔曼滤波器；并根据所述卡尔曼滤波器，得到所述最优导航信息。
根据惯性导航模块所对应的速度矢量和北斗定位导航模块所对应的位移矢量来构建状态向量，再依据该状态向量来得到卡尔曼滤波所需要的状态方程、测量方程和预测方程，便于实现仿生机器鱼的导航信息的预测，根据仿生机器鱼在初始时刻的位移观测值和速度观测值，分别进行预测迭代，可获得位移估计值和速度估计值；然后依据状态方程、测量方程和预测方程得到卡尔曼滤波所需要的更新方程，便于实现位移估计值和速度估计值的最优估计，得到精度较高的最优导航信息；其中，卡尔曼滤波方法是一种基于概率论和数理统计的优化自回归数据数量方法，能有效抑制数据采集过程的噪声等干扰因素，更好地保证基于北斗定位导航和惯性导航组合的导航方式，得到精度较高的导航信息的最优估计值。
优选地，如图1所示，所述***还包括均设置在仿生机器鱼内的电源模块和无线LORA模块，所述电源模块分别与所述控制模块、所述惯性导航模块、所述北斗定位导航模块和所述无线LORA模块电连接，所述无线LORA模块与所述控制模块电连接；
所述电源用于分别为所述控制模块、所述惯性导航模块、所述北斗定位导航模块和所述无线LORA模块供电；
所述无线LORA模块用于将所述仿生机器鱼的所述最优导航信息发送至外部设备。
通过电源模块保证导航定位***中各模块的正常工作，通过无线LORA模块将最优导航信息发送至外部设备，便于实现数据共享、数据远传和数据显示等扩展功能，进一步完善导航定位***。其中，本实施例中外部设备可以是带有人机交互界面的上位机。
具体地，本实施例中按照图1所示的结构将MPU6050导航模块、ATGM332导航模块、无线LORA模块和STM32系列的单片机相连，放入仿生机器鱼体内，将仿生机器鱼放入水中；仿生机器鱼下水之后，初始化ATGM332导航模块、MPU6050导航模块、无线LORA模块以及其它模块，此时需要等待ATGM332导航模块启动；ATGM332导航模块启动之后，在上位机上输入启动指令，仿生机器鱼开始按指定方向开始游动。游动过程中，ATGM332导航模块循环采集第一初始导航信息，采集的第一初始导航信息通过单片机解码得到位移矢量，MPU6050导航模块循环采集第二初始导航信息，通过单片机解码得到四元数导航信息和加速度导航信息，再通过MPU6050导航模块内部的DMP模块(MPU6050内部的运动引擎)计算成导航坐标系下的速度矢量，将位移矢量和速度矢量输入卡尔曼滤波器，得到最优导航信息，再利用仿生机器鱼内的无线LORA模块以及上位机中与该无线LORA模块相匹配的无线LORA模块，将最优导航信息发送到上位机，在上位机上实时显示仿生机器鱼的位置及速度。各模块所执行的功能及执行功能的先后顺序如图2所示。
在上述过程中，使用5V电源模块给单片机、MPU6050导航模块、ATGM332导航模块和无线LORA模块供电，单片机使用串口与ATGM332导航模块通信，单片机使用IO引脚模拟I2C时序和MPU6050导航模块通信。
实施例二、如图3所示，一种用于仿生机器鱼的导航定位方法，采用实施例一的用于仿生机器鱼的导航定位***进行导航定位，包括以下步骤：
S1：利用惯性导航模块对仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；
S2：利用北斗定位导航模块对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；
S3：利用控制模块，对所述第一初始导航信息进行解码，得到处理导航信息；并对所述第二初始导航信息进行解码，得到第二目标导航信息；
S4：利用所述惯性导航模块，根据所述处理导航信息进行解算，得到第一目标导航信息；
S5：利用所述控制模块，基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息。
通过惯性导航模块获取第一初始导航信息，并通过控制模块进行解码，便于后续再通过惯性导航模块根据解码得到的处理导航信息进行解算，得到卡尔曼滤波的其中一个输入变量；通过北斗定位导航模块获取第二初始导航信息，并通过控制模块进行解码，得到的第二目标导航信息作为卡尔曼滤波的另一个输入变量；基于卡尔曼滤波方法，依据两种定位导航模块得到的第一目标导航信息和第二目标导航信息来进行卡尔曼滤波，得到的最优估计的最优导航信息
本实施例的用于仿生机器鱼的导航定位方法，克服了单一导航的缺陷，避免了单一惯性导航模块中温漂、加速度计的误差以及速度和位移随时间的累计误差等各种误差，也避免了单一北斗定位导航模块因接收到的信号，质量较差而出现较大的导航误差；能基于北斗定位导航和惯性导航组合的导航方式，并采用卡尔曼滤波器，得到精度较高的导航信息的最优估计值，极其适用于环境多变、体积较小的仿生机器鱼的定位导航。
优选地，所述处理导航信息包括所述仿生机器鱼在载体坐标系下的四元数导航信息和加速度导航信息，所述第一目标导航信息具体为所述仿生机器鱼在导航坐标系下的速度矢量；则S4的具体步骤包括：
S41：按照第一公式，将所述四元数导航信息转换成欧拉角矩阵；
所述第一公式具体为：
其中，α、β和γ为分别所述欧拉角矩阵中的偏航角、俯仰角和翻滚角；q0、q1、q2和q3均为所述四元数导航信息中的元素，q0、q1、q2和q3均为实数且满足
S42：按照第二公式，根据所述欧拉角矩阵得到所述载体坐标系与所述导航坐标系之间的加速度转移矩阵；
所述第二公式具体为：
其中，为所述加速度转移矩阵，b表示所述载体坐标系，n表示所述导航坐标系；
S43：根据所述加速度转移矩阵和第三公式，将所述加速度导航信息进行坐标转换，得到所述仿生机器鱼在所述导航坐标系下的目标加速度信息；
所述第三公式具体为：
其中，an为所述目标加速度信息，ab为所述载体坐标系下的所述加速度导航信息；
S44：对所述目标加速度信息进行积分运算，得到所述仿生机器鱼在所述导航坐标系下的所述速度矢量。
由于惯性导航模块得到的导航信息是载体坐标系相对于惯性坐标系的信息，因此通过控制模块的解码得到四元数导航信息，并分别依据第一公式和第二公式对四元数导航信息进行转换，以及依据第三公式对加速度导航信息进行转换，便于将惯性导航模块所得到的导航信息和北斗定位导航模块所得到的导航信息统一到同一个坐标系下，即导航坐标系，进而便于后续基于同一个坐标系下的导航信息来构建卡尔曼滤波器，得到最优的导航信息；依据速度和加速度之间的物理关系，通过控制模块的解码得到的加速度导航信息，便于后续得到依据该加速度导航信息进行积分运算，得到速度矢量，进而便于后续的卡尔曼滤波。
优选地，所述第二目标导航信息具体为所述仿生机器鱼在所述导航坐标系下的位移矢量，则S5的具体步骤包括：
S51：根据所述速度矢量和所述位移矢量构建卡尔曼滤波器的状态向量，根据状态向量得到所述卡尔曼滤波器的状态方程和测量方程；
任一时刻的状态向量的表达式具体为：X＝[X1 X2]T；
所述状态方程的表达式具体为：
Xk＝[X1,k X2,k]T＝A[X1,k-1 X2,k-1]T+ωk-1；
所述测量方程的表达式具体为：
Zk＝[Z1,k Z2,k]T＝H[X1,k X2,k]T+vk-1；
其中，X为任一时刻的状态向量，X1和X2分别为任一时刻的所述位移矢量和所述速度矢量，Xk为k时刻的状态向量，X1,k和X2,k分别为k时刻的所述位移矢量和所述速度矢量，[X1,k-1 X2,k-1]T为k-1时刻的状态向量，X1,k-1和X2,k-1分别为k-1时刻的所述位移矢量和所述速度矢量，A为状态变换矩阵，ωk-1为k-1时刻的噪声误差矩阵；Zk为k时刻的导航信息观测向量，Z1,k和Z2,k分别为k时刻的位移观测值和速度观测值，H为观测变换矩阵，vk-1为噪声测量误差矩阵；
S52：根据状态向量、所述状态方程和所述测量方程，得到所述卡尔曼滤波器的预测方程；
所述预测方程的表达式具体为：
其中，为k时刻对下一时刻的状态估计值，/>为k-1时刻对k时刻的状态估计值，/>为k时刻状态估计值/>的协方差矩阵，Pk-1为k-1时刻状态估计值经过校正后的协方差矩阵，Qk-1为k-1时刻的噪声误差矩阵ωk-1的协方差矩阵，AT为状态变换矩阵A的转置矩阵；
S53：根据所述状态方程、所述测量方程和所述预测方程，得到所述卡尔曼滤波器的更新方程：
所述更新方程的表达式具体为：
其中，为k时刻经过校正后的状态估计值，具体为卡尔曼滤波处理之后的所述最优导航信息；Kk为k时刻的卡尔曼增益，Pk为k时刻状态估计值经过校正后的协方差矩阵，HT为观测变换矩阵H的转置矩阵，Rk为k时刻的噪声测量误差矩阵vk-1的协方差矩阵；
S54：根据预测方程和所述更新方程，构建所述卡尔曼滤波器；并根据所述卡尔曼滤波器，得到所述最优导航信息。
根据惯性导航模块所对应的速度矢量和北斗定位导航模块所对应的位移矢量来构建状态向量，再依据该状态向量来得到卡尔曼滤波所需要的状态方程、测量方程和预测方程，便于实现仿生机器鱼的导航信息的预测，根据仿生机器鱼在初始时刻的位移观测值和速度观测值，分别进行预测迭代，可获得位移估计值和速度估计值；然后依据状态方程、测量方程和预测方程得到卡尔曼滤波所需要的更新方程，便于实现位移估计值和速度估计值的最优估计，得到精度较高的最优导航信息；其中，卡尔曼滤波方法是一种基于概率论和数理统计的优化自回归数据数量方法，能有效抑制数据采集过程的噪声等干扰因素，更好地保证基于北斗定位导航和惯性导航组合的导航方式，得到精度较高的导航信息的最优估计值。
具体地，所述惯性导航模块具体为MPU6050型号的导航模块，所述北斗定位导航模块具体为ATGM332型号的导航模块。
优选地，当所述***还包括设置在所述仿生机器鱼内的无线LORA模块时，在S5之后包括：
S6：利用所述无线LORA模块，将所述仿生机器鱼的所述最优导航信息发送至外部设备。
通过无线LORA模块将最优导航信息发送至外部设备，便于实现数据共享、数据远传和数据显示等扩展功能，进一步完善导航定位***。
具体地，本实施例中ATGM332导航模块可能出现信号较差或接收到的卫星可使用数小于或等于4，此时ATGM332导航模块无法正常定位导航，则可采用单惯性导航的导航方式作为紧急备用导航定位模式，即将MPU6050导航模块在上一时刻的位移矢量与MPU6050导航模块在当前时刻的速度矢量输入到本实施例中已构建好的卡尔曼滤波器中，得到的最优导航信息再通过无线LORA模块发送至上位机中进行显示。本实施例中还包含有紧急备用导航定位模式的导航定位方法的完整流程图如图4所示。
本实施例中的未尽细节，详见实施例一以及图1至图2的具体描述内容，此处不再赘述。
Claims (8)
1.一种用于仿生机器鱼的导航定位***，其特征在于，包括均设置在仿生机器鱼内的控制模块、惯性导航模块和北斗定位导航模块，所述控制模块分别与所述惯性导航模块和所述北斗定位导航模块电连接；
所述惯性导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；
所述北斗定位导航模块用于对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；
所述控制模块用于获取所述第一初始导航信息，对所述第一初始导航信息进行解码，得到处理导航信息；还用于获取所述第二初始导航信息，并对所述第二初始导航信息进行解码，得到第二目标导航信息；
所述惯性导航模块还用于根据所述处理导航信息进行解算，得到第一目标导航信息；所述第一目标导航信息具体为所述仿生机器鱼在导航坐标系下的速度矢量；
所述控制模块还用于基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息；
所述第二目标导航信息具体为所述仿生机器鱼在所述导航坐标系下的位移矢量，则所述控制模块具体用于：
根据所述速度矢量和所述位移矢量构建卡尔曼滤波器的状态向量，根据状态向量得到所述卡尔曼滤波器的状态方程和测量方程；
任一时刻的状态向量的表达式具体为：X＝[X1 X2]T；
所述状态方程的表达式具体为：
Xk＝[X1,k X2,k]T＝A[X1,k-1 X2,k-1]T+ωk-1；
所述测量方程的表达式具体为：
Zk＝[Z1,k Z2,k]T＝H[X1,k X2,k]T+vk-1；
其中，X为任一时刻的状态向量，X1和X2分别为任一时刻的所述位移矢量和所述速度矢量，Xk为k时刻的状态向量，X1,k和X2,k分别为k时刻的所述位移矢量和所述速度矢量，[X1,k-1X2,k-1]T为k-1时刻的状态向量，X1,k-1和X2,k-1分别为k-1时刻的所述位移矢量和所述速度矢量，A为状态变换矩阵，ωk-1为k-1时刻的噪声误差矩阵；Zk为k时刻的导航信息观测向量，Z1,k和Z2,k分别为k时刻的位移观测值和速度观测值，H为观测变换矩阵，vk-1为噪声测量误差矩阵；
根据状态向量、所述状态方程和所述测量方程，得到所述卡尔曼滤波器的预测方程；
所述预测方程的表达式具体为：
其中，为k时刻对下一时刻的状态估计值，/>为k-1时刻对k时刻的状态估计值，/>为k时刻状态估计值/>的协方差矩阵，Pk-1为k-1时刻状态估计值经过校正后的协方差矩阵，Qk-1为k-1时刻的噪声误差矩阵ωk-1的协方差矩阵，AT为状态变换矩阵A的转置矩阵；
根据所述状态方程、所述测量方程和所述预测方程，得到所述卡尔曼滤波器的更新方程：
所述更新方程的表达式具体为：
其中，为k时刻经过校正后的状态估计值，具体为卡尔曼滤波处理之后的所述最优导航信息；Kk为k时刻的卡尔曼增益，Pk为k时刻状态估计值经过校正后的协方差矩阵，HT为观测变换矩阵H的转置矩阵，Rk为k时刻的噪声测量误差矩阵vk-1的协方差矩阵；
根据预测方程和所述更新方程，构建所述卡尔曼滤波器；并根据所述卡尔曼滤波器，得到所述最优导航信息。
2.根据权利要求1所述的用于仿生机器鱼的导航定位***，其特征在于，所述处理导航信息包括所述仿生机器鱼在载体坐标系下的四元数导航信息和加速度导航信息，则所述惯性导航模块具体用于：
按照第一公式，将所述四元数导航信息转换成欧拉角矩阵；
所述第一公式具体为：
其中，α、β和γ为分别所述欧拉角矩阵中的偏航角、俯仰角和翻滚角，q0、q1、q2和q3均为所述四元数导航信息中的元素，q0、q1、q2和q3均为实数且满足
按照第二公式，根据所述欧拉角矩阵得到所述载体坐标系与所述导航坐标系之间的加速度转移矩阵；
所述第二公式具体为：
其中，为所述加速度转移矩阵，b表示所述载体坐标系，n表示所述导航坐标系；
根据所述加速度转移矩阵和第三公式，将所述加速度导航信息进行坐标转换，得到所述仿生机器鱼在所述导航坐标系下的目标加速度信息；
所述第三公式具体为：
其中，an为所述目标加速度信息，ab为所述载体坐标系下的所述加速度导航信息；
对所述目标加速度信息进行积分运算，得到所述仿生机器鱼在所述导航坐标系下的所述速度矢量。
3.根据权利要求1所述的用于仿生机器鱼的导航定位***，其特征在于，所述惯性导航模块具体为MPU6050型号的导航模块，所述北斗定位导航模块具体为ATGM332型号的导航模块。
4.根据权利要求1所述的用于仿生机器鱼的导航定位***，其特征在于，所述***还包括均设置在所述仿生机器鱼内的电源模块和无线LORA模块，所述电源模块分别与所述控制模块、所述惯性导航模块、所述北斗定位导航模块和所述无线LORA模块电连接，所述无线LORA模块与所述控制模块电连接；
所述电源用于分别为所述控制模块、所述惯性导航模块、所述北斗定位导航模块和所述无线LORA模块供电；
所述无线LORA模块用于将所述仿生机器鱼的所述最优导航信息发送至外部设备。
5.一种用于仿生机器鱼的导航定位方法，其特征在于，采用如权利要求1至4任一项所述的用于仿生机器鱼的导航定位***进行导航定位，包括以下步骤：
利用惯性导航模块对仿生机器鱼进行定位，得到所述仿生机器鱼的第一初始导航信息；
利用北斗定位导航模块对所述仿生机器鱼进行定位，得到所述仿生机器鱼的第二初始导航信息；
利用控制模块，对所述第一初始导航信息进行解码，得到处理导航信息；并对所述第二初始导航信息进行解码，得到第二目标导航信息；
利用所述惯性导航模块，根据所述处理导航信息进行解算，得到第一目标导航信息；所述第一目标导航信息具体为所述仿生机器鱼在导航坐标系下的速度矢量；
利用所述控制模块，基于卡尔曼滤波方法，根据所述第一目标导航信息和第二目标导航信息进行卡尔曼滤波处理，得到所述仿生机器鱼的最优导航信息；
所述第二目标导航信息具体为所述仿生机器鱼在所述导航坐标系下的位移矢量，则所述控制模块具体用于：
根据所述速度矢量和所述位移矢量构建卡尔曼滤波器的状态向量，根据状态向量得到所述卡尔曼滤波器的状态方程和测量方程；
任一时刻的状态向量的表达式具体为：X＝[X1 X2]T；
所述状态方程的表达式具体为：
Xk＝[X1,k X2,k]T＝A[X1,k-1 X2,k-1]T+ωk-1；
所述测量方程的表达式具体为：
Zk＝[Z1,k Z2,k]T＝H[X1,k X2,k]T+vk-1；
其中，X为任一时刻的状态向量，X1和X2分别为任一时刻的所述位移矢量和所述速度矢量，Xk为k时刻的状态向量，X1,k和X2,k分别为k时刻的所述位移矢量和所述速度矢量，[X1,k- 1X2,k-1]T为k-1时刻的状态向量，X1,k-1和X2,k-1分别为k-1时刻的所述位移矢量和所述速度矢量，A为状态变换矩阵，ωk-1为k-1时刻的噪声误差矩阵；Zk为k时刻的导航信息观测向量，Z1,k和Z2,k分别为k时刻的位移观测值和速度观测值，H为观测变换矩阵，vk-1为噪声测量误差矩阵；
根据状态向量、所述状态方程和所述测量方程，得到所述卡尔曼滤波器的预测方程；
所述预测方程的表达式具体为：
其中，为k时刻对下一时刻的状态估计值，/>为k-1时刻对k时刻的状态估计值，/>为k时刻状态估计值/>的协方差矩阵，Pk-1为k-1时刻状态估计值经过校正后的协方差矩阵，Qk-1为k-1时刻的噪声误差矩阵ωk-1的协方差矩阵，AT为状态变换矩阵A的转置矩阵；
根据所述状态方程、所述测量方程和所述预测方程，得到所述卡尔曼滤波器的更新方程：
所述更新方程的表达式具体为：
其中，为k时刻经过校正后的状态估计值，具体为卡尔曼滤波处理之后的所述最优导航信息；Kk为k时刻的卡尔曼增益，Pk为k时刻状态估计值经过校正后的协方差矩阵，HT为观测变换矩阵H的转置矩阵，Rk为k时刻的噪声测量误差矩阵vk-1的协方差矩阵；
根据预测方程和所述更新方程，构建所述卡尔曼滤波器；并根据所述卡尔曼滤波器，得到所述最优导航信息。
6.根据权利要求5所述的用于仿生机器鱼的导航定位方法，其特征在于，所述处理导航信息包括所述仿生机器鱼在载体坐标系下的四元数导航信息和加速度导航信息，所述第一目标导航信息具体为所述仿生机器鱼在导航坐标系下的速度矢量；则得到所述第一目标导航信息的具体步骤包括：
按照第一公式，将所述四元数导航信息转换成欧拉角矩阵；
所述第一公式具体为：
其中，α、β和γ为分别所述欧拉角矩阵中的偏航角、俯仰角和翻滚角；q0、q1、q2和q3均为所述四元数导航信息中的元素，q0、q1、q2和q3均为实数且满足
按照第二公式，根据所述欧拉角矩阵得到所述载体坐标系与所述导航坐标系之间的加速度转移矩阵；
所述第二公式具体为：
其中，为所述加速度转移矩阵，b表示所述载体坐标系，n表示所述导航坐标系；
根据所述加速度转移矩阵和第三公式，将所述加速度导航信息进行坐标转换，得到所述仿生机器鱼在所述导航坐标系下的目标加速度信息；
所述第三公式具体为：
其中，an为所述目标加速度信息，ab为所述载体坐标系下的所述加速度导航信息；
对所述目标加速度信息进行积分运算，得到所述仿生机器鱼在所述导航坐标系下的所述速度矢量。
7.根据权利要求6所述的用于仿生机器鱼的导航定位方法，其特征在于，所述第二目标导航信息具体为所述仿生机器鱼在所述导航坐标系下的位移矢量，则得到所述最优导航信息的具体步骤包括：
根据所述速度矢量和所述位移矢量构建卡尔曼滤波器的状态向量，根据状态向量得到所述卡尔曼滤波器的状态方程和测量方程；
任一时刻的状态向量的表达式具体为：X＝[X1 X2]T；
所述状态方程的表达式具体为：
Xk＝[X1,k X2,k]T＝A[X1,k-1 X2,k-1]T+ωk-1；
所述测量方程的表达式具体为：
Zk＝[Z1,k Z2,k]T＝H[X1,k X2,k]T+vk-1；
其中，X为任一时刻的状态向量，X1和X2分别为任一时刻的所述位移矢量和所述速度矢量，Xk为k时刻的状态向量，X1,k和X2,k分别为k时刻的所述位移矢量和所述速度矢量，[X1,k-1X2,k-1]T为k-1时刻的状态向量，X1,k-1和X2,k-1分别为k-1时刻的所述位移矢量和所述速度矢量，A为状态变换矩阵，ωk-1为k-1时刻的噪声误差矩阵；Zk为k时刻的导航信息观测向量，Z1,k和Z2,k分别为k时刻的位移观测值和速度观测值，H为观测变换矩阵，vk-1为噪声测量误差矩阵；
根据状态向量、所述状态方程和所述测量方程，得到所述卡尔曼滤波器的预测方程；
所述预测方程的表达式具体为：
其中，为k时刻对下一时刻的状态估计值，/>为k-1时刻对k时刻的状态估计值，/>为k时刻状态估计值/>的协方差矩阵，Pk-1为k-1时刻状态估计值经过校正后的协方差矩阵，Qk-1为k-1时刻的噪声误差矩阵ωk-1的协方差矩阵，AT为状态变换矩阵A的转置矩阵；
根据所述状态方程、所述测量方程和所述预测方程，得到所述卡尔曼滤波器的更新方程：
所述更新方程的表达式具体为：
其中，为k时刻经过校正后的状态估计值，具体为卡尔曼滤波处理之后的所述最优导航信息；Kk为k时刻的卡尔曼增益，Pk为k时刻状态估计值经过校正后的协方差矩阵，HT为观测变换矩阵H的转置矩阵，Rk为k时刻的噪声测量误差矩阵vk-1的协方差矩阵；
根据预测方程和所述更新方程，构建所述卡尔曼滤波器；并根据所述卡尔曼滤波器，得到所述最优导航信息。
8.根据权利要求5至7任一项所述的用于仿生机器鱼的导航定位方法，其特征在于，所述惯性导航模块具体为MPU6050型号的导航模块，所述北斗定位导航模块具体为ATGM332型号的导航模块。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110767838.3A CN113639744B (zh) | 2021-07-07 | 2021-07-07 | 一种用于仿生机器鱼的导航定位方法和*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110767838.3A CN113639744B (zh) | 2021-07-07 | 2021-07-07 | 一种用于仿生机器鱼的导航定位方法和*** |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113639744A CN113639744A (zh) | 2021-11-12 |
| CN113639744B true CN113639744B (zh) | 2023-10-20 |
Family
ID=78416836
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110767838.3A Active CN113639744B (zh) | 2021-07-07 | 2021-07-07 | 一种用于仿生机器鱼的导航定位方法和*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113639744B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116592896B (zh) * | 2023-07-17 | 2023-09-29 | 山东水发黄水东调工程有限公司 | 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20080084435A (ko) * | 2007-03-16 | 2008-09-19 | 한국전자통신연구원 | 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체 |
| CN102608642A (zh) * | 2011-01-25 | 2012-07-25 | 北京七维航测科技股份有限公司 | 北斗/惯性组合导航*** |
| CN106932804A (zh) * | 2017-03-17 | 2017-07-07 | 南京航空航天大学 | 天文辅助的惯性/北斗紧组合导航***及其导航方法 |
| CN107290742A (zh) * | 2017-06-20 | 2017-10-24 | 武汉理工大学 | 一种非线性目标跟踪***中平方根容积卡尔曼滤波方法 |
| CN107478223A (zh) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 |
| CN109443353A (zh) * | 2018-12-25 | 2019-03-08 | 中北大学 | 基于模糊自适应ickf的视觉-惯性紧耦合组合导航方法 |
| CN109709513A (zh) * | 2019-01-25 | 2019-05-03 | 中广核研究院有限公司 | 一种室内应用于核电站定位方法及*** |
| CN110780326A (zh) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | 一种车载组合导航***和定位方法 |
| CN111007557A (zh) * | 2019-12-10 | 2020-04-14 | 中国矿业大学 | 自适应运动学模型辅助的gnss载波相位与多普勒融合测速方法 |
| CN111351482A (zh) * | 2020-03-19 | 2020-06-30 | 南京理工大学 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
-
2021
- 2021-07-07 CN CN202110767838.3A patent/CN113639744B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20080084435A (ko) * | 2007-03-16 | 2008-09-19 | 한국전자통신연구원 | 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체 |
| CN102608642A (zh) * | 2011-01-25 | 2012-07-25 | 北京七维航测科技股份有限公司 | 北斗/惯性组合导航*** |
| CN107478223A (zh) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 |
| CN106932804A (zh) * | 2017-03-17 | 2017-07-07 | 南京航空航天大学 | 天文辅助的惯性/北斗紧组合导航***及其导航方法 |
| CN107290742A (zh) * | 2017-06-20 | 2017-10-24 | 武汉理工大学 | 一种非线性目标跟踪***中平方根容积卡尔曼滤波方法 |
| CN109443353A (zh) * | 2018-12-25 | 2019-03-08 | 中北大学 | 基于模糊自适应ickf的视觉-惯性紧耦合组合导航方法 |
| CN109709513A (zh) * | 2019-01-25 | 2019-05-03 | 中广核研究院有限公司 | 一种室内应用于核电站定位方法及*** |
| CN110780326A (zh) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | 一种车载组合导航***和定位方法 |
| CN111007557A (zh) * | 2019-12-10 | 2020-04-14 | 中国矿业大学 | 自适应运动学模型辅助的gnss载波相位与多普勒融合测速方法 |
| CN111351482A (zh) * | 2020-03-19 | 2020-06-30 | 南京理工大学 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
Non-Patent Citations (2)
| Title |
|---|
| 基于卡尔曼滤波的INS/USBL水下导航***模型研究;李守军;陶春辉;包更生;;海洋技术(第03期);47-50 * |
| 基于自适应卡尔曼滤波的NGMIMU/GPS组合导航设计;丁明理;王祁;殷栩;;哈尔滨工业大学学报(第01期);67-70 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113639744A (zh) | 2021-11-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106780699B (zh) | 一种基于sins/gps和里程计辅助的视觉slam方法 | |
| CN104457754B (zh) | 一种基于sins/lbl紧组合的auv水下导航定位方法 | |
| CN201266089Y (zh) | Ins/gps组合导航*** | |
| CN104656058B (zh) | 分布式多移动节点协作定位*** | |
| CN102052923B (zh) | 一种小型水下机器人组合导航***及导航方法 | |
| CN106352872B (zh) | 一种无人机自主导航***及其导航方法 | |
| US11686744B2 (en) | Posture estimation method, posture estimation device, and vehicle | |
| CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
| CN103941274B (zh) | 一种导航方法及导航终端 | |
| CN112197765B (zh) | 一种实现水下机器人精细导航的方法 | |
| CN113252041B (zh) | 一种适用于小型水下机器人的组合导航方法 | |
| CN102288170A (zh) | 一种水下航行器内电子罗盘的校正方法 | |
| CN109739088B (zh) | 一种无人船有限时间收敛状态观测器及其设计方法 | |
| CN116295511B (zh) | 一种用于管道潜航机器人的鲁棒初始对准方法及*** | |
| CN113639744B (zh) | 一种用于仿生机器鱼的导航定位方法和*** | |
| CN108801250B (zh) | 基于水下机器人的实时姿态获取方法及装置 | |
| CN104280024B (zh) | 一种深水机器人组合导航装置和方法 | |
| CN111238469A (zh) | 一种基于惯性/数据链的无人机编队相对导航方法 | |
| CN113129377B (zh) | 一种三维激光雷达快速鲁棒slam方法和装置 | |
| CN104406592A (zh) | 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 | |
| CN111982126B (zh) | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 | |
| CN103791901B (zh) | 一种星敏感器数据处理*** | |
| CN2788131Y (zh) | 小型水下自主组合导航装置 | |
| Song et al. | Hierarchical underwater localization in dominating background flow fields | |
| CN110954080A (zh) | 一种消除载体磁干扰的磁罗盘校准方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |