JP7404033B2 - Control system and structure, control method, and control program - Google Patents

Description

The present disclosure relates to a control system and structure, a control method, and a control program.

In structures such as underwater vehicles and flying objects, the attitude of the aircraft is controlled by operating means (for example, Patent Document 1). For example, in an underwater vehicle, the attitude of the vehicle is controlled by controlling a rudder provided on the vehicle.

International Publication No. 2018/110598

However, if the rudder collides with a reef, for example, a part of the rudder may be mechanically damaged (for example, stuck or deformed). If some of the plurality of rudders break down, there may be a problem in controlling the attitude of the aircraft. Specifically, if the rudder is stuck at a certain rudder angle, the attitude of the aircraft may be affected depending on the rudder angle of the failed rudder. In such a case, it becomes difficult to operate continuously.

The present disclosure has been made in view of such circumstances, and aims to provide a control system and structure, a control method, and a control program that can be operated more continuously.

A first aspect of the present disclosure is a control system that is applied to a structure that controls a control amount related to the attitude of an aircraft body by a plurality of operating means that is greater than the number of control amounts, and which acquires state quantities of the aircraft body. an acquisition unit; and in a state equation in which the state quantity of the aircraft is a state variable and the operation amount of the operation means is input, a term related to the failure operation means is determined based on information regarding the failure operation means, which is the operation means in which a failure has occurred. is set as a disturbance term and calculates the operation amount of the operation means other than the failure operation means based on the obtained state quantity, the operation section calculates the operation amount of the operation means other than the failure operation means based on the state equation. The control system calculates the operation amount of the operation means other than the failure operation means so as to cancel the disturbance term .

A second aspect of the present disclosure is a control method applied to a structure in which a control amount related to the attitude of an aircraft body is controlled by a plurality of operation means that is greater than the number of control amounts, the method comprising: acquiring state quantities of the aircraft body. In the acquisition step, in a state equation in which the state quantity of the aircraft is a state variable and the operation amount of the operation means is input, a term related to the failure operation means is determined based on information regarding the failure operation means, which is the operation means in which a failure has occurred. a calculation step of setting as a disturbance term and calculating the operation amount of the operation means other than the failure operation means based on the obtained state quantity , and in the calculation step, based on the state equation, The control method calculates the operation amount of the operation means other than the failure operation means so as to cancel the disturbance term .

A third aspect of the present disclosure is a control program that is applied to a structure that controls a control amount regarding the attitude of an aircraft body by a plurality of operation means that is greater than the number of control amounts, the program acquiring state quantities of the aircraft body. In the acquisition process and the state equation in which the state quantity of the aircraft is a state variable and the operation amount of the operation means is input, a term regarding the failure operation means is determined based on information regarding the failure operation means, which is the operation means in which a failure has occurred. is set as a disturbance term, and a calculation process of calculating the operation amount of the operation means other than the failure operation means based on the obtained state quantity, and in the calculation process, the calculation process is performed based on the state equation. The control program calculates the operation amount of the operation means other than the failure operation means so as to cancel the disturbance term .

According to the present disclosure, there is an effect that driving can be performed more continuously.

1 is a diagram illustrating an underwater vehicle according to a first embodiment of the present disclosure. It is a diagram illustrating an X rudder according to a first embodiment of the present disclosure. FIG. 1 is a diagram showing an example of the hardware configuration of a control device according to a first embodiment of the present disclosure. FIG. 2 is a functional block diagram showing functions included in a control device according to a first embodiment of the present disclosure. FIG. 2 is a diagram illustrating a configuration example of a failure diagnosis section according to a first embodiment of the present disclosure. FIG. 3 is a diagram illustrating a flowchart of failure rudder candidate identification processing in a failure candidate identification unit according to the first embodiment of the present disclosure. FIG. 3 is a diagram showing a flowchart of failure steering angle calculation processing in the calculation unit according to the first embodiment of the present disclosure. FIG. 3 is a diagram illustrating a flowchart of degeneration processing in the degeneration command unit according to the first embodiment of the present disclosure. FIG. 2 is a diagram illustrating a configuration example of a fault-tolerant control unit according to a first embodiment of the present disclosure. FIG. 3 is a diagram showing a flowchart of arithmetic processing in the fault-tolerant control unit according to the first embodiment of the present disclosure. FIG. 6 is a diagram showing the effects of the faulty rudder identification process according to the first embodiment of the present disclosure. FIG. 6 is a diagram showing the effect of non-interference by the fault-tolerant control unit according to the first embodiment of the present disclosure. FIG. 7 is a diagram illustrating a configuration example of a failure diagnosis section according to a second embodiment of the present disclosure.

[First embodiment]
DESCRIPTION OF EMBODIMENTS Below, a first embodiment of a control system, structure, control method, and control program according to the present disclosure will be described with reference to the drawings. The control system can be applied to a wide range of structures that are controlled by redundant operating means (for example, a rudder), and is not limited to the underwater vehicle 1 as described below. . For example, the control system can be applied to structures such as aircraft, rockets, and drones, regardless of whether they are manned or unmanned.

The rotation angle around the x-axis in an absolute coordinate system (earth coordinate system) is defined as a roll angle φ, the rotation angle around the y-axis is defined as a pitch angle θ, and the rotation angle around the z-axis is defined as a yaw angle ψ. The roll angle φ, the pitch angle θ, and the yaw angle ψ indicate the attitude of the underwater vehicle 1. That is, in the underwater vehicle 1, attitude control is performed by an operating means (for example, a rudder) to be described later, using the roll angle φ, pitch angle θ, and yaw angle ψ related to the attitude of the aircraft body as control objects (control variables). Note that by controlling the pitch angle θ as the posture, the depth Z (z-axis direction) can be changed, so the pitch angle θ and the depth Z are correlated with each other.

FIG. 1 is a diagram illustrating an underwater vehicle 1 including a control system according to a first embodiment of the present disclosure. As shown in FIG. 1, the underwater vehicle 1 has a stern axis (hereinafter referred to as the "x-axis"), a left-right axis (hereinafter referred to as the "y-axis"), and an up-down axis (hereinafter referred to as the "z-axis"). Six axes (six degrees of freedom) of the hull coordinate system are defined, each consisting of three linear axes perpendicular to each other and three rotational axes relative to each of these linear axes. Then, let the speed in the x-axis direction be speed u, the speed in the y-axis direction be speed v, and the speed in the z-axis direction be speed w. Furthermore, let the rotational angular velocity around the x-axis be the angular velocity p, the rotational angular velocity around the y-axis be the angular velocity q, and the rotational angular velocity around the z-axis be the angular velocity r. That is, the velocity u, velocity v, velocity w, angular velocity p, angular velocity q, and angular velocity r are state quantities of the aircraft.

The state quantity of the underwater vehicle 1 is a value indicating the state of the underwater vehicle 1. In this embodiment, velocity u, velocity v, velocity w, angular velocity p, angular velocity q, and angular velocity r are used as state quantities. Furthermore, roll angle φ, pitch angle θ, yaw angle ψ, and depth Z are used as state quantities. Each state quantity represents the state (attitude, etc.) of the underwater vehicle 1, and is detected by a sensor provided in the underwater vehicle 1. Each detected state quantity is used in the control device 20 described later. The sensor is, for example, an inertial navigation device provided within the aircraft. By providing each sensor inside the fuselage, it is possible to suppress the occurrence of underwater pressure resistance, increase in fluid resistance, etc. Note that the method for acquiring each state quantity is not limited to using a sensor, and other methods such as estimating and acquiring using a simulation model etc. can also be used. As long as the state quantities of the aircraft are acquired, the method for identifying each state quantity (calculation method) is not limited.

The control device 20 controls the roll angle φ, the pitch angle θ, and the yaw angle ψ of the underwater vehicle 1 by controlling a plurality of operation means provided on the aircraft body (attitude control). The operating means is means for controlling the attitude of the underwater vehicle 1, and specifically is a rudder. The operating means is not limited to the rudder as long as it can control the attitude of the aircraft and is driven using an actuator. For example, a thruster or the like may be used. In this embodiment, the operating means uses four rudders (X rudders) R, which are the first rudder R1, the second rudder R2, the third rudder R3, and the fourth rudder R4, as shown in FIG. I will explain about it. Note that FIG. 2 is a view seen from the stern, and also shows the propeller 2 for obtaining propulsive force. A force in the x-axis direction is obtained by the propulsive force from the propeller 2. The control device 20 controls each rudder in the X rudder to control the roll angle φ, pitch angle θ, and yaw angle ψ, which are control targets related to the attitude of the aircraft. In this way, the number of operating means (four in this embodiment) is greater than the number of objects to be controlled regarding the attitude of the aircraft (three in this embodiment). The medium navigation vehicle 1 becomes a redundant system. The object to be controlled may be controlled by four or more operating means provided on the aircraft body as three elements of the aircraft's pitch angle θ, yaw angle ψ, and roll angle φ as the attitude. Any structure in which the objects to be controlled regarding the attitude of the aircraft body are controlled by a plurality of operating means greater than the number of objects to be controlled can be similarly applied as a redundant system.

