Multi-machine real-time cooperative positioning method and device for indoor scene

Abstract

The invention discloses a multi-machine real-time cooperative positioning method and device for an indoor scene, and the method comprises the steps: carrying out the key frame selection operation of continuous frames according to the IMU data and laser radar data of the indoor scene, and generating a key frame sequence; constructing a local semantic map corresponding to the indoor scene based on laser radar 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; carrying out loopback detection on the current key frame in the key frame sequence based on the local semantic map and the Lidar-Iris descriptor; and local-global optimization is carried out on the current key frame based on the loopback detection result, and a global pose corresponding to the current moment of the first target robot is generated. Therefore, the technical problems that in the prior art, perception confusion is caused by indoor scene structure repetition, and distributed optimization is sensitive to an initial value are solved, and therefore the precision of multi-machine cooperative positioning and mapping is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot perception and navigation, and particularly relates to a multi-robot real-time collaborative localization method and device for indoor scenarios. Background Art

[0002] Collaborative perception is an important issue in future robot research, and the common 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 robot perception, which tightly combines the geometric perception of the environment with state estimation. In addition to generating high-quality environmental maps, it also provides the localization 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 suffer from computational and communication bottlenecks, which limit their scalability. Moreover, due to the challenges of network coverage 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 the application of robot clusters, the single-robot SLAM technology has the problem that the trajectories between clusters do not have global consistency. In order to perform collaborative tasks in an unknown environment, the robot swarm must establish a global reference framework and position itself in a shared understanding of the environment. Whether SLAM is used to provide state estimation to support higher-level applications (e.g., estimating the position of each robot for motion planning), or it is at the core of the task (e.g., environmental mapping), it is beneficial and sometimes necessary to extend the SLAM solution to a collaborative SLAM algorithm rather than performing single-robot 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 a multi-robot system, accurate positioning results are not only an important basis for the collaboration of a robot cluster, but also provide key support for high-level tasks such as path planning and mapping. Currently, robots can rely on external devices such as motion capture systems, anchor point systems, GPS systems, and RTK systems to obtain accurate state estimates. However, in unknown and complex environments, such as underground mine shafts, battlefield environments, and alpine canyon environments, due to problems such as limited GPS signals and the need to deploy anchor points in advance, the application of the above positioning systems is restricted. In large indoor scenarios, problems such as a large task scope, limited GPS signals, and insufficient timeliness caused by the need to deploy anchor points in advance, a multi-robot collaborative positioning and mapping system can autonomously achieve the navigation and real-time mapping of a robot cluster. Lidar-inertial Odometry (abbreviated as LIO) can rely on high-precision laser ranging and point clouds with rich measurement information to achieve reliable positioning. However, in indoor scenarios with high structural repeatability, the existing multi-robot collaborative positioning and mapping systems will inevitably have problems such as low efficiency and insufficient accuracy in loop detection and repositioning. Summary of the Invention

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

[0006] According to the first aspect of the embodiments of the present invention, a multi-robot real-time collaborative positioning method for indoor scenarios is provided, including: performing a key frame selection operation on consecutive frames of the indoor scenario according to the IMU data and lidar data collected by a first target robot to generate a key frame sequence; constructing a local semantic map corresponding to the indoor scenario based on the lidar data and the key frame sequence; for any current key frame in the key frame sequence: extracting Lidar-Iris descriptors 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 descriptors to generate a loop detection result; and performing local-global optimization on the current key frame based on the loop detection result to generate the global pose of the first target robot at the current moment.

[0007] Optionally, based on the loop detection result, locally-global optimize the current key frame to generate the global pose corresponding to the first target robot at the current moment, including: if the loop detection result indicates that there is a first quasi-key frame having an in-aircraft loop relationship with the current key frame, then perform IPC registration on the first quasi-key frame and the current key frame to generate the locally 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-aircraft loop relationship with the current key frame, then perform IPC registration on the second quasi-key frame and the current key frame to generate a relative pose transformation; based on the locally optimized pose corresponding to the current key frame, the relative pose transformation, and the locally optimized pose corresponding to the second quasi-key frame, globally optimize the first target robot to determine the global pose corresponding to the first target robot at the current moment.

