Konuyu Değerlendir
  • 0 Oy - 0 Ortalama
  • 1
  • 2
  • 3
  • 4
  • 5
AHRS İLE NESNELERİN YÖNELİM SİMÜLASYONU
#1
Proje Sahibi : Sedat Sert
AHRS terimi, gayro ve acclelerometerden aldığı verilerle istikamet ve durum bilgisi veren cihaz anlamına gelir.
AHRS teriminin ingilizce tanımı : attitude-heading referance system
Projenin Özeti ve Amacı
Günümüzde teknolojinin ilerlemesine paralel olarak hemen hemen her alanda robotların kullanımı hız kazanmış ve bu yapılar insanlara büyük faydalar sağlamıştır. Robotlar hızlı, kaliteli işlem yeteneğine sahip akıllı makinelerdir. Bunun yanında fiziksel olarak insan gücü ile yapılamayan işlemlerin yapılmasında büyük kolaylıklar sağlarlar. Robotların kullanımında en önemli husus robotun kontrol edilmesidir. Bilindiği gibi bir nesnenin 3-boyutlu uzayda konumlandırılması için konum ve yönelim bilgisine ihtiyaç duyulur. Robot gibi akıllı cihazların tam kontrolü için bu konum ve yönelim bilgilerini doğru ölçmek ve buna göre kontrol uygulamak gereklidir. Bu çalışmanın amacı üç eksenli ivmeölçer, jiroskop ve manyetometre verilerini kullanarak dünyaya göre yönelimini belirleyen ve gerçek zamanlı olarak çalışan bir cihaz geliştirmektir. Projeye konu olan AHRS sistemi herhangi bir nesnenin durumunu ve istikametini belirler. Yönelimi bilgisi alınan nesne yine bu projede bilgisayar ortamında simüle edilmiştir. Proje çeşitli amaçlar için kullanılabilir. Bu projenin asıl amacını şu şekilde örnekleyebiliriz. Örneğin hızla dönen bir diskin üzerinde bulunan ahrs sistem ile diskin bilgisayar ortamında gerçek zamanlı simülasyonu yapılabilir. Bu gelen verilere göre diskin yalpa miktarı ölçülebilir.
 
Projenin Teorik İçeriği
Projede kullanılan AHRS sistemi ivmeölçer, jiroskop ve manyetometreden oluşan bileşke bir sensördür. Bu sensörü oluşturan yapıların ve yönelime etkileri farklılık oluşturmaktadır.
İvmeölçer:
Bir nesnenin yönelimini belirlemek için kullanılan sensörlerin başında ivmeölçerler gelmektedir. Özetle bir ivmeölçer üzerine düşen kuvvetin neden olduğu statik veya dinamik ivmeyi ölçen cihazdır. Bu cihazlar günümüzde genellikle kapasitif etki ile çalışır. Bu tip cihazlarda iletken levhalar arasında hareket edebilen kütlenin sebep olduğu kapasite değişimi ivme büyüklüğünü belirler.
[img=404x0]http://i0.wp.com/mekatronizm.com/wp-content/uploads/2016/03/1.png?resize=404%2C314[/img]
Yandaki resim bir ivmeölçerin kapasitif ölçme mantığını göstermektedir. Bu resim elektron mikroskobu ile çekilmiştir.
İvmeölçer uzun süreli ölçümlerde oldukça kararlı çıkış sağlamaktadır. Ancak kısa süreli ölçümlerde ve ani değişimlerde ivmeölçer verisi çok gürültülüdür. Bu sebeple tek başına ivmeölçer kullanılması sistemin kararlılığını düşürür.
 
