MOBILE OBJECT CONTROL SYSTEM, MOBILE OBJECT CONTROL DEVICE AND MOBILE OBJECT CONTROL METHOD

Abstract

A mobile object control system includes a robot body, a float device having a lower density than a body connected to the robot body and including a drive unit capable of changing a buoyancy center of the float device with respect to the robot body, a sensing device, a setpoint setting device, and a buoyancy center controller.The buoyancy center of gravity controller is configured to calculate a control value for the drive unit for changing the buoyancy center of gravity of the float device using a deviation between a position of the buoyancy center of gravity and a position of the center of gravity, a target value of the deviation, a center of gravity sensitivity matrix indicating the change in the position of the center of gravity with respect to the control value, and a buoyancy center of gravity sensitivity matrix indicating a change in the position of the buoyancy center of gravity with respect to the control value.

Description

BACKGROUND OF THE INVENTIONField of the invention

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

[0002] There is an underwater robot, which is a device that performs an operation underwater. Such an underwater robot includes, for example, a frame, a thrust unit, a weight, a buoyancy element, and a drive mechanism. The thrust unit includes multiple thrusters that generate thrust. The weight moves a position of the center of gravity of the weight by moving along a first axis. The buoyancy element moves a position of the center of gravity of the buoyancy element by moving along a second axis. The drive mechanism moves the weight in the direction of the first axis and moves the buoyancy element in the direction of the second axis in synchronization with each other (see, for example, Japanese Patent No. JP 6 167 317 B). SUMMARY OF THE INVENTION

[0003] For example, in a conventional control method, since a relationship between an angle formed by the center of gravity of lift and the center of weight and an operation amount of a lift element is linearly approximated, and a control angle value for the lift element is calculated in order to keep the vehicle body at a target attitude, an approximate expression must be constructed for each attitude of a driven portion such as an arm.

[0004] The present invention has been made in view of the above-mentioned circumstances, and its object is to provide a mobile object control system, a mobile object control device, and a mobile object control method that can maintain a vehicle body in a target attitude without constructing an appropriate expression for each attitude of a driven portion such as a buoyancy member. (1) To achieve the above-mentioned object, according to one aspect of the present invention, there is provided a mobile object control system that controls a robot that performs an underwater operation, the mobile object control system comprising: a robot body; a float device that has a lower density than a body, which is connected to the robot body and includes a drive unit capable of changing the buoyancy center of the float device with respect to the robot body; a detection device configured to detect a position of the buoyancy center of the robot body and the float device in combination, a position of the center of gravity of the robot body and the float device in combination, an angle formed by the position of the buoyancy center of gravity and the position of the center of gravity, and an attitude angle of the robot body; a setpoint setting device configured to set a setpoint of an angle formed by the position of the center of gravity from a setpoint position of the entire robot and / or a setpoint of an angle formed by the position of the center of buoyancy from the setpoint position of the entire robot; and a buoyancy center controller configured to set a control value for the drive unit for a change in the buoyancy center of gravity of the float device by means of a deviation (e.g., Δx) between the position of the center of buoyancy and the position of the center of gravity, a setpoint (e.g., Δx*) of the deviation between the position of the center of buoyancy and the position of the center of gravity, a center of gravity sensitivity matrix (e.g. J COB), which indicates a change in the position of the center of gravity with respect to the control value for the drive unit, and a buoyancy center sensitivity matrix (e.g. J COG ), which indicates a change in the position of the center of lift with respect to the control value for the propulsion unit. (2) In the mobile object control system according to aspect (1), the buoyancy center of gravity controller may be configured to perform feedback control in which a differential value of the attitude angle of the robot body is substituted as a speed term into the command value of the angle (e.g., COX angle) formed by the position of the buoyancy center of gravity and the position of the weight center of gravity. (3) In the mobile object control system according to aspect (1) or (2), the buoyancy center of gravity controller may be configured to perform feedback control in which an integral term of a deviation of the attitude angle of the robot body is substituted into the command value of the angle (e.g., COX angle) formed by the position of the buoyancy center of gravity and the position of the weight center of gravity. (4) In the mobile object control system according to any one of aspects (1) to (3), the robot may include a movable portion and a stationary portion, wherein the movable portion may be connected to the stationary portion by a link movable in a pitch direction and / or a roll direction, and wherein the buoyancy center of gravity controller may be configured to control the position of the buoyancy center of gravity or the position of the weight center of gravity of the robot body by moving the link in the pitch direction and / or the roll direction based on the calculated control value for the drive unit. (5) In the mobile object control system according to any one of aspects (1) to (3), the robot may include a movable portion and a stationary portion, wherein the movable portion may include an element for controlling the position of the center of gravity of the entire robot by two-dimensional movement, and wherein the center of gravity controller may be configured to control the position of the center of gravity of the robot body by moving the movable portion in the pitch direction and / or the roll direction based on the calculated control value for the drive unit. (6) In the mobile object control system according to the aspect of (1), the buoyancy center controller may be configured to: convert the target value (e.g., ϕ*) of the attitude angle (e.g., ϕ) into a target angle (e.g., θ*cox); convert the converted target angle into a target deviation (e.g., Δx*) between the position of the buoyancy center and the position of the center of gravity by means of a distance (e.g., Icox) between the position of the buoyancy center and the position of the center of gravity; and Converting the converted target deviation between the position of the center of buoyancy and the position of the center of gravity and the deviation (e.g. Δx) between the position of the center of buoyancy and the position of the center of gravity into a drive control value (e.g. q ACOX ) for the float device. (7) To achieve the above-mentioned object, according to another aspect of the present invention, there is provided a mobile object control device that controls a robot including a float device having a lower density than a body connected to the robot body, performing an underwater operation, and including a drive unit capable of changing the buoyancy center of the float device with respect to the robot body. The mobile object control device includes: a detection device configured to detect a position of the buoyancy center of the robot body and the float device in combination, a position of the center of gravity of the robot body and the float device in combination, an angle formed by the position of the buoyancy center of gravity and the position of the center of gravity, and an attitude angle of the robot body; a target value setting device configured to set a target value of an angle formed by the position of the center of gravity from a target attitude of the entire robot and / or a target value of an angle formed by the position of the buoyancy center of gravity from the target attitude of the entire robot;and a center of buoyancy controller configured to calculate a control value for the drive unit for a change in the center of buoyancy of the float device using a deviation between the position of the center of buoyancy and the position of the center of gravity, a target value of the deviation between the position of the center of buoyancy and the position of the center of gravity, a center of gravity sensitivity matrix indicating a change in the position of the center of gravity with respect to the control value for the drive unit, and a center of buoyancy sensitivity matrix indicating a change in the position of the center of buoyancy with respect to the control value for the drive unit; (8) To achieve the above-mentioned object, according to another aspect of the present invention, there is provided a mobile object control method for controlling a robot including a float device having a lower density than a body connected to the robot body, which performs an underwater operation and includes a drive unit capable of changing the buoyancy center of the float device with respect to the robot body, the mobile object control method comprising: Detecting a position of the center of buoyancy of the robot body and the float device in combination, a position of the center of gravity of the robot body and the float device in combination, an angle formed by the position of the center of buoyancy and the position of the center of gravity, and a position angle of the robot body; setting a target value of an angle formed by the position of the center of gravity from a target position of the entire robot, and / or a target value of an angle formed by the position of the center of buoyancy from the target position of the entire robot; and

[0005] Calculating a control value for the drive unit for a change in the center of buoyancy of the float device by means of a deviation between the position of the center of buoyancy and the position of the center of weight, a target value of the deviation between the position of the center of buoyancy and the position of the center of weight, a center of weight sensitivity matrix indicating a change in the position of the center of weight with respect to the control value for the drive unit, and a center of buoyancy sensitivity matrix indicating a change in the position of the center of buoyancy with respect to the control value for the drive unit.

[0006] According to aspects (1) to (8), it is possible to maintain a vehicle body in a desired attitude without constructing an appropriate expression for each attitude of a driven portion such as a buoyancy member.

