Robot control apparatus, robot control method, program, recording medium and robot system

Abstract

An end effector can be removed to a target position and calibration loads can be reduced, even if there is an error in a kinematic operation in a robot main body and/or cameras. A positional deviation integral torque obtained by integrating a value corresponding to a positional deviation is applied to joints while being superimposed with torques based on angular deviations. If movements of the joints stop or are about to stop before the end effector reaches a target position due to an error in a kinematic operation, the positional deviation integral torque increases with time, to move said joints and move the end effector to the target position. Thus, the end effector can be reliably moved to the target position by the positional deviation integral torque, and calibration loads can be reduced.

Claims

1. A robot control apparatus that controls a robot configured to include a freely displaceable joint and move an end effector according to a displacement of the joint, comprising: an imager to image a reference position provided on the robot while capturing the reference position and a destination of the reference position in a field of view; a displacement amount detector to detect a displacement amount of the joint; a positional deviation acquirer to acquire a positional deviation from the destination for the reference position based on an imaging result of the imager; an application amount calculator to calculate a first application amount from a result of acquiring a value of integral acquired by performing an integral operation on a value corresponding to the positional deviation for the reference position and a second application amount by performing a proportional operation on a value corresponding to a displacement amount deviation between a detected displacement amount by the displacement amount detector and a target displacement amount which is the displacement amount of the joint when the reference position coincides with the destination, each of said first and second application amount being a force or a torque applied to the joint to displace the joint; and a drive controller to move the reference position to the destination thereof by applying the first application amount and second application amount to the joint.

2. The robot control apparatus according to claim 1, controlling the robot to move the end effector according to a rotation of the joint, wherein: the imager images the reference position provided on the end effector while capturing the reference position and the destination of the reference position; the displacement amount detector is an angle detector to detect a rotation angle of the joint; and the application amount calculator is a torque calculator to calculate a first torque as the first application amount by performing the integral operation on the value corresponding to the positional deviation and a second torque as the second application amount by performing the proportional operation on a value corresponding to an angular deviation between a detected angle by the angle detector and a target angle as the target displacement amount which is the rotation angle of the joint when the reference position is at the destination; and the drive controller controls a joint drive mechanism to drive the joint so as to apply the first and second torques to the joint.

3. The robot control apparatus according to claim 2, wherein: the application amount calculator calculates a third torque by performing a proportional operation on the value corresponding to the positional deviation; and the drive controller controls the joint drive mechanisms so that the third torque is further applied to the joint.

4. The robot control apparatus according to claim 2, wherein the application amount calculator calculates the first torque by performing the integral operation on the positional deviation multiplied by a transposed Jacobian matrix.

5. The robot control apparatus according to claim 1, controlling the robot in which the end effector is attached to one end of an arm that has a plurality of the joints and has six or more degrees of freedom and to which a plurality of the reference positions including three or more reference positions are set, wherein the imager images the three or more reference positions while capturing the destination of each of the three or more reference positions that include at least two positions set on the end effector side from a tip joint closest to one end side out of the joints in the field of view; the positional deviation acquirer acquires the positional deviation for each reference position; the application amount calculator calculates the first application amount and the second application amount for each joint; and the drive controller controls a location and a posture of the end effector in three dimensions by applying the first application amount and second application amount to the plurality of the joints to move the three or more reference positions toward the destinations thereof.

6. The robot control apparatus according to claim 5, wherein the drive controller moves each of the three or more reference positions including a specific reference position that is at a position other than the at least two positions and on the tip joint or on an other end side opposite to the one end side from the tip joint toward the destination thereof.

7. The robot control apparatus according to claim 6, wherein the specific reference position is set on a specific joint or on the other end side from the specific joint which is a third joint from the one end side out of the plurality of the joints.

8. The robot control apparatus according to claim 7, wherein the specific reference position is set on the one end side from the joint which is the third joint from the other end out of the plurality of the joints.

9. The robot control apparatus according to claim 7, wherein the application amount calculator calculates the first application amount by adding a result of multiplying the value of integral for the specific reference position by a 1.sup.st weigh coefficient and a result of multiplying the value of integral for the reference positions other than the specific reference position by a 2.sup.nd weigh coefficient, and the 1.sup.st weigh coefficient is larger than the 2.sup.nd weigh coefficient.

10. The robot control apparatus according to claim 5, wherein the application amount calculator calculates a third application amount from a result of acquiring a proportional value, which is acquired by performing a proportional operation on the value corresponding to the positional deviation, for each reference position and the drive controller further applies the third application amount to the plurality of the joints.

11. The robot control apparatus according to claim 5, wherein the application amount calculator calculates the first application amount by acquiring the value of integral by performing an integral operation on the positional deviation multiplied by a transposed Jacobian matrix.

12. A robot control method that controls a robot configured to include a freely displaceable joint and move an end effector according to a displacement of the joint, comprising: a step of imaging a reference position provided on the robot while capturing the reference position and a destination of the reference position in a field of view; a step of acquiring a positional deviation from the destination for the reference position based on an imaging result of the reference position; a step of acquiring a detected displacement amount by detecting a displacement amount of the joint; a step of calculating a first application amount from a result of acquiring a value of integral acquired by performing an integral operation on a value corresponding to the positional deviation for the reference position and a second application amount by performing a proportional operation on a value corresponding to a displacement amount deviation between the detected displacement amount and a target displacement amount which is the displacement amount of the joint when the reference position coincides with the destination, each of said first and second application amount being a force or a torque applied to the joint to displace the joint; and a step of moving the reference position to the destination thereof by applying the first application amount and second application amount to the joint.

13. A non-transitory computer-readable recording medium having recorded thereon a robot control program that causes a computer to control a robot configured to include a freely displaceable joint and move an end effector according to a displacement of the joint, causing the computer to perform: a step of imaging a reference position provided on the robot while capturing the reference position and a destination of the reference position in a field of view; a step of acquiring a positional deviation from the destination for the reference position based on an imaging result of the reference position; a step of acquiring a detected displacement amount by detecting a displacement amount of the joint; a step of calculating a first application amount from a result of acquiring a value of integral acquired by performing an integral operation on a value corresponding to the positional deviation for the reference position and a second application amount by performing a proportional operation on a value corresponding to a displacement amount deviation between the detected displacement amount and a target displacement amount which is the displacement amount of the joint when the reference position coincides with the destination, each of said first and second application amount being a force or a torque applied to the joint to displace the joint; and a step of moving the reference position to the destination thereof by applying the first application amount and second application amount to the joint.

14. A robot system, comprising: a robot configured to include a freely displaceable joint and move an end effector according to a displacement of the joint; an imager to image a reference position provided on the robot while capturing the reference position and a destination of the reference position in a field of view; a displacement amount detector to detect a displacement amount of the joint; a positional deviation acquirer to acquire a positional deviation from the destination for the reference position based on an imaging result of the imager; an application amount calculator to calculate a first application amount from a result of acquiring a value of integral acquired by performing an integral operation on a value corresponding to the positional deviation for the reference position and a second application amount by performing a proportional operation on a value corresponding to a displacement amount deviation between a detected displacement amount by the displacement amount detector and a target displacement amount which is the displacement amount of the joint when the reference position coincides with the destination, each of said first and second application amount being a force or a torque applied to the joint to displace the joint; and a drive controller to move the reference position to the destination thereof by applying the first application amount and second application amount to the joint.

Description

BRIEF DESCRIPTION OF DRAWINGS

(1) FIG. 1 is a diagram showing an example of a robot system according to a first embodiment of the invention.

(2) FIG. 2 is a block diagram showing an electrical configuration for executing the position control of the end effector 4 in the robot system of FIG. 1.

(3) FIG. 3 is a block diagram showing an example of the position control of the end effector executed in the robot system.

(4) FIG. 4 is a diagram showing an example of a robot system according to a second embodiment of the invention.

(5) FIG. 5 is a diagram showing an example of states of torques acting on the end effector in the process of moving the reference points to the destinations thereof.

(6) FIG. 6 is a graph showing experimental results in Example 1.

(7) FIG. 7 is a graph showing experimental results in Example 1.

(8) FIG. 8 is a graph showing experimental results in Example 2.

(9) FIG. 9 is a graph showing experimental results in Example 2.

(10) FIG. 10 is a graph showing experimental results in Example 2.

(11) FIG. 11 is a graph showing experimental results in Example 2.

(12) FIG. 12 is a graph showing an experimental result in Example 3.

(13) FIG. 13 is a graph showing an experimental result in Example 3.

(14) FIG. 14 is a table showing set values of gains in Example 4.

(15) FIG. 15 is a graph showing a simulation result in Example 4.

(16) FIG. 16 is a table showing the set values of the gains in Example 5.

(17) FIG. 17 is a graph showing a simulation result in Example 5.

(18) FIG. 18 is a table showing the set values of the gains in Example 6.

(19) FIG. 19 is a graph showing a simulation result in Example 6.

(20) FIG. 20 is a table showing the set values of the gains in Example 7.

(21) FIG. 21 is a graph showing a simulation result in Example 7.

(22) FIG. 22 is a table showing the set values of the gains in Example 8.

(23) FIG. 23 is a graph showing a simulation result in Example 8.

DESCRIPTION OF EMBODIMENTS

(24) First Embodiment

(25) FIG. 1 is a diagram showing an example of a robot system according to a first embodiment of the invention. As shown in FIG. 1, the robot system 1 includes a robot 2 and two cameras C1, C2. Note that, in FIG. 1, the robot 2 is schematically shown by symbol notation of joints Q1 to Q3, links L1 to L3 and an end effector 4 and the cameras C1, C2 are schematically shown by being represented by image planes IM1, IM2 thereof.

(26) The robot 2 has such a schematic configuration that the end effector 4 (tool) is attached to the tip of an arm 3 which includes freely rotatable joints Q1 to Q3 and moves according to the rotation of the joints Q1 to Q3. Specifically, the arm 3 is formed by attaching the link L1 between the joints Q1, Q2, attaching the link L2 between the joints Q2, Q3 and attaching one end of the link L3 to the joint Q3. The end effector 4 is attached to the other end of the link L3 (tip of the arm 3). The joint Q1 rotates around a vertical axis, thereby rotating the members on the side of the end effector 4 around the vertical axis. The joint Q2 rotates to change angles between the link L1 and the members on the side of the end effector 4. The joint Q3 rotates to change angles between the members on the side of the end effector 4 and the link L2.

(27) In the thus configured robot 2, the end effector 4 can be moved by changing rotation angles q1 to q3 of the joints Q1 to Q3. Particularly, this robot system 1 controls a position p of the end effector 4 by adjusting the rotation angles q1 to q3 of the joints Q1 to Q3 based on detection results of the position p of the end effector 4 (specifically, representative point such as a TCP (Tool Center Point)) by the cameras C1, C2. On this occasion, a mark such as an LED (Light Emitting Diode) may be attached to the representative point to improve visibility in the cameras C1, C2.

