CN114839968A - Unmanned surface vehicle path planning method - Google Patents
Unmanned surface vehicle path planning method Download PDFInfo
- Publication number
- CN114839968A CN114839968A CN202210349398.4A CN202210349398A CN114839968A CN 114839968 A CN114839968 A CN 114839968A CN 202210349398 A CN202210349398 A CN 202210349398A CN 114839968 A CN114839968 A CN 114839968A
- Authority
- CN
- China
- Prior art keywords
- path
- node
- unmanned ship
- nodes
- algorithm
- 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
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 71
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 19
- 238000011156 evaluation Methods 0.000 claims abstract description 17
- 238000005457 optimization Methods 0.000 claims abstract description 15
- 238000009499 grossing Methods 0.000 claims abstract description 14
- 238000012545 processing Methods 0.000 claims abstract description 9
- 238000005070 sampling Methods 0.000 claims description 10
- 230000004888 barrier function Effects 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000003491 array Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000007547 defect Effects 0.000 abstract description 3
- 238000004088 simulation Methods 0.000 abstract description 2
- 238000013461 design Methods 0.000 description 8
- 238000011160 research Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- KDYFGRWQOYBRFD-UHFFFAOYSA-N succinic acid Chemical compound OC(=O)CCC(O)=O KDYFGRWQOYBRFD-UHFFFAOYSA-N 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000003313 weakening effect Effects 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/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a method for planning a path of an unmanned surface vehicle, which is based on a dynamic window method and an optimized Linear Programming Algorithm (LPA) and is used for planning the path of the unmanned surface vehicle in an environment with moving obstacles. Obtaining a global remote sensing image of a water surface environment through an aircraft aerial image, classifying and binarizing the aerial image, converting the image into a grid map, carrying out global path planning by utilizing an LPA algorithm of an optimized distance heuristic function based on the environment global grid map to obtain an initial path, and carrying out interpolation smoothing processing on the path; and constructing an evaluation function of global optimal path based on a dynamic window method, and performing dynamic planning on the local path to achieve the capacity of local dynamic planning on the premise of the global optimal path. Simulation results show that the method can obtain a smooth path which is better than that of the traditional heuristic A-x algorithm, and can realize good obstacle avoidance performance in a moving obstacle environment; and the method is fused with the structure of an LPA (low power automation) global programming algorithm, so that the defect that a dynamic window method is easy to fall into local optimization is overcome.
Description
Technical Field
The invention mainly belongs to the field of water surface path planning and obstacle avoidance of multi-reef distributed type water autonomous navigation and ship intensive navigation, and relates to a path planning method based on a global grid map and capable of realizing local real-time obstacle avoidance.
Background
Autonomous navigation and obstacle avoidance of the unmanned surface vehicle are one of the hot research subjects in the field of unmanned vehicles. How to conduct safe and efficient autonomous navigation on water surface full of reefs or other obstacles and with dense navigation of ships is a more important research direction in the technical research content of autonomous ships currently developing. The intelligent degree of the unmanned system is higher and higher nowadays, the requirement on the adaptive capacity of autonomous navigation in a complex environment is also enhanced, and when the unmanned ship navigates in a dynamic environment, how to plan an optimal path is also a great test on the navigation efficiency and safety of the unmanned ship. Therefore, a path planning method is needed to be designed, which can rapidly perform dynamic path planning in a complex obstacle environment, control cost and ensure unmanned ship safety and college driving.
The path planning method is developed to the present, and many traditional autonomous intelligent planning algorithms are used for solving the path planning problem of the unmanned ship, such as a Voronoi diagram method based on sampling and a random tree fast searching method; dijkstra, A algorithm and D algorithm based on nodes; an artificial potential field method and a dynamic window method based on a model; neural network, genetic algorithm, bee colony algorithm based on biological heuristic method; the dynamic A algorithm, the LPA algorithm, the D Lite algorithm and the like are improved based on the A algorithm and the intelligent search method.
LPA algorithm (Lifelong Planning a algorithm) was proposed by Koenig and Likhachev in 2004, and the algorithm was improved over the traditional a algorithm mainly with reference to incremental search of artificial intelligence. To deal with the shortest path problem from a given starting point to a given destination point in a dynamic environment.
LPA algorithms have been widely applied to unmanned autonomous path planning, but due to the continuous problem of path curvature, the motion variables at the turns are not conducive to control; and the global planning method does not have dynamic obstacle avoidance capability. The A-x algorithm based on the particle swarm optimization algorithm enhances the local obstacle avoidance capability, but the calculation complexity is high, and the curvature of the planned path cannot be guaranteed to change continuously.
The dynamic window method has good obstacle avoidance capability by planning the path in real time on line, and is suitable for autonomous navigation of the unmanned equipment in a dynamic environment. The global planning and local dynamic planning can be well fused by combining the LPA algorithm of the global planning, the overall optimization planning capability of the unmanned aerial vehicle is enhanced, the cost is saved, and meanwhile, dynamic local obstacle avoidance can be realized.
Disclosure of Invention
The invention aims to provide a water surface unmanned ship path planning method, which optimizes a starting point cost function and an enlightening function of an LPA (low power amplifier) algorithm aiming at the problem of insufficient local planning and dynamic obstacle avoidance capabilities of the original LPA algorithm, enhances the path global optimization capability of the algorithm while ensuring the planning effect of the original algorithm, reduces the complexity, saves the path optimization cost, and simultaneously performs 2 times of interpolation smoothing on the path to obtain an optimal path so as to perfect a fusion algorithm of global and local dynamic planning, thereby effectively solving the path planning problem of an unmanned ship under a local dynamic environment.
The purpose of the invention is realized as follows: step 1: and obtaining a global remote sensing image of the water surface environment through the aerial image of the aircraft, thereby classifying and binarizing the aerial image and converting the image into a two-dimensional grid map.
101: acquiring an aerial image, and performing image cutting and binarization processing on the aerial image;
102: classifying the processed binary image to obtain a two-dimensional grid matrix D n×n And drawing a two-dimensional grid map according to the matrix elements:
step 2: based on the water surface environment global grid map, calibrating the starting point and the arrival point of the unmanned ship navigation, and performing global path planning by using an LPA algorithm of an optimized distance heuristic function to obtain an initial unmanned ship navigation global path.
201: marking a starting point and an arrival point of unmanned ship navigation on a global grid map obtained after the binary segmentation processing of the aerial image, and initializing two nodes to traverse and store arrays A star D1 and A star D2; wherein D1 is a traversed node and D2 is an undiversad node;
202: starting to initialize rhs (n)(s) (0) of a starting point and rhs (n)(s) (n) (infinity) of other nodes which are not traversed by taking a starting point node of the grid map as a center, starting to perform heuristic traversal search to 8 neighborhood grids, designing a new cost function based on an original LPA (linear programming algorithm), comparing cost functions F (n) of adjacent nodes, and selecting a point with the minimum value of F (n) as a next expansion node succ (n).
The cost function designed based on the LPA algorithm is
F(n)＝s(n)+h(n)+G,s(n)＝min(s t (n),rhs(n))
Wherein S (N) is a starting point cost function, which represents the path cost from the starting point S to the current traversal node N, and mu is a diagonal parameter; rhs (n) is an adjacency coefficient in the LPA algorithm, and the minimum value of the distance from S to S and the St value of the adjacent node itself is taken as the rhs value for all adjacent nodes of S.
c (n', n) is a cost function between two nodes; h (N) is a heuristic function, which is a cost estimation function from the current node N to the target point T. Based on the heuristic function h (n) of the traditional LPA algorithm, the invention designs a new heuristic function h a (n) the calculation formula is as follows:
wherein T is an arrival point, N is a current node, and S is an initial node. The subscripts are the horizontal and vertical coordinates of the nodes within the grid map, respectively. G is an environment variable; and when the nodes are continuously traversed, comparing the values of the cost function c (n ', n) between the nodes with the value of the initial point cost function s (n'), and selecting the node as the next traversal node according to the size of the cost function.
203: traversing to a next node, taking the next node as a current node, continuously selecting traversal adjacent nodes according to the cost function, calculating cost values F (n) of all the next nodes, and selecting a node with the minimum cost function value as a subsequent node;
204: updating the value of the cost function s (n) at the starting point in the cost function f (n)(s) (n) + h (n) + G and the heuristic function h (n), and returning to the step 202 for iteration, so that the unmanned boat path iteration moves to the subsequent node until the current node of the unmanned boat iterates to a preset termination point T.
205: and (3) carrying out 2 times of interpolation smoothing treatment on the calculated optimal raster map path in the LPA algorithm while the nodes are updated in an iterative manner, so as to simplify the unmanned ship path and reduce loss.
Deleting redundant nodes of the path, eliminating unnecessary broken line to optimize the path cost:
206-210: arranging each node in the optimal path obtained before smoothing according to the extending direction of the path, and marking as (a) 1 ,a 2 ....a n ),a n ＝T；
Let k be 0 as the initial value, connect a in turn k ,a k+1 ,k∈N + (ii) a Judging whether the connecting line passes through the barrier or not, so as to ensure that the final moving route of the unmanned ship after the smoothing treatment can reasonably avoid the barrier in the fixed grid map; initializing k, connecting a in turn k ,a j ,k,j∈N + J > k; continuing to perform the same operation on the path nodes in sequence, and stopping when the path between the node k and the node j passes through the barrier;
connecting the two points by using the point k and the point j as end points according to the state before stopping, replacing the initial path from the point k to the point j, and sequentially connecting a by changing k to k +1 k ,a j ,k,j∈N + J > k; and repeating the operation until k +1 is T, so that a complete smooth path is obtained.
And step 3: and (3) carrying out local replanning obstacle avoidance under the condition of existence of a moving obstacle by using a fixed path and dynamic window method based on an LPA algorithm.
301: and based on the planned navigation path of the unmanned ship, performing secondary path planning and obstacle avoidance in a local map with moving obstacles by using a dynamic window method. In the algorithm of the dynamic window method, firstly, the motion track of the unmanned ship needs to be simulated, and the known unmanned ship motion model is set as follows:
wherein u is the course linear velocity and the angular velocity of the unmanned ship motion, x is the state equation of the unmanned ship motion, and the simple motion model can be used for simulating the simple motion of the unmanned ship on the water surface.
302: and carrying out speed sampling on the unmanned ship motion model. Infinite groups of speed pairs (v, w) exist in the speed two-dimensional space, and can be limited by the unmanned ship and water surface environment factors, so that the sampling speed range is restricted. Simulating a speed sampling model by using a dynamic window method;
V h ＝{(v t ,w t )|v t ∈(v min ,v max )∧w t ∈(w min ,w max ) The maximum and minimum speed constraint model of the unmanned ship is defined;
303: designing an optimal path evaluation function by a dynamic window method;
in the velocity space, an optimization function needs to be designed to select an optimal trajectory. The basic criteria for designing the optimization function are: in a local path planning window of the unmanned ship, the unmanned ship is required to avoid moving obstacles as much as possible, and a path with the shortest route to a target node is selected.
The evaluation function designed by the invention is as follows:
H(v t ,w t )＝f(ω,λ,ν)(ωW(v t ,w t )+λL(v t ,w t )+νV(v t ,w t ))
wherein f is a linear smoothing function, omega, lambda and nu are weighting coefficients of three evaluation functions, W (v) t ,w t ) Is a heading azimuth evaluation function, representsThe azimuth deviation between the ideal track end point direction and the end point at the current speed; l (v) t ,w t ) The minimum one-dimensional Minkowski distance between the current time speed and the obstacle on the corresponding track; v (V) t ,w t ) The function is evaluated for the current time speed value.
Compared with the prior art, the invention has the beneficial effects that: (1) the invention combines the LPA global path planning algorithm with strong optimizing capability and the dynamic window algorithm capable of carrying out local dynamic planning and real-time obstacle avoidance, complements the defects of the LPA global path planning algorithm and the dynamic window algorithm, and carries out smooth processing on the planned path, thereby not only ensuring the reliability of global path optimization, but also realizing unmanned ship dynamic obstacle avoidance under the environment of moving obstacles.
(2) The invention improves the node cost function in the conventional LPA algorithm and the heuristic function defined based on the simple Manhattan (Manhattan) distance or Euclidean (Euclidean) distance, designs a new cost function F (n) ═ s (n) + h (n) + G, combines the initial point cost function of the LPA algorithm with an evaluation function close to the actual cost, and optimizes the heuristic function into a form closer to the actual cost value, thereby optimizing the path planning optimizing capability, weakening the problem of local optimization of the dynamic window method, improving the searching capability of the algorithm and reducing the running cost of unmanned equipment.
The invention introduces linear smoothing function f (omega, lambda, v), designs new dynamic window method optimal path evaluation function H (v) t ,w t ) And in the local path planning window of the unmanned ship, the unmanned ship avoids moving obstacles as much as possible, and the path with the shortest route to the target node is selected to obtain the optimal track.
Drawings
FIG. 1 is a drawing of the present invention: a flow chart of a dynamic path planning method of the unmanned surface vehicle based on a dynamic window method and an optimized LPA algorithm;
fig. 2 is a schematic diagram of an optimized LPA calculation flow based on a conventional LPA algorithm;
FIG. 3 is a completed grid map drawn based on aerial images;
fig. 4 and 5 show the real-time path planning and local dynamic obstacle avoidance process and planning results of the algorithm.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
The steps of the invention are as follows with reference to the attached drawings:
step 1: and obtaining a global remote sensing image of the water surface environment through the aerial image of the aircraft, so as to classify and binarize the aerial image and convert the image into a two-dimensional grid map.
Comprises the following steps: firstly, acquiring an aerial image, and performing image cutting and binarization processing on the aerial image; classifying the processed binary images, cutting the binary images into grids with equal size at equal intervals, taking binary color blocks (namely 0,1) according to the proportion of black and white blocks in each grid, and obtaining a two-dimensional grid matrix D n×n Setting the environment obstacle part as a black grid, setting the water surface passable area as a white grid, and drawing a two-dimensional grid map according to the matrix elements:
step 2: based on the water surface environment global grid map, calibrating the starting point and the arrival point of the unmanned ship navigation, and performing global path planning by using an LPA algorithm of an optimized distance heuristic function to obtain an initial unmanned ship navigation global path.
The specific process is as follows:
201: marking a starting point and an arrival point of unmanned ship navigation on a global grid map obtained after the binary segmentation processing of the aerial image, and initializing two nodes to traverse and store arrays A star D1 and A star D2; wherein D1 is a traversed node and D2 is an undiversad node;
202: with the starting point node of the grid map as the center, rhs (n)(s) (n) (0) of the starting point is initialized, and rhs (n)(s) (n) (infinity) of other nodes not traversed starts heuristic traversal search to the 8-neighborhood grid. Designing a new cost function based on the original LPA algorithm, comparing cost functions F (n) of adjacent nodes, and selecting a point with the minimum value of F (n) as a next expansion node succ (n). Wherein the adjacency coefficient rhs (n) value and the environmental change update amount G value of the initial point are set to 0.
F (n) includes s (n) and h (n), wherein:
F(n)＝s(n)+h a (n)+G
wherein: s (N) is a starting point cost function, represents the path cost from the starting point S to the current traversal node N, and mu is a diagonal parameter; rhs (n) is an adjacency coefficient in the LPA algorithm, and the minimum value of the distance from S to S and the St value of the adjacent node itself is taken as the rhs value for all adjacent nodes of S. c (n', n) is a cost function between two nodes; h (N) is a heuristic function, a cost estimation function from the current node N to the target point T, and the heuristic function h (N) designed based on the traditional LPA algorithm and the actual cost value condition.
Because the heuristic function of the traditional LPA algorithm is defined based on a simple Manhattan (Manhattan) distance or an Euclidean (Euclidean) distance, the operand of the Euclidean distance is large, and the 8-neighborhood grid template of the current node is used for searching when peripheral nodes are traversed each time, the traditional heuristic function optimization method has a certain difference from the control mode of the unmanned ship, so that the two heuristic function designs are not optimal choices. The LPA algorithm search efficiency is determined by the heuristic function, and the original heuristic function cannot be well close to the actual cost value among nodes, the current node and the initial point of the path planning and the arrival point, so that the invention designs a new heuristic function for improving the path planning goodness and the algorithm search efficiency:
in the heuristic function, T is an arrival point, N is a current node, and S is an initial node. The subscripts are the horizontal and vertical coordinates of the nodes within the grid map, respectively. G is an environment variable.
The detailed optimization method comprises the following steps: aiming at the problem that the heuristic function of the traditional LPA algorithm is defined based on a simple Manhattan distance or an Euclidean distance, the generated operation amount is large, and the deviation from an actual cost value is large due to the fact that an 8-neighborhood grid template of a current node is used for searching, and a class of heuristic functions which are more similar to the actual cost value is designed by combining a one-dimensional Minkowski (Minkowski) distance and an Euclidean distance; designing a heuristic function which is more similar to the actual cost value; the formula carries out weighted summation on the transverse movement cost and the longitudinal movement cost, and designs a variable weight value related to a starting point, so that the farther the current node is away from the end point, the larger the cost function value is, the problem of local optimum of a dynamic window method is weakened, and the searching capability of the algorithm is improved.
And when the nodes are continuously traversed, comparing the values of the cost function c (n ', n) between the nodes with the value of the initial point cost function s (n'), and selecting the node as the next traversal node according to the size of the cost function.
203: when the current node traverses to the next node, the node is covered as the current node, the traversal adjacent node is continuously selected according to the cost function, the cost values F (n) of all the next nodes are calculated, and the node with the minimum cost function value is selected as the subsequent node.
204: updating the value of the cost function s (n) at the starting point in the cost function f (n)(s) (n) + h (n) + G and the heuristic function h (n), and returning to the step 202 for iteration, so that the unmanned boat path iteration moves to the subsequent node until the current node of the unmanned boat iterates to a preset termination point T.
205: and (3) carrying out 2 times of interpolation smoothing treatment on the calculated optimal raster map path in the LPA algorithm while the nodes are updated in an iterative manner, so as to simplify the unmanned ship path and reduce loss.
And deleting redundant nodes of the path, and eliminating unnecessary broken line optimization path cost:
206: arranging each node in the optimal path obtained before smoothing according to the extending direction of the path, and marking as (a) 1 ,a 2 ....a n ),a n ＝T；
207: let k be 0 as the initial value, connect a in turn k ,a k+1 ,k∈N + (ii) a Judging whether the connecting line passes through the barrier or not, so as to ensure that the final moving route of the unmanned ship after the smoothing treatment can reasonably avoid the barrier in the fixed grid map;
208: initializing k, connecting a in turn k ,a j ,k,j∈N + J > k; continuing to perform the same operation on the path nodes in sequence, and stopping when the path between the node k and the node j passes through the barrier;
209: connecting the two points by taking the point k and the point j as end points according to the state before stopping and replacing the initial path from the point k to the point j;
210: let k equal to k +1, connect a in turn k ,a j ,k,j∈N + J > k; and repeating the operation until k +1 is T, so that a complete smooth path is obtained.
And after obtaining a smooth path calculated by an LPA algorithm based on an improved heuristic function, carrying out local path planning and dynamic obstacle avoidance fusion algorithm design based on a dynamic window method.
And step 3: and (3) carrying out local re-planning and obstacle avoidance under the condition of existence of a moving obstacle by using a fixed path and dynamic window method based on an LPA (linear programming algorithm).
301: and based on the planned navigation path of the unmanned ship, performing secondary path planning and obstacle avoidance in a local map with moving obstacles by using a dynamic window method. In the algorithm of the dynamic window method, firstly, the motion track of the unmanned ship needs to be simulated, and the known unmanned ship motion model is set as follows:
wherein u is the course linear velocity and the angular velocity of the unmanned ship motion, x is the state equation of the unmanned ship motion, and the simple motion model can be used for simulating the simple motion of the unmanned ship on the water surface.
302: and carrying out speed sampling on the unmanned ship motion model. Infinite groups of speed pairs (v, w) exist in the speed two-dimensional space, and can be limited by the unmanned ship and water surface environment factors, so that the sampling speed range is restricted. Simulating a speed sampling model by using a dynamic window method:
V h ＝{(v t ,w t )|v t ∈(v min ,v max )∧w t ∈(w min ,w max )}
the model is a maximum and minimum speed constraint model of the unmanned ship;
and the model is an unmanned ship braking distance constraint model. Wherein L (v) t ,w t )＝min(|x t -o t |,|y t -o t |) is the closest one-dimensional Minkowski distance to the obstacle on the unmanned ship track at the corresponding moment.
303: designing an optimal path evaluation function by a dynamic window method;
the evaluation function designed by the invention is as follows:
H(v t ,w t )＝f(ω,λ,ν)(ωW(v t ,w t )+λL(v t ,w t )+νV(v t ,w t ))
L(v t ,w t )＝min(|x t -o t |,|y t -o t |)
wherein f is a linear smoothing function, omega, lambda and nu are weighting coefficients of three evaluation functions, W (v) t ,w t ) The heading azimuth angle evaluation function represents the azimuth angle deviation between the ideal track terminal point direction and the terminal point at the current speed; l (v) t ,w t ) The minimum one-dimensional Minkowski distance between the current time speed and the obstacle on the corresponding track; v (V) t ,w t ) The function is evaluated for the current time speed value.
In a speed space, the designed optimizing function can obtain an optimal track, and the basic design criteria are as follows: in a local path planning window of the unmanned ship, the unmanned ship is required to avoid moving obstacles as much as possible, and a path with the shortest route to a target node is selected. And carrying out real-time dynamic obstacle avoidance on the unmanned ship based on the speed constraint model and the dynamic window evaluation function implementation algorithm to obtain a final optimal path.
In summary, the invention provides an unmanned ship path planning method based on a Dynamic Windowing Algorithm (DWA) and an optimized LPA algorithm under the environment with moving obstacles. Obtaining a global remote sensing image of a water surface environment through an aerial image of an aircraft, classifying and binarizing the aerial image, and converting the image into a grid map; and constructing an evaluation function of global optimal path based on a dynamic window method, and performing dynamic planning on the local path to achieve the capacity of local dynamic planning on the premise of the global optimal path. Simulation results show that the method can obtain a smooth path which is better than that of the traditional heuristic A-x algorithm, and can realize good obstacle avoidance performance in a moving obstacle environment; and the method is fused with the structure of an LPA (low power automation) global programming algorithm, so that the defect that a dynamic window method is easy to fall into local optimization is overcome.
Claims (5)
1. A method for planning the path of an unmanned surface vehicle is characterized by comprising the following steps:
step 1: obtaining a global remote sensing image of the water surface environment through an aerial image of the aircraft, so as to classify and binarize the aerial image and convert the image into a two-dimensional grid map;
step 2: calibrating a starting point and an arrival point of unmanned ship navigation based on a water surface environment global grid map, and performing global path planning by using an LPA algorithm of an optimized distance heuristic function to obtain an initial unmanned ship navigation global path;
and step 3: and (3) carrying out local re-planning and obstacle avoidance under the condition of existence of a moving obstacle by using a fixed path and dynamic window method based on an LPA (linear programming algorithm).
2. The method for planning the path of the unmanned surface vehicle according to claim 1, wherein the step 1 specifically comprises:
101: acquiring an aerial image, and performing image cutting and binarization processing on the aerial image;
102: classifying the processed binary image to obtain a two-dimensional grid matrix D n×n And drawing a two-dimensional grid map according to the matrix elements:
3. the method for planning the path of the unmanned surface vehicle according to claim 1, wherein the step 2 specifically comprises:
201: calibrating a starting point and an arrival point of unmanned ship navigation on a global grid map obtained after the binary segmentation processing of the aerial photography image, and initializing two nodes to traverse and store arrays A star D1 and A star D2; wherein D1 is a traversed node and D2 is an undiversad node;
202: initializing rhs (n) (s (n) 0 of a starting point and rhs (n)(s) (n)) infinity of other nodes which are not traversed by taking a starting point node of the grid map as a center, and starting to perform heuristic traversal search on the 8-neighborhood grid; comparing cost functions F (n) of adjacent nodes, selecting a point with the minimum value of F (n) as a next expansion node, and designing the cost functions based on an LPA algorithm;
F(n)＝s(n)+h(n)+G
wherein: s (N) is a starting point cost function, represents the path cost from the starting point S to the current traversal node N, and mu is a diagonal parameter; rhs (n) an adjacency coefficient in the LPA algorithm, wherein for all adjacent nodes of S, the distance from the adjacent nodes to S plus the minimum value of the St values of the adjacent nodes are used as rhs values; c (n', n) is a cost function between two nodes; h (N) is a heuristic function, namely a cost estimation function from the current node N to the target point T, and G is an environment variable; when the nodes are continuously traversed, selecting the nodes as next traversal nodes according to the cost function;
203: traversing to a next node, taking the next node as a current node, continuing to select traversing adjacent nodes according to a cost function, calculating cost values F (n) of all the next nodes, and selecting a node with the minimum cost as a subsequent node;
204: updating the value of the cost function s (n) of the starting point in the cost function F (n)(s) (n) + h (n) + G) and the heuristic function h (n), returning to the step 202 for iteration, so that the unmanned boat path iteration moves to the subsequent node until the current node of the unmanned boat iterates to a preset termination point T;
205: while the nodes are updated in an iterative manner, the calculated optimal raster map path is subjected to 2 times of interpolation smoothing processing in an LPA algorithm so as to simplify the unmanned ship path and reduce loss;
206: and deleting redundant nodes of the path, and eliminating unnecessary broken line optimization path cost:
arranging each node in the optimal path obtained before smoothing according to the extending direction of the path, and marking as (a) 1 ,a 2 ....a n ),a n ＝T；
207: let k be 0 as the initial value, connect a in turn k ,a k+1 ,k∈N + (ii) a Judging whether the connecting line passes through the barrier or not, so that the smooth final moving route of the unmanned ship can reasonably avoid the barrier in the fixed grid map;
208: initializing k, connecting a in turn k ,a j ,k,j∈N + J > k; continuing to perform the same operation on the path nodes in sequence, and stopping when the path between the node k and the node j passes through the barrier;
209: connecting the two points by taking the point k and the point j as end points according to the state before stopping and replacing the initial path from the point k to the point j;
210: let k equal to k +1, connect a in turn k ,a j ,k,j∈N + J > k; and repeating the operation until k +1 is T, so that a complete smooth path is obtained.
4. The method for planning the path of the unmanned surface vehicle according to claim 1, wherein the step 3 specifically comprises:
301: performing secondary path planning and obstacle avoidance in a local map with moving obstacles by using a dynamic window method based on the planned navigation path of the unmanned ship; in the algorithm of the dynamic window method, firstly, the motion track of the unmanned ship needs to be simulated, and the known unmanned ship motion model is set as follows:
initializing the motion model yields:
wherein u is the course linear velocity and angular velocity of the unmanned ship motion, x is the state equation of the unmanned ship motion, and the simple motion model can be used for simulating the simple motion of the unmanned ship on the water surface;
302: carrying out speed sampling on the unmanned ship motion model; infinite groups of speed pairs (v, w) exist in a speed two-dimensional space, and can be limited by the unmanned ship and water surface environment factors, so that the sampling speed range is restricted; simulating a speed sampling model by using a dynamic window method: v h ＝{(v t ,w t )|v t ∈(v min ,v max )∧w t ∈(w min ,w max ) The maximum and minimum speed constraint model of the unmanned ship is defined;
a braking distance constraint model for the unmanned ship; wherein L (v) t ,w t )＝min(x t -o t |,|y t -o t I) is the nearest one-dimensional Minkowski distance between the unmanned ship track and the obstacle at the corresponding moment;
303: designing an optimal path evaluation function by a dynamic window method;
optimal path evaluation function:
H(v t ,w t )＝f(ω,λ,ν)(ωW(v t ,w t )+λL(v t ,w t )+νV(v t ,w t ))
wherein: f is a linear smoothing function, omega, lambda and nu are the weighting coefficients of three evaluation functions, W (v) t ,w t ) The heading azimuth angle evaluation function represents the azimuth angle deviation between the ideal track terminal point direction and the terminal point at the current speed; l (v) t ,w t ) The minimum one-dimensional Minkowski distance between the current time speed and the obstacle on the corresponding track; v (V) t ,w t ) The function is evaluated for the current time speed value.
5. The method as claimed in claim 3, wherein in step 202, a new heuristic function h is designed based on the heuristic function h (n) of the conventional LPA algorithm a (n) the calculation formula is as follows:
wherein: t is an arrival point, N is a current node, and S is an initial node; subscripts are horizontal and vertical coordinates of the nodes in a grid map, the horizontal and vertical movement costs are subjected to weighted summation by a formula, and a variable weight value related to a starting point is designed, so that the farther the current node is away from a terminal point, the larger the cost function value is, the problem of local optimization of a dynamic window method is weakened, and the searching capability of the algorithm is improved.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210349398.4A CN114839968A (en) | 2022-04-01 | 2022-04-01 | Unmanned surface vehicle path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210349398.4A CN114839968A (en) | 2022-04-01 | 2022-04-01 | Unmanned surface vehicle path planning method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114839968A true CN114839968A (en) | 2022-08-02 |
Family
ID=82563242
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210349398.4A Pending CN114839968A (en) | 2022-04-01 | 2022-04-01 | Unmanned surface vehicle path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114839968A (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116205464A (en) * | 2023-03-21 | 2023-06-02 | 哈尔滨工程大学 | Water surface multi-unmanned-ship task allocation method based on expansion consistency beam algorithm under multi-obstacle environment |
| CN116578098A (en) * | 2023-06-25 | 2023-08-11 | 苏州优世达智能科技有限公司 | Unmanned ship control method and system for self-adaptive learning |
| CN116839594A (en) * | 2023-08-29 | 2023-10-03 | 成都蓉奥科技有限公司 | Submarine global track planning method and device based on optimized bidirectional A-Star algorithm |
| CN116954212A (en) * | 2023-04-25 | 2023-10-27 | 广东工业大学 | Improved D X Lite unmanned ship path planning method facing complex environment |
-
2022
- 2022-04-01 CN CN202210349398.4A patent/CN114839968A/en active Pending
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116205464A (en) * | 2023-03-21 | 2023-06-02 | 哈尔滨工程大学 | Water surface multi-unmanned-ship task allocation method based on expansion consistency beam algorithm under multi-obstacle environment |
| CN116205464B (en) * | 2023-03-21 | 2023-11-24 | 哈尔滨工程大学 | Water surface multi-unmanned-ship task allocation method based on expansion consistency beam algorithm under multi-obstacle environment |
| CN116954212A (en) * | 2023-04-25 | 2023-10-27 | 广东工业大学 | Improved D X Lite unmanned ship path planning method facing complex environment |
| CN116578098A (en) * | 2023-06-25 | 2023-08-11 | 苏州优世达智能科技有限公司 | Unmanned ship control method and system for self-adaptive learning |
| CN116578098B (en) * | 2023-06-25 | 2024-03-19 | 苏州优世达智能科技有限公司 | Unmanned ship control method and system for self-adaptive learning |
| CN116839594A (en) * | 2023-08-29 | 2023-10-03 | 成都蓉奥科技有限公司 | Submarine global track planning method and device based on optimized bidirectional A-Star algorithm |
| CN116839594B (en) * | 2023-08-29 | 2023-11-24 | 成都蓉奥科技有限公司 | Submarine global track planning method and device based on optimized bidirectional A-Star algorithm |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114839968A (en) | Unmanned surface vehicle path planning method | |
| CN110108284B (en) | Unmanned aerial vehicle three-dimensional flight path rapid planning method considering complex environment constraint | |
| CN110442135B (en) | Unmanned ship path planning method and system based on improved genetic algorithm | |
| CN112650237B (en) | Ship path planning method and device based on clustering processing and artificial potential field | |
| CN110703747A (en) | Robot autonomous exploration method based on simplified generalized Voronoi diagram | |
| CN107085437A (en) | A kind of unmanned aerial vehicle flight path planing method based on EB RRT | |
| CN111142522A (en) | Intelligent agent control method for layered reinforcement learning | |
| CN111679692A (en) | Unmanned aerial vehicle path planning method based on improved A-star algorithm | |
| CN108388250B (en) | Water surface unmanned ship path planning method based on self-adaptive cuckoo search algorithm | |
| CN109685237B (en) | Unmanned aerial vehicle flight path real-time planning method based on Dubins path and branch limit | |
| CN109655063B (en) | Marine search route planning method for large amphibious aircraft | |
| CN110320919B (en) | Method for optimizing path of mobile robot in unknown geographic environment | |
| CN112799405B (en) | Unmanned ship path planning method based on dynamic barrier environment | |
| CN111024080A (en) | Unmanned aerial vehicle group-to-multi-mobile time-sensitive target reconnaissance path planning method | |
| CN115373399A (en) | Ground robot path planning method based on air-ground cooperation | |
| CN112947591A (en) | Path planning method, device, medium and unmanned aerial vehicle based on improved ant colony algorithm | |
| CN112363539A (en) | Multi-unmanned aerial vehicle cooperative target searching method | |
| CN116880561A (en) | Optimization method and system based on improved particle swarm unmanned aerial vehicle path planning safety enhancement | |
| CN116400733A (en) | Self-adaptive adjustment random tree full-coverage path planning method for reconnaissance unmanned aerial vehicle | |
| CN114820668A (en) | End-to-end building regular outline automatic extraction method based on concentric ring convolution | |
| CN116518974A (en) | Conflict-free route planning method based on airspace grids | |
| Zhao et al. | Global path planning of unmanned vehicle based on fusion of A∗ algorithm and Voronoi field | |
| Zheng et al. | BRR-DQN: UAV path planning method for urban remote sensing images | |
| CN114509085B (en) | Quick path searching method combining grid and topological map | |
| CN116952239A (en) | Unmanned ship path planning method based on fusion of improved A and DWA |
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 |