A dynamic path planning method and device for underactuated AUV mobile docking

Abstract

The present invention discloses a dynamic path planning method and device for mobile docking of an under-actuated AUV, and relates to the technical field of underwater autonomous vehicle recovery. The method proposes a real-time and robust path planning method for the recovery and docking problem between an under-actuated underwater autonomous vehicle with a lack or insufficient lateral driving capability and a mobile docking device with one-way entry. In the far-end homing phase, a pure tracking method is used to track waypoints, which has the advantages of simple calculation, easy implementation, and suitability for one-way docking. In the terminal docking phase, a model predictive control method is used to perform local planning under constraints, solve the optimization problem under constraints, and obtain the planned speed and heading. The proposed method can avoid the problem of inability to obtain environmental perception information or severe control overshoot due to unreasonable planning, and can effectively improve the robustness and safety of recovery and docking of underwater autonomous vehicles in non-ideal environments.

Description

Technical Field

[0001] The present application relates to the field of underwater autonomous vehicle recovery technology, and in particular to a method and device for dynamic path planning for mobile docking of an underactuated AUV. Background Art

[0002] The strategy of building a strong maritime nation is an essential component of comprehensively building a modern socialist nation. Autonomous underwater vehicles (AUVs) are recognized as essential tools for ocean exploration, possessing significant application value in commercial and defense fields such as marine resource exploration, hydrological environment monitoring, and underwater intelligence reconnaissance. AUVs carry limited energy, so after a period of operation, they must be recovered for energy replenishment. Recovery technology is key to guiding AUVs operating in the marine environment into docking devices. It ensures a physical connection between the AUV and the charging device within the docking device for subsequent charging and data exchange. Currently, the development of underwater docking systems primarily focuses on static docking systems, with numerous successful attempts and explorations both domestically and internationally. However, static recovery and docking methods significantly limit the AUV's navigation range and hinder its long-range operation. Therefore, the development of mobile docking systems, using docking devices mounted on mobile ships, large underwater robots, or mobile carriers, is a major trend in the development of underwater robot recovery and docking technology.

[0003] Currently, the exploration of mobile recovery and docking technologies is still in its infancy, with research primarily focused on environmental perception and robotic control. Planning methods are relatively understudied. The methods currently used in existing recovery and docking systems are primarily based on point-to-point guidance laws. These methods leverage geometric relationships to calculate the desired heading at the next moment in real time from the relative pose relationship between the AUV and the target. Common planning methods based on point-to-point guidance laws include proportional guidance, pure pursuit guidance, and line-of-sight guidance. Guidance-law-based planning algorithms are computationally inefficient and can respond to the environment in real time. However, these methods fail to consider the AUV's motion execution capabilities and struggle to meet stringent docking device entry requirements in the presence of significant environmental interference. Other publicly available research focuses on methods such as graph search, random search tree sampling, curve interpolation, and intelligent optimization. However, these methods are time-consuming and difficult to implement in real time. Terminal path planning based on vision positioning is typically implemented using visual servoing. Position deviations are calculated through visual positioning and then directly transmitted to the controller to drive the corresponding thrusters for deviation correction. This method relies on the fully actuated robot's lateral thrusters for rapid deviation correction and is not suitable for underactuated robots.

[0004] During the implementation of the present invention, the inventors discovered at least the following issues with the existing technology: 1. Planning methods supporting real-time implementation fail to consider the actual execution capabilities of underactuated AUVs, potentially resulting in unreasonable planning results that prevent convergence errors or control overshoot. 2. Path planning ignores the AUV's need for environmental perception, potentially leading to loss of visual positioning data during navigation and, consequently, docking failures. 3. Planning methods for mobile recovery docking tasks fail to fully utilize the adjustable relative speed of mobile docking. Summary of the Invention

[0005] The purpose of the embodiments of the present application is to provide a dynamic path planning method and device for mobile docking of an underactuated AUV, as well as electronic equipment, to address the technical issues in the mobile recovery docking problem, such as the real-time planning method not considering the actual execution capability of the AUV, not considering the environmental perception requirements during the docking process, and not fully utilizing the advantages of mobile docking, thereby effectively improving the robustness of the mobile recovery docking planning method.

[0006] According to a first aspect of an embodiment of the present application, a method for dynamic path planning for mobile docking of an underactuated AUV is provided, comprising:

[0007] S101: The AUV autonomously navigates to the docking waters, and after entering the docking waters, begins circling from the current position, and obtains the position of the mobile docking device through acoustic positioning data;

[0008] S102: The AUV and the mobile docking device establish a connection through acoustic signals, so that the mobile carrier equipped with the mobile docking device starts constant speed and directional navigation at a predetermined docking depth within the docking waters;

[0009] S103: The AUV pursues the waypoint in a pure tracking and guidance manner according to the fused navigation data information to approach the mobile docking device, wherein the fused navigation data information is obtained by fusing information provided by the inertial navigation system and the acoustic sensor;

[0010] S104: When the camera on the AUV captures the mobile docking device, the AUV performs local planning under constraints based on the model predictive control framework according to the positioning data obtained by visual positioning, solves the optimization problem under the constraints, and obtains the navigation speed and direction at the next moment;

[0011] S105: After step S104 is completed, it is determined whether the AUV meets the entry conditions of the mobile docking device. If the entry conditions are met and the AUV successfully enters the mobile docking device, a successful docking signal is sent to the decision module; otherwise, the vector direction from the current position of the AUV to the position of the target device is checked. If the vector direction is reversed between adjacent sampling intervals, it means that the current position of the AUV has exceeded the mobile docking device, and it is determined that the docking has failed, and a docking failure signal is sent; otherwise, the process returns to step S104 to continue docking.

[0012] Furthermore, before step S105, the method further includes:

[0013] S106: If the mobile docking device is lost from the field of view of the AUV during step S104, the AUV immediately decelerates according to the maximum deceleration to a speed lower than the minimum navigation speed allowed by the mobile docking device when the field of view is lost, and navigates towards the predicted position of the mobile docking device using a pure tracking method until the mobile docking device returns to the field of view of the AUV, and then returns to step S104.

[0014] Furthermore, the process of obtaining the predicted position of the mobile docking device includes:

[0015] The target pose is obtained from the last solution when the visual positioning information exists. The position of the mobile docking device during the period when the AUV field of view is lost is predicted:

[0016]

[0017] Among them, Dock x，Last , Dock y，Last , Dock ψ，Last It is the position and heading information of the mobile docking device obtained by the last calculation when the visual positioning information exists. u represents the fixed navigation speed of the mobile docking device during docking, Δt loss It is the time difference from the moment when visual information is lost to the current planning moment.

[0018] Furthermore, step S104 includes:

[0019] Establish kinematic and dynamic models of AUV under the influence of ocean currents;

