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.

Description

TECHNICAL FIELD

The present invention relates to a control system for a flexible joint robot, and more specifically, based on a non-linear disturbance observer, it provides constant performance regardless of internal disturbance according to the robot's movement and external disturbance from the outside of the robot, and moves the robot through singularity removal.

BACKGROUND ART

As robot technology develops rapidly, the use of robots is expanding in various fields of society. A high-speed robot is required for rapid work in automated production, and a lightweight and flexible robot has been required in terms of economic efficiency.

In particular, robots with flexible joints, such as collaborative robots, rescue robots, and rehabilitation robots, have been developed a lot to enable natural interaction with environments.

A robust controller based on a disturbance observer has been used to use a flexible joint robot robustly and appropriately for the purpose, and, in the case of existing disturbance observer based on a linear system, the performance thereof varies according to robot movements, so a controller had to be conservatively designed within a limit in which stability is guaranteed. Due to such a conservative controller design, there has been a limitation in improving the performance of the robot.

In addition, when a robot is controlled in a workspace, the robot diverges at a singularity. Since existing linear disturbance observers have to be designed to avoid such a singularity, a range in which a robot can move is limited, which causes a problem in that diversity, performance, and efficiency of tasks performed by the robot are reduced.

DISCLOSURE

Technical Problem

Therefore, the present invention has been made in view of the above problems, and it is one object of the present invention to provide a nonlinear disturbance observer-based robust position control system capable of preventing a change in controller performance according to robot movements by considering a parameter change according to a change in a robot posture in a flexible joint robot

It is another object of the present invention to provide a system for controlling a flexible joint robot which is capable of providing improved position control robustness and controller performance for a flexible robot by estimating and removing a reaction force between links of a robot.

It is yet another object of the present invention to provide a system for controlling a flexible joint robot which does not limit robot movements in a workspace by interpreting after converting from the Cartesian coordinate system (x-y) to the rotating workspace coordinate system (r−θ_r) to remove the singularities of a robot.

Technical Solution

In accordance with an aspect of the present invention, the above and other objects can be accomplished by the provision of a system for controlling a flexible joint robot, including: 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.

In addition, the robot may include 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.

In addition, the control input dimension conversion part may include a Jacobian transposed matrix (J_R^T) configured to determine a relationship between a determinant for force (f_x^R, f_y^R) acting on an end effector in a rotary coordinate system and a determinant for a torque (τ_m) acting on a single joint link of a first link and a torque (τ_b) acting on two joint links acting simultaneously on two links.

In addition, the Jacobian transposed matrix (J_R^T) may be

$\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₂ represents a relative angle size of the second link relative to the first link.

In addition, a determinant by the entire flexible joint robot dynamics part may vary according to movement of the robot.

In addition, when a determinant by the entire flexible joint robot dynamics part is Λ, an inverse matrix (Λ^R) of A may be calculated according to Equation 1 below:

$\begin{array}{cc}{\Lambda }^R={\left(J_R^T\right)}^{-1}\overline{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_R^T represents a 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_R⁻¹ represents a Jacobian inverse matrix, M represents a nominal matrix for inertia of a robot link, J₁^R=J₁+m₂l²+J₂−2m₂d₂l cos q₂, J₂^R=J₁+m₂l²+J₂+2m₂d₂l cos q₂, J_m^R=J₁−J₂+m₂l², J₁ represents an inertia moment for an axis passing through a center of mass of a first link, J₂ represents an inertia moment for an axis passing through a center of mass of a second link, m₂ represents the mass of the second link, l represents a length of each of the links, d₂ represents a distance between an end of the second link and a center of gravity, and q₂ represents an angle of a second link in the Cartesian coordinate system.

In addition, a determinant (Λ_n^R) included in the mutual reaction force removal part may set at least one matrix value in the inverse matrix (Λ^R) to 0.

In addition, the determinant (Λ_n^R) included in the mutual reaction force removal part may be calculated according to Equation 2 below:

$\begin{array}{cc}\left[\begin{array}{cc}{\Lambda }_{11n}^R& 0\\ 0& {\Lambda }_{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}$

In addition, a determinant

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

for the estimated disturbance may be 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}{\hat{d}}_m\\ {\hat{d}}_b\end{array}\right]+L\left(\left\{\frac{1}{N}{J}_R^T{\Lambda }_n^R{J}_R\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{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}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]+\left[\begin{array}{cc}{\hat{B}}_{ml}& 0\\ 0& {\hat{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]\right)\right\}-\left[\begin{array}{c}{\tau }_m\\ {\tau }_b\end{array}\right]\right)& \left(\mathrm{Equation}3\right)\end{array}$

