A multi-machine real-time collaborative positioning method and device for indoor scenes

Abstract

The present invention discloses a method and device for real-time multi-machine collaborative localization in indoor scenes. The method comprises: performing a keyframe selection operation on consecutive frames to generate a keyframe sequence based on IMU data and lidar data of the indoor scene; constructing a local semantic map corresponding to the indoor scene based on the lidar data and the keyframe sequence; for any current keyframe in the keyframe sequence: extracting a lidar-iris descriptor from the current keyframe; performing loop closure detection on the current keyframe in the keyframe sequence based on the local semantic map and the lidar-iris descriptor; and performing local-global optimization on the current keyframe based on the loop closure detection results to generate the global pose corresponding to the first target robot at the current moment. This method solves the technical problems of perceptual confusion caused by repeated indoor scene structure and the sensitivity of distributed optimization to initial values ​​in the prior art, thereby improving the accuracy of multi-machine collaborative localization and mapping.

Description

Technical Field

[0001] The present invention belongs to the field of robot perception and navigation technology, and in particular relates to a multi-machine real-time collaborative positioning method and device for indoor scenes. Background Art

[0002] Collaborative perception is an important issue in future robotics research. The shared understanding of the environment it provides is a prerequisite for many applications, from autonomous warehouse management to underground exploration. Simultaneous Localization and Mapping (SLAM) is one of the most powerful tools for robotic perception, which closely combines geometric perception of the environment with state estimation. In addition to generating high-quality environmental maps, it also provides positioning estimates necessary for planning and control. Collaborative SLAM (CSLAM) shares a global understanding of the environment through relative measurements or common environmental features. Although centralized CSLAM solutions are effective in some cases, they have computational and communication bottlenecks, which limit their scalability. In addition, due to network coverage challenges in large indoor or underground environments, robots are actually unable to maintain a stable connection with the central server. Therefore, distributed solutions that rely only on sparse communication between robots are more suitable for large-scale deployment.

[0003] In robot swarm applications, stand-alone SLAM technology has the problem that the trajectories between clusters are not globally consistent. In order to perform collaborative tasks in unknown environments, the robot swarm must establish a global reference frame and position itself in a shared understanding of the environment. Whether SLAM is used to provide state estimation to support higher-level applications (for example, estimating the position of each robot to plan actions) or whether it is at the core of the task (for example, environment mapping), it is beneficial and sometimes necessary to extend the SLAM solution to a collaborative SLAM algorithm rather than performing stand-alone SLAM on each robot. Based on the above task background requirements, applying the concept of cluster collaboration to robot localization and mapping tasks is a current research hotspot.

[0004] For multi-robot systems, accurate positioning is not only a crucial foundation for swarm collaboration but also provides critical support for high-level tasks such as path planning and mapping. Currently, robots rely on external devices such as motion capture systems, anchor systems, GPS systems, and RTK systems to obtain accurate state estimates. However, in unknown and complex environments, such as underground mines, battlefields, and mountain valleys, the limited GPS signal and the need to deploy anchor points in advance limit the application of these positioning systems. In large indoor scenarios, where the large mission scope, limited GPS signal, and the need to deploy anchor points lead to timeliness issues, multi-robot collaborative localization and mapping systems can autonomously achieve navigation and real-time mapping for robot swarms. Lidar-inertial odometry (LIO) relies on high-precision laser ranging and information-rich point clouds for reliable localization. However, indoor scenarios are characterized by high structural repetitiveness, and existing multi-robot collaborative localization and mapping systems inevitably suffer from low efficiency and accuracy in loop detection and relocalization. Summary of the Invention

[0005] The present invention provides a multi-machine real-time collaborative positioning method and device for indoor scenes; the method can enhance the robustness and accuracy of loop detection, thereby improving the precision of multi-machine collaborative positioning in indoor scenes.

[0006] According to a first aspect of an embodiment of the present invention, a multi-machine real-time collaborative positioning method for indoor scenes is provided, comprising: performing a key frame selection operation on continuous frames of the indoor scene based on IMU data and lidar data of the indoor scene collected by a first target robot to generate a key frame sequence; constructing a local semantic map corresponding to the indoor scene based on the lidar data and the key frame sequence; for any current key frame in the key frame sequence: extracting a lidar-iris descriptor from the current key frame; performing loop detection on the current key frame in the key frame sequence based on the local semantic map and the lidar-iris descriptor to generate a loop detection result; and performing local-global optimization on the current key frame based on the loop detection result to generate a global pose corresponding to the first target robot at the current moment.

[0007] Optionally, based on the loop detection result, the current key frame is locally-globally optimized to generate the global pose corresponding to the first target robot at the current moment; including: if the loop detection result indicates the existence of a first quasi-key frame having an intra-machine loop relationship with the current key frame; the first quasi-key frame and the current key frame are ICP-aligned to generate a local optimized pose corresponding to the current key frame; if the loop detection result indicates the existence of a second quasi-key frame having an inter-machine loop relationship with the current key frame; the second quasi-key frame and the current key frame are ICP-aligned to generate a relative pose transformation; based on the local optimized pose and relative pose transformation corresponding to the current key frame, and the local optimized pose corresponding to the second quasi-key frame, the first target robot is globally optimized to determine the global pose corresponding to the first target robot at the current moment.

[0008] Optionally, based on the IMU data and lidar data of the indoor scene collected by the first target robot, a key frame selection operation is performed on the continuous frames of the indoor scene to generate a key frame sequence; including: for any current frame in the continuous frames of the indoor scene: based on the IMU data and lidar data corresponding to the current frame, determining the current estimated pose of the target robot; obtaining a key frame located before the current moment and adjacent to the current moment, and using the key frame as a reference frame; based on the IMU data and lidar data corresponding to the reference frame, generating a reference pose of the target robot corresponding to the reference frame; if the change value between the reference pose and the current estimated pose is greater than a first preset threshold, and / or if the position of the current frame is within the edge threshold of the adjacent room layer in the local semantic map and the distance between the current frame descriptor and the reference frame descriptor is greater than a second preset threshold, then determining that the current frame is a key frame; selecting several key frames from the continuous frames of the indoor scene to obtain a key frame sequence.

