Multi-mobile-agent network connection cooperative positioning tracking method based on pose robust association

Abstract

The invention discloses a multi-mobile-agent network connection cooperative positioning tracking method based on pose robust association, and belongs to the technical field of multi-agent cooperative positioning, and an implementation system comprises a self-positioning module, a wireless transmission module, a relative pose acquisition module, an observation selectivity module and a cooperative positioning optimization module. Different mobile intelligent agents with self-positioning capability share data through wireless network connection communication, wherein the data comprises real-time self-positioning results of the intelligent agents and laser point cloud data; modeling is carried out on multi-mobile intelligent body network connection cooperative positioning by constructing a multi-mobile intelligent body posture factor graph, and multi-intelligent body posture robust association is realized by utilizing an improved point cloud registration and observation selectivity method, so that efficient, accurate, robust and deployable multi-mobile intelligent body cooperative positioning tracking is realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-agent collaborative positioning, and specifically relates to a multi-mobile intelligent agent networked collaborative positioning and tracking method based on robust posture association. Background Art

[0002] With the rapid development of intelligent hardware and information processing technology, mobile agents equipped with perception sensors play a huge role in the fields of autonomous driving, industrial manufacturing, logistics and express delivery. Accurate positioning and tracking is an important basis for supporting mobile agents to achieve upper-level perception tasks. Traditional positioning technology usually achieves positioning by processing GPS / Beidou signals, but the accuracy of traditional satellite positioning methods is limited, and they cannot be widely used in indoor scenes or signal shielding scenes, which greatly limits the mobility of mobile agents. Simultaneous Localization and Mapping (SLAM) can use the perception information of the mobile agent itself to achieve autonomous positioning that does not rely on external information. It is currently a key technology for mobile agent positioning.

[0003] In actual intelligent agent operation scenarios, the intelligent agent operation environment is complex and the operation range is large. Single intelligent agents are easily affected by problems such as line of sight occlusion and cumulative errors, resulting in reduced positioning accuracy. Due to the increase in the scale of production operations, actual systems often have a large number of mobile intelligent agents. By designing a multi-mobile intelligent agent collaborative positioning and tracking solution based on efficient communication and interaction between intelligent agents, the limitations of single intelligent agent positioning can be effectively overcome, and accurate collaborative positioning in a large environment can be achieved. Therefore, it is necessary to design an efficient and robust collaborative positioning and tracking method.

[0004] The existing research on multi-agent collaborative positioning algorithms remains at the theoretical level of multi-source information fusion. These algorithms have strong theoretical assumptions on multi-agent collaboration and ignore the impact of the system in actual deployment. There are two key issues in multi-agent collaborative positioning and tracking. The first is how to obtain the relative pose information between multiple agents, that is, to obtain the pose association between agents by jointly processing the sensor information of multiple agents; the second is how to collaboratively optimize the positioning pose results of multiple agents, that is, to improve the positioning accuracy of the agent system by fusing the self-positioning information of multiple agents with the relative pose information. For the relative pose acquisition problem, the existing solutions usually use laser point cloud matching algorithms to align the point clouds of different agents to infer the relative pose between agents. However, the existing point cloud matching algorithms do not improve the situation of partial overlap of point clouds in multi-agent collaborative operation scenarios, and it is difficult to achieve good point cloud matching performance. For the problem of multi-agent collaborative positioning optimization, the existing solutions do not fully consider the robustness of multi-source information fusion, and introduce all multi-agent association information into positioning optimization, which is easy to introduce erroneous positioning results, resulting in a significant decrease in positioning accuracy. Therefore, combined with the above reasons, existing solutions are difficult to achieve efficient and robust multi-agent collaborative positioning and tracking solutions, and cannot effectively support the practical application of multi-agent collaborative systems. Summary of the invention

