Multi-robot dense pose association and visual-inertial joint robust positioning method and system

Abstract

The invention provides a multi-robot dense pose association and visual-inertial joint robust positioning method and system, and the method comprises the steps: S1, receiving frame data comprising poses of robots, and forming a first pose association between the poses of a plurality of pieces of frame data; s2, carrying out loopback detection on the frame data, and carrying out feature matching on map features and 2D features of the frame data to obtain 3D-2D point pairs; solving a relative pose between the current frame and the matching frame based on the 3D-2D point pair, and further constructing a second pose association; and S3, based on the first pose association and the second pose association, aligning a reference system of the robot with pose association, and calculating to obtain a long-term pose association pose of the robot, namely # imgabs0 #. The method solves the problems of a current multi-robot system in research and actual use, and is beneficial to reducing the research cost and improving the system operation efficiency.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot perception and control, and in particular, relates to a method and system for multi-robot dense posture association and visual-inertial joint robust positioning. Background Art

[0002] With the rapid development of mobile robot technology, the demand for intelligent robots in various application scenarios is growing, especially in complex, dynamic and unknown environments, where robots play an increasingly important role. In these environments, robots not only need to have the ability to make autonomous decisions, plan paths and perceive the environment, but also must be able to efficiently collaborate with other robots or systems around them to complete complex tasks.

[0003] For example, in the fields of disaster relief, agricultural monitoring, logistics and distribution, robots often need to work together to improve the efficiency and accuracy of task completion. To achieve this goal, multi-robot system has become a key technology that can complete tasks through the cooperation of multiple robots.

[0004] In multi-robot collaboration, robots not only need to share information, but also need to coordinate and synchronize in the same working environment. This requires robots to have the ability of self-positioning and collaborative positioning. In most environments, multi-robot joint positioning can be completed by relying on high-precision GPS. However, in some indoor or weak-signal GPS-denied environments, it is necessary to complete joint positioning in a unified reference system based on the sensors carried by multiple robots themselves. How to achieve joint positioning of multi-robot systems based on their own sensors is a key issue that needs to be solved in the current field of robotics research.

[0005] In order to ensure the positioning stability and robustness of the robot, sensor fusion technology is usually used, especially visual-inertial positioning technology based on vision and inertial measurement unit, i.e. IMU. The combination of visual-inertial sensors is widely used in intelligent robot positioning, navigation, mapping and other tasks due to its low cost, low power consumption, high stability and rich information. At present, by installing a visual-inertial sensor kit and building a visual-inertial SLAM system, i.e. VI-SLAM, it is possible to provide relatively accurate positioning and motion estimation for a single robot to a certain extent.

[0006] However, there are still many problems in the application of this system in a multi-robot collaborative environment.

[0007] Specifically, most existing VI-SLAM systems can only provide pose estimation for a single robot, but cannot effectively solve the relative pose between robots, thereby achieving effective joint positioning and collaboration between multiple robots. This limits its application in multi-robot systems. How to ensure the joint positioning accuracy, real-time and robustness of multi-robot systems is still an important problem in current intelligent robot research.

[0008] The main problems of existing joint localization methods are: 1) it is impossible to achieve dense posture associations within and between robots, so as to effectively utilize the observation capability advantages brought by the multi-robot system; 2) it is impossible to effectively eliminate abnormal posture associations, so the positioning may crash when key frame mismatch occurs; 3) it is impossible to quickly and accurately complete global posture optimization. Therefore, it is necessary to propose a technical solution to improve the above technical problems, so as to achieve fast, accurate and robust multi-robot joint localization.

[0009] Patent document CN115131434A discloses a method and system for collaborative mapping of multiple mobile robots based on visual sensors, and specifically discloses a method and system for collaborative mapping of multiple mobile robots based on visual sensors. The method includes the following steps: step S1, camera calibration; step S2, each sub-robot independently runs the visual SLAM algorithm to obtain its own posture and local map; step S3, each sub-robot transmits its own data to the server; step S4, the server detects the overlapping area of ​​the map; step S5, the server calculates the transformation matrix between the local map and the global map; step S6, the server fuses the local map into the global map; step S7, the server sends the global map data to each sub-robot; step S8, the sub-robot updates the local local map data. This solution cannot build dense posture associations between multiple robots. This problem needs to be solved urgently. Summary of the invention

[0010] In view of the defects in the prior art, the purpose of the present invention is to provide a multi-robot dense posture association and visual-inertial joint robust positioning method and system.

[0011] A multi-robot dense posture association method provided by the present invention includes:

[0012] Step S1: receiving frame data including robot postures, and forming a first posture association between postures of a plurality of frame data;

[0013] Step S2: loop back detection of the frame data, obtaining 3D-2D point pairs by feature matching of map features and 2D features of the frame data; solving the relative pose between the current frame and the matching frame based on the 3D-2D point pairs, and then constructing a second pose association;

[0014] Step S3: Based on the first posture association and the second posture association, align the reference system of the posture-associated robot, and calculate the long-term posture-associated posture of the robot, that is,

