Transcript Mobile Robot Kinematics سینماتیک دکتر سعید شیری قیداری کتاب 3 & فصل Amirkabir University of Technology Computer Engineering & Information Technology Department.

Mobile Robot
Kinematics
‫سینماتیک‬
‫دکتر سعید شیری قیداری‬
‫ کتاب‬3 ‫& فصل‬
Amirkabir University of Technology
Computer Engineering & Information Technology Department
‫سینماتیک‬
‫‪Kinematics‬‬
‫‪ ‬معموال مطالعه سینماتیک نقطه شروع استاندارد درس روباتیک‬
‫است‪.‬‬
‫‪ ‬سینماتیک عالوه بر روباتیک در سایر علوم نظیر گرافیک و‬
‫انیمیشن نیز کاربرد دارد‪.‬‬
‫سینماتیک ‪:‬‬
‫‪ ‬عبارت است از مطالعه تحلیلی هندسه حرکت روبات ‪:‬‬
‫‪ ‬نسبت به یک محور مختصات ثابت‬
‫‪ ‬نسبت به نیرو ها و یا گشتاوری که باعث حرکت میشوند‬
‫بحث سینماتیک برای روباتهای صنعتی‬
Forward kinematics
z
Given joint variables
q  (q1 , q2 , q3 , q4 , q5 , q6 ,qn )
Y  ( x, y, z, O, A, T )
End-effector position and orientation, -Formula?
y
x
‫بحث سینماتیک برای روباتهای صنعتی‬
‫‪‬‬
‫‪‬‬
‫در بحث سینماتیک مستقیم روباتهای‬
‫صنعتی روشی ارائه میشود که موقعیت‬
‫و جهت لینک ها و ابزار روبات را‬
‫بصورت تابعی از متغیرهای مفصل ها‬
‫نسبت به محور مرجع محاسبه میکند‪.‬‬
‫برای اینکار فریمهای مختصاتی به هر‬
‫بخش از مکانیزم روبات وصل شده و‬
‫سپس ارتباط بین این محورها بیان میشود‪.‬‬
‫‪A 3-DOF Manipulator Arm‬‬
‫بحث سینماتیک معکوس برای روباتهای‬
‫صنعتی‬
Given a desired position (P) &
orientation (R) of the endeffector
q  (q1 , q2 ,qn )
Find the joint variables which
can bring the robot the
desired configuration
z
y
x
Inverse Kinematics
K-1
(q 1 …
q n)
(x,y,z,qx,qy,qz)
‫بحث سینماتیک معکوس برای روباتهای‬
‫صنعتی‬
‫‪‬‬
‫سخت تر از سینماتیک مستقیم است زیرا‪:‬‬
‫‪ ‬معادالت حاصله غیر خطی بوده و از‬
‫اینرو همیشه راه حل سیستماتیکی برای‬
‫حل آنها بصورت ‪ closed form‬وجود‬
‫ندارد‪.‬‬
‫‪ ‬راه حل منحصر بفرد نیست‪.‬‬
‫‪ ‬راه حل بستگی به مشخصات روبات دارد‬
‫‪2‬‬
‫!‪solutions‬‬
Forward and Inverse
Kinematics
Joint 2
q2
q1
Joint 1
Joint 3
yt
Tool Coordinate Frame
zt
Link 1
z11
zw
World (Base) Coordinate Frame
xw
Link Space
n variables
(q 1 …
qn)
Forward K
Tool Space
6 variables
Inverse K
(x,y,z,qx,qy,qz)
‫سینماتیک روباتهای متحرک‬
‫‪‬‬
‫مطالعه سینماتیک روباتهای متحرک در دو زمینه الزم است‪:‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫طراحی مناسب روبات برای انجام عمل مورد نظر‬
‫نوشتن نرم افزار کنترلی روبات ساخته شده‬
‫یک اختالف مهم بین روبات متحرک و روبات صنعتی در‬
‫اندازه گیری موقعیت است ‪.‬روبات صنعتی در یک نقطه ثابت‬
‫است لذا میتوان موقعیت آنرا نسبت به این نقطه ثابت اندازه‬
‫گرفت‪.‬‬
‫کنترل موقعیت یک روبات‬
‫‪‬‬
‫برای کنترل موقعیت یک روبات الزم است تا موارد زیر را‬
‫بدانیم‪:‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫مدل سینماتیکی ‪/‬دینامیکی روبات‬
‫مدل تعامل بین چرخ و زمین‬
‫تعریفی از حرکت مورد نیاز‪:‬‬
‫‪‬‬
‫‪‬‬
‫کنترل سرعت ‪-‬کنترل موقعیت‬
‫قانون کنترلی که نیازمندیهای الزم را برآورده میکند‪.‬‬
‫تبدیل مختصات‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫هنگام بررسی موقعیت یک روبات معموال عالقمند هستیم که‬
‫موقعیت آنرا نسبت به یک محور مختصات مرجع بسنجیم ‪.‬‬
‫در حالیکه حرکت اجزای یک روبوت نظیر چرخها‪ ،‬محل‬
‫قرار گرفتن سنسورها‪ ،‬و غیره نسبت به بدنه روبات اندازه‬
‫گیری میشوند‪.‬‬
‫از اینرو الزم است تا موقعیت روبات ویا اهداف دیگر را که‬
‫نسبت به موقعیت روبات اندازه گیری میشوند نسبت به محور‬
‫مختصات مرجه بیان نمود‪.‬‬
‫برای اینکار نیاز به تبدیل مختصات خواهد بود‬
‫ضرب داخلی‬
3
Let x and y be arbitrary vectors in R and q be
the angle from x to y , then
y
x  y  x y cosq
q
Where q is the angle between the vectors and
is the norm.
X.Y=0 if X is perpendicular to Y.
x
x
‫ضرب داخلی‬
Properties of orthonormal coordinate frame
Mutually perpendicular
 
