Obstacle constraint generation method for real-time obstacle avoidance algorithm development and test of underwater robot

Abstract

The invention discloses an obstacle constraint generation method for developing and testing a real-time obstacle avoidance algorithm of an underwater robot, and relates to the technical field of underwater robot collision avoidance planning, in particular to a three-dimensional obstacle construction and sensing method. The invention provides an obstacle scene construction and efficient and rapid obstacle constraint generation method capable of reflecting obstacle characteristics in an AUV (Autonomous Underwater Vehicle) operation space, aiming at solving the problems of an existing three-dimensional obstacle scene construction and perception method for developing and testing an AUV real-time obstacle avoidance algorithm in a large-scale and three-dimensional unstructured operation space. The method is used for developing and testing the AUV real-time obstacle avoidance algorithm. Firstly, multiple types of fixed and random primitive obstacles are generated based on a triangulation method, and then the multiple primitive obstacles are combined through a random or structured design mode to construct an obstacle scene; and finally, obstacle detection and information processing are carried out based on a designed 5n perception method, and obstacle constraints required by the algorithm are generated.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robot collision avoidance planning, and in particular to an obstacle constraint generation method for underwater robot real-time obstacle avoidance algorithm development and testing. Background Art

[0002] With the increasing demand for underwater robots (AUVs) in military, civilian and commercial fields, the operating environment of AUVs has gradually expanded from the known flat sea areas to unknown and complex sea areas. Unknown environments require AUVs to have higher environmental adaptability. One of the key issues that needs to be solved is how to avoid various unknown obstacles smoothly, that is, how AUVs can use the obstacle information and environmental information perceived and identified by sensors to make real-time decisions on the desired behavior that can avoid unknown obstacles.

[0003] Obstacle perception and real-time decision-making are two basic capabilities that AUVs must have to avoid unknown obstacles in real time, which are directly related to the safety and intelligence level of AUVs. The core of the unknown obstacle avoidance function is the real-time decision-making capability. The real-time decision-making process generates the desired behavior based on the real-time obstacle avoidance algorithm according to the obstacle constraints provided by obstacle perception. The core of real-time decision-making is the real-time obstacle avoidance algorithm. At present, there are many studies on AUV real-time obstacle avoidance algorithms and many progresses have been made, but they are mainly focused on the generation and improvement of desired behaviors or trajectories, and there are relatively few studies on obstacle constraint generation methods. The more commonly reported obstacle constraint generation methods include the pixel traversal method that represents the obstacle scene with a binary image and generates perception information by traversing all pixels, and the feature center traversal method that constructs the obstacle scene with regular geometric shapes such as circles or spheres and generates perception information by traversing feature centers such as the center of the circle, the center of the sphere, or the center of mass. Among the above methods, the pixel traversal method is suitable for small-scale two-dimensional environments, and the feature center traversal method can only generate simple and regular obstacle constraints. Since the working space of AUV is usually a large-scale, unstructured three-dimensional underwater space, the pixel traversal method as the obstacle constraint generation method faces the problem of dimensional expansion. The feature center traversal method is difficult to generate unstructured obstacle constraints, and simplified obstacle constraints often lead to blindly optimistic obstacle avoidance effects. Summary of the invention

[0004] Aiming at the problems existing in the existing three-dimensional obstacle scene construction and perception methods for the development and testing of AUV real-time obstacle avoidance algorithms in large-scale and three-dimensional unstructured working spaces, the technical problem to be solved by the present invention is to provide an obstacle scene construction and constraint generation method that can reflect the characteristics of the AUV working space, which is used for the development and testing of AUV real-time obstacle avoidance algorithms.

[0005] The technical solution adopted by the present invention to achieve the above-mentioned purpose is:

[0006] An obstacle constraint generation method for the development and testing of real-time obstacle avoidance algorithms for underwater robots, comprising the following steps:

[0007] 1) Generate various types of primitive obstacles based on the triangulation method;

[0008] 2) Based on the primitive obstacles, construct a three-dimensional obstacle scene through random combination or structured design;

[0009] 3) Generate obstacle constraints for the three-dimensional obstacle scene based on the 5n perception method.

[0010] The specific steps of step 1) are as follows: Generate different types of primitive obstacles by predefined different vertex positions and the combination order of vertices, that is:

[0011] V ep ={(x 1 ,y 1 ,z 1 ),...(x i ,y i ,z i ),...,(x j ,y j ,z j ),...,(x k ,y k ,z k ),...,(x N ,y N ,z N )}

[0012] V eo ={(1,2,3),(2,3,1),...(i,j,k),...,(j,k,N),...}

[0013] where V ep is the set of all vertex positions in the primitive obstacle, and the elements (x i ,y i ,z i ),(x j ,y j ,z j ) and (x k ,y k ,z k ) represent the coordinate values of the vertices numbered i, j, and k in the three-dimensional rectangular coordinate system respectively, N is the number of vertices in the set, and V eo is the set of all vertex combination orders of the primitive obstacle. The element represents the numbers of the three vertices forming the space triangle. For example, (i, j, k) represents the space triangle formed by vertices i, j, and k, and the position coordinates of the vertices are (x i ,y i ,zi ),(x j ,y j ,z j ) and (x k ,y k ,z k ).

[0014] The three-dimensional obstacle scene is a structured or unstructured scene generated by randomly combining or structurally designing single or multiple primitive obstacles of the same or different types, and is expressed as:

[0015] V sp = {V ep，1 , V ep，2 ,..., V ep，t ,...V ep，M}

[0016] V so = {V eo，1 , V eo，2 ,...V eo，t ,...V eo，M}

[0017] Among them, V sp is the set of all vertex positions of the obstacle scene, and the element V ep，t represents the set of vertex positions of the t-th primitive obstacle in the obstacle scene. M is the number of primitive obstacles in the obstacle scene, and V so is the set of vertex orders of the obstacle scene, and the element V eo，t represents the set of vertex orders of the t-th primitive obstacle in the obstacle scene.

[0018] The random combination methods include: the method of constructing an obstacle scene by randomly combining multiple primitive obstacles, the method of constructing an obstacle scene by adding random perturbations to vertex positions, and the method of constructing an obstacle scene by combining fixed primitive obstacles and random primitive obstacles. The obstacle scene constructed based on the random combination method is called a random obstacle scene.

[0019] The structured design methods include: the method of constructing an obstacle scene based on given types and numbers of fixed primitive obstacles. The obstacle scene constructed based on the structured design method is called a typical obstacle scene.

[0020] Step 3) includes the following steps:

[0021] 3.1) Determine the horizontal direction angle resolution and the vertical direction angle resolution according to the characteristics of the sonar that the AUV plans to adopt or has adopted:

[0022]

[0023] r β = rα or

[0024] where r α is the resolution of the horizontal direction angle, α is the horizontal opening angle of the sonar, k l is the horizontal direction scaling factor, n is the number of sonar beams, r β is the vertical direction angle resolution, β is the vertical opening angle of the sonar, k v is the vertical direction scaling factor;

[0025] 3.2) Starting from the vertex of the sonar sensing range with the azimuth angle determined in the body coordinate system being -0.5α and the elevation angle being -0.5β, respectively using r α and r β as the discrete resolutions in the horizontal and vertical directions, discretize the sonar sensing range into multiple quadrangular pyramid units:

[0026]

[0027]

[0028] n t = n l × n v

[0029] where n l is the number of units after discretization in the horizontal direction, n v is the number of units after discretization in the vertical direction, n t is the total number of units within the sensing range after discretization;

[0030] 3.3) Traverse all the quadrangular pyramid units and generate 5 feature points for each unit:

[0031]

[0032]

[0033]

[0034]

[0035]

[0036] where is the pair of azimuth angle and elevation angle of the k-th point within the (i, j) unit, i = 1, 2,..., n l , j = 1, 2,..., n v , k = 1, 2, 3, 4, 5;

[0037] 3.4) Taking the 5 feature points as the end points and the (0, 0, 0) point of the body coordinate system as the starting point, and using the detection range of the sonar that is planned to be used or has been used as the length, traverse all the tetrahedral pyramid units to generate 5n t lines, and project all the lines from the body coordinate system to the geodetic coordinate system where the obstacle scene is located;

