CN111141273A - Combined navigation method and system based on multi-sensor fusion - Google Patents

Combined navigation method and system based on multi-sensor fusion Download PDF

Info

Publication number
CN111141273A
CN111141273A CN201911310476.4A CN201911310476A CN111141273A CN 111141273 A CN111141273 A CN 111141273A CN 201911310476 A CN201911310476 A CN 201911310476A CN 111141273 A CN111141273 A CN 111141273A
Authority
CN
China
Prior art keywords
error
gps
navigation
coordinate system
output
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
CN201911310476.4A
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.)
Wuxi Bewis Sensing Technology Co ltd
Peking University Shenzhen Graduate School
Original Assignee
Wuxi Bewis Sensing Technology Co ltd
Peking University Shenzhen Graduate School
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 Wuxi Bewis Sensing Technology Co ltd, Peking University Shenzhen Graduate School filed Critical Wuxi Bewis Sensing Technology Co ltd
Priority to CN201911310476.4A priority Critical patent/CN111141273A/en
Publication of CN111141273A publication Critical patent/CN111141273A/en
Pending legal-status Critical Current

Links

Images

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Abstract

The invention discloses a combined navigation method and a system based on multi-sensor fusion, wherein the method comprises the following steps: when the GPS signal is effective, acquiring initial GPS navigation information; acquiring data of the odometer, and judging whether the carrier is in a static state or not according to the data of the odometer; if the carrier is in a motion state, judging whether the GPS signal is effective or not; if the GPS signal is effective, correcting the error of the inertial measurement element by using GPS navigation information and outputting a navigation result; and if the GPS signal fails, correcting the error of the inertial measurement element by using the data of the odometer and outputting a navigation result. According to the combined navigation method and system based on multi-sensor fusion, provided by the invention, when the GPS fails, the odometer is used for compensation to inhibit error divergence, so that accurate position and azimuth information is provided, the navigation requirement of an automatic driving vehicle is met, stable and reliable navigation is kept when the GPS fails, the navigation precision and the anti-interference capability are improved, and the application range of a navigation system is enlarged.

Description

