Multi-vehicle collaborative exploration trajectory planning method fusing positioning loopback point constraint

Abstract

The invention relates to a multi-vehicle collaborative exploration trajectory planning method fusing positioning loopback point constraints, belongs to the technical field of unmanned vehicle trajectory planning, and solves the problems of positioning drift and inaccurate path exploration caused by no loopback for a long time in the prior art. Comprising the steps of generating a global exploration path of each unmanned vehicle based on a global environment map; in the local area of each unmanned vehicle, based on the respective global exploration path, the pose of each unmanned vehicle and the covariance matrix of the pose, screening out a plurality of loopback candidate points with the highest utility score as path anchor points; and performing local path planning according to the intersection point of the global exploration path of each unmanned vehicle and the local area, and the leading viewpoint and the path anchor point in the local area to obtain a local optimization path of each unmanned vehicle. And the multi-vehicle exploration efficiency and the collaboration are improved.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle trajectory planning technology, and in particular to a multi-vehicle collaborative exploration trajectory planning method that integrates positioning loop point constraints. Background Technology

[0002] The core task of autonomous vehicles in exploring unknown environments is to accurately locate their position on a map without prior information, thereby planning a precise path.

[0003] Existing technologies include hierarchical planning, which uses a two-level strategy of "macro-micro" to balance global and local planning; frontier processing technology uses more intelligent frontier clustering, selection and ranking heuristic rules to avoid the robot being disturbed by a large number of repetitive or low-value frontiers, thereby planning a smoother and more efficient trajectory.

[0004] These existing technologies primarily focus on how to distribute exploration tasks among robotic teams to achieve load balancing and reduce redundant work. They often treat loop closure constraints as passive backend optimization factors rather than actively incorporating them into path planning. This results in a lack of proactive maintenance of localization consistency during robot exploration, easily leading to problems such as duplicate coverage, high path overlap, and uneven task distribution. Furthermore, some methods require a prior map and cannot explore from scratch in completely unknown environments. Summary of the Invention

[0005] Based on the above analysis, the embodiments of the present invention aim to provide a multi-vehicle collaborative exploration trajectory planning method that integrates positioning loop point constraints, in order to solve the problems of positioning drift and inaccurate path exploration caused by long periods without loops.

[0006] This invention provides a multi-vehicle cooperative exploration trajectory planning method that integrates positioning loopback point constraints, comprising the following steps:

[0007] Based on the global environment map, a global exploration path is generated for each autonomous vehicle;

[0008] Within the local area of ​​each autonomous vehicle, based on its own global exploration path, the pose of each autonomous vehicle and the covariance matrix of the pose, multiple loop candidate points with the highest utility scores are selected as path anchor points.

[0009] Local path planning is performed based on the intersection of the global exploration path of each autonomous vehicle with the local area, the leading viewpoint and path anchor point within the local area, to obtain the local optimized path for each autonomous vehicle.

[0010] Based on the above method, a further improvement is made, using a global environment map, to generate a global exploration path for each autonomous vehicle, including:

[0011] Identify leading voxels on the global environment map and form a global task point set through clustering;

[0012] The problem of multiple autonomous vehicles accessing the global task point set is modeled as a multi-vehicle parking lot vehicle pathing problem. The optimization objective is to minimize the total driving cost and balance the workload of each autonomous vehicle. By solving the multi-vehicle parking lot vehicle pathing problem, a task point sequence is assigned to each autonomous vehicle, generating a global exploration path for each autonomous vehicle.

[0013] Based on the above method, a global task point set is formed through clustering, including:

[0014] Multiple points are sampled around the centroid of each cluster and projected onto the global environment map. The node closest to the projected point in the global environment map is obtained as the candidate viewpoint for each cluster.

[0015] The expected information gain of each candidate viewpoint is evaluated, and the candidate viewpoint with the highest expected gain in each cluster is selected as the exploration target to form a global task point set.

[0016] Based on further improvements to the above method, the expected information gain of each candidate viewpoint is evaluated, including:

