JP4884303B2 - Underwater vehicle control system - Google Patents

Description

  The present invention relates to an underwater vehicle having a propulsion mechanism, and more particularly to an underwater vehicle control system that controls the position and posture of an underwater vehicle.

  Conventionally, as a preferred embodiment of an underwater vehicle control system that controls the position and orientation of an underwater vehicle, an in-reactor inspection system that performs an inspection operation of an in-reactor structure using an underwater vehicle is known. . As an example of this in-reactor inspection system, an underwater inspection device (underwater moving body) having a thruster for applying thrust to the main body and a detecting means for detecting a state quantity related to the position and posture of the main body, and a cable to the underwater inspection device And a control device that controls the position and posture of the underwater inspection device are disclosed (for example, see Patent Document 1). In the prior art described in Patent Document 1, the detection means is provided in the underwater inspection apparatus, and includes an acceleration sensor that detects acceleration in the three-axis (X-axis, Y-axis, and Z-axis) directions and around the three axes. It comprises a gyro that detects the angular velocity of the camera and a camera that images the object on which the irradiated slit laser is reflected. And a control apparatus calculates the position and attitude | position of an underwater inspection apparatus based on the detection result of these detection means, and drives and controls the thruster of an underwater inspection apparatus based on this.

  By the way, in an environment where an external force (disturbance) other than the thrust of the thruster is applied to the underwater moving body, the accuracy of the position and posture control of the underwater moving body is reduced due to the influence of the external force. Therefore, in order to cope with this, for example, assuming an environment without external force, an underwater beagle model that calculates the position and orientation of an underwater moving body (underwater beagle) that is moved by thrust of a thruster driven by a torque command, and this A position / posture deviation / torque command converter for obtaining an external force suppression torque command from the deviation between the actual value (detected value) and the calculated position / posture value of the underwater moving body calculated by the underwater beagle model, and the torque command and external force described above. There has been proposed a control device including an adder that outputs an external force suppression torque command obtained by adding a suppression torque command. (For example, refer to Patent Document 2).

JP 2005-315709 A Japanese Patent Laid-Open No. 10-203479

However, there are the following problems in the above-described prior art.
That is, in the prior art described in Patent Document 2, the position and posture of the underwater mobile object are calculated using an underwater beagle model assuming an environment without external force, and the influence of external force is determined from the deviation between the calculated value and the actual value. Is estimated and the command value of the thruster is corrected. In other words, since the external force applied to the underwater mobile body is detected indirectly and afterwards, there is room for improvement in terms of the accuracy of detecting the external force, and hence the accuracy of the position and posture control of the underwater mobile body. .

The purpose of the present invention is to provide an underwater mobile control system capable of improving the position and attitude control of the underwater vehicle.

(1) To achieve the above Symbol purpose, the present invention includes a first detection of detecting the underwater vehicle having a propulsion mechanism for imparting thrust to the body, the state quantity relating to the position and orientation of the underwater vehicle And a position / attitude calculation means for calculating the position and orientation of the underwater mobile body based on the detection result of the first detection means. Second detection means for detecting a state quantity directly related to an external force other than the thrust of the propulsion mechanism applied to the external force, an external force calculation means for calculating an external force applied to the underwater moving body based on a detection result of the second detection means, based on the external force applied to the underwater vehicle which is calculated position and posture and the position and orientation of the underwater vehicle which is computed by the computing means in the external-force calculating means, and a propulsion mechanism control means for controlling the propulsion mechanism, the propulsion構制control means, when the external force calculated by the force calculating means exceeds a predetermined threshold value set in advance, stopping the propulsion mechanism.

  (2) In the above (1), preferably, the second detection means includes a relative flow velocity detector that detects a relative flow velocity of the surrounding fluid with respect to the underwater moving body, and the external force calculation means includes the relative flow velocity detection. The fluid drag applied to the underwater moving body is calculated based on the detection result of the vessel.

(3) In the above (1) or (2) , preferably, the second detection means detects an acceleration of the underwater moving body directly related to an impact force generated when the underwater moving body collides with a surrounding structure. The external force calculating means calculates an impact force applied to the underwater moving body based on a detection result of the acceleration detector.

(4) In any one of the above (1) to (3) , preferably, the second detection means detects a state quantity directly related to a tension of a cable connected to the underwater moving body. The external force calculation means calculates a cable tension applied to the underwater moving body based on a detection result of the tension detection means.

(5) In any one of the above (1) to (4) , preferably, the underwater moving body has a winch that feeds and winds the cable, and the second detection means A rotation angle detector for detecting a rotation angle is provided, and the external force calculation means calculates a rotation moment applied to the underwater moving body based on a detection result of the rotation angle detector.

(6) In order to achieve the above object, the present invention includes an underwater mobile body having a propulsion mechanism that applies thrust to the main body, and a first detection unit that detects a state quantity related to the position and orientation of the underwater mobile body. An underwater vehicle control system comprising: a position / attitude calculation means for calculating the position and orientation of the underwater mobile body based on the detection result of the first detection means; provided in the underwater mobile body and applied to the main body A second detecting means for detecting a state quantity directly related to an external force other than the thrust of the propulsion mechanism; an external force calculating means for calculating an external force applied to the underwater moving body based on a detection result of the second detecting means; A propulsion mechanism control means for controlling the propulsion mechanism based on the position and posture of the underwater moving body calculated by the posture calculating means and the external force applied to the underwater moving body calculated by the external force calculating means; The exit means includes an acceleration detector that detects an acceleration of the underwater moving body directly related to an impact force generated when the underwater moving body collides with a surrounding structure, and the external force calculation means includes the acceleration detector Based on the detection result, the impact force applied to the underwater moving body is calculated.

(7) In the above (6), preferably, the second detection means has tension detection means for detecting a state quantity directly related to the tension of the cable connected to the underwater moving body, and the external force calculation means. Calculates the cable tension applied to the underwater moving body based on the detection result of the tension detecting means.
(8) In the above (6) or (7), preferably, the underwater moving body has a winch that feeds and winds the cable, and the second detection means detects a rotation angle of the winch. A rotation angle detector is provided, and the external force calculation means calculates a rotation moment applied to the underwater moving body based on a detection result of the rotation angle detector.

(9) In order to achieve the above object, the present invention includes an underwater mobile body having a propulsion mechanism that applies thrust to the main body, and a first detection unit that detects a state quantity related to the position and orientation of the underwater mobile body. An underwater vehicle control system comprising: a position / attitude calculation means for calculating the position and orientation of the underwater mobile body based on the detection result of the first detection means; provided in the underwater mobile body and applied to the main body A second detecting means for detecting a state quantity directly related to an external force other than the thrust of the propulsion mechanism; an external force calculating means for calculating an external force applied to the underwater moving body based on a detection result of the second detecting means; A propulsion mechanism control means for controlling the propulsion mechanism based on the position and posture of the underwater moving body calculated by the posture calculating means and the external force applied to the underwater moving body calculated by the external force calculating means; The exit means has tension detection means for detecting a state quantity directly related to the tension of the cable connected to the underwater moving body, and the external force calculation means is based on the detection result of the tension detection means. Calculate the cable tension applied to the body.
(10) In the above (9), preferably, the underwater moving body has a winch that feeds and winds the cable, and the second detection means detects a rotation angle of the winch. The external force calculation means calculates a rotational moment applied to the underwater moving body based on a detection result of the rotation angle detector.

(11) In order to achieve the above object, the present invention relates to a propulsion mechanism that imparts thrust to the main body, an underwater moving body that has a winch that feeds and winds the cable, and the position and posture of the underwater moving body. An underwater vehicle control system comprising: a first detection unit that detects a state quantity involved; and a position / attitude calculation unit that calculates a position and an attitude of the underwater vehicle based on a detection result of the first detection unit. A second detecting means for detecting a state quantity directly related to an external force other than a thrust of the propulsion mechanism applied to the main body, and an external force applied to the underwater moving body based on a detection result of the second detecting means; On the basis of the external force applied to the underwater moving body calculated by the external force calculating means and the position and orientation of the underwater moving body calculated by the position / posture calculating means and the external force calculating means. Propulsion mechanism control means for controlling the propulsion mechanism, the second detection means has a rotation angle detector for detecting the rotation angle of the winch, and the external force calculation means is a detection result of the rotation angle detector. Based on the above, the rotational moment applied to the underwater moving body is calculated.

(12) In any one of the above (6) to (11), preferably, the second detection means includes a relative flow velocity detector that detects a relative flow velocity of the surrounding fluid with respect to the underwater moving body, The external force calculating means calculates a fluid drag applied to the underwater moving body based on the detection result of the relative flow velocity detector.

