CN110398250B - Unmanned ship global path planning method - Google Patents

Unmanned ship global path planning method Download PDF

Info

Publication number
CN110398250B
CN110398250B CN201910743186.2A CN201910743186A CN110398250B CN 110398250 B CN110398250 B CN 110398250B CN 201910743186 A CN201910743186 A CN 201910743186A CN 110398250 B CN110398250 B CN 110398250B
Authority
CN
China
Prior art keywords
grid
environment
cost
path
grids
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.)
Active
Application number
CN201910743186.2A
Other languages
Chinese (zh)
Other versions
CN110398250A (en
Inventor
张磊
封佳祥
黄兵
刘涛
许建辉
郑帅
苏玉民
曹建
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201910743186.2A priority Critical patent/CN110398250B/en
Publication of CN110398250A publication Critical patent/CN110398250A/en
Application granted granted Critical
Publication of CN110398250B publication Critical patent/CN110398250B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a global path planning method for an unmanned ship. The method mainly comprises the following steps: (1) acquiring unmanned ship motion state information and environment perception information; (2) establishing a perception environment model; (3) predicting the risk of the environment grid by adopting a K nearest neighbor learning algorithm; (4) and performing path search by adopting an improved A-x algorithm. According to the safety requirement of the unmanned surface vehicle in the actual sailing process, when a path planning environment model is established, a K neighbor algorithm is adopted to predict a dangerous area in the environment where the unmanned surface vehicle is located, and meanwhile, when an A-algorithm is adopted to search paths, safety cost is introduced into an evaluation function of the unmanned surface vehicle, so that the safety of the planned paths is ensured.

Description

Unmanned ship global path planning method
Technical Field
The invention relates to the field of unmanned surface vehicle path planning, in particular to an unmanned surface vehicle global path planning method, and particularly relates to an unmanned surface vehicle global path planning method based on a K neighbor learning algorithm and an A-star algorithm.
Background
An unmanned surface vessel is a small surface mission platform with autonomous operation capability, mainly used for performing dangerous and tasks which are not suitable for being performed by an unmanned ship. With the rapid development of unmanned boats, the unmanned boats have good development prospects in the military and civil fields. The autonomous path planning capability of the unmanned surface vehicle is an important embodiment of the intelligent level of the unmanned surface vehicle and an important link for realizing autonomous navigation of the unmanned surface vehicle.
The a-algorithm is a widely used heuristic path planning algorithm. However, the traditional a-x algorithm only considers the length of the path when planning the path, and does not consider other factors of the unmanned surface vehicle in the process of sailing. Due to the influence of uncertain factors such as sea stormy waves and currents, the safety of the unmanned surface vehicle during sea navigation is greatly influenced, and therefore, the navigation safety of the unmanned surface vehicle is considered to be particularly important when the autonomous path planning of the unmanned surface vehicle is carried out. K-Nearest Neighbor (KNN) learning is a common machine learning method, the working mechanism is very simple, the algorithm is convenient to realize, the risk degree of the environment where the unmanned surface vehicle is located is predicted through the K-Nearest Neighbor learning algorithm, the current environment state is well estimated before path planning, and the safety of path planning is facilitated.
Disclosure of Invention
Aiming at the prior art, the technical problem to be solved by the invention is to provide an unmanned surface vehicle global path planning method which fully considers the safety of a planned path of an unmanned surface vehicle.
In order to solve the technical problem, the invention provides a global path planning method for an unmanned ship, which comprises the following steps:
s1: acquiring unmanned ship motion state information and environment perception information, wherein the motion state information comprises: coordinates(s) of current position of unmanned shipx,sy) Speed u, course angle
Figure BDA0002164675790000011
Attitude angle, environment perception information including obstacle position coordinates;
s2: establishing an environment model by adopting a grid method, comprising the following steps:
s2.1: selecting the range of the unmanned boat operation area;
s2.2: rasterizing an unmanned ship operation area, and taking the side length l of a grid as u & delta t, wherein u is the average speed of the unmanned ship, and delta t is the motion control beat of the unmanned ship;
s2.3: encoding the grid of the work area in S2.2;
s2.4: identifying obstacle elements in the operation area obtained through the sensing equipment into S2.3 coded grids, wherein the grids containing obstacles are obstacle grids, the grids not containing obstacles are free grids, the obstacle grids are marked as 0, and the free grids are marked as 1;
s3: predicting the risk degree of each free grid by adopting a K nearest neighbor learning algorithm, wherein the prediction result is that the free grid of the dangerous grid is marked as 2;
s4: and performing path search by adopting an A-algorithm, wherein the path search comprises the following steps:
s4.1: establishing an OPEN list and a CLOSE list, and adding the starting grids to the OPEN list;
s4.2: searching the grid with the lowest evaluation function f (m) in the OPEN list, marking as the current grid i, and moving the grid i to the CLOSE list;
s4.3: expanding the current grid i to obtain all adjacent grids of the current grid i, and executing the following operations on each adjacent grid:
s4.3.1 if the adjacent grid m is an obstacle grid or is already in the CLOSE list, skip it; otherwise, executing the following steps:
s4.3.2, if the adjacent grid m is not in the OPEN list, adding the adjacent grid m to the OPEN list, using the current grid i as the parent grid of the adjacent grid m, recording the evaluation function value f (m) of the adjacent grid m, the sum G (m) of the path cost and the safety cost and the estimation h (m) of the minimum cost from the current grid m to the end grid;
s4.3.3 if the adjacent grid m is already in the OPEN list, if:
G(m)＜G(i)+L(i,m)
then the parent node of the adjacent grid m is changed to the current grid i and the values of g (m) and f (m) of the adjacent grid m are recalculated; where L (i, m) represents the actual path cost from grid i to the adjacent grid m;
if the grid i is directly connected with the adjacent grid m, the following conditions are satisfied:
|ix-mx|+|iy-my|＝1
in the formula ixRepresenting the lateral coordinates, i, of a grid i in a model of the grid environmentyRepresenting the vertical coordinate, m, of grid i in the grid environment modelxRepresenting the transverse coordinates of a grid m in a model of the grid environment, myRepresenting the vertical coordinate of the grid m in the grid environment model, and L (i, m) is the length of one grid;
if the grid i is connected with the adjacent grid m obliquely, the following conditions are satisfied:
|ix-mx|+|iy-my|＝2
in the formula ixRepresenting the lateral coordinates, i, of a grid i in a model of the grid environmentyRepresenting the vertical coordinate, m, of grid i in the grid environment modelxRepresenting the transverse coordinates of a grid m in a model of the grid environment, myRepresents the vertical coordinate of the grid m in the grid environment model, then L (i, m) is
Figure BDA0002164675790000031
A grid length;
s4.4: repeating S4.2-S4.3 until the target node t is added into the CLOSE list or the OPEN list is empty; when the target node t is added into the CLOSE list, the optimal path is found, parent grids are sequentially traced from the target grid until the starting grid, and the optimal path from the starting grid to the target grid is obtained; no target node is found and the OPEN list is already empty, indicating that a path does not exist.
The invention also includes:
the motion state information in S1 is acquired through a GPS and an electronic compass, and the environment perception information is acquired through a laser radar.
The risk prediction in S3 includes:
s3.1 for each prediction grid (x)p,yp) Selecting and predicting grids (x)p,yp) The distance of (2) is less than or equal to K grid length grid composition blocksA strategy set, wherein K is a set positive integer;
s3.2, calculating the proportion epsilon of the number of the free grids to the number of the barrier grids in the decision set; if ε > p, then predict the grid as a completely free grid; if epsilon is less than or equal to p, the prediction grid is a dangerous grid, wherein p is a prediction coefficient, and p is more than or equal to 0 and less than or equal to 1;
the valuation function f (m) in S4.2 satisfies:
f(m)＝(1-ω)g(m)+ωs(m)+h(m)
wherein f (m) is an evaluation function of grid m; g (m) is the starting grid to grid m path cost; h (m) is a heuristic function, which is an estimate of the minimum cost from grid m to the end grid; s (m) represents the safety cost from the starting point to the grid m, the safety cost value r (m) of each grid is determined according to the safety evaluation result of the grids, and if m is a completely free grid, the m is
r (m) is 0, if m is a dangerous grid, r (m) is k, k is a set constant, and s (m) is the cumulative sum of the safe cost values r (m) of each path node from the starting grid to the grid m; omega is the cost ratio of safety to the length of the path, omega is more than or equal to 0 and less than 1, and the more close omega is to 1, the more important safety is;
s4.2, the sum G (m) of the path cost and the safety cost represents the actual cost from the starting grid to the grid m, namely the sum of the path cost and the safety cost:
G(m)＝(1-ω)g(m)+ωs(m)
s4.2 the estimate h (m) of the minimum cost from the current grid m to the end grid uses the Manhattan distance of the current grid to the target grid in the grid map:
h(m)＝|mx-tx|+|my-ty|
in the formula, mxRepresents the horizontal coordinate of the current grid m in the grid environment model, myRepresents the vertical coordinate of the current grid m in the grid environment model, txRepresenting the transverse coordinates, t, of a target grid t in a grid environment modelyRepresenting the vertical coordinate of the target grid t in the grid environment.
The invention has the beneficial effects that: the method considers the safety of the unmanned surface vehicle in the actual sailing process, predicts the dangerous area in the current environment model through the K neighbor learning algorithm, marks the dangerous area in the environment model, improves the A-algorithm, increases the consideration of the path safety cost in the evaluation function, and improves the safety of the path planned by the A-algorithm. The unmanned surface vehicle has an important effect on the safety of autonomous navigation of the unmanned surface vehicle under actual conditions.
Drawings
FIG. 1 is a flow chart of an overall algorithm for planning a global path of an unmanned surface vehicle according to the invention;
fig. 2 is a schematic flow chart of the overall steps of the unmanned ship global path planning method based on the K-nearest neighbor learning algorithm and the a-algorithm.
Detailed Description
The following description of the embodiments of the present invention will be made with reference to the accompanying drawings
The invention provides a global path planning method of an unmanned ship based on a K neighbor learning algorithm and an A-star algorithm, which comprises the following steps as shown in figure 1:
step 1: and acquiring the motion state information and the environment perception information of the unmanned ship.
And (1.1) receiving data information of a GPS and an electronic compass (TCM) through a serial port, and acquiring environment perception information through a laser radar.
(1.2) respectively checking and decoding the received data strings according to the communication protocols of the corresponding sensors to obtain the current position coordinates(s) of the unmanned surface vesselx,sy) Speed u, course angle
Figure BDA0002164675790000041
Attitude angle and other motion state information, and obstacle position and other environment information.
Step 2: and establishing a perception environment model, and adopting a convenient and efficient grid method for representing the environment.
And (2.1) establishing an environment model range according to the unmanned surface vehicle operation area. The environment model range is a rectangular region formed by using the starting point s and the target point t as diagonal lines and expanding the length of the diagonal lines by 10% in the directions of both ends.
And (2.2) rasterizing the current operation area of the unmanned surface vehicle, wherein the side length of the grid is determined according to the motion capability of the unmanned surface vehicle, the method takes the side length l of the grid as u.delta t, u is the average speed of the unmanned surface vehicle, and delta t is the motion control beat of the unmanned surface vehicle.
And (2.3) encoding the grid of the working area. And coding the grid environment by adopting a rectangular coordinate method.
(2.4) identifying each obstacle element to the perception grid environment, and marking the grid type by numbers for the convenience of algorithm implementation. The grid containing the obstacle is an obstacle grid, and the grid is marked as 0; the grid containing no obstacles at all is a free grid, which is marked 1.
And step 3: and predicting the risk degree of the free grid by adopting a K nearest neighbor learning algorithm, and marking the dangerous grid.
(3.1) setting a K value according to the complexity of the obstacle, wherein the K value is expressed according to the grid (x) to be predicted at presentp,yp) And predicting the grid information in the K range.
(3.2) for each prediction grid (x)p,yp) And calculates its distance from the surrounding grid. The distance is calculated by the Euclidean distance, which can describe the real distance (physical distance) of two points in the n-dimensional space. Two n-dimensional vectors x ═ x (x) are known1,x2,x3,···,xn) And y ═ y1,y2,y3,···,yn) The euclidean distance formula for these two vectors can be expressed as:
Figure BDA0002164675790000051
(3.3) selecting and predicting grid (x)p,yp) The grids with a distance less than or equal to K constitute a decision set. The ratio epsilon of the free grid to the barrier grid is calculated in the decision set. If ε > p, then predict the grid as a completely free grid; if ε is ≦ p, the prediction grid is a dangerous grid.
And (3.4) re-marking the predicted dangerous grid and the complete free grid. The predicted danger grid is labeled 2.
And 4, step 4: and performing path search by adopting an improved A-x algorithm.
The A-algorithm is a heuristic search algorithm, and is mainly used for reducing the number of searched nodes through a heuristic function and improving the search efficiency. Heuristic search is that the search in the state space evaluates each searched position to get the best position, and then searches from this position to the target. Therefore, a large number of unnecessary search paths can be omitted, and the efficiency is improved. In heuristic search, the valuation of the location is very important. Different valuations may be used with different results. The specific steps of the improved a algorithm are shown in fig. 2.
(4.1) considering the safety of the unmanned surface vehicle in sailing, introducing a safety cost s (v) into an evaluation function, wherein the improved evaluation function can be expressed as:
f(v)＝(1-ω)g(v)+ωs(v)+h(v)
wherein f (v) is an evaluation function of the current node v; g (v) is the path cost from the origin to the current node v; h (v) is an estimate of the minimum cost from the current vertex v to the endpoint. If h (v) is 0, i.e. no global information is utilized, the a-algorithm is simplified to Dijkstra's algorithm. The choice of h (v) function depends on the criteria of path optimization, and the minimum cost of v cannot be overestimated when choosing h (v). s (v) represents the safety cost from the starting point to the node v, the safety cost value r (v) of each node is determined according to the result after the safety evaluation of the node, if v is a complete free grid, r (v) is 0, if v is a dangerous grid, r (v) is k, k is a constant, and s (v) is the accumulated sum of the safety cost values r (v) of each path node from the starting node to the node v. Omega is the cost ratio of safety to the length of the path, omega is more than or equal to 0 and less than 1, and the closer omega is to 1, the more important safety is. In practice, a certain trade-off between the shortest path and the safest path is required. Let g (v) denote the actual cost from the origin node to the current node v, i.e. the sum of the path cost and the security cost:
G(v)＝(1-ω)g(v)+ωs(v)
in the grid map, the heuristic function h (v) mostly adopts the Manhattan distance from the current node to the target node.
h(v)＝|vx-tx|+|vy-ty|
In the formula, vxRepresents the horizontal coordinate of the current node v in the grid map, vyRepresents the vertical coordinate, t, of the current node v in the grid mapxRepresents the transverse coordinate of the target point t in the grid map, tyRepresenting the vertical coordinate of the target point t in the grid map.
(4.2) create the OPEN list and CLOSE list, add the start grid s to the OPEN list.
And (4.3) searching the grid with the lowest f value in the OPEN list, and marking as the current grid i. The grid i is switched to the CLOSE list.
(4.4) performing the following for each neighboring grid m of the current grid i:
first, if grid m is not available or is already in the CLOSE list, it is skipped. Otherwise, the next steps are executed.
② if grid m is not in the OPEN list, add it. The current grid i is taken as the parent node of grid m. Record the f, G, and h values for node m.
Checking if the new path is better with the value of G as a reference if grid m is already in the OPEN list, i.e.:
G(m)＜G(i)+L(i,m)
where L (i, m) represents the actual cost from grid i to grid m. A lower G value means a better path. If the above equation holds, the parent node of grid m is changed to the current grid i, and the G and f values of grid m are recalculated.
(4.5) repeating (4.3) - (4.4) until the target grid t is added to the CLOSE list or the OPEN list is empty. Adding the target grid t into a CLOSE list, and finding the optimal path; no target node is found, the OPEN list is already empty and the path does not exist.
The specific implementation mode of the invention also comprises:
as shown in fig. 1 and 2, the method comprises the following steps:
(1) obtaining unmanned ship motion state information and environment perception information
And (1.1) receiving data information of a GPS and an electronic compass (TCM) through a serial port, and acquiring environment perception information through a laser radar.
(1.2) respectively checking and decoding the received data strings according to the communication protocols of the corresponding sensors to obtain the current position coordinates(s) of the unmanned surface vesselx,sy) Speed u, course angle
Figure BDA0002164675790000071
Attitude angle and other motion state information, and obstacle position and other environment information.
(2) And establishing an environment model by adopting a grid method.
And (2.1) determining the range of the environment model according to the unmanned surface vehicle operation area.
And (2.2) rasterizing the operation area of the unmanned ship, wherein the side length of the grid is determined according to the motion capability of the unmanned ship, the method takes the side length l of the grid as u.delta t, u is the average speed of the unmanned ship, and delta t is the motion control beat of the unmanned ship.
And (2.3) encoding the grid of the working area.
And (2.4) identifying the obstacle elements obtained by the sensing equipment into a grid environment model, wherein the grid containing the obstacle is an obstacle grid, and the grid not containing the obstacle is a free grid. To facilitate the algorithm implementation, the grid types are labeled by numbers. The obstacle grid is labeled 0 and the free grid is labeled 1.
(3) And predicting the risk degree of the free grid by adopting a K nearest neighbor learning algorithm, and marking the dangerous grid.
(3.1) setting a value of K according to the complexity of the obstacle, wherein K is a distance value which represents the distance from the current grid (x) to be predictedp,yp) Is less than or equal to K grid lengths. According to the grid information pairs (x) within the K grid length rangesp,yp) The risk of (2) is predicted.
(3.2) for each prediction grid (x)p,yp) Calculating it from the surrounding gridAnd the distance is calculated by the Euclidean distance. ,
(3.3) selecting and predicting grid (x)p,yp) The grids with a distance less than or equal to K constitute a decision set. The ratio epsilon of the free grid to the barrier grid is calculated in the decision set. If ε > p, then predict the grid as a completely free grid; if epsilon is less than or equal to p, the prediction grid is a dangerous grid, wherein p is a prediction coefficient, and p is more than or equal to 0 and less than or equal to 1.
And (3.4) re-marking the predicted dangerous grid and the complete free grid. The predicted danger grid is labeled 2.
(4) And performing path search by adopting an improved A-x algorithm. The A-algorithm is a heuristic search algorithm, and is mainly used for reducing the number of searched nodes through a heuristic function and improving the search efficiency. Heuristic search is that the search in the state space evaluates each searched position to get the best position, and then searches from this position to the target. Therefore, a large number of unnecessary search paths can be omitted, and the efficiency is improved. In heuristic search, the valuation of the location is very important. Different valuations may be used with different results.
(4.1) considering the safety of the unmanned surface vehicle in sailing, introducing a safety cost s (v) into an evaluation function, wherein the improved evaluation function can be expressed as:
f(v)＝(1-ω)g(v)+ωs(v)+h(v)
where f (v) is the valuation function of the current grid v; g (v) is the path cost of the starting raster to the current raster v; h (v) is a heuristic function, which is an estimate of the minimum cost from the current grid v to the end grid. s (v) represents the safety cost from the starting point to the node v, the safety cost value r (v) of each node is determined according to the result after the safety evaluation of the node, if v is a complete free grid, r (v) is 0, if v is a dangerous grid, r (v) is k, k is a constant, and s (v) is the accumulated sum of the safety cost values r (v) of each path node from the starting node to the node v. . Omega is the cost ratio of safety to the length of the path, omega is more than or equal to 0 and less than 1, and the closer omega is to 1, the more important safety is. Let g (v) denote the actual cost from the origin node to the current node v, i.e. the sum of the path cost and the security cost:
G(v)＝(1-ω)g(v)+ωs(v)
in the grid map, the heuristic function h (v) mostly adopts the Manhattan distance from the current grid to the target grid.
h(v)＝|vx-tx|+|vy-ty|
In the formula, vxRepresenting the lateral coordinates, v, of a current grid v in a grid environment modelyRepresents the vertical coordinate, t, of the current grid v in the grid environment modelxRepresenting the transverse coordinates, t, of a target grid t in a grid environment modelyRepresenting the vertical coordinate of the target grid t in the grid environment.
(4.2) create the OPEN list and CLOSE list, add the start grid s to the OPEN list.
And (4.3) searching the grid with the lowest f value in the OPEN list, and marking as the current grid i. Move the grid i to the CLOSE list.
(4.4) expanding the current grid i to obtain an adjacent grid m thereof, and performing the following operations on the grid m:
first, if grid m is an obstacle grid or is already in the CLOSE list, it is skipped. Otherwise, the next steps are executed.
② if grid m is not in the OPEN list, add it to the OPEN list. The current grid i is taken as the parent grid of grid m. The f, G, and h values for grid m are recorded.
If the grid m is already in the OPEN list, with reference to the value G, it is checked whether the new path is better, if the following holds, the parent node of the node m is changed to the current node i, and the values G and f of the node m are recalculated.
G(m)＜G(i)+L(i,m)
Where L (i, m) represents the actual path cost from grid i to its neighbor grid m if grid i is directly connected to its neighbor grid m, i.e.
|ix-mx|+|iy-my|＝1
In the formula ixRepresenting the lateral coordinates, i, of a grid i in a model of the grid environmentyRepresents the vertical coordinate of grid i in the grid environment model,mxrepresenting the transverse coordinates of a grid m in a model of the grid environment, myRepresenting the vertical coordinate of the grid m in the grid environment model, and L (i, m) is the unit length of the grid; if grid i is diagonally connected to its neighbor grid m, i.e.
|ix-mx|+|iy-my|＝2
In the formula ixRepresenting the lateral coordinates, i, of a grid i in a model of the grid environmentyRepresenting the vertical coordinate, m, of grid i in the grid environment modelxRepresenting the transverse coordinates of a grid m in a model of the grid environment, myRepresents the vertical coordinate of the grid m in the grid environment model, then L (i, m) is
Figure BDA0002164675790000091
Unit length of each grid.
(4.5) repeating (4.3) - (4.4) until the target node t is added to the CLOSE list or the OPEN list is empty. Adding the target node t into a CLOSE list to show that the optimal path is found, and sequentially backtracking parent grids from the target grid until the starting grid, namely obtaining the optimal path from the starting grid to the target grid; no target node is found and the OPEN list is already empty, indicating that a path does not exist.