i  j 0
 
i k  0
 
k j 0
Unit vectors

| i | 1

| j | 1

| k | 1
k
j
O
i
‫نمایش نقطه و بردار‬
Point represented in OXYZ:
Pxyz  [ px , py , pz ]
T
z
Vector represented in OXYZ:
P

Pxyz  px i x  p y jy  pz k z
y
k
O, O’
j
i
x
‫تبدیل مختصات‬


Reference coordinate frame
OXYZ
Body-attached frame O’uvw
z
P
Point represented in O’uvw:

Puvw  pu i u  pv jv  pwk w
w
v
O, O’
Two frames coincide ==>
y
u
pu  px pv  py pw  pz
x
‫تبدیل مختصات‬
‫‪z‬‬
‫‪‬‬
‫‪P‬‬
‫‪y‬‬
‫‪v‬‬
‫‪x‬‬
‫‪u‬‬
‫‪w‬‬
‫حالتی که فقط دوران داشته باشیم‬
‫‪‬‬
‫‪Pxyz  px i x  p y jy  pz k z‬‬
‫‪‬‬
‫‪Puvw  pu i u  pv jv  pwk w‬‬
‫‪Pxyz  RPuvw‬‬
‫چگونه میتوان مختصات نقاط ایندو محور مختصات را به هم ربط داد؟‬
‫حالت دوران ساده‬
Px , Py , and Pz represent the projections of P onto
OX, OY, OZ axes, respectively
Since
P  pu i u  pv jv  pwk w
px  i x  P  i x  i u pu  i x  jv pv  i x  k w pw
py  jy  P  jy  i u pu  jy  jv pv  jy  k w pw
pz  k z  P  k z  i u pu  k z  jv pv  k z  k w pw
Basic Rotation Matrix
 px   i x  i u
 p    j i
 y  y u
 p z  k z  i u
i x  jv
j y  jv
k z  jv
i x  k w   pu 



j y  k w   pv 
k z  k w   pw 
Rotation about x-axis with q
0 
1 0
Rot( x,q )  0 Cq  Sq 
0 Sq Cq 

z
w
P
v
q
u
x
y
Basic Rotation Matrix
Rotation about x axis with q
0
 px  1
 p   0 cosq
 y 
 pz  0 sin q
  pu 



 sin q   pv 
cosq   pw 
0
z
w
P
p x  pu
q
p y  pv cosq  pw sin q
p z  pv sin q  pw cosq
v
u
x
y
Basic Rotation Matrices
Rotation about x-axis with q
Rotation about y-axis with q
Rotation about z-axis with q
Pxyz  RPuvw
1 0
Rot( x,q )  0 Cq
0 Sq
 Cq
Rot( y, q )   0
 Sq
Cq
Rot( z , q )   Sq
 0
0 
 Sq 
Cq 
Sq 
1 0 
0 Cq 
0
 Sq
Cq
0
0
0
1
Basic Rotation Matrix
 ix  iu
