معرفی و راه‌اندازی انواع IMU با آردوینو

این مطلب بخش سوم از مجموعه آموزش جامع اینترنت اشیا با آردوینو است. در این آموزش قصد داریم تا مهمترین ابزار موقعیت‌یابی محلی، یعنی IMU را معرفی کنیم. IMU ها ماژول‌های الکترونیکی کوچکی هستند که یک یا چند تراشه برای اندازه‌گیری پارامترهایی مثل شتاب، سرعت زاویه‌ای و غیره در خود دارند. این پارامترها کمک می‌کنند تا موقعیت سنسور تعیین شود. تعیین موقعیت و پارامترهای حرکتی در سیستم‌هایی مثل ربات یا پرنده‌های بدون سرنشین برای رسیدن به یک موقعیت خاص یا حفظ تعادل و پایداری بسیار اهمیت دارد.

قطعات مورد نیاز

معرفی IMU

معمولا IMU یا Inertial measurement unit را با شتاب‌سنج می‌شناسند اما باید بدانید که در حالت کلی IMU مجموعه‌ای از سنسورهای مختلف است که برای اندازه‌گیری موقعیت استفاده می‌شوند. سنسورهای موجود در یک IMU معمولا شامل شتاب‌شنج و ژیروسکوپ در جهت‌های مختلف می‌شود هرچند که یک IMU کامل‌تر ممکن است قطب‌نما یا حتی سنسورهای دما و فشار هم داشته باشد.

انواع مختلفی از ماژول‌های IMU را می‌توانید در بازار پیدا کنید. تعداد پارامترهایی که IMU اندازه می‌گیرد را اصطلاحا درجه آزادی آن ماژول می‌گویند؛ هرچند که این اصطلاح از نظر علمی خیلی درست نیست. در واقع فقط خروجی‌های شتاب‌سنج و ژیروسکوپ به تعریف علمی درجات آزادی ربط دارند. با این وجود تسامحاً از همین واژه استفاده می‌کنیم. ماژول‌های IMU زیادی در بازار وجود دارند برای مثال ماژول GY-86 یک ماژول ١٠ درجه آزادی است که می‌تواند شتاب و سرعت زاویه‌ای در سه جهت، فشار بارومتریک و دما را اندازه بگیرد. این ماژول مجهز به قطب نما نیز هست. این پارامترها توسط سه سنسور مختلف اندازه‌گیری می‌شود که در ادامه هر کدام را توضیح می‌دهیم. اگر یک ماژول IMU در اختیار دارید، ممکن است یک یا چند تا از این سنسورها را داشته باشد. تراشه‌های دیگری نیز وجود دارند که عملکرد مشابهی دارند و نحوه راه‌اندازی آنها تفاوت چندانی ندارد.

تراشه MPU6050

MPU6050 یکی از معروف‌ترین سنسورهای اندازه‌گیری شتاب در ماژول‌های مختلف است. درون این ماژول یک ژیروسکوپ ٣ محوره و یک شتاب‌سنج ٣ محوره تعبیه شده است. MPU6050 به دلیل قیمت کم، مصرف پائین انرژی و عملکرد خوبی که دارد در بسیاری از تلفن‌های هوشمند و تبلت‌ها استفاده شده است.

شتاب‌سنج و ژیروسکوپ به تنهایی دارای خطا در اندازه‌گیری هستند و ممکن است اطلاعات نادرستی بدهند. به همین دلیل درون این تراشه الگوریتم‌هایی استفاده شده است تا با ترکیب و مقایسه داده‌های شتاب‌سنج و ژیروسکوپ و حتی داده‌های دریافت شده از قطب‌نمای خارجی، خروجی هر کدام از سنسورها اصلاح شوند.

تراشه LSM303

این تراشه یک واحد اندازه‌گیری میدان مغناطیسی با رابط ارتباطی دیجیتال است که برای کاربردهای جهت‌یابی و سنجش میدان مغناطیسی مورد استفاده قرار می‌گیرد. این قطعه دارای روش‌های تقویت سیگنال و کاهش خطا بوده و دقت ١ تا ٢ درجه در تشخیص جهت دارد. این قطعه نیز در بسیاری از دستگاه‌های هوشمند استفاده شده است.

تراشه BMP180

BMP180 یک سنسور فشار بارومتری با دقت بالاست که مخصوص تجهیزات همراه مانند تلفن‌های هوشمند، تبلت‌ها و وسایل ورزشی هوشمند طراحی شده است. ویژگی‌های مهم این سنسور، ابعاد کوچک، رابط دیجیتال و مصرف بسیار کم انرژی تا 3µA است. همچنین پایداری این سنسور نسبت به نوسانات ولتاژ از ویژگی‌های قابل توجه آن است. این سنسور را می‌توان برای اندازه‌گیری فشار هوا استفاده کرد اما از آنجایی که ارتفاع از سطح دریا با فشار هوا ارتباط دارد، می‌توان سنسور فشار را برای تعیین ارتفاع هم استفاده کرد. به همین دلیل سنسورهای فشار کاربرد زیادی در ابزارهای موقعیت‌یابی مانند GPS دارند.

پروتکل ارتباطی I2C

قبلا در آموزش جامع آردوینو در مورد پروتکل‌های ارتباطی سریال از جمله SPI و UART صحبت کردیم. سومین پروتکل سریال که در بسیاری از وسایل الکترونیکی استفاده می‌شود، پروتکل I2C یا Inter Integrated Circuit است. این پروتکل نیز مانند UART از دو سیم برای انتقال اطلاعات استفاده می‌کند. در I2C سیم SDA برای ارسال داده و سیم SCL برای همزمان کردن برای زمان‌بندی و تعیین سرعت انتقال اطلاعات استفاده می‌شود؛ به این صورت که همزمان با ارسال هر بیت، یک پالس در مسیر SCL ایجاد می‌شود. به این ترتیب، ماژول دریافت‌کننده سیگنال با دریافت هر پالس از مسیر SCL، داده موجود روی خط SDA را به عنوان یک داده جدید تلقی می‌کند.

سیگنال‌های ارسالی در I2C با استفاده از یک روش مشخص بسته‌بندی می‌شوند تا برای گیرنده قابلیت تفسیر و عیب‌یابی را داشته باشند. این اتفاق در پروتکل‌های دیگر به روش‌های خاص خود صورت می‌گیرد. در مورد پروتکل I2C بسته داده همیشه با یک آدرس شروع می‌شود که مشخص می‌کند که گیرنده سیگنال را تعیین می‌کند. به دلیل وجود آدرس در پروتکل I2C می‌توان چندین ماژول مختلف را به راحتی با پورت یکسانی به هم وصل کرد. هر زمان که سیگنالی توسط ماژول‌ها دریافت شود، ماژول آدرس موجود در بسته را با آدرس خود مقایسه کرده و در صورتی که آدرسها یکسان باشند، پاسخ مناسب را ارسال می‌کند. در یک شبکه از ماژول‌ها که از طریق I2C به هم متصل هستند، حداقل یک Master و یک Slave نیاز است. Master بردی است که ارتباطات را مدیریت می‌کند و معمولا یک میکروکنترلر یا برد توسعه‌ای است. به این ترتیب پروتکل I2C از چند Master و چند Slave در یک شبکه یکسان پشتیبانی می‌کند. مهمترین محدودیت I2C سرعت پائین‌تر آن نسبت به SPI و حداکثر طول داده قابل ارسال با آن است. پروتکل I2C داده‌ها را در بسته‌های ٨ بیتی می‌فرستد. بنابراین اگر داده شما بیش از این طول داشته باشد، باید داده‌تان را پس از دریافت به هم بچسبانید. اگر میخواهید اطلاعات بیشتری در مورد این پروتکل به دست آورید می‌توانید مطلب آشنایی با پروتکل ارتباطی I2C را مطالعه کنید.

راه‌اندازی سنسور MPU6050

