Unmanned underwater vehicle path re-planning method in complex environment

Abstract

The invention provides an unmanned underwater vehicle path re-planning method in a complex environment, which is a path re-planning method combining a space path with a speed strategy, and comprises the following steps: detecting a fault of a propeller, then deciding to continue to execute a task or return to reduce loss, and implementing the path re-planning method through thrust distribution; re-planning a path in a sudden water flow environment through an APF algorithm so as to cope with water flow obstruction and return to an original path at a safe position; in combination with a DWA algorithm, sudden moving obstacles are further considered, and the method can avoid the obstacles on the premise that an original planned path and a re-planned path in a water flow environment are not damaged. According to the method provided by the invention, path re-planning under different events can be completed aiming at potential safety hazards caused by emergencies in a complex underwater scene, reasonable coordination is carried out when multiple types of abnormal events are concurrent, and the path re-planning method under three uncertain events can be simultaneously realized in an overall algorithm framework.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning for unmanned underwater vehicles, and in particular to a path replanning method for unmanned underwater vehicles in complex environments. Background Art

[0002] Unmanned underwater vehicles can replace humans to perform tasks in some dangerous ocean scenarios, including seabed search, underwater rescue, and submarine pipeline maintenance. The basis of these tasks is that underwater vehicles can autonomously plan navigation paths. With the continuous advancement of sensor technology and the continuous development of intelligent algorithms, the path planning technology of autonomous unmanned underwater vehicles has been further improved, realizing two-dimensional path planning in static surface obstacle environments and three-dimensional path planning in seabed environments. Although unmanned underwater vehicles can effectively and reliably realize autonomous path planning in simple scenarios, they still have shortcomings in complex underwater scenarios, as follows: (1) Due to the large amount of water flow interference in dynamic water environments, underwater exploration and search are significantly hindered and unsafe. When the fluid encounters an obstacle, several vortices will form around the obstacle. Strong water flow can easily cause the underwater vehicle to have an unstable posture or even lose control; (2) There are various plankton in the ocean, which may move quickly or be large in size, affecting the safety of underwater vehicles. Traditional path planning methods often do not have the ability to make real-time adjustments based on the state of dynamic obstacles. Once approaching an obstacle, the unmanned underwater vehicle will not be able 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 entangled in water plants or small plankton, resulting in abnormal thrust output. Once the propeller fails, it may be unable to track the planned path, resulting in the unmanned vehicle not only unable to complete the mission, but also unable to return and recover, causing losses and marine pollution. In response to the safety hazards caused by the above-mentioned emergencies, it is necessary to design effective solutions to deal with various emergencies and control the unmanned underwater vehicle to avoid safety hazards. Summary of the invention

[0003] In response to the safety hazards caused by emergencies in complex underwater scenes and the problems existing in the above-mentioned prior art, the present invention provides a path replanning method for an unmanned underwater vehicle in a complex environment, which can also be called a path replanning method of a spatial path fusion velocity strategy (SPFVS). The purpose of the present invention is not only to complete path replanning under different emergencies, but also to reasonably coordinate multiple types of abnormal events when they occur concurrently.

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

[0005] 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;

[0006] 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;

[0007] 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;

[0008] S4, water flow detection; when water flow interference is detected, water flow data is obtained and analyzed, and then a suitable path in the complex water flow range is replanned according to the artificial potential field method APF, and then step S5 is entered; if no water flow interference is detected, step S5 is directly entered;

[0009] 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;

[0010] 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;

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

[0012] Further, in step S3:

[0013] 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:

[0014] 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;

[0015] 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;

[0016] 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.

[0017] More specifically, the thrust distribution based on optimization in step S3 is specifically implemented through secondary sequence programming, including:

[0018] 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:

[0019]

[0020] in,

[0021]

[0022] In the above formula, f(T) is the standard quadratic programming method to minimize the thrust of the propeller to reduce energy consumption; T is the thrust matrix of all propellers; 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 thruster, and W is the calculation equation related to the layout of the thruster; the constraint condition st 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 single thruster limitation in various situations as the constraint, finally achieving re-planning.

[0023] Further, in step S4:

[0024] Obtaining water flow data and analyzing it specifically includes:

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

