Path planning method for self-organizing AUV (Autonomous Underwater Vehicle) dual-body coordinated docking

The path planning method for self-organizing AUV twin bodies addresses the challenges of flexible and accurate underwater docking by using a Gaussian pseudo-spectral method to optimize trajectories, enhancing efficiency and safety in complex deep-sea environments.

CN119065391APending Publication Date: 2024-12-03HARBIN ENG UNIV
View PDF 0 Cites 3 Cited by

Patent Information

Application Number
CN202411156950.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-22
Publication Date
2024-12-03

AI Technical Summary

Technical Problem

Existing autonomous underwater vehicle (AUV) docking technologies face challenges in flexible deployment, complexity, and accuracy due to ocean current disturbances, especially in complex deep-sea environments, with current path planning methods failing to account for dual-body motion coordination.

Method used

A path planning method for self-organizing AUV twin bodies using a Gaussian pseudo-spectral method to transform optimal control problems into nonlinear programming, considering dynamic and environmental constraints, and optimizing trajectories for high-precision docking.

Benefits of technology

Enhances the efficiency and safety of underwater operations by optimizing docking trajectories, allowing flexible deployment and reducing manpower and resource consumption, while maintaining high accuracy and adaptability to ocean currents.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119065391A_ABST
    Figure CN119065391A_ABST
Patent Text Reader

Abstract

The invention relates to a path planning method for self-organizing AUV double-body coordinated docking, and belongs to the technical field of underwater robots. The self-organizing AUV completes sensing positioning and position alignment before and after, and autonomous dynamic docking is achieved; for ocean current disturbance, an optimal trajectory planning method is provided, and the navigation time, energy consumption and safety are optimized; active docking constraint conditions and ocean current influence of the self-organizing AUV double bodies are researched, a kinetic equation under ocean current interference is established, and a self-organizing AUV double body active docking planning algorithm considering multiple constraint conditions is designed; a nonlinear programming problem with constraints is solved by adopting a pseudo-spectral method, and the convergence of motion parameters under constraint conditions is ensured. The method mainly comprises the following steps: establishing a system model; performing trajectory optimization problem expression; discretization of performance indexes is carried out; converting a boundary value problem into an initial value problem by using a KKT multiplier method; and track adjustment and optimization are carried out. According to the invention, the docking efficiency, safety and concealment are improved, and the method has important practical application value.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to an optimal path planning method for coordinated docking of AUV twin bodies against ocean current disturbance, belonging to the technical field of underwater robots. Background Art

[0002] With the development of navigation, more and more deep-sea exploration missions require the participation of underwater robots (Autonomous Underwater Vehicle, AUV), such as underwater reconnaissance, seabed construction, large-scale and long-range work network expansion and formation, etc. Autonomous underwater docking technology is the key technology to complete the docking between AUV and the mother ship or docking station, and plays a vital role in various deep-sea exploration missions. However, during the docking process between AUV and the mother ship or docking station, it is easily affected by many unfavorable factors such as limited operating depth and range. How to eliminate these influences and realize high-efficiency and high-precision autonomous underwater docking technology is an important issue that needs to be explored in the field of underwater robots.

[0003] The existing autonomous underwater docking technology is mainly based on fixed docking, that is, after the AUV completes its staged work, it returns and docks to a docking station fixed at a specific underwater location. The advantage of fixed docking is that the requirements for path planning and tracking control are relatively low, and the docking success rate is high. There are two disadvantages. On the one hand, fixed docking requires the recovery device to be arranged in advance, which is not flexible and not suitable for complex deep-sea exploration tasks; on the other hand, the docking device of the fixed docking station is complex in design and easily affected by other conditions such as marine attachments, making it difficult to maintain. Therefore, the development of mobile autonomous underwater docking technology has solved the limitations of existing technologies.

[0004] Mobile autonomous underwater docking technology refers to the docking device being in motion and the AUV completing docking with the docking device.

[0005] The docking device in the present invention is mounted on the rear section of the self-organizing AUV, and the front section is equipped with acoustic and optical guidance devices, and a physical locking device. This type of mobile autonomous underwater docking technology requires that the AUV dual sections can accurately capture the position information of the large underwater vehicle during the docking process, and also puts forward higher requirements for the path planning and tracking control of the two relatively moving objects. Therefore, in the mobile autonomous underwater docking technology, it is essential to achieve accurate and efficient docking path planning.

[0006] Self-organizing underwater robots have high value. After the front and rear hulls are deployed underwater respectively, they can pass through some complex waters due to their small segmented volume and maneuverability. The front and rear hulls reach the designated waters to complete the hull docking, which can have an unexpected effect. And after the docking is completed, because its docking device adopts an orthogonal joint mechanism, the front and rear hulls of the robot can be deflected at a certain angle, and move forward in the form of thruster propulsion + orthogonal joint rotation, which enhances the adaptability to the environment and its own flexibility. From the perspective of portability, its hull consists of two parts, and the requirements for storage location are relatively small. Overall, self-organizing underwater robots can better meet the needs of convenient, efficient, safe and concealed operations.

[0007] Further consideration, since the dual-segment size of the self-organizing AUV is relatively small compared with the docking station and large underwater vehicles, it is easily disturbed by the ocean current, and it is necessary to focus on the optimal planning technology of dual-body coordination under the interference of ocean current.

