İVME VE GYRO SENSÖRLERİ İLE EĞİM AÇILARININ ÖLÇÜLMESİ

Hareket ve pozisyon kontrolu yaptığımız projelerimizde ADXL345 ve MPU6050 gibi ivme ölçer sensörleri kullanıyoruz.

Bu sensörler 3 eksen doğrultusundaki doğrusal ivmeleri ve bu eksenler etrafındaki dönüş hareketlerinin yarattığı açısal ivmeleri 10 ila 16 bitlik rakamlar halinde veriyor. Biz ise çoğu zaman ivme değerlerinden çok, sensörün bağlı olduğu platformun sağa sola, öne arkaya eğim açılarıyla ilgileniyoruz.

Eğer projelerinizde Arduino kullanmayı seviyorsanız, sizin için “şu kütüphaneyi yükle, şu fonksiyonu çağırınca sana istediğin eğim açılarını verecektir” tarzındaki bir yol tarifi yeterince tatminkar olacaktır. Bu yayın ise işin arka planındaki süreci anlamak isteyenler, yani benim gibi mazoşist teknisyenlere yönelik.

Bu yayında doğrusal ve açısal ivme değerlerinden hareketle bu açıların nasıl hesaplanacağını anlatacağım. Meraklısına …

Not: Kullandığım görsellerden başka yerlerden aldıklarımı mutlaka belirtiyorum. Bunların dışında kalanlar kendi hazırladıklarım ve epey uğraş gerektiren çizimler. Bunlar ticari kazanç amacı güdülmediği sürece, kaynak belirtilmek suretiyle başkaca bir izin gerektirmeden kullanılabilir.

İVME ÖLÇÜMÜ HAKKINDA

İvme ölçen bir sensör duyarlı olduğu yönde hareket ettirildiğinde hız değişiminden dolayı oluşan ivmeyi ölçer. howmechantronics.com sitesinden aldığım aşağıdaki çizim bu sensörlerin iç mekanizmasını açıklıyor.

Bu mikro mekanizma “acceleration” olarak işaretli doğrultuda hareket ettirildiğinde elektrodlar arasında değişen kapasitif kuplajların oranlarını ölçerek oluşan ivme değerini veriyor. Elbette, hız sabit kalırsa herhangi bir ivme oluşmadığından “0” ivme bildiriyor.

Ancak, eğer uzayda değil, yerçekimli ortamdaysak, bu mekanizma tam yatay durduğu konumların dışında yerçekimi ivmesini gösterecektir. Zira kırmızı renkli çekirdek ağırlığı nedeniyle eğim yönünde kaykılacaktır. Eğim ne kadar çok ise o kadar büyük bir değer, yere tam dik olduğunda ise yerçekimi ivmesi olan 1g (9.8m/s²) ölçüm sonucu bildirecektir.

Böylece bu elemanın verdiği ivme değerinden hareketle eğimin ne kadar olduğunu hesaplayabiliriz, basit bir trigonometri hesabından ibaret.

Şimdi bir başka resim üzerinden devam edelim:

Bu ivme ölçeri temsil eden bir çizim. Ortada alttan üstten yaylarla sabitlenmiş bir kütle, bir top var. Sistem tam yatay durduğunda top da iki yayın ortasında dengede duruyor. Şekildeki gibi yere dik duruma getirildiğinde de yerçekimi ivmesi (ağırlığı) nedeniyle aşağıya doğru sarkıyor.

Üstteki ölçü aleti de topun ne kadar yer değiştirdiğini, yani maruz kaldığı ivmenin büyüklüğünü gösteriyor.

Bu topun bir yana doğru iteklenmesi sadece yer çekimi dolayısıyla olmuyor, yatay durumda da olsa hareket ettirildiğinde ataleti nedeniyle harekete direneceğinden, o zaman da ortada kalamayıp yer değiştiriyor. Bu yer değiştirme de hareketin ivmesi ile orantılı oluyor ve ölçü aletinde bu ivmenin değeri görülebiliyor.

İKİ EKSENLİ SİSTEMDE İVME VE AÇI ÖLÇÜMÜ

Anlatım kolaylığı açısından açıklamalarımda ivmeye yol açan şeyin yerçekimi olduğu varsayımı ile devam edeceğim, ama aynı şeylerin hareket ivmesi için de geçerli olacağını belirteyim. Yer çekimi ivmesinin yerine hareket ivmesini, aşağı/yukarı yön yerine hareketin yönünü koyarsanız her şey yerine oturacaktır.