where {circumflex over (d)}_m and {circumflex over (d)}_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)})}_m and {circumflex over ({dot over (d)})}_b respectively represent changed values of {circumflex over (d)}_m and {circumflex over (d)}_b, L represents an observation gain of a disturbance observer, N represents an actuator gear ratio, {umlaut over (q)}₁ and {umlaut over (q)}₁₂ respectively represent angular accelerations of links of a single joint and both joints, Ĵ_m1 and Ĵ_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)}_m1 and {circumflex over (B)}_m2 respectively represent nominal damping values of motors of actuators driving the first link and the second link, {dot over (θ)}₁ and {umlaut over (θ)}₁ respectively represent an acceleration and angular acceleration of the actuator driving the first link, {dot over (θ)}₂ and {umlaut over (θ)}₂ respectively represent an acceleration and angular acceleration of the actuator driving the second link, and τ_m and τ_b are respectively input torques for a single joint and both joints.

In addition, a determinant

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

for the estimated disturbance may be calculated according to Equation 4 below:

$\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{\stackrel{ˆ}{d}}}_m\\ {\stackrel{.}{\stackrel{ˆ}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\hat{d}}_m\\ {\hat{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}{\ddot{q}}_1\\ {\ddot{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}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]+\left[\begin{array}{cc}{\hat{B}}_{m1}& 0\\ 0& {\hat{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]\right)-\left[\begin{array}{c}{\tau }_m\\ {\tau }_b\end{array}\right]\right)& \left(\mathrm{Equation}4\right)\end{array}$

In accordance with another aspect of the present invention, there is provided a system for controlling a flexible joint robot, including: 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 including determinants

$\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{q}}_2\end{array}\right],\left[\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right],\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right],\mathrm{and}\left[\begin{array}{c}{\tau }_m\\ {\tau }_b\end{array}\right],$

and a denominator of a coefficient of

$\left(\left[\begin{array}{c}{\stackrel{.}{\stackrel{ˆ}{d}}}_m\\ {\stackrel{.}{\stackrel{ˆ}{d}}}_b\end{array}\right]\right),\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{q}}_2\end{array}\right],\left[\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right],\mathrm{or}\left[\begin{array}{c}{\tau }_m\\ {\tau }_b\end{array}\right]$

does not include 0, {circumflex over (d)}_m and {circumflex over (d)}_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)})}_m and {circumflex over ({dot over (d)})}_b respectively represent changed values of {circumflex over (d)}_m and {circumflex over (d)}_b, {umlaut over (q)}₁ and {umlaut over (q)}₁₂ respectively represent angular accelerations of links of a single joint and both joints, {dot over (θ)}₁ and {umlaut over (θ)}₁ respectively represent an acceleration and angular acceleration of an actuator driving the first link, {dot over (θ)}₂ and {umlaut over (θ)}₂ respectively represent an acceleration and angular acceleration of the actuator driving the second link, and τ_m and τ_b are respectively input torques for a single joint and both joints.

A determinant

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

for the estimated disturbance may be calculated according to Equation 5 below:

$\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{\stackrel{ˆ}{d}}}_m\\ {\stackrel{.}{\stackrel{ˆ}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\hat{d}}_m\\ {\hat{d}}_b\end{array}\right]+\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}{\ddot{q}}_1\\ {\ddot{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}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]+\left[\begin{array}{cc}{\hat{B}}_{m1}& 0\\ 0& {\hat{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]\right)-\left[\begin{array}{c}{\tau }_m\\ {\tau }_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)}₁ and {umlaut over (q)}₁₂ respectively represent angular accelerations of links of a single joint and both joints, Ĵ_m1 and Ĵ_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)}_m1 and {circumflex over (B)}_m2 respectively represent nominal damping values of motors of actuators driving the first link and the second link.

Advantageous Effects

In accordance with the present invention having the above configuration, first, the restriction on the working radius of a robot is removed due to the elimination of singularity, so various tasks can be performed without restrictions in various fields such as a robot for disaster relief, a robot for collaboration, and a robot for rehabilitation.

Second, a robot to which the present invention is applied can maximize driving efficiency near singularity in the case of many tasks, and by utilizing this effect, the present invention can achieve high efficiency in performing tasks of a robot.

Third, a system for controlling a flexible joint robot according to the present invention can achieve high performance and stability at the same time, unlike existing conservative controllers, because its performance is constant regardless of the posture of a robot.

DESCRIPTION OF DRAWINGS

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.

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.

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

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

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.

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.

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.

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.

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.

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

θ_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.

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 θ_r:

r=√{square root over (x²+y²)}

