SM _ FARAJI
SM _ FARAJI
خواندن ۶ دقیقه·۴ ماه پیش

الگوریتم AHRS (پیدا کردن وضعیت کوادکوپتر)

طبق تعاریف رایج AHRS(attitude and heading reference system) شامل سنسورهای سه محوره است که وضعیت (attitude) یک جسم را در دستگاه NED (North, East, Down) مشخص می کند این زوایا Roll، Pitch و Yaw هستند.

بطور معمول از سنسورهای میکرو الکترومکانیکی (MEMS) بدلیل سادگی و ارزانی استفاده می شود درحالیکه در گذشته از ژایروسکوپ های مکانیکی بسیار بزرگ‌تر و پیچیده استفاده می‌شد که با پیشرفت تکنولوژی جای خود را به MEMS دادند.

در AHRS از سنسورهای شتاب سنج(Accelerometer)، ژایروسکوپ (Gyroscope) و مغناطیس سنج(Magnetometer) بطور معمول استفاده میشود که مورد آخر بصورت دلخواه و وابسته به الگوریتم است. فرق AHRS با IMU(inertial measurement unit) است که یک برد محاسبه گر در کنار سنسورها می تواند تخمینی از وضعیت وسیله ارائه دهد.

ورودی و خروجی الگوریتم AHRS
ورودی و خروجی الگوریتم AHRS

بررسی سنسور ها:

  • سنسور شتاب‌سنج(Accelerometer)

این سنسور مجموع شتابهای وارده بر جسم (بعلاوه شتاب جاذبه) را در سه محور متعامد اندازه گیری می کند در مسئله AHRS شتاب جسم نزدیک صفر فرض می شود و فرض میگردد که تنها شتاب جاذبه اندازه گیری میگردد. در برخی از الگوریتم های ارائه شده در این زمینه از این تکنیک برای محاسبه اولیه مقادیر Roll و Pitch سیستم استفاده میگردد.

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

  • سنسور ژایروسکوپ(Gyroscope)

این سنسور نرخ تغییرات زاویه سیستم را در سه محور محاسبه میکند که در حالت ایده آل می توان با داشتن زوایای اولیه جسم و نرخ تغییرات زاویه در طول زمان، وضعیت سیستم را تخمین زد که بدلیل وجود دریفت درطول زمان همچنین بایاس این کار بسیار سخت است. بعلاوه اینکه با توجه به شرایط محیطی این ویژگی های سنسور میتواند تغییر کند.

  • سنسور مغناطیس‌سنج(Magnetometer)

این سنسور میدان مغناطیسی زمین را در سه محور خود اندازه گیری می کند و بطور معمول برای محاسبه زاویه Yaw استفاده می گردد و نقشی در محاسبه Roll و Pitch ندارد. البته تخمین این زاویه با استفاده از مگنتومتر دارای چالش هایی است برای مثال میدان مغناطیسی زمین بسیار ضعیف است و حضور اجسام فلزی، مغناطیسی و سیم های حامل جریان الکتریکی اندازه گیری ها را دچار اعوجاج می کند. همچنین میدان مغناطیسی زمین بصورت یکنواخت نیست و این خوب سبب ایجاد پدیده هایی مثل Hard-iron و Soft-iron میشود که باید مقادیر محور ها را قبل از محاسبه Yaw کالیبره کرد. مدل کردن میدان مغناطیسی در یک ناحیه را میتوان با 10 پارامتر انجام داد که 6 تای آن عناصر یک ماتریس 3 در 3 متقارن، برای حذف اثر پدیده Soft-iron و سه تای آن برای حذف آفست Hard-iron و آخری نمایانگر شدت میدان مغناطیسی در آن نقطه است.

مدل مغناطیسی زمین
مدل مغناطیسی زمین

اگر میدان مغناطیسی دارای اعوجاج آهن-سخت و آهن-نرم نباشد تمام مقادیر هر دو محور دلخواه آن، اگر نسبت بهم رسم شوند درون یک دایره به مرکز مبدا قرار میگیرد:

عدم اعوجاج مغناطیسی
عدم اعوجاج مغناطیسی

اعوجاج آهن-سخت باعث تغییر مبدا میگردد:

وجود اعوجاج آهن-سخت
وجود اعوجاج آهن-سخت

و در آخر اثر هر دو اعوجاج را میتوان بصورت زیر نمایش داد:

دارای هردو اعوجاج
دارای هردو اعوجاج

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

کالیبراسیون مقادیر محورها
کالیبراسیون مقادیر محورها

خلاصه ای از مزایا و معایب هر سنسور در جدول زیر آمده است:

جمع بندی بررسی سنسورها
جمع بندی بررسی سنسورها

انواع راه حل های AHRS بصورت زیر دسته بندی شده اند:

انواع الگوریتم های AHRS
انواع الگوریتم های AHRS

شبیه سازی یک مقاله در رابطه با AHRS

برای درک بیشتر این مساله یک مقاله در این حوزه در یک بستر مناسب شبیه سازی می شود مقاله انتخاب شده Concept of AHRS Algorithm Designed for Platform Independent Imu Attitude Alignment است که یک الگوریتم پیشنهادی برای بدست آوردن وضعیت سیستم با استفاده داده سنسورهای شتابسنج، ژایروسکوپ و مگنتومتر ارائه داده است داده های این سنسورها بصورت آنلاین با استفاده از الگوریتم فیلتر کالمن با هم ترکیب شده و وضعیت سیستم تخمین زده میشود.

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

سنسور IMU مقادیر شتاب خطی، نرخ زاویه ای و القای مغناطیسی زمین را در سه محور دو به دو عمود بر هم را اندازه گیری میکند همچنین سه محور متناظر هم سه سنسور با هم موازی بوده و مبدا هر سه یک نقطه است. این راهکار دستگاه بدنی IMU را معرفی میکند که فریم بدنی نام دارد.محاسبات AHRS از فیلتر کالمن خطی بهره میگیرد و مقاله از داده های سنسور گوشی Samsung galaxy s5 استفاده کرده است. اما در شبیه سازی از نمونه داده های آماده که قبلا از سنسور یک گوشی گرفته شده استفاده میشود و الگوریتم بصورت کد در متلب پیاده سازی میشود.

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

ماتریس H نیز از ژاکوبین ماتریس بالا بدست می آید:

همچنین ماتریس R نیز بصورت قطری تعریف میگردد:

حالت های سیستم نیز شامل زوایای اویلر و دریفت ژایروسکوپ مرتبط با هر زوایه اویلر است:

ماتریس انتقال حالت A نیز بگونه ای تعریف میگردد که اثر دریفت را در پیش بینی زاویه حذف کند:

ماتریس B نیز که در مقادیر ژایرو که بعنوان ورودی تعریف شده اند بصورت زیر تعریف میگردد:

ماتریس Q نیز بصورت قطری و با ابعاد 6 در 6 تعیین میگردد.

کالمن بصورت دو بخش پیاده سازی میگردد یکی بخش اول که به ان اصطلاحا Predict گفته میشود و یک تخمین اولیه از حالت های سیستم میدهد و در بخش Update با رسیدن مقادیر حالت های سیستم را بروزرسانی میکند.

حال باید الگوریتم را در متلب پیاده سازی کنیم. داده خام سنسورها نیز بصورت زیر است:

کد پیاده سازی در گیت‌هاب قرار داده شده است.

در پایان تخمین زوایای اویلر بدین گونه شد:

اگر با دقت مشاهده کنیم در بخش هایی که اغتشاش زیاد است یا شتاب خارجی به سیستم وارد شده الگوریتم تخمین بهتری ارائه داده است:

در آینده نحوه پیاده سازی این الگوریتم در بسترها سخت افزاری رایج بررسی خواهد شد پس با ما همراه باشید:

آدرس گیت‌هاب
آدرس یوتیوب
آدرس تلگرام
آدرس آپارات
میدان مغناطیسیشبیه سازیکوادکوپترimu
شاید از این پست‌ها خوشتان بیاید