[0009] Optionally, the local semantic map includes: a room layer, a wall layer, and a key frame layer; based on the local semantic map and the Lidar-Iris descriptor, loop detection is performed on the current key frame in the key frame sequence to generate a loop detection result; including: for any current key frame in the key frame sequence: determining the wall layer corresponding to the current key frame based on the local semantic map; establishing a semantic relationship between the Lidar-Iris descriptor corresponding to the current key frame and the wall layer to generate a hybrid descriptor; based on the center descriptor corresponding to the local semantic map and the hybrid descriptor, querying the key frame sequence for a candidate key frame whose similarity with the current key frame meets a preset condition; if the Hamming distance between the candidate key frame and the current key frame is less than a fourth preset threshold, it is determined that the candidate key frame and the current key frame have a loop relationship, and the candidate key frame is determined as a quasi-key frame having a loop relationship with the current key frame.

[0010] Optionally, based on the central descriptor corresponding to the local semantic map and the hybrid descriptor, a candidate key frame whose similarity with the current key frame meets a preset condition is searched from the key frame sequence; including: based on the central descriptor corresponding to the local semantic map, determining the room layer corresponding to the hybrid descriptor, and generating a Lidar-Iris descriptor; for any to-be-matched row key value k in the KD tree: performing a corotation operation on the to-be-matched row key value k and the row key value k corresponding to the Lidar-Iris descriptor to generate a corotation distance; if the corotation distance is greater than a third preset threshold, determining the key frame corresponding to the to-be-matched row key value k in the key frame sequence as a candidate key frame.

[0011] Optionally, the method further includes: constructing a center descriptor corresponding to a local semantic map; the constructing of the center descriptor corresponding to the local semantic map includes: obtaining the center point and boundary corresponding to each room layer from the local semantic map; for any room layer: taking the center point of the room layer as the center, downsampling all key frames in the room layer to generate a center point cloud; obtaining the Lidar-Iris descriptor corresponding to the center point cloud, and generating a center descriptor corresponding to the local semantic map.

[0012] Optionally, based on the local optimized pose and relative pose transformation corresponding to the current key frame, and the local optimized pose corresponding to the second quasi-key frame, the first target robot is globally optimized to determine the global pose corresponding to the first target robot at the current moment; including: obtaining the local optimized pose of the second target robot corresponding to the second quasi-key frame; determining the relative pose of the local coordinate system between the first target robot and the second target robot based on the local optimized pose and relative pose transformation corresponding to the current key frame, and the local optimized pose corresponding to the second quasi-key frame; based on the relative pose, globally optimizing the local optimized pose of the first target robot, and outputting the global pose corresponding to the first target robot at the current moment.

[0013] Optionally, the method further includes: constructing a room-room factor based on the inter-machine loop relationship corresponding to the current key frame to obtain the room layer corresponding to the current key frame; based on the Lidar-Iris descriptor corresponding to the current key frame, obtaining a wall layer matching the Lidar-Iris descriptor from the room layer; combining the wall layer with the global pose of the current key frame to generate a semantic pose corresponding to the current key frame; and generating a positioning map corresponding to the first target robot in the indoor scene based on the semantic pose corresponding to each current key frame in the key frame sequence.

[0014] According to the second aspect of an embodiment of the present invention, a multi-machine real-time collaborative positioning device for indoor scenes is also provided, which includes: a generation module, used to perform a key frame selection operation on continuous frames of the indoor scene according to IMU data and lidar data of the indoor scene collected by a first target robot, and generate a key frame sequence; a local semantic map construction module, used to construct a local semantic map corresponding to the indoor scene based on the lidar data and the key frame sequence; a posture optimization module, used to, for any current key frame in the key frame sequence: extract a Lidar-Iris descriptor from the current key frame; based on the local semantic map and the Lidar-Iris descriptor, perform loop detection on the current key frame in the key frame sequence to generate a loop detection result; based on the loop detection result, perform local-global optimization on the current key frame to generate the global posture corresponding to the first target robot at the current moment.

[0015] According to a third aspect of an embodiment of the present invention, an electronic device is also provided, comprising: a processor; a memory for storing instructions executable by the processor; the processor is configured to read the executable instructions from the memory and execute the instructions to implement the method described in the first aspect.

[0016] According to a fourth aspect of an embodiment of the present invention, a computer-readable medium is further provided, on which a computer program is stored. When the program is executed by a processor, the method described in the first aspect is implemented.

[0017] An embodiment of the present invention provides a multi-machine real-time collaborative positioning method and device for indoor scenes. The method includes: first, based on IMU data and lidar data of the indoor scene collected by a first target robot, performing a key frame selection operation on continuous frames of the indoor scene to generate a key frame sequence; second, based on the lidar data and the key frame sequence, constructing a local semantic map corresponding to the indoor scene; finally, for any current key frame in the key frame sequence: extracting a lidar-iris descriptor from the current key frame; based on the local semantic map and the lidar-iris descriptor, performing loop detection on the current key frame in the key frame sequence to generate a loop detection result; based on the loop detection result, performing local-global optimization on the current key frame to generate the global pose corresponding to the first target robot at the current moment. The method of this embodiment first performs loop detection based on the matching of center descriptors, thereby improving the robustness and accuracy of loop matching. Secondly, a two-stage distributed local-global optimization is performed based on the loop closure relationship, further improving the perception accuracy of the robot's posture. This solves the technical problems of perceptual confusion caused by repeated indoor scene structures and the sensitivity of distributed optimization to initial values ​​in the existing technology, thereby improving the accuracy of multi-machine collaborative localization and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Hereinafter, some specific embodiments of the present invention will be described in detail in an exemplary and non-limiting manner with reference to the accompanying drawings. The same reference numerals in the accompanying drawings indicate the same or similar components or parts. It should be understood by those skilled in the art that these drawings are not necessarily drawn to scale. In the accompanying drawings:

[0019] Figure 1 A flowchart of a multi-machine real-time collaborative positioning method for indoor scenes provided by one embodiment of the present invention;

[0020] Figure 2 1 is a flow chart of loop detection for a current key frame according to an embodiment of the present invention;

[0021] Figure 3 Schematic diagram of a process for determining the global position of the first target robot at the current moment in one embodiment of the present invention;

[0022] Figure 4 A schematic structural diagram of a multi-machine real-time collaborative positioning device for indoor scenarios provided by one embodiment of the present invention. DETAILED DESCRIPTION

[0023] In order to make the purposes, features, and advantages of the present invention more obvious and easy to understand, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative efforts shall fall within the scope of protection of the present invention.

[0024] like Figure 1 FIG2 is a flow chart of a multi-machine real-time collaborative positioning method for indoor scenes provided by an embodiment of the present invention.

[0025] A multi-machine real-time collaborative positioning method for indoor scenes includes at least the following steps:

[0026] S101, performing a key frame selection operation on consecutive frames of the indoor scene based on the IMU data and lidar data of the indoor scene collected by the first target robot to generate a key frame sequence;

[0027] S102, constructing a local semantic map corresponding to the indoor scene based on the lidar data and the key frame sequence;

[0028] S103, for any current key frame in the key frame sequence: extract a Lidar-Iris descriptor from the current key frame; based on the local semantic map and the Lidar-Iris descriptor, perform loop detection on the current key frame in the key frame sequence to generate a loop detection result; based on the loop detection result, perform local-global optimization on the current key frame to generate the global pose corresponding to the first target robot at the current moment.

[0029] In S101, keyframes not only achieve a balance between map density and memory consumption, but also help maintain a relatively sparse factor graph, which is suitable for real-time nonlinear optimization. The choice of keyframes also determines the success rate of loop closure detection, so the keyframes should cover as much of the scanned map as possible.

[0030] The effective fusion and processing of IMU data and lidar data based on preset rules or algorithm models can achieve efficient key frame selection and generate accurate key frame sequences.

[0031] Exemplarily, for any current frame in the continuous frames of the indoor scene: based on the IMU data and lidar data corresponding to the current frame, determine the current estimated pose of the target robot; obtain a key frame located before the current moment and adjacent to the current moment, and use the key frame as a reference frame; based on the IMU data and lidar data corresponding to the reference frame, generate a reference pose of the target robot corresponding to the reference frame; if the change value between the reference pose and the current estimated pose is greater than a first preset threshold, and / or if the position of the current frame is within the edge threshold of the adjacent room layer in the local semantic map and the distance between the current frame descriptor and the reference frame descriptor is greater than a second preset threshold, determine that the current frame is a key frame; select several key frames from the continuous frames of the indoor scene to obtain a key frame sequence.

[0032] Specifically, the odometer front end receives IMU data from the IMU and LiDAR data from the LiDAR. If the IMU sampling frequency is too high, the acceleration and angular velocity data collected by the IMU can be used intermittently in engineering. Considering the kinematic characteristics of the machine platform, if an IMU sensor is installed, IMU data can be used to correct point cloud distortion.

[0033] Considering the actual motion characteristics of the machine platform, the odometry front-end should be an appropriate lidar or inertial odometry system. For example, for unmanned vehicle platforms, the LIO-SAM odometry front-end can be used as the system's odometry front-end. For aerial robot platforms, the FAST-LIO and DLIO odometry front-ends can be used as the system's odometry front-end. If the machine platform does not have an IMU, the LeGO-LOAM odometry front-end can be used as the system's odometry front-end.

[0034] If the current frame and the reference frame are located in the same room layer, then if the change value between the reference pose corresponding to the reference frame and the current estimated pose corresponding to the current frame is greater than a first preset threshold, the current frame is determined to be a key frame; if the current frame enters the next room layer, then if the position of the current frame is within the edge threshold of the adjacent room layer in the local semantic map and the distance between the current frame descriptor and the reference frame descriptor is greater than a second preset threshold, then the current frame is determined to be a key frame. Here, the setting of the first preset threshold can refer to the parameters of LIO-SAM or be preset according to the complexity of the actual scene structure. In order to improve computing and communication efficiency, when selecting a key frame, the first preset threshold includes a position change threshold and a rotation change threshold.

[0035] Keyframe selection is a process that balances computational efficiency and map integrity. By setting appropriate thresholds and controlling conditions, we can effectively support real-time localization and map building tasks. These two constraints ensure that keyframes are not only spatially representative but also sufficiently distinctive in their feature descriptions, thereby optimizing overall system performance.

[0036] In S102 , there is no limitation on the method of constructing the local semantic map.

[0037] For example, a local semantic map is called a scene graph, which is mainly used to encapsulate the scene perception information of the target robot. The entities in this scene graph represent the different components of the building layout.

[0038] Using the S-Graphs+ algorithm, the first target robot's LiDAR data and keyframe sequence are combined to generate a local semantic map corresponding to the indoor scene. This local semantic map not only includes the environment's geometric information but also estimates the first target robot's position information.

[0039] The local semantic map consists of the room layer, wall layer, floor layer, and keyframe layer; these layers contain different types of building information:

[0040] Keyframe layer: This layer consists of the robot's poses (i.e., position and orientation). Each pose is considered a node in the agent's semantic map framework. Constraints are established for these keyframes based on paired odometry.

[0041] Wall layer: Planar wall surfaces are obtained by extracting planar features from the point cloud data acquired by LiDAR and using minimum plane parameterization. Each planar wall surface is constructed based on the corresponding keyframe and modeled as a pose-plane constraint factor.

[0042] Room layer: This layer represents rooms consisting of corridors or four walls. Each room is constrained by the observed wall layer. Each room can be represented as an area with two or four walls.