[0020] Discretizing the kinematic model and the dynamic model, and predicting the state of the AUV at a future moment based on the discretized models;

[0021] Calculating the position and posture of the mobile docking device based on the image captured by the camera;

[0022] Predicting the position of the mobile docking device at a future time based on the current position information of the mobile docking device obtained after performing position and position calculation on the image, combined with the prerequisite of the mobile docking device's constant speed and direction navigation;

[0023] Establishing an objective function between the AUV and the mobile docking device under the conditions of position convergence, heading convergence, and speed convergence based on the state at the future moment predicted by the discretization model and the predicted position and posture of the mobile docking device at the future moment;

[0024] Establish constraints on AUV motion capabilities;

[0025] Establish field of view continuous tracking constraints and introduce control redundancy considerations;

[0026] The objective function minimization optimization problem under the constraints of AUV motion capability and continuous field of view tracking is solved to obtain the navigation speed and direction at the next moment.

[0027] Furthermore, based on the current posture information of the mobile docking device obtained after performing posture calculation on the image, combined with the prerequisite of the mobile docking device's constant speed and direction navigation, the position of the mobile docking device at a future time is predicted, including:

[0028]

[0029] Among them, Dock x，i ,Dock y，i ,Dock ψ，i , respectively represent the position and heading information of the mobile docking device at time i, Dock u represents the fixed navigation speed of the mobile docking device during docking, and Δt represents the sampling interval.

[0030] Furthermore, the objective function is:

[0031]

[0032] Among them, J1 = ΔP′Q1ΔP, J2 = Δψ′Q2Δψ, J3 = Δu′Q3Δu respectively represent the corresponding requirements of position convergence, heading convergence, and speed convergence, w1, w2, and w3 are the weighted coefficients of each indicator, among which ΔP, Δψ, and Δu respectively represent the position deviation, heading deviation, and speed deviation between the AUV and the mobile docking device, and Q1, Q2, and Q3 correspond to the weight coefficient matrices of each optimization indicator.

[0033] Furthermore, the posture of the mobile docking device is calculated based on the image captured by the camera to continuously track the field of view of the mobile docking device, including:

[0034] Acquiring an image of a light source on the mobile docking device captured by a camera, wherein two layers of positioning light arrays are installed on the outer and inner sides of a guide cover of the mobile docking device, the outer positioning light array being used for long-distance visual guidance, and the inner positioning light array being used for close-range visual guidance;

[0035] The position and posture of the mobile docking device are calculated according to the light source in the image.

[0036] According to a second aspect of an embodiment of the present application, a dynamic path planning device for mobile docking of an underactuated AUV is provided, comprising:

[0037] The circle navigation module is used for the AUV to autonomously navigate to the docking waters, and after entering the docking waters, it starts circling from the current position and obtains the position of the mobile docking device through acoustic positioning data;

[0038] A connection establishment module is used to establish a connection between the AUV and the mobile docking device through acoustic signals, so that the mobile carrier equipped with the mobile docking device starts constant speed and direction navigation at a predetermined docking depth in the docking waters;

[0039] A tracking and approaching module is used for the AUV to chase the waypoint in a pure tracking and guidance manner based on the fused navigation data information to approach the mobile docking device, wherein the fused navigation data information is obtained by fusing information provided by the inertial navigation system and the acoustic sensor;

[0040] A local planning module is used when the camera on the AUV captures the mobile docking device. The AUV performs local planning under constraints based on the model predictive control framework according to the positioning data obtained by visual positioning, solves the optimization problem under the constraints, and obtains the navigation speed and direction at the next moment;

[0041] The signal sending module is used to determine whether the AUV meets the entry conditions of the mobile docking device after the local planning module is executed. If the entry conditions are met and the AUV successfully enters the mobile docking device, a successful docking signal is sent to the decision module; otherwise, the vector direction from the AUV's current position to the target device position is checked. If the vector direction reverses between adjacent sampling intervals, it means that the AUV's current position has exceeded the mobile docking device, and the docking is determined to have failed, and a docking failure signal is sent; otherwise, the docking continues.

[0042] According to a third aspect of the embodiments of the present application, there is provided an electronic device, including:

[0043] one or more processors;

[0044] a memory for storing one or more programs;

[0045] When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in the first aspect.

[0046] According to a fourth aspect of an embodiment of the present application, a computer-readable storage medium is provided, on which computer instructions are stored. When the instructions are executed by a processor, the steps of the method described in the first aspect are implemented.

[0047] The technical solutions provided by the embodiments of the present application may have the following beneficial effects:

[0048] It can be seen from the above embodiments that the present application uses different guidance methods according to the different requirements of the mission stage. The pure tracking method is used in the far-end homing stage to track multiple waypoints to achieve the requirements of the AUV quickly approaching the mobile docking device and achieving rough alignment of the heading. It has small computational complexity, low energy consumption and is conducive to the terminal capturing visual positioning data. The terminal docking stage adopts the model predictive control method to directly add environmental perception requirements, AUV motion execution capabilities and environmental interference to the planning problem. By solving the local planning problem, the normal operation of the visual positioning module in the docking stage is guaranteed, and the under-actuated AUV can respond to environmental changes in a timely manner to resist environmental interference and give planning results that are consistent with the actual motion capabilities of the AUV.

[0049] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0051] Figure 1 The present invention is a flowchart showing a method for dynamic path planning of mobile docking of an underactuated AUV according to an exemplary embodiment.

[0052] Figure 2 The present invention is a funnel-shaped mobile docking device mounted under a moving vessel according to an exemplary embodiment.

[0053] Figure 3 The present invention is a camel-type mobile docking device carried on a large underwater robot according to an exemplary embodiment.

[0054] Figure 4 1 is a schematic diagram of the waypoint tracking trajectory during the homing phase of simulation 1 in a current-free environment according to an exemplary embodiment.

[0055] Figure 5 2 is a schematic diagram of a waypoint tracking trajectory during the homing phase in a simulated lateral flow environment according to an exemplary embodiment.

[0056] Figure 6 FIG. 1 is a flowchart of step S104 according to an exemplary embodiment.

[0057] Figure 7 The figure is a schematic diagram of a funnel-shaped docking device using double-layer guide lights according to an exemplary embodiment.

[0058] Figure 8 3 is a schematic diagram of the terminal recovery docking trajectory in a simulated lateral flow environment according to an exemplary embodiment.

[0059] Figure 9 3 is a schematic diagram showing the change in forward speed of an AUV in simulation 3 according to an exemplary embodiment.

[0060] Figure 10 3 is a schematic diagram of AUV heading change in simulation 3 according to an exemplary embodiment.

[0061] Figure 11 FIG3 is a schematic diagram of the lateral speed change of an AUV in Simulation 3 according to an exemplary embodiment.

