Splining on Lie Groups Claire Tomlin

[email protected]

Course Project for CS284, taught by Professor B. A. Barsky Department of Electrical Engineering and Computer Sciences University of California at Berkeley Fall, 1995

1 Introduction This project is a study of interpolatory splines and uniform cubic B-splines over Lie groups. The Lie group SO(3), the special orthogonal group in R , is used for illustration and simulation, but the results are general and may be applied to any Lie group. This work has interesting applications. In the study of motion planning for rigid bodies, we are interested in both position and orientation trajectories. For example, in planning a trajectory for an aircraft, it is required that the aircraft smoothly interpolate a sequence of positions called waypoints, as well as a sequence of orientations speci ed as roll (rotation about x-axis), pitch (rotation about y-axis), and yaw (rotation about z-axis) angles. A spline in SE (3), the special Euclidean group in R , which has the dictated continuity properties at the waypoints, would be such a trajectory. Properties such as local control and optimality are important for these applications: aircraft are often required to change their trajectories locally by interpolating dierent waypoints to avoid turbulence or con icts with other aircraft, and trajectories which minimize functions of angular velocity or acceleration are optimal in terms of fuel consumption. This paper is divided into three sections. In the rst, some concepts of Lie groups are reviewed and the special orthogonal group is described. Three methods of creating interpolatory splines in Lie groups are discussed in the next section, and the following section presents a method for creating uniform cubic B-splines in Lie groups. Visualization issues are discussed in the nal section. Routines were written in MATLAB to implement each of the SO(3) splines described in this paper. These splines are presented in a visual manner using OpenGL with a cube rotating about the origin. 3

3

1

2 Some Basics of Lie Groups We begin with the de nition of a group, and then describe the group SO(3), the set of all rotation matrices. We present the relationship between SO(3) and so(3), where so(3) is the set of all skew symmetric matrices. We then give de nitions of Lie groups and their associated Lie algebras, and show that SO(3) is a Lie group with associated algebra so(3). [2] contains a good summary of Lie groups and rotation matrices.

De nition 1 (Group) A group G is a set with a binary operation (
) : G  G ! G, such that, 8a; b; c in G, the following properties are satis ed: 1. associativity: (a
b)
c = a
(b
c) 2. 9 an identity e 3 a
e = e
a = a 3. 9 an inverse a? 3 a
a? = a?
a = e. De nition 2 (Special Orthogonal Matrix Group) SO(n) is the set of all orthogonal matrices (AT = A? ) of determinant 1 in Rnn. It is called the special orthogonal group 1

1

1

1

with the binary operation being matrix multiplication.

The group SO(3) is the set of all rotation matrices: the columns of a matrix in SO(3) represent the rotation of an object relative to some inertial frame. For example, Rx() 2 SO(3) which is described by: 3 2 1 0 0 7 Rx() = 64 0 cos  ? sin  5 0 sin  cos  represents a rotation of  radians about the x-axis.

De nition 3 (Skew Symmetric Matrices) so(n) is the set of all skew symmetric matrices (AT = ?A) in Rnn. The set so(3) is the set of all skew symmetric matrices in R . An element !^ in so(3) looks like: 3 2 0 ?! ! 0 ?! 75 !^ = 64 ! ?! ! 0 where ! = [! ; ! ; ! ]T . 3

3

2

3

1

2

1

2

1

3

De nition 4 (Exponential Map) The matrix exponential function, exp : Rnn ! Rnn,

is de ned in terms of the Taylor series expansion of the exponential:

eA = I + A + A2! + A3! + : : : Proposition 1 A 2 so(n) =) eA 2 SO(n). 2

2

3

Proof: (eA)T = eAT = eA? = (eA)? , therefore, eA is an orthogonal matrix. Using 1

1

det(eA) = etrace A , we have det(eA ) = e = 1. 2 We now have a relationship, described by the exponential map, between the set of rotation matrices SO(3) and the set of skew symmetric matrices in R  , or so(3). Armed thus, we present Lie groups. ( )

0

3 3

De nition 5 (Manifold) An manifold is a topological space M with the property that, if x 2 M , then there is some neighborhood U of x such that U is homeomorphic to Rn. A manifold is therefore simply a space which looks locally like Rn .

