Unmanned vehicle path planning method based on dynamic step length and speed perception potential field fusion

Abstract

This invention relates to the field of autonomous navigation technology for unmanned ground vehicles, providing a path planning method for unmanned vehicles based on the fusion of dynamic step size and velocity-sensing potential field. This method employs a hierarchical planning architecture: the upper layer, based on the TV-APF strategy, introduces environmental safety factors, task-oriented factors, and path inertia factors into the RRT* algorithm node expansion for ternary coupled dynamic step size calculation. The step size is used as a velocity proxy variable to dynamically adjust the repulsive gain and safety horizon of the artificial potential field, generating an initial path skeleton that conforms to dynamic constraints. The lower layer, based on an elastic band model of the potential field gradient, iteratively smooths the initial path through internal contraction forces and external repulsive forces, ensuring that the final trajectory satisfies geometric smoothness and dynamic safety boundaries. This method significantly improves the planning success rate, path smoothness, and vehicle driving safety in complex environments.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation technology for unmanned ground vehicles, and particularly relates to an unmanned vehicle path planning method based on the fusion of dynamic step size and velocity perception potential field. Background Technology

[0002] Mobile intelligent vehicle technology has significant application value in unstructured scenarios such as disaster relief, field surveying, and complex warehousing. Path planning, as a core component of autonomous navigation, is responsible for planning a collision-free feasible path from the starting point to the target point in complex environments. It must simultaneously consider geometric constraints, terrain features, and vehicle motion states, making it a typical multi-constraint, multi-objective optimization problem.

[0003] Existing methods mainly include sampling-based, graph search, potential field, and curve interpolation. A single method is insufficient to independently solve planning problems in complex, constrained spaces, often requiring the fusion of multiple methods to complement each other's strengths. However, existing fusion algorithms typically employ rigid parameter adjustment strategies, such as linearly adjusting the step size based solely on obstacle distance, ignoring task direction guidance and road inertia constraints. This leads to intelligent vehicles blindly searching in open areas or skidding during sharp turns on unstructured surfaces. Simultaneously, traditional artificial potential field parameters cannot adaptively adjust with vehicle speed, resulting in insufficient obstacle avoidance at high speeds and inability to pass through narrow passages at low speeds due to excessive repulsive forces. These problems make existing algorithms poorly adaptable to unstructured environments, struggling to balance planning success rate and motion smoothness. Summary of the Invention

[0004] The purpose of this invention is to provide an unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field, in order to solve the problems mentioned in the background art.

[0005] The present invention is implemented as follows: an unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field includes the following steps:

[0006] Step 1: Environmental perception and local coordinate system construction;

[0007] By acquiring point cloud data of the external environment and the real-time physical motion state of the vehicle through onboard environmental perception equipment, the intelligent vehicle is simplified into a point mass based on configuration space theory, and obstacles are expanded. The current position of the intelligent vehicle is then used as the reference point. Construct a local coordinate system with the origin and determine the target direction vector. Growth direction vector and the direction vector of motion of the intelligent vehicle before reaching the current point ;

[0008] Step 2: Calculation of dynamic step size for ternary coupled multi-source systems;

[0009] Environmental safety factors are calculated based on environmental perception. ,based on and Calculate the angle between the two sides to determine the task orientation factor. ,based on and Calculation of the change in included angle and path inertia factor The final expansion step size is obtained through the product coupling of the above three factors. ;

[0010] Step 3: Velocity sensing and adaptive potential field update;

[0011] The final expansion step size calculated so far. As a proxy variable for the instantaneous speed of intelligent vehicles, dynamic calculation of speed perception provides a dynamic safety perspective. And based on the measured distance to the obstacle and The ratio is used to adjust the intensity coefficient of the repulsive field in real time nonlinearly to obtain the adaptive repulsive gain. ;

[0012] Step 4: Combined Guidance and RRT* Tree Growth;

[0013] Based on the improved artificial potential field theory, gravity is calculated. Repulsive force Resultant force Along the direction of the resultant force in steps Generate new nodes It performs collision detection, parent node reselection, and rewiring operations using the RRT* algorithm to generate an asymptotically optimal initial path skeleton.

[0014] Step 5: Construct a force model for the elastic band;

[0015] The generated initial path is modeled as a force-balanced elastic band placed in a complex physical field, with each path point subjected to internal contraction force and external repulsion force.

[0016] Step 6: Calculate the virtual forces;

[0017] Calculate internal contraction force based on the geometric relationship between adjacent nodes And directly call the adaptive repulsive force gain calculated in step 3. Calculate external repulsive force ;

[0018] Step 7: Iterative update of path points;

[0019] Iteratively update the intermediate nodes of the path until the convergence condition is met, and output the smoothed final trajectory.

[0020] Step 8: Low-level trajectory tracking and wire-controlled chassis execution;

[0021] The path point sequence, which has undergone physical-level smoothing and safety optimization, is sent to the underlying motion controller as a reference input to generate control commands and drive the drive-by-wire chassis to perform physical obstacle avoidance and path tracking actions.

[0022] A further technical solution, wherein step 1 includes the following specific steps:

[0023] Step 1.1: Environmental perception;

[0024] First, point cloud data of the external environment is acquired through onboard environmental perception equipment or local high-precision maps. This data is then combined with wheel speed sensors or inertial measurement units to obtain the real-time physical motion state of the intelligent vehicle, thereby determining the vehicle's current location. The length of the straight line connecting the object to the nearest obstacle surface in the environment, i.e., the original Euclidean distance. ;

[0025] Based on configuration space theory, intelligent vehicles with physical dimensions are simplified into point mass models, and environmental obstacles are expanded; the definition and calculation of the expansion radius are as follows:

[0026] Define the vehicle body length as The width of the vehicle body is The minimum circumcircle radius of the vehicle's geometric center is used as the physical equivalent radius. and add a safety margin To determine the expansion radius The calculation formula is as follows:

[0027]

[0028] Step 1.2: Constructing the local coordinate system;

[0029] In each iteration of the random tree expansion, define The origin of the local reference coordinate system; with the global target... For the target point, for The previous parent node, from This leads to three key vectors: pointing to the global objective. Target direction vector Pointing to the current random sampling point growth direction vector and the motion direction vector of the intelligent vehicle before reaching the current point. ; for and The angle between them for and The change in the angle between them is used to calculate the angle factor and the inertia factor.

[0030] In a further technical solution, step 2 includes the following specific steps:

[0031] Step 2.1: Calculate the environmental safety factor ;

[0032] Based on environmental perception Measured distance to the nearest obstacle The calculation formula is:

[0033]

[0034] Or will In a grid map, it is directly represented as the shortest straight-line distance from the node to the boundary of the expanded obstacle;

[0035] when When, it indicates that the node is in a safe zone; when When this occurs, it indicates that the node has intruded into the safety buffer zone, which is considered a collision.

[0036] Environmental safety factors A piecewise linear function is used for a refined description, dividing the environment into a safe zone, a transition zone, and a critical zone:

[0037]

[0038] In the formula, This represents the maximum search step size allowed for intelligent vehicles in open areas, with a value range of [value range missing]. ; To find the minimum search step size in the confined space, take... 0.1-0.2 times; To preset a global safe distance threshold; This is the critical obstacle avoidance distance threshold. ,in To find the maximum value function, i.e., select and The larger of the two values ​​is used as the critical obstacle avoidance distance. When the distance to the obstacle is lower than this value, the system will automatically switch to the minimum step size for crawling search.

[0039] Step 2.2: Calculate the task orientation factor ;

[0040] Define the growth direction vector With the target direction vector The step gain is dynamically adjusted by calculating the cosine of the angle between the two:

[0041]

[0042] In the formula, This is a task-oriented weighting coefficient, with a value range of [value range missing]. ;

[0043] Step 2.3: Calculate the path inertia factor ;

[0044] definition Calculate its relationship with the growth direction vector. Change in the included angle The unit is radians;

[0045]

[0046] In the formula, This is the starting node for path planning; The inertia penalty coefficient has a value range of [1.5, 3].

[0047] Step 2.4: Final dynamic step size synthesis;

[0048] The final expansion step size is obtained through the product coupling of the above three factors. :

[0049] .

[0050] In a further technical solution, step 3 includes the following specific steps:

[0051] Step 3.1: Calculate the speed-aware dynamic safety horizon ;

[0052] The final expansion step size calculated so far. The dynamic safety horizon is defined as follows, treating the instantaneous speed of the intelligent vehicle as a proxy variable:

[0053]

[0054] In the formula, To determine the minimum static safety buffer distance for intelligent vehicles, take... ; The velocity gain coefficient has a value range of [value range missing]. ;

[0055] Step 3.2: Calculate the adaptive repulsive force gain ;

[0056] Based on current measurements With the calculated dynamic safety horizon The ratio of the repulsive field intensity coefficient is adjusted nonlinearly in real time.

[0057]

[0058] In the formula: For adaptive repulsive force gain; For the standard repulsive gain under normal conditions, take Used for early warning at long distances; To minimize the repulsive force gain in a confined space, take This is used to prevent intelligent vehicles from being bounced away at extremely close range; To adjust the index, take This is used to control the rate of gain decay.

[0059] A further technical solution is provided in step 3.1. A fixed value of 1.2 is used to ensure at least 20% global security redundancy in unstructured environments.

[0060] In a further technical solution, step 4 includes the following specific steps:

[0061] Step 4.1: Improve the calculation of attraction and repulsion;

[0062]

[0063]

[0064] In the formula: This is the gravitational gain coefficient, and a recommended range of values. In this example, we take ; Current location of the intelligent vehicle To global goal Euclidean distance (i.e. ); To introduce the target distance adjustment term, take ; and These are the unit direction vectors pointing towards the target and away from the obstacle, respectively;

[0065] Step 4.2: New node generation;

[0066]

[0067] New node The calculation formula is:

[0068]

[0069] After generating a new node, the collision detection process of the RRT* algorithm is executed; if the node is safe, the parent node reselection and rewiring operation are further executed.

[0070] A further technical solution involves two virtual forces acting on each path point in step 6:

[0071] 1) Internal contraction force :

[0072] This force is used to simulate the internal tension of an elastic band, calculated based on the geometric relationship between adjacent nodes, and the current node... Pull towards its front and rear nodes and The midpoint of the line connecting them;

[0073]

[0074] In the formula, To determine the smoothing weighting coefficients, take... ;

[0075] 2) External repulsive force :

[0076]

[0077] In the formula, For the safety weighting coefficient, take... And satisfy And it is recommended .

[0078] In a further technical solution, step 7 includes the following specific steps:

[0079] Perform on all intermediate nodes in the path Each iteration of the update formula is as follows:

