Disturbance force estimation and feedforward compensation control method and system for dual-vessel cooperative system
Through hydrodynamic simulation calculation and interference force function fitting, the navigation stability problem of unmanned boats under oil fence interference was solved, and the stable navigation and oil spill recovery tasks of unmanned boats under oil fence interference was achieved. .
Patent Information
- Application Number
- CN202310977828.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-08-04
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2043-08-04
AI Technical Summary
During the oil spill round-up of the double unmanned boat, due to the interference of the oil fence, the expected speed and course cannot be tracked stably. The existing technology has failed to effectively estimate the interference of the oil fence on the unmanned boat, especially the stern of the ship. and the influence of slewing direction.
Hydropower simulation calculation software is used to simulate the interference force of the oil fence, and the interference force function is obtained through fitting, and the interference force direction and component force are calculated based on the current position and speed of the unmanned boat, and then the rudder angle and thruster are calculated. The compensation value of the rotation speed ensures stable navigation of unmanned boats under the interference of oil railings.
It realizes the stable tracking of the expected speed and heading of the unmanned boat under the interference of the oil fence, improves the rapid accuracy of the oil spill recovery task, and has the advantages of simplicity, accuracy and anti-interference.
Smart Images

Figure CN117111440B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of marine oil spill recovery. Background Art
[0002] Due to frequent accidents during the exploitation and transportation of oil at sea, large areas of oil are floating on the sea, causing serious pollution to the marine ecosystem. The use of artificial methods to recover marine oil spills has the disadvantages of high cost and low efficiency. Combining modern unmanned technology, using unmanned boats with high flexibility, low cost and high efficiency to recover oil spills has great application prospects and research significance.
[0003] The application number is 201910032665.3. It is a method for collaborative oil spill containment by dual unmanned boats based on layered guidance and drag force compensation. It provides a compensation method to solve the problem of deviation between the actual position and the expected position due to the influence of the oil boom when the dual unmanned boats are performing oil spill containment operations. After the deviation between the expected position and the actual position has occurred, this method provides a compensating rudder angle for the unmanned boat steering gear when the interference force of the oil boom on the unmanned boat is known. This method does not involve the estimation method of the interference force of the oil boom on the unmanned boat. In addition, the interference force of the oil boom on the unmanned boat will also affect the stern direction of the unmanned boat, and this method does not consider compensating for the stern direction of the unmanned boat.
[0004] The application number is CN202211462366.1, an active suppression method for coupled interference in a multi-unmanned boat system, the method proposes a compensation method to solve the problem of the interference force of the oil boom on the dual unmanned boats during the oil spill containment process. The method calculates the resistance of the oil boom in the water through the resistance formula, and then takes the interference force of the oil boom on the unmanned boat into account in the kinematic model of the unmanned boat, and then performs expected compensation from the three directions of the stern direction, the side of the ship, and the rotation of the unmanned boat. The estimation of the interference force of this method does not take into account the actual situation of the oil boom, and the compensation method requires accurate known parameters of the kinematic model of the unmanned boat, but the kinematic model parameters of the unmanned boat are difficult to accurately grasp.
[0005] In summary, in the method of collaboratively capturing oil spills using two boats connected by an oil boom, the method for estimating the interference force of the oil boom on the unmanned boat currently does not use the results of the oil boom hydrodynamic simulation to derive the oil boom interference force estimation function, and does not consider the influence of the oil boom interference force on the stern and turning direction of the unmanned boat. Therefore, the unmanned boat cannot stably reach the desired speed and desired heading. Summary of the invention
[0006] The present invention aims to solve the problem that two unmanned boats cannot stably track the expected speed and expected heading during oil spill containment due to the influence of oil booms. A disturbance force estimation and feedforward compensation control method and system for a two-boat collaborative system are provided.
[0007] The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system of the present invention comprises:
[0008] Step 1: Use hydrodynamic simulation software to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process, fit the simulation results under different working conditions, and obtain the interference force function of the oil boom on the two unmanned boats;
[0009] Step 2: Using the interference force function of the oil boom on the unmanned boat, the current position and speed of the two unmanned boats, calculate the direction of the interference force of the oil boom on the two unmanned boats, and then obtain the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats;
[0010] Step 3: Using the propeller thrust formula and the rotational torque formula of the unmanned boat propeller, and utilizing the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats, calculate the rudder angle compensation value of the two unmanned boat steering gears and the rotational speed compensation value of the propeller of the propeller;
[0011] Step 4: Calculate the rudder angles of the two unmanned boats' steering gears and the speeds of the propellers of the thrusters without considering the interference of the oil boom;
[0012] Step 5: Use the rudder angle compensation values of the two unmanned boat rudders and the speed compensation values of the propellers of the propellers calculated in step 3 to compensate for the rudder angle of the rudder and the speed of the propellers described in step 4, and obtain the compensated propeller speed values of the two unmanned boats and the rudder angle of the rudder.
[0013] Further, in the present invention, in step 1, the hydrodynamic simulation calculation software is used to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process, and the shape equation of the oil boom is implemented using the catenary equation:
[0014]
[0015] Wherein, y represents the horizontal coordinate axis of the oil boom coordinate system, and x represents the vertical coordinate axis of the oil boom coordinate system;
[0016]
[0017] Where a is the distance between the two ends of the boom, s is the length of the boom, and v = |x 1 -x 2 | is the difference in the ordinates of the two end points of the oil boom in the oil boom coordinate system, h = |y 1 -y 2 | is the difference in the horizontal coordinates of the two end points of the oil boom in the oil boom coordinate system.
[0018] Furthermore, in the present invention, in step 1, the calculation method of the interference force function of the oil boom on the two unmanned boats is the same, and the interference force function of the oil boom on the unmanned boat 1 is:
[0019]
[0020] Among them, F 1 is the interference force of the oil boom on the unmanned boat 1, v is the movement speed of the oil boom, and d is the distance between the two ends of the oil boom model;
[0021]
[0022] Oil boom end points A l , B l The coordinates (x 1 ,y 1 )、(x 2 ,y 2 )for:
[0023]
[0024] (x 1l ,y 1l )、(x 2l ,y 2l ) is the center point of the two unmanned boats in the oil boom coordinate system O w -X w Y w The coordinates inside, is the boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 1 b1 -X b1 Y b1 The angle between is the boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 2 b2 -X b2 Y b2 The angle between 1 is the length of the unmanned boat 1, L 2 is the length of the unmanned boat 2.
[0025] Furthermore, in the present invention, in step 2, the calculation method of the direction of the interference force of the oil boom on the two unmanned boats is the same, wherein the direction of the interference force of the oil boom on the unmanned boat 1 is for:
[0026]
[0027] in,
[0028]
[0029] In the formula, is the coordinate system O of the oil boom w -X w Y w The direction of the disturbance force on the unmanned boat 1 is w X w Axis angle; x'(y 1 ) represents the derivative of the shape equation of the oil boom at the end point A of the oil boom l Y-axis 1 The value of the direction;
[0030]
[0031] In the formula, is the oil boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 1 b1 -X b1 Y b1 The angle between ψ 1 It is known that the unmanned boat 1 is in the geodetic coordinate system O E -X E Y E The heading angle under E Axis and X b1 The angle between 2 It is known that the unmanned boat 2 is in the geodetic coordinate system O E -X E Y E The heading angle under E Axis and X b2 Angle.
[0032] Furthermore, in the present invention, in step 2, the method for calculating the component force of the oil boom interference force along the stern direction of the two unmanned boats is the same, wherein the component force F of the oil boom interference force along the stern direction of the unmanned boat 1 is uw1 :
[0033]
[0034] Furthermore, in the present invention, in step 2, the calculation method of the rotational torque generated by the interference force on the two unmanned boats is the same, wherein the rotational torque T generated by the interference force on the unmanned boat 1 is rw1 for:
[0035]
[0036] Among them, F vw1is the component of the interference force of the oil boom along the transverse direction of the hull of the unmanned boat 1;
[0037]
[0038] Further, in the present invention, in step 3, the method for calculating the rudder angle compensation value of the two unmanned boats is the same, and the rudder angle compensation value Δδ of the rudder of the unmanned boat 1 is calculated. 1 The specific method is:
[0039] The formula for the rotational torque generated by the rudder force generated by the rudder of the unmanned boat 1 on the unmanned boat 1 is:
[0040]
[0041] Derived to obtain the steering angle δ of the unmanned boat 1 1 :
[0042]
[0043] Among them, M 1 is the rotation torque generated by the rudder force of the steering gear of the unmanned boat 1 on the unmanned boat 1, ρ is the density of water, V R1 is the steering speed, A R1 is the rudder area, C N1 is the rudder force coefficient, δ 1 is the rudder angle of the steering gear of the unmanned boat 1;
[0044] Then the disturbance force is used to generate a rotation torque T on the unmanned boat 1. rw1 , the calculated compensation value of the rudder angle of the unmanned boat 1 is:
[0045]
[0046] Furthermore, in the present invention, in step 3, the method for calculating the speed compensation value of the propellers of the two unmanned boats is the same, wherein the speed compensation value Δn of the propeller of the unmanned boat 1 is calculated. 1 The specific method is:
[0047] First, calculate the thrust T produced by the propeller of the unmanned boat 1 according to the propeller thrust formula 1 , and then calculate the thrust T 1 The corresponding propeller speed n 1 ;
[0048] T 1 =K T1 ρn 1 2 D 1 4
[0049]
[0050] Among them, K T1 is the propeller thrust coefficient, ρ is the water density, D 1 is the diameter of the propeller;
[0051] Then use the component force F of the oil boom interference force along the stern direction of the unmanned boat 1 uw1 , calculate the compensation value of the propeller speed of the unmanned boat 1:
[0052]
[0053] Furthermore, in the present invention, in step 5, the method for obtaining the compensated propeller speed values of the two unmanned boats is the same, wherein the expected value of the propeller speed value of the unmanned boat 1 after compensation is:
[0054] n d1 =Δn 1 +n l1
[0055] Among them, n l1 is the speed of the propeller of the unmanned boat 1 without considering the interference of the oil boom;
[0056]
[0057] Among them, e v1 is the current speed deviation, e v1 =V d1 -V 1 , V d1 is the expected speed of the known unmanned boat 1, V 1 is the real-time speed of the unmanned boat 1.
[0058] Furthermore, in the present invention, in step 5, the method for obtaining the rudder angles of the two unmanned boat rudders after compensation is the same, wherein the expected value of the rudder angle of the rudder of the unmanned boat 1 after compensation is:
[0059] δ d1 =Δδ 1 +δ l1
[0060] Among them, δ l1 is the rudder angle of the two unmanned boats without considering the interference of the oil boom;
[0061]
[0062] Among them, e ψ1 The current heading deviation of unmanned boat 1, e ψ1 =ψ d1 -ψ 1 , ψ d1 is the expected heading of the known unmanned boat 1, ψ 1is the real-time heading of the unmanned boat 1, k p1 , k d1 , k i1 are the control parameters of the proportional term, differential term and integral term of the PID controller of the unmanned boat 1, and t is the time.
[0063] The flexible cable interference force estimation and feedforward compensation system of the dual-boat cooperative system includes: a first subtractor, a stern direction PID bow controller, a first adder, a compensation amount calculation module, an oil boom interference force calculation module, a second subtractor, a PID speed controller and a second adder;
[0064] A first subtractor is used to respectively perform a difference between the real-time heading signals of the two unmanned boats and the desired heading signal of the unmanned boat, and the two differences are output to a PID heading controller;
[0065] The PID heading controller uses a PID algorithm to calculate the desired rudder angle signals of the two unmanned boat steering gears;
[0066] A first adder is used to sum the expected rudder angle signals of the two unmanned boat steering gears and the corresponding compensation values of the rudder angles of the rudder gears to obtain the expected values of the rudder angles of the two unmanned boats after compensation; and the expected values of the rudder angles of the two unmanned boats after compensation are fed back to the rudder gears of the two unmanned boats;
[0067] A second subtractor is used to make a difference between the expected speed and the real-time speed of the two unmanned boats; and the two differences are output to a PID speed controller, which uses a PID algorithm to calculate the expected speed signals of the propellers of the two unmanned boats respectively;
[0068] A second adder is used to sum the expected speed signals of the two unmanned boat propellers and the compensation value of the propeller speed to obtain the expected values of the propeller speeds of the two unmanned boats after compensation; and the expected values of the propeller speeds of the two unmanned boats are fed back to the propellers of the two unmanned boats accordingly;
[0069] The compensation values of the rotation speeds of the propellers of the two unmanned boats and the compensation values of the rudder angles are both calculated by the compensation amount calculation module;
[0070] The rotation moment generated by the interference force on the unmanned boat and the component force of the oil boom interference force along the lateral direction of the unmanned boat hull are calculated by the oil boom interference force calculation module;
[0071] The oil boom interference force calculation module first uses hydrodynamic simulation calculation software to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process to obtain an interference force function, and then uses the interference force function and the current positions of the two unmanned boats and the interference force direction to calculate the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats;
[0072] The oil boom interference force calculation module uses the unmanned boat propeller thrust formula and the component of the interference force along the stern direction of the two unmanned boats to calculate the compensation amount of the propeller rotation speed of the two unmanned boats, and uses the rotational torque calculation formula generated by the two unmanned boat steering gears on the unmanned boats and the rotational torque generated by the interference force on the two unmanned boats to calculate the rudder angle compensation amount of the unmanned boat steering gear corresponding to the rotational torque.
[0073] The interference force estimation and feedforward compensation control method of the dual-boat cooperative system described in the present invention uses hydrodynamic simulation calculation software to calculate the interference force generated by the oil boom on the unmanned boat under various working conditions, and fits the calculation data results to obtain the interference force function generated by the oil boom on the unmanned boat. According to the speed and spacing of the two unmanned boats and the oil boom interference force function, the interference force generated by the oil boom on the unmanned boat can be obtained, and then the speed compensation of the unmanned boat propeller and the rudder angle compensation of the unmanned boat steering gear are performed, so that the oil boom can stably track the expected speed and expected heading, thereby enabling the unmanned boat to complete the oil spill recovery task more quickly and accurately. The oil boom interference force calculation and unmanned boat motion compensation method have the advantages of simplicity, accuracy and strong anti-interference ability. BRIEF DESCRIPTION OF THE DRAWINGS
[0074] Figure 1 It is the dimension drawing of oil boom model;
[0075] Figure 2 It is the grid diagram of the oil boom model;
[0076] Figure 3 This is the calculation domain diagram of the hydraulic simulation calculation of the oil boom;
[0077] Figure 4 This is a schematic diagram of oil spill recovery using dual unmanned boats;
[0078] Figure 5 This is the analysis diagram of the interference force of the oil boom on the unmanned boat;
[0079] Figure 6 This is the feedforward compensation control flow chart of the unmanned boat;
[0080] Figure 7 It is a flow chart of the method of the present invention. DETAILED DESCRIPTION
[0081] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.
[0082] Specific implementation method 1: refer to Figures 1 to 7 Specifically describing this embodiment, the disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system described in this embodiment includes:
[0083] Step 1: Use hydrodynamic simulation software to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process, fit the simulation results under different working conditions, and obtain the interference force function of the oil boom on the two unmanned boats;
[0084] Step 2: Using the interference force function of the oil boom on the unmanned boat, the current position and speed of the two unmanned boats, calculate the direction of the interference force of the oil boom on the two unmanned boats, and then obtain the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats;
[0085] Step 3: Using the propeller thrust formula of the unmanned boat propeller, using the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats, calculate the rudder angle compensation value of the two unmanned boat steering gears and the rotation speed compensation value of the propeller of the propeller;
[0086] Step 4: Calculate the rudder angles of the two unmanned boats' steering gears and the speeds of the propellers of the thrusters without considering the interference of the oil boom;
[0087] Step 5: Use the rudder angle compensation values of the two unmanned boat rudders and the speed compensation values of the propellers of the propellers calculated in step 3 to compensate for the rudder angle of the rudder and the speed of the propeller described in step 4, and obtain the expected values of the propeller speed values of the two unmanned boats and the rudder angle of the rudder after compensation.
[0088] In the present invention, different working conditions refer to situations where the distance and speed between the two ends of the oil boom are different.
[0089] Further, in the present invention, in step 1, the hydrodynamic simulation calculation software is used to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process, and the shape equation of the oil boom is implemented using the catenary equation:
[0090]
[0091] Wherein, y represents the horizontal coordinate of the oil boom coordinate system, and x represents the vertical coordinate of the oil boom coordinate system;
[0092]
[0093] Where a is the distance between the two ends of the boom, s is the length of the boom, and v = |x 1 -x 2| is the difference in the ordinates of the two end points of the oil boom in the oil boom coordinate system, h = |y 1 -y 2 | is the difference in the horizontal coordinates of the two end points of the oil boom in the oil boom coordinate system.
[0094] Furthermore, in the present invention, in step 1, the calculation method of the interference force function of the oil boom on the two unmanned boats is the same, and the interference force function of the oil boom on the unmanned boat 1 is:
[0095]
[0096] Among them, F 1 is the interference force of the oil boom on the unmanned boat 1, v is the movement speed of the oil boom, and d is the distance between the two ends of the oil boom model;
[0097]
[0098] Oil boom end points A l , B l The coordinates (x 1 ,y 1 )、(x 2 ,y 2 )for:
[0099]
[0100] (x 1l ,y 1l )、(x 2l ,y 2l ) is the center point of the two unmanned boats in the oil boom coordinate system O w -X w Y w The coordinates inside, is the boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 1 b1 -X b1 Y b1 The angle between is the boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 2 b2 -X b2 Y b2 The angle between 1 is the length of unmanned boat 1, and is the length of unmanned boat 2.
[0101] Furthermore, in the present invention, in step 2, the calculation method of the direction of the interference force of the oil boom on the two unmanned boats is the same, wherein the direction of the interference force of the oil boom on the unmanned boat 1 is for:
[0102]
[0103] in,
[0104]
[0105] In the formula, is the coordinate system O of the oil boom w -X w Y w The direction of the disturbance force on the unmanned boat 1 is w X w Axis angle; x'(y 1 ) represents the derivative of the shape equation of the oil boom at the end point A of the oil boom l Y-axis 1 The value of
[0106]
[0107] In the formula, is the boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 1 b1 -X b1 Y b1 The angle between ψ 1 It is known that the unmanned boat 1 is in the geodetic coordinate system O E -X E Y E The heading angle under E Axis and X b1 The angle between 2 It is known that the unmanned boat 2 is in the geodetic coordinate system O E -X E Y E The heading angle under E Axis and X b2 Angle.
[0108] Furthermore, in the present invention, in step 2, the method for calculating the component force of the oil boom interference force along the stern direction of the two unmanned boats is the same, wherein the component force F of the oil boom interference force along the stern direction of the unmanned boat 1 is uw1 :
[0109]
[0110] Further, in the present invention, in step 2, the calculation method of the rotational torque generated by the interference force on the two unmanned boats is the same, wherein the rotational torque T generated by the interference force on the unmanned boat 1 is rw1 for:
[0111]
[0112] Among them, F vw1 is the component of the interference force of the oil boom along the transverse direction of the hull of the unmanned boat 1;
[0113]
[0114] Further, in the present invention, in step 3, the method for calculating the rudder angle compensation value of the two unmanned boats is the same, and the rudder angle compensation value Δδ of the rudder of the unmanned boat 1 is calculated. 1 The specific method is:
[0115] The formula for the rotational torque generated by the rudder force generated by the rudder of the unmanned boat 1 on the unmanned boat 1 is:
[0116]
[0117] Derived to obtain the steering angle δ of the unmanned boat 1 1 :
[0118]
[0119] Among them, M 1 is the rotation torque generated by the rudder force of the steering gear of the unmanned boat 1 on the unmanned boat 1, ρ is the density of water, V R1 is the steering speed, A R1 is the rudder area, C N1 is the rudder force coefficient, δ 1 is the rudder angle of the steering gear of the unmanned boat 1;
[0120] Then the disturbance force is used to generate a rotation torque T on the unmanned boat 1. rw1 , the calculated compensation value of the rudder angle of the unmanned boat 1 is:
[0121]
[0122] Furthermore, in the present invention, in step 3, the method for calculating the speed compensation value of the propellers of the two unmanned boats is the same, wherein the speed compensation value Δn of the propeller of the unmanned boat 1 is calculated. 1 The specific method is:
[0123] First, according to the thrust T generated by the propeller of the unmanned boat 1 1 , calculate the thrust T 1 The corresponding propeller speed n 1 ;
[0124] T 1 =K T1 ρn 1 2 D 14
[0125]
[0126] Among them, K T1 is the propeller thrust coefficient, ρ is the water density, D 1 is the diameter of the propeller;
[0127] Then use the component force F of the oil boom interference force along the stern direction of the unmanned boat 1 uw1 , calculate the compensation value of the propeller speed of the unmanned boat 1:
[0128]
[0129] Furthermore, in the present invention, the method for obtaining the compensated propeller speed values of the two unmanned boats is the same, wherein the expected value of the propeller speed value of the unmanned boat 1 after compensation is:
[0130] n d1 =Δn 1 +n l1
[0131] Among them, n l1 is the speed of the propeller of the unmanned boat 1 without considering the interference of the oil boom;
[0132]
[0133] Among them, e v1 is the current speed deviation, e v1 =V d1 -V 1 , V d1 is the expected speed of the known unmanned boat 1, V 1 is the real-time speed of the unmanned boat 1.
[0134] Furthermore, in the present invention, in step 5, the method for obtaining the rudder angles of the two unmanned boat rudders after compensation is the same, wherein the expected value of the rudder angle of the rudder of the unmanned boat 1 after compensation is:
[0135] δ d1 =Δδ 1 +δ l1
[0136] Among them, δ l1 is the rudder angle of the two unmanned boats without considering the interference of the oil boom;
[0137]
[0138] Among them, e ψ1 The current heading deviation of unmanned boat 1, e ψ1 =ψ d1-ψ 1 , ψ d1 is the expected heading of the known unmanned boat 1, ψ 1 is the real-time heading of the unmanned boat 1, k p1 , k d1 , k i1 are the control parameters of the proportional term, differential term and integral term of the PID controller of the unmanned boat 1, and t is the time.
[0139] The interference force estimation and feedforward compensation control system of the dual-boat cooperative system includes: a first subtractor 1, a PID heading controller 2, a first adder 3, a compensation amount calculation module 4, an oil boom interference force calculation module 5, a second subtractor 6, a PID speed controller 7 and a second adder 8;
[0140] A first subtractor 1 is used to make a difference between the real-time heading signals of the two unmanned boats and the expected heading signal of the unmanned boat, and the two differences are output to a PID heading controller 2;
[0141] The PID heading controller 2 calculates the desired rudder angle signals of the two unmanned boat steering gears using a PID algorithm;
[0142] The first adder 3 is used to sum the expected rudder angle signals of the two unmanned boat steering gears and the corresponding compensation values of the rudder angles of the rudder gears to obtain the expected values of the rudder angles of the two unmanned boats after compensation, and the expected values of the rudder angles of the two unmanned boats after compensation are fed back to the rudder gears of the two unmanned boats;
[0143] A second subtractor 6 is used to make a difference between the expected speed and the real-time speed of the two unmanned boats; and the two differences are output to a PID speed controller 7, which uses a PID algorithm to calculate the expected speed signals of the propellers of the two unmanned boats respectively;
[0144] The second adder 8 is used to sum the expected speed signals of the two unmanned boat propellers and the compensation value of the propeller speed to obtain the expected values of the propeller speeds of the two unmanned boats after compensation; the expected values of the propeller speeds of the two unmanned boats are fed back to the propellers of the two unmanned boats accordingly;
[0145] The compensation values of the rotation speeds of the propellers of the two unmanned boats and the compensation values of the rudder angles are calculated by the compensation amount calculation module 4;
[0146] The rotation moment generated by the interference force on the unmanned boat and the component force of the oil boom interference force along the lateral direction of the unmanned boat hull are calculated by the oil boom interference force calculation module 5;
[0147] The oil boom interference force calculation module 5 first uses hydrodynamic simulation calculation software to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process to obtain an interference force function, and then uses the interference force function and the current positions of the two unmanned boats and the interference force direction to calculate the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats;
[0148] The oil boom interference force calculation module 4 uses the unmanned boat propeller thrust formula and the component of the interference force along the stern direction of the two unmanned boats to calculate the compensation amount of the propeller speed of the two unmanned boats propellers, and uses the rotational torque calculation formula generated by the two unmanned boat steering gears on the unmanned boats and the rotational torque generated by the interference force on the two unmanned boats to calculate the rudder angle compensation amount of the unmanned boat steering gear corresponding to the rotational torque. Specific embodiment:
[0150] Specifically, the shape equation of the oil boom can be regarded as a general catenary equation, as shown below:
[0151]
[0152] Where a is the distance between the two ends of the boom, s is the length of the boom, and v = |x 1 -x 2 | is the difference in the ordinates of the two end points of the oil boom in the oil boom coordinate system, h = |y 1 -y 2 | is the difference in the horizontal coordinates of the two end points of the oil boom in the oil boom coordinate system.
[0153] Compared with the real oil boom towed by the unmanned boat, the oil boom model was built according to the scale ratio of 20:1. The specific dimensions of the model are as follows: Figure 1 As shown, R 1 is the radius of the upper edge of the oil boom, the thickness of the oil boom is H, the height of the oil boom is L, and the radius of the lower edge of the oil boom is R 2 .
[0154] According to the above-mentioned specific size of the oil boom as the cross-sectional area of the oil boom, equation (1) is used as the shape equation of the oil boom, and 7 groups of oil boom models with a total length of s = 10m and a distance between the two ends of a = 4m, 4.5m, 5m, 5.5m, 6m, 6.5m, and 7m are established. Before the hydrodynamic simulation calculation of the oil boom model, the oil boom model is meshed to obtain the mesh diagram of the oil boom model, as shown in Figure 2 shown.
[0155] The hydrodynamic simulation calculations were performed on the above 7 groups of oil boom models at three speeds v = 0.5m / s, 1.0m / s, and 1.5m / s.
[0156] The computational domain of the hydrodynamic simulation is as follows: Figure 3 As shown, the length of the inlet section is twice the characteristic length, the length of the outlet section is four times the characteristic length, the height of the upper and lower surfaces is twice the characteristic length, and the height of the left and right surfaces is twice the characteristic length.
[0157] The 7 groups of oil boom models established above were subjected to hydrodynamic simulation calculations at three speeds of v = 0.5 m / s, 1.0 m / s, and 1.5 m / s to obtain the interference force of the oil boom on the unmanned boat. The calculation results are shown in Table 1:
[0158] Table 1. Results of hydraulic simulation calculation of oil boom
[0159]
[0160]
[0161] According to the calculation results, the functional relationship between the interference force of the oil boom on an unmanned boat and the speed v and the distance d between the two ends can be fitted:
[0162]
[0163] Among them, F 1 is the interference force of the oil boom on the unmanned boat 1, v is the movement speed of the oil boom, and d is the distance between the two ends of the oil boom model;
[0164] Step 2: Know the positions of the two unmanned boats (x 1l ,y 1l )、(x 2l ,y 2l ), speed v 1 、v 2 , calculate the direction of the interference force of the oil boom on the unmanned boat Calculate the component force F of the interference force of the oil boom on the unmanned boat along the stern direction of the unmanned boat 1 uw1 , the rotational torque T generated by the interference force of the oil boom on the unmanned boat rw1 ;
[0165] Specifically, Figure 4 and 5 As shown, O E -X E Y E is the geodetic coordinate system, O b1 -X b1 Y b1 is the hull coordinate system of the unmanned boat 1, O b1 The origin is the center of the unmanned boat 1, X b1 The positive direction of the Y axis is the stern direction of the unmanned boat 1. b1 The positive direction of the axis is along the lateral direction of the left side of the unmanned boat 1; b2 -X b2Y b2 is the hull coordinate system of the unmanned boat 2, O b2 The origin is the center of the hull of the unmanned boat 2, X b2 The positive direction of the Y axis is the stern direction of the unmanned boat 2. b2 The positive direction of the axis is along the lateral direction of the left side of the unmanned boat 2. w -X w Y w is the boom coordinate system, X w The positive direction of the axis is along ψ o Direction; where:
[0166]
[0167] In the formula, ψ 1 It is known that the unmanned boat 1 is in the geodetic coordinate system O E -X E Y E The heading angle under E Axis and X b1 The angle between 2 It is known that the unmanned boat 2 is in the geodetic coordinate system O E -X E Y E The heading angle under X E Axis and X b2 Angle.
[0168] USV1 and USV2 (unmanned boat 1 and unmanned boat 2) are known to have their centers in the oil boom coordinate system O. w -X w Y w The position (x 1l ,y 1l )、(x 2l ,y 2l ), the length of the oil boom s = 10m. Taking USV1 as the research object, from formula (1) to formula (2), we can get w -X w Y w The towing force of the oil boom on USV1 in the coordinate system (the direction of the interference force on the unmanned boat 1 is the same as that on O w X w The direction of the axis angle) is:
[0169]
[0170] In the formula, O w -X w Y w The disturbance force on USV1 and O in the coordinate system w X w Axis angle. 1) represents the derivative of the shape equation of the oil boom at the end point A of the oil boom l Y-axis 1 The value of .
[0171] Coordinates A at both ends of the oil boom l (x 1 ,y 1 ) and B l (x 2 ,y 2 )for:
[0172]
[0173] For oil boom O w -X w Y w The coordinate system and the hull coordinate system O of the unmanned boat 1 b1 -X b1 Y b1 The angle between For oil boom O w -X w Y w The coordinate system and the hull coordinate system O of the unmanned boat 2 b2 -X b2 Y b2 The angle between 1 The length of unmanned boat 1. 2 It is the length of unmanned boat 2.
[0174] O w -X w Y w The coordinate system is the same as the hull coordinate system of USV1 b1 -X b1 Y b1 The angle between them is:
[0175]
[0176] Through coordinate transformation, we can get USV1 at O b1 -X b1 Y b1 The direction of the drag force in the coordinate system is:
[0177]
[0178] Substituting the distance d between the two end points of the oil boom and the movement speed v of the oil boom into formula (3), we can obtain the interference force of the oil boom on the unmanned boat 1, and then we can obtain the component forces of the interference of the oil boom on each coordinate axis of the boat coordinate system. The component force along the stern direction of the unmanned boat 1 is:
[0179]
[0180] The component force along the lateral direction of the unmanned boat 1 is:
[0181]
[0182] Since the oil boom is connected to the stern of the unmanned boat 1 l At , due to the existence of the component force along the lateral direction of the unmanned boat, a rotation torque will be generated on the unmanned boat, and the rotation torque is:
[0183]
[0184] Where L 1 is the length of the unmanned boat 1.
[0185] Step 3: According to the interference force generated by the oil boom on the unmanned boat 1, the force F is divided along the stern direction. uw1 , Rotational torque T rw1 , calculate the rudder angle compensation value Δδ for the rudder of the unmanned boat 1 1 , the compensation value Δn of the speed of the unmanned boat propeller 1 .
[0186] Specifically, the interference force generated by the oil boom on the unmanned boat 1, the component force in the stern direction of the unmanned boat 1 can be offset by providing a rotation speed compensation amount to the propeller of the unmanned boat 1. The component force along the lateral direction of the unmanned boat 1 will generate a rotational torque on the unmanned boat 1, which can be offset by providing a rudder angle compensation amount to the steering gear of the unmanned boat 1;
[0187] The thrust formula generated by the propeller of the unmanned boat 1 is:
[0188] T 1 =K T1 ρn 1 2 D 1 4 (12)
[0189] K T1 is the thrust coefficient of the propeller, ρ is the density of water, n 1 is the propeller speed, D 1 is the diameter of the propeller; from this, the thrust T generated by the propeller of the unmanned boat 1 can be obtained 1 The corresponding speed n 1 for:
[0190]
[0191] The drag force F generated by the oil boom on the unmanned boat 1 in the stern direction uw1Substituting into formula (13), the compensation value of the propeller speed of the unmanned boat is calculated as:
[0192]
[0193] Similarly, the rudder force generated by the steering gear of the unmanned boat 1 generates a rotation torque on the unmanned boat:
[0194]
[0195] Where ρ is the density of water, V R1 is the steering speed, A R1 is the rudder area, C N1 is the rudder force coefficient, δ 1 is the steering angle of the unmanned boat 1, L 1 is the captain of the unmanned boat 1; the corresponding rudder angle δ can be obtained as:
[0196]
[0197] The rotation moment T generated by the oil boom on the unmanned boat 1 in the rotation direction of the unmanned boat 1 rw1 Substituting into formula (16), the compensation value of the rudder angle of the steering gear of the unmanned boat 1 is calculated as:
[0198]
[0199] Step 4: Calculate the rudder angle δ that the rudder of the unmanned boat 1 needs to execute before compensation (without considering the interference of the oil boom) l1 , calculate the speed n that the propeller of the unmanned boat 1 needs to execute before compensation l1 , according to the compensation value of the rudder angle Δδ 1 , speed compensation value Δn 1 , the speed value n that the propeller of the unmanned boat 1 needs to execute after compensation d1 , the rudder angle δ that the rudder of the unmanned boat 1 needs to execute after compensation d1 .
[0200] Specifically, the rudder angle δ that the rudder of the unmanned boat 1 needs to perform before compensation is calculated. l1 , ψ d1 is the expected heading of the known unmanned boat 1, ψ 1 is the known heading of the unmanned boat 1, then the real-time heading deviation of the unmanned boat 1 in the stern direction is:
[0201]
[0202] According to the current heading deviation, the steering angle that the steering gear of the unmanned boat 1 needs to execute before compensation is:
[0203]
[0204] In the formula, k p1 , k d1 , k i1 are the control parameters of the proportional term, differential term and integral term of the PID controller of the unmanned boat 1, and t is the time.
[0205] Then the rudder angle that the servo of the unmanned boat 1 needs to execute after compensation is:
[0206] δ d1 =Δδ 1 +δ l1 (twenty two)
[0207] Calculate the speed n that the propeller of the unmanned boat 1 needs to execute before compensation l1 , V d1 is the expected speed of the known unmanned boat 1, V 1 is the known speed of the unmanned boat 1, then the current speed deviation of the unmanned boat 1 is:
[0208] e v1 =V d1 -V 1 (20)
[0209] According to the current speed deviation, the speed that the propeller of the unmanned boat 1 needs to execute before compensation is:
[0210]
[0211] Then the speed that the propeller of the unmanned boat 1 needs to execute after compensation is:
[0212] n d1 =Δn 1 +n l1 (twenty three)
[0213] like Figure 6 As shown, the steering engine of the unmanned boat 1 or the unmanned boat 2 will execute the compensated steering angle δ d1 or δ d2 , the propeller of the unmanned boat will execute the compensated speed n d1 or d2 , the speed and heading of the unmanned boat can reach the expected speed and heading under the influence of the oil boom, thereby completing the oil spill containment mission.
[0214] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that features described in conjunction with individual embodiments may be used in other described embodiments.
Claims
1. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system is characterized by: include: Step 1: Use hydrodynamic simulation software to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process, fit the simulation results under different working conditions, and obtain the interference force function of the oil boom on the two unmanned boats; Step 2: Using the interference force function of the oil boom on the unmanned boat, the current position and speed of the two unmanned boats, calculate the direction of the interference force of the oil boom on the two unmanned boats, and then obtain the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats; Step 3: Using the propeller thrust formula and the rotational torque formula of the unmanned boat propeller, and utilizing the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats, calculate the rudder angle compensation value of the two unmanned boat steering gears and the rotational speed compensation value of the propeller of the propeller; Step 4: Calculate the rudder angles of the two unmanned boats' steering gears and the speeds of the propellers of the thrusters without considering the interference of the oil boom; Step 5: Use the rudder angle compensation values of the two unmanned boat rudders and the speed compensation values of the propellers of the propellers calculated in step 3 to compensate for the rudder angle of the rudder and the speed of the propeller described in step 4, and obtain the expected values of the propeller speed values of the two unmanned boats and the rudder angle of the rudder after compensation.
2. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 1 is characterized in that: In step 1, the hydrodynamic simulation calculation software is used to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process, and the shape equation of the oil boom is implemented. The shape equation of the oil boom adopts the catenary equation: Wherein, y represents the horizontal coordinate of the oil boom coordinate system, and x represents the vertical coordinate of the oil boom coordinate system; Among them, a is the distance between the two end points of the oil boom, s is the length of the oil boom, v = |x1-x2| is the difference in the vertical coordinates of the two end points of the oil boom in the oil boom coordinate system, and h = |y1-y2| is the difference in the horizontal coordinates of the two end points of the oil boom in the oil boom coordinate system.
3. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 1 is characterized in that: In step 1, the calculation method of the interference force function of the oil boom on the two unmanned boats is the same. The interference force function of the oil boom on the unmanned boat 1 is: Among them, F1 is the interference force of the oil boom on the unmanned boat 1, v is the movement speed of the oil boom, and d is the distance between the two ends of the oil boom model; Oil boom end points A l , B l The coordinates (x1, y1) and (x2, y2) are: (x 1l ,y 1l )、(x 2l ,y 2l ) is the center point of the two unmanned boats in the oil boom coordinate system O w -X w Y w The coordinates inside, is the boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 1 b1 -X b1 Y b1 The angle between is the oil boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 2 b2 -X b2 Y b2 The angle between them, L1 is the length of unmanned boat 1, and L2 is the length of unmanned boat 2.
4. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 3 is characterized in that: In step 2, the calculation method of the direction of the interference force of the oil boom on the two unmanned boats is the same, where the direction of the interference force of the oil boom on the unmanned boat 1 is for: in, In the formula, is the coordinate system O of the oil boom w -X w Y w The direction of the disturbance force on the unmanned boat 1 is w X w Axis angle; x'(y1) represents the derivative of the shape equation of the oil boom at the end point A of the oil boom l The value of the ordinate y1; In the formula, is the oil boom coordinate system O w -X w Y w and the hull coordinate system O of the unmanned boat 1 b1 -X b1 Y b1 The angle between ψ1 is the known value of unmanned boat 1 in the geodetic coordinate system O E -X E Y E The heading angle under E Axis and X b1 , ψ2 is the known angle between the unmanned boat 2 and the earth coordinate system O E -X E Y E The heading angle under E Axis and X b2 Angle.
5. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 4 is characterized in that: In step 2, the calculation method of the interference force of the oil boom along the stern direction of the two unmanned boats is the same, where the component force F of the interference force of the oil boom along the stern direction of the unmanned boat 1 is uw1 :
6. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 4 is characterized in that: In step 2, the calculation method of the rotation torque generated by the interference force on the two unmanned boats is the same, where the rotation torque T generated by the interference force on the unmanned boat 1 is rw1 for: Among them, F vw1 is the component of the interference force of the oil boom along the transverse direction of the hull of the unmanned boat 1; 7. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 6 is characterized in that: In step 3, the method for calculating the rudder angle compensation value of the rudder gear of the two unmanned boats is the same. The specific method for calculating the rudder angle compensation value Δδ1 of the rudder gear of the unmanned boat 1 is: The formula for the rotational torque generated by the rudder force generated by the rudder of the unmanned boat 1 on the unmanned boat 1 is: The rudder angle δ1 of the steering gear of the unmanned boat 1 is derived as follows: Among them, M1 is the rotation torque generated by the rudder force of the steering gear of the unmanned boat 1 on the unmanned boat 1, ρ is the density of water, V R1 is the steering speed, A R1 is the rudder area, C N1 is the rudder force coefficient, δ1 is the rudder angle of the steering gear of the unmanned boat 1; Then the disturbance force is used to generate a rotation torque T on the unmanned boat 1. rw1 , the calculated compensation value of the rudder angle of the unmanned boat 1 is:
8. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 7 is characterized in that: In step 3, the method for calculating the speed compensation value of the propellers of the two unmanned boats is the same, wherein the specific method for calculating the speed compensation value Δn1 of the propeller of the unmanned boat 1 is: First, the thrust T1 produced by the propeller of the unmanned boat 1 is calculated according to the thrust formula of the propeller, and then the propeller speed n1 corresponding to the thrust T1 is calculated; T1=K T1 ρn1 2 D1 4 Among them, K T1 is the thrust coefficient of the propeller, ρ is the density of water, and D1 is the diameter of the propeller; Then use the component force F of the oil boom interference force along the stern direction of the unmanned boat 1 uw1 , calculate the compensation value of the propeller speed of the unmanned boat 1:
9. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 8 is characterized in that: In step 5, the method for obtaining the compensated propeller speed values of the two unmanned boats is the same, wherein the expected value of the propeller speed value of the unmanned boat 1 after compensation is: n d1 =Δn1+n l1 Among them, n l1 is the speed of the propeller of the unmanned boat 1 without considering the interference of the oil boom; Among them, e v1 is the current speed deviation, e v1 =V d1 -V1,V d1 is the expected speed of the known unmanned boat 1, and V1 is the real-time speed of the unmanned boat 1.
10. The disturbance force estimation and feedforward compensation control method of the dual-vessel cooperative system according to claim 7, characterized in that: In step 5, the method for obtaining the rudder angles of the two unmanned boat rudders after compensation is the same, wherein the expected value of the rudder angle of the unmanned boat 1 rudder after compensation is: d d1 =Δδ1+δ l1 Among them, δ l1 is the rudder angle of the two unmanned boats without considering the interference of the oil boom; Among them, e ψ1 The current heading deviation of unmanned boat 1, e ψ1 =ψ d1 -ψ1,ψ d1 is the expected heading of the known unmanned boat 1, ψ1 is the real-time heading of the unmanned boat 1, k p1 , k d1 , k i1 are the control parameters of the proportional term, differential term and integral term of the PID controller of the unmanned boat 1, and t is the time.
11. The disturbance force estimation and feedforward compensation control system of the dual-vessel cooperative system is characterized by: include: A first subtractor (1), a PID heading controller (2), a first adder (3), a compensation amount calculation module (4), an oil boom interference force calculation module (5), a second subtractor (6), a PID speed controller (7) and a second adder (8); A first subtractor (1) is used to respectively perform a subtraction on the real-time heading signals of the two unmanned boats and the expected heading signal of the unmanned boat, and the two differences are output to a PID heading controller (2); The PID heading controller (2) calculates the expected rudder angle signals of the two unmanned boat steering gears using a PID algorithm; A first adder (3) is used to sum the expected rudder angle signals of the two unmanned boat steering gears and the corresponding compensation values of the rudder angles of the rudder gears to obtain the expected values of the rudder angles of the two unmanned boats after compensation, and the expected values of the rudder angles of the two unmanned boats after compensation are fed back to the rudder gears of the two unmanned boats; A second subtractor (6) is used to make a difference between the expected speed and the real-time speed of the two unmanned boats; and the two differences are output to a PID speed controller (7), wherein the PID speed controller (7) uses a PID algorithm to calculate the expected rotation speed signals of the propellers of the two unmanned boats respectively; A second adder (8) is used to sum the expected speed signals of the propellers of the two unmanned boats and the compensation value of the propeller speed to obtain the expected values of the propeller speeds of the two unmanned boats after compensation; and the expected values of the propeller speeds of the two unmanned boats are fed back to the propellers of the two unmanned boats accordingly; The compensation values of the rotation speeds of the propellers of the two unmanned boats and the compensation values of the rudder angles are both calculated by a compensation value calculation module (4); The rotational moment generated by the interference force on the unmanned boat and the component force of the oil boom interference force along the lateral direction of the unmanned boat hull are calculated by the oil boom interference force calculation module (5); The oil boom interference force calculation module (5) first uses hydrodynamic simulation calculation software to perform hydrodynamic simulation calculation on the oil boom in the dual-boat cooperative system during the oil capture process to obtain an interference force function, and then uses the interference force function and the current positions of the two unmanned boats and the interference force direction to calculate the component force of the interference force along the stern direction of the two unmanned boats and the rotational torque generated by the interference force on the two unmanned boats; The oil boom interference force calculation module (4) calculates the compensation amount of the rotation speed of the propellers of the two unmanned boats by using the thrust formula of the propellers of the unmanned boats and the component force of the interference force along the stern direction of the two unmanned boats, and calculates the rudder angle compensation amount of the unmanned boat rudder corresponding to the rotation torque by using the calculation formula of the rotation torque generated by the two unmanned boat rudders and the rotation torque generated by the interference force on the two unmanned boats.
