An environmental model establishment and online path planning method based on sector area division

Abstract

An environmental model establishment and online path planning method based on sector area division, which belongs to the technical field of AUV path planning. The present invention solves the problems that the existing environmental model construction methods do not consider the limited environmental information, resulting in the inability to accurately establish an environmental model, the low efficiency of the existing environmental model construction methods in processing environmental information, and the existing path planning algorithms ignore the efficiency of path planning and thus cannot be applied to the actual environment. The present invention divides the sonar sector detection area on the horizontal plane into grids and numbers them; records the obstacle information detected by the beam in the central axis direction of each sector grid, and establishes an environmental model according to the detected obstacle information; calculates the comprehensive cost of each sector grid according to the established environmental model; selects the sector grid with the smallest comprehensive cost value for navigation according to the calculation result of the comprehensive cost. The method of the present invention can be applied to environmental model establishment and online path planning.

Description

Technical Field

[0001] The present invention belongs to the technical field of AUV (Autonomous Underwater Vehicle) path planning, and particularly relates to an environmental model establishment and online path planning method based on sector area division. Background Art

[0002] With the continuous in-depth development of the marine industry, AUV, as the main platform for underwater operations, has always been a research hotspot in this field. Online path planning is one of the key technologies reflecting the autonomous and intelligent level of AUV, mainly including two important parts: environmental model establishment and path search algorithm. Due to the complex and changeable characteristics of the actual marine environment, how to online plan a safe, feasible and highly efficient mission execution navigation path based on limited sensing information is the key technical problem restricting AUV from performing complex tasks, and also an important technical means to improve the intelligent level of underwater unmanned equipment. For the online path planning task, establishing a reasonable environmental model is of crucial significance for improving the efficiency of path search, accelerating the algorithm operation speed and reducing the calculation amount.

[0003] In the literature (OctoMap: an efficient probabilistic 3D mapping framework based on octrees), Hornung et al. proposed an open-source framework for generating a volumetric 3D environmental model based on the octree model to model the environment in an accurate manner while minimizing memory requirements. However, this method is not suitable for real-time path planning tasks. In the literature (Path Planning for Autonomous Underwater Vehicles), Petres et al. used the C-space method for modeling and applied it to real AUV path planning experiments. However, it can only be applied to static obstacle environments currently, and the lack of environmental information will affect the optimality of the path. In the literature (Intelligent Vector Field Histogram based collision avoidance method for AUV), Zhang Gengshi et al. proposed an obstacle model of a directed cuboid, which covers all obstacle grids with less storage space and avoids excessive loss of free space, effectively realizing the simplification of environmental information. However, this method is not very suitable for path planning under limited environmental information. At present, the grid method is mostly used for the construction of environmental models, and the selection of grid size is one of the important factors affecting the algorithm performance. In 3D space path planning, the data volume brought by the grid method increases exponentially, greatly increasing the occupation of system resources. Therefore, in a wide range of underwater environments, many scholars ignore the water depth and simplify the 3D space into a 2D environmental model. However, further processing of the 2D grid map is still required for applying specific path planning algorithms, which is less efficient for online path planning.

[0004] Path search algorithms are methods that search for paths between a planned starting point and an ending point based on an established environmental model. They have achieved fruitful results and have been widely applied. In the literature (Closed-loop randomized kinodynamic path planning for an autonomous underwater vehicle), Ehsan Taheri et al. considered the kinematic and dynamic constraints of an AUV and proposed a closed-loop rapidly-exploring random tree (CL-RRT) algorithm to quickly design a feasible path from an initial position and velocity to a target position and velocity in a three-dimensional cluttered space. This traditional path planning algorithm is relatively mature, generally has high real-time performance and applicability, and has low dependence on the environment. However, in an unknown environment, optimality often cannot be satisfied. In the literature (Deep reinforcement learning for adaptive path planning and control of an autonomous underwater vehicle), Behnaz-Hadi et al. proposed an AUV adaptive motion planning and obstacle avoidance technology based on deep reinforcement learning. Decision-making is carried out through navigation measurements, and no video images are required during the training phase. A short, safe, and directed path to the target is generated by designing a reward function. Intelligent algorithms play an important and effective role in solving information path search problems in complex dynamic environments, but they generally have problems such as slow processing speed, poor stability and real-time performance, and being easily trapped in local optima. When an AUV cannot maintain a stationary state in place to wait for the vehicle to plan a local path, the existing path planning algorithms ignore the path planning efficiency, resulting in inapplicability to the actual environment.

