Inertial navigation system error correction method based on road network matching

Abstract

The invention belongs to the technical field of integrated navigation, and particularly relates to an inertial navigation system error correction method based on road network matching, which comprises the following steps: reading an R-tree file obtained after preprocessing to obtain road network information, matching inertial navigation positioning information with a road network, and inputting a matching result into a correction algorithm to calculate longitude and latitude correction values, and the inertial navigation positioning point is translated to the road network through the correction value, so that the accumulated error of the inertial navigation system is effectively corrected. Through an R-tree data structure, candidate road sections within a set range around a positioning point can be indexed extremely quickly and accurately. When the probability of candidate road sections is calculated, geometric similarity, course angle similarity and road section connectivity are considered, and accurate matching of inertial navigation information and a road network is achieved. Different inertial navigation correction strategies are adopted for linear and curve tracks, a translation vector of a track end point pointing to a road network matching point is adopted as a correction value in the linear track to eliminate transverse errors, a correction value is calculated by adopting a starting point, the end point and the road network matching points corresponding to the starting point and the end point in the curve track, and transverse and longitudinal errors are eliminated at the same time.

Description

Technical Field

[0001] The invention belongs to the technical field of combined navigation, and in particular relates to an inertial navigation system error correction method based on road network matching. Background Art

[0002] The inertial navigation system uses a three-axis gyroscope and a three-axis accelerometer as sensitive components to measure the carrier's motion information. It has the advantages of autonomy, real-time, full navigation parameters, and no interference from external signals. It is widely used in vehicle positioning systems. However, due to the drift of the inertial navigation system, it relies on the integration principle to solve the vehicle's speed and position. In the case of long flight time, there is a large cumulative error, which affects the positioning accuracy.

[0003] In the prior art, the cumulative error of inertial navigation is mainly suppressed by the combined navigation of the global positioning system (GPS) and the inertial navigation system. However, the positioning accuracy decreases in the case of GPS denial and interference with satellite signals. Digital maps have long-term invariance and high accuracy, reaching the meter level or even higher. By matching the inertial navigation information with the road network, the longitude and latitude correction of the inertial navigation positioning system is obtained, which significantly improves the positioning accuracy of the inertial navigation system. Summary of the invention

[0004] The purpose of the present invention is to solve the problem of cumulative error of the inertial navigation system during long-distance driving under GPS denial conditions. The present invention proposes an inertial navigation system error correction method based on road network matching.

[0005] Technical solution of the present invention: In order to achieve the above-mentioned purpose of the invention, a method for correcting the error of an inertial navigation system based on road network matching is proposed, in which the road network information is obtained by reading the R-tree file obtained after preprocessing, the inertial navigation positioning information is matched with the road network, and the matching result is input into the correction algorithm to calculate the longitude and latitude correction value, and the inertial navigation positioning point is translated to the road network by the correction value, so as to effectively correct the accumulated error of the inertial navigation system.

[0006] An inertial navigation system error correction method based on road network matching includes the following steps: Step 1: read the map files of administrative cities across the country, traverse the road sections, calculate the minimum circumscribed rectangular frame of the road section and its corresponding quadrant attributes according to the coordinates of the starting point and the end point of the road section, add them to the R-tree data, and generate the national road network R-tree file; Step 2: establish an observation sequence; Step 3: calculate the cumulative mileage according to the historical inertial navigation positioning points, if it is less than the set distance threshold d2, or it is greater than or equal to d2 but the heading angle change of a point in the N observation points before the current position is greater than the set threshold, then repeat step 2; if it is greater than or equal to the set distance threshold d2, and the heading angle change of the N observation points before the current position is greater than the set threshold, then repeat step 2; if it is greater than or equal to the set distance threshold d2, and the heading angle change of the N observation points before the current position is greater than the set threshold, then repeat step 2. If the heading angle changes are all less than the set threshold, the recorded observation sequence is regarded as a trajectory; Step 4: Use the R-tree to search for candidate sections within the set range around the observation point in the observation sequence of the trajectory, and then combine the geometric similarity, heading angle similarity, and road network connectivity factors to calculate the observation probability and state transition probability of each section, and use the Viterbi algorithm to solve and obtain the road network matching point sequence; Step 5: Use the heading angle information of the road network matching point sequence to determine whether the trajectory is a straight line or a curve, and use the corresponding correction strategy to obtain the longitude and latitude correction of the inertial navigation; Step 6: Use the correction amount obtained in step 5 to translate the inertial navigation positioning point received by the serial port in real time, thereby eliminating the cumulative error.

[0007] In a possible embodiment, in step 1, the following steps are specifically included:

[0008] Step 11: Read the digital map files of each administrative city;

[0009] Step 12: Based on the coordinates of the starting point and the end point of the road segment, calculate its minimum circumscribed rectangle, which is represented by the coordinates of the lower left vertex point P and the upper right vertex point Q of the rectangular box, that is, (lat P ,lon P ,lat Q ,lon Q );

