Visual inertia SLAM system pose guiding method and system

Abstract

The invention discloses a visual inertia SLAM system pose guiding method and system, and relates to the technical field of data processing. The method comprises a first data acquisition step, a first calculation step, a preliminary matching step, a second calculation step, a judgment step, a first feedback step, a first data processing step, a second data processing step, a third calculation step, a second data acquisition step, a pose optimization step and the like. According to the method, the precision and robustness of the visual inertia SLAM in a dynamic environment can be improved, the visual inertia SLAM is not easily influenced by speed change in a dynamic scene, the probability of local matching failure is reduced, and the precision of pose guidance is further improved.

Description

Technical Field

[0001] The present application relates to the technical field of data processing, and in particular to a posture guidance method and system for a visual-inertial SLAM system. Background Art

[0002] Visual-inertial SLAM (Simultaneous Localization and Mapping, SLAM for short) is a technology that enables robots / drones and other devices to achieve autonomous positioning and environmental modeling through camera vision and inertial sensors. It can build maps in real time in unknown environments and track its own motion trajectory. Manhattan Frame (MF for short): A local coordinate system that conforms to the Manhattan world hypothesis. Each MF corresponds to a set of orthogonal directions in the environment. By identifying the spatial relationship between different MFs, the structured constraints of the environment are established to assist the SLAM system in positioning.

[0003] In indoor environments such as hospital corridors or office buildings, the environmental structure usually has strong geometric regularity, such as orthogonal walls, linear wall structures, etc. Traditional SLAM algorithms rely solely on natural feature points for MF matching and orientation positioning, which is prone to failure in scenes with low texture or repeated structures. In the prior art, the continuous poses predicted by the SLAM system are usually used to verify the rationality of the MF direction changes in adjacent time windows, and then perform pose guidance.

[0004] At present, a Chinese patent with publication number CN113345087A proposes a surface model fitting method and device based on monocular vision, which determines the road condition and road parameter values ​​according to the coordinate values ​​of the vanishing point of a straight road in the coordinate system and parameters such as the internal and external parameter matrix of the camera, as well as the coordinate values ​​of the road turning point and the road vanishing point in the pixel coordinate system, estimates the surface model, and then adjusts the operating status of the equipment.

[0005] Regarding the above technical solution, this method uses dense feature matching parameters such as coordinate values, which is easily affected by speed changes in dynamic scenes, resulting in local matching failure, poor matching stability between historical motion trends and current observation results, and thus reducing the accuracy of posture guidance. Summary of the invention

[0006] Based on this, it is necessary to provide a visual-inertial SLAM system posture guidance method and system to address the above-mentioned technical problems. The present application improves the accuracy and robustness of visual-inertial SLAM in dynamic environments, is not easily affected by speed changes in dynamic scenes, reduces the probability of local matching failure, and thereby improves the accuracy of posture guidance.

[0007] In the first aspect, the present application provides a visual inertial SLAM system posture guidance method, which uses the following technical solutions to achieve the application purpose: The visual inertial SLAM system posture guidance method comprises the following steps: The first step of acquiring data: acquiring the pose data of the historical frame, which includes the translation vector U and the rotation matrix R, and acquiring the feature point set to be matched in the current frame .

[0008] The first calculation step: Calculate the set of feature points to be matched in the current frame based on the known changes in posture between adjacent frames in the historical frame The predicted projected position point set in the next frame , The calculation model is: .

[0009] Preliminary matching step: Get the actual projection position point set of the feature point set to be matched in the next frame ,Will and Perform geometric verification matching, verify whether the matching pair results satisfy the epipolar constraints through the essential matrix, eliminate the matching pairs that do not satisfy the epipolar constraints, and obtain the remaining matching pairs.

[0010] The second calculation step: Calculate the actual projection position point set of the feature point set to be matched in the next frame among the remaining matching pairs and the predicted projection position point set The trajectory deviation , The calculation model is: ,in, represents the Euclidean distance.

[0011] Judgment step: Get the preset trajectory deviation threshold based on the feature point set in the historical frame's posture data ,like , then the remaining matching pairs obtained are judged to be unreliable, and the first feedback step is executed. , then the remaining matching pairs obtained are judged to be reliable, and the first data processing step is executed.

[0012] The first feedback step: sending the remaining matching pairs as unreliable data to the management end.

[0013] The first data processing step: divide the feature point sets of T consecutive frames including the current frame into N groups of time series feature point sets according to t frames as a group, and divide each frame into M spatial sequence feature point sets according to the image block area, and record the total set of time and space feature points after division for: ,in is the mth spatiotemporal feature point set of the nth group.

[0014] The second data processing step: the actual projection position point set of the feature point set to be matched in the next frame Divide the image into M spatial sequence feature point sets according to the image block area, and record the total set of feature points to be matched after division for: ,in is the mth feature point set to be matched.

[0015] The third calculation step: Calculate the spatiotemporal matching correlation , The calculation model is: , where t is a fixed frame value.

[0016] The second step of obtaining data is to obtain the average value of the historical frame translation change corresponding to the nth group of time series feature points in T consecutive frames. , the mean of the rotation matrix is .

[0017] Pose optimization step: Calculate the correction coefficient , The calculation model is: ,in, is the total correlation, according to the correction coefficient Calculate the translation vector of the current frame after optimization for: , the rotation matrix of the current frame after optimization for: , and update the translation vector and rotation matrix of the current frame.