[0005] In summary, for the online path planning task, the existing environmental model construction and online path planning methods still have the following problems:

[0006] 1. The existing environmental model construction methods ignore the real sonar model, resulting in low efficiency of the AUV in processing environmental information, thereby reducing the efficiency of online path planning.

[0007] 2. Due to the detection characteristics of sonar, the AUV cannot obtain complete obstacle information. The existing environmental model construction methods do not consider the problem of limited environmental information during the online path planning process, so an accurate environmental model cannot be established.

[0008] 3. In online path planning, an AUV needs to have a certain speed to maintain a constant depth of navigation. Therefore, most AUVs cannot stay stationary in place to wait for the vehicle to plan a local path. Existing path planning algorithms often ignore the path planning efficiency, resulting in inapplicability to the actual environment. Summary of the Invention

[0009] The object of the present invention is to solve the problems that the existing environmental model construction method fails to consider the limited environmental information and thus cannot accurately establish an environmental model, the existing environmental model construction method has low efficiency in processing environmental information, and the existing path planning algorithm ignores the path planning efficiency and is inapplicable to the actual environment, and a method for establishing an environmental model and online path planning based on sector area division is proposed.

[0010] The technical solution adopted by the present invention to solve the above technical problems is: a method for establishing an environmental model and online path planning based on sector area division, and the method specifically includes the following steps:

[0011] Step 1: At time t, with the position of the sonar as the vertex, grid-divide the sonar fan-shaped detection area on the horizontal plane, and number each divided sector grid;

[0012] Step 2: Record the obstacle information detected by the beam in the middle axis direction of each sector grid, store the detected obstacle information in the matrix Obs, and the obtained matrix Obs is the established environmental model;

[0013] Among them, the dimension of the matrix Obs is N×2, and N is the total number of divided sector grids;

[0014] Step 3: Calculate the comprehensive cost of each sector grid according to the established environmental model;

[0015] Step 4: According to the calculation result in Step 3, select the sector grid i with the smallest comprehensive cost value * for navigation, that is

[0016] until time t + 1, and then return to execute Step 1.

[0017] Further, the grid division of the sonar fan-shaped detection area on the horizontal plane and the numbering of each divided sector grid are specifically as follows:

[0018] When the value of α H / α F is odd, the central angle of each divided sector grid is α F ;

[0019] When the value of α H / α FWhen the value is even, the central angles of the non-boundary sector grids obtained by division are all α F , and the central angles of the left boundary sector grid and the right boundary sector grid are both α F / 2;

[0020] If the sector grids are numbered in clockwise order, after numbering, the number of the sector grid where the α deflection angle is located is where, represents rounding down.

[0021] Furthermore, the central angle α F =n·α R , n is an integer greater than or equal to 1, and α R is the direction resolution of the sonar transmitting sound waves.

[0022] Furthermore, the obstacle information is:

[0023] Taking the i-th sector grid as an example

[0024] If there is an obstacle in the middle axis direction of the i-th sector grid, the obstacle information detected by the beam is where, is the Euclidean distance between the AUV and the obstacle detected by the beam in the middle axis direction of the i-th sector grid, is the heading angle of the obstacle detected by the beam in the middle axis direction of the i-th sector grid in the AUV body coordinate system;

[0025] If there is no obstacle in the middle axis direction of the i-th sector grid, the obstacle information in the i-th sector grid is represented as [NaN,NaN], and [NaN,NaN] means that no obstacle is detected by the beam in the middle axis direction of the i-th sector grid.

[0026] Furthermore, in the body coordinate system, the heading angle α of the AUV when navigating in the i-th sector grid i is: α i =i·α F .

[0027] Furthermore, the specific process of the third step is:

[0028]

[0029] where, is the comprehensive cost of the i-th sector grid, ω m is the weight coefficient, m = 1,2,...,5, is the obstacle distance threat cost of the i-th sector grid, is the obstacle heading threat cost of the i-th sector grid, is the turning cost of the i-th sector grid, is the end-heading deviation cost of the i-th sector grid, is the additional path cost of the i-th sector grid.

[0030] Furthermore, the obstacle distance threat cost and the obstacle-heading threat cost are calculated as follows:

[0031] Calculate the distance threat level of the obstacle within the i-th sector grid

[0032]

