Ship attitude estimation system and method based on unmanned aerial vehicle-unmanned ship-submersible vehicle

Abstract

The invention provides a ship attitude estimation system and method based on an unmanned aerial vehicle-unmanned ship-submersible vehicle, and belongs to the technical field of ship attitude estimation. Firstly, an unmanned ship, an unmanned aerial vehicle and a submersible vehicle are deployed at specific point locations of a monitored water area; after the unmanned aerial vehicle monitors that the ship enters the monitored water area, the unmanned aerial vehicle, the unmanned ship and the submersible vehicle move towards the target ship point location and return images to the shore-based control center after arriving at designated positions; the shore-based control center identifies the images of the unmanned aerial vehicle and the unmanned ship to obtain a final ship attitude data set; meanwhile, the shore-based control center monitors the attitude above the deck of the ship body and monitors the attitude of cracks at the bottom of the ship and propellers; and finally, correcting the positions of the submersible vehicle, the unmanned ship and the unmanned aerial vehicle according to the distance between the target ship and the central position of the image. And the steps are circulated, and the unmanned aerial vehicle, the unmanned ship and the submersible vehicle return to the initial positions after the task is completed. According to the invention, omnibearing monitoring of the ship attitude can be realized, and the accuracy of the attitude angle of the ship can be improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of ship attitude estimation, and specifically relates to a ship attitude estimation system and method based on unmanned aerial vehicle-unmanned ship-submersible. Background Art

[0002] With the vigorous development of the global marine economy, ships, as key carriers of international trade, have an increasingly prominent strategic position in the maritime logistics and transportation system. As a core indicator of ship safety, accurate estimation of ship attitude has become a key factor in ensuring shipping safety. At present, ship attitude estimation mainly relies on manual observation methods, which is difficult to effectively estimate the ship attitude in real time. The collaborative operation method of drones, unmanned ships and submersibles can estimate the ship's navigation attitude in real time in all directions. This method can not only effectively ensure the safety of ship transportation, but also play an important role in marine resource exploration, maritime safety patrols, environmental monitoring and maritime rescue.

[0003] After searching, it was found that the publication number is CN117739908A, and the name is "A ship attitude detection system and method based on laser radar". This method is to install the laser radar movably on the ship loader, connect the laser radar to the server, and observe the position of the ship. The laser radar data is obtained and processed and modeled. When the ship loader stops moving, the spatial coordinates of the characteristic points on both sides of the ship's sea route are analyzed and calculated to construct a real-time model of the ship's pitch angle to calculate the ship's pitch angle. However, this technical solution relies on a fixed laser radar sensor to estimate the ship's attitude, resulting in a single information dimension and an inability to monitor the attitude information of the bottom of the ship.

[0004] The publication number is CN117288185A, and the name is "Ship attitude monitoring system based on IMU and its algorithm". A ship attitude monitoring system based on IMU is disclosed, and the lower computer sends the data of circuit attitude detection to the upper computer. The monitoring system performs Kalman filtering on the data of the accelerometer and gyroscope to achieve data fusion, thereby improving the accuracy and stability of attitude information. Although this method uses the IMU sensor inside the hull for attitude estimation, it is unable to monitor the status of the ship's deck, bottom and sides, and cannot effectively perform all-round attitude estimation of the ship. Therefore, it is urgent to design a ship attitude estimation method based on the collaboration of unmanned aerial vehicle, unmanned ship and submersible. Summary of the invention

[0005] In order to solve the problem that the existing technology cannot monitor the attitude information of the bottom of the ship, and at the same time to solve the problem that the existing technology cannot monitor the status of the ship deck, bottom and side, and cannot effectively perform all-round attitude estimation of the ship. The present invention provides a ship attitude estimation system and method based on drone-unmanned ship-submersible, which achieves the purpose of improving the accuracy of ship attitude estimation.

[0006] The technical solution of a ship attitude estimation system and method based on unmanned aerial vehicle, unmanned ship and submersible of the present invention is:

[0007] A ship attitude estimation system based on unmanned aerial vehicle, unmanned ship and submersible, comprising a unmanned aerial vehicle for monitoring whether a ship enters a monitoring area and collecting the aerial attitude of the ship in the monitoring area;