[0017] Multiple rays are projected from the position of each candidate viewpoint to obtain the set of unknown voxels covered by each ray;

[0018] The weight coefficients of each unknown voxel in the set of unknown voxels corresponding to each candidate viewpoint are summarized to obtain the expected information gain of each candidate viewpoint.

[0019] Based on further improvements to the above method, several candidate loop points with the highest utility scores were selected, including:

[0020] Within the local area of ​​the current autonomous vehicle, the future pose of the current autonomous vehicle is matched with the historical poses of other autonomous vehicles to generate a loop closure candidate set;

[0021] For each pose pair in the candidate loop closure set, calculate the utility score;

[0022] Select the top K pose pairs with the highest utility scores from the candidate loop closure pair set, and the future poses of these pairs are the candidate loop closure points, where K > 0.

[0023] Based on further improvements to the above method, a candidate set of loop closures is generated, including:

[0024] Extract path segments within a local area from the current global exploration path of the autonomous vehicle, and filter out historical poses located within the local area from the historical trajectories of other autonomous vehicles;

[0025] Based on the current pose of the autonomous vehicle, the covariance matrix of the pose, and the path segments in the local area, predict its future pose sequence and the corresponding predicted covariance matrix sequence.

[0026] The predicted future pose sequence is matched with the selected historical poses, and pose pairs that meet the distance threshold are added to the loop closure candidate set.

[0027] Based on the further improvement of the above method, for each pose pair in the loop closure candidate set, a utility score is calculated, including:

[0028] Based on the relative pose observation model, predict the relative pose observation value for each pose pair;

[0029] The innovation covariance is calculated based on the Jacobian matrix of the relative pose observations and the covariance matrix of the pose pairs.

[0030] The D-optimality information gain is calculated based on the new information covariance; the utility score of each pose pair is calculated by fusing the D-optimality information gain with the Euclidean distance from the current position of the autonomous vehicle to the future pose in the pose pair.

[0031] Based on the further improvement of the above method, the utility score of each pose pair is calculated by the following formula:

[0032]

[0033] Among them, u pq Indicates pose pair The utility score, This represents the predicted future pose of the autonomous vehicle p at the l-th moment. This represents the historical pose of the autonomous vehicle q at time m. s pq Indicates the new information covariance; The logarithm of the determinant of the inverse of the information covariance represents the D-optimality information gain; d pq Future pose from the current position of the autonomous vehicle to its pose alignment The Euclidean distance; γ represents an attenuation coefficient greater than 0.

[0034] Based on further improvements to the above method, the optimization objective is expressed by the following formula:

[0035]

[0036] Where N represents the total number of autonomous vehicles, M represents the total number of targets explored in the global task point set, and L max c represents the maximum path length assigned to all autonomous vehicles; λ represents the balancing weight coefficient. ij Indicates the exploration target v i to vj Path cost, x ijk ∈{0,1} indicates whether the driverless car k has started from v i Drive to v j .

[0037] Based on the further improvements to the above method, the local optimized path for each autonomous vehicle is obtained through the following steps:

[0038] The intersection of the current global exploration path of the autonomous vehicle with the local area, the leading viewpoint and path anchor point within the local area are taken as the target points;

[0039] Starting from the current autonomous vehicle and aiming at finding the shortest path to all target points, the path cost is obtained based on the global environment map. By solving the traveling salesman problem, the local optimized path of the current autonomous vehicle is obtained.

[0040] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0041] 1. By proactively creating high-quality candidate loop closure opportunities, evaluating the utility score of candidate loop closures and using them as path anchors, the maintenance of positioning accuracy is transformed from passive back-end optimization to proactive front-end planning goals. This fundamentally solves the problem of the disconnect between positioning and planning, effectively suppresses the cumulative drift that inevitably occurs during long-term exploration in feature-lacking environments, and achieves a synergistic improvement in the performance of positioning and exploration.