[0080]

[0081] in, For the number of iterations, and The first on the path The node at the th Second and third Spatial coordinates during the next iteration of optimization;

[0082] Convergence judgment and iteration termination:

[0083] To determine the termination point of the elastic band iterative optimization, the following two convergence conditions are set. If either condition is met, the iteration stops and the current path is output as the final smooth trajectory:

[0084] Condition 1, Convergence criterion based on location change: After each iteration update, calculate all movable path points. Find the Euclidean distance between the displacement vectors from the current iteration to the previous iteration, and identify the maximum value. :

[0085]

[0086] In the formula, For the first on the path The node at the th Spatial coordinates during the next iteration of optimization;

[0087] Set a convergence threshold ,when When the iteration converges;

[0088] Condition 2, Maximum Iteration Limit: Set a maximum number of iterations. As an auxiliary termination condition, when the number of iterations... achieve When condition 1 is met, the iteration is forcibly stopped and the current path is output.

[0089] The unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field provided in this invention has the following beneficial effects:

[0090] (1) High environmental adaptability: By constructing a ternary coupled multi-source dynamic step size, the node expansion can actively adapt to environmental geometric features, task requirements and vehicle dynamic constraints, solving the problems of passive strategy and rigid parameters in traditional algorithms. At the same time, a closed-loop mapping between step size and potential field parameters is established based on speed perception, so that the repulsive field changes dynamically with vehicle speed, effectively solving the problem of local minimum oscillation (deadlock) in narrow channels.

[0091] (2) Strong dynamic compatibility: The path inertia factor is directly introduced into the three-element coupled multi-source dynamic step size, which restricts the curvature change of adjacent trajectory segments from the bottom layer of the algorithm, ensuring that the output path meets the non-integrity constraints of the vehicle and the road surface adhesion limit, and fundamentally avoids the risk of control instability and sideslip caused by path break line jitter.

[0092] (3) High trajectory quality: The lower layer adopts elastic band optimization based on potential field gradient, which strictly limits the smoothing process within the dynamic safety horizon calculated by velocity, realizing the unity of path geometric smoothness and physical safety, and can directly connect to the underlying control system without secondary collision detection. Attached Figure Description

[0093] Figure 1 A flowchart illustrating the unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field provided in this embodiment of the invention.

[0094] Figure 2This is a schematic diagram of the geometric vector relationship of the ternary coupling factor;

[0095] Figure 3 This diagram illustrates the principle of dynamic safety field of view and repulsive field changes for speed perception (where a represents high speed / open state, and b represents low speed / narrow channel state).

[0096] Figure 4 This is a schematic diagram illustrating node growth guided by the resultant force based on an adaptive potential field.

[0097] Figure 5 A schematic diagram of optimized force analysis for the elastic band;

[0098] Figure 6 This is a comparison chart of the traditional algorithm and the proposed method in path planning scenarios with narrow passages. Detailed Implementation

[0099] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0100] The specific implementation of the present invention will be described in detail below with reference to specific embodiments.

[0101] like Figure 1 As shown, an embodiment of the present invention provides an unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field. It employs a hierarchical planning architecture of "decision-optimization" to decouple complex unstructured environmental planning problems.

[0102] Upper-level decision planning layer: Based on the Tri-Coupled & Velocity-Aware APF strategy (TV-APF strategy for short), it integrates environmental geometric information, task-oriented information and kinematic inertial information (motion vector of the previous moment and the angle deviation of the extension direction), and is responsible for generating a progressively optimal initial path skeleton that takes into account dynamic constraints and passability in a complex environment containing scattered obstacles and narrow passages.

[0103] The lower trajectory optimization layer is an elastic band based on the potential field gradient. It uses the adaptive physical field calculated by the upper layer to post-process the initial path. It is responsible for geometric smoothing and curvature optimization of the path while maintaining the physical safety boundary. Finally, it realizes the underlying trajectory tracking and the execution of the drive-by-wire chassis based on the smooth trajectory.

[0104] The specific implementation steps are as follows:

[0105] Step 1: Environmental perception and local coordinate system construction;

[0106] Step 1.1: Environmental perception;

[0107] First, external environmental point cloud data is acquired through onboard environmental perception devices (such as lidar and millimeter-wave radar) or local high-precision maps, and combined with wheel speed sensors or inertial measurement units (IMUs) to obtain the real-time physical motion state of the intelligent vehicle (such as instantaneous chassis speed and heading angle), and the current position point (geometric center) of the intelligent vehicle is obtained in real time. The length of the straight line connecting the object to the nearest obstacle surface in the environment, i.e., the original Euclidean distance. .

[0108] To reduce the computational complexity of trajectory planning and ensure safety under high-dimensional constraints, based on configuration space (C-Space) theory, the intelligent vehicle with physical dimensions is simplified to a point mass model, and environmental obstacles are inflated. The definition and calculation of the inflated radius are as follows:

[0109] Considering that intelligent vehicles typically have a rectangular geometric profile, the vehicle body length is defined as... The width of the vehicle body is (Unit: meters). The minimum circumcircle radius of the vehicle's geometric center is used as the physically equivalent radius. and add a safety margin (Used to compensate for positioning errors, recommended value range is) To determine the expansion radius The calculation formula is as follows:

[0110]

[0111] Step 1.2: Constructing the local coordinate system;

