JP2025128995A - MOBILE BODY CONTROL SYSTEM, MOBILE BODY CONTROL DEVICE, AND MOBILE BODY CONTROL METHOD - Google Patents

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

The present invention relates to a mobile object control system, a mobile object control device, and a mobile object control method.

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 the direction of a first axis. The buoyancy material moves the center of buoyancy by moving in the direction of a second axis. The drive mechanism moves the weight in the direction of the first axis and the buoyancy material in the direction of the second axis in synchronization with each other (see, for example, Patent Document 1).

Patent No. 6167317

In conventional control methods, in order to maintain the aircraft at 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 amount of control angle of the buoyancy material is calculated, which requires constructing an approximation formula for each attitude of the drive parts, such as the arms.

The present invention was made in consideration of the above-mentioned problems, and aims to provide a mobile object control system, a mobile object control device, and a mobile object control method that can maintain the aircraft in a target attitude without constructing an approximation equation for each attitude of a drive unit such as a buoyancy material.

(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 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 the center of buoyancy position of the robot main body and the floating device combined, the center of gravity position of the robot main body and the floating device combined, 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 of the center of gravity position from a target attitude of the entire robot and a target value of the angle of the center of buoyancy position from the target attitude of the entire robot; a deviation (e.g., ^Δx ) between the center of buoyancy position and the center of gravity position; a center of gravity sensitivity matrix (e.g., _JCOB ), a center of buoyancy sensitivity matrix (e.g., J _COG ) that indicates a change in the center of buoyancy position relative to the control amount of the drive unit, and a center of buoyancy control unit that calculates the control amount of the drive unit for a change in the center of buoyancy of the floating device using the matrix.

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

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

(4) In a mobile object control system according to any one of the above (1) to (3), the robot may include a movable unit and a fixed unit, the movable unit connected to the fixed unit by a link that is movable in at least one of the pitch direction and the roll direction, and the buoyancy control unit may control 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.

(5) In a mobile object control system according to any one of the above (1) to (3), the robot may include a movable unit and a fixed unit, the movable unit may include 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 unit may control the position of the center of buoyancy or the position of the center of gravity of the robot main body by moving the movable unit in at least one of the pitch direction and the roll direction based on the calculated control amount of the drive unit.

(6) In the mobile body control system according to one aspect of (1) above, the buoyancy control unit may convert a target value (e.g., φ ^* ) of an attitude angle (e.g., φ) into a target angle (θ ^* cox), convert the converted target angle into a deviation (Δx ^* ) of the target buoyancy-center-of-gravity position using the distance (e.g., _lCoX ) between the buoyancy position and the center of gravity position, and convert the deviation of the converted target buoyancy-center-of-gravity position and the deviation (Δx) between the buoyancy position and the center of gravity position into a control amount (e.g., _qACOX ) for the floating device.

(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 the 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.

(8) In order to achieve the above-mentioned object, one aspect of the present invention provides a mobile body control method for controlling a mobile body 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 the center of buoyancy position of the robot main body and the floating device together, the center of gravity position of the robot main body and the floating device together, the angle between the center of buoyancy position and the center of gravity position, and the attitude angle of the robot main body. , a target setting unit sets at least one of a target value for the angle between the center of gravity position and the target posture of the entire robot and a target value for the angle between the center of buoyancy position and the target posture of the entire robot, and a buoyancy control unit 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 for 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.

According to the above (1) to (8), the aircraft can be maintained in a target attitude without constructing an approximation formula for each attitude of the drive unit such as the buoyancy material.
According to (2), vibrations can be suppressed even when the posture of the arm or the like provided 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.

FIG. 1 is a perspective view of a robot according to a first embodiment. FIG. 2 is an explanatory diagram of a pitching motion of the robot according to the first embodiment. FIG. 2 is an explanatory diagram of a rolling motion of the robot according to the first embodiment. 5A and 5B are diagrams for explaining an example of control by movement of the floating device in the first embodiment. 1 is a diagram illustrating an example of the configuration of a mobile object control system according to a first embodiment; FIG. 3 is a block diagram of a control process for the floating device according to the first embodiment. FIG. 4 is a diagram for explaining a conversion process to a target COX angle in the first embodiment. FIG. 10 is a diagram illustrating an example of a cause of a deviation. 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. FIG. 10 is a diagram for explaining conversion into a floating device drive control amount. 4 is a flowchart of a control procedure performed by the mobile body control device according to the first embodiment. 10 shows an example of a verification result in the case of control by the conventional technology. 10 shows an example of a verification result in the case of the control method according to the first embodiment. 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. 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. FIG. 10 is a perspective view of a robot according to a second embodiment. FIG. 10 is a diagram showing an example of the configuration of the upper part of the main body (movable part) of the second embodiment, as viewed from above. FIG. 10 is a diagram illustrating an example of the configuration of a mobile object control system according to a second embodiment. FIG. 10 is a diagram for explaining conversion into a floating device drive control amount.

Embodiments of the present invention will be described below with reference to the drawings. In the following description, a robot (ROV: Remote Operating Vehicle) that operates underwater (an example of an underwater environment) by remote control such as wired communication will be used 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 an arrangement in the strict sense, but also include 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 adjusted to make each component recognizable.

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).

[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.

<Robot>
First, the external appearance of a 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.

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

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

The upper thruster 10 is equipped with, 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, causing the robot 1 to move upward (ascend). For example, the upper thruster 10 rotates the propeller in the other direction around the axis, causing the robot 1 to move downward (descend).

The upper body part 3 is provided with an attachment section 11 for, for example, power lines for supplying power to the 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 section 11 in the upper body part 3. An attitude detection sensor (e.g., a gyro sensor) for detecting the attitude of the robot 1 (such as rotation and orientation in the front-to-back, width, and up-to-down directions) may be provided near the attachment section 11. For example, the attitude detection sensor may be provided in a location where an arm is attached. In the following description, the upper body part 3 is also referred to as "Upper."

<Bottom of the unit>
The lower body part 4 is located below 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."

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-to-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 with its longitudinal axis extending in the width direction is provided at the lower front portion of the frame 20.

The lower body portion 4 is provided with multiple lower propulsion units 23L, 23R, 24L, and 24R for moving the robot 1 in the forward/backward and width directions. The multiple lower propulsion units 23L, 23R, 24L, and 24R consist of 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 widthwise, and a pair of left and right rear thrusters 24L and 24R for moving the robot 1 backward or widthwise.

The front thrusters 23L, 23R are provided, for example, at the front of the frame 20. The front thrusters 23L, 23R are equipped, for example, with 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.

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 to the front of the lower main body 4.

Thruster drive units 25L and 25R are provided in the lower main body section 4 to provide driving force (rotational force for each propeller) to each thruster 10, 23L, 23R, 24L, and 24R. A pair of thruster drive units 25L and 25R are provided on the left and right sides at the front of the frame 20.

For example, a camera 26 is provided in the lower body 4 .
The lights 70L and 70R are provided, for example, on the front side of the lower main body portion 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.

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 configured by 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.

The hand 32 is provided at the tip 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.

For example, the lower main body 4 is provided with a position detection sensor 35 that detects the position of the robot 1 (e.g., the distance from the seabed to the robot 1). The position detection sensor 35 is, for example, 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 Doppler Velocity Log (DVL) sensor for constant altitude navigation, an internal pressure sensor for constant depth navigation, an inertial measurement unit (IMU), etc.
Further, for example, power supply systems 36 and 37 are provided in the lower body portion 4 .

The lower body portion 4 may be provided with a weight installation area 38 for installing weights. The weight installation area 38 is provided, for example, at the rear of the frame 20, behind the power supply system 37.

The robot 1 is equipped with manipulators 30L and 30R and power supply systems 36 and 37. The manipulators 30L and 30R are provided at the front of the lower main body 4 (an example of one longitudinal side). The power supply systems 36 and 37 are provided at the rear of the lower main body 4 (an example of the other longitudinal side). The manipulators 30L and 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 and 37, via the opening 21.

<Link>
The upper body 3 and the lower body 4 are connected by a plurality of links 5L, 5R, 6L, and 6R at a connecting portion 7. The links 5L, 5R, 6L, and 6R are arranged parallel to one another. The 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 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 the 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.

<Joints>
The connecting unit 7 includes joints 8A and 8P that are rotatable 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.

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

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

<Joint drive unit>
The joint drive unit 8A is provided with, as actuators 9, 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.

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 speed or less; a reducer that further reduces the rotation reduced by the driven pulley; and a case that houses the pitching motor and driven pulley. The pitching drive device 40 is controlled by a center of buoyancy control unit 47 (see Figure 5). For an example of the structure of the pitching drive device 40, see, for example, Japanese Patent Application No. 2023-029174.

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 driven pulley. The rolling drive device 50 is controlled by a center of buoyancy control unit 47 (see Figure 5). For an example structure of the rolling drive device 50, see, for example, Japanese Patent Application No. 2023-029174.

<Passive joints>
Below, we will explain the configuration of the passive joint 8P provided at the lower end of the left front link 5L out of the seven passive joints 8P. The passive joints 8P provided at other parts have the same configuration as the passive joint 8P provided at the lower end of the left front link 5L, so detailed explanations 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.

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

<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 the axis in the width direction), the multiple driven joints 8P rotate synchronously around their axes (around the axis in the width direction). This causes the robot main body 2 to rotate in the pitch direction. In the example shown in the figure, the robot main body 2 rotates counterclockwise (an example of one direction in the pitch direction) when viewed from the left side.

<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 main body 2 to rotate in the roll direction. In the example shown in the figure, the robot main body 2 rotates clockwise when viewed from the front (an example of one direction in the roll direction).

<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. Figure 4 is a diagram for explaining an example of control by movement of the floating device of this embodiment. Note that in this 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 this embodiment, "low density" means a high buoyancy and a low weight, i.e., a low density.

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 explanation, 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".

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.

Reference symbol g30 represents an example of a state when the deviation ΔX _err between the center of buoyancy and the center of gravity of the aircraft is not controlled. In this case, the attitude of the aircraft will tilt as shown by reference symbol g30 in order to balance the buoyancy B and the gravity M of the aircraft.

Reference symbol g40 indicates an example of a state when the deviation between the center of buoyancy and the center of gravity of the aircraft is controlled using the method of this embodiment. Reference symbol g41 indicates 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. Furthermore, q _ACOX1 indicates the control angle of the joint. Note that q _ACOX1 is, for example, an angle in the pitch direction.

<Configuration example of a mobile control system>
Next, a configuration example of the mobile object control system 400 of this embodiment will be described. Fig. 5 is a diagram showing a configuration example of the 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.

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

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

The mobile body control device 100 includes, for example, a thruster drive unit 25, a center of buoyancy control unit 47, an acquisition unit 90, a target setting unit 91, a control unit 92, a memory unit 93, and a communication unit 94.

(Operation unit)
The operation unit 200 is used by an operator on a ship, for example. The operation unit 200 and the mobile body 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 and displays images captured by the camera 26, the state of the robot main body 2 and the arm 31, etc. from the mobile object control device 100.
The communication unit 203 transmits and receives information to and from the mobile object control device 100 .

(robot)
The robot body 2 is the portion other than, for example, the arm 31 in Fig. 1. The robot body 2 has attached thereto the various portions described with reference to Fig. 1 .

Camera 26 is, for example, a photographing device using a CMOS (Complementary Metal Oxide Semiconductor) image sensor or a CCD (Charge Coupled Device) image sensor. Camera 26 may also be an RGB (Red, Green, Blue) D camera that can also obtain depth information D.

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

The camera driver 28 tilts the camera 26, for example, in the pan axis direction, based on a camera control instruction included in a control command from the controller 92. The camera driver 28 includes, for example, an actuator and a driver circuit.

The arm sensor 65 may be, for example, an encoder attached to a joint, a six-axis sensor attached to the hand, or a tactile sensor.

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

The attitude sensor 67 is a sensor that detects the pitch direction and roll direction tilt angle of the robot main body 2 and floating device, the attitude angle of the aircraft, etc. The attitude sensor 67 may also be an acceleration sensor, 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 well-known methods (e.g., Takahashi Tomohiro, Hatano Masatoshi, "Research on Attitude Control of Underwater Mobile Manipulators," 25th Transportation and Logistics Conference (TRANSLOG2016), Japan Society of Mechanical Engineers, 2016).

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

The center of buoyancy control unit 47 determines control angles q _ACOX1 and q _ACOX2 for eliminating the center of buoyancy in the pitch and roll directions (or x-axis and y-axis directions). Note that q _ACOX2 is, for example, the angle in the roll direction. Using the determined control angles q _ACOX1 and q _ACOX2 , the center of buoyancy control unit 47 controls the links 5 and 6 to move the upper body 3 in at least one of the pitch and roll directions. The control method used by the center of buoyancy control unit 47 will be described later.

The acquisition unit 90 acquires the combined center of buoyancy position of the main body and floating device, the combined center of gravity position of the 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.

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

The control unit 92 controls, for example, the operation of the robot body 2, arm 31, camera 26, etc.

The memory unit 93 stores programs, thresholds, predetermined values, formulas, etc. necessary for controlling the mobile body control device 100. The memory unit 93 stores, for example, three-dimensional models of the robot main body 2 and arm 31.

The communication unit 94 sends and receives information to and from the operation unit 200.

<Floating device control process>
Next, the control process of the floating device will be described.

Figure 6 is a block diagram of the control processing of the floating device according to this embodiment. The processing of each part in Figure 6 is performed by the center of buoyancy control unit 47. In Figure 6, A ^* is the target value of A, φ is the attitude angle of the aircraft, θ _cox is the angle between the center of buoyancy and the center of gravity, q _ACOX is the state of the driving unit of the floating device, q _all is the state of all driving units of the aircraft including the floating device, Δx is the deviation of the center of buoyancy and the center of gravity position, and l _COX is the distance between the center of buoyancy and the center of gravity.

The first conversion unit 471 converts the input target value φ ^* of the attitude angle φ of the aircraft into a target COX angle θ ^* _cox and outputs it to the second calculation unit 477.

The first calculation unit 472 subtracts the attitude angle φ of the aircraft from the target value φ ^* of the attitude angle φ of the aircraft, and outputs the result to the integrator 473 and the differentiator 475 .

The integrator 473 integrates the attitude angle deviation (φ ^* −φ) from the target attitude angle, which is the calculation result of the first calculation unit 472 .
The first coefficient unit 474 multiplies the result of integration by the integrator 473 by a coefficient _Ki and outputs the result to the second calculation unit 477. The processing by the integrator 473 and the first coefficient unit 474 is deviation correction.

The differentiator 475 differentiates the attitude angle deviation (φ ^* −φ) from the target attitude angle, which is the calculation result of the first calculation unit 472 .
The second coefficient unit 476 multiplies the result differentiated by the differentiator 475 by a coefficient _Kd and outputs the result to the second calculation unit 477. The processing of the differentiator 475 and the second coefficient unit 476 is vibration suppression.
The coefficients K _i and K _d are also gains and are determined in advance, for example, by simulation.

The second calculation unit 477 adds the output of the first coefficient unit 474 and the output of the second coefficient unit 476 to the target COX angle θ ^* _cox , which is the output of the first conversion unit 471 , and outputs the result to the second conversion unit 478 .

The second conversion unit 478 uses the output of the second calculation unit 477 and the distance l _CoX between the center of buoyancy and the center of gravity, which is the output of the center of gravity/center of buoyancy calculation unit 482, to convert it into the deviation Δx ^* of the target center of buoyancy and center of gravity position, and outputs the converted deviation Δx ^* of the target center of buoyancy and center of gravity position to the third calculation unit 479.

The third calculation unit 479 subtracts the deviation Δx of the center of buoyancy and center of gravity position, which is the output of the center of gravity/center of buoyancy calculation unit 482, from the deviation Δx ^* of the target center of buoyancy and center of gravity position, which is the output of the second conversion unit 478, and outputs the subtraction result to the third conversion unit 480.

The third conversion unit 480 converts the output of the third calculation unit 479 into a control amount q _ACOX of the floating device driving unit, and outputs the converted control amount q _ACOX of the floating device driving unit to the plant 481. Note that q _ACOX is expressed by the following equation (1).

Plant 481 is an image of all drive units of the aircraft, including the floating device, and the position and attitude of the aircraft in an underwater environment. Note that sensors for detecting the state of the drive units and attitude sensor 67 for detecting the attitude are attached to the floating device, drive units, or around the drive units. Plant 481 outputs q _all to center of gravity/center of buoyancy calculation unit 482, and outputs the attitude angle φ of the aircraft to first calculation unit 472. Note that q _all is expressed by the following equation (2). Note that q _ACOX1 , q _ACOX2 , and q _others may be angles or lengths.

The center of gravity and center of buoyancy calculation unit 482 calculates the center of gravity and the center of buoyancy using q _all output by Plant 481. Using the calculation results, the center of gravity and center of buoyancy calculation unit 482 calculates the deviation Δx of the center of buoyancy and the distance l _COX between the center of buoyancy and the center of gravity.

(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.

7, θ _COX (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 z-direction is the up-and-down direction of the robot body.
Here, if the line segment L _act is vertical in the inertial coordinate system, then θ _cox coincides with the angle φ between the horizontal direction in the inertial coordinate system and the aircraft, as shown in the following equation (3). As a result, the target value φ ^* of the aircraft's attitude angle φ can be converted into the target COX angle θ ^* _cox , as shown in the following equation (4). The center of buoyancy control unit 47 acquires the aircraft's attitude angle φ via the acquisition unit 90, for example, based on the detection value of the attitude sensor 67.

When there is no external force, the center of buoyancy and the center of gravity are aligned on a vertical line, and 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 when the center of gravity moves within the aircraft (due to movement of arm 31, for example), the angle between the center of buoyancy and the center of gravity can be made the same as the target attitude, allowing the aircraft to be maintained in the target attitude.

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

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 control is performed so that the attitude angular velocity φ ^· becomes 0. Furthermore, by using integral control, control is performed so that the attitude angle deviation (φ ^* -φ) from the target attitude angle becomes 0.

(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 the deviation of the target center of buoyancy and center of gravity position performed by the second conversion unit 478. Fig. 9 is a diagram for explaining the conversion process into the deviation of the target center of buoyancy and 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 on the xz plane, Δx ^* is the target value of the deviation between the center of buoyancy and the center of gravity, and θ ^* _cox is the target COX angle.
Using the relationship of trigonometric functions, the deviation Δx ^* between the center of buoyancy and the center of gravity 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 convert the target COX angle θ ^* _cox into the target value Δx ^* of the deviation between the center of buoyancy and the center of gravity. Note that the second conversion unit 478 obtains the distance l _CoX-XZ between the center of buoyancy and the center of gravity in the xz plane from the center of gravity/center of buoyancy calculation unit 482.

Note that the example explained using Figure 9 is for the pitch direction, but the same calculation can be performed for the roll direction.

(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.

The third conversion unit 480 obtains from the third calculation unit 479 the result of subtracting the deviation Δx between the center of buoyancy and the center of gravity from the target value Δx ^* of the deviation between the center of buoyancy and the center of gravity. Here, the deviation Δx between the center of buoyancy and the center of gravity is given by the following equation (6). The center of buoyancy position x _COB is given by the following equation (7), and the center of gravity position x _COG is given by the following equation (8). Note that in equation (6), ΔX _err is the positional deviation between the center of buoyancy and the center of gravity in the x-axis direction, and ΔY _err is the positional deviation between the center of buoyancy and the center of gravity in the y-axis direction. Furthermore, X _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, Y _G is the center of gravity position in the y-axis direction, and Y _B is the center of buoyancy position in the y-axis direction.

In this embodiment, the target is to set the deviation between the center of buoyancy and the center of gravity in the ROV (Remote Operating Vehicle) coordinate system to a predetermined target value Δx ^* at the control angle q _ACOX1 of the floating device driver. The target value of the deviation is given by the following equation (9).

As a result, equation (9) can be expressed as equation (10) below, and the deviation is given by equation (11) below.

The center of buoyancy sensitivity matrix J _COB , which is a Jacobian matrix, is given by the following equation (12), and the center of gravity sensitivity matrix J _COG , which is a Jacobian matrix, is given by the following equation (13).

Here, if the difference matrix between the center of buoyancy sensitivity and the center of gravity sensitivity is regular, the control variable q can be calculated using the inverse matrix as shown in the following equation (14).

The center of buoyancy control unit 47 uses the control variable q calculated in this way to control the posture of the robot 1, for example, by PID (Proportional-Integral-Differential) control.

<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.

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

(Step S2) The center of buoyancy control unit 47 acquires the attitude angle of the aircraft using the detection value acquired by the acquisition unit 90.

(Step S3) The target setting unit 91 sets a target value for the angle between the center of buoyancy and the center of gravity based on 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.

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

(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 a target COX angle θ ^* cox.

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

(Step S7) The second conversion unit 478 of the center of buoyancy control unit 47 uses the output of the second calculation unit 477 and the distance l _CoX between the center of buoyancy and the center of gravity, which is the output of the center of gravity/center of buoyancy calculation unit 482, to convert it into the deviation Δx ^* of the target center of buoyancy and center of gravity position.

(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 a control amount q _ACOX for the floating device driving unit.

(Step S9) The center of buoyancy control unit 47 uses the calculated control variable q to control the posture of the robot 1, for example, by PID control.

(Step S10) The center of buoyancy control unit 47 determines whether control has ended. The center of buoyancy control unit 47 may determine the start or end of control, for example, based on 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.

<Verification results>
Next, an example of the verification result will be described.
Figure 12 shows an example of verification results for control using conventional technology. The graph indicated by reference symbol g200 shows an example of change in the shoulder joint angle of the arm. In the graph indicated by reference symbol g200, the horizontal axis represents time (sec) and the vertical axis represents the shoulder joint angle (deg) of the arm. The graph indicated by reference symbol g210 shows an example of change in the angle in the pitch direction. In the graph indicated by reference symbol 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, the maximum deviation in pitch angle was approximately 15 degrees when controlled using the conventional method.

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 represents time (sec) and the vertical axis represents 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 represents time (sec) and the vertical axis represents the angle in the pitch direction (deg).
As shown in the graph with reference symbol g230, in the control method of this embodiment, the maximum deviation of the pitch angle was 4 degrees or less.

In this way, the control method of this embodiment can correct the tilt of the robot body caused by changing the posture of the robot's arm.

(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.
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 g250 shows an example of the change in arm length when the arm is extended from a retracted state. In the graph g250, the horizontal axis represents time (sec) and the vertical axis represents arm length.

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. Furthermore, the vibration damping ratio was reduced from 0.1 without vibration suppression control to 0.74 with vibration suppression control.

(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 represents time (sec), and the vertical axis represents 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 end effector and the robot is tilted.

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 specified value (target value), integral control was able to correct a tilt offset of 190 mm to within the specified value in approximately 18 seconds.

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

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 calculated, 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's posture. In other words, by incorporating D control into PID control, it is possible to control so that vibrations quickly subside.

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 the height.

<Robot>
The external shape of the robot 1A according to 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 main body 2, which is the main body portion of the robot 1. The robot main body 2 includes an upper main body part 3A (movable part) located at the top of the robot main body 2, and a lower main body part 4 (stationary part) located at the bottom of the robot main 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 portion 3A includes a cover 301, an upper buoyancy member 302, a frame 303, and a frame 304. The configuration of the upper main body portion 3A will be described in detail with reference to FIG.

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 main 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).

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

Actuator 307L drives active pulley 308L, thereby rotating belt 310L.
The driven pulley 309L rotates due to 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 rotates due to 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.

In this embodiment, controlling the actuator 307 in this manner causes the rotation of the belt 310 to move the upper buoyancy member 302 attached to the slide sections 305 and 306 in the x-axis and y-axis directions. The actuator 307 is equipped with actuators for x-axis movement and y-axis movement. As a result, according to this embodiment, by controlling these two actuators, the two actuators move in coordination in the x-axis and y-axis directions.

<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.

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 mobile body control device 100A includes, for example, a thruster drive 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.

The center of buoyancy control unit 47A detects the positions of the center of buoyancy and the center of gravity, and calculates a control angle q _ACOX1 based on the detected results to eliminate the deviation between the center of buoyancy and the center of gravity in the pitch direction (e.g., the x-axis direction). Using the calculated control angle q _ACOX1 , the center of buoyancy control unit 47A controls the actuator 307 to move the upper body 3A in the pitch direction by translating it without changing its height.

(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. Note that the blocks for the floating device control process are the same as those in FIG. 6 of the first embodiment.

Figure 19 is a diagram illustrating the conversion to a floating device drive control amount. Reference symbol g200 is an example of the state before the arm is extended. 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.

The third conversion unit 480 obtains the result of subtracting the deviation Δx between the center of buoyancy and the center of gravity from the target value Δx ^* of the deviation between the center of buoyancy and the center of gravity 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). Furthermore, the center of buoyancy position x _COB is given by 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 calculated in this manner to control the posture of the robot 1A, for example, by PID control.

In this embodiment, the deviation between buoyancy and reception is set to a target value Δx ^* , and the upper buoyancy material 302 is controlled to move parallel in the pitch direction without changing its height so that the deviation Δx between the buoyancy and the center of gravity on the same vertical line is set to the target value Δx ^* .

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

The processing procedure of the mobile body control device 100A is similar to the processing procedure of the mobile body control device 100 of the first embodiment.
Furthermore, the verification results of this embodiment are the same as those of the first embodiment.

As described above, in this embodiment, the deviation between the center of buoyancy and the center of gravity is targeted, and by controlling the deviation between the center of buoyancy and the center of gravity on the same vertical line to match the target value, the buoyancy material can be moved in the pitch or roll direction without changing its height.

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 correction 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.

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.

It should be noted 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 includes 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.

The above program may also be transmitted from a computer system that stores the program in a storage device or the like to another computer system via a transmission medium, or by transmission waves within 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 above program may also be one that realizes part of the above-mentioned functions. Furthermore, it may be a so-called differential file (differential program) that can realize the above-mentioned functions in combination with a program already recorded on the computer system.

The above describes the form for carrying out the present invention using an embodiment, but the present invention is in no way limited to such an embodiment, and various modifications and substitutions can be made without departing from the spirit of the present invention.

400, 400A... Mobile object 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... Links 5, 6... Links, 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 object 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 (8)

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 formed between the 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: 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 . 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. 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. 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. 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 . 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 formed between the 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: 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.