CN107368086B - Unmanned underwater vehicle path planning device and method based on detection threat domain - Google Patents

Abstract

The present invention provides a path planning device and method for an unmanned underwater vehicle based on the detection threat domain. The path planning algorithm based on the detection threat domain solves the UUV's path planning problem in the terrain obstacle environment, which can satisfy the UUV's own kinematic constraints and avoid collisions. constraints and covert detection constraints. Given the initial position, end position, maximum curvature constraint, path discrete point resolution, hidden safety index, etc., plan a path from the starting point to the end point of the movement, which is smooth, continuous and steerable, meeting the UUV's navigation curvature constraints and hidden safety indicators, etc., so that it can reach the end point safely and concealed in the shortest time. For the first time, the invention applies the detection threat theory and the geometric theory of the navigational tortuosity constraint to the path planning field of the UUV, and can quickly realize the path planning. requirements, improve the practicability of path planning, and have positive significance for the future development of underwater path planning.

Description

Device and method for path planning of unmanned underwater vehicle based on detection threat domain

technical field

The invention relates to the research on hidden path planning algorithm of unmanned underwater vehicle, in particular to a path planning device and method of unmanned underwater vehicle based on detection threat domain, belonging to the field of terrain visibility, underwater acoustic field, complex system and intelligent control Field, the present invention can be used in the fields of unmanned underwater vehicle control, navigation and map construction.

Background technique

Path planning is an important problem in the field of unmanned underwater vehicle research. It can generate a safe and navigable path for the unmanned underwater vehicle, and this path is the most difficult to avoid obstacles in the process from the initial state to the target state. Excellent or near-optimal one.

UUV path planning, as an important function to ensure the safe and efficient completion of UUV tasks, has important practical significance. Many scholars at home and abroad have conducted a lot of research in this area, proposed many effective planning algorithms, and achieved rich research results, such as artificial potential field, A* algorithm, visual graph method, rapidly expanding random number, Voronoi diagram and various optimizations Algorithms have been widely studied and applied. The above path planning problem of generating a path between one or more nodes in a network is well known in the fields of operations research, communication, computational geometry and graphics, however, applying planning concepts from these fields to no Stealth planning for human submersibles is a challenge. The two most important constraints for UUV covert path planning are path navigability and covert safety. Navigability lies in the fact that the navigation path is usually composed of a series of segmented straight segments connecting each waypoint, but such a path is not a navigable path, and it is difficult to meet the vehicle kinematics or motion constraints, because each pair of connected sub-routes The segments must all have the same tangent to form a continuous path of curvature, so it is important to orient each waypoint so that each route matches each other. Concealment security means that when an aircraft performs a certain concealed mission, it will be threatened by the sonar array and detection of defense facilities, which will not only affect the success rate of mission execution, but also cause certain damage to the concealment security of the aircraft itself. threaten. At the same time, since the path planning for detecting the threat area needs to be based on certain geographic data, and the large-scale geographic data volume urgently needs efficient storage and transmission, it is necessary to compress the environmental data to reduce data redundancy and reduce data. Storage and corresponding communication charges.

To sum up, most of the current UUV path planning methods only consider the existence of obstructions, but do not consider how to deal with the path points to meet the constraints such as turning curvature, and safety constraints such as detection of threats based on hydroacoustics. Therefore, the present invention plans a concealed, safe and feasible path for the unmanned underwater vehicle based on the analysis of the threat of underwater acoustic detection of the navigation environment, so as to meet various constraints on the physical performance of the vehicle and realize the path of the unmanned underwater vehicle. Engineering applications in the field of planning.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide an unmanned underwater vehicle path planning device and method based on the detection threat domain in order to solve the hidden path planning problem of the unmanned underwater vehicle under the condition of multiple constraints, which can plan the unmanned underwater vehicle under the condition of terrain obstacles. A continuous feasible path to solve various physical constraints of UUVs in the detection of threats by the enemy.

所述UUV隐蔽路径修正优化单元设置机动可行曲线簇

为候选路径种群，采用安全约束进化算法修正当前机动可行曲线簇

并选取UUV在三维环境下满足运动学约束的最优隐蔽安全路径；所述UUV隐蔽路径输出单元接收并输出规划出的满足约束条件的最优隐蔽安全路径。The object of the present invention is achieved in this way: an unmanned vehicle path planning device based on the detection threat domain is characterized in that: it includes an environmental detection threat domain unit, a UUV concealed route initialization unit, a UUV safe waypoint generation unit, and a UUV concealed route generation unit. unit, UUV covert path correction and optimization unit, UUV covert path output unit, the environmental detection threat domain unit constructs the basic space frame of UUV navigation according to the given underwater environment parameters according to the interpolation method, and adopts the underwater visibility method Calculate the threat indicators that can be detected at each location point; the UUV covert path initialization unit is connected to the environmental detection threat domain unit, defines and discretizes each detection threat domain according to the covert security index, and threatens the geometric representation of obstacles in the domain by covering the circle domain method, And receive the initial parameters or conditions of UUV navigation; the UUV safe waypoint generating unit randomly generates the next waypoint A2 according to the position of the starting point A1 _of the _UUV , so that the path does not collide with the obstacle in the threat domain; the UUV concealed path _The generating unit _{generates successive path points A 13 , A 14} , _. , and in the same way generate a set of path clusters that do not collide with obstacles

