A method and apparatus for depth control of a roll-decoupled underwater vehicle

Abstract

A kind of depth control method and device of underwater vehicle decoupled by roll angle, the method generates longitudinal attitude control quantity of underwater vehicle by calculating the difference between depth instruction and current depth of underwater vehicle;According to the longitudinal attitude control quantity, and in combination with the error of heading instruction and heading angle output, calculate rudder angle control vector;The rudder angle control vector is twice distributed, to generate elevator instruction, vertical rudder instruction and differential rudder instruction.The method can effectively solve the problem of depth control performance decline caused by roll angle coupling during the navigation of underwater vehicle, improve depth control precision.

Description

Technical Field

[0001] This invention relates to the field of underwater vehicle navigation control technology, and in particular to a method and device for controlling the navigation depth of an underwater vehicle. Background Technology

[0002] During navigation, underwater vehicles require real-time depth control through pitch angle adjustment. Due to various disturbances and the vehicle's inherent imbalances, changes in depth can lead to coupled changes in roll angle. Traditional underwater vehicle depth control uses depth error as the input to the depth controller to construct the outer depth control loop, and pitch angle as the input to the inner attitude control loop. When the roll angle is not zero, the gain from the pitch angle to the depth channel changes, resulting in decreased depth control performance and coupled deflection of the heading angle. Summary of the Invention

[0003] To address the issue of reduced depth control performance caused by roll angle coupling during underwater vehicle navigation, this disclosure provides a roll angle decoupled depth control scheme for underwater vehicles.

[0004] The underwater vehicle depth control method disclosed herein mainly includes the following steps:

[0005] 1. Calculate the longitudinal attitude control parameters of the underwater vehicle based on the difference between the depth command and the current depth;

[0006] 2. Calculate the current axial unit vector of the aircraft based on the attitude angle;

[0007] 3. Calculate the heading angle error based on the heading instructions and the current heading angle;

[0008] 4. Calculate the desired axial unit vector based on the longitudinal attitude control variable and the heading angle error;

[0009] 5. Calculate the rudder angle control vector based on the desired axial unit vector and the current axial unit vector;

[0010] 6. Calculate the horizontal rudder command, vertical rudder command, and differential rudder command based on the attitude direction cosine matrix and rudder angle control vector.

[0011] This completes the calculation of the roll decoupling depth control rudder command within one control cycle. When entering the next control cycle, the calculation in step 1 is repeated.

[0012] The roll-decoupled depth control device using the above method includes: a depth controller, a rotation vector attitude controller, and a secondary rudder angle distributor, wherein:

[0013] The depth controller generates longitudinal attitude control parameters for the underwater vehicle based on the difference between the depth command and the current depth.

[0014] The rotation vector attitude controller receives the longitudinal attitude control quantity output by the depth controller, and combines it with the error of the heading command and heading angle output to calculate the rudder angle control vector through the rotation vector attitude control algorithm;

[0015] The secondary rudder angle distributor performs secondary distribution of the rudder angle control vector based on the roll angle of the underwater vehicle, generating horizontal rudder commands, vertical rudder commands, and differential rudder commands.

[0016] The commands output by the secondary rudder angle distributor are sequentially processed through the servo motor model and the six-degree-of-freedom motion model of the aircraft to construct a closed loop control system. The system block diagram is as follows: Figure 2 As shown, where:

[0017] (4) The servo model is used to simulate the working characteristics of the physical servo and to express the servo bandwidth, maximum angle, and maximum angular rate characteristics.

[0018] (5) The six-degree-of-freedom model of the vehicle is used to describe the attitude, speed and position motion characteristics of the vehicle under the control of the servo motor. It is constructed from the six-degree-of-freedom motion equations of the underwater vehicle.

[0019] The servo model and the six-degree-of-freedom model of the aircraft are general mathematical models, and the specific formulas are not elaborated in this publication.

[0020] Compared with the prior art, the beneficial effects of this disclosure are: (1) Compared with the existing depth control technology based only on pitch angle, it can effectively decouple the depth control process from the roll angle, solve the problem of the decline in depth control performance caused by the roll angle, and avoid navigation coupling error caused by depth control; (2) Improve the depth control accuracy of underwater vehicles; (3) The system calculation process is simple; (4) It has good versatility. Attached Figure Description

[0021] The above and other objects, features and advantages of this disclosure will become more apparent from the more detailed description of exemplary embodiments of this disclosure taken in conjunction with the accompanying drawings, in which the same reference numerals generally represent the same components.

[0022] Figure 1 Define a graph for the coordinate system;

[0023] Figure 2 Block diagram of the depth-heading joint control system;

[0024] Figure 3 Here is an example depth controller block diagram;

[0025] Figure 4A diagram for rudder angle control vector calculation is shown. DETAILED DESCRIPTION

