An error correction method for an inertial navigation system based on road network matching

Abstract

The present invention belongs to the technical field of integrated navigation, and specifically relates to an error correction method for an inertial navigation system based on road network matching. The road network information is obtained by reading the R-tree (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 amount. The inertial navigation positioning point is translated onto the road network through the correction amount, effectively correcting the cumulative error of the inertial navigation system. Through the R-tree data structure, candidate road segments within a set range around the positioning point can be indexed extremely quickly and accurately. When calculating the probability of candidate road segments, geometric similarity, heading angle similarity, and road segment connectivity are considered to achieve accurate matching between inertial navigation information and the road network. Different inertial navigation correction strategies are adopted for straight and curved trajectories. When it is a straight trajectory, the translation vector from the trajectory end point to its road network matching point is used as the correction amount to eliminate the lateral error. When it is a curved trajectory, the correction amount is calculated jointly using the start point, end point, and their respective road network matching points to eliminate both lateral and longitudinal errors.

Description

Technical Field

[0001] The present invention belongs to the technical field of integrated navigation, and particularly relates to a method for correcting errors of an inertial navigation system based on road network matching. Background Art

[0002] An inertial navigation system uses three-axis gyroscopes and three-axis accelerometers as sensitive elements to measure the motion information of a carrier. It has the advantages of autonomy, real-time performance, all navigation parameters, and being unaffected by external signals, and is widely used in vehicle positioning systems. However, due to the drift of the inertial navigation system and relying on the integration principle to calculate the speed and position of the vehicle, there are large cumulative errors in the case of long-term operation, which affects the positioning accuracy.

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

[0004] Objective of the present invention: To solve the problem of cumulative errors existing in the long-term operation of an inertial navigation system in the case of GPS denial, the present invention proposes a method for correcting errors of an inertial navigation system based on road network matching.

[0005] Technical solution of the present invention: In order to achieve the above-mentioned invention objective, a method for correcting errors of an inertial navigation system based on road network matching is proposed. The road network information is obtained by reading the R-tree (R-tree) file obtained after preprocessing, and the inertial navigation positioning information is matched with the road network. The matching result is input into the correction algorithm to calculate the longitude and latitude correction amount, and the inertial navigation positioning point is translated onto the road network through the correction amount, effectively correcting the cumulative 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 segments, calculate the minimum bounding rectangle of each road segment and its corresponding quadrant attribute according to the starting and ending coordinates of the road segment, add them to the R-tree data, and generate the national road network R-tree file; Step 2: Establish an observation sequence; Step 3: According to the historical inertial navigation positioning points, calculate the cumulative driving mileage. If it is less than the set distance threshold d2, or if it is greater than or equal to d2 but the change in heading angle of a certain point among the previous N observation points at 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 change in heading angle of the previous N observation points at the current position is less than the set threshold, then regard the recorded observation sequence as a section of trajectory; Step 4: Use the R-tree to search for candidate road segments within a set range around the observation points in the observation sequence of this section of trajectory, and then combine geometric similarity, heading angle similarity, and road network connectivity factors to calculate the observation probability and state transition probability of each road segment, 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 this section of trajectory is a straight line or a curve, adopt corresponding correction strategies, and obtain the longitude and latitude correction amounts for the inertial navigation; Step 6: Use the correction amounts obtained in Step 5 to translate the inertial navigation positioning points received in real time through the serial port, thereby eliminating the cumulative error.

[0007] In a possible embodiment, in the said Step 1, it specifically includes the following steps:

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

[0009] Step 12: Calculate its minimum bounding rectangle according to the starting and ending coordinates of the road segment, represented by the coordinates of the lower left vertex P point and the upper right vertex Q point of the rectangle, that is (lat P , lon P , lat Q , lon Q );

[0010] Step 13: Judge its quadrant attribute according to the starting and ending coordinates of the road segment. The four quadrants correspond to the northeast, southwest, northwest, and southeast directions respectively;

[0011] Step 14: Add the minimum bounding rectangle of the road segment (lat P , lon P , lat Q , lon Q ) and the quadrant attribute as a node to the R-tree data;

[0012] Step 15: Until the traversal of all road segments in the map files of each administrative city is completed, generate the national road network R-tree and save the national road network R-tree file.

[0013] In a possible embodiment, in the said step 2, it specifically includes the following steps: Step 21: Receive the longitude and latitude coordinates and the heading angle data sent by the inertial navigation system, take the first inertial navigation positioning point as the first observation point, and set it as the starting point of this section of the trajectory;

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

[0015]

[0016] distance = R·c

[0017] In the formula, 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, arctan is the arctangent function, and distance is the distance between 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, then select it as an observation point; otherwise, do not select it as an observation point;

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

[0019] In a possible embodiment, in the said step 4, it specifically includes the following steps:

[0020] Step 41: Denote the observation sequence of this section of the trajectory as z k =(z1, z2, z3,...); Through the national R-tree road network file, search for the set of minimum bounding rectangles of candidate road segments within a set range around each observation point;

[0021] Step 42: For each candidate road segment, according to its quadrant attribute, restore the minimum bounding rectangle information (lat P , lon P , lat Q , lon Q ) to the starting and ending coordinates of the road segment, and denote the candidate road segment as r i =(r1, r2, r3,...);

[0022] Step 43: Project the first observation point z k , k = 1 of this section of the trajectory onto the candidate road segment r i , and denote the projection point as x k,i , and calculate the observation probability of the first observation point of this section of the trajectory on each road segment;

[0023] Step 44: For the (k + 1)-th observation point, use the R-tree to search for the minimum bounding rectangle of the road segments within a set range around it, and parse the starting and ending coordinates of the road segments. Discard the candidate road segments whose direction change amount from the observation point's heading angle is greater than the set threshold, and project the (k + 1)-th observation point onto the candidate road segments;

[0024] Step 45: Calculate the state transition probability from the projection point corresponding to the k-th observation point to the projection point corresponding to the (k + 1)-th observation point;

[0025] Step 46: Repeat steps 43 to 45 until the last observation point. Find the candidate road segment with the maximum total probability and its projection point, continuously backtrack to find the road segment and projection point corresponding to the previous observation point, so as to find the hidden state sequence with the maximum probability corresponding to the observation sequence. Save the projection points on each road segment to the matching point sequence to complete the matching of the inertial navigation trajectory and the digital map to obtain the road network matching point sequence.

[0026] In a possible embodiment, in step 43, it specifically includes the following steps:

[0027] Step 431: Project the observation point z k onto the candidate road segment r i to obtain the projection point x k,i , and use the haversine formula to calculate the projection distance, that is, the distance between the two points z k and x k,i ;

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

[0029]

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

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

[0032]

[0033] In the formula, p(z k |r i ) is the probability of the observation point z k on the road segment r i , x k,i is the projection point of the observation point z k on the road segment r iThe projection point on it, σ is the standard deviation of the set normal distribution function, ω1 is the weight of the projection geometric similarity, and ω2 is the weight of the course angle similarity.

[0034] In a possible embodiment, in the said step 45, it specifically includes the following steps:

[0035] Step 451: Denote the projection point of the k-th observation point on the road section r i as x k,i , denote the projection point of the (k + 1)-th observation point on the road section r j as x k+1,j , perform connectivity discrimination on the candidate road sections corresponding to the observation points in the previous and next frames, and calculate the driving distance of the projection points in the previous and next frames along the road network, denoted as ||x k,i -x k+1,j || route ;

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

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

[0038] (2) If the ending point of the road section r i is the same as the coordinate of the starting point of the road section r j , denote this point as x end_point , it is determined to be driving on the subsequent road section, and the road sections are connected; the calculation formula for the driving distance of the projection point x k,i and the projection point x k+1,j along the road network is as follows:

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

[0040] (3) If the ending point x i of the road section r ebd_point is the same as the starting point x j of the road section r start_pointIf the distance dist between them is less than the set threshold d3, it is determined that the road segments are connected; the projection point x k,i and the projection point x k+1,j The calculation formula for the driving distance along the road network is as follows:

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

[0042] If the end point of the road segment r i is projected onto the road segment r j and the projection distance dist is less than the set threshold d3, it is determined that the road segments are connected, and this projection point is denoted as x projection , the projection point x k,i and the projection point x k+1,j The calculation formula for the driving distance along the road network is as follows:

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

[0044] (4) If the above conditions are not met, it is determined that the road segments are not connected. Set the driving distance penalty value d penalty for the unconnected road segments, the projection point x k,i and the projection point x k+1,j The calculation formula for the driving distance along the road network is as follows:

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

[0046] Step 452: Calculate the distance between the observation points of two consecutive frames, and calculate the difference d k between it and the driving distances of the projection points of two consecutive frames along the road network. The formula is as follows:

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

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

[0049]

[0050] In the formula, β is a set parameter, and the larger it is, the greater the tolerance for non-connected road segments.

[0051] In a possible embodiment, in the said step 5, the heading angle change amount is solved by using the heading angles 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 amount of each point relative to the starting point is less than the set threshold, this section of the trajectory is considered a straight line. If the heading angle change amount of a certain point is greater than the set threshold, it is considered that there is a curve in this section of the trajectory.

[0052] In a possible embodiment, for a straight-line trajectory, the calibration strategy is to calculate the difference between the longitude and latitude of the end point A of the inertial navigation trajectory and its road network matching point B, and the calibration amount can be obtained.

[0053] In a possible embodiment, for a curved trajectory, a vector is calculated by taking the difference between the coordinates of the matching point and the observation point of the starting point of the inertial navigation trajectory observation sequence. Denote the road network matching point of the end point A of the inertial navigation trajectory as point B, and translate the coordinates of the trajectory end point A through the vector to obtain point C, and calculate the slopes of the straight line AB and the straight line AC according to the following formula;

[0054]

[0055] Denote the intersection point of the perpendicular line of the straight line AB and the perpendicular line of the straight line AC as point D, and calculate the slopes of the straight line BD and the straight line CD according to the following formula:

[0056]

[0057] Calculate the longitude of point D, and the calculation formula is as follows:

[0058]

[0059] Substitute it into the straight line CD equation to obtain the latitude of point D;

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

[0061] Point D is the actual position of the trajectory observation end point A on the road network, and thus the correction amount is obtained as Advantages of the present invention

[0062] In view of the cumulative error caused by inertial navigation drift during the long-duration driving of a vehicle in a GPS-denied scenario, the present invention suppresses the inertial navigation cumulative error through the matching of inertial navigation information with the road network, improving the positioning accuracy of the vehicle. By pre-processing the original map offline in advance into an R-tree data structure, subsequent algorithms can directly load the national R-tree file, which can be loaded within 3 seconds, avoiding the problems of time-consuming and computational resource consumption caused by directly loading the original digital map. In addition, through the R-tree data structure, candidate road segments within a set range around the positioning point can be indexed extremely quickly and accurately.

[0063] When calculating the probability of candidate road segments, geometric similarity, heading angle similarity, and road segment connectivity are considered to achieve accurate matching of inertial navigation information with the road network. Different inertial navigation correction strategies are adopted for straight-line and curved trajectories. When it is a straight-line trajectory, the translation vector from the trajectory end point to its road network matching point is used as the correction amount to eliminate the lateral error. When it is a curved trajectory, the correction amount is calculated jointly by the starting point and the end point and their respective road network matching points, eliminating both lateral and longitudinal errors at the same time. Experiments prove that the correction amount obtained through the matching result of inertial navigation and the road network in the present invention can effectively reduce the cumulative error of inertial navigation. Description of the drawings

[0064] Figure 1 is a schematic diagram of the minimum circumscribed rectangle of the road segment of the present invention;

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

[0066] Figure 3 is a schematic diagram of the abnormal processing of connected road segments of the present invention.

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

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

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

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

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

[0072] Figure 9 It is a comparison chart of the positioning errors before and after the inertial navigation calibration of the present invention. Specific Embodiments

[0073] To make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with the accompanying drawings and examples. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall 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 each administrative city in the country, traverse the road segments, and refer to Figure 1 , calculate the minimum bounding rectangle of the road segment and its corresponding quadrant attribute according to the start and end coordinates of the road segment, add them to the R-tree data, generate the national road network R-tree file, and realize map preprocessing.

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

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

[0078] Step 12: For each road segment, calculate its minimum bounding rectangle according to the start and end coordinates of the road segment, which is represented by the coordinates of the lower left vertex P and the upper right vertex Q of the rectangle, that is, (lat P , lon P , lat Q , lon Q ).

[0079] Step 13: Judge its quadrant attribute according to the start and end coordinates of the road segment. The four quadrants correspond to the northeast, southwest, northwest, and southeast directions respectively.

[0080] Step 14: Add the minimum bounding rectangle of the road segment (lat P , lon P , lat Q , lon Q ) and the quadrant attribute as a node to the R-tree data.

[0081] Step 15: Until the traversal of all road segments in the map files of each administrative city is completed, generate the national road network R-tree, save the R-tree file, and realize map preprocessing.

[0082] In the present invention, step 1 is completed offline in advance. When the algorithm in the present invention runs online, the national R-tree file obtained after preprocessing is directly loaded. Since the minimum bounding rectangle of the road segment is stored in the R-tree, the present invention saves the quadrant attribute of the road segment so as to obtain the starting point and ending point coordinates of the road segment from the minimum bounding rectangle information by using the quadrant attribute subsequently.

[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. Thus, by preprocessing the map offline as an R-tree file in advance, the problems of time consumption and computing resource consumption for loading the original map during the online operation of the present invention are solved.

[0084] Step 3: Take the first inertial navigation positioning point as the first observation point, obtain the current inertial navigation positioning point through the serial port, and judge whether to select it as an observation point by comparing the distance between this positioning point and the previous observation point with the distance threshold d1.

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

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

[0087]

[0088] distance = R·c

[0089] In the formula, lat1 and lon1 are the latitude and longitude of the first point, lat2 and 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, arctan is the arctangent 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, then select it as an observation point; otherwise, do not select it as an observation point, so as to eliminate redundant inertial navigation positioning points such as stop points and dense points during low-speed driving.

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

[0092] Step 41: Calculate the distance between adjacent inertial navigation positioning points using the above haversine formula and accumulate it to obtain the driving mileage.

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

[0094] Step 43: If the driving mileage is greater than or equal to the set distance threshold d2 and the heading angle changes of the set N observation points before the current position are all less than the set threshold, then regard the current position as the trajectory segmentation point, and record the observation sequence of this section of trajectory as z k =(z1, z2, z3,...). Through the national R-tree road network file, search for the set of minimum bounding rectangles of candidate road segments within a set range around each observation point.

[0095] Step 44: Refer to Figure 1 , for each candidate road segment, according to its quadrant attribute, restore the minimum bounding rectangle information (lat P , lon P , lat Q , lon Q ) to the start and end coordinates of the road segment, and record the candidate road segment as r i =(r1, r2, r3,...).

[0096] Step 45: Project the first observation point z k (k = 1) of this section of trajectory onto the candidate road segment r i , and record the projection point as x k,i , and calculate the observation probability of the first observation point of this section of trajectory on each road segment.

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

[0098] Step 451: Project the observation point zk Project onto the candidate road segment r i to obtain the projection point x k,i , and calculate the projection distance using the haversine formula, that is, z k and x k,i distance between the two points.

[0099] Step 452: Calculate the change in the heading angle Δθ 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, and θ2 is the heading angle of the candidate road segment.

[0102] Step 453: In calculating the observation probability, the present invention takes into account the geometric similarity and heading angle similarity between the observation point and the road segment, and calculates the observation probability of the observation point on the road segment r i using the projection distance and the change in the heading angle Δθ between the observation point and the candidate road segment. The calculation formula is as follows:

[0103]

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

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

[0106] Step 46: For the (k + 1)-th observation point, use the R-tree to search for the minimum bounding rectangle of the road segments within a set range around it, and parse the start and end coordinates of the road segments. Discard the candidate road segments whose road segment direction and the change in the heading angle of the observation point are greater than the set threshold, and project the (k + 1)-th observation point onto the candidate road segments.

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

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

[0109] Step 471: Denote the projection point of the k-th observation point on the road segment r i as x k,i, denote the projection point of the (k + 1)-th observation point on road segment r j as x k+1,j . Conduct connectivity discrimination on the candidate road segments corresponding to the observation points in two consecutive frames, and calculate the driving distance of the projection points in two consecutive frames along the road network, denoted as ||x k,i - x k+1,j || route .

[0110] (1) If the starting and ending points of road segment r i and road segment r j are the same, that is, the (k + 1)-th observation point is still on the road segment corresponding to the observation point at the previous moment, it is determined to be driving on the previous road segment, and the road segments are connected. The calculation formula for the driving distance of projection point x k,i and projection point x k+1,j 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 ending point of road segment r i has the same coordinates as the starting point of road segment r j , denote this point as x end_point , it is determined to be driving on the subsequent road segment, and the road segments are connected. The calculation formula for the driving distance of projection point x k,i and projection point x k+1,j along the road network is as follows:

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

[0114] (3) Refer to Figure 3 (a). If the distance dist between the ending point x i of road segment r end_point and the starting point x j of road segment r start_point is less than the set threshold d3, it is determined that the road segments are connected. The calculation formula for the driving distance of projection point x k,i and projection point x k+1,j along the road network is as follows:

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

[0116] Refer to Figure 3 (b), Figure 3 (c), if the projection distance dist of the end point of the road segment r i onto the road segment r j is less than the set threshold d3, it is determined that the road segments are connected, and this projection point is denoted as x projection , the projection point x k,i and the projection point x k+1,j The calculation formula for the driving distance along the road network is as follows:

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

[0118] (4) If the above conditions are not met, it is determined that the road segments are not connected. Set the driving distance penalty value d of the unconnected road segment penalty , the projection point x k,i and the projection point x k+1,j The calculation formula for the driving 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 in the previous and next frames, and calculate the difference d between it and the driving distance of the projection points in the previous and next frames along the road network k , the formula is as follows:

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

[0122] Refer to Figure 4 , for the two observation points z k and z k+1 , the k-th observation point z k has projection points x t,1 and xt,3 The (k + 1)-th observation point z k+1 also has multiple candidate road segments. Taking the projection point x of z on the road segment r2 as an example, x k+1 or x k+1,2 For example, x k,1 or x k,3 Both of the two projection points can be transferred to x k+1,2 , due to the different magnitudes of d k , they have different state transition probabilities.

[0123] Step 473: The state transition probability from the projection point x on the road segment r to the projection point x on the road segment r follows an exponential distribution, and the calculation formula for the state transition probability is as follows: i The projection point x on the road segment r k,i to the projection point x on the road segment r j The projection point x on the road segment r k+1,j The state transition probability is related to d k Follows an exponential distribution, and the calculation formula for the state transition probability is as follows:

[0124]

[0125] In the formula, β is a set parameter, and the larger it is, the greater the tolerance for non-connected road segments.

[0126] Step 48: According to the calculation method of the observation probability of the candidate road segments of the k-th observation point z in the previous step 45, calculate the observation probability corresponding to the candidate road segment r of the (k + 1)-th observation point z. Refer to the Viterbi algorithm schematic diagram in k . Let the total probability of the first observation point z1 on the road segment r be its own observation probability. For the (k + 1)-th observation point z (k≥1), its total probability on the road segment r and with the projection point x is the total probability of the k-th projection point x multiplied by the state transition probability from the projection point x to the projection point x, and then multiplied by the observation probability corresponding to the projection point x. The calculation formula is as follows. For each observation point, only the M candidate road segments with the largest total probability are retained. k+1 The candidate road segment r j Corresponding observation probability. Refer to Figure 5 In the Viterbi algorithm schematic diagram, assume that the total probability of the first observation point z1 on the road segment r is its own observation probability. For the (k + 1)-th observation point z i (k≥1), its total probability on the road segment r k+1 and with the projection point x j is the total probability of the k-th projection point x k+1,j multiplied by the state transition probability from the projection point x k,i to the projection point x k,i , and then multiplied by the observation probability corresponding to the projection point x k+1,j . The calculation formula is as follows. For each observation point, only the M candidate road segments with the largest total probability are retained. k+1,j p(x

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

[0128] Step 49: Repeat steps 46 to 48 until the last observation point, find the candidate road segment with the maximum total probability and its projection point, such as Figure 5 the road segment r7 in

[0129] Step 5: Refer to Figure 6 to determine whether this section of the trajectory is a straight line or a curve using the heading angle information of the observation sequence, and adopt corresponding correction strategies for straight lines and curves to obtain the longitude and latitude correction amounts for the inertial navigation.

[0130] Step 51: Curve discrimination. Calculate the change in the heading angle by solving the difference between the heading angles of each point in the observation sequence and the starting heading angle of this section of the trajectory. If the change in the heading angle of each point relative to the starting point is less than the set threshold, it is considered that this section of the trajectory is a straight line. If the change in a certain heading angle is greater than the set threshold, it is considered that there is a curve in this section of the trajectory.

[0131] Step 52: If the trajectory is a straight line, refer to Figure 7 then directly calculate the difference between the longitude and latitude of the end point A of the inertial navigation trajectory and its road network matching point B to obtain the correction amount thus eliminating the lateral error.

[0132] Step 53: If the trajectory is a curve, refer to Figure 8 and calculate the vector by taking the difference between the coordinates of the matching point and the observation point at the starting point of the observation sequence of the inertial navigation trajectory Denote the road network matching point of the end point A of the inertial navigation trajectory as point B, and then translate the coordinates of the end point A of the trajectory by the vector to obtain point C, and calculate the slopes of the straight lines AB and AC.

[0133]

[0134] Denote the intersection point of the perpendicular line of the straight line AB and the perpendicular line of the straight line AC as point D, and the slopes of the straight lines BD and CD are as follows:

[0135]

[0136] Calculate the longitude of point D, and the calculation formula is as follows:

[0137]

[0138] Substitute it into the equation of the straight line CD to obtain the latitude of point D.

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

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

[0141] Step 6: Use the obtained correction amount to translate the inertial navigation positioning points received in real time by the serial port, so as to eliminate the cumulative error and improve the positioning accuracy of the inertial navigation.

[0142] The method of the present invention has been deployed on an embedded platform, and a road test is carried out from Xi'an to Baoji, passing through road conditions such as urban sections, highway sections, service areas, gas stations, etc. The whole journey is 176 kilometers. Taking the GPS satellite positioning value as the reference true value, refer to Figure 9 , this figure is the error comparison chart of the inertial navigation system before and after correcting the cumulative error of 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 error correction method of the inertial navigation system based on road network matching proposed by the present invention can significantly reduce the cumulative error of the inertial navigation system, and thus significantly improve the positioning accuracy of the inertial navigation.

[0143] For the purpose of illustration, the above specific embodiments are exemplary, in order to better enable those skilled in the art to understand this patent, and should not be construed as a limitation on the scope of this patent; all technical solutions obtained by using 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 comprises the following steps: Step 1: reading the map files of administrative cities across the country, traversing the road sections, calculating 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, adding them to the R-tree data, and generating the national road network R-tree file; Step 2: establishing an observation sequence; in said 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 section of the trajectory; Step 22: using the semi-haversine formula to calculate the distance between the inertial navigation positioning point currently received by the serial port and the previous observation point, the formula is as follows: distance=R·c Wherein, 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, arctan 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; Step 3: According to the historical inertial navigation positioning points, calculate the cumulative mileage. 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, the recorded observation sequence is taken 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 in real time by the serial port, thereby eliminating 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 2, 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 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.

4. The method for correcting an inertial navigation system error based on road network matching according to claim 3 is 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 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 , σ 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.

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 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) If the road section 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 stqrt_point -x k+1,j || If the road section r i The end point is projected onto the road segment rj 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 determined to be disconnected; a driving distance penalty value d is set 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.

6. 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.

7. The method for correcting an inertial navigation system error based on road network matching according to claim 6, 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:

8. The method for correcting an inertial navigation system error based on road network matching according to claim 6, characterized in that: For curved trajectories, the vector is calculated by subtracting the coordinates of the matching point at 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