[0042] 2. The adoption of MDVRP-based global task allocation effectively balances the workload among multiple unmanned vehicles, avoiding uneven task distribution. Simultaneously, in local planning, the intersection of the global exploration path and the local area, along with the leading viewpoint and loop candidate points, are integrated for TSP optimization, generating an optimal path that simultaneously considers exploration and localization benefits. This significantly reduces redundant coverage and ineffective movement, improving the overall exploration efficiency and collaboration of multiple unmanned vehicles.

[0043] In this invention, the above-described technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of this invention will be set forth in the following description, and some advantages may become apparent from the description or be learned by practicing the invention. The objects and other advantages of this invention can be realized and obtained from what is particularly pointed out in the description and drawings. Attached Figure Description

[0044] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0045] Figure 1This is a flowchart of a multi-vehicle collaborative exploration trajectory planning method that integrates positioning loop point constraints in an embodiment of the present invention. Detailed Implementation

[0046] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.

[0047] A specific embodiment of the present invention discloses a multi-vehicle cooperative exploration trajectory planning method that integrates positioning loopback point constraints, such as... Figure 1 As shown, it includes the following steps:

[0048] S1. Generate a global exploration path for each autonomous vehicle based on the global environment map;

[0049] S2. Within the local area of ​​each autonomous vehicle, based on its global exploration path, the pose of each autonomous vehicle, and the covariance matrix of the pose, select multiple loop candidate points with the highest utility scores as path anchor points.

[0050] S3. Based on the intersection of the global exploration path of each autonomous vehicle with the local area, the leading viewpoint and path anchor point in the local area, perform local path planning to obtain the local optimized path of each autonomous vehicle.

[0051] This embodiment transforms maintaining positioning accuracy from a passive backend optimization to an active frontend planning goal by actively evaluating the loop closure utility score and using it as a path anchor point. This fundamentally solves the problem of the disconnect between positioning and planning, and achieves a synergistic improvement in the performance of positioning and exploration.

[0052] It should be noted that before executing step S1, a global environment map, the pose of each autonomous vehicle, and the covariance matrix of the pose are obtained in advance based on the multi-autonomous vehicle cooperative localization and mapping system.

[0053] Specifically, the multi-vehicle cooperative localization and mapping system integrates sensor data from LiDAR, inertial measurement units, and other sources to construct a globally consistent environmental map while optimizing the pose estimation of each vehicle. During this optimization process, the pose covariance matrix, as a key indicator for measuring the uncertainty of pose estimation, is calculated and output.

[0054] The global environment map can be any form suitable for path planning, including but not limited to sparse topology maps, raster maps, or point cloud maps. For example, a lightweight sparse topology map can be constructed by extracting the skeleton of passable areas from a point cloud map to efficiently support subsequent path planning algorithms.

[0055] The optimized poses and covariance matrices of each autonomous vehicle are obtained using existing distributed cooperative SLAM frameworks. In one specific implementation, a distributed pose graph optimization method (such as the distributed Gauss-Seidel algorithm) is used to collaboratively optimize the poses of all autonomous vehicles. During this pose graph optimization process, the covariance matrix of the pose is obtained by solving for the inverse or approximate inverse of the Fisher information matrix. This is a method for quantifying uncertainty that is well known to those skilled in the art, and will not be elaborated here.

[0056] The multi-vehicle cooperative exploration trajectory planning method in this embodiment is not limited to the implementation method of front-end SLAM and can be integrated into various multi-vehicle cooperative exploration systems, which has good versatility and integrability.

[0057] It should be noted that step S1 takes a global perspective, assigning exploration tasks to each autonomous vehicle and generating a comprehensive and load-balanced global path, laying the foundation for subsequent refined local planning. Specifically, this includes:

[0058] ① Identify leading voxels on the global environment map and form a global task point set through clustering.

[0059] Each autonomous vehicle conducts forward detection based on a global environment map, identifying forward voxels, which are passable voxels adjacent to unknown areas (voxels that have never been covered by LiDAR scanning).

