AUV underwater dynamic docking acousto-optic fusion guiding system and method

Abstract

The invention provides an acousto-optic fusion guiding system and method for AUV underwater dynamic docking, and belongs to the technical field of autonomous underwater robot docking, and a first inertial navigation system and a second inertial navigation system are respectively used for measuring absolute positions, postures and speeds of an AUV and a docking station; the USBL array, the USBL beacon, the multi-beam foresight imaging sonar and the camera are used for measuring the measurement information of the AUV in different guiding stages; and the combined filtering module is used for carrying out filtering fusion by adopting a federated Kalman filter and a maneuvering observer based on the relative motion state at the initial moment and the actual measurement information of the AUV acquired by the USBL array, the multi-beam forward-looking imaging sonar and the camera in real time, so as to acquire the global state estimation information of the AUV. Compared with a traditional estimation method, the method is higher in motion state estimation precision, better in robustness and more suitable for underwater dynamic docking.

Description

Technical Field

[0001] This application belongs to the technical field of autonomous underwater vehicle docking, and more specifically, relates to an acoustic-optic fusion guidance system and method for AUV underwater dynamic docking. Background Art

[0002] Autonomous underwater vehicles (AUVs) have obvious advantages over manned equipment in terms of operation efficiency and safety, and can independently perform tasks such as marine environment monitoring, seabed topography survey, and resource development in waters with a larger range and greater depth.

[0003] In order to expand the operation range of AUVs, improve work efficiency, and meet the needs of AUV seabed wide-area terrain exploration, information collection, and special operations, it is necessary to regularly recover AUVs for energy replenishment. However, the underwater environment is complex and special, and AUV underwater docking guidance still faces great challenges, thus affecting the recovery effect of AUVs.

[0004] When guiding an AUV underwater, the method of underwater acoustic positioning and navigation is usually adopted. However, in shallow water environments, due to the errors of the sensors themselves, the pollution of random noise, and the interference of underwater acoustic reverberation, there are a large number of outliers in the measurement data, which reduces the reliability of the measurement data. Especially in dynamic docking scenarios, the docking success rate of AUVs highly depends on the guidance system, that is, the navigation and positioning quality. However, the existing guidance methods are not sufficient to meet the high-precision positioning requirements of AUVs and are not conducive to dynamic docking. Summary of the Invention

[0005] Aiming at the defects of the existing technology, the purpose of this application is to provide an acoustic-optic fusion guidance system and method for AUV underwater dynamic docking, aiming to solve the problem that the existing guidance methods are not sufficient to meet the high-precision positioning requirements of AUVs and are not conducive to dynamic docking.

[0006] To achieve the above purpose, this application provides an acoustic-optic fusion guidance system for AUV underwater dynamic docking, including: USBL array, USBL beacon, first inertial navigation system, second inertial navigation system, multi-beam forward-looking imaging sonar, camera, data initialization module, and combined filtering module; The USBL beacon and the first inertial navigation system are carried on the AUV; the USBL array, the second inertial navigation system, the multi-beam forward-looking imaging sonar, and the camera are arranged on the docking station; The first inertial navigation system and the second inertial navigation system are respectively used to measure the absolute position, attitude, and velocity of the AUV and the docking station; the USBL array and the USBL beacon, the multi-beam forward-looking imaging sonar, and the camera are used to measure the actual measurement information of the AUV in different guidance stages; The data initialization module is used to convert the absolute positions, attitudes, and velocities of the AUV and the docking station at the initial moment to the docking station coordinate system, and obtain the relative motion state at the initial moment; The combined filtering module is used to perform filtering fusion using a federated Kalman filter and a maneuver observer based on the relative motion state at the initial moment and the actual measurement information of the AUV obtained by the real-time USBL array, multi-beam forward-looking imaging sonar, and camera, and obtain the AUV global state estimation information; Among them, the maneuver observer is used to obtain the relative maneuver estimation value based on the estimated relative motion state and the state transition equation, and compensate the relative maneuver estimation value into the federated Kalman filter.

