An Underwater Unmanned Vehicle Path Re-planning Method in a Complex Environment

Abstract

The present invention proposes a path replanning method for an unmanned underwater vehicle in a complex environment. This is a path replanning method that combines spatial paths with a speed strategy, including: detecting thruster failures and then deciding whether to continue the mission or return to base to reduce losses, and implementing this through thrust allocation; replanning the path in a sudden water flow environment using the APF algorithm to cope with water flow obstacles and return to the original path at a safe location; and then combining the DWA algorithm to further consider suddenly emerging moving obstacles, and proposing a method that can avoid obstacles on the premise of not destroying the original planned path and replanning the path in the water flow environment. The method proposed by the present invention can address potential safety hazards caused by emergencies in complex underwater scenarios, complete path replanning under different events, and reasonably coordinate when multiple types of abnormal events occur simultaneously, enabling the path replanning methods under three types of uncertain events to be simultaneously implemented within the overall algorithm framework.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning for unmanned underwater vehicles, and particularly to a method for re-planning the path of an unmanned underwater vehicle in a complex environment. Background Art

[0002] Unmanned underwater vehicles can replace humans to perform tasks in some dangerous ocean scenarios, including seabed search, underwater rescue, seabed pipeline maintenance, etc. The basis of these tasks is that the underwater vehicle can autonomously plan its navigation path. With the increasing sophistication of sensing technology and the continuous development of intelligent algorithms, the path planning technology of autonomous unmanned underwater vehicles has been further improved, achieving two-dimensional path planning in a static surface obstacle environment, three-dimensional path planning for the seabed environment, etc. Although unmanned underwater vehicles can effectively and reliably achieve autonomous path planning in simple scenarios, there are still deficiencies in complex underwater scenarios, as follows: (1) Due to the large amount of water flow interference in the dynamic water environment, it causes significant obstacles and insecurity to underwater exploration and search. When the fluid encounters an obstacle, several vortices will form around the obstacle. Strong water flow is likely to cause the underwater vehicle performing the task to have an unstable attitude or even lose control; (2) There are various plankton in the ocean, which may move quickly or be relatively large in size, affecting the safety of the underwater vehicle. Traditional path planning methods often do not have the ability to adjust in real time according to the state of dynamic obstacles. Once approaching an obstacle, it may cause the unmanned underwater vehicle to be unable to avoid it in time or even collide; (3) Since the propulsion device of the unmanned underwater vehicle is open, it is easy to be involved in waterweeds or small plankton, resulting in abnormal thrust output. Once a propulsion failure occurs, it may be unable to track the planned path, causing the unmanned vehicle not only to be unable to complete the task, but even unable to return and be recovered, resulting in losses and ocean pollution. In response to the potential safety hazards caused by the above emergencies, it is necessary to design effective solutions to deal with various emergencies and control the unmanned underwater vehicle to avoid potential safety hazards. Summary of the Invention

[0003] In view of the potential safety hazards caused by emergencies in complex underwater scenarios and the problems existing in the above-mentioned prior art, the present invention provides a method for re-planning the path of an unmanned underwater vehicle in a complex environment, which can also be called a path re-planning method of Spatial Path Fusion Velocity Strategy (SPFVS). The purpose of the present invention is not only to complete path re-planning under different emergencies, but also to reasonably coordinate when multiple types of abnormal events occur simultaneously.

[0004] To achieve the above object, the present invention provides the following technical solution, a path replanning method for an unmanned underwater vehicle in a complex environment, specifically including the following steps:

[0005] S1. Initialize the original path; initialize the path in the static environment, and let the unmanned underwater vehicle execute tasks according to this path;

[0006] S2. Uncertain event detection; according to the feedback of the sensors and actuators of the unmanned underwater vehicle, detect whether an uncertain event has occurred. If it has occurred, further determine the type of the uncertain event and perform speed strategy resetting and path replanning; the uncertain events include: thruster failure, presence of water flow interference, presence of moving obstacles;

[0007] S3. Thruster failure detection; when a thruster failure is detected, redesign the traveling strategy and thruster thrust distribution, and then enter step S4; when no thruster failure is detected, directly enter step S4;

[0008] S4. Water flow detection; when the presence of water flow interference is detected, obtain water flow data and analyze it, and then replan a suitable path in the complex water flow range according to the artificial potential field method APF, and then enter step S5; when no water flow interference is detected, directly enter step S5;

[0009] S5. Moving obstacle detection; when a moving obstacle is detected, obtain its status information and input it into the fuzzy logic, and then use the output of the fuzzy logic as the parameter of the DWA dynamic window evaluation function to output a suitable speed strategy to make the unmanned underwater vehicle decelerate safely and try to keep traveling on the path replanned by APF; when no moving obstacle is detected, directly perform path tracking;

[0010] S6. After the strategy resetting and path replanning in steps S3 - S5, the unmanned underwater vehicle returns to the original path and executes tasks;

[0011] S7. After returning to the original path, re - execute steps S2 - S6 at a certain frequency until the underwater task is completed.

[0012] Further, in step S3:

[0013] The thrusters include thrusters for controlling the horizontal degree of freedom, i.e., horizontal thrusters, and thrusters for controlling diving and floating, i.e., vertical thrusters; according to the type and degree of failure of the thrusters, determine whether it is necessary to plan a path to return to the water surface and redesign the speed strategy and thrust distribution, specifically including:

[0014] 1) When there is a minor fault in the horizontal thruster, use the remaining normal thrusters to compensate for the faulty thruster, reallocate the thrust and continue working. Increase the thrust of the normal thrusters to compensate for the deficiency of the faulty thruster, and maintain the original path planning unchanged;

[0015] 2) When the horizontal thruster has a serious fault and cannot work, allocate the vertical thrust of the remaining normal thrusters, that is, float upward through the vertical thrusters, and plan an upward path to return to the water surface as soon as possible for salvage;

[0016] 3) When the vertical thruster fails, adjust the attitude of the unmanned underwater vehicle, tilt the underwater vehicle and then reallocate the thrust, and use the horizontal thruster to compensate to generate an upward resultant force to help float.

[0017] More specifically, in step S3, the thrust allocation based on the optimal solution is specifically realized through quadratic sequential programming, including:

[0018] First, set the inequality constraint of the thrust upper limit, then give the equality constraint of the direction, and give the optimization problem with the typical quadratic function as the objective function, which is expressed by the formula:

[0019]

[0020] Among them,

[0021]

[0022] In the above formula, f(T) is the standard quadratic programming to minimize the thruster thrust to reduce energy consumption; T is the thrust matrix of all thrusters; T i is the thrust of a single thruster, i represents the i-th thruster, and the number of thrusters is n; T m is the maximum thrust that a single thruster can provide; F is the resultant force vector generated by the thrusters, and W is the calculation equation related to the layout of the thrusters; the constraint condition s.t. is: F = W * T and T i ≤T m , that is, taking the resultant force direction of the entire thrust matrix as the target and the limitations of single thrusters in various situations as the constraints, and finally realizing replanning.

[0023] Furthermore, in step S4:

[0024] Obtaining the water flow data and analyzing it specifically is as follows:

[0025] Calculate the projection component U proj of the water flow vector on the path vector, and the formula is expressed as:

[0026]

[0027] Among them, U and D represent the water flow vector and the path vector respectively, and |D| is the modulus of the path vector; according to the calculated projection component, the intensity of the water flow is judged; if the power of the unmanned underwater vehicle is not enough to overcome the action of the water flow, re-planning is carried out according to the artificial potential field method APF, and a new local target point is selected as the target of the re-planned path.

[0028] More specifically, the judgment of the water flow intensity according to the calculated projection component in step S4 is specifically as follows:

[0029] A threshold is set according to the characteristics of the underwater robot; the projection of the water flow velocity vector on the robot motion vector is calculated. If it is positive, it means that the water flow is a boost in the motion direction; if it is negative, it means that the water flow is an obstacle in the motion direction; when the absolute value of the negative value is greater than the set threshold, it is regarded as unable to pass, that is, the power of the unmanned underwater vehicle is not enough to overcome the action of the water flow.

[0030] More specifically, in step S4, re-planning a suitable path in the complex water flow range according to the artificial potential field method APF specifically includes:

[0031] S41. Establish the re-planning target; select a local target point on the original path as the gravitational source to return to the original path, and the gravitational force F of the local target point OPA is expressed as:

[0032]

[0033] Among them, P and G are the current position of the unmanned underwater vehicle and the position of the local target point respectively; d is the distance between the re-planning starting point and the local target point; the space between the starting point and the local target point of the unmanned underwater vehicle is the re-planning space;

[0034] S42. Calculate the repulsive force F of the static obstacle rep ; that is, calculate the repulsive force of the conventional existing static obstacles when planning the path due to the original path not being able to be used normally, and the formula is expressed as:

[0035]

[0036] Among them, O i is the closest position of the surface of each detected static obstacle to the unmanned underwater vehicle; δ is a parameter to adjust the balance between the water flow threat and the obstacle threat; ε is used to prevent affecting the efficiency of the APF algorithm when the distance from the static obstacle is far;

[0037] S43. Calculate the force F exerted by the water flow on the unmanned underwater vehicle cur ; the formula is expressed as:

[0038] F cur= [U(P), V(P), W(P)] × β;

[0039] wherein, the magnitude of the force is determined by the ocean current function of the current position P of the unmanned underwater vehicle, and U, V, and W are the water flow vectors; F cur The final value of is related to the 3*1 matrix β. If the unmanned underwater vehicle has the same dynamic characteristics for water flows in all directions, it is considered that the water flow acts completely on the unmanned underwater vehicle, and β is [1, 1, 1] T ;

[0040] S44. Calculate the resultant force F received by the unmanned underwater vehicle total , and the formula is expressed as:

[0041] F total = -F OPA + F rep + F cur ;

[0042] The resultant force F total guides the unmanned underwater vehicle to plan a replanning path that satisfies static obstacle avoidance and avoids water flow interference in space.

[0043] In addition, in step S41, the selection of the local target point is specifically as follows:

[0044] If the global target point is outside the water flow area, a safe point on the original path outside the water flow area can be set as the local target point;

[0045] If the global target point is within the water flow area, this point is directly regarded as the local target point.

[0046] Furthermore, step S5 specifically includes:

[0047] S51. Establish the relationship between the moving state of the moving obstacle and the speed strategy of the unmanned underwater vehicle through fuzzy logic; rely on sonar and depth cameras to obtain the state information of the moving obstacle, including position and speed; fuzzify the position and speed as input variables to obtain the position fuzzy set and speed fuzzy set;

[0048] The position is the lateral position of the moving obstacle relative to the moving direction of the unmanned underwater vehicle, and the speed is the lateral speed of the obstacle on the moving path of the robot;

[0049] S52. Set reasonable fuzzy logic to realize speed change in response to the obstacle state;

