Kinematics of Mechanisms [26]

Introduction to Mobile RobotsKinematics 2: Kinematics of Mechanisms

1

Alonzo Kelly Fall 1996

Kinematics 2: Kinematics of Mechanisms


21.1Nonlinear Mapping


1 Forward Kinematics Forward kinematics is the process of chaining homogeneoustransforms together in order to represent:

the articulations of a mechanism,or the fixed transformation between two frames which isknown in terms of linear and rotary parameters.

In this process, the joint variables are given, and the problemis to find the transform.

1.1 Nonlinear Mapping So, far we have considered transformations and operations ofthe general form:

and have used this form to perform essentially linear operationson .

The right hand side of this equation is typically linear wrtand nonlinear wrt . Most mechanisms are not linear - being composed of one ormore rotary degrees of freedom. We can still model them usinghomogeneous transforms by considering the parameters tobe associated with the rotary degrees of freedom. The problem of forward kinematics is one of understandingthe variation in a homogeneous transform when it is consideredto be a function of one or more variables. In the study of mechanisms, the real world space in which amechanism moves is often called task space. However, themechanism articulations are most easily expressed in terms of

p2 T pk( ) p1=

p1p1

pk

pk


31.2Mechanism Models


angles and displacements along axes. This space is calledconfiguration space. So far, we have studied linear transformations within taskspace. This section and the following sections will consider themore difficult problem of expressing the relationship betweentask space and configuration space.

1.2 Mechanism Models It is natural to think about the operation of a mechanism in amoving axes sense - because most1 mechanisms are built thatway. That is, the position and orientation of any link in akinematic chain is dependent on all other joints that comebefore it in the sequence. Conventionally, one thinks in terms of moving axis operationsapplied to frames embedded in the mechanism which move thefirst frame sequentially into coincidence with all of the othersin the mechanism. Then, a sequence of operators is written torepresent the mechanism kinematics.

1. The 4 bar mechanism is an important exception.


41.3Denavit Hartenberg Convention for Mechanisms


1.3 Denavit Hartenberg Convention for Mechanisms In 1955, J. Denavit and R. S. Hartenberg [14] first proposedthe use of homogeneous transforms to represent thearticulations of a mechanism. A mechanism is considered to beany collection of joints, either linear or rotary, joined together

Forward Kinematic Modelling The conventional rules for modelling a sequence ofconnected joints are as follows:

assign embedded frames to the links in sequencesuch that the operations which move each frame intocoincidence with the next are a function of theappropriate joint variable write the orthogonal operator matrices whichcorrespond to these operations in left to right order.

This process will generate the matrix that represents theposition of the last embedded frame with respect to thefirst, or equivalently, which converts the coordinates of apoint from the last to the first. This matrix will be called the mechanism model.




by links. The total number of movable joints is called thenumber of degrees of freedom.

It is conventional in many aspects of robotics to use a specialproduct of four fundamental operators as a basic conceptualunit. The resulting single matrix can represent the most generalspatial relationship between two frames of a class ofmechanims called lower pair mechanisms. This convention with a small number of associated rules forassignment of embedded frames has come to be called theDenavit-Hartenberg (DH) convention. You can use these in place of the basic orthogonal operatorsin forward kinematics because they are also orthogonaloperators (compound ones). For two frames positioned in space, the directions of, say, thez axes of two frames define two lines in 3D space. Fornonparallel axes, there is a well-defined measure of thedistance between them given by the length of their mutualperpendicular. Let the first frame be called frame i-1 and thesecond frame i. The first can be moved into coincidence withthe second by a sequence of 4 operations:

rotate around the xi-1 axis by an angle i

Joints

Links




translate along the xi-1 axis by a distance ui rotate around the new z axis by an angle i translate along the new z axis by a distance wi

The following figure indicates this sequence graphically.

The matrix, conventionally called Ai, that convertscoordinates from frame i to frame i-1 can be written using therules for forward kinematic modelling given previously.

xi-1

yi-1

zi-1

i

i

zi yi

xi

ui

wi

zi

zi

yi

yi-1

yi

