تَفْهيمُ حساس الـ IMU: أساسيات حساسي التسارع والجايروسكوب وكيفية استخلاص زوايا الإمالة والدحرجة منهما

إن حساس الـIMU المتضمن لحساسي التسارع و الجايروسكوب ويسمى  بذلك IMU بستة محاور Six Axises IMU حيث يحوي كل حساس على 3 محاور تحسّس أو تسمى أحياناً وحدة IMU بـ 6 درجات حريّة Six DoF ويحوي الـIMU أحياناً حساس مغناطيسي بالإضافة إلى حساسي التسارع والجايروسكوب وعندها يسمى  بذلك IMU بتسع محاور Nine Axises IMU حيث يحوي كل حساس على 3 محاور تحسس  أو تسمة أحياناً وحدة IMU بـ 9 درجات حريّة Nine DoF.  

إن حساس الـIMU من أهم الحساسات في الأجهزة الإلكترونية وهو حساس متوفر ومنخفض السعر ويعطي معلومات مهمة جداً تسمح ببناء تطبيقات عديدة، ولفهم الحساسين لابد من فهم عام لبنيتهما الداخلية وهذا الفهم يساعد على معرفة ميزات كل حساس وميزاته وعيوبه وأيضاً يساعد على فهم شكل خرج الحساس والتطبيقات التي يمكن استخدامه بها، ولمّا لم أجد مرجعاً متكاملاً وبلغة مبسّطة وعمليّة تساعد على فهم هذا الحساس آثرت بدأ هذه السلسلة التي أسعى ان تبدأ من الأساسيات وتنتهي بتطبيقات وخوارزميات مبنية بحساس الـIMU.  

سلسلة حساس الـIMU:

• تَفْهيمُ حساس الـ IMU: أساسيات حساسي التسارع والجايروسكوب وكيفية استخلاص زوايا الإمالة والدحرجة منهما.
• معايرة الحساس المغناطيسي من مسببات تشوه الحقل: لماذا وكيف.
• تفهيم حساس IMU: نُظم تمثيل اتجاهية الجسم وربط قيم الحساس مع مكتبة VPython ثلاثية الأبعاد.

مشروع جهاز تحديد القبلة مع تعويض الإمالة. مشروع يستخدم المفاهيم الموجودة في أجزاء السلسلة الثلاث.

يعد حساسي التسارع Accelerometer  وحساس (المِدوار) الجايروسكوب Gyroscope من أهم الحساسات استخداماً وأكثرها شيوعاً في الدارات الإلكترونية. إن استخدامات الحساسين متنوعة ومنها: التعرف على إشارات اليد Gesture Recognition وأيضاً اكتشاف الحاجة للصيانة المبكرة واكتشاف الأعطال للمحركات والأجهزة الكهربائية Anomaly Detection for Predictive Maintenance ومثال ذلك وضع الحساس على هيكل المحرك ومقارنة نمط الاهتزاز الحالي مع نمط الاهتزاز بالحالة الطبيعية ومن ثم معالجة البيانات لاكتشاف أي خلل في الأداء و من الاستخدامات أيضاً تتبع الخطوات step counter سواءً لتطبيقات الرياضة أو تحديد الموضع وخاصة داخل المباني Indoor Positioning System حيث تغيب تغطية أقمار نظام تحديد المواقع الجي بي إس GPS. ولتنوع المعلومات المهمّة التي يمكن لهذه الحساسات تقديمها، فقد استُخدم في كرة قدم كأس العالم في قطر عام 2022 من شركة أديداس.  

يجب فهم المبدأ الأساسي للحساس لفهم شكل إشارة الخرج في الوضعيات المختلفة له، وإن فهم ذلك يُيسّر كثيراً التعامل مع الحساس بأريحية. لن نتطرق للبنية الداخلية للحساسات تفصيلياً ولكن يكفينا معرفة المبدأ الرئيسي اللازم معرفته لفهم شكل إشارات الخرج. تسمى الشريحة الإلكترونية التي تحوي على حساسي التسارع والجايروسكوب بـ IMU واختصاراً inertial measurement unit وتستخدم في بنيتها نظم كهروميكانيكية مصغّرة Microelectromechanical systems والتي تعرف اختصاراً MEMS. 

طريقة عمل حساس التسارع