The UUV concealed path correction optimization unit sets maneuverable feasible curve clusters

For the candidate path population, the safety constraint evolution algorithm is used to correct the current maneuver feasible curve cluster

And select the optimal hidden safe path that satisfies the kinematic constraints of the UUV in a three-dimensional environment; the UUV hidden path output unit receives and outputs the planned optimal hidden safe path that satisfies the constraints.

The present invention also includes such structural features:

1. An unmanned underwater vehicle path planning method based on the detection threat domain, comprising the described unmanned underwater vehicle path planning device based on the detection threat domain, and the steps are as follows:

Step 1: The environmental detection threat domain unit constructs the basic spatial framework of the underwater environment, sets the latitude, longitude and elevation information of the navigation range, the terrain interpolation method, and the resolution of the environmental unit, and calculates according to the resolution of the environmental unit and the method of underwater visibility. Threats that can be detected at each location point in the underwater environment model are evaluated, fused with the latitude and longitude information of the environment, and described as four-dimensional coordinates;

The threat evaluation is as follows: use the terrain visibility value to characterize the detection threat degree _vis of the _{unmanned underwater} vehicle; The threat evaluation vis _i of the position: when vis _i ≥ vis _max , it means that the position is not concealed at this time, let vis _i =1, when vis _i <vis _max , then the value of vis _i <1 remains unchanged;

Step 2: The UUV sails in a fixed-height manner, and the UUV concealed safe path initialization unit sets the state parameters and constraints of the path planning, the state parameters include the UUV initial position and the end position, and the constraints include the maximum curvature constraint κ _max , the concealment The security index vis _max ; and based on the environmental detection threat domain unit, each detection threat domain is defined and discrete according to the hidden security index, and the geometric representation of the obstacle of the threat domain is solved by the covering circle domain method;

Step 3: UUV safe waypoint generation unit, according to the initial position of the UUV, the UUV sails in a fixed-height manner, set the starting point as A _s , and randomly generate the first way point A ₁₁ , and make the distance between A _s and A ₁₁ The length of the line segment does not exceed d _m , and the path does not collide with the threat domain obstacle, where m and n in A _nm represent the mth point on the nth path, respectively;

Use the principles of computational geometry to determine whether the path between points A _s (x _s , y _s ) and A ₁₁ (x ₁₁ , y ₁₁ ) is reachable: According to A _s (x _s , y _s ) and A ₁₁ (x ₁₁ ) , y ₁₁ ) is the linear function L ₁ where the line segment is obtained:

For a certain obstacle circle Ob _i , calculate the distance l _Ls-11 from its center O _i (x _oi , y _oi ) to the line segment of A _s A ₁₁ , and compare it with the radius R _Oi of Ob _i : when l _Ls When _-11 > R _Oi , it means that Ob _i does not affect the connectivity of A _s A ₁₁ , so each obstacle circle is judged in turn, and the distance between A _s (x _s , y _s ) and A ₁₁ (x ₁₁ , y ₁₁ ) is obtained Whether the path is connected and reachable;

N为自然数，且每个路径点均满足UUV的最大曲率约束下的最大转角范围；Step 4: The UUV hidden path generation unit sequentially generates the subsequent path points A ₁₂ , A ₁₃ , A ₁₄ ,..., A _m1 according to the positional relationship between A _s and A ₁₁ and connects them two by two to obtain the path P ₁ , so that P ₁ does not collide with obstacles in the threat domain, and generates a set of path clusters that do not collide with obstacles in the same way

N is a natural number, and each path point satisfies the maximum turning angle range under the maximum curvature constraint of UUV;

Since the length of the connecting line between each path point does not exceed d _m , when d _m is set within a reasonable range, according to the principle of local linearization, it can be considered that the polyline path P _n is a linearized representation of the curved path, where the planned path The four-dimensional curve control point is composed of three-dimensional spatial coordinates and one-dimensional threat coordinates, and the planned path is also described by four-dimensional coordinates including three-dimensional spatial coordinates and threat coordinates;

为候选路径种群，基于探测威胁域单元计算隐蔽路径的安全综合代价，采用安全约束进化算法修正当前机动可行曲线簇

并选取UUV在三维环境下满足运动学约束的最优隐蔽安全路径；Step 5: UUV concealed path correction optimization unit sets maneuverable feasible curve cluster

For the candidate path population, the security comprehensive cost of the hidden path is calculated based on the detection threat domain unit, and the security constraint evolutionary algorithm is used to correct the current maneuverable feasible curve cluster

And select the optimal concealed safe path that satisfies the kinematic constraints of UUV in 3D environment;

Step 6: The UUV covert path output unit outputs the reconstructed covert safe path that satisfies the constraints.

2. Step 2 is as follows: set the sailing height as z ₀ , and perform step function calculation on z ₀ and each terrain elevation _zi according to the grid representation of the elevation terrain:

的最大范围，并根据覆盖圆域方法对连续障碍物