هر میکروکنترلر درگاه‌های محدودی برای ارتباط I2C در اختیار دارد. مثلا در آردوینو Uno پایه A4 برای SDA و پایه A5 برای SCL قرار داده شده است. بنابراین پایه‌های A4 و A5 آردوینو را به SDA و SCL ماژول متصل کنید. برای برقراری ارتباط با ماژول‌ها از طریق I2C باید رجیسترهای آن را بخوانید یا داده‌های مشخصی را به روی آنها انتساب کنید. جزئیات رجیسترهای هر ماژول یا میکروکنترلر در دفترچه راهنمای آن موجود است. در اینجا قصد نداریم که وارد جزئیات رجیسترها شویم و فقط به مواردی که نیاز داریم اشاره می‌کنیم. سنسور MPU6050 در ماژول‌های مختلفی وجود دارد. به عنوان نمونه در این قسمت ما از ماژول GY-521 برای کار با سنسور MPU6050 استفاده خواهیم کرد.

برای کار با ماژول‌ها از طریق I2C از کتابخانه Wire که به صورت پیش‌فرض در آردوینو وجود دارد استفاده می‌شود. چند دستور اولیه این کتابخانه به صورت زیر است. سایر دستورات مورد نیاز را در حین مطالب توضیح خواهیم داد:

دستور(Wire.beginTransmission(Moduleبرای شروع ارسال پیام یا دستور به ماژول استفاده می‌شود. در این دستور بجای عبارت Module آدرس ماژول مورد نظر را وارد کنید. با دستور()Wire.writeو()Wire.readمی‌توانید به آدرس مورد نظرتان داده‌ای را فرستاده یا از آن مقداری را بخوانید. همچنین دستور()Wire.endTransmissionانتهای پیام را به ماژول اطلاع می‌دهد.

اندازه‌گیری شتاب با IMU

تنها ابزار موجود در IMU برای اندازه‌گیری شتاب خطی، یک شتاب‌سنج ٣ درجه آزادی است. مشکل وجود خطای اندازه‌گیری در این سنسور همواره وجود دارد. تنها راه‌حل برای کاهش خطا (کاهش نویز) فیلتر کردن داده‌‌ها است. ساده‌ترین روش فیلتر کردن داده‌ها میانگین‌گیری از آنهاست. کد زیر مقدار شتاب را در راستای ٣ محور عمود بر هم محاسبه می‌کند.

/*
automee
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.automee.ir
*/
#include <Wire.h>
const int MPU = 0x68;
float AccX, AccY, AccZ;
float velX, velY, velZ;
float posX, posY, posZ;
float lastTime, thisTime, duration;

void accelerationCalc()
{
  float accelConversionRatio = 16384.0;
  int attempts = 20;
  AccX = 0;
  AccY = 0;
  AccX = 0;
  for (int counter = 0; counter < attempts; counter++)
  {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX += (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
    AccY += (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
    AccZ += (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
  }
  AccX = AccX / attempts * 9.81;
  AccY = AccY / attempts * 9.81;
  AccZ = (AccZ / attempts - 1) * 9.81;
  Serial.print("X Acceleration: ");
  Serial.print(AccX, 6);
  Serial.print("\t");
  Serial.print("Y Acceleration: ");
  Serial.print(AccY, 6);
  Serial.print("\t");
  Serial.print("Z Acceleration: ");
  Serial.print(AccZ, 6);
  Serial.println();
}

void velocityCalc()
{
  velX += AccX * duration;
  velY += AccY * duration;
  velZ += AccZ * duration;
}

void positionCalc()
{
  posX += velX * duration;
  posY += velY * duration;
  posZ += velZ * duration;
}

void initIMU()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

void setup()
{
  Serial.begin(115200);
  initIMU();
}

void loop()
{
  lastTime = thisTime;
  thisTime = millis();
  duration = (thisTime - lastTime) / 1000;
  accelerationCalc();
  Serial.println("=====================");
  velocityCalc();
  Serial.println("=====================");
  positionCalc();
  Serial.println("=====================");
  Serial.println("=====================");
  Serial.println("=====================");
}

همان طور که گفتیم برای برقراری ارتباط I2C با ماژول‌ها باید آدرس رجیسترهای آن را داشته باشید. این آدرس‌ها معمولا به صورت Hex نمایش داده می‌شوند. برای مثال آدرس ماژول MPU6050 برابر با0x68است. در کد شتاب‌سنج یک تابع به نام()InitIMUتعریف کرده‌ایم که مقدار رجیستر0x6Bرا ریست می‌کند. این کار در ابتدای راه‌اندازی سنسور لازم است.

void initIMU()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

تابع()accelerationCalcمقدار شتاب را از ماژول می‌خواند. همان طور که قبلا گفتیم مقادیر شتاب‌سنج دارای خطا هستند بنابراین برای کاهش خطا، از داده‌های سنسور میانگین گرفته‌ایم. در اینجا یک حلقه تعریف شده که هر ٢٠ داده سنسور را ذخیره کرده و از آن میانگین می‌گیرد و نتیجه را به عنوان شتاب لحظه‌ای نمایش می‌دهد. نکته دیگر در این تابع این است که طبق چیزی که قبلا گفتیم، پروتکل I2C داده‌ها را در بسته‌های ٨ بیتی جابجا می‌کند، بنابراین باید داده‌های سنسور را به هم متصل کنیم. و مورد آخر اینکه طبق کاتالوگ سنسور، داده‌های دریافت شده با یک نسبت مشخص به شتاب بر حسب g و سرعت زاویه‌ای بر حسب (s/˚) تبدیل می‌شوند. این نسبت در حالت پیش‌فرض برابر با 16384 برای شتاب‌سنج و 131 است.

  for (int counter = 0; counter < attempts; counter++)
  {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX += (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
    AccY += (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
    AccZ += (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
  }

مقداری که پس از میانگین‌گیری به دست می‌آید بر حسب شتاب g است. اگر شتاب را بر حسب m/s2 بخواهید باید شتاب را در ۹.۸۱ ضرب کنید. دقت کنید که در حالت سکون، سنسور تحت شتاب گرانش زمین قرار دارد که برابر با 1g است. به همین دلیل از مقدار شتاب در راستای Z به اندازه 1g کم می‌کنیم.

AccX = AccX / attempts * 9.81;
  AccY = AccY / attempts * 9.81;
  AccZ = (AccZ / attempts - 1) * 9.81;

در ادامه برنامه مقدار سرعت و موقعیت در هر لحظه با استفاده از مقادیر لحظه قبل (٠) و مقادیر فعلی و از طریق روابط زیر به دست آمده است:

هر چند که از لحاظ تئوری می‌توان سرعت و موقعیت را با استفاده از شتاب به دست آورد اما به خاطر خطای زیاد شتاب‌سنج، عملا مقادیر به دست آمده چندان قابل اعتماد نیستند.

اندازه‌گیری زاویه با ترکیب شتاب‌سنج و ژایروسکوپ

همان طور که گفتیم در ماژول MPU6050 یک سنسور شتاب‌سنج و یک سنسور ژایروسکوپ وجود دارد. شتاب‌سنج، شتاب خطی را در راستای سه محور عمود بر هم محاسبه کرده و ژایروسکوپ سرعت زاویه‌ای حول سه محور را اندازه می‌گیرد. لازم است بدانید که تمام ابزارهای اندازه‌گیری دچار خطا هستند. روشهای مختلفی برای کم کردن خطای اندازه‌گیری وجود دارد. یکی از این روشها، ترکیب داده‌های به دست آمده از چند سنسور مختلف است. در ماژول MPU6050 داده‌های سنسور شتاب تحت تاثیر نویز قرار دارد و به همین دلیل داده‌ها تا حدی پراکنده هستند. از طرفی داده‌های ژایروسکوپ کمتر تحت تاثیر نویز هستند ولی با گذشت زمان از مقدار درست فاصله می‌گیرند. برای جبران این مشکل یک راه حل این است که داده‌های شتاب‌سنج را با داده‌های ژایروسکوپ ترکیب کنیم. به این ترتیب، شتاب‌سنج از ایجاد انحراف داده‌های ژایروسکوپ جلوگیری می‌کند. تنها مسئله این است که چطور داده‌های شتاب‌سنج را به مقادیر زاویه چرخش تبدیل کنیم؟

با استفاده از روابط هندسی می‌توان نشان داد که در صورتی که شتاب خطی خالص نداشته باشیم، می‌توانیم زاویه چرخش حول محورها را به مقدار مؤلفه شتاب در راستای محورهای سه‌گانه ربط دهیم. این محاسبات هندسی پیچیدگی‌های خاص خودش را داشته و خارج از محدوده این آموزش است. در اینجا فقط رابطه نهایی که شتاب خطی را به زاویه چرخش ربط می‌دهد معرفی می‌کنیم. با استفاده از این روابط می‌توانید میزان چرخش Roll (چرخش حول محور X) و Pitch (چرخش حول محور Y) را محاسبه کنید.

از آنجا که سنسورهای شتاب نسبت به چرخش حول محور Z حساسیت ندارند، محاسبه زاویه Yaw به این روش ممکن نیست. البته به دلیل اینکه خطای اندازه‌گیری زاویه Yaw کمتر از دو زاویه دیگر است، نیاز زیادی هم به این محاسبه نداریم.

کالیبره کردن سنسور

قبل از اینکه بخواهید از داده‌های IMU استفاده کنید باید آن را کالیبره کنید. معمولا مقداری که IMU نشان می‌دهد تا حدی با مقدار واقعی اختلاف دارد. این اختلاف به دلیل شرایط کاری و لحیم شدن سنسورها به ماژول است. بنابراین همیشه قبل از استفاده از سنسور، آن را بر روی یک سطح صاف و بدون هر گونه حرکت و لرزش قرار داده و مقادیر آن را بخوانید. اگر مقادیر به دست آمده را از داده‌های سنسور کم کنید، مقادیر واقعی سنسور به دست خواهد آمد. برای کالیبره کردن ماژول MPU6050 کد زیر را بر روی آردوینو آپلود کرده و مقادیر به دست آمده را یادداشت کنید.

/*
automee
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.automee.ir
*/

#include <Wire.h>
const int MPU = 0x68;
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float angleXaccel, angleYaccel, velocityXgyro, velocityYgyro, velocityZgyro;

void ErrorCalibration()
{
  float accelConversionRatio = 16384.0;
  float gyroConversionRatio = 131.0;
  int attempts = 1000;

  for (int counter = 0; counter < attempts; counter++)
  {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
    AccY = (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
    AccZ = (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
    angleXaccel += atan(AccY / AccZ) * 180 / PI;
    angleYaccel += (atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI);
  }

  angleXaccel /= attempts;
  angleYaccel /= attempts;

  for (int counter = 0; counter < attempts; counter++)
  {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    velocityXgyro += ((Wire.read() << 8 | Wire.read()) / gyroConversionRatio);
    velocityYgyro += ((Wire.read() << 8 | Wire.read()) / gyroConversionRatio);
    velocityZgyro += ((Wire.read() << 8 | Wire.read()) / gyroConversionRatio);
  }

  velocityXgyro /= attempts;
  velocityYgyro /= attempts;
  velocityZgyro /= attempts;

  Serial.print("angleXaccel: ");
  Serial.println(angleXaccel,3);
  Serial.print("angleYaccel: ");
  Serial.println(angleYaccel,3);
  Serial.print("velocityXgyro: ");
  Serial.println(velocityXgyro,3);
  Serial.print("velocityYgyro: ");
  Serial.println(velocityYgyro,3);
  Serial.print("velocityZgyro: ");
  Serial.println(velocityZgyro,3);
  Serial.println();
}
void setup()
{
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

void loop()
{
  ErrorCalibration();
  Serial.println("=====================");
}

تا اینجا نحوه محاسبه زاویه با استفاده از شتاب‌سنج و کالیبره کردن سنسور را یاد گرفتید. دقت کنید که شتاب‌سنج ماژول با رجیستر0x3Bو ژایروسکوپ با رجیستر0x43قابل استفاده هستند. کد زیر مقدار زاویه لحظه‌ای را در راستای ٣ محور به کمک داده‌های شتاب‌سنج و ژایروسکوپ محاسبه می‌کند.

/*
automee
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.automee.ir
*/

#include <Wire.h>
const int MPU = 0x68; 
float accX, accY, accZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float accErrorX, accErrorY, gyroErrorX, gyroErrorY, gyroErrorZ;
float lastTime, thisTime, duration;

float errorAccAngleX = -1.22;
float errorAccAngleY = 0.13;
float errorGyroVelX = -3.37;
float errorGyroVelY = 0.82;
float errorGyroVleZ = 1.07;

void initIMU()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

void readAccelerometer()
{
  float accelConversionRatio = 16384.0;
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  accX = (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
  accY = (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
  accZ = (Wire.read() << 8 | Wire.read()) / accelConversionRatio;
  accAngleX = atan(accY / accZ) * 180 / PI - errorAccAngleX;
  accAngleY = (atan(-1 * accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * 180 / PI) - errorAccAngleY;
}

void readGyroscope()
{
  float gyroConversionRatio = 131.0;
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  GyroX = (Wire.read() << 8 | Wire.read()) / gyroConversionRatio - errorGyroVelX;
  GyroY = (Wire.read() << 8 | Wire.read()) / gyroConversionRatio - errorGyroVelY;
  GyroZ = (Wire.read() << 8 | Wire.read()) / gyroConversionRatio - errorGyroVleZ;
}

void setup()
{
  Serial.begin(115200);
  initIMU();
}
void loop()
{
  readAccelerometer();
  readGyroscope();
  lastTime = thisTime;
  thisTime = millis();
  duration = (thisTime - lastTime) / 1000;

  gyroAngleX += GyroX * duration;
  gyroAngleY += GyroY * duration;
  yaw += GyroZ * duration;
  roll = 0.95 * gyroAngleX + 0.05 * accAngleX;
  pitch = 0.95 * gyroAngleY + 0.05 * accAngleY;

  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print("\t");
  Serial.print("Pitch: ");
  Serial.print(pitch);
  Serial.print("\t");
  Serial.print("Yaw: ");
  Serial.print(yaw);
  Serial.println("\t");
}

در این کد ابتدا دو تابع()readAccelerometerو()readGyroscopeمقدار لحظه‌ای سنسورها را می‌خوانند. در ادامه برنامه، مقدار زاویه ژایروسکوپ با استفاده از رابطه زیر به دست می‌آید:

  gyroAngleX += GyroX * duration;
  gyroAngleY += GyroY * duration;
  yaw += GyroZ * duration;

برای ترکیب داده‌های دو سنسور از میانگین‌گیری وزن‌دار استفاده می‌کنیم. از آنجا که داده‌های شتاب‌سنج دارای نویز زیاد هستند، نباید وزن آن زیاد باشد. وزن حدود ۵ درصدی شتاب‌سنج برای جبران‌سازی انحراف داده‌های ژایروسکوپ کافی است:

  roll = 0.95 * gyroAngleX + 0.05 * accAngleX;
  pitch = 0.95 * gyroAngleY + 0.05 * accAngleY;

حفظ جهت سروو موتور با IMU

یکی از مهمترین کاربردهای IMU برای حفظ تعادل پرنده‌ها است. این کار می‌تواند به روش‌های مختلفی انجام شود. این موضوع بستگی به ابزارهای در دسترس و خلاقیت شما دارد. به عنوان نمونه در اینجا با استفاده از داده‌های IMU و محاسباتی که انجام دادیم، زاویه یک سروو موتور را ثابت نگه داریم. برای آشنایی با سروو موتور می‌توانید آموزش راه‌اندازی سروو موتور را مطالعه کنید. سروو موتور را به پایه ٣ آردوینو و IMU را مانند قبل وصل کنید.

کد زیر زاویه سروو موتور را به کمک IMU ثابت نگه می‌دارد. این پروژه زاویه موتور را حول محور Z ثابت نگه می‌دارد. برای جذاب‌تر شدن پروژه می‌توانید همین کار را برای دو محور دیگر هم تکرار کنید. در این صورت می‌توانید زاویه یک وسیله را در تمام جهات ثابت نگه دارید.

/*
automee
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.automee.ir
*/
#include <Servo.h>
Servo motor;
#include <Wire.h>
const int MPU = 0x68;
float GyroX, GyroY, GyroZ;
float gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float gyroErrorX, gyroErrorY, gyroErrorZ;
float lastTime, thisTime, duration;

float errorGyroVelX = -3.37;
float errorGyroVelY = 0.82;
float errorGyroVelZ = 1.07;

void initIMU()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

void readGyroscope()
{
  float gyroConversionRatio = 131.0;
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  GyroX = (Wire.read() << 8 | Wire.read()) / gyroConversionRatio - errorGyroVelX;
  GyroY = (Wire.read() << 8 | Wire.read()) / gyroConversionRatio - errorGyroVelY;
  GyroZ = (Wire.read() << 8 | Wire.read()) / gyroConversionRatio - errorGyroVelZ;
}

void setup()
{
  Serial.begin(115200);
  initIMU();
  motor.attach(3);
}
void loop()
{
  readGyroscope();
  lastTime = thisTime;
  thisTime = millis();
  duration = (thisTime - lastTime) / 1000;

  yaw += GyroZ * duration;
  Serial.println(yaw);
  motor.write(map(yaw, -90, 90, 180, 0));
}

راه‌اندازی ماژول قطب‌نما LSM303

قطب‌نمای LSM303 یک سنسور حساس به میدان مغناطیسی و یک شتاب‌سنج ٣ درجه آزادی است. درون این سنسور یک کویل بسیار کوچک قرار داده شده است که تحت تاثیر میدان مغناطیسی زمین، یک جریان الکتریکی در آن ایجاد می‌شود. سنسور با استفاده از همین خاصیت، جهت میدان مغناطیسی زمین را تشخیص داده و به این ترتیب می‌تواند جهت جغرافیایی را تعیین کند. در این قسمت از ماژول GY-511 استفاده می‌کنیم که سنسور LSM303 بر روی آن نصب شده است. اتصال این ماژول نیز مانند GY-87 و GY-521 از طریق پایه‌های I2C صورت می‌گیرد. یکی از کاربردهای قطب‌نما علاوه بر تعیین جهت جغرافیایی، اصلاح مقادیر زاویه به دست آمده از سنسورهای دیگر مثل ژایروسکوپ است. برای راه‌اندازی قطب‌نما از کتابخانه Adafruit_Sensor و Adafruit_LSM303_U استفاده می‌کنیم. کتابخانه‌های دیگری نیز برای قطب‌نما وجود دارد که می‌توانید از آنها استفاده کنید. از طریق لینک زیر می‌توانید کتابخانه‌های ذکر شده را دانلود کنید:

با استفاده از برنامه زیر می‌توانید جهت جغرافیایی را به دست آورید:

/*
automee
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.automee.ir
*/

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
Adafruit_LSM303_Mag_Unified compass = Adafruit_LSM303_Mag_Unified(11111);

void setup()
{
  Serial.begin(115200);
  while (!compass.begin())
    ;
  Serial.println("Compass initiated successfully!");
}

void loop()
{
  sensors_event_t compassRead;
  compass.getEvent(&compassRead);
  float heading = 0;
  float counter = 10.0;
  for (int i = 0; i < counter; i++)
  {
    heading += (atan2(compassRead.magnetic.y, compassRead.magnetic.x) * 180 / PI);
    if (heading < 0)
      heading += 360;
  }
  heading /= counter;
  heading = round(heading);
  Serial.print("Compass Heading: ");
  Serial.println(heading);
}

با استفاده از کتابخانه قطب‌نما تصویر بردار میدان مغناطیسی در دو جهت X و Y را در اختیار خواهید داشت. در نیتجه به کمک دستور ()atan2 می‌توانید جهت میدان مغناطیسی زمین را به دست آورید. این تابع دو مؤلفه عمود بر هم یک بردار را گرفته و زاویه مثلثاتی آن را محاسبه می‌کند. تفاوت دستور ()atan2 با دستور ()atan این است که دستور اول زاویه را بین -π و π و دستور دوم زاویه را بین -π/2 و π/2 می‌دهد. بنابراین دستور()atan2تمام جهات را پوشش می‌دهد. در نهایت با میانگین‌گیری از داده‌ها، نویز سنسور را کاهش می‌دهیم.

 for (int i = 0; i < counter; i++)
  {
    heading += (atan2(compassRead.magnetic.y, compassRead.magnetic.x) * 180 / PI);
    if (heading < 0)
      heading += 360;
  }
  heading /= counter;
  heading = round(heading);

اندازه‌گیری فشار بارومتری

یکی دیگر از سنسورهایی که در بعضی از ماژول‌های IMU وجود دارد، سنسور فشار بارومتری است. GY-87 یکی از ماژول‌هایی است که این سنسور را دارد. ماژول‌های دیگری نیز مثل ماژول GY-63 وجود دارند که فقط سنسور فشار دارند. این سنسور هم مانند سایر سنسورهای موجود در IMU با پروتکل I2C کار می‌کند. سنسور فشار مورد استفاده در ماژول GY-87 یک قطعه حساس به دما و فشار به نام BMP180 است. دلیل اینکه این سنسور، دما را هم اندازه می‌گیرد، این است که برای اندازه‌گیری دقیق فشار نیاز به دانستن دما داریم. تغییر دما باعث تغییر در اندازه‌گیری فشار شده و اگر آن را در نظر نگیرید دقت محاسبه فشار را کاهش می‌دهد. نحوه ارتباط دما و فشار بستگی به طراحی داخلی سنسور دارد و در کتابخانه BMP180 در نظر گرفته شده است. البته می‌توانید برای افزایش دقت سنسور، دما را با یک سنسور مستقل اندازه بگیرید. کتابخانه BMP180 را می‌توانید از لینک زیر دانلود کنید:

از طرفی فشار هوا با ارتفاع از سطح دریا ارتباط مشخصی به صورت زیر دارد:

بنابراین با داشتن فشار در سطح دریا (p0) و فشاری که از سنسور به دست آمده است می‌توانید ارتفاع از سطح دریا را محاسبه کنید. فشار در سطح دریا حدود 1013.25 میلی‌بار است. اگر بجای p0 فشار محل دیگری را قرار دهید، ارتفاع نسبی از آن نقطه محاسبه خواهد شد. برنامه زیر با استفاده از کتابخانه BMP180 و ماژول GY-87 ، دما، فشار و ارتفاع را محاسبه می‌کند:

/*
automee
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.automee.ir
*/

#include <SFE_BMP180.h>
#include <Wire.h>
SFE_BMP180 pressure;

void setup()
{
  Serial.begin(9600);
  Serial.println("Initializing Barometer sensor");
  while (!pressure.begin())
    ;
}

void loop()
{
  char lag;
  double T, P, p0 = 1013.25, altitude;
  lag = pressure.startTemperature();
  delay(lag);
  pressure.getTemperature(T);
  Serial.print("Temperature: ");
  Serial.print(T);
  Serial.println(" C");

  lag = pressure.startPressure(3);
  delay(lag);
  pressure.getPressure(P, T);
  Serial.print("Pressure: ");
  Serial.print(P);
  Serial.println(" mbar ");

  altitude = pressure.altitude(P, p0);
  Serial.print("Altitude: ");
  Serial.print(altitude, 0);
  Serial.println(" meters");
  Serial.println();

  delay(2000);
}

در کتابخانه BMP180 توابع()getTemperatureو()getPressurو()altitudeبه ترتیب دما، فشار و ارتفاع را اندازه می‌گیرند. نکته مهم این است که قبل از استفاده از دو تابع اول، باید توابع()startTemperatureو()startPressureفراخوانی شوند. خروجی این دو تابع طول زمانی را که باید صبر کنید تا سنسور آماده شود مشخص می‌کند. اگر این تاخیر را قرار ندهید، محاسبات دچار خطا خواهد شد.

 lag = pressure.startTemperature();
  delay(lag);
  pressure.getTemperature(T);

  lag = pressure.startPressure(3);
  delay(lag);
  pressure.getPressure(P, T);

نتیجه‌گیری

در این آموزش با یک ابزار قوی برای موقعیت‌یابی به نام IMU آشنا شدید و نحوه راه‌اندازی و روش‌های مختلف اندازه‌گیری با آن را یاد گرفتید.

در آموزش بعدی، نحوه راه‌اندازی ماژول GPS را یاد خواهید گرفت.

نظرات شما باعث بهبود محتوای آموزشی ما می‌شود. اگر این آموزش را دوست داشتید، همین‌طور اگر سوالی در مورد آن دارید، از شنیدن نظراتتان خوشحال خواهیم شد.