معادلات حرکت ربات — به زبان ساده
در آموزشهای قبلی مجله فرادرس، با رباتها و مفاهیم مربوط به آنها آشنا شدیم. در این آموزش، از روش لاگرانژ برای استخراج معادلات حرکت ربات با کمک جعبه ابزار نمادین متلب استفاده میکنیم. مدلهای ریاضیاتی نیز برای تقریب آنچه که سیستم اصلی انجام میدهد ارائه شدهاند. همه مدلهای ریاضیاتی با فرضیاتی برای سادهسازی همراه هستند و معمولاً این فرضیات در نمایش دینامیک سیستم رواج دارند. در نتیجه، روشهای دستی و نوشتن روی کاغذ برای به دست آوردن مدلهای نمایش دهنده دینامیک سیستمهای پیچیده کافی نیستند. تصویر زیر دو جمله از ماتریس اینرسی یا لختی یک ربات انساننما را نشان میدهد.
خوشبختانه میتوان متلب، پایتون و سایر زبانهای برنامهنویسی را در محاسبات نمادین برای استخراج این معادلات به کار برد. در این آموزش، معادلات حرکت یک بازوی رباتیک دو لینکی (یا همان آونگ دوگانه) را با استفاده از متلب به دست میآوریم.
مدل بازوی رباتیک دو لینکی
مدل یک بازوی ساده در شکل زیر نشان داده شده است. این پیکربندی را میتوان به عنوان آونگ دوگانه نیز در نظر گرفت.
فرض میکنیم طول لینکهای بدون جرم $$ 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) نامیده میشوند.
خطیسازی معادلات حرکت ربات
هرچند همه سیستمهای موجود در طبیعت غیرخطی هستند، اما میتوان سیستم را با یک سیستم خطی از معادلات تحت فرضیات مشخصی خطی کرد. یکی از فرضیات اصلی این است که اندازه موقعیت و سرعت سیستم کم است.
این مورد، در مثالهایی که پایدارسازی سیستم در برابر آشفتگیهای خارجی مورد نظر است، رواج دارد. برای مثال، اگر بخواهیم ربات با دو مفصل را در موقعیت مشخص $$ 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 . $$
پیادهسازی معادلات حرکت ربات در متلب
محاسبات مربوط به معادلات حرکت ربات را نمیتوانیم به صورت دستی انجام دهیم. به همین دلیل، از جعبهابزار نمادین متلب برای انجام محاسبات بالا استفاده میکنیم. برنامه زیر، معادلات حرکت ربات را ارائه میکند.
این برنامه، به توابع و کدهایی نیاز دارد که میتوانید آنها را از این لینک [+] دانلود کنید.
1clc
2close all
3clear all
4
5addpath Screws
6addpath fcn_support
7% Defining symbols
8syms m1 m2 l1 l2 q1 q2 dq1 dq2 ddq1 ddq2 tau1 tau2 g real
9syms q10 q20
10
11% Position vectors
12
13
14
15P1 = [ l1 * cos(q1);
16 l1 * sin(q1)];
17
18P2 = [ l1 * cos(q1) + l2 * cos(q1+q2);
19 l1 * sin(q1)+ l2 * sin(q1+q2)];
20
21q_v = [q1;q2];
22dq_v = [dq1;dq2];
23%
24
25% Taking derivative to compute velocities
26V1 = get_vel(P1 ,q_v,dq_v);
27V2 =get_vel(P2,q_v,dq_v);
28
29% Computing Kinetic energy and potential energy
30KE1 =simplify(1/2*m1*V1'*V1);
31KE2 =simplify(1/2*m2*V2'*V2);
32
33PE1 = m1*g*P1(2);
34PE2 = m2*g*P2(2);
35
36% Define Lagrangian
37KE_total = KE1 + KE2;
38PE_total = PE1 + PE2;
39
40
41[D,C,G] = get_mat(KE_total, PE_total, q_v,dq_v);
42D = simplify(D);
43C = simplify(C);
44G = simplify(G);
45
46
47% Now express this in the form of dx/dt = f(x,u)
48x = [q1;q2;dq1;dq2]; % Vector of state space
49ddq0 = [0;0]; % Vector of SS joint accelerations
50x0 = [q10;q20;0;0]; % Vector of SS joint angles and velocites
51tau_v = [tau1;tau2]; % Vector of torques
52% Function to calculate Linearized representation
53[A_lin,B_lin] = linearize_DCG(D,C,G,x,tau_v,x0,ddq0);
54A_lin = simplify(A_lin)
55B_lin = simplify(B_lin)
اگر این مطلب برای شما مفید بوده است، آموزشهای زیر نیز به شما پیشنهاد میشوند:
- مجموعه آموزشهای رباتیک (Robotics)
- آموزش مبانی ربات های برنامه پذیر
- مجموعه آموزشهای میکروکنترلر
- آموزش رباتیک و ربات های سری
- کنترل تطبیقی — از صفر تا صد
- کنترل پیش بین مدل (MPC) — راهنمای جامع
- کنترل فازی — از صفر تا صد
^^
با عرض سلام و خسته نباشید خدمت استاد گرامی شرمنده که مزاحمتون شدم ،آقای مهندس این راه حلی که طی کردین در واقع خطی سازی فیدبک هستش یا نه؟
سلام.
همانگونه که در متن آموزش نیز اشاره شده، از سری تیلور برای خطیسازی استفاده شده است. برای آشنایی با خطیسازی، میتوانید به آموزش «خطی سازی سیستم های غیرخطی — از صفر تا صد (+ دانلود فیلم آموزش گام به گام)» مراجعه کنید. روش خطیسازی فیدبکی یکی از روشهای کنترل سیستمها غیرخطی است که در مطالب آینده مجله فرادس، آموزشی را درباره آن منتشر خواهیم کرد.
از همراهیتان با مجله فرادرس، بسیار خوشحالیم.
با سلام .ممنون از آموزش خوبتون.دو سوال داشتم.اول اینکه متغیر های سیستم (مختصه تعمیم یافته /درجات آزادی ) این سیستم در کدها 2 بود در صورتی که باید 4 باشه. همچنین امکان ترسیم انیمیشن نداشت.ایا انیمیشن ترسیم شده مربوط به این کد نیست؟ممنون میشم اگه پیوست کنید.
سلام.
خروجی کد مورد نظر معادلات خطی شده ربات و در واقع ماتریسهای A و B معادلات حالت آن است. انیمیشن داخل متن ارتباطی به این کد ندارد.
از اینکه با مجله فرادرس همراه هستید، خوشحالیم.