חישוב Forward Kinematics לזרוע רובוטית עם J0, J1, J2

1. מה זה Forward Kinematics?
Forward Kinematics, או בקיצור FK, היא שיטה לחישוב המיקום של חלקי הרובוט כאשר אנחנו כבר יודעים את זוויות המפרקים ( זוויות המנועים)
כלומר: FK אומר לנו: אם אני יודע את זוויות המפרקים של הרובוט, איפה בדיוק נמצא קצה הזרוע במרחב?
| Turns the wrist left and right | |||
| J6 | Tool / End-Effector Roll | Rotate ↻ ↺ | Rotates the gripper or tool around its own axis |
FK = Forward Kinematics
End Effector הוא החלק שנמצא בקצה הזרוע הרובוטית ועושה את הפעולה בפועל
למשל
- Gripper – תפסן שתופס חפץ
- Vacuum cup – כוס יניקה
- Welder – ראש ריתוך
- Drill – מקדחה
- Camera – מצלמה
- Screwdriver – מברגה
- Cutting tool – כלי חיתוך
הנה פרק מלא חדש, באותו סימון בדיוק: J0, J1, J2, הנקודות P0, P1, P2, P3, והאורכים H, L1, L2.
Forward Kinematics לזרוע רובוטית עם J0, J1, J2
1. מבוא
Forward Kinematics, או בקיצור FK, היא השיטה שבה מחשבים את מיקום חלקי הזרוע הרובוטית במרחב מתוך זוויות המפרקים.
כלומר:
|
1 2 3 4 5 |
J0, J1, J2 ↓ FK ↓ P0, P1, P2, P3 |
כל נקודה מתוארת על ידי שלושה ערכים:
|
1 |
(X, Y, Z) |
כלומר:
|
1 |
P = (X, Y, Z) |
2. מבנה הזרוע
נשתמש בזרוע בעלת שלושה מפרקים:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
P3 ● / / L2 / P2 ● / / L1 / P1 ● | | H | P0 ● |
המשמעות של הסימונים:
| סימון | משמעות |
|---|---|
J0 |
סיבוב הבסיס |
J1 |
מפרק הכתף |
J2 |
מפרק המרפק |
P0 |
נקודת הבסיס |
P1 |
נקודת הכתף |
P2 |
נקודת המרפק |
P3 |
קצה הזרוע |
H |
גובה הכתף |
L1 |
אורך הזרוע הראשונה |
L2 |
אורך הזרוע השנייה |
3. מערכת הצירים
הזרוע פועלת במרחב תלת־ממדי.
אנחנו משתמשים בשלושה צירים:
|
1 2 3 |
X Y Z |
כאשר:
|
1 2 3 |
X = קדימה ואחורה Y = ימינה ושמאלה Z = למעלה ולמטה |
לכן כל נקודה נכתבת בצורה:
|
1 2 3 4 |
P0 = (X0, Y0, Z0) P1 = (X1, Y1, Z1) P2 = (X2, Y2, Z2) P3 = (X3, Y3, Z3) |
4. הנקודה P0
P0 היא נקודת הבסיס.
נבחר אותה בתור ראשית הצירים:
|
1 |
P0 = (0, 0, 0) |
לכן:
|
1 2 3 |
X0 = 0 Y0 = 0 Z0 = 0 |
אין כאן עדיין צורך בחישוב.
5. הנקודה P1
P1 היא נקודת הכתף.
הכתף נמצאת בדיוק מעל הבסיס בגובה:
|
1 |
H |
לכן:
|
1 |
P1 = (0, 0, H) |
כלומר:
|
1 2 3 4 5 |
X1 = 0 Y1 = 0 Z1 = H |
6. מה עושה J0?
J0 מסובב את כל הזרוע סביב ציר Z.
כאשר:
|
1 |
J0 = 0° |
הזרוע פונה לכיוון X.
כאשר:
|
1 |
J0 = 90° |
הזרוע פונה לכיוון Y.
הסיבוב מתבצע באמצעות:
|
1 2 |
cos(J0) sin(J0) |
אם יש לנו מרחק אופקי:
|
1 |
r |
אז:
|
1 2 3 |
X = r · cos(J0) Y = r · sin(J0) |
זוהי אחת הנוסחאות החשובות ביותר בחישוב.
7. חישוב P2
P2 היא נקודת המרפק.
המרחק בין P1 לבין P2 הוא:
|
1 |
L1 |
והזרוע הראשונה נמצאת בזווית:
|
1 |
J1 |
תחילה נחשב את ההיטל האופקי של L1:
|
1 |
r1 = L1 · cos(J1) |
ואת הגובה שנוסף:
|
1 |
z1 = L1 · sin(J1) |
עכשיו נשתמש ב־J0 כדי לפרק את ההיטל האופקי לצירי X ו־Y.
לכן:
|
1 |
X2 = L1 · cos(J1) · cos(J0) |
|
1 |
Y2 = L1 · cos(J1) · sin(J0) |
והגובה:
|
1 |
Z2 = H + L1 · sin(J1) |
לכן:
|
1 |
P2 = (X2, Y2, Z2) |
כאשר:
|
1 2 3 4 5 |
X2 = L1 · cos(J1) · cos(J0) Y2 = L1 · cos(J1) · sin(J0) Z2 = H + L1 · sin(J1) |
8. חישוב P3
P3 היא נקודת קצה הזרוע.
החלק השני של הזרוע הוא באורך:
|
1 |
L2 |
אבל חשוב להבין שהזווית שלו ביחס לקרקע אינה רק:
|
1 |
J2 |
אלא:
|
1 |
J1 + J2 |
הסיבה היא ש־J2 מסתובב ביחס לזרוע הראשונה, שכבר נמצאת בזווית J1.
לכן ההיטל האופקי של החלק השני הוא:
|
1 |
L2 · cos(J1 + J2) |
והגובה שהוא מוסיף הוא:
|
1 |
L2 · sin(J1 + J2) |
9. המרחק האופקי הכולל
ההיטל האופקי של החלק הראשון הוא:
|
1 |
L1 · cos(J1) |
וההיטל האופקי של החלק השני הוא:
|
1 |
L2 · cos(J1 + J2) |
לכן המרחק האופקי הכולל הוא:
|
1 2 3 4 |
r = L1 · cos(J1) + L2 · cos(J1 + J2) |
10. חישוב X3
עכשיו משתמשים ב־J0:
|
1 |
X3 = r · cos(J0) |
נציב את r:
|
1 2 3 4 5 |
X3 = [L1 · cos(J1) + L2 · cos(J1 + J2)] · cos(J0) |
11. חישוב Y3
באופן דומה:
|
1 |
Y3 = r · sin(J0) |
כלומר:
|
1 2 3 4 5 |
Y3 = [L1 · cos(J1) + L2 · cos(J1 + J2)] · sin(J0) |
12. חישוב Z3
הגובה מתחיל מ:
|
1 |
H |
אליו מוסיפים את העלייה של L1:
|
1 |
L1 · sin(J1) |
ואת העלייה של L2:
|
1 |
L2 · sin(J1 + J2) |
לכן:
|
1 2 3 4 5 6 |
Z3 = H + L1 · sin(J1) + L2 · sin(J1 + J2) |
ולכן:
|
1 |
P3 = (X3, Y3, Z3) |
13. טבלת FK מלאה
| נקודה | X | Y | Z |
|---|---|---|---|
P0 |
0 |
0 |
0 |
P1 |
0 |
0 |
H |
P2 |
L1·cos(J1)·cos(J0) |
L1·cos(J1)·sin(J0) |
H + L1·sin(J1) |
P3 |
[L1·cos(J1)+L2·cos(J1+J2)]·cos(J0) |
[L1·cos(J1)+L2·cos(J1+J2)]·sin(J0) |
H+L1·sin(J1)+L2·sin(J1+J2) |
14. דוגמה מספרית
ניקח:
|
1 2 3 4 5 |
H = 80 mm L1 = 140 mm L2 = 120 mm |
וזוויות:
|
1 2 3 4 5 |
J0 = 30° J1 = 40° J2 = -20° |
14.1 חישוב P0
|
1 |
P0 = (0, 0, 0) |
14.2 חישוב P1
|
1 |
P1 = (0, 0, 80) |
14.3 חישוב P2
ראשית:
|
1 2 3 4 5 6 7 |
cos(40°) ≈ 0.7660 sin(40°) ≈ 0.6428 cos(30°) ≈ 0.8660 sin(30°) = 0.5 |
נחשב X:
|
1 2 3 4 5 6 |
X2 = 140 · cos(40°) · cos(30°) X2 ≈ 140 · 0.7660 · 0.8660 X2 ≈ 92.9 mm |
נחשב Y:
|
1 2 3 4 5 6 |
Y2 = 140 · cos(40°) · sin(30°) Y2 ≈ 140 · 0.7660 · 0.5 Y2 ≈ 53.6 mm |
נחשב Z:
|
1 2 3 4 5 6 7 |
Z2 = 80 + 140 · sin(40°) Z2 ≈ 80 + 140 · 0.6428 Z2 ≈ 170.0 mm |
לכן:
|
1 |
P2 ≈ (92.9, 53.6, 170.0) |
15. חישוב P3
תחילה:
|
1 2 3 4 |
J1 + J2 = 40° + (-20°) J1 + J2 = 20° |
עכשיו:
|
1 2 3 |
cos(20°) ≈ 0.9397 sin(20°) ≈ 0.3420 |
ההיטל האופקי הראשון:
|
1 2 3 4 |
r1 = 140 · cos(40°) r1 ≈ 107.25 mm |
ההיטל האופקי השני:
|
1 2 3 4 |
r2 = 120 · cos(20°) r2 ≈ 112.76 mm |
ולכן:
|
1 2 3 4 |
r = 107.25 + 112.76 r ≈ 220.01 mm |
חישוב X3
|
1 2 3 4 5 6 7 |
X3 = 220.01 · cos(30°) X3 ≈ 220.01 · 0.8660 X3 ≈ 190.5 mm |
חישוב Y3
|
1 2 3 4 5 6 7 |
Y3 = 220.01 · sin(30°) Y3 ≈ 220.01 · 0.5 Y3 ≈ 110.0 mm |
חישוב Z3
|
1 2 3 4 5 6 |
Z3 = 80 + 140 · sin(40°) + 120 · sin(20°) |
|
1 2 3 4 5 6 |
Z3 ≈ 80 + 89.99 + 41.04 |
|
1 |
Z3 ≈ 211.0 mm |
לכן:
|
1 |
P3 ≈ (190.5, 110.0, 211.0) |
16. התוצאה המלאה
עבור:
|
1 2 3 |
J0 = 30° J1 = 40° J2 = -20° |
נקבל:
| נקודה | X | Y | Z |
|---|---|---|---|
P0 |
0 | 0 | 0 |
P1 |
0 | 0 | 80 |
P2 |
92.9 | 53.6 | 170.0 |
P3 |
190.5 | 110.0 | 211.0 |
17. קוד Python
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 |
import math H = 80 L1 = 140 L2 = 120 J0 = math.radians(30) J1 = math.radians(40) J2 = math.radians(-20) P0 = (0, 0, 0) P1 = (0, 0, H) X2 = L1 * math.cos(J1) * math.cos(J0) Y2 = L1 * math.cos(J1) * math.sin(J0) Z2 = H + L1 * math.sin(J1) P2 = (X2, Y2, Z2) r = ( L1 * math.cos(J1) + L2 * math.cos(J1 + J2) ) X3 = r * math.cos(J0) Y3 = r * math.sin(J0) Z3 = ( H + L1 * math.sin(J1) + L2 * math.sin(J1 + J2) ) P3 = (X3, Y3, Z3) print("P0 =", P0) print("P1 =", P1) print("P2 =", P2) print("P3 =", P3) |
18. למה משתמשים ב־sin ו־cos?
כאשר יש לנו מוט באורך:
|
1 |
L |
ובזווית:
|
1 |
θ |
אנחנו יכולים לפרק אותו לשני רכיבים:
|
1 2 3 |
Horizontal = L · cos(θ) Vertical = L · sin(θ) |
בזרוע הרובוטית עושים בדיוק את אותו הדבר.
cos נותן לנו את ההיטל האופקי.
sin נותן לנו את ההיטל האנכי.
J0 מחלק את ההיטל האופקי בין:
|
1 2 |
X Y |
19. הרעיון המרכזי של FK
מתחילים עם זוויות:
|
1 2 3 |
J0 J1 J2 |
מחשבים תחילה:
|
1 |
P0 |
לאחר מכן:
|
1 |
P1 |
לאחר מכן:
|
1 |
P2 |
ולבסוף:
|
1 |
P3 |
כל נקודה חדשה נבנית על הנקודה הקודמת.
לכן אפשר לחשוב על FK כשרשרת:
|
1 2 3 4 5 6 7 8 9 |
Base ↓ P0 ↓ H P1 ↓ L1, J1 P2 ↓ L2, J2 P3 |
ובמקביל:
|
1 |
J0 |
מסובב את כל השרשרת סביב ציר Z.
20. סיכום
Forward Kinematics עונה על השאלה:
אם אני יודע את זוויות המפרקים J0, J1, J2 — איפה נמצאת כל נקודה של הזרוע?
הקלט הוא:
|
1 |
J0, J1, J2 |
והפלט הוא:
|
1 2 3 4 |
P0 P1 P2 P3 |
כאשר כל נקודה מכילה:
|
1 |
(X, Y, Z) |
לכן:
|
1 2 3 4 5 |
Joint Angles ↓ FK ↓ XYZ Positions |
זהו הבסיס המתמטי לשליטה בזרוע רובוטית.
השלב הבא המתאים הוא לבנות בדיוק באותה שיטת סימון פרק Inverse Kinematics, כלומר להתחיל מ־P3 = (X,Y,Z) ולחשב ממנו את J0, J1, J2.