US 12,636,775 B2 · 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.

A flexible joint robot system comprising:

a flexible joint robot comprising a first link, a second link and a flexible joint to which each of the first link and the second link is rotatably connected;
and
a control system for controlling the flexible joint robot,
wherein the control system comprises joint actuators configured to rotate the first and second links, sensors configured to detect joint angles and/or torques of the first and second links, and a controller including a processor and a memory storing control instructions,
wherein the control system comprises:
a control input dimension conversion part configured to receive a control input of the 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 including torque commands applied to a first joint actuator located at a center of rotation of the first link and a second joint actuator located at a connection point of the first and second links so as to control a position and/or orientation of an end effector of the flexible joint robot in the rotating workspace coordinate system 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,
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,
wherein the mutual reaction force removal part is configured to remove mutual reaction forces between the joints so that vibration and position error of the end effector caused by the interaction between the first and second links are reduced,
wherein, when a determinant by the entire flexible joint robot dynamics part is Λ, an inverse matrix (ΛR) of Λ is calculated according to Equation 1 below:
Λ
R
=
(
J
R
T
)
-
1
⁢
M
¯
⁢
J
R
-
1
=
1
2
⁢
l
2
[
J
1
R
sin
2
⁢
q
2
2
J
m
R
sin
⁢
q
2
2
J
m
R
sin
⁢
q
2
2
J
2
R
cos
2
⁢
q
2
2
]
(
Equation
⁢
1
)
where JRT 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, JR−1 represents the Jacobian inverse matrix, M represents a nominal matrix for inertia of a robot link,
J
1
R
=
J
1
+
m
2
⁢
l
2
+
J
2
-
2
⁢
m
2
⁢
d
2
⁢
l
⁢
cos
⁢
q
2
,
J
2
R
=
J
1
+
m
2
⁢
l
2
+
J
2
+
2
⁢
m
2
⁢
d
2
⁢
l
⁢
cos
⁢
q
2
,
J
m
R
=
J
1
-
J
2
+
m
2
⁢
l
2
,
J
1
represents an inertia moment for an axis passing through a center of mass of the first link, J2 represents an inertia moment for an axis passing through a center of mass of the second link, m2 represents the mass of the second link, l represents a length of each of the first and second links, d2 represents a distance between an end of the second link and a center of gravity, and q2 represents an angle of the second link in the Cartesian coordinate system,
wherein a determinant
(
Λ
n
R
)
comprised in the mutual reaction force removal part sets at least one matrix value in the inverse matrix (ΛR) to 0,
wherein the determinant
(
Λ
n
R
)
comprised in the mutual reaction force removal part is calculated according to Equation 2 below:
[
Λ
11
⁢
n
R
0
0
Λ
2
⁢
2
⁢
n
R
]
=
1
2
⁢
l
2
[
J
1
R
sin
2
⁢
q
2
2
0
0
J
2
R
cos
2
⁢
q
2
2
]
(
Equation
⁢
2
)
wherein a determinant
(
[
d
ˆ
.
m
d
ˆ
.
b
]
)
for the estimated disturbance is calculated according to Equation 3 below:
[
d
ˆ
.
m
d
ˆ
.
b
]
=
-
L
[
d
ˆ
m
d
ˆ
b
]
+
L
⁡
(
{
1
N
⁢
J
R
T
⁢
Λ
n
R
⁢
J
R
[
q
¨
1
q
¨
1
⁢
2
]
+
[
1
1
0
1
]
-
1
⁢
(
[
J
ˆ
m
⁢
1
0
0
J
ˆ
m
⁢
2
]
[
θ
¨
1
θ
¨
2
]
+
[
B
ˆ
m
⁢
1
0
0
B
ˆ
m
⁢
2
]
[
θ
˙
1
θ
˙
2
]
)
}
-
[
τ
m
τ
b
]
)
(
Equation
⁢
3
)
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 the first link, which is one joint, and an observed disturbance value acting on both the first and second 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)}1 and {umlaut over (q)}12 respectively represent angular accelerations of links of a single joint and both joints, Ĵm1 Ĵ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 the 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 (θ)}1 and {umlaut over (θ)}1 respectively represent an acceleration and angular acceleration of the actuator driving the first link, {dot over (θ)}2 and {umlaut over (θ)}2 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.

353 more elements

Robust position control system of flexible joint robots

Life of this patent

Robust position control system of flexible joint robots

Life of this patent

Abstract

Claims8