Durağan nesnelerde başka deyişle lineer ivmelenme kuvveti olmayan nesneler üzerine gelen tek kuvvet dünyanın yerçekimi kuvvetidir. Bu kuvvet nesne yatay durduğunda Z ekseni boyunca dünya merkezine doğrudur. İvmeölçer üzerine düşen bu statik kuvvetin koordinat düzlemlerine izdüşümü kullanılarak nesnenin yataydan açısı ölçülebilir.
Bir nesnenin yönelim bilgisini ölçmek için öncelikle dönüşüm sırası belirlenmelidir. Havacılık ve uzay sistemlerinde kullanılan roll-pitch-yaw dizilişi bu projede temel alınmıştır. Lineer ivmenin sıfır olduğu ve yerçekiminin Z ekseninde olduğu düşünülürse;
[img=635x0]http://i2.wp.com/mekatronizm.com/wp-content/uploads/2016/03/2.png?resize=635%2C426[/img]
Jiroskop:
[img=300x0]http://i2.wp.com/mekatronizm.com/wp-content/uploads/2016/03/3-2.jpg?resize=300%2C287[/img]Nesnelerin yöneliminin ölçümünde kullanılan cihazlardan bir diğeri jiroskoptur. Jiroskoplar açısal momentum ilkesine göre çalışır. Jiroskopların iki temel özelliği vardır.  Yatay eksende dönmekte olan bir jiroskopa yatay eksen doğrultusunda bir kuvvet uyguladığımızda yatay eksen etrafında dönmek yerine eksen etrafında dönmeye başlar. Diğer bir özelliği ise jiroskopun dönmeye başladığı eksenin jiroskobun durduğu yüzey ne açıyla oynatılırsa oynatılsın jiroskobun dönüş ekseni sabit kalır. Bu özelliğinden dolayı uyduların sürekli dünyaya dönük olması, uçaklarda ve çeşitli araçlarda yapay ufuk oluşturulması ve oto pilot gibi uygulamalarda kullanılmaktadır.
Yukarıdaki şekilde de görüldüğü gibi Jiroskoplar temel olarak bir eksen etrafında dönen diskten oluşur. Dönen diske dik başka bir çember başka bir ekseni ifade eder.
Dönen diske yatay düzlemde bir kuvvet uygulandığında jiroskop spin ekseni ile açı yapar. Bu açıdan doğan dengesizliğin dengelenmesi için diskin dönüş hızı değişir.
Jiroskop açısal hızı ölçen cihazlar olduğundan sensörden alınan veri, datasheet de verilen hassasiyet değerine bölündüğünde derece/saniye biriminde değerler elde edilir.
Açısal hız, açının zamana göre türevi olduğundan açıyı bulmak kolaydır. Açıyı bulmak için jiroskoptan gelen veri zaman ile çarpılır. Jiroskop oldukça hassas ve kısa süreli durumlarda kararlıdır. Örneğin durağan bir jiroskop sıfır değerini verir. Ve bu değerler gürültülü değildir. Bu jiroskopların kullanılmasını gerektiren en önemli etkendir. Ancak jiroskobun en büyük sorunu kaymadır. Kayma bir konumdan başka konuma getirilmiş bir jiroskobun tekrar aynı konuma döndürüldüğünde ilk değerinden uzaklaşmasıdır. Bu durumda uzun süreli ölçümlerde jiroskop kayması çok büyük sorun ve hata oluşturur. Bu sebeple yönelim sistemlerinde kısa sürede kararlı olan jiroskop ile uzun sürede kararlı olan ivmeölçer birlikte kullanılır.
Manyetometre:
Dünyanın etrafını saran manyetik alan şiddetinin ölçümünde kullanılan bir cihazdır. Manyetik alan dünyanın bir kutbundan diğer kutbuna giden akılardan oluşmuştur. Bu akı çizgileri ile dünya yüzeyi arasındaki açı kutuplardan ekvatora doğru azalır. Manyetometre manyetik şiddet ve manyetik eğim açısını kullanarak bir nesnenin yönelimini belirler. Pusulalar da bu ilkeye göre yön bildirir. Yönelim tahmininde manyetometre kullanılmasının temel amacı Z ekseni etrafındaki dönüş miktarını belirlemektir. Çünkü ivmeölçer yatay konumda iken yerçekimi vektörü Z ekseni ile çakışıktır. Bu sebeple bu vektörün xy düzlemine izdüşümü alınamaz. Bu sorunu çözmek için manyetometre çok önemli bir araçtır. Ancak manyetometre çevresindeki ferromanyetik ve manyetik nesnelerden hemen etkilenir. Ayrıca dünyanın manyetik alanındaki bozulmalar da yön belirlenmesinde olumsuz sonuçlar oluşturur.
Manyetometreden açı bilgisi alınmadan önce, Hard-Iron ve Soft-Iron etkilerini yok etmek için kalibrasyon işlemi yapılmalıdır. Kalibre edilmemiş bir manyetometre sonuçları uzayda işaretlenirse düzgün olmayan bir şekil ortaya çıkar. Ancak kalibresi tamamlanan manyetometre uzayda orijin merkezli küre oluşturur.

  • Projede ivmeölçer, jiroskop ve manyetometreyi içeren sensör modülü; arduino ve processing kullanılmıştır.