Yani bu yorum ve hesaplar uzayda hareket eden sistemler için de geçerli. (Böyle söyleyince müthiş bir şey anlatıyormuşum gibi havalı oldu :)).

YERÇEKİMİ İVMESİNİN ETKİSİ HAKKINDA

Yerçekimsiz bir ortamda bulunmadığımız sürece sürekli olarak 9.8m/s2 ivme ile hızlanmaktaymışız gibi aşağıya doğru bir kuvvete maruz kalıyoruz, ağırlık dediğimiz şeye yani.

Yer yüzünde iken bu sensörler ile yere ne kadar paralel konumda olduğumuzu, paralel değil isek açımızın ne olduğunu anlamak kolay oluyor, biyolojik yapımız bunun için sensör olarak kaslarımızı, sinirlerimizi ve iç kulağımızdaki doğal sensörleri kullanıyor. Bu sensörlerin de birbirine dikey konumlandırılmış üç adet halkadan oluştuğunu biliyoruz.

Elektronik sensörlerimiz de yer yüzünde iken yere göre eğimimizi kolaylıkla hesaplayıp bildirebiliyor. Zira maruz kaldığı 9.8m/s2 lik düşey ivme oldukça güçlü bir kuvvet yaratıyor. Bu sensörler ivmeye tam dik durumda iken 9.8m/s2 ölçerken, 45o açıyla karşılarlarsa etkilendikleri kuvvet yarı yarıya düşüyor, tam yatay konumda da sıfırlanıyor. Kolaylıkla algılanabilen, süper bir hassasiyete gerek bırakmayan bir kuvvetten söz ediyoruz. Eğer bu ivme çok daha düşük olsaydı, açımızın sensöre etkisi de o ölçüde küçük olacak, doğru ölçümler yapabilmek için o oranda daha yüksek duyarlıklara gereksinim duyacaktık. Daha hassas sensörler yapmak mesele değil, ama hem çok büyük ivmeyi, hem de buna ek olarak gelen küçük ivmeleri algılayabilen sensörler yapmak zor.

Ağırlıksız bir ortamda benzeri bir etkiyi, harekete geçtiğimizde ve hızımızı sürekli olarak artırdığımızda hissedeceğiz. Şöyle söyleyelim, 100km/h ye 5 saniyede ulaşan bir araç içindeysek, yer çekimi ivmesinin ancak yarısına ulaşabilmişiz demektir. Üstelik 5 saniyeden sonra da aynı artış oranını korumalıyız ki bu ivme devam etsin, yoksa 100km/h e ulaşınca hızımız sabit kalırsa ivme de tekrar sıfıra düşecek, sensörümüz hareket yönüne göre açımızın ne olduğuna dair bir ip ucu veremeyecektir. Uzayda ağırlıksız ortamda neresi yukarı, neresi aşağı bilemiyoruz. Halbuki yeryüzünde yerçekimi ivmesi kesintisiz olarak süregeliyor.

Kısacası, ağırlıklı ortam bu tür sensörler ile konum belirlememiz için çok işimize yarıyor. Aynen mıknatıslı ibresi olan bir pusulanın yön göstermesine benziyor. Ama bir yandan da küçük ve yavaş hareketlerin yarattığı ivmeleri perdeleyen bir etkisi de oluyor.

Eh, bu kadar faydası var ise zararlarına da katlanırız artık. Bu yayındaki "Z ekseni düşeye yakın konumdayken" işe yarayan hesaplara kötü gözle bakmayalım o yüzden. Zaten, yayında açıkladığım açısal ivme ölçümlerinin nimetlerinden yararlanarak bu sorunu aşabiliyoruz.

Yukarıdaki açıklamalarda sensörün yerçekiminden dolayı verdiği ivme değerinden hareketle eğimini hesaplayabileceğimiz anlaşılıyor. Ne de olsa ölçülen ivmenin, yer çekimi ivmesine oranına bakmak yeterli. Sensörün bağlı olduğu bir platformun sadece bir yöndeki değil, sağa sola, öne arkaya eğimlerini de bilmek istiyorsak, biraz daha fazlasına ihtiyacımız olacak.

