A crane inertial vision combined positioning method

Abstract

The invention discloses a crane inertial vision combined positioning method, comprising step one: collecting synchronous IMU data and camera images and preprocessing them; step two: extracting feature points in the camera image; step three: performing optical flow tracking and matching; step four: preprocessing the IMU data; step five: performing visual inertial joint initialization; step six: building a key frame database; step seven: optimizing the sliding window; step eight: performing loop detection and global optimization; the method adopts a fusion algorithm of vision and inertial measurement unit data, which can complement each other's advantages compared with a single sensor algorithm and ensure the robustness of the system; for complex environments, a camera and an inertial measurement unit are used for information fusion, and the recognition of feature points is enhanced by extracting multi-scale feature points and bidirectional optical flow, so as to improve the problem of feature point accuracy of the system in complex environments, improve the robustness of the system in the environment, and has the characteristics of high positioning accuracy and good robustness.

Description

Technical Field

[0001] The invention relates to the technical field of crane positioning, and in particular to an inertial vision combined positioning method for a crane. Background Art

[0002] Bridge cranes are widely used in various industries and are important transfer equipment. During use, due to the complex overall working environment of the crane, accidents such as collisions between personnel and loads or between the machine and loads are prone to occur, which are difficult to avoid and are the main safety hazards.

[0003] The crane trolley positioning system is one of the basic conditions for crane automation control. However, in the existing technology, the working mode of the trolley positioning system is relatively backward. Usually, the positioning method relies on the traditional trolley transmission mechanical structure for positioning, such as cooperating with encoders and other sensor components for trolley positioning. However, during the mechanical transmission process, transmission errors and losses are inevitable. The long-term accumulation of these errors is likely to lead to large errors, affecting the working stability and efficiency of the lifting equipment.

[0004] Therefore, there is an urgent need for a method for accurately positioning the crane trolley to ensure the normal and stable operation of the crane trolley. Summary of the invention

[0005] In view of the above-mentioned problems, the present invention aims to provide a crane inertial vision combined positioning method. This method adopts a fusion algorithm of vision and inertial measurement unit data, which can complement each other's advantages compared with a single sensor algorithm and ensure the strong robustness of the system. For complex environments, a camera and an inertial measurement unit are used for information fusion, and the feature point recognition is enhanced by extracting multi-scale feature points and bidirectional optical flow, thereby improving the problem of insufficient feature point accuracy in the system under complex environments, thereby improving the robustness of the system in this environment, and having the characteristics of high positioning accuracy and good robustness.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] A crane inertial vision combined positioning method, comprising:

[0008] Step 1: During the operation of the crane, synchronize IMU data and camera images are collected, and the collected images and IMU data are preprocessed;

[0009] Step 2: Use a multi-scale feature point detection method to extract feature points in the enhanced camera image;

[0010] Step 3: Perform optical flow tracking and matching on the extracted sub-pixel feature points;

[0011] Step 4: Preprocess the synchronized IMU data;

[0012] Step 5: Perform visual-inertial joint initialization on the obtained image feature points and the preprocessed IMU pre-integration model;

[0013] Step 6: Select the key frames of the image after visual-inertial joint initialization and build a key frame database;

[0014] Step 7: Optimize the visual inertial information in the sliding window using a sliding window optimization method;

[0015] Step 8: After obtaining the key frame database and optimizing the sliding window, loop detection and global optimization are performed to complete the crane positioning process.

[0016] Preferably, the process of processing the collected image in step 1 is to equalize the image using an adaptive histogram equalization method, and the process includes:

[0017] S101. Using an adaptive histogram equalization method, the template W is moved row by row on the acquired image A, and the center c(x0, y0) of the template W corresponds to the point f(x0, y0) on the image A;

