Ranging-assisted multi-ground robot cooperative mapping method, device and equipment

Abstract

The disclosure provides a ranging-assisted multi-ground robot cooperative mapping method, device and equipment, which can be applied to the technical field of computer vision, the technical field of engineering and the technical field of data processing. The method comprises the following steps: acquiring first point cloud data and first pose transformation information of a master node robot at the k moment; acquiring a first distance between the master node robot and a slave node robot; acquiring a historical pose transformation information set of a first preset number of historical moments; receiving second point cloud data and second pose transformation information of the slave node robot at the k moment; obtaining third pose transformation information at the k moment according to the first distance, the first pose transformation information, the historical pose transformation information set and the second pose transformation information; and cooperatively mapping according to the first point cloud data, the second point cloud data and the third pose transformation information.

Description

Technical Field

[0001] This disclosure relates to the fields of computer vision technology, engineering technology and data processing technology, and more specifically, to a range-aided multi-ground robot collaborative mapping method, apparatus and equipment. Background Technology

[0002] In the absence of external positioning systems such as Global Navigation Satellite System (GNSS) and motion capture systems, relative positioning between robots typically relies on a visual / LiDAR closed loop between them. This method can only achieve map fusion when robots explore the same area, which significantly limits their range of movement. Cooperative technologies have wide applications in areas such as cooperative VR games, cooperative localization, and mapping. Existing vision-based cooperative mapping technologies require overlapping regions to establish relative pose constraints for cooperative localization; this method completely fails when robots explore different areas.

[0003] Among related technologies, Simultaneous Localization and Mapping (SLAM) has gained popularity among researchers due to its low cost and simple structure. However, visual SLAM maps are mostly sparse point cloud maps, which cannot reflect the structure and texture information of the environment, let alone provide navigation services for robots. Visual information cannot be used to obtain the relative poses between nodes, making collaborative work difficult. Although there are methods that use vision-based object matching or map matching for relative localization between nodes, these methods are complex and difficult to adapt to complex and changing environments.

[0004] In realizing the concept disclosed herein, the inventors discovered at least the following problems in the related technology: without an external positioning system and without using a visual loop closure detection method, it is difficult to map multiple robots together. Summary of the Invention

[0005] In view of this, this disclosure provides a method, apparatus and device for collaborative mapping of multiple ground robots with range-measuring assistance.

[0006] One aspect of this disclosure provides a range-aided collaborative mapping method for multiple ground robots, comprising:

[0007] Acquire the first point cloud data and the first pose transformation information of the master node robot at time k, wherein the first pose transformation information represents the pose transformation information of the master node robot in the first visual odometry coordinate system between time k and time k-1.

[0008] Obtain the first distance between the master node robot and the slave node robot;

[0009] Obtain a set of historical pose transformation information for a first preset number of historical moments, wherein the set of historical pose transformation information represents the set of pose transformation information of the master node robot in the world coordinate system at each of the historical moments and the set of pose transformation information of the slave node robot in the world coordinate system at each of the historical moments.

[0010] Receive the second point cloud data and the second pose transformation information of the slave node robot at time k, wherein the second pose transformation information represents the pose transformation information of the slave node robot in the second visual odometry coordinate system between time k and time k-1.

[0011] Based on the first distance, the first pose transformation information, the set of historical pose transformation information, and the second pose transformation information, the third pose transformation information at time k is obtained. The third pose transformation information represents the pose transformation information of the master node robot at time k in the world coordinate system and the pose transformation information of the slave node robot at time k in the world coordinate system.

[0012] A collaborative mapping is constructed based on the first point cloud data, the second point cloud data, and the third pose transformation information.

[0013] Another aspect of this disclosure provides a range-aided multi-ground robot collaborative mapping device, comprising:

[0014] The first data and first information acquisition module is used to acquire the first point cloud data and the first pose transformation information of the master node robot at time k. The first pose transformation information represents the pose transformation information of the master node robot in the first visual odometry coordinate system between time k and time k-1.

[0015] The first distance acquisition module is used to acquire the first distance between the master node robot and the slave node robot.

[0016] The historical information set acquisition module is used to acquire a first preset number of historical pose transformation information sets, wherein the aforementioned historical pose transformation information set represents the pose transformation information of the master node robot in the world coordinate system at each of the aforementioned historical moments and the set of pose transformation information of the slave node robot in the world coordinate system at each of the aforementioned historical moments.

[0017] The second data and second information receiving module is used to receive the second point cloud data and the second pose transformation information of the slave node robot at time k, wherein the second pose transformation information represents the pose transformation information of the slave node robot in the second visual odometry coordinate system between time k and time k-1.

[0018] The third information acquisition module is used to obtain the third pose transformation information at time k based on the first distance, the first pose transformation information, the historical pose transformation information set, and the second pose transformation information. The third pose transformation information represents the pose transformation information of the master node robot at time k in the world coordinate system and the pose transformation information of the slave node robot at time k in the world coordinate system.

[0019] The mapping module is used to collaboratively build a map based on the first point cloud data, the second point cloud data, and the third pose transformation information.

[0020] Another aspect of this disclosure provides an electronic device, including: one or more processors; and a memory for storing one or more instructions, wherein when the one or more instructions are executed by the one or more processors, they cause the one or more processors to implement the method described above. Attached Figure Description

[0021] The above and other objects, features and advantages of this disclosure will become clearer from the following description of embodiments with reference to the accompanying drawings, in which:

[0022] Figure 1 This diagram schematically illustrates an application scenario of a range-aided multi-ground robot collaborative mapping method according to embodiments of the present disclosure.

[0023] Figure 2 A flowchart illustrating a range-aided multi-ground robot collaborative mapping method according to an embodiment of the present disclosure is shown schematically.

[0024] Figure 3 A schematic diagram illustrating the principle of a range-aided multi-ground robot collaborative mapping method according to an embodiment of the present disclosure is shown.

[0025] Figure 4A This illustration schematically depicts an application scenario of a range-aided multi-ground robot collaborative mapping method according to another embodiment of the present disclosure;

[0026] Figure 4B A schematic diagram of a master node robot according to an embodiment of the present disclosure is shown;

[0027] Figure 4C A schematic diagram of a slave node robot according to an embodiment of the present disclosure is shown;

[0028] Figure 5 This diagram illustrates the effect of mapping using the range-aided multi-ground robot collaborative mapping method according to embodiments of the present disclosure.

[0029] Figure 6 A block diagram of a range-aided multi-ground robot collaborative mapping apparatus according to an embodiment of the present disclosure is shown schematically.

[0030] Figure 7 A block diagram of a computer system suitable for implementing the methods described above, according to embodiments of the present disclosure, is illustrated schematically. Detailed Implementation

[0031] The embodiments of the present disclosure will now be described with reference to the accompanying drawings. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of the disclosure. In the following detailed description, numerous specific details are set forth to provide a thorough understanding of the embodiments of the present disclosure for ease of explanation. However, it will be apparent that one or more embodiments may be practiced without these specific details. Furthermore, descriptions of well-known structures and techniques are omitted in the following description to avoid unnecessarily obscuring the concepts of the present disclosure.

[0032] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit this disclosure. The terms “comprising,” “including,” etc., as used herein indicate the presence of the stated features, steps, operations, and / or components, but do not exclude the presence or addition of one or more other features, steps, operations, or components.

[0033] All terms used herein (including technical and scientific terms) have the meanings commonly understood by those skilled in the art, unless otherwise defined. It should be noted that the terms used herein are to be interpreted in a manner consistent with the context of this specification, and not in an idealized or overly rigid way.

[0034] When using expressions such as "at least one of A, B, and C," the expression should generally be interpreted in accordance with the meaning commonly understood by a person skilled in the art (e.g., "a system having at least one of A, B, and C" should include, but is not limited to, systems having A alone, having B alone, having C alone, having A and B, having A and C, having B and C, and / or having A, B, and C, etc.). Similarly, when using expressions such as "at least one of A, B, or C," the expression should generally be interpreted in accordance with the meaning commonly understood by a person skilled in the art (e.g., "a system having at least one of A, B, or C" should include, but is not limited to, systems having A alone, having B alone, having C alone, having A and B, having A and C, having B and C, and / or having A, B, and C, etc.).

[0035] In the technical solutions disclosed herein, the collection, storage, use, processing, transmission, provision, disclosure, and application of data (including but not limited to user personal information) comply with the provisions of relevant laws and regulations, necessary confidentiality measures have been taken, and they do not violate public order and good morals.

[0036] In the absence of an external positioning system and without the use of visual loop closure detection methods, it is difficult to establish a system map between multiple robots.

[0037] In order to at least partially solve the technical problems existing in the related technologies, this disclosure provides a range-aided multi-ground robot collaborative mapping method, apparatus and equipment, which can be applied to the fields of computer vision technology, engineering technology and data processing technology.

