Method for cooperative acquisition of relative pose of mobile agent by dynamic threshold point cloud matching

Abstract

The application discloses a kind of dynamic threshold point cloud matching mobile intelligent agent relative pose cooperative acquisition method, utilize shared laser point cloud data and autonomous positioning result, using improved iterative nearest neighbor point (ICP) registration algorithm, the relative pose between mobile intelligent agent is calculated, and the reliability of calculation result is evaluated, further improve the observation reliability and application value of relative pose.The method of the present application only needs to reuse lidar data on the basis of autonomous positioning, without additional enhancement mark, the observation calculation of relative pose can be completed, the original system is changed little, does not affect the original layout of system, does not need to rely on other feature enhancement hardware, no additional influence, low cost;It has better accuracy and stronger robustness, and the effectiveness evaluation of point cloud registration result makes the application more suitable for the application scene of relative pose, and enhances the reliability of result utilization.

Description

Technical Field

[0001] This invention belongs to the field of multi-agent cooperative localization technology, specifically relating to a method for cooperative acquisition of relative pose of mobile agents by dynamic threshold point cloud matching. By sharing the autonomous localization results of mobile agents and radar point cloud data, flexible and reliable acquisition and application of relative pose between mobile agents can be achieved. Background Technology

[0002] In recent years, mobile intelligent agents have become an increasingly popular area in the information technology industry. Autonomous localization in unknown environments, exemplified by SLAM (Simultaneous Localization and Mapping), is the foundation for the safe and efficient operation of mobile intelligent agents. Simultaneously, with the promotion of concepts such as swarm intelligence and social intelligence, multi-agent systems are developing rapidly. Especially in indoor environments and low-speed movement scenarios, relative pose provides support for cooperative mapping, localization optimization, and collaborative scheduling, playing a crucial role in multi-agent systems, particularly in multi-robot collaborative SLAM.

[0003] Relative pose, which is the pose of one mobile agent in a multi-agent system relative to the reference frame of another agent, needs to be acquired effectively. Directly obtaining relative pose through the difference of autonomous localization results in the accumulation of localization errors, leading to poor accuracy and making it difficult to use effectively. Therefore, observation-based relative pose acquisition methods are needed.

[0004] With the flourishing research in this field, a number of observation-based relative pose acquisition methods have emerged, most of which rely on known object features for observation. Such methods depend on the quality of individual SLAM or additional markers, exhibiting weak robustness and high equipment costs. In contrast, point cloud registration based on acquired laser point cloud data offers greater independence and is suitable for indirect observation of relative pose, supporting cooperative SLAM and other functions of multi-agent systems. Furthermore, under network conditions, initial values ​​necessary for point cloud registration can be easily obtained by sharing self-localization results, thus making point cloud registration highly applicable in relevant scenarios.

[0005] In point cloud registration methods, the typical method based on traditional mathematical optimization theory is Iterative Nearest Neighbor (ICP). However, the original ICP method's matching performance is not ideal in scenarios involving multi-machine relative observation and partial overlap of point clouds. Other improved ICP-based methods focus on extracting structural features and improving matching speed, without addressing the matching performance in scenarios with partially overlapping point clouds. Furthermore, the original ICP's evaluation method is unsuitable for indoor environments and low-speed motion scenarios, failing to effectively reflect the accuracy of relative pose measurement when point clouds partially overlap. Other improved ICP-based methods do not offer improvements or supplements in result evaluation. Existing related technologies struggle to accurately and reliably acquire the relative poses between multi-agent systems operating at low speeds in indoor scenarios. Summary of the Invention

[0006] This invention proposes a method and application device for collaborative acquisition of relative pose of mobile agents based on dynamic threshold point cloud matching in multi-agent systems. The device includes an autonomous localization module, a data communication module, an ICP registration module, and a pose relationship reliability evaluation module, which enables rapid and reliable acquisition of the relative pose of agents in a multi-agent system at a low cost.