[0007] Further preferably, the combined filtering module includes a first sub-filter, a second sub-filter, a third sub-filter, a main filter, and a maneuver observer; The first sub-filter, the second sub-filter, and the third sub-filter are respectively connected to the USBL array, the multi-beam forward-looking imaging sonar, and the camera; the main filter is connected to the first inertial navigation system and the second inertial navigation system; and a maneuver observer is connected to the main filter side; The first sub-filter, the second sub-filter, the third sub-filter, and the main filter are used to obtain the moment predicted state according to the state transition matrix, the maneuver drive matrix, the relative maneuver estimation value at the moment, and the k global optimal estimated relative motion state at the -1 moment; and are used to obtain the corresponding moment predicted state covariance matrix according to the estimated relative motion state covariance matrix, the process noise, and the state transition matrix at the -1 moment; among them, the global optimal estimated relative motion state at the initial moment is the relative motion state at the initial moment; k The first sub-filter, the second sub-filter, and the third sub-filter are respectively used to update the predicted measurement values corresponding to the USBL beacon and the USBL array, the multi-beam forward-looking imaging sonar, and the camera based on the moment predicted state, combine the k actual measurement values of the AUV of the USBL beacon and the USBL array, the multi-beam forward-looking imaging sonar, and the camera at the moment, as well as the Kalman filter gain and the k moment predicted state covariance matrix, and update the corresponding k moment estimated relative motion state and estimated relative motion state covariance matrix; k The main filter is used to combine the corresponding moment estimated relative motion state and estimated relative motion state covariance matrix obtained by the first sub-filter, the second sub-filter, and the third sub-filter, and combine the corresponding k moment predicted state covariance matrix of the main filter and k ​k The moment prediction state matrix is calculated k The AUV global state estimation information at the moment; Among them, k ≥1.

[0008] Further preferably, the USBL array and the USBL beacon are used to measure the actual measurement information of the AUV during the long-distance guidance stage; during the medium-distance guidance stage, the multi-beam forward-looking imaging sonar is the main one, and the USBL array and the USBL beacon are the auxiliary ones to measure the actual measurement information of the AUV; during the short-distance guidance stage, the camera is the main one, and the multi-beam forward-looking imaging sonar, the USBL array and the USBL beacon are the auxiliary ones to measure the actual measurement information of the AUV; Among them, the area between the AUV and the docking station is divided into a long-distance guidance stage, a medium-distance guidance stage and a short-distance guidance stage; the area greater than the action range of the multi-beam forward-looking imaging sonar and the camera is divided into the long-distance guidance stage; the area greater than the action range of the camera and less than the working range of the beam forward-looking imaging sonar is divided into the medium-distance guidance stage; the area less than or equal to the action range of the camera is divided into the short-distance guidance stage.

[0009] Further preferably, k The moment prediction state Is: ; Among them, Is the state transition matrix, Is the 3 3 identity matrix; Is the maneuver drive matrix; Is the sampling time; Is The global optimal estimation relative motion state at the moment, Is The relative maneuver estimation value at the moment; ; Among them, Is The auxiliary variable at the moment, Is the observation gain matrix, Is the auxiliary gain matrix, Is The pseudo-inverse matrix of.

[0010] Further preferably, the predicted state covariance matrix Is: ; Among them, Is The relative motion state covariance estimated by the i th sub-filter or the main filter at the moment; Is the i th sub-filter or the process noise corresponding to the main filter; Represents the main filter; is the state transition matrix.

[0011] In a second aspect, based on the above acoustic-optical fusion guidance system for AUV underwater dynamic docking, the present application provides a corresponding acoustic-optical fusion guidance method for AUV underwater dynamic docking, including the following steps: Step S1: Convert the absolute positions, attitudes, and velocities of the AUV and the docking station at the initial moment to the docking station coordinate system to obtain the relative motion state at the initial moment; Step S2: Based on the relative motion state at the initial moment and the actual measurement information of the AUV obtained by the real-time USBL array, multi-beam forward-looking imaging sonar, and camera, use a federated Kalman filter and a maneuver observer for filtering fusion to obtain the AUV global state estimation information; Step S3: Use the AUV global state estimation information to guide the movement of the AUV; Among them, the relative maneuver estimation value is obtained by using the maneuver observer based on the AUV relative motion state estimation quantity and the state transition equation, and the relative maneuver estimation value is compensated into the federated Kalman filter.