[0008] Summarizing the current status of AUV dynamic docking trajectory planning problems at home and abroad, it can be found that the commonly used planning methods include the rapid expansion random tree method, the B-spline curve method, etc., but the above methods do not consider the coordination problem of the two-body motion in the docking problem, and it is difficult to solve the complex dynamic equation constraints and path constraints. For example: the publication number is CN114815818A, and the name of the invention is a method and device for dynamic path planning of underactuated AUV mobile docking. Most of its docking devices are carried on large ships or large unmanned surface vehicles, and do not have the concealment of operating in special underwater environments; and most of the docking technology is based on large mother boats for recovery and docking, and the docking accuracy of the AUV itself has not been studied.

[0009] Therefore, it is urgent to propose a path planning method for AUV twin-body coordinated docking to solve the above technical problems. Summary of the invention

[0010] The purpose of the present invention is to provide a path planning method for coordinated docking of a self-organizing AUV twin bodies in order to achieve low-cost, high-precision autonomous underwater docking, which is practical and reliable, and solves the difficulties in the deployment of underwater robots (self-organizing AUVs with high maneuverability and special sea exploration capabilities) and the inability of underwater robots to conduct detection and operations in special operating environments such as vertical three-dimensional profiles, narrow waters, and pipelines. A brief overview of the present invention is given below to provide a basic understanding of certain aspects of the present invention. It should be understood that this overview is not an exhaustive overview of the present invention. It is not intended to identify the key or important parts of the present invention, nor is it intended to limit the scope of the present invention.

[0011] The technical solution of the present invention:

[0012] A path planning method for self-organizing AUV twin-body coordinated docking includes the following steps:

[0013] Step 1: Establish a system model, including a dynamic model and boundary conditions;

[0014] Step 2: Formulate the trajectory optimization problem, construct the objective function and establish corresponding constraints;

[0015] Step 3: Discretize the performance index and use the Gaussian pseudo-spectral method to transform the optimal control problem into a nonlinear programming problem;

[0016] Step 4: Use the KKT multiplier method to transform the boundary value problem into an initial value problem and solve it to obtain the navigation trajectory;

[0017] Step 5: Adjust and optimize the trajectory.

[0018] Preferably: in step 1, the system model includes a dynamic model and an underwater environment model;

[0019] Establish a dynamic model of the AUV (self-organizing underwater robot), including state variables and control variables, and set boundary conditions at the initial and terminal moments; state variables include the position, velocity, and attitude of the AUV, and control variables include the thrust and torque of the propeller;