[0038] 3.5) Based on the line-segment and triangle intersection detection algorithm, traverse all the tetrahedral pyramid units and perform intersection tests with the obstacle scene respectively. If there is an intersection point between a certain line segment among the 5 line segments in a certain unit and the obstacles in the obstacle scene, then take the minimum value among the Euclidean distances from the starting point of this line segment to all the intersection points as the line-segment intersection distance of this line segment; if there is no intersection point, then take a value whose difference from the sonar detection range is within the threshold range as the line-segment intersection distance of this line segment, and take the minimum value among the 5 line-segment intersection distances as the unit intersection distance. Traverse all the units to generate the intersection matrix H:

[0039]

[0040]

[0041]

[0042] where d i，j， k is the minimum distance between the k-th line segment in the (i, j) unit and the obstacles in the obstacle scene, f() is the line-segment and triangle intersection detection algorithm, d i，j is the minimum distance between the obstacles in the (i, j) unit and the sonar center, that is, the intersection distance, and ε is a constant;

[0043] 3.6) Take the minimum value of each column in the intersection matrix H as the output of the three-dimensional perception method, and perform a flattening process on the intersection matrix H:

[0044]

[0045]

[0046] where d i is the minimum intersection distance of all the units with the row number i; L is the perception output array, which is used as the contour information of the three-dimensional obstacles sensed in real time during the AUV navigation process.

[0047] The present invention has the following beneficial effects and advantages:

[0048] 1. The present invention provides a method for constructing and perceiving a three-dimensional obstacle scene for the AUV operation space, which is helpful for the research and development of the autonomous simulation environment of the AUV real-time collision avoidance algorithm.

[0049] 2. The present invention can generate obstacle scenarios containing various types of primitive obstacles, even including sunken ships and ancient cities, etc., by means of random combination or structured design, avoiding the generation of overly simplified and single obstacle constraints.

[0050] 3. The present invention uses the described 5n perception method to simulate the information perception and processing mode of an actual sonar to achieve three-dimensional obstacle scenario perception and information processing, avoiding the generation of obstacle constraints with overly rich information. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 Schematic diagram of a fixed-type primitive obstacle;

[0052] Figure 2 Schematic diagram of a random-type primitive obstacle;

[0053] Figure 3 Schematic diagram of sonar field of view and beam distribution;

[0054] Figure 4 Schematic diagram of discrete element and feature point distribution;

[0055] Figure 5 Schematic diagram of the intersection situation between a line segment and a triangle;

[0056] Figure 6 Schematic diagram of three-dimensional obstacle perception;

[0057] Figure 7 Schematic diagram of flattened processing of perceived information;

[0058] Figure 8 Flowchart of the obstacle constraint generation method. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0059] The present invention will be further described in detail below with reference to the drawings and embodiments.

[0060] As Figure 8 shown, a method for generating obstacle constraints for the development and testing of a real-time obstacle avoidance algorithm for an underwater robot includes the following steps:

[0061] Step 1, generation of primitive obstacles: generating various types of basic three-dimensional obstacle units based on the triangulation method;

[0062] Step 2, construction of the obstacle scenario: constructing a three-dimensional obstacle scenario based on random combination or structured design;

[0063] Step 3, three-dimensional perception of the scenario: generating obstacle constraints for the three-dimensional obstacle scenario based on the designed 5n perception method.

[0064] Further, in the step 1, the primitive obstacle is the basic unit for constructing a three-dimensional obstacle scene, including but not limited to planes, curved surfaces, cubes, tetrahedrons, polyhedrons, U-shaped bodies, G-shaped bodies, and random types, etc. A primitive obstacle with a determined type, vertex positions, and vertex combination order is called a fixed-type obstacle. A primitive obstacle with randomly generated type, vertex positions, and vertex combination order is called a random-type obstacle. Sufficient facts in topology show that most three-dimensional complex models can be generated based on the triangulation method. The primitive obstacles are generated by the triangulation method. The method is to generate different types of primitive obstacles by predefined different vertex positions and vertex combination orders. Its mathematical description is:

[0065] V ep ={(x 1 , y 1 , z 1 ),...(x i , y i , z i ),...,(x j , y j , z j ),...,(x k , y k , z k ),...,(x N , y N , z N )}

