CN106441303A - Path programming method based on A* algorithm capable of searching continuous neighborhoods - Google Patents

Abstract

The invention provides a path programming method based on an A* algorithm capable of searching continuous neighborhoods. The path programming method comprises the following steps: establishing an environment model by using a grid method according to the geometrical size of an existing barrier, and treating a circular barrier by considering UUV as a mass point, taking the maximum width of the barrier as a diameter and taking the center of gravity of the barrier as the original point; acquiring the grid size l according to the information of the barrier; determining an evaluation function f (x) of the A* algorithm according to an established grid map; determining evaluation cost h(y) of an optional point y according to features of neighborhoods and the A* algorithm; and finding out a node of the minimum evaluation function fmin of the neighborhoods according to the evaluation function of the A* algorithm capable of searching the continuous neighborhoods as a next way point, and implementing a UUV sea route plan step by step. By the path programming method based on the A* algorithm capable of searching continuous neighborhoods, the problem that an existing UUV path planning method has path smoothness difference and a non-shortest path in a global environment is solved.

Description

A kind of based on the paths planning method that can search for continuous neighborhood A* algorithm

Technical field

The present invention relates to a kind of UUV global path planning method.

Background technology

UAV navigation (Unmanned underwater vehicle, UUV) as a kind of high-tech means, Vital effect is played in the following valuable development space of this block of ocean, and path planning is autonomous underwater navigation One of key technology of device, is that Autonomous Underwater Vehicle can safely, effectively be travelled in residing environment, smoothly complete to Determine the important guarantee of task.

UUV global path planning essence is in planning region, under the conditions of given barrier and constraint, finds one Optimum or feasible path from starting point to impact point.Traditional path planning algorithm, such as Artificial Potential Field Method, graph search method, In place of particle cluster algorithm etc. has some shortcomings mostly, be for example easily trapped into local minimum and computationally intensive the problems such as.With grid On the basis of method environmental modeling, UUV global path planning is carried out using the A* algorithm that can search for continuous field, effectively can be solved Certainly tradition UUV path planning algorithm be easily trapped into local best points, computationally intensive the problems such as.

Content of the invention

It is an object of the invention to provide one kind can shorten UUV path, saving time, carry efficient based on the company of can search for The paths planning method of continuous neighborhood A* algorithm.

The object of the present invention is achieved like this：

Step one, according to exist barrier using Grid Method set up environmental model acquisition environmental model, to UUV being The circular barrier that particle, the most long width of barrier are diameter, the center of gravity of barrier is initial point, using formulaObtain the area S of each circle barrier_i, wherein l_iMaximum gauge for the i-th barrier；

Step 2, the area of each circle barrier for being obtained according to step one, determine grid size l；

Wherein：S is expressed as the gross area of environment, l_maxIt is barrier maximal side, l_min=min (l_minobs,d_r) it is obstacle The minimum length of side of thing, d_rIt is UUV diameter, l is the final grid length of side for determining；

Step 3, the evaluation function of A* algorithm are：

F (x)=g (x)+h (x)

Wherein, f (x) is the evaluation function from initial point via x node to impact point, evaluation function for f, g (x) be in shape From initial point to the actual cost of x node in state space, actual cost for g, h (x) be from x node to impact point optimal path Estimate cost, appraisal cost is h；

Step 4, the evaluation function of the A* algorithm for being obtained according to step 3, the sideline of each grid is defined as may have access to Node, the appraisal cost that can search for field A* algorithm is

H (y)=d × h (X₂)+(l-d)×h(X₁) d∈[0 l]

Wherein, h (X₁) it is nodes X₁Estimate cost, h (X₂) it is nodes X₂Estimate cost, y is point X₂X₁On line segment Any point, d is point y to X₁Distance, X₁,X₂...X₈It is the node of the adjacent eight sideline intersection point of present node X；

Step 5, the g value that can search for continuous field A* algorithm, f value and the h value that are obtained according to step 4, sequentially find excellent The node of change, till impact point, this paths is shortest path.

The present invention is that UUV has smoothness difference in path planning in the global context of ocean and path is non-most in order to solve Short problem and propose.

Beneficial effects of the present invention：

By area and whole environment area shared by barrier is calculated, determine the grid size of grid environment, both avoided The problem that the data of Computer real-time processing and storage increase, also improves the accuracy of UUV path planning.

By traditional A* algorithm is combined with can search for continuous field, UUV path is shortened, the time has been saved, has been carried High efficiency.

Description of the drawings

Fig. 1 is grid environmental modeling schematic diagram in the present invention；

Fig. 2 is to can search for, in the present invention, the schematic diagram that field node is sideline intersection point；

