که در آن  معرف ضرایب لاگرانژ ­است.
برای حل این معادله از روش Natural Orthogonal Complement استفاده می­ شود. برای این منظور ابتدا دسته­ای از مختصات تعمیم یافته مستقل را انتخاب می­کنیم.

( اینجا فقط تکه ای از متن فایل پایان نامه درج شده است. برای خرید متن کامل پایان نامه با فرمت ورد می توانید به سایت feko.ir مراجعه نمایید و کلمه کلیدی مورد نظرتان را جستجو نمایید. )

(۲-۱۱)
در این صورت رابطه­ زیر برقرار است:
(۲-۱۲)
از رابطه­ (۲-۷) خواهیم داشت:
(۲-۱۳)
بنابراین با ضرب  به سمت چپ معادله­ حرکت، ضرایب لاگرانژ حذف شده و خواهیم داشت:
(۲-۱۴)
۲-۴- صحه­گذاری دینامیک ربات
به منظور تاًیید مدل دینامیکی از جعبه ابزار 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 از سوی دیگر به صورت همزمان و در یک محیط با ورودی­های یکسان اجرا می­شوند تا اختلاف میان خروجی­ها مبنای سنجش صحت مدل باشد.
۳-۴-۱- نحوه­ انتخاب ورودی­ ها و چگونگی اعمال آنها