JP2015033997A - Underwater navigation body, and control device and control method thereof - Google Patents

Abstract

PROBLEM TO BE SOLVED: To reduce a power consumption of a rudder driving system by improving a control precision of a feed forward system.SOLUTION: A control device 10a of an underwater navigation body comprises: a depth control part 11 that carries out feedback control of a depth; a pitch angle control part 12 that carries out feedback control of a pitch angle; and a non-interferential control part 13a. The non-interferential control part 13a uses an interference compensation operational expression to calculate an interference compensation rudder angle to cancel an interference with the pitch angle caused by a depth change. The interference compensation operational expression is an operational expression derived by solving a linearized state equation made adaptable over a whole operation range of a hull with respect to a rudder control amount by getting a coefficient matrix represented as a function of an equilibrium point in the linearized state equation in a vicinity of an arbitrary equilibrium point of the hull. The non-interferential control part 13a acquires the interference compensation rudder angle by substituting the interference compensation operational expression with a value of an equilibrium point with time and a value of a state variable.

Description

  The present invention relates to an underwater vehicle, its control device, and control method.

  Conventionally, in the attitude control of an underwater vehicle, so-called feedback control is mainly performed in which a steering angle command is set by performing PID control on a difference between a target attitude and a current attitude.

JP-A-8-282589

In each axis control such as depth, roll angle, pitch angle, and yaw angle in the hull, mutual interference occurs between specific axes. For example, when only the depth is changed while maintaining the pitch angle at 0 degrees, the pitch angle varies due to the change in depth. In the feedback control, the rudder is operated so as to suppress the fluctuation of the pitch angle, so that there is a problem that the power consumption of the rudder drive system increases.
In order to suppress such interference, conventionally, feedforward control is used in addition to feedback control. However, in the conventional feedforward control, interference cannot be effectively eliminated, and a considerable amount of power is consumed by the rudder drive system.

  FIG. 20 shows an example of a simulation result when changing the depth by conventional hull control including a feedforward system. In FIG. 20, (a) shows the speed of the hull, (b) shows the depth, (c) shows the pitch angle, (d) shows the steering angle related to the depth, and (e) shows the temporal transition of the steering angle related to the pitch angle. Yes. In FIGS. 20D and 20E, the steering angle command is indicated by a broken line, and the actual steering angle is indicated by a solid line. As shown in this figure, the pitch angle is affected by the change in depth, and the steering angle component related to the pitch angle is swung back and forth through zero, and it can be seen that useless power consumption is being performed.

  The present invention has been made in view of such circumstances, and an underwater vehicle and its control device capable of improving the accuracy of control of the feedforward system and reducing the power consumption of the rudder drive system. An object of the present invention is to provide a control method.

  In the first aspect of the present invention, control is performed on at least two axes among six axes including three linear axes including the x-axis, y-axis, and z-axis of the hull and three rotation axes for each of the linear axes. This is a possible underwater vehicle control device that uses a plurality of feedback control units provided for each axis to be controlled and an interference compensation calculation formula to reduce mutual interference between axes. A feed-forward non-interference control unit that calculates an interference compensation rudder angle for cancellation and outputs to the control unit, and the interference compensation calculation formula is a linearized equation of state in the vicinity of an arbitrary equilibrium point of the hull In Fig. 4, the coefficient matrix is expressed as a function of the equilibrium point, and the linearized equation of state changed so as to be adaptive in the entire operating range of the hull is inverted with respect to the parameter relating to the rudder operation amount necessary for performing the desired motion. The non-interacting control unit derives the interference compensation steering by substituting the value of the equilibrium point and the value of the state variable according to time into the interference compensation computation expression. It is an underwater vehicle control device that obtains corners.

According to this aspect, in the linearized equation of state in the vicinity of an arbitrary equilibrium point of the hull, the coefficient matrix is expressed as a function of the equilibrium point, and is changed so as to adapt to the state at each time, so that the entire operating range of the hull The time-varying linearized state equation that can be adapted in the above is obtained, and the time-varying linearized state equation is solved in terms of the parameters related to the rudder operation amount necessary for performing the desired motion. Is used to calculate the interference compensation steering angle for suppressing the mutual interference of the motion axes.
In this way, the interference compensation calculation formula derived using the time-varying linearized equation of state that can be applied over the entire operating range of the hull is used, so that the interference compensation steering angle at the current equilibrium point can be easily obtained in real time. In addition, it is possible to improve the accuracy of control of the feedforward system. Then, by reflecting this interference compensation steering angle in the feedback control of the control unit, it becomes possible to effectively reduce the mutual interference between the axes.
The linearized state equation can be generally obtained by performing a first-order approximation by performing a Taylor expansion of a state equation expressed in a non-linear manner around a certain equilibrium point.
Further, the interference compensation calculation formula is characterized by including, as variables, the speed and acceleration of an element related to a target axis for suppressing mutual interference. This is because an interference compensation calculation formula is created based on the linearized state equation. For example, when the mutual interference between the pitch angle and the depth is suppressed, at least one of the speed and acceleration related to the depth and the speed and acceleration related to the pitch angle are included in the interference compensation calculation formula as variables.

  In the control device for an underwater vehicle, a target value related to the motion of the hull may be used as the equilibrium point and the state variable.

  Thus, since the target value is adopted as the equilibrium point, the mutual interference between the axes when the target value is changed can be effectively reduced. In addition, since the target value itself is used as the state variable, it is possible to reduce the calculation processing load as compared with a case where a minute change amount of the target value described later is used.

  In the underwater vehicle control apparatus, the plurality of control units include a depth control unit corresponding to the z-axis and a pitch angle control unit corresponding to a rotation axis with respect to the y-axis, and the interference compensation The calculation formula includes the target speed in the x-axis direction and the target speed in the z-axis direction as the equilibrium point, and the target speed and target acceleration in the z-axis direction, the target angular speed of the rotation axis with respect to the y-axis, and the state variables. The target angular acceleration and the target pitch angle may be included.

  According to such a configuration, it is possible to reduce mutual interference between the depth and the pitch angle.

  In the control device for an underwater vehicle, the plurality of control units include a depth control unit corresponding to the z axis, a pitch angle control unit corresponding to a rotation axis with respect to the y axis, and a rotation axis with respect to the x axis. And an azimuth angle control unit corresponding to the rotation axis with respect to the z-axis, and the interference compensation calculation formula includes a target speed in the x-axis direction and a target in the z-axis direction as an equilibrium point. Speed, target speed in the y-axis direction, target angular speed and target angle about the x-axis, target angular speed and target angle about the y-axis, target angular speed about the z-axis, and the target speed in the z-axis direction as the state variables And target acceleration, target angular velocity and target angular acceleration of the rotation axis with respect to the y axis, target pitch angle, target angular acceleration around the x axis, target angular velocity and target angle, target angular acceleration around the z axis, and target angular velocity are included. Even There. Note that the observation amount may be used for the target speed in the x-axis direction and the target speed in the y-axis direction.

  According to such a configuration, it is possible to reduce mutual interference among depth, pitch angle, roll angle, and azimuth.

  In the underwater vehicle control apparatus, a target value related to the motion of the hull is used as the equilibrium point, and a minute change amount of the target value related to the motion of the hull is used as the state variable. Good.

  In the underwater vehicle control apparatus, the plurality of control units include a depth control unit corresponding to the z-axis and a pitch angle control unit corresponding to a rotation axis with respect to the y-axis, and the interference compensation The arithmetic expression includes the minute change amount of the target velocity in the x-axis direction and the minute change amount of the target velocity in the z-axis direction as the equilibrium point, and the minute change amount of the velocity in the z-axis direction and the acceleration change as the state variables. The minute change amount, the minute change amount of the angular velocity of the rotation axis with respect to the y axis, the minute change amount of the angular acceleration, and the minute change amount of the target pitch angle may be included.

  According to such a configuration, it is possible to reduce mutual interference between the depth and the pitch angle.

  In the control device for an underwater vehicle, the plurality of control units include a depth control unit corresponding to the z axis, a pitch angle control unit corresponding to a rotation axis with respect to the y axis, and a rotation axis with respect to the x axis. And an azimuth angle control unit corresponding to the rotation axis with respect to the z-axis, and the interference compensation calculation formula includes a target speed in the x-axis direction and a target in the z-axis direction as an equilibrium point. Velocity, target velocity in the y-axis direction, target angular velocity and target angle around the x-axis, target angular velocity and target angle around the y-axis, target angular velocity around the z-axis, and the state variable A minute change amount and a minute change amount of acceleration, a minute change amount of an angular velocity of a rotation axis with respect to the y axis and a minute change amount of an angular acceleration, a minute change amount of a target pitch angle, a minute change amount of a target angular acceleration around the x axis, Minute change in target angular velocity and eye Minute change of angles, small change amount of the target angular acceleration about the z-axis, may be included small amount of change in the target angular velocity. Note that the observation amount may be used for the target speed in the x-axis direction and the target speed in the y-axis direction.

  According to such a configuration, it is possible to reduce the mutual interference among the depth, pitch angle, roll angle, and azimuth angle.

  In the underwater vehicle control device, the non-interacting control unit has in advance an equilibrium point of state quantities with respect to a combination of operation amounts of the hull as an equilibrium point map, and The equilibrium point of the state quantity corresponding to the manipulated variable may be acquired from the equilibrium point map, and the interference compensation steering angle may be calculated using the acquired equilibrium point of the state quantity in the interference compensation calculation formula. .