[0062] Figure 12 The present invention is a flowchart showing a method for dynamic path planning of mobile docking of an underactuated AUV according to an exemplary embodiment.

[0063] Figure 13 The present invention is a block diagram showing a dynamic path planning device for mobile docking of an under-actuated AUV according to an exemplary embodiment. DETAILED DESCRIPTION

[0064] Exemplary embodiments will be described in detail herein, with examples illustrated in the accompanying drawings. In the following description, when referring to the drawings, identical numerals in different figures represent identical or similar elements, unless otherwise indicated. The embodiments described in the following exemplary embodiments are not intended to represent all embodiments consistent with the present application. Rather, they are merely examples of apparatus and methods consistent with certain aspects of the present application, as detailed in the appended claims.

[0065] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. As used in this application and the appended claims, the singular forms "a," "an," "the," and "the" are intended to include the plural forms, unless the context clearly indicates otherwise. It should also be understood that the term "and / or" as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.

[0066] It should be understood that although the terms first, second, third, etc. may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from each other. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0067] Glossary: ​​Underactuated AUV: An AUV with fewer actuators than degrees of freedom. During horizontal motion, an underactuated robot lacks lateral thrusters and cannot directly control the thrust to move the AUV left or right.

[0068] Mobile docking device: a docking device carried on a ship with mobility, a large underwater robot or a mobile carrier.

[0069] Figure 1 FIG. 1 is a flow chart showing a method for planning a mobile docking path for an underactuated AUV according to an exemplary embodiment. Figure 1 As shown, the method is applied to an underactuated AUV and may include the following steps:

[0070] Step S101: The AUV autonomously navigates to the docking waters, and after entering the docking waters, it starts circling from its current position and obtains the position of the mobile docking device through acoustic positioning data;

[0071] Step S102: The AUV and the mobile docking device establish a connection through acoustic signals, so that the mobile carrier equipped with the mobile docking device starts constant speed and directional navigation at a predetermined docking depth within the docking waters;

[0072] Step S103: The AUV pursues the waypoint in a pure tracking guidance mode according to the fused navigation data information to approach the mobile docking device, wherein the fused navigation data information is obtained by fusing the information provided by the inertial navigation system and the acoustic sensor; the simulation examples of the guidance method in this stage under the conditions of no sea current and with side current interference are as follows Figure 4 and Figure 5 shown.

[0073] Step S104: When the camera on the AUV captures the mobile docking device, the AUV performs local planning under constraints based on the model predictive control framework according to the positioning data obtained by visual positioning, solves the optimization problem under the constraints, and obtains the navigation speed and direction at the next moment.

[0074] Step S105: After step S104 is completed, it is determined whether the AUV meets the entry conditions of the mobile docking device. If the entry conditions are met and the AUV successfully enters the mobile docking device, a successful docking signal is sent to the decision module; otherwise, the vector direction from the current position of the AUV to the position of the target device is checked. If the vector direction is reversed between adjacent sampling intervals, it means that the current position of the AUV has exceeded the mobile docking device, and it is determined that the docking has failed. A docking failure signal is sent; otherwise, the process returns to step S104 to continue docking.

[0075] It can be seen from the above embodiments that the present application uses different guidance methods according to the different requirements of the mission stage. The remote homing stage adopts a pure tracking method to track multiple waypoints to achieve the requirements of the AUV quickly approaching the mobile docking device and achieving rough alignment of the heading. It has small computational complexity, low energy consumption and is conducive to the terminal capturing visual positioning data. The terminal docking stage adopts the model predictive control method, which directly adds the environmental perception requirements, AUV motion execution capabilities and environmental interference to the planning problem. By solving the local planning problem, the normal operation of the visual positioning module in the docking stage is guaranteed, and the under-actuated AUV can respond to environmental changes in a timely manner to resist environmental interference and give planning results that are consistent with the actual motion capabilities of the AUV.

[0076] The mobile docking device described in the present invention refers to a docking device that is mounted on a mobile vehicle such as a ship, a large underwater robot or a mobile carrier and moves with it. The mobile docking device applicable to this method is a docking device that only supports one-way entry, such as a funnel-type mobile docking device and a camel-type mobile docking device, respectively. Figure 2 、 Figure 3 As shown. The "mobile" part of the mobile docking device refers to the fact that it also includes an acoustic sensor device for providing positioning data and a guidance light source for visual positioning. One arrangement of the guidance light sources involves placing four positioning light sources in a square pattern at the docking entrance for visual positioning. The acoustic positioning data is provided by an ultra-short baseline sensor.

[0077] In the specific implementation of step S101, the AUV autonomously navigates to the docking waters, and after entering the docking waters, it starts circling from the current position and obtains the position of the mobile docking device through acoustic positioning data;

[0078] Specifically, circular navigation is due to the limited minimum turning radius of underactuated AUVs, which prevents them from rotating in place. Circular navigation involves the AUV turning from its current position at a fixed speed and a constant angular velocity. This rotation allows the AUV to search for acoustic positioning data within a 360-degree range, achieving optimal positioning results.

[0079] If the duration of this phase exceeds the maximum allowed positioning time limit, the docking is considered to have failed, and a docking failure signal is uploaded to the decision center, terminating the docking attempt. The decision center is the AUV's task allocation center and plays the role of task scheduling in the multi-task system.

[0080] Among them, acoustic positioning data is usually provided by ultra-short baseline sensor data or long baseline sensor data.

[0081] In the specific implementation of step S102, the AUV and the mobile docking device establish a connection through acoustic signals, so that the mobile carrier equipped with the mobile docking device starts constant speed and directional navigation at a predetermined docking depth in the docking waters;

[0082] Specifically, the target vehicle, equipped with a mobile docking device, initially hovers within a designated safe docking area using dynamic positioning. Because underactuated vehicles are less resistant to side currents, the initial heading for directional navigation is set against the current to minimize adverse interference from lateral currents during near-end docking. The docking device's constant speed and directional movement facilitates the acquisition of positioning data by the AUV during docking, helping to reduce docking difficulty.

[0083] In a specific implementation of step S103, the AUV pursues the waypoint in a pure tracking guidance manner according to the fused navigation data information to approach the mobile docking device, wherein the fused navigation data information is obtained by fusing information provided by the inertial navigation system and the acoustic sensor;

[0084] Specifically, this step is performed during the far-end homing phase. This phase refers to the period from the AUV entering the recovery area until it acquires visual positioning data while approaching the target docking device. During this phase, the AUV is far from the mobile docking device, allowing for a wide tolerance range. Therefore, a simple, computationally inefficient pure tracking algorithm is suitable for planning. This method directly steers towards the mobile docking device and ensures optimal acoustic positioning.

