Centralized multi-robot collaborative SLAM method based on FPGA platform

Abstract

The invention provides a centralized multi-robot collaborative SLAM (Simultaneous Localization and Mapping) method based on an FPGA (Field Programmable Gate Array) platform. The method comprises the following steps: acquiring a color image and IMU (Inertial Measurement Unit) data of a surrounding environment by using a motion camera The color image is input into an FPGA platform of the single robot and converted into a grey-scale map, image pyramid scaling, corner detection and feature description are carried out, and the image is transmitted to a processing system of the single robot; the processing system generates observation information and further generates an image frame; generating a key frame and sending the key frame to a central server; the central server side carries out loopback detection on the key frame, carries out map fusion on local maps established by different single robots to obtain a global map, and optimizes the global map; and constructing a three-dimensional dense point cloud map. According to the method, cooperative positioning and three-dimensional mapping of multiple robots can be carried out in a large-scale and complex environment scene, the problem that computing resources and a detectable range of a single robot in the complex scene are limited is solved, and the scale, the precision and the robustness of map construction are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of synchronous positioning and map construction, and more specifically to a centralized multi-robot collaborative SLAM method based on an FPGA platform. Background Art

[0002] With the rapid development of multi-sensor technology, robotics, and artificial intelligence, visual-inertial SLAM (visual-inertial SLAM), a relative positioning method in which a robot relies on its onboard visual sensors and inertial measurement units to perceive its surroundings, estimate its own position, and construct a map of the environment, has performed well in small-scale scenarios, achieving relatively high positioning accuracy. However, in practical applications involving large areas, complex geographical environments, and tight search times, single robots often face limitations such as limited processing unit performance, a narrow perceptual range, and low mapping efficiency. Therefore, the collaboration of multiple robots simultaneously enables more efficient SLAM positioning and mapping. Simultaneous SLAM tasks by multiple robots effectively avoid the information loss encountered by a single robot in complex and rapidly changing scenes, resulting in greater robustness. Even if a robot experiences a malfunction during operation, resulting in map loss, this does not affect the SLAM tasks of other robots, allowing the construction of the environmental map to continue. Furthermore, an increase in the number of robots means that the number of visual sensors in the system also increases exponentially, enabling the system to cover a multiplier range per unit time. Furthermore, the environmental map is constructed in blocks, improving both accuracy and efficiency.

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

[0004] The existing technology is to install visual sensors and inertial measurement units on a single robot. However, operations such as multi-sensor fusion and loop detection have high hardware requirements for the robot. In addition, the single robot is limited by size and weight. Processing a large amount of data generated by sensors will reduce its work efficiency and increase power consumption. Summary of the Invention

[0005] To overcome the shortcomings and deficiencies in the prior art, the purpose of the present invention is to provide a centralized multi-robot collaborative SLAM method based on an FPGA platform; this method can perform collaborative positioning and three-dimensional mapping of multiple robots in large-scale and complex environmental scenes, solve the problems of limited computing resources and detectable range of a single robot in complex scenes, and improve the scale, accuracy and robustness of map construction.

[0006] In order to achieve the above object, the present invention is implemented by the following technical solution: 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. Performing image pyramid scaling, corner detection, and feature description on the grayscale image in the FPGA platform of the single robot to obtain feature description results, coordinate positions, and non-maximum scores, which are then transmitted to the processing system of the single robot; the processing system generates observation information, which in turn generates an image frame;

[0009] S3, sending the key frames generated by the single robot from the image frames to the central server;

[0010] S4. The central server performs loop detection on key frames, fuses local maps created by different individual robots to obtain a global map, and optimizes the global map.

[0011] S5. Construct a three-dimensional dense point cloud map based on the global map.

[0012] Preferably, the motion camera is a binocular motion camera; the step S1 comprises the following sub-steps:

[0013] S11, initialize the SLAM system of the single robot and the central server, and establish a connection between the single robot and the central server;

[0014] S12, using a binocular motion camera to collect a color image of the surrounding environment, and inputting the color image into the FPGA platform of the single robot, converting the color image into a grayscale image, and transmitting the grayscale image 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, where the IMU data includes accelerometer and gyroscope data. Based on the IMU data acquired during the time interval from the i-th frame image to the i+1-th frame image, obtain a preliminary estimate of the rigid body pose during the time interval and an IMU pre-integration error.

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

[0017] S21. Perform image pyramid scaling on the FPGA platform of the single robot: First, input and count the pixels of the grayscale image one by one in a row-first manner until the entire grayscale image is converted into a two-dimensional array format; then use the bilinear interpolation method to perform step-by-step downsampling and scaling on the pixels of the two-dimensional array;