Bilmek istediğimiz bu eğimlere havacılıkta Pitch (uçağın ön/arka doğrultusundaki eğimi) , Roll (uçağın kanatlar yönündeki eğimi) diyorlar. Bir de Yaw var, sağa sola doğrultumuzu veren açı.

İş buraya gelince birbirine dik üç eksende ivme ölçmek zorunda kalıyoruz. Zira tek eksenli ivme ölçer bize sadece hareket yönündeki ivmeyi veriyor, eğer hareketimiz sensör eksenine paralel değil ise sensörden toplam ivmeyi değil sadece bir parçasını alabiliyoruz.

Şimdi iki eksenli bir sistemde eğim açısını nasıl hesaplayacağımıza bakalım.

İKİ EKSENLİ SİSTEMDE İVME ÖLÇÜMÜ

Bu çizimde ivme ölçerimiz yere dik durmuyor (ya da hareket yönüne bakmıyor), ß açısı kadar bir eğim ile duruyor. Bu durumda ölçülen ivme değeri olan Acc1, ß açısının büyüklüğüne bağlı olarak küçülüyor. Yani gerçek ivme değeri olan G değerini ölçemiyoruz. Ölçemediğimiz ivme bileşeni sensörümüze dik yönde olan Acc2 ivmesi.

O halde toplam ivmeyi bilmek için, Acc2 ivme bileşenini ölçmek üzere ilk sensörümüze dik konumda bir ikinci ivme sensörü daha koymamız gerekiyor. Bütün bunları değerini zaten bildiğimiz yerçekimi ivmesi G yi ölçmek için yapmadığımızı tekrarlayayım. Sonuçta ölçümlerimizden hareketle sensörümüzün bağlı olduğu platformun eğim açılarını bulacağız.

Resimde her iki sensörden okunan ivme değerlerinin Acc1 ve Acc2 olduğunu, bunların vektörel toplamının da G olduğunu, bu G değerinin hızı değişen -ivmeli- bir hareket durumunda yerçekimi ivmesine eşit olmayacağını not edelim.

Şimdi, her iki sensörden gelen Acc1 ve Acc2 bileşenlerini kullanarak ß açısını hesaplayabiliriz.

ß = ArcTan(Acc2/Acc1);

Bu denklem ß 90^o ye yaklaştığında arıza çıkaracağından alternatif olarak :

ß=ArcCos(Acc1/SQRT(Acc1²+Acc2²))

kullanılabilir.

ÜÇ EKSENLİ SİSTEM

Üç boyutlu uzayda yaşadığımıza göre sensörümüz herhangi bir konumda, hareketimiz herhangi bir yönde olabileceğinden, artık işin içine üçüncü boyutu da katmamız gerekiyor. Geliyoruz birbirine dik 3 eksenden oluşan sisteme. Sensörümüz de birbirine dik konumlandırılmış 3 sensörden oluşuyor. Aşağıdaki gibi (MPU6000 data sheet’inden):

Diyelim ki bu sensör kutu şeklinde bir yapının üstüne yapıştırılmış, x ve y eksenleri yapıştırıldığı yüzeyin iki kenarı doğrultusunda, bu yüzeye dik olan z ekseni de kutunun bu yüzeyine dik olan kenarı yönünde. Sistemi bu şekilde görselleştirmek konunun anlaşılmasını kolaylaştırıyor.

Bu çizimde X, Y ve Z vektörleri sensörün üç eksende okuduğu ivmeleri temsil ediyor. Sistem hareketsiz durumda olduğundan bu denge durumunda aşağıya doğru inen kırmızı bileşke de yerçekimi ivmesi oluyor.

Bu ivme değerlerini sensörümüz bize 10 (ADXL345) ila 16 (MPU6050) bitlik rakamlarla veriyor. Şimdi yapmak istediğimiz XY sensör düzleminin düşey yöne göre (ya da yataydan) sapma açılarını hesaplamak.

XY düzleminin yatay konuma göre eğimi ilk bakışta sanıldığı gibi X ekseninin yataydan sapmasına ya da Y ekseninin yataydan sapmasına eşit değil.

XY düzlemi tam yatay durumda iken X ve Y ivmeleri sıfır olup Z ivmesi yer çekimi ivmesi üzerine çakışmış ve ona eşit oluyor.

