JP4488811B2 - Robot control method and robot system - Google Patents

Description

  The present invention relates to a robot control method capable of controlling a robot with a high degree of freedom such as a humanoid in real time.

  Recently, humanoid robots in the field of research and education or entertainment are in the spotlight. However, these humanoid robots have a low degree of freedom and do not simply take over the work performed by humans. Practical use of humanoid robots that can take over human work in the same environment as human work is expected.

  FIG. 24 is a diagram showing a schematic configuration of a humanoid robot. The robot 1 is a humanoid robot, and includes a head 2, arms 3a and 3b, a torso 4, legs 5a and 5b, and hands 6a and 6b. The head 2 is a part corresponding to a human head, and includes one link and one connecting portion. The arm portions 3a and 3b are portions corresponding to a human arm, each including two links and three connecting portions, and the body portion 4 is a portion corresponding to a human torso and includes two links and one link. Consists of connecting parts. The leg portions 5a and 5b are portions corresponding to human legs, each including three links and three connecting portions, and the hand portions 6a and 6b are portions corresponding to human hands and include one link portion. And one connecting portion. A connection part is a site | part comprised from 1 or several joints, and connects a link and a link. This joint has one degree of freedom.

  If the humanoid robot further includes a degree of freedom such as a finger, the degree of freedom may exceed 30 degrees of freedom, and a robot with 20 degrees of freedom or more may be referred to as a super multi-degree-of-freedom robot. Such a super multi-degree-of-freedom robot has a degree of freedom in many redundant axes as compared with the degree of freedom necessary for performing actual work. In order to operate the robot, it is necessary to instruct the robot position and posture. Specifically, it is necessary to calculate a value of each axis of the robot that realizes the position and orientation, for example, an angle, and to command the calculated value to each axis of the robot. For axes other than axes with known axis values, use the evaluation function to derive the value of each axis that changes the value of each axis to obtain the optimal value for the evaluation function. There is a method of obtaining the derived value of each axis as the value of each axis commanded to each axis of the robot.

  FIG. 25 is a diagram for explaining a method of obtaining values of each axis of the robot using the evaluation function. This processing is performed when work target information (hereinafter referred to as work target), for example, the position of the movement destination of the hand is instructed and the values of each axis of the robot are obtained. In step A1, a work target is instructed. In step A2, initial values are set for the values of each axis with respect to the redundant axis. In step A3, the evaluation value is calculated by substituting the value of each axis into the evaluation function. In step A4, it is determined whether or not the value of each axis for which the evaluation function is the optimal evaluation value is obtained. When the value of each axis that becomes the optimum evaluation value is obtained, the process proceeds to step A5, and when the value of each axis that becomes the optimum evaluation value is not obtained, the process proceeds to step A6.

  In step A5, the obtained value of each axis is instructed to the robot, and the process ends. In step A6, the value of each axis with respect to the redundant axis is changed, and the process returns to step A3. In this way, the value of each axis that optimizes the evaluation value can be obtained by repeating the calculation.

  The degree of freedom that can be instructed by a human being is 12 degrees of freedom at most. For example, even when using a control device such as a master arm, the degree of freedom is 12 degrees of freedom, and the remaining degrees of freedom need to be calculated. There is. In the case of a humanoid robot, the remaining degrees of freedom are 18 degrees or more. Since such a calculation is repeatedly performed using the above-described evaluation function, it takes a long calculation time. For this reason, it is necessary to calculate offline using a computer with high calculation capability, and it is difficult to calculate in real time, that is, when an instruction is given, immediately calculate the value of each axis and instruct the humanoid robot.

  As a first conventional technique for controlling a humanoid robot, an operation corresponding to a human conscious movement is given to the robot by a joystick operation by an operator, and an operation corresponding to a human unconscious movement is realized by autonomous control of the robot. Proposed method. This method is a method of switching and selecting a part to be moved consciously with a switch and instructing it, and does not indicate a method for realizing autonomous control of a robot according to a work purpose (for example, Non-Patent Document 1). reference).

  As a second conventional technique for controlling a humanoid robot, when a target position and a target external force are given to the hand of the humanoid robot, a real-time control method for operating the leg portion by autonomous control so that the robot does not fall down There is. In this method, the optimal position of the shoulder is calculated by iterative calculation by the steepest descent method using the evaluation function, and the optimal posture of the robot of the two-dimensional model is obtained. If the degree of freedom of the robot increases, The number of parameters to be evaluated is increased, and the optimization calculation for the evaluation of the overall robot posture is very computationally intensive and may impair the real-time performance (see Non-Patent Document 2, for example).

  When operating a humanoid robot in real time, in order to achieve the robot's movement as intended by the operator, other redundant axes can be operated based on instructions from the operator to a part of the robot. The value needs to be calculated in a short time. In particular, when controlling a robot by remote operation, not only the real-time property of communication between the robot and the remote control device, but also every calculation cycle in which the value of each axis of the robot is calculated by a computer installed in the robot. It is important to be able to give instructions to the robot in real time. Also for robots other than humanoid robots, the calculation of the value of each axis of the robot uses an evaluation function. Therefore, if the degree of freedom is 20 degrees or more, it takes time to calculate.

Neo Ee Sian, 4 others, “Whole Body Teleoperation of a Humanoid Robot-A Method of Integrating Operator's Intention and Robot's Autonomy”, Proc. IEEE Int. Conf. Robotics and Automation, 2003 Kenji Inoue and two others, “Real-time control of a humanoid robot that moves while working with arms (control of movement in the Sagittal Plane)”, Journal of the Robotics Society of Japan, Vol. 18, No. 1, January 2000, P75- 86

  However, although the above-described first conventional technique proposes one method for realizing an operation corresponding to a human unconscious movement by the autonomous control of the robot, how to realize the autonomous control according to the work target. It is not shown.

  The second prior art described above has a problem that it takes time to calculate the values of each axis for instructing the robot by repeated calculation using an evaluation function, and real-time control cannot be performed. is there.

  An object of the present invention is to provide a robot control method and a robot system capable of obtaining the values of each axis of a robot for performing autonomous control according to a work target in a short time in a robot with a high degree of freedom. .

The present invention, a plurality of control target each area obtained by dividing a robot composed of a plurality of coupling portions as a control object for connecting a plurality of links and each link to each other, the work target indicating an action to the robot performs A setting step for presetting one priority for representing the order of calculating the target position for each control target area for achieving a work target as a priority for the operation condition ;
When work target information representing a work target is instructed , a representative point that is a part for which a movement destination position of the control target area is calculated among the parts included in the control target area for each control target area is A reference position calculation step of calculating a position that matches an operation condition as a reference position that is a candidate for a destination position of the representative point;
Based on the calculated reference position for each representative point in the reference position calculating step, according to the priorities of the operating conditions, and a target position calculating step of calculating a base Ki goal position for moving the respective representative points,
Each axis value calculation step for calculating the value of each axis of the robot based on the target position calculated in the target position calculation step;
And a commanding step of commanding each axis of the robot to the value of each axis calculated in each axis value calculating step.

According to the present invention, first, preset one operating condition and a connecting portion for connecting the links and each link in the plurality of control target each area divided as a control target robot configured, and the target position The priority indicating the order of calculation is set in advance as the priority of the operation condition. And when work target information is instructed to the robot, a representative point that is a part for which the movement destination position of the control target area is calculated among the parts included in the control target area for each control target area Is calculated as a reference position that is a candidate for the destination position of the representative point. Then, based on the calculated reference position for each representative point, according to the priorities of the operating conditions, the calculated the base Ki goal position moving the representative points, the robot on the basis of the calculated target position The value of each axis is calculated, and the calculated value of each axis is commanded to each axis of the robot.

  In this way, since the value of each axis of the robot is calculated for each control target area, it is not necessary to repeatedly calculate the entire robot, and the robot for performing autonomous control according to the work target in a robot with a high degree of freedom. The value of each axis can be obtained in a short time.

According to the present invention, in the target position calculation step, the movement permission that the reference position calculated for each representative point in the reference position calculation step is a predetermined range within a movable range in which each representative point can move. When it is within the range, the reference position of each representative point is calculated as the target position of each representative point,
When the reference position calculated for each representative point in the reference position calculation step is not within the movement permission range of the corresponding representative point, it is the position within the movement permission range and is the closest to the reference position of each control target area. A position satisfying a condition within a close movement permission range is calculated as a target position of each representative point.

  According to the present invention, even when the reference position calculated for each control target area is not within the movement permission range of the representative point, the target position is calculated at a position within the movement permission range, so that the operation can be performed within the allowable range. The value of each axis can be obtained in a short time.

In the target position calculating step, when the representative point of the control target area adjacent to the control target area for which the target position of the representative point is to be calculated has already been calculated, the adjacent control already calculated is calculated. The target position of the representative point of the control target area is calculated on the basis of the target position of the representative point of the target area.

  According to the present invention, the calculation of the target position of the representative point of the adjacent control target area is calculated based on the target position of the control target area in which the operation condition having a high priority is set. Since it is not necessary to consider the relationship with the control target area, the amount of calculation can be reduced, and the value of each axis of the robot can be obtained in a short time.

  Further, according to the present invention, an operation condition having a lower priority is set in a control target area in which an operation condition having a higher priority is set, by dividing the movement permission range of each representative point by a corresponding movable range. It is smaller than the control target area.

  According to the present invention, the ratio of the movement permission range to the movable range of the representative point of the control target area for which the high-priority operation condition is set is made smaller than the control target area for which the low-priority operation condition is set. Therefore, it is possible to reduce the influence of the target position of the representative point of the control target area in which the operation condition with high priority is set.

  Further, the present invention is characterized in that a reference position of a representative point for each of the control target areas is calculated in parallel.

  According to the present invention, since the reference position of the representative point for each control target area is calculated in parallel, the calculation time of the reference position can be performed in a short time, and further, the value of each axis of the robot is obtained in a short time. be able to.

  Further, the present invention is characterized in that the value of each axis in the control target area is calculated in parallel for each control target area.

  According to the present invention, since the value of each axis in the control target area is calculated in parallel for each control target area, the calculation time of the value of each axis of the entire robot can be shortened.

