Mobile body control system, mobile body controller, and mobile body control method

Abstract

To provide a mobile body control system, a mobile body controller, and a mobile body control method capable of keeping an airframe in target posture without constructing an approximate formula for each posture of a drive part of a buoyancy material or the like.SOLUTION: A mobile body control system includes: a robot main body and a floating device including a drive part that has relatively smaller density than the robot main body and can change a buoyancy center of the floating device; an acquisition part that acquires a buoyancy center position and a gravity center position, an angle formed by the buoyancy center position and the gravity center position, and a posture angle of the robot main body; a target setting part that sets at least one of a target value of the angle formed by the gravity center position and a target value of the angle formed by the buoyancy center position; and a buoyancy center control part that calculates an amount of control of the drive part for changing the buoyancy center of the floating device by use of a deviation between the buoyancy center position and the gravity center position, a target value of the deviation between the buoyancy center position and the gravity center position, a gravity center sensitivity matrix indicating a change of the gravity center position with respect to the amount of control of the drive part, and a buoyancy center sensitivity matrix indicating a change of the buoyancy center position with respect to the amount of control of the drive part.SELECTED DRAWING: Figure 5

Description

[Technical Field]

[0001] The present invention relates to a mobile object control system, a mobile object control device, and a mobile object control method. [Background technology]

[0002] There is an underwater robot, which is a device that performs work underwater. Such an underwater robot has, for example, a frame, a propulsion unit, a weight, a buoyancy material, and a drive mechanism. The propulsion unit has multiple propulsors that generate propulsive force. The weight moves the center of gravity by moving in a first axial direction. The buoyancy material moves the center of buoyancy by moving in a second axial direction. The drive mechanism moves the weight in the first axial direction and the buoyancy material in the second axial direction in synchronization with each other (see, for example, Patent Document 1). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent No. 6167317 Summary of the Invention [Problem to be solved by the invention]

[0004] In conventional control methods, in order to keep the aircraft in a target attitude, for example, the relationship between the angle between the center of buoyancy and the center of gravity and the amount of operation of the buoyancy material is approximated to a first order, and the control angle amount of the buoyancy material is calculated, so it was necessary to construct an approximation formula for each attitude of the driving part, such as an arm.

[0005] The present invention has been made in consideration of the above-mentioned problems, and aims to provide a mobile body control system, a mobile body control device, and a mobile body control method that can maintain the aircraft in a target attitude without constructing an approximate equation for each attitude of a driving part such as a buoyancy material. [Means for solving the problem]

[0006] (1) In order to achieve the above object, a mobile object control system according to one aspect of the present invention is a system for controlling a robot that operates in water, the system including: a robot main body; a floating device that is connected to the robot main body and has a density relatively low compared to a main body, and that is equipped with a drive unit that can change the center of buoyancy of the floating device relative to the robot main body; an acquisition unit that acquires a center of buoyancy position of the robot main body and the floating device together; a center of gravity position of the robot main body and the floating device together; an angle between the center of buoyancy position and the center of gravity position; a target setting unit that sets at least one of a target value of the angle of the center of gravity position from a target posture of the entire robot and a target value of the angle of the center of buoyancy position from a target posture of the entire robot; * ), and a center of gravity sensitivity matrix (for example, J COB ), a center of buoyancy sensitivity matrix (for example, J COG ), and a buoyancy center control unit that calculates a control amount of the drive unit for changing the buoyancy center of the floating device using

[0007] (2) In one aspect of the mobile body control system of (1) above, the buoyancy control unit may perform feedback control by inputting a differential value of the attitude angle of the robot body as a velocity term into a target value of the angle (e.g., COX angle) between the buoyancy position and the center of gravity position.

[0008] (3) In the mobile body control system according to one aspect of (1) or (2) above, feedback control may be performed by incorporating an integral term of the deviation of the attitude angle of the robot body into a target value of the angle (e.g., COX angle) between the center of buoyancy position and the center of gravity position.

[0009] (4) In a mobile body control system according to any one of the above (1) to (3), the robot may have a movable part and a fixed part, the movable part being connected to the fixed part by a link that is movable in at least one of the pitch direction and the roll direction, and the buoyancy control part may control the buoyancy position or the center of gravity position of the robot body by moving the link in at least one of the pitch direction and the roll direction based on the calculated control amount of the drive part.

[0010] (5) In a mobile body control system according to any one of the above (1) to (3), the robot may include a movable part and a fixed part, the movable part including a member that controls the position of the center of buoyancy or the position of the center of gravity of the entire robot by moving two-dimensionally, and the buoyancy control part may control the position of the center of buoyancy or the position of the center of gravity of the robot body by moving the movable part in at least one of the pitch direction and the roll direction based on the calculated control amount of the drive part.

[0011] (6) In the mobile object control system according to the aspect of (1), the buoyancy center control unit adjusts a target value (e.g., φ) of an attitude angle (e.g., φ * ) to the target angle (θ * cox), and the converted target angle is converted into the distance (for example, l CoX ) to calculate the deviation of the target center of buoyancy and center of gravity position (Δx * ), and the deviation of the converted target buoyancy center and gravity center position and the deviation (Δx) between the buoyancy center position and the gravity center position are used to calculate the control amount (for example, q ACOX ) may be converted to

[0012] (7) In order to achieve the above object, a mobile body control device according to one aspect of the present invention is a mobile body control device for controlling a robot having a floating device that is relatively low in density compared to a main body connected to a robot main body that performs operation underwater, and that includes a drive unit that can change the center of buoyancy of the floating device relative to the robot main body, and includes an acquisition unit that acquires the combined center of buoyancy position of the robot main body and the floating device, the combined center of gravity position of the robot main body and the floating device, the angle between the center of buoyancy position and the center of gravity position, and an attitude angle of the robot main body. a target setting unit that sets at least one of a target value of the angle between the center of gravity position and the target posture of the entire robot and a target value of the angle between the center of buoyancy position and the target posture of the entire robot; and a buoyancy control unit that calculates the control amount of the drive unit for changing the center of buoyancy of the floating device using the deviation between the center of buoyancy position and the center of gravity position, the target value of the deviation between the center of buoyancy position and the center of gravity position, a center of gravity sensitivity matrix that indicates the change in the center of buoyancy position relative to the control amount of the drive unit, and a center of buoyancy sensitivity matrix that indicates the change in the center of buoyancy position relative to the control amount of the drive unit.

[0013] (8) In order to achieve the above object, a mobile body control method according to one aspect of the present invention is a control method for a mobile body control device that controls a robot having a floating device that is relatively low density compared to a main body connected to a robot main body that performs operation underwater, and that is equipped with a drive unit that can change the center of buoyancy of the floating device relative to the robot main body, wherein an acquisition unit acquires a center of buoyancy position of the robot main body and the floating device together, a center of gravity position of the robot main body and the floating device together, an angle between the center of buoyancy position and the center of gravity position, and an attitude angle of the robot main body. a target setting unit sets at least one of a target value of the angle between the center of gravity position and the target posture of the entire robot and a target value of the angle between the center of buoyancy position and the target posture of the entire robot, and a buoyancy control unit calculates a control amount of the drive unit for changing the center of buoyancy of the floating device using the deviation between the center of buoyancy position and the center of gravity position, the target value of the deviation between the center of buoyancy position and the center of gravity position, a center of gravity sensitivity matrix that indicates a change in the center of buoyancy position relative to the control amount of the drive unit, and a center of buoyancy sensitivity matrix that indicates a change in the center of buoyancy position relative to the control amount of the drive unit. [Effects of the Invention]