[0008] An unmanned boat used to receive drone cluster signals and collect the surface posture of ships in the monitoring area;

[0009] Submersibles used to receive drone cluster signals and collect underwater posture data of ships in the monitoring area;

[0010] A shore-based server used to receive ship attitude data collected by drones, unmanned ships and submersibles and to process and analyze the data.

[0011] A further improvement of the above technical solution of the present invention is that: the submersible mainly includes a motion control system, a control cabin, and a detection system composed of a sonar and a camera; wherein the motion control system includes a floating material assembly and a brushless motor propeller assembly, the floating material assembly is located above the submersible frame, the brushless motor propeller assembly is distributedly installed around the control cabin, and two groups of brushless motors provide power in different directions, one group is located longitudinally on both sides of the control cabin to control its ascent / dive, and the other group is located transversely at the four corners of the control cabin to control its horizontal movement direction; the control cabin is located at the center of the submersible frame, and is equipped with a GPS sensor, an IMU module, a power carrier module, and a depth sensor; the camera is located at the center of the upper front part of the frame, and the sonar is located at the center of the middle front part of the frame.

[0012] A method for estimating ship attitude based on unmanned aerial vehicle, unmanned ship and submersible, using the above system, comprises the following steps:

[0013] S1. Set a monitoring water area and deploy unmanned boats at the four corners of the water area. At the same time, the drone is deployed over the monitoring area to execute a cruise mode with a predetermined elliptical trajectory. The drone performs long-distance and wide-range monitoring in the air, and sends cluster signals to the unmanned boat and submersible after detecting a ship.

[0014] S2. After receiving the cluster signal from the drone, the unmanned ship and the submersible use the obstacle avoidance algorithm and the motion control algorithm to approach the target point at the waterline of the target ship. At the same time, the drone adjusts its course and moves to a predetermined height directly above the ship. After the drone, the unmanned ship and the submersible move to the designated positions, they collect the ship's air, surface and underwater postures respectively, and transmit the collected posture data back to the shore-based control center.

[0015] S3, the shore-based control center processes and analyzes the collected attitude data, identifies the ship attitude, and uses the data fusion algorithm to obtain the final attitude data X of the ship hull. f ;

[0016] S4. Based on image analysis, the position of the submersible, unmanned vessel and drone is corrected by evaluating the relative position of the target relative to the center of the image, so as to align the ship hull and waterline with the center of the image, thereby compensating for the measurement error introduced by the change of the ship's position;

[0017] S5. Execute steps S2-S4 in a loop until the target ship completely leaves the monitored waters, the unmanned ship and the submersible return to the predetermined starting position of the monitored waters, and the unmanned aerial vehicle returns to its elliptical trajectory cruising state.

[0018] A further improvement of the above technical solution of the present invention is that in step S2, the unmanned boat uses an obstacle avoidance algorithm combined with a motion control algorithm to approach the target point at the waterline of the target ship, specifically including the following steps:

[0019] S2.1. Initialize the tree Contains the root node (x, y) and the dynamic window V d , and randomly sample a point x in the environment rd , calculate the distance from the current position of the unmanned ship (x, y) to x rd The direction vector u;

[0020] S2.2. Find the tree Middle distance x rd The nearest node x near , then calculate x near and x rd The connection between them and from x near and x rd Find a new node x on the line between n , if x n If it does not collide with an obstacle, add it to the tree If x n If it collides with an obstacle, it will find a new node x n , until there is no collision with obstacles;

[0021] S2.3, calculate the distance from the current position (x, y) of the unmanned ship to the target position (x g ,y g ) goal ;

[0022] Calculate the distance D from the current position (x, y) of the unmanned ship to the nearest obstacle obs ;

[0023] Calculate the target ship's guidance g1 = α·cos(θ g -θ),

[0024] Among them, α is the weight coefficient, θ g is the angle from the current position of the unmanned ship to the target ship position, θ is the current direction of the unmanned ship;

[0025] Calculate the speed matching of unmanned ships

