A Multi-Agent Networked Cooperative Localization and Tracking Method Based on Pose Robust Association

Abstract

This invention discloses a multi-agent networked cooperative localization and tracking method based on pose robust association, belonging to the field of multi-agent cooperative localization technology. The system includes an autonomous localization module, a wireless transmission module, a relative pose acquisition module, an observation selectivity module, and a cooperative localization optimization module. Different mobile agents with autonomous localization capabilities share data, including real-time autonomous localization results and laser point cloud data, through wireless network communication. Multi-agent networked cooperative localization is modeled by constructing a multi-agent pose factor graph. Robust pose association of multiple agents is achieved using improved point cloud registration and observation selectivity methods, thereby realizing efficient, accurate, robust, and deployable multi-agent cooperative localization and tracking.

Description

Technical Field

[0001] This invention belongs to the field of multi-agent cooperative localization technology, specifically relating to a multi-mobile agent networked cooperative localization and tracking method based on pose robust association. Background Technology

[0002] With the rapid development of smart hardware and information processing technologies, mobile intelligent agents equipped with sensing sensors are playing a significant role in fields such as autonomous driving, industrial manufacturing, and logistics. Accurate positioning and tracking are crucial foundations for enabling mobile intelligent agents to perform higher-level sensing tasks. Traditional positioning technologies typically achieve positioning through GPS / BeiDou signal processing; however, traditional satellite positioning methods have limited accuracy and cannot be widely applied to indoor or signal-obstructed scenarios, significantly restricting the mobility of mobile intelligent agents. Simultaneous Localization and Mapping (SLAM), utilizing the mobile intelligent agent's own sensing information, enables autonomous positioning without relying on external information, and is currently a key technology for mobile intelligent agent positioning.

[0003] In real-world intelligent agent operation scenarios, the operating environment is complex and the operating range is large. A single intelligent agent is susceptible to problems such as line-of-sight obstruction and accumulated errors, leading to a decrease in positioning accuracy. Due to the increasing scale of production operations, real-world systems often involve a large number of mobile intelligent agents. By designing a multi-mobile-agent collaborative positioning and tracking scheme, based on efficient communication and interaction between agents, the limitations of single-agent positioning can be effectively overcome, achieving accurate collaborative positioning in large-scale environments. Therefore, it is necessary to design an efficient and robust collaborative positioning and tracking method.

[0004] Existing research on multi-agent cooperative localization algorithms largely remains at the theoretical level of multi-source information fusion. These algorithms make strong theoretical assumptions about multi-agent cooperation and neglect the impact on actual system deployment. Multi-agent cooperative localization and tracking faces two key challenges: first, how to obtain relative pose information among multiple agents, i.e., obtaining pose correlations between agents by jointly processing sensor information from multiple agents; and second, how to collaboratively optimize the localization pose results of multiple agents, i.e., improving the localization accuracy of the agent system by fusing self-localization information and relative pose information from multiple agents. For the relative pose acquisition problem, existing solutions typically use laser point cloud matching algorithms to align the point clouds of different agents to infer the relative poses between them. However, existing point cloud matching algorithms do not address the issue of partial overlap in point clouds during multi-agent cooperative operations, making it difficult to achieve good point cloud matching performance. For the multi-agent cooperative localization optimization problem, existing solutions do not fully consider the robustness of multi-source information fusion, introducing all multi-agent correlation information into the localization optimization process, which easily introduces erroneous localization results and causes a significant decrease in localization accuracy. Therefore, considering the above reasons, existing solutions are unable to achieve efficient and robust multi-agent cooperative localization and tracking schemes, and cannot effectively support the practical application of multi-agent cooperative systems. Summary of the Invention

[0005] To overcome the shortcomings of the existing technologies, this invention proposes a multi-mobile agent networked cooperative localization and tracking method based on pose robust association. The cooperative localization problem is modeled based on the pose factor graph of the multi-agent agent, and the pose robust association of the multi-agent agent is realized by using an improved point cloud registration algorithm and an observation selectivity mechanism. This enables efficient, accurate, robust, and deployable multi-mobile agent cooperative localization and tracking.

