CN116878500A - High-precision detection method for real-time motion gesture of vehicle - Google Patents
High-precision detection method for real-time motion gesture of vehicle Download PDFInfo
- Publication number
- CN116878500A CN116878500A CN202310845318.9A CN202310845318A CN116878500A CN 116878500 A CN116878500 A CN 116878500A CN 202310845318 A CN202310845318 A CN 202310845318A CN 116878500 A CN116878500 A CN 116878500A
- Authority
- CN
- China
- Prior art keywords
- linear
- data
- moment
- prediction
- 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
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 8
- 238000000034 method Methods 0.000 claims abstract description 9
- 238000001914 filtration Methods 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 3
- 239000004973 liquid crystal related substance Substances 0.000 description 4
- 238000005259 measurement Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000013213 extrapolation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011897 real-time detection 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Abstract
The invention relates to a high-precision detection method for a real-time motion gesture of a vehicle, and belongs to the field of automatic driving. Firstly, refreshing all predicted values and actual measured values by using the measured value of the first state vector, and predicting the next position by using the values; for the moment without positioning data output, using the linear and secondary prediction coefficients estimated at the moment with data output to extrapolate the positioning data, and performing convex linear joint prediction to obtain the positioning data without output data; and for the moment with positioning data output, linear and quadratic prediction coefficients estimated at the moment with data output are used, and convex linear combined prediction is carried out to obtain positioning data with output data. The invention utilizes the military grade high-precision gyroscope and the accelerometer to output data, combines with GNSS output data, continuously corrects the data based on the Kalman filter, outputs accurate data in real time, achieves the effect of low cost and high precision, and is easier to realize than other extended Kalman filter methods.
Description
Technical Field
The invention belongs to the field of automatic driving, and relates to a high-precision detection method for real-time motion gestures of a vehicle.
Background
The increasingly complex road conditions, environmental conditions and realization of high-level automatic driving all require that the vehicle navigation and positioning system must be capable of maintaining high-precision operation for a long time, and have a certain cost performance in terms of cost to meet the market demand. GNSS technology refers to measurement technology that obtains absolute positioning coordinates in a coordinate system by observing GNSS satellites. Due to receiver limitations, the existing civilian GNSS output frequency is 10Hz. The inherent slow frequency of the positioning data makes no data output between two positioning intervals, and cannot be applied to high-precision positioning in a medium-high dynamic environment.
Disclosure of Invention
In view of the above, the present invention aims to provide a method for detecting a real-time motion gesture of a vehicle with high precision.
In order to achieve the above purpose, the present invention provides the following technical solutions:
a high-precision detection method for real-time motion gesture of a vehicle comprises the following steps:
setting a GNSS frequency predictor, filtering and predicting GNSS output data by using primary linear prediction and depth secondary prediction, and correcting by Kalman filtering by combining a gyroscope and acceleration output data;
firstly, refreshing all predicted values and actual measured values by using the measured value of the first state vector, and predicting the next position by using the values; for the moment without positioning data output, using the linear and secondary prediction coefficients estimated at the moment with data output to extrapolate the positioning data, and performing convex linear joint prediction to obtain the positioning data without output data; for the moment with positioning data output, linear and secondary prediction coefficients estimated at the moment with data output are used, and convex linear combined prediction is carried out to obtain positioning data with output data;
let GPS positioning longitude and latitude data be λ (t), ψ (t), respectively, the state vector of the filter predictor be θ (t) =λ (t), ψ (t), the N-dotted linear polynomial filter predictor of the position state vector be: θ t ＝a 0 +a 1 t；
Wherein a is 0 ，a 1 Is the coefficient of the predictor, t is time, t k Time at point k (k=0, 1)
The minimum variance estimate is:
θ t (t k |t k )＝a 0 +a 1 t k (1)
wherein, the liquid crystal display device comprises a liquid crystal display device,
where n=2, t k Time at point k (k=0, 1)
At t k+1 The predicted value of the time GNSS position state vector is:
θ t (t k+1 |t k )＝a 0 +a 1 t k+1 (3)
GNSS position state vector five-point quadratic polynomial filter predictor:
θ q (t)＝b 0 +b 1 t+b 2 t 2
where n=5, θ t (t k ) Representing the linear prediction position, t k Time at k point (k=0, 1,2,3, 4)
At t k+1 The predicted value of the time GNSS position state vector is:
θ q ＝α 1 θ t +α 2 θ q (7)
wherein: θ is the final GPS position state vector
σ(θ l )，σ(θ q ) The integrated square difference between the linear prediction position and the secondary prediction position of the previous five GNSS and the actual position is calculated.
Optionally, the method specifically comprises the following steps:
(1) Initializing true pose location θ (t) k ) Linear prediction position θ t (t k ) Second predicted position θ q (t k )，k＝0，1，2，3，4；
(2) At the moment of GNSS output data, calculating a linear filtering prediction coefficient a by a formula (2) 0 And a 1 T is obtained by the formula (3) k+1 A moment two-point linear predicted value; calculating a secondary filtering prediction coefficient b from (5) 0 ，b 1 ，b 2 T is obtained by the formula (6) k+1 Five-point secondary predicted value at the moment;
(3) Calculating the linear convex joint coefficient alpha from (8) 1 And alpha 2 Finally, t is obtained by the formula (7) k+1 Two-point linear and five-point quadratic convex joint predicted values at the moment.
The invention has the beneficial effects that: the invention provides a low-cost high-precision detection method for real-time motion gestures of a vehicle. And using a continuous output algorithm aiming at GNSS high-speed positioning data in a high dynamic environment to enable the GNSS output frequency to reach 100Hz. Meanwhile, the military grade high-precision gyroscope and the accelerometer are utilized to output data, and the GNSS output data is combined, so that the data is continuously corrected based on the Kalman filter, the accurate data is output in real time, the effect of low cost and high precision is achieved, and the method is easier to realize than other extended Kalman filter methods.
Additional advantages, objects, and features of the invention will be set forth in part in the description which follows and in part will become apparent to those having ordinary skill in the art upon examination of the following or may be learned from practice of the invention. The objects and other advantages of the invention may be realized and obtained by means of the instrumentalities and combinations particularly pointed out in the specification.
Drawings
For the purpose of making the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in the following preferred detail with reference to the accompanying drawings, in which:
fig. 1 is a schematic diagram of the present invention.
Detailed Description
Other advantages and effects of the present invention will become apparent to those skilled in the art from the following disclosure, which describes the embodiments of the present invention with reference to specific examples. The invention may be practiced or carried out in other embodiments that depart from the specific details, and the details of the present description may be modified or varied from the spirit and scope of the present invention. It should be noted that the illustrations provided in the following embodiments merely illustrate the basic idea of the present invention by way of illustration, and the following embodiments and features in the embodiments may be combined with each other without conflict.
Wherein the drawings are for illustrative purposes only and are shown in schematic, non-physical, and not intended to limit the invention; for the purpose of better illustrating embodiments of the invention, certain elements of the drawings may be omitted, enlarged or reduced and do not represent the size of the actual product; it will be appreciated by those skilled in the art that certain well-known structures in the drawings and descriptions thereof may be omitted.
The same or similar reference numbers in the drawings of embodiments of the invention correspond to the same or similar components; in the description of the present invention, it should be understood that, if there are terms such as "upper", "lower", "left", "right", "front", "rear", etc., that indicate an azimuth or a positional relationship based on the azimuth or the positional relationship shown in the drawings, it is only for convenience of describing the present invention and simplifying the description, but not for indicating or suggesting that the referred device or element must have a specific azimuth, be constructed and operated in a specific azimuth, so that the terms describing the positional relationship in the drawings are merely for exemplary illustration and should not be construed as limiting the present invention, and that the specific meaning of the above terms may be understood by those of ordinary skill in the art according to the specific circumstances.
As shown in FIG. 1, the invention is provided with a GNSS frequency predictor, and the GNSS output data is filtered and predicted by utilizing two-point linear prediction and five-point secondary prediction, so that the output frequency of the existing 10Hz reaches 100Hz, and then the correction is carried out by combining a gyroscope and acceleration output data through Kalman filtering, thereby overcoming the defect of low output frequency of real-time detection of the civil GNSS vehicle gesture, and enabling the combination of low-cost civil GNSS and inertial navigation to be applied to high-precision detection of the real-time motion gesture of the vehicle. In addition, in the GPS/INS integrated navigation system, if the GPS signal is interrupted, the measurement value cannot be provided for the integrated navigation Kalman filter and the error of the inertial navigation system cannot be corrected. The result of inertial navigation can not be corrected in time, and the navigation solution error can be increased rapidly under the influence of the error of the inertial device. If the error in the inertial device can be corrected by the Kalman filter during the GPS effective period, even if the GPS signal is interrupted, the navigation solution result can keep higher precision in a certain time because the error of the device is corrected.
And (3) GNSS: linear convex joint using primary linear prediction and depth quadratic prediction
Positioning principle: at the beginning of the system operation, since the previous predicted and actual measured positions are unknown, all predicted and actual measured values are first refreshed with the measured value of the first state vector and the next position is predicted with these values. And for the moment without positioning data output, extrapolation of positioning data by using the linear and quadratic prediction coefficients estimated at the moment with data output, and performing convex linear joint prediction to obtain the positioning data without output data. And for the moment with positioning data output, linear and quadratic prediction coefficients estimated at the moment with data output are used, and convex linear combined prediction is carried out to obtain positioning data with output data.
Let GPS positioning longitude and latitude data be λ (t), ψ (t), respectively, the state vector of the filter predictor be θ (t) =λ (t), the general form of the N-dotted linear polynomial filter predictor for the position state vector be: θ t ＝a 0 +a 1 t。
Where n=2, t k Time at point k (k=0, 1)
The minimum variance estimate is:
θ t (t k |t k )＝a 0 +a 1 t k (1)
wherein, the liquid crystal display device comprises a liquid crystal display device,
where n=2, t k Time at point k (k=0, 1)
At t k+1 The predicted value of the time GNSS position state vector is:
θ t (t k+1 |t k )＝a 0 +a 1 t k+1 (3)
similarly, the GNSS position state vector five-point quadratic polynomial filter predictor:
θ q (t)＝b 0 +b 1 t+b 2 t 2
where n=5, θ t (t k ) Representing the linear prediction position, t k Time at k point (k=0, 1,2,3, 4)
At t k+1 The predicted value of the time GNSS position state vector is:
θ q ＝α 1 θ t +α 2 θ q (7)
wherein: θ is the final GPS position state vector
σ(θ l )，σ(θ q ) The integrated square difference between the linear prediction position and the secondary prediction position of the previous five GNSS and the actual position is calculated.
Algorithm
(1) Initializing true pose location θ (t) k ) Linear prediction position θ t (t k ) Second predicted position θ q (t k )，k＝0，1，2，3，4
(2) At the moment of GNSS output data, calculating a linear filtering prediction coefficient a by a formula (2) 0 And a 1 T is obtained by the formula (3) k+1 A moment two-point linear predicted value; calculating a secondary filtering prediction coefficient b from (5) 0 ，b 1 ，b 2 T is obtained by the formula (6) k+1 Five-point secondary predicted value at the moment.
(3) Calculating the linear convex joint coefficient alpha from (8) 1 And alpha 2 Finally, t is obtained by the formula (7) k+1 Two-point linear and five-point two-shot joint prediction at momentValues.
Finally, it is noted that the above embodiments are only for illustrating the technical solution of the present invention and not for limiting the same, and although the present invention has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that modifications and equivalents may be made thereto without departing from the spirit and scope of the present invention, which is intended to be covered by the claims of the present invention.
Claims (2)
1. A high-precision detection method for real-time motion gestures of a vehicle is characterized in that: the method comprises the following steps:
setting a GNSS frequency predictor, filtering and predicting GNSS output data by using primary linear prediction and depth secondary prediction, and correcting by Kalman filtering by combining a gyroscope and acceleration output data;
firstly, refreshing all predicted values and actual measured values by using the measured value of the first state vector, and predicting the next position by using the values; for the moment without positioning data output, using the linear and secondary prediction coefficients estimated at the moment with data output to extrapolate the positioning data, and performing convex linear joint prediction to obtain the positioning data without output data; for the moment with positioning data output, linear and secondary prediction coefficients estimated at the moment with data output are used, and convex linear combined prediction is carried out to obtain positioning data with output data;
let GPS positioning longitude and latitude data be λ (t), ψ (t), respectively, the state vector of the filter predictor be θ (t) =λ (t), ψ (t), the N-dotted linear polynomial filter predictor of the position state vector be: θ t ＝a 0 +a 1 t；
Wherein a is 0 ，a 1 Is the coefficient of the predictor, t is time, t k Time at point k; k=0, 1;
the minimum variance estimate is:
θ t (t k |t k )＝a 0 +a 1 t k (1)
where n=2, t k Time at point k;
at t k+1 The predicted value of the time GNSS position state vector is:
θ t (t k+1 |t k )＝a 0 +a 1 t k+1 (3)
GNSS position state vector five-point quadratic polynomial filter predictor:
where n=5, θ t (t k ) Representing the linear prediction position, t k Represents the time at point k, k=0, 1,2,3,4;
at t k+1 The predicted value of the time GNSS position state vector is:
wherein: θ is the final GPS position state vector;
σ(θ l )，σ(θ q ) The integrated square difference between the linear prediction position and the secondary prediction position of the previous five GNSS and the actual position is calculated.
2. The method for detecting the real-time motion gesture of the vehicle with high precision according to claim 1, wherein the method comprises the following steps: the method specifically comprises the following steps:
(1) Initializing true pose location θ (t) k ) Linear prediction position θ t (t k ) Second predicted position θ q (t k )，k＝0，1，2，3，4；
(2) At the moment of GNSS output data, calculating a linear filtering prediction coefficient a by a formula (2) 0 And a 1 T is obtained by the formula (3) k+1 A moment two-point linear predicted value; calculating a secondary filtering prediction coefficient b from (5) 0 ，b 1 ，b 2 T is obtained by the formula (6) k+1 Five-point secondary predicted value at the moment;
(3) Calculating the linear convex joint coefficient alpha from (8) 1 And alpha 2 Finally, t is obtained by the formula (7) k+1 Two-point linear and five-point quadratic convex joint predicted values at the moment.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310845318.9A CN116878500A (en) | 2023-07-11 | 2023-07-11 | High-precision detection method for real-time motion gesture of vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310845318.9A CN116878500A (en) | 2023-07-11 | 2023-07-11 | High-precision detection method for real-time motion gesture of vehicle |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116878500A true CN116878500A (en) | 2023-10-13 |
Family
ID=88254261
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310845318.9A Pending CN116878500A (en) | 2023-07-11 | 2023-07-11 | High-precision detection method for real-time motion gesture of vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116878500A (en) |
-
2023
- 2023-07-11 CN CN202310845318.9A patent/CN116878500A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110779521A (en) | Multi-source fusion high-precision positioning method and device | |
| CN103697889B (en) | A kind of unmanned plane independent navigation and localization method based on multi-model Distributed filtering | |
| US20150276783A1 (en) | Positioning apparatus comprising an inertial sensor and inertial sensor temperature compensation method | |
| WO2014002210A1 (en) | Positioning device | |
| CN104729506A (en) | Unmanned aerial vehicle autonomous navigation positioning method with assistance of visual information | |
| WO2014002211A1 (en) | Positioning device | |
| JP6470314B2 (en) | Attitude angle calculation device, attitude angle calculation method, and attitude angle calculation program | |
| US9026362B2 (en) | Position calculating method and position calculating device | |
| US9423486B2 (en) | Position calculating method and position calculating device | |
| EP2560024B1 (en) | Satellite-based positioning method | |
| JP2012207919A (en) | Abnormal value determination device, positioning device, and program | |
| CN111025366B (en) | Grid SLAM navigation system and method based on INS and GNSS | |
| US20140104101A1 (en) | Position calculating method and position calculating device | |
| WO2016203744A1 (en) | Positioning device | |
| CN105910623B (en) | The method for carrying out the correction of course using magnetometer assisted GNSS/MINS tight integration systems | |
| Gu et al. | Vehicle self-localization in urban canyon using 3D map based GPS positioning and vehicle sensors | |
| CN109470276A (en) | Mileage meter calibration method and device based on zero-velocity curve | |
| JP5664059B2 (en) | Vehicle trajectory estimation device | |
| JP2015094689A (en) | Vehicle travelling track calculation apparatus | |
| CN108151765A (en) | Attitude positioning method is surveyed in a kind of positioning of online real-time estimation compensation magnetometer error | |
| CN114690229A (en) | GPS-fused mobile robot visual inertial navigation method | |
| Wen et al. | 3D LiDAR aided GNSS real-time kinematic positioning | |
| CN116576849A (en) | Vehicle fusion positioning method and system based on GMM assistance | |
| Ćwian et al. | GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments | |
| CN116878500A (en) | High-precision detection method for real-time motion gesture of vehicle |
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 |