(28) That is, the cameras C1, C2 are respectively positioned to include a target position pd as a destination of the end effector 4 in a field of view thereof, and a positional deviation p between the position p of the end effector 4 and the target position pd is captured in mutually different planes. Specifically, the cameras C1, C2 are, for example, so arranged that the image planes IM1, IM2 are perpendicular to each other, a YZ plane of a task coordinate system is imaged by the camera C1 and a ZX plane of the task coordinate system is imaged by the camera C2. The position control of the end effector 4 is executed by adjusting the rotation angles q1 to q3 of the joints Q1 to Q3 to reduce the positional deviation p between the target position pd and the position p of the end effector 4 detected by imaging results of the cameras C1, C2 (visual feedback).

(29) On this occasion, it may be configured so that an imaging result of the robot 2 at a singular point is not fed back to the position control of the end effector 4. In a specific example, the cameras C1, C2 may be so arranged that the robot 2 at the singular point is outside the fields of view of the cameras C1, C2. In this way, it can be suppressed that the visual feedback is performed based on the imaging result of the robot 2 at the singular point and the control of the robot 2 becomes unstable.

(30) Here, the notation of coordinate systems and each control amount used in the position control of the end effector 4 is described. As shown in FIG. 1, the three-dimensional task coordinate system configured by X, Y and Z axes perpendicular to each other with the Z axis as a vertical axis is defined for a task space where the end effector 4 operates. Thus, the position p of the end effector 4 is given by a three-dimensional vector (px, py, pz). Similarly, the target position pd of the end effector 4 is also given by a three-dimensional vector (pdx, pdy, pdz).

(31) A three-dimensional camera coordinate system configured by Ui, Vi and Zi axes perpendicular to each other is defined for each camera Ci. Here, i is a number for distinguishing the camera (i=1, 2), the Ui axis is a horizontal axis of the i.sup.th camera Ci, the Vi axis is a vertical axis of the i.sup.th camera Ci and the Wi axis is a depth axis of the i.sup.th camera Ci. Further, in FIG. 1, image planes IMi of the cameras Ci in UiVi planes are shown and coordinates i as projection of the position p (reference point) on the image planes IMi (i.e. coordinates i of the position p in the coordinate systems of the cameras Ci) and coordinates i as projection of the target position pd (destination) on the image planes IMi (i.e. coordinates i of the target position pd in the coordinate systems of the cameras Ci) are shown. These coordinates i, i are specifically as follows.

(32) $\begin{array}{cc}\left[\begin{array}{c}{}^{i}u\\ {}^{i}v\end{array}\right]\text{:}\mathrm{COORDINATE}i\mathrm{AS}\mathrm{PROJECTION}\mathrm{OF}\mathrm{REFERENCE}\mathrm{POINT}p\mathrm{ON}\mathrm{IMAGE}\mathrm{PLANE}\mathrm{IMi}\left[\begin{array}{c}{}^i{u}_d\\ {}^i{v}_d\end{array}\right]\text{:}\mathrm{COORDINATE}i\mathrm{AS}\mathrm{PROJECTION}\mathrm{OF}\mathrm{DESTINATION}\mathrm{pd}\mathrm{ON}\mathrm{IMAGE}\mathrm{PLANE}\mathrm{IMi}& \left[\mathrm{Equation}1\right]\end{array}$

(33) The rotation angles q of the joint Q of the robot 2 are expressed by a vector (q1, q2, q3) including the rotation angle qn of the joint Qn as each component. Here, the notation of joints Q is the collective notation of the joints Q1 to Q3 and n is a number for distinguishing the joint (n=1, 2, 3). Further, target angles qd (=qd1, qd2, qd3) are the rotation angles q of the joints Q when the end effector 4 is at the target position pd. Furthermore, torques applied to the joints Q of the robot 2 are expressed by a vector (1, 2, 3) including a torque n acting on the joint Qn as each component.

(34) The above is description of the notation of the coordinate systems and the control amounts. Next, the position control of the end effector 4 is described in detail. FIG. 2 is a block diagram showing an electrical configuration for executing the position control of the end effector 4 in the robot system of FIG. 1. As shown in FIG. 2, the robot system 1 includes a controller 5 in charge of an arithmetic function and configured by a CPU (Central Processing Unit), a memory and the like. This controller 5 executes the position control of the end effector 4 in accordance with a program 7 recorded in a recording medium 6. Note that various media such as CDs (Compact Discs), DVDs (Digital Versatile Discs), USB (Universal Serial Bus) memories can be used as the recording medium 6. Further, in the robot system 1, a motor Mn for driving the joint Qn is provided for each of the joints Q1 to Q3, and an encoder En for detecting a rotational position of the motor Mn is provided for each of the motors M1 to M3.

(35) The controller 5 adjusts the rotation angles q of the joints Q of the robot 2 by controlling each of the motors M1 to M3. On this occasion, to perform the aforementioned visual feedback, the controller 5 detects the positional deviation p (=pdp) of the end effector 4 from the imaging results of the end effector 4 by the cameras Ci (external sensors). On this occasion, the controller 5 detects the positional deviation p of the end effector 4 while controlling panning/tiling of the cameras Ci so that the target position pd coincides with or is proximate to origins of the coordinate systems of the cameras Ci (centers of the image planes IMi).

(36) Further, in parallel with the detection of the positional deviation p of the end effector 4, the controller 5 detects angular deviations q (=qdq) of the joints Q from outputs of the encoders E1 to E3 (internal sensors). Then, the controller 5 calculates the torques based on the positional deviation p (=pdp) and the angular deviations q (qdq). Then, the motors M1 to M3 apply the torques to the joints Q, thereby adjusting the rotation angles q of the joints Q. As just described, the detection results of the cameras Ci and the encoders En are fed back to the torques to control the position of the end effector 4 in this embodiment.

(37) FIG. 3 is a block diagram showing an example of the position control of the end effector executed in the robot system. In FIG. 3 and the following description, a symbol indicates that a quantity with this symbol possibly includes an error, a matrix with a symbol T on an upper right corner indicates a transposed matrix of the matrix to which the symbol T is attached, a matrix with a symbol 1 on an upper right corner indicates an inverse matrix of the matrix to which the symbol 1 is attached, a symbol s is a variable of Laplace transform (Laplace variable) and a symbol . indicates a derivative. Further, in FIG. 3 and the following equations, a symbol d attached to the target position pd and the target angles qd and the like are written as a subscript as appropriate.

(38) When the torques are applied to the joints Q of the robot 2, the robot 2 moves in accordance with robot dynamics 201 and the joints Q of the robot 2 have the rotation angles q. Here, the robot dynamics 201 specify a relationship between torques acting on the mechanism of the robot 2 and acceleration created by these torques. As a result, the end effector 4 moves to the position p corresponding to the rotation angles q in accordance with robot kinematics 202. Further, to execute the aforementioned feedback control, the controller 5 has an external loop Lx to feed the positional deviation p back to the torques and an internal loop Lq to feed the angular deviations q back to the torques .

(39) In the external loop Lx, the position p of the end effector 4 is detected by two cameras C1, C2. In other words, the position p of the end effector 4 in the task coordinate system is transformed into coordinates 1, 2 of the coordinate systems of the respective cameras C1, C2 by coordinate transform 203. Then, the cameras C1, C2 output values (1-1), (2-2) indicating the positional deviation p in the respective coordinate systems. Specifically, the value (i-i) indicating the positional deviation p in the coordinate system of the camera Ci is as follows.

(40) $\begin{array}{cc}\left[\begin{array}{c}{}^{i}u_d-{}^{i}u\\ {}^{i}v_d-{}^{i}v\end{array}\right]\text{:}\mathrm{VALUE}\left(i-i\right)\mathrm{INDICATING}\mathrm{POSITIONAL}\mathrm{DEVIATION}p\mathrm{IN}\mathrm{COORDINATE}\mathrm{SYSTEM}\mathrm{OF}\mathrm{CAMERA}\mathrm{Ci}& \left[\mathrm{Equation}2\right]\end{array}$

(41) Then, the torques based on the positional deviation p in the task coordinate system are calculated from the positional deviations (i-i) expressed in the coordinate systems of the respective cameras C1, C2. In this calculation of the torques , a relationship established between the positional deviations in the camera coordinate systems and that in the position coordinate system can be used.

(42) That is, the following relationship is established between the coordinate systems of the cameras Ci and the task coordinate system.

(43) $\begin{array}{cc}{}^{i}l\left[\begin{array}{c}{}^{i}u\\ {}^{i}v\\ 1\end{array}\right]={}^{i}A\left[{}^{i}R^T-{}^{i}R^T{}^i\right]\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\\ 1\end{array}\right]={}^{i}P\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\\ 1\end{array}\right]{}^{i}l\text{:}\mathrm{DISTANCE}\mathrm{BETWEEN}\mathrm{IMAGE}\mathrm{PLANE}\mathrm{IMi}\mathrm{AND}\mathrm{POSITION}p\mathrm{IN}\mathrm{DEPTH}\text{-}\mathrm{AXIS}\mathrm{DIRECTION}\mathrm{OF}{i}^{\mathrm{th}}\mathrm{CAMERA}\mathrm{Ci}\left(\mathrm{DEPTH}\mathrm{DISTANCE}\right){}^{i}A\text{:}33\mathrm{ROTATION}\mathrm{MATRIX}\mathrm{EXPRESSION}\mathrm{INTERNAL}\mathrm{PARAMETER}A\mathrm{OF}{i}^{\mathrm{th}}\mathrm{CAMERA}\mathrm{Ci}{}^{i}R\text{:}33\mathrm{ROTATION}\mathrm{MATRIX}\mathrm{TRANSFORMATION}\mathrm{COORDINATE}\mathrm{FROM}\mathrm{COORDINATE}\mathrm{SYSTEM}\mathrm{OF}{i}^{\mathrm{th}}\mathrm{CAMERA}\mathrm{Ci}\mathrm{TO}\mathrm{TASK}\mathrm{COORDINATE}\mathrm{SYSTEM}\text{:}\mathrm{VECTOR}\mathrm{FROM}\mathrm{ORIGIN}\mathrm{OF}\mathrm{COORDINATE}\mathrm{SYSTEM}\mathrm{OF}{i}^{\mathrm{th}}\mathrm{CAMERA}\mathrm{Ci}\mathrm{TO}\mathrm{ORIGIN}\mathrm{OF}\mathrm{TASK}\mathrm{COORDINATE}\mathrm{SYSTEM}{}^{i}P=\left[\begin{array}{cccc}{}^{i}P_{11}& {}^{i}P_{12}& {}^{i}P_{13}& {}^{i}P_{14}\\ {}^{i}P_{21}& {}^{i}P_{22}& {}^{i}P_{23}& {}^{i}P_{24}\\ {}^{i}P_{31}& {}^{i}P_{32}& {}^{i}P_{33}& {}^{i}P_{34}\end{array}\right]\text{:}\mathrm{PERSPECTIVE}\mathrm{PROJECTION}\mathrm{MATRIX}\mathrm{OF}{i}^{\mathrm{th}}\mathrm{CAMERA}\mathrm{Ci}& \left[\mathrm{Equation}3\right]\end{array}$

(44) Note that a matrix expressing an internal parameter A of the camera is specifically given by the following equation.

