Coordinated positioning method and apparatus, electronic device, and computer program product

Abstract

The application is suitable for the technical field of robots, and particularly relates to a cooperative positioning method and device, electronic equipment and a computer program product. In the method, the first electronic equipment can accurately determine a pose deviation corresponding to the first electronic equipment according to a first pose of the first electronic equipment in a local coordinate system, a second pose corresponding to a second electronic equipment, and a first distance between the first electronic equipment and the second electronic equipment, so that the pose of the first electronic equipment can be optimized according to the pose deviation to obtain a target pose corresponding to the first electronic equipment. That is, the first electronic equipment can accurately determine the pose of the first electronic equipment in a global coordinate system without the need for external equipment, and the accuracy and stability of positioning of the first electronic equipment can be improved.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of robots, and particularly relates to a cooperative positioning method and device, an electronic device, and a computer program product. BACKGROUND

[0002] At present, multi-robot systems are widely used in many fields, such as services, dangerous environment exploration, disaster site search and rescue, and formation flight. A multi-robot system can complete a task through cooperation of multiple robots. When a multi-robot system completes a task through cooperation of multiple robots, the poses of the robots in the multi-robot system need to be accurately determined. With the development of simultaneous localization and mapping (SLAM) technology, a single robot can be positioned through multi-sensor fusion technology (such as visual-inertial odometry and laser radar odometry), and the positioning information can be transmitted to other robots through networking between the robots to achieve cooperation of multiple robots to complete a task. However, the positioning error of a single robot will increase with the accumulation of time. In a multi-robot system, this problem is particularly serious. Due to the accumulated error of each robot, the positioning accuracy of the robot is poor, which can easily lead to failure of the task of the multi-robot system. SUMMARY

[0003] Embodiments of the present application provide a cooperative positioning method, device, electronic device, and computer program product, which can improve the positioning accuracy of each robot in a multi-robot system and reduce the possibility of failure of the task of the multi-robot system.

[0004] In a first aspect, embodiments of the present application provide a cooperative positioning method applied to a first electronic device, and the method comprises:

[0005] obtaining a first pose, a second pose, and a first distance; the first pose is a pose of the first electronic device in a local coordinate system of the first electronic device at a first time; the second pose is a corresponding pose of a second electronic device at the first time; and the first distance is a distance between the first electronic device and the second electronic device at the first time;

[0006] determining a pose deviation corresponding to the first electronic device according to the first pose, the second pose, and the first distance;

[0007] optimizing the first pose according to the pose deviation to obtain a target pose corresponding to the first electronic device.

[0008] In the cooperative localization method described above, the first electronic device can accurately determine its pose deviation based on its first pose in the local coordinate system, the second pose of the second electronic device, and the first distance between them. This allows for optimization of the pose of the first electronic device based on the pose deviation, resulting in the target pose. In other words, the first electronic device can accurately determine its pose in the global coordinate system without the need for external devices, thus improving the accuracy and stability of its localization.

[0009] In some embodiments, determining the pose deviation corresponding to the first electronic device based on the first pose, the second pose, and the first distance includes:

[0010] Based on the second pose and the first distance, determine the first initial pose corresponding to the first electronic device;

[0011] Based on the first initial pose and the first pose, determine the pose deviation corresponding to the first electronic device.

[0012] In other embodiments, the method further includes:

[0013] The system acquires a third pose, a fourth pose, a second distance, a first displacement, and a second displacement; the third pose is the pose of the first electronic device in its local coordinate system at a second time moment; the fourth pose is the pose of the second electronic device at the second time moment; the second distance is the distance between the first and second electronic devices at the second time moment; the first displacement is the displacement of the first electronic device at the first time moment; the second displacement is the displacement of the first electronic device at the second time moment; the second time moment is earlier than the first time moment.

[0014] The step of determining the pose deviation corresponding to the first electronic device based on the first pose, the second pose, and the first distance includes:

[0015] Based on the first pose, the second pose, the third pose, the fourth pose, the first distance, the second distance, the first displacement, and the second displacement, the pose deviation corresponding to the first electronic device is determined.

[0016] In one embodiment, determining the pose deviation corresponding to the first electronic device based on the first pose, the second pose, the third pose, the fourth pose, the first distance, the second distance, the first displacement, and the second displacement includes:

[0017] Based on the second pose, the fourth pose, the first distance, the second distance, the first displacement, and the second displacement, determine the first initial pose and the second initial pose corresponding to the first electronic device;

[0018] Based on the first initial pose, the second initial pose, the first pose, and the third pose, the pose deviation corresponding to the first electronic device is determined.

[0019] For example, determining the first initial pose and the second initial pose corresponding to the first electronic device based on the second pose, the fourth pose, the first distance, the second distance, the first displacement, and the second displacement includes:

[0020] By minimizing the first optimization objective, the first initial pose and the second initial pose corresponding to the first electronic device are obtained; the first optimization objective is:

[0021]

[0022] Where m is the number of the second electronic devices, and n is the number of times the second pose or the fourth pose is acquired. Let w be the first initial pose or the second initial pose of the first electronic device. Let p be the first displacement or second displacement corresponding to the first electronic device at time w. k (w) represents the second or fourth pose of the k-th second electronic device at time w. Let w be the first distance or the second distance between the first electronic device and the kth second electronic device.

[0023] For example, determining the pose deviation corresponding to the first electronic device based on the first initial pose, the second initial pose, the first pose, and the third pose includes:

[0024] The pose deviation of the first electronic device is obtained by minimizing the second optimization objective; the pose deviation includes rotation deviation and translation deviation.

[0025] The second optimization objective is:

[0026]