[0018] By adopting the above technical solution, the historical pose data containing the translation vector U and the rotation matrix R in the historical frame and the feature point set to be matched in the current frame are obtained. , calculate the predicted projection position point set of the feature point set to be matched in the current frame in the next frame ,Will The actual projection position point set Perform geometric verification matching, eliminate interference from mismatched pairs, and calculate and The trajectory deviation ,like , the match is judged to be unreliable and the data is sent to the management end. , then the match is determined to be reliable, and the feature point sets of T consecutive frames including the current frame are divided into N groups of time series feature point sets according to t frames as a group, and are divided into M groups of space series feature point sets according to the image block area. The actual projection position point set of the feature point set to be matched in the next frame is According to the image block area, it is divided into M spatial sequence feature point sets, and the spatiotemporal matching correlation is calculated. ,according to Calculate the correction factor , through the correction factor Calculate the translation vector of the current frame after optimization and the rotation matrix , and update the translation vector and rotation matrix of the current frame; in this way, the translation vector and rotation matrix in the pose data of the current frame are corrected and updated through the correction coefficient, so that in the feature point concentration of T consecutive frames, the spatiotemporal matching correlation corresponding to the static environment area is higher, which can suppress the noise existing in the current frame to a certain extent, improve the dependence of the pose data on the pose data of the historical frame, and the spatiotemporal matching correlation corresponding to the dynamic environment area is lower, thereby reducing the dependence of the pose data on the pose data of the historical frame to a certain extent, and improving the trust in the current observation. Through periodic observation and local statistical analysis, the accuracy and robustness of visual inertial SLAM in dynamic environments can be significantly improved. It is not easily affected by speed changes in dynamic scenes, reduces the probability of local matching failure, and thus improves the accuracy of pose guidance.

[0019] As a preferred implementation of the visual-inertial SLAM system posture guidance method provided in the present application, in the posture optimization step, the calculation model of the correction coefficient γ is updated as follows: ,in, is the dynamic coefficient, .

[0020] As a preferred implementation of the visual inertial SLAM system posture guidance method provided in this application, the third calculation step also includes calculating the spatial matching correlation degree. , The calculation model is: .

[0021] In the pose optimization step, the correction coefficient The calculation model is updated as follows: .

[0022] As a preferred implementation of the visual-inertial SLAM system posture guidance method provided in the present application, an adjustment step is also included between the judgment step and the first feedback step.

[0023] Adjustment steps: Get the acceleration variance of the inertial sensor and gyro noise variance , the adjustment threshold of the calculated trajectory error is ,like , then the adjustment is judged to be reliable, and the trajectory error threshold is updated to the adjustment threshold. , then the adjustment is judged to be unreliable and the first feedback step is executed.

[0024] As a preferred implementation of the visual-inertial SLAM system posture guidance method provided in the present application, a second feedback step is also included after the adjustment step. When the adjustment is judged to be reliable in the adjustment step, the second feedback step is executed.

[0025] Second feedback step: Calculate the actual offset of the feature point set to be matched in the next frame , , record the sum of the actual offsets of the feature point set to be matched within the specified number of consecutive frames and record it as D, and obtain the offset threshold D according to the offset of the feature point set in the pose data of the historical frame max , if D>D max , then reinitialize the trajectory error threshold.

[0026] In the second aspect, this application provides a visual inertial SLAM system posture guidance system, which adopts the following technical solutions to achieve the application purpose: The visual inertial SLAM system posture guidance system includes the following modules: Acquisition module I: The output end is connected to the input end of the calculation module I, and is used to obtain the pose data of the historical frame. The pose data of the historical frame includes the translation vector U and the rotation matrix R. The set of feature points to be matched in the current frame is obtained as .

[0027] Calculation module I: The input end is connected to the output end of acquisition module I, and the output end is connected to the input end of matching module, which is used to calculate the predicted projection position point set of the feature point set to be matched in the next frame according to the posture change between adjacent frames in the specified historical frame: .

[0028] Matching module: The input end is connected to the output end of the calculation module I, and the output end is connected to the input end of the calculation module II, which is used to obtain the actual projection position point set of the feature point set to be matched in the next frame. ,Will and Perform geometric verification matching, verify whether the matching pair results satisfy the epipolar constraints through the essential matrix, eliminate the matching pairs that do not satisfy the epipolar constraints, and obtain the remaining matching pairs.

[0029] Calculation module II: The input end is connected to the output end of the matching module, and the output end is connected to the input end of the judgment module, which is used to calculate the actual projection position point set of the feature point set to be matched in the next frame among the remaining matching pairs. and the predicted projection position point set The trajectory deviation .

[0030] Judgment module: The input end is connected to the output end of the calculation module II, and the output end is connected to the input end of the feedback module I and the input end of the processing module I respectively, which is used to obtain the preset trajectory deviation threshold according to the feature point set in the posture data of the historical frame ,like , then the remaining matching pairs obtained are judged to be unreliable, and feedback module I is executed. , then the remaining matching pairs obtained are judged to be reliable, and processing module I is executed.

[0031] Feedback module I: the input end is connected to the output end of the judgment module, and is used to send the data of the remaining matching pairs as unreliable to the management end.

[0032] Processing module I: The input end is connected to the output end of the judgment module, and the output end is connected to the input end of the processing module II, which is used to divide the feature point sets of T consecutive frames including the current frame into N groups of time series feature point sets according to t frames as a group, and at the same time divide them into M spatial sequence feature point sets according to the image block area, and record the total set of space-time feature points after division as K.

[0033] Processing module II: The input end is connected to the output end of processing module I, and the output end is connected to the input end of calculation module III, which is used to convert the actual projection position point set of the feature point set to be matched in the next frame into the actual projection position point set of the feature point set to be matched in the next frame. The image is divided into M spatial sequence feature point sets according to the block area, and the total set of feature points to be matched after the division is recorded as L.

[0034] Calculation module III: The input end is connected to the output end of processing module II, and the output end is connected to the input end of acquisition module II. It is used to calculate the root mean square error between the N sets of time series feature point sets and the M sets of space series feature point sets in the total set K, and record it as the temporal and spatial matching association degree. .

[0035] Acquisition module II: The input end is connected to the output end of the calculation module III, and the output end is connected to the input end of the correction module, which is used to obtain the mean value of the historical frame translation change corresponding to the nth group of time series feature points in T consecutive frames. , the mean of the rotation matrix is .

[0036] Correction module: The input end is connected to the output end of the acquisition module II to calculate the correction coefficient , The calculation model is: ,in, is the total correlation, according to the correction coefficient Calculate the translation vector of the current frame after optimization for , the rotation matrix of the current frame after optimization is , and update the translation vector and rotation matrix of the current frame.