[0007] According to the aspect of (2), it is possible to prevent vibration even if the posture of the arm or the like arranged in a robot changes.

[0008] According to the aspect (3), it is possible to avoid the change of an operating position of a fingertip of an arm even when the posture of the arm arranged in a robot or the like changes. BRIEF DESCRIPTION OF THE DRAWINGS Fig. 1 is a perspective view of a robot according to a first embodiment. Fig. Figure 2 is a diagram showing a pitching motion of the robot according to the first embodiment. Fig. 3 is a diagram showing a rolling motion of the robot according to the first embodiment. Fig. 4 is a diagram showing an example of control associated with the movement of a float device according to the first embodiment. Fig. 5 is a diagram showing an example of a configuration of a mobile object control system according to the first embodiment. Fig. 6 is a block diagram showing a control process for the float device according to the first embodiment. Fig. 7 is a diagram showing a process of converting a target COX angle according to the first embodiment. Fig. Figure 8 is a diagram showing an example of factors contributing to failure. Fig. 9 is a diagram showing a process of converting a target deviation between a position of the center of lift and a position of the center of weight in a pitch direction. Fig. 10 is a diagram showing the conversion of a float device drive control value. Fig. 11 is a flowchart showing an example of a control procedure performed by a mobile object control device according to the first embodiment. Fig. 12 is a diagram showing a verification result in the controller according to the related art. Fig. 13 is a diagram showing a verification result in a control method according to the first embodiment. Fig. 14 is a diagram showing an example of a detection result of vertical vibrations of an operation area which is a fingertip position of an end effector when damping control has been performed according to the first embodiment and when the damping control has not been performed. Fig. 15 is a diagram showing an example of change of an operation range which is a fingertip position of an end effector when deviation displacement correction damping control has been performed according to the first embodiment. Fig. 16 is a perspective view of a robot according to a second embodiment. Fig. 17 is a plan view showing an example of a configuration of an upper body (movable portion) according to the second embodiment. Fig. 18 is a diagram showing an example of a configuration of a mobile object control system according to the second embodiment. Fig. 19 is a diagram showing the conversion for a float device drive control value. DETAILED DESCRIPTION OF THE INVENTION

[0009] Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. In the following description, a robot (remotely operated work vehicle (ROV)) that performs an operation under the sea (as an example, underwater) by remote control via wired connection or the like will be described as an example of a robot that is an example of a mobile object. In the following description, terms expressing a relative or absolute arrangement, such as "parallel," "orthogonal," "center," and "coaxial," indicate such an arrangement and also include a relative displacement with a tolerance or such an angle or distance with which the same function is achieved. In the drawings used for the following description, the scales of components are appropriately changed to show the components in recognizable sizes.

[0010] In all drawings used to describe embodiments, components with the same functions are designated by the same reference numerals, and their repeated description is omitted.

[0011] The "based on XX" mentioned in this description means "based on at least XX" and includes "based on another element in addition to XX." "Based on XX" is not limited to using XX directly and also includes using results obtained by performing calculations or processes on XX. "XX" is any factor (for example, any information). [First version]

[0012] For example, in a first embodiment, it is assumed that an upper body and a lower body of a robot are connected by a link mechanism. <roboter>

[0013] An outer shape or the like of a robot 1 according to the present embodiment will be described first. Fig. 1 is a perspective view of the robot according to the present embodiment.

[0014] As in Fig. As shown in Figure 1, the robot 1 includes a robot body 2, which is a body of the robot 1. The robot body 2 includes an upper body 3 (movable portion) disposed in an upper part of the robot body 2, and a lower body 4 (stationary portion) disposed at a lower part of the robot body 2.

[0015] In the following description, a direction in which the robot 1 moves forward is defined as "front," a direction opposite to the front is defined as "back," a right direction in which the robot 1 moves forward is defined as "right side," a left direction in which the robot 1 moves forward is defined as "left side," and a right-left direction of the robot 1 is defined as "width direction." An up-down direction of the robot 1 is defined as a direction orthogonal to the longitudinal direction and the width direction of the robot 1. The top side of the robot 1 is a side on which the upper body 3 is arranged in the up-down direction of the robot 1. The bottom of the robot 1 is a side (side where the lower body 4 of the robot 1 is arranged) opposite in the up-down direction to the side where the upper body 3 of the robot 1 is arranged.In the example shown in the drawing, the robot 1 is arranged horizontally. The up-down direction of the robot 1, the top of the robot 1, and the bottom of the robot 1 correspond to the up-down direction (vertical direction), the vertical top and the vertical bottom in the state where the robot 1 is arranged horizontally. In the following description, the reference symbol L may be added to the ends of elements on the left side of the robot 1, and the reference symbol R may be added to the ends of elements on the right side. <Oberer Körper>

[0016] The upper body 3 is arranged, for example, in the upper part of the robot body 2. The upper body 3 has a greater buoyancy force than the lower body 4. The upper body 3 has, for example, a rectangular outer shape in plan view. To keep the robot 1 horizontal, for example, a ballast and a buoyancy element are arranged in the upper body 3. An upper thruster 10 for moving the robot 1 in the vertical direction (hereinafter also referred to as "upper thruster 10") is arranged in the upper body 3. The upper thruster 10 is arranged at a position which is the longitudinal center and the width center of the upper body 3.

[0017] For example, the upper thruster 10 includes a propeller that rotates around a vertical shaft of the upper body 3. For example, the upper thruster 10 moves (lifts) the robot 1 upwards by rotating the propeller around the shaft in one direction. For example, the upper thruster 10 moves (lowers) the robot 1 downwards by rotating the propeller around the shaft in the other direction.

[0018] For example, a holder 11 for a power line for supplying electric power to the components of the robot 1 or a signal line for transmitting a signal (not shown) is arranged in the upper body 3. A through-hole 12 through which the power line or the signal line passes is formed in front of or behind the holder 11. A position sensor (for example, a gyro sensor) for detecting a position (rotation or orientation in the longitudinal direction, the width direction, and the vertical direction) of the robot 1 may be arranged near the holder 11. For example, the position sensor may be arranged at a position where an arm is attached. In the following description, the upper body 3 may also be referred to as the "top." <Unterer Körper>

[0019] The lower body 4, for example, is arranged in the lower section of the robot body 2. The lower body 4 has a greater weight and a lower buoyancy force (volume) than the upper body 3. The lower body 4 has a rectangular outer shape, for example, in plan view. For example, to keep the robot 1 horizontal and make the weight greater than that of the upper body 3, a weight is arranged in the lower body 4. In the following description, the lower body 4 may also be referred to as the "lower part."

[0020] The lower body 4 includes, for example, a frame 20, which has a rectangular outer shape in plan view. The frame 20 has, for example, a rectangular outer shape that is long in the longitudinal direction. An opening 21, for example, is formed in the frame 20, in a section overlapping with the upper thruster 10 in plan view. A support 22, which is long in the width direction, is arranged below the front side of the frame 20.

[0021] For example, a plurality of lower thrusters 23L, 23R, 24L, and 24R are arranged in the lower body 4 to move the robot 1 in the longitudinal direction and the width direction. The plurality of lower thrusters 23L, 23R, 24L, and 24R include a total of four thrusters (corresponding to four horizontal thrusters), including a pair of front thrusters 23L and 23R on the right and left sides for moving the robot 1 forward or in the width direction, and a pair of rear thrusters 24L and 24R on the right and left sides for moving the robot 1 backward or in the width direction.

[0022] The front thrusters 23L and 23R are arranged, for example, in a front part of the frame 20. Each of the front thrusters 23L and 23R includes, for example, a propeller that rotates around an axis that is tilted outwardly from the front to the rear of the lower body 4 for arrangement in the width direction.

[0023] The rear thrusters 24L and 24R are arranged, for example, in a rear part of the frame 20. Each of the rear thrusters 24L and 24R includes, for example, a propeller that rotates around an axis that is tilted toward the front of the lower body 4 for placement in the width direction.

