A centralized multi-robot collaborative SLAM method based on FPGA platform

Abstract

This invention provides a centralized multi-robot cooperative SLAM method based on an FPGA platform. It uses a motion camera to acquire color images and IMU data of the surrounding environment; the color images are input into the FPGA platform of a single robot, converted into grayscale images, and subjected to image pyramid scaling, corner detection, and feature description before being transmitted to the processing system of the single robot; the processing system generates observation information, which in turn generates image frames; keyframes are generated and sent to a central server; the central server performs loop closure detection on the keyframes, performs map fusion on the local maps built by different single robots to obtain a global map, and optimizes the global map; finally, a 3D dense point cloud map is constructed. This method enables cooperative localization and 3D mapping of multiple robots in large-scale and complex environments, solving the problems of limited computing resources and detection range of a single robot in complex scenes, and improving the scale, accuracy, and robustness of map construction.

Description

Technical Field

[0001] This invention relates to the field of simultaneous localization and mapping (SLAM) technology, and more specifically, to a centralized multi-robot collaborative SLAM method based on an FPGA platform. Background Technology

[0002] With the rapid development of multi-sensor technology, robotics, and artificial intelligence, visual-inertial SLAM, a relative localization method where robots rely on their onboard visual sensors and inertial measurement units to perceive their surroundings, estimate their pose, and build environmental maps, performs well in small-scale scenarios with relatively high localization accuracy. However, in practical applications involving large areas, complex geographical environments, and tight search times, single visual SLAM technologies struggle to overcome limitations such as limited processing unit performance, narrow perception range per unit, and low mapping efficiency. Therefore, collaboration among multiple robots can more efficiently complete SLAM localization and mapping tasks. Simultaneous SLAM by multiple robots effectively avoids information loss issues when a single robot runs a SLAM system in rapidly changing complex scenes, exhibiting greater robustness. Even if a robot malfunctions and loses map information, it will not affect other robots' SLAM tasks, ensuring continuous environmental map construction. Furthermore, increasing the number of robots means a significant increase in the number of visual sensors in the system, allowing for several times the observation range within a unit of time. Simultaneously, the environmental map is built in blocks, improving map accuracy and construction efficiency.

[0003] Centralized multi-robot collaborative SLAM based on FPGA platform refers to dividing the entire collaborative SLAM system into two modules: edge terminals and a central server. The edge terminals based on FPGA platform only serve as lightweight visual odometry, collecting external environment perception information and completing tasks such as real-time localization and local mapping. The central server is responsible for collecting pose estimation information from the edge terminals and transforming and fusing the local maps of different edge terminals through matching algorithms to construct a global map.

[0004] Existing technology involves mounting visual sensors and inertial measurement units on a single robot. However, multi-sensor fusion and loop closure detection place high demands on the robot's hardware. Furthermore, single robots are limited by size and weight, and processing large amounts of data from sensors reduces their efficiency and increases power consumption. Summary of the Invention

[0005] To overcome the shortcomings and deficiencies of existing technologies, the present invention aims to provide a centralized multi-robot collaborative SLAM method based on an FPGA platform. This method enables collaborative localization and 3D mapping of multiple robots in large-scale complex environments, solving problems such as limited computing resources and detection range of a single robot in complex scenarios, and improving the scale, accuracy and robustness of map construction.

[0006] To achieve the above objectives, the present invention provides a centralized multi-robot collaborative SLAM method based on an FPGA platform, comprising the following steps:

[0007] S1. Use a motion camera to collect color images of the surrounding environment and IMU data; input the color images into the FPGA platform of the single robot and convert them into grayscale images;

[0008] S2. In the FPGA platform of the single robot, the grayscale image is subjected to image pyramid scaling, corner detection and feature description to obtain the feature description results, coordinate position and non-maximum value scores, and then transmitted to the processing system of the single robot; the processing system generates observation information and then generates image frames.

[0009] S3. Send the keyframes generated by the individual robot from the image frames to the central server.

[0010] S4. The central server performs loop closure detection on keyframes, merges local maps from different individual robots to obtain a global map, and optimizes the global map.

[0011] S5. Construct a 3D dense point cloud map based on the global map.

[0012] Preferably, the action camera refers to a binocular action camera; step S1 includes the following sub-steps:

[0013] S11. Initialize the SLAM system of the individual robot and the central server, and establish the connection between the individual robot and the central server;

[0014] S12. Use a binocular motion camera to acquire color images of the surrounding environment, input the color images into the FPGA platform of the single robot, convert the color images into grayscale images, and transmit the grayscale images from the FPGA platform to the processing system of the single robot.

[0015] S13. Use the IMU built into the binocular motion camera to acquire IMU data, which includes accelerometer and gyroscope data; based on the IMU data acquired during the time interval from the i-th frame to the (i+1)-th frame, obtain a preliminary estimate of the rigid body pose and the IMU pre-integration error during that time interval.

[0016] Preferably, step S2 includes the following sub-steps:

[0017] S21. Image pyramid scaling on the FPGA platform of a single robot: First, the grayscale image is input and counted pixel by pixel in row priority until the entire grayscale image is converted into a two-dimensional array format; then, the pixel of the two-dimensional array is downsampled and scaled using bilinear interpolation.

