A direct method-based multi-sensor fusion SLAM positioning method

Abstract

The application discloses a multi-sensor fusion SLAM positioning method based on a direct method, and relates to the field of mobile robot positioning and navigation. The direct method is used for camera tracking by using the pixel point information with obvious gradient in an image, IMU pre-integration information is used as an initial pose, and the direct method is provided with an initial value. According to whether tracking is successful, the image frame is divided into a key frame and a non-key frame. A visual factor graph is constructed, an IMU residual term and a GPS residual term are inserted into the visual factor graph to construct a new fusion factor graph, and the optimization process of the system algorithm is completed through fusion factor graph optimization. A rear-end optimization strategy based on a sliding window optimization is used, the calculation complexity of rear-end pose optimization is fixed, a multi-sensor tight coupling fusion strategy is used, the advantages of marginalization are retained and the defects of marginalization are made up by using old information, and the system accuracy is improved. The system can work in a pure vision mode. Even if a GPS signal cannot be normally received, the system can continue to work in a vision inertial odometer mode.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot positioning and navigation, and in particular to a SLAM positioning method based on multi-sensor fusion of a direct method. Background Art

[0002] With the increasing application of robots, autonomous navigation has become a bottleneck limiting their operational capabilities. Positioning, the foundation and basis for autonomous navigation, is a core technology that restricts widespread robotic adoption. Consequently, simultaneous localization and mapping (SLAM) has become a research hotspot. SLAM involves continuously tracking the trajectory of a moving robot in an unknown environment while simultaneously mapping it. SLAM requires the support of various sensors. Because a single sensor alone cannot guarantee long-term stable operation in a variety of environments, methods that fuse data from multiple sensors for localization in unknown environments are gaining increasing research and application.

[0003] Current mainstream SLAM algorithms utilize sensors including monocular, binocular, and depth cameras, IMUs, inertial measurement units (IMUs), lidar, and sometimes GPS signals and odometry fusion. Furthermore, for visual SLAM, there are feature point methods that calculate descriptors based on sparsely distributed feature points, direct methods that rely on pixel information with explicit gradient information, and semi-direct methods that rely on feature points but do not calculate descriptors. IMUs and monocular cameras each have their own advantages and disadvantages, but they are also complementary. Fusion offers the following advantages: it addresses the scale uncertainty of monocular cameras; it solves the inability of monoculars to estimate pure rotation, and reduces the influence of texture, lighting, and dynamic environments on visual SLAM. It also simplifies IMU bias estimation and eliminates integration drift. Therefore, multi-sensor fusion can construct faster, more accurate, and more robust algorithms, which has significant research and practical value. Multi-sensor fusion is often categorized into filtering-based and factor graph optimization-based approaches, as well as tightly coupled and loosely coupled fusion solutions. This allows users to choose the appropriate fusion solution for their device and upgrade its performance. Summary of the Invention

[0004] The purpose of the present invention is to provide a multi-sensor fusion SLAM positioning method based on the direct method, improve the robustness and operation speed of the existing monocular vision IMU fusion algorithm, add GPS for global fusion positioning, add optimization constraints so that its coordinates can be aligned with the real world, and also improve the accuracy of the system.

[0005] The present invention comprises the following steps:

[0006] 1) Obtain the image frame of monocular camera, IMU output information and GPS latitude and longitude information, calculate the current world coordinate system coordinate from the GPS latitude and longitude information, align the foregoing information to a structure data according to the time stamp, and input into the system for operation;

[0007] 2) Perform the initialization process of the visual part algorithm, construct the image pyramid of the first frame image by using the structure data obtained in step 1), screen the candidate gradient points by gradient threshold value, project the candidate gradient points of the first frame initialization into the second frame, estimate the initial relative motion by using the image pyramid, and initialize the pose transformation from the IMU coordinate system to the world coordinate system; according to the photometric error function formula, optimize the two frames, and determine whether the initialization is completed by the displacement size of the optimization result;

[0008] 3) Perform the image tracking process of the algorithm, use the pose between the two frames obtained by IMU pre-integration as the initial pose, perform the optimization calculation based on the photometric error on the front and rear two frames of images based on the Gauss-Newton method, and track the motion of the camera; in the tracking process, whether the current frame is used as a new key frame is determined according to three conditions, including: ① the system is just started, and the number of stored frames is 1; ② the translation, rotation or photometric change ratio of the current frame pixel is too large; ③ the time interval between the current frame and the previous key frame exceeds the maximum time interval between key frames;

