An unmanned underwater vehicle online path planning method based on APF-DWA fusion algorithm

Abstract

The application discloses an unmanned underwater vehicle (UUV) online path planning method based on an APF-DWA fusion algorithm, and belongs to the field of UUV path planning.The application solves the problems that the existing DWA algorithm cannot meet the real-time response capability requirement of UUV online path planning in a marine environment and the terminal attitude constraint is not considered in the online planning task.According to the direction of the resultant force, the velocity sampling space of the DWA algorithm is dynamically adjusted; the higher the correlation degree between the resultant angular velocity and the velocity sampling space is, the stronger the adjusting effect is.The number of velocity samples can be reduced by narrowing the velocity window, and the real-time performance of the algorithm is effectively improved.The terminal heading is designed, and the terminal heading is taken as the expected heading of the DWA algorithm and the gravity direction of the APF algorithm, so that the UUV is aligned in the process of approaching the target, and the terminal attitude constraint problem is effectively solved.The method can be applied to the online path planning of the UUV.

Description

Technical Field

[0001] This invention belongs to the field of path planning for unmanned underwater vehicles (UUVs), specifically relating to an online path planning method for UUVs based on the APF-DWA fusion algorithm. Background Technology

[0002] Unmanned underwater vehicles (UUVs) have been widely used in underwater inspection, data collection, and reconnaissance. However, UUVs face interference from various factors such as seabed topography, marine life, and other marine equipment during missions, requiring rapid responses to unknown and dynamic obstacles. Therefore, path planning, as a prerequisite for autonomous and safe navigation of UUVs, faces higher standards and more complex challenges.

[0003] Online path planning is based on real-time local environmental information collected by UUV sensors. It autonomously plans a path to avoid obstacles and safely complete the task based on acquired environmental information such as the mission objective and obstacles. This includes D... Numerous path planning algorithms, including algorithms, artificial potential field (APF) methods, dynamic window methods (DWA), and fuzzy logic algorithms, have been widely applied to online path planning problems. Among them, the artificial potential field method guides UUVs along the resultant force by constructing a virtual potential field, and it is simple in principle and easy to implement, but it suffers from drawbacks such as local minima and target unreachability. The dynamic window method predicts feasible trajectories that satisfy UUV maneuvering constraints and selects the optimal trajectory's velocity and angular velocity as the UUV's movement speed, but the underwater environment places higher demands on the dynamic window method. The main actuators of underactuated UUVs consist of the main thruster, pitch rudder, and rudder, lacking lateral and vertical driving forces. Roll motion is usually self-stabilizing, requiring consideration of the UUV's sway, pitch, and yaw motions. Considering the maneuvering characteristics of UUVs, the dynamic window method needs to extend the trajectory prediction time and sampling interval to increase the trajectory coverage and ensure navigation safety. These measures all increase computational complexity, posing challenges to the algorithm's real-time performance and making it difficult to meet the real-time response requirements of UUVs in the marine environment. Common algorithms typically do not consider UUV maneuver constraints, such as speed, acceleration, and pitch angle, and only plan collision-free paths to the destination. Although the dynamic speed window established by the DWA algorithm can satisfy maneuver constraints, when facing complex tasks such as docking and recovery, and fixed-point sampling, the planner not only needs to plan collision-free paths that meet maneuver constraints, but also needs to meet terminal attitude constraints, that is, the attitude of the UUV when it arrives at the destination needs to converge to a specified value. Existing path planning methods still cannot meet the requirements for terminal attitude constraints. Summary of the Invention

[0004] The purpose of this invention is to address the issues that existing DWA algorithms cannot meet the real-time response requirements of UUV online path planning in marine environments and that terminal attitude constraints are not considered in online planning tasks. Therefore, this invention proposes an online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm.

[0005] The technical solution adopted by this invention to solve the above-mentioned technical problems is: an online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm, the method specifically including the following steps:

[0006] Step 1: Establish the kinematic model of the underactuated UUV;

[0007] Step 2: Generate the velocity sampling space of UUV;

[0008] Step 3: Obtain the gravitational and repulsive forces of the UUV at its actual location based on the improved APF algorithm;

[0009] Step 4: Calculate the terminal heading of the UUV, then optimize the gravitational direction based on the terminal heading, and calculate the resultant angular velocity based on the repulsive force and the optimized gravitational force.

[0010] Step 5: Adjust the velocity sampling space according to the resultant angular velocity to obtain the adjusted velocity sampling space;

[0011] Step 6: Within the adjusted velocity sampling space, uniformly sample the sway velocity, pitch angular velocity, and heading angular velocity at different intervals, and then combine the sampled velocities. Based on the kinematic model established in step 1, the simulation trajectory prediction period is calculated. Trajectory within;

[0012] Then, the predicted trajectories are evaluated according to the designed evaluation function. The UUV is controlled based on the speed group corresponding to the selected optimal trajectory. In the next planning cycle, the execution of step 2 is returned based on the updated environmental information until the destination is reached.

[0013] Furthermore, the kinematic model of the underactuated UUV is as follows:

[0014]

