Multi-mobile robot cooperative positioning method and system based on vision-imu fusion

Abstract

The present application relates to sensor fusion, cooperative positioning technology field, more specifically, to a kind of multi-mobile robot cooperative positioning method and system based on vision-IMU fusion.This method includes: step S1, camera and IMU are parameterized;Step S2, IMU pre-integration is calculated, data alignment is carried out;Step S3, extract image ORB feature, and feature matching is carried out according to BRIEF descriptor;Step S4, optimization problem is established, and the optimal pose estimation in the local coordinate system of robot terminal is solved;Step S5, the pose obtained by optimization is sent to server end with the extracted image ORB feature data;Step S6, loop detection is carried out using bag-of-words model;Step S7, pose graph optimization is carried out under global unified coordinate, and the optimization result is sent to robot terminal.The present application can accurately and stably output the pose of multi-robot under global unified coordinate system, and has promoting significance for multi-mobile robot to cooperatively execute task in complex unknown environment.

Description

Technical Field

[0001] This invention relates to the field of sensor fusion and cooperative localization technology, and more specifically, to a method and system for cooperative localization of multiple mobile robots based on vision-IMU fusion. Background Technology

[0002] High-precision positioning, as one of the fundamental common technologies of intelligent unmanned systems, plays a crucial role in areas such as robot autonomous navigation and cooperative control. During movement, a robot needs to estimate its position on a map at every moment, essentially solving the question "Where am I?" In particular, when facing complex tasks, multiple robots need to cooperate to complete them. Therefore, each robot needs to know its own pose and its pose relative to other robots, necessitating a unified coordinate system for positioning all robots.

[0003] Positioning systems are mainly classified into two types based on the type of positioning device: positioning systems that rely on external devices and autonomous positioning systems that rely on their own onboard devices. Positioning systems that rely on external devices, such as GPS and motion capture systems, have limited applicability despite their high accuracy and robustness.

[0004] When GPS is used in scenarios such as long tunnels, low-altitude areas with tall buildings, or indoor environments, it cannot output high-precision positioning results due to signal loss or the multipath effect.

[0005] Motion capture systems require the installation of multiple infrared cameras, and both the cameras and the site need to be carefully calibrated beforehand. Robots equipped with motion capture systems can only move within a predefined area, which greatly limits their applications.

[0006] Compared to external positioning devices, the sensors carried by the robot itself, such as cameras and IMUs (Inertial Measurement Units), have a wider range of applications.

[0007] Cameras can capture images and provide rich visual information. However, camera-based single-vision localization methods are prone to failure when faced with complex environments, such as constantly changing lighting, rapidly moving viewpoints, and weakly textured structured features.

[0008] IMUs can provide triaxial acceleration and triaxial angular acceleration, but the measured data contains a lot of noise and cannot eliminate accumulated errors, making them prone to failure in large-scale, long-distance scenarios.

[0009] Meanwhile, the sensors carried by these bodies collect data in their own coordinate systems, which poses a huge challenge to collaborative positioning in a unified coordinate system. Summary of the Invention

[0010] The purpose of this invention is to provide a multi-mobile robot cooperative localization method and system based on vision-IMU fusion, which solves the problem of low accuracy of multi-mobile robot cooperative localization in complex and unknown environments in the prior art.

[0011] To achieve the above objectives, this invention provides a multi-mobile robot cooperative localization method based on vision-IMU fusion, comprising the following steps:

[0012] Step S1: Perform parameter calibration on the camera and IMU, and obtain the intrinsic parameters of the camera and IMU respectively, and obtain the extrinsic parameters of the camera and IMU respectively;

[0013] Step S2: The robot terminal calculates the IMU pre-integration based on the IMU measurement data and the motion equation, and performs data alignment.

[0014] Step S3: The robot terminal extracts the ORB features of the image, performs feature matching based on the BRIEF descriptor, and removes mismatched features.