Specifically, the control device 20 issues control command values to the first rudder actuator, the second rudder actuator, the third rudder actuator, and the fourth rudder actuator provided in the underwater vehicle 1. and control the aircraft's attitude. By driving each actuator, the angle of each rudder (rudder angle) is changed to control the attitude of the aircraft.

FIG. 3 is a diagram showing an example of the hardware configuration of the control device 20 according to the present embodiment.
As shown in FIG. 3, the control device 20 is a computer system, and includes, for example, a CPU 11, a ROM (Read Only Memory) 12 for storing programs, etc. executed by the CPU 11, and a ROM (Read Only Memory) 12 for storing programs executed by the CPU 11. A RAM (Random Access Memory) 13 that functions as a work area, a hard disk drive (HDD) 14 as a mass storage device, and a communication section 15 for connecting to a network or the like. A solid state drive (SSD) may be used as the mass storage device. These parts are connected via a bus 18.

Further, the control device 20 may include an input unit such as a keyboard and a mouse, and a display unit such as a liquid crystal display device that displays data.

Note that the storage medium for storing programs and the like executed by the CPU 11 is not limited to the ROM 12. For example, other auxiliary storage devices such as a magnetic disk, a magneto-optical disk, and a semiconductor memory may be used.

A series of processing steps for realizing various functions described below are recorded in the form of a program in the hard disk drive 14, etc., and the CPU 11 reads this program into the RAM 13 etc. to process information and perform arithmetic processing. As a result, various functions described below are realized. Note that the program may be pre-installed in the ROM 12 or other storage medium, provided as being stored in a computer-readable storage medium, or distributed via wired or wireless communication means. etc. may also be applied. Computer-readable storage media include magnetic disks, magneto-optical disks, CD-ROMs, DVD-ROMs, semiconductor memories, and the like.

FIG. 4 is a functional block diagram showing the functions of the control device (control system) 20. As shown in FIG. 4, the control device 20 includes a steering angle command calculation section 30, a fault diagnosis section 40, a regression command section 50, and a fault-tolerant control section 60.

(Configuration of steering angle command calculation unit)
The rudder angle command calculation unit 30 calculates command values for each rudder in the X rudder based on the target value. For this reason, the steering angle command calculation section 30 includes a virtual steering angle calculation section 31 and a distribution section 32. The target value is a target value for the control amount regarding the attitude of the aircraft. The controlled quantity is a quantity that is desired to be controlled to a target value among the quantities belonging to the controlled object. The target values correspond to each of the roll angle φ, pitch angle θ, and yaw angle ψ. Since the pitch angle θ is related to the depth Z of the aircraft, the virtual steering angle calculation unit 31 according to the present embodiment uses the target value of the depth Z to calculate the target value of the pitch angle θ by the depth controller 33. Let me explain the case. However, since the control amount related to the attitude of the aircraft can be used as the target value, the target values corresponding to the roll angle φ, pitch angle θ, and yaw angle ψ are input to the virtual steering angle calculation unit 31. Good too. If the rudder angle command calculation unit 30 can calculate the command value corresponding to each rudder based on the target values (roll angle φ, pitch angle θ, and yaw angle ψ) for the control amount regarding the attitude of the aircraft, The configuration is not limited to 4.

The virtual rudder angle calculation unit 31 calculates a command value corresponding to a virtual rudder (horizontal rudder, vertical rudder, roll rudder) based on a target value of a control amount related to the attitude of the aircraft body. The target values are a depth target value Z*, a yaw angle target value ψ*, and a roll angle target value φ*. Since the depth target value Z* is used to calculate the pitch angle target value θ*, the virtual rudder command value is the roll angle target value φ*, the pitch angle target value θ*, and the yaw angle target value ψ *It will be calculated based on. The virtual rudder is a rudder set virtually, and specifically includes a horizontal rudder, a vertical rudder, and a roll rudder.

The difference between each target value and the state quantity corresponding to each target value is input to the virtual steering angle calculation unit 31. Specifically, the difference between the depth target value Z* and the state quantity of depth, the difference between the yaw angle target value ψ* and the state quantity of yaw angle ψ, and the difference between the roll angle target value φ* and the state quantity of roll angle φ. The difference between the two is input to the virtual steering angle calculating section 31. Note that the state quantity of the pitch angle θ is also input to the virtual steering angle calculation unit 31.

The virtual steering angle calculation unit 31 includes a depth controller 33 , an attitude angle controller 36 , an azimuth controller 34 , and a roll controller 35 . The depth controller 33 calculates the pitch angle target value θ* based on the difference between the depth target value Z* and the depth state quantity. Then, the difference between the pitch angle target value θ* and the state quantity of the pitch angle θ is input to the attitude angle controller 36. The attitude angle controller 36 calculates a side rudder command value based on the difference between the pitch angle target value θ* and the state quantity of the pitch angle θ. That is, the command value for the horizontal rudder, which is the virtual rudder, is calculated based on the pitch angle target value θ*.

The azimuth controller 34 calculates a vertical rudder command value based on the difference between the yaw angle target value ψ* and the state quantity of the yaw angle ψ. That is, the command value for the vertical rudder, which is the virtual rudder, is calculated based on the yaw angle target value ψ*.

The roll controller 35 calculates a roll rudder command value based on the difference between the roll angle target value φ* and the state quantity of the roll angle φ. That is, the command value for the roll rudder, which is the virtual rudder, is calculated based on the roll angle target value φ*.

Once the command values for the horizontal rudder, longitudinal rudder, and roll rudder are calculated, they are output to the distribution section 32. An invalidation unit 37 is provided between the virtual steering angle calculation unit 31 and the distribution unit 32. The invalidation unit 37 is controlled by a degeneration command unit 50, which will be described later. Specifically, when there is no degeneration command from the degeneration command section 50, the invalidation section 37 outputs the output value from the virtual steering angle calculation section 31 as it is to the distribution section 32. Then, when there is a command for degeneration from the degeneration command unit 50, the invalidation unit 37 invalidates the output value (specific output) from the virtual steering angle calculation unit 31. In this embodiment, since a low priority is set for the roll angle φ, as a specific configuration, an invalidation unit 37 is provided on the output side of the roll controller 35. That is, when there is a command for degeneration from a degeneration command unit 50, which will be described later, the output of the roll controller 35 is invalidated, and 0 is input to the distribution unit 32 as a command value for the roll rudder. Note that, depending on the priority order regarding degeneracy, it is also possible to adopt a configuration other than the case where the invalidation section 37 is provided on the output side of the roll controller 35.

The distribution unit 32 distributes the X rudders, which are actual rudders (rudder 1 R1, rudder 2 R2, rudder 3 R3, and The command value to the rudder R4) is calculated. For example, the command value for the X rudder is calculated by numerically converting (for example, multiplying by a constant) each command value for the horizontal rudder, vertical rudder, and roll rudder, and adding or subtracting the numerically converted values. For example, the command value for the first rudder R1 is calculated by adding or subtracting a value based on the horizontal rudder command value, a value based on the longitudinal rudder command value, and a value based on the roll rudder command value. Note that the method for calculating the command value of the X rudder based on the virtual rudder is not limited to the above.

The command value to the X rudder calculated by the distribution unit 32 is output to the actuator of each rudder in the X rudder, and the rudder is controlled.

(Configuration of failure diagnosis section)
The failure diagnosis section 40 performs failure diagnosis on the X rudder. That is, the failure diagnosis unit 40 identifies a failed rudder that is a rudder in which a failure has occurred. As shown in FIG. 5, the failure diagnosis unit 40 includes an estimation unit 41, a determination unit 42, and a calculation unit (failure operation amount calculation unit) 45. The speed u, speed v, speed w, angular speed p, angular speed q, angular speed r, pitch angle θ, and roll angle φ are input to the fault diagnosis unit 40 as state quantities of the underwater vehicle 1. The state quantity is the current value (current value) of a predetermined parameter in the underwater vehicle 1. Note that the state quantities used by the failure diagnosis section 40 are not limited to those described above, and are set according to the configuration of the observer described later. For example, as the state quantity, at least one of speed u, speed v, speed w, angular speed p, angular speed q, angular speed r, pitch angle θ, yaw angle ψ, and roll angle φ can be used. be. The state quantity of the aircraft body is measured by, for example, a sensor provided in the aircraft body, and is output to the acquisition unit 61.

The estimating unit 41 performs an estimation process on each operating means to estimate the amount of operation for another of the operating means based on the state quantity of the structure and the command value of one of the operating means. Do it accordingly. Specifically, the estimating unit 41 uses the state quantity of the underwater vehicle 1 and the command value for one of the X rudders (for example, the first rudder R1) to set the other rudder (for example, the second rudder R1). The amount of operation at the rudder R2, rudder 3 R3, and rudder 4 R4 is estimated (estimation process). The amount of operation is the angle of the rudder (rudder angle) when driven based on the command value. Then, the estimation process is similarly performed for other rudders.