[0009] a. If it is determined that the current frame is a key frame, the gradient points extracted from the current frame are subjected to an activation operation, that is, the inverse depth of the gradient points is optimized and calculated by using a depth filter to make it converge, and the key frame is added to the visual factor graph for sliding window optimization, and the edge is supplemented by using Schur;

[0010] b. If it is determined that the current frame is a non-key frame, only the pose estimation between the current frame and the previous frame is used to update the inverse depth, and the system runs in a "visual only" mode;

[0011] 4) Perform the visual-IMU-GPS data fusion process of the algorithm, based on the visual factor graph described in step 3), copy the factor graph as a fusion factor graph, add IMU pre-integration factors and GPS factors, and use lagging edge to perform joint optimization; the fusion factor graph also uses the sliding window optimization method, but the edge strategy is to delay n key frames, and then perform lagging edge with the edge sequence of the visual factor graph to achieve the purpose of keeping the sliding window size; the lagging edge not only makes up for the defects of the edge and retains the advantages of the edge while ensuring the accuracy, and the system enters the "visual-IMU-GPS" model operation.

[0012] Step 2) specifically includes the following steps:

[0013] 2.1) During initialization, an image pyramid is built for the first frame of the first two frames. The pyramid is traversed and the camera intrinsic parameters of each layer are calculated. The first layer image is divided into multiple grids with a grid size of 32*32. Then, the light intensity difference dx and dy in the x and y directions of the pixel points in the pixel block grid are calculated, and the sum of the squares of the light intensity differences is obtained as the gradient. And count the gradients of each pixel block, and select the median as the gradient threshold of the pixel block to extract candidate points;

[0014] 2.2) In the first layer of the image, traverse each n*n pixel block, and randomly select a direction in the pixel coordinate system to calculate the gradient dI of the pixels in the block. Record the pixel position p with the largest gradient that is greater than the threshold as the candidate point; if too few pixels are extracted, reduce the block size by 0.5n*0.5n and resample it recursively; otherwise, increase the block size to 2n*2n and resample it again; and so on for recursion. In the remaining layers of the pyramid, the same point selection is performed, and the following two conditions must be met: ① The gradient must be greater than the set threshold; ② The pixel point in the pixel coordinate system should be the largest point in the four directions of x, y, x+y, and xy. Finally, associate each layer of the image pyramid;

[0015] 2.3) Calculate the transformation quaternion of the current camera coordinate system and gravity alignment; Based on the IMU information obtained in step 1) and aligned with the image time, obtain the acceleration data for a period of time and take the average value as the gravity direction of the current frame; At the same time, construct a fixed-size sliding window to save the gravity direction of each frame, use the sliding mean filter to optimize the IMU gravity vector, and calculate the rotation of the gravity alignment of the current frame with the actual gravity direction [0,0,9.8]

[0016] 2.4) Optimize the second frame of the initial two frames to complete the initialization; the second frame is based on the points of the first frame to construct the photometric error function:

[0017]

[0018] Where N p is the pixel set used in the algorithm, ‖·‖ is the Huber norm, p represents the projection of the spatial point p corresponding to the pixel point in frame i on frame j, t j represents the exposure time of the jth frame; in order to make the algorithm applicable to image sequences with unknown exposure time, the affine photometric formula is used Perform the photometric normalization operation; then, starting from the top layer of the image pyramid to the bottom layer, use the unit matrix as the initial value to optimize each layer of the image pyramid through the Gauss-Newton method, and determine whether the displacement is large enough; if the displacement is large enough and the number of tracking frames in this step is greater than 5, the initialization is completed, and the first frame is then set as the key frame, the gradient points extracted from the first frame are activated, and added to the sliding window optimization of the visual factor graph; otherwise, repeat steps 2.1) to 2.4).

[0019] Step 3) specifically includes the following steps:

[0020] 3.1) Enter the tracking process of the algorithm: Pre-integrate the IMU data obtained in step 1) and aligned with the image time to obtain the pose transformation between the two frames. The pre-integration formula is:

[0021]

[0022]

[0023]

[0024] Where, and for b k+1 Relative to b k The algorithm uses the pose obtained by IMU pre-integration as the initial pose of the direct method;

[0025] 3.2) Use the initial pose to project the points in the reference frame into the current frame and construct a photometric error function. With minimizing the photometric error as the optimization goal, use the Levenberg-Marquardt algorithm to iteratively optimize the candidate points from the top of the image pyramid downwards, i.e., iteratively optimize the inverse depth, photometric parameters, and pose from coarse to fine. There are several different optimization operations: ① If the photometric error is greater than 60% of the energy threshold, the threshold is increased and optimized again; ② If the energy of a layer in the pyramid is greater than 2 times the minimum value, it is directly judged that tracking has failed; ③ If the photometric error meets the conditions, tracking is successful.