[0038] Figure 1 The diagram illustrates an application scenario of a range-aided multi-ground robot collaborative mapping method according to an embodiment of the present disclosure.

[0039] like Figure 1 As shown, application scenario 100 according to this embodiment may include a first terminal device 101, a second terminal device 102, a third terminal device 103, a network 104, and a server 105. The network 104 serves as a medium for providing a communication link between the first terminal device 101, the second terminal device 102, the third terminal device 103, and the server 105. The network 104 may include various connection types, such as wired or wireless communication links, or fiber optic cables, etc.

[0040] Users can interact with server 105 via network 104 using at least one of the first terminal device 101, second terminal device 102, and third terminal device 103 to receive or send messages, etc. Various communication client applications can be installed on the first terminal device 101, second terminal device 102, and third terminal device 103, such as shopping applications, web browser applications, search applications, instant messaging tools, email clients, social media platform software, etc. (for example only).

[0041] The first terminal device 101, the second terminal device 102, and the third terminal device 103 can be various electronic devices with displays and support web browsing, including but not limited to smartphones, tablets, laptops, and desktop computers.

[0042] Server 105 can be a server that provides various services, such as a backend management server that supports websites browsed by users using the first terminal device 101, the second terminal device 102, and the third terminal device 103 (this is just an example). The backend management server can analyze and process data such as received user requests, and feed back the processing results (such as web pages, information, or data obtained or generated according to user requests) to the terminal devices.

[0043] It should be noted that the distance-assisted multi-terrain robot collaborative mapping method provided in this disclosure embodiment can generally be executed by server 105. Correspondingly, the distance-assisted multi-terrain robot collaborative mapping device provided in this disclosure embodiment can generally be located in server 105. The distance-assisted multi-terrain robot collaborative mapping method provided in this disclosure embodiment can also be executed by a server or server cluster that is different from server 105 and capable of communicating with the first terminal device 101, the second terminal device 102, the third terminal device 103, and / or server 105. Correspondingly, the distance-assisted multi-terrain robot collaborative mapping device provided in this disclosure embodiment can also be located in a server or server cluster that is different from server 105 and capable of communicating with the first terminal device 101, the second terminal device 102, the third terminal device 103, and / or server 105.

[0044] It should be understood that Figure 1 The number of terminal devices, networks, and servers shown is merely illustrative. Depending on implementation needs, any number of terminal devices, networks, and servers can be included.

[0045] Figure 2 A flowchart illustrating a range-aided multi-ground robot collaborative mapping method according to an embodiment of the present disclosure is shown schematically.

[0046] like Figure 2 As shown, the method includes operations S201 to S206.

[0047] In operation S201, the first point cloud data and the first pose transformation information of the master node robot at time k are obtained. The first pose transformation information represents the pose transformation information of the master node robot in the first visual odometry coordinate system between time k and time k-1.

[0048] According to embodiments of this disclosure, the first point cloud data can be all the three-dimensional point cloud data of the master node robot at time k, or it can be the remaining three-dimensional point cloud data after removing the three-dimensional point cloud data corresponding to the ground.

[0049] According to embodiments of this disclosure, when the first point cloud data is the remaining three-dimensional point cloud data after removing the three-dimensional point cloud data corresponding to the ground, the first point cloud data has a smaller data volume, which can reduce data transmission time.

[0050] According to an embodiment of this disclosure, the master node robot is equipped with an RGB-D (Red Green Blue-Depth) camera.

[0051] According to embodiments of this disclosure, the master node robot uses an RGB-D camera to acquire two-dimensional images and all three-dimensional point cloud data at each moment.

[0052] According to embodiments of this disclosure, visual odometry information can be obtained from all the three-dimensional point cloud data and two-dimensional images of the master node robot at time k using the ORB-SLAM2 (Oriented FAST and Rotated BRIED-Simultaneous Localization and Mapping) algorithm, that is, the first pose transformation information of the master node robot at time k can be obtained.

[0053] According to an embodiment of this disclosure, the RGB-D camera configured on the master node robot is calibrated in advance to obtain a first camera coordinate system. The first visual odometry coordinate system is obtained by rotating the first camera coordinate system 90 degrees clockwise along the y-axis.

[0054] In operation S202, the first distance between the master node robot and the slave node robot is obtained.

[0055] According to embodiments of this disclosure, the master node robot is equipped with at least four UWB (Ultra Wide Band) wireless ranging sensors, and the slave node robot is equipped with at least two UWB wireless ranging sensors. Both the master node robot and the slave node robot use UWB wireless ranging sensors as ranging devices.

[0056] According to embodiments of this disclosure, the distance between the ranging devices of the master node robot and the slave node robot can be obtained by using the UWB wireless ranging sensors equipped on the master node robot and the slave node robot. This distance can be used as the first distance between the master node robot and the slave node robot.

[0057] According to embodiments of this disclosure, the first distance between the master node robot and the slave node robot at each moment can be obtained using UWB wireless ranging sensors equipped on the master node robot and the slave node robot.

[0058] In operation S203, a set of historical pose transformation information for a first preset number of historical moments is obtained, wherein the set of historical pose transformation information represents the pose transformation information of the master node robot in the world coordinate system at each historical moment and the set of pose transformation information of the slave node robot in the world coordinate system at each historical moment.

[0059] According to the embodiments of this disclosure, the first preset quantity is an integer greater than or equal to 1, such as 1, 4, 7, etc. The embodiments of this disclosure do not limit the specific first preset quantity, and the first preset quantity can be selected according to the actual situation.

[0060] According to embodiments of this disclosure, k is an integer greater than a first preset number.

[0061] According to an embodiment of this disclosure, the first preset number of historical moments are a series of consecutive moments, and one of the historical moments in the first preset number of historical moments is an adjacent moment to the k-th moment, and the historical moment is a moment that occurred before the k-th moment.

[0062] According to embodiments of this disclosure, the world coordinate system can be defined as the first visual odometry coordinate system of the master node robot at the initialization moment.

[0063] According to embodiments of this disclosure, the pose transformation information representation of the master node robot at each historical moment in the world coordinate system transforms the three-dimensional point cloud data of the master node robot in the first visual odometry coordinate system at each historical moment into pose transformation information in the world coordinate system (the first visual odometry coordinate system at the initialization moment).

[0064] According to embodiments of this disclosure, the pose transformation information of the node robot at each historical moment in the world coordinate system is represented by converting the three-dimensional point cloud data of the node robot in the second visual odometry coordinate system at each historical moment into pose transformation information in the world coordinate system (the first visual odometry coordinate system at the initialization moment).

[0065] In operation S204, the second point cloud data and the second pose transformation information of the slave node robot at time k are received, wherein the second pose transformation information represents the pose transformation information of the slave node robot in the second visual odometry coordinate system between time k and time k-1.

[0066] According to embodiments of this disclosure, the second point cloud data can be all the three-dimensional point cloud data of the slave node robot at time k, or it can be the remaining three-dimensional point cloud data after removing the three-dimensional point cloud data corresponding to the ground.

[0067] According to embodiments of this disclosure, when the second point cloud data is the remaining three-dimensional point cloud data after removing the three-dimensional point cloud data corresponding to the ground, the second point cloud data has a smaller data volume, which can reduce data transmission time.

[0068] According to an embodiment of this disclosure, a node robot is equipped with an RGB-D camera.

[0069] According to embodiments of this disclosure, the node robot uses an RGB-D camera to acquire two-dimensional images and all three-dimensional point cloud data at each moment.

[0070] According to embodiments of this disclosure, visual odometry information can be obtained from all three-dimensional point cloud data and two-dimensional images of the slave node robot at time k using the ORB-SLAM2 algorithm, that is, the second pose transformation information of the slave node robot at time k can be obtained.

[0071] According to an embodiment of this disclosure, the RGB-D camera configured on the node robot is calibrated in advance to obtain a second camera coordinate system. The second visual odometry coordinate system is obtained by rotating the second camera coordinate system 90 degrees clockwise along the y-axis.

[0072] According to embodiments of this disclosure, the number of slave node robots can be multiple, such as 1, 6, or 10. Embodiments of this disclosure do not limit the specific number of slave node robots, and the number of slave node robots can be selected according to the actual situation.

[0073] In operation S205, based on the first distance, the first pose transformation information, the set of historical pose transformation information, and the second pose transformation information, the third pose transformation information at time k is obtained. The third pose transformation information represents the pose transformation information of the master node robot at time k in the world coordinate system and the pose transformation information of the slave node robot at time k in the world coordinate system.