[0010] Step 13: According to the coordinates of the starting point and the end point of the road section, determine its quadrant attribute. The four quadrants correspond to the northeast, southwest, northwest, and southeast directions respectively;

[0011] Step 14: Set the minimum circumscribed rectangle (lat P ,lon P ,lat Q ,lon Q ), quadrant attribute, added as a node to the R-tree data;

[0012] Step 15: After all road sections in each administrative city map file have been traversed, a national road network R-tree is generated and the national road network R-tree file is saved.

[0013] In a possible embodiment, in step 2, the following steps are specifically included: Step 21: receiving the latitude and longitude coordinates and heading angle data sent by the inertial navigation system, taking the first frame inertial navigation positioning point as the first observation point, and setting it as the starting point of this segment of the trajectory;

[0014] Step 22: Use the haversine formula to calculate the distance between the inertial navigation positioning point currently received by the serial port and the last observation point. The formula is as follows:

[0015]

[0016] distance=R·c

[0017] Where lat1, lon1 are the latitude and longitude of the first point, lat2, lon2 are the latitude and longitude of the second point, Δlat is the latitude difference, Δlon is the longitude difference, R is the radius of the earth, atan is the inverse tangent function, and distance is the distance between the two points;

[0018] Step 23: If the distance between the current inertial navigation positioning point and the previous observation point is greater than the distance threshold d1, it is selected as the observation point, otherwise, it is not selected as the observation point;

[0019] Step 24: Repeat steps 22-23 to form an observation sequence.

[0020] In a possible embodiment, in step 4, the following steps are specifically included:

[0021] Step 41: Record the observation sequence of this trajectory as z k =(z 1 ,z 2 ,z 3 ,…); Through the national R-tree road network file, search for the minimum circumscribed rectangular frame set of candidate road sections within the set range around each observation point;

[0022] Step 42: For each candidate road segment, according to its quadrant attribute, its minimum bounding rectangle information (lat P ,lon P ,lat Q ,lon Q ) is restored to the starting and ending coordinates of the road segment, and the candidate road segment is denoted as r i =(r 1 ,r 2 ,r 3 ,…);

[0023] Step 43: Set the first observation point z of this trajectory k , k = 1 is projected to the candidate road segment r iOn the left, let the projection point be x k,i , calculate the observation probability of the first observation point of this segment of trajectory on each road segment;

[0024] Step 44: for the k+1th observation point, use the R-tree to search for the minimum circumscribed rectangular frame of the road section within the set range around it, and parse the coordinates of the starting point and the end point of the road section, discard the candidate road section whose direction and the heading angle change of the observation point are greater than the set threshold, and project the k+1th observation point onto the candidate road section;

[0025] Step 45: Calculate the state transition probability of the projection point corresponding to the k-th observation point transferring to the projection point corresponding to the k+1-th observation point;

[0026] Step 46: Repeat steps 43 to 45 until the last observation point, find the candidate road section with the largest total probability and its projection point, and continuously backtrack to find the road section and projection point corresponding to the observation point at the previous moment, so as to find the implicit state sequence with the largest probability corresponding to the observation sequence, save the projection points on each road section to the matching point sequence, complete the matching of the inertial navigation trajectory and the digital map, and obtain the road network matching point sequence.

[0027] In a possible embodiment, step 43 specifically includes the following steps:

[0028] Step 431: Set the observation point x k Projected to the candidate road segment r i On, get the projection point x k,i , use the haversine formula to calculate the projection distance, that is, z k With x k,i Distance between two points;

[0029] Step 432: Calculate the heading angle change Δθ between the observation point and the candidate road segment. The calculation formula is as follows:

[0030]

[0031] In the formula, θ 1 is the heading angle of the observation point, θ 2 is the heading angle of the candidate road segment;

[0032] Step 433: Calculate the position of the observation point in the road segment r according to the projection distance between the observation point and the candidate road segment and the heading angle change Δθ. i The observation probability on is calculated as follows:

[0033]

[0034] In the formula, p(z k |r i ) is the observation point z k On the road section iThe probability of x k,i is the observation point z k On the road section i The projection point on the ,σ is the standard deviation of the normal distribution function, ω 1 is the projected geometric similarity weight, ω 2 is the heading angle similarity weight.

[0035] In a possible embodiment, in step 45, the following steps are specifically included:

[0036] Step 451: Record the kth observation point on section r i The projection point on is x k,i , let the k+1th observation point be on section r j The projection point on is x k+1,j , the connectivity of the candidate road sections corresponding to the observation points of the previous and next two frames is determined, and the travel distance of the projection points of the previous and next two frames along the road network is calculated, which is recorded as ||x k,i -x k+1,j || route ;

[0037] (1) If the road section r i and road segment r j The starting point and the end point are the same, that is, the k+1th observation point is still on the road section corresponding to the observation point at the previous moment, and it is determined to be driving on the previous road section, and the road sections are connected; the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0038] ||x k,i -x k+1,j || route =||x k,i -x k+1,j ||