[0027] Where n is the number of times the second pose or the fourth pose is obtained. The rotational deviation, p is the translational deviation. (S-w)Let w be the first or third pose of the first electronic device. Let w be the first initial pose or the second initial pose of the first electronic device.

[0028] For example, determining the pose deviation corresponding to the first electronic device based on the first pose, the second pose, the third pose, the fourth pose, the first distance, the second distance, the first displacement, and the second displacement includes:

[0029] The pose deviation of the first electronic device is obtained by minimizing the third optimization objective; the pose deviation includes rotation deviation and translation deviation.

[0030] The third optimization objective is:

[0031]

[0032] Where m is the number of the second electronic devices, and n is the number of times the second pose or the fourth pose is acquired. For rotation matrix, p is the translational deviation. (S-w) Let w be the first or third pose of the first electronic device. Let p be the first displacement or second displacement corresponding to the first electronic device at time w. k (w) represents the second or fourth pose corresponding to the k-th second electronic device at time w. Let w be the first distance or the second distance between the first electronic device and the kth second electronic device.

[0033] In some embodiments, the first electronic device includes a first ultra-wideband UWB module, and the second electronic device includes a second UWB module; the acquisition of the first pose, the second pose, and the first distance includes:

[0034] The second pose broadcast by the second UWB module is obtained through the first UWB module;

[0035] The first distance is obtained through the first UWB module.

[0036] Secondly, embodiments of this application provide a cooperative positioning device applied to a first electronic device, the device comprising:

[0037] The data acquisition module is used to acquire a first pose, a second pose, and a first distance; the first pose is the pose of the first electronic device in the local coordinate system of the first electronic device at a first moment; the second pose is the pose of the second electronic device at the first moment; and the first distance is the distance between the first electronic device and the second electronic device at the first moment.

[0038] The pose deviation acquisition module is used to determine the pose deviation corresponding to the first electronic device based on the first pose, the second pose, and the first distance.

[0039] The pose optimization module is used to optimize the first pose based on the pose deviation to obtain the target pose corresponding to the first electronic device.

[0040] Thirdly, embodiments of this application provide an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it causes the electronic device to implement the cooperative positioning method described in any one of the first aspects above.

[0041] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program that, when executed by a computer, causes the computer to perform the cooperative positioning method described in any one of the first aspects.

[0042] Fifthly, embodiments of this application provide a computer program product, the computer program product including a computer program, which, when executed by an electronic device, causes the electronic device to implement the cooperative positioning method described in any one of the first aspects above.

[0043] It is understood that the beneficial effects of the second to fifth aspects mentioned above can be found in the relevant descriptions in the first aspect mentioned above, and will not be repeated here. Attached Figure Description

[0044] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0045] Figure 1 This is a flowchart illustrating the collaborative positioning method provided in the embodiments of this application. Figure 1 ;

[0046] Figure 2This is a flowchart illustrating the collaborative positioning method provided in the embodiments of this application. Figure 2 ;

[0047] Figure 3 This is a schematic diagram of the structure of the cooperative positioning device provided in the embodiments of this application;

[0048] Figure 4 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0049] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0050] It should be understood that, when used in this application specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or a collection thereof.

[0051] It should also be understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0052] As used in this application specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if detected [the described condition or event]" may be interpreted, depending on the context, as meaning "once determined," "in response to determination," "once detected [the described condition or event]," or "in response to detection [the described condition or event]."

[0053] Furthermore, in the description of this application and the appended claims, the terms "first," "second," "third," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0054] References to "one embodiment" or "some embodiments" as described in this specification mean that one or more embodiments of this application include a specific feature, structure, or characteristic described in connection with that embodiment. Therefore, the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in still other embodiments," etc., appearing in different parts of this specification do not necessarily refer to the same embodiment, but rather mean "one or more, but not all, embodiments," unless otherwise specifically emphasized. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless otherwise specifically emphasized.

[0055] Multi-robot systems are widely used in many fields, such as service, hazardous environment exploration, disaster site search and rescue, and formation flying. Multi-robot systems accomplish tasks through the cooperation of multiple robots. Compared to a single robot completing various tasks, coordinating a group of autonomous robots to achieve a common goal has many advantages, such as strong scalability, flexibility, and adaptability. Among these advantages, accurate positioning of each robot within the multi-robot system is crucial when multiple robots collaborate to complete tasks.

[0056] In multi-robot systems, a common localization method involves building a communication network. In this network, each robot knows its global pose within the system and broadcasts it to the other robots. Typically, robots determine their global pose using external localization methods, such as the Global Positioning System (GPS). While external localization methods offer high accuracy in indoor environments, they require the pre-deployment of bulky base stations and involve tedious and challenging deployment work. Furthermore, their reliance on GPS and the known environment hinders their application in GPS-restricted environments, such as in the field. This limits the flexibility of multi-robot systems in certain scenarios (e.g., field search and swarm formation), and interference or damage to the base stations can cause malfunctions in the entire system.

[0057] With the development of SLAM technology, individual robots can achieve localization through multi-sensor fusion technology (such as visual inertial odometry and lidar odometry), and transmit the localization information to other robots through multi-robot networking, enabling collaborative task completion. While SLAM technology can perform localization without external equipment support, fully utilizing the localization information of each robot to improve the overall system's accuracy and robustness, the localization error of a single robot accumulates over time. For example, during prolonged movement, the coordinate system of the SLAM system may deviate from its initial coordinates and orientation, leading to the accumulation of pose errors. This problem is particularly severe in multi-robot systems. For instance, if two robots are moving in a straight line, assuming their respective SLAM systems do not interfere with each other, they may move closer or further apart. In other words, in multi-robot systems, due to the accumulated errors of each robot, the risk of collision increases when robots are out of each other's line of sight, leading to task failure. Therefore, improving the accuracy of robot localization under GPS limitations is a pressing issue that needs to be addressed in this field.

