CN110031876A - A kind of vehicle mounted guidance tracing point offset antidote based on Kalman filtering - Google Patents

Abstract

The invention discloses a vehicle navigation track point offset correction method based on Kalman filtering. The method includes the following steps: step 1: denoising the track points in the navigation track; step 2: removing the missing in the navigation track Step 3: Use Kalman filter to correct the offset of the navigation track points after denoising and completion; use the Kalman filter algorithm to correct the offset points that obey the Gaussian distribution to Within a reasonable range, by correcting the offset points of the historical navigation trajectory, the navigation trajectory returned by the user positioning device is closer to the real driving trajectory, which improves the usability of the historical navigation trajectory.

Description

A Kalman Filter-based Vehicle Navigation Track Point Offset Correction Method

technical field

The invention belongs to the field of path navigation planning, and particularly relates to a method for correcting the offset of vehicle navigation track points based on Kalman filtering.

Background technique

Satellite navigation is a very common technology that can provide navigation information such as longitude, latitude, altitude, speed, heading, time, etc. on a global scale. Through satellite navigation, the driver can be easily guided to the destination. The core technology of navigation is positioning technology, and now data is mainly obtained through GPS system or Beidou system. For navigation path planning, it is necessary to study the historical trajectories of user navigation to provide users with better travel suggestions. In the historical trajectory points of user navigation, due to atmospheric ionospheric delay effect, satellite clock error, satellite orbit error and multipath effect caused by urban buildings, GPS positioning will be inaccurate, which will cause some user navigation trajectory points to deviate from the actual position. For these offset points, it is necessary to correct their positions to make the historical navigation trajectory closer to the user's driving trajectory, which is convenient for further research on the user's historical trajectory characteristics. At present, there are a lot of researches on real-time positioning offset correction in the navigation process at home and abroad in order to correct the real-time navigation scheme in time, but very few focus on the offset correction of historical navigation track points. For the historical trajectory of user navigation, due to the limitation of the user's positioning device, there will be some trajectory points that deviate from the actual position. In the research on the characteristics of the user's historical trajectory, if the positions of some points are far away from the actual position, the user's driving trajectory cannot be accurately known, which will affect the research on the characteristics of the navigation trajectory.

SUMMARY OF THE INVENTION

The invention provides a vehicle navigation track point offset correction method based on Kalman filtering. By using the Kalman filtering algorithm, the offset points obeying the Gaussian distribution are well corrected to a reasonable range. It will cause too long distance offset, restore the user's navigation history track well, and provide great help for the research based on the characteristics of the navigation track.

A vehicle navigation track point offset correction method based on Kalman filtering, comprising the following steps:

Step 1: Denoise the track points in the navigation track;

Step 2: Complete the missing points in the navigation track;

Step 3: Use Kalman filter to perform offset correction on the navigation track points after denoising and completion;

Step 3.1: Obtain the optimal estimated position of the trajectory point of the previous state, use the navigation system process model to predict the position of the trajectory point of the current state, and obtain the covariance of the predicted position of the trajectory point of the navigation system in the current state;

Step 3.2: Use the covariance of the predicted position of the trajectory point of the navigation system in the current state to calculate the Kalman gain of the predicted position of the trajectory point of the navigation system in the current state;

Step 3.3: Based on the predicted position of the navigation system at the current trajectory point and its Kalman gain, as well as the position measurement value of the trajectory point in the current state, use the Kalman filter optimal estimation to obtain the optimal estimated position of the trajectory point in the current state to obtain the optimal estimated position of the trajectory point in the current state. The optimal estimated position of the trajectory point of the current state is used as the corrected position;

Among them, in the initial state of the navigation system, the optimal estimated position value of the first track point of the navigation track is the position measurement value of the first track point; in the initial state of the navigation system, the value of the first track point of the navigation track The covariance takes a value of 1.

Further, the position of the trajectory point of the current state predicted by the navigation system process model is calculated according to the following formula to obtain the predicted value;

X(k|k-1)=AX(k-1|k-1)

Among them, X(k|k-1) represents the position prediction value of the trajectory point of the current state k predicted by the optimal estimated position X(k-1|k-1) of the trajectory point of the previous state k-1, A Indicates the gain of the navigation system, the value is 1;

The obtained covariance of the predicted position of the trajectory point of the navigation system in the current state is calculated and obtained according to the following formula:

P(k|k-1)=AP(k-1|k-1)A'+Q