De nition 6 (Lie Group) A Lie group is a group G which is also a manifold such that, for a; b 2 G, 1. (a; b) 7?! ab 2. a 7?! a? 1

are smooth functions.

All nite dimensional Lie groups may be represented as matrix groups. SO(3) looks locally like R . Multiplication of matrices in SO(3) is continuous, and smoothness of the inverse map follows from Cramer's Rule. Thus, SO(3) is a Lie group. 3

De nition 7 (Lie Algebra) A Lie algebra is a real vector space, V , with a multiplication operation [ ; ] which satis es, for A; B 2 V , 1. [A; B ] = ?[B; A]; 2. [A; B + C ] = [A; B ] + [A; C ], [A + B; C ] = [A; C ] + [B; C ]; 3. for r 2 R; r[A; B ] = [rA; B ] = [A; rB ]; 4. [A; [B; C ]] + [B; [A; C ]] + [C; [A; B ]] = 0.

The set of skew symmetric matrices so(3) forms a Lie algebra under the Lie bracket operation, for A; B 2 so(3): [A; B ] = AB ? BA: Each Lie group has its associated Lie algebra, and the two are related through the exponential map. We have shown that the Lie group SO(3) is related to so(3) through the exponential map: it turns out that so(3) is the Lie algebra associated with SO(3). Since so(3) has a one-to-one relationship with R , the knowledge of this exponential map between so(3) and SO(3) is handy for constructing splines in SO(3). 3

3

3 Interpolating Splines Interpolation in SO(3) is intuitively similar to that in R : given a sequence of vertices de ned by rotation matrices, nd a smooth orientation trajectory which interpolates these vertices. In the following three sections, three dierent interpolatory splines in SO(3) are created. The rst spline is calculated by minimizing the L norm of the angular velocity of the rotating object. The second spline is calculated by rst constructing a cubic interpolatory spline in the Lie algebra so(3) and then using the exponential map to nd the SO(3) counterpart. The third spline is calculated by minimizing the L norm of changes to the angular velocity. 3

2

2

3.1 Splines which minimize angular velocity The spline created in this section minimizes the L norm of the angular velocity of the rotating object. It is based on the work in optimal control on Lie groups, presented in [3] and [4]. Consider a rigid body rotating in space. The state of the system, describing the orientation of the object with time, can be represented as an element g 2 SO(3). The time dierential equation which describes the evolution of g can be written as: 2

3 X

g_ = g( Xiui)

(1)

i=1

where Xi are a basis for the Lie algebra so(3) and ui are the inputs to the system, the angular velocities corresponding to the changes in the roll, pitch, and yaw angles. Suppose we wish to minimize the L norm of the control, ie. minimize 1 Z k u(t) k dt 2 where u = [u ; u ; u ]T . Using the method of [4], we use as a basis for the Lie algebra the following: 3 2 3 2 3 2 0 ?1 0 0 0 1 0 0 0 (2) X = 64 0 0 ?1 75 X = 64 0 0 0 75 X = 64 1 0 0 75 0 0 0 ?1 0 0 0 1 0 2

1

2

0

1

2

3

1

3

2

We form the Hamiltonian

X H = 12 k u(t) k ? < p; g Xi > ui i where the function p is called the costate and lives in the cotangent space T SO(3). Minimizing the Hamiltonian with respect to the ui, we may write the optimal controls symbolically as: ui =< p; gXi > : 3

2

=1

4

De ne the momentum functions Pi = ui . Using the structure of the Lie algebra we obtain: P_ = ?P P + P P = 0 P_ = P P ? P P = 0 P_ = ?P P + P P = 0: Therefore, the optimal controls are constants. 1

3

2

3

2

2

1

3

1

2

1

3

3

1

2

Remark 1: This optimization procedure provides us only with the form of the optimal controls (ie. constants), we must use the provided initial and nal conditions to solve for their actual values.