إن بنية حساس التسارع تعتمد على تثبيت كتلة بنوابض (عملياً هي ليست نوابض ولكن يمكن تشبيهها بها) وهذه الكتلة لا تتحرك إلا في محور واحد تستجيب للحركة فيه وللحساس 3 محاور.

إن الكتلة المتحركة داخل الحساس مسؤولة عن تغير قيم المكثفات المتشكلة بين الطرف الثابت (اللبوس Electrode) والطرف المتحرك منه ونعلم من قوانين الكهرباء الأساسية أن قيمة المكثفة بالفاراد تتعلق طرداً بالسطح وعكساً مع المسافة بين السطحين 

C = (ε × A)/D (Farad)

حيث A هي مساحة السطح وD هي المسافة بين سطحي المكثفة (اللبوسين) وبما أن القوة المطبقة على المكثف المتغير بموضع الكتلة المتحركة داخل الحساس تتناسب مع تغير المسافة بسبب التسارع، فإن حساب قيمة المكثفة الناتجة باستخدام مبدل تماثلي-رقمي ADC يمكِنُنَا من حساب القوة المطبقة التي أدت لهذا التغيير، وبحسب القانون الثاني (قانون نيوتن الثاني)، فإن شعاع القوة يساوي الكتلة مضروبة بالتسارع ومن هذا القانون نستنتج قيمة التسارع. وبما أن قيمة المكثقة يمكن قياسها بالمبدل ومنها تٌحتسب المسافة D والتب يمكن ربطها بقانون نيوتن الثاني لحساب القوة المسببة للتسارع. إن الشرح السابق هو توضيح للمبدأ الفيزيائي الذي يفسر علاقة المكثف بحساب التسارع ،ولكن عملياً لا نحتاج لكل هذا لأن الداتاشيت توفر رقم يربط القيمة الرقمية في الخرج مع قيمة التسارع أو ما يسمى sensitivity.

يتم التحسس لتغير قيم المكثفات عبر محول تماثلي-رقمي ADC ولزيادة الحساسية وتشكيل مكثفة ذات سعة أكبر، فإنه يتم تصميم البنية الداخلية للكتلة المتحركة والإطار الثابت بحيث تُوْصل أكثر من مكثفة متغيرة السعة على التفرع لكل محور.

ولكن كيف يتم ربط القيمة الرقمية المستردة من الحساس بالقيمة الفيزيائية للتسارع؟ إنّ المبدل الرقمي ADC يعطي قيمة رقمية معبّرة عن الجهد على طرفي المكثفة، وباستخدام قيمة مرجعية وهي قيمة التسارع الصفرية (السكون)، وبإيجاد الفرق بين القيمة المرجعية وقيمة الجهد الحالي نحسب الانزياح عن قيمة الصفر وباستخدام قيمة الحساسية من الداتاشيت وهي قيمة الجهد لأجل كل واحد g (وحدة الجاذبية الأرضية) نحصل على قيمة التسارع.

مثال (مقتبس من  الورقة التطبيقية  APPLICATION NOTE 5830 من شركة maxim integrated): 

بفرض استخدام المبدل الرقمي ADC بدقة 10 بت وبفرض أن القراءة منه هي 600 وأن جهد التغذية 3.3 فولت، فإن قيمة الجهد المقروء هي 

_{_} 600 x 3.3 / 1023 = 1.94V

وبالعلم أن قيمة الجهد في حالة السكون (الصغر) هي 1.65V فإن الانزياح يساوي 

1.94V – 1.65V = 0.29V

ومن المُحدد في الداتاشيت (مثلاً) أن 0.475V/g ويعني أن كل 1g تقابل 0.475V وبالتالي 0.29V تقابل 

0.29V/0.475 V/g = 0.6g

 عملياً توفر الداتاشيت للحساس الرقمي قيمة الحساسية التي تربط بين قيمة كل وحدة LSB وهي قيمة البت الواحد من القراءة الرقيمة ADC مع قيمة التسارع

في الجدول أدناه قيمة الحساسية مأخوذ من الداتاشيت للحساس BMX160. 

إن الشكل التقريبي لبنية الحساس الميكانيكية الداخلية مُعبَّر عنها في الشكل أدناه 

حساس التسارع يعطي قيمة غير صفرية عند السكون، لماذا ؟