[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. The strength of the water flow is judged according to the calculated projection component. If the power of the unmanned underwater vehicle is not enough to overcome the effect of the water flow, replanning is performed according to the artificial potential field method APF, and a new local target point is selected as the replanning path target.

[0028] More specifically, judging the intensity of the water flow according to the calculated projection component in step S4 is 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'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.

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

[0031] 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 F OPA It is expressed as:

[0032]

[0033] Where 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 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;

[0034] S42. Calculate static obstacle repulsion force F rep ; 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:

[0035]

[0036] Among them, O i is the closest position of each detected static obstacle surface to the unmanned underwater vehicle; δ is a parameter for adjusting the balance between water flow threat and obstacle threat; ε is used to prevent the efficiency of the APF algorithm from being affected when the distance to 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:

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

[0039] 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 underwater vehicle has the same dynamic characteristics for the water flow in all directions, the water flow is considered to act completely on the unmanned underwater vehicle, and β is [1,1,1] T ;

[0040] S44. Calculate the resultant force F on the unmanned underwater vehicle total , the formula is:

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

[0042] The resultant force F total Guide the unmanned underwater vehicle to plan a replanned 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 outside the water flow area on the original path can be set as the local target point;

[0045] If the global target point is within the water flow area, the point is directly regarded as a 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; 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;

[0048] 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;

[0049] S52, setting reasonable fuzzy logic to achieve speed change to cope with obstacle state;

[0050] S53, set the deceleration parameter, whose value range 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 dcf:

[0051]

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

[0053] 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;

[0054] 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;

[0055] The evaluation criteria θ diff The formula is expressed as:

[0056]

[0057] Among them, d orig and d generate Represent the APF algorithm re-path direction vector and the DWA generated path direction vector respectively; θ diff The smaller the value, the higher the fit of the path, which means that the APF algorithm replanning path can be better tracked;

[0058] S56, consider the safety assessment of moving obstacles; when the moving obstacle is close, or its movement state is complex, that is, its movement trend cannot be predicted; set the close distance evaluation item S ob and the predicted deceleration evaluation item S goal , 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 weights of each item;

[0062] 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.

[0063] More specifically, step S54 specifically includes:

[0064] S541, the linear velocity window (vel min ,vel max ) is set to:

[0065]

[0066] Among them, 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 determined by the longitudinal velocity v of the unmanned underwater vehicle surge and diving speed v heave Directional synthesis is performed 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 the bow roll (yaw min ,yaw max ) is set to:

[0070]

[0071] Among them, yaw C is the current heading angle relative to the path replanned by the APF algorithm, accy is the maximum heading angular acceleration;

[0072] S543, velocity vector sampling; using precision τ1 and τ2, the number of samples N on the linear velocity and angular velocity v and N y They are:

[0073]

[0074] N samples are sampled at each time step v ·N y These paths are input into the DWA evaluation function, and the speed sequence (vel, yaw) with the highest score DWA evaluation function value is selected as the dynamic window.

[0075] More specifically, in step S56:

[0076] The close distance evaluation item S ob The formula is expressed as:

[0077]

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

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

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

[0081] Among them, G0 is the target position at full speed in this time step.

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

[0083] 1. The present invention comprehensively considers the potential safety hazards that unmanned underwater vehicles may encounter during operation. By integrating the spatial path with the speed strategy, an overall framework for path replanning is designed. This framework can reasonably coordinate multiple types of abnormal events when they occur concurrently, provide replanned paths in complex water flow environments, and ensure the real-time and flexibility of path planning.

[0084] 2. In the face of water flow interference, the method proposed in the present invention uses the artificial field potential method APF to use the local safe target point as the source of gravity to return to the original path; the underwater vehicle is driven to generate a spatial path locally through the superposition of different partial forces for replanning. The replanned path can not only ensure the static obstacle avoidance capability of the underwater vehicle, but also cope with the impact of water flow disturbance, thereby reducing the energy consumption of the underwater vehicle in performing tasks in the water flow environment.

[0085] 3. For moving obstacles that may cause collisions on a known path, the method proposed in the present invention uses fuzzy logic output as a parameter of the DWA evaluation function, establishes a connection between the moving state and acceleration and deceleration of the obstacle through fuzzy logic, and then uses DWA to plan the speed on the basis of not deviating as much as possible from the path planned by the APF algorithm. This ensures that the planned path has good smoothness and environmental adaptability, and at the same time, the unmanned underwater vehicle can have a reasonable acceleration and speed strategy, can effectively avoid moving obstacles, and improve 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 the mission. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 The overall framework diagram of the present invention for detecting complex environments and implementing path replanning;

[0088] Figure 2A flowchart of the overall process of detailed path replanning in various complex environments provided by the present invention;

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

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

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

[0092] Figure 6 A schematic diagram of the propulsion layout of the unmanned underwater vehicle involved in the present invention;

[0093] Figure 7 It is the fuzzy logic rule table involved in the embodiment of the present invention. DETAILED DESCRIPTION

[0094] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with 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 numbers, 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 is understood that the term "and / or" used in this article involves and covers any and all possible combinations of one or more of the associated listed items.

[0096] The present invention proposes a path replanning method for an unmanned underwater vehicle in a complex environment, which is a path replanning method based on a spatial path fusion velocity strategy (SPFVS). Figure 1 As shown in FIG. 1 , it is the overall framework of the method proposed in the present invention. The method proposed in the present invention is to reasonably coordinate multiple types of abnormal events when they occur simultaneously. It judges the abnormal events detected and the consequences of these events, and proposes solutions under the same framework according to their characteristics. Figure 2 As shown, the following steps are included:

[0097] 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;

[0098] 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;

[0099] 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;

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

[0101] 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;

[0102] 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;

[0103] 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.

[0104] More specifically, the thrust distribution based on optimization in step S3 is specifically implemented through secondary sequence programming, including:

[0105] 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:

[0106]

[0107] in,

[0108]

[0109] In the above formula, f(T) is the standard quadratic programming to minimize the thrust of the propeller to reduce energy consumption; T is the thrust matrix of all propellers; 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 thruster, and W is the calculation equation related to the layout of the thruster (such as Figure 6 As shown); the constraint condition st 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 single thruster limitation in various situations as the constraint, finally achieving re-planning.

[0110] In this embodiment, the feedback speed signal is used to determine whether the propeller power output is normal, thereby determining whether a fault occurs. Figure 3 As shown in Figure 1, the above thrust distribution is solved by constructing an optimization problem. This design realizes the adjustment under minor faults and the return planning under serious faults.

[0111] S4, water flow detection; when water flow interference is detected, water flow data is obtained and analyzed, and then a suitable path in the complex water flow range is replanned according to the artificial potential field method APF, and then step S5 is entered; if no water flow interference is detected, step S5 is directly entered;

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

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

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

[0115]

[0116] Among them, U and D represent the water flow vector and the path vector respectively, and |D| is the modulus of the path vector. The strength of the water flow is judged according to the calculated projection component. First, a threshold is set according to the characteristics of the underwater robot. Then, 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 an assist 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 not enough to overcome the effect of the water flow. At this time, replanning is performed according to the artificial potential field method APF, and a new local target point is selected as the replanning path target.

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

[0118] 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 FOPA It is expressed as:

[0119]

[0120] Where 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 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 static obstacle repulsion force F rep ; 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:

[0122]

[0123] Among them, O i is the closest position of each detected static obstacle surface to the unmanned underwater vehicle; δ is a parameter for adjusting the balance between water flow threat and obstacle threat; ε is used to prevent the efficiency of the APF algorithm from being affected when the distance to 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:

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

[0126] 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 underwater vehicle has the same dynamic characteristics for the water flow in all directions, the water flow is considered to act completely on the unmanned underwater vehicle, and β is [1,1,1] T ;

[0127] S44. Calculate the resultant force F on the unmanned underwater vehicle total , the formula is:

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

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

[0130] In this embodiment, Figure 4As shown in the figure, if water flow interference is detected, the APF algorithm will use the size 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 obstacle surface distance detected by the sonar as the source of the potential field force, obstacles closer to the target point will generate a larger repulsive force to ensure safety. The local safe target point is used as the source of gravity to return to the original path. The superposition of different partial forces drives the underwater vehicle 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 capability and lower energy consumption.

[0131] 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;

[0132] As a preferred implementation of step S5, Figure 5 As 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; 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;

[0134] 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;

[0135] In this embodiment, fuzzy logic is used to analyze dynamic obstacles so as to take action in advance to obtain a smoother and safer strategy; the fuzzy logic output is used as a parameter of the DWA evaluation function to output a suitable speed strategy to make the underwater unmanned vehicle decelerate safely and try to stay on the path replanned by the APF algorithm. In addition, it is worth noting that even if the spatial path is not replanned, this part of the speed strategy planning can be executed to achieve path tracking.

[0136] Since the previous method has replanned the original path in space, and for moving obstacles, theoretically, it is possible to avoid them by increasing or decreasing the speed, the moving state of the obstacle and the acceleration and deceleration can be linked through fuzzy logic, and then DWA can be used to plan the speed on the basis of not deviating the path planned by the APF algorithm as much as possible. This can not only cope with various uncertain events that may occur at the same time, but also further plan the speed on the basis of spatial planning to ensure that the planned path has good smoothness and environmental adaptability, that is, reasonable acceleration and speed strategies. First, the dynamic information of the obstacle is obtained by the sonar or depth camera of the underwater vehicle. Then the state of the obstacle is input into the variables "position" and "speed" to be fuzzy. The obstacle position refers to the lateral position relative to the direction of motion of the unmanned underwater vehicle, which 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 motion path, which is divided into five fuzzy sets: "left high speed, left low speed, stationary, right low speed, right high speed".

[0137] S52. Set up reasonable fuzzy logic to achieve speed change to cope with obstacle state; for example, when the obstacle is at a relatively close distance on the left and moves to the right at any speed, the unmanned underwater vehicle should slow down to the maximum to wait for the obstacle to pass. When the obstacle is on the right and continues to move to the right, the deceleration level reaches the minimum, that is, the full speed forward state is restored.

[0138] S53, set the deceleration parameter, whose value range is between 0 and 1; a value of 0 indicates that no deceleration is required, and a value of 1 indicates maximum deceleration; the fuzzy output is defuzzified using the centroid method, and the fuzzy output dcf is expressed as:

[0139]

[0140] Among them, μ(x) is the output membership function, 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 a given fuzzy logic table in combination with a fuzzification method. The specific rule table is as follows: Figure 7 As shown in the table, the position and speed refer to the dynamic obstacle state, and decelerate means the need to decelerate. Fuzzifying these states will form a fuzzy logic. The input of the fuzzy logic can correspond to the output value, and the fuzzy output DCF is obtained by defuzzifying the output value.

[0142] S54, generating a DWA dynamic window according to the power constraint; the power constraint includes a linear velocity constraint and an angular velocity constraint, that is, limiting the upper and lower limits of the velocity and the acceleration; in this embodiment, the DWA dynamic window is generated within a unit sampling time dt, and a shorter sampling time will lead to high-frequency calculations. Therefore, in the case of focusing on energy consumption, the sampling time can be increased to reduce the computing power requirements and improve the endurance.

[0143] More specifically, the steps include:

[0144] S541, the linear velocity window (vel min ,vel max ) is set to:

[0145]

[0146] Among them, 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;

[0147] The linear velocity vel is determined by the longitudinal velocity v of the unmanned underwater vehicle surge and diving speed v heave Directional synthesis is performed 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 bow roll (yaw min ,yaw max ) is set to:

[0150]

[0151] Among them, yaw C is the current heading angle relative to the path replanned by the APF algorithm, accy is the maximum heading angular acceleration;

[0152] S543, velocity vector sampling; using precision τ1 and τ2, the number of samples N on the linear velocity and angular velocity v and N y They are:

[0153]

[0154] N samples are sampled at each time step v ·N yThese paths are input into the DWA evaluation function, and the speed sequence (vel, yaw) with the highest score DWA evaluation function value is selected as the dynamic window.

[0155] S55, determining the selected speed strategy based on the DWA dynamic window evaluation function combined with the fuzzy output;

[0156] In this embodiment, directly using the traditional evaluation function of DWA will lead to instability in the generated path due to the algorithm's high sensitivity to moving obstacles; and in the algorithm of the present invention, the path replanned based on the APF algorithm in the water flow environment should not be completely separated and reprocessed separately. In order 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, which helps to better follow the APF path during navigation. The specific process is:

[0157] A series of possible trajectories are generated by calculating feasible speed and steering combinations within a given time window, and then the best 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.

[0158] The evaluation criteria θ diff The formula is expressed as:

[0159]

[0160] Among them, d orig and d generate Represent the APF algorithm re-path direction vector and the DWA generated path direction vector respectively; θ diff The smaller the value, the higher the fit of the path, which means that the APF algorithm replanning path can be better tracked;

[0161] S56, consider the safety assessment of moving obstacles; when the moving obstacle is close, or its movement state is complex, that is, its movement trend cannot be predicted; set the close distance evaluation item S ob and state prediction deceleration evaluation item S goal , to achieve emergency obstacle avoidance;

[0162] The close distance evaluation item S ob The formula is:

[0163]

[0164] Where P is the current position of the unmanned underwater vehicle, O i is the closest position of each moving obstacle surface to the unmanned underwater vehicle; when the obstacle distance |PO i| is very close, this item will produce a larger negative value to avoid collision. On the contrary, when the obstacle is still far away, this item keeps a small value and is adjusted by the predictive deceleration evaluation item based on fuzzy logic, which can better avoid unstable factors such as sudden turns and deviations of the path.

[0165] The predicted deceleration evaluation item S goal The formula is expressed as:

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

[0167] Among them, G0 is the target position at full speed in this time step.

[0168] This item will guide the unmanned underwater vehicle to move towards the slow target point; when there is a moving obstacle, the current sampling speed is as close to the deceleration speed as possible. This reward will weaken the movement trend towards the target point and consider more the location 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] Among them, k1, k2, and k3 are weights of each item;

