JP2016078621A - Navigation body control device, navigation body, method for control of navigation body, program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a navigation body control device which can enhance accuracy of a hull control for a navigation body.SOLUTION: A navigation body control device performs a feed back control on the basis of a target navigation control parameter concerning a target depth, a target roll angle, a target pitch angle and a target azimuth angle and an actual value corresponding to the target navigation control parameter. An advance speed and a drift speed of a navigation body 1 are acquired. Then, a feedforward control value is calculated by use of interference compensation operational expression, which is derived by using linearization state equation near an optional equilibrium point of the navigation body including correction term which corrects interference with navigation control parameter in association with change of an advance speed and is proportional to the product of an advance speed multiplied by a drift speed, a value calculated based on the navigation control parameter, and state variable including the advance speed and the drift speed. A result of feedback control is corrected by use of the feedback control value.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a navigation body control device, a navigation body, a navigation body control method, and a program.

  A navigation object that floats and navigates in a three-dimensional space such as underwater in a free direction based on the driver's control changes the traveling direction based on the inertial force in the traveling direction when the traveling direction is changed. A lateral force is applied to the navigation body, and a velocity in the transverse direction is generated. At that time, the navigation body has the same direction as the turning direction by changing the traveling direction around the axis passing through the center of gravity of the navigation body and perpendicular to the plane formed by including the vector before and after the change in the traveling direction. A rotational force (moment) is generated in the direction. These forces in the cross flow direction and the rotational force around the axis passing through the center of gravity vary depending on the speed in the traveling direction and the position of the center of gravity. As a related technique, Patent Document 1 discloses a technique for controlling an anti-torque applied to a handle in accordance with a lateral acceleration to improve an operation feeling.

Japanese Patent No. 4938271

  By the way, the rotational force when rotating around the axis passing through the center of gravity of the navigation body as described above is the overshoot in the traveling direction (a phenomenon such as the bow direction rotating beyond the target azimuth direction and the target azimuth direction deviating). Produce.

  Therefore, an object of the present invention is to provide a navigation body control device, a navigation body, a navigation body control method, and a program that can solve the above-described problems.

  In order to achieve the above object, the present invention provides target navigation control parameters used for three-dimensional navigation of a navigation body that navigates in a free direction within a three-dimensional space, and includes a target depth, a target roll angle, and a target pitch angle. A feedback control unit that performs feedback control based on a target navigation control parameter for the target azimuth and an actual measurement value corresponding to the target navigation control parameter; a traveling speed in the traveling direction of the navigation body; A speed acquisition unit that acquires a lateral flow speed in a lateral flow direction of the navigation body associated with the change, and a correction term that corrects an interference with the navigation control parameter associated with the change in the travel direction, wherein the traveling speed and the lateral flow speed are An interference compensation calculation equation derived using a linearized equation of state in the vicinity of an arbitrary equilibrium point of the navigation body, including a correction term proportional to a product, and the navigation control parameter A feedforward control unit that calculates a feedforward control value that corrects a result of the feedback control using a value calculated based on a data, a state variable including the traveling speed and the lateral flow speed, and the feedforward control And a control correction unit that corrects a result of the feedback control using a value.

  In the present invention, the equation for calculating the interference compensation can be applied in the entire operating range of the navigation body by expressing a coefficient matrix as a function of the equilibrium point in a linear state equation in the vicinity of an arbitrary equilibrium point of the navigation body. The linearized equation of state changed in this way is an arithmetic expression derived by reversing the parameter relating to the navigation control amount necessary for the navigation body to perform a desired motion, and the feedforward control unit An interference compensation steering angle is calculated by substituting the state variable into the interference compensation calculation formula, and the control correction unit corrects the steering angle command as a result of the feedback control based on the interference compensation steering angle. It is characterized by doing.

  The present invention is characterized in that the navigation body is a navigation body that navigates underwater, and includes a lateral flow speed estimation unit that estimates the lateral flow speed using a Kalman filter.

  Moreover, this invention is a navigation body provided with the above-mentioned navigation body control apparatus.

  The present invention also provides a target navigation control parameter used for three-dimensional navigation of a navigation body that navigates in a free direction within a three-dimensional space, and includes target depth, target roll angle, target pitch angle, and target azimuth angle. Feedback control based on the navigation control parameter of the vehicle and the actual measurement value corresponding to the target navigation control parameter, the traveling speed of the navigation body in the traveling direction and the lateral flow speed of the navigation body in the lateral flow direction associated with the change of the traveling direction And a correction term that corrects an interference with the navigation control parameter associated with the change in the traveling direction, and includes a correction term that is proportional to a product of the traveling speed and the lateral flow speed. An interference compensation calculation formula derived using a linearized state equation in the vicinity of the equilibrium point, a value calculated based on the navigation control parameter, the traveling speed, and the lateral flow speed A feedforward control value that corrects the result of the feedback control using the state variable, and a correction coefficient that corrects interference with the navigation control parameter associated with the change in the traveling direction, the traveling speed and Using a linearized equation of state in the vicinity of an arbitrary equilibrium point of the navigation body, including a correction coefficient proportional to the product of the lateral flow velocity, a state variable calculated based on the navigation control parameter, and an interference compensation equation The navigation object control method is characterized in that a feedforward control value for correcting the result of the feedback control is calculated, and the result of the feedback control is corrected using the feedforward control value.

  The present invention also provides a target navigation control parameter used for three-dimensional navigation of a navigation object that navigates a computer of the navigation control apparatus in a free direction in a three-dimensional space, and includes a target depth, a target roll angle, and a target pitch. Feedback control means for performing feedback control based on the target navigation control parameter for the angle and the target azimuth and the actual measurement value corresponding to the target navigation control parameter, the traveling speed in the traveling direction of the navigation body and the traveling direction A speed acquisition means for acquiring a transverse flow velocity in a transverse flow direction of the navigation body associated with the change, and a correction term for correcting interference with the navigation control parameter associated with the change in the traveling direction, wherein the product of the traveling velocity and the transverse flow velocity An interference compensation equation derived using a linearized equation of state in the vicinity of an arbitrary equilibrium point of the navigation body, including a correction term proportional to A feedforward control means for calculating a feedforward control value for correcting a result of the feedback control using a value calculated based on a control parameter, the traveling speed, and a state variable including the lateral flow speed; and the feedforward control value This is a program that functions as a control correction unit that corrects the result of the feedback control.

  According to the present invention, the feedforward control is performed using the above linearized equation of state to which the interference term for the lateral flow based on the change in the azimuth direction of the navigation body is applied, thereby reducing the mutual interference between the roll angle φ and the azimuth angle ψ. Therefore, the accuracy of the hull control of the navigation body can be improved, and the power consumption of the rudder drive system can be reduced.