According to the present invention, the position and posture control of the underwater moving body can be improved .

  Hereinafter, an in-reactor inspection system that is a preferred embodiment of the underwater vehicle control system of the present invention will be described with reference to the drawings.

  A first embodiment of the present invention will be described with reference to FIGS.

  FIG. 1 is a schematic diagram illustrating an example of the equipment layout of the in-reactor inspection system according to the present embodiment.

  In FIG. 1, a nuclear reactor 1 includes structures such as a shroud 2, an upper lattice plate 3, a core support plate 4, and a shroud support 5, and a PLR (Primary Loop Re-circulation System). (Circulation system) Piping such as piping 6 is connected. Above the reactor 1 is an operation floor 7 which is a work space, and above the same is a fuel changer 8.

  The in-reactor inspection system of the present embodiment performs sending and winding of an underwater inspection device 9 (underwater moving body) used for visual inspection of the structure inside the reactor 1 and a cable 10 connected to the underwater inspection device 9. A ground support device 11 having a winch 29 (see FIG. 2 to be described later), a control device 12 connected to the ground support device 11, and a camera image from the underwater inspection device 9 connected to the control device 12 is displayed. At the same time, a display device 13 for displaying the position, posture, etc. of the underwater inspection device 9 and operation levers 42a and 42b (see FIG. 6 described later) connected to the control device 12 and capable of operating the underwater inspection device 9 and the ground support device 11. ).

  For example, when performing a visual inspection work on the structure inside the nuclear reactor 1, the inspector 15 on the operation floor 7 puts the underwater inspection device 9 into the nuclear reactor 1 and determines the position and posture of the underwater inspection device 9. The controller 14 is operated while checking with the display device 13.

  FIG. 2 is a schematic diagram illustrating the configuration of the ground support apparatus 11.

  In FIG. 2, the ground support device 11 detects a rotation angle of the winch 29 having a drum 30 around which the cable 10 is wound and a motor 31 that drives the drum 30, and a rotation angle of the winch 29 ( And an encoder 32 for outputting a pulse signal) to the control device 12. A slip ring (not shown) is provided between the rotating portion and the fixed portion of the drum 30, and the cable 10 on the underwater inspection device 9 side and the cable on the control device 12 side are connected via this slip ring. ing.

  FIG. 3 is a schematic diagram showing the configuration of the underwater inspection apparatus 9.

  In FIG. 3, the underwater inspection device 9 is provided on the upper surface side (upper side in FIG. 3), rear surface side (right side in FIG. 3), and left side surface (front side toward the paper surface in FIG. 3). Three thrusters 28 (propulsion mechanisms) are provided. The thruster 28 includes a screw and a motor (not shown) that drives the screw to rotate forward or backward, thrust in the vertical direction (vertical direction in FIG. 3), and front-back direction (horizontal direction in FIG. 3). And a thrust in the left-right direction (perpendicular to the paper surface in FIG. 3). Thereby, the underwater inspection apparatus 9 can swim underwater in three dimensions. In the following, the coordinate system of the underwater inspection apparatus 9 is such that the left direction of the main body (front direction toward the paper surface in FIG. 3) is the X axis positive direction, the front direction (left direction in FIG. 3) is the Y axis positive direction, and the bottom direction. The description will be made assuming that the direction (downward direction in FIG. 3) is defined as the positive direction of the Z axis.

  The underwater inspection apparatus 9 includes an inertial sensor unit 16, cameras 21a and 21b, and a depth sensor (pressure sensor) 23 as first detection means for detecting a state quantity related to the position and orientation of the main body. The inertial sensor unit 16 includes three gyros 17 that detect angular velocities around the X, Y, and Z axes, an inclinometer 18 that detects angles (tilt angles) around the X and Y axes, and a Z axis. And a geomagnetic sensor 19 for detecting a surrounding angle (azimuth angle).

  On the front side of the main body (left side in FIG. 3), the camera 21a and a slit laser light source 22a arranged so that the imaging direction of the camera 21a coincides with the optical axis are provided. The image capturing unit 20a captures, for example, an image of an object in which a slit laser image is reflected from the camera 21a, converts the image into electronic information, and outputs it. Similarly, on the rear surface side of the main body, the camera 21b and a slit laser light source 22b arranged so that the imaging direction of the camera 21b coincides with the optical axis are provided. The image capturing unit 20b captures an image of an object on which, for example, a slit laser image is reflected from the camera 21b, converts the image into electronic information, and outputs it.

  In addition, as a major feature of the present embodiment, the underwater inspection apparatus 9 is an ambient fluid (cooling water) with respect to the underwater inspection apparatus 9 as a second detection unit that detects a state quantity directly related to an external force other than the thrust of the thruster 28 applied to the main body. For detecting the acceleration of the underwater inspection device 9 directly related to the impact force generated when the underwater inspection device 9 collides with a structure in the reactor 1. A piezoelectric acceleration sensor 25 (acceleration detector) and a tension detector 26 (first tension detection means) for detecting a state quantity directly related to the tension of the cable 10 on the underwater inspection apparatus side are provided. In addition, since the cooling water in the nuclear reactor 1 mainly flows in the vertical direction, the anemometer 24 is disposed on the upper surface side and the lower surface side of the main body.

  4 is a perspective view showing the structure of the tension detector 26, and FIG. 5 is a cross-sectional view of the tension detector 26 taken along the section VV in FIG.

  4 and 5, the tension detector 26 includes a cylindrical member 35 fixed to the main body of the underwater inspection device 9, an auxiliary cable 33 attached so as to be inserted through the cylindrical member 35, and the auxiliary cable. Two distortions provided at the tip of 33 and opposed to the inner peripheral surface of the cylindrical member 35 and a fixing clip 34 for fixing the cable 10 so that the cable 10 is connected to the underwater inspection device 9 in a slack state. And a gauge 36. Then, the auxiliary cable 33 receives a stress caused by the tension of the cable 10 in the underwater inspection device, and accordingly, a slight distortion occurs in the cylindrical member 35, and the distortion gauge 36 detects the distortion as a change in electric resistance. Yes.

  Returning to FIG. 3 described above, the underwater inspection apparatus 9 is provided with a signal transmission unit 27. Inertial sensor unit 16 (specifically, gyro 17, inclinometer 18, geomagnetic sensor 19), image capturing units 20a and 20b, depth sensor 23, velocimeter 24, acceleration sensor 25, and tension detector 26 (in detail) The detection signal and the image signal from the strain gauge 36) are output to the control device 12 via the signal transmission unit 27, the cable 10, and the support device 11. The thruster 28 is driven according to a control signal from the control device 12 via the support device 11, the cable 10, and the signal transmission unit 27.

  FIG. 6 is a block diagram illustrating a functional configuration of the control device 12.

  In FIG. 6, the control device 12 calculates angular velocities around the X, Y, and Z axes based on the angular velocity signal from the gyro 17, and based on this, the rotation angles around the X, Y, and Z axes. Is calculated based on the angle signal from the rotation angle calculation unit 37 and the inclinometer 18 and the azimuth angle around the Z axis is calculated based on the angle signal from the geomagnetic sensor 19. An inclination angle / azimuth angle calculation unit 38 and an attitude displacement calculation unit 39 that calculates the attitude displacement of the underwater inspection apparatus 9 based on the calculated rotation angle, inclination angle, and azimuth angle are provided. An image center distance calculation unit 43 that calculates the distance between the object near the image center and the cameras 21a and 21b (hereinafter referred to as image center distance) for the images from the image capturing units 20a and 20b. An image position displacement calculation unit 44 for calculating the displacement of the center position of the image (hereinafter referred to as image position displacement), and the position displacement of the underwater inspection apparatus 9 is calculated based on the calculated image center distance and image position displacement. And a position displacement calculation unit 45. Moreover, it has the depth calculating part 41 which calculates the depth of the underwater inspection apparatus 9 based on the pressure signal from the depth sensor 23. FIG. Further, a position / posture calculation unit 46 that calculates the absolute position / absolute posture of the underwater inspection device 9 based on the calculated posture displacement, position displacement, and depth of the underwater inspection device 9 and outputs the calculation result to the display device 13. have. Although not shown in FIG. 6 for the sake of convenience, the images from the image capturing units 20a and 20b are output to the display device 13 and displayed.

  Further, the control device 12 applies an external force (in detail) applied to the underwater inspection device 9 based on the flow velocity signal from the anemometer 24, the acceleration signal from the acceleration sensor 25, and the strain signal from the strain gauge 36 of the tension detector 26. Generates an external force calculation unit 40 that calculates fluid drag, impact force, and tension of the cable 10 underwater inspection device), and a command for controlling the thruster 28 of the underwater inspection device 9 and the winch 29 of the ground support device 11. A central control unit 50 and a thruster control unit 51 that controls the thruster 28 in accordance with a command from the central control unit 50 are provided.

  The control device 12 includes a winch rotation angle calculation unit 47 that calculates the rotation angle of the winch 29 based on the rotation angle signal from the encoder 32 of the ground support device 11, a motor drive current detection circuit, and the like (second tension detection). A support device-side tension calculating unit 49 that calculates the ground support device-side tension of the cable 10 based on the drive current signal of the winch 29 detected by the means), and a winch that controls the winch 29 in accordance with a command from the central control unit 50. And a control unit 52.

  In addition, the central control unit 50 has a cable abnormality determination function for determining whether or not an abnormality has occurred in the cable 10 and a control mode of the winch 29 based on the underwater inspection device side tension and the ground support device side tension of the cable 10 described above. And a winch control mode setting function for setting to either a coupled control mode for controlling in conjunction with the control of the thruster 28 or a manual control mode for controlling in accordance with the operation of the operation lever 42b for the support device. ing.

  Next, the control procedure of the control device 12 in the inspection work will be described. FIG. 7 is a PAD diagram showing the control processing contents of the control device 12.

  In FIG. 7, first, in step 60, the initial position / initial posture of the underwater inspection apparatus 9 is input, and the process proceeds to step 61 to shift to an operation process of the underwater inspection apparatus 9. First, in step 62, the target moving speed of the underwater inspection apparatus 9 is input, and the process proceeds to step 63 to move to position / posture calculation processing. In this position / orientation calculation process, first, in step 64, an attitude displacement calculation process is performed, the process proceeds to step 65, a position displacement calculation process is performed, and the process proceeds to step 66, where an absolute position / absolute attitude calculation process is performed. When this position / posture calculation processing is completed, the routine proceeds to step 67, where external force calculation processing is performed, and then processing proceeds to step 68, where thruster control processing is performed. In step 69, the winch control process of the ground support apparatus 11 is performed. Details of each control process will be described below.