[0014] According to the above (1) to (8), the airframe can be maintained in a target attitude without constructing an approximation formula for each attitude of a drive unit such as a buoyancy member. According to (2), vibrations can be suppressed even when the posture of the arm or the like equipped on the robot changes. According to (3), even if the posture of the arm or the like provided on the robot changes, fluctuations in the working position of the arm's hand can be suppressed. [Brief explanation of the drawings]

[0015] [Figure 1] FIG. 1 is a perspective view of a robot according to a first embodiment. [Figure 2] FIG. 2 is an explanatory diagram of a pitching motion of the robot according to the first embodiment. [Figure 3] FIG. 2 is an explanatory diagram of a rolling motion of the robot according to the first embodiment. [Figure 4] 5A and 5B are diagrams for explaining an example of control by movement of the floating device in the first embodiment. [Figure 5] 1 is a diagram illustrating an example of the configuration of a mobile object control system according to a first embodiment. [Figure 6] FIG. 3 is a block diagram of a control process for the floating device according to the first embodiment. [Figure 7] FIG. 4 is a diagram for explaining a conversion process to a target COX angle in the first embodiment. [Figure 8] FIG. 10 is a diagram illustrating an example of a cause of a deviation. [Figure 9] 10A and 10B are diagrams for explaining a process of converting a target buoyancy center of gravity position into a deviation in the pitch direction. [Figure 10] FIG. 10 is a diagram for explaining conversion into a floating device drive control amount. [Figure 11] 4 is a flowchart of a control procedure performed by the mobile body control device according to the first embodiment. [Figure 12] 10 shows an example of a verification result in the case of control by the conventional technology. [Figure 13] 10 shows an example of a verification result in the case of the control method according to the first embodiment. [Figure 14] 10A and 10B are diagrams showing examples of results obtained by checking vertical vibrations in a working area, which is the tip position of an end effector, when vibration damping control according to the first embodiment is performed and when it is not performed. [Figure 15] 10A and 10B are diagrams illustrating an example of a change in a working area, which is the tip position of the end effector, when deviation offset correction vibration suppression control according to the first embodiment is performed. [Figure 16] FIG. 10 is a perspective view of a robot according to a second embodiment. [Figure 17] FIG. 10 is a diagram showing a configuration example of the upper part of the main body (movable part) of the second embodiment as viewed from above. [Figure 18] FIG. 10 is a diagram illustrating an example of the configuration of a mobile object control system according to a second embodiment. [Figure 19] FIG. 10 is a diagram for explaining conversion into a floating device drive control amount. DETAILED DESCRIPTION OF THE INVENTION

[0016] Hereinafter, embodiments of the present invention will be described with reference to the drawings. In the following description, a robot (ROV: Remote Operating Vehicle) that operates underwater (an example of underwater) by remote control such as wired communication will be described as an example of a mobile object, i.e., a robot. In the following description, expressions indicating relative or absolute arrangement, such as "parallel," "orthogonal," "center," and "coaxial," not only mean such arrangement in a strict sense, but also include a state in which there is a relative displacement with a tolerance or an angle or distance that provides the same function. In the drawings used in the following description, the scale of each component has been appropriately changed to make each component recognizable.

[0017] In addition, in all the drawings for explaining the embodiments, the same reference numerals are used for components having the same functions, and repeated explanations will be omitted. Furthermore, in this application, "based on XX" means "based on at least XX," and includes cases where it is based on other elements in addition to XX. Furthermore, "based on XX" is not limited to cases where XX is used directly, but also includes cases where it is based on XX that has been calculated or processed. "XX" is any element (for example, any information).

[0018] [First embodiment] In this embodiment, an example will be described in which the upper and lower main body parts of the robot are connected by a link mechanism.

[0019] <Robot> First, the external appearance of the robot 1 according to this embodiment will be described. Fig. 1 is a perspective view of the robot according to this embodiment. 1, the robot 1 includes a robot body 2, which is the main body portion of the robot 1. The robot body 2 includes an upper body part 3 (movable part) located at the top of the robot body 2, and a lower body part 4 (stationary part) located at the bottom of the robot body 2.

[0020] In the following description, the direction in which the robot 1 moves forward is referred to as "forward," the direction opposite to forward is referred to as "backward," the right hand relative to the direction in which the robot 1 moves forward is referred to as the "right side," the left hand relative to the direction in which the robot 1 moves forward is referred to as the "left side," and the left-right direction of the robot 1 is referred to as the "width direction." The up-down direction of the robot 1 is the direction perpendicular to the front-to-back and width directions of the robot 1. The upper side of the robot 1 is the side on which the upper body 3 is located in the up-down direction of the robot 1. The lower side of the robot 1 is the opposite side from the side on which the upper body 3 is located in the up-down direction of the robot 1 (the side on which the lower body 4 of the robot 1 is located). In the example shown in the figure, the robot 1 is positioned horizontally. The up-down direction of the robot 1, the upper side of the robot 1, and the lower side of the robot 1 correspond to the up-down direction (vertical direction), the vertically upward, and the vertically downward when the robot 1 is positioned horizontally. In the following description, the symbol L may be added to the end of elements on the left side of the robot 1, and the symbol R may be added to the end of elements on the right side.

[0021] <Upper part of the main unit> The upper body part 3 is located, for example, at the top of the robot body 2. The upper body part 3 has a relatively large buoyancy compared to the lower body part 4. The upper body part 3 has, for example, a rectangular outer shape in a plan view. For example, the upper body part 3 is provided with ballast and buoyancy material to keep the robot 1 horizontal. The upper body part 3 is provided with an upper propulsion unit 10 (hereinafter also referred to as "upper thruster 10") for moving the robot 1 in the up and down direction. One upper thruster 10 is arranged in the center of the upper body part 3 in the front-to-rear direction and the center of the width direction.

[0022] The upper thruster 10 includes, for example, a propeller that rotates around an axis above and below the upper body 3. For example, the upper thruster 10 rotates the propeller in one direction around the axis to move the robot 1 upward (ascend). For example, the upper thruster 10 rotates the propeller in the other direction around the axis to move the robot 1 downward (descend).

[0023] The upper body part 3 is provided with an attachment part 11 for, for example, power lines for supplying power to components of the robot 1 and signal lines (not shown) for transmitting signals. A through-hole 12 through which the power lines and signal lines pass is formed on the front or rear side of the attachment part 11 in the upper body part 3. Note that a posture detection sensor (e.g., a gyro sensor) for detecting the posture of the robot 1 (rotation and orientation in the front-rear direction, width direction, and up-down direction, etc.) may be provided near the attachment part 11. For example, the posture detection sensor may be provided in a location where an arm is attached. Note that in the following description, the upper body part 3 is also referred to as "Upper."

[0024] <Bottom of the unit> The lower body part 4 is located at the bottom of the robot body 2. The lower body part 4 is relatively heavier than the upper body part 3, and has a smaller buoyancy (volume). The lower body part 4 has a rectangular shape in a plan view. For example, the lower body part 4 may be provided with a weight to keep the robot 1 horizontal and to make it heavier than the upper body part 3. In the following description, the lower body part 4 will also be referred to as "Lower."

[0025] The lower main body portion 4 includes a frame 20 having, for example, a rectangular outer shape in a plan view. The frame 20 has, for example, a rectangular outer shape with its longitudinal axis extending in the front-rear direction. The frame 20 has an opening 21 formed in a portion that overlaps with the upper thruster 10 in a top view. A bracket 22 having its longitudinal axis extending in the width direction is provided on the lower front portion of the frame 20.

[0026] The lower body portion 4 is provided with a plurality of lower propulsion units 23L, 23R, 24L, and 24R for moving the robot 1 in the front-rear and width directions, for example. The plurality of lower propulsion units 23L, 23R, 24L, and 24R are four units (corresponding to four horizontal thrusters): a pair of left and right front thrusters 23L and 23R for moving the robot 1 forward or in the width direction, and a pair of left and right rear thrusters 24L and 24R for moving the robot 1 backward or in the width direction.