Combined navigation method and system based on multi-sensor fusion
Technical Field
The application relates to the technical field of vehicle navigation, in particular to a combined navigation method and system based on multi-sensor fusion.
Background
In recent years, the automatic driving technology has been developed. Accurate positioning is an important component of an autonomous vehicle, and the Global Positioning System (GPS) and the Inertial Navigation System (INS) are two commonly used navigation systems.
The Global Positioning System (GPS) has all-weather real-time navigation capability, has the advantages of stable data output, high precision, low cost and the like, but has the defects of low data output rate, easy signal loss, incapability of providing navigation information and the like. The GPS positioning needs to receive signals of at least four satellites, but when a vehicle travels to a road, a tunnel, an underground parking lot, or other areas severely blocked by trees or buildings, the GPS cannot receive enough satellite signals to perform positioning, and there is no positioning output. In this case, the inertial navigation device may be used for the positioning calculation.
The strapdown Inertial Navigation System (INS) does not depend on any external information and has strong autonomy. The method is not influenced by the external environment, and can provide complete real-time navigation parameters of the carrier. The fully autonomous navigation system including the position, the speed and the attitude of the carrier has the capacity of resisting disturbance and confidentiality, but has certain defects in the aspect of navigation error accumulation.
The Global Positioning System (GPS) and the Inertial Navigation System (INS) are usually integrated together to overcome the problem of error accumulation of the Inertial Navigation System (INS), but in the real-time integrated navigation solution process of the GPS/INS, the failure of the GPS may cause the system to work abnormally or the line precision to decrease, if the GPS cannot be recovered for a long time, the errors of the accelerometer and the gyroscope may accumulate over time, which has great influence on the attitude and the positioning precision and influences the navigation result.
Disclosure of Invention
The invention aims to provide a combined navigation method and a system based on multi-sensor fusion, which can solve one or more of the problems in the prior art.
According to one aspect of the invention, a combined navigation method based on multi-sensor fusion is provided, which comprises the following steps:
when the GPS signal is effective, acquiring initial GPS navigation information;
acquiring data of the odometer, and judging whether the carrier is in a static state or not according to the data of the odometer;
if the carrier is in a motion state, judging whether the GPS signal is effective or not;
if the GPS signal is effective, correcting the error of the inertial measurement element by using GPS navigation information and outputting a navigation result;
and if the GPS signal fails, correcting the error of the inertial measurement element by using the data of the odometer and outputting a navigation result.
In some embodiments, collecting data output by the accelerometer and the magnetometer, and determining the carrier coordinate system comprises: if the GPS signal is effective, the correcting the error of the inertial measurement unit by using the GPS navigation information and outputting the navigation result comprises the following steps:
the state model is established as follows:
Figure BDA0002324382480000021
in the formula, F1(t) is a state transition matrix, and the calculation formula of the state transition matrix is as follows:
Figure BDA0002324382480000022
and determining the system state quantity by taking the northeast geographic coordinate system as a reference coordinate system as follows:
Figure BDA0002324382480000023
in the formula, δ L, δ λ, and δ h represent latitude error, longitude error, and altitude error, respectively; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zIs the attitude angle error; epsilonx、εyAnd εzRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure BDA0002324382480000024
and
Figure BDA0002324382480000025
random errors of accelerometers in three axial directions of a northeast coordinate system;
calculating the error covariance:
P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)
calculating a Kalman gain matrix:
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1
calculating an estimated value of the state quantity at the time t:
x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]
updating the error covariance matrix, and replacing the error covariance matrix with the time update:
P(t)＝[I-K(t)H(t)]P(t,t-1)
h (t) is a spatial mapping matrix, and H (t) is expressed as follows:
Figure BDA0002324382480000031
the system observation equation is composed of position difference between an inertial measurement element and GPS navigation information output, and comprises the following steps:
Figure BDA0002324382480000032
in the formula, LINSLatitude, L, output for inertial measurement unitGPSLatitude of GPS output; lambda [ alpha ]INSLongitude, λ, of output of inertial measuring unitGPSLongitude as GPS output; h isINSFor the height, h, output by the inertial measurement unitGPSHeight of the GPS output; selection of the WGS-84 standard, R, in a spheroid modelmRadius of curvature of meridian, RnThe radius of curvature of the mortise and unitary ring; h is height and L is latitude.
In some embodiments, correcting inertial measurement unit errors using odometer data and outputting navigation results if the GPS signal fails comprises:
the state model is established as follows:
Figure BDA0002324382480000033
in the formula, F1(t) is a state transition matrix, and the calculation formula of the state transition matrix is as follows:
Figure BDA0002324382480000034
and determining the system state quantity by taking the northeast geographic coordinate system as a reference coordinate system as follows:
Figure BDA0002324382480000035
in the formula, δ L, δ λ, and δ h represent latitude error, longitude error, and altitude error, respectively; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zIs the attitude angle error; epsilonx、εyAnd εzRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure BDA0002324382480000036
and
Figure BDA0002324382480000037
random errors of accelerometers in three axial directions of a northeast coordinate system;
calculating the error covariance:
P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)
calculating a Kalman gain matrix:
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1
calculating an estimated value of the state quantity at the time t:
x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]
updating the error covariance matrix, and replacing the error covariance matrix with the time update:
P(t)＝[I-K(t)H(t)]P(t,t-1)
h (t) is a spatial mapping matrix, and H (t) is expressed as follows:
Figure BDA0002324382480000041
the system observation equation is composed of position difference between an inertial measurement element and GPS navigation information output, and comprises the following steps:
Figure BDA0002324382480000042
wherein n represents a geodetic coordinate system, b represents a carrier coordinate system,
Figure BDA0002324382480000043
is the speed of the output of the inertia measuring element under the earth coordinate system,
Figure BDA0002324382480000044
the speed output by the odometer under the geodetic coordinate system;
Figure BDA0002324382480000045
in order to convert the matrix, the first and second matrices,
Figure BDA0002324382480000046
the speed output by the odometer in the carrier coordinate system.
In some embodiments, the system state quantities are determined as follows, using a northeast geographic coordinate system as a reference coordinate system:
Figure BDA0002324382480000047
the method specifically comprises the following steps:
selecting WGS84 standard in the ellipsoid of revolution model with the northeast geographic coordinate system as the reference coordinate system, wherein R iseIs the equatorial plane radius, f is the oblateness of an ellipsoid, RmRadius of curvature of meridian, RnThe radius of curvature of the prime circle is delta L, delta lambda and delta h respectively represent latitude error, longitude error and height error; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zIs the attitude angle error; epsilonx、εyAnd εzRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure BDA0002324382480000048
and
Figure BDA0002324382480000049
random error of accelerometer for three axial directions of northeast coordinate system
The attitude error model is established as follows:
Figure BDA00023243824800000410
Figure BDA00023243824800000411
Figure BDA00023243824800000412
in the formula, ωieTo follow the angular velocity of rotation of the earth;
the velocity error model is established as follows:
Figure BDA00023243824800000413
Figure BDA0002324382480000051
Figure BDA0002324382480000052
Figure BDA0002324382480000053
Rm＝Re(1-2f+3f sin2L)
Rn＝Re(1+f sin2L)
in the formula (f)x、fyAnd fzIs specific force;
the position error model is established as follows:
Figure BDA0002324382480000054
Figure BDA0002324382480000055
Figure BDA0002324382480000056
the gyroscope error model is established as follows:
Figure BDA0002324382480000057
in the formula (I), the compound is shown in the specification,
Figure BDA0002324382480000058
Tgiis epsilongiThe relative time constant of (c).
The accelerometer error model is established as follows:
Figure BDA0002324382480000059
in the formula, TaiIs the correlation time constant.
In a second aspect, an embodiment of the present invention provides a multi-sensor fusion-based integrated navigation system, configured to perform any one of the above multi-sensor fusion-based integrated navigation methods, where the system includes: the data output ends of the inertial measurement element, the GPS receiving device and the odometer are connected with the input end of the processor, and the processor is used for acquiring the data output by the inertial measurement element, the GPS receiving device and the odometer, processing the data and outputting final navigation information.
In some embodiments, the inertial measurement unit includes an accelerometer and a gyroscope, the outputs of the accelerometer and gyroscope each being connected to the input of the processor.
In some embodiments, the processor includes a filter.
In some embodiments, outputting the final navigation information includes carrier position information, carrier velocity information, and carrier attitude information.
Compared with the prior art, the technical scheme of the application has the following beneficial effects:
according to the navigation method provided by the invention, when the GPS is effective, the GPS navigation information is used for correcting the error of the inertial measurement element, and when the GPS fails, the odometer is used for compensating so as to inhibit the error divergence, so that accurate position and azimuth information is provided, the navigation method meets the navigation requirement of an automatic driving vehicle, when the GPS fails, stable and reliable navigation can be kept, the navigation precision and the anti-interference capability are effectively improved, and the application range of a navigation system is improved.
In addition, in the technical scheme of the invention, the technical scheme can be realized by adopting the conventional means in the field unless being particularly described.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and those skilled in the art can also obtain other drawings according to the drawings without creative efforts.
Fig. 1 is a flowchart of a combined navigation method based on multi-sensor fusion according to an embodiment of the present application.
Fig. 2 is a schematic structural diagram of a combined navigation system based on multi-sensor fusion according to another embodiment of the present application.
FIG. 3 is a graph comparing the results of the straight line segment experiment.
Fig. 4 is a graph comparing experimental results of straight line sections with complex terrain.
Fig. 5 is a graph comparing experimental results of the S-shaped bridge.
Detailed Description
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, but not all, embodiments of the present invention. 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.
Example 1:
fig. 1 shows a combined navigation method based on multi-sensor fusion according to an embodiment of the present invention, which includes the following steps:
s11: when the GPS signal is effective, acquiring initial GPS navigation information;
s12: acquiring data of the odometer, and judging whether the carrier is in a static state or not according to the data of the odometer;
s13: if the carrier is in a motion state, judging whether the GPS signal is effective or not;
s14: if the GPS signal is effective, correcting the error of the inertial measurement element by using GPS navigation information and outputting a navigation result;
s15: and if the GPS signal fails, correcting the error of the inertial measurement element by using the data of the odometer and outputting a navigation result.
In an alternative embodiment, if the GPS signal is valid, correcting the inertial measurement unit error using the GPS navigation information and outputting the navigation result includes:
the state model is established as follows:
Figure BDA0002324382480000071
in the formula, F1(t) is a state transition matrix, and the calculation formula of the state transition matrix is as follows:
Figure BDA0002324382480000072
and determining the system state quantity by taking the northeast geographic coordinate system as a reference coordinate system as follows:
Figure BDA0002324382480000073
in the formula, δ L, δ λ, and δ h represent latitude error, longitude error, and altitude error, respectively; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zIs the attitude angle error; epsilonx、εyAnd εzRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure BDA0002324382480000074
and
Figure BDA0002324382480000075
random errors of accelerometers in three axial directions of a northeast coordinate system;
calculating the error covariance:
P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)
calculating a Kalman gain matrix:
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1
calculating an estimated value of the state quantity at the time t:
x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]
updating the error covariance matrix, and replacing the error covariance matrix with the time update:
P(t)＝[I-K(t)H(t)]P(t,t-1)
h (t) is a spatial mapping matrix, and H (t) is expressed as follows:
Figure BDA0002324382480000081
the system observation equation is composed of position difference between an inertial measurement element and GPS navigation information output, and comprises the following steps:
Figure BDA0002324382480000082
in the formula, LINSLatitude, L, output for inertial measurement unitGPSLatitude of GPS output; h isINSLongitude, λ, of output of inertial measuring unitGPSLongitude as GPS output; h isINSFor the height, h, output by the inertial measurement unitGPSHeight of the GPS output; selection of the WGS-84 standard, R, in a spheroid modelmRadius of curvature of meridian, RnThe radius of curvature of the mortise and unitary ring; h is height and L is latitude.
In an alternative embodiment, the correcting the inertial measurement unit error using the odometer data and outputting the navigation result if the GPS signal fails comprises:
the state model is established as follows:
Figure BDA0002324382480000083
in the formula, F1(t) is a state transition matrix, and the calculation formula of the state transition matrix is as follows:
Figure BDA0002324382480000084
and determining the system state quantity by taking the northeast geographic coordinate system as a reference coordinate system as follows:
Figure BDA0002324382480000085
in the formula, δ L, δ λ, and δ h represent latitude error, longitude error, and altitude error, respectively; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zIs the attitude angle error; epsilonx、εyAnd εzRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure BDA0002324382480000086
and
Figure BDA0002324382480000087
random errors of accelerometers in three axial directions of a northeast coordinate system;
calculating the error covariance:
P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)
calculating a Kalman gain matrix:
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1
calculating an estimated value of the state quantity at the time t:
x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]
updating the error covariance matrix, and replacing the error covariance matrix with the time update:
P(t)＝[I-K(t)H(t)]P(t,t-1)
h (t) is a spatial mapping matrix, and H (t) is expressed as follows:
Figure BDA0002324382480000091
the system observation equation is composed of position difference between an inertial measurement element and GPS navigation information output, and comprises the following steps:
Figure BDA0002324382480000092
wherein n represents a geodetic coordinate system, b represents a carrier coordinate system,
Figure BDA0002324382480000093
is the speed of the output of the inertia measuring element under the earth coordinate system,
Figure BDA0002324382480000094
the speed output by the odometer under the geodetic coordinate system;
Figure BDA0002324382480000095
in order to convert the matrix, the first and second matrices,
Figure BDA0002324382480000096
the speed output by the odometer in the carrier coordinate system.
In an alternative embodiment, the system state quantity is determined as follows by taking the northeast geographic coordinate system as a reference coordinate system:
Figure BDA0002324382480000097
the method specifically comprises the following steps:
selecting WGS84 standard in the ellipsoid of revolution model with the northeast geographic coordinate system as the reference coordinate system, wherein R iseIs the equatorial plane radius, f is the oblateness of an ellipsoid, RmRadius of curvature of meridian, RnThe radius of curvature of the prime circle is delta L, delta lambda and delta h respectively represent latitude error, longitude error and height error; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zPitch angle error, roll angle error and yaw angle error respectively; epsilonx、εyAnd εzRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure BDA0002324382480000098
and
Figure BDA0002324382480000099
random errors of accelerometers in three axial directions of a northeast coordinate system;
the attitude error model is established as follows:
Figure BDA00023243824800000910
Figure BDA00023243824800000911
Figure BDA00023243824800000912
in the formula, ωieTo follow the angular velocity of rotation of the earth;
the velocity error model is established as follows:
Figure BDA0002324382480000101
Figure BDA0002324382480000102
Figure BDA0002324382480000103
Rm＝Re(1-2f+3f sin2L)
Rn＝Re(1+f sin2L)
in the formula (f)x、fyAnd fzIs specific force;
the position error model is established as follows:
Figure BDA0002324382480000104
Figure BDA0002324382480000105
Figure BDA0002324382480000106
the gyroscope error model is established as follows:
Figure BDA0002324382480000107
in the formula (I), the compound is shown in the specification,
Figure BDA0002324382480000108
Tgiis epsilongiThe relative time constant of (c).
The accelerometer error model is established as follows:
Figure BDA0002324382480000109
in the formula, TaiIs the correlation time constant.
In an alternative embodiment, F1(t) is a state transition matrix of 15 x 15, and partial elements in the matrix are expressed as follows:
Figure BDA00023243824800001010
Figure BDA00023243824800001011
Figure BDA00023243824800001012
F(1,5)＝-fZ
F(1,6)＝fy
Figure BDA0002324382480000111
Figure BDA0002324382480000112
Figure BDA00023243824800001116
Figure BDA00023243824800001117
F(2,4)＝fz
F(2,6)＝-fx
Figure BDA0002324382480000113
Figure BDA0002324382480000114
Figure BDA0002324382480000115
F(3,4)＝-fy
F(3,5)＝fx
F(3,7)＝-2ωieVxsinL
Figure BDA0002324382480000116
Figure BDA0002324382480000117
Figure BDA0002324382480000118
Figure BDA0002324382480000119
Figure BDA00023243824800001110
Figure BDA00023243824800001111
Figure BDA00023243824800001112
F(5,7)＝-ωiesinL
Figure BDA00023243824800001113
Figure BDA00023243824800001114
Figure BDA00023243824800001115
Figure BDA0002324382480000121
Figure BDA0002324382480000122
Figure BDA0002324382480000123
Figure BDA0002324382480000124
F(9,3)＝1
the other elements in the matrix are all 0.
According to the navigation method provided by the invention, when the GPS is effective, the GPS navigation information is used for correcting the error of the inertial measurement element, and when the GPS fails, the odometer is used for compensating so as to inhibit the error divergence, so that accurate position and azimuth information is provided, the navigation method meets the navigation requirement of an automatic driving vehicle, when the GPS fails, stable and reliable navigation can be kept, the navigation precision and the anti-interference capability are effectively improved, and the application range of a navigation system is improved.
Example 2:
fig. 2 shows a combined navigation system based on multi-sensor fusion according to embodiment 2 of the present invention, which is configured to execute any one of the combined navigation methods based on multi-sensor fusion in the foregoing method embodiments, and includes an inertial measurement unit 1, a GPS receiving device 2, an odometer 3, and a processor 4, where data output ends of the inertial measurement unit 1, the GPS receiving device 2, and the odometer 3 are connected to an input end of the processor 4, and the processor 4 is configured to acquire data output by the inertial measurement unit 1, the GPS receiving device 2, and the odometer 3, process the data, and output final navigation information.
In some embodiments, the inertial measurement unit 1 comprises an accelerometer and a gyroscope, the outputs of which are connected to the input of the processor 4.
In some embodiments, the processor 4 comprises a filter.
In some embodiments, outputting the final navigation information includes carrier position information, carrier velocity information, and carrier attitude information.
The integrated navigation system based on multi-sensor fusion in embodiment 2 can be used to execute the integrated navigation method based on multi-sensor fusion in embodiment 1 of the present invention, and accordingly achieve the technical effects achieved by the integrated navigation method based on multi-sensor fusion in embodiment 1 of the present invention, which are not described herein again.
Example 3:
the experiment carrier adopts a recreational vehicle of Foss, an inertia measuring element adopts a high-precision dynamic tilt angle sensor bw-vg500 which comprises a three-axis accelerometer and a three-axis gyroscope, the odometer is an automobile with an odometer, and a GPS which is communicated with a core star is selected. The sampling frequency of the inertial measurement unit is 20Hz and the frequency of the odometer is 20 Hz.
The experiment is divided into three motion scenes which are respectively (1) a straight line section, and the total distance is about 0.35 km; (2) a straight line section (including an ascending slope and a descending slope) with complex terrain has a total distance of about 1.7 km; (3) and the total distance of the S-shaped bridge (including the bent road) is about 2.8 km.
In each motion scene, three conditions are respectively set, wherein the first condition is that an inertial navigation system (inertial measurement unit) is used for outputting a navigation result, the second condition is that the inertial navigation system (inertial measurement unit) and a milemeter are used for outputting the navigation result in a combined manner, and the third condition is that the inertial navigation system (inertial measurement unit), a GPS and the milemeter are used for outputting the navigation result in a fused manner. In the first case, the navigation result output by the combined navigation method of the GPS and the inertial navigation system (inertial measurement unit) is simulated when the GPS fails.
Fig. 3 to 5 are experimental results of three different motion scenes, respectively, where three curves in each motion scene represent distance errors of three different navigation methods, respectively, where a black solid line represents a first navigation method, a dotted line represents a second navigation method, and a gray solid line represents a third navigation method.
It can be seen from the experimental results that when GPS fails and there is no odometer assisted compensation, the errors of the inertial navigation system (inertial measurement unit) accumulate and increase as the complexity of the road situation increases.
When the GPS fails but the odometer assists in compensation, the error of an inertial navigation system (an inertial measurement unit) can be corrected, so that the accurate positioning is realized, and the navigation precision is improved.
The experimental result proves that when the GPS fails, the odometer is used for compensating so as to inhibit error divergence, so that accurate position and azimuth information is provided, the navigation requirement of the automatic driving vehicle is met, stable and reliable navigation can be kept when the GPS fails, the navigation precision and the anti-interference capability are effectively improved, and the application range of the navigation system is enlarged.
The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and the parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.
Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware. With this understanding in mind, the above-described technical solutions may be embodied in the form of a software product, which can be stored in a computer-readable storage medium such as ROM/RAM, magnetic disk, optical disk, etc., and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the methods described in the embodiments or some parts of the embodiments.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (8)