实现的最大圆形表征；Perform the similar OR operation on the threat assessment vis _i and the elevation information _zi , the so-called similarity means that for the non-integer form of vis _i , when vis _i <1, in the same OR operation, vis _i = 0, so that Obtain the comprehensive threat obstacle O _i ; perform the XOR operation on the rasterized obstacle O _i and the surrounding obstacle points to obtain the continuous obstacle

, and according to the coverage circle method for continuous obstacles

The largest circular representation achieved;

所占栅格点构成的多边形，以多边形最大/最小经度、最大/最小纬度为界，扩展为矩阵

则表征连续障碍物

的最大圆形

的圆心位置与半径长度分别为矩阵

的中心位置及1/2对角线长度。The method of covering the circular domain is as follows: in order to prevent the generation of concave obstacle geometry, according to the continuous obstacle

The polygon formed by the occupied grid points is bounded by the maximum/minimum longitude and maximum/minimum latitude of the polygon, and expanded into a matrix

represents a continuous obstacle

the largest circle of

The position of the center of the circle and the length of the radius are the matrices

The center position and 1/2 of the diagonal length.

3. In step 5: the adaptive function is constructed at the cost of detection threat degree and path length as:

F _c (i)=w _thr ·thr(i)+w _d ·dist(i)

Among them: w _thr and w _d are the weighting coefficients, thr(i)=1/vis(i) is the detection threat degree, dist(i) is the path length, and each maneuvering feasible curve is a single independent individual, and the population iteration is set The number of times Num is to obtain the current fitness function of each individual. The safety constraint evolution algorithm is used to select a certain number of individuals with the highest fitness as the next-generation parents, and perform continuous pairing with a certain random mutation probability to generate a new generation of groups, and then The fitness function of the new individual is recalculated and updated iteratively until the maximum number of iterations Num is reached, and the path with the highest fitness is finally selected as the optimal hidden safe path.

Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention adopts the path planning algorithm based on the detection threat domain to solve the path planning problem of the UUV in the terrain obstacle environment, and the planned path can satisfy the kinematic constraints of the UUV itself, avoid collisions and avoid collisions. Constraints and covert detection constraints, etc. Given UUV's initial position, end position, maximum curvature constraint, path discrete point resolution, concealment safety index and other parameters or conditions, plan a path from the starting point to the end point of UUV motion to meet UUV's navigation turning radius constraints, concealment safety indicators, etc., so that it can reach the end point safely and concealed in the shortest time. The method can quickly realize the path planning of UUV concealment and safety. The method is simple and reliable, easy to implement, small in calculation amount, and has good real-time performance, which can better meet the path planning requirements of UUV.

Description of drawings

1 shows a schematic structural diagram of an unmanned underwater vehicle path planning based on detecting a threat domain according to an embodiment of the present invention;

Fig. 2 is the flow chart of the unmanned underwater vehicle path planning algorithm based on the detection threat domain of the present invention;

Figure 3. The range D _m that satisfies the UUV maximum curvature constraint and the geometric description of the generation process when the path point is generated.

Detailed ways

The present invention will be described in further detail below with reference to the accompanying drawings and specific embodiments.

UUV隐蔽路径修正优化单元5与隐蔽路径生成单元4，设置机动可行曲线簇

为候选路径种群，采用安全约束进化算法修正当前机动可行曲线簇

并选取UUV在三维环境下满足运动学约束的最优隐蔽安全路径；UUV隐蔽路径输出单元6与隐蔽路径修正优化单元5，接收并输出规划出的满足约束条件的最优隐蔽安全路径。所述规划路径的曲线控制点四维由空间三维坐标与一维威胁坐标组成，规划路径也由包含空间三维坐标与威胁坐标的四维坐标描述。FIG. 1 shows a schematic structural diagram of an unmanned underwater vehicle path planning device based on detection threat domain according to an embodiment of the present invention. This structure can be implemented in a computer, and is composed of an environmental detection threat domain unit 1 and a UUV concealed path initialization unit. 2. The UUV safe waypoint generating unit 3, the UUV concealed route generation unit 4, the UUV concealed route correction and optimization unit 5, and the UUV concealed route output unit 6 are composed. The environmental detection threat domain unit 1 constructs the basic space frame of UUV navigation according to the given underwater environment parameters according to a certain interpolation method, and uses the underwater visibility method to calculate the threat indicators that can be detected at each position; UUV concealed path The initialization unit 2 is connected to the environmental detection threat domain unit 1, defines and discretizes each detection threat domain according to the hidden safety index, threatens the geometric representation of the obstacles in the domain by covering the circle domain method, and receives the UUV navigation initial parameters or conditions; UUV safe waypoint generation The unit 3 is connected to the hidden path initialization unit 2, and randomly generates the next path point A ₂ according to the position of the starting point A ₁ of the UUV, so that the path does not collide with the obstacles in the threat domain; the UUV hidden path generation unit 4 and the safe path point generation unit _{3. Follow up path points A 13 , A 14 , .} , and in the same way generate a set of path clusters that do not collide with obstacles

UUV hidden path correction and optimization unit 5 and hidden path generation unit 4, set maneuverable feasible curve clusters

For the candidate path population, the safety constraint evolution algorithm is used to correct the current maneuver feasible curve cluster

