AUV underwater local path planning method and system based on improved VFH algorithm

Abstract

The invention discloses an AUV underwater local path planning method and system based on an improved VFH algorithm, and the method comprises the steps: converting the environment where an AUV is located into a three-dimensional grid map, obtaining the distance data of the surrounding environment of the AUV, processing the data, setting the advancing direction, dividing a space into beta * omega spherical sectors according to the resolution alpha, and carrying out the calculation of the beta * omega spherical sectors; calculating an obstacle strength value and determining a sector to which each grid belongs, constructing a sector strength matrix graph for the three-dimensional grid map, setting a threshold value and screening passable sectors, combining adjacent passable sectors and selecting reserved wide valleys, and calculating cost by adopting an improved cost function; the method comprises the following steps: selecting a geometric center line direction with minimum cost and reserved wide valley as a moving direction, constructing a three-dimensional dynamic situation field, calculating a step length base value according to a potential field gradient and an adaptive step length formula, performing correction and dynamic adjustment processing, then determining a final step length, moving the AUV according to the final step length, updating the state, and circulating until the AUV reaches a target. And safe and efficient path planning of the AUV is realized.

Description

TECHNICAL FIELD

[0001] The application particularly relates to an AUV underwater local path planning method and system based on an improved VFH algorithm, and belongs to the technical field of path planning. BACKGROUND

[0002] In the field of underwater robot research and application, path planning of an AUV is a key technology for realizing autonomous navigation and task execution of the AUV. The traditional VFH algorithm, as a classical local path planning method, is mainly applicable to a two-dimensional plane environment and has obvious limitations in dealing with complex scenes in a three-dimensional underwater space.

[0003] The underwater environment has characteristics such as three-dimensionality, complexity and variability, and various obstacles and ocean currents exist as interference factors. However, in the prior art, the traditional VFH algorithm does not fully consider the pitch and yaw kinematic constraints of the AUV in the three-dimensional space, cannot effectively deal with the obstacle distribution in the three-dimensional space, leads to inflexible obstacle avoidance, is prone to path planning errors, cannot effectively identify narrow passages to cause the AUV to be unable to pass through or to collide and cause accidents, does not fully combine the underwater specific influencing factors, the direction selection is not reasonable enough, at the same time, the traditional algorithm does not combine the influence of the direction of the ocean current on energy consumption, and has deficiencies in energy consumption optimization, so that the endurance of the AUV is limited when the AUV is working underwater, the sailing efficiency is low, in addition, the traditional algorithm does not introduce sensor confidence to dynamically adjust the planning strategy, cannot dynamically adjust the step length, and has poor real-time path change response capability, the rationality of path planning lacks foresight and the autonomous response capability of quickly and accurately adapting to the optimal path.

[0004] Therefore, there is an urgent need for an AUV local path planning method applicable to a three-dimensional underwater environment to solve the above problems. SUMMARY

[0005] To solve the problems mentioned in the background art, the purpose of the present application is to provide an AUV underwater local path planning method based on an improved VFH algorithm.

[0006] The AUV underwater local path planning method based on the improved VFH algorithm is as follows: the environment in which the AUV is located is converted into a three-dimensional grid map and the distance data of the AUV's surrounding environment is obtained; the data is denoised and the AUV's travel direction is set; the space is divided into β×ω spherical sectors according to the resolution α; the obstacle intensity value is calculated and the sector to which each grid belongs is determined; a sector intensity matrix diagram is constructed for the three-dimensional grid map; a threshold is set and passable sectors are screened; adjacent passable sectors are merged and wide valleys are selected to be retained; the cost is calculated using an improved cost function; the geometric centerline direction of the retained wide valley with the minimum cost is selected as the movement direction; a three-dimensional dynamic potential field is constructed; the step size base value is calculated according to the potential field gradient and the adaptive step size formula; the final step size is determined after correction and dynamic adjustment; the AUV moves according to the final step size and updates the state, and the process is repeated until the target is reached.

[0007] Preferably, the AUV underwater local path planning method based on the improved VFH algorithm comprises the following steps:

[0008] S1, using the center position of the AUV as the coordinate origin, convert the AUV's environment into a three-dimensional grid map;

[0009] S2. Obtain the distance data of the AUV's surrounding environment, use wavelet threshold denoising, and set the heading angle of the AUV's travel direction to 0°, and the yaw angle θ and pitch angle φ are both in the range of -90° to 90°;

[0010] S3. Divide the spherical surface into horizontal and vertical sectors according to the resolution α. ​​The horizontal and vertical directions are divided into 180° / α sectors. The horizontal direction is numbered β in the counterclockwise direction, and the vertical direction is numbered ω from bottom to top. The space is divided into β×ω spherical sectors.

[0011] S4. Calculate the obstacle intensity value M of each grid in each spherical sector θ φ and the sum of the obstacle strength values ​​of each spherical sector H βω , determine the sector to which each grid belongs;

[0012] S5. Convert the three-dimensional grid map into a Yaw-Pitch sector intensity matrix, where the H of each spherical sector is βω All correspond to the (β, ω) position in the two-dimensional grid coordinate system;

[0013] S6. Set the threshold TH. When the total intensity value H βω When it is less than the threshold TH, the spherical sector is defined as the candidate area, and the polar coordinate two-dimensional grid map data is binarized;

[0014] S7, merge adjacent ones that satisfy H βωThe spherical sector less than the threshold TH forms a wide valley, and a reserved wide valley is selected according to a span condition;

[0015] S8, a cost is calculated according to the improved cost function, and a geometric center line direction of the reserved wide valley with the minimum cost is selected as the moving direction;