It is a figure which shows schematic structure of the navigation body by one Embodiment of this invention. It is the figure which showed typically about arrangement | positioning of the rudder when it sees from the back of the navigation body by one Embodiment of this invention. It is a figure for demonstrating the freedom degree of the motion of the navigation body by one Embodiment of this invention. It is a 1st figure which shows the functional block of the navigation body control apparatus by one Embodiment of this invention. It is a 2nd figure which shows the functional block of the navigation body control apparatus by one Embodiment of this invention. It is a 3rd figure which shows the functional block of the navigation body control apparatus by one Embodiment of this invention. It is a 1st figure which shows the simulation result at the time of the advancing direction control of the navigation body control apparatus by one Embodiment of this invention. It is a 2nd figure which shows the simulation result at the time of the advancing direction control of the navigation body control apparatus by one Embodiment of this invention.

Hereinafter, a navigation body, a navigation body control device, and a navigation body control method according to an embodiment of the present invention will be described with reference to the drawings.
FIG. 1 is a diagram showing a schematic configuration of a navigation body according to an embodiment of the present invention.
FIG. 2 is a diagram schematically showing the arrangement of the rudder when viewed from the rear of the navigation body according to the embodiment of the present invention.
FIG. 3 is a diagram for explaining the degree of freedom of movement of the navigation body.

  1 and 2, reference numeral 1 indicates a hull of a navigation body. As shown in FIGS. 1 and 2, the navigation body is provided with a plurality of rudders 3a to 3e. The navigation body uses the rudders 3a to 3d mainly for control related to the pitch angle θ, the roll angle φ, and the azimuth angle ψ of the navigation body. The navigation body uses the rudder 3e mainly for control related to depth.

  As shown in FIG. 3, in the navigation body, the hull 1 has a stern axis (hereinafter referred to as “x axis”), a left and right axis (hereinafter referred to as “y axis”), a vertical axis (in the absolute coordinate system (earth coordinate system)). This is controlled by the navigation control device based on three linear axes that are orthogonal to each other (hereinafter referred to as “z-axis”).

As shown in FIG. 3, in this embodiment, the traveling speeds in the positive direction of the x-axis direction, y-axis direction, and z-axis direction in the hull coordinate system are u, v, and w, respectively, and angular velocities around the respective axes. And angular acceleration are defined as p, dp / dt, q, dq / dt, r, dr / dt. Note that the command values of the respective speeds and accelerations around the respective axes targeted by the navigation control apparatus 10 are p ^* (target roll angular velocity), dp ^* / dt (target roll angular acceleration), q ^* (target pitch angular velocity). , Dq ^* / dt (target pitch angular acceleration), r ^* (target azimuth angular velocity = target yaw angular velocity), dr ^* / dt (target azimuth angular acceleration = target yaw angular acceleration). * Mark attached to each symbol indicates a command value. Further, in the present embodiment, the rotation angle around the x axis is defined as the roll angle φ, the rotation angle around the y axis as the pitch angle θ, and the rotation angle (azimuth angle) around the z axis as the yaw angle ψ.
The navigation body control device controls the attitude and position, the traveling direction (azimuth angle), the traveling speed, and the depth of the navigation body in the absolute coordinate system by controlling the rudders 3a to 3e by processing to be described later.
The navigation body is an underwater traveling body that travels underwater or an air floating body that floats in the air.

FIG. 4 is a first diagram illustrating functional blocks of the navigation control apparatus according to the present embodiment.
A navigation body control method according to an embodiment of the present invention will be described below with reference to the drawings.
In FIG. 4, the code | symbol 10 has shown the navigation body control apparatus. The navigation body control device 10 is provided in the hull of the navigation body, and controls propulsion speed by the rudders 3a to 3e and a propulsion device (a device including an engine, a propeller, and the like).
As shown in FIG. 4, the navigation control apparatus 10 includes at least a navigation control parameter acquisition unit 11, a feedback control unit 12, a speed acquisition unit 13, a feedforward control unit 14, and a control correction unit 15. Each of these control units is provided in the navigation control apparatus 10 by a CPU (Central Processing Unit) of a computer of the navigation control apparatus 10 reading and executing a program recorded in a storage unit such as a memory or a hard disk drive. Corresponds to the function.
The output from the control correction unit 15 in the navigation system controller is output to the hull motion controller 30. The hull motion control device 30 is the rudder 3a to 3e or the propulsion device described above.

The navigation control parameter acquisition unit 11 is a navigation control parameter used for three-dimensional navigation of a navigation body that navigates in a free direction in an absolute coordinate system (earth coordinate system) (in a three-dimensional space), and includes a depth, The navigation control parameters for the roll angle, pitch angle, and azimuth angle are acquired sequentially.
The feedback control unit 12 performs feedback control based on the target navigation control parameter and the actually measured value corresponding to the target navigation control parameter.
The speed acquisition unit 13 acquires the traveling speed in the traveling direction of the navigation body and the lateral flow speed in the lateral flow direction of the navigation body accompanying the change in the traveling direction.
The feedforward control unit 14 corrects an interference with the navigation control parameter associated with the change in the traveling direction, and includes a correction term proportional to the product of the traveling speed u and the lateral flow speed v. The result of the feedback control is corrected using the interference compensation calculation formula derived using the linearized state equation in the vicinity of the point and the state variable including the value calculated based on the navigation control parameter, the traveling speed u, and the lateral flow speed v. The feedforward control value to be calculated is calculated.
The control correction unit 15 corrects the result of the feedback control using the feedforward control value.

It should be noted that the interference compensation equation is a linearity equation in which the coefficient matrix is expressed as a function of the equilibrium point in the linearized equation of state in the vicinity of any equilibrium point of the navigation body, and is changed so as to be adaptive over the entire operating range of the navigation body. Derived using the generalized equation of state. Specifically, the interference compensation calculation formula is derived by reversing the linearized state equation with respect to parameters related to the navigation control amount necessary for the navigation body to perform a desired motion.
The feedforward control unit 14 calculates the interference compensation steering angle by substituting the state variable into the interference compensation calculation formula, and the control correction unit 15 determines the steering angle command as a result of the feedback control as the interference compensation steering angle. Correct based on

FIG. 5 is a second diagram showing functional blocks of the navigation control apparatus according to the present embodiment.
FIG. 6 is a third diagram showing functional blocks of the navigation control apparatus according to the present embodiment.
The functional block diagram shown in FIG. 5 explains in detail the navigation control parameter acquisition unit 11, the feedback control unit 12, and the control correction unit 15 in the functional block diagram of FIG.
The functional block diagram shown in FIG. 6 explains the speed acquisition unit 13 and the feedforward control unit 14 in the functional block diagram of FIG. 4 in more detail.
As shown in FIG. 5, the navigation control parameter acquisition unit 11 includes a target depth calculation unit 111, a target roll angle calculation unit 112, a target pitch angle calculation unit 113, and a target azimuth angle calculation unit 114.
In addition, the dot (*) attached | subjected on w ^* , p ^* , q ^* , r ^{* in} the figure has shown that it is a differential value d / dt.

The target depth calculation unit 111 calculates the target depth z ^* based on the acquired set depth z _set . The differentiator 92a shown in FIG. 6 differentiates the target depth z ^* to calculate the target depth direction speed w ^* . Further, the differentiator 92b shown in FIG. 6 differentiates the target depth direction velocity w ^* to calculate the target depth direction acceleration dw ^* / dt. The target depth calculation unit 111 may have the functions of the differentiators 92a and 92b, and the target depth calculation unit 111 may calculate the target depth direction speed w ^* and the target depth direction acceleration dw ^* / dt.
The target roll angle calculation unit 112 calculates the target roll angle φ ^* based on the acquired set roll angle φ _set . The differentiator 92c shown in FIG. 6 differentiates the target roll angle φ ^* to calculate the target roll angular velocity p ^* . Further, the differentiator 92d shown in FIG. 6 differentiates the target roll angular velocity p ^* to calculate the target roll angular acceleration dp ^* / dt. The target roll angle calculation unit 112 has the functions of the differentiators 92c and 92d, and the target roll angle calculation unit 112 may calculate the target roll angular velocity p ^* and the target roll angular acceleration dp ^* / dt. .
The target pitch angle calculation unit 113 calculates the target pitch angle θ ^* based on the acquired set pitch angle θ _set . The differentiator 92e shown in FIG. 6 differentiates the target pitch angle θ ^* to calculate the target pitch angular velocity q ^* . Further, the differentiator 92f shown in FIG. 6 differentiates the target pitch angular velocity q ^* to calculate the target pitch angular acceleration dq ^* / dt. The target pitch angle calculation unit 113 has the functions of the differentiators 92e and 92f, and the target pitch angle calculation unit 113 calculates the target pitch angular velocity q ^* and the target pitch angular acceleration dq ^* / dt. Good.
The target azimuth angle calculation unit 114 calculates the target azimuth angle ψ ^* based on the acquired set azimuth angle ψ _set . The differentiator 92g shown in FIG. 6 differentiates the target azimuth angle ψ ^* to calculate the target azimuth angular velocity r ^* . Further, the differentiator 92h shown in FIG. 6 differentiates the target azimuth angular velocity r ^* to calculate the target azimuth acceleration dr ^* / dt. The target azimuth angle calculation unit 114 has the functions of the differentiators 92g and 92h. The target azimuth angle calculation unit 114 may calculate the target azimuth angular velocity r ^* and the target azimuth angular acceleration dr ^* / dt. Good.