[0020] Establish an underwater environment model using existing nautical charts and collected environmental information. Common methods include grid method, visibility graph method, and Voronoi method. It is necessary to generate a trajectory or path for the AUV heading (attitude), position, and velocity distribution in advance, consider the impact of ocean currents on AUV navigation, and add it as a constraint to the path (the path or trajectory between the initial and terminal is referred to as the AUV's running trajectory) planning.

[0021] Preferably: in step 2, the expression of the trajectory optimization problem includes constructing an objective function and establishing corresponding constraints; constructing an objective function, which usually includes indicators such as the shortest navigation time and the least energy consumption; establishing constraints corresponding to the objective function, which include aspects such as dynamics, kinematics and environmental factors to ensure the feasibility and safety of the trajectory;

[0022] When performing trajectory optimization, the Gaussian pseudospectral method is used to numerically approximate the optimal control problem; the time interval is converted to [-1,1] through time domain transformation, and the system state and control variables are approximated using Legendre polynomials and Lagrange interpolation basis functions. In order to perform computer numerical calculations, it is necessary to discretize the system state and control variable equations and convert the differential equation constraints into algebraic constraints; this is achieved by deriving the approximate expression of the state variable and substituting it into the dynamic equation, thereby converting the optimal control problem into a series of algebraic equations and inequalities.

[0023] Preferably: in step 3, Gaussian integration is a high-precision numerical integration method, which is suitable for integrating a function at a specific integration point; by discretizing the objective function, a series of discretized weight coefficients can be obtained;

[0024] The discretized optimal control problem is transformed into a nonlinear programming problem and solved using appropriate numerical methods; appropriate numerical methods include penalty function method and sequential quadratic programming (SQP) method, which can handle optimization problems with constraints and solve the constrained optimal trajectory problem.

[0025] Preferably: in step 4, KKT conditions are a type of optimality conditions, which provide an effective solution framework for constrained optimization problems and are necessary conditions for determining whether a point is an extreme point; KKT conditions unify and expand the optimization problems involving equality constraints and inequality constraints, making it possible to systematically solve various optimization problems with constraints and ensure that the solutions obtained meet the necessary optimality conditions.

[0026] Preferably: in step 5, the actual navigation trajectory of the AUV is adjusted and optimized according to the solved trajectory; this includes fine-tuning the speed, direction and attitude to ensure the accuracy and safety of the docking process.

[0027] Preferably, the method further comprises step 6, simulation and experimental verification:

[0028] The effectiveness of the trajectory optimization algorithm was verified in a simulated environment (underwater environment model), and then experiments were carried out in the actual underwater environment to evaluate the docking accuracy and efficiency; this step is necessary to ensure the reliability of the algorithm in practical applications.

[0029] Preferably, it also includes step 7, performing system integration and testing according to the above algorithm and system: integrating all components and algorithms into a system model, and performing comprehensive testing to ensure the stability and reliability of the system; this includes hardware integration, software debugging and system calibration.

[0030] The present invention has the following beneficial effects:

[0031] The present invention provides a comprehensive solution for optimizing the dynamic docking trajectory of the self-organizing AUV twin bodies, thereby improving the efficiency and safety of underwater operations. The segmented deployment and underwater docking increase the optimization space for the robot deployment and recovery scheme, and the selection of deployment and recovery devices is more diverse and flexible. Different deployment and recovery schemes can be designed for different task requirements, which is convenient for cost control; and the complicated surface deployment and recovery process is eliminated, which greatly saves manpower and material resources; on the other hand, the underwater self-organization design allows it to be deployed quickly and covertly. The method of the present invention is not only applicable to self-organizing AUVs, but can also be extended to other types of underwater robots and automation systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 It is a schematic diagram of the dynamic docking process of the two-body self-organizing underwater robot according to an embodiment of the present invention;

[0033] Figure 2 The figure shows a schematic structural diagram of a self-organizing AUV twin-body cooperative docking device according to an embodiment of the present invention;

[0034] Figure 3 The diagram shows the definition of the self-organizing AUV coordinate system and the radius vector according to an embodiment of the present invention;

[0035] Figure 4 Shown is a schematic diagram of a self-organizing AUV dynamics model according to an embodiment of the present invention;

[0036] Figure 5 It is a schematic diagram showing the equivalent relationship between the direct method and the indirect method for solving the optimal control problem according to an embodiment of the present invention;

[0037] Figure 6 Shown is a flow chart of the pseudospectral method (PS method) according to an embodiment of the present invention.

[0038] In the figure, 1-AUV front section, 2-AUV rear section, 3-rear section docking device. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention is described below by the specific embodiments shown in the accompanying drawings. However, it should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present invention. In addition, in the following description, the description of well-known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present invention.

[0040] Specific implementation method 1: Combination Figure 1-6The present embodiment is described. The present embodiment is a path planning method for coordinated docking of a self-organizing AUV twin body. The front and rear cabins of the self-organizing AUV are combined with acoustic / optical measurement methods to complete perception positioning and position alignment to achieve autonomous dynamic docking. In view of ocean current disturbances, an optimal trajectory planning method is proposed to optimize navigation time, energy consumption and safety. The active docking constraints and ocean current influence of the self-organizing AUV twin bodies are studied, the dynamic equations under ocean current interference are established, and a self-organizing AUV twin body active docking planning algorithm considering multiple constraints is designed. The pseudo-spectral method is used to solve the nonlinear programming problem with constraints to ensure the convergence of motion parameters under constraints. The technology improves docking efficiency, safety and concealment, and has important practical application value. The following steps are specifically included:

[0041] Step 1: During the rendezvous and docking process of the front and rear segments of the AUV, a trajectory for the heading, position and speed distribution of the AUV needs to be generated in advance; at the same time, the interference of influencing factors such as ocean currents needs to be considered to optimize the optimal performance indicators, such as navigation time, energy consumption, safety, etc., so that the AUV can reach the desired rendezvous area and complete the recovery task; the present invention studies the front and rear segment coordinated optimal planning technology designed for self-organizing dual-segment AUVs that is resistant to ocean current interference;

[0042] In order to achieve the above objectives, the dynamic docking trajectory planning problem of the coordinated self-organizing AUV twins is considered, which can be considered as an optimal control problem under a series of constraints;

[0043] According to the actual situation, it is necessary to consider the establishment of the system model: first, define the dynamic model of the AUV, including state variables and control variables, as well as their boundary conditions at the initial and terminal moments; state variables include the position, speed, attitude, etc. of the AUV, and control variables include the thrust and torque of the propeller, etc.;

[0044] Step 2: Formulate the trajectory optimization problem: construct the objective function, such as the shortest flight time and the least energy consumption, and establish the corresponding constraints, including dynamics, kinematics, and environmental factors; these constraints ensure the feasibility and safety of the trajectory;

[0045] According to the constraints required for trajectory optimization, the Gaussian pseudospectral method is used to numerically approximate the optimal control problem. First, a time domain transformation is performed to convert the time interval into [-1,1]. Then, the Legendre polynomial and Lagrange interpolation basis functions are used to approximate the system state and control variables.

[0046] In order to perform computer numerical calculations, it is necessary to discretize the system state and control variable equations obtained in the above steps and transform the differential equation constraints into algebraic constraints. This is achieved by deriving the approximate expressions of the state variables and substituting them into the dynamic equations. In this way, the optimal control problem is transformed into a series of algebraic equations and inequalities.

[0047] Step 3, discretize the performance index: use Gaussian integral to discretize the objective function; Gaussian integral is a high-precision numerical integration method suitable for integrating functions at specific integration points.

[0048] The optimal control problem after discretization is transformed into a nonlinear programming problem and solved using appropriate numerical methods, including penalty function method, sequential quadratic programming method, etc., which can handle optimization problems with constraints.

[0049] Step 4, use the KKT multiplier method to transform the boundary value problem into an initial value problem; KKT conditions are a type of optimality conditions that provide an effective solution framework for constrained optimization problems.

[0050] Step 5, trajectory adjustment and optimization: According to the solved trajectory, the actual navigation trajectory of the AUV is adjusted and optimized; this includes fine-tuning the speed, direction and attitude to ensure the accuracy and safety of the docking process;

[0051] Step 6, simulation and experimental verification: verify the effectiveness of the trajectory optimization algorithm in a simulated environment, and then conduct experiments in an actual underwater environment to evaluate the docking accuracy and efficiency; this step is necessary because it ensures the reliability of the algorithm in practical applications;

[0052] Step 7: Based on the above algorithms and systems, conduct system integration and testing: Integrate all components and algorithms into a system and conduct comprehensive testing to ensure the stability and reliability of the system; this includes hardware integration, software debugging and system calibration;

[0053] The present invention provides a comprehensive solution for optimizing the dynamic docking trajectory of a self-organizing AUV twin body, thereby improving the efficiency and safety of underwater operations. The method of the present invention is not only applicable to self-organizing AUVs, but can also be extended to other types of underwater robots and automation systems.

[0054] The pseudo-spectral method in the optimal control problem solving method of the present invention can not only coordinate various constraints with the motion of the self-organizing AUV twin bodies, but also has the advantages of high solution accuracy. It is applied to the research in the field of AUV underwater recovery, and the pseudo-spectral method is combined with the self-organizing AUV twin body dynamic docking trajectory planning problem considering mutual coordination, so as to plan an optimal trajectory that meets expectations for the AUV homing stage.

[0055] The design concept of underwater self-organizing robot is derived from underwater snake-like robot and underwater self-reconfigurable robot. Its segmented deployment and underwater docking design increases the optimization space of robot deployment and recovery scheme, and the selection of deployment and recovery devices is more diverse and flexible. Different deployment and recovery schemes can be designed according to different task requirements. It also eliminates the complicated surface deployment and recovery process, greatly saving manpower and material resources. On the other hand, the underwater self-organizing design enables it to be deployed quickly and covertly, which has great value.

[0056] The invention focuses on the trajectory planning of the dynamic docking of two bodies of a self-organizing AUV with mutual coordination, and adopts the Gaussian pseudo-spectral method to optimally plan the front and rear double-body dynamic docking of an under-actuated self-organizing AUV.

[0057] Embodiment 1:

[0058] A path planning method for coordinated docking of two-body self-organizing underwater robots. The self-organizing AUV two-body underwater dynamic docking task includes four sub-stages as shown in the attached figure. Figure 1 As shown in the figure, the self-organizing AUV catamaran underwater dynamic docking tasks are: mission departure phase, homing approach phase, docking guidance phase and physical locking phase;

[0059] The first stage is the mission departure stage: this stage is the specific operation when the AUV front and rear sections are deployed and the rear section is ready to dock with the front section; at this time, the AUV rear section 2 sends out an acoustic guidance signal. When the transponder carried by the AUV front section 1 receives the acoustic signal, the AUV front section 1 will rely on the horizontal and vertical thrusters to adjust its attitude, determine the depth of the uniform current, and navigate at a constant depth in the uniform current. The speed is related to the current speed; the AUV front section 1 has no main thrust and cannot navigate at a constant speed. The method can be used to determine the optimal heading and depth navigation, and navigate in the current with the optimal attitude, but the speed of the AUV front section 1 is related to the current. When the AUV rear section 2 receives the acoustic signal of the front section, the system calculates the relative position deviation to determine the approximate position of the target of the AUV front section 1. The AUV rear section 2 adjusts its own attitude and speed to prepare for the next stage;

[0060] The second stage, the homing approach stage: AUV rear section 2 from the surrounding Figure 1 The AUV rear section 2 has obtained the position of the target AUV front section 1 through its own onboard sensor, and adjusted its own position and attitude to shorten the distance with the target point (AUV front section 1); after this process, the AUV rear section 2 will approach the docking device of the target AUV front section 1 according to a certain position, attitude, and speed to ensure that the attitude of the AUV front section 1 and the docking device on the front section is accurately captured, and then enter the next stage;

[0061] The third stage is the guided docking stage: in this stage, the AUV rear section 2 moves from point B to point C. At this time, the distance between the AUV rear section 2 and the docking device carried by the target front section (AUV front section 1) gradually decreases and becomes very close. At this time, the AUV rear section 2 uses the front camera to obtain accurate information of the docking device through optical guidance to prepare for docking.

[0062] The fourth stage is the physical locking stage: in this stage, after the docking device is fully deployed, the front hinge of the docking device is bent downward at a certain angle through hydraulic drive; the rear section of the AUV gradually approaches the stern of the front section, and prepares to lock the docking device; through the coordinated movement of the AUV rear section 2 and the AUV front section 1, the locking device of the AUV front section 1 is locked with the docking device of the rear section, thereby completing the docking.

[0063] The second stage of the above four stages is to study the trajectory planning of the AUV. Its main goal is to make the rear section 2 of the AUV approach the moving front section 1 of the AUV with an appropriate attitude and speed. The docking device 3 carried by the rear section 2 of the AUV has some relative movement with the rear section, and finally make the relative position and attitude of the rear section of the AUV and the front section within the allowable error range.

[0064] The first problem to be solved is how to establish a trajectory planning method that can guide the AUV rear section 2 to safely coordinate docking, so that each motion parameter of the AUV twin body can gradually converge within the constraint conditions during docking;

[0065] The structure diagram of the double-body coordinated docking device of the self-organizing AUV is shown in the attached figure. Figure 2 The self-organizing AUV (AUV twin body) comprises an AUV front section 1, an AUV rear section 2, a rear section docking device 3 and a front section stern docking device. The rear end of the AUV front section 1 has a front section stern docking device, and the front end of the AUV rear section 2 has a rear section docking device 3. The AUV front section 1 and the AUV rear section 2 are docked by connecting the rear section docking device 3 with the front section stern docking device.

[0066] The variable structure dynamics modeling method of the self-organizing AUV includes the following steps:

[0067] Step 1: First, select a suitable inertial reference system to describe the center of mass radius vector and joint radius vector of each module; Figure 3 As shown in Figure 1, in order to obtain the center of mass velocity and acceleration of each module, the center of mass vector of the module can be unified to the same coordinate system (inertial coordinate system) by coordinate transformation, and then derived; then the dynamic mathematical model of the self-organizing AUV is derived from the Kane dynamic equation; the flow chart of the dynamic model is attached Figure 4 ;

[0068] F γ * +F A* +F g +F D +F B +F ctrl =0

[0069] Among them: F γ * represents the generalized inertial force, F A * represents the fluid inertia force, F D Indicates fluid resistance, F g Represents gravity, F B represents buoyancy, F ctrl Represents joint and thruster control forces.

[0070] The Gaussian pseudospectral method solves the optimal control problem, including the following steps:

[0071] Step 2, optimal control problem description:

[0072] Generally, the optimal control problem is to find the control law to satisfy the minimization objective function; the optimal control problem can be discussed in the following form:

[0073]

[0074] Where J is the defined objective function, generally referring to the energy objective function; φ has different meanings depending on the control index, such as the shortest time and the least energy consumption; t is the time, x(t0) is the initial state variable, and g() is used to reflect the docking situation;

[0075] The system needs to satisfy the dynamic constraints:

[0076]

[0077] Boundary Condition Constraints

[0078]

[0079] And satisfy the path constraints

[0080] C(x(t),u(t),t)≤0 (4)

[0081] In the above formula, u(t) is defined as the system control variable, x(t) is defined as the system state variable, t0 is defined as the initial time of the system, and t f Defined as the terminal moment of the system;

[0082] Time domain transformation:

[0083] The Gaussian pseudospectral method first requires time conversion, which converts the optimal control time interval t∈[t0,t f] is converted to τ∈[-1,1], that is:

[0084]

[0085] The time is expressed as:

[0086]

[0087] Through time domain transformation, the original optimal control is transformed into:

[0088]

[0089] C(X(τ),U(τ),τ;t0,t f )≤0, (10)

[0090] In the formula, G() reflects the docking situation after time conversion, U() is the system state variable after time conversion, and X() is the system control variable after conversion; st stands for the abbreviation of subject to (such that) in mathematics, which means constrained. According to Chinese custom, it can be translated into "so that... satisfies... (constraint conditions)"; F() is a function that represents boundary condition constraints;

[0091] in,

[0092]

[0093] Where C() is a function representing the path constraint; U(τ) refers to the system control variable after approximation (time conversion); X(τ) refers to the system state variable after approximation (time conversion);

[0094] Step 3, discretization of the optimal control problem:

[0095] Gaussian pseudospectral method uses Legendre polynomials to approximate the variables of the system; firstly, the roots of N-order Legendre polynomials (τ1, τ2..., τ K ) and τ0=1 as the initial nodes of the system, thereby constructing N+1 Lagrange polynomials L to approximate:

[0096]

[0097] Where N is the order of Legendre polynomial; L is Lagrange polynomial; x(τ) is the state variable of the system, X(τ) is the state variable of the system after approximation; L is the order of Legendre polynomial ... i (τ) is the Lagrange interpolation basis function, which can be expressed as:

[0098]

[0099] τ is the time variable after t approximation (time conversion); τ i and τ j and are unequal roots of Legendre polynomials (initial system nodes);

[0100] The function value of the Lagrange interpolation basis function at each interpolation point is equal, and the values ​​at other points are all 0, that is, property (13):

[0101]

[0102] Therefore, the Lagrange interpolation polynomial at the node position is consistent with the value at the original node, and the state variables at non-node positions can be approximated by the Lagrange interpolation polynomial, as shown in the following formula:

[0103] x(τ i )=X(τ i ),i=0,1,...,K (14)

[0104] Similarly, the system control variable u(τ) can be approximated:

[0105]

[0106] In the above formula, u(τ) is the control variable, and U(τ) is the approximate control variable. According to the properties of (13), we can get u(τ i )=U(τ i ); is the Lagrange interpolation basis function, which is defined as:

[0107]

[0108] Dynamic differential equation conversion:

[0109] By differentiating all the state variables in equation (11), the constraints of the system dynamics differential equation can be transformed into algebraic constraints, namely:

[0110]

[0111] In the formula, an element in the matrix D: ki (τ k ) represents the k-th order Legendre polynomial, the i-th Legendre polynomial is at the k-th root τ k The value of the differential at the Legendre-Gauss point when it is at the root;

[0112] The differentials of all Legendre-Gauss points of the Lagrange polynomial can be obtained by the matrix D∈R N× (N+1) is used to describe it, and its specific expression is:

[0113]

[0114] Therefore, the differential equation constraints in the optimal control problem can be converted into algebraic constraints, as shown in the following formula:

[0115]

[0116] In the formula, f[] represents the boundary condition constraint;

[0117] Terminal state constraints:

[0118] Considering the actual situation, the terminal state X f It should also meet the constraints of the dynamic differential equation:

[0119]

[0120] In the formula, τ f Matching points for Legendre-Gauss;

[0121] Discretization and Gaussian integral approximation give:

[0122]

[0123] In the above formula, τ k is the Legendre-Gauss collocation, μ k is the Gaussian integral weight, which is defined as follows:

[0124]

[0125] In the formula, is about the variable τ k Legendre polynomials (a mathematical method for finding Gaussian integral weights in optimal control);

[0126] Performance indicators under discrete conditions:

[0127] When using Gaussian integral to solve the objective function integral in the optimal control problem, the discretized objective function can be obtained:

[0128]

[0129] Where X0 is the discretized system state variable; X k is the final state constraint; U k is the final control constraint; Φ() is the objective function of the discretized control object (such as time, energy, power consumption);

[0130] The boundary constraints can be discretized as:

[0131]

[0132] The path constraints are discretized as:

[0133] C(X k ,U k ,τ k ,,t0,t f )≤0 (25)

[0134] Through the above series of numerical approximation methods, the optimal control problem can be transformed into a nonlinear programming problem, and then the final performance index (23) can be minimized under the conditions of satisfying the dynamic differential equation constraint (19), the terminal state constraint (21), the boundary condition constraint (24) and the path constraint (25).

[0135] Step 4: Solve the nonlinear programming problem:

[0136] When solving nonlinear programming problems, the KKT multiplier can accurately estimate all state variables of the optimal control problem and combine it with the initial value problem, so that the two-point boundary value problem corresponding to the first-order necessary condition can be transformed into an initial value problem.

[0137] By the attached Figure 5 As shown in the figure, the Gaussian pseudo-spectral method actually transforms the optimal control problem into a type of optimization problem with a discrete form, also known as a nonlinear programming problem. In solving nonlinear programming problems, a large number of experts have conducted research on it, among which the commonly used methods include: penalty function method, sequential quadratic programming method, etc.

[0138] Step 6: Establishment of mathematical model for underactuated AUV trajectory planning problem:

[0139] In actual motion, the underactuated AUV has a large lateral damping, so the influence of heel on it is generally not considered; that is, in the trajectory planning process, the heel angle is and its derivatives Heel angular velocity p and its derivative is set to 0; therefore, the kinematic equation of the AUV studied in this paper can be described by the following five-degree-of-freedom model:

[0140]

[0141] In the formula, x, y, z represent the position of the AUV in the dynamic system, θ and ψ represent the pitch angle and heading angle of the AUV attitude angle in the dynamic system respectively; u, v, w represent the longitudinal velocity, lateral velocity and heave velocity of the AUV respectively, q and r represent the pitch angular velocity and heading angular velocity of the AUV respectively; accordingly, the dynamic equation of the AUV can also be expressed by the five-degree-of-freedom model as follows:

[0142]

[0143] In the above formula, m ii (i=1,2,3,4,5) is a combination of inertia and additional mass, X u , Y v , Z w 、M q , and N r is the linear hydrodynamic coefficient, X u|u| , Y v|v| , Z w|w| 、M q|q| and N r|r| is the nonlinear hydrodynamic damping term; τ u , τ q and τ r They represent the control input variables respectively; W represents the center of gravity of the AUV; the vector It is expressed as the distance between the center of gravity and the center of buoyancy;

[0144] Description of the optimization problem:

[0145] Before the rear section of the AUV performs the docking task with the front section, a fixed time is usually set in advance for docking operation, that is, the start time of the dynamic docking process is known; at this time, the AUV is in a stable running state, that is, the motion state of the AUV at the initial time is known; when the relative position error between the rear section of the AUV and the docking device is 0, the desired AUV state at the terminal moment can be preset and is also known, while the time when the AUV is in the desired state is unknown;

[0146] Optimization problem establishment and constraint analysis:

[0147] (1) Objective function

[0148] In order to reduce the time required for the docking task between the rear section and the front section of the AUV, under the condition of satisfying various constraints, the shortest time required for the AUV docking phase is taken as the performance index function, and a trajectory with the shortest time for the AUV homing phase is planned. At the same time, considering that the docking device on the rear section has some relative motion with the rear section, the energy consumption of the docking device should also be controlled as much as possible, so the optimal performance index combining the shortest AUV navigation time and the least energy consumption of the docking device is selected as:

[0149]

[0150] Among them, v1, v2, v3 are the relative speed errors between the rear docking device and the rear section in three directions X, Y, and Z; t0, t f The system initial time and termination time;

[0151] (2) Constraints

[0152] ① Initial conditions

[0153] The initial conditions include the initial position constraint, velocity constraint and attitude constraint of the AUV, as well as the initial position constraint of the rear section and the docking device. Let t0 be the moment when the AUV and the docking device start to move. The initial conditions refer to the constraints that the state variables need to satisfy at the moment t0.

[0154] The initial position constraint of AUV is:

[0155] x(t0)=x0

[0156] y(t0)=y0

[0157] z(t0)=z0 (29)

[0158] The initial speed constraint of AUV is:

[0159] u(t0)=u0

[0160] v(t0)=v0

[0161] w(t0)=w0

[0162] q(t0)=q0

[0163] r(t0)=r0 (30)

[0164] The initial attitude constraint of AUV is:

[0165] θ(t0)=θ0

[0166] ψ(t0)=ψ0 (31)

[0167] The relative position between the rear section docking device and the rear section is determined by the longitudinal position x1, the transverse position y1 and the vertical position z1;

[0168] x1(t0)=x 10

[0169] y1(t0)=y 10

[0170] z1(t0)=z 10 (32)

[0171] ②Terminal conditions

[0172] The terminal condition refers to the terminal position constraint, speed constraint and attitude constraint of the AUV after the docking mission is completed, and the terminal position constraint and speed constraint of the docking device carried in the rear section. f For the moment when the movement ends;

[0173] The initial position constraint of the AUV terminal is:

[0174] x(t f )=x f

[0175] y(t f )=y f

[0176] z(t f )=z f (33)

[0177] The initial speed constraint of the AUV terminal is:

[0178] u(t f )=u f

[0179] v(t f )=v f

[0180] w(t f )=w f

[0181] q(t f )=q f

[0182] r(t f )=r f (34)

[0183] The AUV terminal attitude constraint is:

[0184] θ(t f )=θ f

[0185] ψ(t f )=ψ f (35)

[0186] The terminal constraints of the rear section docking device and the rear section relative position are:

[0187] x1(t f )=x 1f

[0188] y1(t f )=y 1f

[0189] z1(t f )=z 1f (36)

[0190] ③ Kinematic and dynamic constraints

[0191] In the AUV dynamic docking task considering mutual coordination, the AUV adopts the simplified kinematic and dynamic models introduced above as system constraints;

[0192] There is relative motion between the docking device carried by the rear section and the rear section, and kinematic constraints are also required for it;

[0193]

[0194] where v 1x 、v 1y 、v 1z is the speed error between the rear section docking device and the rear section;

[0195] ④Variable constraints

[0196] Due to the requirements of underwater docking, as well as the limitations of actual water conditions and environmental factors, it is necessary to reasonably constrain the variables and parameters in the AUV, mainly including the following aspects;

[0197] a) Speed ​​constraints