[0027] The front thrusters 23L, 23R are provided, for example, at the front of the frame 20. The front thrusters 23L, 23R include, for example, propellers that rotate around axes that are inclined so as to be positioned outward in the width direction as they move from the front to the rear of the lower body 4.

[0028] The rear thrusters 24L, 24R are provided, for example, at the rear of the frame 20. The rear thrusters 24L, 24R are equipped with propellers that rotate around axes that are inclined so as to be positioned outward in the width direction as they move from the rear of the lower body 4 to the front.

[0029] Thruster drive units 25L and 25R for applying drive force (rotational force for each propeller) to each thruster 10, 23L, 23R, 24L, and 24R are provided in the lower body 4. The thruster drive units 25L and 25R are provided in pairs on the left and right sides at the front of the frame 20.

[0030] For example, a camera 26 is provided in the lower body part 4. The lights 70L and 70R are provided, for example, on the front side of the lower main body part 4. When one of the lights 70L and 70R is not specified, it is referred to as the light 70. The light 70 is a device that can change the on / off state of the light, the illuminance of the light, the angle of the light, etc. based on a light control instruction.

[0031] A pair of left and right manipulators 30L and 30R are provided on the lower body 4. The manipulators 30L and 30R each include an arm 31 and a hand 32. The arm 31 is composed of a combination of joints and links. The base end of the arm 31 is connected to the outer end in the width direction of the bracket 22. The base end of the arm 31 is connected to the frame 20 via the bracket 22. For example, the arm 31 has six rotation axes.

[0032] The hand 32 is provided at the tip end of the arm 31 (the part of the arm 31 opposite the base end). The hand 32 is capable of grasping an object. In the example shown in the figure, the hand 32 has three fingers.

[0033] For example, a position detection sensor 35 is provided on the lower main body 4 to detect the position of the robot 1 (for example, the distance from the seabed to the robot 1). For example, the position detection sensor 35 is an ultrasonic sensor. In the example shown in the figure, one position detection sensor 35 is provided between the pair of left and right manipulators 30L, 30R at the front of the frame 20. The position detection sensor 35 is, for example, a DVL (Doppler Velocity Log) sensor for constant altitude navigation, an internal pressure sensor for constant depth navigation, an inertial measurement unit (IMU), etc. Furthermore, for example, power supply systems 36 and 37 are provided in the lower body 4.

[0034] A weight installation area 38 for installing a weight may be provided in the lower main body portion 4. The weight installation area 38 is provided, for example, at the rear of the frame 20 and behind the power supply system 37.

[0035] The robot 1 includes manipulators 30L, 30R and power supply systems 36, 37. The manipulators 30L, 30R are provided at the front (an example of one longitudinal side) of the lower main body 4. The power supply systems 36, 37 are provided at the rear (an example of the other longitudinal side) of the lower main body 4. The manipulators 30L, 30R are provided on the opposite side of the lower main body 4 in the longitudinal direction from the installation location of the power supply systems 36, 37, via the opening 21.

[0036] <link> The upper body 3 and the lower body 4 are connected by multiple links 5L, 5R, 6L, and 6R at connecting portions 7. The multiple links 5L, 5R, 6L, and 6R are arranged parallel to one another. The multiple links 5L, 5R, 6L, and 6R extend between the four upper corners of the upper body 3 and the four lower corners of the lower body 4. The multiple links 5L, 5R, 6L, and 6R consist of a pair of left and right front links 5L and 5R and a pair of left and right rear links 6L and 6R. The upper body 3 and the lower body 4 are connected in parallel by four links 5L, 5R, 6L, and 6R. When one of the links 5L and 5R is not specified, it is referred to as link 5. When one of the links 6L and 6R is not specified, it is referred to as link 6.

[0037] <Joints> The connecting unit 7 is equipped with joints 8A and 8P that can rotate in the pitch and roll directions of the robot body 2. A total of eight joints 8A and 8P are provided, one at each of the upper and lower ends of the four links 5L, 5R, 6L, and 6R.

[0038] The robot 1 is provided with an actuator 9 that can rotate the links 5L, 5R, 6L, and 6R in the pitch and roll directions at one joint 8A (an example of at least one joint) of the eight joints 8A and 8P. The actuator 9 is provided at the joint 8A at the lower end of the left rear link 6L of the four links 5L, 5R, 6L, and 6R.

[0039] Hereinafter, the joint 8A provided with the actuator 9 will be referred to as the "joint driver 8A," and the joint 8P moved by the movement of the joint driver 8A (the joint 8P not provided with the actuator 9) will be referred to as the "passive joint 8P." The robot 1 has one joint driver 8A and seven passive joints 8P.

[0040] <Joint drive unit> The joint drive unit 8A is provided with an actuator 9, which includes a pitching drive device 40 for rotating the links 5L, 5R, 6L, and 6R in the pitch direction, and a rolling drive device 50 for rotating the links 5L, 5R, 6L, and 6R in the roll direction.

[0041] The pitching drive device 40 includes, for example, a pitching motor that rotates the links 5L, 5R, 6L, and 6R in the pitch direction, a driven pulley that reduces the rotational speed of the pitching motor to a predetermined value or less, a reducer that further reduces the rotation reduced by the driven pulley, and a case that houses the pitching motor and the driven pulley. The pitching drive device 40 is controlled by a center of buoyancy control unit 47 (see FIG. 5). For an example of the structure of the pitching drive device 40, see, for example, Japanese Patent Application No. 2023-029174.

[0042] The rolling drive device 50 includes, for example, a rolling motor that rotates the links 5L, 5R, 6L, and 6R in the roll direction, a driven pulley that reduces the rotational speed of the rolling motor to a predetermined value or less, a reducer that further reduces the rotation reduced by the driven pulley, and a case that houses the rolling motor and the driven pulley. The rolling drive device 50 is controlled by a center of buoyancy control unit 47 (see FIG. 5). For an example of the structure of the rolling drive device 50, see, for example, Japanese Patent Application No. 2023-029174.

[0043] <Passive joint> The following describes the configuration of the seven passive joints 8P provided at the lower end of the left front link 5L. The passive joints 8P provided at other locations have the same configuration as the configuration provided at the lower end of the left front link 5L, so detailed description will be omitted. The passive joint 8P is provided with a mechanism that can tilt in any direction by combining two perpendicular axes, a so-called gimbal mechanism.

[0044] The gimbal mechanism includes a gimbal body, which is the main body of the gimbal mechanism, a pitching shaft member for rotating the links 5L, 5R, 6L, and 6R in the pitch direction, a rolling shaft member for rotating the links 5L, 5R, 6L, and 6R in the roll direction, a support member for supporting the rolling shaft member, and multiple plain bearings. For an example of the structure of the gimbal mechanism, see, for example, Japanese Patent Application No. 2023-029174.

[0045] <An example of a robot's pitching motion> FIG. 2 is an explanatory diagram of the pitching motion of the robot according to the embodiment. For example, when the output shaft of the pitching motor 41 in the active joint 8A is rotated in one direction around its axis (around an axis in the width direction), the multiple driven joints 8P rotate synchronously around their axes (around axes in the width direction). This causes the robot body 2 to rotate in the pitch direction. In the example shown in the figure, the robot body 2 rotates counterclockwise (an example of one direction in the pitch direction) when viewed from the left side.

[0046] <An example of a robot's rolling motion> FIG. 3 is an explanatory diagram of the rolling motion of the robot according to the embodiment. For example, when the output shaft of the rolling motor 51 in the active joint 8A is rotated in one direction around its axis (around an axis in the front-to-rear direction), the multiple driven joints 8P rotate synchronously around their axes (around axes in the front-to-rear direction). This causes the robot body 2 to rotate in the roll direction. In the example shown in the figure, the robot body 2 rotates clockwise (an example of one direction in the roll direction) when viewed from the front.

