راه اندازی AHRS با ماژول 9 محوره ICM20948 و آردوینو
36 بازدید
۱۴۰۴-۰۷-۲۶
9 دقیقه
اجزاء AHRS
ژیروسکوپ
یک ژیروسکوپ به سیستم مرجع وضعیت و جهت (AHRS) اندازه‌گیری نرخ زاویه‌ای سیستم را ارائه می‌دهد. این اندازه‌گیری‌های نرخ زاویه‌ای سپس برای تعیین برآوردی از وضعیت سیستم یکپارچه می‌شوند. با این حال، برای تعیین وضعیت فعلی، وضعیت اولیه سیستم نیز باید شناخته شده باشد. با گذشت زمان، این وضعیت محاسبه‌شده به دلیل ویژگی‌های ذاتی نویز و انحراف ژیروسکوپ از وضعیت واقعی سیستم به طور نامحدود دچار انحراف می‌شود.
شتاب سنج
یک شتاب‌سنج به سیستم مرجع وضعیت و جهت (AHRS) اندازه‌گیری شتاب سیستم را ارائه می‌دهد و فرض بر این است که تنها گرانش را اندازه‌گیری می‌کند. این فرض به شتاب‌سنج اجازه می‌دهد تا زاویه‌های (pitch) و (roll) را از جهت‌گیری بردار گرانش محاسبه کند، با این حال، هرگونه انحراف یا خطای دیگری در اندازه‌گیری‌های شتاب‌سنج باعث خطا در محاسبه زاویه‌های شیب و غلطش می‌شود. علاوه بر این، از آنجا که فرض بر این است که شتاب‌سنج تنها گرانش را اندازه‌گیری می‌کند، هرگونه حرکت دینامیکی اضافی نیز باعث خطا در محاسبه زاویه‌های roll و Pitch سیستم می‌شود.
مغناطیس سنج (قطب نما)
از آنجا که شتاب‌سنج تنها می‌تواند زاویه‌های (pitch) و (roll) را اندازه‌گیری کند، یک مغناطیس‌سنج به (AHRS) اندازه‌گیری (yaw) را ارائه می‌دهد، به این صورت که اندازه‌گیری میدان مغناطیسی اطراف سیستم را با میدان مغناطیسی زمین مقایسه می‌کند، درست مانند یک قطب‌نما مغناطیسی سنتی. در بیشتر واحدهای AHRS، اندازه‌گیری‌های مغناطیس‌سنج تأثیری بر برآوردهای زاویه‌های roll و pitch ندارند.
در یک سیستمAHRS، اندازه‌گیری‌های ژیروسکوپ، شتاب‌سنج و مغناطیس‌سنج ترکیب می‌شوند تا برآوردی از جهت‌گیری سیستم ارائه دهند، که اغلب از یک فیلتر کالمن برای این کار استفاده می‌شود. این تکنیک برآوردی از وضعیت با استفاده از اندازه‌گیری‌های خام این حسگرها به دست می‌آورد و یک برآورد بهینه از وضعیت را با توجه به فرضیات مطرح‌شده برای هر حسگر فردی محاسبه می‌کند. فیلتر کالمن علاوه بر وضعیت، انحراف ژیروسکوپ یا خطای انحراف bias آن را نیز برآورد می‌کند. سپس میتوان از انحراف ژیروسکوپ برای جبران اندازه‌گیری‌های خام ژیروسکوپ استفاده کرده و از انحراف آن در طول زمان جلوگیری کرد. با ترکیب داده‌های هر یک از این حسگرها در یک فیلتر کالمن، میتوان یک راه‌حل جهت‌یابی با نرخ بالا و بدون انحراف برای سیستم به دست آورد.
معرفی ICM20948
ICM20948 یک سنسور واحد اندازه‌گیری اینرسی (IMU) پیشرفته است که توسط شرکت InvenSense تولید شده است. این سنسور ترکیبی از شتاب‌سنج ۳ محوره، ژیروسکوپ ۳ محوره و مغناطیس‌سنج ۳ محوره را در یک تراشه واحد ارائه می‌دهد. ICM20948 برای کاربردهای مختلفی از جمله رباتیک، سیستم‌های موقعیت‌یابی، پهپادها، و دستگاه‌های قابل حمل طراحی شده است. این تراشه به دلیل دقت بالا، مصرف انرژی پایین و قابلیت یکپارچگی آسان در سیستم‌های پیچیده، به‌ویژه در کاربردهای پیشرفته AHRS، محبوب است. همچینین این تراشه بدلیل داشتن پردازنده حرکتی (DMP) در داخل خود توانایی محاسبه انواع محاسبات ریاضی مورد نیاز را دارد و پردازنده host نیاز به درگیر شدن برای انجام این گونه محاسبات را ندارد.
در این آموزش ما از ماژول این سنسور استفاده میکنیم که از فروشگاه امبددتک قابل تهیه است.
راه اندازی AHRS و ماژول ICM20948 و آردوینو
سخت افزار مورد نیاز
- آردوینو UNO R4
- ماژول ICM42688
نحوه اتصال سخت افزار
توجه شود بدلیل نیاز داشتن کتابخانه راه انداز این سنسور به بیش از 35 کیلوبایت حافظه Flash و عدم پشتیبانی Arduino UNO R3 از این مقدار حافظه Flash (نهایتا 32 کیلوبایت) در این آموزش از Arduino UNO R4 Wifi استفاده شده است. همچنین میتوان از سایر مدل های برد های آردوینو با حافظه Flash بیشتر از 32KB استفاده کرد.
برای استفاده از سنسور ICM20948 ابتدا باید کتابخانه آردوینوی آن فراخوانی شود. برای دانلود کتابخانه ICM2048 در نرم افزار آردوینو به مسیر Sketch -> Include Library -> Manage Libraries رفته و با جستجوی ICM 20948 میتوانید کتابخانه مورد نظر را دانلود و نصب کنید.
بررسی برنامه
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 |
#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU //#define USE_SPI // Uncomment this to use SPI #define SERIAL_PORT Serial #define SPI_PORT SPI // Your desired SPI port. Used only when "USE_SPI" is defined #define CS_PIN 2 // Which pin you connect CS to. Used only when "USE_SPI" is defined #define WIRE_PORT Wire // Your desired Wire port. Used when "USE_SPI" is not defined // The value of the last bit of the I2C address. // On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0 #define AD0_VAL 1 #ifdef USE_SPI ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object #else ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object #endif void setup() { SERIAL_PORT.begin(115200); // Start the serial console #ifndef QUAT_ANIMATION SERIAL_PORT.println(F("ICM-20948 Example")); #endif delay(100); #ifdef USE_SPI SPI_PORT.begin(); #else WIRE_PORT.begin(); WIRE_PORT.setClock(400000); #endif #ifndef QUAT_ANIMATION //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial #endif bool initialized = false; while (!initialized) { // Initialize the ICM-20948 // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually. #ifdef USE_SPI myICM.begin(CS_PIN, SPI_PORT); #else myICM.begin(WIRE_PORT, AD0_VAL); #endif #ifndef QUAT_ANIMATION SERIAL_PORT.print(F("Initialization of the sensor returned: ")); SERIAL_PORT.println(myICM.statusString()); #endif if (myICM.status != ICM_20948_Stat_Ok) { #ifndef QUAT_ANIMATION SERIAL_PORT.println(F("Trying again...")); #endif delay(500); } else { initialized = true; } } #ifndef QUAT_ANIMATION SERIAL_PORT.println(F("Device connected!")); #endif bool success = true; // Use success to show if the DMP configuration was successful // Initialize the DMP. initializeDMP is a weak function. You can overwrite it if you want to e.g. to change the sample rate success &= (myICM.initializeDMP() == ICM_20948_Stat_Ok); // DMP sensor options are defined in ICM_20948_DMP.h // INV_ICM20948_SENSOR_ACCELEROMETER (16-bit accel) // INV_ICM20948_SENSOR_GYROSCOPE (16-bit gyro + 32-bit calibrated gyro) // INV_ICM20948_SENSOR_RAW_ACCELEROMETER (16-bit accel) // INV_ICM20948_SENSOR_RAW_GYROSCOPE (16-bit gyro + 32-bit calibrated gyro) // INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED (16-bit compass) // INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED (16-bit gyro) // INV_ICM20948_SENSOR_STEP_DETECTOR (Pedometer Step Detector) // INV_ICM20948_SENSOR_STEP_COUNTER (Pedometer Step Detector) // INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR (32-bit 6-axis quaternion) // INV_ICM20948_SENSOR_ROTATION_VECTOR (32-bit 9-axis quaternion + heading accuracy) // INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR (32-bit Geomag RV + heading accuracy) // INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD (32-bit calibrated compass) // INV_ICM20948_SENSOR_GRAVITY (32-bit 6-axis quaternion) // INV_ICM20948_SENSOR_LINEAR_ACCELERATION (16-bit accel + 32-bit 6-axis quaternion) // INV_ICM20948_SENSOR_ORIENTATION (32-bit 9-axis quaternion + heading accuracy) // Enable the DMP orientation sensor success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok); // Enable any additional sensors / features //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == ICM_20948_Stat_Ok); //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok); //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok); // Configuring DMP to output data at multiple ODRs: // DMP is capable of outputting multiple sensor data at different rates to FIFO. // Setting value can be calculated as follows: // Value = (DMP running rate / ODR ) - 1 // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10. success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok); // Set to the maximum //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum // Enable the FIFO success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok); // Enable the DMP success &= (myICM.enableDMP() == ICM_20948_Stat_Ok); // Reset DMP success &= (myICM.resetDMP() == ICM_20948_Stat_Ok); // Reset FIFO success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok); // Check success if (success) { #ifndef QUAT_ANIMATION SERIAL_PORT.println(F("DMP enabled!")); #endif } else { SERIAL_PORT.println(F("Enable DMP failed!")); SERIAL_PORT.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h...")); while (1) ; // Do nothing more } } void loop() { // Read any DMP data waiting in the FIFO // Note: // readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available. // If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data. // readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete // readDMPdataFromFIFO will return ICM_20948_Stat_Ok if a valid frame was read. // readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data. icm_20948_DMP_data_t data; myICM.readDMPdataFromFIFO(&data); if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) // Was valid data available? { //SERIAL_PORT.print(F("Received data! Header: 0x")); // Print the header in HEX so we can see what data is arriving in the FIFO //if ( data.header < 0x1000) SERIAL_PORT.print( "0" ); // Pad the zeros //if ( data.header < 0x100) SERIAL_PORT.print( "0" ); //if ( data.header < 0x10) SERIAL_PORT.print( "0" ); //SERIAL_PORT.println( data.header, HEX ); if ((data.header & DMP_header_bitmap_Quat9) > 0) // We have asked for orientation data so we should receive Quat9 { // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values. // The quaternion data is scaled by 2^30. //SERIAL_PORT.printf("Quat9 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat9.Data.Q1, data.Quat9.Data.Q2, data.Quat9.Data.Q3, data.Quat9.Data.Accuracy); // Scale to +/- 1 double q1 = ((double)data.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30 double q2 = ((double)data.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30 double q3 = ((double)data.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30 double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); double qw = q0; // See issue #145 - thank you @Gord1 double qx = q2; double qy = q1; double qz = -q3; // roll (x-axis rotation) double t0 = +2.0 * (qw * qx + qy * qz); double t1 = +1.0 - 2.0 * (qx * qx + qy * qy); double roll = atan2(t0, t1) * 180.0 / PI; double t2 = +2.0 * (qw * qy - qz * qx); t2 = (t2 > 1.0) ? 1.0 : t2; t2 = (t2 < -1.0) ? -1.0 : t2; double pitch = asin(t2) * 180.0 / PI; double t3 = +2.0 * (qw * qz + qx * qy); double t4 = +1.0 - 2.0 * (qy * qy + qz * qz); double yaw = atan2(t3, t4) * 180.0 / PI; #ifndef QUAT_ANIMATION // SERIAL_PORT.print(F("Q1:")); // SERIAL_PORT.print(q1, 3); // SERIAL_PORT.print(F(" Q2:")); // SERIAL_PORT.print(q2, 3); // SERIAL_PORT.print(F(" Q3:")); // SERIAL_PORT.print(q3, 3); // SERIAL_PORT.print(F(" Accuracy:")); // SERIAL_PORT.println(data.Quat9.Data.Accuracy); Serial.print("Orientation: "); SERIAL_PORT.print(yaw, 1); SERIAL_PORT.print(F(", ")); SERIAL_PORT.print(pitch, 1); SERIAL_PORT.print(F(", ")); SERIAL_PORT.println(roll, 1); #else // Output the Quaternion data in the format expected by ZaneL's Node.js Quaternion animation tool SERIAL_PORT.print(F("{\"quat_w\":")); SERIAL_PORT.print(q0, 3); SERIAL_PORT.print(F(", \"quat_x\":")); SERIAL_PORT.print(q1, 3); SERIAL_PORT.print(F(", \"quat_y\":")); SERIAL_PORT.print(q2, 3); SERIAL_PORT.print(F(", \"quat_z\":")); SERIAL_PORT.print(q3, 3); SERIAL_PORT.println(F("}")); #endif } } if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay { delay(10); } } |
در تابع setup تنظیمات اولیه مربوط به ماژول پیکربندی می شوند. در خطوط 32 تا 37 پیکربندی پروتکل استفاده شده در ماژول که I2c می باشد انجام می شود. در خط 36 سرعت ارتباط I2c برابر 400KHz تنظیم می شود. در خطوط 44 تا 70 ، while بررسی می کند اگر سنسور به درستی وصل شده نشده باشد داخل آن گیر می کند تا اتصالات سنسور به درستی انجام شود و زمانی که آردوینو سنسور را تشخیص دهد Device connected در ترمینال نشان داده می شود.
خطوط 79 و 99 پیکربندی DMP انجام می شود همچنین سایر گزینه های قابل استفاده برای تنظیم DMP به صورت کامنت در خطوط 81 تا 96 آورده شده اند. این گزینه ها شامل راه اندازی پدومتر ، کالیبراسیون و… می باشند. در خط 111 مشخص می شود که سرعت نمونه برداری از محور ها چقدر باشد که در اینجا ما هر 9 محور را می خواهیم و سرعت نمونه برداری روی حداکثر مقدار قابل تنظیم قرار داده می شود.
خطوط 175 تا 197 داده های Quaternion را به زاویه های رول ، پیچ و یاو تبدیل می کنند. در اینجا، ما از یک Quaternions که مقادیر Q1 و Q2 و Q3 هستند و ازطرف خود ماژول ارسال می شوند برای نشان دادن چرخش‌های سه‌بعدی استفاده میکنیم. کواترنین‌ها معمولاً برای به‌دست آوردن جهت و چرخش در فضای سه‌بعدی به کار می‌روند، به‌ویژه در سیستم‌های مانند AHRS که برای ناوبری استفاده می‌شوند. این فرمول ها برای تبدیل کواترنین ها به زاویه های اویلر در سیستم مختصات سه بعدی استفاده می شوند.
خطوط 201 تا 216 برای نمایش داده های رول ، پیچ و یاو استفاده می شوند که ما از آنها برای نمایش در نمایش دهنده سه بعدی استفاده می کنیم.
نرم افزار نمایش دهنده زاویه جسم به صورت سه بعدی
بعد از وارد شدن و آپلود کردن کد روی آردوینو برای استفاده از نرم افزار نمایش دهنده سه بعدی وارد این لینک می شویم. نحوه اتصال آردوینو به این صورت است که ابتدا بادریت مورد نظر را انتخاب می کنیم که در برنامه ما 115200 است. سپس دکمه connect را کلیک می کنیم.
بعد از اتصال جسم سه بعدی خرگوش با حرکت ماژول زاویه اش تغییر پیدا می کند:
توجه شود که ماژول بعد از هر بار روشن شدن مغناطیس سنجش نیاز به کالیبراسیون دارد تا آفست اولیه اش از بین برود. در واقع هر سنسور مغناطیس سنج بعد از هر بار خاموش روشن شدن نیاز به کالیبره دارد تا آفست اولیه آن از بین برود. این آفست بدلایل مختلفی همچون شدت و یا ضعف میدان مغناطیسی زمین در برخی نقاط، نویز های الکترو مغناطیسی ، سیم های نزدیک به سنسور و … ایجاد می شود.
حرکت دادن ماژول به صورت هشتی کمک زیادی به کالیبره شدن مغناطیس سنج می کند.
خرید ماژول ژیروسکوپ و شتاب سنج و قطب نما 9 محوره ICM 20948 (IM-931)
اطلاعات
اشتراک و حمایت
آخرین مقالات
مقالات مرتبط
نویسنده: امبددتک
متخصص الکترونیک
طراحی و تولید محصولات الکترونیک | embeddedtech.ir | چاپ PCB
مقالات بیشتر
پالت | بازار خرید و فروش قطعات الکترونیک
قطعات اضافه و بدون استفاده همیشه یکی از سرباره‌‌های شرکتها و طراحان حوزه برق و الکترونیک بوده و هست. پالت سامانه‌ای است که بصورت تخصصی اجازه خرید و فروش قطعات مازاد الکترونیک را فراهم می‌کند. فروش در پالت
آیسی | موتور جستجوی قطعات الکترونیک
سامانه آی سی سیسوگ (Isee) قابلیتی جدید و کاربردی از سیسوگ است. در این سامانه سعی شده است که جستجو، انتخاب و خرید مناسب تر قطعات برای کاربران تسهیل شود. جستجو در آیسی
سیسوگ‌شاپ | فروشگاه محصولات Quectel
فروشگاه سیسوگ مجموعه ای متمرکز بر تکنولوژی های مبتنی بر IOT و ماژول های M2M نظیر GSM، GPS، LTE، NB-IOT، WiFi، BT و ... جایی که با تعامل فنی و سازنده، بهترین راهکارها انتخاب می شوند. برو به فروشگاه سیسوگ
سیسوگ فروم | محلی برای پاسخ پرسش‌های شما
دغدغه همیشگی فعالان تخصصی هر حوزه وجود بستری برای گفتگو و پرسش و پاسخ است. سیسوگ فروم یک انجمن آنلاین است که بصورت تخصصی امکان بحث، گفتگو و پرسش و پاسخ در حوزه الکترونیک را فراهم می‌کند. پرسش در سیسوگ فرم
سیکار | اولین مرجع متن باز ECU در ایران
بررسی و ارائه اطلاعات مربوط به ECU (واحد کنترل الکترونیکی) و نرم‌افزارهای متن باز مرتبط با آن برو به سیکار
سیسوگ با افتخار فضایی برای اشتراک گذاری دانش شماست. برای ما مقاله بنویسید.