In the control apparatus for an underwater vehicle, it is preferable that only the state variables related to the axis at which mutual interference occurs are used as the state variables included in the interference compensation calculation formula.
As described above, by limiting the state variables included in the interference compensation calculation formula, calculations not related to interference can be eliminated, and calculation processing can be reduced.

  A second aspect of the present invention is an underwater vehicle equipped with the above-described underwater vehicle control device.

  According to a third aspect of the present invention, at least two of the six linear axes including the x-axis, y-axis, and z-axis of the hull and the three rotational axes for each of the linear axes are provided. A control method for an underwater vehicle having a feedback control system and a feedforward system for calculating an interference compensation rudder angle for canceling mutual interference of axis control, wherein in the feedforward system, any of the hulls In the linearized state equation in the vicinity of the equilibrium point, a coefficient matrix is expressed as a function of the equilibrium point, and a time-varying linearized state equation that can be applied over the entire operating range of the hull is obtained as a rudder necessary for performing a desired motion. By substituting the value of the equilibrium point and the value of the state variable according to time into the interference compensation calculation formula derived by solving the parameter relating to the manipulated variable, the interference A method of controlling the underwater vehicle to obtain 償舵 angle.

  ADVANTAGE OF THE INVENTION According to this invention, there exists an effect that the precision of control of a feedforward system can be improved and the power consumption of a rudder drive system can be reduced.

It is the figure which showed schematic structure of the underwater vehicle which concerns on one Embodiment of this invention. It is the figure which showed typically about arrangement | positioning of the rudder when seeing the underwater vehicle shown in FIG. 1 from the stern. It is a figure for demonstrating the freedom degree of a motion of the underwater vehicle which concerns on one Embodiment of this invention. It is the figure shown about the functional block regarding depth control and pitch angle control in the control apparatus of the underwater vehicle which concerns on 1st Embodiment of this invention. It is the figure which showed an example of the simulation result at the time of the depth change by the control apparatus of the underwater vehicle which concerns on 1st Embodiment of this invention. It is the figure shown about the functional block regarding depth control and pitch angle control in the control apparatus of the underwater vehicle which concerns on 2nd Embodiment of this invention. It is the figure which showed an example of the simulation result at the time of the depth change by the control apparatus of the underwater vehicle which concerns on 2nd Embodiment of this invention. It is the figure shown about the functional block regarding depth control and pitch angle control in the control apparatus of the underwater vehicle which concerns on 3rd Embodiment of this invention. It is the figure which showed an example of the equilibrium point map. It is the figure which showed an example of the simulation result at the time of the depth change by the control apparatus of the underwater vehicle which concerns on 3rd Embodiment of this invention. It is the figure shown about the functional block regarding depth control, roll angle control, pitch angle control, and azimuth angle control in the control apparatus of the underwater vehicle which concerns on 4th Embodiment of this invention. It is the figure shown about the functional block regarding depth control, roll angle control, pitch angle control, and azimuth angle control in the control apparatus of the underwater vehicle which concerns on 4th Embodiment of this invention. It is the figure shown about the functional block regarding depth control, roll angle control, pitch angle control, and azimuth angle control in the control apparatus of the underwater vehicle which concerns on 5th Embodiment of this invention. It is the figure shown about the functional block regarding depth control, roll angle control, pitch angle control, and azimuth angle control in the control apparatus of the underwater vehicle which concerns on 5th Embodiment of this invention. It is the figure which showed one structural example of the target value setting part shown in FIG. It is the figure shown about the functional block regarding depth control and pitch angle control in the control apparatus of the underwater vehicle which concerns on 6th Embodiment of this invention. It is the figure which showed the modification of the control apparatus of the underwater vehicle shown in FIG. It is the figure shown about the functional block regarding depth control and pitch angle control in the control apparatus of the underwater vehicle which concerns on 7th Embodiment of this invention. It is the figure which showed an example of the ideal response model. It is the figure which showed an example of the simulation result at the time of the depth change by the control apparatus of the conventional underwater vehicle.

Embodiments of an underwater vehicle, an underwater vehicle, its control device, and a control method according to the present invention will be described below with reference to the drawings.
FIG. 1 is a diagram showing a schematic configuration of an underwater vehicle according to an embodiment of the present invention, and FIG. 2 is a schematic view of a rudder arrangement when the underwater vehicle shown in FIG. 1 is viewed from the stern. FIG. 3 and FIG. 3 are diagrams for explaining the degree of freedom of movement of the underwater vehicle.

  As shown in FIG. 3, the underwater vehicle hull 1 includes a stern axis (hereinafter referred to as “x-axis”), a left-right axis (hereinafter referred to as “y-axis”), and a vertical axis (hereinafter referred to as “z-axis”). It is configured to be able to control six axes (six degrees of freedom) composed of three linear axes orthogonal to each other and three rotational axes for each of these linear axes.

As shown in FIG. 3, in the present embodiment, the velocities in the x-axis direction, the y-axis direction, and the z-axis direction in the hull coordinate system are u, v, and w, respectively, and the angular velocities around the respective axes are p, q, Define r. Further, the rotation angle around the x axis in the absolute coordinate system (earth coordinate system) is defined as the roll angle φ, the rotation angle around the y axis as the pitch angle θ, and the rotation angle around the z axis as the direction ψ.
As shown in FIGS. 1 and 2, the hull 1 is provided with a plurality of rudders 3 a to 3 e. The rudders 3a to 3d are mainly used for control related to the roll angle φ, the pitch angle θ, and the azimuth angle ψ, and the rudder 3e is mainly used for control related to the depth.
And these rudder 3a-3e is operated based on each rudder angle command produced | generated in the control apparatus which concerns on each embodiment mentioned later, for example, a 6-axis state (for example, mainly in a z-axis direction). The control of making the position, such as depth, pitch angle, roll angle, and azimuth) follow each target value is realized.

[First Embodiment]
Hereinafter, an underwater vehicle control apparatus and control method according to a first embodiment of the present invention will be described with reference to the drawings. The underwater vehicle control apparatus according to the present embodiment includes a plurality of control units provided for the above-described six axes and a non-interacting control unit for suppressing interference between these axes. In the description, for convenience, control in the z-axis direction (depth control) and control of the rotation axis corresponding to the y-axis (pitch angle θ control) are taken as an example, and interference generated between the depth and the pitch angle is reduced. The case will be described.

FIG. 4 is a diagram illustrating functional blocks related to depth control and pitch angle control in the underwater vehicle control apparatus (hereinafter simply referred to as “control apparatus”) 10a according to the present embodiment.
The control device 10a includes a depth control unit 11 that controls the depth z, a pitch angle control unit 12 that controls the pitch angle θ, and a non-interacting control unit 13a.

Depth control unit 11, the discrete target value setting unit 21 from a value at which setting the depth z _{The set} to set a continuous target depth z ^*, the actual depth of the target depth z ^* and the hull 1 from the target value setting unit 21 a difference calculating unit 22 that calculates a difference from z, a feedback control unit 23 that sets a depth feedback steering angle δb _FB from the difference from the difference calculating unit 22, and a non-interacting control unit 13a that sets the depth feedback steering angle δb _FB to be described later. A depth steering angle setting unit 24 that generates a depth steering angle command δb ^* and corrects it using the interference compensation steering angle δb _FF from the main configuration is provided.

For example, the target value setting unit 21 obtains a continuous target depth z ^* by using a predetermined response model with respect to a set depth z _set that is a discrete value input from a host device (not shown). As an example of the response model, there is a second-order low-pass filter as shown in the following equation (1).

In the equation (1), ζ is an attenuation rate [−], and ω _n is a response frequency [rad / s], which is expressed by the following equation (2). For example, in the depth control system, ζ = 1 is set without overshoot.

  In the equation (2), Ts is a settling time [sec], and is arbitrarily set according to operating conditions.

The feedback control unit 23 sets the depth feedback steering angle δb _FB by performing PID control on the difference from the difference calculation unit 22, for example.
The depth steering angle setting unit 24 is realized, for example, as an adder that adds the depth feedback steering angle δb _FB and the interference compensation steering angle δb _FF .