GY-81 Sensör Modülü
[img=221x0]http://i1.wp.com/mekatronizm.com/wp-content/uploads/2016/03/4-2.jpg?resize=221%2C221[/img]
Yandaki resim GY-81 modülüdür. Bu modül üzerinde 3 eksen ivmeölçer, 3 eksen jiroskop, 3 eksen manyetometre ve bir adet basınç sensörü bulunmaktadır.
İvmeölçer ve jiroskop MPU9150 isimli pakette birlikte bulunur.
Manyetometre Hmc5883l ismi ile bilinir.
Basınç sensörü ise Bmp085 olarak isimlendirilir.
 
Projenin Diğer Önemli Komponentlerinden biri 2×16 LCD diğeri Arduino Uno’dur. Diğer Yazılarda ayrıntılı anlatıldığı için bu yazıda anlatılmayacaktır.
Processing
[img=185x0]http://i2.wp.com/mekatronizm.com/wp-content/uploads/2016/03/5.png?resize=185%2C185[/img]
 
Processing seri porttan gelen ve kod ile yazılmış verilerin görselleştirilmesini sağlar. Projede seri porttan gelen pitch, yaw ve roll açıları processing 3D sayesinde görsel hale getirilmiştir.
 
 
Donanım Bağlantıları:
[img=731x0]http://i1.wp.com/mekatronizm.com/wp-content/uploads/2016/03/6.png?resize=860%2C521[/img]
                              Arduino ile İmu Sensörünün Bağlantısı
[img=731x0]http://i2.wp.com/mekatronizm.com/wp-content/uploads/2016/03/7.png?resize=860%2C475[/img]
Arduino ile 2×16 Lcd Bağlantısı
Yazılım Kısmı
Projenin yazılım bölümü temel olarak iki bölüme ayrılır. Bu bölümlerden birisi sensör verilerini bilgisayara gönderirken diğer kısım bu verileri alıp simülasyon gerçekleştirir.
Arduino Yazılımı
Projede kullanılan sensör verileri gürültülü olduklarından bu verilerin düzenlenmesi için kalman filtresi uygulanmıştır. Kalman filtresi uzun bir konu olduğundan ek dosya halinde sunulacaktır.
Ardiuno yazılımının ilk bölümünü başlık dosyalarının eklenmesi oluşturur. Sensör kütüphanesi, LCD kütüphanesi, kalman kütüphanesi ve temel işlemler kütüphanesi programa dahil edilir.
İkinci kısım ise sensör kalibrasyonlarının yapıldığı kodlardan oluşur. Her kullanımda sensörün bulunduğu konuma göre kalibrasyon yapması sağlanmıştır.
Son kısımda ise verilerin sürekli alınması ve seri port aracılığıyla bilgisayara iletilmesini sağlayan kodlar bulunmaktadır.
Ardiuno kodlarını içeren dosya ek olarak sunulacaktır.
Processing Yazılımı
Seri port üzerinden alınan verilerin görsel olarak gösterilmesi işlemi processing ile yapılmıştır. Processing yazılımı da Arduino yazılımı gibi üç bölümden oluşmaktadır. Bu bölümler aşağıda özetlenmiştir.
Birinci kısımda seri porttan verilerin alınmasını sağlayan ve tutan değişkenlerin ve serial kütüphanesinin eklenmesi kısmı oluşturur. Ayrıca bu kısımda ekran ayarları yapılmıştır.
İkinci kısım ise asıl döngüyü oluşturan kısımdır. Bu döngüde elde edilen değerler bir döngü içinde ekrana gönderilmiştir.
Son kısım ise SerialEvent denen bir fonksiyondan oluşur bu fonksiyon seri porttan gelen verinin istenen şekilde ayrılması ve değişkenlere aktarılmasını sağlar.
Processing kodunun indirmek için tıklayıp indirebilirsiniz.
Projede Kullanılan Kalman Filtresinin detaylı anlatımı ektedir. Tıklayıp İndirebilirsiniz.
Arduino yazılım kütüphaneleri tıklayıp indirebilirsiniz.


