CN-121977583-A - Unmanned vehicle path planning method based on fusion of dynamic step length and speed perception potential field

CN121977583ACN 121977583 ACN121977583 ACN 121977583ACN-121977583-A

Abstract

The invention is suitable for the technical field of autonomous navigation of unmanned ground vehicles and provides an unmanned vehicle path planning method based on fusion of dynamic step length and speed perception potential field, wherein the method adopts a layered planning architecture, namely, an upper layer is based on a TV-APF strategy, an environmental safety factor, a task guidance factor and a path inertia factor are introduced into node expansion of an RRT algorithm to perform ternary coupling dynamic step length calculation, and the step length is used as a speed proxy variable to dynamically adjust the repulsive force gain and the safety vision of an artificial potential field to generate an initial path framework conforming to dynamic constraint; the lower layer is based on an elastic band model of potential field gradient, and the initial path is iterated and smoothed through internal contraction force and external repulsive force, so that the final track is ensured to meet the geometric smoothness and the dynamic safety boundary. The method remarkably improves the planning success rate, the path smoothness and the vehicle running safety in a complex environment.

Inventors

LIU YUTONG
LIU KE
LEI YULONG
FU YAO
ZHANG ZE
WANG XINLEI
WANG XIANG

Assignees

吉林大学

Dates

Publication Date: 20260505
Application Date: 20260408

Claims (8)