[0006] This invention relates to a networked cooperative localization and tracking scheme for multiple mobile intelligent agents based on pose robust association, applicable to multi-agent systems. The scheme includes an autonomous localization module, a wireless transmission module, a relative pose acquisition module, an observation selectivity module, and a cooperative localization optimization module. 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. A wireless transmission module is also provided under network conditions to support information sharing. A point cloud registration algorithm based on dynamic thresholds is implemented using shared laser point cloud information and autonomous localization results to calculate the relative pose of the localization nodes. The observation selectivity module filters the established pose constraints, retaining those that meet consistency constraints. Based on the autonomous localization results, intra-agent pose constraints, and inter-agent pose constraints, the cooperative localization optimization module performs global optimization of the multi-agent localization results. This method can be applied to multi-agent systems operating at low speeds in quasi-enclosed indoor environments without affecting the original system layout or relying on other external information or devices, thus achieving efficient and robust multi-agent collaborative positioning and tracking.

[0007] To achieve the above objectives, this invention requires that the mobile intelligent agent be equipped with environmental perception equipment (including a necessary lidar), enabling it to autonomously locate itself. Furthermore, it is equipped with wireless communication equipment, allowing different intelligent agents to share data via a network. Each mobile intelligent agent obtains real-time autonomous location results and lidar point cloud data from other intelligent agents through network communication. The specific implementation scheme for multi-agent collaborative localization and tracking is as follows:

[0008] 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.

[0009] 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).

[0010] 3) Based on the shared laser point cloud data and autonomous localization results in 2), the relative poses between localization nodes of the multi-agent system are calculated using the stepwise ICP (Iterative Closest Point) point cloud registration method based on dynamic thresholds.

[0011] 4) The relative pose observation results obtained in 3) are tested by observation selectivity algorithm. This algorithm judges whether the relative pose observation results are reasonable and reliable by checking the consistency conditions between the relative pose observation results and the prior assumptions, and avoids the introduction of erroneous relative pose observation results.

[0012] In specific implementation, based on the agent's own pose information, the pose information of surrounding agents, and relative pose information, multiple relative pose constraint sequences are established. An observation-selective algorithm is used to verify the consistency between the agent's internal pose constraints and the pose constraints between agents. Relative poses that meet the consistency constraints are obtained through screening. The observation-selective algorithm includes:

[0013] Establish intelligent agent t k Self-relative pose constraint sequence at time step To perform a consistency check, the pose constraints are expressed as follows:

[0014]

[0015] Where |x| represents the absolute value of the result after subtracting the three-dimensional pose vectors, ∈ is the three-dimensional threshold vector of the consistency pose constraint within the agent, and corresponds to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system, respectively.

[0016] The final agent t that satisfies the above pose constraints is obtained. k Self-relative pose constraint sequence at time step

[0017] Establish intelligent agent t k The sequence of pose constraints between time and other agents To perform a consistency check, the pose constraints are expressed as follows:

[0018]

[0019] Where δ is a three-dimensional threshold vector for the consistency pose constraint among agents, corresponding to the horizontal and vertical coordinates and orientation in a two-dimensional Cartesian coordinate system, respectively, and its magnitude is set to a fixed threshold; pose constraints that satisfy the above conditions are retained to form the final agent t. k The sequence of pose constraints between time and other agents

[0020] 5) Using the autonomous localization results obtained in 1) and the relative pose verified by the observation-selective algorithm in 4), construct a multi-agent pose factor graph optimization model, perform multi-source information fusion, and optimize the solution to obtain the cooperative localization pose results.

[0021] Distributed optimization solution, represented as:

[0022]

[0023] Among them, the first item This represents the self-localization pose optimization factor from time t1 to time t2. T Summation; the second term This indicates that at each time t k Summing the self-relative pose optimization factors associated with the pose at that moment, and then summing them over time; the third term This indicates that at each time t k The relative pose optimization factors between agent j and its associated agents are summed, then summed according to agent number, and finally summed in the time dimension.

