Yaw, Pitch, Roll hesaplama

Başlatan Cemre., 11 Nisan 2016, 18:34:40

Cemre.

Herkese iyi günler,

Elimde MPU6050 sensör ve beklediğim HMC5883L sensör var.

Bu sensörlerden alacağım 9 eksen ham veriyle Yaw, Pitch, Roll açılarını hesaplamak istiyorum. Ancak MPU6050'nin DMP özelliğini kullanmadan bunu yapacağım. Bu işin matematiği nedir, beni hızlıca sonuca götürecek kaynak bulabilir miyim?

STM32F407VGT işlemciyi (FPU birimi var) 168MHz'de koşturacağım o yüzden matematiksel işlemleri çok dert etmiyorum açıkçası, InvenSense'in yayınladığı dökümantasyonundan hiç birşey anlamadığım ve ne olup bittiğini çözemediğim DMP kütüphanesini kullanmak yerine 9 eksen verisiyle gimbal lock olmadan (sanırım complementary filter gerekli bunun için bilmiyorum) yaw pitch roll açılarını hesaplamak istiyorum.

Teşekkürler, iyi günler.

mesaj birleştirme:: 11 Nisan 2016, 19:47:15

http://www.x-io.co.uk/open-source-ahrs-with-x-imu/

Bu linkteki algoritmayı daha önce kullanan oldu mu?

mistek

Öncelikle MPU6050 den veri okuyabiliyor musunuz? Gyro ve Acc bilgilerini alabiliyorsanız algoritmaya uyarlaması kolay. İşin matematiğini öğrenmek istiyorum derseniz o konuda çok iyi değilim.

Not: MPU6050 kullanmadım.

http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ burada sayfanın altında C kodu var indirip Mahony olanı projeye dahil edin.
boş işlerin adamı ---- OHM Kanunu: I = V/R ---- Güç Formülü: P = V*I = I^2*R = V^2/R

Cemre.

#2
MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)


bu fonksiyona gireceğimiz değerlerin birimleri konusunda sıkıntım var sanırım.
MPU6050 ve HMC5883L kullandığımı söylemiştim.

MPU6050'nin İvme verisi 2048 LSB/mg Full Scale'e ayarlı. Programın içinde ham veriyi 2048'e bölüyorum.
Jiroskop verisi 2g'ye ayarlı. Programın içinde 131'e bölüyorum.

HMC5883L Gain 1090'a ayarlı, ham veriyi 0,92 ile çarpıyorum.

Gx Gy Gz'yi (pi/180) ile çarpıp radyana çeviriyorum. Hatam var mı? @mistek



mesaj birleştirme:: 16 Nisan 2016, 15:10:44

Hocam MPU6050 ve HMC5883L'den ham veri okuyabiliyorum. Zaten projeyi oluştururken bu konuda sıkıntı çekmemek için adım adım ilerlemiştim.