Kod:
/*
*Ahrs kullanılarak nesnelerin yönelim tahmini ve simülasyonu
*Sensör : GY-81(ivmeölçer,jiroskop,manyetometre ve basınç sensörü içerir).
*MikroDenetleyici : Ardiuno Uno
*Simülasyon : Processing
*Diğer: 1602A Lcd Ekran
*22/03/2016 SEDAT SERT
*170212006
*/
//=======================Kütüphane Başlık Dosyaları Eklenmesi==========================//

#include <Wire.h> //I2C haberleşmesi kütüphanesi
#include <Kalman.h> //Kalman filtresi kütüphanesi
#include <I2Cdev.h> //I2C işlemleri kütüphanesi
#include <MPU9150.h> //Sensör kütüphanesi
#include <HMC5883L.h> //Manyetometre kütüphanesi
#include <LiquidCrystal.h> //Lcd ekran kütüphanesi

//=======================Kütüphanenelere Ait Nesnelerin Oluşturulması==================//

MPU9150 mpu; //Imu için nesne oluşturuldu..
HMC5883L compass; //Manyetometre için nesne oluşturuldu
Kalman kalmanX,kalmanY,kalmanZ; //Kalman filtresi açı değişkenleri tanımlandı.
LiquidCrystal lcd(8, 9, 5, 4, 3 , 2); //lcd pinleri atanarak nesne oluşturuldu.

//=======================Global Değişkenlerin Tanımlanması==============================//

int16_t AccX,AccY,AccZ; //İvme değişkenleri
int16_t GyrX,GyrY,GyrZ; //Açısal Hız değişkenleri
double kalmanRoll, kalmanPitch; //Kalman Açı değişkenleri
float MagX,MagY,MagZ; //Manyetik Alan Değişkenleri
uint32_t timer; //Zaman değişkeni

//======================Sensörlerin Ayarlanması ve Kalibrasyon==========================//
/* Bu bölümde buffersize ile belirtilen sayıda sensör
* verilerini okuyarak bu verilerin ortalamasını alınır.
* Yatay pozisyonda bulunan İMU (0g,0g,1g,0 d/s,0 d/s,0 d/s) vermelidir.
*/
int buffersize=1000;
int acel_deadzone=8;
int giro_deadzone=1;
int16_t ax, ay, az,gx, gy, gz;
int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;