1. The integrated navigation method based on the multi-sensor fusion is characterized by comprising the following steps:
when the GPS signal is effective, acquiring initial GPS navigation information;
acquiring data of the odometer, and judging whether the carrier is in a static state or not according to the data of the odometer;
if the carrier is in a motion state, judging whether the GPS signal is effective or not;
if the GPS signal is effective, correcting the error of the inertial measurement element by using GPS navigation information and outputting a navigation result;
and if the GPS signal fails, correcting the error of the inertial measurement element by using the data of the odometer and outputting a navigation result.
2. The integrated navigation method based on multi-sensor fusion of claim 1, wherein the using the GPS navigation information to correct the inertial measurement unit error and output the navigation result if the GPS signal is valid comprises:
the state model is established as follows:
Figure FDA0002324382470000011
in the formula, F1(t) is a state transition matrix, and the calculation formula of the state transition matrix is as follows:
Figure FDA0002324382470000012
and determining the system state quantity by taking the northeast geographic coordinate system as a reference coordinate system as follows:
Figure FDA0002324382470000013
in the formula, δ L, δ λ, and δ h represent latitude error, longitude error, and altitude error, respectively; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zIs the attitude angle error; epsilonx、εyAnd εzRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure FDA0002324382470000014
and
Figure FDA0002324382470000015
random errors of accelerometers in three axial directions of a northeast coordinate system;
calculating the error covariance:
P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)
calculating a Kalman gain matrix:
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1
calculating an estimated value of the state quantity at the time t:
x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]
updating the error covariance matrix, and replacing the error covariance matrix with the time update:
P(t)＝[I-K(t)H(t)]P(t,t-1)
h (t) is a spatial mapping matrix, and H (t) is expressed as follows:
Figure FDA0002324382470000021
the system observation equation is composed of position difference between an inertial measurement element and GPS navigation information output, and comprises the following steps:
Figure FDA0002324382470000022
in the formula, LINSLatitude, L, output for inertial measurement unitGPSLatitude of GPS output; lambda [ alpha ]INSLongitude, λ, of output of inertial measuring unitGPSLongitude as GPS output; h isINSFor the height, h, output by the inertial measurement unitGPSHeight of the GPS output; selection of the WGS-84 standard, R, in a spheroid modelmRadius of curvature of meridian, RnThe radius of curvature of the mortise and unitary ring; h is height and L is latitude.
3. The integrated navigation method based on multi-sensor fusion of claim 1, wherein the correcting the inertial measurement unit error and outputting the navigation result using the odometer data if the GPS signal fails comprises:
the state model is established as follows:
Figure FDA0002324382470000023
in the formula, F1(t) is a state transition matrix, and the calculation formula of the state transition matrix is as follows:
Figure FDA0002324382470000024
and determining the system state quantity by taking the northeast geographic coordinate system as a reference coordinate system as follows:
Figure FDA0002324382470000025
in the formula, δ L, δ λ, and δ h represent latitude error, longitude error, and altitude error, respectively; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zIs the attitude angle error; epsilonx、εyAnd εtRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure FDA0002324382470000026
and
Figure FDA0002324382470000027
random errors of accelerometers in three axial directions of a northeast coordinate system;
calculating the error covariance:
P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)
calculating a Kalman gain matrix:
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1
calculating an estimated value of the state quantity at the time t:
x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]
updating the error covariance matrix, and replacing the error covariance matrix with the time update:
P(t)＝[I-K(t)H(t)]P(t,t-1)
h (t) is a spatial mapping matrix, and H (t) is expressed as follows:
Figure FDA0002324382470000031
the system observation equation is composed of position difference between an inertial measurement element and GPS navigation information output, and comprises the following steps:
Figure FDA0002324382470000032
wherein n represents a geodetic coordinate system, b represents a carrier coordinate system,
Figure FDA0002324382470000033
is the speed of the output of the inertia measuring element under the earth coordinate system,
Figure FDA0002324382470000034
the speed output by the odometer under the geodetic coordinate system;
Figure FDA0002324382470000035
in order to convert the matrix, the first and second matrices,
Figure FDA0002324382470000036
the speed output by the odometer in the carrier coordinate system.
4. The integrated navigation method based on multi-sensor fusion according to claim 2 or 3, characterized in that the system state quantities are determined by taking the northeast geographic coordinate system as a reference coordinate system as follows:
Figure FDA0002324382470000037
the method specifically comprises the following steps:
selecting WGS84 standard in the ellipsoid of revolution model with the northeast geographic coordinate system as the reference coordinate system, wherein R iseIs the equatorial plane radius, f is the oblateness of an ellipsoid, RmRadius of curvature of meridian, RnThe radius of curvature of the prime circle is delta L, delta lambda and delta h respectively represent latitude error, longitude error and height error; delta Vx、δVyAnd δ VzThe speed errors in the east direction, the north direction and the sky direction; phi shapex、фyPhi-zPitch angle error, roll angle error and yaw angle error respectively; epsilonx、εyAnd εzRandom drift of the gyroscope in three axial directions of a northeast coordinate system;
Figure FDA0002324382470000038
and
Figure FDA0002324382470000039
random errors of accelerometers in three axial directions of a northeast coordinate system;
the attitude error model is established as follows:
Figure FDA00023243824700000310
Figure FDA00023243824700000311
Figure FDA0002324382470000041
in the formula, ωieTo follow the angular velocity of rotation of the earth;
the velocity error model is established as follows:
Figure FDA0002324382470000042
Figure FDA0002324382470000043
Figure FDA0002324382470000044
Rm＝Re(1-2f+3fsin2L)
Rn＝Re(1+fsin2L)
in the formula (f)x、fyAnd fzIs specific force;
the position error model is established as follows:
Figure FDA0002324382470000045
Figure FDA0002324382470000046
Figure FDA0002324382470000047
the gyroscope error model is established as follows:
Figure FDA0002324382470000048
in the formula (I), the compound is shown in the specification,
Figure FDA0002324382470000049
Tgiis epsilongiThe correlation time constant of (d);
the accelerometer error model is established as follows:
Figure FDA00023243824700000410
in the formula, TaiIs the correlation time constant.
5. Combined navigation system based on multi-sensor fusion for performing the combined navigation method based on multi-sensor fusion of any one of claims 1 to 4,
the navigation device comprises an inertial measurement element (1), a GPS receiving device (2), an odometer (3) and a processor (4), wherein the data output ends of the inertial measurement element (1), the GPS receiving device (2) and the odometer (3) are connected with the input end of the processor (4), and the processor (4) is used for acquiring the data output by the inertial measurement element (1), the GPS receiving device (2) and the odometer (3) to process and output final navigation information.
6. Combined navigation system based on multi-sensor fusion according to claim 5, characterized in that the inertial measurement unit (1) comprises an accelerometer and a gyroscope, the outputs of which are connected to the input of the processor (4).
7. Combined navigation system based on multi-sensor fusion according to claim 6, characterized in that the processor (4) comprises a filter.
8. The multi-sensor fusion-based integrated navigation system of claim 7, wherein the output final navigation information includes carrier position information, carrier velocity information, and carrier attitude information.
CN201911310476.4A 2019-12-18 2019-12-18 Combined navigation method and system based on multi-sensor fusion Pending CN111141273A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911310476.4A CN111141273A (en) 2019-12-18 2019-12-18 Combined navigation method and system based on multi-sensor fusion

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911310476.4A CN111141273A (en) 2019-12-18 2019-12-18 Combined navigation method and system based on multi-sensor fusion

