Multi-sensor fusion service robot collaborative mapping method and system

Abstract

The application discloses a multi-sensor fusion service robot cooperative mapping method and system, including a ground service robot and a server, the service robot including a depth vision camera, an inertial measurement unit (IMU), a microcomputer (NUC) and a communication module, front-end processing is carried out, including: the depth vision camera acquires image data and the IMU acquires instantaneous speed information and hands over to the NUC, feature extraction and key frame extraction are carried out on picture data, pre-integration is carried out on IMU data; the data after preprocessing is sent to the background server. The back-end server cooperative mapping method includes: the service robot starts from the same place and shoots an Apriltag label; a conversion matrix between each service robot is acquired; a cost function is constructed, and data sent by each service robot is optimized in the back end; a local map is constructed for each service robot; according to the conversion matrix, the map coordinates of the remaining service robots are converted to one of the service robots, and a global map is formed.

Description

Technical Field

[0001] This invention relates to the field of service robot technology, and more specifically to a collaborative mapping method and system for service robots using multi-sensor fusion. Background Technology

[0002] In recent years, my country has witnessed rapid development in image processing, artificial intelligence, robotics, and related technologies, with continuous innovation and rapid advancements in these fields. In particular, improving the autonomy and intelligence of robots has attracted increasing attention from researchers. Service robots have received particular focus. With my country's aging population, future robots, such as domestic service robots and robots for elderly and disabled care, will inevitably be a key focus. The primary improvement point for enhancing the intelligence level of these robots lies in improving their environmental perception capabilities, namely, localization and mapping capabilities. Since the efficiency of a single robot in exploring its environment is relatively low, the ability of multiple robots to collaboratively explore the environment and construct a global map is especially important.

[0003] Therefore, how to improve the mapping capabilities of service robots is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0004] In view of this, the present invention provides a multi-sensor fusion service robot collaborative mapping method, which enables multiple service robots to jointly explore the environment and collaboratively build a global map.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A collaborative mapping method for service robots using multi-sensor fusion includes:

[0007] Step 1: Initialize several service robots, obtain the pose information of all service robots through the Apriltag visual reference library, and obtain the transformation matrix between each service robot through coordinate transformation; the transformation matrix is ​​stored on the server side;

[0008] Step 2: All service robots start from the same starting point with Apriltag labels affixed indoors and move in different directions, collecting image data and speed data; the depth vision camera continuously acquires image data in front of the service robot, and the inertial measurement unit continuously acquires the instantaneous speed and acceleration information of the current service robot, and transmits the image data, instantaneous speed and acceleration information to the microcomputer for processing;

[0009] Step 3: Preprocess the image data, extract ORB features for feature matching, obtain feature points, estimate the pose of the service robot based on the feature points and pose information, and select keyframes.

[0010] Step 4: Pre-integrate the speed data of each service robot to obtain pre-integrated data;

[0011] Step 5: Construct a cost function based on the estimated pose and pre-integration data of each service robot, and optimize the solution of the cost function to obtain the high-precision pose of each service robot;

[0012] High-precision pose is transmitted to the server; the cost function can minimize sensor errors and improve positioning accuracy; the velocity data and the estimated pose of the service robot are optimized together with the historical trajectory data at the current pose estimation time to reduce trajectory errors during long-term motion.

[0013] Step 6: The server constructs a local map for each service robot based on the high-precision pose and keyframes of each service robot, using its corresponding starting point as the origin; the local map is constructed on the server.

[0014] Step 7: Select any starting point of a service robot as the origin of the global map, and project all local maps onto the coordinate system of the origin of the global map according to the transformation matrix between the service robots to obtain the global map; construct the global map on the server.

[0015] Preferably, the initialization in step 1 refers to obtaining the displacement and rotation relationship between the origin of the coordinate system of the inertial measurement unit (IMU) that collects speed data and the depth vision camera that collects image data, as well as the offset of the IMU. This requires obtaining the displacement and rotation relationship between the origin of the coordinate system of the IMU and the depth vision camera through a calibration method to obtain the transformation matrix. The Kalibr calibration library is usually used for calibration.

[0016] Preferably, the image data includes color images and depth images, and the specific process of step 3 is as follows:

[0017] Step 31: Preprocess the color images; the microcomputer receives and processes the image data from each service robot;

[0018] Preprocessing includes enhancing image contrast through histogram equalization and converting color images to grayscale images through adaptive thresholding to improve image quality. Since the indoor environment where service robots are located has uneven lighting and image contrast, changes in lighting or environment can easily cause a sharp decrease in the number of feature points, affecting image feature extraction. Therefore, preprocessing is used to reduce the impact on image quality.

