พืน้ฐานหุ่นยนต์ อุตสาหกรรม ·...

พนฐานหนยนตอตสาหกรรม

Fundamental of Industrial Robotics

สนทด ชวงคอนทร (Ph.D.)วทยาลยนวตกรรมการผลตขนสง

➢ เฟรมมาตรฐาน (Standard Frames)

➢ ไคเนเมตกส ไปขางหนา(Forward kinematics) 2D & 3D

➢ DH พารามเตอร (Denavit-Hartenberg parameters)

➢ ไคเนเมตกส ยอนกลบ (Inverse Kinematics) 2D & 3D

Kinematics (ไคเนเมตกส )

http://petercorke.com

Treatment of motion without regard to the forces that cause it.

สนใจเฉพาะการเคลอนทโดยไมสนใจแรงทท าใหเกดการเคลอนท

Industrial Robot

https://www.yaskawa-global.com

Degrees of Freedom of a Rigid Body in Space :

Degrees of Freedomวตถแขงเกรงใน 3 มต ม 6 องศาอสระ ประกอบดวย

การเลอนตามแนวแกน x, y และ z และการหมนรอบแกน x, y และ z ตามล าดบ

Number of joints and kinematic computation of robots


แขนคนเรามก DOF?

แขนคนเราม 7 DOF

แขนกลในรปมก DOF?

แขนกลม 6 DOF ไมรวม gripper

Kinematics


สมการไคเนเมตกสส าหรบการค านวณ positions (x, y, z) หรอ orientation (RX, RY, RZ) ของจด P จากคามมของแตละขอตอของแขนกล (S, θL, θU, θR, θB, และ θT) เรยกวา สมการไคเนเมตกสไปขางหนา(Forward kinematics)

Kinematics

รปท 1 ทลเฟรมสมพทธกบเบสเฟรมโดยเปนฟงกชนของตวแปรของขอตอตางๆ

Kinematics (ไคเนเมตกส ) จลนศาสตร:กลศาสตรทเกยวกบการเคลอนท• Forward Kinematics : การค านวณหาต าแหนง

(position: x,y,z ) และทศทาง (orientation:

𝑅𝑥, 𝑅𝑦, 𝑅𝑧) ของกรปเปอรหรอทล (tool) ของหนยนต

ทสมพทธกบเวรคสเตชนของผใชงาน เมอรคามมของ

ขอตอตางๆ (joint angles : θ1, θ2, θ3) ของแขนกล

(manipulator)

รปท 2 ต าแหนงปลายเดยวกนแตมคามมของขอตอทตางกนของ Inv.

𝜃1, 𝜃2, 𝜃3 รคา

หา 𝑥, 𝑦, 𝑧,𝑅𝑥, 𝑅𝑦, 𝑅𝑧ของกรปเปอร

หาคา 𝜃1, 𝜃2, 𝜃3

ร 𝑥, 𝑦, 𝑧,𝑅𝑥, 𝑅𝑦, 𝑅𝑧ของกรปเปอร

Forward Kinematics

Inverse Kinematics

• Inverse Kinematics : การค านวณหาคามมของขอตอตางๆ

ของแขนกล เมอก าหนดต าแหนง และทศทางของกรปเปอร

หรอทลของแขนกลทสมพทธกบเวรคสเตชน หรอเบสมาให

Standard frames

𝐵𝜉𝑇 =𝐵𝜉𝑊⨁

𝑊𝜉𝑇

𝐵𝜉𝐺 = 𝐵𝜉𝑊⨁𝑊𝜉𝑇 ⨁

𝑇𝜉𝐺

ร X Y Z ของเฟรม และรทศทางของเฟรม

• ถาตดโคออรดเนตเฟรมเขากบวตถ (กรณนเปนรถ) เราสามารถอธบายทกจดในวตถดวยเวกเตอรคงทเทยบกบเฟรมนน{B}

• สามารถอธบาย Position และ Orientation (Position + Orientation = Pose) ของจด P ใดๆ บนโคออรดเนต {B} เทยบกบโคออรดเนตเฟรมอางอง {O}หรอ เวลดโคออรดเนตเฟรม

• เพอแยกความแตกตางของเฟรมทก าหนดขน ในกรณนจะใชสญลกษณแสดงโคออรดเนตเฟรมของวตถ {B}

และแกนของมนคอ xB และ yB โดยตวหอยแสดงถงเฟรม B ดงรปท 1

Position + Orientation = pose

รปท 1

• เพออธบายถง Pose ของวตถแขงเกรงใน 3 มต เราตองการถง 6 DOF โดยท 3 เพออธบาย Position

(x,y,z) และอก 3 เพออธบาย Orientation (𝑅𝑥, 𝑅𝑦 , 𝑅𝑧)