[0015] Preferably, in step S1, the pose of the frame data and the pose of the frame data of K frames before the frame data in the local map of the same agent form a first pose association; the first pose association, i.e., short- to medium-term data association, is used for pose constraint; the value range of K is 5 to 20;

[0016] The mathematical expression of the short- to medium-term data association is:

[0017]

[0018] in, and They are respectively a tracking result and another tracking result of the local odometer of the client to which the frame data belongs in the local reference system.

[0019] Preferably, the step S2 includes:

[0020] Step S2.1: loop closure detection of the frame data, and obtaining 3D-2D point pairs by feature matching of map features and 2D features of the frame data;

[0021] Step S2.2: solving the relative pose between the 3D-2D point pairs by using the PnP algorithm, and then constructing a second pose association;

[0022] In the step S2.1, the map feature of the frame data is the BRIEF feature corresponding to the map point output by the client local odometer on the current frame image;

[0023] In the step S2.1, it includes:

[0024] Step S2.1.1: Use the BoW bag-of-words model to calculate the similarity score between the current frame and all historical frames, and determine whether the similarity score is greater than 0.003. If the result is yes, loop detection is performed to make the frame with the highest similarity score be used as the loop frame; if the result is no, re-execute step S2.1.1;

[0025] Step S2.1.2: Calculate the Hamming distance between the map features of the current frame and the 2D features of the loop frame in pairs; determine whether the Hamming distance is less than 80 and less than 70% of the Hamming distance between the current feature point and all other 2D features. If the result is yes, the map features of the current frame and the 2D features complete the 3D-2D matching pair; if the result is no, do not process;

[0026] The second posture association is a long-term posture association; the long-term posture association refers to the data association between the current frame and the sliding window, that is, the frames other than the most recent K frames, or the data association between key frames of different robots;

[0027] In step S3, the long-term posture-related posture of the robot is calculated by the PnP algorithm.

[0028] According to a centralized multi-robot visual-inertial joint robust positioning method provided by the present invention, a multi-robot dense posture association method is adopted, comprising:

[0029] Step A: Multiple robots run local visual-inertial odometers locally to complete pose tracking in the local reference frame, and package the key frame pose and feature information into frame data;

[0030] Step B: Use the multi-robot dense pose association method to obtain the long-term pose association pose of the robot, that is,

[0031] Step C: Based on the long-term posture association posture and the four-degree-of-freedom pairwise consistency measurement, the abnormal posture association is screened and eliminated to obtain a screened posture association group;

[0032] Step D: Based on the screened pose association groups, multi-robot joint positioning is completed through cascade pose graph optimization.

[0033] Preferably, in said step A, it includes:

[0034] Step A1: Input the data obtained by the RGB camera and IMU into the visual-inertial odometer for posture tracking to obtain positioning results and feature data;

[0035] Step A2: Packing the positioning result and feature data to obtain frame data as a key frame object;

[0036] In step A1, the frame For example, the positioning result is the key frame pose T output by the odometer. i ; The characteristic data is frame The pixel coordinates of all sparse feature points in the image, the BRIEF descriptors at the feature points, the 3D coordinates of the odometer output map points, and the corresponding BRIEF descriptors of the map points;

[0037] In the step C, it includes:

[0038] Step C1: Consider all long-term posture associations as nodes, and then construct a posture undirected association graph;

[0039] Step C2: For any two long-term posture association nodes, that is, the first long-term posture association node lij and the second long-term pose associated node l lk , can calculate the pairwise consistency between pose associations based on the four-degree-of-freedom pairwise consistency index, and construct edges between consistent association nodes in the pose undirected association graph;

[0040] Step C3: Use the MAXCLIQUE algorithm to extract the maximum connected subgraph of the pose undirected association graph.

[0041] Preferably, in step C2, the four-degree-of-freedom pairwise consistency index is expressed as follows:

[0042] C(l ij , l lk )=|E(l ij , l lk )|

[0043] Among them, C is the consistency measurement index, C(l ij , l lk ) is the pose association l ij Associated with pose lk The consistency value between them, that is, the first long-term pose associated node l ij Node l is associated with the second long-term pose lk The consistency between E(l ij , l lk ) represents error;

[0044] Error, that is, E(l ij , l lk ) is:

[0045]

[0046] in, is the yaw angle consistency error, is the translation consistency error;

[0047] Yaw angle consistency error The mathematical expression is:

[0048]

[0049] in, and They are pose association l ij Associated with pose lk The corresponding yaw angle, and They are the relative yaw angles between frames j and l and between frames k and i in the local odometer positioning results;

[0050] Translation consistency error, i.e. The mathematical expression is:

[0051]

[0052] in, and They are pose association l ij Associated with pose lk The corresponding pose matrix, T jl With T ki are the relative positions between frame j and frame l and between frame k and frame i in the local odometer positioning results, respectively, and the symbol [·] t The translation vector representing the internal pose matrix;

[0053] In step C3, the MAXCLIQUE algorithm is used to extract the maximum connected subgraph of the pose undirected association graph.

[0054] Preferably, in step D, the process of cascade pose graph optimization includes:

[0055] Step D1: based on the maximum connected subgraph of the posture undirected association graph, roughly optimize the posture to obtain a rough optimization result;

[0056] Step D2: Using the rough optimization result as the initial value, perform global four-degree-of-freedom pose graph optimization to achieve precise joint positioning of multiple robots;

[0057] In the step D1, it includes:

[0058] Step D1.1: Based on the first posture association and the second posture association, define an optimization problem of a posture graph;

[0059] Step D1.2: construct intermediate variables with current pose values ​​and decompose pose errors;

[0060] Step D1.3: In the EM framework, alternately perform E-step updates and M-step updates until the pose converges to obtain a rough optimization result; the convergence condition is that the difference in translation between the output results of two iterations is less than 0.001m, the difference in yaw angle is less than 0.1 degree, or the number of iterations reaches 100.

[0061] Preferably, in step D1.1, the optimization problem is mathematically expressed as:

[0062]

[0063] in, is the set of all poses to be optimized, is the data association set, ||·||2 represents the two-norm; Represents the relative posture error of four degrees of freedom;

[0064] The four-degree-of-freedom relative posture error, referred to as posture error, is mathematically expressed as:

[0065]

[0066] Among them, T i With T j They are one key frame pose to be optimized and another key frame pose to be optimized. is the relative pose constraint matrix between two frames, is the yaw angle attitude error, is the translation pose error;

[0067] Yaw angle attitude error The mathematical expression is:

[0068]

[0069] in, and T i With T j The corresponding yaw angle, for The corresponding yaw angle;

[0070] Translational pose error The mathematical expression is:

[0071]

[0072] Among them, t i ,t j and T i 、T j and The corresponding translation vector, R i T i The corresponding rotation matrix;

[0073] In the step D1.2, an intermediate variable is constructed with the current value of the posture, and the posture error is disassembled to obtain a disassembly result; the disassembly result is used to define the intermediate variable; the intermediate variable includes: and

[0074] The mathematical expression of the disassembly result is:

[0075]

[0076] in, and are an arbitrary constant and another arbitrary constant respectively, and the superscript yaw represents the yaw angle;

[0077] In step D1.3, the intermediate variables are updated in step E. The updated mathematical expression is:

[0078]

[0079] The updated mathematical expression is:

[0080]

[0081] The M steps update the pose variables, t i and t j ;

[0082] The updated mathematical expression is:

[0083]

[0084] t i The updated mathematical expression is:

[0085]

[0086] The updated mathematical expression is:

[0087]

[0088] t j The updated mathematical expression is:

[0089]

[0090] Wherein, Avg(·) represents the average value of all internal summation items, and the symbol := represents an assignment operation.

[0091] A multi-robot dense posture association subsystem provided by the present invention includes:

[0092] Module M1: receiving frame data including robot posture, and forming a first posture association between postures of multiple frame data;

[0093] Module M2: loop closure detection of the frame data, obtaining 3D-2D point pairs by feature matching of map features and 2D features of the frame data; solving the relative poses between the 3D-2D point pairs, and then constructing a second pose association;

[0094] Module M3: Based on the first posture association and the second posture association, align the reference system of the posture-associated robot, and calculate the long-term posture-associated posture of the robot, that is,

[0095] A centralized multi-robot visual-inertial joint robust positioning system provided by the present invention can trigger the operation of a multi-robot dense posture association subsystem, including:

[0096] Module A: Multiple robots run local visual-inertial odometers locally to complete pose tracking in the local reference frame, and package key frame poses and feature information into frame data;

[0097] Module B: Trigger the multi-robot dense posture association subsystem to obtain the long-term posture association posture of the robot, that is,

[0098] Module C: Based on the long-term posture association posture and the four-degree-of-freedom pairwise consistency measurement, the abnormal posture association is screened and eliminated to obtain the screened posture association group;

[0099] Module D: Based on the screened pose association groups, multi-robot joint positioning is completed through cascade pose graph optimization.

[0100] Compared with the prior art, the present invention has the following beneficial effects:

[0101] 1. The present invention uses a short-medium-long three-level data association strategy to build dense posture associations between multiple robots and give full play to the advantages of the multi-robot system in observation capabilities;

[0102] 2. The present invention proposes a posture association screening method based on four-degree-of-freedom pairwise consistency measurement, which can effectively eliminate abnormal posture associations caused by mismatching and ensure positioning robustness. Compared with direct posture graph optimization, when the abnormal association accounts for 20%, the absolute positioning trajectory error can be reduced by about 72%;

[0103] 3. The present invention proposes a cascaded pose graph optimization pipeline, which can accurately realize large-scale pose graph optimization and ensure positioning accuracy. Compared with the standard pose graph optimization pipeline, the absolute positioning trajectory error can be reduced by about 13%;

[0104] 4. The present invention solves the problems of current multi-robot systems in research and practical use, and is effective in reducing research costs and improving system operating efficiency. Compared with a single-machine positioning odometer, the collaborative positioning pipeline proposed in the present invention can reduce the absolute positioning trajectory error by about 37%. BRIEF DESCRIPTION OF THE DRAWINGS