[0058] To address the aforementioned problems, embodiments of this application provide a collaborative positioning method, apparatus, electronic device, and computer program product. In this method, a first electronic device can acquire a first pose, a second pose, and a first distance. The first pose is the pose of the first electronic device in its local coordinate system at a first moment; the second pose is the pose of the second electronic device at the first moment; and the first distance is the distance between the first and second electronic devices at the first moment. Subsequently, the first electronic device can determine its pose deviation based on the first pose, second pose, and first distance, and optimize the first pose based on the pose deviation to obtain the target pose of the first electronic device. That is, in this embodiment, the first electronic device can accurately determine its pose deviation based on its first pose in its local coordinate system, the second pose of other electronic devices, and the distance between the first electronic device and other electronic devices. This allows for optimization of the first pose based on the pose deviation to determine the target pose of the first electronic device. This method can accurately determine the target pose of the first electronic device without the need for external devices, improving the accuracy and stability of the first electronic device's positioning and offering strong usability and practicality.

[0059] In this embodiment, the first electronic device and the second electronic device can be electronic devices in the same distributed system, such as electronic devices in a distributed system where multiple electronic devices collaborate on a task. Both the first and second electronic devices can be robots, unmanned vehicles, drones, or unmanned boats—electronic devices capable of positioning and movement. This embodiment does not limit the specific type of electronic device. The following example illustrates the scenario where both the first and second electronic devices are robots in a multi-robot system.

[0060] The collaborative positioning method provided in the embodiments of this application will be described in detail below with reference to the accompanying drawings and specific application scenarios.

[0061] Please see Figure 1 , Figure 1 A flowchart illustrating the cooperative localization method provided in an embodiment of this application is shown. Figure 1 This method can be applied to any robot in a multi-robot system (the first robot will be used as an example below). Figure 1 As shown, the method may include:

[0062] S101, the first robot acquires the first pose, the second pose, and the first distance.

[0063] It should be noted that the first pose can be the pose of the first robot in its local coordinate system at the first moment. The second pose can be the pose of the second robot at the first moment. The first distance can be the distance between the first and second robots at the first moment. The first moment can be any moment. The second pose of the second robot can be the pose of the second robot in its local coordinate system, or it can be the pose of the second robot in its global coordinate system. The local coordinate system can refer to a coordinate system defined by each robot itself. The global coordinate system can refer to a coordinate system shared by all robots in a multi-robot system.

[0064] For example, for a second robot (e.g., second robot A), when second robot A has not undergone pose optimization, its second pose can be its pose in the local coordinate system of second robot A. After pose optimization, the second pose of second robot A can be its pose in the global coordinate system. Pose optimization can refer to the process of obtaining the target pose of the robot through the cooperative localization method described in this application embodiment.

[0065] In some embodiments, each robot in a multi-robot system can have a standalone localization function. For example, each robot in a multi-robot system can be localized using visual SLAM, laser SLAM, or visual inertial odometry (VIO) to obtain its pose in its local coordinate system.

[0066] In other embodiments, each robot in the multi-robot system can have ultra-wideband (UWB) ranging capabilities. For example, each robot can be equipped with a UWB module, which allows it to acquire the distance between itself and other robots within the UWB signal range. After acquiring its pose in its local coordinate system using visual SLAM, laser SLAM, or VIO, each robot in the multi-robot system can write this pose into its UWB module and broadcast it externally, allowing other robots to acquire the robot's pose based on the broadcast information. UWB technology utilizes ultra-wideband signals for data transmission, offering high precision and strong anti-interference capabilities. This embodiment uses UWB modules to acquire distances between robots and the poses of other robots, determining pose deviations based on the acquired distances and poses to optimize robot poses. This results in small amounts of interactive data, enabling lightweight data communication, lower performance requirements for the first robot, and allowing for high-precision positioning in indoor and complex environments.

[0067] In this embodiment, the first robot can obtain its first pose in its local coordinate system using visual SLAM, laser SLAM, or VIO. The second robot can also obtain its second pose in its local coordinate system using visual SLAM, laser SLAM, or VIO, and can broadcast this second pose to the outside world via its UWB module. The first robot can acquire the second pose broadcast by the second robot's UWB module. Furthermore, the first robot can also measure the distance (i.e., the first distance) between itself and the second robot using its UWB module.

[0068] It should be noted that the embodiments of this application do not limit the method by which the first robot measures the first distance between the first robot and the second robot through the UWB module, and the first distance between the first robot and the second robot can be measured according to existing methods.

[0069] In one embodiment, each robot in a multi-robot system can acquire the distance between itself and other robots, and the poses of the other robots, at a preset frequency. That is, the first robot can acquire the distance between itself and other robots, and the poses of the other robots, at a preset frequency. The preset frequencies for each robot can be the same or different, depending on the actual scenario. Furthermore, the preset frequencies for each robot can also be determined based on the actual scenario; this embodiment does not impose such limitations.

[0070] S102. The first robot determines the pose deviation corresponding to the first robot based on the first pose, the second pose, and the first distance.

[0071] It should be understood that pose deviations can include rotational deviations (Δyaw) and translational deviations (Δx, Δy, Δz).

[0072] The embodiments of this application determine the pose deviation by using the first pose, the second pose, and the first distance, so as to optimize the robot's pose based on the pose deviation, rather than directly optimizing the pose. This can reduce the impact of pose jumps on pose optimization and improve the accuracy and stability of pose optimization.

[0073] In some embodiments, when the number of second robots that the first robot can acquire in the second pose is greater than or equal to a preset number (e.g., a preset number A), that is, when the first robot can acquire a sufficient number of first distances and second poses corresponding to the second robots, the first robot can directly determine the pose deviation corresponding to the first electronic device based on the first pose, the second pose, and the first distance at the first moment. It should be understood that the preset number A can be specifically determined according to the actual scenario, and this application embodiment does not limit it in this regard.