[0024] Thruster drive devices 25L and 25R for supplying a driving force (rotational force for each propeller) to the thrusters 10, 23L, 23R, 24L, and 24R are arranged in the lower body 4. A pair of thruster drive devices 25L and 25R are arranged on the left and right sides of the front of the frame 20.

[0025] For example, a camera 26 is arranged in the lower body 4. Lights 70L and 70R are arranged, for example, on the front side of the lower body 4. If one of the lights 70L and 70R is not identified, they are referred to as light 70. The light 70 is a device that can change the on and off state of the light, the illuminance of the light, the inclination of the light, and the like based on a light control signal.

[0026] A pair of manipulators 30L and 30R are arranged on the left and right sides of the lower body 4. The manipulators 30L and 30R include an arm 31 and a hand 32.

[0027] The arm 31 is constructed by combining a joint and a link. A base end of the arm 31 is connected to an outer end in the width direction of the beams 22. The base end of the arm 31 is connected to the frame 20 via the beam 22. For example, the arm 31 has six axes of rotation.

[0028] The hand 32 is located at a tip end of the arm 31 (an end opposite the base end of the arm 31). The hand 32 can grasp an object. As shown in the drawing, the hand 32 can include three fingers.

[0029] For example, a position sensor 35 for detecting a position of the robot 1 (e.g., a distance from the seabed to the robot 1) is arranged in the lower body 4. For example, the position sensor 35 is an ultrasonic sensor. In the example shown, the position sensor 35 is arranged between the pair of manipulators 30L and 30R on the left and right sides of the front of the frame 20. The position sensor 35 includes, for example, a Doppler velocity log (DVL) sensor for constant altitude navigation, an internal pressure sensor for constant depth navigation, and an inertial measurement unit (IMU).

[0030] For example, four power supply systems 36 and 37 are arranged in the lower body.

[0031] A weight installation area 38 in which a weight is installed may be arranged in the lower body 4. The weight installation area 38 is arranged, for example, behind the power supply system 37 at the rear of the frame 20.

[0032] The robot 1 includes the manipulators 30L and 30R and the power supply systems 36 and 37. The manipulators 30L and 30R are arranged on the front side (which is an example of one side in the longitudinal direction) of the lower body 4. The power supply systems 36 and 37 are arranged on the rear side (which is an example of the other side in the longitudinal direction) of the lower body 4. The manipulators 30L and 30R are arranged opposite the installation positions of the power supply systems 36 and 37 in the longitudinal direction of the lower body 4, with the opening 21 therebetween. <verbindungsglied>

[0033] The upper body 3 and the lower body 4 are connected by a plurality of links 5L, 5R, 6L, and 6R via connectors 7. The plurality of links 5L, 5R, 6L, and 6R are arranged in parallel. The plurality of links 5L, 5R, 6L, and 6R extend so as to be suspended between four corners on the upper side of the upper body 3 and four corners on the lower side of the lower body 4. The plurality of links 5L, 5R, 6L, and 6R include a total of four links, including a pair of front links 5L and 5R on the left and right sides and a pair of rear links 6L and 6R on the left and right sides. The upper body 3 and the lower body 4 are connected in parallel by the four links 5L, 5R, 6L, and 6R. If one of the links 5L and 5R is not identified, the links are referred to as link 5.If one of the links 6L and 6R is not identified, the links are referred to as link 6. <gelenk>

[0034] The connectors 7 contain 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 arranged at the upper and lower ends of the four connecting links 5L, 5R, 6L, and 6R.

[0035] The robot 1 includes an actuator 9 that can rotate the links 5L, 5R, 6L, and 6R in the pitch direction and roll direction at a joint 8A (which is an example of at least one joint) among the eight joints 8A and 8P. The actuator 9 is arranged in the joint 8A at the lower end of the left rear link 6L among the four links 5L, 5R, 6L, and 6R.

[0036] In the following description, the joint 8A provided with the actuator 9 is referred to as the "joint drive unit 8A," and the joints 8P (the joints 8P without the actuator 9) that operate with the movement of the joint drive unit 8A are also referred to as "passive joints 8P." The robot 1 includes one joint drive unit 8A and seven passive joints 8P. <gelenkantriebseinheit>

[0037] A pitch drive device 40 for rotating the links 5L, 5R, 6L and 6R in the pitch direction and a roll drive device 50 for rotating the links 5L, 5R, 6L and 6R in the roll direction are arranged as the actuator 9 in the joint drive unit 8A.

[0038] The pitch drive device 40 includes, for example, a pitch motor for rotating the links 5L, 5R, 6L, and 6R in the pitch direction, a driven pulley for reducing a rotational speed of the pitch motor to equal to or lower than a predetermined speed, a reduction gear for further reducing the rotation reduced by the driven pulley, and a housing for accommodating the pitch motor and the driven pulley. The pitch drive device 40 is controlled by a center of lift controller 47 (see Fig. 5). For example, for a structural example of the pitch drive device 40, reference is made to Japanese Patent Application No. JP 2023-029174 A.

[0039] The rolling drive device 50 includes, for example, a rolling motor for rotating the links 5L, 5R, 6L, and 6R in the rolling direction, a driven pulley for reducing a rotational speed of the rolling motor to equal to or lower than a predetermined speed, a reduction gear for further reducing the rotation reduced by the driven pulley, and a housing for accommodating the rolling motor and the driven pulley. The rolling drive device 50 is controlled by a center of buoyancy controller 47 (see Fig. 5). For example, a structural example of the rolling drive device 50 is referred to Japanese Patent Application No. JP 2023-029174 A. <Passives Gelenk>

[0040] The following describes the configuration arranged at the lower end of the left front link 5L among the seven passive joints 8P. The configurations of the passive joints 8P arranged in the other sections are the same as the configuration arranged at the lower end of the left front link 5L, and therefore a detailed description is omitted.

[0041] A mechanism, a so-called cardan mechanism, which can be tilted in any direction by combining two mutually orthogonal axes, is arranged in the passive joint 8b.

[0042] The gimbal mechanism includes a gimbal body, which is a body of the gimbal mechanism, a pitch shaft member for rotating the connecting links 5L, 5R, 6L, and 6R in the pitch direction, a roll shaft member for rotating the connecting links 5L, 5R, 6L, and 6R in the roll direction, a support member that supports the roll shaft member, and a plurality of sliding bearings. For example, for a structural example of the gimbal mechanism, refer to Japanese Patent Application No. JP 2023-029174 A. <Beispiel vom Nickbetrieb des Roboters>

[0043] Fig. 2 is a diagram showing a pitching operation of the robot according to the embodiment.

[0044] For example, when an output shaft of the pitch motor 41 in the active joint 8A rotates in one direction around an axis (around the axis in the width direction), the plurality of passive joints 8P rotate synchronously around the axis (around the axis in the width direction). Accordingly, the robot body 2 rotates in the pitch direction. In the example shown, the robot body 2, in the left side view, rotates in the counterclockwise direction (as an example of a direction of the pitch direction). <Beispiel vom Rollbetrieb des Roboters>

[0045] Fig. 3 is a diagram showing a rolling operation of the robot according to the embodiment.

[0046] For example, when one output shaft of the rolling motor 51 in the active joint 8A rotates in one direction around an axis (around an axis in the longitudinal direction), the multiple passive joints 8P rotate synchronously around the axis (around the axis in the longitudinal direction). Accordingly, the robot body 2 rotates in the rolling direction. In the example shown, the robot body 2 rotates clockwise when viewed from the front (as an example of a rolling direction). <Steuerbeispiel basierend auf der Bewegung der Schwimmervorrichtung während Armbeugung und -streckung>

[0047] Next, a control example based on the movement of the float device during flexion and extension of the arm 31 according to the present embodiment will be described. Fig. 4 is a diagram showing a control example based on the movement of a float device according to the present embodiment. In the present embodiment, a float device includes, for example, the buoyancy element and the links and joints connected to the buoyancy element. The float device may include a drive unit. The float device is connected to the robot body 2 and has a greater buoyancy force and a lower weight than the robot body 2 (has a lower density than the robot body 2). In the present embodiment, "low density" means a greater buoyancy force and a lower weight, that is, a lower density.

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