[0033] where, when there is no obstacle in the middle-axis direction of the i-th sector grid, then is infinity, d safe represents the minimum safety distance, and R S is the maximum detection range of the sonar;

[0034] According to the distance threat level of the obstacle, assign the obstacle distance threat cost to the i-th sector grid:

[0035]

[0036] Express the heading threat level of the obstacle existing on the middle axis of the j-th sector grid to the i-th sector grid as:

[0037]

[0038] where, N gap is the maximum number of sector grid intervals with obstacle-heading threat, |i - j| is the number of sector grids between the j-th sector grid and the i-th sector grid, e is the base of the natural logarithm, and σ ψobs is the obstacle-heading threat level parameter;

[0039] Then the obstacle-heading threat cost of the i-th sector grid is:

[0040]

[0041] where, J represents that the i-th sector grid is under the obstacle-heading threat from J sector grids.

[0042] Furthermore, the calculation method of the turning cost is as follows:

[0043]

[0044] where, Denote the heading angle of the i-th sector grid in the vehicle coordinate system, σ αF Denote the turning angle constraint parameter.

[0045] Furthermore, the end-point heading deviation cost and the extra path cost are calculated as follows:

[0046]

[0047] where, ψ Udev denotes the angle between the AUV heading and the end-point direction, σ dev denotes the end-point heading deviation constraint parameter;

[0048] The extra path cost is expressed as:

[0049]

[0050] where, Δl i denotes the extra path generated by the AUV when selecting the i-th sector grid for navigation, l T denotes the Euclidean distance between the AUV and the position at the next moment.

[0051] Even further, the angle ψ between the AUV heading and the end-point direction Udev is:

[0052] ψ Udev = ψ UUV - ψ end

[0053] where, ψ UUV denotes the heading angle of the AUV in the geodetic coordinate system, ψ end denotes the angle between the line connecting the AUV and the end-point in the geodetic coordinate system.

[0054] The beneficial effects of the present invention are:

[0055] The present invention takes into account the actual detection characteristics of sonar. After a fixed time interval, it reconstructs the local environment map by dividing the fan-shaped grid, which can avoid the problem that the sound wave is blocked by the front obstacle and cannot detect the rear obstacle, resulting in inaccurate establishment of the environmental model. Moreover, the present invention only establishes the local environment map within the detection area each time, thereby simplifying the environmental information to be processed each time, reducing the algorithm operation amount, and improving the efficiency of online path planning. At the same time, based on the established fan-shaped grid map, considering the safety, smoothness, and rapidity indexes of navigation, the present invention proposes an AUV online path planning method based on fan-shaped area division with the ability to pass through narrow channels, realizing multi-objective optimization, improving the efficiency of path planning, providing a theoretical basis and feasible solution for solving the AUV online path planning problem in complex marine environments, and being more applicable to the actual environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 is a flowchart of an environmental model establishment and online path planning method based on fan-shaped area division;

[0057] Figure 2 is a schematic diagram of grid division of the sonar fan-shaped detection area;

[0058] In the figure, the x-axis and y-axis are the x-axis and y-axis of the carrier coordinate system. The carrier coordinate system takes the position of the AUV centroid as the origin O. The positive directions of the x-axis, y-axis, and z-axis are the front, right, and bottom of the AUV respectively. The multi-beam forward-looking sonar is installed at the head of the AUV, and its axis coincides with the x-axis direction of the AUV carrier coordinate system, α H is the horizontal opening angle of the sonar fan-shaped detection area, and R S is the detection distance of the sonar;

[0059] Figure 3 is a schematic diagram of the environmental model simulation of sonar fan-shaped area division in online path planning;

[0060] The fan shape represents the detection range of the sonar, and * represents the obstacles in the fan-shaped grid, that is, the data points detected by the sonar;

[0061] Figure 4 is a simulation result diagram of online path planning under an environmental model of a certain fan-shaped area division;

[0062] The starting point coordinates are (100, 100), the ending point coordinates are (600, 1000), and * represents the data points detected by the sonar during the whole process of online path planning;

[0063] Figure 5 is a schematic diagram of the obstacle distance threat;

[0064] As the relative distance between the obstacle and the AUV decreases, the threat level continuously increases; when it is less than or equal to the minimum safe distance, the threat level reaches the maximum.

[0065] Figure 6 It is a schematic diagram of the threat level of the obstacle's heading.