Among them, P(k|k-1) and P(k-1|k-1) represent the covariance corresponding to X(k|k-1) and X(k-1|k-1) respectively, when k- When 1=0, P(k-1|k-1) represents the initial point covariance, that is, the initial covariance of the navigation system P(0|0)=1; when k-1>0, P(k-1| k-1)=(I-Kg(k-1)H)P(k-1|k-2); A'=A=1, Q represents the process covariance of the navigation system, which is 1×10 ^{− 5} , I is the identity matrix.

Further, the Kalman filter optimal estimation is used to obtain the optimal estimated position of the trajectory point of the current state, and the following formula is used to calculate and obtain:

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))

Kg(k)=P(k|k-1)H'/(HP(k|k-1)H'+R)

Among them, Kg(k) represents the Kalman gain, Z(k) represents the measured value of the trajectory point at state k, and H represents the gain of the navigation system state to the measured value of the navigation system, which takes a value of 1.

Further, the process of denoising the track points in the navigation track is as follows:

Step 1.1: Divide the speed segment for the track points in the navigation track;

In the navigation track, the speed segment is divided into the segment where the continuous track points belonging to the same speed segment are located, and each speed segment includes at least 5 continuous track points;

Track point speed section includes low speed section [0～10)m/s, medium speed section [20～30)m/s, high speed section [0～10m/s;

Step 1.2: Calculate the distance between adjacent trajectory points in each speed section, and judge whether the distance between adjacent trajectory points in each speed section exceeds the preset normal maximum distance. If it exceeds, it is considered adjacent. The second track point in the track points is a noise point, delete the noise point;

In the absence of missing track points:

In the low-speed section, the normal maximum distance between two points whose sampling time is 1s apart is 20m;

In the medium-speed section, the normal maximum distance between two points whose sampling time is 1s apart is 3 times the instantaneous speed of the previous point;

In the high-speed section, the normal maximum distance between two points whose sampling time is 1s apart is twice the instantaneous speed of the previous point;

In the presence of missing track points:

The product obtained by multiplying the sampling time difference of two adjacent points by the corresponding maximum distance of the speed section in the absence of missing track points is taken as the preset normal maximum distance between two adjacent points.

Further, the process of completing the missing points in the navigation track is as follows:

Step 2.1: Detection of missing degree of trajectory points;

From the starting point of the navigation track, the timestamp information of each point is traversed in turn. If the sampling time of the next track point and the sampling time of the previous track point exceed the sampling time interval, there are missing points. The missing time in the 30 trajectory points, if the missing time exceeds 10s, then 30 consecutive points starting from the next trajectory point are serious missing trajectory, delete the trajectory segment, otherwise, continue to find the next missing point;

Step 2.2: Completion of missing navigation track points based on piecewise linear interpolation;

From the starting point of the navigation track, traverse the timestamp information of each point in turn, find all missing positions, and record a missing position interval as [x _i , x _i+1 ];

The sampling time difference between the two trajectory points before and after the missing position interval [x _i , x _i+1 ] is n(s), where 0<n≤10 and n is an integer;

Divide the missing position interval [x _i , x _i+1 ] into n equal parts, and the abscissa of the complement point position is 1/n, 2/n, 3/n, ..., (n-1) of the missing position interval respectively. The position of the bisected point of /n is substituted into the following linear equation in turn to calculate the ordinate;

Among them, (x, y) represents the coordinates of the complement point, and (x _i , y _i ) and (x _i+1 , y _i+1 ) represent the coordinates of the two track points before and after the missing location point, respectively.

beneficial effect

The invention provides a Kalman filter-based vehicle navigation track point offset correction method. Using the Kalman filter algorithm, the offset points subject to Gaussian distribution are well corrected to a reasonable range. Offset point correction makes the navigation trajectory returned by the user positioning device closer to the real driving trajectory, improving the usability of historical navigation trajectories.

Compared with the existing technology, it has the following advantages:

1. The present invention adopts Kalman filtering, uses simple recursive algorithm, and the required data storage capacity is small, which is convenient for real-time processing with a computer;

2. This solution can correct the navigation offset points without causing long-distance offsets for normal points, and restore the user's driving trajectory well;

3. In the present invention, by correcting the offset of the navigation track points, the characteristic commonality of the user's historical track is highlighted, so that the user's historical navigation track has higher usability.

Description of drawings

Fig. 1 is a flow chart of vehicle navigation trajectory offset correction based on Kalman filtering;

Fig. 2 is a schematic diagram of a navigation track divided into segments by speed;

Fig. 3 is the schematic diagram of the noise point in the corresponding speed section;

Fig. 4 is the linear interpolation complementing schematic diagram of the missing navigation track point;

Fig. 5 is the change curve diagram of longitude and latitude before and after Kalman filtering;

6 is a schematic diagram of a local navigation trajectory before Kalman filtering;

FIG. 7 is a schematic diagram of the local navigation trajectory after Kalman filtering.

Detailed ways

A method of vehicle navigation track point offset correction based on Kalman filter, the specific process is shown in Figure 1. It mainly includes the following steps:

Step 1: Denoise the track points in the navigation track;

The process of denoising the track points in the navigation track is as follows:

Step 1.1: Divide the speed segment for the track points in the navigation track;

In the navigation track, the speed segment is divided into the segment where the continuous track points belonging to the same speed segment are located, and each speed segment includes at least 5 continuous track points;

Track point speed section includes low speed section [0～10)m/s, medium speed section [20～30)m/s, high speed section [0～10m/s;

The final result of velocity sub-section is shown in Figure 2.

Step 1.2: Calculate the distance between adjacent trajectory points in each speed section, and judge whether the distance between adjacent trajectory points in each speed section exceeds the preset normal maximum distance. If it exceeds, it is considered adjacent. The second track point in the track points is a noise point, delete the noise point;

In the absence of missing track points:

In the low-speed section, the normal maximum distance between two points whose sampling time is 1s apart is 20m;

In the medium-speed section, the normal maximum distance between two points whose sampling time is 1s apart is 3 times the instantaneous speed of the previous point;

In the high-speed section, the normal maximum distance between two points whose sampling time is 1s apart is twice the instantaneous speed of the previous point;

In the presence of missing track points:

The product obtained by multiplying the sampling time difference of two adjacent points by the corresponding maximum distance of the speed section in the absence of missing track points is taken as the preset normal maximum distance between two adjacent points. The noise points in the navigation trajectory are shown in Figure 3.

Step 2: Complete the missing points in the navigation track;

The process of completing the missing points in the navigation track is as follows:

Step 2.1: Detection of missing degree of trajectory points;

From the starting point of the navigation track, the timestamp information of each point is traversed in turn. If the sampling time of the next track point and the sampling time of the previous track point exceed the sampling time interval, there are missing points. The missing time in the 30 trajectory points, if the missing time exceeds 10s, then 30 consecutive points starting from the next trajectory point are serious missing trajectory, delete the trajectory segment, otherwise, continue to find the next missing point;

Traverse the timestamp information of each point in turn from the starting point of the navigation track, and find the first missing point. For example, the timestamp of point P76 is 1512094831s, the timestamp of the next point P77 in the trajectory is 1512094834s, and the missing time in the middle The points corresponding to 1512094832s and 1512094833s are stamped, and the missing time between these two points is considered to be 2s. Traverse 30 points backward from this point, and find the total missing time of all the 30 points. If the total missing time exceeds 10s, it is considered that the trajectory is seriously missing and cannot be used (in practice, the probability of this situation does not exceed 2%). ), if the total missing of these 30 points does not exceed 10s, start to look for the next missing point, repeat the above steps until the 30th point from the bottom, if no serious missing is found during this process, it is considered that the trajectory available.

Step 2.2: Completion of missing navigation track points based on piecewise linear interpolation;

From the starting point of the navigation track, traverse the timestamp information of each point in turn, find all missing positions, and record a missing position interval as [x _i , x _i+1 ];

The sampling time difference between the two trajectory points before and after the missing position interval [x _i , x _i+1 ] is n(s), where 0<n≤10 and n is an integer;

Divide the missing position interval [x _i , x _i+1 ] into n equal parts, and the abscissa of the complement point position is 1/n, 2/n, 3/n, ..., (n-1) of the missing position interval respectively. The position of the bisected point of /n is substituted into the following linear equation in turn to calculate the ordinate;

Among them, (x, y) represents the coordinates of the complement point, (x _i , y _i ) and (x _i+1 , y _i+1 ) respectively represent the coordinates of the two track points before and after the missing position;

For example, the timestamp of point P76 is 1512094831s, the coordinates are (x ₇₆ , y ₇₆ ), the timestamp of the next point P77 in the trajectory is 1512094834s, the coordinates are (x ₇₇ , y ₇₇ ), and the middle missing timestamps 1512094832s and 1512094833s correspond to Points, the abscissas of these two points should take x ₇₆ +1/3*(x ₇₇ -x ₇₆ ) and x ₇₆ +2/3*(x ₇₇ -x ₇₆ ), and substitute these two abscissas into the following equations :

The calculated y values are the corresponding ordinates. Completing all missing positions according to this method, there will no longer be missing points on the trajectory. Finally, the effect of using the interpolation algorithm to complete is shown in Figure 4.

Step 3: Use Kalman filter to perform offset correction on the navigation track points after denoising and completion;

Step 3.1: Obtain the optimal estimated position of the trajectory point of the previous state, use the navigation system process model to predict the position of the trajectory point of the current state, and obtain the covariance of the predicted position of the trajectory point of the navigation system in the current state;

The position of the trajectory point of the current state predicted by the navigation system process model is calculated according to the following formula to obtain the predicted value;

X(k|k-1)=AX(k-1|k-1)

Among them, X(k|k-1) represents the position prediction value of the trajectory point of the current state k predicted by the optimal estimated position X(k-1|k-1) of the trajectory point of the previous state k-1, A Indicates the gain of the navigation system, the value is 1;

The obtained covariance of the predicted position of the trajectory point of the navigation system in the current state is calculated and obtained according to the following formula:

P(k|k-1)=AP(k-1|k-1)A'+Q

Among them, P(k|k-1) and P(k-1|k-1) represent the covariance corresponding to X(k|k-1) and X(k-1|k-1) respectively, when k- When 1=0, P(k-1|k-1) represents the initial point covariance, that is, the initial covariance of the navigation system P(0|0)=1; when k-1>0, P(k-1| k-1)=(I-Kg(k-1)H)P(k-1|k-2); A'=A=1, Q represents the process covariance of the navigation system, which is 1×10 ^{− 5} , I is the identity matrix.

Step 3.2: Use the covariance of the predicted position of the trajectory point of the navigation system in the current state to calculate the Kalman gain of the predicted position of the trajectory point of the navigation system in the current state;

Step 3.3: Based on the predicted position of the navigation system at the current trajectory point and its Kalman gain, as well as the position measurement value of the trajectory point in the current state, use the Kalman filter optimal estimation to obtain the optimal estimated position of the trajectory point in the current state to obtain the optimal estimated position of the trajectory point in the current state. The optimal estimated position of the trajectory point of the current state is used as the corrected position;

The Kalman filter optimal estimation is used to obtain the optimal estimated position of the trajectory point in the current state, and the following formula is used to calculate and obtain:

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))

