Error suppression method and system based on MEMS inertial navigation and visual servo fusion

Abstract

The invention relates to an error suppression method and system based on MEMS inertial navigation and visual servo fusion, and belongs to the technical field of navigation and control. The method comprises the following steps: calculating current attitude and position information and generating a fusion data set by acquiring inertial navigation data and visual servo image data of a micro electro mechanical system; based on the fused data set, analyzing error data of micro electro mechanical system inertial navigation, and constructing an error suppression model; environmental information required by error suppression is loaded to a visual fusion prediction model, an error correction path is constructed through visual feature matching, an error drift trajectory is predicted, and a suppression strategy is generated; and based on the navigation task target, adjusting parameters of the control instruction through an optimization control algorithm to suppress global errors, and generating the control instruction. Error suppression and stable navigation of the equipment in a complex environment are realized.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of navigation and control, and particularly relates to an error suppression method and system based on fusion of MEMS inertial navigation and visual servoing. BACKGROUND

[0002] With the development of inertial navigation systems, inertial navigation systems are widely used in robots, unmanned aerial vehicles, autonomous driving and intelligent control fields as a self-positioning method. Micro-electro-mechanical system inertial devices are widely used due to their small size, low cost and low power consumption, but MEMS inertial navigation has problems such as large zero drift, fast cumulative error and sensitive to environmental interference, which leads to a significant decrease in positioning and attitude accuracy during long-term operation. In order to suppress inertial navigation errors, external observation information is usually needed for correction. Visual servoing technology can obtain target position information and attitude changes through image feature point tracking and scene analysis, and has the advantages of high precision and strong intuitiveness. However, visual servoing is easily affected by factors such as light changes, occlusions, insufficient texture and motion blurring, leading to unstable feature point extraction, insufficient real-time performance and robustness. Single reliance on inertial navigation or visual servoing cannot meet the positioning control requirements of high precision and high reliability. In recent years, multi-sensor fusion has become an important research direction for improving navigation and control accuracy. By combining the short-term stability of MEMS inertial navigation and the external observation capability of visual servoing, the shortcomings of each can be complemented to achieve precision improvement and error suppression. However, in the existing technology, there is often a lack of adaptive suppression mechanism for micro-nano level MEMS inertial navigation noise characteristics and visual servoing uncertainty in the data fusion process, resulting in unstable fusion results and still existing cumulative drift and local distortion problems. Therefore, there is an urgent need for an error suppression method and system based on fusion of MEMS inertial navigation and visual servoing to achieve dynamic modeling and real-time compensation of errors. SUMMARY

[0003] To solve the above problems existing in the prior art, the application provides an error suppression method based on fusion of MEMS inertial navigation and visual servoing, The object of the application can be achieved by the following technical solutions: S1: Obtain micro-electro-mechanical system inertial navigation data and visual servoing image data, calculate the current attitude and position information, and generate a fusion data set; S2: Based on the fusion data set, the micro-electro-mechanical system inertial navigation data is used as prediction data and the visual servoing image data is used as observation data to construct an error suppression model through an inertial navigation prediction algorithm; the error suppression model generates error suppression information by inputting attitude information and visual images; S3: Input the error suppression information into a dynamic fusion prediction model, construct a real-time error correction path by analyzing the feature points of the error suppression information, predict the error drift trajectory and generate an error suppression strategy; S4: adjusting an error parameter according to the error suppression strategy to suppress global error to generate a control instruction, and adjusting the error suppression strategy according to feedback of the control instruction.

[0004] Specifically, the fusion data set includes position information and attitude information, which are calculated and generated by a nonlinear estimation algorithm fusing micro-electro-mechanical system (MEMS) inertial navigation data and visual servo image data.

[0005] Specifically, the running process of the error suppression model includes: The attitude angle is measured by the MEMS inertial navigation sensor in combination with the visual servo image data, and the position deviation is calculated according to the environmental adaptability adjustment of the current state to generate a deviation map of the MEMS inertial navigation and the target attitude. Based on the deviation map of the MEMS inertial navigation and the target attitude, the actual deviation of the input attitude information and the target attitude is calculated, and error suppression information is generated.

