Mag, acc ve gyro verilerinden pitch, yaw ve roll hesaplama


19

9 derece serbestlik sensörüne sahip bir Arduino kartım var.

9-DOF sensöründen bir veri kümesi örneği:

İvmeölçer (m / s)

  • AccX = -5,85
  • AccY = 1,46
  • AccZ = 17,98

Jiroskop (RPM)

  • GyrX = 35,14
  • GyrY = -40,22
  • GyrZ = -9,86

Manyetometre (Gauss)

  • MagX = 0,18
  • MagY = -0,04
  • MagZ = -0,15

Bu verilerden adım, sapma ve yuvarlamayı nasıl hesaplayabilirim?


1
Temel ilke: İvmeölçerinizdeki yerçekiminin saptanmasından hangi yolun aşağı olduğunu bilirsiniz; manyetometrenizdeki dünyanın manyetik alanının tespit edilmesinden hangi yolun Kuzey olduğunu bilirsiniz. Buna dayanarak ve başka önemli hızlanmaların veya güçlü manyetik alanların olmadığını varsayarsak, kendi tutumunuzu belirleyebilirsiniz.
Welf

1
Jiroskop verileri, bir dönüş hızı sağlar, ancak mutlak bir konum sağlamaz. Bilinen bir tutumdan değişikliği tahmin etmek için entegre edilebilir, ancak bu genellikle gürültülüdür ve diğer sensörler ile birlikte kullanılmazsa kaymaya eğilimlidir.
Welf

1
aynı zamanda kalman filtrelerine de atıfta bulunun çünkü statik sayıların oldukça fazla işlenmesi gerekir, yuvarlanma aralığı ve yalpalama hakkında güvenilir tahminler verir. Ayrıca, sensörün konumunun önemli olduğunu unutmayın (dikkate almanız gerekir).
Gürkan Çetin

Yanıtlar:


15

Pitch, yuvarlanma ve sapma X, Y ve Z ekseni etrafındaki dönüş olarak tanımlanır. Aşağıdaki tanımı tanımlamak için bir resim olarak.

Rulo Aralığı ve Sapma

Önceki bir projede, rulo ve eğimi hesaplamak için Analog Devices'den bir ADXL345 İvmeölçer kullandım. Aşağıda, hesaplanan yuvarlanma ve eğim için kullanılan denklemler verilmiştir. Bazı kaynak kodlarını kamuya açık hale getirdim.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Tam kaynak kodu burada bulunabilir .

Yukarıdaki tanımlara dayanarak

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Not: M_PI = 3.14159265358979323846 math.h dosyasında sabit olarak tanımlanmıştır

Aşağıda, size yardımcı olabilecek Arduino temel kaynak kodu gibi bazı referanslar verilmiştir.


Referanslar:


2
Güzel bir cevap, sensörün araçtaki konumunun ve yönünün önemli olacağını ve verilerin güvenilir sonuçlar elde etmek için daha fazla işlenmesi gerektiğini eklemeye değer. (GPS gibi daha güvenilir düşük frekanslı verilerle filtrelenmiş veya kaynaştırılmış)
Gürkan Çetin

(@Zubair) "yaw = 180 * atan (accelerationZ / sqrt (accelerationXaccelerationX + accelerationZaccelerationZ)) / M_PI;" bu 'M_PI' nedir ??
Wasabi

@Wasabi M_PI = 3.14159265358979323846. Math.h kütüphanesinde sabit olarak tanımlanır.
Mahendra Gunawardena

9

Bu yüzden aşağıdaki uzun cevabım, tahtanın hızlanacağını varsayar ve bu süre zarfında kısa bir süre içinde saha, yuvarlanma ve sapmayı ölçebilmeniz gerekir. Tahta tüm ölçümler için sabit olacaksa, Mahendra Gunawardena'nın cevabı sizin için mükemmel bir şekilde çalışacaktır. Bu, segway veya model düzlem veya çok rotorlu bir cihaza girerse veya hareket eden bir şey varsa, okumaya devam etmek isteyebilirsiniz. Bu yazı, sensör füzyonu adı verilen bir yöntemle her üç sensörün de nasıl kullanılacağı ile ilgilidir. Sensör füzyonu, her sensörün güçlü yönlerini almanızı ve her sensör zayıflığının etkilerini en aza indirmenizi sağlar.

Sensör özellikleri ve arka planı

İlk olarak, bir ivmeölçerin sadece yerçekimi kuvvetini değil, kendisine uygulanan tüm kuvvetleri ölçtüğünü anlayın. Dolayısıyla, ivmeölçerin herhangi bir titreşim olmadan sabit bir pozisyonda olduğu mükemmel bir dünyada, Mahendra Gunawardena'nın cevabının gösterdiği gibi bazı temel trigonometri kullanarak hangi yolun yükseldiğini mükemmel bir şekilde belirleyebilirsiniz. Ancak bir ivmeölçer tüm kuvvetleri toplayacağından, titreşimler gürültüye neden olur. Ayrıca, pano hızlanıyorsa, ivmeölçerin bildirdiği kuvvet sadece yerçekimi kuvveti değil, aynı zamanda hızlanmanıza neden olan kuvvet olduğu için sadece basit trigonometri kullanamazsınız.