[0024] By optimizing and solving the multimodal multi-agent pose factor graph optimization model, global optimization localization of multi-agent multimodal information in both temporal and spatial dimensions can be achieved.

[0025] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0026] This invention provides a multi-agent networked cooperative localization and tracking scheme based on pose-robust correlation. Addressing two major problems in multi-agent cooperative localization, it proposes improved point cloud registration and observation-selective algorithms. This achieves efficient and robust multi-agent cooperative localization and tracking without relying on external information or devices. The proposed observation-selective algorithm performs consistency checks on numerous pose constraints in the multi-agent system, thereby avoiding the introduction of erroneous pose constraints into the global localization optimization of the multi-agent system, ensuring the accuracy and robustness of multi-agent cooperative localization.

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

[0028] (i) Based on wireless communication networks, information interaction among multiple mobile intelligent agents is carried out to build a multi-agent cooperative positioning and tracking system, which solves the problem of positioning drift and mapping error of single agents in large-scale environments in practical applications and improves the efficiency of cooperative operation of multi-agent systems.

[0029] (ii) Considering that the sensor data of intelligent agents for relative pose coordination in indoor scenes only partially overlap due to factors such as viewpoint occlusion, the existing point cloud registration algorithm is specifically improved by using dynamic threshold and stepwise registration strategies to make it suitable for laser point cloud registration between intelligent agents in indoor environments. It can output accurate registration results, thereby realizing the effective acquisition of relative pose between intelligent agents and providing convenience for the collaborative positioning optimization of intelligent agent systems.

[0030] (III) Considering the numerous pose constraints in a multi-agent system, including agent self-localization pose constraints, relative pose constraints within agents, and relative pose constraints between agents, the observation selectivity algorithm is used to perform consistency checks on pose constraints. For pose constraints that meet the check conditions, a multi-agent pose factor graph model is constructed to realize distributed localization optimization that considers global information in the time and space dimensions, supporting efficient, robust, and deployable multi-agent collaborative localization optimization. Attached Figure Description

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

[0032] Figure 2 This is a flowchart of the point cloud registration module algorithm based on dynamic threshold of the present invention.

[0033] Figure 3 This is a flowchart of the observation selectivity module of the present invention. Detailed Implementation

[0034] 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.

[0035] refer to Figure 1 This invention requires each mobile agent in the system to be equipped with an environmental perception device (at least including LiDAR) for autonomous localization in quasi-enclosed indoor environments. Based on the wireless transmission module sharing the autonomous localization results and LiDAR point cloud information, a relative pose acquisition module performs matching calculations on the LiDAR point clouds of multiple agents to obtain relative pose results. An observation selection module performs consistency checks on the relative pose results. Finally, a collaborative localization optimization module uses the self-localization results and the selected relative pose results for global optimization, achieving multi-agent collaborative localization optimization.

[0036] This invention incorporates a multi-agent cooperative localization and tracking device within a mobile intelligent body. This device includes an autonomous localization module that calculates the agent's own localization result and supports subsequent cooperative optimization processing; a wireless transmission module for supporting necessary data interaction under network conditions; a relative pose acquisition module; an observation selectivity module; and a cooperative localization optimization module. The autonomous localization module includes environmental sensing devices. The specific steps are as follows:

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

[0038] The agent processes sensor information via an autonomous localization module mounted on it, continuously acquiring its own self-localization information. The pose information (including position and orientation) and pose estimation variance information estimated by the agent's self-localization, together with the sensor information at the corresponding time, form a data packet.

[0039] S20: The agent shares self-localization results with laser point cloud data;

[0040] The multi-agent system sends key frame data packets via a wireless transmission module. The data packets include timestamps, agent numbers, self-localization estimates and variances of self-localization estimates (S10), and perception information. Simultaneously, it receives data packets from agents within the communication range to achieve the sharing of self-localization information.