(1) Posture Displacement Calculation Processing FIG. 8 is a PAD diagram showing details of the posture displacement calculation processing in step 64 shown in FIG.

  In the attitude displacement calculation process, first, at step 70, the rotation angle calculation unit 37 takes in the angular velocity signal of the gyro 17 of the underwater inspection device 9, and the tilt angle / azimuth angle calculation unit 38 sets the inclinometer 18 and The angle signal of the geomagnetic sensor 19 is captured.

  Then, the process proceeds to step 71, where the rotation angle calculation unit 37 proceeds to a rotation angle calculation process for calculating a rotation angle around each axis (X axis, Y axis, Z axis) from the angular velocity signal of the gyro 17. The gyro 17 of the present embodiment is a ceramic gyro that outputs a positive voltage value, and outputs an increase / decrease value from a reference voltage (a constant voltage value) in proportion to the angular velocity. The reference voltage is normally shown as a specific specification of the gyroscope 17, but in the present embodiment, a voltage value measured in advance when the angular velocity signal is not inputted is averaged. First, at step 72, basic processing of the angular velocity signal of the gyro 17 is performed to calculate the angular velocity. Then, the process proceeds to step 73, and the rotation angle is calculated by multiplying the calculated angular velocity by the signal capture interval Δt. These steps 72 and 73 are repeatedly performed to calculate the rotation angle around each axis (X axis, Y axis, Z axis).

  When the rotation angle calculation process of step 71 is completed, the process proceeds to step 74, where the inclination angle / azimuth angle calculation unit 38 calculates the inclination angle around each axis (X axis, Y axis) from the angle signal of the inclinometer 18. Move on to angle calculation processing. The inclinometer 18 of the present embodiment converts a change in the liquid level of the enclosed electrolyte (inclination angles around the X axis and the Y axis) into a voltage change and outputs the voltage change. First, in step 75, basic processing is performed for subtracting a reference voltage (a constant voltage value indicated as a specific specification of the inclinometer 18) from the signal of each axis (X axis, Y axis). Then, the process proceeds to step 76, and the inclination angle is calculated by multiplying the fundamentally processed signal by an inclination angle conversion coefficient (a constant value indicated as a specific specification of the inclinometer 18). These steps 75 and 76 are repeated to calculate the tilt angle around each axis (X axis, Y axis).

When the tilt angle calculation process of step 74 is completed, the process proceeds to step 77, and the tilt angle / azimuth angle calculation unit 38 proceeds to an azimuth angle calculation process for calculating an azimuth angle around the Z axis from the angle signal of the geomagnetic sensor 19. The geomagnetic sensor 19 of the present embodiment outputs a magnetic force captured by a Hall element having sensitivity in the X-axis direction and the Y-axis direction. First, in step 78, a basic process is performed in which the reference voltage is subtracted from the X-axis and Y-axis geomagnetic signals and multiplied by a gain. Since the reference voltage and gain vary depending on the environment in which the geomagnetic sensor 19 is used, those measured in advance in the area to be used are used. Then, the process proceeds to step 79, and the azimuth angle θm around the Z-axis is calculated by the following equation (1) using the X-axis and Y-axis signals Mx and My that have been subjected to basic processing.

  When the tilt angle calculation process in step 77 is completed, the process proceeds to step 80, where the posture calculation unit 39 rotates the rotation angles around the X axis, Y axis, and Z axis, the tilt angles around the X axis and Y axis, and the Z axis. The surrounding azimuth angle is input to a Kalman filter (known as this type, see, for example, Patent Document 1 above), and the optimum rotation angle around the X, Y, and Z axes (ie, underwater inspection) The attitude displacement of the device 9 is calculated. When the procedure of step 77 is finished, the posture displacement calculation process is finished.

(2) Position Displacement Calculation Processing FIG. 9 is a PAD diagram showing details of the position displacement calculation processing in step 65 shown in FIG.

  In the position displacement calculation process, first, in step 81, the image center distance calculation unit 43 and the image position displacement calculation unit 44 capture the images of the image capture units 20a and 20b, and proceed to step 82 to enter the image capture unit 20a, It is determined whether or not there is a laser image in the image 20b. For example, when there is a laser image in one of the images of the image capturing units 20a and 20b, the process proceeds to step 84 via step 83, and the position displacement of the camera is calculated based on the image with the laser image (details). For example, refer to the above-mentioned Patent Document 1.) For example, the case where the image of the image capturing unit 20a has a laser image will be described in detail. The image center distance calculation unit 43 includes the position of the laser image in the image, the installation interval between the camera 21a and the slit laser light source 22a, and the camera. The distance between the object near the center of the image and the camera 21a is obtained from the viewing angle of 21a. Further, the image position displacement calculation unit 44 reads from the current image and storage unit (for example, a memory, not shown) while reducing the influence of the posture displacement of the underwater inspection apparatus 9 calculated by the posture displacement calculation unit 39. A two-dimensional correlation process is performed on the previous image, and an image position displacement is obtained from a shift of the same object in the current image and the previous image. Then, the position displacement calculation unit 45 calculates the position displacement in the screen direction of the camera 21a (that is, the position displacement in the X axis direction and the Z direction of the underwater inspection apparatus 9) based on the calculated image position displacement and the like. Further, the position displacement calculation unit 45 calculates the position displacement in the imaging direction of the camera 21a (that is, the underwater inspection device 9) based on the difference between the calculated current image center distance and the image center distance in the previous image read from the storage unit. (Position displacement in the Y-axis direction) is calculated. Then, it progresses to step 85 and preserve | saves the position displacement of the calculated underwater inspection apparatus 9 in a memory | storage part.

  In step 82, for example, if there are laser images in both of the images of the image capturing units 20a and 20b, the process proceeds to step 86 through step 83, and the positional displacement of the cameras 21a and 21b is determined based on the respective images. In step 87, the position displacement calculation unit 45 calculates the position displacement of the underwater inspection apparatus 9 by arithmetically averaging the position displacements of the cameras 21a and 21b. Then, it progresses to the above-mentioned step 85 and preserve | saves the calculated position displacement of the underwater inspection apparatus 9 in a memory | storage part.

In step 82, for example, when there is no laser image in the images of the image capturing units 20a and 20b, the process proceeds to step 88, where the position displacement calculation unit 45 determines the position of the underwater inspection device 9 at the previous time (time t-1). The displacement ΔLt-1 and the position displacement ΔLt-2 of the underwater inspection device 9 of the previous time (time t-2) are read from the storage unit, and the process proceeds to step 89, where interpolation processing is performed according to the following equation (2). The position displacement ΔLt of the device 9 is estimated. Then, it progresses to the above-mentioned step 85 and preserve | saves the calculated position displacement of the underwater inspection apparatus 9 in a memory | storage part. When the procedure of step 85 ends, the position displacement calculation process ends.

(3) Absolute Position / Absolute Posture Calculation Processing FIG. 10 is a PAD showing details of the absolute position / absolute posture calculation processing in step 66 shown in FIG.

  In the absolute position / absolute posture calculation processing, first, in step 90, the position / posture calculation unit 46 fetches the posture displacement calculated by the posture displacement calculation unit 39, proceeds to step 91, and the previous absolute value read from the storage unit. The current absolute posture of the underwater inspection apparatus 9 is calculated by adding the posture displacement to the posture (initially the initial posture).

And it progresses to step 92 and the depth calculating part 41 performs the depth calculating process which calculates the depth of the underwater inspection apparatus 9. FIG. In this depth calculation process, first, in step 93, the pressure signal of the depth sensor 23 is captured, and the process proceeds to step 94, where a reference voltage (a constant voltage value indicated as a specific specification of the depth sensor 23) is subtracted from the pressure signal. The pressure P is calculated by multiplying the pressure conversion coefficient (a constant value indicated as the specific specification of the depth sensor 23). Then, the process proceeds to step 95, and the depth H of the underwater inspection apparatus 9 is calculated by the following equation (3) using the calculated pressure P, the density ρ of the coolant in the nuclear reactor 1, and the gravitational acceleration g.

  Then, the process proceeds to step 96, where the position / posture calculation unit 46 proceeds to an absolute position calculation process for calculating the current absolute position of the underwater inspection apparatus 9. In this absolute position calculation processing, first, in step 97, the position displacement in the Z-axis direction calculated by the position displacement calculation unit 45 and the depth calculated by the depth calculation unit 41 are fetched, and the process proceeds to step 98, from the storage unit. The position displacement is added to the last read absolute position in the Z-axis direction (initially the initial position), and the value and depth are input to the Kalman filter to calculate the optimum value of the absolute position in the Z-axis direction. In step 99, the position displacement in the X-axis and Y-axis directions calculated by the position-displacement calculating unit 45 is fetched. In step 100, the absolute values in the previous X-axis and Y-axis directions read from the storage unit are read. The absolute position in the X-axis and Y-axis directions is calculated by adding the position displacement to the position (initially the initial position).

  Then, it progresses to step 101 and preserve | saves the absolute position and absolute attitude | position of the calculated present underwater inspection apparatus 9 in a memory | storage part. When the procedure of step 101 is completed, the absolute position / absolute posture calculation process ends.

