JP6878186B2 - Underwater vehicle control device and underwater vehicle control method - Google Patents

Description

The present invention relates to a control device for an underwater vehicle, an underwater vehicle, and a method for controlling the underwater vehicle.

Conventionally, in the control of an underwater vehicle, so-called feedback control is mainly used in which a steering angle command is set by performing PID control (Proportional-Integral-Derivative Control) for the difference between the target value and the current state quantity. It is done.

In order to set the rudder angle command, control is performed for each axis such as depth, roll angle, pitch angle, and azimuth angle in the hull, but mutual interference occurs between specific axes.
In particular, when the feedback control system is designed independently on each control axis, even if the hull swings, the feedback control system corresponding to each control axis will independently suppress the swing. Therefore, there is a problem that an interference force acts between the control axes and it takes a considerable amount of time for the fluctuation to converge.

Therefore, in order to make the interference force acting between the plurality of control axes when the target value of the motion of the underwater vehicle is changed non-interfering, the compensation value of the operation amount for each steering is calculated by the feedforward control system. However, a control method for giving a compensation value to the feedback control system corresponding to each control axis has been proposed (Patent Document 1).

JP-A-2015-33997

However, in the invention described in Patent Document 1, for compensation by feedforward control, it is possible to suppress the interference force generated between the control axes when the target value of the motion of the underwater hull changes, for example. The interference force between the control axes generated when the hull swings due to disturbance such as waves and the state quantity fluctuates is not suppressed.

Therefore, when the hull swings due to disturbance, mutual interference between the control axes cannot be effectively suppressed, and it takes a considerable amount of time for the swing of the entire hull to converge.

The present invention has been made in view of such circumstances, and is a control device for an underwater vehicle that can improve the accuracy of control of the feedback system and effectively suppress the swing of the hull due to disturbance. And an underwater vehicle and a method for controlling the underwater vehicle.

The first aspect of the present invention is an underwater vehicle that can be controlled on six axes including three linear axes including the x-axis, y-axis, and z-axis of the hull, and three rotation axes with respect to each of the linear axes. Interference compensation steering angle for canceling mutual interference between axes by using the control unit of the feedback system provided corresponding to at least two axes to be controlled and the interference compensation calculation formula. The non-interference control unit of the feedback system that calculates and outputs to the control unit, and the interference compensation calculation formula uses the coefficient matrix of the hull in the linearization state equation near an arbitrary equilibrium point of the hull. Expressed as a function of the state amount and the operation amount of the hull, the linearized state equation changed so as to be adaptable over the entire operating range of the hull, with respect to the parameters related to the steering operation amount required to perform the desired motion. It is an arithmetic expression derived by inverse problem analysis, and the non-interference control unit substitutes the deviation of the state amount and the target value according to the time into the interference compensation arithmetic expression, thereby causing the interference compensation steering angle. To get.

According to such a configuration, the interference compensation calculation formula derived by using the time-varying linearization state equation applicable to the entire operating range of the hull is used, so that the interference compensation rudder angle at the equilibrium point at each time can be obtained in real time. It can be easily obtained, and the accuracy of feedback system control can be improved. Then, by reflecting this interference compensation rudder angle in the feedback control system designed independently on each control axis, it is possible to effectively reduce the mutual interference between each control axis.
The linearized equation of state can generally be obtained by Taylor-expanding a non-linear equation of state around a certain equilibrium point and performing a first-order approximation.
Further, the interference compensation calculation formula is characterized in that the velocity and acceleration of the element related to the target axis that suppresses mutual interference are included as variables. This is because the interference compensation calculation formula is created based on the linearized equation of state.

Also, for example, when a general feedforward control system and a general feedback control system are used together, if a disturbance occurs when the target value is changed, the feedforward control system due to the change in the target value There is a possibility that the operation and the operation of the feedback control system due to the occurrence of disturbance overlap, and the control systems adversely affect each other. However, in the non-interference control unit of the feedback system according to the first aspect of the present invention, the interference compensation steering angle is obtained by substituting the deviation of the state quantity with respect to the target value into the interference compensation calculation formula, so that a general feed is obtained. Even when used in combination with a forward control system (not limited to those having a non-interference function), the feedforward control system controls changes in the target value and does not control disturbances without affecting each other. The interference control unit can do this. Therefore, it is not necessary to switch between the feedforward control system and the non-interference control unit, and each control can be executed at the same time.
Further, even if the configuration does not have a feedforward control system, the non-interference control unit according to the first aspect of the present invention substitutes the deviation of the state quantity with respect to the target value into the interference compensation calculation formula to compensate for interference. Since the steering angle is obtained, even if the target value is changed, it is possible to accurately perform non-interference control without erroneously detecting the change in the target value as a disturbance.

In the control device for the underwater vehicle, a measured value relating to the state of the hull may be used as the state quantity.

In this way, the interference compensation rudder angle can be obtained from the interference compensation calculation formula using the measured value related to the state of the hull as the state quantity. Therefore, even if the hull swings due to disturbance or the like, mutual interference between the control axes is prevented. It can be effectively reduced to reduce rocking.

