Mobile Robot Kinematics سینماتیک دکتر سعید شیری قیداری کتاب 3 & فصل Amirkabir University of Technology Computer Engineering & Information Technology Department.
Download
Report
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 R1R I3
Puvw QPxyz
Q R1 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
CCq
Sq
SCq
S Cq Sq 0 1 0
0
0 Sq Cq 0 0 C S
C 0
0
1 0 S C
SS CSqC CSqS SC
CqC
CqS
SSqC CS CC SSqS
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 013
• Homogeneous transformation matrix
RB
TB
013
A
A
r R33
1 0
A o'
P31
1
Scaling
Rotation
matrix
Position
vector
Homogeneous Transformation
Special cases
1. Translation
I 33
A
TB
013
A o'
r
1
A
RB
A
TB
013
031
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 44
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)
R33 P31
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
R33
F
0
P31
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
در حالت کلی برای توصیف حرکت روبات مجبور خواهیم بود
تا محدودیت هائی که هر چرخ بر حرکت اعمل میکند را نیز
در نظر بگیریم.