ROBOT CONTROL APPARATUS, AND METHOD AND PROGRAM FOR OBTAINING ANGLE OF JOINT OF ROBOT

Abstract

In a method of obtaining an angle of each joint of a 6-axis vertical articulated robot when a position and a posture of an end effector attached on a sixth axis are given, a predetermined amount of offset exists between a sixth axis and a fourth axis, and the method includes sequentially determining a point of interest, which is a point on a circumference of a circle having the predetermined amount as a radius, around a first intersection point, on a plane which includes the first intersection point which is an intersection point of the sixth axis and the fifth axis and the plane which is orthogonal to the sixth axis, calculating a second intersection point, which is an intersection point of the fourth axis and the third axis, when it is assumed that the point of interest is an intersection point of the fifth axis and the fourth axis, calculating an inner product value of a first vector directed from the calculated second intersection point to the point of interest and a second vector directed from the point of interest to the first intersection point, and estimating that the point of interest, when an absolute value of the inner product value is less than or equal to a predetermined threshold, is an intersection point of the fifth axis and the fourth axis.

Claims

1. A robot control apparatus for obtaining an angle of each joint of a 6-axis vertical articulated robot when a position and a posture of an end effector attached on a sixth axis are given, wherein, in the 6-axis vertical articulated robot, the sixth axis of a sixth joint and a fifth axis of a fifth joint perpendicularly intersect each other, the fifth axis and a fourth axis of a fourth joint perpendicularly intersect each other, the fourth axis and a third axis of a third joint perpendicularly intersect each other, and there is a predetermined amount of offset between the sixth axis and the fourth axis, the robot control apparatus comprising: a determiner to sequentially determine a point of interest, which is a point on a circumference of a circle having the predetermined amount as a radius, around a first intersection point, on a plane which includes the first intersection point which is an intersection point of the sixth axis and the fifth axis and the plane which is orthogonal to the sixth axis; a first calculator to calculate a second intersection point, which is an intersection point of the fourth axis and the third axis, when it is assumed that the point of interest is an intersection point of the fifth axis and the fourth axis; a second calculator to calculate an inner product value of a first vector directed from the second intersection point calculated by the first calculator to the point of interest and a second vector directed from the point of interest to the first intersection point; and an estimator to estimate that the point of interest, when an absolute value of the inner product value is less than or equal to a predetermined threshold, is an intersection point of the fifth axis and the fourth axis.

2. A method of obtaining an angle of each joint of a 6-axis vertical articulated robot when a position and a posture of an end effector attached on a sixth axis are given, wherein, in the 6-axis vertical articulated robot, the sixth axis of a sixth joint and a fifth axis of a fifth joint perpendicularly intersect each other, the fifth axis and a fourth axis of a fourth joint perpendicularly intersect each other, the fourth axis and a third axis of a third joint perpendicularly intersect each other, and there is a predetermined amount of offset between the sixth axis and the fourth axis, the method comprising: sequentially determining a point of interest, which is a point on a circumference of a circle having the predetermined amount as a radius, around a first intersection point, on a plane which includes the first intersection point which is an intersection point of the sixth axis, and the fifth axis and the plane which is orthogonal to the sixth axis; calculating a second intersection point which is an intersection point of the fourth axis and the third axis when it is assumed that the point of interest is an intersection point of the fifth axis and the fourth axis; calculating an inner product value of a first vector directed from the second intersection point to the point of interest and a second vector directed from the point of interest to the first intersection point; and estimating that the point of interest, when an absolute value of the inner product value is less than or equal to a predetermined threshold, is an intersection point of the fifth axis and the fourth axis.

3. A non-transitory computer-readable medium containing a computer program that causes a computer to perform a method of obtaining an angle of each joint of a 6-axis vertical articulated robot when a position and a posture of an end effector attached on a sixth axis are given, wherein, in the 6-axis vertical articulated robot, the sixth axis of a sixth joint and a fifth axis of a fifth joint perpendicularly intersect each other, the fifth axis and a fourth axis of a fourth joint perpendicularly intersect each other, the fourth axis and a third axis of a third joint perpendicularly intersect each other, and there is a predetermined amount of offset between the sixth axis and the fourth axis, the method comprising: sequentially determining a point of interest, which is a point on a circumference of a circle having the predetermined amount as a radius, around a first intersection point, on a plane which includes the first intersection point which is an intersection point of the sixth axis and the fifth axis and the plane which is orthogonal to the sixth axis; calculating a second intersection point which is an intersection point of the fourth axis and the third axis when it is assumed that the point of interest is an intersection point of the fifth axis and the fourth axis; calculating an inner product value of a first vector directed from the second intersection point to the point of interest and a second vector directed from the point of interest to the first intersection point; and estimating that the point of interest, when an absolute value of the inner product value is less than or equal to a predetermined threshold, is an intersection point of the fifth axis and the fourth axis.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0007] FIG. 1 is a schematic view showing a configuration of a robot system of a robot according to an example embodiment of the present disclosure.

[0008] FIG. 2 is a front view of a robot according to an example embodiment of the present disclosure in a state in which a second arm is lowered.

[0009] FIG. 3 is a front view of a robot according to an example embodiment of the present disclosure in a state in which the second arm is raised.

[0010] FIG. 4 is a view showing a link coordinate system of a robot according to an example embodiment of the present disclosure.

[0011] FIG. 5 is a perspective view of a modeled robot according to an example embodiment of the present disclosure in a case in which the robot has a certain posture.

[0012] FIG. 6 is a perspective view of a modeled robot according to an example embodiment of the present disclosure in a case in which the robot has another posture.

[0013] FIG. 7 is a perspective view of a modeled reference robot in a case in which the robot has a certain posture.

[0014] FIG. 8 is a perspective view of the modeled reference robot in a case in which the robot has another posture.

[0015] FIG. 9 is a view showing a relationship between rotation axes of joints of the reference robot of FIG. 8.

[0016] FIG. 10 is a view for describing an algorithm of analysis of inverse kinematics of the reference robot of FIG. 8.

[0017] FIG. 11 is a view showing a relationship between rotation axes of joints of the robot according to the example embodiment of FIG. 6.

[0018] FIG. 12 is a view for describing an algorithm of analysis of inverse kinematics of a robot according to an example embodiment of the present disclosure.

[0019] FIG. 13 is a block diagram showing a configuration of a robot system according to an example embodiment of the present disclosure.

[0020] FIG. 14 is a functional block diagram of a robot control apparatus according to an example embodiment of the present disclosure.

[0021] FIG. 15 is a flowchart executed by a robot control apparatus according to an example embodiment of the present disclosure.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0022] Hereinafter, robot systems including robot control apparatuses according to example embodiments of the present disclosure will be described.

[0023] In the following description, unless otherwise indicated, points refer to points in a world coordinate system (robot coordinate system).

[0024] First, a configuration of a robot system 1 according to a first embodiment will be described with reference to FIGS. 1 to 3. FIG. 1 is a schematic view showing a configuration of the robot system 1 according to the present embodiment. FIG. 2 is a front view of a robot according to the present embodiment in a state in which a second arm (to be described below) is lowered. FIG. 3 is a front view of the robot according to the present embodiment in a state in which the second arm is raised.

[0025] A robot R according to the present embodiment is an articulated robot which is used for assembling or manufacturing predetermined products and is installed and used, for example, on an assembly line or a production line.

[0026] As shown in FIGS. 2 and 3, the robot R includes a plurality of joints and a plurality of arms. In an example of the present embodiment, the robot R includes six joints 11 to 16, a first arm 21, a second arm 22, and an end effector attaching portion 18. Although not shown in the drawings, each of the joints includes a motor, a decelerator connected to the motor, a position detecting mechanism for detecting a rotational position of the motor, and a circuit board on which the motor and the position detecting mechanism are electrically connected.

[0027] Further, the robot R includes a support member B constituting a base end portion of the robot R. The support member B is relatively rotatably connected to a first joint 11. The first arm 21 and the second arm 22 are formed to have a thinly elongated longitudinal shape. Specifically, each of the first arm 21 and the second arm 22 is formed to have a thinly elongated cylindrical shape.

[0028] In the robot R, the following fixed or connected relationships are established.

[0029] (i) The first joint 11 and a second joint 12 are relatively rotatably connected to each other.

[0030] (ii) The second joint 12 and a first end 211 of the first arm 21 are fixed to each other.

[0031] (iii) A second end 212 of the first arm 21 and a third joint 13 are fixed to each other.

[0032] (iv) The third joint 13 and a fourth joint 14 are relatively rotatably connected to each other.

[0033] (v) The fourth joint 14 and a first end 221 of the second arm 22 are relatively rotatably connected to each other.

[0034] (vi) A second end 222 of the second arm 22 and a fifth joint 15 are fixed to each other.

[0035] (vii) The fifth joint 15 and a sixth joint 16 are relatively rotatably connected to each other.

[0036] (viii) The sixth joint 16 and the end effector attaching portion 18 are relatively rotatably connected to each other.

[0037] An end effector such as a hand or a tool may be attached to the end effector attaching portion 18.

[0038] Hereinafter, in order to describe a mechanism of the robot R according to the present embodiment, Denavit-Hartenberg (DH) parameters are shown as link parameters of the robot R according to the present embodiment, and forward kinematics of the robot R according to the present embodiment will be described with reference to FIG. 4 as a reference. FIG. 4 is a view showing a link coordinate system of the robot R according to the embodiment.

[0039] In the robot R shown in FIGS. 2 and 3, when it is assumed that intersection points of rotation axes (an intersection point of Z-axes) set by the respective joints are intersection points O.sub.1 to O.sub.5 and an installation position of an end effector is a point O.sub.6, Link1 to Link6 coordinate systems, which are link coordinate systems having points O.sub.1 to O.sub.6 as original points, are shown in FIG. 4. In FIG. 4, the Link1 to Link6 coordinate systems are denoted by L.sub.1 to L.sub.6, respectively. Further, the link shown in FIG. 4 corresponds to a posture of the robot R in FIG. 3.

[0040] As shown in FIG. 4, in the robot R according to the present embodiment, there is an offset of an interlink distance d.sup.5 between the intersection point O.sub.4 and the intersection point O.sub.5.

[0041] The DH parameters of the robot R are as shown in Table 1 below. In FIG. 4, since an equation, d.sub.2=d.sub.3, is established with respect to interlink distances d.sub.2 and d.sub.3, the interlink distances d.sub.2 and d.sub.3 may be zero on the DH parameters.

TABLE-US-00001 TABLE 1 Interlink Interlink Link Link Twist Coordinate Distance d Angle Length a Angle System [mm] [deg] [mm] [deg] Link1 d.sub.1 90 + .sub.1 0 90 Coordinate System (L.sub.1) Link2 0 90 + .sub.2 a.sub.2 0 Coordinate System (L.sub.2) Link3 0 90 + .sub.3 0 90 Coordinate System (L.sub.3) Link4 d.sub.4 .sub.4 0 90 Coordinate System (L.sub.4) Link5 d.sub.5 .sub.5 0 90 Coordinate System (L.sub.5) Link6 d.sub.6 .sub.6 0 0 Coordinate System (L.sub.6)

[0042] A homogeneous transformation matrix T.sub.1 for the Link1 coordinate system L.sub.1 from a world coordinate diameter is obtained using the DH parameters as follows.

[00001]$\begin{array}{c}{T}_1=.Math.\left[\begin{array}{cccc}\cos \left({}_1+90\right)& -\sin \left({}_1+90\right)& 0& 0\\ \sin \left({}_1+90\right)& \cos \left({}_1+90\right)& 0& 0\\ 0& 0& 0& d_1\\ 0& 0& 0& 1\end{array}\right].Math.\\ .Math.\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \left({}_1\right)& -\sin \left({}_1\right)& 0\\ 0& \sin \left({}_1\right)& \cos \left({}_1\right)& 0\\ 0& 0& 0& 1\end{array}\right]\\ =.Math.\left[\begin{array}{cccc}-\sin \left({}_1\right)& 0& \cos \left({}_1\right)& 0\\ \cos \left({}_1\right)& 0& \sin \left({}_1\right)& 0\\ 0& 1& 0& d_1\\ 0& 0& 0& 1\end{array}\right]\end{array}$

[0043] Hereinafter, when it is assumed that equations, S.sub.n=sin (.sub.n) and C.sub.n=cos(.sub.n), are established, homogeneous transformation matrixes T.sub.1, T.sub.2, T.sub.3, T.sub.4, T.sub.5, and T.sub.6 among the respective link coordinate systems are as follows.

[00002]$T_1=\left[\begin{array}{cccc}-S_1& 0& C_1& 0\\ C_1& 0& S_1& 0\\ 0& 1& 0& d_1\\ 0& 0& 0& 1\end{array}\right].Math.$$T_2=\left[\begin{array}{cccc}-S_2& -C_2& 0& -a_2.Math.S_2\\ C_2& -S_2& 0& a_2.Math.C_2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$T_3=\left[\begin{array}{cccc}-S_3& 0& C_3& 0\\ C_3& 0& S_3& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$T_4=\left[\begin{array}{cccc}{C}_4& 0& -S_4& 0\\ S_4& 0& C_4& 0\\ 0& -1& 0& d_4\\ 0& 0& 0& 1\end{array}\right]$$T_5=\left[\begin{array}{cccc}{C}_5& 0& -S_5& 0\\ S_5& 0& -C_5& 0\\ 0& 1& 0& d_5\\ 0& 0& 0& 1\end{array}\right]$$T_6=\left[\begin{array}{cccc}{C}_6& -S_6& 0& 0\\ S_6& C_6& 0& 0\\ 0& 0& 1& d_6\\ 0& 0& 0& 1\end{array}\right]$

[0044] From the above, a homogeneous transformation matrix T.sub.ee of the end effector is calculated from the homogeneous transformation matrixes among the respective link coordinate systems using the following Equation 1. By applying Equation 1, a position and a posture of the end effector may be obtained when the rotation angle of each joint is known.

Tee=T.sub.1T.sub.2T.sub.3T.sub.4T.sub.5T.sub.6(1)

[0045] Next, a calculation algorithm (hereinafter, simply referred to as an algorithm according to the present embodiment) of analysis of inverse kinematics of the robot R according to the present embodiment will be described with reference to FIGS. 5 to 12.

[0046] Since the algorithm according to the present embodiment is based on a calculation algorithm of analysis of inverse kinematics of a reference robot, the reference robot will be described below in addition to the description of the robot R according to the present embodiment. Unlike the robot R according to the present embodiment, the reference robot refers to a robot (commonly known as a Programmable Universal Machine for Assembly (PUMA)-type robot) having no offset in a wrist, that is, a robot in which the interlink distance d.sub.3 in FIG. 4 is zero.

[0047] First, the robot R according to the present embodiment is modeled as shown in FIGS. 5 and 6.

[0048] FIGS. 5 and 6 are perspective views of a robot model which is modeled based on the joints and the links of the robot R according to the present embodiment and represent different postures of the robot R. The posture of the robot model according to the present embodiment in FIG. 5 is a posture similar to that of the robot R shown in FIG. 2.

[0049] In FIGS. 5 and 6, the robot model according to the present embodiment includes six joints J1 to J6, and axes (rotation axes) of the six joints J1 to J6 are axes Ax.sub.1 to Ax.sub.6, respectively. In the actual robot R shown in FIGS. 2 and 3, the joints J1 to J6 rotating around the axes Ax.sub.1 to Ax.sub.6 correspond to the joints 11 to 16, respectively.

[0050] An intersection point of the axis Ax.sub.3 and the axis Ax.sub.4 is an intersection point O.sub.3, an intersection point of the axis Ax.sub.4 and the axis Ax.sub.5 is an intersection point O.sub.4, and an intersection point of the axis Ax.sub.5 and the axis Ax.sub.6 is an intersection point O.sub.5. A point O.sub.6 is an installation position of an end effector on the axis Ax.sub.6.

[0051] As shown in FIG. 5, in the robot model according to the present embodiment, an offset d.sub.5 is provided between the axis Ax.sub.4 and the axis Ax.sub.6.

[0052] Meanwhile, FIGS. 7 and 8 are perspective views of a robot model (hereinafter, referred to as a reference robot model) which is modeled based on the joints and the links of the reference robot and represent different postures of the robot R. FIG. 7 shows the same posture as that in FIG. 5, and FIG. 8 shows the same posture as that in FIG. 6.

[0053] In FIGS. 7 and 8, the reference robot model includes six joints J1 to J6, and axes of the six joints J1 to J6 are axes Ax.sub.1 to Ax.sub.6 which are the same as those in the robot model according to the present embodiment. However, unlike the robot model according to the present embodiment, in the reference robot model, there is no offset between the axis Ax.sub.4 and the axis Ax.sub.6, and an intersection point O.sub.4 and an intersection point O.sub.5 coincide with each other.

[0054] Next, a calculation algorithm of analysis of inverse kinematics of the reference robot model will be described with reference to FIGS. 9 and 10. FIG. 9 is a view showing a relationship between the rotation axes of the joints of the reference robot model of FIG. 8. FIG. 10 is a view for describing an algorithm of the analysis of the inverse kinematics of the reference robot model of FIG. 8.

[0055] FIG. 9 shows a relationship between the axes of the respective joints of the reference robot model shown in FIG. 8 and intersection points. Further, an intersection point O.sub.1 is an intersection point of an axis Ax.sub.1 and an axis Ax.sub.2.

[0056] The reference robot model, which is a model of a PUMA-type robot, has the following features.

[0057] In the reference robot model, regardless of rotation angles .sub.1, .sub.2, .sub.3, .sub.5, and .sub.6 of the joints J1, J2, J3, J5, and J6, unless the joint J4 rotates, intersection points O.sub.1, O.sub.3, O.sub.4, and O.sub.5 and a point O.sub.6 are on the same plane. The same plane is hereinafter referred to as an arm plane. That is, as the joint J1 rotates, the arm plane itself rotates around the joint J1. However, unless the joint J4 rotates, the intersection point O.sub.1, O.sub.3, O.sub.4, and O.sub.5 and the point O.sub.6 are always on the arm plane. When the rotation angle .sub.4 of the joint J4 is changed, the point O.sub.6 at an installation position of the end effector deviates from the arm plane.

[0058] In the reference robot model, even when the rotation angle .sub.4 of the joint J4 is changed and the end effector deviates from the arm plane, a position of the intersection point O.sub.4, which is an intersection point of the axis Ax.sub.4, the axis Ax.sub.5, and the axis Ax.sub.6 on the arm plane, may be calculated when a position and a posture of the end effector are given. That is, since the point, which is moved by a distance d6 along the axis Ax.sub.6 from the point O.sub.6 at the installation position of the end effector, becomes a common point between the intersection point O.sub.4 and the intersection point O.sub.5, the position of the intersection point O.sub.4 may be easily calculated.

[0059] After the position of the intersection point O.sub.4 is calculated, the rotation angles of the joints J1 to J3 may be calculated from geometry on the arm plane as follows.

[0060] FIG. 10 is a view showing a positional relationship between the intersection point O.sub.4, O.sub.3, and O.sub.1 on the arm plane. In FIG. 10, a point P4 is an intersection point of a perpendicular line drawn from the intersection point O.sub.1 with respect to a perpendicular line drawn from the intersection point O.sub.4 onto a xy plane of a world coordinate system.

[0061] First, referring to FIG. 9, the rotation angle .sub.1 of the joint J1 may be calculated according to the following Equation 2 on the basis of O.sub.4.Math.x and O.sub.4.Math.y, which are x and y coordinates of the world coordinate system of the calculated intersection point O.sub.4.

[00003]$\begin{array}{cc}{}_1={\tan }^{-1}.Math.\frac{O_4.Math.y}{O_4.Math.x}& \left(2\right)\end{array}$

[0062] Further, in the arm plane shown in FIG. 10, a distance Lx between the intersection point O.sub.1 and the point P4 and a distance Ly between the intersection point O.sub.1 and the intersection point O.sub.4 may be calculated according to the following Equations 3 and 4 using O.sub.4.Math.z, which is coordinates of the world coordinate system of the intersection point O.sub.4.

Lx={square root over (O.sub.4.Math.x.sup.2+O.sub.4.Math.y.sup.2)}(3)

Ly={square root over (O.sub.4.Math.x.sup.2+O.sub.4.Math.y.sup.2+(O.sub.4.Math.zd.sub.1).sup.2)}(4)

[0063] Therefore, since angles .sub.a, .sub.b, and .sub.c may be geometrically calculated from FIG. 10, rotation angles .sub.2 and .sub.3 of the joint J2 and J3 may be calculated.

[0064] Next, an algorithm in the present embodiment will be described with reference to FIGS. 11 and 12. FIG. 11 is a view showing a relationship between the rotation axes of the joints of the robot R according to the present embodiment of FIG. 6. FIG. 12 is a view for describing an algorithm of analysis of inverse kinematics of the robot R according to the present embodiment.

[0065] In the robot model according to the present embodiment, since there is an offset between the axis Ax.sub.4 and the axis Ax.sub.6, the intersection point O.sub.4 and the intersection point O.sub.5 do not coincide. When the position and the posture of the end effector are given, since an interlink distance d6 is known, the intersection point O.sub.5 may be obtained from the point O.sub.6, but the intersection point O.sub.4 may not be obtained from the intersection point O.sub.5. This is because it is not possible to know in which direction the axis Ax.sub.5 is directed.

[0066] Therefore, convergence calculation is performed while estimation is performed on the position of the intersection point O.sub.4. It is considered that the intersection point O.sub.4 is positioned on a circumference of a circle (the circle is indicated by a circle C5 in FIGS. 11 and 12) having a radius d.sub.5 around an original point on a xy plane of a link coordinate system with the axis Ax.sub.5 as a z axis z.sub.5 (in FIG. 12).

[0067] As shown in FIG. 12, a point O.sub.4t on the circumference of the circle C5 is determined as a point of interest, and a candidate point O.sub.3t, which is an intersection point O.sub.3 when the point O.sub.4t of interest is taken as the intersection point O.sub.4, is obtained in the same manner as the algorithm of the reference robot model. In the robot model according to the present embodiment, since the arm plane is also determined by the intersection points O.sub.1, O.sub.3, and O.sub.4 in the same manner as that in the reference robot model, the candidate point O.sub.3t may be obtained according to the geometric relationship as shown in FIG. 10.

[0068] Since the axis Ax.sub.4 and the axis Ax.sub.5 are orthogonal to each other due to constraint conditions of the links of the robot R, it may be determined that the point O.sub.4t of interest is the intersection point O.sub.4 when an inner product value of a vector (an example of a first vector) directed from the candidate point O.sub.3t to the point O.sub.4t of interest and a vector (an example of a second vector) directed from the point O.sub.4t of interest to the intersection point O.sub.5 is zero. Therefore, the point O.sub.4t of interest is sequentially moved on the circumference of the circle C5, and processing is performed so as to search for a point at which the inner product value becomes zero. In addition, in consideration of a calculation time, when an absolute value of the inner product value is less than or equal to a predetermined threshold as a convergence condition, it may be estimated that the point O.sub.4t of interest is the intersection point O.sub.4.

[0069] After coordinates of the intersection point O.sub.4 are specified, the rotation angles .sub.1 to .sub.3 of the joints J1 to J3 may be calculated using the same algorithm as that used in the reference robot model.

[0070] Next, a configuration of a robot control apparatus 3 according to the present embodiment will be described with reference to FIG. 13.

[0071] FIG. 13 is a block diagram showing the configuration of the robot system according to the present embodiment.

[0072] As shown in FIG. 13, the robot control apparatus 3 includes a controller 31, a storage 32, and a communication unit 33.

[0073] The controller 31 includes a central processing unit (CPU), a read only memory (ROM), and a random access memory (RAM) The CPU receives a robot program from an external information processing apparatus (not shown), stores the robot program in the storage 32, and loads and executes the robot program in the RAM. Each function which will be described below is realized by the robot program executed by the CPU of the controller 31. The algorithm according to the present embodiment is included in the robot program.

[0074] The controller 31 generates a control pulse for operating each of the joints 11 to 16 by executing the robot program.

[0075] The storage 32 is a mass storage device such as a hard disk drive (HDD) or a solid state drive (SSD). The robot program, the above-described DH parameters (or the link parameters), and execution log data, which is an execution record of the robot program, are stored in the storage 32.

[0076] The communication unit 33 includes a communication interface circuit, which communicates with another communication device and receives the robot program. The communication unit 33 is communicably connected to, for example, a robot teaching device (not shown) operated by an operator, and receives the robot program from the robot teaching device.

[0077] As shown in FIG. 13, the robot R includes a motor driving circuit 101 and a motor 102, which are embedded into each of the joints 11 to 16. The motor driving circuit 101 drives the motor 102 in response to a command from the controller 31.

[0078] Next, functions realized by the robot program which is executed by the controller 31 of the robot control apparatus 3 will be described with reference to FIG. 14. FIG. 14 is a functional block diagram of the robot control apparatus 3 according to the present embodiment.

[0079] As shown in FIG. 14, blocks for functions realized by the robot control apparatus 3 include a point of interest determination unit 311, a first calculation unit 312, a second calculation unit 313, and an estimation unit 314.

[0080] As described above, the robot R according to the present embodiment is a 6-axis vertical articulated robot in which an axis Ax.sub.6 of a joint J6 and an axis Ax.sub.5 of a joint J5 perpendicularly intersect each other, the axis Ax.sub.5 and an axis Ax.sub.4 of a joint J4 perpendicularly intersect each other, the axis Ax.sub.4 and an axis Ax.sub.3 of a joint J3 perpendicularly intersect each other, and there is a predetermined amount of offset between the axis Ax.sub.6 and the axis Ax.sub.4. The controller 31 of the robot control apparatus 3 according to the present embodiment executes the robot program to obtain an angle of each joint of the robot R when the position and the posture of the end effector are given to the axis Ax.sub.6.

[0081] The point of interest determination unit 311 has a function of sequentially determining a point O.sub.4t of interest, which is a point on a circumference of a circle having the interlink distance d.sub.5 (an example of a predetermined amount) as a radius, around the intersection point O.sub.5, on a plane which includes the intersection point O.sub.5 (an example of a first intersection point) which is an intersection point of the axis Ax.sub.6 and the axis Ax.sub.5 and is orthogonal to the axis Ax.sub.6. The plane, which includes the intersection point O.sub.5 and is orthogonal to the axis Ax.sub.6, refers to a xy plane of the link coordinate system with the axis Ax.sub.5 as a Z axis. The controller 31 determines, for example, a point which is sequentially moved on the circumference of the circle C5 shown in FIGS. 11 and 12 in the same direction at a predetermined interval (pitch) as the point O.sub.4t of interest.

[0082] The first calculation unit 312 has a function of calculating a candidate point O.sub.3t (an example of a second intersection point), which is an intersection point of the axis Ax.sub.4 and the axis Ax.sub.3, when it is assumed that the point O.sub.4t of interest determined by the point of interest determination unit 311 is the intersection point O.sub.4 of the axis Ax.sub.5 and the axis Ax.sub.4. As shown in FIG. 10, the candidate point O.sub.3t, which is the intersection point O.sub.3 when it is assumed that the intersection point O.sub.4 is the point O.sub.4t of interest, may be obtained based on the geometric relationship on the arm plane.

[0083] The second calculation unit 313 has a function of calculating an inner product value of a first vector directed from the candidate point O.sub.3t calculated by the first calculation unit 312 to the point O.sub.4t of interest and a second vector directed from the point O.sub.4t of interest to the intersection point O.sub.5. Since coordinates of the point O.sub.4t of interest, the candidate point O.sub.3t, and the intersection point O.sub.5 in the robot coordinate system are known, the inner product value may be calculated.

[0084] The estimation unit 314 has a function of estimating that the point O.sub.4t of interest when an absolute value of the inner product value calculated by the second calculation unit 313 is less than or equal to a predetermined threshold is the intersection point O.sub.4 of the axis Ax.sub.5 and the axis Ax.sub.4.

[0085] Next, processing of the robot control apparatus 3, on which the algorithm according to the present embodiment is implemented, will be described with reference to a flowchart of FIG. 15.

[0086] In the robot program, values of the position and the posture of the end effector, which are required from teaching information of the robot, are described. The controller 31 of the robot control apparatus 3 calculates the intersection point O.sub.5 of the axis Ax.sub.6 and the axis Ax.sub.5 from the required values of the position and the posture of the end effector (S10). Since the point O.sub.6 at the installation position of the end effector, the direction of the axis Ax.sub.6, and the interlink distance d6 are known, the controller 31 may calculate coordinates of the intersection point O.sub.5.

[0087] Next, the controller 31 determines the point O.sub.4t of interest, which is a point on a circumference of a circle having the interlink distance d.sup.5 as a radius, around the intersection point O.sub.5 on a plane which includes the intersection point O.sub.5 (an example of the first intersection point) and the plane which is orthogonal to the axis Ax.sub.6 (S12). A setting start position of the point O.sub.4t of interest on the circumference of the circle C5 may be arbitrarily determined.

[0088] When the point O.sub.4t of interest is determined, the controller 31 calculates the candidate point O.sub.3t of the intersection point O.sub.3 on the basis of the geometric relationship on the arm plane shown in FIG. 10 (S14).

[0089] Next, the controller 31 calculates an inner product value of a vector directed from the candidate point O.sub.3t to the point O.sub.4t of interest and a vector directed from the point O.sub.4t of interest to the intersection point O.sub.5 (S16). When an absolute value of the obtained inner product value is less than or equal to a predetermined threshold (NO in operation S18), the controller 31 moves the point O.sub.4t of interest (i.e., changes the point O.sub.4t of interest) on the circumference of the circle C5 at a predetermined interval (pitch) and re-performs processing of operations S12 to S18.

[0090] When the inner product value obtained in operation S16 is less than or equal to the predetermined threshold (YES in operation S18), it may be determined that a line connecting the candidate point O.sub.3t to the point O.sub.4t of interest and a line connecting the point O.sub.4t of interest to the intersection point O.sub.5 are substantially orthogonal to each other and the constraint conditions of the links are satisfied. Therefore, the controller 31 determines that the candidate point O.sub.3t and the point O.sub.4t of interest when the inner product value is less than or equal to the predetermined threshold are the intersection point O.sub.3 and the intersection point O.sub.4 (S20).

[0091] Next, the controller 31 calculates the rotation angles .sub.1 to .sub.3 of the joints J1 to J3 on the basis of the geometric relationship on the arm plane shown in FIG. 10 based on the intersection point O.sub.3 and O.sub.4 determined in operation S20 (S22).

[0092] As described above, according to the robot control apparatus 3 according to the present embodiment, even when there is an offset (i.e., the interlink distance d.sub.5 in FIG. 4) in the wrist portion of the robot R, the rotation angle of each joint may be calculated by a relatively simple calculation when the position and the posture of the end effector are given. That is, it is possible to reduce a calculation load of inverse kinematics of the robot having an offset in the wrist portion of the robot R.

[0093] Although the embodiments of the robot control apparatus of the present disclosure have been described, the present disclosure is not limited to the above embodiments.

[0094] According to the above description, it is understood by those skilled in the art that a program for causing a computer to implement the functions described in the functional block diagram of FIG. 14 and a computer readable storage medium (including a nonvolatile storage medium), in which the program is recorded, are disclosed.

[0095] Features of the above-described preferred embodiments and the modifications thereof may be combined appropriately as long as no conflict arises.

[0096] While preferred embodiments of the present invention have been described above, it is to be understood that variations and modifications will be apparent to those skilled in the art without departing from the scope and spirit of the present invention. The scope of the present invention, therefore, is to be determined solely by the following claims.