[0085] To further explain, the heading planning in this stage adopts a pure tracking method, that is, sailing in the direction of the line of sight angle towards the predetermined target point. The pure tracking method has the characteristic of approaching from the tail, and can achieve approach from the tail of the mobile docking device, which meets the one-way docking requirement that the docking device can only enter from the tail. One-way docking means that the AUV can only enter the mobile docking device from a single entrance direction, mainly for funnel-type docking devices and camel-type docking devices. The one-way docking device is mainly different from another commonly used docking device form: the fork-rod docking device. The fork-rod docking device can support the AUV to approach the docking device from any direction to complete the docking. The goal of this stage is to approach the mobile docking device from the tail as quickly as possible, so it is sufficient to sail at a fixed depth according to the agreed depth. In summary, the expected navigation, expected speed, and expected depth instructions for the next moment obtained from the planning of this stage are shown in the following formula.

[0086]

[0087] where ψ next ,u next , z next Respectively represent the desired heading, desired speed, and desired depth at the next moment. (x, y) represents the current position of the AUV, (P x , P y ) represents the position of the target waypoint. Since the depth of the target docking device is known, the expected depth at this stage is directly calculated as the fixed depth z fixed Depth-fixed navigation. In order to save energy, this stage directly follows the fixed speed u fixed When the AUV reaches a certain range of the preset target point, it is considered to have reached the target point and the target point is switched to the next target point.

[0088] Among them, in order to make the heading of the AUV during the approach process aligned with the moving direction of the docking device as much as possible to ensure that the AUV can detect the visual positioning data during the approach process, two waypoints are added directly behind the moving direction of the mobile docking device to achieve heading alignment.

[0089] In one embodiment, the waypoints are designed to be 100 meters behind the moving direction of the mobile docking device, 50 meters behind the mobile docking device, and at the mobile docking device, for a total of three waypoints. By continuously switching the waypoints during the tracking process, the heading angle deviation between the AUV and the target docking device will gradually decrease during the approach process, which is conducive to the acquisition of subsequent visual positioning data. Under the above waypoint settings, the simulation results of this method in an environment without ocean currents and in an environment with side current interference are shown in the attached figure. Figure 4 and attached Figure 5 As shown. Figure 4The initial position of the AUV is set to (0,0), the initial heading is due east, and the initial speed is 2m / s. The initial position of the mobile docking device is at (200,30), the initial heading is also due east, and it navigates at a speed of 1m / s. Through the planning method described in the specific implementation of step S103, the AUV successfully approaches the mobile docking device in an environment without current interference and fully achieves heading alignment. Figure 5 The initial parameter settings are the same as in the attached Figure 4 , with attached Figure 4 The difference is that there is a 0.3m / s ocean current interference in the docking waters in the south direction. Figure 5 It can be seen from the figure that in an environment with ocean currents, although the navigation trajectory is not smooth enough when switching waypoints, it can still approach the docking device and capture visual positioning information under the guidance of the proposed method, which proves the robustness of the method in an environment with ocean current interference. Figure 5 As can be seen in the figure, at the end of the homing phase, there is still a certain distance between the AUV and the mobile docking device. The remaining heading and distance deviations will be processed in the terminal docking phase.

[0090] In the specific implementation of step S104, when the camera on the AUV captures the mobile docking device, the AUV performs local planning under constraints based on the model predictive control framework according to the positioning data obtained by visual positioning, solves the optimization problem under the constraints, and obtains the navigation speed and direction at the next moment;

[0091] Specifically, the mobile docking device adopts a funnel-shaped docking device with a trumpet-shaped guide. The funnel-shaped docking device has the advantages of not requiring modification of the AUV, not affecting the fluid mechanics characteristics of the vehicle, and being able to tolerate a certain trajectory error. However, the docking device can only allow one-way docking, and has certain requirements for the position and heading errors at the time of end contact. The update frequency of acoustic positioning data is low, and the cumulative error of inertial navigation data is large, so the loss of visual positioning data may lead to the failure of the docking process. Among them, the scope of this step is the process from the time the AUV captures the visual positioning data from the camera to the successful entry into the docking device. Figure 6 As shown, this step may include the following sub-steps:

[0092] Step S201: establishing a kinematic model and a dynamic model of the AUV under the influence of ocean currents;

[0093] Among them, the environmental interference during the docking process refers to ocean current interference and sudden unknown disturbances. Although the target docking device sails against the current at the beginning of the docking, due to factors such as the sailing distance, the time-varying nature of the ocean current, and the interference of the wake, there may still be small side current interference during the docking. Therefore, the AUV horizontal plane kinematics and dynamics model is first established under the influence of ocean currents in order to describe the motion of the underactuated AUV under the interference of the ocean current environment. The kinematic model and dynamics model under the interference of the ocean current environment are shown as follows:

[0094]

[0095]

[0096] Among them, V x , V y Represent the components of the ocean current in the x and y directions in the world coordinate system. To simplify the problem, it is assumed that the ocean current is a steady irrotational flow. Therefore, this component is a scalar. Since the forecast planning needs to be re-performed at each sampling interval, the assumption of steady flow at a single moment is reasonable. r , v r τ represents the forward velocity and lateral velocity of the AUV relative to the ocean current, ψ represents the current direction of the AUV, and r is the acceleration of the AUV. u , τ r They represent the propulsion force of the AUV in forward motion and turning motion respectively. From the dynamic model, it can be intuitively seen that there is a lack of direct driving force for the lateral speed. Both represent the forces introduced due to fluid mechanics characteristics. represents the differential constraint of the corresponding parameter.

[0097] Step S202: discretizing the kinematic model and the dynamic model, and predicting the state of the AUV at a future moment based on the discretized models;

[0098] Specifically, the actual system control instructions are sent to the actuators in a discrete manner. Therefore, the constraints represented by the equations of motion are discretized to predict the AUV's state at future moments. The resulting prediction equations are expressed as follows. To reduce the computational complexity during planning, the dynamic equations are simplified, and the dynamic equations corresponding to angular velocity are ignored.

[0099]

[0100] Where x(k), y(k), and ψ(k) represent the state of the AUV at time k, representing the position along the x-axis, the position along the y-axis, and the heading in the navigation coordinate system, respectively. u(k) and v(k) represent the forward velocity and lateral velocity of the AUV in the carrier coordinate system. a(k) and r(k) are the control parameters of the AUV at time k, representing the forward acceleration and yaw angular velocity of the AUV in the carrier coordinate system. Δt represents the sampling interval. x(k+1), y(k+1), and ψ(k+1) represent the corresponding state of the AUV at time k+1.

[0101] Step S203: Calculating the position and posture of the mobile docking device based on the image captured by the camera.

