A collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism

Abstract

This invention discloses a collaborative path planning method based on multi-constraint improved A* algorithm and a dynamic look-ahead mechanism. It constructs a feature distance field map containing a nonlinear safety potential field, transforming discrete environmental information into continuous safety gradients. An improved A* algorithm, incorporating dynamic adaptive heuristic weights, nonlinear safety potential field costs, and kinematic smoothness constraints, is used to plan the globally optimal path. A velocity-adaptive dynamic look-ahead mechanism is established to calculate dynamic look-ahead points that conform to the current motion state in real time. A multi-objective evaluation function containing a global traction index is introduced into the local planning process to optimize control commands by measuring the deviation between the local trajectory and the global intent. This invention effectively solves the problems of "wall-hugging effect," high computational cost in large-scale environments, and poor coordination between global and local planning in traditional methods, improving the navigation safety and smoothness of robots in complex unstructured scenarios.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and control of intelligent vehicles, specifically to a collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism. Background Technology

[0002] With the rapid development of intelligent manufacturing and smart logistics technologies, autonomous mobile robots (AMRs / AGVs) have been widely used in complex and unstructured scenarios. Path planning, as the core component of the AMR navigation system, is mainly tasked with planning an optimal trajectory that satisfies the constraints of collision-free operation, smoothness, and vehicle nonholometry in a configuration space containing static obstacles and dynamic disturbances.

[0003] Existing technologies typically employ a hierarchical planning architecture, where a global path planner, such as the traditional A* algorithm, generates a static reference path, followed by a local planner for dynamic obstacle avoidance. However, in practical applications, the traditional A* algorithm, based on a binary grid map and using only path length as an optimization metric, tends to plan paths close to obstacle edges (i.e., the "wall-hugging effect"). In complex scenarios such as large warehouses, the traditional A* algorithm uses static heuristic weights, lacking strong guidance for distant target points in the early stages of the search. This results in traversing a large number of redundant nodes, consuming significant computational resources, and failing to meet the real-time replanning requirements in dynamic environments.

[0004] Existing local planners primarily focus on "local obstacle avoidance," lacking an effective mechanism for tracking the global path. When a smart car deviates from the global path to avoid obstacles, the lack of dynamic guidance based on its current speed state can easily lead to the car failing to smoothly return to the globally optimal path, resulting in control oscillations or getting stuck in local minima. Summary of the Invention

[0005] To address the problems mentioned in the background, this invention discloses a collaborative path planning method based on multi-constraint improved A* algorithm and dynamic look-ahead mechanism. By combining the multi-constraint improved A* algorithm with a dynamic look-ahead mechanism, the wall-hugging effect is effectively avoided. Furthermore, the dynamic look-ahead mechanism is used to design a global traction force to achieve global-local deep collaborative tracking. This provides a better solution for intelligent vehicle path planning.

[0006] Technical solution:

[0007] This invention discloses a collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism, the method comprising the following steps:

[0008] S1: Construct a nonlinear safety feature field; acquire an environmental grid map, calculate the Euclidean distance from the free area to the nearest obstacle, and map this distance to a nonlinear safety potential field value to generate a feature distance field map;

[0009] S2: In the feature distance difference map, the global path planner uses the multi-constraint improved A* algorithm to search for the globally optimal path from the starting point to the target point;

[0010] S3: The local planner establishes a dynamic look-ahead traction mechanism based on speed adaptation, calculates and extracts the corresponding dynamic look-ahead points on the global path;

[0011] S4: Perform trajectory cluster sampling and construct a global traction index. The multi-objective evaluation function scores the candidate trajectory, and the velocity command corresponding to the candidate trajectory with the best comprehensive score is selected as the control output.

[0012] S5: Send the preferred speed command to the underlying motor driver to drive the vehicle to move, and repeat steps S3 to S5 until the vehicle reaches the target point.

[0013] Furthermore, the specific steps of S1 are as follows:

[0014] The original two-dimensional occupancy grid map of the environment is obtained, and the map is binarized by setting an occupancy threshold to distinguish between obstacle areas and free passage areas. Using a linear time complexity Euclidean distance transformation algorithm, each free node in the map is traversed to calculate the precise Euclidean distance from the center of the node to the nearest obstacle boundary. Based on the Euclidean distance, a nonlinear safety potential field function in the form of Sigmoid is constructed to map the geometric distance to the potential field cost, so that the cost of the area close to the obstacle increases exponentially, while the cost of the area far from the obstacle smoothly decays to zero, generating a feature distance field map containing environmental safety gradient information.