[0171] 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.

[0172] 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;

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

[0174] In summary, the method proposed in the present invention realizes the path replanning of unmanned underwater vehicles in complex environments, and can cope with strong water flow interference, dynamic obstacles and even propeller failures in the environment, ensuring the reliability and safety of underwater vehicles in performing tasks.

[0175] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0176] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes 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, and then a suitable path in the complex water flow range is replanned according to the artificial potential field method APF, and then step S5 is entered; if no water flow interference is detected, step S5 is directly entered; 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, f(T) is the standard quadratic programming to minimize the thrust of the propeller to reduce energy consumption; T is the thrust matrix of all propellers; 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 thruster, and W is the calculation equation related to the layout of the thruster; the constraint condition st 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 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, water flow data is obtained and analyzed as follows: Calculate the projection component U of the water flow vector on the path vector proj , the formula is: Among them, U and D represent the water flow vector and the path vector respectively, and |D| is the modulus of the path vector. The strength of the water flow is judged according to the calculated projection component. If the power of the unmanned underwater vehicle is not enough to overcome the effect of the water flow, replanning is performed according to the artificial potential field method APF, and a new local target point is selected as the replanning path target.

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, judging the intensity of the water flow according to the calculated projection component is specifically 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 4, characterized in that: In step S4, a suitable path in the complex water flow range is replanned according to the artificial potential field method APF, which specifically includes: 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 F of the local target point OPA It is expressed as: Where 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 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 F rep ; 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: Among them, O i is the closest position of each detected static obstacle surface to the unmanned underwater vehicle; δ is a parameter for adjusting the balance between water flow threat and obstacle threat; ε is used to prevent the efficiency of the APF algorithm from being affected when the distance to the static obstacle is far; S43. Calculate the force F exerted by the water flow on the unmanned underwater vehicle cur ; The formula is: F cur =[U(P),V(P),W(P)]×β; 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 underwater vehicle has the same dynamic characteristics for the water flow in all directions, the water flow is considered to act completely on the unmanned underwater vehicle, and β is [1,1,1] T ; S44. Calculate the resultant force F on the unmanned underwater vehicle total , the formula is: F total =-F OPA +F rep +F cur ; The resultant force F total Guide the unmanned underwater vehicle to plan a replanned path that satisfies static obstacle avoidance and avoids water flow interference in space.