يظهر حساس التسارع في المحور العامودي Z قيمة غير صفرية بالرغم من سكون الحساس وذلك بتأثير الجاذبية الأرضية ويظهر قيمة مساوية للجاذبية الأرضية وينطبق الأمر ذاته لأي محور عندما يتوازى مع شعاع الجاذبيّة الأرضية. وعند إلقاء الحساس بالهواء أو ما يسمى السقوط الحر Free Fall سيساوي قيمة التسارع في هذه الحالة قيمة الصغر وذلك بسبب انضغاط النابض بقيمة معاكسة للجاذبية الأرضية وبالتالي تعود الكتلة المتحركة داخل الحساس إلى الوضع الصفري. إن فهم هذه الظاهرة في حساس التسارع من غير ممكن من غير فهم البنية الأساسية للحساس التي استعرضناها في الفقرة السابفة. 

إن حساس التسارع يتحسس لقيمة التسارع المتولدة من الجاذبية الأرضية وأي قوى خارجية بوحدة م/ث² توثر على الحساس وإن قراءة الحساس تتألف من القيمة الحقيقة مضاف لها ضجيج القياس الذي يمكن اعتباره بتوزع غاوصي طبيعي.

طريقة عمل حساس الجايروسكوب

 إن ما تم عرضه حتى الآن هو لحساس التسارع وأما بالنسبة لحسّاس الجايروسكوب، فيتشارك حساس التسارع وحساس الجايروسكوب المبدأ العام من استخدام كتلة متحركة تغير قيمة سعة المكثفات الداخلية ولكن لكل منهما تصميم ميكانيكي مختلف. من الاختلافات بين الجايروسكوب وحساس التسارع هو تحرك كتلة الجايروسكوب التي تغير قيم المكثفات بشكل مستمر (وضعية الاهتزاز resonating) وعند وجود مؤثر خارجي يسبب الدوران فإن الكتلة المهتزة تتحرك وفق ظاهرة كوريلوس Coriolis Effect.

إن الحاجة لجعل الكتلة تهتز داخل الجايروسكوب بشكل دائم يجعل استهلاك الطاقة في حساس الجايرو أكثر من حساس التسارع وهذا ما يمكن ملاحظته من الداتاشيت.

إن الجايروسكوب يتحسس للسرعة الزاوية بوحدة درجة/ثانية ويضاف له انزياح drift يمكن اعتباره بقيمة ثابتة وهذا ما يمكن التخلص منه خلال معايرة الحساس و يضاف كذلك ضجيج القياس الذي يمكن اعتباره بتوزع غاوصي طبيعي.

حساب زاوية الميلان وزاوية التدحرج عبر حساس التسارع

إن زوايا الميلان Pitch  والتدحرج Roll هي من أهم المعلومات التي يتم استنباطها من قراءة الحساس. إن زاوية الميلان هي الزاوية التي تتشكل بين سطح الحساس وسطح الأرض عند تحريك مقدمة السطح نحو الأعلى والأسفل وهي تشابه وضعية الطائرة في الإقلاع والهبوط وبينما زاوية التدحرج فهي الزوية بين سطح الحساس وسطح الأرض عند تحريك طرفي السطح نحو اليمين واليسار وهي تشابه وضعية الطائرة في السماء عند إمالة الطائرة بأجنحتها يميناً ويساراً.

إن قوة الجاذبية الأرضية في حالة السكون على سطح الأرض متعامدة على محور x و محور y وتؤثر على محور z فقط ولهذا تكون قيم التسارع صفرية على المحورين. وعند إمالة مقدمة سطح دارة الحساس كما هو موضح في الشكل أدناه تتشكل زاوية الإمالة ثيتا Θ. عند الإمالة يتشكل مركبتين للجاذبية الأرضية على محور x ومحور z بسبب عدم التعامد معهما كما كان في حالة السكون على سطح الأرض. 

إن زاوية الميلانΘ بين سطح الدارة وسطح الأرض -كما هو موضّح في الشكل أدناه- هي ذاتها الزاوية بين شعاع الجاذبية وشعاع المركبة على محور z وذلك بالاعتماد على معرفة أن المثلث المتشكل بين مركبة x  ومركبة z وشعاع الجاذبية هو مثلث بزاوية قائمة 90 درجة وزاوية الميلان مع تتمة الزاوية المتشكلة بين مركبة x وشعاع الجاذبية هي 90 درجة لأن الجاذبية متعامدة مع السطح ونعلم أيضاً أن مجموع زوايا المثلث هي 180 درجة. 