[0198] To ensure the safety of docking, the speed of the rear and front sections of the AUV should not be too large during docking, so the speed is constrained:

[0199] u min ≤u(t)≤u max

[0200] v min ≤v(t)≤v max

[0201] w min ≤w(t)≤w max (38)

[0202] where u min =-1m / s,u max =2m / s,v min =-0.2m / s,v max =0.2m / s,w min =-0.2m / s,

[0203] w max =0.2m / s;

[0204] b) Posture constraints

[0205] Considering the layout of the underactuated AUV thruster, the AUV attitude direction angle cannot be arbitrarily selected;

[0206] θ min ≤θ(t)≤θ max

[0207] ψ min ≤ψ(t)≤ψ max (39)

[0208] in ψmin =-3πm / s,ψ max =3πm / s;

[0209] c) Docking device and AUV front position constraints

[0210] Since the docking device and the rear section of the AUV have relative motion, the position error between the two is limited.

[0211] x 1min ≤x1(t)≤x 1max

[0212] y 1min ≤y1(t)≤y 1max

[0213] z 1min ≤z1(t)≤z 1max (40)

[0214] where x 1min =0m,x 1max =1.5m,y 1min =0m,y 1max =1.5m, z 1min =0m,z 1max =1.5m;

[0215] ⑤Control constraints