[0015] Step S4: The robot terminal establishes an optimization problem based on the observation equation and the motion equation, and solves the optimization problem to obtain the optimal pose estimate of the robot terminal in the local coordinate system.

[0016] Step S5: The robot terminal sends the optimized pose and extracted image ORB feature data to the server.

[0017] Step S6: The server receives pose and feature data sent by all robot terminals, uses the bag-of-words model to perform loop closure detection, and calculates the relative pose transformation between different loops.

[0018] Step S7: The server performs pose graph optimization under globally unified coordinates to obtain a globally consistent optimal pose estimate, and sends the optimization result to the robot terminal.

[0019] In one embodiment, step S1 further includes the following steps:

[0020] Step S11: Using a standard checkerboard calibration board, acquire camera image data, project the three-dimensional space point P of the camera coordinate system onto the normalized image plane to form the normalized coordinates of point P, project the points on the normalized coordinates onto the pixel plane through the camera intrinsic parameters to obtain the corresponding positions in the pixel coordinate system, and calculate the camera intrinsic parameters.

[0021] Step S12: Keep the robot terminal stationary for a specified time, record the IMU measurement data, and calculate the IMU intrinsic parameters based on the IMU measurement data and the IMU error model;

[0022] Step S13: Using a standard checkerboard calibration board, collect camera image data and IMU measurement data. Based on the collected camera image data and IMU measurement data, combined with the camera intrinsic parameters and IMU intrinsic parameters, calculate the transformation relationship between the camera coordinate system and the IMU coordinate system.

[0023] In one embodiment, the pre-integration processing of the IMU data in step S2 is further implemented by the following expression:

[0024]

[0025] in, These are the pre-integral terms for position, velocity, and angle, respectively. These represent the random walk errors for linear acceleration and angular acceleration, respectively. and For IMU internal reference, Let b be the time t. k The rotation parameters of the frame in the IMU coordinate system.

[0026] In one embodiment, step S3 further includes:

[0027] Step S31: Extract FAST key points and BRIEF descriptors;

[0028] Step S32: Use Hamming distance to measure the difference of the M-dimensional vectors of the BRIEF descriptors, and use the fast approximate nearest neighbor algorithm to search and match the same feature in adjacent images;

[0029] Step S33: Use the random sampling consensus algorithm to remove feature mismatches.

[0030] In one embodiment, step S4 further includes:

[0031] Step S41: Establish a least squares optimization problem based on the observation equation and the motion equation. The corresponding expression is:

[0032]

[0033] The first term is the residual term between the IMU measurement and the motion equation, the second term is the residual term between the ORB features extracted from the camera image and the observation equation, and ρ(·) is the robust kernel function.

[0034] Step S42: Use the Levenberg-Marquardt algorithm to solve the least squares optimization problem in step S41 to obtain the optimal pose estimate of the robot terminal in the local coordinate system.

[0035] In one embodiment, step S5 further includes:

[0036] The robot terminal only sends data to the server at specific times.

[0037] The transmitted data includes: the spatial location of the feature, the corresponding descriptor, and the pose at the current time.

[0038] In one embodiment, step S6 further includes:

[0039] The server uses the local coordinate system of the robot terminal corresponding to the first frame of data received as a reference and sets it as a globally unified coordinate system.

[0040] The server performs loop closure detection and relative pose transformation on all subsequent data received from the robot terminals, mapping the poses of all robot terminals in the local coordinate system to a global unified coordinate system for representation.

[0041] In one embodiment, the loop closure detection in step S6 is implemented using a bag-of-words model, and further includes:

[0042] Collect descriptors and generate a dictionary using the K-means clustering algorithm;

[0043] The features of each keyframe are represented by words and their corresponding weights. The difference between two frames of data is calculated by comparing the weights corresponding to the two frames of data.

[0044] When the difference between two frames of data is less than a threshold, it is determined to be a loopback.

[0045] In one embodiment, step S7 further includes:

[0046] After detecting a loop closure, the server performs pose graph optimization in a globally unified coordinate system, optimizes the pose of all robot terminals, and sends the optimization results to the corresponding robot terminals.

