CN115343738A - GNSS-RTK and IMU based integrated navigation method and equipment - Google Patents

GNSS-RTK and IMU based integrated navigation method and equipment Download PDF

Info

Publication number
CN115343738A
CN115343738A CN202210959075.7A CN202210959075A CN115343738A CN 115343738 A CN115343738 A CN 115343738A CN 202210959075 A CN202210959075 A CN 202210959075A CN 115343738 A CN115343738 A CN 115343738A
Authority
CN
China
Prior art keywords
time
real
coordinate system
initial
positioning information
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
Application number
CN202210959075.7A
Other languages
Chinese (zh)
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.)
Zhicheng Spacetime Technology Zhejiang Co ltd
Original Assignee
Zhicheng Spacetime Technology Zhejiang Co ltd
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 Zhicheng Spacetime Technology Zhejiang Co ltd filed Critical Zhicheng Spacetime Technology Zhejiang Co ltd
Priority to CN202210959075.7A priority Critical patent/CN115343738A/en
Publication of CN115343738A publication Critical patent/CN115343738A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • 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/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
    • 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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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/52—Determining velocity
    • 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/53—Determining attitude
    • G01S19/54—Determining attitude using carrier phase measurements; using long or short baseline interferometry

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)

Abstract

The invention relates to the technical field of navigation, in particular to a combined navigation method based on GNSS-RTK and IMU. The purpose is to provide a low-cost and high-precision navigation method. The combined navigation method based on the GNSS-RTK and the IMU comprises the following steps: after the GNSS-RTK module is initialized, acquiring first initial positioning information of the carrier through the GNSS-RTK module; according to the first initial positioning information, second initial positioning information of the carrier is obtained by resolving through combining sensor data fed back by the IMU module; acquiring first real-time positioning information of a carrier through a GNSS-RTK module; solving second real-time positioning information of the carrier according to sensor data fed back by the IMU module; and performing combined navigation according to the first real-time positioning information and the second real-time positioning information to obtain third real-time positioning information of the carrier. The invention only needs to increase a small part of cost to realize continuous high-precision positioning.

Description