(45) $\begin{array}{cc}A=\left[\begin{array}{ccc}{\mathrm{fk}}_u& {\mathrm{fk}}_u\cot & u_0\\ 0& {\mathrm{fk}}_v/\sin & v_0\\ 0& 0& 1\end{array}\right]& \left[\mathrm{Equation}4\right]\end{array}$ f: FOCAL LENGTH OF CAMERA k.sub.u: SCALE IN HORIZONTAL-AXIS DIRECTION OF CAMERA k.sub.v: SCALE IN VERTICAL-AXIS DIRECTION OF CAMERA : ANGLE BETWEEN HORIZONTAL-AXIS DIRECTION AND VERTICAL-AXIS DIRECTION OF CAMERA u.sub.0: HORIZONTAL COMPONENT AT POINT WHERE OPTICAL AXIS INTERSECT WITH IMAGE PLANE v.sub.0: VERTICAL COMPONENT AT POINT WHERE OPTICAL AXIS INTERSECT WITH IMAGE PLANE

(46) Since the relationship of Equation 3 is established, the following relationship is established between the positional deviations (i-i) in the camera coordinate systems and the positional deviation (pdp) in the task coordinate system.

(47) $\begin{array}{cc}\begin{array}{c}\left[\begin{array}{c}{}^{i}u_d-{}^{i}u\\ \begin{array}{c}{}^{i}v_d-{}^{i}v\\ 0\end{array}\end{array}\right]=\frac{1}{{}^{i}l_d}{}^{i}P\left[\begin{array}{c}\mathrm{pdx}\\ \mathrm{pdy}\\ \mathrm{pdy}\\ 1\end{array}\right]-\frac{1}{{}^{i}l}{}^{i}P\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\\ 1\end{array}\right]\\ =\frac{1}{{}^{i}l_d}{}^{i}A^i{R}^T\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\end{array}\right]+\left(\frac{1}{{}^{i}l_d}-\frac{1}{{}^{i}l}\right){}^{i}P\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\\ 1\end{array}\right]\\ =\frac{1}{{}^{i}l_d}{}^{i}A^i{R}^T\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\end{array}\right]+\left(\frac{{}^{i}l-{}^{i}l_d}{{}^{i}l_d}\right)\left[\begin{array}{c}{}^{i}u\\ {}^{i}v\\ 0\end{array}\right]\end{array}& \left[\mathrm{Equation}5\right]\end{array}$ .sup.il.sub.d: DISTANCE BETWEEN IMAGE PLANE IMi AND DESTINATION pd IN DEPTH-AXIS DIRECTION OF i.sup.th CAMERA Ci (DEPTH DISTANCE)

(48) Further, the following relationship is also established.

(49) $\begin{array}{cc}\begin{array}{c}{}^{i}l-{}^{i}l_d=\left[001\right]\left\{{}^{i}P\left[\begin{array}{c}\mathrm{pdx}\\ \mathrm{pdy}\\ \mathrm{pdy}\\ 1\end{array}\right]-{}^{i}P\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\\ 1\end{array}\right]\right\}\\ =\left[001\right]{}^{i}A^i{R}^T\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\end{array}\right]\\ ={}^{i}P_{31}\mathrm{px}+{}^{i}P_{32}\mathrm{py}+{}^{i}P_{33}\mathrm{pz}\end{array}& \left[\mathrm{Equation}6\right]\end{array}$

(50) Then, the following equation is obtained from Equations 5 and 6.

(51) $\begin{array}{cc}\left[\begin{array}{c}{}^{i}u_d-{}^{i}u\\ \begin{array}{c}{}^{i}v_d-{}^{i}v\\ 0\end{array}\end{array}\right]=\frac{1}{{}^{i}l_d}\left[{}^{i}A^i{R}^T-{}^i\right]\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\end{array}\right]{}^i=\left[\begin{array}{ccc}{}^i{P}_{31}^{i}u& {}^i{P}_{32}^{i}u& {}^i{P}_{33}^{i}u\\ {}^i{P}_{31}^{i}v& {}^i{P}_{32}^{i}v& {}^i{P}_{33}^{i}v\\ {}^{i}P_{31}& {}^{i}P_{32}& {}^{i}P_{33}\end{array}\right]& \left[\mathrm{Equation}7\right]\end{array}$

(52) Here, a coefficient matrix i relating the positional deviations (i-i) expressed in the camera coordinate systems and the positional deviation (pdp) expressed in the task coordinate system is defined by the following equation.

(53) $\begin{array}{cc}{}_1={\left[{}^1\tilde{A}^1{\tilde{R}}^T\right]}^{-1}{I}_0^1{\tilde{E}}^{-1}{}_2={\left[{}^2\tilde{A}^2{\tilde{R}}^T\right]}^{-1}{I}_0^2{\tilde{E}}^{-1}{I}_0=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\end{array}\right]& \left[\mathrm{Equation}8\right]\end{array}$ .sup.iE: 22 MATRIX EXPRESSING LENS DISTORTION E OF i.sup.th CAMERA Ci

(54) Note that a matrix expressing a lens distortion E of the camera specifically satisfies the following equation.

(55) $\begin{array}{cc}\left[\begin{array}{c}{}^{i}u_{\mathrm{dt}}-{}^{i}u_t\\ {}^{i}v_{\mathrm{dt}}-{}^{i}v_t\end{array}\right]={}^{i}E\left[\begin{array}{c}{}^{i}u_d-{}^{i}u\\ {}^{i}v_d-{}^{i}v\end{array}\right]\left[\begin{array}{c}{}^{i}u_{\mathrm{dt}}-{}^{i}u_t\\ {}^{i}v_{\mathrm{dt}}-{}^{i}v_t\end{array}\right]\text{:}\mathrm{VALUE}\mathrm{EXPRESSING}\mathrm{POSITION}\mathrm{DEVIATION}p\mathrm{WHERE}\mathrm{EFFECT}\mathrm{OF}\mathrm{LENS}\mathrm{DISTORTION}\mathrm{IS}\mathrm{ELIMINATED}\mathrm{WITH}\mathrm{COORDINATE}\mathrm{SYSTEM}\mathrm{OF}\mathrm{CAMERA}\mathrm{Ci}& \left[\mathrm{Equation}9\right]\end{array}$

(56) The following equation relating the positional deviations (i-i) expressed in the camera coordinate systems of the two cameras C1, C2 and the positional deviation (pdp) expressed in the task coordinate system is obtained from Equations 7 and 8.

(57) 0$\begin{array}{cc}{}^1\tilde{l}_d{}_1\left[\begin{array}{c}{}^1{u}_d-{}^{1}u\\ {}^{1}v_d-{}^{1}v\end{array}\right]+{}^2\tilde{l}_d{}_2\left[\begin{array}{c}{}^2{u}_d-{}^{2}u\\ {}^{2}v_d-{}^{2}v\end{array}\right]=[{\frac{{}^1\tilde{l}_d}{{}^{1}l_d}\left[{}^1\tilde{A}^1{\tilde{R}}^T\right]}^{-1}{I}_0{\tilde{E}}^{-11}{\mathrm{EI}}_o^T\left[{}^{1}A^1{R}^T-{}^1\right]+{\frac{{}^2\tilde{l}_d}{{}^{2}l_d}\left[{}^2\tilde{A}^2{\tilde{R}}^T\right]}^{-1}{I}_0^1{\tilde{E}}^{-12}{\mathrm{EI}}_o^T\left[{}^{2}A^2{R}^T-{}^2\right]]\left[\mathrm{pd}-p\right]& \left[\mathrm{Equation}10\right]\end{array}$ .sup.i{tilde over (l)}.sub.d: ESTIMATE OF DEPTH DISTANCE .sup.il.sub.d TO DESTINATION pd

(58) Note that an estimate of a depth distance to the target position pd is used in Equation 10. This estimate can be an appropriate constant. Specifically, if the task coordinate system and the coordinate system of the camera Ci is sufficient distant, a distance between the origins of these coordinate systems may be set as this estimate. Alternatively, both the camera C1 and the target position pd may be captured in the field of view of the camera C2, the estimate of the camera C1 may be obtained from the imaging result of the camera C2 and the estimate of the camera C2 may be also similarly obtained from the imaging result of the camera C1.

(59) Further, a correction matrix is defined by the following equation.

(60) $\begin{array}{cc}{}_0={{[}^1{\tilde{A}}^1{\tilde{R}}^T]}^{-1}{I}_0{I}_0^T{[}^1{\tilde{A}}^1{\tilde{R}}^T{-}^1\stackrel{~}{}]+{{[}^2{\tilde{A}}^2{\tilde{R}}^T]}^{-1}{I}_0{I}_0^T{[}^2{\tilde{A}}^2{\tilde{R}}^T{-}^2\stackrel{~}{}].& \left[\mathrm{Equation}11\right]\end{array}$ .sub.0: CORRECTION MATRIX

(61) An inverse matrix of this correction matrix has a function of normalizing a coefficient multiplied to the positional deviation (pdp) on the right side of Equation 10. Accordingly, the product of the inverse matrix of the correction matrix and this coefficient is equal to an identity matrix. However, since each parameter has an error, this product includes, strictly speaking, a component corresponding to the position p and is expressed as follows using a matrix (p). Here, (p) is a matrix having a component corresponding to the position p and substantially equal to the identity matrix.

(62) $\begin{array}{cc}{}_0^{-1}\left[{\frac{{}^1{\tilde{l}}_d}{{}^1{l}_d}{[}^1{\tilde{A}}^1{\tilde{R}}^T]}^{-1}{I}_0^1{\tilde{E}}^{-11}{\mathrm{EI}}_o^T{[}^1{A}^1{R}^T{-}^1]+{\frac{{}^2{\tilde{l}}_d}{{}^2{l}_d}{[}^2{\tilde{A}}^2{\tilde{R}}^T]}^{-1}{I}_0^1{\tilde{E}}^{-12}{\mathrm{EI}}_o^T{[}^2{A}^2{R}^T{-}^2]\right]=\stackrel{~}{}\left(p\right)& \left[\mathrm{Equation}12\right]\end{array}$

(63) As a result, the following relationship is obtained from the relationships of Equations 10 and 12.

(64) $\begin{array}{cc}{}_0^{-11}{\tilde{l}}_d{}_1\left[\begin{array}{c}{}^1{u}_d{-}^{1}u\\ {}^1{v}_d{-}^{1}v\end{array}\right]+{}_0^{-11}{\tilde{l}}_d{}_2\left[\begin{array}{c}{}^2{u}_d{-}^{2}u\\ {}^2{v}_d{-}^{2}v\end{array}\right]=\stackrel{~}{}\left(p\right)\left[\mathrm{pd}-p\right]& \left[\mathrm{Equation}13\right]\end{array}$

(65) As just described, a positional deviation detection amount (=(p)(pdp)) corresponding to the positional deviation (pdp) expressed in the task coordinate system can be obtained from the positional deviations (1-1), (2-2) expressed in the respective coordinate systems of the two cameras C1, C2. Accordingly, in the external loop Lx, values obtained by applying operations 204, 205 respectively to the positional deviations (1-1), (2-2) in the coordinate systems of the cameras C1, C2 are added to calculate the positional deviation detection amount (=(p)(pdp)) in the task coordinate system as shown in FIG. 3.