[0026] 3.3) Determine whether the current frame can become a keyframe based on three preset conditions: ① The system has just started and the number of stored frames is 1; ② The translation, rotation, or luminosity change ratio of the current frame's pixels is too large; ③ The time between the current frame and the previous keyframe exceeds the maximum time interval between keyframes;

[0027] 3.4) If the current frame becomes a non-keyframe, update the inverse depth range of the candidate point in step 2.2); the range of the epipolar line search is obtained based on the inverse depth range to calculate the image gradient and the size of the epipolar line angle; if the angle is too large, it means that the directions of the two frames tend to be perpendicular, and it is believed that the error of the result will be large, so the candidate point parameters will not be updated; therefore, when the angle is appropriate, the position p1 with the smallest photometric error and the second smallest position p2 with a radius greater than 2 will be searched on the epipolar line according to a fixed step size, and the latter is divided by the former to obtain a ratio As a mass attribute, the greater the mass, the better the position; based on this, a further detailed search is performed, and the Gauss-Newton method is used to optimize along the epipolar line until the increment is small enough. At this time, the best position of the projection is obtained, and the new inverse depth range is calculated using this best position;

[0028] 3.5) If the current frame needs to become a keyframe, first marginalize the keyframes in the keyframe sequence that meet the following three conditions: ① the photometric parameters change significantly compared to the reference frame; ② if the keyframe is greater than 7 and is the farthest away from the current frame; ③ less than 5% of the candidate points are activated; delete the points and outliers of the marginalized frames, add the current frame to the sequence, and construct the residual of the new keyframe and all keyframes in the sequence to obtain the error function; construct a distance map in the new keyframe, and when the candidate points meet the three conditions of the epipolar search distance less than 8, the quality attribute of step 3.4 is good, and the inverse depth is positive, then these candidate points are selected for activation, that is, the inverse depth of these points is optimized using a depth filter. After activation, these activated points can be added to the error function to prepare for calculating the photometric error;

[0029] 3.6) Start sliding window optimization of the activation points of the visual factor graph based on the photometric error function, and use the Gauss-Newton method to optimize the pose, photometric parameters, and inverse depth. It is necessary to derive the optimization variables:

[0030] ① Derivative of photometric parameters:

[0031]

[0032]

[0033] Where w p is the Huber weight, a and b are the parameters of the photometric affine transformation.

[0034] ②Relative posture derivative:

[0035]

[0036] Where, They are the pixels at p j The horizontal and vertical gradients at is the normalized coordinate, fx and f y is the camera internal parameter;

[0037] ③Inverse depth derivative:

[0038]

[0039] Where ρ is the inverse depth, t z is the translation of the camera in the z direction;

[0040] After obtaining the Jacobian matrix of each optimization variable, the Hessian matrix can be constructed to perform Gauss-Newton optimization to obtain a more accurate pose; after completion, the vision module of the algorithm can run independently and provide a basis for subsequent multi-sensor fusion.

[0041] The step 4) comprises the following steps:

[0042] 4.1) Based on the visual factor graph described in step 3), copy the factor graph and add the IMU pre-integration residual factor and GPS residual factor as a fusion factor graph, and perform joint sliding window optimization using lagged marginalization;

[0043] 4.2) The added IMU pre-integration residual factor is:

[0044]

[0045] Where r represents the residual term corresponding to position p, attitude q, velocity v, IMU accelerometer bias ba and gyroscope bias bg, respectively. Indicates the position of the IMU coordinate system relative to the world coordinate system in the jth frame, Indicates the rotation from the IMU coordinate system to the world coordinate system in the i-th frame, represents the acceleration deviation at the i-th frame, represents quaternion multiplication, and Indicates the measured position, attitude and velocity;

[0046] 4.3) The added GPS residual factor is:

[0047]

[0048] In the formula, the timestamp of GPS data is k∈(i,j), i and j are the timestamps of the two frames of images before and after, represents the difference between the GPS position of timestamp k and the position of frame i. Other details are the same as in step 4.2).

[0049] 4.4) The total residual of the algorithm is the photometric error E pj , IMU residual term r p,,,ba, and GPS position residual The sum of the two, the fusion factor graph is optimized with the minimum total residual as the optimization goal;