Bir manyetometre, bir ivmeölçerden daha basittir. Hareket bununla ilgili sorunlara neden olmaz, ancak demir ve diğer mıknatıslar gibi şeyler çıktınızı etkiler. Eğer bu parazite neden olan kaynaklar sabit ise, bununla başa çıkmak zor değildir, ancak bu kaynaklar sabit değilse, kaldırılması sorunlu olan tonlarca gürültü yaratacaktır.

Üç sensörden, jiroskop en güvenilir olanıdır ve normal olarak dönme hızını ölçmede çok iyidir. Demir kaynakları ve ivme gibi şeylerden etkilenmez, temel olarak dönme hızını ölçme yetenekleri üzerinde hiçbir etkisi yoktur. Cihazın dönme hızını bildirmek için çok iyi bir iş çıkarırlar, ancak mutlak bir açı aradığınızdan, pozisyon almak için hızı entegre etmeniz gerekir. Bunu yapmak, son ölçüm hatasını yeni ölçümlerin hatasına ekleyecektir, çünkü entegrasyon temel olarak bir aralıktaki değerlerin toplamıdır, bir ölçüm hatası saniyede sadece 0.01 derece kapalı olsa bile, 100 ölçümde, konumunuz 1 derece kapalı olabilir, 1000 ölçüm ile 10 derece kapalı olabilir. Saniyede yüzlerce ölçüm yapıyorsanız, bunun sorunlara neden olduğunu görebilirsiniz. Buna genellikle jiroskop kayması denir.

Sensör füzyonu

Artık tüm bu sensörlerin birlikte çalışmasının güzelliği, jiroskopun sapmasını iptal etmek için ivmeölçer ve manyetometreden gelen bilgileri kullanabilmenizdir. Bu, ölümcül jiroskop sapması olmadan jiroskopun doğruluğunu ve hızını vermenizi sağlar.

Bu üç sensörden gelen verileri birleştirmek birden fazla şekilde yapılabilir, tamamlayıcı bir filtre kullanmaktan bahsedeceğim çünkü daha basit bir kalman filtresi ve kalman filtreleri gömülü sistemlerde çok daha fazla kaynak tüketecek. Çoğu zaman tamamlayıcı bir filtre yeterince iyidir, uygulanması daha kolaydır (önceden oluşturulmuş bir kitaplık kullanmadığınız varsayılarak) ve verileri daha hızlı işlemenizi sağlar.

Şimdi sürece. Yapmanız gereken ilk adımlar, açısal hızı açısal konuma dönüştürmek için jiroskop çıktısını entegre etmektir. Ayrıca, çıkıştaki gürültü ile başa çıkmak için ivmeölçer ve manyetometreye düşük geçiş filtresi uygulamanız gerekecektir. Aşağıda gösterilene benzer basit bir FIR filtresi burada çalışır. Bazı trigonometri ile ivmeölçerle perdeyi ve ruloyu ve manyetometre ile sapmayı bulabilirsiniz.

filteredData = (1-weight)*filteredData + weight*newData

Ağırlık, ne kadar gürültüyle uğraşmanız gerektiğine bağlı olarak ayarlanabilen bir sabittir, gürültü ne kadar yüksek olursa ağırlık değeri o kadar küçük olur. Şimdi sensörlerden gelen verileri birleştirmek aşağıdaki kod satırı ile yapılabilir.

fusedData = (1-weight)*gyroData + weight*accelMagData

Verilerin zift, yuvarlanma ve sapma vektörü olduğuna dikkat edilmelidir. İsterseniz diziler yerine bunu yapmak için üç değişken kullanabilirsiniz. Bu hesaplama için cayro hatve, yuvarlanma ve sapma dereceleri olarak bir konum sağlar, manyetometre sapma için bir açı sağlarken ivmeölçer hatve ve yuvarlanma için kendi sayılarını sağlar.

Hala daha fazla bilgi istiyorsanız google "tamamlayıcı filtreli sensör füzyonu" yapabilirsiniz Bu konuda birçok makale var.


3

Gaz pedalı sensörü verilerinden, yalnızca perdeyi ve yuvarlamayı hesaplayabilirsiniz. Freescale'nin feryat belgesi neye ihtiyacınız olduğunu bolca bilgi ile açıklıyor:

AN3461 - Üç Eksenli İvmeölçer Kullanarak Eğim Algılama

Belgenin sözlerine dayanarak,

taba rengiφxyz=G,pyG,pz

taba rengiθxyz=-G,pxG,pygünahφ+G,pzmarulφ=-G,pxG,py2+G,pz2

ki şuna eşittir:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Tabii ki, sonuç sadece belirli bir düzende (Rxyz) rotasyonlar meydana geldiğinde:

  1. X ekseni etrafında açı ile yuvarlayın φ
  2. Y ekseni etrafında açı ile eğim verin θ
  3. Açı z ekseni etrafında sapma ψ

Dönüş sırasına bağlı olarak farklı denklemler elde edersiniz. İçinR,xyz dönme sırası, açıyı bulamıyorsunuz ψ z ekseni etrafındaki Yaw için.


1
O, Mühendislik SE hoş geldiniz! Bu site Lateks'i destekliyor, cevabınızın ne kadar güzel olduğuna bakın. :-)
peterh - Monica
Sitemizi kullandığınızda şunları okuyup anladığınızı kabul etmiş olursunuz: Çerez Politikası ve Gizlilik Politikası.
Licensed under cc by-sa 3.0 with attribution required.