[0050] S53. Set the deceleration parameter, and its value range is between 0 and 1; a value of 0 means no deceleration is required, and a value of 1 means decelerating to the maximum extent; use the centroid method to defuzzify the fuzzy output to obtain the fuzzy output dcf:

[0051]

[0052] Among them, μ(x) is the output membership function, and x is the fuzzy input including the state of the dynamic obstacle;

[0053] S54. Generate a DWA dynamic window according to the dynamic constraints; the dynamic constraints include linear velocity constraints and angular velocity constraints, that is, the upper and lower limits of the speed and the acceleration are restricted;

[0054] S55. Determine the selected speed strategy based on the DWA dynamic window evaluation function combined with the fuzzy output; a series of possible trajectories are generated by calculating the feasible speed and steering combinations within a given time window, and then the optimal trajectory is selected by evaluating the scores of each trajectory; at the same time, an additional evaluation criterion is introduced to better track the APF algorithm replanning path when selecting the speed strategy;

[0055] The evaluation criterion θ diff is expressed by the formula:

[0056]

[0057] Among them, d orig and d generate represent the APF algorithm replanning path direction vector and the DWA generated path direction vector respectively; the smaller θ diff is, the higher the path fitness is, that is, the better the APF algorithm replanning path can be tracked;

[0058] S56. Consider the safety evaluation of the moving obstacle; when the moving obstacle is relatively close, or its motion state is complex, that is, its motion trend cannot be predicted and judged; the close - range evaluation item S ob and the predicted deceleration evaluation item S goal are respectively set to achieve emergency obstacle avoidance;

[0059] S57. According to steps S55 and S56, the final DWA evaluation function S is obtained, and the formula is expressed as:

[0060] S = k1S goal - k2S ob - k3|θ diff |;

[0061] Among them, k1, k2, and k3 are the weights of each item;

[0062] Select the one with the highest DWA evaluation function value among all candidate paths as the final replanning path, and use its corresponding speed strategy as the final strategy.

[0063] More specifically, step S54 specifically includes:

[0064] S541. The linear velocity window (vel min , vel max ) of the unmanned underwater vehicle within the unit sampling time dt is set as:

[0065]

[0066] where vel C is the current linear velocity of the unmanned underwater vehicle, acc is the maximum acceleration scalar under power limitation, and maxspeed is the maximum linear velocity that the unmanned underwater vehicle can reach;

[0067] The linear velocity vel is obtained by synthesizing the surge velocity v surge and the heave velocity v heave of the unmanned underwater vehicle in terms of direction, and the formula is expressed as:

[0068]

[0069] S542. The angular velocity window of the unmanned underwater vehicle within the unit sampling time dt, that is, the sampling range of yaw (yaw min , yaw max ) is set as:

[0070]

[0071] where yaw C is the orientation angle relative to the replanned path of the APF algorithm, and accy is the maximum yaw angular acceleration;

[0072] S543. Velocity vector sampling; with precisions of τ1 and τ2, the sampling numbers N v and N y on the linear velocity and angular velocity are respectively:

[0073]

[0074] At each time step, N v ·N y paths are sampled, and these paths are input into the DWA evaluation function, and the velocity sequence (vel, yaw) with the highest DWA evaluation function value obtained is selected as the dynamic window.

[0075] More specifically, in step S56:

[0076] The formula expression of the short - range evaluation term S ob is:

[0077]

[0078] where P is the current position of the unmanned underwater vehicle, Odi is the nearest position of the surface of each moving obstacle to the unmanned underwater vehicle;

[0079] The predicted deceleration evaluation item S goal is expressed by the formula:

[0080] S goal =-|P - G0|(vel - vel max ·dcf) 2 ;

[0081] where G0 is the target position when moving forward at full speed in this time step.

[0082] Based on the above technical solutions, the method proposed by the present invention has the following beneficial effects:

[0083] 1. The present invention comprehensively considers the potential safety hazards that the unmanned underwater vehicle may encounter during operation. By integrating the speed strategy into the spatial path, the overall framework of path replanning is designed, which can reasonably coordinate when multiple types of abnormal events occur, provide a replanned path in a complex water flow environment, and ensure the real-time performance and flexibility of path planning.

[0084] 2. In the face of water flow interference, the method proposed by the present invention uses the artificial potential field method APF, takes the local safety target point as the source of gravity to return to the original path; through the superposition of different partial forces, the underwater vehicle is driven to generate a spatial path locally for replanning. The replanned path can ensure the static obstacle avoidance ability of the underwater vehicle while coping with the influence caused by water flow disturbance, thereby reducing the energy consumption of the underwater vehicle when performing tasks in the water flow environment.

[0085] 3. For moving obstacles that will cause collisions on the known path, the method proposed by the present invention takes the fuzzy logic output as the parameter of the DWA evaluation function, establishes the connection between the moving state and acceleration / deceleration of the obstacle through fuzzy logic, and then uses DWA for speed planning on the basis of minimizing the deviation from the path planned by the APF algorithm. To ensure that the planned path has good smoothness and environmental adaptability, and at the same time the unmanned underwater vehicle can have reasonable acceleration and speed strategies, and can effectively avoid moving obstacles, improving safety.

[0086] 4. In addition, when the unmanned underwater vehicle itself is damaged, the method proposed by the present invention can also effectively recover or adjust it to continue to perform tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 is the overall framework diagram for the present invention to detect complex environments and implement path replanning;

[0088] Figure 2It is the overall process block diagram for detailed path replanning in multiple complex environments provided by the present invention;