[0005] In order to overcome the shortcomings of the above-mentioned prior art, the present invention proposes a multi-mobile intelligent networked collaborative positioning and tracking method based on robust posture association, models the collaborative positioning problem based on the multi-agent posture factor graph, and utilizes the improved point cloud registration algorithm and observation selectivity mechanism to realize multi-agent posture robust association, thereby achieving efficient, accurate, robust and deployable multi-mobile intelligent collaborative positioning and tracking.

[0006] The present invention is a multi-mobile agent networked collaborative positioning and tracking solution based on robust posture association applied to a multi-agent system, and the solution includes an autonomous positioning module, a wireless transmission module, a relative posture acquisition module, an observation selectivity module, and a collaborative positioning optimization module. In the present invention, a low-speed mobile operating agent (including but not limited to an automated guided vehicle (AGV), a robot with autonomous mobility, etc.) on the indoor ground is equipped with necessary motion sensors and environmental perception devices to support autonomous positioning; and a wireless transmission module is equipped under the networking condition to support information sharing; a point cloud registration algorithm based on a dynamic threshold is implemented using shared laser point cloud information and autonomous positioning results to calculate the relative posture of the positioning node; the established posture constraints are screened using the observation selectivity module to retain the posture constraints that meet the consistency constraint conditions; based on the autonomous positioning results, the posture constraints within the agent, and the posture constraints between agents, the collaborative positioning optimization module is used to achieve global optimization of the multi-agent positioning results. This method can be applied to multi-agent systems operating at low speeds in quasi-enclosed indoor scenes. It does not affect the original system layout and does not need to rely on other external information or external devices, thus achieving efficient and robust multi-agent collaborative positioning and tracking.

[0007] To achieve the above objectives, the present invention requires that the mobile agent be equipped with an environmental perception device (which must include a laser radar) to enable it to have the ability to autonomously locate. It is also equipped with wireless communication equipment so that different agents can share data through networking. Each mobile agent obtains the real-time autonomous positioning results and laser point cloud data of other agents through networking communication. The specific multi-agent collaborative positioning and tracking implementation plan is as follows:

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

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

[0010] 3) According to the laser point cloud data and autonomous positioning results shared in 2), the relative poses between the positioning nodes of the multi-agent system are calculated through a step-by-step ICP (Iterative Closest Point) point cloud registration method based on a dynamic threshold.

[0011] 4) The relative pose observation results obtained in 3) are tested by an observation selectivity algorithm. The algorithm determines 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, thereby avoiding the introduction of erroneous relative pose observation results.

[0012] In the specific implementation, based on the agent's own posture information, the surrounding agent's posture information and the relative posture information, a variety of relative posture constraint sequences are established, and the consistency of the posture constraints within the agent and the posture constraints between agents is checked through the observation selection algorithm, and the relative posture that meets the consistency constraints is obtained through detection and screening; the observation selection algorithm includes:

[0013] Building an Agent k The relative position constraint sequence of itself at the moment To perform consistency check, the pose constraint is expressed as:

[0014]

[0015] Among them, |x| represents the absolute value of the result after subtracting the three-dimensional pose vector, ∈ is the three-dimensional threshold vector of the internal consistency pose constraint of the intelligent agent, which corresponds to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system respectively;

[0016] The final intelligent agent t that satisfies the above posture constraints is obtained k The relative position constraint sequence of itself at the moment

[0017] Building an Agent k The pose constraint sequence between each agent at any time To perform consistency check, the pose constraint is expressed as:

[0018]

[0019] Among them, δ is the three-dimensional threshold vector of the consistent posture constraint between agents, which corresponds to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system, and its size is set to a fixed threshold. The posture constraints that meet the above conditions are retained to form the final agent t k The pose constraint sequence between each agent at any time

[0020] 5) Using the autonomous positioning result obtained in 1) and the relative posture verified by the observation selectivity algorithm in 4), a multi-agent posture factor graph optimization model is constructed to perform multi-source information fusion and optimize the solution to obtain the collaborative positioning posture result.

[0021] Perform distributed optimization solution, expressed as:

[0022]

