TAADC D1V1roboticslab.uc3m.es/publications/Lie Groups and Lie Algebras in... · 2.1 Geometric...

2004

Lie Groups and Lie Algebras

in Robotics.

Author: José Manuel Pardos Gotor Director: Dr. Carlos Balaguer Bernaldo de Quirós

D

Ár

Universidad Carlos III de Madrid epartamento de Ingeniería Eléctrica, Electrónica y

Automática ea de Ingeniería de Sistemas y Automática.

Abstract Humanoid Robot Kinematics modeling for robotics real-time

applications requires computationally efficient solutions of the inverse kinematics problem. As the complexity increases, the need for more elegant formulations of the equations of motion becomes a paramount issue. This paper investigates analytical methods, using techniques and notation from the theory of Lie groups and Lie algebras, to develop geometric algorithms for the kinematical analysis, modeling and simulation of humanoid robots.

We introduce a new closed-form analytical solution, geometrically

meaningful and numerically stable, based on the PoE (Product of Exponentials formula), which allows us to develop a geometric algorithm to solve the humanoid inverse kinematics problem, built upon the Paden-Kahan subproblems.

Our approach to path planning the WBT (Whole Body Trajectory) for

humanoid robots computes collision-free trajectories by the development of the new FM3 (Fast Marching Method Modified) algorithm. We present a general and analytical solution for the inverse kinematics, satisfying the balance constraints of the ZMP (Zero Moment Point) trajectory, based on a Humanoid SKD (Sagital Kinematics Division) approach; this is the humanoid OSG (One Step to Goal) algorithm. The motion of the humanoid consists on applying successive goals from the WBT, to the inverse kinematics algorithm.

Our results are illustrated in studies with the 21 DoF (degree-of-

freedom) humanoid robot RH0 (Universidad Carlos III de Madrid). The algorithm is presented along with computed simulation examples.

Keywords: humanoid robot, inverse kinematics, Lie Groups, PoE product of exponentials, FM3 fast marching method modified, Paden-Kahan subproblems, SKD sagital kinematics division, OSG one step to goal.

2

General Index ABSTRACT............................................................................................. 2 GENERAL INDEX ................................................................................... 3 1. INTRODUCTION ................................................................................. 4 2. BACKGROUND ................................................................................... 5

2.1 Geometric Methods in Robotics - Lie Groups & Algebras – Screw theory. 5 2.2 The PoE (Product of Exponentials) formula – Forward Kinematics. .......... 6 2.3 The PoE Inverse Kinematics by Paden-Kahan subproblems. ....................... 7 2.4 FMM (Fast Marching Methods).......................................................................... 9

3. HUMANOID MODEL – RH0 ROBOT ................................................ 10 3.1 Humanoid SKD (Sagital Kinematics Division). ............................................. 10

4. WBT (WHOLE BODY TRAJECTORY) PATH PLANNING ................. 11 4.1 FM3 (Fast Marching Method Modified) Algorithm....................................... 11

5. HUMANOID KINEMATICS MOTION................................................. 13 5.1 Humanoid OSG (One Step to Goal) Inverse Kinematics Algorithm......... 13 5.2 Humanoid Footstep Planning. .......................................................................... 14 5.3 Half-Humanoid/Sagital-Manipulator Inverse Kinematics Algorithm. ....... 15

6 CONCLUSION.................................................................................... 17 REFERENCES....................................................................................... 18

3

1. Introduction

The Humanoid robot kinematics modeling presents a formidable computational challenge due to the high number of degrees of freedom, complex kinematics models, and balance constraints ([10],[12],[20]). Traditionally, inverse kinematics algorithms have been developed through a numerical approach based on a variety of optimization and Heuristic methods ([7],[11],[15]). Conversely, this paper presents a new analytical closed-form method for the solution of the humanoid robot kinematics modeling using techniques from the theory of Lie Groups, such us the PoE (Product of exponentials formula), allowing elegant, fast and efficient calculations, even for real-time applications.