[0066] When the obstacle is far away from the fan-shaped grid i, the fan-shaped grid i is not threatened by the obstacle's heading.

[0067] Figure 7 It is a schematic diagram of the relative heading cost of the fan-shaped grid.

[0068] The greater the turning angle of the AUV, the greater the cost.

[0069] Figure 8 It is a schematic diagram of the cost of deviating from the end heading.

[0070] The greater the degree to which the AUV's heading deviates from the end heading, the greater the cost.

[0071] Figure 9 It is a schematic diagram of the additional path cost of the fan-shaped grid.

[0072] The longer the additional path generated, the greater the cost.

[0073] Figure 10 It is a schematic diagram of calculating the additional path length generated by path planning based on the fan-shaped grid. Detailed implementation manners

[0074] Detailed implementation manner one: Combined with Figure 1 Describe this implementation manner. The method for establishing an environmental model and online path planning based on fan-shaped area division described in this implementation manner specifically includes the following steps:

[0075] Step 1: At time t, with the position where the sonar is located as the vertex, divide the sonar fan-shaped detection area on the horizontal plane into grids, and number each of the divided fan-shaped grids.

[0076] Step 2: Record the obstacle information detected by the beam in the middle axis direction of each fan-shaped grid, store the detected obstacle information in the matrix Obs, and the obtained matrix Obs is the established environmental model.

[0077] Among them, the dimension of the matrix Obs is N×2, and N is the total number of divided fan-shaped grids.

[0078] Step 3: Calculate the comprehensive cost of each fan-shaped grid according to the established environmental model.

[0079] Step 4: According to the calculation result in Step 3, select the fan-shaped grid i with the minimum comprehensive cost value *Navigation, that is

[0080] Until the arrival time t+1, then return to execute Step 1.

[0081] Due to the detection characteristics of the forward-looking sonar, after the sound wave is emitted and hits an obstacle, an echo is emitted. Therefore, the environment behind the obstacle cannot be detected, and thus a complete environmental map cannot be established. In the present invention, after a fixed time interval, a local environmental map is rebuilt, which can meet the requirements of online path planning. In the existing method that only constructs the environmental map once, due to the sound wave being blocked by the front obstacle, the rear obstacle cannot be detected, so the environmental map cannot be accurately established. Each time the present invention only constructs a local environmental map within the detection area, which also has the advantages of reducing the occupancy rate of stored information and improving the algorithm operation speed.

[0082] Such as Figure 3 and Figure 4 As shown, an environmental model is established by dividing the fan-shaped area, and the path planning result is obtained by using the online path planning algorithm based on the fan-shaped area division.

[0083] Specific Embodiment 2: Combine Figure 2 To illustrate this embodiment. The difference between this embodiment and Specific Embodiment 1 is that the sonar fan-shaped detection area on the horizontal plane is divided into grids, and each divided fan-shaped grid is numbered; specifically:

[0084] When the value of α H / α F is odd, the central angle of each divided fan-shaped grid is α F ;

[0085] When the value of α H / α F is even, the central angles of the non-boundary fan-shaped grids obtained by the division are all α F , and the central angles of the left boundary fan-shaped grid and the right boundary fan-shaped grid are both α F / 2;

[0086] The fan-shaped grids are numbered in the clockwise direction in sequence. After numbering, the number of the fan-shaped grid where the α deflection angle is located is wherein, represents rounding down.

[0087] Other steps and parameters are the same as those in Specific Embodiment 1.

[0088] By dividing the fan-shaped grid, the present invention can simplify the environmental information while retaining many details, improving the obstacle avoidance ability of the AUV in complex environments. The information obtained by the forward sonar is usually local information. When path planning is carried out under the global map, it is necessary to convert the local information to the global map, which will cause a large amount of calculation and time consumption and cannot meet the requirements of real-time collision avoidance of the AUV. By dividing the fan-shaped grid, direct storage and utilization of local environmental information can be realized, avoiding a large amount of operations and improving the efficiency of online path planning.

[0089] When the present invention divides the grid, the axis of the sonar is coincident with the central axis of the central fan-shaped grid to ensure that the heading of the AUV remains unchanged when it sails along the middle fan-shaped grid. When the AUV sails within the boundary fan-shaped grid and the central angle of the boundary fan-shaped grid is When, the heading of the AUV selects the outermost beam heading of the boundary grid.