ومن قوانين المثلثات نجد قيمة الزاوية  Θ باستخدام مركبتي التسارع على المحور x والمحورz

وبالمثل نحسب زاوية التدحرج باستخدام مركبتي التسارع على المحور y والمحور z

في الكود أدناه المبني على الحساس MPU-6050  باستخدام دارة GY-512 يتم حساب زوايا الإمالة theta والدحرجة phi عبر القوانين المستنبطة السابقة

يوجد في المعادلات في الكود تحويل من الراديان إلى الدرجة، فالتابع atan2 يعيد الزاوية بوحدة الراديان. وأيضاً تحويل التسارع لواحدة الجاذبية الأرضية g حيث أن 9.8 متر2/ثانية هي المكافئ لـ 1 g.

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <math.h>
Adafruit_MPU6050 mpu;

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
	delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
	Serial.println("Failed to find MPU6050 chip");
	while (1) {
  	delay(10);
	}
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
	Serial.println("+-2G");
	break;
  case MPU6050_RANGE_4_G:
	Serial.println("+-4G");
	break;
  case MPU6050_RANGE_8_G:
	Serial.println("+-8G");
	break;
  case MPU6050_RANGE_16_G:
	Serial.println("+-16G");
	break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
	Serial.println("+- 250 deg/s");
	break;
  case MPU6050_RANGE_500_DEG:
	Serial.println("+- 500 deg/s");
	break;
  case MPU6050_RANGE_1000_DEG:
	Serial.println("+- 1000 deg/s");
	break;
  case MPU6050_RANGE_2000_DEG:
	Serial.println("+- 2000 deg/s");
	break;
  }

  Serial.println("");
  delay(100);
}

float theta;
float phi;

void loop() {

  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);


theta=atan2(a.acceleration.x/9.8,a.acceleration.z/9.8)/2/3.141592654*360;
phi=atan2(a.acceleration.y/9.8,a.acceleration.z/9.8)/2/3.141592654*360;


Serial.print(":theta:");
Serial.print(theta);
Serial.print(",");
Serial.print("phi:");
Serial.println(phi);

delay(50);
}

محدودية حساب الزوايا عبر حساس التسارع وكيفية تحسين ذلك باستخدام مرشح

يعاني حساب زوايا الإمالة والتدحرج عبر حساس التسارع من الاهتزاز ويبدو هذا واضحاً عند تحريك الحساس على محور واحد من غير أي تغيير للزاوية. إن هذا الاهتزاز هو أحد الحالات الطبيعية لتركيب الحساس في مركبة تسير على طريق وعر أو حتى اهتزازات اليد وبالتالي هذا يجعل من القانون السابق لحساب الزوايا محدود مناسب في الحالات المثالية والتطبيقات البسيطة فقط.

يمكن حل المشكلة المشار لها عبر تطبيق فلتر بسيط من النوع Low Pass Filter من الدرجة الأولى

نعدل على الكود السابق ونضيف له كود المرشّح (الفلتر) :

theta_new = 0.9*theta_old + 0.1* theta;
phi_new = 0.9*phi_old + 0.1* phi;
//Serial.print(a.timestamp);

Serial.print("theta_raw:");
Serial.print(theta);
Serial.print(",");
Serial.print("phi_raw:");
Serial.print(phi);
Serial.print(",");
Serial.print("theta_filter:");
Serial.print(theta_new);
Serial.print(",");
Serial.print("phi_filter:");
Serial.println(phi_new);

theta_old = theta_new;
phi_old = phi_new;
delay(50);

إن القيم 0.9 و 0.1 هي القيم التي تعطي تثقيل للقيم السابقة والجديدة للقراءات وتغييرها يؤدي إلى تغيير سرعة استجابة النظام للتغير في الزاوية وأيضاً الحساسية للضجيج، لذلك يمكن معايرتهم حسب الرغبة. يجب أن يكون مجموع الرقمين مساوي لقيمة 1. 

حساب زاوية الميلان وزاوية التدحرج عبر حساس الجايروسكوب