[0074] According to embodiments of this disclosure, a pose transformation state vector related to the third pose transformation information can be obtained based on the pose transformation information in the historical pose transformation information set. Then, the pose transformation state vector related to the third pose transformation information, the first distance, the first pose transformation information, and the second pose transformation information are used to estimate the third pose transformation information based on an optimization method. That is, the third pose transformation information corresponding to the first pose transformation information in the world coordinate system and the third pose transformation information corresponding to the second pose transformation information in the world coordinate system are estimated.

[0075] According to embodiments of this disclosure, the pose transformation information of the master node robot at time k in the world coordinate system represents the pose transformation information of the master node robot at time k in the first visual odometry coordinate system (the first visual odometry coordinate system at the initialization time) by converting the three-dimensional point cloud data of the master node robot at time k in the first visual odometry coordinate system to the world coordinate system (the first visual odometry coordinate system at the initialization time).

[0076] According to embodiments of this disclosure, the pose transformation information of the node robot at time k in the world coordinate system represents the pose transformation information of the node robot at time k in the second visual odometry coordinate system converted to the world coordinate system (the first visual odometry coordinate system at the initialization time).

[0077] In operation S206, a collaborative mapping is constructed based on the first point cloud data, the second point cloud data, and the third pose transformation information.

[0078] According to embodiments of this disclosure, the first point cloud data and the second point cloud data can be converted to the world coordinate system based on the third pose transformation information to obtain the three-dimensional point cloud data corresponding to the first point cloud data and the three-dimensional point cloud data corresponding to the second point cloud data in the world coordinate system, thereby completing the mapping.

[0079] According to an embodiment of this disclosure, a processor that executes the distance-assisted multi-ground robot collaborative mapping method provided in this disclosure can be installed on a master node robot. In this case, the first point cloud data is all the three-dimensional point cloud data of the master node robot at time k. During the collaborative mapping process, the processor can filter out the three-dimensional point cloud data corresponding to the ground in the first point cloud data to reduce the amount of data involved in mapping, thereby reducing the collaborative mapping time at time k.

[0080] According to an embodiment of this disclosure, when the processor that executes the ranging-assisted multi-ground robot collaborative mapping method provided in the embodiment of this disclosure is installed on the master node robot, the second point cloud data can be the remaining three-dimensional point cloud data after removing the three-dimensional point cloud data corresponding to the ground. In this case, the second point cloud data has a smaller data volume, which can reduce the data transmission time and reduce the amount of data involved in mapping, thereby further reducing the collaborative mapping time at time k.

[0081] According to embodiments of this disclosure, multiple UWB wireless ranging sensors are installed on a single node robot, and the position and orientation of the node can be estimated simultaneously using an optimization-based method, overcoming the limitation of traditional methods that can only directly locate.

[0082] According to the embodiments of this disclosure, the distance-assisted multi-ground robot collaborative mapping method provided by the embodiments of this disclosure can achieve the technical effect of collaborative mapping among multiple robots in the world coordinate system at time k, without the need for an external positioning system and without the use of a visual loop closure detection method. This is achieved by using a set of historical pose transformation information, the first distance between the master node robot and the slave node robot at time k, and the pose transformation information of multiple robots in the visual odometry coordinate system at time k. This results in obtaining the three-dimensional point cloud data of multiple robots in the world coordinate system measured at time k.

[0083] According to the embodiments of this disclosure, the range-assisted multi-ground robot collaborative mapping method provided by the embodiments of this disclosure does not rely on visual loopback conditions to perform collaborative mapping. The method is stable, reliable, and practical, and can be widely applied to indoor and outdoor collaborative mapping tasks.

[0084] According to embodiments of this disclosure, a ground robot team consisting of multiple node robots can be labeled as Where n∈{0,...,N}, N is greater than or equal to 1, and there exists a master node robot, for example, it can be used as... The master node robot is represented by one of the other nodes.

[0085] According to embodiments of this disclosure, the master node robot in the ground robot team is equipped with an RGB-D camera and four UWB wireless ranging sensors, while the other slave node robots are each equipped with an RGB-D camera and two UWB wireless ranging sensors.

[0086] According to embodiments of this disclosure, it is possible to make Let the camera odometry coordinate system of the nth node robot be denoted as . This represents the position of the UWB wireless ranging sensor relative to the camera odometry coordinate system of the nth node robot, where the camera odometry coordinate system includes a first visual odometry coordinate system and a second visual odometry coordinate system. It is pre-calibrated. The world coordinate system can be defined as the coordinate system of the central node robot during initialization, i.e.

[0087] According to embodiments of this disclosure, when n equals 0, the camera odometry coordinate system is the first visual odometry coordinate system of the master node robot, and when n is greater than or equal to 1, the camera odometry coordinate system is the second visual odometry coordinate system of the slave node robot.

[0088] According to embodiments of this disclosure, any coordinate system can be represented by {F}, and the position vector in coordinate system {F} can be represented by... Ft represents, where, To represent a three-dimensional vector, the rotation matrix in {F} can be used... F R represents, where, F R∈SO(3), SO(3) represents a special orthogonal group, and F The quaternion corresponding to R can be used as follows: F q means, Represents quaternions.

[0089] According to embodiments of this disclosure, the homogeneous transformation matrix in coordinate system {F} F T can be defined as formula (1):

[0090]

[0091] in, F T∈SE(3), SE(3) represents a special Euclidean group.

[0092] According to embodiments of this disclosure, at time k, the first distance between the master node robot and the slave node robot measured by the UWB wireless ranging sensor can be used... Let represent the i-th slave node robot, g represent the g-th ranging device on the i-th slave node robot, and h represent the h-th ranging device on the master node robot.

[0093] According to embodiments of this disclosure, the pose transformation information mentioned in these embodiments is a homogeneous transformation matrix. For example, the first pose transformation information is the homogeneous transformation matrix of the master node robot in the first visual odometry coordinate system between time k and time k-1. The second pose transformation information is the homogeneous transformation matrix of the slave node robot in the second visual odometry coordinate system between time k and time k-1. The third pose transformation information is the homogeneous transformation matrix of the master node robot in the world coordinate system at time k and the homogeneous transformation matrix of the slave node robot in the world coordinate system at time k.

[0094] According to embodiments of this disclosure, the second point cloud data is obtained by the slave node robot performing the following operations:

[0095] Acquire the third point cloud information of the slave node robot at time k. The third point cloud information represents all the three-dimensional point cloud data acquired by the slave node robot at time k.

[0096] Based on the third point cloud information, the Random Sample Consensus (RANSAC) algorithm is used to obtain the ground normal vector included in the third point cloud information.

[0097] Based on the normal vector, the 3D point cloud data corresponding to the ground in the third point cloud information is filtered out to obtain the second point cloud data.

[0098] According to embodiments of this disclosure, the third point cloud information of the slave node robot at time k can be obtained using an RGB-D camera on the slave node robot. Then, based on the third point cloud information and two-dimensional image of the slave node robot at time k, the visual odometry information is obtained using the ORB-SLAM2 algorithm, which is the second pose transformation information of the slave node robot at time k.

[0099] According to embodiments of this disclosure, when detecting the ground in each keyframe, an initial approximate distance from the RGB-D camera to the ground is assumed. and normal vector Given that the initial plane coefficient is in, This represents the distance from the RGB-D camera to the ground, determined based on actual conditions. The camera odometry coordinate system that represents any node robot.

[0100] According to embodiments of this disclosure, when extracting the ground within a certain height range from the third point cloud information, three-dimensional points on the ground... The following formula (2) should be satisfied:

[0101]

[0102] According to an embodiment of this disclosure, equation (2) represents the ground in Hessian matrix form, and is derived from the ground's normal vector. and the distance from the ground to the origin of the local coordinate system composition.

[0103] According to embodiments of this disclosure, initial point cloud data for iteratively calculating the ground can be determined from the third point cloud information based on the initial plane coefficient π0 and formula (2). Then, based on the initial point cloud data, the ground parameters are recalculated using the RANSAC algorithm. The normal to the ground being inspected and If the angle between them is less than 10 degrees, the ground is considered to be detected.

[0104] According to embodiments of this disclosure, after obtaining ground parameters Then, using renew Then, based on formula (2) and ground parameters... The three-dimensional point cloud data corresponding to the ground that conforms to formula (2) is filtered out to obtain the second point cloud data.

[0105] According to embodiments of this disclosure, a processor that executes the range-assisted multi-ground robot collaborative mapping method provided in this disclosure can be installed on a master node robot. Then, the second pose transformation information and second point cloud data of the slave node robot at time k can be sent to the master node robot. The processor on the master node robot performs collaborative mapping on the data sent by the master node robot and the slave node robot. Since the second point cloud data is the point cloud data after filtering out the three-dimensional point cloud data corresponding to the ground from the third point cloud information, the second point cloud data has a smaller data volume than the third point cloud information, which can reduce the data transmission time.

