พืน้ ฐานหุน่ ยนต์

อุตสาหกรรม
Fundamental of
Industrial
Robotics

สันทัด ชูวงค์อินทร์ (Ph.D.)

วิทยาลัยนวัตกรรมการผลิตขั้นสูง
 Kinematics (ไคเนเมติกส์ )
Treatment of motion without regard to the forces that cause it.

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

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

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

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

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

http://petercorke.com
 Industrial Robot

https://www.yaskawa-global.com
 Degrees of Freedom
Degrees of Freedom of a Rigid Body in Space : วัตถุแข็งเกร็งใน 3 มิติ มี 6 องศาอิสระ ประกอบด้วย
การเลื่อนตามแนวแกน x, y และ z และการหมุนรอบแกน x, y และ z ตามลาดับ
 Number of joints and kinematic
computation of robots
แขนกลมี 6 DOF ไม่รวม gripper

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

แขนกลในรูปมีกี่ DOF?

https://www.yaskawa-global.com
แขนคนเรามี 7 DOF
 Kinematics

สมการไคเนเมติกส์สาหรับการคานวณ
positions (x, y, z) หรือ orientation
(RX, RY, RZ) ของจุด P จากค่ามุมของ
แต่ละข้อต่อของแขนกล
(S, θL, θU, θR, θB, และ θT)
เรียกว่า สมการไคเนเมติกส์ไปข้างหน้า
(Forward kinematics)
https://www.yaskawa-global.com
Kinematics
 Kinematics (ไคเนเมติกส์ ) จลนศาสตร์:กลศาสตร์𝜃ท1ี่เ,กี𝜃2่ย,วกั
𝜃3 รูบ
ค้ ่าการเคลือ
่ นที่
• Forward Kinematics : การคานวณหาตาแหน่ง
(position: x,y,z ) และทิศทาง (orientation:
หา 𝑥, 𝑦, 𝑧,
𝑅𝑥 , 𝑅𝑦 , 𝑅𝑧 ) ของกริ ปเปอร์หรื อทูล (tool) ของหุ่นยนต์
𝑅𝑥 , 𝑅𝑦, 𝑅𝑧ของกริปเปอร์
Forward Kinematics
ที่สมั พัทธ์กบั เวิร์คสเตชัน่ ของผูใ้ ช้งาน เมื่อรู ้ค่ามุมของ
ข้อต่อต่างๆ (joint angles : θ1 , θ2 , θ3 ) ของแขนกล รูปที่ 1 ทูลเฟรมสัมพัทธ์กบั เบสเฟรมโดยเป็ น
ฟั งก์ชนั ของตัวแปรของข้อต่อต่างๆ Inverse Kinematics
(manipulator) หาค่า 𝜃1 , 𝜃2 , 𝜃3

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

รู ้ 𝑥, 𝑦, 𝑧,
ของแขนกล เมื่อกาหนดตาแหน่ง และทิศทางของกริ ปเปอร์ 𝑅𝑥 , 𝑅𝑦, 𝑅𝑧ของกริปเปอร

หรื อทูลของแขนกลที่สมั พัทธ์กบั เวิร์คสเตชัน่ หรื อเบสมาให้

รูปที่ 2 ตาแหน่งปลายเดียวกันแต่มีค่ามุมของข้อต่อที่ต่างกันของ Inv.

 Standard frames

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

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

 Position + Orientation = pose
• ถ้าติดโคออร์ดิเนตเฟรมเข้ากับวัตถุ (กรณี น้ ีเป็ นรถ) เราสามารถอธิ บายทุกจุดในวัตถุดว้ ยเวกเตอร์ คงที่เทียบกับ
เฟรมนั้น{B}
• สามารถอธิ บาย Position และ Orientation (Position + Orientation = Pose) ของจุด P ใดๆ
บนโคออร์ดิเนต {B} เทียบกับโคออร์ดิเนตเฟรมอ้างอิง {O}หรื อ เวิลด์โคออร์ดิเนตเฟรม
• เพื่อแยกความแตกต่างของเฟรมที่กาหนดขึ้น ในกรณี น้ ีจะใช้สัญลักษณ์แสดงโคออร์ดิเนตเฟรมของวัตถุ {B}
และแกนของมันคือ xB และ yB โดยตัวห้อยแสดงถึงเฟรม B ดังรู ปที่ 1