Sensörümüz X ekseni etrafında döndürüldüğünde Z ekseni de düşey konumdan, Y ekseni de yatay konumdan uzaklaşıyor. X ekseni hala yatay durumda ve bunu etkileyen ivme hala sıfır olduğundan Y ve Z ivme vektörlerinin toplamı yer çekimi ivmesine eşit oluyor. Bu durumda XY düzleminin yataya göre eğimi ile Y ekseninin eğimi de eşit oluyor.

Şimdi Y ekseni etrafında bir dönüş ile X eksenini de yataydan ayırıp eğersek durum değişiyor. Aşağıdaki şekil bu durumu yansıtıyor.

Bu şekilde görüldüğü gibi X ve Y eksenleri ile birlikte hareket eden Z ekseni de iki yöne birden düşey konumdan ayrılıyor. XY düzlemimizin X ekseni çevresindeki açısal dönüşü Angle_x ile etiketlediğim açıya eşit. Bu açıyı aşağıdaki gibi hesaplayabiliriz. Şekilde kolayca görünen dik üçgen ve trigonometrik ilişkileri açıklama gerek yok sanırım:

Angle_x = ArcTan(Y/SQRT(X²+Z²))

Bu denklemin paydasındaki X²+Z² terimi, açının 90^o ye gitmesi durumunda sıfır olacağından kodlama sırasında buna karşı önlemimizi almamız gerekir.

Biraz daha fazla işlem yaparak kosinus kullanıp bu sorunu aşabiliriz :

Angle_x = ArcCos(SQRT(X²+Z²)/SQRT(X²+Y²+Z²))

Burada söz ettiğim tanjant’ın sonsuza gitmesi kodlama sırasında kolayca engellenebildiğinden genelde ilk denklem tercih ediliyor. Z değerinin sıfır olmasına izin verilmemesi yeterli.

Benzer şekilde sensörümüzün Y ekseni çevresindeki dönüş açısını aşağıdaki şekilde gösteriyorum:

Buradan Y ekseni çevresindeki dönüş açısı aşağıdaki gibi hesaplanabiliyor :

Angle_y = ArcTan(X/SQRT(Y²+Z²))

AÇISAL İVMELERİN DİKKATE ALINMASI

Şimdiye kadar sözünü ettiklerim sensörün eksenleri doğrultusundaki hareket ya da yer çekimi ivme bileşenleri idi. Bunları kullanarak istediğimiz eğim açılarını hesaplayabildik.

Bu ivmeler doğrusal nitelikte, yani bir eksenin çevresindeki dönüş hareketi o eksende bir ivme yaratmıyor. Yukarıda anlattığım gibi eksenin çevresindeki dönüş açısı diğer iki eksende algılanan ivmelerden hareket ile hesaplanabiliyor.

Eksen çevresindeki dönüş hareketlerini algılamak üzere aynı yonganın üzerine “gyro” dediğimiz açısal ivme ölçerler de koyuyorlar. Böylece sensörümüz 6 eksenli oluyor, doğrusal ivmelere ilaveten dönüş hareketlerini de algılayabiliyoruz.

Dönüş hareketini algılayabildiğimizde, diyelim 2 sn süre ile 100^o/sn² lik bir ivme algıladıysak başlangıç konumuna göre 200^o lik bir dönüş yaptığımızı hesaplayabiliyoruz. Başlangıç açısını kaydetmiş isek bu eksenin açısal konumunu bu şekilde izleyebiliyoruz.

Görüldüğü gibi hem doğrusal hem de açısal ivmeler ile eksenlerin açısal dönüş ve konumlarını hesaplayabiliyoruz. O halde ikisine birden neden ihtiyaç var?

Birincisi, bunlardan birisi doğrusal, diğeri açısal değişiklikleri fazla bir işlem gerekmeden doğrudan verebiliyor.

Eğer yerçekimi etkisinde isek, doğrusal ivme sensörü hareketsiz durumda dahi eğim açılarını verebiliyor.

Gyro sensörü hareketsiz iken hiçbir çıkış vermiyor. Bu bir dezavantaj gibi görünse de yer çekimi ve doğrusal hareket etkilerini betaraf etmek istediğimizde çok işe yarıyor. Örneğin bir yöne doğru ilerlemekte olan drone’un eğimini hız değişikliklerinden etkilenmeden ölçebiliyoruz. Ama hızını ölçmek için gene de doğrusal ivme sensörlerine ihtiyacımız oluyor.