[0112] In each iteration of the random tree expansion, define Let this be the origin of the local reference coordinate system. The extended vector geometric relationships are established using this origin as follows: Figure 2 As shown.

[0113] In the picture, with With the central point as the focus, and the overall goal as the objective For the target point, for The preceding parent node (i.e., the vehicle's previous historical position), from This leads to three key vectors: pointing to the global objective. Target direction vector Pointing to the current random sampling point growth direction vector and the motion direction vector of the intelligent vehicle before reaching the current point. . for and The angle between them for and The change in the angle between them is used to calculate the angle factor and the inertia factor.

[0114] Step 2: Calculation of dynamic step size for ternary coupled multi-source systems;

[0115] For the current random sampling point First, the three key heuristic factors that determine the current motion capability of the intelligent vehicle are calculated, and then the final expansion step size is solved through a product coupling method. :

[0116] Step 2.1: Calculate the environmental safety factor ;

[0117] This factor is used to ensure the geometric obstacle avoidance safety of intelligent vehicles in unstructured environments. Based on environmental perception, Effective Euclidean distance to the nearest obstacle The calculation formula is:

[0118]

[0119] Or will In a grid map, it is directly represented as the shortest straight-line distance from the node to the boundary of the expanded obstacle.

[0120] when When, it indicates that the node is in a safe zone; when When this occurs, it indicates that a node has intruded into the safety buffer, which is considered a collision.

[0121] Environmental safety factors A piecewise linear function is used for a refined description, dividing the environment into a safe zone, a transition zone, and a critical zone:

[0122]

[0123] In the formula, This represents the maximum search step size allowed for the intelligent vehicle in open areas. This value determines the convergence speed of the algorithm, and its range is determined based on the intelligent vehicle's maximum cruising speed and the planning and control cycle. ; This is the minimum search step size in a confined space. This value determines the precision of the algorithm near obstacles. 0.1-0.2 times higher, to ensure that intelligent vehicles can perform collision detection at high resolution and avoid penetrating thin-walled obstacles; To preset a global safe distance threshold, approximately three vehicle lengths, that is... When the distance to the obstacle exceeds this value, it is considered that there is no risk of collision and full speed can be extended. This is the critical obstacle avoidance distance threshold. ,in To find the maximum value function, i.e., select and The larger of the two values ​​is used as the critical obstacle avoidance distance. When the distance to the obstacle is lower than this value, the collision risk is considered extremely high, and the system automatically switches to the minimum step size for crawling search.

[0124] Step 2.2: Calculate the task orientation factor ;

[0125] This factor is used to evaluate the contribution of the current expansion direction to the final mission objective, introducing the concept of target gravity to reduce ineffective random expansion. For example... Figure 2 As shown, the growth direction vector is defined. With the target direction vector The step gain is dynamically adjusted by calculating the cosine of the angle between the two.

[0126]

[0127] In the formula, For task-oriented weighting coefficients, the recommended value range is [range missing]. The reason for choosing this value is: when When the value is less than 0.5, the gravitational pull of the target is too weak, and the algorithm degenerates into pure random exploration, resulting in extremely slow convergence; when... When the value is greater than 1, the node expansion is too greedy and easily gets trapped in local minima when encountering complex obstacles such as U-shaped traps. In this example, we take... This is to balance the randomness of exploration with the guidance of convergence toward the goal.

[0128] The mechanism of this factor is as follows: when the angle between the expansion direction and the target direction is less than 90 degrees (i.e., the dot product is greater than 0), it indicates that the expansion direction is conducive to approaching the target; the smaller the angle, the better. The larger the value, the larger the expansion step size, which can accelerate convergence in the correct direction; conversely, when the expansion direction deviates from the target, only the basic step size is maintained and no reward is given, thereby suppressing backward expansion while ensuring the randomness of exploration.

[0129] Step 2.3: Calculate the path inertia factor ;

[0130] This factor is specifically designed to accommodate the complex dynamic constraints of unstructured environments, simulating the anti-skid and anti-rollover requirements of road surfaces (such as mud, gravel, and grass). Definition Calculate its relationship with the growth direction vector. The change in the included angle (i.e., an approximate representation of the path curvature). The unit is radians (rad).

[0131]

[0132] In the formula, This is the starting node for path planning. The inertia penalty coefficient is recommended to be in the range of [1.5, 3] (this example is for general off-road terrain). The basic threshold of this coefficient is determined by the minimum turning radius, which is based on the vehicle's wheelbase and the maximum turning angle of the front wheels, and is dynamically mapped inversely to the peak road adhesion coefficient. Its value selection logic is as follows: if... Then the sideslip suppression is insufficient, if The conservative algorithm leads to frequent vehicle stuttering. By applying a penalty to large turning angles using this coefficient, excessive curvature can be eliminated directly at the planning source, and lateral acceleration is strictly constrained within the physical friction circle, ensuring that the output path does not exceed the mechanical limits of the underlying steering and suspension, effectively preventing instability and sideslip.

[0133] On unstructured pavements with low adhesion coefficients, sharp turns imply extremely high lateral forces, easily triggering skidding or instability. This formula utilizes the decaying property of an exponential function, so that when the path experiences a sharp turn (the absolute value of the change in angle...)... When (increase), Rapid decay forces a significant reduction in the expansion step size. This mathematically simulates the safe driving behavior of "decelerating in a curve," ensuring that the generated path conforms to vehicle dynamics constraints.

[0134] Step 2.4: Final dynamic step size synthesis;

[0135] The final expansion step size is obtained through the product coupling of the above three factors. :

[0136]

[0137] The synthetic step size is not merely a function of geometric distance, but a comprehensive decision result that integrates environment, task, and dynamics.

[0138] Step 3: Velocity sensing and adaptive potential field update;

[0139] To address the drawback of traditional rigid potential fields with fixed parameters, which prevent intelligent vehicles from entering narrow passages due to excessive repulsive force (i.e., local minimum oscillation), this invention creatively establishes a dynamic mapping relationship between step size (as a proxy variable for velocity) and artificial potential field parameters. The physical principle is as follows: Figure 3 As shown.

[0140] In the picture, For dynamic safety vision, It is a repulsive force that moves away from obstacles. Figure 3The adaptive mechanism is illustrated using two contrasting state diagrams. Figure 3 "a" indicates a high-speed / open environment: the arrow corresponding to the intelligent vehicle is longer, surrounded by a dashed circle (representing the dynamic safety field of vision). A larger radius indicates a wider range of repulsive force action, allowing for earlier obstacle avoidance. Figure 3 b represents the low-speed / narrow-channel state: the step arrow corresponding to the intelligent vehicle is shorter, and the dashed circle shrinks to a very small size, only slightly larger than the intelligent vehicle itself, allowing the intelligent vehicle to get close to the obstacle. This diagram illustrates the physical principle that "the faster the speed, the farther the obstacle avoidance warning distance; the slower the speed, the more likely it is to drive close to the edge."

[0141] Step 3.1: Calculate the speed-aware dynamic safety horizon ;

[0142] Analogous to the "braking distance model" in vehicle dynamics, this invention posits that the faster a smart vehicle travels, the longer the braking distance, and therefore the larger the obstacle avoidance warning range (i.e., the range of potential field repulsion). This invention uses the currently calculated final expansion step size... The dynamic safety horizon is defined as follows, treating the instantaneous speed of the intelligent vehicle as a proxy variable:

[0143]

[0144] In the formula, To determine the minimum static safety buffer distance for intelligent vehicles, take... This provides a basic physical margin for collision avoidance when stationary. This is the speed gain coefficient, which is essentially an engineering mapping of the braking slip ratio. It is related to the road surface friction coefficient and is used to characterize the impact of braking uncertainties on the safe distance from unstructured road surfaces; the slipperier the road surface, the greater the impact of braking uncertainties on the safe distance. The larger the value, the better. In this embodiment, based on the preset unstructured environment characteristics, the recommended value range is [value range to be filled in]. The reason for this value is: if If the field of view is too small, the dynamic field of view cannot cover the emergency braking distance at high speeds, increasing the risk of collision; if... An excessively high value can cause vehicles to overreact to obstacles at great distances in open areas, affecting traffic efficiency. A fixed value is used in this example. This ensures at least 20% global security redundancy in unstructured environments.

[0145] Step 3.2: Calculate the adaptive repulsive force gain ;

[0146] Based on current measurements With the calculated dynamic safety horizon The ratio of the repulsive field intensity coefficient is adjusted nonlinearly in real time.

[0147]

[0148] In the formula: For adaptive repulsive force gain; For the standard repulsive gain under normal conditions, take Used for early warning at long distances; To minimize the repulsive force gain in a confined space, take This is designed to prevent intelligent vehicles from being bounced off at extremely close range; (Recommended range: Pick , Pick of ). To adjust the index, take This is used to control the rate of gain decay.

[0149] Working mechanism explanation: such as Figure 3 As shown in b, when an intelligent vehicle approaches the entrance of a narrow passage, the proximity of the obstacle and the need to adjust the approach angle (due to the inertial factor) lead to... Decrease. Reduce immediate trigger The contraction, which in turn leads to The descent. This chain reaction significantly reduces the repulsive force on the intelligent vehicle, allowing it to "smoothly" slide into the narrow channel by gravity, effectively solving the local minimum oscillation (i.e., deadlock) problem of traditional algorithms.

[0150] Step 4: Combined Guidance and RRT* Tree Growth;

[0151] Based on the improved artificial potential field theory, the gravitational force pointing towards the target is calculated. Repulsive force away from the obstacle The combined force And use the direction of this resultant force to guide the generation of new nodes, such as Figure 4 As shown.

[0152] Figure 4 This demonstrates the two components of force acting on the intelligent vehicle at its current position: the gravitational force pointing towards the target. and the repulsive force away from the obstacle The resultant force is synthesized according to the parallelogram law. New node It is along Direction, from Extended It is generated based on the length.

[0153] Step 4.1: Improve the calculation of attraction and repulsion;

[0154]

[0155]

[0156] In the formula: This is the gravitational gain coefficient, and a recommended range of values. In this example, we take ; Current location of the intelligent vehicle To global goal Euclidean distance (i.e. ); To introduce the target distance adjustment term, take The purpose of this is to force the repulsive force to approach zero when the intelligent vehicle approaches the target point, thereby solving the "target unreachable" problem of the traditional potential field method. and These are the unit direction vectors pointing towards the target and away from the obstacle, respectively. The repulsive force calculated here is directly obtained from the dynamic calculation in step 3. and This achieves the coupling of the potential field with the motion state of the intelligent vehicle.

[0157] Step 4.2: New node generation;

[0158]

[0159] New node The calculation formula is:

[0160]

[0161] After generating a new node, the standard collision detection process of the RRT* algorithm is executed. If the node is safe, the parent node is chosen and rewired. By optimizing local connectivity, the asymptotic optimality of the tree structure is maintained, ensuring that the final generated path approaches the theoretical optimal length.

[0162] Step 5: Construct a force model for the elastic band;

[0163] While the initial path generated by the decision layer is topologically feasible, it often suffers from jitter and abrupt curvature changes due to its composition of discrete line segments. To address these issues, this invention proposes a post-processing method based on potential field gradients for elastic bands. This method no longer treats the path as a simple geometric curve but models it as a force-balanced elastic band placed within a complex physical field. This elastic band undergoes self-organized evolution under the combined influence of "internal tension" and "external potential force" until it converges to the equilibrium state with the lowest energy, thus achieving a balance between geometric smoothness and physical safety. The force model is as follows: Figure 5 As shown, where, The current stress analysis is the first Intermediate nodes of the path; and The paths and Adjacent predecessor and successor nodes; For the first on the path The node at the th Spatial coordinates during the next iteration of optimization; It is the internal contraction force; It is an external repulsive force.

[0164] Step 6: Calculate the virtual forces (internal forces + external forces);

[0165] This method optimizes the path for smoothing and safety by iteratively applying virtual forces.

[0166] Specifically, this manifests as two virtual forces acting on each path point:

[0167] 1) Internal contraction force :