[0105] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0106] Figure 1 A schematic diagram of the system framework provided by the present invention;

[0107] Figure 2 A schematic diagram of the robust positioning method provided by the present invention;

[0108] Figure 3 Schematic diagram of the cone collision detection mechanism in the joint positioning process provided by the present invention, wherein C1 and C2 represent a camera and another camera respectively. DETAILED DESCRIPTION

[0109] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0110] A centralized multi-robot visual-inertial joint robust positioning method and system are provided according to the present invention.

[0111] Firstly, the present invention proposes a multi-robot dense pose association strategy, which inherits the short-term and medium-term pose associations based on the robot's local odometer and introduces the common view judgment mechanism of cone collision to supplement the long-term association, thereby greatly improving the density of pose associations.

[0112] Secondly, the present invention designs a posture association screening algorithm based on pairwise consistency detection, adopts four-degree-of-freedom error for consistency evaluation, and eliminates abnormal posture associations through adaptive measurement methods, thus ensuring the accuracy and robustness of posture associations;

[0113] Finally, the present invention proposes an efficient cascade pose graph optimization process, which first completes the key frame pose rough solution based on the EM algorithm, and then performs nonlinear optimization to complete the multi-robot precise joint positioning. By optimizing data association and pose graph processing, the present invention effectively solves the data density and accuracy problems in multi-robot joint positioning, improves positioning accuracy and stability, and has strong practicality and application prospects.

[0114] The present invention proposes a centralized multi-robot visual-inertial joint robust positioning method and system, and proposes its optimization strategy accordingly, realizing the complete implementation of the multi-robot joint positioning system. It can avoid the problems of low efficiency, poor accuracy, weak robustness, etc. of multi-robot joint positioning. The system framework diagram of the present invention is as follows Figure 1 shown.

[0115] According to a centralized multi-robot visual-inertial joint robust positioning method provided by the present invention, Figure 2 As shown in the figure, it aims at the research deficiencies in the field of multi-robot joint positioning and the above research key contents, including:

[0116] Step A: Multiple robots run local visual-inertial odometers locally to complete pose tracking in the local reference frame, and send key frame poses and feature information to the central server;

[0117] Step B: The central server builds the pose association between the data based on the data sent by the robot client and completes the reference frame alignment;

[0118] Step C: The central server screens and removes abnormal pose associations based on the four-degree-of-freedom pairwise consistency measurement.

[0119] Step D: The central server completes the multi-robot joint positioning based on the screened pose association groups through cascade pose graph optimization.

[0120] Step A specifically includes the following steps:

[0121] Step A1: Input the data obtained by the RGB camera and IMU into the visual-inertial odometer for posture tracking to obtain positioning results and feature data;

[0122] Step A2: Pack the positioning result and feature data, obtain and send the packed data to the central server;

[0123] Step A3: The central server receives the data and unpacks the data, and then constructs a keyframe object on the server side.

[0124] The positioning results and feature data are: For example, the positioning result is the key frame pose T output by the odometer. i ; The feature data is the frame The pixel coordinates of all sparse feature points in the image, the BRIEF descriptors at the feature points, the 3D coordinates of the odometer output map points, and the corresponding BRIEF descriptors of the map points;

[0125] Step B specifically includes the following steps:

[0126] Step B1: After the server receives the keyframe data, the keyframe pose will form a short- to medium-term data association with the poses of up to five keyframes before the keyframe in the local map of the same agent;

[0127] The data-associated relative posture is obtained based on the posture calculation in the local reference system of the key frame;

[0128] Specifically, the keyframe The corresponding server-side posture to be optimized T i and Corresponding posture T j A relative posture constraint will be formed between them, and the mathematical expression is:

[0129]

[0130] in, and They are respectively a tracking result and another tracking result of the local odometer of the client to which the key frame belongs in the local reference system;

[0131] Step B2: For the newly received key frame Perform loop detection based on the bag-of-words model, and after successfully triggering the loop, match the keyframes based on the BRIEF feature Map features and loop frames in The 2D features between the two frames are used to obtain 3D-2D point pairs; the relative pose between the two frames is solved by the PnP algorithm solution technology under the RANSAC framework, so as to build a long-term pose association between the two frames; the long-term pose association refers to the data association between the current frame and the sliding window, that is, the frames other than the most recent 5 frames, or the data association between different robot key frames; the PnP algorithm is a mature algorithm and is a built-in algorithm of OpenCV;

[0132] Specifically, the BoW bag-of-words model is used to calculate the similarity score between the current frame and all historical frames. If the highest score is greater than 0.003, the loop detection is successful and the frame with the highest similarity score is used as the loop frame.

[0133] Then, the Hamming distance between the map features of the current frame and the 2D features of the loop frame is calculated. If the Hamming distance between a map point feature and a 2D feature is less than 80, and less than 70% of the Hamming distance between the current feature point and all other 2D features, the current map point and the 2D feature are considered to be a 3D-2D matching pair;