Bir başka konu da doğrusal ivme sensörlerinin titreşimlere karşı duyarlılığı. Dönen motor ve pervanelerin titreşimleri önemli ivme parazitlerine yol açıyor. Titreşimi küçümsememek gerek, 2.000 devirle dönen pervanelerin yarattığı 0.1mm genlikli titreşimler tepe değeri 800 g ye varan ivme parazitlerine yol açabiliyor. Aşağıdaki sinüzoidal hareketlerin ivme denklemi bunu açıklayabiliyor.
a=-2π²f²Dsin(2πft)

Eğer çok titizlenip çok güvenilir bir sistem kuracaksak hem mekanik yapımızın, hem de sensörümüzün mekanik rezonans frekanslarını da dikkate almamız gerekir. Kazara bu frekanslara denk titreşimlere maruz kalırsa ölçüm sonuçları çılgına dönecektir.

AÇI HESAPLAMASINDA DOĞRUSAL VE AÇISAL İVME DEĞERLERİNİN BİRLİKTE KULLANILMASI

Doğrusal ivme değerlerinin sarsıntı ve titreşimlere karşı çok duyarlı olduğunu belirtmiştik. Açısal ivme değerleri ise bu tür bozuculara karşı bağışıklığa sahip. Öte yandan açısal ivme değerleri ancak önceki açıya göre kaç derece ileri ya da geriye döndüğümüzü verebiliyor, mutlak eğimi veremiyor. Doğrusal ivmelerden çok sayıda ölçüm yapıp ortalamalarını almak bir yaklaşım ama tam bir çözüm sağlamıyor.

O halde bu ikisinin üstün yanlarından yararlanarak hesaplamanın bir yolunu bulmalıyız.

İşte bu yüzden doğrusal ivme ve açısal ivme ölçümlerini, uygulamaya uygun ağırlık katsayıları ile birleştirerek kullanıyoruz. Titreşimin olmadığı durumlarda doğrusal ivmenin ağırlığını yüksek tutabiliriz. Ama genelde açısal ivmenin, yani gyro’nun payını yüksek tutmak daha iyi sonuç veriyor.

İki adımlık bir algoritma izleyebiliriz.

Açısal ivmeyi hiç devreye sokmadan doğrusal ivme değerlerine bir filtre uygulayalım. Önce sadece doğrusal ivmelerden hareketle açılarımızı hesapladığımızı düşünelim:

Açı_güncel = Açı_önceki x alfa + Açı_{şu_an} x (1-alfa)

Buradaki alfa katsayısı 0..1 arasında. Bu formüle göre sensörden şu an okuyarak hesapladığımız açı değerini olduğu gibi kullanmıyoruz. Açının önceki değerini de alfa ağırlığı ile hesaba katıyoruz. Böylece ivmede ani bir sıçrama olursa bunu yumuşatarak sonuca yansıtmış oluyoruz. Sıçrama geçici ise sistem sonraki adımlarda kendini hızla toparlayabiliyor. Alfa katsayımız 1 e ne kadar yakın olursa filtreleme etkisi de o kadar güçlü oluyor. Tabiki abartıp “1” yaparsak o zaman da yeni ölçümler hiç hesaba katılmıyor.

Yukarıdaki işlem ile açılardaki ani ve geçici sıçramaları filtrelemiş olduk. Şimdi de bu işlemi gyro, yani açısal ivmelerden yararlanarak geliştirelim. Böylece, sarsıntı ve titreşimlerden değil de gerçekten açısal değişiklikler oluyor ise, bunları da filtrelemiş olmayalım:

Bu denklemde eski açı değeri yerine açısal ivme (gyro) ile hesaplanmış açı değerini koyalım. Gyro nin verdiği yeni açı değeri önceki açı değerini de içeriyor, aşağıdaki gibi :

Açi_gyro = Açı_önceki + İvme_gyro x T_süre

Böylece formülümüzün yeni hali şöyle olacak:

Açı_güncel = Açi_gyro x alfa + Açı_{şu_an} x (1-alfa)

Böylece hem önceki açı değerini hem de açısal ivme değerini hesabımıza katmış olduk.

