A point-line fusion positioning method and system based on multi-IMU

Abstract

The present invention provides a point-line fusion positioning method and system based on multiple IMUs, the method comprising: performing image acquisition and inertial data acquisition respectively through a camera and four IMUs of the same model; extracting features from the image frames acquired by the camera in step S1; pre-integrating the multiple groups of data of the four IMUs obtained in step S1 respectively, and calculating the position and speed of the IMUs corresponding to the two frames of images; initializing vision and IMU; mapping the probability of the measured values ​​obtained after initializing multiple IMUs (A, B, C, D) in step S4 to a virtual IMU (E); modeling the line reprojection error as the distance from the two end points of the line segment to the projection line, and then minimizing the reprojection error of the point line and the IMU residual to obtain accurate pose estimation; testing the algorithm designed in this article on public data sets and collected data sets. The present invention solves the technical problems of low positioning and fusion vision accuracy, poor low-texture scene processing effect, and poor line segment extraction quality.

Description

Technical Field

[0001] The present invention relates to the field of multi-sensor fusion positioning in SLAM technology, and in particular to a point-line fusion positioning method and system based on multi-IMU. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) technology has been developed for more than ten years since it was proposed. From the original pure visual SLAM method based on single features to the current SLAM method based on multi-sensor information fusion and multi-feature fusion, SLAM technology has gradually developed and matured. SLAM technology is mainly used for navigation and positioning of robots, drones and unmanned vehicles, and is one of the important technologies in the field of artificial intelligence. At present, the mainstream visual SLAM technologies mainly include ORB_SLAM based on indirect method (point features) and LSD-SLAM, SVO and DSO based on direct method. Among them, ORB_SLAM provides three camera type interfaces: monocular, binocular and RGB-D, including three threads of tracking, local mapping and loop detection, which can ensure the global consistency of maps and motion trajectories. It is a relatively complete and mature framework in the current visual SLAM algorithm. The disadvantage is that the usage scenarios are limited and the positioning robustness is poor. LSD-SLAM based on the direct method estimates the pose of the image grayscale value. It directly operates on the image pixels instead of extracting and matching the feature points of the image. It can build a semi-dense map on the CPU and is suitable for large-scale scenes. The disadvantage is that it is not very robust to changes in environmental illumination. Both SVO and DSO are SLAM solutions based on sparse direct methods. The difference is that the SVO solution uses a hybrid model to combine the direct method and the indirect method to achieve pose estimation, while the DSO solution does not have a loop detection function, which will inevitably produce closed-loop errors.

[0003] With the increasing complexity of scenes and the increasing requirements for accuracy and real-time performance, pure visual SLAM technology has reached a bottleneck. Therefore, SLAM technology based on multi-sensors and multi-features has developed rapidly. Currently, the most commonly used sensors include cameras (monocular, binocular, RGB-D, event cameras), IMU (inertial measurement unit), wheel speed meter and lidar. In terms of scene features, in addition to point features, line features, surface features and point-line fusion feature solutions have also been added. At present, VIO (Visual Inertial Odometry) based on optimized vision and IMU data fusion has a relatively mature framework, such as the monocular VINS-mono solution of the Hong Kong University of Science and Technology and the monocular / IMU and binocular / IMU solutions in ORB_SLAM3. In addition to these VIO algorithm frameworks, there are also SLAM algorithms based on multi-feature fusion, such as PL-SLAM, PL-VINS, etc., which are representatives of point-line fusion methods.

[0004] Most previous solutions only apply a single IMU and point feature visual fusion to estimate the robot's pose. Although such a solution can provide acceptable accuracy and robustness for different use cases, a visual fusion solution that fuses multiple IMUs and point-line features will further improve the overall performance and make the algorithm more robust in low-texture environments.

[0005] Prior art 1 related to the present invention - PL-SLAM based on point-line feature fusion

[0006] Technical solution of prior art 1:

[0007] In order to solve the problem of poor robustness of traditional feature point methods in low-texture areas, Ruben updated PL-SLAM based on PL-StVO in 2019. The point feature extraction of this algorithm uses ORB features, and the line feature extraction part uses the LSD algorithm to extract features. The Gauss-Newton method is used to minimize the reprojection error of point and line features, and the weights of points and lines are adjusted by statistical errors. This algorithm realizes a relatively complete binocular vision SLAM system based on the previous one.

[0008] Disadvantages of prior art 1:

[0009] This method does not analyze the feature matching accuracy, and uses the same error model to construct the error optimization problem for the two features, resulting in the same effect of different features in pose estimation.

[0010] This solution is still a purely visual SLAM solution, and there is still room for improvement in positioning accuracy.

[0011] Prior art 2 related to the present invention - VIO solution based on single IMU (ORB_SLAM3 and VINS-mono)

[0012] Technical solution of the second prior art:

[0013] ORB_SLAM3 is a SLAM system that supports vision, visual inertial fusion, and hybrid maps. It can run on monocular, binocular, and RGB-D cameras using pinhole or fisheye camera models. It is the first tightly coupled feature-based VIO system that relies only on maximum a posteriori estimation (even when the IMU is initialized). The system can run robustly in real time in both large and small scenes, and its accuracy is 2 to 5 times higher than the previous version. In terms of mapping, a hybrid map is built through an improved new relocalization module, which enables the system to run stably in the scene for a long time.

[0014] The VINS-mono scheme is a monocular visual-inertial system based on a tightly coupled sliding window nonlinear optimization method. The scheme proposes the best IMU pre-integration method, estimator initialization mechanism, fault detection and recovery mechanism, and external parameter online calibration.

[0015] Disadvantages of the second prior art:

[0016] 1) Both solutions are excellent and comprehensive system frameworks in VIO. However, in terms of sensor fusion, they still use a single IMU and vision fusion. Although the current accuracy is within the expected ideal range, it can be further improved.

[0017] 2) Both schemes use single point features for tracking in terms of visual features, which is not friendly to structured low-texture scenes.

[0018] Prior art three related to the present invention - PL-VINS solution

[0019] Technical solution of prior art three:

[0020] The PL-VINS solution is a real-time point-line based monocular inertial navigation SLAM solution. The solution is based on the VINS-mono framework based on point features, and adds line feature extraction and matching on this basis. By improving the LSD algorithm, the running speed is increased to three times the original. In addition, the line reprojection residual is modeled as the distance from the midpoint to the line.

[0021] Disadvantages of the prior art three:

[0022] 1) Although the improved LSD algorithm has greatly improved the speed of straight line extraction, it is still lacking in line segment quality. It is still easy to split long line segments into many short line segments, making the constructed scene map inaccurate.

[0023] 2) By integrating a single IMU with vision, positioning accuracy can be further improved.

[0024] In addition, the existing invention patent "A binocular inertial simultaneous positioning and map construction method based on point and line feature fusion" with application number CN202110138834.9 includes S1, front-end visual inertial odometer; S2, back-end optimization; S3 three-dimensional environment mapping; in the front-end visual inertial odometer, a binocular camera is used to collect left and right eye pictures in front of the camera as the detection and analysis input images of the visual inertial simultaneous positioning and map construction system. This existing patent effectively integrates point, line and IMU data information in an optimized sliding window to solve the problem of low accuracy and instability of point feature visual SLAM algorithm in weak texture environment, so as to achieve higher accuracy pose estimation; this existing patent uses length suppression, near line merging and broken line splicing strategies to optimize the quality of line segment extraction of traditional algorithms while ensuring fast extraction, and reduce the mismatch rate of system line features to extract features that can adapt to indoor weak texture and textureless scenes; this existing patent extracts more effective line features in weak texture and textureless indoor structured scenes, and performs multi-source information fusion with point features and IMU pre-integrated data to obtain globally consistent camera pose data. Although this existing patent uses binocular cameras and point features and IMU pre-integrated data for multi-source information fusion, this existing patent uses length suppression, near line merging and broken line splicing strategies, which are different from the specific technical features such as the line segment combination method in the improved LSD line feature extraction algorithm of this application, and the technical effect produced is also different from the technical effect of this application.

[0025] In summary, the existing technology has technical problems such as low positioning and fusion vision accuracy, poor low-texture scene processing effect, and poor line segment extraction quality. Summary of the invention

[0026] The technical problem to be solved by the present invention is how to solve the technical problems of low positioning and fusion visual accuracy, poor low-texture scene processing effect and poor line segment extraction quality.

[0027] The present invention adopts the following technical solution to solve the above technical problems: a point-line fusion positioning method based on multiple IMUs. The method includes:

[0028] S1. Connect the 4IMU array integrated module to the camera to collect environmental image data and motion inertial data;

[0029] S2, using the ORB algorithm to extract and match the point features in the environmental image data, using the line segment combination method to locate and combine short line segments, and extracting suitable long line segments to obtain a suitable LSD algorithm, the step S2 includes:

[0030] S21, adjusting the hidden parameters of the preset LSD algorithm;

[0031] S22, suppose the LSD algorithm output line segment group is: L = {l1,l2,...,l n}(n=1,2,3…), where l b is the baseline in L, and the preset rectangular area R is the baseline l b The impact area;

[0032] S23, arranging all line segments in the line segment group L output by the LSD algorithm in descending order of length;

[0033] S24, according to the main direction of the line segment histogram, select the line segments in the group with similar directions as the combined pre-selected line segments according to the following logic, the step S24 includes:

[0034] S241, setting the approximate standard Φ of the line segment l :

[0035] Φ l =μ|l b |

[0036] , where μ is the approximation coefficient;

[0037] S242, if the calculated distance is greater than the approximate standard Φ l , then cancel the combination mechanism;

[0038] S243, otherwise calculate l b and l i The angle difference θ;

[0039] S244, determine the angle approximation standard Φ using the following logic θ , if the angle difference θ satisfies the angle approximation criterion Φ θ , then the combination

[0040] The short line segment:

[0041]

[0042] , where x, y and ξ are experimental setting parameters, and α is the normalization coefficient;

[0043] S25, splicing the combined pre-selected line segments to obtain a suitable long line segment;

[0044] S3, obtaining an IMU measurement model with a preset logic, obtaining initial rotation data from the motion inertial data, and performing IMU pre-integration according to the IMU measurement model and the initial rotation data;

[0045] S4, initializing vision and IMU according to the environmental image data and the motion inertial data;

[0046] S5. Use a least squares estimator to randomly estimate the motion inertial data measured by the 4IMU array integration module to obtain and marginalize the rotation acceleration probability between IMUs, and fuse them to obtain a virtual IMU E;

[0047] S6. Construct an optimization function to map the inter-IMU rotation acceleration probability to the virtual IMU E, thereby optimizing the reprojection error of point and line features and the residual of the virtual IMU E to improve the camera pose;

[0048] S7. Use the logic designed in the previous steps to test the EuRoC dataset, YorkUrban dataset and indoor dataset to obtain the point-line fusion positioning verification results.

[0049] The present invention proposes to mainly apply the positioning problem of the robot itself in SLAM technology to improve its robustness and positioning accuracy in a weak texture environment. The present invention uses the more mature ORB_SLAM3 framework in the VIO solution. The visual fusion solution of multiple IMUs and point-line feature fusion will further improve the overall performance and make the algorithm more robust in a low-texture environment.

[0050] The present invention proposes a line segment combination method, which locates and merges short line segments that are close enough and have similar main directions in the image line feature dense area, and extracts high-quality long line segments. The present invention improves the traditional LSD line segment feature extraction algorithm, prevents the scene line features from being excessively segmented into small line segments, avoids the problem of inaccurate feature extraction of the scene caused by the traditional LSD line feature extraction algorithm segmenting the long line segments in the scene into many small short line segments, and improves the effect of mapping. The improved algorithm can effectively solve this problem, increase the running speed, and consolidate the real-time requirements.

[0051] In a more specific technical solution, the frequency of the 4IMU array integrated module in step S1 includes: 200 Hz; the operating frequency of the camera includes: 10 Hz.

[0052] In a more specific technical solution, step S21 includes:

[0053] S211, using OpenCV to generate an N-layer Gaussian pyramid to represent the original image, downsampling the original image N-1 times, and blurring it N times;

