Inertial navigation system error correction method fusing template matching and hidden Markov model

Abstract

The invention discloses an inertial navigation system error correction method fusing template matching and a hidden Markov model. The method comprises the following steps: constructing original map data into an R-tree index structure; key frames of the INS points are selected in a self-adaptive threshold value mode, and redundant INS points are screened out; detecting curve features of key frames in the INS trajectory by adopting a sliding window mechanism; creating a template image according to the INS trajectory, generating a road network image by using road section information in the R-tree index structure, and performing template matching processing on the road network image by adopting sliding search; when a plurality of candidate correction values are generated in template matching, a matching process is optimized through an HMM algorithm; and updating a correction value obtained after the matching process is optimized in a recursion mode to obtain a corrected INS position. According to the embodiment of the invention, the inertial navigation information is accurately matched with the map without depending on an external sensor, meanwhile, the transverse error and the longitudinal error of the INS are corrected, and the real-time requirement is met.

Description

Technical Field

[0001] This invention relates to the field of error correction technology, and in particular to an error correction method and system for inertial navigation systems that integrates template matching and hidden Markov models. Background Technology

[0002] High-precision positioning technology is a core technology of modern military vehicle navigation systems, ensuring that military vehicles accurately reach their target locations and providing crucial support for tactical deployment, collaborative operations, and logistical support. Inertial Navigation Systems (INS) have become the most widely used positioning method for military vehicles due to their autonomy, real-time performance, and comprehensive navigation parameters. However, because INS calculates position by integrating acceleration and angular velocity, it inevitably generates cumulative errors, affecting the accuracy and reliability of positioning. Therefore, correcting INS cumulative errors is an essential means to achieve high-precision positioning and a hot research topic both domestically and internationally.

[0003] Currently, INS error correction primarily relies on external information fusion. Wheeled odometers can provide displacement constraints for INS, thus reducing cumulative errors, but errors still accumulate over long periods of operation. While fusing environmental perception data with INS can improve positioning accuracy, it increases system hardware costs and makes the system susceptible to severe weather, making all-weather stable operation difficult. Fusion with absolute position information provided by the Global Positioning System (GPS) can effectively correct INS cumulative errors. However, GPS signals are vulnerable to obstruction, interference, and spoofing attacks; once the signal fails, INS errors cannot be effectively corrected. Road network maps possess long-term invariance and accuracy at the meter level or even higher, providing reliable absolute position information. By comparing the geometric features, topology, and attribute information of INS points with roads, matching INS points with the map corrects cumulative INS errors, offering advantages such as no need for additional hardware support, strong environmental adaptability, and low cost. Increasingly, researchers are exploring map matching algorithms to correct INS cumulative errors.

[0004] Current map matching algorithms are mainly divided into geometric methods, topological relationship methods, fuzzy logic methods, DS evidence theory methods, and probabilistic statistical methods. Hidden Markov Models (HMMs) are one of the most widely used map matching methods. They belong to the probabilistic statistical method, which calculates the observation probability by the projected distance from the trajectory point to the candidate road segment, and combines the topological connectivity of the road segment to model the state transition probability. It can effectively handle the uncertainty and noise of the positioning point and has high robustness.

[0005] In summary, the existing INS error correction methods have the following main drawbacks: (1) Although the INS error correction method based on environmental perception data can improve positioning accuracy, it increases the hardware cost of the system and is easily affected by severe weather, making it difficult to achieve stable operation around the clock.

[0006] (2) When HMM is applied to INS point and map matching, although the road network connectivity between two adjacent points of the trajectory is considered, it is still essentially based on point matching and it is difficult to capture the overall characteristics of the trajectory.

[0007] (3) The HMM-based method can only match normally when the cumulative error of INS is small, which limits its applicability in large-scale positioning offset scenarios and cannot effectively handle the large cumulative error generated after long-term operation.

[0008] (4) INS error includes lateral error perpendicular to the road and longitudinal error along the road. The HMM-based INS error correction algorithm associates trajectory points with the road centerline through vertical projection, which can only correct lateral error and cannot correct longitudinal error. In multi-lane scenarios, because the trajectory points are corrected to the road centerline, the lateral error of INS actually increases.

[0009] (5) Existing methods usually require processing the entire trajectory when performing map matching, which is computationally complex and difficult to meet real-time requirements, especially in complex road network environments where the computational burden is too heavy.

[0010] Therefore, it is necessary to improve one or more of the problems existing in the above-mentioned related technical solutions.