[0047] Global uniform pose graph optimization is achieved through the following formula:

[0048]

[0049] The first term is the residual term constructed by the same robot terminal, and the second term is the residual term constructed by different robot terminals based on the loop.

[0050] To achieve the above objectives, the present invention provides a multi-mobile robot cooperative localization system based on vision-IMU fusion, comprising multiple robot terminals and a server:

[0051] The multiple robot terminals communicate with the server to exchange data.

[0052] The plurality of robot terminals and server terminals are used to implement the method described in any of the above.

[0053] This invention proposes a multi-mobile robot cooperative localization method and system based on vision-IMU fusion, which is used for autonomous cooperative localization of mobile robots. It can accurately and stably output the pose of multiple robots in a globally unified coordinate system, which is of great significance for promoting the cooperative execution of tasks by multiple mobile robots in complex and unknown environments. Attached Figure Description

[0054] The above and other features, properties and advantages of the present invention will become more apparent from the following description taken in conjunction with the accompanying drawings and embodiments, in which the same reference numerals always denote the same features, wherein:

[0055] Figure 1 A flowchart of a multi-mobile robot cooperative localization method based on vision-IMU fusion according to an embodiment of the present invention is disclosed;

[0056] Figure 2 A robot terminal architecture diagram according to an embodiment of the present invention is disclosed;

[0057] Figure 3 A diagram illustrating the architecture of a multi-mobile robot cooperative localization system based on vision-IMU fusion according to an embodiment of the present invention is disclosed.

[0058] Figure 4 A diagram showing the localization trajectory result of a single robot according to an embodiment of the present invention is disclosed;

[0059] Figure 5 A diagram showing the results of multi-robot cooperative localization trajectory according to an embodiment of the present invention is disclosed.

[0060] The meanings of the labels in the figures are as follows:

[0061] 11 robot terminals;

[0062] 1n robot terminal;

[0063] 20. Server-side. Detailed Implementation

[0064] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention.

[0065] The present invention proposes a multi-mobile robot cooperative localization method and system based on vision-IMU fusion, which is designed for complex and unknown environments. Based on a tightly coupled optimization state estimation paradigm, it achieves high-precision multi-robot cooperative localization through sensor fusion. On an embedded platform, it can accurately and stably output the poses of multiple robots in a unified coordinate system and can be applied to service robots, search and rescue robots and autonomous driving.

[0066] Figure 1 A flowchart of a multi-mobile robot cooperative localization method based on vision-IMU fusion according to an embodiment of the present invention is disclosed, such as... Figure 1 As shown, the multi-mobile robot cooperative localization method based on vision-IMU fusion proposed in this invention includes the following steps:

[0067] Step S1: Perform parameter calibration on the camera and IMU, and obtain the intrinsic parameters of the camera and IMU respectively, and obtain the extrinsic parameters of the camera and IMU respectively;

[0068] Step S2: The robot terminal calculates the IMU pre-integration based on the IMU measurement data and the motion equation, and performs data alignment.

[0069] Step S3: The robot terminal extracts the ORB features of the image, performs feature matching based on the BRIEF descriptor, and removes mismatched features.

[0070] Step S4: The robot terminal establishes an optimization problem based on the observation equation and the motion equation, and solves the optimization problem to obtain the optimal pose estimate of the robot terminal in the local coordinate system.

[0071] Step S5: The robot terminal sends the optimized pose and extracted image ORB feature data to the server.

[0072] Step S6: The server receives pose and feature data sent by all robot terminals, uses the bag-of-words model to perform loop closure detection, and calculates the relative pose transformation between different loops.

[0073] Step S7: The server performs pose graph optimization under globally unified coordinates to obtain a globally consistent optimal pose estimate, and sends the optimization result to the robot terminal.