In the invention, it is preferable that the robot has 20 degrees of freedom or more.
According to the present invention, since iterative calculation is not performed, the value of each axis of the robot can be obtained in a shorter time than the calculation time according to the prior art for a robot having 20 degrees of freedom or more.

Further, the invention is characterized in that the degree of freedom of the control target area is 6 degrees of freedom or less.
According to the present invention, since the degree of freedom of the control target area is 6 degrees or less, that is, the degree of freedom in the control target area is small, the redundant degree of freedom can be reduced, and the value of each axis in the control target area. The calculation time can be shortened, and the value of each axis of the robot can be obtained in a short time.

  Further, the present invention is a humanoid robot including an arm portion corresponding to a human arm, a torso portion corresponding to a human torso, and a leg portion corresponding to a human leg, and the arm portion and the torso And the leg are defined as control target regions.

  According to the present invention, since the humanoid robot is divided into the control target areas of the arm part, the torso part, and the leg part, it can be applied to a humanoid robot having 30 degrees of freedom or more.

  According to the present invention, in the humanoid robot, the representative point of the arm part is a hand, the representative point of the torso part is a shoulder, and the representative point of the leg part is a waist, and the representative point of the control target area is moved. The ratio of the permitted range to the movable range is characterized in that the ratio of the representative point of the waist is smaller than the ratio of the representative point of the shoulder, and the ratio of the representative point of the shoulder is smaller than the ratio of the representative point of the hand.

  According to the present invention, the ratio of the movement permission range to the waist movable range is smaller than the shoulder, and the ratio of the movement permission range to the shoulder movable range is smaller than the hand, so that the operation condition with high priority is high. It is possible to reduce the influence of the target position of the waist, which is a representative point of the leg portion set with, from the shoulder and the hand, and the influence of the target position of the shoulder from the hand.

The present invention, in the reference position calculating step, when calculating the reference position of the waist, the speed of the waist, characterized in that calculated as a speed proportional to the speed of the shoulder.

  According to the present invention, the waist moving speed is proportional to the shoulder moving speed, so that the amount of forward bending of the body portion can be suppressed.

  According to another aspect of the present invention, there is provided a robot system in which a computer configured to execute the robot control method is mounted on a robot including a plurality of links and a plurality of connecting portions that connect the links to each other.

  According to the present invention, since the robot is equipped with a computer that executes the robot control method, the control information instructed from the external remote operation device may be only information related to the work target information. The amount of communication information can be reduced, and the value of each axis of the robot can be obtained in a short time.

  According to the present invention, the robot has detection means for determining work target information, generates work target information based on information detected by the detection means, and uses the generated work target information as the robot. It is characterized by the above-mentioned work target information.

  According to the present invention, the new work target information generated by the robot system based on the information from the detection means of the robot can be used as the new work target information for the robot, thereby realizing a more autonomous robot system. In addition, the value of each axis of the robot can be obtained in a short time.

The present invention also includes a dividing step of dividing a robot constituted by a plurality of links and a plurality of connecting portions that connect the links to each other into a plurality of control target areas to be controlled;
For each of the control target areas, a determination step of determining a representative point that is a part in which a movement destination position of the control target area is calculated among the parts included in the control target area;
For each of a plurality of control target areas divided in the dividing step, one operation condition for achieving a work target indicating the action to be performed by the robot is set in advance, and the control target area for achieving the work target A setting step for presetting the priority indicating the order of calculating each target position as the priority of the operation condition ;
When work target information representing a work target is instructed, a position where the representative point matches the operation condition is calculated as a reference position that is a candidate for a destination position of the representative point for each control target region. A reference position calculation step;
Based on the calculated reference position for each representative point in the reference position calculating step, according to the priorities of the operating conditions, and a target position calculating step of calculating a base Ki goal position for moving the respective representative points,
Each axis value calculation step for calculating the value of each axis of the robot based on the target position calculated in the target position calculation step;
And a command step of commanding each axis of the robot with the value of each axis calculated in each axis value calculation step.

  According to the present invention, a control target area obtained by dividing a robot into a plurality of control targets is assumed, operation conditions for achieving work target information are set for each control target area with priority, and the control target areas are defined as Since the reference position that is a candidate for the position to move is calculated, the target position is calculated according to the priority based on the calculated reference position, and the value of each axis of the robot is calculated based on the calculated target position. If the method of dividing the robot into control targets is changed, it can be applied to various robots, and it is not necessary to perform repeated calculation for the entire robot, and the values of each axis of the robot can be obtained in a short time.

  According to the present invention, since the value of each axis of the robot can be obtained in a short time, the robot can be controlled in real time, and if used to determine the value of each axis offline, Calculation time can be shortened.

  FIG. 1 is a diagram showing a configuration of a robot system 10 to which a robot control method according to an embodiment of the present invention is applied. The robot system 10 includes a robot unit 11, a control unit 12, and a communication unit 13. The robot unit 11 is a robot that includes the imaging unit 14 and includes a plurality of links and a plurality of connecting units that connect the links to each other. For example, the head unit 31, the arm unit 32, the body unit 33, and the leg unit This is a humanoid robot composed of 34 subsystems. The imaging unit 14 is a part that captures images such as a CCD camera. For example, in the case of a humanoid robot, the imaging unit 14 is mounted on the head 31 of the robot unit 11 and transmits the captured images to the control unit 12.

