Vous êtes sur la page 1sur 39

1/39

Introduction Robotics
dr Dragan Kosti WTB Dynamics and Control September-October 2009

Introduction Robotics, lecture 3 of 7

2/39

Outline
Recapitulation Forward kinematics Inverse kinematics problem

3/39

Recapitulation

Introduction Robotics, lecture 3 of 7

4/39

Robot manipulators
Kinematic chain is series of links and joints.

SCARA geometry

Types of joints:
rotary (revolute, ) prismatic (translational, d).

5/39

6/39

Forward kinematics problem

Determine position and orientation of the end-effector as function of displacements in robot joints.

7/39

DH convention for homogenous transformations (1/2)

An arbitrary homogeneous transformation is based on 6 independent variables: 3 for rotation + 3 for translation. DH convention reduces 6 to 4, by specific choice of the coordinate frames. In DH convention, each homogeneous transformation has the form:

8/39

DH convention for homogenous transformations (2/2)

Position and orientation of coordinate frame i with respect to frame i-1 is specified by homogenous transformation matrix: xn q0 z0 qi+1 zn qi 0 n yn y0 i zi a i x0 x
i

di

where

zi-1 qi xi-1

9/39

Physical meaning of DH parameters

Link length ai is distance from zi-1 to zi measured along xi.
z q0 qi i ai di zi-1 qi xi-1 zi xi qi+1 n yn xn zn

0 Link twist i is angle between zi-1 and zi measured in plane normal 0 y0 x0 to xi (right-hand rule).

Link offset di is distance from origin of frame i-1 to the intersection xi with zi-1, measured along zi-1. Joint angle i is angle from xi-1 to xi measured in plane normal to zi-1 (right-hand rule).
Introduction Robotics, lecture 3 of 7

10/39

1. Assign zi to be the axis of actuation for joint i+1 (unless otherwise stated zn
coincides with zn-1). 2. Choose x0 and y0 so that the base frame is right-handed.

DH convention to assign coordinate frames

3. Iterative procedure for choosing oixiyizi depending on oi-1xi-1yi-1zi-1 (i=1, 2, , n-1): a) zi1 and zi are not coplanar; there is an unique shortest line segment from zi1 to zi, perpendicular to both; this line segment defines xi and the point where the line intersects zi is the origin oi; choose yi to form a right-handed frame, b) zi1 is parallel to zi; there are infinitely many common normals; choose xi as the normal passes through oi1; choose oi as the point at which this normal intersects zi; choose yi to form a right-handed frame, c) zi1 intersects zi; axis xi is chosen normal to the plane formed by zi and zi1; its positive direction is arbitrary; the most natural choice of oi is the intersection of zi and zi1, however, any point along the zi suffices; choose yi to form a right-handed frame.
Introduction Robotics, lecture 3 of 7

11/39

Forward Kinematics

12/39

Forward kinematics (1/2)

Homogenous transformation matrix relating the frame oixiyizi to
oi-1xi-1yi-1zi-1:

Ai specifies position and orientation of oixiyizi w.r.t. oi-1xi-1yi-1zi-1. Homogenous transformation matrix Tji expresses position and orientation of ojxjyjzj with respect to oixiyizi:

i Tj

= Ai +1 Ai +2 K A j 1 A j

13/39

Forward kinematics (2/2)

Forward kinematics of a serial manipulator with n joints can be represented by homogenous transformation matrix Hn0 which defines position and orientation of the end-effectors (tip) frame onxnynzn relative to the base coordinate frame o0x0y0z0:
0 Hn (q) = Tn0 (q) = A1 (q1 ) K An (qn ), 0 0 R ( q ) x 0 n n (q ) H n (q) = 0 1 31 031 = [0 0 0];

q = [q1 L qn ]
Introduction Robotics, lecture 3 of 7

14/39

Case-study: RRR robot manipulator

d3 x2 d2 1 d1 z0 x1 y2 elbow
q2

y3
-q3

x3 z3 a3

y1

z2 a2

shoulder y0

z1

1 - twist angle ai - link lenghts di - link offsets qi - displacements

waist
Introduction Robotics, lecture 3 of 7

q1

x0

15/39

16/39

Forward kinematics of RRR robot manipulator (1/2)

Coordinate frame o3x3y3z3 is related with the base frame o0x0y0z0 via homogenous transformation matrix:
T0 3 (q) = A1(q)A 2 (q)A 3 (q) =
0 R0 (q) x (q) 3 3 = 1 013

where
q = [ q1 q2 q3 ]T

x0 3 (q ) = [ x

z ]T