[0074] This invention proposes a multi-mobile robot cooperative localization method and system based on vision-IMU fusion. It integrates image features and IMU acceleration data for localization. The robot terminal sends its coordinates in its local coordinate system and the collected feature data to the server. The server performs loop closure detection, relative pose transformation, and global pose graph optimization to map the robot's coordinates to a global unified coordinate system. The optimized global coordinates are then sent to the corresponding robot terminal. This method complements the advantages of camera and IMU sensors, improving the accuracy and robustness of robot localization in complex environments.

[0075] These steps will be described in detail below. It should be understood that, within the scope of this invention, the above-described technical features of this invention and the technical features specifically described below (such as in the embodiments) can be combined and related to each other to form preferred technical solutions.

[0076] Step S1: Perform parameter calibration on the camera and IMU, and obtain the intrinsic parameters of the camera and IMU respectively, and obtain the extrinsic parameters of the camera and IMU respectively.

[0077] RGB camera and IMU parameter calibration: obtain the intrinsic parameters of the camera and IMU, and the extrinsic parameters of the camera and IMU, i.e., the coordinate transformation relationship between the camera and IMU.

[0078] The camera intrinsics were calibrated using the open-source camera calibration tool Kalibr from ROS, and the IMU intrinsics were calibrated using the Imu_utils tool.

[0079] Furthermore, step S1 includes the following steps:

[0080] Step S11: Using a standard checkerboard calibration board, acquire camera image data, project the three-dimensional space point P(X,Y,Z) of the camera coordinate system onto the normalized image plane to form the normalized coordinates [x,y] of point P, project the point on the normalized coordinates onto the pixel plane through the camera intrinsic parameters to obtain its corresponding position [u,v] in the pixel coordinate system, and calculate the camera intrinsic parameters.

[0081] The corresponding expression is as follows:

[0082]

[0083] Among them, f x f y c x c y This refers to the camera's internal parameters.

[0084] Step S12: Place the robot carrying the IMU at rest for a specified period of time, record the IMU measurement data, and calculate the IMU intrinsic parameters based on the IMU measurement data and the IMU error model.

[0085] The corresponding expression is as follows:

[0086]

[0087] in, These are the random walk errors for linear acceleration and angular acceleration, respectively, n a n b Gaussian white noise for linear acceleration and angular acceleration, respectively. Let t be the rotation parameter from the world coordinate system to the IMU coordinate system at time t.

[0088] Step S13: Using a standard checkerboard calibration board, acquire camera image data and IMU measurement data. Based on the acquired camera image data and IMU measurement data, combined with the camera intrinsic parameters and IMU intrinsic parameters obtained in steps S11 and S12, calculate the transformation relationship between the camera coordinate system and the IMU coordinate system.

[0089] Step S2: The robot terminal calculates the IMU pre-integration based on the IMU measurement data and the motion equation, and performs data alignment.

[0090] Figure 2 A robot terminal architecture diagram according to an embodiment of the present invention is disclosed, such as... Figure 2 As shown, since the IMU data acquisition frequency is much higher than that of the camera, and the timestamps of the two sensors are inconsistent, and in order to reduce the computational load in the subsequent optimization process, the IMU data is pre-integrated to align the camera image with the IMU data. This is achieved through the following formula:

[0091]

[0092] in, These are the pre-integral terms for position, velocity, and angle, respectively. These represent the random walk errors for linear acceleration and angular acceleration, respectively. and For IMU internal reference, Let b be the time t. k The rotation parameters of the frame in the IMU coordinate system.

[0093] Step S3: The robot terminal extracts the ORB features of the image, performs feature matching based on the BRIEF descriptor, and removes mismatched features.

[0094] The robot terminal extracts ORB features from the image, performs feature matching based on the BRIEF descriptor, and uses the Random Sample Consensus (RANSAC) algorithm to remove mismatched features.

[0095] like Figure 2As shown, step S3 further includes:

[0096] Step S31: Extract FAST key points and BRIEF descriptors;

[0097] The extracted ORB features consist of two parts: FAST keypoints and BRIEF descriptors.

[0098] The FAST key point extraction process is as follows:

[0099] First, select a pixel p in the image and record its brightness I. Then, select 16 pixels on a circle with a radius of 3 centered on pixel p, and set a threshold T (in this embodiment, it is set to 0.2I).

[0100] If there are N (12 in this embodiment) consecutive points on the selected circle with a brightness greater than I+T or less than IT, they are considered key points.

[0101] The process of extracting BRIEF descriptors is as follows:

[0102] According to the predefined M (set to 128 in this embodiment) relatively random positions, the value of a pixel at a relatively random position is 1 if the brightness of the pixel is higher than that of the FAST key point, and 0 otherwise, forming an M-dimensional vector.

[0103] It should be emphasized that the threshold T and the number of N points can be set according to requirements, and the dimension M of the BRIEF descriptor vector can also be set according to requirements.

[0104] Step S32: Use Hamming distance to measure the difference of the M-dimensional vectors of the BRIEF descriptors, and use the fast approximate nearest neighbor algorithm to search and match the same feature in adjacent images;

[0105] Step S33: Use the random sampling consensus algorithm to remove feature mismatches.

[0106] Random Sample Consensus (RANSAC) is a simple and effective method for noise removal. It initially uses as few points as possible to estimate model parameters, then iteratively expands the range of influence of the obtained model parameters. Data that does not meet the model's requirements at the end of the iteration is considered noise. In this embodiment, RANSAC is used to remove false matches.

[0107] The Random Sample Consensus (RANSAC) algorithm removes mismatches as follows:

[0108] Select n correct matching features from all the matching features, and use these n matching features to fit the transformation relationship between the two frames of images;

[0109] For the remaining matching features, calculate the distance from each matching feature to the transformation relation. If the distance exceeds a threshold, it is considered a mismatch; if the distance does not exceed the threshold, it is considered a correct match. Repeat this process.

[0110] Step S4: The robot terminal establishes an optimization problem based on the observation equation and the motion equation, and solves the optimization problem to obtain the optimal pose estimate of the robot terminal in the local coordinate system.

[0111] The robot terminal establishes an optimization problem based on the observation equation and the motion equation, and solves it using the Levenberg-Marquard algorithm to obtain the optimal pose estimate in its respective local coordinate system.

[0112] like Figure 2 As shown, step S4 further includes:

[0113] Step S41: Establish a least squares optimization problem based on the observation equation and the motion equation, and its formula is expressed as follows:

[0114]

[0115] The first term is the residual term between the IMU measurement and the motion equation, the second term is the residual term between the ORB features extracted from the camera image and the observation equation, and ρ(·) is the robust kernel function.

[0116] ρ(·) is the robust kernel function used to suppress outlier noise terms, and is achieved through the following formula:

[0117]

[0118] Where s is the residual term of the ORB features extracted from the image acquired by the camera and the observation equation.

[0119] Step S42: Use the Levenberg-Marquard algorithm to solve the optimization problem in S41 and obtain the optimal pose estimate in each local coordinate system.

[0120] The Levenberg-Marquardt algorithm is an optimization algorithm that, given initial values ​​for the optimization variables, seeks an increment in each iteration to minimize the objective function, and controls the iteration step size through a trust region. The iteration stops when the obtained increment is sufficiently small.

[0121] In this embodiment, the Levenberg-Marquardt algorithm is used to solve the optimization problem in step S41 to obtain the optimal pose estimate of the robot terminal in its respective local coordinate system.

[0122] Step S5: The robot terminal sends the optimized pose and extracted image ORB feature data to the server.

[0123] Robot terminal architecture such as Figure 2 As shown, the robot terminal sends the estimated pose and extracted image ORB features to the server.

[0124] In order to calculate the coordinates of each robot in a globally unified coordinate system, each robot terminal needs to send its own information to the server for processing.

[0125] In order to save communication bandwidth, the robot terminal only sends necessary data to the server at specific times.

[0126] Specifically, data is sent when the absolute distance between the current position of the robot terminal and the position of the last data transmission exceeds 10cm, or when the time interval between the last transmission and the last transmission exceeds 1 second.

