A method for controlling vertical speed of unmanned aerial vehicle in hovering state based on water discharge and injection system

Abstract

The present invention relates to the field of control technology of non-electrical variables, and in particular to a method for controlling the vertical speed of an unmanned aircraft in a hovering state based on a water discharge and injection system, comprising the following steps: obtaining the depth and pitch of the unmanned aircraft in real time, and obtaining the cumulative water discharge and injection volume; solving the vertical speed of the unmanned aircraft after low-pass filtering at the current moment; solving the vertical acceleration of the unmanned aircraft at the current moment by a vertical plane motion state observer; calculating the command water discharge and injection rate of the buoyancy tank flow regulation system by a vertical speed controller; discretizing the command water discharge and injection rate of the buoyancy tank flow regulation system, and converting it into a switch valve control signal of the buoyancy tank flow regulation system to control the vertical speed of the unmanned aircraft. The method provided by the present invention realizes the precise control of the vertical speed of large or super-large unmanned aircraft, and effectively improves the safety of large or super-large unmanned aircraft operating near the seabed.

Description

Technical Field

[0001] The present invention relates to the technical field of control of non-electrical variables, and in particular to a method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system. Background Art

[0002] Large or extra-large underwater unmanned vehicles generally refer to unmanned underwater platforms with a displacement of not less than 5 tons. They can carry multi-mission payload compartments to perform complex operational tasks such as search and rescue, detection, security and support, and offshore equipment delivery. They are underwater vehicles with the ability to navigate autonomously across sea areas and for long periods of time.

[0003] For medium and small underwater unmanned vehicles, the vertical speed can be stabilized by using the vertical thrusters arranged at the bow and stern. For large or super-large unmanned vehicles, vertical thrusters are not installed. In order to overcome the change in buoyancy caused by the change in seawater density when sailing across sea areas, a ballast water tank is usually configured to adjust the buoyancy through a water discharge and injection system. When a large or super-large unmanned vehicle sails from a sea area with a high seawater density to a sea area with a low density, the buoyancy decreases, and it is necessary to drain a proper amount of water to reduce its own weight. Conversely, it is necessary to inject a proper amount of water. This system is mainly used for static adjustment, that is, by adjusting the weight of the unmanned vehicle to maintain the balance between buoyancy and gravity.

[0004] Since large or super-large unmanned aerial vehicles are not equipped with vertical thrusters, and their water discharge and injection systems are usually only used to maintain the static balance of buoyancy and gravity, large or super-large underwater unmanned aerial vehicles often need to perform large-scale depth maneuvers or operate close to the seabed when performing search and rescue or detection missions in a zero-speed hovering state. When controlling the deepening process of a large or super-large underwater unmanned aerial vehicle in a hovering state, the vertical movement of the unmanned aerial vehicle may be out of control, causing the risk of the unmanned aerial vehicle colliding with the seabed when operating near the seabed. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system, which solves the problem that the vertical speed of an ultra-large unmanned aerial vehicle cannot be accurately controlled in a hovering state.

[0006] The present invention is achieved through the following technical solutions:

[0007] A method for controlling vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system comprises the following steps:

[0008] S1: Obtain the depth and trim of the unmanned vehicle in real time, and obtain the cumulative water discharge and injection volume of the buoyancy tank flow control system;

[0009] S2: According to the real-time acquired depth of the unmanned vehicle, the low-pass filtered vertical speed of the unmanned vehicle at the current moment is calculated by low-pass filtering difference method;

[0010] S3: According to the real-time acquired depth of the unmanned vehicle, the pitch of the unmanned vehicle and the accumulated water discharge volume, the vertical acceleration of the unmanned vehicle is calculated by the vertical plane motion state observer;

[0011] S4: The vertical speed controller calculates the command water discharge and injection rate of the buoyancy water tank flow control system according to the target vertical speed, the calculated vertical acceleration of the unmanned vehicle and the vertical speed of the unmanned vehicle after low-pass filtering at the current moment;

[0012] S5: After discretizing the command water discharge and injection rate of the buoyancy water tank flow regulation system according to the discretized output function, it is converted into a switch valve control signal of the buoyancy water tank flow regulation system to control the vertical speed of the unmanned vehicle.

