3D Sonar kordinat tespiti ve grafiksel yorumu

Bu yazımda basit bir ultrasonic sonar ile 3D tarama yapıp. Konum ve uzaklık bilgisini hesaplayıp grafik olarak çizdireceğim.

Öncelikle neler kullanacağımıza bakalım.

Ultrasonic Sensör HC-SR04
Cisim ile uzaklığımızı ölçmek için Ultrasonic sensör kullandım. Piyasada çok bilinen HC-SR04 ü tercih ettim. Yaklaşık olarak 2cm ile 4 metre ölçüm alabiliyor.
Çalışma Prensibi ise Ultrasonik ses dalgalarının belirli atmosferik koşullardaki hızının biliniyor olmasından faydalanarak ses dalgasının gidip gelme süresinin yarısıyla hızının çarpımıyla mesafe bilgisi elde edilmesi.

FPV-PTZ Kit

FPV kamera açı için kullanılan mekanik. 2 tane G9 servo dan oluşuyor. Montajı yaparken servoları 90 dereceye alıp FPV kiti dik açılarla monte edin. Böylece aşağı yukarı, sağ sol için 0-180 derece hareket imkanı tanırsınız.

Atmel veya Arduino Mikroişlemci

İşlemleri yapıp bilgisayarda yorumlamamız için gerekli mikroişlemci. Bu projede arduino nano yu kullandım. Herhangi bir başka arduino serisi kullanabilirsiniz.

Genel olarak algoritma

Kod algoritma olarak mikroişlemciyle başlıyor. Mikroişlemci servolara teker teker açı sinyalleri göndererek tüm açıları eksiksiz ve tekrarsız olarak tarıyor. Bu döngü loop olarak sürekli tekrarlanıyor. Her açı kontrolünde ise ultrasonic sensörden ses dalgasının gidip gelme süresiyle mesafe ölçülüyor. Ölçüldükten sonra ise seri portla bilgisayarımıza servo açıları ve uzaklık bilgisini gönderiyor. Ardından bilgisayarda yapacağımız hesap ise servo y ve servo x açısıyla beraber uzaklık bilgisi. Öncelikle gelen Y açısından 90 derece çıkarıyoruz çünkü kalibrasyonu montajda 90 derece ayarlamıştık. Servo Y açısını sin ve cos olarak mesafe ile çarparak Y kordinati ve XZ bilgisini bulmuş oluyoruz. Aynı şekilde de Servo X açışısıyla XZ bilgisini çarparak X ve Z kordinatlarını da bulmuş oluyoruz. Bundan sonrası ise 3 boyutlu grafik ile koordinatları çizdiriyoruz. Böylece bir cisim tespit edilmişse grafiksel olarak görebiliyoruz. Koordinat, uzaklık gibi bilgilerini de tespit etmiş oluyoruz. Daha güçlü sensörler (lidar, radar vb..) ve alternatif mekanikler (brusless, step motor vb..) ile ölçüm mesafesini metrelerce hatta kilometrelerce arttırabilirsiniz..

Arduino Kodları

#include <Servo.h>
#include <Ultrasonic.h>

Ultrasonic ultrasonic(12, 13); //trig_echo
Servo servo_x,servo_y;
int mesafe,pos_x,pos_y,hiz=1,sonic=1,pos_x_s=0,pos_x_f=180,pos_y_s=0,pos_y_f=180;//baslangıc ve bitis arasındaki fark cift olmalı

void setup() {
Serial.begin(9600);
servo_y.attach(8);servo_x.attach(9);
}
void loop() {
for (pos_y = pos_y_s; pos_y <= pos_y_f; pos_y += 1) {
servo_y.write(pos_y);
delay(hiz);
for (pos_x = pos_x_s; pos_x <= pos_x_f; pos_x += 1) {
servo_x.write(pos_x);
delay(hiz);
mesafe=ultrasonic.distanceRead();
if(mesafe>=200){mesafe=200;}
Serial.print(pos_y);Serial.print(" ");Serial.print(pos_x);Serial.print(" ");Serial.print(mesafe);Serial.print("\n");
delay(sonic);
}
pos_y++;
servo_y.write(pos_y);
delay(hiz);
for (pos_x = pos_x_f; pos_x>= pos_x_s; pos_x -= 1) {
servo_x.write(pos_x);
delay(hiz);
mesafe=ultrasonic.distanceRead();
Serial.print(pos_y);Serial.print(" ");Serial.print(pos_x);Serial.print(" ");Serial.print(mesafe);Serial.print("\n");
delay(sonic);
}
}
pos_x++;
for (pos_y = pos_y_f; pos_y>= pos_y_s; pos_y -= 1) {
servo_y.write(pos_y);
delay(hiz);
for (pos_x = pos_x_s; pos_x <= pos_x_f; pos_x += 1) {
servo_x.write(pos_x);
delay(hiz);
mesafe=ultrasonic.distanceRead();
if(mesafe>=200){mesafe=200;}
Serial.print(pos_y);Serial.print(" ");Serial.print(pos_x);Serial.print(" ");Serial.print(mesafe);Serial.print("\n");
delay(sonic);
}
pos_y--;
servo_y.write(pos_y);
delay(hiz);
for (pos_x = pos_x_f; pos_x>= pos_x_s; pos_x -= 1) {
servo_x.write(pos_x);
delay(hiz);
mesafe=ultrasonic.distanceRead();
Serial.print(pos_y);Serial.print(" ");Serial.print(pos_x);Serial.print(" ");Serial.print(mesafe);Serial.print("\n");
delay(sonic);
}
}
}

Matlab Kodları

clc;
clear all;

delete(instrfindall);
baglanti = serial ('COM4','BaudRate',9600); %buradan COM portunu değiştirmelisiniz

fopen(baglanti);

while(1)
data = fscanf(baglanti);
[y, xd] = strtok(data);
[x, d] = strtok(xd);

ya = real(str2num(y));
xa = real(str2num(x));
da = real(str2num(d));

ya=ya-90;

[z,xz]=sincos(ya,da);
[x,y]=sincos(xa,xz);

yf = round(y);
xf = round(x);
zf = round(z);

disp(yf);
disp(xf);
disp(zf);

scatter3(xf,yf,zf,'filled')
hold on
drawnow;

end
fclose(baglanti);