[0026] Where β is the weight coefficient, v is the current speed, v g is the expected velocity, v max is the maximum speed;

[0027] Calculate the distance between the unmanned ship and obstacles Among them, γ is the weight coefficient, and the comprehensive score g = g1 + g2 + g3;

[0028] S2.4, in the dynamic window V d Find the speed (v', ω') with the highest comprehensive score g, and calculate the heading angle θ and speed v of the unmanned ship according to the selected speed (v', ω');

[0029] S2.5, repeat steps S2.2 to S2.4 until the unmanned ship reaches the target position (x g ,y g ).

[0030] A further improvement of the above technical solution of the present invention is that: the steps of the motion control algorithm of the submersible in step S2 are:

[0031] First, use the IMU sensor to measure the current posture of the submersible, then determine the deviation between the target posture and the current posture, and then determine the proportional coefficient K p , integral coefficient K i and the differential coefficient K d , into the controller output formula

[0032]

[0033] Among them, err(k) is the deviation between the target posture at time k and the current posture;

[0034] Finally adjust the scale factor K p , integral coefficient K i and the differential coefficient K d , improving control accuracy and robustness.

[0035] A further improvement of the above technical solution of the present invention is that: in the step S3, the ship posture is calculated based on the image of the top of the ship taken by the drone and the image of the draft of the ship taken by the unmanned ship.

[0036] A further improvement of the above technical solution of the present invention is that the algorithm for calculating the ship's posture based on the image of the top of the ship taken by the drone is:

[0037] The shore-based control center marks the key feature points of the four corners of the ship and the top of the mast in the image based on the image sent back by the drone; according to the size and geometric characteristics of the ship, the coordinates (X, Y, Z) of the feature points in the three-dimensional coordinate system of the ship are determined, and the two-dimensional coordinates (x, y) of the marked feature points on the image plane are measured; the camera intrinsic parameters are obtained through camera calibration based on the correspondence between the three-dimensional coordinates and the two-dimensional coordinates; then, the attitude angle of the drone relative to the ship is estimated by combining the camera intrinsic parameters and the three-dimensional and two-dimensional coordinates of the feature points, and the relative attitude angle of the ship to the drone is estimated; finally, the rotation matrix R and the translation vector t of the ship are estimated by combining the attitude angle data in the drone IMU sensor, and the rotation matrix R is converted into three angles: roll angle, pitch angle, and yaw angle to obtain the final attitude data set X of the top of the ship d .

[0038] A further improvement of the above technical solution of the present invention is that the algorithm for calculating the ship's posture based on the ship's draft image taken by the unmanned ship is:

[0039] The shore-based control center records the image recognition result sent back by the unmanned ship as X LT , X LB , X RT , X RB , respectively represent the port front draft, port rear draft, starboard front draft, starboard rear draft, W s Represents the width of the ship, L s represents the length of the ship. The roll angle is

[0040]

[0041] The pitch angle is

[0042]

[0043] Calculated hull posture data set X W .

[0044] A further improvement of the above technical solution of the present invention is that: the step S3 is based on the final posture data set X of the top of the ship extracted from the drone image. d The hull pose dataset X parsed from unmanned ship images w Execute the data fusion algorithm to obtain the comprehensive final posture data X of the hull f The specific steps are:

[0045] S3.1. Construct complementary filters. For the attitude angle data sets of the UAV and the unmanned ship, define the state space equations respectively: k+1 =x k +w k ,y k =x k +v k , where x k is the true attitude angle, w k is the state noise, y k is the measured value, v k To measure noise;

[0046] S3.2, initialize the complementary filter parameters, initial state x0, state noise covariance Q, measurement noise covariance R;

[0047] S3.3, perform complementary filtering iterations, and then output the estimated values ​​of the three fused angles: roll angle, pitch angle, and yaw angle.

[0048] A further improvement of the above technical solution of the present invention is that: in the step S4, the pixel distance of the image target according to the image center is obtained, the actual target offset distance is calculated according to the pixel distance, and the actual offset distance is used as a control signal to control the unmanned ship, the unmanned aerial vehicle and the submersible to adjust their own coordinates to adapt to the current ship position.

