CN115079705A - Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm - Google Patents
Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm Download PDFInfo
- Publication number
- CN115079705A CN115079705A CN202210945247.5A CN202210945247A CN115079705A CN 115079705 A CN115079705 A CN 115079705A CN 202210945247 A CN202210945247 A CN 202210945247A CN 115079705 A CN115079705 A CN 115079705A
- Authority
- CN
- China
- Prior art keywords
- node
- algorithm
- path
- dwa
- improved
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 225
- 238000005457 optimization Methods 0.000 title claims abstract description 81
- 230000004927 fusion Effects 0.000 title claims abstract description 69
- 238000007689 inspection Methods 0.000 title claims abstract description 45
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000011156 evaluation Methods 0.000 claims abstract description 34
- 238000005070 sampling Methods 0.000 claims abstract description 21
- 230000035945 sensitivity Effects 0.000 claims abstract description 6
- 230000004888 barrier function Effects 0.000 claims description 38
- 238000004088 simulation Methods 0.000 claims description 30
- 230000003068 static effect Effects 0.000 claims description 23
- 230000008569 process Effects 0.000 claims description 12
- 239000002131 composite material Substances 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 6
- 230000014759 maintenance of location Effects 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000009499 grossing Methods 0.000 claims description 4
- 238000010606 normalization Methods 0.000 claims description 3
- 230000008030 elimination Effects 0.000 claims description 2
- 238000003379 elimination reaction Methods 0.000 claims description 2
- 230000007613 environmental effect Effects 0.000 claims description 2
- 238000012216 screening Methods 0.000 claims description 2
- 238000012795 verification Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 7
- 238000002474 experimental method Methods 0.000 description 5
- 230000006872 improvement Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 3
- 230000009467 reduction Effects 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000000052 comparative effect Effects 0.000 description 2
- 238000012217 deletion Methods 0.000 description 2
- 230000037430 deletion Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000001988 toxicity Effects 0.000 description 1
- 231100000419 toxicity Toxicity 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0291—Fleet control
- G05D1/0295—Fleet control by at least one leading vehicle of the fleet
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention particularly relates to a routing planning method of an inspection robot based on an improved A star fusion DWA optimization algorithm, and solves the problems that the existing A star algorithm and the existing DWA algorithm are poor in smoothness, long in planning time, multiple in inflection points, discontinuous in path curvature and incapable of realizing global optimization when used independently. The method is realized by adopting the following steps of S1: initializing parameters and a grid map; s2: expanding the child nodes by using an improved A star algorithm to form a primary global path; s3: obtaining an optimized global path by a greedy optimization method; s4: extracting local key nodes; s5: performing speed sampling and track prediction based on a kinematic model, speed constraint and a local target point, and selecting an optimal predicted track and a speed space by improving an evaluation function; s6: and extracting and outputting the optimal path. The invention provides an efficient path planning algorithm with global path optimization and real-time obstacle avoidance capability, and the path planning efficiency, the obstacle avoidance sensitivity and the safety are improved.
Description
Technical Field
The invention relates to a path planning method of a robot, in particular to a routing inspection robot path planning method based on an improved A star fusion DWA optimization algorithm.
Background
The rapid development of various intelligent robots has a deepening influence on the work and life of human beings, wherein the greenhouse technology becomes more and more intelligent and efficient, long-time manual operation is not facilitated under the greenhouse environment with high plant density, high temperature, high humidity and even toxicity, and in addition, due to the influences of insufficient labor force, cost increase and the like, intelligent equipment of the greenhouse robot type is derived and is adopted more and more. The greenhouse mobile inspection robot is used for completing autonomous inspection of the greenhouse environment, and the requirement of fine agriculture is met. The path planning is an extremely important part of the core functions of the greenhouse inspection robot, and is a hot spot and a difficult point.
In short, path planning is to plan a collision-free safe path connecting a start point and a stop point by using a certain algorithm and an evaluation method on the premise of meeting optimization conditions such as planning distance, planning time, energy consumption and smoothness.
The A-star algorithm is a breadth-first heuristic path search algorithm, although a global optimal path can be planned, the traditional A-star algorithm has the limitations of poor smoothness, long planning time, more inflection points, discontinuous path curvature and the like. Unknown obstacles often appear on the planned path, and the global path planning cannot "know" the unknown obstacles, so that safety accidents are caused, and the method is only suitable for a static known environment and cannot realize real-time obstacle avoidance of the unknown obstacles.
The DWA algorithm is a widely used local obstacle avoidance algorithm, but the original DWA algorithm can accurately 'know' unknown obstacles to realize local real-time obstacle avoidance, but has the defects of easy suffering from local optimum, incapability of realizing global optimum and the like.
Disclosure of Invention
The invention provides a routing method of an inspection robot based on an improved A star fusion DWA optimization algorithm, aiming at solving the problems that the existing A star algorithm and DWA algorithm are poor in smoothness, long in planning time, multiple in inflection points, discontinuous in path curvature and incapable of realizing global optimization when used independently.
The invention is realized by adopting the following technical scheme:
a routing planning method of an inspection robot based on an improved A star fusion DWA optimization algorithm is realized by adopting the following steps:
s1: initializing parameters and a grid map;
s2: setting a starting point and an end point of path planning, adding the starting point into an Open list as a father node, and expanding child nodes by using an improved A star algorithm to form a primary global path;
s3: performing redundant node elimination, key node retention and smoothing on the primary global path by a greedy optimization method to obtain an optimized global path;
s4: extracting local key nodes of the optimized global path;
s5: the extracted local key nodes are used as local target points of the DWA optimization algorithm to guide local path planning and real-time obstacle avoidance, speed sampling and track prediction are carried out on the basis of a kinematic model, speed constraint and the local target points, an optimal predicted track and a speed space are selected through an improved evaluation function in the DWA optimization algorithm, and the fusion of the improved A star algorithm and the DWA optimization algorithm is realized;
s6: and after the end point is reached, backtracking all nodes, extracting and outputting the optimal path, finishing the improved A-star fusion DWA optimization algorithm, and completing path planning and dynamic obstacle avoidance based on the algorithm.
Further, parameters for improving the A star fusion DWA optimization algorithm comprise the kinematic parameters of the inspection robot and the parameters of the improved DWA algorithm; wherein, the kinematic parameters of the inspection robot comprise the highest speed (m/s), the highest rotation speed (rad/s) and the highest acceleration (m/s) 2 ) Maximum rotational acceleration (rad/s) 2 ) Speed resolution (m/s), rotating speed resolution (rad/s), time resolution(s) and initial direction angle (rad) of the robot; improving DWA algorithm parametersThe system comprises an end point and local target point azimuth function coefficient, an end point and local target point distance function coefficient, an end point and known static barrier distance function coefficient, an end point and unknown dynamic barrier distance function coefficient, a current speed function coefficient, a simulation time(s) and a barrier conflict radius (m); the grid map used by the algorithm needs to be reinitialized every algorithm run period within the simulation time.
Further, step S2 is implemented by the following steps:
s2.1: setting Open list to store node information which is not accessed; setting a Close list to store the accessed node information; in the calculation process, both the Open list and the Close list adopt a minimum heap fusion two-dimensional array composite data structure to store and process two-dimensional search node information of an improved A star algorithm, so that the search efficiency can be effectively improved;
s2.2: firstly, setting a starting point and an end point of path planning, adding the starting point of the path planning into an Open list as a father node, wherein the Close list is empty;
s2.3: then, starting node searching, deleting the node from the Open list and adding the node into the Close list after the starting point is accessed, selecting the optimal node in the searching period as a next father node by utilizing an evaluation function of the improved A star algorithm, and continuously searching and updating; searching eight adjacent nodes of the current node in each search period, selecting the optimal node by utilizing an evaluation function of the improved A star algorithm, and continuously searching for a target point;
the evaluation function of the improved A star algorithm is as follows:
in the formula, S sc Is the distance from the starting point to the current node, S st Is the distance from the starting point to the end point, H (n) is a heuristic function,
for the adaptive weight coefficient, when the length of the environment map is C and the width is K,
when the algorithm starts, the current point is far away from the end point, the heuristic function estimation value is far smaller than the true value, the searching efficiency is low, the heuristic function value is increased by increasing the weight coefficient, the searching speed is accelerated, and the searching efficiency is improved; when the current point is close to the end point, in order to avoid the algorithm being unable to find the optimal path, the heuristic function estimated value should be close to the true value, the weight coefficient needs to be reduced to reduce the heuristic function estimated value, the search speed is slowed down, the search nodes are increased, although the search efficiency is relatively reduced, the overall efficiency is still improved relative to the original algorithm, and the optimal path can also be found.
S2.4: before reaching the target point, continuously searching the adjacent nodes of the starting point, and judging whether the searched new nodes are in the Open list: if the new node is not in the Open list, adding the node into the Open list, calculating a cost value F of the node by adopting an evaluation function of an improved star A algorithm, setting the node as a father node of the current search point, and adding the father node into the Close list; if the new node is in the Open list, calculating a cost value F of the node by adopting an evaluation function of an improved star A algorithm, comparing the cost value F with the cost value F of the previous node, selecting the optimal node with the minimum cost value F as a father node of the next search point, adding the node with the minimum cost value F into the Close list, continuing searching the node towards the target direction, adding the searched node into the Open list, and updating node information;
s2.5: circularly executing the steps S2.3-S2.4 to expand the child nodes until the target position is reached; when the target point is searched, adding the target point into the Close list, and ending the loop; if the Open list is empty at this time, no path node exists;
s2.6: and backtracking and extracting the optimal nodes in the Close list to form a primary global path of the improved A star algorithm.
In the original A star algorithm, detection node information is stored in a node two-dimensional array, wherein Open list stores alternative nodes to be detected in the current search field, the optimal node with the minimum F (n) value after evaluation by an evaluation function is searched, and Close list stores the searched optimal node. The Open list needs to read, delete, search and update the node information frequently, and the Close list only has no deletion operation, so that the calculation pressure is large and the time is long when processing the alternative node data.
The invention selects a minimum heap fusion two-dimensional array structure to store and process the information of the A star algorithm search nodes, which is characterized in that the A star algorithm two-dimensional search nodes are stored in the minimum heap data structure, wherein the minimum heap is a sequenced complete binary tree, the data value of any non-terminal node is not more than the values of the left child node and the right child node, and the heap top is the node element with the minimum cost value in the whole A star algorithm two-dimensional search node. The invention encapsulates the composite data structure into a priority queue as a storage container for an Open list and a Close list.
The composite data structure with the minimum heap fusion two-dimensional array can find the searching node with the minimum cost in the A star algorithm more quickly. Specifically, the time complexity of the read-in, deletion, search and update operations of the original Open list node information is reduced from O (n) to O (1), even if the time complexity when constructing a complete binary tree to search the minimum cost candidate node is increased to O (logn), the overall time complexity is reduced from O (n) to O (logn), and meanwhile, the time complexity of the read-in, search and update operations of the Close list node information is reduced from O (n) to O (1) by virtue of the composite data structure.
In summary, the advantage of the composite data structure provided by the invention is represented by the reduction of the time complexity of inserting and processing nodes in the Open list and the Close list to different degrees, and the reduction of the time complexity means the reduction of the algorithm search time and the improvement of the search efficiency.
The invention adds adaptive weight coefficients to improve the heuristic function of the traditional A star algorithm, which comprises the following steps:
the search efficiency of the A star algorithm mainly depends on a heuristic function H (n), and if the estimated cost value H (n) of the heuristic function is greater than the real cost value H of the path t (n), the number of searched nodes is small, the node evaluation time is shortened, the search efficiency is improved, and the optimal path cannot be searched; otherwise, the estimated cost value H (n) of the heuristic function is less than the path real cost value H t (n), the path search node and the node evaluation time are increased, resulting in a decrease in the path search efficiency, but the best path can be searched; if the two values are equal, the efficiency is highest.
The calculation result of the Euclidean distance adopted by the invention is generally smaller than the real cost value, so that a weight coefficient with a value larger than 1 is required to be set to keep the heuristic cost value as close as possible to the real cost value, but the weight coefficient value is required to be approximately equal to 1 when the heuristic cost value is close to the target. The present invention therefore proposes adaptive weight coefficients that gradually decrease as the algorithm runs
To improve the heuristic function.
Further, step S3 is implemented by using a step, which aims to remove redundant nodes of each original path;
s3.1: extracting the preliminary global path obtained in the step S2;
s3.2: sequentially extracting current points S by taking the starting point as the current point 0 Child node S 1 And grandchild node S 2 Coordinate (x) of 0 ,y 0 )、(x 1 ,y 1 ) And (x) 2 ,y 2 ) The range of the path node is also related to the size of the map, and the range is as follows:
wherein C is the length of the environment map; the width of the K environment map;
s3.3: calculating the vector between the coordinates of the current point and the child node, the child node and the grandchild node
And determining the angle theta between the two vectors 1 ：
S3.4: when the included angle theta 1 When the value is 0, it means that the current point, child node and grandchild node are on the same line, and the redundant node S is deleted 1 (ii) a When the included angle theta 1 If not, calculating the connection S between the current node and the grandchild node 0 S 2 Is shown in equation (5):
s3.5: judging whether one or more barrier nodes are connected with the grandchild node at the current node S 0 S 2 In the path area, all barrier nodes are traversed, and the coordinates of the barrier nodes under screening are set as (x) obc ,y obc ) The value range is
Judging whether the inequality (6) is true or not;
[x obc ≥min(x 0 ,x 2 )∧x obc ≤max(x 0 ,x 2 )∧y obc ≥min(y 0 ,y 2 )∧y obc ≤max(y 0 ,y 2 )] (6)
s3.6: setting the position of the barrier node and the connection S between the current node and the grandchild node 0 S 2 A safety distance D between, D ranges from
According to the linear equation y line ＝kx obc + b calculates the position of the barrier node and the connection S between the current node and the grandchild node 0 S 2 Vertical distance d:
d＝||(y obc -y line )cos(arctan(k))|| (7)；
s3.7: when the included angle theta 1 Vertical distance d different from 0 and having inequality (6)<D, judging the connection S between the barrier node and the grandchild node at the current node 0 S 2 In the path area, child nodes need to be reserved, and the step S3.2-S3.6 is repeatedly executed by taking the grandchild node as the current node; when the included angle theta 1 Not 0, when the inequality (6) does not hold or when the angle θ is included 1 If the distance D is not less than D, the fact that the barrier node is not connected with the grandchild node S when the distance D is not less than D is proved 0 S 2 In the path area of (2), the child node S is deleted 1 Connecting S with the grandchild node by the current node 0 S 2 Replacing the original broken line path, then the current node is unchanged, and the child node is modified into S 2 Repeating the steps S3.2-S3.6 to obtain a first-stage optimized path;
s3.8: and (3) secondary optimization is carried out to remove redundant nodes in the first-stage optimized path line segment and retain key nodes, and the specific method comprises the following steps: extracting the first-stage optimized path and rearranging the path nodes to obtain the starting point S of the first-stage optimized path 0 (x 0 ,y 0 ) For the current point, obtaining the current point and the optimized path child node G 1 (x 4 ,y 4 ) Is connected to the line segment S 0 G 1 Set of all nodes { g } 1i I |, 0,1 … n }; judging whether the barrier node is in the sub-node G of the optimized path or not according to the method of the steps S3.5-S3.7 1 (x 4 ,y 4 ) Is connected to the line segment S 0 G 1 Within the path area of (a);
s3.9: when S is 0 G 1 Upper all node g 1i And a starting point S 0 If there is no obstacle node in the connection area, S is deleted 0 G 1 Upper all node g 1i Keep the key node G 1 (ii) a Retention of Key node G 1 Then, the next step isG 1 (x 4 ,y 4 ) Is the current node, G 2 (x 5 ,y 5 ) Is a child node, line segment G 1 G 2 Set of all nodes on is { g 2i Repeating the steps S3.8-S3.9 until the child node is the target node, and completing secondary optimization to obtain an optimized global path; when S is 0 G 1 Upper all node g 1i And a starting point S 0 If the obstacle node exists in the connecting line area, the path optimization fails, and the first-stage path is re-optimized.
The greedy optimization is that local optimization is carried out on each segment of path for multiple times without considering overall optimization until each segment of path has no redundant node, and finally only the key nodes of each segment of path are reserved for local guidance of a DWA optimization algorithm in a subsequent fusion algorithm.
Further, step S5 is implemented by the following steps:
s5.1: setting unknown static and unknown dynamic obstacles on the grid map;
s5.2: based on the kinematic model and the speed constraint of the inspection robot, the inspection robot is integrated in a speed space V a (v, omega) sampling speed to simulate infinite feasible motion tracks within a certain time interval;
s5.3: the local key nodes reserved in the step S4 are used as local target points of the DWA optimization algorithm to provide local direction guidance, the track is evaluated by using an evaluation function of the DWA optimization algorithm, and the optimal track is selected from infinite feasible motion tracks generated in the step S5.2;
the evaluation function of the DWA optimization algorithm is shown as the formula (8):
P(v,w)＝σ(α 1 ·Gheading(v,w)+α 2 ·Gdist(v,w)+β 1 ·S_Kdist(v,w)+β 2 ·S_Udist(v,w)+β s ·D_Udist(v,w)+γ·velocity(v,w)) (8)
in the formula, Gheader (v, w) represents the angle difference between the end direction of the sampling track and the local target direction, Gdist (v, w) represents the distance between the end point of the sampling track and the local target point, and the prediction track is close to the local target point under the combined actionA global path; in order to further improve the safety and obstacle avoidance sensitivity of the algorithm, the minimum distance between the end point of the sampling track and the nearest obstacle in the original DWA algorithm is subdivided into three parts: s _ Kdist (v, w), S _ Udist (v, w) and D _ Udist (v, w) respectively represent the minimum distance between the tail end point of the sampling track and a known static obstacle, an unknown static obstacle and an unknown dynamic obstacle; velocity (v, w) represents the current speed; alpha is alpha 1 、α 2 、β 1 、β 2 、β S And gamma is the weight coefficient corresponding to each subfunction of the evaluation function; σ represents the normalization process; since a certain term in the evaluation function may be too prominent in scoring, each term of the evaluation function needs to be divided by the sum of the corresponding terms, i.e. normalization processing;
according to the danger degree of the known static barrier, the unknown static barrier and the unknown dynamic barrier to the inspection robot, the weight coefficient of each item corresponding to the minimum distance between the tail point of the sampling track and the three barriers meets beta 1 <β 2 <β S (ii) a Furthermore, dynamic obstacles are generally random occurrences of a short time, if beta S Is set to a constant beta 3 (0<β 3 <β 2 <β 1 <1) Then, the adaptive weight coefficient β is extracted because it occupies a larger weight most of the time, which affects the efficiency of the algorithm s Setting the conflict radius D of the inspection robot and the barrier s ，D s If the radius is larger than the turning radius R of the robot, the value of the specified conflict radius is R<D s <2R, when the distance between the robot and the dynamic obstacle is less than or equal to the turning radius R of the robot, collision is easy to occur, and beta is used for keeping safety s Must be infinite, then beta s The values can be expressed as:
the optimized DWA algorithm ensures that the inspection robot has better obstacle avoidance sensitivity under the condition of keeping the operation safety.
S5.4: reserving a speed space corresponding to the optimal track, and smoothing the speed;
s5.5: placing a speed instruction to a chassis controller of the inspection robot, and moving the inspection robot along the optimal track obtained in the step S5.3;
s5.6: and judging whether the inspection robot moves to the end point, if so, ending, and if not, repeating the steps S5.2 to S5.4.
On the basis of extracting and improving local key points of the A star algorithm to realize direction guidance, the method carries out path planning by fusing a DWA optimization algorithm and realizes real-time dynamic obstacle avoidance by combining with the laser radar to detect the surrounding real-time environment. The DWA optimization algorithm performs local path planning between every two adjacent local key points, and the optimized DWA evaluation function enables the local planning to be close to the global optimal path as much as possible, and has the capability of automatically switching to the next local target point to be guided by the next local target point after the end of the predicted track is separated from the nearest local target point by a conflict radius.
Further, the kinematic model in step S5.2 assumes that the inspection robot is in a certain time Δ t The kinematics model during the inner uniform linear motion specifically comprises the following steps:
further, in step S5.2 the velocity space set V a (v, ω) is the velocity space after the velocity constraint, because the velocity space within the forward simulation time of the inspection robot obtained according to the kinematics model is an ideal velocity space without considering self and environmental factors, the influence of the constraint condition on the velocity sampling needs to be considered in the actual situation, the velocity space is constrained within a certain range, and the velocity sampling can be constrained as follows:
a. constrained by the robot body to obtain a velocity space set V b ：
V b ＝{(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]} (11)
In the formula, v min ,v max Minimum and maximum linear velocity, ω min ,ω max The minimum and maximum values of the angular velocity;
b. constrained by the safety distance, obtaining a velocity space set V d ：
In the formula, Dist (v, ω) represents the minimum distance between the predicted track end and the obstacle,
represents the maximum line, angular deceleration;
c. constrained by the performance of the motor, a velocity space set V is obtained c ：
In the formula, v e ,ω e Is the current linear and angular velocity of the robot,
the maximum linear and angular acceleration is obtained,
maximum linear, angular deceleration;
the final feasible speed space set is the intersection V of the three a ：
V a ＝V b ∩V d ∩V c (14)。
The invention has the following beneficial effects:
(1) the invention provides a routing planning method of an inspection robot based on an improved A star fusion DWA optimization algorithm, and solves the problems that the existing A star algorithm and the existing DWA algorithm are poor in smoothness, long in planning time, multiple in inflection points, discontinuous in path curvature and incapable of realizing global optimization when being used independently.
(2) For the improvement of the traditional A-star algorithm, firstly, the invention adopts a composite data structure to process the nodes, thereby reducing the time complexity of the algorithm and reducing the path planning time; then, adding a self-adaptive weight coefficient to a heuristic function of the evaluation function of the traditional A-star algorithm, and improving the path search efficiency; and finally, a greedy optimization strategy is provided for eliminating redundant nodes and extracting key nodes, so that the path length is shortened, and the path planning efficiency and the path smoothness are improved.
(3) The sub-functions of the original DWA algorithm track sampling evaluation function are optimized, the global optimal path of the A star algorithm is improved, the barrier distance ion function is subdivided, and the local path searching efficiency and the barrier avoidance sensitivity are improved.
(4) The invention provides a high-efficiency path planning algorithm with global path optimization and real-time obstacle avoidance capacity, which realizes local direction guidance by improving local key target points extracted from the A star algorithm and realizes dynamic obstacle avoidance by fusing the DWA optimization algorithm, thereby overcoming the respective limitations of the two algorithms, improving the path planning efficiency, the obstacle avoidance sensitivity and the safety, and simultaneously improving the working efficiency and the safety reliability of the greenhouse patrol robot in the greenhouse environment.
(5) A Matlab-based simulation comparison experiment verifies the effectiveness and superiority of the improved A star algorithm and the fusion path planning algorithm, and the improved A star algorithm and the fusion path planning algorithm are applied to an independently constructed greenhouse inspection test trolley.
Drawings
FIG. 1 is a diagram of an example of a greedy optimization strategy improving an original A-star algorithm path in the present invention;
FIG. 2 is a schematic flow chart of a fusion path planning algorithm for fusing an improved star A with an optimized DWA algorithm in the present invention;
fig. 3a), fig. 3b), fig. 3c), fig. 3d) are graphs of global path planning comparison simulation results of the ant colony algorithm based on safety consideration, the ant colony algorithm accelerating convergence, the traditional a-star algorithm and the improved a-star algorithm of the present invention in a 30 × 30 grid environment, respectively;
fig. 4a), fig. 4b), fig. 4c), fig. 4d) are graphs of global path planning comparison simulation results of the ant colony algorithm based on safety consideration, the ant colony algorithm accelerating convergence, the traditional a-star algorithm and the improved a-star algorithm of the present invention under a 50 x 50 grid environment, respectively;
fig. 5a), 5b), 5c), 6d) are graphs of comparative simulation results of the original DWA algorithm, the ant colony fusion DWA algorithm based on security consideration, the conventional a-star fusion DWA algorithm, and the improved a-star fusion DWA optimization algorithm of the present invention in a 30 × 30 grid environment, respectively;
fig. 6a), 6b), 6c), 6d) are graphs of comparative simulation results of the original DWA algorithm, the ant colony fusion DWA algorithm based on security consideration, the conventional a-star fusion DWA algorithm, and the improved a-star fusion DWA optimization algorithm of the present invention under a grid environment of 50 × 50, respectively;
fig. 7a), fig. 7b), fig. 7c), fig. 7d) are real-time line velocity diagrams of the original DWA algorithm, the ant colony fusion DWA algorithm based on security consideration, the conventional a-star fusion DWA algorithm, and the improved a-star fusion DWA optimization algorithm of the present invention in a 30 × 30 grid environment in comparison with simulation experiments, respectively;
8a), 8b), 8c), 8d) are real-time linear velocity diagrams of the original DWA algorithm, the ant colony fusion DWA algorithm based on safety consideration, the traditional A-star fusion DWA algorithm and the improved A-star fusion DWA optimization algorithm of the invention in comparison simulation experiments under 50 x 50 grid environment respectively;
FIG. 9 is a simulated greenhouse real-world real-scene verification site with an improved A star algorithm and a fused path planning algorithm and a corresponding two-dimensional grid map constructed in the invention;
FIG. 10 is a global optimal path diagram of the starting and ending point positions of the algorithm entity verification and the improved A star algorithm planning provided by the invention;
FIG. 11 shows a specific process diagram of the improved A-star algorithm for object verification according to the present invention;
FIG. 12 shows a specific process diagram for improving the A-star fusion DWA optimization algorithm for object verification.
Detailed Description
In order to make the above objects, contents and advantages of the present invention more comprehensible, an algorithm improvement example, a simulation experiment and a real-object scene verification are described in detail below with reference to the accompanying drawings.
The technical solutions of the present invention will be described clearly and completely with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only some embodiments of the present invention, not all embodiments.
Example 1
As shown in fig. 1, a greedy optimization strategy in the present invention improves a traditional a-star algorithm path example, a grid map is composed of square cell grids with side length of 1, and the main steps are as follows:
step 1: and removing redundant nodes of each original path.
Step 1.1: the initial global path of the improved A star algorithm after the optimization efficiency is extracted, namely the black dotted line in figure 1, and the line has an original path node (T) 1 ,T 2 ,T 3 ,G 1 ,G 2 ,T 4 ,T 5 ) Sequentially extracting a current point S and a child node T by taking the starting point as the current point 1 And grandchild node T 2 The coordinates of (1) are (0.5 ), (0.5, 1.5) and (1.5, 2.5), respectively.
Step 1.2: computing vectors
And calculating the included angle theta of the two vectors 1 ：
Step 1.3: to obtain the included angle theta 1 If not 0, the node connection ST is first calculated 2 The equation of the straight line of (1):
step 1.4: then, whether all the obstacles are one or more in ST is judged 2 In the path area, all barrier nodes are traversed, and the coordinates of the barrier nodes which are being screened are set as (x) obc ,y obc ) Wherein
Determining whether the following inequality holds:
[x obc ≥0.5&x obc ≤1.5&y obc ≥0.5&y obc ≤2.5] (18)
step 1.5: because the side length of the grid unit is 1m, the position of an obstacle node and a node connecting line ST need to be set 2 The safety distance D between the two is 2m, so the system also needs to be based on the linear equation y line ＝2x obc -0.5 calculating the obstacle node position and node connection line ST 2 Vertical distance d:
step 1.6, when the included angle theta 1 If the inequality (18) is not 0, the vertical distance d is set<D, judging the connection line ST of the nodes of the obstacle 2 In the path area of (2), the node T needs to be reserved 1 And with T 2 (1.5, 2.5) is the current node, T 3 (2.5, 3.5) is a child node, G 1 (3.5, 4.5) the target point is a grandchild node, and the steps are repeated until the final target point is the grandchild node;
step 1.7:if the inequality (18) is not established or if the inequality (18) is established and the vertical distance D is more than or equal to D, the obstacle node is proved to be not on the node connecting line ST 2 In the path area of (2), the redundant node T is deleted 1 Connecting the lines with nodes ST 2 Replacing the original broken-line path ST 1 T 2 Then, the current node is not changed, and the child node is modified into T 2 (1.5, 2.5), the grandchild node is T 3 (2.5, 3.5) repeating the steps until the final target point is the grandchild node.
Step 1.8: finally, the blue line segment in the example of the figure 1 is obtained to be the first-stage optimized path, redundant turning points are reduced, and path smoothness is improved.
Step 2: secondary optimization is carried out to remove redundant nodes of the optimized path line segment and key nodes are reserved;
step 2.1: extracting the first stage optimized path and rearranging the path nodes, taking S (0.5 ) as the current point, G 1 (3.5, 4.5) is a child node, line segment SG 1 Set of all nodes above is { g 1i And i is 0,1 … n, and the method for proving whether the obstacle node is in the node connecting path area in the first-stage optimization judges that:
step 2.2, if all nodes g on the line segment 1i No obstacle node exists in the area of the connection with the starting point S, and the SG can be deleted 1 Upper all node g 1i Retention of key node G 1 ；
Step 2.3, if the barrier node exists, path optimization fails and path is re-optimized, otherwise, G is continued 1 (3.5, 4.5) as current node, G 2 (4.5, 5.5) is a child node, line segment G 1 G 2 Set of all nodes on is { g 2i And i is 0,1 … n, and repeating the steps until the child node is the target node, thereby completing the secondary optimization.
Key node G is extracted after two phased path optimizations in this figure 1 (3.5，4.5)、G 2 And (4.5, 5.5) completing greedy optimization of the preliminary global path.
Example 2
The improved A star algorithm plans a global optimal path comprising a start point, a stop point and a local target point, and can not avoid unknown obstacles; although the DWA algorithm can realize local obstacle avoidance, the DWA algorithm always takes the final target as the direction guide, and the 'detour' behavior is easy to appear. Therefore, on the basis of extracting and improving local key points of the A star algorithm to realize direction guidance, the DWA optimization algorithm is fused to carry out path planning, and the dynamic obstacle avoidance is realized by combining with the laser radar to detect the surrounding real-time environment.
The DWA optimization algorithm performs local path planning between every two adjacent local key points, the optimized DWA evaluation function enables the local planning to be close to the global optimal path as much as possible, and the DWA evaluation function has the capability of automatically switching to the next local target point to be guided by the next local target point after the end of the predicted track is separated from the nearest local target point by a conflict radius, and the specific flow is shown in figure 2.
The steps S2-S4 in the present invention can be summarized as follows: the method for improving the evaluation function of the A-star algorithm and optimizing the node array data structure is adopted to expand the nodes, redundant nodes are removed through a greedy optimization strategy to obtain a global optimal path, and necessary key nodes are extracted.
Example 3
In order to verify the effectiveness of the improved A star algorithm and the fusion path planning algorithm, Matlab is used for carrying out simulation experiments. The simulation environment is a grid map for simulating a complex greenhouse environment, wherein a white grid, a black grid, a gray grid and a yellow grid respectively represent an idle channel, a known static obstacle, an unknown static obstacle and an unknown dynamic obstacle, and a blue delta at the lower left corner and a green delta at the upper right corner respectively represent a starting point and an end point of path planning.
Aiming at the complexity of the working environment of the greenhouse inspection robot, two grid maps with different sizes of 30X 30 and 50X 50 are designed, wherein the barrier rate is 0.130 and 0.150 in sequence, and simulation comparison experiments of the improved A star algorithm and the improved A star fusion DWA optimization algorithm are respectively carried out.
The improved A-star algorithm in the invention respectively performs two sets of simulation comparison experiments in two grid environments with an ant colony algorithm based on safety consideration, an ant colony algorithm accelerating convergence and a traditional A-star algorithm, and the results are shown in the attached figures 3 and 4.
Fig. 3 and 4 are graphs of simulation results in a 30 × 30 and 50 × 50 grid environment, respectively, where (a), (b), (c), and (d) respectively represent ant colony algorithm based on safety consideration, ant colony algorithm for accelerated convergence, traditional a-star algorithm, and path planning simulation result of the improved a-star algorithm of the present invention, and a red line is a global path planned by each algorithm, and it can be seen from the graphs that the above four algorithms can plan a complete global path, but the path performances are different, and a simulation experiment parameter pair is shown in table 1, for example.
TABLE 1 comparison of parameters for global path planning experiments under two kinds of grid maps for each algorithm
As can be seen from table 1, the improved a-star algorithm not only eliminates redundant nodes through a greedy optimization strategy and shortens the path length, but also improves the path search efficiency by adopting methods of improving an evaluation function and optimizing a node array. In addition, because turning points are greatly reduced, the turning angle of the path turning point of the improved A star algorithm is also reduced, and the smoothness of the path is further improved. Although the path of the improved A star algorithm is closer to the obstacle, the robot does not move according to the global optimal path completely, but moves according to the path planned by the fused path planning algorithm.
Based on the global optimal path and the local target points of the improved A-star algorithm, the simulation comparison experiment of the fused path planning algorithm is carried out, and the path planning and real-time obstacle avoidance capability of the fused path planning algorithm is verified. The map environment still uses the two grid maps, the start point and the stop point of the simulation of the fusion algorithm are unchanged, 3, 5 unknown static obstacles and 1 dynamic obstacle are respectively arranged, wherein the running track of the dynamic obstacle is a black dotted line track and traverses the fusion algorithmAnd (5) planning a calculation method track by combining paths. The invention sets the collision radius D of the barrier s The safe distance between the path and each type of obstacle is kept at 0.6m, and the parameters of the robot kinematics and the DWA optimization algorithm in the fusion algorithm simulation are shown in tables 2 and 3.
Table 2: kinematic parameter table of robot
Table 3: optimizing DWA algorithm parameters
The comparison simulation results of the fusion path planning algorithm under the two grid environments are shown in the attached figures 5 and 6.
Fig. 5 and fig. 6 (a), (b), (c), and (d) show simulation results of the original DWA algorithm, the ant colony fusion DWA algorithm based on security consideration, the conventional a-star fusion DWA algorithm, and the herein improved a-star fusion DWA optimization algorithm in two grid environments, respectively. The solid blue lines in the figure are the robot running tracks marked by each algorithm, the dashed black lines are the corresponding global guiding paths, and the red lines on the dashed black lines are the local target points.
Because the four comparison algorithms comprise the DWA algorithm, the DWA algorithm and the fusion path planning algorithm have dynamic obstacle avoidance capability, the original DWA algorithm and the fusion path planning algorithm can plan a complete path and realize dynamic obstacle avoidance. Firstly, compared with the ant colony fusion DWA algorithm based on safety consideration and the traditional A-star fusion DWA algorithm, after the A-star fusion DWA optimization algorithm is improved to remove redundant nodes, the number of local target points is greatly reduced compared with the traditional A-star algorithm and the ant colony algorithm based on safety consideration, so that the improvement of the fusion algorithm of the invention does not need to follow the guidance of unnecessary local target points, the planning time is greatly reduced, and the planning efficiency is improved. Secondly, the simulation result shows that the total turning angle of the fusion algorithm provided by the invention is obviously smaller, and the track is more suitable for the movement of the greenhouse inspection robot in a complex greenhouse environment.
Fig. 7 and 8 are line speed diagrams of path planning simulation of corresponding algorithms in fig. 5 and 6, which show that the speed fluctuation of the fusion algorithm provided by the invention is weak and is maintained at a high speed for a long time, while a little speed fluctuation is caused by real-time obstacle avoidance, and the number of control nodes of the fusion algorithm is small compared with other algorithms, so that the planning efficiency of the fusion algorithm is high and the operation speed is stable, and the method is suitable for a greenhouse inspection robot to work in a greenhouse environment, and the experimental parameter pairs of simulation paths of each algorithm are shown in table 4.
Table 4: comparison of path planning simulation experiment parameters in original DWA and three fusion algorithms under two grid map environments
It can be known from table 4 that under two size grid maps, the path length and the planning time of the improved a-star fusion DWA optimization algorithm are optimized compared with those of the other three algorithms, because the fusion algorithm of the invention strictly follows the global optimal path of the improved a-star algorithm and the local target points after the redundant nodes are removed to search the track, and the optimal predicted track is selected through the improved evaluation function of the DWA algorithm. On the premise of ensuring the safety and smoothness of the path, the method improves the path planning efficiency of the A star fusion DWA optimization algorithm, and has higher practicability.
Example 4
In order to verify that the improved algorithm provided by the invention has effectiveness and safe reliability in a real environment, and due to the particularity of a greenhouse environment, the invention simplifies the environment condition of real scene verification, the vegetation of the greenhouse is replaced by the offspring of the gray carton, the red carton (the carton with darker color) is unknown vegetation, and pedestrians as workers frequently appearing in the greenhouse environment are dynamic obstacles. The improved A star algorithm and the improved A star fusion DWA optimization algorithm are applied to the independently constructed greenhouse inspection test trolley, and the real scene verification is carried out in the simulated greenhouse environment scene constructed in the laboratory.
The test trolley, the real object real scene verification site and the constructed two-dimensional grid map are shown in the attached figure 9, wherein the black area of the grid map is a static obstacle, and the white area of the grid map is a channel.
The set starting point and the ending point are set to start the real-object scene verification of the improved star A algorithm, and the experimental starting point and the global path planned by the improved star A algorithm are shown in the attached figure 10.
In the attached figure 10, a light blue area around the obstacle is an obstacle expansion area for preventing the trolley from colliding, and a purple area near the trolley model is a local cost map and is used for sensing the surrounding environment in real time to help the trolley to dynamically avoid the obstacle. Obviously, the global optimal path of the improved A star algorithm can perfectly avoid static obstacles, basically has no redundant inflection points, and is beneficial to the movement of a trolley.
In the attached figure 11, the specific process that the trolley strictly follows the global optimal path of the improved star A algorithm is shown, autonomous navigation and avoidance of static known obstacles are realized, and the trolley moves smoothly in the experimental process. Since the improved A star algorithm of the invention cannot avoid unknown obstacles, the invention is only suitable for known environments.
The experimental environment for improving the A-star fusion DWA optimization algorithm is consistent with the improved A-star algorithm, the compiled fusion path planning algorithm navigation function package is started, the same starting and stopping points are set, unknown static and dynamic barriers are added, the path planning and real-time barrier avoiding capability of the fusion algorithm are verified, and the result is shown in figure 12.
In the attached figure 12, a green line is a global path of an improved A star algorithm, a red line in front of a dolly model in a red circle is an optimal predicted track of the DWA optimization algorithm after speed sampling, and a dolly strictly moves according to the track guided by the global optimal path. When the laser radar scans unknown obstacles, the unknown obstacles are displayed in a local cost map, then a predicted track is obtained through a DWA optimization algorithm, a speed command is put down, the trolley starts to avoid unknown static obstacles 1 (red boxes, namely, deep-color cartons) as shown in figure 12, returns to the guidance of a global path again after finishing, walks into an experimental environment as unknown dynamic obstacles and just passes through the global path at the moment, as shown in figures 12 III and IV, the trolley perfectly avoids the pedestrians after passing through the fusion algorithm of the invention, then senses unknown static obstacles 2 and starts to avoid as shown in figure 12 fifthly, finally three unknown obstacles are successfully avoided, the movement is smooth without pause, the unknown static obstacles reach a terminal point along the guidance of the global optimal path, and the feasibility of the trolley in the simulated simplified greenhouse environment is verified. The integrated path planning algorithm provided by the invention not only achieves global optimization, but also can avoid obstacles in real time, and improves the working efficiency and safety of the greenhouse inspection robot.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (7)
1. A routing planning method of a patrol robot based on an improved A star fusion DWA optimization algorithm is characterized by comprising the following steps: the method is realized by adopting the following steps:
s1: initializing parameters and a grid map;
s2: setting a starting point and an end point of path planning, adding the starting point into an Open list as a father node, and expanding child nodes by using an improved A star algorithm to form a primary global path;
s3: performing redundant node elimination, key node retention and smoothing on the primary global path by a greedy optimization method to obtain an optimized global path;
s4: extracting local key nodes of the optimized global path;
s5: the extracted local key nodes are used as local target points of the DWA optimization algorithm to guide local path planning and real-time obstacle avoidance, speed sampling and track prediction are carried out on the basis of a kinematic model, speed constraint and the local target points, an optimal predicted track and a speed space are selected through an improved evaluation function in the DWA optimization algorithm, and the fusion of the improved A star algorithm and the DWA optimization algorithm is realized;
s6: and after the terminal is reached, backtracking all nodes, extracting and outputting an optimal path, finishing the improved A star fusion DWA optimization algorithm, and finishing path planning and dynamic obstacle avoidance based on the algorithm.
2. The routing inspection robot path planning method based on the improved A star fusion DWA optimization algorithm according to claim 1, characterized in that: parameters for improving the A star fusion DWA optimization algorithm comprise the kinematic parameters of the inspection robot and the parameters of the improved DWA algorithm; the inspection robot kinematic parameters comprise a highest speed, a highest rotation speed, a highest acceleration, a highest rotation acceleration, a speed resolution, a rotation speed resolution, a time resolution and an initial direction angle of the robot; the improved DWA algorithm parameters comprise an azimuth function coefficient of an end point and a local target point, a distance function coefficient of the end point and the local target point, a distance function coefficient of the end point and a known static obstacle, a distance function coefficient of the end point and an unknown dynamic obstacle, a current speed function coefficient, simulation time and an obstacle conflict radius; the grid map used by the algorithm needs to be reinitialized every algorithm run period within the simulation time.
3. The routing inspection robot path planning method based on the improved A star fusion DWA optimization algorithm according to claim 1, characterized in that: step S2 is implemented by the following steps:
s2.1: setting Open list to store node information which is not accessed; setting a Close list to store the accessed node information; in the calculation process, both the Open list and the Close list adopt a composite data structure of a minimum heap fusion two-dimensional array to store and process two-dimensional search node information of an improved A star algorithm;
s2.2: firstly, setting a starting point and an end point of path planning, adding the starting point of the path planning into an Open list as a father node, wherein the Close list is empty;
s2.3: then, starting node searching, deleting the node from the Open list and adding the node into the Close list after the starting point is accessed, selecting the optimal node in the searching period as a next father node by utilizing an evaluation function of the improved A star algorithm, and continuously searching and updating; searching a plurality of adjacent nodes of the current node in each search period, selecting the optimal node by utilizing an evaluation function of the improved A star algorithm, and continuously searching for a target point;
the evaluation function of the improved A star algorithm is as follows:
in the formula, S sc Is the distance from the starting point to the current node, S st Is the distance from the starting point to the end point, H (n) is a heuristic function,
for the adaptive weight coefficient, when the length of the environment map is C and the width is K,
s2.4: before reaching the target point, continuously searching the adjacent nodes of the starting point, and judging whether the searched new nodes are in the Open list: if the new node is not in the Open list, adding the node into the Open list, calculating a cost value F of the node by adopting an evaluation function of an improved star A algorithm, setting the node as a parent node of the current search point, and adding the parent node into the Close list; if the new node is in the Open list, calculating a cost value F of the node by adopting an evaluation function of an improved star A algorithm, comparing the cost value F with the cost value F of the previous node, selecting the optimal node with the minimum cost value F as a father node of the next search point, adding the node with the minimum cost value F into the Close list, continuing searching the node towards the target direction, adding the searched node into the Open list, and updating node information;
s2.5: circularly executing the steps S2.3-S2.4 to expand the child nodes until the target position is reached; when the target point is searched, adding the target point into the Close list, and ending the loop;
s2.6: and backtracking and extracting the optimal nodes in the Close list to form a preliminary global path.
4. The routing inspection robot path planning method based on the improved A star fusion DWA optimization algorithm according to claim 1, characterized in that: the step S3 is implemented by the following steps:
s3.1: extracting the preliminary global path obtained in the step S2;
s3.2: sequentially extracting current points S by taking the starting point as the current point 0 Child node S 1 And grandchild node S 2 Coordinate (x) of 0 ,y 0 )、(x 1 ,y 1 ) And (x) 2 ,y 2 ) The value range is as follows:
wherein C is the length of the environment map; the width of the K environment map;
s3.3: calculating the vector between the coordinates of the current point and the child node, the child node and the grandchild node
And determining the included angle theta between the two vectors 1 ：
S3.4: when the included angle theta 1 When 0, the description is currentThe point, child node and grandchild node are on the same straight line, and the redundant node S is deleted 1 (ii) a When the included angle theta 1 If not, calculating the connection S between the current node and the grandchild node 0 S 2 Is shown in equation (5):
s3.5: judging whether one or more barrier nodes are connected with the grandchild node at the current node S 0 S 2 In the path area, all barrier nodes are traversed, and the coordinates of the barrier nodes under screening are set as (x) obc ,y obc ) The value range is
Judging whether inequality (6) is true or not;
[x obc ≥min(x 0 ，x 2 )∧x obc ≤max(x 0 ，x 2 )∧y obc ≥min(y 0 ，y 2 )∧y obc ≤max(y 0 ，y 2 )](6)
s3.6: setting the position of the barrier node and the connection S between the current node and the grandchild node 0 S 2 A safety distance D between, D ranges from
According to the linear equation y line ＝kx obc + b calculates the position of the node of the obstacle and the connection S between the current node and the grandchild node 0 S 2 Vertical distance d:
d＝||(y obc -y line )cos(arctan(k))|| (7)；
s3.7: traversing all barrier nodes when the included angle theta 1 If the distance D is less than D, the connection S between the barrier node and the grandchild node at the current node can be judged if the distance D is not less than D and the inequality (6) is true 0 S 2 In the path area, the child nodes need to be reserved and grandchild nodes need to be usedRepeating the steps S3.2-S3.6 for the current node; when the included angle theta 1 Not 0, when the inequality (6) does not hold or when the angle θ is included 1 If the distance D is not less than D, the fact that the barrier node is not connected with the grandchild node S when the distance D is not less than D is proved 0 S 2 In the path area of (2), the child node S is deleted 1 Connecting S with the grandchild node by the current node 0 S 2 Replacing the original broken line path, then the current node is unchanged, and the child node is modified into S 2 Repeating the steps S3.2-S3.6 to obtain a first-stage optimized path;
s3.8: extracting the first-stage optimized path and rearranging the path nodes to use the starting point S of the first-stage optimized path 0 (x 0 ，y 0 ) For the current point, obtaining the current point and the optimized path child node G 1 (x 4 ，y 4 ) Is connected to the line segment S 0 G 1 Set of all nodes above { g 1i I |, 0,1 … n }; judging whether the barrier node is in the sub-node G of the optimized path or not according to the method of the steps S3.5-S3.7 1 (x 4 ，y 4 ) Is connected to the line segment S 0 G 1 Within the path area of (a);
s3.9: when S is 0 G 1 Upper all node g 1i And a starting point S 0 If there is no obstacle node in the connection area, S is deleted 0 G 1 Upper all node g 1i Retention of key node G 1 (ii) a Retention of Key node G 1 Then, with G 1 (x 4 ，y 4 ) Is the current node, G 2 (x 5 ，y 5 ) Is a child node, line segment G 1 G 2 Set of all nodes on is { g 2i Repeating the steps S3.8-S3.9 until the child node is the target node, and completing secondary optimization to obtain an optimized global path; when S is 0 G 1 Upper all node g 1i And a starting point S 0 If the obstacle node exists in the connecting line area, the path optimization fails, and the first-stage path is re-optimized.
5. The routing inspection robot path planning method based on the improved A star fusion DWA optimization algorithm according to claim 1, characterized in that: step S5 is implemented by the following steps:
s5.1: setting unknown static and unknown dynamic obstacles on the grid map;
s5.2: based on the kinematic model and the speed constraint of the inspection robot, the inspection robot is integrated in a speed space V a (v, omega) sampling speed to simulate infinite feasible motion tracks within a certain time interval;
s5.3: taking the local key nodes reserved in the step S4 as local target points of the DWA optimization algorithm, providing local direction guidance, evaluating tracks by using an evaluation function of the DWA optimization algorithm, and selecting an optimal track from infinite feasible motion tracks generated in the step S5.2;
the evaluation function of the DWA optimization algorithm is shown as the formula (8):
P(v，w)＝σ(α 1 ·Gheading(v，w)+α 2 ·Gdist(v，w)+β 1 ·S_Kdist(v，w)+β 2 ·S_Udist(v，w)+β S ·D_Udist(v，w)+γ·velocity(v，w)) (8)
in the formula, Gheader (v, w) represents the angle difference between the tail end direction of the sampling track and the local target direction, Gdist (v, w) represents the distance between the tail end point of the sampling track and the local target point, and the prediction track is close to the global path under the combined action; in order to further improve the safety and obstacle avoidance sensitivity of the algorithm, the minimum distance between the end point of the sampling track and the nearest obstacle in the original DWA algorithm is subdivided into three parts: s _ Kdist (v, w), S _ Udist (v, w) and D _ Udist (v, w) respectively represent the minimum distance between the tail end point of the sampling track and a known static obstacle, an unknown static obstacle and an unknown dynamic obstacle; velocity (v, w) represents the current speed; alpha is alpha 1 、α 2 、β 1 、β 2 、β S And gamma is the weight coefficient corresponding to each subfunction of the evaluation function; σ represents the normalization process;
according to the danger degree of the known static barrier, the unknown static barrier and the unknown dynamic barrier to the inspection robot, the minimum distance between the tail point of the sampling track and the three barriers corresponds to each itemSatisfies the weight coefficient of beta 1 ＜β 2 ＜β S (ii) a Proposing an adaptive weight coefficient beta s Setting the conflict radius D of the inspection robot and the barrier s ，D s If the radius is larger than the turning radius R of the robot, the value of the specified conflict radius is R < D s < 2R, when the distance between the robot and the dynamic obstacle is less than or equal to the turning radius R of the robot, collision is easy to occur, and beta is used for maintaining safety s Must be infinite, then beta s The values are expressed as:
s5.4: reserving a speed space corresponding to the optimal track, and smoothing the speed;
s5.5: placing a speed instruction to a chassis controller of the inspection robot, and moving the inspection robot along the optimal track obtained in the step S5.3;
s5.6: and judging whether the inspection robot moves to the end point, if so, ending, and if not, repeating the steps S5.2 to S5.4.
6. The routing inspection robot path planning method based on the improved A-star fusion DWA optimization algorithm according to claim 5, characterized in that: in step S5.2, the kinematics model assumes that the inspection robot is in a certain period of time delta t The kinematics model during the inner uniform linear motion specifically comprises the following steps:
7. the routing inspection robot path planning method based on the improved A-star fusion DWA optimization algorithm according to claim 5, characterized in that: velocity space set V in step S5.2 a (v, ω) is the velocity space after velocity constraint, since the inspection robot is obtained from the kinematic modelThe speed space in the forward simulation time is an ideal speed space without considering self and environmental factors, in practical situations, the influence of constraint conditions on speed sampling needs to be considered, the speed space is constrained within a certain range, and the speed sampling is constrained by the following constraints:
a. constrained by the robot body to obtain a velocity space set V b ：
V b ＝{(v，ω)|v∈[v min ，v max ]，ω∈[ω min ，ω max ]} (11)
In the formula, v min ，v max Minimum and maximum linear velocity, ω min ，ω max The minimum and maximum values of the angular velocity;
b. constrained by the safety distance, obtaining a velocity space set V d ：
In the formula, Dist (v, ω) represents the minimum distance between the predicted track end and the obstacle,
represents the maximum line, angular deceleration;
c. constrained by the performance of the motor, a velocity space set V is obtained c ：
In the formula, v e ，ω e Is the current line and angular velocity of the robot,
the maximum linear and angular acceleration is obtained,
is the maximum line and angle minusSpeed;
the final feasible speed space set is the intersection V of the three a ：
V a ＝V b ∩V d ∩V c (14)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210945247.5A CN115079705A (en) | 2022-08-08 | 2022-08-08 | Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210945247.5A CN115079705A (en) | 2022-08-08 | 2022-08-08 | Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115079705A true CN115079705A (en) | 2022-09-20 |
Family
ID=83245235
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210945247.5A Pending CN115079705A (en) | 2022-08-08 | 2022-08-08 | Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115079705A (en) |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115562272A (en) * | 2022-10-10 | 2023-01-03 | 北京信息科技大学 | Planning method and device for robot traveling path |
| CN115562265A (en) * | 2022-09-29 | 2023-01-03 | 哈尔滨理工大学 | Mobile robot path planning method based on improved A-x algorithm |
| CN115657687A (en) * | 2022-12-21 | 2023-01-31 | 广东技术师范大学 | Path optimization method and system for mobile robot |
| CN115826586A (en) * | 2023-02-14 | 2023-03-21 | 泉州装备制造研究所 | Path planning method and system fusing global algorithm and local algorithm |
| CN116560382A (en) * | 2023-07-11 | 2023-08-08 | 安徽大学 | Mobile robot path planning method based on hybrid intelligent algorithm |
| CN116804879A (en) * | 2023-07-25 | 2023-09-26 | 东北大学秦皇岛分校 | Robot path planning framework method for improving dung beetle algorithm and fusing DWA algorithm |
| CN116858254A (en) * | 2023-09-01 | 2023-10-10 | 青岛中德智能技术研究院 | Single steering wheel AGV path planning method |
| CN116976535A (en) * | 2023-06-27 | 2023-10-31 | 上海师范大学 | Path planning algorithm based on fusion of few obstacle sides and steering cost |
| CN117111617A (en) * | 2023-10-23 | 2023-11-24 | 山东优宝特智能机器人有限公司 | Robot path planning method and system considering collision uncertainty of perception dead zone |
| CN117215317A (en) * | 2023-11-09 | 2023-12-12 | 烟台哈尔滨工程大学研究院 | Unmanned ship local path planning method, equipment and storage medium |
| CN117451057A (en) * | 2023-12-25 | 2024-01-26 | 长春理工大学 | Unmanned aerial vehicle three-dimensional path planning method, equipment and medium based on improved A-algorithm |
| CN117494919A (en) * | 2023-11-13 | 2024-02-02 | 广州力生机器人技术有限公司 | Path planning method and device based on multi-robot collaborative stacking operation |
| CN117631670A (en) * | 2023-12-01 | 2024-03-01 | 陕西明泰电子科技发展有限公司 | Robot obstacle avoidance path optimization method and system in complex environment |
| CN117746524A (en) * | 2023-12-08 | 2024-03-22 | 中北大学 | Security inspection system and method based on SLAM and crowd abnormal behavior identification |
| CN117746524B (en) * | 2023-12-08 | 2024-10-29 | 中北大学 | Security inspection system and method based on SLAM and crowd abnormal behavior identification |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190094866A1 (en) * | 2017-09-22 | 2019-03-28 | Locus Robotics Corporation | Dynamic window approach using optimal reciprocal collision avoidance cost-critic |
| CN112378408A (en) * | 2020-11-26 | 2021-02-19 | 重庆大学 | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot |
| CN114428499A (en) * | 2021-12-16 | 2022-05-03 | 哈尔滨理工大学 | Astar and DWA algorithm fused mobile trolley path planning method |
| CN114675649A (en) * | 2022-03-28 | 2022-06-28 | 南京工业大学 | Indoor mobile robot path planning method fusing improved A and DWA algorithm |
-
2022
- 2022-08-08 CN CN202210945247.5A patent/CN115079705A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190094866A1 (en) * | 2017-09-22 | 2019-03-28 | Locus Robotics Corporation | Dynamic window approach using optimal reciprocal collision avoidance cost-critic |
| CN112378408A (en) * | 2020-11-26 | 2021-02-19 | 重庆大学 | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot |
| CN114428499A (en) * | 2021-12-16 | 2022-05-03 | 哈尔滨理工大学 | Astar and DWA algorithm fused mobile trolley path planning method |
| CN114675649A (en) * | 2022-03-28 | 2022-06-28 | 南京工业大学 | Indoor mobile robot path planning method fusing improved A and DWA algorithm |
Non-Patent Citations (1)
| Title |
|---|
| XIAOXIAO LI 等: "Path Planning Based on Combinaion of Improved A-STAR Algorithm and DWA Algorithm", 《2020 2ND INTERNATIONAL CONFERENCE ON ARTIFICIAL INTELLIGENCE AND ADVANCED MANUFACTURE (AIAM)》, 31 December 2020 (2020-12-31), pages 99 - 103 * |
Cited By (21)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115562265B (en) * | 2022-09-29 | 2024-01-05 | 哈尔滨理工大学 | Mobile robot path planning method based on improved A-algorithm |
| CN115562265A (en) * | 2022-09-29 | 2023-01-03 | 哈尔滨理工大学 | Mobile robot path planning method based on improved A-x algorithm |
| CN115562272A (en) * | 2022-10-10 | 2023-01-03 | 北京信息科技大学 | Planning method and device for robot traveling path |
| CN115657687A (en) * | 2022-12-21 | 2023-01-31 | 广东技术师范大学 | Path optimization method and system for mobile robot |
| CN115826586A (en) * | 2023-02-14 | 2023-03-21 | 泉州装备制造研究所 | Path planning method and system fusing global algorithm and local algorithm |
| CN116976535B (en) * | 2023-06-27 | 2024-05-17 | 上海师范大学 | Path planning method based on fusion of few obstacle sides and steering cost |
| CN116976535A (en) * | 2023-06-27 | 2023-10-31 | 上海师范大学 | Path planning algorithm based on fusion of few obstacle sides and steering cost |
| CN116560382A (en) * | 2023-07-11 | 2023-08-08 | 安徽大学 | Mobile robot path planning method based on hybrid intelligent algorithm |
| CN116804879A (en) * | 2023-07-25 | 2023-09-26 | 东北大学秦皇岛分校 | Robot path planning framework method for improving dung beetle algorithm and fusing DWA algorithm |
| CN116804879B (en) * | 2023-07-25 | 2024-07-05 | 东北大学秦皇岛分校 | Robot path planning framework method for improving dung beetle algorithm and fusing DWA algorithm |
| CN116858254A (en) * | 2023-09-01 | 2023-10-10 | 青岛中德智能技术研究院 | Single steering wheel AGV path planning method |
| CN117111617A (en) * | 2023-10-23 | 2023-11-24 | 山东优宝特智能机器人有限公司 | Robot path planning method and system considering collision uncertainty of perception dead zone |
| CN117215317A (en) * | 2023-11-09 | 2023-12-12 | 烟台哈尔滨工程大学研究院 | Unmanned ship local path planning method, equipment and storage medium |
| CN117215317B (en) * | 2023-11-09 | 2024-02-09 | 烟台哈尔滨工程大学研究院 | Unmanned ship local path planning method, equipment and storage medium |
| CN117494919A (en) * | 2023-11-13 | 2024-02-02 | 广州力生机器人技术有限公司 | Path planning method and device based on multi-robot collaborative stacking operation |
| CN117494919B (en) * | 2023-11-13 | 2024-04-19 | 广州力生机器人技术有限公司 | Path planning method and device based on multi-robot collaborative stacking operation |
| CN117631670A (en) * | 2023-12-01 | 2024-03-01 | 陕西明泰电子科技发展有限公司 | Robot obstacle avoidance path optimization method and system in complex environment |
| CN117746524A (en) * | 2023-12-08 | 2024-03-22 | 中北大学 | Security inspection system and method based on SLAM and crowd abnormal behavior identification |
| CN117746524B (en) * | 2023-12-08 | 2024-10-29 | 中北大学 | Security inspection system and method based on SLAM and crowd abnormal behavior identification |
| CN117451057A (en) * | 2023-12-25 | 2024-01-26 | 长春理工大学 | Unmanned aerial vehicle three-dimensional path planning method, equipment and medium based on improved A-algorithm |
| CN117451057B (en) * | 2023-12-25 | 2024-03-12 | 长春理工大学 | Unmanned aerial vehicle three-dimensional path planning method, equipment and medium based on improved A-algorithm |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115079705A (en) | Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm | |
| Sudhakara et al. | Trajectory planning of a mobile robot using enhanced A-star algorithm | |
| CN111780777A (en) | Unmanned vehicle route planning method based on improved A-star algorithm and deep reinforcement learning | |
| CN113359718B (en) | Method and equipment for fusing global path planning and local path planning of mobile robot | |
| CN110887484A (en) | Mobile robot path planning method based on improved genetic algorithm and storage medium | |
| CN112229419B (en) | Dynamic path planning navigation method and system | |
| CN114675649A (en) | Indoor mobile robot path planning method fusing improved A and DWA algorithm | |
| CN115309161B (en) | Mobile robot path planning method, electronic equipment and storage medium | |
| CN115562290A (en) | Robot path planning method based on A-star penalty control optimization algorithm | |
| CN114428499A (en) | Astar and DWA algorithm fused mobile trolley path planning method | |
| CN114625150A (en) | Rapid ant colony unmanned ship dynamic obstacle avoidance method based on danger index and distance function | |
| CN116360457A (en) | Path planning method based on self-adaptive grid and improved A-DWA fusion algorithm | |
| Song et al. | A new hybrid method in global dynamic path planning of mobile robot | |
| Yang et al. | Path planning and collision avoidance methods for distributed multi-robot systems in complex dynamic environments | |
| CN115933637A (en) | Path planning method and device for substation equipment inspection robot and storage medium | |
| Westbrook et al. | Anytime kinodynamic motion planning using region-guided search | |
| Chen et al. | A novel navigation system for an autonomous mobile robot in an uncertain environment | |
| Huang et al. | Search-Based Path Planning Algorithm for Autonomous Parking: Multi-Heuristic Hybrid A | |
| Yan et al. | A study of improved global path planning algorithm for parking robot based on Ros | |
| Bigaj et al. | A memetic algorithm based procedure for a global path planning of a movement constrained mobile robot | |
| Tang et al. | Towards coordinated multi-robot exploration under bandwidth-constrained conditions | |
| Jiang et al. | Path planning method for mobile robot based on a hybrid algorithm | |
| Shi et al. | Local path planning of unmanned vehicles based on improved RRT algorithm | |
| Lu et al. | Research on path planning of UAV based on Hybrid A* algorithm | |
| Yu et al. | Safe and Efficient Mobile Robot Path Planning in Open World Environments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20240918 Address after: Unit 3101, Unit 2, Commercial and Residential Building, No. 12, North District, Jinfeng Dijing Garden, Xiaodian District, Taiyuan City, Shanxi Province 030006 Applicant after: Shanxi Jinghongrui Electric Power Technology Co.,Ltd. Country or region after: China Applicant after: Taiyuan University of Technology Address before: No. 16, Shanghu Avenue, Chengdong Town, Haian City, Nantong City, Jiangsu Province, 226600 Applicant before: Haixiang (Jiangsu) Technology Co.,Ltd. Country or region before: China Applicant before: Taiyuan University of Technology |