(66) Note that, as described later, this embodiment has an advantage of being able to precisely move the end effector 4 to the target position pd without requiring highly accurate calibration. Thus, the correction matrix and the coefficient matrix may include an error in the operations 204, 205. Corresponding to this, the symbol is attached to these in the block diagram of FIG. 3.

(67) An operation 206 is performed to multiply the positional deviation detection amount by a transposed Jacobian matrix (transposed matrix of a Jacobian matrix) in the controller 5. Further, a proportional operation and an integral operation are performed on the positional deviation detection amount multiplied by the transposed Jacobin matrix in the controller 5. Specifically, the proportional operation is performed to multiply the positional deviation detection amount multiplied by the transposed Jacobian matrix by a proportional gain (operation 207). Further, the integral operation by time is performed on the positional deviation detection amount multiplied by the transposed Jacobian matrix (operation 208). Furthermore, an integral operation value is multiplied by an integral gain (operation 209). Then, the results of these operations 207, 209 are fed back to an input side of the robot dynamics 201 (i.e. torques ). The above is the operation of the external loop Lx.

(68) In the internal loop Lq, angle detection 210 is performed to detect the rotation angle q of the joint Q. Note that the rotation angle q detected by this angle detection possibly includes an error. Corresponding to this, in the angle detection 210 of FIG. 3, a detection value of the rotation angle q is expressed as an output value of an error function c(q). This error function can be expressed by a linear function of (a true value of) the rotation angle q such as (q)=c1q+c2. Further, in the internal loop Lq, a derivative operation and a proportional operation are performed on the detected value (q) of the rotation angle q. Specifically, the derivative operation by time is performed on the detected value (q) (operation 211). Further, the derivative operation value is multiplied by a derivative gain (operation 212). Furthermore, the proportional operation 213 is performed to multiply the deviation q from the target angle qd by a proportional gain. The results of these operations are fed back to the input side of the robot dynamics 201 (i.e. torques ). The above is the operation of the internal loop Lq.

(69) The controller 5 determines the torques by adding a gravitational force compensation term g(q) to the operation results of these external and internal loops Lx, Lq. Here, the gravitational force compensation term g(q) is equivalent to a torque necessary to make the robot 2 stationary against a gravitational force. As a result, the torques to be applied to the joints Q are given by the following equation.

(70) $\begin{array}{cc}=\tilde{g}\left({\tilde{q}}_d\right)+G_P\left({\tilde{q}}_d-\tilde{q}\right)-G_D\stackrel{.}{\stackrel{~}{q}}+G_{\mathrm{Pc}}{\tilde{J}}^T\left(\tilde{q}\right)\stackrel{~}{}\left(\tilde{p}\right)\left[\mathrm{pd}-p\right]+G_I{}_{\mathrm{to}}^t{\tilde{J}}^T\left(\tilde{q}\right)\stackrel{~}{}\left(\tilde{p}\right)\left[\mathrm{pd}-p\right]& \left[\mathrm{Equation}14\right]\end{array}$ G.sub.P: PROPORTIONAL GAIN FOR ANGULAR DEVIATION q G.sub.D: DERIVATIVE GAIN FOR ANGLE q G.sub.Pc: PROPORTIONAL GAIN FOR POSITIONAL DEVIATION p G.sub.I: INTEGRAL GAIN FOR POSITIONAL DEVIATION p {tilde over (J)}.sup.T: MATRIX CONTAINING OF ESTIMATE OF TRANSPOSED JACOBIAN MATRIX (TRANSPOSED MATRIX OF JACOBIAN MATRIX) t: TIME to: CONTROL START TIME

(71) In Equation 14, the first term is the gravitational force compensation term, the second term is a term for executing a proportional control on the angular deviations q, the third term is a term for executing a derivative control on the rotation angles q, the fourth term is a term for executing a proportional control on the positional deviation p, and the fifth term is a term for executing an integral control on the positional deviation p. Note that although an integral interval is a time from a control start time t.sub.0 in Equation 14, the integral interval is not limited to this and can be appropriately changed. A specific example is as follows. If the end effector 4 starts moving from a position outside the fields of view of the cameras C1, C2, a time at which the end effector 4 enters the fields of view of the both cameras C1, C2 and the visual feedback starts properly functioning may be set as a starting point of the integral interval.

(72) As described above, in this embodiment, the end effector 4 is imaged while the target position pd of the end effector 4 is included in the fields of view, and the rotation angles q of the joints Q are detected (first step). Then, the torques based on the positional deviation p of the end effector 4 (torques given by the fifth term of Equation 14) are calculated based on the imaging results and the torques based on the angular deviations q of the joints Q (torques given by the second term of Equation 14) are calculated (second step). Then, the motors M1 to M3 are controlled to apply these torques to the joints Q (third step). In this way, these torques are superimposed and applied to the joints Q. In such a configuration, the rotation angles q of the joints Q can be detected even if the positional deviation p of the end effector 4 cannot be obtained from the imaging results. Thus, the end effector 4 is brought closer to the target position pd by rotating the joints Q by the torques based on the angular deviations q. However, in a configuration where the torques based on the angular deviations q and the torques based on the positional deviation p are superimposed and applied to the joints Q, if there is an error in a kinematic operation, the rotation of the joints Q may stop before the end effector 4 reaches the target position pd and the end effector 4 may stop at a position deviated from the target position pd.

(73) Contrary to this, in this embodiment, the torque based on the positional deviation p to be applied to the joints Q while being superimposed with the torques based on the angular deviations q is a positional deviation integral torque obtained by performing the integral operation on the value corresponding to the positional deviation p. Thus, if the rotation of the joints Q stops or is about to stop before the end effector 4 reaches the target position pd because of an error in the kinematic operation, the positional deviation integral torque increases with time to rotate the joints Q that have stopped or are about to stop. Then, this positional deviation integral torque keeps the joints Q rotating to move the end effector 4 to the target position pd until the positional deviation is finally eliminated. Thus, in this embodiment, even if there is an error in the kinematic operation, the end effector 4 can be reliably moved to the target position pd by the function of the positional deviation integral torque. As a result, highly accurate calibration is not required and loads of calibration can be reduced.

(74) Particularly, in this embodiment, the positional deviation integral torque is calculated by the kinematic operation of performing the integral operation on the positional deviation p multiplied by the transposed Jacobian matrix. Contrary to this, according to this embodiment, the end effector 4 can be reliably moved to the target position pd even if there is an error in the kinematic operation. Thus, even if the transposed Jacobian matrix is uncertain and there is an error in the kinematic operation of obtaining the positional deviation integral torque, the end effector 4 can be reliably moved to the target position pd. Therefore, it is not particularly necessary to highly accurately carry out calibration to precisely obtain the transposed Jacobian matrix and loads of calibration can be reduced.

(75) The above technical content may be understood as follows. That is, in the configuration where the torques based on the angular deviations q and torques based on the positional deviation p are superimposed and applied to the joints Q, the both torques are balanced out to stop the rotation of the joints Q if there is an error in the kinematic operation. This is thought to be because a potential distribution in the task space has a minimum value at a position different from the target position pd and the end effector 4 falls to this minimum value if there is an error in the kinematic operation. Contrary to this, in the case of applying the above positional deviation integral torque to the joints Q, the end effector 4 can be moved to the target position pd by the action of the positional deviation integral torque that increases with time. Thus, even in the case where the minimum value is located at a position different from the target position pd due to an error in the kinematic operation, the end effector 4 can be reliably moved to the target position pd. As a result, highly accurate calibration is not required and loads of calibration can be reduced.

(76) According to this embodiment, if the end effector 4 has the positional deviation p, the positional deviation integral torque acts on the joints Q and the end effector 4 can be moved to the target position pd. In the above description, an error in the kinematic operation is cited as a cause of this positional deviation p. However, in this embodiment, regardless of a cause of the positional deviation p of the end effector 4, the end effector 4 is moved to the target position pd to eliminate the positional deviation p if there is any positional deviation p. That is, positional deviations p due to the uncertainty of the term for gravitational force compensation and the values of the target angles qd, detected rotation angles q or the like can be eliminated by the action of the positional deviation integral torque. Thus, only if the positional deviation (p=pdp) of the end effector 4 is obtained, it is possible to move the end effector 4 to the target position pd. As a result, there are few parameters required to be accurate, therefore loads of calibration are very light and calibration can be omitted in some cases.

(77) As just described, in the first embodiment, the robot system 1 corresponds to an example of a robot system of the invention, the robot 2 corresponds to an example of a robot of the invention, the controller 5, the cameras Ci and the encoders En function in cooperation as an example of a robot control apparatus of the invention, the controller 5 corresponds to an example of a computer of the invention, the recording medium 6 corresponds to an example of a recording medium of the invention, the program 7 corresponds to an example of a program of the invention, the encoders En correspond to an example of an angle detector of the invention, the controller 5 functions as examples of a torque calculator and a drive controller of the invention, and the motors Mn correspond to an example of a joint drive mechanism of the invention. Further, in Equation 14, the torque given by the fifth term corresponds to an example of a first application amount or a first torque of the invention, the torque given by the second term corresponds to an example of a second application amount or a second torque of the invention and the torque given by the third term corresponds to an example of a third application amount or a third torque of the invention.

(78) Note that the invention is not limited to the first embodiment described above and various changes other than the aforementioned ones can be made without departing from the gist of the invention. For example, in the above first embodiment, the mark attached to the end effector 4 is recognized as the position of the end effector 4. However, a characteristic part (e.g. tip, hole) of the end effector 4 or a characteristic part of a target (e.g. bolt) gripped by the end effector 4 may be recognized as the position of the end effector 4.

(79) Further, a case where the end effector 4 is moved to one target position pd is described in the above first embodiment. However, in the case of moving the end effector 4 in consideration of the posture thereof, three representative points may be set for the end effector 4 and the three points may be moved to target positions thereof. Specifically, when positions of the three points set on the end effector 4 are denoted by x, y and z and the target positions thereof are denoted by zd, yd and zd, positional deviations of the three points are given by x(=xdx), y(=ydy) and z(=zdz). A positional deviation integral torque similar to the above one may be calculated for each of the positional deviations x, y and z and applied to the joints Q.

(80) Further, a control law expressed by Equation 14 can also be appropriately changed. In a specific example, a change to omit the first term for gravitational force compensation, the third term for executing the derivative control on the rotation angles q or the fourth term for executing the proportional control on the positional deviation p can be made.

(81) Further, panning/tilting of the cameras Ci is controlled to bring the target position pd into coincidence with or proximity to the origins of the coordinate systems of the cameras Ci (centers of the image planes IMi). However, the coincidence of the target position pd and the origins of the coordinate systems of the cameras Ci is not always necessary.

(82) Further, it is not necessary to constantly execute the above control to superimpose the external loop Lx and the internal loop Lq and feed them back to the torques. For example, if the end effector 4 is outside the fields of view of the cameras Ci, a feedback amount from the external loop Lx may be set at zero and the external loop Lx may not be performed. After the end effector 4 enters the fields of view of the cameras Ci, the external loop may be performed.