Kg(k)=P(k|k-1)H'/(HP(k|k-1)H'+R)

Among them, Kg(k) represents the Kalman gain, Z(k) represents the measured value of the trajectory point at state k, and H represents the gain of the navigation system state to the measured value of the navigation system, which takes a value of 1.

Among them, in the initial state of the navigation system, the optimal estimated position value of the first track point of the navigation track is the position measurement value of the first track point; in the initial state of the navigation system, the value of the first track point of the navigation track The covariance takes a value of 1.

The change curves of latitude and longitude before and after filtering are shown in Figure 5. We can see that the latitude position of one of the points is obviously offset. The local navigation trajectory before filtering is shown in FIG. 6 , and the points with latitude position offset in FIG. 5 may belong to the points that need to be offset corrected in the local navigation trajectory. The filtered local navigation trajectory is shown in Figure 7. It can be seen that using the Kalman filter algorithm, the offset point positions in Figure 6 are corrected. After correction, the local navigation trajectory is closer to the real driving trajectory.

The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art to which the present invention pertains can make various modifications or additions to the described specific embodiments or substitute in similar manners, but will not deviate from the spirit of the present invention or go beyond the definitions of the appended claims range.

Claims (5)

1. a kind of vehicle mounted guidance tracing point based on Kalman filtering deviates antidote, which comprises the following steps:

Step 1: denoising is carried out to the tracing point in navigation path；

Step 2: completion processing is carried out to the missing point in navigation path；

Step 3: using Kalman filtering to the navigation path point after denoising and completion, carrying out offset correction；

Step 3.1: obtaining the optimal estimation position of the tracing point of preceding state, predicted using navigation system process model current The position of the tracing point of state, and navigation system is obtained in the covariance of the tracing point predicted position of current state；

Step 3.2: using navigation system in the covariance of the tracing point predicted position of current state, calculating navigation system current The kalman gain of the tracing point predicted position of state；

Step 3.3: based on navigation system in current trace points predicted position and its kalman gain and the track of current state The position measurements of point obtain the optimal estimation position of the tracing point of current state using Kalman filtering optimal estimation, to work as The optimal estimation position of the tracing point of preceding state is as the position after correction；

Wherein, in the initial state, the optimal estimation positional value of first tracing point of navigation path is first to navigation system The position measurements of tracing point；In the initial state, the covariance value of first tracing point of navigation path is navigation system 1。

2. the method according to claim 1, wherein described predict current state using navigation system process model Tracing point position according to following formula carry out calculate obtain predicted value；

X (k | k-1)=AX (k-1 | k-1)

Wherein, X (k | k-1) indicates to obtain with the optimal estimation position X of the tracing point of preceding state k-1 (k-1 | k-1) prediction Current state k tracing point position prediction value, A indicate navigation system gain, value 1；