[0018] S22. Perform corner detection on the scaled image: First, set a sliding window in the scaled image; determine whether the center point of the sliding window meets the FAST corner definition: The FAST corner definition means that there are a sufficient number of continuous pixels in a discretized circular region with a certain radius centered on the image center point, the difference between the gray value of these pixels and the gray value of the center point exceeds a threshold t, and the score of the non-maximum value is greater than the score of the 8 surrounding adjacent pixels; if the FAST corner definition is met, the center point of the sliding window is determined to be a corner point, and Gaussian filtering is performed on the pixels in the sliding window;

[0019] Output the corner detection result and the grayscale value after Gaussian filtering for each pixel;

[0020] S23. Homogenize the feature points in the dense region: Split the image after corner detection into n child nodes; for child nodes with more than 1 corner point, retain the corner point with the largest response value as the feature point; for child nodes with 0 corner points, delete them directly; when the absolute value of the difference between the number of remaining child nodes and the threshold t does not exceed the set value, the feature point homogenization operation is completed.

[0021] S24. Describe the attributes of the feature points and convert them into mathematical expressions;

[0022] S25. Output the feature description results, coordinate positions, and non-maximum scores to the processing unit of the single robot.

[0023] S26. The processing unit of the single robot generates observation information based on the feature description results, including pixel information, timestamp, coordinates and orientation information of the extracted feature points; and generates image frames based on the observation information.

[0024] Preferably, step S21, which uses bilinear interpolation to perform step-down sampling scaling on the pixels of the two-dimensional array, means:

[0025] Assuming the coordinates of four pixels in the image pixel coordinate system are (u1, v1), (u2, v2), (u2, v1), and (u2, v2), and point P = (u, v) is a new pixel obtained through bilinear interpolation, the linear interpolation result of the four pixels in the u direction is first calculated using the following formula:

[0026]

[0027] Where f() is the function value of the pixel;

[0028] Then, linear interpolation is performed in the v direction to obtain the bilinearly interpolated point P, calculated using the following formula:

[0029]

[0030] Finally, the pixel boundaries of the scaled image are defined based on the scaling factor to determine the width and height range of the target image, and pixels that are not within the width and height range of the target image are removed.

[0031] The sub-step S24, describing the attributes of the feature points, refers to: assuming the coordinates of feature point p0 are (u0, v0), setting the neighborhood as a window Q centered on point p0, and selecting n pixel pairs {x} within window Q. i x j The quantization index for pixel pairs is defined as follows:

[0032]

[0033] Where I(x) i ) and I(x j ) is defined as the point x in window Q. i and x j The pixel values ​​are then used to construct a string from the least significant bit to the most significant bit of the selected pixel pairs.

[0034]

[0035] By comparing the angular differences between pixels in a pixel pair, we can describe the rotation invariance of the pixels:

[0036]

[0037] Where, θ ij Represented as pixel pairs {x i x j From point x in} i Point x j The rotation angle.

[0038] Preferably, step S3 includes the following sub-steps:

[0039] S31. Establish a communication thread module for individual robots, and establish a network connection with the central server based on the local area network IP address and port number of the central server;

[0040] S32. Set a sliding window to limit the number of image frames, and determine whether the image frames in the sliding window are keyframes;

[0041] If the previous frame of the current frame is a keyframe, then the oldest image frame in the sliding window is taken as the critical frame. The observation information and corresponding IMU pre-integration motion information of the critical frame are no longer retained. Only the co-view relationship constraint between the image frame and other image frames is retained, and the constraint is used as the prior information for optimization. If the previous frame of the current frame is not a keyframe, then the previous frame of the current frame is taken as the critical frame directly. The observation information of the critical frame is deleted, while the IMU pre-integration motion information corresponding to the timestamp of the critical frame is retained.

[0042] S33. The critical frame is processed using the Schur complement elimination method: First, assume that the state variables of the nonlinear optimization are... Its incremental equation is The incremental equation is expressed as:

[0043]

[0044] in, These are state variables for normal frames and keyframes. H is the state variable of the critical frame, which is determined by the state variable. Based on the matrix formed by the second-order partial derivatives of the sum of squared residuals, b represents the gradient information. It is the first-order partial derivative of the state variable; then, H is divided into blocks H 11 H 12 H 21 H 22 b is also divided into b1 and b2, and the state variables are calculated. Solution:

[0045]

[0046] in, The prior error after the sliding window critical processing is obtained based on this equation;

[0047] S34. Construct a local map for all keyframes of the single robot: First, update the information of landmarks in the map based on the feature point extraction results in the keyframes, including position coordinates and feature point description directions; then, remove landmarks that have been observed more than a set number of times; finally, perform feature matching based on the landmarks observed in the previous and subsequent keyframes.

[0048] S35. Transfer the keyframe queue generated by the individual robot in the local mapping thread to the keyframe sending queue of the communication thread, and then start traversing all keyframes in the keyframe sending queue of the communication thread; if a keyframe has been sent before, skip the keyframe to avoid duplicate sending; other unsent keyframes are converted into keyframe message structures.

[0049] S36. Traverse all landmarks in each keyframe one by one, and convert each valid landmark into a landmark message structure.

[0050] S37. Pack the keyframe message structure and the landmark message structure into the data transmission packet of the communication thread, and wait for the subsequent unified transmission process;

[0051] S38. Traverse each keyframe message structure and landmark message structure of the data transmission packet, and convert each message structure into a binary format sequence based on the Cereal message serialization library.

[0052] S39. Add the binary sequence to the message container; after processing all message structures, send the message container to the central server and wait for confirmation.