[0090] Specific Embodiment 3: The difference between this embodiment and Specific Embodiment 1 or 2 is that the central angle α F =n·α R , where n is an integer greater than or equal to 1, and α R is the direction resolution of the sound wave emitted by the sonar.

[0091] Other steps and parameters are the same as those in Specific Embodiment 1 or 2.

[0092] Specific Embodiment 4: The difference between this embodiment and any one of Specific Embodiments 1 to 3 is that the obstacle information is:

[0093] Taking the i-th fan-shaped grid as an example

[0094] If there is an obstacle in the middle axis direction of the i-th fan-shaped grid, the obstacle information detected by the beam is Among them, is the Euclidean distance between the AUV and the obstacle detected by the beam in the middle axis direction of the i-th fan-shaped grid, is the heading angle of the obstacle detected by the beam in the middle axis direction of the i-th fan-shaped grid in the AUV body coordinate system;

[0095] When the value of α H / α F is odd, the heading angle is the deflection angle of the grid central axis. When the value of α H / α F is even, for the left boundary grid, the heading angle is the deflection angle of the left boundary of the grid; for the right boundary grid, the heading angle is the deflection angle of the right boundary of the grid.

[0096] If there is no obstacle in the central axis direction of the i-th sector grid, the obstacle information in the i-th sector grid is represented as [NaN, NaN], and [NaN, NaN] indicates that no obstacle is detected by the beam in the central axis direction of the i-th sector grid.

[0097] Other steps and parameters are the same as those in any one of the specific embodiments one to three.

[0098] Specific embodiment five: The difference between this embodiment and any one of the specific embodiments one to four is that, in the carrier coordinate system, the heading angle α when the AUV sails in the i-th sector grid i is: α i = i·α F .

[0099] Other steps and parameters are the same as those in any one of the specific embodiments one to four.

[0100] Specific embodiment six: The difference between this embodiment and any one of the specific embodiments one to five is that the specific process of step three is:

[0101]

[0102] where, is the comprehensive cost of the i-th sector grid, ω m is the weight coefficient, m = 1, 2,..., 5, is the obstacle distance threat cost of the i-th sector grid, is the obstacle heading threat cost of the i-th sector grid, is the turning cost of the i-th sector grid, is the end heading deviation cost of the i-th sector grid, is the additional path cost of the i-th sector grid.

[0103] Other steps and parameters are the same as those in any one of the specific embodiments one to five.

[0104] The values of the obstacle distance threat cost, the obstacle heading threat cost, the turning cost, the end heading deviation cost, and the additional path cost are all normalized values within the interval [0, 1]. The higher the value, the greater the cost of the sector grid.

[0105] Specific embodiment seven: This embodiment is described in combination with Figure 5 and Figure 6 The difference between this embodiment and any one of the specific embodiments one to six is that the obstacle distance threat cost and the obstacle heading threat cost are calculated as follows:

[0106] Calculate the distance threat degree of the obstacle in the i-th sector grid

[0107]

[0108] Among them, when there is no obstacle in the axial direction of the i-th sector grid, then is infinity, d safe represents the minimum safety distance, and R S is the maximum detection range of the sonar;

[0109] According to the distance threat level of the obstacle, assign the obstacle distance threat cost to the i-th sector grid:

[0110]

[0111] Express the heading threat level of the obstacle existing on the axis of the j-th sector grid to the i-th sector grid (in the form of a Gaussian function):

[0112]

[0113] Among them, N gap is the maximum number of intervals of the sector grid with a heading threat of an obstacle, |i - j| is the number of sector grids separated between the j-th sector grid and the i-th sector grid, e is the base of the natural logarithm, and σ ψobs is the parameter of the obstacle heading threat level (a suitable value can be selected according to experience and debugging);

[0114] Then the obstacle heading threat cost of the i-th sector grid is:

[0115]

[0116] Among them, J represents that the i-th sector grid is affected by the obstacle heading threat from J sector grids.

[0117] Other steps and parameters are the same as those in any one of the specific implementation manners one to six.

[0118] Since the AUV is threatened by the distance and heading of the obstacle during navigation, for the sector grid i, the design of the cost function is related to the relative distance between the obstacle on the sector grid i and the AUV and the relative deflection angle between the sector grid where any obstacle is located and this sector grid. Therefore, by considering the obstacle distance threat cost and the obstacle heading threat cost fully ensures the safety of the AUV navigation path.