รู ปที่ 1
 Position + Orientation = pose
• เพื่ออธิ บายถึง Pose ของวัตถุแข็งเกร็ งใน 3 มิติ เราต้องการถึง 6 DOF โดยที่ 3 เพื่ออธิ บาย Position
(x,y,z) และอีก 3 เพื่ออธิ บาย Orientation (𝑅𝑥, 𝑅𝑦 , 𝑅𝑧 )
• DOF เหล่านี้มีความแตกต่างกันมาก ถ้าเราเพิ่มค่า Position วัตถุจะเคลื่อนที่ต่อเนื่องเป็ นเส้นตรง แต่ถา้ เรา
เพิม่ ค่า Orientation วัตถุจะหมุนไปและในที่สุด วัตถุจะกลับมายัง Orientation เดิม โดยมีแนวการ
หมุนเป็ นเส้นโค้ง ดังนั้น เราจาเป็ นต้องจัดการกับทั้งสองส่ วนนี้แตกต่างกัน
• Pose ของโคออร์ดิเนตเฟรมจะถูกแสดงด้วยสัญลักษณ์ ξ (ออกเสี ยงว่า “ไซ”)

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

ซึ่ งอธิ บาย Pose จาก{B} เทียบกับ {A} โดยที่ ตัวยกหมายถึง
โคออร์ดิเนตเฟรมอ้างอิง และตัวห้อยหมายถึงเฟรมที่กาลังถูกอธิ บาย
• B อาจถูกมองว่าเป็ นการอธิ บายการเคลื่อนไหวบางอย่าง –
Aξ

จินตนาการว่า ยกขึ้น {A} และเลื่อนระยะแล้วหมุนเพื่อให้เปลี่ยน

กลายเป็ น {B} ถ้าไม่มีตวั ยก เช่น ξB จะหมายถึงการเปลี่ยนแปลง
Pose ที่เทียบกับเวิลด์ โคออร์ดิเนตเฟรมซึ่ งโดยทัว่ ไปแสดงด้วย รู ปที่ 2
สัญลักษณ์ {O}
 Position + Orientation = pose
• จุด P ในรู ปที่ 2 เทียบกับทั้งสองโคออร์ดิเนตเฟรมโดยเวกเตอร์ 𝐴𝑝 หรื อ 𝐵𝑝 ตามลาดับ สัมพันธ์กนั ด้วย
สมการ 𝐴𝑝 = 𝐴𝜉𝐵 ∙ 𝐵𝑝
โดยที่ดา้ นขวาของสมการ แสดงถึงการเคลื่อนที่จากเฟรม{A}ถึง{B} และจากนั้นไปยังจุด P โอเปอเรเตอร์ ∙ ทา
การแปลงเวกเตอร์ให้เป็ นเวกเตอร์ใหม่ที่อธิ บายจุด P เดียวกันแต่เทียบกับโคออร์ดิเนตเฟรมอื่นในกรณี น้ ีคือ{B}
• ในรู ปที่ 3 ถ้าเฟรมหนึ่งถูกอธิ บายในเทอมของอีกเฟรมหนึ่งด้วย Relative Pose ดังนั้นเฟรมทั้งหมด
สัมพันธ์กนั ด้วยสมการ 𝐴𝜉𝐶 = 𝐴𝜉𝐵 ⨁ 𝐵𝜉𝐶
โดยที่ Pose ของเฟรม {C} เทียบกับ {A} สามารถหาได้โดยการผสม Relative Pose จากเฟรม {B} เทียบ
{A} และจากเฟรม {C} เทียบ{B} เราใช้โอเปอเรเตอร์ ⊕ เพื่อแสดงการประกอบกันของ Relative Pose

• ในการประกอบกันของ Relative Pose ตัวห้อยและตัวยกในแต่ละด้านของโอเปอเรเตอร์ ⨁ ถ้าจับคู่กนั

ได้ เราสามารถตัดตัวห้อยและตัวยกที่อยูต่ รงกลางทิ้งได้

รู ปที่ 3
รู ปที่ 2
 Multiple 3D coordinate