[0106] According to embodiments of this disclosure, a processor executing the ranging-assisted multi-ground robot collaborative mapping method provided in embodiments of this disclosure can receive pose transformation information (i.e., first pose transformation information and second pose transformation information), point cloud data (i.e., first point cloud data and second point cloud data), and distance data (i.e., first distance) measured by a UWB wireless ranging sensor from the master node robot and the slave node robot, respectively, and build a model based on the pose transformation information and the distance data measured by the UWB wireless ranging sensor.

[0107] According to embodiments of this disclosure, when the noise of the distance measured by the UWB wireless ranging sensor follows an approximately normal distribution, the distance measured by the UWB wireless ranging sensor can be modeled according to formula (3).

[0108]

[0109] In formula (3), i∈{1,...,N}, where The first distance measured by the UWB wireless ranging sensor is represented, k represents time k, i represents the i-th node robot in the distance measurement set, g represents the g-th UWB wireless ranging sensor on the i-th node robot, and h represents the h-th UWB wireless ranging sensor on the master node robot. This represents the position of the g-th UWB wireless ranging sensor of the i-th robot at time k in the world coordinate system W, where W represents the world coordinate system. The position of the h-th UWB wireless ranging sensor on the master node robot at time k is represented in the world coordinate system W, where 0 represents the position of the master node robot, and n represents the position of the h-th UWB wireless ranging sensor on the master node robot. r Characterizes distance noise.

[0110] In formula (3), The third pose transformation information to be solved at time k corresponding to the i-th node robot is represented. The second visual odometry coordinate system characterizing the i-th node robot. The position of the g-th ranging device of the i-th node robot relative to the second visual odometry coordinate system of the i-th slave node robot is represented. The third pose transformation information to be solved at time k, corresponding to the master node robot, is represented. The first visual odometry coordinate system characterizing the master node robot. The h-th ranging device of the master node robot is represented by its position relative to the first visual odometry coordinate system of the master node robot, and 0 represents the master node robot.

[0111] According to the embodiments of this disclosure, since the relative pose transformation between adjacent moments is independent of the coordinate system, the pose transformation in the world coordinate system can be represented by the relative pose of the second visual odometry coordinate system. The pose transformation in the world coordinate system can be modeled according to formula (4).

[0112]

[0113] Where, k represents the k-th time, j represents the j-th node robot in the odometer measurement set, and j is greater than or equal to 0. The pose transformation information of the robot at node j between time k and time k-1 is represented, where j is greater than or equal to 1. The second pose transformation information of the robot at node j in the second visual odometry coordinate system is represented when j equals 0. The first pose transformation information of the robot at node j in a visual odometry coordinate system is represented. The third pose transformation information at time k-1 corresponding to the robot at node j is represented by W, where W represents the world coordinate system. The visual odometry coordinate system representing the robot at node j. The third pose transformation information to be solved at time k corresponding to the j-th node robot is represented by no, and pose noise is represented by no.

[0114] According to embodiments of this disclosure, The visual odometry coordinate system representing the robot at node j, for example, when j equals 0. The first visual odometry coordinate system representing the 0th node robot (master node robot), when j is greater than or equal to 1, The second visual odometry coordinate system characterizing the robot at node j.

[0115] According to embodiments of this disclosure, the third pose transformation information to be solved is a variable used to characterize the third pose transformation information during the iterative process of solving the third pose transformation information at time k.

[0116] According to embodiments of this disclosure, the distance measurement set represents the set of slave node robots corresponding to the first distance at time k. The odometry measurement set represents the set of master node robots corresponding to the first pose transformation information and slave node robots corresponding to the second pose transformation information at time k.

[0117] According to embodiments of this disclosure, odometer measurements can be viewed as a first-order Markov process, with Gaussian noise assumed.

[0118] According to embodiments of this disclosure, the pose noise n o With distance noise n r All of them can be Gaussian noise and follow an approximately normal distribution.

[0119] According to the embodiments of this disclosure, it can be seen from formulas (3) and (4) that, and Given quantities and For unknown quantities, according to And the historical pose transformation information set can be used to estimate and

[0120] According to embodiments of this disclosure, for example, Figure 2 Operation S205, as shown, obtains the third pose transformation information at time k based on the first distance, the first pose transformation information, the historical pose transformation information set, and the second pose transformation information. This operation may include the following:

[0121] Based on the historical pose transformation information set, a first state vector and a second state vector are obtained, wherein the first state vector and the second state vector have the same dimension. The first state vector includes all pose transformation information included in the historical pose transformation information set, and the second state vector includes the third pose transformation information to be solved and the pose transformation information that is closer to the k-th time in the historical pose transformation information set.

[0122] Based on the first state vector and the second state vector, the prior residual is obtained;

[0123] Based on the first distance and the third pose transformation information to be solved, the distance residual is obtained;

[0124] Based on the first pose transformation information, the second pose transformation information, and the third pose transformation information to be solved, the pose residual is obtained;

[0125] The prior residual, distance residual, and pose residual are summed, and the third pose transformation information is obtained based on the summation result.

[0126] According to embodiments of this disclosure, a sliding window can be established to perform nonlinear optimization on the pose transformation information in the world coordinate system. Once the data is filled to a predefined window size M, the relative pose estimator begins to solve for the pose transformation information of all robots in the world coordinate system at time k.

[0127] According to embodiments of this disclosure, the historical pose transformation information set includes historical pose transformation information at least at a historical moment. The number of historical pose transformation information can be, for example, 1, 4, 7, etc. The number of historical pose transformations can be selected according to the actual situation. Embodiments of this disclosure do not limit the specific number of historical poses, and can be selected according to the actual situation.

[0128] According to an embodiment of this disclosure, the number of multiple historical moments corresponding to the historical pose transformation information set can be represented by M, where M is an integer greater than or equal to 1. The first state vector can be obtained by using the historical pose transformation information from moment kM to moment k-1 in the historical pose transformation information set, and the second state vector can be obtained by using the historical pose transformation information from moment k-M+1 to moment k-1 in the historical pose transformation information set and the third pose transformation information to be solved at moment k.

[0129] According to an embodiment of this disclosure, the second state vector at time k, which has been solved, can be represented by formula (5).

[0130]

[0131] in, The second state vector is represented by M, which represents M consecutive time points, and N represents the number of nodes of the robot. The third pose transformation information of the master node robot in the world coordinate system at time K is represented by k1, which represents time k-M+1, and so on. M Representing time k, The third pose transformation information of the master node robot at time k-M+1 is represented. The third pose transformation information of the master node robot at time k is represented. The third pose transformation information of the first slave node robot at time k-M+1 is represented. The third pose transformation information of the Nth slave node robot at time k is represented.

[0132] According to embodiments of this disclosure, a generalized subtraction operation is performed between the first state vector and the second state vector, and then the Mahalanobis quadratic norm is calculated, followed by the Huber norm, to obtain the prior residual.

[0133] According to an embodiment of this disclosure, a subtraction operation is performed between the first distance and the distance obtained from the third pose transformation information to be solved, and then the Mahalanobis second norm is solved to obtain the distance residual.

[0134] According to embodiments of this disclosure, based on the first pose transformation information and the second pose transformation information, the following is obtained: Then use Represented by the third pose transformation information to be solved (Refer to formula (4)) Perform relevant multiplication operations, then calculate the Mahalanobis quadratic norm to obtain the pose residual.

[0135] According to embodiments of this disclosure, the prior residual, distance residual, and pose residual are summed, and then the summation result is processed to minimize the Mahalanobis norm of the residual to obtain the maximum a posteriori estimate, which is the third pose transformation information.

[0136] According to embodiments of this disclosure, a first state vector and a second state vector are obtained based on a set of historical pose transformation information. Then, a priori residual is obtained based on the first state vector and the second state vector. A distance residual is obtained based on a first distance and the third pose transformation information to be solved. A pose residual is obtained based on the first pose transformation information, the second pose transformation information, and the third pose transformation information to be solved. The priori residual, the distance residual, and the pose residual are summed, and the third pose transformation information is obtained based on the summation result. This realizes the estimation of the third pose transformation information based on the set of historical pose transformation information, preparing for subsequent mapping using the third pose transformation information.

[0137] According to embodiments of this disclosure, a first state vector and a second state vector are obtained based on a set of historical pose transformation information. Then, a priori residual is obtained based on the first state vector and the second state vector. A distance residual is obtained based on a first distance and the third pose transformation information to be solved. A pose residual is obtained based on the first pose transformation information, the second pose transformation information, and the third pose transformation information to be solved. The priori residual, the distance residual, and the pose residual are summed, and the third pose transformation information is obtained based on the summation result. This achieves the use of historical pose transformation information in a tightly coupled set of historical pose transformation information, the first pose transformation information and the second pose transformation information obtained at time k, and the first distance to obtain the third pose transformation information, thereby improving the accuracy of the third pose transformation information and improving the positioning accuracy of each node robot at time k.