[0011] It should be noted that this section is intended to provide background or context for the technical solutions of this disclosure as set forth in the claims. The description herein does not constitute an admission that it is prior art simply because it is included in this section. Summary of the Invention

[0012] The purpose of this invention is to provide an error correction method and system for inertial navigation systems that integrates template matching and hidden Markov models, thereby overcoming, to at least some extent, one or more problems caused by the limitations and defects of related technologies.

[0013] This invention first provides an error correction method for inertial navigation systems that integrates template matching and hidden Markov models, comprising the following steps: Step 1: Construct the original map data into an R-tree index structure; Step 2: Select keyframes for INS points using an adaptive threshold method and filter out redundant INS points; Step 3: Use a sliding window mechanism to detect the curve features of keyframes in the INS trajectory; Step 4: Create a template image based on the INS trajectory, generate a road network image using the road segment information in the R-tree index structure, and perform template matching processing on the road network image using sliding search. Step 5: When template matching generates multiple candidate correction values, the matching process is optimized using the HMM algorithm; Step 6: Update the correction value obtained after the matching process optimization using a recursive method to obtain the corrected INS position.

[0014] In this invention, step 1 includes the following specific steps: Step 1-1: Based on the administrative city area division, download the OpenStreetMap map data for the whole country; Steps 1-2: Remove roads used by non-motorized vehicles from the map data; Steps 1-3: Calculate the minimum bounding rectangle (MBR) of the road segment based on the coordinates of the road segment endpoints, and insert the lane-attached attributes and the MBR together as leaf nodes into the R-tree. Steps 1-4: Based on spatial proximity, nodes that are close to each other are aggregated level by level to form parent nodes, thus constructing a multi-level R-tree index structure.

[0015] In this invention, step 2 includes the following specific steps: Step 2-1: Shift the INS point position using the correction amount to obtain the corrected INS point position; Step 2-2: When the distance between the current frame and the previous keyframe is greater than or equal to the distance threshold, the current frame is selected as the keyframe. Steps 2-3: The distance threshold is adaptively adjusted according to the change in heading angle.

[0016] In this invention, step 3 includes the following specific steps: Step 3-1: Set the keyframe sequence of the INS trajectory within the sliding window and calculate the change in the heading angle of the current point relative to the starting point of the window. Step 3-2: When the change in the heading angle of the key frame is greater than the preset heading angle change threshold, it is preliminarily determined that there may be a curve. Step 3-3: Connect the start and end points of the window and calculate the projection distance from the midpoint of the window to the connecting line. Steps 3-4: When the projected distance is greater than the preset heading angle change threshold, it is finally identified as a curve; otherwise, continue to judge the projected distance from the 1 / 4 and 3 / 4 points in the window to the connecting line. Steps 3-5: If the window is ultimately determined to have a curve, perform subsequent map matching on the keyframe sequence within the window.

[0017] In this invention, step 4 includes the following specific steps: Step 4-1: Based on the INS trajectory keyframe sequence, INS trajectory endpoint coordinates, and pixel values ​​of the corresponding region of the INS trajectory, create a trajectory shape image and a trajectory heading angle image; Step 4-2: Create a road network image based on the INS trajectory and candidate road segment information within the R-tree index range; Step 4-3: Using the trajectory shape image and the trajectory heading angle image as templates, slide them on the road network image to calculate the shape matching score and heading angle matching score. Step 4-4: Cluster the high-matching score points, calculate the centroid coordinates for each point in the cluster, and use the centroid coordinates as the correction value.

[0018] In this invention, step 5 includes the following specific steps: Step 5-1: Perform translation correction on the correction amount to obtain a new observation sequence; Step 5-2: Calculate the observation probability and state transition probability of the initial node in the new observation sequence, find the hidden state sequence with the highest probability, and obtain the corresponding matching probability. Step 5-3: Calculate the distance probability of the correction amount, and determine the correction amount that maximizes the joint probability of the distance probability and the state transition probability of the distance probability matching the correction amount road segment.

[0019] In this invention, step 6 includes the following specific steps: Update the correction values; The corrected INS position is obtained based on the updated correction value.

[0020] The present invention further provides an inertial navigation system error correction system that integrates template matching and hidden Markov models, comprising: The map preprocessing module is used to construct an R-tree index structure from the raw map data; The adaptive keyframe selection module is used to select keyframes of INS points using an adaptive threshold method and filter out redundant INS points. The curve detection module is used to detect curve features in keyframes of the INS trajectory using a sliding window mechanism. The template matching module is used to create template images based on INS trajectories, generate road network images using road segment information in the R-tree index structure, and perform template matching processing on the road network images using sliding search. The optimization module is used to optimize the matching process using the HMM algorithm when template matching generates multiple candidate correction values. The correction module is used to update the correction value obtained after the matching process is optimized through a recursive method to obtain the corrected INS position.