One objective is to path planning the collision-free WBT (Whole

Body Trajectory) for the humanoid, by developing the FM3 (Fast Marching Method Modified) algorithm, based on Level Set and Fast Marching Methods ([1]), which are techniques for analyzing evolving interfaces in computational geometry.

Another objective of this paper is to introduce the OSG (One Step to

Goal) algorithm, for generating statically-stable motions through the solution of the humanoid inverse kinematics, by imposing balance constraints of the ZMP (Zero Moment Point) trajectory, upon incremental goals obtained during the WBT search. The OSG algorithms is developed from a divide and conquer approach called SKD (Sagital Kinematics Division), which from the kinematical point of view, considers the humanoid to be divided by the sagital plane into two synchronized manipulators. On so doing, the inverse kinematics problem is solved in an analytical and complete (for all DoF) way, using techniques from the Lie groups, these are, PoE and the Paden-Kahan subproblems ([2],[21],[22]).

The development of general-purpose motion generation tools and

techniques has been based on the use of virtual humanoid robot platforms ([10], [12]). In a similar way, for testing our results with the 21 DoF humanoid robot RH0 (Universidad Carlos III de Madrid), we use a graphical simulation environment with the Humanoid based on the H-Anim (Humanoid Animation) definition, and VRML (Virtual Reality Modeling Language) for the building of the virtual world.

4

2. Background

We provide a description of the rigid body motions using the tools of Lie algebras and Screw theories for the study of robot kinematics ([2], [3], [5], [16]). From these theories, we present the tools we use for the analytical solution of the humanoid inverse kinematics problem, these are: the POE (product of exponentials) and the Paden-Kahan subproblems ([21],[22]).

The question of motion planning and obstacle avoidance for the

humanoid is analyzed through the use of Fast Marching Methods ([1]). 2.1 Geometric Methods in Robotics - Lie Groups & Algebras – Screw theory.

If points on a manifold also satisfy a group structure and both

mappings are smooth then this manifold is a Lie group ([2], [5]). Lie groups are important in mathematical analysis, physics and geometry because they serve to describe the analytical structures. The homogeneous representation of a rigid motion, this is, the space of 4 x 4 matrices “g”, belongs to the Lie group SE(3) (Especial Euclidean group).

positionpnOrientatioRpR

g ==→⎥⎦

⎤⎢⎣

⎡= ;

10