[0019] Step 32: Extract ORB features from the preprocessed color image and perform feature matching on the color images at different time points to obtain feature points;

[0020] Step 321: Detect corner points in the color image;

[0021] Step 322: Calculate the descriptors for the corner points;

[0022] Step 323: Match points based on their descriptors to obtain matching point pairs;

[0023] Step 324: Calculate the matching distance between correctly matched pairs to obtain the maximum and minimum matching distances; calculate the threshold for incorrectly matched pairs based on the minimum matching distance, threshold = max(30.2 * minimum matching distance);

[0024] Step 325: Remove mismatched matching point pairs according to the threshold, and use the remaining matching point pairs as feature points;

[0025] Step 33: Estimate the pose of each service robot based on feature points combined with depth images and pose information, select key frames, and transmit the key frames to the server.

[0026] Preferably, the key frame refers to representative image data selected as optimization data to reduce the computational load of server-side optimization. The key frame selection strategy is as follows: if the number of feature points tracked in the current frame is less than a feature point threshold, it is considered a key frame; if the average disparity of the features of the current frame relative to the nearest key frame is greater than a set disparity threshold, it is considered a key frame.

[0027] Preferably, the effective measurement range of the depth vision camera is about 0.5m to 5.0m. Too far or too close will affect the accuracy of the depth value measurement, and there may be cases where the depth value of some pixels is missing. Therefore, in the above case, when estimating the pose of the service robot, only triangulation is used to obtain the missing depth value and then to obtain the pose. Displacement is the pose during the machine's movement. The pose in step 3 is obtained solely from the image and requires further backend optimization by the backend server to reduce errors.

[0028] Backend optimization includes initialization and pose estimation of image data processing. In order to obtain more reliable pose data under long-endurance conditions, after obtaining visual constraints, IMU constraints and system initialization, the system backend performs state fusion on the obtained constraint relationship and solves the cost function by minimizing the visual residual and the IMU pre-integrated residual function.

[0029] Select the state variables as parameters within the sliding window, including the position, attitude, velocity of all n+1 depth vision cameras and the bias of the inertial measurement units (IMUs), calculate the extrinsic parameters from the depth vision cameras to the IMUs and the inverse depth of m feature points;

[0030]

[0031] X is the parameter within the sliding window, where k = 0, 1, ..., n, the state variables corresponding to each keyframe, with the meanings of position, velocity, attitude, and IMU bias respectively. They are all obtained from the pre-integrated data obtained from the IMU preprocessing. The IMU includes an accelerometer and a gyroscope. The transformation matrices between the depth vision camera and the IMU's accelerometer and gyroscope are respectively derived from the initialization process. The input variable of the cost function is X. The inverse depth λ1 is used to optimize the cost function. The inverse depth refers to the reciprocal of the depth value corresponding to the image feature point, which is convenient for calculating the cost function. It is only used for mathematical optimization calculation. The depth value is usually relatively large, which can easily lead to excessive bias in the optimization of the depth value. Therefore, the reciprocal is taken.

[0032] Preferably, the cost function in step 5 is expressed as:

[0033]

[0034] The function J(.) represents some mathematical optimization calculations performed on the parameter X within the sliding window to reduce the error of X; the above The expansion is as follows:

[0035]

[0036] Where, ||r c (X)|| 2 Represents the reprojection residual of vision; ||r b (X)|| 2 This represents the residual of the pre-integrated data; the purpose of the cost function is to use previous trajectory information for global optimization to eliminate accumulated errors.

[0037] Preferably, the optimization involves fine-tuning the inaccurate camera pose and reconstructed map obtained from the visual front end. A nonlinear method is used to consider all state variables of the system and make an optimal estimate of them in noisy observations, that is, to maximize their posterior probability. Thus, a least squares problem with pose and map points as optimization variables and noise variance as optimization objective can be derived to quickly and accurately solve the cost function. It relies solely on pose information obtained from images, and IMU data also contains errors. Combining the two further improves the accuracy of pose. The specific algorithms all use traditional nonlinear optimization methods.

[0038] A multi-sensor fusion service robot collaborative mapping system includes a service robot and a server; the service robot includes a depth vision camera, an inertial measurement unit, a microcomputer, and a communication module.

[0039] The depth vision camera is used to acquire image data of the service robot's surrounding environment, and the image data includes color images and depth images;

[0040] The inertial measurement unit is used to acquire instantaneous velocity data of the service robot, and the velocity data includes velocity, acceleration and angular acceleration;

[0041] The microcomputer is used to fuse the image data and the speed data, select keyframes from the image data, and obtain the world coordinates of the service robot;

[0042] The communication module is used to send keyframes and world coordinates to the server;

[0043] The server is used to receive and process keyframes and world coordinates sent by several of the service robots, and finally obtain a global map.

[0044] As can be seen from the above technical solution, compared with the prior art, this invention discloses a multi-sensor fusion service robot collaborative mapping method and system. The system includes a ground service robot and a server. The service robot includes a depth vision camera, an inertial measurement unit (IMU), a microcomputer (NUC), and a communication module for front-end processing. The front-end processing method includes: the depth vision camera acquiring image data and the IMU acquiring instantaneous velocity information and handing them over to the microcomputer; the microcomputer performing feature extraction and keyframe extraction on the image data; the microcomputer pre-integrating the IMU data; and sending the pre-processed data to the back-end server. The back-end server collaborative mapping method includes: the service robots starting from the same location and taking Apriltag photos; obtaining the transformation matrix between each service robot; constructing a cost function to optimize the data sent by each service robot; constructing local maps for each service robot; and, based on the transformation matrix, transforming the map coordinates of the other service robots onto one of the service robots to form a global map. Attached Figure Description

[0045] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0046] Figure 1 The attached figure is a schematic diagram of the collaborative mapping system for service robots using multi-sensor fusion provided by the present invention.

[0047] Figure 2 The attached figure is a schematic diagram of the data processing flow provided by the present invention;

[0048] Figure 3 The attached figure is a schematic diagram of the collaborative mapping method provided by the present invention;

[0049] Figure 4 The attached figure is a schematic diagram of the feature matching process provided by the present invention;

[0050] Figure 5 The attached figure is a schematic diagram of the pre-integration process provided by the present invention;

[0051] Figure 6 The attached figure is a schematic diagram illustrating the conversion of a local map into a global map provided by the present invention. Detailed Implementation

[0052] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0053] This invention discloses a collaborative mapping method for service robots using multi-sensor fusion, comprising:

[0054] S1: Initialize several service robots, obtain the pose information of all service robots through the Apriltag visual reference library, and obtain the transformation matrix between each service robot through coordinate transformation; the transformation matrix is ​​stored on the server side;

[0055] S2: All service robots start from the same point indoors where Apriltag tags are affixed, and move in different directions, collecting image and speed data. A depth vision camera continuously acquires image data of the area in front of the service robot, and an inertial measurement unit continuously acquires the instantaneous velocity and acceleration information of the current service robot. The image data, instantaneous velocity, and acceleration information are then transmitted to a microcomputer for processing.

[0056] S3: Preprocess the image data, extract ORB features for feature matching, obtain feature points, estimate the pose of the service robot based on the feature points and pose information, and select keyframes.

[0057] S4: Pre-integrate the speed data of each service robot to obtain pre-integrated data;

[0058] S5: Construct a cost function based on the estimated pose and pre-integration data of each service robot, and optimize the solution of the cost function to obtain the high-precision pose of each service robot;

[0059] High-precision pose is transmitted to the server; the cost function can minimize sensor errors and improve positioning accuracy; the velocity data and the estimated pose of the service robot are optimized together with the historical trajectory data at the current pose estimation time to reduce trajectory errors during long-term motion.

[0060] S6: The server constructs a local map for each service robot based on its high-precision pose and keyframes, using its corresponding starting point as the origin; the local map is constructed on the server.

[0061] S7: Arbitrarily select the starting point of a service robot as the origin of the global map, and project all local maps onto the coordinate system of the origin of the global map according to the transformation matrix between the service robots to obtain the global map; construct the global map on the server.

[0062] To further optimize the above technical solution, the initialization in S1 refers to obtaining the displacement and rotation relationship between the origin of the coordinate system of the inertial measurement unit (IMU) that collects speed data and the depth vision camera that collects image data, as well as the offset of the inertial measurement unit (IMU). It is necessary to first obtain the transformation matrix between the inertial measurement unit and the depth vision camera through calibration methods, usually using the Kalibr calibration library for calibration.

[0063] To further optimize the above technical solution, the image data includes color images and depth images. The specific process of S3 is as follows:

[0064] S31: Preprocess the color images; the microcomputer receives and processes the image data from each service robot;

