CN117761704B - Method and system for estimating relative positions of multiple robots - Google Patents

Abstract

A method and system for estimating the relative positions of multiple robots, wherein each robot is configured to use a laser radar to collect the surrounding point cloud information of the sensor within a certain period of time, and then the information is converted into relative distance and angle information in the form of a homogeneous transformation matrix, and the point cloud information is clustered to generate a corresponding identifier; the relative distance and angle information of each robot is then matched through a pairing optimization algorithm, and the matching method is implemented by constructing an optimization model and using the Levenberg-Marquardt algorithm of the nonlinear least squares method to solve the corresponding relationship between the cluster identifier and each robot. The present invention uses reliable point cloud clustering recognition technology and nonlinear optimization methods to enable a multi-robot cluster to achieve efficient task collaboration and more accurate and robust collaborative positioning. The overall cognitive level and team efficiency of multiple robots are improved.

Description

Method and system for estimating relative positions of multiple robots

Technical Field

The present invention relates to a technology in the field of industrial automation, in particular to a method and system for estimating the relative positions of multiple robots.

Background Art

The positioning and coordination of multiple robots is a complex and thorny industrial problem. Multi-robot collaborative positioning refers to a group of robots jointly determining their positions relative to the global coordinate system to achieve collaborative tasks. In the past technology, the methods used can be summarized as centralized positioning, global positioning and collective control methods. However, the existing centralized positioning technology will accumulate the errors obtained, which will lead to the continuous expansion of the cumulative errors and affect the positioning effect; the global positioning technology requires each robot to use completely consistent algorithms and configurations, which will lead to the problem of overall consistency being difficult to meet; and the collective control technology has completely fixed parameter settings and does not have the shortcomings of portability and maintainability.

Summary of the invention

In view of the above problems in the prior art, the present invention proposes a method and system for estimating the relative positions of multiple robots, which uses reliable point cloud clustering recognition technology and nonlinear optimization methods to enable multi-robot clusters to achieve efficient task collaboration and more accurate and robust collaborative positioning, thereby improving the overall cognitive level and team efficiency of multiple robots.

The present invention is achieved through the following technical solutions:

The present invention relates to a method for estimating the relative positions of multiple robots. Each robot is configured to use a laser radar to collect point cloud information around a sensor within a certain period of time, and the information is converted into relative distance and angle information in the form of a homogeneous transformation matrix. At the same time, a clustering operation is performed on the point cloud information to generate a corresponding identifier. The relative distance and angle information of each robot is then matched through a pairing optimization algorithm. The matching method is implemented by constructing an optimization model and solving the Levenberg-Marquardt algorithm of a nonlinear least squares method to obtain the corresponding relationship between the cluster identifier and each robot.

The present invention relates to a system for implementing the above method, comprising: a point cloud preprocessing module, a point cloud clustering module, a message communication module and a pairing algorithm module, wherein: the point cloud preprocessing module performs ground filtering processing according to the point cloud information obtained by the laser radar sensor, obtains the filtered point cloud after filtering out the ground points, and then screens the scene on the point cloud obtained after the ground filtering, deletes some discrete points to obtain more concentrated and accurate target point cloud information; the point cloud clustering module performs clustering operation on the target point cloud information to obtain the distance and angle information of each cluster from its own coordinate system; the message communication module timely publishes the clustering results obtained after clustering its own point cloud through a local area network according to the information in the message queue of each robot, and receives the clustering results obtained after clustering the point clouds of other robots; the pairing algorithm module constructs a nonlinear optimization function according to the clustering results, and completes the pairing between robots according to minimizing the distance and angle relationship between different robots, so that each robot obtains a relative position relationship with other robots.

Technical Effects

The present invention can realize the robot to confirm its own position in a completely unknown environment through pairing based on nonlinear optimization functions, and can realize multi-robot interaction to help other robots confirm each other's positions. Compared with other collaborative positioning algorithms, this method can greatly reduce the dependence on environmental information and sensors. The actual application scenario is that in a complex environment with multiple robots, as long as any robot establishes its own global position, the global position of all other robots can be obtained through the relative position estimation algorithm of the present invention, thereby being able to perform more complex tasks.

BRIEF DESCRIPTION OF THE DRAWINGS

Fig. 1 is a flow chart of the present invention;

FIG. 2 is a schematic diagram of the effect of the embodiment.

DETAILED DESCRIPTION

As shown in FIG1 , this embodiment relates to a method for estimating relative positions of multiple robots, which specifically includes:

Step 1) Set each robot to obtain the point cloud information obtained by the laser radar, and perform ground filtering and scene segmentation on it, including:

1.1 Remove isolated points: Traverse the original point cloud and delete a certain proportion of the highest and lowest images on the z-axis (height) in the point cloud. At the same time, delete some points that are far away from other point clouds through nearest neighbor search to complete the work of removing isolated points.

1.2 Ground filtering: Use the PTD algorithm to define a triangular bounding box containing the coordinates X_top, Y_top, X_bottom, Y_bottom, and divide the entire point cloud data into: Among them: width w and height h.

1.3 Start PTD traversal from the lowest point as the seed point: Calculate the potential point determination in the triangle. It is known that the potential point can only be on the edge and vertex of the triangle. When the inclination of the slope S exceeds the threshold, the potential point cannot be on this surface. Therefore, the point outside the triangle is excluded as a ground point and the next point is determined. The inclination of the slope S is Among them: A, B is a side of the triangle bounding box, point P is an external point in the point cloud image that does not belong to the triangle bounding box, PA, AB are vectors, ||.|| is the second normal form of the vector, and the angle from point P to the surface formed by the triangle bounding box is taken as the inclination angle θ _PA of the triangle.

Step 2) Set each robot to perform clustering operations on the obtained point cloud and assign a unique identifier to each cluster to represent other robots perceived, including:

2.1) Using Euclidean distance clustering, by setting the distance between point cloud clusters as the external threshold and the internal distance of point cloud clusters as the internal threshold;

2.2) Select any point P of the point cloud, traverse other points, and take the points whose Euclidean distance is less than the internal threshold as the cluster internal points of point P. After the traversal is completed, these points are marked with the cluster center P and the unique ID of the cluster.

2.3) Repeat the above process with the point outside the cluster as the center, and finally traverse the obtained cluster center.

2.4) When there are other cluster centers with Euclidean distance less than the external threshold, the two clusters are merged and the process of finding the inner points of the cluster is repeated again until the program cannot find a new cluster, that is, it filters out the appropriate cluster based on the distance and the number of points in the cluster, and outputs the coordinates of the cluster center and the distance to itself to the next module.

Step 3) Each robot is set to calculate the relative distance and angle information between itself and other robot bodies, and each robot uses the message queue communication mechanism to exchange the cluster identifiers and corresponding relative distance and angle information observed by each robot with other robots.

The relative distance and angle information is represented by a homogeneous transformation matrix.

The message queue communication mechanism specifically includes:

i) Publish message queue: The robot packages the data it collects and publishes it to the ROS local area network at a frequency of 1Hz

ii) Subscribe to message queue: The robot will open its own LAN port. The program can listen to the data released by other robots through the LAN and establish a queue data structure for storage. The integrity of the data packet will be checked every 10 seconds. If the data is complete, it will be injected into the pairing optimization algorithm.

Step 4) Setting each robot to establish and solve constraints based on its own pairing optimization algorithm through the duality of relative distance and angle information, specifically including:

4.1) Construct optimization model: Euclidean distance establishes optimization target: minimize||H _BX *H _XA -H _AB || ² , where: ||.|| represents the Euclidean norm, which indicates the difference between two matrices; the homogeneous transformation matrix from the perspective of robot A is H _AB , and the homogeneous transformation matrix of robot X from the perspective of robot B is H _BX , and the homogeneous transformation matrix of robot X from the perspective of robot A is H _XA as the variable.

4.2) Use the Levenberg-Marquardt algorithm of nonlinear least squares method to solve the objective of step 4.1. The optimal inverse transformation matrix H_XA, which indicates that X is robot A from the perspective of this robot, is specifically: transform the optimization problem into minimize J(H _XA )＝|| _HBX *H _XA -H _AB || ² , where: J(H _XA ) represents the objective function of the optimization problem; first initialize the initial value of H _XA , then calculate the gradient and Hessian matrix of the objective function with respect to H _XA , and then use the Levenberg-Marquardt rule to update and adjust the estimated value so that the objective function meets the requirements.

The Levenberg-Marquardt rule is specifically: in: is the updated estimated H _XA matrix; is the estimate of the H _XA matrix in the previous iteration; H() is the Hessian second-order derivative matrix of the objective function J(H _XA ) with respect to H _XA , which describes the curvature of the objective function; Grad() is the gradient of the objective function J(J _XA ) with respect to H _XA , that is, the first-order derivative of J with respect to H _XA ; λ is a regularization parameter, that is, the Levenberg-Marquardt parameter, and I is the unit matrix.

When λ is small, the algorithm tends to use Newton's method more, and when λ is large, the algorithm tends to use gradient descent more.