[0074] For example, the first robot can determine its first initial pose based on the second pose of each of the second robots at the first moment and the first distance between the first robot and each of the second robots. That is, it can estimate the pose of the first robot in the global coordinate system at the first moment based on the second pose of each of the second robots at the first moment and the first distance between the first robot and each of the second robots. Subsequently, the first robot can determine its pose deviation based on its first initial pose and first pose at the first moment. Therefore, it can optimize its pose in the global coordinate system at the first moment based on the pose deviation. In other words, the first initial pose of the first robot can be the estimated pose of the first robot in the global coordinate system at the first moment.

[0075] In one embodiment, the first robot can predict its first initial pose using a least-squares optimization method. For example, the first initial pose can be predicted by constructing a residual function using a nonlinear optimization library such as CERES or G2O.

[0076] In one possible implementation, the first robot can estimate the first initial pose of the first robot by minimizing the sum of squares of the ranging errors, based on the first distance between the first robot and each of the second robots and the second pose of each of the second robots.

[0077] For example, the first robot can obtain its first initial pose by minimizing a certain optimization objective (e.g., optimization objective A). Optimization objective A can be:

[0078]

[0079] Where m is the number of the second robots. Let p be the first initial pose of the first robot at the first moment. k Let the second pose of the k-th second robot be given at the first moment. Let m be the first distance between the first robot and the k-th second robot at the first moment. It should be understood that the number m of the second robots at this time can refer to the number of second robots that the first robot can acquire the second pose.

[0080] For example, when minimizing the optimization objective A, if the pose optimization of the first robot is performed for the first time, it can be... The initial value is set to 0. If this is not the first time the pose optimization of the first robot has been performed, i.e., the pose optimization of the first robot has been performed before, in order to reduce the number of iterations and ensure the stability of the solution, the target pose obtained from the previous pose optimization can be used as... The initial value.

[0081] In one possible implementation, after determining the first initial pose of the first robot, the first robot can determine its pose deviation based on the first initial pose and the first pose. For example, the first robot can obtain its pose deviation by minimizing a certain optimization objective (e.g., optimization objective B). Optimization objective B can be:

[0082]

[0083] in, For rotational deviation, p represents the translational deviation. SThis represents the first pose of the first robot at the first moment. This represents the first initial pose of the first robot at the first moment.

[0084] In other embodiments, when the number of robots is limited, for example, when the number of second robots for which the first robot can acquire the second pose is less than a preset number (e.g., a preset number B), an extended sliding window can be used to determine the pose deviation of the first robot by utilizing historical data of the robots (e.g., the historical trajectory of the first robot, the historical distance between the first and second robots, and the historical trajectory of the second robot). This solves the problem of multiple solutions and no solutions in distributed localization, and meets the requirements of real-time performance and stability. That is, the limitation on the number of robots can be compensated for by the relative motion of an extended sliding window, enabling cooperative localization between two robots. The preset number B can be determined specifically according to the actual scenario, and this application embodiment does not impose any limitations on it.

[0085] In other words, when the number of robots is limited, the first robot can determine its pose deviation based on its pose, the pose of the second robot, the distance between them, and its displacement when relative motion exists. That is, the first moment can be the moment when relative motion exists between the first and second robots. The second moment, as described later, can also be the moment when relative motion exists between the first and second robots.

[0086] In one embodiment, the first robot may further acquire a third pose, a fourth pose, a second distance, a first displacement, and a second displacement, and determine the pose deviation of the first robot based on the first pose, second pose, third pose, fourth pose, first distance, second distance, first displacement, and second displacement. The third pose can be the pose of the first robot in its local coordinate system at the second time moment. The fourth pose can be the pose of the second robot at the second time moment. The second distance can be the distance between the first and second robots at the second time moment. The first displacement can be the displacement of the first robot at the first time moment. The second displacement can be the displacement of the first electronic device at the second time moment.

[0087] It should be noted that the second robot may include one or more. The second moment may be earlier than the first moment. The number of second moments can be determined based on the size of the sliding window. The size of the sliding window can be determined according to the actual scenario, and this application embodiment does not limit it. For example, the size of the sliding window can be any value such as 5, 6, or 8, depending on the actual scenario.

[0088] It should be understood that at a given moment, the displacement of the first robot can refer to the displacement moved by the first robot during the time interval from the previous moment to the present moment. For example, when the first moment is time T5, and the second moments include times T1, T2, T3, and T4, and time T1 is earlier than time T2, time T2 is earlier than time T3, time T3 is earlier than time T4, and time T4 is earlier than time T5, then at the first moment (e.g., time T5), the displacement of the first robot can refer to the displacement moved by the first robot during the time interval from time T4 to time T5. At a given second moment (e.g., time T3), the displacement of the first robot can refer to the displacement moved by the first robot during the time interval from time T2 to time T3.

[0089] As mentioned above, the first robot can acquire the distance between itself and other robots at a preset frequency, as well as the poses of the other robots. For example, at time T1, the first robot can acquire the distance between itself and robot B (e.g., distance B1), and the pose of robot B (e.g., pose B1); it can also acquire the distance between itself and robot C (e.g., distance C1), and the pose of robot C (e.g., pose C1); and it can acquire the distance between itself and robot D (e.g., distance D1), and the pose of robot D (e.g., pose D1), and so on.

[0090] Subsequently, the first robot can determine whether there is relative motion between the first robot and robot B at time T1 based on the poses of the first robot and robot B at times T1, as well as the poses of the first robot and robot B at times prior to T1. When it is determined that there is relative motion between the first robot and robot B at time T1, the first robot can timestamp the distance B1 and pose B1 and then write them into the sliding window corresponding to robot B within the first robot's memory.