[0047] <Example of control by moving the floating device when bending and stretching the arm> Next, an example of control by movement of the floating device when bending and stretching the arm 31 of this embodiment will be described. FIG. 4 is a diagram for explaining an example of control by movement of the floating device of this embodiment. Note that in the embodiment, the floating device includes, for example, a buoyant material, and links and joints connected to the buoyant material. Note that the floating device may also include a drive unit. Furthermore, the floating device is connected to the robot main body 2, and has a high buoyancy and a low weight relative to the robot main body 2 (a low density relative to the robot main body 2). Note that in the embodiment, "low density" means a high buoyancy and a low weight, that is, a low density.

[0048] Reference symbol g10 indicates an example of the state before the arm 31 is extended. Reference symbol g11 indicates the center of buoyancy. Reference symbol g12 indicates the center of gravity of the main body. In the following description, the center of buoyancy will also be referred to as "COB" and the center of gravity of the main body will also be referred to as "COG".

[0049] Reference symbol g20 is an example of the state immediately after the arm 31 is extended. In this case, as shown by reference symbol g20, the COB moves in accordance with the weight of the upper main body 3, the weight of the arm 31, and the length to which the arm 31 is extended. Also, as shown by reference symbol g20, the COG moves in accordance with the weight of the lower main body 4, the weight of the arm 31, and the length to which the arm 31 is extended.

[0050] The symbol g30 is the deviation ΔX between the center of buoyancy and the center of gravity of the aircraft. err This is an example of the state when the force g30 is not controlled. In this case, the attitude of the body tilts as shown by the symbol g30 so that the buoyancy force B and the gravity M of the body are balanced.

[0051] Reference symbol g40 is an example of a state when the deviation between the center of buoyancy and the center of gravity of the aircraft is controlled by the method of this embodiment. Reference symbol g41 represents the state before the link mechanism is operated. As shown by reference symbol g40, in this embodiment, the attitude of the aircraft is controlled by controlling the link mechanism using the angle (COX angle) between the center of buoyancy and the aircraft coordinate system as the manipulated variable. This allows the attitude of the main body to be maintained without collapsing even when the arm 31 is extended from a retracted state. In addition, q ACOX1 represents the control angle of the joint. ACOX1 is, for example, the angle in the pitch direction.

[0052] <Configuration example of a mobile control system> Next, a configuration example of a mobile object control system 400 of this embodiment will be described. Fig. 5 is a diagram showing a configuration example of a mobile object control system of this embodiment. As shown in Fig. 5, the mobile object control system 400 includes, for example, a robot 1 and an operation unit 200.

[0053] The operation unit 200 includes, for example, a controller 201, an image display unit 202, and a communication unit 203.

[0054] The robot 1 includes, for example, a robot body 2, an upper body part 3, a lower body part 4, a manipulator 30, an upper propulsion unit 10, a lower propulsion unit 23, links 5, links 6, a camera 26, a camera sensor 27, a camera drive unit 28, a position detection sensor 35, lighting 70, an arm sensor 65, a manipulator drive unit 66, an attitude sensor 67, and a mobile body control device 100. Note that in the configuration example of Fig. 5, some of the components of the robot 1 described using Fig. 1 are omitted.

[0055] The moving body control device 100 includes, for example, a thruster driving device 25, a center of buoyancy control unit 47, an acquisition unit 90, a target setting unit 91, a control unit 92, a storage unit 93, and a communication unit 94.

[0056] (Operation unit) The operation unit 200 is used by an operator on a ship, for example. The operation unit 200 and the mobile object control device 100 are connected to each other, for example, by wire. The controller 201 is a device through which an operator inputs operation commands to the robot body 2 and the arm 31. The controller 201 is, for example, a handle, a joystick, a touch panel sensor, or the like. The image display unit 202 acquires from the mobile object control device 100 and displays images captured by the camera 26, the states of the robot main body 2 and the arm 31, and the like. The communication unit 203 transmits and receives information to and from the mobile object control device 100 .

[0057] (robot) The robot body 2 is the part other than, for example, the arm 31 in Fig. 1. The parts described with reference to Fig. 1 are attached to the robot body 2.

[0058] The camera 26 is, for example, an imaging device using a CMOS (Complementary Metal Oxide Semiconductor) imaging element or a CCD (Charge Coupled Device) imaging element. The camera 26 may be an RGB (Red Green Blue) D camera that can also obtain depth information D.

[0059] The camera sensor 27 detects, for example, the tilt of the pan axis of the camera 26 .

[0060] The camera driving unit 28 tilts the camera 26, for example, in the direction of the pan axis, based on a camera control instruction included in a control command from the control unit 92. The camera driving unit 28 includes, for example, an actuator and a driving circuit.

[0061] The arm sensor 65 is, for example, an encoder attached to a joint, a six-axis sensor attached to a hand, or a tactile sensor.

[0062] The manipulator driving unit 66 drives the arm 31 based on a control command from the control unit 92. The manipulator driving unit 66 includes, for example, an actuator and a driving circuit.

[0063] The attitude sensor 67 is a sensor that detects the pitch direction of the robot main body 2 and the floating device, the tilt angle in the roll direction, the attitude angle of the aircraft, etc. The attitude sensor 67 may also be an acceleration sensor, a pressure sensor, etc. In this case, the attitude of the aircraft may be estimated using the detected values ​​of the acceleration sensor, pressure sensor, etc. by a well-known method (for example, Tomohiro Takahashi, Masatoshi Hatano, "Research on Attitude Control of Underwater Mobile Manipulators," 25th Transportation and Logistics Conference (TRANSLOG2016), Japan Society of Mechanical Engineers, 2016).

[0064] (Mobile control device) The thruster drive unit 25 generates thruster control commands to drive the upper propulsion unit 10 and the lower propulsion unit 23.

[0065] The center of buoyancy control unit 47 adjusts the control angle q to eliminate the center of buoyancy in the pitch direction and roll direction (or x-axis direction and y-axis direction). ACOX1 ,q ACOX2 In addition, q ACOX2 is, for example, an angle in the roll direction. The center of buoyancy control unit 47 controls the calculated control angle q ACOX1 ,q ACOX2The links 5 and 6 are controlled using the control unit 47 to move the upper body 3 in at least one of the pitch and roll directions. The control method by the center of buoyancy control unit 47 will be described later.

[0066] The acquisition unit 90 acquires the center of buoyancy position of the combined main body and floating device, the center of gravity position of the combined main body and floating device, the angle θ between the center of buoyancy position and the center of gravity position, and the attitude angle φ of the main body from the attitude sensor 67, etc. Note that the acquisition unit 90 may, for example, calculate and acquire the center of buoyancy position and center of gravity position based on the acquired detection values. Alternatively, the center of buoyancy control unit 47 may calculate and acquire the center of buoyancy position and center of gravity position based on the detection values ​​acquired by the acquisition unit 90.

[0067] The target setting unit 91 sets a target value for the angle between the center of buoyancy and the center of gravity from the target attitude.

[0068] The control unit 92 controls the operations of the robot body 2, the arm 31, the camera 26, and the like, for example.

[0069] The storage unit 93 stores programs, thresholds, predetermined values, mathematical expressions, etc. required for controlling the mobile body control device 100. The storage unit 93 stores three-dimensional models of the robot body 2 and the arm 31, for example.

[0070] The communication unit 94 transmits and receives information to and from the operation unit 200 .

[0071] <Floating device control processing> Next, the control process of the floating device will be described.