(4) External Force Calculation Processing FIG. 11 is a PAD diagram showing details of the external force calculation processing in step 67 shown in FIG.

  In the external force calculation processing, first, in step 102, the external force calculation unit 40 takes in the flow velocity signal of the velocimeter 24, the acceleration signal of the acceleration sensor 25, and the strain signal of the strain gauge 36.

And it progresses to step 103 and the external force calculating part 40 transfers to the fluid drag calculation process which calculates the fluid drag of the Z-axis direction added to the underwater test | inspection apparatus 9. FIG. The anemometer 24 of the present embodiment uses a propeller-type anemometer, and outputs the propeller rotational speed in proportion to the relative flow velocity of the coolant. First, at step 104, the relative speed Vf of the coolant is calculated by multiplying the detected propeller rotational speed by a flow rate conversion coefficient (a constant value indicated as a specific specification of the anemometer 24). Then, the process proceeds to step 105, where the calculated relative flow velocity Vf of the coolant, the density ρ of the coolant, the area S of the body that receives the fluid resistance, and the fluid drag coefficient Cf are used to calculate the fluid drag Ff according to the following equation (4). Is calculated. In this embodiment, it is approximated that the main body has a cubic shape, and Cf = 1.05 is used.

Then, the process proceeds to step 106, where the external force calculation unit 40 calculates the impact force in the direction of each axis (X axis, Y axis, Z axis) generated when the underwater inspection device 9 collides with the reactor 1 internal structure. Move to force calculation processing. In this impact force calculation process, first, in step 107, the gravitational acceleration component is removed from the acceleration signal of the acceleration sensor 25 using a high-pass filter, and an acceleration conversion coefficient (a constant value indicated as a specific specification of the acceleration sensor 25) is obtained. multiplied by calculated acceleration a _cc directly related to the impact force. Then, the process proceeds to step 108, where the average acceleration at the sampling interval Δt is calculated by the following equation (5). Thereafter, the impact force Fi is calculated by multiplying the average acceleration by the mass Mr according to the following equation (6). These step 107 and step 108 are repeatedly performed to calculate the impact force in each axis (X axis, Y axis, Z axis) direction.

  Then, the process proceeds to step 109, where the external force calculation unit 40 multiplies the strain signal of the strain gauge 36 by a force conversion coefficient (a constant value indicated as a specific specification of the strain gauge 36) to obtain the underwater inspection device side tension Ftr of the cable 10. calculate. When the procedure of step 109 ends, the external force calculation process ends.

(5) Thruster Control Processing FIG. 12 is a PAD diagram showing details of the thruster control processing in step 68 shown in FIG. FIG. 13 is a block diagram for explaining a control system related to the control of the thruster 28 of the underwater inspection apparatus 9.

  In the thruster control process, first, at step 110, the central processing control unit 50 determines the absolute position / attitude calculated by the position / posture calculation unit 46 and the external force calculated by the external force calculation unit 40 (specifically, the fluid drag force Ff). , Impact force Fi, and underwater inspection device side tension Ftr) of the cable 10.

  In step 111, the central processing control unit 50 determines whether or not the impact force Fi is greater than a predetermined threshold value. For example, when the impact force Fi is larger than the predetermined threshold value, the routine proceeds to step 112 where the thruster command thrust is set to zero, the routine proceeds to step 113, and the command signal is output to the thruster control unit 51. In response to this, the thruster control unit 51 sets the motor command voltage to zero and stops the thruster 28 of the underwater inspection apparatus 9.

