CN113639744B - Navigation positioning method and system for bionic robot fish - Google Patents

Navigation positioning method and system for bionic robot fish Download PDF

Info

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
Application number
CN202110767838.3A
Other languages
Chinese (zh)
Other versions
CN113639744A (en
Inventor
文小玲
周勇
李自成
王佳
魏涛
杨颖�
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan Institute of Technology
Original Assignee
Wuhan Institute of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhan Institute of Technology filed Critical Wuhan Institute of Technology
Priority to CN202110767838.3A priority Critical patent/CN113639744B/en
Publication of CN113639744A publication Critical patent/CN113639744A/en
Application granted granted Critical
Publication of CN113639744B publication Critical patent/CN113639744B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

The invention relates to a navigation positioning method and a navigation positioning system for a bionic robot fish, wherein the navigation positioning method and the navigation positioning system comprise a control module, an inertial navigation module and a Beidou positioning navigation module which are all arranged in the bionic robot fish; the inertial navigation module obtains first initial navigation information, and the Beidou positioning navigation module obtains second initial navigation information; the control module decodes the first initial navigation information to obtain processed navigation information; decoding the second initial navigation information to obtain second target navigation information; the inertial navigation module performs calculation according to the processed navigation information to obtain first target navigation information; the control module performs Kalman filtering processing according to the first target navigation information and the second target navigation information based on a Kalman filtering method to obtain optimal navigation information. The invention can obtain the optimal estimated value of the navigation information with higher precision based on the navigation mode of combining Beidou positioning navigation and inertial navigation and by adopting the Kalman filter, thereby overcoming the defect of single navigation.

Description

Navigation positioning method and system for bionic robot fish
Technical Field
The invention relates to the field of navigation of bionic robot fish, in particular to a navigation positioning method and system for the bionic robot fish.
Background
In the control system of the bionic robot fish, the navigation positioning and the path planning are based on accurate position parameters, so the navigation positioning is one of key technologies of the control system of the bionic robot fish. Because infrared, sonar and electromagnetic signals can be seriously attenuated under water, some navigation positioning methods suitable for the ground and the sky are not suitable for navigation positioning of the bionic robot fish, and therefore positioning and navigation of the bionic robot fish become hot problems in the research of the bionic robot fish.
Because inertial navigation (Inertial Navigation System, INS) has high accuracy in a short time, the INS is often used in underwater positioning, and the operating principle of the INS is to use 3 accelerometers and 3 gyroscopes perpendicular to each other mounted on a biomimetic robot fish to obtain measured values of the accelerometers and the gyroscopes, and to integrate the measured values to obtain the instantaneous speed and position of the biomimetic robot fish.
However, since the temperature drift and speed and position of the gyroscope are obtained by integration, the speed and heading errors of the biomimetic robotic fish accumulate linearly over time, while the position errors accumulate exponentially over time. Meanwhile, because the bionic robot fish has poor underwater signal and large disturbance, the current single navigation positioning method based on inertial navigation cannot realize real-time navigation positioning of the bionic robot fish, and has large navigation positioning error; in addition, because the volume of the bionic robot fish is smaller, and the working environment is in a water area or a fish pond with smaller depth, once the positioning error of the bionic robot fish is very large, the problems of incapability of avoiding obstacles, loss and the like are easy to occur.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a navigation positioning method and a system for a bionic robot fish, which can obtain an optimal estimated value of navigation information with higher precision based on a navigation mode combining Beidou positioning navigation and inertial navigation and by adopting a Kalman filter, and are extremely suitable for positioning navigation of the bionic robot fish with changeable environment and smaller volume.
The technical scheme for solving the technical problems is as follows:
the navigation positioning system for the bionic robot fish comprises a control module, an inertial navigation module and a Beidou positioning navigation module which are all arranged in the bionic robot fish, wherein the control module is electrically connected with the inertial navigation module and the Beidou positioning navigation module respectively;
the inertial navigation module is used for positioning the bionic robot fish to obtain first initial navigation information of the bionic robot fish;
the Beidou positioning and navigation module is used for positioning the bionic robot fish to obtain second initial navigation information of the bionic robot fish;
the control module is used for acquiring the first initial navigation information, decoding the first initial navigation information and obtaining processed navigation information; the method is also used for acquiring the second initial navigation information, decoding the second initial navigation information and obtaining second target navigation information;
the inertial navigation module is also used for resolving according to the processed navigation information to obtain first target navigation information;
the control module is also used for carrying out Kalman filtering processing according to the first target navigation information and the second target navigation information based on a Kalman filtering method to obtain the optimal navigation information of the bionic robot fish.
According to another aspect of the present invention, there is also provided a navigation and positioning method for a biomimetic robotic fish, for boarding navigation using the navigation and positioning system for a biomimetic robotic fish of the present invention, comprising the steps of:
positioning the bionic robot fish by utilizing an inertial navigation module to obtain first initial navigation information of the bionic robot fish;
positioning the bionic robot fish by using a Beidou positioning navigation module to obtain second initial navigation information of the bionic robot fish;
decoding the first initial navigation information by using a control module to obtain processed navigation information; decoding the second initial navigation information to obtain second target navigation information;
the inertial navigation module is utilized to calculate according to the processed navigation information, so as to obtain first target navigation information;
and carrying out Kalman filtering processing according to the first target navigation information and the second target navigation information based on a Kalman filtering method by using the control module to obtain the optimal navigation information of the bionic robot fish.
The navigation and positioning method and system for the bionic robot fish have the beneficial effects that: acquiring first initial navigation information through an inertial navigation module, decoding through a control module, and resolving according to the processed navigation information obtained through decoding through the inertial navigation module conveniently in the follow-up process to obtain one input variable of Kalman filtering; acquiring second initial navigation information through the Beidou positioning navigation module, and decoding through the control module, wherein the acquired second target navigation information is used as another input variable of Kalman filtering; based on a Kalman filtering method, kalman filtering is carried out according to first target navigation information and second target navigation information obtained by two positioning navigation modules, and the obtained optimal estimated optimal navigation information is obtained
The navigation positioning method and system for the bionic robot fish overcome the defect of single navigation, avoid various errors such as temperature drift, accelerometer errors, accumulated errors of speed and displacement along with time and the like in a single inertial navigation module, and avoid larger navigation errors caused by poor quality of received signals of the single Beidou positioning navigation module; the navigation method based on the combination of Beidou positioning navigation and inertial navigation can obtain the optimal estimated value of navigation information with higher precision by adopting a Kalman filter, and is extremely suitable for positioning navigation of bionic robot fish with changeable environment and smaller volume.
Drawings
Fig. 1 is a schematic structural diagram of a navigation positioning system for a biomimetic robotic fish according to a first embodiment of the present invention;
FIG. 2 is a flow chart illustrating the execution of functions of each module of the navigation and positioning system according to the first embodiment of the present invention;
fig. 3 is a flow chart of a navigation positioning method for a bionic robot fish according to a second embodiment of the invention;
fig. 4 is a complete flow chart of a navigation positioning method in the second embodiment of the invention.
Detailed Description
The principles and features of the present invention are described below with reference to the drawings, the examples are illustrated for the purpose of illustrating the invention and are not to be construed as limiting the scope of the invention.
The present invention will be described below with reference to the accompanying drawings.
In the first embodiment, as shown in fig. 1, a navigation positioning system for a bionic robot fish comprises a control module, an inertial navigation module and a Beidou positioning navigation module which are all arranged in the bionic robot fish, wherein the control module is electrically connected with the inertial navigation module and the Beidou positioning navigation module respectively;
the inertial navigation module is used for positioning the bionic robot fish to obtain first initial navigation information of the bionic robot fish;
the Beidou positioning and navigation module is used for positioning the bionic robot fish to obtain second initial navigation information of the bionic robot fish;
the control module is used for acquiring the first initial navigation information, decoding the first initial navigation information and obtaining processed navigation information; the method is also used for acquiring the second initial navigation information, decoding the second initial navigation information and obtaining second target navigation information;
the inertial navigation module is also used for resolving according to the processed navigation information to obtain first target navigation information;
the control module is also used for carrying out Kalman filtering processing according to the first target navigation information and the second target navigation information based on a Kalman filtering method to obtain the optimal navigation information of the bionic robot fish.
Acquiring first initial navigation information through an inertial navigation module, decoding through a control module, and resolving according to the processed navigation information obtained through decoding through the inertial navigation module conveniently in the follow-up process to obtain one input variable of Kalman filtering; acquiring second initial navigation information through the Beidou positioning navigation module, and decoding through the control module, wherein the acquired second target navigation information is used as another input variable of Kalman filtering; based on a Kalman filtering method, kalman filtering is carried out according to first target navigation information and second target navigation information obtained by two positioning navigation modules, and the obtained optimal estimated optimal navigation information is obtained
The navigation positioning system for the bionic robot fish overcomes the defect of single navigation, avoids various errors such as temperature drift, accelerometer errors, accumulated errors of speed and displacement along with time and the like in a single inertial navigation module, and also avoids larger navigation errors caused by poor quality of received signals of the single Beidou positioning navigation module; the navigation method based on the combination of Beidou positioning navigation and inertial navigation can obtain the optimal estimated value of navigation information with higher precision by adopting a Kalman filter, and is extremely suitable for positioning navigation of bionic robot fish with changeable environment and smaller volume.
Specifically, in this embodiment, the inertial navigation module is specifically an MPU6050 model navigation module, and the beidou positioning navigation module is specifically an ATGM332 model navigation module. The second initial navigation information is obtained by sequentially processing the satellite navigation information through a signal processing unit and a carrier tracking unit in the ATGM332 navigation module.
Specifically, in this embodiment, the control module is specifically a single-chip microcomputer of STM32 series.
Preferably, the processing navigation information comprises quaternion navigation information and acceleration navigation information of the bionic robot fish under a carrier coordinate system, and the first target navigation information is specifically a speed vector of the bionic robot fish under a navigation coordinate system; the inertial navigation module is specifically configured to:
converting the quaternion navigation information into an Euler angle matrix according to a first formula;
the first formula specifically comprises:
wherein alpha, beta and gamma are yaw angle, pitch angle and roll angle in the Euler angle matrix respectively, q 0 、q 1 、q 2 and q3 Are all elements in the quaternion navigation information, q 0 、q 1 、q 2 and q3 Are all real numbers and satisfy
According to a second formula, obtaining an acceleration transfer matrix between the carrier coordinate system and the navigation coordinate system according to the Euler angle matrix;
the second formula is specifically:
wherein ,b represents the carrier coordinate system and n represents the navigation coordinate system for the acceleration transfer matrix;
according to the acceleration transfer matrix and a third formula, carrying out coordinate transformation on the acceleration navigation information to obtain target acceleration information of the bionic robot fish under the navigation coordinate system;
the third formula is specifically:
wherein ,an A for the target acceleration information b Navigation information for the acceleration in the carrier coordinate system;
and carrying out integral operation on the target acceleration information to obtain the speed vector of the bionic robot fish under the navigation coordinate system.
Because the navigation information obtained by the inertial navigation module is the information of the carrier coordinate system relative to the inertial coordinate system, the quaternion navigation information is obtained through the decoding of the control module, the quaternion navigation information is converted according to the first formula and the second formula respectively, and the acceleration navigation information is converted according to the third formula, so that the navigation information obtained by the inertial navigation module and the navigation information obtained by the Beidou positioning navigation module are conveniently unified to the same coordinate system, namely the navigation coordinate system, and a Kalman filter is conveniently constructed based on the navigation information in the same coordinate system to obtain the optimal navigation information; according to the physical relationship between the speed and the acceleration, the acceleration navigation information obtained through decoding of the control module is convenient to obtain the integral operation according to the acceleration navigation information, the speed vector is obtained, and further the subsequent Kalman filtering is convenient.
Preferably, the second target navigation information is specifically a displacement vector of the biomimetic robotic fish under the navigation coordinate system, and the control module is specifically configured to:
constructing a state vector of a Kalman filter according to the speed vector and the displacement vector, and obtaining a state equation and a measurement equation of the Kalman filter according to the state vector;
the expression of the state vector at any time is specifically: x= [ X ] 1 X 2 ] T ；
The expression of the state equation is specifically:
X k ＝[X 1,k X 2,k ] T ＝A[X 1,k-1 X 2,k-1 ] T +ω k-1 ；
the expression of the measurement equation is specifically:
Z k ＝[Z 1,k Z 2,k ] T ＝H[X 1,k X 2,k ] T +v k-1 ；
wherein X is a state vector at any time, X 1 and X2 The displacement vector and the velocity vector, X at any time point respectively k Is the state vector at time k, X 1,k and X2,k The displacement vector and the velocity vector at time k, respectively, [ X ] 1,k-1 X 2,k-1 ] T Is the state vector at time k-1, X 1,k-1 and X2,k-1 The displacement vector and the velocity vector at the moment k-1 respectively, A is a state transformation matrix, omega k-1 A noise error matrix at the moment k-1; z is Z k Observing vector for navigation information at k moment, Z 1,k and Z2,k Respectively, the displacement observation value and the velocity observation value at the moment k, H is an observation transformation matrix, v k-1 Measuring an error matrix for noise;
obtaining a prediction equation of the Kalman filter according to the state vector, the state equation and the measurement equation;
the expression of the prediction equation is specifically:
wherein ,for the state estimate of time k to the next time,/->For the state estimate of time k-1 versus time k +.>State estimation value +.>Covariance matrix, P k-1 For the covariance matrix after the state estimation value at the moment k-1 is corrected, Q k-1 Noise error matrix omega for time k-1 k-1 Covariance matrix of A) T Is the transpose of the state transformation matrix A;
and obtaining an update equation of the Kalman filter according to the state equation, the measurement equation and the prediction equation:
the expression of the update equation is specifically:
wherein ,the state estimation value after the correction at the moment k is specifically the optimal navigation information after Kalman filtering processing; k (K) k Kalman gain at time k, P k As the covariance matrix after the state estimation value at the moment k is corrected, H T To observe the transpose of the transformation matrix H, R k Error matrix v is measured for noise at time k k-1 Is a covariance matrix of (a);
constructing the Kalman filter according to a prediction equation and the update equation; and obtaining the optimal navigation information according to the Kalman filter.
Constructing a state vector according to a speed vector corresponding to the inertial navigation module and a displacement vector corresponding to the Beidou positioning navigation module, obtaining a state equation, a measurement equation and a prediction equation required by Kalman filtering according to the state vector, facilitating the prediction of navigation information of the bionic robot fish, and respectively carrying out prediction iteration according to a displacement observation value and a speed observation value of the bionic robot fish at an initial moment to obtain a displacement estimation value and a speed estimation value; then, an update equation required by Kalman filtering is obtained according to the state equation, the measurement equation and the prediction equation, so that optimal estimation of a displacement estimated value and a velocity estimated value is conveniently realized, and optimal navigation information with higher precision is obtained; the Kalman filtering method is an optimization autoregressive data quantity method based on probability theory and numerical statistics, can effectively inhibit noise and other interference factors in the data acquisition process, better ensures a navigation mode based on a combination of Beidou positioning navigation and inertial navigation, and obtains an optimal estimated value of navigation information with higher precision.
Preferably, as shown in fig. 1, the system further includes a power module and a wireless LORA module both disposed in the biomimetic robotic fish, the power module is electrically connected with the control module, the inertial navigation module, the beidou positioning navigation module and the wireless LORA module, and the wireless LORA module is electrically connected with the control module;
the power supply is used for supplying power to the control module, the inertial navigation module, the Beidou positioning navigation module and the wireless LORA module respectively;
the wireless LORA module is used for sending the optimal navigation information of the bionic robot fish to external equipment.
The power module ensures the normal work of each module in the navigation positioning system, and the wireless LORA module sends the optimal navigation information to the external equipment, so that the expanded functions of data sharing, data remote transmission, data display and the like are conveniently realized, and the navigation positioning system is further perfected. In this embodiment, the external device may be an upper computer with a man-machine interaction interface.
Specifically, in this embodiment, according to the structure shown in fig. 1, an MPU6050 navigation module, an ATGM332 navigation module, a wireless LORA module, and a STM32 series single-chip microcomputer are connected, and the single-chip microcomputer is placed in a bionic robot fish body, and the bionic robot fish is placed in water; after the bionic robot fish is launched, initializing an ATGM332 navigation module, an MPU6050 navigation module, a wireless LORA module and other modules, wherein the ATGM332 navigation module needs to be started at the moment; after the ATGM332 navigation module is started, a starting instruction is input on the upper computer, and the bionic robot fish starts to swim according to the designated direction. In the swimming process, the ATGM332 navigation module circularly collects first initial navigation information, the collected first initial navigation information is decoded through a single chip microcomputer to obtain a displacement vector, the MPU6050 navigation module circularly collects second initial navigation information, the quaternion navigation information and the acceleration navigation information are obtained through decoding of the single chip microcomputer, the velocity vector under a navigation coordinate system is calculated through a DMP module (a motion engine in the MPU 6050) in the MPU6050 navigation module, the displacement vector and the velocity vector are input into a Kalman filter to obtain optimal navigation information, the optimal navigation information is sent to an upper computer by utilizing a wireless LORA module in the bionic robot fish and a wireless LORA module matched with the wireless LORA module in the upper computer, and the position and the velocity of the bionic robot fish are displayed on the upper computer in real time. The functions performed by the modules and the sequence of the performed functions are shown in fig. 2.
In the process, a 5V power module is used for supplying power to the singlechip, the MPU6050 navigation module, the ATGM332 navigation module and the wireless LORA module, the singlechip is communicated with the ATGM332 navigation module by using a serial port, and the singlechip is communicated with the MPU6050 navigation module by using an IO pin to simulate an I2C time sequence.
In a second embodiment, as shown in fig. 3, a navigation positioning method for a biomimetic robotic fish, using the navigation positioning system for a biomimetic robotic fish in the first embodiment, performs navigation positioning, includes the following steps:
s1: positioning the bionic robot fish by utilizing an inertial navigation module to obtain first initial navigation information of the bionic robot fish;
s2: positioning the bionic robot fish by using a Beidou positioning navigation module to obtain second initial navigation information of the bionic robot fish;
s3: decoding the first initial navigation information by using a control module to obtain processed navigation information; decoding the second initial navigation information to obtain second target navigation information;
s4: the inertial navigation module is utilized to calculate according to the processed navigation information, so as to obtain first target navigation information;
s5: and carrying out Kalman filtering processing according to the first target navigation information and the second target navigation information based on a Kalman filtering method by using the control module to obtain the optimal navigation information of the bionic robot fish.
Acquiring first initial navigation information through an inertial navigation module, decoding through a control module, and resolving according to the processed navigation information obtained through decoding through the inertial navigation module conveniently in the follow-up process to obtain one input variable of Kalman filtering; acquiring second initial navigation information through the Beidou positioning navigation module, and decoding through the control module, wherein the acquired second target navigation information is used as another input variable of Kalman filtering; based on a Kalman filtering method, kalman filtering is carried out according to first target navigation information and second target navigation information obtained by two positioning navigation modules, and the obtained optimal estimated optimal navigation information is obtained
The navigation positioning method for the bionic robot fish overcomes the defect of single navigation, avoids various errors such as temperature drift, accelerometer errors, accumulated errors of speed and displacement along with time and the like in a single inertial navigation module, and also avoids larger navigation errors caused by poor quality of received signals of the single Beidou positioning navigation module; the navigation method based on the combination of Beidou positioning navigation and inertial navigation can obtain the optimal estimated value of navigation information with higher precision by adopting a Kalman filter, and is extremely suitable for positioning navigation of bionic robot fish with changeable environment and smaller volume.
Preferably, the processing navigation information comprises quaternion navigation information and acceleration navigation information of the bionic robot fish under a carrier coordinate system, and the first target navigation information is specifically a speed vector of the bionic robot fish under a navigation coordinate system; the specific steps of S4 include:
s41: converting the quaternion navigation information into an Euler angle matrix according to a first formula;
the first formula specifically comprises:
wherein alpha, beta and gamma are yaw angle, pitch angle and roll angle in the Euler angle matrix respectively; q 0 、q 1 、q 2 and q3 Are all elements in the quaternion navigation information, q 0 、q 1 、q 2 and q3 Are all real numbers and satisfy
S42: according to a second formula, obtaining an acceleration transfer matrix between the carrier coordinate system and the navigation coordinate system according to the Euler angle matrix;
the second formula is specifically:
wherein ,b represents the carrier coordinate system and n represents the navigation coordinate system for the acceleration transfer matrix;
s43: according to the acceleration transfer matrix and a third formula, carrying out coordinate transformation on the acceleration navigation information to obtain target acceleration information of the bionic robot fish under the navigation coordinate system;
the third formula is specifically:
wherein ,an A for the target acceleration information b Navigation information for the acceleration in the carrier coordinate system;
s44: and carrying out integral operation on the target acceleration information to obtain the speed vector of the bionic robot fish under the navigation coordinate system.
Because the navigation information obtained by the inertial navigation module is the information of the carrier coordinate system relative to the inertial coordinate system, the quaternion navigation information is obtained through the decoding of the control module, the quaternion navigation information is converted according to the first formula and the second formula respectively, and the acceleration navigation information is converted according to the third formula, so that the navigation information obtained by the inertial navigation module and the navigation information obtained by the Beidou positioning navigation module are conveniently unified to the same coordinate system, namely the navigation coordinate system, and a Kalman filter is conveniently constructed based on the navigation information in the same coordinate system to obtain the optimal navigation information; according to the physical relationship between the speed and the acceleration, the acceleration navigation information obtained through decoding of the control module is convenient to obtain the integral operation according to the acceleration navigation information, the speed vector is obtained, and further the subsequent Kalman filtering is convenient.
Preferably, the second target navigation information is specifically a displacement vector of the biomimetic robotic fish under the navigation coordinate system, and the specific step of S5 includes:
s51: constructing a state vector of a Kalman filter according to the speed vector and the displacement vector, and obtaining a state equation and a measurement equation of the Kalman filter according to the state vector;
the expression of the state vector at any time is specifically: x= [ X ] 1 X 2 ] T ；
The expression of the state equation is specifically:
X k ＝[X 1,k X 2,k ] T ＝A[X 1,k-1 X 2,k-1 ] T +ω k-1 ；
the expression of the measurement equation is specifically:
Z k ＝[Z 1,k Z 2,k ] T ＝H[X 1,k X 2,k ] T +v k-1 ；
wherein X is a state vector at any time, X 1 and X2 The displacement vector and the velocity vector, X at any time point respectively k Is the state vector at time k, X 1,k and X2,k The displacement vector and the velocity vector at time k, respectively, [ X ] 1,k-1 X 2,k-1 ] T Is the state vector at time k-1, X 1,k-1 and X2,k-1 The displacement vector and the velocity vector at the moment k-1 respectively, A is a state transformation matrix, omega k-1 A noise error matrix at the moment k-1; z is Z k Observing vector for navigation information at k moment, Z 1,k and Z2,k Respectively, the displacement observation value and the velocity observation value at the moment k, H is an observation transformation matrix, v k-1 Measuring an error matrix for noise;
s52: obtaining a prediction equation of the Kalman filter according to the state vector, the state equation and the measurement equation;
the expression of the prediction equation is specifically:
wherein ,for the state estimate of time k to the next time,/->For the state estimate of time k-1 versus time k +.>State estimation value +.>Covariance matrix, P k-1 For the covariance matrix after the state estimation value at the moment k-1 is corrected, Q k-1 Noise error matrix omega for time k-1 k-1 Covariance matrix of A) T Is the transpose of the state transformation matrix A;
s53: and obtaining an update equation of the Kalman filter according to the state equation, the measurement equation and the prediction equation:
the expression of the update equation is specifically:
wherein ,the state estimation value after the correction at the moment k is specifically the optimal navigation information after Kalman filtering processing; k (K) k Kalman gain at time k, P k As the covariance matrix after the state estimation value at the moment k is corrected, H T To observe the transpose of the transformation matrix H, R k Error matrix v is measured for noise at time k k-1 Is a covariance matrix of (a);
s54: constructing the Kalman filter according to a prediction equation and the update equation; and obtaining the optimal navigation information according to the Kalman filter.
Constructing a state vector according to a speed vector corresponding to the inertial navigation module and a displacement vector corresponding to the Beidou positioning navigation module, obtaining a state equation, a measurement equation and a prediction equation required by Kalman filtering according to the state vector, facilitating the prediction of navigation information of the bionic robot fish, and respectively carrying out prediction iteration according to a displacement observation value and a speed observation value of the bionic robot fish at an initial moment to obtain a displacement estimation value and a speed estimation value; then, an update equation required by Kalman filtering is obtained according to the state equation, the measurement equation and the prediction equation, so that optimal estimation of a displacement estimated value and a velocity estimated value is conveniently realized, and optimal navigation information with higher precision is obtained; the Kalman filtering method is an optimization autoregressive data quantity method based on probability theory and numerical statistics, can effectively inhibit noise and other interference factors in the data acquisition process, better ensures a navigation mode based on a combination of Beidou positioning navigation and inertial navigation, and obtains an optimal estimated value of navigation information with higher precision.
Specifically, the inertial navigation module is specifically an MPU6050 model navigation module, and the Beidou positioning navigation module is specifically an ATGM332 model navigation module.
Preferably, when the system further comprises a wireless LORA module disposed within the biomimetic robotic fish, after S5 it comprises:
s6: and transmitting the optimal navigation information of the bionic robot fish to external equipment by using the wireless LORA module.
And the optimal navigation information is sent to the external equipment through the wireless LORA module, so that the expanded functions of data sharing, data remote transmission, data display and the like are conveniently realized, and the navigation positioning system is further perfected.
Specifically, in this embodiment, the ATGM332 navigation module may have a poor signal or the number of received satellites used may be less than or equal to 4, and at this time, the ATGM332 navigation module cannot normally perform positioning navigation, and then a navigation mode of single inertial navigation may be adopted as an emergency standby navigation positioning mode, that is, a displacement vector of the MPU6050 navigation module at the last moment and a velocity vector of the MPU6050 navigation module at the current moment are input into a constructed kalman filter in this embodiment, and the obtained optimal navigation information is sent to an upper computer through a wireless LORA module for display. The complete flowchart of the navigation positioning method including the emergency standby navigation positioning mode in this embodiment is shown in fig. 4.
Details of the embodiment I and the detailed descriptions of FIGS. 1-2 are not repeated here.