• DOF เหลานมความแตกตางกนมาก ถาเราเพมคา Position วตถจะเคลอนทตอเนองเปนเสนตรง แตถาเราเพมคา Orientation วตถจะหมนไปและในทสด วตถจะกลบมายง Orientation เดม โดยมแนวการหมนเปนเสนโคง ดงนน เราจ าเปนตองจดการกบทงสองสวนนแตกตางกน

• Pose ของโคออรดเนตเฟรมจะถกแสดงดวยสญลกษณ ξ (ออกเสยงวา “ไซ”)

รปท 2


• รปท 2 แสดงเฟรม {A} และ {B} และ Relative Pose AξB

ซงอธบาย Pose จาก{B} เทยบกบ {A} โดยท ตวยกหมายถงโคออรดเนตเฟรมอางอง และตวหอยหมายถงเฟรมทก าลงถกอธบาย

• AξB อาจถกมองวาเปนการอธบายการเคลอนไหวบางอยาง –จนตนาการวา ยกขน {A} และเลอนระยะแลวหมนเพอใหเปลยนกลายเปน {B} ถาไมมตวยก เชน ξB จะหมายถงการเปลยนแปลง Pose ทเทยบกบเวลด โคออรดเนตเฟรมซงโดยทวไปแสดงดวยสญลกษณ {O}

• จด P ในรปท 2 เทยบกบทงสองโคออรดเนตเฟรมโดยเวกเตอร 𝐴𝑝 หรอ 𝐵𝑝 ตามล าดบ สมพนธกนดวยสมการ 𝐴𝑝 = 𝐴𝜉𝐵 ∙

𝐵𝑝

โดยทดานขวาของสมการ แสดงถงการเคลอนทจากเฟรม{A}ถง{B} และจากนนไปยงจด P โอเปอเรเตอร ∙ ท าการแปลงเวกเตอรใหเปนเวกเตอรใหมทอธบายจด P เดยวกนแตเทยบกบโคออรดเนตเฟรมอนในกรณนคอ{B}

• ในรปท 3 ถาเฟรมหนงถกอธบายในเทอมของอกเฟรมหนงดวย Relative Pose ดงนนเฟรมทงหมดสมพนธกนดวยสมการ 𝐴𝜉𝐶 =

𝐴𝜉𝐵⨁𝐵𝜉𝐶

โดยทPose ของเฟรม {C} เทยบกบ {A} สามารถหาไดโดยการผสม Relative Pose จากเฟรม {B} เทยบ {A} และจากเฟรม {C} เทยบ{B} เราใชโอเปอเรเตอร ⊕ เพอแสดงการประกอบกนของ Relative Pose

• ในการประกอบกนของ Relative Pose ตวหอยและตวยกในแตละดานของโอเปอเรเตอร ⨁ ถาจบคกนได เราสามารถตดตวหอยและตวยกทอยตรงกลางทงได

รปท 2รปท 3


รปท 4𝜉𝐹⨁

𝐹𝜉𝐵 = 𝜉𝑅⨁𝑅𝜉𝐶⨁

𝐶𝜉𝐵𝜉𝐹⨁

𝐹𝜉𝑅 = 𝜉𝑅

• รปตวอยาง ทใสโคออรดเนตเฟรม 3 มตเขากบสงตางๆ เชน กลอง วตถ หนยนต และเวลดโคออรดเนตเฟรม เปนตน และแสดง Relative Poses ของแตละอน (𝜉𝐹 , 𝜉𝐵 , 𝜉𝑅 ,

𝐹𝜉𝐵 ,𝐶𝜉𝐵 ,

𝑅𝜉𝐶 เปนตน)

Multiple 3D coordinateframes and relative poses

หมายเหต ลกศรจาก {X} ไป {Y} แสดงเปน 𝑿𝝃𝒀 อธบาย Pose ของเฟรม Y เทยบกบ X

• กลองทอยกบทมองเหนวตถจากมมมองของมนและจะประมาณคา Pose ของวตถเปน 𝐹𝜉𝐵 เมอเทยบกบตวมน สวนกลองอกอนทไมอยกบทถกตดตงไวกบหนยนตซงมคา Relative Pose คาหนงและจะประมาณคา Pose ของวตถเปน 𝐶𝜉𝐵 เมอเทยบกบตวมน

รปท 5

Representations of pose ξ

แมพ โคออรดเนตเวคเตอรจากเฟรม {B} ไปหาเฟรม {A} Pose ของเฟรม {B} เทยบกบเฟรม {A}

(ลกศรจากเฟรม {A} ไปเฟรม {B})

รปท 6

• จด 𝑝 = 𝑥ෝ𝒙+𝑦ෝ𝔂 เชน 1ෝ𝒙 + 2ෝ𝔂 หรอ −2ෝ𝒙 + 0.5ෝ𝔂