The control unit 12 is a part that controls the entire robot system 10 and is an OS (Operating System).
System). The computer is mounted on the robot unit 11 and can execute the robot control method shown in FIG. 2 or FIG. When the control unit 12 receives work target information (hereinafter referred to as a work target) indicating the operation to be performed by the robot from the remote control device 20, the control unit 12 sets the values of the axes of the robot for realizing the work target as shown in FIG. The robot control method shown in FIG. 21 is used for calculation, and the calculated value of each axis is instructed to the robot unit 11. The control unit 12 transmits the video from the imaging unit 14 to the remote control device 20 via the communication unit 13.

  The communication unit 13 is a part that communicates with the remote operation device 20, transmits information received from the remote operation device 20 to the control unit 12, and transmits information from the control unit 12 to the remote operation device 20.

  The remote operation device 20 is a device for transmitting an instruction to the robot system 10 input by the operator to the robot system 10, and includes an input unit 21, a control unit 22, a communication unit 23, and a display unit 24. Is done. Communication with the robot system 10 may be wired or wireless, for example, communication via a network or a dedicated communication line.

  The input unit 21 includes an input device such as a master arm or a joystick, and may use an input device such as a keyboard and a mouse together. The input unit 21 transmits information input from these input devices to the control unit 22.

  The control unit 22 is a part that controls the entire remote operation device 20, and sends information that needs to be transmitted to the robot system 10 among the information input from the input unit 21 to the communication unit 23. Of the information received from the robot system 10 via the communication unit 23, the control unit 22 transmits information to be displayed on the display unit 24 to the display unit 24.

  The communication unit 23 is a part that communicates with the robot system 10, transmits information received from the robot system 10 to the control unit 22, and transmits information from the control unit 22 to the robot system 10.

  The display unit 24 is a display device having a display screen such as a liquid crystal display, and displays information transmitted from the control unit 22 on the display screen. The operator can operate the robot by instructing a work target from the input device while viewing the video displayed on the display screen.

  Since the robot is equipped with a computer that executes the robot control method, the control information instructed from the external remote control device is only information related to the work target, and the amount of communication information between the robot system and the remote control device is reduced. Furthermore, the value of each axis of the robot can be obtained in a short time.

  The robot system 10 is equipped with detection means for determining a work target, for example, a sensor for measuring a distance, and the control unit 12 generates a work target based on information detected by the mounted sensor, and the remote control device 20 Instead of the work target received from, the work target generated by the control unit 12 may be used as the robot work target. Alternatively, based on the information from the imaging unit 14, the control unit 12 recognizes the hand position, and when the hand position has not reached the target position, a position for approaching the target position is generated as a new work target. The generated work target may be used as the robot work target. In this case, since the work target can be generated by the robot system 10 itself, a more autonomous robot system can be realized.

  FIG. 2 is a flowchart showing the steps of the robot control method according to the embodiment of the present invention. This processing is started when a work target, that is, a movement to be realized by the robot is instructed from a remote operation device having an input device such as a joystick. In this robot control method, the robot's hand, for example, the value of each axis of the robot when the work target of moving the tip of the robot arm 3a shown in FIG. Is obtained by obtaining the angles of the joints constituting the, and commanding the obtained angles to each axis of the robot.

  In step S1, a work target from the remote control device, for example, a designated position of a hand movement destination is acquired. In step S2, the position where the hand matches the movement target, which is the movement condition set for the arm 3a, for example, is calculated as a reference position, and the shoulder, for example, the robot arm 3a and the body 4 shown in FIG. Is calculated as a reference position, and the waist, for example, the connection part between the body part 4 and the leg part 5a of the robot shown in FIG. The position that matches the operation target 5a is calculated as each reference position.

  The operation target is a target set for each control target area, that is, for each subsystem according to the work target instructed from the remote control device. For example, the operation target set for each subsystem is the arm 3a. , “To realize the operation according to the work target”, the body part 4 “to increase the reach of the hand”, the leg 5a “to maintain the balance of the whole body and reach the hand in the vertical direction “Expanding the scope”. Since the operation target for achieving the work target is set in each part, the robot as a whole can maintain the balance of the robot, keep the operation range of the hand wide, and can realize the operation according to the work target.

  The subsystem is a group in which one or a plurality of links are connected by a connecting part so as to be displaceable. For example, in the robot shown in FIG. 24, the arm part 3a, the body part 4, the leg part 5a, and the like are subsystems. The reference position is a candidate for a position to move a representative point such as a hand, shoulder, or waist. The representative point is a part in which the position to be moved among the robot parts is calculated first in order to calculate the value of each axis that determines the movement of the entire robot. The connection part of the system is a representative point, and one is set for each subsystem.

  In step S3, based on the calculated reference position, the target positions of the hand, shoulder and waist are calculated according to the priority of the motion target set to achieve the work target. The priority of the motion target is determined according to the work target, and is in the order of the leg portion 5a, the trunk portion 4, and the arm portion 3a. The target position is a position where the representative point is moved. In step S4, the value of each axis of the robot is calculated based on the calculated target position. In step S5, the calculated value of each axis is instructed to each axis of the robot, and the process ends. In this embodiment, the description has been made by paying attention only to the arm portion 3a, the torso portion 4, and the leg portion 5a shown in FIG. 24, but other subsystems such as the head 2, the arm portion 3b, the leg portion 5b, etc. Also calculate by setting the action target and priority.

  FIG. 3 is a diagram showing an example of dividing the robot 30 into subsystems to which the robot control method shown in FIG. 2 is applied. FIG. 3A is an overview diagram of the robot 30. The robot 30 includes a head portion 31, an arm portion 32, a body portion 33, and a leg portion 34. The robot 30 shows a position where the hand of the right arm moves with respect to the object 62 on the desk 61. A field of view 31s indicates the field of view of the imaging unit mounted on the head 31 of the robot.

  FIG. 3B is a view of the robot 30 as viewed from the side. The robot 30 includes an arm portion 32, a torso portion 33, and a leg portion 34, and is divided into three subsystems. . Regarding the leg part 34, the left leg part overlaps with the right leg part, and for the arm part, the left arm part overlaps with the right arm part. Although the hand portion 35 and the head portion 31 are omitted, each of the hand portion 35 and the head portion 31 may be included as one subsystem.

  The arm portion 32 includes a hand 41, an elbow 42, and a first link 51 and a second link 52, and a connecting portion between the first link 51 and the second link 52 is the elbow 42. The body portion 33 includes a shoulder 43, a chest 44, and a third link 53 and a fourth link 54. A connecting portion between the second link 52 and the third link 53 is the shoulder 43, and the third link. A connecting portion between 53 and the fourth link 54 is a chest 44. The leg portion 34 includes a waist 45, a knee 46, and a fifth link 55 and a sixth link 56, a connecting portion between the fourth link 54 and the fifth link 55 is the waist 45, and the fifth link A connecting portion between 55 and the sixth link 56 is a knee 46. The foot 47 is a part that does not belong to any of them, and is used as a reference point. The connecting portion can be displaced.

  The representative point of the arm part 32 is the hand 41, the representative point of the body part 33 is the shoulder 43, and the representative point of the leg part 34 is the waist 45. This representative point is a part where the position to be moved is calculated first among the parts of the robot in order to calculate the value of each axis that determines the movement of the entire robot, and the part to which an instruction is given by the work target The connecting portion of the subsystem becomes a representative point, and one is set for each subsystem. In this example, a part to which an instruction is given according to a work target is the hand 41, and a connecting portion of the subsystem is a shoulder 43 and a waist 45.

  The robot 30 is in a balanced state at the position of the foot 47 on the horizontal surface 60 such as the floor surface or the ground, that is, in a state where the robot 30 does not fall down. In the following description, “hand operation without moving the foot position” However, the present invention can also be implemented when moving the foot.

  FIG. 4 is a diagram for explaining the degree of freedom of each part of the humanoid robot. 4 (a) and 4 (b) are diagrams for explaining symbols used in FIGS. 4 (c) to 4 (g). FIG. 4A is a view for explaining the coaxial joint 380 that rotates about the axial direction. The coaxial joint 380 connects the link member 381 and the link member 382, and the link member 382 rotates about the axial direction of the link member 381 with respect to the link member 381. FIG. 4B illustrates a bending joint 390 that rotates one link member relative to the other link member about a straight line that passes through the center of the joint in a straight line that is perpendicular to a plane that includes the axes of both link members. FIG. The bending joint 390 rotates the link member 393 around the axis 391 of the bending joint 390 on a plane including the central axis of the link member 392. Each of the coaxial joint 380 and the bending joint 390 has one degree of freedom.

  FIG. 4C is a diagram showing the degree of freedom of the head portion 31, and there are two axes of the coaxial joint 311 and the bending joint 312, which is a total of two degrees of freedom. The coaxial joint 311 and the bending joint 312 correspond to a neck that is a connection portion between the head portion 31 and the body portion 33. FIG. 4D is a diagram showing the degree of freedom of the arm portion 32, and there are six axes of coaxial joints 321, 323, 325 and bending joints 322, 324, 326 for a total of 6 degrees of freedom. The coaxial joints 321 and 323 and the bending joint 322 correspond to the shoulder, the bending joint 324 corresponds to the elbow, and the coaxial joint 325 and the bending joint 326 correspond to the wrist.

  FIG. 4 (e) is a diagram showing the degree of freedom of the body portion 33, and there are two axes of a bending joint 331 and a coaxial joint 332, and there are a total of two degrees of freedom. The bending joint 331 and the coaxial joint 332 correspond to the chest. FIG. 4 (f) is a diagram showing the degree of freedom of the leg portion 34, and there are six axes of coaxial joints 341, 343, 344, 345 and coaxial joints 342, 346, for a total of 6 degrees of freedom. The coaxial joints 341 and 343 and the bending joint 342 correspond to the waist, the coaxial joint 344 corresponds to the knee, and the coaxial joint 345 and the bending joint 346 correspond to the ankle. FIG. 4G is a diagram showing the degree of freedom of the hand portion 35, and there is one axis of the bending joint 351, which is a total of one degree of freedom. The bending joint 351 corresponds to a finger joint and opens and closes the finger.

  FIG. 5 is a diagram illustrating an example of a work target instruction to the robot 30 to which the robot control method illustrated in FIG. 2 is applied and a robot displacement according to the instruction. The work target for the robot 30 is instructed from a remote control device having an input unit such as a master arm or a joystick. In this example, the work target is to move the hand 41 to the position of the hand pointing position 41a. By obtaining the value of each axis of the robot that realizes the work target and instructing each axis of the robot to obtain the value of each axis, the hand moves from the position and posture of the robot of the hand 41 to the designated position 41a. Change the position and posture when.

  FIG. 6 is a flowchart of calculation of the reference position of the hand 41 in the robot control method shown in FIG. This process is performed when the reference position of the hand 41 is calculated. In step S <b> 10, the designated position of the hand 41 instructed from the remote operation device is ended as the reference position of the hand 41.

  FIG. 7 is a flowchart of calculation of the reference position of the shoulder 43 in the robot control method shown in FIG. This processing is performed when the reference position of the shoulder 43 is calculated. The operation target of the body part 33 is “enlarge the reach range of the hand”, and the reference position of the shoulder 43 is obtained from the reference position and the current position of the hand 41 and the current position of the shoulder 43.

In step S20, a distance L _arm between the current position of the shoulder 43, that is, the position moved by the previous command to each axis of the robot and the reference position of the hand 41 is calculated. In step S21, it is determined whether or not the calculated distance L _arm is longer than a predetermined maximum distance L _max . When the calculated distance L _arm is longer than the predetermined maximum distance L _max , the process proceeds to step S24, and when the calculated distance L _arm is not longer than the predetermined maximum distance L _max , the process proceeds to step S22.

In step S22, it is determined whether or not the calculated distance L _arm is shorter than a predetermined minimum distance L _min . When the calculated distance L _arm is shorter than the predetermined minimum distance L _min , the process proceeds to step S23, and when the calculated distance L _arm is not shorter than the predetermined minimum distance L _min , the process proceeds to step S25. In step S23, as shown in FIG. 7, the reference position of the shoulder 43 is set to a vector in the direction opposite to the direction from the hand 41 to the shoulder 43 on the straight line connecting the shoulder 43 and the hand 41, and the waist 45 and the shoulder 43. The process ends with the position of the shoulder 43 being moved in the direction of the combined vector of the direction perpendicular to the connecting direction and the opposite direction of the hand 41.

  In step S24, the reference position of the shoulder 43 is ended as a position for moving the shoulder 43 in the direction of the hand 41 at a constant speed. In this case, the position to be moved at a constant speed is a position at which the speed when moving to the previously calculated target position can be maintained. In step S25, the current position of the shoulder 43 is set as the reference position of the shoulder 43, and the process ends.

  Thus, when the hand 41 moves in the direction in which the arm portion 32 extends, the shoulder 43 also follows, so that the arm portion 32 can be further extended. Conversely, when the hand 41 approaches the shoulder 43, the torso portion 33 moves away from the current position of the hand 41 to avoid interference between the arm portion 32 and the body portion 33, that is, contact or collision.