[0021] The technical solution provided by this invention may include the following beneficial effects: The present invention provides an inertial navigation system error correction method that integrates template matching and hidden Markov models. This method can accurately match inertial navigation information with a map without relying on external sensors, and simultaneously correct the lateral and longitudinal errors of the INS, thus meeting real-time requirements. Attached Figure Description

[0022] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this disclosure and, together with the description, serve to explain the principles of this disclosure. It is obvious that the drawings described below are merely some embodiments of this disclosure, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort.

[0023] Figure 1 A flowchart illustrating an error correction method for an inertial navigation system that integrates template matching and a hidden Markov model in an exemplary embodiment of this disclosure is shown. Figure 2 This diagram illustrates a specific flowchart of step 1 in an exemplary embodiment of this disclosure; Figure 3 This diagram illustrates a specific flowchart of step 2 in an exemplary embodiment of this disclosure; Figure 4 This diagram illustrates a specific flowchart of step 3 in an exemplary embodiment of this disclosure; Figure 5 This diagram illustrates a specific flowchart of step 4 in an exemplary embodiment of this disclosure; Figure 6 This diagram illustrates a specific flowchart of step 5 in an exemplary embodiment of this disclosure; Figure 7 This diagram illustrates a specific flowchart of step 6 in an exemplary embodiment of this disclosure; Figure 8 This diagram illustrates the framework of an inertial navigation system error correction system that integrates template matching and hidden Markov models in an exemplary embodiment of this disclosure. Figure 9 A schematic diagram showing the correction results of Embodiment 2 of this disclosure is provided. Figure 10 A comparison diagram of the error before and after correction in Embodiment 2 of this disclosure is shown. Detailed Implementation

[0024] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, they are provided so that this disclosure will be more comprehensive and complete, and will fully convey the concept of the exemplary embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0025] Furthermore, the accompanying drawings are merely illustrative diagrams of embodiments of this disclosure and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and therefore repeated descriptions of them will be omitted. Some block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities.

[0026] This example implementation first provides an error correction method for inertial navigation systems that integrates template matching and hidden Markov models. Please refer to [reference needed]. Figure 1 The error correction method for this inertial navigation system includes steps 1-6, as detailed below: Step 1: Construct the original map data into an R-tree index structure; Step 2: Select keyframes for INS points using an adaptive threshold method and filter out redundant INS points; Step 3: Use a sliding window mechanism to detect the curve features of keyframes in the INS trajectory; Step 4: Create a template image based on the INS trajectory, generate a road network image using the road segment information in the R-tree index structure, and perform template matching processing on the road network image using sliding search. Step 5: When template matching generates multiple candidate correction values, the matching process is optimized using the HMM algorithm; Step 6: Update the correction value obtained after the matching process optimization using a recursive method to obtain the corrected INS position.

[0027] In this embodiment, the inertial navigation information can be accurately matched with the map without relying on external sensors, while correcting the lateral and longitudinal errors of the INS, thus meeting the real-time requirements.

[0028] The specific process of each step in the above embodiments will be described below.

[0029] Please refer to Figure 2 Step 1 includes the following steps: Step 1-1: Based on the administrative city and regional divisions, download OpenStreetMap map data covering the entire country.

[0030] Steps 1-2: Remove roads used by non-motorized vehicles from the map data, such as waterways, railways, and sidewalks, and retain only valid road segment information for vehicles.

[0031] Steps 1-3: Calculate the minimum bounding rectangle (MBR) of the road segment based on the coordinates of the road segment endpoints, and insert the lane-attached attributes and the MBR together as leaf nodes into the R-tree.

[0032] Steps 1-4: Based on spatial proximity, nodes that are close to each other are aggregated level by level to form parent nodes, thus constructing a multi-level R-tree index structure.

[0033] Please refer to Figure 3 Step 2 includes the following steps: Step 2-1: Shift the INS point position using the correction amount to obtain the corrected INS point position.

[0034] Step 2-2: When the distance between the current frame and the previous keyframe is greater than or equal to a distance threshold, the current frame is selected as the keyframe. Specifically, let the previous keyframe be... Distance threshold is When the current INS point satisfies When that point is selected as a keyframe.

[0035] Steps 2-3: The distance threshold is adaptively adjusted according to the change in heading angle, and the calculation formula is as follows:

[0036] in, d Distance threshold This is the minimum value of the distance threshold. This is the maximum value of the distance threshold. This represents the change in heading angle. This represents the maximum range of changes in heading angle, used for normalization.

[0037] Please refer to Figure 4 Step 3 includes the following steps: Step 3-1: Set the keyframe sequence for the INS trajectory within the sliding window and calculate the change in the heading angle of the current point relative to the window start point. First, let the keyframe sequence of the trajectory within the sliding window be... Calculate the change in heading angle of the current point relative to the starting point of the window in the keyframe. .

[0038] Step 3-2, when the keyframe heading angle changes Greater than the preset heading angle change threshold At that time, it was initially determined that there might be a curve in the road. It can be set to 30 degrees, but it is not limited to that.

[0039] Step 3-3: Connect the start and end points of the window with a line, and calculate the projected distance from the midpoint of the window to the connecting line. .

[0040] Steps 3-4: When the projected distance is greater than the preset heading angle change threshold, i.e., if... Meet the conditions If the curve is not found, it is ultimately identified as a curve; otherwise, the projected distances from points 1 / 4 and 3 / 4 within the window to the connecting line are further determined. It can be set to 3 meters, but it is not limited to that; it can also be set to 4 meters, etc.

[0041] Steps 3-5: If the window is ultimately determined to have a curve, perform subsequent map matching on the keyframe sequence within the window.

[0042] Please refer to Figure 5 Step 4 includes the following steps: Step 4-1: Create a trajectory shape image and a trajectory heading angle image based on the INS trajectory keyframe sequence, INS trajectory endpoint coordinates, and pixel values ​​of the corresponding region of the INS trajectory.

[0043] Step 4-1 specifically includes the following steps: (1) Determine the template image size based on the latitude and longitude range covered by the INS trajectory keyframe sequence and the set pixel resolution (1 meter); (2) Draw line segments in the image according to the coordinates of the trajectory endpoints, set the line segment width to the vehicle width, such as 2 meters, assign the pixel value of the corresponding area of ​​the trajectory to 1, and set the pixel value of the other area to 0, and generate the trajectory shape image T_shape; (3) Assign the pixel values ​​of the corresponding area of ​​the trajectory to the actual heading angle information, and set the pixel values ​​of the other areas to 0 to generate the trajectory heading angle image T_heading.

[0044] Step 4-2: Create a road network image based on the INS trajectory and candidate road segment information within the R-tree index range.

[0045] Step 4-2 specifically includes the following steps: (1) Extend the INS track range by 100 meters in all directions (up, down, left, and right) to determine the size and latitude / longitude range of the road network image; (2) Use an R-tree index to index the candidate road segment information within this range; (3) Draw line segments in the road network image according to the coordinates of the road segment endpoints. The line segment width is set as the product of the number of lanes and the lane width (the lane width is 3 meters). (4) Set the pixel value corresponding to the road segment to 1 and the pixel value of the remaining area to 0 to generate the road network shape image R_shape; (5) Set the pixel value corresponding to the road segment to the heading angle value of the road segment, and set the pixel value of the remaining area to 0 to generate the road network heading angle image R_heading.

[0046] Step 4-3: Using the trajectory shape image and the trajectory heading angle image as templates, slide them on the road network image to calculate the shape matching score and heading angle matching score.

[0047] Step 4-3 specifically includes the following steps: (1) Shape similarity matching: Using the trajectory shape image T_shape as a template, slide it on the road network shape image R_shape, and calculate the shape similarity matching score, as shown in the following formula:

[0048] in, N represents the pixel values ​​of the area covered by the trajectory after translation in the road network shape image, where N is the number of pixels covered by the trajectory.

[0049] (2) Heading angle similarity matching: Using the trajectory heading angle image T_heading as a template, slide it on the road network heading angle image R_heading, and calculate the heading angle similarity matching score, as shown in the following formula:

[0050] in, The trajectory is translated in the road network heading angle image. The pixel values ​​of the area covered by the trajectory, where N is the number of pixels covered by the trajectory.

[0051] (3) The total matching score is calculated as follows:

[0052] Step 4-4: Cluster the high-matching score points, calculate the centroid coordinates for each point in the cluster, and use the centroid coordinates as the correction value.

[0053] Step 4-4 specifically includes the following steps: (1) Clustering of high-matching-score points is performed using a depth-first search algorithm; (2) Calculate the centroid coordinates of each point in the cluster by combining its matching score:

[0054] In the formula, , Let i be the coordinates of the i-th point in the point set. It represents the corresponding weight, and n is the total number of points.

