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
Introduction Robotics, lecture 3 of 7 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).
Introduction Robotics, lecture 3 of 7 schematic representations of robot joints 5/39 Common geometries of robot manipulators
Introduction Robotics, lecture 3 of 7 6/39
Forward kinematics problem
• Determine position and orientation of the end-effector as function of displacements in robot joints.
Introduction Robotics, lecture 3 of 7 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:
Introduction Robotics, lecture 3 of 7 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:
q xn z 0 0 q i+1 zn qi ‘0’ n y0 αi ‘ ’ yn zi ai x0 xi
di z i-1 q where i xi-1
Introduction Robotics, lecture 3 of 7 9/39 Physical meaning of DH parameters
• Link length ai is distance from zi-1 to
zi measured along xi. q xn z 0 • Link twist α is angle between z 0 q i i-1 i+1 zn qi and z measured in plane normal ‘0’ n i y0 αi ‘ ’ yn zi ai to x (right -hand rule). x0 i xi
• Link offset di is distance from origin di z i-1 q of frame i-1 to the intersection x i i xi-1
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 DH convention to assign coordinate frames 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.
3. Iterative procedure for choosing oixiyizi depending on oi-1xi-1yi-1zi-1 (i=1, 2, …, n-1):
a) zi−1 and zi are not coplanar; there is an unique shortest line segment from zi−1 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) zi−1 is parallel to zi; there are infinitely many common normals; choose xi as the normal passes through oi−1 ; choose oi as the point at which this normal intersects zi; choose yi to form a right-handed frame,
c) zi−1 intersects zi; axis xi is chosen normal to the plane formed by zi and zi−1 ; it’s positive direction is arbitrary; the most natural choice of oi is the intersection of zi and zi−1 , 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
Introduction Robotics, lecture 3 of 7 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.
i • Homogenous transformation matrix Tj expresses position and
orientation of ojxjyjzj with respect to oixiyizi: i K T j = Ai+1Ai+2 Aj−1Aj
Introduction Robotics, lecture 3 of 7 13/39
Forward kinematics (2/2) • Forward kinematics of a serial manipulator with n joints can be 0 represented by homogenous transformation matrix Hn which defines position and orientation of the end-effector’s (tip)
frame onxnynzn relative to the base coordinate frame o0x0y0z0: 0 0 K H n (q) = Tn (q) = A1(q1) ⋅ ⋅ An (qn ), 0 0 0 Rn (q) xn (q) H n (q) = 03×1 1
03×1 = [0 0 0]; L T q = [q1 qn ] Introduction Robotics, lecture 3 of 7 14/39
Case-study: RRR robot manipulator
d3 x3 x2 y3 z3 y 2 -q 3 d a3 y 2 1 z2 x1 elbow α1 q 2 a2
z1 z0 shoulder d1 α1 - twist angle y 0 ai - link lenghts
q1 waist di - link offsets Introduction Robotics, lecture 3 of 7 x0 qi - displacements 15/39 DH parameters of RRR robot manipulator
Introduction Robotics, lecture 3 of 7 16/39
Forward kinematics of RRR robot manipulator (1/2)
• Coordinate frame o3x3y3z3 is related with the base frame o0x0y0z0 via homogenous transformation matrix: 0 T3(q) = A1(q)A 2(q)A 3(q) = R0(q) x0(q) = 3 3 01×3 1 where T 0 T q = [q1 q2 q3] x3(q) = [x y z]
01×3 = [0 0 0]
Introduction Robotics, lecture 3 of 7 17/39
Forward kinematics of RRR robot manipulator (2/2)
• Position of end-effector:
x = cos q1[a3cos( q2 + q3) + a2cos q2 ]+ (d2 + d3)sin q1
, y = sin q1[a3cos( q2 + q3) + a2cos q2 ]−(d2 + d3)cos q1
, z = a3sin( q2 + q3) + a2sin q2 + d1
• Orientation of end-effector:
cos q1cos( q2 + q3) − cos q1sin( q2 + q3) sin q1 0 R3 = sin q1cos( q2 + q3) − sin q1sin( q2 + q3) − cos q1 sin( q2 + q3) cos( q2 + q3) 0
Introduction Robotics, lecture 3 of 7 18/39
Inverse Kinematics
Introduction Robotics, lecture 3 of 7 19/39
Inverse kinematics problem • Inverse kinematics (IK): determine displacements in robot joints that correspond to given position and orientation of the end-effector.
Introduction Robotics, lecture 3 of 7 20/39
Illustration: IK for planar RR manipulator (1/2)
• Elbow down IK solution
Introduction Robotics, lecture 3 of 7 21/39
Illustration: IK for planar RR manipulator (2/2)
• Elbow up IK solution
Introduction Robotics, lecture 3 of 7 22/39 The general IK problem (1/2) • Given a homogenous transformation matrix H∈SE (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 0 of the base; T n is product of homogenous transformation matrices relating successive coordinate frames:
Introduction Robotics, lecture 3 of 7 23/39
The general IK problem (2/2)
0 • Since the bottom rows of both T n and H are equal to [0 0 0 1], equation
gives rise to 4 trivial equations and 12 equations in n unknowns
q1,…,qn:
0 Here, Tij and Hij are nontrivial elements of T n and H.
Introduction Robotics, lecture 3 of 7 24/39 Illustration: Stanford manipulator
Introduction Robotics, lecture 3 of 7 25/39
FK of Stanford manipulator
Introduction Robotics, lecture 3 of 7 26/39 Example of IK solution for Stanford manipulator 0 • Rotational equations (correspond to R 6):
0 • One solution: • Positional equations (correspond to o 6):
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:
Introduction Robotics, lecture 3 of 7 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 Atan 2( xc,yc) is defined for all (xc,yc)≠(0,0) and equals the unique angle θ such that: 1 x y cos θ = c , sin θ = c . 1 2 2 1 2 2 Introduction Robotics, lecture 3 of 7 xc + yc xc + yc 32/39 Articulated manipulator: left arm configuration
r
*
*
Introduction Robotics, lecture 3 of 7 33/39 Articulated manipulator: right arm configuration
*
r
* Introduction Robotics, lecture 3 of 7 +π 34/39
Articulated manipulator: IK solution for θθθ3
r
*
• Law of cosines:
Introduction Robotics, lecture 3 of 7 + elbow down; - elbow up 35/39
Articulated manipulator: IK solution for θθθ2
υυυ2 θ2=υ1- υ2 υυυ 1 υ1=Atan 2( r,s)
υ2=Atan 2( a2+a3cos θ3,a3sin θ3)
Introduction Robotics, lecture 3 of 7 36/39
Four IK solutions θθθ1-θθθ3 for articulated manipulator
• PUMA robot as an example of the articulated geometry.
Introduction Robotics, lecture 3 of 7 37/39 Articulated manipulator: inverse orientation problem
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:
±
• If positive square root is chosen in solution for θ5:
Introduction Robotics, lecture 3 of 7 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:
• Analogous approach if negative square root is chosen in
solution for θ5.
Introduction Robotics, lecture 3 of 7