Gx:-0.23,Gy:-0.72,Gz:-0.20,Ax:0.06,Ay:0.02,Az:1.08,Mx:-25.76,My:-219.88,Mz:-361.56
Gx:-0.26,Gy:-0.75,Gz:-0.24,Ax:0.06,Ay:0.02,Az:1.08,Mx:-25.76,My:-219.88,Mz:-362.48
Gx:-0.21,Gy:-0.80,Gz:-0.08,Ax:0.06,Ay:0.02,Az:1.08,Mx:-25.76,My:-219.88,Mz:-361.56
Gx:-0.22,Gy:-0.75,Gz:-0.21,Ax:0.06,Ay:0.02,Az:1.07,Mx:-24.84,My:-219.88,Mz:-362.48
Gx:-0.22,Gy:-0.67,Gz:-0.19,Ax:0.06,Ay:0.02,Az:1.08,Mx:-24.84,My:-220.80,Mz:-361.56
Gx:-0.23,Gy:-0.75,Gz:-0.07,Ax:0.06,Ay:0.02,Az:1.08,Mx:-25.76,My:-219.88,Mz:-361.56
Gx:-0.27,Gy:-0.77,Gz:-0.20,Ax:0.06,Ay:0.02,Az:1.08,Mx:-25.76,My:-221.72,Mz:-361.56
Gx:-0.24,Gy:-0.74,Gz:-0.26,Ax:0.06,Ay:0.02,Az:1.07,Mx:-26.68,My:-220.80,Mz:-362.48
Gx:-0.24,Gy:-0.80,Gz:-0.03,Ax:0.06,Ay:0.02,Az:1.08,Mx:-25.76,My:-219.88,Mz:-361.56
Gx:-0.16,Gy:-0.67,Gz:-0.23,Ax:0.06,Ay:0.02,Az:1.07,Mx:-24.84,My:-220.80,Mz:-361.56
Gx:-0.25,Gy:-0.73,Gz:-0.21,Ax:0.06,Ay:0.02,Az:1.08,Mx:-23.00,My:-221.72,Mz:-363.40
Gx:-0.24,Gy:-0.83,Gz:-0.02,Ax:0.06,Ay:0.02,Az:1.08,Mx:-22.08,My:-223.56,Mz:-363.40
Gx:-0.24,Gy:-0.74,Gz:-0.25,Ax:0.06,Ay:0.02,Az:1.08,Mx:-22.08,My:-224.48,Mz:-364.32
Gx:-0.24,Gy:-0.68,Gz:-0.22,Ax:0.06,Ay:0.02,Az:1.08,Mx:-22.08,My:-223.56,Mz:-364.32
Gx:-0.27,Gy:-0.72,Gz:-0.07,Ax:0.06,Ay:0.02,Az:1.08,Mx:-23.00,My:-222.64,Mz:-364.32
Gx:-0.26,Gy:-0.69,Gz:-0.34,Ax:0.06,Ay:0.01,Az:1.08,Mx:-22.08,My:-222.64,Mz:-364.32
Gx:-0.27,Gy:-0.72,Gz:-0.22,Ax:0.06,Ay:0.02,Az:1.08,Mx:-22.08,My:-222.64,Mz:-363.40
Gx:-0.25,Gy:-0.79,Gz:-0.05,Ax:0.06,Ay:0.02,Az:1.08,Mx:-21.16,My:-221.72,Mz:-364.32
Gx:-0.30,Gy:-0.75,Gz:-0.29,Ax:0.06,Ay:0.02,Az:1.08,Mx:-21.16,My:-223.56,Mz:-364.32
Gx:-0.23,Gy:-0.73,Gz:-0.15,Ax:0.06,Ay:0.02,Az:1.08,Mx:-21.16,My:-221.72,Mz:-364.32
Gx:-0.25,Gy:-0.69,Gz:-0.02,Ax:0.06,Ay:0.01,Az:1.08,Mx:-22.08,My:-222.64,Mz:-364.32
Gx:-0.28,Gy:-0.81,Gz:-0.21,Ax:0.06,Ay:0.02,Az:1.08,Mx:-23.00,My:-222.64,Mz:-363.40
Gx:-0.23,Gy:-0.71,Gz:-0.14,Ax:0.06,Ay:0.02,Az:1.08,Mx:-20.24,My:-220.80,Mz:-363.40
Gx:-0.24,Gy:-0.74,Gz:-0.08,Ax:0.06,Ay:0.01,Az:1.08,Mx:-21.16,My:-222.64,Mz:-363.40
Gx:-0.24,Gy:-0.78,Gz:-0.34,Ax:0.06,Ay:0.02,Az:1.08,Mx:-21.16,My:-221.72,Mz:-364.32
Gx:-0.26,Gy:-0.72,Gz:-0.14,Ax:0.06,Ay:0.01,Az:1.08,Mx:-21.16,My:-221.72,Mz:-363.40
Gx:-0.27,Gy:-0.76,Gz:-0.07,Ax:0.06,Ay:0.02,Az:1.08,Mx:-22.08,My:-221.72,Mz:-365.24
Gx:-0.23,Gy:-0.75,Gz:-0.40,Ax:0.06,Ay:0.02,Az:1.08,Mx:-21.16,My:-221.72,Mz:-363.40
Gx:-0.25,Gy:-0.73,Gz:-0.13,Ax:0.06,Ay:0.01,Az:1.08,Mx:-22.08,My:-222.64,Mz:-364.32
Gx:-0.24,Gy:-0.74,Gz:-0.11,Ax:0.06,Ay:0.01,Az:1.07,Mx:-22.08,My:-221.72,Mz:-364.32
Gx:-0.27,Gy:-0.76,Gz:-0.37,Ax:0.06,Ay:0.02,Az:1.08,Mx:-21.16,My:-222.64,Mz:-364.32
Gx:-0.27,Gy:-0.79,Gz:-0.18,Ax:0.06,Ay:0.01,Az:1.08,Mx:-22.08,My:-222.64,Mz:-364.32
Gx:-0.20,Gy:-0.79,Gz:-0.11,Ax:0.06,Ay:0.02,Az:1.08,Mx:-21.16,My:-220.80,Mz:-364.32


Hareketsiz durumda aldığım ham veriler.

mistek

Hocam birimleri tekrar kontrol ederseniz daha iyi olur. Hareketsiz durumda çıkan veriler biraz büyük. Gyro için en azından.

Onun haricinde AHRS algoritmasına geçirilen parametreler de Radyan Derece dönüşümü yapıyorduk. Proje dosyalarım kaybolduğu için hatırlayamıyorum. Aynısını kullanmıştım.
boş işlerin adamı ---- OHM Kanunu: I = V/R ---- Güç Formülü: P = V*I = I^2*R = V^2/R

Cemre.

#4
Alıntı yapılan: mistek - 16 Nisan 2016, 15:25:58
Hocam birimleri tekrar kontrol ederseniz daha iyi olur. Hareketsiz durumda çıkan veriler biraz büyük. Gyro için en azından.