Claims (2)

1. A global path planning method for an unmanned ship is characterized by comprising the following steps:
s1: acquiring unmanned ship motion state information and environment perception information, wherein the motion state information comprises: coordinates(s) of current position of unmanned shipx,sy) Speed u, course angle
Figure FDA0003320007370000011
Attitude angle, environment perception information including obstacle position coordinates;
s2: establishing an environment model by adopting a grid method, comprising the following steps:
s2.1: selecting the range of the unmanned boat operation area;
s2.2: rasterizing an unmanned ship operation area, and taking the side length l of a grid as u & delta t, wherein u is the average speed of the unmanned ship, and delta t is the motion control beat of the unmanned ship;
s2.3: encoding the grid of the work area in S2.2;
s2.4: identifying obstacle elements in the operation area obtained through the sensing equipment into S2.3 coded grids, wherein the grids containing obstacles are obstacle grids, the grids not containing obstacles are free grids, the obstacle grids are marked as 0, and the free grids are marked as 1;
s3: predicting the risk degree of each free grid by adopting a K nearest neighbor learning algorithm, wherein the prediction result is that the free grid of the dangerous grid is marked as 2; the risk prediction comprises the following steps:
s3.1 for each prediction grid (x)p,yp) Selecting and predicting grids (x)p,yp) The grid with the distance less than or equal to K grid lengths forms a decision set, wherein K is a set positive integer;
s3.2, calculating the proportion epsilon of the number of the free grids to the number of the barrier grids in the decision set; if ε > p, then predict the grid as a completely free grid; if epsilon is less than or equal to p, the prediction grid is a dangerous grid, wherein p is a prediction coefficient, and p is more than or equal to 0 and less than or equal to 1;
s4: and performing path search by adopting an A-algorithm, wherein the path search comprises the following steps:
s4.1: establishing an OPEN list and a CLOSE list, and adding the starting grids to the OPEN list;
s4.2: searching the grid with the lowest evaluation function f (m) in the OPEN list, marking as the current grid i, and moving the grid i to the CLOSE list;
s4.3: expanding the current grid i to obtain all adjacent grids of the current grid i, and executing the following operations on each adjacent grid:
s4.3.1 if the adjacent grid m is an obstacle grid or is already in the CLOSE list, skip it; otherwise, executing the following steps:
s4.3.2, if the adjacent grid m is not in the OPEN list, adding the adjacent grid m to the OPEN list, using the current grid i as the parent grid of the adjacent grid m, recording the evaluation function value f (m) of the adjacent grid m, the sum G (m) of the path cost and the safety cost and the estimation h (m) of the minimum cost from the current grid m to the end grid;
the valuation function f (m) satisfies:
f(m)＝(1-ω)g(m)+ωs(m)+h(m)
wherein f (m) is an evaluation function of grid m; g (m) is the starting grid to grid m path cost; h (m) is an estimate of the minimum cost from the current grid m to the end grid; s (m) represents the safety cost from the starting point to the grid m, the safety cost value r (m) of each grid is determined according to the safety evaluation result of the grids, if m is a completely free grid, r (m) is 0, if m is a dangerous grid, r (m) is k, k is a set constant, and s (m) is the accumulated sum of the safety cost values r (m) of each path node from the starting grid to the grid m; omega is the cost ratio of safety to the length of the path, omega is more than or equal to 0 and less than 1, and the more close omega is to 1, the more important safety is;
the sum g (m) of the path cost and the security cost represents the actual cost from the origin raster to the raster m, i.e. the sum of the path cost and the security cost:
G(m)＝(1-ω)g(m)+ωs(m)
the estimate h (m) of the minimum cost from the current grid m to the end grid uses the manhattan distance of the current grid to the target grid in the grid map:
h(m)＝|mx-tx|+|my-ty|
in the formula, mxRepresents the horizontal coordinate of the current grid m in the grid environment model, myRepresents the vertical coordinate of the current grid m in the grid environment model, txRepresenting the transverse coordinates, t, of a target grid t in a grid environment modelyRepresenting the longitudinal coordinate of the target grid t in the grid environment;
s4.3.3 if the adjacent grid m is already in the OPEN list, if:
G(m)＜G(i)+L(i,m)
then the parent node of the adjacent grid m is changed to the current grid i and the values of g (m) and f (m) of the adjacent grid m are recalculated; where L (i, m) represents the actual path cost from grid i to the adjacent grid m;
if the grid i is directly connected with the adjacent grid m, the following conditions are satisfied:
|ix-mx|+|iy-my|＝1
in the formula ixRepresenting the lateral coordinates, i, of a grid i in a model of the grid environmentyRepresenting the vertical coordinate, m, of grid i in the grid environment modelxRepresenting the transverse coordinates of a grid m in a model of the grid environment, myRepresenting the vertical coordinate of the grid m in the grid environment model, and L (i, m) is the length of one grid;
if the grid i is connected with the adjacent grid m obliquely, the following conditions are satisfied:
|ix-mx|+|iy-my|＝2
in the formula ixRepresenting the lateral coordinates, i, of a grid i in a model of the grid environmentyRepresenting the vertical coordinate, m, of grid i in the grid environment modelxRepresenting the transverse coordinates of a grid m in a model of the grid environment, myRepresents the vertical coordinate of the grid m in the grid environment model, then L (i, m) is
Figure FDA0003320007370000031
A grid length;
s4.4: repeating S4.2-S4.3 until the target node t is added into the CLOSE list or the OPEN list is empty; when the target node t is added into the CLOSE list, the optimal path is found, parent grids are sequentially traced from the target grid until the starting grid, and the optimal path from the starting grid to the target grid is obtained; no target node is found and the OPEN list is already empty, indicating that a path does not exist.
2. The unmanned ship global path planning method according to claim 1, characterized in that: in the S1, the motion state information is acquired through a GPS and an electronic compass, and the environment perception information is acquired through a laser radar.
CN201910743186.2A 2019-08-13 2019-08-13 Unmanned ship global path planning method Active CN110398250B (en)

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 CN110398250A (en) 2019-11-01
CN110398250B true 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)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110703762B (en) * 2019-11-04 2022-09-23 东南大学 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
CN112068564B (en) * 2020-09-10 2022-09-06 中国船舶科学研究中心 Intelligent ship control method based on economic navigation optimization
CN112113573B (en) * 2020-09-18 2021-08-10 武汉理工大学 Planning method for coverage path of single unmanned measurement boat
CN112161631B (en) * 2020-12-01 2021-02-19 湖北第二师范学院 Safe path planning method based on large satellite grid map
CN113419535B (en) * 2021-07-05 2024-02-27 鹏城实验室 Dual-boat path planning method, device, equipment and computer readable storage medium
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
CN113885531B (en) * 2021-11-05 2024-06-21 上海肇观电子科技有限公司 Method for mobile robot, circuit, medium, and program
CN115615447A (en) * 2022-09-20 2023-01-17 泰州市金海运船用设备有限责任公司 Optimal path prediction method, device and equipment
CN117093012B (en) * 2023-08-04 2024-02-02 广东工业大学 Underwater robot path planning method for improving A-algorithm in ocean environment