[0015] Furthermore, the multi-constraint improved A* algorithm described in S2 includes a multi-constraint improved cost function, wherein the multi-constraints include: dynamic adaptive heuristic weights. Kinematic smoothness constraint and the cost of nonlinear safety potential field ;

[0016] The total cost function of a node is defined as a linear weighted sum of four components:

[0017]

[0018] Where G(n) is the actual movement cost from the starting point to the current point; H(n) is the Euclidean distance heuristic to the destination; and These are the safety potential field weights and the kinematic constraint weights, respectively.

[0019] Furthermore, the dynamic adaptive heuristic weights The weighting factor is positively correlated with the Euclidean distance from the current node to the target point in a non-linear manner. In the early stage of the search, the weight is relatively large, and the target region is quickly approached through greedy best-first search. In the later stage of the search, the weight decays to close to 1, and the algorithm reverts to the standard A* algorithm to ensure the geometric optimality of the path.

[0020] Furthermore, the kinematic smoothing constraint Specifically, the path's steering angle is obtained by calculating the angle between the parent node vector and the current extended vector. For nodes that exceed the vehicle's maximum permissible steering angle, an infinite cost is imposed for pruning. For steering angles within the permissible range, a smoothing penalty proportional to the square of the angle is applied.

[0021] Furthermore, the cost of the nonlinear security potential field The potential field value is directly invoked to penalize nodes within the potential field's influence range, forcing the search tree to automatically avoid high-risk areas.

[0022] Furthermore, the speed-adaptive dynamic look-ahead traction mechanism described in S3 includes:

[0023] The vehicle's current linear velocity is acquired in real time using chassis sensors. A piecewise linear mapping model between the look-ahead distance and linear velocity is established. Minimum and maximum look-ahead distance thresholds are set, and the optimal look-ahead distance at the current moment is calculated. In the global pathpoint sequence, the pathpoint closest to the vehicle's current position is located. An index search is performed along the path direction, and pathpoints whose cumulative path length equals the optimal look-ahead distance are extracted and marked as dynamic look-ahead points. .

[0024] Furthermore, the piecewise linear mapping model between the look-ahead distance and linear velocity includes: establishing the look-ahead distance... The linear mapping relationship with the vehicle's real-time linear velocity v(t):

[0025]

[0026] Parameter settings: ; ; , The model ensures that the look-ahead point is always located at the position that the vehicle is expected to reach in about 1 second, achieving constant look-ahead over time.

[0027] Furthermore, the specific steps of S4 are as follows:

[0028] Based on the differential drive kinematic model, in the dynamic window ( , , ) performs gridded sampling within the range, generating N=40 A set of velocity command pairs. For each set of commands, the future is deduced using Euler's integral. The machine pose sequence within a 2.0s time period is used to form a candidate trajectory cluster. For each candidate trajectory... Calculate the cost function :

[0029]

[0030] in, As a consequence of obstacle risk, The cost of speed smoothing, i.e., smoothness. The cost of maintaining course consistency As a cost to global traction force, the speed command corresponding to the candidate trajectory with the best comprehensive score is selected as the control output.

[0031] Furthermore, the global traction cost Defined as the position of the candidate trajectory end and the dynamic look-ahead point Euclidean distance:

[0032]

[0033] in, For the pose of the candidate trajectory at the end of the prediction time domain, the global traction cost is used. The deviation between the local trajectory and the global intention is measured, and a centripetal force for regressing to the global path is introduced during the local planning process.

[0034] Beneficial effects

[0035] 1. This invention addresses the issues of high computational resource consumption and poor real-time performance of traditional A* algorithms in large-scale complex scenarios by employing a multi-constraint improved A* algorithm. In the initial search phase, it utilizes larger weights to achieve a "greedy" search, quickly approaching the target region and significantly reducing the expansion of redundant nodes. As the target point approaches, the weights automatically decay, ensuring the final path achieves near-optimality in geometric distance. While maintaining path quality, it further improves the algorithm's computational speed, meeting the real-time replanning requirements in dynamic environments.