[0043] Floor layer: This layer consists of the floor node located at the center of the current floor and is used to determine the relative position of each room and wall in the entire building.

[0044] After constructing these layers, graph optimization algorithms can be used to integrate and optimize this information, further improving the robot's understanding of the environment and positioning accuracy. This design not only comprehensively captures building geometry but also effectively supports the robot's navigation and task execution in complex environments. This approach to constructing local semantic maps provides strong support for the practical application of intelligent robots.

[0045] In S103, if the loop detection result indicates the existence of a first quasi-key frame having an intra-machine loop relationship with the current key frame, the first quasi-key frame and the current key frame are ICP-aligned to generate a local optimized pose corresponding to the current key frame; if the loop detection result indicates the existence of a second quasi-key frame having an inter-machine loop relationship with the current key frame, the second quasi-key frame and the current key frame are ICP-aligned to generate a relative pose transformation; based on the local optimized pose and relative pose transformation corresponding to the current key frame, and the local optimized pose corresponding to the second quasi-key frame, the first target robot is globally optimized to determine the global pose corresponding to the first target robot at the current moment.

[0046] This embodiment performs loop closure detection on the current keyframe based on the local semantic map and Lidar-Iris descriptor, thereby improving the robustness and accuracy of loop matching; then, the local pose of the current keyframe is optimized based on the intra-machine loop closure relationship, and the global pose of the current keyframe is optimized based on the inter-machine loop closure relationship, thereby improving the accuracy of multi-robot collaborative positioning in indoor scenes.

[0047] like Figure 2 FIG. 1 is a flow chart of loop detection for a current key frame according to an embodiment of the present invention.

[0048] During the global optimization phase, only the Lidar-Iris descriptors and keyframe point clouds need to be exchanged to verify the relative pose transformation between the two frames between the robots. During the inter-robot and intra-robot loop closure phases, the center descriptors need to be transferred and matched first, reducing the keyframe search space and improving the matching accuracy and robustness.

[0049] Performing loop detection on the current key frame in the key frame sequence includes at least the following steps:

[0050] S201, for any current key frame in the key frame sequence: determining the wall layer corresponding to the current key frame based on the local semantic map; establishing a semantic relationship between the Lidar-Iris descriptor corresponding to the current key frame and the wall layer to generate a hybrid descriptor; and searching the key frame sequence for candidate key frames whose similarity to the current key frame meets preset conditions based on the center descriptor and the hybrid descriptor corresponding to the local semantic map;

[0051] S202: If the Hamming distance between the candidate key frame and the current key frame is less than a fourth preset threshold, determine that the candidate key frame and the current key frame have a loop relationship, and determine the candidate key frame as a quasi-key frame having a loop relationship with the current key frame.

[0052] Specifically, first, a central descriptor corresponding to the local semantic map is constructed.

[0053] Constructing the center descriptor corresponding to the local semantic map includes: obtaining the center point and boundary corresponding to each room layer from the local semantic map; for any room layer: taking the center point of the room layer as the center, downsampling all key frames in the room layer to generate a center point cloud; obtaining the Lidar-Iris descriptor corresponding to the center point cloud, and generating the center descriptor corresponding to the local semantic map. The center descriptor is used to indicate the Lidar-Iris descriptor with the room as the center point cloud. For example: first, the center point and boundary of the room are obtained based on the semantic and hierarchical information of the local semantic map to extract all key frames obtained from the room layer and generate the corresponding center point cloud. The center point cloud centered on the center point of the room is downsampled to uniformize the number of point clouds in each room, and finally the corresponding Lidar-Iris descriptor is generated for the downsampled center point cloud. This strategy can effectively reduce the sensitivity to linear motion and enhance the descriptor matching effect.

[0054] Second, query similar candidate keyframes;

[0055] Based on the central descriptor and the hybrid descriptor corresponding to the local semantic map, a candidate key frame whose similarity with the current key frame meets a preset condition is searched from the key frame sequence; including: based on the central descriptor corresponding to the local semantic map, determining the room layer corresponding to the hybrid descriptor, and generating a Lidar-Iris descriptor; for any to-be-matched row key value k in the KD tree: performing a corotation operation on the to-be-matched row key value k and the row key value k corresponding to the Lidar-Iris descriptor to generate a corotation distance; if the corotation distance is greater than a third preset threshold, determining the key frame corresponding to the to-be-matched row key value k in the key frame sequence as a candidate key frame.

[0056] Specifically, determine the row key value k corresponding to the Lidar-Iris descriptor and the KD tree of the center descriptor corresponding to the local semantic map; query the key frame close to the row key value k from the KD tree, and use the key frame as a candidate key frame. For example, select a lightweight Lidar-Iris descriptor with rotation invariance, similar to the Scan Context descriptor, and divide the point cloud data into N points according to the rotation direction and radial direction. r ×N s Point cloud subsets are projected onto a 2D plane. Each element in the descriptor matrix has an 8-bit binary number a. ij Composition, i=[1,2,…,N r ],j=[1,2,…,Ns ], so as to divide the point cloud into 8 consecutive heights. 1 means that there is a point cloud in the current height area, otherwise there is no point cloud in the height area.

[0057] Extract the Lidar-Iris descriptor for each current key frame and calculate the row key value K of the Lidar-Iris descriptor corresponding to the Lidar-Iris descriptor, as shown in formula (1):

[0058]

[0059] Among them, k i Represents the eigenvalue of each ring, satisfying the rotation invariance property.

[0060] Descriptor matching: perform similarity measurement on the row key value K corresponding to the current key frame, search for similar key frames in the KD tree, and use the key frames found as candidate key frames. The logic of selecting candidate key frames based on cosine distance is shown in the following formula (2).

[0061]

[0062] in, and are the row keys of the mth and nth keyframes in robots α and β, respectively, η is the cosine distance threshold, is the cosine operation.

[0063] Finally, outlier filtering is performed on the candidate keyframes.