As shown in FIG. 5, the feedback control unit 12 includes difference calculation units 121, 122, 123, and 124. The feedback control unit 12 includes feedback controllers (hereinafter referred to as FB controllers) 125, 126, 127, and 128.
The difference calculation unit 121 calculates a difference between the target depth z ^* and the depth z of the actual measurement value based on the previous actual measurement value.
The difference calculation unit 122 calculates a difference between the target roll angle φ ^* and the roll angle φ of the current actual measurement value based on the previous actual measurement value.
The difference calculation unit 123 calculates a difference between the target pitch angle θ ^* and the pitch angle θ of the actual measurement value with the previous actual measurement value as a reference.
The difference calculation unit 124 calculates the difference between the target azimuth angle ψ ^* and the azimuth angle ψ of the actual measurement value based on the previous actual measurement value.
The FB controller 125 calculates a feedback control value of the depth interference compensation steering angle (hereinafter, depth feedback steering angle δb _FB ) by performing PID control based on the output of the difference calculation unit 121.
The FB controller 126 calculates a feedback control value of the roll angle interference compensation steering angle (hereinafter, roll angle feedback steering angle δr _rFB ) by performing PID control based on the output of the difference calculation unit 122.
The FB controller 127 calculates a feedback control value of the pitch angle interference compensation steering angle (hereinafter, pitch angle feedback steering angle δr _cFB ) by performing PID control based on the output of the difference calculation unit 123.
The FB controller 128 calculates a feedback control value of the azimuth interference compensation steering angle (hereinafter referred to as azimuth feedback steering angle δr _1FB ) by performing PID control based on the output of the difference calculation unit 124.

As shown in FIG. 6, the feedforward control unit 14 includes a coefficient matrix calculation unit 141, an interference compensation steering angle calculation unit 142, and a coordinate conversion unit 143.
The coefficient matrix calculation unit 141 receives the traveling speed u in the traveling direction of the navigation body input from the speed acquisition unit 13, the lateral flow speed v of the navigation body, the target depth direction speed w ^* , the target roll angular speed p ^* , the target roll angle φ ^* , The values of the target pitch angular velocity q ^* , the target pitch angle θ ^* , and the target azimuth angular velocity r ^* are sequentially obtained, and two matrices of A (n) matrix and B (n) matrix are obtained based on the values of their equilibrium states. calculate. Details of the two matrices will be described later. Note that each element of the A (n) matrix and the A (n) matrix is expressed as a function at certain equilibrium points X￣ and U￣.

The interference compensation steering angle calculation unit 142 acquires information on the A (n) matrix and the B (n) matrix from the coefficient matrix calculation unit 141.
Further, the interference compensation rudder angle calculation unit 142 acquires the target depth direction speed w ^* and the target depth direction acceleration dw ^* / dt from the differentiators 92a and 92b, respectively.
Further, the interference compensation steering angle calculation unit 142 acquires the target roll angle φ ^* from the target roll angle calculation unit 112 and the target roll angular velocity p ^* and the target roll angular acceleration dp ^* / dt from the differentiators 92c and 92d, respectively.
The interference compensation steering angle calculation unit 142 acquires the target pitch angle θ ^* from the target pitch angle calculation unit 113, and the target pitch angular velocity q ^* and the target pitch angular acceleration dq ^* / dt from the differentiators 92e and 92f, respectively.
Further, the interference compensation steering angle calculation unit 142 acquires the target azimuth angular velocity r ^* and the target azimuth angular acceleration dr ^* / dt from the differentiators 92g and 92h, respectively.

Then, the interference compensation rudder angle calculation unit 142 uses the information of each acquired value and matrix to obtain an interference compensation rudder angle δb _FF ^* (depth interference compensation rudder angle) that is a feedforward control value that serves as an interference compensation value for the rudder angle. Is calculated.
Further, the interference compensation steering angle calculation unit 142 uses the acquired information of each value and matrix to provide interference compensation steering angles δr1 ^* , δr2 ^* , δr3 ^* , δr4, which are feedforward control values that serve as steering angle interference compensation values. ^* Calculate,.
Interference means that when any one of depth, roll angle, pitch angle, and azimuth is controlled, other values are affected. For example, when an elliptical navigation body centered on the stern axis navigates underwater and the traveling direction (azimuth angle) of the navigation body is changed, the transverse flow velocity v is generated as described above. At this time, when a convex shape as shown in FIG. 1 exists at the edge of the plane centering on the stern axis of the navigation body, the navigation body is caused by a reaction of water acting on the convex shape based on the lateral flow velocity v. Rotates around the stern axis. Such rotation is called interference.

The coordinate conversion unit 143 uses the interference compensation steering angles δr1 ^* , δr2 ^* , δr3 ^* , and δr4 ^* obtained by the interference compensation steering angle calculation unit 142 to perform interference corresponding to each of the roll angle, pitch angle, and azimuth angle. Conversion to a compensation steering angle (roll angle interference compensation steering angle δr _rFF ^* , pitch angle interference compensation steering angle δr _cFF ^* , azimuth interference compensation steering angle δr _lFF ^* ). This is based on the geometric rudder arrangement, and the four rudders are converted into rudder forces acting on the roll, pitch and bearing.

As shown in FIG. 5, the control correction unit 15 includes a depth steering angle command calculation unit 151, a roll angle steering angle command calculation unit 152, a pitch angle steering angle command calculation unit 153, an azimuth steering angle command calculation unit 154, and a coordinate conversion. Part 155.
The depth steering angle command calculation unit 151 calculates the value of the depth steering angle command δb ^* by adding the depth feedback steering angle δb _FB and the depth interference compensation steering angle δb _FF ^*, and outputs the calculated value to the hull motion control device 30.
The roll angle steering angle command calculation unit 152 calculates the value of the roll angle steering angle command δr _r ^* by adding the roll angle feedback steering angle δr _rFB and the roll angle interference compensation steering angle δr _rFF ^*, and the coordinate conversion unit 155. Output to.
The pitch angle steering angle command calculation unit 153 adds the pitch angle feedback steering angle δr _cFB and the pitch angle interference compensation steering angle δr _cFF ^* to calculate the value of the pitch angle steering angle command δr _c ^* , and the coordinate conversion unit 155. Output to.
The azimuth steering angle command calculation unit 154 calculates the value of the azimuth steering angle command δr _l ^* by adding the azimuth feedback steering angle δr _lFB and the azimuth interference compensation steering angle δr _lFF ^*, and the coordinate conversion unit 155. Output to.
The coordinate conversion unit 155 performs coordinate conversion processing on the basis of the acquired roll angle steering angle command δr _r ^* , pitch angle steering angle command δr _c ^* , and azimuth steering angle command δr _l ^* to each of the rudders 3a to 3d. Rudder control commands δr1 ^* , δr2 ^* , δr3 ^* , δr4 ^* are calculated. Thus, the rudder 3e is based on the depth rudder angle command δb ^* , the rudder 3a is based on the rudder control command δr1 ^* , the rudder 3b is based on the rudder control command δr2 ^* , and the rudder 3c is based on the rudder control command δr3 ^* . Thus, the rudder 3d is driven and controlled based on the rudder control command δr4 ^* , so that the mutual interference among the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ is suppressed and the hull attitude is followed. It becomes possible.