Pitch angle control unit 12, the discrete target value setting unit 31 from a value at which setting the pitch angle theta _{The set} to set a continuous target pitch angle theta ^*, the target pitch angle theta ^* the hull from the target value setting unit 31 A difference calculation unit 32 that calculates a difference from one actual pitch angle θ, a feedback control unit 33 that sets a pitch angle feedback steering angle δr _FB from the difference from the difference calculation unit 32, and a pitch angle feedback steering angle δr _FB will be described later. A pitch angle rudder angle setting unit 34 that corrects using the interference compensation rudder angle δr _CFF from the non-interacting control unit 13a and generates a pitch angle rudder angle command δr _C ^* is mainly provided.
Since each configuration of the pitch angle control unit 12 is substantially the same as that of the depth control unit 11, detailed description thereof is omitted.

The non-interacting control unit 13a is a feedforward system, and uses the interference compensation arithmetic expression derived from the linearized state equation of the hull 1 by inverse problem analysis to perform mutual interference between the depth z and the pitch angle θ. Interference compensation steering angles δb _FF and δr _CFF for suppression are calculated. Here, before describing each component of the non-interacting control unit 13a, a process of deriving an interference compensation calculation formula will be described.

  In general, the motion of the hull 1 is nonlinear, but can be linearized in the vicinity of the equilibrium point (a minute section around the equilibrium point). For example, the linearized state equation in the vicinity of an arbitrary equilibrium point of the hull 1 is expressed by the following equation (3). Here, regarding the equilibrium point, it is assumed that the hull is in an equilibrium state at the target value, and the target value of the hull motion is set as the equilibrium point.

  In Expression (3), Δu is a minute change [m / s] in the velocity in the x-axis direction, Δv is a minute change [m / s] in the y-axis direction, and Δw is a minute change [m / s] in the z-axis direction. / S], Δp are minute changes in roll angular velocity [rad / s], Δq is minute changes in pitch angular velocity [rad / s], Δr is minute changes in yaw angular velocity [rad / s], and Δφ is minute changes in roll angle. [rad], Δθ is a minute change in pitch angle [rad], Δn is a minute change in rotation speed [rps], Δδb is a minute change in the rudder angle of the rudder 3e, in other words, minute change in a rudder angle command related to depth [rad]. , Δδr1, Δδr2, Δδr3, and Δδr4 are minute changes in the rudder angle of the rudder 3a, rudder 3b, rudder 3c, and rudder 3d, in other words, a minute change [rad] in the rudder angle command related to the pitch angle θ.

Subsequently, by expressing the coefficient matrices A and B of the linearized state equation in the vicinity of an arbitrary equilibrium point as a function of the equilibrium point, linearization that has been established locally is continuously performed in the entire operation region of the hull 1. Deform to hold.
The elements of the coefficient matrices A and B as described above are expressed as a function of the equilibrium point when linearized with respect to an arbitrary equilibrium point. Therefore, it is theoretically possible to establish the linearized equation of state that is established only locally by applying the coefficient matrices A and B to the change in the equilibrium point of the hull in the entire operation region.

  Subsequently, the state variable related to the axis causing mutual interference, that is, the state variable related to the depth z and the pitch angle θ is specified, and model reduction is performed to reduce the matrix size so that a linearized state equation consisting of the specified state variable is obtained. To do. In this way, by using a linearized state equation specialized for the depth / pitch angle control system, it is possible to eliminate state variables not related to mutual interference, and to reduce the processing load.

  (4) shows an example of a linearized state equation specialized for the depth / pitch angle control system. Equation (5) shows each element of the coefficient matrix included in Equation (4). As shown in equation (5), each element of the coefficient matrix is expressed as a function of the equilibrium point (note that some elements are expressed as constants).

  In each of the above formulas (5), the coefficient K is a constant determined by the motion characteristics unique to the hull. In the suffix of the coefficient K, the first character indicates the axis of the equation of motion, and the second and subsequent characters indicate state variables.

For example, K _qθ indicates that it is a coefficient K _qθ relating to the θ term in the equation of motion (equation (6)) regarding the q-axis of the hull.

Similarly, for example, K _wudr indicates that it is a coefficient K _wudr related to the u ² · δr term in the equation of motion (equation (7)) regarding the z-axis of the hull.

  Next, an inverse problem analysis is performed on the linearized state equation after model reduction, and the steering angle command for canceling the interference is solved.

  Specifically, when the expressions of the depth velocity w and the pitch angular velocity q are extracted from the above equation (4), the following equations (8) and (9) are obtained.

Here, for example, assuming that the pitch angle θ is always set to 0, the parameters related to the pitch angle are all 0, that is, dq / dt = 0, q = 0, and θ = 0. Also, assuming that the minute changes in the rudder angles of the rudder 3a to 3d with respect to the pitch angle θ are all equal, δr _c = δr1 = δr2 = δr3 = δr4, and the following equations (10) and (11) are obtained. .
The above assumption is merely an example. For example, if it is desired to maintain the pitch angle θ at a predetermined pitch angle, a desired set value may be input as the pitch angle θ.

Then, the equation (10), (11) the simultaneous equations of expression, small changes in the steering angle command regarding the depth z Derutaderutabi, and solving for small changes Derutaderutaaru _c of the steering angle command regarding pitch angle theta, the following equation (12) And (13) Formula is obtained.
The (12) and (13) below is an example of an interference compensation calculation expression, Derutaderutabi minute interference compensation steering angle Derutaderutabi _FF regarding depth required _{decoupling, Δδr c} relates pitch angle necessary decoupling A small interference compensation steering angle Δδr _cFF is obtained.

  Next, returning to FIG. 4, a specific configuration of the decoupling control unit 13 according to the present embodiment will be described. The non-interacting control unit 13a includes a coefficient matrix calculation unit 41, a delay unit 42, differentiators 43a to 43d, a minute change amount calculation unit 44, an interference compensation steering angle calculation unit 45, and an integration unit 46 as main components. Yes.

The coefficient matrix calculation unit 41 receives the set speed in the x-axis direction = target speed u ^* and target depth w ^* . For example, the target speed u ^* is a value input from a host device, and the target depth w ^* is a value obtained by differentiating the target depth z ^* set in the target value setting unit 21 by the differentiator 43a. Strictly speaking, since the conversion from the depth z to the depth velocity w is a conversion from the absolute coordinate system to the relative coordinate system, a correction expression other than the differentiation of z is necessary, but is omitted here.
The coefficient matrix calculation unit 41 holds mathematical formulas for each element of the coefficient matrix represented by the above formula (5), and by substituting the target speed u ^* and the target depth w ^* into these mathematical formulas, The coefficient matrices A (n) and B (n) at the equilibrium point are calculated.

  The calculated coefficient matrices A (n) and B (n) are input to the delay unit 42. The delay unit 42 stores the coefficient matrices A (n) and B (n) obtained in the current control cycle, and stores the coefficient matrix A (n−1) obtained in the previous control cycle. B (n−1) is output to the interference compensation steering angle calculation unit 45. As described above, the coefficient matrices A (n−1) and B (n−1) obtained in the control cycle one cycle before are outputted by each state variable output from the minute change amount calculation unit 44 described later. This is to ensure consistency with the above.

The minute change amount calculation unit 44 includes a target depth velocity w ^* and a target depth acceleration dw ^* / dt, a target angular velocity q ^* and a target angular acceleration dq ^* / dt of the rotation axis corresponding to the target pitch angle θ ^* , and a target pitch. The angle θ ^* is input. The target depth speed w ^* is a value obtained by differentiating the target depth z ^* set by the target value setting unit 21 once by the differentiator 43a. The target acceleration dw ^* / dt is obtained by changing the target depth z ^* by the differentiators 43a and 43b. This is a value obtained by differentiating twice. The target angular velocity q ^* is a value obtained by differentiating the target pitch angle θ ^* set by the target value setting unit 31 once by the differentiator 43c, and the target angular acceleration dq ^* / dt is obtained by differentiating the target pitch angle θ ^* by the differentiators 43c and 43d. Is a value obtained by differentiating twice.

  The differentiators 43a to 43d and the minute change amount calculation unit 44 are configured to calculate a state variable to be substituted into the interference compensation calculation formula in the interference compensation steering angle calculation unit 45 described later. Therefore, for example, for the state variables that are not used in the interference compensation calculation formula among the above state variables, a part of the differentiator can be omitted or the calculation can be omitted.

Minute change amount calculation unit 44, a minute variation of the target value ^{Δw *, Δdw * / dt,} Δq *, Δdq * / dt, and calculates the [Delta] [theta] ^*, and outputs the interference compensation steering angle calculating unit 45.
Specifically, the minute change amount calculation unit 44 calculates the minute change amount by calculating the difference between the previous value and the current value of the control period for each input value.