[0089] Figure 3 It is the block diagram of the thrust redistribution mechanism under thrust failure provided by the present invention;

[0090] Figure 4 It is the block diagram of spatial path replanning under water flow obstruction provided by the present invention;

[0091] Figure 5 It is the block diagram of speed strategy replanning in a dynamic obstacle environment provided by the present invention;

[0092] Figure 6 It is the schematic diagram of the thruster layout of the unmanned underwater vehicle involved in the present invention;

[0093] Figure 7 It is the fuzzy logic rule table involved in the embodiments of the present invention. Specific Embodiments

[0094] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0095] Although the steps in the present invention are arranged with reference numerals, they are not used to limit the order of the steps. Unless the order of the steps is clearly stated or the execution of a certain step requires other steps as a basis, the relative order of the steps can be adjusted. It can be understood that the term "and / or" used herein involves and covers any and all possible combinations of one or more of the related listed items.

[0096] The present invention proposes a path replanning method for an unmanned underwater vehicle in a complex environment. This is a path replanning method of Spatial Path Fusion Velocity Strategy (SPFVS). As Figure 1 shown, it is the overall framework of the method proposed by the present invention. The method proposed by the present invention is to reasonably coordinate when multiple types of abnormal events occur concurrently. It judges the detected abnormal events and the consequences of these events, and proposes solutions under the same framework according to their characteristics. Specifically as Figure 2 shown, it includes the following steps:

[0097] S1. Initialize the original path; Initialize the path in the static environment and let the unmanned underwater vehicle execute tasks according to this path;

[0098] S2. Uncertain event detection: Detect whether an uncertain event has occurred based on the feedback from the sensors and actuators of the unmanned underwater vehicle. If an uncertain event has occurred, further determine the type of the uncertain event and perform speed strategy resetting and path replanning. The uncertain events include: thruster failure, water flow interference, and moving obstacles.

[0099] S3. Thruster failure detection: When a thruster failure is detected, redesign the traveling strategy and thruster thrust allocation, and then proceed to step S4. When no thruster failure is detected, directly proceed to step S4.

[0100] As a preferred implementation manner of step S3, step S3 specifically includes:

[0101] 1) When there is a minor failure in the horizontal thruster, use the remaining normal thrusters to compensate for the failed thruster, reallocate the thrust to continue working, increase the thrust of the normal thrusters to compensate for the deficiency of the failed thruster, and maintain the original path planning unchanged.

[0102] 2) When the horizontal thruster fails severely and cannot work, allocate the vertical thrust of the remaining normal thrusters, that is, perform buoyancy through the vertical thrusters, and plan an upward path to quickly return to the water surface for salvage.

[0103] 3) When the vertical thruster fails, adjust the attitude of the unmanned underwater vehicle, tilt the underwater vehicle and then reallocate the thrust, use the horizontal thrusters to compensate, and generate an upward resultant force to help with buoyancy.

[0104] More specifically, the thrust allocation based on optimization solution in step S3 is specifically implemented through quadratic sequential programming, including:

[0105] First, set the thrust upper limit inequality constraint, then give the direction equality constraint, and give the optimization problem with a typical quadratic function as the objective function, which is expressed by the formula:

[0106]

[0107] Among them,

[0108]

[0109] In the above formula, f(T) is the standard quadratic programming to minimize the thruster thrust to reduce energy consumption; T is the thrust matrix of all thrusters; T i is the thrust of a single thruster, i represents the i-th thruster, and the number of thrusters is n; T m is the maximum thrust that a single thruster can provide; F is the resultant force vector generated by the thrusters, and W is a calculation equation related to the layout of the thrusters (as shown in Figure 6 ); the constraint condition s.t. is: F = W * T and T i≤T m , that is, taking the resultant force direction of the entire thrust matrix as the target and the individual thruster limits in various situations as constraints, and finally realizing replanning.

[0110] In this embodiment, the rotational speed signal is used to judge whether the power output of the thruster is normal, so as to judge whether a fault occurs. Such as Figure 3 shown, the above thrust distribution is achieved by constructing and solving an optimization problem. This design realizes the adjustment under minor faults and the return voyage planning under serious faults.

[0111] S4. Water flow detection; when water flow interference is detected, obtain water flow data and analyze it, then replan a suitable path in the complex water flow range according to the artificial potential field method APF, and then enter step S5; if no water flow interference is detected, directly enter step S5;

[0112] As a preferred implementation manner of step S4, step S4 specifically includes:

[0113] Obtaining water flow data and analyzing it specifically means:

[0114] Calculate the projection component U of the water flow vector on the path vector proj , and the formula is expressed as:

[0115]

[0116] Among them, U and D respectively represent the water flow vector and the path vector, and |D| is the modulus length of the path vector; according to the calculated projection component, judge the intensity of the water flow; first set a threshold according to the characteristics of the underwater robot; then calculate the projection of the water flow velocity vector on the robot's motion vector, if it is positive, it means the water flow is a boost in the motion direction; if it is negative, it means the water flow is an obstacle in the motion direction; when the absolute value of the negative value is greater than the set threshold, it is regarded as unable to pass, that is, the power of the unmanned underwater vehicle is not enough to overcome the water flow effect. At this time, replanning is carried out according to the artificial potential field method APF, and a new local target point is selected as the target of the replanned path.

[0117] In this embodiment, the acoustic Doppler current profiler carried by the underwater vehicle is used to measure the three-dimensional water flow velocity of the path and the area near the path, and judge whether the original planned path can be implemented in this environment according to the power of the underwater vehicle. If the power of the underwater vehicle is not enough to overcome the water flow effect, set a local safety target point in the path. Replan the path from the position entering the water flow field to the local safety target point. The path replanning adopts the artificial potential field method, and its specific process is:

[0118] S41. Establish the replanning target; select a local target point on the original path as the gravitational source to return to the original path, and the gravitational force F of the local target pointOPA Expressed as:

[0119]

[0120] Wherein, P and G are respectively the current position of the unmanned underwater vehicle and the position of the local target point; d is the distance between the replanning starting point and the local target point; the space between the starting point of the unmanned underwater vehicle and the local target point is the replanning space;

[0121] S42. Calculate the repulsive force F of the static obstacle rep ; that is, calculate the repulsive force of the conventionally existing static obstacle when planning the path due to the original path being unusable, and the formula is expressed as:

[0122]

[0123] Wherein, O i is the nearest position of the surface of each detected static obstacle to the unmanned underwater vehicle; δ is a parameter for adjusting the balance between the threat of water flow and the threat of obstacles; ε is used to prevent affecting the efficiency of the APF algorithm when the distance from the static obstacle is far;

[0124] S43. Calculate the force F exerted by the water flow on the unmanned underwater vehicle cur ; the formula is expressed as:

[0125] F cur = [U(P), V(P), W(P)] × β;

[0126] Wherein, the magnitude of this force is determined by the ocean current function of the current position P of the unmanned underwater vehicle, and U, V, W are the water flow vectors; the final value of F cur is related to the 3*1 matrix β. If the unmanned underwater vehicle has the same dynamic characteristics for water flows in all directions, it is regarded that the water flow acts completely on the unmanned underwater vehicle, and β is [1, 1, 1] T ;

[0127] S44. Calculate the resultant force F received by the unmanned underwater vehicle total , and the formula is expressed as:

[0128] F total = -F OPA + F rep + F cur ;

[0129] The resultant force F total guides the unmanned underwater vehicle to plan a replanning path that satisfies static obstacle avoidance and avoids water flow interference in space.

[0130] In this embodiment, as Figure 4As shown, if water flow interference is detected, the APF algorithm will use the magnitude and direction of the water flow in the local environment as the gravitational or repulsive force vector in the artificial potential field method (APF); then, based on the distance to the obstacle surface detected by sonar as the source of the potential field force, obstacles closer will generate a greater repulsive force to ensure safety. The local safety target point is used as the source of gravity to return to the original path. By superimposing different component forces, the underwater vehicle is driven to generate a spatial path locally. This path can also ensure the stability of the underwater vehicle in the water flow environment, static obstacle avoidance ability, and lower energy consumption.

[0131] S5. Moving obstacle detection; when a moving obstacle is detected, obtain its status information and input it into the fuzzy logic, and then use the output of the fuzzy logic as a parameter of the DWA dynamic window evaluation function to output an appropriate speed strategy to make the unmanned underwater vehicle decelerate safely and try to keep moving on the path replanned by APF; if no moving obstacle is detected, directly perform path tracking;

[0132] As a preferred implementation manner of step S5, as Figure 5 shown, step S5 specifically includes:

[0133] S51. Establish the relationship between the moving state of the moving obstacle and the speed strategy of the unmanned underwater vehicle through fuzzy logic; rely on sonar and depth cameras to obtain the status information of the moving obstacle, including position and speed; fuzzify the position and speed as input variables to obtain the position fuzzy set and speed fuzzy set;

[0134] The position is the lateral position of the moving obstacle relative to the moving direction of the unmanned underwater vehicle, and the speed is the lateral speed of the obstacle on the movement path of the robot;

[0135] In this embodiment, the analysis of dynamic obstacles is realized through fuzzy logic to obtain a smoother and safer strategy by taking early actions; the output of the fuzzy logic is used as a parameter of the DWA evaluation function to output an appropriate speed strategy to make the underwater unmanned vehicle decelerate safely and try to keep on the path replanned by the APF algorithm. In addition, it is worth noting that even without replanning the spatial path, this part of the speed strategy planning can be executed to achieve path tracking.

[0136] Since the previous method has re-planned the original path spatially and theoretically, it is possible to avoid moving obstacles by increasing or decreasing the speed. Therefore, the moving state and acceleration / deceleration of the obstacles can be related through fuzzy logic, and then DWA can be used for speed planning on the basis of minimizing the deviation from the path planned by the APF algorithm. This can not only handle various uncertain events that may occur simultaneously, but also further perform speed planning on the basis of spatial planning to ensure that the planned path has good smoothness and environmental adaptability, that is, a reasonable acceleration and speed strategy. First, rely on the sonar or depth camera of the underwater vehicle to obtain the dynamic information of the obstacles. Then, the state input variables "position" and "speed" of the obstacles are fuzzified. The obstacle position refers to the lateral position relative to the moving direction of the unmanned underwater vehicle and is divided into five fuzzy sets: "left far end, left near end, middle, right near end, right far end"; the speed is the lateral speed of the obstacle on the robot's moving path and is divided into five fuzzy sets: "left high speed, left low speed, stationary, right low speed, right high speed".

[0137] S52. Set reasonable fuzzy logic to achieve speed changes in response to obstacle states; for example: when the obstacle is at a relatively close distance on the left side and moving to the right at any speed, the deceleration of the unmanned underwater vehicle should reach the maximum to wait for the obstacle to pass. Until the obstacle is on the right side and continues to move to the right, the deceleration reaches the minimum, that is, resume the full-speed forward state.