[0091] Similarly, the first robot can determine whether there is relative motion between itself and robot C at time T1. If relative motion exists between the first robot and robot C at time T1, the first robot can timestamp the distance C1 and pose C1 and write them into the sliding window corresponding to robot C within the first robot's memory. The first robot can also determine whether there is relative motion between itself and robot D at time T1. If relative motion exists between the first robot and robot D at time T1, the first robot can timestamp the distance D1 and pose D1 and write them into the sliding window corresponding to robot D within the first robot's memory, and so on.

[0092] Suppose that at time T2, the first robot can obtain the distance between the first robot and robot B (e.g., distance B2), and the pose of robot B (e.g., pose B2), and can obtain the distance between the first robot and robot C (e.g., distance C2), and the pose of robot C (e.g., pose C2), and can obtain the distance between the first robot and robot D (e.g., distance D2), and the pose of robot D (e.g., pose D2), and so on.

[0093] Subsequently, the first robot can determine at time T2 whether there is relative motion between itself and robot B, between itself and robot C, and between itself and robot D, etc. When it is determined that there is relative motion between the first robot and robot B, the first robot can timestamp distance B2 and pose B2 and write them into the sliding window corresponding to robot B within the first robot's database. When it is determined that there is relative motion between the first robot and robot C, the first robot can timestamp distance C2 and pose C2 and write them into the sliding window corresponding to robot C within the first robot's database. Similarly, when it is determined that there is relative motion between the first robot and robot D, the first robot can timestamp distance D2 and pose D2 and write them into the sliding window corresponding to robot D within the first robot's database, and so on.

[0094] When the size of a sliding window in the first robot meets the requirements, the first robot can trigger the determination of pose deviation. For example, when the size of the sliding window is 5, if a sliding window in the first robot includes data at five time points (i.e., pose and distance at five time points), the first robot can trigger the determination of pose deviation to optimize the pose deviation of the first robot and improve the accuracy of the pose of the first robot.

[0095] In one possible implementation, the first robot can determine its first initial pose and second initial pose based on the second pose, fourth pose, first distance, second distance, first displacement, and second displacement. It can also determine its pose deviation based on the first initial pose, second initial pose, first pose, and third pose. The first initial pose can be the robot's estimated pose in the global coordinate system at a first moment. The second initial pose can be the robot's estimated pose in the global coordinate system at a second moment.

[0096] In one embodiment, the first robot can estimate its first and second initial poses using a least-squares optimization method. For example, the first robot can obtain the first and second initial poses of the first electronic device by minimizing a certain optimization objective (e.g., a first optimization objective). The first optimization objective can be:

[0097]

[0098] Where m is the number of second robots, and n is the number of times the second or fourth pose is obtained. Let w be the first initial pose or the second initial pose of the first robot. Let p be the first or second displacement of the first robot at time w. k (w) represents the second or fourth pose of the k-th second robot at time w. Let m be the first or second distance between the first robot and the k-th second robot at time w. It should be understood that the number m of the second robots at this time can refer to the number of second robots for which the first robot can acquire n poses (i.e., second and fourth poses). For example, at the first time, assuming the first robot has acquired 1 second pose and (n-) fourth poses corresponding to robot B, 1 second pose and (n-1) fourth poses corresponding to robot C, and 1 second pose and (n-1) fourth poses corresponding to robot D, then the number m of the second robots can be 3.

[0099] In one embodiment, after obtaining the first initial pose and the second initial pose corresponding to the first robot, the first robot can determine its pose deviation based on the first initial pose, the second initial pose, and the third pose. For example, the first robot can obtain its pose deviation by minimizing a certain optimization objective (e.g., a second optimization objective). The second optimization objective can be:

[0100]

[0101] Where n is the total number of times the second or fourth pose is obtained. For rotational deviation, p represents the translational deviation. (S-w) Let w be the first or third pose of the first robot. Let w be the first initial pose or the second initial pose of the first robot.

[0102] In another possible implementation, after acquiring the historical distance (i.e., the distance between the first robot and other robots at the second time step), historical pose (i.e., the poses of other robots at the second time step), and displacement (i.e., the displacement of the first robot at the first time step and the displacement of the first robot at the second time step), the first robot can directly obtain its pose deviation by minimizing a certain optimization objective (e.g., a third optimization objective). The third optimization objective can be:

[0103]

[0104] Where m is the number of second robots, and n is the number of times the second or fourth pose is obtained. For rotation matrix, p represents the translational deviation. (S-w) Let w be the first or third pose of the first robot. Let p be the first or second displacement of the first robot at time w. k (w) represents the second or fourth pose of the k-th second robot at time w. Let m be the first or second distance between the first robot and the k-th second robot at time w. It should be understood that the number m of the second robots at this time can refer to the number of second robots for which the first robot can acquire n poses.

[0105] It should be understood that rotation matrix It can be related to pitch, roll, and yaw. In this embodiment, only the yaw angle may be considered. Therefore, when optimizing pose deviations, the rotation matrix can be... The pitch and roll values ​​are set to 0 to optimize the rotational deviation (Δyaw) and translational deviation of the first robot.

[0106] In one embodiment, when there are only two communicable robots, for example, when the first robot can obtain the pose of the second robot, sufficient historical data (such as the historical distance between the first and second robots, the historical pose of the second robot, and the displacement of the first robot) can be provided by initializing the motion of the robots (e.g., the first robot and / or the second robot), so that the pose deviation of the first robot can be accurately determined based on the historical data.