[0007] In this invention, necessary motion sensors and environmental perception devices are equipped on low-speed mobile intelligent agents (including but not limited to Automated Guided Vehicles (AGVs) and robots with autonomous mobility) operating on indoor ground to support autonomous localization. Under network conditions, a data communication module is also provided to support information sharing. An improved Iterative Nearest Neighbor (ICP) registration algorithm is implemented using shared laser point cloud data and autonomous localization results to calculate the relative poses between intelligent agents. The reliability of the calculation results is evaluated, further improving the observation reliability and application value of relative poses. This method can be applied to multi-agent systems operating at low speeds in quasi-enclosed indoor environments without affecting the original system layout. It does not rely on other hardware markers to enhance features, achieving fast and reliable relative pose acquisition and application at a lower cost.

[0008] To achieve the above objectives, this invention requires that the mobile intelligent agent be equipped with corresponding motion sensors (inertial measurement units, wheel speedometers, etc.) and environmental perception devices (of which lidar is essential), enabling it to form an autonomous positioning module with decimeter-level accuracy. Furthermore, it is equipped with a data communication module, allowing different intelligent agents to share data via a network. Each mobile intelligent agent obtains real-time autonomous positioning results and lidar point cloud data from other intelligent agents through network communication. The specific steps for relative pose collaborative acquisition and application are as follows:

[0009] 1) Based on the data collected by the sensors on each mobile agent, the autonomous localization module obtains the autonomous localization results of each agent.

[0010] 2) Through the data communication module, each mobile intelligent agent that needs to perform relative observations shares its own laser point cloud data and the autonomous positioning results obtained in 1).

[0011] 3) Based on the shared laser point cloud data and autonomous localization results in 2), the relative pose observation results between the two agents are output through coordinate pre-transformation, stepwise ICP point cloud registration based on dynamic threshold, and coordinate inverse transformation.

[0012] 4) Based on the two clusters of laser point cloud data collected by two mobile intelligent agents, the effective points are extracted by KD tree, and the effective points are statistically analyzed and linearly regressed. The overlap index and linear index are output to evaluate the credibility of the relative pose relationship calculated in 3).

[0013] 5) Package the relative pose observation results in 3) with the relative pose reliability index in 4) and provide them to the autonomous localization module for optimization to form a more accurate and reliable cooperative localization; further provide them to the central server or other intelligent agents to support other functions of the system.

[0014] Compared with the prior art, the beneficial effects of the present invention are:

[0015] This invention provides a method for collaborative acquisition and application of relative pose of mobile intelligent agents based on dynamic threshold point cloud matching. It only requires reusing LiDAR data based on autonomous localization, and can complete the observation and calculation of relative pose without additional enhancement markers. This method requires minimal modification to the original system, has no additional impact, and is low-cost. The improved ICP point cloud registration method based on dynamic threshold proposed in this invention has better accuracy and stronger robustness. Furthermore, the pose relationship result reliability evaluation module's assessment of the validity of the point cloud registration results is more consistent with the application scenario of relative pose, enhancing the reliability of the results' utilization.

[0016] The mobile intelligent agent relative pose cooperative acquisition method and application based on dynamic threshold point cloud matching proposed in this invention have the following advantages:

[0017] (i) Using the point cloud data of the original laser sensor, the fixed registration threshold in the original ICP is modified to a set of dynamically decreasing thresholds. Without increasing the time overhead, the error of the point cloud registration transformation result is reduced by at least 20%.

[0018] (II) To address the shortcoming that the original ICP score cannot accurately reflect the accuracy of relative pose measurement in partially overlapping scenarios, a threshold calculation strategy based on clustering algorithms is combined with KD-trees to extract valid points and their distances. The count of valid points and linear regression are used as indicators of the reliability of relative pose observation results, which helps to improve the reliability of the application of relative pose acquisition results.

[0019] (III) The improved point cloud registration algorithm described above can be applied to multi-agent distributed cooperative localization scenarios. Distributed cooperative localization and relative pose observation form a coupled architecture, with the former providing relatively reliable initial values ​​for the latter, and the latter optimizing the results of the former, forming a virtuous cycle. Attached Figure Description