[0216] a) Due to the thrust and layout limitations of the AUV thruster, the thrust size and torque during docking cannot be arbitrarily selected;

[0217] τ umin ≤τ u (t)≤τ umax

[0218] τ qmin ≤τ q (t)≤τ qmax

[0219] τ rmin ≤τ r (t)≤τ rmax (41)

[0220] where τ umin =0N,τ umax =40N, τ qmin =-24N·m,τ qmax =24N·m,τ rmin =-24N·m,

[0221] τ rmax =24N·m;

[0222] b) Because the docking device is fixedly connected to the rear section, the speed error between the two is limited

[0223] v 1xmin ≤v 1x (t)≤v 1xmax

[0224] v 1ymin ≤v 1y (t)≤v 1ymax

[0225] v 1zmin ≤v 1z (t)≤v 1zmax (42)

[0226] where v 1xmin =-0.1m / s,v 1xmax =0.1m / s,v 2xmin =-0.1m / s,v 2xmax =0.1m / s,v 3xmin =-0.1m / s,v 3xmax =0.1m / s;

[0227] In order to solve the problem of underwater robot operation deployment and operation restrictions in special environments, the present invention develops an underwater robot system program with self-organizing docking capabilities, aiming to achieve autonomous detection and operation in complex environments;

[0228] The pseudo-spectral algorithm used in the present invention for solving the optimal control problem uses the Gaussian pseudo-spectral method to transform the complex optimal control problem into a nonlinear programming problem and solve it;