As shown in FIG. 5, the estimation unit 41 performs estimation processing using an observer. Observers are provided corresponding to each rudder. In FIG. 5, only the configuration of the observer OS1 is specifically shown, and the configurations of the other observers (observer OS2, observer OS3, and observer OS4) are schematically shown, but they have the same configuration as the observer OS1.

When the state (state variable) is x and the input is the operation amount for each rudder, the linear approximation model (state equation) becomes the following equation (1). It is assumed that the amount of operation for the first rudder R1 is δr1, the amount of operation for the second rudder R2 is δr2, the amount of operation for the third rudder R3 is δr3, and the amount of operation for the fourth rudder R4 is δr4.

In equation (1), A is a state matrix (A matrix), and B (a matrix whose elements are b1, b2, b3, and b4) is an input matrix (B matrix). The B matrix is an influence coefficient of the rudder, and is also called a lift coefficient or a sensitivity coefficient. Equation (1) shows the relationship between the amount of operation for each rudder and the state variable. The state variables are a matrix of state quantities (velocity u, velocity v, velocity w, angular velocity p, angular velocity q, angular velocity r, pitch angle θ, roll angle φ) that are input to the estimation unit 41. In equation (1), for example, if the first rudder R1 is assumed to be normal, then the B matrix for the second rudder R2, third rudder R3, and fourth rudder R4 is used to convert equation (1) into the following equation ( 2).

Equation (2) can be transformed into Equation (3) using a pseudo-inverse matrix.

That is, assuming that one rudder (the first rudder R1 in Equation (3)) is normal (assuming that the amount of operation and the command value are equal), the amount of operation of the other rudders is calculated by using each state quantity. be able to. Note that each element of the A matrix and the B matrix is set in advance based on the characteristics of the underwater vehicle 1.

The estimation unit 41 estimates the amount of operation of the rudder using an observer using the calculation principle of equation (3). Specifically, as shown in FIG. 5, in the observer OS1, the state quantity is multiplied by the A matrix, and the state quantity is multiplied by s. Multiplying the state quantity by s corresponds to differentiating the state quantity in the time domain. Then, assuming that no failure has occurred in the first rudder R1 (that is, assuming that the operation amount and the command value are equal), the command value of the first rudder R1 is set to the first rudder R1 in the B matrix. Multiply by the corresponding component (b1). Then, a value obtained by subtracting the value obtained by multiplying the state quantity x by the A matrix and the value obtained by multiplying the command value of the first rudder R1 by b1 is calculated from the value obtained by differentiating the state quantity x. The calculated value corresponds to the left side of equation (2). Then, the right side of equation (3) is calculated by multiplying the pseudo inverse matrix B ⁺ ₂₃₄ of the B matrix component corresponding to the second rudder R2, the third rudder R3, and the fourth rudder R4, and the second rudder R2 , the estimated values of the rudder angles of the third rudder R3 and the fourth rudder R4 are calculated.

Similar calculations are performed for other observers. The component (b1) of the B matrix in the observer OS1 corresponding to the first rudder R1, the pseudo inverse matrix of the B matrix component corresponding to the second rudder R2, the third rudder R3, and the fourth rudder R4 are Each is set accordingly.

The estimation unit 41 performs the estimation process corresponding to the first rudder R1 as described above for the other rudders as well. That is, by performing estimation processing assuming that the first rudder R1 is normal, estimated values of the rudder angles of the second rudder R2, the third rudder R3, and the fourth rudder R4 are calculated (observer OS1). By performing estimation processing assuming that the second rudder R2 is normal, estimated values of the rudder angles of the first rudder R1, the third rudder R3, and the fourth rudder R4 are calculated (observer OS2). By performing estimation processing assuming that the third rudder R3 is normal, estimated values of the rudder angles of the first rudder R1, the second rudder R2, and the fourth rudder R4 are calculated (observer OS3). By performing estimation processing assuming that the fourth rudder R4 is normal, estimated values of the rudder angles of the first rudder R1, the second rudder R2, and the third rudder R3 are calculated (observer OS4). In this way, each estimation process calculates three estimated values of the rudder angle corresponding to each rudder (12 in total).

The estimated value of each steering angle calculated in this way is output to the determination unit 42.

The determination unit 42 performs a failure determination for each operating means based on the estimated operation amount corresponding to each operating means. Specifically, the determination unit 42 performs failure determination based on the estimated value of the rudder angle corresponding to each rudder. For this reason, as shown in FIG. 5, the determination section 42 includes a failure candidate identification section 43 and a majority decision section 44. Note that in this embodiment, a case will be described where fault diagnosis is performed based on each estimated value estimated by the estimating unit 41 using majority logic; however, the fault diagnosis is performed based on each estimated value estimated by the estimating unit 41. If possible, it is not limited to majority logic.

The failure candidate identification unit 43 identifies failure rudder candidates based on a plurality of rudder angles (operation amounts) estimated for the same rudder. Specifically, the failure candidate identification unit 43 identifies the operation amount with the largest deviation among the operation amounts estimated for the operation means as the abnormal operation amount, and performs a failure determination based on the identified abnormal operation amount. I do. Therefore, the failure candidate identifying section 43 includes a first identifying section S1 corresponding to the first rudder R1, a second identifying section S2 corresponding to the second rudder R2, and a third identifying section corresponding to the third rudder R3. S3, and a fourth identifying section S4 corresponding to the fourth rudder R4. That is, each rudder angle estimated for the first rudder R1 is input to the first specifying section S1, and each rudder angle estimated for the second rudder R2 is input to the second specifying section S2. Each rudder angle estimated for No. 3 rudder R3 is input to the third specifying section S3, and each rudder angle estimated for No. 4 rudder R4 is input to the fourth specifying section S4. be done.

Specifically, the first identification unit S1 identifies the rudder angle with the largest deviation among the rudder angles estimated for the first rudder R1 as an abnormal rudder angle (a rudder angle estimated to be abnormal). . If the deviation is the largest compared to the others, it is assumed that there was an error in the estimation process, so the rudder, which was assumed to be normal in the estimation process of the rudder angle identified as the abnormal rudder angle, is considered a candidate for the faulty rudder. do. That is, the first identification unit S1 identifies any one of the second rudder R2, the third rudder R3, and the fourth rudder R4 as the faulty rudder. The failure flag of the rudder identified as a failed rudder is set to 1, and the failure flag of a rudder not identified as a failed rudder is set to 0.

The first identifying section S1 outputs a failure flag F21 corresponding to the second rudder R2, a failure flag F31 corresponding to the third rudder R3, and a failure flag F41 corresponding to the fourth rudder R4. The second identifying section S2 outputs a failure flag F12 corresponding to the first rudder R1, a failure flag F32 corresponding to the third rudder R3, and a failure flag F42 corresponding to the fourth rudder R4. The third identification unit S3 outputs a failure flag F13 corresponding to the first rudder R1, a failure flag F23 corresponding to the second rudder R2, and a failure flag F43 corresponding to the fourth rudder R4. The fourth identification unit S4 outputs a failure flag F14 corresponding to the first rudder R1, a failure flag F24 corresponding to the second rudder R2, and a failure flag F34 corresponding to the third rudder R3. Each identification unit outputs a failure flag corresponding to a rudder that is a candidate for a failed rudder as 1. For example, when the second rudder R2 is identified as a failed rudder in the first identification unit S1, the failure flag F21 is output as 1, the failure flag F31 is 0, and the failure flag F31 is output as 0.

FIG. 6 is a flowchart showing an example of a failure rudder candidate identification process in the failure candidate identification unit 43. For example, the flow in FIG. 6 is started after each steering angle is estimated. Regarding FIG. 6, a case is illustrated in which a faulty rudder is estimated based on each rudder angle estimated for the first rudder R1 (first identification unit S1). For other rudders, the faulty rudder candidate identification process can be performed by the same process as in FIG. 6 . In the flow of FIG. 6, the rudder angle of the first rudder R1 estimated by the estimation process assuming that the second rudder R2 is normal is set as dr12, and the rudder angle of the first rudder R1 estimated by the estimation process assuming the third rudder R3 is normal. The explanation will be given assuming that the rudder angle of R1 is dr13 and the rudder angle of the first rudder R1 estimated by the estimation process assuming that the fourth rudder R4 is normal is dr14.

The failure candidate identification unit 43 acquires each of the estimated rudder angles (dr12, dr13, dr14) regarding the first rudder R1 (S101), and calculates the average value of each rudder angle (S102). Then, the deviation (absolute value) from the average value is calculated for each steering angle (S103). It is determined whether the deviation corresponding to each steering angle is less than a threshold value (S104). The threshold value is set based on estimated variations in each steering angle, etc., and is set as an upper limit value at which each deviation can be considered to be equal. In S104, it is more preferable to determine whether the deviations corresponding to each steering angle are all equal (that is, all are 0). If the deviation corresponding to each rudder angle is less than the threshold (YES determination in S104), each rudder angle is determined not to be abnormal, and the failure flag of each rudder is output as 0 (S105).