1. The unmanned vehicle path planning method based on the fusion of the dynamic step length and the speed perception potential field is characterized by comprising the following steps: step 1, constructing an environment sensing and local coordinate system; external environment point cloud data and real-time physical motion state of the vehicle are acquired through vehicle-mounted environment sensing equipment, the intelligent vehicle is simplified into particles based on a configuration space theory, and expansion processing is carried out on obstacles, so that the current position point of the intelligent vehicle is obtained Constructing a local coordinate system for the origin, determining a target direction vector Vector of growth direction Motion direction vector before intelligent vehicle reaches current point ; Step 2, ternary coupling multi-source dynamic step length calculation; computing environmental security factors based on environmental awareness Based on And (3) with Is used for calculating task guidance factors Based on And (3) with Calculating path inertia factor from the angle change of (2) The final expansion step length is obtained through the product coupling of the three factors ; Step 3, speed sensing and self-adaptive potential field updating; the final expansion step length calculated currently Dynamic computing speed aware dynamic safety vision as proxy variable for intelligent vehicle instantaneous speed And according to the measured obstacle distance And (3) with The intensity coefficient of the repulsive force field is adjusted in real time in a nonlinear manner to obtain the self-adaptive repulsive force gain ; Step 4, resultant force guiding and RRT tree growth; calculating attraction force based on improved artificial potential field model Repulsive force Resultant force is synthesized In step length along resultant force direction Generating new nodes Performing collision detection, father node reselection and rerouting operation of an RRT algorithm to generate a progressive optimal initial path skeleton; step 5, constructing an elastic band stress model; modeling the generated initial path as a stress balance elastic band placed in a complex physical field, wherein each path point is acted by internal contraction force and external repulsive force; step 6, calculating virtual acting force; Calculating internal contraction force based on geometric relationship of adjacent nodes And directly calling the self-adaptive repulsive force gain calculated in the step 3 Calculating external repulsive force ; Step 7, path point iterative updating; iteratively updating the path intermediate nodes until convergence conditions are met, and outputting a smoothed final track; Step 8, performing bottom track tracking and wire control chassis; And sending the path point sequence subjected to physical level smoothing and safety optimization to a bottom layer motion controller as a reference input quantity, generating a control instruction and driving the drive-by-wire chassis to execute physical obstacle avoidance and path tracking actions.
2. The unmanned vehicle path planning method based on the fusion of dynamic step size and speed perception potential field according to claim 1, wherein the step 1 comprises the following specific steps: step 1.1, environment sensing; Firstly, acquiring external environment point cloud data through vehicle-mounted environment sensing equipment or a local high-precision map, and acquiring the current real-time physical motion state of an intelligent vehicle by combining a wheel speed sensor or an inertia measurement unit to acquire the current position point of the intelligent vehicle in real time The length of the straight line connecting to the nearest obstacle surface in the environment, i.e. the original Euclidean distance ; Based on the theory of configuration space, the intelligent vehicle with physical dimensions is simplified into a particle model, and the expansion treatment is carried out on the environmental obstacle, wherein the definition and calculation of the expansion radius are as follows: Defining the length of a vehicle body as The width of the vehicle body is The minimum circumscribing circle radius of the geometric center of the vehicle is adopted as the physical equivalent radius And superimposing the safety margin To determine the expansion radius The calculation formula is as follows: Step 1.2, constructing a local coordinate system; During each iteration expansion of the random tree, definition is made Is the origin of a local reference coordinate system and uses a global target As the target point of the object to be processed, Is that From the parent node of the preamble of (a) Three key vectors are led out to point to the global target Is the target direction vector of (1) Pointing to the current random sampling point Is a growth direction vector of (a) Motion direction vector before intelligent vehicle reaches current point ; Is that And (3) with The included angle between the two parts is that, Is that And (3) with The variation of the included angle between the two.
3. The unmanned vehicle path planning method based on the fusion of dynamic step size and speed perception potential field according to claim 2, wherein the step 2 comprises the following specific steps: Step 2.1 computing environmental Security factors ; Based on the perception of the environment, Measured obstacle distance to nearest obstacle The calculation formula is as follows: Or will be The shortest straight line distance from the node to the expanded obstacle boundary is directly represented in the grid map; When (when) Indicating that the node is located in a secure area when When the node enters the safety buffer area, the node is considered to collide; Environmental safety factor The method comprises the steps of carrying out fine description by adopting piecewise linear functions, and dividing the environment into a safety zone, a transition zone and a critical zone: In the formula, Maximum search step allowed for the intelligent vehicle in the open area; For minimum searching step length in limited space, take 0.1-0.2 Times of (3); A global safety distance threshold is preset; As the threshold value of the critical obstacle avoidance distance, Wherein To take the maximum function, i.e. select And The larger value of the two is used as a critical obstacle avoidance distance, and when the obstacle distance is lower than the critical obstacle avoidance distance, the minimum step length is automatically cut into for creeping search; step 2.2 calculating the task direction factor ; Defining a growth direction vector Vector with target direction The step gain is dynamically adjusted by calculating the cosine value of the included angle between the two: In the formula, Guiding weight coefficients for the task; Step 2.3 calculating Path inertia factor ; Definition of the definition Calculate its vector with growth direction Angle variation of (2) The unit is radian; In the formula, A starting node for path planning; is an inertial penalty coefficient; step 2.4, final dynamic step synthesis; the final expansion step length is obtained through the product coupling of the three factors : 。
4. The unmanned vehicle path planning method based on the fusion of dynamic step size and speed perception potential field according to claim 3, wherein the step 3 comprises the following specific steps: step 3.1 calculating a speed aware dynamic security FOV ; The final expansion step length calculated currently The dynamic safety vision is defined as the proxy variable of the instantaneous speed of the intelligent vehicle as follows: In the formula, The minimum static safe buffer distance of the intelligent vehicle; Is a velocity gain coefficient; Step 3.2 calculating the adaptive repulsive force gain ; According to the current actual measurement And calculated dynamic security view Is used for adjusting the intensity coefficient of the repulsive field in real time in a nonlinear manner: wherein: Is self-adaptive repulsive force gain; Is the standard repulsive force gain under the conventional environment; is the minimum repulsive force gain under the limited space; To adjust the index.
5. The unmanned vehicle path planning method based on fusion of dynamic step size and velocity-aware potential field of claim 4, wherein in step 3.1, A fixed value of 1.2 was taken.
6. The unmanned vehicle path planning method based on the fusion of dynamic step size and speed perception potential field according to claim 4, wherein the step 4 comprises the following specific steps: Step 4.1, improving attraction and repulsion calculation; wherein: Is the gravitational gain coefficient; For the current position point of the intelligent vehicle To global targets Is the euclidean distance of (2); Adjusting the term for the introduced target distance; And Unit direction vectors pointing to the target and away from the obstacle, respectively; Step 4.2, generating a new node; New node The calculation formula of (2) is as follows: after generating new node, executing collision detection flow of RRT algorithm standard, if node is safe, further executing parent node reselection and rerouting operation.
7. The unmanned vehicle path planning method based on the fusion of dynamic step size and speed aware potential field of claim 6, wherein in step 6, each path point is subjected to two virtual forces: 1) Internal contractive force : For simulating the internal tension of an elastic belt, calculating the current node based on the geometrical relationship of adjacent nodes To its front and rear nodes And Is connected with the middle point of the connecting line; In the formula, Is a smooth weight coefficient; 2) External repulsive force : In the formula, Is a safety weight coefficient and meets 。
8. The unmanned vehicle path planning method based on the fusion of dynamic step size and speed perception potential field according to claim 7, wherein the step 7 comprises the following specific steps: All intermediate nodes in the path are subjected to And (3) carrying out iterative updating, wherein each updating formula is as follows: Wherein, the For the number of iterations, And Respectively the first on the path The individual node is at the first Secondary and tertiary Space position coordinates during secondary iteration optimization; convergence judgment and iteration termination: In order to determine the termination time of elastic band iteration optimization, the following two convergence conditions are set, and the following two convergence conditions are satisfied, namely, the iteration is stopped and the current path is output as the final smooth track: Condition 1, convergence criterion based on position change, calculating all movable path points after each iterative update The Euclidean distance of the displacement vector from the current iteration to the last iteration is found out : In the formula, Is the first on the path The individual node is at the first Space position coordinates during secondary iteration optimization; Setting a convergence threshold When (when) When the iteration converges; Condition 2, maximum iteration number limit, setting a maximum iteration number As an auxiliary termination condition, when the number of iterations Reach to And (3) forcibly stopping iteration and outputting the current path no matter whether the condition 1 is met or not.