[0016] S9, a three-dimensional dynamic potential field is constructed, a step base value is calculated according to a potential field gradient and an adaptive step length formula, and a final step length is determined through dynamic adjustment of an energy-kinematics combined correction and a sensor confidence decay factor, and the AUV moves according to the step length and updates the state;

[0017] S10, it is judged whether the target point is reached, if not, the above steps are repeated, and if reached, the path is output.

[0018] Preferably, the reserved wide valley is selected according to the span condition, which comprises:

[0019] The horizontal direction span is:

[0020] In the formula, W AUV represents the maximum width of the AUV in the horizontal direction, S safe represents a safety margin, R represents the distance from the AUV to the sector under the current planning step length, and a represents the horizontal direction resolution;

[0021] The vertical direction span is:

[0022] In the formula, H AUV represents the maximum height of the AUV in the vertical direction, and β represents the vertical direction resolution;

[0023] The horizontal direction span S α and the vertical direction span S β are selected, and the spherical sector which is continuously more than the threshold S max is the reserved wide valley.

[0024] Preferably, the improved cost function is:

[0025] C=Δθ g +Δθ c +C energe +C risk -C water ;

[0026]

[0027] In the formula, the three-dimensional heading parameters are: the candidate direction θ candidate =(θ, φ), the target direction θ goal =(θ g , φ g ), and the current direction θcurrent = (θ c , φ c ), Δθ g represents dynamic target direction deviation, μ1 represents dynamic target weight, η1 represents adjustment coefficient of pitch angle deviation, σ represents nonlinear saturation threshold, tanh represents nonlinear saturation function, Δθc represents current heading deviation, μ2 represents steering weight, η2 represents cost of balanced pitch angle change, C energy represents energy consumption function, μ3 represents energy consumption weight, I z represents rotational inertia of AUV around yaw axis, I y represents rotational inertia of AUV around pitch axis, C risk represents three-dimensional risk perception obstacle proximity, μ4 represents safety weight, d represents Euclidean distance between current AUV center point and candidate direction obstacle, ε represents constant, p represents confidence of sensor to obstacle existence, C water represents counter-current energy consumption penalty, μ5 represents current weight, v water represents three-dimensional current field, v AUV represents AUV velocity direction vector.

[0028] Preferably, the three-dimensional dynamic potential field is constructed, a step base value is calculated according to a potential field gradient and an adaptive step length formula, and a final step length is determined by dynamic adjustment of an energy-kinematics combined correction and a sensor confidence decay factor, and the AUV moves according to the step length and updates the state, including:

[0029] A1, constructing a three-dimensional potential field: U total = U goal + U obstacle + U water ;

[0030] In the above formula, U goal represents target point attractive potential field, U obstacle represents obstacle repulsive potential field, U water represents current pushing potential field, wherein,

[0031]

[0032] In the above formula, K g represents target attractive force gain coefficient, θ goal = (θ g , φ g ) represents target direction in three-dimensional environment, θ candidate = (θ c , φ c ) represents candidate direction in three-dimensional environment, σ g represents target direction sensitivity decay coefficient;

[0033]

[0034] In the above formula, K o represents the obstacle rejection gain coefficient, d i Represents the Euclidean distance from the candidate direction to the i-th obstacle, Δθ i ,Δφ i Represents the azimuth deviation between the candidate direction and the obstacle, σ o Represents the obstacle influence range coefficient;

[0035]

[0036] In the above formula, K w represents the ocean current gain coefficient, v AUV(θ) ,v water Represent the yaw velocity direction vector and the ocean current velocity vector, respectively, ||v water || represents the flow rate weighting coefficient;

[0037] A2. Determine the base value of the step size:

[0038] In the above formula, L max represents the maximum allowed step size, Represents the gradient of the potential field in the yaw and pitch directions, Represents the maximum gradient strength in the current environment;

[0039] A3. Energy-kinematics joint correction:

[0040] Among them, the dynamic constraints of AUV are:

[0041]

[0042] In the above formula, Δθ max =|θ c -θ current | represents the deviation between the candidate direction and the current yaw angle, Δφ max =|φ c -φ current | represents the deviation between the candidate direction and the current pitch angle, Represent the maximum yaw and pitch angular velocity of the AUV, v max represents the maximum linear velocity of the AUV;

[0043] The energy consumption constraint is:

[0044]

[0045] In the above formula, C water represents the reverse energy penalty, C nominal represents the baseline value of energy consumption when there is no ocean current, and η represents the energy consumption sensitivity coefficient;

[0046] A4、according to the sensor confidence attenuation factor to dynamically adjust the final step size:

[0047] L final = L energy ×(p+(1-p)×e -λt );

[0048] In the above formula, λ represents the confidence attenuation rate, t represents the time of low confidence, and p represents the real-time confidence of the sonar, wherein p ∈ [0, 1].

[0049] Preferably, the obstacle intensity value M θφ The calculation formula is:

[0050] M θφ = c 2 θφ (a-bd 2 θφ );

[0051] In the above formula, a represents the sensitivity of controlling the obstacle density, b represents the sensitivity of controlling the obstacle distance, wherein a, b > 0, and C θφ represents the confidence of the sensor detecting the obstacle, and d θφ represents the Euclidean distance from the cell to the center point of the AUV.

[0052] Preferably, an AUV underwater local path planning system based on the improved VFH algorithm is also provided, comprising:

[0053] A map conversion module is configured to convert the environment where the AUV is located into a three-dimensional grid map with the center position of the AUV as the coordinate origin;

[0054] A data acquisition and processing module is configured to obtain distance data of the environment around the AUV, adopt wavelet threshold denoising processing, and set the heading angle, yaw angle θ and pitch angle φ of the AUV advancing direction;

