Control device and method for a manipulator
In a method for controlling a manipulator, in particular a robot, a reference path is stored and reference increments are automatically determined while following the path the reference increments are determined based on the dynamics of the manipulator while following the path.
1. A method for controlling a robotic manipulator, comprising the steps of:
storing a reference path(r s [s(t)]);
determining reference increments (Δq s [s(t)]) while following the path;
providing a processor with a model of dynamics of said manipulator represented as a mathematical relation between positions of said manipulator, time derivatives of said positions, and device forces of said manipulator
M
·
ⅆ
2
q
(
t
)
ⅆ
t
2
+
h
(
q
(
t
)
,
ⅆ
q
(
t
)
ⅆ
t
)
=
τ
;
and
in said processor, automatically determining the reference increments based on said dynamics of the manipulator, represented in said model, while following the reference path, by determining a reference motion [Δr s (t)] in said processor while following said path for a time increment (Δt), by interpolation between stored positions or joint coordinates or by evaluating a stored function, and
when drive forces that are necessary to realize a reference motion [Δr s (t)] exceed tolerable drive forces (τ zul ), automatically determining said reference increments [Δq s (s(t))] so as to differ from the reference motion [Δr s (t)] by a linear mapping Δq s (t)=Φ(Δr s (t)) of the reference motion, wherein the linear mapping maps the reference increments within a predetermined tube ((1−θ 1 )·Δr s +θ 2 ·n Δr +θ 3 ·b Δr ) enclosing the reference motion [Δr s (t)] (τ zul ); and
emitting a control signal at an output of the processor that controls movement of said manipulator with respect to said path;
wherein
M is a mass matrix;
q(t) is a vector of reference positions in joint coordinates;
h is a vector embodying weight, gyroscopic, and frictional forces;
τ is a vector of drive forces,
(θ 1 , θ 2 , θ 3 ) are parameters for deviations from the reference motion; and
n Δr , b Δr are normal and bi-normal vectors to the reference motion.
2. A method according to claim 1 , further comprising generating the stored reference path (r s ) to include at least one of positions (r s, i ), representing locations ([x i , y i , z i ]) and/or orientations ([α i , β i , γ i ]), of a manipulator-fixed reference coordinate system (TCP), joint coordinates ([q 1, i , . . . q a, i ]) and a parameterized function
(
r
s
[
s
(
t
)
]
=
∑
i
=
0
a
i
·
t
l
)
.
3. A method according to claim 1 , further comprising determining the reference increments [Δq s (s(t))] in said processor based on a scaled reference motion (Δr s (t))((1−θ)·Δr s (t)=J·Δq s (t)); wherein
J is a Jacobian matrix; and
θ is a reduction factor.
4. A method according to claim 1 , further comprising determine said reference increments[Δq s (s(t))] in said processor so as to differ from the reference motion[Δr s (t)] when said drive forces that are necessary to realize the reference motion exceed tolerable velocities (v zul ).
5. A method according to claim 4 , further comprising determining a reference velocity
(
Δ
q
s
(
t
)
Δ
t
)
for the time increment (Δt) so as to differ from a reference motion velocity
(
ⅆ
q
ⅆ
t
s
)
when velocities that are necessary to realize the reference motion exceed said tolerable velocities (v zul ).
6. A method according to claim 1 , further comprising determining the reference increments in said processor by an optimization.
7. A method according to claim 6 further comprising, in said optimization, employing an optimization criterion representing a deviation ((1−θ)·Δr s (t)−J[q(t)]·Δq s (t)) between the reference motion and the reference increments, a reserve
(
M
·
ⅆ
2
q
(
t
)
ⅆ
t
2
+
h
(
q
(
t
)
,
ⅆ
q
(
t
)
ⅆ
t
)
-
τ
zul
)
between drive forces which are necessary to realize the reference motion and tolerable drive forces, a reserve
(
J
·
Δ
q
s
(
t
)
Δ
t
-
v
zul
)
between a reference velocity for the time increment and a tolerable velocity, a deviation
(
Δ
q
s
(
t
)
Δ
t
-
ⅆ
q
ⅆ
t
s
)
between reference velocity for the time increment and reference joint velocity, a reserve between a pose which is reached by realizing the reference increments and a singular pose, drive forces which are necessary to realize the reference motion and/or a distance between a pose which is reached by realizing the reference increments and a preferred pose.
8. A method according to claim 6 , further comprising employing, in said optimization, criteria selected from the group consisting of penalty functions, equality constraints, and inequality constraints.
9. A method according to claim 4 , further comprising redundantly configuring the manipulator with respect to at least in one section of the stored reference path.
10. A control device for a robotic manipulator comprising:
a memory in which a reference path (r s [s(t)]) and a linearized mode
[
M
·
ⅆ
2
q
(
t
)
ⅆ
t
2
+
h
(
q
(
t
)
,
ⅆ
q
(
t
)
ⅆ
t
)
=
τ
]
of dynamics of the manipulator are stored;
a processor being configured to access the reference path and the model in said memory;
said processor being configured to automatically determine reference increments (Δq s [s(t)]) ((1−θ 1 )·Δr s +θ 2 ·n Δr +θ 3 ·b Δr ) (Δr s (t); based on said dynamics of the manipulator, represented in said model, while following the reference path, and
when drive forces that are necessary to realize a reference motion [Δr s (t)] exceed tolerable drive forces(τ zul ), automatically determine the reference increments [Δq s (s(t))] so as to differ from the reference motion [Δr s (t)] by a linear mapping Δq s (t)=Φ(Δr s (t)) of the reference motion, wherein the linear mapping maps the reference increments within a predetermined tube ((1−θ 1 ) ·Δr s +θ 2 ·n Δr +θ 3 ·b Δr ) enclosing the reference motion [Δr s (t)] (Δr s (t); (Δq s (t)=Φ(Δr s (t))) (τ zul ); and
said processor being configured to emit a control signal at an output of the processor that controls movement of said manipulator with respect to said path;
wherein
M is a mass matrix;
q(t) is a vector of reference positions in ioint coordinates;
h is a vector embodying weight, gyroscopic, and frictional forces;
τ is a vector of drive forces,
(θ 1 , θ 2 , θ 3 ) are parameters for deviations from the reference motion; and
n Δr , b Δr are normal and bi-normal vectors to the reference motion.
11. A non-transitory, computer-readable data storage medium encoded with programming instructions, said medium being loadable into a processor that controls a robotic manipulator, said processor having access to a memory in which a reference path for movement of said manipulator is stored, and said programming instructions causing said processor to:
access a linearized mode
[
M
·
ⅆ
2
q
(
t
)
ⅆ
t
2
+
h
(
q
(
t
)
,
ⅆ
q
(
t
)
ⅆ
t
)
=
τ
]
of dynamics of said manipulator represented as a mathematical relation between positions of said manipulator, time derivatives of said positions, and device forces of said manipulator; and
automatically determine reference increments (Δq s [s(t)]) ((1−θ 1 )·Δr s +θ 2 ·n Δr +θ 3 ·b Δr ) Δr s (t) based on said dynamics of the manipulator, represented in said model, while following the reference path, by determining a reference motion [Δr s (t)] in said processor while following said path for a time increment (Δt), by interpolation between stored positions or joint coordinates or by evaluating a stored function, and
when drive forces that are necessary to realize a reference motion [Δr s (t)] exceed tolerable drive forces(τ zul ), automatically determine said reference increments [Δq s (s(t))] so as to differ from the reference motion [Δr s (t)] by a linear mapping Δq s (t)=Φ(Δr s (t)) of the reference motion, wherein the linear mapping maps the reference increments within a predetermined tube ((1−θ 1 )·Δr s +θ 2 ·n Δr +θ 3 ·b Δr ) enclosing the reference motion [Δr s (t)] (τ zul ); and
emit a control signal at an output of the processor that controls movement of said manipulator with respect to said path;
wherein
M is a mass matrix;
q(t) is a vector of reference positions in joint coordinates;
h is a vector embodying weight, gyroscopic, and frictional forces;
τ is a vector of drive forces,
(θ 1 , θ 2 , θ 3 ) are parameters for deviations from the reference motion; and
n Δr , b Δr are normal and bi-normal vectors to the reference motion.
12. A method as claimed in claim 6 , further comprising determining the reference increments in said processor by a multi-criteria optimization.