تشخیص چرخش با استفاده از ژیروسکوپ + تشخیص جهت + خواندن شتاب

آردوینو – آموزش

مشاهده سایر جلسات آموزش

29 بازدید

۱۴۰۴-۰۸-۲۱

7 دقیقه

نویسنده: Arduino captain
درباره نویسنده: من کاپیتان آردوینو، اسمم میلاده و اینجا هستم تا تجربیاتم در رابطه با آردوینو رو با شما به اشتراک بزارم!

فهرست مطالب

تشخیص چرخش با استفاده از ژیروسکوپ
تشخیص جهت
خواندن شتاب

در قسمت 37 از آموزش آردوینو به بررسی استفاده از ماوس و دریافت موقعیت از GPS پرداختیم. در این قسمت قصد داریم درباره تشخیص چرخش با استفاده از ژیروسکوپ، تشخیص جهت و خواندن شتاب صحبت کنیم.

تشخیص چرخش با استفاده از ژیروسکوپ

فرض کنید می‌خواهید به نرخ چرخش پاسخ دهید. این می‌تواند برای حفظ حرکت یک وسیله نقلیه یا ربات در یک مسیر مستقیم یا چرخش با نرخ دلخواه استفاده شود.

ژیروسکوپ‌ها خروجی‌ای مرتبط با نرخ چرخش ارائه می‌دهند (برخلاف شتاب‌سنج که نرخ تغییر سرعت را نشان می‌دهد). اوایل، بیشتر ژیروسکوپ‌های کم‌هزینه از ولتاژ آنالوگ متناسب با نرخ چرخش استفاده می‌کردند. اکنون، با استفاده فراگیر از ژیروسکوپ‌ها و شتاب‌سنج‌ها در گوشی‌های هوشمند، یافتن ژیروسکوپ‌ها و شتاب‌سنج‌های ترکیبی با پروتکل I2C ارزان‌تر و آسان‌تر شده است.

✅نکته

برد Arduino Nano 33 BLE Sense دارای ژیروسکوپ و شتاب‌سنج داخلی است. واحد اندازه‌گیری اینرسی MPU-9250 یک سنسور نسبتاً ارزان با nine degrees of freedom (DOF9) است که به‌خوبی با آردوینو کار می‌کند. این سنسور روی بردهای breakout توسط تأمین‌کنندگان مختلف از جمله SparkFun شماره قطعه SEN-13762 موجود است. چندین کتابخانه برای پشتیبانی از MPU-9250 وجود دارد. اسکچ زیر از کتابخانه Bolder Flight Systems MPU9250 استفاده می‌کند که می‌توانید آن را از طریق Arduino Library Manager نصب کنید.

/* Gyro sketch
* Read a gyro and display rotation in degrees/sec
*/
#include "MPU9250.h"
// I2C address of IMU. If this doesn't work, try 0x69.
#define IMU_ADDRESS 0x68
MPU9250 IMU(Wire, IMU_ADDRESS); // Declare the IMU object
void setup() {
Serial.begin(9600);
while(!Serial);
// Initialize the IMU
int status = IMU.begin();
if (status < 0) {
Serial.println("Could not initialize the IMU.");
Serial.print("Error value: "); Serial.println(status);
while(1); // halt the sketch
}
// Set the full range of the gyro to +/- 500 degrees/sec
status = IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);
if (status < 0) {
Serial.println("Could not change gyro range.");
Serial.print("Error value: "); Serial.println(status);
}
}
void loop() {
IMU.readSensor();
// Obtain the rotational velocity in rads/second
float gx = IMU.getGyroX_rads();
float gy = IMU.getGyroY_rads();
float gz = IMU.getGyroZ_rads();
// Display velocity in degrees/sec
Serial.print("gx:");
Serial.print(gx * RAD_TO_DEG, 4);
Serial.print(",gy:");
Serial.print(gy * RAD_TO_DEG, 4);
Serial.print(",gz:");
Serial.print(gz * RAD_TO_DEG, 4);
Serial.println();
delay(100);
}