[0036] 2. This invention overcomes the limitations of traditional binary grid maps by constructing a nonlinear safety feature distance field based on the Sigmoid function, thus providing continuous gradient information for environmental obstacles. This provides a safe buffer zone for the intelligent vehicle, further avoiding the wall-hugging effect seen in the traditional A* algorithm. Moreover, by utilizing the smooth transition characteristics of the potential field, the curvature changes of the planned trajectory are made more gentle, which is conducive to the stable execution of the underlying control.

[0037] 3. This invention establishes a speed-adaptive dynamic look-ahead mechanism and an optimal strategy incorporating a "global traction force" index. The dynamic look-ahead mechanism ensures that the intelligent vehicle can obtain the optimal aiming distance at different speeds, solving the problems of cutting the inner circle at low speeds and being prone to overshooting at high speeds. The global traction force index introduces a restoring force similar to a "virtual spring" in local planning, enabling the intelligent vehicle to quickly and smoothly converge back to the globally optimal path after temporarily avoiding dynamic obstacles. Through global-local coupling and cooperation, a balance between obstacle avoidance flexibility and path tracking accuracy is achieved. Attached Figure Description

[0038] Figure 1 This is a flowchart illustrating the overall process architecture of the method of this invention.

[0039] Figure 2 This is a schematic diagram illustrating the principle of constructing the nonlinear security feature distance field of the present invention;

[0040] Figure 3 This is a block diagram of the global path search logic of the present invention;

[0041] Figure 4 This is a flowchart of the dynamic look-ahead mechanism of the present invention; Detailed Implementation

[0042] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0043] like Figure 1 As shown, this invention discloses a collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism. This embodiment constructs a high-fidelity digital simulation verification platform. The computing device runs the Ubuntu 20.04 operating system and, relying on the ROSNoetic middleware and Gazebo physics engine, builds an unstructured complex warehouse scene containing static obstacles (shelves, walls) and dynamic interference (moving pedestrians). The simulation robot model is configured with a differential drive chassis, equipped with a virtual laser radar (LaserRadar) with a scanning radius of 12m and a frequency of 10Hz, and a high-precision virtual odometry. The steps of the method of this invention are as follows:

[0044] S1: Environmental perception and high-dimensional feature map preprocessing. In response to the problem of insufficient information in traditional binary raster maps, this step aims to transform discrete environmental information into a continuous security potential field.

[0045] S1.1: Linearization of map data

[0046] The system subscribes to the global map topic / map in the simulation environment to obtain the resolution. Original raster map with a resolution of 0.05m Let the map size be... First, based on the occupancy probability threshold (e.g. >65) Extract the obstacle pixel set and free space pixel set .

[0047] S1.2: Euclidean Distance Transform (EDT) with linear time complexity

[0048] Real-time computing under limited computing resources Each pixel in To determine the nearest Euclidean distance, this embodiment employs the Meijster algorithm. This algorithm decomposes the two-dimensional distance transformation into two independent one-dimensional scanning processes: first, a vertical distance transformation is performed on each column, and then a horizontal distance transformation is performed on each row using the parabolic envelope principle. The algorithm has a time complexity of O(N), where N is the total number of map pixels, ensuring the efficiency of map preprocessing.

[0049] S1.3: Sigmoid nonlinear potential mapping

[0050] Obtain the distance field Then, it needs to be converted into a cost value. Traditional linear inflation models suffer from gradient discontinuity. This embodiment designs a nonlinear potential field model based on the sigmoid function. To simulate the "repulsive force" in a physical field:

[0051]

[0052] Parameter optimization settings and their physical significance:

[0053] (Maximum Potential Energy): Set to 254. This value determines the repulsion strength of the obstacle's core area, ensuring that the path will never pass through the obstacle.

[0054] (Safe cut-off distance): Set to the width of the robot body 2.5 times (for example) =1.0m). When the distance to the obstacle is greater than this value, the potential field value is 0, to avoid unnecessary interference to paths far from the obstacle.

[0055] (Critical Danger Distance): Set to / 2+0.1m (e.g., 0.4m). Within this distance, the potential field value will increase exponentially, forcing the planner to detour.

