RRT-Connect

Trong motion planning, câu hỏi thường không phải là robot nên đi nhanh hay chậm, mà là robot có tìm được một đường đi hợp lệ hay không. Với một cánh tay robot trong cell lắp ráp, ta có cấu hình bắt đầu, cấu hình đích, và một đống ràng buộc khó chịu: không va vào đồ gá, không chạm bàn, không tự va vào chính nó, không đi qua vùng cấm, đôi khi còn phải né dây cáp hoặc camera gắn trên end-effector.

Nếu không gian chỉ là mặt phẳng 2D, ta còn có thể tưởng tượng việc vẽ đường đi quanh vật cản. Nhưng với robot arm 6 bậc tự do, mỗi điểm trong không gian tìm kiếm không còn là $(x, y)$ nữa. Nó là một vector joint:

Trong đó:

• $q$ là một cấu hình robot.
• $q_i$ là góc hoặc vị trí của joint thứ $i$.
• $n$ là số bậc tự do.

Không gian mà thuật toán phải tìm đường là configuration space, thường gọi là C-space. Một điểm trong C-space tương ứng với một tư thế robot đầy đủ. Một vùng trong C-space bị xem là cấm nếu tư thế robot tương ứng gây va chạm.

RRT-Connect là một cách rất thực dụng để tìm đường trong không gian đó. Nó không cố xây toàn bộ bản đồ C-space. Nó lấy mẫu, nối dần các cấu hình hợp lệ thành cây, và đặc biệt là dùng hai cây: một cây mọc từ start, một cây mọc từ goal. Hai cây này thay phiên nhau tiến về phía nhau cho đến khi gặp được nhau.

Đặc thù của bài toán motion planning trong không gian cấu hình

Giả sử ta muốn đưa đầu gắp từ bên trái sang bên phải một đồ gá. Trong không gian Cartesian, đường thẳng từ điểm đầu đến điểm cuối nhìn có vẻ rất đẹp. Nhưng robot arm không di chuyển trực tiếp bằng điểm đầu gắp. Nó di chuyển bằng joint. Một đoạn thẳng trong workspace có thể yêu cầu joint xoay qua vùng tự va chạm, hoặc elbow đập vào bàn.

Ngay cả phép nội suy đơn giản giữa hai cấu hình cũng chưa chắc hợp lệ:

Trong đó:

• $q_\text{start}$ là cấu hình ban đầu.
• $q_\text{goal}$ là cấu hình đích.
• $\alpha$ là tham số nội suy từ 0 đến 1.
• $q(\alpha)$ là cấu hình trung gian trên đoạn nối thẳng trong C-space.

Nếu bất kỳ $q(\alpha)$ nào gây va chạm, đoạn thẳng này không dùng được. Điều này xảy ra thường xuyên. Hai cấu hình đầu-cuối đều an toàn, nhưng đoạn nối giữa chúng đi xuyên qua vùng cấm.

Vì vậy, motion planning cần một chiến lược tìm đường qua các vùng hẹp của C-space mà không phải mô hình hóa toàn bộ không gian. RRT thuộc nhóm sampling-based planning: thay vì quét lưới dày đặc, nó thử nhiều mẫu ngẫu nhiên và chỉ giữ các kết nối hợp lệ.

Nguyên lý mở rộng cây trong thuật toán RRT

RRT, viết tắt của Rapidly-exploring Random Tree, bắt đầu từ một cấu hình gốc. Ở mỗi vòng lặp, nó lấy một mẫu ngẫu nhiên $q_\text{rand}$ trong C-space, tìm node gần nhất trong cây, rồi kéo cây thêm một đoạn nhỏ về phía mẫu đó.

Gọi node gần nhất là:

Trong đó:

• $T$ là tập node hiện có trong cây.
• $d(\cdot, \cdot)$ là metric trong C-space, thường là khoảng cách Euclidean có cân nhắc giới hạn joint.
• $q_\text{near}$ là node trong cây gần mẫu ngẫu nhiên nhất.

Sau đó ta tạo node mới bằng một bước steer:

Trong đó:

• $\eta$ là step size.
• $q_\text{new}$ là cấu hình mới được đề xuất.
• Vector phân số chỉ hướng từ $q_\text{near}$ đến $q_\text{rand}$.

Nếu đoạn từ $q_\text{near}$ đến $q_\text{new}$ không va chạm, ta thêm $q_\text{new}$ vào cây. Theo thời gian, cây có xu hướng lan ra các vùng chưa được khám phá. Đây là lý do RRT chạy khá tốt trong không gian nhiều chiều.

Nhược điểm là RRT một cây có thể mất nhiều thời gian để tình cờ chạm tới vùng gần goal, nhất là khi goal nằm sau một lối hẹp. RRT-Connect sửa đúng điểm đó bằng cách cho cả start và goal cùng tham gia tìm nhau.

Cấu trúc hai cây trong thuật toán RRT-Connect

RRT-Connect duy trì hai cây:

Một cây mọc từ start, cây còn lại mọc từ goal. Ở mỗi vòng lặp, thuật toán mở rộng một cây về phía mẫu ngẫu nhiên. Sau khi cây đó có node mới, cây còn lại cố gắng nối thẳng nhiều bước về phía node mới này. Nếu nối được, hai cây gặp nhau và ta có đường đi.

Điểm khác biệt nằm ở chữ Connect. RRT thường chỉ Extend một bước ngắn. RRT-Connect, khi đã có mục tiêu nối, sẽ tiếp tục kéo cây còn lại nhiều bước liên tiếp cho đến khi:

• gặp được node mục tiêu,
• bị va chạm,
• hoặc không thể tiến thêm.

Mỗi vòng lặp thường hoán đổi vai trò hai cây. Vòng này cây start mở rộng trước, cây goal cố connect. Vòng sau đổi lại. Nhờ vậy, cả hai phía đều có cơ hội khám phá không gian và tiến về phía đối phương.

So sánh cơ chế Extend và Connect

Ta có thể xem Extend là một bước dè dặt. Nó chỉ đi từ $q_\text{near}$ về phía target một đoạn dài tối đa $\eta$. Nếu đoạn đó hợp lệ, nó thêm một node.

Connect thì tham hơn. Nó lặp lại Extend nhiều lần về cùng một target:

while True:
    status = Extend(tree, target)
    if status != ADVANCED:
        return status

Trong đó:

• ADVANCED nghĩa là cây vừa tiến thêm được một bước.
• REACHED nghĩa là cây đã chạm target.
• TRAPPED nghĩa là đường đi bị chặn bởi va chạm.

Sự tham lam này làm RRT-Connect nhanh hơn RRT hai cây đơn giản trong nhiều bài toán. Khi một cây vừa tìm được một node nằm ở vùng thuận lợi, cây còn lại không chỉ nhích một bước; nó cố chạy thẳng tới đó hết mức có thể.

Đổi lại, nếu C-space có nhiều vùng hẹp và ngoằn ngoèo, việc connect quá tham có thể liên tục đập vào vật cản. Nhưng ngay cả khi thất bại, các node mới đã thêm vào trước khi bị chặn vẫn giúp cây mở rộng vùng đã biết.

Chi phí tính toán của bước kiểm tra va chạm

Nhìn pseudocode, RRT-Connect có vẻ đơn giản: sample, nearest, steer, collision check. Trong triển khai robot thật, collision checking thường là phần đắt nhất.

Một đoạn nối giữa hai cấu hình:

không thể chỉ kiểm tra hai đầu mút. Ta phải lấy nhiều điểm trung gian:

và kiểm tra từng cấu hình $\gamma(\alpha_k)$ có va chạm hay không. Nếu bước kiểm tra quá thưa, robot có thể “nhảy qua” vật cản mỏng. Nếu quá dày, planner chậm.

Trong đó:

• $q_a$ và $q_b$ là hai cấu hình cần nối.
• $\gamma(\alpha)$ là cấu hình nội suy.
• $N$ là số mẫu trung gian dùng để kiểm tra collision.

Với robot arm, mỗi lần collision check có thể phải chạy forward kinematics, dựng hình học từng link, rồi kiểm tra va chạm với môi trường và tự va chạm. Vì vậy, tăng step_size hay giảm độ phân giải collision không đơn giản là tối ưu tốc độ. Nó thay đổi độ an toàn của đường đi.

Minh họa thuật toán RRT-Connect trong không gian 2D

Đoạn code dưới đây minh họa RRT-Connect trên mặt phẳng 2D có vật cản hình chữ nhật. Đây không phải bài toán robot arm nhiều bậc tự do, nhưng cơ chế hai cây, extend, connect và collision checking giống nhau về mặt thuật toán.

import math
import random
 
import matplotlib.pyplot as plt
 
random.seed(4)
 
start = (0.08, 0.1)
goal = (0.92, 0.88)
step = 0.045
max_iter = 2500
obstacles = [
    (0.28, 0.18, 0.44, 0.72),
    (0.58, 0.32, 0.74, 0.9),
]
 
 
def dist(a, b):
    return math.hypot(a[0] - b[0], a[1] - b[1])
 
 
def steer(a, b):
    d = dist(a, b)
    if d <= step:
        return b
    r = step / d
    return (a[0] + r * (b[0] - a[0]), a[1] + r * (b[1] - a[1]))
 
 
def in_obstacle(p):
    x, y = p
    for xmin, ymin, xmax, ymax in obstacles:
        if xmin <= x <= xmax and ymin <= y <= ymax:
            return True
    return False
 
 
def edge_free(a, b, n=16):
    for i in range(n + 1):
        t = i / n
        p = (a[0] * (1 - t) + b[0] * t, a[1] * (1 - t) + b[1] * t)
        if in_obstacle(p):
            return False
    return True
 
 
def nearest(tree, p):
    return min(range(len(tree)), key=lambda i: dist(tree[i]["p"], p))
 
 
def extend(tree, target):
    i = nearest(tree, target)
    q_near = tree[i]["p"]
    q_new = steer(q_near, target)
    if not edge_free(q_near, q_new):
        return "TRAPPED", None
    tree.append({"p": q_new, "parent": i})
    if dist(q_new, target) < 1e-9:
        return "REACHED", len(tree) - 1
    return "ADVANCED", len(tree) - 1
 
 