R   jy  i u
k z  i u
i x  jv
j y  jv
k z  jv
ix  k w 
jy  k w 
k z  k w 
Pxyz  RPuvw
Obtain the coordinate of Puvw from the coordinate of Pxyz
Dot products are commutative!
 pu   i u  i x
 p    j i
 v  v x
 pw  k w  i x
i u  jy
jv  j y
k w  jy
i u  k z   px 
 
jv  k z   p y 
k w  k z   p z 
QR  RT R  R1R  I3
Puvw  QPxyz
Q  R1  RT
<== 3X3 identity matrix
Example
A point auvw  (4,3,2)is attached to a rotating frame,
the frame rotates 60 degree about the OZ axis
of the reference frame. Find the coordinates of
the point relative to the reference frame after
the rotation.
a xyz  Rot( z ,60)auvw
 0.5  0.866 0 4  0.598
 0.866
0.5
0 3   4.964 
 0
0
1 2  2 
Composite Rotation Matrix
A sequence of finite rotations
Matrix multiplications do not commute
Rules:
If rotating coordinate O-U-V-W is rotating about
principal axis of OXYZ frame, then pre-multiply the
previous (resultant) rotation matrix with an
appropriate basic rotation matrix
If rotating coordinate OUVW is rotating about its own
principal axes, then post-multiply the previous
(resultant) rotation matrix with an appropriate basic
rotation matrix
Example
Find the rotation matrix for the following
operations:
Rotation about OY axis
Rotationq about OW axis
Rotation about OU axis
Answer...
R  Rot( y,  ) I 3 Rot( w,q ) Rot(u,  )
 C 0
  0 1
- S 0
 CCq
  Sq
 SCq
S  Cq  Sq 0 1 0
0 
0   Sq Cq 0 0 C  S 
C   0
0
1 0 S C 
SS  CSqC CSqS  SC 

CqC
 CqS

SSqC  CS CC  SSqS 
Pre-multiply if rotate about the OXYZ axes
Post-multiply if rotate about the OUVW axes
Coordinate Transformations
• position vector of P
in {B} is transformed
to position vector of P
in {A}
• description of {B} as
seen from an observer
in {A}
Rotation of {B} with respect to {A}
Translation of the origin of {B} with respect to origin of {A}
Coordinate Transformations

Two special cases
A P A
B P A o'
r  RB r  r


1. Translation only
Axes of {B} and {A} are
parallelA
RB  1


2. Rotation only
Origins of {B} and {A} are
coincidentA o '
r 0
Homogeneous Representation
• Coordinate transformation from {B} to {A}
A P A
B P A o'
r  RB r  r
 A r P   A RB A r o '   B r P 




1  1 
 1   013
• Homogeneous transformation matrix
 RB
TB  
 013
A
A
r   R33

1   0
A o'
P31 

1 
Scaling
Rotation
matrix
Position
vector
Homogeneous Transformation
Special cases

1. Translation
 I 33
A
TB  
013
A o'
r 

1 
A

RB
A
TB  
 013
031 

1 
2. Rotation
Example
Translation along z-axis with h:
1
0
Trans( z , h)  
0

0
z
0
1
0
0
0
0
1
0
0
0
h

1
 x  1
 y  0
 
 z  0
  
 1  0
z

0
1
0
0
0  pu   pu 
0  pv   pv 

h   pw   pw  h 
  

1  1   1 
0
0
1
0
P
P
w
y
w
y
v
v
O,
O’
u
x
h
O,
O’
u
x
Example
Rotation about the x-axis by
1 0
0 Cq
Rot( x,q )  
0 Sq

0 0
0
 Sq
Cq
0

0
0
0

1
 x  1 0
 y  0 Cq
 
 z  0 Sq
  
 1  0 0
z
w
P
u
x
v
y
0
 Sq
Cq
0
0  pu 
0  pv 
0  p w 
 
1  1 
Example
Find the homogeneous transformation matrix (T)
for the following operation:

Rotation about OX axis
T ranslation of a alongOX axis
T ranslation of d alongOZ axis
Rotationof q about OZ axis
Answer :
Cq
 Sq

 0

 0
 Sq
Cq
0
0
0
0
1
0
0 1
0 0
0  0

1  0
T  Tz ,q Tz ,d Tx,aTx, I 44
0
1
0
0
0
0
1
0
0  1
0  0
d  0

1  0
0
1
0
0
0
0
1
0
a  1 0
0  0 C
0  0 S

1  0 0
0
 S
C
0
0
0
0