[0050] 4.5) Fusion Factor Graph: First, a coarse initialization step of the IMU is performed. That is, a sliding window of size 7 is maintained and residuals are calculated according to the optimization objective. If the calculated residual is less than the set value, the algorithm is directly put into VIO-GPS (vision-IMU-GPS fusion) mode. Otherwise, this step is repeated. After the IMU is initialized, the initial visual edge prior is updated using IMU information, and the variables in the factor graph are relinearized, while maintaining all visual and most inertial information.

[0051] 4.6) Furthermore, the marginalization of the fusion factor graph sliding window optimization is lagged by 4 key frames, and the marginalization operation is performed according to the marginalization order of the visual factor graph, so that the algorithm can use the ancient information to perform more accurate optimization calculations on the fusion factor graph and obtain better pose results.

[0052] 4.7) After optimizing for a period of time, if the inverse depths of the optimized variables in the fusion factor graph converge to a certain degree and differ significantly in size, the old keyframes are discarded to reduce the impact of the inverse depth noise of the old keyframes. Otherwise, the algorithm continues to run normally in real time. After that, the algorithm operates in multi-sensor fusion mode, continuously outputting the motion trajectory of the object using the algorithm in real time based on the data input by the sensors.

[0053] The present invention uses a direct method based on pixel information with significant gradients in the image to track the camera, using IMU pre-integration information as the initial pose to provide initial values ​​for direct tracking. Image frames are divided into keyframes and non-keyframes based on whether the tracking is successful. Simultaneously, a visual factor graph is constructed, and IMU residual terms and GPS residual terms are inserted into this visual factor graph to construct a new fusion factor graph. Fusion factor graph optimization is then initiated to complete the optimization process of the system algorithm. Furthermore, the algorithm also uses a back-end optimization strategy based on sliding window optimization, fixing the computational complexity of back-end pose optimization. Compared to the visual factor graph before the IMU and GPS are inserted, the fusion factor graph retains n pieces of old information and then performs delayed marginalization according to the marginalization order of the visual factor graph. This strategy can utilize old information, compensate for the shortcomings of marginalization, and retain the advantages of marginalization, allowing the factor graph optimization process to obtain more ideal optimization results, thereby improving the accuracy of the system. Because the algorithm is divided into two parts, the visual factor graph and the fusion factor graph, the algorithm can still maintain a certain performance before the visual IMU is initialized or when the IMU is inoperative, and can operate in a purely visual mode. Even if GPS signals are not properly received, the algorithm can continue to operate in visual inertial odometry (VIO) mode. The ability of other sensors to complement each other when a single sensor is affected demonstrates the robustness and reliability of multi-sensor fusion positioning algorithms in unknown environments. This invention has significant practical value and a wide range of applications.

[0054] Compared with the prior art, the present invention has the following advantages and beneficial results:

[0055] 1. This invention uses a direct method based on photometric error, directly utilizing the pixel-level photometric information (gradient) of the candidate points. Compared to the feature point method, this method eliminates the relatively time-consuming step of extracting feature descriptors. It also allows for normal operation in scenes where feature points are not obvious, resulting in relatively better real-time performance and faster computational efficiency. For the initial pose required by the direct method, the multi-sensor fusion solution provides the pose obtained by IMU pre-integration, making this algorithm faster and more accurate than traditional solutions.

[0056] 2. The present invention adopts a multi-sensor tightly coupled fusion strategy. Tight coupling utilizes the original information of the image to improve the accuracy of the algorithm. At the same time, a delayed marginalization method is adopted for the fusion factor graph. The method of lagged n frames and then marginalized enables the algorithm to use ancient information to complete initialization and optimization. Compared with conventional marginalization, its accuracy and robustness are better.

[0057] 3. The present invention adopts a key frame selection strategy based on photometric error information. It determines whether the current frame is a key frame by judging whether the number of activation point information of inverse depth convergence in the factor graph is sufficient and whether the exposure change is too large. This strategy is closer to the algorithm itself than the conventional key frame selection strategy of the feature point method.

[0058] 4. The present invention adopts the strategy of GPS data fusion, which can transform the motion trajectory calculated by the algorithm into the Northeast Celestial Coordinate System, that is, it can correspond to the specific position in the real world, making it closer to reality and improving the credibility of the motion trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 It is a schematic diagram of a visual factor graph of the optimization process of the present invention.

[0060] Figure 2 It is a schematic diagram of a fusion factor graph for multi-sensor fusion optimization of the present invention.

[0061] Figure 3 It is a flow chart of the SLAM positioning method of the present invention based on multi-sensor fusion of direct method. DETAILED DESCRIPTION