[0107] In some embodiments, after obtaining the rotational and translational deviations corresponding to the first robot, to ensure the stability of the pose of the first robot in the multi-robot system, low-pass filtering can be applied to the rotational and translational deviations to reduce the influence of high-frequency noise and drift. This can reduce the impact of UWB signal interruptions caused by occlusion or excessively large ranging ranges, and improve the robustness of positioning. For example, methods such as sliding window or Kalman filtering can be used to filter the rotational and translational deviations.

[0108] S103. The first robot optimizes the first pose based on the pose deviation to obtain the target pose corresponding to the first robot.

[0109] In this embodiment, after determining the pose deviation of the first robot, the first robot can perform pose optimization based on the pose deviation to obtain the pose of the first robot in the global coordinate system, thereby improving the accuracy of the first robot's localization. Specifically, pose optimization can involve compensating the pose deviation to restore the pose of the first robot to its local coordinate system.

[0110] For example, the first robot can compensate for the pose deviation to its corresponding first pose to obtain the target pose, that is, the pose of the first robot in the global coordinate system at the first moment. Alternatively, the first robot can compensate for the pose deviation to its subsequent pose in its local coordinate system.

[0111] In some embodiments, after optimizing the pose of the first robot to obtain its pose in the global coordinate system, for example, after obtaining the target pose corresponding to the first robot, the first robot can write the target pose into its UWB module and broadcast the target pose to other robots through the UWB module. This allows other robots to obtain the target pose corresponding to the first robot based on the information broadcast by the UWB module, thereby achieving target pose sharing and optimization. This enables other robots to accurately determine their own pose deviation based on the pose of the first robot in the global coordinate system and optimize their own pose, improving the accuracy and stability of cooperative localization and allowing each robot to provide stable pose information in the global coordinate system.

[0112] It should be noted that each robot in a multi-robot system can continuously optimize its own pose deviation in the global coordinate system based on the acquired poses of other robots and the distance between them, thereby improving its own pose accuracy and positioning accuracy. The cooperative positioning method for multi-robot systems provided in this application is not affected by the increase or decrease in the number of robots, can improve the positioning accuracy and stability of multi-robot systems, and is applicable to dynamically changing multi-robot systems.

[0113] The collaborative positioning method provided in the embodiments of this application will be illustrated below with reference to the above description.

[0114] Please see Figure 2 , Figure 2 A flowchart illustrating the cooperative positioning method provided in an embodiment of this application is shown. Figure 2 .

[0115] like Figure 2 As shown, at a certain moment w1, the first robot can obtain the pose of the first robot and the poses of other robots (e.g., robot E, robot F, and robot G), as well as the distance between the first robot and other robots.

[0116] For robot E, the first robot can determine whether there is relative motion between the first robot and robot E at time w1 based on the pose of the first robot and the pose of robot E. When it is determined that there is relative motion between the first robot and robot E at time w1, the first robot can timestamp and synchronize the pose of robot E at time w1 and the distance between the first robot and robot E, and then write it into the sliding window corresponding to robot E in the first robot.

[0117] For robot F, the first robot can determine whether there is relative motion between the first robot and robot F at time w1 based on the pose of the first robot and the pose of robot F. When it is determined that there is relative motion between the first robot and robot F at time w1, the first robot can timestamp and synchronize the pose of robot F at time w1 and the distance between the first robot and robot F, and then write it into the sliding window corresponding to robot F in the first robot.

[0118] For robot G, the first robot can determine whether there is relative motion between the first robot and robot G at time w1 based on the pose of the first robot and the pose of robot G. When it is determined that there is relative motion between the first robot and robot G at time w1, the first robot can timestamp and synchronize the pose of robot G at time w1 and the distance between the first robot and robot G, and then write it into the sliding window corresponding to robot G in the first robot.

[0119] After the data is written at time w1, the first robot can determine whether the size of any sliding window meets the requirements.

[0120] When it is determined that at least one sliding window has a size that meets the requirements, the first robot can determine the corresponding pose deviation based on the data in the sliding window that meets the requirements (i.e., the pose and distance at each moment in the sliding window that meets the requirements).

[0121] After determining the pose deviation of the first robot, the first robot can perform low-pass filtering on the pose deviation to obtain the corrected pose deviation, and can compensate the corrected pose deviation to the pose of the first robot at time w1, thereby obtaining the pose of the first robot in the global coordinate system.

[0122] When it is determined that there is no relative motion between the first robot and another robot, the first robot may not write the pose and distance of the robot at time w1 into the corresponding sliding window of the first robot. Instead, it may acquire the pose of the first robot and the poses of other robots, as well as the distance between the first robot and other robots, at a preset frequency, such as at time w2.

[0123] When it is determined that the size of all sliding windows does not meet the requirements, the first robot can, after a preset frequency, for example at time w2, obtain the pose of the first robot and the pose of other robots, as well as the distance between the first robot and other robots.

[0124] After acquiring the poses of the first robot and other robots at time w2, as well as the distance between the first robot and the other robots, the first robot can again determine whether its pose deviation needs optimization based on the poses and distances of each robot at time w2, and so on. The specific details of how the first robot determines whether to optimize its pose deviation based on the poses and distances of each robot at time w2 are similar to those of how it determines whether to optimize its pose deviation based on the poses and distances of each robot at time w1, and will not be elaborated upon here.

[0125] It should be noted that, after determining the pose deviation of the first robot based on the poses and distances of each robot at a certain moment, the first robot can continue to acquire its own pose and the poses of other robots, as well as the distances between them, at a preset frequency. Based on the acquired poses and distances, it can further determine whether to prioritize the pose deviation of the first robot. That is, in this embodiment, during the movement of the first robot, it can continuously optimize its own pose deviation. Based on the optimized pose deviation, it can continuously optimize its pose in the global coordinate system, reducing the cumulative error of the first robot and improving its positioning accuracy and stability.