[0060] Based on spatial proximity, a clustering algorithm is used to aggregate these discrete front voxels into several clusters by calculating the Euclidean distance between them. Each cluster represents a potential region to be explored.

[0061] Furthermore, a global task point set is formed through clustering, including:

[0062] Multiple points are sampled around the centroid of each cluster and projected onto the global environment map. The node closest to the projected point in the global environment map is then used as the candidate viewpoint for each cluster.

[0063] For each cluster, multiple points are sampled on a spherical shell surrounding its centroid. To ensure the reachability of these points, they are all projected onto a global environment map, and the node closest to each projected point is found from the map as a candidate viewpoint for each cluster.

[0064] The expected information gain of each candidate viewpoint is evaluated, and the candidate viewpoint with the highest expected gain in each cluster is selected as the exploration target to form a global task point set.

[0065] It should be noted that the expected information gain for each candidate viewpoint is obtained through a visibility-based scoring function. This function simulates the visible range of a lidar by projecting multiple rays from the candidate viewpoint's location, obtaining the set of unknown voxels covered by each ray, and then summing the weight coefficients of each unknown voxel in the set of unknown voxels corresponding to each candidate viewpoint to obtain the expected information gain for each candidate viewpoint, as shown in the formula below:

[0066]

[0067] Among them, U v This represents the set of unknown voxels visible from the candidate viewpoint v, and f(u) is the weight coefficient of the unknown voxel u. The weight coefficient is set according to the actual situation; it can be initialized to 1, or different values ​​can be set according to factors such as distance.

[0068] Finally, the candidate viewpoint with the highest expected information gain in each cluster is selected as the exploration target; all exploration targets constitute the global task point set.

[0069] ② The problem of multiple unmanned vehicles accessing the global task point set is modeled as a multi-vehicle parking lot vehicle pathing problem. The optimization objective is to minimize the total driving cost and balance the workload of each unmanned vehicle. By solving the multi-vehicle parking lot vehicle pathing problem, a task point sequence is assigned to each unmanned vehicle, and a global exploration path is generated for each unmanned vehicle.

[0070] It should be noted that the Multi-Depot Vehicle Routing Problem (MDVRP) involves vehicles departing from multiple depots (originating points), serving customers, and returning to the depot. In this embodiment, each autonomous vehicle has its own starting point, similar to a depot, and needs to explore task points, similar to serving customers. Therefore, the problem of multiple autonomous vehicles accessing the global task point set is modeled as a multi-depot vehicle routing problem, where the number of autonomous vehicles is N, the total number of targets to be explored in the global task point set is M, and the number of targets v to be explored is M. i to v j Path cost c ij The result is calculated on the global environment map using the A* algorithm. A binary decision variable x is introduced. ijk ∈{0,1} indicates whether the driverless car k has started from v i Drive to v j .

[0071] Furthermore, the optimization objective is to minimize the total driving cost and balance the workload of the autonomous vehicle, as shown in the following formula:

[0072]

[0073] Among them, L maxλ represents the maximum path length allocated to all autonomous vehicles; λ represents the balancing weight coefficient, preferably set to 1, to indicate that the total driving cost is equally important as the load balancing of each autonomous vehicle.

[0074] Furthermore, solving the above problem is subject to the following constraints:

[0075] Each task point must be visited by one autonomous vehicle exactly once, as shown in the formula below:

[0076]

[0077] Each autonomous vehicle must start from its own starting point once, as shown in the formula below:

[0078]

[0079] in, Indicate whether the driverless car k starts from the starting point s. k Depart and drive to v j .

[0080] Each autonomous vehicle cannot directly return to its starting point, thus avoiding invalid solutions that only take one step and immediately return to the starting point. The formula is shown below:

[0081]

[0082] in, Indicates whether driverless car k is from v i Depart and drive to the starting point.

[0083] For any intermediate exploration target, the number of times each autonomous vehicle enters that point must equal the number of times it leaves that point. This ensures the continuity of the path, as shown in the formula below:

[0084]