[0018] S22. Perform corner detection on the scaled image: first, set a sliding window on the scaled image; determine whether the center point of the sliding window meets the FAST corner point definition: the FAST corner point definition means that there are a sufficient number of continuous pixels within a discretized circular area of ​​a certain radius with the center point of the image as the center, the grayscale values ​​of these pixels differ from the grayscale value of the center point by more than a threshold t, and the non-maximum scores are greater than the scores of the eight surrounding adjacent pixels; if the FAST corner point definition is met, the center point of the sliding window is determined to be a corner point, and a Gaussian filter operation is performed on the pixels in the sliding window;

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

[0020] S23, homogenizing the feature points in the dense area: split the image after corner point detection once and evenly divide it 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, directly delete them; 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, describing the attributes of the feature points and converting them into mathematical expressions;

[0022] S25. Outputting 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 according to the feature description result, including pixel information, timestamp, coordinates of the extracted feature points, and direction information; and generates an image frame according to the observation information.

[0024] Preferably, the sub-step S21, using a bilinear interpolation method to perform step-wise downsampling and scaling on the pixels of the two-dimensional array, refers to:

[0025] Assume that the coordinates of the four pixels in the image pixel coordinate system are (u1, v1), (u2, v2), (u2, v1), and (u2, v2), respectively. Point P = (u, v) is a new pixel point obtained by bilinear interpolation. First, the linear interpolation results of the four pixels in the u direction are calculated. The calculation formula is:

[0026]

[0027] Among them, f() is the function value of the pixel point;

[0028] Then, linear interpolation is performed in the v direction to obtain the point P after bilinear interpolation. The calculation formula is:

[0029]

[0030] Finally, the pixel boundaries of the scaled image are delineated according to 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, means: assuming that the coordinates of the feature point p0 are (u0, v0), taking the point p0 as the center, setting the neighborhood as the window Q, and selecting n pixel pairs {x i , x j}, define the quantitative index of pixel pairs as:

[0032]

[0033] Among them, I(x i ) and I(x j ) is defined as the midpoint x in the window Q i and x j The pixel value of the pixel pair; then these n pixel pair indices are combined into a string from the lowest bit to the highest bit according to the order of selecting pixel pairs

[0034]

[0035] Compare the angle differences between pixels in a pixel pair to describe the rotation invariance of the pixels:

[0036]

[0037] Among them, θ ij Represented as a pixel pair {x i , x j} from point x i To point x j The rotation angle of .

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

[0039] S31, establish a single robot communication thread module, 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, setting a sliding window to limit the number of image frames, and determining whether the image frame in the sliding window is a key frame;

[0041] If the previous frame of the current frame is a key frame, then the oldest image frame in the sliding window is used as the critical frame, and the observation information of the critical frame and the corresponding IMU pre-integrated motion information are no longer used. Only the co-viewing relationship constraints between the image frame and other image frames are retained, and the constraints are used as the prior information for optimization; if the previous frame of the current frame is not a key frame, then the previous frame of the current frame is directly used as the critical frame, and the observation information of the critical frame is deleted, while retaining the IMU pre-integrated motion information corresponding to the critical frame timestamp;

[0042] S33. Use Schur's elimination method to process the critical frame: First, assume that the state variable of nonlinear optimization is Its incremental equation is And the increment equation is expressed as:

[0043]

[0044] in, are state variables for normal frames and key frames, is the state variable of the critical frame, and H is the state variable According to the matrix formed by the second-order partial derivative of the residual sum of squares, b is the gradient information, is the first-order partial derivative of the state variable; then, H is divided into 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, According to this equation, the prior error after the sliding window critical processing is obtained;

[0047] S34. Build a local map for all keyframes of the single robot: First, update the information of the landmark points in the map based on the feature point extraction results in the keyframe, including the position coordinates and the direction described by the feature points; then, remove the landmark points that have been repeatedly observed more than a set number of times; finally, perform feature matching based on the landmark points observed in the previous and next keyframes;

[0048] S35. The keyframe queue generated by the single robot in the local mapping thread is transferred to the keyframe sending queue of the communication thread, and then all keyframes in the keyframe sending queue of the communication thread are traversed. If a keyframe has been sent before, it is skipped to avoid repeated sending. Other unsent keyframes are converted into keyframe message structures.

[0049] S36, traverse all the landmarks in each key frame one by one, and convert each valid landmark into a landmark message structure;

[0050] S37, packaging the key frame message structure and the waypoint message structure into a data transmission package of the communication thread, and waiting for the subsequent unified transmission process;