• รปแสดงโคออรดเนตเฟรมสแดง{B}ทเราตองการอธบายเมอเทยบกบโคออรดเนตเฟรมอางองสฟา{A}

• จะเหนไดชดเจนวาจดก าเนดของ{B}ถกแทนดวยเวคเตอร 𝑡 = (𝑥, 𝑦)จากนนหมนทวนเขมนาฬกาดวยมม

Working in 2D

𝐴𝜉𝐵อยในเทอมของ (𝑥, 𝑦, 𝜃)

การหมนและการเลอน (Rotation() & Translation(x,y))

• สรางเฟรมใหม {V} ทแกนทงสองขนานกบ{A} แตมจดก าเนดอยท{B}ดงรป

• แสดงจด P เมอเทยบกบ {V} ในรปของเวคเตอรหนงหนวยของแกนทงสองไดดงน

𝑉𝑝 = 𝑉𝑥ෝ𝔁𝑉 + 𝑉𝑦ෝ𝔂𝑉 = ෝ𝔁𝑉 ෝ𝔂𝑉

𝑉𝑥𝑉𝑦

• ดงนนโคออรดเนตเฟรม{B} ถกแสดงดวยเวกเตอรหนงหนวย ෝ𝔁𝑉 , ෝ𝒚𝑉 ไดดงน

𝑥𝐵 = 𝑐𝑜𝑠 𝜃 ෝ𝔁𝑉 + 𝑠𝑖𝑛 𝜃 ෝ𝔂𝑉 และ 𝑦𝐵 = −𝑠𝑖𝑛 𝜃 ෝ𝔁𝑉 + 𝑐𝑜𝑠 𝜃 ෝ𝔂𝑉

Orientation in 2D: Rotation Matrix1. พจารณา Rotation หรอ การหมนกอน

➢ 𝑥𝐵 = 𝑐𝑜𝑠 𝜃 ෝ𝔁𝑉 + 𝑠𝑖𝑛 𝜃 ෝ𝔂𝑉

➢ 𝑦𝐵 = −𝑠𝑖𝑛 𝜃 ෝ𝔁𝑉 + 𝑐𝑜𝑠 𝜃 ෝ𝔂𝑉

➢ ෝ𝔁𝐵 ෝ𝔂𝐵 = ෝ𝔁𝑉 ෝ𝔂𝑉𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃

➢ สามารถแสดงจด P เมอเทยบกบ{B}ดวย 𝐵𝑃 = 𝐵𝑥ෝ𝔁𝐵 + 𝐵𝑦ෝ𝔂𝐵 = ෝ𝔁𝐵 ෝ𝔂𝐵𝐵𝑥𝐵𝑦

Orientation in 2D

ตวอยาง เพอหาโคออรดเนตของจด P ใน{1} เมอทราบ P ใน{0}

T0 = trot2(0, 'deg')

T1 = transl2(1.5, 1.5) * trot2(75, 'deg') % transl2 creates a relative pose with a finite translation&trot2 creates rotation

plotvol([0 3 0 3]);

trplot2(T0, 'frame', '0', 'color', 'm');

trplot2(T1, 'frame', '1', 'color', 'r');

P = [2 ; 3 ];

plot_point(P, 'label', 'P', 'solid', 'k*');P1 = inv(T1) * [P; 1]

Matlab toolbox for Pose in 2D: 𝐴𝑇𝐵

➢0𝑃 = 0𝑇1 ∙

1𝑃

➢1𝑃 = 0𝑇1

−1∙ 0𝑃→ P1=inv(T1)*[2; 3; 1]

➢ โคออรดเนตจด P(2,3) ใน{0} คอจด (1.5783,-0.0947,1) ใน {1}

𝐴𝑇𝐵 =𝐴𝑅𝐵 𝑡

01×2 1=

cos𝜃 − sin 𝜃 𝑥sin 𝜃 cos 𝜃 𝑦0 0 1

=0.2588 −0.9659 1.50.9659 0.2588 1.5

0 0 1

➢𝐵𝑃 = ෝ𝔁𝐵 ෝ𝔂𝐵

𝐵𝑥𝐵𝑦

= ෝ𝔁𝑉 ෝ𝔂𝑉𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃

𝐵𝑥𝐵𝑦

➢𝑉𝑥𝑉𝑦

=𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃

𝐵𝑥𝐵𝑦

➢ สมการขางบน ใชอธบายจด P หรอเวคเตอร P ซงถกแปลงจากเฟรม{B}ไปหา{V} เมอ{B}ถกหมน โดยเมทรกซขนาด 22 นจะถกเรยกวา เมทรกซการหมน (Rotation Matrix) และถกแสดงดวย 𝑉𝑅𝐵

