9. hafta / 14

Yol planlama ve konum belirleme

Araçlar: Python (NumPy), bu platform

Bu haftanın sonunda

Bir haritayı çizge (graph) veya ızgara (grid) olarak temsil etmek
BFS, açgözlü en iyi öncelikli (greedy best-first) arama ve A* algoritmalarını uygulamak
Sezgisel fonksiyonları (heuristics), kabul edilebilirliği (admissibility) ve optimalliği açıklamak
İnanç (belief), tahmin ve düzeltme kavramlarını açıklamak
Yer imi (landmark) tabanlı konumlandırma için bir parçacık filtresi (particle filter) uygulamak
SLAM'in neyi çözdüğünü ve bunun için kullanılan araçları açıklamak

Ders notları

Bir planlayıcı şu soruyu yanıtlar: hiçbir şeye çarpmadan buradan hedefe nasıl giderim? Önce dünyayı üzerinde arama yapabileceğimiz bir yapıya dönüştürürüz.

Konfigürasyon uzayı

Robotun tam şekli için plan yapmak yerine robotu bir noktaya küçültür ve her engeli robotun yarıçapı kadar büyütürüz (engel şişirme, obstacle inflation). Artık bir nokta için plan yapmak yeterlidir. ROS maliyet haritaları (costmaps) tam olarak bunu yapar.

Izgara üzerinde çizge araması

Genişlik öncelikli arama (breadth-first search) halkalar halinde keşfeder. Ağırlıksız bir ızgarada en kısa yolu bulur, ancak pek çok hücreyi genişletir.
Dijkstra, BFS'i ağırlıklı kenarlara genelleştirir (örneğin çamur, fayanstan daha maliyetlidir).
Açgözlü en iyi öncelikli (greedy best-first) arama, her zaman hedefe en yakın görünen hücreyi genişletir. Hızlıdır, ancak yol optimal olmaktan çok uzak olabilir.
A* ikisini birleştirir:

f(n) = g(n) + h(n)

g o ana kadarki maliyettir, h ise kalan maliyeti kestirir. h hiçbir zaman fazla tahmin etmiyorsa (yani kabul edilebilir (admissible) ise), A* optimal bir yol döndürür. 4 bağlantılı bir ızgarada Manhattan uzaklığını, 8 bağlantılı bir ızgarada oktil (octile) uzaklığı kullanın.

Izgaraların ötesi

Kollar ve yüksek boyutlu uzaylar için ızgaraların boyutu patlar. Bunun yerine RRT ve RRT* gibi örnekleme tabanlı planlayıcılar, rastgele seçilmiş çarpışmasız örneklerden bir ağaç büyütür. Gerçek robotlar ayrıca küresel bir planlayıcıyı, hareketli engellere tepki veren yerel bir planlayıcıyla (örneğin DWA) eşleştirir.

Konum belirleme ve SLAM

Odometri kayar (7. hafta) ve planlama (9. hafta) nerede olduğumuzu bildiğimizi varsayar. Konumlandırma (localization), robotun pozunu hareket ve sensör verilerinden kestirir ve bu kestirimin ne kadar belirsiz olduğunu izler.

Bayes filtresi

Robot bir inanç (belief) tutar: olası pozlar üzerinde bir olasılık dağılımı. Her çevrimin iki adımı vardır:

Tahmin (predict): inancı hareket komutunu kullanarak ilerlet. Belirsizlik artar.
Düzeltme (correct): inancı, her pozun ölçümü ne kadar iyi açıkladığına göre ağırlıklandır. Belirsizlik azalır.

Kalman filtresi (4. hafta), Gauss dağılımlı inançlar ve doğrusal modeller için Bayes filtresidir; EKF ise doğrusal olmayan modelleri doğrusallaştırır.

Parçacık filtresi (Monte Carlo konumlandırma)

İnancı çok sayıda örnekle (parçacıkla) temsil edin. Her birini gürültülü hareketle ilerletin, her birine ölçüm olabilirliğinden (likelihood) bir ağırlık verin, ardından yeniden örnekleyin; böylece olası parçacıklar çoğalır, olası olmayanlar kaybolur. Bu yöntem, iki özdeş koridordan birinde olabilecek bir robot gibi çok modlu (multimodal) inançları ele alabilir.