[0053] Preferably, in step S32, the following conditions are used to determine whether an image frame in the sliding window is a keyframe:

[0054] Condition 1: When the number of image frames in the sliding window is less than the set number of frames, the current frame is a keyframe;

[0055] Condition 2: Assume the width and height of the current frame image are w and w respectively. c and h c Calculate the disparity between corresponding feature points in the current frame and the next frame; if the disparity between the current frame and the next frame exceeds 0.15*min(w) c h c If the current frame is a keyframe, then the current frame is a keyframe.

[0056] Condition 3: If the number of feature point matches between the current frame and the next frame is less than 25, then the current frame is a keyframe.

[0057] Preferably, step S4 includes the following sub-steps:

[0058] S41. The central server performs loop closure detection on keyframes: First, it uses a bag-of-words model to calculate the scene similarity score between keyframes.

[0059]

[0060] Among them, vi and v j These are the bag-of-words vectors of the current keyframe and the shared keyframe, respectively. During the traversal of the shared keyframes, the minimum similarity score (minScore) is recorded. Candidate keyframes with similarity scores greater than minScore*0.8 are searched from the map keyframe database to filter out candidate keyframes with higher similarity than the shared keyframes. Then, consistency verification is performed on all candidate keyframes, and the shared keyframes of each candidate keyframe are obtained, forming a candidate keyframe set with the candidate keyframe itself. If this candidate keyframe set is continuously observed by the current keyframe more than a set number of times, then the candidate keyframe forms a loop with the current keyframe.

[0061] S42. Perform map fusion on local maps established from different individual robots: First, assume that the three-dimensional points in the two map coordinate systems are defined as sets P and Q, respectively. Find the transformation relationship between the two sets. The calculation formula is as follows:

[0062] Q = sRP + t

[0063] Where s represents the scale factor, R represents rotation, and t represents translation; the error model for solving the transformation relationship is defined as follows:

[0064]

[0065] Among them, e i The error between the actual and theoretical transformation relationships is calculated; the rotation R is then calculated to convert the 3D points into quaternion form. Then, eigenvalue decomposition is performed on the orthogonal matrix of the rotation relationship expressed in quaternions, and the scale factor s is solved using R.

[0066]

[0067] Among them, Q′ i and P′ i It is P i and Q i Decentralized vector coordinates, s * It is the scale factor that minimizes the error; the translation t is calculated based on the rotation R and the scale factor s:

[0068]

[0069] in, and It is the mean vector of all 3D points in sets P and Q; after map fusion is completed, loop closure constraints are added to the two keyframes;

[0070] S43. Eliminate the cumulative drift error of pose estimation by jointly optimizing all keyframes and landmarks in the system's keyframe database: First, define the state represented by the keyframe at a certain moment as X, whose member variables include the keyframe pose. Road sign location wl i Linear velocity of keyframes in world coordinate system W v k And the offset b for each frame k As shown in the following formula:

[0071]

[0072] in, and X represents the set of all keyframes and landmarks in the system's keyframe database. j Let z represent a single state variable; the optimization problem is represented as a weighted nonlinear least squares problem, where the error of each state variable represents the actual measured value z. i The difference between the predicted value and the value based on the current state:

[0073]

[0074] in, It is related to the measured value z i The corresponding set of relevant state variables, h i (.) is the measurement function, which predicts the measurement value based on the current state variables; the system error is divided into reprojection error, relative pose error, and IMU pre-integration error, resulting in global constraints on the system state variables and their state errors; then, the relative pose error e in the state error is adjusted according to the global constraints. Δp Optimization is performed using a pose graph optimization method, with the objective function defined as follows:

[0075]

[0076] in, It is the Mahalanobis distance, x(i,j) is the indicator function, i and j are the i-th keyframe and the j-th keyframe, and the mathematical definition of x(i,j) is:

[0077]

[0078] After optimization, the poses of all road markers are updated.

[0079] Preferably, step S5 includes the following sub-steps:

[0080] S51. Construct a 3D dense point cloud map based on the global map; filter the received image data, and if a pixel of a certain image has already participated in the dense mapping process, do not convert the pixels in the four directions of that pixel into point cloud information.

[0081] S52. Compressing the 3D dense point cloud map: Assume the maximum value of the point cloud data set in the system's global map on the X, Y, and Z coordinate axes is x. max y max z max The minimum value is x min y min z min Let the side length of a voxel be l, and the number of voxels on each axis be D. x D y D z for:

[0082]

[0083] in Indicates rounding down;

[0084] S53. Calculate the index I of the point cloud p in its corresponding voxel:

[0085]

[0086] I = I x +I y ·D x +I z ·D x ·D y ·

[0087] Where, p x p y p z These are the coordinates of point cloud p on each axis, I. x I y I z These are the indices of I on each axis.

[0088] A readable storage medium storing a computer program that, when executed by a processor, causes the processor to perform the centralized multi-robot cooperative SLAM method based on an FPGA platform.

[0089] A computer device includes a processor and a memory for storing a processor-executable program, wherein when the processor executes the program stored in the memory, it implements the centralized multi-robot collaborative SLAM method based on an FPGA platform.

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

[0091] 1. This invention can complete tasks such as image preprocessing, feature extraction, and feature description for visual odometry on a single robot platform, improving the problem of low efficiency and increased power consumption when small robots have insufficient hardware performance. Furthermore, it can utilize the parallel computing characteristics of FPGA to extract feature points faster than general embedded systems, even in complex environmental scenarios.