Implementation in MATLAB Suppose we are given two orientations and times, (g ; T ) and (g ; T ), where g and g are the vertices in SO(3), and we wish to calculate the minimum angular velocity spline to interpolate these vertices. The previous formulation dictates that we use a constant control u = [u ; u ; u ]T between the vertices. Using the basis for the Lie algebra de ned above, we may rewrite equation (1) as: 3 2 0 ?u u 7 (3) g_ = g 64 u 0 ?u 5 ?u u 0 Integrating equation (3) results in: 2 3 0 ?u u g(t) = g exp(64 u 0 ?u 75 (t ? T )) (4) ?u u 0 where g(T ) = g . The control matrix may be calculated using the matrix logarithm: 3 2 0 ?u u 64 u 0 ?u 75 = log(gT  g )=(T ? T ); (5) ?u u 0 and the spline in SO(3) may then be calculated by substituting this constant control matrix into equation (4). This process is repeated for each pair of vertices in the sequence. The splines calculated by this method are only C continuous. This is because on any segment, the constant angular velocity is determined only by the vertices adjacent to that segment. Thus, the angular velocity changes at each vertex. The resulting spline interpolates the vertices, but rotates in a \jerky" manner. Figure 1 displays the trajectory followed by the corner [1; 1; 1]T of a cube which is centered at the origin of the (x; y; z) space. The cube is dictated to rotate through the sequence: g = I g = Rx(=2) g = Ry (=4) g = Rz (=2) 0

1

2

0

1

3

3

2

3

1

2

1

3

0

2

3

0

1

2

1

1

1

1

3

3

2

1

2

0

1

1

1

0

0

1

2

5

3

0

0

1

where, for example, Rx(=2) corresponds to a rotation about the x-axis by =2 radians, and is written as: 3 3 2 2 1 0 0 1 0 0 Rx(=2) = 64 0 cos =2 ? sin =2 75 = 64 0 0 ?1 75 0 1 0 0 sin =2 cos =2 Splines which minimize magnitude of angular velocity on SO(3)

1.5

g0*[1;1;1]

g3*[1;1;1]

Z axis

1

g1*[1;1;1] 0.5

g2*[1;1;1] 0 2 2

1

1 0

0 −1 −1

Y axis

−2

X axis

Figure 1: Showing the [1; 1; 1] cube corner as it rotates through g = I , g = Rx(=2), g = Ry (=4), g = Rz (=2) 0

2

1

3

3.2 Splining in the Lie algebra The spline which is presented in this section is created by rst constructing a polynomial spline in the Lie algebra so(3), and then exponentiating this spline to the Lie group SO(3). This idea corresponds to the method of [5]. This method works because there is a one-to-one correspondence between the Lie algebra so(3) and R : 2 3 3 2 0 ?! ! ! 64 ! 0 ?! 75 64 ! 75 ?! ! 0 ! 3

3

2

3

1

1

2

1

2 3

6

A natural cubic interpolatory spline is used to interpolate the vertices in R . Because the exponential map is continuous, the continuity properties of the spline are maintained in the Lie group. Thus, this method generates a spline in SO(3) which is C continuous (continuous in angular velocity and acceleration). 3

2

Implementation in MATLAB Given two orientations g and g , we calculate the cubic interpolatory spline by rst applying the inverse exponential map (the logarithmic map) to the two orientations: !^ = log(g ) !^ = log(g ) where !^i 2 so(3). The vertices in R are then extracted from the !^i: p = [^! (3; 2); ?!^ (3; 1); !^ (2; 1)] p = [^! (3; 2); ?!^ (3; 1); !^ (2; 1)]: The spline in R is then constructed between p and p using a cubic interpolatory spline with natural end conditions. Finally, the inverse of the above operations are performed to create the spline in SO(3). 0

1

0

0

1

1

3

0

0

0

0

1

3

1

1

1

1

2

Remark 2: It is important to note that although, in R , the cubic interpolatory spline 3

is the one which minimizes the L norm of the second parametric derivative of the curve (variational principle), the spline in SO(3) does not necessarily have this property! The exponential map does not preserve optimality in this sense. We address this issue in the next section. Figure 2 displays the trajectory followed by the corner [1; 1; 1]T of the cube as it rotates through the same trajectory as in the previous section. The vertices are shown in yellow. 2

3.3 Variational Principle extended to SO(3) The third method of constructing interpolating splines on SO(3) involves calculating the spline which interpolates but which also minimizes the L norm of the second parametric derivative of the curve. It was noted in the previous section that it is not sucient to minimize this norm in the Lie algebra, since the exponential map does not preserve optimality. In this section, a method is presented which performs this optimization in the Lie group, using methods of Riemannian geometry. The method to perform the optimization is presented in [6] and [7]. The method seeks a curve g : R ! SO(3) which minimizes the function 1 Z k !_ (t) k dt 2 where ! is the angular velocity. The curve satis es the dierential equation g_ (t) = g(t)^! (6) 2