[0138] S53. Set the deceleration parameter, and its value range is between 0 and 1; a value of 0 means no deceleration is required, and a value of 1 means decelerating to the maximum extent; the centroid method is used to defuzzify the fuzzy output, and the fuzzy output dcf is expressed as:

[0139]

[0140] where μ(x) is the output membership function and x is the fuzzy input containing the dynamic obstacle state; the centroid method generates a specific value between 0 and 1 by calculating the centroid of the fuzzy output curve.

[0141] In this embodiment, the so-called membership function is implemented by the given fuzzy logic table in cooperation with the fuzzification method. The specific rule table is as Figure 7 shown. The position and speed in the table refer to the dynamic obstacle state, and decelerate means deceleration is required. Fuzzifying these states will form a fuzzy logic. The input of the fuzzy logic can correspond to the output value, and defuzzifying the output value gives the fuzzy output dcf.

[0142] S54. Generate a DWA dynamic window based on dynamic constraints; the dynamic constraints include linear velocity constraints and angular velocity constraints, that is, the upper and lower limits of the velocity and the acceleration are restricted; in this embodiment, a DWA dynamic window is generated within a unit sampling time dt, and a shorter sampling time will result in high-frequency calculations. Therefore, in the case of focusing on energy consumption, the sampling time can be increased to reduce the computing power requirement and improve the endurance.

[0143] More specifically, this step includes:

[0144] S541. The linear velocity window (vel min , vel max ) of the unmanned underwater vehicle within the unit sampling time dt is set as:

[0145]

[0146] where vel C is the current linear velocity of the unmanned underwater vehicle, acc is the maximum acceleration scalar under the dynamic limit, and maxspeed is the maximum linear velocity that the unmanned underwater vehicle can reach;

[0147] The linear velocity vel is obtained by synthesizing the surge velocity v surge and the heave velocity v heave in direction, and the formula is expressed as:

[0148]

[0149] S542. The angular velocity window of the unmanned underwater vehicle within the unit sampling time dt, that is, the sampling range of the yaw (yaw min , yaw max ) is set as:

[0150]

[0151] where yaw C is the orientation angle relative to the replanned path of the APF algorithm, and accy is the maximum yaw angular acceleration;

[0152] S543. Velocity vector sampling; with precisions of τ1 and τ2, the sampling numbers N v and N y on the linear velocity and angular velocity are respectively:

[0153]

[0154] At each time step, sample N v ·N yA number of paths are input into the DWA evaluation function, and the velocity sequence (vel, yaw) with the highest DWA evaluation function value is selected as the dynamic window.

[0155] S55. Determine the selected velocity strategy based on the combination of the DWA dynamic window evaluation function and the fuzzy output;

[0156] In this embodiment, directly using the traditional evaluation function of DWA will lead to unstable path generation due to the excessive sensitivity of the algorithm to moving obstacles; and in the algorithm of the present invention, it should not completely deviate from the path replanned based on the APF algorithm in the water flow environment and then be processed separately. To solve this problem, the present invention introduces an additional evaluation criterion in DWA, which quantifies the overlap between each candidate trajectory and the original path, and helps to better follow the APF path during navigation. The specific process is as follows:

[0157] A series of possible trajectories are generated by calculating the feasible velocity and steering combinations within a given time window, and then the optimal trajectory is selected by evaluating the score of each trajectory; at the same time, an additional evaluation criterion is introduced to better track the path replanned by the APF algorithm when selecting the velocity strategy;

[0158] The evaluation criterion θ diff is expressed by the formula:

[0159]

[0160] where d orig and d generate represent the direction vector of the APF algorithm re-planned path and the direction vector of the path generated by DWA respectively; the smaller θ diff is, the higher the path fitness is, that is, it can better track the path replanned by the APF algorithm;

[0161] S56. Consider the safety assessment of moving obstacles; when the moving obstacle is relatively close, or its motion state is complex, that is, its motion trend cannot be predicted and judged; the close-range evaluation item S ob and the state prediction deceleration evaluation item S goal are set respectively to achieve emergency obstacle avoidance;

[0162] The formula of the close-range evaluation item S ob is expressed as:

[0163]

[0164] where P is the current position of the unmanned underwater vehicle, and O i is the nearest position of the surface of each moving obstacle to the unmanned underwater vehicle; when the distance between the obstacle and |P - O iWhen it is very close, this item will generate a larger negative value to avoid collisions. On the contrary, when the obstacle is still relatively far away, this item maintains a small value and is adjusted by the prediction deceleration evaluation item based on fuzzy logic, which can better avoid unstable factors such as sharp turns and deviations of the path.

[0165] And the prediction deceleration evaluation item S goal is expressed by the formula:

[0166] S goal = -|P - G0|(vel - vel max ·dcf) 2 ;

[0167] where G0 is the target position when advancing at full speed in this time step.

[0168] This item will guide the unmanned underwater vehicle to move forward towards the slow target point; when there are moving obstacles, it will make the current sampling speed as close as possible to the decelerated speed. This reward will weaken the moving trend towards the target point and consider more the position where the obstacle will pose a threat in the future. At the same time, it is necessary to keep the path direction towards the local target point. S57. According to steps S55 and S56, the final DWA evaluation function S is obtained, and the formula is expressed as:

[0169] S = k1S goal - k2S ob - k3|θ diff |;

[0170] where k1, k2, and k3 are the weights of each item;

[0171] Select the candidate path with the highest DWA evaluation function value among all candidate paths as the final replanned path, and use its corresponding speed strategy as the final strategy.