[0055] A sector division module is configured to divide the horizontal and vertical spherical sectors according to the resolution α, divide the horizontal and vertical directions into 180° / α sectors, number the horizontal direction counterclockwise as β, and number the vertical direction from bottom to top as ω, and divide the space into β×ω spherical sectors;

[0056] An intensity calculation module is configured to calculate the obstacle intensity value M θφ of each grid in each spherical sector and the total obstacle intensity value H βω of each spherical sector, and determine the sector to which each grid belongs;

[0057] A matrix construction module is configured to convert the three-dimensional grid map into a Yaw-Pitch sector intensity matrix map;

[0058] A candidate region determination module is configured to set a threshold TH, and when H βω is less than the threshold TH, the spherical sector is determined as a candidate region, and the polar coordinate two-dimensional grid map data is binarized;

[0059] A sector merging module is configured to merge adjacent spherical sectors satisfying H βω less than the threshold TH to form a wide valley, and select a retained wide valley according to a span condition;

[0060] A direction selection module is configured to calculate a cost according to an improved cost function, and select a geometric center line direction of the retained wide valley with the minimum cost as a moving direction;

[0061] A step length calculation and moving module is configured to construct a three-dimensional dynamic potential field, calculate a step length base value according to a potential field gradient and an adaptive step length formula, dynamically adjust and determine a final step length through energy-kinematics joint correction and a sensor confidence decay factor, and move the AUV according to the step length and update a state;

[0062] A judgment module is configured to judge whether a target point is reached, and if not, repeat the above steps, and if yes, output a path.

[0063] Compared with the prior art, the beneficial effects of the present application are:

[0064] I. By constructing a three-dimensional grid map and dividing a spherical sector, the motion characteristics of the AUV in the three-dimensional space are fully considered, the obstacle distribution in the underwater three-dimensional environment can be effectively processed, and the path planning ability of the AUV in the complex underwater environment is improved.

[0065] II. The obstacle intensity calculation, passable sector screening and wide valley merging mechanism are introduced, the target direction deviation, current heading deviation, energy consumption, obstacle risk and current influence are comprehensively considered by combining the improved cost function, a safe and reasonable moving direction can be selected to avoid collision risk, the Yaw and Pitch ranges are reasonably set, the data calculation amount is reduced, and the efficiency of path planning is improved.

[0066] Three, adaptive step algorithm combined with three-dimensional dynamic potential field and energy optimization coupling, considering the dynamic constraints of AUV, energy consumption constraints and ocean current factors, can dynamically adjust the step according to the environment, the design of adaptive step and considering the environmental factors such as ocean current, let AUV can reasonably plan the path and adjust the motion state in different underwater environment, improve its working ability in complex environment, in addition, while avoiding collision, optimize energy consumption, improve the endurance of AUV, and the sonar sensor confidence is put into the model, through the confidence attenuation mechanism dynamic adjustment step, enhance the adaptability of AUV in different environmental conditions, improve the reliability of path planning, make its system can maintain stable operation when the sensor performance fluctuation or environmental change, at the same time, through the reasonable algorithm design, ensure the real-time continuation feedback of path planning, realize the accurate planning process of path. BRIEF DESCRIPTION OF DRAWINGS

[0067] In order to facilitate the description, the present application is described in detail by the following specific embodiments and drawings.

[0068] Figure 1 It is an improved VFH algorithm based on AUV underwater local path planning method flow chart;

[0069] Figure 2 It is a three-dimensional grid environment schematic diagram;

[0070] Figure 3 It is a spherical sector division schematic diagram;

[0071] Figure 4 It is a sector index schematic diagram;

[0072] Figure 5 It is an example of Yaw-Pitch sector intensity matrix diagram;

[0073] Figure 6 It is an adaptive step implementation flow chart;

[0074] Figure 7 It is an improved VFH algorithm based on AUV underwater local path planning system schematic diagram. DETAILED DESCRIPTION

[0075] In order to make the purpose, technical scheme and advantages of the present application more clear and obvious, the following describes the present application by the specific embodiments shown in the drawings. However, it should be understood that these descriptions are only exemplary, and are not intended to limit the scope of the present application. In addition, in the following description, the description of the known structure and technology is omitted to avoid unnecessary confusion of the concept of the present application.

[0076] It should be noted that, in order not to obscure the present application with unnecessary details, only the structures and / or processing steps closely related to the solution according to the present application are shown in the drawings, while other details not closely related to the present application are omitted.

[0077] Specific embodiment one: this embodiment only provides a preferred embodiment, based on the improved vector field histogram (VFH) algorithm for autonomous underwater vehicle (AUV) underwater local path planning method is to convert the environment where the AUV is located into a three-dimensional grid map and obtain the distance data of the environment around the AUV, the data is denoised and the moving direction of the AUV is set, the space is divided into β×ω spherical sectors according to the resolution α, the obstacle intensity value is calculated and the sector to which each grid belongs is determined, the sector intensity matrix graph is constructed for the three-dimensional grid map, the threshold is set and the passable sector is screened, the adjacent passable sectors are merged and the reserved valley is selected, the cost is calculated by using the improved cost function, the geometric center line direction of the reserved valley with the minimum cost is selected as the moving direction, the three-dimensional dynamic potential field is constructed, the step base value is calculated according to the potential field gradient and the adaptive step formula, and the final step is determined after correction and dynamic adjustment, the AUV moves according to the final step and updates the state, and the process is repeated until the target is reached.