[0229] In the pseudospectral methods (PS methods) algorithm used in the present invention, an optimal trajectory optimization program for AUV dynamic docking considering mutual coordination is composed of nine files, namely: The following is an overview of the file structure of the system to implement the docking task:

[0230] Specific implementation file structure

[0231] Main program file (main): calls each sub-function and defines the initial and terminal values ​​of related parameters, state variables and control variables;

[0232] System constraint program file (deriv): defines the system dynamic constraints to ensure that the motion of the AUV satisfies the dynamic equations;

[0233] Boundary function program file (bounds): defines the boundary values ​​of state variables and control variables to ensure that the variables are within a reasonable range;

[0234] Boundary function constraint program file (boundary): impose boundary constraints on state variables and control variables to ensure that boundary conditions are met;

[0235] System objective function program file (mayer, lagrange): set the optimal objective function of the system, including Lagrange objective function and Mayer objective function;

[0236] System variable initialization program file (initial): initializes all state variables and control variables of the system to provide initial values ​​for the solution process;

[0237] System path constraint program file (path): specifies path constraints to ensure that the motion trajectory of the AUV meets the path constraint conditions;

[0238] System drawing program file (plot): draws the images required for optimal control problems to help analyze and verify the results;

[0239] PS_Solve function: By calling this function to solve the nonlinear programming problem, the dynamic docking trajectory of the AUV under different working conditions is obtained;