Step 5) Set each robot to publish the obtained pairing relationship between the cluster identifier and the robot ID to the local area network, and publish the pairing information to inform the robot of the relative position and angle information with other robots for subsequent node optimization and scheduling.

The pairing information means: for example, robot A receives the cluster location information H_BX sent by robot B. Robot A knows through the pairing algorithm that one of its point cloud clusters H_AX represents robot B. Then A will publish the ID of its point cloud cluster paired with robot B and the ID of the point cloud cluster belonging to robot A in the point cloud sent by robot B.

After specific actual experiments, a car chassis and a small autonomous robot were used as two robot platforms. Both platforms were equipped with laser radars and could communicate through a local area network. The experimental environment was robot A and robot B. Experiment 1 was based on robot A, and the information obtained through point cloud clustering was used as the observed position of robot B without pairing. Experiment 2 was the position of robot B obtained by averaging the distance and orientation observed by the pair after pairing optimization after point cloud clustering. The positions obtained in Experiments 1 and 2 were compared with the absolute coordinates converted from the GPS true position of robot B. Specifically, the distances and orientations from the two perspectives obtained after pairing optimization were averaged, which significantly reduced the positioning deviation caused by long-term operation for the autonomous robot that only used the car chassis for self-positioning. In this experiment, the system was able to run stably until the autonomous robot ran out of power.

As shown in Figure 2, the average error of the method is 0.0263, while the average error of the traditional method is 0.0270. The error of the traditional method has a clear upward trend as the running time increases; however, the error of the algorithm used in the present invention is relatively stable. It can be seen that the method can maintain a stable positioning error during long-term operation.

The above-mentioned specific implementation can be partially adjusted in different ways by those skilled in the art without departing from the principle and purpose of the present invention. The protection scope of the present invention shall be based on the claims and shall not be limited by the above-mentioned specific implementation. Each implementation scheme within its scope shall be subject to the constraints of the present invention.

Claims (7)