1
Homogeneous Representation
A frame in space (geometric
interpretation)
 R33 P31 
F 

1 
 0
 nx
n
F  y
 nz

0
sx
sy
sz
0
ax
ay
az
0

P( px , py , pz )
z
a
px 
p y 
pz 

1
Principal axis n w.r.t. the reference coordinate system
s
n
y
x
Homogeneous Transformation
Translation
1
0
Fnew  
0

0
 nx
n
 y
 nz

0

a
0 0 d x   nx s x
1 0 d y  n y s y


0 1 d z  nz s z
 
0 0 1 0 0
sx ax px  d x 
s y a y p y  d y 
sz az pz  d z 

0 0
1 
ax
ay
az
0
px 
p y 
pz 

1
z
s
n
a
s
n
Fnew  Trans(d x , d y , d z )  Fold
y
Homogeneous Transformation
Composite Homogeneous Transformation Matrix
z1
z2
y2
z0
0
y1
A1
y0
1
A2
x1
x0
i 1
?
A
i
0
x2
A2 0A11 A2
Transformation matrix for
adjacent coordinate frames
Chain product of successive
coordinate transformation matrices
Homogeneous Transformations
With respect to the universal frame , the position
of is:
Homogeneous Transformations
This equation can be reduced in a very 
handsome way by introducing homogenous
coordinates and transformation.
Minimal Representation of Orientation
 R33
F 
 0
P31 