[0062] The following embodiments will illustrate the present invention in detail with reference to the accompanying drawings.

[0063] like Figure 3 The embodiment of the SLAM positioning method based on multi-sensor fusion of the direct method of the present invention comprises the following steps:

[0064] 1) Obtain the image frame of the monocular camera, IMU output information and GPS latitude and longitude information, solve the GPS latitude and longitude information to obtain the current world coordinate system coordinates, and then align the above information into a structure data according to the timestamp and input it into the system for calculation;

[0065] 2) Initialize the visual algorithm. Use the structure data obtained in step 1) to construct an image pyramid for the first frame. Use the gradient threshold to filter candidate gradient points. Then project the candidate gradient points initialized from the first frame into the second frame. Use the image pyramid to estimate the initial relative motion, initialize the inverse depth and the pose transformation from the IMU coordinate system to the world coordinate system. According to the photometric error function formula, optimize the two frames and determine whether the initialization is complete by the displacement of the optimization result. See the diagram of the visual factor graph for details. Figure 1 .

[0066] Described step 2) comprises the following steps:

[0067] 2.1) During initialization, an image pyramid is built for the first frame of the first two frames. The pyramid is traversed and the camera intrinsic parameters of each layer are calculated. The first layer image is divided into multiple grids with a grid size of 32*32. Then, the light intensity difference dx and dy in the x and y directions of the pixel points in the pixel block grid are calculated, and the sum of the squares of the light intensity differences is obtained as the gradient. The gradient of each pixel block is counted, and the median is selected as the gradient threshold for extracting candidate points from the pixel block.

[0068] 2.2) In the first layer of the image, traverse each n*n pixel block, and randomly select a direction in the pixel coordinate system to calculate the gradient dI of the pixels in the block. Record the pixel position p with the largest gradient that is greater than the threshold as the candidate point. If too few pixels are extracted, reduce the block size by 0.5n*0.5n and resample recursively; otherwise, increase the block size to 2n*2n and resample again. The recursion is generated in the same way. In the remaining layers of the pyramid, the points are selected in the same way, and the following two conditions must be met: ① The gradient must be greater than the set threshold; ② The pixel point in the pixel coordinate system should be the largest point in the four directions of x, y, x+y and xy. Finally, the layers of the image pyramid are associated.

[0069] 2.3) Calculate the transformation quaternion of the current camera coordinate system and gravity alignment. Based on the IMU information obtained in step 1) and aligned with the image time, obtain the acceleration data for a period of time and take the average value as the gravity direction of the current frame. At the same time, build a fixed-size sliding window to save the gravity direction of each frame, and use the sliding mean filter to optimize the IMU gravity vector. Calculate the rotation of the current frame gravity alignment with the actual gravity direction [0,0,9.8]

[0070] 2.4) The second frame of the initial two frames is optimized to complete the initialization. The second frame is based on the points of the first frame and the photometric error function is constructed:

[0071]

[0072] Where N p is the pixel set used in the algorithm, ‖·‖ is the Huber norm, p represents the projection of the spatial point p corresponding to the pixel point in frame i on frame j, t j In order to make the algorithm applicable to image sequences with unknown exposure time, the affine photometric formula is used. Perform photometric normalization. Then, starting from the top of the image pyramid and working downwards, optimize each layer using the Gauss-Newton method, using the identity matrix as the initial value. Check whether the displacement is sufficiently large. If the displacement is large enough and the number of frames tracked in this step is greater than five, initialization is complete. The first frame is then set as a keyframe, and the gradient points extracted from the first frame are activated and added to the sliding window optimization of the visual factor graph. Otherwise, repeat steps 2.1) to 2.4).

[0073] 3) The algorithm performs image tracking, using the pose between two frames obtained by IMU pre-integration as the initial pose. The camera's motion is tracked by optimizing the photometric error between the two frames using the Gauss-Newton method. During the tracking process, whether the current frame is designated as a new keyframe is determined based on three predefined conditions: ① The system has just started up and the number of stored frames is 1; ② The current frame's pixel translation, rotation, or photometric change is excessive; and ③ The time between the current frame and the previous keyframe exceeds the maximum time interval between keyframes.

[0074] a. If the current frame is decided to be the key frame, the gradient points extracted from the current frame are activated. That is, the inverse depth optimization calculation of the gradient points is performed using a deep filter to converge. At the same time, the key frame is added to the visual factor graph for sliding window optimization, and the marginalization is performed using Schur complement.