[0013] Further, in step S2, the vertical speed of the unmanned aerial vehicle is calculated according to equation (1):

[0014] (1);

[0015] in: is the calculated value of vertical velocity difference of the unmanned vehicle, For unmanned aerial vehicles Depth measurement at the moment, For unmanned aerial vehicles Depth measurement at the moment, is the sampling duration, is the low-pass filter intermediate variable, is the low-pass filter time constant, For unmanned aerial vehicles The vertical speed after low-pass filtering at each moment, For unmanned aerial vehicles Vertical speed after low-pass filtering at all times; is the calculation step length.

[0016] Furthermore, the vertical plane motion state observer in step S3 is formula (2):

[0017] (2);

[0018] in: represents the system model matrix, represents the system input matrix, represents the observer gain coefficient matrix, represents the input signal, represents the system state matrix, represents the system output matrix, Indicates the cumulative amount of water discharged. represents the differential of the system state matrix.

[0019] Furthermore, the vertical plane motion state observer is expanded to obtain the expanded form of the vertical plane motion state observer as formula (3):

[0020] (3);

[0021] in: represents the vertical acceleration of the UAV, represents the estimated vertical velocity of the UAV, represents the differential of the estimated value of the UAV pitch angular velocity, represents the estimated value of the pitch angular velocity of the unmanned vehicle, represents the differential of the UAV pitch, Indicates the pitch of the unmanned aerial vehicle, represents the differential of the UAV depth estimate, represents the estimated depth of the unmanned vehicle, represents the measured value of the pitch of the unmanned aerial vehicle, represents the depth measurement value of the unmanned aerial vehicle, represents the vertical acceleration observer gain coefficient, represents the pitch angular acceleration observer gain coefficient, represents the gain coefficient of the pitch angular velocity observer, represents the depth rate observer gain coefficient, The vertical velocity coefficient represents the vertical acceleration, The pitch angular velocity coefficient representing the vertical acceleration, The pitch coefficient representing the vertical acceleration, The cumulative water discharge coefficient representing the vertical acceleration, The vertical velocity coefficient representing the pitch angular acceleration, The pitch velocity coefficient represents the pitch acceleration, The pitch coefficient represents the pitch acceleration, The cumulative water discharge coefficient representing the pitch angular acceleration, The vertical velocity coefficient representing the depth rate, The pitch coefficient representing the depth rate.

[0022] Furthermore, the expanded form of the vertical motion state observer is used to perform integral iteration calculations using the Euler method according to equation (4) to solve the vertical acceleration of the unmanned vehicle:

[0023] (4);

[0024] in: Unmanned aerial vehicle The estimated vertical velocity at time, Unmanned aerial vehicle The estimated vertical velocity at time, Unmanned aerial vehicle The estimated value of the pitch angular velocity at the moment, Unmanned aerial vehicle The estimated value of the pitch angular velocity at the moment, Unmanned aerial vehicle Tilt at all times, Unmanned aerial vehicle Tilt at all times, Unmanned aerial vehicle Depth estimate at the moment, Unmanned aerial vehicle Depth estimate at that moment.

[0025] Furthermore, the vertical speed controller in step S4 is expressed as formula (5):

[0026] (5);

[0027] in: is the command drain water injection rate, is the proportional coefficient of the vertical speed controller, is the vertical speed controller differential coefficient, is the target vertical velocity.

[0028] Furthermore, the discretization output function formula in step S5 is formula (6):

[0029] (6);

[0030] in: is the flow control valve control signal output after discretization, is the discretization threshold for control instructions.

[0031] Optimally, in step S1, the measurement frequencies of the depth of the unmanned vehicle, the trim of the unmanned vehicle, and the cumulative water discharge and injection volume of the buoyancy water tank flow control system are all 10 Hz.

[0032] Optimized, calculation step size The value is 0.1 seconds.

[0033] Optimized, low-pass filter time constant The value is 0.5 seconds.

[0034] Beneficial effects of the invention:

[0035] The present invention provides a method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system. The method calculates the water discharge and injection rate command signal of the buoyancy water tank flow regulation system by solving the vertical acceleration of the unmanned aerial vehicle and the vertical speed of the unmanned aerial vehicle after low-pass filtering at the current moment, and then controls the water discharge and injection of the water tank, thereby achieving precise control of the vertical speed of large or super-large unmanned aerial vehicles without vertical thrusters installed, effectively improving the safety of large or super-large unmanned aerial vehicles operating near the seabed. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION

[0037] A method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system, the flow chart of which is as follows: Figure 1 As shown, the specific steps include:

[0038] S1: Obtain the depth and trim of the unmanned vehicle in real time, and obtain the cumulative water discharge and injection volume of the buoyancy tank flow control system;

[0039] Specifically, for large or extra-large unmanned aerial vehicles that are not equipped with vertical thrusters, the depth of the unmanned aerial vehicle can be measured by a depth sensor during navigation, the cumulative drainage and injection volume can be obtained by adding up the single drainage and injection volumes measured by the flow meter on the drainage and injection pipeline, and the longitudinal inclination of the unmanned aerial vehicle can be measured by the inertial navigation system.

[0040] The measurement frequency of the unmanned vehicle depth, the unmanned vehicle pitch and the cumulative water discharge and injection volume of the buoyancy water tank flow control system can all be preferably 10 Hz.

[0041] S2: According to the real-time acquired depth of the unmanned vehicle, the low-pass filtered vertical speed of the unmanned vehicle at the current moment is calculated by low-pass filtering difference method;

[0042] Specifically, the vertical speed of the unmanned vehicle can be calculated according to formula (1):

[0043] (1);

[0044] in: is the calculated value of vertical velocity difference of the unmanned vehicle, For unmanned aerial vehicles Depth measurement at the moment, For unmanned aerial vehicles Depth measurement at the moment, is the sampling duration, is the low-pass filter intermediate variable, is the low-pass filter time constant, is the low-pass filter time constant, the specific value of which can be determined based on experience, and can be preferably 0.5 seconds. For unmanned aerial vehicles The vertical speed after low-pass filtering at each moment, For unmanned aerial vehicles Vertical speed after low-pass filtering at all times; To calculate the step length, calculate the step length The value of may preferably be 0.1 seconds.

[0045] S3: According to the real-time acquired depth of the unmanned vehicle, the pitch of the unmanned vehicle and the accumulated water discharge volume, the vertical acceleration of the unmanned vehicle is calculated by the vertical plane motion state observer;

[0046] Specifically, the vertical motion state observer is formula (2):

[0047] (2);

[0048] in: represents the system model matrix, , The vertical velocity coefficient represents the vertical acceleration, The pitch angular velocity coefficient representing the vertical acceleration, The pitch coefficient representing the vertical acceleration, The vertical velocity coefficient representing the pitch angular acceleration, The pitch velocity coefficient represents the pitch acceleration, The pitch coefficient represents the pitch acceleration, The vertical velocity coefficient representing the depth rate, The trim coefficient representing the depth rate;

[0049] represents the system input matrix, , The cumulative water discharge coefficient representing the vertical acceleration, The cumulative water discharge coefficient representing the pitch angular acceleration;

[0050] Here the system model matrix The coefficients in and the system input matrix The coefficients in can be determined by tank tests during the design phase of the underwater unmanned vehicle;

[0051] represents the observer gain coefficient matrix, , represents the vertical acceleration observer gain coefficient, represents the pitch angular acceleration observer gain coefficient, represents the gain coefficient of the pitch angular velocity observer, represents the depth rate observer gain coefficient;

[0052] Observer gain coefficient matrix The coefficients in can be obtained by using the pole placement method.

[0053] represents the system output matrix, ;

[0054] represents the input signal, , represents the measured value of the pitch of the unmanned aerial vehicle, represents the depth measurement value of the unmanned aerial vehicle;

[0055] represents the system state matrix, ,

[0056] represents the differential of the system state matrix, , represents the vertical acceleration of the UAV, represents the estimated vertical velocity of the UAV, represents the differential of the estimated value of the UAV pitch angular velocity, represents the estimated value of the pitch angular velocity of the unmanned vehicle, represents the differential of the UAV pitch, Indicates the pitch of the unmanned aerial vehicle, represents the differential of the UAV depth estimate, represents the estimated depth of the UAV;

[0057] Indicates the cumulative amount of water discharged and injected.