frames and relative poses
• รู ปตัวอย่าง ที่ใส่ โคออร์ดิเนตเฟรม 3 มิติเข้ากับสิ่ งต่างๆ เช่น กล้อง วัตถุ หุ่นยนต์ และเวิลด์โคออร์ดิเนตเฟรม เป็ น
ต้น และแสดง Relative Poses ของแต่ละอัน (𝜉𝐹 , 𝜉𝐵 , 𝜉𝑅 , 𝐹𝜉𝐵 , 𝐶𝜉𝐵 , 𝑅𝜉𝐶 เป็ นต้น)
• กล้องที่อยูก่ บั ที่มองเห็นวัตถุจากมุมมองของมันและจะประมาณค่า Pose ของวัตถุเป็ น 𝐹𝜉𝐵 เมื่อเทียบกับตัวมัน
ส่ วนกล้องอีกอันที่ไม่อยูก่ บั ที่ถูกติดตั้งไว้กบั หุ่นยนต์ซ่ ึ งมีค่า Relative Pose ค่าหนึ่งและจะประมาณค่า
Pose ของวัตถุเป็ น 𝐶 𝜉𝐵 เมื่อเทียบกับตัวมัน

รู ปที่ 5

𝜉𝐹 ⨁ 𝐹𝜉𝐵 = 𝜉𝑅 ⨁ 𝑅𝜉𝐶 ⨁ 𝐶𝜉𝐵

รู ปที่ 4
𝜉𝐹 ⨁ 𝐹𝜉𝑅 = 𝜉𝑅

หมายเหตุ ลูกศรจาก {X} ไป {Y} แสดงเป็ น 𝑿𝝃𝒀 อธิบาย Pose ของเฟรม Y เทียบกับ X
 Representations of pose ξ

แมพ โคออร์ดิเนตเว็คเตอร์จาก
เฟรม {B} ไปหาเฟรม {A} Pose ของเฟรม {B} เทียบกับเฟรม {A}
(ลูกศรจากเฟรม {A} ไปเฟรม {B})
 Working in 2D
• จุด 𝑝 = 𝑥ෝ ෝ เช่น 1ෝ
𝒙+𝑦𝔂 ෝ หรื อ −2ෝ
𝒙 + 2𝔂 ෝ
𝒙 + 0.5𝔂
• รู ปแสดงโคออร์ดิเนตเฟรมสี แดง{B}ที่เราต้องการอธิ บายเมื่อเทียบกับโคออร์ดิเนตเฟรมอ้างอิงสี ฟ้า{A}
• จะเห็นได้ชดั เจนว่าจุดกาเนิดของ{B}ถูกแทนด้วยเว็คเตอร์ 𝑡 = (𝑥, 𝑦)จากนั้นหมุนทวนเข็มนาฬิกาด้วยมุม 

𝐴
𝜉𝐵 อยูใ่ นเทอมของ (𝑥, 𝑦, 𝜃)
รู ปที่ 6

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

Orientation in 2D: Rotation Matrix
1. พิจารณา Rotation หรือ การหมุนก่อน
• สร้างเฟรมใหม่ {V} ที่แกนทั้งสองขนานกับ{A} แต่มีจุดกาเนิดอยูท่ ี่{B}ดังรู ป
• แสดงจุด P เมื่อเทียบกับ {V} ในรู ปของเว็คเตอร์หนึ่งหน่วยของแกนทั้งสองได้ดงั นี้
𝑉𝑥
ෝ 𝑉 + 𝑉𝑦 𝔂
𝑉𝑝 = 𝑉𝑥 𝔁 ෝ𝑉
ෝ𝑉 = 𝔁 ෝ𝑉
𝔂
𝑉𝑦

• ดังนั้นโคออร์ดิเนตเฟรม{B} ถูกแสดงด้วยเวกเตอร์หนึ่งหน่วย 𝔁 ෝ𝑉 ได้ดงั นี้

ෝ𝑉 , 𝒚
ෝ 𝑉 และ 𝑦𝐵 = −𝑠𝑖𝑛 𝜃 𝔁
ෝ 𝑉 + 𝑠𝑖𝑛 𝜃 𝔂
𝑥𝐵 = 𝑐𝑜𝑠 𝜃 𝔁 ෝ𝑉
ෝ 𝑉 + 𝑐𝑜𝑠 𝜃 𝔂
 Orientation in 2D
