Autonomous underwater vehicle flexible formation navigation obstacle avoidance method and related equipment

Abstract

The application discloses a kind of autonomous underwater vehicle flexible formation navigation obstacle avoidance method and related equipment, method includes: obtaining the state information and target point of each vehicle, vehicle includes navigation vehicle and following vehicle, target point is as the target reference point of navigation vehicle, according to navigation vehicle state information and the expected offset of following vehicle relative to navigation vehicle, generate the virtual reference point of following vehicle, and then generate the initial guide vector field of navigation vehicle and following vehicle, adjust initial guide vector field based on barrier, obtain the reference movement direction after obstacle avoidance;According to current state information and the reference movement direction after obstacle avoidance generate the candidate control sequence of each vehicle, and then determine the current optimal control instruction of each vehicle, repeat the above steps until each vehicle reaches the end point.The application realizes local obstacle avoidance while maintaining global target orientation, and realizes the flexible deformation of formation, and can be widely applied in navigation obstacle avoidance technical field.

Description

Technical Field

[0001] This application relates to the field of navigation and obstacle avoidance technology, and in particular to a method and related equipment for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles. Background Technology

[0002] Autonomous underwater vehicles (AUVs) are widely used in marine scientific research, underwater surveillance, target detection, seabed surveying, and underwater infrastructure inspection. As mission complexity increases, the limitations of individual AUVs in terms of operational efficiency, coverage, and environmental adaptability have led to the development of multi-AUV swarm cooperative navigation as a crucial area of ​​advancement. In complex three-dimensional underwater environments, multi-AUV swarms not only need to converge as a whole towards the target area but also need to avoid static and dynamic obstacles and complex seabed topography in real time, while preventing collisions between swarm members. Furthermore, the nonlinear, underactuated, and actuator-constrained characteristics of AUVs necessitate navigation methods that balance global guidance, local obstacle avoidance, dynamic feasibility, and swarm cooperative stability.

[0003] However, existing multi-AUV formation navigation and obstacle avoidance methods have the following shortcomings: On the one hand, existing predictive optimization methods, such as model predictive control and model predictive path integral control, can predict trajectories and optimize control within a limited time domain, but they are essentially local optimization methods and lack global guidance capabilities from the starting area to the target area. In environments with dense obstacles, non-convex obstacles, or complex three-dimensional terrain, they are prone to problems such as low detour efficiency, local trajectory oscillations, or even deviation from the mission objective. On the other hand, most of them emphasize rigid formation maintenance. In narrow passages, dense obstacles, or complex seabed terrain, the formation shape lacks the ability to deform as needed, making it difficult to take into account collision avoidance within the formation during obstacle avoidance, and it is difficult to adaptively restore the desired formation after passing through the obstacle area. Summary of the Invention

[0004] The main objective of this application is to propose a flexible formation navigation and obstacle avoidance method and related equipment for autonomous underwater vehicles, which can achieve local obstacle avoidance while maintaining global target orientation, effectively avoiding problems such as low detour efficiency, local trajectory oscillation, or even deviation from the mission objective, while realizing flexible deformation of the formation.

[0005] To achieve the above objectives, one aspect of this application proposes a flexible formation navigation and obstacle avoidance method for autonomous underwater vehicles, including: Acquire the status information and target points of each vehicle, wherein the vehicles include a lead vehicle and a follow vehicle; The target point is used as the target reference point for the navigation vehicle; The desired offset of the follower vehicle relative to the lead vehicle is determined according to the preset formation configuration, and a virtual reference point of the follower vehicle is generated according to the state information corresponding to the lead vehicle and the desired offset. The initial guidance vector field of the navigator is generated based on the target reference point, and the initial guidance vector field of the follower is generated based on the virtual reference point; Based on obstacles in the navigation environment, the initial guidance vector fields corresponding to the navigator and the follower are adjusted to obtain the reference motion directions of the navigator and the follower after obstacle avoidance. Based on the current state information of the navigator and the follower, and the reference motion direction after obstacle avoidance, candidate control sequences for the navigator and the follower are generated. The current optimal control command is determined from the candidate control sequences; Based on the current optimal control command, drive the navigator and the follower to move and update the status information. Then, based on the updated status information, return to the step of determining the target reference point of the navigator until each vehicle reaches the mission endpoint.

[0006] In some embodiments, the state information includes the vehicle's position, attitude, linear velocity, and angular velocity; The step of generating the initial guidance vector field of the navigator based on the target reference point, and generating the initial guidance vector field of the follower based on the virtual reference point, includes: Calculate the first position error between the current position of the pilot vehicle and the target reference point; Calculate the first distance between the current position of the pilot vehicle and the target reference point based on the first position error; The initial guidance vector field of the pilot vehicle is calculated based on the first position error, the first distance, and the preset first reference speed. Calculate the second position error between the current position of the following vehicle and the virtual reference point; Calculate the second distance between the current position of the following vehicle and the virtual reference point based on the second position error; The initial guidance vector field of the following vehicle is calculated based on the second position error, the second distance, and the preset second reference speed.

[0007] In some embodiments, adjusting the initial guidance vector field corresponding to the lead vehicle and the follower vehicle based on obstacles in the navigation environment includes: Based on the relative positional relationship between each vehicle's current position and the obstacle, determine the symbolic distance field of the obstacle; The local normal direction and tangential bypass direction of the obstacle are determined based on the symbolic distance field; The modulation matrix corresponding to the obstacle is constructed based on the local normal direction and the tangential bypass direction; The initial guiding vector field is adjusted according to the modulation matrix.

[0008] In some embodiments, generating candidate control sequences for the navigator and the follower based on their respective current state information and the reference motion direction after obstacle avoidance includes: A first range of values ​​for the thruster thrust, horizontal rudder angle, and vertical rudder angle of the pilot vehicle is determined, and a second range of values ​​for the thruster thrust, horizontal rudder angle, and vertical rudder angle of the follower vehicle is determined. The current state information of the pilot vehicle is used as the initial state information of the pilot vehicle, and the reference motion direction of the pilot vehicle after obstacle avoidance is used as the desired motion direction of the pilot vehicle. A candidate control sequence of the pilot vehicle is generated within the first value range. The current state information of the following vehicle is used as the initial state information of the following vehicle, and the reference motion direction of the following vehicle after obstacle avoidance is used as the expected motion direction of the following vehicle. A candidate control sequence of the following vehicle is generated within the second value range. Each candidate control sequence includes the corresponding thruster thrust value, horizontal rudder angle value, and vertical rudder angle value at each moment in the prediction time domain.

[0009] In some embodiments, determining the current optimal control command from the candidate control sequence includes: Predict the candidate motion trajectory generated for each of the candidate control sequences; Calculate the comprehensive cost of each candidate motion, and determine the current optimal control command from the candidate control sequence based on the comprehensive cost.

[0010] In some embodiments, predicting the candidate motion trajectory generated for each of the candidate control sequences includes: The current state information of each vehicle is used as the current predicted state at the starting point of the prediction time domain; Within the prediction time domain, the thruster thrust value, horizontal rudder angle value, and vertical rudder angle value corresponding to the current control step in the candidate control sequence are determined as the current control input according to the control step sequence; Substitute the current predicted state and the current control into a preset aircraft dynamics model to calculate the predicted state for the next control step. The predicted state of the next control step is used as the updated current predicted state. The process returns to the steps of determining the current control input and predicting the state, until all control steps in the prediction time domain have been traversed, and the candidate motion trajectory generated by each candidate control sequence is predicted.

