หลบหลีกสิ่งกีดขวางและแผนที่

ทำไมต้องรู้

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

Occupancy grid และ A*

Occupancy grid แบ่งพื้นที่เป็นช่องตาราง แต่ละช่องบอกว่าว่างหรือมีสิ่งกีดขวาง แนวคิดนี้เสนอโดย Elfes (1989) และยังใช้แพร่หลาย เพราะรวมข้อมูลจากเซนเซอร์หลายตัวลงตารางเดียวได้ง่าย

โดรนไม่ใช่จุด จึงต้อง ขยายสิ่งกีดขวาง (inflation) ออกไปเท่ารัศมีของลำบวกระยะปลอดภัย แล้วจึงหาเส้นทางบนพื้นที่ที่เหลือ A* (Hart, Nilsson และ Raphael, 1968) หาเส้นทางสั้นที่สุดโดยเลือกขยายช่องที่มีค่า $f=g+h$ ต่ำสุด เมื่อ $g$ คือต้นทุนที่เดินมาแล้ว และ $h$ คือค่าประมาณระยะที่เหลือ ถ้า $h$ ไม่เกินระยะจริง A* รับประกันว่าได้เส้นทางสั้นที่สุด

import heapq

grid = ["............",
        "............",
        "....###.....",
        "....###.....",
        "....###..#..",
        ".........#..",
        "............",
        "............"]
ROWS, COLS = len(grid), len(grid[0])
start, goal = (6, 1), (2, 10)


def inflate(g, r=1):
    occ = {(i, j) for i in range(ROWS) for j in range(COLS) if g[i][j] == "#"}
    return {(i + a, j + b) for i, j in occ for a in range(-r, r + 1) for b in range(-r, r + 1)
            if 0 <= i + a < ROWS and 0 <= j + b < COLS}


def astar(blocked, s, g):
    h = lambda p: abs(p[0] - g[0]) + abs(p[1] - g[1])       # ระยะแมนฮัตตัน ไม่เกินระยะจริง
    heap, came, cost, expanded = [(h(s), 0, s)], {s: None}, {s: 0}, 0
    while heap:
        _, c, p = heapq.heappop(heap)
        expanded += 1
        if p == g:
            break
        for d in ((1, 0), (-1, 0), (0, 1), (0, -1)):
            q = (p[0] + d[0], p[1] + d[1])
            if 0 <= q[0] < ROWS and 0 <= q[1] < COLS and q not in blocked and c + 1 < cost.get(q, 1e9):
                cost[q], came[q] = c + 1, p
                heapq.heappush(heap, (c + 1 + h(q), c + 1, q))
    path = [g]
    while came[path[-1]] is not None:
        path.append(came[path[-1]])
    return path[::-1], expanded


blocked = inflate(grid)
path, expanded = astar(blocked, start, goal)
print(f"path length {len(path) - 1} m, cells expanded {expanded}")
for i in range(ROWS):
    print("".join("S" if (i, j) == start else "G" if (i, j) == goal else "*" if (i, j) in path
                  else "#" if grid[i][j] == "#" else "+" if (i, j) in blocked else "." for j in range(COLS)))

path length 17 m, cells expanded 56
..*********.
..*+++++..*.
.**+###+..G.
.*.+###++++.
.*.+###++#+.
.*.++++++#+.
.S......+++.
............

หยุดทันหรือไม่

ระยะหยุดมีสองส่วน คือระยะที่เคลื่อนไประหว่างเวลาหน่วงก่อนเริ่มเบรก และระยะเบรก เหมือนการขับรถที่ต้องรวมเวลาที่เท้ายังไม่ถึงแป้นเบรก

v, latency, decel, sensor_range = 5.0, 0.25, 3.0, 8.0   # m/s, s, m/s², m
reaction = v * latency
braking = v ** 2 / (2 * decel)
stop = reaction + braking
print(f"reaction {reaction:.2f} m + braking {braking:.2f} m = {stop:.2f} m (sensor range {sensor_range} m)")
v_max = (-latency + (latency**2 + 2 * sensor_range / decel) ** 0.5) * decel
print(f"highest speed that still stops within the sensor range: {v_max:.2f} m/s")

reaction 1.25 m + braking 4.17 m = 5.42 m (sensor range 8.0 m)
highest speed that still stops within the sensor range: 6.22 m/s

ที่ 5 m/s โดรนหยุดได้ภายใน 5.42 m ซึ่งน้อยกว่าระยะเซนเซอร์ 8 m แต่ความเร็วสูงสุดที่ยังหยุดทันคือประมาณ 6.2 m/s เท่านั้น ในทางปฏิบัติต้องเผื่อเพิ่มอีกสำหรับลม ความคลาดเคลื่อนของตำแหน่ง และระยะที่เซนเซอร์ตรวจจับวัตถุบางอย่างไม่ได้ เช่น สายไฟ