[0168] This force simulates the internal tension of an elastic band, calculated based on the geometric relationship between adjacent nodes, and attempts to reduce the tension of the current node. Pull towards its front and rear nodes and The midpoint of the line connecting the points reduces the curvature and length of the path (this force is mathematically equivalent to a penalty term on the curvature and length of the path).

[0169]

[0170] In the formula, To determine the smoothing weighting coefficients, take... This determines the intensity of path contraction.

[0171] 2) External repulsive force :

[0172] To ensure that the smoothed path still meets the safety requirements of unstructured road surfaces, the force directly calls the adaptive potential field model calculated by the decision layer.

[0173]

[0174] In the formula, For the safety weighting coefficient, take... It must meet the following requirements. And it is recommended The basis for this design is that, in the force model, the internal contraction force must be slightly greater than the external repulsion force (i.e., ...). Only by ensuring that the path effectively "straightens" and "shortens" while avoiding obstacles can we prevent the path from being pushed away indefinitely by tiny obstacles. Simultaneously, to guarantee the numerical stability of the iterative calculations and prevent path points from diverging and oscillating during iterations, the following conditions must be met. The convergence condition.

[0175] Here, we directly use the adaptive repulsive force gain calculated in step 3.2. This means that if a waypoint is too close to an obstacle, or in a high-speed section (where the safety field of view is large), it will be pushed back to a safe area by a strong repulsive force; while in narrow passages, due to... The smaller the size, the more likely the waypoint will remain close to the obstacle without being pushed too far away.