[0064] Compare the Lidar-Iris descriptor corresponding to the current keyframe with the Lidar-Iris descriptor corresponding to the candidate keyframe. If the Hamming distance between the candidate keyframe and the current keyframe is less than the threshold, a loop is considered to have been found. The judgment logic based on the Hamming distance is shown in the following formula (3).

[0065]

[0066] in, and The binary descriptors of the i-th row and j-th column elements of the descriptors of robots α and β are represented respectively, μ is the similarity distance threshold, It is an exclusive OR operation.

[0067] Therefore, this embodiment enhances the robustness and accuracy of loop detection based on local semantic maps and hybrid descriptors, thereby improving the foundation for achieving accurate multi-robot collaborative positioning.

[0068] like Figure 3, which is a schematic diagram of a process for determining the global posture corresponding to the first target robot at the current moment in one embodiment of the present invention.

[0069] Determining the global position of the first target robot at the current moment includes at least the following steps:

[0070] S301, obtaining a local optimized pose of a second target robot corresponding to a second quasi-key frame;

[0071] S302, determining the relative pose of the local coordinate systems of the first target robot and the second target robot based on the local optimized pose and relative pose transformation corresponding to the current key frame, and the local optimized pose corresponding to the second quasi-key frame;

[0072] S303: Based on the relative posture, globally optimize the locally optimized posture of the first target robot, and output the global posture corresponding to the first target robot at the current moment.

[0073] Exemplarily, based on the relative pose of the local coordinate systems between the first target robot and the second target robot, the global coordinate system transformation matrix of the first target robot is calculated; based on the local optimized pose of the first target robot at the current moment and the global coordinate system transformation matrix, the global pose corresponding to the first target robot at the current moment is output.

[0074] The pose graph optimization receives the relative poses between frames from the inter-machine loop and the intra-machine loop, and calibrates the trajectory drift of each robot through joint optimization. This system uses a two-stage distributed Gauss-Seidel method as the back-end optimization method. First, the robot pose is optimized. Based on the optimized robot pose, a full state variable perturbation objective function is constructed to further optimize the robot pose.

[0075] For example, back-end pose graph optimization requires good initial pose values ​​to ensure convergence speed. The global optimization phase aims to provide a relatively accurate initial pose transformation for pose graph optimization.

[0076] In the global optimization stage, common features are detected between robots (i.e., they exist in the same common area). Based on the loop detection process, two frames with common features between robots α and β (α n ,β m )'s relative pose transformation In the local coordinate system of robots α and β, α n and β m The local optimized poses are and They satisfy the following relationship:

[0077]

[0078] Among them, T βα is the relative pose of the local coordinate systems of robots α and β, T represents the position of robot α in the local coordinate system of robot β in the nth frame. βα It can be obtained based on the relative pose transformation of the loop frame between robots and their pose transformation in the local coordinate system.

[0079]

[0080] Based on the continuous loop detection data, the relative pose measurement set of robots α and β in the local coordinate system can be obtained l is the size of the relative pose measurement set. Set the coordinate system of robot 1 to the unified global coordinate system, then T g1 =I 4×4 , the parameter matrix T of the rest of the robot's local coordinate system to the global coordinate system gα =T 1α Set the set T to be the set of transformation matrices from the local coordinate system to the global coordinate system of all robots except robot No. 1.

[0081] T={T gγ ,γ=2,…} Formula (6);

[0082] In order to solve the accurate initial transformation matrix from the local coordinate system of each robot to the global coordinate system, the error function of the relative posture between robots is constructed, such as

[0083]

[0084] in, is the estimated relative pose of robots α and β,

[0085]

[0086] The residual sum of the relative postures of the robots is minimized by the LM method to iteratively optimize the robot's global coordinate system transformation matrix;

[0087]

[0088] Among them, R βα is the covariance matrix.

[0089] This embodiment is based on a two-stage distributed local-global optimization method, which not only solves the pose graph optimization problem and achieves global consistency of pose, but also improves the perception accuracy of the robot's pose.

[0090] The multi-machine real-time collaborative positioning method for indoor scenes proposed in this embodiment is applicable to the multi-machine real-time collaborative positioning and mapping framework for large-scale indoor scenes using lidar.

[0091] The following describes in detail a multi-machine real-time collaborative positioning method for indoor scenarios provided by this embodiment in conjunction with a specific application scenario.

[0092] S1, for any current frame in the continuous frames of the indoor scene: based on the IMU data and lidar data corresponding to the current frame, determine the current estimated pose of the target robot; obtain a key frame located before the current moment and adjacent to the current moment, and use the key frame as a reference frame; based on the IMU data and lidar data corresponding to the reference frame, generate a reference pose of the target robot corresponding to the reference frame; if the change value between the reference pose and the current estimated pose is greater than a first preset threshold, and / or if the position of the current frame is within the edge threshold of the adjacent room layer in the local semantic map and the distance between the current frame descriptor and the reference frame descriptor is greater than a second preset threshold, then determine that the current frame is a key frame; select several key frames from the continuous frames of the indoor scene to obtain a key frame sequence.

[0093] S2: constructing a local semantic map corresponding to the indoor scene based on the lidar data and the key frame sequence.

[0094] S3, for any current key frame in the key frame sequence: extracting a Lidar-Iris descriptor from the current key frame; for any current key frame in the key frame sequence: determining the wall layer corresponding to the current key frame based on the local semantic map; establishing a semantic relationship between the Lidar-Iris descriptor corresponding to the current key frame and the wall layer to generate a hybrid descriptor; determining the room layer corresponding to the hybrid descriptor based on the center descriptor corresponding to the local semantic map to generate a Lidar-Iris descriptor; for any to-be-matched row key value k in the KD tree: performing a corotation operation on the to-be-matched row key value k and the row key value k corresponding to the Lidar-Iris descriptor to generate a corotation distance; if the corotation distance is greater than a third preset threshold, determining the key frame corresponding to the to-be-matched row key value k in the key frame sequence as a candidate key frame. If the Hamming distance between the candidate key frame and the current key frame is less than a fourth preset threshold, determining that the candidate key frame and the current key frame have a loop closure relationship, and determining the candidate key frame as a quasi-key frame having a loop closure relationship with the current key frame.