[0006] Specifically, the method for real-time precision evaluation of the attitude data is: The collected multi-dimensional attitude data is converted into standardized attitude data, and pre-processed attitude data is generated by filtering and denoising; The gradient amplitude and gradient direction of the sampling points in the pre-processed attitude data are calculated, wherein the gradient amplitude is used to represent the attitude change amplitude, and the gradient direction is used to represent the direction information of the attitude edge, and the overall precision of the attitude data is evaluated according to the concentration degree and peak position of the sampling points; Based on the gradient direction information, a direction histogram of the attitude data is constructed, the direction distribution characteristics of the attitude edge are analyzed, and the precision index value of the attitude data is generated in combination with the gradient amplitude and direction information.

[0007] Specifically, the division method of the direction histogram is: The visual servo image is divided into pixel units, and the gradient direction and amplitude of each pixel unit are calculated; the average value of the precision index value of the pixel unit is calculated by weighted average method; The average value is taken as the overall precision index value of the attitude data, which is input into the visual servo control model to obtain regional image information; Based on the principle of regional edge detail information, the size and position of the local region are divided, and the gradient direction is counted into the histogram interval according to the precision index.

[0008] Specifically, the nonlinear estimation algorithm obtains the precision index value of the attitude data by inputting the visual servo image data, and obtains the fusion data containing position information and attitude information according to the change sequence of the precision index value.

[0009] Specifically, the processing steps of the dynamic fusion prediction model include: The collected micro-electro-mechanical system inertial navigation data and visual servo image data are analyzed to extract geometric feature information of error sources; Based on the geometric feature information, error drift state is sampled and estimated to generate a predicted error trajectory; According to the predicted error trajectory, a dynamic fusion map is constructed, and the position and drift trajectory of the error are updated in real time to generate an error suppression strategy.

[0010] Specifically, the change sequence of the precision index value is calculated by a time series analysis model to include the mean, variance and trend slope of the sequence, and the error accumulation mode of the precision index value is identified.

[0011] Specifically, the error suppression strategy feedback adjustment method is: According to the error suppression strategy, a control instruction is generated to adjust the error parameter to suppress the global error; The feedback data of the micro-electro-mechanical system inertial navigation and visual servo image are collected by executing the control instruction, the deviation between the actual execution effect and the target state is recorded, and a feedback data set is generated; The feedback data set is analyzed, the error value of the actual deviation and the predicted error is calculated, the weight and error correction path of the dynamic fusion prediction model are updated, and the adjustment strategy of error suppression is generated.

[0012] Specifically, the execution process of the time series analysis model includes: The change sequence of the precision index value is preprocessed, and the mean, variance and trend slope of the sequence are calculated to extract statistical features in the time dimension; Based on the statistical features, the error accumulation mode is identified according to the time series analysis algorithm, and the prediction trend of error change is generated; According to the prediction trend, the error suppression strategy of the dynamic fusion prediction model is optimized, and the weight distribution of the position information and the attitude information in the fusion data is adjusted.

[0013] Specifically, the regional edge detail information is based on the gradient direction and amplitude of the pixel unit of the visual servo image to identify the edge feature points in the image, and according to the distribution density and geometric characteristics of the edge feature points, the boundary of the local region is divided.

[0014] Specifically, an error suppression system based on MEMS inertial navigation and visual servo fusion, characterized in that it comprises: Data acquisition module: acquires micro-electro-mechanical system inertial navigation data, visual servo image data, calculates the current attitude and position information and generates a fusion data set; The model construction module: based on the fusion data set, the micro-electro-mechanical system inertial navigation data is taken as prediction data and the visual servo image data is taken as observation data for error suppression model construction through an inertial navigation prediction algorithm; the error suppression model generates error suppression information by inputting attitude information and visual images; The fusion prediction module: the error suppression information is input into a dynamic fusion prediction model, a real-time error correction path is constructed by analyzing feature points of the error suppression information, an error drift trajectory is predicted, and an error suppression strategy is generated; The feedback adjustment module: error parameters are adjusted according to the error suppression strategy to suppress global error, and the error suppression strategy is adjusted in real time according to the feedback of the control instruction to generate a control instruction.