And select the optimal hidden safe path that satisfies the kinematic constraints of the UUV in a three-dimensional environment; the UUV hidden path output unit 6 and the hidden path correction and optimization unit 5 receive and output the planned optimal hidden safe path that satisfies the constraints. The four-dimensional curve control point of the planned path is composed of three-dimensional spatial coordinates and one-dimensional threat coordinates, and the planned path is also described by four-dimensional coordinates including three-dimensional spatial coordinates and threat coordinates.

In order to make the purpose, technical solutions and advantages of the present invention clearer, the following describes the UUV path planning method and device based on detection of threat domain based on the present invention in further detail with reference to the accompanying drawings and specific embodiments.

Utilize this structure below to realize the unmanned underwater vehicle path planning method based on detection threat domain of the present invention, please refer to the specific steps shown in FIG. 2 as follows:

Step 1: The environmental detection threat domain unit constructs the basic spatial framework of the underwater environment, sets the latitude, longitude and elevation information of the navigation range, the terrain interpolation method, and the resolution of the environmental unit. According to the resolution of the environmental unit, the calculation is based on the underwater visibility method. Threat indicators that can be detected at each location point in the underwater environment model are fused with the latitude and longitude information of the environment and described as four-dimensional coordinates;

The basic spatial framework of the underwater environment is based on the Digital Elevation Model (DEM) and adopts a regular grid model. The characteristic analysis method is used to traverse and analyze all the terrain points in the grid model in the current environment. At the same time, according to the actual design needs, certain limiting factors such as sound intensity attenuation are also considered. For example, passive sonar is selected for analysis, according to the sonar equation:

SL-TL-(NL-DI)≥DT (1)

The trajectory between AB is valid only if the SL-TL-(NL-DI) of target noise A is greater than the detection threshold DT of sonar B. Each point on the sound line emitted by A, where the sound source level SL is the radiated noise of the target, generated by the target A; NL-DI is the self-noise and ambient noise levels received by the directional transducer, which are related to the detection Threshold DT is determined by sonar B. When target A and sonar B are determined, SL and NL-DI can be regarded as constants, so judging whether the sound ray trajectory is effective mainly depends on the propagation loss TL. Due to the geometric attenuation and physical absorption caused by the expansion of the wavefront, the sound intensity will gradually weaken during the propagation process, and the process of converting the sound energy into heat energy is physical absorption, and the corresponding reduced sound intensity is the propagation loss. The propagation loss can be obtained from the propagation distance and the radiated noise frequency, and the formula is expressed as:

TL=15lgr+0.036f ^3/2 r+60(dB) (2)

The unit of r is km and the unit of frequency is kHz. The first and third terms of the right equation are geometric losses, and the middle term is the physical absorption loss. The sound wave spreads around in the form of a sphere, and its maximum spherical radius is represented by r. According to the definition of terrain visibility, for the unmanned underwater vehicle, the probability of being found in the area with a large terrain visibility value is high, and it is not suitable to navigate in this range. Therefore, this patent firstly evaluates the terrain visibility value After normalization, the terrain visibility value is used to characterize the detection threat degree vis of the UUV. Compare the detection threat degree vis of each location (x _i , y _i ) with the concealment security index vis _max and obtain the threat evaluation vis _i of each location, that is, when vis _i ≥vis _max , it means that the location is not concealed at this time, let vis _i =1, when vis _i <vis _max , the value of vis _i <1 remains unchanged.

Step 2: UUV covert safety path initialization unit, set parameters or conditions such as UUV initial position, end point position, maximum curvature constraint, path discrete point resolution, covert safety index, etc., based on the environment detection threat domain unit according to covert safety index Define and discretize each detection threat domain, and solve the geometric representation of the obstacles in the threat domain by covering the circle domain method;

The UUV adopts a fixed-altitude navigation state, and the parameters include the UUV initial position coordinates (x _s , y _s , z _s ), the initial position coordinates (x _f , y _f , z _f ), and the constraints include the maximum curvature constraint κ _max and Covert security indicator vis _max . Let the sailing height be z ₀ , then when the elevation information is certain, the three-dimensional UUV path planning problem is transformed into a two-dimensional problem, and the step function calculation is performed between z ₀ and each terrain elevation _zi according to the grid representation of the elevation terrain ,Right now

的最大范围，并根据覆盖圆域方法对连续障碍物

实现的最大圆形表征。Compare the detection threat degree vis of each position (x _i , y _i ) with the hidden security index vis _max to obtain the threat assessment vis _i of each position, and perform the similar OR operation on the threat assessment vis _i and the elevation information _zi , The so-called similarity means that for non-integer vis _i , when vis _i <1, vis _i =0 is considered in the same-OR operation, thereby obtaining the comprehensive threat obstacle O _i . XOR the rasterized obstacle O _i with the surrounding obstacle points to obtain continuous obstacles

, and according to the coverage circle method for continuous obstacles

The largest circular representation achieved.

所占栅格点构成的多边形，以多边形最大/最小经度、最大/最小纬度为界，扩展为矩阵

则表征连续障碍物

的最大圆形

的圆心位置与半径长度分别为矩阵

的中心位置及1/2对角线长度。The specific implementation method of the covering circle domain method is, in order to prevent the generation of concave obstacle geometry, according to the continuous obstacle