[0127] The necessary data to be transmitted includes the spatial location of the feature, the corresponding descriptor, and the pose at the current time.

[0128] Step S6: The server receives pose and feature data sent by all robots, uses the bag-of-words model to perform loop closure detection, and calculates the relative pose transformation between different loops.

[0129] The server uses a bag-of-words model for loop closure detection. If a loop is detected, the relative pose transformation is calculated.

[0130] The server uses the robot terminal's local coordinate system corresponding to the first frame of data received as a reference and sets it as a globally unified coordinate system.

[0131] For subsequent data received from other robot terminals, the poses of all robot terminals are transformed into a globally unified coordinate system for representation through loop closure detection and relative pose transformation.

[0132] On the server side, a bag-of-words model is used for loop closure detection. Images captured by the camera at different times for the same robot terminal and when different robot terminals pass through the same place are found and data associations are established. If a loop is detected between different robots, the relative pose transformation between the two is calculated and aligned with the global coordinate system.

[0133] More specifically, after receiving data from different robot terminals, the server detects keyframes from different perspectives of the same scene using loop closure detection. Based on the pose in their respective local coordinate systems and the relative transformation of the keyframes detected by loop closure, the transformation relationship between the two local coordinate systems is calculated, thereby mapping the coordinates of the robot terminals in the local coordinate systems to the global unified coordinate system.

[0134] In this embodiment, loop closure detection is implemented using the bag-of-words model.

[0135] The bag-of-words model uses words to describe image features, with words derived from descriptors.

[0136] Using the bag-of-words model to implement loop closure detection includes the following steps:

[0137] First, descriptors are collected, and a dictionary is generated based on the K-means clustering algorithm to accelerate word lookup in subsequent keyframes.

[0138] To accelerate word lookup in subsequent keyframes, a hierarchical clustering approach is adopted. For each data sample in each layer, the K-Means algorithm is used to divide it into K classes, forming a k-ary tree to represent the dictionary.

[0139] The features of each keyframe are represented by words and their corresponding weights. To calculate the difference between two frames, the corresponding weights are compared, which is achieved using the following formula:

[0140]

[0141] Where, q i d i These are the feature vectors of two frames of data described using the bag-of-words model;

[0142] When the difference between two frames of data is less than a threshold, it is determined to be a loop closure, meaning that the two frames of image data observed the same location.

[0143] The K-Means algorithm is a commonly used clustering algorithm in unsupervised machine learning. It divides data samples into K classes. Initially, K centroids are randomly selected. For each sample, the distance between it and each centroid is calculated, and the smallest distance is taken as its class. Then, the centroid of each class is recalculated. The algorithm iterates until the change in each centroid is very small, at which point it converges and exits.

[0144] Step S7: The server performs pose graph optimization under globally unified coordinates to obtain a globally consistent optimal pose estimate, and sends the optimization result to the robot terminal.

[0145] After detecting a loop closure, the server performs pose graph optimization in a globally unified coordinate system, optimizes the pose of all robot terminals, eliminates the cumulative error in each robot terminal, and sends the optimization results to the corresponding robot terminal.

[0146] Global pose graph optimization is achieved through the following formula:

[0147]

[0148] The first term is the residual term constructed by the same robot terminal, and the second term is the residual term constructed by different robot terminals based on the loop.

[0149] Figure 3 A diagram illustrating the architecture of a multi-mobile robot cooperative localization system based on vision-IMU fusion according to an embodiment of the present invention is disclosed, such as... Figure 3 As shown, the multi-mobile robot collaborative positioning and coordination system based on vision-IMU fusion uses a processor with abundant computing resources as the server 20, and robot terminals 11, ..., 1n as clients.

[0150] like Figure 3 In the illustrated embodiment, the architecture used is a centralized architecture, meaning that each robot terminal 11, ..., robot terminal 1n must communicate with the server 20 to exchange data.