[0055] (3) The centroid coordinates of the cluster are used as the correction value, denoted as .

[0056] Please refer to Figure 6 Step 5 includes the following steps: Step 5-1: The correction amount is shifted to obtain a new observation sequence. First, let the candidate correction amount output by the TM algorithm be... Then, after translational correction using candidate correction values, a new observation sequence is obtained. .

[0057] Step 5-2: Calculate the observation probability and state transition probability of the initial node in the new observation sequence, find the hidden state sequence with the highest probability, and obtain the corresponding matching probability.

[0058] Step 5-2 specifically includes the following steps: (1) Initialization: Calculate the observation probability of the initial node:

[0059] In the formula, For observation point Matched road segment On The probability of observation, Let the standard deviation of the normal distribution function be defined. As the weight of the projection distance factor, For the heading angle factor weight, This represents the current heading and route. The difference between headings.

[0060] (2) Recursion: Calculate the state transition probability:

[0061] In the formula, To set parameters that represent the degree of tolerance to non-connecting pathway segments, The larger the value, the greater the tolerance to non-connecting pathway segments; It represents the distance or cost between two consecutive states.

[0062] (3) Backtracking optimization: Find the hidden state sequence with the highest probability and obtain the corresponding matching probability P_match.

[0063] Step 5-3: Calculate the distance probability of the correction amount, and determine the correction amount that maximizes the joint probability of the distance probability and the state transition probability of the distance probability matching the correction amount road segment.

[0064] First, calculate the distance probability of the correction:

[0065] in, As an adjustment factor, a larger value tends to favor a correction value with a smaller modulus.

[0066] Then, select the correction factor that maximizes the joint probability:

[0067] in, The optimal state under the maximum a posteriori probability estimate. As a prior probability of a state, it represents the initial judgment of the state; The likelihood probability represents the observed data in this state. The possibility.

[0068] Please refer to Figure 7 Step 6 includes the following steps: Step 6-1: Update the calibration values:

[0069] in, This is the amount of the previous correction. This is the updated correction amount.

[0070] Step 6-2: Obtain the corrected INS position based on the updated correction amount.

[0071] in, This is the original Instagram location. This is for the corrected INS position.

[0072] This application also provides an inertial navigation system error correction system that integrates template matching and hidden Markov models. Please refer to [reference needed]. Figure 8 The system includes: The map preprocessing module is used to construct an R-tree index structure from the raw map data; The adaptive keyframe selection module is used to select keyframes of INS points using an adaptive threshold method and filter out redundant INS points. The curve detection module is used to detect curve features in keyframes of the INS trajectory using a sliding window mechanism. The template matching module is used to create template images based on INS trajectories, generate road network images using road segment information in the R-tree index structure, and perform template matching processing on the road network images using sliding search. The optimization module is used to optimize the matching process using the HMM algorithm when template matching generates multiple candidate correction values; the optimization module is a Hidden Markov Model optimization module. The correction module updates the corrected INS position by recursively updating the correction value obtained after the matching process optimization. In this embodiment, the usage process and beneficial effects of the system are the same as the steps and beneficial effects of the aforementioned method, and will not be repeated here.

[0073] The following specific application examples further illustrate the error correction method for inertial navigation systems based on the fusion of template matching and hidden Markov models proposed in this application.

[0074] Example 1 I. Preparatory Work Hardware requirements: Use a computer equipped with an NVIDIA RTX 2060 or higher GPU.

[0075] Software requirements: Conda must be installed, version 3.8.0 or higher, and libraries such as numpy, random, and os must be installed.

[0076] II. Operating Procedures Step 1: System Initialization See Figure 1 The map preprocessing module is started, the pre-built R-tree index file is loaded, and the correction vector of the corrector module is initialized to zero.

[0077] Step 2: INS Data Reception The system receives INS data input in real time, including position coordinates (x, y), heading angle θ, and timestamp t.

[0078] Step 3: Adaptive Keyframe Selection The adaptive keyframe selection module performs the following sub-steps: First, the INS points are corrected, then a suitable distance threshold is selected through keyframes, and adaptive adjustments are made based on the distance threshold.

[0079] Step 4: Curve Inspection Connect the start and end points of the window, and calculate the projection distance from the midpoint of the window to the connecting line. If the conditions are met, it is identified as a curve; otherwise, continue to judge the projection distance from the 1 / 4 and 3 / 4 points in the window to the connecting line.

