Xem mẫu
- 38
CHƯƠNG III:
PHÂN TÍCH ĐỘNG HỌC ROBOT SONG SONG 3RPS
3.1 Bài toán phân tích vị trí
3.1.1 Các phương trình liên kết cho Robot song song 3RPS tổng quát
Trên hình 3.1 mô tả sơ đồ động học của Robot song song 3RPS. Cấu trúc các
khâu của Robot này được mô tả trong mục 1.6
z1
B3
z0 α3
P
Bàn máy động B2 y1
B1
z3 α2
A3
x1 α1 z2
z1 x3
x2
O
Đế cố định
y0
A1
A2
x0 x1
Hình 3.1: Cấu trúc chấp hành song song 3RPS
a) Quan hệ về hình học và các hệ toạ độ
Do cấu trúc Robot đảm bảo tính hợp lý nên các chân AiBi ⊥ Zi (các trục
quay). Gọi O và P là trọng tâm của hai tam giác A1A2A3 và B1B2B3, ta đặt lên
đó các hệ toạ độ như trên hình 3.1:
- Ox0y0z0 : Hệ toạ độ cố định.
- Pxyz : Hệ toạ độ động gắn liền với bàn máy động.
- Aixiyizi, (i = 1,2,3): Hệ động gắn với chân thứ i. Với xi≡ Ai Bi và zi≡ trục
quay, trục yi lập với xi và zi một hệ quy chiếu thuận.
Ta đưa thêm vào 3 toạ độ suy rộng αi (i = 1,2,3) là góc hợp bởi trục z0 và trục
xi như hình vẽ.
38
- 39
Ngoài ra ta sử dụng các ký hiệu:
- ai : Véc tơ đại số chứa các toạ độ của điểm Ai trên hệ cố định, ai= [ai1 , ai 2 , ai 3 ]T
bi: Véc tơ đại số chứa các toạ độ của điểm Bi trên hệ cố định.
- Bbi : Véc tơ đại số chứa các toạ độ của điểm Bi trên hệ cố động, B
bi=
[ ] T
b ix , b iy , b iz .
- p: véc tơ đại số chứa các toạ độ của điểm P trên hệ cố định, p = [p1,p2,p3]T.
- di: độ dài chân thứ i (di = AiBi).
- bi và Bbi : Xác định được từ kết cấu hình học của robot.
- ARB : Ma trận cosin chỉ hướng của hệ động Pxyz so với hệ cố định Ox0y0z0.
- ARi : Ma trận cosin chỉ hướng của hệ động Aixiyizi so với hệ cố định
Ox0y0z0.
⎡t x vx wx ⎤ ⎡u ix v ix w ix ⎤
A
RB = ⎢ t y vy wy ⎥ , AR = ⎢ u v iy w iy ⎥ với: (i = 1,2,3). (3.1)
⎢ ⎥ i
⎢ iy ⎥
⎢t z
⎣ vz wz ⎥
⎦ ⎢ u iz
⎣ v iz w iz ⎥
⎦
Các phần tử của ma trận ARB và ma trân ARi tuỳ theo kết cấu của bàn đế cố
định, là hàm của góc αi.
Từ hình vẽ ta có:
OBi = OA i + A i Bi (i = 1,2,3) (3.2)
OBi = OP + PB i (i = 1,2,3) (3.3)
Ta có thể biểu diễn (3.2) và (3.3) dưới dạng sau:
⎡d i ⎤
bi = ai + ARi. ⎢ 0 ⎥ (i = 1,2,3) (3.4)
⎢ ⎥
⎢0⎥
⎣ ⎦
⎡d i ⎤
bi = p + ARB. ⎢ 0 ⎥ (i = 1,2,3) (3.5)
⎢ ⎥
⎢0⎥
⎣ ⎦
Kết hợp hai phương trình trên ta có :
⎡d i ⎤
p + RB. bi = ai + Ri. ⎢ 0 ⎥
A B A
(i=1,2,3) (3.6)
⎢ ⎥
⎢0⎥
⎣ ⎦
39
- 40
Hệ thức (3.6) gồm có 9 phương trình chứa các ẩn là toạ độ diểm P, độ dài các
chân di, các góc αi. Khi giải các bài toán động học thuận/ngược, ta đã biết 3
thông số p/di nên công việc còn lại chỉ chỉ giải bài toán 6 phương trình 6 ẩn,
các thông số còn lại như hướng của bài máy động, hướng của các chân A i Bi
sẽ được xác định khi đã biết các thông số này.
Hệ thức này có ý nghĩa rất quan trọng, qua đó ta có thể giải quyết bài toán
động học một cách trọn vẹn cả bài toán thuận và bài toán ngược, điều mà các
phương pháp trước đây chưa giải quyết được hay mới chỉ đưa ra cách giải
quyết bài toán thuận
b) Tính toán các phần tử của hệ thức (3.6)
Các ma trận cosin chỉ hướng ARi được xác định bởi các phép quay liên tiếp
dựa vào lý thuyết trình bày ở mục 2.2, ta thực hiện các phép biến đổi liên tiếp
để hệ toạ độ cố định Oxyz trùng với hệ Aixiyizi .
Z3
β3 Z2
β2
ϕ3
β1
ϕ2
Z1
X0
Y0
Hình 3.2
Áp dụng hệ thức trong tam giác ta có thể tính được độ dài các đường trung
bình, như vậy sẽ xác định được ϕ2 và ϕ3.
OA1 + OA 2 + A1A 2
2
OA1 + OA 3 + A1A 3
2 2 2
cos ϕ2 = 2 2
cos ϕ3 =
2OA1 OA 2 2OA1 OA 3
Ma trận ARi được xác định bởi các phép quay liên tiếp từ hệ cố định quanh
chính nó như sau:
A
Ri = Az1. Ax2. Az3. Ax4
Với Az1 , Ax2 , Az3 , Ax4 lần lượt là ma trận cosin chỉ hướng của các phép
quay sau:
∏
-Quay một góc ( − β1 ) quanh trục z0.
2
40
- 41
-Quay tiếp quanh trục x của hệ mới một góc (Π/2)
-Quay tiếp quanh trục z của hệ mới một góc (Π/2 + α1)
-Quay tiếp quanh trục x của hệ mới một góc Π
Vậy :
A
R1=
⎡ sin β1 − cos β1 0⎤ ⎡1 0 0 ⎤ ⎡− sin α1 − cos α1 0 ⎤ ⎡1 0 0⎤
⎢cos β sin β1 0⎥.⎢0 0 − 1⎥.⎢ cos α1 − sin α1 1⎥.⎢0 − 1 0 ⎥
⎢ 1
⎥⎢ ⎥⎢ ⎥⎢ ⎥
⎢ 0
⎣ 0 1 ⎥ ⎢0 1 0 ⎥ ⎢ 0
⎦⎣ ⎦⎣ 0 0⎥ ⎢0 0 − 1⎥
⎦⎣ ⎦
⎡ − sin β1 sin α1 sin β1 cos α1 − cos β1 ⎤
A
R1= ⎢− cos β1 sin α1 cos β1 cos α1 sin β1 ⎥
⎢ ⎥
⎢
⎣ cos α1 sin α1 0 ⎥ ⎦
Bằng cách tương tự ta cũng xác định được các ma trận AR2 và AR3
Ma trận AR2 được xác định bởi các phép quay sau:
∏
- Quay hệ cố định một góc ( − β 2 + ϕ 2 ) quanh trục z0.
2
- Quay tiếp quanh trục x của hệ mới một góc (Π/2).
- Quay tiếp quanh trục z của hệ mới một góc (Π/2 + α2).
- Quay tiếp quanh trục x của hệ mới một góc Π.
A
R2=
⎡ sin(β2 − ϕ2 ) − cos(β2 − ϕ2 ) 0⎤ ⎡1 0 0 ⎤ ⎡− sinα2 − cosα2 0⎤ ⎡1 0 0 ⎤
⎢cos(β − ϕ ) sin(β − ϕ ) 0⎥.⎢0 0 − 1⎥.⎢ cosα − sinα2 1⎥.⎢0 − 1 0 ⎥
⎢ 2 2 2 2 ⎥⎢ ⎥⎢ 2 ⎥⎢ ⎥
⎢
⎣ 0 0 1⎥ ⎢0 1 0 ⎥ ⎢ 0
⎦⎣ ⎦⎣ 0 0⎥ ⎢0 0 − 1⎥
⎦⎣ ⎦
⎡ − sin(β 2 − ϕ 2 ) sin α 2 sin(β 2 − ϕ 2 ) cos α 2 − cos(β 2 − ϕ 2 )⎤
A
R2= ⎢− cos(β 2 − ϕ 2 ) sin α 2 cos(β 2 − ϕ 2 ) cos α 2 sin(β 2 − ϕ 2 ) ⎥
⎢ ⎥
⎢
⎣ cos α 2 sin α 2 0 ⎥
⎦
Ma trận AR3 được xác định bởi các phép rơ quay sau:
∏
- Quay hệ cố định một góc ( − β 3 − ϕ 3 ) quanh trục z0.
2
- Quay tiếp quanh trục x của hệ mới một góc (Π/2).
41
- 42
- Quay tiếp quanh trục z của hệ mới một góc (Π/2 + α3).
- Quay tiếp quanh trục x của hệ mới một góc Π.
A
R3=
⎡ sin(β3 − ϕ3 ) − cos(β3 − ϕ3 ) 0⎤ ⎡1 0 0 ⎤ ⎡− sin α3 − cosα3 0⎤ ⎡1 0 0 ⎤
⎢cos(β − ϕ ) sin(β − ϕ ) 0⎥.⎢0 0 − 1⎥.⎢ cosα − sin α3 1⎥.⎢0 − 1 0 ⎥
⎢ 3 3 3 3 ⎥⎢ ⎥⎢ 3 ⎥⎢ ⎥
⎢
⎣ 0 0 1⎥ ⎢0 1 0 ⎥ ⎢ 0
⎦⎣ ⎦⎣ 0 0⎥ ⎢0 0 − 1⎥
⎦⎣ ⎦
⎡ − sin(β 3 − ϕ3 ) sin α 3 sin(β 3 − ϕ3 ) cos α 3 − cos(β 3 − ϕ3 )⎤
A
R3= ⎢− cos(β 3 − ϕ3 ) sin α 3 cos(β 3 − ϕ3 ) cos α 3 sin(β 3 − ϕ3 ) ⎥
⎢ ⎥
⎢
⎣ cos α 3 sin α 3 0 ⎥
⎦
Ta thấy các thành phần của các ma trận ARi (i=1,2,3) chỉ chưa các ẩn là các
góc αi còn ϕ2 , ϕ3 và βi (i=1,2,3) đã biết do kết cấu của robot.
Mặt khác dựa vào kết cấu của bàn di động B ta có :
(b1 – b2)T(b1- b2) = B1B 2
2
(3.7)
(b1 – b3)T(b1- b3) = B1B3
2
(3.8)
(b2 – b3)T(b2- b3) = B 2 B3
2
(3.9)
⎡d i ⎤
Với bi = ai + Ri. ⎢ 0 ⎥
A
(i = 1,2,3)
⎢ ⎥
⎢0⎥
⎣ ⎦
Như vậy các thành phần của hệ thức (3.6) được xác định thông qua 9 ẩn số
α1, α2, α3, d1, d2, d3, p1, p2, p3 .
Khi giải quyết bài toá động học thuận hay ngược, ta biết trước được 3 ẩn (d1,
d2, d3 hoặc p1, p2, p3 ). Công việc còn lại chỉ phải giải hệ 6 phương trình 6 ẩn
số.
3.1.2 Bài toán động học thuận
Bài toán động thuận là bài toán biết độ dài các chân di (i=1,2,3), ta phải tìm vị
trí của bàn máy động p và ma trận ARB.
Ta thay các giá trị di vào hệ (3.6), ta được hệ 6 phương trình với 6 ẩn số là:
α1, α2, α3,, p1, p2, p3 .
3.1.3 Bài toán động học ngược
42
- 43
Bài toán động ngựoc là bài toán biết vị trí của bàn máy di động pi (i=1,2,3), ta
phải tìm vị trí của các chân di (i=1,2,3) và các góc αi (i=1,2,3).
Ta thay các giá trị pi vào hệ (3.6), ta được hệ 6 phương trình với 6 ẩn số là:
α1, α2, α3, d1, d2, d3 .
3.1.4 Tính toán vị trí cho robot song song 3RPS cụ thể
Trên hình 3.3 mô tả sơ đồ động học của một con robot song song 3RPS, có đế
cố định A1A2A3 và bàn di động B1B2B3 là các tam giác đều. Độ dài PB1=h:
OA1= g. Khi các góc ϕ2= ϕ3 = 2π/3. Để đảm bảo tính hợp lý của kết cấu ta có
zi = AiBi , zi ⊥ OAi nên βi = π/2
z1
B3
z0 α3
P
Bàn máy động B2
B1 y1
z3 α2
A3
x1 α1 z2
z1 x3
x2
O
Đế cố định
y0
A1
A2
x0 x1
Hình 3.3: Cấu trúc chấp hành song song 3RPS
⎡ h ⎤ ⎡ h ⎤
⎢ −2 ⎥ ⎢ −2 ⎥
⎡h ⎤ ⎢ ⎥ ⎢ ⎥
Khi đó Bb1 = ⎢ 0 ⎥ ; B
b2 = ⎢h. 3 ⎥ ; B
b3 = ⎢ − h. 3 ⎥ (3.10)
⎢ ⎥ ⎢ 2 ⎥ ⎢ 2 ⎥
⎢0⎥
⎣ ⎦ ⎢ 0 ⎥ ⎢ 0 ⎥
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
43
- 44
⎡ g ⎤ ⎡ g ⎤
⎢ −2 ⎥ ⎢ −
2 ⎥
⎡g ⎤ ⎢ ⎥ ⎢ ⎥
a1= ⎢ 0 ⎥ ; B
b2 = ⎢g. 3 ⎥ ; B
b3 = ⎢− g. 3 ⎥ (3.11)
⎢ ⎥ ⎢ 2 ⎥ ⎢ 2 ⎥
⎢0 ⎥
⎣ ⎦ ⎢ 0 ⎥ ⎢ 0 ⎥
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
Sử dụng công thức (3.7), (3.8), (3.9) thay vào hê thức (3.6) :
Các ma trận AR1 trở thành:
⎡ − sin α1 cos α1 0⎤
A
R1 = ⎢ 0 0 1⎥ (3.12)
⎢ ⎥
⎢ cos α1
⎣ sin α1 0⎥
⎦
⎡ 1 1 3⎤
⎢ − sin α 21 − cos α 2 − ⎥
⎢ 2 2 2 ⎥
3 3 1
A
R2 = ⎢− sin α 2 cos α 2 − ⎥ (3.13)
⎢ 2 2 2 ⎥
⎢ cos α 2 sin α 2 0 ⎥
⎢ ⎥
⎣ ⎦
⎡ 1 1 3⎤
⎢ sin α 3 − cos α 3 ⎥
⎢ 2 2 2 ⎥
3 3 1
A
R3 = ⎢ sin α 3 − cos α 3 − ⎥ (3.14)
⎢ 2 2 2⎥
⎢ cos α 3 sin α 3 0 ⎥
⎢ ⎥
⎣ ⎦
Thay vào hệ thức (3.6) ta được:
+ Với i = 1:
p1 + ux .h = g – d1.sinα1
p2 + uy.h = 0
p3 + uz.h = d1cosα1
Suy ra:
g − p1 − d1 sin α1
ux =
h
44
- 45
p2
uy = −
h
d cos α1 − p 3
uz = 1
h
+ Với i = 2
h 3 g 1
p1 − u x . + v x .h = − + d 2 sin α 2
2 2 2 2
h 3 g 3 3
p1 − u y . + v y .h = − d 2 sin α 2 (3.16)
2 2 2 2
h 3
p 3 − u z . + v x .h = d 2 cos α 2
2 2
+ Với i = 3
h 3 g 1
p1 − u x . − v x .h = − + d 3 sin α 3
2 2 2 2
h 3 g 3 3
p1 − u y . − v y .h =− + d 3 sin α 3 (3.17)
2 2 2 2
h 3
p 3 − u z . − v x .h = d 3 cos α 3
2 2
Thực hiện các phép biến đổi (3.15), (3.16), (3.17) và thay các Bbi vào (3.7),
(3.8), (3.9) ta được:
1
3p1 = (d2sinα2 + d3sinα3) – d1sinα1
2
3
3p2 = − (d2sinα2 - d3sinα3)
2
3p3 = d1cosα1 + d2cosα2 – d3cosα3 (3.18)
3g2 – 3gd1sinα1 - 3gd2sinα2 +d1 d2sinα1.sinα2 – 2d1d2cosα1.cosα2+ d
2
1
+d 2 = 3h 2
2
3g2 – 3gd1sinα1 - 3gd3sinα3 +d1 d3sinα1.sinα3 – 2d1d3cosα1.cosα3+
d12 + d 3 = 3h 2
2
45
- 46
3g2 – 3gd2sinα2 - 3gd3sinα3 +d2d3sinα2.sinα3 – 2d2d3cosα2.cosα3+
d 3 + d 3 = 3h 2
2 2
Hệ (3.18) là dạng khai triển của hệ (3.6) áp dụng cho một robot song song
3RPS cụ thể, nhờ hệ này ta có thể áp dụng tính toán và giải bài toán động học
robot một cách trọn vẹn.
a) Bài toán động học thuận.
Bài toán động học thuận là bài toán biết độ dài các chân di (i = 1,2,3) ta phải
tìm vị trí của bàn máy động p và ma trận ARB.
Theo phần trên ta thay các giá trị di (i = 1,2,3) vào hệ (3.8) ta sẽ được hệ 6
phương trình 6 ẩn là α1, α2, α3, p1, p2, p3 .
Chú ý 3 phương trình sau của hệ (3.18) chỉ chứa di và αi nên việc giải hệ 6
phương trình 6 ẩn đơn giản còn có 3 phương trình 3 ẩn là αi. Sau đó thay các
giá trị của di và αi vào 3 phương trình đầu ta sẽ được các giá trị của p.
Các gía trị còn lại được tính bằng cách thay trực tiếp vào phương trình (3.15),
(3.16), (3.17).
b) Bài toán động học ngược
Bài toán động học ngược là bài toán biết vị trí bàn máy động p, ta phải tìm độ
dài các chân di (i = 1,2,3) và các góc αi (i=1,2,3).
Tương tự như cách làm đối với bài toán động học thuận ta thay các giá trị p
vào hệ (3.18), ta sẽ được hệ 6 phương trình với 6 ẩn số là α1, α2, α3, d1, d2,
d3 .
Các giá trị còn lại được tính bằng cách thay trực tiếp vào phương trình (3.15),
(3.16), (3.17).
3.2 Bài toán phân tích Jacobi
3.2.1 Ma trận Jacobi của robot song song không gian
Trong phần trước ta đã xây dựng được các điều kiện dàng buộc động học của
cơ cấu, các điều kiện này có dạng tổng quát :
f(x,q) = 0 (3.19)
Trong đó: q là biến khớp tác động.
x đặc trưng vị trí bàn máy di động.
f là hàm ẩn n chiếu theo q và x.
Đạo hàm (3.19) theo thời gian ta được:
∂f ∂f
x= q
& & (3.20)
∂x ∂q
46
- 47
∂f ∂f
Nếu đặt J x = và J q = thì hệ (3.20) có dạng :
∂x ∂q
J x x = J q .q
& & (3.21)
Từ đó ta có:
q = J q 1 .J x x
& −
& (3.22)
Ta có thể biểu diễn (3.22) dưới dạng sau:
q = J 1x
& & với J1 = J q-1 .J x (3.23)
Tương tự ta cũng có được :
x = J −1 .J q .q
& x
& (3.24)
x = J 2 .q
& & với J 2 = J −1 .J q
x
(3.25)
Trong đó J1 , J2 là các ma trận Jacobi ứng với 2 trạng thái động học thuận và
động học ngược.
Các trạng thái đặc biệt .
Với sự tồn tại hai ma trận Jacobi, cơ cấu chấp hành song song có cấu hình đặc
biệt khi Jx, Jq hoặc cả hai trạng thái đặc biệt do đó có thể tìm được ba kiểu
trạng thái đặc biệt:
+ Khi định thức của Jq tiến đến zero
det(Jx) = 0 (3.26)
Khi đó tồn tại các véc tơ q khác zero dẫn đến kết quả vectơ x bằng zero. Tức
& &
là chuyển động vi phân của bệ di động theo một số chiều không thể thực hiện
được, cơ cấu chấp hành bị dàng buộc lại và mất đi một số bậc tư do. Trạng
thái đặc biệt động học đảo thường xảy ra ở biên không gian hoạt động của cơ
cấu chấp hành.
+ Khi định thức Jx bằng zero
det(Jx) = 0 (3.27)
Khi đó tồn tại các véc tơ x khác zero dẫn đến kết quả vectơ q bằng zero.
& &
Trong trường hợp này bệ di động có thể chuyển động vi phân theo một số
chiều, còn mọi bộ tác động khác đều bị khoá tức là hệ sẽ tăng lên một số bậc
tự do.
+Khi cả hai định thức của Jx và Jq đều bằng zero.
Trong phạm vi đồ án này ta không xét tới các trạng thái động học đặc biệt
này.
47
- 48
3.2.2 Phân tích Jacobi Robot song song 3RPS tổng quát
z1
B3
z0 α3
P
Bàn máy động B2
B1 y1
z3 α2
A3
x1 α1 z2
z1 x3
x2
O
Đế cố định
y0
A1
A2
x0 x1
Hình 3.4: Cấu trúc chấp hành song song 3RPS
Từ công thức (3.6):
⎡d i ⎤
p + ARB.Bbi = ai + ARi. ⎢ 0 ⎥ (i = 1,2,3)
⎢ ⎥
⎢0⎥
⎣ ⎦
Đạo hàm công thức (3.6) ttheo thời gian ta được :
48
- 49
⎡d i ⎤ &
⎡d i ⎤
⎢ ⎥
p + A R B .b i = a i + R i .⎢ 0 ⎥ + R i .⎢ 0 ⎥
& & & & (i=1,2,3) (3.28)
⎢ ⎥
⎢0⎥
⎣ ⎦ ⎢0⎥
⎣ ⎦
Do ai là hằng số nên đạo hàm a i = 0
&
Từ phương trình (2.28) ta có:
& ~
R .R T = ω
i i i
Nên & ~
R = ω i .R i (3.29)
. .
~
R B = ω.A R B
A
Trong đó:
⎡ 0 − ωi 3 ωi 2 ⎤
~ =⎢ ω
ωi 0 − ω i1 ⎥ (i=1,2,3)
⎢ i3 ⎥
⎢ − ωi 2
⎣ ω i1 0 ⎥ ⎦
ω i1 , ω i 2 , ω i 3 là các thành phần vận tốc góc của chân thứ i so với hệ cố định.
⎡ 0 − ω3 ω2 ⎤
ω = ⎢ ω3
~ 0 − ω1 ⎥
⎢ ⎥
⎢− ω2
⎣ ω1 0 ⎥ ⎦
ω 1 , ω 2 , ω 3 là các thành phần vận tốc góc của bàn máy động so với hệ cố định.
r
Chú ý:Do vector vận tốc góc ωi có hướng theo các trục zi tương ứng trên hình
chiếu của nó lên các trục x0,y0,z0 cho ta ωι3 =0.
⎡ 0 0 ωi 2 ⎤
Vậy ωi = ⎢ 0
~ 0 − ω i1 ⎥ (i=1,2,3)
⎢ ⎥
⎢ − ωi 2
⎣ ω i1 0 ⎥ ⎦
Nếu biết ωi1 (hoặc ωi 2 ) ta có thể xất định các thành phần còn lại từ các tham
số hình học của hệ.
Phương trình (3.28) được viết dưới dạng :
⎡d i ⎤ &
⎡d i ⎤
⎢ ⎥
p + ω.Α R B .B b i = ω.R i .⎢ 0 ⎥ + R i ⎢ 0 ⎥ (i=1,2,3)
& ~ ~ (3.30)
⎢ ⎥
⎢0⎥
⎣ ⎦ ⎢0⎥
⎣ ⎦
Khai triển (3.30) ta sẽ được 9 phương trình, các đại lượng đã biết từ bài toán
tìm vị trí và tham số hình học của robot là: di,Bbi,Ri,ARB (i=1,2,3)
49
- 50
&
-Bài toán Jacobi thuận : Biết d i (i=1,2,3) ta có 9 phương trình đại số tuyến tính
để giải 9 ẩn: p1 , p 2 , p 3 , ω1 , ω2 , ω3 , ω11 , ω21 , ω31 .
& & &
-Bài toán phân tích Jacobi ngược: Biết p = [p1 , p 2 , p 3 ] hoặc ω ta có 9 phương
& & & & T
& & &
trình đại số tuyến tính để giải 9 ẩn: d 1 , d 2 , d 3 , ω1 , ω 2 , ω 3 (hoặc p ),
&
ω11 , ω21 , ω31 .
Ma trận Jacob được mô tả trong mục (3.2.1) sẽ được xát định khi sắp xếp các
số hạng của phương trình (3.30).
3.2.3 Phân tích Jacobi của một robot song song 3RPS cụ thể
Đạo hàm phương trình (3.18) ta được hệ (3.31):
1&
[ & & & & & & &
3p1 = d2 sinα2 + α2d2 cosα2 + d3 sinα3 + α3d3 cosα3 − d1 sinα1 − α1d1 cosα1
&
2
]
3p2 =
&
− 3&
2
[ & & & & &
d2 sinα2 + α2d2 cosα2 − d3 sinα3 − α3d3 cosα3 ]
& & & &
3p3 = d1 cosα1 − α1d1 sinα1 + d2 cosα2 − α2d2 sinα2 − d3 cosα3 − α3d3 sinα3
& & &
d1 [− 3g sin α1 − d 2 sin α1 sin α 2 + 2d1 − 2d 2 cos α1 cos α 2 ] +
&
d [− 3g sin α + d sin α sin α + 2d − 2d cos α cos α ] +
&
2 2 1 1 2 2 1 1 2
α1 [− 3gd1 cos α1 + d1d 2 cos α1 sin α 2 − 2d1d 2 cos α 2 sin α1 ] +
&
α 2 [− 3gd 2 cos α 2 + d1d 2 cos α 2 sin α1 − 2d1d 2 cos α1 sin α 2 ] = 0
&
d1 [− 3g sin α1 − d 3 sin α1 sin α 3 + 2d1 − 2d 3 cos α1 cos α 3 ] +
&
d [− 3g sin α + d sin α sin α + 2d − 2d cos α cos α ] +
&
3 3 1 1 3 3 1 1 3
α1 [− 3gd1 cos α1 + d1d 3 cos α1 sin α 3 − 2d1d 3 cos α 3 sin α1 ] +
&
α 3 [− 3gd 3 cos α 3 + d1d 3 cos α 3 sin α1 − 2d1d 3 cos α1 sin α 3 ] = 0
&
d 2 [− 3g sin α2 − d 3 sin α2 sin α3 + 2d 2 − 2d3 cosα2 cosα3 ] +
&
d [− 3g sin α + d sin α sin α + 2d − 2d cosα cosα ] +
&
3 3 2 2 3 3 2 2 3
α2 [− 3gd2 cosα2 + d 2 d 3 cosα2 sin α3 − 2d 2 d 3 cosα3 sin α2 ] +
&
α3 [− 3gd3 cosα3 + d 2 d 3 cosα3 sin α2 − 2d 2 d 3 cosα2 sin α3 ] = 0
&
50
- 51
Đo các đại lượng về vị trí đã tính được ở bài toán vị trí nên hệ (3.31) là một
& & &
hệ phương trình tuyến tính với các ẩn là các vận tốc p i , d i , x i (i = 1,2,3).
Nếu ta sử dụng các ký hiệu:
f1 = −3g sin α1 + d 2 sin α1 sin α 2 + 2d1 − 2d 2 cos α1 cos α 2
f 2 = −3g sin α 2 + d1 sin α1 sin α 2 + 2d 2 − 2d1 cos α1 cos α 2
f 3 = −3gd1 cos α1 + d1d 2 cos α1 sin α 2 + 2d1d 2 sin α1 cos α 2
f 4 = −3gd 2 cos α 2 + d1d 2 sin α1 cos α 2 + 2d1d 2 sin α 2 cos α1
f 5 = −3g sin α1 + d 3 sin α1 sin α 3 + 2d1 − 2d 3 cos α1 cos α 3
f 6 = −3g sin α 3 + d1 sin α1 sin α 3 + 2d 3 − 2d1 cos α1 cos α 3
f 7 = −3gd1 cos α1 + d1d 3 cos α1 sin α 3 + 2d1d 3 sin α1 cos α 3
f 8 = −3gd 3 cos α 3 + d1d 3 cos α 3 sin α1 + 2d1d 3 sin α 3 cos α1
f 9 = −3g sin α 2 + d1 sin α 2 sin α 3 + 2d 2 − 2d 3 cos α 2 cos α 3
f10 = −3g sin α 3 + d 2 sin α 2 sin α 3 + 2d 3 − 2d 2 cos α 2 cos α 3
f11 = −3gd 2 cos α 2 + d 2 d 3 sin α 3 cos α 2 + 2d 2 d 3 sin α 2 cos α 3
f12 = −3gd 3 cos α 3 + d 2 d 3 cos α 3 sin α 2 + 2d 2 d 3 sin α 3 cos α 2
Vậy hệ (3.31) có thể viết thành:
3p1 =
&
1 &
2
[ & & & & & & ] &
d2 sin α2 + α2d2 cosα2 + d3 sin α3 + α3d3 cosα3 − d1 sin α1 − α1d1 cosα1
3p2 =
&
− 3 &
2
[ & & & & &
d2 sin α2 + α2d2 cosα2 − d3 sin α3 − α3d3 cosα3 ]
& & & &
3p3 = d1 cosα1 − α1d1 sin α1 + d2 cosα2 − α2d2 sin α2 − d3 cosα3 − α3d3 sin α3
& & &
& &
d 1f 1 + d 2 f 2 + α 1 f 3 + α 2 f 4 = 0
& &
& &
d 1f 5 + d 3 f 6 + α 1 f 7 + α 3 f 8 = 0
& & (3.32)
& &
d f +d f +α f +α f =0
& &
2 9 3 10 2 11 3 12
a) Bài toán phân tích Jacobi thuận:
&
Bài toán phân tích Jacobi thuận: Biết d i (i =1,2,3) ta có 6 phương trình đại số
tuyến tính dể giải 6 ẩn p1 , p 2 , p 3 , α 1 , α 2 , α 3 ,
& & & & & &
Ta viết lại hệ (3.32) dưới dạng ma trận :
51
- 52
[& & & T
]
J d d1 , d 2 , d 3 ,0,0,0 = J p [p1 , p 2 , p 3 , α1 , α 2 , α 3 ,]
& & & & & & T (3.33)
Trong đó :
⎡ 1 1 ⎤
⎢− sin α1 − sin α 2
2 2
sin α 3 0 0 0⎥
⎢ 3 3 ⎥
⎢ 0 − sin α 2 sin α 3 0 0 0⎥
⎢ 2 2 ⎥
J d = ⎢ cos α1 cos α 2 cos α 3 0 0 0⎥
⎢ f f2 0 0 0 0⎥
⎢ 1
⎥
⎢ f5 0 f6 0 0 0⎥
⎢ ⎥
⎣ 0 f9 f10 0 0 0⎦
⎡ 1 1 ⎤
⎢3 0 0 d 1 cos α1 − d 2 cos α 2 − d 3 cos α 3 ⎥
2 2
⎢ 3 3 ⎥
⎢0 3 0 0 d 2 cos α 2 − d 3 cos α 3 ⎥
⎢ 2 2 ⎥
J p = ⎢0 0 3 d1 sin α1 d 2 sin α 2 d 3 sin α 3 ⎥
⎢0 0 0 − f3 − f4 0 ⎥
⎢ ⎥
⎢0 0 0 − f7 0 − f8 ⎥
⎢ ⎥
⎣0 0 0 0 − f11 − f12 ⎦
Các thành phần của ma trận Jd , Jp đã xác định được từ bài toán vị trí.
a)Bài toán phân tích Jacobi ngược
Bài toán phân tích Jacobi ngược: Biết p = [p1 , p 2 , p 3 ] ta có 6 phương trình
T
& & & &
& & & & & &
đại số tuyến tính để xác định 6 ẩn: d1 , d 2 , d 3 , α1 , α 2 , α 3 .
Ta viết lại hệ phương trình (3.32) dưới dạng ma trận:
[p , p
& &
1 2 , p 3 ,0,0,0
& [
]T = J 2 d1 , d 2 , d 3 , α1 , α 2 , α 3
& & & & & & ]
T
(3.34)
Trong đó:
52
- 53
⎡ sin α1 sin α 2 sin α3 d1 cosα1 d 2 cosα 2 d 3 cosα3 ⎤
⎢− 3 6 6
−
3 6 6 ⎥
⎢ ⎥
⎢ 0 3 sin α 2 3 sin α3 3d 2 cosα 2 3d 3 cosα3 ⎥
− 0 −
⎢ 6 6 6 6 ⎥
J2= ⎢ cosα1 cosα 2 cosα3
−
d1 sin α1
−
d 2 sin α 2
−
d 3 sin α3 ⎥
⎢ ⎥
⎢ 3 3 3 3 3 3 ⎥
⎢ f1 f2 0 f3 f4 0 ⎥
⎢ f5 0 f6 f7 0 f8 ⎥
⎢ ⎥
⎣ 0 f9 f10 0 f11 f12 ⎦
Các thành phần của ma trận J2 đã xác định được từ bài toán vị trí.
53
nguon tai.lieu . vn