Euclidean metrics for motion generation on SE(3) - Semantic Scholar
Euclidean metrics for motion generation on SE (3) Calin Belta and Vijay Kumar
fcalin j [email protected] General Robotics, Automation, Sensing and Perception (GRASP) Laboratory University of Pennsylvania, Philadelphia, PA, USA
Abstract Previous approaches to trajectory generation for rigid bodies have been either based on the so-called invariant screw motions or on ad hoc decompositions into rotations and translations. This paper formulates the trajectory generation problem in the framework of Lie groups and Riemannian geometry. The goal is to determine optimal curves joining given points with appropriate boundary conditions on the Euclidean group. Since this results in a two-point boundary value problem that has to be solved iteratively, a computationally efficient, analytical method that generates near-optimal trajectories is derived. The method consists of two steps. The first step involves generating the optimal trajectory in an ambient space, while the second step is used to project this trajectory onto the Euclidean group. The paper describes the method, its applications, and its performance in terms of optimality and efficiency.
3401 Walnut St., Philadelphia, PA 19104-6228, Tel:(215) 898-5814
Notation IRn
SO(n) SE (n) GL(n) GA(n) so(n) se(n) fF g fM g O O0 R A B M d TP M ! v S X; Y V
A
< ; > G G~ Tr L0i Li x; y; z D dt
Euclidean space of dimension n special orthogonal group special Euclidean group general linear group of dimension n the affine group the Lie algebra of SO(n)
the Lie algebra of SE (n) reference (fixed) frame mobile frame
origin of fF g
origin of fM g
rotation matrix (2 SO(n))
homogeneous transformation matrix ( 2 SE (n)) affine transformation (2 GA(3)) nonsingular matrix (2 GL(3))) position vector in fF g
exponential coordinates on SO(3)
2 M to manifold M angular velocity in fM g linear velocity in fM g twist 2 se(3) tangent space at P
tangent vectors velocity vector acceleration vector Riemmanian metric matrix of metric in SO(3)
matrix of metric in SE (3) matrix trace basis vector in so(3) basis vector in se(3) Cartesian axes covariant derivative
1 Introduction The problem of finding a smooth motion that interpolates between two given positions and orientations in IR3 is well understood in Euclidean spaces [4, 8], but it is not clear how these techniques can be generalized to curved spaces. There are two main issues that need to be addressed, particularly on non-Euclidean spaces. It is desirable that the computational scheme be independent of the description of the space and invariant with respect to the choice of the coordinate systems used to describe the motion. Secondly, the smoothness properties
2
and the optimality of the trajectories need to be considered. Shoemake [14] proposed a scheme for interpolating rotations with Bezier curves based on the spherical analog of the de Casteljau algorithm. This idea was extended by Ge and Ravani [7] and Park and Ravani [13] to spatial motions. The focus in these articles is on the generalization of the notion of interpolation from the Euclidean space to a curved space. Another class of methods is based on the representation of Bezier curves with Bernstein polynomials. Ge and Ravani [6] used the dual unit quaternion representation of
SE (3) and subsequently applied Euclidean
methods to interpolate in this space. J¨utler [9] formulated a more general version of the polynomial interpolation by using dual (instead of dual unit) quaternions to represent
SE (3).
In such a representation, an element of
SE (3) corresponds to a whole equivalence class of dual quaternions. Park and Kang [12] derived a rational interpolating scheme for the group of rotations SO(3) by representing the group with Cayley parameters and using Euclidean methods in this parameter space. The advantage of these methods is that they produce rational curves. It is worth noting that all these works (with the exception of [13]) use a particular coordinate representation of the group. In contrast, Noakes et al. [10] derived the necessary conditions for cubic splines on general manifolds without using a coordinate chart. These results are extended in [2] to the dynamic interpolation problem. Necessary conditions for higher-order splines are derived in Camarinha et al. [1]. A coordinate free formulation of the variational approach was used to generate shortest paths and minimum acceleration and jerk trajectories on
SO(3) and SE (3) in [16].
However, analytical solutions are available only in the simplest of
cases, and the procedure for solving optimal motions, in general, is computationally intensive. If optimality is sacrificed, it is possible to generate bi-invariant trajectories for interpolation and approximation using the exponential map on the Lie algebra [15]. While the solutions are of closed-form, the resulting trajectories have no optimality properties. This paper is built on the results from [16, 15]. It is first showed that a left or right invariant metric on SO(3)
(SE (3)) is inherited from the higher dimensional manifold
GL(3) (GA(3)) equipped with the appropriate
metric. Next, a projection operator is defined and subsequently used to project optimal curves from the ambient
SO(3) and the projected geodesic from GL(3) follow the same path, but with a different parameterization. The line from GL(3) is then shown to be parameterizable to yield the exact geodesic on SO(3) by projection. Several examples are presented to illustrate manifold onto
SO(3) (SE (3)).
It is proved that the geodesic on
the merits of the method and to show that it produces near-optimal results, especially when the excursion of the trajectories is “small”.
2 Background 2.1 The Lie groups SO(3) and SE (3) Let GL(n) denote the general linear group of dimension n. As a manifold, GL(n) can be regarded as an open
subset of IRn . Moreover, matrix multiplication and inversion are both smooth operations, which make GL(n) 2
a Lie group. The special orthogonal group is a subgroup of the general linear group, defined as
SO(n) = R j R 2 GL(n); RRT = I; detR = 1 3
SO(n) is referred to as the rotation group on IRn . GA(n) = GL(n) IRn is the affine group. SE (n) = SO(n)IRn is the special Euclidean group, and is the set of all rigid displacements in IRn . Special consideration will be given to SO(3) and SE (3). Consider a rigid body moving in free space. Assume any inertial reference frame fF g fixed in space and a frame fM g fixed to the body at point O0 as shown in Figure 1. At each instance, the configuration (position and orientation) of the rigid body can be described by a homogeneous transformation matrix, A, corresponding to the displacement from frame fF g to frame fM g. SE (3) is the set of all rigid body transformations in 8
is defined on the tangent space at
each point on the manifold, such a form is called a Riemannian metric and the manifold is Riemannian [3]. If the form is non-degenerate but indefinite, the metric is called semi-Riemannian. On a n dimensional manifold, the metric is locally characterized by a
n n matrix of C 1 functions gij =< Xi ; Xj > where Xi are basis
vector fields1 . Note that the entries gij at each point on the manifold are functions of the local coordinates at that point. If the basis vector fields can be defined globally, then the matrix [gij ] completely defines the metric.
On SE (3) (on any Lie group), an inner product on the Lie algebra can be extended to a Riemannian metric
over the manifold using left (or right) translation. To see this, consider the inner product of two elements
S1 ; S2 2 se(3) defined by
< S1 ; S2 >jI = sT1 Gs2 ;
(6)
where s1 and s2 are the 6 1 vectors of components of S1 and S2 with respect to some basis and G is a positive definite matrix. If V1 and V2 are tangent vectors at an arbitrary group element
< V1 ; V2 >jA in the tangent space TA SE (3) can be defined by:
A 2 SE (3), the inner product
< V1 ; V2 >jA = < A?1 V1 ; A?1 V2 > I :
(7)
The metric obtained in such a way is said to be left invariant [3] since left translation by any element A is an isometry.
2.4 Affine connection, covariant derivative, and geodesic flow Any motion of a rigid body is described by a smooth curve A(t)
to the curve V (t) =
2 SE (3). The velocity is the tangent vector
dA (t). The vector dV (t) does not in general belong to the tangent space TA(t) SE (3), and, dt dt
therefore, usual differentiation of a vector field is not an ”intrinsic” geometric notion on SE (3). The notion of
covariant derivative allows one to take derivative of the velocity vector of A(t), which gives the acceleration of the curve in SE (3). The covariant derivative is the orthogonal projection of
1
dV (t) on TA(t) SE (3). dt
The basis vector fields need not be the coordinate basis; we only require that they are smooth.
6
An affine connection on SE (3) is a map that assigns to each pair of C 1 vector fields X and Y on SE (3)
another C 1 vector field rX Y which is IR-bilinear in X and Y and, for any smooth real function f on SE (3) satisfies rfX Y
= f rX Y
and rX fY
= f rX Y + X (f )Y . i k = ?ijk L i , where The Christoffel symbols ?jk of the connection at a point A 2 SE (3) are defined by rL j L L 1 ; : : : ; L 6 is the basis in TA SE (3) and the summation is understood. If A(t) is a curve and X is a vector field, the covariant derivative of X along A is defined by
DX = r X A_ (t) dt X is said to be autoparallel along A if DX=dt = 0. A curve A is a geodesic if A_ is autoparallel along A. An equivalent characterization of a geodesic is the following set of equations:
ai + ?ijk a_ j a_ k = 0
(8)
where ai , i = 1; : : : ; 6 is an arbitrary set of local coordinates on SE (3). For a manifold with a Riemannian (or pseudo-Riemannian) metric, there exists a unique symmetric connection which is compatible with the metric [3]:
X < Y; Z >=< rX Y; Z > + < Y; rX Z >; rX Y ? rY X = XY ? Y X Given a connection, the acceleration and higher derivatives of the velocity can be defined. The acceleration,
A(t), is the covariant derivative of the velocity along the curve:
D dA = r V A = dt V dt
(9)
2.5 Exponential map If M is a manifold with a connection r, the exponential map at an arbitrary q
2 M is defined as follows. Let
V (t) be the unique geodesic passing through q at t = 0 with velocity V , i.e. V (0) = q and _ V (0) = V . Then, by definition, expq maps V 2 Tq M to the point V (1) 2 M . Using homogeneity of geodesics, it is easy to prove [3] that tV (s) = V (ts) which gives expq (tV ) = V (t). Also, expq is a diffeomorphism of a neighborhood of 0 2 Tq M to a neighborhood of q 2 M . This gives a local chart for M called normal coordinates. These coordinates are convenient for computations (as in this work) because rays through
0 are
geodesics.
SO(3) with metric G = I is given special consideration in this paper. For R 2 SO(3) and V 2 TR SO(3), one can define expR (V ) = ReRT V . If v = [v1 v2 v3 ] is the expansion of V o1 + v2 L o2 + v3 L o3 ), it is easy to see that expR (V ) = Rev^. As a in the local basis of TR SO(3) (i.e. V = v1 L special case, for S 2 so(3), expI (S ) = e^ , where = [1 2 3 ] is the expansion of S in the basis Lo1 ; Lo2 ; Lo3 . This gives a local parameterization of SO(3) around identity known as exponential coordinates. The exponential map on
2.6 Local parameterization of SE (3)
SE (3) induced by the product structure SO(3) IR3 is chosen. words, a set of coordinates 1 , 2 , 3 , d1 , d2 , d3 for an arbitrary element A 2 SE (3):
In this paper, a parameterization of
2
A=4
R d 0 1
7
3 5
In other
is defined so that d1 , d2 , d3 are the coordinates of d in IR3 . Exponential coordinates, as defined in Section 2.5,
are chosen as local parameterization of SO(3). For R 2
SO(3) sufficiently close to the identity (i.e excluding the points Tr(R) = ?1 (Tr(A) = 0), or, equivalently, rotations through angles up to ), the exponential
coordinates are given by:
R = e^ ; 2 IR3
2.7 Screw motions One of the fundamental results in rigid body kinematics was proved by Chasles at the beginning of the 19th century: Any rigid body displacement can be realized by a rotation about an axis combined with a translation parallel to that axis. Note that a displacement must be understood as an element of a curve on
SE (3).
SE (3) while a motion is
If the rotation from Chasles’s theorem is performed at constant angular velocity and the
translation at constant translational velocity, the motion leading to the displacement becomes a screw motion. Chasles’s theorem then says that any rigid body displacement can be realized by a screw motion. A curve A(t) on a Lie group is called a one-parameter subgroup if A(t + s)
are equivalent ways of defining a screw motion A(t) 2 SE (3):
= A(t)A(s).
The following
A?1 (t)A_ (t) = constant. f!; vg = constant. A(t) is a one-parameter subgroup of SE (3). The tangent vectors A_ (t) to the curve form a left invariant vector field. With this mathematical definition, the Chasles’s theorem can be restated in the form: For every element in
SE (3) different from identity, there is a unique one-parameter subgroup to which that element belongs. Note that the definition of an one-parameter subgroup is not dependent on a metric. Given two end positions on
SE (3), one concludes that there always exists a interpolating screw motion.
Is this motion physically meaningful and/or optimal from some point of view? To talk about optimality, one first needs to define a metric on the manifold. Optimal interpolating motions with respect to a given metric are geodesics, minimum acceleration curves, minimum jerk curves, and so on. What is the connection between geodesics as defined in Section 2.4 and screw motions (one-parameter subgroups)? The following result is true for any Lie group [3]: for a bi-invariant metric, the geodesics that start from identity are one-parameter subgroups. As a particular case, geodesics through identity on SO(3) with metric G = I are one-parameter subgroups
(! = constant). Also, for the bi-invariant semi-Riemannian metric on SE (3) 2
3
I I G = 4 3 3 5 ; ; > 0 I3 0
(10)
geodesics through identity are screw motions. The conclusion is that an interpolating screw motion is not the appropriate choice if the metric on SE (3) is different from the bi-invariant metric (10), which is the case of the kinetic energy metric.
8
3 Riemannian metrics on SO(3) and SE (3) In this section it is shown that there is a simple way of defining a left or right invariant metric in SO(3) (SE (3))
by introducing an appropriate constant metric in GL(3) (GA(3)). Defining a metric at the Lie algebra so(3) (or
se(3)) and extending it through left (right) translations is equivalent to inheriting the appropriate metric from the ambient manifold at each point. In this paper, only metrics on SE (3) which are products of the bi-invariant metric on SO(3) and the Euclidean metric on IR3 are considered. A more general treatment accomodating arbitrary metrics on SO(3) is to be published elsewhere. 3.1 Metrics on GL(3) and SO(3) For any M
2 GL(3) and any X; Y 2 TM GL(3), define < X; Y >GL= Tr(X T Y )
(11)
Tr denotes the trace of a square matrix and TM GL(3) is the tangent space to GL(3) at M , which is isomorphic to GL(3). By definition, form (11) is the same at all points in GL(3). It is easy to see that GL where
is a positive definite quadratic form in the entries of
X and Y , therefore a metric. This induces the Euclidean
norm on TM GL(3), also called Frobenius matrix norm.
Proposition 3.1 The metric given by (11) defined on GL(3) is bi-invariant when restricted to SO(3). Proof: Let any Then, we have
M 2 GL(3) and any vectors X , Y
in the tangent space at an arbitrary point of
GL(3).
< X; Y >GL= Tr(X T Y ); < MX; MY >GL= Tr(X T M T MY )
from which we conclude that the metric is invariant under left translations with elements from SO(3). Therefore, when restricted to have
SO(3), the metric becomes left invariant.
For right invariance, if
R 2 SO(3), we
< X; Y >GL= Tr(Y X T ); < XR; Y R >GL= Tr(Y RRT X T ) = Tr(Y X T )
and the claim is proved. To find the induced metric on SO(3), let R be an arbitrary element from SO(3), X; Y be two vectors from
TR SO(3) and Rx (t); Ry (t) the corresponding local flows so that
X = R_ x (0); Y = R_ y (0); Rx (0) = Ry (0) = R: The metric inherited from GL(3) can be written as:
< X; Y >SO =< X; Y >GL= Tr(R_ xT (0)R_ y (0)) = Tr(R_ xT (0)RRT R_ y (0)) = Tr(^!xT !^y ) where ! ^x
= Rx(0)T R_ x (0) and !^y = Ry (0)T R_ y (0) are the corresponding twists from the Lie algebra so(3).
If the above relation is written using the vector form of the twists, some elementary algebra leads to:
< X; Y >SO = 2!xT !y A different equivalent way of arriving at expression (12) would be defining the metric in identity of SO(3)) as being the one inherited from TI GL(3):
gij = Tr(Loi T Loj ) = ij ; i; j = 1; 2; 3 9
(12)
so(3) (i.e.
at
where
Lo1 ; Lo2 ; Lo3 is the basis in so(3) and ij is the Kronecker symbol.
Left or right translating this metric
throughout the manifold is equivalent to inheriting the metric at each 3-dimensional tangent space of from the corresponding 9-dimensional tangent space of GL(3). Remark 3.2 The matrix G of the metric as defined in (6) is G
SO(3)
= 2I , which is the standard scale-independent
bi-invariant metric on SO(3). This is consistent with Proposition 3.1.
Remark 3.3 The metric given by (12) can be interpreted as the (rotational) kinetic energy metric of a spherical rigid body.
3.2 Metrics on GA(3) and SE (3) Let X and Y be two vectors from the tangent space at an arbitrary point of GA(3) (X and Y are 4 4 matrices with all entries of the last row equal to zero). Similar to Section 3.1, a quadratic form defined by
< X; Y >GA= Tr(X T Y )
(13)
is a point-independent Riemmanian metric on GA(3).
We can get a left invariant metric on SE (3) by inheriting the metric
GA given by (13) from GA(3).
To derive the induced metric in SE (3) we follow the same procedure as in Section 3.1. Let A be an arbitrary element from
SE (3). Let X; Y
be two vectors from TA SE (3) and Ax (t); Ay (t) the
corresponding local flows so that
X = A_ x (0); Y = A_ y (0); Ax (0) = Ay (0) = A: Let
2
Ai (t) = 4
Ri (t) di (t) 0
1
3 5
; i 2 fx; yg
and the corresponding twists at time 0: 3
2
!^ v Si = A?i 1 (0)A_ i (0) = 4 i i 5 ; i 2 fx; yg 0 0
The metric inherited from GA(3) can be writen as:
< X; Y >SE =< X; Y >GA = Tr(A_ Tx (0)A_ y (0)) = Tr(SxT AT ASy ) Now using the orthogonality of the rotational part of A and the special form of the twist matrices, straightforward calculations lead to
< X; Y >SE = Tr(SxT Sy ) = Tr(^!xT !^y ) + vxT vy
or, equivalently,
2
3
2
2I 0 ! < X; Y >SE = !xT vxT G~ 4 y 5 ; G~ = 4 3 vy 0 I3 h
i
Remark 3.4 Metric (14) is the scale-independent metric on
5
(14)
SE (3) proposed by Park and Brockett [11] for
= 2 and = 1. It is a product metric and has been extensively studied in [16]. 10
3
SE (3) can be provided with the same metric (14) by
Remark 3.5 Straightforward calculations show that inheriting the metric from the ambient space at se(3):
8 > >
ij ; > :
i; j = 1; 2; 3 i; j = 4; 5; 6
0;
elsewhere
and left translating it throughout the manifold. Therefore, the metric invariant when restricted to SE (3).
Tr(X T Y ) from GA(3) becomes left
Remark 3.6 Metric (14) can be interpreted as being the kinetic energy of a moving (rotating and translating)
spherical rigid body when the body fixed frame fM g is placed at the centroid of the body and aligned with its principal axes.
4 Projection on SO(3)
GL(3). Using this distance, for a given M 2 GL(3), we define the projection of M on SO(3) as being the closest R 2 SO(3) We can use the norm induced by metric (11) to define the distance between elements in with respect to norm
k kGL.
The following proposition gives the solution of the projection problem for the
general case of GL(n):
Proposition 4.1 Let M
2 GL(n) and M = U V T its singular value decomposition. Then the projection of
M on SO(n) is given by R = UV T .
Proof: The problem to be solved is a minimization problem:
min kM ? Rk2GL
R2SO(n) With M
= U V T , we have
Note that
kM ? Rk2GL = Tr[(M ? R)T (M ? R)] = = Tr(M T M ? M T R ? RT M + RT R)
RT R = I , Tr(M T R) = Tr(RT M ) and the quantity M T M is a constant and therefore does not
affect the optimization. Therefore, the problem to be solved becomes:
max Tr(M T R)
R2SO(n) Let = diagf1 ; : : : ; n g, C
= RT U and consider columnwise partitions for V
and C
V = [v1 ; : : : ; vn ]; C = [c1 ; : : : ; cn ] Then, we have:
Tr(M T R) = Tr( Now C and V are both orthogonal, and then
n X i=1
i vi cTi ) =
n X i=1
i viT ci
kci k = kvi k = 1 On the other, by Cauchy-Schwartz, (viT ci )2
kvi k2 kci k2 = 1 and the equality holds for vi = ci , or V = C . Tr(M T R) which is attained for R = UV T .
11
Therefore,
Pn
i=1 i is an upper bound for
Remark 4.2 It is easy to see that the distance between M and
P R in metric (11) is given by ni=1 (i ? 1)2 ,
which is the standard way of describing how ”far” a matrix is from being orthogonal. The question one might ask is what happens with the solution to the projection problem when the manifold
GL(n) is acted upon by the group SO(n). The answer is given in the following proposition: Proposition 4.3 The solution to the projection problem described above is both left and right invariant under actions of elements from SO(n).
M 2 GL(n), M = U V T and the corresponding projection R 2 SO(n), R = UV T . = LM . Then an SVD for M can be found from the SVD for M in the form Let any L 2 SO(n) and M M = (LU )V T . Then, by Proposition 4.1, the projection of M is R = LUV T = LR, which proves left = ML = U (V T L) projects to invariance. Similarly, if M is acted from the right by L 2 SO(n), then M R = UV T L = RL, which implies right invariance. Proof: Let
It is worthwhile to note that other projection methods do not exhibit bi-invariance. For instance, it is cus-
tomary to find the projection R 2 SO(n) by applying a Gram-Schmidt procedure (QR decomposition). In this case it is easy to see that the solution is left invariant, but in general it is not right invariant.
5 Projection on SE (n) Similar to the previous section, if a metric of form (13) is defined in
SE (n) can be found. Proposition 5.1 Let B
2 GA(n) with the following block partition 2
B=4 and B1
= USigmaV T
B1 B2 0
1
3 5
; B1 2 GL(n); B2 2 IRn
the singular value decomposition of B1 . Then the projection of B on SE (n) is given
by
2
A=4 Proof: Let
GA(n), the corresponding projection on
2
A=4
UV T 0
R d 0 1
B2 1
3 5
2 SE (n)
3 5
; R 2 SO(n); d 2 IRn
The problem to be solved can be formulated as follows:
min kB ? Ak2GA
A2SE(n) Then
kB ? Ak2GA = Tr[(B ? A)T (B ? A)] = Tr(B T B ) ? 2Tr(B T A) + Tr(AT A)
The quantity B T B is not involved in the optimization. The observation that
Tr(B T A) = Tr(B1T R) + (B2T d + 1); Tr(AT A) = 4 + dt d 12
separates the initial problem into two subproblems:
(a) max Tr(B1T R) R2SO(n)
and
(b) minn ?2B2T d + dT d
d2IR From Proposition 4.1, the solution to subproblem (a) is R = UV T . For the second subproblem, let
f : IRn ! IR; f (x) = ?2B2T x + xT x The critical points of the scalar function f are given by
rf (x) = ?2B2 + 2x = 0 ) x = B2 and the Hessian r2 f (x)
= 2I is always positive definite. Therefore, the solution is d = B2 which concludes
the proof. Similar to SO(n), invariance properties are exhibited by the projection on SE (n): Proposition 5.2 The solution to the projection problem on SE (n) is left invariant under actions of elements from SE (n). The projection is bi-invariant under rotations. Proof: Let
2
B1 B2
B=4
0
and define A, U , , V such that
1
2
5
2 GA(n)
UV T
B1 = U V T ; A = 4 Let
3
B2
0
2
X=4
R d 0 1
1
3 5
2 SE (n)
3 5
be an arbitrary element from SE (n). Under left actions of X , the solution pair becomes 2
XB = 4 2
XA = 4
RB1 RB2 + d 0
1
RUV T 0
3 5
RB2 + d 1
; 3 5
which proves left invariance of the projection. For the second part, note that the right translated solution pair is 2
BX = 4 2
AX = 4
B1 R B 1 d + B 2 0
1
3 5
UV T R UV T d + B2 0
1
; 3 5
It is easy to see that B1 R = U V T R. If only rotations (d = 0) are taken into consideration, right invariance is proved.
13
6 Generating smooth curves on SE (3) Based on the results from the previous sections, a procedure for generating near optimal curves on SE (3) follows: generate the curves in the ambient space and project them onto
SE (3). Due to the fact that the defined
metric in GA(3) is the same at all points, the corresponding Christoffel symbols are all zero. Consequently, the
optimal curves in the ambient space assume simple analytical forms (i.e geodesics - straight lines, minimum acceleration curves - cubic polynomial curves, minimum jerk curves - fifth order polynomial curves, all parameterized by time). The resulting curve in GA(3) is linear in the boundary conditions, and therefore left and right
invariant. Recall that the projection procedure on SE (3) is left invariant, and so is the overall procedure.
The focus is on SO(3). Due to the product structure of both SE (3) = SO(3) IR3 and the metric SE
for a = 0, all the results are straightforward to extend to SE (3).
6.1 Geodesics on SO(3) The problem to be solved is generating a geodesic R(t) between given end positions R1 on SO(3). Without loss of generality, it is assumed R1
= R(0) and R2 = R(1)
= I . Indeed, a geodesic between two arbitrary positions
R1 and R2 is the geodesic between I and R1?1 R2 left translated by R1 . Exponential coordinates 1 ; 2 ; 3 are considered as local parameterization of SO(3). If R2 = e!^ 0 , then the geodesic is the exponential mapping of
the uniformly parameterized segment passing through 0 and !0 ( (t) = !0 t) from the exponential coordinates:
R(t) = e^(t) = e!^ 0 t The geodesic in the ambient manifold GL(3) satisfying the given boundary conditions on SO(3) is
M (t) = I + (R2 ? I )t; t 2 [0; 1] An analytical expression for the projection of an arbitrarily parameterized line in the ambient
GL(3) onto
SO(3) is derived, which will answer the following three questions. Does the projection of a geodesic from GL(3) follow the same path as the true geodesic on SO(3)?
If
the answer is yes, then the following question makes sense:
Do the above two curves have the same parameterization? If the answer is no, Can one find an appropriate parameterization of the line in the ambient manifold so that the projection is identical to the true geodesic on SO(3)?
Proposition 6.1 is the key result of this section.
M (t) = I + (R2 ? I )f (t); t 2 [0; 1] be a line in GL(3) with R2 = e!^ 0 2 SO(3) (f continuous, f (0) = 0; f (1) = 1). Then the projection of this line R? (t) onto SO(3) is the exponential mapping of a segment drawn between the origin and !0 in exponential coordinates parameterized by (t): Proposition 6.1 Let
M (t) = U (t)(t)V T (t) ) R?(t) = U (t)V T (t) = e!^ 0 (t) (t) = k!1 k atan2(1 ? f (t) + f (t) cos k!0k; f (t) sin k!0k) 0
14
(15) (16)
= I + (R2 ? I )f (t) = U (t)(t)V (t)T is needed, where R2 = e!^ 0 and f (t) is a continuous function defined on [0; 1] satisfying f (0) = 0, f (1) = 1. The first observation is Proof: The SVD decomposition of M (t)
M T (t)M (t) = I ? f (t)(1 ? f (t))N; N = 2I ? R2 ? R2T The eigenstructure of the constant and symmetric matrix N completely determines the SVD of M (t). Because
N is symmetric and real, its eigenvalues will be real and the corresponding eigenspaces orthogonal. Let i ; vi be an eigenvalue-eigenvector pair of N . Then, Nvi = i vi ) M T (t)M (t) = (1 ? f (t)(1 ? f (t))i )vi So, theoretically, the desired SVD decomposition is determined at this moment:
V (t) can be chosen constant of the form V = [v1 v2 v3 ] where v1 ; v2 ; v3 are orthonormal eigenvectors of N ; The singular values are given by s2i (t) = 1 ? f (t)(1 ? f (t))i (it will be shown shortly that the right The matrix
hand side of this equality is always positive)
The time-dependence of the projection will be contained in
U (t) = [u1 (t) u2(t) u3 (t)]; ui (t) = M (st)vi ; i = 1; 2; 3 i
Using the Rodrigues formula for R2
= e!^ 0 , it is easy to see that N = ? 1 ? cos k!0 k (^!2 + !^ 2T )
k!0k2
0
0
from which it follows that the eigenvalues of N are given by
(N ) = f0; 2(1 ? cos k!0k); 2(1 ? cos k!0k)g and a set of three orthonormal eigenvectors by 8 > > < > > :
where !0
2
?!3
1 !0 6 k!0k ; p!32 + !12 4 0 6
!1
3 7 7 5
2
;
6 1 6 !2 + ! 2 2 2 2 ! ! + (! + !1 ) + !3 !2 4 3
p
2 2 2 1
2 3
39 > > 7= 2 7 1 5> > ;
?!2!1
?!3!2
= [!1 !2 !3 ]T . With the eigenstructure of N determined, one can write p
(t) = diagf1; s(t); s(t)g; s(t) = 2(1 ? cos k!0 k)f 2 (t) ? 2(1 ? cos k!0 k)f (t) + 1
(17)
where the binomial under the square root is always positive because it is positive at zero and 1 ? cos k!0 k
(0; 2) gives a negative discriminant. Some straightforward but rather tedious calculation leads to: 2 U (t)V T = I + !^0 (t) + (1 ? (t)) + !^0
k!0 k
where
2
1
2
k!0 k2
k!0 k
1 (t) = 1 ? f (t) +sf(t()t) cos k!0 k ; 2 (t) = f (t) ssin (t)
k!0 k 2 (0; ) (in accordance to the exponential coordinates) which will give
2 (t) > 0. Note that (t) + (t) = 1, so it is appropriate to define a function (t) 2 (0; 1) so that The discussion is restricted to 2 1
2 2
1 (t) = cos(k!0 k(t)); 2 (t) = sin(k!0k(t)) 15
By use of the Rodrigues formula again,
U (t)V T = e!^ 0 (t)
so the projected line is the exponential mapping of a segment between the origin and !0 in exponential coordinates. The parameterization of the segment is given by
(t) = k!1 k atan2(1 ? f (t) + f (t) cos k!0k; f (t) sin k!0k) 0
Note that the obtained parameterization (t) satisfies the boundary conditions (0) = 0, (1) = 1
As a particular case of Proposition 6.1 for f (t) = t, the following corollary answers the first two questions at the beginning of this section: Corollary 6.2 The true geodesic on SO(3) and the projected geodesic from GL(3) with ends on SO(3) follow
the same path on SO(3) but with different parameterizations. The projected curve is the exponential mapping
of the same segment from the exponential coordinates
R?(t) = e!^ 0 (t) with the following parameterization
(t) = k!1 k atan2(1 ? t + t cos k!0k; t sin k!0 k) 0
The derivative of the function (t) is given by
sin k!0 k d dt (t) = k!0ks(t) where s(t) is given by (17). Plots of the function (t) and its derivative are given in Figure 2 for t 2 [0; 1] and the magnitude of the displacement on the manifold k!0 k 2 (0; ). The conclusion is that even though the line in GL(3) is followed at constant velocity, the projected curve on the manifold has low speed at the beginning, attains its maximum in the middle, and slows down as approaching the end point. The larger the displacement k!0 k, the larger the discrepancy in speeds. Also note that the middle of the line is projected into the middle of the true geodesic because (0:5)
are equal at t
= 0:5 (i.e the functions t and (t)
= 0:5). This result has been stated in [14] in the context of unit quaternions as local parameters
of SO(3) (viewed as the unit sphere S 3 in the projective space IRP 3 ).
f (t) (f (0) = 0, f (1) = 1) of the line in GL(3) with ends on SO(3), which gives uniform parameterization t of the projected curve in exponential coordinates. The solution of the following equation in f To answer the third question, one needs to find a parameterization
atan2(1 ? f (t) + f (t) cos k!0 k; f (t) sin k!0 k = t is of the form
k!0 kt) f (t) = sin(k! k(1sin( ? t)) + sin(k!0kt) 0
The answer to the third question is stated in the following corollary:
16
(a)
(b)
1 0.75 0.5 0.25 0
2 1.5 1 0.5 0
1 0.8 0.6 0.4 Time
1 Displacement
1 0.8 0.6 0.4 Time
1
0.2
2
0.2
2
Displacement 3 0
3 0
Figure 2: (a) Function (t); (b) The derivative
SO(3) starting at I following line from the ambient manifold GL(3):
Corollary 6.3 The true geodesic on
and ending at
d dt (t)
R2 = e!^ 0
is the projection of the
k!0 kt) M (t) = I + (R2 ? I )f (t); t 2 [0; 1]; f (t) = sin(k! k(1sin( ? t)) + sin(k!0kt) 0 Illustrative plots of f (t) and its derivative are given in Figure 3 for t 2 [0; 1] and different values of the displacement k!0 k 2 (0; ). As expected, to get a uniform speed on SO(3), the line in GL(3) should be followed at high speed at the beginning, slow down in the middle, and accelerating again near the end point. Remark 6.4 The result in Corollary 6.3 is similar to the formula for spherical linear interpolation ’Slerp’ in terms of quaternions [14]. The curve interpolating q1 and q2 , with parameter u moving from 0 to 1, is given by:
where q1 q2
= cos .
u q Slerp(q1 ; q2 ; u) = sin(1sin?u) q1 + sin sin 2
6.2 Minimum acceleration curves on SO(3) First, the computation of optimal trajectories described in [16] is summarized. Then, near optimal trajectories are generated via the projection method. The necessary conditions for the curves that minimize the square of the
L2 norm of the acceleration are
found by considering the first variation of the acceleration functional
La = where V (t) =
Z
a
b
< rV V; rV V > dt;
(18)
dR(t) , r is the symmetric affine connection compatible with a suitable Riemmanian metric, and dt
R(t) is a curve on the manifold.
The initial and final points as well as the initial and final velocities for the
motion are prescribed.
17
(a)
(b)
1 0.75 0.5 0.25 0
1.5 1 0.5 0
1 0.8 0.6 0.4 Time
1 Displacement
1 0.8
1
0.2
2
0.6 0.4 Time
Displacement
0.2
2
3 0
3 0
Figure 3: (a) Function f (t); (b) The derivative
d dt f (t)
The following result is stated and proved in [16]:
R(t) be a curve between two prescribed points on SO(3) with metric SO that has prescribed initial and final velocities. If ! is the vector from so(3) corresponding to V = dR dt , the curve minimizes the cost function La derived from the canonical metric only if the following equation hold:
Proposition 6.5 Let
!(3) + ! ! = 0
(19)
where ()(n) denotes the nth derivative of (). The above equation can be integrated to obtain
!(2) + ! !_ = constant
(20)
However, this equation cannot be further integrated analytically for arbitrary boundary conditions. In the special case when the initial and final velocities are tangential to the geodesic passing through the same points, the solution can be found by re-parameterizing the geodesic [16]. In the general case, one must solve equation (19) numerically. A local parameterization of
SO(3) should be chosen and three first order differential equations
will augment the system. The most convenient local coordinates are the exponential coordinates. Eventually, one ends up with solving a system of
12 first order nonlinear coupled differential equations with 6 boundary
conditions at each end. A solution can be found using iterative procedures such as the shooting method or the relaxation method. The latter has been chosen in this paper. A more attractive and much simpler approach is the projection method described above. The main idea is to
GL(3), while keeping the proper boundary conditions on SO(3) with the corresponding velocities. Minimum acceleration curves are found in GL(3) and eventually projected back onto SO(3). In what follows, the time interval will be t 2 [0; 1] and the boundary conditions R(0), R(1), R_ (0), R_ (1) are assumed to be specified. The minimum acceleration curve in GL(3) with a constant metric GL is a cubic relax the problem to
18
(a)
(b)
(c)
1.6
True Projection
1.6 1.4
1.4
True Projection
1.2
1.2 1
1
2
True Projection
0.8
σ3
σ
3
0.8 1.5
σ3
0.6 0.4
1.4
1 1.2
0.5
0.2
1
0
0 1.5 1
0
0.2
0.6
0.2 0
σ
0.8
σ
1
1.5 1
0
0.4
0.4
0.4
0.5
0.2 0 −0.5
0.6
0.2
0.6
σ2
0.4
0.8
0
0.8
0.6
σ2
0
1
0.5
0.5 1
0
σ
σ2
1
Figure 4: Minimum acceleration curves on SO (3) with canonical metric: (a) Velocity boundary conditions along the geodesic; (b) End velocity perturbed by e1 ; and (c) End velocity perturbed by 5e1 .
given by
M (t) = M0 + M1 t + M2 t2 + M3t3 ; where M0 , M1 , M2 , M3
2 GL(3) are M0 = R(0); M1 = R_ (0);
M2 = ?3R(0) + 3R(1) ? 2R_ (0) ? R_ (1); M3 = 2R(0) ? 2R(1) + R_ (0) + R_ (1): Now the curve on SO(3) is obtained by projecting M (t) onto SO(3) as described in Section 4. The following examples present comparisons between the minimum acceleration curves generated using the projection method and the curves obtained directly on SO(3) by solving equations (19) using the relaxation method. All the generated curves are drawn in exponential local coordinates. In Figure 6.2, the following position boundary conditions were used: h
(0) = 0 0 0
iT
iT h ; (1) = 6 3 2
(21)
The initial velocity is the one corresponding to the geodesic passing through the two positions: iT h !0 = 6 3 2
Cases (a), (b) and (c) differ by the velocity at the end point. Figure 6.2 (a) corresponds to a final velocity
!1 = !0 , therefore one gets a minimum acceleration curve for which the end velocities are along the velocity of the corresponding geodesic, leading to a geodesic parameterized by a cubic of time [16]. The final velocity is !1
= !0 + e1 in case (b) and !1 = !0 + 5e1 in case (c), where e1 = [1 0 0]T .
As seen in Figure 6.2 (a), the paths of the projected and the optimal curves are the same, the parameterizations are slightly different though, as expected. In cases (b) and (c), although the deviation of the final velocity
19
(b)
14
14
12
12
10
10
8
8
6
6
z
z
(a)
4
4
2
2
0
0
−2 15
−2 15 10
10
10
10
8 5
8 5
6
6
4 0
4 0
2
2
0 y
−5
−2
0 −5
y
x
−2
x
Figure 5: Minimum acceleration motion for a cube in free space: (a) relaxation method; (b) projection method.
from being homogeneous is large, the curves are close. Note that the boundary conditions are rigorously satisfied.
6.3 Motion generation on SE (3) Since a method to generate (near) optimal curves on 3
SO(3) has been developed, the extension to SE (3) is
simply adding the well known optimal curves from IR . A homogeneous cubic rigid body is assumed to move (rotate and translate) in free space. The body frame
fM g is placed at the center of mass and alligned with the principal axes of the body. A small square is drawn on one of its faces. The trajectory of the center of the cube is starred. The following boundary conditions were considered: h
(0) = 0 0 0
iT
h
!(0) = 1 2 3 h
d(0) = 0 0 0 h
d_(0) = 1 1 1
h
; (1) = 6
iT
iT
2 3
i T 2
h
; !(1) = 2 1 1 h
; d(1) = 8 10 12
iT
iT
iT
h iT ; d_(1) = 1 5 3
True and projected minimum acceleration motions for a cubic rigid body with a = 2 and m = 12 are given in Figure 6.3 for comparison. In this example, the total displacement between initial and final positions on SO(3) is large. If the rotational displacement is restricted near the origin of exponential coordinates, the simulated motions look identitical.
20
7 Conclusion This paper presented a method for generating smooth trajectories for a moving rigid body with specified bound-
SE (3), the set of all rigid body translations and orientations, was seen as a submanifold (and a subgroup) of the Lie group of affine maps in IR3 , GA(3). The method involved two key steps: (1) the generation of optimal trajectories in GA(3); and (2) the projection of the trajectories from GA(3) to SE (3). The overall ary conditions.
procedure was proved to be invariant with respect to both the local coordinates on the manifold and the choice of the inertial frame. The projected geodesic from
GL(3) and the actual geodesic on SO(3) were shown to
have identical paths on SO(3). The parameterization of the line whose projection is the actual geodesic was derived.
References [1] M. Camarinha, F. Silva Leite, and P. Crouch. Splines of class ck on non-Euclidean spaces. IMA J. Math. Control Inform., 12(4):399–410, 1995. [2] P. Crouch and F. Silva Leite. The dynamic interpolation problem: on Riemannian manifolds, Lie groups, and symmetric spaces. J. Dynam. Control Systems, 1(2):177–202, 1995. [3] M. P. do Carmo. Riemannian geometry. Birkhauser, Boston, 1992. [4] G. E. Farin. Curves and surfaces for computer aided geometric design : a practical guide. Academic Press, Boston, 3 edition, 1992. [5] J. Gallier. Curves and Surfaces in Geometric Modeling - Theory and Algorithms. Morgan Kaufmann Publishers, San Francisco, CA, 2000. [6] Q. J. Ge and B. Ravani. Computer aided geometric design of motion interpolants. ASME Journal of Mechanical Design, 116:756–762, 1994. [7] Q. J. Ge and B. Ravani. Geometric construction of Bezier motions. ASME Journal of Mechanical Design, 116:749–755, 1994. [8] J. Hoschek and D. Lasser. Fundamentals of Computer Aided Geometric Design. AK Peters, 1993. [9] B. J¨utler. Visualization of moving objects using dual quaternion curves. Computers and graphics, 18(3):315–326, 1994. [10] L. Noakes, G. Heinzinger, and B. Paden. Cubic splines on curved spaces. IMA J. of Math. Control & Information, 6:465–473, 1989. [11] F. C. Park and R. W. Brockett. Kinematic dexterity of robotic mechanisms. International Journal of Robotics Research, 13(1):1–15, 1994. [12] F. C. Park and I. G. Kang. Cubic interpolation on the rotation group using Cayley parameters. In Proceedings of the ASME 24th Biennial Mechanisms Conference, Irvine, CA, 1996. [13] F. C. Park and B. Ravani. Bezier curves on Riemannian manifolds and Lie groups with kinematics applications. ASME Journal of Mechanical Design, 117(1):36–40, 1995. [14] K. Shoemake. Animating rotation with quaternion curves. ACM Siggraph, 19(3):245–254, 1985.
21
ˇ [15] M. Zefran and V. Kumar. Interpolation schemes for rigid body motions. Computer-Aided Design, 30(3), 1998. ˇ [16] M. Zefran, V. Kumar, and C. Croke. On the generation of smooth three-dimensional rigid body motions. IEEE Transactions on Robotics and Automation, 14(4):579–589, 1995.
22
fcalin j [email protected] General Robotics, Automation, Sensing and Perception (GRASP) Laboratory University of Pennsylvania, Philadelphia, PA, USA
Abstract Previous approaches to trajectory generation for rigid bodies have been either based on the so-called invariant screw motions or on ad hoc decompositions into rotations and translations. This paper formulates the trajectory generation problem in the framework of Lie groups and Riemannian geometry. The goal is to determine optimal curves joining given points with appropriate boundary conditions on the Euclidean group. Since this results in a two-point boundary value problem that has to be solved iteratively, a computationally efficient, analytical method that generates near-optimal trajectories is derived. The method consists of two steps. The first step involves generating the optimal trajectory in an ambient space, while the second step is used to project this trajectory onto the Euclidean group. The paper describes the method, its applications, and its performance in terms of optimality and efficiency.
3401 Walnut St., Philadelphia, PA 19104-6228, Tel:(215) 898-5814
Notation IRn
SO(n) SE (n) GL(n) GA(n) so(n) se(n) fF g fM g O O0 R A B M d TP M ! v S X; Y V
A
< ; > G G~ Tr L0i Li x; y; z D dt
Euclidean space of dimension n special orthogonal group special Euclidean group general linear group of dimension n the affine group the Lie algebra of SO(n)
the Lie algebra of SE (n) reference (fixed) frame mobile frame
origin of fF g
origin of fM g
rotation matrix (2 SO(n))
homogeneous transformation matrix ( 2 SE (n)) affine transformation (2 GA(3)) nonsingular matrix (2 GL(3))) position vector in fF g
exponential coordinates on SO(3)
2 M to manifold M angular velocity in fM g linear velocity in fM g twist 2 se(3) tangent space at P
tangent vectors velocity vector acceleration vector Riemmanian metric matrix of metric in SO(3)
matrix of metric in SE (3) matrix trace basis vector in so(3) basis vector in se(3) Cartesian axes covariant derivative
1 Introduction The problem of finding a smooth motion that interpolates between two given positions and orientations in IR3 is well understood in Euclidean spaces [4, 8], but it is not clear how these techniques can be generalized to curved spaces. There are two main issues that need to be addressed, particularly on non-Euclidean spaces. It is desirable that the computational scheme be independent of the description of the space and invariant with respect to the choice of the coordinate systems used to describe the motion. Secondly, the smoothness properties
2
and the optimality of the trajectories need to be considered. Shoemake [14] proposed a scheme for interpolating rotations with Bezier curves based on the spherical analog of the de Casteljau algorithm. This idea was extended by Ge and Ravani [7] and Park and Ravani [13] to spatial motions. The focus in these articles is on the generalization of the notion of interpolation from the Euclidean space to a curved space. Another class of methods is based on the representation of Bezier curves with Bernstein polynomials. Ge and Ravani [6] used the dual unit quaternion representation of
SE (3) and subsequently applied Euclidean
methods to interpolate in this space. J¨utler [9] formulated a more general version of the polynomial interpolation by using dual (instead of dual unit) quaternions to represent
SE (3).
In such a representation, an element of
SE (3) corresponds to a whole equivalence class of dual quaternions. Park and Kang [12] derived a rational interpolating scheme for the group of rotations SO(3) by representing the group with Cayley parameters and using Euclidean methods in this parameter space. The advantage of these methods is that they produce rational curves. It is worth noting that all these works (with the exception of [13]) use a particular coordinate representation of the group. In contrast, Noakes et al. [10] derived the necessary conditions for cubic splines on general manifolds without using a coordinate chart. These results are extended in [2] to the dynamic interpolation problem. Necessary conditions for higher-order splines are derived in Camarinha et al. [1]. A coordinate free formulation of the variational approach was used to generate shortest paths and minimum acceleration and jerk trajectories on
SO(3) and SE (3) in [16].
However, analytical solutions are available only in the simplest of
cases, and the procedure for solving optimal motions, in general, is computationally intensive. If optimality is sacrificed, it is possible to generate bi-invariant trajectories for interpolation and approximation using the exponential map on the Lie algebra [15]. While the solutions are of closed-form, the resulting trajectories have no optimality properties. This paper is built on the results from [16, 15]. It is first showed that a left or right invariant metric on SO(3)
(SE (3)) is inherited from the higher dimensional manifold
GL(3) (GA(3)) equipped with the appropriate
metric. Next, a projection operator is defined and subsequently used to project optimal curves from the ambient
SO(3) and the projected geodesic from GL(3) follow the same path, but with a different parameterization. The line from GL(3) is then shown to be parameterizable to yield the exact geodesic on SO(3) by projection. Several examples are presented to illustrate manifold onto
SO(3) (SE (3)).
It is proved that the geodesic on
the merits of the method and to show that it produces near-optimal results, especially when the excursion of the trajectories is “small”.
2 Background 2.1 The Lie groups SO(3) and SE (3) Let GL(n) denote the general linear group of dimension n. As a manifold, GL(n) can be regarded as an open
subset of IRn . Moreover, matrix multiplication and inversion are both smooth operations, which make GL(n) 2
a Lie group. The special orthogonal group is a subgroup of the general linear group, defined as
SO(n) = R j R 2 GL(n); RRT = I; detR = 1 3
SO(n) is referred to as the rotation group on IRn . GA(n) = GL(n) IRn is the affine group. SE (n) = SO(n)IRn is the special Euclidean group, and is the set of all rigid displacements in IRn . Special consideration will be given to SO(3) and SE (3). Consider a rigid body moving in free space. Assume any inertial reference frame fF g fixed in space and a frame fM g fixed to the body at point O0 as shown in Figure 1. At each instance, the configuration (position and orientation) of the rigid body can be described by a homogeneous transformation matrix, A, corresponding to the displacement from frame fF g to frame fM g. SE (3) is the set of all rigid body transformations in 8
is defined on the tangent space at
each point on the manifold, such a form is called a Riemannian metric and the manifold is Riemannian [3]. If the form is non-degenerate but indefinite, the metric is called semi-Riemannian. On a n dimensional manifold, the metric is locally characterized by a
n n matrix of C 1 functions gij =< Xi ; Xj > where Xi are basis
vector fields1 . Note that the entries gij at each point on the manifold are functions of the local coordinates at that point. If the basis vector fields can be defined globally, then the matrix [gij ] completely defines the metric.
On SE (3) (on any Lie group), an inner product on the Lie algebra can be extended to a Riemannian metric
over the manifold using left (or right) translation. To see this, consider the inner product of two elements
S1 ; S2 2 se(3) defined by
< S1 ; S2 >jI = sT1 Gs2 ;
(6)
where s1 and s2 are the 6 1 vectors of components of S1 and S2 with respect to some basis and G is a positive definite matrix. If V1 and V2 are tangent vectors at an arbitrary group element
< V1 ; V2 >jA in the tangent space TA SE (3) can be defined by:
A 2 SE (3), the inner product
< V1 ; V2 >jA = < A?1 V1 ; A?1 V2 > I :
(7)
The metric obtained in such a way is said to be left invariant [3] since left translation by any element A is an isometry.
2.4 Affine connection, covariant derivative, and geodesic flow Any motion of a rigid body is described by a smooth curve A(t)
to the curve V (t) =
2 SE (3). The velocity is the tangent vector
dA (t). The vector dV (t) does not in general belong to the tangent space TA(t) SE (3), and, dt dt
therefore, usual differentiation of a vector field is not an ”intrinsic” geometric notion on SE (3). The notion of
covariant derivative allows one to take derivative of the velocity vector of A(t), which gives the acceleration of the curve in SE (3). The covariant derivative is the orthogonal projection of
1
dV (t) on TA(t) SE (3). dt
The basis vector fields need not be the coordinate basis; we only require that they are smooth.
6
An affine connection on SE (3) is a map that assigns to each pair of C 1 vector fields X and Y on SE (3)
another C 1 vector field rX Y which is IR-bilinear in X and Y and, for any smooth real function f on SE (3) satisfies rfX Y
= f rX Y
and rX fY
= f rX Y + X (f )Y . i k = ?ijk L i , where The Christoffel symbols ?jk of the connection at a point A 2 SE (3) are defined by rL j L L 1 ; : : : ; L 6 is the basis in TA SE (3) and the summation is understood. If A(t) is a curve and X is a vector field, the covariant derivative of X along A is defined by
DX = r X A_ (t) dt X is said to be autoparallel along A if DX=dt = 0. A curve A is a geodesic if A_ is autoparallel along A. An equivalent characterization of a geodesic is the following set of equations:
ai + ?ijk a_ j a_ k = 0
(8)
where ai , i = 1; : : : ; 6 is an arbitrary set of local coordinates on SE (3). For a manifold with a Riemannian (or pseudo-Riemannian) metric, there exists a unique symmetric connection which is compatible with the metric [3]:
X < Y; Z >=< rX Y; Z > + < Y; rX Z >; rX Y ? rY X = XY ? Y X Given a connection, the acceleration and higher derivatives of the velocity can be defined. The acceleration,
A(t), is the covariant derivative of the velocity along the curve:
D dA = r V A = dt V dt
(9)
2.5 Exponential map If M is a manifold with a connection r, the exponential map at an arbitrary q
2 M is defined as follows. Let
V (t) be the unique geodesic passing through q at t = 0 with velocity V , i.e. V (0) = q and _ V (0) = V . Then, by definition, expq maps V 2 Tq M to the point V (1) 2 M . Using homogeneity of geodesics, it is easy to prove [3] that tV (s) = V (ts) which gives expq (tV ) = V (t). Also, expq is a diffeomorphism of a neighborhood of 0 2 Tq M to a neighborhood of q 2 M . This gives a local chart for M called normal coordinates. These coordinates are convenient for computations (as in this work) because rays through
0 are
geodesics.
SO(3) with metric G = I is given special consideration in this paper. For R 2 SO(3) and V 2 TR SO(3), one can define expR (V ) = ReRT V . If v = [v1 v2 v3 ] is the expansion of V o1 + v2 L o2 + v3 L o3 ), it is easy to see that expR (V ) = Rev^. As a in the local basis of TR SO(3) (i.e. V = v1 L special case, for S 2 so(3), expI (S ) = e^ , where = [1 2 3 ] is the expansion of S in the basis Lo1 ; Lo2 ; Lo3 . This gives a local parameterization of SO(3) around identity known as exponential coordinates. The exponential map on
2.6 Local parameterization of SE (3)
SE (3) induced by the product structure SO(3) IR3 is chosen. words, a set of coordinates 1 , 2 , 3 , d1 , d2 , d3 for an arbitrary element A 2 SE (3):
In this paper, a parameterization of
2
A=4
R d 0 1
7
3 5
In other
is defined so that d1 , d2 , d3 are the coordinates of d in IR3 . Exponential coordinates, as defined in Section 2.5,
are chosen as local parameterization of SO(3). For R 2
SO(3) sufficiently close to the identity (i.e excluding the points Tr(R) = ?1 (Tr(A) = 0), or, equivalently, rotations through angles up to ), the exponential
coordinates are given by:
R = e^ ; 2 IR3
2.7 Screw motions One of the fundamental results in rigid body kinematics was proved by Chasles at the beginning of the 19th century: Any rigid body displacement can be realized by a rotation about an axis combined with a translation parallel to that axis. Note that a displacement must be understood as an element of a curve on
SE (3).
SE (3) while a motion is
If the rotation from Chasles’s theorem is performed at constant angular velocity and the
translation at constant translational velocity, the motion leading to the displacement becomes a screw motion. Chasles’s theorem then says that any rigid body displacement can be realized by a screw motion. A curve A(t) on a Lie group is called a one-parameter subgroup if A(t + s)
are equivalent ways of defining a screw motion A(t) 2 SE (3):
= A(t)A(s).
The following
A?1 (t)A_ (t) = constant. f!; vg = constant. A(t) is a one-parameter subgroup of SE (3). The tangent vectors A_ (t) to the curve form a left invariant vector field. With this mathematical definition, the Chasles’s theorem can be restated in the form: For every element in
SE (3) different from identity, there is a unique one-parameter subgroup to which that element belongs. Note that the definition of an one-parameter subgroup is not dependent on a metric. Given two end positions on
SE (3), one concludes that there always exists a interpolating screw motion.
Is this motion physically meaningful and/or optimal from some point of view? To talk about optimality, one first needs to define a metric on the manifold. Optimal interpolating motions with respect to a given metric are geodesics, minimum acceleration curves, minimum jerk curves, and so on. What is the connection between geodesics as defined in Section 2.4 and screw motions (one-parameter subgroups)? The following result is true for any Lie group [3]: for a bi-invariant metric, the geodesics that start from identity are one-parameter subgroups. As a particular case, geodesics through identity on SO(3) with metric G = I are one-parameter subgroups
(! = constant). Also, for the bi-invariant semi-Riemannian metric on SE (3) 2
3
I I G = 4 3 3 5 ; ; > 0 I3 0
(10)
geodesics through identity are screw motions. The conclusion is that an interpolating screw motion is not the appropriate choice if the metric on SE (3) is different from the bi-invariant metric (10), which is the case of the kinetic energy metric.
8
3 Riemannian metrics on SO(3) and SE (3) In this section it is shown that there is a simple way of defining a left or right invariant metric in SO(3) (SE (3))
by introducing an appropriate constant metric in GL(3) (GA(3)). Defining a metric at the Lie algebra so(3) (or
se(3)) and extending it through left (right) translations is equivalent to inheriting the appropriate metric from the ambient manifold at each point. In this paper, only metrics on SE (3) which are products of the bi-invariant metric on SO(3) and the Euclidean metric on IR3 are considered. A more general treatment accomodating arbitrary metrics on SO(3) is to be published elsewhere. 3.1 Metrics on GL(3) and SO(3) For any M
2 GL(3) and any X; Y 2 TM GL(3), define < X; Y >GL= Tr(X T Y )
(11)
Tr denotes the trace of a square matrix and TM GL(3) is the tangent space to GL(3) at M , which is isomorphic to GL(3). By definition, form (11) is the same at all points in GL(3). It is easy to see that GL where
is a positive definite quadratic form in the entries of
X and Y , therefore a metric. This induces the Euclidean
norm on TM GL(3), also called Frobenius matrix norm.
Proposition 3.1 The metric given by (11) defined on GL(3) is bi-invariant when restricted to SO(3). Proof: Let any Then, we have
M 2 GL(3) and any vectors X , Y
in the tangent space at an arbitrary point of
GL(3).
< X; Y >GL= Tr(X T Y ); < MX; MY >GL= Tr(X T M T MY )
from which we conclude that the metric is invariant under left translations with elements from SO(3). Therefore, when restricted to have
SO(3), the metric becomes left invariant.
For right invariance, if
R 2 SO(3), we
< X; Y >GL= Tr(Y X T ); < XR; Y R >GL= Tr(Y RRT X T ) = Tr(Y X T )
and the claim is proved. To find the induced metric on SO(3), let R be an arbitrary element from SO(3), X; Y be two vectors from
TR SO(3) and Rx (t); Ry (t) the corresponding local flows so that
X = R_ x (0); Y = R_ y (0); Rx (0) = Ry (0) = R: The metric inherited from GL(3) can be written as:
< X; Y >SO =< X; Y >GL= Tr(R_ xT (0)R_ y (0)) = Tr(R_ xT (0)RRT R_ y (0)) = Tr(^!xT !^y ) where ! ^x
= Rx(0)T R_ x (0) and !^y = Ry (0)T R_ y (0) are the corresponding twists from the Lie algebra so(3).
If the above relation is written using the vector form of the twists, some elementary algebra leads to:
< X; Y >SO = 2!xT !y A different equivalent way of arriving at expression (12) would be defining the metric in identity of SO(3)) as being the one inherited from TI GL(3):
gij = Tr(Loi T Loj ) = ij ; i; j = 1; 2; 3 9
(12)
so(3) (i.e.
at
where
Lo1 ; Lo2 ; Lo3 is the basis in so(3) and ij is the Kronecker symbol.
Left or right translating this metric
throughout the manifold is equivalent to inheriting the metric at each 3-dimensional tangent space of from the corresponding 9-dimensional tangent space of GL(3). Remark 3.2 The matrix G of the metric as defined in (6) is G
SO(3)
= 2I , which is the standard scale-independent
bi-invariant metric on SO(3). This is consistent with Proposition 3.1.
Remark 3.3 The metric given by (12) can be interpreted as the (rotational) kinetic energy metric of a spherical rigid body.
3.2 Metrics on GA(3) and SE (3) Let X and Y be two vectors from the tangent space at an arbitrary point of GA(3) (X and Y are 4 4 matrices with all entries of the last row equal to zero). Similar to Section 3.1, a quadratic form defined by
< X; Y >GA= Tr(X T Y )
(13)
is a point-independent Riemmanian metric on GA(3).
We can get a left invariant metric on SE (3) by inheriting the metric
GA given by (13) from GA(3).
To derive the induced metric in SE (3) we follow the same procedure as in Section 3.1. Let A be an arbitrary element from
SE (3). Let X; Y
be two vectors from TA SE (3) and Ax (t); Ay (t) the
corresponding local flows so that
X = A_ x (0); Y = A_ y (0); Ax (0) = Ay (0) = A: Let
2
Ai (t) = 4
Ri (t) di (t) 0
1
3 5
; i 2 fx; yg
and the corresponding twists at time 0: 3
2
!^ v Si = A?i 1 (0)A_ i (0) = 4 i i 5 ; i 2 fx; yg 0 0
The metric inherited from GA(3) can be writen as:
< X; Y >SE =< X; Y >GA = Tr(A_ Tx (0)A_ y (0)) = Tr(SxT AT ASy ) Now using the orthogonality of the rotational part of A and the special form of the twist matrices, straightforward calculations lead to
< X; Y >SE = Tr(SxT Sy ) = Tr(^!xT !^y ) + vxT vy
or, equivalently,
2
3
2
2I 0 ! < X; Y >SE = !xT vxT G~ 4 y 5 ; G~ = 4 3 vy 0 I3 h
i
Remark 3.4 Metric (14) is the scale-independent metric on
5
(14)
SE (3) proposed by Park and Brockett [11] for
= 2 and = 1. It is a product metric and has been extensively studied in [16]. 10
3
SE (3) can be provided with the same metric (14) by
Remark 3.5 Straightforward calculations show that inheriting the metric from the ambient space at se(3):
8 > >
ij ; > :
i; j = 1; 2; 3 i; j = 4; 5; 6
0;
elsewhere
and left translating it throughout the manifold. Therefore, the metric invariant when restricted to SE (3).
Tr(X T Y ) from GA(3) becomes left
Remark 3.6 Metric (14) can be interpreted as being the kinetic energy of a moving (rotating and translating)
spherical rigid body when the body fixed frame fM g is placed at the centroid of the body and aligned with its principal axes.
4 Projection on SO(3)
GL(3). Using this distance, for a given M 2 GL(3), we define the projection of M on SO(3) as being the closest R 2 SO(3) We can use the norm induced by metric (11) to define the distance between elements in with respect to norm
k kGL.
The following proposition gives the solution of the projection problem for the
general case of GL(n):
Proposition 4.1 Let M
2 GL(n) and M = U V T its singular value decomposition. Then the projection of
M on SO(n) is given by R = UV T .
Proof: The problem to be solved is a minimization problem:
min kM ? Rk2GL
R2SO(n) With M
= U V T , we have
Note that
kM ? Rk2GL = Tr[(M ? R)T (M ? R)] = = Tr(M T M ? M T R ? RT M + RT R)
RT R = I , Tr(M T R) = Tr(RT M ) and the quantity M T M is a constant and therefore does not
affect the optimization. Therefore, the problem to be solved becomes:
max Tr(M T R)
R2SO(n) Let = diagf1 ; : : : ; n g, C
= RT U and consider columnwise partitions for V
and C
V = [v1 ; : : : ; vn ]; C = [c1 ; : : : ; cn ] Then, we have:
Tr(M T R) = Tr( Now C and V are both orthogonal, and then
n X i=1
i vi cTi ) =
n X i=1
i viT ci
kci k = kvi k = 1 On the other, by Cauchy-Schwartz, (viT ci )2
kvi k2 kci k2 = 1 and the equality holds for vi = ci , or V = C . Tr(M T R) which is attained for R = UV T .
11
Therefore,
Pn
i=1 i is an upper bound for
Remark 4.2 It is easy to see that the distance between M and
P R in metric (11) is given by ni=1 (i ? 1)2 ,
which is the standard way of describing how ”far” a matrix is from being orthogonal. The question one might ask is what happens with the solution to the projection problem when the manifold
GL(n) is acted upon by the group SO(n). The answer is given in the following proposition: Proposition 4.3 The solution to the projection problem described above is both left and right invariant under actions of elements from SO(n).
M 2 GL(n), M = U V T and the corresponding projection R 2 SO(n), R = UV T . = LM . Then an SVD for M can be found from the SVD for M in the form Let any L 2 SO(n) and M M = (LU )V T . Then, by Proposition 4.1, the projection of M is R = LUV T = LR, which proves left = ML = U (V T L) projects to invariance. Similarly, if M is acted from the right by L 2 SO(n), then M R = UV T L = RL, which implies right invariance. Proof: Let
It is worthwhile to note that other projection methods do not exhibit bi-invariance. For instance, it is cus-
tomary to find the projection R 2 SO(n) by applying a Gram-Schmidt procedure (QR decomposition). In this case it is easy to see that the solution is left invariant, but in general it is not right invariant.
5 Projection on SE (n) Similar to the previous section, if a metric of form (13) is defined in
SE (n) can be found. Proposition 5.1 Let B
2 GA(n) with the following block partition 2
B=4 and B1
= USigmaV T
B1 B2 0
1
3 5
; B1 2 GL(n); B2 2 IRn
the singular value decomposition of B1 . Then the projection of B on SE (n) is given
by
2
A=4 Proof: Let
GA(n), the corresponding projection on
2
A=4
UV T 0
R d 0 1
B2 1
3 5
2 SE (n)
3 5
; R 2 SO(n); d 2 IRn
The problem to be solved can be formulated as follows:
min kB ? Ak2GA
A2SE(n) Then
kB ? Ak2GA = Tr[(B ? A)T (B ? A)] = Tr(B T B ) ? 2Tr(B T A) + Tr(AT A)
The quantity B T B is not involved in the optimization. The observation that
Tr(B T A) = Tr(B1T R) + (B2T d + 1); Tr(AT A) = 4 + dt d 12
separates the initial problem into two subproblems:
(a) max Tr(B1T R) R2SO(n)
and
(b) minn ?2B2T d + dT d
d2IR From Proposition 4.1, the solution to subproblem (a) is R = UV T . For the second subproblem, let
f : IRn ! IR; f (x) = ?2B2T x + xT x The critical points of the scalar function f are given by
rf (x) = ?2B2 + 2x = 0 ) x = B2 and the Hessian r2 f (x)
= 2I is always positive definite. Therefore, the solution is d = B2 which concludes
the proof. Similar to SO(n), invariance properties are exhibited by the projection on SE (n): Proposition 5.2 The solution to the projection problem on SE (n) is left invariant under actions of elements from SE (n). The projection is bi-invariant under rotations. Proof: Let
2
B1 B2
B=4
0
and define A, U , , V such that
1
2
5
2 GA(n)
UV T
B1 = U V T ; A = 4 Let
3
B2
0
2
X=4
R d 0 1
1
3 5
2 SE (n)
3 5
be an arbitrary element from SE (n). Under left actions of X , the solution pair becomes 2
XB = 4 2
XA = 4
RB1 RB2 + d 0
1
RUV T 0
3 5
RB2 + d 1
; 3 5
which proves left invariance of the projection. For the second part, note that the right translated solution pair is 2
BX = 4 2
AX = 4
B1 R B 1 d + B 2 0
1
3 5
UV T R UV T d + B2 0
1
; 3 5
It is easy to see that B1 R = U V T R. If only rotations (d = 0) are taken into consideration, right invariance is proved.
13
6 Generating smooth curves on SE (3) Based on the results from the previous sections, a procedure for generating near optimal curves on SE (3) follows: generate the curves in the ambient space and project them onto
SE (3). Due to the fact that the defined
metric in GA(3) is the same at all points, the corresponding Christoffel symbols are all zero. Consequently, the
optimal curves in the ambient space assume simple analytical forms (i.e geodesics - straight lines, minimum acceleration curves - cubic polynomial curves, minimum jerk curves - fifth order polynomial curves, all parameterized by time). The resulting curve in GA(3) is linear in the boundary conditions, and therefore left and right
invariant. Recall that the projection procedure on SE (3) is left invariant, and so is the overall procedure.
The focus is on SO(3). Due to the product structure of both SE (3) = SO(3) IR3 and the metric SE
for a = 0, all the results are straightforward to extend to SE (3).
6.1 Geodesics on SO(3) The problem to be solved is generating a geodesic R(t) between given end positions R1 on SO(3). Without loss of generality, it is assumed R1
= R(0) and R2 = R(1)
= I . Indeed, a geodesic between two arbitrary positions
R1 and R2 is the geodesic between I and R1?1 R2 left translated by R1 . Exponential coordinates 1 ; 2 ; 3 are considered as local parameterization of SO(3). If R2 = e!^ 0 , then the geodesic is the exponential mapping of
the uniformly parameterized segment passing through 0 and !0 ( (t) = !0 t) from the exponential coordinates:
R(t) = e^(t) = e!^ 0 t The geodesic in the ambient manifold GL(3) satisfying the given boundary conditions on SO(3) is
M (t) = I + (R2 ? I )t; t 2 [0; 1] An analytical expression for the projection of an arbitrarily parameterized line in the ambient
GL(3) onto
SO(3) is derived, which will answer the following three questions. Does the projection of a geodesic from GL(3) follow the same path as the true geodesic on SO(3)?
If
the answer is yes, then the following question makes sense:
Do the above two curves have the same parameterization? If the answer is no, Can one find an appropriate parameterization of the line in the ambient manifold so that the projection is identical to the true geodesic on SO(3)?
Proposition 6.1 is the key result of this section.
M (t) = I + (R2 ? I )f (t); t 2 [0; 1] be a line in GL(3) with R2 = e!^ 0 2 SO(3) (f continuous, f (0) = 0; f (1) = 1). Then the projection of this line R? (t) onto SO(3) is the exponential mapping of a segment drawn between the origin and !0 in exponential coordinates parameterized by (t): Proposition 6.1 Let
M (t) = U (t)(t)V T (t) ) R?(t) = U (t)V T (t) = e!^ 0 (t) (t) = k!1 k atan2(1 ? f (t) + f (t) cos k!0k; f (t) sin k!0k) 0
14
(15) (16)
= I + (R2 ? I )f (t) = U (t)(t)V (t)T is needed, where R2 = e!^ 0 and f (t) is a continuous function defined on [0; 1] satisfying f (0) = 0, f (1) = 1. The first observation is Proof: The SVD decomposition of M (t)
M T (t)M (t) = I ? f (t)(1 ? f (t))N; N = 2I ? R2 ? R2T The eigenstructure of the constant and symmetric matrix N completely determines the SVD of M (t). Because
N is symmetric and real, its eigenvalues will be real and the corresponding eigenspaces orthogonal. Let i ; vi be an eigenvalue-eigenvector pair of N . Then, Nvi = i vi ) M T (t)M (t) = (1 ? f (t)(1 ? f (t))i )vi So, theoretically, the desired SVD decomposition is determined at this moment:
V (t) can be chosen constant of the form V = [v1 v2 v3 ] where v1 ; v2 ; v3 are orthonormal eigenvectors of N ; The singular values are given by s2i (t) = 1 ? f (t)(1 ? f (t))i (it will be shown shortly that the right The matrix
hand side of this equality is always positive)
The time-dependence of the projection will be contained in
U (t) = [u1 (t) u2(t) u3 (t)]; ui (t) = M (st)vi ; i = 1; 2; 3 i
Using the Rodrigues formula for R2
= e!^ 0 , it is easy to see that N = ? 1 ? cos k!0 k (^!2 + !^ 2T )
k!0k2
0
0
from which it follows that the eigenvalues of N are given by
(N ) = f0; 2(1 ? cos k!0k); 2(1 ? cos k!0k)g and a set of three orthonormal eigenvectors by 8 > > < > > :
where !0
2
?!3
1 !0 6 k!0k ; p!32 + !12 4 0 6
!1
3 7 7 5
2
;
6 1 6 !2 + ! 2 2 2 2 ! ! + (! + !1 ) + !3 !2 4 3
p
2 2 2 1
2 3
39 > > 7= 2 7 1 5> > ;
?!2!1
?!3!2
= [!1 !2 !3 ]T . With the eigenstructure of N determined, one can write p
(t) = diagf1; s(t); s(t)g; s(t) = 2(1 ? cos k!0 k)f 2 (t) ? 2(1 ? cos k!0 k)f (t) + 1
(17)
where the binomial under the square root is always positive because it is positive at zero and 1 ? cos k!0 k
(0; 2) gives a negative discriminant. Some straightforward but rather tedious calculation leads to: 2 U (t)V T = I + !^0 (t) + (1 ? (t)) + !^0
k!0 k
where
2
1
2
k!0 k2
k!0 k
1 (t) = 1 ? f (t) +sf(t()t) cos k!0 k ; 2 (t) = f (t) ssin (t)
k!0 k 2 (0; ) (in accordance to the exponential coordinates) which will give
2 (t) > 0. Note that (t) + (t) = 1, so it is appropriate to define a function (t) 2 (0; 1) so that The discussion is restricted to 2 1
2 2
1 (t) = cos(k!0 k(t)); 2 (t) = sin(k!0k(t)) 15
By use of the Rodrigues formula again,
U (t)V T = e!^ 0 (t)
so the projected line is the exponential mapping of a segment between the origin and !0 in exponential coordinates. The parameterization of the segment is given by
(t) = k!1 k atan2(1 ? f (t) + f (t) cos k!0k; f (t) sin k!0k) 0
Note that the obtained parameterization (t) satisfies the boundary conditions (0) = 0, (1) = 1
As a particular case of Proposition 6.1 for f (t) = t, the following corollary answers the first two questions at the beginning of this section: Corollary 6.2 The true geodesic on SO(3) and the projected geodesic from GL(3) with ends on SO(3) follow
the same path on SO(3) but with different parameterizations. The projected curve is the exponential mapping
of the same segment from the exponential coordinates
R?(t) = e!^ 0 (t) with the following parameterization
(t) = k!1 k atan2(1 ? t + t cos k!0k; t sin k!0 k) 0
The derivative of the function (t) is given by
sin k!0 k d dt (t) = k!0ks(t) where s(t) is given by (17). Plots of the function (t) and its derivative are given in Figure 2 for t 2 [0; 1] and the magnitude of the displacement on the manifold k!0 k 2 (0; ). The conclusion is that even though the line in GL(3) is followed at constant velocity, the projected curve on the manifold has low speed at the beginning, attains its maximum in the middle, and slows down as approaching the end point. The larger the displacement k!0 k, the larger the discrepancy in speeds. Also note that the middle of the line is projected into the middle of the true geodesic because (0:5)
are equal at t
= 0:5 (i.e the functions t and (t)
= 0:5). This result has been stated in [14] in the context of unit quaternions as local parameters
of SO(3) (viewed as the unit sphere S 3 in the projective space IRP 3 ).
f (t) (f (0) = 0, f (1) = 1) of the line in GL(3) with ends on SO(3), which gives uniform parameterization t of the projected curve in exponential coordinates. The solution of the following equation in f To answer the third question, one needs to find a parameterization
atan2(1 ? f (t) + f (t) cos k!0 k; f (t) sin k!0 k = t is of the form
k!0 kt) f (t) = sin(k! k(1sin( ? t)) + sin(k!0kt) 0
The answer to the third question is stated in the following corollary:
16
(a)
(b)
1 0.75 0.5 0.25 0
2 1.5 1 0.5 0
1 0.8 0.6 0.4 Time
1 Displacement
1 0.8 0.6 0.4 Time
1
0.2
2
0.2
2
Displacement 3 0
3 0
Figure 2: (a) Function (t); (b) The derivative
SO(3) starting at I following line from the ambient manifold GL(3):
Corollary 6.3 The true geodesic on
and ending at
d dt (t)
R2 = e!^ 0
is the projection of the
k!0 kt) M (t) = I + (R2 ? I )f (t); t 2 [0; 1]; f (t) = sin(k! k(1sin( ? t)) + sin(k!0kt) 0 Illustrative plots of f (t) and its derivative are given in Figure 3 for t 2 [0; 1] and different values of the displacement k!0 k 2 (0; ). As expected, to get a uniform speed on SO(3), the line in GL(3) should be followed at high speed at the beginning, slow down in the middle, and accelerating again near the end point. Remark 6.4 The result in Corollary 6.3 is similar to the formula for spherical linear interpolation ’Slerp’ in terms of quaternions [14]. The curve interpolating q1 and q2 , with parameter u moving from 0 to 1, is given by:
where q1 q2
= cos .
u q Slerp(q1 ; q2 ; u) = sin(1sin?u) q1 + sin sin 2
6.2 Minimum acceleration curves on SO(3) First, the computation of optimal trajectories described in [16] is summarized. Then, near optimal trajectories are generated via the projection method. The necessary conditions for the curves that minimize the square of the
L2 norm of the acceleration are
found by considering the first variation of the acceleration functional
La = where V (t) =
Z
a
b
< rV V; rV V > dt;
(18)
dR(t) , r is the symmetric affine connection compatible with a suitable Riemmanian metric, and dt
R(t) is a curve on the manifold.
The initial and final points as well as the initial and final velocities for the
motion are prescribed.
17
(a)
(b)
1 0.75 0.5 0.25 0
1.5 1 0.5 0
1 0.8 0.6 0.4 Time
1 Displacement
1 0.8
1
0.2
2
0.6 0.4 Time
Displacement
0.2
2
3 0
3 0
Figure 3: (a) Function f (t); (b) The derivative
d dt f (t)
The following result is stated and proved in [16]:
R(t) be a curve between two prescribed points on SO(3) with metric SO that has prescribed initial and final velocities. If ! is the vector from so(3) corresponding to V = dR dt , the curve minimizes the cost function La derived from the canonical metric only if the following equation hold:
Proposition 6.5 Let
!(3) + ! ! = 0
(19)
where ()(n) denotes the nth derivative of (). The above equation can be integrated to obtain
!(2) + ! !_ = constant
(20)
However, this equation cannot be further integrated analytically for arbitrary boundary conditions. In the special case when the initial and final velocities are tangential to the geodesic passing through the same points, the solution can be found by re-parameterizing the geodesic [16]. In the general case, one must solve equation (19) numerically. A local parameterization of
SO(3) should be chosen and three first order differential equations
will augment the system. The most convenient local coordinates are the exponential coordinates. Eventually, one ends up with solving a system of
12 first order nonlinear coupled differential equations with 6 boundary
conditions at each end. A solution can be found using iterative procedures such as the shooting method or the relaxation method. The latter has been chosen in this paper. A more attractive and much simpler approach is the projection method described above. The main idea is to
GL(3), while keeping the proper boundary conditions on SO(3) with the corresponding velocities. Minimum acceleration curves are found in GL(3) and eventually projected back onto SO(3). In what follows, the time interval will be t 2 [0; 1] and the boundary conditions R(0), R(1), R_ (0), R_ (1) are assumed to be specified. The minimum acceleration curve in GL(3) with a constant metric GL is a cubic relax the problem to
18
(a)
(b)
(c)
1.6
True Projection
1.6 1.4
1.4
True Projection
1.2
1.2 1
1
2
True Projection
0.8
σ3
σ
3
0.8 1.5
σ3
0.6 0.4
1.4
1 1.2
0.5
0.2
1
0
0 1.5 1
0
0.2
0.6
0.2 0
σ
0.8
σ
1
1.5 1
0
0.4
0.4
0.4
0.5
0.2 0 −0.5
0.6
0.2
0.6
σ2
0.4
0.8
0
0.8
0.6
σ2
0
1
0.5
0.5 1
0
σ
σ2
1
Figure 4: Minimum acceleration curves on SO (3) with canonical metric: (a) Velocity boundary conditions along the geodesic; (b) End velocity perturbed by e1 ; and (c) End velocity perturbed by 5e1 .
given by
M (t) = M0 + M1 t + M2 t2 + M3t3 ; where M0 , M1 , M2 , M3
2 GL(3) are M0 = R(0); M1 = R_ (0);
M2 = ?3R(0) + 3R(1) ? 2R_ (0) ? R_ (1); M3 = 2R(0) ? 2R(1) + R_ (0) + R_ (1): Now the curve on SO(3) is obtained by projecting M (t) onto SO(3) as described in Section 4. The following examples present comparisons between the minimum acceleration curves generated using the projection method and the curves obtained directly on SO(3) by solving equations (19) using the relaxation method. All the generated curves are drawn in exponential local coordinates. In Figure 6.2, the following position boundary conditions were used: h
(0) = 0 0 0
iT
iT h ; (1) = 6 3 2
(21)
The initial velocity is the one corresponding to the geodesic passing through the two positions: iT h !0 = 6 3 2
Cases (a), (b) and (c) differ by the velocity at the end point. Figure 6.2 (a) corresponds to a final velocity
!1 = !0 , therefore one gets a minimum acceleration curve for which the end velocities are along the velocity of the corresponding geodesic, leading to a geodesic parameterized by a cubic of time [16]. The final velocity is !1
= !0 + e1 in case (b) and !1 = !0 + 5e1 in case (c), where e1 = [1 0 0]T .
As seen in Figure 6.2 (a), the paths of the projected and the optimal curves are the same, the parameterizations are slightly different though, as expected. In cases (b) and (c), although the deviation of the final velocity
19
(b)
14
14
12
12
10
10
8
8
6
6
z
z
(a)
4
4
2
2
0
0
−2 15
−2 15 10
10
10
10
8 5
8 5
6
6
4 0
4 0
2
2
0 y
−5
−2
0 −5
y
x
−2
x
Figure 5: Minimum acceleration motion for a cube in free space: (a) relaxation method; (b) projection method.
from being homogeneous is large, the curves are close. Note that the boundary conditions are rigorously satisfied.
6.3 Motion generation on SE (3) Since a method to generate (near) optimal curves on 3
SO(3) has been developed, the extension to SE (3) is
simply adding the well known optimal curves from IR . A homogeneous cubic rigid body is assumed to move (rotate and translate) in free space. The body frame
fM g is placed at the center of mass and alligned with the principal axes of the body. A small square is drawn on one of its faces. The trajectory of the center of the cube is starred. The following boundary conditions were considered: h
(0) = 0 0 0
iT
h
!(0) = 1 2 3 h
d(0) = 0 0 0 h
d_(0) = 1 1 1
h
; (1) = 6
iT
iT
2 3
i T 2
h
; !(1) = 2 1 1 h
; d(1) = 8 10 12
iT
iT
iT
h iT ; d_(1) = 1 5 3
True and projected minimum acceleration motions for a cubic rigid body with a = 2 and m = 12 are given in Figure 6.3 for comparison. In this example, the total displacement between initial and final positions on SO(3) is large. If the rotational displacement is restricted near the origin of exponential coordinates, the simulated motions look identitical.
20
7 Conclusion This paper presented a method for generating smooth trajectories for a moving rigid body with specified bound-
SE (3), the set of all rigid body translations and orientations, was seen as a submanifold (and a subgroup) of the Lie group of affine maps in IR3 , GA(3). The method involved two key steps: (1) the generation of optimal trajectories in GA(3); and (2) the projection of the trajectories from GA(3) to SE (3). The overall ary conditions.
procedure was proved to be invariant with respect to both the local coordinates on the manifold and the choice of the inertial frame. The projected geodesic from
GL(3) and the actual geodesic on SO(3) were shown to
have identical paths on SO(3). The parameterization of the line whose projection is the actual geodesic was derived.
References [1] M. Camarinha, F. Silva Leite, and P. Crouch. Splines of class ck on non-Euclidean spaces. IMA J. Math. Control Inform., 12(4):399–410, 1995. [2] P. Crouch and F. Silva Leite. The dynamic interpolation problem: on Riemannian manifolds, Lie groups, and symmetric spaces. J. Dynam. Control Systems, 1(2):177–202, 1995. [3] M. P. do Carmo. Riemannian geometry. Birkhauser, Boston, 1992. [4] G. E. Farin. Curves and surfaces for computer aided geometric design : a practical guide. Academic Press, Boston, 3 edition, 1992. [5] J. Gallier. Curves and Surfaces in Geometric Modeling - Theory and Algorithms. Morgan Kaufmann Publishers, San Francisco, CA, 2000. [6] Q. J. Ge and B. Ravani. Computer aided geometric design of motion interpolants. ASME Journal of Mechanical Design, 116:756–762, 1994. [7] Q. J. Ge and B. Ravani. Geometric construction of Bezier motions. ASME Journal of Mechanical Design, 116:749–755, 1994. [8] J. Hoschek and D. Lasser. Fundamentals of Computer Aided Geometric Design. AK Peters, 1993. [9] B. J¨utler. Visualization of moving objects using dual quaternion curves. Computers and graphics, 18(3):315–326, 1994. [10] L. Noakes, G. Heinzinger, and B. Paden. Cubic splines on curved spaces. IMA J. of Math. Control & Information, 6:465–473, 1989. [11] F. C. Park and R. W. Brockett. Kinematic dexterity of robotic mechanisms. International Journal of Robotics Research, 13(1):1–15, 1994. [12] F. C. Park and I. G. Kang. Cubic interpolation on the rotation group using Cayley parameters. In Proceedings of the ASME 24th Biennial Mechanisms Conference, Irvine, CA, 1996. [13] F. C. Park and B. Ravani. Bezier curves on Riemannian manifolds and Lie groups with kinematics applications. ASME Journal of Mechanical Design, 117(1):36–40, 1995. [14] K. Shoemake. Animating rotation with quaternion curves. ACM Siggraph, 19(3):245–254, 1985.
21
ˇ [15] M. Zefran and V. Kumar. Interpolation schemes for rigid body motions. Computer-Aided Design, 30(3), 1998. ˇ [16] M. Zefran, V. Kumar, and C. Croke. On the generation of smooth three-dimensional rigid body motions. IEEE Transactions on Robotics and Automation, 14(4):579–589, 1995.
22