The hull motion control device 30 controls the rudders 3a to 3e based on the depth rudder angle command δb ^* and the rudder control commands δr1 ^* , δr2 ^* , δr3 ^* , δr4 ^* .
As a result of the control of the hull motion control device 30, the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ change. The depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ are measured by sensors such as a depth meter (or altimeter), a gyro sensor, and an angular velocity sensor provided in the navigation body, and the navigation body control device acquires the values. And used for feedback control.

Next, details of processing of the navigation control apparatus according to the present embodiment will be described.
First, the target depth calculation unit 111 acquires a set depth z _set that is a discrete value from a higher-level device (for example, a device that outputs a driver's operation), and continuously uses a predetermined response model for this value. A target depth z ^* is calculated. An example of the response model is a second-order low-pass filter as shown in the following equation (1).

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

In Equation (2), Ts is the establishment time [sec], and is arbitrarily set according to the operating conditions. Then, the target depth calculation unit 111 outputs the target depth z ^* to the difference calculation unit 121, the coefficient matrix calculation unit 141, and the interference compensation steering angle calculation unit 142.

The target roll angle calculation unit 112 acquires a set roll angle φ _set which is a discrete value from the host device, and calculates a continuous target roll angle φ ^* using a predetermined response model for this value. . The calculation process of the target roll angle φ ^* of the target roll angle calculation unit 112 is the same as the calculation process of the target depth z ^* in the target depth calculation unit 111 described above, and thus detailed description thereof is omitted. Then, the target roll angle calculation unit 112 outputs the target roll angle φ ^* to the difference calculation unit 122, the coefficient matrix calculation unit 141, and the interference compensation steering angle calculation unit 142.
The target pitch angle calculation unit 113 acquires a set pitch angle θ _set which is a discrete value from the host device, and calculates a continuous target pitch angle θ ^* using a predetermined response model for this value. . Note that the calculation process of the target pitch angle θ ^* of the target pitch angle calculation unit 113 is the same as the calculation process of the target depth z ^* in the target depth calculation unit 111 described above, and thus detailed description thereof is omitted. Then, the target pitch angle calculation unit 113 outputs the target pitch angle θ ^* to the difference calculation unit 123, the coefficient matrix calculation unit 141, and the interference compensation steering angle calculation unit 142.
The target azimuth angle calculation unit 114 acquires a set azimuth angle ψ _set that is a discrete value from the host device, and calculates a continuous target azimuth angle ψ ^* using a predetermined response model for this value. . Note that the calculation processing of the target azimuth angle ψ ^* of the target azimuth calculation unit 114 is the same as the calculation processing of the target depth z ^* in the target depth calculation unit 111 described above, and thus detailed description thereof is omitted. Then, the target azimuth calculation unit 114 outputs the target azimuth ψ ^* to the difference calculation unit 124, the coefficient matrix calculation unit 141, and the interference compensation steering angle calculation unit 142.

The feedforward control unit 14 including the coefficient matrix calculation unit 141 and the interference compensation rudder angle calculation unit 142 uses the interference compensation arithmetic expression derived from the linearized state equation of the hull 1 by inverse problem analysis, and the navigation body Depth interference compensation steering angle δb _FF ^* , roll angle interference compensation steering angle δr _rFF ^* , and pitch angle, which are interference compensation values for suppressing mutual interference between the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ. The interference compensation steering angle δr _cFF ^* and the azimuth interference compensation steering angle δr _lFF ^* are calculated.

  Here, the derivation of the interference compensation calculation formula will be described. Generally, the motion of the hull 1 is non-linear, but can be linearized in the vicinity of the equilibrium point of the hull 1 (a small space around the equilibrium point). For example, the linearized state equation in the vicinity of an arbitrary equilibrium point of the hull 1 is expressed by the following equations (3) and (4). Here, regarding the equilibrium point, it is assumed that the hull 1 is in an equilibrium state at the target value, and the target value of the hull motion is set as the equilibrium point.

In the formulas (3) to (6), u, v, w, p, q, r, φ, θ, ψ are the above-described u ^* , v ^* , w ^* , p ^* , q ^* , r ^* , corresponds to φ ^* , θ ^* , ψ ^* , n is the rotation speed [rps] of the propeller provided in the navigation body to obtain propulsive force, δb is the rudder angle of the rudder 3e, in other words, the rudder angle [rad] related to the depth, δr1, δr2, δr3, and δr4 correspond to the above-described δr1 ^* , δr2 ^* , δr3 ^* , and δr4 ^* , and are the steering angles of the rudder 3a, the rudder 3b, the rudder 3c, and the rudder 3d, respectively.

  Subsequently, assuming that there is an equilibrium point (dx / dt = 0) at which the state quantity in the equilibrium state settles at a certain X with respect to an arbitrary manipulated variable U, the following expression (7) is obtained.

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

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

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

  Here, the speeds u and v are not control amounts, the rotational speed n is an operation amount that affects only the speed u, and the azimuth angle ψ does not act as a restoring force on the hull. Since it is irrelevant to the purpose, it is possible to omit the model reduction. However, a direction change is performed when the navigation body is navigating at an arbitrary propulsion speed u, and an interference term (correction term) that acts when a lateral flow velocity v is generated is defined as F · u · v. F is a coefficient determined by the characteristics of the hull 1 of the navigation body. That is, the direction change is performed when the navigation body is navigating at an arbitrary speed u, and the interference force acting when the transverse flow velocity v is generated is proportional to the product of the propulsion velocity u and the transverse flow velocity v. Equation (9) is a state equation considering this interference term. In equation (9), the A (n) matrix is given by the following equation (10), and the matrix B is given by the following equation (11).

  Considering the above equations (10) and (11) and the interference term F · u · v, the linearized state equation is expressed by the following equation (12).

  In Expression (12), Δw is a minute change [m / s] in the z-axis direction. Δp is a minute change [rad / s] of the target roll angular velocity. Δq is a minute change [rad / s] of the target pitch angular velocity. Δr is a minute change [rad / s] of the yaw angular velocity. Δφ is a minute change [rad] in the roll angle. Δθ is a minute change [rad] in the pitch angle. Δδb is a minute change of the rudder 3e, in other words, a minute change [rad] of the rudder angle command related to the depth. Δδrr, Δδrc, and Δδrl are small changes in the roll rudder, pitch rudder, and azimuth (yaw) rudder, in other words, small changes in the rudder angle command regarding the roll angle φ, pitch angle θ, and azimuth (yaw angle) ψ [rad]. It is.

The elements a11 to a66 and b11 to b65 of the A (n) matrix and B (n) matrix are expressed as functions or constants at certain equilibrium points X ^* and U ^* . For example, the elements a14, a15, and a16 are represented by the following formulas (13) to (15). Note that the other elements are naturally derived from the above formulas (3) to (9).

In the equations (13) to (15), the coefficient K is a constant determined by the kinematic characteristic of the hull. As for the subscript of the coefficient K, the first character indicates the axis of the equation of motion, and the second and subsequent characters indicate state variables. For example, in K _uprr , “u” represents an axis (x axis) in the direction indicated by the traveling speed u of the hull 1 of the navigation body, and “prr (pr ² )” represents a state variable.

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

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

Here, in the present embodiment, the interference compensation steering angle is calculated using an inverse analysis calculation formula obtained by replacing the minute change amount shown in the formula (16) with an absolute value as an interference compensation calculation formula. That is, the interference compensation calculation formula according to this embodiment is expressed by the following formula (17).
Thus, by replacing with an absolute value instead of a minute change amount, the configuration can be simplified as compared with the case where a minute change amount is used, and the processing load can be reduced. In equation (17), U ^* is the command value for the manipulated variable, X ^* is the target value for the state quantity, and dX ^* / dt (with a dot on X ^{* in} the formula) is the target value for the differential state quantity. Show.

  The coefficient matrix calculator 141 receives the traveling velocity u in the x-axis direction and the lateral flow velocity v in the lateral flow direction (y-axis direction). The traveling speed u and the lateral flow speed v are values acquired from the host device. The host device is a speed sensor that measures the traveling speed and a speed sensor that measures the lateral flow speed. Alternatively, the host device uses the lateral flow velocity v by using the traveling velocity and other parameters (the depth direction velocity, the roll direction angular velocity, the pitch direction angular velocity, the azimuth direction angular velocity, the roll angle, the pitch angle, and the traveling direction velocity of the navigation body). A device that calculates the internal estimated value by the Kalman filter may be used. Further, the lateral flow speed estimation unit provided in the navigation control apparatus 10 may calculate the lateral flow speed v by the same processing.

The coefficient matrix calculation unit 141 calculates u￣ indicating the value of the equilibrium state based on the acquired traveling speed u.
The coefficient matrix calculation unit 141 calculates v￣ indicating the value of the equilibrium state based on the acquired lateral flow velocity v.
The coefficient matrix calculation unit 141 acquires the target depth direction speed w ^* calculated by the differentiator 92a, and calculates w￣ indicating the value of the equilibrium state based on the value.
Similarly, the coefficient matrix calculation unit 141 calculates p￣ indicating the value of the equilibrium state based on the target roll angular velocity p ^* calculated by the differentiator 92c target roll angle calculation unit 112.
Similarly, the coefficient matrix calculation unit 141 calculates φ￣ indicating the value of the equilibrium state based on the target roll angle φ ^* calculated by the target roll angle calculation unit 112.
Similarly, the coefficient matrix calculation unit 141 calculates q￣ indicating the value of the equilibrium state based on the target pitch angular velocity q ^* calculated by the differentiator 92e.
Similarly, the coefficient matrix calculation unit 141 calculates θ￣ indicating the value of the equilibrium state based on the target pitch angle θ ^* calculated by the target pitch angle calculation unit 113.
Similarly, the coefficient matrix calculation unit 141 calculates r￣ indicating the value of the equilibrium state based on the target azimuth angular velocity r ^* calculated by the differentiator 9292g.

  The coefficient matrix calculation unit 141 includes each element of the coefficient matrix A (the above-described A (n) matrix) and B (the above-described B (n) matrix) as illustrated in the above formulas (13) to (15). Have functions (some of which are constants and zeros), and u 関 数, v￣, w￣, p￣, φ￣ indicating the values of the above-mentioned equilibrium states. , Q￣, θ￣, and r￣ are substituted to calculate coefficient matrices A (n) and B (n) at the current equilibrium point.

  Note that the interference coefficient (correction coefficient) F in the interference term in the above linearized state equation is expressed by the following expression (18).

  Since the interference coefficient F mainly acts in the traveling direction and the rotation direction of the roll angle φ, the interference coefficient F is set by a coefficient fp for the rotation direction of the roll angle φ and a coefficient fr for the traveling direction as shown in Equation (18). Is done.

On the other hand, the interference compensation steering angle calculation unit 142 acquires the coefficient matrices A (n) and B (n) calculated by the coefficient matrix calculation unit 141. Further, the interference compensation steering angle calculation unit 142 acquires the target depth direction speed w ^* and the target depth direction acceleration dw ^* / dt from each of the differentiators 92a and 92b.
Further, the interference compensation steering angle calculation unit 142 acquires the target roll angle φ ^* from the target roll angle calculation unit 112, and the target roll angular velocity p ^* and the target roll angular acceleration dp ^* / dt from the differentiators 92c and 92d, respectively.
In addition, the interference compensation steering angle calculation unit 142 acquires the target pitch angle θ ^* from the target pitch angle calculation unit 113, and the target pitch angular velocity q ^* and the target pitch angular acceleration dq ^* / dt from the differentiators 92e and 92f, respectively.
Further, the interference compensation rudder angle calculation unit 142 acquires the target azimuth angular velocity r ^* and the target azimuth angular acceleration dr ^* / dt from the differentiators 92g and 92h.

The interference compensation rudder angle calculation unit 142 stores an interference compensation calculation expression (for example, the above-described expression (17)) in a memory or the like, reads information of these expressions, and substitutes each value acquired in the expression. Thus, the depth interference compensation steering angle δb _FF ^* , the roll angle interference compensation steering angle δr _rFF ^* , the pitch angle interference compensation steering angle δr _cFF ^* , and the azimuth interference compensation steering angle δr _lFF ^* are calculated.

Next, as described above, the coordinate conversion unit 143 converts the interference compensation steering angles δr1 ^* , δr2 ^* , δr3 ^* , δr4 ^* obtained by the interference compensation steering angle calculation unit 142 into interference compensation steering angles (roll angle interference). Compensation rudder angle δr _rFF ^* , pitch angle interference compensation rudder angle δr _cFF ^* , azimuth angle interference compensation rudder angle δr _lFF ^* ).