The interference compensation rudder angle calculation unit 45 has interference compensation calculation formulas (for example, the above-described formulas (12) and (13)). From these formulas, the delay unit 42 and the minute change amount calculation unit 44 By substituting these input values, the fine interference compensation steering angles Δδb _FF and Δδr _cFF are obtained.
Integrating section 46, calculated by the interference compensation steering angle calculating section 45 microscale interference compensation steering angle Δδb _FF, Δδr interference compensation steering angle in the previous control cycle the _CFF [delta] b _FF, by adding the [delta] r _CFF, the current control The interference compensation steering angle δb _FF related to the depth in the cycle and the interference compensation steering angle δr _cFF related to the pitch angle are calculated. That is, an arithmetic expression is expressed as follows.

δb _FF (current value) = δb _FF (previous value) + Δδb _FF
δr _cFF (current value) = δr _cFF (previous value) + Δδr _cFF

The depth-related interference compensation steering angle δb _FF obtained in this way is added to the depth feedback steering angle δb _FB in the depth steering angle setting unit 24 of the depth control unit 11, thereby obtaining a depth steering angle command δb ^*. Is obtained.
Similarly, an interference compensation steering angle δr _cFF related to the pitch angle is added to the pitch angle feedback steering angle δr _cFB in the pitch angle steering angle setting unit 34 of the pitch angle control unit 12, so that the pitch steering angle command δr _c ^* Is calculated.

Next, the operation of the underwater vehicle control apparatus 10a according to the present embodiment will be described.
When the set depth z _set and the set pitch angle θ _set (= 0) _{set in the host} device are input, the set depth z _set is input to the depth control unit 11 and the pitch angle set value θ _set is input to the pitch angle control unit 12. Entered.
In the depth control unit 11, the set depth z _set is input to the target value setting unit 21 and converted from a discrete value to a target depth z ^* which is a continuous value. The target depth z ^* is output to the non-interacting control unit 13a and the difference calculation unit 22. The difference calculation unit 22 calculates the difference between the target depth z ^* and the actual depth z of the hull 1, and the feedback control unit 23 sets the depth feedback steering angle δb _FB based on this difference.

Similarly, in the pitch angle control unit 12, the set pitch angle θ _set is input to the target value setting unit 31, and is converted from a discrete value to a target pitch angle θ ^* that is a continuous value. The target pitch angle θ ^* is output to the non-interacting control unit 13a and the difference calculation unit 32. The difference calculation unit 32 calculates the difference between the target pitch angle θ ^* and the actual pitch angle θ of the hull 1, and the feedback control unit 33 sets the pitch angle feedback steering angle δr _cFB based on this difference.

In the non-interacting control unit 13a, the target depth z ^* and the target pitch angle θ ^* are input to the differentiators 43a to 43d, whereby the target depth speed w ^* and the target depth acceleration dw ^* / dt are obtained from the target depth z ^*. In addition to the calculation, the target pitch angular velocity r ^* and the target pitch angular acceleration dr ^* / dt are calculated from the target pitch angle θ ^* . The target depth velocity w ^* is output to the coefficient matrix calculation unit 41 and the minute change amount calculation unit 44, and the target depth acceleration dw ^* / dt, the target pitch angular velocity r ^* , and the target pitch angular acceleration dr ^* / dt are minute variations. It is output to the calculation unit 44.

The coefficient matrix calculation unit 41 calculates coefficient matrices A (n) and B (n) using the set speed in the x-axis direction = target speed u ^* and target depth speed w ^* input from the host device. It is output to the delay unit 42. The delay unit 42 stores the current value and outputs the previous values A (n−1) and B (n−1) of the stored coefficient matrix to the interference compensation steering angle calculation unit 45.
On the other hand, the minute change amount calculation unit 44 calculates the minute change amount for each parameter by calculating the difference from the previous value for each input parameter. The calculated minute change amounts Δw ^* , Δdw ^* / dt, Δr ^* , Δdr ^* / dt, Δθ ^* are output to the interference compensation steering angle calculation unit 45.

In the interference compensation steering angle calculation unit 45, the previous values A (n−1) and B (n−1) of the input coefficient matrix and the minute change amounts Δw ^* , Δdw ^* / dt, Δr ^* , Δdr ^* / dt , Δθ ^* are substituted into the interference rudder angle calculation formula possessed in advance to calculate the minute interference compensation rudder angles Δδb _FF and Δδr _cFF . The minute interference compensation rudder angles Δδb _FF and Δδr _cFF are respectively added to the previous interference compensation rudder angle by the integrating unit 46, whereby the current interference compensation rudder angles δb _FF and δr _cFF are obtained.

The interference-compensated steering angle δb _FF related to the depth calculated in this way is added to the depth feedback steering angle δb _FB in the depth steering angle setting unit 24 of the depth control unit 11 to calculate a depth steering angle command δb ^* .
Similarly, the interference compensation steering angle δr _cFF related to the pitch angle is added to the pitch angle feedback steering angle δr _cFB in the pitch angle steering angle setting unit 34 of the pitch angle control unit 12 to calculate the pitch steering angle command δr _c ^*. The
Then, the rudder angles of the rudders 3a to 3e provided in the hull 1 are controlled based on the depth rudder angle command δb ^* and the pitch rudder angle command δr _c ^* .
As a result, the depth z can be made to follow the target depth z ^* while the pitch angle θ is kept at zero, and finally the depth z can be matched with the set depth z _set .

In FIG. 5, the simulation result at the time of the depth change by the control apparatus 10a of the underwater vehicle which concerns on this embodiment is shown. In FIG. 5, (a) shows the speed of the hull, (b) shows the depth, (c) shows the pitch angle, (d) shows the steering angle related to the depth, and (e) shows the temporal transition of the steering angle related to the pitch angle. Yes. 5 (d) and 5 (e), the steering angle command is indicated by a broken line, and the actual steering angle is indicated by a solid line. As can be seen from this figure, as compared with the conventional simulation result shown in FIG. 15, the depth z changes rapidly to the set value, and the variation of the pitch angle θ is suppressed.
Therefore, according to the underwater vehicle control apparatus and control method according to the present embodiment, the accuracy of the hull control can be improved, and the power consumption of the rudder drive system can be reduced.

[Second Embodiment]
Next, an underwater vehicle according to a second embodiment of the present invention, a control device therefor, and a control method will be described with reference to the drawings.
In the first embodiment described above, the state variable used in the interference compensation calculation formula is the minute change amount of the target value. This is based on the theory that linearization is established locally near the equilibrium point. However, when the minute change amount of the target value is used as the state variable, there are many differential calculations and the processing load is large. Therefore, in the present embodiment, the state variable used in the interference compensation calculation formula is set as the target value itself, thereby reducing the processing burden due to differential calculation.

That is, in this embodiment, instead of the state variables Δw ^* , Δdw ^* / dt, Δr ^* , Δdr ^* / dt, and Δθ ^* in the above equations (12) and (13), the state variable that is the target value itself. w ^* , dw ^* / dt, r ^* , dr ^* / dt, θ ^* are used. Thereby, (12) Formula and (13) Formula are represented by the following (14) Formula and (15) Formula.

In FIG. 6, an example of the functional block diagram of the control apparatus 10b of the underwater vehicle which concerns on this embodiment is shown. In FIG. 6, the same components as those shown in FIG. 4 are denoted by the same reference numerals. As shown in FIG. 6, compared with the configuration of the control device 10a according to the first embodiment, the delay unit 42, the minute change amount calculation unit 44, and the integration unit 46 are omitted, and the configuration is simplified.
Thus, according to the control apparatus 10b for the underwater vehicle according to the present embodiment, the configuration can be simplified as compared with the first embodiment, and the burden of calculation processing can be reduced.

In FIG. 7, the simulation result at the time of the depth change by the control apparatus 10b of the underwater vehicle which concerns on this embodiment is shown. In FIG. 7, (a) is the speed of the hull, (b) is the depth, (c) is the pitch angle, (d) is the steering angle related to the depth, and (e) is the temporal transition of the steering angle related to the pitch angle. Yes. In FIGS. 7D and 7E, the steering angle command is indicated by a broken line, and the actual steering angle is indicated by a solid line. As can be seen from this figure, as compared with the conventional simulation result shown in FIG. 20, the depth is rapidly changed to the set value and the variation of the pitch angle is suppressed.
Therefore, according to the underwater vehicle control apparatus and control method according to the present embodiment, the accuracy of the hull control can be improved, and the power consumption of the rudder drive system can be reduced.

[Third Embodiment]
Next, an underwater vehicle according to a third embodiment of the present invention, a control device therefor, and a control method will be described with reference to the drawings.
In the first or second embodiment described above, the equilibrium point in the state equation is set as the target value. However, the present embodiment is different in that the equilibrium point is acquired from an equilibrium point map described later.
The equilibrium point map is a state quantity (for example, a speed u in the x-axis direction, a speed v in the y-axis direction, etc.) for a combination of rudder operation amounts (for example, propeller rotation speed, rudder angle components with respect to the respective orthogonal axes). The equilibrium point of the velocity w in the z-axis direction, the roll angular velocity p, the pitch angular velocity q, the yaw angular velocity r, the roll angle φ, and the pitch angle θ) is obtained in advance by numerical analysis, and the result of the numerical analysis is mapped. FIG. 9 shows an example of an equilibrium point map when the speed setting is equivalent to 2 kt. In the equilibrium point map shown in FIG. 9, when the speed setting is 2 kt, the speed u can be acquired from the steering angle component of the depth and the steering angle component of the pitch angle.