Şimdi bir grafik ile bu işlemin etkisini görselleştirelim. Aşağıdaki yeşil çizgi sensörden gelen doğrusal ivme değerini gösteriyor. Görüldüğü iki sarsıntıdan kaynaklanan iki tane zirve var, biraz abartılı bir örnek :

Turuncu çizgi ise hesaplanan açı değerini gösteriyor. Burada açısal ivme değerini “0”, alfa katsayısını da 0.96 aldım. Yani açısal sensörümüz bu sarsıntıdan hiç etkilenmemiş durumda. Görüldüğü gibi ani ve geçici değişiklikleri filtrelemiş oluyoruz.

Peki, bu sarsıntıdan açısal sensörümüz de bir miktar etkilenmiş olsaydı ne olacaktı, onu da görelim:

Görüldüğü gibi bu defa açısal ivmeden gelen bilgi de hesaba katıldığından, hesaplanan açımızı etkiliyor. Ancak bu konum izleyen periyotlarda kalıcı olmadığından, doğrusal sensörümüzün yardımı ile tekrar kararlı duruma doğru düzeltiliyor. Açısal ve doğrusal sensörlerin iş birliği böyle.

KODLAR

Gelelim bu hesaplamaların uygulamasına. Programın tamamını değil, ilgili kod parçacıklarını vereceğim. Aslında bu kadarı da bir projeye girişmek için yeterli. Kodların tamamını buna dayalı bir projeyi yayınladığımda veririm. Kodlar MPU6050 için. Başlangıçta Tilen Majerle’nin kütüphanesinden yararlanmıştım ama üzerinde epeyi çalışma yaptığımdan artık onunkilere ne kadar benziyor bilemiyorum.

DOĞRUSAL İVMELERDEN AÇI HESAPLAMASI

void MPU6050_Read_Accel (MPU6050_t* sensor)
{
	uint16_t Accel_X_RAW, Accel_Y_RAW, Accel_Z_RAW;
	uint8_t Rec_Data[6];
	uint8_t SampleCount = 1;  // Tek örnekle çalışıyoruz
// MPU6050 den her bir eksendeki doğrusal ivmeleri okuyacağız.g cinsinden
	sensor->Ax = 0;
	sensor->Ay = 0;
	sensor->Az = 0;

// ACCEL_XOUT_H register'inden başlayarak 6 byte oku

for(uint8_t i = 0; i<SampleCount; i++){
HAL_I2C_Mem_Read (&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, 1, Rec_Data, 6, 1000);

// İvme değerleri ikişer byte halinde gelen 16 bit
	Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1]);
	Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3]);
	Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5]);

	/*** RAW değerleri G ye çevirelim. Bunun için okunan ham değeri tam skala
	 *   değerine bölmek gerekiyor. Tam skala seçimi ACCEL_CONFIG register
	 *   ayarlarında FS_SEL ile yapılıyor. FS_SEL = 0 yani 2g seçtiğimiz için
	 *   MPU6050_ACC_SENS_2 yani 16384 kullanıyoruz. Bu en hassas skala.      
	 *                 ****/

	sensor->Ax += Accel_X_RAW/MPU6050_ACC_SENS_2;
	sensor->Ay += Accel_Y_RAW/MPU6050_ACC_SENS_2;
	sensor->Az += Accel_Z_RAW/MPU6050_ACC_SENS_2;
}

/* okumaların ortalamasını hesaplayalım, bu değerler "g" cinsinden */

sensor->Ax = Ax/SampleCount;
sensor->Ay = Ay/SampleCount;
sensor->Az = Az/SampleCount;
}

Bu doğrusal ivme değerlerinden hareketle eğim açılarını hesaplayalım. Eğim açısı derken, her bir eksen çevresindeki dönüş açısından söz ediyoruz. X ekseni çevresindeki açısal değişim, Y ekseninin eğimindeki değişimden kaynaklanıyor, benzer şekilde Y ekseni çevresindeki açısal değişim de X ekseninin eğim değişikliğinden kaynaklanıyor :

        MPU6050_Read_Accel(&MPU6050_Sensor);

// Aşağıdaki değerler g cinsinden

      accel_x = MPU6050_Sensor.Ax ;
      accel_y = MPU6050_Sensor.Ay;
      accel_z = MPU6050_Sensor.Az ;

// Aşağıda g cinsindeki X ve Y ivmelerinden PITCH ve ROLL açılarını hesaplıyoruz.

 float hipotenus = sqrt(pow(accel_y,2)+pow(accel_z,2));

