การวางแผนเส้นทางและหลบหลีก

ตัวอย่างที่ 1 ข้ามแถวแผงไปยังตู้อินเวอร์เตอร์

พื้นที่ 20 × 20 m มีแถวแผงสามแถวสลับด้านกัน รถเริ่มที่ (1, 1) และต้องไปตู้อินเวอร์เตอร์ที่ (19, 19) ช่วงยื่นกิ่ง 1 m สุ่มเป้าหมาย 10% ของรอบ กำหนดเมล็ดสุ่มให้ผลซ้ำได้ (ข้อมูลจำลอง)

import numpy as np

rng = np.random.default_rng(7)
start, goal = np.array([1.0, 1.0]), np.array([19.0, 19.0])
# แถวแผงโซลาร์ (xmin, ymin, xmax, ymax) m
rows = [(0, 5, 15, 6.5), (5, 10, 20, 11.5), (0, 15, 15, 16.5)]

def free(p):
    return all(not (x0 <= p[0] <= x1 and y0 <= p[1] <= y1) for x0, y0, x1, y1 in rows)

def segment_free(a, b, step=0.1):
    n = max(2, int(np.linalg.norm(b - a) / step))
    return all(free(a + (b - a) * s) for s in np.linspace(0, 1, n))

def rrt(max_nodes=3000, step=1.0, goal_bias=0.1):
    nodes, parent = [start], [-1]
    while len(nodes) < max_nodes:
        sample = goal if rng.random() < goal_bias else rng.uniform(0, 20, 2)
        i = int(np.argmin([np.linalg.norm(n - sample) for n in nodes]))
        d = sample - nodes[i]
        new = nodes[i] + d / max(np.linalg.norm(d), 1e-9) * min(step, np.linalg.norm(d))
        if free(new) and segment_free(nodes[i], new):
            nodes.append(new); parent.append(i)
            if np.linalg.norm(new - goal) < step and segment_free(new, goal):
                nodes.append(goal); parent.append(len(nodes) - 2)
                path, k = [], len(nodes) - 1
                while k != -1:
                    path.append(nodes[k]); k = parent[k]
                return path[::-1], len(nodes)
    return None, len(nodes)

def length(path):
    return sum(np.linalg.norm(path[i+1] - path[i]) for i in range(len(path) - 1))

def shortcut(path):
    out, i = [path[0]], 0
    while i < len(path) - 1:
        j = len(path) - 1
        while j > i + 1 and not segment_free(path[i], path[j]):
            j -= 1
        out.append(path[j]); i = j
    return out

path, n = rrt()
short = shortcut(path)
print(f"nodes in tree {n}")
print(f"RRT path {length(path):.1f} m ({len(path)} points)")
print(f"after shortcutting {length(short):.1f} m ({len(short)} points)")
print(f"straight line {np.linalg.norm(goal - start):.1f} m (blocked by panels)")

nodes in tree 389
RRT path 70.7 m (74 points)
after shortcutting 52.6 m (7 points)
straight line 25.5 m (blocked by panels)

RRT ได้เส้นทางที่ผ่านได้ภายในไม่กี่ร้อยโหนด แต่คดเคี้ยวยาว 70.7 m การตัดทางลัดลดเหลือ 52.6 m ด้วยจุดเลี้ยวเพียง 7 จุด เส้นทางยังชิดขอบแผง งานจริงจึงต้องขยายสิ่งกีดขวางออกเท่าครึ่งความกว้างรถบวกระยะเผื่อก่อนวางแผน ผลของ RRT ขึ้นกับการสุ่ม ถ้าเปลี่ยนเมล็ดสุ่มจะได้เส้นทางต่างกัน จึงควรทดสอบหลายครั้ง

ตัวอย่างที่ 2 เข้าแนวแถวแผงเมื่อคำสั่งเลี้ยวหน่วง 0.3 s

รถวิ่ง 1 m/s เริ่มห่างแนวแถวแผง 1.0 m หันขนานกับแนว คำสั่งเลี้ยวไปถึงล้อช้า 0.3 s จากการสื่อสารและตัวขับมอเตอร์ เปรียบเทียบระยะมองหน้า 0.5, 1.0, 1.5 และ 3.0 m (แบบจำลองจลนศาสตร์)

import numpy as np

V, DT, T = 1.0, 0.05, 15.0     # ความเร็ว m/s ช่วงเวลา s เวลาจำลอง s
DELAY = 0.3                    # คำสั่งเลี้ยวถึงล้อช้า 0.3 s

def track(Ld):
    x, y, th = 0.0, 1.0, 0.0            # เริ่มห่างเส้นทาง y = 0 อยู่ 1.0 m
    buf = [0.0] * int(round(DELAY / DT))
    ys = []
    for _ in range(int(T / DT)):
        dx, dy = Ld, -y                 # จุดเป้าบนเส้นทาง ห่างไปข้างหน้า Ld ตามแนว x
        alpha = np.arctan2(dy, dx) - th
        buf.append(2 * np.sin(alpha) / np.hypot(dx, dy))
        kappa = buf.pop(0)
        th += V * kappa * DT
        x += V * np.cos(th) * DT
        y += V * np.sin(th) * DT
        ys.append(y)
    return np.array(ys)

for Ld in [0.5, 1.0, 1.5, 3.0]:
    ys = track(Ld)
    out = np.where(np.abs(ys) >= 0.05)[0]
    settle = (out[-1] + 1) * DT
    s = f"settles within ±5 cm by {settle:.1f} s" if settle < T else "does not settle in 15 s"
    print(f"Ld={Ld:.1f} m  {s}  max overshoot {max(0, -ys.min())*100:.1f} cm")

Ld=0.5 m  does not settle in 15 s  max overshoot 24.9 cm
Ld=1.0 m  settles within ±5 cm by 2.3 s  max overshoot 1.8 cm
Ld=1.5 m  settles within ±5 cm by 3.2 s  max overshoot 2.7 cm
Ld=3.0 m  settles within ±5 cm by 6.1 s  max overshoot 3.8 cm

ถ้าไม่มีเวลาหน่วง ระยะมองหน้าสั้นจะเข้าเส้นทางเร็วที่สุด แต่เมื่อคำสั่งหน่วง 0.3 s ระยะ 0.5 m แกว่งไปมาไม่หยุด ส่วน 1.0 m เข้าแถบ ±5 cm เร็วที่สุด ระยะ 3.0 m นิ่งแต่ช้า การเลือกระยะมองหน้าจึงต้องคิดรวมกับความเร็วและเวลาหน่วงของระบบจริง ระบบหลายตัวปรับระยะมองหน้าตามความเร็ว