[0054] S212, extracting straight lines using the LSD algorithm at each layer of the Gaussian pyramid;

[0055] S213. If the alignment area points in the closed rectangular support domain of the line segment are less than a preset threshold, the LSD algorithm is used to set a minimum density threshold to eliminate unreliable line segments, wherein the minimum density threshold is set to 0.6 to speed up the processing.

[0056] In a more specific technical solution, in step S212, the scale of the Gaussian pyramid is adjusted to s=0.5, and the number of layers is adjusted to N=2.

[0057] In a more specific technical solution, step S3 includes:

[0058] S31, obtain the IMU measurement model by the following logic processing:

[0059] ω m =ω b +b g +n g

[0060]

[0061] Among them, ω m and are the measurements of the gyroscope and accelerometer, ω b and a w Respectively represent the IMU angular velocity and linear acceleration in the coordinate system b and w, where b is the body system and w is the world system. represents the transformation matrix between coordinate system w and coordinate system b, n a , n g is Gaussian noise, b a , b g is the random walk measurement deviation model, and g w is the known gravity vector in the w system.

[0062] S32, integrate the IMU using the following logic:

[0063]

[0064]

[0065]

[0066] in They correspond to IMU from b k to b k+1 The position, velocity and attitude quaternion at the moment, is quaternion multiplication, Here we assume that the acceleration observation a m and the rotation matrix It is constant during the time Δt;

[0067] S33, multiply both sides of the equation in step S32 by the following logic The rotation matrix in the integral term becomes The initial value is the unit matrix to eliminate the initial rotation Dependencies:

[0068]

[0069]

[0070]

[0071] Among them, the IMU pre-integration items that do not depend on the initial state include:

[0072]

[0073]

[0074]

[0075] The present invention is to eliminate the initial rotation Dependence, multiply the integral by The rotation matrix in the integral term becomes The initial value is the unit matrix, which eliminates the dependence on the initial value.

[0076] In a more specific technical solution, in step S4, the environmental image data and the motion inertial data are subjected to pure visual maximum a posteriori estimation, pure IMU maximum a posteriori estimation, and VI maximum a posteriori estimation to find the gravity direction, initial velocity, and gyroscope and accelerometer biases in the system's initial inertial system.

[0077] In the inertial mode of the present invention, the mapping thread will use the maximum a posteriori estimation to initialize and optimize the IMU parameters, thereby improving the robustness of the algorithm in the technical solution.

[0078] In a more specific technical solution, step S5 includes:

[0079] S51, using the IMU measurement model to define the IMU state vector and error state definition:

[0080] The IMU state vector is defined by the following logic:

[0081]

[0082] ,in, Represents the quaternion transformation from the IMU coordinate system (b system) to the world coordinate system;

[0083] S52. Describe the IMU continuous time motion dynamics using the following logic:

[0084]

[0085] S53, defining the error state with the following logic:

[0086]

[0087] S54, define the quaternion local error with the following logic

[0088]

[0089] ,in, represents an antisymmetric matrix, for An estimated value of

[0090] S55, the nonlinear equation for predicting state estimation is obtained by the following logical processing and the linearized equation describing the error state To estimate the IMU probability:

[0091]

[0092]

[0093] , where F and G represent the state transfer matrix and noise Jacobian matrix of continuous-time linearization, n c is the process noise, which follows a Gaussian distribution;

[0094] S56, replace the gyroscope measurement value ω in step S55 with the measurement value of the virtual IMU E m and the accelerometer measurement a m , to generate the virtual IMU E, wherein the measurement values ​​of the gyroscope and accelerometer of the virtual IMU E in step S56 are generated using the following logic:

[0095] ω mV =N + ω m

[0096] a mV =M(a m -S(ω mV ))

[0097] in,

[0098] M=(Z T N) + Z T

[0099]

[0100] Z T is the left adjacent space projection of Y, that is, Z T Y=0,N + is the generalized inverse matrix of N. In practice, for the four IMUs N, its generalized inverse matrix N + The calculation can be simplified as Using the generated virtual IMU measurement ω mV , a mV The prediction equations for IMU state estimation can be used without modification;

[0101] In a more specific technical solution, step S6 includes:

[0102] S61. Define the transformation matrix between the world coordinate system and the camera coordinate system using the following logic:

[0103]

[0104] S62, obtain the straight line L in the world coordinate system by the following logic processing w L converted to the camera coordinate system c :

[0105]

[0106] , where [.] × is a symmetric matrix of vectors;

[0107] S63, converting the line segments in the image to the pixel coordinate system plane using the logic described in step S62:

[0108]

[0109] , where K′ is the projection matrix of the line feature;

[0110] S64, the error between the projected point on the image and the actual measured point is defined as the point feature reprojection error according to the following logic:

[0111] e ij =x ij -π(ζ wj , X wj )

[0112] , where π is the projection function, xi is the jth feature point in the point cloud, i is the key frame number;

[0113] S65, define the reprojection error of the line feature line l by the following logic:

[0114]

[0115] , where X b =(x b ,y b , 1) T , X e =(x e ,y e , 1) T Represents the two endpoints of a line segment;

[0116] S66, according to the point feature reprojection error and the line feature reprojection error, the error function of the line fusion feature is obtained by the following logic processing:

[0117]

[0118]

[0119] , where ρ is the Huber kernel function, Ω ij ,Ω l are the covariance matrices of point and line features respectively;

[0120] S67. According to the error function of the line fusion feature, the final optimization function of the system is obtained by the following logic processing:

[0121] C=e Vmu +e pl

[0122] The present invention uses multiple IMUs (4) to assist vision for precise positioning. Compared with a single IMU, without significantly increasing or equalizing the amount of calculation, the proposed method maps the probability of data collected by 4 IMUs to a virtual IMU, and then fuses the visual information, thereby improving the positioning accuracy of the robot in a low-texture environment.

[0123] In a more specific technical solution, the step S7 includes: the EuRoC dataset includes: a low-texture scene dataset; the YorkUrban dataset includes: an urban environment line feature dataset.

[0124] In a more specific technical solution, a point-line fusion positioning system based on multiple IMUs includes:

[0125] A combined sensor device is used to connect the 4IMU array integrated module to the camera to collect and acquire environmental image data and motion inertial data;

[0126] An improved LSD algorithm module is used to locate the combined short line segments by using the line segment combination method, thereby extracting the applicable long line segments to obtain the applicable LSD algorithm. The improved LSD algorithm module is connected to the combined sensor device, and the improved LSD algorithm module includes:

[0127] A parameter adjustment module, used to adjust the hidden parameters of the preset LSD algorithm;

[0128] The output line segment group and influence area module is used to set the LSD algorithm output line segment group as: L = {l1,l2,...,l n}(n=1,2,3…), where l b is the baseline in L, and the preset rectangular area R is the baseline l b The output line segment group and the influence area module are connected with the parameter adjustment module;

[0129] A line segment length arrangement module, used to arrange all line segments in the line segment group L output by the LSD algorithm in descending order of length, the line segment length arrangement module being connected to the output line segment group and the impact area module;

[0130] A line segment combination module is used to select line segments with similar directions in a group as combined pre-selected line segments according to the main direction of the line segment histogram using the following logic. The line segment combination module is connected to the line segment length arrangement module and includes:

[0131] Approximation standard module, used to set the approximation standard Φ of the line segment l :

[0132] Φ l =μ|l b |

[0133] , where μ is the approximation coefficient;

[0134] The non-approximate processing module is used to calculate the distance greater than the approximate standard Φ l , the combination mechanism is cancelled, and the non-approximate processing module is connected to the approximate standard module;

[0135] The approximate angle difference processing module is used to calculate the distance less than or equal to the approximate standard Φ l When l b and l i The angle difference θ, the approximate angle difference processing module is connected with the approximate standard module;

[0136] The angle approximation standard module is used to determine the angle approximation standard Φ with the following logic θ , if the angle difference θ satisfies the angle approximation criterion Φθ , then combine the short line segments:

[0137]

[0138] , where x, y and ξ are experimental setting parameters, α is a normalization coefficient, and the angle approximation standard module is connected to the approximate angle difference processing module;

[0139] A short line segment splicing module is used to splice the combined pre-selected line segments to obtain a suitable long line segment, and the short line segment splicing module is connected to the angle approximation standard module;

[0140] An IMU pre-integration module, used to obtain an IMU measurement model with a preset logic, obtain initial rotation data from the motion inertial data, and perform IMU pre-integration according to the IMU measurement model and the initial rotation data, wherein the IMU pre-integration module is connected to the combined sensor device;

[0141] An initialization module, used to initialize vision and IMU according to the environmental image data and the motion inertial data, wherein the initialization module is connected to the combined sensor device;

[0142] An IMU probability module is used to randomly estimate the motion inertial data measured by the 4IMU array integration module using a least squares estimator to obtain and marginalize the probability of rotation acceleration between IMUs, and fuse them to obtain a virtual IMU E. The IMU probability module is connected to the initialization module;

[0143] A camera pose optimization module is used to construct an optimization function to map the inter-IMU rotation acceleration probability to the virtual IMU E, thereby optimizing the reprojection error of point and line features and the IMU residual to improve the accuracy of camera pose estimation. The camera pose optimization module is connected to the IMU probability module;

[0144] The data set verification module is used to test the EuRoC data set, YorkUrban data set and indoor data set using the logic designed in the above steps to obtain the point-line fusion positioning verification result. The data set verification module is connected to the IMU probability module and the camera pose optimization module.

[0145] Compared with the prior art, the present invention has the following advantages: the present invention is mainly applied to the positioning problem of the robot itself in the SLAM technology to improve its robustness and positioning accuracy in a weak texture environment. The present invention uses the more mature ORB_SLAM3 framework in the VIO solution. The visual fusion solution of multiple IMUs and point-line feature fusion will further improve the overall performance and make the algorithm more robust in a low-texture environment.

[0146] The present invention proposes a line segment combination method, which locates and merges short line segments that are close enough and have similar main directions in the image line feature dense area, and extracts high-quality long line segments. The present invention improves the traditional LSD line segment feature extraction algorithm, prevents the scene line features from being excessively segmented into small line segments, avoids the problem of inaccurate feature extraction of the scene caused by the traditional LSD line feature extraction algorithm segmenting the long line segments in the scene into many small short line segments, and improves the effect of mapping. The improved algorithm can effectively solve this problem, increase the running speed, and consolidate the real-time requirements.

[0147] The present invention is to eliminate the initial rotation Dependence, multiply the integral by The rotation matrix in the integral term becomes The initial value is the unit matrix, which eliminates the dependence on the initial value. In the inertial mode of the present invention, the mapping thread will use the maximum a posteriori estimation to initialize and optimize the IMU parameters, thereby improving the robustness of the algorithm in the technical solution.

[0148] The present invention uses multiple IMUs (4) to assist vision for precise positioning. Compared with a single IMU, without significantly increasing or equalizing the amount of calculation, the proposed method maps the probability of data collected by 4 IMUs to a virtual IMU, and then fuses the visual information, thereby improving the positioning accuracy of the robot in a low-texture environment. The present invention solves the technical problems of low positioning and fusion vision accuracy, poor low-texture scene processing effect, and poor quality of extracted line segments in the prior art. BRIEF DESCRIPTION OF THE DRAWINGS

[0149] Figure 1 This is a schematic diagram of the system flow of Example 1 of the present invention applied to the ORB_SLAM3 framework;

[0150] Figure 2 Schematic diagram of a 4IMU array according to Embodiment 2 of the present invention;

[0151] Figure 3 This is a schematic diagram of the steps of a point-line fusion positioning method based on multi-IMU according to Embodiment 2 of the present invention;

[0152] Figure 4 Schematic diagram of the line segment combination method according to Embodiment 2 of the present invention. DETAILED DESCRIPTION

[0153] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0154] Example 1