FIG. 8 shows functional blocks related to depth control and pitch angle control in the underwater vehicle control apparatus 10c according to the present embodiment.
The same components as those shown in FIG. 6 are denoted by the same reference numerals.
As shown in FIG. 8, the control device 10c according to the present embodiment has a configuration in which an equilibrium point setting unit 48 and a parameter conversion unit 49 are added to the configuration of the control device 10b according to the second embodiment. Has been.
The equilibrium point setting unit 48 has an equilibrium point map that associates the rudder operation amount and the state quantity of the hull 1, and sets the equilibrium points u _ep and w _ep of the state quantity from the equilibrium point map and the current operation amount. To do.
The parameter conversion unit 49 converts the set speed u _set into a rotational speed command n ^* . The reason for converting the set speed u _{The set} the rotational speed command n ^* as one of the steering operation amount at the equilibrium point map, because the rotational speed command n ^* is used.

According to such a control device 10c, the equilibrium point setting unit 48 has the rotational speed command n ^* converted from the set speed u _set, the depth steering angle command δb ^* set by the depth control unit 11, and the pitch angle. The pitch angle steering angle command δr _c ^* set by the control unit 12 is input. Equilibrium point setting unit 48, the equilibrium point u _ep is uniquely identified from a combination of these input _values, acquired by referring to the equilibrium point map w _ep, acquired equilibrium point u _ep, the w _ep coefficient matrix Output to the calculation unit 41. In FIG. 8, as described in the second embodiment, a configuration example in which the target value itself is used as a state variable is shown. However, as shown in FIG. The present invention may be applied to non-interference control used as a state variable.

In FIG. 10, the simulation result at the time of the depth change by the control apparatus 10c of the underwater vehicle which concerns on this embodiment is shown. 10, (a) is the speed of the hull, (b) is the depth, (c) is the pitch angle, (d) is the steering angle related to the depth, and (e) is the time transition of the steering angle related to the pitch angle. Yes. Further, in FIGS. 10D and 10E, the steering angle command is indicated by a broken line, and the actual steering angle is indicated by a solid line. As can be seen from this figure, as compared with the conventional simulation result shown in FIG. 20, the depth is rapidly changed to the target value and the variation of the pitch angle is suppressed.
Therefore, according to the underwater vehicle control apparatus and control method according to the present embodiment, the accuracy of the hull control can be improved, and the power consumption of the rudder drive system can be reduced.

[Fourth Embodiment]
Hereinafter, a control device and a control method for an underwater vehicle according to a fourth embodiment of the present invention will be described with reference to the drawings.
Although the control apparatus of the underwater vehicle according to the first to third embodiments described above has been described for the case of reducing interference generated between the depth and the pitch angle, in this embodiment, the depth z and the pitch angle θ. A case where the interference generated between the roll angle φ and the azimuth angle ψ is reduced will be described. That is, the embodiment differs from the above embodiments in that the roll angle φ and the azimuth angle ψ are further considered.
Hereinafter, the same components as those of the control device 10b according to the second embodiment will be denoted by the same reference numerals and description thereof will be omitted, and differences from the second embodiment will be mainly described.

11 and 12 are diagrams illustrating functional blocks related to depth control, pitch angle control, roll angle control, and azimuth angle control in the control device 10g according to the present embodiment.
As shown in FIGS. 11 and 12, the control device 10g includes a depth control unit 11 that controls the depth z, a pitch angle control unit 12 that controls the pitch angle θ, a roll angle control unit 70 that controls the roll angle, An azimuth angle control unit 80 that controls the azimuth angle and a non-interference control unit 90 are provided as main components.

The roll angle control unit 70 has the same configuration as the depth control unit 11 described above, and a target value setting unit 71 that sets a continuous target roll angle φ ^* from a set roll angle φ _set that is a discrete value. A difference calculation unit 72 that calculates a difference between the target roll angle φ ^* from the target value setting unit 71 and the actual roll angle φ of the hull 1, and a feedback that sets the roll angle feedback steering angle δr _rFB from the difference from the difference calculation unit 72. The roll angle steering angle that corrects the control unit 73 and the roll angle feedback steering angle δr _rFB using the interference compensation steering angle δr _rFF from the non- _interacting control unit 90 to be described later, and generates a roll angle steering angle command δr _r ^*. A setting unit 34 is provided as a main configuration.

Similarly to the roll angle control unit 70, the azimuth control unit 80 also has a target value setting unit 81 and a target value setting unit for setting a continuous target azimuth angle ψ ^* from a set azimuth angle ψ _set that is a discrete value. A difference calculation unit 82 that calculates the difference between the target azimuth angle ψ ^* from 81 and the actual azimuth angle ψ of the hull 1; a feedback control unit 83 that sets the azimuth feedback steering angle δr _1FB from the difference from the difference calculation unit 82; And an azimuth steering angle setting unit 84 that corrects the azimuth feedback steering angle δr _lFB using an interference compensation steering angle δr _lFF from the non- _interacting control unit 90 described later, and generates an azimuth steering angle command δr _l ^*. It is provided as the main configuration.

The non-interacting control unit 90 is a feedforward system, and uses an interference compensation arithmetic expression derived by an inverse problem analysis from the linearized state equation of the hull 1, and uses a depth z, a pitch angle θ, a roll angle φ, and Interference compensation steering angles δb _FF , δr _cFF , δr _rFF , and δr _lFF for suppressing mutual interference of the azimuth angle ψ are respectively calculated. Here, each configuration of the non-interacting control unit 90 is substantially the same as in the second embodiment described above, but in this embodiment, the depth z, the pitch angle θ, the roll angle φ, and the azimuth angle ψ are mutually determined. Since the interference is suppressed, the calculation formula used for the calculation is different. Hereinafter, each arithmetic expression will be described.

  The linearized equation of state in the vicinity of an arbitrary equilibrium point of the hull 1 is expressed by the following equations (16) and (17). Here, regarding the equilibrium point, it is assumed that the hull is in an equilibrium state at the target value, and the target value of the hull motion is set as the equilibrium point.

  In the equations (16) to (19), u, v, w, p, q, r, φ, θ, and ψ are as defined in FIG. 3, n is the rotational speed [rps], and δb is the rudder 3e. In other words, the steering angles [rad], δr1, δr2, δr3, and δr4 related to the depth are the steering angles of the rudder 3a, rudder 3b, rudder 3c, and rudder 3d, respectively.

  Next, assuming that there is an equilibrium point (dX / dt = 0) where the state quantity in the equilibrium state settles at a certain X with respect to an arbitrary manipulated variable U, the following expression (20) is established.

f (X ^* , U ^* ) = 0 (20)

  When the minute fluctuation amounts from this state are ΔX and ΔU, the following equation (21) is obtained.

  If the above equation (21) is Taylor-expanded in the vicinity of the equilibrium point to be a first order approximation, a state equation around the equilibrium point expressed by the following equation (22) is obtained.

  In the formula (22), A is given by the following formula (23), and B is given by the following formula (24).

  From the above, the linearized state equation is expressed by the following equation (25).

The elements a _{11 to} b ₉₆ of the A and B matrices are expressed as functions or constants at certain equilibrium points X ^* and U ^* . For example, the elements a ₁₄ , a ₁₅ and a ₁₆ are represented by the following formulas (26) to (28). The other elements are naturally derived from the above equations (16) to (22).

  In the equations (26)-(28), the suffix of the coefficient K is the same as the suffix of the coefficient K in the first embodiment described above.

  Next, an inverse problem analysis is performed on the linearized equation of state, and a steering angle command for canceling the interference is solved. That is, the manipulated variable when it is desired to control to an arbitrary state target value can be calculated by the following equation (29) by performing inverse problem analysis on the above equation (22).

