CN118293907A - Zero-speed detection and correction method, device, equipment and medium of integrated navigation system - Google Patents
Zero-speed detection and correction method, device, equipment and medium of integrated navigation system Download PDFInfo
- Publication number
- CN118293907A CN118293907A CN202310012682.7A CN202310012682A CN118293907A CN 118293907 A CN118293907 A CN 118293907A CN 202310012682 A CN202310012682 A CN 202310012682A CN 118293907 A CN118293907 A CN 118293907A
- Authority
- CN
- China
- Prior art keywords
- data
- zero
- vehicle
- detection result
- speed
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 251
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000012937 correction Methods 0.000 title claims abstract description 44
- 239000011159 matrix material Substances 0.000 claims description 63
- 238000001914 filtration Methods 0.000 claims description 60
- 230000001133 acceleration Effects 0.000 claims description 24
- 238000012545 processing Methods 0.000 claims description 23
- 238000000546 chi-square test Methods 0.000 claims description 16
- 238000012360 testing method Methods 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 12
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 5
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 238000005259 measurement Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000007619 statistical method Methods 0.000 description 1
- 238000010998 test method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- 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/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
The application belongs to the technical field of vehicle navigation, and discloses a zero-speed detection and correction method, device, equipment and medium of a combined navigation system, wherein the method comprises the following steps: acquiring navigation data information of the integrated navigation system, wherein the navigation data information comprises IMU data, GPS data and WHEEL data; zero-speed detection is carried out according to the navigation data information to obtain a detection result, and the motion state of the vehicle is judged according to the detection result; when the motion state of the vehicle is in a completely stationary state, the integrated navigation system is subjected to zero-speed correction. The application can accurately detect the zero-speed state of the vehicle motion by utilizing the combination mode of the IMU data and the GPS data WHEEL data, thereby being convenient for correcting bias of the IMU when the vehicle is stationary, reducing track drift and obviously improving the positioning navigation precision.
Description
Technical Field
The application relates to the technical field of vehicle navigation, in particular to a zero-speed detection and correction method, device, equipment and medium of an integrated navigation system.
Background
Along with the continuous development of computer technology and sensor technology, the research field of vehicle integrated navigation also makes great progress, and the positioning precision requirement of vehicles on integrated navigation is also continuously improved; at present, a combined navigation system is commonly used for improving the accuracy of vehicle navigation.
At present, most solutions of integrated navigation systems use an IMU sensor, a GPS sensor and a WHEEL sensor for fusion positioning to form the integrated navigation positioning system of the IMU sensor, the GPS sensor and the WHEEL sensor. However, during the movement of the vehicle, the IMU (Inertial measurement unit ) estimates the position of the vehicle under the influence of bias, which results in a deviation between the estimated result and the actual position of the vehicle, and if the errors are accumulated for a long time without correction, the vehicle track estimated by the combined navigation will have a significant drift.
Disclosure of Invention
The invention provides a zero-speed detection and correction method, device, equipment and medium of an integrated navigation system, and aims to solve the technical problem that in the existing vehicle movement process, the IMU is influenced by bias to the vehicle positioning estimation, so that the estimation result is offset from the actual position of the vehicle, and if long-time error accumulation is not carried out, the vehicle track estimated by integrated navigation is obviously drifted.
In order to achieve the above object, a first aspect of the present invention provides a zero-speed detecting and correcting method for an integrated navigation system, comprising:
Acquiring navigation data information of the integrated navigation system, wherein the navigation data information comprises IMU data, GPS data and WHEEL data;
zero-speed detection is carried out according to the navigation data information to obtain a detection result, and the motion state of the vehicle is judged according to the detection result;
when the motion state of the vehicle is in a completely stationary state, the integrated navigation system is subjected to zero-speed correction.
Further, the acquiring navigation data information of the integrated navigation system includes:
Acquiring IMU data of the vehicle based on the IMU sensor, acquiring GPS data of the vehicle based on the GPS sensor and acquiring WHEEL data of the vehicle based on the WHEEL sensor;
Performing first-order low-pass filtering processing on the IMU data to obtain filtering processing data of the IMU sensor;
and outputting the filtering processing data, the GPS data and the WHEEL data as the navigation data information.
Further, the first-order low-pass filtering has a calculation formula:
gk(Xk)＝b·Xk+(1-b)gk-1(Xk-1)；
wherein:
Xk＝(a g)T，
X k represents accelerometer and gyroscope values sampled by the IMU sensor for the kth time, a represents an acceleration count value sampled by the IMU sensor, g represents a gyroscope value sampled by the IMU sensor, g k(Xk) represents an output value after the kth filtering, g k-1(Xk-1) represents an output value after the kth-1 filtering, b represents a filtering coefficient, T is a transposed matrix, k epsilon N *, and k is more than or equal to 1.
Further, the step of performing zero-speed detection according to the navigation data information to obtain a detection result includes:
performing first zero-speed detection on the vehicle based on the IMU data to obtain a first detection result;
Performing second zero-speed detection on the vehicle based on the GPS data to obtain a second detection result;
Performing third zero-speed detection on the vehicle based on the WHEEL data to obtain a third detection result;
detecting the vehicle at a fourth zero speed based on chi-square test to obtain a fourth detection result;
and judging the motion state of the vehicle according to the first detection result, the second detection result, the third detection result and the fourth detection result.
Further, the step of performing a first zero speed detection on the vehicle based on the IMU data to obtain a first detection result includes:
judging whether the acceleration count value obtained by sampling the IMU sensor is smaller than a first preset threshold value and whether the gyroscope value is smaller than a second preset threshold value;
And when the acceleration count value is smaller than the first preset threshold value and the gyroscope value is smaller than the second preset threshold value, obtaining the first detection result, wherein the first detection result is that the vehicle is in a relatively stationary state.
Further, the GPS data includes GPS location data and GPS velocity data;
the step of performing a second zero-speed detection on the vehicle based on the GPS data to obtain a second detection result comprises the following steps:
judging whether the GPS position data meets a first preset condition and whether the GPS speed data meets a second preset condition;
And when the GPS position data meets a first preset condition and the GPS speed data meets a second preset condition, obtaining a second detection result, wherein the second detection result is that the vehicle is in a relatively stationary state.
Further, the step of performing a third zero speed detection on the vehicle based on the WHEEL data to obtain a third detection result includes:
judging whether the WHEEL data output is zero or not;
and when the WHEEL data output is zero, obtaining a third detection result, wherein the second detection result is that the vehicle is in a relatively stationary state.
Further, the step of performing a fourth zero-speed detection on the vehicle based on chi-square test to obtain a fourth detection result includes:
Defining a combined navigation coordinate system based on the position of each sensor relative to the vehicle, the combined navigation coordinate system comprising an IMU coordinate system, a GPS coordinate system, a vehicle body coordinate system, and a world coordinate system;
Defining the system state quantity of the integrated navigation system as:
X＝(Pwb Vwb θwb ba bg θbc pbc)T；
The system state quantity X is a vector with a preset dimension, a subscript w represents a world coordinate system when the integrated navigation system is started, a subscript b represents the IMU coordinate system, a subscript c represents the vehicle body coordinate system, P wb represents three-dimensional position information of the system state quantity, V wb represents three-dimensional speed information theta wb of the system state quantity, three-dimensional rotation information b a of the system state quantity represents accelerometer bias, b g represents gyroscope bias, theta bc represents a rotation conversion relation between the vehicle body coordinate system and the IMU coordinate system, and P bc represents a displacement conversion relation between the vehicle body coordinate system and the IMU coordinate system;
Calculating the state quantity of the noise of the integrated navigation system:
M＝(Vi θi)，
Wherein M is the state quantity of the combined navigation system noise, V i is the state quantity of the accelerometer noise, and theta i is the state quantity of the gyroscope noise;
Calculating covariance matrix of the noise of the combined navigation system:
Wherein,
Representing the noise variance value of the accelerometer,Representing the noise variance value of the gyroscope, I is the identity matrix,AndThe value of (2) is calculated by an Allan variance method;
Constructing an observation equation with zero speed update based on accelerometer constraint and gyroscope constraint:
wherein a is an acceleration count value obtained after first-order low-pass filtering processing, g is a gyroscope value obtained after first-order low-pass filtering processing, and the matrix is expressed as follows:
representing a rotation matrix between the IMU coordinate system and the world coordinate system, G representing local gravitational acceleration;
Calculating a jacobian matrix of the system state quantity according to the observation equation updated at zero speed:
Wherein, An antisymmetric matrix of the matrix G b;
And carrying out zero-speed detection based on chi-square detection, and constructing a chi-square detection value of a zero-speed detection model:
Wherein,
Sc＝H·P·HT+50·R，
P represents a covariance matrix of an error state Kalman filtering error state, and e represents a chi-square test value;
and inquiring a critical value of the test statistic based on the degree of freedom and the significance level, judging whether the test statistic is in a preset reject domain, and generating a fourth detection result when the test statistic is in the preset reject domain.
Further, the step of determining the motion state of the vehicle according to the first detection result, the second detection result, the third detection result, and the fourth detection result includes:
And when the first detection result, the second detection result, the third detection result and the fourth detection result are all that the vehicle is in a relatively stationary state, judging that the motion state of the vehicle is a completely stationary state.
Further, the step of performing zero-speed correction on the integrated navigation system includes:
Calculating the Kalman gain of error state Kalman filtering according to the Jacobian matrix:
K＝P·HT·S-1，
Wherein,
S＝H·P·HT+R，
Calculating the variation of the error state according to the Kalman gain:
ΔR＝K·f(x)；
Updating and correcting a covariance matrix of the error state in the error Kalman filtering based on the variation of the error state, wherein the covariance matrix is:
Pk+1＝(I-K·H)·Pk·(I-K·H)-1+K·R·K。
a second aspect of the present invention provides a zero-speed detecting and correcting device for an integrated navigation system, including:
The navigation data acquisition module is used for acquiring navigation data information of the integrated navigation system, wherein the navigation data information comprises IMU data, GPS data and WHEEL data;
The motion state judging module is used for carrying out zero-speed detection according to the navigation data information to obtain a detection result, and judging the motion state of the vehicle according to the detection result;
And the correction module is used for carrying out zero-speed correction on the integrated navigation system when the motion state of the vehicle is a completely stationary state.
A third aspect of the present invention proposes a computer device comprising a memory and a processor, said memory having stored therein a computer program, said processor implementing the steps of the zero-speed detection and correction method of the integrated navigation system as defined in any one of the preceding claims when said computer program is executed.
A fourth aspect of the invention proposes a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the zero speed detection and correction method of a integrated navigation system as described in any of the preceding claims.
The beneficial effects are that:
According to the application, the IMU data, the GPS data and the WHEEL data are used for respectively carrying out zero-speed detection, and the detection result obtained by combining each data can be used for accurately detecting the zero-speed state of the vehicle motion, so that the bias of the IMU can be conveniently corrected when the vehicle is stationary, the track drift is reduced, and the positioning navigation precision can be obviously improved.
Drawings
FIG. 1 is a flow chart of a method for detecting and correcting zero speed of a combined navigation system according to an embodiment of the invention;
FIG. 2 is a diagram illustrating coordinate system definition of a integrated navigation system according to an embodiment of the invention;
FIG. 3 is a schematic diagram of a zero-speed detection and correction device of an integrated navigation system according to an embodiment of the invention;
FIG. 4 is a block diagram of a zero-speed detection and correction device of a combined navigation system according to an embodiment of the invention;
The achievement of the objects, functional features and advantages of the present invention will be further described with reference to the accompanying drawings, in conjunction with the embodiments.
Detailed Description
The present application will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present application more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the application.
As used herein, the singular forms "a", "an", "the" and "the" are intended to include the plural forms as well, unless expressly stated otherwise, as understood by those skilled in the art. It will be further understood that the terms "comprises" and/or "comprising," when used in this specification, specify the presence of stated features, integers, steps, operations, elements, modules, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, modules, components, and/or groups thereof. It will be understood that when an element is referred to as being "connected" or "coupled" to another element, it can be directly connected or coupled to the other element or intervening elements may also be present. Further, "connected" or "coupled" as used herein may include wirelessly connected or wirelessly coupled. The term "and/or" as used herein includes all or any module and all combination of one or more of the associated listed items.
It will be understood by those skilled in the art that all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs unless defined otherwise. It will be further understood that terms, such as those defined in commonly used dictionaries, should be interpreted as having a meaning that is consistent with their meaning in the context of the prior art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein.
In a first aspect, referring to fig. 1, an embodiment of the present invention provides a zero-speed detection and correction method of a combined navigation system, including:
s1: acquiring navigation data information of a combined navigation system, wherein the navigation data information comprises IMU data, GPS data and WHEEL data;
Specifically, the integrated navigation system of the present application includes IMU (Inertial measurement unit ) data, GPS positioning navigation data and where the Inertial Measurement Unit (IMU) generally refers to a combined unit composed of 3 accelerometers and 3 gyroscopes, the accelerometers and gyroscopes are mounted on measurement axes perpendicular to each other, the IMU data includes a timestamp of vehicle action, an acceleration count value of 3 axes and a gyro value of 3 axes, the IMU data is acquired by an IMU sensor, accuracy of the IMU data can be corrected by other means, the GPS positioning navigation data is vehicle movement data acquired based on an on-board satellite navigation system, including a timestamp, longitude, latitude, altitude, speed and heading angle information, the GPS data can be used to correct long-term drift of vehicle position, the GPS data is acquired by a GPS sensor, the wheer data is WHEEL rotation speed data of a vehicle, obtained by a wheer (rotation speed) sensor provided at a tire of the vehicle, including a timestamp and speed information of the vehicle;
S2: zero-speed detection is carried out according to the navigation data information to obtain a detection result, and the motion state of the vehicle is judged according to the detection result;
Specifically, when the IMU data, the GPS data, and the where data acquired in step S1 are used alone to perform the zero speed detection, the zero speed detection is not performed well at this time when the vehicle is traveling in a tunnel or an underground parking lot due to the defects of the IMU sensor, the GPS sensor, and the where sensor itself, such as the GPS sensor, is used, but the vehicle is stopped in these cases, and thus these zero speed states are undetectable; the speed information output by the WHEL sensor also has a dead zone phenomenon, namely when the vehicle speed is smaller than a certain threshold value, the output of the WHEL sensor is always 0, so when the speed is detected to be 0 by the WHEL sensor, the vehicle is not necessarily in a completely stationary state, and a tiny speed possibly exists; at the moment that the vehicle turns from motion to stationary, four WHEELs of the vehicle are in a completely stationary state, but the vehicle body is still in a non-stationary state due to the action of inertia, and the IMU sensor is arranged on the vehicle body and is not arranged on the WHEELs, so that the accelerometer and the gyroscope of the IMU sensor still have data output at the moment, but the vehicle is in the completely stationary state, and the problem that the error of zero speed detection is overlarge when any one of the three modes is singly used for zero speed detection is caused, therefore, the application combines the IMU data, the GPS data and the WHEL data to carry out zero speed detection on the vehicle, and comprehensively judges whether the motion state of the vehicle is the completely stationary state or not by combining the data output by the IMU sensor, the GPS sensor and the WHEL sensor;
S3: when the motion state of the vehicle is in a completely stationary state, the integrated navigation system is subjected to zero-speed correction.
Specifically, when the motion state of the vehicle is in a completely stationary state, that is, when the motion state of the vehicle is determined by a zero-speed detection method of determining whether a GPS signal in GPS data is valid and whether a vehicle speed displayed in GPS data is 0, whether a vehicle speed displayed in the WHEEL data is 0, whether measured values of an IMU accelerometer and a gyroscope are smaller than a threshold value, and a chi-square test, zero-speed correction is performed on the integrated navigation system according to the IMU data, the GPS data, and the WHEEL data, so that drift of a vehicle movement track is reduced;
The application can accurately detect the zero-speed state of the vehicle motion by utilizing the mode of combining the IMU data, the GPS data and the WHEEL data, thereby being convenient for correcting bias of the IMU when the vehicle is stationary, reducing track drift and obviously improving the positioning navigation precision.
In one embodiment, the acquiring navigation data information of the integrated navigation system includes:
Acquiring IMU data of the vehicle based on the IMU sensor, acquiring GPS data of the vehicle based on the GPS sensor and acquiring WHEEL data of the vehicle based on the WHEEL sensor;
Specifically, the output frequency of the IMU sensor is 100Hz, the IMU data comprises a time stamp, a 3-axis acceleration count value and a 3-axis gyroscope value, the output frequency of the GPS sensor is 1Hz, the GPS data comprises a time stamp, longitude, latitude, altitude, speed and course angle information, and the output frequency of the WHEEL sensor is 1Hz and comprises a time stamp and speed information;
Performing first-order low-pass filtering processing on the IMU data to obtain filtering processing data of the IMU sensor;
Specifically, because the interference effect of noise, namely an interference signal, on the IMU data is large in the data acquired by the IMU sensor, the IMU data acquired by the IMU sensor is subjected to first-order low-pass filtering processing between zero-speed detection by utilizing the IMU data, the first-order low-pass filtering has the characteristics of small filtering coefficient and stable filtering result, the operation amount of the first-order filtering is small, the parameters required to be regulated are small, and the influence of high-order noise on the input data can be obviously restrained, so that the influence of noise on the IMU data is reduced;
and outputting the filtering processing data, the GPS data and the WHEEL data as the navigation data information.
And finally, outputting the filtering processing data, the GPS data and the WHEEL data obtained after the first-order low-pass filtering processing as navigation data information, and providing more accurate initial values for zero-speed detection and zero-speed correction to be carried out subsequently.
In one embodiment, the first order low pass filtering is calculated as:
gk(Xk)＝b·Xk+(1-b)gk-1(Xk-1)；
wherein:
Xk＝(a g)T，
X k represents accelerometer and gyroscope values sampled by the IMU sensor for the kth time, a represents an acceleration count value sampled by the IMU sensor, g represents a gyroscope value sampled by the IMU sensor, g k(Xk) represents an output value after the kth filtering, g k-1(Xk-1) represents an output value after the kth-1 filtering, b represents a filtering coefficient, T is a transposed matrix, k epsilon N *, and k is more than or equal to 1.
As described above, the IMU data after the first-order low-pass filtering process is similar to the original output data of the accelerometer and the original output data of the gyroscope with waveforms, but the mean value is different, and the variance is approximately the same, so that the influence of high-order noise of the accelerometer can be obviously restrained, and the IMU data after the first-order low-pass filtering process is beneficial to providing more accurate initial values for the follow-up zero-speed detection and zero-speed correction.
In one embodiment, the step of performing zero-speed detection according to the navigation data information to obtain a detection result includes:
When the three types of data are independently used for zero-speed detection based on the acquired IMU data, GPS data and white data, the zero-speed detection is not performed well at this time when the vehicle is traveling in a tunnel or an underground parking lot due to the defects of the IMU sensor, the GPS sensor and the white sensor, such as the use of the GPS sensor, but the vehicle is stopped under these conditions, so that these zero-speed states cannot be detected; the speed information output by the WHEL sensor also has a dead zone phenomenon, namely when the vehicle speed is smaller than a certain threshold value, the output of the WHEL sensor is always 0, so when the speed is detected to be 0 by the WHEL sensor, the vehicle is not necessarily in a completely stationary state, and a tiny speed possibly exists; at the moment that the vehicle turns from motion to stationary, four WHEELs of the vehicle are in a completely stationary state, but the vehicle body is still in a non-stationary state due to the action of inertia, and the IMU sensor is mounted on the vehicle body and is not mounted on the WHEELs, so that at the moment, the accelerometer and the gyroscope of the IMU sensor still have data output, but the vehicle is in the completely stationary state, so that the problem that the error of zero speed detection is overlarge when any one of the three modes is singly used for zero speed detection is caused, and therefore, the embodiment combines the IMU data, GPS data and WHEEL data and the data result of chi-square detection to carry out zero speed detection on the vehicle, thereby comprehensively judging the motion state of the vehicle by combining the data output by the IMU sensor, the GPS sensor and the WHEL sensor, and is realized by the following steps:
performing first zero-speed detection on the vehicle based on the IMU data to obtain a first detection result;
specifically, the measurement values of an accelerometer and a gyroscope of an IMU sensor are utilized to perform zero-speed detection, the data obtained by the accelerometer and the gyroscope of the IMU data acquired by the IMU sensor are needed to be subjected to zero-speed detection of a vehicle, and when the data of the accelerometer and the gyroscope of the sensor of the IMU are smaller than a certain threshold value, the vehicle can be considered to be in a stationary state, namely a first detection result is obtained;
Performing second zero-speed detection on the vehicle based on the GPS data to obtain a second detection result;
Specifically, the speed information of the vehicle acquired by the GPS sensor is used to determine whether the vehicle is in a stationary state, and the GPS data needs to satisfy the following conditions: the GPS signal is valid and the vehicle speed output in the GPS data is 0. By this condition, it is detected whether the vehicle is in a stationary state, wherein whether the GPS signal is valid or not can be judged by using the latitude, longitude and altitude information (altitude) in the GPS data, and when the GPS signal is invalid (for example, the GPS signal is blocked in the indoor and tunnel), the latitude, longitude and altitude output information in the GPS data is 0, thereby judging that the GPS signal is valid. The output speed of the GPS is 0, namely the stationary state of the vehicle is satisfied, so that whether the vehicle is in the stationary state or not can be judged according to the GPS data, and a second detection result is obtained;
Performing third zero-speed detection on the vehicle based on the WHEEL data to obtain a third detection result;
Specifically, whether the vehicle is in a stationary state is detected by using the speed information in the WHEEL data output by the WHEEL sensor, and when the speed of the vehicle in the speed information in the WHEEL data is 0, it can be judged that the vehicle satisfies the stationary state, namely, a third detection result is obtained
Detecting the vehicle at a fourth zero speed based on chi-square test to obtain a fourth detection result;
Specifically, in addition to the above three zero speed detection methods, the method of error kalman filtering and chi-square detection in this embodiment can also be used to effectively and accurately detect the zero speed state of the vehicle, that is, four detection results;
and judging the motion state of the vehicle according to the first detection result, the second detection result, the third detection result and the fourth detection result.
In summary, only when the first detection result, the second detection result, the third detection result and the fourth detection result show that the motion state of the vehicle is in a stationary state, the vehicle is in a completely stationary state, and then zero-speed detection and subsequent zero-speed correction can be started.
In one embodiment, the step of performing a first zero speed detection on the vehicle based on the IMU data to obtain a first detection result includes:
judging whether the acceleration count value obtained by sampling the IMU sensor is smaller than a first preset threshold value and whether the gyroscope value is smaller than a second preset threshold value;
And when the acceleration count value is smaller than the first preset threshold value and the gyroscope value is smaller than the second preset threshold value, obtaining the first detection result, wherein the first detection result is that the vehicle is in a relatively stationary state.
As described above, based on the acceleration count value and the gyroscope value sampled by the IMU sensor, only two of them simultaneously meet that the acceleration count value is smaller than a first preset threshold, such as smaller than 0.05m/s 2; and when the gyroscope value obtained by sampling the IMU sensor is smaller than a second preset threshold value, such as smaller than 0.05rad 2, the detection result obtained based on IMU data detection can be judged to be that the vehicle is in a relatively stationary state, wherein the first preset threshold value and the second preset threshold value are obtained according to a large amount of experimental data, and can be manually preset.
In one embodiment, the GPS data includes GPS location data and GPS velocity data;
the step of performing a second zero-speed detection on the vehicle based on the GPS data to obtain a second detection result comprises the following steps:
judging whether the GPS position data meets a first preset condition and whether the GPS speed data meets a second preset condition;
And when the GPS position data meets a first preset condition and the GPS speed data meets a second preset condition, obtaining a second detection result, wherein the second detection result is that the vehicle is in a relatively stationary state.
Specifically, the GPS position data includes latitude, longitude and altitude information of a GPS, the GPS speed is speed data of a vehicle obtained based on a satellite navigation system, such as a GPS navigation system, a beidou navigation system, a gnomonas navigation system, a galileo navigation system and the like, the first preset condition is that a GPS signal of the vehicle is valid, that is, latitude, longitude and altitude output information in the GPS data are not all 0, the GPS signal is judged to be valid, and the second preset condition is that the vehicle speed in the GPS data is 0, that is, a stationary state of the vehicle is satisfied.
In one embodiment, the step of performing a third zero speed detection on the vehicle based on the said white data to obtain a third detection result includes:
judging whether the WHEEL data output is zero or not;
and when the WHEEL data output is zero, obtaining a third detection result, wherein the second detection result is that the vehicle is in a relatively stationary state.
Specifically, when the speed is detected to be 0 by the WHEEL sensor, the vehicle is not necessarily in a completely stationary state, and there may be a small speed just meeting the speed of the dead zone phenomenon of the WHEEL sensor, so that when the speed is detected to be zero, the vehicle is still relatively in a stationary state, namely, the third detection result, in combination with other zero-speed detection modes is also needed.
In one embodiment, the step of performing a fourth zero-speed detection on the vehicle based on the chi-square test to obtain a fourth detection result includes:
Defining a combined navigation coordinate system based on the position of each sensor relative to the vehicle, the combined navigation coordinate system comprising an IMU coordinate system, a GPS coordinate system, a vehicle body coordinate system, and a world coordinate system;
Defining the system state quantity of the integrated navigation system as:
X＝(Pwb Vwb θwb ba bg θbc pbc)T；
The system state quantity X is a vector with a preset dimension, a subscript w represents a world coordinate system when the integrated navigation system is started, a subscript b represents the IMU coordinate system, a subscript c represents the vehicle body coordinate system, P wb represents three-dimensional position information of the system state quantity, V wb represents three-dimensional speed information theta wb of the system state quantity, three-dimensional rotation information b a of the system state quantity represents accelerometer bias, b g represents gyroscope bias, theta bc represents a rotation conversion relation between the vehicle body coordinate system and the IMU coordinate system, and P bc represents a displacement conversion relation between the vehicle body coordinate system and the IMU coordinate system;
Specifically, the IMU coordinate system is established with the position of the IMU sensor as an origin, the vehicle moving direction as an X axis and the direction perpendicular to the X axis as a Y axis, the GPS coordinate system is established with the position of the vehicle-mounted GPS antenna as an origin, the vehicle moving direction as an X axis and the direction perpendicular to the X axis as a Y axis, the vehicle body coordinate system is established with the position of the wheer sensor as an origin, the vehicle moving direction as an X axis and the direction perpendicular to the X axis as a Y axis, the world coordinate system is established with the vehicle center as an origin, the positive eastern direction as an X axis and the positive northern direction as a Y axis as a vector of 21 dimensions, the subscript w represents the world coordinate system when the integrated navigation system is started, that is, the vehicle-mounted northeast coordinate system of the vehicle, the coordinate definition of each sensor in the vehicle-mounted integrated navigation system is as shown in fig. 2, the IMU coordinate system is established with the position of the IMU sensor as an origin, the vehicle moving direction as an X axis and the direction perpendicular to the X axis as a GPS coordinate system, the vehicle moving direction as a latitude coordinate system, the vehicle coordinate system is established with the X axis, the vehicle-mounted antenna as an X axis and the vertical direction as a latitude coordinate system, and the latitude coordinate system is 84 is output; the vehicle body coordinate system is established by taking the position of the WHEEL sensor as an origin, the vehicle moving direction as an X axis and the direction perpendicular to the X axis as a Y axis, the world coordinate system is established by taking the center of the vehicle as the origin, the forward direction as the X axis and the forward direction as the Y axis, and the ENU coordinate system and the GPS coordinate system of the GPS sensor, namely the WGS-84 coordinate system, can be mutually converted.
Calculating the state quantity of the noise of the integrated navigation system:
M＝(Vi θi)，
Wherein M is the state quantity of the combined navigation system noise, V i is the state quantity of the accelerometer noise, and theta i is the state quantity of the gyroscope noise;
Calculating covariance matrix of the noise of the combined navigation system:
Wherein,
Representing the noise variance value of the accelerometer,Representing the noise variance value of the gyroscope, I is the identity matrix,AndThe value of (2) is calculated by an Allan variance method;
In particular, the method comprises the steps of, Representing the noise variance value of the accelerometer,Representing the noise variance value of the gyroscope, I is the identity matrix,AndIs calculated using an Allan-variance statistical method.
Constructing an observation equation with zero speed update based on accelerometer constraint and gyroscope constraint:
wherein a is an acceleration count value obtained after first-order low-pass filtering processing, g is a gyroscope value obtained after first-order low-pass filtering processing, and the matrix is expressed as follows:
representing a rotation matrix between the IMU coordinate system and the world coordinate system, G representing local gravitational acceleration;
Calculating a jacobian matrix of the system state quantity according to the observation equation updated at zero speed:
Wherein, An antisymmetric matrix of the matrix G b;
In particular, the method comprises the steps of, For the antisymmetric matrix of matrix G b, the jacobian matrix is a matrix with a size of 6 x 21, I represents the identity matrix of 3*3; the chi-square test is a hypothesis test method based on chi-square distribution, which divides the square of the difference between the actual observation times and the theoretical observation times by the theoretical times to obtain an approximate chi-square distribution, and then the abnormal value can be removed by using the property of the chi-square distribution.
And carrying out zero-speed detection based on chi-square detection, and constructing a chi-square detection value of a zero-speed detection model:
Wherein,
Sc＝H·P·HT+50·R，
P represents a covariance matrix of an error state Kalman filtering error state, and e represents a chi-square test value;
and inquiring a critical value of the test statistic based on the degree of freedom and the significance level, judging whether the test statistic is in a preset reject domain, and generating a fourth detection result when the test statistic is in the preset reject domain.
Specifically, P represents the covariance matrix of the error state kalman filter error state, which is a 21 x 21 dimension matrix, e is the chi-square test value, i.e. using the formulaAnd then inquiring the critical value of the test statistic through the degree of freedom and the significance level, and finally checking whether the test statistic is in a reject domain, and when the value of the test statistic, namely the value of E, is in a preset reject domain, judging that the vehicle is in a completely stationary state by combining IMU data, GPS data and WHEEL data, thereby carrying out zero-speed detection.
In one embodiment, the step of determining the motion state of the vehicle according to the first detection result, the second detection result, the third detection result, and the fourth detection result includes:
And when the first detection result, the second detection result, the third detection result and the fourth detection result are all that the vehicle is in a relatively stationary state, judging that the motion state of the vehicle is a completely stationary state.
Specifically, in view of the fact that any one of the three modes of the single IMU data, the GPS data and the white data generates a deviation when the zero speed detection is performed, in this embodiment, the detection results obtained by the single zero speed detection based on the single IMU data, the GPS data and the white data are respectively and individually combined with the detection results of the chi-square test to perform the comprehensive determination, that is, the detection results of the zero speed detection based on the IMU data, the detection results of the zero speed detection based on the white data, and the detection results of the chi-square test, when the vehicle is determined to be in the stationary state, and the subsequent zero speed correction is possible.
In one embodiment, the step of performing zero-speed correction on the integrated navigation system includes:
Calculating the Kalman gain of error state Kalman filtering according to the Jacobian matrix:
K＝P·HT·S-1，
Wherein,
S＝H·P·HT+R，
Calculating the variation of the error state according to the Kalman gain:
ΔR＝K·f(x)；
Updating and correcting a covariance matrix of the error state in the error Kalman filtering based on the variation of the error state, wherein the covariance matrix is:
Pk+1＝(I-K·H)·Pk·(I-K·H)-1+K·R·K。
Specifically, after the zero speed detection is performed by the method in the above steps, the zero speed correction needs to be completed after the detection is successful. During the movement process of the vehicle, the IMU is influenced by bias under the long-time movement, and the estimated position can generate obvious drift; when the vehicle is in a parking static state, the bias of the IMU is corrected by utilizing the characteristic of the vehicle static state, the precision of the combined navigation system can be obviously improved, the zero speed correction step is approximately the same as that of a zero speed detection method, and the difference is that after the jacobian matrix is calculated by utilizing an observation equation, the covariance matrix and the jacobian matrix of the error state Kalman filter of the combined navigation system are calculated, the Kalman gain of the error state Kalman filter of the combined navigation system is calculated, the variation of the error state of the combined navigation system is calculated by utilizing the error Kalman filter mode, so that the covariance matrix of the error state in the error Kalman filter is updated, namely the zero speed correction is completed, but after the above state updating is completed, the position information P wb of the state quantity at the previous moment is required to be assigned to the position at the current moment, the speed information V wb of the state quantity is reset to 0, the rotation information theta wb of the state quantity at the previous moment is assigned to the rotation quantity at the current moment, and the angle variation is ensured, and then the gyroscope and the gyroscope is updated to be zero speed.
In summary, the covariance matrix of the error state obtained by the error Kalman filtering calculation is utilized, the covariance matrix of noise and the Jacobian matrix of the error state are calculated to obtain an expected value of the chi-square test, then the expected value is compared with a set threshold value to obtain a zero-speed state of the vehicle, and the accelerometer bias and the gyroscope bias in the IMU sensor are updated in the vehicle stationary stage based on the error state Kalman filtering method, so that bias drift of the accelerometer and the gyroscope in the IMU sensor of the vehicle integrated navigation system at the vehicle stationary moment can be effectively restrained, and the positioning precision and the robustness of the integrated navigation system are improved.
In a second aspect, referring to fig. 2, an embodiment of the present invention further provides a zero-speed detecting and correcting device of an integrated navigation system, including:
The navigation data acquisition module 100 is configured to acquire navigation data information of the integrated navigation system, where the navigation data information includes IMU data, GPS data, and white data;
the motion state judging module 200 is configured to perform zero-speed detection according to the navigation data information, obtain a detection result, and judge a motion state of the vehicle according to the detection result;
the correction module 300 is used for performing zero-speed correction on the integrated navigation system when the motion state of the vehicle is a completely stationary state.
In one embodiment, the navigation data acquisition module 100 is further configured to:
Acquiring IMU data of the vehicle based on the IMU sensor, acquiring GPS data of the vehicle based on the GPS sensor and acquiring WHEEL data of the vehicle based on the WHEEL sensor;
Performing first-order low-pass filtering processing on the IMU data to obtain filtering processing data of the IMU sensor;
and outputting the filtering processing data, the GPS data and the WHEEL data as the navigation data information.
In one embodiment, the first order low pass filtering is calculated as:
gk(Xk)＝b·Xk+(1-b)gk-1(Xk-1)；
wherein:
Xk＝(a g)T，
X k represents accelerometer and gyroscope values sampled by the IMU sensor for the kth time, a represents an acceleration count value sampled by the IMU sensor, g represents a gyroscope value sampled by the IMU sensor, g k(Xk) represents an output value after the kth filtering, g k-1(Xk-1) represents an output value after the kth-1 filtering, b represents a filtering coefficient, T is a transposed matrix, k epsilon N *, and k is more than or equal to 1.
In one embodiment, the motion state determination module 200 further includes:
The first detection unit is used for carrying out first zero-speed detection on the vehicle based on the IMU data to obtain a first detection result;
The second detection unit is used for carrying out second zero-speed detection on the vehicle based on the GPS data to obtain a second detection result;
the third detection unit is used for carrying out third zero-speed detection on the vehicle based on the WHEEL data to obtain a third detection result;
the fourth detection unit is used for carrying out fourth zero-speed detection on the vehicle based on chi-square detection to obtain a fourth detection result;
and the judging unit is used for judging the motion state of the vehicle according to the first detection result, the second detection result, the third detection result and the fourth detection result.
In an embodiment, the first detection unit is further configured to:
judging whether the acceleration count value obtained by sampling the IMU sensor is smaller than a first preset threshold value and whether the gyroscope value is smaller than a second preset threshold value;
And when the acceleration count value is smaller than the first preset threshold value and the gyroscope value is smaller than the second preset threshold value, obtaining the first detection result, wherein the first detection result is that the vehicle is in a relatively stationary state.
In one embodiment, the GPS data includes GPS location data and GPS velocity data, and the second detection unit is further configured to:
judging whether the GPS position data meets a first preset condition and whether the GPS speed data meets a second preset condition;
And when the GPS position data meets a first preset condition and the GPS speed data meets a second preset condition, obtaining a second detection result, wherein the second detection result is that the vehicle is in a relatively stationary state.
In an embodiment, the third detection unit is further configured to:
judging whether the WHEEL data output is zero or not;
and when the WHEEL data output is zero, obtaining a third detection result, wherein the second detection result is that the vehicle is in a relatively stationary state.
In an embodiment, the fourth detection unit is further configured to:
Defining a combined navigation coordinate system based on the position of each sensor relative to the vehicle, the combined navigation coordinate system comprising an IMU coordinate system, a GPS coordinate system, a vehicle body coordinate system, and a world coordinate system;
Defining the system state quantity of the integrated navigation system as:
X＝(Pwb Vwb θwb ba bg θbc pbc)T；
The system state quantity X is a vector with a preset dimension, a subscript w represents a world coordinate system when the integrated navigation system is started, a subscript b represents the IMU coordinate system, a subscript c represents the vehicle body coordinate system, P wb represents three-dimensional position information of the system state quantity, V wb represents three-dimensional speed information theta wb of the system state quantity, three-dimensional rotation information b a of the system state quantity represents accelerometer bias, b g represents gyroscope bias, theta bc represents a rotation conversion relation between the vehicle body coordinate system and the IMU coordinate system, and P bc represents a displacement conversion relation between the vehicle body coordinate system and the IMU coordinate system;
Calculating the state quantity of the noise of the integrated navigation system:
M＝(Vi θi)，
Wherein M is the state quantity of the combined navigation system noise, V i is the state quantity of the accelerometer noise, and theta i is the state quantity of the gyroscope noise;
Calculating covariance matrix of the noise of the combined navigation system:
Wherein,
Representing the noise variance value of the accelerometer,Representing the noise variance value of the gyroscope, I is the identity matrix,AndThe value of (2) is calculated by an Allan variance method;
Constructing an observation equation with zero speed update based on accelerometer constraint and gyroscope constraint:
wherein a is an acceleration count value obtained after first-order low-pass filtering processing, g is a gyroscope value obtained after first-order low-pass filtering processing, and the matrix is expressed as follows:
representing a rotation matrix between the IMU coordinate system and the world coordinate system, G representing local gravitational acceleration;
Calculating a jacobian matrix of the system state quantity according to the observation equation updated at zero speed:
Wherein, An antisymmetric matrix of the matrix G b;
And carrying out zero-speed detection based on chi-square detection, and constructing a chi-square detection value of a zero-speed detection model:
Wherein,
Sc＝H·P·HT+50·R，
P represents a covariance matrix of an error state Kalman filtering error state, and e represents a chi-square test value;
and inquiring a critical value of the test statistic based on the degree of freedom and the significance level, judging whether the test statistic is in a preset reject domain, and generating a fourth detection result when the test statistic is in the preset reject domain.
In an embodiment, the judging unit is further configured to:
And combining the first detection result, the second detection result, the third detection result and the fourth detection result, and judging that the motion state of the vehicle is a completely stationary state when the first detection result, the second detection result, the third detection result and the fourth detection result are all that the vehicle is in a relatively stationary state.
In one embodiment, the correction module 300 is configured to:
Calculating the Kalman gain of error state Kalman filtering according to the Jacobian matrix:
K＝P·HT·S-1，
Wherein,
S＝H·P·HT+R，
Calculating the variation of the error state according to the Kalman gain:
ΔR＝K·f(x)；
Updating and correcting a covariance matrix of the error state in the error Kalman filtering based on the variation of the error state, wherein the covariance matrix is:
Pk+1＝(I-K·H)·Pk·(I-K·H)-1+K·R·K。
Referring to fig. 4, an embodiment of the present invention further provides a computer device, and an internal structure of the computer device may be as shown in fig. 4. The computer device includes a processor, a memory, a network interface, and a database connected by a system bus. Wherein the processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The nonvolatile storage medium stores an operating device, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The database of the computer device is used for storing zero-speed detection and correction data and the like of the integrated navigation system. The network interface of the computer device is used for communicating with an external terminal through a network connection. Further, the above-mentioned computer apparatus may be further provided with an input device, a display screen, and the like. The computer program is executed by a processor to realize a zero speed detection and correction method of the integrated navigation system, and comprises the following steps: acquiring navigation data information of the integrated navigation system, wherein the navigation data information comprises IMU data, GPS data and WHEEL data; zero-speed detection is carried out according to the navigation data information to obtain a detection result, and the motion state of the vehicle is judged according to the detection result; when the motion state of the vehicle is in a completely stationary state, the integrated navigation system is subjected to zero-speed correction.
An embodiment of the present application further provides a computer readable storage medium having a computer program stored thereon, which when executed by a processor, implements a zero-speed detection and correction method for a combined navigation system, including the steps of: acquiring navigation data information of the integrated navigation system, wherein the navigation data information comprises IMU data, GPS data and WHEEL data; zero-speed detection is carried out according to the navigation data information to obtain a detection result, and the motion state of the vehicle is judged according to the detection result; when the motion state of the vehicle is in a completely stationary state, the integrated navigation system is subjected to zero-speed correction. It is understood that the computer readable storage medium in this embodiment may be a volatile readable storage medium or a nonvolatile readable storage medium.
Those skilled in the art will appreciate that implementing all or part of the above described methods may be accomplished by way of a computer program stored on a non-transitory computer readable storage medium, which when executed, may comprise the steps of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium provided by the present application and used in embodiments may include non-volatile and/or volatile memory. The nonvolatile memory can include Read Only Memory (ROM), programmable ROM (PROM), electrically Programmable ROM (EPROM), electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual speed data rate SDRAM (SSRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (SYNCHLINK) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), among others.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, apparatus, article, or method that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, apparatus, article, or method. Without further limitation, an element defined by the phrase "comprising one … …" or the like does not exclude the presence of other like elements in a process, apparatus, article, or method that comprises the element.
The foregoing description is only of the preferred embodiments of the present invention and is not intended to limit the scope of the invention, and all equivalent structures or equivalent processes using the descriptions and drawings of the present invention or directly or indirectly applied to other related technical fields are included in the scope of the invention.
Claims (13)
1.A zero-speed detection and correction method for a combined navigation system, the method comprising:
Acquiring navigation data information of a combined navigation system, wherein the navigation data information comprises IMU data, GPS data and WHEEL data;
zero-speed detection is carried out according to the navigation data information to obtain a detection result, and the motion state of the vehicle is judged according to the detection result;
when the motion state of the vehicle is in a completely stationary state, the integrated navigation system is subjected to zero-speed correction.
2. The method for detecting and correcting zero speed of integrated navigation system according to claim 1, wherein the acquiring navigation data information of integrated navigation system comprises:
Acquiring the IMU data of a vehicle based on an IMU sensor, acquiring the GPS data of the vehicle based on a GPS sensor, and acquiring the white data of the vehicle based on a white sensor;
Performing first-order low-pass filtering processing on the IMU data to obtain filtering processing data of the IMU sensor;
and outputting the filtering processing data, the GPS data and the WHEEL data as the navigation data information.
3. The method for detecting and correcting zero speed of integrated navigation system according to claim 2, wherein the first order low pass filtering is calculated by:
gk(Xk)＝b·Xk+(1-b)gk-1(Xk-1)；
wherein:
Xk＝(a g)T，
X k represents accelerometer and gyroscope values sampled by the IMU sensor for the kth time, a represents an acceleration count value sampled by the IMU sensor, g represents a gyroscope value sampled by the IMU sensor, g k(Xk) represents an output value after the kth filtering, g k-1(Xk-1) represents an output value after the kth-1 filtering, b represents a filtering coefficient, T is a transposed matrix, k epsilon N *, and k is more than or equal to 1.
4. The method for detecting and correcting zero speed of integrated navigation system according to claim 3, wherein the step of detecting zero speed according to the navigation data information to obtain a detection result comprises:
performing first zero-speed detection on the vehicle based on the IMU data to obtain a first detection result;
Performing second zero-speed detection on the vehicle based on the GPS data to obtain a second detection result;
Performing third zero-speed detection on the vehicle based on the WHEEL data to obtain a third detection result;
detecting the vehicle at a fourth zero speed based on chi-square test to obtain a fourth detection result;
and judging the motion state of the vehicle according to the first detection result, the second detection result, the third detection result and the fourth detection result.
5. The method for detecting and correcting zero speed of integrated navigation system according to claim 4, wherein the step of performing the first zero speed detection on the vehicle based on the IMU data to obtain the first detection result comprises:
judging whether the acceleration count value obtained by sampling the IMU sensor is smaller than a first preset threshold value and whether the gyroscope value is smaller than a second preset threshold value;
And when the acceleration count value is smaller than the first preset threshold value and the gyroscope value is smaller than the second preset threshold value, obtaining the first detection result, wherein the first detection result is that the vehicle is in a relatively stationary state.
6. The integrated navigation system zero-speed detection and correction method of claim 5, wherein the GPS data includes GPS location data and GPS speed data;
the step of performing a second zero-speed detection on the vehicle based on the GPS data to obtain a second detection result comprises the following steps:
judging whether the GPS position data meets a first preset condition and whether the GPS speed data meets a second preset condition;
And when the GPS position data meets a first preset condition and the GPS speed data meets a second preset condition, obtaining a second detection result, wherein the second detection result is that the vehicle is in a relatively stationary state.
7. The method for detecting and correcting zero speed of integrated navigation system according to claim 6, wherein the step of performing a third zero speed detection on the vehicle based on the said green data to obtain a third detection result comprises:
judging whether the WHEEL data output is zero or not;
and when the WHEEL data output is zero, obtaining a third detection result, wherein the second detection result is that the vehicle is in a relatively stationary state.
8. The method for detecting and correcting zero speed of integrated navigation system according to claim 7, wherein the step of detecting the vehicle at the fourth zero speed based on the chi-square test to obtain the fourth detection result comprises:
Defining a combined navigation coordinate system based on the position of each sensor relative to the vehicle, the combined navigation coordinate system comprising an IMU coordinate system, a GPS coordinate system, a vehicle body coordinate system, and a world coordinate system;
Defining the system state quantity of the integrated navigation system as:
X＝(PwbVwbθwbbabgθbcpbc)T；
The system state quantity X is a vector with a preset dimension, a subscript w represents a world coordinate system when the integrated navigation system is started, a subscript b represents the IMU coordinate system, a subscript c represents the vehicle body coordinate system, P wb represents three-dimensional position information of the system state quantity, V wb represents three-dimensional speed information of the system state quantity, theta wb represents three-dimensional rotation information of the system state quantity, b a represents accelerometer bias, b g represents gyroscope bias, theta bc represents a rotation conversion relation between the vehicle body coordinate system and the IMU coordinate system, and P bc represents a displacement conversion relation between the vehicle body coordinate system and the IMU coordinate system;
Calculating the state quantity of the noise of the integrated navigation system:
M＝(Viθi)，
wherein M is the state quantity of the combined navigation system noise, v i is the state quantity of the accelerometer noise, and θ i is the state quantity of the gyroscope noise;
Calculating covariance matrix of the noise of the combined navigation system:
Wherein,
Representing the noise variance value of the accelerometer,Representing the noise variance value of the gyroscope, I is the identity matrix,AndThe value of (2) is calculated by an Allan variance method;
Constructing an observation equation with zero speed update based on accelerometer constraint and gyroscope constraint:
wherein a is an acceleration count value obtained after first-order low-pass filtering processing, g is a gyroscope value obtained after first-order low-pass filtering processing, and the matrix is expressed as follows:
representing a rotation matrix between the IMU coordinate system and the world coordinate system, G representing local gravitational acceleration;
Calculating a jacobian matrix of the system state quantity according to the observation equation updated at zero speed:
Wherein, An antisymmetric matrix of the matrix G b;
And carrying out zero-speed detection based on chi-square detection, and constructing a chi-square detection value of a zero-speed detection model:
Wherein,
Sc＝H·P·HT+50·R，
P represents a covariance matrix of an error state Kalman filtering error state, and e represents a chi-square test value;
and inquiring a critical value of the test statistic based on the degree of freedom and the significance level, judging whether the test statistic is in a preset reject domain, and generating a fourth detection result when the test statistic is in the preset reject domain.
9. The method of claim 8, wherein the step of determining the motion state of the vehicle according to the first detection result, the second detection result, the third detection result, and the fourth detection result comprises:
And when the first detection result, the second detection result, the third detection result and the fourth detection result are all that the vehicle is in a relatively stationary state, judging that the motion state of the vehicle is a completely stationary state.
10. The integrated navigation system zero-speed detection and correction method of claim 9, wherein the step of performing zero-speed correction on the integrated navigation system comprises:
Calculating the Kalman gain of error state Kalman filtering according to the Jacobian matrix:
K＝P·HT·S-1，
Wherein,
S＝H·P·HT+R，
Calculating the variation of the error state according to the Kalman gain:
ΔR＝K·f(x)；
Updating and correcting a covariance matrix of the error state in the error Kalman filtering based on the variation of the error state, wherein the covariance matrix is:
Pk+1＝(I-K·H)·Pk·(I-K·H)-1+K·R·K。
11. A zero-speed detection and correction device for an integrated navigation system, comprising:
The navigation data acquisition module is used for acquiring navigation data information of the integrated navigation system, wherein the navigation data information comprises IMU data, GPS data and WHEEL data;
The motion state judging module is used for carrying out zero-speed detection according to the navigation data information to obtain a detection result, and judging the motion state of the vehicle according to the detection result;
And the correction module is used for carrying out zero-speed correction on the integrated navigation system when the motion state of the vehicle is a completely stationary state.
12. A computer device comprising a memory and a processor, the memory having stored therein a computer program, characterized in that the processor, when executing the computer program, implements the steps of the zero-speed detection and correction method of a integrated navigation system as claimed in any one of claims 1 to 10.
13. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the steps of the zero-speed detection and correction method of a combined navigation system according to any one of claims 1 to 10.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310012682.7A CN118293907A (en) | 2023-01-05 | 2023-01-05 | Zero-speed detection and correction method, device, equipment and medium of integrated navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310012682.7A CN118293907A (en) | 2023-01-05 | 2023-01-05 | Zero-speed detection and correction method, device, equipment and medium of integrated navigation system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN118293907A true CN118293907A (en) | 2024-07-05 |
Family
ID=91683505
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310012682.7A Pending CN118293907A (en) | 2023-01-05 | 2023-01-05 | Zero-speed detection and correction method, device, equipment and medium of integrated navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118293907A (en) |
-
2023
- 2023-01-05 CN CN202310012682.7A patent/CN118293907A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9273966B2 (en) | Technique for calibrating dead reckoning positioning data | |
| US9921065B2 (en) | Unit and method for improving positioning accuracy | |
| CN111156994B (en) | INS/DR & GNSS loose combination navigation method based on MEMS inertial component | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| JP7073052B2 (en) | Systems and methods for measuring the angular position of a vehicle | |
| CN110779521A (en) | Multi-source fusion high-precision positioning method and device | |
| CN113405545B (en) | Positioning method, positioning device, electronic equipment and computer storage medium | |
| CN108051839B (en) | Vehicle-mounted three-dimensional positioning device and three-dimensional positioning method | |
| CN104880189B (en) | A kind of antenna for satellite communication in motion low cost tracking anti-interference method | |
| CN115407376B (en) | Vehicle positioning calibration method, device, computer equipment, and storage medium | |
| CN106403952A (en) | Method for measuring combined attitudes of Satcom on the move with low cost | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| CN114019954B (en) | Course installation angle calibration method, device, computer equipment and storage medium | |
| CN112946681B (en) | Laser radar positioning method fusing combined navigation information | |
| CN115166802A (en) | Aircraft positioning method, device and electronic device | |
| CN114413934A (en) | Vehicle positioning system correction method and device | |
| CN105910623B (en) | The method for carrying out the correction of course using magnetometer assisted GNSS/MINS tight integration systems | |
| CN113074757A (en) | Calibration method for vehicle-mounted inertial navigation installation error angle | |
| US20210190499A1 (en) | Method for providing a navigation information, corresponding system and program product | |
| CN117053802A (en) | Method for reducing positioning error of vehicle navigation system based on rotary MEMS IMU | |
| CN115096321B (en) | Robust unscented information filtering alignment method and system for vehicle-mounted strapdown inertial navigation system | |
| CN118293907A (en) | Zero-speed detection and correction method, device, equipment and medium of integrated navigation system | |
| CN108957508B (en) | Vehicle-mounted POS (point of sale) offline combined estimation method and device | |
| CN111256708A (en) | Vehicle-mounted integrated navigation method based on radio frequency identification | |
| CN115343738A (en) | GNSS-RTK and IMU based integrated navigation method and equipment |
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 |