[0049] Due to the adoption of the above technical solution, the technical advances achieved by the present invention include:

[0050] Traditional ship waterline monitoring methods can usually only monitor the waterline at a single fixed position, and are often limited by the water environment and the ship's berthing position, and cannot monitor the ship in all directions. This solution can monitor the ship in all directions through the cooperation of drones and unmanned ships, and observe the ship's waterline and deck conditions; at the same time, the present invention can shoot the bottom of the ship through the submersible team, and can observe the cracks on the side of the ship and the bottom reef images.

[0051] At the same time, the present invention improves the reliability and accuracy of data by fusing and optimizing the monitoring attitude information of the unmanned aerial vehicle and the monitoring attitude information of the unmanned ship, realizes the sharing and utilization of data information, and improves the value and utilization rate of data. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 It is a schematic diagram of the submersible structure of a ship attitude estimation system and method based on a drone-unmanned ship-submersible of the present invention;

[0053] Figure 2 It is a schematic diagram of the structure of an unmanned ship of the present invention, which is a ship attitude estimation system and method based on a UAV-unmanned ship-submersible;

[0054] Figure 3 It is a schematic diagram of system equipment deployment of a ship attitude estimation system and method based on a drone-unmanned ship-submersible of the present invention;

[0055] Figure 4 It is a flow chart of a ship attitude estimation system and method based on unmanned aerial vehicle-unmanned ship-submersible of the present invention.

[0056] In the attached figure: 1. buoyancy material of submersible; 2. ascent / descent thruster of submersible; 3. control cabin; 4. rear assembly of brushless motor thruster; 5. submersible frame; 6. sonar; 7. underwater camera; 8. searchlight; 9. front assembly of brushless motor thruster; 10. bow camera A; 11. camera gimbal A; 12. observation tower; 13. main hull; 14. camera B; 15. camera gimbal B; 16. side hull connecting frame A; 17. side hull A; 18. camera C; 19. drone B; 20. helipad B; 21. tension sensor; 22. power battery compartment; 23. submersible recovery cabin B; 24. main thruster B; 25. main thruster A; 26. steering thruster A. DETAILED DESCRIPTION

[0057] In order to make the purpose, technical solution and advantages of the present invention more clear, the present invention is further described in detail below in combination with specific implementation methods and with reference to the accompanying drawings. In the following description, the description of the public structure and technology is omitted to avoid unnecessary confusion of the concept of the present invention.

[0058] The present invention provides a ship attitude estimation system and method based on unmanned aerial vehicle, unmanned ship and submersible. The system in the present invention includes a unmanned aerial vehicle for monitoring whether a ship enters a monitoring area and collecting the aerial attitude of the ship in the monitoring area; an unmanned ship for receiving unmanned aerial vehicle cluster signals and collecting the surface attitude of the ships in the monitoring area; a submersible for receiving unmanned aerial vehicle cluster signals and collecting the underwater attitude of the ships in the monitoring area; and a shore-based server for receiving the ship attitude data collected by the unmanned aerial vehicle, the unmanned ship and the submersible and processing and analyzing the data.

[0059] refer to Figure 1It can be seen that the above-mentioned submersible mainly includes a motion control system, a control cabin 3, and a detection system composed of a sonar 6 and a camera; wherein the motion control system includes a submersible buoyancy material 1, a submersible ascent / descent thruster 2, a brushless motor thruster rear component 4, and a brushless motor thruster front component 9, the submersible buoyancy material 1 is located above the submersible frame 5, the submersible ascent / descent thruster 2, the brushless motor thruster rear component 4 and the brushless motor thruster front component 9 are distributedly installed around the control cabin 3, and two groups of brushless motors provide power in different directions, one group is longitudinally distributed on both sides of the control cabin 3 to control its ascent / descent, and the other group is transversely distributed at the four corners of the control cabin 3 to control its horizontal movement direction; the control cabin 3 is located at the center of the submersible frame 5, and is equipped with a GPS sensor, an IMU module, a power carrier module, and a depth sensor; the underwater camera 7 is located at the center of the upper front part of the frame, and the sonar 6 is located at the center of the middle front part of the submersible frame 5.