➢𝑉𝑅𝐵 แมพเวคเตอรจากเฟรม{B}ไปหา{V}แตเปนฟงกชนของการหมนจากเฟรม{V}ไปหา{B}

➢𝑉𝑥𝑉𝑦

= 𝑉𝑅𝐵𝐵𝑥𝐵𝑦

Orientation in 2D

clear

syms theta

R = rot2(theta)

B = trot2(30, 'deg');

V = eye(3,3);

plotvol([-1 1 -1 1]);

trplot2(V, 'frame','V','color', 'b');

trplot2(B, 'frame','B','color', 'r');

Matlab toolbox for Orientation in 2D

𝑉𝑥𝑉𝑦

=𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃

𝐵𝑥𝐵𝑦

สมมตวา θ = 30

➢𝑋𝑅𝑌 =

𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃

➢𝑉𝑥𝑉𝑦

= 𝑉𝑅𝐵𝐵𝑥𝐵𝑦

➢𝐵𝑥𝐵𝑦

= 𝑉𝑅𝐵−1 𝑉𝑥

𝑉𝑦= 𝑉𝑅𝐵

𝑇 𝑉𝑥𝑉𝑦

= 𝐵𝑅𝑉𝑉𝑥𝑉𝑦

Orientation in 2D

Rotation Matrix มคณสมบตพเศษดงตอไปน

1. เปน orthonormal (หรอเรยกอกอยางวา orthogonal = ตงฉาก) เนองจากแตละคอลมนเปนเวกเตอรหนงหนวยและคอลมนทงสองตงฉากกนดวย (dot product เปนศนย)

2. ดเทอรมแนนต det ( 𝐴𝑅𝐵) มคา = +1 (คณเวคเตอรใดๆดวยเมทรกซนขนาดเวคเตอรไมเปลยน)

3. อนเวอรสเมทรกซ = ทรานสโพสเมทรกซ หรอ 𝑅−1 = 𝑅𝑇

𝑉𝑅𝐵−1

= 𝑉𝑅𝐵𝑇

= 𝐵𝑅𝑉

➢ การเลอน (Translation) ของจดก าเนดของเฟรมแสดงในรปท 8 เนองจากแกนของเฟรม {V}

และ {A} ขนานกนการเลอนจงเปนเพยงการบวกเวกเตอรธรรมดา

AxAy

=VxVy

+x

y=

cos θ −sin θsin θ cos θ

BxBy

+x

y=

cos θsin θ

− sin θcos θ

xy

BxBy1

AxAy1

=𝐴𝑅𝐵 𝑡

01×2 1

BxBy1

โดยท 𝑡 คอ (x,y)

Pose in 2D:Homogeneous Transformation Matrix

รปท 8 𝐴𝑅𝐵 = 𝑉𝑅𝐵

2. พจารณา Translation หรอ การเลอน

Homogeneous Transformation Matrix

บวกกน

AxAy1

=𝐴𝑅𝐵 𝑡

01×2 1

BxBy1

𝐴 𝑝 = 𝐴𝑇𝐵𝐵 𝑝

โดยท 𝐴 𝑝 = Ax, Ay, 1 , 𝐵 𝑝 = Bx, By, 1 และ𝐴𝑇𝐵 คอ Homogeneous Transformation

Matrix ซงแทน translation + orientation หรอ relative pose ()


01×2 1=

cos 𝜃 − sin 𝜃 𝑥sin 𝜃 cos 𝜃 𝑦0 0 1

Pose in 2D: 𝐴𝑇𝐵

T1 = transl2(1, 2) * trot2(30, 'deg')

T2 = transl2(2, 1)

T3 = T1*T2; T4 = T2*T1

plotvol([0 5 0 5]);

trplot2(T1, 'frame', '1', 'color', 'r');

trplot2(T2, 'frame', '2', 'color', 'b');

trplot2(T3, 'frame', '3', 'color', 'g');

trplot2(T4, 'frame', '4', 'color', 'c');

P = [3 ; 2 ];

plot_point(P, 'label', 'P', 'solid', 'ko');P1 = inv(T1) * [P; 1]

Matlab toolbox for Pose in 2D: 𝐴𝑇𝐵

➢ เพอหาโคออดเนตของจดเทยบกบ {1}

➢0𝑃 = 0𝑇1 ∙

1𝑃

➢1𝑃 = 0𝑇1

−1∙ 0𝑃→ 𝑃1=inv(T1)*[3; 2;

1]


01×2 1=

cos 𝜃 − sin 𝜃 𝑥sin 𝜃 cos 𝜃 𝑦0 0 1

sysm x y thetaT1 = transl2(x, y) * trot2(theta)

plotvol([-5 4 -1 5]);

T0 = eye(3,3);

trplot2(T0, 'frame', '0');

X = transl2(2, 3);