(83) On this occasion, an operation until the end effector 4 enters the fields of view of the cameras Ci may be taught to the robot 2. Such teaching suffices to be rough since it is sufficient to move the end effector 4 into the fields of view of the cameras Ci. However, it is, of course, all right to execute a control to track a trajectory of the end effector 4 until entry into the fields of view of the cameras Ci and locate the end effector 4 in the fields of view of the cameras Ci.

(84) Alternatively, it is also possible not to perform the external loop Lx and to use only the internal loop Lq while the end effector 4 is outside a predetermined range from the target position pd even if the end effector Ci enters the fields of view of the cameras Ci. In this case, the external loop Lx may be performed after the end effector 3 enters the predetermined range from the target position pd. In such a configuration, the end effector 4 can be highly accurately positioned with the external loop Lx at a final stage of a movement to the target position pd.

(85) Second Embodiment

(86) FIG. 4 is a diagram showing an example of a robot system according to a second embodiment of the invention. As shown in FIG. 4, the robot system 1 includes a robot 2 and two cameras C1, C2. Note that, in FIG. 1, the robot 2 is schematically shown by symbol notation of joints Q1 to Q6, links 30 and an end effector 4 and the cameras C1, C2 are schematically shown by being represented by image planes IM1, IM2 thereof.

(87) The robot 2 has such a schematic configuration that the end effector 4 (tool) is attached to the tip of an arm 3 which includes freely rotatable joints Q1 to Q6 and moves according to the rotation of the joints Q1 to Q6. Specifically, the joints Q1 to Q6 are coupled in this order via the links 30 from a base side (other end side) toward a tip side (one end side) of the robot 2 and one link 30 further projects toward the tip side from the joint Q6. The arm 3 is configured by the joints Q1 to Q6 and the links 30 in this way. Then, the end effector 4 is attached to the tip of the arm 3 (tip of the link 30 projecting toward the one end side from the joint Q6). Note that the joint Q6 closest to the tip side out of six joints Q1 to Q6 coupled to each other via the links 30 is appropriately called a tip joint.

(88) Each of the six joints Q1 to Q6 can rotate with one degree of freedom. Accordingly, the arm 3 can move the end effector 4 attached to the tip with six degrees of freedom by changing rotation angles q1 to q6 of the joints Q1 to Q6. Particularly, in this robot system 1, a reference point p1 is set on the tip joint Q6 or in a range of the base end side from the tip joint Q6, whereas reference points p2, p3 are set in a range of the end effector 4 side from the tip joint Q6. More specifically, in an example shown in FIG. 4, the reference point p1 is set on the link 30 between the joints Q3 and Q4, whereas the reference points p2, p3 are set on the end effector 4. The rotation angles q1 to q6 of the joints Q1 to Q6 are adjusted based on detection results of the reference points p1 to p3 by the cameras C1, C2 and the location and posture of the end effector 4 in a three dimensions are controlled. On this occasion, marks such as LEDs (Light Emitting Diodes) may be attached to the respective reference points p1 to p3 to improve visibility in the cameras C1, C2.

(89) That is, the cameras C1, C2 are positioned to capture destinations pd1 to pd3 of the reference points p1 to p3 in fields of view thereof and capture positional deviations p1 to p3 between the reference points p1 to p3 and the destinations pd1 to pd3 in mutually different planes (p1=pd1p1, p2=pd2p2, p3 =pd3p3 ). Specifically, the cameras C1, C2 are, for example, so arranged that the image planes IM1, IM2 are perpendicular to each other, a YZ plane of a task coordinate system XYZ is imaged by the camera C1 and a ZX plane of the task coordinate system XYZ is imaged by the camera C2. The location and posture of the end effector 4 in the three dimensions are controlled by adjusting the rotation angles q1 to q6 of the joints Q1 to Q6 to reduce the positional deviations p1 to p3 between the reference points p1 to p3 and the destinations pd1 to pd3 detected from imaging results of the cameras C1, C2 (visual feedback).

(90) In this visual feedback, the position control can be executed by using different degrees of freedom for the reference point p1 set on the link 30 between the joints Q3 and Q4 and for the reference points p2, p3 set on the end effector 4. As described above, the reference point p1 is set on the link 30 between the joints Q3 and Q4. In other words, the set position of the reference point p1 (specific reference position) is on the base side from the joint Q4 (specific joint), at which the total count is three when counting the degree of freedom of the joint in order from the tip side, and on the tip side from the joint Q3, at which the total count is three when counting the degree of freedom of the joint in order from the base side. Thus, the position of the reference point p1 can be controlled with three degrees of freedom realized by the joints Q1 to Q3, whereas the positions of the reference points p2, p3 can be controlled with three degrees of freedom realized by the joints Q4 to Q6. In this way, it is possible to execute a control to separately use the degrees of freedom for the position control of the reference point p1 (three degrees of freedom by the joints Q1 to Q3) and the degrees of freedom for the position control of the reference points p2, p3 (three degrees of freedom by the joints Q4 to Q6).

(91) Further, in this visual feedback, it is possible to adopt such a configuration that an imaging result of the robot 2 at a singular point is not fed back to the position control of the end effector 4. In a specific example, the cameras C1, C2 may be so arranged that the robot 2 at the singular point is outside the fields of view of the cameras C1, C2. In this way, it can be suppressed that the visual feedback is performed based on the imaging result of the robot 2 at the singular point and the control of the robot 2 becomes unstable.

(92) Here, the notation of coordinate systems and each control amount used in the position control of the end effector 4 is described. As shown in FIG. 4, the three-dimensional task coordinate system configured by X, Y and Z axes perpendicular to each other with the Z axis as a vertical axis is defined for a task space where the end effector 4 operates. Thus, the positions of the reference points p1 to p3 are respectively given by three-dimensional vectors (p1x, p1y, p1z), (p2x, p2y, p2z) and (p3 x, p3 y, p3 z). Similarly, the destinations pd1 to pd3 are respectively given by three-dimensional vectors (pd1x, pd1y, pd1z), (pd2x, pd2y, pd2z) and (pd3x, pd3y, pd3z). Further, the positional deviations p1 to p3 are respectively given by three-dimensional vectors (p1x, p1y, p1z), (p2x, p2y, p2z) and (p3 x, p3 y, p3 z).

(93) Note that the representative notation of the three reference points p1 to p3 by the reference points p without distinction, the representative notation of the three destinations pd1 to pd3 by the reference points pd without distinction and the representative notation of the three positional deviations pd1 to pd1 by the positional deviations pd without distinction are used as appropriate below. On this occasion, the positional deviation p is given by an equation p=pdp using the reference point p and the destination pd. Further, corresponding to this, the position of the reference point p is expressed by a three-dimensional vector (px, py, pz), the position of the destination pd is expressed by a three-dimensional vector (pdx, pdy, pdz) and the deviation p is expressed by a three-dimensional vector (px, py, pz).

(94) The rotation angles q of the joints Q of the robot 2 are expressed by a vector (q1, q2, q3, q4, q5, q6) including the rotation angle qn of the joint Qn as each component. Here, the notation of joints Q is the collective notation of the joints Q1 to Q6 and n is a number for distinguishing the joint (n=1, 2, 3, 4, 5, 6). Further, target angles qd (=qd1, qd2, qd3, qd4, qd5, qd6) are the rotation angles q of the joints Q when all the reference points p1 to p3 coincide with the corresponding destinations pd1 to pd3 thereof. Furthermore, torques applied to the joints Q of the robot 2 are expressed by a vector (1, 2, 3, 4, 5, 6) including a torque n acting on the joint Qn as each component.

(95) The above is description of the notation of the coordinate systems and the control amounts. Next, the position control of the end effector 4 is described in detail. The robot system 1 according to the second embodiment also has the electrical configuration shown in FIG. 2. A position control similar to the one for converging the position p of the end effector 4 to the target position pd in the first embodiment is executed for each of the reference points p1 to p3 (p) and the reference points p1 to p3 (p) are converged toward the destinations pd1 to pd3 (pd) thereof. The following description is made, centering on a configuration different from the first embodiment with the description of the configuration common to the first embodiment omitted as appropriate.

(96) In the robot system 1, a motor Mn for driving the joint Qn is provided for each of the joints Q1 to Q6, and an encoder En for detecting a rotational position of the motor Mn is provided for each of the motors M1 to M6. A controller 5 adjusts the rotation angles q of the joints Q of the robot 2 by controlling each of the motors M1 to M6. Particularly, to perform the aforementioned visual feedback, the controller 5 detects the positional deviations p (=pdp) of the reference points p from the imaging results of the reference points p by the cameras Ci (external sensors). On this occasion, the controller 5 detects the positional deviations p while controlling panning/tiling of the cameras Ci so that the destinations pd1 to pd3 are captured in the fields of view with bringing, e.g. geometric centers of gravity of the destinations pd1 to pd3 into coincidence with or proximity to the origins of the coordinate systems of the cameras Ci (centers of the image planes IMi). Note that the set positions of the reference points p on the robot 2 are stored in advance in a memory of the controller 5 (storage).

(97) Further, in parallel with the detection of the positional deviations p of the reference points p, the controller 5 detects angular deviations q (=qdq) of the joints Q from outputs of the encoders E1 to E6 (internal sensors). Then, the controller 5 calculates the torques based on the positional deviations p (=pdp) and the angular deviations q (qdq). Then, the motors M1 to M6 apply the torques to the joints Q, thereby adjusting the rotation angles q of the joints Q. As just described, the detection results of the cameras Ci and the encoders En are fed back to the torques i to control the position of the end effector 4 in this embodiment.

(98) Also in the second embodiment, the position control of the end effector 4 is executed in accordance with the block diagram of FIG. 3. Accordingly, the torques to be applied to the joints Q are determined based on the control law of Equation 14 derived from Equations 1 to 13. Note that, as described above, three reference points p1 to p3 are provided for the robot 2. Thus, the term for executing the proportional control on the positional deviations p (fourth term of Equation 14) is given by a linear combination of terms for executing a proportional control on each positional deviation p1 to p3 , and the terms for executing the integral control on the positional deviations p (fifth term of Equation 14) is given by a linear combination of terms for executing an integral control on each positional deviation p1 to p3. As a result, an equation for obtaining the torques applied to the joint Q can be rewritten from Equation 14 into the following equation.