Kaçırılmış robot (kidnapped robot) problemi: robot haberi olmadan başka bir yere taşınırsa tüm parçacıklar yanlış olur. Her çevrimde birkaç rastgele parçacık eklemek, filtrenin toparlanmasını sağlar.

SLAM

Eşzamanlı Konumlandırma ve Haritalama (Simultaneous Localization and Mapping), haritayı oluştururken aynı zamanda o harita içinde konumlanır. LiDAR SLAM ardışık taramaları eşleştirir ve robot daha önce gördüğü bir yeri tanıdığında birikmiş hatayı düzeltir (döngü kapatma, loop closure). Yaygın araçlar: LiDAR için slam_toolbox ve Cartographer, kameralar için ORB-SLAM3.

Derinlemesine

Çözümlü örnek: A*'ın ilk genişletmesi

Düz hareketlerin maliyeti 1, çapraz hareketlerin maliyeti √2 olan 8 komşulu bir ızgara, başlangıç S = (0, 0), hedef G = (3, 2) ve engelsiz bir ortam alalım. Oktil (octile) sezgisel fonksiyon şudur:

h(n) = max(Δx, Δy) + (√2 − 1) · min(Δx, Δy)

Dolayısıyla h(S) = 3 + 0,414 × 2 = 3,83 olur. S genişletildiğinde:

Hücre	g	h	f = g + h
(1, 0)	1,00	2,83	3,83
(1, 1)	1,41	2,41	3,83
(0, 1)	1,00	3,41	4,41

(1, 0) ve (1, 1), 3,83'te berabere kalır. Eşitliği daha büyük g yönünde (hedefe daha yakın hücre) bozmak, A*'ın eşit derecede iyi tüm yollara yayılmak yerine tek bir en iyi yol boyunca ilerlemesini sağlar. Boş bir ızgarada oktil mesafe gerçek maliyete eşittir, bu yüzden A* yalnızca en iyi yollar üzerindeki hücreleri genişletir; engeller h'yi gerçek maliyetin altına düşürür ve daha fazla hücre genişletilir.

Şişirme, çözünürlük ve bellek

0,05 m'lik bir ızgarada 0,18 m yarıçaplı bir robot için engeller ⌈0,18 / 0,05⌉ = 4 hücre şişirilir. 0,05 m çözünürlükte 50 m × 50 m'lik bir zemin 1000 × 1000 = 10⁶ hücredir, hücre başına bir baytla yaklaşık 1 MB; 0,01 m'de 25 MB olur ve planlama benzer bir oranda yavaşlar. Şişirilmiş robotun en dar kapıdan hâlâ geçebildiği en kaba çözünürlüğü seçin. Ağırlıklı A* (weighted A*) (ε > 1 ile f = g + εh) hız için en iyiliği bir garantiyle feda eder: yolun maliyeti en fazla en iyinin ε katıdır.

Çözümlü örnek: parçacık ağırlıkları

Bir robot, bilinen bir işaret noktasına (landmark) σ = 0,2 m ile z = 2,0 m ölçüyor. Üç parçacık 1,8 m, 2,1 m ve 2,6 m tahmin ediyor. Gauss olabilirlik fonksiyonuyla

w_i ∝ exp(−(z − ẑ_i)² / (2σ²))

ham ağırlıklar 0,607, 0,882 ve 0,011 olur; normalleştirilince 0,404, 0,588 ve 0,007 eder. Etkin örnek sayısı şudur:

N_eff = 1 / Σ w_i² = 1 / (0.163 + 0.346 + 0.00005) ≈ 1.96

Yani üç parçacıktan yalnızca yaklaşık ikisi hâlâ bilgi taşır. Yaygın bir kural, yeniden örneklemeyi yalnızca N_eff < N / 2 olduğunda yapmak ve tek bir rastgele sayı gerektiren, çeşitliliği daha iyi koruyan düşük varyanslı (sistematik) yeniden örneklemeyi kullanmaktır. Çok sayıda LiDAR ışınıyla olabilirlikleri logaritmaların toplamı olarak çarpın; aksi hâlde çarpım sıfıra taşar (underflow).