[0095] S4. If the loop detection result indicates that there is a first quasi-key frame having an intra-machine loop relationship with the current key frame, the first quasi-key frame and the current key frame are ICP-aligned to generate a local optimized pose corresponding to the current key frame.

[0096] S5. If the loop detection result indicates that there is a second quasi-key frame having an inter-machine loop relationship with the current key frame, the second quasi-key frame and the current key frame are subjected to ICP registration to generate a relative pose transformation.

[0097] S6, obtain the local optimized posture of the second target robot corresponding to the second quasi-key frame; determine the relative posture of the local coordinate system between the first target robot and the second target robot based on the local optimized posture and relative posture transformation corresponding to the current key frame, and the local optimized posture corresponding to the second quasi-key frame; based on the relative posture, globally optimize the local optimized posture of the first target robot, and output the global posture corresponding to the first target robot at the current moment.

[0098] S7: Construct a room-room factor based on the inter-machine loop relationship corresponding to the current key frame to obtain the room layer corresponding to the current key frame; based on the Lidar-Iris descriptor corresponding to the current key frame, obtain a wall layer that matches the Lidar-Iris descriptor from the room layer; combine the wall layer with the global pose of the current key frame to generate a semantic pose corresponding to the current key frame; and based on the semantic pose corresponding to each current key frame in the key frame sequence, generate a positioning map corresponding to the first target robot in the indoor scene.

[0099] This embodiment provides a method for real-time collaborative localization and mapping of multiple robots in large indoor scenes. First, loop detection is performed based on the matching of center descriptors, which improves the robustness and accuracy of loop matching. Second, a two-stage distributed local-global optimization is performed based on the loop relationship, further improving the perception accuracy of the robot's position. This solves the technical problems of perceptual confusion caused by repeated indoor scene structures and the sensitivity of distributed optimization to initial values ​​in the existing technology, thereby improving the accuracy of multi-machine collaborative localization and mapping.

[0100] like Figure 4 FIG. 1 is a schematic diagram of the structure of a multi-machine real-time collaborative positioning device for indoor scenes provided by one embodiment of the present invention.

[0101] A multi-machine real-time collaborative positioning device for indoor scenes, the device 400 including: a generation module 401, for performing a key frame selection operation on continuous frames of the indoor scene based on IMU data and lidar data of the indoor scene collected by a first target robot, to generate a key frame sequence; a local semantic map construction module 402, for constructing a local semantic map corresponding to the indoor scene based on the lidar data and the key frame sequence; a pose optimization module 403, for extracting a lidar-iris descriptor from any current key frame in the key frame sequence; performing loop detection on the current key frame in the key frame sequence based on the local semantic map and the lidar-iris descriptor to generate a loop detection result; and performing local-global optimization on the current key frame based on the loop detection result to generate a global pose corresponding to the first target robot at the current moment.

[0102] In a preferred implementation of this embodiment, the posture optimization module includes: a first alignment unit, which is used to, if the loop detection result indicates that there is a first quasi-key frame having an intra-machine loop relationship with the current key frame; then perform ICP alignment on the first quasi-key frame and the current key frame to generate a local optimized posture corresponding to the current key frame; a second alignment unit, which is used to, if the loop detection result indicates that there is a second quasi-key frame having an inter-machine loop relationship with the current key frame; then perform ICP alignment on the second quasi-key frame and the current key frame to generate a relative posture transformation; a global posture optimization unit, which is used to perform global posture optimization on the first target robot based on the local optimized posture and relative posture transformation corresponding to the current key frame, and the local optimized posture corresponding to the second quasi-key frame, and determine the global posture corresponding to the first target robot at the current moment.

[0103] In a preferred implementation of this embodiment, the generation module includes: a determination unit, which is used to: determine, for any current frame in the continuous frames of the indoor scene: based on the IMU data and lidar data corresponding to the current frame, the current estimated pose of the target robot; obtain a key frame located before the current moment and adjacent to the current moment, and use the key frame as a reference frame; based on the IMU data and lidar data corresponding to the reference frame, generate a reference pose of the target robot corresponding to the reference frame; if the change value between the reference pose and the current estimated pose is greater than a first preset threshold, and / or if the position of the current frame is within the edge threshold of the adjacent room layer in the local semantic map and the distance between the current frame descriptor and the reference frame descriptor is greater than a second preset threshold, then determine that the current frame is a key frame; a selection unit, which is used to select several key frames from the continuous frames of the indoor scene to obtain a key frame sequence.

[0104] In a preferred implementation of this embodiment, the posture optimization module includes: a selection unit, which is used to: for any current key frame in the key frame sequence: determine the wall layer corresponding to the current key frame based on the local semantic map; establish a semantic relationship between the Lidar-Iris descriptor corresponding to the current key frame and the wall layer to generate a hybrid descriptor; based on the center descriptor corresponding to the local semantic map and the hybrid descriptor, query the candidate key frame whose similarity with the current key frame meets the preset conditions from the key frame sequence; a determination unit, which is used to determine that the candidate key frame and the current key frame have a loop relationship if the Hamming distance between the candidate key frame and the current key frame is less than a fourth preset threshold, and determine the candidate key frame as a quasi-key frame having a loop relationship with the current key frame.

[0105] In a preferred implementation of this embodiment, the selection unit includes: a first determination subunit, used to determine the room layer corresponding to the hybrid descriptor based on the central descriptor corresponding to the local semantic map, and generate a Lidar-Iris descriptor; a second determination subunit, used to: for any to-be-matched row key value k in the KD tree: perform a corotation operation on the to-be-matched row key value k and the row key value k corresponding to the Lidar-Iris descriptor to generate a corotation distance; if the corotation distance is greater than a third preset threshold, determine the key frame corresponding to the to-be-matched row key value k in the key frame sequence as a candidate key frame.