➢ 𝑥𝐵 = 𝑐𝑜𝑠 𝜃 𝔁 ෝ𝑉
ෝ 𝑉 + 𝑠𝑖𝑛 𝜃 𝔂
➢ 𝑦𝐵 = −𝑠𝑖𝑛 𝜃 𝔁 ෝ𝑉
ෝ 𝑉 + 𝑐𝑜𝑠 𝜃 𝔂

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

𝐵𝑥
➢ สามารถแสดงจุด P เมื่อเทียบกับ{B}ด้วย 𝐵𝑃 = 𝐵𝑥 𝔁 ෝ𝐵
ෝ𝐵 = 𝔁
ෝ 𝐵 + 𝐵𝑦 𝔂 ෝ𝐵
𝔂 𝐵𝑦
 Matlab toolbox for Pose in 2D: 𝐴
𝑇𝐵
𝐴 cos 𝜃 − sin 𝜃 𝑥 0.2588 −0.9659 1.5
𝐴 𝑅𝐵 𝑡
𝑇𝐵 = = sin 𝜃 cos 𝜃 𝑦 = 0.9659 0.2588 1.5
01×2 1
0 0 1 0 0 1
ตัวอย่าง เพื่อหาโคออร์ดิเนตของจุด 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]
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}
 Orientation in 2D
➢ 𝐵
ෝ𝐵
𝑃= 𝔁 ෝ𝐵
𝔂 𝐵𝑥
𝐵𝑦
ෝ𝑉
= 𝔁 ෝ 𝑉 𝑐𝑜𝑠 𝜃
𝔂
−𝑠𝑖𝑛 𝜃 𝐵𝑥
𝐵𝑦
𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃

𝑉𝑥 𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃 𝐵𝑥
➢ 𝑉𝑦
= 𝐵𝑦
𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃
➢ สมการข้างบน ใช้อธิบายจุด P หรื อเว็คเตอร์ P ซึ่งถูกแปลงจากเฟรม{B}ไปหา{V} เมื่อ{B}ถูกหมุน
โดยเมทริ กซ์ขนาด 22 นี้จะถูกเรี ยกว่า เมทริ กซ์การหมุน (Rotation Matrix) และถูกแสดงด้วย 𝑉𝑅𝐵
𝑉
➢ 𝑅𝐵 แมพเว็คเตอร์จากเฟรม{B}ไปหา{V}แต่เป็ นฟั งก์ชนั ของการหมุนจากเฟรม{V}ไปหา
{B}

𝑉𝑥 𝐵𝑥
➢ 𝑉𝑦
= 𝑉𝑅𝐵 𝐵𝑦
 Matlab toolbox for Orientation in 2D
𝑉𝑥 𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃 𝐵𝑥
𝑉𝑦
= 𝐵𝑦
สมมุติวา่ θ = 30
𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃

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');
 Orientation in 2D
𝑋 𝑐𝑜𝑠 𝜃 −𝑠𝑖𝑛 𝜃
➢ 𝑅𝑌 =
𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃
𝑉 −1 𝑉 𝑇
➢ 𝑉𝑥
𝑉𝑦
= 𝑉𝑅𝐵 𝐵𝑥
𝐵𝑦
𝑅𝐵 = 𝑅𝐵
𝐵
= 𝑅𝑉
𝐵𝑥 𝑉 −1 𝑉𝑥 𝑉 𝑇 𝑉𝑥 𝑉𝑥
➢ 𝐵𝑦
= 𝑅𝐵 𝑉𝑦
= 𝑅𝐵 𝑉𝑦
= 𝐵𝑅𝑉 𝑉𝑦

Rotation Matrix มีคณ

ุ สมบัติพิเศษดังต่อไปนี ้
1. เป็ น orthonormal (หรือเรียกอีกอย่างว่า orthogonal = ตัง้ ฉาก) เนื่องจากแต่ละคอลัมน์
เป็ นเวกเตอร์หนึ่งหน่วยและคอลัมน์ทงั้ สองตัง้ ฉากกันด้วย (dot product เป็ นศูนย์)
2. ดีเทอร์มิแนนต์ det ( 𝐴𝑅𝐵 ) มีคา่ = +1 (คูณเว็คเตอร์ใดๆด้วยเมทริกซ์นีข้ นาดเว็คเตอร์ไม่เปลี่ยน)

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

 Pose in 2D:Homogeneous
