CN116878500A - High-precision detection method for real-time motion gesture of vehicle - Google Patents

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

High-precision detection method for real-time motion gesture of vehicle

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 ₀ +a ₁ t；

Wherein a is ₀ ，a ₁ 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 ₀ +a ₁ t _k (1)

wherein,,

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 ₀ +a ₁ t _k+1 (3)

GNSS position state vector five-point quadratic polynomial filter predictor:

θ _q (t)＝b ₀ +b ₁ t+b ₂ t ²

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 ＝α ₁ θ _t +α ₂ θ _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) ₀ And a ₁ 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) ₀ ，b ₁ ，b ₂ 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) ₁ And alpha ₂ 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 ₀ +a ₁ t。

Where n=2, t _k Time at point k (k=0, 1)

The minimum variance estimate is:

θ _t (t _k |t _k )＝a ₀ +a ₁ t _k (1)

wherein,,

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 ₀ +a ₁ t _k+1 (3)

similarly, the GNSS position state vector five-point quadratic polynomial filter predictor:

θ _q (t)＝b ₀ +b ₁ t+b ₂ t ²

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 ＝α ₁ θ _t +α ₂ θ _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) ₀ And a ₁ 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) ₀ ，b ₁ ，b ₂ 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) ₁ And alpha ₂ 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 ₀ +a ₁ t；

Wherein a is ₀ ，a ₁ 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 ₀ +a ₁ 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 ₀ +a ₁ 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) ₀ And a ₁ 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) ₀ ，b ₁ ，b ₂ 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) ₁ And alpha ₂ Finally, t is obtained by the formula (7) _k+1 Two-point linear and five-point quadratic convex joint predicted values at the moment.