trplot2(X, 'frame', 'X');

R = trot2(3*pi/5)

trplot2(R*X, 'framelabel', 'RX', 'color', 'r');

trplot2(X*R, 'framelabel', 'XR', 'color', 'r');

C = [1 2]';

plot_point(C, 'label', ' C', 'solid', 'ko')

Centers of Rotation

➢ จะเหนวาเฟรม {RX} หมนรอบจดก าเนดของเฟรม{O}

ในขณะทเฟรม{XR}หมนรอบจดก าเนดของเฟรม{X}

➢0𝑝 = 0𝜉1 ∙

1𝑝

➢1𝑝 = 0𝜉1

−1∙ 0𝑝

รปท 10

• จด P แสดงดวยพกด x, y และ z (x, y, z) หรอเวกเตอร 𝑝 = 𝑥ෝ𝒙 + 𝑦ෝ𝔂 + zෝ𝔃

• ෝ𝔃 = ෝ𝒙 × ෝ𝔂 , ෝ𝒙 = ෝ𝔂 × ෝ𝔃, ෝ𝔂 = ෝ𝔃 × ෝ𝒙 ดงแสดงในรปท 9

• รปท 10 แสดงโคออรดเนตเฟรมสแดง {B} ทเราตองการอธบายเมอเทยบกบโคออรดเนตเฟรมอางองสฟา {A} เหนไดวาจดก าเนดของ {B} ถกยายทไปดวยเวกเตอร t = (x, y, z) แลวหมนในรปแบบทซบซอน (𝑅𝑥(𝜃𝑥)=? , 𝑅𝑦(𝜃𝑦)=? , 𝑅𝑧(𝜃𝑧)=? )

• วธการคอการพจารณาจด P ใดๆเทยบกบแตละโคออรดเนตเฟรม และหาความสมพนธระหวาง 𝐴𝑝และ 𝐵𝑝 เราจะพจารณาปญหาเปนสองสวนเหมอน 2D คอ การหมน และการเลอน

Working in 3D

รปท 9

รปท 10

➢ จนตนาการวาหยบเฟรม {A} ไวในมอของเราและหมนไปเรอย ๆจนกระทงมนดเหมอนเฟรม{B}

➢ ทฤษฎบทการหมนของออยเลอรกลาววา “การหมนใด ๆ ถอวาเกดจากล าดบการหมนแกน 3 แกน (x

y และ z) ไมเกน 3 ครงของการหมนแกนโคออรดเนตโดยตองไมหมนแกนเดยวกน 2 ครงตดกน”

➢ ในการหมน 3 มตสลบล าดบการหมนรอบแกนใดๆ จะใหผลลพธไมเทากน ดงแสดงในรปท 11-12

Orientation in 3D

Orientation in 3D

รปท 11 การหมนโคออรดเนตเฟรม 3 มต a=โคออรดเนตเฟรมเดม,b ถง f =โคออรดเนตเฟรมหลงจากการหมนแบบตางๆตามทระบไว

Orientation in 3D

รปท 12 ตวอยางทแสดงการสลบการหมนใหผลลพธไมเทากน แถวดานบนโคออรดเนต

เฟรมถกหมนไป 𝜋

2เรเดยนรอบ

แกน x และจากนนถกหมนไป𝜋

2เรเดยนรอบแกน y ในแถว

ลางล าดบของการหมนสลบกน ผลทไดแตกตางกน

➢ เวกเตอรแตละตวม 3องคประกอบและสรางคอลมนของ เมทรกซ 𝐴𝑅𝐵 ขนาด 33 แบบ orthonormal

AxAyAz

= 𝐴𝑅𝐵

BxByBz

ซงจะแปลงเวกเตอรใน(เทยบกบ)เฟรม {B} ไปเปนเวกเตอรใน(เทยบกบ) เฟรม{A}

Orientation in 3D: Orthonormal Rotation Matrix

3D-Rotation Matrix 𝐴𝑅𝐵 มคณสมบตพเศษดงตอไปน

1. เปน orthonormal (หรอเรยกอกอยางวา orthogonal = ตงฉาก) เนองจากแตละคอลมนเปนเวกเตอรหนงหนวยและคอลมนทงสองตงฉากกนดวย (dot product เปนศนย)

2. คอลมนเปนเวกเตอรหนงหนวย ทนยามแกนของเฟรม B ทหมนเทยบกบ A

3. ดเทอรมแนนต det ( 𝐴𝑅𝐵) มคา = +1 (คณเวคเตอรใดๆดวยเมทรกซนขนาดเวคเตอรไมเปลยน)

4. อนเวอรสเมทรกซ = ทรานสโพสเมทรกซ หรอ 𝑅−1 = 𝑅𝑇