[0020] Figure 1 This is a block diagram of the relative pose acquisition and application system of the present invention.

[0021] Figure 2 This is a flowchart of the point cloud ICP registration module algorithm of the present invention.

[0022] Figure 3 This is a flowchart of the registration result evaluation process of the present invention.

[0023] Figure 4 This is a flowchart of the effective threshold calculation module of the present invention. Detailed Implementation

[0024] To make the objectives, features, and advantages of this invention clearer and easier to understand, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0025] refer to Figure 1 This invention requires each mobile intelligent agent in the system to be equipped with a set of sensors, among which the environmental perception device must include LiDAR, and the motion sensor must include at least an inertial measurement unit and a wheel speed meter, for autonomous positioning in quasi-enclosed indoor environments. Figure 1 As shown, relying on the autonomous localization results of each agent and laser point cloud data, relative pose collaborative acquisition based on dynamic threshold point cloud matching can be achieved. Therefore, each agent needs to share its real-time autonomous localization results and laser point cloud data under network conditions through a data communication module. The relative pose data obtained by the relative pose acquisition module is further verified for reliability by the relative pose result reliability evaluation module, which can be used to further optimize the autonomous localization results and serve other functions of the multi-agent system.

[0026] This invention incorporates a relative pose collaborative acquisition and application device within a mobile intelligent body. This device includes a data communication module to support necessary data interaction under network conditions; an autonomous positioning module that supports both relative pose acquisition and effective utilization of the relative pose results; a relative pose acquisition module; and a relative pose result evaluation module. The autonomous positioning module includes environmental sensing devices and motion sensors. The specific steps are as follows:

[0027] S10: The agents acquire autonomous localization results respectively;

[0028] refer to Figure 1 The device requires mobile agents to be equipped with motion sensors including at least an inertial measurement unit and a wheel speedometer, and environmental perception devices including at least a lidar. The autonomous localization module relies on each agent's own sensor data for localization. It outputs an estimate of the self-localization result. The autonomous localization module is an independent localization module that can apply existing mainstream localization methods; introducing relative pose optimization does not require changes to the architecture of the autonomous localization module.

[0029] S20: The autonomous positioning results obtained in the previous step are timestamped and repackaged with the laser point cloud data of the mobile intelligent agent itself, and the data is shared using the data communication module.

[0030] S30: Point cloud matching with dynamic threshold; Point cloud P of the agent to be observed m By transforming the coordinate system, the target point cloud P′ to be registered is obtained. m Self-smart agent point cloud P n After performing stepwise ICP point cloud registration based on dynamic threshold and rigid body transformation, point cloud P′ is obtained. n Then output the relative pose observation results between the two agents;

[0031] refer to Figure 2 The self-localization results and point cloud data of the agent (numbered n) and the agent to be observed (numbered m) obtained from S10 / S20 are used to calculate a coordinate system transformation relationship between the two localization results, and the other party's point cloud P is transformed. m Substituting the coordinate system transformation, we obtain the target point cloud to be registered, called point cloud P′. m This invention designs a distance threshold calculation method, which uses the opposing point cloud P... m With the point cloud P of the intelligent agent itself n Calculate a set of progressively decreasing distance thresholds as parameters for the ICP algorithm, perform dynamic threshold ICP point cloud registration, and calculate the point cloud P′. m With point cloud P n The registration result between them is a linear rigid body transformation matrix. This leads to the point cloud P. m P nThe transformation relationship between the two agents is used to obtain their relative poses.

[0032] S40: Design credibility assessment metrics to evaluate the credibility of matching results; credibility assessment metrics include overlap metrics and linearity metrics;