[0037] By adopting the above technical solution, the acquisition module I obtains the historical pose data containing the translation vector U and the rotation matrix R in the historical frame and the feature point set to be matched in the current frame. , the calculation module I calculates the predicted projection position point set of the feature point set to be matched in the current frame in the next frame , the matching module will The actual projection position point set Perform geometric verification matching to eliminate interference from mismatched pairs, and calculate module II and The trajectory deviation ,like , the judgment module determines that the match is unreliable, and the feedback module I sends the data to the management end. , the judgment module determines that the match is reliable, and the processing module I divides the feature point sets of T consecutive frames including the current frame into N groups of time series feature point sets according to t frames as a group, and divides them into M groups of space series feature point sets according to the image block area. The processing module II calculates the actual projection position point set of the feature point set to be matched in the next frame. According to the image block area, it is divided into M spatial sequence feature point sets, and the calculation module III calculates the spatiotemporal matching correlation degree. ,according to Calculate the correction factor Module II obtains the mean value of the translation change of the historical frame and the mean rotation matrix The correction module calculates the translation vector of the current frame after optimization through the correction coefficient γ and the rotation matrix , and update the translation vector and rotation matrix of the current frame; in this way, the translation vector and rotation matrix in the pose data of the current frame are corrected and updated through the correction coefficient, so that in the feature point concentration of T consecutive frames, the spatiotemporal matching correlation corresponding to the static environment area is higher, which can suppress the noise existing in the current frame to a certain extent, improve the dependence of the pose data on the pose data of the historical frame, and the spatiotemporal matching correlation corresponding to the dynamic environment area is lower, thereby reducing the dependence of the pose data on the pose data of the historical frame to a certain extent, and improving the trust in the current observation. Through periodic observation and local statistical analysis, the accuracy and robustness of visual inertial SLAM in dynamic environments can be significantly improved. It is not easily affected by speed changes in dynamic scenes, reduces the probability of local matching failure, and thus improves the accuracy of pose guidance.

[0038] As a preferred embodiment of the visual inertial SLAM system posture guidance system provided in this application, in the correction module, the correction coefficient The calculation model is updated as follows: ,in, is the dynamic coefficient, .

[0039] As a preferred implementation of the visual inertial SLAM system posture guidance system provided in the present application, in the calculation module III, the root mean square error between the M spatial sequence feature point sets in the total set L is also calculated and recorded as the spatial matching association degree .

[0040] In the correction module, the correction coefficient The calculation model is updated as follows: .

[0041] As a preferred embodiment of the visual inertial SLAM system posture guidance system provided in the present application, it also includes an adjustment module: the input end is connected to the output end of the judgment module, and the output end is respectively connected to the input end of the feedback module I and the input end of the judgment module, for obtaining the acceleration variance of the inertial sensor and gyro noise variance , the adjustment threshold of the calculated trajectory error is ,like , then the adjustment is judged to be reliable, and the trajectory error threshold is updated to the adjustment threshold. , then the adjustment is judged to be unreliable and feedback module I is executed.

[0042] As a preferred implementation of the visual-inertial SLAM system posture guidance system provided in the present application, it also includes a feedback module II. When the adjustment module determines that the adjustment is reliable, the feedback module II is also executed.

[0043] Feedback module II: The input end is connected to the output end of the adjustment module, and the output end is connected to the input end of the judgment module, which is used to calculate the actual offset of the feature point set to be matched in the next frame , , record the sum of the actual offsets of the feature point set to be matched within the specified number of consecutive frames and record it as D, obtain the offset threshold according to the offset of the feature point set in the pose data of the historical frame, if D>D max , then reinitialize the trajectory error threshold.

[0044] Compared with the prior art, this application has the following beneficial effects: 1. The translation vector and rotation matrix in the pose data of the current frame are corrected and updated through the correction coefficient, so that in the feature point concentration of T consecutive frames, the temporal and spatial matching correlation corresponding to the static environment area is higher, which can suppress the noise in the current frame to a certain extent, and improve the dependence of the pose data of the current frame on the pose data of the historical frame. The temporal and spatial matching correlation corresponding to the dynamic environment area is lower, thereby reducing the dependence of the pose data of the current frame on the pose data of the historical frame to a certain extent, and improving the degree of trust in the current observation. Through periodic observation and local statistical analysis, the accuracy and robustness of visual inertial SLAM in dynamic environments can be significantly improved. It is not easily affected by speed changes in dynamic scenes, reducing the probability of local matching failure, and thus improving the accuracy of pose guidance.

[0045] 2. By optimizing the calculation model of the correction coefficient through the dynamic coefficient, the system can timely adjust the weight of the spatiotemporal matching correlation in different usage scenarios. For example, reducing the weight of the spatiotemporal matching correlation in dynamic scenes or unstructured scenes can reduce the interference of unstable spatiotemporal characteristics on the correction coefficient, and increasing the weight of the spatiotemporal matching correlation in static scenes or structured scenes, thereby further enhancing the system's robustness to dynamic interference, which can improve the system's scope of application and reliability, further reduce the probability of local matching failure, and improve the accuracy of posture guidance.

[0046] 3. The correction coefficient is calculated jointly by the spatiotemporal matching correlation and the spatial matching correlation, so that when there is dynamic interference in some weak texture areas in unstructured scenes or complex buildings, the device can quickly detect local anomalies and reduce the spatial matching correlation, thereby improving the dependence of the current frame's pose data on the global motion trend in the historical frame's pose data to a certain extent. When there is dynamic interference in some texture-rich areas, it is more inclined to improve the utilization rate of local structural information in the pose data, thereby improving the reliability in unstructured scenes such as tunnels, increasing the scope of application of the system, improving the accuracy and robustness of the visual inertial SLAM system in dynamic environments, reducing the probability of local matching failure, and thus improving the accuracy of pose guidance.