[0075] b. If the current frame is determined to be a non-keyframe, only the pose estimate between the current frame and the previous frame is used to update the inverse depth. In this case, the system operates in "vision-only" mode. Step 3) includes the following steps:

[0076] 3.1) Enter the tracking process of the algorithm. Pre-integrate the IMU data obtained in step 1) and aligned with the image time to obtain the pose transformation between the two frames. The pre-integration formula is

[0077]

[0078]

[0079]

[0080] Where, and for b k+1 Relative to b k The algorithm uses the pose obtained by IMU pre-integration as the initial pose of the direct method.

[0081] 3.2) Using the initial pose, the reference frame's points are projected into the current frame, and a photometric error function is constructed. With minimizing the photometric error as the optimization goal, the Levenberg-Marquardt algorithm is used to iteratively optimize the candidate points, starting from the top of the image pyramid downwards. This involves coarse-to-fine iterative optimization of inverse depth, photometric parameters, and pose. Several optimization operations are performed: ① If the photometric error exceeds 60% of the energy threshold, the threshold is increased and the optimization is repeated; ② If the energy of a pyramid layer exceeds twice the minimum value, tracking is considered a failure; ③ If the photometric error meets the requirements, tracking is successful.

[0082] 3.3) Determine whether the current frame can become a keyframe based on three preset conditions: ① The system has just started and the number of stored frames is 1; ② The translation, rotation, or luminosity change ratio of the current frame's pixels is too large; ③ The time between the current frame and the previous keyframe exceeds the maximum time interval between keyframes.

[0083] 3.4) If the current frame becomes a non-key frame, update the inverse depth range of the candidate point in step 2.2). The range of the epipolar line search is obtained based on the inverse depth range to calculate the image gradient and the size of the epipolar line angle. If the angle is too large, it means that the directions of the two frames tend to be perpendicular, and it is believed that the error of the result will be large, so the candidate point parameters will not be updated. Therefore, when the angle is appropriate, the position p1 with the smallest photometric error and the second smallest position p2 with a radius greater than 2 will be searched on the epipolar line according to a fixed step size, and the latter is divided by the former to obtain a ratio As a quality attribute, the greater the mass, the better the position. Based on this, a further detailed search is performed, and the Gauss-Newton method is used to optimize along the epipolar line until the increment is small enough. At this time, the best position of the projection is obtained, and the new inverse depth range is calculated using this best position.

[0084] 3.5) If the current frame needs to become a key frame, first marginalize the key frames in the key frame sequence that meet the following three conditions: ① The photometric parameters have changed significantly compared to the reference frame; ② If the key frame is greater than 7 and is the farthest away from the current frame; ③ Less than 5% of the candidate points are activated. Delete the points and external points of the marginalized frames, then add the current frame to the sequence, and construct the residual of the new key frame and all key frames in the sequence to obtain the error function. Then, a distance map is constructed in the new key frame. When the candidate points meet the three conditions of the extreme line search distance being less than 8, the quality attribute of step 3.4) being good, and the inverse depth being positive, these candidate points are selected for activation, that is, the inverse depth of these points is optimized using a depth filter. After activation, these activated points can be added to the error function to prepare for calculating the photometric error.

[0085] 3.6) Start sliding window optimization of the activation points of the visual factor graph based on the photometric error function, and use the Gauss-Newton method to optimize the pose, photometric parameters, and inverse depth. It is necessary to derive the optimization variables:

[0086] ① Derivative of photometric parameters:

[0087]

[0088]

[0089] Where w p is the Huber weight, a and b are the parameters of the photometric affine transformation.

[0090] ②Relative posture derivative:

[0091]

[0092] In the formula They are the pixels at p j The horizontal and vertical gradients at is the normalized coordinate, f x and f y is the camera internal parameter;

[0093] ③Inverse depth derivative:

[0094]

[0095] Where ρ is the inverse depth, t z is the translation of the camera in the z direction;

[0096] After obtaining the Jacobian matrix of each optimization variable, the Hessian matrix can be constructed for Gauss-Newton optimization to obtain a more accurate pose. After step 3), the algorithm's vision module can run independently and provide a foundation for subsequent multi-sensor fusion.

[0097] 4) Perform the algorithm's vision-IMU-GPS data fusion process. Based on the visual factor graph described in step 3), copy the factor graph as the fusion factor graph, add the IMU pre-integration factor and GPS factor, and use lagged marginalization for joint optimization. The fusion factor graph also uses the sliding window optimization method, but the marginalization strategy is to postpone n key frames, and then perform lagged marginalization in the same order as the marginalization of the visual factor graph to achieve the purpose of maintaining the sliding window size. While ensuring accuracy, lagged marginalization also makes up for the defects of marginalization and retains the advantages of marginalization. At this point, the system enters the "vision-IMU-GPS" model operation. See the schematic diagram of the fusion factor graph for multi-sensor fusion optimization for details. Figure 2 .