{ } )3()3(,:),()3( 33 SOSORpRpSE ×ℜ=∈ℜ∈= Lie algebra is an algebraic structure whose main use lies in studying

geometric objects such as Lie groups. A Lie algebra is a vector space “g” over some field, together with a binary operation [·, ·] : g × g -> g, called the Lie bracket, which is bilinear and satisfies the Jacobi identity. The Lie algebra of SE(3), denoted se(3), can be identified with the matrices ξ^, called “twists”.

^,00

^^ 3 ξυω

υωξ ofsCoordinateTWISTtheareℜ∈→⎥

⎦

⎤⎢⎣

⎡=

[ ] ^1

^2

^2

^1

^2

^1 , ξξξξξξ −=

5

________ 6

)3(0

00

^

12

13

23

3

2

1

somatrixsymmetricskew ∈−⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−

−=→

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

ωωωωωω

ωωωω

ω

A “twist” can be interpreted geometrically using the theory of screws

([16]). Chasles´s theorem proved that any rigid body motion could be produced by a translation along a line followed by a rotation about that line, this is, a screw motion, and the infinitesimal version of a screw motion is a twist.

2.2 The PoE (Product of Exponentials) formula – Forward Kinematics.

The matrix exponential, maps “twist” into the corresponding screw

motion. There are several advantages to using the matrix exponential for describing rigid body kinematics. One is that they allow a global description of rigid body motion, which does not suffer from singularities due to the use of local coordinates. Another is that screw theory provides a very geometric description of rigid motion, which hugely simplifies the analysis of robots.

The exponential map for a twist gives the relative motion of a rigid

body. The interpretation of this transformation is not as mapping points from one coordinate frame to another, but rather as mapping points from their initial coordinates, to their coordinates after the rigid motion is applied.

formulaRodriguesIe

eIepRg

T

';)cos1(sin

010

)()(10

2^^^

^^

θωθω

ωυθωωυω

θω

θωθω

−++=

≠⎥⎥⎦

⎤

⎢⎢⎣

⎡ +×⋅−=⎥⎦

⎤⎢⎣

⎡=

This formulation presents a geometric treatment of spatial body

motion, as a generalized sequel of the elegant notion of the exponential mapping of a rotation introduced by Rodrigues ([2], [5], [16], [17]).

Since modern robot manipulators and humanoids are typically

constructed by connecting different joints (the most common are revolute and prismatic) using rigid links, restricting the motion to a subgroup of SE(3), the PoE formula represents a natural starting point for the analysis of the kinematics of a mechanism as the product of exponential of twists.

It is possible to generalize the forward kinematics map for an arbitrary

open-chain manipulator with n DoF (Degrees of Freedom). Let S be a frame attached to the base system and T be a frame attached to the last link of the manipulator. Let θ be value of the DoF, this is, rotation for revolute joints and translation for prismatic ones. The reference configuration of the manipulator is the one corresponding to θ = 0, and gst(0) represents the rigid body transformation between T and S when the manipulator is in its reference configuration. For a revolute joint, ω is a unit vector in the direction of the twist and q is any point on

© 2004 J.M. Pardos - Universidad Carlos III - Ingeniería de Sistemas y Automática

________ 7

the axis. For a prismatic joint, υ is a unit vector pointing in the direction of translation. Then, the product of exponentials formula for the manipulator forward kinematics is gst(θ).

prismaticfortwist

revolutefortwistq

ii

i

iii

;0

;

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡=

⎥⎦

⎤⎢⎣

⎡ ×−=⎥

⎦

⎤⎢⎣

⎡=

υωυ

ξ

ωω

ωυ

ξ

( ) ( )0^

2^

21^

1stst geeeg nn ⋅⋅= θξθξθξθ L

2.3 The PoE Inverse Kinematics by Paden-Kahan subproblems.

The Inverse Kinematics problem consists on finding for an arbitrary

open-chain manipulator the value θ, of the DoF, which given the forward kinematics map gst, and the desired configuration gd, makes gst(θ)= gd.

This problem may have multiple solutions, for instance, 16 is the

upper bound number of solutions for any open-link mechanism with six DoF. As the number of DoF increases (e.g: the RH0 Humanoid has 21 DoF), the number of solutions increases as well, and a general numerical solution based on elimination theory of the variables in a system, becomes a “brute-force” procedure not very suitable for fast real-time calculations.

One payoff for the product of exponentials formalism is that it provides

an elegant formulation of a set of canonical problems (the Paden-Kahan subproblems) for solving the robotics inverse kinematics problem in a closed-form by developing geometric algorithms ([21],[22]).

( )[ ]

⎥⎦

⎤⎢⎣

⎡=

−=

−=

⋅×=

ωυ

ξ

ωω

ωω

ωθ

vvvuuu

vuvua

T

T

TT

''

'',''2tan

Figura 2: PK Subproblem 1 – Rotation about a single axis.


________ 8

( )( )

( )( )

( )

⎥⎦

⎤⎢⎣

⎡=∧⎥

⎦

⎤⎢⎣

⎡=

×

−−−=

−

−=

−

−=

×±++=

2

22

1

11

221

21222

2

2

21

2121

2

21

1221

2121

2

1

1

ωυ

ξωυ

ξ

ωω

ωαβωβαγ

ωω

ωωωωβ

ωω

ωωωωα

ωωγβωαω

T

T

TTT

T

TTT

u

uv

vu

rc

Figura 3: PK Subproblem 2 – Rotation about two subsequent axis.

Once we get c for the second subproblem, we can apply the first Paden-Kahan subproblem to get the solutions for θ1 and θ2. Beware that there might exist two solutions for c , each one of them gives a solution for θ1 and θ2.

( )[ ]( )

⎥⎦

⎤⎢⎣

⎡=

−=

−=

−−=

⋅×=

⎟⎟⎠

⎞⎜⎜⎝

⎛ −+±= −

ωυ

ξ

ωω

ωω

ωδδ

ωθ

δθθ

vvvuuu

qp

vuvua

vuvu

T

T

T

TT

''

'

'',''2tan

''2'''

cos

222

0

2221

0

Figura 4: PK Subproblem 3 – Rotation to a given distance.

From the solution for the third subproblem is clear that it could lead to

multiple solutions as well. Once these subproblems are solved, the full inverse kinematics

problem can be solved by reducing it into appropriate subproblems ([2]), which are geometrically meaningful and numerically stable.


________ 9

2.4 FMM (Fast Marching Methods). The Level Set and Fast Marching Methods ([1]), are numerical

techniques for analyzing interface motion. They rely on an Eulerian perspective for the view of moving boundaries. The results can be used to track three-dimensional complex fronts that can change topology as they evolve.

The FMM (Fast Marching Methods) can be applied to solve optimal

path planning problems, through solving the Eikonal equation. The Eikonal equation gives the first arrival or viscosity solution, extracting among all possible solutions, the one that corresponds to the first arrival of information from the initial disturbance.

11

)(

=∇

==

FTdxdTF

dTFdx

Figura 5: One dimension Eikonal Equation example.

The algorithm applies the FMM to solve the Eikonal equation to

produce the Arrival Function T. Then, given the goal point, explicit construction of the shortest path comes through back propagation via the solution of the ordinary differential equation. We can do so by using Heun’s method. An efficient scheme for R2 is presented hereafter, being h the integration step ant t the time.

hthTtTTD

htTthTTD

FTDTD

TDTD

ij

ij

ijy

ijy

ij

xij

xij

),(),(

),(),(

1)0,,max(

)0,,max( 21

2

2

−−≡

−+≡

=⎥⎥⎦

⎤

⎢⎢⎣

⎡

−+

−

−

+

+−

+−

αα

αα

α

α

Figura 6: Two dimension FMM scheme example.

To path planning the collision-free WBT (Whole Body Trajectory) for

the humanoid, we develop the FM3 (Fast Marching Method Modified) algorithm, based on the presented scheme.


________ 10

3. Humanoid Model – RH0 Robot

The Universidad Carlos III de Madrid has lunched RH0 humanoid robotics project. The mechanical design concepts of RH0 are light, compact, but performable for application tasks like working cooperation with humans.

Several architecture humanoid robotics platforms have been

developed in recent years ([10], [12]). There are many advantages for an open architecture software and hardware to obtain the consistency between the simulator and a real humanoid robot, and therefore we work on the same idea, even though we did not joint to any specific architecture. The models of robots and their working environment are described by VRML97 format that is extended for a humanoid animation by h-anim working group [19].

3.1 Humanoid SKD (Sagital Kinematics Division). Our objective is to find a close-form solution for the Humanoid

inverse kinematics problem which allows us fast and real time computation, but because the RH0 has many degrees of freedom (21 DoF for the body - without hands and head), there might be many solutions for any motion, and we search for some kind of simplification to solve the problem.

We introduce the idea of considering the Humanoid to be divided by

the sagital plane into two kinematical different mechanisms, this is what we call SKD (Sagital Kinematics Division). This is a divide and conquer approach, which allows us to solve the humanoid inverse kinematics problem in a much easier way; this is, solving the inverse kinematics problem for two synchronized but independent manipulators.

The idea behind the SKD is that we can analyze and control the whole

humanoid body, considering separately the left and right body halves, as a resemblance of the two hemispherical body locomotion control of the human brain.


________ 11

4. WBT (Whole Body Trajectory) Path

Planning The automatic WBT (whole body trajectory) motion planning for

humanoid robots presents a formidable computational challenge due to the high number of degrees of freedom, complex kinematics models, and balance constraints ([10],[12]). The challenge is to search for a solution path that lies within the free configuration space, checking for collisions with obstacles. All that remains is to calculate a final solution trajectory that is collision-free.

We have developed a new version of FMM (Fast Marching Method)

([1]), for generating collision-free trajectory motions for the entire body. The points of the solution trajectory will become the goals for the humanoid inverse kinematics algorithm we present in this paper.

4.1 FM3 (Fast Marching Method Modified) Algorithm.

The FMM practical schemes for solving the Eikonal equation have to

solve a quadratic equation for each step of the algorithm, but in general the differentiability is not guaranteed, making necessary the use of slow numeric methods, such us bisection.

1=∇ FT The new version of FMM is the recently developed algorithm FM3,

which stands for Fast Marching Method Modified. This new algorithm overcomes the problem of having slow numeric methods trough the building of a lineal scheme for approximating the Eikonal equation, directly resoluble, and therefore


________ 12

computationally very fast and suitable for real time applications. This method allows to always find out a cuasi-optimal humanoid WBT path, whatever the nature of the obstacles, this is, the FM3 solves the humanoid collision-free path inside a world with regular, irregular and even non-convex geometry obstacles.

The FM3 solves the Eikonal equation to produce the Arrival Function

T. Then, given the goal point, explicit construction of the shortest path comes through back propagation following the maximum negative gradient. An efficient scheme for R3 is presented hereafter, being h the integration step ant t the time.

∫=

⎟⎠⎞

⎜⎝⎛ −±

≡

=

±

+−+−+−

),,(),,(min),,(

),(),(

1)0,,,,,,max(

zyx

A

ij

ij

zij

zij

yij

yij

xij

xij

dzyxFzyxT

hyTyhTAbsTD

FTDTDTDTDTDTD

τ

ααα

From the proposed scheme, we get an Arrival Function T, which is an

increasing function in sense of information propagation from the origin to the objective points, for all points inside the configuration space. Graphically, the arrival function in R3 is the development of a plane interface in motion; it’s a kind of accumulative pyramidal fronts. Therefore, the lineal approached of the FM3 generates a non-differentiable but continuous T function, which allows us to solve the path planning. The obtained path can easily be smoothed if necessary.

Figure 2: Fast Marching Method Modified (FM3), with collision free trajectory.


________ 13

5. Humanoid Kinematics Motion

We introduce the OSG (One Step to Goal) algorithm, for generating statically-stable humanoid motions based on incremental goals obtained from the WBT path planning, by imposing balance constraints of the ZMP (Zero Moment Point) trajectory, upon the inverse kinematics algorithm. The OSG algorithm is developed from the previously presented Humanoid model SKD (Sagital Kinematics Division) humanoid model. The humanoid inverse kinematics motion problem is solved in an analytical and complete way, using techniques from the Lie groups (PoE and the Paden-Kahan subproblems) ([2]).

5.1 Humanoid OSG (One Step to Goal) Inverse Kinematics Algorithm.

The idea behind the new OSG (One Step to Goal) algorithm is to

develop a general purpose statically-stable solution for the inverse kinematics problem of a humanoid moving one step towards a given goal.

The algorithm uses the concept of humanoid SKD (Sagital Kinematics

Division), for a divide and conquers approach, and the problem is separated on solving the inverse kinematics for two independent half-humanoid manipulators, subject to the following constraints at any time: keep the balance of the humanoid ZMP and impose the same position and orientation for the common parts (pelvis, thoracic, cervical) of the left and right humanoid manipulators.

The OSG receives as inputs the goal and the humanoid present

position, giving as outputs the values for each DoF (joint rotations), which make the humanoid to move one step towards the desired goal. The total movement is generated by five phases: the first is to Orientate the humanoid body towards the goal, the second is to Tilt the ZMP on a stable position over the pillar foot, the third is to Elevate the flying foot, the fourth is to Lean the flying foot according with the footstep planning and the fifth is to Balance the humanoid body to leave the ZMP in the center of the convex hull.


________ 14

Figure 3: The five phases for the OSG (One Step to Goal) Algorithm: 1-Orientate, 2-Tilt, 3-Elevate, 4-Lean and 5-Balance.

5.2 Humanoid Footstep Planning. The objective of the footstep planning is to define the position and

orientation for the humanoid feet along the five phases of the OSG algorithm. Actually, as the OSG algorithm only solves the inverse kinematics for one step of the humanoid, the footstep planning consists on solving the position and orientation for the flying foot, because the pillar foot remains unmoved at its present position. Obviously, when the OSG is repeated for a new goal, there is a switch in the algorithm, and the foot previously considered as flying becomes pillar, to allow a natural humanoid gait between the right and left feet.

There are several data to be taken into account for the calculation of

the footstep planning, mostly related with the gate definition (robot hip height on walking, flying foot maximum height, step width and step length), but some others are necessary, as world definition for obstacle avoidance and the orientation of humanoid body. The latter is very important, and is obtained after the phase one of the OSG algorithm; whether the humanoid body can be oriented towards the goal then the footstep planning will permit a step forward, but if the humanoid body can not be sufficiently turned in order to be directly orientated towards the goal, then the method will planned a humanoid step only to turn around the present humanoid axis to correct the orientation of the robot.


________ 15

5.3 Half-Humanoid/Sagital-Manipulator Inverse Kinematics Algorithm.

The OSG gives the humanoid ZMP position and orientation values for

each of the five phases of the algorithm (orientate, tilt, elevate, lean and balance. Besides, the position and orientation for the feet are given from the footstep planning. At this point, the last remaining problem to solve is the inverse kinematics for the left and right half-humanoid sagital-manipulators, which constitute the humanoid according with the SKD (Sagital Kinematics Division) approach, for the five phases of the OSG algorithm.

Hereafter we develop a general method, which is the core of this

paper, to solve the inverse kinematics problem for one humanoid sagital-manipulator in an analytical and complete way, using techniques from the Lie groups, these are, PoE and the Paden-Kahan subproblems ([21],[22]).

It is possible to consider the half-humanoid as and open-chain

manipulator and to generalize the leg forward kinematics map with 12 DoF (θ1…θ12). The first six DoF correspond to the position (θ1,θ2,θ3) and orientation (θ4,θ5,θ6) of the foot; beware that these DoF do not correspond with any real joint and for that reason we call them “non-physical DoF”. The other DoF are called “physical DoF” because they correspond with real motorized joints, these are: θ7 for the hindfoot, θ8 for the ankle, θ9 for the knee, θ10 for the hip on the x axis, θ11 for the hip on the z axis and θ12 for the hip on the y axis. Let S be a frame attached to the base system and T be a frame attached to the humanoid ZMP. The reference configuration of the manipulator is the one corresponding to θ = 0, and gst(0) represents the rigid body transformation between T and S when the manipulator is in its reference configuration. For any joint, ω is a unit vector in the direction of the twist and q is any point on the axis. Then, the product of exponentials formula for the manipulator forward kinematics is gst(θ).

⎥⎦

⎤⎢⎣

⎡ ×−=⎥

⎦

⎤⎢⎣

⎡=

i

iii

qωω

ωυ

ξ( ) ( )012^

122^

21^

1stst geeeg ⋅⋅= θξθξθξθ L

Figure 1: RH0 Humanoid and Sagital Kinematics Division (SKD) .


________ 16

Our inverse kinematics problem consists on finding the joint angles, this is, the six physical DoF (θ7…θ12), given the non-physical DoF (θ1…θ6) from the humanoid footstep planning, which achieve the ZMP humanoid desired configuration gst(θ). Using the PoE formula for the forward kinematics map presented above, it is possible to develop a geometric algorithm, numerically stable, to solve this problem, by the use of the Paden-Kahan geometric subproblems. For being able to apply them, we must define four interesting points in the humanoid leg geometry: p is a common point for the axis of the last three DoF, q is a common point for the axis of the two first physical DoF, t is a point on the axis of the last DoF and s is a point not on the axis of the last physical DoF. Now, it is straightforward to solve the inverse kinematics problem in an analytic, closed-form and geometrically meaningful way, with the following formulation.

We obtain θ9 using the third P-K subproblem.

( ) ( )

93

1

9^

9

12^

127^

71^

16^

6 0

θδ

θ

θξ

θξθξθξθξ

⎯⎯ →⎯−⋅=⎯→⎯

−⋅⋅⋅⋅=−⋅⋅⋅⋅⋅⋅

−−

−−−

KP

stst

qpe

qpeeqpggee

We obtain θ7 and θ8 using the second P-K subproblem.

( ) ( )

872

1

,''

08

^87

^7

12^

127^

71^

16^

6

θθ

θθξθξ

θξθξθξθξ

⎯⎯ →⎯⋅⋅=⎯→⎯

⋅⋅⋅⋅=⋅⋅⋅⋅⋅⋅−−

−−−

KP

stst

peeq

peepggee

We obtain θ10 and θ11 using the second P-K subproblem.

( ) ( )

11102

1

,''

011

^1110

^10

12^

1211^

1110^

101^

19^

9

θθ

θθξθξ

θξθξθξθξθξ

⎯⎯ →⎯⋅⋅=⎯→⎯

⋅⋅⋅=⋅⋅⋅⋅⋅⋅−−

−−−

KP

stst

peeq

teeetggee

Finally, we obtain θ12 using the first P-K subproblem.

( ) ( )

121

1

''

012

^12

12^

121^

111^

11

θ

θθξ

θξθξθξ

⎯⎯ →⎯⋅=⎯→⎯

⋅=⋅⋅⋅⋅⋅⋅−−

−−−

KP

stst

peq

sesggee

That is it!


________ 17

6 Conclusion We believe that abstraction saves time in the end, in return for an

initial investment in learning some mathematics. Therefore, this paper presents a slightly more abstract formulation, for humanoid robot kinematics modeling using the theory of Lie groups and Lie algebras. These techniques allow elegant, fast and efficient calculations for real-time applications.

The main development of this work is to introduce the new OSG (One

Step to Goal) algorithm, for generating statically stable humanoid motions through the solution of the inverse kinematics problem, by imposing balance constraints of the ZMP trajectory. We present the new approach called SKD (Sagital Kinematics Division), which from the kinematical point of view, considers the humanoid divided by its sagital plane into two synchronized manipulators. We solve the problem by an analytical method using techniques from the theory of Lie Groups, such us the PoE (Product of exponentials formula), and the Paden-Kahan subproblems.

Besides, this paper investigates the path planning problem for the

collision-free WBT (Whole Body Trajectory) of the humanoid, by developing the new FM3 (Fast Marching Method Modified) algorithm, based on techniques for analyzing evolving interfaces in computational geometry.

For testing our results we worked with the 21 DoF humanoid robot

RH0 (Universidad Carlos III de Madrid), we use a graphical simulation environment based on the H-Anim (Humanoid Animation) definition, and VRML for the building of the virtual world.

We hope that with the use of new mathematical developments, goal-

level planning algorithms and interactive simulation software, the capabilities of humanoid robotics technology will enter mainstream society during the next several decades.


References [1] J.A. Sethian. Level Set Mehods and Fast Marching Methods,

Evolving Interfaces in Computational…Cambridge University Press. 1999.

[2] R.M. Murray, Z. Li, and S.S. Sastry, A Mathematical Introduction to

Robot Manipulation. Boca Raton, FL: CRC Press, 1993. [3] R. Featherstone, Robot Dynamics Algorithms. Boston: Kluwer,

1987. [4] J.C. Latombe. Robot Motion Planning. Kluwer Academic

Publishers, Boston, USA, 1991. [5] F.C. Park, J.E. Bobrow, and S.R. Ploen, \A Lie group formulation

of robot dynamics," Int. J. Robotics Research. Vol. 14, No. 6, pp. 609-618, 1995.

[6] Ronald C. Arkin. Behavior Based Robotics. The MIT Press 1998. [7] Liegeois, A., Automatic supervisory control of the configuration and

behavior of multibody mechanisms. IEEE Transactions on Systems, Man, and Cybernetics, 1977. 7(12): p. 868-871.

[8] G. Rodriguez, A. Jain, and K. Kreutz-Delgado, \Spatial operator

algebra for multibody system dynamics," J. Astronautical Sciences. Vol. 40, No. 1, pp. 27-50, 1992.

[9] M. Vukobratovic and D. Juricic, “Contribution to the Synthesis of

Biped Gait,” IEEE Tran. On Bio-Medical Engineering, Vol. 16, No. 1, pp. 1-6, 1969.

[10] F. KANEHIRO, K. FUJIWARA, et al. Open Architecture

Humanoid Robotics Platform. Proceedings of the 2002 IEEE International Conference on Robotics & Automation Washington, DC May 2002.

18

Referencias 19

[11] Baillieul, J. Kinematic programming alternatives for redundant manipulators. in IEEE International Conference on Robotics and Automation. 1985.

[12] J.J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue.

Motion Planning for Humanoid Robots. In Proc. 11th Int’l Symp. of Robotics Research (ISRR 2003).

[13] Matlab 6.5, The MathWorks, Inc., 2003. [14] M. Hardt, K. Kreutz-Delgado, and J. William Helton, \Minimal

Energy Control of a Biped Robot with Numerical Methods and a Recursive Symbolic Dynamic Model," Proc. 37th IEEE Conference on Decision and Control, pp. 413{6, 1998.

[15] Klein, C.A., C. Chu-Jenq, and S. Ahmed, A new formulation of the

Extended Jacobian method and its use in mapping algorithmic singularities for kinematically redundant manipulators. IEEE Transactions on Robotics and Automation, 1995. 11(1): p. 50-55.

[16] R.S. Ball. The Theory of Screws. Cambridge University Press,

Cambridge, 1900. [17] R. Brockett. Robotic manipulators and the product of exponential

formula. In P. Fuhrman ed. Proc. Mathematical Theory of Networks and Systems pp. 120–129, 1984.

[18] F. Hausdorff. Die Symbolische exponential formel in den gruppen

theorie. Berichte de S¨achicen Akademie de Wissenschaften (Math Phys Klasse) vol. 58, pp. 19–48, 1906.

[19] http://www.h-anim.org/. HUMANOID ANIMATION

WORKING GROUP. [20] Inoue H., Tachi S., Tnie K., Yokoi K., Huirai S., Hirukawa H., Hirai

K., Nakayama S., Sawada K., Nishiyama T., Miki O., Itoko T., Inaba H., Sudo M., University of Tokyo, MEL, ETL, Honda R&D Co. Ltd., Matsuita Ltd, Kawasaki Ltd., Fanuc Ltd., “HRP: Humanoid Robotics Project of MITI”, Proc. First IEEE-RAS International Conference on Humanoid Robots (HUMANOID2000), Sep. 2000.

[21] B. Paden. Kinematics and Control Robot Manipulators. PhD thesis,

Department of Electrical Engineering and Computer Sciences, University of California, Berkeley, 1986.

[22] W. Kahan. Lectures on computational aspects of geometry.

Department of Electrical Engineering and Computer Sciences, University of California, Berkeley, 1983.


TAADC D1V1roboticslab.uc3m.es/publications/Lie Groups and Lie Algebras in... · 2.1 Geometric...

Documents

Transcript of TAADC D1V1roboticslab.uc3m.es/publications/Lie Groups and Lie Algebras in... · 2.1 Geometric...