معادلات حرکت ربات – به زبان ساده

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

خوشبختانه می‌توان متلب، پایتون و سایر زبان‌های برنامه‌نویسی را در محاسبات نمادین برای استخراج این معادلات به کار برد. در این آموزش، معادلات حرکت یک بازوی رباتیک دو لینکی (یا همان آونگ دوگانه) را با استفاده از متلب به دست می‌آوریم.

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

مدل یک بازوی ساده در شکل زیر نشان داده شده است. این پیکربندی را می‌توان به عنوان آونگ دوگانه نیز در نظر گرفت.

فرض می‌کنیم طول لینک‌های بدون جرم $$l _ 1$$ و $$l _ 2$$ بوده و به انتهای هریک از آن‌ها، به ترتیب، جرم‌های $$m_ 1$$ و $$m _ 2$$ متصل شده باشد. زوایای $$\theta _ 1$$ و $$\theta _ 2$$ مربوط به این لینک‌ها در شکل زیر نشان داده شده است. فرض می‌کنیم برای هر مفصل یک موتور وجود داشته باشد.

زاویه‌های $$\theta _ 1$$ و $$\theta _ 2$$ برای توصیف مناسب سیستم انتخاب شده‌اند. زیرا در این نوع نمایش، به تعداد حداقل متغیرهای مستقل برای توصیف کامل سیستم نیاز است. می‌توان از موقعیت‌های $$x$$ و $$y$$ جرم‌ها نیز برای به دست آوردن معادلات حرکت استفاده کرد. البته در این روش، محدودیت‌های بیشتری را باید روی سیستم اعمال کرد تا تضمین شود طول لینک‌ها ثابت می‌ماند. چنین محدودیت‌ها و قیودی با اعمال «محدودیت‌های پفافی» (Pfaffin constraints) اعمال می‌شوند.

البته، در این آموزش به این موضوع نمی‌پردازیم. در سیستم‌های رباتیک پیچیده یا شبیه‌سازی‌های بیومکانیکی، موقعیت بردارها را می‌توان با استفاده از «نظریه پیچ‌ها» (Screw Theory) یا پارامترهای «دناویت-هارتنبرگ» (Denavit–Hartenberg Parameters) به دست آورد. با استفاده از دستگاه مختصات مناسب، می‌توان معادلات حرکت را بسیار ساده کرد. البته این کار شاید همیشه ممکن نباشد. برای مثال، مدل راه رفتن انسان با کف پای قوسی را نمی‌توان با چشم‌پوشی از قیود موقعیت و سرعت بین پا و زمین مدل کرد.

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

محاسبه موقعیت و سرعت

اولین گام محاسبه موقعیت همه اجرام سیستم است. در اینجا، موقعیت جرم‌ها را نسبت به مرکزی که به پایه بازو الصاق شده و برحسب زوایای $$\theta _ 1$$ و $$\theta _ 2$$ محاسبه می‌کنیم:

$$\large \left [ \begin {array} { c } P _ { 1 , x } \\ P _ { 1 , y } \end {array} \right ] = \left [ \begin {array} { c } l _ 1 \cos ( \theta _ 1 ) \\ l _ 1 \sin ( \theta _ 1 ) \end {array} \right ]$$

و

$$\large \left [ \begin {array} { c } P _ { 2 , x } \\ P _ { 2 , y } \end {array} \right ] = \left [ \begin {array} { c } l _ 1 \cos ( \theta _ 1 ) + l _ 2 \cos ( \theta _ 1 + \theta _ 2 ) \\ l _ 1 \sin ( \theta _ 1 ) + l _ 2 \sin ( \theta _ 1 + \theta _ 2 ) \end {array} \right ] .$$

سپس سرعت را با استفاده از معادلات بالا به دست می‌آوریم:

$$\large v = \frac { d P } { d t } = \frac { \partial P } { \partial \theta _ 1 } \dot { \theta } _ 1 + \frac { \partial P } { \partial \theta _ 2 } \dot { \theta } _ 2$$