[0018] S102. Calculate the histogram equalization change relationship of the template W area: g(x,y)=T(f(x,y), calculate the equalization corresponding pixel value of the template center point c(x0,y0): g(x0,y0)=T(f(x0,y0));

[0019] S103. Replace f(x0, y0) with g(x0, y0), calculate row by row, obtain an adaptive histogram equalization image of the entire image, and complete the equalization of the acquired image.

[0020] Preferably, the feature point extraction process described in step 2 includes the steps of:

[0021] S201. Firstly, the Fast and Harris detection algorithms are used to perform the first round of corner point detection on the enhanced camera image;

[0022] S202. Then, the sub-pixel corner detection method of the least squares estimation method is used to perform a secondary screening on the detected pixel-level corner points to obtain sub-pixel features with higher accuracy;

[0023] S203. Finally, the sub-pixel detection results are used to perform image edge constraints and optimize sub-pixel feature points.

[0024] Preferably, the process of performing optical flow tracking and matching on the extracted sub-pixel feature points in step 3 includes:

[0025] S301. In the image coordinate system, let the coordinate of pixel I at time (x, y). After dt time, its coordinate is (x+dx, y+dy)

[0026] I(x+dx,y+dy,t+dt)=I(x,y,t) (7)

[0027] After the first-order Taylor expansion of the left side of equation (7):

[0028]

[0029] S302. Based on the grayscale invariance assumption, we get:

[0030]

[0031] Taking the derivative of dx and dy with respect to dt at the same time, we get:

[0032]

[0033] S303. Denoted as u, as the speed of the pixel in the x direction of the image coordinate system; Denoted as v, as the speed of the pixel in the y direction of the image coordinate system; They are respectively recorded as the gradient of the image in the x and y directions, and are recorded as I x ,I y ; The gradient of image grayscale with respect to time is recorded as I t , as shown in formula (11):

[0034]

[0035] S304. In the optical flow method, suppose a window of size w×w contains w 2 pixels, if each pixel in the window has the same motion process, then there exists w 2 Equations:

[0036]

[0037] but:

[0038]

[0039] Equation (13) can be simplified as:

[0040]

[0041] Solving equation (14) by the least squares method yields:

[0042]

[0043] S304. Swap the current frame with the previous frame, perform reverse optical flow, compare the distances between the feature points matched by the reverse optical flow and the feature points matched by the forward optical flow, that is, determine whether the feature points of the two optical flow trackings are consistent, remove the feature points with large differences, and complete the optical flow tracking and matching of sub-pixel feature points.

[0044] Preferably, the process of preprocessing the synchronized IMU data described in step 4 includes:

[0045] S401. The measurement information of acceleration and angular velocity of the IMU data obtained after preliminary selection can be expressed as:

[0046]

[0047] Among them, a t(real) is the true value of IMU, is the acceleration bias, is the rotation matrix from the world coordinate system to the IMU coordinate system at time t, g w is the gravitational acceleration in the world coordinate system, b wt is the gyroscope bias, n a 、n w for noise;

[0048] S402. Assume that equation (16) obeys Gaussian distribution, then two consecutive frames of images b k , b k+1 At the corresponding time t k ,t k+1 Location speed The rotation relationship is as follows:

[0049]

[0050] in, It is the rotation matrix from the IMU coordinate system to the world coordinate system at time t;

[0051]

[0052] in: To convert the IMU coordinate system to the world coordinate system at time t expressed by quaternion,

[0053]

[0054] S403. Convert the reference coordinate system of the key frame at time t from the world coordinate system to the IMU coordinate system to obtain:

[0055]

[0056] in:

[0057]

[0058] Represent the position, speed and posture of the two frames respectively. Yes k The frame is rotated from the world coordinate system to the carrier system. The matrix is ​​obtained according to the above integral result. k+1 to b k The relative motion of

[0059] S404.IMU data is in a discrete state. The estimation of the IMU error state and the differential equation of the median integral are solved to obtain the pre-integration model under the IMU error state:

[0060]

[0061] in, are the position, attitude and velocity observations.

[0062] Preferably, the visual-inertial joint initialization process described in step 5 includes:

[0063] S501. First, the residual equation is constructed by the posture obtained from two consecutive frames of motion and IMU pre-integration to estimate the gyroscope bias, and then the velocity and gravity vector are optimized; the gyroscope bias optimization equation is:

[0064]

[0065] in, for b k+1 and b k The pose quaternion of two consecutive frames,

[0066] S502. After obtaining the gyroscope bias, the gyroscope bias is substituted into the pre-integration again for updating, and the state quantity to be initialized for the carrier in the Kth frame is obtained:

[0067]

[0068] For two consecutive frames we have the following equation:

[0069]

[0070] S503. Construct the measurement equation:

[0071]

[0072] The gravitational acceleration and carrier velocity of the initial camera coordinate system can be obtained by solving the least squares problem of the following equation:

[0073]

[0074] Preferably, the key frame selection process described in step 6 includes:

[0075] S601. When there are 15 consecutive image frames from the nearest inserted key frame or the tracking points in a key frame are less than the set minimum value, a new key frame will be inserted;

[0076] S602. If the number of feature points of the current frame is not less than 20, the current frame is taken as a key frame.

[0077] Preferably, the sliding window optimization process described in step 7 includes:

[0078] S701. Perform nonlinear optimization in the sliding window, where the state vector includes the position, velocity, attitude, acceleration bias, gyroscope bias, external parameters from the monocular camera to the inertial device, and the input three-dimensional landmark point coordinates of the n+1 cameras in the sliding window in the inertial device coordinate system. The above state vector needs to be optimized in the back end, and is defined as follows:

[0079]

[0080] Among them, x k is the state variable of IMU, for camera pose;

[0081] The error state quantity of the above state vector during back-end optimization is shown in formula (36):

[0082]

[0083] S704. Use the least squares method to construct the cost function for backend optimization, and use a tight coupling method to optimally estimate the visual information and inertial measurement information:

[0084]

[0085] Among them, (r B ,H P ) is the prior information. The three items in the objective function shown in formula (37) are: marginalized prior information, r B is the IMU residual function, r C is the residual function of visual measurement, and the distances used in the three items are all Mahalanobis distances;

[0086] S705. When constructing the information matrix, move the data with larger index values ​​to the lower right corner of the matrix to retain more prior knowledge. To accelerate marginalization, first marginalize the camera position part, and then marginalize the remaining information. The acceleration effect is achieved by using two threads.

[0087] Preferably, the loop detection and global optimization process described in step eight includes:

[0088] S801. Search the current key frame database for three key frames that are most similar to the current key frame;

[0089] S802. After obtaining similar keyframes, the current keyframe is matched with similar keyframes and keyframes with the most common map points with the similar keyframes. If there are few matching pairs, it is considered that there is no loop, otherwise, go to step S803;

[0090] S803. Verify the gravity direction of the key frame with better matching effect. If the direction difference is lower than the threshold, select the key frame with the smallest reprojection error as the loop frame;

[0091] S804. Perform global BA optimization to eliminate accumulated errors. If the key frame database is large, only the key frames between the loop frames are optimized;

[0092] S805. After loop detection, estimate the pose between the loop candidate frame and the current frame, and add the rotation of the nth frame of the loop information to the pose graph. With pan The nonlinear cost function is obtained:

[0093]

[0094] The above nonlinear cost function is optimized to obtain the globally optimized posture and complete the crane positioning process.

[0095] The beneficial effects of the present invention are as follows: the present invention discloses a crane inertial vision combined positioning method. Compared with the prior art, the present invention has the following improvements:

[0096] The present invention designs a crane inertial vision combined positioning method. The method adopts a fusion algorithm of vision and inertial measurement unit data, which can complement each other's advantages compared with a single sensor algorithm and ensure the strong robustness of the system. For complex environments, a camera and an inertial measurement unit are used for information fusion, and the feature point recognition is enhanced by extracting multi-scale feature points and bidirectional optical flow, thereby improving the problem of insufficient feature point accuracy of the system in complex environments, improving the robustness of the system in this environment, and having the advantages of high positioning accuracy and good robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0097] Figure 1 The figure is a flow chart of the algorithm of the crane inertial vision combined positioning method of the present invention. DETAILED DESCRIPTION

[0098] In order to enable those skilled in the art to better understand the technical solution of the present invention, the technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0099] Embodiment 1: refer to the attached Figure 1 A crane inertial vision combined positioning method is shown, comprising:

[0100] Step 1: During the operation of the crane, synchronous IMU data and camera images are collected, and the collected images are equalized and enhanced, including:

[0101] (1) The method for equalizing the image is to process the image using an adaptive histogram equalization method. The process of the image histogram equalization includes:

[0102] S101. Using an adaptive histogram equalization method, the template W is moved row by row on the acquired image A, and the center c(x0, y0) of the template W corresponds to the point f(x0, y0) on the image A;

[0103] S102. Calculate the histogram equalization change relationship of the template W area: g(x,y)=T(f(x,y), calculate the equalization corresponding pixel value of the template center point c(x0,y0): g(x0,y0)=T(f(x0,y0));

[0104] S103. Replace f(x0, y0) with g(x0, y0), calculate row by row, obtain an adaptive histogram equalization image of the entire image, and complete the equalization of the acquired image;

[0105] (2) The processing process of the synchronized IMU data includes:

[0106] For the acquisition of synchronous IMU data, since the image information frequency is quite different from the IMU information acquisition frequency, the IMU data closest to the image frame is selected as the inertial information data to achieve time synchronization for data acquisition;

[0107] Step 2: Extract feature points in the enhanced camera image using a multi-scale feature point detection method based on pixel / sub-pixel fusion, including the following steps:

[0108] S201. First, two pixel-level feature detection algorithms, Fast and Harris, are used to perform the first round of corner detection on the enhanced camera image.

[0109] (1) First, use Fast to perform the first round of corner point detection on the image and extract the corner point features of the image; (2) After obtaining the corner point features of the image, use the Harris algorithm to optimize the point features;

[0110] S202. Then, the sub-pixel corner detection method of the least squares estimation method is used to perform a secondary screening on the detected pixel-level corner points to obtain sub-pixel features with higher accuracy;

[0111] S203. Finally, the sub-pixel detection results are used to perform image edge constraints and optimize sub-pixel feature points;

[0112] Step 3: Perform optical flow tracking and matching on the extracted sub-pixel feature points

[0113] S301. In the image coordinate system, let the coordinate of pixel I at time (x, y). After dt time, its coordinate is (x+dx, y+dy), that is,

[0114] I(x+dx,y+dy,t+dt)=I(x,y,t) (7)

[0115] After the first-order Taylor expansion of the left side of equation (7):

[0116]

[0117] S302. Since the optical flow method is based on the grayscale invariance assumption, we can obtain:

[0118]

[0119] Taking the derivative of dx and dy with respect to dt at the same time, we get:

[0120]

[0121] S303. Denoted as u, as the speed of the pixel in the x direction of the image coordinate system; Denoted as v, as the speed of the pixel in the y direction of the image coordinate system; They are respectively recorded as the gradient of the image in the x and y directions, and are recorded as I x ,I y ; The gradient of image grayscale with respect to time is recorded as I t , as shown in formula (11):

[0122]

[0123] S304. In the optical flow method, suppose a window of size w×w contains w 2 pixels, if each pixel in the window has the same motion process, then there exists w 2 Equations:

[0124]

[0125] but:

[0126]

[0127] Equation (13) can be simplified as:

[0128]

[0129] Solving equation (14) by the least squares method yields:

[0130]

[0131] S304. Swap the current frame with the previous frame, perform reverse optical flow, compare the distance between the feature points matched by the reverse optical flow and the feature points matched by the forward optical flow, that is, determine whether the feature points of the two optical flow trackings are consistent, remove the feature points with large differences, and complete the optical flow tracking matching of sub-pixel feature points;

[0132] Step 4: Preprocess the synchronized IMU data

[0133] S401. The measurement information of acceleration and angular velocity of the IMU data obtained after preliminary selection can be expressed as:

[0134]

[0135] Among them, a t(real) is the true value of IMU, is the acceleration bias, is the rotation matrix from the world coordinate system to the IMU coordinate system at time t, g w is the gravitational acceleration in the world coordinate system, b wt is the gyroscope bias, n a 、n w for noise;

[0136] S402. Assuming that equation (16) obeys Gaussian distribution, then two consecutive frames of image b k , b k+1 At the corresponding time t k ,t k+1 Location speed The rotation relationship is shown in formula (17):

[0137]

[0138] in, It is the rotation matrix from the IMU coordinate system to the world coordinate system at time t;

[0139]

[0140] in: To convert the IMU coordinate system to the world coordinate system at time t expressed by quaternion,

[0141]

[0142] S403. Convert the reference coordinate system of the key frame at time t from the world coordinate system to the IMU coordinate system to obtain:

[0143]

[0144] in:

[0145]

[0146] According to the above integral results, we can get b k+1 to b k This means that no matter how the position, speed, and rotation of the keyframes are adjusted during subsequent optimization, the to produce an impact so that the integration process does not repeat itself. Represent the position, speed and posture of the two frames respectively. Yes k The frame is rotated from the world coordinate system to the carrier system. The matrix is ​​obtained according to the above integral result. k+1 to b k The relative motion of

[0147] S404.IMU data is in a discrete state. Based on the estimation of the IMU error state and the solution of the differential equation of the median integral, the pre-integration model under the IMU error state can be obtained as follows:

[0148]

[0149] in, are the position, attitude and velocity observations;

[0150] Step 5: Perform visual-inertial joint initialization on the obtained image feature points and the preprocessed IMU pre-integration model

[0151] S501. First, calculate the relative displacement and rotation of the camera through feature point matching

[0152] First, the residual equation is constructed by the posture obtained from two consecutive frames of motion and IMU pre-integration to estimate the gyroscope bias, and then the velocity and gravity vector are optimized; the gyroscope bias optimization equation is as follows:

[0153]

[0154] in, for bk+1 and b k The pose quaternion of two consecutive frames,

[0155] S502. After the gyroscope bias is obtained, the gyroscope bias is substituted into the pre-integration again for updating, so the state quantity to be initialized for the carrier in the Kth frame is obtained as follows:

[0156]

[0157] For two consecutive frames we have the following equation:

[0158]

[0159] S503. Construct the measurement equation based on this:

[0160]

[0161] The gravitational acceleration and carrier velocity of the initial camera coordinate system can be obtained by solving the least squares problem of the following equation:

[0162]

[0163] Step 6: Select the key frames of the image after visual-inertial joint initialization, key frame database

[0164] S601. When there are 15 consecutive image frames from the nearest inserted key frame or the tracking points in a key frame are less than the set minimum value, a new key frame will be inserted;

[0165] S602. The number of feature points of the current frame is not less than 20, and the current frame is used as a key frame;

[0166] Step 7: Use the sliding window optimization method to optimize the visual inertial information in the sliding window

[0167] S701. Perform nonlinear optimization in the sliding window, where the state vector includes the position, velocity, attitude, acceleration bias, gyroscope bias, external parameters from the monocular camera to the inertial device, and the input three-dimensional landmark point coordinates of the n+1 cameras in the sliding window in the inertial device coordinate system. The above state vector needs to be optimized in the back end and can be defined as shown in formula (35):

[0168]

[0169] The error state quantity of the above state vector during back-end optimization is shown in formula (36):

[0170]

[0171] In the visual inertial positioning system, the front end continuously extracts and matches the point and line features in the image, which gradually increases the constraints on the back end and the dimension of the estimated quantity to be optimized in formula (35).

[0172] S703. In order to bring the residual terms of multiple estimators to be optimized into an objective function, the least square method is used to construct the cost function of the back-end optimization; the maximum a posteriori estimation method in formula (37) is used. The objective function uses a tight coupling method to optimally estimate the visual information and inertial measurement information. The function formula is as follows:

[0173]

[0174] Among them, (r B ,H P ) is the prior information. The three items in the objective function shown in formula (37) are: marginalized prior information, r B is the IMU residual function, r C is the residual function of visual measurement, and the distances used in the three items are all Mahalanobis distances;

[0175] S704. When constructing the information matrix, move the data with larger index values ​​to the lower right corner of the matrix to retain more prior knowledge; since the amount of camera measurement information is quite different from the IMU measurement information, in order to accelerate marginalization, the camera position part is first marginalized, and then the remaining information is marginalized, which can be accelerated by using two threads;

[0176] Step 8: After obtaining the keyframe database and optimizing the sliding window, perform loop detection and global optimization

[0177] S801. Search the current key frame database for three key frames that are most similar to the current key frame;

[0178] S802. After obtaining similar keyframes, the current keyframe is matched with similar keyframes and keyframes with the most common map points with the similar keyframes. If there are few matching pairs, it is considered that there is no loop, otherwise, go to step S803;

[0179] S803. Verify the gravity direction of the key frame with better matching effect. If the direction difference is lower than the threshold, select the key frame with the smallest reprojection error as the loop frame;

[0180] S804. Perform global BA optimization to eliminate accumulated errors. If the key frame database is large, only the key frames between the loop frames are optimized;

[0181] S805. After loop detection, estimate the pose between the loop candidate frame and the current frame, and add the rotation of the nth frame of the loop information to the pose graph. With pan The nonlinear cost function is obtained:

[0182]

[0183] The above nonlinear cost function is optimized to obtain the globally optimized posture and complete the crane positioning process.

[0184] Preferably, the process of extracting camera image corner point features using the Fast method in step S201 includes:

[0185] (1) Taking a pixel point p in the image as the center, determine a circular detection neighborhood with a radius of 3 pixels;

[0186] (2) Set the threshold T and select the four pixel values ​​1, 5, 9, and 13 in the circular detection area If three consecutive pixel values ​​of the four pixels do not satisfy Then the pixel point is a corner point; and the FAST score value of the pixel point p is recorded;

[0187] (3) Repeat the above steps (1) to (2) to check whether there are other corner points that meet the conditions in the selected neighborhood;

[0188] (4) The non-maximum suppression method is used to select the point with the highest Fast score in a certain area as the corner point of the area.

[0189] Preferably, the process of extracting camera image corner points using the Harris algorithm described in step S201 includes:

[0190] (1) After obtaining the camera image corner features using the Fast algorithm, the pixels in the area are calculated using vertical and horizontal difference operators in the sliding window of the image, and the filtering is used to optimize I x ,I y , thus obtaining the values ​​of the four parameters in the M matrix;

[0191] (2) To reduce the influence of isolated points and protrusions in the window area, Gaussian smoothing filtering is performed on the four parameters in the M matrix;

[0192] (3) Obtain two eigenvalues ​​of the M matrix to obtain the improved corner point response function R of the pixel;

[0193] (4) According to the principle of non-maximum suppression, all R values ​​judged as corner points in the region are compared, and the maximum value is selected as the corner point of the region;

[0194] (5) When the R value is greater than the set threshold and is the maximum value of the window area, the pixel point is defined as a corner point.

[0195] Preferably, the process of performing secondary screening on the detected pixel-level corner points by using the sub-pixel-level corner point detection method in step S202 includes:

[0196] (1) Assume that the original image is in pixels, with n rows and m columns. The area surrounded by four pixels is divided into (k+1)×(k+1) sub-pixel grids. The original image becomes N rows and M columns, where:

[0197]

[0198] (2) Assume that an arbitrary initial point p is selected in the neighborhood determined by the sub-pixel point q to be determined. If point p is inside the grayscale uniform region, its pixel gradient is 0; if point p is in the edge region, the vector Perpendicular to the pixel gradient in the region;

[0199] (3) In the above two cases, the vector The product with the gradient vector of the region is 0; G i is the pixel gradient vector in the area, p i is the i-th pixel in the sub-pixel neighborhood, (p i -q) is from point q to point p i The vector of the point, so the conditions that the sub-pixel corner point must satisfy are:

[0200] G i ×(p i -q)=0 (2)

[0201] Solving the equation for q gives:

[0202]

[0203] (4) The corner point selected by the Harris feature detection algorithm is used as the initial point q0. With q0 as the center, a window of a certain size is determined. All points in the window can be used as p i ;

[0204] (5) According to the Sobel operator convolution, we can get p i The pixel gradient G of the point i for:

[0205] G i =[dx dy] (4)

[0206] (6) Sum the pixel gradients of each point and rewrite equation (3) as follows:

[0207]

[0208] (7) Since each p iThe distance between points q is different. i Gaussian weight ω is introduced at point i , formula (5) can be further updated as:

[0209]

[0210] The algorithm flow of the above process is:

[0211] (1) Based on the corner point q0 detected by Harris, a window of a certain size is taken as the center, and the Sobel operator is used to calculate the p in the window i The gradient of a point;

[0212] (2) Establish a system equation and solve it to obtain a new sub-pixel point q1;

[0213] (3) Taking q1 as the center, iterate again to calculate the new sub-pixel point q i ; The algorithm terminates when the specified number of iterations or the specified result accuracy is reached.

[0214] Embodiment 2: Different from the above embodiment 1, the positioning method described in the above embodiment 1 is verified by this embodiment:

[0215] In order to verify the accuracy of the proposed algorithm, the EuRoC public dataset is used to verify the algorithm. The verification results are shown in Table 1.

[0216] Table 1: Accuracy of different algorithms in 5 scenarios in the EuRoC dataset

[0217]

[0218]

[0219] Compared with VINS-mono, the improved algorithm has basically improved in the three evaluation indicators of RMSE, SSE, and STD of the five scenes in the EuRoC dataset, proving that the positioning accuracy of the algorithm has been improved.

[0220] The above shows and describes the basic principles, main features and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments, and the above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A crane inertial vision combined positioning method, characterized in that: include Step 1: During the operation of the crane, synchronize IMU data and camera images are collected, and the collected images and IMU data are preprocessed; Step 2: Use a multi-scale feature point detection method to extract feature points in the enhanced camera image; S201. First, two pixel-level feature detection algorithms, Fast and Harris, are used to perform the first round of corner point detection on the enhanced camera image: (1) First, use Fast to perform the first round of corner detection on the image to extract the corner features of the image; (2) After obtaining the corner point features of the image, the Harris algorithm is used to optimize the point features; S202. Then, the sub-pixel corner detection method of the least squares estimation method is used to perform a secondary screening on the detected pixel-level corner points to obtain sub-pixel features with higher accuracy; S203. Finally, the sub-pixel detection results are used to perform image edge constraints and optimize sub-pixel feature points; Step 3: Perform optical flow tracking and matching on the extracted sub-pixel feature points; S301. In the image coordinate system, let the coordinate of pixel I at time (x, y). After dt time, its coordinate is (x+dx, y+dy) I(x+dx,y+dy,t+dt)=I(x,y,t) (7) After the first-order Taylor expansion of the left side of equation (7): S302. Based on the grayscale invariance assumption, we get: Taking the derivative of dx and dy with respect to dt at the same time, we get: S303. Denoted as u, as the speed of the pixel in the x direction of the image coordinate system; Denoted as v, as the speed of the pixel in the y direction of the image coordinate system; They are respectively recorded as the gradient of the image in the x and y directions, and are recorded as I x ,I y ; The gradient of image grayscale with respect to time is recorded as I t , as shown in formula (11): S304. In the optical flow method, suppose a window of size w×w contains w 2 pixels, if each pixel in the window has the same motion process, then there exists w 2 Equations: but: Equation (13) can be simplified as: Solving equation (14) by the least squares method yields: S304. Swap the current frame with the previous frame, perform reverse optical flow, compare the distance between the feature points matched by the reverse optical flow and the feature points matched by the forward optical flow, that is, determine whether the feature points of the two optical flow trackings are consistent, remove the feature points with large differences, and complete the optical flow tracking matching of sub-pixel feature points; Step 4: Preprocess the synchronized IMU data; Step 5: Perform visual-inertial joint initialization on the obtained image feature points and the preprocessed IMU pre-integration model; Step 6: Select the key frames of the image after visual-inertial joint initialization and build a key frame database; Step 7: Optimize the visual inertial information in the sliding window using a sliding window optimization method; Step 8: After obtaining the key frame database and optimizing the sliding window, loop detection and global optimization are performed to complete the crane positioning process.

2. The crane inertial vision combined positioning method according to claim 1 is characterized by: The process of preprocessing the collected image in step 1 is to equalize the image using an adaptive histogram equalization method, and the process includes: S101. Using an adaptive histogram equalization method, the template W is moved row by row on the acquired image A, and the center c(x0, y0) of the template W corresponds to the point f(x0, y0) on the image A; S102. Calculate the histogram equalization change relationship of the template W area: g(x,y)=T(f(x,y), calculate the equalization corresponding pixel value of the template center point c(x0,y0): g(x0,y0)=T(f(x0,y0)); S103. Replace f(x0, y0) with g(x0, y0), calculate row by row, obtain an adaptive histogram equalization image of the entire image, and complete the equalization of the acquired image.

3. The crane inertial vision combined positioning method according to claim 1 is characterized by: The process of preprocessing the synchronized IMU data described in step 4 includes: S401. The measurement information of acceleration and angular velocity of the IMU data obtained after preliminary selection can be expressed as: Among them, a t(real) is the true value of IMU, is the acceleration bias, is the rotation matrix from the world coordinate system to the IMU coordinate system at time t, g w is the gravitational acceleration in the world coordinate system, b wt is the gyroscope bias, n a 、n w for noise; S402. Assume that equation (16) obeys Gaussian distribution, then two consecutive frames of images b k , b k+1 At the corresponding time t k ,t k+1 Location speed The rotation relationship is as follows: in, It is the rotation matrix from the IMU coordinate system to the world coordinate system at time t; in: To convert the IMU coordinate system to the world coordinate system at time t expressed by quaternion, S403. Convert the reference coordinate system of the key frame at time t from the world coordinate system to the IMU coordinate system to obtain: in: Represent the position, speed and posture of the two frames respectively. Yes k The frame is rotated from the world coordinate system to the carrier system. The matrix is ​​obtained according to the above integral result. k+1 to b k The relative motion of S404.IMU data is in a discrete state. The estimation of the IMU error state and the differential equation of the median integral are solved to obtain the pre-integration model under the IMU error state: in, are the position, attitude and velocity observations.

4. The crane inertial vision combined positioning method according to claim 1 is characterized by: The visual-inertial joint initialization process described in step 5 includes S501. First, the residual equation is constructed by the posture obtained by two consecutive frames of motion and IMU pre-integration, the gyroscope bias is estimated, and then the velocity and gravity vector are optimized; The gyroscope bias optimization equation is: in, for b k+1 and b k The pose quaternion of two consecutive frames, S502. After obtaining the gyroscope bias, the gyroscope bias is substituted into the pre-integration again for updating, and the state quantity to be initialized for the carrier in the Kth frame is obtained: For two consecutive frames we have the following equation: S503. Construct the measurement equation: The gravitational acceleration and carrier velocity of the initial camera coordinate system can be obtained by solving the least squares problem of the following equation:

5. The crane inertial vision combined positioning method according to claim 1 is characterized by: The key frame selection process described in step 6 includes: S601. When there are 15 consecutive image frames from the nearest inserted key frame or the tracking points in a key frame are less than the set minimum value, a new key frame will be inserted; S602. If the number of feature points of the current frame is not less than 20, the current frame is taken as a key frame.

6. The crane inertial vision combined positioning method according to claim 1 is characterized by: The sliding window optimization process described in step 7 includes: S701. Perform nonlinear optimization in the sliding window, where the state vector includes the position, velocity, attitude, acceleration bias, gyroscope bias, external parameters from the monocular camera to the inertial device, and the input three-dimensional landmark point coordinates of the n+1 cameras in the sliding window in the inertial device coordinate system. The above state vector needs to be optimized in the back end, and is defined as follows: Among them, x k is the state variable of IMU, for camera pose; The error state quantity of the above state vector during back-end optimization is shown in formula (36): S704. Use the least squares method to construct the cost function for backend optimization, and use a tight coupling method to optimally estimate the visual information and inertial measurement information: Among them, (r B ,H P ) is the prior information. The three items in the objective function shown in formula (37) are: marginalized prior information, r B is the IMU residual function, r C is the residual function of visual measurement, and the distances used in the three items are all Mahalanobis distances; S705. When constructing the information matrix, move the data with larger index values ​​to the lower right corner of the matrix to retain more prior knowledge. To accelerate marginalization, first marginalize the camera position part, and then marginalize the remaining information. The acceleration effect is achieved by using two threads.

7. The crane inertial vision combined positioning method according to claim 1 is characterized by: The loop detection and global optimization process described in step 8 includes S801. Search the current key frame database for three key frames that are most similar to the current key frame; S802. After obtaining similar keyframes, the current keyframe is matched with similar keyframes and keyframes with the most common map points with the similar keyframes. If there are few matching pairs, it is considered that there is no loop, otherwise, go to step S803; S803. Verify the gravity direction of the key frame with better matching effect. If the direction difference is lower than the threshold, select the key frame with the smallest reprojection error as the loop frame; S804. Perform global BA optimization to eliminate the accumulated error. If the key frame database is large, only the key frames between the loop frames are optimized. S805. After loop detection, estimate the pose between the loop candidate frame and the current frame, and add the rotation of the nth frame of the loop information to the pose graph. With pan The nonlinear cost function is obtained: The above nonlinear cost function is optimized to obtain the globally optimized posture and complete the crane positioning process.

Images