[0126] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0127] Corresponding to the collaborative localization method described in the above embodiments, Figure 3 A structural block diagram of a cooperative positioning device provided in an embodiment of this application is shown. For ease of explanation, only the parts relevant to the embodiment of this application are shown. This device can be applied to a first electronic device.

[0128] Reference Figure 3 The device may include:

[0129] The data acquisition module 301 is used to acquire a first pose, a second pose, and a first distance; the first pose is the pose of the first electronic device in the local coordinate system of the first electronic device at a first moment; the second pose is the pose of the second electronic device at the first moment; and the first distance is the distance between the first electronic device and the second electronic device at the first moment.

[0130] The pose deviation acquisition module 302 is used to determine the pose deviation corresponding to the first electronic device based on the first pose, the second pose, and the first distance.

[0131] The pose optimization module 303 is used to optimize the first pose based on the pose deviation to obtain the target pose corresponding to the first electronic device.

[0132] In some embodiments, the pose deviation acquisition module 302 is specifically used to determine the first initial pose corresponding to the first electronic device based on the second pose and the first distance; and to determine the pose deviation corresponding to the first electronic device based on the first initial pose and the first pose.

[0133] In other embodiments, the data acquisition module 301 is further configured to acquire a third pose, a fourth pose, a second distance, a first displacement, and a second displacement; the third pose is the pose of the first electronic device in its local coordinate system at a second time moment; the fourth pose is the pose of the second electronic device at the second time moment; the second distance is the distance between the first electronic device and the second electronic device at the second time moment; the first displacement is the displacement of the first electronic device at the first time moment; the second displacement is the displacement of the first electronic device at the second time moment; the second time moment is earlier than the first time moment.

[0134] The pose deviation acquisition module 302 is further configured to determine the pose deviation corresponding to the first electronic device based on the first pose, the second pose, the third pose, the fourth pose, the first distance, the second distance, the first displacement, and the second displacement.

[0135] In one embodiment, the pose deviation acquisition module 302 is further configured to determine the first initial pose and the second initial pose corresponding to the first electronic device based on the second pose, the fourth pose, the first distance, the second distance, the first displacement, and the second displacement; and to determine the pose deviation corresponding to the first electronic device based on the first initial pose, the second initial pose, the first pose, and the third pose.

[0136] For example, the pose deviation acquisition module 302 is further configured to obtain a first initial pose and a second initial pose corresponding to the first electronic device by minimizing a first optimization objective; the first optimization objective is:

[0137]

[0138] Where m is the number of the second electronic devices, and n is the number of times the second pose or the fourth pose is acquired. Let w be the first initial pose or the second initial pose of the first electronic device. Let p be the first displacement or second displacement corresponding to the first electronic device at time w. k (w) represents the second or fourth pose of the k-th second electronic device at time w. Let w be the first distance or the second distance between the first electronic device and the kth second electronic device.

[0139] For example, the pose deviation acquisition module 302 is further configured to obtain the pose deviation corresponding to the first electronic device by minimizing the second optimization objective; the pose deviation includes rotation deviation and translation deviation.

[0140] The second optimization objective is:

[0141]

[0142] Where n is the number of times the second pose or the fourth pose is obtained. The rotational deviation, p is the translational deviation. (S-w) Let w be the first or third pose of the first electronic device. Let w be the first initial pose or the second initial pose of the first electronic device.

[0143] For example, the pose deviation acquisition module 302 is further configured to obtain the pose deviation corresponding to the first electronic device by minimizing a third optimization objective; the pose deviation includes rotation deviation and translation deviation.

[0144] The third optimization objective is:

[0145]

[0146] Where m is the number of the second electronic devices, and n is the number of times the second pose or the fourth pose is acquired. For rotation matrix, p is the translational deviation. (S-w) Let w be the first or third pose of the first electronic device. Let p be the first displacement or second displacement corresponding to the first electronic device at time w. k (w) represents the second or fourth pose corresponding to the k-th second electronic device at time w. Let w be the first distance or the second distance between the first electronic device and the kth second electronic device.

[0147] In some embodiments, the first electronic device includes a first ultra-wideband UWB module, and the second electronic device includes a second UWB module.

[0148] The data acquisition module 301 is further configured to acquire the second pose broadcast by the second UWB module through the first UWB module; and acquire the first distance through the first UWB module.

[0149] It should be noted that the information interaction and execution process between the above-mentioned devices / units are based on the same concept as the method embodiments of this application. For details on their specific functions and technical effects, please refer to the method embodiments section, and they will not be repeated here.

[0150] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0151] Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Figure 4 As shown, the electronic device 4 in this embodiment includes: a UWB module 43, and at least one processor 40. Figure 4 Only one is shown in the diagram. A memory 41 and a computer program 42 stored in the memory 41 and executable on the at least one processor 40 are also shown. The UWB module 43 is used to acquire pose and distance. When the processor 40 executes the computer program 42, it implements the steps in any of the above-described cooperative localization method embodiments.

[0152] The electronic device 4 can be a robot, unmanned vehicle, drone, or unmanned boat, or other mobile electronic device capable of positioning. The electronic device 4 may include, but is not limited to, a processor 40 and a memory 41. Those skilled in the art will understand that... Figure 4 This is merely an example of electronic device 4 and does not constitute a limitation on electronic device 4. It may include more or fewer components than shown, or combine certain components, or different components. For example, it may also include input / output devices, network access devices, etc.

[0153] The processor 40 can be a central processing unit (CPU), but it can also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor.