Publications (1)

Publication Number Publication Date
CN111141273A true CN111141273A (en) 2020-05-12

Family

ID=70518794

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911310476.4A Pending CN111141273A (en) 2019-12-18 2019-12-18 Combined navigation method and system based on multi-sensor fusion

Country Status (1)

Country Link
CN (1) CN111141273A (en)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111854730A (en) * 2020-06-19 2020-10-30 中国煤炭科工集团太原研究院有限公司 Positioning method and system for unmanned light trackless rubber-tyred passenger car and freight car
CN112082547A (en) * 2020-09-08 2020-12-15 北京邮电大学 Integrated navigation system optimization method and device, electronic equipment and storage medium
CN112180412A (en) * 2020-09-23 2021-01-05 中国人民解放军空军工程大学 Relative positioning and orientation compensation method based on satellite navigation positioning system
CN113188539A (en) * 2021-04-27 2021-07-30 深圳亿嘉和科技研发有限公司 Combined positioning method of inspection robot
CN113295156A (en) * 2021-05-08 2021-08-24 西安达升科技股份有限公司 AGV fusion navigation system and method
CN113375667A (en) * 2021-07-15 2021-09-10 北京百度网讯科技有限公司 Navigation method, device, equipment and storage medium
CN113739817A (en) * 2020-05-29 2021-12-03 上海华依科技集团股份有限公司 Online automatic debugging method for signal fusion algorithm parameters of automobile combined navigation equipment
CN114964270A (en) * 2022-05-17 2022-08-30 驭势科技（北京）有限公司 Fusion positioning method and device, vehicle and storage medium
CN114964270B (en) * 2022-05-17 2024-04-26 驭势科技（北京）有限公司 Fusion positioning method, device, vehicle and storage medium

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB9803528D0 (en) * 1997-04-02 1998-04-15 Caterpillar Inc Method for monitoring the integrity of an intergrated gps and inu system
CN1948910A (en) * 2006-11-09 2007-04-18 复旦大学 Combined positioning method and apparatus using GPS, gyroscope, speedometer
CN105241454A (en) * 2015-10-23 2016-01-13 中国兵器工业集团第二一四研究所苏州研发中心 System and method for pedestrian navigation based on multiple sensors
CN106969762A (en) * 2017-01-12 2017-07-21 广州市泰斗鑫信息科技有限公司 A kind of Combinated navigation method for GNSS+INS+odo
CN108196285A (en) * 2017-11-30 2018-06-22 中山大学 A kind of Precise Position System based on Multi-sensor Fusion
CN109855646A (en) * 2019-04-30 2019-06-07 奥特酷智能科技(南京)有限公司 It is distributed centralized automated driving system and method

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB9803528D0 (en) * 1997-04-02 1998-04-15 Caterpillar Inc Method for monitoring the integrity of an intergrated gps and inu system
CN1948910A (en) * 2006-11-09 2007-04-18 复旦大学 Combined positioning method and apparatus using GPS, gyroscope, speedometer
CN105241454A (en) * 2015-10-23 2016-01-13 中国兵器工业集团第二一四研究所苏州研发中心 System and method for pedestrian navigation based on multiple sensors
CN106969762A (en) * 2017-01-12 2017-07-21 广州市泰斗鑫信息科技有限公司 A kind of Combinated navigation method for GNSS+INS+odo
CN108196285A (en) * 2017-11-30 2018-06-22 中山大学 A kind of Precise Position System based on Multi-sensor Fusion
CN109855646A (en) * 2019-04-30 2019-06-07 奥特酷智能科技(南京)有限公司 It is distributed centralized automated driving system and method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
宋志雪: "基于GPS/SINS/里程计的智能车导航定位算法研究", 《中国优秀硕士学位论文全文数据库•工程科技Ⅱ辑》 *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113739817A (en) * 2020-05-29 2021-12-03 上海华依科技集团股份有限公司 Online automatic debugging method for signal fusion algorithm parameters of automobile combined navigation equipment
CN113739817B (en) * 2020-05-29 2023-09-26 上海华依智造动力技术有限公司 Online automatic debugging method for signal fusion algorithm parameters of automobile integrated navigation equipment
CN111854730A (en) * 2020-06-19 2020-10-30 中国煤炭科工集团太原研究院有限公司 Positioning method and system for unmanned light trackless rubber-tyred passenger car and freight car
CN112082547A (en) * 2020-09-08 2020-12-15 北京邮电大学 Integrated navigation system optimization method and device, electronic equipment and storage medium
CN112082547B (en) * 2020-09-08 2022-05-10 北京邮电大学 Method and device for optimizing integrated navigation system, electronic equipment and storage medium
CN112180412B (en) * 2020-09-23 2023-05-02 中国人民解放军空军工程大学 Relative positioning and orientation compensation method based on satellite navigation positioning system
CN112180412A (en) * 2020-09-23 2021-01-05 中国人民解放军空军工程大学 Relative positioning and orientation compensation method based on satellite navigation positioning system
CN113188539A (en) * 2021-04-27 2021-07-30 深圳亿嘉和科技研发有限公司 Combined positioning method of inspection robot
CN113295156A (en) * 2021-05-08 2021-08-24 西安达升科技股份有限公司 AGV fusion navigation system and method
CN113295156B (en) * 2021-05-08 2024-05-03 西安达升科技股份有限公司 AGV fusion navigation system and method
CN113375667A (en) * 2021-07-15 2021-09-10 北京百度网讯科技有限公司 Navigation method, device, equipment and storage medium
CN114964270A (en) * 2022-05-17 2022-08-30 驭势科技（北京）有限公司 Fusion positioning method and device, vehicle and storage medium
CN114964270B (en) * 2022-05-17 2024-04-26 驭势科技（北京）有限公司 Fusion positioning method, device, vehicle and storage medium