[0155] like Figure 1 As shown in the figure, the entire technical solution is built on the framework of ORB_SLAM3. The whole process is divided into three threads: tracking thread, local mapping thread, and loop detection thread. The tracking thread processes sensor information (multiple IMUs and cameras) and calculates the posture of the current frame in the active map in real time. At the same time, the tracking thread also determines whether to use the current frame as a key frame. Figure 1 The main contents of this work include Multiple IMU (IMU array), IMU Pre-intergration, Extract Lines based on Combination, Line Tracking, Map Lines, Recent Map Points and Maplines Culling (reprojection error of point and line features), New Points and Lines Creation, Multible IMU (A, B, C, D) Initialization and Fused IMU E (fused virtual IMU E).

[0156] The local mapping thread adds keyframes and map points to the currently active map, deletes redundant frames, and optimizes the map using visual BA or visual-inertial BA techniques by operating on keyframes near the current frame. In addition, in inertial mode, the mapping thread uses maximum a posteriori estimation to initialize and optimize IMU parameters.

[0157] Whenever a new keyframe is added to the loop detection thread, the thread detects the common area in the activation map and the entire Atlas map. If the common area belongs to the activation map, loop correction is performed. Otherwise, it is integrated into other maps and the integrated map is used as the new activation map. After loop correction, a separate thread is started to perform global BA to further optimize the map.

[0158] Example 2

[0159] like Figure 2 The specific technical solution process is as follows:

[0160] S1. Use the camera and 4 IMUs of the same model to collect images and inertial data respectively, and the IMU acquisition frequency is greater than the camera acquisition frequency (IMU A, B, C, D frequencies are all 200HZ, and the camera operating frequency is 10HZ). Figure 2 Shown is a schematic diagram of the 4IMU array sensor structure;

[0161] In this embodiment,

[0162] Data collection and enhancement:

[0163] pass Figure 2 The 4IMU array integrated module is connected to the camera (the positional relationship between the IMU and the camera is fixed. The frequencies of IMU A, B, C, and D are all 200HZ, and the operating frequency of the camera is 10HZ). The combined sensor device is used to collect data about the environment to form a video stream. The camera and IMU record the image data of the environment and the inertial data of the movement respectively. In the process of collecting data, in order to avoid the scene blur caused by moving objects and rapid movement, the device can be installed on the mobile platform of RIA100 throughout the process, and the handle can be used to collect data in the corridor, studio, and classroom (non-working hours) at a low speed.

[0164] The collected data are time-stamped and aligned at a fixed frame rate, and the IMU data are also aligned to form a test data set.

[0165] S2, extracting features from the image frame acquired by the camera in step S1. Point features are extracted and matched using the ORB algorithm, and line features are extracted using the improved LSD algorithm;

[0166] In this embodiment,

[0167] Feature extraction and matching:

[0168] The image information collected in step S1 is subjected to feature extraction and matching. The point feature extraction and matching uses the ORB algorithm. The feature line segment extraction uses the improved LSD algorithm.

[0169] The LSD algorithm improvements are as follows:

[0170] Parameter adjustment:

[0171] LSD (Line Segment Detector) is a line detection and segmentation algorithm that can produce sub-pixel accuracy detection results in linear time. Although the algorithm is designed to work on any digital image without parameter adjustment, there are still some hidden parameters that can be optimized from actual experiments to speed up the detection. First, OpenCV uses the generated N-layer Gaussian pyramid to represent the original image, where the image is downsampled N-1 times, blurred N times, and then the LSD algorithm is used to extract lines at each layer. We simplified the scale and number of layers of the pyramid, adjusting the scale to s = 0.5 and the number of layers to N = 2. Next, if the aligned area points in the closed rectangular support domain of the line segment are less than the threshold, LSD will set a minimum density threshold to suppress the line segment, which we set to 0.6 to speed up the processing. Although the refinement process is also time-consuming, it is necessary to remove unreliable line segments.

[0172] Line segment combination method:

[0173] Since the LSD algorithm has no screening and combination mechanism, it is difficult to distinguish the situation where the line features are dense in the local area of ​​the image, so the extracted line segments are divided into many short line segments, which is not conducive to the construction of the scene map. Based on this, we propose a line segment combination method to locate and merge short line segments with close enough distance and similar main directions in the area with dense line features in the image, and extract high-quality long line segments.

[0174] like Figure 4 As shown, the specific combination method is:

[0175] Let L = {l1,l2,...,l n}(n=1,2,3…) is a set of line segments output by the LSD algorithm, l b is the baseline in L, and the rectangular area R is the baseline l b To improve the speed of the algorithm, we first sort all the line segments in L in descending order of length, and then select the line segments in the group with similar directions according to the main direction of the line segment histogram as the combined pre-selected line segments. Figure 4 Middle i and l j are the i-th and j-th line segments in L, l i Located in rectangle R, l j is outside rectangle R. Calculate l i The midpoint to l b The distance is denoted as d i ; Calculate l j The endpoint to l b The distance between the endpoints is denoted as d j1 d j2 d j3 d j4, select the minimum value, and use d min Indicates. Set the approximation standard of the line segment Φ l :

[0176] Φ l =μ|l b | (1)

[0177] Where μ is the approximation coefficient. If the calculated distance is greater than the approximation standard Φ l , then cancel the combination mechanism. Otherwise, calculate l b

[0178] and l i The angle difference θ, if the angle difference θ satisfies the angle approximation standard Φ θ , then perform line segment combination. Angle approximation standard Φ θ for:

[0179]

[0180] Where x, y and ξ are experimental parameters, and α is the normalization coefficient. Based on the above grouping method, long line segments with better quality can be obtained.

[0181] S3, pre-integrate the multiple groups of data of the four IMUs obtained in step S1, and calculate the position and speed of the IMUs corresponding to the two frames of images;

[0182] In this embodiment,

[0183] IMU Pre-integration:

[0184] The measurement model of IMU is:

[0185] ω m =ω b +b g +n g (3)

[0186]

[0187] Among them, ω m and are the measurements of the gyroscope and accelerometer, ω b and a w Represents the IMU angular velocity and linear acceleration in the b (body) and w (world) coordinate systems, respectively. represents the transformation matrix between coordinate system w and coordinate system b, n a , n g is Gaussian noise, b a , b g is the random walk measurement deviation model, and g w is the known gravity vector in the w system.

