JP2014124734A - Robot, and motion trajectory control system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot and a motion trajectory control system, which are capable of preventing an arm from shielding a target article when the target article worked on by a robot body is imaged by an electronic camera.SOLUTION: A robot 1 includes: a robot body 10 having an arm; an electronic camera for imaging a surrounding of a reference point set on a target article worked on by the robot body 10 and obtaining imaged data; and a control device for controlling an operation of the robot body 10. The control device includes: a virtual repulsive force vector field setup part for setting a virtual repulsive force vector field constituted by a virtual repulsive force vector having a line connecting between the electronic camera and the reference point as a virtual repulsive force generating origin, on the basis of the image data; a virtual repulsive force data obtainment part; a first torque data obtainment part; a virtual attractive force vector field setup part for setting a virtual attractive force vector field constituted by a virtual attractive force vector having the reference point as a virtual attractive force generating origin; a virtual attractive force data obtainment part; a second torque data obtainment part; and a control part.

Description

  The present invention relates to a robot and an operation trajectory control system.

A multi-axis robot is known that includes a plurality of pivotable arms and performs work freely in a three-dimensional space. When the robot performs work, the periphery of the object on which the robot works is imaged with an electronic camera, and the position of the object is confirmed based on the obtained image data.
Further, when the robot performs work, it is necessary to generate a trajectory of the arm. In Patent Document 1, a virtual repulsive force vector field configured by a virtual repulsive force vector having an obstacle as a virtual repulsive force generation source is set. A robot that generates an arm trajectory using the virtual repulsive force vector field is disclosed.

JP 2011-31309 A

However, the robot described in Patent Document 1 has a problem that when an object is imaged with an electronic camera, the arm may block the object, and in this case, the object cannot be imaged.
An object of the present invention is to provide a robot and an operation trajectory control system that can prevent an arm from blocking an object when an object to be worked by a robot body is imaged by an electronic camera.

Such an object is achieved by the following application examples.
(Application example 1)
The robot of this application example includes an arm driven by a drive source,
An imaging device that captures imaging data by imaging the object on which the arm works and the periphery of the object;
A control device for controlling the arm,
The control device includes:
A virtual repulsive force vector field setting unit for setting a virtual repulsive force vector field including a virtual repulsive force vector having a straight line connecting the reference point of the object set in advance and the image capturing device as a source, from the imaging data;
A virtual repulsive force data acquisition unit for setting a first representative point on the arm and obtaining a virtual repulsive force acting on the first representative point in the virtual repulsive force vector field;
Based on the virtual repulsive force obtained by the virtual repulsive force data obtaining unit, a first torque data obtaining unit for obtaining a first torque of the drive source generated by the virtual repulsive force and acting on the arm;
A virtual attractive force vector field setting unit for setting a virtual attractive force vector field including a virtual attractive force vector having a reference point of the object as a generation source;
A virtual attraction data acquisition unit for setting a second representative point on the arm and obtaining a virtual attraction acting on the second representative point in the virtual attraction vector field;
A second torque data obtaining unit for obtaining a second torque of the drive source generated by the virtual attractive force and acting on the arm based on the virtual attractive force obtained by the virtual attractive force data obtaining unit;
And a control unit that controls the operation of the arm based on the first torque and the second torque.
As a result, the arm operates avoiding a straight line connecting the electronic camera and the reference point, so that when the robot main body captures an object to be worked by the electronic camera, the arm can be prevented from blocking the object. An object can be reliably imaged.

(Application example 2)
In the robot of this application example, it is preferable that the magnitude of the virtual repulsive force vector in the virtual repulsive force vector field is inversely proportional to the distance from the reference point of the object.
Thereby, the operation of the arm can be controlled more reliably.

(Application example 3)
In the robot of this application example, the arm rotates around the rotation axis,
A drive data acquisition unit that obtains the rotation angle, angular velocity, and angular acceleration of the arm based on the first torque, the second torque, and the moment of inertia of the arm around the rotation axis;
It is preferable that the control unit controls the operation of the arm so that the arm rotates the rotation angle at the angular velocity and the angular acceleration.
As a result, the closer to the object, the farther the arm can be from the straight line connecting the electronic camera and the reference point, and the more reliable that the arm blocks the object when the robot body images the object to be worked on by the electronic camera. Can be prevented.

(Application example 4)
The robot of this application example includes an arm driven by a drive source,
An imaging device for imaging the object on which the arm works and the periphery of the object;
A control device for controlling the arm,
The control device includes:
A first virtual repulsive force vector field setting unit that sets a first repulsive force vector field including a virtual repulsive force vector having a straight line connecting the set reference point and the image capturing device as a virtual repulsive force generation source from the image data of the image capturing device. When,
A first virtual repulsive force data acquisition unit for setting a first representative point on the arm and obtaining a virtual repulsive force acting on the first representative point in the first virtual repulsive force vector field;
Based on the virtual repulsive force obtained by the first virtual repulsive force data obtaining unit, a first torque data obtaining unit for obtaining a first torque of the drive source generated by the virtual repulsive force and acting on the arm;
A virtual attractive force vector field setting unit for setting a virtual attractive force vector field including a virtual attractive force vector having a reference point of the object as a generation source;
A virtual attraction data acquisition unit for setting a second representative point on the arm and obtaining a virtual attraction acting on the second representative point in the virtual attraction vector field;
A second torque data obtaining unit for obtaining a second torque of the drive source generated by the virtual attractive force and acting on the arm based on the virtual attractive force obtained by the virtual attractive force data obtaining unit;
A second virtual repulsive force vector field setting unit for setting a second repulsive force vector field including a virtual repulsive force vector having an obstacle as a generation source;
A second virtual repulsive force data acquisition unit for setting a third representative point on the arm and obtaining a virtual repulsive force acting on the third representative point in the second virtual repulsive force vector field;
A third torque data obtaining unit for obtaining a third torque of the drive source generated by the virtual repulsive force and acting on the arm based on the virtual repulsive force obtained by the second virtual repulsive force data obtaining unit;
And a control unit that controls the operation of the arm based on the first torque, the second torque, and the third torque.
As a result, the arm rotates while avoiding the straight line connecting the electronic camera and the reference point, so that when the robot body images the object to be worked by the electronic camera, the arm can be prevented from blocking the object. The object can be reliably imaged. Further, the arm can be rotated while avoiding an obstacle.

(Application example 5)
In the robot of this application example, the imaging device captures an image including the obstacle,
It is preferable that the second virtual repulsive force vector field setting unit recognizes the obstacle based on imaging data obtained by imaging with the imaging device and sets the second virtual repulsive force vector field.
Thereby, the operation of the arm can be controlled more reliably.

(Application example 6)
In the robot according to this application example, it is preferable that the magnitude of the virtual repulsive force vector of the first virtual repulsive force vector field is inversely proportional to the distance from the reference point of the object.
Thereby, compared with the case where the electronic camera which images an obstruction separately is provided, the number of components can be reduced and cost can be reduced.

(Application example 7)
In the robot of this application example, the arm rotates around the rotation axis,
A drive data acquisition unit that obtains the rotation angle, angular velocity, and angular acceleration of the arm based on the first torque, the second torque, the third torque, and the moment of inertia of the arm around the rotation axis; ,
It is preferable that the control unit controls the operation of the arm so that the arm rotates the rotation angle at the angular velocity and the angular acceleration.
As a result, the closer to the object, the farther the arm can be from the straight line connecting the electronic camera and the reference point, and the more reliable that the arm blocks the object when the robot body images the object to be worked on by the electronic camera. Can be prevented.

(Application example 8)
In the robot according to this application example, it is preferable that the imaging device is installed at a position separated from the arm.
Thereby, compared with the case where the electronic camera is installed in the robot main body, when the target object on which the robot main body works is imaged by the electronic camera, it is possible to more reliably prevent the arm from blocking the target object.

(Application example 9)
The motion trajectory control system of this application example includes an arm driven by a drive source,
An imaging device for imaging the object on which the arm works and the periphery of the object;
A control device for controlling the arm,
The control device includes:
A virtual repulsive force vector field setting unit that sets a virtual repulsive force vector field including a virtual repulsive force vector having a straight line connecting the reference point of the object and the image capturing device as a source, from the imaging data of the image capturing device;
A virtual repulsive force data acquisition unit for setting a first representative point on the arm and obtaining a virtual repulsive force acting on the first representative point in the virtual repulsive force vector field;
Based on the virtual repulsive force obtained by the virtual repulsive force data obtaining unit, a first torque data obtaining unit for obtaining a first torque of the drive source generated by the virtual repulsive force and acting on the arm;
A virtual attractive force vector field setting unit for setting a virtual attractive force vector field including a virtual attractive force vector having a reference point of the object as a generation source;
A virtual attraction data acquisition unit for setting a second representative point on the arm and obtaining a virtual attraction acting on the second representative point in the virtual attraction vector field;
A second torque data obtaining unit for obtaining a second torque of the drive source generated by the virtual attractive force and acting on the arm based on the virtual attractive force obtained by the virtual attractive force data obtaining unit;
And a control unit that controls the operation of the arm based on the first torque and the second torque.
As a result, the arm rotates while avoiding the straight line connecting the electronic camera and the reference point, so that when the robot body images the object to be worked by the electronic camera, the arm can be prevented from blocking the object. The object can be reliably imaged.