[0092] 2. This invention improves upon the information loss problem encountered by single robots performing visual SLAM tasks in large-scale complex scenes when the scene changes rapidly. It constructs a global map by dividing the environment map into blocks and then transforming and fusing the local maps of different individual robots using a matching algorithm. This overcomes the limitations of single-robot visual SLAM, improves positioning accuracy and robustness, and is more suitable for feature-rich and complex spatial scenes of nearly one million square meters. Attached Figure Description

[0093] Figure 1 This is a flowchart illustrating the present invention;

[0094] Figure 2 This is a diagram showing the FPGA feature extraction effect of the present invention;

[0095] Figure 3 This is a diagram illustrating the uniformity effect of the FPGA quadtree in this invention.

[0096] Figure 4 This is a comparison diagram of the actual trajectory of the present invention and the trajectory estimated by the present invention;

[0097] Figure 5 This is a comparison diagram of the actual trajectory of the present invention and the trajectory estimated by the present invention along the x, y, and z axes;

[0098] Figure 6 This is a comparison diagram of the actual trajectory of the present invention and the trajectory estimated by the present invention along the xyz axis Euler angles.

[0099] Figure 7 This invention reconstructs a sparse 3D map of the surrounding environment.

[0100] Figure 8 This invention reconstructs a three-dimensional dense point cloud map of the surrounding environment. Detailed Implementation

[0101] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0102] Example 1

[0103] This embodiment presents a centralized multi-robot collaborative SLAM method based on an FPGA platform, such as... Figure 1 As shown, it includes the following steps:

[0104] S1. Use a motion camera to collect color images of the surrounding environment and IMU data; input the color images into the FPGA platform of the single robot and convert them into grayscale images;

[0105] S2. In the FPGA platform of the single robot, the grayscale image is subjected to image pyramid scaling, corner detection and feature description to obtain the feature description results, coordinate position and non-maximum value scores, and then transmitted to the processing system of the single robot; the processing system generates observation information and then generates image frames.

[0106] S3. Send the keyframes generated by the individual robot from the image frames to the central server.

[0107] S4. The central server performs loop closure detection on keyframes, merges local maps from different individual robots to obtain a global map, and optimizes the global map.

[0108] S5. Construct a 3D dense point cloud map based on the global map.

[0109] Specifically, the action camera refers to a binocular action camera; step S1 includes the following sub-steps:

[0110] S11. Initialize the SLAM system of the individual robot and the central server, and establish the connection between the individual robot and the central server.

[0111] S12. Use a binocular motion camera to acquire color images of the surrounding environment and input the color images into the FPGA platform of the single robot. Convert the color images from RAW format to RGB888 format, then convert them into grayscale images, and transmit the grayscale images from the FPGA platform to the processing system of the single robot via the AXI bus.

[0112] S13. Use the IMU built into the binocular motion camera to acquire IMU data, which includes accelerometer and gyroscope data; based on the IMU data acquired during the time interval from the i-th frame to the (i+1)-th frame, obtain a preliminary estimate of the rigid body pose and the IMU pre-integration error during that time interval. IMU stands for Inertial Measurement Unit.

[0113] Step S2 includes the following sub-steps:

[0114] S21. Image pyramid scaling on the FPGA platform of a single robot: First, the grayscale image is input and counted pixel by pixel in row priority until the entire grayscale image is converted into a two-dimensional array format; then, the pixel of the two-dimensional array is downsampled and scaled using bilinear interpolation.

[0115] Using bilinear interpolation to perform step-down sampling scaling on pixels of a two-dimensional array means:

[0116] Assuming the coordinates of four pixels in the image pixel coordinate system are (u1, v1), (u2, v2), (u2, v1), and (u2, v2), and point P = (u, v) is a new pixel obtained through bilinear interpolation, the linear interpolation result of the four pixels in the u direction is first calculated using the following formula:

[0117]

[0118] Where f() is the function value of the pixel;

[0119] Then, linear interpolation is performed in the v direction to obtain the bilinearly interpolated point P, calculated using the following formula:

[0120]

[0121] Finally, the pixel boundaries of the scaled image are defined based on the scaling factor to determine the width and height range of the target image, and pixels that are not within the width and height range of the target image are removed.

[0122] S22. Perform corner detection on the scaled image: First, set a 7×7 sliding window in the scaled image; determine if the center point of the sliding window meets the FAST corner definition: The FAST corner definition is that within a discretized circular region of a certain radius centered on the image center, there exists a sufficient number of consecutive pixels, the difference between the gray values ​​of these pixels and the gray value of the center point exceeds a threshold t, where t is 15, and the score of the non-maximum value is greater than the scores of its 8 surrounding adjacent pixels; if the FAST corner definition is met, the center point of the sliding window is determined to be a corner, and a Gaussian filtering operation is performed on the pixels in the sliding window, calculated using the following formula:

[0123]

[0124] Where u and v are the two-dimensional coordinates of the pixel, respectively; σ is the standard deviation;

[0125] The corner detection result and the Gaussian-filtered grayscale value of each pixel are output in two 8-bit formats, as shown in the following output: Figure 2 As shown.