[0134] Specifically, long-term association is a term in SLAM, short-term association refers to the association between adjacent frames, medium-term association refers to the data association of frames within the sliding window, and long-term association refers to the association between the current frame and the frame outside the sliding window or the data association between different robot key frames. The sliding window size is defined as 5 frames;

[0135] Step B3: If robot a i With robot a j The long-term pose association between the key frames in each reference frame is successfully constructed, and the association is connected to the robot reference frame. i has been aligned to the global reference system, and the other side aj If not aligned yet, complete a j Local reference frame alignment;

[0136] Specifically, the odometers running locally on different robots are positioned in their own reference frames. Only by completing the alignment of the reference frames can the joint positioning of multiple robots be achieved.

[0137] Assume robot a i Loopback frame With robot a j Corresponding frame Long-term posture association Robot j Relative transformation between local reference frame and global reference frame for:

[0138]

[0139] in, Loopback frame The corresponding pose in the global reference frame, and The frame In a j The pose in the local reference frame;

[0140] Step B4: Through the cone collision detection, such as Figure 3 As shown, the long-term pose association is supplemented, that is, given two frames, namely, frames With frame Calculate frame The viewing frustum encloses the sphere and the frame Whether there is a common area between the bounding spheres of the viewing cone; frame The viewing cone encloses the center of the sphere O2 and the frame The radius r2 of the bounding sphere of the viewing cone is:

[0141]

[0142] Among them, T2 is the frame The aligned pose of the frustum bounding the sphere, D is the frame The frustum of the bounding sphere corresponds to the effective observation distance of the camera, and θ is the frame The bounding sphere of the viewing cone corresponds to the field of view angle of the camera. The superscript T indicates the matrix transpose;

[0143] If there is co-viewing between two frames, then:

[0144]

[0145] Among them, the symbol Indicates existence; N indicates an element in the set N; the symbol · indicates vector dot product; the symbol T1 indicates frame The pose matrix of It is the normal vector set of the four surfaces outside the bottom of the camera frustum in its camera coordinate system; when the collision constraint conditions are met between two frames, you can try to match the frames With frame If the number of matching pairs is greater than 15, the PnP algorithm can be used to solve the long-term pose correlation between the two frames, that is,

[0146] Step C specifically includes the following steps:

[0147] Step C1: Consider all long-term posture associations as nodes and preliminarily construct a posture undirected association graph;

[0148] Step C2: For any two long-term posture association nodes, that is, the first long-term posture association node l ij and the second long-term pose associated node l lk , can calculate the pairwise consistency between pose associations based on the four-degree-of-freedom pairwise consistency index, and build edges between consistent association nodes in the association graph;

[0149] Specifically, in step C2, the undirected graph structure includes nodes and edges. Two nodes connected by an edge are considered to be connected nodes. After the nodes and edges are constructed, the MAXCLIQUE algorithm can be used to calculate the maximum connected subgraph of the graph.

[0150] Among them, the mathematical expression of the four-degree-of-freedom pairwise consistency index is:

[0151] C(l ij , l lk )=|E(l ij , l lk )|

[0152] Among them, C is the consistency measurement index, C(l ij , l lk ) is the pose association l ij With l lk The consistency value between them, error E(l ij , l lk ) is defined as:

[0153]

[0154] in, and They are pose association l ij With l lk The corresponding yaw angle, and are the relative yaw angles between frames j and l and between frames k and i in the local odometer positioning results, and They are pose association l ij With l lk The corresponding pose matrix, T jl With T ki are the relative positions between frames j and l and between frames k and i in the local odometer positioning results, respectively, and the symbol [·] t The translation vector representing the intrinsic pose matrix.

[0155] Step C3: Use the MAXCLIQUE algorithm to extract the maximum connected subgraph of the undirected association graph, and all nodes outside the subgraph are removed as abnormal data associations.

[0156] Specifically, the MAXCLIQUE algorithm is used to extract the maximum connected subgraph of an undirected graph. Specifically, the MAXCLIQUE algorithm takes an undirected graph as input and the maximum connected subgraph of the undirected graph as output. A connected graph refers to a graph in which all nodes are connected to each other, that is, there is an edge between any two nodes. A subgraph of a graph refers to a graph that only contains some edges and nodes of the graph. A connected subgraph refers to a subgraph that meets the conditions of a connected graph. The maximum connected subgraph refers to a connected subgraph with the largest number of nodes. This algorithm is a mature algorithm.

[0157] The cascade pose graph optimization pipeline is embodied in step D;

[0158] Step D comprises the following steps:

[0159] Step D1: By introducing intermediate variables, rough pose optimization is completed under the EM framework;

[0160] The specific steps include:

[0161] Step D1.1: Define the pose graph optimization problem as:

[0162]

[0163] in, is the set of all poses to be optimized, is the set of two-tuples of key frame numbers connected by all pose associations, i.e., short-medium-long-term pose associations, and ||·||2 represents the two-norm;

[0164] in, is a data association set, is the relative posture error of four degrees of freedom:

[0165]