(Application Example 10)
In the motion trajectory control system of this application example, the arm rotates around the rotation axis,
A drive data acquisition unit that obtains the rotation angle, angular velocity, and angular acceleration of the arm based on the first torque, the second torque, and the moment of inertia of the arm around the rotation axis;
It is preferable that the control unit controls the operation of the arm so that the arm rotates the rotation angle at the angular velocity and the angular acceleration.
Thereby, the operation of the arm can be controlled more reliably.

(Application Example 11)
The motion trajectory control system of this application example includes an arm driven by a drive source,
An imaging device for imaging the object on which the arm works and the periphery of the object;
A control device for controlling the arm,
The control device includes:
A first virtual repulsive force vector field setting unit that sets a first repulsive force vector field including a virtual repulsive force vector having a straight line connecting the set reference point and the image capturing device as a virtual repulsive force generation source from the image data of the image capturing device. When,
A first virtual repulsive force data acquisition unit for setting a first representative point on the arm and obtaining a virtual repulsive force acting on the first representative point in the first virtual repulsive force vector field;
Based on the virtual repulsive force obtained by the first virtual repulsive force data obtaining unit, a first torque data obtaining unit for obtaining a first torque of the drive source generated by the virtual repulsive force and acting on the arm;
A virtual attractive force vector field setting unit for setting a virtual attractive force vector field including a virtual attractive force vector having a reference point of the object as a generation source;
A virtual attraction data acquisition unit for setting a second representative point on the arm and obtaining a virtual attraction acting on the second representative point in the virtual attraction vector field;
A second torque data obtaining unit for obtaining a second torque of the drive source generated by the virtual attractive force and acting on the arm based on the virtual attractive force obtained by the virtual attractive force data obtaining unit;
A second virtual repulsive force vector field setting unit for setting a second repulsive force vector field including a virtual repulsive force vector having an obstacle as a generation source;
A second virtual repulsive force data acquisition unit for setting a third representative point on the arm and obtaining a virtual repulsive force acting on the third representative point in the second virtual repulsive force vector field;
A third torque data obtaining unit for obtaining a third torque of the drive source generated by the virtual repulsive force and acting on the arm based on the virtual repulsive force obtained by the second virtual repulsive force data obtaining unit;
And a control unit that controls the operation of the arm based on the first torque, the second torque, and the third torque.
As a result, the arm rotates while avoiding the straight line connecting the electronic camera and the reference point, so that when the robot body images the object to be worked by the electronic camera, the arm can be prevented from blocking the object. The object can be reliably imaged. Further, the arm can be rotated while avoiding an obstacle.

(Application Example 12)
In the motion trajectory control system of this application example, the arm rotates around the rotation axis,
A drive data acquisition unit for respectively determining the rotation angle, angular velocity, and angular acceleration of the arm based on the first torque, the second torque, the third torque, and the moment of inertia of the arm around the rotation axis; And
It is preferable that the control unit controls the operation of the arm so that the arm rotates the rotation angle at the angular velocity and the angular acceleration.
Thereby, the operation of the arm can be controlled more reliably.

It is the perspective view which looked at 1st Embodiment of the robot concerning this invention from the front side. It is the perspective view which looked at the robot shown in FIG. 1 from the back side. It is the schematic of the robot shown in FIG. It is a block diagram of the principal part of the robot shown in FIG. It is a figure for demonstrating the action | operation of the robot shown in FIG. It is a figure for demonstrating the action | operation of the robot shown in FIG. It is a flowchart which shows the control action of the robot shown in FIG. It is a block diagram of the principal part of 2nd Embodiment of the robot concerning this invention. It is a figure for demonstrating the action | operation of the robot shown in FIG. It is a flowchart which shows the control action of the robot shown in FIG. It is a perspective view which shows the other structural example of the robot concerning this invention.

Hereinafter, a robot and a motion trajectory control system according to the present invention will be described in detail based on preferred embodiments shown in the accompanying drawings.
<First Embodiment>
FIG. 1 is a perspective view of a robot according to a first embodiment of the present invention as viewed from the front side. FIG. 2 is a perspective view of the robot shown in FIG. 1 viewed from the back side. FIG. 3 is a schematic diagram of the robot shown in FIG. FIG. 4 is a block diagram of the main part of the robot shown in FIG. 5 and 6 are diagrams for explaining the operation of the robot shown in FIG. FIG. 7 is a flowchart showing a control operation of the robot shown in FIG. FIG. 6 schematically shows only the first arm of the robot.
In the following, for convenience of explanation, the upper side in FIGS. 1 to 3 is referred to as “upper” or “upper”, and the lower side is referred to as “lower” or “lower”. Moreover, the base side in FIGS. 1-3 is called "base end", and the opposite side is called "tip".

  The robot (industrial robot) 1 shown in FIGS. 1 to 3 can be used in a manufacturing process for manufacturing precision equipment such as a wristwatch, for example, and includes a robot body 10 and a control device that controls the operation of the robot body 10. (Control means) 20 (see FIG. 4), a first electronic camera 71 and a second electronic camera 72 are provided. The robot body 10, the first electronic camera 71, the second electronic camera 72, and the control device 20 are electrically connected to each other. The control device 20, the first electronic camera 71, and the second electronic camera 72 constitute an operation trajectory control system. Further, the control device 20 can be configured by, for example, a personal computer (PC) with a built-in CPU (Central Processing Unit). The control device 20 will be described in detail later.

  The robot body 10 includes a base 11, four arms (links) 12, 13, 14, 15, a list (link) 16, and six drive sources 401, 402, 403, 404, 405, 406. I have. The robot body 10 is a vertical articulated (6-axis) robot (robot body) in which a base 11, arms 12, 13, 14, 15 and a wrist 16 are connected in this order from the base end side to the tip end side. ). In the vertical articulated robot, the base 11, the arms 12 to 15 and the wrist 16 can be collectively referred to as “arm”. The arm 12 is “first arm” and the arm 13 is “second arm”. The arm 14 can be divided into “third arm”, the arm 15 can be divided into “fourth arm”, and the wrist 16 can be divided into “fifth arm, sixth arm”. The list 16 may have a fifth arm and a sixth arm. An end effector or the like can be attached to the wrist 16.

  As shown in FIG. 3, the arms 12 to 15 and the wrist 16 are supported so as to be independently displaceable with respect to the base 11. The lengths of the arms 12 to 15 and the wrist 16 are not particularly limited, but in the illustrated configuration, the lengths of the arms 12 to 14 are set longer than those of the other arms 15 and the wrist 16. For example, the length of the third arm 14 may be shorter than the lengths of the first arm 12 and the second arm 13.

  The base 11 and the first arm 12 are connected via a joint 171. The first arm 12 is rotatable about the first rotation axis O1 with respect to the base 11 with the first rotation axis O1 parallel to the vertical direction as the center of rotation. The first rotation axis O <b> 1 coincides with the normal line of the upper surface of the floor 101 that is the installation surface of the base 11. The rotation about the first rotation axis O <b> 1 is performed by driving the first drive source 401. The first drive source 401 is driven by a motor 401M and a cable (not shown), and the motor 401M is controlled by the control device 20 via an electrically connected motor driver 301. The first drive source 401 may receive drive from the motor 401M by a speed reducer (not shown) provided together with the motor 401M, and the speed reducer may be omitted.

  The first arm 12 and the second arm 13 are connected via a joint (joint) 172. The second arm 13 is rotatable with respect to the first arm 12 about the second rotation axis O2 parallel to the horizontal direction. The second rotation axis O2 is orthogonal to the first rotation axis O1. The rotation around the second rotation axis O <b> 2 is performed by driving the second drive source 402. The second drive source 402 is driven by a motor 402M and a cable (not shown), and the motor 402M is controlled by the control device 20 via a motor driver 302 that is electrically connected. The second drive source 402 may receive drive from the motor 402M by a speed reducer (not shown) provided in addition to the motor 402M, and the speed reducer may be omitted. The second rotation axis O2 may be parallel to an axis orthogonal to the first rotation axis O1.

  The second arm 13 and the third arm 14 are connected via a joint 173. The third arm 14 is rotatable about a third rotation axis O3 about a rotation axis O3 parallel to the horizontal direction with respect to the second arm 13. The third rotation axis O3 is parallel to the second rotation axis O2. The rotation about the third rotation axis O <b> 3 is performed by driving the third drive source 403. The third drive source 403 is driven by a motor 403M and a cable (not shown), and the motor 403M is controlled by the control device 20 via an electrically connected motor driver 303. The third drive source 403 may be provided with a speed reducer (not shown) in addition to the motor 403M to transmit the drive from the motor 403M, or the speed reducer may be omitted.

  The third arm 14 and the fourth arm 15 are connected via a joint (joint) 174. Then, the fourth arm 15 rotates about a fourth rotation axis O4 with respect to the third arm 14 (base 11), with a fourth rotation axis O4 parallel to the central axis direction of the third arm 14 as a rotation center. It is free to move. The fourth rotation axis O4 is orthogonal to the third rotation axis O3. The rotation about the fourth rotation axis O4 is performed by driving the fourth drive source 404. The fourth drive source 404 is driven by a motor 404M and a cable (not shown), and the motor 404M is controlled by the control device 20 via an electrically connected motor driver 304. The fourth drive source 404 may receive drive from the motor 404M by a speed reducer (not shown) provided together with the motor 404M, and the speed reducer may be omitted. The fourth rotation axis O4 may be parallel to an axis orthogonal to the third rotation axis O3.

  The fourth arm 15 and the wrist 16 are connected via a joint 175. The wrist 16 has a fifth rotation axis O5 parallel to the horizontal direction (y-axis direction) with respect to the fourth arm 15, and is rotatable about the fifth rotation axis O5. The fifth rotation axis O5 is orthogonal to the fourth rotation axis O4. The rotation about the fifth rotation axis O5 is performed by driving the fifth drive source 405. The fifth drive source 405 is driven by a motor 405M and a cable (not shown), and the motor 405M is controlled by the control device 20 via a motor driver 305 that is electrically connected. The fifth drive source 405 may receive drive from the motor 405M by a speed reducer (not shown) provided together with the motor 405M, and the speed reducer may be omitted. Further, the wrist 16 is rotatable about a sixth rotation axis O6 with a sixth rotation axis O6 perpendicular to the fifth rotation axis O5 as a rotation center via a joint (joint) 176. The rotation axis O6 is orthogonal to the rotation axis O5. The rotation about the sixth rotation axis O6 is performed by driving the sixth drive source 406. The driving of the sixth drive source 406 is driven by a motor 406M and a cable (not shown), and the motor 406M is controlled by the control device 20 through an electrically connected motor driver 306. The sixth drive source 406 may be provided with a speed reducer (not shown) in addition to the motor 406M to transmit the drive from the motor 406M, or the speed reducer may be omitted. The fifth rotation axis O5 may be parallel to the axis orthogonal to the fourth rotation axis O4, and the sixth rotation axis O6 may be parallel to the axis orthogonal to the fifth rotation axis O5. Good.

  The driving sources 401 to 406 include a first angle sensor 411, a second angle sensor 412, a third angle sensor 413, a fourth angle sensor 414, a fifth angle sensor 415, and a sixth angle sensor. 416 is provided. An encoder, a rotary encoder, etc. can be used as these angle sensors. These angle sensors 411 to 416 detect the rotation angles of the rotation shafts of the motors of the drive sources 401 to 406 or the reduction gears, respectively. The motors of the drive sources 401 to 406 are not particularly limited, and for example, a servo motor such as an AC servo motor or a DC servo motor is preferably used. Each cable may be inserted through the robot body 10.