ΔU is a minute change amount of the command value of the operation amount, and ΔX ^* is a minute change amount of the state amount. A simultaneous equation is created by substituting a value based on each target value for each of the parameters relating to the depth, roll angle, pitch angle, and azimuth angle with respect to the above equation (29). By solving for the minute changes Δδb, Δδr1, Δδr2, Δδr3, and Δδr4, an interference compensation steering angle command for reducing the mutual interference can be obtained.
For example, when only the azimuth angle ψ is changed and it is desired to maintain the current values for the depth z, the roll angle φ, and the pitch angle θ, a value based on the target azimuth angle ψ is substituted for the parameter related to the azimuth angle ψ. All parameters other than the azimuth angle ψ are set to 0. Then, by solving the simultaneous equations at that time for minute changes Δδb, Δδr1, Δδr2, Δδr3, Δδr4 of each steering angle command, a steering angle command for avoiding the influence on the other parameters due to the change of the azimuth angle ψ is obtained. Can be obtained.

  Here, in the present embodiment, the interference compensation steering angle is calculated using the inverse analysis calculation formula obtained by replacing the minute change amount shown in the formula (29) with an absolute value as the interference compensation calculation formula. That is, the interference compensation calculation formula according to this embodiment is expressed by the following formula (30).

  Thus, by replacing with an absolute value instead of a minute change amount, the configuration can be simplified as compared with the case where a minute change amount is used, and the burden of calculation processing can be reduced. Hereinafter, a specific configuration of the non-interference control unit 90 in the case where non-interference control is performed using the interference compensation calculation formula shown in the above equation (30) will be mainly described with reference to FIG.

  As illustrated in FIG. 11, the non-interacting control unit 90 includes a coefficient matrix calculation unit 91, differentiators 92a to 92h, an interference compensation steering angle calculation unit 93, and a coordinate conversion unit 94 as main components.

The coefficient matrix calculator 91 receives the target speed u ^* in the x-axis direction and the target speed v ^* in the y-axis direction. The target speeds u ^* and v ^* are, for example, values input from the host device. The coefficient matrix calculation unit 91 also includes a target speed w ^* in the z-axis direction obtained by differentiating the target depth z ^* set by the target value setting unit 21 and the target roll set by the target roll angle setting unit 71. The pitch angular velocity q obtained by differentiating the angle φ ^* and the roll angular velocity p ^* obtained by differentiating the target roll angle φ, the target pitch angle θ ^* set by the target value setting unit 31 and the target pitch angle θ ^{*. * And} the target azimuth velocity r ^* obtained by differentiating the target azimuth angle ψ ^* set by the target value setting unit 81 are input. Strictly speaking, conversion from the absolute coordinate system (z, φ, θ, ψ) to the relative coordinate system (w, p, q, r) requires a correction expression other than differentiation, but is omitted here. Yes.

  The coefficient matrix calculation unit 91 calculates functions (some of which are constants and zeros) for calculating each element of the coefficient matrices A and B, as exemplified by the above expressions (26) to (28). The coefficient matrices A (n) and B (n) at the current equilibrium point are calculated by substituting the input values for these functions.

The interference compensation steering angle calculation unit 93 receives the coefficient matrices A (n) and B (n) calculated by the coefficient matrix calculation unit 91, the target depth velocity w ^* and the target depth acceleration dw ^* / dt, Target pitch angle θ ^* , target pitch angular velocity q ^* and target pitch angular acceleration dq ^* / dt, target roll angle φ ^* , target roll angular velocity p ^* and target roll angular acceleration dp ^* / dt, target azimuth angular velocity r ^* and target The azimuth acceleration dr ^* / dt is input. In order to calculate these, differentiators 92a to 92h are provided, respectively.

The interference compensation rudder angle calculation unit 93 has an interference compensation calculation formula (for example, the above-described formula (30)), and by substituting the input value into these formulas, the interference compensation rudder angle δb _FF , δr1, δr2, δr3, and δr4 are obtained, respectively.
Coordinate conversion unit 94, the interference compensation steering angle calculating section 93 interference compensating steering angle δr1 obtained by, .DELTA.R2, .DELTA.R3, roll angle .DELTA.R4, pitch angle, azimuth angle of the interference compensation steering angle [delta] r _RFF corresponding to each, [delta] r _{Convert to cFF,} δr _lFF . This is based on the geometric rudder arrangement, and the four rudders are converted into rudder forces acting on the roll, pitch and bearing.

The depth-compensated steering angle δb _FF is added to the depth feedback steering angle δb _FB in the depth steering angle setting unit 24 of the depth control unit 11 shown in FIG. 12 to obtain the depth steering angle command δb ^* .
Similarly, the roll angle steering angle command δr _r is _obtained by adding the interference compensation steering angle δr _rFF related to the roll angle to the roll angle feedback steering angle δr _rFB in the roll angle steering angle setting unit 74 of the roll angle control unit 70. ^* Is calculated.
The pitch angle steering angle command δr _c ^* is calculated by adding the interference compensation steering angle δr _cFF related to the pitch angle to the pitch angle feedback steering angle δr _cFB in the pitch angle steering angle setting unit 34 of the pitch angle control unit 12. Is done.
The interference compensation steering angle δr _lFF related to the azimuth is added to the azimuth feedback steering angle δr _lFB in the azimuth steering angle setting unit 84 of the azimuth control unit 80, _thereby calculating the azimuth steering angle command δr _l ^*. Is done.

The roll angle rudder angle command δr _r ^* , pitch angle rudder angle command δr _c ^* , and azimuth angle rudder angle command δr _l ^* are controlled by the coordinate conversion unit 85 to the rudder control commands δr1 ^* , δr2 ^* , δr3. ^* And δr4 ^* , respectively. Thereby, the rudder 3e is based on the rudder angle command δb ^* , the rudder 3a is based on the rudder control command δr1 ^* , the rudder 3b is based on the rudder control command δr2 ^* , and the rudder 3c is based on the rudder control command δr3 ^*. The rudder 3d is driven and controlled based on the rudder control command δr4 ^* , thereby suppressing the mutual interference among the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ, and setting the hull posture to the target posture. It is possible to follow.

As described above, according to the underwater vehicle control apparatus 10g according to the present embodiment, it is possible to suppress the mutual interference among the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ. Therefore, the accuracy of hull control can be improved, and the power consumption of the rudder drive system can be reduced.
In the present embodiment, the state variable used in the interference compensation calculation formula is the absolute value of the target value, but a minute change amount may be used as in the first embodiment. In this case, as in the first embodiment described above, a configuration having functions of the delay unit 42, the minute change amount calculation unit 44, and the integration unit 46 is required.

[Fifth Embodiment]
Hereinafter, a control device and a control method for an underwater vehicle according to a fifth embodiment of the present invention will be described with reference to the drawings.
In the underwater vehicle control apparatus 10g according to the fourth embodiment described above, the differentiators 92a to 92h are used to obtain the velocity / angular velocity and the acceleration / angular acceleration. Instead, in this embodiment, as shown in FIGS. 13 and 14, by extracting the internal state of the ideal response model included in the target value setting units 21 ′, 31 ′, 71 ′, 81 ′, the speed / angular speed And acceleration / angular acceleration are output.

  13 and 14 are functional block diagrams of the control device 10h according to the present embodiment. As shown in FIGS. 13 and 14, in the control device 10h according to the present embodiment, the differentiators 92a to 92h shown in FIGS. 11 and 12 are omitted, and the target value setting units 21 ′, 31 ′, 71 ′, and 81 are omitted. Each velocity / angular velocity and acceleration / angular acceleration is output from the '. Other configurations are the same as the configurations shown in FIGS.

FIG. 15 is a diagram illustrating a configuration example of a target value setting unit 31 ′ included in the pitch angle control unit 12 ′. Note that the target value setting unit 21 ′ of the depth control unit 11 ′, the target value setting unit 71 ′ of the roll angle control unit 70 ′, and the target value setting unit 81 ′ of the azimuth angle control unit 80 ′ have the same configuration. ing. As shown in FIG. 15, the target value setting unit 31 ′ includes subtractors 101 and 102, proportional devices 103 and 104, and integrators 105 and 106. When the pitch angle set value θset is input to the target value setting unit 31 ′, the input of the integrator 105 is the target pitch angular acceleration dq ^* / dt, the input of the integrator 106 is the target pitch angular velocity q ^* , and the output is The target pitch angle θ ^* . The target pitch angular acceleration dq ^* / dt, the target pitch angular velocity q ^* , and the target pitch angle θ ^* are output to the non-interacting control unit 90. Here, the proportional gain Kp of the proportional device 103 is given by the following equation (31), and the proportional gain Kv of the proportional device 104 is given by the following equation (32).

Kp = ω _n / 2ζ (31)
Kv = 2ζω _n (32)

In equations (31) and (32), ω _n is the natural angular frequency, and ζ is the attenuation factor. The natural angular frequency ω _n is a set value given from the outside. However, since the responsiveness of the hull changes when the speed u changes, it is preferable to change the set value according to the speed u. For example, when the speed u is relatively slow, the responsiveness of the pitch angle is dull. When the speed u is relatively fast, the responsiveness of the pitch angle is good. Accordingly, a table in which the speed u and the natural angular frequency ω _n are associated with each other according to such characteristics is prepared, and the natural frequency ω _n corresponding to the speed u is sequentially set from this table. Good. The natural angular frequency ω _n may be set to a different value in each of the target value setting units 21 ′, 31 ′, 71 ′, and 81 ′. This is because the response of each parameter differs depending on the speed u.
Although the attenuation rate ζ is a fixed value, for example, different values may be set for the proportional gains Kp and Kv.