The covariance of tracing point predicted position of the acquisition navigation system in current state, calculates according to following formula and obtains:

P (k | k-1)=AP (k-1 | k-1) A'+Q

Wherein, P (k | k-1) and P (k-1 | k-1) respectively indicate the corresponding covariance of X (k | k-1) and X (k-1 | k-1), work as k-1= When 0, P (k-1 | k-1) indicates initial point covariance, i.e. the initial covariance P (0 | 0)=1 of navigation system；Work as k-1 > 0, P (k-1 | K-1)=(I-Kg (k-1) H) P (k-1 | k-2)；A'=A=1, Q indicate that the process covariance of navigation system, value are 1 × 10^-5, I is unit matrix.

3. according to the method described in claim 2, acquisition is current it is characterized in that, described use Kalman filtering optimal estimation The optimal estimation position of the tracing point of state, is calculated using the following equation acquisition:

X (k | k)=X (k | k-1)+Kg (k) (Z (k)-HX (k | k-1))

Kg (k)=P (k | k-1) H'/(HP (k | k-1) H'+R)

Wherein, Kg (k) indicates that kalman gain, Z (k) indicate that the measured value of the tracing point in state k, H indicate navigation system shape Gain of the state for navigation system in measured value, value 1.

4. method according to claim 1-3, which is characterized in that the tracing point in navigation path carries out The process of denoising is as follows:

Step 1.1: speed sectors division is carried out to the tracing point in navigation path；

To in navigation path, section where belonging to the continuous path point of identical speed sectors carries out speed sectors division, each 5 continuous path points are included at least in speed sectors；

Tracing point speed sectors include low speed section [0~10) m/s, middling speed section [20~30) m/s, high-velocity section [0~10m/ s；

Step 1.2: calculating the distance between adjacent track point in each speed sectors, judge the adjacent track in each speed sectors Whether the distance between point is more than preset normal maximum distance, if being more than, then it is assumed that second track in adjacent track point Point is noise spot, by noise point deletion；

There is no tracing point missing:

Low speed section, it is 20m that the sampling time, which is separated by the distance of the normal maximum between two points of 1s,；

Middling speed section, sampling time are separated by the 3 of the instantaneous velocity that the distance of the normal maximum between two points of 1s is previous point Times；

High-velocity section, sampling time are separated by the 2 of the instantaneous velocity that the distance of the normal maximum between two points of 1s is previous point Times；

There are tracing point missing:

It is corresponding there is no tracing point missing multiplied by place speed sectors with the sampling time difference of two neighboring point The product that maximum distance obtains, as the preset normal maximum distance between adjacent two o'clock.

5. according to the method described in claim 4, it is characterized in that, the missing point in navigation path carries out completion processing Process it is as follows:

Step 2.1: tracing point lacks degree detecting；

The timestamp information of each point is successively traversed since navigation path starting point, if the sampling instant of the latter tracing point with The sampling instant of previous tracing point is more than sampling time interval, then there is missing point, counts the subsequent company of the latter tracing point Missing time in continuous 30 tracing points is with continuous 30 points that the latter tracing point starts if missing time is more than 10s Serious track is lacked, the orbit segment is deleted, otherwise, continually looks for next missing point；

Step 2.2: the navigation path point based on piecewise linear interpolation lacks completion；

The timestamp information that each point is successively traversed since navigation path starting point finds all positions for having missing, by certain One deletion sites section is denoted as [x_i,x_i+1]；

In deletion sites section [x_i,x_i+1] the sampling time differences of former and later two tracing points is n (s), wherein 0 < n≤10 and n are whole Number；

By deletion sites section [x_i,x_i+1] n equal part is carried out, completion point position abscissa distinguishes 1/n, the 2/n in deletion sites section, The position of 3/n ..., (n-1)/n Along ent successively substitute into following once linear equation and calculate ordinate；

Wherein, (x, y) indicates the coordinate of completion point, (x_i,y_i) and (x_i+1,y_i+1) respectively indicate deletion sites and take front and back two a little The coordinate of a tracing point.