The robot body 10 is electrically connected to the control device 20. That is, the drive sources 401 to 406 and the angle sensors 411 to 416 are electrically connected to the control device 20, respectively.
The control device 20 can operate the arms 12 to 15 and the wrist 16 independently. That is, the control device 20 can independently control the drive sources 401 to 406 via the motor drivers 301 to 306, respectively. it can. In this case, the control device 20 performs detection using the angle sensors 411 to 416, and controls driving of the drive sources 401 to 406, for example, angular acceleration, angular velocity, rotation angle, and the like based on the detection results. This control program is stored in advance in a recording medium built in the control device 20.

  As shown in FIGS. 1 and 2, when the robot 1 is a vertical articulated robot, the base 11 is a portion that is positioned at the lowest position of the vertical articulated robot and is fixed to the floor 101 of the installation space. The fixing method is not particularly limited. For example, in the present embodiment shown in FIGS. 1 and 2, a fixing method using a plurality of bolts 111 is used. In addition, as a fixed location in the installation space of the base 11, it can also be set as the wall and ceiling of an installation space other than a floor.

The base 11 has a hollow base body (housing) 112. The base body 112 can be divided into a cylindrical portion 113 having a cylindrical shape and a box-shaped portion 114 having a box shape formed integrally with the outer peripheral portion of the cylindrical portion 113. In such a base body 112, for example, a motor 401M and motor drivers 301 to 306 are accommodated.
Each of the arms 12 to 15 has a hollow arm body 2, a drive mechanism 3, and a sealing means 4. In the following, for convenience of explanation, the arm body 2, the drive mechanism 3, and the sealing means 4 of the first arm 12 are referred to as "arm body 2a", "drive mechanism 3a", and "sealing means 4a", respectively. The arm main body 2, the drive mechanism 3, and the sealing means 4 included in the second arm 13 are referred to as “arm main body 2 b”, “drive mechanism 3 b”, and “sealing means 4 b”, respectively, and the arm main body included in the third arm 14. 2, the drive mechanism 3 and the sealing means 4 are referred to as “arm body 2c”, “drive mechanism 3c”, and “sealing means 4c”, respectively, and the arm body 2, drive mechanism 3, and sealing means that the fourth arm 15 has. 4 may be referred to as “arm body 2d”, “drive mechanism 3d”, and “sealing means 4d”, respectively.

  Each of the joints 171 to 176 has a rotation support mechanism (not shown). This rotation support mechanism is a mechanism that supports one of the two arms connected to each other so as to be rotatable relative to the other, and one of the base 11 and the first arm 12 connected to each other is the other. It is a mechanism that rotatably supports the mechanism, and a mechanism that rotatably supports one of the fourth arm 15 and the fifth wrist 16 connected to each other. When the fourth arm 15 and the wrist 16 that are connected to each other are taken as an example, the rotation support mechanism can rotate the wrist 16 with respect to the fourth arm 15. Each rotation support mechanism decelerates the rotation speed of the corresponding motor by a predetermined reduction ratio, and transmits the driving force to the corresponding arm, the wrist body 161 of the wrist 16, and the support ring 162 ( (Not shown).

The first arm 12 is connected to the upper end portion (tip portion) of the base 11 in a posture inclined with respect to the horizontal direction. In the first arm 12, the drive mechanism 3a has a motor 402M and is housed in the arm body 2a. The arm body 2a is hermetically sealed by the sealing means 4a.
The second arm 13 is connected to the tip of the first arm 12. In the second arm 13, the drive mechanism 3b has a motor 403M and is housed in the arm body 2b. The arm body 2a is hermetically sealed by a sealing means 4b.
The third arm 14 is connected to the tip of the second arm 13. In the third arm 14, the drive mechanism 3c has a motor 404M and is housed in the arm body 2c. Further, the inside of the arm main body 2c is hermetically sealed by the sealing means 4c.

The fourth arm 15 is connected to the tip of the third arm 14 in parallel with the central axis direction. In this arm 15, the drive mechanism 3d has motors 405M and 406M and is housed in the arm main body 2d. The arm body 2d is hermetically sealed by the sealing means 4d.
A wrist 16 is connected to the tip of the fourth arm 15 (the end opposite to the base 11). For example, a manipulator (not shown) that holds a precision device such as a wristwatch is detachably attached to the tip 16 (the end opposite to the fourth arm 15). In addition, it does not specifically limit as a manipulator, For example, the thing of the structure which has a several finger part (finger) is mentioned. And this robot 1 can convey the said precision apparatus by controlling operation | movement of arms 12-15, wrist | wrist 16, etc., holding a precision apparatus with a manipulator. The precision device is an example of an object on which the robot body 10 of the robot 1 operates.

The wrist 16 includes a wrist body (sixth arm) 161 having a cylindrical shape, and a wrist body 161 separately from the wrist body 161. The wrist 16 is provided at the base end of the wrist body 161 and has a ring-shaped support ring (fifth arm). 162).
The front end surface 163 of the wrist body 161 is a flat surface and is a mounting surface on which the manipulator is mounted. The wrist body 161 is connected to the drive mechanism 3d of the fourth arm 15 via the joint 176, and rotates around the rotation axis O6 by driving the motor 406M of the drive mechanism 3d.
The support ring 162 is connected to the drive mechanism 3d of the fourth arm 15 via the joint 175, and rotates around the rotation axis O5 together with the wrist body 161 by driving of the motor 405M of the drive mechanism 3d.

Each of the first electronic camera 71 and the second electronic camera 72 has an image sensor, and images the periphery of a reference point, which will be described later, which is set as an object on which the robot body 10 of the robot 1 operates, that is, an object. Device. Image data obtained by imaging with the first electronic camera 71 and the second electronic camera 72 are respectively input to the control device 20. The control device 20 obtains the position of the object, that is, the position (coordinates) of the reference point, based on the image data. By providing two electronic cameras, the position of the reference point can be obtained reliably.
Further, the first electronic camera 71 and the second electronic camera 72 are respectively installed at positions separated from the robot body 10 by a predetermined distance. Thereby, even if the robot main body 10 operates, the first electronic camera 71 and the second electronic camera 72 are always in a fixed position, and the position of the reference point can be easily obtained.

Next, the configuration of the control device 20 will be described with reference to FIGS. 1, 2, and 4.
The control device 20 is a device that controls the operations of the first drive source 401, the second drive source 402, the third drive source 403, the fourth drive source 404, the fifth drive source 405, and the sixth drive source 406, respectively. The control device 20 includes a vector field setting unit 61, a trajectory generation unit 62, an angular velocity control unit 63, and a drive source control unit 64. The angular velocity control unit 63 and the drive source control unit 64 constitute a control unit that controls the operations of the drive sources 401 to 406, that is, the operations of the arms 12 to 15 and the list 16.