[0039] (2) If the road section r i The end point and section r j The coordinates of the starting point are the same, and this point is denoted as x end_point , it is determined that the vehicle is traveling on the subsequent road segment, and the road segments are connected; the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0040] ||x k,i -x k+1,j || route =||x k,i -x end_point ||+||x end_point -x k+1,j ||

[0041] (3) SeeFigure 3 (a), if the road segment r i The end point x end_point With road section r j Starting point x start_point If the distance dist is less than the set threshold d3, the road section is determined to be connected; the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0042] ||x k,i -x k+1,j || route =||x k,i -x end_point ||+dist+||x start_point -x k+1,j ||

[0043] If the road section r i The end point is projected onto the road segment r j The projection distance dist on the road is less than the set threshold d3, and the road section is determined to be connected. The projection point is recorded as x projection , the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0044] ||x k,i -x k+1,j || route =||x k,i -x end_point ||+dist+||x projection -x k+1,j ||

[0045] (4) If none of the above conditions are met, the road section is considered disconnected. Set the driving distance penalty value d for the disconnected road section penalty , the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0046] ||x k,i -x k+1,j || route =||x k,i -x k+1,j ||+d penalty

[0047] Step 452: Calculate the distance between the observation points of the previous and next two frames, and calculate the difference d between the distance between the observation points of the previous and next two frames and the travel distance along the road network of the projection points. k , the formula is as follows:

[0048] d k =|||zk -z k+1 ||-||x k,i -x k+1,j || route |;

[0049] Step 453: From the road segment r i The projection point x on k,i Go to road segment r j The projection point x on k+1,j The state transition probability and d k It obeys the exponential distribution, and the calculation formula of the state transition probability is as follows:

[0050]

[0051] Where β is a setting parameter, and the larger the value, the greater the tolerance to non-connected sections.

[0052] In a possible embodiment, in step 5, the heading angle change is solved using the heading angle of each point in the road network matching sequence and the heading angle of the starting point of this section of the trajectory. If the heading angle change of each point relative to the starting point is less than a set threshold, this section of the trajectory is considered to be a straight line. If a heading angle change is greater than the set threshold, it is considered that there is a curve in this section of the trajectory.

[0053] In a possible embodiment, for a straight track, the correction strategy is to subtract the longitude and latitude of the inertial navigation track end point A from its road network matching point B to obtain the correction value:

[0054] In a possible embodiment, for a curved trajectory, the vector is calculated by subtracting the coordinates of the matching point of the starting point of the inertial navigation trajectory observation sequence from the coordinates of the observation point The road network matching point of the end point A of the inertial navigation track is called point B, and the coordinates of the end point A of the track are calculated through the vector Translate to get point C, and calculate the slopes of line AB and line AC according to the following formula;

[0055]

[0056] Let the intersection of the perpendicular line of line AB and the perpendicular line of line AC be point D, and calculate the slope of line BD and line CD according to the following formula:

[0057]

[0058] Calculate the longitude of point D using the following formula:

[0059]

[0060] Substituting into the equation of line CD, find the latitude of point D;

[0061] lat D =K CD ·(lon D -lon C )+lat C

[0062] Point D is the actual position of the trajectory observation end point A on the road network, so the correction amount is obtained as The beneficial effects of the present invention are aimed at the cumulative error caused by inertial navigation drift during long-distance driving of vehicles in GPS denial scenarios. The present invention suppresses the cumulative inertial navigation error by matching inertial navigation information with the road network, thereby improving the positioning accuracy of the vehicle. By pre-processing the original map offline into an R-tree data structure in advance, the subsequent algorithm directly loads the national R-tree file, which can be loaded within 3 seconds, avoiding the problem of directly loading the original digital map, which is time-consuming and consumes computing resources. In addition, through the R-tree data structure, the candidate sections within the set range around the positioning point can be indexed extremely quickly and accurately.

[0063] When calculating the probability of candidate sections, the geometric similarity, heading angle similarity and section connectivity are considered to achieve accurate matching of inertial navigation information with the road network. Different inertial navigation correction strategies are used for straight and curved trajectories. For straight trajectories, the translation vector from the end point of the trajectory to its road network matching point is used as the correction amount to eliminate lateral errors. For curved trajectories, the correction amount is calculated using the starting point and the end point and their corresponding road network matching points to eliminate lateral and longitudinal errors at the same time. Experiments have shown that the correction amount obtained by the present invention through the inertial navigation and road network matching results can effectively reduce the cumulative error of the inertial navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 It is a schematic diagram of the minimum circumscribed rectangular frame of the road section of the present invention;

[0065] Figure 2 is a flow chart of the process of trajectory observation sequence and map matching of the present invention;

[0066] Figure 3 It is a schematic diagram of the present invention for handling abnormalities of a Unicom segment.

[0067] Figure 4 It is a schematic diagram of the state transition probability calculation of the present invention;

[0068] Figure 5 It is a schematic flow chart of the Viterbi algorithm adopted by the present invention;