def connect(tree, target):
    last = None
    while True:
        status, idx = extend(tree, target)
        if status == "TRAPPED":
            return "TRAPPED", last
        last = idx
        if status == "REACHED":
            return "REACHED", idx
 
 
def trace(tree, idx):
    path = []
    while idx is not None:
        path.append(tree[idx]["p"])
        idx = tree[idx]["parent"]
    return path[::-1]
 
 
ta = [{"p": start, "parent": None}]
tb = [{"p": goal, "parent": None}]
solution = None
 
for _ in range(max_iter):
    q_rand = (random.random(), random.random())
    status, ia = extend(ta, q_rand)
    if status != "TRAPPED":
        q_new = ta[ia]["p"]
        status_b, ib = connect(tb, q_new)
        if status_b == "REACHED":
            path_a = trace(ta, ia)
            path_b = trace(tb, ib)
            solution = path_a + path_b[::-1]
            break
    ta, tb = tb, ta
 
fig, ax = plt.subplots(figsize=(6, 6), dpi=180)
for xmin, ymin, xmax, ymax in obstacles:
    ax.add_patch(plt.Rectangle((xmin, ymin), xmax - xmin, ymax - ymin, color="#D9D9D9"))
 
for tree, color in [(ta, "#111111"), (tb, "#808080")]:
    for i, node in enumerate(tree):
        if node["parent"] is not None:
            p = node["p"]
            q = tree[node["parent"]]["p"]
            ax.plot([p[0], q[0]], [p[1], q[1]], color=color, linewidth=0.5, alpha=0.75)
 
if solution:
    xs, ys = zip(*solution)
    ax.plot(xs, ys, color="#111111", linewidth=2.4)
 
ax.scatter(*start, color="#111111", s=30)
ax.scatter(*goal, color="#111111", marker="x", s=45)
ax.set_xlim(0, 1)
ax.set_ylim(0, 1)
ax.set_aspect("equal")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.grid(True, color="#D9D9D9", linewidth=0.5)
plt.tight_layout()
plt.savefig("rrt_connect_demo.png")

Điều nên nhìn trong hình không phải là đường đi đã tối ưu hay chưa. RRT-Connect không sinh đường ngắn nhất. Nó sinh một đường hợp lệ khá nhanh. Sau đó, hệ motion planning thường thêm bước shortcutting hoặc path smoothing để làm đường đi gọn hơn trước khi đưa xuống controller.

Ảnh hưởng của tham số đến hiệu năng lập kế hoạch

RRT-Connect có ít tham số hơn nhiều thuật toán tối ưu, nhưng mỗi tham số đều có tác động rõ.

Với robot arm, metric rất đáng chú ý. Nếu ta dùng Euclidean distance trực tiếp trên joint, một joint nhỏ ở cổ tay và một joint lớn ở vai có thể bị đối xử quá giống nhau. Trong nhiều hệ thực tế, metric cần scale theo joint range, giới hạn vận tốc, hoặc ảnh hưởng của joint lên end-effector.

RRT-Connect cũng không đảm bảo đường đi đẹp. Nó có tính probabilistic completeness: nếu tồn tại đường đi và ta cho thuật toán đủ thời gian, xác suất tìm được đường sẽ tiến tới 1. Nhưng điều đó không nói đường tìm được là ngắn, mượt, hay phù hợp để robot chạy ngay.

Phạm vi áp dụng của thuật toán RRT-Connect

RRT-Connect thường hợp khi ta cần tìm một đường khả thi nhanh trong không gian nhiều chiều, nhất là khi collision checking là hộp đen phức tạp. Đây là lý do nó xuất hiện rất nhiều trong motion planning cho robot arm.

Nó kém phù hợp nếu bài toán yêu cầu tối ưu mạnh ngay từ đầu: đường ngắn nhất, quỹ đạo mượt theo động lực học, ràng buộc vận tốc/gia tốc chặt, hoặc chi phí đi qua các vùng khác nhau. Khi đó, RRT-Connect thường đóng vai trò tạo seed ban đầu, rồi một thuật toán khác sẽ làm smoothing hoặc trajectory optimization.

Khi debug RRT-Connect, hãy nhìn vào cây trước khi nhìn vào path. Nếu cây không đi qua được một vùng hẹp, có thể step_size quá lớn, sampling không đủ, collision resolution quá thô, hoặc metric khiến cây ưu tiên sai hướng. Nếu cây tìm được đường nhưng robot chạy không mượt, đó không còn là lỗi tìm đường thô nữa; đó là việc của smoothing, time parameterization và controller.

RRT-Connect đáng học vì nó cho thấy một tư duy rất robotics: thay vì giải chính xác một không gian quá khó, ta lấy mẫu đủ thông minh, kiểm tra va chạm thật cẩn thận, và chấp nhận tìm một đường hợp lệ trước. Trong nhiều hệ robot công nghiệp, có được một đường hợp lệ nhanh và đáng tin đã là nửa trận đấu.