FIG. 8 is a diagram for explaining the calculation of the reference position of the shoulder 43 in the robot control method shown in FIG. When the distance L _arm between the current position of the shoulder 43 and the reference position of the hand 41 is longer than the predetermined distance L _max , the movement direction of the shoulder 43 is the direction vector 80 from the hand 41 to the shoulder 43, the waist 45 and the shoulder 43. The direction of the combined vector 83 with the direction vector 82 perpendicular to the straight line 81 connecting the two.

  FIG. 9 is a flowchart of calculation of the reference position of the waist 45 in the robot control method shown in FIG. This process is performed when the reference position of the waist 45 is calculated. The movement target of the leg 34 is two points of “maintaining the balance of the whole body of the humanoid robot” and “enlarging the reach range of the hand in the vertical direction”. The reference position of the waist 45 is obtained from the position and the current positions of the hand 41, the shoulder 43, and the waist 45.

  In step S30, the current center-of-gravity position is calculated. The calculation of the center of gravity position assumes that the motion of the body portion 33 and the leg portion 34 moves at a speed that does not greatly affect the dynamic motion of the robot motion, that is, considers the inertia accompanying the motion of the robot. It is assumed that the operation is performed at a speed that is not necessary. Whether the floor projection point (also referred to as the floor center of gravity), which is the position of the center of gravity of the whole body projected on the floor, is within a predetermined range within the support polygon, which is a polygon composed of the soles of both feet. Thus, the position of the horizontal component of the reference position of the waist 45 is obtained. A method for obtaining the position of the center of gravity of the robot will be described later.

  In step S31, it is determined whether or not the calculated gravity center position, that is, the floor projection point, is a predetermined distance or more from the center point of the support polygon. If the distance is greater than or equal to the predetermined distance, the process proceeds to step S36. If the distance is not greater than the predetermined distance, the process proceeds to step S32. In step S32, it is determined whether or not the movement direction of the shoulder 43 is downward. When the moving direction of the shoulder 43 is downward, the process proceeds to step S37, and when the moving direction of the shoulder 43 is not downward, the process proceeds to step S33.

In step S33, it is determined whether the moving direction of the shoulder 43 is upward. When the moving direction of the shoulder 43 is upward, the process proceeds to step S34, and when the moving direction of the shoulder 43 is not upward, the process proceeds to step S39. In step S34, it is determined whether or not the distance L _body between the current position of the waist 45 and the current position of the shoulder 43 is _equal to or greater than _a predetermined body distance La. When the distance _{L body} is equal to or larger than a predetermined fuselage distance _{L a,} the process proceeds to step S35, when the distance _{L body} is not predetermined fuselage distance _{L a} or more, the process proceeds to step S39. In step S35, the vertical component of the reference position of the waist 45 is ended as a position to move upward at a speed proportional to the vertical component of the moving speed of the shoulder 43.

In step S36, the process proceeds to step S32 as a position where the horizontal component of the reference position of the waist 45 moves in the center direction of the support polygon at a speed proportional to the distance from the center. At step S37, it is determined whether or not the inclination theta _w of the body 33 to be described later is inclined a predetermined angle theta _a more from the vertical direction. When tilted a predetermined angle theta _a more, the process proceeds to step S38, the when not tilted a predetermined angle theta _a or more, the process proceeds to step S39.

  In step S38, the vertical component of the reference position of the waist 45 is set as a position to move downward at a speed proportional to the vertical component of the moving speed of the shoulder 43. When the hand 41 moves downward, an operation of bending the body part 33 deeply without moving the position of the waist 45 is conceivable. However, the movement of the leg joint is limited, which is difficult to realize. Move in conjunction with the movement. In step S39, the current position of the waist is set as the waist reference position, and the process ends.

FIG. 10 is a diagram for explaining that the movement of the waist 45 is interlocked with the movement of the shoulder 43. 10 (a) is a diagram for explaining the inclination theta _w of the body 33. The inclination θ _w of the body part 33 is an inclination of a straight line 81 connecting the waist 45 and the shoulder 43 with respect to the vertical direction 84. When the inclination theta _w is greater than or equal to a predetermined angle theta _a, lowering the reference position of the waist 45 in conjunction with the shoulder 43. 10B shows the movement of the body part 33 when the waist 45 is not moved, and FIG. 10C shows the movement of the body part 33 when the waist 45 is moved at a speed proportional to the movement speed of the shoulder 43. FIG. 10D is a diagram for explaining the movement of the body portion 33 when the waist 45 is moved at a constant speed.

FIG. 10 (b), without moving the hips 45, when moved in the direction of arrow 86a of the shoulder 43 to the position of the shoulder 43a, the inclination of the links 53 between the breast 44 and the shoulder 43 is theta _b changes. In contrast, in FIG. 10C, when the shoulder 43 moves in the direction of the arrow 86a to the position of the shoulder 43a, the waist 45 moves in the direction of the arrow 87a to the position of the waist 45a. angles, theta _c only only does not change, can be less than the slope of the change in theta _b of the link when the fixed waist FIG 10 (b). That is, the speed at which the body part 33 tilts can be suppressed, and the amount of forward bending of the body part can also be suppressed. In particular, when the body portion 33 is fully extended, the rotation angle of the shoulder centered on the chest 44 is larger than the rotation angle of the shoulder 43 centered on the waist 45. When the body portion 33 is largely inclined, for example, when the inclination theta _w of the body portion 33 is inclined more than 45 degrees, the rotational speed of the proportion of large body portion 33 occupying the total mass of the robot is large, dynamic The influence of movement, that is, the influence of inertia increases, and the robot easily falls. In order to prevent overturning, suppressing the rotational speed of the body portion 33 is effective for realizing a stable control operation of the humanoid robot.

  FIG. 10 (d) shows a case where the waist 45 moves at a constant speed independent of the movement of the shoulder 43. In this case, the speed at which the body portion 33 tilts cannot always be suppressed. . When the shoulder 43 moves in the direction 86b, that is, in the forward direction, the shoulder 43 moves to the position of the shoulder 43b. However, when the waist 45 moves in the direction 87b and comes to the position of the waist 45b, the shoulder 43 It will move to the position 43c, and the reference position of the shoulder may not be realized. Therefore, when the shoulder 43 moves downward, it is necessary to move the waist 45 at a speed proportional to the movement speed of the shoulder 43.

The position of the center of gravity of the robot is calculated from the value of each axis of the robot, that is, the angle and the center of gravity information of the link. For example, if the front-rear direction of the robot 30 is the x-axis direction, the center-of-gravity position X in the x-axis direction can be obtained by Expression (1).
X = (M ₁ X ₁ + M ₂ X ₂ +... + M _n X _n ) / (M ₁ + M ₂ +... + M _n )
... (1)
Here, M _i is the weight of the link i, and X _i is the barycentric coordinate of the link i. i is a natural number from 1 to n, and X _i is the value of the barycentric coordinate of the link i when the robot's x coordinate when the robot is upright is used as a reference.

Similarly, the center-of-gravity position Y in the y-axis direction, which is a direction perpendicular to the x-axis direction of the robot on the horizontal plane, can also be obtained by Expression (2).
Y = (M ₁ Y ₁ + M ₂ Y ₂ +... + M _n Y _n ) / (M ₁ + M ₂ +... + M _n )
... (2)
Here, Y _i is the center-of-gravity coordinate of link i in the y-axis direction, and is the value of the center-of-gravity coordinate of link i based on the y-coordinate of the robot when the robot is upright.

  FIG. 11 is a diagram for explaining how to obtain the center of gravity position of the link. The link A90 and the link B91 are connected by a joint 92. The joint 92 is integral with the link B91, and the rotating shaft 93 constituting the joint 92 is integral with the link A90. In this case, the weight of the rotating shaft 93 is included in the link A90. That is, the weight of the joint portion is calculated by including it in the link that is integral. The weight and the position of the center of gravity of each link can be obtained from 3D CAD design data or the like.

FIG. 12 is a diagram for explaining the coordinate system of the robot. For humanoid robot, the reference position and the target position is represented by the absolute coordinate system sigma _W seen from the coordinate system of the robot. Absolute coordinate system sigma _W of the robot, at the time of initializing the robot, and the coordinate system fixed on the floor of the vertically downward direction from the waist coordinate system sigma _k of the robot. Hand position and orientation instruction indicates the position and orientation of the hand coordinate system sigma _T viewed from the absolute coordinate system sigma _W of the robot. Viewed in three dimensions, the robot's hip coordinate system Σ _k is an intermediate point 8 between the hip joints of the left and right legs, as shown in FIG.

  The position of the joint part of the link is obtained from the value of each axis of the robot, that is, the angle, by a forward conversion formula. The forward conversion calculation formula is created using a geometric relationship depending on the link structure of each part of the robot.