[0060] refer to Figure 2 It can be seen that the above-mentioned unmanned boat mainly includes a helipad B20, a submersible recovery cabin, and a power system; the power system includes a power battery compartment 22 and a motor propulsion system located in the front middle part of the hull, and the motor propulsion system includes a main propeller B24 and a main propeller A25 located at the rear of the hull, and two steering propellers A26 located on both sides.

[0061] The method of the present invention uses the above system and comprises the following steps:

[0062] S1. Set the monitored waters on the server software of the shore-based control center. After the upper computer software obtains the GPS coordinates of the four corner points of the monitored waters, it sends signals to the control centers of the drone, unmanned ship and submersible through a low-frequency long-wave signal generator; the drone executes a cruise mode with a predetermined elliptical trajectory, and sends cluster signals to the unmanned ship and submersible after detecting a ship.

[0063] S2. After the unmanned ship and the submersible receive the cluster signal from the drone, they use the obstacle avoidance algorithm and combine it with the motion control algorithm to approach the monitoring points of the target ship. At the same time, the drone adjusts its course and moves to a predetermined height directly above the ship. After the drone, the unmanned ship and the submersible move to the designated positions, they collect images from the ship's air, surface and underwater cameras respectively, and transmit the collected image data back to the shore-based control center.

[0064] The above-mentioned unmanned submersible uses an obstacle avoidance algorithm combined with a motion control algorithm to approach each monitoring point of the target ship, specifically including the following steps:

[0065] S2.1. Initialize the tree Contains the root node (x, y) and the dynamic window V d, and randomly sample a point x in the environment rd , calculate the distance from the current position (x, y) of the unmanned ship and the submersible to x rd The direction vector u.

[0066] S2.2. Find the tree Middle distance x rd The nearest node x near , then calculate x near and x rd The connection between them and from x near and x rd Find a new node x on the line between n , if x n If it does not collide with an obstacle, add it to the tree If x n If it collides with an obstacle, it will find a new node x n , until there is no collision with obstacles.

[0067] S2.3, calculate the distance from the current position (x, y) of the unmanned ship and the submersible to the target position (x g ,y g ) goal ;

[0068] Calculate the distance D from the current position (x, y) of the unmanned ship and the submersible to the nearest obstacle obs ;

[0069] Calculate the target ship's guidance g1 = α·cos(θ g -θ),

[0070] Among them, α is the weight coefficient, θ g is the angle from the current position of the unmanned ship to the position of the target ship, and θ is the current direction of the unmanned ship and the submersible;

[0071] Calculate the speed matching between unmanned ship and submersible

[0072] Where β is the weight coefficient, v is the current speed, v g is the expected velocity, v max is the maximum speed;

[0073] Calculate the distance between the unmanned ship and the submersible and obstacles Where γ is the weight coefficient, and the comprehensive score g=g1+g2+g3.

[0074] S2.4, in the dynamic window V d Find the speed (v', ω') with the highest comprehensive score g, and calculate the heading angle θ and speed v of the unmanned ship and the submersible according to the selected speed (v', ω').

[0075] S2.5, repeat steps S2.2 to S2.4 until the unmanned ship and the submersible reach the target position (x g ,y g ). At the same time, the steps of the motion control algorithm of the unmanned ship and the submersible are as follows: first, use the IMU sensor to measure the current posture of the unmanned ship and the submersible, then determine the deviation between the target posture and the current posture, and then determine the proportional coefficient K p , integral coefficient K i and the differential coefficient K d , into the controller output formula

[0076]

[0077] Among them, err(k) is the deviation between the target posture at time k and the current posture.

[0078] Finally adjust the scale factor K p , integral coefficient K i and the differential coefficient K d , improving control accuracy and robustness.

[0079] S3, the shore-based control center processes and analyzes the collected attitude data, identifies the ship attitude, and uses the data fusion algorithm to obtain the final attitude data X of the ship hull. f .