void meansensors(){
long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

while (i<(buffersize+101)){
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
if (i>100 && i<=(buffersize+100)){
buff_ax=buff_ax+ax;
buff_ay=buff_ay+ay;
buff_az=buff_az+az;
buff_gx=buff_gx+gx;
buff_gy=buff_gy+gy;
buff_gz=buff_gz+gz;
}
if (i==(buffersize+100)){
mean_ax=buff_ax/buffersize;
mean_ay=buff_ay/buffersize;
mean_az=buff_az/buffersize;
mean_gx=buff_gx/buffersize;
mean_gy=buff_gy/buffersize;
mean_gz=buff_gz/buffersize;
}
i++;
delay(2);
}
}
void calibration(){
ax_offset=-mean_ax/8;
ay_offset=-mean_ay/8;
az_offset=(16384-mean_az)/8;

gx_offset=-mean_gx/4;
gy_offset=-mean_gy/4;
gz_offset=-mean_gz/4;
while (1){
int ready=0;
mpu.setXAccelOffset(ax_offset);
mpu.setYAccelOffset(ay_offset);
mpu.setZAccelOffset(az_offset);

mpu.setXGyroOffset(gx_offset);
mpu.setYGyroOffset(gy_offset);
mpu.setZGyroOffset(gz_offset);

meansensors();
Serial.println("...");

if (abs(mean_ax)<=acel_deadzone) ready++;
else ax_offset=ax_offset-mean_ax/acel_deadzone;

if (abs(mean_ay)<=acel_deadzone) ready++;
else ay_offset=ay_offset-mean_ay/acel_deadzone;

if (abs(16384-mean_az)<=acel_deadzone) ready++;
else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

if (abs(mean_gx)<=giro_deadzone) ready++;
else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

if (abs(mean_gy)<=giro_deadzone) ready++;
else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

if (abs(mean_gz)<=giro_deadzone) ready++;
else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

if (ready==6) break;
}
}

void calibrateMpu(){

while(!Serial.available()){
Serial.println(F("Send any character to calibrate Mpu...\n"));

delay(1000);
}
//while(Serial.available() && Serial.read()); // Buffer boşaltma..
Serial.println("MPU9150 Calibration Starting..");
lcd.setCursor(0,0);
lcd.print("Calibrating Mpu ");
if(!mpu.testConnection()){
Serial.println("Mpu connection failed, Please control your wires");
while(1);
}
mpu.setXAccelOffset(0);
mpu.setYAccelOffset(0);
mpu.setZAccelOffset(0);
mpu.setXGyroOffset(0);
mpu.setYGyroOffset(0);
mpu.setZGyroOffset(0);

while(1){
if (state==0){
Serial.println("\nReading sensors for first time...");
meansensors();
state++;
delay(1000);
}

if (state==1) {
Serial.println("\nCalculating offsets...");
calibration();
state++;
delay(1000);
}
if (state==2) {
meansensors();
Serial.println("calibration successful");
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Calibrate success ");
delay(500);
}

break;
}
lcd.clear();
}

//===========================Manyetometre değerlerinin Alınması ve Kalibrasyonu===================//
/*calibration matrix ve bias değerleri
*manuel olarak yapılan kalibrasyon ile elde edilmiştir.
*/
void getHeading(){

double calibration_matrix[3][3] =
{{1.056, 0.004, 0.041},
{0.004, 1.062, 0.072},
{0.018, -0.025, 1.2}};

double bias[3] =
{115.389,-154.823,77.1};
float values[3];
MagnetometerRaw raw = compass.ReadRawAxis();
values[0] = (float)raw.XAxis;
values[1] = (float)raw.YAxis;
values[2] = (float)raw.ZAxis;
for(int i=0;i<3;++i)
values[i]=values[i]-bias[i];
float result[3]={0,0,0};
for (int i=0; i<3; ++i)
for (int j=0; j<3; ++j)
result[i] += calibration_matrix[i][j] *values[j];
MagX=result[0];
MagY=result[1];
MagZ=result[2];
}

//====================================Başlangıç Ayarlarının Uygulanması==============================================//