FIG. 13 is a diagram for explaining the coordinate system of each axis when obtaining the position and orientation from the value of each axis of the leg portion 34. The angle and the coordinate system of the joint 341～ joint 346, theta ₁ through? _6, respectively, and Σ ₁ ~Σ _6, the ankle of the coordinate system and sigma _f. In FIG. 13, the reference coordinate system Σ ₀ of the robot is the intersection of the rotation axes of the coaxial joints 341 and 343 and the bending joint 342, and is the position of the connecting portion 7 shown in FIG.

The distances L ₁ to L ₃ represent the length from the bending joint 342 to the bending point 350, the length from the bending point 350 to the coaxial joint 344, and the length from the coaxial joint 344 to the coaxial joint 345, respectively. The length from the coaxial joint 345 to the bending joint 346 is zero.

The forward conversion is to obtain the position and orientation ⁰ T _f of the ankle coordinate system Σ _f viewed from the reference coordinate system Σ ₀ when the angles of the respective axes, that is, the joints 341 to 346 are given. When the position and orientation ⁰ T _f is represented by a matrix, it can be represented by Expression (3).

However,
R ₁₁ = C ₁ C ₃₄₅ -S ₁ S ₂ S ₃₄₅
R ₁₂ = S ₁ S ₂ C ₃₄₅ S ₆ + C ₁ S ₃₄₅ S ₆ -S ₁ C ₂ C ₆
R ₁₃ = S ₁ S ₂ C ₃₄₅ C ₆ + C ₁ S ₃₄₅ C ₆ + S ₁ C ₂ S ₆
R ₂₁ = S ₁ C ₃₄₅ + C ₁ S ₂ S ₃₄₅
R ₂₂ = −C ₁ S ₂ C ₃₄₅ S ₆ + S ₁ S ₃₄₅ S ₆ + C ₁ C ₂ C _{6
R 23 = -C 1 S 2 C} 345 C 6 + S 1 S 345 C 6 -C 1 C 2 S 6
R ₃₁ = —C ₂ S ₃₄₅
R ₃₂ = C ₂ C ₃₄₅ S ₆ + S ₂ C ₆
R ₃₃ = C ₂ C ₃₄₅ C ₆ -S ₂ S _{6
X = -L 2 (S 1 S} 2 C 3 + C 1 S 3) -L 3 S 1 S 2 C 34 -L 3 C 1 S 34
+ L ₁ S ₁ C _{2
Y = L 2 (C 1 S} 2 C 3 -S 1 S 3) + L 3 C 1 S 2 C 34 -L 3 S 1 S 34
-L ₁ C ₁ C _{2
Z = -L 3 C 2 C 3} -L 2 C 2 C 34 -L 1 S 2
S _n = sin (θ _n ), C _n = cos (θ _n ), n is a natural number,
S ₃₄₅ = sin (θ ₃ + θ ₄ + θ ₅ ), C ₃₄₅ = cos (θ ₃ + θ ₄ + θ ₅ )
It is.

The coordinates of the shoulder 43 and the hand 41 can also be calculated using a forward conversion formula.
FIG. 14 is a diagram for explaining the relationship between the support polygon 100 and the floor projection point 103. The floor projection point 103 can be expressed as a position (x, y) where the center-of-gravity position of the whole body is projected onto the floor surface by using the above-described x, y coordinates. The support polygon 100 is a polygon constituted by the soles of both feet, and the position of the horizontal component of the reference position of the waist 45 is determined by whether or not the floor projection point 103 is within a predetermined range in the support polygon. Ask.

It is determined whether or not the position of the center of gravity of the robot, that is, the floor projection point 103 is a predetermined distance or more from the center point 102 of the support polygon 100, that is, outside the range 101. When the range 101 out of the horizontal component of the reference position of the waist 45, in the central direction 105 of the support polygon 100, and a position that moves a speed proportional to the distance L _M of the center. When not outside the range 101, the horizontal component of the current position of the waist 45 is set as the horizontal component of the reference position of the waist 45.

  FIG. 15 is a flowchart of calculation of the target position of the waist 45 in the robot control method shown in FIG. This process is performed when the target position of the waist 45 is calculated. In step S40, it is determined whether or not the reference position of the waist 45 is within a movement permission range described later of the waist 45. When it is within the movement permission range, the process proceeds to step S41, and when it is not within the movement permission range, the process proceeds to step S42. In step S41, the reference position of the waist 45 is set as the target position of the waist 45, and the process ends. In step S42, the position closest to the reference position of the waist 45 in the horizontal direction from the current position of the waist 45 in the movement permission range of the waist 45 is ended as the target position of the waist 45.

  FIG. 16 is a diagram for explaining the movement permission range of the representative point. FIG. 16A is a diagram for explaining the movement permission range of the waist 45. The movement permission range 110 of the waist 45 is a predetermined range within the movable range in which the waist 45 can move. In this case, since the reference position 111 of the waist 45 is not in the movement permission range 110, the target position of the waist 45 is closest to the reference position 111 in the horizontal direction 113 of the waist 45 and within the movement permission range 110. The position 112 is set as a target position. When the reference position of the waist 45 is within the movement permission range 110, the reference position becomes the target position as it is. The movement permission range 110 is a predetermined range of the movable range 114.

  FIG. 16B is a diagram for explaining the operation permission range of the shoulder 43. The movement permission range 120 of the shoulder 43 is a predetermined range within the movable range in which the shoulder 43 can move. In this case, since the reference position 121 of the shoulder 43 is not within the movement permission range 120, the target position of the shoulder 43 is the position 122 closest to the reference position 121 within the movement permission range 120. When the reference position of the shoulder 43 is within the movement permission range 120, the reference position becomes the target position as it is. The movement permission range 120 is a predetermined range of the movable range 123.

  FIG. 17 is a flowchart for calculating the target position of the shoulder 43 in the robot control method shown in FIG. This processing is performed when the target position of the shoulder 43 is calculated. In step S50, it is determined whether or not the reference position of the shoulder 43 is within the movement permission range of the shoulder 43. When it is within the movement permission range, the process proceeds to step S51, and when it is not within the movement permission range, the process proceeds to step S52. In step S51, the reference position of the shoulder 43 is set as the target position of the shoulder 43. In step S <b> 52, the position closest to the reference position of the shoulder 43 within the movement permission range of the shoulder 43 is ended as the target position of the shoulder 43.

  FIG. 18 is a flowchart of calculation of the target position of the hand 41 in the robot control method shown in FIG. This processing is performed when the target position of the hand 41 is calculated. The movement permission range of the hand 41 is a predetermined range within the movable range in which the hand 41 can move. In step S <b> 60, it is determined whether the reference position of the hand 41 is within the movement permission range of the hand 41. When it is within the movement permission range, the process proceeds to step S61, and when it is not within the movement permission range, the process proceeds to step S62. In step S <b> 61, the reference position of the hand 41 is ended as the target position of the hand 41. In step S 62, the position closest to the reference position of the hand 41 within the movement permission range of the hand 41 is ended as the target position of the hand 41.

  The calculation of the target position of the representative point of the adjacent subsystem is calculated based on the target position of the subsystem for which a high-priority operation target has been set, that is, considering the relationship with other subsystems Since there is no need, the amount of calculation can be reduced, and the value of each axis of the robot can be obtained in a short time.

  Further, since a new target position value is obtained based on the previously obtained target position and the previously obtained values of the respective axes, the next target position based on the current state of the operating robot can be calculated.

  Furthermore, since the ratio of the movement permission range to the movable range of the representative point of the subsystem with the high-priority operation target set is smaller than the subsystem with the low-priority operation target set, the priority It is possible to reduce the influence of the target position of the representative point of the subsystem in which a high operation target is set.

  The ratio of the movement permission range to the movable range of the representative point is such that the ratio of the waist 45 is smaller than the ratio of the shoulder 43 and the ratio of the shoulder 43 is smaller than the ratio of the hand 41. For example, the ratio of the waist 45, the shoulder 43, and the hand 41 is 70%, 80%, and 90%, respectively. This is because the target position is determined from a subsystem in which a high-priority operation target is set, so that the influence of the waist 45 is larger than the influence of the shoulder 43 and the influence of the shoulder 43 is larger than the influence of the hand 41. The movement permission range of subsystems that have a large impact has been tightened.

  The ratio of the permitted movement range to the hip movable range is smaller than the shoulder, and the ratio of the permitted movement range to the shoulder movable range is smaller than the hand. The influence of the target position of the waist, which is a representative point, can be made smaller than that of the shoulder and the hand, and the influence of the target position of the shoulder can be made smaller than that of the hand.

  As described above, the calculation of the target position can be reduced because the reference position is set as the target value if the reference position is within the movement permission range. Even when the reference position calculated for each subsystem is not within the movement permission range of the representative point, the target position is calculated at a position within the movement permission range, so the value of each axis that can operate within the allowable range is shortened. You can ask for time. That is, instead of obtaining an optimum value as in the prior art, the calculation is performed as long as it is within a range of a certain evaluation criterion, so that real-time control of the robot can be performed.

  When the target positions of the hand 41, the shoulder 43, and the waist 45 are calculated, a value of each axis of the robot, for example, an angle, for example, is obtained by inverse kinematic calculation based on the information on the target position and the foot position. It is done. Inverse kinematics calculation is performed by creating a conversion formula using a geometric relational expression depending on the link structure of each part of the robot. Specifically, the robot can be obtained by solving an equation that can uniquely calculate the angle of each axis from the above-described forward conversion calculation formula (3) and the positional relationship formula of the link that is derived depending on the link structure. Find the value of each axis.