[0049] Reference symbol g20 denotes a state example immediately after the arm 31 is extended. In this case, as indicated by reference symbol g20, the COB moves according to the weight of the upper body 3, the weight of the arm 31, and the extension length of the arm 31. As indicated by reference symbol g20, the COG moves according to the weight of the lower body 4, the weight of the arm 31, and the extension length of the arm 31.

[0050] The reference symbol g30 denotes a state example when an error ΔX err between the center of buoyancy and the center of gravity of the vehicle body has not been controlled. In this case, as indicated by reference symbol g30, the position of the body is tilted such that the buoyancy force B and the gravity force M of the body coincide.

[0051] Reference symbol g40 denotes a state example when the error between the center of buoyancy and the center of gravity of the vehicle body has been controlled by a method according to the present embodiment. Reference symbol g41 denotes a state before the link mechanism operates. As indicated by reference symbol g40, in the present embodiment, the attitude of the vehicle body is controlled by a link mechanism using an angle (a COX angle) formed by the center of buoyancy and the center of gravity, and a vehicle body coordinate system as the operation amount.

[0052] Accordingly, even if the arm 31 is extended in the bent state, the posture of the vehicle body can be maintained without collapsing. Here, q ACOX1 a control angle of a joint section and is, for example, an angle in the pitch direction. <Konfigurationsbeispiel eines Mobiles-Objekt-Steuersystems>

[0053] An example of a configuration of a mobile object control system 400 according to the present embodiment will be described below. Fig. 5 is a diagram showing an example of the configuration of the mobile object control system according to the present embodiment. As shown in Fig. 5, the mobile object control system 400 includes, for example, a robot 1 and an operating element 200.

[0054] The operating element 200 includes, for example, a controller 201, an image display 202 and a communication device 203.

[0055] The robot 1 includes, for example, a robot body 2, an upper body 3, a lower body 4, a manipulator 30, an upper thruster 10, a lower thruster 23, a link 5, a link 6, a camera 26, a camera sensor 27, a camera driver 28, a position sensor 35, a lighting 70, an arm sensor 65, a manipulator driver 66, a position sensor 67 and a mobile object control device 100. In the Fig. In the configuration example shown in Figure 5, some of the components of the robot 1 described above with reference to Fig. 1 is not shown.

[0056] The mobile object control device 100 includes, for example, a thruster drive device 25, a buoyancy center controller 47, a detection device 90, a setpoint setting device 91, a controller 92, a memory 93, and a communication device 94. (control element)

[0057] The control element 200 is used, for example, by an operator of a vehicle. The control element 200 and the mobile object control device 100 are connected to each other, for example, by cabling.

[0058] The controller 201 is a device used by the operator to input an operation command for the robot body 2 or the arm 31. The controller 201 is, for example, a handle, a joystick, or a touch panel sensor.

[0059] The image display 202 acquires an image captured by the camera 26 or a state or the like of the robot body 2 or the arm 31 from the mobile object control device 100 and displays the acquired information.

[0060] The communication device 203 sends and receives information to and from the mobile object control device 100. (Robot)

[0061] For example, the robot body 2 is a different part than the arm 31 in Fig. 1. The elements mentioned above in relation to Fig. 1 are attached to the robot body 2.

[0062] The camera 26 is, for example, an image pickup device using a complementary metal oxide semiconductor (CMOS) image pickup element or an image pickup device using a charge-coupled device (CCD) image pickup element. The camera 26 may be an RGB (red, green, and blue) D camera capable of capturing depth information D.

[0063] The camera sensor 27 detects, for example, an inclination of a tilt shaft of the camera 26.

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

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

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

[0067] The attitude sensor 67 is a sensor that detects tilt angles in the pitch and roll directions of the robot body 2 and the float device, an attitude angle of the vehicle body, and the like. The attitude sensor 67 may be an acceleration sensor, a pressure sensor, or the like. In this case, the attitude of the vehicle body can be estimated by a known method (see, for example, Tomohiro Takahashi, Masatoshi Hatano, "Study of Posture Control of Underwater Movable Manipulator," 25th Traffic and Distribution Category Meeting (TRANSLOG2016), Japan Society of Mechanical Engineers, 2016) using values ​​detected by the acceleration sensor, the pressure sensor, and the like. (Mobile object control device)

[0068] The thruster drive device 25 generates a thruster control instruction and drives the upper thruster 10 and the lower thruster 23.

[0069] The center of lift controller 47 calculates the control angles q ACOX1 and q ACOX2 , to move the center of lift in the pitch and roll directions (or the x-axis and y-axis directions). The control angle q ACOX2 is, for example, an angle in the roll direction. The center of lift controller 47 controls the links 5 and 6 such that the upper body 3 operates in at least one of the pitch direction and the roll direction, using the calculated control angles q ACOX1 and q ACOX2 . The control method in the center of lift controller 47 will be described later.

[0070] The detection device 90 detects a combined position of the center of gravity of the body and the float device, a combined position of the center of gravity of the body and the float device, an angle θ formed by the position of the buoyancy center and the position of the center of gravity, and an attitude angle ϕ of the body from the attitude sensor 67, and the like. The detection device 90 can detect the position of the buoyancy center and the position of the center of gravity by calculation based on the detected values. Alternatively, the buoyancy center controller 47 can detect the position of the buoyancy center and the position of the center of gravity by calculation based on the values ​​detected by the detection device 90.

[0071] The setpoint setting device 91 sets a setpoint of the angle formed by the center of buoyancy and the center of weight from the setpoint position.

[0072] For example, the controller 92 controls operations of the robot body 2, the arm 31, and the camera 26.

[0073] The memory 93 stores programs, thresholds, predetermined values, mathematical expressions, and the like required for controlling the mobile object control device 100. The memory 93 stores, for example, a three-dimensional model of the robot body 2 or the arm 31.

[0074] The communication device 94 sends and receives information to and from the control element 200. <Prozess zum Steuern der Schwimmervorrichtung>

[0075] A process for controlling the float device is described below.

[0076] Fig. Figure 6 is a block diagram showing the process for controlling the float device according to the present embodiment. The processes of the components in Fig. 6 are carried out by the center of buoyancy controller 47. In Fig. 6, A* is a setpoint of A, ϕ denotes an attitude angle of the vehicle body, θ denotes COX an angle formed by the center of buoyancy and the center of weight is called q ACOX a drive unit state of the float device, denoted q all a state of the entire propulsion unit of the vehicle body including the float device, Δx denotes a positional deviation between the center of buoyancy and the center of weight, and denotes I COX a distance between the center of buoyancy and the center of weight.

[0077] A first converter 471 converts an input target value ϕ* of the attitude angle ϕ of the vehicle body into a target COX angle θ* COX and gives the target COX angle θ* COX to a second computer 477.

[0078] A first calculator 472 subtracts the attitude angle ϕ of the vehicle body from the target value ϕ* of the attitude angle ϕ of the vehicle body and outputs the subtraction result to an integrator 473 and a differentiator 475.

[0079] The integrator 473 integrates the position angle deviation (ϕ* - ϕ) from the desired position angle, which is the calculation result from the first calculator 472.

[0080] A first coefficient multiplier 474 multiplies the integration result from the integrator 473 by a coefficient K i and outputs the multiplication result to the second calculator 477. The processes of the integrator 473 and the first coefficient multiplier 474 are deviation correction.

[0081] The differentiator 475 differentiates the position angle deviation (ϕ* - ϕ) from the desired position angle, which is the calculation result of the first calculator 472.

[0082] A second coefficient multiplier 476 multiplies the differentiation result from the differentiator 475 by a coefficient K d and outputs the multiplication result to the second calculator 477. The processes of the differentiator 475 and the second coefficient multiplier 476 are damping.

[0083] The coefficients K i and K d are amplification factors and are determined in advance, for example by simulation.

[0084] The second calculator 477 adds the output of the first coefficient multiplier 474 and the output of the second coefficient multiplier 476 to the target COX angle θ* COX , which is the output of the first converter 471, and outputs the addition result to a second converter 478.