[0138] According to embodiments of this disclosure, the prior residual is obtained based on the first state vector and the second state vector, including calculation according to the following formula (a):

[0139]

[0140] in, Characterizing prior residuals, Representing the first state vector, The second state vector is represented, and k represents the k-th time. Characterizing the Maharanobis quadratic norm, The covariance of the prior residuals at time k is equal to the covariance of the pose at time k1. The operator θ represents the relationship between the prior residuals and the pose. and Perform generalized subtraction and logarithmic mapping on the tangent vectors corresponding to the quaternion variables in the subtraction result. k1 represents the first moment in the second state vector.

[0141] According to embodiments of this disclosure, a priori residual It carries the results of the last update.

[0142] According to embodiments of this disclosure, the distance residual is obtained based on the first distance and the third pose transformation information to be solved, including calculation according to the following formula (ii):

[0143]

[0144] in, Characterizing distance residuals, The vector represents the first state, k represents the k-th time, and ρ{} represents the Hubel norm. Characterizing the Mahalanobis norm, Characterizing the variance of distance noise, Let represent the first distance, ... The third pose transformation information to be solved at time k corresponding to the i-th node robot is represented by W, where W represents the world coordinate system. The second visual odometry coordinate system characterizing the i-th node robot. The position of the g-th ranging device of the i-th node robot relative to the second visual odometry coordinate system of the i-th node robot is represented. The third pose transformation information to be solved at time k, corresponding to the master node robot, is represented. The first visual odometry coordinate system characterizing the master node robot. The h-th ranging device of the master node robot is represented by its position relative to the first visual odometry coordinate system of the master node robot, and 0 represents the master node robot.

[0145] According to embodiments of this disclosure, in the distance residual Using the Huber norm can reduce the impact of outliers.

[0146] According to embodiments of this disclosure, based on the first pose transformation information, the second pose transformation information, and the third pose transformation information to be solved, the pose residual is obtained by calculation according to the following formula (iii):

[0147]

[0148] in, Characterizing pose residuals, The first pose transformation information state vector is represented by k, and k represents the k-th time. Characterizing the Mahalanobis norm, the second norm, The variance of pose noise is represented by , and j represents the j-th node robot in the odometry measurement set, where j is greater than or equal to 0. The pose transformation information of the robot at node j between time k and time k-1 is represented, where j is greater than or equal to 1. The second pose transformation information of the robot at node j in the second visual odometry coordinate system is represented when j equals 0. The first pose transformation information of the robot at node j in a visual odometry coordinate system is represented. The third pose transformation information at time k-1 corresponding to the robot at node j is represented by W, where W represents the world coordinate system. The visual odometry coordinate system representing the robot at node j. The third pose transformation information to be solved at time k corresponding to the j-th node robot is represented.

[0149] According to embodiments of this disclosure, the prior residual, distance residual, and pose residual are summed, and the third pose transformation information is obtained based on the summation result, including calculation according to the following formula (iv):

[0150]

[0151] in, The function representing the solution to the second state vector. Representing the first state vector, Characterizing prior residuals, Characterizing distance residuals, Characterizing pose residuals, Let i represent the distance measurement set, where i represents the i-th node robot in the distance measurement set, and i is greater than or equal to 1. The sum of the distance residuals corresponding to each node robot in the distance measurement set represents the k-th time interval. Let j represent the set of odometer measurements, where j represents the j-th node robot in the set of odometer measurements, and j is greater than or equal to 0. The sum of the pose residuals corresponding to each node robot in the characterization and odometry set. The second state vector is obtained by minimizing the Mahalanobis norm of the residual.

[0152] According to the embodiments of this disclosure, formulas (i), (ii), and (iii) can be substituted into formula (iv), and then formula (iv) is processed by minimizing the Mahalanobis norm of the residual to obtain the second state vector, thereby obtaining the third pose transformation information included in the second state vector. Then, the pose transformation information in the second state vector can be used as a set of historical pose transformation information to calculate the second state vector at the next moment, and the third pose transformation information at the next moment can be calculated.

[0153] According to embodiments of this disclosure, after each round of optimization, the oldest state is marginalized in the window and provides prior information.

[0154] According to embodiments of this disclosure, the latest odometry measurements can be used to predict the robot's posture as initial values ​​for the optimizer.

[0155] According to embodiments of this disclosure, the result obtained at time k can be... Add the third pose transformation information at time k-1 to obtain the initial third pose transformation information at time k. Then, substitute the initial third pose transformation information into formula (4) and use the gradient descent method to iteratively solve the third pose transformation information. If the difference between the second state vectors solved in two adjacent solutions is less than the threshold, the final second state vector is obtained, and then the third pose transformation information is obtained. The threshold can be, for example, 0.001.

[0156] According to embodiments of this disclosure, the pose transformation information of each node robot at the first time point (initial time point) can be calculated using a trilateration algorithm.

[0157] According to the embodiments of this disclosure, formulas (i) to (iv) use tightly coupled historical pose transformation information, the first pose transformation information obtained at time k, the second pose transformation information, and the first distance to obtain the third pose transformation information, thereby improving the accuracy of the third pose transformation information and further improving the positioning accuracy of each node robot at time k.

[0158] According to embodiments of this disclosure, collaborative mapping based on first point cloud data, second point cloud data, and third pose transformation information includes:

[0159] The first and second point cloud data are transformed into the world coordinate system using the third pose transformation information to obtain the third and fourth point cloud data, thus completing the system mapping. The third point cloud data corresponds to the first point cloud data, and the fourth point cloud data corresponds to the second point cloud data.

[0160] According to embodiments of this disclosure, the first point cloud data can be converted to the world coordinate system using the third pose transformation information to obtain the third point cloud data, and the second point cloud data can be converted to the world coordinate system using the third pose transformation information to obtain the fourth point cloud data, thus completing the system mapping.

[0161] Figure 3 The schematic diagram illustrates the principle of a range-aided multi-ground robot collaborative mapping method according to an embodiment of the present disclosure.

[0162] like Figure 3 As shown, Figure 3 The system includes a central robot 301 as the master node robot, robot i302 as the i-th slave node robot, and robot j303 as the j-th node robot.

[0163] At time k, robot i302 acquires 2D and depth images using an RGB-D camera in step 3021, and then obtains all 3D point cloud data at time k based on the 2D and depth images. Then, in step 3022, visual odometry information is obtained using the RANSAC algorithm based on all 3D point cloud data, resulting in the second pose transformation information of robot i302 at time k. Next, in step 3023, the ground-corresponding normal vector is extracted from all 3D point cloud data, thus extracting the ground. In step 3024, based on the ground-corresponding normal vector, the 3D point cloud data containing ground-corresponding elements is filtered out from all 3D point cloud data, resulting in the second point cloud data, while simultaneously acquiring the visual odometry information from step 3023. In step 3025, the second point cloud data and second pose transformation information related to robot i302 are sent to the central robot 301.

[0164] At time k, robot j303 processes the image data obtained by robot j303 to obtain the second point cloud data and the second pose transformation information related to robot j303, and sends the second point cloud data and the second pose transformation information related to the j-th robot j303 to the central robot j01.

[0165] At time k, in step 3011, the central robot 301 acquires two-dimensional and depth images using an RGB-D camera, and then obtains all three-dimensional point cloud data at time k based on the two-dimensional and depth images. Then, in step 3012, based on all the three-dimensional point cloud data, the RANSAC algorithm is used to obtain visual odometry information, thus obtaining the first pose transformation information of the central robot 301 at time k. Next, in step 3013, based on all the three-dimensional point cloud data, the normal vector corresponding to the ground is extracted, and the ground is extracted.

[0166] In step 3014, the central robot acquires the first distance between the central robot 301 and robot i302, and the first distance between the central robot 301 and robot j302, measured by the UWB wireless ranging sensor at time k. Since at each time, any UWB wireless ranging sensor on the master node robot and any UWB wireless ranging sensor on the slave node robot can measure multiple first distances, it is necessary to remove the first distances with larger deviations from the multiple first distances, i.e., outlier removal in step 3015.

[0167] In step 3016, the central robot acquires the second point cloud data and second pose transformation information related to robot i302 and robot j303.

[0168] In step 3017, the central robot estimates the state of each node robot. Based on the first point cloud data and first pose transformation information related to the central robot 301, the second point cloud data and second pose transformation information related to robot i302, the second point cloud data and second pose transformation information related to robot j303, the first distance between the central robot 301 and robot i302, the first distance between the central robot 301 and robot j302, and multiple historical pose transformation information, the central robot obtains the third pose transformation information of the central robot 301, robot i302, and robot j302 in the world coordinate system at time k.

