จลนศาสตร์หุ่นยนต์

ตัวอย่างที่ 1 ยื่นแขนออกไปจนเกือบสุด

แขนสองข้อต่อยาว 0.40 และ 0.30 m ข้อต่อแรกอยู่ที่ 0° ต้องการให้ปลายแขนเลื่อนออกตามแนว x ด้วย 0.10 m/s เพื่อดันแบตเตอรี่เข้าช่อง เทียบที่มุมข้อต่อที่สองต่าง ๆ (ข้อมูลจำลอง)

import numpy as np

l1, l2 = 0.40, 0.30          # ความยาวข้อแขน m
v_tool = np.array([0.10, 0.0])  # ปลายแขนต้องเลื่อน 0.10 m/s

def jacobian(t1, t2):
    return np.array([
        [-l1*np.sin(t1) - l2*np.sin(t1+t2), -l2*np.sin(t1+t2)],
        [ l1*np.cos(t1) + l2*np.cos(t1+t2),  l2*np.cos(t1+t2)]])

t1 = np.radians(0)
for deg in [90, 30, 10, 2]:
    J = jacobian(t1, np.radians(deg))
    w = abs(np.linalg.det(J))
    qdot = np.linalg.solve(J, v_tool)
    print(f"θ2={deg:>2}°  manipulability={w:.4f}  "
          f"max joint speed={np.degrees(np.abs(qdot)).max():6.1f} deg/s")

θ2=90°  manipulability=0.1200  max joint speed=  19.1 deg/s
θ2=30°  manipulability=0.0600  max joint speed=  63.0 deg/s
θ2=10°  manipulability=0.0208  max joint speed= 191.2 deg/s
θ2= 2°  manipulability=0.0042  max joint speed= 957.4 deg/s

ยิ่งแขนเหยียดตรง (θ2 เข้าใกล้ 0) manipulability ยิ่งลดลง และความเร็วข้อต่อที่ต้องใช้พุ่งขึ้นจนเกินขีดความสามารถของมอเตอร์ทั่วไป การออกแบบท่าทำงานจึงควรให้แขนงออยู่พอสมควร และตัวควบคุมควรจำกัดความเร็วข้อต่อ ไม่ใช่พยายามทำตามคำสั่งที่เป็นไปไม่ได้

ตัวอย่างที่ 2 แขนสามข้อต่อ เป้าที่เอื้อมถึงและเป้าที่ไกลเกินไป

แขนสามข้อต่อยาว 0.40, 0.30 และ 0.20 m (ยื่นได้ไกลสุด 0.90 m) เป้าแรกคือช่องแบตเตอรี่ที่ (0.50, 0.40) m เป้าที่สองคือจุดที่อยู่นอกระยะ (1.20, 0) m เปรียบเทียบ λ = 0 กับ λ = 0.2 วนซ้ำ 200 รอบ (ข้อมูลจำลอง)

import numpy as np

L = np.array([0.40, 0.30, 0.20])     # แขนสามข้อ ยื่นได้ไกลสุด 0.90 m

def fk(q):
    a = np.cumsum(q)
    return np.array([np.sum(L*np.cos(a)), np.sum(L*np.sin(a))])

def jac(q):
    a = np.cumsum(q)
    J = np.zeros((2, 3))
    for i in range(3):
        J[0, i] = -np.sum(L[i:]*np.sin(a[i:]))
        J[1, i] = np.sum(L[i:]*np.cos(a[i:]))
    return J

def ik(target, lam, iters=200):
    q = np.radians([10.0, 20.0, 20.0])
    max_step = 0.0
    for _ in range(iters):
        e = target - fk(q)
        J = jac(q)
        dq = J.T @ np.linalg.solve(J @ J.T + (lam**2 + 1e-12) * np.eye(2), e)
        max_step = max(max_step, np.degrees(np.abs(dq)).max())
        q = q + dq
    return np.linalg.norm(target - fk(q)), max_step

for lam in [0.0, 0.2]:
    for target in [np.array([0.50, 0.40]), np.array([1.20, 0.0])]:
        err, step = ik(target, lam)
        print(f"lambda={lam}  target {target}  error {err*1000:5.1f} mm  "
              f"largest joint step {step:6.1f} deg")

lambda=0.0  target [0.5 0.4]  error   0.0 mm  largest joint step   76.2 deg
lambda=0.0  target [1.2 0. ]  error 799.9 mm  largest joint step 1480.9 deg
lambda=0.2  target [0.5 0.4]  error   0.0 mm  largest joint step   22.7 deg
lambda=0.2  target [1.2 0. ]  error 300.0 mm  largest joint step   28.4 deg

เมื่อเป้าเอื้อมถึง ทั้งสองวิธีไปถึงเป้า แต่ DLS ขยับข้อต่อต่อรอบน้อยกว่ามาก เมื่อเป้าอยู่นอกระยะ pseudoinverse สั่งข้อต่อกระโดดกว่าพันองศาต่อรอบและจบห่างเป้า 0.80 m ส่วน DLS ยืดแขนตรงไปทางเป้าอย่างนุ่มนวล ห่างเป้า 0.30 m ซึ่งเท่ากับระยะที่ขาดจริง ($1.20-0.90$) ระบบจริงควรตรวจก่อนว่าเป้าอยู่ในพื้นที่ทำงานหรือไม่ และแจ้งเตือนแทนที่จะพยายามต่อไป