accel_angle_y = atan(-1*accel_x/hipotenus)*radToDegree;
accel_angle_x = atan(accel_y/hipotenus)*radToDegree;
accel_angle_z = 0; // z açısını (YAW) sadece gyro ile hesaplıyoruz.

Genelde Z ekseni yere dik ya da dike yakın konumda, bu eksen etrafındaki dönüşler diğer eksenlerde çok küçük doğrusal ivmeler yaratıyor. Bu durumda Z çevresindeki dönüş hareketlerini diğer eksenlerin doğrusal ivmelerinden hesaplamak hatalı sonuçlar veriyor. O nedenle Z çevresindeki dönüşleri sadece Z ekseni gyrosu ile izlemeyi tercih ediyoruz. Yerçekimsiz ortamda olsaydık eksenler arasında bir fark olmayacaktı.

AÇISAL İVMELERDEN (GYRO) AÇI HESAPLAMALARI

Gyro ölçümleri ile çalıştığımızda, açıları başlangıçta sıfır olarak kaydedilen konumlarına sonraki açısal hareketleri ekleyerek ya da çıkartarak hesaplıyoruz.

Açısal ivmelerin okunması

void MPU6050_Read_Gyro (MPU6050_t* sensor)
{
	int16_t Gyro_X_RAW, Gyro_Y_RAW, Gyro_Z_RAW;
	uint8_t Rec_Data[6];

	/* MPU6050_GYRO_XOUT_H dan başlayarak 6 byte oku.
	 * offset değerleri sistem açılışında sensör hareketsiz iken
	 * ilk 20 ms içinde ölçülüp kaydediliyor.
	 */

	HAL_I2C_Mem_Read (&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_GYRO_XOUT_H, 1, Rec_Data, 6, 1000);

	Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1])- offset_x_gyro;
	Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3])- offset_y_gyro;
	Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5])- offset_z_gyro;

/*  Bu satırları yazmak 23 ms CPU 64MHz, SPI 16 MHz */

    TFT_Printf(0,30,16,"RGx:%d     ",Gyro_X_RAW);
    TFT_Printf(100,30,16,"RGy:%d    ",Gyro_Y_RAW);
    TFT_Printf(200,30,16,"RGz:%d    ",Gyro_Z_RAW);

	/*** Ham değerleri derece/sn ye çevireceğiz. Tam skalayı GYRO_CONFIG
	 *   registerde 250 Derece/sn olarak seçtiğimiz için
	 *   MPU6050_GYRO_SENS_250, yani 131 çarpanını kullanıyoruz.
	 * ****/

	/* bu değerler derece/sn cinsinden */

	sensor->Gx = Gyro_X_RAW/MPU6050_GYRO_SENS_250;
	sensor->Gy = Gyro_Y_RAW/MPU6050_GYRO_SENS_250;
	sensor->Gz = Gyro_Z_RAW/MPU6050_GYRO_SENS_250;
}

AÇISLA İVMELERDEN AÇI HESABI

        MPU6050_Read_Gyro(&MPU6050_Sensor);
	t_now = HAL_GetTick(); // Ölçüm anını kaydet.

      gyro_x = MPU6050_Sensor.Gx;  // Değerler derece/sn
      gyro_y = MPU6050_Sensor.Gy;
      gyro_z = MPU6050_Sensor.Gz;

      dt =(t_now - t_last)/1000.0; // Saniye olarak geçen süre
      t_last = t_now;

      /* Geçen ölçümden bu yana ne kadar dönmüş ?
       * Örnekleme süresi içindeki gyro değerinin
       * sabit kaldığını varsayıyoruz.
       *
       * Belki bir önceki gyro okuması ile bu son
       * okumanın ortalamasını kullanmak daha iyi
       * sonuç verebilir.
       */

      gyro_angle_x = gyro_x*dt + last_x_angle;
      gyro_angle_y = gyro_y*dt + last_y_angle;

/* Z eksenindeki açısal titreşimleri ayıklayalım. */ 
int gz_threshold = 2; 
if(gyro_z < gz_threshold && gyro_z > -gz_threshold) gyro_z = 0;

      gyro_angle_z = gyro_z*dt + last_z_angle;