If the deviation corresponding to each steering angle is not less than the threshold value (NO determination in S104), it is determined whether the deviation of dr12 is the maximum (S106). When the deviation of dr12 is the maximum (YES determination in S106), the failure flag of the second rudder R2 is set to 1 (S107). If the deviation of dr12 is not the maximum (NO determination in S106), it is determined whether the deviation of dr13 is the maximum (S108). When the deviation of dr13 is the maximum (YES determination in S108), the failure flag of the third rudder R3 is set to 1 (S109). If the deviation of dr13 is not the maximum (NO determination in S108), the deviation of dr14 is the maximum, so the failure flag of the fourth rudder R4 is set to 1 (S110). Note that the failure flag that is not set to 1 becomes 0.

In this manner, the first identification unit S1 identifies the faulty rudder based on each rudder angle estimated for the first rudder R1, and sets the fault flag to 1. The second specifying section S2, the third specifying section S3, and the fourth specifying section S4 perform the same process, and a failure flag indicating that a failed rudder is specified is set. Specifically, the second identification unit S2 identifies the faulty rudder based on each rudder angle estimated for the second rudder R2, and sets the fault flag to 1. The third identification unit S3 identifies the faulty rudder based on each rudder angle estimated for the third rudder R3, and sets the fault flag to 1. The fourth identification unit S4 identifies the faulty rudder based on each rudder angle estimated for the fourth rudder R4, and sets the fault flag to 1.

By performing the processing in the failure candidate identification unit 43 in this way, the failure flags (F21, F31, F41) are sent from the first identification unit S1, the failure flags (F12, F32, F42) are sent from the second identification unit S2, and the third The identification unit S3 outputs failure flags (F13, F23, F43), and the fourth identification unit S4 outputs failure flags (F14, F24, F34).

That is, the failure candidate identification unit 43 outputs the failure flags (F12, F13, F14) corresponding to the first rudder R1, the failure flags (F21, F23, F24) corresponding to the second rudder R2, and the failure flags corresponding to the third rudder R3. The failure flags (F31, F32, F34) corresponding to the fourth rudder R4 and the failure flags (F41, F42, F43) corresponding to the fourth rudder R4 are output. Each flag is output to the majority decision section 44.

In the majority decision unit 44, after the abnormal operation amount is specified corresponding to each operation means, if the operation means corresponding to the command values used for estimating each specified abnormal operation amount are equal, each abnormal operation amount is determined. It is determined that a failure has occurred in the corresponding operating means. In other words, the majority decision unit 44 determines that an abnormality has occurred in the failed rudders when the identified failed rudders are equal after the failed rudder candidates are identified corresponding to each rudder in the failed candidate identifying unit 43. It is estimated that In this way, after the abnormal operation amount is specified corresponding to each operation means, the operation means that is assumed to be normal in estimating each abnormal operation amount is specified, and when each of the identified operation means is equal, the operation It is possible to diagnose that an abnormality has occurred in the means.

For example, the first identifying section S1 identifies the second rudder as the failed rudder, the third identifying section S3 identifies the second rudder as the failed rudder, and the fourth identifying section S4 identifies the second rudder as the failed rudder. In this case, it is determined that a failure has occurred in the second rudder. For this reason, the majority decision section 44 includes a first AND section A1, a second AND section A2, a third AND section A3, and a fourth AND section A4. The failure flag (F12, F13, F14) corresponding to the first rudder R1 is input to the first AND section A1, and the failure flag (F21, F23, F24) corresponding to the second rudder R2 is input to the second AND section A2. The failure flag (F31, F32, F34) corresponding to the third rudder R3 is input to the third AND section A3, and the failure flag (F41, F42, F43) corresponding to the fourth rudder R4 is input to the fourth AND section A4. is input.

The first AND unit A1 receives the failure flags (F12, F13, F14) corresponding to the first rudder R1, and determines that a failure has occurred in the first rudder R1 when all the failure flags are 1. do. That is, the first AND unit A1 outputs the first rudder failure flag F(R1) as 1 when F12, F13, and F14 are all 1.

The second AND unit A2 receives the failure flags (F21, F23, F24) corresponding to the second rudder R2, and determines that a failure has occurred in the second rudder R2 when all the failure flags are 1. do. That is, the second AND unit A2 outputs the second rudder failure flag F(R2) as 1 when F21, F23, and F24 are all 1.

The third AND unit A3 determines that a failure has occurred in the third rudder R3 when the failure flags (F31, F32, F34) corresponding to the third rudder R3 are input and all the failure flags are 1. do. That is, the third AND unit A3 outputs the third rudder failure flag F(R3) as 1 when F31, F32, and F34 are all 1.

The fourth AND unit A4 receives the failure flags (F41, F42, F43) corresponding to the fourth rudder R4, and determines that a failure has occurred in the fourth rudder R4 when all failure flags are 1. do. That is, the fourth AND unit A4 outputs the fourth rudder failure flag F(R4) as 1 when F41, F42, and F43 are all 1.

In each of the first identifying section S1, second identifying section S2, third identifying section S3, and fourth identifying section S4 in the failure candidate identifying section 43, a failure flag corresponding to one rudder is set to 1. Therefore, the majority decision unit 44 selects one of the first rudder failure flag F (R1), the second rudder failure flag F (R2), the third rudder failure flag F (R3), and the fourth rudder failure flag F (R4). One of the flags becomes 1 (or all flags may become 0). Therefore, a failure in one of the X rudders can be identified.

In the above example, the malfunctioning rudder is identified in the majority decision unit 44, but OR processing (logical sum processing) may be performed instead of AND processing (logical product processing). Alternatively, a faulty rudder may be identified by analyzing the information output from the failure candidate identification unit 43. For analysis, statistical methods may be used, or machine learning such as AI may be used.

If each deviation corresponding to each operation amount estimated for the operating means is less than a threshold value, it may be determined that a failure has occurred in the operating means. Specifically, a case where each deviation of the estimated rudder angle corresponding to a certain rudder is less than a threshold value means a case where no failure has occurred in each rudder, or a case where a failure has occurred in the rudder. Either. That is, assuming that an abnormality has occurred in the X rudder, if each deviation of the estimated rudder angle corresponding to a specific rudder is less than a threshold, it is determined that a failure has occurred in the specific rudder. It can also be assumed that there are. In this case, it can be estimated that a failure has occurred in one of the rudders due to a cause such as deviation from the target trajectory.

The calculation unit (failure operation amount calculation unit) 45 calculates a failure operation amount, which is the operation amount of the failed operation means, based on each operation amount estimated for the operation means. Specifically, the calculation unit 45 estimates the faulty rudder angle based on each rudder angle estimated for the rudder identified as the faulty rudder. For example, the faulty steering angle is calculated by calculating an average value for each steering angle. Note that the calculation method is not limited to the average value, but may be a median value or the like.

FIG. 7 is a flowchart showing an example of a failure steering angle calculation process in the calculation unit 45. For example, the flow of FIG. 7 is initiated after a faulty rudder is identified. In addition, in FIG. 7, the case where the number 1 rudder R1 is specified as a faulty rudder is taken as an example. The estimated rudder angles (dr12, dr13, dr14) for the rudder identified as the failed rudder are acquired (S201), and the average value of each rudder angle is calculated (S202). Then, the calculated average value is output as the rudder angle of the failed rudder (S203). Note that since the average value of the steering angle is also calculated in S102, the average value calculated in S102 may be used.

When calculating the average value, the calculation unit 45 calculates the faulty manipulated variable by excluding the manipulated variable estimated based on the command value of the operating means determined to be malfunctioning. In the flow of FIG. 7, when a failure occurs in the first rudder R1, the rudder angle is estimated by the observer assuming that the normal rudders (the second rudder R2, the third rudder R3, and the fourth rudder R4) are normal. Since the faulty rudder angle of the first rudder R1 is calculated based on (dr12, dr13, dr14), the faulty rudder angle can be calculated more accurately.

(Effects of faulty rudder identification processing by fault diagnosis section)
Next, the effect of the faulty rudder identification process by the above-mentioned fault diagnosis section 40 will be explained with reference to FIG. 11. FIG. 11 illustrates a case where the steering angle is estimated when a sticking failure occurs in the fourth rudder R4. Therefore, in FIG. 11, the vertical axis is the angle (steering angle), and the horizontal axis is the time. Assume that a failure occurs in the fourth rudder R4 at time T1. In this case, the command value for the fourth rudder R4 is not reflected in the rudder angle, and the rudder angle of the fourth rudder R4 is fixed at a specific rudder angle. When a failure occurs, the failure diagnosis section 40 executes failure rudder identification processing. By the faulty rudder identification process, the estimated rudder angle converges to the faulty rudder angle more quickly, as shown in FIG. 11. A more accurate faulty steering angle is then estimated.

(Configuration of degeneration command unit)
The degeneration command unit 50 outputs a command to the invalidation unit 37 in the steering angle command calculation unit 30 to degenerate the control system in the steering angle command calculation unit 30. Specifically, when the number of operating means that can be normally controlled is greater than or equal to the number of control variables, the degeneration command unit 50 controls the control for each operating means based on the target value corresponding to each controlled variable. If a command value is calculated and the number of operating means that can be normally controlled is less than the number of control variables, the number of control variables used to calculate each command value is reduced, and at least one of each target value is A command value for each operating means is calculated based on one. Note that the results of the failure diagnosis section 40 are used for the number of failed rudders.