Thus, according to the control apparatus and control method for an underwater vehicle according to the present embodiment, a state that occurs in the calculation process of the ideal response model included in the target value setting units 21 ′, 31 ′, 71 ′, and 81 ′. Since the variable speed (angular velocity) and acceleration (angular acceleration) are input to the non-interacting control unit 90, the differentiators 92a to 92h can be omitted.
In the underwater vehicle control apparatus according to the first to third embodiments, the target value setting units 21 ′ and 31 ′ may be employed instead of the target value setting units 21 and 31. Thereby, the differentiators 43a to 43d can be omitted.

[Sixth Embodiment]
Next, an underwater vehicle according to a sixth embodiment of the present invention, a control device therefor, and a control method will be described with reference to the drawings.
In each of the above-described embodiments, the non-interference control units 13a, 13b, and 90 for suppressing interference in the control between the axes are provided and the influence of the interference is suppressed by feedforward control. When this occurs, the interference suppression effect is reduced.

  Therefore, the control device 10d (see FIG. 16) according to the present embodiment further includes a disturbance compensation unit 15a for reducing a control error caused by an interference force that cannot be compensated by the feedforward control, an unknown disturbance, or the like. It is characterized by that.

  FIG. 16 is a diagram illustrating functional blocks related to depth control and pitch angle control in the control device 10d according to the present embodiment. In FIG. 16, the same reference numerals are given to components common to the above-described first embodiment. As for the non-interacting control unit 13a, it is possible to employ the non-interacting control unit 13b according to the second embodiment and the non-interacting control unit 13c according to the third embodiment. It is also possible to employ an interference control unit. Moreover, it is good also as a structure which abbreviate | omitted the non-interacting control part. Thus, the presence or absence of the non-interacting control unit and its configuration are not particularly limited.

  The disturbance compensation unit 15a calculates a disturbance compensation steering angle for suppressing the disturbance using a disturbance compensation calculation formula derived from the linearized state equation of the hull 1 by inverse problem analysis. Here, before describing each component of the disturbance compensation unit 15a, a process for deriving a disturbance compensation calculation expression will be described. The concept is substantially the same as the above-described interference compensation calculation formula.

  First, in the linearized state equation of the hull 1 in the vicinity of an arbitrary equilibrium point, the coefficient matrix is expressed as a function of the equilibrium point, and the linearity is adaptively changed so as to be continuously established in the entire operation region of the hull 1. Create a generalized equation of state. Here, also in the present embodiment, the target value is adopted as the equilibrium point of the hull motion.

Subsequently, a state variable related to an axis causing mutual interference is specified, and model reduction is performed so as to obtain a linearized state equation including the specified state variable, thereby reducing the matrix size.
For example, when mutual interference occurs between the depth z and the pitch angle θ, state variables related to the depth z and the pitch angle θ are extracted, and a linearized state equation specialized for the depth / pitch angle control system is extracted. Reduce the model. Here, in the present embodiment, one of the purposes is to suppress the disturbance, so a term related to the disturbance is included in the linearized state equation.
(33) shows an example of the linearized state equation after model reduction. Each element of the coefficient matrix in equation (33) is as described in equation (5) above.

In the above equation (33), d ₁ is a component related to the depth direction (z-axis direction) of the disturbance, and d ₂ is a component related to the pitch angle of the disturbance.

Next, an inverse problem analysis is performed on the linearized state equation after model reduction including disturbances, and solving for the disturbances d ₁ and d ₂ yields Equation (34).

When the minute disturbance components Δd ₁ and Δd ₂ are calculated in this manner, the minute disturbance compensation steering angles Δb _dis and Δr _cdis for canceling the calculated disturbance are _expressed as follows using conversion coefficients k1 and k2: You can get like that.

Δb _dis = k1 · Δd ₁ (35)
Δr _cdis = k2 · Δd ₂ (36)

In Expressions (35) and (36), Δb _dis is a minute disturbance compensation steering angle for canceling the minute disturbance component Δd ₁ in the z-axis direction, and Δr _cdis is a minute value for canceling the minute disturbance component Δd ₂ in the y-axis direction. This is the disturbance compensation rudder angle.

Next, the configuration of the disturbance compensation unit 15a according to the present embodiment will be described with reference to FIG.
The disturbance compensation unit 15a mainly includes a coefficient matrix calculation unit 51, a delay unit 52, differentiators 53a to 53d, a minute change amount calculation unit 54, a disturbance compensation steering angle calculation unit 55, and an integration unit 56.

The coefficient matrix calculation unit 51 receives the actual speed u in the x-axis direction and the actual speed w in the z-axis direction. The actual speed u is a value detected by, for example, a sensor attached to the hull 1, and the actual speed w is obtained by differentiating the actual depth z detected by the sensor attached to the hull 1 by the differentiator 53a. Values are used.
The coefficient matrix calculation unit 51 holds mathematical formulas of the respective elements represented by the above formula (5). By substituting the real speed u and the real depth w into these mathematical formulas, the current matrix 1 can be obtained. The corresponding coefficient matrices A (n) and B (n) are calculated.

The calculated coefficient matrices A (n) and B (n) are input to the delay unit 52. The delay unit 52 outputs the coefficient matrices A (n−1) and B (n−1) obtained in the previous control cycle to the disturbance compensation steering angle calculation unit 55. Thus, the reason why the coefficient matrices A (n−1) and B (n−1) obtained in the control cycle one cycle before are output is to match the minute change amount calculation unit 54 described later. is there.
The minute change amount calculation unit 54 receives the actual velocity w and actual acceleration dw / dt in the z-axis direction, the actual angular velocity q and actual angular acceleration dq / dt of the rotating shaft corresponding to the pitch angle, and the actual pitch angle θ. The The actual speed w is a value obtained by differentiating the actual depth z once by the differentiator 53a, and the actual acceleration dw / dt is a value obtained by differentiating the actual depth z twice by the differentiators 53a and 53b. The actual angular velocity q is a value obtained by differentiating the actual pitch angle θ detected by a sensor attached to the hull 1 once by the differentiator 53c, and the actual angular acceleration dq / dt is obtained by differentiating the actual pitch angle θ by the differentiators 53c and 53d. This is a value obtained by differentiating twice.

  The minute change amount calculation unit 54 calculates the minute change amounts Δw, Δdw / dt, Δq, Δdq / dt, Δθ of these input values and outputs them to the disturbance compensation steering angle calculation unit 55. Specifically, the minute change amount calculation unit 54 calculates the minute change amount by calculating the difference between the previous value and the current value of the control cycle for each input value.

The disturbance compensation rudder angle calculation unit 55 has disturbance compensation calculation formulas prepared in advance according to the above-described procedure, and substitutes the state variables input from the delay unit 52 and the minute change amount calculation unit 54 into these formulas. Thus, the minute disturbance compensation steering angles Δδ _dis and Δδr _cdis for canceling out the minute disturbance component Δd ₁ related to the depth direction and the minute disturbance component d ₂ related to the pitch angle are calculated.

The accumulating unit 56 _adds the minute disturbance compensation steering angles Δb _dis and Δr _cdis to the disturbance compensation steering angle obtained in the previous control cycle, so that the disturbance compensation steering angle δb _bis and the pitch angle related to the depth in the current control cycle are obtained. A disturbance compensation steering angle δr _cbis is calculated.
The disturbance compensation steering angle δb _dis regarding the depth output from the integration unit 56 is added to the depth feedback steering angle δb _FF in the depth steering angle setting unit 24 ′ of the depth control unit 11, thereby obtaining a depth steering angle command δb ^*. Is calculated.
Similarly, the disturbance compensation steering angle Δδr _cdis regarding the pitch angle output from the integrating unit 56 is added to the pitch angle feedback steering angle δr _cFB in the pitch angle steering angle setting unit 34 ′ of the pitch angle control unit 12. , ^* the pitch rudder angle command δr _c is calculated.

Then, based on the depth rudder angle command δb ^* and the pitch rudder angle command δr _c ^* , the rudder angles of the rudders 3a to 3e provided in the hull 1 are controlled, thereby reducing the control accuracy due to unknown disturbances and the like. It becomes possible to suppress.

  In this embodiment, the state variable used in the disturbance compensation calculation formula is the minute change amount of the detected value. However, as in the second embodiment described above, the detected value itself is used to reduce the processing load due to differential calculation. Also good.

In the above-described embodiment, in the calculation of the coefficient matrix, the coefficient matrix is calculated using the actual speed u in the x-axis direction and the actual speed w in the z-axis direction. However, as in the control device 10e illustrated in FIG. In addition, the values input to the coefficient matrix calculation unit 51 of the disturbance compensation unit 15b may be the set speed u _set in the x-axis direction and the target speed w ^* in the z-axis direction, instead of the sensor detection value.
For example, when a sensor detection value is used, there is a possibility that a signal in an unfavorable frequency band is included due to the influence of noise and waves, and the calculation accuracy of the coefficient matrix is lowered, thereby reducing the control accuracy. There is a fear. On the other hand, by calculating the coefficient matrix using the set value and the target value, it is possible to avoid a decrease in control accuracy due to noise or the like included in the sensor detection value.