The polygon formed by the occupied grid points is bounded by the maximum/minimum longitude and maximum/minimum latitude of the polygon, and expanded into a matrix

represents a continuous obstacle

the largest circle of

The position of the center of the circle and the length of the radius are the matrices

The center position and 1/2 diagonal length of .

Step 3: The UUV safe waypoint generation unit randomly generates the first waypoint _A11 according to the position of the starting point A _s of the UUV, so that the length of the line segment between A _s and A ₁₁ does not exceed d _m , and the path is not related to the threat domain. The obstacle collides, where m and n in A _nm represent the mth point on the nth path, respectively;

Determine whether the path between the point A _s (x _s , y _s ) and A ₁₁ (x ₁₁ , y ₁₁ ) is connected and reachable by applying the principles of computational geometry. According to the line segment of A _s (x _s , y _s ) and A ₁₁ (x ₁₁ , y ₁₁ ), the linear function L _s-11 where it is located is obtained, that is,

For a certain obstacle circle Ob _i , calculate the distance l _Ls-11 from its center O _i (x _oi , y _oi ) to the line segment of A _s A ₁₁ , and compare it with the radius R _Oi of Ob _i , when l _Ls When _-11 > R _Oi , it means that Ob _i does not affect the connectivity of A _s A ₁₁ . From this, each obstacle circle is judged in turn, and it can be judged that the point A _s (x _s , y _s ) and A ₁₁ (x ₁₁ , y ₁₁ ) Whether the path between them is reachable or not. in

N为自然数，且每个路径点均满足UUV的最大曲率约束下的最大转角范围。其中规划路径的曲线控制点四维由空间三维坐标与一维威胁坐标组成，规划路径也由包含空间三维坐标与威胁坐标时间坐标的四维坐标描述；Step 4: The UUV hidden path generation unit generates subsequent path points A ₁₂ , A ₁₃ , A ₁₄ ,..., A _1m in turn according to the positional relationship between A _s and A ₁₁ and connects them two by two to obtain a path P ₁ , Make P ₁ do not collide with obstacles in the threat domain, and generate a set of path clusters that do not collide with obstacles in the same way

N is a natural number, and each path point satisfies the maximum turning angle range under the maximum curvature constraint of UUV. The four-dimensional curve control point of the planned path is composed of three-dimensional spatial coordinates and one-dimensional threat coordinates, and the planned path is also described by four-dimensional coordinates including three-dimensional spatial coordinates and time coordinates of threat coordinates;

The entire environment is constructed based on the north-east coordinate system. Since the length of the connecting line between each path point does not exceed d _m , when d _m is set within a reasonable range, the polyline path P _n can be considered according to the principle of local linearization is a linearized representation of the curvilinear path. The maximum step size ρ _max of the UUV path point is set, and ρ _max should not be set too large, so that the curve path of the UUV is represented by local linearization between the path points. The position of each waypoint is directly coded by coordinates, the gene is represented by real point coordinates, and the chromosome is represented by an indeterminate number of point connections.

As shown in Figure 3, the points Am in the path are composed of A _{m -1} (x _m-1 , y _m-1 ) and A _m-2 (x _m-2 , y _m-2 ) which satisfy the UUV The maximum curvature constraint range D _m is randomly generated, where the range of D _m is represented by the polar coordinate form (ρ ^m-1 , θ ^m-1 ), the superscript m-1 indicates that A _m-1 is the origin, and the coordinate axis The coordinates when X ^m-1 and Y ^m-1 are respectively parallel to the northeast and satisfy ρ ^m-1 ∈(0,ρ _max ), θ ^m - ¹ ∈(-γ+ε,γ+ε), where ε is the slope of the straight line where the line segment between A _m-1 (x _m-1 , y _m-1 ) and A _m-2 (x _m-2 , y _m-2 ) is located.

令最大曲率约束为κ_max，根据余弦定理可知

则γ由式(7)求得：Since the length of the connection between each path point is set within a reasonable range, according to the principle of local linearization, it can be considered that the polyline path is a linearized representation of the curve path. According to the principle of analytic geometry, when O'A _m-1 =O'A _m-2 =1/κ _max , the maximum range D _m can be obtained, because

Let the maximum curvature constraint be κ _max , according to the law of cosines

Then γ can be obtained from formula (7):

为A_m-1A_m-2的斜率。在范围D_m中随机生成

和

下一路径点

并通过坐标转换得到北东坐标系下的坐标

Therefore, taking A _m-1 as the center of the circle, the opening angle is 2γ, the sector formed within the radius d _m is the range D _m constrained by the maximum curvature of the UUV, and the coordinates of the two segments of the arc are (d _m cos(γ+ε)- x _m-1 ,d _m cos(γ+ε)-y _m-1 ), (d _m cos(ε-γ)-x _m-1 ,d _m cos(ε-γ)-y _m-1 ),

is the slope of A _m-1 A _m-2 . Randomly generated in range D _m

and

next waypoint

And get the coordinates in the north-east coordinate system through coordinate transformation

为候选路径种群，基于探测威胁域单元计算隐蔽路径的安全综合代价F_c，采用安全约束进化算法修正当前机动可行曲线簇