In the underwater vehicle 1 with redundancy, if a failure occurs in a rudder, and the number of normal controllable rudders is smaller than the number of control variables (objects of control related to attitude) of the aircraft, It becomes difficult to perform control corresponding to each target value using only a normal rudder. For example, if the control variables are pitch angle θ, yaw angle ψ, and roll angle φ, and two of the four rudders on the aircraft fail (there are two normal rudders), two rudders will adjust the pitch angle. It is difficult to control θ, yaw angle ψ, and roll angle φ. If controlled, each control may interfere with each other, making it impossible to perform the desired control. Therefore, the degeneration command unit 50 degenerates the control system depending on the number of rudder failures.

Degeneration means degenerating the control system in the steering angle command calculation unit 30. Specifically, the steering angle command calculation unit 30 executes a control system corresponding to each target value (a control system corresponding to depth (pitch angle θ), a control system corresponding to yaw angle ψ, a control system corresponding to roll angle φ). system), degeneracy is performed by reducing the number of controlled variables (number of dimensions) of the control system. For example, the command value is calculated using only a control system corresponding to depth (pitch angle θ) and a control system corresponding to yaw angle ψ, without using a control system corresponding to roll angle φ. In other words, the number of target values used to calculate the command value for the X rudder is reduced, and the command value for the X rudder is calculated based on at least one of the target values.

Regarding degeneration, degeneration is performed based on a preset priority order for each control amount. In this embodiment, a case will be described in which the roll angle φ has a low priority. In other words, the control amount is the pitch angle θ, yaw angle ψ, and roll angle φ of the aircraft as the attitude, and when the number of normally controllable rudders is two, the pitch angle θ and the yaw angle ψ are A command value for each rudder is calculated based on the target value. In this way, degeneration is performed so that the number of normally controllable rudders and the number of target values used are equal.

For this reason, as shown in FIG. 4, a nullifying section 37 is provided on the output side of the roll controller 35 (input side of the distribution section 32). By preferentially controlling the pitch angle θ and the yaw angle ψ, it becomes possible to continue controlling the traveling direction. Therefore, even if a failure occurs in the rudder, it is possible to continue navigating toward the target trajectory. The method of invalidation is not limited to the above, as long as it does not reflect the target value of the controlled object with a low priority in the calculation of the command value of the X rudder. For example, a method such as blocking input of the target value may be used. The control objects (priorities) to be invalidated can also be changed as appropriate depending on the purpose of operation, etc.

When the number of normal rudders is equal to or greater than the number of control variables, the degeneration command unit 50 causes the rudder angle command calculation unit 30 to calculate a command value for each rudder based on the target value corresponding to each controlled object. Have it calculated. The case where the number of normal rudders is greater than the number of controlled objects is, for example, the case where three or more rudders are normal with respect to the control variables pitch angle θ, yaw angle ψ, and roll angle φ. . When the number of normal rudders is greater than or equal to the number of control amounts, the degeneration command unit 50 causes the invalidation unit 37 to output the output of the roll controller 35 to the distribution unit 32 as is. That is, when the number of normal rudders is equal to or greater than the number of control variables, normal control is performed without degenerating the control system.

If the number of operating means that can be normally controlled is less than the number of control variables, the degeneration command unit 50 causes the steering angle command calculation unit 30 to degenerate the number of control variables and set at least one of each target value. A command value for each rudder is calculated based on one. The case where the number of normal rudders is less than the number of control variables is, for example, the case where two or less rudders are normal with respect to the control variables pitch angle θ, yaw angle ψ, and roll angle φ. . When the number of normal rudders is less than the number of control amounts, the degeneration command unit 50 does not cause the invalidation unit 37 to output the output of the roll controller 35 to the distribution unit 32 (invalidation), and outputs the output of the roll controller 35 to 0 (zero). ) is output to the distribution unit 32 as a roll rudder command value. That is, if the number of normal rudders is less than the number of control variables, the control system is degenerated to perform control. Degeneration may be performed so that the number of rudders that can be normally controlled is equal to the number of target values used, or the number of target values used may be made smaller than the number of rudders that can be normally controlled. You can also use it as

When degeneration is performed, the horizontal rudder command value, the vertical rudder command value, and the roll rudder command value (zero) are input to the distribution unit 32, so the horizontal rudder command value and the vertical rudder command value are A command value for each rudder is calculated based on the command value. In this embodiment, since the distribution unit 32 adds and subtracts the command values of each virtual rudder, invalidation can be performed by setting the output of the control system to be degenerated to 0.

By performing the degeneration in this manner, the roll angle target value φ* is no longer reflected in the calculation of the command value for the X rudder. Therefore, it becomes possible to invalidate the control on the roll angle φ and perform control so that the pitch angle θ and the yaw angle φ follow the target values. In other words, it becomes possible to continue navigating to the target orbit.

In this embodiment, a case has been described in which the priority order for roll angle φ is set low, but the priority order for degeneration can be set as appropriate depending on the purpose of operation (operation priority). For example, the pitch angle θ may be given a lower priority. Specifically, the control amount is the pitch angle θ, yaw angle ψ, and roll angle φ of the aircraft as the attitude, and if the number of operating means that can be normally controlled is two, the yaw angle ψ and A command value for each operating means is calculated based on the target value of the roll angle φ. In this case, although the control for the pitch angle θ is disabled, the yaw angle ψ and roll angle φ can be controlled, so it is possible to keep the aircraft in place without moving it as much as possible. can.

FIG. 8 is a flowchart showing an example of the degeneration process in the degeneration command unit 50. For example, the flow in FIG. 8 is repeatedly executed at a predetermined control cycle.

First, it is determined whether a failure has occurred (S301). S301 is performed according to the diagnosis result by the failure diagnosis section 40. If no failure has occurred (NO determination in S301), the process ends without degeneracy. Since the underwater vehicle 1 is a redundant system, the NO determination in S301 indicates that the number of normal rudders is greater than or equal to the number of control objects, and the rudder angle command calculation unit 30 calculates the number of normal rudders for each control object. A command value for each rudder is calculated based on the corresponding target value.

If a failure occurs (YES determination in S301), it is determined whether the number of normally controllable rudders is less than the number of control variables related to the attitude of the aircraft (S302). If the number of normally controllable rudders is not less than the number of control variables related to the attitude of the aircraft (NO determination in S302), the number of normal rudders is greater than or equal to the number of control variables, so degeneration is performed. The process ends without any changes. In this case, the rudder angle command calculation unit 30 calculates the command value for each rudder based on the target value corresponding to each control amount.

If the number of normally controllable rudders is less than the number of control variables related to the attitude of the aircraft (YES determination in S302), degeneration is performed (S303). Specifically, in order to invalidate control corresponding to a control amount with a low priority (for example, roll angle φ), an invalidation instruction is given to the invalidation unit 37. Therefore, in the rudder angle command calculation unit 30, the number of control variables is reduced, and a command value for each rudder is calculated based on at least one of each target value. Note that the degeneration is not limited to the case where the degeneration is performed by an instruction to the invalidation unit 37, and the specific degeneration processing in S303 differs depending on the degeneration method.

By performing degeneracy control in this way, it becomes possible to perform operations that more reliably meet the objectives.

(Configuration of fault-tolerant control unit)
The fault-tolerant control unit 60 calculates command values for each rudder so as to eliminate the influence of the faulty rudder. For example, if one of the X rudders fails and becomes stuck at a certain rudder angle, the failed rudder exerts a force on the aircraft according to the stuck rudder angle. The force exerted by such a failed rudder affects the attitude control of the aircraft. Therefore, the fault-tolerant control unit 60 controls the other rudders (rudders that operate normally) so as to suppress the influence of the faulty rudder.

As shown in FIG. 9, the fault-tolerant control section 60 includes an acquisition section 61 and a calculation section (command value calculation section) 62.

The acquisition unit 61 acquires the state quantity of the aircraft. The state quantity of the body is the current value (current value) of a predetermined parameter in the underwater vehicle 1. The acquisition unit 61 acquires state quantities for use in calculations in the fault-tolerant control unit 60. As the state quantities, for example, velocity u, velocity v, velocity w, angular velocity p, angular velocity q, angular velocity r, pitch angle θ, and roll angle φ are input. Note that the state quantities used in the fault-tolerant control unit 60 are not limited to those described above, and are set according to the configuration of the state equation described later. For example, as the state quantity, at least one of speed u, speed v, speed w, angular speed p, angular speed q, angular speed r, pitch angle θ, yaw angle ψ, and roll angle φ can be used. be.

The state quantity of the aircraft body is measured by, for example, a sensor provided in the aircraft body, and is output to the acquisition unit 61. The value of each state quantity may be estimated using a simulation model of the aircraft.

The state quantity acquired by the acquisition unit 61 is output to the calculation unit 62.