void setup() {
Wire.begin(); //İletişim başlatılması
Serial.begin(38400); //Serial haberleşme başlatıldı.
mpu.initialize(); //IMU başlatıldı.
compass=HMC5883L(); //Manyetometre başlatıldı.
compass.SetScale(0.88); //Manyetometre skalası 0.88 Gauss olarak ayarlandı.
compass.SetMeasurementMode(Measurement_Continuous); //Manyetometre ölçüm modu sürekli olarak ayarlandı.
if(!mpu.testConnection()){ //IMU bağlantısının test edildi.
Serial.println("mpu connection failed"); //IMU bağlantı sorununun bildirildi.
while(1); } //IMU bağlantısı sorunlu ise program burada kalmalı.
calibrateMpu(); //IMU kalibrasyon fonksiyonu çağırıldı.
lcd.begin(16,2); //Lcd başlatıldı.
lcd.setCursor(0,0); //Kursor konumlandırıldı.
lcd.write("X:"); //Kursorün olduğu yere X: yazıldı.
lcd.setCursor(0,1); //Kursor konumlandırıldı.
lcd.write("Y:"); //Kursorün olduğu yere Y: yazıldı.
lcd.setCursor(9,0); //Kursor konumlandırıldı.
lcd.write("Z:"); //Kursorün olduğu yere Z: yazıldı.
mpu.getAcceleration(&AccX,&AccY,&AccZ); //İvme bilgileri alındı.
double roll=atan2(AccY,AccZ); //Roll açısı hesaplandı.
double pitch=atan(-AccX/(sqrt(square(AccY)+square(AccZ)))); //Pitch açısı hesaplandı.
kalmanX.setAngle(roll); //Kalman ilk açı değeri verildi.
kalmanY.setAngle(pitch); //Kalman ilk açı değeri verildi.
timer = micros(); //Zaman hafızaya alındı.

}

//==========================================================Loop Döngüsü============================================================//
void loop(){

mpu.getMotion6(&AccX,&AccY,&AccZ,&GyrX,&GyrY,&GyrZ); //IMU verilerinin alındı.
double dt = (double)(micros() - timer) / 1000000; //Zaman aralığı hesaplandı.
timer = micros(); //Zaman hafızaya alındı.
getHeading(); //Manyetometre değerleri alındı.
double roll=atan2(AccY,AccZ)*180/M_PI; //Roll açısı hesaplandı.
double pitch=atan(-AccX/(sqrt(square(AccY)+square(AccZ))))*180/M_PI; //Pitch açısı hesaplandı.

double gyroXrate = GyrX / 131.0; //GyroX verileri derece/saniye dönüşümü yapıldı.
double gyroYrate = GyrY / 131.0; //GyroY verileri derece/saniye dönüşümü yapıldı.
double gyroZrate = GyrZ / 131.0; //GyroX verileri derece/saniye dönüşümü yapıldı.

/*Bu kısımda Z ekseni etrafındaki açının hesapları yapılır. * Açının tanjantı X ve Y bileşenlerinin oranını verir*/
double bfy= (MagZ*sin(radians(kalmanRoll)))-(MagY*cos(radians(kalmanPitch)));
double bfx= (MagX*cos(radians(kalmanPitch)))+(MagY*sin(radians(kalmanRoll))*sin(radians(kalmanPitch)))+(MagZ*cos(radians(kalmanRoll))*sin(radians(kalmanPitch)));
double yaw=atan2(bfy,bfx)*180/M_PI;
/* */

/*Bu bölümde Açıların Aerospace dizilişine göre yeniden sınırlandırılması işlemi yapıldı.
*Bu diziliş sırası roll-pitch-yaw şeklindedir.
*Roll açısı -180 ve +180 arasında, pitch açısı ise -90 ve +90 arasında değişmektedir.
*/
if ((roll < -90 && kalmanRoll > 90) || (roll > 90 && kalmanRoll < -90)) {
kalmanX.setAngle(roll);
kalmanRoll = roll;
} else
kalmanRoll = kalmanX.getAngle(roll, gyroXrate, dt);

if (abs(kalmanRoll) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalmanPitch = kalmanY.getAngle(pitch, gyroYrate, dt);
//======================================Verilerin Seri Porta Gönderilmesi================================================================//
Serial.print("Roll:");
Serial.print(-kalmanRoll,2);
Serial.print("\tPitch");
Serial.print(kalmanPitch,2);
Serial.print("\tYaw :");
Serial.println(yaw,2);

//======================================Verilerin Lcd ekrana gönderilmesi================================================================//
lcd.setCursor(2,0);
lcd.print(kalmanRoll);
lcd.setCursor(2,1);
lcd.print(kalmanPitch);
lcd.setCursor(11,0);
lcd.print(yaw);
}
Bul
Alıntı


Foruma Git:


Bu konuyu görüntüleyen kullanıcı(lar): 1 Ziyaretçi
loading...