GYRO VE DOĞRUSAL İVME SONUÇLARINI BİRLEŞTİREREK NİHAİ AÇILARIN HESAPLANMASI

      /* Açıdaki değişim (gyro) ve static ivme değerlerini birleştirerek
       * "complementary filter" ile nihai düzlem açılarını hesaplayalım.
       * Pitch(y ya da x), Roll (y ya da x) ve Yaw (z) açıları.
       *
       * Buradaki alpha katsayısı Gyro ve düz ivme okumalarına verilecek
       * önemi belirleyen ağırlık faktörü. Genelde titreşimlerin etkisinden
       * çok etkilenen düz ivme değerlerinin payını küçük tutuyoruz.
       *
       * Not: bu açılar sisteme güç verildiğindeki konumu referans almakta.
       * sistem resetlendiğinde yatay ve hareketsiz durumda olmalıdır.
       *
       * Sistem resetlendiğinde sadece düz ivme değerlerinin (çok sayıda örnek
       * ortalaması ile hesaplanarak) başlangıç değeri olarak kabul edilmesi
       * düşünülebilir.
       */
      const float alpha = 0.96;

      angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
      angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
      angle_z = gyro_angle_z; 

// Bir sonraki gyro/açı hesaplaması için bu konumu kaydedelim 

      last_x_angle = angle_x;
      last_y_angle = angle_y;
      last_z_angle = angle_z;

PROJE KURULUMU

Bu hesaplamaların uygulama ve doğrulamasını MPU6050 6 eksenli ivme ölçer ile yaptım. Denetleyicim 64Mhz de çalışan STM32F103 mapple kiti, üzerinde de 320×240 SPI TFT takılı.

Ekrandaki ilk iki satırda Rax,Ray ve Raz değerleri doğrusal ivmeler, Rgx, Rgy ve Rgz değerleri de açısal ivmeler. Bunlar sensörden geldikleri halleri ile, RAW yani ham değerler.

Üçüncü ve dördüncü satırlarda ham değerlerin g ve derece/sn karşılıkları görülüyor.

Beşinci satırdaki Xang, Yang, Zang değerleri doğrusal ivmelerden hesaplanmış açı değerleri.

Alt taraftaki X, Y ve Z değerleri de açısal ve doğrusal ivmelerden hareketle hesaplanmış eğim açıları.

En altta da örnekleme periyodu, ms olarak. Periyod oldukça uzun, zira ekrandaki yazıları yazmak bu kadar sürüyor. Yoksa MPU6050 her milisaniyede bir okuma verebiliyor, diğer işlemler de o kadar vakit almıyor. Ama, ekrana hiç birşey yazmasam da örnekleme periyodu 10ms nin altına inmiyor. En fazla vakit alanı da kare, karekök ve Arctan işlemleri, Daha fazla hız gerektiğinde daha hızlı bir mikro denetleyiciye geçilebilir.

SONUÇ

Bu yayında, pek çok örnek uygulamada karşımıza çıkan hesaplamaların dayandığı temeli açıklamaya çalıştım. Eğer benim gibi, bu hesap neden böyle yapılıyor diye takıntı yapanlardan iseniz, 3 boyutlu bir sistemdeki vektörleri, açıları gözünüzde canlandırmakta güçlük çekiyorsanız işinize yarayacağını düşünüyorum.

Buradaki hesaplamalar Z ekseninin düşey ve düşeye yakın olduğu uygulamalara yönelik. Eksenlerin yer değiştirdiği uygulamalarda, havada uçarken resetlenen sistemlerde sorun yaşanabilir. Yani taklalar atarak düşmekte olan bir dronu buradaki hesaplamalarla düzeltmek mümkün değil. Bunu yapmak iddialı bir proje olarak ele alınabilir, yaparsanız NASA ya başvurabilirsiniz. Neil Armstrong’un Gemini 8/Agena misyonundaki (1966) muhteşem pilotluk becerisini kodlamış olursunuz.

Yayında özellikle X açısı (Angle_x) dediğimiz şeyin X ekseninin eğimi değil, X eksenindeki açısal dönüş, Y açısı (Angle_y) dediğimizin de Y ekseninin eğimi değil, Y ekseni çevresindeki dönüş olduğunu vurgulamaya çalıştım. Zira bu kafa karışıklığına yol açan bir durum.

Bu yayının sonu – Selçuk Özbayraktar Ağusts 2021