Since the target positions of the hand 41, the shoulder 43, and the waist 45 are positions and orientations as seen from the reference coordinate system Σ ₀ of the robot, for example, when performing the inverse transformation of the arm 32, the hand as seen from the coordinate system of the shoulder 43 The position and orientation of the 41 coordinate system is calculated, and this is used in the arm inverse transformation calculation formula to calculate the angle of each axis of the arm. As a specific example, the reverse conversion of the leg part 34 described with reference to FIG. 13 will be described. The angles θ ₁ to θ ₆ of the joints 341 to 346 when the position and orientation ⁰ T _{f of} the ankle coordinate system Σ _f viewed from the reference coordinate system Σ ₀ are given are obtained.

The position and orientation ⁰ T _f of the ankle coordinate system Σ _f viewed from the reference coordinate system Σ ₀ to the reference coordinate system Σ ₀ is given by Expression (3), and the matrix R is expressed by Expression (4). The following relational expression holds.

At this time, the coordinates (x, y, z) of the ankle have the following relationship.
_{x = -L 3 S 5 -L 2} S 45 ... (6)
y = (L ₃ C ₅ + L ₂ C ₄₅ ) S ₆ + L ₁ C ₆ (7)
z = (L ₃ C ₅ + L ₂ C ₄₅ ) C ₆ -L ₁ S ₆ (8)

From the equations (7) and (8), the following equation is established, and the angle θ _{6 of the} joint 346 is obtained.
_{C 6 y-S 6 z =} L 1

From the equations (6) to (8), the following equation is established, and the angle θ _{4 of the} joint 344 is obtained.
x ² + y ² + z ² = L ₁ ² + L ₂ ² + L ₃ ² + 2L ₂ L ₃ C ₄

When the equation (6) is modified, the following equation is established. Similarly, the equation (8) is modified, and the angles θ ₄ and θ ₆ can be substituted to calculate S ₅ and C ₅ .
_{x = (- L 3 -L 2} C 4) S 5 -L 2 S 4 C 5

The angle θ _{5 of the} joint 345 can be calculated from S ₅ and C ₅ .
θ ₅ = −atan2 (S ₅ , C ₅ )

Next, from the above-described relational expression for forward conversion, the following expression is established, and the angle θ _{2 of the} joint 342 can be calculated.

_{S 2 = R 32 C 6 -R} 33 S 6
Similarly, from the relational expression of the forward conversion described above, the following expression is established, and S ₃₄₅ and C ₃₄₅ can be calculated.
S ₃₄₅ = _-R 31 / _{C 2
C 345 = (R 33 + S} 2 S 6) / (C 2 C 6)

The angle θ _{3 of the} joint 343 can be calculated by substituting θ ₄ and θ ₅ into θ ₃₄₅ calculated from S ₃₄₅ and C ₃₄₅ .
θ ₃₄₅ = atan2 (S ₃₄₅ , C ₃₄₅ ) = θ ₃ + θ ₄ + θ ₅

From the relational expression of the forward conversion described above, the following expression is established, and S ₁ and C ₁ can be calculated.
S ₁ = (− S ₂ S ₃₄₅ R ₁₁ + C ₃₄₅ R ₂₁ )
/ (C ₃₄₅ ² + S ₂ ² S ₃₄₅ ² )
C ₁ = (C ₃₄₅ R ₁₁ + S ₂ S ₃₄₅ R ₂₁ )
/ (C ₃₄₅ ² + S ₂ ² S ₃₄₅ ² )