[0047] 4. When the match is judged to be unreliable, the adjustment threshold of the trajectory error is calculated by obtaining the acceleration variance and gyroscope noise variance of the inertial sensor, and further judging whether the adjustment is reliable. When the adjustment is judged to be reliable, the trajectory error threshold is updated to the adjustment threshold. In this way, the acceleration variance and gyroscope noise variance of the inertial sensor are introduced to dynamically adjust the threshold of the trajectory error, thereby improving the robustness of the system in the case of sensor jitter or environmental vibration, thereby reducing the probability of misjudgment of the system in a dynamic environment, improving the reliability of dynamic judgment, and thus improving the accuracy of posture guidance.

[0048] 5. By calculating the actual offset of the feature point set to be matched in the next frame, the sum D of the actual offsets of the feature point set to be matched within the specified number of consecutive frames is counted, and the offset threshold is obtained according to the offset of the feature point set in the posture data of the historical frame. When D exceeds the offset threshold, the trajectory error threshold is reinitialized. This setting reflects the long-term drift trend of the system. When it exceeds the preset offset threshold, the trajectory deviation judgment standard of the matching model is reset to achieve regular error calibration, which is beneficial to prevent further amplification of errors in time when performing long-term tasks, improve the correction accuracy and correction efficiency of the system in long-term dynamic environments, reduce the probability of positioning failure, and thus improve the accuracy of posture guidance. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the scheme in the present application, a brief introduction is given below to the drawings required for use in the description of the embodiments. Obviously, the drawings described below are some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0050] Figure 1 This is a flow chart of Embodiment 1 of the present application; Figure 2 This is a system diagram of Example 2 of the present application. DETAILED DESCRIPTION

[0051] In order to make the technical personnel in this field better understand the present application scheme, the following will be combined with the present application Figure 1 and Figure 2 , the technical solutions in the embodiments of the present application are clearly and completely described. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present application.

[0052] Example 1: Please refer to Figure 1 , a visual inertial SLAM system posture guidance method, comprising the following steps: The first step of acquiring data: acquiring the pose data of the historical frame, which includes the translation vector U and the rotation matrix R, and acquiring the set of feature points to be matched in the current frame. .

[0053] The first calculation step: Calculate the set of feature points to be matched in the current frame based on the known changes in posture between adjacent frames in the historical frame The predicted projected position point set in the next frame , The calculation model is: .

[0054] Preliminary matching step: Get the actual projection position point set of the feature point set to be matched in the next frame ,Will and Perform geometric verification matching, verify whether the matching pair results satisfy the epipolar constraints through the essential matrix, eliminate the matching pairs that do not satisfy the epipolar constraints, and obtain the remaining matching pairs.

[0055] The second calculation step: Calculate the actual projection position point set of the feature point set to be matched in the next frame among the remaining matching pairs and the predicted projection position point set The trajectory deviation , The calculation model is: ,in, represents the Euclidean distance.

[0056] Judgment step: Get the preset trajectory deviation threshold based on the feature point set in the historical frame's posture data ,like , then the remaining matching pairs obtained are judged to be unreliable, and the first feedback step is executed. , then the remaining matching pairs obtained are judged to be reliable, and the first data processing step is executed.

[0057] Among them, the preset trajectory deviation threshold The feature point set in the posture data of the historical frame is obtained, including but not limited to its average data, peak data or normally distributed interval data.

[0058] Adjustment steps: Get the acceleration variance of the inertial sensor and gyro noise variance , the adjustment threshold of the calculated trajectory error is ,like , then the adjustment is judged to be reliable, the trajectory error threshold is updated to the adjustment threshold, and the second feedback step is executed. , then the adjustment is judged to be unreliable and the first feedback step is executed.

[0059] The first feedback step: sending the remaining matching pairs as unreliable data to the management end.

[0060] Second feedback step: Calculate the actual offset of the feature point set to be matched in the next frame , , record the sum of the actual offsets of the feature point set to be matched within the specified number of consecutive frames and record it as D, and obtain the offset threshold D according to the offset of the feature point set in the pose data of the historical frame max , if D>D max, then reinitialize the trajectory error threshold.

[0061] Among them, the offset threshold D max It is obtained based on the actual offset of the feature point set in the posture data of the historical frame, specifically including but not limited to its average data, peak data or normally distributed interval data.

[0062] The first data processing step: divide the feature point sets of T consecutive frames including the current frame into N groups of time series feature point sets according to t frames as a group, and divide each frame into M spatial sequence feature point sets according to the image block area, and record the total set of time and space feature points after division for: ,in is the mth spatiotemporal feature point set of the nth group.

[0063] The second data processing step: the actual projection position point set of the feature point set to be matched in the next frame Divide the image into M spatial sequence feature point sets according to the image block area, and record the total set of feature points to be matched after division for: ,in is the mth feature point set to be matched.

[0064] The third calculation step: Calculate the spatiotemporal matching correlation , The calculation model is: , where t is a fixed frame value, and the spatial matching correlation is calculated , The calculation model is: .

[0065] The second step of obtaining data is to obtain the average value of the historical frame translation change corresponding to the nth group of time series feature points in T consecutive frames. , the mean of the rotation matrix is .

[0066] Pose optimization step: Calculate the correction coefficient , The calculation model is: ,in, is the total correlation, is the dynamic coefficient, , according to the correction factor Calculate the translation vector of the current frame after optimization for: , the rotation matrix of the current frame after optimization for: , and update the translation vector and rotation matrix of the current frame.

[0067] The working principle of this embodiment is: Get the historical pose data containing the translation vector U and the rotation matrix R in the historical frame, as well as the set of feature points to be matched in the current frame , according to the pose changes between adjacent frames in the specified number of historical frames, calculate the predicted projection position point set of the feature point set to be matched in the current frame in the next frame , get the actual projection position point set of the feature point set to be matched in the next frame ,Will and Perform geometric verification matching and preliminarily verify whether the matching pair results satisfy the epipolar constraints through the essential matrix, thereby eliminating the interference of false matching pairs and utilizing the motion continuity between adjacent frames to effectively reduce the interference of dynamic object occlusion or noise in the environment on the matching process.