In step 111, for example, when the impact force Fi is equal to or smaller than a predetermined threshold value, the process proceeds to step 114, and the thruster command thrust is calculated. More specifically, the central processing control unit 50 receives the target speed of the underwater inspection apparatus 9 according to the operation of the operation lever 42a for the inspection apparatus of the controller 14, and the target speed and the current speed (in detail, The difference from the value obtained by dividing the position displacement calculated as described above by the sampling interval is calculated. Further, the acceleration a is calculated by differentiating the current speed, and the displacement Δx between the reference position and the actual absolute position is calculated. And the thruster command thrust given to the underwater inspection apparatus 9 is calculated by the underwater inspection apparatus model shown by the following formula (7).

  The first term on the right side of Equation (7) represents an inertia term, and M is an inertia coefficient. The second term on the right side represents the viscosity term, and D is the viscosity coefficient. The third term on the right side represents an elastic term, and K is an elastic coefficient. Since these coefficients M, D, and K vary depending on the shape of the underwater inspection apparatus 9 and the usage environment, those calculated in advance through experiments are used. Since the left side of Expression (7) is the total force F applied to the underwater inspection apparatus 9, the above-described external force calculated from the total force F (specifically, the fluid drag Ff, the impact force Fi, and the cable 10). The thruster command thrust (thrust of each thruster 28 and its direction) is calculated. Thereafter, the process proceeds to step 113 described above, and the command signal is output to the thruster control unit 51. The thruster control unit 51 outputs a corresponding motor command voltage, and drives and controls the thruster 28 of the underwater inspection apparatus 9. When the procedure of step 113 ends, the thruster control process ends.

(6) The winch control process of the ground support apparatus FIG. 14 is a PAD showing the details of the winch control process of the ground support apparatus 11 in step 69 shown in FIG. FIG. 15 is a block diagram for explaining a control system related to the control of the winch 29 of the ground support apparatus 11.

In the winch control process of the ground support apparatus 11, first, in step 115, the support apparatus side tension calculation unit 49 proceeds to the calculation process of the ground support apparatus side tension of the cable 10. First, at step 116, the drive current signal of the winch 29 of the ground support apparatus 11 is captured, and the drive torque signal is multiplied by a torque conversion coefficient to calculate the rotational torque Mw of the winch 29. Then, the process proceeds to step 117, and the ground support device side tension Ftw of the cable 10 is calculated by the following equation (8) using the rotational torque Mw of the winch 29 and the cable winding radius R.

  Then, proceeding to step 118, the central processing control unit 50 compares the underwater inspection device side tension Ftr of the cable 10 calculated by the external force calculation unit 40 with a preset first threshold value Fs1, and also supports the support device side tension. The ground support device side tension Ftw of the cable 10 calculated by the calculation unit 49 is compared with a preset second threshold value Fs2, and it is determined whether or not an abnormality has occurred in the cable 10 based on the comparison result. Accordingly, the control mode of the winch 29 is set to the coupled control mode or the manual control mode (see FIG. 16).

  More specifically, for example, when the underwater inspection device side tension Ftr is greater than the first threshold value Fs1 and the ground support device side tension Ftw is greater than the second threshold value Fs2, it is determined that the cable 10 is in a normal tension state. The coupled control mode is set and the process proceeds to step 119, where the coupled control mode display signal is output to the display device 13 for display. Thereafter, the process proceeds to step 120, and for example, in order to send out the cable 10 according to the speed of the underwater inspection apparatus 9, the amount of delivery of the cable 10 corresponding to the movement amount (position displacement) of the underwater inspection apparatus 9 is calculated, and the winch command rotation is performed. Find the corner. Then, the difference between the winch command rotation angle and the current rotation angle calculated by the winch rotation angle calculation unit 47 is calculated, and the winch command angular velocity is calculated according to the difference, and the process proceeds to step 121, where the command signal Is output to the winch control unit 52. The winch control unit 52 outputs a corresponding motor command voltage to drive the winch 29 of the ground support apparatus 11 and send out the cable 10.

  For example, when the underwater inspection device side tension Ftr is equal to or less than the first threshold value Fs1 and the ground support device side tension Ftw is equal to or less than the second threshold value Fs2, it is determined that the cable 10 is in a normal slack state and coupled control is performed. The mode is set and the process proceeds to step 119 described above, and the display signal of the coupled control mode is output to the display device 13 for display. Thereafter, the process proceeds to step 120 described above to determine a winch command rotation angle for winding the cable 10 at a preset low speed, for example. Then, the difference between the winch command rotation angle and the current rotation angle calculated by the winch rotation angle calculation unit 47 is calculated, the winch command angular velocity is calculated according to the difference, and the process proceeds to step 121 described above. The command signal is output to the winch control unit 52. The winch control unit 52 outputs the corresponding motor command voltage to drive the winch 29 of the ground support apparatus 11 and wind the cable 10.

  On the other hand, in step 118, for example, when the underwater inspection device side tension Ftr is greater than the first threshold value Fs1 and the ground support device side tension Ftw is equal to or less than the second threshold value Fs2, or for example, the underwater inspection device side tension Ftr is the first threshold value. If it is less than Fs1 and the ground support device side tension Ftw is greater than the second threshold value Fs2, it is determined that an abnormality has occurred in the cable 10 and the manual control mode is set, and the process proceeds to step 122 to display the display signal of the manual control mode. It is output to the device 13 and displayed. Thereafter, the process proceeds to step 123, and a winch command rotation angle for sending out or winding the cable 10 at a speed corresponding to the operation of the support device operation lever 42b of the controller 14 is obtained. Then, the difference between the winch command rotation angle and the current rotation angle calculated by the winch rotation angle calculation unit 47 is calculated, the winch command angular velocity is calculated in accordance with this, and the process proceeds to step 121 described above. The signal is output to the winch control unit 52. The winch control unit 52 outputs a corresponding motor command voltage to drive the winch 29 of the ground support apparatus 11.

  In the above, the rotation angle calculator 37, the tilt angle / azimuth calculator 38, the attitude displacement calculator 39, the image center distance calculator 43, the image position displacement calculator 44, the position displacement calculator 45 of the control device 12, The depth calculator 41 and the position / posture calculator 46 constitute a position / posture calculator that calculates the position and posture of the underwater moving body based on the detection result of the first detector described in the claims. The external force calculation unit 40 of the control device 12 constitutes an external force calculation unit that calculates an external force applied to the underwater moving body based on the detection result of the second detection unit. The central processing control unit 50 and the thruster control unit 51 of the control device 12 operate the propulsion mechanism based on the position and posture of the underwater moving body calculated by the position / posture calculating means and the external force applied to the underwater moving body calculated by the external force calculating means. The propulsion mechanism control means to control is comprised.

  Further, the external force calculation unit 40 of the control device 12 constitutes first tension calculation means for calculating the underwater moving body side tension of the cable based on the detection result of the first tension detection means, and the support device side of the control device 12 The tension calculating unit 49 constitutes a second tension calculating unit that calculates the assist device side tension of the cable based on the detection result of the second tension detecting unit. Further, the central calculation control unit 50 of the control device 12 detects an abnormality in the cable based on the underwater moving body side tension of the cable calculated by the first tension calculation means and the support apparatus side tension of the cable calculated by the second tension calculation means. An abnormality determining means for determining whether or not has occurred. Moreover, the central processing control part 50 and the winch control part 52 of the control apparatus 12 comprise the winch control means which controls the winch of an assistance apparatus. Further, the central processing control unit 50 of the control device 12 controls the control mode of the winch control means according to the operation of the coupled control mode for controlling the winch control means coupled with the control of the propulsion mechanism control means and the operation of the winch operating means. Configures the control mode setting means to set to any of the manual control modes, and sets the coupled control mode when the abnormality determination means determines that there is no abnormality in the cable, and the abnormality determination means sets the abnormality in the cable. The control mode setting means is configured to set the manual control mode when it is determined that the occurrence has occurred.

  In the present embodiment configured as described above, the control device 12 is based on the detection results of the gyro 17, the inclinometer 18, and the geomagnetic sensor 19 of the underwater inspection device 9, the image processing results of the cameras 21a and 21b, and the like. The position and orientation of the underwater inspection device 9 are calculated. Further, based on the detection results of the anemometer 24, the acceleration sensor 25, and the tension detector 26 of the underwater inspection device 9, external forces applied to the underwater inspection device 9 (specifically, fluid drag, impact force, and underwater of the cable 10) (Inspection device side tension) is calculated. Thereby, the external force applied to the underwater inspection apparatus 9 can be detected directly and immediately, and the detection accuracy can be improved. Then, the control device 12 calculates a thruster command thrust based on the calculated external force applied to the underwater inspection device 9 and the position and orientation of the underwater inspection device 9, and drives the thruster 28 of the underwater inspection device 9 accordingly. Therefore, the accuracy of the position and posture control of the underwater inspection device 9 can be improved. Moreover, the control apparatus 12 stops the thruster 28 of the underwater inspection apparatus 9, for example, when the impact force produced when the underwater inspection apparatus 9 collides with the reactor 1 internal structure is large. As a result, even if the position and posture control of the underwater inspection apparatus 9 falls into an unstable state, the underwater inspection device 9 can be quickly returned to a stable state.

  Further, the control device 12 calculates the ground support device side tension of the cable 10 based on the drive current of the winch 29 of the ground support device 11, and based on the ground support device side tension of the cable 10 and the above-described underwater inspection device side tension. It is determined whether or not an abnormality has occurred in the cable 10. Then, the control mode of the winch 29 is set according to the determination result, and the set control mode is displayed on the display device 13. That is, for example, when an abnormality occurs such that the cable 10 connected to the underwater inspection apparatus 9 is caught by the reactor 1 internal structure or the like, the manual control mode is controlled according to the operation of the support lever operation lever 42b of the controller 14. And the manual control mode is displayed on the display device 13. Thereby, the inspector 15 can recognize and deal with the abnormality of the cable 10 and can prevent the cable 10 from being broken.

  In the first embodiment, it is determined whether or not the impact force applied to the underwater inspection device 9 exceeds a predetermined threshold value, and if the predetermined threshold value is exceeded, the thruster 28 of the underwater inspection device 9 is determined. However, the present invention is not limited to this. That is, for example, it may be determined whether or not the fluid resistance applied to the underwater inspection apparatus 9 exceeds a predetermined threshold value, and the thruster 28 of the underwater inspection apparatus 9 may be stopped when the predetermined threshold value is exceeded. . Further, for example, it may be determined whether the tension of the cable 10 on the underwater inspection device side exceeds a predetermined threshold value set in advance, and the thruster 28 of the underwater inspection device 9 may be stopped when the predetermined threshold value is exceeded. . In these cases, the same effect as described above can be obtained.

  Further, in the first embodiment, the underwater inspection device side tension of the cable 10 is compared with the first threshold value, the support device side tension of the cable 10 is compared with the second threshold value, and the cable 10 is determined based on the comparison result. Although the case where it is determined whether or not an abnormality has occurred has been described as an example, the present invention is not limited to this. That is, for example, an index value representing the relationship between the underwater inspection device side tension and the support device side tension of the cable 10 is calculated, and this index value is compared with a corresponding threshold value to determine whether or not an abnormality has occurred in the cable 10. You may make it do. In such a case, the same effect as described above can be obtained.

  In the first embodiment, the control device 12 determines whether or not an abnormality has occurred in the cable 10, and sets the control mode of the winch 29 as a result of the determination and displays it on the display device 13 as an example. However, this is not a limitation. That is, for example, the control device 12 may only display the result of the abnormality determination on the display device 13, and the inspector 15 may switch the control mode of the winch 29 with an input means or the like. In this case, the same effect as described above can be obtained.

  Moreover, in the said 1st Embodiment, the control apparatus 12 displays the control mode of the set winch 29 on the display apparatus 13, and notifies that abnormality has arisen in the cable 10 by display of the manual control mode, for example However, the present invention is not limited to this. That is, for example, it may be notified that an abnormality has occurred in the cable 10 due to a buzzer. In this case, the same effect as described above can be obtained.

  Moreover, in the said 1st Embodiment, although demonstrated taking the underwater test | inspection apparatus 9 which can swim underwater as an example, it is not restricted to this. That is, for example, an underwater inspection apparatus that can move along the wall surface of the reactor 1 internal structure may be used. In this case, the same effect as described above can be obtained.

  A second embodiment of the present invention will be described with reference to FIGS. The in-reactor inspection system of the present embodiment is an embodiment including an underwater inspection device, an underwater support device, and a ground support device.

  FIG. 17 is a schematic diagram illustrating an example of the equipment layout of the in-reactor inspection system according to the present embodiment. In the present embodiment, the same parts as those in the first embodiment are denoted by the same reference numerals, and description thereof will be omitted as appropriate.

  The in-reactor inspection system of the present embodiment can be mounted with a small underwater inspection apparatus 143 capable of visual inspection inside the PLR pipe 6 of the reactor 1 and the underwater inspection apparatus 143, for example. A winch 141 having a winch 141 (see FIG. 18 to be described later) for feeding and winding the secondary cable 149 and a winch for feeding and winding the primary cable 150 connected to the underwater support device 142 A ground support device 151 (not shown), a control device 148 connected to the ground support device 151, and camera images from the underwater inspection device 143 and the underwater support device 142 connected to the control device 148 are displayed. And a display device 146 for displaying the positions and postures of the underwater inspection device 143 and the underwater support device 142, and a control device. It is connected to 148, and includes underwater inspection apparatus 143, underwater support device 142, and an operable controller 147 ground support apparatus 151.

  For example, when performing a visual inspection work of the PLR pipe 6 of the nuclear reactor 1, the inspector 15 on the operation floor 7 puts the underwater inspection device 143 and the underwater support device 142 into the nuclear reactor 1, and these underwater inspection devices. The controller 147 is operated while confirming the position and posture of the 143 and the underwater support device 142 with the display device 146. Specifically, first, the underwater support device 142 equipped with the underwater inspection device 143 is submerged to the vicinity of the PLR pipe 6. Then, the underwater inspection device 143 is detached from the underwater support device 142, and then, while the secondary cable 149 is sent from the underwater support device 142, the underwater inspection device 143 is navigated into the PLR pipe 6 to perform the inspection. At this time, the underwater support device 142 stops at a predetermined position set in the nuclear reactor 1 and supports the navigation of the underwater inspection device 143. In addition, after completion | finish of test | inspection, the underwater assistance apparatus 142 accommodates the underwater inspection apparatus 143, winding up the secondary cable 149, and comes to surface.

  FIG. 18 is a schematic diagram illustrating the configuration of the underwater support device 142 and the underwater inspection device 143.

  In FIG. 18, the underwater support device 142 has the same configuration as the underwater inspection device 9 of the first embodiment. The underwater support device 142 includes three thrusters 28 provided on the upper surface side (upper side in FIG. 18), rear surface side (right side in FIG. 18), and left side surface (front side in FIG. 18). Equipped with three-dimensional underwater swimming. Further, the underwater support device 142 is an inertial sensor unit (not shown, but similar to the inertial sensor unit 16 of the first embodiment described above), as a detecting means for detecting a state quantity related to the position and posture of the main body. , And a geomagnetic sensor), cameras 21a and 21b, and a depth sensor (not shown). Further, a slit laser light source 22a is provided on the front side of the main body (left side in FIG. 18) so that the imaging direction of the camera 21a coincides with the optical axis, and the imaging of the camera 21b is provided on the rear side of the main body. A slit laser light source 22b is provided so that the direction and the optical axis coincide.

  The underwater support device 142 detects the relative flow rate of the surrounding fluid (cooling water) with respect to the underwater support device 142 as detection means for detecting a state quantity directly related to an external force other than the thrust of the thruster 28 applied to the main body. For example, a semiconductor piezo type acceleration sensor (not shown) for detecting the acceleration of the underwater support device 142 directly related to the impact force generated when the underwater support device 142 collides with the reactor 1 internal structure, and a secondary cable. 150 a tension detector that detects a state quantity directly related to the tension on the underwater support device side (not shown, but is composed of a strain gauge or the like, similar to the tension detector 26 of the first embodiment). I have.

  The underwater support device 142 includes a drum (not shown) around which a secondary cable 149 connected to the underwater inspection device 143 is wound, and a winch 141 having a motor (not shown) for driving the drum, An encoder (not shown) that detects the rotation angle of the winch 141 and outputs the rotation angle signal is provided. This encoder constitutes detection means for detecting the state quantity directly related to the rotational moment applied to the underwater support device 142 and also constitutes detection means for detecting the state quantity related to the position of the underwater inspection device 143.

  The underwater inspection device 143 includes an inspection camera 160 provided on the front surface side (left side in FIG. 18) of the main body and a thruster 162 provided on the rear surface side (right side in FIG. 18) of the main body. The underwater inspection device 143 also includes three gyros (not shown) that detect angular velocities around three axes as detection means for detecting a state quantity related to the posture of the main body. Further, the underwater inspection device 143 is a tension means for detecting a state quantity directly related to the tension of the secondary cable 149 on the underwater inspection apparatus side as a detection means for detecting a state quantity directly related to an external force other than the thrust of the thruster 28 applied to the main body. A detector 161 (not shown in detail, but composed of a strain gauge or the like as in the tension detector 26 of the first embodiment) is provided.

  Next, the control procedure of the control device 148 when navigating the underwater support device 142 equipped with the underwater inspection device 143 will be described. FIG. 19 is a PAD diagram showing the control processing contents of the control device 148.

  In FIG. 19, first, in step 170, the initial position and initial posture of the underwater support device 142 are input, and the process proceeds to step 171, and the operation process of the underwater support device 142 is started. First, in step 172, the target moving speed of the underwater support device 142 is input, and the process proceeds to step 173 to perform position / posture calculation processing of the underwater support device 142. This position / orientation calculation process performs the same procedure as the position / orientation calculation process (see FIGS. 7 to 10 described above) of the first embodiment. That is, the attitude displacement of the underwater support device 142 is calculated based on the angular velocity signal of the gyro of the underwater support device 142, the angle signal of the inclinometer and the geomagnetic sensor, and the underwater support is based on the image processing of at least one of the cameras 21a and 21b. The position displacement of the device 142 is calculated, the depth of the underwater support device 142 is calculated based on the pressure signal of the depth sensor 23, and the underwater support device 142 of the underwater support device 142 is calculated based on the calculated posture displacement, position displacement, and depth of the underwater support device 142. Calculate absolute position and attitude.

  Then, it progresses to step 174 and the external force calculation process of the underwater assistance apparatus 142 is performed. This external force calculation process performs the same procedure as the external force calculation process of the first embodiment (see FIG. 11 described above). That is, based on the flow velocity signal of the flowmeter 24 of the underwater support device 142, the acceleration signal of the acceleration sensor, and the strain signal of the strain gauge, external forces applied to the underwater support device 142 (specifically, fluid drag, impact force, and primary cable) 150 underwater support device side tension) is calculated.

  Thereafter, the process proceeds to step 175, where thruster control processing of the underwater support device 142 is performed. This thruster control process performs the same procedure as the thruster control process (see FIG. 12 described above) of the first embodiment. That is, for example, when the impact force calculated in step 174 described above is larger than a predetermined threshold, the thruster 28 of the underwater support device 142 is stopped. On the other hand, for example, when the impact force is equal to or less than a predetermined threshold, the position and posture of the underwater support device 142 calculated in the above-described steps 173 and 174 and the external force applied to the underwater support device 142 (specifically, fluid drag force, impact force) , And the underwater support device side tension of the primary cable 150), the thruster command thrust is calculated, and the thruster 28 of the underwater support device 142 is driven accordingly.

  Thereafter, the process proceeds to step 176, and winch control processing of the ground support apparatus 142 is performed. This winch control process performs the same procedure as the winch control process (see FIG. 14 described above) of the first embodiment. First, the ground support device side tension of the primary cable 150 is calculated based on the winch drive current signal of the ground support device 151. Then, the underwater support device side tension of the primary cable 150 calculated in the above step 174 is compared with a preset first threshold value, and the ground support device side tension of the primary cable 150 and a preset second threshold value are compared. Are compared, and it is determined whether an abnormality has occurred in the primary cable 150 based on the comparison result, and the winch control mode of the ground support device 151 is set to the coupled control mode or the manual control mode according to the determination result ( (See FIG. 16 above).

  That is, for example, when the underwater support device side tension is greater than the first threshold and the ground support device side tension is greater than the second threshold, or for example, the underwater support device side tension is equal to or less than the first threshold and the ground support device side tension is the second threshold. If it is equal to or less than the threshold value, it is determined that the primary cable 150 is in a normal state, the coupled control mode is set, and a display signal for the coupled control mode is output to the display device 146 for display. On the other hand, for example, when the underwater support device side tension is larger than the first threshold value and the ground support device side tension is less than the second threshold value, or for example, the underwater support device side tension is less than the first threshold value and the ground support device side tension is the first threshold value. If it is greater than two thresholds, it is determined that an abnormality has occurred in the primary cable 150, the manual control mode is set, and a display signal for the manual control mode is output to the display device 146 for display.

  Next, a control procedure of the control device 148 when the underwater inspection device 143 that has left the underwater support device 142 is sailed while the underwater support device 142 is stopped will be described. FIG. 20 is a PAD diagram showing the control processing contents of the control device 148. FIG. 21 is a PAD diagram showing details of the external force calculation processing of the underwater support device in FIG. 20, and FIG. 22 is a PAD diagram showing details of the external force calculation processing of the underwater inspection device in FIG.

  20 to 22, first, in step 180, initial positions and initial postures of the underwater support device 142 and the underwater inspection device 143 are input, and the process proceeds to step 181 to move to an operation process of the underwater support device 142. First, in step 182, the position / posture calculation process of the underwater support device 142 (the same procedure as the position / posture calculation process of step 173 shown in FIG. 19 described above) is performed.

Then, it progresses to step 183 and moves to the external force calculation process of the underwater assistance apparatus 142. In this external force calculation processing, first, in step 184, the flow velocity signal of the flowmeter 24 of the underwater support device 142, the strain signal of the strain gauge of the tension detector, the drive current signal of the winch 141, and the rotation angle signal of the encoder are captured. Then, the process proceeds to step 185 to calculate the fluid drag in each axis (X axis, Y axis, Z axis) direction applied to the underwater support device 142 based on the flow velocity signal of the anemometer 24. Further, the process proceeds to step 186, and the underwater support device side tension of the primary cable 150 applied to the underwater support device 142 is calculated based on the strain gauge strain signal. Further, the process proceeds to step 187, where the rotational torque is calculated based on the drive current signal of the winch 141, and the underwater support device side tension of the secondary cable 149 applied to the underwater support device 142 is calculated. In step 188, the rotational angular velocity ωw is calculated based on the rotational angle signal of the encoder, and this rotational angular velocity ωw and the moment of inertia Iw of the winch 149 (calculated in advance by experiment because it varies depending on the shape and mass distribution of the winch 149). The rotational moment Fmw of the winch 149 is calculated by the following equation (9). When the procedure of step 188 ends, the external force calculation process ends.

  Thereafter, the process proceeds to step 189 to perform a thruster control process of the underwater support device 142. In this thruster control process, the position and posture of the underwater support device 142 calculated in the above-described steps 182 and 183 and the external force applied to the underwater support device 142 (specifically, the fluid drag, the underwater support device side tension of the primary cable 150, the two The thruster command thrust is calculated based on the underwater support device side tension of the next cable 149 and the rotational moment of the winch 141), and the thruster 28 of the underwater support device 142 is driven accordingly. When the procedure of step 189 ends, the operation process of the underwater support device 142 ends.

  Then, the process proceeds to step 191 and the operation process of the underwater inspection apparatus 143 is started. First, in step 192, the target moving speed of the underwater inspection apparatus 143 is input, and the process proceeds to step 193 to move to position / posture calculation processing of the underwater inspection apparatus 143. In this position / posture calculation processing, the amount of the secondary cable 149 delivered is calculated based on the rotation angle signal of the encoder of the underwater support device 142, and the position of the underwater inspection device 143 is calculated. Further, based on the gyro angular velocity signal of the underwater inspection apparatus 143, the rotation angle around the three axes is calculated, and the attitude of the underwater inspection apparatus 143 is calculated.

  Thereafter, the process proceeds to step 194, and the process proceeds to an external force calculation process of the underwater inspection apparatus 143. In this external force calculation processing, first, in step 195, the strain signal of the strain gauge of the underwater inspection device 143 is fetched, and the process proceeds to step 196, where the underwater inspection device side tension of the secondary cable 149 is calculated based on the strain signal. When the procedure of step 196 ends, the external force calculation process ends.

  Thereafter, the process proceeds to step 197, where thruster control processing of the underwater inspection apparatus 143 is performed. In this thruster control process, the thruster command thrust is calculated based on the position and posture of the underwater support device 142 calculated in step 193, taking into account the underwater support device side tension of the secondary cable 149 calculated in step 194. In response to this, the thruster 162 of the underwater support device 142 is driven.

  Then, it progresses to step 198 and the winch control process of the underwater assistance apparatus 142 is performed. This winch control process performs the same procedure as the winch control process (see FIG. 14 described above) of the first embodiment. First, the underwater support device side tension of the secondary cable 149 is calculated based on the drive current signal of the winch 141 of the underwater support device 142. Then, the underwater inspection device side tension of the secondary cable 149 calculated in the above-described step 197 is compared with a preset third threshold value, and the underwater support device side tension of the secondary cable 149 is set to the preset fourth threshold value. It compares with a threshold value, it is determined whether abnormality has arisen in the secondary cable 149 by those comparison results, and the control mode of the winch 141 of the underwater support device 142 is set to the coupled control mode or the manual control mode according to the determination result. (See FIG. 16 described above).

  That is, for example, when the underwater inspection device side tension is greater than the third threshold and the underwater support device side tension is greater than the fourth threshold, or for example, the underwater inspection device side tension is equal to or less than the third threshold and the underwater support device side tension is the fourth. If it is equal to or less than the threshold value, it is determined that the secondary cable 149 is in a normal state, the coupled control mode is set, and the coupled control mode display signal is output to the display device 146 for display. On the other hand, for example, when the underwater inspection device side tension is greater than the third threshold value and the underwater support device side tension is less than or equal to the fourth threshold value, or for example, the underwater inspection device side tension is less than the third threshold value and the underwater support device side tension is the first threshold value. When the threshold value is larger than four threshold values, it is determined that an abnormality has occurred in the secondary cable 149, the manual control mode is set, and a display signal for the manual control mode is output to the display device 146 for display.

  In the present embodiment configured as described above, for example, when the underwater support device 142 on which the underwater inspection device 143 is mounted is submerged, the control device 148 includes the gyroscope, the inclinometer, and the geomagnetic sensor of the underwater support device 142. Based on the detection result and the image processing results of the cameras 21a and 21b, the position and posture of the underwater support device 142 are calculated. Further, based on the detection results of the anemometer 24, the acceleration sensor, and the tension detector of the underwater support device 142, external forces applied to the underwater support device 142 (specifically, fluid drag, impact force, and underwater support of the primary cable 150) Calculate device side tension). Thereby, the external force applied to the underwater support device 142 can be detected directly and immediately, and the detection accuracy can be improved. Then, the control device 148 calculates a thruster command thrust based on the calculated external force applied to the underwater support device 142 and the position and orientation of the underwater support device 142, and drives the thruster 28 of the underwater support device 142 according to this. Therefore, the accuracy of the position and posture control of the underwater support device 142 can be improved. Further, the control device 148 stops the thruster 28 of the underwater support device 142 when the impact force generated when, for example, the underwater support device 142 collides with the internal structure of the reactor 1 is large. Thereby, even if the position or posture control of the underwater support device 142 falls into a unstable state, the underwater support device 142 can be quickly returned to a stable state.

  Further, the control device 148 calculates the ground support device side tension of the primary cable 150 based on the winch drive current of the ground support device 151, and calculates the ground support device side tension of the primary cable 150 and the above-described underwater support device side tension. Based on this, it is determined whether or not an abnormality has occurred in the primary cable 150. And according to the determination result, while setting the control mode of the winch of the ground assistance apparatus 151, the set control mode is displayed on the display device 146. That is, for example, when an abnormality occurs such that the primary cable 150 connected to the underwater support device 142 is caught by the internal structure of the reactor 1 or the like, manual control is performed according to the operation of the operation lever for the ground support device winch of the controller 146. The control mode is set and the manual control mode is displayed on the display device 13. Thereby, the inspector 15 can recognize and deal with the abnormality of the primary cable 150, and can prevent the primary cable 150 from being broken.

  On the other hand, for example, when the underwater inspection device 143 detached from the underwater support device 142 is submerged while the underwater support device 142 is stopped, the control device 148 includes the velocimeter 24 of the underwater support device 142, the tension detector, and the motor of the winch 141. Based on the detection result of the drive current detection circuit and the encoder, external force applied to the underwater support device 142 (specifically, fluid drag, underwater support device side tension of the primary cable 150, underwater support device side tension of the secondary cable 149, and Rotation moment) is calculated. Thereby, the external force applied to the underwater support device 142 can be detected directly and immediately, and the detection accuracy can be improved. Then, the control device 148 calculates a thruster command thrust for stopping the underwater support device 142 based on the calculated external force applied to the underwater support device 142 and the position and posture of the underwater support device 142, and in response to this, the underwater support The thruster 28 of the device 142 is driven. Therefore, the accuracy of the stop control of the underwater support device 142 can be improved.

  Further, the control device 12 calculates the position and orientation of the underwater inspection device 143 based on the detection results of the encoder of the underwater support device 142 and the gyro of the underwater inspection device 143. Further, based on the detection result of the tension detector 161 of the underwater inspection device 143, an external force applied to the underwater inspection device 143 (specifically, the underwater inspection device side tension of the secondary cable 149) is calculated. Thereby, the external force applied to the underwater inspection apparatus 143 can be detected directly and immediately, and the detection accuracy can be increased. Then, the control device 148 calculates the thruster command thrust based on the position and orientation of the underwater inspection device 143 in consideration of the underwater inspection device side tension of the secondary cable 149, and according to this, the thruster 28 of the underwater inspection device 143 is calculated. Drive. Therefore, the accuracy of the position and posture control of the underwater inspection device 143 can be improved.

  Further, the control device 12 calculates the underwater support device side tension of the secondary cable 149 based on the drive current of the winch 141 of the underwater support device 142, and based on the underwater inspection device side tension and the above-described underwater support device side tension, It is determined whether or not an abnormality has occurred in the next cable 149. And according to the determination result, while setting the control mode of the winch 141 of the underwater assistance apparatus 143, the set control mode is displayed on the display apparatus 146. That is, for example, when an abnormality occurs such that the secondary cable 149 connected to the underwater inspection device 143 is caught by a surrounding structure or the like, the manual control mode is controlled according to the operation of the operation lever for the underwater support device winch of the controller 146. And the manual control mode is displayed on the display device 146. Thereby, the inspector 15 can recognize and deal with the abnormality of the secondary cable 149, and can prevent the secondary cable 149 from being broken.

  In the second embodiment, the underwater inspection apparatus 143 uses the tension of the secondary cable 149 on the underwater inspection apparatus side as a detection means for detecting a state quantity directly related to an external force other than the thrust of the thruster 162 applied to the main body. Although the case where the tension detector 161 for directly detecting the state quantity directly related is described as an example, the present invention is not limited to this. That is, for example, a flowmeter that detects the relative flow velocity of the surrounding fluid (cooling water) with respect to the underwater inspection device 143 is provided, and the control device 148 calculates the fluid drag applied to the underwater inspection device 143 based on the detection result of the flowmeter. It may be. Further, for example, a semiconductor piezo type acceleration sensor for detecting the acceleration of the underwater inspection device 143 directly related to the impact force generated when the underwater inspection device 143 collides with a surrounding structure is provided, and the control device 148 includes the acceleration sensor. You may make it calculate the impact force added to the underwater inspection apparatus 143 based on a detection result. The control device 12 may stop the thruster 162 of the underwater inspection device 143, for example, when the external force exceeds a predetermined threshold value set in advance. In such a case, the same effect as described above can be obtained.