[0015] in, This indicates the oscillation velocity of the UUV. This indicates the pitch angle of the UUV. Indicates the heading angle of the UUV. This indicates the pitch angular velocity of the UUV. This indicates the bow angular velocity of the UUV. Representing UUV in a spatial rectangular coordinate system Axial coordinates, Representing UUV in a spatial rectangular coordinate system Axial coordinates, Representing UUV in a spatial rectangular coordinate system Axial coordinates, express The first derivative, express The first derivative, express The first derivative, express The first derivative, express The first derivative.

[0016] Furthermore, the specific process of step 2 is as follows:

[0017] Step 2.1, Boundary Constraint Space for:

[0018]

[0019] in, The maximum oscillation velocity of the UUV. The minimum sway velocity of a UUV. This represents the maximum pitch angular velocity of the UUV. The minimum pitch angular velocity of the UUV. The maximum heading angular velocity of the UUV. This represents the minimum heading angular velocity of the UUV.

[0020] Step 2.2, Acceleration-limited space for:

[0021]

[0022] in, This refers to the actual UUV oscillation speed. This represents the actual pitch angular velocity of the UUV. This represents the actual bow angular velocity of the UUV. The maximum sway deceleration of the UUV The maximum oscillation acceleration of the UUV. The sampling time interval, This represents the deceleration at the maximum pitch angle of the UUV. This represents the maximum pitch acceleration of the UUV. This represents the maximum heading angular deceleration of the UUV. This represents the maximum heading angular acceleration of the UUV.

[0023] Step 2.3, Obstacles restrict space for:

[0024]

[0025] in, This indicates the closest distance between the predicted trajectory end and the obstacle.

[0026] Step 2.4, Pitch Angle Restriction Space for:

[0027]

[0028] in, Indicates the minimum pitch angle. Indicates the maximum pitch angle. Indicates the period for predicting the combined force. Indicates the actual pitch angle;

[0029] Step 2.5, UUV velocity sampling space for:

[0030] .

[0031] Furthermore, the specific process of step 3 is as follows:

[0032] Step 3.1: Record the current actual location of the UUV as... , and the finish line The gravitational potential field between them is:

[0033]

[0034] in, This is the gravitational gain coefficient. For actual location and the finish line Vector distance between them Indicates actual location and the finish line The gravitational potential field between them;

[0035] Step 3.2, Obstacle Location With actual location The repulsive potential field between them is:

[0036]

[0037] in, The repulsive gain coefficient is... For actual location and obstacles Vector distance between boundaries The maximum distance at which an obstacle affects a UUV. Indicates the location of the obstacle With actual location The repulsive potential field between them;

[0038] Step 3.3: Calculate the actual location of the UUV based on the gravitational potential field. The gravitational force is used to calculate the actual position of the UUV based on the repulsive potential field. The repulsive force.

[0039] Furthermore, the specific process of step 3.3 is as follows:

[0040]

[0041] in, Represents the gravitational potential field The negative gradient, Represents a repulsive potential field The negative gradient, Indicates the actual location of the UUV Gravity, Indicates the actual location of the UUV The repulsive force.

[0042] Furthermore, the specific process of step 4 is as follows:

[0043] Step 4.1: Based on the direction vector of the endpoint position and the final attitude direction vector Calculate the virtual direction vector :

[0044]

[0045] in, Represents a virtual direction vector. For location weighting coefficients, These are the attitude weighting coefficients;

[0046] Location weight coefficient and attitude weight coefficients The calculation processes are as follows:

[0047]

[0048]

[0049] in, This represents the distance from the actual location of the UUV to its destination. , , and All of these are distance threshold parameters;

[0050] Step 4.2: When the virtual direction deviates from the direction of the endpoint position by an angle greater than or equal to 90 degrees, the projection of the virtual direction onto the normal plane of the endpoint position direction is taken as the final virtual direction. :

[0051] in, Indicates the direction of the endpoint. With virtual direction The included angle, Indicates the final virtual direction;

[0052] The actual terminal heading for:

[0053]

[0054] in, Indicates the direction of the endpoint. With the final virtual direction The cosine of the included angle;

[0055] Step 4.3: Set the actual location Terminal heading As the optimized direction of gravity, the optimized actual position Gravity at the location for:

[0056]

[0057] in, Represents the optimized gravity Represents gravity Size;

[0058] Step 4.4: Calculate the improved resultant force attitude angle. reuse Calculate the resultant angular velocity.

[0059] Furthermore, the specific process of step 4.4 is as follows:

[0060]

[0061] in, The pitch angle, representing the direction of gravity, The heading angle, which indicates the direction of gravity. The pitch angle represents the direction of the repulsive force. The heading angle, indicating the direction of the repulsive force. The pitch angle represents the direction of the resultant force at the actual location. The heading angle, representing the direction of the resultant force at the actual position. This indicates the calculation of the L2 norm;

[0062] Then the resultant angular velocity for:

[0063]

[0064] in, This represents the actual pitch angle. This represents the actual heading angle. This represents the longitudinal angular velocity of the resultant force. This represents the forward angular velocity of the resultant force.

[0065] Furthermore, the specific process of step 5 is as follows:

[0066] Step 5.1, Limitation of Resultant Angular Velocity for:

[0067]