[0069] Figure 6 It is a schematic diagram of the correction algorithm structure of the present invention;

[0070] Figure 7 It is a schematic diagram of the straight line trajectory correction amount calculation strategy of the present invention;

[0071] Figure 8 is a schematic diagram of a curve trajectory correction amount calculation strategy of the present invention;

[0072] Fig. 9 It is a comparison diagram of positioning errors before and after the inertial navigation correction of the present invention. DETAILED DESCRIPTION

[0073] In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and examples. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0074] The present invention is specifically completed according to the following steps:

[0075] Step 1: Read the map files of administrative cities across the country, traverse the road sections, and refer to Figure 1 , the minimum circumscribed rectangular box of the road section and its corresponding quadrant attributes are calculated according to the coordinates of the starting point and end point of the road section, added to the R-tree data, and the national road network R-tree file is generated to realize map preprocessing.

[0076] In this embodiment, the map file comes from the OpenStreetMap platform, which provides a free global map database. The map files of all administrative cities in the country are downloaded and processed into a national road network file of R-tree data type.

[0077] Step 11: Read the digital map files of each administrative city and traverse the road sections.

[0078] Step 12: For each road segment, calculate its minimum circumscribed rectangle based on the coordinates of the starting point and the end point of the road segment, expressed as the coordinates of the lower left vertex point P and the upper right vertex point Q of the rectangular box, that is, (lat P ,lon P ,lat Q ,lon Q ).

[0079] Step 13: According to the coordinates of the starting point and the end point of the road section, determine its quadrant attribute. The four quadrants correspond to the northeast, southwest, northwest, and southeast directions respectively.

[0080] Step 14: Set the minimum circumscribed rectangle (lat P ,lon P ,lat Q ,lon Q ), quadrant attribute, added as a node to the R-tree data.

[0081] Step 15: After all road sections in the map files of each administrative city have been traversed, an R-tree of the national road network is generated, and the R-tree file is saved to realize map preprocessing.

[0082] In the present invention, step 1 is completed offline in advance, and the algorithm in the present invention directly loads the national R-tree file obtained after preprocessing when it is run online. Since the R-tree stores the minimum bounding rectangle of the road section, the present invention saves the quadrant attribute of the road section so that the quadrant attribute can be used to obtain the starting point and end point coordinates of the road section from the minimum bounding rectangle frame information later.

[0083] Step 2: Load the national road network R-tree file obtained after map preprocessing. This step can be completed in only 3 seconds on the embedded platform. It can be seen that by preprocessing the map offline into an R-tree file in advance, the problem of time-consuming and computing resource-consuming loading of the original map during online operation of the present invention is solved.

[0084] Step 3: Take the first frame of inertial navigation positioning point as the first observation point, obtain the current inertial navigation positioning point through the serial port, and compare the distance between the positioning point and the previous observation point with the distance threshold d1 to determine whether to select it as the observation point.

[0085] Step 31: The serial port receives the latitude and longitude coordinates and heading angle data sent by the inertial navigation system, takes the first frame of inertial navigation positioning point as the first observation point, and sets it as the starting point of this trajectory.

[0086] Step 32: Use the haversine equation to calculate the distance between the inertial navigation positioning point currently received by the serial port and the last observation point. The formula is as follows:

[0087]

[0088] distance=R·c

[0089] Where lat1, lon1 are the latitude and longitude of the first point, lat2, lon2 are the latitude and longitude of the second point, Δlat is the latitude difference, Δlon is the longitude difference, R is the radius of the earth, atan is the inverse tangent function, and distance is the distance between the two points.

[0090] Step 33: If the distance between the current inertial navigation positioning point and the previous observation point is greater than the distance threshold d1, it is selected as the observation point. Otherwise, it is not selected as the observation point, thereby eliminating redundant inertial navigation positioning points such as stop points and dense points during low-speed driving.

[0091] Step 4: Calculate the cumulative mileage based on the historical inertial navigation positioning points. If it is less than the set distance threshold d2, or it is greater than or equal to d2 but the heading angle change of one of the N observation points before the current position is greater than the set threshold, repeat step 3. If it is greater than or equal to the set distance threshold d2, and the heading angle changes of the N observation points before the current position are all less than the set threshold, then the recorded observation sequence is regarded as a trajectory, refer to Figure 2 , use the R-tree to search for candidate road sections within a set range around the observation point in the observation sequence of the trajectory, use the geometric similarity, heading angle similarity, and road network connectivity factors to calculate the observation probability and state transition probability of each road section, and use the Viterbi algorithm to solve and obtain the road network matching point sequence.

[0092] Step 41: Calculate the distances between adjacent inertial navigation positioning points using the above-mentioned haversine formula, and accumulate them to obtain the mileage.

[0093] Step 42: If the mileage is less than the set distance threshold d2, or the mileage is greater than or equal to the distance threshold d2, but the heading angle change of a point among the set N observation points before the current position is greater than the set threshold, repeat step 3.