θ_r=arctan(x,y)  <Equation 1>

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)} ^R ={dot over (r)}

{dot over (y)} ^r =r{dot over (θ)} _r  <Equation 2>

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:

$\begin{array}{cc}r=\sqrt{l_1^2+l_1^2+2l_1{l}_2\cos q_2}& <\mathrm{Equation}3>\end{array}$ ${\theta }_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)$

Here, assuming that a length (l₁) of the first link 2 and a length (l₂) of the second link 3 are identically l, the following formula may be obtained:

$\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}$ ${\theta }_r=q_1+\frac{1}{2}{q}_2=\frac{1}{2}\left(q_1+q_{12}\right)$

where q₁₂ represents q₁+q₂, i.e., an absolute angle of the second link.

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:

$\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}$

The Jacobian matrix may be separated as follows:

$\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}$

In the case of

$\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:

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

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:

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

Here, according to the geometric structure of FIG. 2, θ_r may be

$q_1+\frac{q_2}{2},$

and, assuming that l₁=l₂=l, the following equation may be derived:

$\begin{array}{cc}\begin{array}{c}\left[\begin{array}{c}{\dot{x}}^R\\ {\dot{y}}^R\end{array}\right]=\left[\begin{array}{cc}\cos {\theta }_r& \sin {\theta }_r\\ -\sin {\theta }_r& \cos {\theta }_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}{\dot{q}}_1\\ {\dot{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}{\dot{q}}_1\\ {\dot{q}}_{12}\end{array}\right]=J_R\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_{12}\end{array}\right]\end{array}& <\mathrm{Equation}9>\end{array}$

As can be seen in the above equation, a relationship between absolute angular velocities ({umlaut over (q)}₁ and {umlaut over (q)}₁₂) of the first link 2 and the second link 3 and speeds ({dot over (x)}^R and {dot over (y)}^R) of the end effector 6 in the rotating workspace coordinate system may be defined a new Jacobian matrix J_R.

Meanwhile, forces (f_x, f_y) at the end effector 6 defined in the Cartesian coordinate system may be converted to forces (f_x^R, f_y^R) of the end effector 6 in the rotating workspace coordinate system according to the following equation:

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

In addition, a torque (τ₁) acting on the first link 2, a torque (τ₂) acting on the second link 3, a torque (τ_m) acting on a single joint link as only in the first link 2, and a torque (τ_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 (τ₁ and τ₂) may be replaced with τ_m and τ_b such that they can be analyzed in the rotary coordinate system:

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

Assuming that l₁=l₂=l, a relationship between an output force (f_x, f_y) and an input torque (τ_m and τ_b) in the end effector 6 may be defined according to the following equation:

$\begin{array}{cc}\begin{array}{c}\left[\begin{array}{c}{\tau }_m\\ {\tau }_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 {\theta }_r& -s\mathrm{in}{\theta }_r\\ \sin {\theta }_r& \cos {\theta }_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}$

Meanwhile, the control system 10 of the flexible joint robot 1 may derive an acceleration ({umlaut over (x)}^R, ÿ^R) of the end effector 6 in the rotating workspace coordinate system as a control output by using the force (f_x, f_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.

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.

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.

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.

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_x, f_y) at the end effector 6, and the control input dimension conversion part 11 may converted to a determinant

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

for τ_m and τ_b such that the control input can be interpreted in the rotary coordinate system by the above determinant containing Equations 10 and 12.

Accordingly, the control input dimension conversion part 11 may include a transposed matrix (J_R^T) of the Jacobian matrix which determines a relationship between a determinant for the force (f_x^R, f_y^R) acting on the end effector 6 in the rotary coordinate system and a determinant for the torque (τ_m) acting on a single joint link of the first link 2 and the torque (τ_b) acting on two joint links acting simultaneously on two links.

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.

The first matrix conversion part 121 multiplies a converted determinant

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

by a determinant

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

to convert to a determinant

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

The converted

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

is added with a disturbance

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

introduced from the disturbance input part 122 to form

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

and the

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

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

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

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_m and B_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.

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

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

by the second matrix conversion part 124 to derive a determinant

$\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{q}}_{12}\end{array}\right],$

and the derived

$\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{q}}_{12}\end{array}\right]$

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

$\left[\begin{array}{c}{\ddot{x}}^R\\ {\ddot{y}}^R\end{array}\right]$

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

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 (Λ^R) of Λ is obtained according to the following equation:

$\begin{array}{cc}{\Lambda }^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]& \langle \mathrm{Equation}14\rangle \end{array}$