Then, the depth steering angle command calculation unit 151 calculates the value of the depth steering angle command δb ^* by adding the depth feedback steering angle δb _FB and the depth interference compensation steering angle δb _FF ^*, and outputs the calculated value to the hull motion control device 30. To do.
The roll angle steering angle command calculation unit 152 calculates the value of the roll angle steering angle command δr _r ^* by adding the roll angle feedback steering angle δr _rFB and the roll angle interference compensation steering angle δr _rFF ^*, and the coordinate conversion unit. To 155.
The pitch angle steering angle command calculation unit 153 adds the pitch angle feedback steering angle δr _cFB and the pitch angle interference compensation steering angle δr _cFF ^* to calculate the value of the pitch angle steering angle command δr _c ^* , and the coordinate conversion unit To 155.
Further, the azimuth steering angle command calculation unit 154 calculates the value of the azimuth steering angle command δl ^* by adding the azimuth feedback steering angle δr _lFB and the azimuth interference compensation steering angle δr _lFF ^*, and the coordinate conversion unit 155. Output to.

The coordinate conversion unit 155 performs coordinate conversion processing based on the acquired roll angle steering angle command δr _r ^* , pitch angle steering angle command δr _c ^* , and azimuth angle steering angle command δl ^*, and sends the information to each of the rudders 3a to 3d. Rudder control commands δr1 ^* , δr2 ^* , δr3 ^* , δr4 ^* are calculated and output to the hull motion control device 30.