[0068] in, This represents the minimum pitch angular velocity under the constraint of the resultant force angular velocity. This represents the maximum pitch angular velocity under the constraint of the resultant force angular velocity. This represents the minimum heading angular velocity under the constraint of the resultant force angular velocity. This represents the maximum heading angular velocity under the constraint of the resultant force angular velocity.

[0069] Ignoring collision limitations, the velocity sampling space under velocity boundary constraints, acceleration constraints, and pitch angle constraints. for:

[0070]

[0071]

[0072] in, Represents the sampling space The minimum oscillation velocity, Represents the sampling space Maximum oscillation speed, Represents the sampling space The minimum pitch angular velocity, Represents the sampling space Maximum pitch angular velocity, Represents the sampling space The minimum heading angular velocity, Represents the sampling space Maximum heading angular velocity;

[0073] Resultant angular velocity limit The resultant angular velocity and the velocity sampling space without considering collision limitations The degree of difference was obtained as follows:

[0074]

[0075] in, To limit the minimum linear correlation coefficient by the resultant force, To limit the rate of spatial expansion through the combined force;

[0076] Step 5.2, Adjusted velocity sampling space guided by resultant force .

[0077] Furthermore, the evaluation function design method for the predicted trajectory is as follows:

[0078] Step 6.1: Design the terminal heading angle evaluation function

[0079]

[0080] in, For the predicted trajectory within the heading prediction period Location at any given moment express The expected direction at that location for The terminal's attitude and orientation at that location;

[0081] That is, according to Obtain the desired pitch angle and expected heading angle The terminal heading angle evaluation function is:

[0082]

[0083] in, This represents the terminal heading angle evaluation function. Indicates the desired pitch angle of the terminal heading. The desired heading angle representing the terminal heading. Indicates the predicted trajectory The longitudinal tilt angle along the tangent direction. Indicates the predicted trajectory The heading angle in the tangential direction, Indicates taking the absolute value;

[0084] Step 6.2: Design the resultant force heading angle evaluation function as follows:

[0085]

[0086] in, This represents the resultant force heading angle evaluation function;

[0087] Step 6.3: Record the shortest distance from the predicted trajectory to the obstacle as... The difference between the sampled linear velocity and the desired linear velocity is denoted as... And then according to , , and Calculate the comprehensive evaluation function value:

[0088]

[0089] in, , , and These are the coefficients of each evaluation function. These are adaptive coefficients.

[0090] Furthermore, the adaptive coefficient Specifically:

[0091]

[0092] in, Represents the longitudinal tilt velocity of the resultant force Compared with the actual pitch angular velocity The difference, Represents the forward angular velocity of the resultant force. Compared with actual bow angular velocity The difference.

[0093] The beneficial effects of this invention are:

[0094] 1. This invention dynamically adjusts the velocity sampling space of the DWA algorithm based on the direction of the resultant force. The higher the correlation between the resultant force angular velocity and the velocity sampling space, the stronger the adjustment effect. By narrowing the velocity window, the number of velocity samples can be reduced, effectively improving the real-time performance of the algorithm.

[0095] 2. This invention improves the correlation between the APF algorithm and the DWA algorithm by using the resultant heading angle evaluation function, ensuring the stability of the resultant force adjustment effect. In particular, for obstacle avoidance processes where the resultant force direction is constantly changing, it can effectively guide the UUV to sail in the direction of the resultant force.

[0096] 3. This invention designs a terminal heading, which is used as the desired heading of the DWA algorithm and the gravitational direction of the APF algorithm, so that the UUV can align its attitude as it approaches the target, effectively solving the terminal attitude constraint problem. Attached Figure Description

[0097] Figure 1This is a flowchart of an online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm of the present invention;

[0098] Figure 2 This is a schematic diagram of the velocity sampling space of the three-dimensional DWA algorithm;

[0099] Figure 3 This is a schematic diagram of the terminal heading calculation process;

[0100] Figure 4 This is a schematic diagram of the angular velocity sampling space when the resultant angular velocity and the original velocity space have no deviation;

[0101] Figure 5 This is a schematic diagram of the angular velocity sampling space when the spatial deviation between the resultant angular velocity and the original velocity is small;

[0102] Figure 6 This is a schematic diagram of the angular velocity sampling space when the spatial deviation between the resultant angular velocity and the original velocity is large;

[0103] Figure 7 The results are simulation experiments of the traditional 3D DWA algorithm in a single large obstacle scenario (corresponding to scenario 1).

[0104] Figure 8 The results are simulations of the APF-DWA algorithm at the terminal location in a single large obstacle scenario.

[0105] Figure 9 The results are simulation results of the APF-DWA algorithm (i.e., the method of this invention) considering terminal attitude constraints in a single large obstacle scenario.

[0106] Figure 10 The results are simulation experiments of the traditional 3D DWA algorithm in a complex multi-obstacle scenario (corresponding to scenario 2).

[0107] Figure 11 These are simulation results of the APF-DWA algorithm for the terminal location only in complex, multi-obstacle scenarios.

[0108] Figure 12 The results are simulation results of the APF-DWA algorithm (i.e., the method of this invention) considering terminal attitude constraints in complex multi-obstacle scenarios. Detailed Implementation