It is the schematic showing the equipment arrangement | positioning of the in-reactor inspection system by the 1st Embodiment of this invention as an example. It is the schematic showing the structure of the assistance apparatus in the 1st Embodiment of this invention. It is the schematic showing the structure of the underwater inspection apparatus in the 1st Embodiment of this invention. It is a perspective view showing the schematic structure of the tension detector of the underwater inspection apparatus in the 1st Embodiment of this invention. It is sectional drawing by the cross section VV in FIG. It is a block diagram showing the functional structure of the control apparatus in the 1st Embodiment of this invention. It is a PAD figure showing the control processing content of the control apparatus in the 1st Embodiment of this invention. FIG. 8 is a PAD diagram showing details of posture displacement calculation processing in FIG. 7. FIG. 8 is a PAD diagram showing details of the position displacement calculation process in FIG. 7. FIG. 8 is a PAD diagram showing details of absolute position / absolute posture calculation processing in FIG. 7. FIG. 8 is a PAD diagram showing details of the external force calculation process in FIG. 7. FIG. 8 is a PAD showing details of the thruster control process in FIG. 7. It is a block diagram for demonstrating the control system concerning the thruster control of the underwater inspection apparatus in the 1st Embodiment of this invention. FIG. 8 is a PAD diagram showing details of the winch control process of the ground support apparatus in FIG. 7. It is a block diagram for demonstrating the control system regarding the winch control of the ground assistance apparatus in the 1st Embodiment of this invention. It is a figure for demonstrating the cable abnormality determination function and winch control mode setting function of the control apparatus in the 1st Embodiment of this invention. It is the schematic showing the equipment arrangement | positioning of the in-reactor inspection system by the 2nd Embodiment of this invention as an example. It is the schematic showing the structure of the underwater assistance apparatus and underwater inspection apparatus in the 2nd Embodiment of this invention. It is a PAD figure showing the control processing content of the control apparatus in the case of navigating the underwater assistance apparatus in which the underwater inspection apparatus in the 2nd Embodiment of this invention is mounted. It is a PAD showing the control processing contents of the control device when navigating the underwater inspection device detached from the underwater support device while stopping the underwater support device in the second embodiment of the present invention. It is a PAD figure showing the detail of the external force calculation process of the underwater assistance apparatus in FIG. FIG. 21 is a PAD diagram showing details of an external force calculation process of the underwater inspection apparatus in FIG. 20.