[0126] S23. Homogenize the feature points in dense regions: Divide the image after corner detection into n child nodes; for child nodes with more than one corner, retain the corner with the largest response value as the feature point; for child nodes with zero corners, delete them directly; when the absolute value of the difference between the number of remaining child nodes and the threshold t does not exceed a set value (e.g., 8), the feature point homogenization operation is complete. Figure 3 As shown.

[0127] S24. Describe the attributes of the feature points and convert them into a quantifiable mathematical expression: Assume the coordinates of feature point p0 are (u0, v0). Centered on point p0, set its neighborhood of size 31*31 as a window Q. Select n pixels in window Q to pair {x...} i x j The quantization index for pixel pairs is defined as follows:

[0128]

[0129] Where I(x) i ) and I(x j ) is defined as the point x in window Q. i and x j The pixel values ​​are then used to construct a string from the least significant bit to the most significant bit of the selected pixel pairs.

[0130]

[0131] By comparing the angular differences between pixels in a pixel pair, we can describe the rotation invariance of the pixels:

[0132]

[0133] Where, θ ij Represented as pixel pairs {x i x j From point x in} i Point x j The rotation angle.

[0134] S25. Pack the feature description results, coordinate positions and non-maximum values ​​into a 512-bit data stream format and output it to the processing unit of the single robot for subsequent processing of vision-inertial odometry.

[0135] S26. The processing unit of the single robot generates observation information based on the feature description results, including pixel information, timestamp, coordinates and orientation information of the extracted feature points; and generates image frames based on the observation information.

[0136] Step S3 includes the following sub-steps:

[0137] S31. Establish a communication thread module for individual robots, and establish a network connection with the central server based on the local area network IP address and port number of the central server.

[0138] S32. Set a sliding window to limit the number of image frames, and determine whether an image frame in the sliding window is a keyframe based on the following conditions:

[0139] Condition 1: When the number of image frames in the sliding window is less than the set number of frames (for example, the set number of frames is 2), the current frame is a keyframe.

[0140] Condition 2: Assume the width and height of the current frame image are w and w respectively. c and h c Calculate the disparity between corresponding feature points in the current frame and the next frame. Feature point disparity is defined as the coordinate offset of the matched feature points in the two frames. That is, place the matched feature points in the same pixel plane, calculate the pixel distance between the matched points, and then calculate the average distance of all matched points as the disparity. If the disparity between the current frame and the next frame exceeds 0.15 * min(w)... c h c If the current frame is a keyframe, then the current frame is a keyframe.

[0141] Condition 3: If the number of feature point matches between the current frame and the next frame is less than 25, then the current frame is a keyframe.

[0142] If the previous frame of the current frame is a keyframe, then the oldest image frame in the sliding window is taken as the critical frame. The observation information and corresponding IMU pre-integration motion information of the critical frame are no longer retained. Only the constraints such as the co-view relationship between the image frame and other image frames are retained, and the constraints are used as prior information for optimization. If the previous frame of the current frame is not a keyframe, then the previous frame of the current frame is directly taken as the critical frame. The observation information of the critical frame is deleted, while the IMU pre-integration motion information corresponding to the critical frame timestamp is retained to ensure the continuity of the object's motion.

[0143] S33. The critical frame is processed using the Schur complement elimination method: First, assume that the state variables of the nonlinear optimization are... Its incremental equation is The incremental equation is expressed as:

[0144]

[0145] in, These are state variables for normal frames and keyframes. H is the state variable of the critical frame, which is determined by the state variable. Based on the matrix formed by the second-order partial derivatives of the sum of squared residuals, b represents the gradient information. It is the first-order partial derivative of the state variable; then, H is divided into blocks H 11 H 12 H 21 H 22 b is also divided into b1 and b2, and the solution for state variable χ1 is calculated:

[0146]

[0147] in, The prior error after the sliding window critical processing is obtained based on this equation.

[0148] S34. After filtering the keyframes in the image frames, a local map is constructed for all keyframes in the visual odometry of the single robot: First, the information of the landmarks in the map is updated based on the feature point extraction results in the keyframes, including position coordinates, feature point description directions, etc. Then, landmarks that have been observed more than 3 times are removed; finally, feature matching is performed on the landmarks observed in the same keyframes in the visual odometry.

[0149] S35. Transfer the keyframe queue generated by the individual robot in the local mapping thread to the keyframe sending queue of the communication thread, and then start traversing all keyframes in the keyframe sending queue of the communication thread; if a keyframe has been sent before, skip the keyframe to avoid duplicate sending; other unsent keyframes are converted into keyframe message structures.

[0150] S36. Iterate through all the landmarks in each keyframe and convert each valid landmark into a landmark message structure.

[0151] S37. Pack the keyframe message structure and the landmark message structure into the data transmission packet of the communication thread, and wait for the subsequent unified transmission process.

[0152] S38. Traverse each keyframe message structure and landmark message structure of the data transmission packet, and convert each message structure into a binary format sequence based on the Cereal message serialization library.

[0153] S39. Add the binary sequence to the message container; after processing all message structures, send the message container to the central server and wait for confirmation.

[0154] Step S4 includes the following sub-steps:

[0155] S41. The central server performs loop closure detection on keyframes: First, it uses a bag-of-words model to calculate the scene similarity score between keyframes.

[0156]