(99) $\begin{array}{cc}=\tilde{g}\left({\tilde{q}}_d\right)+G_p\left({\tilde{q}}_d-\tilde{q}\right)-G_D\stackrel{.}{\stackrel{~}{q}}+G_{\mathrm{pc}}\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{pd}}_j-p_j\right)+G_I{}_{\mathrm{to}}^t\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{pd}}_j-p_j\right)t\underset{j=1}{\overset{3}{.Math.}}{}_j=1& \left[\mathrm{Equation}15\right]\end{array}$ j=1, 2, 3 p.sub.j: REFERENCE POINT P1, P2, P3 p.sub.dj: DESTINATION Pd1, Pd2, Pd3 G.sub.pc: POSITIONAL FEEDBACK GAIN MATRIX G.sub.I: INTEGRAL FEEDBACK GAIN MATRIX .sub.j>0: WEIGHT COEFFICIENT FOR REFERENCE POINT P.sub.j {tilde over (J)}.sub.j.sup.T: TRANSPOSED JACOBIAN MATRIX (TRANSPOSED MATRIX OF JACOBIAN MATRIX) CONTAINING ERROR FOR REFERENCE POINT P.sub.j {tilde over ()}.sub.j: MATRIX {tilde over ()} (p) FOR REFERENCE POINT p.sub.j

(100) Note that the fourth and fifth terms of Equation 15 are given by weighted averages of the result of (proportional/integral) operations on each positional deviation p1 to p3. Weight coefficients 1 to 3 of these weighted averages have a positive value greater than zero when the corresponding reference points p1 to p3 are in the fields of view of the both cameras C1, C2 and are zero when the corresponding reference points p1 to p3 are outside the field of view of either one of the cameras C1, C2 and the visual feedback for the reference points p1 to p3 outside the field(s) of view does not work.

(101) As described above, in this embodiment, the position of the end effector 4 is controlled by moving three or more reference points p1 to p3 , which is set for the robot 2 and includes at least two points p2, p3 set for the end effector 4 side from the tip joint Q6, toward the respective destinations pd1 to pd3. Specifically, the reference points p1 to p3 are imaged in a state where the destinations pd1 to pd3 of the respective reference points p1 to p3 are captured in the fields of view and the positional deviations p1 to p3 between the reference points p1 to p3 and the destinations pd1 to pd3 are obtained. In parallel with this, the rotation angles q1 to q6 of the joints Q1 to Q6 are detected and the angular deviations q between the detected angles q and the target angles qd are obtained. Then, the torques based on the positional deviations p1 to p3 (torques given by the fifth term of Equation 14 or 15) are calculated and the torques based on the angular deviations q (torques given by the second term of Equation 14 or 15) are calculated. Then, these torques are superimposed and applied to the joints Q. In such a configuration, even if the positional deviations p1 to p3 of the reference points p1 to p3 cannot be obtained from the imaging results, the rotation angles q of the joints Q can be detected. Thus, the joints Q are rotated by the torques based on the angular deviations q to move the reference points p1 to p3 toward the destinations pd1 to pd3, as a result, the end effector 4 can be moved toward the target position.

(102) However, as described above, in the configuration where the torques based on the positional deviations p and the torques based on the angular deviations q are superimposed and applied to the joints Q, unless a kinematic operation is certain, the torques based on the positional deviations p may act on the torques based on the angular deviations q in an opposite direction to stop the rotation of the joints Q. In this case, the end effector 4 stops with the positional deviations p left.

(103) Contrary to this, in this embodiment, the torques to be superimposed with the torques based on the angular deviations p are positional deviation integral torques obtained by performing the integral operation on values corresponding to the positional deviations p of the reference points p. Accordingly, if the rotation of the joints Q stops or are about to stop before the reference points preach the destinations pd due to an existence of an error in the kinematic operation, the positional deviation integral torques increase with time to rotate the joints Q that have stopped or are about to stop since the positional deviations p remain. These positional deviation integral torques keep the joints Q rotating to move the reference points p to the destination pd until the positional deviations p are finally eliminated. As a result, the end effector 4 can be moved to the target position. Thus, in this embodiment, even if there is an error in the kinematic operation, the end effector 4 can be reliably moved to the target position by the function of the torques based on the positional deviations p. As a result, highly accurate calibration is not required and loads of calibration can be reduced.

(104) Particularly, in this embodiment, the positional deviation integral torques are calculated by the kinematic operation of performing the integral operation on the positional deviations p multiplied by the transposed Jacobian matrix. Contrary to this, according to this embodiment, the end effector 4 can be reliably moved to the target position even if there is an error in the kinematic operation. Thus, even if the transposed Jacobian matrix is uncertain and there is an error in the kinematic operation of obtaining the positional deviation integral torques, the end effector 4 can be reliably moved to the target position. Therefore, it is not particularly necessary to highly accurately carry out calibration to precisely obtain the transposed Jacobian matrix and loads of calibration can be reduced.

(105) The above technical content can be understood as follows. That is, in the configuration where the torques based on the angular deviations q and the torques based on the positional deviations p are superimposed and applied to the joints Q, the both torques are balanced out to stop the rotation of the joints Q if there is an error in the kinematic operation. This is thought to be because a potential distribution in a task space has a minimum value at a position different from the target position and the end effector 4 falls to this minimum value if there is an error in the kinematic operation. Contrary to this, in the case of applying the above positional deviation integral torques to the joints Q, the end effector 4 can be moved to the target position by the action of the positional deviation integral torques that increase with time. Thus, even in the case where the minimum value is located at a position different from the target position due to an error in the kinematic operation, the end effector 4 can be reliably moved to the target position. As a result, highly accurate calibration is not required and loads of calibration can be reduced.

(106) According to this embodiment, if there is the positional deviation p between the reference point p and the destination pd, the positional deviation integral torque acts on the joints Q and the end effector 4 can be moved to the target position. In the above description, an error in the kinematic operation is cited as a cause of these positional deviations p. However, in this embodiment, regardless of a cause of the positional deviation p, the reference point p is moved to the destination pd to eliminate the positional deviation p if there is any positional deviation p. That is, positional deviations p due to the uncertainty of the term for gravitational force compensation and the values of the target angles qd, detected rotation angles q or the like can be eliminated by the action of the positional deviation integral torque. Thus, only if the positional deviation (p=pdp) of the reference point p is obtained, the end effector 4 can be moved to the target position by converging the reference point p toward the destination pd. As a result, there are few parameters required to be accurate, therefore loads of calibration are very light and calibration can be omitted in some cases.

(107) Further, in this embodiment, the position of the end effector 4 is controlled by moving three or more reference points p1 to p3 , which is set for the robot 2 and includes at least two points p2, p3 set for the end effector 4, toward the respective destinations pd1 to pd3. As a result, it is possible to properly control the end effector 4 to the target location and posture in the three dimensions.

(108) In the configuration where three or more reference points p1 to p3 are moved to the destinations pd1 to pd3, there may occur a problem of being difficult to converge all the reference points p1 to p3 to the respective destinations pd1 to pd3 thereof depending on a positional relationship of the set positions of the reference points p1 to p3 and the destinations pd1 to pd3. Particularly, such a problem is likely to occur when all the reference points p1 to p3 are set on the end effector 4 side from the tip joint Q6. This point is described, giving a specific example.

(109) FIG. 5 is a diagram showing an example of states of torques acting on the end effector in the process of moving the reference points to the destinations thereof. That is, a positional relationship between the reference points p1 to p3 and the destinations pd1 to pd3 possibly established in the process of moving the reference points p1 to p3 to the destinations pd1 to pd3 is illustrated in FIG. 5. If a state of FIG. 5 is reached, a torque T1 generated based on the positional deviation p1 and directed from the reference point p1 to the destination pd1, a torque T2 generated based on the positional deviation p2 and directed from the reference point p2 to the destination pd2 and a torque T3 generated based on the positional deviation p3 and directed from the reference point p3 to the destination pd3 are balanced out each other, therefore the end effector 4 stops. In other words, the end effector 4 falls in a local minimum in a potential distribution of a task space with the positional deviations p1 to p3 left. For this, in the configuration where all the reference points p1 to p3 are set on the end effector 4 side from the tip joint Q6, degrees of freedom for moving all the reference points p1 to p3 are common, therefore it has been difficult to move the end effector 4 out of the local minimum.

(110) Contrary to this, in this embodiment, the specific reference point p1 is set on the tip joint Q6 or on the other end side from the tip joint Q6. In the case of providing some reference point(s) (specific reference point p1) on the tip joint Q6 or on the other end side from the tip joint Q6, it is easy to destroy a balance of the torques T1 to T3 as described above, move the end effector 4 out of the local minimum and converge all the reference points p1 to p3 to the destinations pd1 to pd3 thereof.

(111) Particularly, in this embodiment, the specific reference point p1 is set on the specific joint Q4 or on other end side (base side) from the specific joint Q4, at which the total count is equal to or more than three when counting the degree of freedom of the joint in order from the one end side (tip side). In such a configuration, the specific reference point p1 can be properly converged to the destination pd1 with the degrees of freedom on the other end side from the specific joint Q4 (degrees of freedom by the joints Q1 to Q3). Further, the reference point p2, p3 set on the end effector 4 side from the specific reference point p1 can be properly converged to the destination pd2, pd3 with three or more degrees of freedom on the specific joint Q4 and on the one end side from the specific joint Q4 (degrees of freedom by the joints Q4 to Q6). That is, the control using different degrees of freedom for the specific reference point p1 and for the other reference points p2, p3 can be executed. As a result, even if the end effector 4 falls in the local minimum as described above, the balance of the torques T1 to T3 as described above can be easily destroyed by displacing the reference point p1 and the reference points p2, p3 respectively with different degrees of freedom. As a result, it becomes easy to move the end effector 4 out of the local minimum and properly converge the respective reference points p1 to p3 to the destinations.

(112) Further, in this embodiment, the specific reference point p 1 is set on the one end side (tip side) from the joint Q3, at which the total count is equal to or more than three when counting the degree of freedom of the joint in order from the other end side (base side). This enables the specific reference point p1 to be more reliably converged to the destination pd1 with three or more degrees of freedom. Particularly, in such a configuration, the reference point p 1 and the reference points p2, p3 can be respectively relatively freely displaced with different three or more degrees of freedom. As a result, the balance of the torques T1 to T3 as described above can be easily destroyed and it becomes easy to move the end effector 4 out of the local minimum and properly converge the respective reference points p1 to p3 to the destinations.

(113) Note that, in this specification, an expression that the joint at which the total count is equal to or more than N when counting the degree of freedom of the joint in order is used as appropriate. This expression indicates the first joint at which the total count is equal to or more than N when counting the degree of freedom of the joint in order. Thus, as described above, the joint at which the total count is equal to or more than three when counting the degree of freedom of the joint in order from the one end side is only the joint Q4, and the other joints Q1 to Q3 are not considered as such. Similarly, the joint having a successive total of three or more degrees of freedom of the joints from the other end side is only the joint Q3, and the other joints Q4 to Q6 are not considered as such.

(114) As shown in Equation 15, the positional deviation integral torques are given by the linear combination of the values of integral on the positional deviations p1 to p3 of the respective reference points p1 to p3 , particularly by the weight averages multiplied by the weight coefficients j (1, 2, 3). On this occasion, the weight coefficient of the value of integral on the specific reference point p1 (value of integral on the positional deviation p1) may be set larger than those 2, 3 of the values of integral on the reference points p2, p3 (values of integral on the positional deviations p2, p3 ) (1>2 and 1>3). In such a configuration, after the specific reference point p1 corresponding to the larger weight coefficient 1 is more quickly converged to the destination pd1, the reference points p2, p3 set on the end effector 4 side from the tip joint Q6 can be converged to the destinations pd2, pd3. Thus, the degrees of freedom on the specific joint Q4 and on the one end side from the specific joint Q4, i.e. degrees of freedom on the end effector 4 side from the specific reference point p1 can be substantially used only to move the reference points p2, p3. Therefore, these reference points p2, p3 can be reliably converged to the destinations pd2, pd3 with sufficient degrees of freedom.