[0015] The beneficial effects of the present application are: The present application provides an error suppression method and system based on MEMS inertial navigation and visual servo fusion, which can effectively solve the problems of error accumulation, unstable features and environmental interference in the positioning and attitude estimation process of single inertial navigation or visual servo. By designing a fusion data set, the extended inertial navigation prediction nonlinear estimation algorithm is used to realize the dynamic fusion of inertial navigation prediction and visual observation, which significantly reduces the influence of inertial navigation zero drift on long-term positioning accuracy. In terms of error suppression model, the present application not only constructs the deviation graph of inertial navigation attitude and target attitude, but also introduces a direction histogram-based attitude accuracy evaluation mechanism, which can quantitatively represent the overall accuracy of attitude data, thereby realizing real-time detection and analysis of error features. Further combining the dynamic fusion prediction model, feature point analysis and trajectory prediction of error suppression information can generate error correction path and error suppression strategy in advance, so that the system has a feedforward correction capability. At the control layer, the present application adjusts error parameters and continuously optimizes error suppression strategy through a feedback adjustment mechanism, realizing long-term suppression effect of error in the global range. Compared with the prior art, the present application improves the stability and reliability of attitude and position information while ensuring real-time operation, and is especially suitable for complex application scenarios such as unmanned aerial vehicles, robots and automatic driving with high requirements for navigation accuracy, having significant engineering practical value and promotion prospect. BRIEF DESCRIPTION OF DRAWINGS

[0016] In order to facilitate understanding of those skilled in the art, the present application will be further described below with reference to the accompanying drawings.

[0017] Figure 1 The flowchart of the present application, a MEMS inertial navigation and visual servo fusion-based error suppression method and system.

[0018] Figure 2 The schematic diagram of real-time accuracy evaluation of attitude data in the present application.

[0019] Figure 3 A schematic diagram of the direction histogram construction process in the present application. DETAILED DESCRIPTION

[0020] To further illustrate the technical means and effects adopted by the present application to achieve the predetermined inventive purposes, the specific embodiments, structures, features and effects according to the present application are described in detail below in combination with the drawings and preferred embodiments.

[0021] Please refer to Figure 1 An error suppression method based on MEMS inertial navigation and vision servo fusion, comprising: S1: Obtain micro-electromechanical system inertial navigation data and vision servo image data, calculate the current attitude and position information and generate a fusion data set; S2: Based on the fusion data set, the micro-electromechanical system inertial navigation data is used as prediction data and the vision servo image data is used as observation data to construct an error suppression model through an inertial navigation prediction algorithm; the error suppression model generates error suppression information by inputting attitude information and vision image; S3: Input the error suppression information into a dynamic fusion prediction model, construct a real-time error correction path by analyzing the feature points of the error suppression information, predict the error drift trajectory and generate an error suppression strategy; S4: Adjust the error parameters according to the error suppression strategy to suppress the global error to generate a control command, and adjust the error suppression strategy according to the feedback of the control command.

[0022] Specifically, the fusion data set includes position information and attitude information, which is calculated and generated by fusing micro-electromechanical system inertial navigation data and vision servo image data through a nonlinear estimation algorithm.

[0023] Specifically, the operation process of the error suppression model includes: Measure the attitude angle through the micro-electromechanical system inertial navigation sensor combined with the vision servo image data, and calculate the position deviation according to the environmental adaptability of the current state to generate a deviation map of the micro-electromechanical system inertial navigation and the target attitude; Calculate the actual deviation of the input attitude information and the target attitude based on the deviation map of the micro-electromechanical system inertial navigation and the target attitude, and generate error suppression information.

[0024] In this embodiment, the formula of the error suppression model is specifically: , , , Where rp is the position residual, rθ is the minimum attitude angle residual, vee(.) is the vectorization operator, R(γ) is the observation noise covariance matrix, γ is the attitude accuracy index value, diag(.) is the diagonal matrix, ψ(rk) is the robust cost function, rk is the kth residual component, and δ is the dividing point between the control linear region and the saturation region.

[0025] Specifically, the method for real-time accuracy evaluation of posture data is: The collected multi-dimensional posture data is converted into standardized posture data, and pre-processed posture data is generated by filtering and denoising; Calculating the gradient magnitude and gradient direction of the sampling points in the preprocessed posture data, wherein the gradient magnitude is used to represent the amplitude of posture change, and the gradient direction is used to represent the direction information of the posture edge, and evaluating the overall accuracy of the posture data based on the concentration and peak position of the sampling points; The direction histogram of the posture data is constructed based on the gradient direction information, the direction distribution characteristics of the posture edge are analyzed, and the accuracy index value of the posture data is generated by combining the gradient amplitude and direction information.

[0026] In this embodiment, Figure 3 As shown in the directional histogram construction process, the visual servoing image is divided into several pixel units. The system calculates the gradient direction and amplitude of each pixel unit and uses a weighted average method to obtain the average value of the pixel unit accuracy index. This average value is used as the overall accuracy indicator of the posture data. Next, based on the regional edge detail features, the size and position of the local area are divided. The gradient direction is statistically assigned to the corresponding histogram interval according to the accuracy index, thus generating a directional histogram feature to assist in evaluating and optimizing the accuracy of the error suppression model.

[0027] Specifically, the direction histogram is divided as follows: Dividing the visual servo image into pixel units, calculating the gradient direction and amplitude of each pixel unit; calculating the average value of the pixel unit accuracy index value by weighted average method; The average value is used as the overall accuracy index value of the posture data and input into the visual servo control model to obtain regional image information; The size and position of the local area are divided based on the principle of regional edge detail information, and the gradient direction is counted into histogram intervals according to the accuracy index.

[0028] Specifically, the nonlinear estimation algorithm obtains the accuracy index value of the posture data by inputting visual servoing image data, and obtains fusion data including position information and posture information according to a change sequence of the accuracy index value.

[0029] Specifically, the processing steps of the dynamic fusion prediction model include: The collected micro-electro-mechanical system inertial navigation data and visual servo image data are analyzed to extract geometric feature information of error sources; Based on the geometric feature information, error drift states are sampled and estimated to generate a predicted error trajectory; According to the predicted error trajectory, a dynamic fusion map is constructed, and the position and drift trajectory of the error are updated in real time to generate an error suppression strategy.

[0030] Specifically, the change sequence of the precision index value is calculated by a time series analysis model to include the mean, variance and trend slope of the sequence, and the error accumulation mode of the precision index value is identified.

[0031] In this embodiment, the main execution process includes: collecting three-axis acceleration, angular velocity and attitude angle data by a micro-electro-mechanical system inertial navigation sensor, and simultaneously obtaining image sequences of a target environment by a visual servo module. The inertial navigation data and image data are synchronously processed, and a nonlinear estimation algorithm is used to calculate the current position information and attitude information, and finally a fusion data set is generated. The fusion data set includes position information, attitude information and timestamp information to ensure data synchronization and integrity; based on the fusion data set, an inertial navigation prediction algorithm is used to fuse the inertial navigation data as a predicted quantity and the visual servo data as an observed quantity. As shown in Figure 2 In this process, the system generates a deviation map between the micro-electro-mechanical system inertial navigation and the target attitude, and calculates the actual deviation between the input attitude information and the target attitude through an error suppression formula; the generated error suppression information is input into a dynamic fusion prediction model to analyze error feature points and construct a real-time error correction path. At the same time, based on a trajectory prediction method, a possible future error drift trajectory is calculated; according to the error suppression strategy, error parameters are adjusted in real time to generate control instructions to correct the overall navigation output. After the control is executed, the system dynamically corrects the error suppression strategy according to the feedback data to form a closed-loop control mechanism.