[0094] Step 43: If the mileage is greater than or equal to the set distance threshold d2, and the heading angle changes of the N observation points before the current position are all less than the set threshold, the current position is used as the trajectory segmentation point, and the observation sequence of this segment of the trajectory is recorded as z k =(z 1 ,z 2 ,z 3 ,…). Through the national R-tree road network file, the minimum bounding rectangle set of candidate road sections within the set range around each observation point is searched.

[0095] Step 44: See Figure 1 For each candidate road segment, according to its quadrant attributes, its minimum bounding rectangle information (lat P ,lon P ,lat Q ,lon Q ) is restored to the starting and ending coordinates of the road segment, and the candidate road segment is denoted as r i =(r 1 ,r 2 ,r 3 ,…).

[0096] Step 45: Set the first observation point z of this trajectory k (k=1) Projected to candidate road segment r i Let the projection point be x k,i , calculate the observation probability of the first observation point of this trajectory on each road segment.

[0097] The specific calculation steps of observation probability are as follows:

[0098] Step 451: Set the observation point z k Projected to the candidate road segment r i On, get the projection point x k,i , use the haversine formula to calculate the projection distance, that is, z k With x k,i The distance between two points.

[0099] Step 452: Calculate the heading angle change Δθ between the observation point and the candidate road segment. The calculation formula is as follows:

[0100]

[0101] In the formula, θ 1 is the heading angle of the observation point, θ 2 is the heading angle of the candidate road segment.

[0102] Step 453: The present invention considers the geometric similarity and heading angle similarity between the observation point and the road section when calculating the observation probability, and calculates the observation point on the road section r according to the projection distance between the observation point and the candidate road section and the heading angle change Δθ. i The observation probability on is calculated as follows:

[0103]

[0104] In the formula, p(z k |r i ) is the observation point z k On the road section i The probability of x k,i is the observation point z k On the road section i The projection point on the ,σ is the standard deviation of the normal distribution function, ω 1 is the projected geometric similarity weight, ω 2 is the heading angle similarity weight.

[0105] In this embodiment, the geometric similarity weight ω 1 and heading angle similarity weight ω 2 Both are set to 0.5.

[0106] Step 46: For the k+1th observation point, use the R-tree to search for the minimum circumscribed rectangular box of the road section within the set range around it, and parse the starting and ending coordinates of the road section, discard the candidate road sections whose direction and the heading angle change of the observation point are greater than the set threshold, and project the k+1th observation point onto the candidate road section.

[0107] Step 47: Calculate the state transition probability of the projection point corresponding to the k-th observation point transferring to the projection point corresponding to the k+1-th observation point.

[0108] The calculation steps of state transition probability are as follows:

[0109] Step 471: Record the kth observation point on section r i The projection point on is x k,i , let the k+1th observation point be on section r j The projection point on is x k+1,k , the connectivity of the candidate road sections corresponding to the observation points of the previous and next two frames is determined, and the travel distance of the projection points along the road network of the previous and next two frames is calculated, which is recorded as ||x k,i -x k+1,k || route .

[0110] (1) If the road section r i and road segment r j The starting point and end point are the same, that is, the k+1th observation point is still on the road section corresponding to the observation point at the previous moment, and it is determined to be driving on the previous road section, and the road sections are connected. k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0111] ||x k,i -x k+1,j || route =||x k,i -x k+1,j ||

[0112] (2) If the road section r i The end point and section r j The coordinates of the starting point are the same, and this point is denoted as x end_point , it is determined that the vehicle is traveling on the subsequent road segment, and the road segments are connected. Projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0113] ||x k,i -x k+1,i || route =||x k,i -x end_point ||+||x end_point -x k+1,j ||

[0114] (3) See Figure 3 (a), if the road segment r i The end point x ebd_point With road section r j Starting point x start_pointIf the distance dist is less than the set threshold d3, the road section is determined to be connected. k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0115] ||x k,i -x k+1,j || route =||x k,i -x end_point ||+dist+||x start_point -x k+1,j ||

[0116] See also Figure 3 (b), Figure 3 (c) If the road segment r i The end point is projected onto the road segment r j The projection distance dist on the road is less than the set threshold d3, and the road section is determined to be connected. The projection point is recorded as x projection , the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0117] ||x k,i -x k+1,i || route =||x k,i -x end_point ||+dist+||x projection -x k+1,j ||

[0118] (4) If none of the above conditions are met, the road section is considered disconnected. Set the driving distance penalty value d for the disconnected road section penalty , the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows:

[0119] ||x k,i -x k+1,j || route =||x k,i -x k+1,j ||+d penalty

[0120] Step 472: Calculate the distance between the observation points of the previous and next two frames, and calculate the difference d between the distance between the observation points of the previous and next two frames and the travel distance along the road network of the projection points. k , the formula is as follows:

[0121] d k =|||z k -z k+1 ||-||x k,i -xk+1,j || route |