[0154] In some embodiments, the memory 41 may be an internal storage unit of the electronic device 4, such as a hard disk or memory of the electronic device 4. In other embodiments, the memory 41 may be an external storage device of the electronic device 4, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., equipped on the electronic device 4. Furthermore, the memory 41 may include both internal and external storage units of the electronic device 4. The memory 41 is used to store the operating system, applications, bootloader, data, and other programs, such as the program code of the computer program. The memory 41 can also be used to temporarily store data that has been output or will be output.

[0155] This application also provides a computer-readable storage medium storing a computer program that, when executed by an electronic device, causes the electronic device to perform the steps described in the various method embodiments above.

[0156] This application provides a computer program product, which includes a computer program. When the computer program is executed by an electronic device, it causes the electronic device to perform the steps in the various method embodiments described above.

[0157] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of this application can be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable storage medium can include at least: any entity or device capable of carrying computer program code to a device / electronic device, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium. Examples include USB flash drives, portable hard drives, magnetic disks, or optical disks. In some jurisdictions, according to legislation and patent practice, computer-readable storage media cannot be electrical carrier signals or telecommunication signals.

[0158] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0159] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0160] In the embodiments provided in this application, it should be understood that the disclosed devices / electronic devices and methods can be implemented in other ways. For example, the device / electronic device embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the displayed or discussed mutual couplings or direct couplings or communication connections may be through some interfaces; indirect couplings or communication connections between devices or units may be electrical, mechanical, or other forms.

[0161] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0162] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A method of cooperative positioning, characterized by, The method applied to a first electronic device comprises: obtaining a first pose, a second pose and a first distance; the first pose is a pose of the first electronic device in a local coordinate system of the first electronic device at a first time; the second pose is a corresponding pose of a second electronic device at the first time; and the first distance is a distance between the first electronic device and the second electronic device at the first time; determining a pose deviation of the first electronic device according to the first pose, the second pose and the first distance; optimizing the first pose according to the pose deviation to obtain a target pose of the first electronic device; wherein the determining of the pose deviation of the first electronic device according to the first pose, the second pose and the first distance comprises: determining a first initial pose of the first electronic device according to the second pose and the first distance; determining the pose deviation of the first electronic device according to the first initial pose and the first pose; alternatively, the method further comprises: obtaining a third pose, a fourth pose, a second distance, a first displacement and a second displacement; the third pose is a pose of the first electronic device in the local coordinate system of the first electronic device at a second time; the fourth pose is a corresponding pose of the second electronic device at the second time; the second distance is a distance between the first electronic device and the second electronic device at the second time; the first displacement is a displacement of the first electronic device at the first time; the second displacement is a displacement of the first electronic device at the second time; and the second time is earlier than the first time; the determining of the pose deviation of the first electronic device according to the first pose, the second pose and the first distance comprises: determining a first initial pose and a second initial pose of the first electronic device according to the second pose, the fourth pose, the first distance, the second distance, the first displacement and the second displacement; determining the pose deviation of the first electronic device according to the first initial pose, the second initial pose, the first pose and the third pose.

2. The method of claim 1, wherein, the determining of the first initial pose and the second initial pose of the first electronic device according to the second pose, the fourth pose, the first distance, the second distance, the first displacement and the second displacement comprises: obtaining the first initial pose and the second initial pose of the first electronic device by minimizing a first optimization target; the first optimization target is: ； wherein m is the number of the second electronic devices, n is the number of the time instants at which the second poses or the fourth poses are obtained, is the first initial pose or the second initial pose corresponding to the first electronic device at time instant w, is the first displacement or the second displacement corresponding to the first electronic device at time instant w, is the second pose or the fourth pose of the kth second electronic device at time instant w, is the first distance or the second distance between the first electronic device and the kth second electronic device at time instant w.

3. The method of claim 1, wherein, the determining of the pose deviation of the first electronic device according to the first initial pose, the second initial pose, the first pose and the third pose comprises: obtaining the pose deviation of the first electronic device by minimizing a second optimization target; the pose deviation comprises a rotation deviation and a translation deviation; the second optimization target is: ； wherein n is the number of time instants at which the second pose or the fourth pose is obtained, is the rotation deviation, is the translation deviation, is the first pose or the third pose corresponding to the first electronic device at time instant w, is the first initial pose or the second initial pose corresponding to the first electronic device at time instant w.

4. The method of claim 1, wherein, The determining the pose deviation corresponding to the first electronic device according to the first pose, the second pose, the third pose, the fourth pose, the first distance, the second distance, the first displacement and the second displacement comprises: The pose deviation corresponding to the first electronic device is obtained by minimizing the third optimization target; the pose deviation comprises a rotation deviation and a translation deviation; The third optimization target is: ； wherein m is the number of the second electronic devices, n is the number of the time instants at which the second poses or the fourth poses are obtained, is a rotation matrix, is the translation bias, is the first pose or the third pose corresponding to the first electronic device at time instant w, is the first displacement or the second displacement corresponding to the first electronic device at time instant w, is the second pose or the fourth pose corresponding to the kth second electronic device at time instant w, is the first distance or the second distance between the first electronic device and the kth second electronic device at time instant w.

5. The method according to any one of claims 1 to 4, characterized in that, The first electronic device comprises a first ultra-wideband (UWB) module, and the second electronic device comprises a second UWB module; the obtaining the first pose, the second pose and the first distance comprises: The second pose broadcast by the second UWB module is obtained by the first UWB module; The first distance is obtained by the first UWB module.

6. An electronic device comprising a UWB module, a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, The UWB module is used to obtain a pose and a distance, and the processor executes the computer program, so that the electronic device implements the cooperative positioning method in any one of claims 1 to 5.

7. A computer program product comprising a computer program, characterized in that, When the computer program is executed by the electronic device, the electronic device implements the cooperative positioning method in any one of claims 1 to 5.

Images