Transformation Matrix
2. พิจารณา Translation หรือ การเลื่อน
➢ การเลื่อน (Translation) ของจุดกาเนิดของเฟรมแสดงในรู ปที่ 8 เนื่องจากแกนของเฟรม {V}
และ {A} ขนานกันการเลื่อนจึงเป็ นเพียงการบวกเวกเตอร์ธรรมดา
Ax Vx x B x x Bx
cos θ −sin θ x cos θ − sin θ By
= + = + = y
Ay Vy y sin θ cos θ By y sin θ cos θ
1
บวกกัน
Ax 𝐴 Bx Homogeneous
𝑅𝐵 𝑡
Ay = By โดยที่ 𝑡 คือ (x,y)
01×2 1 Transformation
1 1
Matrix

รู ปที่ 8 𝐴 𝑉
𝑅𝐵 = 𝑅𝐵
 Pose in 2D: 𝐴
𝑇𝐵
Ax 𝐴 Bx
𝑅𝐵 𝑡
Ay = By
01×2 1
1 1
𝐴 𝐴 𝐵
𝑝෤ = 𝑇𝐵 𝑝෤
โดยที่ 𝐴𝑝෤ = Ax , Ay , 1 , 𝐵𝑝෤ = Bx , By , 1 และ 𝐴𝑇𝐵 คือ Homogeneous Transformation
Matrix ซึ่ งแทน translation + orientation หรื อ relative pose ()

𝐴 cos 𝜃 − sin 𝜃 𝑥
𝐴 𝑅𝐵 𝑡
𝑇𝐵 = = sin 𝜃 cos 𝜃 𝑦
01×2 1
0 0 1
 Matlab toolbox for Pose in 2D: 𝐴
𝑇𝐵
𝐴 cos 𝜃 − sin 𝜃 𝑥
𝐴 𝑅𝐵 𝑡
𝑇𝐵 = = sin 𝜃 cos 𝜃 𝑦
01×2 1
0 0 1
T1 = transl2(1, 2) * trot2(30, 'deg') sysm x y theta
T2 = transl2(2, 1) T1 = transl2(x, y) * trot2(theta)
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]
➢ เพื่อหาโคออดิเนตของจุดเทียบกับ {1}
0
➢ 𝑃 = 0𝑇1 ∙ 1𝑃
1 0 −1
➢ 𝑃= 𝑇1 ∙ 0𝑃 → 𝑃1=inv(T1)*[3; 2;
1]
 Centers of Rotation
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')

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

ในขณะที่เฟรม{XR}หมุนรอบจุดกาเนิดของเฟรม{X}
0
➢ 𝑝 = 0𝜉1 ∙ 1𝑝
1 0 −1
➢ 𝑝= 𝜉1 ∙ 0𝑝
 Working in 3D
• จุด P แสดงด้วยพิกดั x, y และ z (x, y, z) หรื อเวกเตอร์ 𝑝 = 𝑥ෝ𝒙 + 𝑦𝔂
ෝ + zෝ
𝔃

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

• รู ปที่ 10 แสดงโคออร์ดิเนตเฟรมสี แดง {B} ที่เราต้องการอธิบายเมื่อเทียบกับโคออร์ดิเนตเฟรมอ้างอิง

สี ฟ้า {A} เห็นได้วา่ จุดกาเนิดของ {B} ถูกย้ายที่ไปด้วยเวกเตอร์ t = (x, y, z) แล้วหมุนใน
รู ปแบบที่ซบั ซ้อน (𝑅𝑥 (𝜃𝑥 )=? , 𝑅𝑦 (𝜃𝑦 )=? , 𝑅𝑧 (𝜃𝑧 )=? )
• วิธีการคือการพิจารณาจุด P ใดๆเทียบกับแต่ละโคออร์ดิเนตเฟรม และหาความสัมพันธ์ระหว่าง 𝐴𝑝
และ 𝐵𝑝 เราจะพิจารณาปัญหาเป็ นสองส่ วนเหมือน 2D คือ การหมุน และการเลื่อน
รู ปที่ 9