并选取UUV在三维环境下满足运动学约束的最优隐蔽安全路径；Step 5: UUV hidden path correction optimization unit, set maneuverable feasible curve cluster

is the candidate path population, calculates the security comprehensive cost F _c of the hidden path based on the detection threat domain unit, and uses the security constraint evolution algorithm to correct the current maneuverable feasible curve cluster

And select the optimal concealed safe path that satisfies the kinematic constraints of UUV in 3D environment;

满足UUV的运动学条件，但不满足其安全性约束，本要求以探测威胁度和路径长度为代价构建适应性函数，如(8)式进行描述：The current path cluster consisting of N maneuvering feasible paths

Satisfying the kinematic conditions of UUV, but not meeting its security constraints, this requirement builds an adaptive function at the cost of detection threat degree and path length, as described in equation (8):

F _c (i)=w _thr ·thr(i)+w _d ·dist(i) (8)

Where w _thr and w _d are weighting coefficients, thr(i)=1/vis(i) is the detection threat degree, and dist(i) is the path length, which can be obtained by analytical geometry. Each maneuvering feasible curve is a single independent individual, and the population iteration number Num is set, and the current fitness function of each individual is calculated according to formula (3), and the safety constraint evolution algorithm is used to select a certain number of individuals with the highest fitness to be the parents of the next generation, And carry out continuous pairing with a certain random mutation probability to generate a new generation of groups, and then recalculate the fitness function of the new individual, iteratively update until the maximum number of iterations Num is reached, and finally select the path with the highest adaptability is the optimal concealment Safe path; this function indicates that the shorter the path and the better the concealment, the greater the fitness of the chromosome, the greater the probability of inheritance to the next generation, and finally an optimal or sub-optimal path will be planned under the guidance of the fitness function. (satisfactory solution). In order to facilitate the understanding of the relationship of each waypoint in the route, the waypoint in the following text will be represented by X _i . The genetic manipulation module of the present invention includes a selection operator, a crossover operator, a mutation operator, a special insertion operator and a special deletion operator, and its specific implementation method is:

1. Selection process

(1) Calculate the fitness F _c (i) of each individual according to the designed fitness function;

(2) Find the sum of the fitness of all individuals;

(3) Calculate the proportion of fitness of each individual cfitness(i)=F _c (i)/sum;

(4) Calculate the cumulative value of the fitness ratio of each individual;

(5) By generating a random number X between (0,1);

(6) From the first individual, its cfitness value is compared with the random number X in turn, and the first individual i whose cfitness value is larger than X (that is, the random number falls within the probability area of the individual) is selected as the operation. the genetic object of

(7) Repeat the operations (5) and (6) above until the number of inherited individuals reaches a predetermined number.

In order to speed up the overall optimization and ensure that the individual fitness is getting better and better, the elite retention strategy is added after the selection is completed:

(1) Find the best individual and the worst individual in the current group;

(2) Compare the fitness of the current optimal individual with the fitness of the optimal individual so far, and if the fitness of the optimal individual is better, take the current optimal individual as the new optimal individual so far;

(3) Replace the worst individual in the current population with the best individual so far.

2. The intersection process theoretically includes single-point intersection and multi-point intersection. In view of the need to ensure the continuity of the path when planning the aircraft path, the present invention chooses to start from a certain point for multi-point intersection to ensure the continuity of the path as much as possible:

(1) The chromosomes are randomly paired in pairs, and each pair of chromosomes is judged whether to perform the crossover operation with the crossover probability _Pc ;

(2) If the crossover probability is satisfied, a crossover point i is randomly generated first;

(3) The paired two chromosomes are crossed with each other from the crossover point i to the last gene n, that is, multipoint crossover at the same time;

(4) After the two chromosomes are randomly crossed, there may be obstacles between the randomly generated crossover position Xi and the previous position Xi _-1 for the two new chromosomes _{, or X i is} not combined with Xi _-2 and Xi _-1 The determined range of navigation constraints does not meet the requirements, so we introduce a special insertion operator for this situation to perform insertion operations to rationalize the final navigation path;

(5) Steps (2) to (4) are repeated, and the above process is performed for all paired chromosomes, and the crossover is completed.

3. The purpose of the mutation process is two: one is to enhance the local search ability of the genetic algorithm, and the other is to maintain the diversity of the population, prevent the phenomenon of premature maturity, and appear the local optimal solution. The present invention adopts the basic bit mutation method:

(1) Judging whether to perform mutation operation on each individual with mutation probability P _m ;

(2) If the mutation probability is satisfied, a mutation position X _i is randomly generated;

(3) Variation of chromosomal genes at the mutation position. There are two basic principles of mutation:

On the one hand, the newly generated position X _i must be based on the first two adjacent positions X _i-2 and X _i-1 and satisfy the navigation constraint range D _m of the UUV;

On the other hand, there must be no obstacle between the newly generated position X _i and the previous position X _i-1 .

If the new position generated by the mutation does not meet the above two basic principles, the mutation position is reselected to meet the requirements.

(4) Repeat operations (2) and (3), and operate on all chromosomes according to the above process, and then the mutation is completed.

4. Insert and delete operations