[0151] Robot terminals 11, ..., 1n and server 20 are used to implement, for example Figures 1 to 2 The method shown is a multi-mobile robot cooperative localization and coordination method based on vision-IMU fusion.

[0152] Each robot terminal includes a visual-inertial odometry system (VIS) to estimate the pose of the robot in its local coordinate system. For example... Figure 3 As shown, robot terminal 11 includes camera 111, IMU 112 and visual inertial odometry 113, and robot terminal 1n includes camera 1n1, IMU 1n2 and visual inertial odometry 1n3.

[0153] Each robot terminal sends its coordinates in its local coordinate system and the extracted image features to the server 20.

[0154] After receiving data from different robot terminals, the server 20 performs loop closure detection, relative pose transformation, and global pose graph optimization, and sends the optimized results to the corresponding robot terminals.

[0155] Figure 4 A robot terminal positioning trajectory result diagram according to an embodiment of the present invention is shown, such as... Figure 4 As shown, experimental results obtained through testing in real-world complex scenarios demonstrate that the multi-mobile robot cooperative localization method and system based on vision-IMU fusion proposed in this invention has high localization accuracy and robustness.

[0156] Figure 5The results of multi-robot cooperative localization trajectory are shown in an embodiment of the present invention. Experimental results show that the multi-mobile robot cooperative localization method and system based on vision-IMU fusion proposed in this invention has good performance in actual cooperative localization scenarios.

[0157] This invention proposes a multi-mobile robot cooperative localization method and system based on vision-IMU fusion, which is used for autonomous cooperative localization of mobile robots. It can accurately and stably output the pose of multiple robots in a globally unified coordinate system, which is of great significance for promoting the cooperative execution of tasks by multiple mobile robots in complex and unknown environments.

[0158] Although the methods described above are illustrated and depicted as a series of actions for the sake of simplicity, it should be understood and appreciated that these methods are not limited by the order of the actions, as some actions may occur in a different order and / or concurrently with other actions from the illustrations and descriptions herein or not illustrated and described herein but which may be understood by those skilled in the art, according to one or more embodiments.

[0159] As indicated in this application and claims, unless the context clearly indicates otherwise, the words "a," "an," "an," and / or "the" are not specifically singular and may include plural forms. Generally speaking, the terms "comprising" and "including" only indicate the inclusion of explicitly identified steps and elements, which do not constitute an exclusive list, and the method or apparatus may also include other steps or elements.

[0160] The above embodiments are provided for those skilled in the art to implement or use the present invention. Those skilled in the art can make various modifications or changes to the above embodiments without departing from the inventive concept of the present invention. Therefore, the protection scope of the present invention is not limited to the above embodiments, but should be the maximum scope that conforms to the innovative features mentioned in the claims.

Claims

1. A method for multi-mobile robot cooperative localization based on vision-IMU fusion, characterized in that, The method comprises the following steps: Step S1, calibrate the camera and the IMU, respectively obtain the intrinsic parameters of the camera and the IMU, and respectively obtain the extrinsic parameters of the camera and the IMU; Step S2, the robot terminal calculates the IMU pre-integration according to the IMU measurement data and the motion equation, and performs data alignment; Step S3, the robot terminal extracts image ORB features, performs feature matching according to BRIEF descriptors, and removes feature false matching; Step S4, the robot terminal establishes an optimization problem according to the observation equation and the motion equation, solves the optimization problem, and obtains the optimal pose estimation of the robot terminal in a local coordinate system; Step S5, the robot terminal sends the optimized pose and the extracted image ORB feature data to the server end; Step S6, the server end receives the poses and feature data sent by all robot ends, performs loop detection using a bag-of-words model, and calculates the relative pose transformation between different loops; Step S7, the server end performs pose graph optimization in a global unified coordinate system to obtain a globally consistent optimal pose estimation, and sends the optimization result to the robot terminal; The step S7 further comprises: After detecting the loop, the server end performs pose graph optimization in a global unified coordinate system, optimizes the poses of all robot terminals, and sends the optimization result to the corresponding robot terminal; The global unified pose graph optimization is realized by the following formula: The first term is a residual term constructed by the same robot terminal, and the second term is a residual term constructed by different robot terminals according to the loop.