[0011] To achieve the above objectives, another aspect of this application proposes a flexible formation navigation and obstacle avoidance device for autonomous underwater vehicles, the device comprising: The status information acquisition module is used to acquire the status information and target points of each vehicle, wherein the vehicles include a lead vehicle and a follow vehicle; The target reference point determination module is used to use the target point as the target reference point of the navigation vehicle; The virtual reference point generation module is used to determine the expected offset of the follower vehicle relative to the lead vehicle according to the preset formation configuration, and to generate a virtual reference point for the follower vehicle according to the state information corresponding to the lead vehicle and the expected offset. The guidance vector field generation module is used to generate the initial guidance vector field of the pilot vehicle based on the target reference point, and to generate the initial guidance vector field of the follower vehicle based on the virtual reference point. The reference motion direction determination module is used to adjust the initial guidance vector field corresponding to the navigator and the follower based on obstacles in the navigation environment, so as to obtain the reference motion direction of the navigator and the follower after obstacle avoidance. The candidate control sequence generation module is used to generate candidate control sequences for the navigator and the follower based on their respective current state information and the reference motion direction after obstacle avoidance. An optimal control command generation module is used to determine the current optimal control command from the candidate control sequence; The target point arrival module is used to drive the navigator and the follower vehicles to move based on the current optimal control command, update the status information, and then return to the step of determining the target reference point of the navigator based on the updated status information, until each vehicle reaches the mission endpoint.

[0012] To achieve the above objectives, another aspect of this application provides an electronic device, which includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the method described above.

[0013] To achieve the above objectives, another aspect of the embodiments of this application proposes a computer-readable storage medium storing a computer program that, when executed by a processor, implements the methods described above.

[0014] This application also discloses a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium and execute the computer instructions, causing the computer device to perform the aforementioned method.

[0015] The embodiments of this application include at least the following beneficial effects: This application provides a method for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles. This method first acquires the state information and target point of each vehicle, including a lead vehicle and follower vehicles; the target point is used as the target reference point for the lead vehicle; the expected offset of the follower vehicle relative to the lead vehicle is determined according to a preset formation configuration, and a virtual reference point for the follower vehicle is generated based on the state information and expected offset of the lead vehicle; an initial guidance vector field for the lead vehicle is generated based on the target reference point, and an initial guidance vector field for the follower vehicle is generated based on the virtual reference point; based on the navigation... Obstacles in the environment are used to adjust the initial guidance vector fields corresponding to the navigator and follower vehicles to obtain the reference motion directions of the navigator and follower vehicles after obstacle avoidance. Based on the current state information of the navigator and follower vehicles and the reference motion directions after obstacle avoidance, candidate control sequences for the navigator and follower vehicles are generated. The current optimal control command is determined from the candidate control sequences. The navigator and follower vehicles are driven to move based on the current optimal control command, and the state information is updated. Based on the updated state information, the process returns to the step of determining the target reference point of the navigator vehicle until each vehicle reaches the mission endpoint. This application adopts a navigator-follower flexible formation architecture. During obstacle avoidance, the following aircraft can temporarily deviate from the virtual reference point direction based on the guidance vector field modulated by the obstacle, realizing flexible deformation of the formation. After passing through the obstacle area, since the virtual reference point always follows the state update of the navigator aircraft, the following aircraft resumes tracking the virtual reference point, enabling the formation to adaptively return to the preset formation configuration. This can improve the safety and stability of multiple AUVs cooperating through complex areas. Secondly, by combining the guidance vector field with the obstacle modulation mechanism, each aircraft is provided with a reference motion direction with a global convergence trend and corrected by the obstacle, guiding the aircraft to detour in a safe direction. This achieves local obstacle avoidance while maintaining global target orientation, effectively avoiding problems such as low detour efficiency, local trajectory oscillation, and even deviation from the mission target. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of an implementation environment provided in an embodiment of this application; Figure 2 This is a flowchart of a flexible formation navigation and obstacle avoidance method for autonomous underwater vehicles provided in an embodiment of this application; Figure 3 This is a schematic diagram of the structure of a flexible formation navigation and obstacle avoidance device for autonomous underwater vehicles provided in an embodiment of this application; Figure 4 This is a schematic diagram of the hardware structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0017] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of this application and are not intended to limit it. In the following description, when referring to the accompanying drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with those of this application; they are merely examples of apparatuses and methods consistent with some aspects of the embodiments of this application as detailed in the appended claims.

[0018] It is understood that the terms "first," "second," "third," "fourth," etc. (if present) in the specification and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of this application described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0019] It should be understood that in this application, "at least one (item)" means one or more, and "more than" means two or more. "And / or" is used to describe the relationship between related objects, indicating that three relationships can exist. For example, "A and / or B" can represent three cases: only A exists, only B exists, and both A and B exist simultaneously, where A and B can be singular or plural. The character " / " generally indicates that the preceding and following related objects are in an "or" relationship. "At least one (item) of the following" or similar expressions refer to any combination of these items, including any combination of single or plural items. For example, at least one (item) of a, b, or c can represent: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, and c can be single or multiple.

[0020] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing embodiments of this application only and is not intended to limit this application.

[0021] Existing multi-AUV formation navigation and obstacle avoidance methods have the following shortcomings: On the one hand, while existing predictive optimization methods such as model predictive control and model predictive path integral control can perform trajectory prediction and control optimization within a limited time domain, they are essentially local optimization methods and lack global guidance capabilities from the starting area to the target area. In environments with dense obstacles, non-convex obstacles, or complex three-dimensional terrain, they are prone to problems such as low detour efficiency, local trajectory oscillation, or even deviation from the mission objective. On the other hand, most methods emphasize rigid formation maintenance. In narrow passages, dense obstacles, or complex seabed terrain, the formation shape lacks the ability to deform as needed, making it difficult to take into account collision avoidance within the formation during obstacle avoidance, and it is difficult to adaptively restore the desired formation after passing through the obstacle area.

[0022] In view of this, this application provides a method and related equipment for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles. This method first acquires the state information and target point of each vehicle, including a lead vehicle and follower vehicles; the target point is used as the target reference point for the lead vehicle; the expected offset of the follower vehicles relative to the lead vehicle is determined according to a preset formation configuration, and a virtual reference point for the follower vehicles is generated based on the state information and expected offset of the lead vehicle; an initial guidance vector field for the lead vehicle is generated based on the target reference point, and an initial guidance vector field for the follower vehicles is generated based on the virtual reference point; based on the navigation environment... For obstacles in the path, the initial guidance vector fields corresponding to the navigator and follower are adjusted to obtain the reference motion directions of the navigator and follower after obstacle avoidance. Based on the current state information of the navigator and follower and the reference motion directions after obstacle avoidance, candidate control sequences for the navigator and follower are generated. The current optimal control command is determined from the candidate control sequences. The navigator and follower are driven to move based on the current optimal control command, and the state information is updated. Based on the updated state information, the process returns to the step of determining the target reference point of the navigator until each vehicle reaches the mission endpoint. This application adopts a navigator-follower flexible formation architecture. During obstacle avoidance, the following aircraft can temporarily deviate from the virtual reference point direction based on the guidance vector field modulated by the obstacle, realizing flexible deformation of the formation. After passing through the obstacle area, since the virtual reference point always follows the state update of the navigator aircraft, the following aircraft resumes tracking the virtual reference point, enabling the formation to adaptively return to the preset formation configuration. This can improve the safety and stability of multiple AUVs cooperating through complex areas. Secondly, by combining the guidance vector field with the obstacle modulation mechanism, each aircraft is provided with a reference motion direction with a global convergence trend and corrected by the obstacle, guiding the aircraft to detour in a safe direction. This achieves local obstacle avoidance while maintaining global target orientation, effectively avoiding problems such as low detour efficiency, local trajectory oscillation, and even deviation from the mission target.