รู ปที่ 10
 Orientation in 3D
➢ จินตนาการว่าหยิบเฟรม {A} ไว้ในมือของเราและหมุนไปเรื่ อย ๆจนกระทัง่ มันดูเหมือนเฟรม{B}

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

y และ z) ไม่เกิน 3 ครั้งของการหมุนแกนโคออร์ดิเนตโดยต้องไม่หมุนแกนเดียวกัน 2 ครั้งติดกัน”

➢ ในการหมุน 3 มิติสลับลาดับการหมุนรอบแกนใดๆ จะให้ผลลัพธ์ไม่เท่ากัน ดังแสดงในรู ปที่ 11-12

รู ปที่ 10
Orientation in 3D
รู ปที่ 11 การหมุนโคออร์ดิเนตเฟรม 3 มิติ a=โคออร์ดิเนตเฟรมเดิม,
b ถึง f =โคออร์ดิเนตเฟรมหลังจากการหมุนแบบต่างๆตามที่ระบุไว้
 Orientation in 3D

รู ปที่ 12 ตัวอย่างที่แสดงการ
สลับการหมุนให้ผลลัพธ์ไม่
เท่ากัน แถวด้านบนโคออร์ดิเนต
𝜋
เฟรมถูกหมุนไป เรเดียนรอบ
2
แกน x และจากนั้นถูกหมุนไป
𝜋
เรเดียนรอบแกน y ในแถว
2
ล่างลาดับของการหมุนสลับกัน
ผลที่ได้แตกต่างกัน
 Orientation in 3D: Orthonormal
Rotation Matrix
➢ เวกเตอร์แต่ละตัวมี 3องค์ประกอบและสร้างคอลัมน์ของ เมทริ กซ์ 𝐴
𝑅𝐵 ขนาด 33 แบบ orthonormal
Ax Bx
Ay = 𝐴𝑅𝐵 By
Az Bz
ซึ่งจะแปลงเวกเตอร์ใน(เทียบกับ)เฟรม {B} ไปเป็ นเวกเตอร์ใน(เทียบกับ) เฟรม{A}
3D-Rotation Matrix 𝐴𝑅𝐵 มีคณ
ุ สมบัติพิเศษดังต่อไปนี ้
1. เป็ น orthonormal (หรือเรียกอีกอย่างว่า orthogonal = ตัง้ ฉาก) เนื่องจากแต่ละคอลัมน์
เป็ นเวกเตอร์หนึ่งหน่วยและคอลัมน์ทงั้ สองตัง้ ฉากกันด้วย (dot product เป็ นศูนย์)
2. คอลัมน์เป็ นเวกเตอร์หนึ่งหน่วย ที่นิยามแกนของเฟรม B ที่หมุนเทียบกับ A
3. ดีเทอร์มิแนนต์ det ( 𝐴𝑅𝐵 ) มีคา่ = +1 (คูณเว็คเตอร์ใดๆด้วยเมทริกซ์นีข้ นาดเว็คเตอร์ไม่เปลี่ยน)

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

 Orientation in 3D: Orthonormal
Rotation Matrix
➢ The orthonormal rotation matrices สาหรับการหมุน θ รอบแกน 𝑥, 𝑦 หรื อ 𝑧 คือ
1 0 0
𝑅𝑥 0° = 𝑅𝑦 0° = 𝑅𝑧 0° = 0 1 0
0 0 1
syms theta
𝑋𝑥 𝑌𝑥 𝑍𝑥 Rx = rotx(theta)
𝑅 𝜃 = 𝑋𝑦 𝑌𝑦 𝑍𝑦 Ry = roty(theta)
𝑋𝑧 𝑌𝑧 𝑍𝑧 Rz = rotz(theta)
Demonstrate axes toolbox R = rotx(90)*roty(90)
trplot(R);tranimate(R)
inv(R)
R’
det(R)
➢ Toolbox มีฟังก์ชนั ในการคานวณเมทริ กซ์การหมุนเหล่านี้ท้ งั 𝑅𝑥 𝜃 , 𝑅𝑦 𝜃 , 𝑅𝑧 (𝜃)
➢ The orthonormal rotation matrices มี 9 ค่า แต่ไม่ได้เป็ นอิสระต่อกัน 3 ข้อจากัดแรกคือใน
แต่ละคอลัมน์มีขนาดหนึ่งหน่วย 3 ข้อจากัดที่สองคือแต่ละคอลัมน์ต้งั ฉากซึ่ งกันและกัน
 3- Angle Representations