The angle θ _{1 of the} joint 341 can be calculated by calculating from S ₁ and C ₁ .
θ ₁ = atan2 (S ₁ , C ₁ )

  FIG. 19 is a diagram showing an algorithm from the calculation of the reference position to the calculation of the value of each axis in the robot control method shown in FIG. The first reference position calculation step 70 is a step of calculating the reference position of the hand 41. When the indicated position of the hand 41 that is the work target acquired from the remote operation device is given as an input, the given hand 41 is given. Is output as the reference position of the hand 41. The second reference position calculation step 71 is a step of calculating the reference position of the shoulder 43, and the indicated position of the hand 41, the current position of the hand 41 held in the target position holding step 76, and the current position of the shoulder 43. Is given as an input, the reference position of the shoulder 43 is calculated and output based on the information.

  The third reference position calculation step 72 is a step of calculating the reference position of the waist 45, and the current position of the shoulder 43 and the current position of the waist 45 held in the target position holding step 76, and the gravity center position calculation step. When the current center-of-gravity position calculated in 78 is given as an input, the reference position of the waist 45 is calculated and output based on the information. The calculation of the reference positions of the tip 41, the shoulder 43, and the waist 45 is performed in parallel.

  Since the reference position of the representative point for each subsystem is calculated in parallel, the calculation time of the reference position can be performed in a short time, and further, the value of each axis of the robot can be determined in a short time.

  The first target position calculation step 73 is a step of calculating the target position of the waist 45. The reference position of the waist 45, which is the output of the third reference position calculation step 72, and the reference position are fixed. When the position of a foot is given as an input, the target position of the waist 45 is calculated and output. The second target position calculation step 74 is a step of calculating the target position of the shoulder 43. The reference position of the shoulder 43, which is the output of the second reference position calculation step 71, and the first target position calculation step 73. When the target position of the waist 45, which is the output of, is given as an input, the target position of the shoulder 43 is calculated and output.

  The third target position calculation step 75 is a step of calculating the target position of the hand 41, and the reference position of the hand 41 that is the output of the first reference position calculation step 70 and the second target position calculation step 74. When the target position of the shoulder 43, which is the output of, is given as an input, the target position of the hand 41 is calculated and output.

  In the target position holding step 76, the target position of the hand 41 that is the output of the third target position calculating step 75, the target position of the shoulder 43 that is the output of the second target position calculating step 74, and the first target position calculation. When the target position of the waist 45, which is the output of step 73, is given as input, the information is held and output. Each axis value calculation step 77 is a step of calculating values of the respective axes of the arm portion 32, the body portion 33, and the leg portion 34, and the target position of the hand 41 and the target of the shoulder 43 that are outputs of the target position holding step 76. When the position, the target position of the waist 45, and the position of the foot are given as inputs, inverse kinematics calculation is performed to determine the value of each axis of the robot, and the determined value of each axis is commanded to each axis of the robot. . The output of each axis value calculation step 77 is input to the gravity center position calculation step 78. In the center-of-gravity position calculation step 78, the center-of-gravity position of the robot is calculated based on the input values of each axis of the robot. Calculation of the value of each axis in each axis value calculation step 77 is performed in parallel for each subsystem. Since the value of each axis in the subsystem is calculated in parallel for each subsystem, the calculation time of the value of each axis of the entire robot can be shortened.

  In the above-described embodiment, it is assumed that the motion is performed at a speed that does not significantly affect the dynamic motion of the robot motion, that is, the motion is performed at a speed that does not require consideration of the inertia associated with the motion of the robot. It is possible to maintain the overall balance of the robot when it is assumed that In order to perform the robot operation quickly, it is necessary to consider a dynamic element accompanying the robot operation, that is, inertia. When calculating the target position, recalculate the target position based on the inertia of the movement of the center of gravity accompanying the movement of the robot, and calculate the value of each axis of the robot considering dynamic elements can do.

  The target position is recalculated in consideration of the dynamic factors resulting from the robot's movement, for example, the inertia of the robot in motion, so that a more stable target position can be calculated and the robot's stability can be improved. .

  In the above-described embodiment, the head 31 is not included as a subsystem. However, when the head 31 is included as a subsystem, an operation target with a lower priority is set as “the field of view that the head operation target is necessary for the work”. In this case, the torso unit 33 also has an operation target of “keep the movement range of the head wide”. If the head of the humanoid robot is also divided as a subsystem, the movement target of the head can be realized, and the movement range of the head can be kept wide.

  FIG. 20 is a diagram illustrating an example of dividing a robot 200 into subsystems to which a robot control method according to another embodiment of the present invention is applied. The robot 200 includes a subsystem A 201, a subsystem B 202, and a subsystem C 203. The subsystem A201 includes a link 220 and a connecting portion 210, the subsystem B202 includes a link 221 and a connecting portion 211, and the subsystem C203 includes a link 222 and a connecting portion 212. The connecting portion 213 causes a horizontal surface 230 such as a floor surface to be formed. It is fixed. The representative points of these subsystems are connecting portions 210, 211, and 212, respectively. In this example, one link is represented on behalf of a plurality of links.

  These subsystems form a hierarchical structure with the subsystem C203 as the lowest layer. For example, in the case of a humanoid robot, subsystem A201 corresponds to arm 32, subsystem B202 corresponds to torso 33, and subsystem C203 corresponds to leg 34. The representative points are the hand 41, shoulder 43, and waist. 45.

  The reference position, which is the position to move the representative point, is “a position where evaluation is improved according to the work target”. For example, in the case of a humanoid robot, when the motion target to be realized by the leg 34 corresponding to the subsystem C203 is “to balance the whole body”, the waist 45 corresponding to the connecting portion 212 which is a representative point is selected. As for the reference position, when the entire center of gravity moves forward, the position behind the current position of the waist 45 is set as the reference position of the waist 45 so that the position of the waist 45 is pulled backward. At this time, the reference position of the representative point of each subsystem is calculated independently.

  These subsystems have a connection part to each other, but each reference position is calculated independently. Therefore, the constraint condition of each subsystem, specifically, the position of the representative point of the adjacent subsystem, the link In some cases, it cannot be realized at the reference position calculated by the mutual connecting part, depending on the constraints such as the structure of the movement of the representative point, the permitted movement range of the representative point, and the avoidance of interference with surroundings.

  For example, in the case of a humanoid robot, when the hand 41 corresponding to the connecting portion 210 that is a representative point is about to move forward, the body portion 33 corresponding to the subsystem B 202 can expand the operating range of the hand 41. In this way, the reference position of the shoulder 43 corresponding to the connecting portion 211 that is the representative point is changed in the direction in which the hand 41 moves. In order to maintain the overall balance, the leg 34 corresponding to the subsystem C203 returns the entire center of gravity moving forward to the reference position 213, and the leg 34 corresponding to the connecting portion 212, which is a representative point. 45 reference position is changed backward.

  At this time, since maintaining the overall balance is the most important movement target of the robot, first, it is determined whether or not the reference position of the waist 45 corresponding to the connecting portion 212 which is a representative point is realizable. When the reference position of the waist 45 is within the movement permission range of the waist 45, the reference position of the waist 45 can be realized, so the reference position of the waist 45 is set as the target position of the waist 45.

  Next, it is determined whether or not the reference position of the shoulder 43 corresponding to the connecting portion 211 that is a representative point is realizable. When the reference position of the shoulder 43 is within the movement permission range of the shoulder 43, the reference position of the shoulder 43 can be realized, and therefore the reference position of the shoulder 43 is set as the target position of the shoulder 43. When the reference position of the waist 45 or the reference position of the shoulder 43 is not within the respective movement permission ranges, the position within the movement permission range closest to the respective reference positions is set as the target position.

  In this way, the target position of the representative point is determined in consideration of the constraint condition of the subsystem that is one level higher than the representative point of the subordinate subsystem in the hierarchical structure. When the target positions of all the representative points are determined, the inverse motion calculation is performed, and the value of each axis of the subsystem is calculated.

  FIG. 21 is a flowchart showing steps of a robot control method according to another embodiment of the present invention. This processing is started when a work target is given from a remote control device that is an input device such as a joystick. In step S70, the instructed work target is acquired. In step S71, a position that matches a condition that satisfies the operation target set for each subsystem is calculated as each reference position.

  In step S72, based on the calculated reference position, the target position of the representative point of each subsystem is calculated according to the priority of the operation target set to achieve the work target. In step S73, the value of each axis of the robot is calculated based on the calculated target position. In step S74, the calculated value of each axis is instructed to each axis of the robot, and the process ends.

  FIG. 22 is a diagram showing an algorithm from the calculation of the reference position to the calculation of the value of each axis in the robot control method shown in FIG. In the remote operation instruction input step 250, when the work target given from the remote operation device or the like is acquired, the acquired work target is input to the subsystem A reference position calculation step 252. In the subsystem A reference position calculation step 252, a position that satisfies a condition that satisfies the operation target set in the subsystem A is calculated as the reference position of the representative point of the subsystem A based on the input work target.

  In each representative point current position step 251, the current position of the representative point of each subsystem was calculated last time, that is, the subsystem A target position calculation step 257, the subsystem B target position calculation step 256, and the subsystem C target. The target position that is the output of the position calculation step 255 is acquired, and the acquired current position of the representative point of each subsystem is input to the subsystem B reference position calculation step 253 and the subsystem C reference position calculation step 254. In the subsystem B reference position calculation step 253 and the subsystem C reference position calculation step 254, the conditions satisfying the operation target set for each subsystem are set based on the input current position of the representative point of each subsystem. The matching position is calculated as the reference position of the representative point of each subsystem. The calculations in the subsystem A reference position calculation step 252, the subsystem B reference position calculation step 253, and the subsystem C reference position calculation step 254 are performed in parallel.

  In the subsystem C target position calculation step 255, whether or not the reference position can be realized based on the reference position and the reference position output from the subsystem C reference position calculation step 254, that is, the reference position of the subsystem C is calculated. It is determined whether it is within the movement permission range of the representative point. When the reference position is realizable, that is, when the reference position is within the movement permission range, the reference position is set as the target position of the representative point of the subsystem C. When the reference position is not feasible, that is, when the reference position is not within the movement permission range, the position closest to the reference position in the movement permission range is set as the target position.

  In the subsystem B target position calculation step 25, a reference position can be realized based on the target position output from the subsystem C target position calculation step 255 and the reference position output from the subsystem B reference position calculation step 253. It is determined whether or not the reference position is within the movement permission range of the representative point of the subsystem B. When the reference position is realizable, that is, when the reference position is within the movement permission range, the reference position is set as the target position of the representative point of the subsystem B. When the reference position is not feasible, that is, when the reference position is not within the movement permission range, the position closest to the reference position in the movement permission range is set as the target position.

  In the subsystem A target position calculation step 257, the reference position can be realized based on the target position output from the subsystem B target position calculation step 256 and the reference position output from the subsystem A reference position calculation step 252. Whether or not the reference position is within the movement permission range of the representative point of subsystem A is determined. When the reference position is realizable, that is, when the reference position is within the movement permission range, the reference position is set as the target position of the representative point of the subsystem A. When the reference position is not feasible, that is, when the reference position is not within the movement permission range, the position closest to the reference position in the movement permission range is set as the target position. In this case, since the priority of the operation target set for each subsystem is the order of the subsystem C, the subsystem B, and the subsystem A, the target position is calculated in that order.

  In each subsystem command value calculation step 258, each subsystem target position output from the subsystem A target position calculation step 257, subsystem B target position calculation step 256, and subsystem C target position calculation step 255 is output. Based on this, the value of each axis of the robot is calculated. The calculated value of each axis is commanded to each axis of the robot.

  As described above, according to the embodiment of the present invention, by dividing the humanoid robot into subsystems, the calculation for the work target can be performed in parallel, and the repeated calculation for searching for the optimum value is performed. Since there is no such thing, the amount of calculation for controlling the robot can be reduced. It is possible to deal with various work targets by switching and combining reference position calculation algorithms prepared for each subsystem according to the work targets.