In the present invention, the navigation path generated by the genetic algorithm according to the random principle is beneficial to the diversity of the population, but there will be many identical positions (the actual path is shown as a circle). Aiming at this problem, the present invention proposes a deletion operator, and the crossover operation is very important. It may make the new path no longer satisfy the two basic principles of intersection near the intersection position, so the insertion operator is proposed:

(1) Take the previous position X _i-1 of the intersection position as the starting point, and the intersection position X _i as the end point;

(2) Using the method of generating the initial population, a series of sailing positions that meet the requirements are generated between Xi _-1 and _Xi , so that Xi _-1 and _Xi become a feasible path.

The specific implementation steps of the deletion operation are as follows:

(1) Starting from the current position X _i (including the starting point) of the path, find the first identical position X _m from front to back, and delete all positions (including X _i ) between X _i and X _m ;

(2) Continue to find the next identical position with X _m as the current position, and delete all positions (including X _m ) between it and the next identical position;

(3) Repeat steps (1) and (2) until the current position is the last end point, stop the deletion operation, and the deletion operation is completed.

Finally, since the map environment that the present invention relies on is relatively large, there is no mandatory requirement for planning time. Therefore, the termination condition is directly set as the genetic algebra of the population exceeds the set value, and the genetic operation is terminated immediately, and the final result is output.

Step 6: The UUV covert path output unit outputs the reconstructed covert safe path that satisfies the constraints.

Output the planned path of the UUV, including each path point A _nm of the optimal path P _n obtained in steps 2 to 5, as well as the initial and termination points, and the detection threat degree of each path point.

In summary, the present invention is a path planning device for unmanned underwater vehicles based on the detection threat domain. The path planning algorithm based on the detection threat domain is used to solve the UUV route planning problem in the terrain obstacle environment, and the planned route can satisfy the UUV itself. Kinematic constraints, collision avoidance constraints, and covert detection constraints. Given parameters or conditions such as the initial position, end position, maximum curvature constraint, resolution of discrete points of the path, hidden safety indicators, etc. of the UUV, plan a path from the starting point to the end point of the UUV motion. Constraints on the curvature of the sailing turn, concealment safety indicators, etc., so that it can reach the end point safely and concealed in the shortest time. The invention firstly applies the detection threat theory and the geometric theory of navigation turning curvature constraint to the path planning field of UUV. The method can quickly realize UUV path planning. The method is simple and reliable, easy to implement, small in calculation amount, and good in real-time performance. It can better meet the UUV path planning requirements, and improve the practicability of UUV path planning. Developments in the field of planning have positive implications.

Claims (4)

1. Unmanned underwater vehicle route planning device based on survey threat territory, its characterized in that: the method comprises an environment detection threat domain unit, a UUV hidden path initialization unit, a UUV safe path point generation unit, a UUV hidden path correction optimization unit and a UUV hidden path output unit, wherein the environment detection threat domain unit constructs a UUV navigation basic space frame according to given underwater environment parameters by an interpolation method and calculates a threat index which can be detected at each position point by adopting an underwater visibility method; the UUV hidden path initialization unit is connected with the environment detection threat domain unit, defines and disperses each detection threat domain according to hidden safety indexes, threatens the geometric representation of the domain obstacle by a covering circle domain method, and receives UUV navigation initial parameters or conditions; the UUV safe path point generating unit generates a UUV safe path point according to the initial point A of the UUV_sPosition, randomly generating the next path point A₁₁To prevent the path from colliding with the threat domain obstacleCollision; the UUV hidden path generating unit generates a hidden path according to A_sAnd A₁₁Sequentially generates the subsequent path points A₁₂,A₁₃,...,A_1mAnd connecting the two paths to obtain a path P₁So that P is₁Does not collide with the obstacle in the threat domain, and generates a group of path curve clusters which do not collide with the obstacle in the same way

The UUV hidden path correction optimization unit is provided with a flexible and feasible path curve cluster

For candidate path population, a safety constraint evolutionary algorithm is adopted to correct the path curve cluster which is feasible by the current maneuver

Selecting an optimal hidden safety path of the UUV which meets the kinematic constraint in the three-dimensional environment; and the UUV hidden path output unit receives and outputs the planned optimal hidden safety path meeting the constraint condition.

2. The unmanned underwater vehicle path planning method based on the detection threat domain is characterized by comprising the following steps: the unmanned underwater vehicle path planning device based on the detection threat domain comprises the following steps:

step 1: the method comprises the following steps that an environment detection threat domain unit constructs a basic space frame of an underwater environment, sets latitude and longitude and elevation information of a navigation range, a terrain interpolation method and an environment unit resolution, calculates detected threat evaluation of each position point in an underwater environment model according to the environment unit resolution and an underwater visibility method, and fuses with the latitude and longitude information of the environment to form four-dimensional coordinate description;

the threat assessment is: the detection threat vis of the unmanned underwater vehicle is characterized by a terrain visibility value; each position point (x)_i,y_i) Detection of threat level vis and covert security index vis_maxCompared with each other to obtainThreat assessment vis to locations_i: when vis_i≥vis_maxThe time indicates that the position is not hidden at this time, so that vis_iWhen vis 1_i＜vis_maxThen, then vis_iThe value is less than 1 and is not changed;

