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

یکشنبه, 17 دی 1396 15:20
این مورد را ارزیابی کنید
(2 رای‌ها)

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

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

برد آردوینو و کابل رابط 

1 عدد

ماژول GY-87

1 عدد

ماژول GY-521

1 عدد

ماژول GY-511

1 عدد

سروو موتور

1 عدد

بردبورد

1 عدد

سیم جامپر

1 بسته

 

 


معرفی IMU

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

راه اندازی ماژول IMU با آردوینو

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

تراشه MPU6050

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

راه اندازی ماژول IMU با آردوینو

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

تراشه LSM303

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

راه اندازی ماژول IMU با آردوینو

تراشه BMP180

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

راه اندازی ماژول IMU با آردوینو


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

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

راه اندازی ماژول IMU با آردوینو

سیگنال‌های ارسالی در 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 استفاده خواهیم کرد.

راه اندازی ماژول IMU با آردوینو

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

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

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

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

/*
SanatBazar
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.sanatbazar.com

*/

#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;

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

راه اندازی ماژول IMU با آردوینو

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

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

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

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

راه اندازی ماژول IMU با آردوینو

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

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

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

/*
SanatBazar
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.sanatbazar.com

*/

#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قابل استفاده هستند. کد زیر مقدار زاویه لحظه‌ای را در راستای ٣ محور به کمک داده‌های شتاب‌سنج و ژایروسکوپ محاسبه می‌کند.

/*
SanatBazar
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.sanatbazar.com

*/

#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مقدار لحظه‌ای سنسورها را می‌خوانند. در ادامه برنامه، مقدار زاویه ژایروسکوپ با استفاده از رابطه زیر به دست می‌آید:

راه اندازی ماژول IMU با آردوینو

  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 با آردوینو

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

/*
SanatBazar
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.sanatbazar.com

*/

#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));
}

راه اندازی ماژول IMU با آردوینو


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

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

دانلود کتابخانه Adafruit_LSM303_U دانلود کتابخانه Adafruit_Sensor

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

/*
SanatBazar
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.sanatbazar.com

*/

#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 را می‌توانید از لینک زیر دانلود کنید:

دانلود کتابخانه BMP180

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

راه اندازی ماژول IMU با آردوینو

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

/*
SanatBazar
Arduino Tutorial Series
Author: Davood Dorostkar
Website: www.sanatbazar.com

*/

#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 را یاد خواهید گرفت.

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

خواندن 9798 دفعه
داوود درستکار

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

کاربرانی که در این گفتگو شرکت کرده اند

نظرات (14)

  1. مهدی

سلام وقت بخیر
پروژه بنده اندازه گیری مفاصل بدن با دو سنسور MPU6050 هستش .کدی که از ابتدا باید بزنم برای 6 عدد و تبدیل انها به زاویه رو ندارم .از کجا میتونم داشته بالشم و میخوام کد رو در فضای اردوینو بزنم

 
  1. پشتیبانی    مهدی

سلام مهدی عزیز
سوالتون نامفهوم هست. لطفا دوباره به صورت دقیقتر مطرح کنید.
با تشکر

 
  1. حسین

با سلام. یک مشکلی که در استفاده از سنسور IMU داریم این است که کارفرما از ما خواسته است که یک گواهی کالیبراسیون از سنسور توسط شرکت ثالث اخذ کنیم. از اونجایی که فرکانس داده برداری در این سنسور پایین است آزمایشگاه‌های معمول نمی‌توانند شرایط تست را...

با سلام. یک مشکلی که در استفاده از سنسور IMU داریم این است که کارفرما از ما خواسته است که یک گواهی کالیبراسیون از سنسور توسط شرکت ثالث اخذ کنیم. از اونجایی که فرکانس داده برداری در این سنسور پایین است آزمایشگاه‌های معمول نمی‌توانند شرایط تست را برقرار کنند. آیا شما می‌توانید آزمایشگاهی برای این کار معرفی کنید که گواهی کالیبراسیون در اختیار ما قرار بدهد؟

ادامه مطلب
 
  1. پشتیبانی    حسین

سلام دوست عزیز

خیر چنین امکانی را صنعت بازار ندارد متاسفانه.
موفق باشید

 
  1. محسن شعبانی

سلام.چطور میشه با استفاده از شتاب سنج موقعیت مکانی رو پیدا کرد؟یعنی وقتی خود سنسور رو بطور کامل به چپ و راست میبریم مکان دقیق مشخص بشه که چند متر به چپ یا راست رفته؟چندتا کلیپ تو یوتیوب دیدم اما موفق نشدم متوجه شم که چجوری میشه اینکارو کرد.

 
  1. پشتیبانی    محسن شعبانی

سلام دوست عزیز

برای بدست آوردن موقعیت از شتاب باید طبیعتا دوبار انتگرال گیری کنید. ولی این کار خطا و محاسبات زیادی داشته و مطلوب نیست. اما کتابخانه زیر با دقت مطلوبی این کار رو صورت داده.
https://github.com/tockn/MPU6050_tockn