[0072] FIG. 6 is a block diagram of the control process of the floating device according to this embodiment. The processing of each part in FIG. 6 is performed by the center of buoyancy control unit 47. In FIG. 6, A * is the target value of A, φ is the attitude angle of the aircraft, and θ cox is the angle between the center of buoyancy and the center of gravity, and q ACOX is the state of the driving part of the floating device, and q allis the state of all the drive parts of the aircraft including the floating device, Δx is the deviation of the center of buoyancy and center of gravity, and l COX is the distance between the center of buoyancy and the center of gravity.

[0073] The first conversion unit 471 converts the target value φ of the attitude angle φ of the aircraft into * The target COX angle θ * cox and outputs it to the second calculation unit 477.

[0074] The first calculation unit 472 calculates the target value φ of the attitude angle φ of the airframe. * The attitude angle φ of the aircraft is subtracted from this and output to the integrator 473 and differentiator 475.

[0075] The integrator 473 calculates the attitude angle deviation (φ * -φ). The first coefficient unit 474 applies a coefficient K i and outputs the result to the second calculation unit 477. The processing of the integrator 473 and the first coefficient unit 474 is deviation correction.

[0076] The differentiator 475 calculates the attitude angle deviation (φ * -φ) is differentiated. The second coefficient unit 476 applies a coefficient K d and outputs the result to second calculation unit 477. The processing of differentiator 475 and second coefficient unit 476 is vibration suppression. In addition, the coefficient K i , K. d is also a gain and is determined in advance, for example, by simulation.

[0077] The second calculation unit 477 converts the target COX angle θ * cox The output of the first coefficient unit 474 and the output of the second coefficient unit 476 are added together and output to the second conversion unit 478.

[0078] The second conversion unit 478 converts the output of the second calculation unit 477 and the distance l between the center of buoyancy and the center of gravity, which is the output of the center of gravity and center of buoyancy calculation unit 482. CoX Using the above, the deviation of the target center of buoyancy and center of gravity position Δx * The deviation of the converted target center of buoyancy and center of gravity position Δx * is output to the third calculation unit 479.

[0079] The third calculation unit 479 calculates the deviation Δx of the target center of buoyancy and center of gravity position, which is the output of the second conversion unit 478. * The deviation Δx between the center of buoyancy and center of gravity position, which is the output of the center of gravity / center of buoyancy calculation unit 482, is subtracted from the above, and the result of the subtraction is output to the third conversion unit 480.

[0080] The third conversion unit 480 converts the output of the third calculation unit 479 into a control amount q ACOX and the converted control amount q of the floating device driver ACOX is output to Plant481. Note that q ACOX is expressed as the following equation (1).

[0081]

number

[0082] Plant481 is an image of all the driving parts of the vehicle, including the floating device, and the vehicle's position and attitude in the underwater environment. Sensors that detect the state of the driving parts and attitude sensors 67 that detect the attitude are attached to the floating device, driving parts, or around the driving parts. Plant481 is q all to the center of gravity and center of buoyancy calculation unit 482, and outputs the attitude angle φ of the aircraft to the first calculation unit 472. all is expressed by the following equation (2). ACOX1 ,q ACOX2 ,q others can be either an angle or a length.

[0083]

number

[0084] The center of gravity and center of buoyancy calculation unit 482 calculates the q all The center of gravity and center of buoyancy calculation unit 482 uses the calculation results to calculate the deviation Δx of the center of buoyancy and the distance l between the center of buoyancy and the center of gravity. COX Calculate.

[0085] (Processing of the first conversion unit) Next, a detailed description will be given of the processing of the first conversion unit 471. Fig. 7 is a diagram for explaining the processing for conversion to the target COX angle in this embodiment. The center of buoyancy control unit 47 controls the attitude of the robot 1 body using the angle between the center of buoyancy and the center of gravity as the manipulated variable.

[0086] In Figure 7, θ COX The (CoX angle) is the angle between the line connecting the center of buoyancy and the center of gravity and the z-axis of the robot body coordinate system of the robot 1. In the robot body coordinate system, the up and down direction of the robot body is defined as the z-direction. Here, the line segment L act If it is assumed that the inertial coordinate is vertical, then θ COX As shown in the following equation (3), the angle φ between the horizontal direction in the inertial coordinate system and the aircraft is equal to the angle φ. As a result, the target value φ of the aircraft attitude angle φ is calculated as shown in the following equation (4). * The target COX angle θ * cox The center of buoyancy control unit 47 acquires the attitude angle φ of the aircraft via the acquisition unit 90 based on the detected value of the attitude sensor 67, for example.

[0087]

number

[0088]

number

[0089] Here, when there is no external force, the center of buoyancy and the center of gravity are aligned on a vertical line, so the angle between the center of buoyancy and the center of gravity is the same as the attitude angle. Therefore, according to this embodiment, even if the center of gravity moves within the aircraft (due to the movement of arm 31, etc.), the aircraft can be maintained in the target attitude by making the angle between the center of buoyancy and the center of gravity the same as the target attitude.

[0090] (integral control, differential control) Next, we will explain the integral control by the integrator 473 and the differential control by the differentiator 475. Figure 8 is a diagram showing examples of factors that cause deviations. When moving or working underwater, as shown in Figure 8, it may not be possible to maintain the target attitude due to factors such as the transportation of heavy objects, rapid shifting of the center of gravity, thrust from thrusters, and other environmental factors.

[0091] Therefore, as a countermeasure to these problems, in this embodiment, the actual attitude angle of the aircraft is fed back and reflected in the control amount of the floating device. By using differential control, vibration is suppressed and the attitude angular velocity φ · is controlled to be 0. Also, by using integral control, the attitude angle deviation (φ * -φ) to 0.

[0092] (Conversion into deviation of target center of buoyancy and center of gravity position) Next, a detailed description will be given of the conversion process into deviation of the target buoyancy center of gravity position performed by the second conversion unit 478. Fig. 9 is a diagram for explaining the conversion process into deviation of the target buoyancy center of gravity position in the pitch direction. In Figure 9, l CoX-XZ is the distance between the center of buoyancy and the center of gravity in the xz plane. Δx * is the target deviation between the center of buoyancy and the center of gravity. * cox is the target COX angle. From the trigonometric function relationship, the deviation between the center of buoyancy and the center of gravity is Δx * can be expressed as in the following equation (5). Note that the y-axis direction is the depth direction into the paper. In this way, the second conversion unit 478 uses equation (5) to calculate the target COX angle θ * cox The target value of the deviation between the center of buoyancy and the center of gravity is Δx* The second conversion unit 478 converts the distance l between the center of buoyancy and the center of gravity in the xz plane into CoX-XZ is obtained from the center of gravity and center of buoyancy calculation unit 482.

[0093]

number

[0094] Although the example described with reference to FIG. 9 is an example of the pitch direction, the calculation can be similarly converted for the roll direction.

[0095] (Conversion to floating device drive control amount) Next, the conversion process into the floating device drive control amount performed by the third conversion unit 480 will be described in detail. Fig. 10 is a diagram for explaining the conversion into the floating device drive control amount. Reference symbol g100 is an example of a state before the arm is extended, for example. Reference symbol g110 is an example of a state after the arm is extended, in which the center of buoyancy position and center of gravity position have moved and the floating device has been moved.

[0096] The third conversion unit 480 converts the target value Δx of the deviation between the center of buoyancy and the center of gravity into * The result of subtracting the deviation Δx between the center of buoyancy and the center of gravity from the center of buoyancy is obtained from the third calculation unit 479. Here, the deviation Δx between the center of buoyancy and the center of gravity is given by the following equation (6). COB is the following equation (7), and the center of gravity position x COG is expressed by the following equation (8). In equation (6), ΔX err is the displacement in the x-axis direction between the center of buoyancy and the center of gravity, ΔY err is the positional deviation in the y-axis direction between the center of buoyancy and the center of gravity. G is the center of gravity position in the x-axis direction, X B is the center of buoyancy position in the x-axis direction, and Y G is the center of gravity position in the y-axis direction, Y B is the center of buoyancy position in the y-axis direction.