GNSS-RTK and IMU based integrated navigation method and equipment
Technical Field
The invention relates to the technical field of navigation, in particular to a combined navigation method and equipment based on GNSS-RTK and IMU.
Background
At present, when motor vehicles such as cars, buses, trucks and special work vehicles are positioned in Real time with high precision, a scheme based on a GNSS (Global Navigation Satellite System) is mostly adopted to be matched with an RTK (Real-time kinematic) to realize the positioning, and the scheme can realize centimeter-level precision positioning of the Real-time position of the motor vehicles in the traditional open and open areas. However, when the vehicle is driven in a scene where satellite positioning signals of a city, a campus, a specific road and the like are greatly affected by interference, shielding and the like, or is applied in an actual operation scene facing the field of automatic driving, and when the positioning data of the motor vehicle needs to be continuously and continuously maintained with high precision, the positioning is performed only by a combination of a conventional GNSS and an RTK, a large positioning offset is still generated, and it is difficult to achieve the expected positioning precision.
The ultra-compact GPS (Global Positioning System) and the Inertial coupling are a pure hardware implementation scheme, and have the disadvantages of high technical threshold and high cost, because the ultra-compact GPS and the Inertial Measurement Unit (IMU) are coupled in the ultra-compact manner, higher requirements are provided for hardware manufacturing, integration, yield and the like of manufacturers, and the application of the technology has larger limitations, which is not favorable for popularization and application, and can significantly improve the production cost of the product.
In a conventional GNSS and IMU loosely combined navigation mode, differences between a position and a velocity output by the GNSS and a position and a velocity solved by the IMU are generally transmitted to a kalman filter as measurement information, and then an error of the IMU solution is estimated and corrected by the filter. The method ignores that fluctuation of the accuracy of the GNSS itself also exists with a certain probability, the position data given by the GNSS module also has an error, and the error value of the GNSS may be higher than that obtained by the IMU in some cases. Neglecting this, there may be a position flying spot that suddenly appears to be greatly offset when the carrier is continuously moving, resulting in an excessively large deviation of the position data.
In view of the above-mentioned related technologies, the inventor believes that how to improve the accuracy of continuous positioning without increasing much cost is a problem to be solved.
Disclosure of Invention
In order to solve the problems in the prior art, the invention provides a combined navigation method and device based on GNSS-RTK and IMU, which improves the positioning accuracy without increasing too much cost.
In a first aspect of the present invention, a combined navigation method based on GNSS-RTK and IMU is provided, the method including:
after the GNSS-RTK module is initialized, acquiring first initial positioning information of a carrier through the GNSS-RTK module; the first initial positioning information comprises: a first initial position and a first initial velocity;
according to the first initial positioning information, second initial positioning information of the carrier is obtained by resolving in combination with sensor data fed back by an IMU (inertial measurement Unit) module; the second initial positioning information includes: a second initial position, a second initial velocity, and an initial attitude angle;
acquiring first real-time positioning information of the carrier through a GNSS-RTK module; the first real-time positioning information comprises: a first real-time position and a first real-time velocity;
according to the sensor data fed back by the IMU module, calculating second real-time positioning information of the carrier; the second real-time positioning information comprises: a second real-time position, a second real-time velocity, and a real-time attitude angle;
performing combined navigation according to the first real-time positioning information and the second real-time positioning information to obtain third real-time positioning information of the carrier; the third real-time positioning information comprises: a third real-time position, a third real-time velocity, and the real-time attitude angle.
Preferably, the sensor data fed back by the IMU module includes: linear acceleration measured by an accelerometer and angular velocity information measured by a gyroscope;
the initial attitude angle includes: an initial pitch angle, an initial roll angle and an initial course angle;
the step of calculating the second initial positioning information of the carrier according to the first initial positioning information and by combining sensor data fed back by the IMU module comprises the following steps:
after the initialization of the IMU module is completed, sensor data fed back by the IMU module is obtained, and an initial transformation matrix from a machine body coordinate system to a navigation coordinate system is further determined according to the following formula:
Figure BDA0003791291790000031
wherein n and b respectively represent a navigation coordinate system and a body coordinate system;
Figure BDA0003791291790000032
and
Figure BDA0003791291790000033
respectively representing the gravity acceleration and the earth rotation angular velocity under a navigation coordinate system acquired according to the first initial position;
Figure BDA0003791291790000034
and
Figure BDA0003791291790000035
respectively representing linear acceleration measured by an accelerometer and angular velocity information measured by a gyroscope in the IMU module, and
Figure BDA0003791291790000036
and
Figure BDA0003791291790000037
all values are under a body coordinate system;
converting the first initial position into the second initial position under a body coordinate system according to the initial transformation matrix;
determining the second initial velocity and the initial attitude angle.
Preferably, the step of "calculating the second real-time positioning information of the carrier according to the sensor data fed back by the IMU module" includes:
calculating the real-time attitude angle;
calculating the second real-time speed;
calculating the second real-time location.
Preferably, the real-time attitude angles include: real-time course angle, real-time pitch angle and real-time roll angle;
the step of "calculating the real-time attitude angle" includes:
calculating the real-time course angle according to the following formula:
Figure BDA0003791291790000038
calculating the real-time pitch angle according to:
θ＝arcsinC 32
calculating the real-time roll angle according to the following formula:
Figure BDA0003791291790000039
wherein psi, theta and phi respectively represent the real-time course angle, the real-time pitch angle and the real-time roll angle; c 12 、C 22 、C 32 、C 31 And C 33 Are respectively attitude matrix
Figure BDA00037912917900000310
The value of the corresponding position in the image; the attitude matrix
Figure BDA00037912917900000311
A coordinate transformation matrix for a coordinate system from the body coordinate system to the navigation computation coordinate system.
Preferably, the step of "calculating the second real-time speed" includes:
the speed increment is calculated according to:
Figure BDA0003791291790000041
wherein the content of the first and second substances,
Figure BDA0003791291790000042
Figure BDA0003791291790000043
Figure BDA0003791291790000044
representing the velocity increment at time k under the navigation coordinate system, n representing the navigation coordinate system,
Figure BDA0003791291790000045
the specific force increment in the navigation coordinate system is shown,
Figure BDA0003791291790000046
showing the cosine vector of the acceleration in the navigation coordinate system, delta t showing the time increment, g n Representing the acceleration of gravity under the navigation coordinate system,
Figure BDA0003791291790000047
expressed as an attitude matrix at the moment k-1, b represents a coordinate system of the body, I represents a gravity acceleration compensation quantity,
Figure BDA0003791291790000048
representing the angle of the rotation vector of the navigation coordinate system to the body coordinate system,
Figure BDA0003791291790000049
x is a number
Figure BDA00037912917900000410
The anti-symmetric matrix of (a) is,
Figure BDA00037912917900000411
representing the specific force increment of the body coordinate system,
Figure BDA00037912917900000418
representing the specific force added per cycle;
and calculating the second real-time speed according to the speed increment.
Preferably, the step of "calculating the second real-time position" comprises:
calculating the displacement of the carrier according to:
Figure BDA00037912917900000412
wherein the content of the first and second substances,
Figure BDA00037912917900000413
representing the displacement of the carrier at the time k under the navigation coordinate system, n representing the navigation coordinate system,
Figure BDA00037912917900000414
representing the navigation coordinate system velocity at time k-1,
Figure BDA00037912917900000415
denotes the amount of compensation of the gravitational Coriolis acceleration, T k Which represents the calculation period at time k and,
Figure BDA00037912917900000416
represents the specific force velocity increment integral at time k;
according to the displacement and real-time attitude matrix of the carrier
Figure BDA00037912917900000417
Calculating the second real-time position under b.
Preferably, the step of performing combined navigation according to the first real-time positioning information and the second real-time positioning information to obtain the third real-time positioning information of the carrier includes:
respectively carrying out filtering estimation on the first real-time position and the second real-time position to obtain a first position estimation value and a second position estimation value;
selecting one of the first position estimate and the second position estimate with a smaller error as the third real-time position;
and calculating the third real-time speed according to the third real-time position.
Preferably, the step of filtering the estimate comprises:
estimating the state of the current moment according to the position posterior state estimated value of the previous moment by using the following formula to obtain the position prior state estimated value of the k moment
Figure BDA0003791291790000051
Sum prior estimated mean square error matrix
Figure BDA0003791291790000052
Figure BDA0003791291790000053
Figure BDA0003791291790000054
Wherein the content of the first and second substances,
Figure BDA0003791291790000055
the position prior state estimated value representing the k moment is a filtering intermediate calculation result; a represents a state transition matrix;
Figure BDA0003791291790000056
representing the posterior state estimation value of the position at the k-1 moment; b represents a matrix that converts the input to a state; u. u k-1 Represents the control gain at time k-1;
Figure BDA0003791291790000057
representing the a priori estimated mean square error matrix at time k,is the intermediate calculation result of the filtering; p k-1 Expressing an posterior estimation mean square error matrix at the k-1 moment; a. The T Representing a state transition matrix associated with time; q represents a process excitation noise variance matrix;
according to the prior state estimated value of k time
Figure BDA0003791291790000058
Self-adaptive Kalman filtering is carried out by using the following formula to obtain the posterior state estimation value of the k moment
Figure BDA0003791291790000059
Sum a posteriori estimation mean square error matrix P k ：
Figure BDA00037912917900000510
Figure BDA00037912917900000511
Figure BDA00037912917900000512
Wherein, K k Representing a filter gain matrix which is the intermediate calculation result of the filtering; h T Representing a transition matrix to measurements based on the time-dependent state variables; h represents a state variable to measurement transition matrix; r represents a measurement noise variance matrix;
Figure BDA00037912917900000513
the posterior state estimation value of the position representing the k time is one of the filtering results;
Figure BDA00037912917900000514
the position prior state estimated value of k moment is represented and is the intermediate calculation result of filtering; z is a radical of formula k Representing the position measurement as an input to the filtering; p is k An a posteriori estimation mean square error matrix representing the k time, which is one of the results of the filtering; i isRepresenting an identity matrix.
In a second aspect of the present invention, a navigation apparatus is provided, which includes: a GNSS-RTK module and an IMU module and executes a computer program of the method as described above.
In a third aspect of the present invention, a computer-readable storage device is presented, characterized in that a computer program is stored which can be loaded by a processor and which performs the method as described above.
Compared with the closest prior art, the invention has the following beneficial effects:
according to the combined navigation method based on the GNSS-RTK and the IMU, on the basis of a traditional GNSS-RTK satellite positioning module and an IMU positioning module, filtering estimation is respectively carried out on positioning information obtained by resolving through the two modules, and then the positioning information with smaller error is selected as an output result of combined navigation. Compared with the GPS and IMU ultra-tight coupling mode, the invention can effectively utilize the existing module and only needs to increase a small part of cost. Compared with a traditional GNSS and IMU loose combined navigation mode, the method considers the situation that the GNSS error is possibly larger than the IMU resolving error, and can effectively reduce the influence on the final navigation result when the situation occurs, so that the motion track of the carrier is more continuous and the positioning data is more accurate. In addition, the method simplifies the Kalman filtering mode adopted by the traditional GNSS and IMU integrated navigation to a certain extent, improves the efficiency of integrated navigation calculation, enables the equipment to realize high-frequency position data calculation with lower cost, and meets the requirements of more scenes. The positioning method has the characteristics of low time delay, high stability, high robustness, universality and the like.
Drawings
FIG. 1 is a schematic diagram of the GNSS-RTK and IMU based integrated navigation method of the present invention;
FIG. 2 is a schematic diagram of the main steps of an embodiment of the GNSS-RTK and IMU based combined navigation method of the present invention.
Detailed Description
Preferred embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these embodiments are only for explaining the technical principle of the present invention, and are not intended to limit the scope of the present invention.
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
It should be noted that the terms "first" and "second" in the description of the present invention are used for convenience of description only and do not indicate or imply relative importance of the devices, elements or parameters, and therefore should not be construed as limiting the present invention. In addition, the term "and/or" in the present invention is only an association relationship describing an associated object, and means that three relationships may exist, for example, a and/or B, and may mean: a exists alone, A and B exist simultaneously, and B exists alone. In addition, the character "/" herein generally indicates that the former and latter associated objects are in an "or" relationship, unless otherwise specified.
In order to perform continuous high-precision positioning capability on the running track of a motor vehicle, monitor and manage the running condition of the motor vehicle in a specific scene, a park in a specific environment and an industrial occasion, and perform service requirements such as real-time resource scheduling, safety alarm analysis and prediction and the like based on the real-time position condition of the motor vehicle, the invention adopts a method of combining GNSS-RTK and IMU (Inertial Measurement Unit) navigation to form continuous high-precision positioning in a continuous running state.
FIG. 1 is a schematic diagram of a GNSS-RTK and IMU based integrated navigation method of the present invention. As shown in fig. 1, after the GNSS-RTK module and the IMU module are both powered on and initialized, initial positioning information output by the GNSS-RTK module is acquired, so that the IMU module completes the resolution of the initial positioning information by means of the information; then, the two modules respectively update the positioning information in real time; and respectively filtering the two groups of updated positioning information, and selecting the positioning information with smaller error from the positioning information obtained after filtering as the real-time positioning information after combined navigation.
Description of the coordinate systems relevant to the invention:
(1) and a geographic coordinate system g is defined as a local horizontal coordinate system, the x axis points to the north direction defined by the meridian in the horizontal plane, the y axis points to the east direction defined by the latitude line in the horizontal plane, and the z axis points to the ground along the plumb line.
(2) And a carrier coordinate system b defined as a front-right-lower coordinate system, wherein the x axis points forward along the longitudinal axis of the carrier, the y axis points rightward along the transverse axis of the carrier, and the z axis points downward along the vertical axis of the carrier.
(3) And the navigation coordinate system n can be selected differently according to different navigation schemes. Here the geographical coordinate system is chosen as the navigation reference system.
FIG. 2 is a schematic diagram of the main steps of an embodiment of the GNSS-RTK and IMU based combined navigation method of the present invention. As shown in fig. 2, the positioning method of the present embodiment includes steps S10 to S50:
s10, after the GNSS-RTK module is initialized, acquiring first initial positioning information of the carrier through the GNSS-RTK module; the first initial positioning information includes: a first initial position and a first initial velocity.
Normally, when the GNSS-RTK module is initialized, the carrier is in the stationary base state, and the first initial velocity is zero. Longitude, latitude and altitude information of the carrier can be obtained through the GNSS-RTK module.
And S20, calculating to obtain second initial positioning information of the carrier according to the first initial positioning information and by combining sensor data fed back by the IMU module.
Wherein the second initial positioning information comprises: a second initial position, a second initial velocity, and an initial attitude angle; the sensor data fed back by the IMU module comprises: linear acceleration measured by an accelerometer and angular velocity information measured by a gyroscope; the initial attitude angles include: an initial pitch angle, an initial roll angle, and an initial heading angle.
In this embodiment, the method for calculating the second initial position includes: after initialization of the IMU module is completed, sensor data fed back by the IMU module are obtained, and then an initial transformation matrix from a machine body coordinate system to a navigation coordinate system is determined according to a formula (1):
Figure BDA0003791291790000081
wherein n and b respectively represent a navigation coordinate system and a body coordinate system;
Figure BDA0003791291790000082
and
Figure BDA0003791291790000083
respectively representing the gravity acceleration and the earth rotation angular velocity under a navigation coordinate system acquired according to the first initial position;
Figure BDA0003791291790000084
and
Figure BDA0003791291790000085
respectively representing linear acceleration measured by an accelerometer and angular velocity information measured by a gyroscope in the IMU module, and
Figure BDA0003791291790000086
and
Figure BDA0003791291790000087
all values are values in a coordinate system of the body.
Then, the first initial position is converted into a second initial position in the body coordinate system according to the initial transformation matrix.
In the static base state, the second initial speed is 0, and the initial pitch angle and the initial roll angle are both 0; the initial course angle is equal to the initial orientation of the IMU module; the initial orientation is determined by the mounting direction of the IMU module. According to the installation specification of the equipment, three installation directions of 0 degrees, 90 degrees or 180 degrees exist.
S30, acquiring first real-time positioning information of the carrier through the GNSS-RTK module; the first real-time positioning information comprises: a first real-time position and a first real-time velocity.
S40, resolving second real-time positioning information of the carrier according to sensor data fed back by the IMU module; the second real-time positioning information comprises: a second real-time position, a second real-time velocity, and a real-time attitude angle. This step may specifically include steps S41-S43:
and step S41, calculating a real-time attitude angle.
At this time, the centroid of the carrier is used as the origin o, ox b Representing the right, oy, along the horizontal axis in the coordinate system of the body b Expressed forward along the longitudinal axis in the body coordinate system, oz b Expressed perpendicular to ox in the coordinate system of the body b y b Upward, ox b y b z b The coordinate system forms a right-hand rectangular coordinate system, and ox is used when the machine body is not pitching or tilting b Y b I.e. horizontal plane, oz b The axis points to the zenith along the vertical line.
Body coordinate system ox b y b z b And a navigation coordinate system ox n y n z n The relative relationship of (a) is the posture of the body, which can be described by three posture angles, defined as follows:
heading angle ψ: y is b And oy n (indicating the north direction) is measured in the horizontal plane with the east direction being positive.
Pitch angle θ: y is b The angle between the vertical plane and the horizontal plane is positive when measured in the vertical plane.
Roll angle phi: x is the number of b The included angle between the right side and the horizontal plane is measured in the cross section of the machine body, and the right side rises to be positive.
Navigation coordinate system ox n y n z n By rotating continuously according to the attitude angle, a mobile body coordinate system ox can be obtained b y b z b Wherein the rotation matrix corresponding to the rotation angle is shown in equations (2) - (4):
Figure BDA0003791291790000091
Figure BDA0003791291790000092
Figure BDA0003791291790000093
at this time, a coordinate transformation matrix from the body coordinate system to the navigation computation coordinate system is as shown in equation (5):
Figure BDA0003791291790000101
Figure BDA0003791291790000102
as an attitude matrix, the corresponding real-time attitude angle can be calculated by the matrix, as shown in equations (6) - (8):
Figure BDA0003791291790000103
θ＝arcsinC 32 (7)
Figure BDA0003791291790000104
wherein psi, theta and phi respectively represent a real-time course angle, a real-time pitch angle and a real-time roll angle; c 12 、C 22 、C 32 、C 31 And C 33 Are respectively attitude matrix
Figure BDA0003791291790000105
The value of the corresponding position in the image; attitude matrix
Figure BDA0003791291790000106
A coordinate transformation matrix for a coordinate system from the body coordinate system to the navigation computation coordinate system.
After the real-time attitude angle is calculated, the real-time orientation can be calculated by combining the IMU initial orientation recorded during installation.
And step S42, calculating a second real-time speed.
First, the speed increment is calculated according to equations (9) to (11):
Figure BDA0003791291790000107
wherein the content of the first and second substances,
Figure BDA0003791291790000108
Figure BDA0003791291790000109
wherein the content of the first and second substances,
Figure BDA00037912917900001010
representing the velocity increment at time k under the navigation coordinate system, n representing the navigation coordinate system,
Figure BDA00037912917900001011
the specific force increment in the navigation coordinate system is shown,
Figure BDA00037912917900001012
showing the cosine vector of the acceleration in the navigation coordinate system, delta t showing the time increment, g n Representing the acceleration of gravity under the navigation coordinate system,
Figure BDA0003791291790000111
expressed as an attitude matrix at the moment k-1, b represents a coordinate system of the body, I represents a gravity acceleration compensation quantity,
Figure BDA0003791291790000112
representing the angle of the rotation vector of the navigation coordinate system to the body coordinate system,
Figure BDA0003791291790000113
x is
Figure BDA0003791291790000114
The anti-symmetric matrix of (a) is,
Figure BDA0003791291790000115
representing the specific force increment of the body coordinate system,
Figure BDA0003791291790000116
indicating the specific force applied per cycle.
Then, a second real-time speed is calculated from the speed increment.
Step S43, calculating a second real-time position.
First, the displacement of the carrier is calculated according to equation (12):
Figure BDA0003791291790000117
wherein the content of the first and second substances,
Figure BDA0003791291790000118
representing the displacement of the carrier at the time k under the navigation coordinate system, n representing the navigation coordinate system,
Figure BDA0003791291790000119
representing the navigation coordinate system velocity at time k-1,
Figure BDA00037912917900001110
representing the amount of compensation for the Coriolis acceleration, T k Indicating the calculation period at time k,
Figure BDA00037912917900001111
representing the specific force velocity increment integral at time k.
Then, according to the displacement and real-time attitude matrix of the carrier
Figure BDA00037912917900001112
Can calculate the second under bReal-time location.
And S50, performing combined navigation according to the first real-time positioning information and the second real-time positioning information to obtain third real-time positioning information of the carrier.
Wherein the third real-time positioning information comprises: a third real-time position, a third real-time velocity, and a real-time attitude angle.
In this embodiment, the real-time attitude angle still uses the value calculated in step S40, and after kalman filtering is performed on the real-time position output by the GNSS-RTK module and the real-time position resolved by the IMU module, the real-time position with the smaller error is selected as the real-time position of the integrated navigation; similarly, the real-time position is also the real-time speed of the combined navigation, which is selected to have a smaller error after filtering.
Step S50 may specifically include steps S51-S53:
and S51, respectively carrying out filtering estimation on the first real-time position and the second real-time position to obtain a first position estimation value and a second position estimation value.
In this embodiment, the first real-time position is used as the measured value z of the filtering input k Carrying out filtering estimation by using the following formulas (13) to (17) to obtain a posterior state estimation value, namely a first position estimation value; then the second real-time position is taken as a measured value z of the filtering input k The posterior state estimation value, i.e., the second position estimation value, is obtained by performing filter estimation using the following equations (13) to (17).
The following describes the steps of the filter estimation:
estimating the state of the current time (k moment) by using time updating equations of the following formulas (13) to (14) according to the posterior state estimation value of the position at the last time (k-1 moment), and obtaining the prior state estimation value at the k moment
Figure BDA0003791291790000121
Sum prior estimated mean square error matrix
Figure BDA0003791291790000122
Figure BDA0003791291790000123
Figure BDA0003791291790000124
Wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003791291790000125
the a priori state estimate representing the position at time k is the intermediate result of the filtering, i.e. the optimal estimate from the previous time (time k-1)
Figure BDA0003791291790000126
Predicting the obtained k time result to be the result of the prediction equation; a represents a state transition matrix, which is actually a guess model of the target state transition;
Figure BDA0003791291790000127
an estimate of the posterior state representing the position at time k-1; b denotes a matrix converting the input into states. u. u k-1 Represents the control gain at time k-1;
Figure BDA0003791291790000128
a priori estimated mean square error matrix representing k time instants (
Figure BDA0003791291790000129
The mean square error matrix of) is the intermediate calculation result of filtering; p is k-1 The array of a posteriori estimated mean square errors representing the k-1 time instant (i.e. the
Figure BDA00037912917900001210
The mean square error matrix of (a), representing the uncertainty of the state), is one of the results of the filtering; a. The T Representing a time-dependent state transition matrix, Q representing a process excitation noise variance matrix (variance of the system process); the parameter, which is used to represent the error between the state transition matrix and the actual process, is a Kalman filter used to estimate the state variable of the discrete-time process, also called the prediction modelThe noise itself.
Combining the prior state estimated value of the k time position
Figure BDA00037912917900001211
The posterior state estimated value of the k time position is obtained by carrying out self-adaptive Kalman filtering through the following formulas (15) to (17)
Figure BDA00037912917900001212
Sum a posteriori estimation mean square error matrix P k ：
Figure BDA00037912917900001213
Figure BDA00037912917900001214
Figure BDA00037912917900001215
Wherein, K k Representing a filter gain matrix which is the intermediate calculation result of filtering, namely Kalman gain or Kalman coefficient;
Figure BDA00037912917900001216
represents: a priori estimated mean square error matrix at time k (
Figure BDA00037912917900001217
The mean square error matrix of) is the intermediate calculation result of filtering; h T A transformation matrix representing state variables to measurements (observations) based on time correlation, representing a relationship connecting states and observations based on a time dimension; h represents a conversion matrix from the state variable to the measurement (observation), which represents the relation connecting the state and the observation, and Kalman filtering is linear relation, which is responsible for converting the m-dimensional measurement value to the n-dimensional measurement value to be in accordance with the mathematical form of the state variable, and is one of the preconditions of filtering; r represents the measurement noise variance matrix, the filter actualDuring implementation, the measured noise variance matrix R can be generally observed and is a known condition of the filter;
Figure BDA0003791291790000131
the a posteriori state estimate (i.e. the first position estimate or the second position estimate) representing time k is one of the results of the filtering, i.e. the updated result, also called the optimal estimate;
Figure BDA0003791291790000132
the prior state estimation value representing the k moment is the intermediate calculation result of filtering, namely the k moment result predicted according to the optimal estimation of the last moment (k-1 moment) is the result of a prediction equation; z is a radical of k The representative measurement (i.e., the first real-time location or the second real-time location) is the filtered input; p is k The array of a posteriori estimated mean square errors representing the k time instants (i.e.
Figure BDA0003791291790000133
The mean square error matrix of (a), representing the uncertainty of the state), is one of the results of the filtering; i denotes an identity matrix.
Step S52, selecting one with smaller error from the first position estimation value and the second position estimation value as a third real-time position.
And S53, calculating a third real-time speed according to the third real-time position.
Specifically, the displacement of the carrier moving in the interval time may be calculated according to the third real-time position at the current time, the third real-time position at the previous time, and the carrier moving direction, and then the displacement is divided by the moving time to obtain the third real-time speed at the current time.
Although the foregoing embodiments describe the steps in the above sequential order, those skilled in the art will understand that, in order to achieve the effect of the present embodiments, the steps may not be executed in such an order, and may be executed simultaneously (in parallel) or in an inverse order, and these simple variations are within the scope of the present invention.
Further, based on the same technical concept as the method embodiment, the application also provides an embodiment of the navigation device. The navigation device in this embodiment includes: a GNSS-RTK module and an IMU module and executes a computer program of the combined GNSS-RTK and IMU based navigation method as described above.
Still further, the present invention provides a computer readable storage device, in which a computer program capable of being loaded by a processor and executing the combined GNSS-RTK and IMU based navigation method as described above is stored.
The computer-readable storage device in the embodiment includes: various devices capable of storing program codes, such as a usb disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk.
Those of skill in the art will appreciate that the method steps of the examples described in connection with the embodiments disclosed herein may be embodied in electronic hardware, computer software, or combinations of both, and that the components and steps of the examples have been described above generally in terms of their functionality in order to clearly illustrate the interchangeability of electronic hardware and software. Whether such functionality is implemented as electronic hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
So far, the technical solution of the present invention has been described in connection with the preferred embodiments shown in the accompanying drawings. However, it is to be understood by those skilled in the art that the scope of the present invention is obviously not limited to these specific embodiments. Equivalent changes or substitutions of related technical features can be made by those skilled in the art without departing from the principle of the invention, and the technical scheme after the changes or substitutions can be within the protection scope of the invention.

