קורס רובוטיקה ובינה מלאכותית Inverse Kinematics (IK) – : 01-RB302

פרק 2 – Inverse Kinematics (IK)

1. מבוא

Inverse Kinematics, או בקיצור IK, היא השיטה שבה מחשבים את זוויות המפרקים של הרובוט לפי מיקום רצוי במרחב.

ב־Forward Kinematics התחלנו מזוויות המפרקים:

J0, J1, J2

1	J0, J1, J2

וחישבנו את מיקום קצה הזרוע:

P3 = (X, Y, Z)

1	P3 = (X, Y, Z)

ב־Inverse Kinematics עושים את הפעולה ההפוכה:

P3 = (X, Y, Z)
        ↓
        IK
        ↓
J0, J1, J2

P3 = (X, Y, Z)

↓

J0, J1, J2

כלומר:

אנחנו יודעים לאן אנחנו רוצים שהרובוט יגיע, ומחשבים באילו זוויות צריך להציב את המפרקים.

2. מתי משתמשים ב־IK?

משתמשים ב־IK כאשר אנחנו יודעים את מיקום היעד, אבל לא יודעים מראש את זוויות המפרקים.

לדוגמה:

הרובוט צריך להגיע לקופסה שנמצאת בנקודה:

X = 190.5 mm
Y = 110.0 mm
Z = 211.0 mm

X = 190.5 mm

Y = 110.0 mm

Z = 211.0 mm

אנחנו רוצים לחשב:

J0 = ?
J1 = ?
J2 = ?

J0 = ?

J1 = ?

J2 = ?

כך הבקר יכול להזיז את המנועים לזוויות הדרושות.

שימושים נפוצים:

תפיסת חפץ
הנחת חפץ במקום
ריתוך
קידוח
Pick and Place
תנועה לנקודת XYZ
שליטה ב־End Effector
תכנון מסלול לזרוע רובוטית

3. למה צריך IK?

רובוט לא מקבל בדרך כלל פקודה כמו:

סובב J1 ל־40°

1	סובב J1 ל־40°

אלא פקודה ברמה גבוהה יותר, למשל:

העבר את הגריפר לנקודה:
X = 190
Y = 110
Z = 211

העבר את הגריפר לנקודה:

X = 190

Y = 110

Z = 211

כדי לבצע את הפקודה, המערכת צריכה לתרגם את המיקום לזוויות.

לכן IK הוא החיבור בין:

מיקום במרחב

1	מיקום במרחב

לבין:

תנועת המפרקים

1	תנועת המפרקים

4. מבנה הזרוע

נשתמש באותה זרוע:

●

/ L2

●

/ L1

●

| H

●

הפרמטרים הם:

סימון	משמעות
`P0`	נקודת הבסיס
`P1`	נקודת הכתף
`P2`	נקודת המרפק
`P3`	נקודת קצה הזרוע
`J0`	סיבוב הבסיס
`J1`	זווית הכתף
`J2`	זווית המרפק
`H`	גובה הכתף
`L1`	אורך הזרוע הראשונה
`L2`	אורך הזרוע השנייה

5. נקודות P

הנקודות במערכת הן:

P0 = (0, 0, 0)

1	P0 = (0, 0, 0)

P1 = (0, 0, H)

1	P1 = (0, 0, H)

P2 = מיקום המרפק

1	P2 = מיקום המרפק

P3 = (X, Y, Z)

1	P3 = (X, Y, Z)

ב־IK הנקודה החשובה ביותר היא:

P3

כי היא נקודת היעד של ה־End Effector.

6. שלב 1 – חישוב J0

J0 הוא סיבוב הבסיס.

הוא קובע לאיזה כיוון במישור XY הזרוע צריכה לפנות.

הנוסחה:

J0 = atan2(Y, X)

1	J0 = atan2(Y, X)

לדוגמה:

X = 190.5
Y = 110.0

1 2	X = 190.5 Y = 110.0

לכן:

J0 = atan2(110.0, 190.5)

1	J0 = atan2(110.0, 190.5)

בקירוב:

J0 ≈ 30°

J0 ≈ 30°

כלומר הבסיס צריך להסתובב בערך 30 מעלות.

7. שלב 2 – חישוב המרחק האופקי r

אחרי שסובבנו את הבסיס, מחשבים את המרחק האופקי מהבסיס אל המטרה.

