CN105843234A - Two-dimensional route planning method for UUV to geometrically bypass circular barriers - Google Patents

Abstract

The invention provides a two-dimensional route planning method for a UUV to geometrically bypass circular barriers. The method comprises the following steps: 1, reading a route origin Ob, a route terminal point Oe and parameters of each circular barrier from a mission text; 2, performing expansion processing on each circular barrier, and calculating the parameters of each circular barrier after expansion; 3, establishing a bypassing point set S, making a planning current point Oc to be a starting point Ob, and placing the starting point in the bypassing point set S; 4, if the planning current point Oc is the route terminal point Oe, or the planning current point Oc and the route terminal point O2 are visual, skipping to the sixth step, and otherwise, executing the fifth step; 5, performing geometric bypassing on a circular barrier closest to the planning current point Oc, obtaining a bypassing point, placing the bypassing point in the bypassing point set S, updating the planning current point Oc, and skipping to the fourth step; and 6, placing the route terminal point Oe in the bypassing point set S, and at the moment, planning ends. According to the invention, bypassing of the circular barriers is realized through a simple geometric principle, and the UVV can rapidly and efficiently obtain a safe bump-free two-dimensional route in a complex environment with multiple circular barriers.

Description

The two-dimentional Route planner that circular barrier geometry is detoured by a kind of UUV

Technical field

The present invention relates to one is UUV Route planner, particularly a kind of UUV to circular barrier geometry around The two-dimentional Route planner of row.

Background technology

Routeing be underwater unmanned vehicle (Unmanned Underwater Vehicle, UUV) key technology it One, it is the important embodiment of UUV capacity of will.Routeing refers to, under known obstacle environment, cook up one from starting point Walk around all barriers and reach home without touching path.According to Spatial Dimension, routeing can be divided into two dimension routeing and Three-dimensional routeing.Wherein, two dimension routeing is the basis of three-dimensional routeing, and the most extensive in the application of UUV, It it is the focus of UUV routeing technical research.At present, the Route planner of UUV is a lot, but how at complicated obstacle ring Under border not only quickly but also feasible one nothing of acquisition touches path, particularly planing method can be adapted to engineer applied, remains one Difficult point.

Prior art related to the present invention is " submarine navigation device path planning based on geometric algorithm " (" naval engineering College journal ", page 2009,21 (6): 41-44), it is mentioned that consider that during circular barrier, submarine navigation device is calculated based on geometry The path planning of method, but the Route planner that circular barrier is detoured by the document is different from the present invention.

Summary of the invention

It is an object of the invention to provide a kind of calculate simple, planning efficiency is high, plan fireballing UUV to circular obstacle The two-dimentional Route planner that thing geometry detours.

The object of the present invention is achieved like this:

Step one: from mission Reading text air route starting point O_b, air route terminal O_eParameter with each circular barrier；

Step 2: each circular barrier is carried out expansion process, calculates the parameter of each circular barrier after expanding；

Step 3: set up a set S that detours, O is currently put in order planning_cFor starting point O_b, and put into and detour in set S；

Step 4: if O is currently put in planning_cIt is air route terminal O_e, or planning currently put O_cWith air route terminal O_eVisually, Go to step six, otherwise perform step 5；

Step 5: currently put O to away from planning_cNearest circular barrier carries out geometry and detours, and obtains detouring a little and puts into Detouring in a set S, Regeneration planning currently puts O_c, go to step four；

Step 6: by air route terminal O_ePutting into and detour in a set S, planning terminates.

The present invention can also include:

1, circular barrier carries out the method that geometry detours is:

(1), judge that O is currently put in planning_cWhether on circular barrier, if, order point O₁=O_c；Otherwise, planning is solved Currently put O_cWith the intersection point of the line of center of circle O with circumference, and to make it be an O₁；

(2) air route terminal O, is solved_eWith the intersection point of the line of center of circle O with circumference, and to make it be an O₂；

(3) ∠ O, is solved₁OO₂The intersection point of angular bisector and circumference, and to make it be an O₃；

(4) some O, was solved₁Circle tangent line with cross some an O₃The intersection point of tangent line of circle, and to make it be an O₄；