Similar Documents

Publication Publication Date Title
CN111141273A (en) Combined navigation method and system based on multi-sensor fusion
CN112629538B (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN110501024B (en) Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system
CN109946732B (en) Unmanned vehicle positioning method based on multi-sensor data fusion
US9921065B2 (en) Unit and method for improving positioning accuracy
CN110031882B (en) External measurement information compensation method based on SINS/DVL integrated navigation system
CN110779521A (en) Multi-source fusion high-precision positioning method and device
US6459990B1 (en) Self-contained positioning method and system thereof for water and land vehicles
CN112097763B (en) Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
CN111536972B (en) Vehicle-mounted DR navigation method based on odometer scale factor correction
CN112504275B (en) Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm
CN111121766B (en) Astronomical and inertial integrated navigation method based on starlight vector
CN113203418B (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN102169184A (en) Method and device for measuring installation misalignment angle of double-antenna GPS (Global Position System) in integrated navigation system
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
US20170074678A1 (en) Positioning and orientation data analysis system and method thereof
CN111399023B (en) Inertial basis combined navigation filtering method based on lie group nonlinear state error
CN111982106A (en) Navigation method, navigation device, storage medium and electronic device
CN111721290A (en) Multi-source sensor information fusion positioning switching method
CN108225312B (en) Lever arm estimation and compensation method in GNSS/INS loose combination
CN107830873B (en) High-precision vehicle positioning and orientation method based on combination of single-shaft horizontal rotation inertial navigation and odometer
CN112212862A (en) GPS/INS integrated navigation method for improving particle filtering
CN111722295A (en) Underwater strapdown gravity measurement data processing method
CN113566850B (en) Method and device for calibrating installation angle of inertial measurement unit and computer equipment
CN117053802A (en) Method for reducing positioning error of vehicle navigation system based on rotary MEMS IMU

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
CB03 Change of inventor or designer information

Inventor after: Xian Chengjun

Inventor after: Xu Minghong

Inventor after: Shi Guangdie

Inventor after: Wang Chunbo

Inventor after: Jin Yufeng

Inventor before: Xian Chengjun

Inventor before: Xu Minghong

Inventor before: Shi Guangdie

Inventor before: Wang Chunbo

Inventor before: Wu Zhigang

Inventor before: Xu Kaiming

Inventor before: Jin Yufeng

CB03 Change of inventor or designer information
RJ01 Rejection of invention patent application after publication

Application publication date: 20200512

RJ01 Rejection of invention patent application after publication