1. A method for estimating the relative position of multiple robots, characterized in that each robot is configured to use a laser radar to collect the surrounding point cloud information of the sensor within a certain period of time, and then convert it into relative distance and angle information in the form of a homogeneous transformation matrix, and at the same time, cluster the point cloud information to generate corresponding identifiers; then the relative distance and angle information of each robot is matched through a pairing optimization algorithm, and the matching method is implemented by constructing an optimization model and using the Levenberg-Marquardt algorithm of the nonlinear least squares method to solve the corresponding relationship between the cluster identifier and each robot; The collection of the surrounding point cloud information of the sensor within a certain period of time refers to: setting each robot to obtain the point cloud information obtained by the laser radar, and performing ground filtering and scene segmentation on it, specifically including: 1.1 Remove isolated points: traverse the original point cloud and delete the z-axis in the point cloud, that is, a certain proportion of the highest and lowest images; at the same time, delete some points that are far away from other point clouds through nearest neighbor search to complete the work of removing isolated points; 1.2 Ground filtering: Use the PTD algorithm to define a triangular bounding box containing the coordinates X_top, Y_top, X_bottom, Y_bottom, and divide the entire point cloud data into: Among them: width w and height h; 1.3 Start PTD traversal from the lowest point as the seed point: Calculate the potential point determination in the triangle. It is known that the potential point can only be on the edge and vertex of the triangle. When the inclination of the slope S exceeds the threshold, the potential point cannot be on this surface. Therefore, the point outside the triangle is excluded as a ground point and the next point is determined. The inclination of the slope S is Where: A, B is one side of the triangle bounding box, point P is an external point in the point cloud image that does not belong to the triangle bounding box, PA, AB are vectors, ||.|| is the second normal form of the vector, and the angle from point P to the surface formed by the triangle bounding box is taken as the inclination angle θ _PA of the triangle; The clustering operation to generate the corresponding identifier means: setting each robot to perform clustering operation on the obtained point cloud and assigning a unique identifier to each cluster to represent other robots sensed, specifically including: 2.1) Using Euclidean distance clustering, by setting the distance between point cloud clusters as the external threshold and the internal distance of point cloud clusters as the internal threshold; 2.2) Select any point P of the point cloud, traverse other points, and take those points whose Euclidean distance is less than the internal threshold as the cluster internal points of point P. After the traversal is completed, mark these points with the cluster center P and the unique ID of the cluster; 2.3) Repeat the above process with the point outside the cluster as the center, and finally traverse the obtained cluster center; 2.4) When there are other cluster centers whose Euclidean distance is less than the external threshold, the two clusters are merged and the process of finding the inner points of the cluster is repeated again; until the program can find no new clusters, that is, filter out the appropriate clusters according to the distance and the number of points in the cluster, and output the coordinates of the cluster center and the distance to itself to the next module; The relative distance and angle information of each robot refers to setting each robot to calculate the relative distance and angle information between itself and other robot bodies, and each robot uses a message queue communication mechanism to exchange the cluster identifiers and the corresponding relative distance and angle information observed by each robot with other robots; The matching by pairing optimization algorithm means: setting each robot according to its own pairing optimization algorithm, establishing and solving constraint conditions by the duality of relative distance and angle information, specifically including: 4.1) Construct optimization model: Euclidean distance establishes optimization target: minimize||H _BX *H _XA -H _AB || ² , where: ||.|| represents the Euclidean norm, which represents the difference between two matrices; the homogeneous transformation matrix from the perspective of robot A is HA _{B, and} the homogeneous transformation matrix of robot X from the perspective of robot B is H _BX , and the homogeneous transformation matrix of robot X from the perspective of robot A is H _XA as a variable; 4.2) Use the Levenberg-Marquardt algorithm of nonlinear least squares method to solve the objective of step 4.1. The optimal inverse transformation matrix H_XA indicates that X is robot A from the perspective of this robot. Specifically, the optimization problem is transformed into minimizeJ(H _XA )＝||H _BX *H _XA -H _AB || ² , where: J(H _XA ) represents the objective function of the optimization problem; first initialize the initial value of H _XA , then calculate the gradient and Hessian matrix of the objective function with respect to H _XA , and then use the Levenberg-Marquardt rule to update and adjust the estimated value so that the objective function meets the requirements; The correspondence between the clustering identifier and each robot is established by setting each robot to publish the obtained pairing relationship between the clustering identifier and the robot ID to the local area network, and publishing the pairing information to inform the robot of the relative position and angle information with other robots for subsequent node optimization and scheduling. 2. The method for estimating the relative positions of multiple robots according to claim 1 is characterized in that the relative distance and angle information are represented using a homogeneous transformation matrix. 3. The method for estimating relative positions of multiple robots according to claim 1, wherein the message queue communication mechanism specifically comprises: i) Publish message queue: The robot packages the data it collects and publishes it to the ROS local area network at a frequency of 1Hz; ii) Subscribe to message queue: The robot will open its own LAN port. The program can listen to the data released by other robots through the LAN and establish a queue data structure for storage. The integrity of the data packet will be checked every 10 seconds. If the data is complete, it will be injected into the pairing optimization algorithm. 4. The method for estimating relative positions of multiple robots according to claim 1, wherein the Levenberg-Marquardt algorithm is specifically: in: is the updated estimated H _XA matrix; is the estimate of the H _XA matrix in the previous iteration; H() is the Hessian second-order derivative matrix of the objective function J(H _XA ) with respect to H _XA , which describes the curvature of the objective function; Grad() is the gradient of the objective function J(H _XA ) with respect to H _XA , that is, the first-order derivative of J with respect to H _XA ; λ is a regularization parameter, that is, the Levenberg-Marquardt parameter, and I is the unit matrix. 5. The method for estimating the relative positions of multiple robots according to claim 4 is characterized in that when λ is small, the algorithm tends to use the Newton method more, and when λ is large, the algorithm tends to use the gradient descent method more. 6. The method for estimating the relative positions of multiple robots according to claim 1 is characterized in that the pairing information refers to: robot A receives the clustered position information H_BX sent by robot B, and robot A knows through the pairing algorithm that one of its point cloud clusters H_AX represents robot B, then A will publish the ID of its own point cloud cluster paired with robot B and the ID of the point cloud cluster belonging to robot A in the point cloud sent by robot B. 7. A system for estimating the relative position of multiple robots that implements the method described in any one of claims 1 to 6, characterized in that it includes: a point cloud preprocessing module, a point cloud clustering module, a message communication module and a pairing algorithm module, wherein: the point cloud preprocessing module performs ground filtering processing based on the point cloud information obtained by the laser radar sensor, obtains the filtered point cloud after filtering out the ground points, and then screens the point cloud obtained after the ground filtering for scenes, deletes some discrete points to obtain more concentrated and accurate target point cloud information; the point cloud clustering module performs clustering operations on the target point cloud information to obtain the distance and angle information of each cluster from its own coordinate system; the message communication module timely publishes the clustering results obtained after clustering its own point cloud through the local area network according to the information in the message queue of each robot and receives the clustering results obtained after clustering the point clouds of other robots; the pairing algorithm module constructs a nonlinear optimization function based on the clustering results, and completes the pairing between robots by minimizing the distance and angle relationship between different robots, so that each robot obtains a relative position relationship with other robots.