r = √(X² + Y²)

1	r = √(X² + Y²)

בדוגמה:

r = √(190.5² + 110²)

1	r = √(190.5² + 110²)

ולכן:

r ≈ 220 mm

1	r ≈ 220 mm

עכשיו אפשר להתייחס לבעיה כבעיה דו־ממדית.

8. שלב 3 – חישוב הגובה היחסי z

P1 נמצאת בגובה:

H

לכן הגובה של המטרה יחסית לכתף הוא:

z = Z - H

z = Z - H

לדוגמה:

Z = 211
H = 80

1 2	Z = 211 H = 80

ולכן:

z = 211 - 80

1	z = 211 - 80

z = 131 mm

1	z = 131 mm

9. שלב 4 – חישוב המרחק d

עכשיו מחשבים את המרחק הישר מ־P1 אל P3.

d = √(r² + z²)

1	d = √(r² + z²)

בדוגמה:

d = √(220² + 131²)

1	d = √(220² + 131²)

ולכן:

d ≈ 256 mm

1	d ≈ 256 mm

זהו המרחק בין הכתף לבין נקודת היעד.

10. שלב 5 – חישוב J2

כעת מסתכלים על המשולש:

P1
P2
P3

אורכי הצלעות הם:

L1
L2
d

משתמשים בחוק הקוסינוסים:

חוק הקוסינוסים אומר: בכל משולש, ריבוע של צלע אחת שווה לסכום ריבועי שתי הצלעות האחרות, פחות פעמיים מכפלת שתי הצלעות בקוסינוס הזווית שביניהן.

ברובוטיקה משתמשים בזה כדי לחשב את זווית המרפק מתוך אורכי הזרועות והמרחק אל היעד

cos(J2) =
(r² + z² - L1² - L2²)
/
(2 · L1 · L2)

cos(J2) =

(r² + z² - L1² - L2²)

(2 · L1 · L2)

ואז:

J2 = acos(cos(J2))

1	J2 = acos(cos(J2))

אבל בדרך כלל יש שתי אפשרויות:

J2 = +acos(...)

1	J2 = +acos(...)

או:

J2 = -acos(...)

1	J2 = -acos(...)

ולכן:

J2 = ±acos(...)

1	J2 = ±acos(...)

11. Elbow Up ו־Elbow Down

אותה נקודת P3 יכולה לעיתים להגיע בשתי דרכים.

פתרון ראשון:

J2 &gt; 0

J2 > 0

יכול להתאים לתצורה אחת של המרפק.

פתרון שני:

J2 &lt; 0

J2 < 0

יכול להתאים לתצורה השנייה.

לכן נקבל:

Elbow Up

Elbow Up

או:

Elbow Down

1	Elbow Down

מבחינה מתמטית:

J2 = +acos(...)

1	J2 = +acos(...)

או:

J2 = -acos(...)

1	J2 = -acos(...)

שני הפתרונות יכולים להביא את P3 לאותו מקום.

12. שלב 6 – חישוב J1

עכשיו מחשבים את זווית הכתף.

תחילה מחשבים את הזווית הישירה אל המטרה:

α = atan2(z, r)

1	α = atan2(z, r)

לאחר מכן מחשבים את התיקון שנוצר בגלל J2:

β =
atan2(
L2 · sin(J2),
L1 + L2 · cos(J2)
)

β =

atan2(

L2 · sin(J2),

L1 + L2 · cos(J2)

)

ולבסוף:

J1 = α - β

1	J1 = α - β

כלומר:

J1 =
atan2(z, r)
-
atan2(
L2 · sin(J2),
L1 + L2 · cos(J2)
)

J1 =

atan2(z, r)

atan2(

L2 · sin(J2),

L1 + L2 · cos(J2)

)

13. טבלת החישוב המרכזית

שלב	פרמטר	נוסחה	משמעות
1	`J0`	`atan2(Y,X)`	סיבוב הבסיס
2	`r`	`√(X²+Y²)`	מרחק אופקי
3	`z`	`Z-H`	גובה ביחס לכתף
4	`d`	`√(r²+z²)`	מרחק P1→P3
5	`J2`	`±acos((r²+z²-L1²-L2²)/(2L1L2))`	זווית המרפק
6	`α`	`atan2(z,r)`	זווית כיוון ליעד
7	`β`	`atan2(L2·sin(J2),L1+L2·cos(J2))`	תיקון בגלל המרפק
8	`J1`	`α-β`	זווית הכתף