[0058] Furthermore, the vertical plane motion state observer is expanded to obtain the expanded form of the vertical plane motion state observer as formula (3):

[0059] (3);

[0060] Furthermore, the vertical acceleration of the unmanned vehicle can be solved by using the Euler method to perform integral iteration calculations based on the expanded form of the vertical motion state observer according to equation (4):

[0061] (4).

[0062] in: Unmanned aerial vehicle The estimated vertical velocity at time, Unmanned aerial vehicle The estimated vertical velocity at time, Unmanned aerial vehicle The estimated value of the pitch angular velocity at the moment, Unmanned aerial vehicle The estimated value of the pitch angular velocity at the moment, Unmanned aerial vehicle Tilt at all times, Unmanned aerial vehicle Tilt at all times, Unmanned aerial vehicle Depth estimate at the moment, Unmanned aerial vehicle Depth estimate at that moment.

[0063] S4: The vertical speed controller calculates the command water discharge and injection rate of the buoyancy water tank flow control system according to the target vertical speed, the calculated vertical acceleration of the unmanned vehicle and the vertical speed of the unmanned vehicle after low-pass filtering at the current moment;

[0064] Specifically, the vertical speed controller is formula (5):

[0065] (5);

[0066] in: is the command drain water injection rate, is the proportional coefficient of the vertical speed controller, is the differential coefficient of the vertical speed controller, is the target vertical velocity.

[0067] Here the target vertical speed is a constant that can be set manually. Based on the calculation of unmanned aerial vehicles Vertical speed after low-pass filtering and the vertical acceleration of the UAV , the command water injection rate can be calculated , the water injection rate can be calculated according to the command The buoyancy tank flow regulating system is controlled to achieve precise control of the vertical speed of large or super-large unmanned vehicles without vertical thrusters.

[0068] Through the above method, the vertical acceleration of the unmanned vehicle and the vertical speed of the unmanned vehicle after low-pass filtering at the current moment are solved, and then the command discharge and injection rates of the buoyancy water tank flow regulation system are calculated. The calculated command discharge and injection rates are used to control the discharge and injection of the water tank. This can achieve precise control of the vertical speed of large or super-large unmanned vehicles that are not equipped with vertical thrusters, prevent the risk of collision between the unmanned vehicle and the seabed when operating near the seabed, and effectively improve the safety of large or super-large unmanned vehicles operating near the seabed.

[0069] S5: After discretizing the command water discharge and injection rate of the buoyancy water tank flow regulation system according to the discretized output function, it is converted into a switch valve control signal of the buoyancy water tank flow regulation system to control the vertical speed of the unmanned vehicle.

[0070] Specifically, the discretized output function formula is (6):

[0071] (6);

[0072] in: is the flow control valve control signal output after discretization, is the discretization threshold for control instructions.

[0073] In formula (6), 1 represents water injection and -1 represents drainage.

[0074] The command water discharge and injection rate of the buoyancy water tank flow regulation system is discretized according to the discretized output function and converted into the switch valve control signal of the buoyancy water tank flow regulation system to control the vertical speed of the unmanned vehicle, which can reduce the operation frequency of the buoyancy water tank flow regulation system, thereby reducing energy consumption and equipment wear.

[0075] In summary, the present invention provides a method for controlling the vertical speed of an unmanned vehicle in a hovering state based on a water discharge and injection system, which realizes the precise control of the vertical speed of a large or super-large unmanned vehicle without a vertical thruster installed, and effectively improves the safety of large or super-large unmanned vehicles operating near the seabed.

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

Claims