➢ ทฤษฎีบทการหมุนของออยเลอร์ จาเป็ นต้องหมุน 3แกน ต่อเนื่องกันโดยที่ไม่หมุนแกนเดียวกัน 2 ครั้ง
ต่อเนื่องกัน การหมุนมี 2 แบบ: Eulerian and Cardanian
➢ Eulerian : XYX, XZX, YXY, YZY, ZXZ, or ZYZ
➢ Cardanian : XYZ, XZY, YZX, YXZ, ZXY, or ZYX. รู ปที่ 13
➢ โดยทัว่ ไป เพื่ออ้างถึงการแสดงผล 3 มุมที่มีท้งั หมด จะหมายถึงมุมออยเลอร์ซ่ ึ งมี 12 แบบให้เลือก
➢ เมื่อจะอธิ บายการเคลื่อนที่ของยานพาหนะ เช่น เรื อ เครื่ องบิน และรถยนต์ โดยมากแล้ว แกน X จะชี้ในทิศทางไป
ข้างหน้า และแกน Z ชี้ข้ ึนหรื อลง และมีลาดับการหมุนดังนี้ yaw (ทิศทางการเคลื่อนที่), pitch (ความสู งของ
ด้านหน้าเทียบกับแนวนอน) จากนั้นคือหมุน roll (หมุนรอบแกนด้านหน้าของยานพาหนะ) นาไปสู่ มุม
ZYX = 𝑅𝑧 (𝜃𝑦 )𝑅𝑦 (𝜃𝑝 )𝑅𝑥 (𝜃𝑟 ) R = rotz(0.1) * roty(0.2) * rotz(0.3)
R = eul2r(0.1, 0.2, 0.3)
➢ ZYZ หมายถึง 𝑅 = 𝑅𝑧 (𝜙)𝑅𝑦 (𝜃)𝑅𝑧 (𝜓)
➢ roll − pitch − yaw angles = ZYX = 𝑅𝑧 (𝜃𝑦 )𝑅𝑦 (𝜃𝑝 )𝑅𝑥 (𝜃𝑟 )
➢ เมื่ออธิ บายลักษณะกริ ปเปอร์ของหุ่นยนต์ดงั แสดงในรู ปที่ 13 โดยส่ วนมาก แกน z ชี้ไปข้างหน้า และแกน x ชี้
ขึ้นหรื อลง นาไปสู่ มุม 𝑋𝑌𝑍 = 𝑅𝑥 (𝜃𝑦 )𝑅𝑦 (𝜃𝑝 )𝑅𝑧 (𝜃𝑟 )
 Pose in 3D:Homogeneous
Transformation Matrix
Ax Bx
𝐴
Ay 𝑅𝐵 𝑡 By
=
Az 01×3 1 Bz
1 1
𝐴
𝐴 𝑅𝐵 𝑡 𝐵
𝑝෤ = 𝑝෤ = 𝐴𝑇𝐵 𝐵𝑝෤
01×3 1
โดยที่ 𝐴𝑝෤ = Ax , Ay , Az , 1 , 𝐵𝑝෤ = Bx , By , Bz , 1 และ 𝐴𝑇𝐵 คือ 44 Homogeneous
Transformation Matrix ซึ่ งแทน translation และ orientation หรื อ relative pose ()
Robot Arm Kinematics import ETS2.*

➢ Forward Kinematics a1 = 1;

➢ 2D (Planar) Robotic Arms E = Rz('q1') * Tx(a1)

E.fkine( 45, 'deg')
➢ หุ่นยนต์ในรู ป a มี Pose of the end-effector
เท่ากับ E.teach

➢ หุ่นยนต์ในรูป c มี Pose of the end-effector import ETS2.*

เท่ากับ a1 = 1; a2=1;

➢ หุ่นยนต์ในรูป b มี Pose of the end-effector E = Rz('q1') * Tx(a1)