[0085] The second converter 478 converts the addition result into a target deviation Δx* between the position of the center of buoyancy and the position of the center of gravity by using the output of the second calculator 477 and the distance I COX between the center of buoyancy and the center of weight, which is an output of a center of weight and center of buoyancy calculator 482, and outputs the converted position deviation Δx* between the center of buoyancy and the center of weight to a third calculator 479.

[0086] The third calculator 479 subtracts the deviation Δ between the position of the center of buoyancy and the position of the center of gravity, which is the output of the center of gravity and the center of buoyancy calculator 482, from the target deviation Δx* between the position of the center of buoyancy and the position of the center of gravity, which is the output of the second converter 478, and outputs the subtraction result to a third converter 480.

[0087] The third converter 480 converts the output of the third computer 479 into a control value q ACOX for the float device drive unit and outputs the converted control value q ACOX for the float device drive unit to a system 481. Here q ACOX expressed by expression (1). [Expression 1] qACOX=(qACOX1qACOX2)

[0088] The system 481 contains all the driven units of the vehicle body, including the floatation device, and is an image of the position and attitude of the vehicle body in an underwater environment. Sensors for detecting the states of the driven units and the attitude sensor 67 for detecting an attitude are mounted on the floatation device, the driven units, or near the driven units. The system 481 provides q all to the center of gravity and center of lift calculator 482 and outputs the attitude angle ϕ of the vehicle body to the first calculator 472. q all is expressed by expression (2). q ACOX1 , q ACOX2 and q others can be angles or lengths. [Expression 2] qall=(qACOX1QACOX2 qothers)

[0089] The center of gravity and center of buoyancy calculator 482 calculates the center of gravity and the center of buoyancy using the value q output by the system 481. all . The center of gravity and center of buoyancy calculator 482 calculates the deviation Δx between the position of the center of buoyancy and the position of the center of gravity as well as the distance I COX between the center of buoyancy and the center of weight using the calculation result. (Process of the first converter)

[0090] Below, the process of the first converter 471 is described in detail. Fig. 7 is a diagram showing a conversion process to a target COX angle according to the present embodiment.

[0091] The buoyancy center of gravity controller 47 controls the attitude of the vehicle body of the robot 1 using the angle formed by the buoyancy center of gravity and the weight center of gravity as the operation amount.

[0092] In Fig. 7 is θ COX (COX angle) an angle formed by a line connecting the center of buoyancy and the center of gravity and a z-axis of the vehicle body coordinate system of the robot 1. The vehicle body coordinate system defines the vertical direction of the vehicle body as the z-direction.

[0093] If a line segment L act is vertical in an inertial coordinate system, θ COX equal to the angle ϕ formed by the horizontal direction in the inertial coordinate system and the vehicle body, as expressed by expression (3). Accordingly, the target value ϕ* of the attitude angle ϕ of the vehicle body can be converted into the target COX angle θ* COX be converted as expressed by expression (4). The center of buoyancy controller 47 detects the attitude angle ϕ of the vehicle body, for example, based on the detected value of the attitude sensor 67 via the detection device 90. [Expression 3] ϕ=θCOX [Expression 4] ϕ*=θCOX*

[0094] When no external force acts here, the center of buoyancy and the center of gravity are arranged in a vertical line, and therefore, the angle formed by the center of buoyancy and the center of gravity is equal to the attitude angle. Accordingly, according to the present embodiment, even when the center of gravity moves within the vehicle body (such as when arm 31 is operating), the vehicle body can be maintained at a desired attitude by maintaining the angle formed by the center of buoyancy and the center of gravity at the same angle as the desired attitude. (Integral control and differential control)

[0095] The integral control using the integrator 473 and the differential control using the differentiator 475 are described below. Fig. Figure 8 is a diagram showing an example of factors that cause an error. When the robot moves underwater or performs an operation, the target position may not be maintained due to the transport of a heavy object, rapid movement of the center of gravity, the thrust of a thruster, or other environmental factors, as shown in Fig. 8 shown.

[0096] Therefore, as a countermeasure in the present embodiment, an actual attitude angle of the vehicle body is fed back and reflected in a float device drive control value. By using differential control, damping control is performed such that the attitude angular velocity ϕ' is 0. By using integral control, control is performed such that the attitude angle deviation (ϕ* - ϕ) from the target attitude angle is 0. (Conversion into target deviation between the position of the center of buoyancy and the position of the center of gravity)

[0097] A conversion process into a target deviation between the position of the center of buoyancy and the position of the center of gravity, which is carried out by the second converter 478, will be described in detail below. Fig. Figure 9 is a diagram showing a conversion process to a desired deviation between the position of the center of lift and the position of the center of weight in the pitch direction.

[0098] In Fig. 9 is I COX-xz a distance between the center of buoyancy and the center of gravity in the xz-plane. Δx* is a nominal value of the deviation between the center of buoyancy and the center of gravity. q* COX is a target COX angle.

[0099] From a triangular function relationship, the deviation Δx* between the center of buoyancy and the center of gravity can be expressed using expression (5). The y-axis direction is a depth direction toward the ground surface. In this way, the second converter 478 converts the desired COX angle θ* COX into the desired value Δx* of the deviation between the center of lift and the center of weight using equation (5). The second transducer 478 detects the distance I COX-xz between the center of buoyancy and the center of gravity in the xz-plane from the center of gravity and center of buoyancy calculator 482. [Expression 5] Δx*=[lCOX−XY sin θCOX*lCOX−YZ sin θCOX*]

[0100] The above with regard to Fig. The example described in Figure 9 is associated with the pitch direction, and the same calculation and conversion can be performed similarly in the roll direction. (Conversion to float device drive control value)

[0101] Next, a conversion process into a float device drive control value performed by the third converter 480 will be described in detail. Fig. 10 is a diagram showing conversion to a float device drive control value. Reference symbol g100 indicates a state example before the arm is extended. Reference symbol g110 denotes a state example after the arm is extended, the position of the buoyancy center of gravity and the position of the weight center of gravity move, and the float device moves.

[0102] The third converter 480 acquires a subtraction result of 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 calculator 479. Here, the deviation Δx between the center of buoyancy and the center of gravity is expressed by expression (6). The position x COB of the center of lift is expressed by expression (7), and the position x COG of the center of gravity is expressed by expression (8). In expression (6), Δx err a position error in the x-axis direction between the center of buoyancy and the center of weight, and is Δy err a position error in the y-axis direction between the center of buoyancy and the center of gravity. X G is the position of the center of gravity in the x-axis direction, X B is the position of the center of lift in the x-axis direction, Y G is the position of the center of gravity in the y-axis direction and Y B is the position of the center of lift in the y-axis direction. [Expression 6] Δx=[ΔXerrΔYerr]=[XG−XB YG−YB] [Expression 7] xCOB=[XBYB] [Expression 8] xCOG=[XGYG]

[0103] In the present embodiment, if the deviation between the center of buoyancy and the center of weight in a coordinate system of a remotely operated work vehicle (ROV) is expressed in a control value q ACOX1 for the float device driver is to be set to a predetermined set value Δx*, the set value of the deviation is expressed by expression (9). [Expression 9] [(XG+ΔXG)−(XB+ΔXB)(YG+ΔYG)−(YB+ΔYB)]=[ΔX*ΔY*]

[0104] Accordingly, expression (9) can be expressed as expression (10), and the deviation is expressed by expression (11). [Expression 10] [XG−XBYG−YB]−[XG−XBYG−YB]=[ΔXB−ΔXGΔYG−ΔYG] [Expression 11] [ΔXerrΔYerr]−[ΔX*ΔY*]=[JCOB−JCOG][qACOX1qACOX2]

[0105] A center of buoyancy sensitivity matrix J COB , which is a Jacobian matrix, is expressed by expression (12), and a center-of-weight sensitivity matrix J COG , which is a Jacobian matrix, is expressed by expression (13). [Expression 12] JCOB=[∂XB∂qACOX1∂XB∂qACOX2∂YB∂qACOX1∂YB∂qACOX2] [Expression 13] JCOB=[∂XB∂qACOX1∂XB∂qACOX2∂YB∂qACOX1∂YB∂qACOX2]