013 = [0 0 0]
Introduction Robotics, lecture 3 of 7

17/39

Forward kinematics of RRR robot manipulator (2/2)

Position of end-effector:
x = cosq1[a3cos( q2 + q3 ) + a2cosq2 ] + ( d 2 + d3 )sinq1
,

y = sin q1[a3cos( q2 + q3 ) + a2cosq2 ] ( d 2 + d3 )cosq1

z = a3sin( q2 + q3 ) + a2sinq2 + d1

Orientation of end-effector: cosq1cos( q2 + q3 ) cosq1sin( q2 + q3 ) sinq1 0 R3 = sinq1cos( q2 + q3 ) sinq1sin( q2 + q3 ) cosq1 cos( q2 + q3 ) 0 sin( q2 + q3 )
Introduction Robotics, lecture 3 of 7

18/39

Inverse Kinematics

19/39

Inverse kinematics problem

Inverse kinematics (IK): determine displacements in robot joints that correspond to given position and orientation of the end-effector.

20/39

Illustration: IK for planar RR manipulator (1/2)

Elbow down IK solution

21/39

Illustration: IK for planar RR manipulator (2/2)

Elbow up IK solution

22/39

The general IK problem (1/2)

Given a homogenous transformation matrix HSE(3)

find (multiple) solution(s) q1,,qn to equation

Here, H represents the desired position and orientation of the tip coordinate frame onxnynzn relative to coordinate frame o0x0y0z0 of the base; T0n is product of homogenous transformation matrices relating successive coordinate frames:
Introduction Robotics, lecture 3 of 7

23/39

The general IK problem (2/2)

Since the bottom rows of both T0n and H are equal to [0 0 0 1], equation

Here, Tij and Hij are nontrivial elements of T0n and H.

Introduction Robotics, lecture 3 of 7

24/39

Introduction Robotics, lecture 3 of 7

25/39

FK of Stanford manipulator

26/39

Example of IK solution for Stanford manipulator

Rotational equations (correspond to R06):

One solution:

Introduction Robotics, lecture 3 of 7

27/39

Nature of IK solutions
FK problem has always unique solution whereas IK problem may or may not have a solution; if IK solution exists, it may or may not be unique; solving IK equations, in general, is much too difficult. It is preferable to find IK solutions in closed-form:
faster computation (e.g. at sampling time of 1 [ms]), if multiple IK solutions exist, then closed-form allows us to develop rules for choosing a particular solution among several.

Existence of IK solutions depends on mathematical as well as engineering considerations. We assume that the given position and orientation is such that at least one IK solution exists.
Introduction Robotics, lecture 3 of 7

28/39

Kinematic decoupling (1/3)

General IK problem is difficult BUT for manipulators having 6 joints with the last 3 joint axes intersecting at one point, it is possible to decouple the general IK problem into two simpler problems: inverse position kinematics and inverse orientation kinematics. IK problem: for given R and o solve 9 rotational and 3 positional equations:

29/39

Kinematic decoupling (2/3)

Spherical wrist as paradigm.

Let oc be the intersection of the last 3 joint axes; as z3, z4, and z5 intersect at oc, the origins o4 and o5 will always be at oc; the motion of joints 4, 5 and 6 will not change the position of oc; only motions of joints 1, 2 and 3 can influence position of oc.
Introduction Robotics, lecture 3 of 7

30/39

Kinematic decoupling (3/3)

q1, q2, q3
Introduction Robotics, lecture 3 of 7

q4, q5, q6

31/39

Articulated manipulator: inverse position problem

Inverse tangent function Atan2(xc,yc) is defined for all (xc,yc)(0,0) and equals the unique angle 1 such that: xc yc cos 1 = , sin 1 = . 2 2 2 2 xc + yc xc + yc Introduction Robotics, lecture 3 of 7

32/39

33/39

Articulated manipulator: right arm configuration

*
Introduction Robotics, lecture 3 of 7

34/39

r

Law of cosines:

35/39

Articulated manipulator: IK solution for 2

2 1

2=1- 2
1=Atan2(r,s) 2=Atan2(a2+a3cos3,a3sin3)

36/39

Four IK solutions 1-3 for articulated manipulator

PUMA robot as an example of the articulated geometry.

37/39

Introduction Robotics, lecture 3 of 7

Equation to solve:

38/39

Articulated manipulator: IK solutions for 4 and 5

Equations given by the third column in :

If not both right-hand sides of the first two equations are zero:

39/39

Articulated manipulator: IK solutions for 6

The first two equations given by the last row in -s5c6 = s1r11 - c1r21 s5s6 = s1r12 - c1r22 From these equations it follows: :