7. The method for replanning the path of an unmanned underwater vehicle in a complex environment according to claim 6, 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.

8. 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, whose value range 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 dcf: Among them, μ(x) is the output membership function, and x 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 θ diff The formula is expressed as: Among them, d orig and d generate Represent the APF algorithm re-path direction vector and the DWA generated path direction vector respectively; θ diff 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 obstacle is close, or its movement state is complex, that is, its movement trend cannot be predicted; set the close distance evaluation item S ob and the predicted deceleration evaluation item S goal , to achieve emergency obstacle avoidance; S57. According to steps S55 and S56, the final DWA evaluation function S is obtained, and the formula is expressed as: S=k1S goal -k2S ob -k3|θ diff |; Among them, k1, k2, and k3 are weights 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.

9. The method for replanning the path of an unmanned underwater vehicle in a complex environment according to claim 8, characterized in that: Step S54 specifically includes: S541, the linear velocity window (vel min ,vel max ) is set to: Among them, 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; The linear velocity vel is determined by the longitudinal velocity v of the unmanned underwater vehicle surge and diving speed v heave Directional synthesis is performed and the formula is expressed as: S542, the angular velocity window of the unmanned underwater vehicle within the unit sampling time dt, that is, the sampling range of the bow roll (yaw min ,yaw max ) is set to: Among them, yaw C is the current heading angle relative to the path replanned by the APF algorithm, accy is the maximum heading angular acceleration; S543, velocity vector sampling; using precision τ1 and τ2, the number of samples N on the linear velocity and angular velocity v and N y They are: N samples are sampled at each time step v ·N y These paths are input into the DWA evaluation function, and the speed sequence (vel, yaw) with the highest DWA evaluation function value is selected as the dynamic window.

10. 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 S ob The formula is: Where P is the current position of the unmanned underwater vehicle, O di is the closest position of each moving obstacle surface to the unmanned underwater vehicle; The predicted deceleration evaluation item S goal The formula is: S goal =-|P-G0|(velocity max ·dcf) 2 ; Among them, G0 is the target position at full speed in this time step.

Images