[0066] V eo ={(1, 2, 3), (2, 3, 1),...(i, j, k),...,(j, k, N),...}

[0067] Among them, V ep is the set of all vertex positions in the primitive obstacle. The elements (x i , y i , z i ), (x j , y j , z j ) and (x k , y k , z k ) represent the coordinate values of the vertices numbered i, j, and k in the three-dimensional rectangular coordinate system; N is the number of vertices in the set; V eo is the set of all vertex combination orders of the primitive obstacle. The elements represent the numbers of the three vertices forming the spatial triangle. For example, (i, j, k) represents the spatial triangle formed by vertices i, j, and k, and the position coordinates of the vertices are (x i , y i , z i ), (x j , y j, z j ), and (x k , y k , z k ).

[0068] Furthermore, in the said step 2, the obstacle scenario is the scenario for the development and testing of real-time obstacle avoidance algorithms, and is a structured or unstructured scenario generated by randomly combining or structurally designing single or multiple primitive obstacles of the same type or different types. The obstacle scenario is composed of primitive obstacles. Its mathematical description is as follows:

[0069] V sp = {V ep，1 , V ep，2 ,..., V ep，t ,...V ep，M}

[0070] V so = {V eo，1 , V eo，2 ,...V eo，t ,...V eo，M}

[0071] Among them, V sp is the set of all vertex positions of the obstacle scenario, and the element V ep，t represents the set of vertex positions of the t-th primitive obstacle in the obstacle scenario; M is the number of primitive obstacles in the obstacle scenario; V so is the set of vertex orders of the obstacle scenario, and the element V eo，t represents the set of vertex orders of the t-th primitive obstacle in the obstacle scenario.

[0072] The said random combination method includes but is not limited to the method of constructing an obstacle scenario by randomly combining multiple primitive obstacles, the method of constructing an obstacle scenario by increasing random perturbations of vertex positions, and the method of constructing an obstacle scenario by combining fixed-type primitive obstacles and random-type primitive obstacles. The obstacle scenario constructed based on the random combination method is called a random obstacle scenario.

[0073] The said structured design method is the method of constructing an obstacle scenario based on given fixed-type primitive obstacle types and numbers. The obstacle scenario constructed based on the structured design method is called a typical obstacle scenario.

[0074] Furthermore, in the said step 3, three-dimensional perception of the obstacle scenario is realized according to the obstacle perception and information processing method of the AUV three-dimensional obstacle avoidance sensor, and obstacle constraints are generated. Currently, the AUV usually uses a multi-beam forward-looking sonar as the three-dimensional obstacle avoidance sensor. Suppose the total number of sonar beams is n. If there is an obstacle within the perception range of the i-th beam, the sonar perception information can be described in the following mathematical form:

[0075]

[0076] Among them, (x o，i , yo，i , z o，i ) is the obstacle position information relative to the center of the sonar receiver sensed by beam i; d max is the detection distance of the sensor; α is the horizontal opening angle of the sonar; β is the vertical opening angle of the sonar.

[0077] Generally, the three-dimensional obstacle information sensed by the sonar is projected into two-dimensional plane information and used as the obstacle constraint for the collision avoidance algorithm. The projection method is to project the information with the same distance and the same azimuth angle onto the central plane of the vertical opening angle. The azimuth angle mentioned above is the angle difference (with the smallest absolute value) between a certain point and the central plane of the horizontal opening angle.

[0078] Based on the above obstacle sensing and information processing methods, the 5n sensing method is designed as the three-dimensional sensing method for the scenario, and the specific steps are as follows:

[0079] Step 3.1, determine the horizontal direction angle resolution and the vertical direction angle resolution according to the characteristics of the sonar to be adopted or already adopted. Its mathematical description is as follows:

[0080]

[0081] r β = r α or

[0082] Among them, r α is the resolution of the horizontal direction angle; α is the horizontal opening angle of the sonar; k k is the horizontal direction scaling factor; n is the number of sonar beams; r β is the vertical direction angle resolution; β is the vertical opening angle of the sonar; k v is the vertical direction scaling factor.