/* Gyro sketch

* Read a gyro and display rotation in degrees/sec

#include "MPU9250.h"

// I2C address of IMU. If this doesn't work, try 0x69.

#define IMU_ADDRESS 0x68

MPU9250 IMU(Wire, IMU_ADDRESS); // Declare the IMU object

void setup() {

Serial.begin(9600);

while(!Serial);

// Initialize the IMU

int status = IMU.begin();

if (status < 0) {

Serial.println("Could not initialize the IMU.");

Serial.print("Error value: "); Serial.println(status);

while(1); // halt the sketch

}

// Set the full range of the gyro to +/- 500 degrees/sec

status = IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);

if (status < 0) {

Serial.println("Could not change gyro range.");

Serial.print("Error value: "); Serial.println(status);

}

void loop() {

IMU.readSensor();

// Obtain the rotational velocity in rads/second

float gx = IMU.getGyroX_rads();

float gy = IMU.getGyroY_rads();

float gz = IMU.getGyroZ_rads();

// Display velocity in degrees/sec

Serial.print("gx:");

Serial.print(gx * RAD_TO_DEG, 4);

Serial.print(",gy:");

Serial.print(gy * RAD_TO_DEG, 4);

Serial.print(",gz:");

Serial.print(gz * RAD_TO_DEG, 4);

Serial.println();

delay(100);

}

✅نکته

MPU-9250 یک دستگاه I2C با ولتاژ ۳.۳ ولت است، بنابراین اگر از برد آردوینو با ولتاژ متفاوت از ۳.۳ ولت استفاده می‌کنید، نیاز به مبدل سطح منطقی (logic-level converter) خواهید داشت تا پین‌های SCL و SDA ژیروسکوپ محافظت شوند.

تشخیص چرخش با استفاده از ژیروسکوپ + تشخیص جهت + خواندن شتاب | قسمت 38 آموزش آردوینو

شکل 1: اتصال واحد اندازه‌گیری اینرسی (IMU) MPU-9250 با استفاده از I2C

این اسکچ با درج کتابخانه MPU9250 و اعلام یک شیء برای نمایش IMU آغاز می‌شود. درون تابع setup()، تلاش می‌کند IMU را مقدارسازی (initialize) کند. اگر این مرحله موفقیت‌آمیز نباشد، ممکن است نیاز باشد تعریف IMU_ADDRESS را به 0x69 تغییر دهید یا اتصالات سیم‌کشی خود را بررسی کنید. پس از مقدارسازی IMU، اسکچ محدوده کامل ژیروسکوپ را به ±۵۰۰ درجه بر ثانیه تغییر می‌دهد.

درون تابع loop()، اسکچ مقدار سنسور را می‌خواند و سرعت چرخش را به رادیان بر ثانیه به دست می‌آورد. سپس از ثابت RAD_TO_DEG در آردوینو برای تبدیل آن به درجه بر ثانیه استفاده می‌کند. خروجی اسکچ را می‌توان در Serial Monitor یا Serial Plotter مشاهده کرد.

تشخیص جهت

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

در این دستورالعمل از شتاب‌سنجِ موجود در ماژول MPU-9250 استفاده می‌کنیم. MPU-9250 یک واحد اندازه‌گیری اینرسی (IMU) با ۹ درجهٔ آزادی (9-DOF) است. سنسور را طبق شکل 1 متصل کنید.

شاید برای شما مفید باشد:

مدارات DC قسمت دوم: قانون اهم و توان

هر یک از سه سنسور اصلی MPU-9250 (ژیروسکوپ، مغناطیس‌سنج و شتاب‌سنج) مقادیر را در سه بعد (x, y, z) می‌خوانند و از همین‌جا مفهوم nine degrees of freedom به دست می‌آید.

✅نکته

قبل از استفاده از مغناطیس‌سنج (magnetometer)، باید آن را کالیبره کنید. می‌توانید یک اسکچ کالیبراسیون را در این GitHub issue پیدا کنید. این اسکچ مقادیر کالیبراسیون را در حافظه EEPROM برد میکروکنترلر شما ذخیره می‌کند.

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

/* Magnetometer sketch
Read a magnetometer and display magnetic field strengths
*/
#include "MPU9250.h"
#include <math.h>
#include "EEPROM.h"
// I2C address of IMU. If this doesn't work, try 0x69.
#define IMU_ADDRESS 0x68
// Change this to the declination for your location.
// See https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml
#define DECLINATION (-14)
MPU9250 IMU(Wire, IMU_ADDRESS); // Declare the IMU object
void setup() {
int status;
Serial.begin(9600);
while (!Serial);
// Initialize the IMU
status = IMU.begin();
if (status < 0)
{
Serial.println("Could not initialize the IMU.");
Serial.print("Error value: "); Serial.println(status);
while (1); // halt the sketch
}
load_calibration();
}
void loop() {
IMU.readSensor();
// Obtain the magnetometer values across each axis in units of microTesla
float mx = IMU.getMagX_uT();
float my = IMU.getMagY_uT();
float mz = IMU.getMagZ_uT();
// From https://github.com/bolderflight/MPU9250/issues/33
// Normalize the magnetometer data.
float m = sqrtf(mx * mx + my * my + mz * mz);
mx /= m;
my /= m;
mz /= m;
// Display the magnetometer values
Serial.print("mx:");
Serial.print(mx, 4);
Serial.print(",my:");
Serial.print(my, 4);
Serial.print(",mz:");
Serial.print(mz, 4);
Serial.println();
float constrained =
constrainAngle360(atan2f(-my, mx) + (DECLINATION * DEG_TO_RAD));
float calcAngle = constrained * RAD_TO_DEG;
Serial.print(calcAngle);
Serial.println(" degrees");
delay(100);
}
// From https://github.com/bolderflight/MPU9250/issues/33
float constrainAngle360(float dta) {
dta = fmod(dta, 2.0 * PI);
if (dta < 0.0)
dta += 2.0 * PI;
return dta;
}
// Load the calibration from the eeprom
// From https://github.com/bolderflight/MPU9250/issues/33
void load_calibration() {
float hxb, hxs, hyb, hys, hzb, hzs;
uint8_t eeprom_buffer[24];
for (unsigned int i = 0; i < sizeof(eeprom_buffer); i++ ) {
eeprom_buffer[i] = EEPROM.read(i);
}
memcpy(&hxb, eeprom_buffer, sizeof(hxb));
memcpy(&hyb, eeprom_buffer + 4, sizeof(hyb));
memcpy(&hzb, eeprom_buffer + 8, sizeof(hzb));
memcpy(&hxs, eeprom_buffer + 12, sizeof(hxs));
memcpy(&hys, eeprom_buffer + 16, sizeof(hys));
memcpy(&hzs, eeprom_buffer + 20, sizeof(hzs));
IMU.setMagCalX(hxb, hxs);
IMU.setMagCalY(hyb, hys);
IMU.setMagCalZ(hzb, hzs);
}