The calculation unit (command value calculation unit) 62 uses the state quantity of the aircraft as a state variable and the operation amount of the operation means as an input in a state equation, and calculates a failure based on information regarding the failed operation means, which is the operation means in which the failure has occurred. A term related to the operating means is set as a disturbance term, and the manipulated variables of the operating means other than the faulty operating means are calculated based on the acquired state quantity. The information regarding the faulty operating means is the identification information of the faulty operating means (faulty rudder identification information) and the faulty operation amount (faulty rudder angle) of the faulty operating means. The calculation unit 62 calculates the operation amount of the operation means other than the failure operation means based on the state equation so as to cancel the disturbance term. Specifically, the calculation unit 62 performs the calculation by setting the differential term of the state variable to 0 in the state equation and solving for the operation amount of the operation means other than the failure operation means. That is, in the calculation unit 62, by solving the state equation for the operation amount, the command value (operation obtained as a solution) of the normally controllable rudder (normal rudder) is calculated so as to eliminate the influence of the failed rudder. amount).

In the present embodiment, a case will be described in which the calculation unit 62 calculates a correction value for making the influence of the failed rudder non-interfering with respect to the command value for each rudder calculated by the rudder angle command calculation unit 30. In addition, in the fault-tolerant control unit 60 in this embodiment, the identification information of the faulty operating means (faulty rudder identification information), which is the output of the fault diagnosis unit 40, and the faulty rudder identification information are used as information regarding the faulty operating means, which is the operating means in which the fault has occurred. Although a case has been described in which the faulty operation amount (failure steering angle) of the operating means is used, if the failure tolerant control section 60 specifies the faulty operation means and inputs the faulty operation amount, then the output of the fault diagnosis section 40 is used. but not limited to. Moreover, although the degeneration command section 50 is provided in the control device 20 together with the fault-tolerant control section 60, only the degeneration command section 50 may be provided in the control device 20.

The state equation of the underwater vehicle 1 is expressed by the following equation (4). Note that equation (4) is a linearized state equation in the hull coordinate system.

In equation (4), Δu is the amount of change in speed u (minimal increase/decrease), Δv is the amount of change in speed v, Δw is the amount of change in speed w, Δp is the amount of change in angular velocity p, and Δq is the amount of change in angular velocity q. The amount of change, Δr, is the amount of change in the angular velocity r. a11 to a66 are each element of the A matrix corresponding to velocity u, velocity v, velocity w, angular velocity p, angular velocity q, and angular velocity r, respectively. Δφ is the amount of change in roll angle φ, and Δθ is the amount of change in pitch angle θ. a18 to a69 are each element of the A matrix corresponding to the roll angle φ and the pitch angle θ, respectively. Δn is the amount of change in the rotation speed of the propeller 2 (thruster rotation speed) for obtaining propulsive force in the aircraft, Δδr1 is the amount of change in the rudder angle of the first rudder R1, and Δδr2 is the amount of change in the rudder angle of the second rudder R2. Δδr3 is the amount of change in the rudder angle of the third rudder R3, and Δδr4 is the amount of change in the rudder angle of the fourth rudder R4. b11 to b65 are each element of the B matrix corresponding to the rotation speed, the rudder angle of the first rudder R1, the rudder angle of the second rudder R2, the rudder angle of the third rudder R3, and the rudder angle of the fourth rudder R4, respectively. It is. In equation (4), the A matrix and the B matrix are set in advance according to the characteristics of the underwater vehicle 1.

In equation (4), the rudder angles (Δδr1, Δδr2, Δδr3, Δδr4) of each rudder are variables, but if the rudder fails and the rudder angle becomes fixed, the rudder angle becomes constant (fixed value). . Specifically, when the No. 1 rudder R1 is stuck, Equation (5) is obtained by transforming the term related to the No. 1 rudder R1 into a disturbance term (constant term) in Equation (4).

In Equation (5), the rightmost term on the right side is a term indicating the force caused by the failure of the first rudder R1, and is a disturbance term. As for the disturbance term, if the faulty rudder is identified and the rudder angle of the faulty rudder is known, it is set as a fixed value. The output of the failure diagnosis section 40 is used to identify the failed rudder and to determine the rudder angle of the failed rudder (that is, information regarding the failed operating means). That is, in the disturbance term, Δδr1 becomes the sticking angle itself. In order to treat the influence of the stuck failure as a disturbance term, the difference from the equilibrium state of the rudder (δr1=0deg) is set as Δδr1.

When formula (5) is transformed so that the acceleration and diagonal terms become 0 with respect to the attitude of the aircraft (roll angle φ, pitch angle θ, yaw angle ψ), formula (6) is obtained. In equation (6), equation (5) is modified so that there is no influence of angular velocity p, angular velocity q, and angular velocity r.

In other words, Equation (6) shows the relationship between the disturbance term, the interference term of the operation amount of the normal rudders (No. 2 rudder R2, No. 3 rudder R3, and No. 4 rudder R4), and the interference term of the state amount of each control axis. It is a ceremony. By solving equation (6) for the operation amount of the normal rudder (2nd rudder R2, 3rd rudder R3, 4th rudder R4), the normal rudder (2nd rudder R2, 3rd rudder R3, 4th rudder R4) is calculated to cancel the disturbance term and the interference term between the manipulated variable and the state quantity. The operation amounts of the second rudder R2, the third rudder R3, and the fourth rudder R4 can be obtained. That is, the operation amounts (Δδr2, Δδr3, Δδr4) calculated from equation (6) are correction values for the command values for each rudder calculated by the rudder angle command calculation unit 30.

In the state equation, the state quantity includes the forward speed (velocity u) of the aircraft, and the coefficient related to the forward speed is preferably set to zero. The coefficient related to the forward speed may be influenced by information regarding the amount of operation of the operating means. In such a case, if the operation amount is calculated by taking the forward speed into account, there is a possibility that an accurate value for the operation amount will not be obtained. Therefore, by setting the coefficient related to the forward speed to 0 in the state equation, instability of calculation can be suppressed. Specifically, in equation (6), a41, a51, and a61 are set to 0.

It is preferable that at least one of the coefficients (elements of the A matrix and/or the B matrix) in the state equation be expressed as a function of a parameter related to the state quantity of the aircraft. The values of the elements of the A matrix and/or the B matrix are set according to the characteristics of the underwater vehicle 1, but linearization is set approximately. Specifically, a linear approximation is performed around a particular equilibrium point. That is, in operating conditions far from the linearly approximated equilibrium point, the approximation accuracy may decrease. Therefore, by setting the elements of the A matrix and/or B matrix as a function of parameters related to the state quantity of the aircraft, it becomes possible to appropriately set the A matrix and B matrix according to the operating state. . Note that at least one of the elements of the A matrix and/or the B matrix can be expressed as a function of a parameter related to the state quantity of the aircraft.

Therefore, the output of the fault-tolerant control unit 60 is added to the command value for each rudder calculated based on the target value on the output side of the rudder angle command calculation unit 30. In this way, the command value for the normal rudder is corrected using the correction value set to eliminate the disturbance force related to the faulty rudder, so the disturbance force is nullified by operating the normal rudder, and navigation can be continued more stably. becomes possible.

In the present embodiment, a case will be described in which the calculation unit 62 calculates a correction value for non-interfering with the influence of the failed rudder with respect to the command value for each rudder calculated by the rudder angle command calculation unit 30. However, the functions of the fault-tolerant control section 60 may be included in the steering angle command calculation section 30. That is, the command value for each rudder may be calculated so as to reduce the influence of the failed rudder while taking the target value into consideration. That is, by solving equation (5) without transforming it into equation (6), it is also possible to calculate a command value instead of a correction value.

Furthermore, in equation (5), a case where a failure occurs due to sticking is taken as an example, but if a failure occurs due to deformation, equation (7) may be used instead of equation (5).

In the deformation, although the steering angle can be manipulated, the steering angle becomes a command value with a fixed amount of deformation added. In Equation (7), the amount of deformation (the amount of offset) of the first rudder R1 is set as δ1, and the disturbance term is shown in the rightmost term on the right side.

Although the above calculation processing of the fault-tolerant control unit 60 has been described by exemplifying the case where the first rudder R1 has failed, it is possible to apply the same to other rudders. Further, the same can be applied to a case where two rudders are out of order (two rudders out of the X rudders are out of order).

FIG. 10 is a flowchart showing an example of arithmetic processing in the fault-tolerant control unit 60. For example, the flow in FIG. 10 is initiated when a failure occurs in the rudder.

First, the state quantity of the aircraft is acquired (S401). For example, the speed u, speed v, speed w, angular speed p, angular speed q, angular speed r, pitch angle θ, and roll angle φ are acquired as state quantities of the aircraft.

Next, information regarding the failed rudder is acquired (S402). For example, it is the faulty rudder identification information and its faulty rudder angle. Information regarding a faulty rudder can be used without being limited to the faulty rudder identification information and its faulty rudder angle, as long as the faulty rudder is identified and the rudder angle at the time of the fault is known.