[0109] This invention addresses the terminal attitude constraint and planning efficiency issues of UUVs by designing an APF-DWA fusion algorithm. It utilizes the resultant force direction of an improved APF algorithm to guide the velocity sampling process of the DWA algorithm, achieving dynamic adjustment of the velocity space and significantly improving the real-time performance of the DWA algorithm. Furthermore, a terminal heading calculation method is designed, using it as the gravitational direction of the APF algorithm and the desired heading of the DWA algorithm, enabling the UUV to align its attitude before reaching its destination.

[0110] The method of the present invention will be further described in detail below with reference to the accompanying drawings:

[0111] Specific Implementation Method 1: Combination Figure 1 This embodiment describes an online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm. The method specifically includes the following steps:

[0112] Step 1: Establish the kinematic model of the underactuated UUV;

[0113] Specifically, assuming the sway velocity of the UUV heave speed Tilt angle and yaw rate All are zero, the volume coordinate system is from the fixed coordinate system (i.e., the three-dimensional rectangular coordinate system) along the heading angle. Pitch angle The sequence of rotations is repeated twice to achieve overlap, and the control input is the oscillation speed. Pitch angular velocity angular velocity of the bow Composition, i.e., state vector velocity vector The kinematic model of the underactuated UUV is as follows:

[0114]

[0115] in, This indicates the oscillation velocity of the UUV. This indicates the pitch angle of the UUV. Indicates the heading angle of the UUV. This indicates the pitch angular velocity of the UUV. This indicates the bow angular velocity of the UUV. Representing UUV in a spatial rectangular coordinate system Axial coordinates, Representing UUV in a spatial rectangular coordinate system Axial coordinates, Representing UUV in a spatial rectangular coordinate system Axial coordinates, express The first derivative, express The first derivative, express The first derivative, express The first derivative, express The first derivative.

[0116] Step 2, as follows Figure 2 As shown, the speed of UUVs is usually subject to speed boundary constraints, acceleration constraints, and obstacle constraints. Based on the above three constraints, this invention also designs a pitch angle constraint to address the pitch angle constraint of UUVs in three-dimensional space. That is, this invention considers speed boundary constraints, acceleration constraints, obstacle constraints, and pitch angle constraints to generate a speed sampling space for UUVs.

[0117] Specifically,

[0118] Step 2.1: At the kinematic level, due to mechanical or design limitations, the linear and angular velocities that a UUV can achieve are within a range of maximum and minimum values, meaning there is a velocity boundary limitation space. for:

[0119]

[0120] in, The maximum oscillation velocity of the UUV. The minimum sway velocity of a UUV. This represents the maximum pitch angular velocity of the UUV. The minimum pitch angular velocity of the UUV. The maximum heading angular velocity of the UUV. This represents the minimum heading angular velocity of the UUV.

[0121] Step 2.2: At the dynamic level, due to the performance constraints of the thruster, both linear velocity and angular velocity acceleration are limited. Furthermore, within a certain time interval, velocity changes with acceleration, meaning there is a space for acceleration limitations. for:

[0122]

[0123] in, This refers to the actual UUV oscillation speed. This represents the actual pitch angular velocity of the UUV. This represents the actual bow angular velocity of the UUV. The maximum sway deceleration of the UUV The maximum oscillation acceleration of the UUV. The sampling time interval, i.e., the acceleration period, This represents the deceleration at the maximum pitch angle of the UUV. This represents the maximum pitch acceleration of the UUV. This represents the maximum heading angular deceleration of the UUV. This represents the maximum heading angular acceleration of the UUV.

[0124] Step 2.3: Considering the safety of the UUV, under the condition of maximum braking speed, the UUV must not collide with surrounding obstacles, i.e., there is an obstacle-limited space. for:

[0125]

[0126] in, This indicates the closest distance between the predicted trajectory end and the obstacle.

[0127] Step 2.4: Due to the singularity of Euler angles, UUVs are subject to pitch angle constraints. Furthermore, excessive pitch angles threaten the stability and safety of UUVs, necessitating pitch angle limiting to adjust the resultant force prediction period. Afterwards, the UUV's pitch angle is at its minimum pitch angle. and maximum pitch angle Between these, there exists a space constrained by the pitch angle. for:

[0128]

[0129] in, Indicates the minimum pitch angle. Indicates the maximum pitch angle. Indicates the period for predicting the combined force. Indicates the actual pitch angle;

[0130] Step 2.5, UUV velocity sampling space for:

[0131]

[0132] Step 3: Obtain the gravitational and repulsive forces of the UUV at its actual location based on the improved APF algorithm;

[0133] Specifically, to address the target unreachability problem in the APF algorithm, an adjustment factor is added to the repulsive field. To address the local minima problem inherent in the APF algorithm, which only guides the course without adjusting speed, the UUV can navigate away from the extreme point under the course and speed evaluations of the DWA algorithm.

[0134] Step 3.1: Record the current actual location of the UUV as... , and the finish line The gravitational potential field between them is:

[0135]

[0136] in, This is the gravitational gain coefficient. For actual location and the finish line The vector distance between them, the direction of which is... , Indicates actual location and the finish line The gravitational potential field between them;

[0137] Step 3.2, Obstacle Location With actual location The repulsive potential field between them is:

[0138]

[0139] in, The repulsive gain coefficient is... For actual location and obstacles Vector distance between boundaries, direction pointing , The maximum distance at which an obstacle affects a UUV. Indicates the location of the obstacle With actual location The repulsive potential field between them;

[0140] Step 3.3: Calculate the actual location of the UUV based on the gravitational potential field. The gravitational force is used to calculate the actual position of the UUV based on the repulsive potential field. Repulsive force:

[0141]

[0142] in, Represents the gravitational potential field The negative gradient, Represents a repulsive potential field The negative gradient, Indicates the actual location of the UUV Gravity, Indicates the actual location of the UUV The repulsive force.

[0143] Step 4: Design a terminal heading calculation method for the terminal attitude constraint problem, calculate the terminal heading of the UUV, optimize the gravitational direction based on the terminal heading, and calculate the resultant angular velocity based on the repulsive force and the gravitational force after the direction optimization.

[0144] Specifically,

[0145] Step 4.1: Based on the direction vector of the endpoint position (i.e., the direction from the actual position to the endpoint position). and the final attitude direction vector (desired attitude direction) Calculate the virtual direction vector :

[0146]

[0147] in, Represents a virtual direction vector. For location weighting coefficients, These are the attitude weighting coefficients;

[0148] Virtual direction vector Depend on and The terminal heading is obtained by weighted average. With virtual direction vector Regarding the position direction vector Symmetry, such as Figure 3 As shown. It should also be noted that the direction vectors in this invention are all unit vectors, and the vector synthesis is carried out by synthesizing them separately according to the attitude angle. The specific synthesis method is explained in step 4.4.

[0149] Location weight coefficient and attitude weight coefficients The calculation processes are as follows:

[0150]

[0151]

[0152] in, This represents the distance from the actual location of the UUV to its destination. , , and All of these are distance threshold parameters;

[0153] Step 4.2: To prevent the UUV from moving away from the endpoint due to attitude adjustment, the virtual direction needs to be restricted. When the virtual direction deviates from the endpoint position direction by an angle greater than or equal to 90 degrees, the projection of the virtual direction onto the normal plane of the endpoint position direction is taken as the final virtual direction. :

[0154] in, Indicates the direction of the endpoint. With virtual direction The included angle, Indicates the final virtual direction;

[0155] The actual terminal heading for:

[0156]

[0157] in, Indicates the direction of the endpoint. With the final virtual direction The cosine of the included angle;

[0158] Step 4.3: In the traditional APF algorithm, the direction of gravity points to the endpoint. To meet the terminal attitude constraints, the direction of gravity needs to be optimized, but the magnitude of gravity remains unchanged, i.e., the actual position... Terminal heading As the optimized direction of gravity, the optimized actual position Gravity at the location for:

[0159]

[0160] in, Represents the optimized gravity Represents gravity Size;

[0161] Step 4.4: Calculate the improved resultant force attitude angle based on the method of synthesizing the attitude angles in the horizontal and vertical planes respectively. This allows us to obtain the actual location of the UUV. Direction of the resultant force at the point ;

[0162] The pitch angle constraint requires the UUV to sail with a small pitch attitude, while the bow attitude can reach any angle under the drive of the rudder and main thruster. When the horizontal component of the three-dimensional vector synthesis becomes smaller due to cancellation, the vertical component will dominate the synthesis direction, producing an angle much larger than the vertical component vectors, resulting in an abnormal pitch angle. This causes the UUV to experience violent pitch motion, or even fail and become unable to respond to pitch commands.

[0163] To avoid this problem, the magnitude of the force is used as a weight and synthesized according to the attitude angle. The pitch angle and heading angle of the attraction and repulsion forces are synthesized separately to ensure the stability of the resultant force vector.

[0164] Specifically,

[0165]

[0166] in, The pitch angle, representing the direction of gravity, The heading angle, which indicates the direction of gravity. The pitch angle represents the direction of the repulsive force. The heading angle, indicating the direction of the repulsive force. The pitch angle represents the direction of the resultant force at the actual location. The heading angle, representing the direction of the resultant force at the actual position. This indicates the calculation of the L2 norm;

[0167] The following is a simple explanation of angle subtraction, for the heading angle:

[0168]

[0169] In addition, the direction of the resultant force needs to satisfy the pitch angle constraint. This will exceed the angular boundary limit. Set as a boundary value for use in subsequent calculations.

[0170] When the constrained resultant force attitude angle is obtained Then, the direction of the resultant force can be calculated. :

[0171]

[0172] Assuming a set of constant angular velocities Under the control input, the UUV is in the resultant force prediction cycle. The subsequent course and Direction of the resultant force at the point Consistent, this set of angular velocities is defined as the resultant force angular velocity; therefore, the resultant force angular velocity can be calculated:

[0173]

[0174] in, This represents the actual pitch angle. This represents the actual heading angle. This represents the longitudinal angular velocity of the resultant force. This represents the forward angular velocity of the resultant force.

[0175] Step 5: Adjust the velocity sampling space according to the resultant angular velocity to obtain the adjusted velocity sampling space;

