Method and device for controlling movements in the case of manipulators
View Patent ↗In a method for controlling the movement of a manipulator associated with an interpretation of a given point sequence of poses (positions and orientations) by splines, the motion components are separately parameterized. Thus, marked, subsequent changes to the orientation of robot axes have no undesired effects on the Cartesian movement path of the robot. Suitable algorithms are provided for orientation control by using quaternions or Euler angles.
1. A method for orientation control of a robotal device, the method comprising:
interpolating given dot sequences of poses by splines, and representing the orientation represented by Eulerian angles A 1 , B 1 , C 1 ∈R expanded in the entire quantity of real numbers R 3 , wherein components of the motion are separately parametrized;
orienting the robotal device based on said Eulerian angles.
2. Method according to claim 1 , wherein positions of the manipulator are parameterized separately from the orientations thereof.
3. Method according to claim 1 , wherein movements of auxiliary axes are parameterized separately from the positions and/or orientations of the manipulator.
4. Method according to claim 1 , wherein an orientation control takes place by means of successive rotations through Euler angles A 1 , B i , C i , i=1, . . . , n, and in each case directly succeeding rotations take place about axes perpendicular to one another.
5. Method according to claim 1 , wherein for minimum reorientation successively the following steps are performed and the Euler angles A, B and C, without restricting the general nature, indicate rotations about the instantaneous z axis, y axis or x axis:
representation of the orientations (A i , B i , C i ) by:
a) determination of standard Euler angles (A i , B i , C i ),
b) for I:={i=1, . . . , n|B i ≠π/2+2kπ, k∈Z} and i∈I definition of
( A i,1 ( g ), B i,1 ( h ), C i,1 ( k ))=( A i +2 gπ,B i +2 hπ,C i +2 k π), g,h,k∈Z ;
and
( A i,2 ( l ), B i,2 ( m ), C i,2 ( o ))=( A i +2 lπ,π−B i +2 mπ,C i +π+2 o π), l,m,o∈Z;
c) for J:={i=1, . . . , n|B i =π/2+2kπ, kεZ} and i∈J definition of
( A i ( t ), B i ( h ), C i ( k,t ))=( A i +t,B i 2 hπ,C i +t +2 k π), t∈R,h,k∈Z
d) for L:={i=1,. . . , n|B i =−π/2+2kπ, k∈Z} and i∈L definition of
( A i ( t ), B i ( h ), C i ( k,t ))=( A i +t,B i 2 hπ,C i −t +2 k π), t∈R,h,k∈Z
e) choice of a random starting pair (A iStart , B iStart , C iStart ) with iStart∈I, e.g. iStart=min {i∈I};
f) for all j∈{1, . . . , n) apart from j=iStart determination of expanded Euler angles (A j , B j , C j ) according to steps g) and h), where i∈{1, . . . , n} is an adjacent index, i.e. i=j+1 or i=j−1 and additionally either i=iStart or a reference pair (A i , B i , C i ) has already been adapted according to steps g) and h):
g) for j∈I determination of
d j,1 =min{ A j,1 ( g )− A i |+|B j,1 ( h )− B i |+|C j,1 ( k )− C i |,g,h,k∈Z}
d j,2 =min{ A j,2 ( l )− A i |+|B j,2 ( m )− B i |+|C j,2 ( o )− C i |,l,m,o∈Z}
assuming the minimum for g*, h*, k* or l*, m*, o*; and choice of
(
A
j
,
B
j
,
C
j
)
=
{
(
A
j
,
1
(
g
*
)
,
B
j
,
1
(
h
*
)
,
C
j
,
1
(
k
*
)
)
,
{
if
d
j
,
1
≤
d
j
,
2
{
{
(
A
j
,
2
(
l
*
)
,
B
j
,
2
(
m
*
)
,
C
j
,
2
(
o
*
)
)
,
{
if
d
j
,
1
>
d
j
,
2
;
h) for j∈J or j∈L definition of
dA j ( t ):=| A j ( t )− A i |
dB j ( h ):=| B j ( h )− B i |
dC j ( k,t ):=| C j ( k,t )− C i |
with h, k∈Z and t∈R;
determination of h*∈Z with dB j (h*)=min{dB j (h), h∈Z);
calculation of zero points of dA j (t) or dC j (k, t) with respect to t, in which ta 0,j is the zero point of dA j (t) and tc 0,j (k) is the zero point of dC j (k, t) dependent on the parameter k;
choice of k*∈Z, so that there is a minimum spacing of the zero points , i.e.
|ta 0,j −tc 0,j ( k *)|=min{| ta 0,j −tc 0,j ( k )|, k∈Z);
choice of t* random between ta 0,j and tc 0,j (k*), because there is always fulfilment there of
dA j ( t *)= dB j ( h *)= dC j ( k*,t *)=min{ dA j ( t )= dB j ( h )= dC j ( k,t ), t∈R,h,k∈Z)
and choice of
( A j ,B j ,C j )=( A j ( t *), B j ( h *), C j ( k*,t *));
interpolation of the triple (A i , B i , C i ) by a spline and use of the interpolation function in the rotation matrix R(A, B, C).
6. Method according to claim 5 , wherein t*=½ (ta 0,j +tc 0,j (k*)) is chosen.
7. A method for orientation control of a robotal device, the method comprising:
orienting the robotal device based on quaternions (qi, i=1, . . . , ), wherein for minimum reorientation between individual points successively the following steps are performed:
representation of each quaternion on the four-dimensional unit sphere according to
q (α,β,φ)=(cos ½φ), sin(½φ)· v (α,β))
with
(v(α,β)=(cos(α) cos(β), sin(α) cos(β), sin(β)),
in which α, β, (and r=1) give the spherical coordinates of the rotating vectors v=v(α, β)
determination of motion-optimum pairs (v i , φ i ) by
a) adapting the sign of q i , so that
q i T q i+1 ≧0 for all i= 1, . . . ,n−1 ; b ) for all i= 1,
b) for all i=1. . . , n determination of a possible pair (v i , φ i ) with
q i =(cos(½φ i ), sin(½φ i )v i );
c) choice of a random starting pair (V iStart , φ istart ) with iStart∈I:={i=1, . . . , n|q i ≠(±1, 0, 0, 0)}, e.g. iStart=min{∈I);
on choosing iStartεI, the completely freely selectable rotary vector V iStart would be unnecessarily fixed, which can lead to a suboptimum orientation, because the starting pair would itself no longer be adapted;
d) for all j∈(1, . . . , n} with the exception of j=iStart determination of in each case precisely one pair (v j , φ j ) starting from a reference pair (v i , φ i ) in accordance with steps e) to g), in which i∈(1, . . . , n) is a neighbouring index, i.e. i=j+1 or i=j−1 and also either i=iStart or the reference pair has already been adapted according to steps e) to g);
e) determination of k*, h*∈Z, so that
φ
j
,
1
(
k
*
)
-
φ
i
=
min
{
φ
j
,
1
(
k
)
-
φ
i
,
k
∈
Z
}
=
min
{
φ
j
+
4
k
π
-
φ
i
,
k
∈
Z
}
,
φ
j
,
2
(
h
*
)
-
φ
i
=
min
{
φ
j
,
2
(
h
)
-
φ
i
,
h
∈
Z
}
=
min
{
-
φ
j
+
4
h
π
-
φ
i
,
h
∈
Z
}
;
f) subsequently for j∈I determination of
d j,1 =|arccos( v i T v j )+|φ j,1 ( k *)−φ i |,
d j,2 =|arccos( v i T (− v j ))|=|φ j,2 ( h *)−φ i |
or for j∉I
d j,1 =|φ j,1 ( k *)−φ i |,
d j,2 =|φ j,2 ( h *)−φ i |;
g) subsequently in each case choice of (v j , φ j ) where for
j
∈
I
(
v
j
,
φ
j
)
=
{
(
v
j
,
φ
j
,
1
(
k
*
)
)
,
d
j
,
1
≤
d
j
,
2
{
{
(
-
v
j
,
φ
j
,
2
(
h
*
)
)
,
d
j
,
1
>
d
j
,
2
or
for
j
∈
I
(
v
j
,
φ
j
)
=
{
(
v
i
,
φ
j
,
1
(
k
*
)
)
,
if
d
j
,
1
≤
d
j
,
2
{
{
(
v
i
,
φ
j
,
2
(
h
*
)
)
,
if
d
j
,
1
>
d
j
,
2
;
determination of pairs (α i , β i ) from v i in spherical coordinate representation by
h) calculation of standard spherical coordinates (α i β i ) of v i for all i∈I:
v i =(cos(α 1 ) cos(β i ), sin(α 1 ) cos(β i ), sin(β i ))
i) definition of
J:={j ∈I with v j ≠(0,0,±1)}
L:={j ∈I with v j =(0,0,±1)}
and therefore definition of expanded spherical coordinates from R×R
for i∈J:
(α i,1 ( h ),β i,1 ( k ))=(α i +2 h π,β i +2 k π), h,k∈Z;
(α i,2 (1),β i,2 ( m ))=(α i +π2 lπ,π−βi 30 2 m π), l,m∈Z;
for i∈L:
(α i ( x ),β i ( k ))=( x,B i 2 k π), x∈R,k∈Z;
j) fixing a random starting pair (α jStart , β jStart ) with jStart∈J;
k) for all j∈I with the exception of j=jStart in each case determination of expanded polar coordinates in accordance with steps l) and m), in which i∈I is an adjacent index of j, so that there is no k∈I between i and j and also either i=jStart or the reference pair (α i , β i ) has already been adapted in accordance with steps l) and m):
l) determination of j∈J of values h*, k*, l*, m*∈Z with
d
j
,
1
=
min
{
α
j
,
1
(
h
)
-
α
i
+
β
j
,
1
(
k
)
-
β
i
,
with
h
,
k
∈
Z
}
=
α
j
,
1
(
h
*
)
-
α
i
+
β
j
,
1
(
k
*
)
-
β
i
;
d
j
,
2
=
min
{
α
j
,
2
(
l
)
-
α
i
+
β
j
,
2
(
m
)
-
β
i
,
with
l
,
m
∈
Z
}
=
α
j
,
2
(
l
*
)
-
α
i
+
β
j
,
2
(
m
*
)
-
β
i
or
for
j
∈
L
of
a
value
k
*
∈
Z
with
β
j
(
k
*
)
-
β
i
=
min
{
β
j
(
k
)
-
β
i
,
k
∈
Z
}
;
m) subsequently choice of (α j , β j ), in which for j∈J
(
α
j
,
β
j
)
=
{
(
α
j
,
1
(
h
*
)
,
β
j
,
1
(
k
*
)
)
d
j
,
1
≤
d
j
,
2
{
{
(
α
j
,
2
(
l
*
)
,
β
j
,
2
(
m
*
)
)
d
j
,
1
>
d
j
,
2
or
for
j
∈
L
(
α
j
,
β
j
)
=
(
α
i
,
β
j
(
k
*
)
)
;
n) choice of a number of triples determined according to steps a) to m)
(
α
i
,
β
i
,
φ
i
)
=
{
(
α
i
,
β
i
,
φ
i
)
for
i
∈
J
{
(*
,
β
i
,
φ
i
)
for
i
∈
L
{
(*
,
*
,
φ
i
)
for
i
∈
I
,
in which all components designated * can be selected at random;
o) adaptation of the selectable components in the triples (α i , β i , φ i );
interpolation of the triples (α i , β i , φ i ) selected in step n) by a spline; and
use of the interpolation function in q(a,P,(p).
8. Method according to claim 7 , wherein adaptation takes place of the freely selectable components of (α i , β i , φ i ) using:
iMin:=min {i∈I)
iMax:=max {i∈I)
iMin:=min {j∈J)
jMax:=max {j∈J),
so that
α
p
=
{
(
α
jMin
for
1
≤
p
<
jMin
,
{
{
α
jMax
for
jMax
<
p
≤
n
and
β
p
=
{
(
β
iMin
for
1
≤
p
<
iMin
,
{
{
β
iMax
for
iMax
<
p
≤
n
.
9. Method according to claim 8 , wherein for adjacent i,j∈J for all i<p<j, i.e. p∉J a choice is made of
α p =α i +( p−i )/( j−i )·(α j −α i )
and correspondingly for adjacent i,j∈I for all i<p<j, i.e. p∉I is chosen
β p =β i +( p−i )/( j−i )·(β j −β i ).
10. Device for orientation control of a robotal device, the device comprising:
an interpolation unit;
an interpolation means positioned within said interpolation unit for interpolating a given dot sequence of poses by splines, said interpolation means representing an orientation represented by Eulerian angles A i , B i , C i ∈R expanded in the entire quantity of real numbers R 3 , wherein the interpolation unit is constructed for separate parametrization of components of motion during interpolation.
11. Device according to claim 10 , wherein the interpolation unit is constructed for the separate parameterization of positions and orientations of the manipulator.
12. Device according to claim 10 , wherein the interpolation unit is constructed for the separate parameterization of on the one hand movements of auxiliary axes and on the other positions and/or orientations of the manipulator.
13. Device according to claim 10 , wherein the interpolation device is set up for an orientation control by means of successive rotations through Euler angles A i , B i , C i and in each case directly succeeding rotations take place about axes perpendicular to one another.
14. Device according to claim 13 , wherein the interpolation unit is set up for the use of expanded Euler angles A i , B i , C i ∈R.
15. Device according to claim 14 , wherein the interpolation device is set up for the sequential performance of the following steps:
representation of the orientations (A i , B i , C i ) by:
a) determining the standard Euler angles (A i , B i , C i );
b) for I:={i=1, . . . ,n|B i ≠π/2+2kπ, k∈Z} and i∈I definition of
( A i,1 ( g ), B i,1 ( h ), C i,1 ( k ))=( A i +2 gπ,B i +2 hπ,C i +2 k π), g,h,k∈Z;
and
( A i,2 ( l ), B i,2 ( m ), C i,2 ( o ))=( A i +π+2 lπ,π−B i +2 mπ,C i +π+2 o π), l,m,o∈Z;
b) for J:={i=1, . . . ,n|B i =π/2+2kπ, kεZ} and i∈J definition of
( A i,1 ( t ), B i,1 ( h ), C i,1 ( k,t ))=( A i +t,B i +2 hπ,C i +t +2 k π), t∈R,h,k∈Z;
d) for L:={i=1, . . . , n|B i =−π/2+2kπ, k∈Z} and i∈L definition of
( A i,1 ( t ), B i,1 ( h ), C i,1 ( k,t ))=( A i +t,B i +2 hπ,C i −t +2 k π), t∈R,h,k∈Z;
e) choice of a random starting pair (A iStart , B iStart , C iStart ) with iStart∈I
f) for all j∈{I, . . . , n}, apart from j=iStart, determining expanded Euler angles (A j , B j , C j ) according to steps g) and h), in which i∈(1, . . . , n} is an adjacent index, i.e. i=j+1 or i=j−1 and additionally either i=iStart or a reference pair (A i , B i , C i ) has already been adapted according to steps g) and h);
g) for j∈I determination of
d j,1 =min{| A j,1 ( g )− A i |+|B j,1 ( h )− B i |+|C j,1 ( k )−C i |,g,h,k∈Z}
d j,2 =min{| A j,2 ( l )− A i |+|B j,2 ( m )− B i |+|C j,2 ( o )−C i |,l,m,o∈Z}
assuming the minimum for g*, h*, k* or l*, m*, o*; and choice of
(
A
j
,
B
j
,
C
j
)
=
{
(
A
j
,
1
(
g
*
)
,
B
j
,
1
(
n
*
)
,
C
j
,
1
(
k
*
)
)
,
{
if
d
j
,
1
≤
d
j
,
2
{
{
(
A
j
,
2
(
l
*
)
,
B
j
,
2
(
m
*
)
,
C
j
,
2
(
o
*
)
)
,
{
if
d
j
,
1
>
d
j
,
2
;
h) for j∈J or j∈L definition of
dA j ( t ):=| A j ( t )− A i |
dB j ( h ):=| B j ( h )− B i |
dC j ( k,t ):=| C j ( k,t )− C i |
with h, k∈Z and t∈R;
determination of h*∈Z with dB j (h*)=min{dB j (h), h∈Z);
determination of zero points of dA j (t) or dC j (k, t) with respect to t, in which ta 0,j is the zero point of dA j (t) and tc 0,j (k) is the zero point of dC j (k, t);
choice of k*∈Z, so that the spacing of the zero points is at a minimum, i.e. |ta 0,j −tc 0,j (k*)=min{|ta 0,j (k), k∈Z);
choice of t* random between ta 0,j and tc 0,j (k*); and choice of
( A j ,B j ,C j )=( A j ( t *), B j ( h *), C j ( k*,t *));
interpolation of the triples (A i , B i , C i ) by a spline and use of the interpolation function in the rotation matrix R (A, B, C).
16. Device according to claim 15 , wherein the interpolation unit is set up for the choice of t*=½(ta 0,j +tc 0,j (k*)).
17. A device for orientation control of a robotal device, the device comprising:
an interpolation unit for controlling orientation and position of the robotal device based on quaternions (qi, i=1, . . . , n), wherein the interpolation unit is set up for performing the following:
representation of each quaternion on the four-dimensional unit sphere according to
q (α,β,φ)=(cos(½φ), sin(½φ)· v (α,β))
with
v(α,β)=(cos(α) cos(β), sin(α) cos(β), sin(β)),
in which α, β (and r=1) indicate the spherical coordinates of the rotary vectors v=v(α, β);
determining motion-optimum pairs (v i , φ i ) by
a) adapting the sign of q i , so that q i Tq i+1 >0 for all i=1, . . . , n−1;
b) for all i=1, . . . , n determination of a possible pair (v i , φ i ) with
q i =(cos(½φ i ), sin(½φ i )v i );
c) choice of a random starting pair (v iStart , φ iStart ) with iStart∈I:={i=1, . . . , n|q i ≠(±1,0,0,0)};
d) for all j∈}i=1, . . . , n) with the exception of j=iStart determination of in each case precisely one pair (v j , φ i ) starting from a reference pair (v i , φ i ) in accordance with steps e) to g), in which i∈{1, . . . , n) is an adjacent index, i.e. i=j1 or i=j−1 and also either i=iStart or the reference pair has already been adapted according to steps e) to g);
e) determination of k*, h*∈Z, so that
φ
j
,
1
(
k
*
)
-
φ
i
=
min
{
φ
j
,
1
(
k
)
-
φ
i
,
k
∈
Z
}
=
min
{
φ
j
+
4
k
-
φ
i
,
k
∈
Z
}
,
φ
j
,
2
(
h
*
)
-
φ
i
=
min
{
φ
j
,
2
(
h
)
-
φ
i
,
h
∈
Z
}
=
min
{
-
φ
j
+
4
h
π
-
φ
i
,
h
∈
Z
}
;
f) subsequently for j∈I determination of
d j,1 =|arccos(v i T v j )|+|φ j,1 ( k *)−φ i |,
d j,2 =|arccos(v i T (−v j )|+|φ j,2 ( h *)−φ i |
or for j∉I
d j,1 =|φ j,1 ( k *)−φ i |,
d j,2 =|φ j,2 ( h *)−φ i |;
g) subsequently in each case choice of(v j , φ j ), where for j∈I
(
v
j
,
φ
j
)
=
{
(
v
j
,
φ
j
,
1
(
k
*
)
)
,
d
j
,
1
≤
d
j
,
2
{
{
(
-
v
j
,
φ
j
,
2
(
h
*
)
)
,
d
j
,
1
>
d
j
,
2
or
for
j
∉
I
(
v
j
,
φ
j
)
=
{
(
v
i
,
φ
j
,
1
(
k
*
)
)
if
d
j
,
1
≤
d
j
,
2
{
{
(
v
i
,
φ
j
,
2
(
h
*
)
)
if
d
j
,
1
>
d
j
,
2
;
determination of pairs (α i , β i ) from v i in spherical coordinate representation by:
h) calculating standard spherical coordinates (α i , β i ) of v i for all i∈I:
v i =(cos(α 1 ) cos(β i ), sin(α 1 ) cos(β i ), sin(β i ));
i) definition of
J:={j∈I with v j ≠(0,0,±1)},
L:={j∈I with v j =(0,0,±1)},
and therefore definition of expanded spherical coordinates from R×R
for i∈J:
(α i,1 ( h ),β i,1 ( k ))=(α i +2 hπ,β i 2 k π), h,k∈Z;
(α i,2 ( l ),β i,2 ( m ))=(α i +π+2 lπ,−βi 2 m π), l,m∈Z;
for i∈L:
(α i,1 ( x ),β i,1 ( k ))=( x,B i +2 k π), x∈R,k∈Z;
j) fixing a random starting pair (α jStart , β iStart ) with jStart∈J;
k) for all j∈I with the exception of j=jStart, in each case single determination of expanded polar coordinates according to steps l) and m), in which i∈I is an adjacent index of j, so that there is no k∈I between i and j and also either i=jStart or the reference pair (α i , β i ) was already adapted according to steps l) and m);
l) determination for j∈J of values h*, k*, l*, m*∈Z with
d
j
,
1
=
min
{
α
j
,
1
(
h
)
-
α
i
+
β
j
,
1
(
k
)
-
β
i
,
with
h
,
k
∈
Z
}
=
α
j
,
1
(
h
*
)
-
α
i
+
β
j
,
1
(
k
*
)
-
β
i
;
d
j
,
2
=
min
{
α
j
,
2
(
l
)
-
α
i
+
β
j
,
2
(
m
)
-
β
i
,
with
l
,
m
∈
Z
}
=
α
j
,
2
(
l
*
)
-
α
i
+
β
j
,
2
(
m
*
)
-
β
i
or
for
j
∈
L
of
a
value
k
*
∈
Z
with
β
j
(
k
*
)
-
β
i
=
min
{
β
j
(
k
)
-
β
i
,
k
∈
Z
}
;
m
)
subsequent
choice
of
(
α
j
,
β
j
)
,
in
which
for
j
∈
J
(
α
j
,
β
j
)
=
{
(
α
j
,
1
(
h
*
)
,
β
j
,
1
(
k
*
)
)
,
d
j
,
1
≤
d
j
,
2
{
{
(
α
j
,
2
(
l
*
)
,
β
j
,
2
(
m
*
)
)
,
d
j
,
1
>
d
j
,
2
or
for
j
∈
L
(
α
j
,
β
j
)
=
(
α
i
,
β
j
(
k
*
)
)
;
n) choice of a number of triples determined according to steps a) to m)
(
α
i
,
β
i
,
φ
i
)
=
{
(
α
i
,
β
i
,
φ
i
)
for
i
∈
J
{
(*
,
β
i
,
φ
i
)
for
i
∈
L
{
(*
,
*
,
φ
i
)
for
i
∉
I
,
o) adapting the selectable components in the trios (α i , β i , φ i );
interpolation of the triples (α i , β i , φ i ) selected in step n) by a spline and
use of the interpolation function in q(α, β, φ).
18. Device according to claim 17 , wherein the interpolation unit is set up for adapting the freely selectable components of (α i , β i , φ i ) using:
iMin:=mm {i∈I)
iMax:=max {i∈I)
iMin:=min {j∈J)
jMax:=max {j∈J),
so that
α
p
=
{
α
jMin
for
1
≤
p
<
jMin
,
{
{
α
jMax
for
jMax
<
p
≤
n
and
β
p
=
{
(
β
iMin
for
1
≤
p
<
iMin
,
{
{
β
iMax
for
iMax
<
p
≤
n
.
19. Device according to claim 18 , wherein the interpolation unit is set up for adjacent i,j∈J for all i<p<j, i.e. p∉J for the determination of
α p =α i +( p−i )/( j−i )·(α j −α i )
and correspondingly for neighbouring i,j∈I for all i<p<j, i.e. p∉I for determining
β p =β i +( p−i )/( j−i )·(β j −β i ).
20. A method for orientation control of a robotal device, the method comprising:
interpolating given dot sequences of poses by splines;
representing each orientation through quaternions on a four-dimensional unit sphere according to
q(α,β,φ)=(cos(φ/2), sin(φ/2) v(α,β))
v(α,β)=(cos(α) cos(β), sin(α) cos(β), sin(β))
wherein α, β represent the spherical coordinates of rotating vectors v-v(α, β);
representing spherical coordinates to determine pairs of spherical coordinates (α i , β i ) from rotating vectors v 1 by means of:
calculating the spherical coordinates
(α i , β i ) of the v i for all i∈I:
v i =(cos(α i ) cos(β i ), sin(α i ) cos(β i ) sin(β i );
defining
J: =( j∈I with v j ≠(0,0±1)),
L: =( j∈I with v j ≠(0,0±1))
to define expanded spherical coordinates from R×R
for i∈J:
(α i,1 ( h ),β i,1 ( k ))=(α i +2 h π,β i +2 k π) h,k∈Z;
(α i,2 ( l ),β i,2 ( m ))=(α i +π+2 l π,π−πβ i +2 m π) l,m∈Z;
for i∈L:
(α i ( x ),β i ( k ))=( x,β i +2 k π) x∈R,k∈Z;
positioning and orienting the robotal device based on said spherical coordinates.
21. Method in accordance with claim 20 , wherein the following steps are carried out when determining pairs from the v i in spherical coordinates:
Setting any desired start pair (α jStart , β jStart ) with jStart ⊂ J;
determination of expanded polar coordinates each according to steps l) and m) for all j ⊂ I with the exception of j=jStart, wherein i ⊂ I is a neighboring index of j, so that there is no k ⊂ I between i and j and, in addition, i=j Start or the reference pair (α i , β i ) was already fitted according to steps l) and m);
determination for j ⊂ J of values h*, k*, l*, m* ⊂ Z with
d j,1 −min{|α j,1 ( h )−α i |+|β j,1 ( k )−β i |, with h,k ⊂ Z}=|α j,1 ( h *)−α 1 |+|β j,1 ( k *)−β 1 |;
d j,2 =min{|α j,2 ( l )−α 1 |+|β j,2 ( m )−β 1 |, with l,m ⊂ 2}=|α j,2 ( l *)−α 1 |+|β j,2 ( m *)−β 1 |
or for j ⊂ L of a value k* ⊂ 2 with
|β j ( k *)−β 1 =min{β j ( k )−β i |,k ⊂ Z};
subsequently selection of (α j β j ), wherein for j ⊂ J
(
α
j
,
β
j
)
=
{
(
α
j
,
1
(
h
*
)
,
β
j
,
1
(
k
*
)
)
d
j
,
1
≤
d
j
,
2
(
α
j
,
2
(
l
*
)
,
β
i
,
2
(
m
*
)
)
d
j
,
1
>
d
j
,
2
and for j ⊂ L
(α j ,β j )=(α j ,β j (k*));
selection of a number of triples determined according to steps a) through m)
(α i ,β i ,φ i )
for i ⊂ J
(α i ,β i ,φ ii )={(*,β i ,φ i )
for i ⊂ L
(*,*,φ i )
for i ⊂ I,
wherein all components designated by “*” are freely selectable;
fitting of the selectable components in the triples (α i , β i , φ i );
interpolation of the selected triples (α i , β i , φ i ) by a spline; and
substitution of the interpolation function in q(α, β, φ).
22. Method in accordance with claim 20 or 21 , wherein the following steps are additionally carried out for the minimum reorientation from point to point:
Representation of a quarternion on the four-dimensional unit sphere according to
q (α,β,φ)=(cos ½φ), sin(½φ)· v (α,β))
with
v(α,β)=(cos(α) cos(β), sin(α) cos(β), sin(β)),
wherein α, β, (and r=1) indicate the spherical coordinates of the rotating vectors v=v(α, β);
determination of motion-optimal pairs (v 1 , φ 1 ) by
a) adapting signs of q i , so that q i T q i+1 ≧0 for all i=1. . . , n−1;
b) determination of a possible pair (v 1 , φ 1 ) for all i=1, . . . , n with q 1 =(cos(½φ 1 ), sin(½φ 1 )v 1 );
c) selection of any desired start pair (V iStart , φ iStart ), with 1Start ⊂ I:={1=1, . . . , n|q i ≠(±1, 0, 0, 0)}
d) determination of exactly one pair (v j , φ j ) for all j ⊂ (1, . . . , n) with the exception of j=iStart, starting from a reference pair (v i , φ i ) according to the steps e) through g), wherein i ⊂ (1, . . . , n) is a neighboring index, i.e., i=j1 or i=j−1 and, furthermore, either is i=iStart true or the reference pair was already fitted according to steps e) through g);
e) determination of k*, h* ⊂ Z so that
φ
j
,
1
(
k
*
)
-
φ
i
=
min
{
φ
j
,
1
(
k
)
-
φ
i
,
k
⊆
Z
}
=
min
(
φ
j
+
4
k
π
-
φ
i
,
k
⊆
Z
)
,
φ
j
,
2
(
h
*
)
-
φ
1
=
min
{
φ
j
,
2
(
h
)
-
φ
1
,
h
⊆
Z
}
=
min
{
-
φ
j
+
4
h
π
-
φ
i
,
h
⊆
Z
]
;
f) subsequently determination of
d j,1 =arccos( v i T v i )|+|φ j,1 ( k *)−φ i |,
d j,2 =arccos( v i T (−v j ))|+|φ j,2 ( h *)−φ i |
for j ⊂ I and of
d j,1 =|φ j,1 ( k *)−φ 1 |,
d j,2 =|φ j,2 ( h *)−φ 1 |,
for j ⊂ I;
g) then, selection of (v j , φ j ), wherein
(
v
j
,
φ
j
)
=
{
(
v
j
,
φ
j
,
1
(
k
*
)
)
,
d
j
,
1
≤
d
j
,
2
(
(
-
v
j
,
φ
j
,
2
(
h
*
)
)
,
d
j
,
1
>
d
j
,
2
for
j
⊆
1
,
and
(
v
j
,
φ
j
)
=
{
(
v
i
,
φ
j
,
1
(
k
*
)
)
if
d
j
,
1
≤
d
j
,
2
(
v
i
,
,
φ
j
,
2
(
h
*
)
)
if
d
j
,
1
>
d
j
,
2
for
j
⊆
I
.
23. Method in accordance with claim 22 , wherein the fitting of the freely selectable components of (α i , β i , φ i ) using
iMin:=min {i ⊂ I}
iMax:=max {i ⊂ I}
iMin:=min {j ⊂ J}
jMax:=max {j ⊂ J},
so that
α
P
=
{
α
i
Min
for
1
≤
p
<
j
Min
,
α
i
Max
for
j
Max
<
p
≤
n
and
β
P
=
{
β
i
Min
for
1
≤
p
<
i
Min
,
β
i
Max
for
i
Max
<
p
≤
n
.
24. A device for orientation control of a robotal device, the device comprising:
an interpolation unit;
an interpolation means located within said interpolation unit for interpolating a given dot sequence of poses by splines, said interpolation means representing an orientation represented by quarternions on a four-dimensional sphere according to
q(α,β,φ)=(cos(φ/2), sin(φ/2) v(α,β))
v(α, β)=(cos(α) cos(β), sin(α) cos(β), sin(β)), wherein α, β indicate spherical coordinates of rotating vectors v-v(α, β); and
means for calculating spherical coordinates (α i , β i ) of the v i for all i∈I:
v i =(cos(α i ) cos(β i ), sin(α i ) cos(β i ) sin(β i ));
and means for defining
J:=(j∈I with v j ≠(0,0,±1),
L:=(j∈I with v j =(0,0,±1),
to define expanded spherical coordinates from R×R
for i∈J:
(α i,1 ( h ),β i,1 ( k ))=(α i +2 h π,β i +2 k π) h,k∈Z;
(α i,2 ( l ),β i,2 ( m ))=(α i +π+2 l π,π−β i 2 m π) l,m∈Z;
for i∈L:
(α i ( x ),β i ( k ))=( x ,β i +2 k π) x∈R,k∈Z.
25. Device in accordance with claim 24 , wherein the interpolation means performs the following steps during the determination of pairs (α 1 , β 1 ) from the v 1 values in a representation in spherical coordinates:
setting of any desired start pair (α jStart , β jStart ) with jStart ⊂ J;
determination of expanded polar coordinates according to steps l) and m) once each for all j ⊂ I with the exception of j=jStart, wherein l ⊂ I is a neighboring index of j, so that there is no k ⊂ I between i and j, and, in addition, either is i=jStart true, or the reference pair (α 1 , β 1 was already fitted according to the steps l) and m);
determination of values h*, k*, l*, m ⊂ ⊂ Z with
d j,1 =min{|α j,1 ( h )−α i |+|β j,1 ( k )−β i |, with h,k ⊂ Z}=a j,1 ( h *)−α 1 |+|β j,1 ( k *)−β 1 |;
d j,2 =min{|α j,2 ( l )−α i |+|β j,2 ( m )−β i |, with l,m ⊂ Z}=α j,2 ( l *)−α 1 |+|β j,2 ( m *)−β 1 |
for j ⊂ J, and determination of a value k* ⊂ Z for j ⊂ L with
β j ( k *)−β 1 |−min{|β j ( k )−β 1 |k ⊂ Z};
then, selection of (α j , β j ), wherein
(
α
j
,
β
j
)
=
{
(
α
j
,
1
(
h
*
)
,
β
j
,
1
(
k
*
)
)
,
d
j
,
1
≤
d
j
,
2
(
α
j
,
2
(
l
*
)
,
β
j
,
2
(
m
*
)
)
,
d
j
,
1
>
d
j
,
2
for j ⊂ J, and
(α j ,β j )=(β i ,β j (k*))
for j ⊂ L;
selection of a number of triples determined according to steps a) through m)
(
α
1
,
β
1
,
φ
1
)
=
{
(
α
1
,
β
1
,
φ
1
)
for
i
⊆
J
(*
,
β
1
,
φ
1
)
for
i
⊆
L
(*
,
*
,
φ
1
)
for
i
⊆
I
,
wherein all components designated by “*” are freely selectable;
fitting of the selectable components into the triples (α i , β i , φ i );
interpolation of the triples (α i , β i , φ i ) selected in step n) by a spline; and
substitution of the interpolation functions in q(α i , β i , φ i ).
26. Device in accordance with claim 24 or 25 , wherein the interpolation means performs the following steps:
representation of each quaternion on the four-dimensional unit sphere according to
q (α,β,φ)=(cos(½φ), sin(½φ)· v (α,β))
with
v(α,β)=(cos(α) cos(β), sin(α) cos(β), sin(β)),
wherein α, β, (and r=1) indicate the spherical coordinates of the rotating vectors v=v(α, β);
determination of motion-optimal pairs (v, φ i ) by
a) fitting signs of q i , so that q i T q l+1 ≧0 for all i=1, . . . , n−1;
b) determination of a possible pair (v 1 , φ 1 ) for all i=1, . . . , n, with q 1 =(cos(½φ 1 ), sin(½φ 1 )v 1 )′
c) selection of any desired start pair (V iStart , φ iStart ) with iStart ⊂ I:=}i=1, . . . , n|q i ≠(±1, 0, 0 ,0)};
d) determination of exactly one pair (v i , φ i ) for all j ⊂ (1, . . . , n) with the exception of j=iStart, starting from a reference pair (v i , φ i ) according to steps e) through g), wherein i ⊂ (1, . . . , n) is a neighboring index, i.e., i=j1 or i=j−1 and, further, either is 1=1iStart true, or the reference pair was already fitted according to steps e) through g);
e) determination of k* h* ⊂ , so that
φ
j
,
1
(
k
*
)
-
φ
1
=
min
{
φ
j
,
1
(
k
)
-
φ
1
,
k
⊆
Z
}
=
min
{
φ
j
+
4
k
π
-
φ
i
,
k
⊆
Z
}
,
φ
j
,
2
(
h
*
)
-
φ
i
=
min
{
φ
j
,
2
(
h
)
-
φ
i
,
h
⊆
Z
}
=
min
{
-
φ
1
+
4
h
π
-
φ
i
,
h
⊆
Z
}
;
f) then, determination of
d j,1 =arccos( v 1 T v j )|+|φ j,1 ( k *)−(φ 1 |,
d j,2 =arccos( v 1 T (− v j ))|+|φ j,2 ( h *)−(φ 1 |
for j ⊂ I, and of
d j,1 =|φ j,1 ( k *)−φ i |,
d j,2 =|φ j,2 ( h *)−φ i |
for j ⊂ I;
g) then, selection of (v j , φ j ), wherein
(
v
j
,
φ
j
)
=
{
(
v
j
,
φ
j
,
1
(
K
*
)
)
,
d
j
,
1
≤
d
j
,
2
(
-
v
j
,
φ
j
,
2
(
h
*
)
)
,
d
j
,
1
>
d
j
,
2
for
j
⊆
I
,
and
of
(
v
j
,
φ
j
)
=
{
(
v
i
,
φ
j
,
1
(
k
*
)
)
if
d
j
,
1
≤
d
j
,
2
(
v
i
,
φ
j
,
2
(
h
*
)
)
if
d
j
,
1
>
d
j
,
2
for
j
⊆
I
.
27. Device in accordance with claim 26 , wherein the interpolation means is set up for fitting the freely selectable components of (α i , β i , φ i ) using
iMin:=min {i ⊂ I}
iMax:=max {i ⊂ I}
iMin:=min {j ⊂ J)
jMax:=max {j ⊂ J}
such that
α
p
=
{
α
1
min
for
1
≤
p
<
j
Min
,
α
1
Max
for
j
Max
<
p
≤
n
and
β
p
=
{
β
i
Min
for
1
≤
p
<
i
Min
,
β
i
Max
for
i
Max
<
p
≤
n
.