[0176] Step 7: Iterative update of path points;

[0177] Perform operations on all intermediate nodes in the path (with the start and end points fixed). Each iteration of the update formula is as follows:

[0178]

[0179] in, For the number of iterations, and The first on the path The node at the th Second and third Spatial location coordinates during the next iteration of optimization.

[0180] Convergence judgment and iteration termination:

[0181] To determine the termination point of the elastic band iterative optimization, the following two convergence conditions are set. If either condition is met, the iteration stops and the current path is output as the final smooth trajectory:

[0182] Condition 1, Convergence criterion based on location change: After each iteration update, calculate all movable path points. (Except for fixed start and end points) Find the Euclidean distance between the displacement vectors from the current iteration to the previous iteration, and find the maximum value. :

[0183]

[0184] In the formula, For the first on the path The node at the th Spatial location coordinates during the next iteration of optimization.

[0185] Set a convergence threshold .when At this point, the path point positions are considered to be basically stable, and the elastic band has reached a state of force equilibrium, at which point the iteration converges. Convergence threshold. The setting is related to system accuracy and vehicle size; a value of vehicle length is recommended. One-thousandth to one-hundredth, that is In this embodiment, take .

[0186] Condition 2, Maximum Iteration Limit: To avoid iterative oscillations or slow convergence in extremely complex fields, a maximum number of iterations is set. As an auxiliary termination condition. When the number of iterations... achieve When condition 1 is met, the iteration is forcibly stopped and the current path is output. The value of needs to ensure sufficient optimization time, and is typically set to 200 to 1000. In this embodiment, is taken as... .

[0187] Through the aforementioned dual-judgment mechanism, this method can ensure both the real-time performance and robustness of the algorithm while maintaining optimization accuracy. After iteration termination, the current path point sequence... This refers to the final feasible trajectory after physical-level smoothing and safety optimization.

[0188] After several iterations and convergence, a final smooth path is obtained. This optimization method guarantees that the final trajectory is geometrically smooth (ensuring this through contraction force) and physically always remains within a dynamically safe horizon. Within the defined safe passage (guaranteed by repulsive forces), a balance between geometric smoothness and physical safety is achieved.

[0189] Step 8: Low-level trajectory tracking and wire-controlled chassis execution;