[0078] Specifically, the method provided by the present application, the AUV perceives the surrounding environment through the front-looking sonar and other devices carried by the AUV, and carries out local path planning in the underwater three-dimensional space by using the improved VFH algorithm, realizes autonomous obstacle avoidance and navigation, and is suitable for various scenes requiring AUV autonomous navigation in underwater three-dimensional environment, such as ocean exploration, underwater operation, environmental monitoring, seabed resource development, etc. The front-looking sonar carried by the AUV is used to scan the area where the AUV travels, three-dimensional imaging is realized through multi-beam superposition, the obstacle intensity value of each active cell is calculated according to the real-time obtained obstacle information, the advancing spherical sector is divided and numbered according to the pre-set horizontal and vertical resolution, the intensity value sum of each spherical sector is calculated, the horizontal and vertical numbers corresponding to each spherical sector are used as indexes to establish a Yaw-Pitch sector intensity matrix graph, and the values in the matrix graph correspond one by one to the intensity value sum of each spherical sector. According to the actual engineering requirements and sensor accuracy, a threshold value is formulated, the spherical sectors below the threshold value are passable, and the spherical sectors above the threshold value are impassable. After screening the passable spherical sectors, adjacent passable spherical sectors are merged, the number of adjacent sectors greater than the threshold value is called a valley, the cost of each passable valley is calculated by using the cost formula of the selected direction, the minimum cost is selected as the moving direction, a step is moved, the step uses the adaptive step of three-dimensional dynamic potential field and energy optimization coupling, and the process is repeated until the target point is reached.

[0079] Specific implementation method two: this embodiment only provides a preferred implementation, the AUV underwater local path planning method based on improved VFH algorithm includes the following steps:

[0080] S1, as shown in the figure, taking the center position of the AUV as the coordinate origin, the environment where the AUV is located is converted into a three-dimensional grid map; Figure 2

[0081] S2, the distance data of the environment around the AUV is obtained by using the forward-looking sonar, wavelet threshold denoising processing is adopted, and the heading angle of the AUV advancing direction is set to 0°, the horizontal direction yaw angle θ and the vertical direction pitch angle φ range are-90°-90°, preferably, in the horizontal direction, the angle increases counterclockwise, the AUV pitch angle range is-90°-0°, and the pitch angle range is 0°-90°.

[0082] S3, according to the resolution α, the horizontal and vertical direction spherical sectors are divided, as shown in the figure, since the AUV advances in three-dimensional space, the horizontal and vertical directions are divided into 180° / α sectors, and the horizontal direction is numbered counterclockwise as β, and the vertical direction is numbered from bottom to top as ω, so that the space is divided into β×ω spherical sectors; Figure 3

[0083] S4, the obstacle intensity value M θ of each grid (r, θ, φ) in each spherical sector is calculated, and the total obstacle intensity value H βω of each spherical sector is determined, and the sector to which each grid belongs is determined, specifically,

[0084]

[0085] Wherein, β is the index in the yaw angle direction, ω is the index in the pitch angle direction, and the sector index schematic diagram is shown in the figure; Figure 4

[0086] S5, the three-dimensional grid map is converted into a Yaw-Pitch sector intensity matrix diagram, wherein the H βω of each spherical sector corresponds to the (β, ω) position in the two-dimensional grid coordinate system;

[0087] As can be seen from the Yaw-Pitch diagram, the yaw angle θ range is-90°-90°, and α is the resolution. Assuming that α is 30°, then after formula mapping, β can be mapped from left to right to the two-dimensional plane x axis index such as-3, -2..., 3, and the pitch angle φ is the same, which is mapped to the two-dimensional plane y axis index ω, so that the sector composed of the two three-dimensional space angle intervals θ and φ is mapped to the two-dimensional plane;

[0088] S6, set threshold TH, when the intensity value total H​​​βω When it is less than the threshold TH, the spherical sector is defined as a candidate area, and the polar coordinate two-dimensional grid map data is binarized;

[0089] S7. Merge adjacent ones that satisfy H βω The spherical sectors less than the threshold TH form wide valleys, and the wide valleys to be retained are selected according to the span condition;

[0090] S8. Calculate the cost according to the improved cost function, and select the direction of the geometric center line of the retained wide valley with the minimum cost as the moving direction;

[0091] S9. Construct a three-dimensional dynamic potential field, calculate the step base value according to the potential field gradient and the adaptive step formula, and determine the final step through the joint correction of energy-kinematics and the dynamic adjustment of the sensor confidence attenuation factor. The AUV moves according to this step and updates its state;

[0092] S10. Determine whether the target point has been reached. If not, repeat the above steps. If so, output the path.

[0093] Specifically, this embodiment only provides a specific implementation manner. As Figure 5 shown in (a), assume that the resolutions in the horizontal and vertical directions are both 30°. In the figure, the abscissa is the yaw angle, that is, Yaw, and the ordinate is the pitch angle, that is, Pitch. Each cell corresponds to a conical sector, and the value in the cell is the sum of the grid costs in the spherical sector. For example, the cell 7427 in the lower left corner of the first quadrant in the figure represents the sum of the obstacle intensities of all cells that satisfy the yaw angle between 0 and 30° and the pitch angle between 0 and 30°. Specifically, as Figure 5 shown in (b), assume that the threshold TH = 1800, and binarize each existing spherical sector. 0 represents passable, and 1 represents impassable. According to the above data, adjacent ones that satisfy H βω < TH can be merged. It should be noted that here, for the convenience of drawing and demonstration, the resolution value is set relatively large. In actual engineering, to ensure that the AUV can safely and accurately pass through the obstacle area, the resolution value should be much smaller than 30°. Preferably, since the underwater environment is different from the land, there are basically no complex and changeable terrains and obstacles that affect the movement of the AUV, and there are few complex terrains in the underwater environment, and the probability that the AUV falls into local extrema is relatively low. Therefore, in engineering, the Yaw and Pitch ranges can be further reduced according to the actual situation, which can greatly reduce the amount of data calculation.