[0166] Among them, Ti With T j is the key frame pose to be optimized, is the relative pose constraint matrix between two frames, and They are the poses to be optimized T i With T j The corresponding yaw angle, Relative pose constraint The corresponding yaw angle, t i ,t j and T i 、T j and The corresponding translation vector, R i Then T i The corresponding rotation matrix.

[0167] Step D1.2: Introducing intermediate variables and Disassembly of pose error The mathematical expression is:

[0168]

[0169] in, and It can be any constant, and the superscript yaw represents the yaw angle.

[0170] Step D1.3: In the EM framework, alternate between E and M steps, and update the intermediate variables in the E step. and

[0171]

[0172] In the M step, the state variables are updated and t j ;by t i For example:

[0173]

[0174] Wherein, Avg(·) represents the average value of all internal summation items, and the symbol := represents an assignment operation.

[0175] Step D2: Using the rough optimized pose as the initial value, perform global four-degree-of-freedom pose graph optimization to achieve precise joint positioning of multiple robots.

[0176] The present invention also provides a centralized multi-robot visual-inertial combined robust positioning system, the system comprising the following modules:

[0177] Module A: The robot completes local motion tracking based on the visual-inertial odometer locally, and transmits the tracking results and related data to the central server through the ROS communication interface;

[0178] Module A1: The robot carries a calibrated RGB camera and IMU to capture the surrounding environment information. The camera intrinsic parameters, IMU intrinsic parameters, and sensor extrinsic parameters are calibrated by the Kalibr calibration tool;

[0179] Module A2: Based on the sensor data from the camera and IMU, the robot uses VINS-Mono to complete positioning in the local reference frame;

[0180] Module A3: The robot packages the odometer keyframe data and sends it to the central server. The packaged data includes posture, feature point location, feature point descriptor, 3D map point location and map point descriptor;

[0181] Module B: Construct multi-robot multi-level data association based on multi-robot transmission data;

[0182] Module B1: Receive data transmitted by the robot client based on the ROS system communication module, unpack the data and construct keyframe objects;

[0183] Module B2: Based on the odometer tracking posture, build the short-term and medium-term posture association between the current key frame and the previous five frames;

[0184] Module B3: Based on BRIEF features and the BoW bag-of-words model, loop detection is performed between the current frame and all previous key frames, long-term pose associations are constructed between key frames that successfully trigger loop detection, and relative pose calculation is completed based on the PnP algorithm;

[0185] Module B4: Based on the cone collision detection, loop detection is performed between the current frame and all key frames whose local reference frames are aligned. Long-term pose association is constructed between the key frames that successfully trigger the loop detection, and relative pose solution is completed based on the PnP algorithm.

[0186] Module C: Screening multi-level data associations and eliminating abnormal data associations;

[0187] Module C1: Treat the long-term posture association as a node, and calculate the pairwise consistency value between nodes based on the four-degree-of-freedom evaluation index;

[0188] Module C2: Construct a long-term pose association undirected graph based on the pairwise consistency measurement results;

[0189] Module C3: Use the MAXCLIQUE algorithm to extract the maximum connected subgraph of the association graph. All long-term associations outside the subgraph are removed as abnormal associations.

[0190] Module D: Perform cascade pose graph optimization based on data association to achieve precise joint positioning of multiple robots.

[0191] Module D1: Construct a pose graph object based on the current pose association result;

[0192] Module D2: Based on the coarse optimization framework, the initial adjustment of key frame poses is completed under the EM algorithm paradigm;

[0193] Module D2.1: Construct all keyframes with current pose values and The corresponding intermediate variable and

[0194] Module D2.2: Alternately execute E-step and M-step operations until the pose converges. In the E-step operation, update the intermediate variables according to the current pose value. and In the M-step operation; according to the current intermediate variable value, complete the key frame pose t i and t j Updates;

[0195] Module D3: Using the rough optimization result as the initial value, the Levenberg-Marquardt method provided by the CERES optimization library is used to complete the keyframe four-degree-of-freedom pose graph optimization;

[0196] The present invention also provides a multi-robot dense pose association subsystem, which can be implemented by executing the process steps of the multi-robot dense pose association method, that is, those skilled in the art can understand the multi-robot dense pose association method as a preferred implementation of the multi-robot dense pose association subsystem.

[0197] A multi-robot dense posture association subsystem provided by the present invention includes:

[0198] Module M1: receiving frame data including robot posture, and forming a first posture association between postures of multiple frame data;

[0199] Module M2: loop closure detection of the frame data, obtaining 3D-2D point pairs by feature matching of map features and 2D features of the frame data; solving the relative poses between the 3D-2D point pairs, and then constructing a second pose association;

[0200] Module M3: Based on the first posture association and the second posture association, align the reference system of the posture-associated robot, and calculate the long-term posture-associated posture of the robot, that is,

[0201] The present invention also provides a centralized multi-robot visual-inertial combined robust positioning system, which can be implemented by executing the process steps of the centralized multi-robot visual-inertial combined robust positioning method, that is, those skilled in the art can understand the centralized multi-robot visual-inertial combined robust positioning method as a preferred implementation of the centralized multi-robot visual-inertial combined robust positioning system.