[0023] Among them, the first Represents the optimization factor of the self-positioning posture from time t1 to time t T Perform the summation; the second term Indicates that at every moment t k Sum the relative posture optimization factors associated with the posture at that moment, and then sum them up in the time dimension; the third term Indicates that at every moment t k Sum the relative posture optimization factors between the j-th agent and its associated agents, then sum them up according to the agent number, and finally sum them up from the time dimension.

[0024] By optimizing and solving the multimodal multi-agent pose factor graph optimization model, the global optimization positioning of multi-agent multimodal information in time and space dimensions is achieved.

[0025] Compared with the prior art, the present invention has the following beneficial effects:

[0026] The present invention provides a multi-mobile intelligent networked collaborative positioning and tracking solution based on robust association of postures, and proposes an improved point cloud registration algorithm and an observation selectivity algorithm for the two major problems of multi-agent collaborative positioning, without relying on other external information or devices, to achieve efficient and robust multi-agent collaborative positioning and tracking. The observation selectivity algorithm proposed in the present invention performs consistency checks on a large number of posture constraints in the multi-agent system, thereby avoiding the introduction of erroneous posture constraints into the multi-agent global positioning optimization, and ensuring the accuracy and robustness of multi-agent collaborative positioning.

[0027] The method and application of collaborative acquisition of relative posture of mobile intelligent bodies based on dynamic threshold point cloud registration proposed in the present invention have the following advantages:

[0028] (I) Based on the wireless communication network, information exchange between multiple mobile agents is carried out to build a multi-agent collaborative positioning and tracking system, solve the positioning drift and mapping error problems of a single agent in a large-scale environment in actual applications, and improve the collaborative operation efficiency of the multi-agent system.

[0029] (ii) Considering the indoor scenes where the sensor data of the intelligent agents for collaborative acquisition of relative posture only partially overlap due to reasons such as view occlusion, the existing point cloud registration algorithm is improved specifically using the dynamic threshold and step-by-step registration strategy to make it suitable for laser point cloud registration between intelligent agents in indoor environments. It can output accurate registration results, thereby achieving effective acquisition of relative posture between intelligent agents and facilitating the collaborative positioning optimization of the intelligent system.

[0030] (III) Considering a large number of posture constraints in the multi-agent system, including the self-positioning posture constraints of the agent, the relative posture constraints within the agent, and the relative posture constraints between multiple agents, the observation selectivity algorithm is used to check the consistency of the posture constraints, and a multi-agent posture factor graph model is constructed for the posture constraints that meet the test conditions, so as to realize distributed positioning optimization that considers global information in time and space dimensions, and support efficient, robust, and deployable multi-agent collaborative positioning optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 It is a structural block diagram of the relative posture acquisition and application system of the present invention.

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

[0033] Figure 3 It is a flowchart of the observation selectivity module of the present invention. DETAILED DESCRIPTION

[0034] In order to make the purpose, features and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0035] refer to Figure 1 The present invention requires that each mobile agent in the system is equipped with an environmental perception device (at least including a laser radar) for autonomous positioning in indoor quasi-enclosed scenes. Based on the wireless transmission module, the autonomous positioning results of the agents and the laser point cloud information are shared. The relative posture acquisition module is used to match and calculate the multi-agent laser point clouds to obtain the relative posture results. The observation selectivity module performs consistency check on the relative posture results. Finally, the collaborative positioning optimization module uses the self-positioning results and the filtered relative posture results for global optimization to achieve multi-agent collaborative positioning optimization.

[0036] The present invention sets a multi-agent collaborative positioning and tracking device inside the mobile intelligent body. The device includes an autonomous positioning module to calculate the positioning result of the intelligent body itself and support the subsequent collaborative optimization processing; a wireless transmission module to support the necessary data interaction under the network connection condition; a relative posture acquisition module; an observation selectivity module; and a collaborative positioning optimization module. The autonomous positioning module includes an environmental perception device. The specific steps are as follows:

[0037] S10: The intelligent agents obtain autonomous positioning results respectively;