[0068] Calculate the actual projection position point set of the feature point set to be matched in the next frame and the predicted projection position point set The trajectory deviation , according to the trajectory deviation value between the historical feature point sets in the historical frame pose data, the preset trajectory deviation threshold is obtained ,like , then the match is judged to be unreliable, otherwise, the match is judged to be reliable.

[0069] When the match is judged to be unreliable, the acceleration variance of the inertial sensor is obtained and gyro noise variance , calculate the adjustment threshold of trajectory error , and further judge that if , the adjustment is judged to be reliable, and the trajectory error threshold is updated to the adjustment threshold. Otherwise, the adjustment is judged to be unreliable, and the data is sent to the management end. The acceleration variance and gyroscope noise variance of the inertial sensor are introduced to dynamically adjust the trajectory error threshold, thereby improving the robustness of the system in the case of sensor jitter or environmental vibration, thereby reducing the probability of misjudgment of the system in a dynamic environment, improving the reliability of dynamic judgment, and thus improving the accuracy of posture guidance.

[0070] When the adjustment is judged to be reliable, calculate the actual offset of the feature point set to be matched in the next frame , count the actual offsets of the feature point set to be matched within the specified number of consecutive frames and record it as D, and obtain the offset threshold D according to the offset of the feature point set in the pose data of the historical frame max , if D>D max, then the trajectory error threshold is reinitialized. With this setting, the cumulative value D reflects the long-term drift trend of the system. When it exceeds the preset offset threshold, the trajectory deviation judgment standard of the matching model is reset to achieve regular error calibration, which is beneficial to prevent further error amplification in time when running long-term tasks, improve the correction accuracy and correction efficiency of the system in a long-term dynamic environment, reduce the probability of positioning failure, and thus improve the accuracy of posture guidance.

[0071] When the match is judged to be reliable, the feature point sets of T consecutive frames including the current frame are divided into N groups of time series feature point sets according to t frames as a group, and are divided into M spatial series feature point sets according to the image block area. The total set of space-time feature points after division is recorded, and the m-th space-time feature point set of the n-th group is , the actual projection position point set of the feature point set to be matched in the next frame According to the image block area, it is divided into M spatial sequence feature point sets, and the total set of feature points to be matched after division is recorded. The mth feature point set to be matched is ,according to and Calculate the spatiotemporal matching correlation And spatial matching association .

[0072] Get the mean value of the historical frame translation change corresponding to the nth set of time series feature points in T consecutive frames and the mean rotation matrix , according to the dynamic coefficient Matching correlation between time and space and spatial matching association Weighted calculation of total correlation , and according to the total correlation Calculate the correction factor , through the correction factor The translation vector of the current frame after calculation and optimization is , the rotation matrix of the current frame after optimization is , and update the translation vector and rotation matrix of the current frame.

[0073] With such a setting, the translation vector and rotation matrix in the pose data of the current frame are corrected and updated through the correction coefficient, so that in the feature point concentration of T consecutive frames, the spatiotemporal matching correlation corresponding to the static environment area is higher, which can suppress the noise existing in the current frame to a certain extent, and improve the dependence of the pose data on the pose data of the historical frame. The spatiotemporal matching correlation corresponding to the dynamic environment area is lower, thereby reducing the dependence of the pose data on the pose data of the historical frame to a certain extent, and improving the degree of trust in the current observation. Through periodic observation and local statistical analysis, the accuracy and robustness of visual inertial SLAM in dynamic environments can be significantly improved. It is not easily affected by speed changes in dynamic scenes, reduces the probability of local matching failure, and thus improves the accuracy of pose guidance.

[0074] By jointly calculating the correction coefficient by using the spatiotemporal matching correlation and the spatial matching correlation, the device can quickly detect local anomalies and reduce the spatial matching correlation when there is dynamic interference in some weak-texture areas in unstructured scenes or complex buildings, thereby improving the dependence of pose data on the global motion trend in the pose data of historical frames to a certain extent. When there is dynamic interference in some texture-rich areas, it tends to improve the utilization rate of local structural information of pose data, thereby improving reliability in unstructured scenes such as tunnels, increasing the scope of application of the system, improving the accuracy and robustness of the visual-inertial SLAM system in dynamic environments, reducing the probability of local matching failure, and thereby improving the accuracy of pose guidance.

[0075] Through the dynamic coefficient Optimize overall relevance and correction factor The calculation model enables the system to timely adjust the weight ratio of spatiotemporal matching correlation and spatial matching correlation in different usage scenarios. For example, reducing the weight of spatiotemporal matching correlation in dynamic scenes or unstructured scenes can reduce the interference of unstable spatiotemporal characteristics on the correction coefficient, and increasing the weight of spatiotemporal matching correlation in static scenes or structured scenes, thereby further enhancing the system's robustness to dynamic interference, improving the system's scope of application and reliability, further reducing the probability of local matching failure, and improving the accuracy of posture guidance.

[0076] Example 2: Please refer to Figure 2 , the visual inertial SLAM system posture guidance system includes the following modules: Acquisition module I: The output end is connected to the input end of the calculation module I, and is used to obtain the pose data of the historical frame. The pose data of the historical frame includes the translation vector U and the rotation matrix R. The set of feature points to be matched in the current frame is obtained as .

[0077] Calculation module I: The input end is connected to the output end of acquisition module I, and the output end is connected to the input end of matching module, which is used to calculate the predicted projection position point set of the feature point set to be matched in the next frame according to the posture change between adjacent frames in the specified historical frame: .

[0078] Matching module: The input end is connected to the output end of the calculation module I, and the output end is connected to the input end of the calculation module II, which is used to obtain the actual projection position point set of the feature point set to be matched in the next frame. ,Will and Perform geometric verification matching, verify whether the matching pair results satisfy the epipolar constraints through the essential matrix, eliminate the matching pairs that do not satisfy the epipolar constraints, and obtain the remaining matching pairs.