[0032] Specifically, the method for feedback adjustment of the error suppression strategy is: According to the error suppression strategy, a control instruction is generated to adjust the error parameters to suppress the global error; The feedback data of the micro-electro-mechanical system inertial navigation and the visual servo image are collected by executing the control instruction, the deviation between the actual execution effect and the target state is recorded, and a feedback data set is generated; The feedback data set is analyzed, the error value of the actual deviation and the predicted error is calculated, the weight and error correction path of the dynamic fusion prediction model are updated, and an adjustment strategy for error suppression is generated.

[0033] Specifically, the execution process of the time series analysis model includes: The change sequence of the precision index value is preprocessed, and the mean, variance and trend slope of the sequence are calculated to extract statistical features in the time dimension; Based on the statistical features, an error accumulation pattern is identified according to a time series analysis algorithm, and a prediction trend of error change is generated; According to the prediction trend, the error suppression strategy of the dynamic fusion prediction model is optimized, and the weight distribution of position information and attitude information in the fusion data is adjusted.

[0034] Specifically, the regional edge detail information is identified based on the gradient direction and amplitude of the pixel unit of the visual servo image, and the boundaries of the local region are divided according to the distribution density and geometric characteristics of the edge feature points.

[0035] Specifically, an error suppression system based on MEMS inertial navigation and visual servo fusion comprises: Data acquisition module: acquires micro-electro-mechanical system inertial navigation data and visual servo image data, calculates the current attitude and position information and generates a fusion data set; Model construction module: based on the fusion data set, the micro-electro-mechanical system inertial navigation data is taken as prediction data and the visual servo image data is taken as observation data for error suppression model construction through inertial navigation prediction algorithm; the error suppression model generates error suppression information by inputting attitude information and visual image; Fusion prediction module: input the error suppression information into the dynamic fusion prediction model, construct real-time error correction path by analyzing the feature points of the error suppression information, predict error drift trajectory and generate error suppression strategy; Feedback adjustment module: adjust error parameters to suppress global error according to the error suppression strategy, and generate control instructions by real-time adjusting the error suppression strategy according to the feedback of the control instructions.

[0036] The above is only a preferred embodiment of the present application, not any form of limitation on the present application, although the present application has been disclosed as above with a preferred embodiment, however, it is not intended to limit the present application, any person skilled in the art, without departing from the scope of the technical solution of the present application, can make some changes or modifications of the above disclosed technical content as equivalent embodiments, but as long as it does not deviate from the technical solution of the present application, according to the technical essence of the present application, any simple modification, equivalent change and modification of the above embodiments are still within the scope of the technical solution of the present application.

Claims

1. A MEMS-based inertial navigation and vision servo fusion error suppression method, characterized in that, The method comprises the following steps: S1: acquiring micro-electro-mechanical system inertial navigation data and visual servo image data, calculating current attitude and position information, and generating a fusion data set; S2: based on the fusion data set, the micro-electro-mechanical system inertial navigation data is taken as prediction data, and the visual servo image data is taken as observation data to construct an error suppression model through an inertial navigation prediction algorithm; the error suppression model generates error suppression information by inputting attitude information and visual image; S3: inputting the error suppression information into a dynamic fusion prediction model, constructing a real-time error correction path by analyzing the feature points of the error suppression information, predicting an error drift trajectory, and generating an error suppression strategy; S4: according to the error suppression strategy, adjusting error parameters to suppress global errors through an optimization control algorithm to generate a control instruction, and adjusting the error suppression strategy according to the feedback of the control instruction.

2. The method of claim 1, wherein, The fusion data set includes position information and attitude information, which is calculated and generated by fusing micro-electro-mechanical system inertial navigation data and visual servo image data through a nonlinear estimation algorithm.

3. The method of claim 1, wherein, The operation process of the error suppression model comprises: measuring the attitude angle through the micro-electro-mechanical system inertial sensor combined with the visual servo image data, calculating the position deviation according to the environmental adaptability of the current state, and generating the deviation map of the micro-electro-mechanical system inertial navigation and the target attitude; calculating the actual deviation of the input attitude information and the target attitude based on the deviation map of the micro-electro-mechanical system inertial navigation and the target attitude, and generating error suppression information.

