python
import math
import random
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
START = (9.0, 50.0)
GOAL = (92.0, 50.0)
WORLD = (0.0, 0.0, 100.0, 100.0)
OBSTACLES = [
(36.0, 26.0, 65.0, 73.0)
]
ROBOT_RADIUS = 1.0
STEP_SIZE = 6.0
GOAL_BIAS = 0.10
MAX_ITERATIONS = 5000
def distance(a, b):
return math.hypot(
a[0] - b[0],
a[1] - b[1]
)
def steer(start, target, step_size):
"""从start向target推进不超过step_size的距离。"""
d = distance(start, target)
if d == 0:
return start
scale = min(1.0, step_size / d)
return (
start[0] + scale * (target[0] - start[0]),
start[1] + scale * (target[1] - start[1])
)
def segment_intersects_box(a, b, rect, margin=0.0):
"""
用参数区间裁剪法判断线段是否与矩形相交。
将矩形向四周扩展margin，提供保守安全裕量。
此法以轴对齐外扩矩形近似机器人圆形占据区域。
"""
xmin, ymin, xmax, ymax = rect
xmin -= margin
ymin -= margin
xmax += margin
ymax += margin
dx = b[0] - a[0]
dy = b[1] - a[1]
tmin, tmax = 0.0, 1.0
for pos, delta, lower, upper in [
(a[0], dx, xmin, xmax),
(a[1], dy, ymin, ymax)
]:
if abs(delta) < 1e-12:
if pos < lower or pos > upper:
return False
continue
t1 = (lower - pos) / delta
t2 = (upper - pos) / delta
if t1 > t2:
t1, t2 = t2, t1
tmin = max(tmin, t1)
tmax = min(tmax, t2)
if tmin > tmax:
return False
return True
def collision_free(a, b):
"""检查整条候选线段是否处于允许的空间。"""
xmin, ymin, xmax, ymax = WORLD
for p in (a, b):
if not (
xmin + ROBOT_RADIUS <= p[0] <= xmax - ROBOT_RADIUS
):
return False
if not (
ymin + ROBOT_RADIUS <= p[1] <= ymax - ROBOT_RADIUS
):
return False
for rect in OBSTACLES:
if segment_intersects_box(
a, b, rect, ROBOT_RADIUS
):
return False
return True
def rrt(seed=42):
rng = random.Random(seed)
nodes = [(START, -1)]
for iteration in range(1, MAX_ITERATIONS + 1):
if rng.random() < GOAL_BIAS:
sample = GOAL
else:
sample = (
rng.uniform(ROBOT_RADIUS, 100 - ROBOT_RADIUS),
rng.uniform(ROBOT_RADIUS, 100 - ROBOT_RADIUS)
)
nearest_idx = min(
range(len(nodes)),
key=lambda i: distance(nodes[i][0], sample)
)
nearest = nodes[nearest_idx][0]
new_state = steer(
nearest, sample, STEP_SIZE
)
if distance(nearest, new_state) < 1e-10:
continue
if not collision_free(nearest, new_state):
continue
nodes.append((new_state, nearest_idx))
new_idx = len(nodes) - 1
if (
distance(new_state, GOAL) <= STEP_SIZE
and collision_free(new_state, GOAL)
):
nodes.append((GOAL, new_idx))
goal_idx = len(nodes) - 1
path = []
idx = goal_idx
while idx != -1:
state, parent_idx = nodes[idx]
path.append(state)
idx = parent_idx
path.reverse()
return nodes, path, iteration
return nodes, None, MAX_ITERATIONS
nodes, path, iterations = rrt(seed=42)
print("采样轮次:", iterations)
print("树节点数量:", len(nodes))
if path is None:
print("本次预算内没有找到路径")
else:
length = sum(
distance(path[i], path[i + 1])
for i in range(len(path) - 1)
)
print("可行路径长度:", round(length, 2))
fig, ax = plt.subplots(figsize=(8, 8))
for xmin, ymin, xmax, ymax in OBSTACLES:
ax.add_patch(
Rectangle(
(xmin, ymin),
xmax - xmin,
ymax - ymin,
alpha=0.55
)
)
for i, (state, parent) in enumerate(nodes):
if parent == -1:
continue
p = nodes[parent][0]
ax.plot(
[p[0], state[0]],
[p[1], state[1]],
linewidth=0.7,
alpha=0.45
)
if path is not None:
xs, ys = zip(*path)
ax.plot(xs, ys, linewidth=3, label="RRT Path")
ax.scatter(*START, s=90, label="Start")
ax.scatter(*GOAL, s=90, label="Goal")
ax.set_xlim(0, 100)
ax.set_ylim(0, 100)
ax.set_aspect("equal")
ax.set_title("Rapidly-exploring Random Tree")
ax.legend()
plt.show()