[0079] Calculation module II: The input end is connected to the output end of the matching module, and the output end is connected to the input end of the judgment module, which is used to calculate the actual projection position point set of the feature point set to be matched in the next frame among the remaining matching pairs. and the predicted projection position point set The trajectory deviation .

[0080] Judgment module: The input end is connected to the output end of the calculation module II, the adjustment module and the feedback module II respectively, and the output end is connected to the input end of the adjustment module and the input end of the processing module I respectively, and is used to obtain the preset trajectory deviation threshold according to the feature point set in the posture data of the historical frame ,like , then the remaining matching pairs obtained are judged to be unreliable, and the adjustment module is executed. If , then the remaining matching pairs obtained are judged to be reliable, and processing module I is executed.

[0081] Adjustment module: The input end is connected to the output end of the judgment module, and the output end is connected to the input end of the feedback module I and the judgment module and feedback module II respectively, which is used to obtain the acceleration variance of the inertial sensor and gyro noise variance , the adjustment threshold of the calculated trajectory error is ,like , then the adjustment is judged to be reliable, the trajectory error threshold is updated to the adjustment threshold, and the feedback module II is executed. , then the adjustment is judged to be unreliable and feedback module I is executed.

[0082] Feedback module I: the input end is connected to the output end of the adjustment module, and is used to send the data of the remaining matching pairs that are unreliable to the management end.

[0083] Feedback module II: The input end is connected to the output end of the adjustment module, and the output end is connected to the input end of the judgment module, which is used to calculate the actual offset of the feature point set to be matched in the next frame , , record the sum of the actual offsets of the feature point set to be matched within the specified number of consecutive frames and record it as D, obtain the offset threshold according to the offset of the feature point set in the pose data of the historical frame, if D>D max , then reinitialize the trajectory error threshold.

[0084] Processing module I: The input end is connected to the output end of the judgment module, and the output end is connected to the input end of the processing module II, which is used to divide the feature point sets of T consecutive frames including the current frame into N groups of time series feature point sets according to t frames as a group, and at the same time divide them into M spatial sequence feature point sets according to the image block area, and record the total set of space-time feature points after division as K.

[0085] Processing module II: The input end is connected to the output end of processing module I, and the output end is connected to the input end of calculation module III, which is used to convert the actual projection position point set of the feature point set to be matched in the next frame into the actual projection position point set of the feature point set to be matched in the next frame. The image is divided into M spatial sequence feature point sets according to the block area, and the total set of feature points to be matched after the division is recorded as L.

[0086] Calculation module III: The input end is connected to the output end of processing module II, and the output end is connected to the input end of acquisition module II. It is used to calculate the root mean square error between the N sets of time series feature point sets and the M sets of space series feature point sets in the total set K, and record it as the temporal and spatial matching association degree. , calculate the root mean square error between the M spatial sequence feature point sets in the total set L, and record it as the spatial matching association degree .

[0087] Acquisition module II: The input end is connected to the output end of the calculation module III, and the output end is connected to the input end of the correction module, which is used to obtain the mean value of the historical frame translation change corresponding to the nth group of time series feature points in T consecutive frames. , the mean of the rotation matrix is .

[0088] Correction module: The input end is connected to the output end of the acquisition module II to calculate the correction coefficient , The calculation model is: ,in, is the total correlation, is the dynamic coefficient, , calculate the translation vector of the current frame after optimization according to the correction coefficient γ for , the rotation matrix of the current frame after optimization is , and update the translation vector and rotation matrix of the current frame.

[0089] The working principle of this embodiment is: Acquisition module I acquires the historical pose data including the translation vector U and the rotation matrix R in the historical frame, as well as the feature point set to be matched in the current frame. , the calculation module I calculates the predicted projection position point set of the feature point set to be matched in the current frame in the next frame according to the posture changes between adjacent frames in the historical frames of the specified number of frames , the matching module obtains the actual projection position point set of the feature point set to be matched in the next frame ,Will and Perform geometric verification matching and preliminarily verify whether the matching pair results satisfy the epipolar constraints through the essential matrix, thereby eliminating the interference of false matching pairs and utilizing the motion continuity between adjacent frames to effectively reduce the interference of dynamic object occlusion or noise in the environment on the matching process.

[0090] Calculation module II calculates the actual projection position point set of the feature point set to be matched in the next frame and the predicted projection position point set The trajectory deviation The judgment module obtains the preset trajectory deviation threshold according to the trajectory deviation value between the historical feature point sets in the pose data of the historical frame ,like , the judgment module determines that the match is unreliable, otherwise, the judgment module determines that the match is reliable.

[0091] When the judgment module determines that the match is unreliable, the adjustment module obtains the acceleration variance of the inertial sensor and gyro noise variance , calculate the adjustment threshold of trajectory error , and further judge that if , the adjustment module determines that the adjustment is reliable and updates the trajectory error threshold to the adjustment threshold. Otherwise, the adjustment module determines that the adjustment is unreliable. The feedback module I sends the data to the management end, and dynamically adjusts the trajectory error threshold by introducing the acceleration variance and gyroscope noise variance of the inertial sensor to improve the robustness of the system in the case of sensor jitter or environmental vibration, thereby reducing the probability of misjudgment of the system in a dynamic environment, improving the reliability of dynamic judgment, and thus improving the accuracy of posture guidance.

[0092] When the adjustment module determines that the adjustment is reliable, the feedback module II calculates the actual offset of the feature point set to be matched in the next frame. , count the actual offsets of the feature point set to be matched within the specified number of consecutive frames and record it as D, and obtain the offset threshold D according to the offset of the feature point set in the pose data of the historical frame max , if D>D max , then the trajectory error threshold in the judgment module is reinitialized. In this way, D reflects the long-term drift trend of the system. When it exceeds the preset offset threshold, the trajectory deviation judgment standard of the matching model is reset to achieve regular error calibration, which is beneficial to prevent the further amplification of the error in time when running long-term tasks, improve the correction accuracy and correction efficiency of the system in a long-term dynamic environment, reduce the probability of positioning failure, and thus improve the accuracy of posture guidance.