Claims (10)

1. A combined navigation method based on GNSS-RTK and IMU, the method comprising:
after the GNSS-RTK module is initialized, acquiring first initial positioning information of a carrier through the GNSS-RTK module; the first initial positioning information comprises: a first initial position and a first initial velocity;
according to the first initial positioning information, second initial positioning information of the carrier is obtained by resolving in combination with sensor data fed back by an IMU (inertial measurement Unit) module; the second initial positioning information includes: a second initial position, a second initial velocity, and an initial attitude angle;
acquiring first real-time positioning information of the carrier through a GNSS-RTK module; the first real-time positioning information comprises: a first real-time position and a first real-time velocity;
according to the sensor data fed back by the IMU module, calculating second real-time positioning information of the carrier; the second real-time positioning information comprises: a second real-time position, a second real-time velocity, and a real-time attitude angle;
performing combined navigation according to the first real-time positioning information and the second real-time positioning information to obtain third real-time positioning information of the carrier; the third real-time positioning information comprises: a third real-time position, a third real-time velocity, and the real-time attitude angle.
2. The GNSS-RTK and IMU based combined navigation method of claim 1,
the sensor data fed back by the IMU module comprises: linear acceleration measured by an accelerometer and angular velocity information measured by a gyroscope;
the initial attitude angle includes: an initial pitch angle, an initial roll angle and an initial course angle;
the step of calculating the second initial positioning information of the carrier according to the first initial positioning information and by combining sensor data fed back by the IMU module comprises the following steps:
after the initialization of the IMU module is completed, sensor data fed back by the IMU module is obtained, and an initial transformation matrix from a machine body coordinate system to a navigation coordinate system is further determined according to the following formula:
Figure FDA0003791291780000021
wherein n and b respectively represent a navigation coordinate system and a body coordinate system;
Figure FDA0003791291780000022
and
Figure FDA0003791291780000023
respectively representing the gravity acceleration and the earth rotation angular velocity under a navigation coordinate system acquired according to the first initial position;
Figure FDA0003791291780000024
and
Figure FDA0003791291780000025
respectively representing linear acceleration measured by an accelerometer and angular velocity information measured by a gyroscope in the IMU module, and
Figure FDA0003791291780000026
and
Figure FDA0003791291780000027
all values are values under a coordinate system of the body;
converting the first initial position into the second initial position under a body coordinate system according to the initial transformation matrix;
determining the second initial velocity and the initial attitude angle.
3. The combined GNSS-RTK and IMU based navigation method of claim 1, wherein the step of "calculating the second real-time positioning information of the carrier based on the sensor data fed back from the IMU module" comprises:
calculating the real-time attitude angle;
calculating the second real-time speed;
calculating the second real-time location.
4. The GNSS-RTK and IMU based combined navigation method of claim 3, wherein the real-time attitude angle comprises: real-time course angle, real-time pitch angle and real-time roll angle;
the step of "calculating the real-time attitude angle" includes:
calculating the real-time course angle according to the following formula:
Figure FDA0003791291780000028
calculating the real-time pitch angle according to:
θ＝arcsinC 32
calculating the real-time roll angle according to the following formula:
Figure FDA0003791291780000029
wherein ψ, θ and φ represent the real-time heading angle, the real-time pitch angle and the real-time roll angle, respectively; c 12 、C 22 、C 32 、C 31 And C 33 Are respectively attitude matrix
Figure FDA0003791291780000031
The value of the corresponding position in the image; the attitude matrix
Figure FDA0003791291780000032
A coordinate transformation matrix for a coordinate system from the body coordinate system to the navigation computation coordinate system.
5. The GNSS-RTK and IMU based combined navigation method of claim 3, wherein the step of "calculating the second real-time velocity" comprises:
the speed increment is calculated according to:
Figure FDA0003791291780000033
wherein the content of the first and second substances,
Figure FDA0003791291780000034
Figure FDA0003791291780000035
Figure FDA0003791291780000036
representing the velocity increment at time k under the navigation coordinate system, n representing the navigation coordinate system,
Figure FDA0003791291780000037
the specific force increment in the navigation coordinate system is represented,
Figure FDA0003791291780000038
denotes the cosine vector of the acceleration in the navigation coordinate system, Δ t denotes the time increment, g n Represents the gravity acceleration under the navigation coordinate system,
Figure FDA0003791291780000039
expressed as an attitude matrix at the moment of k-1, b represents a coordinate system of the body, I represents a gravity acceleration compensation quantity,
Figure FDA00037912917800000310
representing the angle of the rotation vector of the body coordinate system to the navigation coordinate system,
Figure FDA00037912917800000311
to represent
Figure FDA00037912917800000312
The anti-symmetric matrix of (a) is,
Figure FDA00037912917800000313
representing the specific force increment of the body coordinate system,
Figure FDA00037912917800000314
representing the specific force added per cycle;
and calculating the second real-time speed according to the speed increment.
6. The GNSS-RTK and IMU based combined navigation method of claim 3, wherein the step of "calculating the second real-time position" comprises:
calculating the displacement of the carrier according to:
Figure FDA00037912917800000315
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA00037912917800000316
the displacement of the carrier at the moment k under a navigation coordinate system is shown, n is the navigation coordinate system,
Figure FDA00037912917800000317
representing the navigation coordinate system velocity at time k-1,
Figure FDA00037912917800000318
denotes the amount of compensation of the gravitational Coriolis acceleration, T k Which represents the calculation period at time k and,
Figure FDA0003791291780000041
represents the specific force velocity increment integral at time k;
according to the displacement and real-time attitude matrix of the carrier
Figure FDA0003791291780000042
Calculating the second real-time position under b.
7. The GNSS-RTK and IMU based combined navigation method of claim 1, wherein the step of performing combined navigation based on the first real-time positioning information and the second real-time positioning information to obtain the third real-time positioning information of the carrier comprises:
respectively carrying out filtering estimation on the first real-time position and the second real-time position to obtain a first position estimation value and a second position estimation value;
selecting one of the first position estimate and the second position estimate having a smaller error as the third real-time position;
and calculating the third real-time speed according to the third real-time position.
8. The GNSS-RTK and IMU based combined navigation method of claim 7, wherein the step of filtering the estimate comprises:
estimating the state of the current moment by using the following formula according to the position posterior state estimation value of the previous moment to obtain the position prior state estimation value of the k moment
Figure FDA0003791291780000043
Sum prior estimated mean square error matrix
Figure FDA0003791291780000044
Figure FDA0003791291780000045
Figure FDA0003791291780000046
Wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003791291780000047
the position prior state estimated value representing the k moment is a filtering intermediate calculation result; a represents a state transition matrix;
Figure FDA0003791291780000048
representing the posterior state estimation value of the position at the k-1 moment; b represents a matrix that converts the input to a state; u. u k-1 Represents the control gain at time k-1;
Figure FDA0003791291780000049
the prior estimation mean square error matrix representing the k moment is the intermediate calculation result of filtering; p k-1 Expressing an posterior estimation mean square error matrix at the k-1 moment; a. The T Representing a state transition matrix in relation to time; q represents a process excitation noise variance matrix;
according to the prior state estimated value of k time
Figure FDA00037912917800000410
Self-adaptive Kalman filtering is carried out by using the following formula to obtain the posterior state estimation value of the k moment
Figure FDA00037912917800000411
Sum a posteriori estimation mean square error matrix P k ：
Figure FDA00037912917800000412
Figure FDA0003791291780000051
Figure FDA0003791291780000052
Wherein, K k Representing a filter gain matrix, which is an intermediate calculation result of filtering; h T Representing a transition matrix to a measurement based on the time-dependent state variable; h represents a state variable to measurement transition matrix; r represents a measurement noise variance matrix;
Figure FDA0003791291780000053
the posterior state estimation value of the position representing the k time is one of the filtering results;
Figure FDA0003791291780000054
the position prior state estimated value representing the k moment is a filtering intermediate calculation result; z is a radical of k Representing the position measurement as an input to the filtering; p k The posterior estimation mean square error matrix of k time is one of the filtering results; i denotes an identity matrix.
9. A navigation device, characterized in that the navigation device comprises: a GNSS-RTK module and an IMU module and executes a computer program of the method according to any of claims 1-8.
10. A computer-readable storage device, in which a computer program is stored which can be loaded by a processor and which executes the method according to any of claims 1-8.
CN202210959075.7A 2022-08-10 2022-08-10 GNSS-RTK and IMU based integrated navigation method and equipment Pending CN115343738A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210959075.7A CN115343738A (en) 2022-08-10 2022-08-10 GNSS-RTK and IMU based integrated navigation method and equipment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210959075.7A CN115343738A (en) 2022-08-10 2022-08-10 GNSS-RTK and IMU based integrated navigation method and equipment