In the control device for the underwater vehicle, the control units include a depth control unit, a roll angle control unit corresponding to the rotation axis with respect to the x-axis, a pitch angle control unit corresponding to the rotation axis with respect to the y-axis, and z. The azimuth angle control unit corresponding to the rotation axis with respect to the axis is included, and the interference compensation calculation formula includes the actual velocity in the x-axis direction, the actual velocity in the y-axis direction, the actual velocity in the z-axis direction, and the actual velocity in the z-axis direction. Real acceleration, real angular velocity and real angle acceleration of the rotation axis with respect to the x-axis, real roll angle, real-angle velocity and real-angle acceleration of the rotation axis with respect to the y-axis, real-pitch angle, real-angle velocity and real-angle acceleration of the rotation axis with respect to the z-axis It may be included.

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

In the control device for the underwater vehicle, only the state variables related to the axes on which mutual interference occurs may be used as the state variables included in the interference compensation calculation formula.

By limiting the state variables included in the interference compensation calculation formula in this way, it is possible to eliminate operations that are not related to interference, and it is possible to reduce the load related to the calculation processing and improve the processing speed.

A second aspect of the present invention is an underwater vehicle including the above-mentioned control device for the underwater vehicle.

A third aspect of the present invention corresponds to at least two of the six axes consisting of three linear axes consisting of the x-axis, y-axis, and z-axis of the hull and three rotation axes with respect to each of the linear axes. This is a control method for an underwater hull having a feedback control unit provided therein and a non-interference control unit of a feedback system for calculating an interference compensation steering angle for canceling mutual interference between axes. In the linearization state equation near an arbitrary equilibrium point, the coefficient matrix is expressed as a function of the state amount of the hull and the operation amount of the hull, and the linearization state is changed so as to be adaptable over the entire operating range of the hull. By substituting the deviation between the state quantity and the target value according to the time into the interference compensation calculation formula derived by performing an inverse problem analysis of the equation with respect to the parameters related to the steering operation amount required to perform the desired motion. The interference compensation steering angle is obtained.

According to the present invention, it is possible to improve the accuracy of the control of the feedback system and effectively suppress the swing of the hull due to the disturbance.

It is a figure which showed the schematic structure of the underwater vehicle which concerns on one Embodiment of this invention. It is a figure which shows typically the arrangement of the rudder when the underwater vehicle shown in FIG. 1 is seen from the stern. It is a figure for demonstrating the degree of freedom of movement of the underwater vehicle which concerns on one Embodiment of this invention. It is a figure which showed the functional block which concerns on depth control, roll angle control, pitch angle control, and azimuth angle control in the control device of the underwater vehicle which concerns on one Embodiment of this invention. It is a figure which showed the functional block about the non-interference control part in the control device of the underwater vehicle which concerns on one Embodiment of this invention. It is a figure which showed the simulation result about the depth at the time of applying the wave disturbance by the control device of the underwater vehicle which concerns on one Embodiment of this invention. It is a figure which showed the simulation result about the pitch angle at the time of applying the wave disturbance by the control device of the underwater vehicle which concerns on one Embodiment of this invention. It is a figure which showed the simulation result about the azimuth when the wave disturbance is applied by the control device of the underwater vehicle which concerns on one Embodiment of this invention.

Hereinafter, an embodiment of the control device for the underwater vehicle, the underwater vehicle, and the control method for the underwater vehicle of the present invention will be described 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 diagram of a rudder arrangement when the underwater vehicle shown in FIG. 1 is viewed from the stern. The figure and FIG. 3 shown are diagrams for explaining the degree of freedom of movement of the underwater vehicle.

As shown in FIG. 3, the hull 1 of the underwater vehicle is composed of 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 capable of controlling 6 axes (6 degrees of freedom) including 3 linear axes orthogonal to each other and 3 rotation 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, Defined as 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 is defined as the pitch angle θ, and the rotation angle around the z-axis is defined as the azimuth angle ψ.
As shown in FIGS. 1 and 2, the hull 1 is provided with a plurality of rudders 3a to 3e. The rudders 3a to 3d are mainly used for controlling the roll angle φ, the pitch angle θ, and the azimuth angle ψ, and the rudders 3e are mainly used for controlling the depth.
Then, these rudders 3a to 3e are operated based on each rudder angle command generated by the control device 10 according to each embodiment described later, so that the six-axis states (for example, mainly in the z-axis direction) are operated. (Depth, pitch angle, roll angle, and azimuth), which are the positions in the above, are controlled to follow the respective target values.

Hereinafter, the control device 10 for the underwater vehicle, the underwater vehicle, and the control method for the underwater vehicle according to the embodiment of the present invention will be described with reference to the drawings. The underwater vehicle control device 10 according to the present embodiment includes a plurality of control units provided for each of the above-mentioned six axes and a non-interference control unit 16 for suppressing interference between these axes. In the following description, for convenience, control in the z-axis direction (depth control), control of the rotation axis corresponding to the x-axis (roll angle φ control), control of the rotation axis corresponding to the y-axis (pitch angle θ control), and Taking control of the rotation axis corresponding to the z-axis (azimuth angle ψ control) as an example, a case of reducing interference between each control axis of depth, roll angle, pitch angle and azimuth angle will be described. Regarding the interference between the control axes, at least two or more of the control axes can be appropriately selected because of mutual interference, and the interference is not limited to the above-mentioned set of depth, roll angle, pitch angle and azimuth angle.

