3. ünite / 10 · 4 saat
Ters kinematik: hedeften eklem açılarına
Küresel bilekli kolda kapalı biçimli çözüm: bilek merkezi, omuz-dirsek düzlemi, bilek açıları, sekiz çözüm ve tekillikler.
Bu ünitenin sonunda
- Konum ve yönelimi bilek merkezi ile bilek açılarına ayırmak
- Kosinüs teoremiyle omuz ve dirsek açısını hesaplamak
- Sekiz çözümün nereden geldiğini (omuz, dirsek, bilek) açıklamak
- Eklem sınırları ve tekilliklerin çözümü nasıl etkilediğini yorumlamak
Ders notları
Ters kinematik (IK) ileri kinematiğin tersidir: tutucuyu şu konuma ve yönelime koymak istiyorum, eklemler kaç derece olmalı? Genel bir altı eksen için bu zordur; ama küresel bilek sorunu ikiye böler: önce konum (q1, q2, q3), sonra yönelim (q4, q5, q6).
1. Bilek merkezi
İstenen TCP konumu p ve araç ekseni (R'nin üçüncü sütunu) biliniyor. Bilek merkezi TCP'nin araç ekseni boyunca 0,22 m gerisindedir: w = p − 0,22 · zaraç. Bundan sonra yalnızca w'yi karşılayan üç eklem aranır.
2. Taban, omuz ve dirsek
q1 = atan2(wy, wx) kolu hedefin düzlemine çevirir. Bu düzlemde iki uzuvlu bir kol kalır; kosinüs teoremi (ESOF 323'teki gibi) dirsek açısını verir:
r bilek merkezinin eksenden yatay uzaklığı, z omuza göre yüksekliğidir. |cos q3| > 1 ise hedef erişilemez. q3 için iki işaret vardır: dirsek yukarı (q3 > 0) ve dirsek aşağı. Ayrıca kol hedefe sırtını dönüp q1 + 180° ile r'yi eksi alarak da ulaşabilir (omuz arkada).
3. Bilek
R03 = Rz(q1)·Ry(q2 + q3) bilinince bilek dönüşü R36 = R03T·R kalır. R36 = Rz(q4)·Ry(q5)·Rz(q6) olduğundan q5 = atan2(√(R13² + R23²), R33), q4 = atan2(R23, R13), q6 = atan2(R32, −R31). Bilek de ters çözülebilir: (q4 + 180°, −q5, q6 + 180°) aynı yönelimi verir.
Sekiz çözüm ve tekillik
Omuz (2) × dirsek (2) × bilek (2) = en çok sekiz çözüm; eklem sınırları dışındakiler atılır. Genel bir duruşta bu derste sekiz çözümün hepsi sınırlar içinde kalabilir; masadaki bir alma noktasında genelde dört kalır. Tekillikte (q5 ≈ 0, kol tam açık ya da kapalı, bilek merkezi taban ekseninde) sonsuz çözüm vardır ya da bir eksen kaybolur: q4 ve q6 aynı dönüşü paylaşır. Kod bu durumda q4'ü bir önceki değerden alır.
Simülasyonlar
Öğretim modeli: sanal kol bu dersin kendi modelidir. Uzuv ölçüleri, eklem sınırları, hız ve ivme değerleri ve güvenlik eşikleri bu sayfadaki kodda tanımlı ders sabitleridir; hiçbir robot üreticisinin ürünü, dili ya da eğitim içeriği değildir ve ders resmî bir üretici eğitimi değildir. Gerçek bir hücrede ölçüler üreticinin belgelerinden, güvenlik değerleri risk değerlendirmesinden gelir.
Ters kinematik: hedefi sürükle, çözümleri gez
Hedef konumu (x, y, z) ve tutucunun yaw açısını ayarla. Bulunan çözümlerin (en çok sekiz: omuz, dirsek, bilek) sayısı gösterilir; birini seçince kol o eklem açılarına geçer. Ulaşılamayan hedefte çözüm sayısı sıfırdır. Doğrulama satırı FK(IK) farkını gösterir.
İki eklemli kol kinematiği
Hedefi taşımak ve ters kinematiği çözmek için tuvalde sürükle, ya da ileri kinematiğe geçip eklem açılarını kendin ayarla. Gölgeli halka erişilebilir çalışma alanıdır.
Yukarıdaki iki eklemli kol simülasyonu ESOF 323 7. haftadakinin aynısıdır, yeniden kullanıldı. 7. haftayı aç
Kendin dene
Ters kinematik simülasyonunda hedefi alma bölgesine (0,55; −0,32; 0,03) koy ve çözüm sayısını oku. Çözümler arasında geçiş yap: hangileri kolu hedefin önünde, hangileri arkasında tutuyor? Hedefi tabana yaklaştır ve bir çözüm eklem sınırına çarpıp kaybolana kadar götür. Sonra hedefi 0,89 m ötesine al: kaç çözüm kalıyor? Son olarak "doğrulama" satırında FK(IK(hedef)) farkının neredeyse sıfır olduğunu gör.
Kod
ArmScript bloklarını ArmScript simülasyonuna yapıştırıp tarayıcında çalıştırabilirsin. Python blokları yalnızca standart kütüphane kullanır; kendi bilgisayarında çalıştır.
import math
A2, A3 = 0.45, 0.45
def planar_ik(r, z, elbow_up=True):
# r: bilek merkezinin eksenden yatay uzaklığı, z: omuza göre yükseklik
c3 = (r * r + z * z - A2 * A2 - A3 * A3) / (2 * A2 * A3)
if abs(c3) > 1:
return None # erişilemez
q3 = math.acos(c3) * (1 if elbow_up else -1)
q2 = math.atan2(r, z) - math.atan2(A3 * math.sin(q3), A2 + A3 * math.cos(q3))
return q2, q3
print([round(math.degrees(x)) for x in planar_ik(0.675, 0.39)]) # [30, 60] dirsek yukarı
print([round(math.degrees(x)) for x in planar_ik(0.675, 0.39, False)]) # [90, -60] dirsek aşağıimport math
def rz(a): c, s = math.cos(a), math.sin(a); return [[c, -s, 0], [s, c, 0], [0, 0, 1]]
def ry(a): c, s = math.cos(a), math.sin(a); return [[c, 0, s], [0, 1, 0], [-s, 0, c]]
def mm(A, B): return [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
def mv(A, v): return [sum(A[i][k] * v[k] for k in range(3)) for i in range(3)]
D1, A2, A3, D6, TOOL = 0.4, 0.45, 0.45, 0.1, 0.12 # uzuv ölçüleri (m)
def fk(q): # q: altı eklem açısı, radyan
R2 = mm(rz(q[0]), ry(q[1])) # omuz: taban dönüşü sonra eğilme
R3 = mm(R2, ry(q[2])) # dirsek
R6 = mm(mm(mm(R3, rz(q[3])), ry(q[4])), rz(q[5])) # küresel bilek: roll-pitch-roll
shoulder = [0, 0, D1]
elbow = [a + b for a, b in zip(shoulder, mv(R2, [0, 0, A2]))]
wrist = [a + b for a, b in zip(elbow, mv(R3, [0, 0, A3]))]
tcp = [a + b for a, b in zip(wrist, mv(R6, [0, 0, D6 + TOOL]))]
return tcp, R6
print(fk([math.radians(x) for x in (0, 30, 60, 0, 90, 0)])[0]) # [0.675, 0.0, 0.5697...]Sık yapılan hatalar
- Tek çözüm varsayıp ilkini seçmek: robot başka çözüme atlarsa eklemler büyük dönmek zorunda kalır; mevcut duruşa en yakın çözümü seç.
- Eklem sınırlarını denetlemeden çözümü kullanmak: matematiksel olarak doğru çözüm kolun yapamayacağı bir açıda olabilir.
- Tekilliğe yaklaşırken hızı düşünmemek: bilek tekilliğinde q4 ve q6 çok hızlı döner.
- IK sonucunu doğrulamamak: her çözümü FK'ya geri verip hedefle karşılaştır.
Yazılım mühendisliği bağlantısı
Ters kinematik gibi türetilmiş bir algoritmayı gidiş-dönüş testiyle sına: rastgele sınır içi duruşlar için FK → IK → FK.
- Bu derste üç yüz rastgele duruş için çözümlerin hepsi aynı duruşu verir ve çözümler arasında orijinal açılar bulunur.
- Sınır durumları ayrı test et: erişim dışı hedef boş liste dönmeli, tekil duruş da çözüm dönmeli.
Kendini sına
Küresel bilek ters kinematiği neden kolaylaştırır?
cos q3 > 1 çıkarsa ne anlama gelir?
Genel bir duruş için en çok kaç ters kinematik çözümü vardır ve nereden gelir?
q5 ≈ 0 iken ne olur?
Bu üniteden sonra bitirme projesinde
- Hücre ve erişim raporu: Alma ve bırakma noktalarının ileri/ters kinematikle erişilebilirliği ve çözüm sayıları.