[0023] The autonomous underwater vehicle (AUV) flexible formation navigation and obstacle avoidance method provided in this application relates to the field of navigation and obstacle avoidance technology. This method can be applied to a terminal, a server, or software running on either a terminal or a server. In some embodiments, the terminal can be a smartphone, tablet, laptop, desktop computer, smart speaker, smartwatch, or vehicle-mounted terminal, but is not limited to these. The server can be configured as an independent physical server, a server cluster or distributed system composed of multiple physical servers, or a cloud server providing basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, middleware services, domain name services, security services, CDN, and big data and artificial intelligence platforms. The server can also be a node server in a blockchain network. The software can be an application implementing the autonomous underwater vehicle flexible formation navigation and obstacle avoidance method, but is not limited to the above forms.

[0024] This application can be used in a wide variety of general-purpose or special-purpose computer system environments or configurations. Examples include: personal computers, server computers, handheld or portable devices, tablet devices, multiprocessor systems, microprocessor-based systems, set-top boxes, programmable consumer electronics devices, network PCs, minicomputers, mainframe computers, and distributed computing environments including any of the above systems or devices. This application can be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc., that perform specific tasks or implement specific abstract data types. This application can also be practiced in distributed computing environments where tasks are performed by remote processing devices connected via a communication network. In distributed computing environments, program modules can reside in local and remote computer storage media, including storage devices.

[0025] like Figure 1 The diagram shown is a schematic representation of an implementation environment provided in an embodiment of this application. (Refer to...) Figure 1 The implementation environment includes at least one terminal 102 and a server 101. The terminal 102 and the server 101 can be connected via a network, either wirelessly or via a wired connection, to complete data transmission and exchange.

[0026] Server 101 can be a standalone physical server, a server cluster or distributed system consisting of multiple physical servers, or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, middleware services, domain name services, security services, CDN (Content Delivery Network), and big data and artificial intelligence platforms.

[0027] Additionally, server 101 can also be a node server in a blockchain network. Blockchain is a novel application model of computer technologies such as distributed data storage, peer-to-peer transmission, consensus mechanisms, and encryption algorithms.

[0028] Terminal 102 can be a smartphone, tablet, laptop, desktop computer, smart speaker, smartwatch, etc. It can also be a vehicle-mounted terminal of the various device types described above, but is not limited to these. Terminal 102 and server 101 can be directly or indirectly connected via wired or wireless communication, and this embodiment does not impose any limitations.

[0029] For example, based on Figure 1 The implementation environment shown in this application embodiment provides a flexible formation navigation and obstacle avoidance method for autonomous underwater vehicles. The following description uses the application of this flexible formation navigation and obstacle avoidance method for autonomous underwater vehicles in server 101 as an example. It can be understood that this method can also be applied in terminal 102.

[0030] Reference Figure 2 , Figure 2 The flowchart illustrates a flexible formation navigation and obstacle avoidance method for autonomous underwater vehicles applied to a server, as provided in this application embodiment. The execution subject of this method can be any of the aforementioned computer devices (including servers or terminals). (Refer to...) Figure 2 The method may include the following steps: S100. Obtain the status information and target point of each vehicle, wherein the vehicles include a lead vehicle and a follow vehicle.

[0031] In this embodiment, the status information and global mission target point of each autonomous underwater vehicle (AUV) participating in the formation navigation mission are first obtained. Specifically, the autonomous underwater vehicles (AUVs) in this embodiment include a lead vehicle and a follower vehicle, which respectively undertake the formation guidance mission and the following mission.

[0032] Specifically, the status information of autonomous underwater vehicles This includes the position, attitude, linear velocity, and angular velocity of each autonomous underwater vehicle; among which, This indicates the three-dimensional position, pitch angle, and yaw angle of the AUV. () represents the three-dimensional position coordinates of the vehicle in the ground coordinate system. Indicates pitch angle, Indicates the heading angle. , representing the oscillation speed of the AUV sway speed heave speed and pitch angular velocity and heading angular velocity .

[0033] S200, The target point is used as the target reference point of the navigation vehicle.

[0034] In this embodiment of the application, the global mission target point is set as the target reference point of the navigator; wherein, the target reference point is responsible for guiding the entire formation toward the mission target.

[0035] S300. Determine the expected offset of the follower vehicle relative to the lead vehicle according to the preset formation configuration, and generate a virtual reference point for the follower vehicle based on the state information corresponding to the lead vehicle and the expected offset.

[0036] In this embodiment, based on a preset formation configuration, the expected offset of each follower vehicle relative to the lead vehicle is determined. This expected offset is a three-dimensional relative position vector defined in the lead vehicle's body coordinate system or the formation's local coordinate system, including longitudinal offset components, lateral offset components, and vertical offset components. In a specific embodiment, the vertical offset component can be 0. Then, the current state information of the lead vehicle is acquired in real time, including the lead vehicle's position and real-time heading angle. Based on the lead vehicle's position and heading angle, and combined with the expected offset, a virtual reference point corresponding to each follower vehicle is calculated. This virtual reference point is dynamically updated as the lead vehicle moves, ensuring that the follower vehicles always have a variable tracking target that matches the preset formation configuration.

[0037] S400: Generate the initial guidance vector field of the navigator based on the target reference point, and generate the initial guidance vector field of the follower based on the virtual reference point.

[0038] In this embodiment of the application, initial guidance vector fields are generated for each pilot vehicle and each follower vehicle based on the spatial relationship between their respective positions and corresponding reference points. Exemplarily, the steps of generating the initial guidance vector field of the pilot vehicle based on the target reference point and the initial guidance vector field of the follower vehicle based on the virtual reference point include S410-S460: S410. Calculate the first position error between the current position of the pilot vehicle and the target reference point. Its expression is: ; in, This indicates the current three-dimensional position of the navigator. Used as the target reference point.

[0039] S420. Calculate the first distance between the current position of the pilot vehicle and the target reference point based on the first position error. Its expression is: ; S430. Calculate the initial guidance vector field of the pilot vehicle based on the first position error, the first distance, and the preset first reference speed. Specifically, the initial guidance vector field of the navigator is calculated. The expression is: ; in, The preset first reference speed, To prevent positive numbers with zero denominators when the distance is too small.

[0040] The initial guidance vector field of the navigator is used to generate a reference motion direction from the current position of the navigator to the target reference point, so that the navigator always has a motion tendency to converge toward the target reference point.

[0041] S440. Calculate the second position error between the current position of the following vehicle and the virtual reference point. Its expression is: ; in, This indicates the current three-dimensional position of the navigator. This serves as a virtual reference point.

