Inverse Kinematics of Open Kinematic-Chains

The robot mechanism's output motion is usually at the end-effector link of a simple open kinematic-chain. If we know the joint variables and their velocities, it is simple to compute the robot mechanism's output motion because of the simple formulation of forward kinematics. Then, the question is: Why is there a need to study inverse kinematics?

( Tasks J

Motion Planning

& Programming

/< ^ ^ V - > T Inverse Kinematics ~~~-~W^^ ^ ^ X / Motion - y I 1 Y Desired Motion A

V in Task Space ; | 1 V^ in Joint s J

\ ^ ^ ^^y ^~— Forward Kinematics A \ ^ "^/

I i r

Robot Mechanism Robot Control

^ "~-~^^ ^r

( Executed Motion \^

V • T • c J~ Robot Dynamics

\ m Joint S p a c e / |

Fig. 3.20 The motion-related data flow inside a robot.

Let us examine the motion-related data flow inside a robot. From the illustration shown in Fig. 3.20, it is clear that the desired motion is specified in task space. The motion specification in task space needs to be mapped into joint space, as the direct input to robot control is the desired motion in joint space.

The purpose of robot control is to apply energy into the robot's mech-anism which will alter the dynamic behavior of the robot. The direct out-come of robot control is the executed motion in joint space, which is the robot mechanism's input motion. Through the robot's mechanism, the executed motion in joint space is physically manifested by the robot mech-anism's output motion in task space. Therefore, the robot mechmech-anism's

Mechanical System of Robots 87

inverse kinematics is crucial, it determines the ability of a robot to fulfill a task through the execution of motion in joint space. Accordingly, the objective of inverse kinematics is to compute the desired motions of the joint variables for a given motion specification of the end-effector's frame in task space.

3.3.3.1 Determination of Joint Displacements

Refer to Fig. 3.19. The input for the determination of joint displacements is the posture or configuration (position and orientation) of the end-effector's frame with respect to the base frame. That is,

(

^{ax sx nx tx\}

ay sy ny ty

az sz nz tz I 0 0 0 1 /

where a = {a^X)ay,a^z)¹, s = (sx^y^z)* and n = {n^x,n^y,n^zY are called the approaching vector, the sliding vector and the normal vector of the end-effector's frame, as shown in Fig. 3.19. For consistency of notation with the planar open kinematic-chain, we consider that a, s and ft coincide respectively with the X, Y and Z axes of frame n.

Now, what we want to determine is the set of joint angles q, which satisfies the following equation:

°Mn = f M i f o ) } • {lM2(q2)} . ... . {"-1Mn(gn)}. (3.38) Eq. 3.38 describes the equality of two matrices. It implies that their cor-responding elements must be equal to each other. In general, it is unlikely for us to derive a closed form solution from Eq. 3.38.

Example 3.17 Refer to Fig. 3.9 and Fig. 3.18. Frame 3 is the hand's frame for the right arm (or the foot's frame, in the case of the right leg).

Assume that, for a given task at a time-instant, the configuration of frame 3 needs to be with respect to frame 0. Now, the question is: What should the joint angles

(3.37)

(01,02,03) be?

Refer to Example 3.14 and Eq. 3.3. The three link matrices of the right arm are as follows:

(

C6i -SOi 0 JxC0i\

50! ce1 o i1sel

0 0 1 0 '

0 0 0 1 /

(

^{C02 - 5 02 0 Z2C02\}

50² C0² 0 1²S0²

0 0 1 0 '

0 0 0 1 /

/C03 - 5 03 o i3ce3\

2 j l. 50³ C0³ 0 1³S0³

Ms~ o o i o -V 0 0 0 1 /

(NOTE: CO = cos(0) and 50 = sin{6)).

Substituting °M\ and ^xMi into the equation

°M2 = { ° M1} . {1M2} yields

(C{e1 + e2) -5(0i + 02) o i2c(e1 + e2) + i1ce1\

On. 5(0x+ 02) C ( 0 i + 02) 0 Z25(0! + 02)+Zi50i

M 2 = o o i o

\ 0 0 0 1 / According to the frame assignment shown in Fig. 3.18, frame 2's origin, with respect to frame 0, can be expressed in terms of frame 3's origin with an offset of l³ along the X axis. That is,

°O2 = °O3 - l3 • {%}•

If we denote 0O2 = (a;2,y2,0)*, we can easily establish the following equation:

f i2c(e1 + 02) + i1cel = x2

\i2s{el + e2) + hS6l = y2 (6-6y)

Mechanical System of Robots 89

with

/ x2 = x3 - l3Cf3 12/2 = 2/3 - kS/3.

From Eq. 3.39, the computation of x\ + y\ yields 2lxl2C82=x\+yl-l\-l\.

Hence, the solution for the unknown 62 is

^

= ± a r C C O S

( 2 ^

²

J

-Two possible solutions for 62 mean that the mapping from task space to joint space is usually a one-to-many mapping (or conversely, the mapping from joint space to task space is usually a many-to-one mapping). Knowing 82, we can now rewrite Eq. 3.39 in a matrix form in terms of the unknown 8\ as follows:

fh+hCOi -hS02\ [C0i\ = fx2\ { i2se2 il + i2ce2)*{sej \y2) or

\sej~ k \ -i2se2 h + i2ce2 )\y2) with

k = {h + i2ce2f + {i2se2)2 = i\ + i22 + 2hhce2. If we know CQ\ and S#i, the solution for the unknown Q\ will be

&i = arctan ( —^- ) .

\CViJ

Analytically, from the equality of the corresponding elements at location (1, 1) of the matrices in the following equation:

0M3 = {°M1}»{lM2}»{2M3},

we can easily obtain the solution for the unknown 63 as follows:

t/3 — p — U\ — u².

oooooooooooooooooo

If we examine the solution for #2 and define 7 — IT ± #2 we have

72 , / 2 2 _ 2 COS(7) = COs(7T ± 02) = / l + \ . X 2 ^

The above expression is precisely the proof of the cosine theorem applied to the triangle formed by the origins of frames 0, 1 and 2. And angle 7 is the inner angle between the two edges corresponding to link 1 and link 2.

Example 3.18 Refer to Fig. 3.21. The posture of the world frame, with respect to the base frame (frame 0) of the right arm, is described by

(

0 - 1 0 8 0 \ 1 0 0 0

o o i o

-0 -0 -0 1 / Now, we want to move frame 3's origin to

J xw =29cm

\ y-w = 86cm

where the coordinates (x^w,y^w) are expressed in the world frame. The coordinates of this point, expressed in frame 0, will be

(33,03,0,1)*= °M^w»(x^w,y^w,O,l)^t or

f £3 = 80 - yw = - 6 c m

\y3 — x^w = 29cm.

By applying the inverse-kinematic solution discussed in the previous example, and choosing

j 13 = aictan ( g )

the computed joint angles of the right arm (as shown in Fig. 3.21) are r <9i = 44.3972°

I 62 = 114.5844°

[ 03 = -57.2922°.

Mechanical System of Robots 91

•I tf

2D Humanoid Robot in Posture of Seating

_j iAipNa(Arm} 100pi 111111 | ^ ^ S J T T T I T n T | 111111] 111111 IITTI11111 F| ^ W p h a M ^

—J iBeta (Arm) 70 ::::: fl|S|i^fflfBR:::::::::::::::::::::::::::: |8ete_C-fS)

Gamma(Arm).. 40 ffHH|||^MHij||tt : : : : : : : : : : : : : : : : : : : : : : : : : : : | iGatwna(Leg)

"* o c " -°20 0 20 40 60 80 L-J Bo , \C£

j [ Home | |- EM! | [»J

Fig. 3.21 Examples of an inverse-kinematic solution for t h e right a r m and right leg.

Similarly, the posture of the world frame, with respect to the base frame (frame 0) of the right leg, is described by

/ 0 - 1 0 40\

oM _ 1 0 0 0

Mw~ o o i o •

\0 0 0 1 /

For the posture of the right leg shown in Fig. 3.21, the origin of frame 3 of the right leg is

{

^{yxww — 22cm = 30cm}

expressed in the world frame. Its coordinates, with respect to the base frame (frame 0) are

f x3 = 40 - yw = 18cm

\ 2/3 = xw = 30cm.

If we choose

J f3 = arctan 0&) + 60

the computed joint angles of the right leg, as shown in Fig. 3.21, will be

{

0i = 74.2736°

62 = -81.0677°

03 = 125.8303°.

000000000000000000

For a simple open kinematic-chain with n+1 links, it is clearly difficult in practice to obtain a closed-form solution for determining the joint variables.

But, it is possible to derive a general numerical solution from knowledge of the velocities of the joint variables.

Let us denote q(t) the joint velocity vector. If q(t) follows a trajectory from an initial configuration (i.e. in joint space) to a desired final configu-ration within time interval [£»,£/], then the joint variables at time instant tf will be

q{tf) = q{U) + f ' q(t)dt. (3.40) Ju

If we perform the integration in a discrete time domain with the sam-pling step of At, Eq. 3.40 can numerically be computed as follows:

J V - 1

q(tf) = q{U) + J2 4(U + * • At) • At (3.41)

fe=0

with N = tf^i • If we denote tk = U + k» At, Eq. 3.41 can also be written in the following recursive form:

q{tk+l) = q{tk) + q{tk) • At, Mk £ [0, N - 1]. (3.42) In the next section, we will discuss in detail the numerical solution for the computation of inverse kinematics.

3.3.3.2 Determination of Joint Velocities

The dynamics of task execution is more appropriately illustrated by the behavior of a trajectory following, as shown in Fig. 3.22. In general, all motions intrinsically involve velocity. For a given task, the desired motion for the fulfillment of the task is normally described in the form of a trajec-tory which is a spatial curve with a time constraint. Thus, it is crucial to know how to determine the joint variables' velocities q if the end-effector

Mechanical System of Robots 93

frame's velocity vector P is given as input (NOTE: P is a vector which de-notes the posture or configuration of a frame). We will study how to plan the corresponding trajectory or spatial path for a given task in Chapter 9.

e ^

Fig. 3.22 Trajectory following by the end-effector's frame in task space.

A given task's planned spatial curve describes both the position and orientation of the tool's frame denoted by frame e, as shown in Fig. 3.22.

Let us assume that frame n has already been taken into account the offset between the frame of the last link in a simple open kinematic-chain and the frame assigned to a tool. In this way, the end-effector's frame is considered to coincide with the tool's frame. Thus, robotic task execution is accom-plished by the end-effector's frame which follows the planned trajectory represented by the time evolution of frame e.

Recall Eq. 3.36. The relationship between joint velocity vector q and velocity vector P of the end-effector's frame is as follows:

P = J(q) • q (3.43) with

P=C°

^:

). (3.44)

If P is a TO x 1 vector in a three-dimensional Cartesian space, the maxi-mum dimension of P will be six (i.e. TO = 6). The first three elements of P are the end-effector frame's linear velocity vector. The last three elements of P are the end-effector frame's angular velocity vector. If a simple open kinematic-chain has n+1 links (inclusive of the base link), the dimension of the Jacobian matrix J will be TO x n.

From Eq. 3.43, it is easy to see that the unknown q is over-constrained if 77i > n. In this case, there may not be any feasible solution. But there is one approximate solution exists based on an optimal estimation. In general, if TO > n, we can derive the solution for joint velocity vector q as follows:

• Step 1: Multiplication of the transpose of J to both sides of Eq. 3.44 yields

J^t*P = (J^tJ)»q. (3.45)

• Step 2: By multiplying the inverse of (./*</) to both sides of Eq. 3.45, we obtain

q = ji»P (3.46) with

jt = (jtjy1 • j \

Matrix j t is commonly called the pseudo-inverse of the Jacobian matrix J. It is clear that the pseudo-inverse does not exist if matrix J*J is not of full rank. In this case, the simple open kinematic-chain is said to be in a singular posture. Therefore, when determining the joint velocity vector, it is necessary to pay attention to whether or not the simple open kinematic-chain is in a singular posture.

Refer to Eq. 3.43 again. Joint velocity vector q is under-constrained if TO < n. In other words, the simple open kinematic-chain has more de-grees of freedom than necessary to configure the end-effector's frame to a desired posture. In this situation, the simple open kinematic-chain is said to be kinematically redundant. From a mathematical point of view, there is no unique solution for q if there is a kinematic redundancy (i.e. m < n).

In fact, the number of feasible solutions is infinite. This is a very advan-tageous situation in robotics because the redundancy allows us to impose constraints. In this way, optical solutions can be obtained by optimization.

For a given posture of the end-effector's frame, it is useful to find a solution among the infinite number of possible solutions that requires the

Mechanical System of Robots 95

least amount of energy. The minimum-energy solution is the one which guarantees the shortest path in joint space. In other words, it is the solution which minimizes the following cost function:

f(q) = \tf»4)- (3.47)

The minimum-energy solution for q can be derived by using Lagrangian multipliers as follows:

• Step 1: Considering the primary constraint expressed by Eq. 3.43, a new cost function using Lagrangian Multipliers is constructed as follows:

/(<?, A) = ~ (q* . q) + A* • (P - J . q) (3.48) where A is an m x 1 vector whose elements are called Lagrangian Mul-tipliers.

• Step 2: The solution for q that we are interested in must satisfy the following necessary conditions:

dq - u

(3.49)

• Step 3: Substituting Eq. 3.48 into Eq. 3.49 yields

{r/;.\ ^A : ⁰ ° <»•»)

• Step 4: From the first equation in Eq. 3.50, we have

q = J^t»X. (3.51)

Substituting it into the second equation in Eq. 3.50 yields P=(JmJt)mX

or

A= {J*J^t)-¹*P.

• Step 5: Finally, substituting A into Eq. 3.51 provides the solution for q as follows:

q = J^P (3.52)

with

J t = J * . ( J . J*)-l where J t is also called the pseudo-inverse of J.

Another useful constraint in robotics is called the internal-motion of a kinematically-redundant open kinematic-chain. The physical meaning of internal motion can mathematically be described as the difference between any two feasible solutions of q for a given input of P. If we denote q¹ as feasible solution 1 and q² as feasible solution 2, the corresponding internal motion vector will be

? = ?-?•

If qn is the minimum-energy solution of q, the sum of

«

ⁿ

+<r

is also a feasible solution of q which satisfies Eq. 3.43.

Since there are an infinite number of feasible solutions to q for a re-dundant open kinematic-chain, we have full freedom to impose an internal motion. However, it has to be clear that not any given velocity vector of q can be treated as an internal motion vector. Fortunately, there is an analytical method for us to map any given velocity vector of q to its corre-sponding internal motion vector. This powerful method can be described as follows:

For any given velocity vector q^a in joint (velocity) space, it is obvious that the sum of qn + qa is generally not a solution for q satisfying Eq. 3.43.

What we can hope to obtain is a feasible solution which is as close to q^a as possible. This requirement can easily be translated into a cost function as follows:

f{q) = \{{i-qa)t*(q-qa)}- (3-53) By applying Lagrangian multipliers, the solution satisfying Eq. 3.43 and Eq. 3.53 can be derived as follows:

• Step 1: Considering the primary constraint expressed by Eq. 3.43, a new cost function using Lagrangian Multipliers is constructed as follows:

M A) = \{{q - QaY • (q - qa)} + \* • (P - J • q) (3.54)

Mechanical System of Robots 97

where A is an m x 1 vector, the elements of which are called Lagrangian Multipliers.

• Step 2: The solution for q that we're interested in must satisfy the following conditions:

(91-0

\ dq ~ U

I (3.55)

9/ - n

• Step 3: Substituting Eq. 3.54 into Eq. 3.55 yields

H - *a-J t'A = 0 (3.56)

\P-J*q = 0. V '

• Step 4: Prom the first equation in Eq. 3.56, we have

q = q^a + J^l • A. (3.57) Substituting it into the second equation in Eq. 3.56 yields

P = J*q^a + {J* J*).A or

A = ( J . J^t)-¹»{P- J*q^a).

• Step 5: Finally, substituting A into Eq. 3.57 gives the solution for q, as follows:

q=ji»P+[I-ji»J]»q^a (3.58) with

Jt = J * , ( J . J ' ) "¹

where j t is again the pseudo-inverse of J, and / is an identity matrix.

Now, assume that the end-effector frame stands still. Since P = 0, Eq. 3.58 becomes

q=[I- J t , J]mq^a. (3.59) Eq. 3.59 physically means that the mapping of any given velocity vector in joint (velocity) space into its corresponding internal motion vector will

not cause any change to the end-effector's frame (i.e. J • q = 0). If we denote q^l the internal motion vector corresponding to q^a, we have

qi = [I - J t . J] . q°-_ (3.⁶0)

For a posture of a simple open kinematic chain:

/ Joint Space \ J ^ ^ f Task Space \

Fig. 3.23 Mapping from joint space to task space using the Jacobian J.

As illustrated in Fig. 3.23, all feasible velocity vectors P obtained from q by Eq. 3.43 are called the range of J, and denoted by R(J). The range of J is the reachable subspace of task (velocity) space from the mapping using Jacobian Matrix J. On the other hand, all the internal motion vectors in joint (velocity) space form a subspace which is called the Null space of J.

It is denoted by N(J). In terms of the null space of J, Eq. 3.60 means the mapping from any given velocity vector q^a into the null space. And, Jacobian Matrix J maps the null space into the zero-velocity vector in task (velocity) space.

How to advantageously explore kinematic redundancy depends on the applications. The general guideline for choosing an internal motion con-straint is simple. One common method is to align the internal motion vector in the direction that makes a simple open kinematic-chain either approach or move away from a particular configuration, such as: collision with obstacles, singularities, or joint limits. If we construct an objective function g(q) which measures the extent to which a simple open kinematic-chain is reaching a particular configuration, the internal motion vector can

Mechanical System of Robots 99

be chosen as

dq

where c is a coefficient which determines the magnitude of the internal mo-tion vector. If we choose the "+" sign in the above equamo-tion, this means that the simple open kinematic-chain should approach a desired configuration;

Otherwise, it means it should move away from a particular configuration.

Example 3.19 Fig. 3.24 shows the results of internal motions applied to a simple open kinematic-chain with 20 degrees of freedom. Notice the end-effector frame stands still after successive applications of internal motions on one, two, and four joint variables.

( a ) ( b )

(c) (d)

Fig. 3.24 Illustration of the effects of internal motion on a simple open kinematic-chain with 20 degrees of freedom: a) the initial posture, b) effect due to the internal motion of one joint variable, c) effect due to the internal motion of two joint variables, and d) effect due to the internal motion of four joint variables.

•••••••ooooooooooo

In summary, the joint velocity vector can be computed analytically from Eq. 3.46 or Eq. 3.52 for any given velocity vector of the end-effector's frame.

These solutions are crucial for velocity control in joint space. These solu-tions also serve as the computational basis for determining the joint dis-placements if the end-effector frame's posture or configuration is given as input.

3.3.3.3 Numerical Solution for Joint Displacements

Refer to Fig. 3.22. Assume that the initial posture or configuration of the end-effector's frame is at time instant £$, denoted by °Mn(ti). In order to determine joint displacements, the input will be the final posture or configuration of the end-effector's frame at time instant tf, which is denoted by °Mn(tf). What we want to compute are the joint angles at time instant tf (i.e. q(tf)).

The basic idea underlying the numerical solution for determining q(tf) is to compute the series of incremental joint displacements q(tk) (see Eq. 3.42).

These displacements correspond to a series of incremental motion transfor-mations eMe(tk,tk+i) of the end-effector's frame within a series of equal time intervals [tk,tk+i\. The origin of the end-effector's frame will travel along a spatial curve which connects the initial posture to the final posture.

Refer to Fig. 3.22 again. eMe(tk,tk+i) indicates the posture or config-uration of the end-effector's frame at t^+i with respect to itself at tk. The symbol "e" highlights the fact that the incremental motion transformations are obtained from any chosen spatial curve regardless of the geometrical property of the simple open kinematic-chain under consideration. If we know a desired series of incremental motion transformations along a chosen spatial curve, the desired final posture of the end-effector's frame will be

J V - l

(NOTE: At is the sampling step in time. In practice, we can choose (U,tf)

Mechanical System of Robots 101

to be (0,1). Then At is automatically computed once we decide the number N of iterations).

In a recursive form, Eq. 3.61 can equivalently be rewritten as follows:

°M^e(t^k+1) = {°M^e(t^k)}*{^eM^e(t^k},t^k+1), Vfce[0,JV-l]. (3.63) In fact, Eq. 3.63 is a discrete representation of a given spatial curve because one can choose eMe(tk, tfc+i) to be a constant matrix if the spatial curve is linearly interpolated with a series of piecewise segments.

Now, let Pe{tk) be the velocity vector of the end-effector's frame which executes the incremental motion transformation eMe(tk,tk+x) within time interval [tk,tk+i]. Substituting Eq. 3.58 into Eq. 3.42 yields

q(t^k+l)=q(t^k) + {ji.P^e(tk) + [I-J¹»J]*q^a}^t, Vfc e [O.JV-1]. (3.64) If eMe(tk,tk+i) is expressed as follows:

and we denote (A9, ^ef^e) the equivalent rotation angle and equivalent ro-tation axis of eRe(tk,tk-\-i), the velocity vector Pe{tk) of the end-effector's frame at tk will be

Pe(tk) = (°o0n) (3-66) with

(°dn = ±* {°Rn(tk)} • {eTe(tk,tk+1)}

\ V = If . {°Rn(tk)} . {«fe}

where °Rn(tk) is the rotation matrix which represents the orientation of the end-effector's frame at tk with respect to frame 0 (base frame).

Eq. 3.64 is the recursive algorithm for the computation of the joint variables if the end-effector frame's final posture is given as input. The accuracy of the computation depends on the scale of the time interval At.

In order to avoid an accumulation of numerical errors, a good strategy is to replace °M^e(t^k) in Eq. 3.62 with the computed °Mⁿ(t^k) from the solution of forward kinematics. That is,

°Mn(if c)= °Mn(q(tk)) (3.67)

(3.65)

where q(tk) is the actually computed joint angle vector from Eq. 3.64. This leads to the use of

eMe(tk,tk+1) = f M ^ f c ) } "1 •{°Me(tk+1)} (3.68) to replace Eq. 3.65 for the computation of P^e(t^k).

Example 3.20 Fig. 3.25 is an example of the use of a numerical solution to solve inverse kinematics. The arm manipulator has 12 degrees of freedom.

At time U, the robot is at its initial posture and the end-effector frame's final posture at time tf is given as input. When applying the numerical solution to iteratively solve the inverse kinematics, the end-effector's frame reaches its desired final posture after 50 iterations (i.e. N = 50).

( a) Initial posture ( b ) Final posture

Fig. 3.25 Example of the use of a numerical solution to solve inverse kinematics: a)

Fig. 3.25 Example of the use of a numerical solution to solve inverse kinematics: a)

Dans le document ROBOTICS LINKING PERCEPTION TO ACTION (Page 111-129)