[0080] The above ship posture includes the ship top posture X d and the hull attitude X W , where the ship top posture X d The calculation method is as follows: the shore-based control center marks the key feature points of the four corners of the ship and the top of the mast in the image based on the image sent back by the drone; according to the size and geometric characteristics of the ship, the coordinates (X, Y, Z) of the feature points in the three-dimensional coordinate system of the ship are determined, and the two-dimensional coordinates (x, y) of the marked feature points on the image plane are measured; the camera intrinsic parameters are obtained through camera calibration according to the correspondence between the three-dimensional coordinates and the two-dimensional coordinates; then, the attitude angle of the drone relative to the ship is estimated by combining the camera intrinsic parameters and the three-dimensional and two-dimensional coordinates of the feature points, and the relative attitude angle of the ship to the drone is estimated; finally, the rotation matrix R and the translation vector t of the ship are estimated by combining the attitude angle data in the drone IMU sensor, and the rotation matrix R is converted into three angles: roll angle, pitch angle, and yaw angle to obtain the final attitude data set X of the top of the ship d .

[0081] Hull attitude X W The calculation method is as follows: the shore-based control center records the image recognition result sent back by the unmanned ship as X LT , X LB , X RT, X RB , respectively represent the port front draft, port rear draft, starboard front draft, starboard rear draft, W s Represents the width of the ship, L s represents the length of the ship. The roll angle is

[0082]

[0083] The pitch angle is

[0084]

[0085] Calculated hull posture data set X W .

[0086] According to the final pose dataset X of the top of the ship extracted from the drone image d The hull pose dataset X parsed from unmanned ship images w Execute the data fusion algorithm to obtain the comprehensive final posture data X of the hull f , the specific steps are:

[0087] S3.1. Construct complementary filters. For the attitude angle data sets of the UAV and the unmanned ship, define the state space equations respectively: k+1 =x k +w k ,y k =x k +v k , where x k is the true attitude angle, w k is the state noise, y k is the measured value, v k To measure noise.

[0088] S3.2. Initialize the complementary filter parameters, initial state x0, state noise covariance Q, and measurement noise covariance R.

[0089] S3.3, perform complementary filtering iterations, and then output the estimated values ​​of the three fused angles: roll angle, pitch angle, and yaw angle.

[0090] S4. Based on image analysis, the positions of submersibles, unmanned ships and drones are corrected by evaluating the relative position of the target with respect to the center of the image, so as to align the ship's hull and waterline with the center of the image, thereby compensating for the measurement error introduced by the change in the ship's position; specifically, the pixel distance of the image target from the center of the image is obtained, the actual target offset distance is calculated based on the pixel distance, and the actual offset distance is used as a control signal to control the unmanned ship, drone and submersible to adjust their own coordinates to adapt to the current ship position.

[0091] S5. Execute steps S2-S4 in a loop until the target ship completely leaves the monitored waters, the unmanned ship and the submersible return to the predetermined starting position of the monitored waters, and the unmanned aerial vehicle returns to its elliptical trajectory cruising state.

[0092] In the above embodiment, the present invention provides a ship attitude estimation system and method based on drone-unmanned ship-submersible. The present invention can perform all-round monitoring of the ship to observe the ship's waterline, deck condition, ship side cracks, and bottom reef images; at the same time, the present invention integrates and optimizes the drone monitoring attitude information with the unmanned ship monitoring attitude information, thereby improving the reliability and accuracy of the data, realizing the sharing and utilization of data information, and improving the value and utilization of the data.

[0093] The above-described embodiments are merely descriptions of preferred implementations of the present invention, and are not intended to limit the concept and scope of the present invention. Without departing from the design concept of the present invention, various modifications and improvements made by ordinary persons in the art to the technical solution of the present invention should fall within the protection scope of the present invention, and the technical contents for which protection is sought in the present invention have been fully recorded in the claims.

Claims

1. A ship attitude estimation system based on unmanned aerial vehicle, unmanned ship and submersible, characterized by: Including drones used to monitor whether ships have entered the monitoring area and collect the aerial posture of ships in the monitoring area; An unmanned boat used to receive drone cluster signals and collect the surface posture of ships in the monitoring area; Submersibles used to receive drone cluster signals and collect underwater posture data of ships in the monitoring area; A shore-based server used to receive ship attitude data collected by drones, unmanned ships and submersibles and to process and analyze the data.