[0106] Here, if a different matrix between the center of lift sensitivity and the center of weight sensitivity is regular, the control value q can be calculated as an inverse matrix expressed by Expression (14). [Expression 14] [qACOX1qACOX2]=[JCOB−JCOG]−1(Δx−Δx*)

[0107] The center of lift controller 47 controls the attitude of the robot 1, for example by proportional-integral-derivative (PID) control using the calculated control value q. <Beispiel vom Prozessablauf>

[0108] An example of a process flow executed by the mobile object control device 100 will be described below. Fig. 11 is a flowchart showing a control procedure executed by the mobile object control device 100 according to the present embodiment.

[0109] (Step S1) The detection device 90 detects a value detected by a sensor such as the position sensor 67.

[0110] (Step S2) The center of gravity controller 47 detects the attitude angle of the vehicle body using the detected value detected by the detecting device 90.

[0111] (Step S3) The target value setting device 91 sets a target value of an angle formed by the center of buoyancy and the center of gravity from the target attitude. The target attitude may be preset or may be set by an operator operating the control element 200. The center of buoyancy controller 47 detects a target value of the attitude angle of the vehicle body set by the target value setting device 91.

[0112] (Step S4) The buoyancy center of gravity controller 47 detects the position of the buoyancy center of gravity and the position of the weight center of gravity by calculation based on the value detected by the detecting device 90.

[0113] (Step S5) The first converter 471 of the center of gravity controller 47 converts an input target value ϕ* of the attitude angle ϕ of the vehicle body into a target COX angle θ* COX around.

[0114] (Step S6) The integrator 473 and the first coefficient multiplier 474 of the center of lift controller 47 perform feedback control through an integration process, which is a deviation correction process. The differentiator 475 and the second coefficient multiplier 476 of the center of lift controller 47 perform feedback control through a differentiation process, which is a damping process.

[0115] (Step S7) The second converter 478 of the center of lift controller 47 converts the distance I COX between the center of buoyancy and the center of weight, which is the output of the second calculator 477 and the output of the center of weight and center of buoyancy calculator 482, into the target deviation Δx* between the position of the center of buoyancy and the position of the center of weight.

[0116] (Step S8) The third converter 480 of the center of lift controller 47 converts the output of the third calculator 479 into the control value q ACOX for the float driver.

[0117] (Step S9) The center of gravity controller 47 controls the attitude of the robot 1, for example, by PID control using the calculated control value q.

[0118] (Step S10) The buoyancy center controller 47 determines whether to end the control. The buoyancy center controller 47 can determine whether to start or end the control based on whether the arm is extended or bent, based on the detected value of the arm sensor 65, or based on the result of an operator's operation on the operating member 200. If it is determined that the control should be ended (step S10: YES), the buoyancy center controller 47 ends the process flow. If it is determined that the control should not be ended (step S10: NO), the buoyancy center controller 47 returns the process flow to step S1. <verifikationsergebnis>

[0119] An example of a verification example is described below.

[0120] Fig. 12 shows an example of a verification result when the control is performed conventionally. A graph with reference symbol g200 denotes an example of a change in a shoulder joint angle of the arm. In the graph with reference symbol g200, the horizontal axis represents time (seconds), and the vertical axis represents the shoulder joint angle (degrees) of the arm. A graph with reference symbol g210 denotes an example of a change in an angle in the pitch direction. In the graph with reference symbol g210, the horizontal axis represents time (seconds), and the vertical axis represents the angle (degrees) in the pitch direction.

[0121] As indicated by the graph with reference symbol g210, in the conventional control, a maximum error of the attitude angle is about 15 (degrees).

[0122] Fig. 13 shows an example of a verification result when a control method according to the present embodiment is performed. A graph with reference symbol g220 denotes an example of the change in a shoulder joint angle of the arm. In the graph with reference symbol g220, the horizontal axis represents time (seconds), and the vertical axis represents the shoulder joint angle (degrees) of the arm. A graph with reference symbol g230 denotes an example of the change in an angle in the pitch direction. In the graph g230, the horizontal axis represents time (seconds), and the vertical axis represents the angle (degrees) in the pitch direction.

[0123] As indicated by the graph with reference symbol g230, in the control method according to the present embodiment, a maximum error of the attitude angle is about 4 (degrees).

[0124] In this way, with the control method according to the present embodiment, it is possible to correct a tilting of the vehicle body due to a change in the position of the arm of the robot 1. (Damping control)

[0125] Now, an example of a detection result of vertical vibration of an operation area which is a fingertip position of an end effector when the damping control based on differential control according to the present embodiment has been executed and when the damping control has not been executed will be described.

[0126] Fig. 14 is a diagram showing an example of a detection result of vertical vibration of an operating area, which is a fingertip position of an end effector, when damping control according to the present embodiment was performed and when damping control was not performed. A graph with reference symbol g240 denotes an example of the height change of the operating area. In the graph with reference symbol g240, the horizontal axis represents time (seconds), and the vertical axis represents the height (mm) of the operating area. The curves with reference symbol g241 denote a height change of the operating area when damping control was not performed. The curves with reference symbol g242 denote a height change of the operating area when damping control was performed.

[0127] A graph labeled g250 indicates an example of arm length change in a state where the arm has been extended from a bent state. In the graph labeled g250, the horizontal axis represents time (seconds), and the vertical axis represents arm length.

[0128] As indicated by the graph with reference symbol g240, the height of the operating area during lifting changes from 33 (mm) when damping control is not performed to 19 (mm) when damping control is performed, thus preventing vibration. The vibration damping ratio changes from 0.1 when damping control is not performed to 0.74 when damping control is performed, thus preventing vibration. (Tilt deviation offset correction control)

[0129] Now, an example of a detection result of the vertical vibration of an operation area which is a fingertip position of an end effector when tilt deviation displacement correction control according to the present embodiment has been executed will be described.

[0130] Fig. Fig. 15 is a diagram showing an example of the change in an operating range, which is a fingertip position of an end effector, when deviation offset correction damping control according to the present embodiment has been performed. The horizontal axis represents time (seconds), and the vertical axis represents a height (mm) of the operating range. Fig. In the verification shown in Figure 15, integral control is performed when a weight of 2.5 (kg) is held by a fingertip, thus causing the vehicle body to tilt.

[0131] If a deviation of the operating range with the center of lift as the center of rotation is limited to less than a predetermined value (setpoint), as in Fig. 15, by integral control, a tilt displacement of 190 (mm) can be corrected to about 18 (seconds), which is smaller than the predetermined value.

[0132] As described above, according to the present embodiment, for example, when the arm is moving and thus an error occurs between the buoyancy center of gravity and the weight center of gravity, control is performed based on the control angle value such that the positional deviation between the buoyancy center of gravity and the weight center of gravity is 0 (or less than a predetermined value). In the present embodiment, the deviation correction process is performed by integral control. That is, in the present embodiment, I control is added to the PID control. In the present embodiment, a damping process is performed by differential control. For example, when the robot body 2 starts moving and then stops, vibration occurs, and the vibration appears in a camera image of an operating member. Accordingly, D control is added to the PID control.

[0133] Therefore, according to the present embodiment, when the posture of the arm changes, a control angle value for correcting the tilt of the vehicle body of the robot 1 can be calculated, and this tilt can be corrected using this correction control value. That is, according to the present embodiment, it is not necessary to construct a suitable expression for each posture of the driven unit, such as the arm, and it becomes possible to cope with various types of arm movement. According to the present embodiment, it is possible to move the float device according to the movement of the arm and keep the vehicle body horizontal.

[0134] According to the present embodiment, it is possible to perform tilt deviation offset correction (deviation correction) through integral control. That is, by adding I control to PID control, it is possible to perform control such that the vehicle body returns to a horizontal state even when the arm is holding a heavy object.

