Bir Mobil Robotun Otonom Şekilde Yol Bulması: RRT Algoritması
Bu makaleyi İngilizce olarak okuyun / Read this article in English: https://medium.com/@kucar17/autonomous-navigation-of-a-mobile-robot-rrt-path-planning-algorithm-a8f780adef9d
Robotikte Motion Planning (Hareket Planlaması), bir robotu başlangıç konumundan hedef/istenen konuma ulaştıran geçerli konfigürasyon setini bulmak için kullanılan bir kavramdır. Temel RRT (Rapidly-Exploring Random Trees — Hızlı-Keşfedilen Gelişigüzel Ağaçlar) Algoritması da başlangıç noktasını bitiş noktasına (hedef noktası) bağlayan, gelişigüzel olarak seçilmiş bir nokta ağacı oluşturarak bunu sağlar.
Algoritma her bir adımda öncelikle gelişigüzel bir nokta seçer ve ardından bu noktayı, kendisine en yakında bulunan noktayla (halihazırda hesaplanmış ve nokta ağacında bulunan bir nokta) birleştirir. Böylece bu iki nokta arasında bir kenar/yol oluşturulur. Ardından iki noktayı birbirine bağlayan bu yolun ortamdaki engellere çarpıp çarpmadığı kontrol edilir. Engellere bir temas olmadığı takdirde bu nokta, nokta ağacına eklenir. Yolun engellerden biriyle kesişmesi (çarpışması) durumunda, hareketin mümkün olmadığına karar verilir ve yeni bir nokta seçilerek aynı hesaplamalar tekrar gerçekleştirilir. Hedef noktasına ulaşana veya tolerans aralığına girilene kadar bu işlemler sürekli olarak tekrar edilir ve robotun hedefe ulaşması sağlanır.
Engellerden oluşan ortamlara da uygulanabiliyor olması sebebiyle algoritma Robotikte yaygın şekilde kullanılmaktadır. Robotu hedef noktasına ulaştırabiliyor olmasına rağmen, nokta ağacının gelişigüzel olarak seçilmiş noktalardan oluşması nedeniyle, Temel RRT Algoritması tarafından hesaplanan yol çoğunlukla optimal (en kısa) yol değildir. Bu yolu optimize etmek için RRT* gibi modifiye edilmiş algoritmalar da bulunsa da, bu yazımda sizlerle Temel RRT Algoritmasının birkaç ufak dokunuş yapılmış halini inceleyeceğiz.
Aşağıda; birden fazla sayıda engelden oluşan bir ortamda, robotumuzun engellere çarpmadan hedefe ulaşmasını inceleyeceğiz. Hareket Planlaması probleminin farklı çözümler bulmasını sağlamak amacıyla da RRT Algoritması birkaç kez çalıştırılacak.
Öncelikle robotumuzun yolunu bulacağı ortamı (C-space) görelim:
Başlangıç noktamız (-0.5, -0.5) koordinatlarında, bitiş noktamız ise (0.5, 0.5) koordinatlarında bulunmaktadır.
Ortamımızın görece basit olması sebebiyle RRT Algoritmamızın optimale yakın bir çözüm vermesini bekleyebiliriz.
Algoritmamızın MATLAB üzerinde koda dökülmüş halini aşağıda görebiliriz. Kodumuz öncelikle “obstacles.csv” isimli bir .csv dosyasını girdi olarak alıyor ve bu dosya üzerinden bir engeller (obstacles) matrisi oluşturuyor. Ardından başlangıç (startNode) ve hedef/bitiş (endNode) noktalarımızı iki değişkene atıyor ve algoritmamızı 50 nokta ile sınırlıyoruz. Yani algoritmamız en fazla 50 farklı nokta hesaplayarak robotumuzu hedef noktasına ulaştırmayı deneyecek. 50 farklı noktaya ulaştığımızda robotumuz halen hedef noktasına ulaşmamışsa, algoritmamız bize başarısız olduğunu söyleyecek.
Kodda dikkatinizi çekebilecek birkaç nokta var. Bunlardan birisi her bir adımda “X” ve “Y” eksenlerinde ayrı ayrı olarak örnek uzayın daraltılması. Örneğin robotumuz engellere çarpmadan (-0.15, 0. 20) noktasına ulaşabiliyorsa, algoritmamıza artık arama yaparken x eksenindeki örnek uzayının (-0.15, 0.50] ve y eksenindeki örnek uzayının (0.20, 0.50] olduğunu söylüyoruz. Yani gelişigüzel yeni noktalar seçilirken algoritmamız bu aralıklar içerisinde bir seçim yapacak. Bu yaklaşım çeşitli ortamlarda problem çıkarma potansiyeline sahip olsa da, kendi örneğimizde optimale yakın bir sonuç almaya büyük ölçüde yardımcı oluyor. Bir diğer konu da; iki noktayı birbirine bağlarken doğruların kullanılması. Yani bir noktadan yeni bir noktaya hareket düz çizgi şeklinde ilerliyor. Hareket kısıtları bulunan sistemlerde iki noktayı birbirine bağlarken, doğrular yerine başka “planlayıcılar” kullanılmakta. Kendi örneğimizde ise anlatımı basit tutmak amacıyla robotumuzun herhangi bir hareket kısıtlaması olmadığını varsaydık.
clc
clear%% Engellerimizi tanımlıyoruz:obstacles = readmatrix('obstacles.csv');
centerX = obstacles(:,1);
centerY = obstacles(:,2);
radius = obstacles(:,3)/2;obstacleCenterX = zeros(1, length(obstacles));
obstacleCenterY = zeros(1, length(obstacles));
obstacleRadius = zeros(1, length(obstacles));for i = 1 : length(obstacles)
obstacleCenterX(i) = centerX(i);
obstacleCenterY(i) = centerY(i);
obstacleRadius(i) = radius(i);
end%% RRT Algoritmamızın hedef ve parametrelerini ayarlıyoruz:startNode = [-0.5 -0.5];
endNode = [0.5 0.5];
maxTreeSize = 50;T{1} = startNode;
rangeX = startNode(1) : 0.01 : endNode(1);
rangeY = startNode(2) : 0.01 : endNode(2);
nodeNumber = 1;
edge = [];%% RRT Algoritmasını başlatıyoruz:while length(T) < maxTreeSize
if(length(rangeX) ~= 0)
xAxis = randsample(rangeX*0.1, 1);
end
if(length(rangeY) ~= 0)
yAxis = randsample(rangeY*0.1, 1);
end
xSamp = [xAxis yAxis];
for i = 1 : length(T)
distances = sqrt((T{1}(1) - xAxis)^2 + (T{1}(2) - yAxis)^2);
index = find(distances == min(distances));
xNearest = T{i};
end
dRange = 0 : 0.005: 0.1;
distanceX = randsample(dRange, 1);
distanceY = randsample(dRange, 1);
xNewAxisX = xNearest(1) + distanceX;
xNewAxisY = xNearest(2) + distanceY;
xNew = [xNewAxisX xNewAxisY];
if (xNew(1) - xNearest(1) == 0)
continue
end
lineCoeffs = polyfit([xNearest(1) xNew(1)], [xNearest(2) xNew(2)], 1);
slope = lineCoeffs(1);
yIntercept = lineCoeffs(2);
% Çizilen doğrunun engellere temas edip etmediğini kontrol ediyoruz: for i = 1 : length(obstacles)
a = linecirc(slope,yIntercept,obstacleCenterX(i),obstacleCenterY(i),obstacleRadius(i)); % a değişkeni bir NaN array'i değilse (2 x 1), bu çarpışma anlamına gelir ve döngü sonlandırılır: if (~isnan(a))
addCondition = 0;
break
else
addCondition = 1;
end
end
if xNew(1)> 0.5
xNew(1) = 0.5;
end
if xNew(2)> 0.5
xNew(2) = 0.5;
end
% Çizilen doğru engellere temas etmiyorsa; xNew, T array'ine eklenir ve xNew ile xNearest arasında yeni bir kenar/bağlantı oluşturulur:
if (addCondition ~= 0)
T{length(T) + 1} = xNew;
nodeDistance = sqrt((xNearest(1) - xNew(1))^2 + (xNearest(2) - xNew(2))^2);
edge{length(edge) + 1} = [nodeNumber nodeNumber+1 nodeDistance];
nodeNumber = nodeNumber + 1; % Hedef noktaya yakınlaşabilmek için örnek uzayımızı daraltıyoruz: narrowRangeX = rangeX < xNew(1);
narrowRangeY = rangeY < xNew(2);
rangeX(narrowRangeX) = [];
rangeY(narrowRangeY) = [];
if xNew == endNode
disp('RRT is completed successfully!')
for j = 1 : length(T)
nodes(j, :) = [j T{j}];
end
for j = 1 : length(edge)
edges(j, :) = edge{j};
end
writematrix(edges, 'edges.csv');
writematrix(nodes, 'nodes.csv');
writematrix(nodes(:,1)', 'path.csv')
return
end
end
end
disp('RRT is not completed successfully!')
Kodumuzu her çalıştırdığımızda “RRT is completed successfully! (RRT başarıyla tamamlandı!)” sonucunu almayı bekliyoruz. Zira, kodumuzun başında algoritmamızı 50 noktayla sınırlamıştık ve bu 50 sayısı algoritmamız için zor bir limit değil. Kodu yaklaşık 15 defa çalıştırdım ve bu 15 seferde algoritmanın hesapladığı en yüksek nokta sayısı 29'du. Ancak 50 olan bu sınırı 25'e çekersek ya da farklı bir ortamda deneme yaparsak algoritmamızın başarısız olduğu durumlarla da karşılaşabiliriz.
Algoritma tarafından hesaplanan iki farklı yolu aşağıda görebiliriz (Yolların simulasyonu CoppeliaSim isimli Robot Simulasyon programında gerçekleştirilmiştir):
İki farklı sonucun da gösterdiği üzere, örnek uzay üzerinden (neredeyse) gelişigüzel noktaların seçilmesi sebebiyle algoritma her çalıştırıldığında farklı sonuçlar verebiliyor.
Bu projenin kodunu ve engelleri içeren .csv dosyasının da bulunduğu diğer simulasyon dosyalarımı GitHub sayfamda bulabilirsiniz.
Yazımda kullandığım simulasyon ortamı; Northwestern University tarafından Coursera’da yayınlanan “Modern Robotics: Mechanics, Planning, and Control Specialization” isimli eğitim serisinin sağlamış olduğu simulasyon ortamı paketinin içerisinde bulunmaktadır. Bu proje de aynı eğitim serisinin dördüncü kursu olan “Modern Robotics, Course 4: Robot Motion Planning and Control” kursundaki 2. hafta ödevinden esinlenilerek yapılmıştır.