* Rz('q2') * Tx(a2)
เท่ากับ
E.fkine( [30, 40],
import ETS2.* 'deg')
a1 = 1;
E.teach
E = Rz('q1') * Tx(a1)
* Tx('q2')
E.fkine(30, 0.5)
E.structure
Robot Arm Kinematics
➢ 3D Robotic Arms
➢ หุ่นยนต์ในรู ป a มี Pose of the end-effector เท่ากับ

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
mdl_puma560
p560 ; p560.plot(qz) ; p560.plot(qr) ; p560.teach
p560.fkine([pi/6 pi/6 pi/6 0 0 0])

https://robotacademy.net.au/
Denavit-Hartenberg Parameters

a2

d2 d4
a3
d6
 Denavit-Hartenberg Parameters

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

Z

X Y
 Denavit-Hartenberg Parameters
mdl_KR16_L6
d4
KR16 ; KR16.plot(qz);
KR16.teach; d6

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

a3
a2 เลื่อนถอยหลัง เลื่อนถอยหลัง

d1 Z

Y X

a1
DH Parameters for KUKA KR16_L6
 ABB IRB120
d4 d6

a3

a2

d1
DH Parameters for ABB IRB120
Inverse Kinematics: UR5
mdl_ur5
ur5
ur5.teach
ur5.fkine([pi/6 pi/6 pi/6 0 0 0])
 d6 d4
Y6
Z6 Z5
d5 X6
Z4 Y4
X5
X4
a3

Y3
Z3

X3
a2
Z1
Z2 Y2
X1 Y1
d1 Z0
X2 Y0
X0
DH Parameters for UR5

θj dj 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])

https://robotacademy.net.au/
 Robot Arm in 2D
➢ Forward & Inverse Kinematics 𝑑𝑝 𝑑𝑞
= 𝐽(𝑞)
➢ mdl_planar2 𝑑𝑡 𝑑𝑡
clear
mdl_planar2_sym; p2 𝜈 = 𝑝ሶ = 𝐽(𝑞)𝑞ሶ
syms q1 q2 real 𝑞ሶ = 𝐽(𝑞)−1 𝜈
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
syms a1 a2 a3 q1 q2 q3
Robot Arm in 2D
trchain('Rz(q1)Tx(a1)Rz(q2)Tx(a2)Rz(q3)Tx(a3)',[q1 q2 q3])
 Robot Arm in 3D
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])

https://robotacademy.net.au/
 Base & Tool Transform

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

https://robotacademy.net.au/ p560.base = transl(10, 15, 2) ; p560.fkine([0.1 0.2 0.3 0 0 0])
 Matlab toolbox
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');
 Inverse Kinematics : 2D (Planar)
Robotic Arms
➢ 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

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] )
Inverse Kinematics: 3D Robotic Arms
➢ 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

mdl_puma560
qn
T = p560.fkine(qn)
qi = p560.ikine6s(T) % why qi not equal to qn
p560.fkine(qi)
 Denavit-Hartenberg Notation

ai-1
i-1

ai-1 Link Length : Mutual perpendicular

(unique except for parallel axis)
i-1 Link Twist : Measured in the right-hand sense about ai-1
a  are constant
Denavit-Hartenberg Notation
 Denavit-Hartenberg Notation

ai

i
di

di Link Offset : Variable if joint i is prismatic

i Joint Angle : Variable if joint i is revolute
 Denavit-Hartenberg Parameters_v2

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

Z

X Y
 Velocity Transform
ถ้า Joints เคลือ่ นทีด่ ว้ ยความเร็วค่าหนึ่งแล้วความเร็วของ End-effector จะมีค่าเท่าใด
𝒅𝝃𝑬
ความเร็วของ Pose หรือ
𝒅𝒕

𝒅𝒒
ความเร็วของ Joints หรือ
𝒅𝒕
Velocity of robot in 2D
Velocity of robot in 2D

Jacobian Matrix

ሶ
ξE = 𝐽qሶ
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
Velocity of 3-joints Planar robot
Velocity of 3-joints Planar robot
Velocity of 3-joints Planar robot
Velocity of 3-joints Planar robot
Transform spatial vel. Between frames
 Velocity of Robot’s End-effector
ถ้า Joints เคลือ่ นทีด่ ว้ ยความเร็วค่าหนึ่งแล้วความเร็วของ End-effector จะมีค่าเท่าใด