[0094] Specific Embodiment 3: This embodiment only provides a preferred implementation manner. Selecting the wide valleys to be retained according to the span condition includes: The spherical sectors covered by the merged wide valleys in the horizontal direction (yaw angle) need to satisfy:

[0095] The horizontal direction span is:

[0096] Specifically, the safety space of the wide valley is divided into two directions in the present application, and the step R in the formula is an adaptive step length, which can realize adaptive adjustment of the wide valley size to avoid the AUV ignoring a better route. In the above formula, W AUV represents the maximum width of the AUV in the horizontal direction, Ssafe represents the safety margin, which is usually 1-2 times the size of the robot, when the current intensity is high, the safety margin should be appropriately increased, R represents the distance from the AUV to the sector under the current planning step length, and a represents the horizontal direction resolution;

[0097] The vertical direction span is:

[0098] In the above formula, H AUV represents the maximum height of the AUV in the vertical direction, and β represents the vertical direction resolution;

[0099] The horizontal direction span S α and the vertical direction span S β are selected, and the spherical sector that continuously exceeds the threshold S max is a reserved wide valley. If any direction of the wide valley S α or S β does not meet the threshold, it is considered that the channel is narrow and the robot cannot pass safely. The threshold Smax is set, and the wide valley that exceeds the Smax adjacent spherical sector is reserved to ensure that the AUV can pass safely.

[0100] Specific implementation four: this embodiment only provides a preferred implementation, and the direction of the next movement is selected from the reserved wide valley. In the selection process, the kinematic constraints of the AUV and the influence of the underwater current factor should be considered. The improved selection direction cost function is improved. The cost function is:

[0101] C=μ1*the angle difference between the candidate direction and the target direction+μ2*the angle difference between the candidate direction and the current movement direction+μ3*the obstacle distance in the candidate direction+μ4*the included angle between the candidate direction and the current direction. The present application calculates the overall cost function in combination with five aspects:

[0102]

[0103]

[0104] Specifically, the three-dimensional heading parameters are: the candidate direction θ candidate =(θ, φ), the target direction θ goal =(θ g , φ g ), and the current direction θ current =(θc , φ c );

[0105] Δθ g represents the dynamic target direction deviation, specifically introduces the comprehensive target deviation of the yaw angle and the pitch angle, introduces the tanh function applied to the saturation control of the target tracking direction deviation, μ1 represents the dynamic target weight, η1 represents the adjustment coefficient of the pitch angle deviation, if the pitch angle change cost is higher, η1 is greater than 1, and it can also be applied to the control depth direction, the AUV should avoid frequent up and down floating in the process of advancing σ represents the non-linear saturation threshold, which is used to control the deviation sensitivity, tanh represents a non-linear saturation function, which is applied to the saturation control of the target tracking direction deviation;

[0106] Δθc represents the current heading deviation, specifically introduces the comprehensive target deviation of the yaw angle and the pitch angle, μ2 represents the turning weight, η2 represents the cost of balancing the pitch angle change, considering the dynamic constraint problem of the AUV, the pitch maneuverability of the AUV is much lower than the yaw and usually more energy-consuming than the yaw, so a higher weight is set for the change of the pitch angle in the cost function, that is, η2 is greater than 1;

[0107] C energy represents the energy consumption function, specifically introduces the turning angle of the yaw angle and the pitch angle, introduces Iz, Iy, the moment of inertia of the yaw axis and the pitch axis, μ3 represents the energy consumption weight, I z represents the moment of inertia of the AUV around the yaw axis, that is, the Z axis, I y represents the moment of inertia of the AUV around the pitch axis, that is, the Y axis;

[0108] C risk represents the three-dimensional risk perception obstacle proximity, specifically introduces the combination of the confidence of the sonar sensor and the obstacle distance, μ4 represents the safety weight, d represents the Euclidean distance between the current AUV center point and the candidate direction obstacle, ε represents a constant to avoid zero, p ∈ [0, 1], p represents the confidence of the sensor to the existence of the obstacle, that is, the sonar echo intensity, the value is related to the underwater environment and the hardware of the sensor, when the confidence is low, even if the distance is far, a high cost is applied to avoid the risk of missing detection;

[0109] C water represents the energy consumption penalty of the counter current, μ5 represents the current weight, v water represents the three-dimensional current field, v water = (v x , v y , v z ), v AUV represents the AUV velocity direction vector;

[0110] Wherein, the dynamic target weight μ1 is the main navigation direction, usually set to a higher value, the steering weight μ2 is adjusted according to the robot mobility, μ2 should be larger when the underwater robot inertia is large, the energy consumption weight μ3 is formulated according to the actual application scene of the underwater robot and the specific structure and performance of the robot, the safety weight μ4 is increased when the sensor accuracy is low or the environment is complex, which is used to ensure the priority of obstacle avoidance, the current weight μ5 needs to be determined according to the actual marine environment, μ5 should be appropriately increased when the AUV speed direction is opposite to the current direction, so as to reduce the energy consumption of AUV moving against the current, μ5 should be reduced when the angle between the AUV speed direction and the current direction is small, which rewards the current sailing, based on this, for the final selected valley, the geometric center line direction is calculated as the motion direction of the AUV

[0111] Specific implementation five: this embodiment only provides a preferred implementation, a three-dimensional dynamic potential field is constructed, the step length base value is calculated according to the potential field gradient and the adaptive step length formula, and the final step length is determined by dynamic adjustment of energy-kinematics joint correction and sensor confidence decay factor, the AUV moves according to the step length and updates the state, specifically, the three-dimensional potential field is first constructed according to the environment in the sea, the potential field function is composed of target point attraction force, obstacle repulsion force and current driving force, the three-dimensional potential field function is constructed according to the influence of the marine environment on the AUV, specifically:

[0112] A1, construct a three-dimensional potential field: U total = U goal + U obstacle + U water ;

[0113] In the above formula, U goal represents the target point attraction potential field, U obstacle represents the obstacle repulsion potential field, and U water represents the current driving potential field, wherein,

[0114]

[0115] Specifically, the yaw angle and the pitch angle are introduced in the present application, K g represents the target attraction gain coefficient, K g is greater than 0, θ goal =(θ g ,φ g ) represents the target direction in the three-dimensional environment, that is, the yaw angle and the pitch angle, θ candidate =(θ c ,φ c ) represents the candidate direction in the three-dimensional environment, that is, the yaw angle and the pitch angle, and σ g represents the target direction sensitivity decay coefficient.

[0116]

[0117] Specifically, the formula covers three-dimensional information, and utilizes three-dimensional Gaussian attenuation to simulate obstacle repulsion, wherein the three-dimensional Gaussian attenuation is simulated by The three-dimensional repulsion effect of the obstacle is simulated by integrating the distance and angle deviation, K o represents an obstacle repulsion gain coefficient, K o is greater than 0, d i represents the Euclidean distance from the candidate direction to the i-th obstacle, Δθ i , Δφ i respectively represent the candidate direction and the azimuth angle deviation of the obstacle, σ o represents an obstacle influence range coefficient;

[0118]

[0119] Specifically, the present application introduces the thrust / resistance effect of the ocean current into the potential field, K w represents an ocean current gain coefficient, and is greater than 0, v AUV(θ) , v water respectively represent the yaw speed direction vector and the ocean current speed vector, ||v water || represents a flow rate weighting coefficient;

[0120] A2, determine a step base value according to the potential field gradient:

[0121] Specifically, the present application uses the potential field strength in three-dimensional space to adjust the step base value, L max represents the maximum allowed step length, which is mainly determined by the maximum speed of the AUV, the planning period, the seabed environment, the sensor accuracy and the like, represents the gradient of the potential field in the yaw angle and the pitch angle direction, represents the maximum gradient strength in the current environment, which is used for normalization processing, and the maximum value of the gradient strength should be dynamically updated in each planning period, and the maximum value of the gradient strength is calculated by traversing all candidate directions to ensure adaptation to environmental changes;

[0122] A3, energy-kinematics joint correction:

[0123] wherein the dynamics constraint of the AUV is:

[0124]

[0125] Specifically, the present application considers the maneuvering ability of the AUV in the pitch and yaw directions when determining the step length, Δθ max = |θ c - θ current | represents the deviation of the candidate direction from the current yaw angle, Δφ max = |φc -φ current | represents the deviation of the candidate direction from the current pitch angle, respectively represent the maximum yaw and pitch velocity of the AUV, which are determined by the propeller performance and hydrodynamic characteristics of the AUV, v max represents the maximum linear velocity of the AUV;

[0126] The energy consumption constraint is:

[0127]

[0128] Specifically, the application dynamically adjusts the step size in combination with the direction of the water flow, C water represents the energy consumption penalty of the reverse flow, C nominal represents the baseline value of energy consumption when there is no ocean current, and η represents the energy consumption sensitivity coefficient, when η is greater than 0, the reverse flow step size is shortened, and specifically, the remaining power in the AUV operation can be dynamically adjusted;

[0129] A4, dynamically adjust the final step size according to the sensor confidence decay factor, specifically, dynamically adjust the final step size according to the real-time confidence of the sonar p∈[0,1] :

[0130] L final =L energy ×(p+(1-p)×e -λt );

[0131] The application integrates kinematics, energy, and sensor confidence into a unified model, λ represents the confidence decay rate, t represents the time of continuous low confidence, which is used to trigger the exponential decay mechanism, and p represents the real-time confidence of the sonar, wherein p∈[0,1], p→1 is a high confidence state, at this time, the step size of the AUV is completely determined, and p→0 is a low confidence state, at this time, the step size is exponentially decayed to avoid collision risk, and the value of p is determined by factors such as sonar signal intensity, actual ocean current environment, and multipath interference.

[0132] Specific implementation method six: this embodiment only provides a preferred implementation, the obstacle intensity value M θφ

[0133] The calculation formula is:

[0134] M θφ =c 2 θφ (a-bd 2 θφ );

[0135] In the formula, a represents the sensitivity of controlling the obstacle density, specifically, the larger the value of a, the stronger the punishment for the high-density obstacle region, and the path is more inclined to the open area, the typical range of a is 1.0-2.0, and in the actual application process, it needs to be adjusted according to the sensor noise, and the embodiment is not specifically limited, b represents the sensitivity of controlling the obstacle distance, wherein b is related to the safety distance of the robot or the range of the sensor, and the formula can be referred to as C θφ represents the confidence of the sensor detecting the obstacle, d θφ represents the Euclidean distance from the cell to the center point of the AUV, that is, the corresponding r.

[0136] Specific implementation seven: the application also provides an AUV underwater local path planning system based on an improved VFH algorithm, comprising:

[0137] A map conversion module is configured to convert the environment in which the AUV is located into a three-dimensional grid map with the center position of the AUV as the coordinate origin;

[0138] A data acquisition and processing module is configured to obtain distance data of the environment around the AUV, adopt wavelet threshold denoising processing, and set a heading angle, a yaw angle θ and a pitch angle φ of the advancing direction of the AUV;

[0139] A sector division module is configured to divide the horizontal and vertical spherical sectors according to a resolution α, divide the horizontal and vertical directions into 180° / α sectors, number the horizontal direction in a counterclockwise direction as β, and number the vertical direction in a direction from bottom to top as ω, and divide the space into β×ω spherical sectors;