[0065] Preprocessing includes enhancing image contrast through histogram equalization and converting color images to grayscale images using adaptive thresholding to improve image quality. Because the indoor environment where service robots operate is characterized by uneven lighting and image contrast, changes in lighting or the environment can cause a sharp decrease in the number of feature points, affecting image feature extraction. Therefore, preprocessing is used to reduce the impact on image quality.

[0066] S32: Extract ORB features from the preprocessed color image and perform feature matching on the color images at different time points to obtain feature points;

[0067] S321: Detect corner points in a color image;

[0068] S322: Calculate the descriptor for the corner point;

[0069] S323: Match points based on their descriptors to obtain matching point pairs;

[0070] S324: Calculate the matching distance between correctly matched pairs of points to obtain the maximum and minimum matching distances; calculate the threshold for incorrectly matched pairs of points based on the minimum matching distance, threshold = max(30.2 * minimum matching distance);

[0071] S325: Remove mismatched matching point pairs based on the threshold, and use the remaining matching point pairs as feature points;

[0072] S33: Estimate the pose of each service robot based on feature points combined with depth images and pose information, select key frames, and transmit the key frames to the server.

[0073] The 2D coordinates of the service robot are obtained from the feature points, the 3D coordinates of the service robot are obtained from the depth image, and the pose transformation matrix between image frames [Rt] is solved by the EPnP algorithm based on the 3D coordinates and the corresponding 2D coordinates, as well as the transformation matrix between the inertial measurement unit and the depth vision camera.

[0074] The expression for estimating the pose of a service robot is:

[0075]

[0076] in, These represent the poses of the depth vision camera at times t and t++1, respectively. Let [Rt] represent the pose transformation matrix of the camera from time t to time t+1, where R represents the rotation matrix and t represents the translation matrix. The pose here is obtained solely from the image and is the displacement of the service robot. The initial pose is obtained from the pose information, and the pose at the next time t+1 is estimated.

[0077] EPnP is a 3D-2D matching algorithm. Given the 3D coordinates of a feature point in the world coordinate system, p = [x,y,z]^T, the algorithm uses the projected coordinates of the feature point in the current frame, p = [u,v]^T, to solve for the pose transformation [Rt] between image frames, where u and v are the coordinates in the image.

[0078] To further optimize the above technical solution, a key frame refers to representative image data selected as the optimization solution to reduce the computational load on the server side. The key frame selection strategy is as follows: if the number of feature points tracked in the current frame is less than a feature point threshold, it is considered a key frame; if the average disparity of the features of the current frame relative to the nearest key frame is greater than a set disparity threshold, it is considered a key frame.

[0079] To further optimize the above technical solution, the effective measurement range of the depth vision camera is about 0.5m to 5.0m. Too far or too close will affect the accuracy of the depth value measurement, and there are cases where the depth values ​​of some pixels are missing. Therefore, in the above case, when estimating the pose of the service robot, only triangulation is used to obtain the missing depth values ​​and then to obtain the pose. The displacement is the pose during the machine's movement. The pose of S3 is obtained solely from the image and requires further backend optimization by the backend server to reduce errors.

[0080] Backend optimization includes initialization and pose estimation of image data processing. In order to obtain more reliable pose data under long-endurance conditions, after obtaining visual constraints, IMU constraints and system initialization, the system backend performs state fusion on the obtained constraint relationship and solves the cost function by minimizing the visual residual and the IMU pre-integrated residual function.

[0081] Select the state variables as parameters within the sliding window, including the position, attitude, velocity of all n+1 depth vision cameras and the bias of the inertial measurement units (IMUs), calculate the extrinsic parameters from the depth vision cameras to the IMUs and the inverse depth of m feature points;

[0082]

[0083] X is the parameter within the sliding window, where k = 0, 1, ..., n, where k represents the state variables corresponding to each keyframe, namely position, velocity, attitude, and IMU bias, respectively. These variables are obtained from pre-integrated data processed by the IMU, which includes an accelerometer and a gyroscope. These are the transformation matrices between the depth vision camera and the accelerometer and gyroscope of the IMU, respectively, derived from the initialization process. The cost function is optimized using inverse depth, which refers to the reciprocal of the depth value corresponding to the image feature point. This facilitates the calculation of the cost function, but it is only used for mathematical optimization. Depth values ​​are usually quite large, which can easily lead to an overemphasis on depth value optimization during optimization, so the reciprocal is taken.

[0084] To further optimize the above technical solution, the cost function in S5 is expressed as:

[0085]

[0086] The function J(.) represents some mathematical optimization calculations performed on the parameter X within the sliding window to reduce the error of X; the above The expansion is as follows:

[0087]

[0088] Where, ||r c (X)|| 2 Represents the reprojection residual of vision; ||r b (X)|| 2 Represents the residuals of the pre-integrated data;

[0089] The purpose of the cost function is to use previous trajectory information for global optimization to eliminate accumulated errors.

[0090] To further optimize the above technical solution, the optimization involves fine-tuning the inaccurate camera pose and reconstructed map obtained from the visual front end. A nonlinear method is adopted, considering all state variables of the system, and making an optimal estimate of them in noisy observations, that is, maximizing their posterior probability. Thus, a least squares problem with pose and map points as optimization variables and noise variance as optimization objective can be derived, and the cost function can be solved quickly and accurately. It relies solely on pose information obtained from images, and IMU data also contains errors. Combining the two further improves the accuracy of pose. The specific algorithms all use traditional nonlinear optimization methods.

[0091] Example

[0092] This embodiment discloses a multi-sensor fusion service robot collaborative mapping system, such as... Figure 1As shown, the system includes a ground service robot, a depth vision camera, an inertial measurement unit (IMU), a microcomputer (NUC), a communication module, and a server. The depth vision camera is used to acquire image data of the service robot's surrounding environment, including color and depth images. The inertial measurement unit (IMU) is used to acquire instantaneous velocity data of the service robot, including velocity, acceleration, and angular acceleration information. The microcomputer is deployed on the service robot and is used to fuse the image data from the depth vision camera and the velocity data from the inertial measurement unit (IMU) to obtain the service robot's world coordinates. The communication module is used to send relevant data to the backend server. The server is used to receive and process the data sent by each service robot, ultimately obtaining a global map.

[0093] Service robots, depth vision cameras, inertial measurement units (IMUs), and minicomputers (NUCs) form the front end. The data acquisition and processing process of a single service robot within this front end is as follows: Figure 2 As shown, it includes:

[0094] S11: Before a single service robot moves indoors, it is initialized. During the movement, the depth camera continuously acquires image data in front of the service robot, including color images and depth images, and then hands the image data over to a microcomputer for processing.

[0095] S12: During the movement of a single service robot indoors, the inertial measurement unit (IMU) continuously acquires the instantaneous velocity data of the current service robot, including velocity and acceleration information. The acceleration information includes linear acceleration and angular acceleration, and the information is then processed by a microcomputer.

[0096] S13: The microcomputer receives image data of the indoor environment, preprocesses the color images, extracts ORB features, performs feature matching on the color images at different times, estimates the pose of the service robot based on the found feature points and the depth image, and selects key frames.

[0097] S14: The microcomputer receives IMU data from the inertial measurement unit (IMU), including velocity data, position data, and acceleration data, and pre-integrates the IMU data;

[0098] S15: Construct a cost function from the estimated pose and aligned IMU pre-integrated data in steps S13 and S14, perform backend optimization, and improve pose accuracy;

[0099] The initial pose is estimated from images, which are captured at 30 frames per second, while the IMU data acquisition rate is approximately 200 frames per second. Therefore, the IMU receives significantly more data than the images captured by the depth vision camera. Thus, integration is performed on the IMU data between each keyframe; a schematic diagram of the integration process is shown below. Figure 5 As shown;

[0100] S16: Send the pose and keyframe information of the service robot to the backend server;

[0101] The server serves as the backend, and the process of collaborative mapping using multiple service robots and multi-sensor fusion is as follows: Figure 3 As shown, it specifically includes:

[0102] Step S21: Service robots depart from the same location in different directions, and Apriltag labels are affixed to the departure points of the service robots;

[0103] Step S22: Before each service robot departs, it obtains its own pose information through Apriltag, and obtains the transformation matrix between each service robot through coordinate transformation, and saves it on the server.

[0104] Step S23: Using the poses and keyframes sent by each service robot, the server constructs a local map with the initial point of each robot as the origin of the robot's coordinates.

[0105] Step S24: Using the transformation matrix between the various service robots obtained in S2, take the initial origin of one of the service robots A as the origin of the global map, project the local maps of the other service robots onto it, and obtain the global map.

[0106] A schematic diagram of the projection of the global map constructed from the local maps of each service robot is shown below. Figure 6 As shown.