Then, a correction value for the normal rudder is calculated (S403). Specifically, by solving a state equation in which the influence of the failed rudder is set as a disturbance, correction values for the command values of rudders other than the failed rudder are calculated so as to reduce the influence of the failed rudder. Note that S403 is not limited to calculating a correction value, but may also calculate a rudder command value that takes into account disturbances by solving a state equation.

(Effect of non-interference by fault-tolerant control unit)
Next, the effect of non-interference of the above-mentioned fault-tolerant control section 60 will be explained with reference to FIG. 12. FIG. 12 shows a movement trajectory of the underwater vehicle 1 when a sticking failure occurs in the fourth rudder R4. For this reason, FIG. 12 shows a three-dimensional orthogonal coordinate system in the absolute coordinate system. Specifically, the z-axis direction is shown as the depth Z, and the x-axis direction and the y-axis direction are shown as the position X and the position Y.

It is assumed that the underwater vehicle 1 is being operated with the straight-ahead direction as the target trajectory, and that a failure (the number four rudder R4 is stuck) occurs at a point P. Then, if non-interference control is not performed, the aircraft will deviate significantly from the target trajectory due to the influence of the stuck faulty rudder. On the other hand, when non-interference control is performed, the aircraft can immediately return to the target trajectory and continue operation.

In this way, by performing non-interference control, it becomes possible to suppress the influence of the failed rudder and quickly return to operation.

In addition, in degenerate control, even in the case of 2 failures (when the number of normally controllable rudders becomes smaller than the number of control variables related to the aircraft attitude), control for lower priority control targets is omitted. Then, control is executed for the control target with high priority. Therefore, in degenerate control, control instability can be suppressed even in the case of a more serious failure, and preferential target operation can be performed more reliably.

In other words, degeneracy control and non-interference control make it possible to continue operation more effectively in the event of a rudder failure.

As explained above, according to the control system, structure, control method, and control program according to the present embodiment, the state equation uses the state quantity of the underwater vehicle 1 as the state variable and the rudder operation amount as the input. In this case, by using the state quantity acquired as a state variable and setting the term related to the failed rudder as a disturbance term based on the information regarding the failed rudder, it is possible to calculate the operation amount of the rudder other than the failed rudder. The manipulated variable is calculated in consideration of the disturbance term related to the failed rudder in which the failure has occurred. That is, the operation amounts of rudders other than the failed rudder are calculated so as to reduce the influence of the failed rudder. For example, even if a failure (for example, sticking or deformation) occurs in a rudder, which may affect the attitude of the underwater vehicle 1, other rudders (rudders with no failure) may It becomes possible to suppress this influence. Note that the information regarding the failed rudder is, for example, information for specifying the rudder in which the failure has occurred, or the amount of operation of the rudder in which the failure has occurred (for example, the amount of operation that is stuck).

In addition, in the state equation, by calculating the operation amount of rudders other than the faulty rudder so as to cancel the disturbance term, we can calculate the operation amount of the rudder (no fault has occurred) to cancel the influence of the faulty rudder. Can be calculated. For example, by using the calculated operation amount as a correction value, it is possible to calculate the operation amount of each rudder so as to suppress the influence of the failed rudder.

Further, by making the coefficients in the state equation a function of parameters related to the state quantities of the underwater vehicle 1, the values of the coefficients can be appropriately set depending on the state of the underwater vehicle 1. Therefore, it becomes possible to perform calculations using the state formula with higher accuracy. In the state equation, the coefficient related to the forward speed of the underwater vehicle 1 was set to 0. The coefficient related to the forward speed may be influenced by information regarding the amount of operation of the rudder. In such a case, if the operation amount is calculated by taking the forward speed into account, there is a possibility that an accurate value for the operation amount will not be obtained. Therefore, by setting the coefficient related to the forward speed to 0 in the state equation, it becomes possible to perform more accurate calculations.

[Second embodiment]
Next, a control system, structure, control method, and control program according to a second embodiment of the present disclosure will be described.
In the first embodiment described above, the case where an observer due to a single failure is used is described, but in this embodiment, an observer based on multiple failures will be described. Hereinafter, the control system, structure, control method, and control program according to the present embodiment will be mainly described with respect to differences from the first embodiment.

When one faulty rudder is identified by majority logic using each observer as in the above embodiment, one faulty rudder is identified in one process. If multiple failures occur (for example, two rudders are out of order), different failed rudders will be identified in each identification process. Specifically, when the first rudder R1 and the second rudder R2 are out of order, one specific process identifies the failure in the first rudder R1, and another specific process identifies the failure in the second rudder R2. Ru. This is because the faulty rudder specified in the majority logic may differ depending on the change in the state quantity used. That is, by retaining information on the identified failed rudder, it is possible to identify the failed rudder in the case of multiple failures.

Therefore, the failure diagnosis unit 40 in this embodiment estimates the failure steering angle using an observer for two failures. As shown in FIG. 13, the failure diagnosis unit 40 further includes a multiple failure estimation unit 71 and a selection unit 72. In this embodiment, a case of two failures will be described, but the present invention can be similarly applied to cases of multiple failures.

When it is determined that a failure has occurred with respect to a plurality of operation means, the multiple failure estimation unit 71 calculates the state quantity of the structure and the operation of the operation means other than the operation means for which it has been determined that a failure has occurred. Based on the command value of the means, the amount of operation for the operating means determined to be malfunctioning is estimated. Specifically, the multiple failure estimation unit 71 estimates the failure steering angle using two failure observers.

The two-failure observer is an observer that assumes that two rudders are normal and estimates the rudder angles of the other rudders. The principle of estimating the steering angle using the two failure observers is the same as in the above embodiment. For example, a two-failure observer that estimates the rudder angles of the third rudder R3 and the fourth rudder R4 assuming that the first rudder R1 and the second rudder R2 are normal is expressed by the following equation (8).

Equation (8) is a modified form of Equation (1), assuming that the first rudder R1 and the second rudder R2 are normal. According to Equation (8), the rudder angles of the third rudder R3 and the fourth rudder R4 can be estimated assuming that the first rudder R1 and the second rudder R2 are normal (using the command values).

As shown in FIG. 13, the multiple failure estimation unit 71 includes two failure observers corresponding to two failures of each rudder. Specifically, two failure observers #12, which estimate the rudder angles of the third rudder R3 and the fourth rudder R4 assuming that the first rudder R1 and the second rudder R2 are normal, the first rudder R1 and the third rudder 2 failure observer #13 estimates the rudder angles of the first rudder R1 and the fourth rudder R4 assuming that R3 is normal, and the second rudder R2 and R3 assumes that the first rudder R1 and the fourth rudder R4 are normal. 2 failure observer #14 that estimates the rudder angle of rudder R3; 2 failure observer #14 that estimates the rudder angle of 1st rudder R1 and 4th rudder R4 assuming that 2nd rudder R2 and 3rd rudder R3 are normal; #23, 2-failure observer #24 that estimates the rudder angles of No. 1 rudder R1 and No. 3 rudder R3 assuming that No. 2 rudder R2 and No. 4 rudder R4 are normal, and No. 3 rudder R3 and No. 4 rudder R4 The second failure observer #34 is provided for estimating the rudder angles of the first rudder R1 and the second rudder R2 assuming that the rudders are normal.

That is, depending on the faulty rudder, the faulty rudder angle is estimated using two corresponding faulty observers. For example, the estimated rudder angles of the two failed rudders are calculated using a two-failure observer that assumes that the rudders other than the rudder determined to have failed are normal.

For example, when the determination unit 42 determines that a failure has occurred in the first rudder R1 and the second rudder R2, the second failure observer #34 is used to The rudder angles of the first rudder R1 and the second rudder R2 are estimated assuming that the rudder is normal.

The selection unit 72 selects the second failure observer when it is determined that the second rudder has a failure. Specifically, the selection unit 72 retains the output from the determination unit 42, and selects two corresponding failure observers when it is determined that failures have occurred in two different rudders. Since the determination unit 42 identifies one faulty rudder, in the case of two faults, a different faulty rudder will be identified for each process. Therefore, by retaining the output from the determination unit 42, the selection unit 72 can recognize two different rudder failures.

For example, when the selection unit 72 recognizes failures in the first rudder R1 and the second rudder R2, the second failure observer #34 is selected.

When the selection unit 72 selects the two failure observers, the multiple failure estimation unit 71 estimates the estimated rudder angles of two different failure rudders using the two failure observers. For example, when the multiple failure estimator 71 is selected, the rudder angles of the first rudder R1 and the second rudder R2, which are the failed rudders, are estimated. The selection unit 72 outputs the estimated rudder angles of the two failed rudders output from the selected two failure observers as the failed rudder angles of each failed rudder. In this way, a plurality of faulty rudders and the faulty rudder angles of each of the faulty rudders are identified.

In the above example, the case of two failures was explained as an example, but the same can be applied to the case of multiple failures.

As explained above, according to the control system, structure, control method, and control program according to the present embodiment, even when it is determined that a failure has occurred in two operating means, , based on the state quantity of the structure and the command values of two of the operating means, a multi-failure estimation process is applied to each operating means to estimate the operation amount for other operating means. By doing so, each operation amount corresponding to each operation means can be estimated. Therefore, failure can be determined based on the estimated operation amounts corresponding to each operation means.

