CN117053782A - Combined navigation method for amphibious robot - Google Patents
Combined navigation method for amphibious robot Download PDFInfo
- Publication number
- CN117053782A CN117053782A CN202311022091.4A CN202311022091A CN117053782A CN 117053782 A CN117053782 A CN 117053782A CN 202311022091 A CN202311022091 A CN 202311022091A CN 117053782 A CN117053782 A CN 117053782A
- Authority
- CN
- China
- Prior art keywords
- state
- observation
- equation
- gnss
- amphibious robot
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 239000011159 matrix material Substances 0.000 claims abstract description 51
- 230000008569 process Effects 0.000 claims abstract description 16
- 238000001914 filtration Methods 0.000 claims abstract description 8
- 230000001133 acceleration Effects 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 13
- 238000005259 measurement Methods 0.000 claims description 11
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 9
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000005295 random walk Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 4
- 238000009795 derivation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000009472 formulation Methods 0.000 description 1
- 238000007429 general method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 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/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
Abstract
The invention belongs to the field of navigation and positioning, and particularly relates to an amphibious robot combined navigation method. The method comprises the following steps: (1) navigation standing initialization: estimating initial noise and state of the system by using an IMU, a DVL and a GNSS arranged on the amphibious robot; (2) nominal state prediction: using a median integral result of the IMU as a nominal state of the amphibious robot navigation process; (3) error state prediction: calculating a discrete kinematic equation and a prediction covariance matrix of an error state by adopting error state Kalman filtering; (4) observing an update: the DVL, pressure depth gauge and GNSS are used to construct the observation equations and the observation covariance matrix for correcting the nominal state. When the amphibious robot sails, the response speed is higher, the operation load is smaller, and the obtained pose estimation is more accurate.
Description
Technical Field
The invention belongs to the field of navigation and positioning, and particularly relates to an amphibious robot combined navigation method.
Background
Along with the development of science and technology, amphibious robots are increasingly widely applied to the fields of military, environmental monitoring, disaster relief and the like. In order to realize high-precision and high-stability navigation of the amphibious robot in a complex environment, researchers have conducted intensive research on a combined navigation technology. The integrated navigation technology mainly improves navigation accuracy and stability by integrating information of various sensors, such as an Inertial Navigation System (INS), a Global Positioning System (GPS), a geomagnetic sensor and the like.
The existing amphibious robot combined navigation technology is mainly based on an Extended Kalman Filter (EKF) method, the navigation accuracy of the method is improved to a certain extent, and some problems still exist. The EKF takes a long time because it computes the jacobian calculation every time the filter optimization is performed, and also causes a loss in accuracy due to the omission of the second order term.
Error State Kalman Filtering (ESKF) is based on EKF, is generally used for a navigation positioning system of an unmanned vehicle, a land area is generally provided with GNSS sensors, and the accumulated error of positioning is prevented from being overlarge, but in water, GNSS signals fail, the traditional ESKF only considers the states of a current frame and a previous frame, and under the condition that global observation limitation is not available, the errors are continuously accumulated, so that the positioning error of an amphibious robot becomes large along with the increase of a range.
Disclosure of Invention
The invention aims to provide an amphibious robot combined navigation method, which realizes the pose prediction of the amphibious robot and improves the precision and efficiency of a combined navigation system.
The technical solution for realizing the purpose of the invention is as follows: an amphibious robot combined navigation method comprises the following steps:
step (1): navigation standing initialization: estimating initial noise and state of the system by using an IMU, a DVL and a GNSS arranged on the amphibious robot;
step (2): nominal state prediction: using a median integral result of the IMU as a nominal state of the amphibious robot navigation process;
step (3): error state prediction: calculating a discrete kinematic equation and a prediction covariance matrix of an error state by adopting error state Kalman filtering;
step (4): and (5) observing and updating: the DVL, pressure depth gauge and GNSS are used to construct the observation equations and the observation covariance matrix for correcting the nominal state.
Further, the navigation standing initialization in the step (1) specifically includes the following steps:
step (11): estimating initial noise of the system by using the IMU;
step (12): estimating the position of the integrated navigation system by using the GNSS, and converting the coordinate information measured by the GNSS into a northeast coordinate system ENU used for navigation;
step (13): and the magnetometer and the attitude sensor integrated in the DVL are used for measuring the rotation vector of the robot to obtain the initial rotation matrix of the amphibious robot.
Further, the step (11) specifically comprises:
standing the amphibious robot on the water surface, receiving the angular velocity and acceleration output of the IMU in the standing period, taking the average value, and respectively recording asAnd->Initial gyroscope and accelerometer zero bias b as IMU g And b a The method comprises the steps of carrying out a first treatment on the surface of the The measurement equation of the accelerometer is
Wherein g represents gravitational acceleration, eta a As a noise item of the accelerometer, when the actual acceleration a of the amphibious robot is zero, the rotation matrix R=I from the machine body coordinate system to the world coordinate system, and the accelerometer actually measures b a G, neglecting small amounts b a The modulus length of g is regarded as a fixed value, and the orientation isA vector of size 9.81 as a gravity vector, thereby initializing a gravity term;
the step (12) is specifically as follows:
estimating the position of the system by using GNSS, converting the coordinates measured by the GNSS under the longitude and latitude high coordinate system into a coordinate system used for navigation, namely a northeast coordinate system, and recording the real-time coordinates P of the amphibious robot measured by the GNSS 1 ＝[lat,lon,height]Initial coordinate P 0 ＝[lat 0 ,lon 0 ,height 0 ]Wherein the first time period of the last,lon, height represent longitude, latitude, and altitude of the target point, respectively;
firstly, converting into an geocentric geodetic fixed coordinate system ECEF, wherein ECEF coordinates P 2 ＝[x,y,z]The conversion relationship is as follows:
wherein, the earth is regarded as an ellipsoid, a is a long half shaft, b is a short half shaft,
converting ECEF coordinates into a northeast coordinate system, and recording a target point under the northeast coordinate system as P 3 ＝[E,N,U]At P 1 A rotation matrix R at 1 The method comprises the following steps:
through the calculation, the initial coordinate of the GNSS is set as the origin of the northeast day coordinate system, and the subsequent position observation is obtained;
the step (13) comprises the following steps:
measuring a rotation vector of the amphibious robot through DVL internal integrated magnetometer and attitude sensorAccording to the methodDergas formula>θ is the modular length of the rotation vector, +.>Obtaining an initial rotation matrix of the amphibious robot for the direction of the rotation vector, wherein
Further, the step (2) specifically includes the following steps:
step (21): the state variable of the integrated navigation system is set as a 15-dimensional column vector:
X＝[p,v,R,b g ,b a ,g] · (36)
wherein, p represents displacement in the three-axis direction, v represents three-axis speed, and R represents a rotation matrix from the amphibious robot system to the world system;
step (22): the equation of motion under continuous time is calculated as:
the measurement equation of the IMU is calculated as:
wherein the method comprises the steps ofAnd->Representing measured acceleration and angular velocity, η of the IMU a And eta g Respectively representing velocity random walk and angle walk; zero bias modeling is:
wherein t represents the time of the moment, and t' represents the time of the last moment;
step (23): sorting (9) (10) (11) to obtain a motion equation of the state variable nominal value of the IMU under continuous time, wherein the motion equation is as follows:
discretizing the nominal state to obtain a prediction equation of the nominal state as follows:
wherein Δt represents the time interval of each sampling, the measurement value of the IMU adopts median integration, and the angular velocity and the acceleration at the present time and the next time are respectively averaged to be the measurement value at the present time.
Further, the step (3) specifically includes the following steps:
step (31): calculating transfer equations of all state errors by adopting error state Kalman filtering, and defining true state X of ESKF t ＝[p t ,v t ,R t ,b at ,b gt ,g t ] · From equation (12), the relationship between the state variable derivative with respect to the observed quantity can be found:
defining the calculation of the error state variables is:
here the delta symbol represents the error of the nominal state from the true value, where δr of the rotated part can be represented by its lie algebra exp (δθ #) and the rotation matrix is updated with the left perturbation model, the derivative of the error state with time is represented as follows:
deriving the two sides of the formula (15) respectively, and introducing the formulas (12) (14) (16) to the two sides of the derived equal sign, the kinematic equation of the error state variable can be obtained as follows:
also discretizing the above formula (17) to obtain a discrete kinematic equation of the error state as follows:
step (32): calculating a covariance matrix in a prediction process, and obtaining a state transition matrix according to a model of a Kalman filter, wherein the state transition matrix is as follows:
the state noise covariance matrix Q is calculated as follows:
Q＝diag(0 3 ,cov(η v ),cov(η θ ),cov(η g ),cov(η a ),0 3 ) (48)
from equations (18) (19) (20), the error state prediction process may be derived as:
wherein δx pred Indicating the error state before the update at the present time, P pred The state process covariance matrix before updating at this time is represented by P, which is the state process covariance matrix at the previous time, and the error state δx is set to 0 after each update.
Further, the step (4) includes the steps of:
step (41): the DVL is used to construct the observation equation, the basic form of which is as follows:
Z＝h(x)+υ，υ～N(0，V) (50)
wherein Z is observation data, V is observation noise, and V is covariance matrix of the observation noise;
calculating a jacobian matrix of the observation equation for the error state:
wherein,
the rotation observation equation for DVL is calculated as:
R DVL ＝R·exp(δθ∧) (53)
wherein R is DVL Is a direct observation of δθ, and therefore:
Z δθ ＝h(δθ)＝Log(R · R DVL ) (54)
the jacobian calculation of the observation equation for δθ is:
step (42) using GNSS to construct an observation equation, observing the position state, and calculating as follows:
Z δp ＝h(δp)＝p gnss -p (56)
the observation equation is a jacobian unit array of state quantity;
step (43): an observation equation is constructed by using a pressure depth gauge, the displacement of the z axis is observed, when the amphibious robot submerges into the water, the GNSS signal fails, and the observation sensor at the moment is provided with a DVL and the pressure depth gauge, so that the state constraint is carried out on the amphibious robot, and the track accuracy is ensured;
step (44): correcting the predicted nominal state by using the error state after observation and updating to obtain the final nominal state as the output of the system:
compared with the prior art, the invention has the remarkable advantages that:
the form and calculation of the jacobian matrix are relatively simpler, and meanwhile, as the error state is small, the influence on the system precision is small by neglecting small second-order terms, so that the system precision and the operation efficiency are improved; meanwhile, the method is improved on the basis of the traditional ESKF, the attitude variable adopts a representation form of a rotation matrix, the error state of the attitude is multiplied to the nominal state, the derivation discovers that the error state of the attitude updates the measured value of the angular velocity, and the positioning error caused by overlarge angular velocity noise is avoided, so that the accumulated error of the combined navigation of the amphibious robot is further restrained.
Drawings
FIG. 1 is a flow chart of integrated navigation according to an embodiment of the present invention.
FIG. 2 is a comparison of position errors in an embodiment of the present invention.
Detailed Description
The invention is described in further detail below with reference to the accompanying drawings.
According to fig. 1, the invention uses IMU, GNSS, DVL and pressure depth meter on amphibious robot to make initial estimation of filter parameters based on error state Kalman filter, to determine initial zero bias, position and attitude parameters of combined navigation. When the amphibious robot sails, the median integral result of the IMU is used as the nominal state of the filter, under the water environment, the GNSS fails, the observation sensor is a DVL and a pressure depth gauge, in the land environment, the DVL and the pressure depth gauge fail, the observation sensor is a GNSS, when the amphibious robot sails on the water surface, the sensors are effective, meanwhile, the error state is corrected through observation by using the observation information of the sensors, and the corrected error state is used for updating the nominal state so as to restrain the combined navigation pose of the robot and inhibit the divergence of the robot.
An amphibious robot integrated navigation method, the method comprising the steps of:
navigation standing initialization step: estimating initial noise and state of the system using an IMU (inertial measurement unit), a DVL (doppler velocimeter) and a GNSS (global navigation satellite system) provided on the amphibious robot;
nominal state prediction step: using a median integral result of the IMU as a nominal state of the amphibious robot navigation process;
and an error state prediction step: calculating a discrete kinematic equation and a prediction covariance matrix of an error state by adopting error state Kalman filtering;
and (3) observing and updating: the DVL, pressure depth gauge and GNSS are used to construct the observation equations and the observation covariance matrix for correcting the nominal state.
The navigation standing initialization step comprises the following steps:
step 1) estimating initial noise of a system by using an IMU, standing the amphibious robot on the water surface, receiving the angular speed and the acceleration output of the IMU in the period of time, taking an average value, and respectively recording asAnd->Initial gyroscope and accelerometer zero bias b as IMU g And b a The method comprises the steps of carrying out a first treatment on the surface of the The measurement equation of the accelerometer is
Wherein g represents gravitational acceleration, eta a As a noise item of the accelerometer, when the actual acceleration a of the amphibious robot is zero, the rotation matrix R=I from the machine system to the world coordinate system, and the accelerometer actually measures b a G, neglecting small amounts b a The mode length of g can be regarded as a fixed value, and the direction isA vector of size 9.81 acts as a gravity vector to initialize the gravity term.
Step 2) estimating the position of the integrated navigation system by using GNSS, converting the position of the integrated navigation system measured by the GNSS into coordinates under LLA coordinate system, namely northeast-sky coordinate system (ENU), and recording real-time coordinates P of the amphibious robot measured by the GNSS 1 ＝[lat,lon,height]Initial coordinate P 0 ＝[lat 0 ,lon 0 ,height 0 ]Wherein lat, lon, height represent longitude, latitude, and altitude, respectively, of the target point;
firstly, converting the three-dimensional coordinate into a geocentric geodetic fixed coordinate system ECEF, wherein ECEF coordinates P 2 ＝[x,y,z]The conversion relationship is as follows:
wherein, the earth is regarded as an ellipsoid, a is a long half shaft, b is a short half shaft,
converting ECEF coordinates into northeast coordinate system, and recording target point under east ENU as P 3 ＝[E,N,U]At P 1 A rotation matrix R at 1 The method comprises the following steps:
through the calculation, the GNSS initial coordinate is set as the origin of the northeast coordinate system, and the subsequent position observation can be obtained;
step 3) integrating a magnetometer and a gesture sensor in the DVL to measure the rotation vector of the amphibious robotAccording to the formula of lode->θ is the modular length of the rotation vector, +.>Is the direction of the rotation vector, thereby obtaining an initial rotation matrix of the amphibious robot, wherein
Further, the nominal state prediction step includes the steps of:
step 1) setting the state variable of the integrated navigation system as a 15-dimensional column vector:
X＝[p,v,R,b g ,b a ,g] · (8)
wherein, p represents displacement in three-axis direction, v represents three-axis speed, R represents rotation matrix from amphibious robot system to world system, different from general method, the attitude of filter is processed by using Liqun lie algebra expression method, high-latitude column vector x falls on high-dimensional manifold, called Kalman filter on manifold, the processing mode makes calculation form more concise;
the equation of motion under the continuous time of step 2) is calculated as follows:
the measurement equation of the IMU is calculated as:
wherein the method comprises the steps ofAnd->Representing measured acceleration and angular velocity, η of the IMU a And eta g Respectively representing velocity random walk and angle walk; on the other hand, the zero bias in the normal case can be modeled as:
wherein t represents the present time and t' represents the last time.
Step 3) finishing the formulas (9) (10) (11) to obtain the motion equation of the state variable nominal value of the IMU under continuous time, wherein the motion equation is as follows:
discretizing the nominal state to obtain a prediction equation of the nominal state as follows:
wherein deltat represents the sampling time interval, the measured value of the IMU adopts median integration, and the angular velocity and the acceleration at the moment and the next moment are respectively averaged to be used as the measured value at the moment, so that the estimation result is more accurate;
further, the error state prediction step includes the steps of:
step 1) adopting error state Kalman filtering, calculating a transfer equation of each state error, and defining a true state X of ESKF t ＝[p t ,v t ,R t ,b at ,b gt ,g t ] · From equation (12), the relationship between the state variable derivative with respect to the observed quantity can be found:
defining the calculation of the error state variables is:
where δ represents the error of the nominal state from the true value, δr of the rotating part may be represented by its lie algebra exp (δθΛ), and updating the rotation matrix with the left perturbation model, and the derivative of the error state with time may be represented as follows:
deriving the two sides of the formula (15) respectively, and introducing the formulas (12) (14) (16) to the two sides of the derived equal sign, the kinematic equation of the error state variable can be obtained as follows:
also discretizing the above formula (17) to obtain a discrete kinematic equation of the error state as follows:
step 2) calculating a covariance matrix in the prediction process, and obtaining a state transition matrix according to a model of a Kalman filter, wherein the state transition matrix is as follows:
the state noise covariance matrix Q is calculated as follows:
Q＝diag(0 3 ,cov(η v ),cov(η θ ),cov(η g ),cov(η a ),0 3 ) (20)
from equations (18) (19) (20), the error state prediction process may be derived as:
wherein δx pred Indicating the error state before the update at the present time, P pred Representing a state process covariance matrix before updating at the moment, wherein P is a state process covariance matrix at the last moment, and an error state delta x is set to be 0 after each updating;
further, the observation updating step includes the steps of:
step 1) constructing an observation equation by using DVL, wherein the basic form of the observation equation is as follows:
Z＝h(x)+υ，υ～N(0，V) (22)
wherein Z is observation data, V is observation noise, and V is covariance matrix of the observation noise;
calculating a jacobian matrix of the observation equation for the error state:
wherein,
the rotation observation equation for DVL is calculated as:
R DVL ＝R·exp(δθ∧) (25)
wherein R is DVL Is a direct observation of δθ, therefore we have:
Z δθ ＝h(δθ)＝Log(R · R DVL ) (26)
the jacobian calculation of the observation equation for δθ is:
step 2) constructing an observation equation by using GNSS, and observing the position state, wherein the calculation is as follows:
Z δp ＝h(δp)＝p gnss -p (28)
the observation equation is a jacobian unit array of state quantity;
and 3) constructing an observation equation by using a pressure depth gauge, observing the displacement of the z axis, and when the amphibious robot submerges into the water, enabling the GNSS signals to fail, wherein the observation sensor is provided with a DVL and the pressure depth gauge, so that the state constraint is carried out on the amphibious robot, and the accuracy of the track is ensured.
And 4) correcting the predicted nominal state by using the error state after observation and updating to obtain the final nominal state as the output of the system:
p t ＝p+δp,
v t ＝v+δv,
R t ＝δR·R
b gt ＝b g +δb g ,
b at ＝b a +δb a ,
g t ＝g+δg.
according to fig. 2, data acquired by the sensor are imported into a filter, and after the filter is optimized, the estimated values of the pose of each axis are output and compared with true values, so as to make a track error comparison graph.
The above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention, but various embodiments are possible for those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (8)
1. An amphibious robot combined navigation method is characterized by comprising the following steps:
step (1): navigation standing initialization: estimating initial noise and state of the system by using an IMU, a DVL and a GNSS arranged on the amphibious robot;
step (2): nominal state prediction: using a median integral result of the IMU as a nominal state of the amphibious robot navigation process;
step (3): error state prediction: calculating a discrete kinematic equation and a prediction covariance matrix of an error state by adopting error state Kalman filtering;
step (4): and (5) observing and updating: the DVL, pressure depth gauge and GNSS are used to construct the observation equations and the observation covariance matrix for correcting the nominal state.
2. The method of claim 1, wherein the navigation rest initialization of step (1) specifically comprises the steps of:
step (11): estimating initial noise of the system by using the IMU;
step (12): estimating the position of the integrated navigation system by using the GNSS, and converting the coordinate information measured by the GNSS into a northeast coordinate system ENU used for navigation;
step (13): and the magnetometer and the attitude sensor integrated in the DVL are used for measuring the rotation vector of the robot to obtain the initial rotation matrix of the amphibious robot.
3. The method according to claim 2, wherein step (11) is specifically:
standing the amphibious robot on the water surface, receiving the angular velocity and acceleration output of the IMU in the standing period, taking the average value, and respectively recording asAnd->Initial gyroscope and accelerometer zero bias b as IMU g And b a The method comprises the steps of carrying out a first treatment on the surface of the The measurement equation of the accelerometer is
Wherein g represents gravitational acceleration, eta a As a noise item of the accelerometer, when the actual acceleration a of the amphibious robot is zero, the rotation matrix R=I from the machine body coordinate system to the world coordinate system, and the accelerometer actually measures b a G, neglecting small amounts b a The modulus length of g is regarded as a fixed value, and the orientation isA vector of size 9.81 acts as a gravity vector to initialize the gravity term.
4. A method according to claim 3, wherein step (12) is specifically:
estimating the position of the system by using GNSS, converting the coordinates measured by the GNSS under the longitude and latitude high coordinate system into a coordinate system used for navigation, namely a northeast coordinate system, and recording the real-time coordinates P of the amphibious robot measured by the GNSS 1 ＝[lat,lon,height]Initial coordinatesP 0 ＝[lat 0 ,lon 0 ,height 0 ]Wherein lat, lon, height represent longitude, latitude, and altitude, respectively, of the target point;
firstly, converting into an geocentric geodetic fixed coordinate system ECEF, wherein ECEF coordinates P 2 ＝[x,y,z]The conversion relationship is as follows:
wherein, the earth is regarded as an ellipsoid, a is a long half shaft, b is a short half shaft,
converting ECEF coordinates into a northeast coordinate system, and recording a target point under the northeast coordinate system as P 3 ＝[E,N,U]At P 1 A rotation matrix R at 1 The method comprises the following steps:
through the calculation, the initial coordinate of the GNSS is set as the origin of the northeast day coordinate system, and the subsequent position observation is obtained.
5. The method according to claim 4, wherein the step (13) is specifically:
measuring a rotation vector of the amphibious robot through DVL internal integrated magnetometer and attitude sensorAccording to the formula of lode->θ is the modular length of the rotation vector, +.>Obtaining an initial rotation matrix of the amphibious robot for the direction of the rotation vector, wherein
6. The method according to claim 5, wherein step (2) comprises the steps of:
step (21): the state variable of the integrated navigation system is set as a 15-dimensional column vector:
X＝[p,v,R,b g ,b a ,g] · (8)
wherein, p represents displacement in the three-axis direction, v represents three-axis speed, and R represents a rotation matrix from the amphibious robot system to the world system;
step (22): the equation of motion under continuous time is calculated as:
the measurement equation of the IMU is calculated as:
wherein the method comprises the steps ofAnd->Representing measured acceleration and angular velocity, η of the IMU a And eta g Respectively representing velocity random walk and angle walk; zero bias modeling is:
wherein t represents the time of the moment, and t' represents the time of the last moment;
step (23): sorting (9) (10) (11) to obtain a motion equation of the state variable nominal value of the IMU under continuous time, wherein the motion equation is as follows:
discretizing the nominal state to obtain a prediction equation of the nominal state as follows:
wherein Δt represents the time interval of each sampling, the measurement value of the IMU adopts median integration, and the angular velocity and the acceleration at the present time and the next time are respectively averaged to be the measurement value at the present time.
7. The method according to claim 6, wherein the step (3) specifically comprises the steps of:
step (31): calculating transfer equations of all state errors by adopting error state Kalman filtering, and defining true state X of ESKF t ＝[p t ,v t ,R t ,b at ,b gt ,g t ] · From equation (12), the relationship between the state variable derivative with respect to the observed quantity is obtained:
defining the calculation of the error state variables is:
the delta symbol represents the error of the nominal state from the true value, wherein δR of the rotating part is expressed as its lie algebra exp (δθ) ∧ ) To show that the rotation matrix is updated with the left perturbation model and the derivative of the error state with respect to time is shown as follows:
deriving both sides of the formula (15), and bringing the formulas (12) (14) (16) to both sides of the derived equal sign, the kinematic equation for obtaining the error state variable is as follows:
also discretizing the above formula (17) to obtain a discrete kinematic equation of the error state as follows:
step (32): calculating a covariance matrix in a prediction process, and obtaining a state transition matrix according to a model of a Kalman filter, wherein the state transition matrix is as follows:
the state noise covariance matrix Q is calculated as follows:
Q＝diag(0 3 ,cov(η v ),cov(η θ ),cov(η g ),cov(η a ),0 3 ) (20)
the error state prediction process is obtained according to the formulas (18) (19) (20):
wherein δx pred Indicating the error state before the update at the present time, P pred The state process covariance matrix before updating at this time is represented by P, which is the state process covariance matrix at the previous time, and the error state δx is set to 0 after each update.
8. The method of claim 7, wherein step (4) comprises the steps of:
step (41): the DVL is used to construct the observation equation, the basic form of which is as follows:
Z＝h(x)+υ，υ～N(0，V) (22)
wherein Z is observation data, V is observation noise, and V is covariance matrix of the observation noise;
calculating a jacobian matrix of the observation equation for the error state:
wherein,
the rotation observation equation for DVL is calculated as:
R DVL ＝R·exp(δθ^) (25)
wherein R is DVL Is a direct observation of δθ, and therefore:
Z δθ ＝h(δθ)＝Log(R · R DVL ) (26)
the jacobian calculation of the observation equation for δθ is:
step (42) using GNSS to construct an observation equation, observing the position state, and calculating as follows:
Z δp ＝h(δp)＝p gnss -p (28)
the observation equation is a jacobian unit array of state quantity;
step (43): an observation equation is constructed by using a pressure depth gauge, the displacement of the z axis is observed, when the amphibious robot submerges into the water, the GNSS signal fails, and the observation sensor at the moment is provided with a DVL and the pressure depth gauge, so that the state constraint is carried out on the amphibious robot, and the track accuracy is ensured;
step (44): correcting the predicted nominal state by using the error state after observation and updating to obtain the final nominal state as the output of the system:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311022091.4A CN117053782A (en) | 2023-08-14 | 2023-08-14 | Combined navigation method for amphibious robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311022091.4A CN117053782A (en) | 2023-08-14 | 2023-08-14 | Combined navigation method for amphibious robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117053782A true CN117053782A (en) | 2023-11-14 |
Family
ID=88661990
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311022091.4A Pending CN117053782A (en) | 2023-08-14 | 2023-08-14 | Combined navigation method for amphibious robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117053782A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117590751A (en) * | 2023-12-28 | 2024-02-23 | 深圳市德威胜潜水工程有限公司 | Underwater environment monitoring method and system based on underwater robot |
-
2023
- 2023-08-14 CN CN202311022091.4A patent/CN117053782A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117590751A (en) * | 2023-12-28 | 2024-02-23 | 深圳市德威胜潜水工程有限公司 | Underwater environment monitoring method and system based on underwater robot |
| CN117590751B (en) * | 2023-12-28 | 2024-03-22 | 深圳市德威胜潜水工程有限公司 | Underwater environment monitoring method and system based on underwater robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
| CN111426318B (en) | Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering | |
| CN112629538B (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
| US7979231B2 (en) | Method and system for estimation of inertial sensor errors in remote inertial measurement unit | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| CN112146655B (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN111380516B (en) | Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information | |
| CN111006675B (en) | Self-calibration method of vehicle-mounted laser inertial navigation system based on high-precision gravity model | |
| CN113340298A (en) | Inertial navigation and dual-antenna GNSS external reference calibration method | |
| CN111722295B (en) | Underwater strapdown gravity measurement data processing method | |
| CN115060257B (en) | Vehicle lane change detection method based on civil-grade inertia measurement unit | |
| CN117053782A (en) | Combined navigation method for amphibious robot | |
| CN113252031A (en) | NARX neural network assisted integrated navigation method | |
| CN110849360A (en) | Distributed relative navigation method for multi-machine cooperative formation flight | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN111982126B (en) | Design method of full-source BeiDou/SINS elastic state observer model | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
| CN112697154A (en) | Self-adaptive multi-source fusion navigation method based on vector distribution | |
| CN116594000A (en) | Laser Doppler velocimeter online calibration method and device based on position observation | |
| CN117053802A (en) | Method for reducing positioning error of vehicle navigation system based on rotary MEMS IMU | |
| CN114111792B (en) | Vehicle-mounted GNSS/INS/odometer integrated navigation method | |
| CN112923934A (en) | Laser SLAM technology suitable for combining inertial navigation in unstructured scene | |
| CN115523919A (en) | Nine-axis attitude calculation method based on gyro drift optimization |
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 |