1

2

0

7

C2 Interpolating Spline: Splining in the Lie Algebra

1.5

g3*[1;1;1]

g0*[1;1;1]

Z axis

1

g1*[1;1;1] 0.5

g2*[1;1;1]

0 2

2

1

1 0

0 −1 Y axis

−1

−2

X axis

Figure 2: Showing the corner [1; 1; 1] of the cube as it rotates through g = I , g = Rx(=2), g = Ry (=4), g = Rz (=2) 0

2

1

3

where !^ is the skew symmetric matrix corresponding to the angular velocity. Using a biinvariant metric in SO(3), it is shown in [6] that a minimizing curve g : R ! SO(3) has angular velocity which satis es the dierential equation:

d ! = d !  !: dt dt 3

2

3

2

(7)

Closed form solutions to this dierential equation cannot, in general, be found. However, for some special cases these solutions can be calculated. Consider the special case of a system evolving on SO(3) with state g(t) = g(T )e t : (8) Note that this case corresponds to a rotation about a xed axis: the individual elements of ^ are xed and cannot change with time, as they could in the !^ (t) of the previous section. Taking the derivative, we have that 0

( )^

g_ (t) = g(t)_(t)^: 8

(9)

Comparing equations (6) and (9) results in !^ = _(t)^: Thus !(t) = _(t) !_ (t) = (t) ! (t) =  (t) ! (t) =  (t) Substituting into equation (7), we have that  (t) =  (t)  _(t) = 0; so that  (t) = 0 and (t) must be a cubic. (2)

(3)

(3)

(4)

(4)

(10)

(11)

(3)

(4)

Implementation in MATLAB Given two orientations g = e0 and g = e1 to interpolate, the minimizing spline in SO(3) may be calculated for a few special cases. If the angular velocities at these two vertices, ! and ! , are either set to zero or to some multiple of , then _ and _ may be calculated. The spline is then completely determined from  ,  , _ , and _ , and the knowledge that (t) is a cubic. Implementation is trickier than in the previous cases. The inertial frame of reference must be rede ned at each vertex to align with this vertex, and each rotation in the ensuing segment must be calculated relative to this inertial frame. This is because the axis of rotation is xed along a segment, but may change at a vertex. For example, along the rst segment, the inertial frame is represented by the identity I , and the vertex g is rede ned as g~ = gT g . Once the frames have been rede ned in this way, the cubic Hermite interpolating spline (t) is calculated, and is mapped through the exponential to obtain the spline in SO(3). Figure 3 displays the trajectory followed by the corner [1; 1; 1]T of the cube as it rotates through the same trajectory as in the previous sections. The angular velocity at each of the vertices was chosen to be zero !i = 0. 0

^

^

1

0

1

0

0

1

1

0

1

1

1

0

1

Remark 3: Note that the spline is C , even though it resembles the C spline of the rst 2

0

method. The angular velocity of the cube goes to zero at the vertices, and the cube actually stops. This can be seen from the relative density of the trajectory at the vertices as opposed to at the midpoint between the segments.

4 Approximating Splines Again, the notion of approximation in SO(3) is the same as that in R : we wish to nd a smooth curve which approximates a set of vertices, where a vertex corresponds to a particular 3

9

C2 Interpolating Spline, Minimizes Changes in Angular Velocity

1.5

g0*[1;1;1]

g3*[1;1;1]

Z axis

1 g1*[1;1;1] 0.5

g2*[1;1;1] 0 2 2

1 1

0 0

−1 Y axis

−1 −2

−2

X axis

Figure 3: Showing the corner [1; 1; 1] of the cube as it rotates through g = I , g = Rx(=2), g = Ry (=4), g = Rz (=2) 0

2

1

3

orientation of the rigid body. This section constructs uniform cubic B-splines on SO(3). The method used is the same as the second method of interpolating splines: create the spline on the Lie algebra and then map the curve through the exponential map to SO(3). Figure 4 and gure 5 display two examples: the rst with oating end conditions, the second with end conditions such that the curve is closed.

5 Visualization of Splines in SO(3) The MATLAB routines described in the previous sections were used to create the splines, which were then displayed using a routine in OpenGL which depicts a cube rotating about the origin of the (x; y; x) frame. The graphics are displayed in gure 6.