[0106] In a preferred implementation of this embodiment, the pose optimization module further includes: a construction unit for constructing a center descriptor corresponding to a local semantic map; the construction unit includes: an acquisition subunit for obtaining the center point and boundary corresponding to each room layer from the local semantic map; a downsampling subunit for downsampling all key frames in the room layer with the center point of the room layer as the center to generate a center point cloud for any room layer; and a generation subunit for obtaining the Lidar-Iris descriptor corresponding to the center point cloud and generating a center descriptor corresponding to the local semantic map.

[0107] In a preferred implementation of this embodiment, the global posture optimization unit includes: an acquisition subunit for acquiring the local optimized posture of the second target robot corresponding to the second quasi-key frame; a determination subunit for determining the relative posture of the local coordinate system between the first target robot and the second target robot based on the local optimized posture and relative posture transformation corresponding to the current key frame, and the local optimized posture corresponding to the second quasi-key frame; an output subunit for globally optimizing the local optimized posture of the first target robot based on the relative posture, and outputting the global posture corresponding to the first target robot at the current moment.

[0108] In a preferred implementation of this embodiment, the device also includes: a semantic pose construction module, which is used to construct a room-room factor based on the inter-machine loop relationship corresponding to the current key frame to obtain the room layer corresponding to the current key frame; based on the Lidar-Iris descriptor corresponding to the current key frame, obtain a wall layer matching the Lidar-Iris descriptor from the room layer; combine the wall layer with the global pose of the current key frame to generate the semantic pose corresponding to the current key frame; and a positioning map generation module, which is used to generate a positioning map corresponding to the first target robot in the indoor scene based on the semantic pose corresponding to each current key frame in the key frame sequence.

[0109] The above-described device can implement a multi-machine real-time collaborative positioning method for indoor scenarios provided by one embodiment of the present invention, and possesses the functional modules and beneficial effects corresponding to executing a multi-machine real-time collaborative positioning method for indoor scenarios. For technical details not fully described in this embodiment, please refer to the multi-machine real-time collaborative positioning method for indoor scenarios provided by one embodiment of the present invention.

[0110] The present invention also provides an electronic device, comprising: a processor; a memory for storing executable instructions of the processor; the processor is used to read the executable instructions from the memory and execute the instructions to implement the multi-machine real-time collaborative positioning method for indoor scenes described in the present invention.

[0111] In addition to the above-mentioned methods and devices, an embodiment of the present application may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the method according to various embodiments of the present application described in the above-mentioned "Exemplary Method" section of this specification.

[0112] The computer program product may be written in any combination of one or more programming languages ​​to implement the program code for performing the operations of the embodiments of the present application, including object-oriented programming languages ​​such as Java, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0113] In addition, an embodiment of the present application may also be a computer-readable storage medium having computer program instructions stored thereon, which, when executed by a processor, enable the processor to execute the steps of the method according to the following embodiments of the present application described in the above "Exemplary Method" section of this specification.

[0114] The computer-readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can, for example, include but is not limited to a system, device or component of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination thereof. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof.

[0115] The basic principles of the present application have been described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, and effects mentioned in this application are merely illustrative and not restrictive, and it should not be assumed that these advantages, strengths, and effects are required of each embodiment of this application. In addition, the specific details disclosed above are merely illustrative and facilitating understanding, and are not restrictive. The above details do not limit this application to necessarily being implemented using the above specific details.

[0116] The block diagrams of the devices, devices, equipment, and systems involved in this application are merely illustrative examples and are not intended to require or imply that they must be connected, arranged, or configured in the manner shown in the block diagrams. As will be appreciated by those skilled in the art, these devices, devices, equipment, and systems can be connected, arranged, or configured in any manner. Words such as "include," "comprise," "have," and the like are open-ended words, meaning "including but not limited to," and can be used interchangeably therewith. The words "or" and "and" used herein refer to the words "and / or" and can be used interchangeably therewith, unless the context clearly indicates otherwise. The word "such as" used herein refers to the phrase "such as but not limited to," and can be used interchangeably therewith.

[0117] It should also be noted that in the apparatus, device, and method of the present application, each component or each step can be decomposed and / or recombined, and such decomposition and / or recombination should be regarded as equivalent solutions of the present application.

[0118] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use the present application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to other aspects without departing from the scope of the present application. Therefore, the present application is not intended to be limited to the aspects shown herein, but rather to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0119] The above description has been provided for the purpose of illustration and description. Furthermore, this description is not intended to limit the embodiments of the present application to the forms disclosed herein. Although a number of example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations thereof.

[0120] In the description of this specification, reference to the terms "one embodiment," "some embodiments," "examples," "specific examples," or "some examples" means that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. In addition, those skilled in the art may combine and integrate different embodiments or examples described in this specification, as well as features of different embodiments or examples, unless otherwise inconsistent.

[0121] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of such features. In the description of the present invention, "plurality" means two or more, unless otherwise specifically defined.

[0122] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A multi-machine real-time collaborative positioning method for indoor scenes, characterized by: include: Performing a key frame selection operation on consecutive frames of the indoor scene based on the IMU data and lidar data of the indoor scene collected by the first target robot to generate a key frame sequence; Constructing a local semantic map corresponding to the indoor scene based on the lidar data and the key frame sequence; For any current key frame in the key frame sequence: extract a Lidar-Iris descriptor from the current key frame; based on the local semantic map and the Lidar-Iris descriptor, perform loop detection on the current key frame in the key frame sequence to generate a loop detection result; perform local pose optimization on the current key frame based on the intra-machine loop relationship in the loop detection result, and perform global pose optimization on the current key frame based on the inter-machine loop relationship in the loop detection result to generate a global pose corresponding to the first target robot at the current moment.