[0119] Specific implementation manner eight: Combine Figure 7 to illustrate this implementation manner. The difference between this implementation manner and any one of the specific implementation manners one to seven is that the turning angle cost The calculation method is as follows:

[0120]

[0121] Wherein, represents the heading angle of the i-th sector grid in the carrier coordinate system, and σ αF represents the turning angle constraint parameter (a suitable value can be selected according to experience and debugging).

[0122] Other steps and parameters are the same as those in any one of the specific embodiments one to seven.

[0123] The present invention constrains the turning angle of the AUV, which can make the path as smooth as possible, reduce large-angle turning, and the smooth path can improve the effectiveness of subsequent path tracking and the navigation efficiency of the AUV.

[0124] Specific embodiment nine: Combine Figure 8 and Figure 9 to illustrate this embodiment. The difference between this embodiment and any one of the specific embodiments one to eight is that the end-heading deviation cost and the additional path cost The calculation method is as follows:

[0125]

[0126] Wherein, ψ Udev represents the angle between the AUV heading and the end direction, and σ dev represents the end-heading deviation constraint parameter (a suitable value can be selected according to experience and debugging);

[0127] The additional path cost is expressed as:

[0128]

[0129] Wherein, Δl i represents the additional path generated by the AUV when selecting the i-th sector grid for navigation, and l T represents the Euclidean distance between the AUV and the next moment position.

[0130] Other steps and parameters are the same as those in any one of the specific embodiments one to eight.