[0083] Step 3.2, taking the vertex of the sonar sensing range with the azimuth angle of -0.5α and the elevation angle of -0.5β determined by the body-fixed coordinate system recommended by the International Standard Pond Conference as the starting point, and using r α and r β as the discrete resolutions in the horizontal and vertical directions respectively, discretize the sonar sensing range into multiple quadrangular pyramid units (hereinafter referred to as units). The body-fixed coordinate system is the coordinate system that moves with the AUV, with the origin being the center of the AUV or the center of the sonar receiver, the x-axis corresponding to the navigation direction, the y-axis corresponding to the yaw direction, and the Z-axis corresponding to the pitch direction. The elevation angle is the angle difference (with the smallest absolute value) between a certain point and the central plane of the vertical opening angle. The mathematical description of the discretization method is as follows:

[0084]

[0085]

[0086] n t = n l × n v

[0087] where n k is the number of discretized units in the horizontal direction; n v is the number of discretized units in the vertical direction; n t is the total number of units within the sensing range after discretization.

[0088] Step 3.3: Traverse all the units generated above and generate 5 feature points for each unit in the following manner:

[0089]

[0090]

[0091]

[0092]

[0093]

[0094] where is the pair of azimuth angle and elevation angle of the k-th point in the (i, j)-th unit, i = 1, 2,..., n k , j = 1, 2,..., n v , k = 1, 2, 3, 4, 5; α, β, r α and r β have the same meanings as above.

[0095] Step 3.4: Using the above 5 feature points as endpoints, with the (0, 0, 0) point of the body-fixed coordinate system as the starting point, and with the detection range of the sonar to be adopted or already adopted as the length, traverse all the units to generate 5n t line segments. Project all the above line segments from the body-fixed coordinate system to the geodetic coordinate system where the obstacle scene is located based on the body-fixed coordinate system and geodetic coordinate system projection method recommended by the International Standard Pond Conference. The body-fixed coordinate system and geodetic coordinate system projection method is a common standard method and is not a limitation of this patent.

[0096] Step 3.5, based on the line segment and triangle intersection detection algorithm, traverse all cells and perform intersection tests with the obstacle scenario. The line segment and triangle intersection detection algorithm is a classic algorithm in the field of intersection detection and is not limited by this patent. If there is an intersection point between a certain line segment among the 5 line segments located in a certain cell and an obstacle in the obstacle scenario, then the minimum value among the Euclidean distances from the starting point of this line segment to all intersection points is used as the line segment intersection distance of this line segment; if there is no intersection point, then a value slightly larger than the sonar detection range is used as the line segment intersection distance of this line segment. Take the minimum value among the 5 line segment intersection distances as the cell intersection distance. Based on the above method, traverse all cells to generate an intersection matrix. The mathematical description of the above process is as follows:

[0097]

[0098]

[0099]

[0100] where, d i，j，k is the minimum distance (from the starting point of the line segment to the intersection point) between the k-th line segment in the (i, j) cell and the obstacle in the obstacle scenario; f() is the line segment and triangle intersection detection algorithm; V sp , V so , θ j，k , n l and n v have the same meanings as above; d i，j is the minimum distance between the obstacle in the (i, j) cell and the sonar center, that is, the intersection distance; ε is a small constant; H is the intersection matrix.

[0101] Step 3.6, referring to the processing method of projecting the aforementioned sonar three-dimensional perception information into two-dimensional plane information, perform flattening processing on the above H. The processing method is to take the minimum value of each column as the output of the three-dimensional perception method. The mathematical description of the above process is as follows:

[0102]

[0103]

[0104] where, d i is the minimum intersection distance of all cells with row number i; L is the perception output array.

[0105] Embodiment

[0106] Step 1, generate primitive obstacles.

[0107] The primitive obstacle is a fundamental component for constructing an obstacle scenario. Depending on different scenario requirements, the generated primitive obstacle library can vary. When the development and testing of the collision avoidance algorithm are oriented towards random scenarios, mainly random obstacles are generated, and a small number of fixed obstacles can also be generated. When the development and testing are oriented towards typical obstacle scenarios, mainly fixed obstacles are generated.

[0108] Figure 1 Illustrates a fixed obstacle. This obstacle is a U-shaped fixed obstacle. The generation method is as follows: Given 32 vertices with fixed positions, 36 groups of vertex combination orders with 3 vertex numbers in each group are predefined. Using the vertex positions corresponding to the numbers in each group as the three vertices of a spatial triangle, 36 spatial triangles are generated to form the above-mentioned obstacle. Its mathematical description is:

[0109] V ep，U ={p 1 =(x 1 , y 1 , z 1 ), p 2 =(x 2 , y 2 , z 2 ),... p 32 =(x 32 , y 32 , z 32 )}

[0110] V eo，U ={o 1 =(1, 2, 3), o 2 =(2, 3, 4),..., o 36 =(16, 31, 32)}

[0111] Among them, V ep，U is the set of vertex positions of the U-shaped obstacle, containing 32 elements with values of vertex positions and the element values are fixed; V eo，U is the set of vertex orders of the U-shaped obstacle, containing 36 elements with values of vertex combination orders and the element values are fixed, and each element consists of 3 vertex numbers.

[0112] Figure 2 Illustrates a random obstacle. Its generation method is as follows: Randomly given 48 vertex positions, 84 spatial triangles are generated based on 84 predefined vertex combination orders to form the above-mentioned obstacle. Its mathematical description is:

[0113] V ep，R ={p 1 =(x 1 , y 1 , z 1 ), p 2 =(x 2, y 2 , z 2 ),... p 48 =(x 48 , y 48 , z 48 )}

[0114] V eo，R ={o 1 =(1, 2, 3), o 2 =(2, 3, 4),..., o 84 =(40, 46, 48)}

[0115] Among them, V ep，R is a set of random obstacle vertex positions, containing 48 elements with vertex position values randomly given; V eo，R is a set of random obstacle vertex orders, containing 84 elements with vertex combination order values fixed, and each element consists of 3 vertex numbers.

[0116] Based on the above method, simple primitive obstacles such as planes, curved surfaces, cubes, tetrahedrons, polyhedrons, U-shaped bodies, G-shaped bodies, and random types can be generated. By reasonably specifying the positions and combination orders of the vertices, complex primitive obstacles such as sunken ships and ancient cities can be generated.

[0117] Step 2. Construction of the obstacle scene.

[0118] The obstacle scene is divided into a random obstacle scene and a typical obstacle scene. The random obstacle scene can be constructed by randomly combining or specifying one or more fixed or random obstacle methods, or by randomly perturbing the vertex positions. The typical obstacle scene is constructed by specifying a fixed obstacle method.

[0119] The obstacle scene is constructed by selecting and combining the corresponding primitive obstacles according to the clear scene requirements. An example of a random obstacle scene construction method by combining the above U-shaped obstacle and random obstacle is given. First, it is clear that 1 U-shaped obstacle and 1 random obstacle need to be included in the scene; then, the U-shaped obstacle and random obstacle are selected from the generated primitive obstacles for combination. The mathematical description of the combination process is:

[0120] V ep，1 =V ep，U

[0121] V eo，1 =V eo，U

[0122] V ep，2 =V ep，R

[0123] V eo，2 =V eo，R

[0124] V sp，UR = {V ep，1 , V ep，2}

[0125] V so，UR = {V eo，1 , V eo，2}

[0126] Among them, V sp，UR is the set of all primitive obstacle vertex positions in the above obstacle scenario, and the element V ep，1 represents the set of vertex positions of the U-shaped obstacle, and the element V ep，2 represents the set of vertex positions of the random obstacle; V so is the set of all primitive obstacle vertex orders in the obstacle scenario, and the element V eo，1 represents the set of vertex orders of the U-shaped obstacle, and the element V eo，2 represents the set of vertex orders of the random obstacle.

[0127] Step 3, 3D scene perception.

[0128] The function implemented by 3D scene perception is to simulate the obstacle perception and information processing process of a multi-beam forward-looking sonar based on the designed 5n perception method, and convert the obstacles in the scene into the obstacle constraint form required by the obstacle avoidance algorithm. Taking the horizontal opening angle α = 60°, the vertical opening angle β = 10°, the number of beams n = 60, and the detection distance d max = 100 meters as an example to illustrate 3D scene perception. The corresponding relationship between the above parameters and the sonar perception range is as Figure 3 shown.