[0188] The integral formula of IMU integration is as follows:

[0189]

[0190]

[0191]

[0192] in They correspond to IMU from b k to b k+1 The position, velocity and attitude quaternion at the moment, is quaternion multiplication, Here we assume that the acceleration observation a m and the rotation matrix It is constant within the time of Δt. From the above IMU integral, it can be seen that the integral term at the next moment depends on the value at the previous moment. If b k The state at the moment changes, then b k+1 The state at the moment must be recalculated. In the optimized VIO algorithm, the state will change at each iteration and must be recalculated, resulting in excessive computation. Therefore, in order to eliminate the need for initial rotation Dependence, multiply the integral by The rotation matrix in the integral term becomes The initial value is the unit matrix, which eliminates the dependence on the initial value. Multiply both sides of equations (5), (6), and (7) by have:

[0193]

[0194]

[0195]

[0196] in:

[0197]

[0198]

[0199]

[0200] is the IMU pre-integration term that does not depend on the initial state.

[0201] S4, vision and IMU initialization. Use the initialization method of ORB_SLAM3;

[0202] In this embodiment, the initialization of vision and IMU is to find the gravity direction, initial velocity and bias (bias of gyroscope and accelerometer) in the inertial system at the beginning of the system. ORB-SLAM3 completes the initialization process through three processes, namely, the maximum a posteriori estimation of pure vision, the maximum a posteriori estimation of pure IMU and the maximum a posteriori estimation method of VI.

[0203] S5, mapping the measurement value probabilities obtained after initializing multiple IMUs (A, B, C, D) in step S4 to the virtual IMU (E);

[0204] In this embodiment,

[0205] Generation of virtual IMU E:

[0206] The results of IMU A, B, C, and D are randomly estimated by the least squares estimator and the probability of rotation acceleration between IMUs is marginalized to perform fusion to form a virtual IMU E. The specific situation is as follows:

[0207] According to the IMU measurement model (3) and (4), the IMU state vector and error state can be defined as follows:

[0208] The state of the IMU is defined as:

[0209]

[0210] in, Represents the quaternion transformation from the IMU coordinate system (b system) to the world coordinate system.

[0211] The IMU continuous-time motion dynamics is described as:

[0212]

[0213] The error state of the IMU is defined as:

[0214]

[0215] The standard additive error is defined for position, velocity, and bias. Quaternion local error The definition is as follows:

[0216]

[0217] represents an antisymmetric matrix, for The estimated value of .

[0218] In order to achieve probability estimation, a nonlinear equation for predicting state estimation is generally derived and a linearized equation describing the error state

[0219]

[0220]

[0221] (19) where F and G represent the state transfer matrix and noise Jacobian matrix of continuous-time linearization, n c is process noise and obeys Gaussian distribution. The generation of virtual IMU E is based on the information of IMU A, B, C, and D. The measurement value of virtual IMU E is used to replace ω in formula (18). m and a m The following is explained using two IMUs A and B.

[0222] If IMU A and B provide synchronized measurements ω mA ,ω mB , a mA , a mB , and their external parameters Assume that the external parameters of the virtual IMU relative to IMU A are The corresponding virtual gyroscope and accelerometer measurements can be generated as:

[0223] ω mV =N + ω m (20)

[0224] a mV =M(a m -S(ω mV )) (21) Among them,

[0225]

[0226]

[0227] Z T is the left adjacent space projection of Y, that is, Z T Y=0,N + is the generalized inverse matrix of N. In practice, for the four IMUs N, its generalized inverse matrix N + The calculation can be simplified as Using the generated virtual IMU measurement ω mV , a mV The prediction equation (18) for the IMU state estimate can be used without modification.

[0228] S6, model the line reprojection error as the distance from the two end points of the line segment to the projection line, and then minimize the point line reprojection error and IMU residual to obtain accurate pose estimation;

[0229] In this embodiment,

[0230] Error model establishment:

[0231] The present invention constructs an optimization function, uniformly optimizes the reprojection error of the fused point and line features, and systematically optimizes the IMU residual to obtain a more accurate camera pose. If the transformation matrix between the world coordinate system and the camera coordinate system is:

[0232]

[0233] Then the line L in the world coordinate system w L converted to the camera coordinate system c :

[0234]

[0235] Where: [.] × is a symmetric matrix of vectors.

[0236] The line segments in the image are transferred to the pixel coordinate plane by equation (25):

[0237]

[0238] Where: K′ is the projection matrix of the line feature.

[0239] For point features, the error between the projected point on the image and the actual measured point is defined as the point feature reprojection error. Assume x i is the jth feature point in the point cloud, then the reprojection error of the feature point in the i-th key frame can be expressed as:

[0240] e ij =x ij -π(ζ wj ,X wj ) (27)

[0241] Where: π is the projection function.

[0242] The line feature reprojection error is the distance from the endpoints of the line to the projection line. Assume that the two endpoints of the line segment are expressed as:

[0243] X b =(x b ,y b , 1) T , X e =(x e ,y e ,1) T (28)

[0244] Then the reprojection error of line segment l in the i-th image frame is:

[0245]

[0246] Then the error function of point and line fusion features can be expressed as:

[0247]

[0248]

[0249] Where: ρ is the Huber kernel function, Ω ij ,Ω l are the covariance matrices of point and line features respectively.

[0250] The final optimization function of the system is:

[0251] C=e Vimu +e pl (32)

[0252] S7. The designed algorithm was tested on the EuRoC dataset (with more low-texture scenes), the YorkUrban dataset (containing 102 urban environment images with rich line features), and our own collected corridor, studio and classroom datasets to obtain verification results.

[0253] In summary, the present invention is mainly applied to the positioning problem of the robot itself in SLAM technology to improve its robustness and positioning accuracy in weak texture environments. The present invention uses the more mature ORB_SLAM3 framework in the VIO solution. The visual fusion solution of multiple IMUs and point-line features will further improve the overall performance and make the algorithm more robust in low-texture environments.

