A two-level path planning method based on fast expanding random tree and dynamic window method

By combining the fast-scaling random tree algorithm and dynamic window method, the problems of path planning inefficient and local minimum traps in the existing technology under dynamic environment and kinematic constraints are solved, and efficient and accurate path planning is achieved.

CN115454083BInactive Publication Date: 2024-11-29FUZHOU UNIV
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202211170487.9
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-09-23
Publication Date
2024-11-29
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

Existing path planning algorithms have problems with inefficiency and local minimum traps when dealing with dynamic environments and kinematic constraints.

Method used

A two-layer path planning method based on the fast-scaling random tree and dynamic window method is adopted. The rapid-scaling random tree algorithm is used to carry out global planning, and the kinematic constraints of unmanned vehicles are integrated into the dynamic window method to carry out local path planning.

Benefits of technology

The path planning that effectively integrates kinematic constraints in a dynamic environment is realized, which improves the efficiency and accuracy of path planning and avoids the local minimum value trap.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115454083B_ABST
    Figure CN115454083B_ABST
Patent Text Reader

Abstract

The present invention proposes a two-layer path planning method based on a rapidly expanding random tree and a dynamic window method. The obstacles in the map are first processed, including safety distance setting and obstacle filling. Then, the processed map is preliminarily planned using a rapidly expanding random tree algorithm, wherein a bionic design that imitates the phototropism of plant growth is proposed. The local target point is obtained by the rule of the farthest obstacle-free point, and the dynamic window method is further used to run towards it with the local target point as a guide. When the smart car reaches a certain range of the local target point, it turns to run to the next target point, and the above steps are repeated until the final global target point is reached. The application of this technical solution can realize the integration of the kinematic constraints of the unmanned vehicle into the path planning process, so that the path planned by the algorithm meets the driving requirements of the unmanned vehicle.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving path planning and autonomous navigation technology, in particular to a double-layer path planning method based on a fast-expanding random tree and a dynamic window method. Background Art

[0002] At present, the automobile industry is undergoing a major change that has not been seen in a century. The "electrification", "networking", "intelligence" and "sharing" of automobiles are considered to be the future development direction of the industry.

[0003] Smart vehicles usually refer to vehicles that have advanced sensors (such as laser radar, millimeter-wave radar, cameras, etc.), controllers, actuators and other devices added to them. Through the on-board environmental perception system and information terminal, information exchange with people, vehicles, roads, etc. is achieved, so that the vehicle has intelligent environmental perception capabilities, can automatically analyze the safety and danger status of vehicle driving, and make the vehicle reach the destination according to the wishes of the person, and finally achieve the purpose of replacing human operation. The key technologies of smart vehicles include environmental perception, navigation positioning, path planning, and decision control, among which path planning is a key part of smart vehicle technology and is of great significance to the research of smart vehicles.

[0004] Path planning can be divided into global path planning in a static environment and local path planning in a dynamic environment. In a static environment, obstacles do not move, so the core idea of ​​the global path planning algorithm is to obtain the optimal path based on the location, size and other related information of each obstacle in a known global environment. In a dynamic environment, there are some moving obstacles with unknown trajectories in a static environment. Sensors need to capture the information of moving obstacles and plan paths in real time. Therefore, the local path planning algorithm focuses on the dynamic obstacle avoidance capability of the unmanned vehicle. How to obtain local information through sensors in an unknown environment to obtain paths is a key research issue at present. Summary of the invention

[0005] In view of this, the purpose of the present invention is to provide a two-layer path planning method based on a rapidly expanding random tree and a dynamic window method, so as to integrate the kinematic constraints of the unmanned vehicle into the path planning process, so that the path planned by the algorithm meets the driving requirements of the unmanned vehicle.

[0006] To achieve the above object, the present invention adopts the following technical solution: a two-layer path planning method based on fast expanding random tree and dynamic window method, comprising the following steps:

[0007] Step S1: Determine the coordinates of the starting point and the target point of the unmanned vehicle, read the coordinates of the obstacle, and mark the irregular obstacle as a rectangular obstacle;

[0008] Step S2: Set a certain obstacle gap size. If it is smaller than the set value, fill it with a number of square obstacles and set a "potential energy zone" near the existing obstacles;

[0009] Step S3: using the rapidly expanding random tree algorithm to perform global planning on the processed map to obtain a series of path points;

[0010] Step S4: Select the above path points and select appropriate path points as local target points for local path planning. The selection method follows the principle of the farthest obstacle-free object.

[0011] Step S5: Incorporate the kinematic constraints of the unmanned vehicle into the dynamic window method, and visit a series of local target points in sequence until the final global target point is reached.

[0012] In a preferred embodiment, in step S1, the irregular obstacle is discretized, thereby calibrating the length L of the rectangular obstacle = (x max -x min ), width W=(y max -y min ), where x max , x min ,y max ,y min are the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in the discrete points respectively.

[0013] In a preferred embodiment, in step S2, after the irregular obstacles are marked as rectangular obstacles, if the distance S between two adjacent obstacles is 1 =K·a, where a is the width of the unmanned vehicle and K is the safety factor, then fill the gap; The area is set as the range of the potential energy zone.

[0014] In a preferred embodiment, in step S3, the starting point is used as the root node of the rapidly expanding random number, and based on the node, the tree node closest to the node is found, and a step length is expanded. This method is continuously iterated to eventually generate a feasible optimization path; the rapidly expanding random tree algorithm uses the starting point of the path planning as the root node q of the random tree T. start , randomly sample in space and get a random point q rand , then calculate the random q rand With all nodes q in the random tree T i Find the distance from the random point q rand The nearest node q i As a neighboring point q near , random tree T from neighboring points q near To a random point qrand Move in the given step length esp and expand to generate a new node q new ; If at the neighboring point q near To the new node q new If there is no collision with obstacles during the expansion of the direction, and the new node is not in the potential energy zone, the new node q new Add to the random tree T; if there is no collision with the obstacle, the new node q is added with a certain probability new Add to the random tree T; if a collision occurs, resample the random point q rand ;Finally determine the new node q new Is it at the target point q? goal If it is not in the target point area, repeat the above process; if it is in the target point area, move the new node q new As the parent node of the target point, and the target point q goal Add to the random tree T, from the target point q goal Backtrack to the root node q start , get the path points of the planned path.

[0015] In a preferred embodiment, when selecting a random node, a series of concentric arcs are selected as target nodes with a certain probability, and the selection formula is as follows:

[0016]

[0017] Where r:

[0018]

[0019] Among them, q rand (x, y) is the coordinate of the random point to be selected; n 1 , n 2 , n 3 , n 4 are mutually independent random numbers between 0 and 1; max , Y max They are the maximum values ​​of the horizontal and vertical axes of the map respectively; are the horizontal and vertical coordinates of the target point respectively; R is the distance from the starting point to the target point, and m is the concentric circle coefficient, which is generally between 1 and 2;

[0020] In a preferred embodiment, in step S4, the selection of local target points follows the principle of farthest obstacle-free, and the specific rules for selecting sub-target points are as follows:

[0021] Assume the extracted path point is {P k ,k=1,2,…,n}, connect P 1 P 3 , if P 1 P3 Does not pass through obstacles, that is, line segment P 1 P 3 If the distance to the obstacle is greater than the preset threshold, continue to connect P 1 P 4 , until P 1 P k (k= 4,5,…,n) passes through an obstacle, then P k-1 With P 1 Connect them, delete the intermediate redundant points, and update the path; from node P k-1 Repeat the above operation until there are no redundant turning points in the path, and record these path nodes as local target points.

[0022] 6. A two-layer path planning method based on a rapidly expanding random tree and a dynamic window method according to claim 1, characterized in that, in step S5, a dynamic window method is used to sample multiple groups of speeds in the speed (v, w) space, where v and w are the speed and angular velocity of the unmanned vehicle, respectively, and the trajectory of the unmanned vehicle at these speeds within a certain period of time is simulated; after obtaining multiple groups of trajectories, these trajectories are evaluated, and the speed corresponding to the optimal trajectory is selected to drive the unmanned vehicle to move; the evaluation function is:

[0023] G(v,ω)=δ·(α·H G (v,ω)+β·D(v,ω)+γ·V(v,ω))

[0024] Among them, G(v,ω) is the evaluation function, H G (v, ω) is the heading angle function, D(v, ω) is the distance function to the obstacle, V(v, ω) is the speed function, the three parameters α, β, and γ represent the corresponding weights, and δ makes the weights of the three parts smoother.

[0025] In a preferred embodiment, the motion trajectory between two adjacent points is regarded as a straight line, that is, a movement of v along the x-axis of the robot coordinate system. t Δt; simply project the distance onto the x-axis and y-axis of the world coordinate system to obtain the displacement Δx and Δy of the smart car in the world coordinate system at time t+1 relative to time t;

[0026] Δx=v t Δtcos(θ t )

[0027] Δy=v t Δtsin(θ t )

[0028] Among them, θ t The larger change per unit time interval, and so on, to calculate the trajectory within a period of time, you only need to accumulate and sum the displacement increments during this period:

[0029] x=x+v t Δtcos(θ t )

[0030] y=y+v t Δtsin(θ t )

[0031] θ t =θ t +w t Δt

[0032] Traverse the above local target points in turn until reaching the end point; in the two-dimensional space of speed (v, w), there are infinite sets of speeds; according to the limitations of the smart car itself and the environment, control the sampling speed within a certain range:

[0033] Smart cars are limited by their own maximum and minimum speeds:

[0034] V s ={v∈[v min ,v max ],w∈[w min ,w max ]}

[0035] Smart cars are limited by their own acceleration and deceleration performance:

[0036]

[0037] where v c , w c is the current speed of the smart car, are the maximum braking deceleration and maximum acceleration, are the maximum angular deceleration and maximum angular acceleration respectively;

[0038] At maximum deceleration, speed range:

[0039]

[0040] Among them, dist(v,w) is the distance between the current position and the obstacle;

[0041] The actual vehicle turning angle has the following kinematic constraints;

[0042]

[0043] Among them, r is the turning radius, R is the limit turning radius, v, ω are the speed and angular velocity of the vehicle respectively, L is the wheelbase of the vehicle, δ max is the maximum turning angle of the front wheels;

[0044] Therefore, Vs Rewritten as:

[0045]

[0046] Modified speed space:

[0047] V r =V′ s ∩V d ∩V a .

[0048] Compared with the prior art, the present invention has the following beneficial effects: The purpose of the present invention is to provide a two-layer path planning method based on a rapidly expanding random tree algorithm (RRT) and a dynamic window method (RRT), using an improved rapidly expanding random tree algorithm (RRT) and an optimized dynamic window method (DWA) at the global layer and the local layer respectively. This method solves the problems that the RRT algorithm cannot plan for situations involving dynamic obstacles and the kinematic constraints of the vehicle, and that the DWA algorithm is prone to fall into local minima. The original RRT is improved accordingly to adapt to the fusion with the DWA algorithm. The kinematic constraints of the unmanned vehicle are integrated into the path planning process, so that the path planned by the algorithm meets the driving requirements of the unmanned vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 The figure is a flow chart of a method according to a preferred embodiment of the present invention.

[0050] Figure 2 This is a schematic diagram of the obstacle merging principle of a preferred embodiment of the present invention.

[0051] Figure 3 Schematic diagram of obstacle potential energy zone in a preferred embodiment of the present invention.

[0052] Figure 4 Schematic diagram of the biomimetic designed RRT according to the preferred embodiment of the present invention.

[0053] Figure 5 Schematic diagram of an unmodified RRT according to a preferred embodiment of the present invention.

[0054] Figure 6 A schematic diagram of local target point selection according to a preferred embodiment of the present invention.

[0055] Figure 7 Schematic diagram of the kinematic model of the unmanned vehicle according to the preferred embodiment of the present invention.