[0085] Where, x ihk Indicates whether driverless car k is from v i Drive to the middle to explore the target v h x hjk Indicates whether the autonomous vehicle k explores the target v from the middle. h Drive to v j .

[0086] The total path length assigned to each autonomous vehicle cannot exceed L. max The formula is shown below:

[0087]

[0088] Among them, L kThis represents the total path length of the autonomous vehicle k.

[0089] This embodiment does not limit the specific algorithm for solving MDVRP; any method that can effectively solve the model is applicable to this step, such as using the Gurobi or CPLEX solvers. After solving, the sequence of task points that each autonomous vehicle needs to visit is obtained, i.e., the global exploration path, laying the foundation for subsequent local path planning using loopback sensing.

[0090] Step S2 is used to actively evaluate the value of loop closures, select the loop closure point that is most effective in improving positioning accuracy, and use it as the path anchor point.

[0091] Specifically, it includes the following steps:

[0092] ① Within the local area of ​​the current autonomous vehicle, the future pose of the current autonomous vehicle is matched with the historical poses of other autonomous vehicles to generate a candidate set of loop closures.

[0093] Extract path segments within a local area from the current global exploration path, and filter historical poses located within the local area from the historical trajectories of other autonomous vehicles;

[0094] It should be noted that the area centered on the current location of the autonomous vehicle and with the effective sensing range of the sensors (LiDAR) as its radius is considered the local area for fine-tuning in this step. The intersection points between the global exploration path and the boundary of this local area are then calculated, defining the boundaries of the local area. Subsequently, path segments falling within this local area are extracted from the global exploration path.

[0095] From the historical keyframes of other autonomous vehicles, identify whether there are historical keyframes located in the local area. If it is empty, it means that no other autonomous vehicle has been in this area and there is no opportunity for collaborative loopback. Return an empty result directly and terminate this step. If it is not empty, obtain the optimized pose of the historical keyframes located in the local area as the historical optimized pose to be compared.

[0096] Furthermore, based on the current optimized pose of the autonomous vehicle, the covariance matrix of the pose, and the path segments within the local region, its future pose sequence and the corresponding predicted covariance matrix sequence are predicted.

[0097] It should be noted that the Lie group motion model is used to recursively predict the future pose and its covariance matrix (i.e., uncertainty information).

[0098] Predict future pose using the following pose propagation formula:

[0099] X n =X n-1 exp([Ω n ] ∧ ),

[0100] Among them, X n ∈SE(3) represents the predicted pose at time step n, X n-1 Ω represents the pose at time step n-1. n ∈R 6 Represents the Lie algebraic motion increment based on the control input; [·] ∧ This represents the operation of converting the 6-dimensional Lie algebraic motion increment into a 4×4 matrix in the Lie group SE(3); exp(·) represents the exponential function.

[0101] The covariance matrix of the future pose can be predicted using the following covariance propagation formula:

[0102]

[0103] Where, ∑ n,n Indicates the predicted pose X n The covariance matrix, ∑ n-1,n-1 Indicates pose X n-1 The covariance matrix of Q; n Φ(·) represents the control noise covariance; Φ(·) represents the right Jacobian matrix. Represents Ω n The corresponding SE(3) matrix; T represents the transpose operation; F n ∈R 6×6 This represents the state transition matrix.

[0104] Furthermore, the state transition matrix is ​​calculated using the following formula:

[0105]

[0106] Among them, L n ∈R 6×6 Indicates the increment of motion Ω n Relative to the error state at the previous moment, Ad(·) represents an adjoint operator on SE(3), which transforms the reference coordinate system of the covariance from the pose coordinate system at the previous moment to the pose coordinate system at the current moment.

[0107] The predicted future pose sequence is matched with the selected historical optimized poses, and pose pairs that meet the distance threshold are added to the loop closure candidate set.

[0108] Iterate through each future pose predicted at each moment within the local region, find the nearest neighbor pose whose Euclidean distance to the future pose is within the distance threshold among the selected historical optimized poses, and add each pair of poses that meets the condition to the loop closure candidate set.