Description

Unmanned vehicle path planning method based on fusion of dynamic step length and speed perception potential field Technical Field The invention belongs to the technical field of autonomous navigation of unmanned ground vehicles, and particularly relates to an unmanned vehicle path planning method based on fusion of dynamic step length and speed perception potential field. Background The mobile intelligent vehicle technology has important application value in unstructured scenes such as post-disaster rescue, field survey and complex warehousing. The path planning is used as a core link of autonomous navigation and is responsible for planning a collision-free feasible path from a starting point to a target point in a complex environment, geometrical constraint, topography characteristics and vehicle motion states are considered at the same time, and the method belongs to a typical multi-constraint multi-target optimization problem. The existing method mainly comprises sampling, graph searching, potential field and curve interpolation and the like, and the planning problem of a complex limited space is difficult to independently solve by a single method, and the advantages are complemented by fusion of a plurality of methods. However, existing fusion algorithms typically employ a stiffer parameter adjustment strategy, such as linearly adjusting the step size based on obstacle distance alone, ignoring mission direction guidance and road inertia constraints, resulting in intelligent vehicles that sideslip when open-road searching is blind or when unstructured roads are cornering. Meanwhile, the parameters of the traditional artificial potential field cannot be adjusted in a self-adaptive manner along with the speed of the vehicle, so that obstacle avoidance is not enough at high speed, and the obstacle avoidance cannot be realized through a narrow channel due to overlarge repulsive force at low speed. These problems make the existing algorithms poorly adaptable in unstructured environments, and it is difficult to combine planning success rate with motion smoothness. Disclosure of Invention The embodiment of the invention aims to provide an unmanned vehicle path planning method based on fusion of dynamic step length and speed perception potential field, and aims to solve the problems in the background technology. The embodiment of the invention is realized in such a way that the unmanned vehicle path planning method based on the fusion of the dynamic step length and the speed perception potential field comprises the following steps: step 1, constructing an environment sensing and local coordinate system; external environment point cloud data and real-time physical motion state of the vehicle are acquired through vehicle-mounted environment sensing equipment, the intelligent vehicle is simplified into particles based on a configuration space theory, and expansion processing is carried out on obstacles, so that the current position point of the intelligent vehicle is obtained Constructing a local coordinate system for the origin, determining a target direction vectorVector of growth directionMotion direction vector before intelligent vehicle reaches current point; Step 2, ternary coupling multi-source dynamic step length calculation; computing environmental security factors based on environmental awareness Based onAnd (3) withIs used for calculating task guidance factorsBased onAnd (3) withCalculating path inertia factor from the angle change of (2)The final expansion step length is obtained through the product coupling of the three factors; Step 3, speed sensing and self-adaptive potential field updating; the final expansion step length calculated currently Dynamic computing speed aware dynamic safety vision as proxy variable for intelligent vehicle instantaneous speedAnd according to the measured obstacle distanceAnd (3) withThe intensity coefficient of the repulsive force field is adjusted in real time in a nonlinear manner to obtain the self-adaptive repulsive force gain; Step 4, resultant force guiding and RRT tree growth; calculating attraction force based on improved artificial potential field model Repulsive forceResultant force is synthesizedIn step length along resultant force directionGenerating new nodesPerforming collision detection, father node reselection and rerouting operation of an RRT algorithm to generate a progressive optimal initial path skeleton; step 5, constructing an elastic band stress model; modeling the generated initial path as a stress balance elastic band placed in a complex physical field, wherein each path point is acted by internal contraction force and external repulsive force; step 6, calculating virtual acting force; Calculating internal contraction force based on geometric relationship of adjacent nodes And directly calling the self-adaptive repulsive force gain calculated in the step 3Calculating external repulsive force; Step 7, path point iterative updating; iteratively upd