[0202] A centralized multi-robot visual-inertial joint robust positioning system provided by the present invention can trigger the operation of a multi-robot dense posture association subsystem, including:

[0203] Module A: Multiple robots run local visual-inertial odometers locally to complete pose tracking in the local reference frame, and package key frame poses and feature information into frame data;

[0204] Module B: Trigger the multi-robot dense posture association subsystem to obtain the long-term posture association posture of the robot, that is,

[0205] Module C: Based on the long-term posture association posture and the four-degree-of-freedom pairwise consistency measurement, the abnormal posture association is screened and eliminated to obtain the screened posture association group;

[0206] Module D: Based on the screened pose association groups, multi-robot joint positioning is completed through cascade pose graph optimization.

[0207] Those skilled in the art know that, in addition to realizing the system and its various devices, modules, and units provided by the present invention in a purely computer-readable program code, it is entirely possible to realize the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a hardware component, and the devices, modules, and units included therein for realizing various functions can also be regarded as structures within the hardware component; the devices, modules, and units for realizing various functions can also be regarded as both software modules for realizing the method and structures within the hardware component.

[0208] In the description of the present application, it should be understood that the terms "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0209] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A multi-robot dense pose association method, characterized in that: include: Step S1: receiving frame data including robot postures, and forming a first posture association between postures of a plurality of frame data; Step S2: loop back detection of the frame data, obtaining 3D-2D point pairs by feature matching of map features and 2D features of the frame data; solving the relative pose between the current frame and the matching frame based on the 3D-2D point pairs, and then constructing a second pose association; Step S3: Based on the first posture association and the second posture association, align the reference system of the posture-associated robot, and calculate the long-term posture-associated posture of the robot, that is, 2. The multi-robot dense pose association method according to claim 1, characterized in that: In the step S1, the pose of the frame data and the pose of the frame data of K frames before the frame data in the local map of the same agent form a first pose association; the first pose association, i.e., short-term data association, is used for pose constraint; the value range of K is 5 to 20; The mathematical expression of the short- to medium-term data association is: in, and They are respectively a tracking result and another tracking result of the local odometer of the client to which the frame data belongs in the local reference system.

3. The multi-robot dense pose association method according to claim 2, characterized in that: In the step S2, it includes: Step S2.1: loop closure detection of the frame data, and obtaining 3D-2D point pairs by feature matching of map features and 2D features of the frame data; Step S2.2: solving the relative pose between the 3D-2D point pairs by using the PnP algorithm, and then constructing a second pose association; In the step S2.1, the map feature of the frame data is the BRIEF feature corresponding to the map point output by the client local odometer on the current frame image; In the step S2.1, it includes: Step S2.1.1: Use the BoW bag-of-words model to calculate the similarity score between the current frame and all historical frames, and determine whether the similarity score is greater than 0.

003. If the result is yes, loop detection is performed to make the frame with the highest similarity score be used as the loop frame; if the result is no, re-execute step S2.1.1; Step S2.1.2: Calculate the Hamming distance between the map features of the current frame and the 2D features of the loop frame in pairs; determine whether the Hamming distance is less than 80 and less than 70% of the Hamming distance between the current feature point and all other 2D features. If the result is yes, the map features of the current frame and the 2D features complete the 3D-2D matching pair; if the result is no, do not process; The second posture association is a long-term posture association; the long-term posture association refers to the data association between the current frame and the sliding window, that is, the frames other than the most recent K frames, or the data association between key frames of different robots; In step S3, the long-term posture-related posture of the robot is calculated by the PnP algorithm.

4. A centralized multi-robot visual-inertial joint robust positioning method, using the multi-robot dense posture association method of claim 3, characterized in that: include: Step A: Multiple robots run local visual-inertial odometers locally to complete pose tracking in the local reference frame, and package the key frame pose and feature information into frame data; Step B: Use the multi-robot dense pose association method to obtain the long-term pose association pose of the robot, that is, Step C: Based on the long-term posture association posture and the four-degree-of-freedom pairwise consistency measurement, the abnormal posture association is screened and eliminated to obtain a screened posture association group; Step D: Based on the screened pose association groups, multi-robot joint positioning is completed through cascade pose graph optimization.

5. The centralized multi-robot visual-inertial joint robust positioning method according to claim 4 is characterized in that: In the step A, it includes: Step A1: Input the data obtained by the RGB camera and IMU into the visual-inertial odometer for posture tracking to obtain positioning results and feature data; Step A2: Packing the positioning result and feature data to obtain frame data as a key frame object; In step A1, the frame For example, the positioning result is the key frame pose T output by the odometer. i ; The characteristic data is frame The pixel coordinates of all sparse feature points in the image, the BRIEF descriptors at the feature points, the 3D coordinates of the odometer output map points, and the corresponding BRIEF descriptors of the map points; In the step C, it includes: Step C1: Consider all long-term posture associations as nodes, and then construct a posture undirected association graph; Step C2: For any two long-term posture association nodes, i.e., the first long-term posture association node l ij and the second long-term pose associated node l lk , can calculate the pairwise consistency between pose associations based on the four-degree-of-freedom pairwise consistency index, and construct edges between consistent association nodes in the pose undirected association graph; Step C3: Use the MAXCLIQUE algorithm to extract the maximum connected subgraph of the pose undirected association graph.