[0042] S450: Calculate the second distance between the current position of the following vehicle and the virtual reference point based on the second position error. Its expression is: ; S460. Calculate the initial guidance vector field of the following vehicle based on the second position error, the second distance, and the preset second reference speed.

[0043] Specifically, the initial guidance vector field of the following aircraft is calculated. The expression is: ; in, The preset second reference speed, To prevent positive numbers with zero denominators when the distance is too small.

[0044] In this embodiment, the initial guidance vector field of the follower vehicle is used to generate a reference motion direction pointing from the current position of the follower vehicle to a virtual reference point, ensuring that the follower vehicle always has a tendency to converge toward the virtual reference point. In this embodiment, the reference motion direction of each vehicle is determined by the opposite direction of its corresponding position error, and is used to characterize the tendency of each vehicle to move toward the reference point when there are no obstacles or when obstacle modulation is not applied.

[0045] In this embodiment, reference motion directions are generated for the navigator and follower vehicles respectively through the initial guidance vector field (GVF). The initial guidance vector field of the navigator points to the global mission target, and the initial guidance vector field of the follower vehicle points to a virtual reference point generated online based on the navigator's status, thereby achieving overall formation target convergence and follower member formation tracking.

[0046] S500. Based on obstacles in the navigation environment, adjust the initial guidance vector field corresponding to the navigator and the follower to obtain the reference motion direction of the navigator and the follower after obstacle avoidance.

[0047] In this embodiment, for each vehicle, including the lead vehicle and each follower vehicle, obstacle information from the navigation environment is further introduced into the generated initial guidance vector field to locally correct the initial guidance direction. This ensures that the adjusted reference motion direction balances the global convergence trend towards the reference point and the obstacle avoidance requirement. For example, the steps of adjusting the initial guidance vector fields corresponding to the lead vehicle and follower vehicles based on obstacles in the navigation environment include S510-S540: S510. Determine the symbolic distance field of the obstacle based on the relative positional relationship between the current position of each vehicle and the obstacle; Specifically, constructing obstacle functions It is used to characterize the relative positional relationship between the current position of the AUV and the k-th obstacle; obstacle function The expression is: ; in, is the signed distance field of the k-th obstacle, used to describe the signed distance relationship between the current position and the obstacle boundary. Typically, the value is 0 at the obstacle boundary, positive outside the obstacle, and negative inside the obstacle. is the distance-normalized feature length.

[0048] S520. Determine the local normal direction and tangential bypass direction of the obstacle based on the symbolic distance field; Specifically, the local normal direction is used to determine and weaken the component of the initial guide vector that points towards the inside of the obstacle or the danger zone, while the tangential direction is used to guide the AUV to detour around the outside of the obstacle or the safe zone.

[0049] S530. Construct the modulation matrix corresponding to the obstacle based on the local normal direction and the tangential bypass direction; S540. Adjust the initial guiding vector field according to the modulation matrix.

[0050] Specifically, the initial guidance vector field is adjusted and corrected based on the modulation matrix to obtain the reference motion directions of the navigator and follower vehicles after obstacle avoidance. This guides the vehicle to navigate in a safe direction, thereby achieving local obstacle avoidance while maintaining the overall navigation trend.

[0051] Reference direction of motion after obstacle avoidance The expression is: ; in, For the modulation matrix, This is the initial guidance vector field corresponding to the aircraft.

[0052] In this embodiment, when there are multiple obstacles, the total modulation matrix is ​​obtained by weighted fusion based on the degree of influence of each obstacle on the vehicle. Finally, the initial guidance vector is adjusted and corrected using the total modulation matrix to obtain the adjusted initial guidance vector field, which is the reference motion direction after obstacle avoidance.

[0053] This application embodiment combines the initial guidance vector field with the obstacle modulation mechanism in the above manner to correct the reference motion direction near the obstacle. When the AUV approaches the obstacle area, the original target attraction vector is modulated by the obstacle normal and tangential information to suppress the motion component toward the inside of the obstacle and guide the AUV to detour in a safe direction. This achieves local obstacle avoidance while maintaining the global navigation trend, reducing the problem of local detour instability caused by the lack of global guidance in local prediction optimization.

[0054] S600: Generate candidate control sequences for the navigator and the follower based on their respective current state information and the reference motion direction after obstacle avoidance.

[0055] In this embodiment of the application, for the lead vehicle and each follower vehicle, a candidate control sequence satisfying actuator constraints is generated by model prediction path integral (MPPI), that is, the reference motion direction is converted into an executable control input for the vehicle; for example, the step of generating candidate control sequences for the lead vehicle and the follower vehicle within the feasible range according to the current state information of the lead vehicle and the follower vehicle and the reference motion direction after obstacle avoidance includes S610-S630: S610. Determine a first range of values ​​for the thruster thrust, horizontal rudder angle, and vertical rudder angle of the pilot vehicle, and determine a second range of values ​​for the thruster thrust, horizontal rudder angle, and vertical rudder angle of the follower vehicle. For example, the thrust values ​​of both the pilot and follower vehicles satisfy the following range: ,in, For thruster thrust, This represents the maximum thrust of the thruster; Horizontal rudder angles of the navigator and follower The range of values ​​for all satisfy ,in, This represents the maximum horizontal rudder angle; Vertical rudder angles of the navigator and follower The range of values ​​for all satisfy ,in, This represents the maximum value of the vertical rudder angle.

[0056] S620. Using the current state information of the pilot vehicle as the initial state information of the pilot vehicle, and using the reference motion direction of the pilot vehicle after obstacle avoidance as the desired motion direction of the pilot vehicle, a candidate control sequence of the pilot vehicle is generated within the first value range. S630. Using the current state information of the following vehicle as the initial state information of the following vehicle, and using the reference motion direction of the following vehicle after obstacle avoidance as the expected motion direction of the following vehicle, a candidate control sequence of the following vehicle is generated within the second value range. Each candidate control sequence includes the corresponding thruster thrust value, horizontal rudder angle value, and vertical rudder angle value at each moment in the prediction time domain.

[0057] S700: Determine the current optimal control command from the candidate control sequence.

[0058] For example, the step of determining the current optimal control command from the candidate control sequence includes S710-S720: S710. Predict the candidate motion trajectory generated by each of the candidate control sequences; In this embodiment, the state of the next predictive control step is obtained by substituting the current predicted state and the control input of the current predictive step into a preset vehicle dynamics model. ,in, The discrete form of the aircraft dynamics model can be used to obtain the predicted motion trajectory corresponding to the candidate control sequence by continuously recursively applying it in the prediction time domain.

[0059] For example, the step of predicting the candidate motion trajectory generated by each of the candidate control sequences includes S711-S714: S711. Use the current state information of each vehicle as the current predicted state of the prediction time domain starting point; S712. In the prediction time domain, according to the control step sequence, determine the thruster thrust value, horizontal rudder angle value and vertical rudder angle value corresponding to the current control step in the candidate control sequence as the current control input; S713. Substitute the current predicted state and the current control into the preset aircraft dynamics model to calculate the predicted state for the next control step. For example, the five-degree-of-freedom kinematic model of the AUV in this application embodiment is as follows: ,in, Given the coordinate transformation matrix, the five-degree-of-freedom dynamic model of the AUV can be further expressed as: ; in, The inertia matrix, The matrix represents the Coriolis force and the centripetal force. For hydrodynamic damping matrix, For the restoring force and restoring torque in still water, To control the input, This refers to lumped disturbances caused by external disturbances or unmodeled hydrodynamic forces. The control inputs include propeller thrust, horizontal rudder angle, and vertical rudder angle.