[0056] Figure 8 It is a schematic diagram of the fusion effect of RRT and DWA algorithms in a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0057] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0058] It should be noted that the following detailed descriptions are illustrative and are intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present application belongs.

[0059] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application; as used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or their combinations.

[0060] like Figure 1-Figure 7 As shown, the two-layer path planning method based on the dynamic window method guided by the fast expanding random tree algorithm provided in this embodiment includes the following implementation process:

[0061] Step 1: Determine the coordinates of the starting point and the target point of the unmanned vehicle, read the coordinates of the obstacle, and calibrate the irregular obstacle as a rectangular obstacle. Discretize the irregular obstacle and calibrate the length of the rectangular obstacle L = (x max -x min ), width W=(y max -y min ), where x max , x min , y max ,y min They are the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in the discrete points respectively.

[0062] Step 2: Set a certain obstacle gap size. If it is smaller than the set value, fill it with several square obstacles. At the same time, set a "potential energy zone" near the existing obstacles to reduce the probability of path nodes entering the area. After marking the irregular obstacles as rectangular obstacles, if the distance S between two adjacent obstacles is 1 =K·a (where a is the width of the unmanned vehicle and K is the safety factor), then fill the gap. Figure 1 . The distance between the obstacle surface and The area is set as the range of the potential energy zone, refer to Figure 2 For specific application, see how to add new nodes to the random tree in step S3.

[0063] Step 3: Use the fast expansion random tree algorithm to perform global planning on the processed map and obtain a series of path points. The starting point is used as the root node of the fast expansion random number, and based on this node, the tree node closest to this node is found and a step length is expanded. This method is continuously iterated to finally generate a feasible optimization path. Specifically, the algorithm uses the starting point of the path planning as the root node q of the random tree T start , randomly sample in space and get a random point q rand , then calculate the random q rand With all nodes q in the random tree T i Find the distance from the random point q rand The nearest node q i As a neighboring point q near , random tree T from neighboring points q near To a random point q rand Move in the given step length esp and expand to generate a new node q new ( Figure 2 ). If at the neighboring point q near To the new node q new If there is no collision with obstacles during the expansion of the direction, and the new node is not in the potential energy zone, the new node q new Add to the random tree T; if there is no collision with the obstacle, the new node q is added with a certain probability new Add to the random tree T; if a collision occurs, resample the random point q rand Finally, determine the new node q new Is it at the target point q? goal If it is not in the target point area, repeat the above process; if it is in the target point area, move the new node q new As the parent node of the target point, and the target point q goal Add to the random tree T, from the target point q goal Backtrack to the root node q start , get the path points of the planned path.

[0064] In order to speed up the arrival at the target point and improve the efficiency of the algorithm, a bionic design is proposed, which mainly imitates the phototropism of plant growth. The "sunlight" is set near the target point, so that the random tree growing outward has the tendency to expand toward the target point. Specifically, when selecting random nodes, a series of concentric arcs are selected as the target node (target point) with a certain probability. The selection formula is as follows:

[0065]

[0066] Where r:

[0067]

[0068] Among them, q rand (x, y) is the coordinate of the random point to be selected; n 1 , n 2 , n 3 , n 4 are mutually independent random numbers between 0 and 1; max , Y max They are the maximum values ​​of the horizontal and vertical axes of the map respectively; are the horizontal and vertical coordinates of the target point respectively. R is the distance from the starting point to the target point, and m is the concentric circle coefficient, which is generally between 1 and 2. Figure 3 , where the black rectangle is the obstacle, the green five-pointed star is the starting point, the red five-pointed star is the target point, and the blue × is the selected random point, showing that the target point is dense in the center and sparse at the edge. The green line is the expanded random tree, and the blue line is the planned path. Figure 4 It is the path planned by the unimproved RRT. The random tree expands slowly and has no direction.

[0069] Step 4: Select the above path points and select appropriate path points as local target points for local path planning. The selection method follows the principle of the farthest obstacle-free point. The specific rules for selecting sub-target points are as follows:

[0070] Assume the extracted path point is {P k ,k=1,2,…,n}, connect P 1 P 3 , if P 1 P 3 Does not pass through obstacles, that is, line segment P 1 P 3 If the distance to the obstacle is greater than the preset threshold, continue to connect P 1 P 4 , until P 1 P k (k= 4,5,…,n) passes through an obstacle, then P k-1 With P 1 Connect them, delete the intermediate redundant points, and update the path; from node P k-1 Repeat the above steps until there are no redundant turning points in the path, and record these path nodes as local target points. Figure 5 , the random points are hidden in this figure, where the green line segment is the path after deleting redundant points, the red * is the selected local target point, and other information is as above.

[0071] Step 5: Incorporate the kinematic constraints of the unmanned vehicle into the dynamic window method, and visit a series of local target points in turn until the final global target point. Use the dynamic window method to sample multiple sets of speeds in the speed (v, w) space (v, w are the speed and angular velocity of the unmanned vehicle, respectively), and simulate the trajectory of the unmanned vehicle at these speeds for a certain period of time. After obtaining multiple sets of trajectories, evaluate these trajectories and select the speed corresponding to the optimal trajectory to drive the unmanned vehicle to move. Evaluation function:

[0072] G(v,ω)=δ·(α·H G (v,ω)+β·D(v,ω)+γ·V(v,ω))

[0073] Among them, G(v,ω) is the evaluation function, H G (v, ω) is the heading angle function, D(v, ω) is the distance function to the obstacle, V(v, ω) is the speed function, the three parameters α, β, and γ represent the corresponding weights, and δ makes the weights of the three parts smoother.

[0074] In the dynamic window algorithm, to simulate the trajectory of the smart car, we need to know the motion model of the smart car. First, simplify the smart car into a two-wheel model. Assume that the trajectory of the two-wheeled mobile smart car is a series of arcs or straight lines (when the rotation speed is 0). A pair (v t , w t ) represents an arc trajectory. For simplicity, since the distance of movement of the smart car in adjacent moments (usually ms) is short, the motion trajectory between two adjacent points can be regarded as a straight line, that is, it moves v along the x-axis of the robot coordinate system. t Δt (interval time Δt). Simply projecting this distance onto the x-axis and y-axis of the world coordinate system will yield the displacement Δx and Δy of the smart car’s coordinate movement in the world coordinate system at time t+1 relative to time t.

[0075] Δx=v t Δtcos(θ t )

[0076] Δy=v t Δtsin(θ t )

[0077] Among them, θ t The larger change per unit time interval.

[0078] Similarly, to calculate the trajectory within a period of time, we only need to accumulate and sum the displacement increments during this period:

[0079] x=x+v t Δtcos(θ t )

[0080] y=y+v t Δtsin(θ t )

[0081] θ t =θ t +w t Δt

[0082] Traverse the above local target points in turn until you reach the end. In the two-dimensional space of speed (v, w), there are infinite sets of speeds. However, according to the limitations of the smart car itself and the environment, the sampling speed can be controlled within a certain range:

[0083] 1. Smart cars are limited by their own maximum and minimum speeds:

[0084] V s ={v∈[v min ,v max ],w∈[w min ,w max ]}

[0085] 2. Smart cars are limited by their own acceleration and deceleration performance:

[0086]

[0087] where v c , w c is the current speed of the smart car, are the maximum braking deceleration and maximum acceleration, are the maximum angular deceleration and maximum angular acceleration respectively.

[0088] 3. Considerations based on the safety of smart cars:

[0089] In order to stop before hitting an obstacle, the speed has a range under maximum deceleration:

[0090]

[0091] Due to the constraints of the actual vehicle turning angle, some of the speed spaces do not meet the requirements. The actual vehicle turning angle has the following kinematic constraints:

[0092]

[0093] Among them, r is the turning radius, R is the limit turning radius, v, ω are the speed and angular velocity of the vehicle respectively, L is the wheelbase of the vehicle, δ max is the maximum turning angle of the front wheels. Figure 6 , the kinematic constraints of the vehicle.