[0240] The flowchart for solving the optimal control problem using the PS method is as follows:

[0241] Flowchart: as attached Figure 6 Through the guidance of specific flowcharts, the algorithm implementation steps are shown in detail from inputting the optimization problem to solving the final optimal trajectory;

[0242] Step 7: System integration and testing:

[0243] Hardware integration: embed the optimized control strategy into the AUV control system to ensure seamless integration between software and hardware;

[0244] Environmental adaptability test: Conduct docking tests in simulated and real marine environments to verify the stability and robustness of the system;

[0245] Performance evaluation: Evaluate system performance through key indicators such as docking success rate, docking time, and energy consumption, and adjust the optimization algorithm based on test feedback;

[0246] In summary, the underwater robot docking system achieves efficient and reliable autonomous docking capabilities in complex marine environments through sophisticated algorithm design, dynamic strategy planning, strict model verification and continuous adaptive learning, providing strong technical support for underwater exploration and operations.

[0247] The present invention is suitable for underwater robots to perform detection and operations under conditions where special operating environments such as vertical three-dimensional sections, narrow waters and pipelines are restricted.

[0248] In summary, the technical key points of the present invention are self-organizing control strategy and planning algorithm; the existing autonomous underwater docking technology is mainly based on the docking of fixed docking stations and mobile large-scale aircraft (including mother ships and mother boats); it has the defects of low flexibility and high cost; the present invention is different from other mobile docking technologies in two main aspects: First, most of the previous docking devices are carried on large ships or large unmanned surface vehicles, which do not have the concealment to operate in special underwater environments compared to the present invention. Second, most of the previous underwater docking technologies are based on large mother boats for recovery and docking, while the research object of the present invention is the front and rear catamarans of the self-organizing AUV, which has high automation requirements, better adaptability to different ocean currents, and higher docking accuracy requirements.