ระบบหลบหลีกในตัวควบคุมการบิน

• PX4 Collision Prevention ทำงานเฉพาะโหมด Position ของโดรนหลายใบพัด จำกัดความเร็วเมื่อเข้าใกล้สิ่งกีดขวาง โดยรับข้อมูลระยะจากตัววัดระยะหรือจากคอมพิวเตอร์บนลำผ่านข้อความ OBSTACLE_DISTANCE ระยะที่ต้องการเว้นตั้งด้วย CP_DIST ซึ่งค่าเริ่มต้นเป็น −1 คือ ปิดอยู่
• ArduPilot มีหลายระดับ ตั้งแต่ Simple avoidance ที่หยุดหน้าสิ่งกีดขวาง ไปจนถึง BendyRuler ที่หาทางอ้อมรอบสิ่งกีดขวาง (ใช้ในโหมด Auto, Guided และ RTL) และ Dijkstra’s ที่วางเส้นทางรอบ fence และพื้นที่ห้ามบินที่รู้ล่วงหน้า

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

SLAM และการประเมิน trajectory

SLAM (simultaneous localization and mapping) คือการสร้างแผนที่และหาตำแหน่งตัวเองในแผนที่นั้นไปพร้อมกัน ใช้ในที่ที่ไม่มี GNSS เช่น ในอาคารหรือใต้สะพาน บทความของ Cadena และคณะ (2016) สรุปพัฒนาการของ SLAM และ ORB-SLAM3 เป็นไลบรารีเปิดที่รองรับกล้องเดี่ยว กล้องคู่ และกล้องร่วมกับ IMU

วิธีวัดคุณภาพที่นิยมคือ ATE (absolute trajectory error) จากชุดทดสอบของ Sturm และคณะ (2012) คือค่า RMSE ของระยะระหว่างตำแหน่งที่ประมาณได้กับตำแหน่งจริง Zhang และ Scaramuzza (2018) เตือนว่าต้อง จัดแนว (align) สอง trajectory ให้อยู่ในกรอบเดียวกันก่อน และต้องบอกวิธีจัดแนวทุกครั้ง

import numpy as np

truth = np.array([[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [5, 0]], dtype=float)
estimate = np.array([[0, 0], [1.1, 0.1], [2.1, 0.2], [3.2, 0.2], [4.2, 0.3], [5.3, 0.4]])
errors = np.linalg.norm(estimate - truth, axis=1)
print("error per pose (m):", np.round(errors, 2))
print(f"ATE RMSE {np.sqrt(np.mean(errors ** 2)):.3f} m, final drift {errors[-1]:.2f} m")

error per pose (m): [0.   0.14 0.22 0.28 0.36 0.5 ]
ATE RMSE 0.297 m, final drift 0.50 m

ความคลาดเคลื่อนเพิ่มขึ้นตามระยะทาง ซึ่งเป็นลักษณะของการลอย (drift) ตัวอย่างนี้สองชุดอยู่ในกรอบเดียวกันอยู่แล้วจึงไม่ต้องจัดแนว ข้อมูลจริงต้องจัดแนวก่อน

ปฏิบัติการประจำโมดูล

ข้อผิดพลาดที่พบบ่อย

สรุป

• Occupancy grid แทนพื้นที่เป็นตาราง ต้องขยายสิ่งกีดขวางตามขนาดลำก่อนหาเส้นทาง
• A* เลือกช่องที่ $g+h$ ต่ำสุด และได้เส้นทางสั้นที่สุดเมื่อ $h$ ไม่เกินระยะจริง
• ระยะหยุดคือระยะระหว่างเวลาหน่วงบวกระยะเบรก ต้องน้อยกว่าระยะเซนเซอร์พร้อมส่วนเผื่อ
• PX4 และ ArduPilot มีระบบหลบหลีกในตัว แต่พึ่งเซนเซอร์และต้องเปิดใช้เอง
• SLAM สร้างแผนที่และหาตำแหน่งพร้อมกัน และประเมินด้วย ATE หลังจัดแนว

แบบฝึกตรวจความเข้าใจ

1. ใน A* ถ้า $g=7$ และ $h=5$ ค่า $f$ ของช่องนั้นเท่าใด
2. ความเร็ว 4 m/s เวลาหน่วง 0.5 s หน่วง 2 m/s² ระยะหยุดเท่าใด
3. ทำไมต้องขยายสิ่งกีดขวางก่อนหาเส้นทาง
4. ค่าเริ่มต้น CP_DIST = −1 ใน PX4 หมายความว่าอะไร
5. ความคลาดเคลื่อนของตำแหน่งสามจุดคือ 0, 3 และ 4 m ATE RMSE เท่าใด