2. The ship attitude estimation system based on unmanned aerial vehicle, unmanned ship and submersible according to claim 1, characterized in that: The submersible mainly includes a motion control system, a control cabin, and a detection system composed of a sonar and a camera; wherein the motion control system includes a float assembly and a brushless motor propeller assembly, the float assembly is located above the submersible frame, the brushless motor propeller assembly is distributedly installed around the control cabin, and two groups of brushless motors provide power in different directions, one group is longitudinally distributed on both sides of the control cabin to control its ascent / dive, and the other group is transversely distributed at the four corners of the control cabin to control its horizontal movement direction; the control cabin is located at the center of the submersible frame, and is equipped with a GPS sensor, an IMU module, a power carrier module, and a depth sensor; the camera is located at the center of the upper front part of the frame, and the sonar is located at the center of the middle front part of the frame.

3. A ship attitude estimation method based on unmanned aerial vehicle, unmanned ship and submersible, characterized by: Using the system according to any one of claims 1 to 2, comprising the following steps: S1. Set a monitoring water area and deploy unmanned boats at the four corners of the water area. At the same time, the drone is deployed above the monitoring area to execute a cruise mode with a predetermined elliptical trajectory. The drone performs long-distance and wide-range monitoring in the air, and sends cluster signals to the unmanned boat and submersible after detecting a ship. S2. After receiving the cluster signal from the drone, the unmanned ship and the submersible use the obstacle avoidance algorithm and the motion control algorithm to approach the target point at the waterline of the target ship. At the same time, the drone adjusts its course and moves to a predetermined height directly above the ship. After the drone, the unmanned ship and the submersible move to the designated positions, they collect the ship's air, surface and underwater postures respectively, and transmit the collected posture data back to the shore-based control center. S3, the shore-based control center processes and analyzes the collected attitude data, identifies the ship attitude, and uses the data fusion algorithm to obtain the final attitude data X of the ship hull. f ; S4. Based on image analysis, the position of the submersible, unmanned vessel and drone is corrected by evaluating the relative position of the target relative to the center of the image, so as to align the ship hull and waterline with the center of the image, thereby compensating for the measurement error introduced by the change of the ship's position; S5. Execute steps S2-S4 in a loop until the monitored target ship completely leaves the monitored waters, the unmanned ship and the submersible return to the predetermined starting position of the monitored waters, and the unmanned aerial vehicle returns to its elliptical trajectory cruising state.

4. The method for estimating ship attitude based on unmanned aerial vehicle, unmanned ship and submersible according to claim 3, characterized in that: In step S2, the unmanned ship uses an obstacle avoidance algorithm combined with a motion control algorithm to approach the target point at the waterline of the target ship. The following steps are included: S2.

1. Initialize the tree Contains the root node (x, y) and the dynamic window V d , and randomly sample a point x in the environment rd , calculate the distance from the current position of the unmanned ship (x, y) to x rd The direction vector u; S2.

2. Find the tree Middle distance x rd The nearest node x near , then calculate x near and x rd The connection between them and from x near and x rd Find a new node x on the line between n , if x n If it does not collide with an obstacle, add it to the tree If x n If it collides with an obstacle, it will find a new node x n , until there is no collision with obstacles; S2.3, calculate the distance from the current position of the unmanned ship (x, y) to the target position (x g ,y g ) goal ; Calculate the distance D from the current position (x, y) of the unmanned ship to the nearest obstacle obs ; Calculate the target ship's guidance g1 = α·cos(θ g -θ), Among them, α is the weight coefficient, θ g is the angle from the current position of the unmanned ship to the target ship position, θ is the current direction of the unmanned ship; Calculate the speed matching of unmanned ships Where β is the weight coefficient, v is the current speed, and v g is the expected velocity, v max is the maximum speed; Calculate the distance between the unmanned ship and obstacles Among them, γ is the weight coefficient, and the comprehensive score g = g1 + g2 + g3; S2.4, in the dynamic window V d Find the speed (v', ω') with the highest comprehensive score g, and calculate the heading angle θ and speed v of the unmanned ship according to the selected speed (v', ω'); S2.5, repeat steps S2.2 to S2.4 until the unmanned ship reaches the target position (x g ,y g ).