[0190] Finally, the path point sequence after physical-level smoothing and security optimization... This serves as a reference input, which is then sent to the underlying motion controller of the unmanned ground vehicle. The motion controller can employ classic trajectory tracking algorithms such as Model Predictive Control (MPC) or PurePursuit to calculate the desired yaw rate and longitudinal acceleration based on the vehicle's current pose. Subsequently, combined with the chassis's kinematic model and dynamic boundaries, this is translated into specific control commands for the drive-by-wire chassis. For example, it outputs continuous steering wheel / steering column angle commands; simultaneously, for the distributed drive architecture, it utilizes a torque vectoring strategy to calculate the optimal torque and speed control commands for each of the four AMK hub motors. This drives the mechanical actuators to precisely complete the actual physical obstacle avoidance and path tracking actions, ensuring the vehicle's driving stability on complex, unstructured road surfaces.

[0191] like Figure 6 As shown, to verify the feasibility and effectiveness of the present invention, a comparative simulation of path planning in a two-dimensional unstructured constrained environment was conducted. The red broken line with data nodes represents the path generated by the traditional algorithm, while the blue curve with data nodes represents the path generated by the present invention. From the trajectory and node density in the figure, it can be seen that when facing extremely narrow channels, the traditional algorithm, due to its rigid static safety horizon, is blocked at the entrance by a "high potential energy barrier" formed by the combined repulsive forces of obstacles on both sides, resulting in severe back-and-forth oscillations and getting stuck in local minima (i.e., deadlock). In contrast, the planning nodes of the present invention are spaced further apart in open areas (rapid search), and the node spacing adaptively becomes denser as it approaches the channel entrance (dynamic shrinkage of step size), thereby reducing the dynamic safety horizon. This allows the final smooth trajectory to be immune to far-end repulsive interference, smoothly and safely traversing physically narrow corridors.