Publications (1)

Publication Number Publication Date
CN115343738A true CN115343738A (en) 2022-11-15

Family

ID=83951750

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210959075.7A Pending CN115343738A (en) 2022-08-10 2022-08-10 GNSS-RTK and IMU based integrated navigation method and equipment

Country Status (1)

Country Link
CN (1) CN115343738A (en)

Similar Documents

Publication Publication Date Title
CN108226980B (en) Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit
Fakharian et al. Adaptive Kalman filtering based navigation: An IMU/GPS integration approach
CN110779521A (en) Multi-source fusion high-precision positioning method and device
JP5586994B2 (en) POSITIONING DEVICE, POSITIONING METHOD OF POSITIONING DEVICE, AND POSITIONING PROGRAM
CN111982106A (en) Navigation method, navigation device, storage medium and electronic device
JP5602070B2 (en) POSITIONING DEVICE, POSITIONING METHOD OF POSITIONING DEVICE, AND POSITIONING PROGRAM
CN112835085B (en) Method and device for determining vehicle position
CN113252048B (en) Navigation positioning method, navigation positioning system and computer readable storage medium
CN111751857A (en) Vehicle pose estimation method, device, storage medium and system
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN114061570A (en) Vehicle positioning method and device, computer equipment and storage medium
CN114964222A (en) Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device
CN113093239A (en) Method, corresponding system and program product for providing navigation information
CN117053802A (en) Method for reducing positioning error of vehicle navigation system based on rotary MEMS IMU
CN114897942B (en) Point cloud map generation method and device and related storage medium
CN114994732B (en) Vehicle-mounted course rapid initialization device and method based on GNSS carrier phase
CN114397480B (en) Acoustic Doppler velocimeter error estimation method, device and system
CN114111792B (en) Vehicle-mounted GNSS/INS/odometer integrated navigation method
CN115343738A (en) GNSS-RTK and IMU based integrated navigation method and equipment
CN115112119A (en) Vehicle navigation method based on LSTM neural network assistance
CN116380119A (en) Calibration method, device and system for integrated navigation
CN113048987A (en) Vehicle navigation system positioning method
CN111811512B (en) MPOS offline combination estimation method and device based on federal smoothing
Do et al. An Improvement of 3D DR/INS/GNSS Integrated System using Inequality Constrained EKF
CN111340952A (en) Method and device for mapping mobile measurement unlocking region

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