[0129] In the following process, r α is the resolution of the horizontal direction angle; r β is the resolution of the vertical direction angle; n l is the number of units discretized in the horizontal direction; n v is the number of units discretized in the vertical direction; n t is the total number of units within the perception range after discretization; i and j are the serial numbers of the units in the horizontal and vertical directions respectively, i = 1, 2,..., n k , j = 1, 2,..., n v ; k is the index of the internal feature point of the unit, k = 1, 2, 3, 4, 5; is the azimuth angle and elevation angle pair of the k-th feature point in the (i, j)-th unit; O is the origin of the aforementioned body coordinate system, O = (0, 0, 0); d i，j is the minimum distance between the obstacle and the sonar center in the (i, j)-th unit, that is, the intersection distance; H is the intersection matrix; L is the perception output array.

[0130] Step 3.1, determine the horizontal and vertical angular resolutions based on α, β, and n;

[0131] r α = r β = 1

[0132] Step 3.2, based on r α , r β , α, and β, perform discretization processing on the sensing range;

[0133] n l = 60

[0134] n v = 10

[0135] n t = 60 × 10

[0136] Step 3.3, based on r α , r β , α, β, and the following 5 formulas, generate 5 feature points inside each cell:

[0137]

[0138]

[0139]

[0140]

[0141]

[0142] Step 3.4, based on the method shown in Figure 4 , generate OP 1 , OP 2 , OP 3 , OP 4 , and OP 5 line segments inside each cell and project them into the geodetic coordinate system where the obstacle scene is located;

[0143] Step 3.5, based on the line segment and triangle intersection detection algorithm, traverse the 60 × 10 × 5 line segments and the spatial triangles in the obstacle scene, and perform intersection tests. Figure 5 Schematically shows the intersection situation between a line segment and a triangle, where a and b are the cases without intersection points, and at this time, d i，j，k > 100; c is the case with an intersection point, and at this time, d i，j，k <= 100. Generate d i，j and H according to the aforementioned intersection distance generation method.

[0144] Step 3.6, based on the aforementioned method of projecting three-dimensional information into two-dimensional plane information, take the minimum value of each column in H to generate L. Figure 6 and Figure 7 illustrates the three-dimensional information perception and information processing process. The mathematical example of this process is as follows:

[0145]

[0146] where H 60×10 is a 60×10 intersection matrix; L 60 is the corresponding perception output array.

Claims

1. A method for generating obstacle constraints for the development and testing of real-time obstacle avoidance algorithms for underwater robots, characterized in that, it includes the following steps: 1) Generate various types of primitive obstacles based on the triangulation method; 2) Based on the primitive obstacles, construct a three-dimensional obstacle scene through random combination or structured design; 3) Generate obstacle constraints for the three-dimensional obstacle scene based on the 5n perception method.

2. The method for generating obstacle constraints for the development and testing of real-time obstacle avoidance algorithms for underwater robots according to claim 1, characterized in that, the specific step 1) is: generate different types of primitive obstacles by predefined different vertex positions and the combination order of vertices, that is: V ep ={(x 1 , y 1 , z 1 ),...(x i , y i , z i ),...,(x j , y j , z j ),...,(x k , y k , z k ),...,(x N , y N , z N )} V eo = {(1, 2, 3), (2, 3, 1),...(i, j, k),..., (j, k, N),...} Among them, V ep is the set of all vertex positions in the primitive obstacle. The elements (x i , y i , z i ), (x j , y j , z j ) and (x k , y k , z k ) respectively represent the coordinate values of the vertices numbered i, j, and k in the three-dimensional rectangular coordinate system. N is the number of vertices in the set, and V eo is the set of all vertex combination orders of the primitive obstacle. The elements represent the numbers of the three vertices that form a spatial triangle. For example, (i, j, k) represents the spatial triangle formed by vertices i, j, and k, and the position coordinates of the vertices are (x i , y i , z i ), (x j , y j , z j ) and (x k , y k , z k ).