[0026] Preferred embodiments of the present disclosure will be described in greater detail below with reference to the accompanying drawings. Although preferred embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure is more thoroughly and completely conveyed to those skilled in the art, and the scope of the present disclosure is fully conveyed to those skilled in the art.

[0027] The present disclosure provides a roll angle decoupled underwater vehicle depth control scheme, under an exemplary embodiment:

[0028] The coordinate systems and names involved are described as follows:

[0029] Translation coordinate system (North-Sky-East) o m -x m y m z m : origin o m located at the center of buoyancy of the underwater vehicle, x m axis pointing to the North Pole, y m axis pointing to the sky, z m axis pointing east, moving with the underwater vehicle.

[0030] Carrier coordinate system o b -x b y b z b : fixed to the body, origin o b located at the center of buoyancy of the underwater vehicle, x b axis pointing forward of the underwater vehicle, y b axis located in the longitudinal symmetry plane of the underwater vehicle and x b axis pointing vertically upward, z b axis to the right, x b axis y b axis forming a right-handed coordinate system.

[0031] Underwater vehicle attitude angle In three angles in the order of Y-Z-X, the translation coordinate system can be rotated to the carrier coordinate system.

[0032] The roll decoupled depth control device provided by the embodiment includes three functional modules: a depth controller, a rotation vector attitude controller, and a rudder angle secondary distributor. By establishing a rudder model and a six-degree-of-freedom motion model of the vehicle, a closed loop of the control system can be constructed, and a system block diagram is shown as Figure 2 .

[0033] The depth controller generates a longitudinal attitude control quantity of the underwater vehicle by calculating a difference between the depth command and a current depth of the underwater vehicle;

[0034] The rotational vector attitude controller receives the longitudinal attitude control quantity output by the depth controller, and combines an error between the heading command and a heading angle output to calculate a rudder angle control vector through a rotational vector attitude control algorithm.

[0035] The rudder angle secondary distributor generates a horizontal rudder command, a vertical rudder command and a differential rudder command according to the underwater vehicle roll angle, for secondary distribution of the rudder angle control vector.

[0036] The specific scheme is as follows:

[0037] 1) Depth controller

[0038] The depth controller adopts a feedforward PI regulator to improve the response speed of depth control, and adapt to the large inertia characteristics of underwater vehicle depth control. The depth controller block diagram is shown in Figure 3 , in which, is a differential operator, and K dH is a differential gain. K PH is a depth error gain parameter, and K IH is a depth error integral gain parameter.

[0039] The calculation formula is as follows:

[0040]

[0041] In formula (1), θ y is a longitudinal attitude control quantity, K dH is a differential gain, K PH is a depth error gain parameter, and K IH is a depth error integral gain parameter. H out is a vehicle depth output, and H cmd is a vehicle depth control command.

[0042] 2) Rotational vector attitude controller

[0043] The rotational vector attitude controller controls the rotation of the current axial unit vector of the vehicle to the desired axial unit vector to achieve vehicle attitude control, and outputs a rudder angle control vector.

[0044] First, the current axial unit vector u bc of the vehicle is calculated. The unit vector along the x b axis of the vehicle is taken as u b = [1, 0, 0] T , and the expression of the current axial unit vector of the vehicle in the translation coordinate system is ubc is:

[0045]

[0046] in equation (2), is the attitude direction cosine matrix, ψ is the heading angle, θ is the pitch angle, is the roll angle.

[0047] Secondly, the heading angle error is calculated. The heading angle error is obtained by subtracting the current heading angle from the heading command, and the calculation formula is as follows:

[0048] ψ err = ψ cmd - ψ out (3)

[0049] In equation (3), ψ err is the heading angle error, ψ cmd is the heading command, and ψ out is the current heading angle of the vehicle.

[0050] Thirdly, the rudder angle control vector is calculated. By the longitudinal attitude control quantity θ y and the heading angle error ψ err , the expected axial unit vector u br of the vehicle is calculated according to the following formula.

[0051]

[0052] The rudder angle control vector δ cmd can be calculated by the mathematical cross product of the expected axial unit vector u br and the current axial unit vector u bc , and the calculation process is shown in Figure 4 , where u bc is the current x b axis unit direction vector of the vehicle, and u br is the expected x b axis unit direction vector of the vehicle.

[0053] The calculation formula is as follows:

[0054]

[0055] 3) Rudder angle secondary distributor

[0056] The rudder angle secondary distributor is calculated by the attitude direction cosine matrix and the rudder angle control vector, and the formula is as follows.

[0057]

[0058] In equation (6), δ e is the horizontal rudder command, and δr is the rudder command for the vertical plane. d is the rudder command for the differential plane.

[0059] The depth control method of the underwater vehicle based on the roll decoupling of the above system mainly includes the following steps:

[0060] 1. Calculate the longitudinal attitude control amount of the underwater vehicle according to the difference between the depth command and the current depth, and the calculation formula is shown in formula (1);

[0061] 2. Calculate the current axial unit vector of the vehicle according to the attitude angle, and the calculation formula is shown in formula (2);

[0062] 3. Calculate the heading angle error according to the heading command and the current heading angle, and the calculation formula is shown in formula (3);

[0063] 4. Calculate the expected axial unit vector according to the longitudinal attitude control amount and the heading angle error, and the calculation formula is shown in formula (4);

[0064] 5. Calculate the rudder angle control vector according to the expected axial unit vector and the current axial unit vector, and the calculation formula is shown in formula (5);

[0065] 6. Calculate the horizontal rudder command, the vertical rudder command and the differential rudder command according to the attitude direction cosine matrix and the rudder angle control vector, and the calculation formula is shown in formula (6).

[0066] Thus, the roll decoupling depth control rudder command calculation in a control cycle is completed, and when entering the next control cycle, step 1 is re-entered for calculation.

[0067] The above technical solution is only an exemplary embodiment of the present application, and for those skilled in the art, on the basis of the application of the method and principle disclosed in the present application, various types of improvements or modifications can be easily made, and are not limited to the method described in the above embodiment of the present application, therefore, the above described method is only preferred, and does not have the meaning of limitation.

Claims

1. A roll-decoupled depth control method for underwater vehicle, comprising the following steps: S1, calculating a longitudinal attitude control quantity of the underwater vehicle according to the difference between a depth command and a current depth; S2, calculating a current axial unit vector of the vehicle according to an attitude angle; S3, calculating a heading angle error according to a heading command and a current heading angle; S4, calculating a desired axial unit vector according to the longitudinal attitude control quantity and the heading angle error; S5, calculating a rudder angle control vector according to the desired axial unit vector and the current axial unit vector; S6, re-distributing the rudder angle control vector in a vehicle carrier coordinate system, and calculating a horizontal rudder command, a vertical rudder command and a differential rudder command according to an attitude direction cosine matrix and the rudder angle control vector; Thus, the roll-decoupled depth control rudder command calculation in a control cycle is completed, and when entering the next control cycle, the step S1 is re-entered for calculation; The calculation method in the step S1 comprises: where θ y is the longitudinal attitude control quantity, K dH is the derivative gain, K PH is the depth error gain parameter, K IH is the depth error integral gain parameter, H out is the vehicle depth output, H cmd is the vehicle depth control command; The calculation method in the step S2 comprises: Let us denote the unit vector along the x-axis of the vehicle body coordinate system as u b The unit vector in the direction of the axis is u b = [1, 0, 0] T The current axial unit vector of the vehicle in the translation coordinate system is denoted as u bc : wherein the origin o of the body coordinate system b located at the center of buoyancy of the underwater vehicle, x b axis pointing forward of the underwater vehicle, y b axis located in the longitudinal plane of symmetry of the underwater vehicle and perpendicular to x b axis pointing vertically upward, z b axis pointing to the right, perpendicular to x b axis y b axis forming a right-handed coordinate system; The translation coordinate system, origin o m Located at the center of buoyancy of the underwater vehicle, x m The axis points to the North Pole, y m The axis points to the sky, z m The axis points to the east, with the underwater vehicle moving; In the above formulae, is the cosine matrix of the orientation, ψ is the heading angle, θ is the pitch angle, is the roll angle; The calculation method in the step S3 comprises: ψ err = ψ cmd - ψ out where ψ err is the heading angle error, ψ cmd is the heading command, ψ out is the current heading angle of the vehicle; The calculation method in the step S4 comprises: where u br is the desired axial unit vector for the vehicle.

2. The method of claim 1, wherein, The calculation method in the step S5 comprises: where δ cmd is the rudder control vector.

3. The method of claim 2, wherein, The calculation method in the step S6 comprises: where δ e is the rudder command, δ r is the elevator command, and δ d is the differential command.

4. A roll-decoupled depth control device applying the method of any of claims 1 to 3, characterized in that comprising: a depth control module, a rotating vector attitude control module and a rudder angle secondary distribution module; wherein: the depth control module is configured to generate a longitudinal attitude control quantity of the underwater vehicle by calculating the difference between a depth command and a current depth of the underwater vehicle; the rotating vector attitude control module is configured to receive the longitudinal attitude control quantity output by the depth control module, and calculate a rudder angle control vector in combination with the error output by the heading command and the heading angle; the rudder angle secondary distribution module is configured to perform secondary distribution of the rudder angle control vector according to the roll angle of the underwater vehicle, and generate a horizontal rudder command, a vertical rudder command and a differential rudder command.

Images