[0093] When the judgment module determines that the match is reliable, the processing module I divides the feature point sets of T consecutive frames including the current frame into N groups of time series feature point sets according to t frames as a group, and divides them into M groups of space series feature point sets according to the image block area, and records the total set of space-time feature points after division. The processing module II records the actual projection position point set of the feature point set to be matched in the next frame. According to the image block area, it is divided into M spatial sequence feature point sets, and the total set of feature points to be matched after the division is recorded. The calculation module III calculates the spatiotemporal matching correlation degree. And spatial matching association .

[0094] Acquisition module II obtains the mean value of the historical frame translation change corresponding to the nth group of time series feature points in T consecutive frames and the mean rotation matrix , the correction module is based on the dynamic coefficient Matching correlation between time and space and spatial matching association Weighted calculation of total correlation , and according to the total correlation Calculate the correction factor , the translation vector of the current frame after optimization is calculated by the correction coefficient γ and the rotation matrix , and update the translation vector and rotation matrix of the current frame.

[0095] With such a setting, the translation vector and rotation matrix in the pose data of the current frame are corrected and updated through the correction coefficient, so that in the feature point concentration of T consecutive frames, the spatiotemporal matching correlation corresponding to the static environment area is higher, which improves the dependence of the pose data on the pose data of the historical frames, and the spatiotemporal matching correlation corresponding to the dynamic environment area is lower, which improves the trust in the current observation. Through periodic observation and local statistical analysis, the accuracy and robustness of visual inertial SLAM in dynamic environments can be significantly improved. It is not easily affected by speed changes in dynamic scenes, which reduces the probability of local matching failure, thereby improving the accuracy of pose guidance.

[0096] The correction coefficient is calculated jointly by the spatiotemporal matching correlation and the spatial matching correlation. When there is dynamic interference in some weak-texture areas in unstructured scenes or complex buildings, the spatial matching correlation is reduced, thereby increasing the dependence of the pose data on the global motion trend in the pose data of the historical frames. When there is dynamic interference in some texture-rich areas, the utilization rate of the pose data on local structural information is increased, thereby improving the reliability in unstructured scenes such as tunnels, increasing the scope of application of the system, improving the accuracy and robustness of the visual-inertial SLAM system in dynamic environments, reducing the probability of local matching failure, and thereby improving the accuracy of pose guidance.

[0097] Through the dynamic coefficient Optimize overall relevance The calculation model of the correction coefficient γ enables the system to timely adjust the weight ratio of the spatiotemporal matching correlation and the spatial matching correlation in different usage scenarios, reduce the interference of unstable spatiotemporal characteristics on the correction coefficient, thereby further enhancing the system's robustness to dynamic interference, which can improve the system's scope of application and reliability, further reduce the probability of local matching failure, and improve the accuracy of posture guidance.

[0098] Obviously, the embodiments described above are only some embodiments of the present application, rather than all embodiments. The preferred embodiments of the present application are given in the accompanying drawings, but they do not limit the patent scope of the present application. The present application can be implemented in many different forms. On the contrary, the purpose of providing these embodiments is to make the understanding of the disclosure of the present application more thorough and comprehensive. Although the present application is described in detail with reference to the aforementioned embodiments, for those skilled in the art, it is still possible to modify the technical solutions recorded in the aforementioned specific implementation methods, or to perform equivalent replacement of some of the technical features therein. Any equivalent structure made using the contents of the specification and drawings of this application, directly or indirectly used in other related technical fields, is also within the scope of patent protection of this application.

Claims

1. A visual inertial SLAM system posture guidance method, characterized in that: The following steps are involved: The first step of acquiring data: acquiring the pose data of the historical frame, which includes the translation vector U and the rotation matrix R, and acquiring the feature point set to be matched in the current frame ; The first calculation step: Calculate the set of feature points to be matched in the current frame based on the known changes in posture between adjacent frames in the historical frame The predicted projected position point set in the next frame , The calculation model is: ; Preliminary matching step: Get the actual projection position point set of the feature point set to be matched in the next frame ,Will and Perform geometric verification matching, verify whether the matching pair results meet the epipolar constraints through the essential matrix, eliminate the matching pairs that do not meet the epipolar constraints, and obtain the remaining matching pairs; The second calculation step: Calculate the actual projection position point set of the feature point set to be matched in the next frame among the remaining matching pairs and the predicted projection position point set The trajectory deviation , The calculation model is: ,in, represents the Euclidean distance; Judgment step: Get the preset trajectory deviation threshold based on the feature point set in the pose data of the historical frame ,like , then the remaining matching pairs obtained are judged to be unreliable, and the first feedback step is executed. , then the remaining matching pairs obtained are judged to be reliable, and the first data processing step is executed; First feedback step: sending the remaining matching pairs as unreliable data to the management end; The first data processing step: divide the feature point sets of T consecutive frames including the current frame into N groups of time series feature point sets according to t frames as a group, and divide each frame into M spatial sequence feature point sets according to the image block area, and record the total set of time and space feature points after division for: ,in is the mth spatiotemporal feature point set of the nth group; The second data processing step: the actual projection position point set of the feature point set to be matched in the next frame Divide the image into M spatial sequence feature point sets according to the image block area, and record the total set of feature points to be matched after division for: ,in is the mth feature point set to be matched; The third calculation step: Calculate the spatiotemporal matching correlation , The calculation model is: , where t is a fixed frame value; The second step of obtaining data is to obtain the average value of the historical frame translation change corresponding to the nth group of time series feature points in T consecutive frames. , the mean of the rotation matrix is ; Pose optimization step: Calculate the correction coefficient , The calculation model is: ,in, is the total correlation, according to the correction coefficient Calculate the translation vector of the current frame after optimization for: , the rotation matrix of the current frame after optimization for: , and update the translation vector and rotation matrix of the current frame.

2. The visual-inertial SLAM system posture guidance method according to claim 1, wherein: In the pose optimization step, the correction coefficient The calculation model is updated as follows: ,in, is the dynamic coefficient, .