[0102] In one embodiment, the part of performing pose calculation based on the image collected by the camera, i.e., visual positioning, is obtained by calculating the image data captured by the camera carried by the AUV using the EPnP algorithm (Kirk JT O.Light and photosynthesis in aquatic ecosystems[M].Cambridge University Press,1994.). In a specific implementation, the image pose calculation can also be implemented by the LineMode algorithm (HodaňT,Zabulis X,Lourakis M,et al.Detection and fine 3D pose estimation of texture-less objects in RGB-D images[C] / / 2015IEEE / RSJ International Conference on Intelligent Robots and Systems(IROS).IEEE,2015:4421-4428.) and the PvNet algorithm (Peng S,Zhou X,Liu Y,etal.Pvnet:pixel-wise voting network for 6dof object pose estimation[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2020.).

[0103] Step S204: Based on the current posture information of the mobile docking device obtained after performing posture calculation on the image, combined with the prerequisite of the mobile docking device's constant speed and direction straight-line navigation, the position of the mobile docking device at a future time is estimated;

[0104] Specifically, this step is inferred by the following formula:

[0105]

[0106] Among them, Dock x，i ,Dock y，i ,Dock ψ，i , respectively represent the position and heading information of the mobile docking device at time i, Dock u represents the fixed navigation speed of the mobile docking device during docking. Δt represents the sampling interval.

[0107] Step S205: establishing an objective function between the AUV and the mobile docking device under the conditions of position convergence, heading convergence, and speed convergence based on the state at the future time predicted by the discretization model and the predicted position and posture of the mobile docking device at the future time;

[0108] Specifically, the objective function is shown in the following formula:

[0109]

[0110] J1=ΔP'Q1ΔP

[0111] J2=Δψ'Q2Δψ

[0112] J3=Δu'Q3Δu

[0113] Among them, J1=ΔP′Q1ΔP, J2=Δψ′Q2Δψ, J3=Δu′Q3Δu respectively represent the corresponding requirements of position convergence, heading convergence, and speed convergence. w1, w2, w3 are the weighted coefficients of each indicator, among which ΔP, Δψ, Δu respectively represent the position deviation, heading deviation, and speed deviation between the AUV and the mobile docking device. Q1, Q2, Q3 correspond to the weight coefficient matrix of each optimization indicator,

[0114] The objective function converts the multi-objective optimization problem into a single-objective optimization problem through linear weighting. Due to the large scale differences between the various indicators, the weighting coefficients need to be scaled to the same scale to prevent a single objective from dominating and rendering other sub-objectives ineffective. The position convergence, velocity convergence, and heading convergence of an underactuated vehicle under the interference of ocean currents are conflicting objective parameters. It is difficult to achieve the minimum value at the same time under the premise of local optimization. Therefore, according to the tolerance requirements of the funnel-shaped docking device, the constraint coefficients for heading and speed are appropriately weakened, and position convergence is prioritized while satisfying the constraints. Q1, Q2, and Q3 correspond to the weight coefficients of each optimization indicator, and the corresponding weights can be assigned according to the mission requirements.

[0115] The specific calculation of each deviation at time i is as follows. i ,y i, ψ i ,u i Respectively represent the current status information of AUV at time i, Dock x，i ,Dock y，i ,Dock ψ，i ,Dock u，i Respectively represent the status information of the mobile docking device at time i.

[0116]

[0117] To further clarify, position convergence means that the AUV must reach the entrance of the mobile docking device at the docking end. Heading convergence means that the funnel-shaped docking device only allows one-way entry. Therefore, the heading during docking and the docking device entrance orientation must not deviate beyond a certain range. Speed ​​convergence means that the relative speed between the AUV and the mobile target during approach must not be too high to prevent a strong collision upon contact. The successful docking conditions defined by the funnel-shaped docking device are shown in the following formula.

[0118] |P AUV -P dock |≤ε p

[0119] |ψ AUV -ψ dock |≤ε ψ

[0120] |u AUV -u dock |≤εu

[0121] Among them, ε p , ε ψ , ε u They represent the redundancy of the docking device for position, heading and speed deviations respectively, and these parameters are determined by the design parameters of the docking device.

[0122] Step S206: Establishing AUV motion capability constraints;

[0123] Specifically, the AUV's motion capability constraints mean that the actuator indirectly changes the control and state variables by varying the driving force, preventing sudden changes in the control variables without external interference. This means that the planned trajectory must comply with the corresponding limits on angular velocity, acceleration, and speed. Given the short docking distance, the generated trajectory must fully consider the AUV's motion capability to avoid overshoot caused by the AUV's inability to track the target trajectory; this can result in deviations that cannot be corrected in a short period of time.

[0124] Furthermore, due to the limited driving force, underactuated AUVs can only control their position through forward movement and steering. Therefore, when operating in a disturbing marine environment, AUVs typically have a minimum speed limit to ensure they can withstand current disturbances. Furthermore, lateral velocity is a function of kinematic coupling and is not directly controlled by the driving force, so it is not explicitly constrained. The above-mentioned AUV motion constraint is expressed as follows.

[0125]

[0126] Step S207: Establishing field of view continuous tracking constraints and taking control redundancy into consideration.

[0127] Specifically, establishing the field of view continuous tracking constraint is achieved by performing posture calculation of the mobile docking device based on the image captured by the camera, which may include the following steps:

[0128] Step S301: Acquire an image of a light source on the mobile docking device captured by a camera, wherein two layers of positioning light arrays are installed on the outer and inner sides of a guide cover of the mobile docking device, the outer positioning light array being used for long-distance visual guidance, and the inner positioning light array being used for close-range visual guidance;

[0129] Specifically, Figure 7 A schematic diagram of a dual-layer positioning light array installed on a conical docking device is provided. Existing docking device positioning light arrays are only installed on the outside of the docking device, corresponding to the outer light array in the diagram. However, due to limited close-range vision, an AUV cannot capture images from the outer light array to calculate positioning information when very close to the docking device entrance. In this case, it must rely on acoustic positioning data for continued guidance. During entry, positioning errors may cause the AUV to deviate from the entrance, further preventing it from entering the docking device. Therefore, to ensure the continuity of visual positioning throughout the entire end-to-end docking process and without affecting the range of the existing outer light array, an inner layer of guiding light sources is added to the docking device. This allows for visual positioning to be achieved by capturing positioning images from the inner light sources when the outer light source cannot be captured at close range. This visual positioning information can then be used to guide the AUV directly into the docking device storage compartment, ensuring successful docking. This dual-layer light guidance scheme, by adding a layer of positioning lights to the mobile docking device, addresses the problem of visual positioning data loss caused by the AUV's limited close-range vision. This allows the AUV to be fully guided into the mobile docking device under visual guidance.

[0130] Step S302: Calculate the position of the mobile docking device based on the light source in the image

[0131] Visual positioning uses the method described in S1043 to calculate the pose data of multiple light sources on the conical docking device, captured by a camera mounted on the bow of the AUV. Acoustic positioning data has a long update cycle, and during mobile docking, the complex relative motion between the transceiver and the transmitter can affect the accuracy and bit error rate of acoustic positioning data. Therefore, at the docking end where precise pose information is required, visual positioning, with its high precision, fast data update rate, and high accuracy, is used as the primary positioning method.

[0132] Among them, continuous visual tracking means that in order to ensure the continuous acquisition of visual positioning data, the AUV needs to have the ability to perceive the visual target at all times when approaching the target, that is, all light sources used for positioning installed on the docking device must be within the field of view of the AUV camera.

[0133] To further illustrate, continuous visual positioning data acquisition requires that during the AUV's driving process, the angle between the line of sight from the current position to all light sources on the light array and the AUV's current heading is less than half of the camera's opening angle, as shown in the following formula.

[0134] abs(atan((P light_y，i -y) / (P light_x，i -x))-ψ)≤visionAngle

[0135] in represents the position coordinates of N light sources, visionAngle is half of the viewing angle of the bow camera, and (x, y, ψ) represents the current position and heading of the AUV, respectively.

[0136] When the AUV maintains continuous visual tracking while approaching the docking device, it can simultaneously ensure safety during the terminal docking process. The safety of terminal docking is also ensured through continuous field of view tracking, avoiding safety issues caused by the AUV colliding with the docking device from a non-entrance direction due to positioning errors. Since the recovery docking waters are artificially selected safe docking waters, there are no obstacles to interfere. The safety issues at this stage are introduced due to unreasonable collisions between the AUV and the mobile docking device. Continuous field of view tracking is a favorable guarantee for docking safety during terminal docking, and can absolutely ensure that the AUV does not collide with the non-guide port part of the target docking device, so as to avoid damage to the AUV and the docking device.

[0137] Specifically, due to the complexity of AUV motion in the ocean, it is difficult to accurately represent AUV motion in real-world environments using a predictive model. Factors such as communication latency, control deadband voltage, and control error can all cause a certain deviation between the AUV's actual motion and the prediction. This is known as a model mismatch. To ensure that the AUV can successfully dock even in the presence of a certain degree of model mismatch, the continuous field of view tracking constraint is scaled by a factor of 0.7. This allows the AUV to meet the continuous field of view tracking requirement even with certain deviations during actual mission execution. The corresponding change in control redundancy is shown in the following equation.

[0138] abs(atan((P light_y，i -y) / (P light_x，i -x))-ψ)≤visionAngle·0.7

[0139] Step S208: Solve the objective function minimization optimization problem under the AUV motion capability constraint and continuous field of view tracking constraint to obtain the navigation speed and direction at the next moment.

[0140] After these steps, a complete constrained nonlinear optimization problem is established. The large-scale nonlinear optimization solver IPOPT is used to solve the optimization problem at each planning time, obtain the speed and heading instructions for the current planning time, and send the planning information to the vehicle for execution.

[0141] IPOPT is a large-scale open-source nonlinear optimization software based on the interior-point algorithm. This algorithm is a classic method for solving nonlinear problems. It transforms the original nonlinear programming problem into a new one consisting only of equality constraints by converting the inequality constraints in the nonlinear optimization problem into a penalty term and adding it to the optimization objective function. This makes problem solving much easier.

[0142] By solving the velocity and heading at each sampling moment during the docking process, local planning is performed at the current location, enabling docking in non-ideal environments. Because local planning re-plans using the latest available position information after each iteration, this method is able to respond promptly to changes in the environment and its own position. Furthermore, the planning process predicts future behavior, enabling timely deceleration to ensure sufficient relative distance for position correction.

[0143] The simulation trajectory of the terminal docking in the presence of lateral flow is shown in the attached figure. Figure 8 As shown. Figure 8In the simulation, the docking device sails at a constant speed of 1.5m / s in the east direction, and the initial position is (20,10). The initial position of the AUV is (0,0), initially facing the east direction, and the initial speed is 2m / s. In this simulation, the sampling interval Δt is set to 0.1 seconds. Through the terminal docking planning method described in S104, the AUV successfully enters the mobile docking device in the presence of a 0.3m / s ocean current environment in the south direction. The speed value planned in this planning process is shown in the attached figure. Figure 9 As shown, the heading is as shown in the attached Figure 10 As shown in the attached figure, the lateral velocity Figure 11 As shown. Figure 9 It can be seen that the total docking time for this docking is 24.1 seconds. The maximum permissible speed of the AUV during navigation is set to 2.5m / s, and the minimum navigation speed is set to 1m / s. Figure 9 As shown in the figure, the AUV sails at the maximum speed allowed during the docking process for most of the time to achieve rapid docking. There are two speed reductions in the figure. The first is the active speed reduction in the initial stage to adjust the heading and maintain continuous field of view tracking. The second speed reduction is to achieve the optimization goal of speed convergence. Due to the coupled nature of the underactuated vehicle and the existence of lateral ocean current interference, the speed reduction is not conducive to the AUV's heading adjustment. The AUV then completed the heading convergence index by increasing the sailing speed. Appendix Figure 10 It is the heading change of the AUV during the entire docking process. From the planning results, it can be seen that the heading change during the entire docking process is relatively gentle, which can better maintain a stable field of view tracking. At the docking end, in order to meet the docking requirements of heading convergence, the heading of the end is greatly deflected when approaching the docking device, and is basically aligned with the heading of the docking device. When the docking is successful, the heading difference between the AUV heading and the target docking device is about 1 degree. Figure 11 This is the change in the AUV's lateral velocity during the docking process. Lateral velocity is not directly controlled by the actuator but is caused by kinematic coupling. The figure shows that while high lateral velocity occurs only during the significant turn, it is relatively small at all other times, making its impact on the AUV's navigation almost negligible.

[0144] In the specific implementation of step S105, after step S104 is executed, it is determined whether the AUV meets the entry conditions of the mobile docking device. If the entry conditions are met and the AUV successfully enters the mobile docking device, a successful docking signal is sent to the decision module; otherwise, the vector direction from the current position of the AUV to the position of the target device is checked. If the vector direction is reversed between adjacent sampling intervals, it means that the current position of the AUV has exceeded the mobile docking device, and it is determined that the docking has failed, and a docking failure signal is sent; otherwise, the process returns to step S104 to continue docking.

[0145] Among them, the decision module is the task allocation center of AUV and plays the role of task scheduling in the multi-task system.

[0146] Specifically, if Figure 12 As shown, before step S105, the following steps may also be included:

[0147] Step S106: If the mobile docking device is lost from the field of view of the AUV during step S104, the AUV immediately decelerates according to the maximum deceleration to a speed lower than the minimum navigation speed allowed by the mobile docking device when the field of view is lost, and navigates towards the predicted position of the mobile docking device using a pure tracking method until the mobile docking device returns to the field of view of the AUV, and then returns to step S104.

[0148] Specifically, the process of obtaining the predicted mobile docking device position includes:

[0149] The target pose is obtained from the last solution when the visual positioning information exists. The position of the mobile docking device during the period when the AUV field of view is lost is predicted:

[0150]

[0151] Among them, Dock x，Last , Dock y，Last , Dock ψ，Last It is the position and heading information of the mobile docking device obtained by the last calculation when the visual positioning information exists. u represents the fixed navigation speed of the mobile docking device during docking, Δt loss It is the time difference from the moment when visual information is lost to the current planning moment.

[0152] If the target is lost from view due to sudden interference or control errors during the docking process, the field of view tracking constraints in the local optimization problem in step S104 above become difficult to satisfy. When a feasible solution to a nonlinear optimization problem is unavailable, the solution time may exceed real-time constraints, and the correctness of the local optimal solution cannot be guaranteed. To address this issue and ensure docking safety in the event of field of view loss, the AUV immediately decelerates to a speed lower than the minimum permissible speed of the mobile docking device according to the maximum deceleration upon field of view loss. In other words, the AUV actively reduces speed to increase the relative distance from the mobile docking device, thereby expanding its field of view. Furthermore, since the AUV's speed after deceleration is lower than the target docking device, collisions between the AUV and the target docking device are avoided during field of view recovery, ensuring docking safety. Based on historical data, the position change of the mobile target docking device is inferred, and the AUV's current heading is adjusted toward the predicted target position to promptly restore field of view detection. The prediction method is the same as that shown in the formula in S106 above. The heading is determined as shown in the heading calculation formula in S103, replacing the waypoint with the predicted target position. Once visual information is restored, the process returns to step S104 and terminal planning is repeated. When vision loss occurs, the AUV is guided purely by tracking the predicted mobile docking device location, supplemented by active speed reduction to expand the field of view. This resolves the issue of recovering lost visual positioning data. This technology leverages the adjustable relative speed during mobile docking, enabling the AUV to restore vision and correct for close-range errors during docking, improving docking success rates in non-ideal docking waters with environmental interference.

[0153] Figure 13 1 is a block diagram of a dynamic path planning device for underactuated AUV mobile docking according to an exemplary embodiment. Figure 13 , the device is applied to an underactuated AUV and may include:

[0154] The circling navigation module 21 is used for the AUV to autonomously navigate to the docking waters, and after entering the docking waters, to start circling navigation from the current position and obtain the position of the mobile docking device through acoustic positioning data;

[0155] a connection establishing module 22 for establishing a connection between the AUV and the mobile docking device through acoustic signals, so that the mobile carrier equipped with the mobile docking device starts constant speed and directional navigation at a predetermined docking depth within the docking waters;

[0156] a tracking and approaching module 23 for the AUV to chase the waypoint in a pure tracking and guidance manner to approach the mobile docking device based on the fused navigation data information, wherein the fused navigation data information is obtained by fusing information provided by the inertial navigation system and the acoustic sensor;

[0157] A local planning module 24 is configured to, when the camera on the AUV captures the mobile docking device, perform local planning under constraints based on the model predictive control framework based on the positioning data obtained by visual positioning, solve the optimization problem under the constraints, and obtain the navigation speed and direction at the next moment;

[0158] The signal sending module 25 is used to determine whether the AUV meets the entry conditions of the mobile docking device after the execution of the local planning module 24. If the entry conditions are met and the AUV successfully enters the mobile docking device, a successful docking signal is sent to the decision module; otherwise, the vector direction from the AUV's current position to the target device position is checked. If the vector direction reverses between adjacent sampling intervals, it means that the AUV's current position has exceeded the mobile docking device, and it is determined that the docking has failed. A docking failure signal is sent; otherwise, the docking continues.

[0159] Regarding the apparatus in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.

[0160] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial description of the method embodiments. The device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present application scheme. A person of ordinary skill in the art can understand and implement it without paying any creative work.

[0161] Accordingly, the present application also provides an electronic device, comprising: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement a dynamic path planning method for mobile docking of an under-actuated AUV as described above.

[0162] Correspondingly, the present application also provides a computer-readable storage medium having computer instructions stored thereon, characterized in that when the instructions are executed by a processor, a dynamic path planning method for mobile docking of an under-actuated AUV as described above is implemented.

[0163] Those skilled in the art will readily appreciate other embodiments of the present application after considering the specification and practicing the contents disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein. The description and examples are to be considered as exemplary only, and the true scope and spirit of the present application are indicated by the following claims.

[0164] It should be understood that the present application is not limited to the exact structures described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present application is limited only by the appended claims.

Claims

1. A dynamic path planning method for mobile docking of underactuated AUV, characterized by: include: S101: The AUV autonomously navigates to the docking waters, and after entering the docking waters, begins circling from the current position, and obtains the position of the mobile docking device through acoustic positioning data; S102: The AUV and the mobile docking device establish a connection through acoustic signals, so that the mobile carrier equipped with the mobile docking device starts constant speed and directional navigation at a predetermined docking depth within the docking waters; S103: The AUV pursues the waypoint in a pure tracking and guidance manner according to the fused navigation data information to approach the mobile docking device, wherein the fused navigation data information is obtained by fusing information provided by the inertial navigation system and the acoustic sensor; S104: When the camera on the AUV captures the mobile docking device, the AUV performs local planning under constraints based on the model predictive control framework according to the positioning data obtained by visual positioning, solves the optimization problem under the constraints, and obtains the navigation speed and direction at the next moment; S105: After step S104 is completed, it is determined whether the AUV meets the entry conditions of the mobile docking device. If the entry conditions are met and the AUV successfully enters the mobile docking device, a successful docking signal is sent to the decision module; otherwise, the vector direction from the current position of the AUV to the position of the target device is checked. If the vector direction is reversed between adjacent sampling intervals, it means that the current position of the AUV has exceeded the mobile docking device, and it is determined that the docking has failed, and a docking failure signal is sent; Otherwise, return to step S104 to continue docking; Step S104 includes: Establish kinematic and dynamic models of AUV under the influence of ocean currents; Discretizing the kinematic model and the dynamic model, and predicting the state of the AUV at a future moment based on the discretized models; Calculating the position and posture of the mobile docking device based on the image captured by the camera; Predicting the position of the mobile docking device at a future time based on the current position information of the mobile docking device obtained after performing position and position calculation on the image, combined with the prerequisite of the mobile docking device's constant speed and direction navigation; Establishing an objective function between the AUV and the mobile docking device under the conditions of position convergence, heading convergence, and speed convergence based on the state at the future moment predicted by the discretization model and the predicted position and posture of the mobile docking device at the future moment; Establish constraints on AUV motion capabilities; Establish field of view continuous tracking constraints and introduce control redundancy considerations; Solve the objective function minimization optimization problem under the constraints of AUV motion capability and continuous field of view tracking to obtain the navigation speed and direction at the next moment; Continuous visual tracking means that in order to ensure the continuous acquisition of visual positioning data, the AUV needs to have the ability to perceive the target visually at all times while approaching the target. That is, all light sources used for positioning installed on the mobile docking device must be within the field of view of the AUV camera. Continuous visual positioning data acquisition requires that during the AUV's driving process, the angle between the line of sight from the current position to all light sources on the light array and the AUV's current heading is less than half of the camera's opening angle, as shown in the following formula: abs(atan((P light_y，i -y) / (P light_x，i -x))-ψ)≤visionAngle in represents the position coordinates of N light sources, visionAngle is half of the sight angle of the bow-mounted camera; (x, y, ψ) represent the current position and heading of the AUV respectively; the corresponding changes in the introduction of control redundancy are shown in the following formula abs(atan((P light_y，i -y) / (P light_x，i -x))-ψ)≤visionAngle·0.7。 2. The method according to claim 1, characterized in that Before step S105, the method further includes: S106: If the mobile docking device is lost from the field of view of the AUV during step S104, the AUV immediately decelerates according to the maximum deceleration to a speed lower than the minimum navigation speed allowed by the mobile docking device when the field of view is lost, and navigates towards the predicted position of the mobile docking device using a pure tracking method until the mobile docking device returns to the field of view of the AUV, and then returns to step S104.

3. The method according to claim 2, characterized in that The process of obtaining the predicted mobile docking device position includes: The target pose is obtained from the last solution when the visual positioning information exists. The position of the mobile docking device during the period when the AUV field of view is lost is predicted: Among them, Dock x，Last , Dock y，Last , Dock ψ，Last It is the position and heading information of the mobile docking device obtained by the last calculation when the visual positioning information exists. u represents the fixed navigation speed of the mobile docking device during docking, Δt loss It is the time difference from the moment when visual information is lost to the current planning moment.

4. The method according to claim 1, wherein Based on the current posture information of the mobile docking device obtained after performing posture calculation on the image, combined with the prerequisite of the mobile docking device's constant speed and direction navigation, the position of the mobile docking device at a future time is predicted, including: Among them, Dock x，i ,Dock y，i ,Dock ψ，i , respectively represent the position and heading information of the mobile docking device at time i, Dock u represents the fixed navigation speed of the mobile docking device during docking, and Δt represents the sampling interval.

5. The method according to claim 1, wherein The objective function is: Among them, J1 = ΔP′Q1ΔP, J2 = Δψ′Q2Δ′, J3 = Δu′Q3Δu respectively represent the corresponding requirements of position convergence, heading convergence, and speed convergence, w1, w2, and w3 are the weighted coefficients of each indicator, among which ΔP, Δψ, and Δu respectively represent the position deviation, heading deviation, and speed deviation between the AUV and the mobile docking device, and Q1, Q2, and Q3 correspond to the weight coefficient matrices of each optimization indicator.

6. The method according to claim 1, characterized in that Calculating the position and posture of the mobile docking device according to the image captured by the camera to continuously track the field of view of the mobile docking device includes: Acquiring an image of a light source on the mobile docking device captured by a camera, wherein two layers of positioning light arrays are installed on the outer and inner sides of a guide cover of the mobile docking device, the outer positioning light array being used for long-distance visual guidance, and the inner positioning light array being used for close-range visual guidance; The position and posture of the mobile docking device are calculated according to the light source in the image.

7. A dynamic path planning device for underactuated AUV mobile docking, characterized in that: include: The circle navigation module is used for the AUV to autonomously navigate to the docking waters, and after entering the docking waters, it starts circling from the current position and obtains the position of the mobile docking device through acoustic positioning data; A connection establishment module is used to establish a connection between the AUV and the mobile docking device through acoustic signals, so that the mobile carrier equipped with the mobile docking device starts constant speed and direction navigation at a predetermined docking depth in the docking waters; A tracking and approaching module is used for the AUV to chase the waypoint in a pure tracking and guidance manner based on the fused navigation data information to approach the mobile docking device, wherein the fused navigation data information is obtained by fusing information provided by the inertial navigation system and the acoustic sensor; A local planning module is used when the camera on the AUV captures the mobile docking device. The AUV performs local planning under constraints based on the model predictive control framework according to the positioning data obtained by visual positioning, solves the optimization problem under the constraints, and obtains the navigation speed and direction at the next moment; The signal sending module is used to determine whether the AUV meets the entry conditions of the mobile docking device after the local planning module is executed. If the entry conditions are met and the AUV successfully enters the mobile docking device, a successful docking signal is sent to the decision module; otherwise, the vector direction from the AUV's current position to the target device position is checked. If the vector direction reverses between adjacent sampling intervals, it means that the AUV's current position has exceeded the mobile docking device, and the docking is determined to have failed, and a docking failure signal is sent; Otherwise, continue with the docking; The processing in the local planning module includes: Establish kinematic and dynamic models of AUV under the influence of ocean currents; Discretizing the kinematic model and the dynamic model, and predicting the state of the AUV at a future moment based on the discretized models; Calculating the position and posture of the mobile docking device based on the image captured by the camera; Predicting the position of the mobile docking device at a future time based on the current position information of the mobile docking device obtained after performing position and position calculation on the image, combined with the prerequisite of the mobile docking device's constant speed and direction navigation; Establishing an objective function between the AUV and the mobile docking device under the conditions of position convergence, heading convergence, and speed convergence based on the state at the future moment predicted by the discretization model and the predicted position and posture of the mobile docking device at the future moment; Establish constraints on AUV motion capabilities; Establish field of view continuous tracking constraints and introduce control redundancy considerations; Solve the objective function minimization optimization problem under the constraints of AUV motion capability and continuous field of view tracking to obtain the navigation speed and direction at the next moment; Continuous visual tracking means that in order to ensure the continuous acquisition of visual positioning data, the AUV needs to have the ability to perceive the target visually at all times while approaching the target. That is, all light sources used for positioning installed on the mobile docking device must be within the field of view of the AUV camera. Continuous visual positioning data acquisition requires that during the AUV's driving process, the angle between the line of sight from the current position to all light sources on the light array and the AUV's current heading is less than half of the camera's opening angle, as shown in the following formula: abs(atan((P light_y，i -y) / (P light_x，i -x))-ψ)≤visionAngle in represents the position coordinates of N light sources, visionAngle is half of the sight angle of the bow-mounted camera; (x, y, ψ) represent the current position and heading of the AUV respectively; the corresponding changes in the introduction of control redundancy are shown in the following formula abs(atan((P light_y，i -y) / (P light_x，i -x))-ψ)≤visionAngle·0.7。 8. An electronic device, characterized in that: include: one or more processors; a memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 6.

9. A computer-readable storage medium having computer instructions stored thereon, characterized in that: When the instruction is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images