10

Uniform Cubic B−Splines on SO(3)

1.4 1.2

Z axis

1 0.8 0.6 0.4 0.2 0 2 1.5

1

1 0.5

0

0 −0.5 −1

Y axis

−1

X axis

Figure 4: Showing the [1; 1; 1] cube corner as it rotates through g = I , g = Rx(=2),

g = Ry (=4), g = Rz (=2), g = Ry (=3), g = Rz (=4), g = I 2

3

4

5

0

1

6

While all of the MATLAB routines were written in terms of rotation matrices R in SO(3): 2 R = 64

r r r

11 21 31

r r r

12 22 32

r r r

13 23

3 7 5

33

the rotation routines in OpenGL require orientations in terms of ZY X Euler angles, or roll (), pitch (), and yaw ( ) angles. The conversion is calculated as follows:

R = Rz ( )Ry ()Rx() yielding

q

 = atan2(?r ; (r + r )) = atan2(r = cos ; r = cos )  = atan2(r = cos ; r = cos ) 31

2 32

21

11

32

33

11

2 33

Uniform Cubic B−Splines on SO(3)

1.4 1.2

Z axis

1 0.8 0.6 0.4 0.2 0 1 1.5

0.5 1.4

0

1.3 1.2

−0.5 Y axis

1.1 −1

1

X axis

Figure 5: Showing the [1; 1; 1] of the cube as it rotates through g = I , g = Rx(=2),

g = Ry (=4), g = I , g = Rx(=2), g = Ry (=4) 2

3

4

0

1

5

Singularities One of the problems encountered in converting from rotation matrices to Euler angles is the existence of singularities (the lack of global smooth solutions to the inverse problem of determining the Euler angles from the rotation). For example, when  = ?=2, a unique solution for the angles cannot be found. Any 3-dimensional representation of SO(3) will always have singularities, which is why 4-dimensional representations (quaternions) are often used to parameterize SO(3).

6 Conclusions Lie group splines which simply minimize the L norm of the rst parametric derivative are only C continuous at the vertices. These splines are therefore not useful when applied to trajectory planning in SO(3) or SE (3). Splines which are constructed by forming a cubic interpolatory spline in the Lie algebra and 2

0

12

Figure 6: Visualization of Splines in SO(3) then exponentiating this to the Lie group are C continuous, yet they are not optimal in the sense of minimizing the L norm of the second parametric derivative of the curve. To achieve a spline in a Lie group which is both C continuous and optimal in some sense, it is necessary to perform the optimization on the Lie group itself. Much of the research to date in splines through Lie groups seems to use the well known notions of splining in Rn and extends this through the exponential map to the Lie group. There are some exceptions, namely [6], [7], [8], and [9] (where quaternions are used). It would be interesting to study in more depth how subdivision and re nement algorithms (such as Oslo or de Casteljau) would evolve on a Lie group. 2

2

2

References [1] Richard H. Bartels, John C. Beatty, and Brian A. Barsky. An Introduction to Splines for use in Computer Graphics and Geometric Modeling. Morgan Kaufmann, 1987. [2] Richard M. Murray, Zexiang Li, and S. Shankar Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. [3] John Baillieul. Geometric methods for nonlinear optimal control problems. Journal of Optimization Theory and Applications, 25(4):519{548, 1978. [4] S. Shankar Sastry and Richard Montgomery. The structure of optimal controls for a steering problem. In International Federation of Automatic Control, pages 135{140, 1992. 13

[5] Richard Betts Kraft. Splines through groups. Master's Thesis, Department of Mathematics, University of California, Berkeley, 1990. [6] Lyle Noakes, Greg Heinzinger, and Brad Paden. Cubic splines on curved spaces. IMA Journal of Mathematical Control and Information, 6:465{473, 1989. [7] Gregory Paul Heinzinger. Splines on curved spaces. Master's Thesis, Department of Mathematics, University of California, Berkeley, 1990. [8] Peter Crouch, Robert Hermann, and Fatima Silva-Leite. Splines, lie theory, and recursion. To be submitted to Acta Applicanda Mathematica, November 1995. [9] Ken Shoemake. Animating rotation with quaternion curves. In SIGGRAPH Proceedings, pages 245{254, 1985.

14