[0140] An intensity calculation module is configured to calculate the obstacle intensity value M θφ of each grid in each spherical sector and the total obstacle intensity value H βω of each spherical sector, and determine the sector to which each grid belongs;

[0141] A matrix construction module is configured to convert the three-dimensional grid map into a Yaw-Pitch sector intensity matrix map;

[0142] A candidate region determination module is configured to set a threshold TH, and when H βω is less than the threshold TH, the spherical sector is determined as a candidate region, and the polar coordinate two-dimensional grid map data is binarized;

[0143] A sector merging module is configured to merge adjacent spherical sectors that satisfy H βω is less than the threshold TH to form a wide valley, and select a reserved wide valley according to a span condition;

[0144] The direction selection module is used for calculating the cost according to the improved cost function, and selecting the direction of the geometric center line of the reserved valley with the minimum cost as the moving direction;

[0145] The step calculation and moving module is used for constructing a three-dimensional dynamic potential field, calculating a step base value according to a potential field gradient and an adaptive step formula, dynamically adjusting and determining a final step through an energy-kinematics joint correction and a sensor confidence decay factor, and moving the AUV according to the step and updating the state.

[0146] The judgment module is used for judging whether the target point is reached, and if not, repeating the above steps, and if so, outputting the path.

[0147] The basic principle and main features of the present application and the advantages of the present application are shown and described above. It should be understood by those skilled in the art that the present application is not limited by the above examples, and the above examples and descriptions in the specification are only to illustrate the principles of the present application. Without departing from the spirit and scope of the present application, various changes and improvements can be made to the present application, and these changes and improvements all fall within the scope of the present application. The scope of protection of the present application is defined by the appended claims and their equivalents.

Claims

1. An AUV underwater local path planning method based on an improved VFH algorithm, characterized in that, The AUV underwater local path planning method based on the improved VFH algorithm converts the environment where the AUV is located into a three-dimensional grid map and obtains distance data of the environment around the AUV, denoises the data, sets a moving direction of the AUV, divides the space into beta*omega spherical sectors according to a resolution alpha, calculates obstacle intensity values and determines sectors to which each grid belongs, constructs a sector intensity matrix graph for the three-dimensional grid map, sets a threshold and screens passable sectors, merges adjacent passable sectors and selects a reserved valley, calculates a cost by using an improved cost function, selects a geometric center line direction of the reserved valley with the minimum cost as a moving direction, constructs a three-dimensional dynamic potential field, calculates a step base value according to a potential field gradient and an adaptive step length formula, and determines a final step length after correction and dynamic adjustment, the AUV moves according to the final step length and updates a state, and the process is repeated until the target is reached.

2. The improved VFH algorithm-based AUV underwater local path planning method according to claim 1, characterized in that, The AUV underwater local path planning method based on the improved VFH algorithm comprises the following steps: S1, converting the environment where the AUV is located into a three-dimensional grid map with a center position of the AUV as a coordinate origin; S2, obtaining distance data of the environment around the AUV, denoising the data by using a wavelet threshold, and setting a heading angle of the moving direction of the AUV as 0°, a yaw angle theta and a pitch angle phi each in a range of -90° to 90°; S3, dividing spherical sectors in horizontal and vertical directions according to a resolution alpha, dividing the horizontal and vertical directions into 180° / alpha sectors, numbering the horizontal direction in a counterclockwise direction as beta, numbering the vertical direction from bottom to top as omega, and dividing the space into beta*omega spherical sectors; S4, calculating the obstacle intensity value M of each grid in each spherical sector θφ and the total obstacle intensity value H of each spherical sector βω determining the sector to which each grid belongs; S5, converting the three-dimensional grid map into a Yaw-Pitch sector intensity matrix map, wherein H βω of each spherical sector corresponds to a (β, ω) position in the two-dimensional grid coordinate system; S6, set a threshold TH, when the intensity value sum H βω When the intensity value sum H is less than the threshold TH, the spherical sector is defined as a candidate region, and the polar coordinate two-dimensional grid chart data is binarized. S7, merge adjacent Hs βω Spherical sectors smaller than the threshold TH form wide valleys, and the wide valleys are selected to be preserved according to the span condition. S8, calculating a cost according to an improved cost function, and selecting a geometric center line direction of a reserved valley with the minimum cost as a moving direction; S9, constructing a three-dimensional dynamic potential field, calculating a step base value according to a potential field gradient and an adaptive step length formula, and determining a final step length through energy-kinematics joint correction and dynamic adjustment of a sensor confidence decay factor, the AUV moving according to the step length and updating a state; S10, judging whether a target point is reached, repeating the above steps if not, and outputting a path if the target point is reached.

3. The improved VFH algorithm-based AUV underwater local path planning method according to claim 2, characterized in that, The selecting a reserved valley according to a span condition comprises: horizontal direction is: In the above formula, W AUV represents the maximum width of the AUV in the horizontal direction, S safe represents the safety margin, R represents the distance from the AUV to the sector at the current planning step, and a represents the horizontal resolution; The vertical direction span is: In the above formula, H AUV represents the maximum height of the AUV in the vertical direction, and β represents the vertical resolution. Selecting horizontal direction span S α and vertical direction span S β , both of which continuously exceed threshold S max as a reserved valley.