إن حساب الزوايا ممكن باستخدام القراءة من حساس الجايروسكوب الذي يزودنا بالسرعة الزاوية والذي يعبّر عن تغير الزاوية خلال الزمن، وبالتالي يمكن إيجاد الزاوية من السرعة الزاوية من خلال ضربها بتغير الزمن (الزمن بين كل قراءة والتي تليها). تعتمد قيمة الزاوية على القيمة الابتدائية للزاوية والتي يمكن اعتبارها صغر عند البداية.

يمكن حساب تغير الزمن عبر استخدام مؤقت وتسجيل قيمة المؤقت عند كل قراءة وأخذ فرق القيمة بين كل قيمتين متلاحقتين. نلاحظ أن إيجاد قيمة الزاوية عبر حساس الجايروسكوب أقل تأثراً بالاهتزازت من حساس التسارع.

// Acc
float theta_acc;
float phi_acc;

//Gyro
float theta_gyro;
float phi_gyro;

float theta_gyro_old = 0.0;
float phi_gyro_old = 0.0;

float theta_gyro_new;
float phi_gyro_new;


int32_t last_timestamp = 0;
float delta_time = 0;

void loop() {

  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

theta_acc=atan2(a.acceleration.x/9.8,a.acceleration.z/9.8)/2/3.141592654*360;
phi_acc=atan2(a.acceleration.y/9.8,a.acceleration.z/9.8)/2/3.141592654*360;

delta_time =  (millis()-last_timestamp)/1000.;
last_timestamp  = millis() ;

theta_gyro = theta_gyro + g.gyro.y * delta_time * 360 / (2*3.141592654) ; // rad/sec
phi_gyro = phi_gyro + g.gyro.x * delta_time * 360 / (2*3.141592654) ; // rad/sec


Serial.print("theta_acc:");
Serial.print(theta_acc);
Serial.print(",");
Serial.print("theta_gyro:");
Serial.println(-1* theta_gyro );

theta_acc_old = theta_acc_new;
phi_acc_old = phi_acc_new;
delay(100);
}

إن طريقة كتابة الكود ليست مُثلى، فلضمان الدقة يجب استخدام المقاطعة من الـIMU وليس استخدام millis حيث لا يعبر بشكل دقيق تماماً عن وقت القراءة كما أن الحساسات غالباً ما توفّر عداد زمني يعبر عن اللحظة االزمنية التي تم التقاط القراءة فيها ويسمى timestamp.
يجب الانتباه لوحدات قياس الزوايا وعدم الخلط بين وحدة الراديان rad ووحدة الدرجات

إن سبب ثبات قيمة الزاوية المحسوبة من الجايروسكوب أمام الاهتزازات (الضجيج) [1] مقارنة بالقيمة المحسوبة من حساس التسارع هو بسبب مكاملة الإشارة وهذا ما قمنا به عبر مراكمة قيمة الزاوية وضرب قيمة السرعة الزاوية بالقيمة الزمنية.

نلاحظ من المعادلة السابقة وبفرض أن الضجيج له إشارة جيبية وبتردد f فإن تكامل الإشارة يعطي إشارة جيبية بالخرج مخمدة بعامل \frac{1}{2 \pi f} ولذلك فإن رفع تردد التحصيل يقلل من تأثير الضجيح ويزيد من جودة الزاوية المحسوبة، وبالمثل استخدام تردد منخفض يزيد من مطال الضجيج وتأثيره. 

محدودية حساب الزوايا عبر حساس الجايروسكوب

حساب الزوايا باستخدام الجايروسكوب ليس الحل الأمثل، إذ يجتاج الجايروسكوب للمعايرة قبل استخدامه. حيث أن حساب الزاوية يعتمد على القيمة الابتدائية وقيمة القراءة مضروبة بفرق الزمن بين القرأتين، ولذلك نفترض أن قيمة القراءة عند السكون صفر بسبب عدم وجود حركة ولكن وبسبب الانزياح drift فهذا يؤدي إلى تراكم مستمر على قيمة الزاوية. إن مهمة المعايرة التقليل من قيمة الانزياح نحو الصفر. 

إن حساب الزوايا باستخدام الجايرو بالطريقة السابقة فيه خطأ يسمى خطأ التقريب approximation error وذلك لاستخدامه متسلسلات تايلور  بالدرجة الأولى first-order Taylor series بالشكل التالي [1] 