Fig. 3 is the adjacent sideline schematic diagram vertical with current sideline for can search for field node in the present invention；

Fig. 4 is the adjacent sideline schematic diagram parallel with current sideline for can search for field node in the present invention；

Fig. 5 is UUV Path Planning Simulation figure in the present invention.

Specific embodiment

Illustrate below and the present invention is described in more detail.

In conjunction with Fig. 1, Fig. 2, Fig. 3, Fig. 4 and Fig. 5, the one kind described in present embodiment is based on and can search for continuous neighborhood A* algorithm Paths planning method, the concretely comprising the following steps of the method：

Step one：According to there is barrier, the environmental model that environmental model is obtained being set up using Grid Method, selecting on map Any one barrier is taken, and judges whether which is circular；If it is not, then selecting in all summits of i-th barrier The maximum of coordinate and minima x under environmental map coordinate system_max,y_max,x_min,y_min, then compare | | x_max-x_min| | with | | y_max-y_min| | maximum gauge is obtained, finally circular barrier is formed as initial point with center of gravity.Using formula：

Obtain the area S of each circle barrier_i, l_iMaximum gauge for the i-th barrier.

Step 2：The size of computation grid is

Wherein, S is expressed as the gross area of environment, l_maxIt is barrier maximal side, l_min=min (l_minobs,d_r) it is obstacle The minimum length of side of thing, d_rIt is the diameter of mobile robot, l is the final grid length of side for determining.

Do not consider the elevation information of UUV, working environment is represented with two dimensional surface, the east-west direction of wherein working environment is X-axis, North and South direction is Y-axis, to set up rectangular coordinate system XOY, if the horizontal and vertical maximum correspondence of two dimensional surface is sat in plane It is X in mark system_maxAnd Y_max, due to the square that the grid length of side for defining is l.

Step 3：According to the grid map that has set up, the evaluation function of A* algorithm is：

F (x)=g (x)+h (x) (4)

Wherein：X is node, and f (x) is the evaluation function from initial point via node to impact point, and g (x) is empty in state Between in actual cost from initial point to node, h (x) is estimate cost of the from node to impact point optimal path.When h (x) estimates Meter cost be not more than node to impact point actual distance value when, the points of search are many, and efficiency is low, but can obtain optimal solution； When h (x) is more than the actual distance value of node to impact point, the points of search are few, efficiency high, but it cannot be guaranteed that obtain optimum Solution；Euclidean distance air line distance in actual environment, choosing between two nodes) as estimate cost, i.e.,

Wherein (x_G,y_G) be impact point G coordinate, (x_i,y_i) it is arbitrary node X_iCoordinate.

Step 4：The evaluation function of the A* algorithm for being obtained according to step 3, A* algorithm is mutually tied with can search for continuous neighborhood Close, make the sideline of each grid that addressable node is defined as, estimate cost value h (y) of the arbitrfary point y on one section of sideline is which The linear combination of two-end-point assessment values, i.e.,

H (y)=d × h (X₂)+(l-d)×h(X₁) d∈[0 l] (6)

Wherein, h (X₁) it is nodes X₁Estimate cost, h (X₂) it is nodes X₂Estimate cost, y is point X₂X₁On line segment Any point, d is point y to X₁Distance, X₁,X₂...X₈It is the node of the adjacent eight sideline intersection point of present node X.For Sideline XX_iOn cost be min (a, b).Its child node can be any one in these adjacent nodes, then may move side Also just expand for continuous any direction to angle.G value, f value for different nodes of locations y is different with h value.

When nodes X is the situation of sideline intersection point, nodes X to sideline X₂X₁The cost of upper arbitrary node y can be expressed as

And

Wherein, g (X) be in state space from initial point to the actual cost of X node, g (y) be in state space from Initial point is to the actual cost of y node, and a, b are sideline XX_iOn cost.

When nodes X is not sideline intersection point but during with this section of vertical situation in sideline, sideline nodes X to sideline X₂X₁Take up an official post The cost of meaning node y can be expressed as

Wherein, t ∈ [0, l] and

When nodes X is not sideline intersection point node but during with this section of parallel situation in sideline, sideline nodes X to sideline X₂X₃ The cost of upper arbitrary node y can be expressed as

Wherein m ∈ [0, l], and

By the calculating of above different situations, can compare and obtain f_minAnd g.

Step 5：Create two tables：Open table and closed table, open table is not investigated for preserving all own generations Node, closed table accessed, for recording oneself, the node for being not required to investigate again.Then according to below step is operated：