The vector field setting unit 61 includes a virtual repulsive force vector field setting unit 611 and a virtual attractive force vector field setting unit 612.
The trajectory generation unit 62 includes a virtual repulsive force data acquisition unit 621, a first torque data acquisition unit 622, a virtual attractive force data acquisition unit 623, a second torque data acquisition unit 624, and a drive data acquisition unit 625. doing.

The virtual repulsive force vector field setting unit 611 recognizes the reference point set for the object based on the imaging data obtained by imaging with the first electronic camera 71 and the second electronic camera 72, and the first electronic camera 71. The second electronic camera 72 has a function of obtaining a straight line connecting the reference point and setting a virtual repulsive force vector field composed of virtual repulsive force vectors having the straight line as a virtual repulsive force generation source.
The virtual repulsive force data acquisition unit 621 has a function of setting a first representative point in the arms 12 to 15 and the list 16 and obtaining a virtual repulsive force acting on the first representative point in the virtual repulsive force vector field.

The first torque data acquiring unit 622 is generated by the virtual repulsive force based on the repulsive force obtained by the virtual repulsive force data acquiring unit 621, acts on the arms 12 to 15 and the list 16, and the first torque data around the rotation axes O1 to O6. It has a function to obtain 1 torque.
The virtual attraction vector field setting unit 612 has a function of setting a virtual attraction vector field composed of virtual attraction vectors having a reference point as a virtual attraction generation source.

The virtual attractive force data acquisition unit 623 has a function of setting a second representative point in the arms 12 to 15 and the list 16 and obtaining a virtual attractive force acting on the second representative point in the virtual attractive force vector field.
The second torque data acquisition unit 624 is generated by the virtual attraction based on the virtual attraction obtained by the virtual attraction data acquisition unit 623, acts on the arms 12 to 15 and the list 16, and rotates around the rotation axes O1 to O6. The second torque is obtained.
The drive data acquisition unit 625 includes the first torque obtained by the first torque data acquisition unit 622, the second torque obtained by the second torque data acquisition unit 624, and the rotation axes O1 to O6 of the arms 12 to 15 and the list 16. On the basis of the moment of inertia around the arm 12, the rotation angle, the angular velocity and the angular acceleration of the arms 12 to 15 and the wrist 16 are obtained.

  The angular velocity control unit 63 and the drive source control unit 64 include the first torque obtained by the first torque data obtaining unit 622, the second torque obtained by the second torque data obtaining unit 624, the arms 12 to 15, and the list 16 It has a function of controlling the operation of the arms 12 to 15 and the wrist 16 based on the moment of inertia around the rotation axes O1 to O6. That is, the angular velocity control unit 63 and the drive source control unit 64 are configured so that the arms 12 to 15 and the list 16 rotate with the rotation angle and the angular velocity determined by the drive data acquisition unit 625 at the angular velocity and the angular acceleration. 15 and has a function of controlling the operation of the list 16.

  Here, the control device 20 determines the target position of the front end of the list 16 based on the contents of the processing performed by the robot 1 and the image data obtained by imaging with the first electronic camera 71 and the second electronic camera 72, that is, The target position of the end effector mounted on the wrist 16 is obtained, and the trajectories of the arms 12 to 15 and the wrist 16 for moving the end effector to the target position are generated. This trajectory is generated little by little every predetermined control period.

Specifically, when generating the trajectory, the control device 20 causes the rotation angles and angular velocities of the arms 12 to 15 and the wrist 16 at predetermined control periods so that the end effector moves toward the target position. The target value of the angular acceleration, that is, the target value of the rotation angle, angular velocity, and angular acceleration of each of the drive sources 401 to 406 is obtained. Then, position commands Pc of the drive sources 401 to 406 that are signals indicating target values of the rotation angles, angular velocities, and angular accelerations of the drive sources 401 to 406 are generated for each predetermined control cycle.
In the above and the following, “value is input and output” and the like are described, which means “a signal corresponding to the value is input and output”.

  In the angular velocity control unit 63, the position command Pc of the first drive source 401 is input from the trajectory generation unit 62, and a detection signal is input from the first angle sensor 411 to the angular velocity control unit 63. In the angular velocity control unit 63, the rotation angle (position feedback value Pfb) of the first drive source 401 calculated from the detection signal of the first angle sensor 411 becomes the position command Pc, and an angular velocity feedback value ωfb described later is described later. The first drive source 401 is driven by feedback control using each detection signal so that the angular velocity command ωc is obtained.

  That is, a position command Pc is input to a first subtracter (not shown) of the angular velocity control unit 63, and a position feedback value Pfb described later is input. In the angular velocity control unit 63, the number of pulses input from the first angle sensor 411 is counted, and the rotation angle of the first drive source 401 corresponding to the count value is output to the first subtracter as the position feedback value Pfb. The The first subtracter outputs a deviation between the position command Pc and the position feedback value Pfb (a value obtained by subtracting the position feedback value Pfb from the target value of the rotation angle of the first drive source 401).

  Further, the angular velocity control unit 63 performs a predetermined calculation process using the deviation input from the first subtractor and a proportional gain, which is a predetermined coefficient, so that the first drive source corresponding to the deviation is obtained. A target value of the angular velocity 401 is calculated. And the signal which shows the target value (command value) of the angular velocity of the 1st drive source 401 is output to a 2nd subtracter (not shown) as angular velocity command (1st angular velocity command) (omega) c. Here, in this embodiment, proportional control (P control) is performed as feedback control, but the present invention is not limited to this.

Further, the angular velocity control unit 63 calculates the angular velocity of the first drive source 401 based on the frequency of the pulse signal input from the first angle sensor 411, and outputs the angular velocity to the second subtracter as the angular velocity feedback value ωfb. Is done.
The second subtracter receives an angular velocity command ωc and an angular velocity feedback value ωfb. The second subtracter outputs a deviation between the angular velocity command ωc and the angular velocity feedback value ωfb (a value obtained by subtracting the angular velocity feedback value ωfb from the target value of the angular velocity of the first drive source 401).

  Further, the angular velocity control unit 63 uses a deviation input from the second subtractor and a predetermined coefficient, such as a proportional gain, an integral gain, and the like, and performs a predetermined calculation process including integration, thereby adjusting the deviation. The target value of the angular acceleration (torque) of the corresponding first drive source 401 is calculated. Then, the angular velocity control unit 631 outputs a signal indicating the target value (command value) of the angular acceleration of the first drive source 401 to the drive source control unit 64 as an angular acceleration command (torque command). Here, in this embodiment, PI control is performed as feedback control, but the present invention is not limited to this.

The drive source control unit 64 generates a drive signal (drive current) for the first drive source 401 based on the angular acceleration command input from the angular velocity control unit 63 and supplies it to the motor 401M via the motor driver 301.
In this way, the angular acceleration, that is, the torque of the first drive source 401 becomes as equal as possible to the target value, and the position feedback value Pfb becomes as equal as possible to the position command Pc, and the angular velocity feedback. Feedback control is performed so that the value ωfb is as equal as possible to the angular velocity command ωc, and the drive current of the first drive source 401 is controlled.
The control of the drive sources 402 to 406 by the drive source control unit 64 is the same as the control of the first drive source 401, and the description thereof is omitted.

  Next, a procedure when the control device 20 generates the trajectories of the arms 12 to 15 and the list 16 will be described based on FIGS. 5, 6, and 7. Since the procedure for generating the trajectories of the arms 12 to 15 and the list 16 is the same, the procedure for generating the trajectory of the first arm 12 will be typically described below. The working space in which the robot works is originally three-dimensional, but here, in order to facilitate understanding, two-dimensional coordinates are assumed, and two-dimensional coordinates having an x-axis and a z-axis orthogonal to each other are assumed. (See FIG. 5). In this embodiment, the number of electronic cameras is two, and there are two straight lines connecting each electronic camera and the reference point. Here, in order to facilitate understanding, of these two straight lines, Typically, one straight line, that is, a straight line 95 (see FIG. 5) connecting the first electronic camera 71 and the reference point will be described.

  The control device 20 knows the positions of the first electronic camera 71 and the second electronic camera 72, the initial arms 12 to 15, the wrist 16, and the position and orientation of the end effector. Moreover, the control apparatus 20 grasps | ascertains the rotation angle of each drive source 401-406 when each arm 12-15, list | wrist 16 act | operates, and position of each arm 12-15, list | wrist 16 from each rotation angle And always keep track of posture. Thereby, the control apparatus 20 always grasps | ascertains the position (coordinate) of the 1st representative point and 2nd representative point which are mentioned later.

  First, briefly explaining the whole, as shown in FIG. 7, the control method executed by the control device 20 of the robot 1 includes an imaging step of performing imaging (step S101) and a recognition step of recognizing the reference point 91 (steps). S102), a straight line setting step (step S103) for setting a straight line 95 connecting the first electronic camera 71 and the reference point 91, a virtual repulsive force vector field setting step (step S104) for setting a virtual repulsive force vector field, A first representative point setting step (step S105) for setting one representative point, a virtual repulsive force obtaining step for obtaining a virtual repulsive force (step S106), a first torque obtaining step for obtaining a first torque (step S107), and a virtual attractive force A virtual attractive vector field setting step (step S108) for setting a vector field, and a second representative point setting step (step S1) for setting a second representative point 9), a virtual attractive force obtaining step for obtaining a virtual attractive force (step S110), a second torque obtaining step for obtaining a second torque (step S111), and a drive data obtaining step for obtaining a rotation angle, angular velocity and angular acceleration (step S112). ).