[0172] S6. After the strategy resetting and path replanning in steps S3 - S5, the unmanned underwater vehicle returns to the original path and executes the task;

[0173] S7. After returning to the original path, re - execute steps S2 - S6 at a certain frequency until the underwater task is completed.

[0174] In summary, through the method proposed by the present invention, the path replanning of the unmanned underwater vehicle in a complex environment is realized. It can cope with strong water flow interference, dynamic obstacles, and even thruster failures in the environment, ensuring the reliability and safety of the underwater vehicle to execute tasks.

[0175] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above-described exemplary embodiments, and the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the present invention. Therefore, in any aspect, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Therefore, all changes falling within the meaning and scope of the equivalent elements of the claims are intended to be embraced within the present invention. Any reference signs in the claims should not be construed as limiting the claims involved.

[0176] In addition, it should be understood that although this specification is described according to embodiments, not every embodiment only contains an independent technical solution. This narrative way of the specification is only for clarity. Those skilled in the art should regard the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A method for replanning a path of an unmanned underwater vehicle in a complex environment, characterized in that: The following steps are involved: S1. Initialize the original path; initialize the path in the static environment, and let the unmanned underwater vehicle perform the task according to the path; S2, uncertain event detection: based on the feedback from the sensors and actuators of the unmanned underwater vehicle, detect whether an uncertain event has occurred. If so, further determine the type of uncertain event and reset the speed strategy and replan the path; the uncertain events include: propeller failure, water flow interference, and moving obstacles; S3, propeller fault detection; when a propeller fault is detected, the travel strategy and propeller thrust distribution are redesigned, and then the process goes to step S4; when no propeller fault is detected, the process goes directly to step S4; S4, water flow detection: when water flow interference is detected, water flow data is obtained and analyzed, specifically: Calculate the water flow vector to determine the strength of the water flow; if the power of the unmanned underwater vehicle is not enough to overcome the effect of the water flow, then replan a suitable path in the complex water flow range based on the artificial potential field method APF, including: S41, establish the re-planning target; select a local target point on the original path as the gravity source to return to the original path, the gravity of the local target point It is expressed as: ； in, and are the current position of the unmanned underwater vehicle and the position of the local target point respectively; is the distance between the replanning starting point and the local target point; the space between the starting point of the unmanned underwater vehicle and the local target point is the replanning space; S42. Calculate static obstacle repulsion force ; That is, calculate the repulsion of static obstacles that exist in the planning path because the original path cannot be used normally. The formula is expressed as: ； in, is the closest position of each detected static obstacle surface to the UUV; It is a parameter that adjusts the balance between the threat of water flow and the threat of obstacles; Used to prevent the efficiency of the APF algorithm from being affected when the distance to static obstacles is far; S43. Calculate the force exerted by the water flow on the unmanned underwater vehicle ; The formula is: ； The magnitude of this force is determined by the current position of the UUV. P The ocean current function, is the water flow vector; The final value and 3*1 matrix If the underwater vehicle has the same dynamic characteristics for water flow in all directions, it is considered that the water flow completely acts on the unmanned underwater vehicle. for ; S44. Calculate the resultant force on the unmanned underwater vehicle , the formula is: ； The combined force Guide the unmanned underwater vehicle to plan a replanned path that satisfies static obstacle avoidance and avoids water flow interference in space; Then proceed to step S5; If no water flow interference is detected, directly proceed to step S5; S5, mobile obstacle detection: when a mobile obstacle is detected, its status information is obtained and input into the fuzzy logic, and the output of the fuzzy logic is used as the parameter of the DWA dynamic window evaluation function to output a suitable speed strategy to make the unmanned underwater vehicle decelerate safely and try to keep moving on the path replanned by the APF; if no mobile obstacle is detected, the path tracking is directly performed; S6. After the strategy resetting and path replanning in steps S3-S5, the unmanned underwater vehicle returns to the original path and performs the mission; S7. After returning to the original path, steps S2-S6 are re-executed at a certain frequency until the underwater mission is completed.

2. The method for replanning a path of an unmanned underwater vehicle in a complex environment according to claim 1, characterized in that: In step S3: The thrusters include thrusters for controlling horizontal degrees of freedom, i.e., horizontal thrusters, and thrusters for controlling submergence, i.e., vertical thrusters. According to the type of thrusters and the degree of failure, it is determined whether it is necessary to plan a path back to the surface and redesign the speed strategy and thrust distribution, which specifically includes: 1) When a horizontal thruster has a minor fault, the remaining normal thrusters are used to compensate for the faulty thrusters, and the thrust is redistributed to continue working. The thrust of the normal thruster is increased to compensate for the deficiency of the faulty thruster, and the original path planning is maintained unchanged; 2) When the horizontal thruster fails seriously and cannot work, the vertical thrust of the remaining normal thrusters is allocated, that is, the vertical thrusters are used for buoyancy, and an upward path is planned to return to the water surface as soon as possible for salvage; 3) When the vertical thruster fails, adjust the attitude of the unmanned underwater vehicle, tilt the underwater vehicle and then redistribute the thrust, use the horizontal thruster to compensate, generate an upward force, and help it float.