[0094] Therefore, V s Can be rewritten as:

[0095]

[0096] Modified speed space:

[0097] V r =V′ s ∩V d ∩V a

[0098] Finally, the DWA algorithm is used to traverse the above local target points in sequence until the final target point. Figure 7 .

[0099] The work carried out by the present invention mainly includes: first, summarizing the feasibility and shortcomings of the previous improvements on the fast-expanding random tree, and in view of the low search efficiency caused by the randomness of the random tree expansion, a design that imitates the phototropism of plant growth is proposed, and the target point is defined as a "light source", so that the random point is taken to a series of concentric circles with the target point as the center near the target point with a certain probability. Second: In order to improve the effect of local layer planning, that is, the dynamic window method, it is proposed to pre-process the obstacles, by reducing the passability of the obstacles within a certain distance, and thus better avoid the obstacles. Further, for the connection between the fast-expanding random tree algorithm and the dynamic window method, the farthest obstacle-free strategy is proposed. Starting from the starting point, the node that is farthest from the current node and has not collided with the obstacle is selected, the node is recorded, and the node is updated to the current node, and the operation is repeated until all nodes are traversed.

Claims

1. A two-layer path planning method based on fast-expanding random trees and dynamic window method, characterized in that: The following steps are involved: Step S1: Determine the coordinates of the starting point and the target point of the unmanned vehicle, read the coordinates of the obstacle, and mark the irregular obstacle as a rectangular obstacle; Step S2: Set a certain obstacle gap size. If it is smaller than the set value, fill it with a number of square obstacles and set a "potential energy zone" near the existing obstacles; Step S3: using the rapidly expanding random tree algorithm to perform global planning on the processed map to obtain a series of path points; Step S4: Select the above path points and select appropriate path points as local target points for local path planning. The selection method follows the principle of the farthest obstacle-free object. Step S5: Incorporating the kinematic constraints of the unmanned vehicle into the dynamic window method, and sequentially visiting a series of local target points until the final global target point; In step S3, the starting point is used as the root node of the fast expansion random number, and based on the node, the tree node closest to the node is found, and a step length is expanded. This method is continuously iterated to finally generate a feasible optimization path; The fast expansion random tree algorithm uses the starting point of path planning as the root node q of the random tree T start , randomly sample in space and get a random point q rand , then calculate the random q rand With all nodes q in the random tree T i Find the distance from the random point q rand The nearest node q i As a neighboring point q near , random tree T from neighboring points q near To a random point q rand Move in the given step length esp and expand to generate a new node q new ; If at the neighboring point q near To the new node q new If there is no collision with obstacles during the expansion of the direction, and the new node is not in the potential energy zone, the new node q new Add to the random tree T; if there is no collision with the obstacle, the new node q is added with a certain probability new Add to the random tree T; if a collision occurs, resample the random point q rand ;Finally determine the new node q new Is it at the target point q? goal If it is not in the target point area, repeat the above process; if it is in the target point area, move the new node q new As the parent node of the target point, and the target point q goal Add to the random tree T, from the target point q goal Backtrack to the root node q start , get the path points of the planned path; In step S5, a dynamic window method is used to sample multiple groups of speeds in the speed (v, w) space, where v and w are the speed and angular velocity of the unmanned vehicle, respectively, and the trajectory of the unmanned vehicle at these speeds within a certain period of time is simulated; after obtaining multiple groups of trajectories, these trajectories are evaluated, and the speed corresponding to the optimal trajectory is selected to drive the unmanned vehicle to move; the evaluation function is: G(v,ω)=δ·(α·H G (v,ω)+β·D(v,ω)+γ·V(v,ω)) Among them, G(v,ω) is the evaluation function, H G (v, ω) is the heading angle function, D(v, ω) is the distance function to the obstacle, V(v, ω) is the speed function, the three parameters α, β, and γ represent the corresponding weights, and δ makes the weights of the three parts smoother.