step 2: the UUV navigates in a fixed-height mode, the hidden safe path initialization unit of the UUV sets state parameters and constraint conditions of path planning, the state parameters comprise the initial position and the final position of the UUV, and the constraint conditions comprise maximum curvature constraint kappa_maxConcealed safety index vis_max(ii) a Defining and dispersing each detection threat domain according to the concealed safety index based on an environment detection threat domain unit, and solving the geometric representation of the threat domain obstacle by a covering circular domain method;

and step 3: a UUV safe path point generating unit, wherein the UUV navigates in a height-fixed mode according to the initial position of the UUV, and the initial point is set as A_sAnd randomly generating a first path point A₁₁And make A_sAnd A₁₁Length of line between does not exceed d_mAnd the path does not collide with the threat domain obstacle, wherein A_nmM and n in (1) respectively represent the m-th point on the n-th path;

judging point A by using the principles of computational geometry_s(x_s,y_s) And A₁₁(x₁₁,y₁₁) Whether the paths between the two are connected can be reached: according to A_s(x_s,y_s) And A₁₁(x₁₁,y₁₁) Linear function L of the line segment_s-11：

For a certain obstacle circle Ob_iRespectively find out the center O of the circle_i(x_oi,y_oi) To A_sA₁₁Distance of line segment l_Ls-11And respectively reacting them with Ob_iRadius R of_OiAnd (3) comparison: when l is_Ls-11＞R_OiWhen it is shown that Ob is_iDoes not affect A_sA₁₁Thereby sequentially judging each barrierObstruct the circle, obtain A_s(x_s,y_s) Point and point A₁₁(x₁₁,y₁₁) Whether the paths between the two paths are communicated and can be reached;

and 4, step 4: the UUV hidden path generating unit generates a hidden path according to A_sAnd A₁₁Sequentially generates the subsequent path points A₁₂,A₁₃,A₁₄,...,A_1mAnd connecting the two paths to obtain a path P₁So that P is₁Does not collide with the obstacle in the threat domain, and generates a group of path curve clusters which do not collide with the obstacle in the same way

N is a natural number, and each path point meets the maximum rotation angle range under the maximum curvature constraint of the UUV;

the length of the connecting line between each path point does not exceed d_mWhen d is_mWhen the distance is set within a reasonable range, the broken line path P can be considered according to the local linearization principle_nThe method is characterized by comprising the following steps that a curve path is represented in a linear mode, wherein the four dimensions of a curve control point of a planned path are composed of a space three-dimensional coordinate and a one-dimensional threat coordinate, and the planned path is also described by a four-dimensional coordinate comprising the space three-dimensional coordinate and the threat coordinate;

and 5: UUV hidden path correction optimization unit is set with flexible and feasible path curve cluster

Calculating the safe comprehensive cost of the hidden path for the candidate path population based on the detection threat domain unit, and correcting the current feasible path curve cluster by adopting a safe constraint evolution algorithm

Selecting an optimal hidden safety path of the UUV which meets the kinematic constraint in the three-dimensional environment;

step 6: and the UUV hidden path output unit outputs the reconstructed hidden security path which meets the constraint condition.

3. The unmanned underwater vehicle path planning method based on the detection threat domain according to claim 2, characterized in that: the step 2 specifically comprises the following steps: setting the sailing height as z₀And z is expressed in terms of a grid representation of the elevation terrain₀To various terrain elevations z_iPerforming step function calculation:

evaluating threats vis_iAnd elevation information z_iPerforming an identity-or operation, i.e. for non-integer forms of vis_iIn other words, when vis_iWhen < 1, the vis is viewed in the same or operation_i0, thereby obtaining the comprehensive threat obstacle CO_i(ii) a Rasterizing barrier GCO_iPerforming XOR operation with surrounding obstacle points to obtain continuous obstacles

And according to a method of covering a circular field for a continuous obstacle

Maximum circular characterization achieved;

the method for covering the circular domain specifically comprises the following steps: according to continuous obstacles, to prevent concave obstacle geometry

The occupied polygon formed by the grid points is expanded into a matrix by taking the maximum/minimum longitude and the maximum/minimum latitude of the polygon as boundaries

Then a continuous obstacle is characterized

Maximum circular shape of

The circle center position and the radius length of the matrix are respectively

And 1/2 diagonal length.

4. The unmanned underwater vehicle path planning method based on the detection threat domain according to claim 2, characterized in that: in the step 5: constructing an adaptive function at the cost of probing threat and path length as follows:

F_c(i)＝w_thr·thr(i)+w_d·dist(i)

wherein: w is a_thrAnd w_dFor weighting coefficients, thr (i) ═ 1/vis (i) is the degree of detection threat, dist (i) is the path length, each feasible maneuver curve is a single independent individual, the population iteration number Num is set to obtain the adaptability function of each individual at present, a certain number of parents with the highest adaptability of the individual with the highest adaptability are selected by adopting a safety constraint evolution algorithm, continuous pairing accompanied by a certain random mutation probability is carried out to generate a new generation population, then the adaptability function of the new individual is recalculated, iterative updating is carried out until the maximum iteration number Num is reached, and finally the path with the highest adaptability is selected as the optimal covert safety path.

Images