[0041] S30: The agent calculates the relative poses between keyframe nodes;

[0042] The multi-agent system establishes keyframe pairs to be matched by combining its own pose information with keyframe data received from other agents, and uses an improved dynamic threshold point cloud matching algorithm to obtain the relative pose results between keyframe nodes.

[0043] S40: The agent verifies the relative pose results using an observational selectivity algorithm to obtain a consistent relative pose;

[0044] The multi-agent system performs consistency checks on the relative pose acquisition module's generated relative pose results, and adopts different check strategies for the relative pose constraints within the agent and the relative pose constraints between agents. The system retains the relative pose results that meet the check conditions for collaborative localization optimization.

[0045] S50: The agent constructs a multi-agent pose factor graph from its own relative pose data with other agents and performs global optimization.

[0046] The collaborative localization optimization module utilizes various relative pose data from itself and other agents to construct a multi-agent multimodal factor graph, further performs multi-source information fusion, optimizes the solution to obtain pose estimation values, and constructs a global map based on the optimized pose estimation values.

[0047] In step S10: Each agent obtains its own pose information (three-dimensional, including the horizontal and vertical coordinates and orientation angle in a two-dimensional Cartesian coordinate system), and provides the estimated variance for each pose dimension. The specific process is as follows: Agent i (hereinafter referred to as Agent i) performs inter-frame feature matching or frame-map feature matching based on its own laser point cloud data, thus obtaining the pose information of Agent i in step t. k The three-dimensional pose information at time t is represented as The variance of pose estimation is expressed as: It is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in a two-dimensional Cartesian coordinate system, and the corresponding laser point cloud data is represented as follows.

[0048] In step S20: The agent transmits its own pose information and corresponding sensor information via wireless communication methods such as WIFI and 4 / 5G. This step includes the following processes S21 to S22:

[0049] S21: The intelligent agent will place the autonomous localization module in t k Pose information at any moment Variance of pose estimation Laser point cloud data Agent ID i and timestamp t k The data is packaged into keyframe data packets and broadcast to surrounding intelligent agents in a one-to-many manner at a certain period to share its own state information and laser point cloud data.

[0050] S22: Receive keyframe data packets sent by surrounding agents to obtain the pose state and sensor data of surrounding agents. Agent j (j≠i) is in t m Pose state information at any given time Variance of pose estimation Laser point cloud data

[0051] In step S30: The agent uses its own keyframe pose information (S10) and the received keyframe pose information of other agents (S20) to establish a pair of keyframe nodes to be matched, and the laser point cloud P from the target keyframe is used to... m By transforming the coordinate system, we obtain the point cloud P′ to be registered. m Point cloud P from candidate keyframes n After performing stepwise ICP point cloud registration based on dynamic threshold and rigid body transformation, point cloud P′ is obtained. n Then, the relative pose calculation results between the two frames are obtained, and the specific process is as follows: Figure 2 As shown.

[0052] In step S30, the autonomous localization result of candidate keyframe A is recorded as follows: in These are the estimated horizontal and vertical coordinates of agent 'a' in the two-dimensional Cartesian coordinate system of the real environment at the corresponding moment in this frame. This is the agent's estimated orientation angle. Similarly, the autonomous localization result of target keyframe B is recorded as... in These are the estimated horizontal and vertical coordinates of agent b in the two-dimensional Cartesian coordinate system of the real environment at the corresponding moment in this frame. This is the agent's orientation angle estimate. The relative pose between candidate keyframe A and target keyframe B is denoted as... in In a two-dimensional Cartesian coordinate system with agent a at the time corresponding to candidate keyframe A as the reference frame, the x and y coordinates of agent b at the time corresponding to target keyframe B are estimated values. This is the estimated orientation angle. Obtaining the relative pose between keyframes includes the following steps S31 to S34:

[0053] S31: Obtain the distance threshold by taking 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).

