หลบหลีกสิ่งกีดขวาง

ตัวอย่างที่ 1 จำลอง potential field หน้าเสา

เสารัศมีอิทธิพล 2.5 m อยู่ที่ (5, 0) เป้าหมายที่ (10, 0) เทียบเริ่มตรงแนวกับเริ่มเยื้อง 0.5 m

import math

def fly(start, goal=(10, 0), obstacle=(5, 0), k_att=1.0, k_rep=5.0, rho0=2.5, step=0.05):
    x, y = start
    for i in range(2000):
        fx, fy = k_att * (goal[0] - x), k_att * (goal[1] - y)
        dx, dy = x - obstacle[0], y - obstacle[1]
        d = math.hypot(dx, dy)
        if d < rho0:
            m = k_rep * (1 / d - 1 / rho0) / d ** 2
            fx, fy = fx + m * dx / d, fy + m * dy / d
        f = math.hypot(fx, fy)
        if f < 1e-3:
            return f"stuck at ({x:.2f}, {y:.2f}) after {i} steps"
        x, y = x + step * fx / max(f, 1), y + step * fy / max(f, 1)
        if math.hypot(goal[0] - x, goal[1] - y) < 0.1:
            return f"reached goal after {i} steps"
    return "timeout"

print("start on the line:", fly((0, 0)))
print("start 0.5 m to the side:", fly((0, 0.5)))

start on the line: stuck at (4.17, 0.00) after 88 steps
start 0.5 m to the side: reached goal after 233 steps

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

ตัวอย่างที่ 2 เลือกทิศจากระยะ LiDAR

แบบง่ายของ VFH: 36 ช่อง ช่องละ 10° ถือว่าช่องที่มีสิ่งกีดขวางใกล้กว่า 3 m ถูกกีดขวาง แล้วเลือกช่องว่างที่ใกล้ทิศเป้าหมายที่สุด

ranges = [6.0] * 36                                   # m ต่อช่อง 10° เริ่มที่ 0° ตามเข็ม
for k in (34, 35, 0, 1, 2): ranges[k] = 1.2           # กล่องขวางด้านหน้า
for k in (3, 4): ranges[k] = 2.5
for k in (31, 32, 33): ranges[k] = 2.8
THRESH, GOAL_DEG = 3.0, 0

def angle_diff(a, b):
    return abs((a - b + 180) % 360 - 180)

blocked = [k * 10 for k, r in enumerate(ranges) if r < THRESH]
free = [k * 10 for k, r in enumerate(ranges) if r >= THRESH]
best = min(free, key=lambda a: angle_diff(a, GOAL_DEG))
print("blocked headings:", blocked)
print(f"chosen heading {best} deg ({angle_diff(best, GOAL_DEG)} deg from goal)")

blocked headings: [0, 10, 20, 30, 40, 310, 320, 330, 340, 350]
chosen heading 50 deg (50 deg from goal)

VFH จริงดูความกว้างของหุบด้วย เพื่อไม่ให้เลือกช่องว่างแคบที่ตัวโดรนผ่านไม่ได้ และปรับความเร็วตามความหนาแน่นของสิ่งกีดขวางข้างหน้า