[Seventh Embodiment]
Next, an underwater vehicle according to a seventh embodiment of the present invention, a control device therefor, and a control method will be described with reference to the drawings.
In the sixth embodiment described above, the set value, target value, or detected value is differentiated by the differentiators 53a to 53d to obtain the speed and acceleration. However, when these differentiators 53a to 53d are band differential, a delay occurs, so that it is difficult to maintain a common phase relationship for each element of position, velocity, and acceleration. In particular, since the differentiation of the sensor detection value includes a noise component, the noise component included in the differential signal may further increase.
Therefore, in the present embodiment, the above-mentioned inconvenience due to differentiation is solved by calculating speed and acceleration using an ideal response model instead of a differentiator.

FIG. 18 is a diagram illustrating functional blocks related to depth control and pitch angle control in the control device 10f according to the present embodiment.
As shown in FIG. 18, the disturbance compensator 15 c according to the present embodiment replaces the differentiators 53 a to 53 d with a velocity / acceleration calculator 58 a that calculates the velocity and acceleration related to the depth, and the angular velocity and angular acceleration related to the pitch angle. And an angular velocity / angular acceleration calculation unit 58b.

For example, the speed / acceleration calculation unit 58a has an ideal response model as shown in FIG. 19, and the internal state (position, speed, acceleration) when the position z is input is extracted from the ideal response model. , Continuous position zm, velocity dzm / dt, acceleration d ² zm / dt ² are acquired. Here, the ideal response model is not limited to the model shown in FIG. 19, and may be a secondary system, a model simulating control dynamic characteristics, a model having a low-pass characteristic (for example, one following model), or the like. Good.

Similarly, the angular velocity / angular acceleration calculation unit 58b acquires an angle, an angular velocity, and an angular acceleration using an ideal response model regarding the pitch angle.
As described above, by obtaining information on speed and acceleration without using differentiation, noise components and the like can be reduced, and control accuracy can be improved.

  Further, in the control device 10f according to the present embodiment, the response model used in the target value setting unit 21 ′ and the target value setting unit 31 ′ of the depth control unit 11 ′ is changed to the ideal response model as described above, and this ideal A continuous target value, velocity (angular velocity), and acceleration (angular acceleration) are obtained from the response model. In this way, the differentiators 43a to 43d can be omitted, and a decrease in control accuracy due to noise can be suppressed.

  As mentioned above, although each embodiment of the present invention has been described, the scope of the present invention is not limited only to the above-described embodiment. For example, each of the above-described embodiments is within the scope of the invention. Various modifications can be made, for example, by combining them partially or entirely.

1 Hull 10a-10h Underwater vehicle control device 11, 11 'Depth control unit 12, 12' Pitch angle control units 13a-13c, 13a ', 90 Decoupling control units 15a, 15b, 15c Disturbance compensation unit 70, 70 'roll angle control unit 80, 80' azimuth control unit

Claims (11)

Control of an underwater vehicle capable of controlling at least two of six axes including three linear axes including the x-axis, y-axis, and z-axis of the hull and three rotational axes for each of the linear axes. A device,
A plurality of feedback control units provided corresponding to the respective axes to be controlled;
Using an interference compensation calculation formula, calculating an interference compensation steering angle for canceling the mutual interference between the axes, and providing a non-interference control unit of the feedforward system that outputs to the control unit,
In the linearization equation of state in the vicinity of an arbitrary equilibrium point of the hull, the interference compensation calculation formula is expressed by a coefficient matrix as a function of the equilibrium point, and is linearized so as to be adaptive over the entire operating range of the hull. It is an arithmetic expression derived by solving the state equation inversely with respect to a parameter related to the rudder operation amount necessary for performing a desired motion,
The non-interference control unit is an underwater vehicle control apparatus that obtains the interference compensation steering angle by substituting the value of the equilibrium point and the state variable according to time into the interference compensation calculation formula.   The underwater vehicle control apparatus according to claim 1, wherein a target value related to the motion of the hull is used as the equilibrium point and the state variable. The plurality of control units include a depth control unit corresponding to the z-axis and a pitch angle control unit corresponding to a rotation axis with respect to the y-axis,
The interference compensation calculation formula includes a target speed in the x-axis direction and a target speed in the z-axis direction as equilibrium points, and the target speed and target acceleration in the z-axis direction as the state variables and the rotation axis relative to the y-axis. The underwater vehicle control apparatus according to claim 1, wherein the target angular velocity, the target angular acceleration, and the target pitch angle are included. The plurality of control units include a depth control unit corresponding to the z-axis, a pitch angle control unit corresponding to the rotation axis with respect to the y-axis, a roll angle control unit corresponding to the rotation axis with respect to the x-axis, an azimuth angle control unit corresponding to the rotation axis with respect to the z axis,
In the interference compensation calculation formula, the target speed in the x-axis direction, the target speed in the z-axis direction, the target speed in the y-axis direction, the target angular speed and target angle around the x-axis, the target angular speed and target around the y-axis as equilibrium points Angle, target angular velocity around the z-axis,
As the state variables, target velocity and target acceleration in the z-axis direction, target angular velocity and target angular acceleration of the rotation axis with respect to the y-axis, target pitch angle, target angular acceleration around the x-axis, target angular velocity and target angle, z-axis The control apparatus for an underwater vehicle according to claim 1 or 2, wherein the surrounding target angular acceleration and target angular velocity are included. For the equilibrium point, a target value related to the motion of the hull is used,
The underwater vehicle control device according to claim 1, wherein a minute change amount of a target value related to the motion of the hull is used as the state variable. The plurality of control units include a depth control unit corresponding to the z-axis and a pitch angle control unit corresponding to a rotation axis with respect to the y-axis,
The interference compensation calculation formula includes a target speed in the x-axis direction and a target speed in the z-axis direction as equilibrium points, and the state variables include a minute change amount in speed in the z-axis direction and a minute change amount in acceleration, 6. The underwater vehicle control apparatus according to claim 1 or 5, wherein a minute change amount of angular velocity and a minute change amount of angular acceleration of the rotation axis with respect to the y axis and a minute change amount of the target pitch angle are included. The plurality of control units include a depth control unit corresponding to the z-axis, a pitch angle control unit corresponding to the rotation axis with respect to the y-axis, a roll angle control unit corresponding to the rotation axis with respect to the x-axis, an azimuth angle control unit corresponding to the rotation axis with respect to the z axis,
In the interference compensation calculation formula, the target speed in the x-axis direction, the target speed in the z-axis direction, the target speed in the y-axis direction, the target angular speed and target angle around the x-axis, the target angular speed and target around the y-axis as equilibrium points Angle, target angular velocity around the z-axis,
As the state variables, the minute change amount of the velocity in the z-axis direction and the minute change amount of the acceleration, the minute change amount of the angular velocity of the rotation axis with respect to the y axis and the minute change amount of the angular acceleration, the minute change amount of the target pitch angle, x The minute change amount of the target angular acceleration around the axis, the minute change amount of the target angular velocity and the minute change amount of the target angle, the minute change amount of the target angular acceleration around the z axis, and the minute change amount of the target angular velocity are included. The underwater vehicle control apparatus according to claim 5. The non-interacting control unit is
It has an equilibrium point of state quantity for the combination of the manipulated variables of the hull as an equilibrium point map in advance.
The balance point of the state quantity corresponding to the manipulated variable of the hull for each control cycle is acquired from the balance point map, and the acquired balance point of the state quantity is used in the interference compensation calculation formula, and the interference compensation steering The underwater vehicle control device according to claim 1, wherein the control unit calculates an angle.   The underwater vehicle control device according to any one of claims 1 to 8, wherein only the state variable related to the axis in which mutual interference occurs is used as the state variable included in the interference compensation calculation formula. .   An underwater vehicle comprising the control device for an underwater vehicle according to any one of claims 1 to 9. A feedback control system provided for each of at least two of six axes including three linear axes including the x-axis, y-axis, and z-axis of the hull and three rotation axes for each of the linear axes; A control method for an underwater vehicle having a feedforward system for calculating an interference compensation rudder angle to cancel the mutual interference of control,
In the feedforward system, in the linearization state equation in the vicinity of an arbitrary equilibrium point of the hull, the linearization state is expressed so that the coefficient matrix is expressed as a function of the equilibrium point and is adaptively changed over the entire operating range of the hull. Substituting the value of the equilibrium point and the value of the state variable according to time into the interference compensation equation derived by reversing the equation with respect to the parameters related to the rudder operation amount necessary to perform the desired motion The control method of the underwater vehicle which obtains the interference compensation rudder angle by.

Images