3. The method for replanning a path of an unmanned underwater vehicle in a complex environment according to claim 2, characterized in that: In step S3, the thrust distribution is solved based on optimization, which is specifically implemented through secondary sequence planning, including: First, the thrust upper limit inequality constraint is set, and then the direction equality constraint is given. The typical quadratic function is used as the objective function to give the optimization problem, which is expressed as follows: ； in, ； ； In the above formula, Minimize thruster thrust for standard quadratic programming to reduce energy consumption; is the thrust matrix for all thrusters; is the thrust of a single thruster, Indicates that it is The number of thrusters is n ; The maximum thrust that a single thruster can provide; is the resultant force vector generated by the thruster, is the calculation equation related to the layout of the thruster; the constraint condition st for: and , that is, taking the resultant force direction of the entire thrust matrix as the target and the limitation of a single thruster in various situations as the constraint, finally achieving re-planning.

4. The method for replanning a path of an unmanned underwater vehicle in a complex environment according to claim 1, characterized in that: In step S4, the projection component of the water flow vector on the path vector is calculated To judge the intensity of water flow, the calculation formula of the projection component is: ； in, and represent the water flow vector and path vector respectively, is the magnitude of the path vector.

5. The method for replanning the path of an unmanned underwater vehicle in a complex environment according to claim 4, characterized in that: In step S4, the water flow intensity is determined according to the projection component as follows: A threshold is set according to the characteristics of the underwater robot; the projection of the water flow velocity vector on the robot's motion vector is calculated. If it is a positive value, it means that the water flow is a boost in the direction of motion; if it is a negative value, it means that the water flow is an obstacle in the direction of motion; when the absolute value of the negative value is greater than the set threshold, it is considered to be impassable, that is, the power of the unmanned underwater vehicle is insufficient to overcome the effect of the water flow.

6. The method for replanning a path of an unmanned underwater vehicle in a complex environment according to claim 1, characterized in that: In step S41, the selection of the local target point is specifically as follows: If the global target point is outside the water flow area, a safe point outside the water flow area on the original path can be set as the local target point; If the global target point is within the water flow area, the point is directly regarded as a local target point.

7. The method for replanning a path of an unmanned underwater vehicle in a complex environment according to claim 1, characterized in that: Step S5 specifically includes: S51. Establish the relationship between the moving state of the moving obstacle and the speed strategy of the unmanned underwater vehicle through fuzzy logic; obtain the state information of the moving obstacle, including the position and speed, by relying on sonar and depth camera; fuzzify the position and speed as input variables to obtain the position fuzzy set and the speed fuzzy set; The position is the lateral position of the moving obstacle relative to the direction of motion of the unmanned underwater vehicle, and the speed is the lateral speed of the obstacle on the robot's motion path; S52, setting reasonable fuzzy logic to achieve speed change to cope with obstacle state; S53, set the deceleration parameter, the value range of which is between 0 and 1; a value of 0 means no deceleration is required, and a value of 1 means maximum deceleration; use the centroid method to defuzzify the fuzzy output to obtain the fuzzy output : ； in, is the output membership function, is the fuzzy input containing the dynamic obstacle state; S54, generating a DWA dynamic window according to a dynamic constraint; the dynamic constraint includes a linear velocity constraint and an angular velocity constraint, that is, an upper limit, a lower limit and an acceleration of a restricted velocity; S55, based on the DWA dynamic window evaluation function combined with fuzzy output, the selected speed strategy is determined; a series of possible trajectories are generated by calculating the feasible speed and steering combinations within a given time window, and then the optimal trajectory is selected by evaluating the score of each trajectory; at the same time, an additional evaluation criterion is introduced to better track the APF algorithm replanning path when selecting the speed strategy; The evaluation criteria The formula is expressed as: ； in, and They represent the re-path direction vector of the APF algorithm and the path direction vector generated by DWA respectively; The smaller the value, the higher the fit of the path, which means that the APF algorithm replanning path can be better tracked; S56. Consider the safety assessment of moving obstacles; when the moving obstacles are close or their movement state is complex, that is, their movement trend cannot be predicted; set the close-range evaluation items separately. and predicted deceleration evaluation items , to achieve emergency obstacle avoidance; S57: According to steps S55 and S56, the final DWA evaluation function is obtained. S , the formula is: ； in, k 1 、k 2 、k 3 is the weight of each item; The path with the highest DWA evaluation function value among all candidate paths is selected as the final replanning path, and its corresponding speed strategy is used as the final strategy.

8. The method for replanning the path of an unmanned underwater vehicle in a complex environment according to claim 7, characterized in that: Step S54 specifically includes: S541, unit sampling time dt Linear velocity window of the unmanned underwater vehicle ( , ) is set to: ； in, is the current linear velocity of the unmanned underwater vehicle, is the maximum acceleration scalar under power limitation, is the maximum linear speed that the unmanned underwater vehicle can achieve; The linear velocity vel is determined by the longitudinal velocity of the unmanned underwater vehicle. and diving speed Directional synthesis is performed and the formula is expressed as: ； S542, unit sampling time dt The angular velocity window of the unmanned underwater vehicle, that is, the sampling range of the bow roll Set to: ； in, is the current orientation angle relative to the APF algorithm replanning path, is the maximum yaw angular acceleration; S543, velocity vector sampling; using precision and , then the number of samples for linear velocity and angular velocity is and They are: ； Sample at each time step paths, input these paths into the DWA evaluation function, and select the speed sequence with the highest DWA evaluation function value As a dynamic window.

9. The method for replanning the path of an unmanned underwater vehicle in a complex environment according to claim 8, characterized in that: In step S56: The close distance evaluation item The formula is: ； in, P is the current position of the unmanned underwater vehicle, is the closest position of each moving obstacle surface to the unmanned underwater vehicle; The predicted deceleration evaluation item The formula is: ； in, is the target position at full speed in this time step.

Images