[0122] See also Figure 4 , for the two observation points z k and z k+1 , the kth observation point z k In the candidate section x 1 and x 3 There is a projection point x on t,1 and x t,3 The k+1th observation point z k+1 There are also multiple candidate sections, with z k+1 In r 2 Projection point x on the road segment k+1,2 For example, x k,1 or x k,3 Both projection points can be transferred to x k+11,2 , due to d k Different sizes have different state transition probabilities.

[0123] Step 473: From the road segment r i The projection point x on k,i Go to road segment r j The projection point x on k+1,j The state transition probability and d k It obeys the exponential distribution, and the calculation formula of the state transition probability is as follows:

[0124]

[0125] Where β is a setting parameter, and the larger the value, the greater the tolerance to non-connected sections.

[0126] Step 48: According to the k-th observation point z in the above step 45 k The calculation method of the observation probability of the candidate road section is to calculate the k+1th observation point z k+1 The candidate road segment r j The corresponding observation probability. Figure 5 Schematic diagram of the Viterbi algorithm, assuming the first observation point z 1 On the road section i The total probability on is its own observation probability, for the k+1th observation point z k+1 (k≥1), which is on section r j On and the projection point is x k+1,j The total probability of the kth projection point x k,i The total probability multiplied by the projected point x k,i Transfer to the projection point x k+1,j The state transition probability is multiplied by the projection point x k+1,jThe corresponding observation probability is calculated as follows: for each observation point, only the M candidate sections with the largest total probability are retained.

[0127] p(x k+1,j )=p(x k,i )p(x k+1,j |x k,i )p(z k+1 |x k+1,j )

[0128] Step 49: Repeat steps 46 to 48 until the last observation point is reached to find the candidate road section with the highest total probability and its projection point, such as Figure 5 The road section r 7 , continuously backtrack to find the road section and projection point corresponding to the observation point at the previous moment, so as to find the implicit state sequence with the highest probability corresponding to the observation sequence, save the projection points on each road section to the matching point sequence, and complete the matching of the inertial navigation trajectory and the digital map.

[0129] Step 5: See Figure 6 , the heading angle information of the observation sequence is used to determine whether the trajectory is a straight line or a curve, and corresponding correction strategies are adopted for the straight line and the curve to obtain the longitude and latitude correction values ​​of the inertial navigation.

[0130] Step 51: Curve identification. The heading angle change is solved by using the heading angle of each point in the observation sequence and the heading angle of the starting point of this segment of the trajectory. If the heading angle change of each point relative to the starting point is less than the set threshold, the current segment of the trajectory is considered to be a straight line. If a heading angle change is greater than the set threshold, it is considered that there is a curve in this segment of the trajectory.

[0131] Step 52: If the trajectory is a straight line, refer to Figure 7 , then the correction value can be obtained by directly subtracting the longitude and latitude of the inertial navigation track end point A from its road network matching point B This eliminates lateral errors.

[0132] Step 53: If the track is a curve, refer to Figure 8 , the vector is calculated by subtracting the coordinates of the matching point of the starting point of the inertial navigation trajectory observation sequence from the coordinates of the observation point The road network matching point of the end point A of the inertial navigation track is recorded as point B, and then the coordinates of the end point A of the track are calculated through the vector Translate to get point C, and calculate the slopes of lines AB and AC.

[0133]

[0134] Let the intersection of the perpendicular line of line AB and the perpendicular line of line AC be point D. The slopes of lines BD and CD are as follows:

[0135]

[0136] Calculate the longitude of point D using the following formula:

[0137]

[0138] Substituting into the equation of line CD, find the latitude of point D.

[0139] lat D =K CD ·(lon D -lon C )+lat C

[0140] Point D is the actual position of the trajectory observation end point A on the road network, so the correction amount is obtained as Eliminate longitudinal and lateral errors.

[0141] Step 6: Use the obtained correction value to translate the inertial navigation positioning point received by the serial port in real time, thereby eliminating the cumulative error and improving the positioning accuracy of the inertial navigation.

[0142] The method of the present invention has been deployed on an embedded platform, and a sports car experiment was conducted from Xi'an to Baoji, passing through urban sections, expressways, service areas, gas stations and other road conditions, with a total distance of 176 kilometers. The GPS satellite positioning value was used as the reference true value. Fig. 9 , which is an error comparison diagram before and after the cumulative error of the inertial navigation system is corrected by the present invention. The gray dotted line represents the inertial navigation error before correction, and the black solid line represents the inertial navigation error after correction. It can be seen from the data in the figure that the inertial navigation system error correction method based on road network matching proposed by the present invention can significantly reduce the cumulative error of the inertial navigation system, thereby significantly improving the accuracy of inertial navigation positioning.

[0143] For the purpose of explanation, the above-mentioned specific implementation methods are only exemplary, in order to better enable those skilled in the art to understand this patent, and should not be understood as limiting the scope of this patent; any technical solutions obtained by equivalent substitution or equivalent transformation fall within the protection scope of the present invention.

Claims