The virtual repulsive force vector field setting step and the virtual attractive force vector field setting step may be performed first or simultaneously.
In addition, the first representative point setting step and the second representative point setting step may be performed first or simultaneously.
Moreover, the virtual attractive force acquisition step and the virtual attractive force acquisition step may be performed first or simultaneously.
Further, the first torque acquisition step and the second torque acquisition step may be performed first or simultaneously.

This will be specifically described below.
First, as shown in FIGS. 5 and 6, the control device 20 of the robot 1 uses the first electronic camera 71 to move around the reference point 91 set on the object 90 on which the robot 1 (robot body 10) works. Imaging is performed to obtain imaging data. The control device 20 obtains the position of the object 90, that is, the position (coordinates) of the reference point 91 based on the image data.

Next, the virtual repulsive force vector field setting unit 611 of the control device 20 obtains a straight line 95 that connects the first electronic camera 71 and the reference point, and a virtual repulsive force vector that uses the straight line 95 as a virtual repulsive force source. Set the repulsive vector field. As the position of the first electronic camera 71 for obtaining the straight line 95, the position (coordinates) of a predetermined point of the first electronic camera 71 is set in advance.
Here, the two-dimensional coordinates are set such that the origin coincides with a predetermined point of the first electronic camera 71 and the z-axis coincides with a straight line connecting the first electronic camera 71 and the reference point. Is done.

The virtual repulsive force vector field is not particularly limited and is appropriately set according to various conditions. Here, an example of the virtual repulsive force vector field is shown.
First, in the case of x> 0, the potential V at each coordinate (x, z) for obtaining the virtual repulsive force vector field is expressed by the following equation (1).
V = k · z / x (1)
However, k in the above equation (1) is a coefficient.

Then, the x component and the z component of the virtual repulsive force vector f (x, z) at each coordinate (x, z) when x> 0, that is, the virtual repulsive force vector field is expressed by the following equation (2).
f (x, z) = − ∇ · V
= (K · z / x ² , −k / x) (2)
Further, in the case of x <0, the potential V at each coordinate (x, z) for obtaining the virtual repulsive force vector field is expressed by the following equation (3).
V = −k · z / x (3)
However, k in the above equation (3) is a coefficient.

Then, the x and z components of the virtual repulsive force vector f (x, z) at each coordinate (x, z) when x <0, that is, the virtual repulsive force vector field is expressed by the following equation (4).
f (x, z) = − ∇ · V
= (− K · z / x ² , k / x) (4)
A virtual repulsive force vector field represented by the above equations (2) and (4) is set.

  Note that the magnitude of the virtual repulsive force vector in the virtual repulsive force vector field, that is, the magnitudes of the x component and the z component of the virtual repulsive force vector are larger as they are closer to the straight line 95 connecting the first electronic camera 71 and the reference point 91, respectively. . Accordingly, the first arm 12 can be moved away from the straight line 95 connecting the first electronic camera 71 and the reference point 91, and when the first electronic camera 71 images the object 90, the first arm 12 moves the object 90. Blocking can be prevented.

  In addition, the magnitude of the virtual repulsive force vector in the virtual repulsive force vector field, that is, the magnitude of the x component of the virtual repulsive force vector is larger as it is closer to the reference point 91. Thereby, the closer to the object 90, the farther the first arm 12 can be from the straight line 95 connecting the first electronic camera 71 and the reference point 91. Since the field of view of the first electronic camera 71 is wider as it is farther from the first electronic camera 71, that is, as it is closer to the reference point 91, the first electronic camera 71 captures the first object 90 when imaging the object 90. It is possible to more reliably prevent the arm 12 from blocking the object.

Next, the virtual repulsive force data acquisition unit 621 sets the first representative point 92 in the first arm 12 and obtains a virtual repulsive force that acts on the first representative point 92 in the virtual repulsive force vector field.
The number of the first representative points 92 is not particularly limited, and may be one or plural, but is preferably plural. Specifically, it is preferably 2 or more and 30 or less, and more preferably 3 or more and 10 or less. Further, when the number of the first representative points 92 is relatively large, it is preferable that the first representative points are arranged uniformly. As a result, the first torque acting on the first arm 12 and more accurately around the first rotation axis O1 can be obtained.

Next, the first torque data acquisition unit 622 generates the first revolving force around the first rotation axis O <b> 1 that is generated by the virtual repulsive force and acts on the first arm 12 based on the virtual repulsive force obtained by the virtual repulsive force data acquisition unit 621. Obtain 1 torque. When there are a plurality of first representative points 92, the first torque is the total value of the respective torques obtained from the virtual repulsive force acting on each first representative point 92.
If the first arm 12 is the most advanced arm, only the torque generated only from the virtual repulsive force acting on the first arm 12 acts on the first arm 12. Therefore, the first torque is obtained only from the virtual repulsive force acting on the first arm 12.

  However, since the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 exist at the distal end side of the first arm 12, a virtual repulsive force acting on the first arm 12 is applied to the first arm 12. The torque generated by the virtual repulsive force acting on the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 also acts. Therefore, the first torque is obtained from the virtual repulsive force acting on the first arm 12, the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16. That is, the first torque acting on the first arm 12 is generated by the virtual repulsive force acting on the first arm 12, the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16, and acts on the first arm 12. It is the total value of each torque to be.

Here, as a method for obtaining each torque generated by the virtual repulsive force acting on the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 and acting on the first arm 12, the second arm 13 is typically represented. A method for obtaining the torque generated by the virtual repulsive force acting on the first arm 12 and acting on the first arm 12 will be described.
First, of the virtual repulsive force that acts on the second arm 13, a virtual repulsive force component that is a torque that contributes to the rotational component of the second rotation axis O <b> 2 does not act on the first arm 12. Therefore, a virtual repulsive force component that is generated by a virtual repulsive force acting on the second arm 13 and that acts on the first arm 12 is a virtual repulsive force component that acts on the second arm 13 out of the virtual repulsive force acting on the second arm 13. Is obtained on the assumption that a virtual component perpendicular to is acting on the position of the second rotation axis O2 of the second arm 13 of the first arm 12.

Next, the virtual attractive force vector field setting unit 612 sets a virtual attractive force vector field composed of virtual attractive force vectors having the reference point 91 as a virtual attractive force generation source.
The virtual attractive force vector field is not particularly limited and is appropriately set according to various conditions. Here, an example of the virtual attractive force vector field is shown.
First, when the coordinates of the reference point 91 are (x ₀ , z ₀ ), the potential V at each coordinate (x, z) for obtaining the virtual attractive vector field is expressed by the following equation (5).
V = −L / {(x−x ₀ ) ² + (z−z ₀ ) ² } ^1/2 (5)
However, L in the above equation (5) is a coefficient.

Then, the x component and the z component of the virtual attractive vector f (x, z) at each coordinate (x, z), that is, the virtual attractive vector field is expressed by the following equation (6).
f (x, z) = − ∇ · V
= (− L (x−x ₀ ) / {(x−x ₀ ) ² + (z−z ₀ ) ² } ^3/2 , −L (z−z ₀ ) / {(x−x ₀ ) ² + (Z−z ₀ ) ² } ^3/2 ) (6)
The virtual attractive vector field shown by the above equation (6) is set.
Note that the magnitude of the virtual attraction vector in the virtual attraction vector field, that is, the magnitudes of the x component and the z component of the virtual attraction vector are larger as the reference point 91 is closer.

Next, the virtual attractive force data acquisition unit 623 sets the second representative point 93 in the first arm 12 and obtains a virtual attractive force acting on the second representative point 93 in the virtual attractive force vector field.
The number of the second representative points 93 is not particularly limited, and may be one or plural, but is preferably plural. Specifically, it is preferably 2 or more and 30 or less, and more preferably 3 or more and 10 or less. Further, when the number of the second representative points 93 is relatively large, it is preferable that the second representative points 93 are arranged uniformly. As a result, the second torque acting on the first arm 12 and more accurately around the first rotation axis O1 can be obtained.
It should be noted that the second representative point 93 and the first first representative point 92 may be the same or different, but the control can be further simplified by making them the same.

Next, the second torque data acquisition unit 624 generates the second torque data around the first rotation axis O <b> 1 that is generated by the virtual attraction and acts on the first arm 12 based on the virtual attraction obtained by the virtual attraction data acquisition unit 623. 2 Calculate the torque. When there are a plurality of second representative points 93, the second torque is a total value of the respective torques obtained from the virtual attractive force acting on each second representative point 93.
If the first arm 12 is the most advanced arm, only the torque generated only from the virtual attractive force acting on the first arm 12 acts on the first arm 12. For this reason, the second torque is obtained only from the virtual attractive force acting on the first arm 12.