[0060] S714. Take the predicted state of the next control step as the updated current predicted state, return to execute the steps of determining the current control input and the predicted state, that is, return to repeat the above steps S713 and S714 until all control steps in the prediction time domain have been traversed, and the candidate motion trajectory generated by each candidate control sequence is predicted.

[0061] S720. Calculate the comprehensive cost of each candidate motion, and determine the current optimal control command from the candidate control sequence based on the comprehensive cost.

[0062] In this embodiment, MPPI samples control quantities such as thruster thrust, horizontal rudder angle and vertical rudder angle within a finite prediction time domain, predicts candidate trajectories based on the AUV dynamics model, and performs weighted optimization based on cost terms such as heading / pitch tracking error, reference point distance, speed maintenance, control energy consumption, motion smoothness and obstacle penalty to generate actual control inputs that satisfy AUV dynamics constraints and actuator constraints.

[0063] Specifically, for each candidate trajectory, MPPI calculates its single-step cost for each control step in the prediction time domain. This cost is used to evaluate the overall performance of the candidate control sequence in terms of target tracking, obstacle avoidance, energy consumption, and stability. The cost terms include: consistency with the reference direction, distance error with the target point or virtual reference point, velocity maintenance error, control energy consumption, motion smoothness, and penalties for obstacle and intra-team collision risks. Then, based on the single-step cost, the comprehensive cost of each candidate predicted trajectory is calculated. Its expression is: ; in, This represents the cumulative comprehensive cost of the candidate predicted trajectory corresponding to the k-th candidate control sequence in the prediction time domain; Indicates the length of the prediction time domain; Indicates the control step number within the prediction time domain; This indicates the effect of the k-th candidate control sequence on the first... Predicted state of the aircraft in each predictive control step; Indicates the k-th candidate control sequence. The control input corresponding to each predictive control step; This represents the single-step cost calculated based on the predicted state and control input.

[0064] For example, its single-step cost function The expression is: ; in, and These represent the heading angle error and pitch angle error between the current direction of motion of the AUV and the GVF reference direction, respectively. This is the current location of the AUV. The reference point is the global target point for the navigator and the virtual reference point for the follower. For oscillation speed, The desired oscillation speed; It is used to constrain sway, heave, and attitude angular velocity, and reduce unnecessary motion oscillations; For control input; and This is the weight matrix; This is a penalty item for obstacles.

[0065] The smaller the overall cost in the embodiments of this application, the more the candidate trajectory meets the requirements of navigation, obstacle avoidance and dynamics execution.

[0066] Furthermore, the candidate control sequence corresponding to the minimum comprehensive cost is selected as the current optimal control instruction.

[0067] It should be noted that the Model Predictive Path Integral (MPPI) adopts a rolling optimization mechanism: MPPI updates the control sequence based on the comprehensive cost of each candidate trajectory and only executes the first control input of the current control cycle; in the next control cycle, the AUV state is reacquired and the above prediction, evaluation and update process is repeated, thus forming rolling optimization control.

[0068] Furthermore, in this embodiment, to avoid collisions within the formation, other AUVs are treated as dynamic obstacles and added to the obstacle set of the current AUV during each control cycle, so that each AUV can avoid other formation members while avoiding environmental obstacles; then, when the formation passes through areas with dense obstacles or narrow passages, the following AUVs are allowed to temporarily deviate from the preset relative position to prioritize obstacle avoidance safety; after passing through the obstacle area, the following AUVs continue to track the virtual reference point, so that the formation gradually returns to the preset configuration.

[0069] The embodiments of this application use MPPI to sample control sequences, predict trajectories, and optimize costs within a finite prediction time domain, so that the generated control inputs simultaneously satisfy constraints such as AUV nonlinear dynamics, thruster thrust, and rudder angle, thus ensuring the dynamic executableness of control actions.

[0070] S800: Drive the navigator and the follower vehicles to move based on the current optimal control command, update the status information, and then return to the step of determining the target reference point of the navigator based on the updated status information, until each vehicle reaches the mission endpoint.

[0071] Specifically, the current position, attitude, and velocity information of each AUV are periodically acquired, and the global target point, virtual reference point, obstacle information, and the position of the crew are updated. Subsequently, each AUV sequentially completes GVF calculation, MPPI predictive control optimization, and control input execution, and feeds back the executed state to the next control cycle until each vehicle in the formation reaches the mission endpoint area and completes the navigation and obstacle avoidance mission.

[0072] In summary, the embodiments of this application provide a stable reference motion direction through GVF, and the MPPI generates control inputs that satisfy dynamic constraints in the local prediction time domain. This enables obstacle avoidance, intra-team collision avoidance, and formation maintenance to be unified into the same control framework, thereby improving the adaptability of multi-AUV formations in complex underwater environments such as dense obstacles, cross encounters, and external disturbances.

[0073] Furthermore, in another embodiment of this application, the method described in steps S100 to S800 above is used to conduct simulation tests on the navigation and obstacle avoidance of multiple AUV flexible formations in a three-dimensional underwater environment. The steps are as follows: I. AUV Model: This embodiment uses the REMUS autonomous underwater vehicle as the simulation object, and its main hydrodynamic parameters are shown in Table 1. Table 1 Hydrodynamic parameters

[0074] II. Algorithm Parameters: GVF parameters: The preset reference speed is 1.0 m / s, the obstacle influence distance is 5.0 m, and the obstacle reaction coefficient is adaptively set according to the obstacle distance to adjust the normal suppression and tangential deflection intensity of the guidance direction near the obstacle.

[0075] MPPI parameters: Simulation step size is set to 0.1s, prediction time domain is set to 30 steps (corresponding to 3s physical time), 100 candidate control sequences are sampled for each optimization, temperature parameter is set to 0.8, control disturbance covariance is [0.15, 0.05, 0.15], and control input smoothness coefficient is 0.6. Control constraints: thrust range is 0-100N, horizontal and vertical rudder angles do not exceed ±30°. Desired longitudinal velocity is 1.5 m / s. Cost coefficients: control energy consumption weight matrix is ​​diag(1,5,5), motion damping weight matrix is ​​diag(10,50,500,200), corresponding to sway velocity, heave velocity, pitch velocity, and yaw velocity, respectively. Obstacle avoidance parameters: obstacle warning distance is 2.0m, danger distance is 0.8m. Role weights: The navigator's heading / pitch tracking weight is 12, the target distance weight is 0.1, and the speed maintenance weight is 5; the follower's heading / pitch tracking weight is 5, the virtual reference point distance weight is 20, and the speed maintenance weight is 0.5.

[0076] Formation parameters: The formation consists of one lead vehicle and two follower vehicles. The expected offsets of the follower vehicles relative to the lead vehicle are as follows: [-5m, -5m, 0m] for the first follower vehicle and [-5m, 5m, 0m] for the second follower vehicle. The heading angle filter coefficient for the lead vehicle is 0.05.