[0080] Step 5: Template Matching Processing When a curve is detected, the template matching module performs the following steps: Create a trajectory shape image and a heading angle image based on the trajectory range; Expand the range by 100 meters and use an R-tree index to retrieve candidate road segments; Create road network shape images and heading angle images; Slide a trajectory template on the road network image and calculate the matching score matrix; Cluster the high-scoring regions and calculate the candidate set of correction values. .

[0081] Step Six: HMM Optimization Selection If multiple candidate correction values ​​exist, the Hidden Markov Model optimization module performs the following: Constructing the observation sequence: Applying each candidate correction to the trajectory to obtain... ; Calculate the probability of observation: ; Calculate the state transition probability: ; Choose the correction amount with the highest joint probability: .

[0082] Step 7: Update Calibration Values The calibration module updates the calibration amount: Perform a recursive update: ; Application correction amount: Output the corrected positioning result.

[0083] Example 2 This embodiment includes a map preprocessing module, an adaptive keyframe selection module, a curve detection module, a template matching module, an optimization module, and a correction module.

[0084] The map preprocessing module includes a map data acquisition unit, a data filtering unit, and an R-tree construction unit. The map data acquisition unit downloads OpenStreetMap map data based on administrative city area divisions; the data filtering unit removes non-motorized vehicle roads such as waterways, railways, and sidewalks, retaining only valid road segment information; the R-tree construction unit calculates the minimum bounding rectangle based on the road segment endpoint coordinates, inserts attributes such as the number of lanes as leaf nodes into the R-tree, and constructs a multi-level tree-like index structure.

[0085] II. The adaptive keyframe selection module receives INS data input and includes a distance calculation unit, a heading angle analysis unit, and a threshold adjustment unit. The distance calculation unit calculates the distance between the current INS point and the previous keyframe; the heading angle analysis unit calculates the heading angle change Δθ; and the threshold adjustment unit is used to adaptively adjust the distance threshold according to the heading angle change.

[0086] III. The curve detection module includes a sliding window unit, a heading angle detection unit, and a projection distance calculation unit. The sliding window unit maintains a keyframe queue of fixed length; the heading angle detection unit calculates the change in heading angle within the window, and when Δθ > 30 degrees, it initially determines that a curve exists; the projection distance calculation unit calculates the projection distance from the midpoint and the 1 / 4 and 3 / 4 points to the starting and ending points, and when the projection distance > 3 meters, it confirms that a curve exists.

[0087] IV. The template matching module includes an image creation unit, a matching calculation unit, and a clustering analysis unit. The image creation unit creates a trajectory shape image and a trajectory heading angle image based on the trajectory coverage area, with a pixel resolution of 1 meter and a trajectory line width of 2 meters; it also creates a road network shape image and a road network heading angle image, extending the road network coverage outwards by 100 meters, with a road segment line width of lanes × 3 meters. The matching calculation unit calculates the shape matching score through a sliding search. Matching score with heading angle Total score The clustering analysis unit uses a depth-first search algorithm to cluster high-scoring regions and calculates the centroid as a correction factor.

[0088] V. Optimization Module (Hidden Markov Model Optimization Module) includes an observation sequence construction unit, a probability calculation unit, and an optimal selection unit. The observation sequence construction unit applies candidate corrections to the trajectory to obtain new observation sequences; the probability calculation unit calculates the observation probabilities. and state transition probability The optimal selection unit is determined by joint probability. Choose the optimal correction amount The larger the value of the parameter, the greater the tolerance to non-connecting pathway segments.

[0089] VI. The calibration module includes a calibration update unit and a position calibration unit. The calibration update unit uses a recursive formula... Update the correction amount; the position correction unit applies the correction amount to the original INS position to obtain the corrected positioning output.

[0090] Each module runs in a multi-threaded manner. The R-tree index of the map preprocessing module provides road segment information for subsequent modules. The adaptive keyframe selection module outputs keyframes to the curve detection module. The curve detection module triggers the template matching module. The multiple candidate results of the template matching module are input into the hidden Markov model optimization module. Finally, the correction amount is passed to the corrector module to realize error correction.

[0091] Parameter Configuration: The map data used in this embodiment is in OpenStreetMap format with a map accuracy of 1 meter. The pixel resolution is set to 1 meter / pixel, and the maximum image size is 1024×1024 pixels. The distance threshold range for adaptive keyframe selection is 5-20 meters, and the normalized heading angle range is 0-180 degrees. The sliding window length for curve detection is 10 keyframes, the heading angle change threshold is 30 degrees, and the projection distance threshold is 3 meters. Template matching uses a normalized cross-correlation algorithm with a search step size of 1 pixel. The observation probability standard deviation σ of the HMM model is 5 meters, the state transition parameter β is 100, and the distance adjustment factor γ is 50. The recursive update weight α of the corrector is 0.3. The hardware configuration is an Intel i7-10700K CPU with 32GB of memory, and the average processing time of the algorithm is less than 100 milliseconds.