14. דוגמה מספרית מלאה

נשתמש באותם נתונים:

H = 80 mm
L1 = 140 mm
L2 = 120 mm

H = 80 mm

L1 = 140 mm

L2 = 120 mm

והמטרה:

P3 = (190.5, 110.0, 211.0)

1	P3 = (190.5, 110.0, 211.0)

חישוב J0

J0 =
atan2(110.0, 190.5)

1 2	J0 = atan2(110.0, 190.5)

לכן:

J0 ≈ 30°

J0 ≈ 30°

חישוב r

r =
√(190.5² + 110.0²)

1 2	r = √(190.5² + 110.0²)

ולכן:

r ≈ 220 mm

1	r ≈ 220 mm

חישוב z

z = 211 - 80

1	z = 211 - 80

ולכן:

z = 131 mm

1	z = 131 mm

חישוב d

d =
√(220² + 131²)

1 2	d = √(220² + 131²)

ולכן:

d ≈ 256 mm

1	d ≈ 256 mm

15. חישוב J2

cos(J2) =
(220² + 131² - 140² - 120²)
/
(2 · 140 · 120)

cos(J2) =

(220² + 131² - 140² - 120²)

(2 · 140 · 120)

בקירוב:

cos(J2) ≈ 0.94

1	cos(J2) ≈ 0.94

ולכן:

J2 ≈ ±20°

J2 ≈ ±20°

נבחר לדוגמה:

J2 = -20°

J2 = -20°

16. חישוב J1

תחילה:

α = atan2(131,220)

1	α = atan2(131,220)

בקירוב:

α ≈ 30.8°

α ≈ 30.8°

עכשיו:

β =
atan2(
120·sin(-20°),
140 + 120·cos(-20°)
)

β =

atan2(

120·sin(-20°),

140 + 120·cos(-20°)

)

בקירוב:

β ≈ -9.2°

β ≈ -9.2°

ולכן:

J1 = α - β

1	J1 = α - β

J1 ≈ 30.8 - (-9.2)

1	J1 ≈ 30.8 - (-9.2)

J1 ≈ 40°

J1 ≈ 40°

17. התוצאה

קיבלנו:

J0 ≈ 30°
J1 ≈ 40°
J2 ≈ -20°

J0 ≈ 30°

J1 ≈ 40°

J2 ≈ -20°

כלומר:

מפרק	זווית
`J0`	`30°`
`J1`	`40°`
`J2`	`-20°`

18. בדיקה באמצעות FK

כדי לוודא שה־IK נכון, אפשר לבצע שוב FK.

נכניס:

J0 = 30°
J1 = 40°
J2 = -20°

J0 = 30°

J1 = 40°

J2 = -20°

לנוסחאות FK.

נקבל בקירוב:

P3 =
(190.5, 110.0, 211.0)

1 2	P3 = (190.5, 110.0, 211.0)

כלומר:

XYZ
 ↓
IK
 ↓
J0,J1,J2
 ↓
FK
 ↓
XYZ

XYZ

↓

J0,J1,J2

↓

XYZ

אם קיבלנו את אותה נקודה, הפתרון תקין.

19. בדיקת טווח הגעה

לא כל נקודה במרחב ניתנת להשגה.

המרחק אל המטרה הוא:

d

הטווח המקסימלי:

dmax = L1 + L2

1	dmax = L1 + L2

והטווח המינימלי:

dmin = |L1 - L2|

1	dmin = \|L1 - L2\|

לכן חייב להתקיים:

|L1-L2| ≤ d ≤ L1+L2

1	\|L1-L2\| ≤ d ≤ L1+L2

בדוגמה:

|140-120| ≤ 256 ≤ 140+120

1	\|140-120\| ≤ 256 ≤ 140+120

כלומר:

20 ≤ 256 ≤ 260

1	20 ≤ 256 ≤ 260

ולכן המטרה ניתנת להשגה.

20. קשר בין P ל־J

חשוב להפריד בין שני סוגי הנתונים:

J0, J1, J2

1	J0, J1, J2

הם זוויות המפרקים.

לעומת זאת:

P0, P1, P2, P3

1	P0, P1, P2, P3

הם מיקומים במרחב.