[0012] Further preferably, the method for obtaining the actual measurement information of the AUV in step S2 is specifically as follows: In the long-distance guidance stage, the USBL array and the USBL beacon are used to measure the actual measurement information of the AUV; in the medium-distance guidance stage, the multi-beam forward-looking imaging sonar is mainly used, and the USBL array and the USBL beacon are used as supplements to measure the actual measurement information of the AUV; in the short-distance guidance stage, the camera is mainly used, and the multi-beam forward-looking imaging sonar, the USBL array, and the USBL beacon are used as supplements to measure the actual measurement information of the AUV.

[0013] Further preferably, step S2 specifically includes the following steps: Step S2.1: In the first sub-filter, the second sub-filter, the third sub-filter, and the main filter, according to the state transition matrix, the maneuver drive matrix, the relative maneuver estimation value at the moment, and the global optimal estimation relative motion state at the -1 moment, obtain the k predicted state at the moment; and according to the estimated relative motion state covariance matrix at the -1 moment, the process noise, and the state transition matrix, obtain the corresponding k predicted state covariance matrix at the moment; among them, the global optimal estimation relative motion state at the initial moment is the relative motion state at the initial moment; Step S2.2: In the first sub-filter, the second sub-filter, and the third sub-filter, based on kPredict the state at the moment, update the predicted measurement values corresponding to the USBL beacon, USBL array, multi-beam forward-looking imaging sonar, and camera, and combine k the actual measurement values of the AUV of the USBL beacon, USBL array, multi-beam forward-looking imaging sonar, and camera at the moment, as well as the Kalman filter gain and k the predicted state covariance matrix at the moment, and update the corresponding k estimated relative motion state and estimated relative motion state covariance matrix at the moment; Step S2.3: In the main filter, combine the estimated relative motion states and estimated relative motion state covariance matrices obtained by the first sub-filter, second sub-filter, and third sub-filter at the corresponding k moment, and combine the predicted state covariance matrix corresponding to the main filter and k the predicted state matrix at the moment, and calculate the k AUV global state estimation information at the moment; where k ≥1. k ≥1.

[0014] Further preferably, k the predicted state is: ; where is the state transition matrix, is the 3 ×3 identity matrix; is the maneuver drive matrix; is the sampling time; is the globally optimal estimated relative motion state at the moment, is the relative maneuver estimate value at the moment; ; where is the auxiliary variable at the moment, is the observation gain matrix, is the auxiliary gain matrix, is the pseudo-inverse matrix of

[0015] Further preferably, the predicted state covariance matrix is: ; where is the estimated relative motion state covariance of the i th sub-filter or main filter at the moment; is the i th sub-filter or the process noise corresponding to the main filter; represents the main filter; is the state transition matrix.

[0016] Generally speaking, compared with the prior art, the above technical solution conceived by the present application has the following beneficial effects: In the prior art, the AUV motion state estimation is directly based on sensor data and filtering models, without considering the relative maneuverability of the actual AUV, resulting in a problem of mismatch between the AUV relative motion and the filtering model, and causing deviation in the AUV relative motion state estimation. The maneuver observer provided by the present application can effectively estimate the AUV relative maneuver, and the federated Kalman filter can well fuse multi-sensor information. The combined compensation federated Kalman filter can effectively estimate the AUV relative motion state with respect to the docking station. Compared with the traditional estimation method, it has higher estimation accuracy, better robustness, and is more suitable for underwater dynamic docking. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 FIG. is a schematic diagram of the composition of the acoustic-optic fusion guidance system provided by an embodiment of the present application.

[0018] Figure 2 FIG. is a schematic diagram of the acoustic-optic fusion guidance method provided by an embodiment of the present application.

[0019] Figure 3 FIG. is a schematic diagram of the compensation federated Kalman filter provided by an embodiment of the present application.

[0020] Figure 4 FIG. is a flowchart of the acoustic-optic fusion guidance method provided by an embodiment of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0021] The embodiments of the present application will be described below with reference to the accompanying drawings in the embodiments of the present application.

[0022] The underwater dynamic docking of an AUV generally consists of a mobile docking station and an AUV. The successful entry of the AUV into the docking station means the completion of docking; the guidance system is responsible for the navigation and positioning of the AUV, aiming to guide the AUV to successfully enter the docking station, and is the key to the docking system.