[0169] In step 3018, the central robot transforms the first point cloud data corresponding to the central robot 301 into the world coordinate system based on the third pose transformation information of the central robot 301 in the world coordinate system and the first point cloud data corresponding to the central robot 301, thereby obtaining the third point cloud data corresponding to the central robot 301 in the world coordinate system.

[0170] In step 3018, the central robot transforms the second point cloud data corresponding to robot i302 into the world coordinate system based on the third pose transformation information of robot i302 in the world coordinate system and the second point cloud data corresponding to robot i302, thereby obtaining the fourth point cloud data corresponding to robot i301 in the world coordinate system.

[0171] In step 3018, the central robot transforms the second point cloud data corresponding to robot j303 into the world coordinate system based on the third pose transformation information of robot j303 in the world coordinate system and the second point cloud data corresponding to robot j303, thereby obtaining the fourth point cloud data corresponding to robot j303 in the world coordinate system.

[0172] In step 3018, the central robot obtains the third point cloud data in the world coordinate system corresponding to the central robot 301, the fourth point cloud data in the world coordinate system corresponding to robot i301, and the fourth point cloud data in the world coordinate system corresponding to robot j303, thus completing the mapping.

[0173] In step 3018, the central robot can voxelize the third and fourth point cloud data obtained in step 3018, converting the measured scene into voxels within individual grids, which facilitates subsequent use of the voxelized scene data for further region division and pathfinding.

[0174] according to Figure 3 As can be seen, the distance-assisted multi-ground robot collaborative mapping method provided in this disclosure can achieve the technical effect of collaborative mapping among multiple robots in the world coordinate system at time k, without external positioning systems such as GNSS signals and motion capture systems, and without visual closed loops. This is achieved by using historical pose transformation information set, the first distance between the master node robot and the slave node robot at time k, and the pose transformation information of multiple robots in the visual odometry coordinate system at time k. This results in obtaining the 3D point cloud data of multiple robots in the world coordinate system measured at time k.

[0175] Figure 4A The illustration schematically depicts an application scenario of a range-aided multi-ground robot collaborative mapping method according to another embodiment of the present disclosure.

[0176] like Figure 4A As shown, Figure 4A It includes three node robots: a master node robot 401, a first slave node robot 402, and a second slave node robot 403. The master node robot 401 is equipped with a processor that executes the range-assisted multi-ground robot collaborative mapping method provided in the embodiments of this disclosure.

[0177] Figure 4B A schematic diagram of a master node robot according to an embodiment of the present disclosure is shown.

[0178] Figure 4A An enlarged view of the master node robot 401 is shown below. Figure 4B As shown. Figure 4B The master node robot 401 is equipped with four UWB wireless ranging sensors 4011 (Nooploop UWB), an embedded computer 4012 (Nvidia Jetson AGX), and an RGB-D camera 4013 (Intel RealSense D435i RGB-D). The single-robot visual odometry uses ORB-SLAM3 in RGB-D mode. The maximum output frequency of the ultra-wideband is 50Hz.

[0179] Figure 4C A schematic diagram of a slave node robot according to an embodiment of the present disclosure is shown.

[0180] Figure 4A An enlarged view of the slave node robot 402 is shown below. Figure 4C As shown. Figure 4C The slave node robot 402 is equipped with two UWB wireless ranging sensors 4021 (Nooploop UWB model) and an RGB-D camera 4022 (IntelRealSense D435iRGB-D model). The single-robot visual odometry uses ORB-SLAM3 RGB-D mode. The maximum output frequency of the ultra-wideband is 50Hz.

[0181] Figure 5 The diagram illustrates the effect of mapping using the range-aided multi-ground robot collaborative mapping method according to embodiments of the present disclosure.

[0182] Using the multiple node robots shown in Figure 4, a map is constructed according to the range-aided multi-ground robot collaborative mapping method provided in this embodiment of the present disclosure, resulting in... Figure 5 The rendering of the map.

[0183] Figure 5 The region representation in (a) uses the master node robot 401 and slave node robot 402 in Figure 4 to perform collaborative mapping of indoor dense point clouds according to the range-assisted multi-ground robot collaborative mapping method of this disclosure.

[0184] Figure 5 Region (c) in Figure 4 is represented using the master node robot 401 and slave node robot 402, according to the range-aided multi-ground robot collaborative mapping method of this disclosure. Figure 5The image shows the effect of collaborative mapping of the indoor dense point cloud in region (a) after filtering the ground.

[0185] Figure 5 The region representation in (b) uses the master node robot 401 and slave node robot 402 in Figure 4 to perform collaborative mapping of outdoor dense point clouds according to the range-assisted multi-ground robot collaborative mapping method of this disclosure.

[0186] Figure 5 The region (d) in Figure 4 is represented using the master node robot 401 and slave node robot 402, according to the range-aided multi-ground robot collaborative mapping method of this disclosure. Figure 5 The image shows the effect of collaborative mapping of outdoor dense point clouds in region (b) after filtering the ground.

[0187] Figure 5 The lines in regions (a), (b), (c), and (d) represent the trajectory of the nodal robot, and the local coordinate axes represent the posture of the nodal robot.

[0188] according to Figure 5 As can be seen, the distance-assisted multi-ground robot collaborative mapping method provided in this disclosure can achieve collaborative mapping of dense point clouds collected by multiple node robots indoors or outdoors without an external positioning system or a visual loop closure detection method. It can also perform collaborative mapping after filtering out the ground from the dense point clouds collected by multiple node robots indoors or outdoors. It can be widely applied to indoor and outdoor collaborative mapping tasks.

[0189] It should be noted that, unless it is explicitly stated that there is a sequential order of execution between different operations, or that there is a sequential order of execution between different operations in terms of technical implementation, the execution order between multiple operations may not be significant, and multiple operations may be executed simultaneously.

[0190] Figure 6 A block diagram of a range-aided multi-ground robot collaborative mapping apparatus according to an embodiment of the present disclosure is shown schematically.

[0191] like Figure 6 As shown, the distance-assisted multi-ground robot collaborative mapping device 600 includes a first data and first information acquisition module 610, a first distance acquisition module 620, a historical information set acquisition module 630, a second data and second information receiving module 640, a third information acquisition module 650, and a mapping module 660.

[0192] The first data and first information acquisition module 610 is used to acquire the first point cloud data and the first pose transformation information of the master node robot at time k. The first pose transformation information represents the pose transformation information of the master node robot in the first visual odometry coordinate system between time k and time k-1.

[0193] The first distance acquisition module 620 is used to acquire the first distance between the master node robot and the slave node robot.

[0194] The historical information set acquisition module 630 is used to acquire a first preset number of historical pose transformation information sets, wherein the historical pose transformation information set represents the pose transformation information of the master node robot in the world coordinate system at each historical moment and the pose transformation information of the slave node robot in the world coordinate system at each historical moment.

[0195] The second data and second information receiving module 640 is used to receive the second point cloud data and the second pose transformation information of the slave node robot at time k. The second pose transformation information represents the pose transformation information of the slave node robot in the second visual odometry coordinate system between time k and time k-1.

[0196] The third information acquisition module 650 is used to obtain the third pose transformation information at time k based on the first distance, the first pose transformation information, the historical pose transformation information set, and the second pose transformation information. The third pose transformation information represents the pose transformation information of the master node robot at time k in the world coordinate system and the pose transformation information of the slave node robot at time k in the world coordinate system.

[0197] The mapping module 660 is used for collaborative mapping based on the first point cloud data, the second point cloud data, and the third pose transformation information.

[0198] According to embodiments of this disclosure, the third information obtaining module includes a first vector and a second vector obtaining submodule, a priori residual obtaining submodule, a distance residual obtaining submodule, a pose residual obtaining submodule, and a third information obtaining submodule.

[0199] The first and second vector acquisition submodule is used to obtain the first state vector and the second state vector based on the historical pose transformation information set. The first state vector and the second state vector have the same dimension. The first state vector includes all pose transformation information included in the historical pose transformation information set, and the second state vector includes the third pose transformation information to be solved and the pose transformation information that is closer to the k-th time in the historical pose transformation information set.

[0200] The prior residual acquisition submodule is used to obtain the prior residual based on the first state vector and the second state vector.

[0201] The distance residual acquisition submodule is used to obtain the distance residual based on the first distance and the third pose transformation information to be solved.

[0202] The pose residual acquisition submodule is used to obtain the pose residual based on the first pose transformation information, the second pose transformation information, and the third pose transformation information to be solved.

[0203] The third information acquisition submodule is used to sum the prior residual, distance residual, and pose residual, and obtain the third pose transformation information based on the summation result.

[0204] According to embodiments of this disclosure, the prior residual acquisition submodule includes a first calculation unit.