[0077] III. Simulation Scenarios: The simulation area is set to a 100m×100m×100m three-dimensional underwater space; 5 to 8 spherical static obstacles with radii ranging from 5 to 10m are placed in the environment. The task is set as follows: the formation starts from the initial position, avoids all obstacles, and reaches the target point [100m, 100m, 100m]. The simulation step size is set to 0.1s, and the maximum simulation time is set to 60s to 100s.

[0078] IV. Implementation steps: Initialization: Set the simulation step size to 0.1s, the maximum number of simulation steps to 1000, and load the AUV dynamics model, control constraints, obstacle information, and formation parameters.

[0079] Formation setup: Set up 1 navigator and 2 follower vehicles. The initial position of the navigator is [0m, 0m, 0m], and the target point is [100m, 100m, 100m]. The initial relative offsets of the follower vehicles are [-5m, -5m, 0m] and [-5m, 5m, 0m], respectively.

[0080] Reference point generation: The navigator uses the target point as a reference point, and the follower generates a virtual reference point based on the position of the navigator and the filtered heading angle. The heading angle filtering coefficient is 0.05.

[0081] GVF guidance: Set the GVF reference speed to 1.0 m / s, and generate corresponding guidance directions based on the reference points and obstacle information of each vehicle.

[0082] MPPI optimization: Set the prediction time domain to 30 steps, the candidate control sequence to 100, and the desired velocity to 1.5 m / s. Evaluate the cost of the candidate trajectory and generate the current control command.

[0083] Control execution: Executes the thrust, horizontal rudder angle and vertical rudder angle control quantities output by MPPI, with a control input smoothness coefficient of 0.6.

[0084] Cyclic Update: Update the position, attitude, and velocity of each vehicle, and repeat the above-mentioned reference point generation, GVF guidance, and MPPI optimization processes.

[0085] Mission termination: The simulation ends when the formation reaches the vicinity of the target point or reaches the maximum number of simulation steps.

[0086] It should be noted that the above parameters are only an example, and in actual applications, they can be adapted to factors such as AUV model, mission area, obstacle density, and desired speed.

[0087] To explain in detail the principles of the technical solution of this application, the overall process of this application will be described below with reference to some specific embodiments. It is easy to understand that the following is an explanation of the technical principles of this application and should not be regarded as a limitation of this application.

[0088] In one specific embodiment, the flexible formation navigation and obstacle avoidance process of the autonomous underwater vehicle of this application includes the following steps: S1, AUV dynamics modeling: In the embodiments of this application, a ground coordinate system, a volume coordinate system, and a NED coordinate system are first defined, and a five-degree-of-freedom spatial kinematics and dynamics model of an autonomous underwater vehicle is established on this basis.

[0089] Define the state variables of the AUV as follows and .in, This indicates the three-dimensional position of the AUV in the ground coordinate system. Indicates pitch angle, Indicates the heading angle; These represent the sway velocity, roll velocity, and heave velocity of the AUV in volume coordinates, respectively. Indicates pitch angular velocity, This indicates the angular velocity of the heading.

[0090] The five-degree-of-freedom kinematic model of an AUV is represented as follows: ; in, This is the coordinate transformation matrix.

[0091] The five-degree-of-freedom dynamic model of an AUV can be further expressed as: ; in, The inertia matrix, The matrix represents the Coriolis force and the centripetal force. For hydrodynamic damping matrix, For the restoring force and restoring torque in still water, To control the input, This refers to lumped disturbances caused by external disturbances or unmodeled hydrodynamic forces.

[0092] Control input Composed of thruster thrust, horizontal rudder angle, and vertical rudder angle, it is represented as: ; in, For thruster thrust, For horizontal rudder angle, For the vertical rudder angle, the control input must satisfy the actuator's physical constraints: ; The aforementioned dynamic model is used for Model Predictive Path Integral Control (MPPI) to recursively predict the motion state of the AUV based on candidate control inputs in the prediction time domain, thereby generating control commands that satisfy the AUV's dynamic constraints and actuator constraints.

[0093] S2. Initial Guiding Vector Field Construction: First, obtain the current 3D position of the AUV. and the corresponding reference point positions for each AUV For the lead AUV, the reference point is the global mission target point; for the follower AUV, the reference point is a virtual reference point generated online based on the lead AUV's status. Then, the position error between the AUV's current position and the reference point is calculated. And calculate the distance between them. Next, an initial guiding vector pointing towards the reference point is generated based on the direction of the position error. .in, This is the initial guiding vector. For preset reference speed, To prevent the denominator from being a positive number with zero when the distance is too small, the above method ensures that when the AUV deviates from the reference point, the initial guiding vector always points towards the reference point, giving the AUV a tendency to converge towards the reference point.

[0094] The initial guiding vector field is used to generate a reference motion direction pointing to the reference point. The reference motion direction is determined by the opposite direction of the position error and is used to characterize the tendency of the AUV to move toward the reference point when there are no obstacles or when no obstacle modulation is performed.

[0095] S3, Obstacle Modulation Guiding Vector Field: Based on the initial guiding vector field, an obstacle modulation mechanism is further introduced to locally correct the original guiding direction.

[0096] First, construct the obstacle function. , which is used to characterize the relative positional relationship between the current position of the AUV and the k-th obstacle. Wherein, Let the symbolic distance field be the k-th obstacle. The distance is the normalized feature length. Then, the local normal direction and tangential detour direction of the obstacle are obtained based on the signed distance field. The normal direction is used to identify and weaken the component in the initial guidance vector that points towards the interior of the obstacle or the danger zone, while the tangential direction is used to guide the AUV to detour along the outside of the obstacle or the safe zone. Next, the modulation matrix corresponding to the k-th obstacle is constructed based on the normal and tangential directions. When multiple obstacles exist, the total modulation matrix is ​​obtained by weighted fusion based on the degree of influence of each obstacle on the AUV. .in, Let K be the influence weight of the k-th obstacle, and K be the number of obstacles. Finally, the initial guidance vector is corrected using the total modulation matrix to obtain the reference motion direction after obstacle avoidance. ;in The reference motion direction is modulated to the obstacle. Through the above process, the AUV can reduce its tendency to move towards the inside of the obstacle or the danger zone when approaching an obstacle, and instead detour along the outside of the obstacle or a safe zone.

[0097] S4, MPPI Forecasting Control: Here, MPPI refers to the model predicted path integral. In the embodiments of this application, MPPI predictive control is used to convert the reference motion direction generated by GVF into an AUV executable control input. During the rolling prediction process, MPPI preferentially selects control inputs that are consistent with the GVF guidance direction and satisfy the AUV dynamics and actuator constraints. Specifically, the MPPI predictive control steps are as follows: MPPI based on AUV current status As an initial state, candidate control sequences are generated within the feasible ranges of thruster thrust, horizontal rudder angle, and vertical rudder angle. .in, This indicates the AUV's three-dimensional position, pitch angle, and yaw angle; , representing the AUV's sway, roll, heave speeds, pitch angular velocity, and yaw angular velocity. For thruster thrust, For horizontal rudder angle, This refers to the vertical rudder angle. For each set of candidate control sequences, MPPI substitutes the current predicted state and the control input for the current predicted step into the AUV dynamics model to obtain the state for the next predicted step. ,in, This represents the discrete form of the AUV dynamics model. The predicted motion trajectory corresponding to the candidate control sequence is obtained through continuous recursion within the prediction time domain.