[0023] The system composition is as Figure 1 shown. The present application provides an acoustic-optic fusion guidance system. An ultra-short baseline beacon (USBL) and an inertial navigation system (INS) are carried on the AUV. The USBL can realize underwater acoustic communication between the AUV and the docking station, and the INS can sense the absolute position, attitude, and speed of the AUV; a USBL array, a multi-beam forward-looking imaging sonar (MFLS), a binocular camera, and an INS are carried on the docking station; the USBL array and the USBL beacon can obtain the AUV relative slant range and position through acoustic signal propagation, the MFLS can obtain the AUV two-dimensional imaging through acoustic propagation, and further obtain the AUV two-dimensional position; the camera can obtain the AUV image through optical photography, and further obtain the AUV three-dimensional position; the INS can sense the absolute position, speed, and attitude of the docking station.

[0024] The three sensors for measuring the relative position of the AUV each have their own advantages and disadvantages. The USBL has a wide range of applications, is vulnerable to the inherent defects of underwater acoustic propagation, and wild values are likely to appear in positioning. However, the slant range measurement accuracy is relatively reliable, and the short-range positioning accuracy is not high. The MFLS is also an acoustic measurement instrument and is also vulnerable to underwater acoustic signals. Its range of application is smaller than that of the USBL. However, considering the AUV position information obtained by visual imaging and then detection and positioning, when the AUV features are accurately detected, there are fewer wild values in positioning, and the accuracy is also higher than that of the USBL. The binocular camera is an optical sensing device and is thus not affected by sound propagation, but its range of application is indeed very small and is highly dependent on the visibility of the underwater environment. The advantage is that its positioning accuracy is higher than that of the USBL and MFLS.

[0025] To make the most of the above-mentioned acoustic and optical sensors, as Figure 2 shown, the AUV underwater dynamic docking guidance system designed in this application is divided into three stages, namely the long-range guidance stage, the medium-range guidance stage, and the short-range guidance stage. The long-range guidance stage relies only on the ultra-short baseline positioning system. The medium-range guidance stage mainly uses the multi-beam forward-looking imaging sonar, supplemented by the ultra-short baseline positioning system. The short-range guidance stage relies on the binocular camera, and the multi-beam forward-looking imaging sonar and the ultra-short baseline positioning system are used as auxiliary means.

[0026] As described above, the AUV underwater docking guidance system provided in this application integrates three acoustic and optical navigation sensors, namely the USBL, MFLS, and binocular camera. Their information data measurement values are all different and their accuracies vary. Therefore, to obtain high-precision navigation and positioning information, it is also necessary to combine and filter the measurement values observed by each sensor. For the integrated navigation system, compared with using a single Kalman filter to centrally process the information of all navigation sensors, the federated Kalman filter (FKF) can overcome the disadvantages of the former, such as high state dimension, large computational amount, and low fault tolerance. Therefore, this application uses the FKF as the method for state estimation. The FKF is a typical information fusion technology, which consists of multiple sub-filters and a main filter, and uses two-stage decentralized filtering. Finally, the optimal state estimation is obtained through the data fusion method. However, for the AUV underwater dynamic docking, since the AUV and the docking station are in relative motion and the motion is uncertain, the traditional FKF cannot effectively obtain the relative positioning of the AUV, resulting in the inability of the guidance system to work well. Therefore, this application designs a maneuvering observer, obtains the relative maneuvering estimation value based on the AUV relative motion state estimation quantity and the state transition equation, and then feeds it back and compensates into the FKF, and finally obtains a more accurate motion state estimation of the AUV. This method is called the compensated federated Kalman filter (CFKF).

[0027] Before performing CFKF, since the measurement data of each sensor is based on the vehicle coordinate system, it is necessary to unify the measurement data into the dock coordinate system; ; where, is the measurement value after unifying the measurement data of each navigation sensor into the dock coordinate system; is the slant range value of the USBL measurement AUV; is the two-dimensional coordinate value of the MFLS measurement AUV; is the three-dimensional coordinate value of the camera measurement AUV; is the rotation and translation matrix for converting the USBL coordinate system to the dock coordinate system; is the rotation and translation matrix for converting the MFLS coordinate system to the dock coordinate system; is the rotation and translation matrix for converting the binocular camera coordinate system to the dock coordinate system. The above rotation and translation matrices can be determined according to the installation position of the sensor; and it is also necessary to convert the AUV motion state to the relative motion state in the dock coordinate system; ; where, is the three-axis position and velocity of the AUV measured by the inertial navigation; is the three-axis position and velocity of the dock; is the relative position and velocity of the AUV to the dock; is the coordinate transformation matrix, which can be determined according to the attitudes of the AUV and the dock.