(115) As described above, in the second embodiment, the robot system 1 corresponds to an example of the robot system of the invention, the robot 2 corresponds to an example of the robot of the invention, the controller 5, the cameras Ci and the encoders En function in cooperation as an example of the robot control apparatus of the invention, the controller 5 corresponds to an example of the computer of the invention, the recording medium 6 corresponds to an example of the recording medium of the invention, and the program 7 corresponds to an example of the program of the invention. Further, the cameras Ci correspond to an example of an imager of the invention, the encoders En correspond to an example of a displacement amount detector of the invention, the controller 5 functions as examples of a positional deviation acquirer, an application amount calculator and the drive controller of the invention. Further, in Equations 14 and 15, the torques given by the fifth term correspond to an example of the first application amount of the invention, the torques given by the second term corresponds to an example of the second application amount of the invention and the torques given by the third term corresponds to an example of the third application amount of the invention. Further, the reference points p1 to p3 correspond to an example of reference positions of the invention, the specific reference point p1 corresponds to a specific reference position of the invention, the tip joint Q6 corresponds to an example of a tip joint of the invention, and the specific joint Q4 corresponds to an example of a specific joint of the invention.

(116) As described above, the respective reference points p are reliably converged to the destinations pd by superimposing the torques obtained by performing the integral operation on the positional deviations p and the torques obtained by performing the proportional operation on the angular deviations q. Note that the invention is not limited to the second embodiment described above and various changes other than the aforementioned ones can be made without departing from the gist of the invention. For example, in the above second embodiment, the marks such as LEDs attached to the robot 2 are set as the reference points p1 to p3. However, characteristic parts (e.g. tips, holes) of the robot 2 may be set as the reference points p1 to p3.

(117) Further, the control law expressed by Equation 15 can also be appropriately changed. In a specific example, a change to omit the first term for gravitational force compensation, the third term for executing the derivative control on the rotation angles q or the fourth term for executing the proportional control on the positional deviations p can be made.

(118) Further, specific set values of the weight coefficients 1 to 3 of weighted addition in Equation 15 can also be appropriately changed. Accordingly, the weight coefficients for the other reference points p2, p3 may be set larger than the reference point 1 for the specific reference point p1 or all the weight coefficients 1 to 3 may be set at the same value.

(119) Further, the set position of the specific reference point p1 on the robot 2 can also be appropriately changed. Thus, the specific reference point p1 can be set at a suitable position on the tip joint Q6 or on the base end side from the tip joint Q6 on the robot 2. For example, the specific reference point p1 may be set on the tip joint Q6 or on the specific joint Q4.

(120) Further, the set positions of the reference points p2, p3 on the robot 2 can also be appropriately changed. Thus, the reference point p2 or p3 may be provided on the link 30 projecting toward the tip side from the tip joint Q6.

(121) Furthermore, it is not always necessary to provide the specific reference point p1 as described above. Thus, for example, all the reference points p1 to p3 may be provided on the end effector 4.

(122) Further, the number of the reference points p is not limited to three as in the above description. Thus, four or more reference points p may be set for the robot 2.

(123) Further, the number of degrees of freedom of the robot 2 is also not limited to six as in the above description. Thus, the invention can be applied also to a robot 2 having seven or more degrees of freedom.

(124) Further, in the above second embodiment, any of the joints Q1 to Q6 has one degree of freedom. However, the invention can be applied also to a robot 2 configured using joint Q having a plurality of degrees of freedom.

(125) Further, in the above second embodiment, panning/tiling of the cameras Ci is controlled to bring, for example, the geometric centers of gravity of the destinations pd1 to pd3 into coincidence with or proximity to the origins of the coordinate systems of the cameras Ci (centers of the image planes IMi). However, a control mode of the positional relationship between the destinations pd1 to pd3 and the coordinate systems of the cameras Ci is not limited to this.

(126) Further, in the above embodiments, the feedback amount from the external loop Lx becomes zero and the external loop Lx for the corresponding reference point p is not performed if the reference point p is outside the fields of view. After the corresponding reference point enters the fields of view of the cameras Ci, the external loop Lx is performed. On this occasion, an operation until the reference point p enters the fields of view of the cameras Ci may be taught to the robot 2 in advance. Such teaching suffices to be rough since it is sufficient to move the reference point p into the fields of view of the cameras Ci. However, it is, of course, all right to execute a control to track a trajectory of the end effector 4 until entry into the fields of view of the cameras Ci and locate the end effector 4 in the fields of view of the cameras Ci.

(127) Alternatively, it is also possible not to perform the external loop Lx while the reference point p is outside a predetermined range from the destination pd and to use only the internal loop Lq even if the reference point p enters the fields of view of the cameras Ci. In this case, the external loop Lx may be performed after the reference point p enters the predetermined range from the destination pd. In such a configuration, the reference point p can be highly accurately positioned with the external loop Lx at a final stage of a movement to the destination pd.

(128) Stability

(129) Next, the stability of the control law given by Equation 15 is studied. Robot dynamics are generally given by the following equation.
R(q){umlaut over (q)}+({dot over (R)}(q)+S(q,{dot over (q)})){dot over (q)}+g(q)=[Equation 16] R(q): (33) INERTIA MATRIX S(q,{dot over (q)}): (33) ALTERNATE MATRIX OF NONLINEAR TERM g(q): (31) VECTOR SHOWING GRAVITY TERM : (31) VECTOR SHOWING JOINT TORQUE q: (31) VECTOR SHOWING ROTATION ANGLE OF JOINT

(130) Here, a variable is defined as follows.

(131) $\begin{array}{cc}=\frac{\tilde{g}\left({\tilde{q}}_d\right)-g\left(q_d\right)+G_p\left({\tilde{q}}_d-q_d\right)}{g_I}+{}_{\mathrm{to}}^t\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{pd}}_j-p_j\right)t& \left[\mathrm{Equation}17\right]\end{array}$ DEFINED AS G.sub.I=g.sub.iI I: IDENTITY MATRIX

(132) The following equation is obtained by substituting Equation 17 into Equation 16.

(133) $\begin{array}{cc}R\left(q\right)\stackrel{\mathrm{.Math.}}{q}=-\left(\frac{1}{2}\stackrel{.}{R}\left(q\right)+S\left(q,\stackrel{.}{q}\right)\right)\stackrel{.}{q}-g\left(q\right)+g\left(q_d\right)+G_p\left(q_d-q\right)-G_D\stackrel{.}{q}+G_{\mathrm{pc}}\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{pd}}_j-p_j\right)+g_I& \left[\mathrm{Equation}18\right]\end{array}$

(134) Note that it is assumed that the following conditions constantly hold.

(135) $\begin{array}{cc}.Math.{\mathrm{pd}}_j-p_j.Math.c_1.Math.K.Math.c_2{G}_p=g_{p}I{G}_D=g_{D}I{G}_{\mathrm{pc}}=g_{\mathrm{pc}}I{G}_I=g_{I}I\mathrm{rank}\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j=6& \left[\mathrm{Equation}19\right]\end{array}$ C.sub.1, C.sub.2: POSITIVE CONSTANT K={tilde over (J)}.sub.j.sup.T{tilde over ()}.sub.j; : EUCLIDEAN NORM

(136) Here, the following function V is considered as a Lyapunov function candidate.

(137) $\begin{array}{cc}V=\frac{1}{2}{\left(q_d-q\right)}^T{G}_p\left(q_d-q\right)+\underset{j=1}{\overset{3}{.Math.}}\frac{1}{2}{\left({\mathrm{pd}}_j-p_j\right)}^T\left({}_j{G}_{\mathrm{pc}}+{}_j{G}_D\right)\left({\mathrm{pd}}_j-p_j\right)+\frac{1}{2}{\stackrel{.}{q}}^{T}R\left(q\right)\stackrel{.}{q}+\frac{1}{2}{g}_I{}^T-\underset{j=1}{\overset{3}{.Math.}}{{}_j\left({\mathrm{pd}}_j-p_j\right)}^T{\stackrel{~}{}}_j^T{\tilde{J}}_{j}R\left(q\right)\stackrel{.}{q}+{g_I\left(q_d-q\right)}^T& \left[\mathrm{Equation}20\right]\end{array}$

(138) V>0 holds if is properly small and g.sub.I is sufficiently smaller than G.sub.P.

(139) Further, a time derivative of the function V is given by the following equation.

(140) 0$\begin{array}{cc}\stackrel{.}{V}=-{\stackrel{.}{q}}^T{G}_p\left(q_d-q\right)-\underset{j=1}{\overset{3}{.Math.}}{\stackrel{.}{p}}_j\left(k_p{}_{j}I+G_D\right)\left({\mathrm{pd}}_j-p_j\right)+{\stackrel{.}{q}}^{T}R\left(q\right)\stackrel{\mathrm{.Math.}}{q}+\frac{1}{2}{\stackrel{.}{q}}^T\stackrel{.}{R}\left(q\right)\stackrel{.}{q}+g_I{\stackrel{.}{}}^T-\underset{j=1}{\overset{3}{.Math.}}{{}_j\left({\mathrm{pd}}_j-p_j\right)}^T{\stackrel{~}{}}_j{\tilde{J}}_{j}R\left(q\right)\stackrel{\mathrm{.Math.}}{q}+\underset{j=1}{\overset{3}{.Math.}}{}_j{\stackrel{.}{p}}_j^T{\stackrel{~}{}}_j{\tilde{J}}_{j}R\left(q\right)\stackrel{.}{q}-\underset{j=1}{\overset{3}{.Math.}}{{}_j\left({\mathrm{pd}}_j-p_j\right)}^T\frac{}{t}\left({\stackrel{~}{}}_j{\tilde{J}}_{j}R\left(q\right)\right)\stackrel{.}{q}-g_I{\stackrel{.}{q}}^T+{g_I\left(q_d-q\right)}^T\stackrel{.}{}& \left[\mathrm{Equation}21\right]\end{array}$

(141) By transforming Equation 21, the time derivative of the function V is given by the following equation.