[0135] According to the present embodiment, it is possible to prevent (damp) an increase in vibration due to a change in the arm's position through differential control. That is, since differential control is added to PID control, it is possible to perform control such that the vibration is instantly damped. [Second version]

[0136] In the first embodiment, by driving the linkages 5 and 6 to tilt the float device, control is performed such that the balance of the vehicle body is maintained, for example, when the arm is extended from a bent state. In a second embodiment, the buoyancy center of gravity or the center of gravity of the robot as a whole is controlled by moving the buoyancy element horizontally in a two-dimensional direction without changing its height. <roboter>

[0137] Next, an outer shape or the like of a robot 1A according to the present embodiment will be described. Fig. 16 is a perspective view of the robot according to the present embodiment.

[0138] As in Fig. 16, the robot 1A includes a robot body 2, which is a body of the robot 1A. The robot body 2 includes an upper body 3A (a movable portion) arranged at an upper part of the robot body 2, and a lower body 4 (a stationary portion) arranged at a lower part of the robot body 2. In Fig. 16 only basic functional units of the robot 1A are designated by reference numerals.

[0139] The upper body 3A includes a cover 301, an upper buoyancy element 302, a frame 303 and a frame 304. The configuration of the upper body 3A is described with respect to Fig. 17 described in detail.

[0140] Fig. Fig. 17 is a plan view showing an example of the configuration of the upper body (the movable portion) according to the present embodiment. Fig. 17, the cover 301 is omitted.

[0141] As in Fig. For example, as shown in Fig. 17, the upper body 3A includes the upper buoyancy member 302, the frame 303, the frame 304, a carriage portion 305, a carriage portion 306, an actuator 307 (307L and 307R), an active pulley 308 (308L and 308R), a passive pulley 309 (309L and 309R), and a belt 310 (310L and 310R).

[0142] For example, frame 304 is attached to frame 303.

[0143] Belt 310, for example, is a flat crawler belt.

[0144] The actuator 307L rotates the belt 310L by driving the active pulley 308L.

[0145] The passive pulley 309L rotates with the rotation of the belt 310L.

[0146] One end of the carriage portion 305 is attached to the upper buoyancy body 302, and the carriage portion 305 moves in the x-axis direction with the rotation of the belt 310L.

[0147] The actuator 307R rotates the belt 310R by driving the active pulley 308R.

[0148] The passive pulley 309R rotates with the rotation of the belt 310R.

[0149] One end of the carriage portion 306 is attached to the upper buoyancy body 302, and the carriage portion 306 moves in the x-axis direction with the rotation of the belt 310R.

[0150] In the present embodiment, the actuator 307 is controlled in such a way that the upper lift member 302 attached to the carriage sections 305 and 306 is moved in the x-axis direction and the y-axis direction with the rotation of the belt 310. The actuator 307 includes an actuator for movement in the x-axis direction and an actuator for movement in the y-axis direction. Accordingly, according to the present embodiment, a structure is provided in which, by controlling the two actuators, the two actuators cooperate to move in the x-axis direction and the y-axis direction. <Konfigurationsbeispiel vom Mobiles-Objekt-Steuersystem>

[0151] Next, a configuration example of a mobile object control system 400A according to the present embodiment will be described. Fig. 18 is a diagram showing a configuration example of the mobile object control system according to the present embodiment. As shown in Fig. 18, the mobile object control system 400A includes, for example, a robot 1A and an operator 200.

[0152] The operating element 200 includes, for example, a controller 201, an image display 202 and a communication device 203.

[0153] The robot 1A includes, for example, a robot body 2, an upper body 3A, a lower body 4, a manipulator 30, an upper thruster 10, a lower thruster 23, a camera 26, a camera sensor 27, a camera driver 28, a position sensor 35, a lighting 70, an arm sensor 65, a manipulator driver 66, an attitude sensor 67, an actuator 307, an upper buoyancy element 302, and a mobile object control device 100A. Fig. 18 shown configuration example are some of the above in relation to the Fig. 16 and Fig. 17 shown components of the robot 1A are not shown.

[0154] The mobile object control device 100A includes, for example, a thruster drive device 25, a buoyancy center controller 47A, a detection device 90, a setpoint setting device 91, a controller 92, a memory 93, and a communication device 94.

[0155] The center of buoyancy controller 47A detects the position of the center of buoyancy and the position of the center of gravity and calculates a steering angle q ACOX1 to eliminate a deviation between the center of lift and the center of weight in the pitch direction (for example, the x-axis direction) based on the detection result. The center of lift controller 47A controls the actuator 307 based on the calculated control angle q ACOX1 such that the upper body 3A moves parallel without changing a height in the pitch direction. (Conversion to float device drive control value)

[0156] The following describes in detail a conversion process into a float device drive control value performed by the third converter 480. The blocks for the float device control process are the same as those in Fig. 6 according to the first version.

[0157] Fig. 19 is a diagram showing the conversion to a float device drive control value. Reference symbol g200 denotes a state example before the arm is extended. Reference symbol g210 denotes a state example after the arm is extended, in which the position of the buoyancy center of gravity and the position of the weight center of gravity move, and the float device moves.

[0158] The third converter 480 detects the subtraction result of 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 calculator 479. Here, the deviation Δx* between the center of buoyancy and the center of gravity is expressed by expression (6). The position x COB of the center of lift is expressed by expression (7) and the position x COG The center of gravity is expressed by expression (8). The center of gravity controller 47A controls the attitude of the robot 1A, for example, by PID control using the calculated control value q.

[0159] In the present embodiment, the upper lift member 302 is moved in parallel in the pitch direction without changing the altitude by performing such a control that the error between the center of lift and the center of weight takes the target value Δx* and the deviation Δx between the center of lift and the center of weight on the same vertical line takes the target value Δx*.

[0160] In the example given above regarding the Fig. 16 and Fig. 17, a configuration and example are shown in which the upper lift member 302 is moved in the pitch direction, but the upper lift member 302 may also be moved in the roll direction. In this case, the upper body 3A may include an actuator, a carriage section, an active pulley, a passive pulley, and a belt for the roll direction. The center of gravity controller 47A may control the roll direction actuator such that the upper lift member 302 moves parallel in the roll direction without changing its height.

[0161] The process flow executed by the mobile object control device 100A is the same as the process flow executed by the mobile object control device 100 according to the first embodiment.

[0162] A verification result according to the present embodiment is the same as the verification result according to the first embodiment.

[0163] As described above, according to the present embodiment, the lift member is moved in the pitch or roll direction without changing the altitude by performing such control that the error between the lift center of gravity and the weight center of gravity becomes the target value and the deviation between the lift center of gravity and the weight center of gravity on the same vertical line becomes the target value.

[0164] Therefore, according to the present embodiment, when the posture of the arm changes, it is possible to calculate the control angle value to correct the tilting of the vehicle body of the robot 1A and correct the tilting by means of this correction control value.

[0165] According to the present embodiment, it is possible to perform tilt deviation offset correction control (deviation correction) by integral control.

[0166] According to the present embodiment, it is possible to avoid (damp) an increase in vibration due to a change in the posture of the arm by differential control.

[0167] In the above-mentioned embodiments, a robot 1 (or 1A) for performing underwater surgery is described as an example of a mobile object, but a working environment is not limited to this. The working environment only needs to be one in which an error occurs between the center of buoyancy and the center of gravity.

[0168] All or some of the processes performed by the mobile object control device 100 (or 100A) according to the above-mentioned embodiments can be realized by recording a program on a computer-readable recording medium to realize all or some of the functions of the mobile object control device 100 (or 100A) according to the present invention, and causing a computer system to read and execute the program recorded on the recording medium. The "computer system" mentioned here may include an operating system (OS) or hardware such as peripherals. The "computer system" mentioned here may include a WWW system including a homepage provision (or display) environment.The "computer-readable recording medium" may include a portable medium such as a flexible disk, a magneto-optical disk, a ROM or a CD-ROM, or a memory such as a hard disk built into a computer system. The "computer-readable recording medium" may include a medium that holds a program for a predetermined time, such as volatile memory (RAM) in a computer system serving as a server or client when the program is transmitted over a network such as the Internet or a communication line such as a telephone line.