2. The multi-mobile robot cooperative localization method based on vision-IMU fusion according to claim 1, characterized in that, The step S1 further comprises the following steps: Step S11, use a standard checkerboard calibration board to collect camera image data, project a three-dimensional space point P in the camera coordinate system to the normalized image plane to form a normalized coordinate of the point P, project the point on the normalized coordinate to the pixel plane through the camera intrinsic parameter to obtain the corresponding position in the pixel coordinate system, and calculate the camera intrinsic parameter; Step S12, record the IMU measurement data by keeping the robot terminal still for a specified time, and calculate the IMU intrinsic parameter according to the IMU measurement data and the IMU error model; Step S13, use a standard checkerboard calibration board to collect camera image data and IMU measurement data, calculate the transformation relationship between the camera coordinate system and the IMU coordinate system according to the collected camera image data and IMU measurement data, and the camera intrinsic parameter and the IMU intrinsic parameter.

3. The multi-mobile robot cooperative localization method based on vision-IMU fusion according to claim 1, characterized in that, The pre-integration processing of the IMU data in the step S2 is further realized by the following expression: where, are the pre-integrated terms of position, velocity, and angle, respectively, are the linear and angular acceleration random walk errors, respectively, and are the IMU intrinsic parameters, is the rotation parameter of frame b k at time t in the IMU coordinate system.

4. The multi-mobile robot cooperative localization method based on vision-IMU fusion according to claim 1, characterized in that, The step S3 further comprises: Step S31, extract FAST key points and BRIEF descriptors; Step S32, use Hamming distance to measure the difference of the M-dimensional vectors of the BRIEF descriptors, use a fast approximate nearest neighbor algorithm to search, and match the same features in adjacent images; Step S33, use a random sample consensus algorithm to remove feature false matching.

5. The multi-mobile robot cooperative localization method based on vision-IMU fusion according to claim 1, characterized in that, The step S4 further comprises: Step S41, establish a least squares optimization problem according to the observation equation and the motion equation, and the corresponding expression is: Wherein, the first term is the residual term of IMU measurement and motion equation, the second term is the residual term of ORB feature matching and observation equation extracted from the image collected by the camera, and p(·) is a robust kernel function. In step S42, the least square optimization problem in step S41 is solved by using the Levenberg-Marquardt algorithm to obtain the optimal pose estimation of the robot terminal in the local coordinate system.

6. The multi-mobile robot cooperative localization method based on vision-IMU fusion according to claim 1, characterized in that, The step S5 further comprises: The robot terminal only sends data to the server end at a specific time; The sent data includes the spatial position of the feature, the corresponding descriptor and the current time pose.

7. The multi-mobile robot cooperative localization method based on vision-IMU fusion according to claim 1, characterized in that, The step S6 further comprises: The server end sets the local coordinate system of the robot terminal corresponding to the received first frame data as the reference and sets it as the global unified coordinate system; The server end performs loop detection and relative pose transformation on all the data received from the robot end subsequently, and maps all the poses of the robot terminal in the local coordinate system to the global unified coordinate system for representation. 8.The method of claim 1, wherein, The loop detection in step S6 is realized by using a bag-of-words model, and further comprises: Collecting descriptors and generating a dictionary by using a K-means clustering algorithm; The features of each key frame are represented by words and corresponding weights, and the difference between two frames of data is calculated by comparing the weights corresponding to the two frames of data; When the difference between the two frames of data is lower than a threshold, it is determined that there is a loop. 9.A multi-mobile robot cooperative positioning system based on vision-IMU fusion, characterized in that, The method comprises a plurality of robot terminals and a server end: The plurality of robot terminals respectively communicate with the server end for data interaction, The plurality of robot terminals and the server end are used to realize the method according to any one of claims 1-8.

Images