محاسبه انرژی جنبشی و انرژی پتانسیل سیستم

در این بخش، انرژی جنبشی و پتانسیل سیستم را محاسبه می‌کنیم. انرژی جنبشی به صورت زیر است:

$$\large KE = \frac { 1 } { 2 } m _ 1 v _ 1 ^ T v _ 1 + \frac { 1 } { 2 } m _ 2 v _ 2 ^ T v _ 2$$

انرژی پتانسیل نیز به صورت زیر تعریف می‌شود:

$$\large PE = m _ 1 g P _ { 1 , y } + m _ 2 g P _ { 2 , y }$$

استخراج معادلات حرکت ربات

برای به دست آوردن معادلات حرکت، ابتدا لاگرانژین $$L$$ را تشکیل می‌دهیم:

$$\large L = KE - PE$$

سپس، معادلات حرکت با کمک رابطه زیر به دست می‌آیند:

$$\large \frac { d } { d t } \left ( \frac { \partial L } { \partial \dot { q } } \right ) - \frac { \partial L } { \partial q } = \tau$$

که در آن، $$q = [ \theta_1, \theta_2]^T$$ بردار موقعیت و سرعت زاویه‌ای است و $$\tau$$ بردار گشتاورهای اعمالی توسط موتورها در دو مفصل است. پس از مرتب‌سازی جملات، معادلات حرکت را می‌توان به صورت زیر نوشت:

$$\large D ( q ) \ddot { q } + C ( q , \dot { q } ) \dot { q } + G ( q ) = \tau$$

یا

$$\large \ddot { q } = D ( q ) ^ { - 1 } ( \tau - C ( q , \dot { q } ) \dot { q } - G ( q ) )$$

می‌توانیم معادله بالا را به صورت زیر بازنویسی کنیم:

$$\large \ddot { q } = \alpha ( q , \dot { q } ) + \beta ( q ) \tau$$

که در آن، $$\alpha ( q , \dot { q } ) = D ( q ) ^ { - 1 } ( - C ( q , \dot { q } ) \dot { q } - G ( q ) )$$ و $$\beta ( q ) = D ( q ) ^ { - 1 }$$.

این فرم معادله در کنترل بسیاری از سیستم‌های غیرخطی معمول است. دسته‌های خاص سیستم‌های بالا که در آن‌ها ورودی یک جمله خطی اضافه دارد، «فرم کنترل-افاین» (Control-Affine Form) نامیده می‌شوند.

خطی‌سازی معادلات حرکت ربات

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

فیلم آموزش مقدماتی STM32 میکروکنترلر ARM + پروژه های عملی + گواهینامه در فرادرس

کلیک کنید

این مورد، در مثال‌هایی که پایدارسازی سیستم در برابر آشفتگی‌های خارجی مورد نظر است، رواج دارد. برای مثال، اگر بخواهیم ربات با دو مفصل را در موقعیت مشخص $$q_0$$ نگه داریم، آنگاه باید فرض کنیم که نتیجه آشفتگی‌های خارجی کوچک است. بنابراین، می‌توانیم از سری تیلور به صورت ماتریسی حول نقطه پایدار $$q_0$$ استفاده کنیم. سیستم در $$q _ 0$$، $$\dot {q } = 0$$ و $$\ddot { q} = 0$$ در تعادل است. بنابراین، خواهیم داشت:

$$\large 0 = \alpha ( q _ 0 , 0 ) + \beta ( q _ 0 ) \tau _ 0 ,$$

یا

$$\large \tau _ 0 = - \beta ( q _ 0 ) ^ { - 1 } \alpha ( q _ 0 , 0 ) .$$

از بسط تیلور برای یک تابع دومتغیره استفاده می‌کنیم:

$$\large f ( x + \delta x , y + \delta y ) \approx f ( x , y ) + \left . \frac { \partial f } { \partial x } \right | _ { ( x , y ) } \delta x + \left . \frac { \partial f } { \partial y } \right | _ { ( x , y ) } \delta y .$$

با استفاده از معادله بالا حول $$q = q_0$$ و $$\dot{q} = 0$$، داریم:

$$\large \begin {align*}\delta \ddot { q } & = \alpha ( q _ 0 , 0 ) + \left . \frac { \partial \alpha } { \partial q } \right | _ { ( q _ 0 , 0 ) } \delta q + \left . \frac { \partial \alpha } { \partial q } \right | _ { ( q _ 0 , 0 ) } \delta \dot { q } \\ & + \left ( \beta ( q _ 0 , 0 ) + \left . \frac { \partial \beta } { \partial q } \right | _ { ( q _ 0 , 0 ) } \delta q \right ) \left ( \tau _ 0 + \delta \tau _ 0 \right ) + H.O.T \end {align*}$$

با بازآرایی و صرف‌نظر کردن از جملات مرتبه بالاتر، خواهیم داشت:

$$\large \delta \ddot { q } = \underbrace { \left . \frac { \partial \alpha } { \partial q } \right | _ { ( q _ 0 , 0 ) } \delta q + \left . \frac { \partial \alpha } { \partial q } \right | _ { ( q _ 0 , 0 ) } \delta \dot { q } + \left . \frac { \partial \beta } { \partial q } \right | _ { ( q _ 0 , 0 ) } \delta q \tau _ 0 } _ { \text {Linear in } \delta q \text { and } \delta \dot { q } } + \underbrace { \beta ( q _ 0 ) \delta \tau _ 0 } _ { \text {Linear in } \delta \tau _ 0 }$$

بنابراین، معادلات حرکت را می‌توان با یک سیستم دینامیکی خطی ساده‌تر حول $$q _ 0$$ و $$\dot { q } = 0$$ بیان کرد.

در مواردی که سیستم افاین نیست، بسط تیلور را می‌توان به صورت زیر مورد استفاده قرار داد:

$$\large \ddot q _ 0 + \delta \ddot q = f ( q _ 0 + \delta q , u _ 0 + \delta u ) \approx f ( q _ 0 , u _ 0 ) + \left. \frac { \partial f } { \partial q } \right | _ { ( q _ 0 , u _ 0 ) } \delta q + \left . \frac { \partial f } { \partial u } \right | _ { ( q _ 0 , u _ 0 ) } \delta u .$$

با در نظر گرفتن $$\ddot q_0 = f(q_0 ,u_0)$$، داریم:

$$\large \delta \ddot q = \left . \frac { \partial f } { \partial q } \right | _ { ( q _ 0 , u _ 0 ) } \delta q + \left . \frac { \partial f } { \partial u } \right | _ { ( q _ 0 , u _ 0 ) } \delta u .$$

پیاده‌سازی معادلات حرکت ربات در متلب

محاسبات مربوط به معادلات حرکت ربات را نمی‌توانیم به صورت دستی انجام دهیم. به همین دلیل، از جعبه‌ابزار نمادین متلب برای انجام محاسبات بالا استفاده می‌کنیم. برنامه زیر، معادلات حرکت ربات را ارائه می‌کند.

این برنامه، به توابع و کدهایی نیاز دارد که می‌توانید آن‌ها را از این لینک [+] دانلود کنید.

اگر این مطلب برای شما مفید بوده است، آموزش‌های زیر نیز به شما پیشنهاد می‌شوند:

• مجموعه آموزش‌‌های رباتیک (Robotics)
• آموزش مبانی ربات های برنامه پذیر
• مجموعه آموزش‌‌های میکروکنترلر
• آموزش رباتیک و ربات های سری
• کنترل تطبیقی — از صفر تا صد
• کنترل پیش بین مدل (MPC) — راهنمای جامع
• کنترل فازی — از صفر تا صد

^^