However, since the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 exist on the distal end side of the first arm 12, the virtual attractive force acting on the first arm 12 is exerted on the first arm 12. In addition to the torque generated only from the torque, the torque generated by the virtual attractive force acting on the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 also acts. Therefore, the second torque is obtained from the virtual attractive force acting on the first arm 12, the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16. That is, the second torque acting on the first arm 12 is generated by a virtual attractive force acting on the first arm 12, the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16, and acts on the first arm 12. It is the total value of each torque to be.
The torques generated by the virtual attractive force acting on the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 and acting on the first arm 12 are obtained in the same manner as the first torque.

Next, the drive data acquisition unit 625 calculates the rotation angle, angular velocity, and angular acceleration of the first arm 12 based on the first torque, the second torque, and the moment of inertia around the first rotation axis O1 of the first arm 12. These are obtained and set as target values.
The moment of inertia of the first arm 12 around the first rotation axis O1 is the moment of inertia of only the first arm 12 if the first arm 12 is the most advanced arm. Since the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 exist on the distal end side of the one arm 12, the first arm 12, the second arm 13, the third arm 14, and the fourth arm 15 are present. , The moment of inertia of the entire list 16.

The moment of inertia of the first arm 12, the second arm 13, the third arm 14, the fourth arm 15, and the entire list 16 is determined by the second arm 13 based on the detection results of the angle sensors 412, 413, 414, 415, and 416. The postures of the third arm 14, the fourth arm 15, and the wrist 16 are obtained and obtained based on the postures.
Then, in the angular velocity control unit 63 and the drive source control unit 64, the first arm 12 rotates at the rotation angle obtained by the drive data acquisition unit 625 at the angular velocity and angular acceleration obtained by the drive data acquisition unit 625. Thus, the operation of the first arm 12 is controlled.