לדוגמה:

J1 = 40°

J1 = 40°

זו זווית.

אבל:

P2 = (X2,Y2,Z2)

1	P2 = (X2,Y2,Z2)

זה מיקום.

לכן:

J = Joint Angle
P = Point Position

1 2	J = Joint Angle P = Point Position

21. קוד Python קצר

import math

H = 80
L1 = 140
L2 = 120

X = 190.5
Y = 110.0
Z = 211.0

J0 = math.atan2(Y, X)

r = math.sqrt(X**2 + Y**2)
z = Z - H

cos_J2 = (
    r**2 + z**2 - L1**2 - L2**2
) / (2 * L1 * L2)

cos_J2 = max(-1, min(1, cos_J2))

J2 = -math.acos(cos_J2)

J1 = (
    math.atan2(z, r)
    -
    math.atan2(
        L2 * math.sin(J2),
        L1 + L2 * math.cos(J2)
    )
)

print("J0 =", math.degrees(J0))
print("J1 =", math.degrees(J1))
print("J2 =", math.degrees(J2))

import math

H = 80

L1 = 140

L2 = 120

X = 190.5

Y = 110.0

Z = 211.0

J0 = math.atan2(Y, X)

r = math.sqrt(X**2 + Y**2)

z = Z - H

cos_J2 = (

r**2 + z**2 - L1**2 - L2**2

) / (2 * L1 * L2)

cos_J2 = max(-1, min(1, cos_J2))

J2 = -math.acos(cos_J2)

J1 = (

math.atan2(z, r)

math.atan2(

L2 * math.sin(J2),

L1 + L2 * math.cos(J2)

)

print("J0 =", math.degrees(J0))

print("J1 =", math.degrees(J1))

print("J2 =", math.degrees(J2))

תוצאה בקירוב:

J0 = 30°
J1 = 40°
J2 = -20°

J0 = 30°

J1 = 40°

J2 = -20°

22. סיכום

Inverse Kinematics מאפשר לחשב את זוויות המפרקים לפי נקודת היעד.

הקלט:

P3 = (X,Y,Z)

1	P3 = (X,Y,Z)

הפלט:

J0,J1,J2

J0,J1,J2

שרשרת החישוב:

P3 = (X,Y,Z)
       ↓
J0
       ↓
r
       ↓
z
       ↓
J2
       ↓
J1
       ↓
J0,J1,J2

P3 = (X,Y,Z)

↓

J0,J1,J2

זהו אחד החישובים המרכזיים ביותר ברובוטיקה, משום שהוא מאפשר להגיד לרובוט לאן להגיע, במקום לחשב ידנית כל זווית של כל מפרק.

admin

Administrator

Visit Website View All Posts

Related Stories

קורס רובוטיקה ובינה מלאכותית Forward Kinematics – : 01-RB301

pan tilt

RB1001-08 : סימולציה פשוטה מאוד של נהיגה אוטונומית דו־ממדית RL PPO

You may have missed

קורס רובוטיקה ובינה מלאכותית Inverse Kinematics (IK) – : 01-RB302

קורס רובוטיקה ובינה מלאכותית Forward Kinematics – : 01-RB301

pan tilt

RB1001-08 : סימולציה פשוטה מאוד של נהיגה אוטונומית דו־ממדית RL PPO

קורס רובוטיקה ובינה מלאכותית Inverse Kinematics (IK) – : 01-RB302

פרק 2 – Inverse Kinematics (IK)

1. מבוא

2. מתי משתמשים ב־IK?

3. למה צריך IK?

4. מבנה הזרוע

5. נקודות P

6. שלב 1 – חישוב J0

7. שלב 2 – חישוב המרחק האופקי r

8. שלב 3 – חישוב הגובה היחסי z

9. שלב 4 – חישוב המרחק d

10. שלב 5 – חישוב J2

11. Elbow Up ו־Elbow Down

12. שלב 6 – חישוב J1

13. טבלת החישוב המרכזית

14. דוגמה מספרית מלאה

חישוב J0

חישוב r

חישוב z

חישוב d

15. חישוב J2

16. חישוב J1

17. התוצאה

18. בדיקה באמצעות FK

19. בדיקת טווח הגעה

20. קשר בין P ל־J

21. קוד Python קצר

22. סיכום

About the Author

Related Stories

You may have missed