[0169] Some or all of these components may be implemented by hardware (a circuit unit (containing a circuit) such as a large-scale integration (LSI) circuit, an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), a graphics processing unit (GPU), or a system on a chip (SOC)) or may be implemented by software and hardware cooperatively.

[0170] The program can be transmitted from one computer system, in which the program is stored in a storage device or the like, to another computer system via a transmission medium or carrier waves in the transmission medium. Here, the "transmission medium" for transmitting a program is a medium having an information transmission function, such as a network (a communication network) such as the Internet or a communication circuit (communication line) such as a telephone line. The program may be a program for implementing some of the aforementioned functions. The program may be a so-called differential file (differential program) that can implement the aforementioned functions in combination with another program pre-stored in the computer system.

[0171] While modes for implementing the present invention have been described above in conjunction with embodiments, the present invention is not limited to such embodiments, and various modifications and substitutions may be applied without departing from the spirit of the inventions.

[0172] A mobile object control system includes a robot body, a float device having a lower density than a body connected to the robot body and including a drive unit capable of changing a buoyancy center of the float device with respect to the robot body, a sensing device, a setpoint setting device, and a buoyancy center controller.The buoyancy center of gravity controller is configured to calculate a control value for the drive unit for changing the buoyancy center of gravity of the float device using a deviation between a position of the buoyancy center of gravity and a position of the center of gravity, a target value of the deviation, a center of gravity sensitivity matrix indicating the change in the position of the center of gravity with respect to the control value, and a buoyancy center of gravity sensitivity matrix indicating a change in the position of the buoyancy center of gravity with respect to the control value.

[0173] A mobile object control system includes a robot body, a float device having a lower density than a body connected to the robot body and including a drive unit capable of changing a buoyancy center of the float device with respect to the robot body, a sensing device, a setpoint setting device, and a buoyancy center controller.The buoyancy center of gravity controller is configured to calculate a control value for the drive unit for changing the buoyancy center of gravity of the float device using a deviation between a position of the buoyancy center of gravity and a position of the center of gravity, a target value of the deviation, a center of gravity sensitivity matrix indicating the change in the position of the center of gravity with respect to the control value, and a buoyancy center of gravity sensitivity matrix indicating a change in the position of the buoyancy center of gravity with respect to the control value. QUOTES CONTAINED IN THE DESCRIPTION

[0000] This list of documents submitted by the applicant was generated automatically and is included solely for the convenience of the reader. This list is not part of the German patent or utility model application. The DPMA assumes no liability for any errors or omissions. Cited patent literature

[0000] JP 6 167 317 B

[0002] JP 2023

[0038] JP 029174 A

[0038] JP 2023-029 174 A [0039, 0042]< / roboter> < / verifikationsergebnis> < / gelenkantriebseinheit> < / gelenk> < / verbindungsglied> < / roboter>

Claims

[1] A mobile object control system that controls a robot performing an operation underwater, the mobile object control system comprising: a robot body; a float device that has a lower density than a body, which is connected to the robot body and contains a drive unit, which is capable of changing the buoyancy center of the float device with respect to the robot body; a detection device configured to detect a position of the buoyancy center of the robot body and the float device in combination, a position of the weight center of the robot body and the float device in combination, an angle formed by the position of the buoyancy center of gravity and the position of the weight center of gravity, and an attitude angle of the robot body; a setpoint setting device configured to set a setpoint of an angle formed by the position of the center of gravity from a setpoint position of the entire robot and / or a setpoint of an angle formed by the position of the center of buoyancy from the setpoint position of the entire robot; and a buoyancy center controller configured to provide a control value for the drive unit for a change in the buoyancy center of the float device by means of a deviation between the position of the buoyancy center and the position of the center of gravity, a target value of the deviation between the position of the buoyancy center and the position of the center of gravity, a center of gravity sensitivity matrix, which indicates a change in position of the center of gravity with respect to the control value for the drive unit, and a buoyancy center of gravity sensitivity matrix which indicates a change in position of the buoyancy center of gravity with respect to the control value for the drive unit. [2] The mobile object control system according to claim 1, wherein the buoyancy center controller is configured to perform feedback control in which a differential value of the attitude angle of the robot body is substituted as a speed term into the command value of the angle formed by the position of the buoyancy center and the position of the weight center. [3] The mobile object control system according to claim 1 or 2, wherein the buoyancy center of gravity controller is configured to perform feedback control in which an integral term of a deviation of the attitude angle of the robot body is substituted into the command value of the angle formed by the position of the buoyancy center of gravity and the position of the weight center of gravity. [4] The mobile object control system according to claim 1 or 2, wherein the robot includes a movable portion and a stationary portion, wherein the movable portion is connected to the stationary portion by a connecting member which is movable in a pitch direction and / or a roll direction, and wherein the center of lift controller is configured to control the position of the center of lift or the position of the center of weight of the robot body by moving the link in the pitch direction and / or the roll direction based on the calculated control value for the drive unit. [5] The mobile object control system according to claim 1 or 2, wherein the robot includes a movable portion and a stationary portion, wherein the movable portion includes an element for controlling the position of the center of gravity of the entire robot by two-dimensional movement, and wherein the center of gravity controller is configured to control the position of the center of gravity of the robot body by moving the movable portion in the pitch direction and / or the roll direction based on the calculated control value for the drive unit. [6] The mobile object control system of claim 1, wherein the center of buoyancy controller is configured to: Converting the position angle setpoint into a target angle; Converting the converted target angle into a target deviation between the position of the center of buoyancy and the position of the center of gravity by means of a distance between the position of the center of buoyancy and the position of the center of gravity; and Converting the converted target deviation between the position of the center of buoyancy and the position of the center of gravity and the deviation between the position of the center of buoyancy and the position of the center of gravity into a drive control value for the float device. [7] A mobile object control device that controls a robot including a float device having a lower density than a body connected to the robot body, performing an operation underwater and including a drive unit capable of changing the buoyancy center of the float device with respect to the robot body, the mobile object control device comprising: a detection device configured to detect a position of the buoyancy center of the robot body and the float device in combination, a position of the weight center of the robot body and the float device in combination, an angle formed by the position of the buoyancy center of gravity and the position of the weight center of gravity, and an attitude angle of the robot body; a setpoint setting device configured to set a setpoint of an angle formed by the position of the center of gravity from a setpoint position of the entire robot and / or a setpoint of an angle formed by the position of the center of buoyancy from the setpoint position of the entire robot; and a buoyancy center controller configured to provide a control value for the drive unit for a change in the buoyancy center of the float device by means of a deviation between the position of the buoyancy center and the position of the center of gravity, a target value of the deviation between the position of the buoyancy center and the position of the center of gravity, a center of gravity sensitivity matrix, which indicates a change in position of the center of gravity with respect to the control value for the drive unit, and a buoyancy center of gravity sensitivity matrix which indicates a change in position of the buoyancy center of gravity with respect to the control value for the drive unit. [8] A mobile object control method for controlling a robot including a float device having a lower density than a body connected to the robot body performing an underwater operation and including a drive unit capable of changing the buoyancy center of the float device with respect to the robot body, the mobile object control method comprising: Detecting a position of the buoyancy center of the robot body and the float device in combination, a position of the weight center of the robot body and the float device in combination, an angle formed by the position of the buoyancy center of gravity and the position of the weight center of gravity, and an attitude angle of the robot body; Setting a target value of an angle formed by the position of the center of gravity from a target position of the entire robot and / or a target value of an angle formed by the position of the center of buoyancy from the target position of the entire robot; and Calculating a control value for the drive unit for a change in the center of buoyancy of the float device by means of a deviation between the position of the center of buoyancy and the position of the center of weight, a target value of the deviation between the position of the center of buoyancy and the position of the center of weight, a center of weight sensitivity matrix indicating a change in the position of the center of weight with respect to the control value for the drive unit, and a center of buoyancy sensitivity matrix indicating a change in the position of the center of buoyancy with respect to the control value for the drive unit.

Images