[0056] (Gradient Steepness Factor): A value of 6.0 is preferred. This parameter determines the slope of the S-curve. The larger the potential field, the "harder" the edge of the potential field, and the heavier the punishment for "wall-hugging" behavior.

[0057] S2: Global planning based on multi-constraint improved A*, performing global path search after the feature distance field is constructed. This embodiment constructs a quaternary evaluation function:

[0058]

[0059] Multiple constraints include: dynamic adaptive heuristic weights Kinematic smoothness constraint and the cost of nonlinear safety potential field G(n) is the actual movement cost from the starting point to the current point; H(n) is the heuristic value of the Euclidean distance to the destination. and These are the safety potential field weights and the kinematic constraint weights, respectively.

[0060] S2.1: Dynamic Adaptive Heuristic Weights Design and derivation:

[0061] To address the problem of node explosion in OpenList maps at large scales, this invention introduces a dynamic weighting strategy. The core idea is to employ a "greedy search" approach for rapid progression when far from the endpoint, and to revert to an "optimal search" approach as the endpoint approaches. The weighting function is constructed as follows:

[0062]

[0063] Parameter analysis:

[0064] (Weighting coefficient): Preferably set to 3.0. This means that at the starting point, the heuristic weight is approximately 4.0, and the algorithm behaves as a weighted A*, which is extremely inclined to expand in the direction of the target and ignores the width of the search.

[0065] (Attenuation index): Preferably set to 2.0. This causes the weight to decrease parabolically as the distance decreases.

[0066] Technical Results: Experiments show that this strategy reduces the number of expanded nodes during the search process by approximately 45%, while also reducing the number of nodes near the endpoint. This ensures that the final path is geometrically near-optimal.

[0067] S2.2: Kinematic Smoothing Constraint Implementation:

[0068] To generate smooth paths that meet nonholonomic constraints, sharp turns are suppressed during the topology search phase. Let the current expanded node be n, its parent node be P, and the parent node's parent node be GP. Define the previous path vector. Current path vector Calculate the heading change angle. :

[0069]

[0070] Construct a piecewise penalty function:

[0071]

[0072] (Maximum permissible steering angle): Set based on the vehicle's minimum turning radius (e.g.) ).

[0073] Pruning mechanism: When When the cost is infinite, node n will not be added to the OpenList. This is equivalent to directly pruning all non-executable polyline paths at the graph search level.

[0074] S3: The local planner establishes a dynamic look-ahead traction mechanism based on speed adaptation, calculates and extracts the corresponding dynamic look-ahead points on the global path;

[0075] After the global path is generated, a local planner is needed for tracking. Traditional PurePursuit or DWA algorithms typically use a fixed look-ahead distance, leading to slow inner path cutting and oscillations at high speeds. This embodiment designs a dynamic look-ahead strategy.

[0076] S3.1: Velocity-Distance Mapping Model:

[0077] Establish forward-looking distance The linear mapping relationship with the vehicle's real-time linear velocity v(t):

[0078]

[0079] Parameter settings: ; ; , .

[0080] Physical meaning: This formula ensures that the look-ahead point is always located at the position that the vehicle is expected to reach in about 1 second, achieving constant look-ahead in the time dimension.

[0081] S3.2: Dynamic Look-Ahead Point Search and projection:

[0082] In each control cycle (ControlLoop, T=50ms), perform the following operations:

[0083] Closest point search (KD-Tree acceleration): To quickly find the point closest to the vehicle in a large set of waypoints. In this embodiment, global pathpoints are constructed as a KD-Tree structure, reducing the search complexity from O(N) to O(N). ).

[0084] Lookahead point extraction: Starting from the index of P_{nearest}, accumulate the path segment length along the path index in ascending direction until the accumulated distance is reached. The corresponding path point at this point is the dynamic lookahead point. .

[0085] S4: Real-time trajectory extrapolation and optimization with global-local coordination

[0086] S4.1: Trajectory Cluster Sampling