[0157] Among them, v i and v j These are the bag-of-words vectors of the current keyframe and the shared keyframe, respectively. During the traversal of the shared keyframes, the minimum similarity score (minScore) is recorded. Candidate keyframes with a similarity score greater than minScore*0.8 are searched from the map keyframe database to filter out candidate keyframes with higher similarity than the shared keyframes. Then, consistency verification is performed on all candidate keyframes, and the shared keyframes of each candidate keyframe are obtained and combined with the candidate keyframe to form a candidate keyframe set. If this candidate keyframe set is continuously observed by the current keyframe more than a set number of times (e.g., 3 times), then the candidate keyframe forms a loop with the current keyframe.

[0158] S42. Perform map fusion on local maps established from different individual robots: First, assume that the three-dimensional points in the two map coordinate systems are defined as sets P and Q, respectively. Find the transformation relationship between the two sets. The calculation formula is as follows:

[0159] Q = sRP + t

[0160] Where s represents the scale factor, R represents rotation, and t represents translation; since the coordinate points of the two maps are subject to noise interference in real-world scenarios, the transformation relationship will have some error. An error model for solving the transformation relationship is defined as follows:

[0161]

[0162] Among them, e i The error between the actual and theoretical transformation relationships is calculated; the rotation R is then calculated to convert the 3D points into quaternion form. Then, eigenvalue decomposition is performed on the orthogonal matrix of the rotation relationship expressed in quaternions, and the scale factor s is solved using R.

[0163]

[0164] Among them, Q′ i and P′ i It is P i and Q i Decentralized vector coordinates, s * It is the scale factor that minimizes the error; the translation t is calculated based on the rotation R and the scale factor s:

[0165]

[0166] in, and It is the mean vector of all 3D points in sets P and Q; after map fusion is completed, loop closure constraints are added to the two keyframes, and the map fusion visualization result is as follows. Figure 7 As shown;

[0167] S43. Eliminate the cumulative drift error of pose estimation by jointly optimizing all keyframes and landmarks in the system's keyframe database: First, define the state represented by the keyframe at a certain moment as X, whose member variables include the keyframe pose. Road sign location wl i Linear velocity of keyframes in world coordinate system w v k And the offset b for each frame k As shown in the following formula:

[0168]

[0169] in, and X represents the set of all keyframes and landmarks in the system's keyframe database. j Let z represent a single state variable; the optimization problem is represented as a weighted nonlinear least squares problem, where the error of each state variable represents the actual measured value z. i The difference between the predicted value and the value based on the current state:

[0170]

[0171] in, It is related to the measured value z i The corresponding set of relevant state variables, h i (·) is the measurement function, which predicts the measurement value based on the current state variables; the system error is divided into reprojection error, relative pose error, and IMU pre-integration error, resulting in global constraints on the system state variables and their state errors; then, the relative pose error e in the state error is adjusted according to the global constraints. Δp Optimization is performed using a pose graph optimization method, with the objective function defined as follows:

[0172]

[0173] in, It is the Mahalanobis distance, x(i,j) is the indicator function, i and j are the i-th keyframe and the j-th keyframe, and the mathematical definition of x(i,j) is:

[0174]

[0175] After optimization, the poses of all landmarks are updated. A comparison of the estimated trajectory and the actual trajectory after optimization is shown below. Figure 4 , Figure 5and Figure 6 As shown.

[0176] Step S5 includes the following sub-steps:

[0177] S51. Construct a 3D dense point cloud map based on the global map; filter the received image data, and if a pixel of a certain image has already participated in the dense mapping process, do not convert the pixels in the four directions of that pixel into point cloud information.

[0178] S52. Compressing the 3D dense point cloud map: Assume the maximum value of the point cloud data set in the system's global map on the X, Y, and Z coordinate axes is x. max y max z max The minimum value is x min y min z min Let the side length of a voxel be l, and the number of voxels on each axis be D. x D y D z for:

[0179]

[0180] in Indicates rounding down;

[0181] S53. Calculate the index I of the point cloud p in its corresponding voxel:

[0182]

[0183] I = I x +I y ·D x +I z ·D x ·D y ·

[0184] Where, p x p y p z These are the coordinates of point cloud p on each axis, I. x I y I z These are the indices of I on each axis. The dense plotting results are as follows: Figure 8 As shown.

[0185] Example 2

[0186] This embodiment provides a readable storage medium storing a computer program that, when executed by a processor, causes the processor to perform the centralized multi-robot collaborative SLAM method based on an FPGA platform as described in Embodiment 1.

[0187] Example 3

[0188] This embodiment discloses a computer device, including a processor and a memory for storing processor-executable programs. When the processor executes the program stored in the memory, it implements the centralized multi-robot collaborative SLAM method based on an FPGA platform as described in Embodiment 1.