[0054] S32: Calculate the coordinate system transformation relationship between the two 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 original coordinate system, 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 coordinates of each point in the point cloud P n The coordinate expression in the coordinate system is:

[0055]

[0056] This represents the transformation relationship of the point cloud between coordinate systems; where R is a two-dimensional rotation matrix, i.e. It is a two-dimensional rotation matrix with rotation angle β.

[0057] 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 integer i (i = 1…k), the threshold set in the i-th step is a. 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-1The 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 the (i+1)th step. Then 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: 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 In this stage, the point cloud registration results tran output by k steps are... i (i = 1…k) are all linear rigid body transformation matrices. Expression The registration result totaltran is also a linear rigid body transformation matrix.

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

[0059]

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

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

[0062]

[0063] Based on this, due to the one-to-one correspondence between the registered two-dimensional data points, that is:

[0064] The above equation is combined with the expression in S32: have to

[0065]

[0066] Thus, the calculation results of the relative pose are obtained.

[0067]

[0068] In step S40: Based on its own pose information, the pose information of surrounding agents, and relative pose information (S30), the agent establishes various relative pose constraints, and uses an observation selectivity algorithm to filter the pose constraints within the agent and the pose constraints between agents, such as... Figure 3 As shown, this step includes the following processes S41 to S44:

[0069] S41: The agent extracts t based on its own historical pose information. k Self-localization pose information of agent i at any given moment Self-pose information within a certain range, i.e. Where ||x||2 represents the Euclidean distance calculated from the two-dimensional translation information in the pose information. express The self-localization pose information of agent i at time n, where d is the distance range threshold, and n is the position of agent i. l (l = 0, 1, ... L) i To determine the pose number of itself that meets the above conditions, L i The number of poses required to satisfy the above conditions is obtained through the relative pose acquisition module. Time and t k Relative pose transformation at time t. and relative pose transformation estimation variance It is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in a two-dimensional Cartesian coordinate system, respectively, to establish the agent t. k Self-relative pose constraint sequence at time step

[0070] S42: For agent t obtained in S41 k Self-relative pose constraint sequence at time step Perform a consistency check as follows:

[0071]

[0072] Where |x| represents the absolute value of the result after subtracting the three-dimensional pose vectors, ∈ is the three-dimensional threshold vector of the agent's internal consistency pose constraint, corresponding to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system, respectively. Its magnitude is related to the accumulation of the agent's self-localization error, requiring that the relative pose provided by the relative pose acquisition module be consistent with the prior localization error of the autonomous localization algorithm. Retaining the pose constraints that satisfy the above conditions forms the final agent t. k Self-relative pose constraint sequence at time step In practice, the autonomous localization algorithm can adopt the 2D LiDAR localization algorithm Hector SLAM algorithm.

[0073] S43: The agent extracts t based on the pose information received from other agents. k Self-localization pose information at time step The pose information of other intelligent agents within a certain range, i.e. express The self-localization pose information of agent j at time t, where d is the distance range threshold, and n l (l = 0, 1, ..., L) j→i L represents the pose number of the j-th intelligent agent that satisfies the above conditions. j→i Let this be the number of poses of agent j that satisfy the above conditions. (Based on laser point cloud data) and Feature extraction and feature matching are performed to obtain the j-th agent in At time t, the i-th intelligent agent... k Relative pose transformation at time t. and relative pose transformation estimation variance It is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in a two-dimensional Cartesian coordinate system, respectively, to establish the agent t. k The sequence of pose constraints between time and other agents

[0074] S44: For agent t obtained in S43 k The sequence of pose constraints between time and other agents Perform a consistency check as follows:

[0075]

[0076] δ is a three-dimensional threshold vector for the consistency pose constraint among agents, corresponding to the horizontal and vertical coordinates and orientation in a two-dimensional Cartesian coordinate system, respectively, and its magnitude is set to a fixed threshold. The pose constraints that satisfy the above conditions are retained to form the final agent t. k The sequence of pose constraints between time and other agents