Claims (8)

1. The navigation positioning system for the bionic robot fish is characterized by comprising a control module, an inertial navigation module and a Beidou positioning navigation module which are all arranged in the bionic robot fish, wherein the control module is electrically connected with the inertial navigation module and the Beidou positioning navigation module respectively;
the inertial navigation module is used for positioning the bionic robot fish to obtain first initial navigation information of the bionic robot fish;
the Beidou positioning and navigation module is used for positioning the bionic robot fish to obtain second initial navigation information of the bionic robot fish;
the control module is used for acquiring the first initial navigation information, decoding the first initial navigation information and obtaining processed navigation information; the method is also used for acquiring the second initial navigation information, decoding the second initial navigation information and obtaining second target navigation information;
the inertial navigation module is also used for resolving according to the processed navigation information to obtain first target navigation information; the first target navigation information is specifically a speed vector of the bionic robot fish under a navigation coordinate system;
the control module is further used for carrying out Kalman filtering processing according to the first target navigation information and the second target navigation information based on a Kalman filtering method to obtain optimal navigation information of the bionic robot fish;
the second target navigation information is specifically a displacement vector of the bionic robot fish under the navigation coordinate system, and the control module is specifically configured to:
constructing a state vector of a Kalman filter according to the speed vector and the displacement vector, and obtaining a state equation and a measurement equation of the Kalman filter according to the state vector;
the expression of the state vector at any time is specifically: x= [ X ] 1 X 2 ] T ；
The expression of the state equation is specifically:
X k ＝[X 1,k X 2,k ] T ＝A[X 1,k-1 X 2,k-1 ] T +ω k-1 ；
the expression of the measurement equation is specifically:
Z k ＝[Z 1,k Z 2,k ] T ＝H[X 1,k X 2,k ] T +v k-1 ；
wherein X is a state vector at any time, X 1 and X2 The displacement vector and the velocity vector, X at any time point respectively k Is the state vector at time k, X 1,k and X2,k The displacement vector and the velocity vector at time k, respectively, [ X ] 1,k-1 X 2,k-1 ] T Is the state vector at time k-1, X 1,k-1 and X2,k-1 The displacement vector and the velocity vector at the moment k-1 respectively, A is a state transformation matrix, omega k-1 A noise error matrix at the moment k-1; z is Z k Observing vector for navigation information at k moment, Z 1,k and Z2,k Respectively, the displacement observation value and the velocity observation value at the moment k, H is an observation transformation matrix, v k-1 Measuring an error matrix for noise;
obtaining a prediction equation of the Kalman filter according to the state vector, the state equation and the measurement equation;
the expression of the prediction equation is specifically:
wherein ,for the state estimate of time k to the next time,/->For the state estimate of time k-1 versus time k +.>State estimation value +.>Covariance matrix, P k-1 For the covariance matrix after the state estimation value at the moment k-1 is corrected, Q k-1 Noise error matrix omega for time k-1 k-1 Covariance matrix of A) T Is the transpose of the state transformation matrix A;
and obtaining an update equation of the Kalman filter according to the state equation, the measurement equation and the prediction equation:
the expression of the update equation is specifically:
wherein ,the state estimation value after the correction at the moment k is specifically the optimal navigation information after Kalman filtering processing; k (K) k When k isKalman gain, P k As the covariance matrix after the state estimation value at the moment k is corrected, H T To observe the transpose of the transformation matrix H, R k Error matrix v is measured for noise at time k k-1 Is a covariance matrix of (a);
constructing the Kalman filter according to a prediction equation and the update equation; and obtaining the optimal navigation information according to the Kalman filter.
2. The navigation and positioning system for a biomimetic robotic fish according to claim 1, wherein the processed navigation information includes quaternion navigation information and acceleration navigation information of the biomimetic robotic fish in a carrier coordinate system, and the inertial navigation module is specifically configured to:
converting the quaternion navigation information into an Euler angle matrix according to a first formula;
the first formula specifically comprises:
wherein alpha, beta and gamma are yaw angle, pitch angle and roll angle in the Euler angle matrix respectively, q 0 、q 1 、q 2 and q3 Are all elements in the quaternion navigation information, q 0 、q 1 、q 2 and q3 Are all real numbers and satisfy
According to a second formula, obtaining an acceleration transfer matrix between the carrier coordinate system and the navigation coordinate system according to the Euler angle matrix;
the second formula is specifically:
wherein ,b represents the carrier coordinate system and n represents the navigation coordinate system for the acceleration transfer matrix;
according to the acceleration transfer matrix and a third formula, carrying out coordinate transformation on the acceleration navigation information to obtain target acceleration information of the bionic robot fish under the navigation coordinate system;
the third formula is specifically:
wherein ,an A for the target acceleration information b Navigation information for the acceleration in the carrier coordinate system;
and carrying out integral operation on the target acceleration information to obtain the speed vector of the bionic robot fish under the navigation coordinate system.
3. The navigation and positioning system for a biomimetic robotic fish according to claim 1, wherein the inertial navigation module is specifically an MPU6050 model navigation module, and the beidou positioning navigation module is specifically an ATGM332 model navigation module.
4. The navigation and positioning system for a biomimetic robotic fish of claim 1, further comprising a power module and a wireless LORA module both disposed within the biomimetic robotic fish, the power module being electrically connected to the control module, the inertial navigation module, the beidou positioning navigation module and the wireless LORA module, respectively, the wireless LORA module being electrically connected to the control module;
the power supply is used for supplying power to the control module, the inertial navigation module, the Beidou positioning navigation module and the wireless LORA module respectively;
the wireless LORA module is used for sending the optimal navigation information of the bionic robot fish to external equipment.
5. A navigation positioning method for a biomimetic robotic fish, characterized in that the navigation positioning system for a biomimetic robotic fish according to any one of claims 1 to 4 is used for navigation positioning, comprising the steps of:
positioning the bionic robot fish by utilizing an inertial navigation module to obtain first initial navigation information of the bionic robot fish;
positioning the bionic robot fish by using a Beidou positioning navigation module to obtain second initial navigation information of the bionic robot fish;
decoding the first initial navigation information by using a control module to obtain processed navigation information; decoding the second initial navigation information to obtain second target navigation information;
the inertial navigation module is utilized to calculate according to the processed navigation information, so as to obtain first target navigation information; the first target navigation information is specifically a speed vector of the bionic robot fish under a navigation coordinate system;
performing Kalman filtering processing according to the first target navigation information and the second target navigation information based on a Kalman filtering method by using the control module to obtain optimal navigation information of the bionic robot fish;
the second target navigation information is specifically a displacement vector of the bionic robot fish under the navigation coordinate system, and the control module is specifically configured to:
constructing a state vector of a Kalman filter according to the speed vector and the displacement vector, and obtaining a state equation and a measurement equation of the Kalman filter according to the state vector;
the expression of the state vector at any time is specifically: x= [ X ] 1 X 2 ] T ；
The expression of the state equation is specifically:
X k ＝[X 1,k X 2,k ] T ＝A[X 1,k-1 X 2,k-1 ] T +ω k-1 ；
the expression of the measurement equation is specifically:
Z k ＝[Z 1,k Z 2,k ] T ＝H[X 1,k X 2,k ] T +v k-1 ；
wherein X is a state vector at any time, X 1 and X2 The displacement vector and the velocity vector, X at any time point respectively k Is the state vector at time k, X 1,k and X2,k The displacement vector and the velocity vector at time k, respectively, [ X ] 1,k- 1 X 2,k-1 ] T Is the state vector at time k-1, X 1,k-1 and X2,k-1 The displacement vector and the velocity vector at the moment k-1 respectively, A is a state transformation matrix, omega k-1 A noise error matrix at the moment k-1; z is Z k Observing vector for navigation information at k moment, Z 1,k and Z2,k Respectively, the displacement observation value and the velocity observation value at the moment k, H is an observation transformation matrix, v k-1 Measuring an error matrix for noise;
obtaining a prediction equation of the Kalman filter according to the state vector, the state equation and the measurement equation;
the expression of the prediction equation is specifically:
wherein ,for the state estimate of time k to the next time,/->For the state estimate of time k-1 versus time k +.>State estimation value +.>Covariance matrix, P k-1 For the covariance matrix after the state estimation value at the moment k-1 is corrected, Q k-1 Noise error matrix omega for time k-1 k-1 Covariance matrix of A) T Is the transpose of the state transformation matrix A;
and obtaining an update equation of the Kalman filter according to the state equation, the measurement equation and the prediction equation:
the expression of the update equation is specifically:
wherein ,the state estimation value after the correction at the moment k is specifically the optimal navigation information after Kalman filtering processing; k (K) k Kalman gain at time k, P k As the covariance matrix after the state estimation value at the moment k is corrected, H T To observe the transpose of the transformation matrix H, R k Error matrix v is measured for noise at time k k-1 Is a covariance matrix of (a);
constructing the Kalman filter according to a prediction equation and the update equation; and obtaining the optimal navigation information according to the Kalman filter.
6. The navigation positioning method for a biomimetic robotic fish according to claim 5, wherein the processed navigation information comprises quaternion navigation information and acceleration navigation information of the biomimetic robotic fish in a carrier coordinate system, and the first target navigation information is specifically a velocity vector of the biomimetic robotic fish in a navigation coordinate system; the specific step of obtaining the first target navigation information includes:
converting the quaternion navigation information into an Euler angle matrix according to a first formula;
the first formula specifically comprises:
wherein alpha, beta and gamma are yaw angle, pitch angle and roll angle in the Euler angle matrix respectively; q 0 、q 1 、q 2 and q3 Are all elements in the quaternion navigation information, q 0 、q 1 、q 2 and q3 Are all real numbers and satisfy
According to a second formula, obtaining an acceleration transfer matrix between the carrier coordinate system and the navigation coordinate system according to the Euler angle matrix;
the second formula is specifically:
wherein ,b represents the carrier coordinate system and n represents the navigation coordinate system for the acceleration transfer matrix;
according to the acceleration transfer matrix and a third formula, carrying out coordinate transformation on the acceleration navigation information to obtain target acceleration information of the bionic robot fish under the navigation coordinate system;
the third formula is specifically:
wherein ,an A for the target acceleration information b Navigation information for the acceleration in the carrier coordinate system;
and carrying out integral operation on the target acceleration information to obtain the speed vector of the bionic robot fish under the navigation coordinate system.
7. The navigation positioning method for a biomimetic robotic fish according to claim 6, wherein the specific step of obtaining the optimal navigation information is performed by using the second target navigation information specifically as a displacement vector of the biomimetic robotic fish in the navigation coordinate system, and includes:
constructing a state vector of a Kalman filter according to the speed vector and the displacement vector, and obtaining a state equation and a measurement equation of the Kalman filter according to the state vector;
the expression of the state vector at any time is specifically: x= [ X ] 1 X 2 ] T ；
The expression of the state equation is specifically:
X k ＝[X 1,k X 2,k ] T ＝A[X 1,k-1 X 2,k-1 ] T +ω k-1 ；
the expression of the measurement equation is specifically:
Z k ＝[Z 1,k Z 2,k ] T ＝H[X 1,k X 2,k ] T +v k-1 ；
wherein X is a state vector at any time, X 1 and X2 The displacement vector and the velocity vector, X at any time point respectively k Is the state vector at time k, X 1,k and X2,k The displacement vector and the velocity vector at time k, respectively, [ X ] 1,k-1 X 2,k-1 ] T Is the state vector at time k-1, X 1,k-1 and X2,k-1 The displacement vector and the velocity vector at the moment k-1 respectively, A is a state transformation matrix, omega k-1 A noise error matrix at the moment k-1; z is Z k Observing vector for navigation information at k moment, Z 1,k and Z2,k Respectively, the displacement observation value and the velocity observation value at the moment k, H is an observation transformation matrix, v k-1 Measuring an error matrix for noise;
obtaining a prediction equation of the Kalman filter according to the state vector, the state equation and the measurement equation;
the expression of the prediction equation is specifically:
wherein ,for the state estimate of time k to the next time,/->For the state estimate of time k-1 versus time k +.>State estimation value +.>Covariance matrix, P k-1 For the covariance matrix after the state estimation value at the moment k-1 is corrected, Q k-1 Noise error matrix omega for time k-1 k-1 Covariance matrix of A) T Is the transpose of the state transformation matrix A;
and obtaining an update equation of the Kalman filter according to the state equation, the measurement equation and the prediction equation:
the expression of the update equation is specifically:
wherein ,the state estimation value after the correction at the moment k is specifically the optimal navigation information after Kalman filtering processing; k (K) k Kalman gain at time k, P k As the covariance matrix after the state estimation value at the moment k is corrected, H T To observe the transpose of the transformation matrix H, R k Error matrix v is measured for noise at time k k-1 Is a covariance matrix of (a);
constructing the Kalman filter according to a prediction equation and the update equation; and obtaining the optimal navigation information according to the Kalman filter.
8. The navigation positioning method for a biomimetic robotic fish according to any one of claims 5 to 7, wherein the inertial navigation module is specifically an MPU6050 model navigation module, and the beidou positioning navigation module is specifically an ATGM332 model navigation module.
CN202110767838.3A 2021-07-07 2021-07-07 Navigation positioning method and system for bionic robot fish Active CN113639744B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110767838.3A CN113639744B (en) 2021-07-07 2021-07-07 Navigation positioning method and system for bionic robot fish

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110767838.3A CN113639744B (en) 2021-07-07 2021-07-07 Navigation positioning method and system for bionic robot fish