(5) some O, was calculated₂Circle tangent line with cross some an O₃The intersection point of tangent line of circle, and to make it be an O₅；

(6), will an O₁, some O₄, some O₅, some O₂Detour that a little putting into detours gathers in S as circular barrier, and more New planning currently puts O_cFor an O₂, end of detouring.

2, the method that circular barrier is carried out expansion process is to terminate the basis of barrier geometry in normal planning On, according to barrier shape edges with radius of safety r_uuvOutward expansion goes out a radius of safety region.

The present invention utilizes geometrical principle to carry out the two-dimentional routeing of UUV, have employed simple geometry on environmental model Model, uses the simple geometrical principle to detour barrier when calculating without touching path, it is to avoid other planing methods need Map to be set up, cyclic search are without touching the problem of the complexity that contains much information, calculates caused by path, and not only planning efficiency is high, rule Draw speed fast, and principle is simple, amount of calculation is little, it is easy to Project Realization.

The present invention and background technology " submarine navigation device path planning based on geometric algorithm (" naval engineering college journal ", Page 2009,21 (6): 41-44) " differ primarily in that:

1, the present invention has carried out expansion process to circular barrier, and background technology " underwater navigation based on geometric algorithm Device path planning " do not carry out expansion process.Circular barrier is carried out expansion process, it is to avoid UUV is along the navigation of planning air route Time with the collision of circular barrier, improve the nevigation safety of UUV.

2, the present invention and background technology " submarine navigation device path planning based on geometric algorithm " are to single circular barrier The method that detours different.The method that detours of the present invention as shown in Figure 4 and Figure 5, formation be a plurality of line segment composition air route of detouring； And background technology is formed is line segment and air route of detouring that circular arc combines, and control is followed the tracks of in the air route that circular arc air route is unfavorable for UUV System.The present invention only has the air route of detouring of line segment to be more conducive to the air route tracing control of UUV, can improve the air route tracking effect of UUV.

The beneficial effects of the present invention is:

1, environmental model uses geometric space model, compared to traditional grid, cartographic model, required planning information Amount is few, and planning efficiency is high, is particularly suitable for the environment of complicated multi-obstacle avoidance.

2, circular barrier has been applied only to geometrical principle around line algorithm, calculate simple, be prone to Project Realization, and Amount of calculation is the least, and planning speed is fast.

3, for radius of safety, circular barrier has been carried out expansion process with the overall dimensions of UUV, it is to avoid UUV is along planning With the collision of circular barrier during the navigation of air route, improve the nevigation safety of UUV.

4, the air route of detouring formed only has line segment, the air route tracing control of beneficially UUV, and the air route that can improve UUV is followed the tracks of Effect.

Accompanying drawing explanation

Circular barrier schematic diagram in Fig. 1 planning environment model；

The radius of safety schematic diagram of Fig. 2 circle barrier expansion process；

The expansion schematic diagram of Fig. 3 circle barrier；

Fig. 4 plans the schematic diagram that when current point is not on circular barrier detours circular barrier；

Fig. 5 plans the schematic diagram that when current point is on circular barrier detours circular barrier；

The flow chart that the geometry of circular barrier is detoured by Fig. 6 UUV；

Fig. 7 judges that be linked to be the flow chart whether line segment intersects with circular barrier at 2；

The two-dimentional routeing flow chart that circular barrier geometry is detoured by Fig. 8 UUV；

Fig. 9 utilizes the present invention to carry out the design sketch that the geometry of circular barrier is detoured by UUV.

Detailed description of the invention

The present invention is described in detail in citing below.

The environmental model of UUV two dimension routeing is introduced in conjunction with Fig. 1.

In the present invention, the environmental model of routeing uses two-dimensional geometry spatial model.If the air route starting point of planning is O_b, air route terminal is O_e, O_bAnd O_eIt is expressed as with two-dimensional coordinate respectively:

O_b=(x_ob,y_ob)；O_e=(x_oe,y_oe) (1)

The planning that during separately setting routeing, each step is used currently is put as O_c, it is expressed as with two-dimensional coordinate:

O_c=(x_oc,y_oc) (2)

If two-dimensional geometry space exists a number of circular barrier, as it is shown in figure 1, set circular barrier as Z_circ, its parametrization is expressed as:

Z_circ=(x_circ,y_circ,r) (3)

In formula, (x_circ,y_circ) representing the two-dimensional coordinate in the center of circle, r represents the radius of circular barrier.

The method for building up of circular barrier inflationary model is introduced in conjunction with Fig. 2 and Fig. 3.

When carrying out routeing, usually UUV being considered as particle, the air route therefore planned may be away from obstacle Thing is nearer.But it practice, UUV is the entity having physical dimension, when the air route of planning is when barrier is nearer, it is more likely that lead Cause UUV to collide with barrier.For this when carrying out routeing, a radius of safety is set to prevent UUV along planning boat Collide with barrier during the navigation of road.The method that the present invention uses is circumradius r of the overall dimensions with UUV_uuvFor peace Full radius (as shown in Figure 2), then on the basis of the geometry of normal circular barrier, according to its shape edges with radius r_uuvOutward expansion goes out a radius of safety region.Fig. 3 gives the expansion signal after circular barrier outward expansion radius of safety Figure.

After expansion, the parametrization of circular barrier is expressed as:

Z′_circ=(x_circ,y_circ,r′) (4)

In formula: (x_circ,y_circ) still represent the two-dimensional coordinate in the center of circle；And r '=r+r_uuvRepresent the circular obstacle after expanding Thing radius.

Introduce UUV in conjunction with Fig. 4, Fig. 5, Fig. 6 circular barrier to be detoured method.

Circular barrier is detoured and is divided into the current point of planning on circular barrier and not on circular barrier two kinds Situation, Fig. 4 gives the planning current point not schematic diagram that detours on circular barrier, and Fig. 5 gives the current point of planning to be justified The schematic diagram that detours on shape barrier.As can be seen from Figure 4 and Figure 5, circular barrier is detoured, use utilize a plurality of The intersection point of tangent line of circle a little detours as detouring the method for circle barrier, and utilizes geometrical principle to solve respectively to detour a little.

Fig. 6 gives the flow process that detours of circular barrier.

Step one: judge that O is currently put in planning_cWhether on barrier, if on barrier, make O₁=O_c, go to step Three；Otherwise perform step 2；

Step 2: solve planning and currently put O_cLine and round intersection point O with center of circle O₁=(x_O1,y_O1), O₁Position sit Mark is calculated by formula (5), has two solutions, chooses and currently puts O_cApart near solution as some O₁Coordinate.

In formula: k_cRepresent that O is currently put in planning_cThe slope of straight line connected with center of circle O, and have

Step 3: solve air route terminal O_eLine and round intersection point O with center of circle O₂=(x_O2,y_O2), O₂Position coordinates Calculate by formula (6), have two solutions, choose and air route terminal O_eApart near solution as some O₂Coordinate.

In formula: k_eRepresent air route terminal O_eThe slope of straight line connected with center of circle O, and have

Step 4: solve ∠ O₁OO₂Angular bisector L₀Intersection point O with circle₃=(x_O3,y_O3), O₃Position coordinates by formula (7) calculate, have two solutions, choose and put O₁(or some O₂) apart near solution as some O₃Coordinate.

In formula:Represent ∠ O₁OO₂Angular bisector L₀Slope, and have

Step 5: solved some O respectively₁、O₂、O₃The tangent line L of circle₁、L₂、L₃, its tangential equation is respectively by formula (8), formula (9) calculate with formula (10).