1 
‫سینماتیک روباتهای متحرک‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫هدف ‪:‬توصیف عملکرد مکانیکی روبات به منظور طراحی و کنترل‬
‫سینماتیک رواتهای متحرک شبیه به روباتهای صنعتی است با این تفاوت‬
‫که روبات متحرک میتواند آزادانه در محیط حرکت نماید‪.‬‬
‫عالوه بر آن روش مستقیمی برا یاندازه گیری موقعیت روبات نیست و‬
‫موفعیت را باید در طول زمان با انتگرال گیری از حرکت های انجام‬
‫شده بدست آورد‪.‬‬
‫اینکار منجر به ایجاد خطا در اندازه گیری خواهد شد‪.‬‬
‫مقابله با این مسئله یکی ازمباحث جدی در روبات های متحرک میباشد‪.‬‬
‫برای فهم حرکت روبات باید از محدودیتهائی که چرخها بر سر راه‬
‫حرکت ایجاد میکنند شروع نمود‪.‬‬
‫نشان دادن موقعیت روبات‬
‫‪‬‬
‫‪‬‬
‫فرض میشود که روبات یک جسم صلب باشد که روی چرخها‬
‫قرار گرفته و بر روی یک صفحه حرکت میکند‪.‬‬
‫موقعیت روبات را میتوان با دو متغیر ‪ x,y‬در صفحه و یک‬
‫متغیر ‪ q‬برای نشان دادن جهت آن مشخص نمود‬
‫‪ x‬‬
‫‪   y ‬‬
‫‪q ‬‬
‫نشان دادن موقعیت روبات‬
‫‪‬‬
‫برای اینکار از دو فریم مختصات استفاده میشود ‪:‬‬
‫‪‬‬
‫یکی فریم مختصات مرجع و‬
‫دیگری فریم محلی که بر روی روبات قرار دارد‬
‫‪‬‬
‫فریم مرجع بصورت زیر نشان دادن میشود‬
‫‪‬‬
‫} ‪O : { X I ,Y I‬‬
‫‪‬‬
‫فریم روبات بصورت زیر نشان داده میشود‬
‫}‪{ X R ,Y R‬‬
‫نشان دادن موقعیت روبات‬
‫‪ ‬اگر اختالف زاویه بین فریم روبات و فریم مرجع برابر با ‪q‬‬
‫بوده و مبدا فریم روبات )‪ (P‬درنقطه ‪ x,y‬نسبت به فریم مرجع‬
‫قرار داشته باشد در اینصورت فریم روبات نسبت به فریم‬
‫مرجع بصورت زیر نشان داده میشود‪.‬‬
‫‪ x‬‬
‫‪ I   y ‬‬
‫‪q ‬‬
‫نشان دادن موقعیت روبات‬
‫‪‬‬
‫برای توصیف حرکت روبات الزم است تا حرکت در راستای‬
‫فریم مرجع به حرکت در فریم روبات نگاشت شود ‪.‬برای‬
‫اینکار از ماتریس دوران زیر استفاده میشود‪.‬‬
‫‪ R  R(q ) I‬‬
‫‪‬‬
‫‪0‬‬
‫‪0‬‬
‫‪1‬‬
‫‪Sq‬‬
‫‪Cq‬‬
‫‪0‬‬
‫‪ Cq‬‬
‫‪Rot(q )   Sq‬‬
‫‪ 0‬‬
‫برای مثال برای حالت شکل قبل داریم‬
‫‪ 0 1 0‬‬
‫‪Rot( 2 )   1 0 0‬‬
‫‪ 0 0 1‬‬
‫مدل سینماتیک مستقیم‬
‫‪‬‬
‫هدف ‪:‬اگر سرعت چرخهای روبات و ابعاد هندسی آنرا را‬
‫داشته باشیم حرکت روبات چگونه خواهد بود؟‬
‫روبات شکل زیر دارای دو چرخ هر یک با شعاع ‪ r‬بوده و باندازه ‪ l‬از نقطه ‪ P‬که در وسط بین دو چرخ قرار‬
‫دارد فاصله دارد ‪.‬سرعت چرخها برابر با ‪ q1,q2‬میباشد ‪.‬در اینصورت مدل سینماتیک مستقیم روبات بصورت‬
‫زیر خواهد بود‪:‬‬
‫مثال‬
‫محاسبه سینماتیک مستقیم‬
‫‪‬‬
‫‪‬‬
‫برای محاسبه حرکت روبات در فریم مرجع میتوان تاثیر هر یک از‬
‫چرخها در فریم روبات را محاسبه کرده و نتیجه را به فریم مرجع منتقل‬
‫نمائیم‪.‬‬
‫اگر فرض کنیم که روبات در جهت محور ‪ X‬در حرکت باشد سرعت‬
‫حرکت نقطه ‪ P‬بازای چرخش هر یک از چرخها بصورت زیر خواهد‬
‫بود‪:‬‬
‫‪.‬‬
‫‪.‬‬
‫‪ (1 / 2)r 2‬‬
‫‪‬‬
‫‪r2‬‬
‫‪x‬‬
‫‪.‬‬
‫‪.‬‬
‫‪ (1 / 2)r 1‬‬
‫‪r1‬‬
‫‪x‬‬
‫در یک روبات با درایو دیفرانسیلی میتوان این دو مولفه را با هم جمع‬
‫نمود‬
‫محاسبه سینماتیک مستقیم‬
‫‪‬‬
‫‪‬‬
‫مولفه ‪ y‬این حرکت صفر خواهد بود‪.‬‬
‫برای محاسبه مولفه ‪ q‬در نظر داشته باشید اگر فقط چرخ راست دوران‬
‫کند روبات حول چرخ چپ به چرخش در خواهد آمد‪.‬سرعت زاویه ای‬
‫نقطه ‪ P‬برابر خواهد بود با‪:‬‬
‫‪r‬‬
‫‪.‬‬
‫‪1‬‬
‫‪‬‬
‫‪2l‬‬
‫‪‬‬
‫به همین ترتیب برای چرخ چپ داریم‬
‫‪1 ‬‬
‫‪2‬‬
‫‪ r‬‬
‫‪2l‬‬
‫با ترکیب این روابط مدل‬
‫سینماتیکی روبات بصورت زیر‬
‫خواهد بود‬
‫‪2 ‬‬
‫‪ r r ‬‬
‫‪1‬‬
‫‪2‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫‪2 ‬‬
‫‪ 2‬‬
‫‪1‬‬
‫‪ R(q ) ‬‬
‫‪0‬‬
‫‪‬‬
‫‪ r  r ‬‬
‫‪2‬‬
‫‪ 1‬‬
‫‪‬‬
‫‪2l ‬‬
‫‪ 2l‬‬
‫‪I‬‬
‫‪‬‬
‫محاسبه سینماتیک مستقیم‬
‫‪‬‬
‫در این رابطه مقدار ماتریس دوران از رابطه زیر بدست میاید‪.‬‬
‫‪0‬‬
‫‪0‬‬
‫‪1‬‬
‫‪‬‬
‫‪ Sq‬‬
‫‪Cq‬‬
‫‪0‬‬
‫‪Cq‬‬
‫‪Rot(q ) 1   Sq‬‬
‫‪ 0‬‬
‫در حالت کلی برای توصیف حرکت روبات مجبور خواهیم بود‬
‫تا محدودیت هائی که هر چرخ بر حرکت اعمل میکند را نیز‬
‫در نظر بگیریم‪.‬‬