Ai T ii 1 Rotx i( )Trans ui 0 0, ,( )Rotz i( )Trans 0 0 wi, ,( )= =

Ai T ii 1

=

1 0 0 00 ci s i 00 si ci 00 0 0 1

1 0 0 ui0 1 0 00 0 1 00 0 0 1

ci s i 0 0si ci 0 00 0 1 00 0 0 1

1 0 0 00 1 0 00 0 1 wi0 0 0 1

=

Ai T ii 1

=

ci s i 0 uicisi cici s i s iwisisi sici ci ciwi

0 0 0 1

=


71.4Example: 3-Link Planar Manipulator


Note especially that it has 4 degrees of freedom although ageneral 3D relationship between frames has 6. This worksbecause each DH matrix typically models only a single real dofwhile the other 3 are constant. This matrix has the following interpretations:

it will move a point or rotate a direction by the operatorwhich describes how frame i is related to frame i-1. its columns represent the axes and origin of frame iexpressed in frame i-1 coordinates it converts coordinates from frame i to frame i-1

1.4 Example: 3-Link Planar Manipulator Coordinate frames are assigned to the center of each rotaryjoint and the datum angle for each is set to zero as illustrated inthe figure below. It is conventional to assign the z axis of a frame so that theassociated degree of freedom, linear or rotary, coincides with it.Also, the x axis is normally chosen so that it points along themutual perpendicular. In this case, this means that frame 1 is arotated version of frame 0:

x0,1

y0,1

x2

y2

x3

y3

x4

y4

L1 L2 L3

21 3




The joint parameters are indicated in the table below:

This gives the following Denavit-Hartenberg matrices. Theirinverses are computed from the inverse formula because theywill be useful later: The position and orientation of the end effector with respectto the base is given by:

Table 1: 3-Link Planar Manipulator

Link u w

0 0 0 0

1 0 L1 0

2 0 L2 0

3 0 L3 0 0

123

T 40 T 10T 21T 32T 43 A1A2A3A4= =

T40

c1 s1 0 0

s1 c1 0 0

0 0 1 00 0 0 1

c2 s2 0 L1s2 c2 0 0

0 0 1 00 0 0 1

c3 s3 0 L2s3 c3 0 0

0 0 1 00 0 0 1

1 0 0 L30 1 0 00 0 1 00 0 0 1

=

T40

c123 s123 0 c123L3 c12L2 c1L1+ +( )s123 c123 0 s123L3 s12L2 s1L1+ +( )

0 0 1 00 0 0 1

=




A1

c1 s1 0 0

s1 c1 0 0

0 0 1 00 0 0 1

=

A2

c2 s2 0 L1s2 c2 0 0

0 0 1 00 0 0 1

=

A3

c3 s3 0 L2s3 c3 0 0

0 0 1 00 0 0 1

=

A4

1 0 0 L30 1 0 00 0 1 00 0 0 1

=

A11

c1 s1 0 0

s1 c1 0 0

0 0 1 00 0 0 1

=

A21

c2 s2 0 c2L1

s2 c2 0 s2L10 0 1 00 0 0 1

=

A31

c3 s3 0 c 3L2s3 c3 0 s3L20 0 1 00 0 0 1

=

A41

1 0 0 L 30 1 0 00 0 1 00 0 0 1

=


2 Inverse Kinematics 102.1Existence and Uniqueness


2 Inverse Kinematics The problem of finding the joint parameters given only thenumerical values of the homogeneous transforms which modelthe mechanism. Solves the problem of where to drive the joints in order to getthe hand of an arm or the foot of a leg in the right place.

2.1 Existence and Uniqueness Involves the solution of nonlinear equations. These may have no solution, or a non unique solution. It has been shown theoretically by Pieper, that a six degree offreedom manipulator for which the last three joints intersect ata point is always solvable. Most manipulators are constructedin this way, so most are solvable. Any real mechanism has a finite reach, so it can only achievepositions in a region of space known as the workspace. Unlessthe initial transform to be solved is in the workspace, theinverse kinematic problem will have no solution. Thismanifests itself, for example, as inverse sines and cosines ofarguments greater than 1. Mechanisms with many rotary joints will often have morethan one solution and these solutions will exhibit various formsof symmetry. This problem is known as redundancy.