➢ The orthonormal rotation matrices ส าหรบการหมน θ รอบแกน 𝑥, 𝑦 หรอ 𝑧 คอ

➢ Toolbox มฟงกชนในการค านวณเมทรกซการหมนเหลานทง 𝑅𝑥 𝜃 , 𝑅𝑦 𝜃 , 𝑅𝑧(𝜃)

➢ The orthonormal rotation matrices ม 9 คา แตไมไดเปนอสระตอกน 3 ขอจ ากดแรกคอในแตละคอลมนมขนาดหนงหนวย 3 ขอจ ากดทสองคอแตละคอลมนตงฉากซงกนและกน

Orientation in 3D: Orthonormal Rotation Matrix

syms theta

Rx = rotx(theta)

Ry = roty(theta)

Rz = rotz(theta)

R = rotx(90)*roty(90)

trplot(R);tranimate(R)

inv(R)

R’

det(R)

𝑅𝑥 0° = 𝑅𝑦 0° = 𝑅𝑧 0° =1 0 00 1 00 0 1

𝑅 𝜃 =

𝑋𝑥 𝑌𝑥 𝑍𝑥𝑋𝑦 𝑌𝑦 𝑍𝑦𝑋𝑧 𝑌𝑧 𝑍𝑧

Demonstrate axes toolbox

➢ ทฤษฎบทการหมนของออยเลอร จ าเปนตองหมน 3แกน ตอเนองกนโดยทไมหมนแกนเดยวกน 2 ครงตอเนองกน การหมนม 2 แบบ: Eulerian and Cardanian

➢ Eulerian : XYX, XZX, YXY, YZY, ZXZ, or ZYZ

➢ Cardanian : XYZ, XZY, YZX, YXZ, ZXY, or ZYX.

➢ โดยทวไป เพออางถงการแสดงผล 3 มมทมทงหมด จะหมายถงมมออยเลอรซงม 12 แบบใหเลอก

➢ เมอจะอธบายการเคลอนทของยานพาหนะ เชน เรอ เครองบน และรถยนต โดยมากแลว แกน X จะชในทศทางไปขางหนา และแกน Z ชขนหรอลง และมล าดบการหมนดงน yaw (ทศทางการเคลอนท), pitch (ความสงของดานหนาเทยบกบแนวนอน) จากนนคอหมน roll (หมนรอบแกนดานหนาของยานพาหนะ) น าไปสมม ZYX = 𝑅𝑧(𝜃𝑦)𝑅𝑦(𝜃𝑝)𝑅𝑥(𝜃𝑟)

➢ ZYZ หมายถง 𝑅 = 𝑅𝑧(𝜙)𝑅𝑦(𝜃)𝑅𝑧(𝜓)

➢ roll − pitch − yaw angles = ZYX = 𝑅𝑧(𝜃𝑦)𝑅𝑦(𝜃𝑝)𝑅𝑥(𝜃𝑟)

➢ เมออธบายลกษณะกรปเปอรของหนยนตดงแสดงในรปท 13 โดยสวนมาก แกน z ชไปขางหนา และแกน x ชขนหรอลง น าไปสมม 𝑋𝑌𝑍 = 𝑅𝑥(𝜃𝑦)𝑅𝑦(𝜃𝑝)𝑅𝑧(𝜃𝑟)

3- Angle Representations

R = rotz(0.1) * roty(0.2) * rotz(0.3)

R = eul2r(0.1, 0.2, 0.3)

รปท 13

AxAyAz1

=𝐴𝑅𝐵 𝑡

01×3 1

BxByBz1

𝐴 𝑝 =𝐴𝑅𝐵 𝑡

01×3 1𝐵 𝑝 = 𝐴𝑇𝐵

𝐵 𝑝

โดยท 𝐴 𝑝 = Ax, Ay, Az, 1 , 𝐵 𝑝 = Bx, By, Bz, 1 และ𝐴𝑇𝐵 คอ 44 Homogeneous

Transformation Matrix ซงแทน translation และ orientationหรอ relative pose ()

Pose in 3D:Homogeneous Transformation Matrix

➢ Forward Kinematics

➢ 2D (Planar) Robotic Arms

➢ หนยนตในรป a ม Pose of the end-effector

เทากบ

➢ หนยนตในรป c ม Pose of the end-effector

เทากบ

➢ หนยนตในรป b ม Pose of the end-effector

เทากบ

Robot Arm Kinematicsimport ETS2.*

a1 = 1;

E = Rz('q1') * Tx(a1)

E.fkine( 45, 'deg')

E.teach

import ETS2.*

a1 = 1; a2=1;

E = Rz('q1') * Tx(a1)

* Rz('q2') * Tx(a2)

E.fkine( [30, 40],

'deg')

E.teach

import ETS2.*

a1 = 1;

E = Rz('q1') * Tx(a1)