[0038] The intelligent agent processes the information of the sensing device through the autonomous positioning module installed on it, and continuously obtains the self-positioning information of the intelligent agent. The pose information (including position and orientation) estimated by the intelligent agent's self-positioning and the pose estimation variance information, together with the sensor information at the corresponding moment, form a data packet.

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

[0040] The multi-agent sends key frame data packets through the wireless transmission module, which include timestamp, agent number, self-positioning estimation value and self-positioning estimation variance (S10), perception information, and simultaneously receives data packets from agents within the communication range to realize the sharing of self-positioning information.

[0041] S30: The agent calculates the relative pose between key frame nodes;

[0042] Multi-agents establish key frame pairs to be matched based on their own posture information and the key frame data received from other agents, and use the improved dynamic threshold point cloud matching algorithm to obtain the relative posture results between key frame nodes.

[0043] S40: The agent verifies the relative pose result through the observation selection algorithm to obtain a consistent relative pose;

[0044] Multi-agents perform consistency check on the relative pose results generated by the relative pose acquisition module, and adopt different verification strategies for the relative pose constraints within the agent and the relative pose constraints between agents, retaining the relative pose results that meet the verification conditions for collaborative positioning optimization.

[0045] S50: The agent constructs a multi-agent pose factor graph using the relative pose data from itself and other agents, and performs global optimization.

[0046] The collaborative positioning optimization module uses a variety of relative posture data from itself and other intelligent agents to construct a multi-agent multimodal factor graph, further performs multi-source information fusion, obtains the posture estimation value after optimization, and constructs a global map based on the optimized posture estimation value.

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

[0048] In step S20: the agent sends its own posture information and sensor information at the corresponding time through wireless communication methods such as WIFI, 4 / 5G, etc. This step includes the following processes S21-S22:

[0049] S21: The agent will autonomously locate the module at t k The posture information at the moment Pose estimation variance Laser point cloud data Agent number i and timestamp t k Packing forms key frame data packets, broadcasting one-to-many to surrounding intelligent agents at a certain period, sharing their own status information and laser point cloud data;

[0050] S22: Receive the key frame data packets sent by the surrounding intelligent agents, obtain the posture status and sensor data of the surrounding intelligent agents, and the j-th (j≠i) intelligent agent at t m The posture status information at the moment Pose estimation variance Laser point cloud data

[0051] In step S30: the agent uses its own agent key frame pose information (S10) and the received other agent key frame pose information (S20) to establish a key frame node pair to be matched, and the laser point cloud P from the target key frame is m Through coordinate system transformation, we get the point cloud P′ to be registered m ; Point cloud P from candidate keyframes n Perform step-by-step ICP point cloud registration based on dynamic threshold and perform rigid body transformation to obtain point cloud P′ n ; Then the relative pose calculation result between the two frames is obtained. The specific process is as follows Figure 2 shown.

[0052] In step S30, the autonomous positioning result of the candidate key frame A is recorded as in are the horizontal and vertical coordinates of the estimated positioning of the agent numbered a in the two-dimensional Cartesian coordinate system of the real environment at the corresponding moment of the frame, is the estimated value of the agent’s orientation angle. Similarly, the autonomous positioning result of the target keyframe B is recorded as in are the horizontal and vertical coordinates of the estimated positioning of the agent numbered b in the two-dimensional Cartesian coordinate system of the real environment at the corresponding moment of the frame, is the estimated value of the agent’s orientation angle. The relative pose between the candidate keyframe A and the target keyframe B is recorded as in are the estimated values ​​of the horizontal and vertical coordinates of agent b at the time corresponding to the target key frame B in the two-dimensional Cartesian coordinate system with agent a at the time corresponding to the candidate key frame A as the reference system, is the estimated value of the orientation angle. Obtaining the relative pose between key frames includes the following steps S31 to S34:

[0053] S31: Get the distance threshold, the method is: take the point cloud P m With point cloud P n The maximum laser measurement distance is denoted as l; the larger value of the laser angular resolution is δ (in radians); assuming that the number of dynamic matching is k, k is generally a positive integer between 4 and 10. α is the distance threshold scaling parameter, which is generally a real number greater than 0 and less than 1. Construct a set of decreasing arithmetic progressions as the distance threshold, with k items, the first item is αklδ, the last item is αlδ, and the tolerance is -αlδ. The arithmetic progression is denoted as {a i}(i=1…k).

[0054] S32: Calculate the coordinate transformation relationship between the two agents through the autonomous positioning results of the two agents, 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, each of which contains data in two dimensions in the x / y direction. 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] It represents the transformation relationship between the point cloud coordinate systems; where R is a two-dimensional rotation matrix, that is, is a two-dimensional rotation matrix with β as the rotation angle.

[0057] S33: Calculate point cloud P n Towards point cloud P′ m The registration matrix is ​​divided into 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 step i 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 transformation in the previous step (i-1 step) is matched to obtain the linear rigid body transformation matrix tran i , P n，i-1 The transformed point cloud data is recorded as P n，i (For i=1…k-1, P n，i is the next step, i.e. the point cloud to be registered in step i+1). Then the final transformed point cloud P′ n The expression is P′ n =P n，k , the expression of the rigid body transformation matrix totaltran of the registration result is Specifically, for 2D 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 recorded as The 2x2 sub-matrix in the upper left corner is the two-dimensional rotation matrix with β as the rotation angle, which represents the rotation angle of the two-dimensional rigid body transformation. x ,t y They represent the translation of the two-dimensional rigid body transformation in the x / y direction. For any point in the two-dimensional Cartesian coordinate system, the coordinates are The two-dimensional rigid body transformation conv acts on this point The meaning of the above is to transform the coordinates of the point into In this step, the point cloud registration result tran output from k steps i (i=1…k) are all linear rigid body transformation matrices. Expression The given registration result totaltran is also a linear rigid body transformation matrix.

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

[0059]

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

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

[0062]

[0063] On this basis, due to the one-to-one correspondence between the two-dimensional data points after registration, that is:

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

[0065]

[0066] So as to obtain the calculation result of relative posture

[0067]

[0068] In step S40: the agent establishes multiple relative posture constraints based on its own posture information, the posture information of surrounding agents and the relative posture information (S30), and uses the observation selection algorithm to filter the posture constraints within the agent and the posture 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 historical posture information k The self-positioning pose information of agent i at time The self-position information within a certain range, that is, Where ||x||2 represents the two-dimensional translation information in the pose information to calculate the Euclidean distance, express The self-positioning posture information of agent i at time d is the distance range threshold, n is l (l=0,1,...L i ) is the pose number that satisfies the above conditions, L i is the number of postures that meet the above conditions. Time and t k Relative posture change at time And the relative pose transformation estimation variance It is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system, and establishes the intelligent agent t k The relative position constraint sequence of itself at the moment

[0070] S42: For the agent t i obtained in S41 k The relative position constraint sequence of itself at the moment Perform consistency check as follows

[0071]

[0072] Among them, |x| represents the absolute value of the result after subtracting the three-dimensional pose vector, ∈ is the three-dimensional threshold vector of the internal consistency pose constraint of the intelligent agent, which corresponds to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system respectively. Its size setting is related to the accumulation of the self-positioning error of the intelligent agent, requiring that the relative pose provided by the relative pose acquisition module should be consistent with the prior positioning error of the autonomous positioning algorithm. The pose constraints that meet the above conditions are retained to form the final intelligent agent t k The relative position constraint sequence of itself at the moment During specific implementation, the autonomous positioning algorithm may adopt the 2D lidar positioning algorithm Hector SLAM algorithm.

[0073] S43: The agent extracts t based on the received posture information of other agents. k Self-positioning pose information at the moment The pose information of other agents within a certain range, that is, express The self-positioning posture information of agent j at time d is the distance range threshold, n is l (l=0,1,…,L j→i ) is the position number of the jth agent that meets the above conditions, L j→i is the number of poses of the jth agent that meets the above conditions. and Perform feature extraction and feature matching to obtain the jth agent in At time t, the i-th agent k Relative posture change at time And the relative pose transformation estimation variance It is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system, and establishes the intelligent agent t k The pose constraint sequence between each agent at any time