2.2 Technique Using the DH convention, a mechanism can be solved moreor less one joint at a time. The inverse kinematic problem is solved by rewriting theforward transform in many different ways to attempt to isolate


2 Inverse Kinematics 112.2Technique


unknowns. Although there are only a few independentrelationships, they can be rewritten in many different ways. Using a three degree of freedom mechanism for example, theforward kinematics can be written in all of these ways bypremultiplying or postmultiplying by the inverse of each linktransform in turn:

These are, of course, redundant specifications of the same setof equations, so they contain no new information. However,they often generate equations which are easy to solve becausethe DH convention tends to isolate each joint. It is conventionalto use the left column of equations, but the right column iseasier to follow in our case, so it will be used in our example.

T 40 A1A2A3A4=

A11 T 40 A2A3A4=A21 A11 T 40 A3A4=A31 A21 A11 T 40 A4=A41 A31 A21 A11 T 40 I=

T 40 A1A2A3A4=T 40A41 A1A2A3=T 40A41 A31 A1A2=T 40A41 A31 A21 A1=T 40A41 A31 A21 A11 I=


2 Inverse Kinematics 122.3Example: 3 Link Planar Manipulator


2.3 Example: 3 Link Planar Manipulator The process is started by assuming that the forward kinematicsolution is known. Names are assigned to each of its elementsas follows:

For our example, we know that some of these are zeros andones, so that the end effector frame is:

The first equation is already known:

T n0

r11 r12 r13 pxr21 r22 r23 pyr31 r32 r33 pz0 0 0 1

=

T 40

r11 r12 0 pxr21 r22 0 py0 0 1 00 0 0 1

=

r11 r12 0 pxr21 r22 0 py0 0 1 00 0 0 1

c123 s123 0 c123L3 c12L2 c1L1+ +( )s123 c123 0 s123L3 s12L2 s1L1+ +( )

0 0 1 00 0 0 1

=

T 40 A1A2A3A4=




From the (2,1) and (1,1) elements we have:

The next equation is:

From the (1,4) and (2,4) elements:

These can be squared and added to yield:

123 2 r21 r11,( )atan=

T 40A41 A1A2A3=

r11 r12 0 r11L3 px+

r21 r22 0 r21L3 py+

0 0 1 00 0 0 1

c123 s123 0 c12L2 c1L1+

s123 c123 0 s12L2 s1L1+

0 0 1 00 0 0 1

=

r11 r12 0 r11L3 px+

r21 r22 0 r21L3 py+

0 0 1 00 0 0 1

c123 s123 0 c1 c2L2 L1+( ) s1 s2L2( )s123 c123 0 s1 c2L2 L1+( ) c1 s2L2( )+

0 0 1 00 0 0 1

=

k1 r11L3 px+ c12L2 c1L1+= =k2 r21L3 py+ s12L2 s1L1+= =

k12 k22+ L22 L12 2L2L1 c1c12 s1s12+( )+ +=k12 k22+ L22 L12 2L2L1c2+ +=




And this gives the angle as:

The result implies that there are two solutions for this anglewhich are symmetric about zero. These correspond to theelbow up and elbow down configurations. Now, before theexpressions were reduced to include a sum of angles, theywere:

With now known, these can be written as:

This is one of the standard forms that recur in inversekinematics problems for which the solution is:

Finally, the last angle is:

2

2k12 k22+( ) L22 L12+( )

2L2L1----------------------------------------------------acos=

k1 r11L3 px+ c1 c2L2 L1+( ) s1 s2L2( )= =k2 r21L3 py+ s1 c2L2 L1+( ) c1 s2L2( )+= =2

c1k3 s1k4 k1=s1k3 c1k4+ k2=

1 2atan k2k3 k1k4( ) k1k3 k2k4+( ),[ ]=

3 123 2 1=


2 Inverse Kinematics 152.4Standard Forms