5. The method for estimating ship attitude based on unmanned aerial vehicle, unmanned ship and submersible according to claim 3, characterized in that: The steps of the motion control algorithm of the submersible in step S2 are: First, use the IMU sensor to measure the current posture of the submersible, then determine the deviation between the target posture and the current posture, and then determine the proportional coefficient K p , integral coefficient K i and the differential coefficient K d , into the controller output formula Among them, err(k) is the deviation between the target posture at time k and the current posture; Finally adjust the scale factor K p , integral coefficient K i and the differential coefficient K d , improving control accuracy and robustness.

6. The method for estimating ship attitude based on unmanned aerial vehicle, unmanned ship and submersible according to claim 3, characterized in that: In step S3, the ship posture is calculated based on the image of the top of the ship taken by the drone and the image of the ship's draft taken by the unmanned ship, and the propeller posture and reef image at the bottom of the ship are obtained based on the image sent back by the submersible.

7. The method for estimating ship attitude based on unmanned aerial vehicle, unmanned ship and submersible according to claim 6, characterized in that: The algorithm for calculating the ship's posture based on the image of the top of the ship taken by the drone is: The shore-based control center marks the key feature points of the four corners of the ship and the top of the mast in the image based on the image sent back by the drone; according to the size and geometric characteristics of the ship, the coordinates (X, Y, Z) of the feature points in the three-dimensional coordinate system of the ship are determined, and the two-dimensional coordinates (x, y) of the marked feature points on the image plane are measured; the camera intrinsic parameters are obtained through camera calibration based on the correspondence between the three-dimensional coordinates and the two-dimensional coordinates; then, the attitude angle of the drone relative to the ship is estimated by combining the camera intrinsic parameters and the three-dimensional and two-dimensional coordinates of the feature points, and the relative attitude angle of the ship to the drone is estimated; finally, the rotation matrix R and the translation vector t of the ship are estimated by combining the attitude angle data in the drone IMU sensor, and the rotation matrix R is converted into three angles: roll angle, pitch angle, and yaw angle to obtain the final attitude data set X of the top of the ship d .

8. The method for estimating ship attitude based on unmanned aerial vehicle, unmanned ship and submersible according to claim 7, characterized in that: The algorithm for calculating the ship's posture based on the ship's draft image taken by the unmanned ship is: The shore-based control center records the image recognition result sent back by the unmanned ship as X LT ,X LB ,X RT ,X RB , respectively represent the port front draft, port rear draft, starboard front draft, starboard rear draft, W s Represents the width of the ship, L s represents the length of the ship; the rolling angle is The pitch angle is Calculated hull posture data set X W .

9. The method for estimating ship attitude based on unmanned aerial vehicle, unmanned ship and submersible according to claim 8, characterized in that: The step S3 is based on the final posture dataset X of the top of the ship extracted from the drone image. d The hull pose dataset X parsed from unmanned ship images w Execute the data fusion algorithm to obtain the comprehensive final posture data X of the hull f The specific steps are: S3.

1. Construct complementary filters. For the attitude angle data sets of the UAV and the unmanned ship, define the state space equations: k+1 =x k +w k ,y k =x k +v k , where x k is the true attitude angle, w k is the state noise, y k is the measured value, v k To measure noise; S3.2, initialize the complementary filter parameters, initial state x0, state noise covariance Q, measurement noise covariance R; S3.3, perform complementary filtering iterations, and then output the estimated values ​​of the three fused angles: roll angle, pitch angle, and yaw angle.

10. The method for estimating ship attitude based on unmanned aerial vehicle, unmanned ship and submersible according to claim 3, characterized in that: In step S4, the pixel distance of the image target from the image center is calculated, and the actual target offset distance is calculated based on the pixel distance. The actual offset distance is used as a control signal to control the unmanned ship, the unmanned aerial vehicle and the submersible to adjust their own positions until the image target returns to the image center.

Images