Publications (2)

Publication Number Publication Date
CN113639744A CN113639744A (en) 2021-11-12
CN113639744B true CN113639744B (en) 2023-10-20

Family

ID=78416836

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110767838.3A Active CN113639744B (en) 2021-07-07 2021-07-07 Navigation positioning method and system for bionic robot fish

Country Status (1)

Country Link
CN (1) CN113639744B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116592896B (en) * 2023-07-17 2023-09-29 山东水发黄水东调工程有限公司 Underwater robot navigation positioning method based on Kalman filtering and infrared thermal imaging

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20080084435A (en) * 2007-03-16 2008-09-19 한국전자통신연구원 Position estimation method, system and recorded media of mobile system using odometer and heading sensor
CN102608642A (en) * 2011-01-25 2012-07-25 北京七维航测科技股份有限公司 Beidou/inertial combined navigation system
CN106932804A (en) * 2017-03-17 2017-07-07 南京航空航天大学 Inertia/the Big Dipper tight integration navigation system and its air navigation aid of astronomy auxiliary
CN107290742A (en) * 2017-06-20 2017-10-24 武汉理工大学 Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN109443353A (en) * 2018-12-25 2019-03-08 中北大学 Vision based on fuzzy self-adaption ICKF-inertia close coupling Combinated navigation method
CN109709513A (en) * 2019-01-25 2019-05-03 中广核研究院有限公司 A kind of indoor application is in nuclear power station localization method and system
CN110780326A (en) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 Vehicle-mounted integrated navigation system and positioning method
CN111007557A (en) * 2019-12-10 2020-04-14 中国矿业大学 Adaptive kinematics model assisted GNSS carrier phase and Doppler fusion speed measurement method
CN111351482A (en) * 2020-03-19 2020-06-30 南京理工大学 Multi-rotor aircraft integrated navigation method based on error state Kalman filtering

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20080084435A (en) * 2007-03-16 2008-09-19 한국전자통신연구원 Position estimation method, system and recorded media of mobile system using odometer and heading sensor
CN102608642A (en) * 2011-01-25 2012-07-25 北京七维航测科技股份有限公司 Beidou/inertial combined navigation system
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN106932804A (en) * 2017-03-17 2017-07-07 南京航空航天大学 Inertia/the Big Dipper tight integration navigation system and its air navigation aid of astronomy auxiliary
CN107290742A (en) * 2017-06-20 2017-10-24 武汉理工大学 Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system
CN109443353A (en) * 2018-12-25 2019-03-08 中北大学 Vision based on fuzzy self-adaption ICKF-inertia close coupling Combinated navigation method
CN109709513A (en) * 2019-01-25 2019-05-03 中广核研究院有限公司 A kind of indoor application is in nuclear power station localization method and system
CN110780326A (en) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 Vehicle-mounted integrated navigation system and positioning method
CN111007557A (en) * 2019-12-10 2020-04-14 中国矿业大学 Adaptive kinematics model assisted GNSS carrier phase and Doppler fusion speed measurement method
CN111351482A (en) * 2020-03-19 2020-06-30 南京理工大学 Multi-rotor aircraft integrated navigation method based on error state Kalman filtering

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于卡尔曼滤波的INS/USBL水下导航系统模型研究;李守军;陶春辉;包更生;;海洋技术(第03期);47-50 *
基于自适应卡尔曼滤波的NGMIMU/GPS组合导航设计;丁明理;王祁;殷栩;;哈尔滨工业大学学报(第01期);67-70 *