[0028] CFKF consists of the following four steps, and the structure diagram is as shown in Figure 3 shown.

[0029] Step S1: Information distribution: Allocate the overall information weights of each filter according to the variance upper bound method: ; where, and are the covariance and process noise of the global optimal estimated relative motion state, and are the covariance and process noise of the sub-filter estimated relative motion state, is the weight factor, satisfying: .

[0030] In this application, during long-distance guidance , during medium-distance guidance , during short-distance guidance .

[0031] Step S2: Time update: Update the main filter and sub-filters independently in time.

[0032] First, update the predicted state : ; where, is the state transition matrix, is 3 3 identity matrix; is the maneuver driving matrix; is the sampling time; is the estimated relative motion state at time is the estimated relative maneuver at time ; Estimate according to the following maneuver observer ; where is the auxiliary variable at time, is the observation gain matrix, is the auxiliary gain matrix, which is set according to the actual situation, is the pseudo-inverse matrix of

[0033] Calculate the predicted state covariance matrix : .

[0034] Step S3: Measurement update: Use the subsystem measurement value to perform measurement update on its corresponding sub-filter; For a linear system, the sub-filter can use the conventional Kalman filter. For a non-linear system, a non-linear filter such as the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), Cubature Kalman Filter (CKF), and Particle Filter (PF) etc. needs to be used; In the embodiment of the present application, to reduce the computational complexity and facilitate implementation and deployment, the relatively conventional EKF is used as the sub-filter.

[0035] The sub-filters update the predicted measurement values respectively: .

[0036] Calculate the Kalman filter gain respectively: ; where is the pseudo-measurement matrix; is the measurement noise variance matrix; is the measurement equation.

[0037] Then perform the estimated relative motion state update and covariance matrix update: .

[0038] The main filter has no measurement value, so no measurement update is performed.

[0039] Step S4: Information fusion: The estimation information of all sub-filters and the main filter is fused according to the following rules to calculate the global state estimation information: 。

[0040] The globally optimal estimated relative motion state after CFKF filtering is transmitted to the AUV via underwater acoustic communication to guide the movement of the AUV, as Figure 4 shown.

[0041] The present application has the following advantages compared with the prior art: The prior art directly estimates the AUV motion state based on sensor data and a filtering model, without considering the actual relative maneuverability of the AUV, resulting in a problem of mismatch between the AUV relative motion and the filtering model, leading to deviations in the estimation of the AUV relative motion state. The maneuver observer provided by the present application can effectively estimate the AUV relative maneuver, and the federated Kalman filter can well fuse multi-sensor information. The combined compensation federated Kalman filter can effectively estimate the AUV relative motion state to the docking station. Compared with traditional estimation methods, it has higher estimation accuracy, better robustness, and is more suitable for underwater dynamic docking.

[0042] It should be understood that expressions such as "including" and "may include" that can be used in the present application indicate the existence of disclosed functions, operations, or constituent elements, and do not limit one or more additional functions, operations, and constituent elements. In the present application, terms such as "including" and / or "having" can be interpreted as indicating a specific characteristic, number, operation, constituent element, component, or a combination thereof, but cannot be interpreted as excluding the existence or possibility of addition of one or more other characteristics, numbers, operations, constituent elements, components, or a combination thereof.

[0043] In the description of the embodiments of the present application, it should be noted that unless otherwise clearly specified and limited, the term "connection" should be understood in a broad sense.