[0109] ②Calculate the utility score for each pose pair in the candidate loop closure set.

[0110] It should be noted that for each pose pair in the loop closure candidate set, the utility score is calculated using the predicted covariance matrix of the future pose and the covariance matrix of the historical optimized pose. The utility score is obtained by calculating the product of the D-optimality information gain and the Euclidean distance penalty term.

[0111] Specifically, firstly, based on the relative pose observation model, the relative pose observation value of each pose pair is predicted.

[0112] The autonomous vehicle p predicts the future pose at time l. The optimized pose of the autonomous vehicle q at the m-th time in the stored historical keyframes. It is a pose pair in the candidate set of loop closures, which is modeled as a relative pose observation z. pq :

[0113]

[0114] Where log(·) represents a 4×4 matrix mapped from the pose matrix in the Lie group SE(3) to its corresponding Lie algebra space se(3); [·] ∨ This indicates that the 4×4 matrix in the Lie algebra se(3) is transformed into a 6-dimensional vector, where v is a 6-dimensional vector representing Gaussian observation noise with a mean of 0 and a covariance of R1; (·) -1 This indicates the calculation of the inverse matrix.

[0115] Furthermore, the innovation covariance is calculated based on the Jacobian matrix of the relative pose observations and the covariance matrix of the pose pairs.

[0116] It should be noted that, relative to the observation of the global state, the Jacobian matrix H pq It is a very sparse matrix with only two non-zero blocks H. pq,p and H pq,q The formula corresponding to the two poses connected by this loop constraint is shown below:

[0117] H pq,q =I6,

[0118] Among them, H pq,p Indicates if pose A small change occurs, relative to the pose observation value z pq The change in H pq,q Indicates if pose A small change occurs, relative to the pose observation value z pq The change in pose, I6 is a 6×6 identity matrix representing the pose. Tiny changes will directly affect the observed value z in a 1:1 ratio. pq .

[0119] Furthermore, the information covariance S of the loop observations is calculated. pq To quantify the observed value z pq The total uncertainty is a 6×6 matrix, represented by the following formula:

[0120]

[0121] Where ∑ represents the joint covariance matrix, which contains the covariance matrix of the historical keyframes and the covariance matrix of the predicted pose, describing the uncertainty of the autonomous vehicle's pose itself; H pq Represents the relative pose observation value z pq The Jacobian matrix; This represents the uncertainty matrix in the observation space, describing the relative pose observation value z. pq The uncertainty is R2, which is the covariance matrix of the observation noise, representing the uncertainty of the observation itself. This is a fixed value, usually determined by the characteristics of the sensor and the registration algorithm.

[0122] Furthermore, the D-optimality information gain is calculated based on the new information covariance; the utility score is obtained by calculating the product of the D-optimality information gain and the Euclidean distance penalty term.

[0123] Specifically, by fusing the D-optimality information gain and the Euclidean distance from the current position of the autonomous vehicle to the future pose in the pose pair, the utility score of each pose pair in the candidate loop closure set is calculated using the following formula:

[0124]

[0125] Among them, u pq Indicates pose pair The utility score, The logarithm of the determinant of the inverse of the information covariance represents the D-optimality information gain. A larger value indicates that the loop observation provides more information and can reduce current uncertainty to a greater extent; exp(-γd pq ) represents a distance penalty term, d pq Let represent the Euclidean distance from the current position of the autonomous vehicle to the loop closure candidate point (the future pose in the pose pair), and γ represent a decay coefficient greater than 0, which increases with d. pq The exponential decay of the value is used to penalize loops that require a long detour to complete, as long journeys are time-consuming, energy-intensive, and may introduce more errors.

[0126] ③ Select the top K pose pairs with the highest utility scores from the candidate loop closure pair set, and the future poses of these pairs are the candidate loop closure points, where K > 0.