Bir SLAM arka ucu ne yapar?

Graf SLAM, robot konumlarını düğüm, göreli ölçümleri (odometri, tarama eşleştirmeleri) ise kenar olarak saklar. Döngü kapatma (loop closure), eski ve yeni bir konum arasına bir kenar daha ekler; ardından doğrusal olmayan bir en küçük kareler çözücüsü (Ceres, g2o, GTSAM) birikmiş hatayı tüm döngüye yayar. ROS 2'deki slam_toolbox bu şekilde çalışır.

Simülasyonlar

Izgarada yol planlama

Duvar çiz, başlangıcı ve hedefi taşı, sonra aramayı çalıştır. Her algoritmanın yol bulana kadar kaç hücre genişlettiğini ve yolun en kısa olup olmadığını karşılaştır.

Parçacık filtresiyle konum belirleme

Robot dört işaret noktasına gürültülü mesafeler ölçer. Her mavi nokta robotun nerede olduğuna dair bir tahmindir; kırmızı halka filtrenin kestirimidir. Filtrenin robotu kaybedip yeniden bulduğunu görmek için robotu kaçır.

Kod

Python: doluluk ızgarası (occupancy grid) üzerinde A*

import heapq

def astar(grid, start, goal):
    rows, cols = len(grid), len(grid[0])
    h = lambda p: abs(p[0] - goal[0]) + abs(p[1] - goal[1])
    open_ = [(h(start), 0, start)]
    came, g = {start: None}, {start: 0}
    while open_:
        _, cost, cur = heapq.heappop(open_)
        if cur == goal:
            path = []
            while cur:
                path.append(cur)
                cur = came[cur]
            return path[::-1]
        if cost > g[cur]:
            continue                       # eskimiş kuyruk kaydı
        r, c = cur
        for nb in ((r+1, c), (r-1, c), (r, c+1), (r, c-1)):
            if 0 <= nb[0] < rows and 0 <= nb[1] < cols and not grid[nb[0]][nb[1]]:
                ng = cost + 1
                if ng < g.get(nb, float('inf')):
                    g[nb], came[nb] = ng, cur
                    heapq.heappush(open_, (ng + h(nb), ng, nb))
    return None

Python: mesafe ölçümleriyle tek bir parçacık filtresi adımı

import numpy as np

def pf_step(P, u, z, landmarks, dt, sig_z=10.0):
    """P: (N, 3) parçacıklar [x, y, th]; u = (v, w); z: ölçülen mesafeler (M,)."""
    v, w = u
    N = len(P)
    P[:, 2] += (w + np.random.randn(N) * 0.05) * dt
    step = (v + np.random.randn(N) * v * 0.08) * dt
    P[:, 0] += step * np.cos(P[:, 2])
    P[:, 1] += step * np.sin(P[:, 2])
    d = np.linalg.norm(P[:, None, :2] - landmarks[None], axis=2)   # (N, M)
    logw = -((d - z) ** 2).sum(axis=1) / (2 * sig_z ** 2)
    wts = np.exp(logw - logw.max())
    wts /= wts.sum()
    idx = np.random.choice(N, N, p=wts)                               # yeniden örnekle
    return P[idx].copy()

Sık yapılan hatalar

8 komşulu bir ızgarada sezgisel olarak Manhattan mesafesini kullanmak; çapraz hareketleri fazla tahmin eder, A* artık en iyi yolu garanti etmez.
Engel şişirmesi olmadan planlamak; yol, gerçek robotun geçemeyeceği kadar duvarlara sürtünür.
Bir hücreyi kuyruktan çekildiğinde değil, kuyruğa eklendiğinde kapalı işaretlemek; daha kötü bir g değeri kalıcı hâle gelebilir.
Çok sayıda küçük ışın olabilirliğini doğrudan çarpıp ağırlıkları sıfıra taşırmak; log-olabilirlik kullanın.
Ağırlıklar neredeyse eşitken bile her adımda yeniden örneklemek; parçacık çeşitliliğini gereksiz yere yok eder.
Parçacık filtresinin hareket gürültüsünü çok küçük seçmek; bulut çöker ve gerçek robotu izleyemez.