where J_R^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_R⁻¹ represents the Jacobian inverse matrix, M represents a nominal matrix for inertia of a robot link, J₁^R=J₁+m₂l²+J₂−2m₂d₂l cos q₂, J₂^R=J₁+m₂l²+J₂+2m₂d₂l cos q₂, J_m^R=J₁−J₂+m₂l², J₁ represents an inertia moment for an axis passing through a center of mass of a first link, J₂ represents an inertia moment for an axis passing through a center of mass of a second link, m₂ represents the mass of the second link, l represents a length of each of the links, d₂ represents a distance between an end of the second link and a center of gravity, and q₂ represents an angle of a second link in an orthogonal coordinate system.

In Equation 14, as the value q₂ is included in the component of the matrix of Λ_n^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.

The disturbance observer 13 may calculate an estimated disturbance

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

obtained by estimating the disturbance

$\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.

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.

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

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

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

$\begin{array}{cc}{\Lambda }_n^R=\left[\begin{array}{cc}{\Lambda }_{11n}^R& 0\\ 0& {\Lambda }_{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]& \langle \mathrm{Equation}15\rangle \end{array}$

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 Λ^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.

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.

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_R^T) in the estimated-disturbance dimension conversion part 133:

$\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{\stackrel{ˆ}{d}}}_m\\ {\stackrel{.}{\stackrel{ˆ}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\hat{d}}_m\\ {\hat{d}}_b\end{array}\right]+L\left(\left\{\frac{1}{N}{J}_R^T{\Lambda }_n^R{J}_R\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{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}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]+\left[\begin{array}{cc}{\hat{B}}_{m1}& 0\\ 0& {\hat{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.}{\theta }}_1\\ {\stackrel{.}{\theta }}_2\end{array}\right]\right)\right\}-\left[\begin{array}{c}{\tau }_m\\ {\tau }_b\end{array}\right]\right)& \langle \mathrm{Equation}16\rangle \end{array}$

where {circumflex over (d)}_m and {circumflex over (d)}_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)})}_m and {circumflex over ({dot over (d)})}_b respectively represent changed values of {circumflex over (d)}_m and {circumflex over (d)}_b, L represents an observation gain of a disturbance observer, N represents an actuator gear ratio, {umlaut over (q)}₁ and {umlaut over (q)}₁₂ respectively represent angular accelerations of links of a single joint and both joints, Ĵ_m1 and Ĵ_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)}_m1 and {circumflex over (B)}_m2 respectively represent nominal damping values of motors of actuators driving the first link and the second link, {dot over (θ)}₁ and {umlaut over (θ)}₁ respectively represent an acceleration and angular acceleration of the actuator driving the first link, {dot over (θ)}₂ and {umlaut over (θ)}₂ respectively represent an acceleration and angular acceleration of the actuator driving the second link, and τ_m and τ_b are respectively input torques for a single joint and both joints.

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

$\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{\stackrel{ˆ}{d}}}_m\\ {\stackrel{.}{\stackrel{ˆ}{d}}}_b\end{array}\right]=-L\left[\begin{array}{c}{\hat{d}}_m\\ {\hat{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}{\ddot{q}}_1\\ {\ddot{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}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]+\left[\begin{array}{cc}{\hat{B}}_{m1}& 0\\ 0& {\hat{B}}_{m2}\end{array}\right]\left[\begin{array}{c}{\stackrel{.}{\theta }}_1\\ {\stackrel{.}{\theta }}_2\end{array}\right]\right)-\left[\begin{array}{c}{\tau }_m\\ {\tau }_b\end{array}\right]\right)& \langle \mathrm{Equation}17\rangle \end{array}$

In Equation 17, since J₁^R=J₁+m₂l²+J₂−2m₂d₂l cos q₂ and J₂^R=J₁+m₂l²+J₂+2m₂d₂l cos q₂ with regard to the determinant

$\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

$\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{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.

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

$\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{q}}_{12}\end{array}\right]$

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

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.

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; anda 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 (JRT) configured to determine a relationship between a determinant for force (fxR, fyR) acting on an end effector in the rotating workspace coordinate system and a determinant for a torque (τm) acting on a single joint link of a first link and a torque (τb) acting on two joint links acting simultaneously on two links.

4. The system according to claim 3, wherein the Jacobian transposed matrix (JRT) is

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 (ΛR) of A is calculated according to Equation 1 below:

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

8. The system according to claim 7, wherein the determinant (ΛnR) comprised in the mutual reaction force removal part is calculated according to Equation 2 below:

9. The system according to claim 8, wherein a determinant

10. The system according to claim 9, wherein a determinant

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; anda 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

12. The system according to claim 11, wherein a determinant