The hull motion control device 30 mainly controls the rudders 3a to 3e based on the depth rudder angle command δb ^* and the rudder control commands δr1 ^* , δr2 ^* , δr3 ^* , δr4 ^* .
As a result of the control of the hull motion control device 30, the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ change, and the sensor measures the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ, The respective values are output to the difference calculation units 121, 122, 123, and 124.

FIG. 7 is a first diagram illustrating a simulation result when the traveling body control device controls the traveling direction.
FIG. 8 is a second diagram showing a simulation result during the traveling direction control of the navigation control apparatus.
FIG. 7 shows a simulation result when the feed-forward control is performed without applying the interference term shown in the above-described linearized state equation to the linearized state equation. FIG. 8 shows a simulation result when feedforward control is performed using the above linearized state equation. According to the simulation result shown in FIG. 8, it can be seen that the actually measured values follow the target value with high accuracy with respect to the azimuth angle and the roll angle as compared with the simulation result shown in FIG. 7.

As described above, according to the navigation control apparatus 10 according to the present embodiment, it is possible to suppress the mutual interference of the depth z, the roll angle φ, the pitch angle θ, and the azimuth angle ψ. Therefore, the accuracy of the hull control of the navigation body can be improved, and the power consumption of the rudder drive system can be reduced.
And according to the navigation body control apparatus 10 which concerns on this embodiment, by performing feedforward control using the above-mentioned linearization state equation to which the above-mentioned interference term is applied, the roll angle φ and the azimuth angle ψ can be further reduced. Interference can be suppressed, the accuracy of hull control of the navigation body can be improved, and the power consumption of the rudder drive system can be reduced.

  In addition, the above-mentioned navigation body control apparatus 10 has a computer system inside. Each process described above is stored in a computer-readable recording medium in the form of a program, and the above process is performed by the computer reading and executing the program. Here, the computer-readable recording medium means a magnetic disk, a magneto-optical disk, a CD-ROM, a DVD-ROM, a semiconductor memory, or the like.

  The program may be for realizing a part of the functions described above. Furthermore, what can implement | achieve the function mentioned above in combination with the program already recorded on the computer system, and what is called a difference file (difference program) may be sufficient.