Explanation of symbols

9 Underwater inspection equipment (underwater moving body)
10 Cable 11 Ground support device 12 Control device (position / attitude calculation means, external force calculation means, propulsion mechanism control means, winch control means, abnormality determination means, control mode setting means)
13 Display device (notification means)
17 Gyro (first detection means)
18 Inclinometer (first detection means)
19 Geomagnetic sensor (first detection means)
21a, 21b Camera (first detection means)
23 Depth sensor (first detection means)
24 Current meter (relative flow velocity detector, second detection means)
25 Acceleration sensor (acceleration detector, second detection means)
26 Tension detector (tension detection means, second detection means)
28 Thruster (propulsion mechanism)
29 winch 42b support lever operating lever (winch operating means)
141 winch 142 underwater support device (underwater vehicle, support device)
143 Underwater inspection equipment (underwater moving body)
146 Display device (notification means)
148 Control device (position / attitude calculation means, external force calculation means, propulsion mechanism control means, winch control means, abnormality determination means, control mode setting means)
149 Secondary cable 150 Primary cable 151 Ground support device

Claims (12)

An underwater moving body having a propulsion mechanism for applying thrust to a main body; a first detecting means for detecting a state quantity related to a position and posture of the underwater moving body; and the underwater moving body based on a detection result of the first detecting means. Underwater vehicle control system equipped with position / posture calculation means for calculating the position and posture of
A second detecting means provided on the underwater moving body for detecting a state quantity directly related to an external force other than a thrust of the propulsion mechanism applied to the main body;
An external force calculation means for calculating an external force applied to the underwater moving body based on a detection result of the second detection means;
A propulsion mechanism control means for controlling the propulsion mechanism based on the position and posture of the underwater moving body calculated by the position / posture calculating means and the external force applied to the underwater moving body calculated by the external force calculating means ;
The underwater vehicle control system, wherein the propulsion mechanism control means stops the propulsion mechanism when the external force calculated by the external force calculation means exceeds a predetermined threshold value set in advance . The underwater vehicle control system according to claim 1,
The second detection means has a relative flow velocity detector that detects a relative flow velocity of the surrounding fluid with respect to the underwater moving body,
The underwater moving body control system, wherein the external force calculating means calculates a fluid drag force applied to the underwater moving body based on a detection result of the relative flow velocity detector. In the underwater vehicle control system according to claim 1 or 2 ,
The second detection means includes an acceleration detector that detects an acceleration of the underwater moving body directly related to an impact force generated when the underwater moving body collides with a surrounding structure.
The underwater moving body control system, wherein the external force calculating means calculates an impact force applied to the underwater moving body based on a detection result of the acceleration detector. The underwater vehicle control system according to any one of claims 1 to 3 ,
The second detection means has tension detection means for detecting a state quantity directly related to the tension of the cable connected to the underwater moving body,
The underwater moving body control system, wherein the external force calculating means calculates a cable tension applied to the underwater moving body based on a detection result of the tension detecting means. The underwater vehicle control system according to any one of claims 1 to 4 ,
The underwater vehicle has a winch that feeds and winds the cable,
The second detection means includes a rotation angle detector that detects a rotation angle of the winch,
The underwater moving body control system, wherein the external force calculating means calculates a rotational moment applied to the underwater moving body based on a detection result of the rotation angle detector. An underwater moving body having a propulsion mechanism for applying thrust to a main body; a first detecting means for detecting a state quantity related to a position and posture of the underwater moving body; and the underwater moving body based on a detection result of the first detecting means. Underwater vehicle control system equipped with position / posture calculation means for calculating the position and posture of
A second detecting means provided on the underwater moving body for detecting a state quantity directly related to an external force other than a thrust of the propulsion mechanism applied to the main body;
An external force calculation means for calculating an external force applied to the underwater moving body based on a detection result of the second detection means;
A propulsion mechanism control means for controlling the propulsion mechanism based on the position and posture of the underwater moving body calculated by the position / posture calculating means and the external force applied to the underwater moving body calculated by the external force calculating means;
The second detection means includes an acceleration detector that detects an acceleration of the underwater moving body directly related to an impact force generated when the underwater moving body collides with a surrounding structure.
The underwater moving body control system, wherein the external force calculating means calculates an impact force applied to the underwater moving body based on a detection result of the acceleration detector. In the underwater vehicle control system according to claim 6,
The second detection means has tension detection means for detecting a state quantity directly related to the tension of the cable connected to the underwater moving body,
The underwater moving body control system, wherein the external force calculating means calculates a cable tension applied to the underwater moving body based on a detection result of the tension detecting means. The underwater vehicle control system according to claim 6 or 7,
The underwater vehicle has a winch that feeds and winds the cable,
The second detection means includes a rotation angle detector that detects a rotation angle of the winch,
The underwater moving body control system, wherein the external force calculating means calculates a rotational moment applied to the underwater moving body based on a detection result of the rotation angle detector. An underwater moving body having a propulsion mechanism for applying thrust to a main body; a first detecting means for detecting a state quantity related to a position and posture of the underwater moving body; and the underwater moving body based on a detection result of the first detecting means. Underwater vehicle control system equipped with position / posture calculation means for calculating the position and posture of
A second detecting means provided on the underwater moving body for detecting a state quantity directly related to an external force other than a thrust of the propulsion mechanism applied to the main body;
An external force calculation means for calculating an external force applied to the underwater moving body based on a detection result of the second detection means;
A propulsion mechanism control means for controlling the propulsion mechanism based on the position and posture of the underwater moving body calculated by the position / posture calculating means and the external force applied to the underwater moving body calculated by the external force calculating means;
The second detection means has tension detection means for detecting a state quantity directly related to the tension of the cable connected to the underwater moving body,
  The underwater moving body control system, wherein the external force calculating means calculates a cable tension applied to the underwater moving body based on a detection result of the tension detecting means. The underwater vehicle control system according to claim 9,