[0033] A coordinate system transformation relationship is calculated from the self-localization results of the agent and the agent to be observed, and the point cloud P of the other agent (the agent to be observed) is transformed. m Substituting the coordinate system transformation, we obtain the target point cloud to be registered, called point cloud P′. m Self-point cloud P n Perform a rigid body transformation on S30 to obtain the point cloud P′. n .like Figure 3 As shown, the point cloud registration result evaluation module evaluates the point cloud P′ m With point cloud P′ n The relationship between the two indicators is used to derive two metrics for the reliability of the registration results.

[0034] S50: Utilize the relative pose results and their reliability indicators in the system;

[0035] The relative pose between two agents calculated by S30 is reflected by the result evaluation index in S40. It can be fed back into the autonomous localization module in S10, or sent to other agents and the central controller through the communication module to serve other functions of the multi-agent system.

[0036] In step S10, each mobile agent obtains an independent localization result based on its own sensor data and corresponding filtering and optimization algorithms, estimating its own position. The autonomous localization result of agent n is... in In a two-dimensional Cartesian coordinate system of a real environment, these are the x and y coordinates of the estimated localization of agent n. This is the estimated orientation angle of agent n in a two-dimensional Cartesian coordinate system of the real environment. Similarly, the autonomous localization result of agent m is... in In a two-dimensional Cartesian coordinate system of a real environment, these are the x and y coordinates of the agent numbered m, representing the estimated localization. It is the estimated orientation angle of agent m in a two-dimensional Cartesian coordinate system of a real environment.

[0037] In step S20, necessary data sharing is required between the agents performing relative pose observation. The data communication module includes two modules: data sending and data receiving. Each mobile agent needs to share its own laser point cloud data and autonomous localization results through the data sending module, and also needs to obtain the laser point cloud data and autonomous localization results of other agents to be observed through the data receiving module.

[0038] In step S30, the relative poses between agents numbered n and m are denoted as follows: in In a two-dimensional Cartesian coordinate system with the agent itself (numbered n) as the reference frame, the estimated x and y coordinates of the observed agent (numbered m) are the localization estimates. This is the estimated orientation angle of agent m in a two-dimensional Cartesian coordinate system with agent n as the reference frame. Obtaining the relative pose between agents includes the following steps S31–S34:

[0039] S31: Obtain the distance threshold by assigning the agent's own ID to n and the other agent's ID to m; retrieve the point cloud P. m With point cloud P n The maximum laser measurement distance is denoted as l; the larger of the two laser angular resolution values ​​is δ (in radians); assuming the number of dynamic matching attempts is k, k is generally a positive integer between 4 and 10. α is the distance threshold scaling parameter, generally a real number greater than 0 and less than 1. A decreasing arithmetic sequence is constructed as the distance threshold, with k terms, the first term being αklδ, the last term being αlδ, and a common difference of -αlδ. This arithmetic sequence is denoted as {a i (i = 1...k).

[0040] S32: Calculate the coordinate system transformation relationship between the two mobile agents based on their autonomous localization results, and transform the point cloud P... m Transformed into point cloud P′ m Point cloud P m In the intelligent agent coordinate system numbered m, each cluster of point cloud data consists of several points, and each point contains data in two dimensions, x and y. The coordinates of one of these data points are used as the reference point. For example, we have: point cloud P m The coordinate expression of each point in the agent coordinate system numbered n is:

[0041]

[0042] The above equation represents the transformation relationship of the point cloud between two different agent coordinate systems; where A is a two-dimensional rotation matrix, i.e. It is a two-dimensional rotation matrix with rotation angle β.

[0043] S33: Calculate point cloud P n To point cloud P′ m The registration matrix is ​​obtained. This is performed in k steps, where k has the same meaning as described in S31. First, let P... n,0 =P n For a positive integer i (i = 1...k), the threshold for ICP matching is set to a in the i-th step. i Perform ICP matching, where the point cloud to be registered is P. n,i-1 (For i = 1, P) n,i-1 =P n,0 =P n For i = 2...k, P n,i-1 The point cloud after the transformation in the previous step (i.e., the (i-1)th step) is matched to obtain a rigid body transformation result that is a linear rigid body transformation matrix tran. i P n,i-1 The transformed point cloud data is denoted as P. n,i (For i = 1...k-1, P) n,i This is precisely the next step, i.e., the point cloud to be registered in step i+1. Therefore, the final transformed point cloud P′... n The expression is P′ n =P n,k The expression for the rigid body transformation matrix totaltran of the registration result is: The meaning of ∏ is cumulative multiplication.

[0044] Specifically, for two-dimensional point cloud matching, any linear rigid body transformation matrix (denoted as conv) is a 3x3 matrix containing information of 3 degrees of freedom, which can be written as... The 2x2 submatrix in the upper left corner is the two-dimensional rotation matrix with rotation angle β, representing the rotation angle of this two-dimensional rigid body transformation. x t y These represent the translations in the x and y directions of the two-dimensional rigid body transformation, respectively. For any point in this two-dimensional Cartesian coordinate system, the coordinates are denoted as... The 2D rigid body transformation conv acts on this point. The meaning above is to transform the coordinates of this point to It is worth emphasizing that the multiplication operation of linear rigid body transformation matrices has closure property, meaning that the product of any two linear rigid body transformation matrices is still a valid linear rigid body transformation matrix. In this section, the point cloud registration result tran output from k steps... i (i = 1...k) are all linear rigid body transformation matrices. Expression The given total registration result, totaltran, is also a linear rigid body transformation matrix.

[0045] S34: totaltran is a 3x3 linear rigid body transformation matrix, denoted as:

[0046]

[0047] Based on this linear rigid body transformation matrix, three elements Δx, Δy, and Δθ are defined:

[0048] Δx = totaltran 1，3 ; Δy = totaltran 2，3 ;

[0049]

[0050] Where Δθ is the rotation angle of the two-dimensional rigid body transformation totaltran. Δx and Δy represent the translation amounts of the two-dimensional rigid body transformation totaltran in the x and y directions, respectively.

[0051] Based on this, because point cloud P n After the S33 calculation and registration process, it was transformed into a point cloud P′. n Therefore, each two-dimensional data point in both corresponds one-to-one. Point cloud P n Coordinates of a data point For example, the coordinates obtained by applying Totaltran to this point are the point cloud P′. n The corresponding data points The coordinates. The expression is:

[0052] The above equation is combined with the expression in S32:

[0053] have to:

[0054]

[0055] Obtain the relative pose between the two agents

[0056] Step S40, based on S30, uses the point cloud P′ m 、P′ n The reliability of the registration results is evaluated based on the relationship between the two parameters, reflecting the reliability of the relative pose observation results (including the overlap index Ov and the linearity index Li, both of which are real numbers between 0 and 1). The overlap index (Ov) addresses the issue of poor performance of ICP-based point cloud registration when the overlap is low, while the linearity index (Li) addresses the problem of the ICP method easily getting trapped in local optima when the effective point cloud has strong linearity. For example... Figure 3 As shown, it can be completed in steps S41 to S45:

[0057] S41: Transfer the point cloud P′ from module S30 m (Target Point Cloud) and Point Cloud P′ n (Point cloud after registration transformation) is preserved, and P′ is extracted using the K-Dimension Tree method. n Each member point in P′ m Find the nearest point in the array and store that distance as P′. n Find the nearest neighbor distance of the member point. Extract P′. n Calculate the nearest neighbor distances of all member points in the array and generate a new distance array dis_vec.

[0058] S42: Calculate the effective distance threshold;

[0059] like Figure 4 As shown, singular values ​​in the nearest neighbor distance array dis_vec are cleared (in practice, the distance is in klδ).

[0060] The above are singular values ​​(where k, l, and δ have the same meanings as described in S31). The remainder is the valid part of the array, called dis_vec. val For the array dis_vec val Clustering methods (such as 2-means clustering) are used to divide the data into two classes, and the boundary value d between the two classes is the effective distance threshold to be obtained.

[0061] S43: Consider the point cloud P′ n For each member point in the array, if its nearest neighbor distance does not exceed d, it is retained; otherwise, it is removed. n All the retained member points in the cluster form a point cluster P″ n .

[0062] S44: Calculate the point cluster P″ n Quantity and point cloud P′ n The ratio of the number of midpoints is output as the overlap index (Ov).

[0063] S45: Point cluster P″ n All points are subjected to linear regression analysis, and the absolute value of their linear correlation coefficient is output as the linear index (Li).

[0064] In step S50, based on the outputs of S30 and S40, a complete relative pose observation result with reliability evaluation metrics (including overlap and linearity metrics) is obtained. This result can be fused with the independent self-localization result in S10 to obtain a more accurate and reliable cooperative localization result. This result can also be applied to other functions of the multi-agent system. The specific steps are as follows: S51-S53:

[0065] S51: Pack and store the localization results of other agents obtained in S20, the relative pose relationships output in S30, and the overlap and linearity indices output in S40.

[0066] S52: Provide the data packets from S51 to the autonomous positioning module in S10. Set the threshold Ov for the overlap index. thr With the threshold Li of the linear index thr (Both are real numbers between 0 and 1). If both the overlap index and the linearity index in step S40 meet the requirements (overlap index greater than or equal to the specified overlap index threshold and linearity index less than or equal to the specified linearity threshold), that is, when Ov ≥ Ov thr And Li≤Li thr At that time, the autonomous localization module will use the data packets in S51 and a general fusion filtering algorithm to optimize the autonomous localization results as the final cooperative localization output.

[0067] S53: Through the data communication module, the data packets in S51 are provided to the central server or other mobile intelligent agents to support the relevant functions of the multi-agent system.

[0068] This invention provides a mobile intelligent agent relative pose collaborative acquisition and application technology. Utilizing motion sensors and environmental perception settings, it ensures independent and relatively accurate autonomous positioning even during low-speed movement of a single intelligent agent. Based on data sharing between intelligent agents, it employs a dynamic threshold point cloud matching method, leveraging environmental information from a quasi-enclosed indoor environment to calculate relative pose observations. A result evaluation module further assesses the reliability of the relative pose observation results. Relative pose observation can also influence the autonomous positioning module, with the two complementing each other to improve performance. Motion sensors, environmental perception devices, and data transmission modules are all commonly installed devices in mobile intelligent agents. This invention requires no additional hardware, achieving accurate and robust relative pose acquisition and application. It has application value for multi-agent systems and meets the practical requirements of simple deployment, flexible and reliable operation, and low cost.

[0069] It should be noted that the purpose of disclosing the embodiments is to help further understand the present invention. However, those skilled in the art will understand that various substitutions and modifications are possible without departing from the spirit and scope of the present invention and the appended claims. Therefore, the present invention should not be limited to the content disclosed in the embodiments, and the scope of protection of the present invention is defined by the scope of the claims.

Claims

1. A method for cooperative acquisition of relative poses of mobile agents by dynamic threshold point cloud matching, using shared laser point cloud data and autonomous positioning results, employing an improved iterative closest point (ICP) registration algorithm to calculate the relative poses between mobile agents, and evaluating the reliability of the calculation results to further improve the observation reliability and application value of the relative poses without affecting the original layout of the system and without relying on other feature-enhancing hardware. Comprising the following steps: 1) According to the data collected by the sensors on each mobile agent, the autonomous positioning results of each mobile agent are obtained respectively; 2) The autonomous positioning results of each mobile agent obtained in the previous step are time-stamped and re-encapsulated with the laser point cloud data of the mobile agent itself; each mobile agent shares data through network communication and obtains the real-time autonomous positioning results and laser point cloud data of other agents; 3) Dynamic threshold point cloud matching is performed to obtain the relative pose between agents; According to the shared laser point cloud data and the autonomous positioning result, the point cloud P of the observed intelligent agent m Through coordinate system transformation, the target point cloud P' to be registered is obtained m ; The intelligent agent point cloud P of the self n Step-by-step ICP point cloud registration based on a dynamic threshold is performed, and the point cloud P' is obtained after performing a rigid body transformation n ; The relative pose observation result between the two intelligent agents is output; including the following steps: S31: Obtain the distance threshold, the method is: take the observed agent point cloud P m The maximum laser measurement distance of the self-agent point cloud P n is recorded as l; the value of the larger laser angle resolution of the two is δ; a set of decreasing arithmetic sequences is constructed as the distance threshold, and the arithmetic sequence is recorded as {a i}, i = 1…k, wherein the term number k is also the dynamic matching times; the first term of {a i} is αklδ, the last term is αlδ, and the tolerance is -αlδ; α is the distance threshold scaling parameter; S32: Calculate the coordinate system transformation relationship between the two mobile agents through the autonomous positioning results of the two mobile agents; transform the point cloud P m into the point cloud P' m ; In the agent coordinate system numbered m, each point in the point cloud P m contains data in two dimensions of x direction and y direction, and the point coordinates are represented as P m Each point coordinate in the agent coordinate system numbered n is represented as: The above formula represents the transformation relationship of the point cloud between two different agent coordinate systems; wherein A is a two-dimensional rotation matrix, i.e. is a two-dimensional rotation matrix with a rotation angle of β; the autonomous positioning result of the agent numbered n is wherein is the horizontal coordinate and the vertical coordinate of the positioning estimation of the agent numbered n in the two-dimensional Cartesian coordinate system of the real environment, is the orientation angle estimation value of the agent numbered n in the two-dimensional Cartesian coordinate system of the real environment; the autonomous positioning result of the agent numbered m is wherein is the horizontal coordinate and the vertical coordinate of the positioning estimation of the agent numbered m in the two-dimensional Cartesian coordinate system of the real environment, is the orientation angle estimation value of the agent numbered m in the two-dimensional Cartesian coordinate system of the real environment; S33: calculating the point cloud P in k steps n to the point cloud P m registration matrix of the point cloud P; comprising: First, let P n,0 = P n ; For positive integers i, i = 1…k, the threshold for ICP matching is set to a in the i-th step. i Perform ICP matching, where the point cloud to be registered is P. n,i-1 The rigid body transformation result obtained from the matching is a linear rigid body transformation matrix tran. i P n,i-1 The transformed point cloud data is denoted as P. n,i The point cloud P is obtained after transformation. n ′;P n ′=P n,k The result after registration is a two-dimensional linear rigid body transformation matrix totaltran. Where ∏ represents cumulative multiplication; S34: totaltran is a 3x3 linear rigid body transformation matrix, expressed as: Define three elements Δx, Δy, Δθ: Δx = totaltran 1，3 ; Δy = totaltran 2，3 ; Where Δθ is the rotation angle of the two-dimensional rigid body transformation totaltran; Δx, Δy represent the translation amount of totaltran in the x / y direction respectively; For the data point coordinates in point cloud P n totaltran acts on the data point to obtain the coordinates of the data point corresponding to the data point in point cloud P totaltran acts on the data point to obtain the coordinates of the data point corresponding to the data point in point cloud P n totaltran acts on the data point to obtain the coordinates of the data point corresponding to the data point in point cloud P totaltran acts on the data point to obtain the coordinates of the data point corresponding to the data point in point cloud P The above equation is coupled with the expression in S32: obtained: obtaining a relative pose between two agents numbered n, m wherein is an estimate of the lateral and longitudinal coordinates of the position estimate of the agent numbered m in a two-dimensional Cartesian coordinate system with the agent numbered n as the reference frame; is an estimate of the orientation angle of the agent numbered m in a two-dimensional Cartesian coordinate system with the agent numbered n as the reference frame; 4) design a credibility evaluation index to evaluate the credibility of the matching result between the point cloud P m ′ and the point cloud P n ′; the credibility evaluation index includes an overlap index and a linearity index; the evaluation process includes S41-S45: S41: The point cloud P m ' is retained, and the nearest distance point of each member point in P n ' is extracted using the multi-dimensional space partition tree method, and the distance is stored as P n ' The nearest neighbor distance of the member point in P m ' is stored as P n ' The nearest neighbor distance of all member points in P n ' is extracted, and a new distance array dis_vec is generated; S42: Calculate the effective distance threshold; The singular values in the recent neighbor distance array dis_vec are removed, and the remaining part of the array is called dis_vec val ; The array dis_vec val According to the value, it is divided into two categories, and the boundary value d between the two categories is the effective distance threshold value. S43: Consider each member point in the point cloud P n and if its nearest neighbor distance is not more than d, then retain it, otherwise discard it; P n All retained member points in P n "; S44: Calculate point cluster P n The ratio of the number of points in the point cloud P n The ratio of the number of points in the point cloud P S45: To the point cluster P n The absolute value of the linear correlation coefficient of the linear regression analysis of all points in the middle is the linear index; through the above steps, the relative pose of the mobile agent with the dynamic threshold point cloud matching is obtained, and the relative pose observation result of the mobile agent with the credibility evaluation index is obtained.