3. The method for generating obstacle constraints for the development and testing of real-time obstacle avoidance algorithms for underwater robots according to claim 1, characterized in that, the three-dimensional obstacle scene is a structured or unstructured scene generated by randomly combining or structuring a single or multiple primitive obstacles of the same type or different types, expressed as: V sp = {V ep，1 , V ep，2 ,..., V ep，t ,...V ep，M} V so = {V eo，1 , V eo，2 ,... V eo，t ,... V eo，M} Among them, V sp is the set of all vertex positions of the obstacle scenario, and the element V ep，t represents the set of vertex positions of the t-th primitive obstacle in the obstacle scenario. M is the number of primitive obstacles in the obstacle scenario, and V so is the set of vertex orders of the obstacle scenario, and the element V eo，t represents the set of vertex orders of the t-th primitive obstacle in the obstacle scenario.

4. The method for generating obstacle constraints for the development and testing of real-time obstacle avoidance algorithms for underwater robots according to claim 1, characterized in that, the random combination method includes: the method of constructing an obstacle scene by randomly combining multiple primitive obstacles, the method of constructing an obstacle scene by increasing random perturbations of vertex positions, and the method of constructing an obstacle scene by combining fixed primitive obstacles and random primitive obstacles. The obstacle scene constructed based on the random combination method is called a random obstacle scene.

5. The method for generating obstacle constraints for the development and testing of real-time obstacle avoidance algorithms for underwater robots according to claim 1, characterized in that, the structured design method includes: the method of constructing an obstacle scene based on a given type and number of fixed primitive obstacles. The obstacle scene constructed based on the structured design method is called a typical obstacle scene.

6. The method for generating obstacle constraints for the development and testing of real-time obstacle avoidance algorithms for underwater robots according to claim 1, characterized in that, the step 3) includes the following steps: 3.1) Determine the horizontal direction angle resolution and the vertical direction angle resolution according to the characteristics of the sonar that the AUV plans to adopt or has adopted; r β = r α or where r α is the resolution of the horizontal direction angle, α is the horizontal opening angle of the sonar, k l is the horizontal direction scaling factor, n is the number of beams of the sonar, r β is the resolution of the vertical direction angle, β is the vertical opening angle of the sonar, k v is the vertical direction scaling factor; 3.2) Starting from the vertex of the sonar perception range with an azimuth angle of -0.5α and an elevation angle of -0.5β determined in the body coordinate system, and using r α and r β as the discrete resolutions in the horizontal and vertical directions respectively, discretize the sonar perception range into multiple quadrangular pyramid units: n t = n l × n v where n l is the number of discretized units in the horizontal direction, n v is the number of discretized units in the vertical direction, n t is the total number of units within the sensing range after discretization; 3.3) Traverse all tetrahedral pyramid units and generate 5 feature points for each unit; Among them, is the azimuth and elevation angle pair of the k-th point in the (i, j) unit, where i = 1, 2, …, n l , j = 1, 2, …, n v , k = 1, 2, 3, 4, 5; 3.4) With five feature points as the end points and the (0, 0, 0) point of the body-fixed coordinate system as the starting point, using or having used the detection range of the sonar as the length, traverse all the tetrahedral pyramid units to generate 5n t line segments, and project all the line segments from the body-fixed coordinate system to the geodetic coordinate system where the obstacle scene is located; 3.5) Based on the line segment and triangle intersection detection algorithm, traverse all tetrahedral pyramid units and perform intersection tests with the obstacle scene respectively. If there is an intersection point between a certain line segment among the 5 line segments in a certain unit and the obstacles in the obstacle scene, then take the minimum value of the Euclidean distances from the starting point of this line segment to all intersection points as the line segment intersection distance of this line segment; if there is no intersection point, then take a value whose difference from the sonar detection range is within the threshold range as the line segment intersection distance of this line segment, and take the minimum value of the 5 line segment intersection distances as the unit intersection distance. Traverse all units to generate the intersection matrix H; where d i,j,k is the minimum distance between the k-th line segment in the (i, j) unit and the meta-obstacle in the obstacle scenario, f() is the line segment and triangle intersection detection algorithm, d i,j is the minimum distance between the obstacle in the (i, j) unit and the sonar center, that is, the intersection distance, and ε is a constant; 3.6) Take the minimum value of each column in the intersection matrix H as the output of the three-dimensional perception method and flatten the intersection matrix H. where d i is the minimum intersection distance of all cells with row number i; L is the perception output array, which serves as the contour information of the three-dimensional obstacles sensed in real time during the AUV navigation process.

Images