2. The method according to claim 1, characterized in that The method of performing local-global optimization on the current key frame based on the loop detection result to generate the global pose corresponding to the first target robot at the current moment includes: If the loop detection result indicates that there is a first quasi-key frame having an intra-machine loop relationship with the current key frame, the first quasi-key frame and the current key frame are subjected to ICP registration to generate a local optimized pose corresponding to the current key frame; If the loop detection result indicates that there is a second quasi-key frame having an inter-machine loop relationship with the current key frame, the second quasi-key frame and the current key frame are subjected to ICP registration to generate a relative pose transformation; Based on the local optimized pose and relative pose transformation corresponding to the current key frame, and the local optimized pose corresponding to the second quasi-key frame, the global pose of the first target robot is optimized to determine the global pose corresponding to the first target robot at the current moment.

3. The method according to claim 1, characterized in that The method comprises: performing a key frame selection operation on continuous frames of the indoor scene based on the IMU data and the lidar data of the indoor scene collected by the first target robot to generate a key frame sequence; For any current frame in the continuous frames of the indoor scene: based on the IMU data and the lidar data corresponding to the current frame, determine the current estimated pose of the target robot; obtain a key frame located before the current moment and adjacent to the current moment, and use the key frame as a reference frame; based on the IMU data and the lidar data corresponding to the reference frame, generate a reference pose of the target robot corresponding to the reference frame; if the change value between the reference pose and the current estimated pose is greater than a first preset threshold, and / or if the position of the current frame is within the adjacent room layer edge threshold in the local semantic map and the distance between the current frame descriptor and the reference frame descriptor is greater than a second preset threshold, determine that the current frame is a key frame; A number of key frames are selected from the continuous frames of the indoor scene to obtain a key frame sequence.

4. The method according to claim 1, wherein The local semantic map includes: a room layer, a wall layer, and a keyframe layer; Based on the local semantic map and the Lidar-Iris descriptor, loop closure detection is performed on the current key frame in the key frame sequence to generate a loop closure detection result, comprising: For any current key frame in the key frame sequence: determining a wall layer corresponding to the current key frame based on the local semantic map; establishing a semantic relationship between the Lidar-Iris descriptor corresponding to the current key frame and the wall layer to generate a hybrid descriptor; and querying, from the key frame sequence, for candidate key frames whose similarity to the current key frame meets a preset condition based on the center descriptor corresponding to the local semantic map and the hybrid descriptor; If the Hamming distance between the candidate key frame and the current key frame is less than a fourth preset threshold, it is determined that the candidate key frame and the current key frame have a loop relationship, and the candidate key frame is determined as a quasi-key frame having a loop relationship with the current key frame.

5. The method according to claim 4, characterized in that The step of searching, based on the central descriptor corresponding to the local semantic map and the hybrid descriptor, for a candidate key frame whose similarity to the current key frame satisfies a preset condition from the key frame sequence comprises: Based on the central descriptor corresponding to the local semantic map, determining the room layer corresponding to the hybrid descriptor, and generating a Lidar-Iris descriptor; For any to-be-matched row key value k in the KD tree: perform a corotation operation on the to-be-matched row key value k and the row key value k corresponding to the Lidar-Iris descriptor to generate a corotation distance; if the corotation distance is greater than a third preset threshold, determine the key frame corresponding to the to-be-matched row key value k in the key frame sequence as a candidate key frame.

6. The method according to claim 4, characterized in that Also includes: Construct the central descriptor corresponding to the local semantic map; The method of constructing a central descriptor corresponding to a local semantic map includes: Obtain the center point and boundary corresponding to each room layer from the local semantic map; For any room layer: with the center point of the room layer as the center, downsample all key frames in the room layer to generate a center point cloud; Obtain the Lidar-Iris descriptor corresponding to the central point cloud and generate a central descriptor corresponding to the local semantic map.

7. The method according to claim 2, characterized in that The method of performing global pose optimization on the first target robot based on the local optimized pose and relative pose transformation corresponding to the current key frame and the local optimized pose corresponding to the second quasi-key frame to determine the global pose corresponding to the first target robot at the current moment comprises: Obtaining a local optimized pose of the second target robot corresponding to the second quasi-key frame; Determining the relative pose of the local coordinate systems of the first target robot and the second target robot based on the local optimized pose and relative pose transformation corresponding to the current key frame and the local optimized pose corresponding to the second quasi-key frame; Based on the relative posture, the locally optimized posture of the first target robot is globally optimized, and the global posture corresponding to the first target robot at the current moment is output.

8. The method according to claim 1, characterized in that Also includes: Constructing a room-room factor based on the inter-machine loop relationship corresponding to the current key frame, and obtaining the room layer corresponding to the current key frame; Based on the Lidar-Iris descriptor corresponding to the current key frame, obtaining a wall layer that matches the Lidar-Iris descriptor from the room layer; combining the wall layer with the global pose of the current key frame to generate a semantic pose corresponding to the current key frame; Based on the semantic pose corresponding to each current key frame in the key frame sequence, a positioning map corresponding to the first target robot in the indoor scene is generated.

9. A multi-machine real-time collaborative positioning device for indoor scenes, characterized by: include: A generation module is used to perform a key frame selection operation on continuous frames of the indoor scene based on the IMU data and lidar data of the indoor scene collected by the first target robot to generate a key frame sequence; A local semantic map construction module, configured to construct a local semantic map corresponding to the indoor scene based on the lidar data and the key frame sequence; The posture optimization module is used to: for any current key frame in the key frame sequence: extract a Lidar-Iris descriptor from the current key frame; based on the local semantic map and the Lidar-Iris descriptor, perform loop detection on the current key frame in the key frame sequence to generate a loop detection result; perform local posture optimization on the current key frame based on the intra-machine loop relationship in the loop detection result, and perform global posture optimization on the current key frame based on the inter-machine loop relationship in the loop detection result to generate a global posture corresponding to the first target robot at the current moment.

10. A computer-readable medium having a computer program stored thereon, wherein when the program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

Images