إن هذا الخطأ يسبب انزياحاً drift مع الوقت ويمكن تقليل تأثير ذلك باستخدام تردد أعلى للقراءة ، ولهذا تعتبر هذه الطريقة مناسبة الاستخدام على مدى زمني محدود. 

معايرة حساس الجايروسكوب

مما وجدنا سابقاً أن الجايروسكوب بحاجة لمعايرة تجعل الانزياح drfit أقرب ما يمكن للصفر، أحد طرق المعايرة هي أخذ عدد من العينات في حالة السكون والدارة على سطح مستوي وحساب أعظم قيمة وأصغر قيمة لكل محور ومن ثم حساب المتوسط. تكون القيمة الناتجة هي قيمة المعايرة بطرحها من قيمة القراءة قبل المعايرة. توفر شركة Adafruit سكربت بايثون لمعايرة الحساس الجايروسكوب والحساس المعناطيسي ولتسهيل معايرة الجايروسكوب قمت بتعديل السكربت ليعدل الجايروسكوب فقط. يقرأ السكربت من مأخذ الاتصال في كل سطر قيمة المحاور الثلاث مفصول بين كل محور بفاصلة.  

# A modified version of: https://learn.adafruit.com/adafruit-sensorlab-gyroscope-calibration?view=all
#  By Yahya Tawil (Atadiat.com)
import time
%matplotlib notebook
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import datetime
import matplotlib.dates as mdates
from collections import deque
import numpy as np

import serial
import re

PORT = "/dev/ttyACM0"

# How many sensor samples we want to store
HISTORY_SIZE = 2500

# Pause re-sampling the sensor and drawing for INTERVAL seconds
INTERVAL = 0.01

serialport = None

def get_imu_data():
	global serialport
	if not serialport:
    	# open serial port
    	serialport = serial.Serial(PORT, 115200, timeout=0.1)
    	# check which port was really used
    	print("Opened", serialport.name)
    	# Flush input
    	time.sleep(3)
    	serialport.readline()

	# Poll the serial port
	line = str(serialport.readline(), 'utf-8')
	if not line:
    	return None
	vals = line.strip().split(',')
	#print(vals)
	if len(vals) != 3:
    	return None
	try:
    	vals = [float(i) for i in vals]
	except ValueError:
    	return None
	#print(vals)
	return vals

print("Put down the board and do not touch or move it!")
for s in range(3, 0, -1):
	print(s, end='...')
	time.sleep(1)
print("COLLECTING GYRO DATA")

# close port in case its open
if serialport:
	try:
    	serialport.close()
	except NameError:
    	pass

serialport = None