Onun haricinde AHRS algoritmasına geçirilen parametreler de Radyan Derece dönüşümü yapıyorduk. Proje dosyalarım kaybolduğu için hatırlayamıyorum. Aynısını kullanmıştım.

Hocam Gyro ve Acc verileri için başka bir projeden aldığım otomatik Offset hesaplama fonksiyonu vardı. Onlara falan gerek var mı? Ondan böyle bir değer üretiyor olabilir mi?

mesaj birleştirme:: 16 Nisan 2016, 16:18:40

@mistek Hocam sorumu şöyle düzelteyim.

HMC5883L yere paralel sadece azimut açısını (pusula yani) hesaplasa, bu algoritmanın 6 değişken girilerek Euler hesaplayan halini kullanarak Pitch açısını alsam ne kadar sağlıklı olur? Birbirinden izole iki sensörün toplamda 3 açı üretmesini sağlamaktan bahsediyorum.

mistek

Hocam magnetometre etrafındaki nesnelerden çok etkileniyor. Bu algoritmaların sadece gyro ve acc kullanarak açı hesaplayan fonksiyonuda olacaktı. Açıların doğruluğu benim için yeterliydi. Doğru ölçülüyor. Tabiki 0.5 dereceye kadar fark oluyordu belki ama sizede yeterli olur diye düşünüyorum.
boş işlerin adamı ---- OHM Kanunu: I = V/R ---- Güç Formülü: P = V*I = I^2*R = V^2/R

Cemre.

Hocam benim asıl amacım Azimuth açısı ve o anki Pitch açısını bulmak. Azimuth kuzey kutup noktası ile doğu yönünde yaptığımız açıyı tanımlıyor. Yani magnetometre kullanmak durumundayım. Zaten kullanılmadığı durumda algoritma ilk konumu 0 olarak algılıyor anladığım kadarıyla. Kullanıldığı durumda ise yaw eksenine azimuth açısını döndürüyor. Ancak bu da düz bir zemindeysek geçerli. Bunu da kompanze eden matematiksel hesaplamalar vardı. Gyro ve Acc verisini kullanarak azimuth'un düz zeminde olmasak da doğru hesaplanmasını sağlayan hesaplar.

Şu an DMP ile uğraşıyorum. MPU ile HMC5883'ün ham verisini okuyup FifoBuffer'a aktardım DMP'de iken ancak DMP programı bu veriyi işlemiyor olacak ki yine yukarıda anlattığım durum oluşuyor. Her enerji verdiğimde o noktayı 0 kabul edip 40* drift yapıyor ve sonra hareket algılıyor. Belki bu durumda gelen Quaternion verileriyle oynayarak yazılımsal birşeyler yapılabilir. Bayağı bir karıştı kafam zaten. Mahony Madgwick algoritmaları da tam bir muamma, hiç onların videolarındaki etkiyi yakalayamadım. Biryerlerde hata yapıyorum ama nerede? Tek istediğim 3 tane açı :( Çok mu yav :(

mistek

Hocam keşke daha fazla yardımcı olabilsem. Sensörlerden okunan verilerden muhakkak emin olduktan sonra diğer algoritmalara geçerseniz daha iyi olur. Ben X/Y/Z eksenleri için açı değerlerini bu algoritmalarla elde etmiştim. Azimuth konusunda diyecek bişeyim yok :)
boş işlerin adamı ---- OHM Kanunu: I = V/R ---- Güç Formülü: P = V*I = I^2*R = V^2/R

ogoculu

merhabalar,
Bahsettiğin konuyla alakalı bende uğraşıyorum fakat bende henüz işin içinden çıkamadım. mpu 6050 yaw değerini bir türlü çözemedim. hatta benim için referans noktası önemli değil, 10 dakika istikrarlı olacak şekilde sadece yaw değerini hesaplayabilmek benim için yeterli. Ardunio ile yapılmış mükemmel bir kütüphane var. direk sana roll pitch ve yaw değerini oldukça istikrarlı şekilde veriyor. fakat ne yaptım ise PIC için herhangi bir dile çeviremedim. ben pic için roll ve pitch işini çözdüm ama matematiğim yaw değeri için yetmiyor. Arduino kullanıyor isen github dan direk kütüphanesini bulabilirsin. Ben pic için aramadığım yer kalmadı diyebilirim. aslında arduino yazım diline biraz alışık biri kolaylıkla çözebilir. İşine yarayacak ise roll ve pitch için ccs kodları verebilirim.

atknkrt

iyi çalışmalar,
bitirme projesi için kamera dengeleyici yapıyorum ve stm32 kartını kullandım. proje şu an çalışıyor fakat veri değerlerini bir txt dosyasına kaydediyorum fakat dosya içindeki verileri anlamladıramadım.  bu konu hakkında bildiklerinizi paylaşabilir misiniz?