[0127] Finally, the obtained loop closure candidate points are used as path anchor points in the next step of local path planning. In other words, the current autonomous vehicle is guided to actively move to the future pose with a high utility score to create loop closure constraints with other autonomous vehicles, thereby optimizing global positioning accuracy.

[0128] In step S3, local path planning is performed based on the intersection of the global exploration path and the local area of ​​each autonomous vehicle, the leading viewpoint and path anchor point within the local area, to obtain their respective local optimized paths, including:

[0129] The intersection of the current autonomous vehicle's global exploration path with the local area, the leading viewpoint within the local area, and the path anchor point are used as target points. Among them, the intersection point ensures a smooth connection between the local planned path and the global exploration path, maintaining the continuity of the path; the leading viewpoint is the exploration target within the local area where the global task points are concentrated when the global exploration path is obtained in step S1, ensuring that the autonomous vehicle can still effectively perform exploration tasks and acquire environmental information within the local area; the path anchor point is the key to achieving active localization, guiding the autonomous vehicle to actively go to locations that can significantly improve positioning accuracy.

[0130] Starting from the current autonomous vehicle and aiming at finding the shortest path to the target point, the path cost is obtained based on the global environment map of the current autonomous vehicle. By solving the Traveling Salesman Problem (TSP), the local optimized path of the current autonomous vehicle is obtained, which is used to generate control commands to guide the movement of the current autonomous vehicle.

[0131] For example, a local search heuristic algorithm or a metaheuristic algorithm can be used to solve the TSP problem.

[0132] It should be noted that steps S2 and S3 are a cyclical process. For each autonomous vehicle, after obtaining the local optimized path based on steps S2 and S3 for the current local area, the local optimized path is executed with the corresponding control command to update the autonomous vehicle's pose and covariance matrix. When the autonomous vehicle moves to a new position and the local area undergoes substantial changes, it returns to step S2 to determine the new local area and perform a new round of local area path planning until the exploration task is completed.

[0133] Compared with existing technologies, this embodiment provides a multi-vehicle collaborative exploration trajectory planning method that integrates positioning loopback point constraints. By actively creating high-quality candidate loopback opportunities, evaluating the utility scores of candidate loopbacks and using them as path anchor points, it transforms maintaining positioning accuracy from a passive backend optimization to an active frontend planning objective. This fundamentally solves the problem of the disconnect between positioning and planning, effectively suppressing the cumulative drift that inevitably occurs during long-term exploration in feature-lacking environments, and achieving a synergistic improvement in positioning and exploration performance. Employing MDVRP-based global task allocation effectively balances the workload among multiple unmanned vehicles, avoiding uneven task distribution. Simultaneously, in local planning, it integrates the intersection of the global exploration path and the local area, the leading viewpoint, and loopback candidate points for TSP optimization, generating an optimal path that simultaneously considers exploration and positioning benefits. This significantly reduces redundant coverage and ineffective movement, improving the overall exploration efficiency and collaboration of multiple unmanned vehicles.

[0134] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware, and the program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0135] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A multi-vehicle cooperative exploration trajectory planning method integrating positioning loop point constraints, characterized in that, Includes the following steps: Based on the global environment map, a global exploration path is generated for each autonomous vehicle; Within the local area of ​​each autonomous vehicle, based on its own global exploration path, the pose of each autonomous vehicle and the covariance matrix of the pose, multiple loop candidate points with the highest utility scores are selected as path anchor points. Local path planning is performed based on the intersection of the global exploration path of each autonomous vehicle with the local area, the leading viewpoint within the local area, and the path anchor point to obtain the local optimized path for each autonomous vehicle.

2. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints as described in claim 1, characterized in that, The generation of a global exploration path for each autonomous vehicle based on a global environment map includes: Identify leading voxels on the global environment map and form a global task point set through clustering; The problem of multiple unmanned vehicles accessing the global task point set is modeled as a multi-vehicle parking lot vehicle pathing problem. The optimization objective is to minimize the total driving cost and balance the workload of each unmanned vehicle. By solving the multi-vehicle parking lot vehicle pathing problem, a task point sequence is assigned to each unmanned vehicle, and a global exploration path is generated for each unmanned vehicle.

3. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints as described in claim 2, characterized in that, The process of forming a global task point set through clustering includes: Multiple points are sampled around the centroid of each cluster and projected onto the global environment map. The node closest to the projected point in the global environment map is obtained as the candidate viewpoint for each cluster. The expected information gain of each candidate viewpoint is evaluated, and the candidate viewpoint with the highest expected gain in each cluster is selected as the exploration target to form a global task point set.

4. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints as described in claim 3, characterized in that, The evaluation of the expected information gain for each candidate viewpoint includes: Multiple rays are projected from the position of each candidate viewpoint to obtain the set of unknown voxels covered by each ray; The weight coefficients of each unknown voxel in the set of unknown voxels corresponding to each candidate viewpoint are summarized to obtain the expected information gain of each candidate viewpoint.

5. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints according to claim 1, characterized in that, The process of selecting multiple loop candidate points with the highest utility scores includes: Within the local area of ​​the current autonomous vehicle, the future pose of the current autonomous vehicle is matched with the historical poses of other autonomous vehicles to generate a loop closure candidate set; For each pose pair in the candidate loop closure set, calculate the utility score; Select the top K pose pairs with the highest utility scores from the candidate loop closure pair set, and the future poses of these pairs are the candidate loop closure points, where K > 0.

6. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints according to claim 5, characterized in that, The generation of the loop closure candidate set includes: Extract path segments within a local area from the current global exploration path of the autonomous vehicle, and filter out historical poses located within the local area from the historical trajectories of other autonomous vehicles; Based on the current pose of the autonomous vehicle, the covariance matrix of the pose, and the path segments in the local area, predict its future pose sequence and the corresponding predicted covariance matrix sequence. The predicted future pose sequence is matched with the selected historical poses, and pose pairs that meet the distance threshold are added to the loop closure candidate set.

7. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints according to claim 5, characterized in that, For each pose pair in the candidate loop closure set, a utility score is calculated, including: Based on the relative pose observation model, predict the relative pose observation value for each pose pair; The innovation covariance is calculated based on the Jacobian matrix of the relative pose observations and the covariance matrix of the pose pairs. The D-optimality information gain is calculated based on the new information covariance; the utility score of each pose pair is calculated by fusing the D-optimality information gain with the Euclidean distance from the current position of the autonomous vehicle to the future pose in the pose pair.

8. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints according to claim 7, characterized in that, The utility score for each pose pair is calculated using the following formula: Among them, u pq Indicates pose pair The utility score, This represents the predicted future pose of the autonomous vehicle p at the l-th moment. This represents the historical pose of the autonomous vehicle q at time m. S pq Indicates the new information covariance; The logarithm of the determinant of the inverse of the information covariance represents the D-optimality information gain; d pq Future pose from the current position of the autonomous vehicle to its pose alignment The Euclidean distance; γ represents an attenuation coefficient greater than 0.

9. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints according to claim 2, characterized in that, The optimization objective is expressed by the following formula: Where N represents the total number of autonomous vehicles, M represents the total number of targets explored in the global task point set, and L max c represents the maximum path length assigned to all autonomous vehicles; λ represents the balancing weight coefficient. ij Indicates the exploration target v i to v j Path cost, x ijk ∈{0,1} indicates whether the driverless car k has started from v i Drive to v j .

10. The multi-vehicle cooperative exploration trajectory planning method with integrated positioning loop point constraints according to claim 1, characterized in that, The local optimized path for each autonomous vehicle is obtained through the following steps: The intersection of the current global exploration path of the autonomous vehicle with the local area, the leading viewpoint within the local area, and the path anchor point are taken as the target points; Starting from the current autonomous vehicle and aiming at finding the shortest path to all target points, the path cost is obtained based on the global environment map. By solving the traveling salesman problem, the local optimized path of the current autonomous vehicle is obtained.

Images