RRTをPythonで実装する｜ロボットが「狭い隙間」を抜ける経路計画

pip install numpy

import random
import math
import numpy as np
 
random.seed(7)
 
# 障害物: (中心x, 中心y, 半径)
obstacles = [(5, 5, 1.5), (3, 7, 1.0), (7, 3, 1.2), (6, 7.5, 0.8)]
START = (0.5, 0.5)
GOAL = (9.5, 9.5)
BOUNDS = (0, 10)
STEP = 0.5          # 1回に伸ばす歩幅
GOAL_RADIUS = 0.5    # ここまで近づけばゴール扱い
MAX_ITERS = 5000
 
 
def collides(p):
    for cx, cy, r in obstacles:
        if math.hypot(p[0] - cx, p[1] - cy) <= r:
            return True
    return False
 
 
def segment_collides(a, b, samples=10):
    for i in range(samples + 1):
        t = i / samples
        p = (a[0] + (b[0] - a[0]) * t, a[1] + (b[1] - a[1]) * t)
        if collides(p):
            return True
    return False

def steer(a, b, step):
    dx, dy = b[0] - a[0], b[1] - a[1]
    dist = math.hypot(dx, dy)
    if dist <= step:
        return b
    return (a[0] + dx / dist * step, a[1] + dy / dist * step)

nodes = [START]
parents = {0: None}
 
goal_reached_idx = None
for it in range(MAX_ITERS):
    if random.random() < 0.05:
        rnd = GOAL
    else:
        rnd = (random.uniform(*BOUNDS), random.uniform(*BOUNDS))
 
    dists = [math.hypot(rnd[0] - n[0], rnd[1] - n[1]) for n in nodes]
    nearest_idx = int(np.argmin(dists))
    nearest = nodes[nearest_idx]
 
    new_node = steer(nearest, rnd, STEP)
    if collides(new_node) or segment_collides(nearest, new_node):
        continue
 
    nodes.append(new_node)
    new_idx = len(nodes) - 1
    parents[new_idx] = nearest_idx
 
    if math.hypot(new_node[0] - GOAL[0], new_node[1] - GOAL[1]) <= GOAL_RADIUS:
        goal_reached_idx = new_idx
        break

print(f"サンプリング回数: {it + 1}")
print(f"木のノード数: {len(nodes)}")
 
if goal_reached_idx is not None:
    path = []
    idx = goal_reached_idx
    while idx is not None:
        path.append(nodes[idx])
        idx = parents[idx]
    path.reverse()
    length = sum(
        math.hypot(path[i + 1][0] - path[i][0], path[i + 1][1] - path[i][1])
        for i in range(len(path) - 1)
    )
    print(f"ゴールまでの経路: ノード数 {len(path)}, 経路長 {length:.2f}")
    print("経路の一部:", [(round(x, 2), round(y, 2)) for x, y in path[:5]])
else:
    print("ゴールに到達できませんでした")

サンプリング回数: 229
木のノード数: 175
ゴールまでの経路: ノード数 32, 経路長 15.50
経路の一部: [(0.5, 0.5), (0.58, 0.99), (1.02, 1.24), (1.51, 1.13), (2.01, 1.14)]