[0051] S38. Traverse each key frame message structure and waypoint 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 receipt confirmation.

[0053] Preferably, in the sub-step S32, whether the image frame in the sliding window is a key frame is determined by the following conditions:

[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 key frame;

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

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

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

[0058] S41. The central server performs loop detection on key frames: First, the bag-of-words model is used to calculate the scene similarity score between key frames:

[0059]

[0060] Among them, vi and v j are the bag-of-words vectors of the current keyframe and the common-view keyframe respectively; in the process of traversing the common-view keyframes, record the minimum similarity score minScore; search for candidate keyframes with a similarity score greater than minScore*0.8 from the map keyframe database to screen out candidate keyframes with a higher degree of similarity than the common-view keyframe; then perform consistency verification on all candidate keyframes, obtain the common-view keyframe of each candidate keyframe and form a candidate keyframe set with itself; if the candidate keyframe set is continuously observed by the current keyframe for more than the set number of times, the candidate keyframe forms a loop with the current keyframe;

[0061] S42. Perform map fusion on the local maps created by different individual robots: First, assume that the three-dimensional points in the two map coordinate systems are defined as sets P and Q respectively, and 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 the rotation, and t represents the translation. The error model for solving the transformation relationship is defined as:

[0064]

[0065] Among them, e i is the error between the actual transformation relationship and the theoretical transformation relationship; calculate the rotation R, convert the three-dimensional point into the form of quaternion, and then perform eigenvalue decomposition on the orthogonal matrix of the rotation relationship represented by the quaternion, and use R to solve the scale factor s:

[0066]

[0067] Among them, Q′ i and P′ i It's P i and Q i The vector coordinate after decentralization, s * 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 is the mean vector of all three-dimensional points in the sets P and Q; after completing the map fusion, add loop constraints to the two key frames;

[0070] S43, by jointly optimizing all key frames and landmarks in the system key frame database, the accumulated drift error of the pose estimation is eliminated: First, the state represented by the key frame of the system at a certain moment is defined as X, whose member variables include the key frame pose Waypoint location wl i , linear velocity of the keyframe in the world coordinate system W v k And the bias b of each frame k , as shown below:

[0071]

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

[0073]

[0074] in, is the same as 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 variable; the system error is divided into reprojection error, relative posture error and IMU pre-integration error, and the global constraints of the system state variables and their state errors are obtained; then the relative posture error e in the state error is constrained according to the global constraints. Δp Use the pose graph optimization method for optimization, and define the optimization objective function as:

[0075]

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

[0077]

[0078] After the optimization is completed, the poses of all landmarks are updated.

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

[0080] S51. Construct a three-dimensional dense point cloud map based on the global map; filter the received image data. If a pixel of a certain image has already participated in the dense mapping process, the pixel points in the four directions of the pixel are not converted into point cloud information;

[0081] S52. Compress the three-dimensional dense point cloud map: Assume that the maximum value of the point cloud data set on the X, Y, and Z coordinate axes in the system global map is x max 、y max 、z max , the minimum value is x min 、y min 、z min , let the side length of the 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 the voxel to which it belongs:

[0085]

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

[0087] Among them, p x 、p y 、p z are the coordinates of point cloud p on each axis, I x , I y , I z are the indices of I on each axis respectively.

[0088] A readable storage medium, wherein the storage medium stores a computer program, and when the computer program is executed by a processor, the processor executes the centralized multi-robot collaborative SLAM method based on an FPGA platform.

[0089] A computer device comprises a processor and a memory for storing a program executable by the processor. When the processor executes the program stored in the memory, the centralized multi-robot collaborative SLAM method based on the FPGA platform is implemented.

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

[0091] 1. This invention can complete image preprocessing, feature extraction, and feature description tasks for a single robot's visual odometry task on an FPGA platform. This solves the problem of low efficiency and increased power consumption in running visual odometry on small robots when hardware performance is insufficient. Furthermore, the parallel computing characteristics of FPGAs can be utilized for complex environmental scenarios, allowing feature point extraction to be completed more quickly than in common embedded systems.

[0092] 2. This invention addresses the information loss problem encountered when a single robot performs visual SLAM tasks in large, complex scenes, often encountering rapid scene changes. By constructing a global map through localized segmentation and then transforming and fusing the local maps of different individual robots using a matching algorithm, this approach overcomes the limitations of single-robot visual SLAM, improving positioning accuracy and robustness, and is more suitable for complex spatial scenes of nearly one million square meters. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] Figure 1 It is a schematic flow diagram of the present invention;

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