1. A method for correcting an inertial navigation system error based on road network matching, characterized in that: The method includes the following steps: Step 1: read the map files of administrative cities across the country, traverse the road sections, calculate the minimum circumscribed rectangular frame of the road section and its corresponding quadrant attributes according to the coordinates of the starting point and the end point of the road section, add them to the R-tree data, and generate the national road network R-tree file; Step 2: establish an observation sequence; Step 3: calculate the cumulative mileage according to the historical inertial navigation positioning points. If it is less than the set distance threshold d2, or it is greater than or equal to d2 but the heading angle change of a point in the N observation points before the current position is greater than the set threshold, repeat step 2; if it is greater than or equal to the set distance threshold d2, and the heading angle changes of the N observation points before the current position are all less than the set threshold, then repeat step 2. Threshold, the recorded observation sequence is regarded as a trajectory; Step 4: Use R-tree to search for candidate sections within a set range around the observation point in the observation sequence of the trajectory, and then combine the geometric similarity, heading angle similarity, and road network connectivity factors to calculate the observation probability and state transition probability of each section, and use the Viterbi algorithm to solve and obtain the road network matching point sequence; Step 5: Use the heading angle information of the road network matching point sequence to determine whether the trajectory is a straight line or a curve, and use the corresponding correction strategy to obtain the longitude and latitude correction of the inertial navigation; Step 6: Use the correction amount obtained in step 5 to translate the inertial navigation positioning point received by the serial port in real time, so as to eliminate the cumulative error.

2. The method for correcting an inertial navigation system error based on road network matching according to claim 1, characterized in that: In the step 1, the following steps are specifically included: Step 11: Read the digital map files of each administrative city; Step 12: Based on the coordinates of the starting point and the end point of the road segment, calculate its minimum circumscribed rectangle, which is represented by the coordinates of the lower left vertex point P and the upper right vertex point Q of the rectangular box, that is, (lat P ,lon P ,lat Q ,lon Q ); Step 13: According to the coordinates of the starting point and the end point of the road section, determine its quadrant attribute. The four quadrants correspond to the northeast, southwest, northwest, and southeast directions respectively; Step 14: Set the minimum circumscribed rectangle (lat P ,lon P ,lat Q ,lon Q ), quadrant attribute, added as a node to the R-tree data; Step 15: After all road sections in each administrative city map file have been traversed, a national road network R-tree is generated and the national road network R-tree file is saved.

3. The method for correcting an inertial navigation system error based on road network matching according to claim 1, characterized in that: In the step 2, the following steps are specifically included: Step 21: receiving the latitude and longitude coordinates and heading angle data sent by the inertial navigation system, taking the first frame of the inertial navigation positioning point as the first observation point, and setting it as the starting point of this segment of the trajectory; Step 22: Use the haversine formula to calculate the distance between the inertial navigation positioning point currently received by the serial port and the last observation point. The formula is as follows: distance=R·c Where lat1, lon1 are the latitude and longitude of the first point, lat2, lon2 are the latitude and longitude of the second point, Δlat is the latitude difference, Δlon is the longitude difference, R is the radius of the earth, atan is the inverse tangent function, and distance is the distance between the two points; Step 23: If the distance between the current inertial navigation positioning point and the previous observation point is greater than the distance threshold d1, it is selected as the observation point, otherwise, it is not selected as the observation point; Step 24: Repeat steps 22-23 to form an observation sequence.

4. The method for correcting an inertial navigation system error based on road network matching according to claim 1, characterized in that: In step 4, the following steps are specifically included: Step 41: Record the observation sequence of this trajectory as z k =(z1,z2,z3,…); through the national R-tree road network file, search for the minimum circumscribed rectangular frame set of the candidate road sections within the set range around each observation point; Step 42: For each candidate road segment, according to its quadrant attribute, its minimum bounding rectangle information (lat P ,lon P ,lat Q ,lon Q ) is restored to the starting and ending coordinates of the road segment, and the candidate road segment is denoted as r i =(r1,r2,r3,…); Step 43: Set the first observation point z of this trajectory k , k = 1 is projected to the candidate road segment r i On the left, let the projection point be x k,i , calculate the observation probability of the first observation point of this segment of trajectory on each road segment; Step 44: for the k+1th observation point, use the R-tree to search for the minimum circumscribed rectangular frame of the road section within the set range around it, and parse the coordinates of the starting point and the end point of the road section, discard the candidate road section whose direction and the heading angle change of the observation point are greater than the set threshold, and project the k+1th observation point onto the candidate road section; Step 45: Calculate the state transition probability of the projection point corresponding to the k-th observation point transferring to the projection point corresponding to the k+1-th observation point; Step 46: Repeat steps 43 to 45 until the last observation point, find the candidate road section with the largest total probability and its projection point, and continuously backtrack to find the road section and projection point corresponding to the observation point at the previous moment, so as to find the implicit state sequence with the largest probability corresponding to the observation sequence, save the projection points on each road section to the matching point sequence, complete the matching of the inertial navigation trajectory and the digital map, and obtain the road network matching point sequence.