[0176] Specifically, such as Figure 4 , Figure 5 and Figure 6 As shown, the resultant force angular velocity is limited in space. The area indicated by the dashed line represents the original velocity sampling space. For the solid line region, the intersection of the resultant force angular velocity limitation space, the original velocity sampling space, and the obstacle limitation space is taken as the adjusted velocity sampling space. In the region indicated by the diagonal lines in the diagram, the greater the spatial difference between the resultant angular velocity and the original velocity, the better. The larger the range, the better.

[0177] Step 5.1: Use the resultant force angular velocity as the desired angular velocity to guide the sampling process of the DWA algorithm, i.e., the resultant force angular velocity constraint. for:

[0178]

[0179] in, This represents the minimum pitch angular velocity under the constraint of the resultant force angular velocity. This represents the maximum pitch angular velocity under the constraint of the resultant force angular velocity. This represents the minimum heading angular velocity under the constraint of the resultant force angular velocity. This represents the maximum heading angular velocity under the constraint of the resultant force angular velocity.

[0180] Ignoring collision limitations, the velocity sampling space under velocity boundary constraints, acceleration constraints, and pitch angle constraints. for:

[0181]

[0182]

[0183] in, Represents the sampling space The minimum oscillation velocity, Represents the sampling space Maximum oscillation speed, Represents the sampling space The minimum pitch angular velocity, Represents the sampling space Maximum pitch angular velocity, Represents the sampling space The minimum heading angular velocity, Represents the sampling space Maximum heading angular velocity;

[0184] Resultant angular velocity limit The resultant angular velocity and the velocity sampling space without considering collision limitations The degree of difference was obtained as follows:

[0185]

[0186] in, To limit the minimum linear correlation coefficient by the resultant force, To limit the rate of spatial expansion, ;

[0187] Step 5.2, Adjusted velocity sampling space guided by resultant force .

[0188] Step 6: Within the adjusted velocity sampling space, uniformly sample the sway velocity, pitch angular velocity, and heading angular velocity at different intervals (resolution), and then combine the sampled velocities. Based on the kinematic model established in step 1, the simulation trajectory prediction period is calculated. Trajectory within;

[0189] It should be noted that this invention refers to the trajectory prediction period. With heading prediction period By decoupling, the period can be predicted based on the kinematic model. The trajectory within is used for collision avoidance, and the cycle is recorded. The position and direction after that are used for heading assessment, where By extending This allows for the prediction of longer trajectories, improving the algorithm's security; by shortening... This can prevent the UUV from losing its ability to distinguish the course due to excessive changes in course, and thus prevent the UUV from performing incorrect turning maneuvers and producing a detour due to sudden changes in the optimal course.

[0190] Then, the predicted trajectories are evaluated according to the designed evaluation function. The UUV is controlled based on the speed group corresponding to the selected optimal trajectory. In the next planning cycle, the execution of step 2 is returned based on the updated environmental information until the destination is reached.

[0191] Specifically, the evaluation function design method for the predicted trajectory is as follows:

[0192] Step 6.1: Design the terminal heading angle evaluation function

[0193]

[0194] in, For the predicted trajectory within the heading prediction period Location at any given moment express The expected direction at that location for The terminal attitude direction at the location (the calculation method is the same as in steps 4.1 and 4.2).

[0195] The traditional heading angle is decomposed into bow angle and pitch angle to evaluate the UUV attitude, i.e., based on... Obtain the desired pitch angle and expected heading angle The terminal heading angle evaluation function is:

[0196]

[0197] in, This represents the terminal heading angle evaluation function. Indicates the desired pitch angle of the terminal heading. The desired heading angle representing the terminal heading. Indicates the predicted trajectory The longitudinal tilt angle along the tangent direction. Indicates the predicted trajectory The heading angle in the tangential direction, Indicates taking the absolute value;

[0198] Step 6.2: The stronger the consistency between the resultant force direction at the actual position and the original velocity sampling space, the stronger the ability of the resultant force constraint to adjust the velocity sampling space, and the higher the planning efficiency. To measure the degree of deviation between the predicted heading and the resultant force direction, and to improve the correlation between the APF algorithm and the DWA algorithm, the resultant force heading angle evaluation function is designed as follows:

[0199]

[0200] in, This represents the resultant force heading angle evaluation function;

[0201] However, the APF algorithm suffers from local extrema, especially in complex situations with multiple obstacles. Dramatic changes in the resultant force direction cause abrupt changes in the resultant force angular velocity, failing to provide effective guidance for the DWA algorithm and even causing it to fall into local optima. To address this, an adaptive weight adjustment strategy is proposed based on the deviation between the resultant force angular velocity and the actual angular velocity. When the error is too large, the confidence level in the resultant force direction is reduced.

[0202] Adaptive coefficient of the weight of the resultant heading angle evaluation Specifically, it is expressed as follows:

[0203]

[0204] in, Represents the longitudinal tilt velocity of the resultant force Compared with the actual pitch angular velocity The difference, Represents the forward angular velocity of the resultant force. Compared with actual bow angular velocity The difference;

[0205] Step 6.3: Record the shortest distance from the predicted trajectory to the obstacle as... The difference between the sampled linear velocity and the desired linear velocity is denoted as... And then according to , , and Calculate the comprehensive evaluation function value:

[0206]

[0207] in, , , and These are the coefficients of each evaluation function. The adaptive coefficient is used to avoid the APF-DWA algorithm from getting trapped in local optima while still achieving efficient planning.

[0208] It should be noted that before substituting the four evaluation functions into the above formula, it is necessary to first... , , and After normalization, the normalized evaluation function values ​​are substituted into the above formula to calculate the comprehensive evaluation function value.

[0209] Experimental Section

[0210] To demonstrate the effectiveness of the method of this invention, comparative simulation experiments were conducted using the traditional 3D DWA algorithm, the APF-DWA algorithm with resultant force-guided DWA velocity sampling, and the method of this invention (combining terminal attitude and resultant force-guided DWA velocity sampling). The trajectory prediction period was among the parameters discussed. The heading prediction period is 10 seconds. The resultant force prediction period is 5 seconds. The sampling interval is 3 seconds. For 1 second, , , , The simulation environment uses a northeast-northeast coordinate system, with depth indicated downwards.

[0211] The simulation data is shown in Table 1:

[0212] Table 1. Comparison of path planning performance of the three algorithms in different environments.

[0213]

[0214] Furthermore, scenario 1 provides the following... Figures 7 to 9 The simulation results show that, compared with the traditional 3D DWA algorithm, the method of this invention effectively improves the real-time performance of the algorithm. The pitch angle error between the arrival attitude of the UUV and the expected terminal attitude is within ±2°, and the heading error is within ±5°, which can meet the attitude requirements of conventional tasks. Scenario 2 provides... Figures 10 to 12The simulation results show that the APF-DWA algorithm leaves the resultant force equilibrium point under the action of DWA. By reducing the evaluation weight of the resultant force heading angle, it can solve the local optimum problem to a certain extent.

[0215] The above examples of the present invention are merely illustrative of the computational model and process of the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is impossible to exhaustively list all possible implementations here. Any obvious variations or modifications derived from the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. An online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm, characterized in that, The method specifically includes the following steps: Step 1: Establish the kinematic model of the underactuated UUV; Step 2: Generate the velocity sampling space of UUV; Step 3: Obtain the gravitational and repulsive forces of the UUV at its actual location based on the improved APF algorithm; Step 4: Calculate the terminal heading of the UUV, then optimize the gravitational direction based on the terminal heading, and calculate the resultant angular velocity based on the repulsive force and the optimized gravitational force. The specific process of step 4 is as follows: Step 4.1: Based on the direction vector of the endpoint position and the final attitude direction vector Calculate the virtual direction vector : in, Represents a virtual direction vector. For location weighting coefficients, These are the attitude weighting coefficients; Location weight coefficient and attitude weight coefficients The calculation processes are as follows: in, This represents the distance from the actual location of the UUV to its destination. , , and All of these are distance threshold parameters; Step 4.2: When the virtual direction deviates from the direction of the endpoint position by an angle greater than or equal to 90 degrees, the projection of the virtual direction onto the normal plane of the endpoint position direction is taken as the final virtual direction. : in, Indicates the direction of the endpoint. With virtual direction The included angle, Indicates the final virtual direction; The actual terminal heading for: in, Indicates the direction of the endpoint. With the final virtual direction The cosine of the included angle; Step 4.3: Set the actual location Terminal heading As the optimized direction of gravity, the optimized actual position Gravity at the location for: in, This represents the optimized gravity. Represents gravity Size; Step 4.4: Calculate the improved resultant force attitude angle. reuse Calculate the resultant angular velocity; Step 5: Adjust the velocity sampling space according to the resultant angular velocity to obtain the adjusted velocity sampling space; Step 6: Within the adjusted velocity sampling space, uniformly sample the sway velocity, pitch angular velocity, and heading angular velocity at different intervals, and then combine the sampled velocities. Based on the kinematic model established in step 1, the simulation trajectory prediction period is calculated. Trajectory within; Then, the predicted trajectories are evaluated according to the designed evaluation function. The UUV is controlled based on the speed group corresponding to the selected optimal trajectory. In the next planning cycle, the execution of step 2 is returned according to the updated environmental information until the destination is reached.

2. The online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm according to claim 1, characterized in that, The kinematic model of the underactuated UUV is as follows: in, Indicates the oscillation velocity of the UUV. This indicates the pitch angle of the UUV. Indicates the heading angle of the UUV. This indicates the pitch angular velocity of the UUV. This indicates the bow angular velocity of the UUV. Representing UUV in a spatial rectangular coordinate system Axial coordinates, Representing UUV in a spatial rectangular coordinate system Axial coordinates, Representing UUV in a spatial rectangular coordinate system Axial coordinates, express The first derivative, express The first derivative, express The first derivative, express The first derivative, express The first derivative.