[0131] In an ideal obstacle-free environment, the shortest path moving direction is to move towards the end point. However, in reality, the AUV needs to avoid obstacles in the environment, which will generate additional paths. As Figure 10 shown, l u_e is the Euclidean distance from the AUV to the end point. Let the position and heading angle of the AUV at the current moment be [x u , y u , ψu T During the navigation cycle, the AUV moves in a straight line, and the Euclidean distance between the AUV and the node at the next moment is l T , and the coordinates of the node where the AUV reaches the i-th sector grid at the next moment in the geodetic coordinate system are:

[0132]

[0133] The node at the next moment The Euclidean distance from the end point is The node at the next moment that the AUV reaches by moving towards the end point is P o , P o The Euclidean distance from P to the end point is:

[0134] l o_e =l u_e -l T

[0135] The additional path generated by the AUV choosing to navigate in the i-th sector grid is:

[0136]

[0137] When the AUV moves towards the end point, Δl i is at least 0; when the AUV moves away from the end point, Δl i is at most 2l T .

[0138] The present invention enables the AUV to preferably select a sector grid with a course and distance close to the end point, which can ensure that the planned path is as short as possible and meet the rapidity requirement of the planned path.

[0139] Specific Embodiment Ten: The difference between this embodiment and any one of Embodiments One to Nine is that the included angle ψ between the AUV course and the end point direction Udev is:

[0140] ψ Udev =ψ UUV -ψ end

[0141] where ψ UUV represents the course angle of the AUV in the geodetic coordinate system, and ψ end represents the angle between the AUV and the direction of the end point connection in the geodetic coordinate system.

[0142] Other steps and parameters are the same as any one of Embodiments One to Nine.

[0143] ​The above numerical examples of the present invention are only for explaining in detail the calculation model and calculation process of the present invention, rather than limiting the implementation manners of the present invention. For those of ordinary skill in the art, other different forms of changes or variations can be made on the basis of the above description. It is impossible to list all the implementation manners here. Any obvious changes or variations derived from the technical solutions of the present invention still fall within the protection scope of the present invention.

Claims

1. An environmental model establishment and online path planning method based on sector area division, characterized in that The method specifically includes the following steps: Step 1: At time t, taking the position where the sonar is located as the vertex, grid-divide the sonar fan-shaped detection area on the horizontal plane, and number each of the divided fan-shaped grids; The grid division of the sonar fan-shaped detection area on the horizontal plane and the numbering of each of the divided fan-shaped grids are specifically as follows: When α H / α F is an odd number, the central angle of each sector grid obtained by division is α F , α H is the horizontal opening angle of the sonar sector detection area; When α H / α F is an even number, the central angles of the non-boundary sector grids obtained by division are all α F , and the central angles of the left boundary sector grid and the right boundary sector grid are both α F / 2; The fan-shaped grids are numbered in a clockwise direction in sequence. After numbering, the number of the fan-shaped grid where the α deflection angle is located is wherein, represents rounding down; Step 2: Record the obstacle information detected by the beam in the central axis direction of each fan-shaped grid, store the detected obstacle information in the matrix Obs, and the obtained matrix Obs is the established environmental model; Among them, the dimension of the matrix Obs is N×2, and N is the total number of the divided fan-shaped grids; Step 3: Calculate the comprehensive cost of each fan-shaped grid according to the established environmental model; Step 4: According to the calculation result in Step 3, select the fan-shaped grid i with the minimum comprehensive cost value * Navigation, that is is the comprehensive cost of the i-th fan-shaped grid; Until time t+1 is reached, then return to execute Step 1.

2. The environmental model establishment and online path planning method based on fan-shaped area division according to claim 1, wherein The central angle α F = n·α R , where n is an integer greater than or equal to 1, and α R is the direction resolution of the sonar emitted sound wave.

3. The method for establishing an environmental model based on sector area division and online path planning according to claim 1, wherein The obstacle information is: Taking the i-th fan-shaped grid as an example If there is an obstacle in the middle axis direction of the i-th sector grid, the obstacle information detected by the beam is where is the Euclidean distance between the AUV and the obstacle detected by the beam in the middle axis direction of the i-th sector grid, is the heading angle of the obstacle detected by the beam in the middle axis direction of the i-th sector grid in the AUV body coordinate system; If there is no obstacle in the central axis direction of the i-th fan-shaped grid, the obstacle information in the i-th fan-shaped grid is represented as [NaN, NaN], and [NaN, NaN] means that the beam in the central axis direction of the i-th fan-shaped grid does not detect an obstacle.

4. The environmental model establishment and online path planning method based on sector area division according to claim 3, characterized in that, Under the carrier coordinate system, the heading angle α of the AUV when navigating within the i-th sector grid i is: α i = i · α F .

5. The environmental model establishment and online path planning method based on sector area division according to claim 3, characterized in that, The specific process of Step 3 is: Among them, is the comprehensive cost of the i-th sector grid, ω m is the weight coefficient, m = 1, 2,..., 5, is the threat cost of obstacle distance of the i-th sector grid, is the threat cost of obstacle course of the i-th sector grid, is the turning cost of the i-th sector grid, is the end course deviation cost of the i-th sector grid, is the additional path cost of the i-th sector grid.

6. The environmental model establishment and online path planning method based on fan-shaped area division according to claim 5, wherein The obstacle distance threat cost and the obstacle heading threat cost are calculated as follows: Calculate the distance threat level of obstacles within the i-th sector grid Among them, when there is no obstacle in the axial direction of the i-th sector grid, then is infinite, d safe represents the minimum safety distance, and R S is the maximum detection range of the sonar; According to the distance threat degree of the obstacle, assign the obstacle distance threat cost to the i-th fan-shaped grid: The heading threat degree of the obstacle existing on the central axis of the j-th fan-shaped grid to the i-th fan-shaped grid is expressed as: Among them, N gap is the maximum number of sector grids with obstacle heading threats, |i - j| is the number of sector grids between the j-th sector grid and the i-th sector grid, e is the base of the natural logarithm, and σ ψobs is the obstacle heading threat degree parameter; Then the obstacle heading threat cost of the i-th sector grid is as follows: Among them, J represents that the i-th fan-shaped grid is affected by obstacles' heading threats from J fan-shaped grids.

7. A method for establishing an environmental model based on sector area division and online path planning according to claim 5, characterized in that, The corner cost is calculated as follows: Among them, represents the heading angle of the i-th sector grid in the carrier coordinate system, and σ αF represents the corner constraint parameter.

8. A method for establishing an environmental model based on sector region division and online path planning according to claim 5, characterized in that The end-course deviation cost and the additional path cost are calculated as follows: Among them, ψ Udev represents the angle between the AUV heading and the end direction, and σ dev represents the end heading deviation constraint parameter; Additional path cost Expressed as: where, Δl i represents the additional path generated when the AUV sails in the i-th sector grid, and l T represents the Euclidean distance between the AUV and its position at the next moment.

9. The method for establishing an environmental model based on sector area division and online path planning according to claim 8, wherein The included angle ψ between the heading of the AUV and the end direction Udev is as follows: ψ Udev = ψ UUV - ψ end where ψ UUV represents the heading angle of the AUV in the geodetic coordinate system, and ψ end represents the angle between the AUV and the direction of the line connecting to the end point in the geodetic coordinate system.

Images