Underwater vehicle depth control method and device for rolling decoupling

Abstract

The invention discloses an underwater vehicle depth control method and device for roll angle decoupling, and the method comprises the steps: generating a longitudinal attitude control quantity of an underwater vehicle through calculating a difference value between a depth instruction and the current depth of the underwater vehicle; calculating a rudder angle control vector according to the longitudinal attitude control quantity in combination with an error output by a heading instruction and a heading angle; and performing secondary distribution on the rudder angle control vector to generate a horizontal rudder instruction, a vertical rudder instruction and a differential rudder instruction. According to the method, the problem that the depth control performance is reduced due to roll angle coupling in the navigation process of the underwater vehicle can be effectively solved, and the depth control precision is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater vehicle navigation control, and in particular to a method and device for controlling the navigation depth of an underwater vehicle. Background Art

[0002] During navigation, underwater vehicles need to adjust the pitch angle to control the depth in real time. Due to various disturbance factors during navigation and the imbalance of the vehicle itself, the roll angle may change when the depth of the vehicle changes. In the traditional depth control process of underwater vehicles, the depth error is used as the input of the depth controller to build the outer loop of depth control, and the pitch angle is used as the input of the inner loop controller to build the inner loop of attitude control. When the roll angle is not zero, the gain of the pitch angle to the depth channel will change to a certain extent, resulting in a decrease in depth control performance and causing coupled deflection of the heading angle. Summary of the invention

[0003] In order to solve the problem of reduced depth control performance due to roll angle coupling during the navigation of an underwater vehicle, the present disclosure provides an underwater vehicle depth control solution with roll angle decoupling.

[0004] The underwater vehicle depth control method provided by the present disclosure mainly includes the following steps:

[0005] 1. Calculate the longitudinal attitude control value of the underwater vehicle according to the difference between the depth command and the current depth;

[0006] 2. Calculate the current axial unit vector of the spacecraft according to the attitude angle;

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

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

[0009] 5. Calculate the rudder angle control vector according to 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 according to the attitude direction cosine matrix and the rudder angle control vector.

[0011] At this point, the calculation of the depth control rudder command for roll decoupling within one control cycle is completed. When entering the next control cycle, the calculation is re-entered into step 1.

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

[0013] The depth controller generates a longitudinal attitude control value of the underwater vehicle according to the difference between the depth instruction of the underwater vehicle and the current depth;

[0014] The rotating vector attitude controller receives the longitudinal attitude control quantity output by the depth controller, and calculates the rudder angle control vector by combining the heading instruction and the error of the heading angle output through the rotating vector attitude control algorithm;

[0015] The rudder angle secondary distributor performs secondary distribution of the rudder angle control vector according to the roll angle of the underwater vehicle to generate horizontal rudder instructions, vertical rudder instructions and differential rudder instructions.

[0016] The command output by the rudder angle secondary distributor can build a closed loop of the control system by establishing the rudder model and the six-degree-of-freedom motion model of the aircraft in turn. 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 actual servo and 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, velocity and position motion characteristics of the vehicle under the control of the servo, and is constructed based on 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 formula expressions are not repeated in this disclosure.

[0020] Compared with the prior art, the beneficial effects of the present invention are: (1) compared with the existing depth control technology based only on the pitch angle, it can effectively realize the decoupling of the depth control process and the roll angle, solve the problem of the depth control performance degradation caused by the roll angle, and avoid the navigation coupling error caused by the depth control; (2) the depth control accuracy of the underwater vehicle is improved; (3) the system calculation process is simple; (4) it has good versatility. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] The above and other objects, features and advantages of the present disclosure will become more apparent through a more detailed description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present disclosure.

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

[0023] Figure 2 This is the block diagram of the depth heading joint control system;

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

[0025] Figure 4This is a diagram of the rudder angle control vector calculation. DETAILED DESCRIPTION

[0026] The preferred embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present disclosure are shown in the accompanying 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. On the contrary, these embodiments are provided to make the present disclosure more thorough and complete, and to fully convey the scope of the present disclosure to those skilled in the art.

[0027] The present disclosure provides a roll angle decoupled underwater vehicle depth control solution. In an exemplary embodiment:

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

[0029] Translation coordinate system (North-Sky-East) m -x m y m z m :Origin o m Located at the buoyancy center of the underwater vehicle, x m The y axis points to the North Pole. m The axis points to the sky, z m The axis points eastward and moves with the underwater vehicle.

[0030] Carrier coordinate system o b -x b y b z b : Fixedly connected to the projectile, origin o b Located at the buoyancy center of the underwater vehicle, x b The y axis points forward of the underwater vehicle. b The axis is located in the longitudinal symmetry plane of the underwater vehicle and is b The axis is vertically upward, z b Axis to the right, with x b Axis y b The axes form a right-handed coordinate system.

[0031] Underwater vehicle attitude angle by The three angles can be rotated from the translation coordinate system to the carrier coordinate system in the order of YZX.

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

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

[0034] The rotating vector attitude controller receives the longitudinal attitude control quantity output by the depth controller, and calculates the rudder angle control vector by combining the heading instruction and the error of the heading angle output through the rotating vector attitude control algorithm;

[0035] The rudder angle secondary distributor performs secondary distribution of the rudder angle control vector according to the roll angle of the underwater vehicle to generate horizontal rudder instructions, vertical rudder instructions and differential rudder instructions.

[0036] The specific plan is as follows:

[0037] 1) Depth controller

[0038] The depth controller uses 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 block diagram of the depth controller is as follows: Figure 3 As shown in the figure, is a differential operator that differentiates the depth instruction, K dH is the differential gain. K PH is the depth error gain parameter, K IH Depth error integral gain parameter.

[0039] The calculation formula is as follows:

[0040]

[0041] In formula (1), θ y is the longitudinal attitude control variable, K dH is the differential gain, K PH is the depth error gain parameter, K IH Depth error integral gain parameter, H out is the vehicle depth output, H cmd It is the depth control command of the spacecraft.

[0042] 2) Rotational vector attitude controller

[0043] The rotating vector attitude controller realizes the attitude control of the aircraft by controlling the current axial unit vector of the aircraft to rotate toward the desired axial unit vector. The rotating vector attitude controller gives the rudder angle control vector.

[0044] First, calculate the current axial unit vector u of the spacecraft bc Calculation of . Take the x b The unit vector in the axis direction is u b =[1,0,0] T , the current axial unit vector of the spacecraft is expressed in the translation coordinate system ubc for:

[0045]

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

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

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

[0049] In formula (3), ψ err is the heading angle error, ψ cmd is the heading instruction, ψ out is the current heading angle of the aircraft.

[0050] Next, calculate the rudder angle control vector. y and heading angle error ψ err , calculate the desired axial unit vector u of the spacecraft according to the following formula: br .

[0051]

[0052] The desired axial unit vector u br and the current axial unit vector u bc The mathematical cross product of can be calculated to get the rudder angle control vector δ cmd , the calculation process diagram is shown in Figure 4 , in the figure u bc is the current x of the aircraft b Axis unit direction vector, u br x is the vehicle's expected b Axis unit direction vector.

[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 formula (6), δ e is the horizontal rudder command, δr is the vertical rudder command, δ d It is the differential rudder command.

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

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

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

[0062] 3. Calculate the heading angle error based on the heading instruction and the current heading angle. The calculation formula is shown in formula (3);

[0063] 4. Calculate the expected axial unit vector based on the longitudinal attitude control value and the heading angle error. 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. The calculation formula is shown in formula (5);

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

[0066] At this point, the calculation of the depth control rudder command for roll decoupling within one control cycle is completed. When entering the next control cycle, the calculation is re-entered into step 1.

[0067] The above technical scheme is only an exemplary embodiment of the present invention. For those skilled in the art, it is easy to make various types of improvements or modifications based on the application methods and principles disclosed in the present invention, and it is not limited to the method described in the above specific embodiment of the present invention. Therefore, the method described above is only preferred and does not have a restrictive meaning.

Claims

1. A roll decoupled underwater vehicle depth control method, comprising the following steps: S1, calculating the longitudinal attitude control value of the underwater vehicle according to the difference between the depth command and the current depth; S2, calculate the current axial unit vector of the spacecraft according to the attitude angle; S3, calculating the heading angle error according to the heading instruction and the current heading angle; S4, calculating the desired axial unit vector according to the longitudinal attitude control amount and the heading angle error; S5, calculating a rudder angle control vector according to a desired axial unit vector and a current axial unit vector; S6, redistributing the rudder angle control vector in the aircraft carrier coordinate system, and calculating 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; At this point, the calculation of the depth control rudder command for roll decoupling within one control cycle is completed. When entering the next control cycle, the calculation is re-entered into step S1.

2. The method according to claim 1, characterized in that The calculation method in step S1 includes: Among them, θ y is the longitudinal attitude control variable, K dH is the differential gain, K PH is the depth error gain parameter, K IH Depth error integral gain parameter, H out is the vehicle depth output, H cmd It is the depth control instruction of the spacecraft.

3. The method according to claim 1 or 2, characterized in that: The calculation method in step S2 includes: Take the coordinate system x along the vehicle carrier b The unit vector in the axis direction is u b =[1,0,0] T , the current axial unit vector of the spacecraft is expressed in the translation coordinate system u bc for: Among them, the origin of the carrier coordinate system is o b Located at the buoyancy center of the underwater vehicle, x b The y axis points forward of the underwater vehicle. b The axis is located in the longitudinal symmetry plane of the underwater vehicle and is b The axis is vertically upward, z b Axis to the right, with x b Axis y b The axes form a right-handed coordinate system; The translation coordinate system, the origin o m Located at the buoyancy center of the underwater vehicle, x m The y axis points to the North Pole. m The axis points to the sky, z m The axis points eastward and moves with the underwater vehicle; In the above formula, is the attitude direction cosine matrix, ψ is the heading angle, θ is the pitch angle, is the roll angle.

4. The method according to claim 3, characterized in that The calculation method in step S3 includes: ψ err =ψ cmd -ψ out Among them, ψ err is the heading angle error, ψ cmd is the heading instruction, ψ out is the current heading angle of the aircraft.

5. The method according to claim 4, characterized in that The calculation method in step S4 includes: Among them, u br is the desired axial unit vector of the vehicle.

6. The method according to claim 5, characterized in that The calculation method in step S5 includes: Among them, δ cmd is the rudder angle control vector.

7. The method according to claim 6, characterized in that The calculation method in step S6 includes: Among them, δ e is the horizontal rudder command, δ r is the vertical rudder command, δ d It is the differential rudder command.

8. A roll decoupled depth control device using the method described in any one of claims 1 to 7, characterized in that: include: Depth control module, rotation vector attitude control module, rudder angle secondary distribution module; in: A depth control module, used to generate a longitudinal attitude control value of the underwater vehicle by calculating the difference between the depth instruction and the current depth of the underwater vehicle; The rotation vector attitude control module is used to receive the longitudinal attitude control quantity output by the depth control module, and calculate the rudder angle control vector by combining the heading instruction and the error of the heading angle output; The rudder angle secondary distribution module is used to perform secondary distribution of the rudder angle control vector according to the roll angle of the underwater vehicle, and calculate and generate horizontal rudder instructions, vertical rudder instructions and differential rudder instructions.

Images