Also Published As

Publication number Publication date
CN113639744A (en) 2021-11-12

Similar Documents

Publication Publication Date Title
CN104457754B (en) SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method
CN104656058B (en) Distributed multiple mobile node co-positioned system
CN106980133A (en) The GPS INS Combinated navigation methods and system for being compensated and being corrected using neural network algorithm
CN102052923B (en) Small-sized underwater robot combined navigation system and navigation method
US11686744B2 (en) Posture estimation method, posture estimation device, and vehicle
CN104422948A (en) Embedded type combined navigation system and method thereof
CN108387236B (en) Polarized light SLAM method based on extended Kalman filtering
CN103941274B (en) Navigation method and terminal
CN108151737A (en) A kind of unmanned plane bee colony collaborative navigation method under the conditions of the mutual observed relationships of dynamic
CN112197765B (en) Method for realizing fine navigation of underwater robot
CN113252041B (en) Combined navigation method suitable for small underwater robot
CN109739088B (en) Unmanned ship finite time convergence state observer and design method thereof
CN113129377B (en) Three-dimensional laser radar rapid robust SLAM method and device
CN113639744B (en) Navigation positioning method and system for bionic robot fish
AU2020104248A4 (en) Integrated gps and inertial navigation system for industrial robots
CN108801250B (en) Real-time attitude acquisition method and device based on underwater robot
CN116295511A (en) Robust initial alignment method and system for pipeline submerged robot
CN104280024B (en) Device and method for integrated navigation of deepwater robot
CN111238469A (en) Unmanned aerial vehicle formation relative navigation method based on inertia/data chain
CN110954080A (en) Magnetic compass calibration method for eliminating carrier magnetic interference
CN111982126B (en) Design method of full-source BeiDou/SINS elastic state observer model
CN206095257U (en) Integrated navigation system of robot is tourd to intelligence
CN2788131Y (en) Mini-type underwater self-combination navigation equipment
Song et al. Hierarchical underwater localization in dominating background flow fields
CN110954081A (en) Quick calibration device and method for magnetic compass

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