[0077] S50: The agent is modeled as a multi-agent pose factor graph model based on the various pose constraints obtained in S40. The nodes in the factor graph represent the pose variables of the multi-agent to be optimized, and the edges in the factor graph represent the relative pose constraints between pose nodes. By solving the factor graph optimization problem, the globally optimized localization pose and environment map are obtained. This step includes the following processes S51 to S56:

[0078] S5 1: Construct the pose sequence to be optimized for the i-th agent as follows T represents the i-th intelligence.

[0079] The body generates a total of T self-localization pose results;

[0080] S52: The pose sequence obtained from S10 is as follows Self-localization pose error is expressed as:

[0081] Further expressed as a self-localization pose optimization factor

[0082] S53: Obtain the agent's relative pose constraint from S41. The pose constraint error is expressed as: Further expressed as the relative pose optimization factor within the agent.

[0083] S54: Obtain the relative pose constraints between agents based on S42. This pose error is expressed as Further expressed as the relative pose optimization factor between agents

[0084] S55: Based on the four optimization factors obtained from S52 to S54, the multi-agent cooperative localization optimization problem assisted by multimodal information can be modeled as the following nonlinear least squares problem. Using a general nonlinear least squares optimization solution method, such as the Gaussian-Newton method, the optimized pose sequence of the i-th agent can be obtained. Other agents can perform distributed optimization solutions according to the same process, as follows:

[0085]

[0086] First item This represents the self-localization pose optimization factor from time t1 to time t2. T Summation; the second term This indicates that at each time t kSumming the self-relative pose optimization factors associated with the pose at that moment, and then summing them over time; the third term This indicates that at each time t k The relative pose optimization factors between agent j and its associated agents are summed, then summed according to agent number, and finally summed in the time dimension. By optimizing the problem composed of the above multimodal optimization factors, global optimization localization of multi-agent multimodal information in the time and space dimensions can be achieved.

[0087] S56: Optimized pose sequence obtained based on S55 Sensor data The sequence is projected and stitched according to the optimized pose at the corresponding time to construct an optimized environmental map. The optimized pose is fed back to the autonomous localization module, which continues to perform autonomous localization pose estimation based on the global optimization results.

[0088] This invention provides a multi-agent cooperative localization and tracking technology. It utilizes the sensing devices equipped on the agents themselves to ensure independent and relatively accurate autonomous localization even during low-speed movement of a single agent. Based on data sharing among agents, it employs a dynamic threshold point cloud matching method and leverages environmental information from a quasi-enclosed indoor environment to calculate relative pose observation results. An observation selectivity module removes potentially erroneous relative pose observation results. A cooperative localization optimization module jointly optimizes the localization results of multiple agents with relative pose constraints globally, achieving accurate and robust cooperative localization. Environmental sensing devices and wireless transmission modules are commonly installed on mobile agents; this invention requires no additional hardware and provides an efficient and robust multi-agent cooperative localization and tracking solution, which is of significant value for practical multi-agent production applications.