[0044] The above is only the specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present application, and all should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. An acoustic-optical fusion guidance system for AUV underwater dynamic docking, characterized in that, Comprising: A USBL array, a USBL beacon, a first inertial navigation system, a second inertial navigation system, a multibeam forward-looking imaging sonar, a camera, a data initialization module, and a combined filtering module; The USBL beacon and the first inertial navigation system are carried on the AUV; the USBL array, the second inertial navigation system, the multibeam forward-looking imaging sonar, and the camera are arranged on the dock; The first inertial navigation system and the second inertial navigation system are respectively used to measure the absolute position, attitude, and velocity of the AUV and the dock; the USBL array and the USBL beacon, the multibeam forward-looking imaging sonar, and the camera are used to measure the actual measurement information of the AUV in different guiding stages; The data initialization module is used to convert the absolute position, attitude, and velocity of the AUV and the dock at the initial moment to the dock coordinate system to obtain the relative motion state at the initial moment; The combined filtering module is used to perform filtering fusion using a federated Kalman filter and a maneuver observer based on the relative motion state at the initial moment and the actual measurement information of the AUV obtained by the real-time USBL array, multibeam forward-looking imaging sonar, and camera to obtain the AUV global state estimation information; Wherein, the maneuver observer is used to obtain a relative maneuver estimation value based on the estimated relative motion state and the state transition equation, and compensate the relative maneuver estimation value into the federated Kalman filter.

2. The acousto-optic fusion guidance system according to claim 1, wherein The combined filtering module includes a first sub-filter, a second sub-filter, a third sub-filter, a main filter, and a maneuver observer; The first sub-filter, the second sub-filter, and the third sub-filter are respectively connected to the USBL array, the multibeam forward-looking imaging sonar, and the camera; the main filter is connected to the first inertial navigation system and the second inertial navigation system; and a maneuver observer is connected to the main filter side; The first sub-filter, the second sub-filter, the third sub-filter, and the main filter are used to obtain the predicted state at time based on the state transition matrix, the maneuver drive matrix, the relative maneuver estimate at time k , and the relative motion state of the global optimal estimate at time -1; and are used to obtain the corresponding k covariance matrix of the predicted state at time based on the covariance matrix of the estimated relative motion state at time -1, the process noise, and the state transition matrix; where the relative motion state of the global optimal estimate at the initial time is the relative motion state at the initial time. The first sub-filter, the second sub-filter, and the third sub-filter are respectively used to predict the state based on k the moment, update the predicted measurement values corresponding to the USBL beacon, the USBL array, the multibeam forward-looking imaging sonar, and the camera, and combine k the actual measurement values of the AUV of the USBL beacon, the USBL array, the multibeam forward-looking imaging sonar, and the camera at the moment, as well as the Kalman filter gain and k the predicted state covariance matrix at the moment, and update the corresponding k estimated relative motion state and estimated relative motion state covariance matrix at the moment; The main filter is used to combine the relative motion state estimates and the covariance matrices of the relative motion state estimates obtained by the first sub-filter, the second sub-filter, and the third sub-filter, and combine them with the predicted state covariance matrix and k the predicted state matrix at the corresponding time of the main filter to calculate the AUV global state estimation information at k the corresponding time and k calculate the AUV global state estimation information at k the corresponding time; Among them, k ≥ 1.

3. The acousto-optic fusion guidance system according to claim 1 or 2, characterized in that The USBL array and the USBL beacon are used to measure the actual measurement information of the AUV in the long-distance guiding stage; in the medium-distance guiding stage, the multibeam forward-looking imaging sonar is the main, and the USBL array and the USBL beacon are the auxiliary, for measuring the actual measurement information of the AUV; In the short-distance guiding stage, the camera is the main, and the multibeam forward-looking imaging sonar, the USBL array, and the USBL beacon are the auxiliary, for measuring the actual measurement information of the AUV; Wherein, the area between the AUV and the dock is divided into a long-distance guiding stage, a medium-distance guiding stage, and a short-distance guiding stage; the area greater than the action range of the multibeam forward-looking imaging sonar and the camera is divided into the long-distance guiding stage; the area greater than the action range of the camera and less than the working range of the beam forward-looking imaging sonar is divided into the medium-distance guiding stage; the area less than or equal to the action range of the camera is divided into the short-distance guiding stage.