2. The two-layer path planning method based on fast expanding random tree and dynamic window method according to claim 1 is characterized in that: In step S1, the irregular obstacle is discretized, thereby calibrating the length L of the rectangular obstacle = (x max -x min ), width W=(y max -y min ), where x max , x min ,y max ,y min are the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in the discrete points respectively.

3. The two-layer path planning method based on fast expanding random tree and dynamic window method according to claim 1 is characterized in that: In step S2, after the irregular obstacle is marked as a rectangular obstacle, if the distance between two adjacent obstacles is S1 = K·a, where a is the width of the unmanned vehicle and K is the safety factor, the gap is filled; the distance between the obstacle surface is The area is set as the range of the potential energy zone.

4. The two-layer path planning method based on fast expanding random tree and dynamic window method according to claim 1 is characterized in that: When selecting a random node, a series of concentric arcs are selected as the target node with a certain probability. The selection formula is as follows: Where r: Among them, q rand (x, y) is the coordinate of the random point to be selected; n1, n2, n3, n4 are independent random numbers between 0 and 1; X max , Y max They are the maximum values ​​of the horizontal and vertical axes of the map respectively; are the horizontal and vertical coordinates of the target point respectively; R is the distance from the starting point to the target point, and m is the concentric circle coefficient, which is generally between 1 and 2.

5. The two-layer path planning method based on fast expanding random tree and dynamic window method according to claim 1 is characterized in that: In step S4, the selection of local target points follows the principle of farthest obstacle-free. The specific rules for selecting sub-target points are as follows: Assume the extracted path point is {P k , k = 1, 2, ..., n}, connect P1P3, if P1P3 does not pass through obstacles, that is If the distance from line segment P1P3 to the obstacle is greater than the preset threshold, then continue to connect P1P4 until P1P k (k=4,5,…,n) passes through an obstacle, then P k-1 Connect with P1, delete the intermediate redundant points, and update the path; from node P k-1 Repeat the above operation until there are no redundant turning points in the path, and record these path nodes as local target points.

6. The two-layer path planning method based on fast expanding random tree and dynamic window method according to claim 1 is characterized in that: The motion trajectory between two adjacent points is considered as a straight line, that is, it moves v along the x-axis of the robot coordinate system. t Δt; simply project the distance onto the x-axis and y-axis of the world coordinate system to obtain the displacement Δx and Δy of the smart car in the world coordinate system at time t+1 relative to time t; Δx=v t Δtcos(θ t ) Δy=v t Δtsin(θ t ) Among them, θ t The larger change per unit time interval, and so on, to calculate the trajectory within a period of time, you only need to accumulate and sum the displacement increments during this period: x=x+v t Δtcos(θ t ) y=y+v t Δtsin(θ t ) i t =θ t +w t Δt Traverse the above local target points in sequence until reaching the end point; in the two-dimensional space of speed (v, w), there are infinite sets of speeds; the sampling speed is controlled within a certain range according to the limitations of the smart car itself and the environment: Smart cars are limited by their own maximum and minimum speeds: V s ={v∈[v min ,v max ],w∈[w min ,w max ]} Smart cars are limited by their own acceleration and deceleration performance: where v c , w c is the current speed of the smart car, are the maximum braking deceleration and maximum acceleration, are the maximum angular deceleration and maximum angular acceleration respectively; At maximum deceleration, speed range: Among them, dist(v, w) is the distance between the current position and the obstacle; The actual vehicle turning angle has the following kinematic constraints; Among them, r is the turning radius, R is the limit turning radius, v, ω are the speed and angular velocity of the vehicle respectively, L is the wheelbase of the vehicle, δ max is the maximum turning angle of the front wheels; Therefore, V s Rewritten as: Modified speed space: V r =V s ∩V d ∩V a 。

Citation Information

Patent Citations

  • Unmanned vehicle real-time global path planning method based on dynamic window of safe A* guide point

    CN113608531A

  • Mobile robot path planning method based on improved A-RRT

    CN114995431A