[0107] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0108] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A collaborative mapping method for service robots using multi-sensor fusion, characterized in that, include: Step 1: Initialize several service robots, obtain the pose information of all service robots through the Apriltag visual reference library, and obtain the transformation matrix between each service robot through coordinate transformation; Step 2: All service robots start from the same point where Apriltag labels are pasted and move in different directions to collect image data and speed data; Step 3: Preprocess the image data, extract ORB features for feature matching, obtain feature points, estimate the pose of the service robot based on the feature points and pose information, and select keyframes. Step 4: Pre-integrate the speed data of each service robot to obtain pre-integrated data; Step 5: Construct a cost function based on the estimated pose and pre-integration data of each service robot, optimize the solution of the cost function, and obtain the high-precision pose of each service robot; A cost function is constructed based on the estimated pose and pre-integration data of each service robot, including: Select state variables as parameters within the sliding window, including the position, attitude, and velocity of all n+1 depth vision cameras and the bias of the inertial measurement unit (IMU), calculate the extrinsic parameters from the depth vision camera to the IMU and the inverse depth of m feature points; ； Where X is the parameter within the sliding window, k=0,1,...,n For the state variable corresponding to the k-th keyframe, The meanings are, in order, position, velocity, attitude, and IMU bias, all obtained through pre-integrated data preprocessed by the inertial measurement unit (IMU), which includes an accelerometer and a gyroscope. The transformation matrices between the accelerometer and gyroscope of the depth vision camera and the inertial measurement unit (IMU) are respectively derived from the initialization process; the input variable of the cost function is X, using inverse depth... To optimize the cost function, the inverse depth refers to the reciprocal of the depth value corresponding to the image feature point; Based on the parameter X within the sliding window, a cost function is constructed, expressed as: ； The function J(.) represents an optimization calculation of parameter X within the sliding window, reducing the error of X; the above The expansion is as follows: ； in, Represents the reprojection residual of vision; Represents the residuals of the pre-integrated data; Step 6: Based on the high-precision pose and keyframes of each service robot, and using their corresponding starting points as the origin, construct a local map for each service robot. Step 7: Select any starting point of a service robot as the origin of the global map. Based on the transformation matrix between the service robots, project all local maps onto the coordinate system of the origin of the global map to obtain the global map.

2. The multi-sensor fusion collaborative mapping method for service robots according to claim 1, characterized in that, The initialization in step 1 refers to obtaining the displacement and rotation relationship between the inertial measurement unit (IMU) for collecting speed data and the origin of the depth vision camera for collecting image data on the service robot, as well as the offset of the IMU; and obtaining the displacement and rotation relationship between the origin of the coordinate system of the IMU and the depth vision camera through calibration methods to obtain the transformation matrix.

3. The multi-sensor fusion collaborative mapping method for service robots according to claim 1, characterized in that, The specific process of pose estimation in step 3 is as follows: Step 31: The image data includes color images and depth images. The color images are preprocessed by histogram equalization to enhance image contrast and by adaptive thresholding to convert the color images into grayscale images. Step 32: Extract ORB features from the preprocessed color image and perform feature matching on the color images at different time points to obtain feature points; Step 33: Estimate the pose of each service robot based on feature points, depth images, and pose information, and select keyframes.

4. The multi-sensor fusion collaborative mapping method for service robots according to claim 1, characterized in that, The keyframe selection strategy is as follows: when the number of feature points tracked in the current frame is less than a set feature point threshold, the current frame is selected as the keyframe; when the average disparity of the current frame relative to the nearest keyframe is greater than a set disparity threshold, the current frame is selected as the keyframe.

5. The multi-sensor fusion collaborative mapping method for service robots according to claim 2, characterized in that, Based on the pose and pre-integration data, a cost function is constructed using the transformation matrix. A nonlinear algorithm is then used to optimize and solve the cost function to obtain a high-precision pose.

6. A collaborative mapping system for a service robot collaborative mapping method based on multi-sensor fusion according to any one of claims 1-5, characterized in that, It includes a service robot and a server; the service robot includes a depth vision camera, an inertial measurement unit, a microcomputer, and a communication module. The depth vision camera is used to acquire image data of the service robot's surrounding environment, and the image data includes color images and depth images; The inertial measurement unit is used to acquire the instantaneous speed data of the service robot; The microcomputer is used to fuse the image data and the speed data, select keyframes from the image data, and obtain the world coordinates of the service robot; The communication module is used to send keyframes and world coordinates to the server; The server is used to receive and process keyframes and world coordinates sent by several of the service robots, and finally obtain a global map.

Images