موفق باشید

 
  1. میلاد

آیا میشه از سنسور MPU 6050 و داده‌های شتاب برای تحلیل ارتعاشات و پیک‌های ارتعاشی استفاده کرد؟ به عنوان مثال یک سیستم داریم که داخل لوله حرکت می‌کنه و در بازه‌های مشخص یک ارتعاش لحظه‌ای به سیستم وارد میشه. آیا سنسور MPU 6050 برای گرفتن رفتار این...

آیا میشه از سنسور MPU 6050 و داده‌های شتاب برای تحلیل ارتعاشات و پیک‌های ارتعاشی استفاده کرد؟ به عنوان مثال یک سیستم داریم که داخل لوله حرکت می‌کنه و در بازه‌های مشخص یک ارتعاش لحظه‌ای به سیستم وارد میشه. آیا سنسور MPU 6050 برای گرفتن رفتار این موج‌های ارتعاشی مناسب هست یا نه؟ پیشنهاد شما برای در‌نظرگرفتن این حالت چیه؟

ادامه مطلب
 
  1. پشتیبان    میلاد

سلام میلاد عزیز
سوال خیلی خوبی پرسیدید. باید دو نکته رو بررسی کنید تا مطمئن بشید که این سنسور قابل استفاده هست یا نه.
یک: ارتعاشات مورد نظرتون باید در محدوده فرکانس پاسخ سنسور باشه. این محدوده رو میتونید توی کاتالوگ سنسور پیدا کنید.
دو: فرکانس...

سلام میلاد عزیز
سوال خیلی خوبی پرسیدید. باید دو نکته رو بررسی کنید تا مطمئن بشید که این سنسور قابل استفاده هست یا نه.
یک: ارتعاشات مورد نظرتون باید در محدوده فرکانس پاسخ سنسور باشه. این محدوده رو میتونید توی کاتالوگ سنسور پیدا کنید.
دو: فرکانس ارتعاش باید کمتر از نصف فرکانس داده برداری آردوینو باشه وگرنه چیزی رو نمیتونید ثبت کنید.

ادامه مطلب
 
  1. سوار

سلام ممنون از پروژه های خوبتون
من تمام کارها رو برای وصل کردن ماژول gy 521 تمام این کارا رو انجام دادم ولی وقتی مثال مربوط به اونو باز میکنم تا امتحانش کنم این ارور رو تو سریال مانیتور میده
In⸮Initializing I2C devices...
Testing device...

سلام ممنون از پروژه های خوبتون
من تمام کارها رو برای وصل کردن ماژول gy 521 تمام این کارا رو انجام دادم ولی وقتی مثال مربوط به اونو باز میکنم تا امتحانش کنم این ارور رو تو سریال مانیتور میده
In⸮Initializing I2C devices...
Testing device connections...
MPU6050 connection failed

Send any character to begin DMP programming and demo:

نمیتونه به MPU6050 کانکت شه
اگه میشه راهنمایی کنید

ادامه مطلب
 
  1. پشتیبان    سوار

سلام دوست عزیز
اتصالاتتون رو چک کنید. کدی که در بالا آمده رو آپلود کنید و پنجره سریال مانیتور را باز کنید و نتیجه را ببینید. اگر سنسور سالم باشه باید اعداد معقولی ببینید و با حرکت دادن سنسور اعداد عوض بشه.

 
  1. بلو

سلام. اگر بخواهیم دیتاهایی که از mpu گرفتیم رو سه بعدی توی متلب نشان بدهیم. شما میدونین چجوری میشه؟

 
  1. پشتیبان    بلو

سلام دوست عزیز
ابزارهای گرافیکی برای این کار در متلب وجود دارد مانند ابزار زیر:
https://www.mathworks.com/matlabcentral/fileexchange/62511-rotationviewer

 
  1. حامد

با سلام در مورد برنامه نویسی این پروژه در ابتدا متغیر temp رو با int16_t تعریف کردید اما چطوری خروجی عددی اعشاری شده؟ یعنی int16_t به صورت اعشاری ذخیره میکنه؟

 
  1. پشتیبان    حامد

سلام دوست عزیز
دلیل این موضوع این هست که عدد دما با یک عدد float ضرب و جمع شده.

 
هیچ نظری در اینجا وجود ندارد

نظر خود را اضافه کنید.

ارسال نظر بعنوان یک مهمان
پیوست ها (0 / 3)
مکان خود را به اشتراک بگذارید
عبارت تصویر زیر را بازنویسی کنید. واضح نیست؟

محتوای آموزشی استفاده شده در این سایت محافظت شده می باشد و هرگونه کپی برداری از محتواهای تولید شده توسط تیم صنعت بازار موجب پیگرد خواهد بود. استفاده از مطالب صنعت بازار تنها با ذکر منبع به صورت لینک فعال بلامانع است

ما را دنبال کنید:

Copyright © 2017-2018 | Sanatbazar.com