5. The method for correcting an inertial navigation system error based on road network matching according to claim 4, characterized in that: In the step 43, the following steps are specifically included: Step 431: Set the observation point z k Projected to the candidate road segment r i On, get the projection point x k,i , use the haversine formula to calculate the projection distance, that is, z k With x k,i Distance between two points; Step 432: Calculate the heading angle change Δθ between the observation point and the candidate road segment. The calculation formula is as follows: Where θ1 is the heading angle of the observation point, and θ2 is the heading angle of the candidate road section; Step 433: Calculate the position of the observation point in the road segment r according to the projection distance between the observation point and the candidate road segment and the heading angle change Δθ. i The observation probability on is calculated as follows: In the formula, p(z k |r i ) is the observation point x k On the road section i The probability of x k,i is the observation point z k On the road section i The projection point on , σ is the standard deviation of the set normal distribution function, ω1 is the projection geometry similarity weight, and ω2 is the heading angle similarity weight.

6. The method for correcting an inertial navigation system error based on road network matching according to claim 4, characterized in that: In the step 45, the following steps are specifically included: Step 451: Record the kth observation point on section r i The projection point on is x k,i , let the k+1th observation point be on section r j The projection point on is x k+1,j , the connectivity of the candidate road sections corresponding to the observation points of the previous and next two frames is determined, and the travel distance of the projection points of the previous and next two frames along the road network is calculated, which is recorded as ||x k,i -x k+1,j || route ; (1) If the road section r i and road segment r j The starting point and the end point are the same, that is, the k+1th observation point is still on the road section corresponding to the observation point at the previous moment, and it is determined to be driving on the previous road section, and the road sections are connected; the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows: ||x k,i -x k+1,j || route =||x k,i -x k+1,j || (2) If the road section r i The end point and section r j The coordinates of the starting point are the same, and this point is denoted as x end_point , it is determined that the vehicle is traveling on the subsequent road segment, and the road segments are connected; the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows: ||x k,i -x k+1,j || route =||x k,i -x end_point ||+||x end_point -x k+1,j || (3) Refer to Figure 3(a). If the road segment r i The end point x end_point With road section r j Starting point x start_point If the distance dist is less than the set threshold d3, the road section is determined to be connected; the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows: ||x k,i -x k+1,j || route =||x k,i -x end_point ||+dist+||x start_point -x k+1,j || If the road section r i The end point is projected onto the road segment r j The projection distance dist on the road is less than the set threshold d3, and the road section is determined to be connected. The projection point is recorded as x projection , the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows: ||x k,i -x k+1,j || route =||x k,i -x end_point ||+dist+||x projection -x k+1,j || (4) If none of the above conditions are met, the road section is considered disconnected. Set the driving distance penalty value d for the disconnected road section penalty , the projection point x k,i and the projection point x k+1,j The calculation formula for the travel distance along the road network is as follows: ||x k,i -x k+1,j || route =||x k,i -x k+1,j ||+d penalty Step 452: Calculate the distance between the observation points of the previous and next two frames, and calculate the difference d between the distance between the observation points of the previous and next two frames and the travel distance along the road network of the projection points. k , the formula is as follows: d k =|||z k -z k+1 ||-||x k,i -x k+1,j || route |; Step 453: From the road segment r i The projection point x on k,i Go to road segment r j The projection point x on k+1,j The state transition probability and d k It obeys the exponential distribution, and the calculation formula of the state transition probability is as follows: Where β is a setting parameter, and the larger the value, the greater the tolerance to non-connected sections.

7. The method for correcting an inertial navigation system error based on road network matching according to claim 1, characterized in that: In step 5, the heading angle change is solved using the heading angle of each point in the road network matching sequence and the heading angle of the starting point of this section of the trajectory. If the heading angle change of each point relative to the starting point is less than the set threshold, the current section of the trajectory is considered to be a straight line. If a heading angle change is greater than the set threshold, it is considered that there is a curve in the current section of the trajectory.

8. The method for correcting an inertial navigation system error based on road network matching according to claim 7, characterized in that: For a straight track, the correction strategy is to subtract the longitude and latitude of the inertial navigation track end point A from its road network matching point B to obtain the correction value:

9. The method for correcting an inertial navigation system error based on road network matching according to claim 7, characterized in that: For curved trajectories, the vector is calculated by subtracting the coordinates of the matching point of the starting point of the inertial navigation trajectory observation sequence from the coordinates of the observation point. The road network matching point of the end point A of the inertial navigation track is called point B, and the coordinates of the end point A of the track are calculated through the vector Translate to get point C, and calculate the slopes of line AB and line AC according to the following formula; Let the intersection of the perpendicular line of line AB and the perpendicular line of line AC be point D, and calculate the slope of line BD and line CD according to the following formula: Calculate the longitude of point D using the following formula: Substituting into the equation of line CD, find the latitude of point D; years D =K CD ·(lon D -lon C )+years C Point D is the actual position of the trajectory observation end point A on the road network, so the correction amount is obtained as

Images