[0098] The step 4) comprises the following steps:

[0099] 4.1) Based on the visual factor graph described in step 3), copy the factor graph and add the IMU pre-integration residual factor and GPS residual factor as a fusion factor graph, and perform joint sliding window optimization using lagged marginalization.

[0100] 4.2) The added IMU pre-integration residual factor is

[0101]

[0102] Where r represents the residual term corresponding to position p, attitude q, velocity v, IMU accelerometer bias ba and gyroscope bias bg, respectively. Indicates the position of the IMU coordinate system relative to the world coordinate system in the jth frame, Indicates the rotation from the IMU coordinate system to the world coordinate system in the i-th frame, represents the acceleration deviation at the i-th frame, represents quaternion multiplication, and Represents the measured position, attitude, and velocity.

[0103] 4.3) The added GPS residual factor is:

[0104]

[0105] In the formula, the timestamp of GPS data is k∈(i,j), i and j are the timestamps of the two frames of images before and after. It represents the difference between the GPS position of timestamp k and the position of frame i. The rest is the same as in step 4.2).

[0106] 4.4) The total residual of the algorithm is the photometric error E pj , IMU residual term r p,,,ba, and GPS position residual The fusion factor graph is optimized with the minimum total residual as the optimization goal.

[0107] 4.5) Fusion Factor Graph: First, a coarse initialization step of the IMU is performed. This involves maintaining a sliding window size of 7 and calculating the residual according to the optimization objective. If the calculated residual is less than the set value, the algorithm directly enters VIO-GPS (vision-IMU-GPS fusion) mode. Otherwise, this step is repeated. After IMU initialization, the initial visual edge prior is updated using IMU information, and the variables in the factor graph are relinearized, while maintaining all visual and most inertial information.

[0108] 4.6) Furthermore, the marginalization of the fusion factor graph sliding window optimization is delayed by 4 key frames, and then the marginalization operation is performed according to the marginalization order of the visual factor graph. This enables the algorithm to use ancient information to perform more accurate optimization calculations on the fusion factor graph and obtain better pose results.

[0109] 4.7) After optimizing for a period of time, if the inverse depths of the optimized variables in the fusion factor graph converge to a certain level and vary significantly, the old keyframes are discarded to reduce the impact of inverse depth noise from the old keyframes. Otherwise, the algorithm continues to run normally in real time. From then on, the algorithm operates in multi-sensor fusion mode, continuously outputting the trajectory of the object using the algorithm in real time based on the sensor input data.

[0110] In summary, the algorithm of the present invention uses a direct method based on photometric error, directly utilizing the pixel-level photometric information (gradient) of the candidate points, which is relatively more real-time and speeds up the algorithm calculation efficiency. For the initial pose required by the direct method, the multi-sensor tightly coupled fusion solution provides it with the pose obtained by IMU pre-integration, reducing the influence of noise. Tight coupling utilizes the original information of the image to improve the accuracy of the algorithm. Furthermore, the GPS data fusion strategy is adopted, which can correspond to the specific position in the real world and improve the credibility of the motion trajectory. In addition, the method of lagged marginalization is adopted for the fusion factor graph, and the method of lagged n frames and then marginalized enables the algorithm to complete initialization and optimization using ancient information. Compared with conventional marginalization, its accuracy and robustness are better. In addition, by judging whether the number of activation point information of inverse depth convergence in the factor graph is sufficient and whether the exposure change is too large, etc., it is determined whether the current frame is a key frame. It is closer to the algorithm itself than the conventional strategy of key frame selection of the feature point method and has better effect.

Claims