2. The collaborative acquisition method for relative pose of mobile intelligent agents based on dynamic threshold point cloud matching as described in claim 1, characterized in that, By setting the overlap index threshold and the linear index threshold, when the overlap index in the credibility evaluation index of the relative pose matching result of the mobile agent is greater than or equal to the overlap index threshold and the linear index is less than or equal to the linear index threshold, the mobile agent relative pose observation result with the credibility evaluation value is applied to the autonomous positioning of the mobile agent, and the autonomous positioning result of the mobile agent is optimized through the fusion filtering algorithm.

3. The collaborative acquisition method for relative pose of mobile intelligent agents based on dynamic threshold point cloud matching as described in claim 1, characterized in that, The mobile agent is equipped with corresponding motion sensors and environmental perception devices to form an autonomous positioning module with decimeter-level accuracy; and through the installation of a data communication module, different mobile agents share data through network connection.

4. The collaborative acquisition method for relative pose of mobile intelligent agents based on dynamic threshold point cloud matching as described in claim 3, characterized in that, The motion sensor includes an inertial measurement unit and a wheel speed meter; the environmental perception device includes a laser radar.

5. The method for collaborative acquisition of relative pose of mobile intelligent agents by dynamic threshold point cloud matching as described in claim 1, characterized in that, In step S42, when the singular values in the nearest neighbor distance array dis_vec are removed, data with a distance of klδ or more are considered as singular values.