[0192] As a preferred embodiment of the present invention, in calculation The ternary product coupling method was adopted at that time. As an alternative, it can be replaced with a weighted summation model (i.e., ,in , and Alternatively, a fuzzy logic controller can be used, with environmental safety factors, task-oriented factors, and path inertia factors as fuzzy input rules, to output corresponding step size adjustment instructions, thus achieving dynamic step size adjustment in unstructured environments.

[0193] As a preferred embodiment of the present invention, the lower-level trajectory optimization can also be solved using a quadratic programming (QP) optimizer incorporating a collision penalty function, or using a gradient-based B-spline curve optimization method. This is as long as the upper-level trajectory, generated based on velocity (step size), is invoked during the smoothing process. All of these, as safety corridor constraints, are equivalent alternatives to the present invention.

[0194] In a preferred embodiment of the present invention, the obstacle distance is calculated using Euclidean distance in step 1. In practical applications based on grid maps, to further improve the real-time performance of the calculation, it can be replaced by dilated calculations using Chebyshev distance or Manhattan distance.

[0195] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. An unmanned vehicle path planning method based on dynamic step length and speed perception potential field fusion, characterized in that, Includes the following steps: Step 1: Environmental perception and local coordinate system construction; By acquiring point cloud data of the external environment and the real-time physical motion state of the vehicle through onboard environmental perception equipment, the intelligent vehicle is simplified into a point mass based on configuration space theory, and obstacles are expanded. The current position of the intelligent vehicle is then used as the reference point. Construct a local coordinate system with the origin and determine the target direction vector. Growth direction vector and the direction vector of motion of the intelligent vehicle before reaching the current point ; Step 2: Calculation of dynamic step size for ternary coupled multi-source systems; Environmental safety factors are calculated based on environmental perception. ,based on and Calculate the angle between the two sides to determine the task orientation factor. ,based on and Calculation of the change in included angle and path inertia factor The final expansion step size is obtained through the product coupling of the above three factors. ; Step 3: Velocity sensing and adaptive potential field update; The final expansion step size calculated so far. As a proxy variable for the instantaneous speed of intelligent vehicles, dynamic calculation of speed perception provides a dynamic safety perspective. And based on the measured distance to the obstacle and The ratio is used to adjust the intensity coefficient of the repulsive field in real time nonlinearly to obtain the adaptive repulsive gain. ; Step 4: Combined Guidance and RRT* Tree Growth; Based on the improved artificial potential field theory, gravity is calculated. Repulsive force Resultant force Along the direction of the resultant force in steps Generate new nodes It performs collision detection, parent node reselection, and rewiring operations using the RRT* algorithm to generate an asymptotically optimal initial path skeleton. Step 5: Construct a force model for the elastic band; The generated initial path is modeled as a force-balanced elastic band placed in a complex physical field, with each path point subjected to internal contraction force and external repulsion force. Step 6: Calculate the virtual forces; Calculate internal contraction force based on the geometric relationship between adjacent nodes And directly call the adaptive repulsive force gain calculated in step 3. Calculate external repulsive force ; Step 7: Iterative update of path points; Iteratively update the intermediate nodes of the path until the convergence condition is met, and output the smoothed final trajectory. Step 8: Low-level trajectory tracking and wire-controlled chassis execution; The path point sequence, which has undergone physical-level smoothing and safety optimization, is sent to the underlying motion controller as a reference input to generate control commands and drive the drive-by-wire chassis to perform physical obstacle avoidance and path tracking actions.

2. The unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field as described in claim 1, characterized in that, Step 1 includes the following specific steps: Step 1.1: Environmental perception; First, point cloud data of the external environment is acquired through onboard environmental perception equipment or local high-precision maps. This data is then combined with wheel speed sensors or inertial measurement units to obtain the real-time physical motion state of the intelligent vehicle, thereby determining the vehicle's current location. The length of the straight line connecting the object to the nearest obstacle surface in the environment, i.e., the original Euclidean distance. ; Based on configuration space theory, intelligent vehicles with physical dimensions are simplified into point mass models, and environmental obstacles are expanded; the definition and calculation of the expansion radius are as follows: Define the vehicle body length as The width of the vehicle body is The minimum circumcircle radius of the vehicle's geometric center is used as the physical equivalent radius. and add a safety margin To determine the expansion radius The calculation formula is as follows: Step 1.2: Constructing the local coordinate system; In each iteration of the random tree expansion, define The origin of the local reference coordinate system; with the global target... For the target point, for The previous parent node, from This leads to three key vectors: pointing to the global objective. Target direction vector Pointing to the current random sampling point growth direction vector and the motion direction vector of the intelligent vehicle before reaching the current point. ; for and The angle between them for and The change in the angle between them.

3. The unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field as described in claim 2, characterized in that, Step 2 includes the following specific steps: Step 2.1: Calculate the environmental safety factor ; Based on environmental perception Measured distance to the nearest obstacle The calculation formula is: Or will In a grid map, it is directly represented as the shortest straight-line distance from the node to the boundary of the expanded obstacle; when When, it indicates that the node is in a safe zone; when When this occurs, it indicates that the node has intruded into the safety buffer zone, which is considered a collision. Environmental safety factors A piecewise linear function is used for a refined description, dividing the environment into a safe zone, a transition zone, and a critical zone: In the formula, The maximum search step size allowed for intelligent vehicles in open areas; To find the minimum search step size in the confined space, take... 0.1-0.2 times; To preset a global safe distance threshold; This is the critical obstacle avoidance distance threshold. ,in To find the maximum value function, i.e., select and The larger of the two values ​​is used as the critical obstacle avoidance distance. When the distance to the obstacle is lower than this value, the system will automatically switch to the minimum step size for crawling search. Step 2.2: Calculate the task orientation factor ; Define the growth direction vector With the target direction vector The step gain is dynamically adjusted by calculating the cosine of the angle between the two: In the formula, Task-oriented weighting coefficients; Step 2.3: Calculate the path inertia factor ; definition Calculate its relationship with the growth direction vector. Change in the included angle The unit is radians; In the formula, This is the starting node for path planning; This is the inertia penalty coefficient; Step 2.4: Final dynamic step size synthesis; The final expansion step size is obtained through the product coupling of the above three factors. : 。 4. The unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field as described in claim 3, is characterized in that, Step 3 includes the following specific steps: Step 3.1: Calculate the speed-aware dynamic safety horizon ; The final expansion step size calculated so far. The dynamic safety horizon is defined as follows, treating the instantaneous speed of the intelligent vehicle as a proxy variable: In the formula, The minimum static safety buffer distance for intelligent vehicles; This is the velocity gain coefficient; Step 3.2: Calculate the adaptive repulsive force gain ; Based on current measurements With the calculated dynamic safety horizon The ratio of the two forces is used to adjust the intensity coefficient of the repulsive field in real time nonlinearly. In the formula: For adaptive repulsive force gain; This represents the standard repulsive force gain under normal conditions. The minimum repulsive force gain in a confined space; To adjust the index.

5. The unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field according to claim 4, characterized in that, In step 3.1, Take a fixed value of 1.

2.

6. The unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field according to claim 4, characterized in that, Step 4 includes the following specific steps: Step 4.1: Improve the calculation of attraction and repulsion; In the formula: This is the gravitational gain coefficient; Current location of the intelligent vehicle To global goal The Euclidean distance; For the introduction of target distance adjustment term; and These are the unit direction vectors pointing towards the target and away from the obstacle, respectively; Step 4.2: New node generation; New node The calculation formula is: After generating a new node, the collision detection process of the RRT* algorithm is executed; if the node is safe, the parent node reselection and rewiring operation are further executed.

7. The unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field as described in claim 6, characterized in that, In step 6, each path point experiences two virtual forces: 1) Internal contraction force : Used to simulate the internal tension of an elastic band, calculated based on the geometric relationship between adjacent nodes, with the current node... Pull towards its front and rear nodes and The midpoint of the line connecting them; In the formula, For smoothing weighting coefficients; 2) External repulsive force : In the formula, The safety weight coefficient, and satisfies .

8. The unmanned vehicle path planning method based on the fusion of dynamic step size and velocity-sensing potential field according to claim 7, characterized in that, Step 7 includes the following specific steps: Perform on all intermediate nodes in the path Each iteration of the update formula is as follows: in, For the number of iterations, and The first on the path The node at the th Second and third Spatial coordinates during the next iteration of optimization; Convergence judgment and iteration termination: To determine the termination point of the elastic band iterative optimization, the following two convergence conditions are set. If either condition is met, the iteration stops and the current path is output as the final smooth trajectory: Condition 1, Convergence criterion based on location change: After each iteration update, calculate all movable path points. Find the Euclidean distance between the displacement vectors from the current iteration to the previous iteration, and identify the maximum value. : In the formula, For the first on the path The node at the th Spatial coordinates during the next iteration of optimization; Set a convergence threshold ,when When the iteration converges; Condition 2, Maximum Iteration Limit: Set a maximum number of iterations. As an auxiliary termination condition, when the number of iterations... achieve When condition 1 is met, the iteration is forcibly stopped and the current path is output.

Images