[0074] S44: For the agent t i obtained in S43 k The pose constraint sequence between each agent at any time Perform consistency check as follows

[0075]

[0076] δ is the three-dimensional threshold vector of the consistent posture constraint between agents, corresponding to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system, and its size is set to a fixed threshold. The posture constraints that meet the above conditions are retained to form the final agent t k The pose constraint sequence between each agent at any time

[0077] S50: The agent is modeled as a multi-agent pose factor graph model based on the multiple pose constraints obtained in S40. The nodes in the factor graph represent the multi-agent pose variables to be optimized, and the edges in the factor graph represent the relative pose constraints between the pose nodes. By solving the factor graph optimization problem, the globally optimized positioning 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: T represents the i-th intelligence

[0079] The body generates T self-positioning pose results;

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

[0081] Further expressed as the self-positioning pose optimization factor

[0082] S53: Obtain the relative position constraints of the intelligent body itself for S41 The pose constraint error is expressed as Further expressed as the relative posture optimization factor inside the agent

[0083] S54: Obtain relative pose constraints between agents for S42 The pose error is expressed as Further expressed as the relative posture optimization factor between agents

[0084] S55: Based on the four optimization factors obtained in S52 to S54, the multi-agent collaborative positioning 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 posture sequence of the i-th agent can be obtained. Other agents can perform distributed optimization solutions according to the same process, which can be expressed as:

[0085]

[0086] Item 1 Represents the optimization factor of the self-positioning posture from time t1 to time t T Perform the summation; the second term Indicates that at every moment t kSum the relative posture optimization factors associated with the posture at that moment, and then sum them up in the time dimension; the third term Indicates that at every moment t k Sum the relative posture optimization factors between the j-th agent and its associated agents, then sum them up according to the agent number, and finally sum them up from the time dimension; by optimizing and solving the problem composed of the above multimodal optimization factors, the global optimization positioning of multi-agent multimodal information in the time and space dimensions can be achieved.

[0087] S56: Optimized pose sequence based on S55 Sensor data The sequence is projected and spliced ​​according to the optimized pose at the corresponding moment to construct an optimized environment map. The optimized pose is fed back to the autonomous positioning module, and the autonomous positioning pose estimation continues based on the global optimization result.

[0088] The present invention provides the above-mentioned multi-mobile intelligent agent collaborative positioning and tracking technology, which uses the sensing equipment equipped by the intelligent agent itself to ensure independent and accurate autonomous positioning when a single intelligent agent moves at a low speed; based on data sharing between intelligent agents, a point cloud matching method with a dynamic threshold is used to calculate relative posture observation results with the help of environmental information in indoor quasi-closed scenes; an observation selectivity module is used to remove potentially erroneous relative posture observation results; and a collaborative positioning optimization module is used to globally optimize the positioning results of multiple intelligent agents and relative posture constraints to achieve accurate and robust collaborative positioning. Environmental sensing devices and wireless transmission modules are both commonly installed devices for mobile intelligent agents. The present invention has no additional hardware requirements and can achieve an efficient and robust multi-agent collaborative positioning and tracking solution, which is of great value for the actual production application of multiple intelligent agents.

[0089] It should be noted that the purpose of publishing the embodiments is to help further understand the present invention, but those skilled in the art can 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 contents disclosed in the embodiments, and the scope of protection claimed by the present invention shall be subject to the scope defined in the claims.

Claims