/* Magnetometer sketch

Read a magnetometer and display magnetic field strengths

#include "MPU9250.h"

#include <math.h>

#include "EEPROM.h"

// I2C address of IMU. If this doesn't work, try 0x69.

#define IMU_ADDRESS 0x68

// Change this to the declination for your location.

// See https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml

#define DECLINATION (-14)

MPU9250 IMU(Wire, IMU_ADDRESS); // Declare the IMU object

void setup() {

int status;

Serial.begin(9600);

while (!Serial);

// Initialize the IMU

status = IMU.begin();

if (status < 0)

{

Serial.println("Could not initialize the IMU.");

Serial.print("Error value: "); Serial.println(status);

while (1); // halt the sketch

}

load_calibration();

}

void loop() {

IMU.readSensor();

// Obtain the magnetometer values across each axis in units of microTesla

float mx = IMU.getMagX_uT();

float my = IMU.getMagY_uT();

float mz = IMU.getMagZ_uT();

// From https://github.com/bolderflight/MPU9250/issues/33

// Normalize the magnetometer data.

float m = sqrtf(mx * mx + my * my + mz * mz);

mx /= m;

my /= m;

mz /= m;

// Display the magnetometer values

Serial.print("mx:");

Serial.print(mx, 4);

Serial.print(",my:");

Serial.print(my, 4);

Serial.print(",mz:");

Serial.print(mz, 4);

Serial.println();

float constrained =

constrainAngle360(atan2f(-my, mx) + (DECLINATION * DEG_TO_RAD));

float calcAngle = constrained * RAD_TO_DEG;

Serial.print(calcAngle);

Serial.println(" degrees");

delay(100);

}

// From https://github.com/bolderflight/MPU9250/issues/33

float constrainAngle360(float dta) {

dta = fmod(dta, 2.0 * PI);

if (dta < 0.0)

dta += 2.0 * PI;

return dta;

}

// Load the calibration from the eeprom

// From https://github.com/bolderflight/MPU9250/issues/33

void load_calibration() {

float hxb, hxs, hyb, hys, hzb, hzs;

uint8_t eeprom_buffer[24];

for (unsigned int i = 0; i < sizeof(eeprom_buffer); i++ ) {

eeprom_buffer[i] = EEPROM.read(i);

}

memcpy(&hxb, eeprom_buffer, sizeof(hxb));

memcpy(&hyb, eeprom_buffer + 4, sizeof(hyb));

memcpy(&hzb, eeprom_buffer + 8, sizeof(hzb));

memcpy(&hxs, eeprom_buffer + 12, sizeof(hxs));

memcpy(&hys, eeprom_buffer + 16, sizeof(hys));

memcpy(&hzs, eeprom_buffer + 20, sizeof(hzs));

IMU.setMagCalX(hxb, hxs);

IMU.setMagCalY(hyb, hys);

IMU.setMagCalZ(hzb, hzs);

}

ماژول قطب‌نما شدت‌های میدان مغناطیسی را در سه محور x ،y و z فراهم می‌کند.

شاید برای شما مفید باشد:

شیلد رایگان SIM800 برای برد Arduino UNO

این مقادیر تغییر می‌کنند وقتی که جهت‌گیری قطب‌نما نسبت به میدان مغناطیسی زمین (شمال مغناطیسی) تغییر داده می‌شود.

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

تفاوت بزرگ دیگر این است که داده‌های کالیبراسیون از EEPROM بارگذاری می‌شوند.

برای اینکه این کد درست کار کند، IMU باید روی یک سطح صاف قرار داشته باشد.

شما همچنین باید میل مغناطیسی (declination) مربوط به موقعیت جغرافیایی خود را تنظیم کنید با تغییردادن مقدار DECLINATION در بالای کد (برای میل مغناطیسی غربی از عدد منفی استفاده کنید، برای میل شرقی از عدد مثبت).

خوانش‌های مغناطیس‌سنج نرمال‌سازی می‌شوند، به این صورت که هر خوانش تقسیم می‌شود بر ریشه‌ی دوم مجموع مربعات (RSS) همه‌ی خوانش‌ها.

زاویه نسبت به شمال مغناطیسی محاسبه می‌شود با اضافه‌کردن میل مغناطیسی (بر حسب رادیان) به فرمول زیر:

radians = arctan2(–my, mx)

این مقدار توسط تابع constrainAngle360 به بازه‌ی 360 degrees (2 * pi radians) محدود می‌شود.

سپس نتیجه با ضرب در ثابت RAD_TO_DEG به درجه تبدیل می‌شود.

صفر درجه نشان‌دهنده‌ی شمال مغناطیسی است.

برای حرکت‌دادن servo، از calcAngle استفاده کنید همان‌طور که در اینجا نشان‌داده‌شده است:

angle = constrain(calcAngle, 0, 180);
myservo.write(calcAngle);

1 2	angle = constrain(calcAngle, 0, 180); myservo.write(calcAngle);

خواندن شتاب

شما می‌خواهید به شتاب واکنش نشان دهید؛ برای مثال، برای تشخیص اینکه چه زمانی چیزی شروع به حرکت می‌کند یا متوقف می‌شود.
یا می‌خواهید تشخیص دهید که یک جسم نسبت به سطح زمین چگونه قرار گرفته است (اندازه‌گیری شتاب ناشی از گرانش).
این دستورالعمل از شتاب‌سنج موجود در MPU-9250 که یک واحد اندازه‌گیری اینرسی با nine degrees of freedom (9DOF IMU) است. سنسور را همان‌طور که در شکل 1 نشان‌داده‌شده است متصل کنید.

نمونه کد ساده‌ای که اینجا آمده است از MPU-9250 استفاده می‌کند تا شتاب را در سه محور x ،y و z نمایش دهد.

/* Accelerometer sketch
* Read an accelerometer and display acceleration in m/s/s
*/
#include "MPU9250.h"
// I2C address of IMU. If this doesn't work, try 0x69.
#define IMU_ADDRESS 0x68
MPU9250 IMU(Wire, IMU_ADDRESS); // Declare the IMU object
void setup() {
Serial.begin(9600);
while(!Serial);
// Initialize the IMU
int status = IMU.begin();
if (status < 0) {
Serial.println("Could not initialize the IMU.");
Serial.print("Error value: "); Serial.println(status);
while(1); // halt the sketch
}
}
void loop() {
IMU.readSensor();
// Obtain the rotational velocity in rads/second
float ax = IMU.getAccelX_mss();
float ay = IMU.getAccelY_mss();
float az = IMU.getAccelZ_mss();
// Display velocity in degrees/sec
Serial.print("ax:"); Serial.print(ax, 4);
Serial.print(",ay:"); Serial.print(ay, 4);
Serial.print(",az:"); Serial.print(az, 4);
Serial.println();
delay(100);
}