Starting point A is added in open table, and by the grid point X adjacent with A point₁,X₂...X₈And appointing on sideline Meaning point y adds open and enters table.Starting point A is moved on to closed table from open table, and be defined as the grid of its adjacent extension Father node.The g value of grid point, f value and h value in open table are solved by formula (7,8,9,10,11,12).Select from open table The grid point of f value minimum is selected, present node X is designated as, and which is moved on to closed table from open list.Investigate respectively and work as prosthomere All adjacent nodes of point X (are designated as S, such as fruit dot is added open and enters neither in closed table nor in open table Table, solves its g value, f value and h value, and juxtaposition present node X is its father node；If S point oneself through being present in open table, compare Compared with the size of g (X)+c (X, S) (wherein, c (X, S) is the cost from X point to S point) and g (S), as g (X)+c (X, S) ＜ g (S) When, g (S)=g (X)+c (X, S), and the accordingly f value of renewal point S being made, and point X is put for its father node, otherwise, does not then do any place Reason；If S is also left intact Already in closed table.Repeat above-mentioned until present node X for impact point G be Only.From the beginning of impact point G, its father node is sequentially found, till starting point A, this paths is the Algorithm for Solving and goes out most Short path.

Instantiation：

UUV is navigated by water to terminal (80,80), 45 ° of original heading from starting point (10,10), devises multiple and barrier O₁, O₂...., the area of barrier being obtained using formula (1), obtains the size of grid using formula (2), is obtained using formula (3) In evaluation function.Then, found using the evaluation function that can search for continuous neighborhood A* algorithm and can search for continuous neighborhood evaluation function Minimum node, as next way point, is done step-by-step UUV routeing.Simulation track is as shown in Figure 5.

Claims (5)

1. a kind of based on the paths planning method that can search for continuous neighborhood A* algorithm, it is characterized in that：

Step one, according to exist barrier using Grid Method set up environmental model acquisition environmental model, to UUV as matter The circular barrier that point, the most long width of barrier are diameter, the center of gravity of barrier is initial point, using formulaObtain the area S of each circle barrier_i, wherein l_iMaximum gauge for the i-th barrier；

Step 2, the area of each circle barrier for being obtained according to step one, determine grid size l；

$obs=\underset{i=1}{\overset{n}{\Σ}}{S}_i$

$l_{temp}=\frac{S_{obs}}{S}\×l_{max}$

$l=\left\{\begin{array}{cc}{l}_{temp},& if{l}_{temp}>l_{min}\\ l_{\min },& else\end{array}\right.$

Wherein：S is expressed as the gross area of environment, l_maxIt is barrier maximal side, l_min=min (l_{min obs},d_r) it is barrier The minimum length of side, d_rIt is the diameter of UUV, l is the final grid length of side for determining；

Step 3, the evaluation function of A* algorithm are：

F (x)=g (x)+h (x)

Wherein, f (x) is the evaluation function from initial point via x node to impact point, and evaluation function is empty in state for f, g (x) Between in from initial point to the actual cost of x node, actual cost is estimation that g, h (x) are from x node to impact point optimal path Cost, appraisal cost is h；

Step 4, the evaluation function of the A* algorithm for being obtained according to step 3, the sideline of each grid is defined as addressable section Point, the appraisal cost that can search for field A* algorithm is

H (y)=d × h (X₂)+(l-d)×h(X₁) d∈[0 l]

Wherein, h (X₁) it is nodes X₁Estimate cost, h (X₂) it is nodes X₂Estimate cost, y is point X₂X₁Any on line segment A bit, d is point y to X₁Distance, X₁,X₂...X₈It is the node of the adjacent eight sideline intersection point of present node X；

Step 5, the g value that can search for continuous field A* algorithm, f value and the h value that are obtained according to step 4, sequentially find optimization Node, till impact point, this paths is shortest path.

2. according to claim 1 based on the paths planning method that can search for continuous neighborhood A* algorithm, it is characterized in that estimating Cost h (x) is：

$h\left(x\right)=\sqrt{{(x_G-x_i)}^2+{(y_G-y_i)}^2}$

(x_G,y_G) be impact point G coordinate, (x_i,y_i) it is arbitrary node X_iCoordinate.

3. according to claim 1 and 2 based on the paths planning method that can search for continuous neighborhood A* algorithm, it is characterized in that： When nodes X is sideline intersection point, nodes X to sideline X₂X₁The cost of upper arbitrary node y is expressed as

$f\left(y\right)=\left\{\begin{array}{c}g\left(X\right)+a\·\sqrt{l^2+d^2}+d\·h\left(X_2\right)+(l-d)\·h\left(X_1\right),y\≠X_1\\ g\left(X\right)+min\{a,b\}+h\left(X_1\right),y=X_1\end{array}\right.$

And

$g\left(y\right)=\left\{\begin{array}{c}g\left(X\right)+a*\sqrt{l^2+d^2},y\≠X_1\\ g\left(X\right)+\min \{a,b\},y=X_1\end{array}\right.$

Wherein, g (X) be in state space from initial point to the actual cost of X node, g (y) be in state space from initial Point is to the actual cost of y node, and a, b are sideline XX_iOn cost；

When nodes X is not sideline intersection point but is vertical with this section of sideline, sideline nodes X to sideline X₂X₁The generation of upper arbitrary node y Valency is expressed as

$f\left(y\right)=\left\{\begin{array}{c}g\left(X\right)+a\·\sqrt{t^2+d^2}+d\·h(X_2+(l-d)\·h(X_1),y\≠X_1\\ g\left(X\right)+min\{a,b\}\·t+h\left(X_1\right),y=X_1\end{array}\right.$

Wherein, t ∈ [0, l] and

$g\left(y\right)=\left\{\begin{array}{c}g\left(X\right)+a*\sqrt{t^2+d^2},y\≠X_1\\ g\left(X\right)+\min \{a,b\}*t,y=X_1\end{array}\right.$

When nodes X is not sideline intersection point node but is parallel with this section of sideline, sideline nodes X to sideline X₂X₃Upper arbitrary node y Cost be expressed as

$f\left(y\right)=g\left(X\right)+a*\sqrt{l^2+{(t-m)}^2}+m*h\left(X_3\right)+h\left(X_2\right)*(l-m)$

Wherein m ∈ [0, l], and

$g\left(y\right)=g\left(X\right)+a*\sqrt{l^2+{(t-m)}^2}.$

4. according to claim 1 and 2 based on the paths planning method that can search for continuous neighborhood A* algorithm, it is characterized in that step Rapid five specifically include：Create two tables：Open table and closed table, open table is used for preserving all sections that oneself generates and does not investigate Point, closed table is used for record and had accessed the node for being not required to investigate again, then according to below step is operated：

Starting point A is added in open table, and by the grid point X adjacent with A point₁,X₂...X₈And the arbitrfary point y on sideline Add open and enter table；Starting point A is moved on to closed table from open table, and is defined as father's section of the grid of its adjacent extension Point, solves the g value of grid point, f value and h value in open table, selects the grid point of f value minimum, be designated as working as prosthomere from open table Point X, and which is moved on to closed table from open list, all adjacent nodes for investigating present node X respectively are designated as S, if Point is added open and is entered table, solved its g value, f value and h value neither in closed table nor in open table, and juxtaposition is current Nodes X is its father node；If S point is Already in open table, compare the size of g (X)+c (X, S) and g (S), c (X, S) it is cost from X point to S point, as g (X)+c (X, S) ＜ g (S), g (S)=g (X)+c (X, S) is made, and accordingly updates point S F value, and put point X for its father node, otherwise, be then left intact；If S is not also done Already in closed table Any process, repeat above-mentioned until present node X be impact point G till, from the beginning of impact point G, sequentially find its father node, directly To starting point A, this paths is shortest path.

5. according to claim 3 based on the paths planning method that can search for continuous neighborhood A* algorithm, it is characterized in that step Five specifically include：Create two tables：Open table and closed table, open table is used for preserving all sections that oneself generates and does not investigate Point, closed table is used for record and had accessed the node for being not required to investigate again, then according to below step is operated：

Starting point A is added in open table, and by the grid point X adjacent with A point₁,X₂...X₈And the arbitrfary point y on sideline Add open and enter table；Starting point A is moved on to closed table from open table, and is defined as father's section of the grid of its adjacent extension Point, solves the g value of grid point, f value and h value in open table, selects the grid point of f value minimum, be designated as working as prosthomere from open table Point X, and which is moved on to closed table from open list, all adjacent nodes for investigating present node X respectively are designated as S, if Point is added open and is entered table, solved its g value, f value and h value neither in closed table nor in open table, and juxtaposition is current Nodes X is its father node；If S point is Already in open table, compare the size of g (X)+c (X, S) and g (S), c (X, S) it is cost from X point to S point, as g (X)+c (X, S) ＜ g (S), g (S)=g (X)+c (X, S) is made, and accordingly updates point S F value, and put point X for its father node, otherwise, be then left intact；If S is not also done Already in closed table Any process, repeat above-mentioned until present node X be impact point G till, from the beginning of impact point G, sequentially find its father node, directly To starting point A, this paths is shortest path.