که در آن معرف ضرایب لاگرانژ است.
برای حل این معادله از روش Natural Orthogonal Complement استفاده می شود. برای این منظور ابتدا دستهای از مختصات تعمیم یافته مستقل را انتخاب میکنیم.
(۲-۱۱)
در این صورت رابطه زیر برقرار است:
(۲-۱۲)
از رابطه (۲-۷) خواهیم داشت:
(۲-۱۳)
بنابراین با ضرب به سمت چپ معادله حرکت، ضرایب لاگرانژ حذف شده و خواهیم داشت:
(۲-۱۴)
۲-۴- صحهگذاری دینامیک ربات
به منظور تاًیید مدل دینامیکی از جعبه ابزار SimMechanics استفاده شده است. در این راستا مدل ساخته شده در محیط Simulink از یکسو و مدل استخراجی از روابط لاگرانژ از سوی دیگر و با ورودیهای مشابه اجرا شده و خروجیها مقایسه میشوند.
۲-۴-۱- نحوه انتخاب ورودی ها و چگونگی اعمال آنها
انتخاب ورودی ها باید به گونه ای باشد که یک حرکت کلی را ایجاد نماید. در این راستا مسیرهائی با سرعت و شتاب متغیر به شکل زیر انتخاب شده اند:
(۲-۱۵)
مسیرهای انتخابی همان مسیرهای پایدار طراحی شده اند که به دلیل پیچیدگی رابطه با دقت بالائی به وسیله توابع چندجملهای فیت[۶] زده شده اند. برای سایر مفاصل نیز توابعی چندجملهای به صورت مشابه انتخاب میشوند. برای حل دینامیک معکوس، این مقادیر به صورت ورودی به توابع ماتریسهای جرمی، گرانش و ماتریس اجزای غیرخطی و کوریولیس داده میشوند و بر اساس معادله حرکت مقداری عددی برای گشتاورها به دست می آید. همین ورودی ها به مدل ساخته شده در محیط Simulink نیز اعمال میشوند.
۲-۴-۲- نتایج صحهگذاری
جعبه ابزار SimMechanics این امکان را میدهد تا با نصب سنسور[۷]هائی به هر مفصل گشتاور عکسالعملی را اندازه گیری نمائیم. مقادیر خروجی از مدل استخراجی از روابط لاگرانژ با مقادیر خروجی سنسورها مقایسه می شود. نمودار (۲-۳) میزان خطای موجود را نشان میدهد.
شکل (۲-۳-الف). خطای موجود در مفصل مچ پای راست شکل (۲-۳-ب). خطای موجود در سایر مفاصل
شکل (۲-۳-الف) خطای صحهگذاری در مفصل مچ پای راست را نشان میدهد. این مقدار خطا در واقع صفر است. شکل (۲-۳-ب) خطای صحهگذاری در سایر مفاصل را نشان میدهد. این میزان خطا، اصولاً خطای محاسباتی نرمافزار است، به طوری که میتوان آن را معادل صفر واقعی دانست. بنابراین میتوان گفت صحت مدل با دقت بالائی مورد تایید است. شکل (۲-۴) گشتاور ایجاد شده در زانو در اثر طی نمودن مسیر مطلوب ذکر شده در رابطه (۲-۱۵) را به همراه شتاب و سرعت مسیر مورد نظر را نشان می دهد.
شکل (۲-۴-الف). گشتاور عکسالعملی روی زانوی پای راست شکل (۲-۴-ب). شتاب زاویهای زانو
شکل (۲-۴-ج). سرعت زاویهای زانو
جهت مقایسه مقادیر گشتاورهای مفاصل با نمونههای ساخته شده نموداری از مقادیر گشتاورهای مفصلی در ربات HRSP (ساخت Institute Mihailo Pupin) در حال راه رفتن عادی شکل (۲-۵) آمده است. این نمودار از آدرس اینترنتی زیر که متعلق به همین موًسسه است، گرفته شده است.
http://www.imp.bg.ac.yu/RnDProfile/ROBOTIKA/HRSP.htm
شکل (۲-۵). گشتاورهای مفصلی در ربات HRSP
در صفحه بعد تصویری از محیط Simulink که در آن صحهگذاری دینامیک انجام شده، آمده است. مدل ربات ساخته شده به کمک جعبه ابزار SimMechanics در بالا و توابع حامل ماتریسهای جرمی، گرانش و سرعتها در پائین آمده است.
فصل سوم
مدل سازی دینامیک حرکت سه بعدی
۳-۱- مقدمه
روند تحقیق انجام گرفته در مورد مدل سهبعدی مشابه مدل دوبعدی است با این تفاوت کلی که در هر مرحله کار با پیچیدگی و حجم مضاعفی انجام می شود. پس از معرفی ساختار ربات به چگونگی محاسبهی دینامیک و تاًیید آن میپردازیم.
۳-۲- معرفی ساختار ربات
ربات مورد نظر دارای ۱۶ درجه آزادی است. قوزک پا، زانو و آرنج با یک درجه آزادی و کتف و ران در اتصال به بالاتنه با دو درجه آزادی مدل شده است. همچنین یک درجه آزادی برای دوران بالاتنه حول محور خود در نظر گرفته شده است. مختصات تعمیم یافته به صورت زیرانتخاب می شود:
(۳-۱)
که در آن به ترتیب تا زوایای پای راست، زاویه کمر حول محور خود، تا زوایای پای چپ، تا زوایای دست راست، تا زوایای دست چپ و زاویه کف پای راست با زمین است.
۳-۳- دینامیک ربات
برای تشکیل رابطه انرژی جنبشی ، شناخت ماتریسهای دوران مربوط به هر مفصل مورد نیاز است. همچنین میبایست موقعیت مراکز جرم اعضا را نسبت به مبدا اینرسی توصیف کنیم. بدین منظور و بر اساس روش DH ماتریسهای دوران را استخراج میکنیم.
شکل (۳-۱). ساختار ربات سهبعدی
شکل (۳-۲). زوایای مچ و زانو و اعضای مربوط به آن
(۳-۲)
که در آن معرف ماتریس دوران کف پای راست نسبت به اینرسی و نشان دهنده ماتریس دوران مچ پای راست نسبت به کف پا میباشد. با داشتن ماتریسهای دوران میتوان موقعیت مراکز جرمی را توصیف نمود. به عنوان مثال این مقدار برای ساق پای راست چنین است:
(۳-۳)
ماتریس ژاکوبی مربوطه نیز با بهره گرفتن از رابطه زیر به دست می آید:
(۳-۴)
که درآن ، ماتریس ژاکوبی سرعت خطی، ، ماتریس ژاکوبی سرعت دورانی و توصیف محور z دومین درجه آزادی در راستای محور اینرسی است. (j شمارنده ستون ماتریس است). با استخراج ماتریسهای ژاکوبی تمامی اعضا ماتریس جرم با رابطه زیر به دست می آید:
(۳-۵)
در مرحله بعد ماتریس عوامل غیرخطی و کوریولیس و ماتریس گرانش و به دنبال آن رابطه گشتاورهای اعمالی با روابط زیر به دست می آید:
(۳-۶)
نیروی تعمیم یافته مفصل i ام میباشد. در اینجا به دلیل حجم بالای محاسبات و طولانی بودن روابط از ذکر جزئیات صرفنظر میکنیم.
۳-۴- صحهگذاری دینامیک
به منظور اعمال کنترل روی ربات و پیش از آن صحهگذاری دینامیک امری ضروری است. بدین منظور با بهره گرفتن از جعبه ابزار SimMechanics مدل استخراجی از روابط از یک سو و مدل ساخته شده در فضای Simulink از سوی دیگر به صورت همزمان و در یک محیط با ورودیهای یکسان اجرا میشوند تا اختلاف میان خروجیها مبنای سنجش صحت مدل باشد.
۳-۴-۱- نحوه انتخاب ورودی ها و چگونگی اعمال آنها