Citations (4)

* Cited by examiner, † Cited by third party
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
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

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180070212A1 (en) * 2016-09-02 2018-03-08 Athentek Innovations, Inc. Systems and methods to track movement of a device in an indoor environment

Patent Citations (4)

* Cited by examiner, † Cited by third party
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
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)

* Cited by examiner, † Cited by third party
Title
Guidance and Control of An Unmanned Surface Vehicle Exhibiting Sternward Motion;Aditya S. Gadre et al.;《MTS/IEEE Oceans Conference》;20121231;全文 *
无人水面艇自适应路径跟踪算法;朱骋等;《导航与控制》;20190228;全文 *

Also Published As

Publication number Publication date
CN110398250A (en) 2019-11-01

Similar Documents

Publication Publication Date Title
CN110398250B (en) Unmanned ship global path planning method
CN112650237B (en) Ship path planning method and device based on clustering processing and artificial potential field
CN108681321B (en) Underwater detection method for unmanned ship cooperative formation
CN107356254B (en) Particle swarm optimization method suitable for geomagnetic-assisted navigation track planning
CN110956853B (en) Multi-ship collision prediction method, system and storage medium
CN103900573B (en) A kind of underwater research vehicle multiple constraint Route planner based on S57 standard electronic sea chart
CN107942095B (en) Wave glider actual heading prediction technique
CN111721296B (en) Data driving path planning method for underwater unmanned vehicle
CN109374004A (en) A kind of Intelligent unattended ship paths planning method based on IA* algorithm
CN111412918B (en) Unmanned ship global safety path planning method
Chen et al. Research on Ship Meteorological Route Based on A‐Star Algorithm
CN115167474A (en) Mobile robot path planning optimization method
CN112880678A (en) Unmanned ship navigation planning method in complex water area environment
CN115615447A (en) Optimal path prediction method, device and equipment
JP4699598B2 (en) Data processing apparatus and storage medium operating as problem solver
CN117193296A (en) Improved A star unmanned ship path planning method based on high safety
CN111176281A (en) Multi-surface unmanned ship coverage type collaborative search method and system based on quadrant method
Al-Sabban et al. Extending persistent monitoring by combining ocean models and Markov decision processes
CN117007067A (en) River course inspection unmanned aerial vehicle path planning method based on A star algorithm
Lin et al. Bounded cost htn planning for marine autonomy
CN113776535A (en) Unmanned ship route planning method based on rasterized electronic chart
CN114019967A (en) Unmanned ship route planning method suitable for long and narrow channel
JP2022123401A (en) Route planning device, moving object, route planning method and program
CN113160614A (en) Ice zone near-field route dynamic optimization method
CN113110530A (en) Underwater robot path planning method for three-dimensional environment

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