[0008] Optionally, perform key frame selection operations on consecutive frames of the indoor scene collected by the first target robot according to IMU data and lidar data of the indoor scene to generate a key frame sequence, including: for any current frame in the consecutive 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 this key frame as a reference frame; based on the IMU data and lidar data corresponding to the reference frame, generate the 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, then determine that the current frame is a key frame; select several key frames from the consecutive 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; loop detection is performed on the current key frame in the key frame sequence based on the local semantic map and the Lidar-Iris descriptor, and a loop detection result is generated, 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; querying, from the key frame sequence, a candidate key frame whose similarity to the current key frame meets a preset condition based on the central 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, determining that the candidate key frame and the current key frame have a loop relationship, and determining the candidate key frame as a quasi-key frame having a loop relationship with the current key frame.

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

[0011] Optionally, the method further includes: constructing a central descriptor corresponding to the local semantic map; the constructing a central 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: 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; obtaining the Lidar-Iris descriptor corresponding to the center point cloud to generate a central descriptor corresponding to the local semantic map.

[0012] Optionally, globally optimizing the global pose of 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 of the first target robot corresponding to 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 systems 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; globally optimizing the local optimized pose of the first target robot based on the relative pose, and outputting the global pose of the first target robot corresponding to 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; obtaining the wall layer matching the Lidar-Iris descriptor from the room layer based on the Lidar-Iris descriptor corresponding to the current key frame; combining the wall layer with the global pose of the current key frame to generate the semantic pose corresponding to the current key frame; generating a positioning map of 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 a second aspect of an embodiment of the present invention, there is also provided a multi-robot real-time collaborative positioning device for an indoor scene. The device includes: a generation module, configured to perform key frame selection operations on consecutive frames of the indoor scene according to IMU data and lidar data collected by a first target robot, and 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; a pose optimization module, configured to, for any current key frame in the key frame sequence: extract a Lidar-Iris descriptor from the current key frame; perform 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 perform local-global optimization on the current key frame based on the loop detection result to generate the global pose of the first target robot corresponding to the current moment.

[0015] According to a third aspect of an embodiment of the present invention, there is also provided an electronic device, including: a processor; a memory for storing executable 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 as described in the first aspect.

[0016] According to a fourth aspect of the embodiments of the present invention, there is also provided a computer-readable medium, on which a computer program is stored, and when the program is executed by a processor, the method described in the first aspect is implemented.

[0017] The embodiments of the present invention provide a multi-robot real-time collaborative localization method and device for an indoor scene. The method includes: First, according to the IMU data and lidar data of the indoor scene collected by a first target robot, perform a key frame selection operation on consecutive frames of the indoor scene to generate a key frame sequence; Second, based on the lidar data and the key frame sequence, construct a local semantic map corresponding to the indoor scene; Finally, for any current key frame in the key frame sequence: extract Lidar-Iris descriptors from the current key frame; Based on the local semantic map and the Lidar-Iris descriptors, 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 of the first target robot at the current moment. The method of this embodiment first performs loop detection based on the matching of central descriptors, improving the robustness and accuracy of loop matching; Secondly, based on the loop relationship, perform two-stage distributed local-global optimization, further improving the perception accuracy of the robot's pose; Thus, the technical problems of perception confusion caused by repeated indoor scene structures and the sensitivity of distributed optimization to initial values in the prior art are solved, thereby improving the accuracy of multi-robot collaborative localization and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Some specific embodiments of the present invention will be described in detail hereinafter with reference to the drawings in an exemplary but not restrictive manner. The same reference numerals in the drawings denote the same or similar components or parts. Those skilled in the art should understand that these drawings are not necessarily drawn to scale. In the drawings:

[0019] Figure 1 is a flowchart of a multi-robot real-time collaborative localization method for an indoor scene provided by an embodiment of the present invention;

[0020] Figure 2 is a flowchart of loop detection for a current key frame in an embodiment of the present invention;

[0021] Figure 3 is a flowchart of determining the global pose of the first target robot at the current moment in an embodiment of the present invention;

[0022] Figure 4 is a structural diagram of a multi-robot real-time collaborative localization device for an indoor scene provided by an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0023] To make the objectives, features, and advantages of the present invention more obvious and understandable, the following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative efforts belong to the scope of protection of the present invention.

[0024] As Figure 1 shown, it is a schematic flowchart of a multi-robot real-time collaborative positioning method for an indoor scenario provided by an embodiment of the present invention.

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

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

[0027] S102. Based on the lidar data and the key frame sequence, construct a local semantic map corresponding to the indoor scenario;

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

[0029] In S101, the key frame not only achieves a balance between map density and memory consumption, but also helps to maintain a relatively sparse factor graph, which is suitable for real-time non-linear optimization. The selection of key frames also determines the success rate of loop closure detection. Therefore, the selection of key frames should cover the scanned map as much as possible.

[0030] Based on a preset rule or algorithm model, the effective fusion and processing of IMU data and lidar data can achieve efficient key frame selection, and then generate an accurate key frame sequence.

[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 that is before the current moment and adjacent to the current moment, and use this key frame as the reference frame; Based on the IMU data and lidar data corresponding to the reference frame, generate the 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, 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.

[0032] Specifically, the front end of the odometer receives IMU data from the IMU and lidar data from the lidar; If the sampling frequency of the IMU is too high, the acceleration and angular velocity data collected by the IMU can be used at intervals in engineering. Considering the motion characteristics of the machine platform, if an IMU sensor is installed, the IMU data can be used for point cloud distortion correction.

[0033] Considering the motion characteristics of the actual machine platform, the front end of the odometer selects a suitable lidar inertial odometer. For example: For an unmanned vehicle platform, the front end of the LIO-SAM odometer can be used as the odometer front end of the system. For an aerial robot platform, the front ends of FAST-LIO and DLIO can be selected as the odometer front ends of the system. If the machine platform is not equipped with an IMU, LeGO-LOAM can be selected as the odometer front end of the system.

[0034] If the current frame and the reference frame are 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, then determine that the current frame is a key frame; If the current frame enters the next room layer, then 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, then determine that the current frame is 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. To improve the calculation and communication efficiency, when selecting key frames, the first preset threshold includes a position change threshold and a rotation change threshold.

[0035] The selection of key frames is a process of balancing computational efficiency and map integrity. Through reasonable threshold settings and condition controls, it can effectively support real-time localization and mapping tasks. By restricting the above two conditions, it can be ensured that key frames are not only representative in space but also have sufficient distinctiveness in feature description, thus optimizing the performance of the entire system.

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

[0037] For example: The 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 different constituent elements in the building layout.

[0038] Using the S-Graphs+ algorithm, combined with the lidar data and key frame sequence of the first target robot, a local semantic map corresponding to the indoor scene is generated. This local semantic map not only includes the geometric information of the environment but also can estimate the pose information of the first target robot.

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

[0040] Key frame layer: This layer consists of the poses (i.e., positions and orientations) of the robot, and each pose is regarded as a node in the proxy semantic map framework. Constraint relationships are established for these key frames based on paired odometry measurements.

[0041] Wall layer: Plane features are extracted from the point cloud data obtained from the Lidar, and the plane wall surface is obtained using the minimum plane parameterization. Each plane wall surface is constructed according to the corresponding key frame and is modeled as a pose-plane constraint factor.

[0042] Room layer: This layer represents the rooms formed by corridors or four walls, and 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 floor nodes located at the center of the current floor, which is used to determine the relative positions of each room and wall in the entire building.

[0044] After constructing these layers, the graph optimization algorithm can be used to integrate and optimize this information, further improving the robot's understanding of the environment and positioning accuracy. Thus, through such a design, not only can the geometric information of the building be comprehensively captured, but also the robot's navigation and task execution in complex environments can be effectively supported. This construction method of the local semantic map provides strong support for intelligent robots in practical applications.

[0045] In S103, if the loop detection result indicates the existence of a first quasi-keyframe having an in-machine loop relationship with the current keyframe, the first quasi-keyframe and the current keyframe are subjected to IPC registration to generate a locally optimized pose corresponding to the current keyframe; if the loop detection result indicates the existence of a second quasi-keyframe having an inter-machine loop relationship with the current keyframe, the second quasi-keyframe and the current keyframe are subjected to IPC registration to generate a relative pose transformation; based on the locally optimized pose corresponding to the current keyframe, the relative pose transformation, and the locally optimized pose corresponding to the second quasi-keyframe, 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.

[0046] In this embodiment, based on the local semantic map and the Lidar-Iris descriptor, loop relationship detection is performed on the current keyframe, thereby improving the robustness and accuracy of loop matching; then, local pose optimization is performed on the current keyframe based on the in-machine loop relationship, and global pose optimization is performed on the current keyframe based on the inter-machine loop relationship, thereby improving the accuracy of multi-robot cooperative positioning in an indoor scene.

[0047] As Figure 2 shown, it is a schematic flowchart of loop detection for the current keyframe in an embodiment of the present invention.

[0048] In the global optimization stage, only the Lidar-Iris descriptor and the keyframe point cloud need to be interacted to verify the relative pose transformation between two frames of robots. In the inter-machine loop stage and the in-machine loop stage, the central descriptor needs to be transmitted. First, the central descriptor is matched to reduce the search space of the keyframe and improve the accuracy and robustness of the matching.

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

[0050] S201, for any current keyframe in the keyframe sequence: determining the wall layer corresponding to the current keyframe based on the local semantic map; establishing a semantic relationship between the Lidar-Iris descriptor corresponding to the current keyframe and the wall layer to generate a hybrid descriptor; querying candidate keyframes that satisfy a preset condition with the current keyframe from the keyframe sequence based on the central descriptor corresponding to the local semantic map and the hybrid descriptor;

[0051] S202, if the Hamming distance between the candidate keyframe and the current keyframe is less than a fourth preset threshold, it is determined that the candidate keyframe and the current keyframe have a loop relationship, and the candidate keyframe is determined as a quasi-keyframe having a loop relationship with the current keyframe.

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

[0053] Constructing a central 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 within the room layer to generate a center point cloud; obtaining the Lidar-Iris descriptor corresponding to the center point cloud to generate a central descriptor corresponding to the local semantic map. Among them, the central descriptor is used to indicate the Lidar-Iris descriptor with the room as the center point cloud. For example: First, obtain the center point and boundary of the room according to the semantic and hierarchical information of the local semantic map to extract all key frames obtained from within the room layer and generate the corresponding center point cloud. Downsample the center point cloud centered on the center point of the room to equalize the number of point clouds in each room, and finally generate the corresponding Lidar-Iris descriptor for the downsampled center point cloud. This strategy can effectively reduce the sensitivity to linear motion and enhance the descriptor matching effect.

[0054] Secondly, query similar candidate key frames;

[0055] Based on the central descriptor corresponding to the local semantic map and the hybrid descriptor, query candidate key frames from the key frame sequence whose similarity to the current key frame meets a preset condition; including: based on the central descriptor corresponding to the local semantic map, determine the room layer corresponding to the hybrid descriptor and generate a Lidar-Iris descriptor; for any row key value k to be matched in the KD tree: perform a cosine operation on the row key value k to be matched and the row key value k corresponding to the Lidar-Iris descriptor to generate a cosine distance; if the cosine distance is greater than a third preset threshold, then determine the key frame corresponding to the 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 central descriptor corresponding to the local semantic map; query the key frame similar to the row key value k from the KD tree and use this key frame as a candidate key frame. For example: Select a lightweight Lidar-Iris descriptor with rotation invariance characteristics. Similar to the Scan Context descriptor, divide the point cloud data into N r ×N s point cloud subsets according to the rotation direction and radial direction, and project the 3D point cloud onto a 2D plane. Each element in the descriptor matrix consists of 8-bit binary numbers a ij i = [1, 2,..., N r , j = [1, 2,..., Ns , thus dividing the point cloud into 8 consecutive heights. 1 indicates that there is a point cloud in the current height region, and conversely, there is no point cloud in that height region.

[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 Equation (1):

[0058]

[0059] where k i represents the eigenvalue of each ring and satisfies the rotation invariance property.

[0060] Descriptor matching: Perform a 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 searched key frames as candidate key frames. The logic for selecting candidate key frames based on the cosine distance is as shown in Equation (2) below.

[0061]

[0062] where and are the row key values of the m-th and n-th key frames in robots α and β respectively, η is the cosine distance threshold, is the cosine operation.

[0063] Finally, filter out outliers from the candidate key frames.

[0064] Compare the Lidar-Iris descriptor corresponding to the current key frame with the Lidar-Iris descriptor corresponding to the candidate key frame. If the Hamming distance between the candidate key frame and the current key frame is less than the threshold, it is considered that a loop closure is found. The judgment logic based on the Hamming distance is as shown in Equation (3) below.

[0065]

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

[0067] Thus, this embodiment enhances the robustness and accuracy of loop closure detection based on the local semantic map and the hybrid descriptor, thereby providing a basis for realizing precise multi-robot collaborative localization.

[0068] Such as Figure 3As shown in the figure, it is a schematic flowchart of determining the global pose corresponding to the first target robot at the current moment in an embodiment of the present invention.

[0069] Determine the global pose corresponding to the first target robot at the current moment; at least including the following steps:

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

[0071] S302, 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, determine the relative pose between the local coordinate systems of the first target robot and the second target robot;

[0072] S303, based on the relative pose, globally optimize the local optimized pose of the first target robot, and output the global pose corresponding to the first target robot at the current moment.

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

[0074] The pose graph optimization receives the inter-frame relative poses output from the inter-robot loop and the intra-robot loop, and calibrates the trajectory drift of each robot through joint optimization. The present invention adopts a two-stage distributed Gauss-Seidel method as the backend optimization method of the system. First, optimize the attitude of the robot pose, and construct an objective function of the full state variable perturbation based on the optimized attitude of the robot to further optimize the robot pose.

[0075] For example: The backend pose graph optimization needs to provide a good initial pose value to ensure the convergence speed. The global optimization stage aims to provide a relatively accurate initial pose transformation for the pose graph optimization.

[0076] In the global optimization stage, when a common feature is detected between robots (i.e., in the same common area), based on the loop detection process, solve the relative pose transformation between two frames (α n , β m ) with the common feature between robots α and β In the local coordinate systems of robots α and β, the local optimized poses of α n and β m are respectively and They satisfy the following relational expressions:

[0077]

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

[0079]

[0080] Based on continuous loopback detection data, a measurement set of the relative pose 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 No. 1 as the unified global coordinate system, then T g1 = I 4×4 , and the parameter matrix T gα from the local coordinate system of the remaining robots to the global coordinate system 1α = T

[0081] Set the set T as the set of transformation matrices from the local coordinate systems of the remaining robots except robot No. 1 to the global coordinate system, 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, an error function of the relative pose between robots is constructed, such as

[0083]

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

[0085]

[0086] The sum of the residuals of the relative pose between each robot is minimized by the LM method to iteratively optimize the global coordinate system transformation matrix of the robot;

[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 realizes the global consistency of poses, but also improves the perception accuracy of the robot poses.

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

[0091] The following will describe in detail a multi-robot real-time collaborative positioning method for indoor scenarios provided in this embodiment in combination with a specific application scenario.

[0092] S1. For any current frame in a continuous sequence of frames of the indoor scenario: 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 that is before the current moment and adjacent to the current moment, and use this key frame as a reference frame; based on the IMU data and lidar data corresponding to the reference frame, generate the 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 descriptor of the current frame and the descriptor of the reference frame is greater than a second preset threshold, then determine that the current frame is a key frame; select several key frames from the continuous sequence of frames of the indoor scenario to obtain a key frame sequence.

[0093] S2. Based on the lidar data and the key frame sequence, construct a local semantic map corresponding to the indoor scenario.

[0094] S3. For any current key frame in the key frame sequence: Extract the Lidar-Iris descriptor from the current key frame; for any current key frame in the key frame sequence: Based on the local semantic map, determine the wall layer corresponding to the current key frame; 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 central descriptor corresponding to the local semantic map, determine the room layer corresponding to the hybrid descriptor to generate a Lidar-Iris descriptor; for any row key value k to be matched in the KD tree: perform a cosine operation on the row key value k to be matched and the row key value k corresponding to the Lidar-Iris descriptor to generate a cosine distance; if the cosine distance is greater than a third preset threshold, then determine the key frame corresponding to the row key value k to be matched 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, then determine that the candidate key frame and the current key frame have a loop closure relationship, and determine the candidate key frame as a quasi-key frame having a loop closure relationship with the current key frame.

[0095] S4. If the loop closure detection result indicates that there is a first quasi-key frame having an in-aircraft loop closure relationship with the current key frame; then perform IPC registration on the first quasi-key frame and the current key frame to generate the local optimized pose corresponding to the current key frame.

[0096] S5. 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, then perform IPC registration on the second quasi-key frame and the current key frame to generate a relative pose transformation.

[0097] S6. Obtain the locally optimized pose of the second target robot corresponding to the second quasi-key frame; based on the locally optimized pose and the relative pose transformation corresponding to the current key frame, and the locally optimized pose corresponding to the second quasi-key frame, determine the relative pose of the local coordinate systems between the first target robot and the second target robot; based on the relative pose, globally optimize the locally optimized pose of the first target robot, and output the global pose of 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 the 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; based on the semantic poses corresponding to each current key frame in the key frame sequence, generate a positioning map of the first target robot in the indoor scene.

[0099] In this embodiment, for the multi-robot real-time collaborative positioning and mapping method in a large indoor scene, loop detection is first performed based on the matching of central descriptors, which improves the robustness and accuracy of loop matching; secondly, two-stage distributed local-global optimization is performed based on the loop relationship, which further improves the perception accuracy of the robot pose; thus, the technical problems of perception confusion caused by repeated indoor scene structures and the sensitivity of distributed optimization to initial values in the prior art are solved, thereby improving the accuracy of multi-robot collaborative positioning and mapping.

[0100] As Figure 4 shown, it is a schematic structural diagram of a multi-robot real-time collaborative positioning device for an indoor scene provided by an embodiment of the present invention.

[0101] A multi-robot real-time collaborative positioning device for indoor scenarios. The device 400 includes: a generation module 401, configured to perform a key frame selection operation on consecutive frames of the indoor scenario according to the IMU data and lidar data collected by the first target robot, and generate a key frame sequence; a local semantic map construction module 402, configured to construct a local semantic map corresponding to the indoor scenario based on the lidar data and the key frame sequence; a pose optimization module 403, configured to, for any current key frame in the key frame sequence: extract Lidar-Iris descriptors from the current key frame; perform loop closure detection on the current key frame in the key frame sequence based on the local semantic map and the Lidar-Iris descriptors, and generate a loop closure detection result; and perform local-global optimization on the current key frame based on the loop closure detection result, and generate the global pose of the first target robot corresponding to the current moment.

[0102] In a preferred implementation manner of this embodiment, the pose optimization module includes: a first registration unit, configured to, if the loop closure detection result indicates the existence of a first quasi-key frame having an intra-aircraft loop closure relationship with the current key frame, perform IPC registration on the first quasi-key frame and the current key frame to generate a locally optimized pose corresponding to the current key frame; a second registration unit, configured to, if the loop closure detection result indicates the existence of a second quasi-key frame having an inter-aircraft loop closure relationship with the current key frame, perform IPC registration on the second quasi-key frame and the current key frame to generate a relative pose transformation; and a global pose optimization unit, configured to perform global pose optimization on the first target robot based on the locally optimized pose corresponding to the current key frame, the relative pose transformation, and the locally optimized pose corresponding to the second quasi-key frame, and determine the global pose of the first target robot corresponding to the current moment.

[0103] In a preferred implementation manner of this embodiment, the generation module includes: a determination unit, configured to, for any current frame in the consecutive frames of the indoor scenario: determine the current estimated pose of the target robot based on the IMU data and lidar data corresponding to the current frame; obtain a key frame located before the current moment and adjacent to the current moment, and use this key frame as a reference frame; generate a reference pose of the target robot corresponding to the reference frame based on the IMU data and lidar data 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 descriptor of the current frame and the descriptor of the reference frame is greater than a second preset threshold, determine that the current frame is a key frame; and a selection unit, configured to select several key frames from the consecutive frames of the indoor scenario to obtain a key frame sequence.

[0104] In a preferred embodiment of this embodiment, the pose optimization module includes: a selection unit, which is used 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; querying, from the key frame sequence, candidate key frames whose similarity to the current key frame meets a preset condition based on the central descriptor corresponding to the local semantic map and the hybrid descriptor; a determination unit, which is used for 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 if the Hamming distance between the candidate key frame and the current key frame is less than a fourth preset threshold.

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

[0106] In a preferred embodiment of this embodiment, the pose optimization module further includes: a construction unit, which is used for constructing a central descriptor corresponding to the local semantic map; the construction unit includes: an acquisition subunit, which is used for acquiring the center point and boundary corresponding to each room layer from the local semantic map; a downsampling subunit, which is used 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; and a generation subunit, which is used for acquiring the Lidar-Iris descriptor corresponding to the center point cloud to generate a central descriptor corresponding to the local semantic map.

[0107] In a preferred embodiment of this embodiment, the global pose optimization unit includes: an acquisition subunit, which is used for acquiring the local optimized pose of the second target robot corresponding to the second quasi-key frame; a determination subunit, which is used for determining the relative pose of the local coordinate systems 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; and an output subunit, which is used for globally optimizing the local optimized pose of the first target robot based on the relative pose and outputting the global pose of the first target robot corresponding to the current moment.

[0108] In a preferred implementation manner of this embodiment, the device further includes: a semantic pose construction module, configured to construct room-room factors based on the inter-machine loop closure relationship corresponding to the current key frame, obtain the room layer corresponding to the current key frame; based on the Lidar-Iris descriptor corresponding to the current key frame, obtain the 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 the semantic pose corresponding to the current key frame; a positioning map generation module, configured to generate a positioning map corresponding to the first target robot in the indoor scene based on the semantic poses corresponding to each current key frame in the key frame sequence.

[0109] The above device can execute a multi-robot real-time collaborative positioning method for indoor scenes provided by an embodiment of the present invention, and has corresponding functional modules and beneficial effects for executing a multi-robot real-time collaborative positioning method for indoor scenes. Technical details not described in detail in this embodiment can be referred to in a multi-robot real-time collaborative positioning method for indoor scenes provided by an embodiment of the present invention.

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

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

[0112] The computer program product can be written in any combination of one or more programming languages for programming code to perform the operations of the embodiments of the present application. The programming languages include object-oriented programming languages such as Java, C++, etc., and also include conventional procedural programming languages such as the "C" language or similar programming languages. The program code can be executed entirely on the user computing device, partially on the user device, executed as an independent software package, partially on the user 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 storing computer program instructions, which, when run by a processor, cause the processor to execute the steps in the methods according to the following embodiments of the present application described in the "Exemplary Methods" section above of this specification.

[0114] The computer-readable storage medium may adopt any combination of one or more readable media. The readable media may be a readable signal medium or a readable storage medium. The readable storage medium may, for example, include but is not limited to an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples (a non-exhaustive list) of the readable storage medium 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 of the above.

[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, benefits, effects, etc. mentioned in the present application are only examples and not limitations, and it cannot be considered that these advantages, benefits, effects, etc. are essential for each embodiment of the present application. In addition, the above-disclosed specific details are only for the purposes of illustration and facilitating understanding, and are not limitations. The above details do not limit the present application to necessarily adopt the above specific details for implementation.

[0116] The block diagrams of the devices, apparatuses, equipment, and systems involved in the present application are only illustrative examples and do not intend to require or imply that they must be connected, arranged, and configured in the manner shown in the block diagrams. As those skilled in the art will recognize, these devices, apparatuses, equipment, and systems can be connected, arranged, and configured in any manner. Words such as "including", "comprising", "having", etc. are open-ended words, meaning "including but not limited to", and can be used interchangeably with each other. The word "or" and "and" used herein refer to the word "and / or", and can be used interchangeably with each other 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 with each other.

[0117] It should also be noted that in the devices, equipment, and methods of the present application, each component or each step can be decomposed and / or recombined. These decompositions and / or recombinations should be regarded as equivalent solutions of the present application.

[0118] The above description of the disclosed aspects enables any person skilled in the art to make or use this application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein can be applied to other aspects without departing from the scope of this application. Therefore, this 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 given for purposes of illustration and description. In addition, this description is not intended to limit the embodiments of this application to the forms disclosed herein. Although multiple 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, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples", etc. means that the specific features, structures, materials, or characteristics described in connection 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 can be combined in a suitable manner in any one or more embodiments or examples. In addition, without contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples.

[0121] In addition, the terms "first" and "second" are used for descriptive purposes only and cannot be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include at least one of such features. In the description of the present invention, "a plurality" means two or more unless otherwise specifically defined.

[0122] As described above, the above are only the specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of changes or substitutions, which should all be covered by the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claimed rights.

Claims

1. A multi-machine real-time collaborative positioning method for indoor scenes, characterized in that: include: According to the IMU data and the laser radar 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; Based on the laser radar data and the key frame sequence, construct a local semantic map corresponding to the indoor scene; 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 a global pose corresponding to the first target robot at the current moment.

2. The method according to claim 1, characterized in that: Based on the loop detection result, the local-global optimization is performed on the current key frame to generate the global posture corresponding to the first target robot at the current moment; comprising: 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 performing IPC registration on the first quasi-key frame and the current key frame 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; then performing IPC registration on the second quasi-key frame and the current key frame to generate a relative pose transformation; 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, the global posture of the first target robot is optimized to determine the global posture corresponding to the first target robot at the current moment.

3. The method according to claim 1, characterized in that The method of performing a key frame selection operation on continuous frames of the indoor scene according to the IMU data and the laser radar data of the indoor scene collected by the first target robot to generate a key frame sequence comprises: For any current frame in the continuous frames of the indoor scene: determine the current estimated pose of the target robot based on the IMU data and the lidar data corresponding to the current frame; 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 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; 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, characterized in that: 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 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: 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 central 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; 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 method of searching for a candidate key frame whose similarity with the current key frame meets a preset condition from the key frame sequence based on the central descriptor corresponding to the local semantic map and the hybrid descriptor 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 cospin 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 cospin distance; if the cospin 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 construction of the central 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, downsample all key frames in the room layer to generate a center point cloud; The Lidar-Iris descriptor corresponding to the central point cloud is obtained, and a central descriptor corresponding to the local semantic map is generated.

7. The method according to claim 2, characterized in that: The method of performing 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 to determine the global posture corresponding to the first target robot at the current moment comprises: Obtaining a local optimized position and posture of a second target robot corresponding to the second quasi-key frame; Determine 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 posture, the local 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, a wall layer matching the Lidar-Iris descriptor is obtained from the room layer; the wall layer is combined 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 in that: include: A generation module, configured to perform a key frame selection operation on continuous frames of the indoor scene according to the IMU data and the laser radar data of the indoor scene collected by the 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 laser radar data and the key frame sequence; The posture optimization module is used for: 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 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 the program, when executed by a processor, implements the method according to any one of claims 1 to 8.

Images