[0254] The present invention proposes a line segment combination method, which locates and merges short line segments that are close enough and have similar main directions in the image line feature dense area, and extracts high-quality long line segments. The present invention improves the traditional LSD line segment feature extraction algorithm, prevents the scene line features from being excessively segmented into small line segments, avoids the problem of inaccurate feature extraction of the scene caused by the traditional LSD line feature extraction algorithm segmenting the long line segments in the scene into many small short line segments, and improves the effect of mapping. The improved algorithm can effectively solve this problem, increase the running speed, and consolidate the real-time requirements.

[0255] The present invention is to eliminate the initial rotation Dependence, multiply the integral by The rotation matrix in the integral term becomes The initial value is the unit matrix, which eliminates the dependence on the initial value. In the inertial mode of the present invention, the mapping thread will use the maximum a posteriori estimation to initialize and optimize the IMU parameters, thereby improving the robustness of the algorithm in the technical solution.

[0256] The present invention uses multiple IMUs (4) to assist vision for precise positioning. Compared with a single IMU, without significantly increasing or equalizing the amount of calculation, the proposed method maps the probability of data collected by 4 IMUs to a virtual IMU, and then fuses the visual information, thereby improving the positioning accuracy of the robot in a low-texture environment. The present invention solves the technical problems of low positioning and fusion vision accuracy, poor low-texture scene processing effect, and poor line segment extraction quality in the prior art.

[0257] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A point-line fusion positioning method based on multi-IMU, characterized in that: The method comprises: S1. Connect the 4IMU array integrated module to the camera to collect environmental image data and motion inertial data; S2, using the ORB algorithm to extract and match the point features in the environmental image data, using the line segment combination method to locate and combine short line segments, and extracting suitable long line segments to obtain a suitable LSD algorithm, the step S2 includes: S21, adjusting the hidden parameters of the preset LSD algorithm; S22, assuming that the LSD algorithm outputs a line segment group: L = {l1, l2, ..., l n }(n=1,2,3...), where l b is the baseline in L, and the preset rectangular area R is the baseline l b The impact area; S23, arranging all line segments in the line segment group L output by the LSD algorithm in descending order of length; S24, according to the main direction of the line segment histogram, the line segments in the group with similar directions are selected as combined pre-selected line segments according to the following logic, and the step S24 includes: S241, setting the approximate standard Φ of the line segment l : F l =μ|l b |, Among them, μ is the approximation coefficient; S242, if the calculated distance is greater than the approximate standard Φ l , then cancel the combination mechanism; S243, otherwise calculate l b and l i The angle difference θ; S244, determine the angle approximation standard Φ using the following logic θ , if the angle difference θ satisfies the angle approximation criterion Φ θ , then combine the short line segments: ， Where x, y and ξ are experimental setting parameters, and α is the normalization coefficient; S25, splicing the combined pre-selected line segments to obtain a suitable long line segment; S3, obtaining an IMU measurement model with a preset logic, obtaining initial rotation data from the motion inertial data, and performing IMU pre-integration according to the IMU measurement model and the initial rotation data; Wherein, the step S3 comprises: S31, obtain the IMU measurement model by the following logic processing: oh m =ω b +b g +n g Among them, ω m and are the measurements of the gyroscope and accelerometer, ω b and a w Respectively represent the IMU angular velocity and linear acceleration in the coordinate system b and w, where b is the body system and w is the world system. represents the transformation matrix between coordinate system w and coordinate system b, n a , n g is Gaussian noise, b a , b g is the random walk measurement deviation model, and g w is the known gravity vector in the w system; S32, integrate the IMU using the following logic: in They correspond to IMU from b k to b k+1 The position, velocity and attitude quaternion at the moment, is quaternion multiplication, Here we assume that the acceleration observation a m and the rotation matrix It is constant during the time Δt; S33, multiply both sides of the equation in step S32 by the following logic The rotation matrix in the integral term becomes The initial value is the unit matrix to eliminate the initial rotation Dependencies: ， Among them, the IMU pre-integration items that do not depend on the initial state include: S4, initializing vision and IMU according to the environmental image data and the motion inertial data; S5. Use a least squares estimator to randomly estimate the motion inertial data measured by the 4IMU array integration module to obtain and marginalize the rotation acceleration probability between IMUs, and fuse them to obtain a virtual IMU E; S6. Construct an optimization function to map the inter-IMU rotation acceleration probability to the virtual IMU E, thereby optimizing the reprojection error of point and line features and the residual of the virtual IMU E to improve the camera pose; S7. Use the logic designed in the previous steps to test the EuRoC dataset, YorkUrban dataset and indoor dataset to obtain the point-line fusion positioning verification results.

2. The point-line fusion positioning method based on multi-IMU according to claim 1 is characterized in that: The frequency of the 4IMU array integrated module in the step S1 includes: 200 Hz; the operating frequency of the camera includes: 10 Hz.

3. The point-line fusion positioning method based on multi-IMU according to claim 1 is characterized in that: The step S21 comprises: S211, using OpenCV to generate an N-layer Gaussian pyramid to represent the original image, downsampling the original image N-1 times, and blurring it N times; S212, extracting straight lines using the LSD algorithm at each layer of the Gaussian pyramid; S213. If the alignment area points in the closed rectangular support domain of the line segment are less than a preset threshold, the LSD algorithm is used to set a minimum density threshold to eliminate unreliable line segments, wherein the minimum density threshold is set to 0.6 to speed up the processing.

4. The point-line fusion positioning method based on multiple IMUs according to claim 3 is characterized in that: In the step S212, the scale of the Gaussian pyramid is adjusted to s=0.5, and the number of layers is adjusted to N=2.

5. The point-line fusion positioning method based on multiple IMUs according to claim 1, characterized in that: In step S4, the environmental image data and the motion inertial data are subjected to pure visual maximum a posteriori estimation, pure IMU maximum a posteriori estimation, and VI maximum a posteriori estimation to find the gravity direction, initial velocity, and gyroscope and accelerometer biases in the system's initial inertial system.

