Professional Documents
Culture Documents
Robotics: Takafumi Matsumaru
Robotics: Takafumi Matsumaru
Robotics: Takafumi Matsumaru
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Coordinate transformation Coordinate transformation
at rotation joint (1/4) at rotation joint (2/4)
θi Zi =Zi-1’ # Coordinate transformation at Z-axis rotation joint
Xi-1’ (relationship between coordinate Oi and coordinate Oi-1’)
Xi-1’ Zi-1’
(describe axes in coordinate Oi using axes in coordinate Oi-1’)
Zi Z i-1 θi Yi-1’ when
X i = (X i ,1 , X i ,2 , X i ,3 )T
Xi-1 Yi
Yi-1’
Xi
cos θi X i −1 ' = (cos θi X i −1,1 ' , cos θi X i −1,2 ' , cos θi X i −1,3 ')T
Yi
Xi Yi Xi
Yi-1 θi sinθi θi
Yi-1’
Coordinate: Oi Coordinate: Oi-1’ Coordinate: Oi-1 Rotation joint around Z-axis cosθi Xi-1’ X i = cos θi X i −1 '+ sin θiYi −1 '
Yi-1’ Yi = − sin θi X i −1 '+ cos θiYi −1 '
Yi
Transform matrix: Ai Transform matrix: Bi-1
cosθi Z i = Z i −1 '
θi
Decided with Parallel translation based therefore
rotational axis θi on link geometric structure
Xi cos θi − sin θi 0
sinθi Xi-1’ ( X i , Yi , Z i ) = ( X i −1 ' , Yi −1 ' , Z i −1 ') sin θi cos θi 0
Corresponds with coordinate O i when θi = 0 0
7
0 8 1
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Coordinate transformation
at rotation joint (3/4) Calculate each joint position (1/2)
# Coordinate transform matrix at rotation angle θi: Ai Joint position --- origin position of local coordinate system Oi fixed to link.
# In case of rotation joint
( X i , Yi , Zi ) = ( X i−1 ' , Yi −1 ' , Zi −1 ')Ai ・・・(a1)
Origin position Pi-1’ of
1 0 0 1 0 0 coordinate system Oi-1’ Displacement vector
Li-1 = lx Xi-1 + ly Yi-1 + lz Zi-1
Ai = Axi = 0 cos θi − sin θi
= 0 ci − si ・・・X − axis Origin position Pi of Zi
0 sin θ cos θi 0 si
ci coordinate system Oi Origin position Pi-1 of
i
Zi-1 coordinate system Oi-1
cos θi 0 sin θi Xi Yi
Xi-1
Ai = Ayi = 0 1 0 ・・・Y − axis i th link
− sin θ 0 cos θ Yi-1
i i θi i-1 th link
cos θi − sin θi 0
Ai = Azi = sin θi cos θi 0 ・・・Z − axis
(
Pi = Pi −1 + Li −1 = Pi −1 + l x X i −1 + l yYi −1 + l z Z i −1 ・・・(a7) )
0 0 1 i-1 th link vector
9 10
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Procedure of direct kinematics about
Calculate each joint position (2/2) position (from joints to end-tip)
Joint position --- origin position of local coordinate system Oi fixed to link. (1) Understand robot structure/configuration(構造) --- movable portions = joints
# In case of prismatic joint operation.
(2) Settle reference posture(基準姿勢) --- posture/appearance when all joint values
Displacement vector Origin position Pi-1 of are zero, for example, all links and joints are aligned on a line.
Li-1 = lx Xi-1 + ly Yi-1 + lz Zi-1 coordinate system Oi-1 (3) Draw mechanistic model(機構モデル) --- link-joint model using symbols.
(4) Set local coordinate system(ローカル座標系) on each link from base to end-tip in
Origin position Pi-1’ of Zi-1 order --- paying attention to origin position of each coordinate systems, direction
coordinate system Oi-1’ Xi-1 of coordinate axes, etc.
(5) Declare link vector on each link in local coordinate system.
Zi-1’
Translation Si along Xi-axis (6) Describe relationship between neighboring local coordinate systems(隣り合った
Si Xi Yi-1 ローカル座標系どおしの関係) --- for example, rotation around X-axis and telescopic
Zi along Y-axis, in order to understand coordinate transformation(座標変換) by
Origin position Pi of Xi-1’ Yi-1’ each joint.
coordinate system Oi Yi i-1 th link (7) Develop transformation matrixes from each local coordinate system to base
Xi coordinate system(それぞれのローカル座標系からベース座標系への変換行列).
i th link (changed amount) (8) Add link-vectors from basement to joint position in order to calculate joint
( )
Pi = Pi −1 + l x X i −1 + l yYi −1 + l z Z i −1 + Si X i ・・・(a8) position --- add all joint-vectors from basement to end-tip to calculate end-tip
position(手先の位置).
i-1 th link vector i th link (changed amount) (9) Coordinate axes’ direction of local coordinate system put on end-tip link shows
11 12
end-tip orientation(手先の向き).
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
ex.) Articulated (multi-joint) manipulator ex.) Articulated (multi-joint) manipulator
(多関節マニピュレータ)(1/4) (多関節マニピュレータ)(2/4)
# Reference posture of manipulator l4 z
Posture/appearance when all θi = 0. 6 x O6
All Bi becomes unit matrix (I), if all joint y A6 (around z-axis)
coordinate systems (local coordinate z
5 x B5 = unit matrix
systems) are aligned in the same O5
direction, so that they can be y A5 (around y-axis)
negligible. z B4 = unit matrix
4 x
O4 A4 (around z-axis)
l3 y
z
x B3 = unit matrix
3 O3
y A3 (around y-axis)
l2 z
2 x B2 = unit matrix
O2
y A2 (around y-axis)
z B1 = unit matrix
Coordinate systems O1~O6 are set so that axes are x
1 O1 A1 (around z-axis)
corresponding during reference posture(基準姿 l1 y
z B0 = unit matrix
勢) -- all θi = 0 0 x
O0
all Bi become unit matrix(単位行列) 13
y 14
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Multiple rotation joint in serial link Joint position of articulated (multi-
mechanism (1/2) joint) manipulator (1/2)
# Coordinate transform from joint (local) coordinate system P7 : end-tip position vector
P7
L7 to basement (reference) coordinate system L6 (l4) z L6 : link vector (joint #6 to end-tip)
6 x
O6 P6 O6 P6 : joint #6 position vector
L5 C6 (transform from O6 to O5) C1C2C3C4C5C6 (transform from O6 to O0) L5 (0) y A6 (z-rotation) C6 L5 : link vector (joint #5 to joint #6)
5 = 0C6 z B5 =I
x
O5 P5 O5 P5 : joint #5 position vector
C5 (transform from O5 to O4) C1C2C3C4C5 (trans. from O5 to O0) y A5 (y-rotation) C5
l4 L4 (0) z L4 : link vector (joint #4 to joint #5)
4 = 0C5 x B4 =I
O4 P4 O4 P4 : joint #4 position vector
l3 C4 (transform from O4 to O3) C1C2C3C4 (trans. from O4 to O0) L3 (l3) y A4 (z-rotation) C4 L3 : link vector (joint #3 to joint #4)
= 0C4 z B3 =I
3 x
O3 P3 O3 P3 : joint #3 position vector
l2 C3 (transform from O3 to O2) C1C2C3 (trans. from O3 to O0) y A3 (y-rotation) C3
L2 (l2) z L2 : link vector (joint #2 to joint #3)
2 = 0C3 B2 =I
O2 P2 x O2 P2 : joint #2 position vector
l1 C2 (transform from O2 to O1) C1C2 (trans. from O2 to O0) y A2 (y-rotation) C
2
= 0C2 L1 (0) z B1 =I L1 : link vector (joint #1 to joint #2)
1 x
O1 P1 O1 P1 : joint #1 position vector
C1 (transform from O1 to O0) y A1 (z-rotation) C1
l0 0 L0 (l1) z B0 =I L0 : link vector (basement to joint #1)
O0 15
P0 x O0 P0 : basement position vector 16
y
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Joint position of articulated (multi- Joint orientation of articulated (multi-
joint) manipulator (2/2) joint) manipulator
0P7 = 0P6 + L6
P7 P7
L6 (l4) L6 = l4 Z6 = (0,0,l4)6T describe L6 (l4)
z in reference z
x 0C6
0P6 = 0P5 + L5 coordinate x 0C6
#6 z-axis rotation Z6
P6 O6 P6 O6
L5 (0) y A6 (z-rotation) C6 L5 = 0 = (0,0,0)5T L5 (0) y A6 (z-rotation) C6 (0,0,1)6T describe
z B5 =I z B5 = I in reference
x 0C5 x 0C5
P5 O5 0P5 = 0P4 + L4 P5 O5 #5 y-axis rotation Y5 coordinate
y A5 (y-rotation) C5 y A5 (y-rotation) C5
L4 (0) z L4 = 0 = (0,0,0)4T L4 (0) z (0,1,0)5T describe
x B4 =I 0C 0 P4 = 0P3 + L3 x B4 = I 0C4 in reference
P4 O4 4
P4 O4 #4 z-axis rotation Z4 coordinate
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Joint position of articulated (multi- Local coordinate system and
joint) manipulator reference coordinate system (1/3)
# manipulator configuration # relationship between neighboring local (joint) coordinate systems
base joint-1 joint-2 joint-3 joint-4 joint-5 joint-6 hand
( X i , Yi , Zi ) = ( X i −1 ' , Yi −1 ' , Zi −1 ')Ai
link-0 link-1 link-2 link-3 link-4 link-5 link-6
x 0 0 0 0 0 0 0 = ( X i −1 , Yi −1 , Z i −1 )Bi −1 Ai
y 0 0 0 0 0 0 0 = ( X i −1 , Yi −1 , Z i −1 )C i ・・・(a3)
z l1 0 l2 l3 0 0 l4
# relationship between reference (basement) coordinate system and
# joint position vector local (joint) coordinate system
P7 = P6 + l4 Z 6 = l1 Z 0 + l2 Z 2 + l3 Z 3 + l4 Z 6
( X i , Yi , Zi ) = ( X i −1, Yi −1, Z i −1 )Ci
P5 = P6 = P4
= ( X i −2 , Yi −2 , Z i −2 )C i −1Ci
P4 = P3 + l3 Z 3 = l1 Z 0 + l2 Z 2 + l3 Z 3
:
P3 = P2 + l2 Z 2 = l1 Z 0 + l2 Z 2 = ( X1, Y1 , Z1 )C 2 ⋅ ⋅ ⋅ C i −1C i
P2 = P1 Add link vectors from basement in order, = ( X 0 , Y0 , Z 0 )C1C 2 ⋅ ⋅ ⋅ Ci −1Ci
P1 = P0 + l1 Z 0 = l1 Z 0 but,
adding in components (x, y, and z elements) =( X 0 , Y0 , Z 0 )0Ci
P0 = 0 must be in the same coordinate system 19 20
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Local coordinate system and Local coordinate system and
reference coordinate system (2/3) reference coordinate system (3/3)
# (cont.) relationship between reference (basement) coordinate system c11 c12 c13
and local (joint) coordinate system ( X i , Yi , Z i ) = ( X 0 , Y0 , Z 0 ) c21 c22 c23
( X i , Yi , Z i )
= ( X 0 , Y0 , Z 0 )C1C 2 ⋅ ⋅ ⋅ C i −1C i c
31 c32 c33
=( X 0 , Y0 , Z 0 )0C i
1 0 0 c11
c11 c12
c13
X i = X 0c11 + Y0c21 + Z 0c31 = 0 c11 + 1 c21 + 0 c31 = c21
= ( X 0 , Y0 , Z 0 ) c21 c22
c23 0
0
0
0
1
0
c
31 0
c
31 c32 c33 c12
Yi = X 0c12 + Y0c22 + Z 0c32 = c22
1 0 0 1 0 0 c11 c12 c13
c
0 , 1 , 0 = 0 , 1 , 0 c21 c22 c23 32 0
c13
0 i 0 i 1 i 0 0 0 0 1 0 c31 c32 c33
Z i = X 0c13 + Y0c23 + Z 0c33 = c23
c
21 33 0 22
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Setting of reference posture and local
coordinate systems Practice (1) – Stanford manipulator
Difficulty, complexity, and complication of coordinate 1. Understand robot structure/configuration.
2. Settle reference posture.
transformation calculation depend on …
3. Draw mechanistic model.
4. Describe relationship between neighboring
setting of local coordinate systems.
(1) Reference posture (state at initial value, zero) 5. Declare link vector on each link in local
(2) Local coordinate system (orientation of coordinate axes) coordinate system.
6. Develop transformation matrixes
from each local coordinate system
to base coordinate system.
-----------------------------
Format : A4 sheet
23 24
Time limit : 15-20 min.
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
25 26
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Setting of local Z6
Joint position
coordinate system Z6-direction
l3
of Stanford manipulator
(c) devising in local coordinate
system : directions of link Z3
vectors are corresponding (z- Y5 Z6-direction Z6
direction) as much as l3
possible during reference Z4
posture. Y5
Z2-direction
S(t) Z3-direction Z4
End-tip
S(t)
Joint #6 z-rotation
Joint #5 y-rotation Y2 Z3
Joint #4 z-rotation Z1 0
Y2
Joint #3 z-direction Y1-direction Z1
Joint #2 y-rotation -l 2 Y1-direction
Joint #1 z-rotation -l 2
Z0-direction
Basement Z0-direction
l1 l1
27 28
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Joint orientation D-H (Denavit–Hartenberg)
of Stanford manipulator parameters -1
Coordinate transformations along a serial robot
Z6-direction Z6
l3 [T] transformation locating the end-link
[Z] transformation associated with the joint
[X] second transformation associated with the link
Y5
Convention Denavit-Hartenberg introduced
Z3-direction Z4 Z coord-axes are assigned to the joint axes Si
S(t) θi is the rotation around, di is the slide along the Z axis
Z3
0
Y2
Z1
Y1-direction
X coord-axes are assigned to the common normals Ai,i+1
-l 2 αi,i+1 and ri,i+1 define the physical dimensions of the link
Z0-direction
l1
29 30
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
D-H (Denavit–Hartenberg) Postural (orientation of local
parameters -2 coordinate system) change rate (1/2)
Coordinate transformations Postural change rate
from the previous coordinate system to the next coordinate system Zi = time-derivative of vector / matrix showing local coordinate
Yi i
∂ •
( X i , Yi , Z i ) = ∑ ( X i , Yi , Z i )θ j ・・・(b1)
d
Oi Xi dt j =1 ∂θ j
dθj
∂
(
( X i , Yi , Z i ) = p j × X i , p j × Yi , p j × Z i ・・(b2)
∂θ j
)
dt
* Outer product : p j = ( p1 , p2 , p3 )T
Denavit-Hartenberg matrix
X i = ( X 1 , X 2 , X 3 )T
p2 X 3 − p3 X 2
p j × X i = p3 X 1 − p1 X 3
R: 3×3 submatrix describing rotation pX −p X
T: 3×1 submatrix describing translation
31
1 2 2 1 32
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Postural (orientation of local Translational speed of origin of local
coordinate system) change rate (2/2) coordinate system on link (1/2)
If assign eq. (b2) to eq. (b1), # In case of i th rotational joint,
•
( )
i eq. (a7) indicating rotational joint position is differentiated.
( X i , Yi , Z i ) = ∑ p j × X i , p j × Yi , p j × Z i θ j ・・・(b3)
d
dt j =1
d d
(
Pi = Pi −1 + ωi −1 × l x X i −1 + l yYi −1 + l z Z i −1 ・・・(b6)
dt dt
)
Rotational speed vector of i th link.
i • • • •
# In case of i th prismatic joint,
ωi = ∑ p j θ j
= p1 θ1 + p2 θ 2 + ⋅ ⋅ ⋅ + pi θ i ・・・(b4) eq. (a8) indicating prismatic joint (along x) position is differentiated.
j =1
If assign eq. (b4) to eq. (b3), posture change rate is
d d
(
Pi = Pi −1 + ωi −1 × l x X i −1 + l yYi −1 + l z Z i −1
dt dt
)
( X i , Yi , Z i ) = (ωi × X i , ωi × Yi , ωi × Z i ) ・・・(b5)
d •
dt + S (ωi × X i ) + S X i ・・・(b7)
•
while prismatic speed S
rotational speed (i th link is not rotated) ωi = ωi-1
33 34
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Translational speed of origin of local ex.) Rotation / translation speed
coordinate system on link (2/2) on articulated manipulator (1/2)
# Translation speed by rotation joint = geometric image of eq. (b6) # Reference posture of manipulator l4 z
Posture/appearance when all θi = 6 x O6
dt : short period 0. y A6 (around z-axis)
z
Transform All Bi becomes unit matrix (I), if 5 x B5 = unit matrix
O5
from small rotational displacement dθ all joint coordinate systems y A5 (around y-axis)
to small translational displacement dL (local coordinate systems) are z B4 = unit matrix
4 x
aligned in the same direction, O4
dL = L sin α sin dθ so that they can be negligible. l3
y
z
A4 (around z-axis)
x B3 = unit matrix
dL = L sin α dθ Small displacement 3
y
O3 A3 (around y-axis)
vector dL |L| sinα l2 z
dL = L sin α ω dt
Rotation speed 2 x B2 = unit matrix
vector ω O2
α y A2 (around y-axis)
dL z
= L sin α ω A point distant |L| Rotation x B1 = unit matrix
dt from rotation centre center 1
y
O1 A1 (around z-axis)
dL l1 z
= ω× L 0 x B0 = unit matrix
dt 35
O0 36
y
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
ex.) Rotation / translation speed Acceleration of origin of local
on articulated manipulator (2/2) coordinate system on link (1/2)
# Rotational speed vector # Translational speed vector of origin # In case of i th rotational joint,
: from eq. (b4) of local coordinate : from eq. (b6) eq. (b6) indicating translational speed is differentiated.
• d d
P1 = P2 = 0 •
ω1 = Z1 θ1
•
dt dt
d2 d2
(
2 Pi = 2 Pi −1 + ωi −1× l x X i −1 + l yYi −1 + l z Z i −1 )
dt dt
P3 = ω2 × (l2 Z 2 ) ( ( ))
ω2 = ω1 + Y2 θ2 d
+ ωi −1 × ωi −1 × l x X i −1 + l yYi −1 + l z Z i −1
• dt
ω3 = ω2 + Y3 θ3 ・・・(b8)
P4 = P3 + ω3 × (l3 Z 3 )
d d
• dt dt
ω4 = ω3 + Z 4 θ4 • d d i −1 •
d d
P5 = P4
while ωi −1 =
ωi −1 = ∑ p j θ j
• dt dt j =1
ω5 = ω4 + Y5 θ5 dt dt
d d i −1 •• i −1 •
•
ω6 = ω5 + Z 6 θ6 dt
P6 = P5
dt = ∑ p j θ j + ∑ ωi −1 × p j θ j
( )
j =1 j =1
P7 = P6 + ω6 × (l4 Z 6 ) 37
d d
38
dt dt
Practice (2) –
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Acceleration of origin of local
coordinate system on link (2/2) (pseudo) polar coordinate type manipulator
# In case of i th prismatic joint, Show position, speed, acceleration at basement (P0), joint-1 (P1),
eq. (b7) indicating translational speed is differentiated. joint-2 (P2), and end-tip (P3) of (pseudo) polar coordinate type
•
d2
Pi =
d2
(
Pi −1 + ωi −1× l x X i −1 + l yYi −1 + l z Z i −1 ) manipulator (2 d.o.f. = rotational + rotational).
Z2
dt 2 dt 2
( (
+ ωi −1 × ωi −1 × l x X i −1 + l yYi −1 + l z Z i −1 )) Coordinate system O3 (position P3)
Z3 l2 X2
translational movement P2 Y 2
• x-direction link l2
+ S ωi × X i + ωi × (ωi × X i )
Coordinate system O2 (position P2) X3 l1
θ2
θ1
• ••
y-axis rotation θ2 P 3 Y3 Z1
P1
+ 2 S (ωi × X i ) + S X i ・・・(b9)
z-direction link l1
X1
Coordinate system O1 (position P1)
0 Y
while rotational speed / acceleration (i th link is not rotated) z-axis rotation θ1 Z0 1
• • z-direction link 0 P0
ωi = ωi-1,
ωi = ωi-1 Coordinate system O0 (position P0) ----------------------------- X 0
39
Format : A4 sheet Y040
Time limit : 15-20 min.
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Direct kinematics of (pseudo) polar Direct kinematics of (pseudo) polar
coordinate type manipulator (1/3) coordinate type manipulator (2/3)
• Manipulator configuration (link, joint) and orientation vector and position • Translational speed of origin of local coordinate system on link
vector of each joint : assign to eq. (b6)
(referring examples of articulated manipulator and Stanford manipulator) d
P0 = 0
P3 O3 0P3
= 0P2 + L2 = l1Z1 + l2X2 dt
X2-direction l2 A3 = I
B2 = I
I L2 = l2X2 = (l2,0,0)2T d
P1 =
d
dt
(
P0 + ω0 × l x X 0 + l yY0 + l z Z0 = )
X2 P2 O2 0C 0
2 P2 = 0P1 + L 1 = l1Z1 dt
構成図
Z1-direction l1
A2 = (y-rotation θ2) C2
L1 = l1Z1 = (0,0,l1)1T
d
P2 =
B1 = I dt
Z1 P1 O 1 0C 0
1 P1 = 0P1 + L1 = 0
A1 = (z-rotation θ1) C1 d
0 L0 = (0,0,0)0T P3 =
B0 = I 0P0 = 0 dt
P0 O 0
• •
• rotation speed vector of each joint ω2 = = l2 θ1 (Z1 × X 2 ) − l2 θ 2 Z 2
(referring articulated manipulator) ω1 =
ω0 = 0
41 42
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Direct kinematics of (pseudo) polar Practice (3) –
coordinate type manipulator (3/3) (pseudo) cylindrical coordinate type manipulator
• Translational acceleration of origin of local coordinate system on link Show position, speed, acceleration at basement (P0), joint-1 (P1),
: assign to eq. (b8) joint-2 (P2), and end-tip (P3) of (pseudo) cylindrical coordinate
d2 type manipulator (2 d.o.f. = rotational + prismatic).
2 P0 =
0
dt
Coordinate system O3 (position P3)
d2
P1 = translational movement
dt 2 x-direction link l2
d2 Coordinate system O2 (position P2)
P2 = y-axis rotation θ2
dt 2
z-direction link l1
d2
P3 = Coordinate system O1 (position P1)
dt 2 z-axis rotation θ1
z-direction link 0
Coordinate system O0 (position P0) -----------------------------
43
Format : A4 sheet 44
Time limit : 15-20 min.
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015
Direct kinematics of (pseudo) cylindrical Direct kinematics of (pseudo) cylindrical
coordinate type manipulator (1/3) coordinate type manipulator (2/3)
• Manipulator configuration (link, joint) and orientation vector and position • Translational speed of origin of local coordinate system on link
vector of each joint : assign to eq. (b6) and (b7)
(referring examples of articulated manipulator and Stanford manipulator) d
P0 = 0
0P3 = dt
d
dt
P1 =
d
dt
(
P0 + ω0 × l x X 0 + l yY0 + l z Z0 = )
0P2 =
構成図 d
P2 =
dt
0P1 =
d
P3 =
0P0 =0 dt
• rotation speed vector of each joint ω2 =
(referring articulated manipulator)
ω1 = • •
= S (t ) θ1 Y1
+ S (t ) X 1
ω0 = 0 45 46
•• • • •• • 2
= S θ1 + 2 S θ1 Y1 + S − S θ1 X 1
47