[0097]

number

[0098]

number

[0099]

number

[0100] The goal of this embodiment is to control the control angle q of the floating device driver. ACOX1 The deviation between the center of buoyancy and the center of gravity of the ROV (Remote Operating Vehicle) coordinate system is set to a predetermined target value Δx * Then, the target value of the deviation is expressed by the following equation (9).

[0101]

number

[0102] Therefore, equation (9) is expressed as equation (10) below, and the deviation is expressed as equation (11) below.

[0103]

number

[0104]

number

[0105] The center of buoyancy sensitivity matrix J, which is a Jacobian matrix, COB is the following equation (12), and the center of gravity sensitivity matrix J COG is expressed as the following equation (13).

[0106]

number

[0107]

number

[0108] Here, if the difference matrix between the center of buoyancy sensitivity and the center of gravity sensitivity is regular, the control amount q can be found by the inverse matrix as shown in the following equation (14).

[0109]

number

[0110] The center of buoyancy control unit 47 uses the control amount q thus determined to control the attitude of the robot 1 by, for example, PID (Proportional-Integral-Differential) control.

[0111] <Example of processing procedure> Next, a description will be given of an example of a control procedure performed by the mobile body control device 100. Fig. 11 is a flowchart of a control procedure performed by the mobile body control device 100 according to this embodiment.

[0112] (Step S1) The acquisition unit 90 acquires the detection values ​​detected by the sensors such as the attitude sensor 67.

[0113] (Step S2) The center of buoyancy control unit 47 uses the detection value acquired by the acquisition unit 90 to acquire the attitude angle of the aircraft.

[0114] (Step S3) The target setting unit 91 sets a target value for the angle between the center of buoyancy and the center of gravity from the target attitude. The target attitude may be set in advance, or may be set by the operator operating the operation unit 200. The center of buoyancy control unit 47 acquires the target value for the attitude angle of the aircraft set by the target setting unit 91.

[0115] (Step S4) The buoyancy control unit 47 calculates and determines the position of the buoyancy center and the position of the center of gravity based on the detection values ​​acquired by the acquisition unit 90.

[0116] (Step S5) The first conversion unit 471 of the center of buoyancy control unit 47 converts the input target value φ of the attitude angle φ of the aircraft into *The target COX angle θ * Convert to cox.

[0117] (Step S6) The integrator 473 and the first coefficient unit 474 of the center of buoyancy control unit 47 perform integration processing to perform feedback control, which is deviation correction processing. The differentiator 475 and the second coefficient unit 476 of the center of buoyancy control unit 47 perform differentiation processing to perform feedback control, which is vibration suppression processing.

[0118] (Step S7) The second conversion unit 478 of the center of buoyancy control unit 47 converts the output of the second calculation unit 477 and the distance l between the center of buoyancy and the center of gravity, which is the output of the center of gravity and center of buoyancy calculation unit 482. CoX Using the above, the deviation of the target center of buoyancy and center of gravity position Δx * Convert to.

[0119] (Step S8) The third conversion unit 480 of the center of buoyancy control unit 47 converts the output of the third calculation unit 479 into the control amount q of the floating device driving unit. ACOX Convert to.

[0120] (Step S9) The center of buoyancy control unit 47 uses the calculated control amount q to control the attitude of the robot 1, for example, by PID control.

[0121] (Step S10) The center of buoyancy control unit 47 determines whether or not control has ended. The center of buoyancy control unit 47 may determine the start or end of control based on, for example, whether the arm is extended or retracted based on the detection value of the arm sensor 65, or may determine this based on the result of the operator operating the operation unit 200. If control has ended (Step S10; YES), the center of buoyancy control unit 47 ends the processing. If control has not ended (Step S10; NO), the center of buoyancy control unit 47 returns the processing to Step S1.

[0122] <Verification results> Next, an example of the verification result will be described. FIG. 12 shows an example of verification results for control using conventional technology. The graph labeled g200 shows an example of change in the shoulder joint angle of the arm. In the graph labeled g200, the horizontal axis represents time (sec) and the vertical axis represents the shoulder joint angle (deg) of the arm. The graph labeled g210 shows an example of change in the angle in the pitch direction. In the graph labeled g210, the horizontal axis represents time (sec) and the vertical axis represents the angle in the pitch direction (deg). As shown in the graph with reference symbol g210, when controlled using the conventional method, the maximum deviation in pitch angle was approximately 15 degrees.

[0123] Figure 13 shows an example of verification results for the control method of this embodiment. The graph indicated by reference symbol g220 shows an example of change in the shoulder joint angle of the arm. In the graph indicated by reference symbol g220, the horizontal axis is time (sec) and the vertical axis is the shoulder joint angle (deg) of the arm. The graph indicated by reference symbol g230 shows an example of change in the angle in the pitch direction. In the graph indicated by reference symbol g230, the horizontal axis is time (sec) and the vertical axis is the angle in the pitch direction (deg). As shown in the graph with reference symbol g230, with the control method of this embodiment, the maximum deviation of the pitch angle was 4 (deg) or less.

[0124] In this way, according to the control method of this embodiment, the tilt of the robot body caused by the change in the posture of the arm of the robot 1 can be corrected.

[0125] (Vibration control) Next, an example of the results of checking the vertical vibration of the working area, which is the tip position of the end effector, when vibration suppression control by differential control according to this embodiment is performed and when it is not performed will be described. Figure 14 shows an example of the results of checking the vertical vibration of the working area, which is the tip position of the end effector, when vibration suppression control of this embodiment is performed and when it is not performed. The graph indicated by reference symbol g240 is an example of change in the height of the working area. In the graph indicated by reference symbol g240, the horizontal axis is time (sec) and the vertical axis is the height of the working area (mm). The lines indicated by reference symbol g241 are the change in the height of the working area when vibration suppression control is not performed. The lines indicated by reference symbol g242 are the change in the height of the working area when vibration suppression control is performed. The graph labeled g250 shows an example of the change in arm length when the arm is extended from a contracted state. In the graph labeled g250, the horizontal axis represents time (sec) and the vertical axis represents arm length.

[0126] As shown in the graph with symbol g240, the change in height of the work area at the start of the vibration was reduced from 33 mm without vibration suppression control to 19 mm with vibration suppression control. Also, the vibration damping ratio was reduced from 0.1 without vibration suppression control to 0.74 with vibration suppression control.

[0127] (Tilt deviation offset correction control) Next, an example of the results of checking the vertical vibration of the working area, which is the tip position of the end effector, when deviation offset correction control is performed by integral control according to this embodiment will be described. Figure 15 is a diagram showing an example of change in the working area, which is the tip position of the end effector, when deviation offset correction vibration suppression control of this embodiment is performed. The horizontal axis is time (sec) and the vertical axis is the height (mm) of the working area. The verification in Figure 15 is an example of integral control when a 2.5 (kg) weight is attached to the tip of the hand and the robot is tilted.

[0128] As shown in Figure 15, when the deviation of the working area with the center of buoyancy as the center of rotation was set within a predetermined value (target value), the offset of the tilt of 190 mm could be corrected within the predetermined value in approximately 18 seconds using integral control.