1. A SLAM positioning method based on multi-sensor fusion of direct method, characterized by The following steps are involved: 1) Obtain the image frame of the monocular camera, IMU output information and GPS latitude and longitude information, solve the GPS latitude and longitude information to obtain the current world coordinate system coordinates, align the above information into a structure data according to the timestamp, and input it into the system for calculation; 2) Initialize the visual algorithm. Use the structured data obtained in step 1) to construct an image pyramid for the first frame. Use the gradient threshold to filter candidate gradient points. Project the candidate gradient points initialized from the first frame to the second frame. Use the image pyramid to estimate the initial relative motion, initialize the inverse depth and the pose transformation from the IMU coordinate system to the world coordinate system. According to the photometric error function formula, the two frames are optimized, and the displacement size of the optimization result is used to determine whether the initialization is completed; 3) Perform the algorithm's image tracking process, using the pose between two frames obtained by IMU pre-integration as the initial pose, and perform an optimization calculation based on the photometric error of the two frames based on the Gauss-Newton method to track the camera's motion; During the tracking process, whether to use the current frame as a new keyframe is determined based on three conditions: ① The system has just started and the number of stored frames is 1; ② The translation, rotation, or luminosity change ratio of the current frame's pixels is too large; ③ The time between the current frame and the previous keyframe exceeds the maximum time interval between keyframes; a. If the current frame is decided to be the key frame, the gradient points extracted from the current frame are activated. That is, the inverse depth optimization calculation of the gradient points is performed using a deep filter to converge. At the same time, the key frame is added to the visual factor graph for sliding window optimization, and the marginalization is performed using Schur complement. b. If the current frame is decided to be a non-keyframe, only the pose estimate between the current frame and the previous frame is used to update the inverse depth, and the system operates in "vision-only" mode; 4) Perform the algorithm's vision-IMU-GPS data fusion process. Based on the visual factor graph described in step 3), copy the factor graph as the fusion factor graph, add the IMU pre-integration factor and GPS factor, and use lagged marginalization to jointly optimize them. The fusion factor graph also uses the sliding window optimization method, but the marginalization strategy is to postpone n key frames and then perform lagged marginalization in the same order as the marginalization of the visual factor graph to achieve the purpose of maintaining the sliding window size. Lagged marginalization ensures accuracy while also compensating for the shortcomings of marginalization and retaining the advantages of marginalization. The system enters the "vision-IMU-GPS fusion" mode.

2. a kind of SLAM positioning method of the multi-sensor fusion based on direct method as claimed in claim 1, is characterized in that in step 2), the described initialization process of carrying out vision part algorithm specifically comprises the following steps: 2.1) During initialization, an image pyramid is built for the first frame of the first two frames. The pyramid is traversed and the camera intrinsic parameters of each layer are calculated. The first layer image is divided into multiple grids with a grid size of 32×32. Then, the light intensity difference dx and dy in the x and y directions of the pixel points in the pixel block grid are calculated, and the sum of the squares of the light intensity differences is obtained as the gradient. And count the gradients of each pixel block, and select the median as the gradient threshold of the pixel block to extract candidate points; 2.2) In the first layer of the image, traverse each n×n pixel block. Randomly select a direction in the pixel coordinate system from the pixel points within the pixel block to calculate the gradient dI of the pixels within the block. Record the pixel position p with the maximum gradient that is greater than the threshold as the candidate point. If too few pixels are extracted, reduce the block size by 0.5n×0.5n and resample it recursively. Otherwise, increase the block size to 2n×2n and resample it again. The same process is repeated for recursion. In the remaining layers of the pyramid, select points in the same way, and the following two conditions must be met: ① The gradient must be greater than the set threshold; ② The pixel point in the pixel coordinate system must be the largest point in the four directions of x, y, x+y, and xy. Finally, associate each layer of the image pyramid. 2.3) Calculate the transformation quaternion of the current camera coordinate system and gravity alignment; Based on the IMU information obtained in step 1) and aligned with the image time, obtain the acceleration data for a period of time and take the average value as the gravity direction of the current frame; At the same time, construct a fixed-size sliding window to save the gravity direction of each frame, use the sliding mean filter to optimize the IMU gravity vector, and calculate the rotation of the gravity alignment of the current frame with the actual gravity direction [0,0,9.8] 2.4) Optimize the second frame of the initial two frames to complete the initialization; the second frame is based on the points of the first frame to construct the photometric error function E pj : Where N p is the pixel set used in the algorithm, ||·|| is the Huber norm, p' represents the projection of the spatial point p corresponding to the pixel point in frame i on frame j, t j represents the exposure time of the jth frame; In order to make the algorithm applicable to image sequences with unknown exposure time, the affine photometric formula is used. Normalize the photometric data. Starting from the top layer of the image pyramid and working downwards, optimize each layer of the image pyramid using the Gauss-Newton method using the identity matrix as the initial value to determine whether the displacement is large enough. If the displacement is large enough and the number of tracking frames in this step is greater than 5, the initialization is completed, and the first frame is set as the key frame. The gradient points extracted from the first frame are activated and added to the sliding window optimization of the visual factor graph. Otherwise, repeat steps 2.1) to 2.4).

Images