4. The improved VFH algorithm-based AUV underwater local path planning method according to claim 3, characterized in that, The improved cost function is: C = Δθ g + Δθ c + C energe + C risk - C water ; In the above equation, the three-dimensional heading parameters are: candidate direction θ candidate =(θ, φ), target direction θ goal =(θ g , φ g ), current direction θ current =(θ c , φ c ), Δθ g represents the dynamic target direction deviation, μ1 represents the dynamic target weight, η1 represents the adjustment coefficient of the pitch angle deviation, σ represents the nonlinear saturation threshold, tanh represents the nonlinear saturation function, Δθc represents the current heading deviation, μ2 represents the steering weight, η2 represents the cost of balancing the pitch angle change, C energy represents the energy consumption function, μ3 represents the energy consumption weight, I z represents the moment of inertia of the AUV around the yaw axis, I y represents the moment of inertia of the AUV around the pitch axis, C risk represents the three-dimensional risk perception obstacle proximity, μ4 represents the safety weight, d represents the Euclidean distance between the current AUV center point and the candidate direction obstacle, ε represents a constant, p represents the confidence of the sensor to the existence of the obstacle, C water represents the energy consumption penalty of the counter current, μ5 represents the current weight, v water represents the three-dimensional current field, v AUV represents the AUV velocity direction vector.

5. The improved VFH algorithm-based AUV underwater local path planning method according to claim 4, characterized in that, The constructing a three-dimensional dynamic potential field, calculating a step base value according to a potential field gradient and an adaptive step length formula, and determining a final step length through energy-kinematics joint correction and dynamic adjustment of a sensor confidence decay factor, the AUV moving according to the step length and updating a state comprises: A1, construct a three-dimensional potential field: U total = U goal + U obstacle + U water ; In the above equation, U goal represents the target point attractive potential field, U obstacle represents the obstacle repulsive potential field, U water represents the ocean current pushing potential field, where, In the above equation, K g represents the target attractiveness gain coefficient, θ goal = (θ g , φ g ) represents the target direction in a three-dimensional environment, θ candidate = (θ c , φ c ) represents the candidate direction in a three-dimensional environment, and σ g represents the target direction sensitivity decay coefficient. In the above formula, K o represents the obstacle repulsion gain coefficient, d i represents the Euclidean distance from the candidate direction to the ith obstacle, Δθ i , Δφ i respectively represent the candidate direction and the azimuth angle deviation from the obstacle, σ o represents the obstacle influence range coefficient; U water = k w × (1 + cos(v AUV(θ) , v water )) × ||v water ||; In the above formula, K w represents the ocean current gain coefficient, v AUV(θ) ,v water Represent the yaw velocity direction vector and the ocean current velocity vector, respectively, ||v water || represents the flow rate weighting coefficient; A2, determine step base value: In the above formula, L max represents the maximum allowed step size, represents the gradient of the potential field in the yaw, pitch direction, represents the maximum gradient strength in the current environment; A3, energy-kinematics joint correction: The kinetic constraint of the AUV is: In the above equation, Δθ max = |θ c - θ current | represents the deviation of the candidate direction from the current yaw angle, Δφ max = |φ c - φ current | represents the deviation of the candidate direction from the current pitch angle, respectively represent the maximum yaw and pitch velocities of the AUV, v max represents the maximum linear velocity of the AUV; The energy consumption constraint is: In the above formula, C water represents the energy consumption penalty of the reverse flow, C nominal represents the energy consumption of the reference value without ocean current, and η represents the energy consumption sensitivity coefficient. A4, dynamically adjusting the final step length according to a sensor confidence decay factor: L final = L energy × (p + (1 - p) x e -λt ); In the above formula, lambda represents a confidence decay rate, t represents a time of continuous low confidence, and p represents a real-time confidence of a sonar, wherein p [0, 1].

6. The improved VFH algorithm based AUV underwater local path planning method according to claim 5, characterized in that, The obstacle strength value M θ The calculation formula of φ is: M θ φ = c 2 θ φ(a-bd 2 θ φ); In the above formula, a represents the sensitivity of controlling the obstacle density, b represents the sensitivity of controlling the obstacle distance, wherein, C θ φ represents the confidence of the sensor detecting the obstacle, d θ φ represents the Euclidean distance from the cell to the center point of the AUV.

7. An AUV underwater local path planning system based on improved VFH algorithm, characterized in that, It comprises: a map conversion module configured to convert the environment where the AUV is located into a three-dimensional grid map with a center position of the AUV as a coordinate origin; The data acquisition and processing module is used for acquiring distance data of the environment around the AUV, adopting wavelet threshold denoising processing, and setting a heading angle of a travel direction of the AUV, a yaw angle θ and a pitch angle φ; The sector division module is used for dividing spherical sectors in horizontal and vertical directions according to a resolution α, dividing the horizontal and vertical directions into 180° / α sectors, numbering the horizontal direction in a counterclockwise direction as β, numbering the vertical direction in a direction from bottom to top as ω, and dividing the space into β×ω spherical sectors; an intensity calculation module for calculating the obstacle intensity value M of each grid in each spherical sector θφ and the total obstacle intensity value H of each spherical sector βω determining the sector to which each grid belongs The matrix construction module is used for converting the three-dimensional grid map into a Yaw-Pitch sector intensity matrix map; The candidate region determining module is configured to set a threshold TH, and when H βω When H is less than the threshold TH, the spherical sector is determined as a candidate region, and the polar coordinate two-dimensional grid chart data is binarized. a sector merging module for merging adjacent sectors satisfying H βω Sectors smaller than the threshold TH form wide valleys and the retention of wide valleys is selected according to the span condition. The direction selection module is used for calculating a cost according to an improved cost function, and selecting a direction of a geometric center line of a reserved valley with a minimum cost as a moving direction; The step length calculation and moving module is used for constructing a three-dimensional dynamic potential field, calculating a step length base value according to a potential field gradient and an adaptive step length formula, dynamically adjusting and determining a final step length through energy-kinematics joint correction and a sensor confidence decay factor, and moving the AUV according to the step length and updating a state; The judgment module is used for judging whether a target point is reached, repeating the above steps if the target point is not reached, and outputting a path if the target point is reached.

Images