[0092] Operation procedure: The correction method of Example 1 is adopted, including the following steps: ① When the user starts the INS error correction system, the map preprocessing module automatically loads the R-tree index file of the specified area.

[0093] ② The adaptive keyframe selection module receives real-time INS data input and filters keyframes based on changes in distance and heading angle.

[0094] ③ The curve detection module performs sliding window analysis on the keyframe sequence to determine whether curve features exist.

[0095] ④ When a curve is detected, the template matching module performs image matching processing. The specific workflow of the template matching module is as follows: I. Create a trajectory shape image and a heading angle image based on the trajectory range; II. Use R-tree indexing to retrieve candidate road segments and create road network shape image and heading angle image; III. Calculate the matching score matrix through sliding search and perform cluster analysis on high-scoring regions.

[0096] ⑤ If there are multiple matching results, the Hidden Markov Model optimization module selects the optimal correction amount through probability calculation.

[0097] ⑥ The calibrator module receives the calibration value, updates and outputs the calibration result through a recursive method.

[0098] Analysis of experimental results: (1) Traditional INS error correction methods typically focus on correcting lateral errors. However, this system achieves simultaneous correction of both lateral and longitudinal errors, such as... Figure 9The comparison between the blue and red trajectories and the actual road shows that the corrected trajectory (blue line in the figure) more closely matches the actual path, especially at complex intersections or curves, demonstrating a more comprehensive correction effect, making the positioning results more accurate and stable.

[0099] (2) For example Figure 10 As shown, without external assistance, the cumulative INS error over a 200km range can be significantly improved from over 50 meters to an average error of less than 15 meters. This improvement in accuracy is crucial for military applications requiring high-reliability positioning, ensuring precise vehicle navigation in complex environments.

[0100] (3) It is applicable to large-scale positioning offset scenarios, expands the correction range of INS cumulative error, and improves the reliability of the system during long-term and long-distance driving.

[0101] (4) It reduces the system hardware cost and eliminates the need for additional GPS, lidar and other sensor equipment, providing an economical and effective solution for high-precision navigation of military vehicles.

[0102] In summary, the present invention achieves the following technical effects: (1) This invention achieves high-precision INS error correction using existing map information without relying on external sensors. By constructing road network images and using template matching technology, high-precision INS error correction can be achieved using only map data; (2) This invention considers both the overall and local characteristics of the trajectory, thus improving the accuracy of map matching. By combining template matching with a hidden Markov model, it utilizes template matching to capture the overall shape features of the trajectory while also considering local characteristics such as road segment connectivity through the HMM; (3) This invention achieves effective correction in scenarios with large-scale positioning offsets through precise matching of inertial navigation and maps. By performing horizontal and vertical sliding searches in the road network image, it can handle positioning offsets over a large range; (4) This invention simultaneously corrects the lateral and longitudinal errors of the INS, achieving comprehensive error correction. By performing lateral and longitudinal searches simultaneously in the two-dimensional road network image through template matching, it is possible to simultaneously acquire and correct lateral and longitudinal errors; (5) This invention improves the computational efficiency of the algorithm and meets the real-time requirements. By using adaptive keyframe selection and curve detection based on sliding windows, matching is performed only on key trajectory segments, which greatly reduces the computational complexity.

[0103] It should be noted that although several modules of the system for executing actions are mentioned in the detailed description above, this division is not mandatory. In fact, according to embodiments of the present invention, the features and functions of two or more modules described above can be embodied in one module. Conversely, the features and functions of one module described above can be further divided into multiple modules for embodiment. Components shown as modules may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of the present invention according to actual needs. Those skilled in the art can understand and implement this without any inventive effort.

[0104] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of the present invention without departing from the spirit and scope of the claims. All of these forms are within the protection scope of the present invention.

Claims

1. A method for error correction in inertial navigation systems that integrates template matching and hidden Markov models, characterized in that, Includes the following steps: Step 1: Construct the original map data into an R-tree index structure; Step 2: Select keyframes for INS points using an adaptive threshold method and filter out redundant INS points; Step 3: Use a sliding window mechanism to detect the curve features of keyframes in the INS trajectory; Step 4: Create a template image based on the INS trajectory, generate a road network image using the road segment information in the R-tree index structure, and perform template matching processing on the road network image using sliding search. Step 5: When template matching generates multiple candidate correction values, the matching process is optimized using the HMM algorithm; Step 6: Update the correction value obtained after the matching process optimization using a recursive method to obtain the corrected INS position.