[0129] As described above, in this embodiment, when a deviation between the center of buoyancy and the center of gravity occurs when the arm is moved, the control angle is adjusted to make the positional deviation between the center of buoyancy and the center of gravity zero (or within a predetermined value). Also, in this embodiment, deviation correction processing is performed by integral control. That is, in this embodiment, I control is incorporated into PID control. Furthermore, in this embodiment, vibration suppression processing is performed by differential control. For example, after the robot body 2 starts moving, shaking occurs when it stops, and this shaking appears in the camera image of the operator, so D control is incorporated into PID.

[0130] As a result, according to this embodiment, when the posture of the arm is changed, a control angle amount for correcting the tilt of the robot 1 body can be obtained, and the tilt can be corrected using this corrective control amount. In other words, according to this embodiment, there is no need to construct an approximation equation for each posture of a drive unit such as an arm, and various arm movements can be accommodated. Furthermore, according to this embodiment, the floating device can be moved in accordance with the movement of the arm, and the body can be kept horizontal. Furthermore, according to this embodiment, tilt deviation offset correction (deviation correction) can be performed by integral control. That is, by incorporating I control into PID control, it is possible to control the arm so that it can return to a horizontal state even when it is holding a heavy load. Furthermore, according to this embodiment, partial control can be used to suppress (damping) the rise of vibration caused by changes in the arm posture. In other words, by incorporating D control into PID control, it is possible to control so that vibrations quickly subside.

[0131] [Second embodiment] In the first embodiment, an example was described in which the floating device is controlled to maintain the balance of the robot, for example, when the arm is extended from a retracted state, by tilting it by driving the links 5 and 6. In this embodiment, the center of buoyancy or center of gravity of the entire robot is controlled by moving the buoyancy material horizontally in two dimensions without changing its height.

[0132] <Robot> The external shape of the robot 1A of this embodiment will be described below. Fig. 16 is a perspective view of the robot according to this embodiment. As shown in Fig. 16, the robot 1A includes a robot body 2, which is the main body portion of the robot 1. The robot body 2 includes an upper body part 3A (movable part) located at the top of the robot body 2, and a lower body part 4 (stationary part) located at the bottom of the robot body 2. Note that in Fig. 16, only the main functional parts of the robot 1A are indicated by reference numerals. The upper main body part 3A includes a cover 301, an upper buoyancy member 302, a frame 303, and a frame 304. The configuration of the upper main body part 3A will be described in detail with reference to FIG.

[0133] 17 is a top view of an example of the configuration of the upper part of the main body (movable part) of this embodiment, with the cover 301 removed. As shown in Figure 17, the upper body part 3A includes, for example, an upper buoyancy material 302, a frame 303, a frame 304, a slide portion 305, a slide portion 306, an actuator 307 (307L, 307R), an active pulley 308 (308L, 308R), a passive pulley 309 (309L, 309R), and a belt 310 (310L, 310R).

[0134] The frame 304 is attached to the frame 303, for example. The belt 310 is, for example, a flat type, a caterpillar belt, or the like.

[0135] Actuator 307L drives active pulley 308L, thereby rotating belt 310L. The driven pulley 309L is rotated by the rotation of the belt 310L. One end of the slide portion 305 is attached to the upper buoyancy member 302, and moves in the x-axis direction as the belt 310L rotates. Actuator 307R drives active pulley 308R, thereby rotating belt 310R. The driven pulley 309R is rotated by the rotation of the belt 310R. One end of the slide portion 306 is attached to the upper buoyancy member 302, and moves in the x-axis direction as the belt 310R rotates.

[0136] In this embodiment, by controlling actuator 307 in this manner, belt 310 rotates to move upper buoyancy member 302 attached to slide portions 305, 306 in the x-axis and y-axis directions. Actuator 307 is equipped with actuators for x-axis movement and y-axis movement. Thus, according to this embodiment, by controlling these two actuators, the two actuators move in coordination in the x-axis and y-axis directions.

[0137] <Configuration example of a mobile control system> Next, a configuration example of a mobile object control system 400A of this embodiment will be described. Fig. 18 is a diagram showing a configuration example of a mobile object control system of this embodiment. As shown in Fig. 18, the mobile object control system 400A includes, for example, a robot 1A and an operation unit 200.

[0138] The operation unit 200 includes, for example, a controller 201, an image display unit 202, and a communication unit 203. The robot 1A includes, for example, a robot main body 2, an upper main body part 3A, a lower main body part 4, a manipulator 30, an upper propulsion unit 10, a lower propulsion unit 23, a camera 26, a camera sensor 27, a camera drive unit 28, a position detection sensor 35, lighting 70, an arm sensor 65, a manipulator drive unit 66, an attitude sensor 67, an actuator 307, an upper buoyancy member 302, and a mobile body control device 100A. Note that in the configuration example of Fig. 18, some of the components of the robot 1A described using Figs. 16 and 17 are omitted. The moving body control device 100A includes, for example, a thruster driving device 25, a center of buoyancy control unit 47A, an acquisition unit 90, a target setting unit 91, a control unit 92, a storage unit 93, and a communication unit 94.

[0139] The buoyancy control unit 47A detects the position of the buoyancy center and the position of the center of gravity, and based on the detected results, determines a control angle q ACOX1 The center of buoyancy control section 47A calculates the control angle q ACOX1 Using this, the actuator 307 is controlled to move the upper body 3A in the pitch direction in parallel without changing the height.

[0140] (Conversion to floating device drive control amount) Next, a detailed description will be given of the conversion process into the floating device drive control amount performed by the third conversion unit 480. The blocks for the floating device control process are the same as those in FIG. 6 of the first embodiment.

[0141] 19 is a diagram for explaining conversion to a floating device drive control amount. Reference symbol g200 is an example of the state before the arm is extended, for example. Reference symbol g210 is an example of the state after the arm is extended, in which the center of buoyancy and center of gravity have moved and the floating device has been moved.

[0142] The third conversion unit 480 converts the target value Δx of the deviation between the center of buoyancy and the center of gravity into * The result of subtracting the deviation Δx between the center of buoyancy and the center of gravity from the buoyancy position x is obtained from the third calculation unit 479. Here, the deviation Δx between the center of buoyancy and the center of gravity is given by equation (6). COB is Equation (7), and the center of gravity position x COG is given by equation (8). The center of buoyancy control unit 47A uses the control amount q thus determined to control the posture of the robot 1A by, for example, PID control.

[0143] In this embodiment, the deviation between the buoyancy and the reception is set to a target value Δx * The deviation Δx between the buoyancy and the center of gravity on the same vertical line is the target value Δx * In this manner, the upper buoyancy member 302 is controlled to move parallel in the pitch direction without changing its height.

[0144] 16 and 17, the configuration and example of movement for moving the upper buoyancy material 302 in the pitch direction have been described, but it may also be moved in the roll direction. In this case, the upper body 3A may be equipped with a roll direction actuator, a slide unit, an active pulley, a passive pulley, and a belt. The buoyancy center control unit 47A may then control the roll direction actuator to move the upper buoyancy material 302 parallel to the roll direction without changing its height.

[0145] The processing procedure of the mobile body control device 100A is the same as the processing procedure of the mobile body control device 100 of the first embodiment. Moreover, the verification results of this embodiment are the same as those of the first embodiment.

[0146] As described above, in this embodiment, the deviation between the center of buoyancy and the center of gravity is targeted, and the deviation between the center of buoyancy and the center of gravity on the same vertical line is controlled to match the target value, thereby allowing the buoyancy material to move in the pitch or roll direction without changing its height.

[0147] As a result, according to this embodiment, when the posture of the arm is changed, a control angle amount for correcting the tilt of the body of the robot 1 can be obtained, and the tilt can be corrected using this corrective control amount. Furthermore, according to this embodiment, tilt error offset correction (deviation correction) can be performed by integral control. Furthermore, according to this embodiment, partial control can suppress (damping) the rise of vibration caused by a change in the posture of the arm.