[0189] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A centralized multi-robot collaborative SLAM method based on an FPGA platform, characterized in that: Includes the following steps: S1. Use a motion camera to collect color images of the surrounding environment and IMU data; input the color images into the FPGA platform of the single robot and convert them into grayscale images; S2. In the FPGA platform of the single robot, the grayscale image is subjected to image pyramid scaling, corner detection and feature description to obtain the feature description results, coordinate position and non-maximum value score, and then transmitted to the processing system of the single robot. The processing system generates observation information, which in turn generates image frames; S3. Send the keyframes generated by the individual robot from the image frames to the central server. S4. The central server performs loop closure detection on keyframes, merges local maps from different individual robots to obtain a global map, and optimizes the global map. S5. Construct a 3D dense point cloud map based on the global map; Step S4 includes the following sub-steps: S41. The central server performs loop closure detection on keyframes: First, it uses a bag-of-words model to calculate the scene similarity score between keyframes. ； in, and These are the bag-of-words vectors of the current keyframe and the shared keyframe, respectively; during the traversal of the shared keyframes, the minimum similarity score is recorded. Search the map keyframe database for elements with a similarity score greater than 1. Candidate keyframes are selected to filter out candidate keyframes with a higher similarity than the common keyframes; then, consistency verification is performed on all candidate keyframes, and the common keyframes of each candidate keyframe are obtained and formed into a candidate keyframe set with itself; if the candidate keyframe set is observed by the current keyframe more than a set number of times, then the candidate keyframe forms a loop with the current keyframe. S42. Perform map fusion on local maps created from different individual robots: First, assume that the three-dimensional points in the two map coordinate systems are defined as sets. and To find the transformation relationship between two sets, the calculation formula is as follows: ； in, Indicates the scale factor. Indicates rotation, Represents translation; defines the error model for solving the transformation relationship: ； Among them, e i The error between the actual transformation relationship and the theoretical transformation relationship; calculate the rotation. The three-dimensional points are converted into quaternion form, and then eigenvalue decomposition is performed on the orthogonal matrix of rotation relations represented by quaternions. Solve for the scale factor : ； in, and yes and Decentralized vector coordinates It is the scale factor that minimizes the error; based on rotation and scale factor Calculate translation : ； in, and It is a set and The mean vector of all 3D points; after map fusion is completed, add loop closure constraints to the two keyframes; S43. Eliminate the cumulative drift error of pose estimation by jointly optimizing all keyframes and landmarks in the system's keyframe database: First, define the state represented by the keyframe of the system at a certain moment as... Its member variables include keyframe pose. Road sign locations Linear velocity of keyframes in world coordinate system and the offset of each frame As shown in the following formula: ； in, and This represents the set of all keyframes and landmarks in the system's keyframe database. Representing a single state variable; the optimization problem is expressed as a weighted nonlinear least squares problem, where the error of each state variable represents the actual measured value. The difference between the predicted value and the value based on the current state: ； in, It is related to the measured value The corresponding set of related state variables, It is a measurement function that predicts the measured value based on the current state variables; the system error is divided into reprojection error, relative pose error, and IMU pre-integration error to obtain the global constraints of the system state variables and their state errors; then, the relative pose error in the state error is adjusted according to the global constraints. Optimization is performed using a pose graph optimization method, with the objective function defined as follows: ； in, It is Mahalanobis distance. It is an indicator function. and For the first The first keyframe and the first One keyframe The mathematical definition of is: ； After optimization, the poses of all road markers are updated.

2. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 1, characterized in that: The action camera refers to a binocular action camera; step S1 includes the following sub-steps: S11. Initialize the SLAM system of the individual robot and the central server, and establish the connection between the individual robot and the central server; S12. Use a binocular motion camera to acquire color images of the surrounding environment, input the color images into the FPGA platform of the single robot, convert the color images into grayscale images, and transmit the grayscale images from the FPGA platform to the processing system of the single robot. S13. Use the built-in IMU of the binocular motion camera to acquire IMU data, which includes accelerometer and gyroscope data; Based on the IMU data acquired during the time interval from the i-th frame to the (i+1)-th frame, a preliminary estimate of the rigid body pose and the IMU pre-integration error are obtained within that time interval.

3. The centralized multi-robot collaborative SLAM method based on an FPGA platform according to claim 1, characterized in that: Step S2 includes the following sub-steps: S21. Image pyramid scaling on the FPGA platform of a single robot: First, the grayscale image is input and counted pixel by pixel in row priority until the entire grayscale image is converted into a two-dimensional array format; then, the pixel of the two-dimensional array is downsampled and scaled using bilinear interpolation. S22. Perform corner detection on the scaled image: First, set a sliding window in the scaled image; determine whether the center point of the sliding window meets the FAST corner definition: The FAST corner definition means that there are a sufficient number of continuous pixels in a discretized circular region with a certain radius centered on the image center point, the difference between the gray value of these pixels and the gray value of the center point exceeds a threshold t, and the score of the non-maximum value is greater than the score of the 8 surrounding adjacent pixels; if the FAST corner definition is met, the center point of the sliding window is determined to be a corner point, and Gaussian filtering is performed on the pixels in the sliding window; Output the corner detection result and the grayscale value after Gaussian filtering for each pixel; S23. Homogenize the feature points in dense regions: Perform a split on the image after corner detection, dividing it evenly into... The number of child nodes is calculated; for child nodes with more than 1 corner point, the corner point with the largest response value is retained as the feature point; for child nodes with 0 corner points, they are directly deleted; when the number of remaining child nodes is less than or equal to a threshold... When the absolute value of the difference does not exceed the set value, the feature point homogenization operation is completed; S24. Describe the attributes of the feature points and convert them into mathematical expressions; S25. Output the feature description results, coordinate positions, and non-maximum scores to the processing unit of the single robot. S26. The processing unit of the single robot generates observation information based on the feature description results, including pixel information, timestamp, coordinates of the extracted feature points, and orientation information. Image frames are generated based on observation information.