Yazılım mühendisliği bağlantısı

Planlayıcılar ve filtreler strateji kalıbının (strategy pattern) klasik kullanım yerleridir: plan(map, start, goal) -> path gibi tek bir arayüz ve onun birbirinin yerine geçebilen gerçeklemeleri olarak BFS, A* ve RRT. Sistemin geri kalanı ve her test yalnızca arayüze bağlıdır.

Kıyaslamalar (benchmarks) da birer testtir: her planlayıcıyı sabit bir harita kümesinde çalıştırın; yol maliyetini, genişletilen düğüm sayısını ve çalışma süresini kaydedin. Maliyet kötüleşirse veya süre bütçeyi aşarsa derlemeyi başarısız kılın.
Determinizm: rastgele algoritmalar (RRT, parçacık filtreleri) bir tohum (seed) parametresi alır; böylece CI'da başarısız olan bir çalışma dizüstü bilgisayarda birebir yeniden üretilebilir.
Karmaşıklığı bilin: ikili yığınlı (binary heap) A*, n hücreli bir ızgarada en kötü durumda O(n log n)'dir; bu, 25 MB'lık haritanın maliyetini denemeden önce söyler.

Dönem projesi uygulaması: planlayıcınızı veya konum belirleyicinizi bir arayüzün arkasına koyun, üç sabit haritalı ve tohumlu bir kıyaslama betiği ekleyin ve onu saklanan referans sonuçlarla CI'da çalıştırın.

Laboratuvar görevi

Simülatörde üç rastgele harita oluşturun. Her biri için BFS, açgözlü arama ve A*'ın genişlettiği hücre sayısını ve yol uzunluğunu kaydedin. Açgözlü aramanın A*'tan belirgin şekilde daha uzun bir yol döndürdüğü bir harita bulun. Simülatörde 100, 500 ve 2000 parçacıkla ve 5 ile 30 mesafe gürültüsüyle kestirim hatasını ölçün. Robotu kaçırın ve filtrenin toparlanmasının ne kadar sürdüğünü ölçün.

Kendini sına

Çapraz maliyeti √2 olan 8 komşulu bir ızgarada (1, 1)'den (6, 3)'e oktil sezgisel değer nedir?
Birim adım maliyetli 4 komşulu bir ızgarada hangi sezgisel fonksiyon kabul edilebilir (admissible) DEĞİLDİR?
Dört parçacığın normalleştirilmiş ağırlıkları 0,5, 0,5, 0 ve 0. Etkin örnek sayısı nedir?
0,25 m yarıçaplı bir robot 0,1 m çözünürlüklü bir ızgarada plan yapıyor. Engeller kaç hücre şişirilmelidir?

Bu hafta dönem projesinde

Otonom kara aracı: Araç sürülüyor. Webots'ta aracı koddan sür; sabit hız ve direksiyon komutları; sensör verileri kaydediliyor.
Otonom drone görevi: SITL çalışıyor. PX4 SITL + Gazebo kuruldu; MAVSDK ile kalkış ve iniş betiği.
Görsel kılavuzlu robot kol: Kol hareket ediyor. Eklem açılarıyla kolu sür; ileri kinematiği Webots konumuyla karşılaştır.

Ödev

Python A* kodunu, çapraz maliyeti √2 olan 8 bağlantılı komşuluğa ve oktil sezgisel fonksiyona genişletin. Aynı haritalarda düğüm genişletme sayılarını Manhattan sürümüyle karşılaştırın. Kısa bir rapor yazın: parçacık sayısı çok az olduğunda filtre neden başarısız olur ve rastgele parçacık eklemenin size maliyeti nedir? İki iyileştirme önerin ve gerekçelendirin.