6. The point-line fusion positioning method based on multiple IMUs according to claim 1, characterized in that: The step S5 comprises: S51, using the IMU measurement model to define the IMU state vector and error state definition: The IMU state vector is defined by the following logic: ， in, Represents the quaternion transformation from the IMU coordinate system to the world coordinate system; S52. Describe the IMU continuous time motion dynamics using the following logic: S53, defining the error state with the following logic: S54, define the quaternion local error with the following logic ， in, represents an antisymmetric matrix, for An estimated value of S55, the nonlinear equation for predicting state estimation is obtained by the following logical processing and the linearized equation describing the error state To estimate the IMU probability: ， Where F and G represent the state transfer matrix and noise Jacobian matrix of continuous-time linearization, n c is the process noise, which follows a Gaussian distribution; S56, replace the gyroscope measurement value ω in step S55 with the measurement value of the virtual IMU E m and the accelerometer measurement a m , to generate the virtual IMU E, wherein the measurement values ​​of the gyroscope and accelerometer of the virtual IMU E in step S56 are generated using the following logic: oh mV =N + oh m a mV =M(a m -S(ω m V)) in, Z T is the left adjacent space projection of Y, that is, Z T Y=0,N + is the generalized inverse matrix of N; in practice, for the four IMUs N, its generalized inverse matrix N + Simplified calculation: Using the generated virtual IMU measurement ω mV , a mV The prediction equations using the IMU state estimate without modification.

7. The point-line fusion positioning method based on multiple IMUs according to claim 1, characterized in that: The step S6 comprises: S61. Define the transformation matrix between the world coordinate system and the camera coordinate system using the following logic: S62, obtain the straight line L in the world coordinate system by the following logic processing ω L converted to the camera coordinate system c : ， In the formula, [.] × is a symmetric matrix of vectors; S63, converting the line segments in the image to the pixel coordinate system plane using the logic described in step S62: ， Where K′ is the projection matrix of the line feature; S64, the error between the projected point on the image and the actual measured point is defined as the point feature reprojection error according to the following logic: e ij =x ij -π(ζ wj ,X wj ), In the formula, π is the projection function, x i is the jth feature point in the point cloud, i is the key frame number; S65. Define the line feature reprojection error using the following logic: ， Where, X b =(x b ,y b , 1) T , X e =(x e ,y e , 1) T represents the two endpoints of the line segment, and l represents the line segment; S66, according to the point feature reprojection error and the line feature reprojection error, the error function of the line fusion feature is obtained by the following logic processing: ， Where ρ is the Huber kernel function, Ω ij ,Ω l are the covariance matrices of point and line features respectively; S67. According to the error function of the line fusion feature, the final optimization function of the system is obtained by the following logic processing: C=e Vimu +e pl 。 8. The point-line fusion positioning method based on multiple IMUs according to claim 1, characterized in that: The step S7 includes: the EuRoC data set includes: a low-texture scene data set; the YorkUrban data set includes: an urban environment line feature data set.

9. A point-line fusion positioning system based on multiple IMUs, used to execute the point-line fusion positioning method based on multiple IMUs as described in any one of claims 1 to 8, characterized in that: The system comprises: A combined sensor device is used to connect the 4IMU array integrated module to the camera to collect and acquire environmental image data and motion inertial data; An improved LSD algorithm module is used to locate the combined short line segments by using the line segment combination method, thereby extracting the applicable long line segments to obtain the applicable LSD algorithm. The improved LSD algorithm module is connected to the combined sensor device, and the improved LSD algorithm module includes: A parameter adjustment module, used to adjust the hidden parameters of the preset LSD algorithm; The output line segment group and influence area module is used to set the LSD algorithm output line segment group as: L = {l1, l2, ..., l n }(n=1,2,3…), where l b is the baseline in L, and the preset rectangular area R is the baseline l b The output line segment group and the influence area module are connected with the parameter adjustment module; A line segment length arrangement module, used to arrange all line segments in the line segment group L output by the LSD algorithm in descending order of length, the line segment length arrangement module being connected to the output line segment group and the impact area module; A line segment combination module is used to select line segments with similar directions in a group as combined pre-selected line segments according to the main direction of the line segment histogram using the following logic. The line segment combination module is connected to the line segment length arrangement module and includes: Approximation standard module, used to set the approximation standard Φ of the line segment l : F l =μ|l b |, Among them, μ is the approximation coefficient; A non-approximate processing module, used to cancel the combination mechanism when the calculated distance is greater than the approximate standard Φ, and the non-approximate processing module is connected to the approximate standard module; The approximate angle difference processing module is used to calculate the distance less than or equal to the approximate standard Φ l When l b and l i The angle difference θ, the approximate angle difference processing module is connected with the approximate standard module; The angle approximation standard module is used to determine the angle approximation standard Φ with the following logic θ , if the angle difference θ satisfies the angle approximation criterion Φ θ , then combine the short line segments: ， Wherein x, y and ξ are experimental setting parameters, α is a normalization coefficient, and the angle approximation standard module is connected to the approximate angle difference processing module; A short line segment splicing module is used to splice the combined pre-selected line segments to obtain a suitable long line segment, and the short line segment splicing module is connected to the angle approximation standard module; An IMU pre-integration module, used to obtain an IMU measurement model with a preset logic, obtain initial rotation data from the motion inertial data, and perform IMU pre-integration according to the IMU measurement model and the initial rotation data, wherein the IMU pre-integration module is connected to the combined sensor device; An initialization module, used to initialize vision and IMU according to the environmental image data and the motion inertial data, wherein the initialization module is connected to the combined sensor device; An IMU probability module is used to randomly estimate the motion inertial data measured by the 4IMU array integration module using a least squares estimator to obtain and marginalize the probability of rotation acceleration between IMUs, and fuse them to obtain a virtual IMU E. The IMU probability module is connected to the initialization module; A camera pose optimization module is used to construct an optimization function to map the inter-IMU rotation acceleration probability to the virtual IMU E, thereby optimizing the reprojection error of point and line features and the IMU residual to improve the accuracy of camera pose estimation. The camera pose optimization module is connected to the IMU probability module; The data set verification module is used to test the EuRoC data set, YorkUrban data set and indoor data set using the logic designed in the above steps to obtain the point-line fusion positioning verification result. The data set verification module is connected to the IMU probability module and the camera pose optimization module.

Images