آیا میدانید مکانیزم عمل خودرانها (segway) چگونه است ؟؟؟

[imijoon]

ممنون .

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

Complementary رو با میکروکنترلر راه اندازی کنم

و اگه فرض کنیم یه سنسور زاویه مثل adxl202 دارم که حساسیت اون 312mv/g هست و یه ژیروسکوپ LISY300AL دارم که حساسیت اون 3.3mv/deg/sec است محاسبات چطوریه ؟ من شروع میکنم لطف کنید هر جا مشکلی داشتم و یا کم آوردم کمکم کنید .

1) اول از همه اینکه فرض میکنیم کنترلر یه ADC داره 10 بیتی . و ما میخوایم خروجی دو سنسور زیر رو بدیم به اون و ببینیم به ازای چند deg و چند deg/sec یک بیت به ADC افزوده میشه و بطور کلی دقت در چه حده ؟ برای سنسور زاویه داریم 312mv/g و بنابراین 312mv/90deg پس داریم (5/1024) * (90/0.312) که برابر است با 1.4deg/LSB . پس این یعنی به ازای هر 1.4 درجه گردش سنسور حول محورش یک واحد به ADC میکرو افزوده میشه و این یعنی دقت یا Gain سنسور زاویه با این میکرو 1.4 است . ( البته همینجا یه مشکلی هست که من یه جا دیدم بجای 312mv/90deg محاسبه کرده بود 312mv/1rad که میشه 312mv/57.3deg که نفهمیدم چرا همچین کاری انجام داده لطفا کمک کنید . )
برای ژیروسکوپ هم به همین ترتیب داریم 3.3mv/deg/sec بنابراین داریم (5/1024) * (1/0.0033) که برابر است با 1.48deg/sec/LSB . پس این یعنی به ازای هر 1.48 درجه گردش در ثانیه ژیروسکوپ حول محورش یک واحد به ADC میکرو افزوده میشه .

ادامه دارد .....

[imijoon]

البته هنوز این مشکلی که توی پست قبلی نوشتم رو دارم :

(

البته همینجا یه مشکلی هست که من یه جا دیدم بجای 312mv/90deg محاسبه کرده بود 312mv/1rad که میشه 312mv/57.3deg که نفهمیدم چرا همچین کاری انجام داده لطفا کمک کنید . ) .

ولی اگرفرض کنیم این مشکل وجود نداره یا بعدا حل بشه تا اینجا دو Gain داریم که یکی 1.4 (که اسمش رو میزاریم Acc-g) هست و یکی 1.48 (که اسمش رو میزاریم Gyro-g) هست . این Gain ها به ما میگه که اگر مثلا ADC سنسور زاویه ، مقدار 12 رو نشون داد ، زاویه سنسور 16.8 درجه ست .

دو مقدار دیگه لازم داریم که یکی مقدار offset سنسور زاویه و دیگری مقدار offset ژیروسکوپه . این مقادیر رو کاملا باید آزمایشی بدست بیاریم . ابتدا سنسور زاویه رو بصورت کاملا افقی (موازی زمین) قرار میدیم و خروجی سنسور رو میگیریم که میشه offset سنسور زاویه و برای ژیروسکوپ هم کافیه سنسور رو کاملا ثابت و بدون حرکت (بدون گردش حول محور) قرار بدیم و مقدار offset ژیروسکوپ رو بدست می آوریم .

ادامه دارد .... .

[farzadsw]

فیلتر کالمن اونقدر ها هم پیچیده نیست که نیاز به چند تا پردازنده باشه، با همین AVR های 8بیتی هم میشه فیلتر کالمن رو اجرا کرد اما با نرخ حدود 10-20 هرتز که برای این کار (تعادل سگوی) مناسب نیست. اما فیلتر complementary تو همین avr تا 100-200 هرتز قابل اجراست.

اون قضیه 312mv/1rad یا اشتباه لپی بوده، یا اینکه حساسیت خود سنسور (که تو دیتا شیت هست) یه مقدار بیشتری هست (حدود 500 میلی ولت بر g).

موفق باشید.

[imijoon]

اون قضیه 312mv/1rad یا اشتباه لپی بوده، یا اینکه حساسیت خود سنسور (که تو دیتا شیت هست) یه مقدار بیشتری هست (حدود 500 میلی ولت بر g).

موفق باشید.

با این عکس توضیحات داده و همین قضیه رادیان رو گفته .

[imijoon]

یه سری توضیحات عمومی و کلی :