6. The centralized multi-robot visual-inertial combined robust positioning method according to claim 5, characterized in that: In step C2, the four-degree-of-freedom pairwise consistency index is expressed as follows: C(l ij ,l lk )=|E(l ij ,l lk )| Among them, C is the consistency measurement index, C(l ij , l lk ) is the pose association l ij Associated with pose lk The consistency value between them, that is, the first long-term pose associated node l ij Node l is associated with the second long-term pose lk The consistency between E(l ij , l lk ) represents error; Error, that is, E(l ij , l lk ) is: in, is the yaw angle consistency error, is the translation consistency error; Yaw angle consistency error The mathematical expression is: in, and They are pose association l ij Associated with pose lk The corresponding yaw angle, and They are the relative yaw angles between frames j and l and between frames k and i in the local odometer positioning results; Translation consistency error, i.e. The mathematical expression is: in, and They are pose association l ij Associated with pose lk The corresponding pose matrix, T jl With T ki are the relative positions between frame j and frame l and between frame k and frame i in the local odometer positioning results, respectively, and the symbol [·] t The translation vector representing the internal pose matrix; In step C3, the MAXCLIQUE algorithm is used to extract the maximum connected subgraph of the pose undirected association graph.

7. The centralized multi-robot visual-inertial combined robust positioning method according to claim 6, characterized in that: In step D, the process of cascade pose graph optimization includes: Step D1: based on the maximum connected subgraph of the posture undirected association graph, roughly optimize the posture to obtain a rough optimization result; Step D2: Using the rough optimization result as the initial value, perform global four-degree-of-freedom pose graph optimization to achieve precise joint positioning of multiple robots; In the step D1, it includes: Step D1.1: Based on the first posture association and the second posture association, define an optimization problem of a posture graph; Step D1.2: construct intermediate variables with current pose values ​​and decompose pose errors; Step D1.3: In the EM framework, alternately perform E-step updates and M-step updates until the pose converges to obtain a rough optimization result; the convergence condition is that the difference in translation between the output results of two iterations is less than 0.001m, the difference in yaw angle is less than 0.1 degree, or the number of iterations reaches 100.

8. The centralized multi-robot visual-inertial combined robust positioning method according to claim 7, characterized in that: In step D1.1, the optimization problem is mathematically expressed as: in, is the set of all poses to be optimized, is the data association set, ||·||2 represents the two-norm; Represents the relative posture error of four degrees of freedom; The four-degree-of-freedom relative posture error, referred to as posture error, is mathematically expressed as: Among them, T i With T j They are one key frame pose to be optimized and another key frame pose to be optimized. is the relative pose constraint matrix between two frames, is the yaw angle attitude error, is the translation pose error; Yaw angle attitude error The mathematical expression is: in, and T i With T j The corresponding yaw angle, for The corresponding yaw angle; Translational pose error The mathematical expression is: Among them, t i ,t j and T i , T j and The corresponding translation vector, R i T i The corresponding rotation matrix; In the step D1.2, an intermediate variable is constructed with the current value of the posture, and the posture error is disassembled to obtain a disassembly result; the disassembly result is used to define the intermediate variable; the intermediate variable includes: and The mathematical expression of the disassembly result is: in, and are an arbitrary constant and another arbitrary constant respectively, and the superscript yaw represents the yaw angle; In step D1.3, the intermediate variables are updated in step E. The updated mathematical expression is: The updated mathematical expression is: The M steps update the pose variables, and The updated mathematical expression is: t i The updated mathematical expression is: The updated mathematical expression is: t j The updated mathematical expression is: Wherein, Avg(·) represents the average value of all internal summation items, and the symbol := represents an assignment operation.

9. A multi-robot dense posture association subsystem, characterized in that: include: Module M1: receiving frame data including robot posture, and forming a first posture association between postures of multiple frame data; Module M2: loop closure detection of the frame data, obtaining 3D-2D point pairs by feature matching of map features and 2D features of the frame data; solving the relative poses between the 3D-2D point pairs, and then constructing a second pose association; Module M3: Based on the first posture association and the second posture association, align the reference system of the posture-associated robot, and calculate the long-term posture-associated posture of the robot, that is, 10. A centralized multi-robot visual-inertial joint robust positioning system, capable of triggering the multi-robot dense posture association subsystem of claim 9, characterized in that: include: Module A: Multiple robots run local visual-inertial odometers locally to complete pose tracking in the local reference frame, and package key frame poses and feature information into frame data; Module B: Trigger the multi-robot dense posture association subsystem to obtain the long-term posture association posture of the robot, that is, Module C: Based on the long-term posture association posture and the four-degree-of-freedom pairwise consistency measurement, the abnormal posture association is screened and eliminated to obtain the screened posture association group; Module D: Based on the screened pose association groups, multi-robot joint positioning is completed through cascade pose graph optimization.

Images