4. The centralized multi-robot collaborative SLAM method based on an FPGA platform according to claim 3, characterized in that: The sub-step S21, which uses bilinear interpolation to perform step-down sampling scaling on the pixels of the two-dimensional array, refers to: Assume the coordinates of the four pixels in the image pixel coordinate system are as follows: , , , ,point A new pixel is obtained through bilinear interpolation. First, the values ​​of the four pixels are calculated... The linear interpolation result in the direction is calculated using the following formula: ； Where f() is the function value of the pixel; Then, in Perform linear interpolation in the direction to obtain the points after bilinear interpolation. The calculation formula is: ； Finally, the pixel boundaries of the scaled image are defined based on the scaling factor to determine the width and height range of the target image, and pixels that are not within the width and height range of the target image are removed. The sub-step S24, describing the attributes of the feature points, refers to: assuming the feature points... The coordinates are , with point Set the neighborhood as the center as the window. In the window Selected from Pixel pairs { The quantization index for pixel pairs is defined as follows: ； in, and Define as window midpoint and The pixel value; then this The pixel pair index is formed into a string by selecting the pixel pairs in order from least significant bit to most significant bit. : ； By comparing the angular differences between pixels in a pixel pair, we can describe the rotation invariance of the pixels: ； in, Represented as pixel pairs { From point} Time The rotation angle.

5. The centralized multi-robot collaborative SLAM method based on an FPGA platform according to claim 1, characterized in that: Step S3 includes the following sub-steps: S31. Establish a communication thread module for individual robots, and establish a network connection with the central server based on the local area network IP address and port number of the central server; S32. Set a sliding window to limit the number of image frames, and determine whether the image frames in the sliding window are keyframes; If the previous frame of the current frame is a keyframe, then the oldest image frame in the sliding window is taken as the critical frame. The observation information and corresponding IMU pre-integration motion information of the critical frame are no longer retained. Only the co-view relationship constraint between the image frame and other image frames is retained, and the constraint is used as the prior information for optimization. If the previous frame of the current frame is not a keyframe, then the previous frame of the current frame is taken as the critical frame directly. The observation information of the critical frame is deleted, while the IMU pre-integration motion information corresponding to the timestamp of the critical frame is retained. S33. The critical frame is processed using the Schur complement elimination method: First, assume that the state variables of the nonlinear optimization are... Its incremental equation is The incremental equation is expressed as: ； in, These are state variables for normal frames and keyframes. These are the state variables for the critical frame. It is determined by state variables The matrix is ​​constructed from the second-order partial derivatives of the sum of squared residuals. It is gradient information. It is the first-order partial derivative of the state variable; then, Divided into blocks , , , , It also corresponds to being divided into blocks , Calculate state variables Solution: ； in, Based on this equation, the prior error after the sliding window critical processing is obtained; S34. Construct a local map for all keyframes of the single robot: First, update the information of landmarks in the map based on the feature point extraction results in the keyframes, including position coordinates and feature point description directions; then, remove landmarks that have been observed more than a set number of times; finally, perform feature matching based on the landmarks observed in the previous and subsequent keyframes. S35. Transfer the keyframe queue generated by the individual robot in the local mapping thread to the keyframe sending queue of the communication thread, and then start traversing all keyframes in the keyframe sending queue of the communication thread; if a keyframe has been sent before, skip the keyframe to avoid duplicate sending; other unsent keyframes are converted into keyframe message structures. S36. Traverse all landmarks in each keyframe one by one, and convert each valid landmark into a landmark message structure. S37. Pack the keyframe message structure and the landmark message structure into the data transmission packet of the communication thread, and wait for the subsequent unified transmission process; S38. Traverse each keyframe message structure and landmark message structure of the data transmission packet, and convert each message structure into a binary format sequence based on the Cereal message serialization library. S39. Add the binary sequence to the message container; after processing all message structures, send the message container to the central server and wait for confirmation.

6. The centralized multi-robot collaborative SLAM method based on an FPGA platform according to claim 5, characterized in that: In step S32, the following conditions are used to determine whether an image frame in the sliding window is a keyframe: Condition 1: When the number of image frames in the sliding window is less than the set number of frames, the current frame is a keyframe; Condition 2: Assume the width and height of the current frame image are respectively... and Calculate the disparity between corresponding feature points in the current frame and the next frame; if the disparity between the current frame and the next frame exceeds... If so, the current frame is the keyframe; Condition 3: If the number of feature point matches between the current frame and the next frame is less than 25, then the current frame is a keyframe.

7. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 1, characterized in that: Step S5 includes the following sub-steps: S51. Construct a 3D dense point cloud map based on the global map; filter the received image data, and if a pixel of a certain image has already participated in the dense mapping process, do not convert the pixels in the four directions of that pixel into point cloud information. S52. Compression of 3D dense point cloud map: Assume the maximum value of the point cloud data set in the system's global map on the X, Y, Z coordinate axes is... , , The minimum value is , , Let the side length of the voxel be... Number of voxels on each axis , , for: ； in Indicates rounding down; S53, Point Cloud Calculate its index in its corresponding voxel. : ； in, , , Point clouds Coordinates on each axis , , They are respectively Indexes on each axis.

8. A readable storage medium, characterized in that, The storage medium stores a computer program that, when executed by a processor, causes the processor to perform the centralized multi-robot collaborative SLAM method based on an FPGA platform as described in any one of claims 1-7.

9. A computer device comprising a processor and a memory for storing a processor-executable program, characterized in that, When the processor executes the program stored in the memory, it implements the centralized multi-robot collaborative SLAM method based on the FPGA platform as described in any one of claims 1-7.

Images