6. The method of claim 1, wherein the method further comprises: Step S4 2. In particular, the array dis_vec val The two classes are separated using a clustering method; the clustering method can use 2-means clustering.

7. The method for collaborative acquisition of relative pose of mobile intelligent agents by dynamic threshold point cloud matching as described in claim 1, characterized in that, According to the method, a mobile agent relative pose collaborative acquisition system based on dynamic threshold point cloud matching is realized, including an autonomous positioning module, a data communication module, an ICP registration relative pose acquisition module, and a pose relationship credibility evaluation module; The autonomous positioning module is used for each mobile agent to obtain data through its own environmental perception device and motion sensor, and to obtain independent positioning results of its own position estimation by using filtering and optimization algorithms; The data communication module is used for different mobile agents to share data through network connection; The ICP registration relative pose acquisition module is used to obtain the relative pose of different mobile agents; The pose relationship credibility evaluation module is used to design a credibility evaluation index for the obtained relative pose data to test its reliability.

8. The method of claim 7, wherein the method further comprises: determining the relative pose of the mobile agent based on the dynamic threshold point cloud matching. The data communication module includes a data sending module and a data receiving module; each mobile agent shares its laser point cloud data and autonomous positioning result through the data sending module; and obtains the laser point cloud data and autonomous positioning result of the observed agent through the data receiving module. ​

Images