CN110398250A - A kind of unmanned boat global path planning method - Google Patents
A kind of unmanned boat global path planning method Download PDFInfo
- Publication number
- CN110398250A CN110398250A CN201910743186.2A CN201910743186A CN110398250A CN 110398250 A CN110398250 A CN 110398250A CN 201910743186 A CN201910743186 A CN 201910743186A CN 110398250 A CN110398250 A CN 110398250A
- Authority
- CN
- China
- Prior art keywords
- grid
- cost
- environmental model
- indicate
- path
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 22
- 230000007613 environmental effect Effects 0.000 claims abstract description 45
- 230000006870 function Effects 0.000 claims abstract description 22
- 238000011156 evaluation Methods 0.000 claims abstract description 18
- 230000004888 barrier function Effects 0.000 claims description 29
- 230000005484 gravity Effects 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 2
- 230000008447 perception Effects 0.000 abstract description 3
- 230000006872 improvement Effects 0.000 abstract description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000010845 search algorithm Methods 0.000 description 2
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000013598 vector Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a kind of unmanned boat global path planning methods.Key step includes: that (1) obtains unmanned boat movement state information and environment sensing information；(2) perception environmental model is established；(3) danger level prediction is carried out to environment grid using k nearest neighbor learning algorithm；(4) route searching is carried out using improvement A* algorithm.The present invention is directed to security requirement of unmanned surface vehicle during real navigation, when establishing path planning environmental model, the danger zone in unmanned surface vehicle local environment is predicted using k nearest neighbor algorithm, simultaneously, when carrying out route searching using A* algorithm, safe cost is introduced in its evaluation function, it is ensured that the safety of planning path.
Description
Technical field
The present invention relates to unmanned surface vehicle path planning fields, and in particular to a kind of unmanned boat global path planning method,
Especially a kind of unmanned boat global path planning method based on k nearest neighbor learning algorithm and A* algorithm.
Background technique
Unmanned water surface ship is the small-size water surface task platform for possessing autonomous operation ability, be mainly used for executing it is dangerous with
And it is unsuitable for the task of someone's ship execution.With the fast development of unmanned boat, all shown in military and civilian field
Good development prospect.The autonomous path planning ability of unmanned surface vehicle is the important embodiment of unmanned surface vehicle level of intelligence,
It is the important link that unmanned surface vehicle realizes autonomous navigation.
A* algorithm is a kind of widely used heuristic path planning algorithm.But tradition A* algorithm is carrying out path planning
When only considered the length in path, there is no consider other factors of unmanned surface vehicle during navigation.Due to sea
The influence of the uncertain factors such as stormy waves stream, safety of the unmanned surface vehicle in ocean navigation will receive very big influence, therefore, In
When carrying out the autonomous path planning of unmanned surface vehicle, consider that the safety of its navigation is particularly important.K nearest neighbor (k-Nearest
Neighbor, abbreviation KNN) learn to be a kind of common machine learning method, working mechanism is very simple, and algorithm is convenient for real
It is existing, it is predicted by degree of danger of the k nearest neighbor learning algorithm to unmanned surface vehicle local environment, to current before path planning
Ambient condition has a preferable estimation, is conducive to the safety of path planning.
Summary of the invention
For the above-mentioned prior art, the technical problem to be solved in the present invention is to provide one kind to fully consider that unmanned surface vehicle is advised
The unmanned boat global path planning method for drawing the safety in path, when establishing path planning environmental model, using k nearest neighbor algorithm
Danger zone in unmanned surface vehicle local environment is predicted, meanwhile, when carrying out route searching using A* algorithm, at it
Safe cost is introduced in evaluation function, it is ensured that the safety of planning path.
In order to solve the above technical problems, a kind of unmanned boat global path planning method of the invention, comprising the following steps:
S1: unmanned boat movement state information and environment sensing information are obtained, wherein movement state information includes: that unmanned boat is worked as
Front position coordinate (sx,sy), speed u, course angleAttitude angle, environment sensing information include Obstacle Position coordinate；
S2: environmental model is established using Grid Method, comprising:
S2.1: selected unmanned boat operating area range；
S2.2: rasterizing is carried out to unmanned boat operating area, takes grid side length l=u Δ t, wherein u is unmanned boat
Average speed, Δ t are the motion control beat of unmanned boat；
S2.3: the grid of the operating area in S2.2 is encoded；
S2.4: by the grid after the barrier component identification in the operating area obtained by awareness apparatus to S2.3 coding
In, the grid comprising barrier is barrier grid, and the grid not comprising barrier is free grid, by barrier grid tag
It is 0, is 1 by free grid tag；
S3: danger level prediction is carried out using k nearest neighbor learning algorithm to each free grid, prediction result is dangerous grid
Free grid tag be 2；
S4: route searching is carried out using A* algorithm, comprising:
S4.1: establishing OPEN list and CLOSE list, and starting grid s is added to OPEN list；
S4.2: finding the grid that evaluation function f (m) is minimum in OPEN list, be denoted as current grid i, and grid i is mobile
To CLOSE list；
S4.3: extension current grid i obtains whole adjacent cells of current grid i, executes to each adjacent cells following
Operation:
S4.3.1: if adjacent cells m is barrier grid or in CLOSE list, it is skipped over；Otherwise it executes
Following step:
S4.3.2: if adjacent cells m is added to adjacent cells m in OPEN list not in OPEN list, working as
Father grid of the preceding grid i as adjacent cells m records the evaluation function value f (m), path cost and safe cost of adjacent cells m
The sum of G (m) and from current grid m to the estimation h (m) of terminal grid minimum cost；
S4.3.3: if adjacent cells m in OPEN list, if met:
G (m) < G (i)+L (i, m)
The father node of adjacent cells m is then changed to current grid i, and recalculates the G (m) and f (m) of adjacent cells m
Value；Wherein, L (i, m) indicates the Actual path cost from grid i to adjacent cells m；
If grid i and adjacent cells m be it is direct-connected connect, that is, meet:
|ix-mx|+|iy-my|=1
In formula, ixIndicate lateral coordinates of the grid i in grid environmental model, iyIndicate grid i in grid environmental model
Longitudinal coordinate, mxIndicate lateral coordinates of the grid m in grid environmental model, myIndicate grid m in grid environmental model
Longitudinal coordinate, then L (i, m) is a grid length；
If grid i grid m adjacent thereto is tiltedly to connect, that is, meet:
|ix-mx|+|iy-my|=2
In formula, ixIndicate lateral coordinates of the grid i in grid environmental model, iyIndicate grid i in grid environmental model
Longitudinal coordinate, mxIndicate lateral coordinates of the grid m in grid environmental model, myIndicate grid m in grid environmental model
Longitudinal coordinate, then L (i, m) beA grid length；
S4.4: repeating S4.2-S4.3 until destination node t has been added to CLOSE list or OPEN list
It is empty；When destination node t has been added to CLOSE list, then shows that optimal path is found, successively recall since target grid
Father's grid, until starting grid is to get the optimal path arrived from starting grid to target grid；Destination node is not found,
OPEN list is empty, shows that path is not present.
The invention also includes:
Movement state information is obtained by GPS and electronic compass in 1.S1, and the environment sensing information passes through laser radar
It obtains.
In 2.S3 danger level prediction include:
S3.1: for each prediction grid (xp,yp), it chooses and prediction grid (xp,yp) distance be less than or equal to K
The grid composition decision set of grid length closes, and wherein K is the positive integer of a setting；
S3.2: in the ratio epsilon of decision centralized calculation free grid quantity and barrier grid quantity；If ε > p, in advance
Surveying grid is complete free grid；If ε≤p, grid is predicted as dangerous grid, wherein p is predictive coefficient, 0≤p≤1；
Evaluation function f (m) meets in 3.S4.2:
F (m)=(1- ω) g (m)+ω s (m)+h (m)
In formula, f (m) is the evaluation function of grid m；G (m) is the path cost for originating grid to grid m；H (m) is to inspire
Function is the estimation from grid m to terminal grid minimum cost；S (m) indicates starting point to the safe cost of grid m, according to grid
Result after lattice security evaluation determines the safe cost value r (m) of each grid, if m is complete free grid
R (m)=0, r (m)=k if m is dangerous grid, k are the constant of setting, and s (m) is from starting grid to grid
The safe cost value r (m) of each path node of m accumulative and；ω is the cost specific gravity of safety and path length, 0≤ω <
1, ω indicates that safety is more important closer to 1；
From starting point grid to the actual cost of grid m, i.e., the sum of path cost described in S4.2 and safe cost G (m) are indicated
The sum of path cost and safe cost:
G (m)=(1- ω) g (m)+ω s (m)
It is used in grating map described in S4.2 from current grid m to the estimation h (m) of terminal grid minimum cost and works as front gate
Manhatton distance of the lattice to target grid:
H (m)=| mx-tx|+|my-ty|
In formula, mxIndicate current grid m lateral coordinates, m in grid environmental modelyIndicate current grid m in grid environment
Longitudinal coordinate in model, txIndicate target grid t lateral coordinates, t in grid environmental modelyIndicate target grid t in grid ring
Longitudinal coordinate in border.
The invention has the advantages that: this method to consider safety of unmanned surface vehicle during real navigation, passes through k nearest neighbor
Learning algorithm predicts the danger zone in current environment model, and danger zone in environmental model is marked, meanwhile,
The considerations of improving to A* algorithm, cost safe to path is increased in evaluation function improves A* algorithmic rule path
Safety.Safety for unmanned surface vehicle in actual conditions autonomous navigation plays an important role.
Detailed description of the invention
Fig. 1 is unmanned surface vehicle global path planning total algorithm flow chart of the invention；
Fig. 2 is a kind of unmanned boat global path planning method based on k nearest neighbor learning algorithm and A* algorithm of the invention
Overall step flow diagram.
Specific embodiment
The specific embodiment of the invention is described further with reference to the accompanying drawing
A kind of unmanned boat global path planning method based on k nearest neighbor learning algorithm and A* algorithm proposed by the present invention, such as
Shown in Fig. 1, specifically there are following steps:
Step 1: obtaining unmanned boat movement state information and environment sensing information.
(1.1) data information that GPS, electronic compass (TCM) are received by serial ports, obtains environment sensing by laser radar
Information.
(1.2) serial data received is verified, is decoded according to the communications protocol of respective sensor respectively, obtained
Current position coordinates (the s of unmanned water surface shipx,sy), speed u, course angleThe movement state informations such as attitude angle and barrier
The environmental informations such as position.
Step 2: establishing perception environmental model, the expression for environment, using the Grid Method of convenient and efficient.
(2.1) environmental model range is established according to unmanned surface vehicle operating area.Environmental model range is with starting point s and mesh
The rectangular area of formation after punctuate t is diagonal line and cornerwise length is extended 10% along direction of both ends respectively.
(2.2) operating area current to unmanned surface vehicle carries out rasterizing, movement of the side length of grid according to unmanned boat
Depending on ability, it is the average speed of unmanned boat that this method, which takes grid side length l=u Δ t, u, and Δ t is the motion control of unmanned boat
Beat.
(2.3) grid of operating area is encoded.Grid environment is encoded using method of direct coordinate.
(2.4) by each barrier component identification into perception grid environment, for the ease of algorithm realization, by lattice types
It is marked by number.Grid comprising barrier is barrier grid, is 0 by the grid tag；Obstacle is not included completely
The grid of object is free grid, is 1 by the grid tag.
Step 3: danger level prediction being carried out using k nearest neighbor learning algorithm to free grid, dangerous grid is marked.
(3.1) a K value is set according to barrier complexity, K value indicates currently to predict grid (x according to distancep,yp)K
Grid information in range is predicted.
(3.2) for each prediction grid (xp,yp), it is calculated at a distance from surrounding grid.The calculating of distance passes through Europe
Formula distance realizes that Euclidean distance can describe two o'clock in the actual distance (physical distance) of n-dimensional space.Known two n tie up to
Measure x=(x1,x2,x3,···,xn) and y=(y1,y2,y3,···,yn), the Euclidean distance formula of the two vectors can be with
It indicates are as follows:
(3.3) it chooses and prediction grid (xp,yp) distance less than or equal to K grid composition decision set close.In decision set
Calculate the ratio epsilon of free grid and barrier grid.If ε > p, predict that grid is complete free grid；If ε≤p,
Predict that grid is dangerous grid.
(3.4) the dangerous grid after prediction is re-flagged with complete free grid.Dangerous grid mark after prediction
It is denoted as 2.
Step 4: carrying out route searching using A* algorithm is improved.
A* algorithm is a kind of heuristic search algorithm, and the number of nodes of search is mainly reduced by heuristic function, is improved
Search efficiency.Heuristic search is exactly that each position searched for is assessed in the search in state space, is obtained best
Position, then scan for from this position until target.It can be omitted a large amount of meaningless searching routes in this way, improve effect
Rate.In heuristic search, the appraisal to position is highly important.There can be different effects using different appraisals.
The specific steps for improving A* algorithm are as shown in Figure 2.
(4.1) safety for considering unmanned surface vehicle navigation, introduces safe cost s (v) in evaluation function, improved
Evaluation function can indicate are as follows:
F (v)=(1- ω) g (v)+ω s (v)+h (v)
In formula, f (v) is the evaluation function of present node v；G (v) is path cost of the starting point to present node v；H (v) is
From current vertex v to the estimation of terminal minimum cost.If h (v)=0, i.e., when not utilizing global information, A* algorithm is just simplified to
Dijkstra's algorithm.The standard that the selection of h (v) function depends on path optimization cannot excessively high estimation v when selecting h (v)
Minimum cost.S (v) indicates that starting point to the safe cost of node v, determines each according to the result after node security assessment
The safe cost value r (v) of node, r (v)=0 if v is complete free grid, r (v)=k, k if v is dangerous grid
For constant, s (v) be the accumulative of the safe cost value r (v) of each path node from start node to node v and.ω is safety
With the cost specific gravity of path length, 0≤ω < 1, ω indicate that safety is more important closer to 1.In practice, it needs to shortest path
Diameter and most secure path carry out certain tradeoff.G (v) is enabled to indicate from starting point node to the actual cost of present node v, i.e. path
The sum of cost and safe cost:
G (v)=(1- ω) g (v)+ω s (v)
In grating map, heuristic function h (v) mostly uses present node to the manhatton distance of destination node.
H (v)=| vx-tx|+|vy-ty|
In formula, vxIndicate present node v lateral coordinates in grating map, vyIndicate that present node v is indulged in grating map
To coordinate, txIndicate target point t lateral coordinates in grating map, tyIndicate target point t longitudinal coordinate in grating map.
(4.2) OPEN list and CLOSE list are established, starting grid s is added to OPEN list.
(4.3) grid that f value is minimum in OPEN list is found, current grid i is denoted as.Grid i is switched to CLOSE column
Table.
(4.4) following operation is executed to current grid i each adjacent grid m:
1. if grid m can not by or in CLOSE list, skip over it.Otherwise execute following step.
2. if grid m adds it not in OPEN list.Using current grid i as the father node of grid m.
Record f, G and the h value of node m.
3. being with reference to checking whether new path is more preferable with G value if grid m is in OPEN list, it may be assumed that
G (m) < G (i)+L (i, m)
Wherein, L (i, m) is indicated from grid i to the actual cost of grid m.Lower G value means better path.Such as
Fruit above formula is set up, and the father node of grid m is just changed to current grid i, and recalculate G the and f value of grid m.
(4.5) (4.3)-(4.4) are repeated until target grid t has been added to CLOSE list or OPEN list
Through sky.Target grid t CLOSE list is added to, optimal path is found；Do not find destination node, OPEN list
Empty, path is not present.
The specific embodiment of the invention further include:
As depicted in figs. 1 and 2 the following steps are included:
(1) unmanned boat movement state information and environment sensing information are obtained
(1.1) data information that GPS, electronic compass (TCM) are received by serial ports, obtains environment sensing by laser radar
Information.
(1.2) serial data received is verified, is decoded according to the communications protocol of respective sensor respectively, obtained
Current position coordinates (the s of unmanned water surface shipx,sy), speed u, course angleThe movement state informations such as attitude angle and barrier
The environmental informations such as position.
(2) environmental model is established using Grid Method.
(2.1) environmental model range is determined according to unmanned surface vehicle operating area.
(2.2) rasterizing is carried out to the operating area of unmanned boat, depending on the side length of grid is according to the locomitivity of unmanned boat,
It is the average speed of unmanned boat that this method, which takes grid side length l=u Δ t, u, and Δ t is the motion control beat of unmanned boat.
(2.3) grid of operating area is encoded.
(2.4) the barrier component identification obtained by awareness apparatus is included into barrier into grid environmental model
Grid is barrier grid, and the grid not comprising barrier is free grid completely.For the ease of algorithm realization, by lattice types
It is marked by number.It is 0 by barrier grid tag, is 1 by free grid tag.
(3) danger level prediction is carried out using k nearest neighbor learning algorithm to free grid, dangerous grid is marked.
(3.1) a K value is set according to barrier complexity, K is a distance value, indicates that distance will currently predict grid
(xp,yp) distance be less than or equal to K grid length.According to the grid information in K grid length range to (xp,yp) danger
Dangerous degree is predicted.
(3.2) for each prediction grid (xp,yp), it is calculated with surrounding at a distance from close grid, and the calculating of distance is logical
Euclidean distance is crossed to realize.,
(3.3) it chooses and prediction grid (xp,yp) distance less than or equal to K grid composition decision set close.In decision set
Calculate the ratio epsilon of free grid and barrier grid.If ε > p, predict that grid is complete free grid；If ε≤p,
Predict that grid is dangerous grid, wherein p is predictive coefficient, 0≤p≤1.
(3.4) the dangerous grid after prediction is re-flagged with complete free grid.Dangerous grid mark after prediction
It is denoted as 2.
(4) route searching is carried out using improvement A* algorithm.A* algorithm is a kind of heuristic search algorithm, mainly by opening
Number send a letter to reduce the number of nodes of search, improves search efficiency.Heuristic search is exactly search in state space to each
The position of a search is assessed, and obtains best position, then scan for until target from this position.It can be omitted in this way
A large amount of meaningless searching routes, improve efficiency.In heuristic search, the appraisal to position is highly important.It uses
Different appraisals can have different effects.
(4.1) safety for considering unmanned surface vehicle navigation, introduces safe cost s (v) in evaluation function, improved
Evaluation function can indicate are as follows:
F (v)=(1- ω) g (v)+ω s (v)+h (v)
In formula, f (v) is the evaluation function of current grid v；G (v) is the path cost for originating grid to current grid v；h
(v) it is heuristic function, is the estimation from current grid v to terminal grid minimum cost.S (v) indicates starting point to the safety of node v
Cost, according to the safe cost value r (v) for determining each node to the result after node security assessment, if v is completely freely
Then r (v)=0, r (v)=k if v is dangerous grid, k are constant to grid, and s (v) is each of from start node to node v
The safe cost value r (v) of path node accumulative and.ω is the cost specific gravity of safety and path length, and 0≤ω < 1, ω is got over
Indicate that safety is more important close to 1.Enable G (v) indicate from starting point node to the actual cost of present node v, i.e., path cost with
The sum of safe cost:
G (v)=(1- ω) g (v)+ω s (v)
In grating map, heuristic function h (v) mostly uses current grid to the manhatton distance of target grid.
H (v)=| vx-tx|+|vy-ty|
In formula, vxIndicate current grid v lateral coordinates, v in grid environmental modelyIndicate current grid v in grid environment
Longitudinal coordinate in model, txIndicate target grid t lateral coordinates, t in grid environmental modelyIndicate target grid t in grid ring
Longitudinal coordinate in border.
(4.2) OPEN list and CLOSE list are established, starting grid s is added to OPEN list.
(4.3) grid that f value is minimum in OPEN list is found, current grid i is denoted as.Grid i is moved to CLOSE column
Table.
(4.4) current grid i is extended, its adjacent cells m is obtained, following operation is executed to grid m:
1. skipping over it if grid m is barrier grid or in CLOSE list.Otherwise it executes next
Step.
2. if grid m is added it in OPEN list not in OPEN list.Using current grid i as grid m
Father's grid.Record f, G and the h value of grid m.
3. if grid m in OPEN list, is reference with G value, checks whether new path is more preferable, if following formula
It sets up, then the father node of node m is changed to present node i, and recalculate G the and f value of node m.
G (m) < G (i)+L (i, m)
Wherein, L (i, m) is indicated from grid i to the Actual path cost of its adjacent cells m, if grid i grid adjacent thereto
Lattice m be it is direct-connected connect, i.e.,
|ix-mx|+|iy-my|=1
In formula, ixIndicate lateral coordinates of the grid i in grid environmental model, iyIndicate grid i in grid environmental model
Longitudinal coordinate, mxIndicate lateral coordinates of the grid m in grid environmental model, myIndicate grid m in grid environmental model
Longitudinal coordinate, then L (i, m) is a grid units length；If grid i grid m adjacent thereto is tiltedly to connect, i.e.,
|ix-mx|+|iy-my|=2
In formula, ixIndicate lateral coordinates of the grid i in grid environmental model, iyIndicate grid i in grid environmental model
Longitudinal coordinate, mxIndicate lateral coordinates of the grid m in grid environmental model, myIndicate grid m in grid environmental model
Longitudinal coordinate, then L (i, m) beA grid units length.
(4.5) (4.3)-(4.4) are repeated until destination node t has been added to CLOSE list or OPEN list
Through sky.Destination node t has been added to CLOSE list, has shown that optimal path is found, has successively been recalled since target grid
Father's grid, until starting grid is to get the optimal path arrived from starting grid to target grid；Destination node is not found,
OPEN list is empty, shows that path is not present.
Claims (4)
1. a kind of unmanned boat global path planning method, which comprises the following steps:
S1: unmanned boat movement state information and environment sensing information are obtained, wherein movement state information includes: unmanned boat present bit
Set coordinate (sx,sy), speed u, course angleAttitude angle, environment sensing information include Obstacle Position coordinate；
S2: environmental model is established using Grid Method, comprising:
S2.1: selected unmanned boat operating area range；
S2.2: rasterizing is carried out to unmanned boat operating area, takes grid side length l=u Δ t, wherein u is being averaged for unmanned boat
The speed of a ship or plane, Δ t are the motion control beat of unmanned boat；
S2.3: the grid of the operating area in S2.2 is encoded；
S2.4: in the grid after the barrier component identification in the operating area obtained by awareness apparatus to S2.3 is encoded,
Grid comprising barrier is barrier grid, and the grid not comprising barrier is free grid, is by barrier grid tag
0, it is 1 by free grid tag；
S3: carrying out danger level prediction using k nearest neighbor learning algorithm to each free grid, prediction result be dangerous grid from
It is 2 by grid tag；
S4: route searching is carried out using A* algorithm, comprising:
S4.1: establishing OPEN list and CLOSE list, and starting grid s is added to OPEN list；
S4.2: the grid that evaluation function f (m) is minimum in OPEN list is found, current grid i is denoted as, grid i is moved to
CLOSE list；
S4.3: extension current grid i obtains whole adjacent cells of current grid i, executes following behaviour to each adjacent cells
Make:
S4.3.1: if adjacent cells m is barrier grid or in CLOSE list, it is skipped over；Otherwise it executes and connects down
The step come:
S4.3.2: if adjacent cells m is added to adjacent cells m in OPEN list not in OPEN list, working as front gate
Father grid of the lattice i as adjacent cells m records the sum of evaluation function value f (m), path cost and the safe cost of adjacent cells m
G (m) and from current grid m to the estimation h (m) of terminal grid minimum cost；
S4.3.3: if adjacent cells m in OPEN list, if met:
G (m) < G (i)+L (i, m)
The father node of adjacent cells m is then changed to current grid i, and recalculates G (m) and f (m) value of adjacent cells m；Its
In, L (i, m) indicates the Actual path cost from grid i to adjacent cells m；
If grid i and adjacent cells m be it is direct-connected connect, that is, meet:
|ix-mx|+|iy-my|=1
In formula, ixIndicate lateral coordinates of the grid i in grid environmental model, iyIndicate that grid i is vertical in grid environmental model
To coordinate, mxIndicate lateral coordinates of the grid m in grid environmental model, myIndicate longitudinal direction of the grid m in grid environmental model
Coordinate, then L (i, m) is a grid length；
If grid i grid m adjacent thereto is tiltedly to connect, that is, meet:
|ix-mx|+|iy-my|=2
In formula, ixIndicate lateral coordinates of the grid i in grid environmental model, iyIndicate that grid i is vertical in grid environmental model
To coordinate, mxIndicate lateral coordinates of the grid m in grid environmental model, myIndicate longitudinal direction of the grid m in grid environmental model
Coordinate, then L (i, m) beA grid length；
S4.4: repeating S4.2-S4.3 until destination node t has been added to CLOSE list or OPEN list sky；
When destination node t has been added to CLOSE list, then shows that optimal path is found, father's grid are successively recalled since target grid
Lattice, until starting grid is to get the optimal path arrived from starting grid to target grid；Do not find destination node, OPEN column
Table is empty, shows that path is not present.
2. a kind of unmanned boat global path planning method according to claim 1, it is characterised in that: move shape described in S1
State information is obtained by GPS and electronic compass, and the environment sensing information is obtained by laser radar.
3. a kind of unmanned boat global path planning method according to claim 1, it is characterised in that: danger level is predicted in S3
Include:
S3.1: for each prediction grid (xp,yp), it chooses and prediction grid (xp,yp) distance be less than or equal to K grid it is long
The grid composition decision set of degree closes, and wherein K is the positive integer of a setting；
S3.2: in the ratio epsilon of decision centralized calculation free grid quantity and barrier grid quantity；If ε > p, predicts grid
Lattice are complete free grid；If ε≤p, grid is predicted as dangerous grid, wherein p is predictive coefficient, 0≤p≤1.
4. a kind of unmanned boat global path planning method according to claim 1, it is characterised in that: evaluate letter described in S4.2
Number f (m) meets:
F (m)=(1- ω) g (m)+ω s (m)+h (m)
In formula, f (m) is the evaluation function of grid m；G (m) is the path cost for originating grid to grid m；H (m) is to inspire letter
Number, is the estimation from grid m to terminal grid minimum cost；S (m) indicates starting point to the safe cost of grid m, according to grid
Result after security evaluation determines the safe cost value r (m) of each grid, r (m)=0 if m is complete free grid,
R (m)=k if m is dangerous grid, k are the constant of setting, and s (m) is each path node from starting grid to grid m
Safe cost value r (m) accumulative and；ω is the cost specific gravity of safety and path length, and 0≤ω < 1, ω are indicated closer to 1
Safety is more important；
The sum of path cost described in S4.2 and safe cost G (m) are indicated from starting point grid to the actual cost of grid m, i.e. path
The sum of cost and safe cost:
G (m)=(1- ω) g (m)+ω s (m)
Described in S4.2 from current grid m to the estimation h (m) of terminal grid minimum cost in grating map using current grid to
The manhatton distance of target grid:
H (m)=| mx-tx|+|my-ty|
In formula, mxIndicate current grid m lateral coordinates, m in grid environmental modelyIndicate current grid m in grid environmental model
Middle longitudinal coordinate, txIndicate target grid t lateral coordinates, t in grid environmental modelyIndicate target grid t in grid environment
Longitudinal coordinate.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910743186.2A CN110398250B (en) | 2019-08-13 | 2019-08-13 | Unmanned ship global path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910743186.2A CN110398250B (en) | 2019-08-13 | 2019-08-13 | Unmanned ship global path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110398250A true CN110398250A (en) | 2019-11-01 |
| CN110398250B CN110398250B (en) | 2022-01-11 |
Family
ID=68328168
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910743186.2A Active CN110398250B (en) | 2019-08-13 | 2019-08-13 | Unmanned ship global path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110398250B (en) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110703762A (en) * | 2019-11-04 | 2020-01-17 | 东南大学 | Hybrid path planning method for unmanned surface vehicle in complex environment |
| CN111176281A (en) * | 2019-12-31 | 2020-05-19 | 大连民族大学 | Multi-surface unmanned ship coverage type collaborative search method and system based on quadrant method |
| CN111189468A (en) * | 2020-01-13 | 2020-05-22 | 天津工业大学 | Wave glider global path planning method |
| CN111412918A (en) * | 2020-03-13 | 2020-07-14 | 天津大学 | Unmanned ship global safety path planning method |
| CN112068564A (en) * | 2020-09-10 | 2020-12-11 | 中国船舶科学研究中心 | Intelligent ship control method based on economic navigation optimization |
| CN112113573A (en) * | 2020-09-18 | 2020-12-22 | 武汉理工大学 | Planning method for coverage path of single unmanned measurement boat |
| CN112161631A (en) * | 2020-12-01 | 2021-01-01 | 湖北第二师范学院 | Safe path planning method based on large satellite grid map |
| CN113419535A (en) * | 2021-07-05 | 2021-09-21 | 鹏城实验室 | Double-boat path planning method, device, equipment and computer readable storage medium |
| CN113608531A (en) * | 2021-07-26 | 2021-11-05 | 福州大学 | Unmanned vehicle real-time global path planning method based on dynamic window of safety A-guiding point |
| CN113885531A (en) * | 2021-11-05 | 2022-01-04 | 上海肇观电子科技有限公司 | Method for moving robot, circuit, medium, and program |
| CN113934159A (en) * | 2021-10-25 | 2022-01-14 | 中国船舶工业综合技术经济研究院 | Unmanned ship reliability test environment model construction method |
| CN115615447A (en) * | 2022-09-20 | 2023-01-17 | 泰州市金海运船用设备有限责任公司 | Optimal path prediction method, device and equipment |
| CN117093012A (en) * | 2023-08-04 | 2023-11-21 | 广东工业大学 | Underwater robot path planning method for improving A-algorithm in ocean environment |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102810118A (en) * | 2012-07-05 | 2012-12-05 | 上海电力学院 | K nearest neighbor search method for variable weight network |
| CN106017472A (en) * | 2016-05-17 | 2016-10-12 | 成都通甲优博科技有限责任公司 | Global path planning method, global path planning system and unmanned aerial vehicle |
| US20180070212A1 (en) * | 2016-09-02 | 2018-03-08 | Athentek Innovations, Inc. | Systems and methods to track movement of a device in an indoor environment |
| CN108444488A (en) * | 2018-02-05 | 2018-08-24 | 天津大学 | Based on etc. steps sample A* algorithms unmanned local paths planning method |
| CN109781126A (en) * | 2018-12-30 | 2019-05-21 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of global static path planning method based on vector method |
-
2019
- 2019-08-13 CN CN201910743186.2A patent/CN110398250B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102810118A (en) * | 2012-07-05 | 2012-12-05 | 上海电力学院 | K nearest neighbor search method for variable weight network |
| CN106017472A (en) * | 2016-05-17 | 2016-10-12 | 成都通甲优博科技有限责任公司 | Global path planning method, global path planning system and unmanned aerial vehicle |
| US20180070212A1 (en) * | 2016-09-02 | 2018-03-08 | Athentek Innovations, Inc. | Systems and methods to track movement of a device in an indoor environment |
| CN108444488A (en) * | 2018-02-05 | 2018-08-24 | 天津大学 | Based on etc. steps sample A* algorithms unmanned local paths planning method |
| CN109781126A (en) * | 2018-12-30 | 2019-05-21 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of global static path planning method based on vector method |
Non-Patent Citations (2)
| Title |
|---|
| ADITYA S. GADRE ET AL.: "Guidance and Control of An Unmanned Surface Vehicle Exhibiting Sternward Motion", 《MTS/IEEE OCEANS CONFERENCE》 * |
| 朱骋等: "无人水面艇自适应路径跟踪算法", 《导航与控制》 * |
Cited By (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110703762A (en) * | 2019-11-04 | 2020-01-17 | 东南大学 | Hybrid path planning method for unmanned surface vehicle in complex environment |
| CN111176281A (en) * | 2019-12-31 | 2020-05-19 | 大连民族大学 | Multi-surface unmanned ship coverage type collaborative search method and system based on quadrant method |
| CN111189468A (en) * | 2020-01-13 | 2020-05-22 | 天津工业大学 | Wave glider global path planning method |
| CN111412918B (en) * | 2020-03-13 | 2022-01-07 | 天津大学 | Unmanned ship global safety path planning method |
| CN111412918A (en) * | 2020-03-13 | 2020-07-14 | 天津大学 | Unmanned ship global safety path planning method |
| CN112068564A (en) * | 2020-09-10 | 2020-12-11 | 中国船舶科学研究中心 | Intelligent ship control method based on economic navigation optimization |
| CN112068564B (en) * | 2020-09-10 | 2022-09-06 | 中国船舶科学研究中心 | Intelligent ship control method based on economic navigation optimization |
| WO2022057701A1 (en) * | 2020-09-18 | 2022-03-24 | 武汉理工大学 | Coverage-path planning method for single unmanned surface mapping vessel |
| CN112113573A (en) * | 2020-09-18 | 2020-12-22 | 武汉理工大学 | Planning method for coverage path of single unmanned measurement boat |
| CN112161631A (en) * | 2020-12-01 | 2021-01-01 | 湖北第二师范学院 | Safe path planning method based on large satellite grid map |
| CN113419535A (en) * | 2021-07-05 | 2021-09-21 | 鹏城实验室 | Double-boat path planning method, device, equipment and computer readable storage medium |
| CN113419535B (en) * | 2021-07-05 | 2024-02-27 | 鹏城实验室 | Dual-boat path planning method, device, equipment and computer readable storage medium |
| CN113608531A (en) * | 2021-07-26 | 2021-11-05 | 福州大学 | Unmanned vehicle real-time global path planning method based on dynamic window of safety A-guiding point |
| CN113608531B (en) * | 2021-07-26 | 2023-09-12 | 福州大学 | Unmanned vehicle real-time global path planning method based on safety A-guidance points |
| CN113934159A (en) * | 2021-10-25 | 2022-01-14 | 中国船舶工业综合技术经济研究院 | Unmanned ship reliability test environment model construction method |
| CN113885531A (en) * | 2021-11-05 | 2022-01-04 | 上海肇观电子科技有限公司 | Method for moving robot, circuit, medium, and program |
| CN115615447A (en) * | 2022-09-20 | 2023-01-17 | 泰州市金海运船用设备有限责任公司 | Optimal path prediction method, device and equipment |
| CN117093012A (en) * | 2023-08-04 | 2023-11-21 | 广东工业大学 | Underwater robot path planning method for improving A-algorithm in ocean environment |
| CN117093012B (en) * | 2023-08-04 | 2024-02-02 | 广东工业大学 | Underwater robot path planning method for improving A-algorithm in ocean environment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110398250B (en) | 2022-01-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110398250A (en) | A kind of unmanned boat global path planning method | |
| Pereira et al. | Risk‐aware path planning for autonomous underwater vehicles using predictive ocean models | |
| US9405445B2 (en) | Apparatus and methods for routing | |
| Huynh et al. | Predictive motion planning for AUVs subject to strong time-varying currents and forecasting uncertainties | |
| CN103900573B (en) | A kind of underwater research vehicle multiple constraint Route planner based on S57 standard electronic sea chart | |
| Hernández et al. | Online motion planning for unexplored underwater environments using autonomous underwater vehicles | |
| US9945673B2 (en) | Apparatus and methods for routing | |
| CN107356254A (en) | Suitable for the particle group optimizing method of geomagnetic auxiliary navigation trajectory planning | |
| CN109374004A (en) | A kind of Intelligent unattended ship paths planning method based on IA* algorithm | |
| Pereira et al. | Toward risk aware mission planning for autonomous underwater vehicles | |
| Chen et al. | Research on ship meteorological route based on A-star algorithm | |
| US20170254649A1 (en) | Apparatus and methods for routing | |
| CN115615447A (en) | Optimal path prediction method, device and equipment | |
| CN111307158A (en) | AUV three-dimensional route planning method | |
| CN111412918A (en) | Unmanned ship global safety path planning method | |
| Zhou et al. | Compressing AIS trajectory data based on the multi-objective peak douglas–peucker algorithm | |
| Lee et al. | Generation of Ship’s passage plan using data-driven shortest path algorithms | |
| Gao et al. | An Optimized Path Planning Method for Container Ships in Bohai Bay Based on Improved Deep Q-Learning | |
| Al-Sabban et al. | Extending persistent monitoring by combining ocean models and Markov decision processes | |
| CN112880678A (en) | Unmanned ship navigation planning method in complex water area environment | |
| Berger et al. | A hybrid genetic algorithm for rescue path planning in uncertain adversarial environment | |
| CN113776535A (en) | Unmanned ship route planning method based on rasterized electronic chart | |
| CA2933304C (en) | A system and method for updating cartographic information | |
| JP2022123401A (en) | Route planning device, moving object, route planning method and program | |
| Molchanov et al. | Active drifters: Sailing with the ocean currents |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |