ROBUST POSITION CONTROL SYSTEM OF FLEXIBLE JOINT ROBOTS

Abstract

A system for controlling a flexible joint robot includes a control input dimension conversion part configured to receive a control input of a robot and convert the Cartesian coordinate system of the control input to the rotating workspace coordinate system; an entire flexible joint robot dynamics part configured to receive the control input and a disturbance and output a control output by multiplying the control input reflecting the disturbance by at least one determinant; and a disturbance observer configured to reflect the estimated disturbance in the control input by calculating an estimated disturbance obtained by estimating the disturbance, wherein the disturbance observer includes a mutual reaction force removal part including a determinant for removing mutual reaction force between joints of the robot; a low-pass filter; and an estimated-disturbance dimension conversion part.

Claims

1. A system for controlling a flexible joint robot, comprising: a control input dimension conversion part configured to receive a control input of a robot and convert the Cartesian coordinate system of the control input to the rotating workspace coordinate system; an entire flexible joint robot dynamics part configured to receive the control input and a disturbance and output a control output by multiplying the control input reflecting the disturbance by at least one determinant; and a disturbance observer configured to reflect the estimated disturbance in the control input by calculating an estimated disturbance obtained by estimating the disturbance, wherein the disturbance observer comprises a mutual reaction force removal part comprising a determinant for removing mutual reaction force between joints of the robot; a low-pass filter; and an estimated-disturbance dimension conversion part.

2. The system according to claim 1, wherein the robot comprises a first link rotatably coupled to an installation surface in horizontal and vertical directions; and a second link rotatably coupled to the first link, wherein the first and second links are connected to each other by a flexible joint.

3. The system according to claim 2, wherein the control input dimension conversion part comprises the Jacobian transposed matrix (J.sub.R.sup.T) configured to determine a relationship between a determinant for force (f.sub.x.sup.R, f.sub.y.sup.R) acting on an end effector in the rotating workspace coordinate system and a determinant for a torque (.sub.m) acting on a single joint link of a first link and a torque (.sub.b) acting on two joint links acting simultaneously on two links.

4. The system according to claim 3, wherein the Jacobian transposed matrix (J.sub.R.sup.T) is $\left[\begin{array}{cc}l\sin \frac{q_2}{2}& l\cos \frac{q_2}{2}\\ -l\sin \frac{q_2}{2}& l\cos \frac{q_2}{2}\end{array}\right],$ wherein l represents a length of the first link and second link, and q.sub.2 represents a relative angle size of the second link relative to the first link.

5. The system according to claim 1, wherein a determinant by the entire flexible joint robot dynamics part varies according to movement of the robot.

6. The system according to claim 1, wherein, when a determinant by the entire flexible joint robot dynamics part is , an inverse matrix (.sup.R) of A is calculated according to Equation 1 below: $\begin{array}{cc}{}^R={\left(J_R^T\right)}^{-1}\stackrel{}{M}{J}_R^{-1}=\frac{1}{2l^2}\left[\begin{array}{cc}\frac{J_1^R}{{\sin }^2\frac{q_2}{2}}& \frac{J_m^R}{\sin \frac{q_2}{2}}\\ \frac{J_m^R}{\sin \frac{q_2}{2}}& \frac{J_2^R}{{\cos }^2\frac{q_2}{2}}\end{array}\right]& \left(\mathrm{Equation}1\right)\end{array}$ where J.sub.R.sup.T represents the Jacobian transposition matrix representing a relationship between a force of a robot link in rotational coordinates and a robot joint torque in joint coordinates, J.sub.R.sup.1 represents the Jacobian inverse matrix, M represents a nominal matrix for inertia of a robot link, J.sub.1.sup.R=J.sub.1+m.sub.2l.sup.2+J.sub.22m.sub.2d.sub.2l cos q.sub.2, J.sub.2.sup.R=J.sub.1+m.sub.2l.sup.2+J.sub.2+2m.sub.2d.sub.2l cos q.sub.2, J.sub.m.sup.R=J.sub.1J.sub.2+m.sub.2l.sup.2, J.sub.1 represents an inertia moment for an axis passing through a center of mass of a first link, J.sub.2 represents an inertia moment for an axis passing through a center of mass of a second link, m.sub.2 represents the mass of the second link, l represents a length of each of the links, d.sub.2 represents a distance between an end of the second link and a center of gravity, and q.sub.2 represents an angle of a second link in the Cartesian coordinate system.

7. The system according to claim 6, wherein a determinant (.sub.n.sup.R) comprised in the mutual reaction force removal part sets at least one matrix value in the inverse matrix (.sup.R) to 0.

8. The system according to claim 7, wherein the determinant (.sub.n.sup.R) comprised in the mutual reaction force removal part is calculated according to Equation 2 below: $\begin{array}{cc}\left[\begin{array}{cc}{}_{11n}^R& 0\\ 0& {}_{22n}^R\end{array}\right]=\frac{1}{2l^2}\left[\begin{array}{cc}\frac{J_1^R}{{\sin }^2\frac{q_2}{2}}& 0\\ 0& \frac{J_2^R}{{\cos }^2\frac{q_2}{2}}\end{array}\right]& \left(\mathrm{Equation}2\right)\end{array}$

9. The system according to claim 8, wherein a determinant $\left(\left[\begin{array}{c}{\stackrel{.}{\stackrel{}{d}}}_m\\ {\stackrel{.}{\stackrel{}{d}}}_b\end{array}\right]\right)$ for the estimated disturbance is calculated according to Equation 3 below: $\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{\stackrel{}{d}}}_m\\ {\stackrel{.}{\stackrel{}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\stackrel{}{d}}_m\\ {\stackrel{}{d}}_b\end{array}\right]+L\left(\left\{\frac{1}{N}{J}_R^T{}_n^R{J}_R\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_{12}\end{array}\right]+{\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]}^{-1}\left(\left[\begin{array}{cc}{\stackrel{}{J}}_{m1}& 0\\ 0& {\stackrel{}{J}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.Math.}{}}_1\\ {\stackrel{.Math.}{}}_2\end{array}\right]+\left[\begin{array}{cc}{\stackrel{}{B}}_{m1}& 0\\ 0& {\stackrel{}{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{}{}}_1\\ {\stackrel{}{}}_2\end{array}\right]\right)\right\}-\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]\right)& \left(\mathrm{Equation}3\right)\end{array}$ where {circumflex over (d)}.sub.m and {circumflex over (d)}.sub.b, which are disturbances observed by a disturbance observer, respectively represents an observed disturbance value acting on a first link, which is one joint, and an observed disturbance value acting on both first and second joint links, {circumflex over ({dot over (d)})}.sub.m and {circumflex over ({dot over (d)})}.sub.b respectively represent changed values of {circumflex over (d)}.sub.m and {circumflex over (d)}.sub.b, L represents an observation gain of a disturbance observer, N represents an actuator gear ratio, {umlaut over (q)}.sub.1 and {umlaut over (q)}.sub.12 respectively represent angular accelerations of links of a single joint and both joints, .sub.m1 and .sub.m2 respectively represent nominal inertia values of motors of first joint actuator (located at a center of rotation of the first link) and second joint actuator (located at a connection point of first and second links), {circumflex over (B)}.sub.m1 and {circumflex over (B)}.sub.m2 respectively represent nominal damping values of motors of actuators driving the first link and the second link, {dot over ()}.sub.1 and {umlaut over ()}.sub.1 respectively represent an acceleration and angular acceleration of the actuator driving the first link, {dot over ()}.sub.2 and {umlaut over ()}.sub.2 respectively represent an acceleration and angular acceleration of the actuator driving the second link, and .sub.m and .sub.b are respectively input torques for a single joint and both joints.

10. The system according to claim 9, wherein a determinant $\left(\left[\begin{array}{c}{\stackrel{.}{\stackrel{}{d}}}_m\\ {\stackrel{.}{\stackrel{}{d}}}_b\end{array}\right]\right)$ for the estimated disturbance is calculated according to Equation 4 below: $\begin{array}{cc}\left[\begin{array}{c}{\stackrel{}{d}}_m\\ {\stackrel{}{\stackrel{}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\stackrel{}{d}}_m\\ {\stackrel{}{d}}_b\end{array}\right]+L\left(\left\{\frac{1}{4N}\right\}\left[\begin{array}{cc}{J}_1^R+J_2^R& -J_1^R+J_2^R\\ -J_1^R+J_2^R& J_1^R+J_2^R\end{array}\right]\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_2\end{array}\right]+{\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]}^{-1}\left(\left[\begin{array}{cc}{\hat{J}}_{m1}& 0\\ 0& {\hat{J}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.Math.}{}}_1\\ {\stackrel{.Math.}{}}_2\end{array}\right]+\left[\begin{array}{cc}{\stackrel{}{B}}_{m1}& 0\\ 0& {\stackrel{}{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{}{}}_1\\ {\stackrel{}{}}_2\end{array}\right]\right)-\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]\right)& \left(\mathrm{Equation}4\right)\end{array}$

11. A system for controlling a flexible joint robot, comprising: a control input dimension conversion part configured to receive a control input of a robot and convert the Cartesian coordinate system of the control input to the rotating workspace coordinate system; an entire flexible joint robot dynamics part configured to receive the control input and a disturbance and output a control output by multiplying the control input reflecting the disturbance by at least one determinant; and a disturbance observer configured to reflect the estimated disturbance in the control input by calculating an estimated disturbance obtained by estimating the disturbance, wherein a determinant $\left(\left[\begin{array}{c}{\stackrel{.}{\stackrel{}{d}}}_m\\ {\stackrel{.}{\stackrel{}{d}}}_b\end{array}\right]\right)$ for an estimated disturbance calculated in the disturbance observer is expressed as a sum of terms comprising determinants $\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_2\end{array}\right],\left[\begin{array}{c}{\stackrel{.Math.}{}}_1\\ {\stackrel{.Math.}{}}_2\end{array}\right],\left[\begin{array}{c}{\stackrel{}{}}_1\\ {\stackrel{}{}}_2\end{array}\right],\mathrm{and}\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right],$ and a denominator of a coefficient of $\left[\begin{array}{c}{\stackrel{.}{\stackrel{}{d}}}_m\\ {\stackrel{.}{\stackrel{}{d}}}_b\end{array}\right],\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_2\end{array}\right],\left[\begin{array}{c}{\stackrel{.Math.}{}}_1\\ {\stackrel{.Math.}{}}_2\end{array}\right],\mathrm{or}\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]$ does not comprise 0, {circumflex over (d)}.sub.m and {circumflex over (d)}.sub.b, which are disturbances observed by a disturbance observer, respectively represents an observed disturbance value acting on a first link, which is one joint, and an observed disturbance value acting on both first and second joint links, {circumflex over ({dot over (d)})}.sub.m and {circumflex over ({dot over (d)})}.sub.b respectively represent changed values of {circumflex over (d)}.sub.m and {circumflex over (d)}.sub.b, {umlaut over (q)}.sub.1 and {umlaut over (q)}.sub.12 respectively represent angular accelerations of links of a single joint and both joints, {dot over ()}.sub.1 and {umlaut over ()}.sub.1 respectively represent an acceleration and angular acceleration of an actuator driving the first link, {dot over ()}.sub.2 and {umlaut over ()}.sub.2 respectively represent an acceleration and angular acceleration of the actuator driving the second link, and .sub.m and .sub.b are respectively input torques for a single joint and both joints.

12. The system according to claim 11, wherein a determinant $\left(\left[\begin{array}{c}{\stackrel{.}{\stackrel{}{d}}}_m\\ {\stackrel{.}{\stackrel{}{d}}}_b\end{array}\right]\right)$ for the estimated disturbance is calculated according to Equation 5 below: $\begin{array}{cc}\left[\begin{array}{c}{\stackrel{}{d}}_m\\ {\stackrel{}{\stackrel{}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\stackrel{}{d}}_m\\ {\stackrel{}{d}}_b\end{array}\right]+L\left(\left\{\frac{1}{4N}\right\}\left[\begin{array}{cc}{J}_1^R+J_2^R& -J_1^R+J_2^R\\ -J_1^R+J_2^R& J_1^R+J_2^R\end{array}\right]\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_2\end{array}\right]+{\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]}^{-1}\left(\left[\begin{array}{cc}{\hat{J}}_{m1}& 0\\ 0& {\hat{J}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.Math.}{}}_1\\ {\stackrel{.Math.}{}}_2\end{array}\right]+\left[\begin{array}{cc}{\stackrel{}{B}}_{m1}& 0\\ 0& {\stackrel{}{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{}{}}_1\\ {\stackrel{}{}}_2\end{array}\right]\right)-\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]\right)& \left(\mathrm{Equation}5\right)\end{array}$ where L represents an observation gain of a disturbance observer, N represents an actuator gear ratio, {umlaut over (q)}.sub.1 and {umlaut over (q)}.sub.12 respectively represent angular accelerations of links of a single joint and both joints, .sub.m1 and .sub.m2 respectively represent nominal inertia values of motors of first joint actuator (located at a center of rotation of the first link) and second joint actuator (located at a connection point of first and second links), and {circumflex over (B)}.sub.m1 and {circumflex over (B)}.sub.m2 respectively represent nominal damping values of motors of actuators driving the first link and the second link.

Description

DESCRIPTION OF DRAWINGS

[0028] FIG. 1 is a diagram schematically illustrating a flexible joint robot to which a control system according to an embodiment of the present invention is applied.

[0029] FIG. 2 illustrates, with regard to the Cartesian coordinate system and the rotating workspace coordinate system of a flexible joint robot to which a control system according to an embodiment of the present invention is applied, a structure of the flexible joint robot and a structure wherein a motor of the flexible joint robot is connected to a spring to constitute a flexible joint.

[0030] FIG. 3 is a block diagram schematically illustrating a system for controlling a flexible joint robot according to an embodiment of the present invention.

[0031] FIG. 4 is a block diagram schematically illustrating an internal flexible joint robot dynamics part included in a system for controlling a flexible joint robot according to an embodiment of the present invention.

BEST MODE

[0032] Hereinafter, the present invention will be described in detail by explaining a preferred embodiment of the invention with reference to the attached drawings. However, it should be understood that the spirit and scope of the present disclosure are not limited to the embodiment and can be modified by addition, modification, or deletion of elements constituting the embodiment and such additions, modifications, and deletions are also within the spirit and scope of the present disclosure.

[0033] FIG. 1 is a diagram schematically illustrating a flexible joint robot to which a control system according to an embodiment of the present invention is applied, and with regard to the Cartesian coordinate system and the rotating workspace coordinate system of a flexible joint robot to which a control system according to an embodiment of the present invention is applied, FIG. 2 illustrates a structure of the flexible joint robot and a structure wherein a motor of the flexible joint robot is connected to a spring to constitute a flexible joint.

[0034] Referring to FIGS. 1 and 2, a flexible joint robot 1 to which the control system according to an embodiment of the present invention is applied may include a first link 2, a second link 3 and a control system 10. One end of the first link 2 may be rotatably installed in horizontal and vertical directions on an installation surface 4 of a robot 1 and the other end of the first link 2 may be rotatably coupled with the second link 3.

[0035] The first link 2 and the second link 3 may be rotatably connected to each other by a flexible joint 5, and the first link 2 and the installation surface 4 may be rotatably connected to each other by a flexible joint 7.

[0036] One end of the second link 3 may be rotatably coupled to the first link 2, and an end effector 6, which can directly act on a work target, may be formed at the other end of the second link 3.

[0037] In FIG. 2, the length of the first link 2 and the length of the second link 3 are respectively defined as l.sub.1 and l.sub.2, q.sub.1 represents an angle of the first link 2 in the Cartesian coordinate system, i.e., the Cartesian coordinate system, and q.sub.2 represents a relative angle size of the second link 3 relative to the first link 2.

[0038] .sub.r, which is an angle between the rotating workspace coordinate system and the Cartesian coordinate system, represents an angular displacement of the end effector 6 from the Cartesian coordinate system.

[0039] When a position of the end effector 6 in the Cartesian coordinate system is (x, y), a rotational workspace of the end effector 6 is defined as the rotating workspace coordinate system according to the following equation using r and .sub.r:

r={square root over (x.sup.2+y.sup.2)}

.sub.r=arctan(x,y)<Equation 1>

[0040] As the position of the end effector 6 at a reference position is defined as the rotating workspace coordinate system as described above, the problem of singularity may be solved. This will be described below. A motion of the end effector 6 may be defined by a velocity, which is a derivative of a position of the end effector 6 with respect to time, and a formula for this is as follows:

{dot over (x)}.sup.R={dot over (r)}

{dot over (y)}.sup.r=r{dot over ()}.sub.r<Equation 2>

[0041] In more detail, a relationship between a position of the end effector 6 and an angle of the two links 2 and 3 in a rotary coordinate system may be expressed by the following equation:

[00013]$\begin{array}{cc}r=\sqrt{l_1^2+l_1^2+2l_1{l}_2\cos q_2}& <\mathrm{Equation}3>\end{array}$${}_r=\arctan \left(\frac{l_1\cos q_1+l_2\cos \left(q_1+q_2\right)}{l_1\sin q_1+l_2\sin \left(q_1+q_2\right)}\right)$

[0042] Here, assuming that a length (l.sub.1) of the first link 2 and a length (l.sub.2) of the second link 3 are identically l, the following formula may be obtained:

[00014]$\begin{array}{cc}r=2l\cos \frac{q_2}{2}=2l\cos \frac{1}{2}\left(q_{12}-q_1\right)& <\mathrm{Equation}4>\end{array}$${}_r=q_1+\frac{1}{2}{q}_2=\frac{1}{2}\left(q_1+q_{12}\right)$

[0043] where q.sub.12 represents q.sub.1+q.sub.2, i.e., an absolute angle of the second link.

[0044] Meanwhile, a relationship between a point where the flexible joint robot 1 is installed, i.e., an origin point in the Cartesian coordinate system, and the end effector 6 may be defined by the Jacobian matrix. The Jacobian matrix is as follows:

[00015]$\begin{array}{cc}J=\left[\begin{array}{cc}-l_1\sin q_1-l_2\sin \left(q_1+q_2\right)& -l_2\sin \left(q_1+q_2\right)\\ l_1\cos q_1+l_2\cos \left(q_1+q_2\right)& l_2\cos \left(q_1+q_2\right)\end{array}\right]& <\mathrm{Equation}5>\end{array}$

[0045] The Jacobian matrix may be separated as follows:

[00016]$\begin{array}{cc}J=\left[\begin{array}{cc}-s\mathrm{in}{q}_1& -\sin \left(q_1+q_2\right)\\ \cos q_1& \cos \left(q_1+q_2\right)\end{array}\right]\left[\begin{array}{cc}{l}_1& 0\\ 0& l_2\end{array}\right]\left[\begin{array}{cc}1& 0\\ 1& 1\end{array}\right]& <\mathrm{Equation}6>\end{array}$

[0046] In the case of

[00017]$\left[\begin{array}{cc}1& 0\\ 1& 1\end{array}\right]$

among the determinant, a relative angular velocity may be converted to an absolute angular velocity. An equation thereof is as follows:

[00018]$\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 1& 1\end{array}\right]\left[\begin{array}{c}{\stackrel{}{q}}_1\\ {\stackrel{}{q}}_2\end{array}\right]=\left[\begin{array}{c}{\stackrel{}{q}}_1\\ {\stackrel{}{q}}_1+{\stackrel{}{q}}_2\end{array}\right]=\left[\begin{array}{c}{\stackrel{}{q}}_1\\ {\stackrel{}{q}}_{12}\end{array}\right]& <\mathrm{Equation}7>\end{array}$

[0047] A speed of the end effector 6 in the Cartesian coordinate system may be converted to a speed in the rotating workspace coordinate system. A related equation is as follows:

[00019]$\begin{array}{cc}\left[\begin{array}{c}{\stackrel{}{x}}^R\\ {\stackrel{}{y}}^R\end{array}\right]=\left[\begin{array}{cc}\cos {}_r& \sin {}_r\\ -\sin {}_r& \cos {}_r\end{array}\right]\left[\begin{array}{c}\stackrel{}{x}\\ \stackrel{}{y}\end{array}\right]& <\mathrm{Equation}8>\end{array}$

[0048] Here, according to the geometric structure of FIG. 2, .sub.r may be

[00020]$q_1+\frac{q_2}{2},$

and, assuming that l.sub.1=l.sub.2=l, the following equation may be derived:

[00021]$\begin{array}{cc}\begin{array}{c}\left[\begin{array}{c}{\stackrel{}{x}}^R\\ {\stackrel{}{y}}^R\end{array}\right]=\left[\begin{array}{cc}\cos {}_r& \sin {}_r\\ -\sin {}_r& \cos {}_r\end{array}\right]\left[\begin{array}{cc}-l\sin q_1& -l\sin q_{12}\\ l\cos q_1& l\cos q_{12}\end{array}\right]\left[\begin{array}{c}{\stackrel{}{q}}_1\\ {\stackrel{}{q}}_{12}\end{array}\right]\\ =\left[\begin{array}{cc}l\sin \frac{q_2}{2}& -l\sin \frac{q_2}{2}\\ l\cos \frac{q_2}{2}& l\cos \frac{q_2}{2}\end{array}\right]\left[\begin{array}{c}{\stackrel{}{q}}_1\\ {\stackrel{}{q}}_{12}\end{array}\right]=J_R\left[\begin{array}{c}{\stackrel{}{q}}_1\\ {\stackrel{}{q}}_{12}\end{array}\right]\end{array}& <\mathrm{Equation}9>\end{array}$

[0049] As can be seen in the above equation, a relationship between absolute angular velocities ({umlaut over (q)}.sub.1 and {umlaut over (q)}.sub.12) of the first link 2 and the second link 3 and speeds ({dot over (x)}.sup.R and {dot over (y)}.sup.R) of the end effector 6 in the rotating workspace coordinate system may be defined a new Jacobian matrix J.sub.R.

[0050] Meanwhile, forces (f.sub.x, f.sub.y) at the end effector 6 defined in the Cartesian coordinate system may be converted to forces (f.sub.x.sup.R, f.sub.y.sup.R) of the end effector 6 in the rotating workspace coordinate system according to the following equation:

[00022]$\begin{array}{cc}\left[\begin{array}{c}{f}_x^R\\ f_y^R\end{array}\right]=\left[\begin{array}{cc}\cos {}_r& \sin {}_r\\ -s\mathrm{in}{}_r& \cos {}_r\end{array}\right]\left[\begin{array}{c}{f}_x\\ f_y\end{array}\right]& <\mathrm{Equation}10>\end{array}$

[0051] In addition, a torque (.sub.1) acting on the first link 2, a torque (.sub.2) acting on the second link 3, a torque (.sub.m) acting on a single joint link as only in the first link 2, and a torque (.sub.b) acting on two joint links which simultaneously act on two links may be defined by the following determinant. By the following determinant, the torques (.sub.1 and .sub.2) may be replaced with .sub.m and .sub.b such that they can be analyzed in the rotary coordinate system:

[00023]$\begin{array}{cc}\left[\begin{array}{c}{}_1\\ {}_2\end{array}\right]=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]=\left[\begin{array}{c}{}_m+{}_b\\ {}_b\end{array}\right]& <\mathrm{Equation}11>\end{array}$

[0052] Assuming that l.sub.1=l.sub.2=l, a relationship between an output force (f.sub.x, f.sub.y) and an input torque (.sub.m and .sub.b) in the end effector 6 may be defined according to the following equation:

[00024]$\begin{array}{cc}\begin{array}{c}\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]=\left[\begin{array}{cc}-l\sin q_1& l\cos q_1\\ -l\sin q_{12}& l\cos q_{12}\end{array}\right]\left[\begin{array}{cc}\cos {}_r& -s\mathrm{in}{}_r\\ \sin {}_r& \cos {}_r\end{array}\right]\left[\begin{array}{c}{f}_x^R\\ f_y^R\end{array}\right]\\ =\left[\begin{array}{cc}l\sin \frac{q_2}{2}& l\cos \frac{q_2}{2}\\ -l\sin \frac{q_2}{2}& l\cos \frac{q_2}{2}\end{array}\right]\left[\begin{array}{c}{f}_x^R\\ f_y^R\end{array}\right]=J_R^T\left[\begin{array}{c}{f}_x^R\\ f_y^R\end{array}\right]\end{array}& <\mathrm{Equation}12>\end{array}$

[0053] Meanwhile, the control system 10 of the flexible joint robot 1 may derive an acceleration ({umlaut over (x)}.sup.R, .sup.R) of the end effector 6 in the rotating workspace coordinate system as a control output by using the force (f.sub.x, f.sub.y) at the end effector 6 defined in the Cartesian coordinate system as a control input and by controlling the control input by a flexible joint robot dynamics equation, etc.

[0054] In this process, the control system 10 may control the flexible joint robot 1 by estimating external or internal disturbances (d_1, d_2) and subtracting them from the control input. This control system 10 is described in more detail below.

[0055] FIG. 3 is a block diagram schematically illustrating a system for controlling a flexible joint robot according to an embodiment of the present invention, and FIG. 4 is a block diagram schematically illustrating an internal flexible joint robot dynamics part included in a system for controlling a flexible joint robot according to an embodiment of the present invention.

[0056] The control system 10 of the flexible joint robot 1 according to an embodiment of the present invention may include a control input dimension conversion part 11, an entire flexible joint robot dynamics part 12, a disturbance observer 13, and an estimated-disturbance reflection part 14.

[0057] The control input dimension conversion part 11 may receive a control input and convert a control input of the Cartesian coordinate system into the rotating workspace coordinate system. The control input may be force (f.sub.x, f.sub.y) at the end effector 6, and the control input dimension conversion part 11 may converted to a determinant

[00025]$\left(\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]\right)$

for .sub.m and .sub.b such that the control input can be interpreted in the rotary coordinate system by the above determinant containing Equations 10 and 12.

[0058] Accordingly, the control input dimension conversion part 11 may include a transposed matrix (J.sub.R.sup.T) of the Jacobian matrix which determines a relationship between a determinant for the force (f.sub.x.sup.R, f.sub.y.sup.R) acting on the end effector 6 in the rotary coordinate system and a determinant for the torque (.sub.m) acting on a single joint link of the first link 2 and the torque (.sub.b) acting on two joint links acting simultaneously on two links.

[0059] The entire flexible joint robot dynamics part 12 may include a first matrix conversion part 121, a disturbance input part 122, an internal flexible joint robot dynamics part 123, a second matrix conversion part 124, and a dimension conversion part 125.

[0060] The first matrix conversion part 121 multiplies a converted determinant

[00026]$\left(\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]\right)$

by a determinant

[00027]$\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]$

to convert to a determinant

[00028]$\left[\begin{array}{c}{}_1\\ {}_2\end{array}\right].$

[0061] The converted

[00029]$\left[\begin{array}{c}{}_1\\ {}_2\end{array}\right]$

is added with a disturbance

[00030]$\left(\left[\begin{array}{c}{d}_1\\ d_2\end{array}\right]\right)$

introduced from the disturbance input part 122 to form

[00031]$\left[\begin{array}{c}{}_{m1}\\ {}_{m2}\end{array}\right],$

and the

[00032]$\left[\begin{array}{c}{}_{m1}\\ {}_{m2}\end{array}\right]$

is calculated by the following flexible joint robot dynamics equation in the internal flexible joint robot dynamics part 123:

[00033]$\begin{array}{cc}\mathrm{Robot}:M\left(q\right)\stackrel{.Math.}{q}=K\left(\frac{}{N}-q\right)+d_1& .Math.\mathrm{Equation}13.Math.\end{array}$$\mathrm{Flexible}\mathrm{actuator}:J_m\stackrel{.Math.}{}+B_m\stackrel{.}{}={}_m+d_2-\frac{K\left(\frac{}{N}-1\right)}{N}$

[0062] where M(q) represents an inertia matrix value of the robot link, q and {umlaut over (q)} respectively represent an angle and angular acceleration of the robot joint, , {dot over ()} and {umlaut over ()} respectively represent an angle, angular speed and angular acceleration of the actuator, J.sub.m and B.sub.m respectively represent an inertial value and damping value of an actuator motor, represents an actuator-applied value, K represents a stiffness value of the flexible joint, and N represents an actuator gear ratio.

[0063] The determinant calculated by the internal flexible joint robot dynamics part 123 is multiplied by a determinant

[00034]$\left[\begin{array}{cc}1& 0\\ 1& 1\end{array}\right]$

by the second matrix conversion part 124 to derive a determinant

[00035]$\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_{12}\end{array}\right],$

and the derived

[00036]$\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_{12}\end{array}\right]$

is calculated with the Jacobian matrix J.sub.R of <Equation 9> by the dimension conversion part 125 to derive a determinant

[00037]$\left[\begin{array}{c}{\stackrel{.Math.}{x}}^R\\ {\stackrel{.Math.}{y}}^R\end{array}\right]$

for the acceleration ({umlaut over (x)}.sup.R, .sup.R) of the end effector 6 in the rotary coordinate system which is a control output.

[0064] When a determinant obtained by all of the determinants of the first matrix conversion part 121 of the entire flexible joint robot dynamics part 12, the internal flexible joint robot dynamics part 123, and the second matrix conversion part 124 is , an inverse matrix (.sup.R) of is obtained according to the following equation:

[00038]$\begin{array}{cc}{}^R={\left(J_R^T\right)}^{-1}\stackrel{\_}{M}{J}_R^{-1}=\frac{1}{2l^2}\left[\begin{array}{cc}\frac{J_1^R}{{\sin }^{2\frac{q_2}{2}}}& \frac{J_m^R}{{\sin }^{\frac{q_2}{2}}}\\ \frac{J_m^R}{{\sin }^{\frac{q_2}{2}}}& \frac{J_2^R}{{\cos }^{2\frac{q_2}{2}}}\end{array}\right]& .Math.\mathrm{Equation}14.Math.\end{array}$

[0065] where J.sub.R.sup.T represents the Jacobian transposition matrix representing a relationship between a force of a robot link in rotational coordinates and a robot joint torque in joint coordinates, J.sub.R.sup.1 represents the Jacobian inverse matrix, M represents a nominal matrix for inertia of a robot link, J.sub.1.sup.R=J.sub.1+m.sub.2l.sup.2+J.sub.22m.sub.2d.sub.2l cos q.sub.2, J.sub.2.sup.R=J.sub.1+m.sub.2l.sup.2+J.sub.2+2m.sub.2d.sub.2l cos q.sub.2, J.sub.m.sup.R=J.sub.1J.sub.2+m.sub.2l.sup.2, J.sub.1 represents an inertia moment for an axis passing through a center of mass of a first link, J.sub.2 represents an inertia moment for an axis passing through a center of mass of a second link, m.sub.2 represents the mass of the second link, l represents a length of each of the links, d.sub.2 represents a distance between an end of the second link and a center of gravity, and q.sub.2 represents an angle of a second link in an orthogonal coordinate system.

[0066] In Equation 14, as the value q.sub.2 is included in the component of the matrix of .sub.n.sup.R, the component value of the matrix of the determinant by the entire flexible joint robot dynamics part 12 is changed according to a motion of the flexible joint robot 1, so that changes in controller performance according to the motion of the flexible joint robot 1 may be eliminated.

[0067] The disturbance observer 13 may calculate an estimated disturbance

[00039]$\left(\left[\begin{array}{c}{\stackrel{}{d}}_m\\ {\stackrel{}{d}}_b\end{array}\right]\right)$

obtained by estimating the disturbance

[00040]$\left(\left[\begin{array}{c}{d}_1\\ d_2\end{array}\right]\right)$

induced from a disturbance inlet part 122 and may reflect the estimated disturbance to the control input through the estimated-disturbance reflection part 14.

[0068] The disturbance observer 13 may include a mutual reaction force removal part 131 including a determinant for removing mutual reaction force between joints of the flexible joint robot 1, a low-pass filter 132, an estimated-disturbance dimension conversion part 133.

[0069] The mutual reaction force removal part 131 may remove mutual reaction force between the robot joints by multiplying a determinant, obtained by replacing a component value of a diagonal matrix in the determinant of the above Equation 14 with 0, by a control output

[00041]$\left[\begin{array}{c}{\stackrel{.Math.}{x}}^R\\ {\stackrel{.Math.}{y}}^R\end{array}\right].$

The determinant included in the mutual reaction force removal part 131 is as follows:

[00042]$\begin{array}{cc}{}_n^R=\left[\begin{array}{cc}{}_{11n}^R& 0\\ 0& {}_{22n}^R\end{array}\right]=\frac{1}{2l^2}\left[\begin{array}{cc}\frac{J_1^R}{{\sin }^{2\frac{q_2}{2}}}& 0\\ 0& \frac{J_2^R}{{\cos }^{2\frac{q_2}{2}}}\end{array}\right]& .Math.\mathrm{Equation}15.Math.\end{array}$

[0070] As the component values of 1st row, 2nd column and 2nd row, 1st column of the above Equation 15 included in the mutual reaction force removal part 131 become 0 and the component values of 1st row, 1st column and 2nd row, 2nd column are the component values of 1st row, 1st column and 2nd row, 2nd column of .sup.R, mutual reaction forces between the joints may be removed by setting other component values to 0 while increasing in proportion to some component values of the determinant included in the internal flexible joint robot dynamics part 123.

[0071] In the mutual reaction force removal part 131, the control output is multiplied by the determinant of the above Equation 15, and the remaining determinant may be disturbed and introduced into the low-pass filter 132.

[0072] A determinant for an estimated disturbance may be calculated as in the following equation by multiplying a value calculated in the low-pass filter 132 by the Jacobian transposed matrix (J.sub.R.sup.T) in the estimated-disturbance dimension conversion part 133:

[00043]$\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{\stackrel{}{d}}}_m\\ {\stackrel{.}{\stackrel{}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\stackrel{}{d}}_m\\ {\stackrel{}{d}}_b\end{array}\right]+L\left(\left\{\frac{1}{N}{J}_R^T{}_n^R{J}_R\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_{12}\end{array}\right]+{\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]}^{-1}\left(\left[\begin{array}{cc}{\hat{J}}_{m1}& 0\\ 0& {\hat{J}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.Math.}{}}_1\\ {\stackrel{.Math.}{}}_2\end{array}\right]+\left[\begin{array}{cc}{\hat{B}}_{m1}& 0\\ 0& {\hat{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.}{}}_1\\ {\stackrel{.}{}}_2\end{array}\right]\right)\right\}-\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]\right)& .Math.\mathrm{Equation}16.Math.\end{array}$

[0073] where {circumflex over (d)}.sub.m and {circumflex over (d)}.sub.b, which are disturbances observed by a disturbance observer, respectively represents an observed disturbance value acting on a first link, which is one joint, and an observed disturbance value acting on both first and second joint links, s, {circumflex over ({dot over (d)})}.sub.m and {circumflex over ({dot over (d)})}.sub.b respectively represent changed values of {circumflex over (d)}.sub.m and {circumflex over (d)}.sub.b, L represents an observation gain of a disturbance observer, N represents an actuator gear ratio, {umlaut over (q)}.sub.1 and {umlaut over (q)}.sub.12 respectively represent angular accelerations of links of a single joint and both joints, .sub.m1 and .sub.m2 respectively represent nominal inertia values of motors of first joint actuator (located at a center of rotation of the first link) and second joint actuator (located at a connection point of first and second links), {circumflex over (B)}.sub.m1 and {circumflex over (B)}.sub.m2 respectively represent nominal damping values of motors of actuators driving the first link and the second link, {dot over ()}.sub.1 and {umlaut over ()}.sub.1 respectively represent an acceleration and angular acceleration of the actuator driving the first link, {dot over ()}.sub.2 and {umlaut over ()}.sub.2 respectively represent an acceleration and angular acceleration of the actuator driving the second link, and .sub.m and .sub.b are respectively input torques for a single joint and both joints.

[0074] The above Equation 16 may be expressed in the form of the following equation:

[00044]$\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{\stackrel{}{d}}}_m\\ {\stackrel{.}{\stackrel{}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\stackrel{}{d}}_m\\ {\stackrel{}{d}}_b\end{array}\right]+L\left(\left\{\frac{1}{4N}\right\}\left[\begin{array}{cc}{J}_1^R+J_2^R& -J_1^R+J_2^R\\ -J_1^R+J_2^R& J_1^R+J_2^R\end{array}\right]\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_2\end{array}\right]+{\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]}^{-1}\left(\left[\begin{array}{cc}{\hat{J}}_{m1}& 0\\ 0& {\hat{J}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.Math.}{}}_1\\ {\stackrel{.Math.}{}}_2\end{array}\right]+\left[\begin{array}{cc}{\hat{B}}_{m1}& 0\\ 0& {\hat{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.}{}}_1\\ {\stackrel{.}{}}_2\end{array}\right]\right)-\left[\begin{array}{c}{}_m\\ {}_b\end{array}\right]\right)& .Math.\mathrm{Equation}17.Math.\end{array}$

[0075] In Equation 17, since J.sub.1.sup.R=J.sub.1+m.sub.2l.sup.2+J.sub.22m.sub.2d.sub.2l cos q.sub.2 and J.sub.2.sup.R=J.sub.1+m.sub.2l.sup.2+J.sub.2+2m.sub.2d.sub.2l cos q.sub.2 with regard to the determinant

[00045]$\left[\begin{array}{cc}{J}_1^R+J_2^R& -J_1^R+J_2^R\\ -J_1^R+J_2^R& J_1^R+J_2^R\end{array}\right]$

multiplied by a front end of

[00046]$\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_{12}\end{array}\right],$

the possibility that a denominator of each component of the determinant becomes 0 is blocked, so there is an advantage in that singularity can be removed.

[0076] In the case of related technologies, there is a problem that an estimated disturbance value diverges when the denominator of the determinant multiplied by the front end of

[00047]$\left[\begin{array}{c}{\stackrel{.Math.}{q}}_1\\ {\stackrel{.Math.}{q}}_{12}\end{array}\right]$

includes sin q.sub.2 and the q.sub.2 value becomes 0 or . This problem can be addressed by the present invention.

[0077] While the present invention has been described referring to the preferred embodiments, those skilled in the art will appreciate that many modifications and changes can be made to the present invention without departing from the spirit and essential characteristics of the present invention.