1. A collaborative positioning and tracking method for multiple mobile agents based on robust association of postures, characterized in that: The mobile intelligent body has the ability to locate itself autonomously; different mobile intelligent bodies share data through wireless network communication, including the real-time autonomous positioning results and laser point cloud data of the intelligent body; The collaborative positioning of multiple mobile agents is modeled by constructing a multi-mobile agent pose factor graph, and the robust association of multi-agent poses is achieved by using an improved point cloud registration algorithm and an observation selectivity method, thereby achieving collaborative positioning and tracking of multiple mobile agents. The method includes the following steps: 1) The mobile agents obtain autonomous positioning information respectively, including the pose information estimated by the agent's self-positioning, the pose estimation variance information, and the sensor information at the corresponding time; 2) The intelligent agent shares self-positioning information and laser point cloud data; 3) According to the laser point cloud data and autonomous positioning information shared in step 2), the relative poses between the positioning nodes of the multi-agent system are calculated through a step-by-step iterative closest point registration method (ICP) based on a dynamic threshold; Multi-agents establish key frame pairs to be matched based on their own posture information and the key frame data received from other agents, and use the improved dynamic threshold point cloud matching algorithm to obtain the relative posture results between key frame nodes; 4) Based on the agent's own posture information, the posture information of surrounding agents and the relative posture information, multiple relative posture constraint sequences are established. The consistency of the posture constraints within the agent and the posture constraints between agents is checked through the observation selection algorithm. The relative posture that meets the consistency constraints is obtained through detection and screening. The observation selectivity algorithm includes: Building an Agent k The relative position constraint sequence of itself at the moment To perform consistency check, the pose constraint is expressed as: Among them, |x| represents the absolute value of the result after subtracting the three-dimensional pose vector, ∈ is the three-dimensional threshold vector of the internal consistency pose constraint of the intelligent agent, which corresponds to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system respectively; The final intelligent agent t that satisfies the above posture constraints is obtained k The relative position constraint sequence of itself at the moment Building an Agent k The pose constraint sequence between each agent at any time To perform consistency check, the pose constraint is expressed as: Among them, δ is the three-dimensional threshold vector of the consistent posture constraint between agents, which corresponds to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system, and its size is set to a fixed threshold. The posture constraints that meet the above conditions are retained to form the final agent t k The pose constraint sequence between each agent at any time 5) Using the autonomous positioning result obtained in step 1) and the relative pose that meets the consistency test conditions in step 4), a multi-agent pose factor graph optimization model is constructed, and multi-source information fusion is performed to obtain a pose estimation value through distributed optimization, and a global map is constructed based on the optimized pose estimation value, thereby obtaining a collaborative positioning pose result; Perform distributed optimization solution, expressed as: Among them, the first Represents the optimization factor of the self-positioning posture from time t1 to time t T Perform the summation; the second term Indicates that at every moment t k Sum the relative posture optimization factors associated with the posture at that moment, and then sum them up in the time dimension; the third term Indicates that at every moment t k Sum the relative posture optimization factors between the jth agent and its associated agents, then sum them up according to the agent number, and finally sum them up from the time dimension; By optimizing and solving the multimodal multi-agent pose factor graph optimization model, the global optimization positioning of multi-agent multimodal information in time and space dimensions is achieved.

2. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 1, characterized in that: In step 2), the intelligent agents share data via wireless network communication, which specifically includes S21 to S22: S21: The agent will t k The posture information at the moment Pose estimation variance Laser point cloud data Agent number i and timestamp t k Packing forms key frame data packets, broadcasting one-to-many to surrounding intelligent agents at a certain period, sharing their own status information and laser point cloud data; S22: Receive the key frame data packets sent by the surrounding intelligent agents, obtain the posture status and sensor data of the surrounding intelligent agents, and the jth intelligent agent at t m The posture status information at the moment Pose estimation variance Laser point cloud data 3. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 2, characterized in that: In step 3), the agent uses its own agent key frame pose information and the received key frame pose information of other agents to establish a key frame node pair to be matched, and the laser point cloud P of the target key frame m Through coordinate system transformation, we get the point cloud P′ to be registered m ; Point cloud P of candidate keyframes n Perform step-by-step ICP point cloud registration based on dynamic threshold, and obtain point cloud P′ after rigid body transformation n ; Then the relative pose calculation result between the two frames is obtained.

4. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 3, characterized in that: Obtaining the relative pose between key frames includes the following steps: S31: Obtain the distance threshold. The method is: construct a set of decreasing arithmetic progressions 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δ. The arithmetic progression is {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 laser angular resolution of the two is δ, in radians; the number of dynamic matching times is k, and α is the distance threshold scaling parameter; S32: Calculate the coordinate transformation relationship between the two agents through the autonomous positioning results of the two agents, and transform the point cloud P m Transformed into point cloud P′ m ; S33: Calculate the point cloud P n Towards 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 between the two-dimensional data points after registration, the calculation result of the relative posture is calculated.

5. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 4, characterized in that: The autonomous positioning algorithm specifically adopts a two-dimensional lidar positioning algorithm.

6. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 4, characterized in that: In step 4), establish the intelligent agent t k The specific method of constraining the relative posture sequence of the moment is: Based on its historical posture information, the agent extracts t k The self-positioning pose information of agent i at time The self-position information within a certain range, that is, Where ||x||2 represents the two-dimensional translation information in the pose information to calculate the Euclidean distance, express The self-positioning posture information of agent i at time d is the distance range threshold, n is l is the pose number of itself, l=0,1,…L i ; L i is the number of postures; obtained through the relative posture acquisition module Time and t k Relative posture change at time And the relative pose transformation estimation variance is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system; Building an Agent k The specific method of constraining the sequence of postures between the moment and other agents is: Based on the received pose information of other agents, the agent extracts t k Self-positioning pose information at the moment The pose information of other agents within a certain range, that is, in, express The self-positioning posture information of agent j at time d is the distance range threshold, n is l is the position number of the j-th agent, l = 0, 1, ..., L j→i , L j→i is the number of poses of the jth agent; for the laser point cloud data and Perform feature extraction and feature matching to obtain the jth agent in At time t, the i-th agent k Relative posture change at time And the relative pose transformation estimation variance It is a 3×3 matrix, corresponding to the horizontal and vertical coordinates and orientation in the two-dimensional Cartesian coordinate system.

7. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 6, characterized in that: Obtaining the pose optimization factor in step 5) includes the following steps: S51: Construct the pose sequence to be optimized for the i-th agent: T means that the i-th agent generates a total of T self-positioning pose results; S52: The pose sequence obtained in step 1) is expressed as The self-localization pose error is expressed as Further expressed as the self-positioning pose optimization factor S53: constrain the relative posture of the intelligent body obtained in step 4) The pose constraint error is expressed as Further expressed as the relative posture optimization factor inside the agent S54: constrain the relative postures between the agents obtained in step 4) The pose error is expressed as Further expressed as the relative posture optimization factor between agents 8. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 7, characterized in that: In step 5), based on the obtained optimized pose sequence Sensor data The sequences are projected and spliced ​​according to the optimized poses at the corresponding moments to construct an optimized environment map, and then self-localization pose estimation is performed based on the optimized pose results.

9. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 1, characterized in that: Based on the method, a multi-agent collaborative positioning and tracking device is implemented, including an autonomous positioning module, a wireless transmission module, a relative posture acquisition module, an observation selectivity module and a collaborative positioning optimization module; The autonomous positioning module includes an environmental perception device.

10. The method for collaborative positioning and tracking of multiple mobile agents based on robust association of postures as claimed in claim 9, characterized in that: The autonomous positioning module is used to calculate the positioning result of the intelligent body itself and perform collaborative optimization processing; According to the data collected by the sensors on each mobile agent, the autonomous positioning module obtains the autonomous positioning results of each agent; Wireless transmission module, used for necessary data interaction under networking conditions; The relative pose acquisition module is used to match and calculate the multi-agent laser point cloud to obtain the relative pose result; The observation selectivity module is used to determine whether the relative posture observation results are reasonable and reliable by checking the consistency conditions between the relative posture observation results and the prior assumptions, so as to avoid introducing erroneous relative posture observation results; The collaborative positioning optimization module is used to globally optimize the positioning results of multiple agents and relative pose constraints to achieve accurate and robust collaborative positioning.

Images