4. The method of claim 1, wherein, The method for real-time precision evaluation of attitude data is: converting the collected multi-dimensional attitude data into standardized attitude data, and generating preprocessed attitude data through filtering and denoising; calculating the gradient amplitude and gradient direction of the sampling points in the preprocessed attitude data, wherein the gradient amplitude is used to represent the attitude change amplitude, and the gradient direction is used to represent the direction information of the attitude edge, and evaluating the overall precision of the attitude data according to the concentration degree and peak position of the sampling points; constructing a direction histogram of the attitude data based on the gradient direction information, analyzing the direction distribution characteristics of the attitude edge, and combining the gradient amplitude and direction information to generate the precision index value of the attitude data.

5. The method of claim 1, wherein, The division method of the direction histogram is: dividing the visual servo image into pixel units, calculating the gradient direction and amplitude of each pixel unit, and calculating the average value of the precision index value of the pixel unit through weighted average method; taking the average value as the overall precision index value of the attitude data, inputting it into the visual servo control model, and obtaining regional image information; dividing the size and position of the local region based on the regional edge detail information principle, and statistically classifying the gradient direction into the histogram interval according to the precision index.

6. The method of claim 2, wherein, The nonlinear estimation algorithm obtains the precision index value of the attitude data by inputting the visual servo image data, and obtains the fusion data containing position information and attitude information according to the change sequence of the precision index value.

7. The method of claim 6, wherein, The processing steps of the dynamic fusion prediction model comprise: analyzing the collected micro-electro-mechanical system inertial navigation data and visual servo image data, and extracting the geometric feature information of the error source; sampling and estimating the error drift state based on the geometric feature information to generate a predicted error trajectory; A dynamic fusion map is constructed according to the predicted error trajectory, and the position and drift trajectory of the error are updated in real time to generate an error suppression strategy.

8. The method of claim 2, wherein, The change sequence of the precision index value is calculated by a time series analysis model to include the mean, variance and trend slope of the sequence, and an error accumulation mode of the precision index value is identified.

9. The method of claim 6, wherein, The error suppression strategy feedback adjustment method is: A control instruction is generated according to the error suppression strategy to adjust the error parameter to suppress the global error; The control instruction is executed to collect feedback data of the micro-electro-mechanical system inertial navigation and visual servo image, record the deviation of the actual execution effect and the target state, and generate a feedback data set; The feedback data set is analyzed, the error value of the actual deviation and the predicted error is calculated, the weight and error correction path of the dynamic fusion prediction model are updated, and an adjustment strategy for error suppression is generated.

10. The method of claim 8, wherein, The execution process of the time series analysis model includes: The change sequence of the precision index value is preprocessed to calculate the mean, variance and trend slope of the sequence to extract statistical features in the time dimension; Based on the statistical features, an error accumulation mode is identified according to a time series analysis algorithm to generate a predicted trend of error change; According to the predicted trend, the error suppression strategy of the dynamic fusion prediction model is optimized, and the weight distribution of the position information and the attitude information in the fusion data is adjusted.

11. The method of claim 5, wherein, The regional edge detail information principle is based on the gradient direction and amplitude of the pixel unit of the visual servo image to identify the edge feature points in the image, and according to the distribution density and geometric characteristics of the edge feature points, the boundaries of the local region are divided.

12. An error suppression system based on MEMS inertial navigation and vision servo fusion, characterized in that, It includes: A data acquisition module: acquires micro-electro-mechanical system inertial navigation data and visual servo image data, calculates the current attitude and position information and generates a fusion data set; A model construction module: based on the fusion data set, the micro-electro-mechanical system inertial navigation data is taken as prediction data and the visual servo image data is taken as observation data by an inertial navigation prediction algorithm to construct an error suppression model; the error suppression model generates error suppression information by inputting attitude information and visual image; A fusion prediction module: the error suppression information is input into a dynamic fusion prediction model, a real-time error correction path is constructed by analyzing the feature points of the error suppression information, an error drift trajectory is predicted, and an error suppression strategy is generated; A feedback adjustment module: according to the error suppression strategy, the error parameter is adjusted to suppress the global error, and the error suppression strategy is adjusted in real time according to the feedback of the control instruction to generate a control instruction.

Images