FIG. 4 is a diagram showing functional blocks related to depth control, roll angle control, pitch angle control, and azimuth angle control in the control device for the underwater vehicle according to the present embodiment (hereinafter, simply referred to as “control device 10”). Is.
The control device 10 includes a depth control unit 11 that controls the depth, a roll angle control unit 12 that controls the roll angle, a pitch angle control unit 13 that controls the pitch angle, and an azimuth angle control unit 14 that controls the azimuth angle. , A feedforward control unit (hereinafter, simply referred to as "FF control unit 15") and a non-interference control unit 16 are provided.

Depth control unit 11, the discrete target value setting unit 11a that the value a is set depth z _{The set} to set a continuous target depth z ^*, the target depth from the target value setting unit 11a z ^* and the actual depth of the hull 1 The difference calculation unit 11b that calculates the difference from z, the _{feedback control unit 11c that sets the depth feedback rudder angle δb FB} from the difference from the difference calculation unit 11b, and the FF control unit 15 that sets the depth feedback rudder angle δb _{FB later.} The main configuration is a depth steering angle setting unit 11d that corrects using the compensation steering angle δb _{FF and the interference compensation steering angle δb fb} from the non-interference control unit 16 ^{and generates a depth steering angle command δb *.}

^{The target value setting unit 11a obtains a continuous target depth z *} by using a predetermined response model for _{a set depth z set} which is a discrete value input from a higher-level device (not shown), for example. An example of the response model is a second-order low-pass filter as shown in the following equation (1).

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

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

_{The feedback control unit 11c sets the depth feedback steering angle δb FB} by, for example, performing PID control on the difference from the difference calculation unit 11b.
The depth steering angle setting unit 11d is realized as an adder / subtractor _{that adds the depth feedback steering angle δb FB} and the compensation steering angle δb _FF from the FF control unit 15 and subtracts the interference compensation steering angle δb _fb.

Roll angle controller 12, discrete target value setting unit 12a that the value a is set roll angle phi _{The set} to set a continuous target roll angle phi ^*, the target roll angle phi ^* the hull from the target value setting unit 12a The difference calculation unit 12b that calculates the difference from the actual roll angle φ of 1, _{the feedback control unit 12c that sets the roll angle feedback steering angle δr rFB} from the difference from the difference calculation unit 12b, and the roll angle feedback steering angle δr _rFB will be described later. _{Compensation steering angle δr r} FF from the FF control unit 15 _{and interference compensation steering angle δr rfb} from the non-interference control unit 16 are used to correct and generate a roll angle steering angle _{command δr r} ^*. Is provided as the main configuration.
Since each configuration of the roll angle control unit 12 is substantially the same as that of the depth control unit 11, detailed description thereof will be omitted.

Pitch control unit 13, the discrete target value setting unit 13a that the value a is set 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 13a The difference calculation unit 13b that calculates the difference from the actual pitch angle θ of 1 and the _{feedback control unit 13c that sets the pitch angle feedback rudder angle δr cFB} from the difference from the difference calculation unit 13b, and the pitch angle feedback rudder angle δr _cFB will be described later. _{Compensation steering angle δr cFF} from the FF control unit 15 _{and interference compensation steering angle δr cfb} from the non-interference control unit 16 are used to correct and generate the pitch angle steering angle _{command δr c} ^*. Is provided as the main configuration.
Since each configuration of the pitch angle control unit 13 is substantially the same as that of the depth control unit 11, detailed description thereof will be omitted.

