robotics: forward and inverse kinematics
Post on 02-Jul-2015
10.874 Views
Preview:
TRANSCRIPT
Forward Kinematics
“Finding the end effector given the joint angles”
Types of robot joints
• Rotary– Angle θ about z axis
• Prismatic/sliding– Linear displacement d along axis of joint z
• Hooke (2-D Rotary)– 2-D of freedom described by Yaw and Pitch
• Spherical– Described by 3 rotation axes (wrist & shoulder)
Robot configurations
• Cartesian(inspection)– 3 Prismatic joints
• SCARA (pick and place)– Min 3 axes (Z axis and 2+ rotary joints)
• Spherical Wrist– Most common type 6-D freedom – hooke shoulder, rotary elbow and roll-pitch-roll wrist
• …
Setting up the coordinate system:Denavit-Hartenberg Coordinates
• Key basis for DH coordinates– There is only one normal between two lines in
space– Except: Parallel lines and intersecting lines
(common normal has zero length)
– Suppose there are two joint axes zi and zi-1
ii
iiin zz
zz
××=
−
−
1
1
• DH coordinates are numbered relative to the base 0
• Joint i connects link i-1 to link i
• The intersection of ni with zi defines the origin of the link
• We take zi to be parallel to the ni
• The coordinate system for link i is at the distal end
• The coordinate system at joint i is the i-1 system
∀ αI is the skew angle from zi-1 to zi measured about xi
• di is the distance from xi-1 to xi measured along zi-1
∀ θi is the angle from xi-1 to xi measured about zi-1
xi-2
zi-1
xi-1 xi
zi-2
zi
Joint i
Link i
Link i-1
ai
zi
zi-1
xi, ni
Parallel or intersecting axes
• When neighbouring axes intersect ai =0 ⇒ni=0
– Arbitrary choice xi ni or –ni
• For Parallel axes– Chose xi that intersects xi-1 at Oi-1 (prefered)
– Chose xi that intersects xi+1 at Oi+1
• In these cases DH parameters are very sensitive to small changes in alignment – An alternative is the Hayati coordinates
Examples
• 2- Planar Manipulator– Axes at distal ends– Avoid transformation
to find endpoint
– Make x2 the approach vector
θ2
θ1
a1
a2
O2
O1
O0
x1
x0
x2
y1
y2
y0
SCARA
• Note for joint 3 displacement is the joint variable and θ is constant
• Location of z2 is arbtrary
• Location of O3 and x3 are arbitrary, however O3 determines d3
• For simplicity frame 4 is placed at the gripper and z2 , z3 and z4 coincident
z0
x0
z1
x1x2
z2
x3
x4
z3 z4
i ai di αi θi
1 a1 0 0 θ1
2 a2 0 π θ2
3 0 d3 0 04 0 d3 0 θ4
Forward Kinematics
• Link Coordinate Transform– From link i to link i-1of the 2-d planar robot
• Rotate about the zi-1 axis by θi , then about the xi axis by αi.
( ) ( )
−
−=
−
−==−
ii
iiiii
iiiii
ii
iiii
ii
ixzii
cs
scccs
sscsc
cs
sccs
sc
RRR
αααθαθθ
αθαθθ
ααααθθ
θθαθ
00
0
001
100
0
01
Forward Kinematics• Translational component referenced to axes i-1
• Homogeneous transform i-1Ti from frame i to frame i-1
=
+
=+=+= −
−−−
−−
−−
i
ii
ii
i
i
iiiii
iii
iii
iii
iiii
i
d
sa
ca
s
c
adRadad θθ
θθ
01
0
01
111
11
,11 xzxzd
−
−
=
= −
−−−
1000
010,1
111
iii
iiiiiii
iiiiiii
T
iii
ii
ii
dcs
sascccs
casscsc
dRT
ααθαθαθθθαθαθθ
Relating any two link frames
• By composition of intervening frames we can link any two frames
• For efficiency we normally decompose the frames into separate Rotations and Translation components
ji 11
22
11 <= −
−−
++
+ jj
jj
ii
ii
ji TTTTT
Forward Kinematic computations
• Aim: find the position and orientation of the last frame, n, wrt the base frame 0
• Sometimes additional frames are added at the beginning or end– Vision systems– Tools or object which are picked up
• Once an object is grasped the its kinematics are constant and for practical purposes can be considered part of the last link.
Kinematic Calibration
• Set during Manufacture• Wear, error, etc… • Kinematic callibration schemes
– Measurement– Experimental
• Tool Transform– Most objects geometric and known– Vision system
Inverse Kinmatics (IK)
“Given a goal position find the joint angles for the robot arm”
Inverse Kinematics
• IK generally harder than FK
• Sometimes no analytical solution
• Sometimes multiple solutions– Redundant manipulators
• Sometimes no solution– Outside workspace
• 2-D planar manipulator (again)• Solve for θ2
• Two solutions: elbow up & elbow down
θ2
θ1
a1
a2
O2
O1
O0
x1
x0
x2
y1
y2
y0
φ
ψ
(x,y)
θ2
( ) ( )( ) ( )
( ) ( )( ) ( )22
221
22
22222
211
2
222
21
22
22222
21
22
21
2221
22
21
222122
21
22
21
22
2
22122
21
22
tan2
2
2
cos1
cos1
2tan
accuracygreater for
2cos
)cos(2
aayx
yxaa
aayx
yxaa
aayxaa
aayxaa
aa
aayx
aaaayx
−−++−+±=
−−++−+=
−−++++−−=
+−=
−−+=
−−+=+
−θ
θθθ
θ
θπ
• Given θ2 find θ1
• Note that there are two answers for θ1 based on elbow up or down
• 3-DOF of freedom robot arms– Most robots are made of two interconnected 3-DOF
arms• Elbow joint (position wrist in space)
• Wrist joint (orient object/tool in space)
)cos,sin(2tan
),(2tan
22122
1
θθψφ
ψφθ
aaaa
xya
+==
−=
Workspaces
Workspace limitations depend on•Joint limits•Presence of obstacles
Holes: doughnut shaped WSVoids: empty space in WS
Total or Reachable WorkspacePrimary WS: points reachable in all OrientationsSecondary WS: total -primary
Trajectory Planning
Path Planning
Trajectory planning
• Start to Goal avoiding obstacles along the way• Joint space easiest because no IK
– But end effector pose is not controlled
• Cartesian space planning is easier but IK must be solved
Joint Space Trajectories
• Cubic Trajectories– 4 coefficients– Satisfy position and velocity constraits
– For a joint variable qi
( ) ( )( ) ( )
velocityandposition final theare and
velocityandposition initial theare and
timeend theis start time theis
11
00
0
100
100
tt
qtqqtq
qtqqtq
f
fii
fii
′′
′=′=
==
• Polynomials for joint position and velocity
• The ai coeff. have to be related to the end point constraits
( )( ) 2
321
33
2210
32 tataatq
tatataatq
i
i
++=
+++=
( )( )( ) 1
2321
13
32
210
01000
32
0 ,)0( ,0
qtataatq
qtatataatq
qaqqaqt
fffi
ffffi
ii
′=++=
=+++=
′=====
• This allows us to determine a2 and a3
• Example
( ) ( )
( ) ( )3
10103
2
10012
2
23
f
f
f
f
t
tqqqqa
t
tqqqqa
′+′+−=
′+′−−=
60 ,90 ,0 ,10
1 deg/sec, 0 ,20 ,10
3210
1010
=−===⇒
==′=′−==
aaaa
tqqqq foo
0 100-20
10
Linear segments with parabolic bends
• We want the middle part of the trajectory to have a constant velocity V– Ramp up– Linear segment – Ramp down
Ramp Up
• A quadratic requires 3 constraits – 2 for the start and 1 for const velocity at the end
( )( )
( )( ) tatq
taqtq
aqaqa
taatq
tataatq
i
i
i
i
2
220
20100
21
2210
2
stagenext at found , ,
0 velocity and 0with t
2
=+=
====
+=++=
Linear Section
• Given constant velocity V, which we ramp up to in a unknown time tb, we can find a2 in terms of tb
( )
( )
V
Vtqqt
VtVtqq
tt
Vq
Vtqqt
ttttVtttq
t
VaVtatq
fb
bf
bb
ff
bfbi
bbi
+−=
+−+
=+
+=+=
−≤≤+=+=
===
10
1020b
010
i
010
22
22 tat time
2
22q :symmeteryby
:segmentLinear
2 ,2 :up ramp of end From
α
ααα
• Similarly the end position can be show to be
( )
≤≤+++
≤≤+=⇒==
t-tt t2
tt0 2
1
Let
bfb10
b2
0
VtVtqq
atqtq
t
Vqa
fi
bi
( )( ) ( ) 0 and :where
22
1
221
==
−+−=
fifi
ffi
tqqtq
ta
tatta
qtq
top related