[0148] In the above-described embodiments, the robot 1 (or 1A) that works underwater has been described as an example of a moving object, but the working environment is not limited to this. The working environment may be any environment in which buoyancy and a misalignment of the center of gravity occur.

[0149] Note that a program for realizing all or part of the functions of the mobile object control device 100 (or 100A) of the present invention may be recorded on a computer-readable recording medium, and the program recorded on the recording medium may be loaded into a computer system and executed to perform all or part of the processing performed by the mobile object control device 100 (or 100A). Note that the term "computer system" as used herein includes hardware such as an OS and peripheral devices. The term "computer system" also includes a WWW system equipped with a homepage provision environment (or display environment). The term "computer-readable recording medium" refers to portable media such as flexible disks, optical magnetic disks, ROMs, and CD-ROMs, as well as storage devices such as hard disks built into computer systems. The term "computer-readable recording medium" also refers to devices that retain a program for a certain period of time, such as volatile memory (RAM) within a computer system that serves as a server or client when a program is transmitted via a network such as the Internet or a communication line such as a telephone line. Alternatively, some or all of these components may be realized by hardware (including circuitry) such as an LSI (Large Scale Integration), an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), a GPU (Graphics Processing Unit), or an SOC (System On Chip), or may be realized by a combination of software and hardware.

[0150] The program may also be transmitted from a computer system storing the program in a storage device or the like to another computer system via a transmission medium or by transmission waves in the transmission medium. Here, the "transmission medium" that transmits the program refers to a medium that has the function of transmitting information, such as a network (communication network) such as the Internet or a communication line (communication line) such as a telephone line. The program may also be a program that realizes part of the above-mentioned functions. Furthermore, the program may be a so-called differential file (differential program) that can realize the above-mentioned functions in combination with a program already recorded in the computer system.

[0151] The above describes the form for carrying out the present invention using an embodiment, but the present invention is not limited to such an embodiment, and various modifications and substitutions can be made within the scope that does not deviate from the gist of the present invention. [Explanation of symbols]

[0152] 400, 400A... Mobile body control system, 1, 1A... robot, 200... operation unit, 201... controller, 202... image display unit, 203... communication unit, 2... robot body, 3... upper body, 4... lower body, 30... manipulator, 10... upper propulsion unit, 23... lower propulsion unit, 5... link 5, 6... link, 26... camera, 27... camera sensor, 28... camera drive unit, 35... position detection sensor, 70... lighting, 65... arm sensor, 66... ​​manipulator drive unit, 67... attitude sensor, 100, 100A... mobile body control device, 25... thruster drive unit, 8A... joint drive unit, 90... acquisition unit, 91... target setting unit, 92... control unit, 93... memory unit, 94... communication unit, 40... pitching drive unit, 50... rolling drive unit, 47, 47A... center of buoyancy control unit

Claims

1. A system for controlling a robot that operates underwater, comprising: The robot body, the floating device has a density relatively low with respect to a main body connected to the robot main body, and includes a drive unit capable of changing the center of buoyancy of the floating device with respect to the robot main body; an acquisition unit that acquires a combined center of buoyancy position of the robot body and the floating device, a combined center of gravity position of the robot body and the floating device, an angle between the combined center of buoyancy position and the center of gravity position, and an attitude angle of the robot body; a target setting unit that sets at least one of a target value of the angle between the target posture of the entire robot and the position of the center of gravity and a target value of the angle between the target posture of the entire robot and the position of the center of buoyancy; a buoyancy center control unit that calculates a control amount of the drive unit for changing the center of buoyancy of the floating device using a deviation between the center of buoyancy position and the center of gravity position, a target value of the deviation between the center of buoyancy position and the center of gravity position, a center of gravity sensitivity matrix that indicates a change in the center of gravity position relative to a control amount of the drive unit, and a center of buoyancy sensitivity matrix that indicates a change in the center of buoyancy position relative to a control amount of the drive unit; A mobile object control system comprising:

2. The buoyancy center control section is a feedback control is performed by inputting a differential value of the attitude angle of the robot body as a velocity term into a target value of the angle between the position of the center of buoyancy and the position of the center of gravity; The mobile object control system according to claim 1 .

3. a feedback control is performed by adding an integral term of a deviation of the attitude angle of the robot main body to a target value of the angle between the position of the center of buoyancy and the position of the center of gravity; 3. A mobile object control system according to claim 1 or 2.

4. the robot includes a movable part and a stationary part; the movable portion is connected to the stationary portion by a link that is movable in at least one of a pitch direction and a roll direction; the buoyancy control unit controls the position of the buoyancy center or the position of the center of gravity of the robot main body by moving the link in at least one of the pitch direction and the roll direction based on the calculated control amount of the drive unit.

3. A mobile object control system according to claim 1 or 2.

5. the robot includes a movable part and a stationary part; the movable unit includes a member that controls the position of the center of buoyancy or the position of the center of gravity of the entire robot by moving two-dimensionally, the buoyancy control unit controls the position of the buoyancy center or the position of the center of gravity of the robot main body by moving the movable unit in at least one of a pitch direction and a roll direction based on the calculated control amount of the drive unit.

3. A mobile object control system according to claim 1 or 2.

6. The buoyancy center control section is Convert the target value of the attitude angle into a target angle, converting the converted target angle into a deviation of a target position of the center of buoyancy and the center of gravity using the distance between the position of the center of buoyancy and the position of the center of gravity; converting the deviation of the converted target buoyancy center and gravity center position and the deviation between the buoyancy center position and the gravity center position into a control amount for the floating device; The mobile object control system according to claim 1 .

7. A mobile body control device for controlling a robot having a floating device that has a density relatively low compared to a main body connected to a robot main body that performs operations in water, and that includes a drive unit that can change the center of buoyancy of the floating device relative to the robot main body, an acquisition unit that acquires a combined center of buoyancy position of the robot body and the floating device, a combined center of gravity position of the robot body and the floating device, an angle between the combined center of buoyancy position and the center of gravity position, and an attitude angle of the robot body; a target setting unit that sets at least one of a target value of the angle between the target posture of the entire robot and the position of the center of gravity and a target value of the angle between the target posture of the entire robot and the position of the center of buoyancy; a buoyancy center control unit that calculates a control amount of the drive unit for changing the center of buoyancy of the floating device using a deviation between the center of buoyancy position and the center of gravity position, a target value of the deviation between the center of buoyancy position and the center of gravity position, a center of gravity sensitivity matrix that indicates a change in the center of gravity position relative to a control amount of the drive unit, and a center of buoyancy sensitivity matrix that indicates a change in the center of buoyancy position relative to a control amount of the drive unit; A mobile object control device comprising:

8. A control method for a mobile body control device that controls a robot having a floating device that has a density relatively low compared to a main body connected to a robot main body that performs operations in water, and that has a drive unit that can change the center of buoyancy of the floating device relative to the robot main body, comprising: an acquisition unit acquires a buoyancy center position of the robot body and the floating device combined, a center of gravity position of the robot body and the floating device combined, an angle formed between the buoyancy center position and the center of gravity position, and an attitude angle of the robot body; a target setting unit sets at least one of a target value of an angle formed by a target posture of the entire robot and the position of the center of gravity and a target value of an angle formed by a target posture of the entire robot and the position of the center of buoyancy, a buoyancy control unit calculates a control amount of the drive unit for changing the buoyancy of the floating device using a deviation between the buoyancy position and the center of gravity position, a target value of the deviation between the buoyancy position and the center of gravity position, a center of gravity sensitivity matrix indicating a change in the center of gravity position relative to a control amount of the drive unit, and a center of buoyancy sensitivity matrix indicating a change in the center of buoyancy position relative to a control amount of the drive unit. A mobile object control method.

Images