Specifically, target values for the rotation angle, angular velocity, and angular acceleration of the first arm 12 are converted into target values for the rotation angle, angular velocity, and angular acceleration of the first drive source 401, respectively. As described above, the position command Pc, which is a signal indicating the target value of the rotation angle, angular velocity, and angular acceleration of the first drive source 401 is generated, and the above-described control is performed based on the position command Pc.
The above operation is repeatedly executed at every predetermined control cycle, and finally, the manipulator attached to the tip of the list 16 reaches the object 90.

  As described above, according to the robot 1, the arms 12 to 15 and the list 16 include the straight line 95 that connects the first electronic camera 71 and the reference point 91, and the straight line that connects the second electronic camera 72 and the reference point 91 (see FIG. Therefore, the arms 12 to 15 and the list 16 block the object 90 when the robot body 10 takes an image of the object 90 to be worked on by the first electronic camera 71 and the second electronic camera 72. This can be prevented, and the object 90 can be reliably imaged.

Second Embodiment
FIG. 8 is a block diagram of the main part of the second embodiment of the robot according to the present invention. FIG. 9 is a diagram for explaining the operation of the robot shown in FIG. FIG. 10 is a flowchart showing a control operation of the robot shown in FIG. FIG. 9 schematically shows only the first arm of the robot.
Hereinafter, the second embodiment will be described with a focus on the differences from the first embodiment described above, and the description of the same matters will be omitted.
The robot 1 according to the second embodiment is configured to operate while avoiding an obstacle even when the obstacle exists.

As shown in FIG. 8, in the robot 1 of the second embodiment, the vector field setting unit 61 of the control device 20 includes a first virtual repulsive force vector field setting unit 613, a second virtual repulsive force vector field setting unit 614, a virtual And an attractive vector field setting unit 612.
The trajectory generating unit 62 includes a first virtual repulsive force data acquisition unit 626, a first torque data acquisition unit 622, a virtual attractive force data acquisition unit 623, a second torque data acquisition unit 624, and a second repulsive force data acquisition unit. 627, a third torque data acquisition unit 628, and a drive data acquisition unit 625.

  The first virtual repulsive force vector field setting unit 613 recognizes the reference point set for the object based on the imaging data obtained by imaging with the first electronic camera 71 and the second electronic camera 72, and the first electronic It has a function of obtaining a straight line connecting the camera 71 and the second electronic camera 72 and the reference point, and setting a first virtual repulsive force vector field composed of virtual repulsive force vectors using the straight line as a virtual repulsive force generation source.

The first virtual repulsive force data acquisition unit 626 has a function of setting a first representative point in the arms 12 to 15 and the list 16 and obtaining a virtual repulsive force acting on the first representative point in the first virtual repulsive force vector field. .
The first torque data acquiring unit 622 is generated by the virtual repulsive force based on the virtual repulsive force obtained by the first virtual repulsive force data acquiring unit 626, and acts on the arms 12 to 15 and the list 16. It has a function for obtaining the first torque around.

The virtual attraction vector field setting unit 612 has a function of setting a virtual attraction vector field composed of virtual attraction vectors having a reference point as a virtual attraction generation source.
The virtual attractive force data acquisition unit 623 has a function of setting a second representative point in the arms 12 to 15 and the list 16 and obtaining a virtual attractive force acting on the second representative point in the virtual attractive force vector field.

The second torque data acquisition unit 624 is generated by the virtual attraction based on the virtual attraction obtained by the virtual attraction data acquisition unit 623, acts on the arms 12 to 15 and the list 16, and rotates around the rotation axes O1 to O6. The second torque is obtained.
The second repulsive force vector field setting unit 614 recognizes an obstacle based on the imaging data obtained by imaging with the first electronic camera 71 and the second electronic camera 72 and uses the obstacle as a repulsive force generation source. It has a function of setting a second virtual repulsive force vector field composed of vectors.

The second virtual repulsive force data acquisition unit 627 has a function of setting a third representative point in the arms 12 to 15 and the list 16 and obtaining a repulsive force acting on the third representative point in the second virtual repulsive force vector field.
The third torque data acquisition unit 628 is generated by the virtual repulsive force based on the virtual repulsive force obtained by the second virtual repulsive force data acquisition unit 627, acts on the arms 12 to 15 and the list 16, and the rotation axes O1 to O6. It has a function for obtaining the third torque around.

  The drive data acquisition unit 625 includes a first torque obtained by the first torque data acquisition unit 622, a second torque obtained by the second torque data acquisition unit 624, and a third torque obtained by the third torque data acquisition unit 627. Based on the torque and the moments of inertia of the arms 12 to 15 and the wrist 16 around the rotation axes O1 to O6, the rotation angles, angular velocities and angular accelerations of the arms 12 to 15 and the wrist 16 are obtained.

The angular velocity control unit 63 and the drive source control unit 64 are operated by the first torque obtained by the first torque data obtaining unit 622, the second torque obtained by the second torque data obtaining unit 624, and the third torque data obtaining unit 627. Based on the calculated third torque and the moments of inertia around the rotation axes O1 to O6 of the arms 12 to 15 and the wrist 16, the arm 12 to 15 and the wrist 16 are controlled to operate. That is, the angular velocity control unit 63 and the drive source control unit 64 are configured so that the arms 12 to 15 and the list 16 rotate with the rotation angle and the angular velocity determined by the drive data acquisition unit 625 at the angular velocity and the angular acceleration. 15 and has a function of controlling the operation of the list 16.
The first virtual repulsive force vector field setting unit 613 is the same as the virtual repulsive force vector field setting unit 611 of the first embodiment, and the first virtual repulsive force data acquisition unit 626 is the virtual repulsive force data acquisition unit of the first embodiment. Since this is the same as 621, the description thereof is omitted.

The second virtual repulsive force vector field is defined by defining a virtual repulsive force vector at each coordinate in the work space, like the virtual repulsive force vector field and the virtual attractive force vector field of the first embodiment. That is, the second virtual repulsive force vector field is set as a function, for example, like the virtual repulsive force vector field and the virtual attractive force vector field of the first embodiment.
The second virtual repulsive force vector field is not particularly limited and is appropriately set according to various conditions. Here, the virtual repulsive force vector f (x (x) at each coordinate (x, z) in the work space is used. , Z), the x component and the z component are set larger as they are closer to the obstacles 81 and 82 (see FIG. 9), respectively. Accordingly, the arms 12 to 15 and the wrist 16 can be rotated while avoiding the obstacles 81 and 82. Here, the obstacle 81 is a work table on which the object is placed.

Specifically, the direction of the virtual repulsive force vector set for each coordinate is the normal direction of the surfaces of the obstacles 81 and 82, that is, the direction perpendicular to the surfaces of the obstacles 81 and 82, and the magnitude of the virtual repulsive force vector is It sets so that it may become large, so that it is near the obstacles 81 and 82. Further, when there are a plurality of obstacles 81 and 82 in the work space, for example, a virtual repulsive force vector generated from the obstacles 81 and 82 closest to each coordinate is finally set as a virtual repulsive force vector of the coordinates, Alternatively, the combined force of the virtual repulsive force vectors generated from the obstacles 81 and 82 is set as the virtual repulsive force vector.
When setting the second virtual repulsive force vector field, when the obstacles 81 and 82 have a complicated shape, for example, a shape or arrangement where the hand of the manipulator such as a protrusion or an extremely narrow portion is difficult to pass, You may perform weighting, such as enlarging the virtual repulsion vector vicinity of a applicable location.

Next, a procedure when the control device 20 generates the trajectories of the arms 12 to 15 and the list 16 will be described based on FIGS. 9 and 10.
First, in brief, as shown in FIG. 10, the control method executed by the control device 20 of the robot 1 includes an imaging step (step S201) for imaging, a reference point 91, and obstacles 81 and 82. A recognizing step (step S202), a straight line setting step (step S203) for setting a straight line 95 connecting the first electronic camera 71 and the reference point 91, and a first virtual repulsive force vector for setting a first virtual repulsive force vector field. A field setting step (step S204), a first representative point setting step (step S205) for setting a first representative point, a first virtual repulsive force acquisition step (step S206) for obtaining a virtual repulsive force, and a first torque for obtaining a first torque. 1 torque acquisition step (step S207), virtual attractive force vector field setting step (step S208) for setting a virtual attractive force vector field, and setting a second representative point 2 representative point setting step (step S209), virtual attractive force obtaining step for obtaining virtual attractive force (step S210), second torque obtaining step for obtaining second torque (step S211), and second virtual repulsive force vector field are set. A second virtual repulsive force vector field setting step (step S212), a third representative point setting step (step S213) for setting a third representative point, a second virtual repulsive force obtaining step (step S214) for obtaining a virtual repulsive force, A third torque obtaining step (step S215) for obtaining three torques, and a drive data obtaining step (step S216) for obtaining a rotation angle, angular velocity, and angular acceleration.
The first virtual repulsive force vector field setting step, the virtual attractive force vector field setting step, and the second virtual repulsive force vector field setting step may be performed in any order, and any two or three may be performed simultaneously. You may go.

Further, the first representative point setting step, the second representative point setting step, and the third representative point setting step may be performed in any order, and any two or three may be performed simultaneously. .
Further, the first virtual repulsive force acquisition step, the virtual attractive force acquisition step, and the second virtual repulsive force acquisition step may be performed in any order, and any two or three may be performed simultaneously.
Further, the first torque acquisition step, the second torque acquisition step, and the third torque acquisition step may be performed in any order, and any two or three of them may be performed simultaneously.

This will be specifically described below.
First, as shown in FIG. 8, the control device 20 of the robot 1 captures an image of the periphery of the reference point 91 set on the object 90 on which the robot 1 (robot body 10) works by using the first electronic camera 71. Obtain imaging data. The control device 20 obtains the position of the object 90, that is, the position (coordinates) of the reference point 91 based on the image data. Moreover, the control apparatus 20 images the obstacles 81 and 82 with the 1st electronic camera 71, and acquires imaging data. The control device 20 obtains the positions of the obstacles 81 and 82, that is, the positions (coordinates) of the contours of the obstacles 81 and 82 based on the image data.
The imaging around the reference point 91 and the imaging of the obstacles 81 and 82 may be performed simultaneously.

Hereinafter, since it is the same as that of 1st Embodiment, the description to the part is abbreviate | omitted.
After the second torque is obtained, the second virtual repulsive force vector field setting unit 614 sets a second virtual repulsive force vector field composed of a virtual repulsive force vector using the obstacles 81 and 82 described above as a virtual repulsive force generation source. .
Next, the second virtual repulsive force data acquisition unit 627 sets the third representative point 94 in the first arm 12 and obtains a virtual repulsive force that acts on the third representative point 94 in the second second force vector field.

The number of the third representative points 94 is not particularly limited, and may be one or plural, but is preferably plural. Specifically, it is preferably 2 or more and 30 or less, and more preferably 3 or more and 10 or less. In addition, when the number of the third representative points 94 is relatively large, it is preferable that the third representative points 94 are arranged uniformly. As a result, the first torque acting on the first arm 12 and more accurately around the first rotation axis O1 can be obtained.
The third representative 94 point, the first first representative point 92, and the second second representative point 93 may be the same or different, but the control is further simplified by making them the same. be able to.

Next, the third torque data acquisition unit 628 is rotated around the first rotation axis O <b> 1 generated by the virtual repulsion based on the virtual repulsion obtained by the second virtual repulsion data acquisition unit 627 and acting on the first arm 12. The third torque is obtained. When there are a plurality of third representative points, the third torque is a total value of the respective torques obtained from the virtual repulsive force acting on each third representative point.
If the first arm 12 is the most advanced arm, only the torque generated only from the virtual repulsive force acting on the first arm 12 acts on the first arm 12. For this reason, the third torque is obtained only from the virtual repulsive force acting on the first arm 12.

However, since the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 exist at the distal end side of the first arm 12, a virtual repulsive force acting on the first arm 12 is applied to the first arm 12. The torque generated by the virtual repulsive force acting on the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 also acts. Therefore, the third torque is obtained from the virtual repulsive force acting on the first arm 12, the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16. That is, the third torque that acts on the first arm 12 is generated by the repulsive force that acts on the first arm 12, the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16, and acts on the first arm 12. This is the total value of each torque.
Note that each torque generated by the virtual repulsive force acting on the second arm 13, the third arm 14, the fourth arm 15, and the wrist 16 and acting on the first arm 12 is the first torque described in the first embodiment. Find in the same way as

Next, similarly to the first embodiment described above, the drive data acquisition unit 625 is based on the first torque, the second torque, the third torque, and the moment of inertia of the first arm 12 around the first rotation axis O1. Thus, the rotation angle, angular velocity, and angular acceleration of the first arm 12 are obtained. Then, the control unit controls the operation of the first arm 12 so that the first arm 12 rotates at the rotation angle obtained by the drive data acquisition unit at the angular velocity and angular acceleration obtained by the drive data acquisition unit. To do.
According to this robot 1, the same effects as those of the first embodiment described above can be obtained.
In the robot 1, the arms 12 to 15 and the wrist 16 are rotated while avoiding the obstacles 81 and 82, and the manipulator attached to the tip of the wrist 16 can reach the target 90.

The robot and the motion trajectory control system according to the present invention have been described based on the illustrated embodiment. However, the present invention is not limited to this, and the configuration of each unit is an arbitrary configuration having the same function. Can be substituted. In addition, any other component may be added to the present invention.
Further, the present invention may be a combination of any two or more configurations (features) of the above embodiments.
In addition, as a motor of each drive source, for example, a stepping motor or the like can be cited in addition to the servo motor. Moreover, when using a stepping motor as a motor, you may use what detects the rotation angle of a motor by measuring the number of the drive pulses input into a stepping motor as an angle sensor, for example.

In addition, the method of each angle sensor is not particularly limited, and examples thereof include an optical method, a magnetic method, an electromagnetic method, and an electric method.
In the embodiment, the number of electronic cameras is two. However, the present invention is not limited to this, and the number of electronic cameras may be one, or may be three or more. However, the number of electronic cameras is preferably plural.

Moreover, in the said embodiment, although the electronic camera is installed in the position away from the robot main body, in this invention, it is not limited to this, For example, the electronic camera may be installed in the robot main body.
Moreover, in the said embodiment, although the electronic camera is being fixed, in this invention, it is not limited to this, For example, the electronic camera may be installed so that a movement is possible.

In the embodiment, the number of rotation axes of the robot is six. However, the present invention is not limited to this, and the number of rotation axes of the robot is one, two, three, four. There may be five or seven or more.
That is, in the embodiment, since the list has two arms, the number of robot arms is six. However, in the present invention, the number of robot arms is not limited to this. One, two, three, four, five, or seven or more may be used.

In the embodiment, the robot is a single-arm robot having one arm connection body in which a plurality of arms are rotatably connected. However, in the present invention, the robot is not limited to this. It may be a robot having a plurality of the arm connecting bodies such as a double-arm robot having two arm connecting bodies each having an arm rotatably connected.
The robot according to the present invention is not limited to an arm type robot (robot arm), but may be another type of robot, for example, a scalar robot, a legged walking (running) robot, or the like.

Hereinafter, an example when the robot according to the present invention is applied to a legged walking robot will be briefly described.
FIG. 11 is a perspective view showing another configuration example of the robot according to the present invention.
As shown in FIG. 11, the robot body 10B of the robot 1B has a head 20B, a trunk 30B, a pair of arms 40B, and a pair of legs 50B.

Each arm part 40B is comprised by the arm coupling body formed by connecting the some arm so that rotation is possible, respectively. The basic configuration of each arm connection body is different from the number of joints, the number of arms, and the like, but is the same as that of the arm connection body of the robot according to the above-described embodiment.
Each leg portion 50B is composed of a leg coupling body formed by pivotally coupling a plurality of legs. The basic configuration of each leg link is different from the number of joints, the number of legs, and the like, but is the same as the arm link of the robot according to the above-described embodiment, and thus the description thereof is omitted.
In the present application, a robot having an arm that rotates about the rotation axis is illustrated, but the same effect can be obtained even if the present application is applied to a linear motion arm, a translation arm, or the like.

  1, 1B ... Robot (industrial robot) 10, 10B ... Robot body 11 ... Base 12, 13, 14, 15 ... Arm (link) 16 ... List (link) 131, 141 ... Axis 161 ... wrist body 162 ... support ring 163 ... tip surface 171, 172, 173, 174, 175, 176 ... joint (joint) 18 ... arm connector 2, 2a, 2b, 2c, 2d ... arm body 3, 3a, 3b, 3c, 3d ... Drive mechanism 4, 4a, 4b, 4c, 4d ... Sealing means 20 ... Control device 101 ... Floor 111 ... Bolt 112 ... Base body 113 ... Cylindrical 114-Box-shaped part 201-206 ... Drive source control part 301, 302, 303, 304, 305, 306 ... Motor driver 401, 402, 03, 404, 405, 406 ... Driving source 401M, 402M, 403M, 404M, 405M, 406M ... Motor 411, 412, 413, 414, 415, 416 ... Angle sensor 61 ... Vector field setting unit 611, 613 614 ... Virtual repulsive force vector field setting part 612 ... Virtual attractive force vector field setting part 62 ... Trajectory generation part 621, 626, 627 ... Virtual repulsive force data acquisition part 622, 624, 628 ... Torque data acquisition part 623 ... ... Virtual attractive force data acquisition unit 625 ... Drive data acquisition unit 63 ... Angular velocity control unit 64 ... Drive source control unit 71, 72 ... Electronic camera 81, 82 ... Obstacle 90 ... Object 91 ... Reference point 92, 93, 94 …… Representative point 95 …… Line 20B …… Head 30B …… Body 40B …… Arm 5 B ...... legs O1, O2, O3, O4, O5, O6 ...... rotary shaft S101~S112 ...... step S201~S216 ...... Step

Claims (12)

An arm driven by a drive source;
An imaging device that captures imaging data by imaging the object on which the arm works and the periphery of the object;
A control device for controlling the arm,
The control device includes:
A virtual repulsive force vector field setting unit for setting a virtual repulsive force vector field including a virtual repulsive force vector having a straight line connecting the reference point of the object set in advance and the image capturing device as a source, from the imaging data;
A virtual repulsive force data acquisition unit for setting a first representative point on the arm and obtaining a virtual repulsive force acting on the first representative point in the virtual repulsive force vector field;
Based on the virtual repulsive force obtained by the virtual repulsive force data obtaining unit, a first torque data obtaining unit for obtaining a first torque of the drive source generated by the virtual repulsive force and acting on the arm;
A virtual attractive force vector field setting unit for setting a virtual attractive force vector field including a virtual attractive force vector having a reference point of the object as a generation source;
A virtual attraction data acquisition unit for setting a second representative point on the arm and obtaining a virtual attraction acting on the second representative point in the virtual attraction vector field;
A second torque data obtaining unit for obtaining a second torque of the drive source generated by the virtual attractive force and acting on the arm based on the virtual attractive force obtained by the virtual attractive force data obtaining unit;
And a control unit that controls the operation of the arm based on the first torque and the second torque.   The robot according to claim 1, wherein the magnitude of the virtual repulsive force vector in the virtual repulsive force vector field is inversely proportional to a distance from a reference point of the object. The arm rotates about the axis of rotation,
A drive data acquisition unit that obtains the rotation angle, angular velocity, and angular acceleration of the arm based on the first torque, the second torque, and the moment of inertia of the arm around the rotation axis;
The robot according to claim 1, wherein the control unit controls the operation of the arm so that the arm rotates the rotation angle at the angular velocity and the angular acceleration. An arm driven by a drive source;
An imaging device for imaging the object on which the arm works and the periphery of the object;
A control device for controlling the arm,
The control device includes:
A first virtual repulsive force vector field setting unit that sets a first repulsive force vector field including a virtual repulsive force vector having a straight line connecting the set reference point and the image capturing device as a virtual repulsive force generation source from the image data of the image capturing device. When,
A first virtual repulsive force data acquisition unit for setting a first representative point on the arm and obtaining a virtual repulsive force acting on the first representative point in the first virtual repulsive force vector field;
Based on the virtual repulsive force obtained by the first virtual repulsive force data obtaining unit, a first torque data obtaining unit for obtaining a first torque of the drive source generated by the virtual repulsive force and acting on the arm;
A virtual attractive force vector field setting unit for setting a virtual attractive force vector field including a virtual attractive force vector having a reference point of the object as a generation source;
A virtual attraction data acquisition unit for setting a second representative point on the arm and obtaining a virtual attraction acting on the second representative point in the virtual attraction vector field;
A second torque data obtaining unit for obtaining a second torque of the drive source generated by the virtual attractive force and acting on the arm based on the virtual attractive force obtained by the virtual attractive force data obtaining unit;
A second virtual repulsive force vector field setting unit for setting a second repulsive force vector field including a virtual repulsive force vector having an obstacle as a generation source;
A second virtual repulsive force data acquisition unit for setting a third representative point on the arm and obtaining a virtual repulsive force acting on the third representative point in the second virtual repulsive force vector field;
A third torque data obtaining unit for obtaining a third torque of the drive source generated by the virtual repulsive force and acting on the arm based on the virtual repulsive force obtained by the second virtual repulsive force data obtaining unit;
And a control unit that controls the operation of the arm based on the first torque, the second torque, and the third torque. The imaging device captures an image including the obstacle,
The robot according to claim 4, wherein the second virtual repulsive force vector field setting unit recognizes the obstacle and sets the second virtual repulsive force vector field based on imaging data obtained by imaging with the imaging device. .   The robot according to claim 4 or 5, wherein the magnitude of the virtual repulsive force vector in the first virtual repulsive force vector field is inversely proportional to a distance from a reference point of the object. The arm rotates about the axis of rotation,
A drive data acquisition unit that obtains the rotation angle, angular velocity, and angular acceleration of the arm based on the first torque, the second torque, the third torque, and the moment of inertia of the arm around the rotation axis; ,
The robot according to claim 4, wherein the control unit controls the operation of the arm so that the arm rotates the rotation angle at the angular velocity and the angular acceleration.   The robot according to claim 1, wherein the imaging device is installed at a position separated from the arm. An arm driven by a drive source;
An imaging device for imaging the object on which the arm works and the periphery of the object;
A control device for controlling the arm,
The control device includes:
A virtual repulsive force vector field setting unit that sets a virtual repulsive force vector field including a virtual repulsive force vector having a straight line connecting the reference point of the object and the image capturing device as a source, from the imaging data of the image capturing device;
A virtual repulsive force data acquisition unit for setting a first representative point on the arm and obtaining a virtual repulsive force acting on the first representative point in the virtual repulsive force vector field;
Based on the virtual repulsive force obtained by the virtual repulsive force data obtaining unit, a first torque data obtaining unit for obtaining a first torque of the drive source generated by the virtual repulsive force and acting on the arm;
A virtual attractive force vector field setting unit for setting a virtual attractive force vector field including a virtual attractive force vector having a reference point of the object as a generation source;
A virtual attraction data acquisition unit for setting a second representative point on the arm and obtaining a virtual attraction acting on the second representative point in the virtual attraction vector field;
A second torque data obtaining unit for obtaining a second torque of the drive source generated by the virtual attractive force and acting on the arm based on the virtual attractive force obtained by the virtual attractive force data obtaining unit;
An operation trajectory control system comprising: a control unit that controls the operation of the arm based on the first torque and the second torque. The arm rotates about the axis of rotation,
A drive data acquisition unit that obtains the rotation angle, angular velocity, and angular acceleration of the arm based on the first torque, the second torque, and the moment of inertia of the arm around the rotation axis;
The motion trajectory control system according to claim 9, wherein the control unit controls the operation of the arm so that the arm rotates the rotation angle at the angular velocity and the angular acceleration. An arm driven by a drive source;
An imaging device for imaging the object on which the arm works and the periphery of the object;
A control device for controlling the arm,
The control device includes:
A first virtual repulsive force vector field setting unit that sets a first repulsive force vector field including a virtual repulsive force vector having a straight line connecting the set reference point and the image capturing device as a virtual repulsive force generation source from the image data of the image capturing device. When,
A first virtual repulsive force data acquisition unit for setting a first representative point on the arm and obtaining a virtual repulsive force acting on the first representative point in the first virtual repulsive force vector field;
Based on the virtual repulsive force obtained by the first virtual repulsive force data obtaining unit, a first torque data obtaining unit for obtaining a first torque of the drive source generated by the virtual repulsive force and acting on the arm;
A virtual attractive force vector field setting unit for setting a virtual attractive force vector field including a virtual attractive force vector having a reference point of the object as a generation source;
A virtual attraction data acquisition unit for setting a second representative point on the arm and obtaining a virtual attraction acting on the second representative point in the virtual attraction vector field;
A second torque data obtaining unit for obtaining a second torque of the drive source generated by the virtual attractive force and acting on the arm based on the virtual attractive force obtained by the virtual attractive force data obtaining unit;
A second virtual repulsive force vector field setting unit for setting a second repulsive force vector field including a virtual repulsive force vector having an obstacle as a generation source;
A second virtual repulsive force data acquisition unit for setting a third representative point on the arm and obtaining a virtual repulsive force acting on the third representative point in the second virtual repulsive force vector field;
A third torque data obtaining unit for obtaining a third torque of the drive source generated by the virtual repulsive force and acting on the arm based on the virtual repulsive force obtained by the second virtual repulsive force data obtaining unit;
And a control unit that controls the operation of the arm based on the first torque, the second torque, and the third torque. The arm rotates about the axis of rotation,
A drive data acquisition unit for respectively determining the rotation angle, angular velocity, and angular acceleration of the arm based on the first torque, the second torque, the third torque, and the moment of inertia of the arm around the rotation axis; And
The motion trajectory control system according to claim 11, wherein the control unit controls the operation of the arm so that the arm rotates the rotation angle at the angular velocity and the angular acceleration.

Images