3. The visual-inertial SLAM system posture guidance method according to claim 2, wherein: The third calculation step also includes calculating the spatial matching correlation , The calculation model is: ; In the pose optimization step, the correction coefficient The calculation model is updated as follows: .

4. The visual-inertial SLAM system posture guidance method according to claim 1, characterized in that: An adjustment step is also included between the judgment step and the first feedback step; Adjustment steps: Get the acceleration variance of the inertial sensor and gyro noise variance , the adjustment threshold of the calculated trajectory error is ,like , then the adjustment is judged to be reliable, and the trajectory error threshold is updated to the adjustment threshold. , then the adjustment is judged to be unreliable and the first feedback step is executed.

5. The visual-inertial SLAM system posture guidance method according to claim 4, characterized in that: The method further includes a second feedback step after the adjustment step, and when the adjustment is determined to be reliable in the adjustment step, the second feedback step is executed; Second feedback step: Calculate the actual offset of the feature point set to be matched in the next frame , , record the sum of the actual offsets of the feature point set to be matched within the specified number of consecutive frames and record it as D, and obtain the offset threshold D according to the offset of the feature point set in the pose data of the historical frame max , if D>D max , then reinitialize the trajectory error threshold.

6. Visual inertial SLAM system posture guidance system, characterized by: Includes the following modules: Acquisition module I: The output end is connected to the input end of the calculation module I, and is used to obtain the pose data of the historical frame. The pose data of the historical frame includes the translation vector U and the rotation matrix R. The set of feature points to be matched in the current frame is obtained as ; Calculation module I: The input end is connected to the output end of acquisition module I, and the output end is connected to the input end of matching module, which is used to calculate the predicted projection position point set of the feature point set to be matched in the next frame according to the posture change between adjacent frames in the specified historical frame: ; Matching module: The input end is connected to the output end of calculation module I, and the output end is connected to the input end of calculation module II, which is used to obtain the actual projection position point set of the feature point set to be matched in the next frame ,Will and Perform geometric verification matching, verify whether the matching pair results meet the epipolar constraints through the essential matrix, eliminate the matching pairs that do not meet the epipolar constraints, and obtain the remaining matching pairs; Calculation module II: The input end is connected to the output end of the matching module, and the output end is connected to the input end of the judgment module, which is used to calculate the actual projection position point set of the feature point set to be matched in the next frame among the remaining matching pairs. and the predicted projection position point set The trajectory deviation ; Judgment module: The input end is connected to the output end of the calculation module II, and the output end is connected to the input end of the feedback module I and the input end of the processing module I respectively, which is used to obtain the preset trajectory deviation threshold according to the feature point set in the posture data of the historical frame ,like , then the remaining matching pairs obtained are judged to be unreliable, and feedback module I is executed. , then the remaining matching pairs obtained are judged to be reliable, and processing module I is executed; Feedback module I: the input end is connected to the output end of the judgment module, and is used to send the data that the remaining matching pairs are unreliable to the management end; Processing module I: the input end is connected to the output end of the judgment module, and the output end is connected to the input end of the processing module II, which is used to divide the feature point sets of T consecutive frames including the current frame into N groups of time series feature point sets according to t frames as a group, and at the same time divide them into M spatial sequence feature point sets according to the image block area, and record the total set of space-time feature points after division as K; Processing module II: The input end is connected to the output end of processing module I, and the output end is connected to the input end of calculation module III, which is used to convert the actual projection position point set of the feature point set to be matched in the next frame into the actual projection position point set of the feature point set to be matched in the next frame. The image is divided into M spatial sequence feature point sets according to the image block area, and the total set of feature points to be matched after the division is recorded as L; Calculation module III: The input end is connected to the output end of processing module II, and the output end is connected to the input end of acquisition module II. It is used to calculate the root mean square error between the N sets of time series feature points and the M sets of space series feature points in the total set K, and record it as the temporal and spatial matching correlation degree. ; Acquisition module II: The input end is connected to the output end of the calculation module III, and the output end is connected to the input end of the correction module, which is used to obtain the mean value of the historical frame translation change corresponding to the nth group of time series feature points in T consecutive frames. , the mean of the rotation matrix is ; Correction module: The input end is connected to the output end of the acquisition module II to calculate the correction coefficient , The calculation model is: ,in, is the total correlation, according to the correction coefficient Calculate the translation vector of the current frame after optimization for , the rotation matrix of the current frame after optimization is , and update the translation vector and rotation matrix of the current frame.

7. The visual inertial SLAM system posture guidance system according to claim 6, characterized in that: In the correction module, the correction coefficient The calculation model is updated as follows: ,in, is the dynamic coefficient, .

8. The visual-inertial SLAM system posture guidance system according to claim 7, characterized in that: In calculation module III, the root mean square error between the M spatial sequence feature point sets in the total set L is also calculated and recorded as the spatial matching association degree ; In the correction module, the correction coefficient The calculation model is updated as follows: .

9. The visual inertial SLAM system posture guidance system according to claim 6, characterized in that: It also includes an adjustment module: the input end is connected to the output end of the judgment module, and the output end is connected to the input end of the feedback module I and the input end of the judgment module respectively, for obtaining the acceleration variance of the inertial sensor and gyro noise variance , the adjustment threshold of the calculated trajectory error is ,like , then the adjustment is judged to be reliable, and the trajectory error threshold is updated to the adjustment threshold. , then the adjustment is judged to be unreliable and feedback module I is executed.

10. The visual inertial SLAM system posture guidance system according to claim 9, characterized in that: It also includes a feedback module II, and when the adjustment module determines that the adjustment is reliable, the feedback module II is also executed; Feedback module II: The input end is connected to the output end of the adjustment module, and the output end is connected to the input end of the judgment module, which is used to calculate the actual offset of the feature point set to be matched in the next frame , , record the sum of the actual offsets of the feature point set to be matched within the specified number of consecutive frames and record it as D, and obtain the offset threshold according to the offset of the feature point set in the pose data of the historical frame. If D>D max , then reinitialize the trajectory error threshold.

Images