2.4 Standard Forms There are a few forms of trigonometric equations that recur ismost mechanisms. The following set of solutions is sufficientfor most applications. One of the most difficult forms, solved by square and add,was presented in the example. In the following, the letters a, b, and c represent arbitraryknown expressions.

2.4.1 Explicit Tangent This form generates a single solution because both the sineand cosine are fixed in value. It can arise in the last two jointsof a three-axis wrist, for example. The equation:

has the trivial solution:

2.4.2 Point Symmetric Redundancy This form generates two solutions that are symmetric aboutthe origin. It can arise in the shoulder or hip joint, for example.The equation:

a cn=b sn=

n 2 b a,( )atan=

sna cnb 0=


2 Inverse Kinematics 162.4Standard Forms


can be solved by isolating the ratio of the two trig functions.There are two angles in one revolution which have the sametangent so the two solutions are:

2.4.3 Line Symmetric Redundancy Generates two solutions that are symmetric about an axisbecause the sine or cosine of the deviation from the axis mustbe constant. The sine case will be illustrated. It can arise in theshoulder or hip joint, or in an elbow or knee joint, for example.The equation:

can be solved by the trig substitution:

where:

This gives:

n 2 b a,( )atan=n 2 b a,( )atan=

sna cnb c=

a r ( )cos= b r ( )sin=

r sqrt a2 b2+( )= 2 b a,( )atan=

snc cns c r=s n( ) c r=


2 Inverse Kinematics 172.5Inverse DH Transform


So the cosine is:

This gives the result:

2.5 Inverse DH Transform The general inverse DH transform can be computed for asingle mechanism degree of freedom. This transform can be used as the basis of a completelygeneral inverse kinematic solution for robotic mechanisms. This solution can be considered to be the procedure forextracting a joint angle from a link coordinate frame.Proceeding as for a mechanism, the elements of the transformare assumed to be known:

The premultiplication set of equations will be used. The firstequation is:

c n( ) sqrt 1 c r( )2( )=

n

2 b a,( ) 2 c sqrt r2 c2( ),[ ]atanatan= ba

T ba


=


2 Inverse Kinematics 182.5Inverse DH Transform


The translational elements are trivial:



T ba Rotx i( )Trans ui 0 0, ,( )Rotz i( )Trans wi 0 0, ,( )=


ci s i 0 uicisi cici s i s iwisisi sici ci ciwi

0 0 0 1

=

ui px=

wi py2 pz

2+=

2 r 12 r11,( )atan=

2 r 23 r33,( )atan=


3 Differential Kinematics 193.1Derivatives of Fundamental Operators


3 Differential Kinematics Differential kinematics is the study of the derivatives ofkinematic models. These derivatives are called Jacobians andthey have many uses ranging from:

resolved rate control sensitivity analysis & uncertainty propagation static force transformation

3.1 Derivatives of Fundamental Operators The derivatives of the fundamental operators with respect totheir own parameters will be important. They can be used tocompute derivatives of very complex expressions by using thechain rule of differentiation. For reference, they are quotedbelow:

u Trans u v w, ,( )

1 0 0 10 1 0 00 0 1 00 0 0 1

=

v Trans u v w, ,( )

1 0 0 00 1 0 10 0 1 00 0 0 1

=

w Trans u v w, ,( )

1 0 0 00 1 0 00 0 1 10 0 0 1

=

Rotx ( )

1 0 0 00 s c 00 c s 00 0 0 1

=

Roty ( )

s 0 c 00 1 0 0c 0 s 00 0 0 1

=

Rotz ( )

s c 0 0c s 0 00 0 1 00 0 0 1

=


3 Differential Kinematics 203.2The Mechanism Jacobian


3.2 The Mechanism Jacobian It is natural to ask about the effect of a differential change injoint variables on the position and orientation of the end of themechanism. A frame matrix represents orientation indirectly in terms ofthree unit vectors, so an extra set of equations is required toextract three angles from the rotation matrix in order torepresent orientation. In terms of position, however, the last column of themechanism model gives the position of the end effector withrespect to the base. In general, let a mechanism have variables represented by thevector , and let the position and orientation, or pose, of theend of the mechanism be given by the vector . Then, the endeffector position is given by:

where the nonlinear multidimensional function comes fromthe mechan i sm mode l . The J acob i an ma t r i x i s amultidimensional derivative defined as:

qx

x F q( )=F

Jq

x

q F q( )( )

q jxi

q1x1

qnxn

q1xn

qnxn

= = = =


3 Differential Kinematics 213.3Singularity


The differential mapping from small changes in to thecorresponding small changes in is:

The Jacobian also gives velocity relationships via the chainrule of differentiation as follows:

which maps joint rates onto end effector velocity. Note that the Jacobian is nonlinear in the joint variables, butlinear in the joint rates. This implies that reducing the jointrates by half reduces the end velocity by exactly half andpreserves the direction.

3.3 Singularity Redundancy takes the form of singularity of the Jacobianmatrix in the differential kinematic solution. A mechanism canlose one or more degrees of freedom:

at points where two different inverse kinematic solutionsconverge when joint axes become aligned or parallel when the boundaries of the workspace are reached

Singularity implies that the Jacobian loses rank and is notinvertible. At the same time, the inverse kinematic solutiontends to fail because axes become aligned, and infinite rates canbe generated by rate control laws.

qx

dx Jdq=

tdd

xq

x td

d q =


3 Differential Kinematics 223.4Example: 3-Link Planar Manipulator


3.4 Example: 3-Link Planar Manipulator For the example manipulator, the last column of themanipulator model gives the following two equations:

which can be differentiated with respect to , , and inorder to determine the velocity of the end effector as the jointsmove. The solution is:

which can be written as:

3.5 Jacobian Determinant It is known from the implicit function theorem of calculusthat the ratio of differential volumes between the domain andrange of a multidimensional mapping is given by the Jacobiandeterminant. This quantity has applications to a technique ofnavigation and ranging called triangulation.

x c123L3 c12L2 c1L1+ +( )=y s123L3 s12L2 s1L1+ +( )=

1 2 3

x s123123L3 s1212L2 s11L1+ +( )=y c123123L3 c1212L2 c11L1+ +( )=

x

y

s123L3 s12L2 s1L1( ) s123L3 s12L2( ) s123L3c123L3 c12L2 c1L1+ +( ) c123L3 c12L2+( ) c123L3

121

=


3 Differential Kinematics 233.6Jacobian Tensor


Thus the product of the differentials forms a volume in bothconfiguration space and in task space. The relationshipbetween them is:

3.6 Jacobian Tensor At times it is convenient to compute the derivative of atransform matrix with respect to a vector of variables. In thiscase, the result is the derivative of a matrix with respect to avector. For example:

This is a three-dimensional cube of numbers which canloosely be called a tensor. The mechanism model itself is amatrix function of a vector. For example, if:

Then there are three slices of the tensor, each a matrix, givenby:

dx1dx2dxn( ) J dq1dq2dqm( )=

q T q( )[ ] qk

T ij=

T q( ) A1 q1( )A2 q2( )A3 q3( )=


3 Differential Kinematics 243.6Jacobian Tensor


q1T

q1A1A2A3=

q2T A1 q2

A2A3=

q3T A1A2 q3

A3=


4 Summary 253.6Jacobian Tensor


4 Summary We can model the forward kinematics of mechanisms by

embedding frames in rigid bodies employing the fundamantal orthogonal operator matrices employing a few rules for writing them in the right orderto represent a mechanism.

The DH convention consists of a special compoundorthogonal transform and a few rules for orienting the linkframes. These can be used just like the fundamental orthogonaloperator matrices to do forward kinematics. Inverse kinematics requires more skill and relies on rewritingthe forward equations oin an attempt to isolate unknowns. Various derivatives of kinemati transforms can be taken andeach has some use.


5 Notes 263.6Jacobian Tensor


5 Notes Do a model of the Dante leg. Do the equations to move the Dante body forward when thelegs are fixed. Show how moving one leg with fixed body issimiliar.

Kinematics of Mechanisms [26]

Documents