RRT — швидкозростаючі випадкові дерева для планування шляху робота

1. Простір конфігурацій

Конфігурація робота q — це повний опис його стану (кути в суглобах, 6D-поза тощо). Простір конфігурацій C — це множина всіх можливих конфігурацій. C_free ⊂ C виключає конфігурації, що стикаються з перешкодами.

2. Алгоритм RRT

3. Зміщення до цілі та варіант Connect

4. RRT* — асимптотична оптимальність

5. Інформований RRT*

6. Двонаправлений RRT

7. Реалізація на JavaScript

// RRT для точкового робота у 2D з круговими перешкодами
class RRT {
  constructor(start, goal, obstacles, bounds, {stepSize=20, goalBias=0.1, maxIter=5000}={}) {
    this.goal = goal; this.obstacles = obstacles; this.bounds = bounds;
    this.stepSize = stepSize; this.goalBias = goalBias; this.maxIter = maxIter;
    this.nodes = [start];         // {x, y, parentIdx}
    start.parentIdx = -1;
  }

  sample() {
    if (Math.random() < this.goalBias) return {...this.goal};
    return {
      x: this.bounds.minX + Math.random() * (this.bounds.maxX - this.bounds.minX),
      y: this.bounds.minY + Math.random() * (this.bounds.maxY - this.bounds.minY)
    };
  }

  nearest(q) {
    let best = 0, bestD = Infinity;
    for (let i = 0; i < this.nodes.length; i++) {
      const d = dist(this.nodes[i], q);
      if (d < bestD) { bestD = d; best = i; }
    }
    return best;
  }

  steer(from, to) {
    const d = dist(from, to);
    if (d <= this.stepSize) return {...to};
    const t = this.stepSize / d;
    return {x: from.x + t*(to.x-from.x), y: from.y + t*(to.y-from.y)};
  }

  collisionFree(a, b) {
    // Вибираємо точки вздовж сегмента, перевіряємо кожну на перешкоди
    const steps = Math.ceil(dist(a, b) / 5);
    for (let i = 0; i <= steps; i++) {
      const t = i / steps;
      const px = a.x + t*(b.x-a.x), py = a.y + t*(b.y-a.y);
      if (this.obstacles.some(o => dist({x:px,y:py}, o) <= o.r)) return false;
    }
    return true;
  }

  build() {
    for (let i = 0; i < this.maxIter; i++) {
      const q_rand = this.sample();
      const nearIdx = this.nearest(q_rand);
      const q_new = this.steer(this.nodes[nearIdx], q_rand);
      if (!this.collisionFree(this.nodes[nearIdx], q_new)) continue;
      q_new.parentIdx = nearIdx;
      this.nodes.push(q_new);
      if (dist(q_new, this.goal) < this.stepSize && this.collisionFree(q_new, this.goal))
        return this.extractPath(this.nodes.length - 1);
    }
    return null; // не знайдено
  }

  extractPath(idx) {
    const path = [];
    while (idx !== -1) { path.unshift(this.nodes[idx]); idx = this.nodes[idx].parentIdx; }
    path.push(this.goal);
    return path;
  }
}

function dist(a, b) { return Math.hypot(a.x-b.x, a.y-b.y); }

// Використання:
const rrt = new RRT(
  {x:50, y:50},  {x:750, y:550},
  [{x:300, y:200, r:80}, {x:500, y:400, r: 100}],
  {minX:0, maxX:800, minY:0, maxY:600}
);
const path = rrt.build();
if (path) console.log(`Знайдено шлях: ${path.length} проміжних точок`);

8. Варіанти та застосування