1. A method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system, characterized in that: The steps include: S1: Obtain the depth and trim of the unmanned vehicle in real time, and obtain the cumulative water discharge and injection volume of the buoyancy water tank flow control system; S2: According to the real-time acquired depth of the unmanned vehicle, the low-pass filtered vertical speed of the unmanned vehicle at the current moment is calculated by low-pass filtering difference method according to formula (1); （1）； in: is the calculated value of vertical velocity difference of the unmanned vehicle, For unmanned aerial vehicles Depth measurement at the moment, For unmanned aerial vehicles Depth measurement at the moment, is the sampling duration, is the low-pass filter intermediate variable, is the low-pass filter time constant, For unmanned aerial vehicles The vertical speed after low-pass filtering at each moment, For unmanned aerial vehicles Vertical speed after low-pass filtering at all times; To calculate the step length; S3: According to the real-time acquired depth of the unmanned vehicle, the trim of the unmanned vehicle and the accumulated water discharge volume, the vertical acceleration of the unmanned vehicle at the current moment is calculated by the vertical plane motion state observer; The vertical motion state observer is formula (2): （2）； in: represents the system model matrix, represents the system input matrix, represents the observer gain coefficient matrix, represents the input signal, represents the system state matrix, represents the system output matrix, Indicates the cumulative amount of water discharged. represents the differential of the system state matrix; The vertical plane motion state observer is expanded to obtain the expanded form of the vertical plane motion state observer as formula (3): （3）； in: represents the vertical acceleration of the UAV, represents the estimated vertical velocity of the UAV, represents the differential of the estimated value of the UAV pitch angular velocity, represents the estimated value of the pitch angular velocity of the unmanned vehicle, represents the differential of the UAV pitch, Indicates the pitch of the unmanned aerial vehicle, represents the differential of the UAV depth estimate, represents the estimated depth of the unmanned vehicle, represents the measured value of the pitch of the unmanned aerial vehicle, represents the depth measurement value of the unmanned aerial vehicle, represents the vertical acceleration observer gain coefficient, represents the pitch angular acceleration observer gain coefficient, represents the gain coefficient of the pitch angular velocity observer, represents the depth rate observer gain coefficient, The vertical velocity coefficient represents the vertical acceleration, The pitch angular velocity coefficient representing the vertical acceleration, The pitch coefficient representing the vertical acceleration, The cumulative water discharge coefficient representing the vertical acceleration, The vertical velocity coefficient representing the pitch angular acceleration, The pitch velocity coefficient represents the pitch acceleration, The pitch coefficient represents the pitch acceleration, The cumulative water discharge coefficient representing the pitch angular acceleration, The vertical velocity coefficient representing the depth rate, The trim coefficient representing the depth rate; The expanded form of the vertical motion state observer is used to perform integral iteration calculation using the Euler method according to equation (4) to solve the vertical acceleration of the unmanned vehicle: （4）； in: Unmanned aerial vehicle The estimated vertical velocity at time, Unmanned aerial vehicle The estimated vertical velocity at time, Unmanned aerial vehicle The estimated value of the pitch angular velocity at the moment, Unmanned aerial vehicle The estimated value of the pitch angular velocity at the moment, Unmanned aerial vehicle Tilt at all times, Unmanned aerial vehicle Tilt at all times, Unmanned aerial vehicle Depth estimate at the moment, Unmanned aerial vehicle Depth estimate at the moment; S4: The vertical speed controller calculates the command water discharge and injection rate of the buoyancy water tank flow control system according to the target vertical speed, the calculated vertical acceleration of the unmanned vehicle at the current moment, and the vertical speed of the unmanned vehicle after low-pass filtering at the current moment; The vertical speed controller is formula (5): （5）； in: is the command drain water injection rate, is the proportional coefficient of the vertical speed controller, is the differential coefficient of the vertical speed controller, is the target vertical speed; S5: After discretizing the command water discharge and injection rate of the buoyancy water tank flow regulation system according to the discretized output function, it is converted into a switch valve control signal of the buoyancy water tank flow regulation system to control the vertical speed of the unmanned vehicle.

2. The method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system according to claim 1, characterized in that: The discretization output function formula in step S5 is formula (6): （6）； in: is the flow control valve control signal output after discretization, is the discretization threshold of the control command, 1 represents water injection and -1 represents drainage.

3. The method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system according to claim 1, characterized in that: In step S1, the measurement frequency of the depth of the unmanned aerial vehicle, the trim of the unmanned aerial vehicle and the cumulative water discharge and injection volume of the buoyancy water tank flow control system is 10 Hz.

4. The method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system according to claim 1, characterized in that: Calculation step length The value is 0.1 seconds.

5. The method for controlling the vertical speed of an unmanned aerial vehicle in a hovering state based on a water discharge and injection system according to claim 1, characterized in that: Low pass filter time constant The value is 0.5 seconds.

Images