The underwater vehicle has a winch that feeds and winds the cable,
The second detection means includes a rotation angle detector that detects a rotation angle of the winch,
The underwater moving body control system, wherein the external force calculating means calculates a rotational moment applied to the underwater moving body based on a detection result of the rotation angle detector. An underwater moving body having a propulsion mechanism for applying thrust to the main body, a winch for feeding and winding the cable, first detection means for detecting a state quantity related to the position and orientation of the underwater moving body, and the first In an underwater mobile body control system comprising a position / attitude calculation means for calculating the position and orientation of the underwater mobile body based on the detection result of the detection means,
A second detecting means provided on the underwater moving body for detecting a state quantity directly related to an external force other than a thrust of the propulsion mechanism applied to the main body;
An external force calculation means for calculating an external force applied to the underwater moving body based on a detection result of the second detection means;
A propulsion mechanism control means for controlling the propulsion mechanism based on the position and posture of the underwater moving body calculated by the position / posture calculating means and the external force applied to the underwater moving body calculated by the external force calculating means;
The second detection means includes a rotation angle detector that detects a rotation angle of the winch,
The underwater moving body control system, wherein the external force calculating means calculates a rotational moment applied to the underwater moving body based on a detection result of the rotation angle detector. The underwater vehicle control system according to any one of claims 6 to 11,
The second detection means has a relative flow velocity detector that detects a relative flow velocity of the surrounding fluid with respect to the underwater moving body,
The underwater moving body control system, wherein the external force calculating means calculates a fluid drag force applied to the underwater moving body based on a detection result of the relative flow velocity detector.

Images