/* Accelerometer sketch

* Read an accelerometer and display acceleration in m/s/s

#include "MPU9250.h"

// I2C address of IMU. If this doesn't work, try 0x69.

#define IMU_ADDRESS 0x68

MPU9250 IMU(Wire, IMU_ADDRESS); // Declare the IMU object

void setup() {

Serial.begin(9600);

while(!Serial);

// Initialize the IMU

int status = IMU.begin();

if (status < 0) {

Serial.println("Could not initialize the IMU.");

Serial.print("Error value: "); Serial.println(status);

while(1); // halt the sketch

}

void loop() {

IMU.readSensor();

// Obtain the rotational velocity in rads/second

float ax = IMU.getAccelX_mss();

float ay = IMU.getAccelY_mss();

float az = IMU.getAccelZ_mss();

// Display velocity in degrees/sec

Serial.print("ax:"); Serial.print(ax, 4);

Serial.print(",ay:"); Serial.print(ay, 4);

Serial.print(",az:"); Serial.print(az, 4);

Serial.println();

delay(100);

}

این نمونه‌کد شبیه نمونه کد ژیروسکوپ از مثال‌های قبلی است، با این تفاوت که شتاب در هر محور را بر حسب متر بر ثانیه مربع (m/s²) نمایش می‌دهد.

حتی وقتی سنسور ثابت است، متوجه خواهید شد که شتاب محور z حدود m/s² ۹.۸– است. حداقل این چیزی است که وقتی این کد را روی زمین اجرا می‌کنید مشاهده خواهید کرد، جایی که شتاب گرانش تقریباً m/s² ۹.۸ است.

اگر مقدار ۰ روی محور z مشاهده کنید، یعنی سنسور در سقوط آزاد قرار دارد. نیرویی که باعث شتاب ۹.۸ m/s² می‌شود، نیروی مکانیکی چیزی است که مانع سقوط سنسور می‌شود (دست شما، میز یا کف زمین).

با اینکه از دیدگاه شما جسم شتابی ندارد، اما نسبت به سقوط آزاد شتاب دارد؛ این همان شرایطی است که اگر هیچ‌چیزی بین سنسور و مرکز زمین نبود، اعمال می‌شد.

اگر هیچ‌چیزی بین سنسور و مرکز زمین نبود، این وضعیت کمی غیرمعمول و قطعاً نامطلوب برای ساکنان زمین بود، حداقل از دیدگاه موجودات زنده روی زمین.

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

شاید برای شما مفید باشد:

کنترلر PID چیست و چرا از آن استفاده کنیم؟

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

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

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