Azimuth control unit 14, the discrete target value setting unit 14a that the value a is set azimuth [psi _{The set} to set a continuous target azimuth [psi ^*, the target azimuth [psi ^* the hull from the target value setting unit 14a The difference calculation unit 14b that calculates the difference from the actual azimuth angle ψ of 1, the feedback control unit 14c that sets the _{azimuth feedback steering angle δr lFB from the difference from the difference calculation unit 14b, and the azimuth feedback steering angle δr lFB} will be described later. compensation steering angle [delta] r _LFF and corrected using the interference compensation steering angle [delta] r _LFB from decoupling control unit 16, azimuth steering angle setting unit 14d that generates an azimuth steering angle command [delta] r _l ^* from the FF control unit 15 for Is provided as the main configuration.
Since each configuration of the azimuth control unit 14 is substantially the same as that of the depth control unit 11, detailed description thereof will be omitted.

The FF control unit 15 is a feedforward system, and is a compensating rudder for the amount of operation for each rudder in order to make the interference force acting between a plurality of control axes when the target value of the motion of the hull 1 is changed non-interfering. The angles δb _FF , δr _rFF , δr _cFF , and δr _lFF are calculated. For example, target values such as target depth z ^* , target roll angle φ ^* , target pitch angle θ ^* , and target azimuth angle ψ ^* are input. The FF control unit 15 does not have to make the interference force between the control axes non-interfering as long as it is a control method for compensating the steering angle based on the target value of the motion of the hull 1, and the above configuration. Not limited to.

The non-interference control unit 16 is a feedback system, and is a state quantity with respect to a time and a state quantity with respect to a target value with respect to an interference compensation calculation formula derived by an inverse problem analysis from the linearized equation of state of the hull 1. _{By substituting the deviation, the interference compensation steering angles δb fb} , δr _rfb , δr _cfb , and δr _lfb for suppressing mutual interference with the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ are calculated. Here, before explaining each configuration of the non-interference control unit 16, the process of deriving the interference compensation calculation formula will be described.

Generally, the motion of the hull 1 is non-linear, but it can be linearized near the equilibrium point. For example, the linearized equation of state near an arbitrary equilibrium point of the hull 1 is expressed by the following equation (3).

In equation (3), u is the speed force in the x-axis direction [m / s], v is the speed force in the y-axis direction [m / s], w is the speed force in the z-axis direction [m / s], and p is the roll angular velocity [m / s]. rad / s], q is pitch angular velocity [rad / s], r is azimuth angular velocity [rad / s], z is depth [m], φ is roll angle [rad], θ is pitch angle [rad], ψ is Azimuth [rad], n is the number of revolutions [rps], δb is the steering angle of the steering wheel 3e, in other words, steering angle commands [rad], δr _φ , δr _θ , δr _ψ are the steering wheel 3a, the steering wheel 3b, and the steering wheel. In other words, it is a steering angle command [rad] relating to the roll angle φ, the pitch angle θ, and the azimuth angle ψ in relation to the steering angles of 3c and the steering 3d.

Subsequently, by expressing the coefficient matrices A and B of the linearization state equation near an arbitrary equilibrium point as a function of an arbitrary equilibrium point, the locally established linearization is continuously performed in the entire operating region of the hull 1. Transform so that it holds true. Here, the equilibrium point represents the current state of the hull 1, and the current state is measured by sensors provided on the hull 1, such as the current depth, roll angle, pitch angle, and azimuth of the hull 1. Information (state amount) that can be acquired, and the current depth rudder angle command δb ^* , roll angle rudder angle command δr _r ^* , pitch angle rudder angle command δr _c ^* , and azimuth rudder angle command δr _l ^{* of the hull 1.} Information about the operation (operation amount).

The elements of the coefficient matrices A and B as described above are expressed as a function of the state quantity and the manipulated quantity of the hull 1 at the equilibrium point when linearized with respect to an arbitrary equilibrium point. Therefore, by adapting the coefficient matrices A and B to the change of the equilibrium point of the hull 1, the linearized equation of state that holds only locally can be held in the entire operating region.

Next, the state variables related to the axes where mutual interference occurs, that is, the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ are specified, and the model is formed so as to be a linearized state equation consisting of the specified state variables. Perform reduction to reduce matrix size. In this way, by using a linearized equation of state specialized for the axis in which mutual interference occurs, state variables unrelated to mutual interference can be excluded, and the processing load can be reduced.

Equation (4) shows an example of a linearized equation of state specialized for the control system related to the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ. Further, the equations (5)-(33) show the elements of the coefficient matrices A and B included in the equation (4). As shown in Eqs. (5)-(33), each element of the coefficient matrices A and B is mathematically expressed as a function of the state quantity and the manipulated quantity of the hull 1 at the equilibrium point (note that some of them). Elements are represented as constants).

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

_{For example, K wvp} described in Eq. (6) indicates that it is a _{coefficient K wvp} related to the v · p term in the equation of motion (Equation (34)) relating to the depth velocity w in the z-axis direction of the hull 1. ..

Similarly it, for example, _{K Wuudb} according to equation (8) in the z-axis direction of the depth speed w about the equation of motion of the hull 1 ((35)), a coefficient _{K Wuudb} according to ^{u 2} · [delta] b term Is shown.

Further, in each element of the coefficient matrices A and B represented by the equations (5)-(33), dr1, dr2, dr3, and dr4 represent the rudder angles of the rudders 3a to 3d, respectively. For example, when the rudder angle of the rudder 3a is changed, the forces of three components, the horizontal rudder component contributing to the pitch direction of the hull, the vertical rudder component contributing to the yaw direction, and the roll rudder component contributing to the roll direction, are exerted. Acts on the hull. Therefore, dr1, dr2, dr3, and dr4 used in each element of the coefficient matrices A and B give the roll angle steering angle command δr _r ^* , the pitch angle steering angle command δ _{r c} ^* , and the azimuth steering angle command δ r _l ^* . Calculated by vector composition.

Next, the linearization equation of state after model reduction is analyzed by an inverse problem, and the rudder angle command for canceling the interference is solved.

Specifically, the state equation of the above equation (4) is applied to the steering angle command δb [rad] regarding the depth, the steering angle command δr _{φ [rad] regarding the roll angle φ, and the steering angle command δr θ} [rad] regarding the pitch angle θ. By solving the steering angle command δr _ψ [rad] with respect to the azimuth angle ψ, the following equations (36)-(39) can be obtained. In the process of deriving equations (36)-(39), the state variables depth velocity w [m / s] in the z-axis direction, roll angular velocity p [rad / s], pitch angular velocity q [rad / s], The azimuth angular velocity r [rad / s] was replaced with the deviation between the target value and the state quantity, respectively. Roll angular velocity Specifically, the depth speed w and depth speed w and the target depth which is a deviation of the speed w ^* depth speed deviation e _w, which is a deviation of the roll angular velocity p and the roll angular velocity p and the target roll angular velocity p ^* The deviation e _{p , the pitch angular velocity q is the pitch angular velocity deviation e q} , which is the deviation between the pitch angular velocity q and the target pitch angular velocity q ^*, and the azimuth angular velocity r is the azimuth angular velocity r *, which is the deviation between the azimuth angular velocity r and the target azimuth angular velocity r ^*. It is the deviation _{e r.} The depth acceleration dw / dt, roll angular acceleration dp / dt, pitch angular acceleration dq / dt, and azimuth angular acceleration dr / dt are also the deviations between the ^{depth acceleration dw / dt and the target depth acceleration dw * / dt, respectively.} A certain depth acceleration deviation _edw , a roll angular acceleration dp / dt and a target roll angular acceleration dp ^* / dt, which is a deviation between the roll angular acceleration deviation _edp , a pitch angular acceleration dq / dt and a target pitch angular acceleration dq ^* / dt. The pitch angular acceleration deviation _edq , which is the deviation of, and the azimuth angular acceleration deviation _edr , which is the deviation between the azimuth angular acceleration dr / dt and the target angular acceleration dr ^* / dt, are replaced. The roll angle φ and the pitch angle θ were also replaced with the _{roll angle deviation e φ} and the pitch angle deviation e _{θ, respectively.}

Next, a specific configuration of the non-interference control unit 16 according to the present embodiment will be described. FIG. 5 is a diagram showing a functional block related to the non-interference control unit 16 in the control device 10 of the underwater vehicle according to the embodiment of the present invention. The non-interference control unit 16 mainly includes a coefficient matrix calculation unit 31, a differentiation unit 32 to 39, a coordinate conversion calculation unit 40, a deviation calculation unit 41 to 48, and a non-interference compensation steering angle calculation unit 50. ..

The coefficient matrix calculation unit 31 includes navigation speed u, skid speed v, depth speed w, roll angular velocity p, pitch angular velocity q, azimuth angular velocity r, depth steering angle command δb ^* , roll angle steering angle command δr _r ^* , and pitch angle. The steering angle command δ r _c ^* and the azimuth angle steering angle command δ r _l ^* are input. The coefficient matrix calculation unit 31 has mathematical formulas for each element of the coefficient matrices A and B represented by the above equations (5)-(33), and the navigation speed u and the skid speed v input in these mathematical formulas. , Depth angular velocity w, roll angular velocity p, pitch angular velocity q, azimuth angular velocity r, depth steering angle command δb ^* , roll angle steering angle command δr _r ^* , pitch angle steering angle command δr _c ^* , azimuth angle steering angle command δr _l ^* By substituting, the coefficient matrices A (n) and B (n) at the current equilibrium point are calculated. Then, the coefficient matrices A (n) and B (n) at the current equilibrium point are stored, and the coefficient matrices A (n-1) and B (n-1) obtained in the previous control cycle are used as a non-interference compensation rudder. Output to the angle calculation unit 50.

The coordinate conversion calculation unit 40 has a roll angle φ, a pitch angle θ, a depth velocity dz / dt output from the differential unit 32, and a roll angular velocity dφ / dt output from the differential unit 33 in the absolute coordinate system (earth coordinate system). The pitch angular velocity dθ / dt output from the differentiation unit 34 and the azimuth angular velocity dψ / dt output from the differentiation unit 35 are coordinate-converted into the depth angular velocity w, the roll angular velocity p, the pitch angular velocity q, and the azimuth angular velocity r in the hull coordinate system. .. Specifically, the coordinate conversion calculation unit 40 has a coordinate conversion calculation formula (formula (40)), and the input roll angle φ, the pitch angle θ, the z output from the differentiation unit 32, and the differentiation unit. By substituting the roll angular velocity dφ / dt output from 33, the pitch angular velocity dθ / dt output from the differential unit 34, and the azimuth angular velocity dψ / dt output from the differential unit 35 into the coordinate conversion calculation formula, the depth angular velocity w , Roll angular velocity p, pitch angular velocity q, and azimuth angular velocity r are obtained.

The deviation calculation unit 41 calculates the deviation e _w between the depth speed w output from the coordinate conversion calculation unit 40 and the target depth speed w ^*. Similarly, the other deviation calculation unit 42 to 44, the roll angular velocity p and the target roll angular velocity p ^* deviation between e _p, pitch angular velocity q and the target pitch angular velocity q ^* deviation between e _q, azimuth angular velocity r and the target azimuth angular velocity to calculate the deviation _{e r} and r ^*.
Deviation calculation unit 45 is output from the coordinate transformation calculator 40 calculates the deviation _{e dw} between the differential processing depth-acceleration dw / dt target depth acceleration ^dw * / dt by the differentiation unit 36. Similarly, the other deviation calculation unit 46 to 48, and the deviation _{e dp,} the pitch angle acceleration dq / dt and the target pitch angle acceleration ^dq * / dt of the roll angle acceleration dp / dt and the target roll angle acceleration ^dp * / dt deviation _{e dq} of, calculating the deviation _{e dr} between azimuth acceleration dr / dt and the target azimuth acceleration ^dr * / dt.

The non-interference compensation steering angle calculation unit 50 has interference compensation calculation formulas (for example, the above-mentioned (36)-(39) formulas), and the coefficient matrix calculation unit 31 and the deviation calculation unit 41 to these formulas are included. By substituting the values output from 48, the interference compensation steering angles δb _fb , δr _rfb , δr _cfb , and δr _lfb are obtained.

Next, the operation of the control device 10 of the underwater vehicle according to the present embodiment will be described.
When the set depth z _set , the set roll angle φ _{set, the} set pitch angle θ _set , and the set azimuth angle ψ _{set set in the host device} are input, the set depth z _set is sent to the depth control unit 11 and the set roll angle φ _set. Is input to the roll angle control unit 12, the set pitch angle θ _set is input to the pitch angle control unit 13, and the set azimuth angle ψ _set is input to the azimuth angle control unit 14.
In the depth control unit 11, the set depth z _set is input to the target value setting unit 11a, and is converted ^{from a discrete value to a target depth z * which is a continuous value.} The target depth z ^* is output to the difference calculation unit 11b. The difference calculation unit 11b calculates the difference ^{between the target depth z *} and the actual depth z of the hull 1, and the feedback control unit 11c sets the _{depth feedback steering angle δb FB based on this difference.}

Similarly, in the roll angle control unit 12, the set roll angle φ _set is input to the target value setting unit 12a, and is converted ^{from a discrete value to a target roll angle φ * which is a continuous value.} The target roll angle φ ^* is output to the difference calculation unit 12b. The difference calculation unit 12b calculates the difference between the target roll angle φ ^* and the actual roll angle φ of the hull 1, and the feedback control unit 12c sets the _{roll angle feedback steering angle δr rFB based on this difference.}

Similarly, in the pitch angle control unit 13, the set pitch angle θ _set is input to the target value setting unit 13a, and is converted ^{from a discrete value to a target pitch angle θ * which is a continuous value.} The target pitch angle θ ^* is output to the difference calculation unit 13b. The difference calculation unit 13b calculates the difference between the target pitch angle θ ^* and the actual pitch angle θ of the hull 1, and the feedback control unit 13c sets the _{pitch angle feedback rudder angle δr cFB based on this difference.}

Similarly, in the azimuth control unit 14, the set azimuth angle ψ _set is input to the target value setting unit 14a, and is converted from a discrete value to a continuous value, the target azimuth angle ψ ^*. The target azimuth angle ψ ^* is output to the difference calculation unit 14b. The difference calculation unit 14b calculates the difference between the target azimuth angle ψ ^* and the actual azimuth angle ψ of the hull 1, and the feedback control unit 14c sets the _{azimuth feedback steering angle δr lFB based on this difference.}

By inputting the target value of the motion of the hull 1, the FF control unit 15 calculates _{the compensating rudder angles δb FF} , δr _rFF , δr _cFF , and δ _{r lFF for compensating the rudder angle.}

In the non-interference control unit 16, the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ are input to the differential units 32 to 35, so that the depth velocity dz / dt, the roll angular velocity dφ / dt, and the pitch angular velocity dθ / dt and azimuth angular velocity dψ / dt are output. The output depth velocity dz / dt, roll angular velocity dφ / dt, pitch angular velocity dθ / dt, azimuth angular velocity dψ / dt, roll angle φ, and pitch angle θ are input to the coordinate conversion calculation unit 40, and the depth velocity in the hull coordinate system. Coordinates are converted into w, roll angular velocity p, pitch angular velocity q, and azimuth angular velocity r.

Coordinate conversion depth-speed w, the roll angular velocity p, the pitch angular velocity q, azimuth angular velocity r is the deviation calculation unit 41 to 44, the depth speed w and the target depth speed w ^* and the depth speed deviation e _w, the roll angular velocity p target roll angular velocity p ^* and roll angular velocity deviation e _p of _the pitch angular velocity deviation e _q and ^* pitch angular velocity q and the target pitch angular velocity _q, azimuth angular speed deviation e _r of the azimuth angular velocity r and the target azimuth angular velocity r ^* is calculated ..

Further, the coordinate-transformed depth velocity w, roll angular velocity p, pitch angular velocity q, and directional angular velocity r are input to the deviation calculation units 45 to 48 via the differential units 36 to 39, respectively. Then, the deviation calculation unit 45 to 48, the depth acceleration dw / dt and the target depth acceleration ^dw * / dt deviation between _{e dw,} the roll angle acceleration dp / dt and the deviation _{e dp} between the target roll angle acceleration ^dp * / dt, deviation _{e dq} between the pitch angular acceleration dq / dt and the target pitch angle acceleration ^dq * / dt, the deviation _{e dr} between azimuth acceleration dr / dt and the target azimuth acceleration ^dr * / dt is calculated.

In the coefficient matrix calculation unit 31, the navigation speed u, the skid speed v, the depth speed w, the roll angular velocity p, the pitch angular velocity q, the azimuth angular velocity r, the depth steering angle command δb ^* , and the roll angle steering angle command δr input from the host device. _The coefficient matrices A (n) and B (n) are calculated using ^{r *} , the pitch angle steering angle command δ _{r c} ^* , and the azimuth angle steering angle command δ r _l ^*. Then, the coefficient matrices A (n) and B (n) at the current equilibrium point are stored, and the coefficient matrices A (n-1) and B (n-1) obtained in the previous control cycle are used as a non-interference compensation rudder. Output to the angle calculation unit 50.

The non-interference compensation steering angular velocity calculation unit 50 includes the coefficient matrices A (n-1) and B (n-1) calculated by the coefficient matrix calculation unit 31, and the depth velocity deviation e calculated by the deviation calculation units 41 to 48. _w, roll angular velocity deviation _{e p,} the pitch angular velocity deviation _{e q,} azimuth angular speed deviation _{e r,} depth acceleration deviation _{e dw,} the roll angle acceleration deviation _{e dp,} the pitch angle acceleration deviation _{e dq,} azimuth acceleration deviation _{e dr,} sailing speed u , Side slip speed v, roll angle deviation e _φ , pitch angle deviation e _θ are input. Each of the input values is substituted into the interference compensation calculation formula held by the non-interference compensation steering angle calculation unit 50, and the interference compensation steering angles δb _fb , δr _rfb , δr _cfb , and δr _lfb are calculated.

_{The interference compensation rudder angle δb fb with} respect to the depth calculated in this way is negatively fed back to the _{depth feedback rudder angle δb FB} in the depth steering angle setting unit 11d of the depth control unit 11. _{Specifically, the compensation steering angle δb FF} from the FF control unit 15 _{is added to the depth feedback steering angle δb FB} from the depth steering angle setting unit 11d, and further, the interference compensation steering angle δb _{fb from the non-interference control unit 16} Is subtracted to calculate the depth rudder angle command δb ^*.
Similarly, the interference compensation steering angle δr rfb _regarding the roll angle is negatively fed _back to the roll angle feedback steering angle δr rFB in the roll angle steering angle setting unit 12d of the roll angle control unit 12. _{Specifically, the compensation steering angle δr rFF} from the FF control unit 15 is added to the roll angle feedback steering angle δr _{rFB from the roll angle setting unit, and further, the interference compensation steering angle δr rfb} from the non-interference control unit 16 is added. By subtracting, the roll angle rudder angle command δr _r ^* is calculated.
Similarly, the interference compensation rudder angle δr _cfb regarding the pitch angle is negatively fed _back to the pitch angle feedback rudder angle δr cFB in the pitch angle rudder angle setting unit 13d of the pitch angle control unit 13. _{Specifically, the compensation steering angle δr cFF} from the FF control unit 15 is added to the pitch angle feedback steering angle δr _{cFB from the pitch angle setting unit, and further, the interference compensation steering angle δr cfb} from the non-interference control unit 16 is added. By subtracting, the pitch angle rudder angle command δr _c ^* is calculated.
Similarly, the interference compensation rudder angle δr lfb _regarding the azimuth angle is negatively fed _back to the azimuth feedback rudder angle δr lFB in the azimuth angle rudder angle setting unit 14d of the azimuth angle control unit 14. _{Specifically, the compensation steering angle δr lFF} from the FF control unit 15 _{is added to the azimuth feedback steering angle δr lFB} from the azimuth steering angle setting unit 14d, and the interference compensation steering angle from the non-interference control unit 16 is further added. By _{subtracting δr lfb} , the azimuth angle rudder angle command δr _l ^* is calculated.

Then, the ship motion system, depth steering angle command [delta] b ^*, roll angle steering angle command [delta] r r ^_*, pitch rudder angle command [delta] r c ^_*, based azimuth steering angle command [delta] r _l ^*, provided the hull 1 rudder The rudder angles of 3a to 3d are controlled.

FIG. 6 shows a simulation result regarding the depth when wave disturbance is applied by the control device 10 of the underwater vehicle according to the present embodiment. In FIG. 6, (a) shows the relationship between depth and time, and (b) shows the relationship between rudder angle and time. The alternate long and short dash line indicates the target value, and the depth is set to a constant value as the target value. When a disturbance is applied, the depth is greatly swung up and down with respect to the target value in the conventional control (dotted line), whereas in the control according to the present embodiment, the rudder angle is appropriately controlled. It can be seen that the fluctuation is greatly suppressed by. The conventional control is a control when the control device 10 of the underwater vehicle according to the present embodiment does not have the non-interference control unit 16. That is, in the conventional control, the interference force between the control axes is not made non-interfering, and the feedback control system designed independently on each control axis controls the fluctuation due to the disturbance.

FIG. 7 shows a simulation result regarding a pitch angle when a wave disturbance is applied by the control device 10 of the underwater vehicle according to the present embodiment. In FIG. 7, (a) shows the relationship between the pitch angle and time, and (b) shows the relationship between the rudder angle and time. The alternate long and short dash line indicates the target value, and the pitch angle is set to a constant value as the target value. When a disturbance is applied, the pitch angle is greatly shaken with respect to the target value in the conventional control (dotted line), whereas in the control according to the present embodiment, the rudder angle is appropriately controlled. It can be seen that the rocking is greatly suppressed.

FIG. 8 shows a simulation result regarding the azimuth angle when the wave disturbance is applied by the control device 10 of the underwater vehicle according to the present embodiment. In FIG. 8, (a) shows the relationship between the azimuth angle and time, and (b) shows the relationship between the rudder angle and time. The alternate long and short dash line indicates the target value, and the azimuth angle is set to a constant value as the target value. When a disturbance is applied, the pitch angle is greatly shaken with respect to the target value in the conventional control (dotted line), whereas in the control according to the present embodiment, the rudder angle is appropriately controlled. It can be seen that the rocking is suppressed.

Based on the simulation results, the underwater hull control device 10 according to the present embodiment compensates for each steering angle calculated by the non-interference control unit 16 for the feedback control system independently designed on each control axis. By feeding back the values, it is possible to effectively suppress the interference force generated between the control axes and reduce the swing of the hull as a whole against disturbance.

As described above, according to the control device for the underwater vehicle, the underwater vehicle, and the control method for the underwater vehicle according to the present embodiment, the linearized state linearized at an arbitrary equilibrium point of the hull 1. From the equation, the amount of interference is estimated for each control axis, and the amount of operation for canceling the amount of interference is estimated. That is, since the linearized equation of state can be sequentially obtained according to the current state of the hull 1, it is possible to estimate the operation amount that deinterferes the interference amount between the control axes in any state of the hull 1. Can be done. Therefore, it is possible to deal with non-linear motion characteristics and suppress interference between each control axis.

Therefore, even if the hull 1 swings due to a disturbance such as a wave, the rudder control can be performed while suppressing the interference between the control axes, and the swing of the hull 1 can be effectively suppressed. .. Due to this effect, automatic navigation of the hull can be realized in a stable attitude even in stormy weather, which can greatly contribute to the improvement of motion control performance.

Also, for example, when a general feedforward control system and a general feedback control system are used together, if a disturbance occurs when the target value is changed, the feedforward control system due to the change in the target value There is a possibility that the operation and the operation of the feedback control system due to the occurrence of disturbance overlap, and the control systems adversely affect each other. However, the non-interference control unit 16 of the feedback system according to the present embodiment has a configuration in which the deviation of the state quantity with respect to the target value is substituted into the interference compensation calculation formula to obtain the interference compensation steering angle, so that general feedforward control is performed. Even when used in combination with a system (not limited to those having a non-interference function), the feedforward control system controls changes in the target value and controls against disturbances without interfering with each other. Unit 16 can do this. Therefore, it is not necessary to switch between the feedforward control system and the non-interference control unit 16, and each control can be executed at the same time.
Further, even if the configuration does not have a feedforward control system, the non-interference control unit 16 according to the present embodiment substitutes the deviation of the state amount with respect to the target value into the interference compensation calculation formula, and the interference compensation steering angle. Therefore, even if the target value is changed, it is possible to accurately perform non-interference control without erroneously detecting the change in the target value as a disturbance.

1: Rudder 3a to 3e: Rudder 10: Control device 11: Depth control unit 11a: Target value setting unit 11b: Difference calculation unit 11c: Feedback control unit 11d: Depth rudder angle setting unit 12: Roll angle control unit 12a: Target value Setting unit 12b: Difference calculation unit 12c: Feedback control unit 12d: Roll angle steering angle setting unit 13: Pitch angle control unit 13a: Target value setting unit 13b: Difference calculation unit 13c: Feedback control unit 13d: Pitch angle steering angle setting unit 14: Direction angle control unit 14a: Target value setting unit 14b: Difference calculation unit 14c: Feedback control unit 14d: Direction angle rudder angle setting unit 15: FF control unit 16: Non-interference control unit 31: Coefficient matrix calculation unit 32 to 39 : Differentiation unit 40: Coordinate conversion calculation unit 41-48: Deviation calculation unit 50: Non-interference compensation rudder angle calculation unit

Claims (6)

A control device for an underwater vehicle that can be controlled on six axes consisting of three linear axes consisting of the x-axis, y-axis, and z-axis of the hull and three rotation axes for each of the linear axes.
A feedback system control unit provided corresponding to at least two axes to be controlled, and
Using the interference compensation calculation formula, the non-interference control unit of the feedback system that calculates the interference compensation steering angle for canceling the mutual interference between the axes and outputs it to the control unit,
With
The interference compensation calculation formula is applied to the entire operating range of the hull by expressing the coefficient matrix as a function of the state quantity of the hull and the operation amount of the hull in the linearized state equation near an arbitrary equilibrium point of the hull. It is an arithmetic expression derived by performing an inverse problem analysis of the linearized state equation changed as possible for the parameters related to the steering operation amount required to perform the desired motion.
The non-interference control unit is a control device for an underwater vehicle that obtains the interference compensation steering angle by substituting the deviation between the state quantity and the target value according to the time into the interference compensation calculation formula. The control device for an underwater vehicle according to claim 1, wherein a measured value relating to the state of the hull is used as the state quantity. The control unit includes a depth control unit, a roll angle control unit corresponding to the rotation axis with respect to the x-axis, a pitch angle control unit corresponding to the rotation axis with respect to the y-axis, and an azimuth angle control unit corresponding to the rotation axis with respect to the z-axis. Includes parts,
In the interference compensation calculation formula, the state quantities include the actual velocity in the x-axis direction, the actual velocity in the y-axis direction, the actual velocity and the actual acceleration in the z-axis direction, the actual angular velocity and the actual angular acceleration of the rotation axis with respect to the x-axis, and the actual angular velocity. The underwater vehicle according to claim 1 or 2, which includes the real roll angle, the real angular velocity and the real angular acceleration of the rotation axis with respect to the y-axis, and the real pitch angle, the real angular velocity and the real angle acceleration of the rotation axis with respect to the z-axis. Control device. The control device for an underwater vehicle according to any one of claims 1 to 3, wherein only the state variables related to the axis in which mutual interference occurs are used as the state variables included in the interference compensation calculation formula. .. An underwater vehicle including the control device for the underwater vehicle according to any one of claims 1 to 4. A feedback control unit provided corresponding to at least two of the six axes consisting of three linear axes consisting of the x-axis, y-axis, and z-axis of the hull and three rotation axes for each of the linear axes. It is a control method of an underwater vehicle having a non-interference control unit of a feedback system that calculates an interference compensation steering angle for canceling mutual interference between axes.
In the linearized state equation near an arbitrary equilibrium point of the hull, the coefficient matrix is expressed as a function of the state quantity of the hull and the manipulated quantity of the hull, and is changed so as to be adaptable over the entire operating range of the hull. Substitute the deviation between the state quantity and the target value according to time into the interference compensation calculation formula derived by inverse problem analysis of the linearized state equation for the parameters related to the steering operation amount required to perform the desired motion. A method of controlling an underwater hull to obtain the interference compensation steering angle.

Images