3. The online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm according to claim 2, characterized in that, The specific process of step 2 is as follows: Step 2.1, Boundary Constraint Space for: in, The maximum oscillation velocity of the UUV. The minimum sway velocity of a UUV. This represents the maximum pitch angular velocity of the UUV. The minimum pitch angular velocity of the UUV. The maximum heading angular velocity of the UUV. This represents the minimum heading angular velocity of the UUV. Step 2.2, Acceleration-limited space for: in, This refers to the actual UUV oscillation speed. This represents the actual pitch angular velocity of the UUV. This represents the actual bow angular velocity of the UUV. The maximum sway deceleration of the UUV The maximum oscillation acceleration of the UUV. The sampling time interval, This represents the deceleration at the maximum pitch angle of the UUV. This represents the maximum pitch acceleration of the UUV. This represents the maximum heading angular deceleration of the UUV. This represents the maximum heading angular acceleration of the UUV. Step 2.3, Obstacles restrict space for: in, This indicates the closest distance between the predicted trajectory end and the obstacle; Step 2.4, Pitch Angle Restriction Space for: in, Indicates the minimum pitch angle. Indicates the maximum pitch angle. Indicates the period for predicting the combined force. Indicates the actual pitch angle; Step 2.5, UUV velocity sampling space for: 。 4. The online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm according to claim 3, characterized in that, The specific process of step 3 is as follows: Step 3.1: Record the current actual location of the UUV as... , and the finish line The gravitational potential field between them is: in, This is the gravitational gain coefficient. For actual location and the finish line Vector distance between them Indicates actual location and the finish line The gravitational potential field between them; Step 3.2, Obstacle Location With actual location The repulsive potential field between them is: in, The repulsive force gain coefficient, For actual location and obstacles Vector distance between boundaries The maximum distance at which an obstacle affects a UUV. Indicates the location of the obstacle With actual location The repulsive potential field between them; Step 3.3: Calculate the actual location of the UUV based on the gravitational potential field. The gravitational force is used to calculate the actual position of the UUV based on the repulsive potential field. The repulsive force.

5. The online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm according to claim 4, characterized in that, The specific process of step 3.3 is as follows: in, Represents the gravitational potential field The negative gradient, Represents a repulsive potential field The negative gradient, Indicates the actual location of the UUV Gravity, Indicates the actual location of the UUV The repulsive force.

6. The online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm according to claim 5, characterized in that, The specific process of step 4.4 is as follows: in, The pitch angle, representing the direction of gravity. The heading angle, which indicates the direction of gravity. The pitch angle represents the direction of the repulsive force. The heading angle, indicating the direction of the repulsive force. The pitch angle represents the direction of the resultant force at the actual location. The heading angle, representing the direction of the resultant force at the actual position. This indicates the calculation of the L2 norm; Then the resultant angular velocity for: in, This represents the actual pitch angle. This represents the actual heading angle. This represents the longitudinal angular velocity of the resultant force. This represents the forward angular velocity of the resultant force.

7. The online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm according to claim 6, characterized in that, The specific process of step 5 is as follows: Step 5.1, Limitation of Resultant Force Angular Velocity for: in, This represents the minimum pitch angular velocity under the constraint of the resultant force angular velocity. This represents the maximum pitch angular velocity under the constraint of the resultant force angular velocity. This represents the minimum heading angular velocity under the constraint of the resultant force angular velocity. This represents the maximum heading angular velocity under the constraint of the resultant force angular velocity. Ignoring collision limitations, the velocity sampling space under velocity boundary constraints, acceleration constraints, and pitch angle constraints. for: in, Represents the sampling space The minimum oscillation velocity, Represents the sampling space Maximum oscillation speed, Represents the sampling space The minimum pitch angular velocity, Represents the sampling space Maximum pitch angular velocity, Represents the sampling space The minimum heading angular velocity, Represents the sampling space Maximum heading angular velocity; Resultant angular velocity limit The resultant angular velocity and the velocity sampling space without considering collision limitations The degree of difference was obtained as follows: in, To limit the minimum linear correlation coefficient by the resultant force, To limit the rate of spatial expansion through the combined force; Step 5.2, Adjusted velocity sampling space guided by resultant force .

8. The online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm according to claim 7, characterized in that, The evaluation function design method for the predicted trajectory is as follows: Step 6.1: Design the terminal heading angle evaluation function in, For the predicted trajectory within the heading prediction period Location at any given moment express The expected direction at that location for The terminal's attitude and orientation at that location; That is, according to Obtain the desired pitch angle and expected heading angle The terminal heading angle evaluation function is: in, This represents the terminal heading angle evaluation function. Indicates the desired pitch angle of the terminal heading. The desired heading angle representing the terminal heading. Indicates the predicted trajectory The longitudinal tilt angle along the tangent direction. Indicates the predicted trajectory The heading angle in the tangential direction, Indicates taking the absolute value; Step 6.2, Design the resultant force heading angle evaluation function as follows: in, This represents the resultant force heading angle evaluation function; Step 6.3: Record the shortest distance from the predicted trajectory to the obstacle as... The difference between the sampled linear velocity and the desired linear velocity is denoted as... And then according to , , and Calculate the comprehensive evaluation function value: in, , , and These are the coefficients of each evaluation function. These are adaptive coefficients.

9. The online path planning method for unmanned underwater vehicles based on the APF-DWA fusion algorithm according to claim 8, characterized in that, The adaptive coefficient Specifically: in, Represents the longitudinal tilt velocity of the resultant force Compared with the actual pitch angular velocity The difference, Represents the forward angular velocity of the resultant force. Compared with actual bow angular velocity The difference.

Images