![Page 1: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/1.jpg)
1
Robot Kinematics II
Introduction to ROBOTICS
![Page 2: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/2.jpg)
2
Outline
• Review– Manipulator Specifications
• Precision, Repeatability– Homogeneous Matrix
• Denavit-Hartenberg (D-H) Representation
• Kinematics Equations• Inverse Kinematics
![Page 3: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/3.jpg)
3
Review
• Manipulator, Robot arms, Industrial robot – A chain of rigid bodies (links) connected by
joints (revolute or prismatic)• Manipulator Specification
– DOF, Redundant Robot– Workspace, Payload – Precision– Repeatability
How accurately a specified point can be reached
How accurately the same position can be reached if the motion is repeated many times
![Page 4: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/4.jpg)
4
Review• Manipulators:
Cartesian: PPP Cylindrical: RPP Spherical: RRP
SCARA: RRP(Selective Compliance Assembly Robot Arm)
Articulated: RRR
Hand coordinate:n: normal vector; s: sliding vector;
a: approach vector, normal to the
tool mounting plate
![Page 5: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/5.jpg)
5
Review• Basic Rotation Matrix
uvwxyz RPP =
xyzuvw QPP =
TRRQ == −1
uvw
w
v
u
z
y
x
xyz RPppp
ppp
P =⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
⋅⋅⋅⋅⋅⋅⋅⋅⋅
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=
wzvzuz
wyvyuy
wxvxux
kkjkikkjjjijkijiii
x
z
y
v
wP
u
![Page 6: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/6.jpg)
6
Basic Rotation Matrices– Rotation about x-axis with
– Rotation about y-axis with
– Rotation about z-axis with
uvwxyz RPP =
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=θθθθθ
CSSCxRot
00
001),(
0
0100
),(⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−=
θθ
θθθ
CS
SCyRot
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡ −=
10000
),( θθθθ
θ CSSC
zRot
θ
θ
θ
![Page 7: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/7.jpg)
7
Review• Coordinate transformation from {B} to {A}
• Homogeneous transformation matrix
'oAPBB
APA rrRr +=
⎥⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡
× 1101 31
' PBoAB
APA rrRr
⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡= ××
× 10101333
31
' PRrRT
oAB
A
BA
Position vector
Rotation matrix
Scaling
![Page 8: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/8.jpg)
8
Review• Homogeneous Transformation
– Special cases1. Translation
2. Rotation
⎥⎦
⎤⎢⎣
⎡=
×
×
100
31
13BA
BA RT
⎥⎦
⎤⎢⎣
⎡=
×
×
10 31
'33
oA
BA rIT
![Page 9: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/9.jpg)
9
Review
• Composite Homogeneous Transformation Matrix
• Rules:– Transformation (rotation/translation) w.r.t. (X,Y,Z)
(OLD FRAME), using pre-multiplication– Transformation (rotation/translation) w.r.t.
(U,V,W) (NEW FRAME), using post-multiplication
![Page 10: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/10.jpg)
10
Review• Homogeneous Representation
– A point in space
– A frame in space
3R
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=⎥⎦
⎤⎢⎣
⎡=
10001000 zzzz
yyyy
xxxx
pasnpasnpasn
PasnF
x
y
z ),,( zyx pppP
ns
a⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1z
y
x
ppp
P Homogeneous coordinate of P w.r.t. OXYZ
3R
![Page 11: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/11.jpg)
11
Review• Orientation Representation
(Euler Angles) – Description of Yaw, Pitch, Roll
• A rotation of about the OX axis ( ) -- yaw
• A rotation of about the OY axis ( ) -- pitch
• A rotation of about the OZ axis ( ) -- roll
X
Y
Z
ψθ
φψ
θ
φ
ψ,xR
θ,yR
φ,zR
yaw
pitch
roll
![Page 12: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/12.jpg)
12
Quiz 1• How to get the resultant rotation matrix for YPR?
X
Y
Z
ψθ
φ
ψθφ ,,, xyz RRRT =
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
100001000000
φφφφ
CSSC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−100000001000
θθ
θθ
CS
SC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
100000000001
ψψψψ
CSSC
![Page 13: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/13.jpg)
13
Quiz 2• Geometric Interpretation?
•
⎥⎦
⎤⎢⎣
⎡= ××
101333 PR
T
⎥⎦
⎤⎢⎣
⎡ −=−
101 PRR
TTT
441
100
1010 ×− =⎥
⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡ −= I
RRPRPRRTT
TTT
Position of the origin of OUVW coordinate frame w.r.t. OXYZ frame
Inverse of the rotation submatrixis equivalent to its transposePosition of the origin of OXYZ reference frame w.r.t. OUVW frame
Orientation of OUVW coordinate frame w.r.t. OXYZ frame
Inverse Homogeneous Matrix?
![Page 14: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/14.jpg)
14
Kinematics Model• Forward (direct) Kinematics
• Inverse Kinematics
),,( 21 nqqqq L=
),,,,,( ψθφzyxY =
x
y
z
Direct Kinematics
Inverse Kinematics
Position and Orientationof the end-effector
Jointvariables
![Page 15: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/15.jpg)
15
Robot Links and Joints
![Page 16: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/16.jpg)
16
Denavit-Hartenberg Convention• Number the joints from 1 to n starting with the base and ending with
the end-effector. • Establish the base coordinate system. Establish a right-handed
orthonormal coordinate system at the supporting base with axis lying along the axis of motion of joint 1.
• Establish joint axis. Align the Zi with the axis of motion (rotary or sliding) of joint i+1.
• Establish the origin of the ith coordinate system. Locate the origin of the ith coordinate at the intersection of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Ziaxis.
• Establish Xi axis. Establish or along the common normal between the Zi-1 & Zi axes when they are parallel.
• Establish Yi axis. Assign to complete the right-handed coordinate system.
• Find the link and joint parameters
),,( 000 ZYX
iiiii ZZZZX ××±= −− 11 /)(
iiiii XZXZY ××+= /)(
0Z
![Page 17: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/17.jpg)
17
Example I• 3 Revolute Joints
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
Link 1 Link 2
![Page 18: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/18.jpg)
18
Link Coordinate Frames• Assign Link Coordinate Frames:
– To describe the geometry of robot motion, we assign a Cartesian coordinate frame (Oi, Xi,Yi,Zi) to each link, as follows:
• establish a right-handed orthonormal coordinate frame O0 at the supporting base with Z0 lying along joint 1 motion axis.
• the Zi axis is directed along the axis of motion of joint (i + 1), that is, link (i + 1) rotates about or translates along Zi;
Link 1 Link 2
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
![Page 19: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/19.jpg)
19
Link Coordinate Frames– Locate the origin of the ith coordinate at the intersection
of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Zi axis.
– the Xi axis lies along the common normal from the Zi-1axis to the Zi axis , (if Zi-1 is parallel to Zi, then Xi is specified arbitrarily, subject only to Xi being perpendicular to Zi);
iiiii ZZZZX ××±= −− 11 /)(
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
![Page 20: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/20.jpg)
20
Link Coordinate Frames– Assign to complete the right-
handed coordinate system.• The hand coordinate frame is specified by the geometry
of the end-effector. Normally, establish Zn along the direction of Zn-1 axis and pointing away from the robot; establish Xn such that it is normal to both Zn-1 and Zn axes. Assign Yn to complete the right-handed coordinate system.
iiiii XZXZY ××+= /)(
nO
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
![Page 21: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/21.jpg)
21
Link and Joint Parameters• Joint angle : the angle of rotation from the Xi-1 axis to
the Xi axis about the Zi-1 axis. It is the joint variable if joint i is rotary.
• Joint distance : the distance from the origin of the (i-1) coordinate system to the intersection of the Zi-1 axis and the Xi axis along the Zi-1 axis. It is the joint variable if joint i is prismatic.
• Link length : the distance from the intersection of the Zi-1axis and the Xi axis to the origin of the ith coordinate system along the Xi axis.
• Link twist angle : the angle of rotation from the Zi-1 axis to the Zi axis about the Xi axis.
iθ
id
ia
iα
![Page 22: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/22.jpg)
22
Example I
Joint i αi ai di θi
1 0 a0 0 θ0
2 -90 a1 0 θ1
3 0 0 d2 θ2
D-H Link Parameter Table
: rotation angle from Xi-1 to Xi about Zi-1iθ: distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1
: distance from intersection of Zi-1 & Xito origin of i coordinate along Xi
id
: rotation angle from Zi-1 to Zi about Xi
iaiα
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
![Page 23: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/23.jpg)
23
Example II: PUMA 260
iiiii ZZZZX ××±= −− 11 /)(
iiiii XZXZY ××+= /)(
1θ2θ
3θ
4θ
5θ6θ
0Z
1Z
2Z
3Z
4Z5Z
1O
2O3O
5O4O
6O
1X1Y
2X2Y
3X
3Y
4X
4Y5X
5Y6X
6Y
6Z
1. Number the joints
2. Establish base frame
3. Establish joint axis Zi
4. Locate origin, (intersect. of Zi & Zi-1) OR (intersect of common normal & Zi )
5. Establish Xi,Yi
PUMA 260
t
![Page 24: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/24.jpg)
24
Link Parameters
1θ2θ
3θ
4θ
5θ6θ
0Z
1Z
2Z
3Z
4Z5Z
1O
2O3O
5O4O
6O
1X1Y
2X2Y
3X
3Y
4X
4Y5X
5Y6X
6Y
6Z
: angle from Zi-1 to Ziabout Xi
: distance from intersectionof Zi-1 & Xi to Oi along Xi
Joint distance : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1
: angle from Xi-1 to Xiabout Zi-1
iθ
iα
ia
id
t0060090580-904
00903802
130-901J iθ
1θ
4θ
2θ3θ
6θ5θ
iα ia id
-l
![Page 25: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/25.jpg)
25
Transformation between i-1 and i• Four successive elementary transformations
are required to relate the i-th coordinate frame to the (i-1)-th coordinate frame:– Rotate about the Z i-1 axis an angle of θi to align the
X i-1 axis with the X i axis.– Translate along the Z i-1 axis a distance of di, to bring
Xi-1 and Xi axes into coincidence.– Translate along the Xi axis a distance of ai to bring
the two origins Oi-1 and Oi as well as the X axis into coincidence.
– Rotate about the Xi axis an angle of αi ( in the right-handed sense), to bring the two coordinates into coincidence.
![Page 26: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/26.jpg)
26
Transformation between i-1 and i• D-H transformation matrix for adjacent coordinate
frames, i and i-1.– The position and orientation of the i-th frame coordinate
can be expressed in the (i-1)th frame by the following homogeneous transformation matrix:
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−
= −−−
10000
),(),(),(),( 111
iii
iiiiiii
iiiiiii
iiiiiiiii
i
dCSSaCSCCSCaSSSCC
xRaxTzRdzTT
ααθθαθαθθθαθαθ
αθ
Source coordinate
ReferenceCoordinate
![Page 27: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/27.jpg)
27
Kinematic Equations • Forward Kinematics
– Given joint variables– End-effector position & orientation
• Homogeneous matrix – specifies the location of the ith coordinate frame w.r.t.
the base coordinate system– chain product of successive coordinate transformation
matrices of
⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡=
= −
100010000
12
11
00
nnn
nn
n
PasnPR
TTTT K
),,( 21 nqqqq L=
),,,,,( ψθφzyxY =
iiT 1−
nT0
Orientation matrix
Position vector
![Page 28: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/28.jpg)
28
Kinematics Equations• Other representations
– reference from, tool frame
– Yaw-Pitch-Roll representation for orientation
tooln
nref
toolref HTBT 0
0=
ψθφ ,,, xyz RRRT =
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
100001000000
φφφφ
CSSC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−100000001000
θθ
θθ
CS
SC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
100000000001
ψψψψ
CSSC
![Page 29: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/29.jpg)
29
Solving forward kinematics
• Forward kinematics
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
⇒
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
ϕθφ
θθθθθθ
z
y
x
p
pp
6
5
4
3
2
1
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1000zzzz
yyyy
xxxx
pasnpasnpasn
T
• Transformation Matrix
![Page 30: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/30.jpg)
30
Solving forward kinematics• Yaw-Pitch-Roll representation for orientation
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−−++−
=
1000
0z
y
x
n
pCCSCSpSCCSSCCSSSCSpSSCSCCSSSCCC
Tψθψθθ
ψφψθφψφψθφθφψφψθφψφψθφϑφ
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1000
0zzzz
yyyy
xxxx
n
pasnpasnpasn
T
)(sin 1zn−= −θ
)cos
(cos 1
θψ za−=
)cos
(cos 1
θφ xn−=
Problem? Solution is inconsistent and ill-conditioned!!
![Page 31: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/31.jpg)
31
atan2(y,x)
x
y
⎪⎪⎩
⎪⎪⎨
⎧
−+≤≤−−−−≤≤−
+−≤≤++≤≤
==
yandxforyandxfor
yandxforyandxfor
xya
oo
oo
oo
oo
09090180
18090900
),(2tan
θθθθ
θ
![Page 32: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/32.jpg)
32
Yaw-Pitch-Roll Representationψθφ ,,, xyz RRRT =
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
100001000000
φφφφ
CSSC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−100000001000
θθ
θθ
CS
SC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
100000000001
ψψψψ
CSSC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1000000
zzz
yyy
xxx
asnasnasn
![Page 33: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/33.jpg)
33
Yaw-Pitch-Roll Representation
ψθφ ,,1, xyz RRTR =−
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
100001000000
φφφφ
CSSC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−=
100000001000
θθ
θθ
CS
SC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
100000000001
ψψψψ
CSSC
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
1000000
zzz
yyy
xxx
asnasnasn
(Equation A)
![Page 34: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/34.jpg)
34
Yaw-Pitch-Roll Representation
0cossin =⋅+⋅− yx nn φφ
⎩⎨⎧
−==⋅+⋅
θθφφ
sincossincos
z
yx
nnn
⎩⎨⎧
−=⋅+⋅−=⋅+⋅−
ψφφψφφ
sincossincoscossin
yx
yx
aass
• Compare LHS and RHS of Equation A, we have:
),(2tan xy nna=φ
)sincos,(2tan yzz nnna ⋅+⋅−= φφθ
)cossin,cos(sin2tan yxyx ssaaa ⋅+⋅−⋅−⋅= φφφφψ
![Page 35: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/35.jpg)
35
Kinematic Model
• Steps to derive kinematics model:– Assign D-H coordinates frames– Find link parameters– Transformation matrices of adjacent joints– Calculate Kinematics Matrix– When necessary, Euler angle representation
![Page 36: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/36.jpg)
36
Example
Joint i αi ai di θi
1 0 a0 0 θ0
2 -90 a1 0 θ1
3 0 0 d2 θ2
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
![Page 37: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/37.jpg)
37
ExampleJoint i αi ai di θi
1 0 a0 0 θ0
2 -90 a1 0 θ1
3 0 0 d2 θ2
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
10000100
0cosθsinθ0sinθcosθ
00
00
000
00
1 sincos
θθ
aa
T
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−
−
=
1000000
sinθcosθ
1
1
1 1sincos0cossin0
111
111
2 θθθθ
aa
T
))()(( 2103213
0 TTTT =⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
1000
0sinθcosθ 22
22
223
10000cossin0
dT
θθ
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−
=−
100001
iii
iiiiiii
iiiiiii
ii dCS
SaCSCCSCaSSSCC
Tαα
θθαθαθθθαθαθ
![Page 38: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/38.jpg)
38
Example: Puma 560
![Page 39: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/39.jpg)
39
Example: Puma 560
![Page 40: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/40.jpg)
40
Link Coordinate Parameters
Joint i θi αi ai(mm) di(mm)
1 θ1 -90 0 0 2 θ2 0 431.8 149.09
3 θ3 90 -20.32 0
4 θ4 -90 0 433.075 θ5 90 0 0 6 θ6 0 0 56.25
PUMA 560 robot arm link coordinate parameters
![Page 41: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/41.jpg)
41
Example: Puma 560
![Page 42: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/42.jpg)
42
Example: Puma 560
![Page 43: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/43.jpg)
43
Inverse Kinematics• Given a desired position (P)
& orientation (R) of the end-effector
• Find the joint variables which can bring the robot the desired configuration
),,( 21 nqqqq L=
x
y
z
![Page 44: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/44.jpg)
44
Inverse Kinematics• More difficult
– Systematic closed-form solution in general is not available
– Solution not unique• Redundant robot• Elbow-up/elbow-down
configuration – Robot dependent
(x , y)l2
l1l2
l1
![Page 45: Robot Kinematics IIw3.uch.edu.tw/control/download/3動力學II.pdf · 21 Link and Joint Parameters • Joint angle: the angle of rotation from the Xi-1 axis to the X i axis about](https://reader033.vdocuments.us/reader033/viewer/2022041802/5e52038c4cb618166437b596/html5/thumbnails/45.jpg)
45
Inverse Kinematics
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
6
5
4
3
2
1
θθθθθθ
65
54
43
32
21
10
1000
TTTTTTpasnpasnpasn
Tzzzz
yyyy
xxxx
=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
• Transformation Matrix
Special cases make the closed-form arm solution possible:
1. Three adjacent joint axes intersecting (PUMA, Stanford)
2. Three adjacent joint axes parallel to one another (MINIMOVER)