Uzun sayılabilecek bir aradan sonra ofisime geri dönüp çalışmalara yavaştan başladım ve tabi hemen yarım kalan hareket takip uygulamasının başına geçtim. Bu yazıda da size kullandığım araçların ne olduğunu nasıl çalıştığını ve nasıl kullanılabileceğini anlatacağım.Konu ile alakalı kaynak bulunması biraz zor olduğundan bir çok kişiye faydalı olacağını düşünüyorum. Yazının sonunda da uygulamamın bir test videosunu bulacaksınız.
Sensörlerin anlatımınıda kullandığım grafikleri ve örneği Starlino’nun sitesinden aldım. Yazının orjinalini linkten okuyabilirsiniz. Ayrıca sitesinde yazmış olduğu örnek kodlarıda mevcut.Şimdi accelerometer, gyroscope ve IMU nun ne olduğuna bakalım.
Accelerometer
Accelerometerlar üzerlerine düşen statik(yerçekimi) veya dinamik (aniden hızlanma veya durma) ivmeyi ölçmektedirler. Sensörden aldığımız değer m/s2 veya yer çekimi (g-Force) türünden ifade edilebilir. Uygulamalarda genelde yerçekimi türünden ifade edilmektedir. Eğer uzayda veya herhangi bir çekim alanının kapsamında değilseniz sensör üzerine 1g lik bir yerçekimi kuvveti etki etmektedir. Buda hepinizin bildiği gibi yaklaşık olarak 9.8m/s2 dir ve dünyadaki bulunduğunuz noktaya göre değişiklik göstermektedir. Sensör sürekli olarak yer çekimi etkisi altında kaldığından eğim ölçer (örneğin yeni nesil akıllı cep telefonlarında kullanılmaktadır ve siz telefonu dikey veya yatay konuma getirdiğinizde telefonun ekranı hareketinize göre değişmektedir) veya hareket algılayıcı (nintendo wii gibi ürünlerde elinizi salladığınızda oyundaki karakterlerde benzeri bir hareket yapar) olarak kullanılabilmektedir. Ölçü skalası olarak ± 1g, ± 2g, ± 4g ... gibi değerler ile ifade edilmektedir ve bir, iki ve üç eksende ölçüm yapabilen türevleri vardır. Şimdi bu sensörlerin nasıl çalıştığına bakalım.
Şimdi uzayda olduğunuzu düşünün. Herhangi bir çekim etkisi yok ve ağırlığınız 0 dır. Önünüzde de aşağıdaki şekildeki gibi bir kutu, kutunun ortasındada bir küre olduğunu hayal edin. Herhangi bir çekim etkisi olmadığından küre herhangi bir yüzeye temas etmeden hareketsiz bir şekilde durmaktadır. Kürenin hareketini görebilmek için kutunun +Y yönünde kalan yüzeyinide kesip atalım.
Kutuyu elinizde tutup +X yönünde 1g kuvveti ile hızlandırdığımızda küre kutunun –X yüzeyine eylemsizlikten dolayı 1g lik bir kuvvet uygulayacaktır.
Şimdi kutumuzu alıp dünyaya dönelim. Kutuyu yere koyduğumuzda dünyamızın 1g lik yer çekimi kuvvetinden dolayı küre –Z yüzeyine 1g kuvvet uygular.
İvmeölçerlerde benzer bir şekilde çalışmaktadır. Yüzeyleri basınca ( piezoelektrik olabilir ) veya manyetik alana tepki verecek şekilde yapılmaktadır ve ivmeölçerde bu tepkiyi ölçerek bize bir değer vermektedir. Sensörün yer yüzü ile yaptığı açı değiştiğinde sensörün eksenlerine uygulanan kuvvette değişecektir ve bizde yeni değerleri okuyarak yeryüzü ile yaptığımız açıyı trigonometri yardımıyla hesaplarız. Örneğin kutumuzu 45 derece sağa doğru çevirdiğimizi düşünelim. Bu durumda kürenin –X ve –Z yüzeylerine kök içinde ½ lik bir kuvvet uygulanır oda 0,707g ye eşittir.
Şimdi kutu modelinden koordinat sistemi modeline geçelim ve aşağıdaki şekli inceleyelim.
Burada R vektörü ivmeölçerimiz üzerine düşen kuvvet vektörü olsun. Bu kuvvet yukarda anlattığımız gibi yerçekimi veya sensörün hareketi sonucu yerçekimi kuvveti ile eylemsizlik kuvvetinin bileşkesi olabilir. R vektörünün 3 bileşeni vardır ve R=[Rx,Ry,Rz] olarak ifade edilmektedir. Pisagor teoreminden R vektörünü bu bileşenlerden aşağıdaki gibi hesaplayabiliriz.
R^2 = Rx^2 + Ry^2 + Rz^2
Rx, Ry ve Rz bileşenlerini bildiğimiz taktirde yukarıdaki şekilden görüldüğü gibi trigonometrik fonksiyonlar yardımı ile R vektörünün X ve Y eksenleri ile yaptığı açıları hesaplayıp sensörümüzün yeryüzüne göre konumunu bulabiliriz. R vektörünün bileşenlerinide bize ivme ölçer g kuvveti türünden vermektedir. Şimdi bir örnek yapıp konuyu anlayalım.
Analog sensör kullandığımızı, besleme gerilimimizin 3v3 olduğunu ve 10 bitlik bir ADC kullandığımızı varsayalım. Accelerometerdan ADC yardımı ile aşağıdaki binary değerleri okuduğumuzu varsayalım. Bu değerler ile işlem yapabilmek için g türünden ifade edebilmemiz gerekmektedir. Bunun içinde ADC den okuduğumuz değerleri voltaj türünden ifade etmeli ve kullandığımız sensörün datasheetinden faydalanarak bu voltaj değerlerini g kuvveti türünden ifade etmemiz gerekmektedir. Böylece elde ettiğimiz değerleri trigonometrik fonksiyonlar ile açıya çevirebiliriz.
AdcRx = 586
AdcRy = 630
AdcRz = 561
Bu değerler ham binary değelerdir ve volt türünden ifade edebilmek için LSB başına düşen gerilim miktarı ile çarpmamız gerekmektedir. Besleme 3V3 olduğundan ve 10bit ADC kullandığımızdan yukarıdaki değerleri 3.3V/ 2^10 ile çarparsak volt değerlerini elde ederiz.
VoltsRx = 586 * 3.3V / 1023 =~ 1.89V
VoltsRy = 630 * 3.3V / 1023 =~ 2.03V
VoltsRz = 561 * 3.3V / 1023 =~ 1.81V
Bu değerleride g türünde ifade etmemiz gerekmektedir. Her ivmeölçerin 0g de vermiş olduğu bir gerilim değeri vardır ve Zero-G değeri olarak isimlendirilmektedir. Bu değer genelde Vdd/2 dir (bizim örneğimizde 3.3/2=1.65V. Kullandığınız sensörün datasheetinden bakabilirsiniz. Yukarıdaki voltaj değerlerinden bu değeri çıkartıp sensörün hassasiyeti ile çarparsak hangi eksene ne kadar kuvvet uygulandığını bulabiliriz. Sensör hassasiyetide 0.4785V/g olsun. O halde tam denklemimiz aşağıdaki gibi olur ve yukarıdaki değerleri denklemde yerine koyarsak eksenlere uygulanan kuvvetleri bulabiliriz.
Rx = (AdcRx * Vref / 1023 - VzeroG) / Sensitivity = (586 * 3.3V / 1023 – 1.65V) / 0.4785V/g =~ 0.5g
Ry = (AdcRy * Vref / 1023 - VzeroG) / Sensitivity = (630 * 3.3V / 1023 – 1.65V) / 0.4785V/g =~ 0.79g
Rz = (AdcRz * Vref / 1023 - VzeroG) / Sensitivity = (561 * 3.3V / 1023 – 1.65V) / 0.4785V/g =~ 0.33g
Şimdi yukarıdaki şekilde gösterilen R vektörünün X ve Y eksenleri ile yaptığı açıları hesaplayalım. Şekilde görülen Axr açısının cosinüsü bize Rx/R değerini vermektedir. Rx ve R değerleri bilindiğine göre arccosinüs ile Axr açısını elde edebiliriz. Benzer şekilde Ayr açısını hesaplayabiliriz. R vektörünüde yukarıdaki birinci eşitlikten hesaplayıp aşağıdaki arccos fonksiyonlarına değerleri koyduğumuzda istediğimiz açıları almış oluruz.
cos(Axr) = Rx / R , Axr = arccos(Rx/R)
cos(Ayr) = Ry / R , Ayr = arccos(Ry/R)
cos(Azr) = Rz / R , Azr = arccos(Rz/R)
Gyroscopes
Gyroscopelar basitçe bir tekerleğin ekseni etrafında hızlıca döndürülmesi sonucu ortaya çıkarlar. Tekerleğin etrafındaki çembere dik açıyla kenetlenmiş başka bir çember ve bu çemberlere dik açıyla tutturulmuş başka bir çember jiroskobu modeller. Jiroskobun öne çıkan iki ö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 olarak dünyaya dönük kalması, uçaklarda ve çeşitli araçlarda yapay ufuk oluşturulması ve otopilot gibi uygulamalarda kullanılmaktadır.Aşağıdaki videodan anlattığım bu özellikleri görsel olarak izleyebilirsiniz.
Biz uygulamamızda bir şeyin bir eksen etrafında ne kadar hızla döndüğünü başka bir deyişle açısal hızını öğrenmek için kullanıyoruz. Bu hız dakikadaki dönüş sayısı (RPM) yada saniyedeki dönüş derecesi (°/sn) olarak ifade edilmektedir. Piyasada entegre devre olarak satılan modelleri rahatlıkla bulunmaktadırlar. İvmeölçerlerde olduğu gibi bir, iki veya üç eksende ölçüm yapabilen modelleri vardır ve saniyedeki dönüş hızı ölçümüne göre değerlendirilmektedirler. Bu hızların üzerindeki dönüşler sonucu sensör çıkışları anlamsız olabilmektedir. Uygulamanızda kullanacağınız sensörü bu özellikler ve hassasiyetine bakarak alabilirsiniz. Şimdi IMU başlığında ivme ölçer ve gyroyu beraber kullanacağımızdan tekrar ivme ölçer konusunda kullandığımız koordinat sistemlerine dönelim ve sensörlerin bu modele göre nasıl kullanılabileceğine bakalım.
İki eksen (X,Y) gyro kullandığımızı düşünerek devam edelim. Buradaki R vektörünün XZ uzayındaki izdüşümü Rxz, YZ uzayındaki izdüşümü ise Ryz vektörü ile ifade edilmektedir.Bu vektörleri pisagor teoreminden
Rxz^2 = Rx^2 + Rz^2
Ryz^2 = Ry^2 + Rz^2 olarak hesaplayabiliriz.
Vektörlerin Z ekseni ile yapmış olduğu açılar ise Axz ve Ayz dir. Sistemi Y ekseni etrafında döndürdüğümüzde Axy açısı, X ekseni etrafında döndürdüğümüzde ise Ayz açısı değişecektir. Gyroscope un dönüş hızını ölçtüğünü söylemiştik. Dönüş hızını zaman ile çarparsak dönüş açısını elde etmiş oluruz. t0 anındaki açımızın Axz0 olduğunu ve t1 anındaki açımızın ise Axz1 olduğunu düşünelim. O halde dönüş açımız
(Axz1 – Axz0) = RateAxz * (t1 – t0) ile ifade edilir.
Sensörden alacağımız değerleri dönüş hızına çevirmek içinse aşağıdaki formülleri kullanabiliriz.
RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity
RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity
Yine analog sensör kullandığımızı, 3v3 ile çalıştığımızı ve 10bit adc ile örnekleme yaptığımızı düşünelim. Sensörlerin hareketsiz durumda sabit olarak vermiş olduğu bir gerilim vardır ve VzeroRate olarak isimlendirilir. Bu gerilimi binary değerden elde ettiğimiz gerilimden çıkartmamız gerekmektedir.Örneğimiz için 1.23V olduğunu düşünelim. Sensör hassasiyetimiz ise volt başına 0.002 deg/s olsun. Bu değerleri kullanacağınız sensörün datasheetinden elde edebilirsiniz. gyroX ten 571 gyroY den 323 binary değerini okuduğumuzu düşünelim. Tüm bu değerleri yerine koyduğumuzda dönüş hızlarını aşağıdaki gibi buluruz.
RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ 306 deg/s
RateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ -94 deg/s
Elde ettiğimiz değerler görüldüğü gibi açısal hızlardır. Bu değerleride iki örnekleme arasında geçen süre ile çarparsak dönüş açımızı elde ederiz. Örneğin 1ms de örnekleme yaptığımızı farz edersek X için dönüş açısı 0.306 derece, Y için dönüş açısı -0.094 derece olacaktır.
IMU (Inertial Measurement Unit)
Gyroscope ve accelerometer tek başlarına bize yeterince ve güvenli bilgi vermezler. Bu yüzden bu iki sensörü birleştirerek yönelim, hız, pozisyon gibi bilgileri tek bir uniteden alabiliriz. Bu uniteye IMU (Inertial Measurement Unit) denilmektedir. Serbestlik derecesi DOF (Degrees of Freedom) ile ifade edilmektedirler. Örneğin 2 eksen gyro ve 3 eksen ivmeölçeriniz varsa 5DOF IMU elde etmiş olursunuz.
Gyro ve accelerometer bias drift adı verilen bir kayma yaparlar ve bundan dolayı hassas açı ölçümünde tek başlarına kullanılamazlar. Ayrıca accelerometerlar kuvvete karşı çok duyarlı olduğundan en ufak titreşimlerde çok yüksek gürültüler oluşturmaktadırlar.Gyroların bu kuvvetlerden etkilenmediğini söylemiştik. Aşağıdaki şekilde görüldüğü gibi gyrolar ivmeölçer çıkışlarını filitreleyerek daha doğru bir ölçüm yapmamızı sağlarlar.
Filtreleme için çeşitli algoritmalar bulunmaktadır. En yaygın olarak kullanılanlarından birtanesi kalman filitresidir. Sistemin bir önceki çıkışları ile yeni ölçümlerinden yeni çıkışları tahmin edecek şekilde çalışmaktadır.Kalman filitresinin etkisini videodan izleyebilrisiniz.
Şimdi kalman filitresine benzeyen bir örnek ile devam edelim. Bu örneğin kalman filitresinden eksiği örneğin sonunda göreceğiniz ağırlıklı ortalamanın sabit olması, kalman filitresinde ise çıkışlara göre tekrar hesaplanıp dinamik olarak kullanılmasıdır.
Başlamak için accelerometer ve gyro koordinat sistemlerini birleştirmemiz gerekmektedir. Bunun için accelerometer koordinat sistemini referans olarak seçmeli ve sensörlerin XZ ile YZ düzlemlerini çakıştırmamız gerekmektedir. Bu işlemleri yaptıktan sonra ivmeölçer verilerini filitremiz için direk giriş olarak kullanacağız. Be verilerin aşağıdaki formül ile hesaplandığını hatırlayalım.
RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity
RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity
İvmelenme (hızlanma, yavaşlama) gibi durumlarda sensör üzerine etkiyen kuvvet 1g den büyük veya küçük olabilmektedir. İşlem yapabilmek için önce R vektörünü normalize etmemiz gerekmektedir. Bunun için aşağıdaki işlemleri kullanabiliriz. Bunlar R vektörünün herzaman 1g ye eşit olmasını sağlayacaktır.
|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2)
Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|]
Şimdi filtre çıkışından tahmin edilen Rest = [RxEst,RyEst,RzEst] vektörümüz olsun. Yapacağımız iş accelerometer çıkışlarını okuyup gyro çıkışları ile gerçekten bir dönüş hareketi yapıyormuyuz diye karşılaştırmaktır. Bunun için t0 anında Rest(0) = Racc(0) vektörlerini birbirine eşitlememiz gerekmektedir.Daha sonra T aralıkları ile düzenli örnekler alıp yeni örnek ile önceki çıkışları işleme sokmamız gerekmektedir.Hesaplamalara gyro ile başlayalım ve vektörünü Rgyro = [RxGyro,RyGyro,RzGyro] olarak ifade edelim.
Şekilde görülen Axz açısını tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz) ile hesaplarız. Burada atan2 fonksiyonu bize açı değerini –PI ile PI aralığında verecektir. RxEst(n-1) , ve RzEst(n-1) değerlerini bildiğimiz için bir önceki Axz açısı olan Axz(n-1) şöyle hesaplayabiliriz.
Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) )
Gyro Axz açısının değişim hızını ölçtüğünden yeni Axz açısını Axz(n) = Axz(n-1) + RateAxz(n) * T şeklinde hesaplayabiliriz. İfade kolaylığı açısından |Rgyro| = 1 x =RxGyro , y=RyGyro, z=RzGyro yazalım.
x = x / 1 = x / SQRT(x^2+y^2+z^2) eşitliğinde payı ve paydayı SQRT(x^2 + z^2)e bölelim. Yeni sonucumuz
x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) ) şeklinde olacaktır. x / SQRT(x^2 + z^2) = sin(Axz) olduğundan
x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) ) yazabiliriz. Şimdi kökün içindeki pay ve paydayı z^2 ile çarpalım.
x = sin(Axz) / SQRT (1 + y^2 * z ^2 / (z^2 * (x^2 + z^2)) ) elde ederiz. z / SQRT(x^2 + z^2) = cos(Axz) ve y / z = tan(Ayz) olduğundan
RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 )
RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 ) şeklinde elde ederiz. gyroZ ise
RzGyro = Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2) şeklindedir. Burada RzGyro>=0 olduğunda Sign(RzGyro) = 1 , ve RzGyro<0 olduğunda Sign(RzGyro) = -1 olur. Ayrıca Rz Axz ve Ayz açılarını hesaplamada kullanıldığından 0 a yaklaştığında istenmeyen sonuçlar elde edilebilir. Bu durumda bir önceki çıkışları yeni gyro ölçümü olarak alabiliriz. Şimdi elimizde Racc ve Rest(n-1) vektöründen elde ettiğimiz Rgyro vektörleri olduğuna göre Rest(n) vektörünü hesaplayalım. Bunun için ağırlıklı ortalama alacağız.
Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2)
Formülde pay ve paydayı w1 ile bölüp w2/w1 = wGyro yazarsak
Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro) denklemini elde ederiz. Burada wGyro accelerometera oranla gyroya ne kadar güvenebileceğimizi belirtmektedir. Starlino 5-20 arası değerlerin deneysel olarak iyi sonuç verdiğini yazmış. Rest vektörünüde normalize ederek açı hesaplamasında kullanabiliriz.
R = SQRT(RxEst(n) ^2 + RyEst(n)^2 + RzEst(n)^2 )
RxEst(n) = RxEst(n)/R
RyEst(n) = RyEst(n)/R
RzEst(n) = RzEst(n)/R
Yukarıdaki algoritmada wGyro değeri sabit bir değerdir fakat Kalman filtresinde bu değer accelerometer gürültüsü analiz edilerek yeniden hesaplanmaktadır. Kalman filitresi sistemin sürekli değişen girişlerini izleyerek bir sonraki çıkışın en iyi değerini tahmin etmektedir.Görüntü işlemeden, yönelim, hareket takibi gibi bir çok alanda kullanılmaktadır.
Bu kadar anlatımdan sonra gerçek bir örnek yapalım. Örnekte lpcxpresso boardumu kullandım. lpcxpressoyu modelledim ve wpf ile bir uygulama yazdım. 6DOF IMU sayesinde boardun hareketlerini uygulama üzerinde canlı olarak görebilirsiniz.
Yukarıdaki örnekte kullanmış olduğum filtre ile hareketsiz iken max hata 0.3 dereceyi geçmedi. Ortalama hata olaraksa Kalman filitresinden daha doğru çalışmakta