* Tx('q2')

E.fkine(30, 0.5)

E.structure

➢ 3D Robotic Arms

➢ หนยนตในรป a ม Pose of the end-effector เทากบ

Robot Arm Kinematics

clear import

import ETS3.*

L1 = 0; L2 = -0.2337; L3 = 0.4318; L4 = 0.0203;

L5 = 0.0837; L6 = 0.4318;

E3 = Tz(L1) * Rz('q1') * Ry('q2') * Ty(L2) *

Tz(L3) * Ry('q3')* Tx(L4) * Ty(L5) * Tz(L6) *

Rz('q4') * Ry('q5') * Rz('q6');

E3.fkine([0 0 0 0 0 0])

Denavit-Hartenberg Notationชอ สญลกษณ ความหมาย คา

Joint angle 𝜽𝒋 มมระหวาง 𝒙𝒋−𝟏 และ 𝒙𝒋 รอบแกน 𝒛𝒋−𝟏 Revolute เปนตวแปร

Link offset 𝒅𝒋 ระยะทางจากจดก าเนดของเฟรม 𝒋 − 𝟏ถงแกน 𝒙𝒋 วดตามแนวแกน 𝒛𝒋−𝟏 Prismatic เปนตวแปร

Link length 𝒂𝒋 ระยะทางระหวางแกน 𝒛𝒋−𝟏 และ 𝒛𝒋 ตามแนวแกน 𝒙𝒋 ซงขนานกบ 𝒛𝒋−𝟏 × 𝒛𝒋 คาคงท

Link twist 𝜶𝒋 มมจากแกน 𝒛𝒋−𝟏 ถง 𝒛𝒋 รอบแกน 𝒙𝒋 คาคงท

Joint type 𝝈𝒋 𝝈 = 𝑹 ส าหรบ Revolute joint, 𝝈 = 𝑷 ส าหรบ Prismatic joint คาคงท

Denavit-Hartenberg Parameters

https://robotacademy.net.au/

mdl_puma560

p560 ; p560.plot(qz) ; p560.plot(qr) ; p560.teach

p560.fkine([pi/6 pi/6 pi/6 0 0 0])


d2

a2

d4

a3d6


X Y

Zขอบงคบ : แนวแกน xj ตองสามารถตดและตงฉากกบแกน zj−1


Y

Z

X

mdl_KR16_L6

KR16 ; KR16.plot(qz);

KR16.teach;

KR16.fkine([pi/6 pi/6 pi/6 0 0 0])

d1

a1

a2

a3

d4

d6

เลอนถอยหลง เลอนถอยหลง

DH Parameters for KUKA KR16_L6

d1

a2

a3

d4 d6

ABB IRB120

DH Parameters for ABB IRB120

Inverse Kinematics: UR5mdl_ur5

ur5

ur5.teach

ur5.fkine([pi/6 pi/6 pi/6 0 0 0])

X0Y0

Z0d1

a2

Z1

X1 Y1

Y4

X4

Z4X5

Z5

Y6

X6Z6

X3

Y3Z3

X2

Y2Z2

a3

d4

d5

d6

DH Parameters for UR5

djθj aj−1 alphaj−1

Robot Arm in 2D


clear

a1=1;a2=1;q1=0.2;q2=0.3;

trchain2('R(q1) Tx(a1) R(q2) Tx(a2)', [q1 q2])

syms q1 q2 a1 a2

E=trchain2('R(q1) Tx(a1) R(q2) Tx(a2)', [q1 q2])

mdl_planar2;p2.teach;p2.plot([0 pi/2]) ;p2.plot([pi/2 -pi/2])

➢ Forward & Inverse Kinematics

➢ mdl_planar2

Robot Arm in 2D

clear

mdl_planar2_sym; p2

syms q1 q2 real

TE = p2.fkine( [q1 q2] );

p = TE.t; p = p(1:2)

J = jacobian(p, [q1 q2])

clear

mdl_planar2

figure();p2.plot(qz)

T=transl(1,1,0);

q = p2.ikine(T, [0, 0], 'mask',[1, 1, 0, 0, 0, 0])

p2.plot(q); p2.teach

𝑑𝑝

𝑑𝑡= 𝐽(𝑞)

𝑑𝑞

𝑑𝑡

𝜈 = ሶ𝑝 = 𝐽(𝑞) ሶ𝑞ሶ𝑞 = 𝐽(𝑞)−1𝜈

syms a1 a2 a3 q1 q2 q3

trchain('Rz(q1)Tx(a1)Rz(q2)Tx(a2)Rz(q3)Tx(a3)',[q1 q2 q3])

Robot Arm in 2D

dh=[0 0 1 0;0 0 1 0]

r=SerialLink(dh)

r.plot([0.2,0.3])

r.teach