[0098] During trajectory evaluation, MPPI calculates a comprehensive cost for each candidate predicted trajectory. Simultaneously considering factors such as target point or virtual reference point tracking, obstacle distance, safe distance within the formation, speed maintenance, control energy consumption, and motion smoothness, the control input with the lower overall cost is selected as the execution command for the current moment. Single-step cost function. The expression is: ; in, and These represent the heading angle error and pitch angle error between the current direction of motion of the AUV and the GVF reference direction, respectively. This is the current location of the AUV. As a reference point, the lead AUV corresponds to the global target point, while the follow AUV corresponds to the virtual reference point; For oscillation speed, The desired oscillation speed; It is used to constrain sway, heave, and attitude angular velocity, and reduce unnecessary motion oscillations; For control input; and This is the weight matrix; This is a penalty item for obstacles.

[0099] MPPI updates the control sequence based on the comprehensive cost of each candidate trajectory and executes only the first control input of the current control cycle; in the next control cycle, the AUV state is reacquired and the above prediction, evaluation and update process is repeated, thus forming rolling optimization control.

[0100] Finally, the current optimal control command is obtained through MPPI predictive control, and each AUV is driven to move based on the current optimal control command.

[0101] S5. Drive each vehicle to move based on the current optimal control command and update the status information. Based on the updated status information, repeat the execution of steps S2-S4 until each vehicle reaches the mission endpoint.

[0102] In summary, this application provides a method and related equipment for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles (AUVs). This method first acquires the state information and target point of each vehicle, including a lead vehicle and follower vehicles. The target point is used as the target reference point for the lead vehicle. The expected offset of the follower vehicles relative to the lead vehicle is determined according to a preset formation configuration. A virtual reference point for the follower vehicles is generated based on the state information and expected offset of the lead vehicle. An initial guidance vector field is generated for the lead vehicle based on the target reference point, and an initial guidance vector field is generated for the follower vehicles based on the virtual reference point. Based on the navigation environment… For obstacles in the path, the initial guidance vector fields corresponding to the navigator and follower are adjusted to obtain the reference motion directions of the navigator and follower after obstacle avoidance. Based on the current state information of the navigator and follower and the reference motion directions after obstacle avoidance, candidate control sequences for the navigator and follower are generated. The current optimal control command is determined from the candidate control sequences. The navigator and follower are driven to move based on the current optimal control command, and the state information is updated. Based on the updated state information, the process returns to the step of determining the target reference point of the navigator until each vehicle reaches the mission endpoint. This application adopts a navigator-follower flexible formation architecture. During obstacle avoidance, the following aircraft can temporarily deviate from the virtual reference point direction based on the guidance vector field modulated by the obstacle, realizing flexible deformation of the formation. After passing through the obstacle area, since the virtual reference point always follows the state update of the navigator aircraft, the following aircraft resumes tracking the virtual reference point, enabling the formation to adaptively return to the preset formation configuration. This can improve the safety and stability of multiple AUVs cooperating through complex areas. Secondly, by combining the guidance vector field with the obstacle modulation mechanism, each aircraft is provided with a reference motion direction with a global convergence trend and corrected by the obstacle, guiding the aircraft to detour in a safe direction. This achieves local obstacle avoidance while maintaining global target orientation, effectively avoiding problems such as low detour efficiency, local trajectory oscillation, and even deviation from the mission target.

[0103] like Figure 3 As shown in the figure, this application also provides a structural schematic diagram of an autonomous underwater vehicle flexible formation navigation and obstacle avoidance device. This device can implement the above-mentioned method and may include: The status information acquisition module 21 is used to acquire the status information and target points of each vehicle, wherein the vehicles include a lead vehicle and a follow vehicle; The target reference point determination module 22 is used to use the target point as the target reference point of the navigation vehicle; The virtual reference point generation module 23 is used to determine the expected offset of the follower vehicle relative to the lead vehicle according to the preset formation configuration, and generate the virtual reference point of the follower vehicle according to the state information corresponding to the lead vehicle and the expected offset. The guidance vector field generation module 24 is used to generate the initial guidance vector field of the navigator based on the target reference point, and to generate the initial guidance vector field of the follower based on the virtual reference point. The reference motion direction determination module 25 is used to adjust the initial guidance vector field corresponding to the navigator and the follower based on obstacles in the navigation environment, so as to obtain the reference motion direction of the navigator and the follower after obstacle avoidance. The candidate control sequence generation module 26 is used to generate candidate control sequences for the navigator and the follower based on their respective current state information and the reference motion direction after obstacle avoidance. The optimal control command generation module 27 is used to determine the current optimal control command from the candidate control sequence; The target point arrival module 28 is used to drive the navigator and the follower vehicles to move based on the current optimal control command, update the status information, and then return to the step of determining the target reference point of the navigator based on the updated status information, until each vehicle reaches the mission endpoint.

[0104] It is understood that the content of the above method embodiments is applicable to the present device embodiments. The specific functions implemented by the present device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0105] This application also provides an electronic device, which includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the aforementioned autonomous underwater vehicle flexible formation navigation and obstacle avoidance method. This electronic device can be any smart terminal, including tablet computers, in-vehicle computers, etc.

[0106] It is understood that the content of the above method embodiments is applicable to this device embodiment. The specific functions implemented by this device embodiment are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0107] Please see Figure 4 , Figure 4 The hardware structure of an electronic device according to another embodiment is illustrated. The electronic device includes: The processor 901 can be implemented using a general-purpose CPU (Central Processing Unit), microprocessor, application-specific integrated circuit (ASIC), or one or more integrated circuits, and is used to execute relevant programs to implement the technical solutions provided in the embodiments of this application. The memory 902 can be implemented as a read-only memory (ROM), static storage device, dynamic storage device, or random access memory (RAM). The memory 902 can store the operating system and other application programs. When the technical solutions provided in the embodiments of this specification are implemented through software or firmware, the relevant program code is stored in the memory 902 and is called and executed by the processor 901 to execute the autonomous underwater vehicle flexible formation navigation and obstacle avoidance method of the embodiments of this application. The input / output interface 903 is used to implement information input and output; The communication interface 904 is used to enable communication and interaction between this device and other devices. Communication can be achieved through wired means (such as USB, Ethernet cable, etc.) or wireless means (such as mobile network, WIFI, Bluetooth, etc.). Bus 905 transmits information between various components of the device (e.g., processor 901, memory 902, input / output interface 903, and communication interface 904); The processor 901, memory 902, input / output interface 903, and communication interface 904 are connected to each other within the device via bus 905.

[0108] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described autonomous underwater vehicle flexible formation navigation and obstacle avoidance method.

[0109] It is understood that the content of the above method embodiments is applicable to this storage medium embodiment. The specific functions implemented in this storage medium embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.

[0110] Memory, as a non-transitory computer-readable storage medium, can be used to store non-transitory software programs and non-transitory computer-executable programs. Furthermore, memory may include high-speed random access memory, and may also include non-transitory memory, such as at least one disk storage device, flash memory device, or other non-transitory solid-state storage device. In some embodiments, memory may optionally include memory remotely located relative to the processor, and these remote memories can be connected to the processor via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

[0111] The embodiments described in this application are for the purpose of more clearly illustrating the technical solutions of the embodiments of this application, and do not constitute a limitation on the technical solutions provided by the embodiments of this application. As those skilled in the art will know, with the evolution of technology and the emergence of new application scenarios, the technical solutions provided by the embodiments of this application are also applicable to similar technical problems.