Any of the robot control methods described above is realized as a program that causes a computer to execute each robot control method. The computer includes a CPU (Central Processing Unit) (not shown) that performs various controls and a memory (not shown) used for processing performed by the CPU, such as a ROM (Read Only Memory) or a RAM (Random).
The program that includes the “Access Memory” and causes the computer to execute the robot control method is stored in this memory.

  The program for causing the computer to execute the robot control method only needs to be stored in a computer-readable recording medium. In the above-described embodiment, the memory is used as the recording medium, but the program reading device is used as an external storage device. May be provided and can be read by inserting the recording medium into the recording medium. The program stored in the recording medium may be any configuration that can be accessed and executed by the CPU. Alternatively, a program for causing a computer to execute the robot control method only needs to be stored in a storage device or the like of a computer connected to a network, and the program stored in the recording medium or the storage device is read and read. May be downloaded to a program recording area (not shown) and the program may be executed. This download program is stored in the ROM in advance.

  When the recording medium is configured to be separable from the computer, the recording medium may be a tape system such as a magnetic tape / cassette tape, a magnetic disk such as a flexible disk / hard disk, or a CD-ROM (Compact Disk-Read Only Memory) / MO ( Optical discs such as Magneto Optical Disk (MD) / MD (Mini Disk) / DVD (Digital Versatile Disk), IC (Integrated Circuit) cards (including memory cards) / optical cards, etc., or mask ROM / EPROM ( Erasable Programmable Read Only Memory) / semiconductor memory such as a flash ROM may be used as long as it is a recording medium that holds a fixed program.

  Although the above-described embodiment has been described with respect to a humanoid robot, it can be similarly applied to a robot having many degrees of freedom. In particular, for a robot with 20 degrees of freedom or more, the effect of shortening the calculation time is great. Since repeated calculation is not performed, the value of each axis of the robot can be obtained in a shorter time for a robot having 20 degrees of freedom or more than the calculation time according to the prior art. Even when applied to a humanoid robot with 30 degrees of freedom or more, the degree of freedom of the subsystem is set to 6 degrees of freedom or less, and the degree of freedom in the subsystem is small. It is possible to shorten the calculation time of the value of the robot, and furthermore, it is possible to obtain the value of each axis of the robot in a short time.

  In the above-described humanoid robot, the value of each axis is an angle. However, a robot having an axis that is displaced in a linear direction can be applied by using a displacement amount in the linear direction instead of the angle.

  FIG. 23 is a diagram showing an example of the configuration of another robot system 150 to which the present invention can be applied. The robot system 150 includes a gate structure moving device 151 and a 6-axis vertical articulated robot 152 mounted on the gate structure moving device 151. The gate structure moving device 151 can move along the rails 153 and 154. The 6-axis vertical articulated robot 152 can perform processing on the workpiece 160 that moves on the line 155 by attaching a tool for welding or the like to the wrist.

  The link of the gate structure moving device 151 is a movable portion 151a to a movable portion 151c, and the connecting portion is a connecting portion between the rails 153 and 154 and the moving portion a, a connecting portion between the movable portion 151a and the moving portion 151b, and a movable portion. A connecting portion between the portion 151b and the movable portion 151c, and a connecting portion between the movable portion 151c and the six-axis vertical articulated robot 152. The gate structure moving device 151 has four axes of a travel axis 170, a traverse axis 171, an elevating axis 172, and a turning axis 173, and the 6-axis vertical articulated robot 152 has 6 axes. Is a robot with 10 degrees of freedom. In this case, the sub-system is divided into two, that is, the gate-structure moving device 151 and the six-axis vertical articulated robot 152, that is, the sub-system is divided into two sub-systems having 4 degrees of freedom and 6 degrees of freedom.

  The work target for the robot system 150 is to move the tool to the machining position of the workpiece 160, for example. As an operation target, for example, “expand the work range of the hand position” and “avoid interference”, and the priorities are “avoid interference” and “expand the work range of the hand position”. By setting as an order, the present invention can be applied.

  Furthermore, the present invention can be applied to a robot that performs work on a space station or the like. In this case, the robot performs the work by holding the structure of the truss structure constituting the space station. In other words, the body is supported by the arm held by the truss and the work is performed by the other arm. This robot is a modification of a humanoid robot, has a high degree of freedom, and the present invention can be applied. In this case, the arm that supports the body is set as one subsystem, and the arm that performs the work is set as one subsystem. The work target is to move the hand of the arm performing the work to the work position. As the operation target, for example, “grasping a truss”, “avoid interference”, and “work range of the hand position” Set “Enlarge”. As for the priority, priority is given to “avoid interference”.

  The robot that performs work in the sea also performs the work while keeping the body of the robot floating in the sea unmoved, and the present invention can be applied. In this case, for example, “not to be swept away by the ocean current and to prevent the body from moving due to the reaction of the arm or the like” and “to expand the work range of the hand position” are set as the movement targets.

1 is a diagram illustrating a configuration of a robot system 10 to which a robot control method according to an embodiment of the present invention is applied. It is a flowchart which shows the process of the robot control method which is one Embodiment of this invention. It is a figure which shows an example of the division | segmentation into the subsystem of the robot 30 to which the robot control method shown in FIG. 2 is applied. It is a figure for demonstrating the freedom degree of each part of a humanoid robot. It is a figure which shows an example of the instruction | indication of the work target with respect to the robot 30 to which the robot control method shown in FIG. 2 is applied, and the displacement of the robot by the instruction | indication. It is a flowchart of the calculation of the reference position of the hand 41 by the robot control method shown in FIG. It is a flowchart of calculation of the reference position of the shoulder 43 by the robot control method shown in FIG. It is a figure for demonstrating calculation of the reference position of the shoulder 43 by the robot control method shown in FIG. It is a flowchart of calculation of the reference position of the waist 45 in the robot control method shown in FIG. It is a figure for demonstrating interlocking | moving the movement of the waist 45 with the movement of the shoulder 43. FIG. It is a figure for demonstrating how to obtain | require the gravity center position of a link. It is a figure for demonstrating the coordinate system of a robot. It is a figure for demonstrating the coordinate system of each axis | shaft at the time of calculating | requiring a position and orientation from the value of each axis | shaft of the leg part. It is a figure for demonstrating the relationship between the support polygon and the floor projection point. It is a flowchart of calculation of the target position of the waist 45 by the robot control method shown in FIG. It is a figure for demonstrating the movement permission range of a representative point. It is a flowchart of calculation of the target position of the shoulder 43 by the robot control method shown in FIG. It is a flowchart of calculation of the target position of the hand 41 by the robot control method shown in FIG. It is a figure which shows the algorithm from the calculation of the reference position by the robot control method shown in FIG. 2 to the calculation of the value of each axis. It is a figure which shows an example of the division | segmentation into the subsystem of the robot 200 with which the robot control method which is another form of implementation of this invention is applied.

It is a flowchart which shows the process of the robot control method which is the other form of implementation of this invention. It is a figure which shows the algorithm from the calculation of the reference position to the calculation of the value of each axis in the robot control method shown in FIG. It is a figure which shows an example of a structure of the other robot system 150 which can apply this invention. It is a figure which shows the schematic structure of a humanoid robot. It is a figure for demonstrating the method of calculating | requiring the value of each axis | shaft of a robot using an evaluation function.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1,30 Robot 2,31 Head part 3,32 Arm part 4,33 Body part 5,34 Leg part 6 Hand part 10 Robot system 11 Robot part 12,22 Control part 13,23 Communication part 14 Imaging part 20 Remote control device DESCRIPTION OF SYMBOLS 21 Input part 24 Display part 41 Hand 42 Elbow 43 Shoulder 44 Chest 45 Waist 46 Knee 47 Leg 51-56 Link 150 Robot system 151 Portal structure moving device 152 6-axis vertical articulated robot 153, 154 Rail 155 Line 160 Work 170 Traveling axis 171 Traverse axis 172 Elevating axis 173 Turning axis

Claims (14)

1 for achieving a work target indicating an operation to be performed by the robot for each of a plurality of control target areas obtained by dividing a robot constituted by a plurality of links and a plurality of connecting portions that connect the links to each other as a control target. A setting step for presetting the priority indicating the order of calculating the target position for each control target region for achieving the work target in advance as the priority of the operation condition;
When work target information representing a work target is instructed, a representative point that is a part for which a movement destination position of the control target area is calculated among the parts included in the control target area for each control target area is A reference position calculation step of calculating a position that matches an operation condition as a reference position that is a candidate for a destination position of the representative point;
Based on the reference position calculated for each representative point in the reference position calculating step, a target position calculating step for calculating a target position to move each representative point according to the priority of the operation condition;
Each axis value calculation step for calculating the value of each axis of the robot based on the target position calculated in the target position calculation step;
And a commanding step of commanding each axis of the robot with the value of each axis calculated in each axis value calculating step. In the target position calculating step, when the reference position calculated for each representative point in the reference position calculating step is within a movement permission range that is a predetermined range within a movable range in which each representative point can move. Calculates the reference position of each representative point as the target position of each representative point,
When the reference position calculated for each representative point in the reference position calculation step is not within the movement permission range of the corresponding representative point, it is the position within the movement permission range and is the closest to the reference position of each control target area. The robot control method according to claim 1, wherein a position that satisfies a condition within a close movement permission range is calculated as a target position of each representative point.   In the target position calculation step, when the representative point of the control target region adjacent to the control target region for which the target position of the representative point is to be calculated has already been calculated, the representative point of the adjacent control target region that has already been calculated The robot control method according to claim 1, wherein the target position of the representative point of the control target region is calculated with reference to the target position.   The ratio obtained by dividing the movement permission range of each representative point by the corresponding movable range is smaller in the control target area in which the high-priority operation condition is set than in the control target area in which the low-priority operation condition is set. The robot control method according to claim 2, wherein: Wherein the reference position calculation step, the robot control method according to any one of claims 1-4, characterized in that to calculate in parallel a reference position of the representative point of the control target each area. In each axis value computation process, a robot control method according to any one of claims 1-5, characterized in that to calculate parallel values of each axis of the control target region to the control target each area . The robot is a robot control method according to any one of claims 1-6, characterized in that it has a 20 degrees of freedom or more degrees of freedom. The degree of freedom of the control target region, the robot control method according to any one of claims 1-7, characterized in that at most six degrees of freedom. The robot is a humanoid robot including an arm portion corresponding to a human arm, a torso portion corresponding to a human torso, and a leg portion corresponding to a human leg, and the arm portion, the torso portion, and the leg the robot control method according to any one of claims 1-8, characterized in that a respective control region of the part. In the humanoid robot, the representative point of the arm part is the hand, the representative point of the torso part is the shoulder, and the representative point of the leg part is the hip, and the movement permission range of the representative point of the control target area is movable 10. The robot according to claim 9 , wherein the ratio of the representative point of the waist is smaller than the ratio of the representative point of the shoulder, and the ratio of the representative point of the shoulder is smaller than the ratio of the representative point of the hand. Control method. 11. The robot control method according to claim 10 , wherein when calculating the reference position of the waist in the reference position calculation step, the speed of the waist is calculated to be a speed proportional to the speed of the shoulder. A computer configured to execute the robot control method according to any one of claims 1 to 11 is mounted on a robot including a plurality of links and a plurality of connecting portions that connect the links to each other. Robot system. The robot has detection means for determining work target information, generates work target information based on information detected by the detection means, and uses the generated work target information as work target information of the robot. The robot system according to claim 12 , wherein: A dividing step of dividing a robot constituted by a plurality of links and a plurality of connecting portions that connect the links to each other into a plurality of control target areas to be controlled;
For each of the control target areas, a determination step of determining a representative point that is a part in which a movement destination position of the control target area is calculated among the parts included in the control target area;
For each of a plurality of control target areas divided in the dividing step, one operation condition for achieving a work target indicating the action to be performed by the robot is set in advance, and the control target area for achieving the work target A setting step for presetting the priority indicating the order of calculating each target position as the priority of the operation condition;
When work target information representing a work target is instructed, a position where the representative point matches the operation condition is calculated as a reference position that is a candidate for a destination position of the representative point for each control target region. A reference position calculation step;
Based on the reference position calculated for each representative point in the reference position calculating step, a target position calculating step for calculating a target position to move each representative point according to the priority of the operation condition;
Each axis value calculation step for calculating the value of each axis of the robot based on the target position calculated in the target position calculation step;
A robot command determination method comprising: a command step of commanding each axis value of each robot calculated in each axis value calculation step.

Images