2. The inertial navigation system error correction method based on template matching and hidden Markov model as described in claim 1, characterized in that, Step 1 includes the following specific steps: Step 1-1: Based on the administrative city area division, download the OpenStreetMap map data for the whole country; Steps 1-2: Remove roads used by non-motorized vehicles from the map data; Steps 1-3: Calculate the minimum bounding rectangle (MBR) of the road segment based on the coordinates of the road segment endpoints, and insert the lane-attached attributes and the MBR together as leaf nodes into the R-tree. Steps 1-4: Based on spatial proximity, nodes that are close to each other are aggregated level by level to form parent nodes, thus constructing a multi-level R-tree index structure.

3. The inertial navigation system error correction method based on template matching and hidden Markov model as described in claim 2, characterized in that, Step 2 includes the following specific steps: Step 2-1: Shift the INS point position using the correction amount to obtain the corrected INS point position; Step 2-2: When the distance between the current frame and the previous keyframe is greater than or equal to the distance threshold, the current frame is selected as the keyframe. Steps 2-3: The distance threshold is adaptively adjusted according to the change in heading angle.

4. The inertial navigation system error correction method based on template matching and hidden Markov model as described in claim 3, characterized in that, Step 3 includes the following specific steps: Step 3-1: Set the keyframe sequence of the INS trajectory within the sliding window and calculate the change in the heading angle of the current point relative to the starting point of the window. Step 3-2: When the change in the heading angle of the key frame is greater than the preset heading angle change threshold, it is preliminarily determined that there may be a curve. Step 3-3: Connect the start and end points of the window and calculate the projection distance from the midpoint of the window to the connecting line. Steps 3-4: When the projected distance is greater than the preset heading angle change threshold, it is finally identified as a curve; otherwise, continue to judge the projected distance from the 1 / 4 and 3 / 4 points in the window to the connecting line. Steps 3-5: If the window is ultimately determined to have a curve, perform subsequent map matching on the keyframe sequence within the window.

5. The inertial navigation system error correction method based on the fusion of template matching and hidden Markov model as described in claim 4, characterized in that, Step 4 includes the following specific steps: Step 4-1: Based on the INS trajectory keyframe sequence, INS trajectory endpoint coordinates, and pixel values ​​of the corresponding INS trajectory region, create a trajectory shape image and a trajectory heading angle image; Step 4-2: Create a road network image based on the INS trajectory and candidate road segment information within the R-tree index range; Step 4-3: Using the trajectory shape image and the trajectory heading angle image as templates, slide them on the road network image to calculate the shape matching score and heading angle matching score. Step 4-4: Cluster the high-matching score points, calculate the centroid coordinates for each point in the cluster, and use the centroid coordinates as the correction value.

6. The inertial navigation system error correction method based on the fusion of template matching and hidden Markov model as described in claim 5, characterized in that, Step 5 includes the following specific steps: Step 5-1: Perform translation correction on the correction amount to obtain a new observation sequence; Step 5-2: Calculate the observation probability and state transition probability of the initial node in the new observation sequence, find the hidden state sequence with the highest probability, and obtain the corresponding matching probability. Step 5-3: Calculate the distance probability of the correction amount, and determine the correction amount that maximizes the joint probability of the distance probability and the state transition probability of the distance probability matching the correction amount road segment.

7. The inertial navigation system error correction method based on template matching and hidden Markov model as described in claim 6, characterized in that, Step 6 includes the following specific steps: Step 6-1: Update the calibration values; Step 6-2: Obtain the corrected INS position based on the updated correction amount.

8. An error correction system for an inertial navigation system that integrates template matching and hidden Markov models, characterized in that, include: The map preprocessing module is used to construct an R-tree index structure from the raw map data; The adaptive keyframe selection module is used to select keyframes of INS points using an adaptive threshold method and filter out redundant INS points. The curve detection module is used to detect curve features in keyframes of the INS trajectory using a sliding window mechanism. The template matching module is used to create template images based on INS trajectories, generate road network images using road segment information in the R-tree index structure, and perform template matching processing on the road network images using sliding search. The optimization module is used to optimize the matching process using the HMM algorithm when template matching generates multiple candidate correction values. The correction module is used to update the correction value obtained after the matching process is optimized through a recursive method to obtain the corrected INS position.

Images