[0112] Those skilled in the art will understand that the technical solutions shown in the figures do not constitute a limitation on the embodiments of this application, and may include more or fewer steps than shown, or combine certain steps, or different steps.

[0113] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0114] Those skilled in the art will understand that all or some of the steps in the methods disclosed above, as well as the functional modules / units in the systems and devices, can be implemented as software, firmware, hardware, or suitable combinations thereof.

[0115] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of the units described above is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0116] The units described above as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0117] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0118] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes multiple instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this application. The aforementioned storage medium includes various media capable of storing programs, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0119] The preferred embodiments of the present application have been described above with reference to the accompanying drawings, but this does not limit the scope of the claims of the present application. Any modifications, equivalent substitutions, and improvements made by those skilled in the art without departing from the scope and substance of the embodiments of the present application shall be within the scope of the claims of the present application.

Claims

1. A flexible formation navigation and obstacle avoidance method for autonomous underwater vehicles, characterized in that, include: Acquire the status information and target points of each vehicle, wherein the vehicles include a lead vehicle and a follow vehicle; The target point is used as the target reference point for the navigation vehicle; The desired offset of the follower vehicle relative to the lead vehicle is determined according to the preset formation configuration, and a virtual reference point of the follower vehicle is generated according to the state information corresponding to the lead vehicle and the desired offset. The initial guidance vector field of the navigator is generated based on the target reference point, and the initial guidance vector field of the follower is generated based on the virtual reference point; Based on obstacles in the navigation environment, the initial guidance vector fields corresponding to the navigator and the follower are adjusted to obtain the reference motion directions of the navigator and the follower after obstacle avoidance. Based on the current state information of the navigator and the follower, and the reference motion direction after obstacle avoidance, candidate control sequences for the navigator and the follower are generated. The current optimal control command is determined from the candidate control sequences; Based on the current optimal control command, drive the navigator and the follower to move and update the status information. Then, based on the updated status information, return to the step of determining the target reference point of the navigator until each vehicle reaches the mission endpoint.

2. The method for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles according to claim 1, characterized in that, The status information includes the vehicle's position, attitude, linear velocity, and angular velocity; The step of generating the initial guidance vector field of the navigator based on the target reference point, and generating the initial guidance vector field of the follower based on the virtual reference point, includes: Calculate the first position error between the current position of the pilot vehicle and the target reference point; Calculate the first distance between the current position of the pilot vehicle and the target reference point based on the first position error; The initial guidance vector field of the pilot vehicle is calculated based on the first position error, the first distance, and the preset first reference speed. Calculate the second position error between the current position of the following vehicle and the virtual reference point; Calculate the second distance between the current position of the following vehicle and the virtual reference point based on the second position error; The initial guidance vector field of the following vehicle is calculated based on the second position error, the second distance, and the preset second reference speed.

3. The method for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles according to claim 2, characterized in that, The adjustment of the initial guidance vector field corresponding to the navigator and follower vehicles based on obstacles in the navigation environment includes: Based on the relative positional relationship between each vehicle's current position and the obstacle, determine the symbolic distance field of the obstacle; The local normal direction and tangential bypass direction of the obstacle are determined based on the symbolic distance field; The modulation matrix corresponding to the obstacle is constructed based on the local normal direction and the tangential bypass direction; The initial guiding vector field is adjusted according to the modulation matrix.

4. The method for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles according to claim 1, characterized in that, The step of generating candidate control sequences for the navigator and the follower based on their respective current state information and the reference motion direction after obstacle avoidance includes: A first range of values ​​for the thruster thrust, horizontal rudder angle, and vertical rudder angle of the pilot vehicle is determined, and a second range of values ​​for the thruster thrust, horizontal rudder angle, and vertical rudder angle of the follower vehicle is determined. The current state information of the pilot vehicle is used as the initial state information of the pilot vehicle, and the reference motion direction of the pilot vehicle after obstacle avoidance is used as the desired motion direction of the pilot vehicle. A candidate control sequence of the pilot vehicle is generated within the first value range. The current state information of the following vehicle is used as the initial state information of the following vehicle, and the reference motion direction of the following vehicle after obstacle avoidance is used as the expected motion direction of the following vehicle. A candidate control sequence of the following vehicle is generated within the second value range. Each candidate control sequence includes the corresponding thruster thrust value, horizontal rudder angle value, and vertical rudder angle value at each moment in the prediction time domain.

5. The method for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles according to claim 4, characterized in that, Determining the current optimal control command from the candidate control sequences includes: Predict the candidate motion trajectory generated for each of the candidate control sequences; Calculate the comprehensive cost of each candidate motion, and determine the current optimal control command from the candidate control sequence based on the comprehensive cost.

6. The method for flexible formation navigation and obstacle avoidance of autonomous underwater vehicles according to claim 5, characterized in that, The prediction of the candidate motion trajectory generated for each of the candidate control sequences includes: The current state information of each vehicle is used as the current predicted state at the starting point of the prediction time domain; Within the prediction time domain, the thruster thrust value, horizontal rudder angle value, and vertical rudder angle value corresponding to the current control step in the candidate control sequence are determined as the current control input according to the control step sequence; Substitute the current predicted state and the current control into a preset aircraft dynamics model to calculate the predicted state for the next control step. The predicted state of the next control step is used as the updated current predicted state. The process returns to the steps of determining the current control input and predicting the state, until all control steps in the prediction time domain have been traversed, and the candidate motion trajectory generated by each candidate control sequence is predicted.

7. A flexible formation navigation and obstacle avoidance device for autonomous underwater vehicles, characterized in that, The device includes: The status information acquisition module is used to acquire the status information and target points of each vehicle, wherein the vehicles include a lead vehicle and a follow vehicle; The target reference point determination module is used to use the target point as the target reference point of the navigation vehicle; The virtual reference point generation module is used to determine the expected offset of the follower vehicle relative to the lead vehicle according to the preset formation configuration, and to generate a virtual reference point for the follower vehicle according to the state information corresponding to the lead vehicle and the expected offset. The guidance vector field generation module is used to generate the initial guidance vector field of the pilot vehicle based on the target reference point, and to generate the initial guidance vector field of the follower vehicle based on the virtual reference point. The reference motion direction determination module is used to adjust the initial guidance vector field corresponding to the navigator and the follower based on obstacles in the navigation environment, so as to obtain the reference motion direction of the navigator and the follower after obstacle avoidance. The candidate control sequence generation module is used to generate candidate control sequences for the navigator and the follower based on their respective current state information and the reference motion direction after obstacle avoidance. An optimal control command generation module is used to determine the current optimal control command from the candidate control sequence; The target point arrival module is used to drive the navigator and the follower vehicles to move based on the current optimal control command, update the status information, and then return to the step of determining the target reference point of the navigator based on the updated status information, until each vehicle reaches the mission endpoint.

8. An electronic device, characterized in that, Including the processor and memory; The memory is used to store programs; The processor executes the program to implement the autonomous underwater vehicle flexible formation navigation and obstacle avoidance method as described in any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the autonomous underwater vehicle flexible formation navigation and obstacle avoidance method as described in any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the autonomous underwater vehicle flexible formation navigation and obstacle avoidance method as described in any one of claims 1 to 6.

Images