(142) $\begin{array}{cc}\stackrel{.}{V}=-g_D{\stackrel{.}{q}}^T\stackrel{.}{q}-g_{\mathrm{pc}}{}^T-\left(g_p-g_I\right){\left(q_d-q\right)}^T+g_{\mathrm{pc}}{\stackrel{.}{q}}^T\underset{j=1}{\overset{3}{.Math.}}{}_j\left({\tilde{J}}_j^T{\stackrel{~}{}}_j-{\tilde{J}}_j^T\right)\left({\mathrm{pd}}_j-p_j\right)+g_D{\stackrel{.}{q}}^T\underset{j=1}{\overset{3}{.Math.}}{}_j\left({\tilde{J}}_j^T{\stackrel{~}{}}_j-{\tilde{J}}_j^T\right)\left({\mathrm{pd}}_j-p_j\right)+{\stackrel{.}{q}}^T\left(g\left(q_d\right)-g\left(q\right)\right)-{}^T\left(g\left(q_d\right)-g\left(q\right)\right)+{}^T\left(\frac{1}{2}\stackrel{.}{R}+S\right)\stackrel{.}{q}+\underset{j=1}{\overset{3}{.Math.}}{}_j{\stackrel{.}{p}}_j^T{\stackrel{~}{}}_j^T{\tilde{J}}_{j}R\left(q\right)\stackrel{.}{q}-\underset{j=1}{\overset{3}{.Math.}}{{}_j\left({\mathrm{pd}}_j-p_j\right)}^T\frac{}{t}\left({\stackrel{~}{}}_j^T{\tilde{J}}_{j}R\left(q\right)\right)\stackrel{.}{q}& \left[\mathrm{Equation}22\right]\end{array}$

(143) Here, a variable is defined as follows.

(144) $\begin{array}{cc}=\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{pd}}_j-p_j\right)& \left[\mathrm{Equation}23\right]\end{array}$

(145) On this occasion, the following inequality is obtained for the time derivative of the function V.

(146) $\begin{array}{cc}\stackrel{.}{V}-g_D{\stackrel{.}{q}}^T\stackrel{.}{q}-g_{\mathrm{pc}}{}^T+g_{\mathrm{pc}}\underset{j=1}{\overset{3}{.Math.}}{}_j{\stackrel{.}{q}}^T\left({\tilde{J}}_j^T{\stackrel{~}{}}_j-J_j^T\right)\left({\mathrm{pd}}_j-p_j\right)+g_D\underset{j=1}{\overset{3}{.Math.}}{}_j{\stackrel{.}{q}}^T\left({\tilde{J}}_j^T{\stackrel{~}{}}_j-J_j^T\right)\left({\mathrm{pd}}_j-p_j\right)-\left(g_D-g_I\right){\left(q_d-q\right)}^T+\left(c_1+c_3\right){.Math.\stackrel{.}{q}.Math.}^2+\left(c_2+c_4\right){.Math.q_d-q.Math.}^2& \left[\mathrm{Equation}24\right]\end{array}$

(147) The following relationship is utilized in obtaining the above inequality.

(148) $\begin{array}{cc}{\stackrel{.}{q}}^T\left(g\left(q_d\right)-g\left(q\right)\right)c_1{.Math.\stackrel{.}{q}.Math.}^2+c_2{.Math.q_d-q.Math.}^2{}^T(\frac{1}{2}\stackrel{.}{R}\left(q\right)+S\left(q,\stackrel{.}{q}\right)\stackrel{.}{q}+\underset{j=1}{\overset{3}{.Math.}}{\stackrel{.}{p}}_j^T{\stackrel{~}{}}_j^T{\tilde{J}}_{j}R\left(q\right)\stackrel{.}{q}-\underset{j=1}{\overset{3}{.Math.}}{\left({\mathrm{pd}}_j-p_j\right)}^T\frac{}{t}\left({\stackrel{~}{}}_j^T{\tilde{J}}_{j}R\left(q\right)\right)\stackrel{.}{q}{c}_3{.Math.\stackrel{.}{q}.Math.}^2{}^T\left(g\left(q_d\right)-g\left(q\right)\right)c_4{.Math.q_d-q.Math.}^2& \left[\mathrm{Equation}25\right]\end{array}$ C.sub.1, C.sub.2, C.sub.3, C.sub.4: POSITIVE CONSTANT

(149) The following equation is obtained by Taylor expansion about the target angle qd.

(150) $\begin{array}{cc}{p}_j\left(q\right)=p_j\left(q_d\right)+\frac{p_j}{q}\left(q-q_d\right)+{}_j\left(q-q_d\right)& \left[\mathrm{Equation}26\right]\end{array}$

(151) Further, the following equation is obtained from Equation 26.
pd.sub.jp.sub.j=(Jd.sub.j+.sub.j)(q.sub.dq)
Jd.sub.j=J.sub.j(q)[Equation 27] .sub.j: HIGH-ORDER COEFFICIENT AT TAYLOR EXPANSION

(152) Further, it is assumed that the following equation holds.
Jd.sub.j.sub.jC.sub.5
{tilde over (J)}.sub.j.sup.T{tilde over ()}.sub.jJ.sub.j.sup.TC.sub.[Equation 28] C.sub.5, C.sub.: POSITIVE CONSTANT

(153) On this occasion, the following inequality is obtained for the second term of Equation 24 for giving the time derivative of the function V.

(154) $\begin{array}{cc}{k}_p\underset{j=1}{\overset{3}{.Math.}}{}_j{\stackrel{.}{q}}^T\left({\tilde{J}}_j^T{\stackrel{~}{}}_j-J_j^T\right)\left({\mathrm{pd}}_j-p_j\right)=k_p\underset{j=1}{\overset{3}{.Math.}}{\stackrel{.}{q}}^T\left({\tilde{J}}_j^T{\stackrel{~}{}}_j-J_j^T\right)\left({\mathrm{Jd}}_j+{}_j\right)\left(q_d-q\right)\frac{3}{2}{k}_p{c}_{\mathrm{.Math.}}{c}_5\left({.Math.\stackrel{.}{q}.Math.}^2+{.Math.q_d-q.Math.}^2\right)& \left[\mathrm{Equation}29\right]\end{array}$

(155) Further, the following inequality is obtained for the third term of Equation 24 for giving the time derivative of the function V.

(156) $\begin{array}{cc}\underset{j=1}{\overset{3}{.Math.}}{}_j{\stackrel{.}{q}}^T\left(G_D{\tilde{J}}_j^T{}_j-J_j^T{G}_D\right)\left({\mathrm{pd}}_j-p_j\right)\frac{3}{2}{g}_D{c}_{\mathrm{.Math.}}{c}_5\left({.Math.\stackrel{.}{q}.Math.}^2+{.Math.q_d-q.Math.}^2\right)& \left[\mathrm{Equation}30\right]\end{array}$

(157) On this occasion, it is important that the following equation can hold in Equation 24 so that the time derivative of the function V becomes negative.
(q.sub.dq).sup.T0[Equation 31]

(158) Here, the following equation holds.

(159) $\begin{array}{cc}\begin{array}{c}{\left(q_d-q\right)}^T={\left(q_d-q\right)}^T\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{pd}}_j-p_j\right)\\ ={\left(q_d-q\right)}^T\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{Jd}}_j+{}_j\right)\left(q_d-q\right)\end{array}& \left[\mathrm{Equation}32\right]\end{array}$

(160) Thus, a condition of Equation 31 can be transformed into the following condition.

(161) $\begin{array}{cc}\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{Jd}}_j+{}_j\right)>0& \left[\mathrm{Equation}33\right]\end{array}$

(162) Finally, the following inequality is obtained for the time derivative of the Lyapunov function.

(163) 0$\begin{array}{cc}\stackrel{.}{V}-g_{\mathrm{pc}}{.Math..Math.}^2-\left[\left(1-\frac{3}{2}{c}_{\mathrm{.Math.}}{c}_5\right)g_D-c_1-c_3-\frac{3}{2}{g}_{\mathrm{pc}}{c}_{\mathrm{.Math.}}{c}_5\right]{.Math.\stackrel{.}{q}.Math.}^2-\left[\left(g_p-g_I\right){}_{\min }\left(\frac{+{}^T}{2}\right)-c_2-c_4-\frac{3}{2}\left(g_{\mathrm{pc}}+g_D\right)c_{\mathrm{.Math.}}{c}_5\right]{.Math.q_d-q.Math.}^2\mathrm{DEFINED}\mathrm{AS}=\underset{j=1}{\overset{3}{.Math.}}{\tilde{J}}_j^T{\stackrel{~}{}}_j\left(J_{\mathrm{dj}}+{}_j\right)& \left[\mathrm{Equation}34\right]\end{array}$ .sub.minA: MINIMAL EIGENVALUE OF MATRIX A

(164) On this occasion, the following equation is given as in Equation 33.

(165) $\begin{array}{cc}\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{Jd}}_j+{}_j\right)>0& \left[\mathrm{Equation}35\right]\end{array}$

(166) Accordingly, if the left side of Equation 35 is a positive definite matrix and Equation 35 is satisfied, the time derivative of the function V can be negative and the respective reference points p1 to p3 are stable at the destinations pd1 to pd3 thereof in the control law given by Equation 15. Thus, a setting mode and stability of the reference points p1 to p3 are considered next.

(167) In the above second embodiment, the reference point p1 is set between the joints Q3 and Q4 and the reference points p2, p3 are set on the end effector 4 for the robot 2 having six degrees of freedom by coupling the joints Q1 to Q6. Since the joints Q4 to Q6 are not used to move the reference point p1 in this case, the following equation is obtained.
{dot over (p)}.sub.1=J.sub.1{dot over (q)}=[A.sub.1O]{dot over (q)}[Equation 36] A.sub.1: 33 MATRIX O: 33 MATRIX WITH ALL IT'S COMPONENT BEING ZERO [A.sub.1O]: 36 MATRIX

(168) Accordingly, the left side of Equation 35 can be given by the following equation.

(169) $\begin{array}{cc}\underset{j=1}{\overset{3}{.Math.}}{}_j{\tilde{J}}_j^T{\stackrel{~}{}}_j\left({\mathrm{Jd}}_j+{}_j\right)={}_1\left[\begin{array}{c}{\tilde{A}}_1^T\\ O\end{array}\right]{\stackrel{~}{}}_1\left[A_1+{}_{1}O\right]+\left[\begin{array}{c}{\tilde{A}}_2^T\\ {\tilde{B}}_2^T\end{array}\right]{\stackrel{~}{}}_2\left[\begin{array}{cc}{A}_2+{}_2& B_2+{}_2\end{array}\right]+\left[\begin{array}{c}{\tilde{A}}_3^T\\ {\tilde{B}}_3^T\end{array}\right]{\stackrel{~}{}}_3\left[\begin{array}{cc}{A}_3+{}_3& B_3+{}_3\end{array}\right]& \left[\mathrm{Equation}37\right]\end{array}$

(170) Here, it is assumed that the following equation is satisfied.
{tilde over ()}.sub.j1
.sub.j0[Equation 38]

(171) On this occasion, Equation 37 is equal to the following equation.

(172) $\begin{array}{cc}\left[\begin{array}{cc}{}_1{\tilde{A}}_1^T{A}_1+{}_2{\tilde{A}}_2^T{A}_2+{\tilde{A}}_3^T{A}_3,& {}_2{\tilde{A}}_2^T{B}_2+{}_3{\tilde{A}}_3^T{B}_3\\ {}_2{\tilde{B}}_2^T{A}_2+{}_3{\tilde{B}}_3^T{A}_3,& {}_2{\tilde{B}}_2^T{B}_2+{}_3{\tilde{B}}_3^T{B}_3\end{array}\right]& \left[\mathrm{Equation}39\right]\end{array}$