$L_1:\left\{\begin{array}{c}y=k_{L1}x+b_{L1}\\ k_{L1}=-1/k_c\\ b_{L1}=y_{O1}-k_{L1}{x}_{O1}\end{array}\right.---\left(8\right)$

$L_2:\left\{\begin{array}{c}y=k_{L2}x+b_{L2}\\ k_{L2}=-1/k_e\\ b_{L2}=y_{O2}-k_{L2}{x}_{O2}\end{array}\right.---\left(9\right)$

$L_3:\left\{\begin{array}{c}y=k_{L3}x+b_{L3}\\ k_{L3}=-1/k_{L0}\\ b_{L3}=y_{O3}-k_{L3}{x}_{O3}\end{array}\right.---\left(10\right)$

In formula: k_L1、b_L1Represent round tangent line L respectively₁Slope and intercept；k_L2、b_L2Represent round tangent line L respectively₂'s

Slope and intercept；k_L3、b_L3Represent round tangent line L respectively₃Slope and intercept.

Step 6: calculate tangent line L₁And L₃Intersection point O₄=(x_O4,y_O4), O₄Position coordinates by formula (11) calculate

$x_{O_4}=\frac{b_{L3}-b_{L1}}{k_{L1}-k_{L3}};y_{O_4}=\frac{k_{L1}(b_{L3}-b_{L1})}{k_{L1}-k_{L3}}+b_{L1}---\left(11\right)$

Step 7: calculate tangent line L₂And L₃Intersection point O₅=(x_O5,y_O5), O₅Position coordinates by formula (12) calculate

$x_{O_5}=\frac{b_{L3}-b_{L2}}{k_{L2}-k_{L3}};y_{O_5}=\frac{k_{L2}(b_{L3}-b_{L2})}{k_{L2}-k_{L3}}+b_{L2}---\left(12\right)$

Step 8: will an O₁、O₄、O₅、O₂Detouring a little to put into and detour in a set S as circular barrier, and update ought Front some O_cFor O₂, end of detouring.

Introduce judgement planning in conjunction with Fig. 7 and currently put O_cWith air route terminal O_eThe most visual method.

Point O_cWith an O_eRefer to that do not stopped by any circular barrier at 2.Judge that 2 methods the most visual are exactly Judge whether the line segment that 2 lines are formed intersects with all of circular barrier, if do not intersected with any circular barrier Then show at 2 visually.Judge that O is currently put in planning_cWith air route terminal O_eThe most visual flow process is as shown in Figure 7:.

Step one: select first circular barrier；

Step 2: solve and currently put O_cWith air route terminal O_eThe discriminant of the root of the quadratic equation of 2 lines and circle simultaneous Δ, shown in method for solving such as formula (13):

$\mathrm{\Δ}={({2k}_{\mathrm{ce}}{b}_{\mathrm{ce}}-2x_{\mathrm{circ}}-2y_{\mathrm{circ}}{k}_{\mathrm{ce}})}^2-4(1+k_{\mathrm{ce}}^2)(x_{\mathrm{circ}}^2+b_{\mathrm{ce}}^2+y_{\mathrm{circ}}^2-2y_{\mathrm{circ}}{b}_{\mathrm{ce}}-r^2)---\left(13\right)$

In formula, k_ceAnd b_ceRepresent some O respectively_cWith an O_eConnected straight slope and intercept, and hadb_ce=y_oc-k_cex_oc；

Step 3: judge that the discriminant Δ of root, whether more than or equal to 0, goes to step four if greater than equal to 0, otherwise goes to step Six

Step 4: solve and currently put O_cWith air route terminal O_eThe abscissa x of two intersection points of 2 lines and circle_pc1And x_pc2, Shown in method for solving such as formula (14):

$x_{pc1},x_{pc2}=\frac{-(2k_{ce}{b}_{ce}-2x_{circ}-2y_{circ}{k}_p)\±\sqrt{\mathrm{\Δ}}}{2(1+k_{ce}^2)}---\left(14\right)$

Step 5: judge x_pc1Value at x_ocAnd x_oeBetween or x_pc2Value is at x_ocAnd x_oeBetween whether meet, if meet turn Step 7, otherwise goes to step six；

Step 6: judge whether also circular barrier, if it has, select next circular barrier, goes to step two, no Then go to step eight；

Step 7: currently put O_cWith air route terminal O_eNot visible, it is judged that to terminate；

Step 8: currently put O_cWith air route terminal O_eVisually, it is judged that terminate.

The UUV whole flow process to the two-dimentional routeing that circular barrier geometry detours is introduced in conjunction with Fig. 8.

Step one: from mission Reading text air route starting point O_b, air route terminal O_eParameter with each circular barrier；

Step 2: set up each circular barrier inflationary model, calculates the parameter of each circular barrier after expanding, set up around Row point set S；

Step 3: O is currently put in order planning_cFor starting point O_b, and put into and detour in set S；

Step 4: judge that O is currently put in planning_cIt is air route terminal O_e, if going to step ten, otherwise go to step five；

Step 5: judge that O is currently put in planning_cWith air route terminal O_eThe most visual, if visually going to step ten, otherwise turn step Rapid six；

Step 6: O is currently put in Search Length planning_cNearest circular barrier；First searching method for finding obstruction current Point O_cWith air route terminal O_eThe all circular barrier of line, then solves above each circular barrier center and currently puts O_c's Distance, being of distance minimum currently puts O away from planning_cNearest circular barrier；

Step 7: currently put O to away from planning_cNearest circular barrier carries out geometry and detours；

Step 8: will detour a little to put into and detour in a set S；

Step 9: Regeneration planning currently puts O_c, go to step four；

Step 10: by air route terminal O_ePutting into and detour in a set S, planning terminates.

Fig. 9 gives and utilizes the present invention to carry out UUV to detour circular barrier geometry one realization of two dimension routeing Case.

In present case, it is provided with 10 circular barriers, starting point O in air route altogether_bTerminal O with air route_eMark the most in the drawings Go out.First, when planning, each barrier having been carried out expansion process, chain-dotted line is used on the expansion border of each barrier the most in the drawings Mark.Then, part circular barrier is detoured by UUV, has obtained a set S={O that detours_b,P₁,P₂,P₃,…,P₁₆, O_e, and the UUV that by detour a little form has been represented by dashed line out in the drawings detours air route.

Claims (3)

1. the two-dimentional Route planner that circular barrier geometry is detoured by UUV, is characterized in that:

Step one: from mission Reading text air route starting point O_b, air route terminal O_eParameter with each circular barrier；

Step 2: each circular barrier is carried out expansion process, calculates the parameter of each circular barrier after expanding；

Step 3: set up a set S that detours, O is currently put in order planning_cFor starting point O_b, and put into and detour in set S；

Step 4: if O is currently put in planning_cIt is air route terminal O_e, or planning currently put O_cWith air route terminal O_eVisually, go to step Six, otherwise perform step 5；

Step 5: currently put O to away from planning_cNearest circular barrier carries out geometry and detours, and obtains detouring a little and puts into and detours a little In set S, Regeneration planning currently puts O_c, go to step four；

Step 6: by air route terminal O_ePutting into and detour in a set S, planning terminates.

The two-dimentional Route planner that circular barrier geometry is detoured by UUV the most according to claim 1, it is right to it is characterized in that Circular barrier carries out the method that geometry detours:

(1), judge that O is currently put in planning_cWhether on circular barrier, if, order point O₁=O_c；Otherwise, planning is solved current Point O_cWith the intersection point of the line of center of circle O with circumference, and to make it be an O₁；

(2) air route terminal O, is solved_eWith the intersection point of the line of center of circle O with circumference, and to make it be an O₂；

(3) ∠ O, is solved₁OO₂The intersection point of angular bisector and circumference, and to make it be an O₃；

(4) some O, was solved₁Circle tangent line with cross some an O₃The intersection point of tangent line of circle, and to make it be an O₄；

(5) some O, was calculated₂Circle tangent line with cross some an O₃The intersection point of tangent line of circle, and to make it be an O₅；

(6), will an O₁, some O₄, some O₅, some O₂Detour that a little putting into detours gathers in S as circular barrier, and Regeneration planning Currently put O_cFor an O₂, end of detouring.

The two-dimentional Route planner that circular barrier geometry is detoured by UUV the most according to claim 1 and 2, its feature It is that the method that circular barrier is carried out expansion process is: on the basis of normal planning terminates barrier geometry, according to Barrier shape edges is with radius of safety r_uuvOutward expansion goes out a radius of safety region.