[0249] It should be noted that the implementation methods described in the examples do not represent all implementation methods of the present invention. They are merely examples of devices and methods consistent with some aspects disclosed in the present invention as detailed in the claims, and the scope of the present invention is not limited thereto. The features of the various embodiments of the present invention can be combined with each other without contradiction. The path planning for typical docking conditions of the present invention for same-direction docking and opposite-direction docking and the path planning for docking conditions at different angles should all be included in the scope of protection of the present invention.

[0250] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A path planning method for self-organizing AUV twin-body coordinated docking, characterized in that: The following steps are involved: Step 1: Establish a system model, including a dynamic model and boundary conditions; Step 2: Formulate the trajectory optimization problem, construct the objective function and establish corresponding constraints; Step 3: Discretize the performance index and use the Gaussian pseudo-spectral method to transform the optimal control problem into a nonlinear programming problem; Step 4: Use the KKT multiplier method to transform the boundary value problem into an initial value problem and solve it to obtain the navigation trajectory; Step 5: Adjust and optimize the trajectory.

2. A path planning method for self-organizing AUV twin-body coordinated docking according to claim 1, characterized in that: In step 1, a dynamic model is established, including state variables and control variables, and boundary conditions are set at the initial and terminal moments; the state variables include the position, velocity and attitude of the AUV, and the control variables include the thrust and torque of the propeller.

3. A path planning method for self-organizing AUV twin-body coordinated docking according to claim 2, characterized in that: In step 2, the formulation of the trajectory optimization problem includes constructing an objective function and establishing corresponding constraints; constructing an objective function, the objective function includes the shortest navigation time and the least energy consumption; establishing constraints corresponding to the objective function, the constraints include dynamics, kinematics and environmental factors; When performing trajectory optimization, the Gaussian pseudospectral method is used to numerically approximate the optimal control problem; the time interval is converted to [-1,1] through time domain transformation, the system state and control variables are approximated using Legendre polynomials and Lagrange interpolation basis functions, the system state and control variable equations are discretized, and the differential equation constraints are converted into algebraic constraints.

4. A path planning method for self-organizing AUV twin-body coordinated docking according to claim 3, characterized in that: In step 3, the discretized optimal control problem is transformed into a nonlinear programming problem and solved using appropriate numerical methods; appropriate numerical methods include penalty function method and sequential quadratic programming method to deal with optimization problems with constraints.

5. A path planning method for self-organizing AUV twin-body coordinated docking according to claim 4, characterized in that: In step 4, KKT conditions are a class of optimality conditions that provide an effective solution framework for constrained optimization problems.

6. A path planning method for self-organizing AUV twin-body coordinated docking according to claim 5, characterized in that: In step 5, the actual navigation trajectory of the AUV is adjusted and optimized according to the solved trajectory, including fine-tuning of speed, direction and attitude.

7. A path planning method for self-organizing AUV twin-body coordinated docking according to claim 6, characterized in that: It also includes step 6, simulation and experimental verification: The effectiveness of the trajectory optimization algorithm is verified in a simulated environment, and then experiments are carried out in an actual underwater environment to evaluate the docking accuracy and efficiency.

8. A path planning method for self-organizing AUV twin-body coordinated docking according to claim 7, characterized in that: It also includes step 7, system integration and testing: integrating all components and algorithms into a system model and conducting comprehensive testing.

Citation Information

Cited By

  • Energy-saving track generation method of under-actuated AUV (Autonomous Underwater Vehicle)

    CN119668105A

  • Fluted disc structure design method and system for docking and separation of unmanned aerial vehicle and unmanned vehicle

    CN120257487A

  • Gear disc structure design method and system for docking and separation between UAV and unmanned vehicle

    CN120257487B