ما باید با استفاده از سنسور زاویه ، زاویه رو بدست بیاریم . سپس با استفاده از ژیروسکوپ هم زاویه رو بدست بیاریم . همون طور که دوستان هم توضیح دادن این کار باعث میشه که ثبات بیشتر باشه و زاویه بدست آمده قابل اطمینان تر .
سنسور زاویه یه عیب بد داره و اون اینکه سنسور زاویه (از اونجا که هم شتاب استاتیک (جاذبه زمین) و هم شتاب دینامیک (حرکت افقی) رو اندازه میگیره) ، در حالتی که زاویه segway هیچ تغییری نمیکنه و فقط segway در حال حرکت هست نمی تونه تغییر زاویه رو از شتاب حرکت تمایز بده و در خروجی اون یه سری نویز با فرکانس بالا وجود داره . از همین رو خروجی اون رو از یک فیلتر پائین گذر عبور میدیم .
ژیروسکوپ هم یه ایراد عمده داره . از اونجا که خروجی ژیرسکوپ (درجه / ثانیه) ست برای بدست آوردن درجه باید از خروجی اون در زمان های مشخص و یکسان انتگرال بگیریم . و انتگرال گیری به این صورت انجام میگیره : angle = angle + (gyro*DT) . بطور ساده یعنی در هر چند میلی ثانیه زاویه بدست آمده قبلی رو با خروجی ژیروسکوپ جمع کنیم (طبق قوانین ساده شتاب در فیزیک ) . همین قضیه باعث مشکل میشه چون خروجی ژیروسکوپ در بهترین شرایط هم صفر نیست و با انتگرال گیری برای بدست آوردن درجه ، مقدار درجه ای که بدست میاد لحظه به لحظه از مقدار واقعی فاصله میگیره که به این فاصله Drift میگن . از همین رو خروجی ژیروسکوپ بعد از انتگرال گیری از یک فیلتر بالا گذر عبور میدیم .

با جمع کردن مقدار دو خروجی ، زاویه segway بدست می آید .

نتیجه میشه :




ادامه دارد .... .

[farzadsw]

خب با توجه به این نمودار مشخص هست که منظور از 312mv/1rad یا همون 0.3v/57.3degree رنج (محدوده) خروجی سنسور نبوده. بلکه توی این شرایط یک رابطه خطی (تقریبی) بین زاویه و ولتاژ سنسور درنظر گرفته و این عدد هم شیب خط هست. همونطور که مشخصه این رابطه خطی تو زوایای کم (زیر 30 درجه) قابل قبول هست ولی تو زوایای بیشتر خطا زیاد میشه.
این کار رو کرده تا محاسبات کمتر بشه (نیازی به عملیات مثلثاتی نیست)که در عمل برای شرایط کاری سگوی قابل قبول هست چون زاویه بیشتر از 20-30 درجه نمیشه.

[imijoon]

مقادیر بدست آمده تا کنون عبارتند از Acc-offset و Gyro-offset و Acc-gain و Gyro-gain که مقادیر offset بصورت آزمایشی بدست میان و مقادیر Gain با استفاده از دیتاشیت سنسورها . برای شروع برنامه نویسی فیلتر Complimentary ، پس از پیکره بندی ADC ها و اختصاص دادن هر سنسور به یک ADC باید دو متغیر از نوع signed یعنی علامت دار ( به علتی که بعدا مشخص میشه ) تعریف کنیم . برای مقدار دهی اونا باید به شکل مقابل عمل کنیم : ( Acc= acc_ADC - Acc_offset ) و ( Gyro= Gyro_offset - gyro_ADC ) .

ادامه دارد .... .

[imijoon]

خوب در این حالت از اونجا که offset ها رو کم کردیم قائدتا هنگامی که segway کاملا ثابت است متغییر های Gyro و Acc هر دو برابر 0 هستند . حالا با حرکت و یا چرخش segway سنسور ها آشکار کرده و با دادن به ADC ها مقادیر متغییرهای Gyro و Acc تغییر میکنند . از اونجا که مقادیر متغییرها اعداد صحیح مرتبط با زاویه و (درجه/ثانیه) هستند حالا باید این مقادیر صحیح رو به زاویه و (درجه/ثانیه) تبدیل کنیم . این کار را با ضرب کردن Gain هایی که قبلا بدست آورده ایم انجام می دهیم . به این ترتیب

G = Gyro * Gyro_gain و A = Acc * Acc_gain .

ادامه دارد .... .

[imijoon]

شکل موج سنسورهای ما با تغییرات زاویه segway به صورت زیر است ( قرمز مقدار سنسور شتاب و زرد مقدار ژیروسکوپ ) .



همونطور که قبلا هم گفتم و در شکل موج ها هم مشاهده می کنید این مقادیر هر کدام به تنهایی معایبی دارند و اونها رو باید به فیلتر Complimentary ( شکل زیر ) اعمال کنیم تا شکل موج خروجی تلفیقی از هر دو باشه و تا حد مطلوبی بدون عیب .



خوب مقادیر ورودی های ما همون متغییرهای G و A هستند . از متغییر G باید در بازه DT انتگرال گرفته شود . DT چیه ؟ این مقدار کاملا اختیاریه و مثلا اگر شما هر حلقه برنامه انتگرال گیری را مثلا 50ms تعریف کنید ، DT=0.05 .

خوب با استفاده از رابطه زیر کل فیلتر بالا اجرا میشه .

angle = N * (angle + G * DT) + (1 - N) * A

در رابطه فوق N ، ضریبی است که با استفاده از آن فیلتر های بالاگذر و پایین گذر اجرا می شوند ( معمولا این ضریب نزدیک 1 انتخاب می شود - مثلا 0.8 ) . - البته من تئوری نحوه اجرای فیلتر های بالا گذر و پائین گذر رو خوب نفهمیدم . از دوستان هر کسی اگه بتونه از لحاظ تئوری یه خورده در مورد نحوه فیلتر کردن دو مقدار تنها با دو ضریب رو توضیح بده ممنون میشم - . با استفاده از عبارت angle + G * DT از مقدار G در بازه DT انتگرال گیری می شود . پس از جمع کردن دو مقدار به ازای ورودی های شکل بالا در خروجی شکل زیر را داریم .



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

ادامه دارد .... .

[hamid9543]

سلام
لطف میکنید و پست را ادامش بدین