[0087] Using the robot's current pose (position and heading) in the global coordinate system as the initial boundary conditions, and based on the differential drive kinematics model, within a dynamic window... Perform gridded sampling within the area to generate N groups (e.g. (Velocity command pairs.) For each set of commands, the future time domain is predicted using Euler integrals. The machine pose sequence within (e.g., 2.0 s) is used to form a candidate trajectory cluster. Specifically, the prediction time domain... The settings need to be consistent with the dynamic look-ahead parameters in step S3. Maintaining consistency in spatiotemporal scales (usually) This ensures that the derived candidate trajectories can extend geometrically to the dynamic lookahead point. The neighborhood or beyond that point. This design ensures that the candidate trajectory ends within... With global forward-looking perspective Being on the same spatial scale provides an effective geometric basis for subsequent calculations, avoiding global guidance failure due to excessively short predicted trajectories.

[0088] S4.2 Design of Collaborative Evaluation Function

[0089] For each candidate trajectory Calculate the cost function This invention innovatively introduces the term "global traction force".

[0090]

[0091] The specific calculation logic for each indicator is as follows:

[0092] (Obstacle Risk): Potential field map generated in step S1 for all points on the sampling trajectory The coordinates in the equation are used to determine the risk cost of the trajectory, with the maximum potential field value being taken as the risk cost.

[0093] (Smoothness): Calculation Limit abrupt acceleration changes.

[0094] (Global traction): Core synergy indicator, see step S4.3 for details.

[0095] (Heading Consistency Cost): A penalty introduced during the planning process to ensure that the vehicle's actual heading is consistent with the desired heading (such as a reference path or target direction) in order to improve tracking accuracy and stability.

[0096] S4.3: Calculation of Global Traction Index Based on Spatiotemporal Consistency

[0097] Extracting candidate trajectories in the prediction time domain End-effector pose And calculate its dynamic look-ahead point determined in step S3. The square of the Euclidean distance between them:

[0098]

[0099] This indicator constructs a deep constraint on local control and global planning in the spatiotemporal dimensions. This represents the robot's behavior under the current control commands, after... The physical location that can actually be reached after a certain time; and This represents the optimal reference position that, based on the current speed and the global path topology, should theoretically be reached on the same time scale. Minimizing this generation's value essentially forces the robot's future motion state (local prediction) and the optimal path guidance (global expectation) to be spatially aligned through a "virtual spring" force field, thereby providing a centripetal traction force that continuously returns to the global path during obstacle avoidance.

[0100] S4.4: Multidimensional Feature Normalization and Optimal Trajectory Decision

[0101] Due to the evaluation function The components in the equation have different physical dimensions (e.g., ...). For potential field strength, Due to speed difference, (For spatial distance), direct linear weighting can cause larger indicators to mask smaller ones. This step uses normalization and a competition mechanism to achieve the optimal control command output.

[0102] Dimensional unification:

[0103]

[0104] Traverse all candidate trajectories and count the extreme values ​​of each indicator. The Min-Max normalization method is used to map each cost to... The dimensionless interval ensures that all evaluation factors are on the same order of magnitude.

[0105] Dynamic game theory and collaborative mechanisms:

[0106] The total cost J is calculated based on the normalized data. This process embodies a dynamic game mechanism of "local constraints" and "global guidance":

[0107] Obstacle avoidance dominant state: When the vehicle approaches an obstacle, the nonlinear safety potential field term... As the number of obstacles increases dramatically, the evaluation function forces the robot to prioritize trajectories that deviate from the obstacles, at which point the local constraint layer becomes dominant.

[0108] Return to dominant state: Once the obstacle is bypassed, It decays rapidly, at which point the global traction term defined in step S4.3 is... It regains its dominant position. The stretched "virtual spring" releases potential energy, generating a strong guiding restoring force that pulls the robot to converge rapidly back to the globally optimal path with a smooth tangential angle.

[0109] Optimal instruction closed loop: After scoring is completed, the retrieval has the minimum overall cost. The trajectory is used to extract its corresponding linear velocity v and angular velocity. The optimal control quantity for the current control cycle is sent to the underlying driver to complete one closed-loop control cycle.

[0110] S5: Closed-loop control and iterative loop

[0111] The preferred speed command is sent to the speed control interface ( / cmd_vel) of the simulation robot to drive the vehicle to move, and steps S3 to S5 are executed repeatedly until the vehicle reaches the target point.

[0112] S6: Simulation Experiment Analysis