The present disclosure is not limited to the above-described embodiments, and various modifications can be made without departing from the gist of the invention. Note that it is also possible to combine each embodiment.

The control system, structure, control method, and control program described in each of the embodiments described above can be understood, for example, as follows.
The control system according to the present disclosure is applied to a structure (1) in which control amounts (θ, ψ, φ) regarding the attitude of the aircraft are controlled by a plurality of operating means (R) larger than the number of control amounts. In the control system, a failure has occurred in the acquisition unit (61) that acquires the state quantity of the aircraft and the state equation in which the state quantity of the aircraft is the state variable and the manipulated variable of the operating means (R) is input. A term related to the failure operation means (R) is set as a disturbance term based on information regarding the failure operation means (R) which is the operation means (R), and a term other than the failure operation means (R) is set based on the acquired state quantity. a calculation unit (62) that calculates the operation amount of the operation means (R).

According to the control system according to the present disclosure, in the state equation in which the state quantity of the aircraft body is the state variable and the operation amount of the operating means (R) is input, the state quantity acquired as the state variable is used, and the fault operating means (R) is By setting the term related to the failure operation means (R) as a disturbance term based on the information regarding the failure operation means (R), it is possible to calculate the operation amount of the operation means (R) other than the failure operation means (R). The operation amount is calculated in consideration of the disturbance term related to the faulty operating means (R) in which the fault has occurred. That is, the operation amount of the operating means (R) other than the faulty operating means (R) is calculated so as to reduce the influence of the faulty operating means (R). For example, even if a failure (e.g. sticking or deformation) occurs in the operating means (R), which may affect the attitude of the aircraft, other operating means (R) (which is not malfunctioning) The operating means (R)) makes it possible to suppress this influence. Note that the information regarding the faulty operating means (R) includes, for example, information for identifying the operating means (R) in which the fault has occurred, and the amount of operation of the operating means (R) in which the fault has occurred (for example, the amount of operation that is stuck). It is.

In the control system according to the present disclosure, the calculation unit (62) adjusts the operation amount of the operation means (R) other than the failure operation means (R) based on the state equation so as to cancel the disturbance term. It may also be calculated.

According to the control system according to the present disclosure, by calculating the operation amount of the operating means (R) other than the fault operating means (R) so as to cancel the disturbance term in the state equation, It is possible to calculate the amount of operation of the operating means (R) (in which no failure has occurred) for canceling the influence. For example, by using the calculated operation amount as a correction value, it is possible to calculate the operation amount of each operation means (R) that suppresses the influence of the failed operation means (R).

In the control system according to the present disclosure, the calculation unit (62) sets the differential term of the state variable to 0 in the state equation, and sets the operation amount of the operation means (R) other than the failure operation means (R) to 0. The calculation may be performed by solving for .

According to the control system according to the present disclosure, by setting the differential term of the state variable in the state equation to 0, and solving the state equation for the operation amount of the operation means (R) other than the failure operation means (R), It is possible to calculate the operation amount of the operation means (R) other than the failure operation means (R) that cancels out the disturbance term.

In the control system according to the present disclosure, at least one of the coefficients in the state equation may be expressed as a function of a parameter related to the state quantity of the aircraft.

According to the control system according to the present disclosure, by making the coefficient in the state equation a function of a parameter related to the state quantity of the aircraft, the value of the coefficient can be appropriately set depending on the state of the aircraft. Therefore, it becomes possible to perform calculations using the state formula with higher accuracy.

In the control system according to the present disclosure, in the state equation, the state quantity may include a forward speed of the aircraft, and a coefficient related to the forward speed may be set to zero.

According to the control system according to the present disclosure, the coefficient related to the forward speed of the aircraft is set to 0 in the state equation. The forward speed may be influenced by information regarding the amount of operation of the operating means (R). In such a case, if the manipulated variable is calculated by taking into account the coefficient related to the forward speed, an accurate value may not be obtained as the manipulated variable. Therefore, by setting the coefficient related to the forward speed to 0 in the state equation, it becomes possible to perform more accurate calculations.

In the control system according to the present disclosure, the operating means (R) may be driven by an actuator.

According to the control system according to the present disclosure, an actuator can be used as the operating means (R).

A structure (1) according to the present disclosure includes a plurality of operating means (R) for controlling the attitude of the aircraft body, and the above-mentioned control system.

A control method according to the present disclosure is a control method applied to a structure (1) in which a control amount related to the attitude of an aircraft body is controlled by a plurality of operating means (R) that is larger than the number of control amounts, In the state equation in which the state quantity of the aircraft is the state variable and the operation amount of the operation means (R) is input, the failure operation means is the operation means (R) in which the failure has occurred. A term related to the failure operation means (R) is set as a disturbance term based on information regarding the failure operation means (R), and the operation amount of the operation means (R) other than the failure operation means (R) is set based on the obtained state quantity. and a step of calculating.

A control program according to the present disclosure is a control program that is applied to a structure (1) that controls a control amount regarding the attitude of an aircraft body by a plurality of operating means (R) that is larger than the number of control amounts, In a state equation in which the state quantity of the aircraft is a state variable and the operation amount of the operation means (R) is input, a failure operation means is the operation means (R) in which a failure has occurred. A term related to the failure operation means (R) is set as a disturbance term based on information regarding the failure operation means (R), and the operation amount of the operation means (R) other than the failure operation means (R) is set based on the obtained state quantity. A computer is made to perform a process of calculating .

1: Underwater vehicle (structure)
2: Propeller 11: CPU
12:ROM
13: RAM
14: Hard disk drive 15: Communication section 18: Bus 20: Control device (control system)
30: Rudder angle command calculation unit 31: Virtual steering angle calculation unit 32: Distribution unit 33: Depth controller 34: Direction controller 35: Roll controller 36: Attitude angle controller 37: Invalidation unit 40: Failure diagnosis unit ( failure diagnosis system)
41: Estimation unit 42: Determination unit 45: Calculation unit (failure operation amount calculation unit)
50: Degeneration command unit 60: Fault-tolerant control unit 61: Acquisition unit 62: Calculation unit (command value calculation unit)
71: Multiple failure estimation section 72: Selection section A1: First AND section A2: Second AND section A3: Third AND section A4: Fourth AND section R1 to R4: Rudder (operating means)
S1: First specifying section S2: Second specifying section S3: Third specifying section S4: Fourth specifying section Z: Depth p: Angular velocity q: Angular velocity r: Angular velocity u: Velocity v: Velocity w: Velocity θ: Pitch angle φ : Roll angle ψ : Yaw angle

Claims (7)

A control system applied to a structure in which a control amount regarding the attitude of an aircraft body is controlled by a plurality of operating means greater than the number of control amounts, the control system comprising:
an acquisition unit that acquires state quantities of the aircraft;
In a state equation in which the state quantity of the aircraft is a state variable and the operation amount of the operation means is input, a term regarding the failure operation means is set as a disturbance term based on information regarding the failure operation means, which is the operation means in which a failure has occurred. a calculation unit that calculates the operation amount of the operation means other than the failure operation means based on the set and acquired state quantity;
Equipped with
The calculation unit is a control system that calculates the operation amount of the operation means other than the failure operation means based on the state equation so as to cancel the disturbance term. The control system according to claim 1 , wherein at least one of the coefficients in the state equation is expressed as a function of a parameter related to a state quantity of the aircraft. 3. The control system according to claim 1 , wherein in the state equation, the state quantity includes a forward speed of the aircraft, and a coefficient related to the forward speed is set to zero. The control system according to any one of claims 1 to 3 , wherein the operating means is driven by an actuator. multiple operating means for controlling the attitude of the aircraft,
A structure comprising a control system according to any one of claims 1 to 4 . A control method applied to a structure in which a control amount regarding the attitude of an aircraft body is controlled by a plurality of operating means greater than the number of control amounts, the control method comprising:
an acquisition step of acquiring state quantities of the aircraft;
In a state equation in which the state quantity of the aircraft is a state variable and the operation amount of the operation means is input, a term regarding the failure operation means is set as a disturbance term based on information regarding the failure operation means, which is the operation means in which a failure has occurred. a calculation step of calculating the operation amount of the operation means other than the failure operation means based on the set and acquired state quantity;
has
In the calculation step, the control method calculates the operation amount of the operation means other than the failure operation means based on the state equation so as to cancel the disturbance term . A control program applied to a structure that controls a control amount related to the attitude of an aircraft body by a plurality of operating means greater than the number of control amounts, the control program comprising:
Acquisition processing for acquiring state quantities of the aircraft,
In a state equation in which the state quantity of the aircraft is a state variable and the operation amount of the operation means is input, a term regarding the failure operation means is set as a disturbance term based on information regarding the failure operation means, which is the operation means in which a failure has occurred. calculation processing for calculating the operation amount of the operation means other than the failure operation means based on the set and acquired state quantity;
make the computer run
In the calculation process, the control program calculates the operation amount of the operation means other than the failure operation means based on the state equation so as to cancel the disturbance term .

Images