# Deque for axes
gyro_x = deque(maxlen=HISTORY_SIZE//10)
gyro_y = deque(maxlen=HISTORY_SIZE//10)
gyro_z = deque(maxlen=HISTORY_SIZE//10)
while len(gyro_x) < (HISTORY_SIZE//10):
	ret = get_imu_data()
	#print(ret)
	if not ret:
    	continue
	x, y, z = ret[0:3]
	gyro_x.append(x)
	gyro_y.append(y)
	gyro_z.append(z)

for _ in range(3):
	gyro_x.popleft()
	gyro_y.popleft()
	gyro_z.popleft()

min_x = min(gyro_x)
max_x = max(gyro_x)
min_y = min(gyro_y)
max_y = max(gyro_y)
min_z = min(gyro_z)
max_z = max(gyro_z)

print("Gyro X range: ", min_x, max_x)
print("Gyro Y range: ", min_y, max_y)
print("Gyro Z range: ", min_z, max_z)

gyro_calibration = [ (max_x + min_x) / 2, (max_y + min_y) / 2, (max_z + min_z) / 2]
print("Final calibration in deg/s:", gyro_calibration)

serialport.close()

سيطبع السكربت في الخرج مصفوفة المعايرة الواجب استخدمها. 

نلاحظ بعد المعايرة ثبات الزاوية المحسوبة عبر الجايروسكوب وذلك بسبب اقتراب قيمة السرعة الزاوية للصفر بعد المعايرة بينما لم تكن كذلك في الفقرة الماضية وذلك قبل المعايرة. 

نعدل على كود الأردوينو السابق ليستخدم قيم الجايروسكوب بعد المعايرة كالتالي: 

cal_gyro_x = g.gyro.x* 360 / (2*3.141592654) - 7.005 ;
cal_gyro_y = g.gyro.y* 360 / (2*3.141592654) + 0.84 ;
cal_gyro_z = g.gyro.z* 360 / (2*3.141592654) - 0.815 ;

theta_gyro = theta_gyro + cal_gyro_y * delta_time  ; // deg/sec
phi_gyro = phi_gyro + cal_gyro_x * delta_time  ; // deg/sec

أيهما أفضل حساب الزوايا باستخدام حساس التسارع أو حساس الجايروسكوب

إن قيمة الزاوية المحسوبة بحساس التسارع لا تعاني من مشكلة الانزياح drift ولكنها حساسة أكثر للضجيج، ينما قيمة الزاوية المحسوبة بحساس الجايروسكوب أكثر ثباتاً أمام الضجيج ولكنها تتأثر بمشكلة الانزياح. أحد الحلول الممكنة هي استخدام مرشح نوع Complementary Filtering 

تتم معايرة المرشح عبر قيمة ألفا \alpha التي تعطي وزن لقيمة الزاوية المحسوبة من حساس التسارع أو الجايروسكوب. 

في ما يلي الكود المستخدم لإنتاج الشكل أعلاه 

// Acc
float theta_acc;
float phi_acc;

float theta_acc_old = 0.0;
float phi_acc_old = 0.0;

float theta_acc_new;
float phi_acc_new;

//Gyro
float theta_gyro;
float phi_gyro;

float theta_gyro_old = 0.0;
float phi_gyro_old = 0.0;

float theta_gyro_new;
float phi_gyro_new;

float cal_gyro_x;
float cal_gyro_y;
float cal_gyro_z;

int32_t last_timestamp = 0;
float delta_time = 0;

float complementary_theta_old = 0.0 ;
float complementary_theta_new ;

float complementary_phi_old = 0.0 ;
float complementary_phi_new ;

void loop() {

  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

//  /* Print out the values */
//  Serial.print("Acceleration X: ");
//  Serial.print(a.acceleration.x);
//  Serial.print(", Y: ");
//  Serial.print(a.acceleration.y);
//  Serial.print(", Z: ");
//  Serial.print(a.acceleration.z);
//  Serial.println(" m/s^2");
//
//  Serial.print("gyro_x:");
//  Serial.print(g.gyro.x* 360 / (2*3.141592654));
//  Serial.print(",");
//  Serial.print("gyro_y:");
//  Serial.print(g.gyro.y* 360 / (2*3.141592654));
//  Serial.print(",");
//  Serial.print("gyro_z:");
//  Serial.print(g.gyro.z* 360 / (2*3.141592654));
//  Serial.print(",");
// calibration [7.005, -0.84, 0.815]

cal_gyro_x = g.gyro.x* 360 / (2*3.141592654)- 7.005 ;
cal_gyro_y = g.gyro.y* 360 / (2*3.141592654) + 0.84 ;
cal_gyro_z = g.gyro.z* 360 / (2*3.141592654) - 0.815 ;

//  Serial.print("cal_gyro_x:");
//  Serial.print(cal_gyro_x);
//  Serial.print(",");
//  Serial.print("cal_gyro_y:");
//  Serial.print(cal_gyro_y);
//  Serial.print(",");
//  Serial.print("cal_gyro_z:");
//  Serial.println(cal_gyro_z);

//Serial.print("Ax:");
//Serial.print(a.acceleration.x);
//Serial.print(",");
//Serial.print("Ay:");
//Serial.print(a.acceleration.y);
//Serial.print(",");
//Serial.print("Az:");
//Serial.println(a.acceleration.z);

theta_acc=atan2(a.acceleration.x/9.8,a.acceleration.z/9.8)/2/3.141592654*360;
phi_acc=atan2(a.acceleration.y/9.8,a.acceleration.z/9.8)/2/3.141592654*360;

theta_acc_new = 0.9*theta_acc_old + 0.1* theta_acc;
phi_acc_new = 0.9*phi_acc_old + 0.1* phi_acc;
//Serial.print(a.timestamp);



delta_time =  (millis()-last_timestamp)/1000.;
last_timestamp  = millis() ;

//Serial.print("delta_time:");
//Serial.print(delta_time);
//Serial.print(",");

theta_gyro = theta_gyro + cal_gyro_y * delta_time  ; // deg/sec
phi_gyro = phi_gyro + cal_gyro_x * delta_time  ; // deg/sec

complementary_theta_new = 0.5 * (complementary_theta_old + cal_gyro_y * delta_time) + 0.5 * theta_acc_new ;
complementary_phi_new = 0.5 * (complementary_phi_old + cal_gyro_x * delta_time) + 0.5 * phi_acc_new ;

Serial.print("theta_acc:");
Serial.print(theta_acc);
Serial.print(",");
//Serial.print("phi_raw:");
//Serial.print(phi_acc);
//Serial.print(",");
Serial.print("theta_acc_filter:");
Serial.print(theta_acc_new);
Serial.print(",");
//Serial.print("phi_acc_filter:");
//Serial.println(phi_acc_new);

Serial.print("theta_gyro_calibrated:");
Serial.print(-1* theta_gyro );
Serial.print(",");
//Serial.print("phi_gyro:");
//Serial.println(phi_gyro);

Serial.print("complementary_theta_new:");
Serial.println(complementary_theta_new );


theta_acc_old = theta_acc_new;
phi_acc_old = phi_acc_new;

complementary_theta_old = complementary_theta_new;
complementary_phi_old = complementary_phi_new;
delay(10);
}

زاوية الانعراج Yaw 

ما ذُكر في الأقسام السابقة كان عن زاوية الإمالة pitch وزاوية الدحرجة roll ولكن لم نذكر زاوية الميلان (الانعراج) yaw. إن زاوية الميلان لا يمكن احتسابها من حساس التسارع إذا تعتمد على إدارة الجسم حول المحور Z والقيام بهذا الدوران لن يغير أي قيمة في خرج حساس التسارع وذلك لتعامد شعاع الجاذبية مع المحورين X و Y وتوازيه مع المحور Z، ولهذا يجب استخدام الحساس المعناطيسي الذي نحسب به الانعراج بالاعتماد على الحقل المغناطيسي الأرضي أو نستطيع استخدام حساس الجايروسكوب. نستخدم في هذه المرة حساس الجايروسكوب لكوننا قمنا بإضاح كيفية استخدام الحساس المغناطيسي لهذا الغرض في مقالة سابقة ولكون حسابها لا يمكن دون معايرة دقيقة للحساس المغناطيسي كما أوضحنا في المقالة المشار لها. 

نعدل على كود الأردوينو السابق ليستخدم قيم الجايروسكوب بعد المعايرة كالتالي: 

cal_gyro_x = g.gyro.x* 360 / (2*3.141592654) - 7.005 ;
cal_gyro_y = g.gyro.y* 360 / (2*3.141592654) + 0.84 ;
cal_gyro_z = g.gyro.z* 360 / (2*3.141592654) - 0.815 ;

theta_gyro = theta_gyro + cal_gyro_y * delta_time  ; // deg/sec
phi_gyro = phi_gyro + cal_gyro_x * delta_time  ; // deg/sec
yaw_gyro = yaw_gyro + cal_gyro_z * delta_time  ; // deg/sec

المراجع

[1] محاضرة من Google Tech Talks 2010 بعنوان Sensor Fusion on Android Devices: A Revolution in Motion Processing. هذه المحاضرة مهمة لما فيها من استعراض لمفاهيم ومصطلحات لحساس التسارع والجايروسكوب عبر تطبيق عملي. [2] صفحة ويكيبيديا Vibrating structure gyroscope. [ 3] مقال من شركة أنالوغ  MEMS Gyroscope Provides Precision Inertial Sensing in Harsh, High Temperature Environments [4] المحاضرة العاشرة (سلايدات و ملاحظات) من كورس جامعة ستاندفورد  EE267 Virtual Reality course.  [5] سلسلة 9-Axis Inertial Measurement Unit (IMU) series من قناة Paul McWhorter وهذه السلسلة من أفضل السلاسل المتكاملة والعملية عن الـIMU وهي باستخدام الحساس BNO055 من شركة BOSCH [6] الورقة التطبيقية رقم 5830 من شركة Maxim Integrated بعنوان  Accelerometer and gyroscopes sensors: operation, sensing, and applications