4. The acousto-optic fusion guidance system according to claim 3, characterized in that k Moment prediction state is as follows: Among them, is the state transition matrix, is the 3 × 3 identity matrix; is the maneuver driving matrix; is the sampling time; is the global optimal estimation of the relative motion state at time is the relative maneuver estimation value at time wherein, is the auxiliary variable at the moment, is the observation gain matrix, is the auxiliary gain matrix, is the pseudo-inverse matrix of 5. The acousto-optic fusion guidance system according to claim 4, characterized in that Predicted state covariance matrix is as follows: Among them, is the relative motion state covariance estimated by the i sub-filter or the main filter at time is the process noise corresponding to the i sub-filter or the main filter; represents the main filter; is the state transition matrix.

6. An acoustic-optic fusion guidance method for AUV underwater dynamic docking based on the acoustic-optic fusion guidance system described in claim 1, characterized in that, Including the following steps: Step S1: Convert the absolute position, attitude, and velocity of the AUV and the dock at the initial moment to the dock coordinate system to obtain the relative motion state at the initial moment; Step S2: Based on the relative motion state at the initial moment and the actual measurement information of the AUV obtained by the real-time USBL array, multibeam forward-looking imaging sonar, and camera, perform filtering fusion using a federated Kalman filter and a maneuver observer to obtain the AUV global state estimation information; Step S3: Use the AUV global state estimation information to guide the movement of the AUV; Among them, a maneuvering observer is used to obtain the relative maneuvering estimation value based on the AUV relative motion state estimation quantity and the state transition equation, and the relative maneuvering estimation value is compensated into the federated Kalman filter.

7. The acousto-optic fusion guidance method according to claim 6, wherein, The method for obtaining the actual measurement information of the AUV in step S2 is specifically as follows: In the long-distance guidance stage, the USBL array and the USBL beacon are used to measure the actual measurement information of the AUV; in the medium-distance guidance stage, the multi-beam forward-looking imaging sonar is mainly used, and the USBL array and the USBL beacon are used as supplements to measure the actual measurement information of the AUV; In the short-distance guidance stage, the camera is mainly used, and the multi-beam forward-looking imaging sonar, the USBL array and the USBL beacon are used as supplements to measure the actual measurement information of the AUV.

8. The acoustic-optical fusion guidance method according to claim 6 or 7, characterized in that Step S2 specifically includes the following steps: Step S2.1: Among the first sub-filter, the second sub-filter, the third sub-filter and the main filter, according to the state transition matrix, the maneuver drive matrix, the relative maneuver estimation value at the moment, and the global optimal estimation relative motion state at the -1 moment, obtain k the predicted state at the moment; and according to the covariance matrix of the estimated relative motion state at the -1 moment, the process noise and the state transition matrix, obtain the corresponding k covariance matrix of the predicted state at the moment; where the global optimal estimation relative motion state at the initial moment is the relative motion state at the initial moment; Step S2.2: Among the first sub-filter, the second sub-filter, and the third sub-filter, based on k the predicted state at the moment, update the predicted measurement values corresponding to the USBL beacon, the USBL array, the multibeam forward-looking imaging sonar, and the camera, and combine k the actual measurement values of the AUV of the USBL beacon, the USBL array, the multibeam forward-looking imaging sonar, and the camera at the moment, as well as the Kalman filter gain and k the predicted state covariance matrix at the moment, and update the corresponding k estimated relative motion state and estimated relative motion state covariance matrix at the moment; Step S2.3: In the main filter, combine the relative motion state estimates and the covariance matrices of the relative motion state estimates obtained by the first sub-filter, the second sub-filter, and the third sub-filter at the corresponding k time instances with the covariance matrix of the predicted state and the k predicted state matrix at the corresponding k time instances to calculate the AUV global state estimation information at the k time instance; where k ≥ 1.

9. The acousto-optic fusion guidance method according to claim 8, characterized in that k Moment prediction status is as follows: Among them, is the state transition matrix, is 3 3 identity matrix; is the maneuver drive matrix; is the sampling time; is the global optimal estimated relative motion state at time is the relative maneuver estimation value at time Among them, is the auxiliary variable at a moment, is the observation gain matrix, is the auxiliary gain matrix, is the pseudo-inverse matrix of.

10. The acousto-optic fusion guidance method according to claim 8, wherein Predicted state covariance matrix is as follows: Among them, is the relative motion state covariance estimated by the i sub-filter or the main filter at the th i sub-filter or the process noise corresponding to the main filter; represents the main filter; is the state transition matrix.

Images