r.fkine([0.2 0.3])

Robot Arm in 3D


mdl_puma560; p560 ; p560.fkine([0.1 0.2 0.3 0 0 0])

p560.base = transl(10, 15, 2) ; p560.fkine([0.1 0.2 0.3 0 0 0])

Base & Tool Transform


T1 = transl2(1, 2) * trot2(30, 'deg'); % transl2(1, 2)creates a relative pose with a finite translation but zero rotation.

T2 = transl2(2, 1); % trot2 creates a relative pose with a finite rotation but zero translation.

T3 = T1*T2;

T4 = T2*T1;

plotvol([0 5 0 5]);

trplot2(T1, 'frame', '1', 'color', 'b')

trplot2(T2, 'frame', '2', 'color', 'r')

trplot2(T3, 'frame', '3', 'color', 'g')

trplot2(T4, 'frame', '4', 'color', 'c')

Matlab toolbox

plotvol([-5 4 -1 5]); T0 = eye(3,3);

trplot2(T0, 'frame', '0'); X = transl2(2, 3);

trplot2(X, 'frame', 'X'); R = trot2(2);

trplot2(R*X, 'framelabel', 'RX', 'color', 'r');

% frame {RX} rotated about the origin

trplot2(X*R, 'framelabel', 'XR', 'color', 'r');

% frame {XR} rotated about the origin of {X}

C = [1 2]';

plot_point(C, 'label', ' C', 'solid', 'ko')

RC = transl2(C) * R * transl2(-C)

%to rotate a coordinate frame about an arbitrary point C

trplot2(RC*X, 'framelabel', 'XC', 'color', 'r');

Matlab toolbox

➢ Given pose of the end-effector 𝜉𝐸 what are the required joint coordinates?

𝑞 = 𝒦−1(𝜉)

➢ Robot’s forward kinematics – the end-effector pose as a fn of the joint variables.

➢ Inverse kinematics does not have a unique solution

Inverse Kinematics : 2D (Planar) Robotic Arms

import ETS2.* %Elementary transform sequence in 2D

a1 = 1; a2 = 1;

E = Rz('q1') * Tx(a1) * Rz('q2') * Tx(a2)

syms q1 q2 real

TE = E.fkine( [q1, q2] ) % compute the forward kinematics

syms x y real

e1 = x == TE.t(1) % e1=x-coordinate of end-effector

e2 = y == TE.t(2) % e2=y-coordinate of end-effector

[s1,s2] = solve( [e1 e2], [q1 q2] )

➢ Closed-form inverse kinematics using the Denavit-Hartenberg model for

the Puma robot.

➢ Puma 560 is a 6-axis robot arm with a spherical wrist we use the method ikine6s

to compute the inverse kinematics using a closed-form solution

Inverse Kinematics: 3D Robotic Arms

mdl_puma560

qn

T = p560.fkine(qn)

qi = p560.ikine6s(T) % why qi not equal to qn

p560.fkine(qi)

Denavit-Hartenberg Notation

i-1

i-1 Link Twist : Measured in the right-hand sense about ai-1

ai-1

ai-1 Link Length : Mutual perpendicular

(unique except for parallel axis)

ai-1 , i-1 are constant


a i

di Link Offset : Variable if joint i is prismatic

d i

i Joint Angle : Variable if joint i is revolute

i

Denavit-Hartenberg Parameters_v2

X Y

Zขอบงคบ : แนวแกน xj ตองสามารถตดและตงฉากกบแกน zj−1

Velocity Transform

ถา Joints เคลอนทดวยความเรวคาหนงแลวความเรวของ End-effector จะมคาเทาใด

ความเรวของ Pose หรอ 𝒅𝝃𝑬

𝒅𝒕

ความเรวของ Joints หรอ 𝒅𝒒

𝒅𝒕

Velocity of robot in 2D

Velocity of robot in 2D

ሶξE = 𝐽 ሶq

Jacobian Matrix

Jacobian Matrix of robot in 2D

Jacobian = J(q)

J(q)-1

Joint angle velocity ตองเปนเทาใด เพอใหไดความเรว pose ทตองการ

J(q)-1

Singularity of 2D robot

Resolved-Rate Motion Control เราตองการให End-Effector เคลอนทดวยความเรว v ดงนน ตองใหแตละ Joint angle เคลอนทดวยความเรวเทาใด

q เปลยนอพเดท Jacobian

ท าซ าวนไป

Resolved-Rate Motion Control

Velocity of 3-joints Planar robot

Transform spatial vel. Between frames

Velocity of Robot’s End-effector

ถา Joints เคลอนทดวยความเรวคาหนงแลวความเรวของ End-effector จะมคาเทาใด

พืน้ฐานหุ่นยนต์ อุตสาหกรรม ·...

Documents