[0095] Figure 3 This is a diagram showing the effect of FPGA quadtree homogenization of the present invention;

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

[0097] Figure 5 It is a comparison diagram of the xyz axis of the actual trajectory of the present invention and the trajectory estimated by the present invention;

[0098] Figure 6 This is a comparison diagram of the xyz-axis Euler angle posture of the actual trajectory of the present invention and the trajectory estimated by the present invention;

[0099] Figure 7 is a sparse three-dimensional map of the surrounding environment reconstructed by the present invention;

[0100] Figure 8 It is a three-dimensional dense point cloud map of the surrounding environment reconstructed by the present invention. DETAILED DESCRIPTION

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

[0102] Example 1

[0103] This embodiment is a centralized multi-robot collaborative SLAM method based on FPGA platform, such as Figure 1 As shown, the following steps are included:

[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. Performing image pyramid scaling, corner detection, and feature description on the grayscale image in the FPGA platform of the single robot to obtain feature description results, coordinate positions, and non-maximum scores, which are then transmitted to the processing system of the single robot; the processing system generates observation information, which in turn generates an image frame;

[0106] S3, sending the key frames generated by the single robot from the image frames to the central server;

[0107] S4. The central server performs loop detection on key frames, fuses local maps created by different individual robots to obtain a global map, and optimizes the global map.

[0108] S5. Construct a three-dimensional dense point cloud map based on the global map.

[0109] Specifically, the motion camera is a binocular motion camera; step S1 includes the following sub-steps:

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

[0111] S12. Use a binocular motion camera to collect 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, and then convert them into grayscale images. 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, where the IMU data includes accelerometer and gyroscope data. Based on the IMU data acquired during the time interval from the i-th frame image to the (i+1)-th frame image, obtain a preliminary estimate of the rigid body posture during the time interval and an IMU pre-integration error. The full name of IMU is Inertial Measurement Unit.

[0113] The step S2 includes the following sub-steps:

[0114] S21. Perform image pyramid scaling on the FPGA platform of the single robot: First, input and count the pixels of the grayscale image one by one in a row-first manner until the entire grayscale image is converted into a two-dimensional array format; then use the bilinear interpolation method to perform step-by-step downsampling and scaling on the pixels of the two-dimensional array.

[0115] Using bilinear interpolation to perform step-wise downsampling and scaling of the pixels of a two-dimensional array means:

[0116] Assume that the coordinates of the four pixels in the image pixel coordinate system are (u1, v1), (u2, v2), (u2, v1), and (u2, v2), respectively. Point P = (u, v) is a new pixel point obtained by bilinear interpolation. First, the linear interpolation results of the four pixels in the u direction are calculated. The calculation formula is:

[0117]

[0118] Among them, f() is the function value of the pixel point;

[0119] Then, linear interpolation is performed in the v direction to obtain the point P after bilinear interpolation. The calculation formula is:

[0120]

[0121] Finally, the pixel boundaries of the scaled image are delineated according to 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 sliding window of size 7×7 on the scaled image; determine whether the center point of the sliding window meets the FAST corner point definition: the FAST corner point definition means that there are a sufficient number of continuous pixels in a discretized circular area of ​​a certain radius with the center point of the image as the center, the grayscale values ​​of these pixels differ from the grayscale value of the center point by more than a threshold t, where t is 15, and the non-maximum score is greater than the scores of the 8 adjacent pixels around it; if the FAST corner point definition is met, the center point of the sliding window is determined to be a corner point, and a Gaussian filter operation is performed on the pixels in the sliding window, and the calculation formula is:

[0123]

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

[0125] Output the corner detection result and the grayscale value after Gaussian filtering of each pixel in two 8-bit formats. The output results are as follows Figure 2 shown.

[0126] S23, homogenize the feature points in the dense area: split the image after corner point detection once and evenly divide it into n child nodes; for the child nodes with more than 1 corner point, retain the corner point with the largest response value as the feature point; for the 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 (for example, the set value is 8), the feature point homogenization operation is completed, as shown in Figure 2. Figure 3 shown.

[0127] S24. Describe the attributes of the feature points and convert them into a quantifiable mathematical expression: Assume that the coordinates of the feature point p0 are (u0, v0), take point p0 as the center, set its neighborhood of size 31*31 as the window Q, select n pixel pairs {x i , x j}, define the quantitative index of pixel pairs as:

[0128]

[0129] Among them, I(x i ) and I(x j ) is defined as the midpoint x in the window Q i and x j The pixel value of the pixel pair; then these n pixel pair indices are combined into a string from the lowest bit to the highest bit according to the order of selecting pixel pairs

[0130]

[0131] Compare the angle differences between pixels in a pixel pair to describe the rotation invariance of the pixels:

[0132]

[0133] Among them, θ ij Represented as a pixel pair {x i , x j} from point x i To point x j The rotation angle of .

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

[0135] S26. The processing unit of the single robot generates observation information according to the feature description result, including pixel information, timestamp, coordinates of the extracted feature points, and direction information; and generates an image frame according to the observation information.

[0136] The step S3 includes the following sub-steps:

[0137] S31. Establish a single robot communication thread module and establish a network connection with the central server based on the LAN IP address and port number of the central server.

[0138] S32: Setting a sliding window to limit the number of image frames, and determining whether an image frame in the sliding window is a key frame according to 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 key frame;

[0140] Condition 2: Assume that the width and height of the current frame image are w c and h c , calculate the disparity between the corresponding feature points in the current frame image and the next frame image; the feature point disparity is defined as the coordinate offset of the matching feature points in the two frames, that is, put the matching feature points in the same pixel plane, calculate the pixel distance between the matching points, and then calculate the average value of the distance of all matching points as the disparity; if the disparity between the current frame and the next frame exceeds 0.15*min(w c , h c ), the current frame is a key frame;

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

[0142] If the previous frame of the current frame is a key frame, then the oldest image frame in the sliding window is used as the critical frame, and the observation information of the critical frame and the corresponding IMU pre-integrated motion information are no longer used. Only constraints such as the co-viewing relationship between the image frame and other image frames are retained, and the constraints are used as the prior information for optimization; if the previous frame of the current frame is not a key frame, then the previous frame of the current frame is directly used as the critical frame, and the observation information of the critical frame is deleted. At the same time, the IMU pre-integrated motion information corresponding to the critical frame timestamp is retained to ensure the continuity of the object's motion.

[0143] S33. Use Schur's elimination method to process the critical frame: First, assume that the state variable of nonlinear optimization is Its incremental equation is And the increment equation is expressed as:

[0144]

[0145] in, are state variables for normal frames and key frames, is the state variable of the critical frame, and H is the state variable According to the matrix formed by the second-order partial derivative of the residual sum of squares, b is the gradient information, is the first-order partial derivative of the state variable; then, H is divided into H 11 、H 12 、H 21 、H 22 , b is also divided into blocks b1 and b2, and the solution of the state variable χ1 is calculated:

[0146]

[0147] in, According to this equation, the prior error after sliding window critical processing is obtained.

[0148] S34. After the keyframes in the image frames are screened, 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 from the keyframes, including the location coordinates, feature point description direction, etc. Then, landmarks that have been observed more than three times are removed. Finally, feature matching is performed based on the landmarks that are observed together in the previous and next keyframes in the visual odometry.

[0149] S35. Transfer the keyframe queue generated by the single 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 repeated sending; other unsent keyframes are converted into keyframe message structures.

[0150] S36. Traverse all the landmarks in each key frame one by one, and convert each valid landmark into a landmark message structure.

[0151] S37. Pack the key frame message structure and the waypoint message structure into the data sending package of the communication thread, and wait for the subsequent unified sending process.

[0152] S38. Traverse each key frame message structure and waypoint message structure of the data transmission package, 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 receipt confirmation.

[0154] The step S4 includes the following sub-steps:

[0155] S41. The central server performs loop detection on key frames: First, the bag-of-words model is used to calculate the scene similarity score between key frames:

[0156]

[0157] Among them, v i and v j are the bag-of-words vectors of the current keyframe and the common-view keyframe respectively; in the process of traversing the common-view keyframes, record the minimum similarity score minScore; search for candidate keyframes with a similarity score greater than minScore*0.8 from the map keyframe database to screen out candidate keyframes with a higher degree of similarity than the common-view keyframe; then perform consistency verification on all candidate keyframes, obtain the common-view keyframe of each candidate keyframe and form a candidate keyframe set with itself; if the candidate keyframe set is continuously observed by the current keyframe for more than a set number of times (for example, 3 times), the candidate keyframe forms a loop with the current keyframe;

[0158] S42. Perform map fusion on the local maps created by different individual robots: First, assume that the three-dimensional points in the two map coordinate systems are defined as sets P and Q respectively, and 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 actual scenarios, there will be certain errors in the transformation relationship. The error model for solving the transformation relationship is defined as follows:

[0161]

[0162] Among them, e i is the error between the actual transformation relationship and the theoretical transformation relationship; calculate the rotation R, convert the three-dimensional point into the form of quaternion, and then perform eigenvalue decomposition on the orthogonal matrix of the rotation relationship represented by the quaternion, and use R to solve the scale factor s:

[0163]

[0164] Among them, Q′ i and P′ i It's P i and Q i The vector coordinate after decentralization, s * 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 is the mean vector of all three-dimensional points in the set P and Q; after completing the map fusion, add loop constraints to the two key frames, and the map fusion visualization result is as follows Figure 7 As shown;

[0167] S43, by jointly optimizing all key frames and landmarks in the system key frame database, the accumulated drift error of the pose estimation is eliminated: First, the state represented by the key frame of the system at a certain moment is defined as X, whose member variables include the key frame pose Waypoint location wl i , linear velocity of the keyframe in the world coordinate system w v k And the bias b of each frame k , as shown below:

[0168]

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

[0170]

[0171] in, is the same as 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 variable; the system error is divided into reprojection error, relative posture error and IMU pre-integration error, and the global constraints of the system state variables and their state errors are obtained; then the relative posture error e in the state error is constrained according to the global constraints. Δp Use the pose graph optimization method for optimization, and define the optimization objective function as:

[0172]

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

[0174]

[0175] After the optimization is completed, the position and posture of all landmark points are updated. After the optimization, the comparison between the estimated trajectory and the real trajectory of the present invention is as follows: Figure 4 、 Figure 5and Figure 6 shown.

[0176] The step S5 includes the following sub-steps:

[0177] S51. Construct a three-dimensional dense point cloud map based on the global map; filter the received image data. If a pixel of a certain image has already participated in the dense mapping process, the pixel points in the four directions of the pixel are not converted into point cloud information;

[0178] S52. Compress the three-dimensional dense point cloud map: Assume that the maximum value of the point cloud data set on the X, Y, and Z coordinate axes in the system global map is x max 、y max 、z max , the minimum value is x min 、y min 、z min , let the side length of the 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 the voxel to which it belongs:

[0182]

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

[0184] Among them, p x 、p y 、p z are the coordinates of point cloud p on each axis, I x , I y , I z are the indices of I on each axis respectively, and the dense mapping results are as follows Figure 8 shown.

[0185] Example 2

[0186] This embodiment provides a readable storage medium, wherein the readable storage medium stores a computer program, and when the computer program is executed by a processor, the processor executes the centralized multi-robot collaborative SLAM method based on the FPGA platform described in the first embodiment.

[0187] Example 3

[0188] This embodiment provides a computer device, including a processor and a memory for storing a program executable by the processor. When the processor executes the program stored in the memory, the centralized multi-robot collaborative SLAM method based on the FPGA platform described in the first embodiment is implemented.

[0189] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A centralized multi-robot collaborative SLAM method based on FPGA platform, characterized by: The following steps are involved: 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. Perform image pyramid scaling, corner detection, and feature description on the grayscale image in the FPGA platform of the single robot to obtain feature description results, coordinate positions, and non-maximum scores, and transmit them to the processing system of the single robot; The processing system generates observation information, which in turn generates image frames; S3, sending the key frames generated by the single robot from the image frames to the central server; S4. The central server performs loop detection on key frames, fuses local maps created by different individual robots to obtain a global map, and optimizes the global map. S5. Construct a three-dimensional dense point cloud map based on the global map.

2. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 1, characterized in that: The motion camera is a binocular motion camera; step S1 includes the following sub-steps: S11, initialize the SLAM system of the single robot and the central server, and establish a connection between the single robot and the central server; S12, using a binocular motion camera to collect a color image of the surrounding environment, and inputting the color image into the FPGA platform of the single robot, converting the color image into a grayscale image, and transmitting the grayscale image from the FPGA platform to the processing system of the single robot; S13, using the IMU built into the binocular motion camera to collect IMU data, the IMU data including: accelerometer and gyroscope data; According to the IMU data collected in the time interval from the i-th frame image to the i+1-th frame image, the preliminary estimate of the rigid body posture and the IMU pre-integration error in this time interval are obtained.

3. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 1, characterized in that: The step S2 includes the following sub-steps: S21. Perform image pyramid scaling on the FPGA platform of the single robot: First, input and count the pixels of the grayscale image one by one in a row-first manner until the entire grayscale image is converted into a two-dimensional array format; then use the bilinear interpolation method to perform step-by-step downsampling and scaling on the pixels of the two-dimensional array; S22. Perform corner detection on the scaled image: first, set a sliding window on the scaled image; determine whether the center point of the sliding window meets the FAST corner point definition: the FAST corner point definition means that there are a sufficient number of continuous pixels within a discretized circular area of ​​a certain radius with the center point of the image as the center, the grayscale values ​​of these pixels differ from the grayscale value of the center point by more than a threshold t, and the non-maximum scores are greater than the scores of the eight surrounding adjacent pixels; if the FAST corner point definition is met, the center point of the sliding window is determined to be a corner point, and a Gaussian filter operation is performed on the pixels in the sliding window; Output the corner detection results and the grayscale value after Gaussian filtering for each pixel respectively; S23, homogenizing the feature points in the dense area: split the image after corner point detection once and evenly divide it 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, directly delete them; 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; S24, describing the attributes of the feature points and converting them into mathematical expressions; S25. Outputting 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 according to the feature description result, including pixel information, timestamp, coordinates of the extracted feature points, and direction information; Generate image frames based on observation information.

4. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 3, characterized in that: The sub-step S21, using a bilinear interpolation method to perform step-wise downsampling and scaling on the pixels of the two-dimensional array, refers to: Assume that the coordinates of the four pixels in the image pixel coordinate system are (u1, v1), (u2, v2), (u2, v1), and (u2, v2), respectively. Point P = (u, v) is a new pixel point obtained by bilinear interpolation. First, the linear interpolation results of the four pixels in the u direction are calculated. The calculation formula is: Among them, f() is the function value of the pixel point; Then, linear interpolation is performed in the v direction to obtain the point P after bilinear interpolation. The calculation formula is: Finally, the pixel boundaries of the scaled image are delineated according to 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, means: assuming that the coordinates of the feature point p0 are (u0, v0), taking the point p0 as the center, setting the neighborhood as the window Q, and selecting n pixel pairs {x i , x j }, define the quantitative index of pixel pairs as: Among them, I(x i ) and I(x j ) is defined as the midpoint x in the window Q i and x j The pixel value of the pixel pair; then these n pixel pair indices are combined into a string from the lowest bit to the highest bit according to the order of selecting pixel pairs Compare the angle differences between pixels in a pixel pair to describe the rotation invariance of the pixels: Among them, θ ij Represented as a pixel pair {x i , x j } from point x i To point x j The rotation angle of .

5. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 1, characterized in that: The step S3 includes the following sub-steps: S31, establish a single robot communication thread module, 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, setting a sliding window to limit the number of image frames, and determining whether the image frame in the sliding window is a key frame; If the previous frame of the current frame is a key frame, then the oldest image frame in the sliding window is used as the critical frame, and the observation information of the critical frame and the corresponding IMU pre-integrated motion information are no longer used. Only the co-viewing relationship constraints between the image frame and other image frames are retained, and the constraints are used as the prior information for optimization; if the previous frame of the current frame is not a key frame, then the previous frame of the current frame is directly used as the critical frame, and the observation information of the critical frame is deleted, while retaining the IMU pre-integrated motion information corresponding to the critical frame timestamp; S33. Use Schur's elimination method to process the critical frame: First, assume that the state variable of nonlinear optimization is Its incremental equation is And the increment equation is expressed as: in, are state variables for normal frames and key frames, is the state variable of the critical frame, and H is the state variable According to the matrix formed by the second-order partial derivative of the residual sum of squares, b is the gradient information, is the first-order partial derivative of the state variable; then, H is divided into H 11 、H 12 、H 21 、H 22 , b is also divided into b1 and b2, and the state variables are calculated Solution: in, According to this equation, the prior error after the sliding window critical processing is obtained; S34. Build a local map for all keyframes of the single robot: First, update the information of the landmark points in the map based on the feature point extraction results in the keyframe, including the position coordinates and the direction described by the feature points; then, remove the landmark points that have been repeatedly observed more than a set number of times; finally, perform feature matching based on the landmark points observed in the previous and next keyframes; S35. The keyframe queue generated by the single robot in the local mapping thread is transferred to the keyframe sending queue of the communication thread, and then all keyframes in the keyframe sending queue of the communication thread are traversed. If a keyframe has been sent before, it is skipped to avoid repeated sending. Other unsent keyframes are converted into keyframe message structures. S36, traverse all the landmarks in each key frame one by one, and convert each valid landmark into a landmark message structure; S37, packaging the key frame message structure and the waypoint message structure into a data transmission package of the communication thread, and waiting for the subsequent unified transmission process; S38. Traverse each key frame message structure and waypoint 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 receipt confirmation.

6. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 5, characterized in that: In the sub-step S32, whether the image frame in the sliding window is a key frame is determined by the following conditions: 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 key frame; Condition 2: Assume that the width and height of the current frame image are w c and h c , calculate the disparity between the corresponding feature points in the current frame image and the next frame image; if the disparity between the current frame and the next frame exceeds 0.15*min(w c , h c ), the current frame is a key frame; Condition three: If the number of feature point matches between the current frame and the next frame is less than 25, the current frame is a key frame.

7. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 1, characterized in that: The step S4 includes the following sub-steps: S41. The central server performs loop detection on key frames: First, the bag-of-words model is used to calculate the scene similarity score between key frames: Among them, v i and v j are the bag-of-words vectors of the current keyframe and the common-view keyframe respectively; in the process of traversing the common-view keyframes, record the minimum similarity score minScore; search for candidate keyframes with a similarity score greater than minScore*0.8 from the map keyframe database to screen out candidate keyframes with a higher degree of similarity than the common-view keyframe; then perform consistency verification on all candidate keyframes, obtain the common-view keyframe of each candidate keyframe and form a candidate keyframe set with itself; if the candidate keyframe set is continuously observed by the current keyframe for more than the set number of times, the candidate keyframe forms a loop with the current keyframe; S42. Perform map fusion on the local maps created by different individual robots: First, assume that the three-dimensional points in the two map coordinate systems are defined as sets P and Q respectively, and find the transformation relationship between the two sets. The calculation formula is as follows: Q=sRP+t Where s represents the scale factor, R represents the rotation, and t represents the translation. The error model for solving the transformation relationship is defined as: Among them, e i is the error between the actual transformation relationship and the theoretical transformation relationship; calculate the rotation R, convert the three-dimensional point into the form of quaternion, and then perform eigenvalue decomposition on the orthogonal matrix of the rotation relationship represented by the quaternion, and use R to solve the scale factor s: Among them, Q′ i and P′ i It's P i and Q i The vector coordinate after decentralization, s * is the scale factor that minimizes the error; the translation t is calculated based on the rotation R and the scale factor s: in, and is the mean vector of all three-dimensional points in the sets P and Q; after completing the map fusion, add loop constraints to the two key frames; S43, by jointly optimizing all key frames and landmarks in the system key frame database, the accumulated drift error of the pose estimation is eliminated: First, the state represented by the key frame of the system at a certain moment is defined as X, whose member variables include the key frame pose Waypoint location wl i , linear velocity of the keyframe in the world coordinate system W ν k And the bias b of each frame k , as shown below: in, and represents the set of all keyframes and landmarks in the system keyframe database, X j represents a single state variable; the optimization problem is expressed as a weighted nonlinear least squares problem, and the error of each state variable represents the actual measurement value z i The difference between the predicted value based on the current state: in, is the same as 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 variable; the system error is divided into reprojection error, relative posture error and IMU pre-integration error, and the global constraints of the system state variables and their state errors are obtained; then the relative posture error e in the state error is constrained according to the global constraints. Δp Use the pose graph optimization method for optimization, and define the optimization objective function as: in, is the Mahalanobis distance, x(i, j) is the indicator function, i and j are the i-th key frame and the j-th key frame, and the mathematical definition of x(i, j) is: After the optimization is completed, the poses of all landmarks are updated.

8. The centralized multi-robot collaborative SLAM method based on FPGA platform according to claim 1, characterized in that: The step S5 includes the following sub-steps: S51. Construct a three-dimensional dense point cloud map based on the global map; filter the received image data. If a pixel of a certain image has already participated in the dense mapping process, the pixel points in the four directions of the pixel are not converted into point cloud information; S52. Compress the three-dimensional dense point cloud map: Assume that the maximum value of the point cloud data set on the X, Y, and Z coordinate axes in the system global map is x max 、y max 、z max , the minimum value is x min 、y min 、z min , let the side length of the voxel be l, and the number of voxels on each axis be D x 、D y 、D z for: in Indicates rounding down; S53. Calculate the index I of the point cloud p in the voxel to which it belongs: I=I x +I y ·D x +I z ·D x ·D y· Among them, p x 、p y 、p z are the coordinates of point cloud p on each axis, I x , I y , I z are the indices of I on each axis respectively.

9. A readable storage medium, characterized in that: The storage medium stores a computer program, which, when executed by a processor, enables the processor to execute the centralized multi-robot collaborative SLAM method based on an FPGA platform according to any one of claims 1 to 8.

10. A computer device comprising a processor and a memory for storing a program executable by the processor, characterized in that: When the processor executes the program stored in the memory, the centralized multi-robot collaborative SLAM method based on the FPGA platform described in any one of claims 1 to 8 is implemented.

Images