[0205] The first calculation unit is used to calculate according to the following formula (I):

[0206]

[0207] in, Characterizing prior residuals, Representing the first state vector, The second state vector is represented, and k represents the k-th time. Characterizing the Maharanobis quadratic norm, The covariance of the prior residuals at time k is equal to the covariance of the pose at time k1. The operator θ represents the relationship between the prior residuals and the pose. and Perform generalized subtraction and logarithmic mapping on the tangent vectors corresponding to the quaternion variables in the subtraction result. k1 represents the first moment in the second state vector.

[0208] According to embodiments of this disclosure, the distance residual obtaining submodule includes a second calculation unit.

[0209] The second calculation unit is used to calculate according to the following formula (II):

[0210]

[0211] in, Characterizing distance residuals, The vector represents the first state, k represents the k-th time, and ρ{} represents the Hubel norm. Characterizing the Mahalanobis norm, Characterizing the variance of distance noise, Let represent the first distance, ... The third pose transformation information to be solved at time k corresponding to the i-th node robot is represented by W, where W represents the world coordinate system. The second visual odometry coordinate system characterizing the i-th node robot. The position of the g-th ranging device of the i-th node robot relative to the second visual odometry coordinate system of the i-th node robot is represented. The third pose transformation information to be solved at time k, corresponding to the master node robot, is represented. The first visual odometry coordinate system characterizing the master node robot. The h-th ranging device of the master node robot is represented by its position relative to the first visual odometry coordinate system of the master node robot, and 0 represents the master node robot.

[0212] According to embodiments of this disclosure, the pose residual acquisition submodule includes a third calculation unit.

[0213] The third calculation unit is used to calculate according to the following formula (iii):

[0214]

[0215] in, Characterizing pose residuals, The first pose transformation information state vector is represented by k, and k represents the k-th time. Characterizing the Mahalanobis norm, the second norm, The variance of pose noise is represented by , and j represents the j-th node robot in the odometry measurement set, where j is greater than or equal to 0. The pose transformation information of the robot at node j between time k and time k-1 is represented, where j is greater than or equal to 1. The second pose transformation information of the robot at node j in the second visual odometry coordinate system is represented when j equals 0. The first pose transformation information of the robot at node j in a visual odometry coordinate system is represented. The third pose transformation information at time k-1 corresponding to the robot at node j is represented by W, where W represents the world coordinate system. The visual odometry coordinate system representing the robot at node j. The third pose transformation information to be solved at time k corresponding to the j-th node robot is represented.

[0216] According to embodiments of this disclosure, the third information obtaining submodule includes a fourth calculation unit.

[0217] The fourth calculation unit is used to calculate according to the following formula (iv):

[0218]

[0219] in, The function representing the solution to the second state vector. Representing the first state vector, Characterizing prior residuals, Characterizing distance residuals, Characterizing pose residuals, Let i represent the distance measurement set, where i represents the i-th node robot in the distance measurement set, and i is greater than or equal to 1. The sum of the distance residuals corresponding to each node robot in the distance measurement set represents the k-th time interval. Let j represent the set of odometer measurements, where j represents the j-th node robot in the set of odometer measurements, and j is greater than or equal to 0. The sum of the pose residuals corresponding to each node robot in the characterization and odometry set. The second state vector is obtained by minimizing the Mahalanobis norm of the residual.

[0220] According to embodiments of this disclosure, the second point cloud data is obtained by the slave node robot performing the following operations:

[0221] Acquire the third point cloud information of the slave node robot at time k. The third point cloud information represents all the three-dimensional point cloud data acquired by the slave node robot at time k.

[0222] Based on the third point cloud information, the ground normal vector included in the third point cloud information is obtained using the random sampling consensus algorithm.

[0223] Based on the normal vector, the 3D point cloud data corresponding to the ground in the third point cloud information is filtered out to obtain the second point cloud data.

[0224] According to embodiments of this disclosure, the mapping module includes a mapping submodule.

[0225] The mapping submodule is used to transform the first point cloud data and the second point cloud data into the world coordinate system using the third pose transformation information, so as to obtain the third point cloud data and the fourth point cloud data, and complete the system mapping. The third point cloud data corresponds to the first point cloud data, and the fourth point cloud data corresponds to the second point cloud data.

[0226] Any one or more of the modules, submodules, units, and subunits according to embodiments of the present disclosure, or at least part of the functions of any one or more of them, can be implemented in one module. Any one or more of the modules, submodules, units, and subunits according to embodiments of the present disclosure can be implemented by dividing them into multiple modules. Any one or more of the modules, submodules, units, and subunits according to embodiments of the present disclosure can be at least partially implemented as hardware circuitry, such as Field Programmable Gate Arrays (FPGAs), Programmable Logic Arrays (PLAs), Systems-on-Chip, Systems-on-Substrate, Systems-on-Package, Application-Specific Integrated Circuits (ASICs), or implemented in hardware or firmware by any other reasonable means of integrating or packaging circuitry, or implemented in software, hardware, or firmware, or in any suitable combination of any of these three implementation methods. Alternatively, one or more of the modules, submodules, units, and subunits according to embodiments of the present disclosure can be at least partially implemented as computer program modules, which, when run, can perform corresponding functions.

[0227] It should be noted that the distance-aided multi-terrain robot collaborative mapping device part in the embodiments of this disclosure corresponds to the distance-aided multi-terrain robot collaborative mapping method part in the embodiments of this disclosure. For a detailed description of the distance-aided multi-terrain robot collaborative mapping device part, please refer to the distance-aided multi-terrain robot collaborative mapping method part, which will not be repeated here.

[0228] Figure 7 A block diagram of a computer system suitable for implementing the methods described above, according to embodiments of the present disclosure, is illustrated schematically. Figure 7 The computer system shown is merely an example and should not be construed as limiting the functionality and scope of the embodiments disclosed herein.

[0229] like Figure 7 As shown, a computer system 700 according to an embodiment of this disclosure includes a processor 701, which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 702 or a program loaded from a storage portion 708 into a random access memory (RAM) 703. The processor 701 may include, for example, a general-purpose microprocessor (e.g., a CPU), an instruction set processor and / or an associated chipset and / or a special-purpose microprocessor (e.g., an application-specific integrated circuit (ASIC)), etc. The processor 701 may also include onboard memory for caching purposes. The processor 701 may include a single processing unit or multiple processing units for performing different actions of the method flow according to an embodiment of this disclosure.

[0230] RAM 703 stores various programs and data required for the operation of system 700. Processor 701, ROM 702, and RAM 703 are interconnected via bus 704. Processor 701 performs various operations of the method flow according to embodiments of the present disclosure by executing programs in ROM 702 and / or RAM 703. It should be noted that the programs may also be stored in one or more memories other than ROM 702 and RAM 703. Processor 701 may also perform various operations of the method flow according to embodiments of the present disclosure by executing programs stored in said one or more memories.

[0231] According to embodiments of this disclosure, system 700 may further include an input / output (I / O) interface 705, which is also connected to bus 704. System 700 may also include one or more of the following components connected to I / O interface 705: an input section 706 including a keyboard, mouse, etc.; an output section 707 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and a speaker, etc.; a storage section 708 including a hard disk, etc.; and a communication section 709 including a network interface card such as a LAN card, modem, etc. Communication section 709 performs communication processing via a network such as the Internet. Drive 710 is also connected to I / O interface 705 as needed. Removable media 711, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., are installed on drive 710 as needed so that computer programs read from there are installed into storage section 708 as needed.

[0232] Those skilled in the art will understand that the features described in the various embodiments and / or claims of this disclosure can be combined and / or combined in various ways, even if such combinations or combinations are not explicitly described in this disclosure. In particular, the features described in the various embodiments and / or claims of this disclosure can be combined and / or combined in various ways without departing from the spirit and teachings of this disclosure. All such combinations and / or combinations fall within the scope of this disclosure.

[0233] The embodiments of this disclosure have been described above. However, these embodiments are for illustrative purposes only and are not intended to limit the scope of this disclosure. Although various embodiments have been described above, this does not mean that the measures in the various embodiments cannot be used advantageously in combination. The scope of this disclosure is defined by the appended claims and their equivalents. Various substitutions and modifications can be made by those skilled in the art without departing from the scope of this disclosure, and all such substitutions and modifications should fall within the scope of this disclosure.

Claims