[0113] The experimental platform uses the ROSNoetic and Gazebo physics engines to build an unstructured complex warehouse scenario that includes static obstacles (shelves, walls) and dynamic disturbances (moving pedestrians). The experimental data is taken from the average of 50 complete navigation tasks to eliminate the randomness of a single experiment.

[0114] The test data for the search efficiency, security, and smoothness of the global path are shown in Table 1:

[0115] Table 1:

[0116] Performance evaluation indicators Traditional A* algorithm Improved method of the present invention Optimization range Average number of expansion nodes 12,450 6,847 Reduced by 45.0% Average planning time (ms) 145.2 82.4 Shortened by 43.2% Minimum distance (m) between path and obstacle 0.05 (Risk of contact with wall) 0.42 (Safe) 0.10MB Path cumulative corner cost 18.5 5.2 Smoothness significantly improved

[0117] The test data for the robot's dynamic obstacle avoidance and trajectory tracking capabilities at a running speed of 1.0 m / s are shown in Table 2:

[0118] Table 2

[0119] Performance evaluation indicators Traditional fixed look-ahead DWA Improved method of the present invention Optimization range Lateral tracking error RMSE (m) 0.35 0.12 Accuracy improved by 65.7% Convergence time after obstacle avoidance (s) 4.5 2.1 Response speed increased by 53.3% Maximum speed overshoot (m / s) 0.28 0.04 Smoother control Control command oscillation situation There is significant fluctuation. Smooth and oscillating Robustness enhancement

[0120] Results analysis:

[0121] (1) As shown in Table 1, this invention, through a dynamic adaptive weight strategy, effectively reduces the traversal of invalid nodes in the early stages of the search, resulting in a 45% reduction in the average number of expanded nodes and a 43.2% reduction in planning time. This significantly reduces computational resource consumption and improves search efficiency. Unlike the "wall-hugging" polyline path generated by traditional algorithms, this invention utilizes nonlinear feature distance fields and kinematic constraints to increase the minimum distance between the path and obstacles to 0.42m (corresponding to...). The value of the cumulative turning angle decreased significantly, generating a smooth and safe path that can be directly tracked, resulting in a qualitative change in both safety and smoothness.

[0122] (2) As shown in Table 2, the lateral tracking error RMSE of this invention is only 0.12m, which is 65.7% lower than that of the traditional method. This is mainly due to the speed-based dynamic look-ahead mechanism, which allows the system to adjust in real time according to the vehicle speed. This method effectively avoids the low-speed inner-loop cutting and high-speed overshoot phenomena caused by fixed look-ahead, achieving high-precision tracking. After encountering dynamic obstacles, the regression convergence time of this invention is reduced from 4.5s to 2.1s. This demonstrates that the global traction force index plays a crucial role in the local planner; the "virtual centripetal force" it provides enables the robot to quickly and smoothly converge back to the globally optimal path after obstacle avoidance, achieving deep collaboration between global planning and local control.

[0123] In summary, the simulation data quantitatively demonstrates that the collaborative path planning method proposed in this invention improves computational efficiency by approximately 43% and trajectory tracking accuracy by approximately 65% ​​compared to existing technologies. It also successfully solves the problems of "wall-hugging effect" and "control oscillation" in traditional methods, demonstrating significant technological advancements and engineering application value.

[0124] The above embodiments are only for illustrating the technical concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement it accordingly. They should not be construed as limiting the scope of protection of the present invention. All equivalent transformations or modifications made in accordance with the spirit and essence of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism, characterized in that, The method includes the following steps: S1: Construct a nonlinear safety feature field; acquire an environmental grid map, calculate the Euclidean distance from the free area to the nearest obstacle, and map this distance to a nonlinear safety potential field value to generate a feature distance field map; S2: In the feature distance difference map, the global path planner uses the multi-constraint improved A* algorithm to search for the globally optimal path from the starting point to the target point; S3: The local planner establishes a dynamic look-ahead traction mechanism based on speed adaptation, calculates and extracts the corresponding dynamic look-ahead points on the global path; S4: Perform trajectory cluster sampling and construct a global traction index. The multi-objective evaluation function scores the candidate trajectory, and the velocity command corresponding to the candidate trajectory with the best comprehensive score is selected as the control output. S5: Send the preferred speed command to the underlying motor driver to drive the vehicle to move, and repeat steps S3 to S5 until the vehicle reaches the target point.

2. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 1, characterized in that, The specific steps for S1 are as follows: The original two-dimensional occupancy grid map of the environment is obtained, and the map is binarized by setting an occupancy threshold to distinguish between obstacle areas and free passage areas. Using a linear time complexity Euclidean distance transformation algorithm, each free node in the map is traversed to calculate the precise Euclidean distance from the center of the node to the nearest obstacle boundary. Based on the Euclidean distance, a nonlinear safety potential field function in the form of Sigmoid is constructed to map the geometric distance to the potential field cost, so that the cost of the area close to the obstacle increases exponentially, while the cost of the area far from the obstacle smoothly decays to zero, generating a feature distance field map containing environmental safety gradient information.

3. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 2, characterized in that, The multi-constraint improved A* algorithm described in S2 includes a multi-constraint improved cost function, where the multi-constraints include: dynamic adaptive heuristic weights. Kinematic smoothness constraint and the cost of nonlinear safety potential field ; The total cost function of a node is defined as a linear weighted sum of four components: ； Where G(n) is the actual movement cost from the starting point to the current point; H(n) is the heuristic value of the Euclidean distance to the destination; and These are the safety potential field weights and the kinematic constraint weights, respectively.

4. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 3, characterized in that, The dynamic adaptive heuristic weights The weighting factor is positively correlated with the Euclidean distance from the current node to the target point in a non-linear manner. In the early stage of the search, the weight is relatively large, and the target region is quickly approached through greedy best-first search. In the later stage of the search, the weight decays to close to 1, and the algorithm reverts to the standard A* algorithm to ensure the geometric optimality of the path.

5. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 3, characterized in that, The kinematic smoothing constraint Specifically, the path's steering angle is obtained by calculating the angle between the parent node vector and the current extended vector. For nodes that exceed the vehicle's maximum permissible steering angle, an infinite cost is imposed for pruning. For steering angles within the permissible range, a smoothing penalty proportional to the square of the angle is applied.

6. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 3, characterized in that, The nonlinear security potential cost The potential field value is directly invoked to penalize nodes within the potential field's influence range, forcing the search tree to automatically avoid high-risk areas.

7. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 1, characterized in that, The speed-adaptive dynamic forward-looking traction mechanism described in S3 includes: The vehicle's current linear velocity is acquired in real time using chassis sensors. A piecewise linear mapping model between the look-ahead distance and linear velocity is established. Minimum and maximum look-ahead distance thresholds are set, and the optimal look-ahead distance at the current moment is calculated. In the global pathpoint sequence, the pathpoint closest to the vehicle's current position is located. An index search is performed along the path direction, and pathpoints whose cumulative path length equals the optimal look-ahead distance are extracted and marked as dynamic look-ahead points. .

8. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 7, characterized in that, The piecewise linear mapping model between the look-ahead distance and linear velocity includes: establishing the look-ahead distance... The linear mapping relationship with the vehicle's real-time linear velocity v(t): ； Parameter settings: ; ; , The model ensures that the look-ahead point is always located at the position that the vehicle is expected to reach in about 1 second, achieving constant look-ahead over time.

9. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 7, characterized in that, The specific steps for S4 are as follows: Based on the differential drive kinematic model, in the dynamic window ( , , ) performs gridded sampling within the range, generating N=40 For each set of speed command pairs, the future is deduced using Euler integrals. The machine pose sequence within a 2.0s time period is used to form a candidate trajectory cluster. For each candidate trajectory... Calculate the cost function : ； in, As a consequence of obstacle risk, The cost of speed smoothing, i.e., smoothness. For the sake of course consistency, As a cost to global traction force, the speed command corresponding to the candidate trajectory with the best comprehensive score is selected as the control output.

10. The collaborative path planning method based on multi-constraint improved A* and dynamic look-ahead mechanism according to claim 7, wherein the global traction cost Defined as the position of the candidate trajectory end and the dynamic look-ahead point Euclidean distance: ； in, For the pose of the candidate trajectory at the end of the prediction time domain, the global traction cost is used. The deviation between the local trajectory and the global intention is measured, and a centripetal force for regressing to the global path is introduced during the local planning process.

Images