1 ... hull (navigation body)
3a, 3b, 3c, 3d, 3e ... rudder 10 ... navigation control device 11 ... navigation control parameter acquisition unit 12 ... feedback control unit 13 ... speed acquisition unit 14 ... feed forward Control unit 15... Control correction unit 15
30 ... hull motion control device 111 ... target depth calculator 112 ... target roll angle calculator 113 ... target pitch angle calculator 114 ... target azimuth calculator 121, 122, 123, 124 ... Calculation unit 125, 126, 127, 128 ... FB controller 151 ... Depth steering angle command calculation unit 152 ... Roll angle steering angle command calculation unit 153 ... Pitch angle steering angle command calculation 154... Steering angle command calculation unit 155... Coordinate conversion units 92 a to 92 h... Differentiator 141... Coefficient matrix calculation unit 142. Conversion part

Claims (6)

A target navigation control parameter used for three-dimensional navigation of a navigation body that navigates in a free direction in a three-dimensional space, the target navigation control parameters for target depth, target roll angle, target pitch angle, and target azimuth angle; A feedback control unit that performs feedback control based on an actual measurement value corresponding to the target navigation control parameter;
A speed acquisition unit that acquires a traveling speed in a traveling direction of the navigation body and a lateral flow speed in a lateral flow direction of the navigation body according to the change in the traveling direction;
A linear term in the vicinity of any equilibrium point of the navigation body, including a correction term that corrects an interference with the navigation control parameter associated with the change in the traveling direction and is proportional to a product of the traveling speed and the lateral flow speed. A feed that corrects the result of the feedback control using an interference compensation arithmetic expression derived using a generalized state equation, and a state variable including a value calculated based on the navigation control parameter, the traveling speed, and the lateral flow speed A feedforward control unit for calculating a forward control value;
A control correction unit that corrects the result of the feedback control using the feedforward control value;
A navigation body control device characterized by comprising: In the linearized equation of state in the vicinity of an arbitrary equilibrium point of the navigation body, the interference compensation calculation formula is changed so as to be adaptable over the entire operating range of the navigation body by expressing a coefficient matrix as a function of the equilibrium point. An equation derived by reversing a linearized equation of state with respect to a parameter relating to a navigation control amount necessary for the navigation body to perform a desired motion,
The feedforward control unit calculates an interference compensation steering angle by substituting the state variable into the interference compensation calculation formula,
The navigation control apparatus according to claim 1, wherein the control correction unit corrects a steering angle command that is a result of the feedback control based on the interference compensation steering angle. The navigation body is a navigation body that navigates underwater, and a lateral flow speed estimation unit that estimates the lateral flow speed using a Kalman filter;
The navigation body control device according to claim 1, comprising:   A navigation body comprising the navigation body control device according to any one of claims 1 to 3. A target navigation control parameter used for three-dimensional navigation of a navigation body that navigates in a free direction in a three-dimensional space, the target navigation control parameters for target depth, target roll angle, target pitch angle, and target azimuth angle; Feedback control based on the actual measurement value corresponding to the target navigation control parameter,
Obtaining the traveling speed in the traveling direction of the navigation body and the lateral flow speed in the lateral direction of the navigation body in accordance with the change in the traveling direction;
A linear term in the vicinity of any equilibrium point of the navigation body, including a correction term that corrects an interference with the navigation control parameter associated with the change in the traveling direction and is proportional to a product of the traveling speed and the lateral flow speed. A feed that corrects the result of the feedback control using an interference compensation arithmetic expression derived using a generalized state equation, and a state variable including a value calculated based on the navigation control parameter, the traveling speed, and the lateral flow speed Calculate the forward control value,
Linearity in the vicinity of an arbitrary equilibrium point of the navigation body, including a correction coefficient that corrects interference with the navigation control parameter due to the change in the traveling direction and is proportional to a product of the traveling speed and the lateral flow speed Calculating a feedforward control value for correcting the result of the feedback control using a state equation, a state variable calculated based on the navigation control parameter, and an interference compensation calculation formula,
The navigation object control method, wherein the result of the feedback control is corrected using the feedforward control value. The computer of the navigation control device,
A target navigation control parameter used for three-dimensional navigation of a navigation body that navigates in a free direction in a three-dimensional space, the target navigation control parameters for target depth, target roll angle, target pitch angle, and target azimuth angle; Feedback control means for performing feedback control based on an actual measurement value corresponding to the target navigation control parameter;
Speed acquisition means for acquiring a traveling speed in the traveling direction of the navigation body and a lateral flow speed in the lateral flow direction of the navigation body in accordance with the change in the traveling direction;
A linear term in the vicinity of any equilibrium point of the navigation body, including a correction term that corrects an interference with the navigation control parameter associated with the change in the traveling direction and is proportional to a product of the traveling speed and the lateral flow speed. A feed that corrects the result of the feedback control using an interference compensation arithmetic expression derived using a generalized state equation, and a state variable including a value calculated based on the navigation control parameter, the traveling speed, and the lateral flow speed Feed forward control means for calculating a forward control value;
Control correction means for correcting the result of the feedback control using the feedforward control value;
A program characterized by functioning as

Images