1. A method for collaborative mapping of a multi-ground robot assisted by range finding, comprising: obtaining first point cloud data and first pose transformation information of a master robot at a kth time, wherein the first pose transformation information represents pose transformation information of the master robot between the kth time and a (k-1)th time in a first visual odometry coordinate system; obtaining a first distance between the master robot and a slave robot; obtaining a first set of historical pose transformation information of a first preset number of historical times, wherein the first set of historical pose transformation information represents a set of pose transformation information of the master robot at each of the historical times in a world coordinate system and pose transformation information of the slave robot at each of the historical times in the world coordinate system; receiving second point cloud data and second pose transformation information of the slave robot at the kth time, wherein the second pose transformation information represents second pose transformation information of the slave robot between the kth time and the (k-1)th time in a second visual odometry coordinate system; obtaining third pose transformation information at the kth time according to the first distance, the first pose transformation information, the first set of historical pose transformation information, and the second pose transformation information, comprising: obtaining a first state vector and a second state vector according to the first set of historical pose transformation information, wherein the first state vector and the second state vector are consistent in dimension, the first state vector includes all pose transformation information included in the first set of historical pose transformation information, and the second state vector includes the third pose transformation information to be solved and pose transformation information in the first set of historical pose transformation information that is closest to the kth time; obtaining a prior residual according to the first state vector and the second state vector; obtaining a distance residual according to the first distance and the third pose transformation information to be solved; obtaining a pose residual according to the first pose transformation information, the second pose transformation information, and the third pose transformation information to be solved; and adding the prior residual, the distance residual, and the pose residual to obtain the third pose transformation information according to an addition result, wherein the third pose transformation information represents pose transformation information of the master robot at the kth time in the world coordinate system and pose transformation information of the slave robot at the kth time in the world coordinate system; converting the first point cloud data and the second point cloud data into the world coordinate system by using the third pose transformation information to obtain third point cloud data and fourth point cloud data, and completing system mapping, wherein the third point cloud data corresponds to the first point cloud data, and the fourth point cloud data corresponds to the second point cloud data.

2. The method of claim 1, wherein, obtaining the prior residual according to the first state vector and the second state vector comprises calculating according to the following formula (I): (I) wherein, characterizing the a priori residual, characterizing the first state vector, characterizing the second state vector, k characterizing the kth time, characterizing the Mahalanobis quadratic norm, characterizing that the covariance of the a priori residual at kth time equals the covariance of the pose at k1th time, the operator characterizing that and performing a generalized subtraction operation, and performing a logarithmic mapping on the tangent vectors corresponding to the quaternion variables in the subtraction operation result, k1 characterizing the first time in the second state vector.

3. The method of claim 1, wherein, obtaining the distance residual according to the first distance and the third pose transformation information to be solved comprises calculating according to the following formula (II): (two) wherein, characterizing the distance residual, characterizing the first state vector, k characterizing the kth moment, characterizing the Huber norm, characterizing the Mahalanobis norm, characterizing the distance noise variance, characterizing the first distance, i characterizing the ith node robot in the distance measurement set, i is greater than or equal to 1, g characterizing the gth ranging device on the ith node robot, h characterizing the hth ranging device on the master node robot, G characterizing the total number of ranging devices on the ith node robot, G is greater than or equal to 2, H characterizing the total number of ranging devices on the master node robot, H is greater than or equal to 4, characterizing the third pose transformation information corresponding to the ith node robot at the kth moment to be solved, W characterizing the world coordinate system, characterizing the second visual odometry coordinate system of the ith node robot, characterizing the position of the gth ranging device of the ith node robot relative to the second visual odometry coordinate system of the ith node robot, characterizing the third pose transformation information corresponding to the master node robot at the kth moment to be solved, characterizing the first visual odometry coordinate system of the master node robot, characterizing the position of the hth ranging device of the master node robot relative to the first visual odometry coordinate system of the master node robot, 0 characterizing the master node robot.

4. The method of claim 1, wherein, The third pose transformation information is obtained according to the first pose transformation information, the second pose transformation information and the to-be-solved third pose transformation information, and includes calculation according to the following formula (three): (three) wherein, characterizing the pose residual, characterizing the first pose transformation information state vector, k characterizing the kth time, characterizing the Mahalanobis norm quadratic norm, characterizing the pose noise variance, j characterizing the jth node robot in the set of odometry measurements, j being greater than or equal to 0, characterizing the pose transformation information of the jth node robot at the kth time and the k-1th time, wherein, in the case that j is greater than or equal to 1, characterizing the second pose transformation information of the jth node robot in the second visual odometry coordinate system, in the case that j is equal to 0, characterizing the first pose transformation information of the jth node robot in the visual odometry coordinate system, characterizing the third pose transformation information corresponding to the jth node robot at the k-1th time, W characterizing the world coordinate system, characterizing the visual odometry coordinate system of the jth node robot, characterizing the third pose transformation information corresponding to the jth node robot at the kth time to be solved.

5. The method of claim 1, wherein, The third pose transformation information is obtained according to the first pose transformation information, the second pose transformation information and the to-be-solved third pose transformation information, and includes calculation according to the following formula (three): (iv) wherein, characterizing a function of solving the second state vector, characterizing the first state vector, characterizing the prior residual, characterizing the distance residual, characterizing the pose residual, characterizing a set of distance measurements, i characterizing an i-th node robot in the set of distance measurements, i being greater than or equal to 1, characterizing a sum of the distance residuals corresponding to each node robot in the set of distance measurements, k characterizing a k-th time instant, characterizing a set of odometry measurements, j characterizing a j-th node robot in the set of odometry measurements, j being greater than or equal to 0, characterizing a sum of the pose residuals corresponding to each node robot in the set of odometry measurements, characterizing a second state vector by minimizing a Mahalanobis norm of the residuals.

6. The method of claim 1, wherein, The second point cloud data is obtained by the slave node robot performing the following operations: Obtain third point cloud information of the slave node robot at the kth moment, and the third point cloud information represents all three-dimensional point cloud data acquired by the slave node robot at the kth moment; According to the third point cloud information, the normal vector of the ground included in the third point cloud information is obtained by using a random sample consensus algorithm; According to the normal vector, the three-dimensional point cloud data corresponding to the ground in the third point cloud information is filtered out to obtain the second point cloud data.

7. A ranging-assisted multi-ground robot cooperative mapping device, comprising: A first data and first information acquisition module for acquiring first point cloud data and first pose transformation information of a master node robot at the kth moment, wherein the first pose transformation information represents the first visual odometry pose transformation information of the master node robot between the kth moment and the k-1th moment; A first distance acquisition module for acquiring a first distance between the master node robot and a slave node robot; A historical information set acquisition module for acquiring a first preset number of historical pose transformation information sets at historical moments, wherein the historical pose transformation information set represents a set of the pose transformation information of the master node robot in the world coordinate system at each historical moment and the pose transformation information of the slave node robot in the world coordinate system at each historical moment; A second data and second information receiving module for receiving second point cloud data and second pose transformation information of a slave node robot at the kth moment, wherein the second pose transformation information represents the second visual odometry pose transformation information of the slave node robot between the kth moment and the k-1th moment; A third information obtaining module for obtaining third pose transformation information at the kth moment according to the first distance, the first pose transformation information, the historical pose transformation information set and the second pose transformation information, wherein the third pose transformation information represents the pose transformation information of the master node robot in the world coordinate system at the kth moment and the pose transformation information of the slave node robot in the world coordinate system at the kth moment; The third information obtaining module includes a first vector and second vector obtaining submodule, a prior residual obtaining submodule, a distance residual obtaining submodule, a pose residual obtaining submodule and a third information obtaining submodule; The first vector and the second vector obtaining submodule is configured to obtain a first state vector and a second state vector according to the set of historical pose transformation information, wherein the first state vector and the second state vector are consistent in dimension, the first state vector includes all pose transformation information included in the set of historical pose transformation information, and the second state vector includes the third pose transformation information to be solved and the pose transformation information closest to the kth moment in the set of historical pose transformation information. The prior residual obtaining submodule is configured to obtain a prior residual according to the first state vector and the second state vector. The distance residual obtaining submodule is configured to obtain a distance residual according to the first distance and the third pose transformation information to be solved. The pose residual obtaining submodule is configured to obtain a pose residual according to the first pose transformation information, the second pose transformation information and the third pose transformation information to be solved. The third information obtaining submodule is configured to add the prior residual, the distance residual and the pose residual, and obtain the third pose transformation information according to an addition result. The mapping module is configured to cooperatively map according to the first point cloud data, the second point cloud data and the third pose transformation information. The mapping module includes a mapping submodule. The mapping submodule is configured to convert the first point cloud data and the second point cloud data into a world coordinate system by using the third pose transformation information to obtain third point cloud data and fourth point cloud data, and complete system mapping, wherein the third point cloud data corresponds to the first point cloud data, and the fourth point cloud data corresponds to the second point cloud data. 8.An electronic device, comprising: one or more processors; a memory for storing one or more instructions, wherein when the one or more instructions are executed by the one or more processors, the one or more processors implement the method of any one of claims 1 to 6.

Images