[0089] 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 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 multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association, characterized in that, Mobile intelligent agents have autonomous positioning capabilities; different mobile intelligent agents share data through wireless network communication, including real-time autonomous positioning results and laser point cloud data of the agents; Multi-agent cooperative localization is modeled by constructing a pose factor map of multiple mobile agents. Robust association of multi-agent poses is achieved using an improved point cloud registration algorithm and observation selectivity method, thereby realizing multi-agent cooperative localization and tracking. The steps include: 1) The mobile intelligent agent acquires autonomous localization information, including pose information estimated by the agent's self-localization, pose estimation variance information, and sensor information at the corresponding time. 2) Intelligent agents share self-localization information and laser point cloud data; 3) Based on the shared laser point cloud data and autonomous positioning information in step 2), the relative poses between the positioning nodes of the multi-agent system are calculated using the iterative nearest point registration method (ICP) based on dynamic thresholds. The multi-agent system establishes keyframe pairs to be matched by its own pose information and the keyframe data received from other agents, and uses an improved dynamic threshold point cloud matching algorithm to obtain the relative pose results between keyframe nodes. 4) Based on the pose information of the agent itself, the pose information of surrounding agents and the relative pose information, establish multiple relative pose constraint sequences. Use the observation selectivity algorithm to check the consistency between the pose constraints within the agent and the pose constraints between agents. Detect and filter to obtain the relative poses that meet the consistency constraints. Observation selectivity algorithms include: Establish the i-th intelligent agent t k Self-relative pose constraint sequence at time step To perform a consistency check, the pose constraints are expressed as follows: Where |x| represents the absolute value of the result after subtracting the three-dimensional pose vectors, ∈ is the three-dimensional threshold vector of the consistency pose constraint within the agent, corresponding to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system, respectively; i represents the i-th agent; n l The pose number that satisfies the above conditions; L i The number of poses required to satisfy the above conditions; The final agent t that satisfies the above pose constraints is obtained. k Self-relative pose constraint sequence at time step Establish intelligent agent t k The sequence of pose constraints between time and other agents Perform a consistency check, where j represents the j-th agent; the pose constraint is expressed as: Where δ is a three-dimensional threshold vector for the consistency pose constraint among agents, corresponding to the horizontal and vertical coordinates and orientation in a two-dimensional Cartesian coordinate system, respectively, and its magnitude is set to a fixed threshold; pose constraints that satisfy the above conditions are retained to form the final agent t. k The sequence of pose constraints between time and other agents L j→i The number of poses of agent j that satisfy the above conditions; 5) Using the autonomous localization results obtained in step 1) and the relative poses that meet the consistency test conditions in step 4), a multi-agent pose factor graph optimization model is constructed, and multi-source information is fused. Distributed optimization is performed to obtain pose estimates, and a global map is constructed based on the optimized pose estimates, thereby obtaining the cooperative localization pose results. Distributed optimization solution, represented as: Among them, the first item This represents the self-localization pose optimization factor from time t1 to time t2. T Summation; the second term This indicates that at each time t k Summing the self-relative pose optimization factors associated with the pose at that moment, and then summing them over time; the third term This indicates that at each time t k Summing the relative pose optimization factors between agent j and its associated agents, summing them according to agent number, and finally summing them in the time dimension; By optimizing and solving the multimodal multi-agent pose factor graph optimization model, global optimization localization of multi-agent multimodal information in both temporal and spatial dimensions can be achieved.

2. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 1, characterized in that, In step 2), the intelligent agents share data through wireless network communication, specifically including S21 to S22: S21: The agent will t k Pose information at any moment Variance of pose estimation Laser point cloud data Agent ID i and timestamp t k The data is packaged into keyframe data packets and broadcast to surrounding intelligent agents in a one-to-many manner at a certain period to share its own state information and laser point cloud data. S22: Receive keyframe data packets sent by surrounding agents to obtain the pose state and sensor data of the surrounding agents. Agent j is in t m Pose state information at any given time Variance of pose estimation Laser point cloud data 3. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 2, characterized in that, In step 3), the agent uses its own keyframe pose information and the received keyframe pose information of other agents to establish a pair of keyframe nodes to be matched, and the laser point cloud P of the target keyframe is obtained. m By transforming the coordinate system, we obtain the point cloud P′ to be registered. m Point cloud P of candidate keyframes n Stepwise ICP point cloud registration based on dynamic thresholding is performed, and point cloud P′ is obtained after rigid body transformation. n This leads to the calculation of the relative pose between the two frames.

4. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 3, characterized in that, Obtaining the relative pose between keyframes includes the following steps: S31: Obtain the distance threshold by constructing a decreasing arithmetic sequence as the distance threshold. The number of terms is k, the first term is αklδ, the last term is αlδ, and the common difference is -αlδ. This arithmetic sequence is denoted as {a i }, i = 1…k; where, take 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 resolutions is δ, in radians; the number of dynamic matching times is k, and α is the distance threshold scaling parameter. S32: Calculate the coordinate system transformation relationship between the two agents based on their autonomous localization results, and transform the point cloud P... m Transformed into point cloud P′ m ; S33: Calculate the point cloud P n To point cloud P′ m The registration matrix is ​​a linear rigid body transformation matrix; S34: Based on the linear rigid body transformation matrix and the one-to-one correspondence of the registered two-dimensional data points, the relative pose calculation result is obtained.

5. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 4, characterized in that, The autonomous positioning algorithm specifically employs a two-dimensional lidar positioning algorithm.

6. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 4, characterized in that, In step 4), the intelligent agent t is established. k The specific method for the self-relative pose constraint sequence at each time step is as follows: The agent extracts t based on its own historical pose information. k Self-localization pose information of agent i at any given moment Self-pose information within a certain range, i.e. Where ||x||2 represents the Euclidean distance calculated from the two-dimensional translation information in the pose information. express The self-localization pose information of agent i at time n, where d is the distance range threshold, and n is the position of agent i. l Number your own pose, l = 0, 1, ... L i L i The number of poses; obtained through the relative pose acquisition module. Time and t k Relative pose transformation at time t. and relative pose transformation estimation variance It is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in a two-dimensional Cartesian coordinate system; Establish intelligent agent t k The specific method for determining the pose constraint sequence between other agents at any given time is as follows: Based on the pose information received from other agents, the agent extracts t k Self-localization pose information at time step The pose information of other intelligent agents within a certain range, i.e. in, express The self-localization pose information of agent j at time t, where d is the distance range threshold, and n l Let l be the pose number of the j-th intelligent agent, where l = 0, 1, ..., L j→i L j→i Let j be the number of poses of the j-th intelligent agent; for the laser point cloud data and Feature extraction and feature matching are performed to obtain the j-th agent in At time t, the i-th intelligent agent... k Relative pose transformation at time t. and relative pose transformation estimation variance It is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in a two-dimensional Cartesian coordinate system.

7. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 6, characterized in that, Step 5) involves obtaining the pose optimization factor, which includes the following steps: S51: Construct the pose sequence to be optimized for the i-th agent as follows: T indicates that the i-th agent generates a total of T self-localization pose results; S52: Represent the pose sequence obtained in step 1) as follows Self-localization pose error is expressed as: Further expressed as a self-localization pose optimization factor S53: Apply the agent's relative pose constraint obtained in step 4) The pose constraint error is expressed as Further expressed as the relative pose optimization factor within the agent. S54: Apply the relative pose constraints between agents obtained in step 4) The pose error is expressed as Further expressed as the relative pose optimization factor between agents 8. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 7, characterized in that, In step 5), based on the obtained optimized pose sequence Sensor data The sequence is projected and stitched according to the optimized pose at the corresponding time to construct an optimized environment map, and then self-localization pose estimation is performed based on the optimized pose result.

9. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 1, characterized in that, A multi-agent cooperative localization and tracking device based on the method is implemented, including an autonomous localization module, a wireless transmission module, a relative pose acquisition module, an observation selectivity module, and a cooperative localization optimization module; The autonomous positioning module includes environmental sensing devices.

10. The multi-mobile intelligent agent networked cooperative localization and tracking method based on pose robust association as described in claim 9, characterized in that, The autonomous positioning module is used to calculate the agent's own positioning results and perform collaborative optimization processing; Based on the data collected by the sensors on each mobile intelligent agent, the autonomous localization module obtains the autonomous localization result of each intelligent agent. A wireless transmission module is used for necessary data exchange under network conditions. The relative pose acquisition module is used to match and calculate the relative pose results of the laser point cloud of multiple agents; The observation selectivity module is used to determine whether the relative pose observation results are reasonable and reliable by checking the consistency conditions between the relative pose observation results and the prior assumptions, so as to avoid introducing erroneous relative pose observation results. The collaborative localization optimization module is used to jointly optimize the localization results of multiple agents with relative pose constraints to achieve accurate and robust collaborative localization.

Images