Áp dụng định luật Kirchhoff đối với điện áp trong động cơ ta được: 48
adt a
Tương tác cơ điện trong động cơ DC được thể hiện bằng mối liên hệ giữa dịng điện và mơ men động cơ; giữa tốc độ động cơ và điện áp phản sức điện động như sau:
0
K i ,
m
Hệ các phương trình vi phân và đại số từ (3.9) đến (3.4) mô tả tương đối đầy đủ động lực học của robot song song dẫn động bởi động cơ DC với đầu vào là điện áp u đặt lên các động cơ và đầu ra là chuyển động của robot q(t ) , trong đó có chuyển động của bàn máy động.
3.2.2 Mơ hình động lực học robot song song có kể đến hệ dẫn động
3.2.2.1 Cơ sở xây dựng mơ hình
Với robot song song, các phương trình vi phân mơ tả động lực học của hệ và các phương trình đại số mô tả các liên kết đặt lên hệ. Khảo sát cơ hệ với n bậc tự do với các tọa độ suy rộng dư. Cần lưu ý rằng, so với việc sử dụng các tọa độ suy rộng tối thiểu các hệ sử dụng các tọa độ suy rộng dư cho phép dễ dàng hơn trong việc thiết lập phương trình vi phân chuyển động, và cũng thuận tiện hơn để mơ phỏng trên máy tính. Gọi q [ q1 , q2 ,..., qm ]T m n là véctơ tọa độ suy rộng dư. Việc thiết lập các phương
trình vi phân chuyển động cho các hệ này đã được trình bày trong rất nhiều các tài liệu tham khảo. Với các nhân tử Lagrange phương trình vi phân chuyển động cho hệ được viết ở dạng:
d dt
f(q ) 0
trong đó T 12 q T M (q )q là động năng hệ, M (q) là ma trận khối lượng có cỡ m m ;
P P(q) là biểu thức thế năng; véctơ Q là lực suy rộng của các lực điều khiển và
các lực không thế; véctơ [ 1 2 ... r ]T với cỡ r 1, r m n , chứa các nhân tử
Lagrange; véctơ f(q ) 0 , với f [f1 f2 ... fr ]T , chứa m n phương trình liên kết; và
J (q ) f / q với cỡ r m là ma trận Jacobi.
Như đã biết phương trình (3.5) có thể được viết lại thành [31]
M (q )q C (q , q )q Dq g (q ) = B 2 J qT (q) (3.6)
trong đó: Ma trận khối lượng M (q) được tính theo 49
n
i 1
Đó là ma trận đối xứng và xác định dương.
Với Q B 2 Dq , B là ma trận liên quan việc bố trí tín hiệu điều khiển và
2 là véctơ lực/ngẫu lực trong các khớp chủ động.
Ma trận ly tâm và Côriôlis C (q , q) được xác định theo công thức Christoffel [99] như sau:
C(q , q ) cij (q , q)
cij (q , q )
Tóm lại, đối với hệ nhiều vật cấu trúc mạch vịng phương trình vi phân chuyển động được viết ở dạng tọa độ suy rộng dư như sau:
M (q )q C (q , q )q Dq g (q ) B 2 J qT (q)
f(q ) 0
Ma trận C(q, q) tính theo (3.8) đảm bảo cho N = M (q ) 2C (q , q) là ma trận
phản đối xứng, một tính chất quan trọng cho thiết kế điều khiển.
Do hằng số thời gian điện nhỏ hơn hằng số thời gian cơ rất nhiều, nên ta có thể sử dụng xấp xỉ La d i / dt 0 khi t 0 , để đơn giản hóa hệ phương trình vi phân mơ tả hệ thống. Với sự xấp xỉ này từ phương trình (3.3) và (3.4) ta giải được các dịng điện
i R 1
a
sau đó thay vào phương trình (3.4) ta nhận được mơ men động cơ do cuộn dây tạo ra tác dụng lên rô-to:
0
K i K R 1
mm a
Thay (3.12) vào (3.2) ta nhận được phương trình vi phân chuyển động của rơ-to
I m m Dm m 0
I m m Dm m K m Ra 1 (u Ke m ) rg 1 2 hay
I m m (Dm K m Ra 1K e ) m K m Ra 1u rg 1 2
Thay m rgqa rg vào ta được phương trình trên ta nhận được
I r2q (D KR1K
m g a mm ae
Chú ý rằng, do ma trận B có dạng sau
B 0En n
m n ,n
nên việc nhân từ trái ma trận phương trình này:
B B
m n
khi đó phương trình (3.15) được viết lại thành
Thực hiện cộng hai phương trình (3.9) và (3.17) ta nhận được
M (q ) rg2 BI m B q C (q , q )qD B (Dm K m Ra 1K e ) rg2B q g (q) BK m Ra 1rg u J qT (q)
Để cho ngắn gọn ta đưa vào các ký hiệu
M s (q ) M (q ) rg2BI m B C s (q , q ) C (q , q)
Phương trình trên được viết lại thành
Cùng với các phương trình ràng buộc
f(q ) 0
Như thế, phương trình vi phân chuyển động của robot song song dẫn động bởi động cơ DC được mơ tả bởi hệ phương trình vi phân đại số (3.19) và (3.20). Các phương trình này sẽ được sử dụng để giải các bài toán động lực học thuận và ngược robot song song.
Do r2BI m B là ma trận hằng, nên việc tính tốn ma trận Côriolis
C s (q , q ) C (q , q) từ ma trận khối lượng M s (q) hay M (q) là như nhau, và tính chất
phản đối xứng của ma trận N M s (q ) 2C s (q , q) vẫn được đảm bảo.
3.2.2.2 Mơ hình động lực của robot song song 3RRR
Khảo sát đối tượng nghiên cứu chính là robot song song đã được thể hiện trong mục 1.5. Trong phần này trình bày về việc xây dựng mơ hình động lực cho robot. Vì loại robot này có ba chân giữ vai trò ngang nhau nên chúng ta có thể xây dựng các phương trình tổng qt bao gồm phương trình liên kết (2.2) và các phương trình sau:
Tọa độ trọng tâm của các khâu tương ứng
xc1, i xo1,i l1, 2i cos( i ) y y o1,i c1, i x x c 2, i o1, i y c 2, i y o1, i
Ma trận quay tương ứng với các khâu của mỗi chân
cos( i )
R
1,i
Ten xơ quán tính I x1,i 0 I y 1,i I C 1, i
Áp dụng các công thức xây dựng mơ hình động lực đã trình bày ở phần trên ta thu được các ma trận trong mơ hình động lực tổng hợp của robot bao gồm:
Ma trận khối lượng
Ma trận khối lượng M ( q) được tính tốn dựa theo cơng thức (3.7), ta thu được kết quả như sau:
m I 11 c1 m I 14 c2 m I 22 c1 m I 25 c2 m I 33 c1 m I 36 c2 m I 41 c2 m52 I c2 14 m2 l22 1 2 m2 l1l2 cos( 2 ) ; m55 I c2 1 4 m2 l22 m63 I c2 1 4 m2 l22 1 2 m2 l1l2 cos( 3 ) ; m66 I c2 1 4 m2 l22 m77 m3 , m88 m3 , m99 Ic3 Các phần tử khác của ma trận bằng không. Ma trận ly tâm và Coriolis
Ma trận ly tâm và Coriolis C (q, q) được tính theo cơng thức (3.8) với kết quả như sau:
c 1 m l l sin( ) ; c 1 m l l sin( ) 1
m l l sin( )
c33 1
2 m2 l1l2 3 sin( 3 ) ; c36 12 m2 l1l2 3 sin( 3 ) 1
2 m2 l1l2 3 sin( 3 )
c41 1
2 m2 l1l2 1 sin( 1 ) ; c52 1 2 m2 l1l2 2 sin( 2 ) ; c63 12 m2 l1l2 3 sin( 3 ) Các phần tử khác của ma trận bằng không.
Ma trận cản nhớt
D diag ([ c, c, c, 0, 0, 0, 0, 0, 0]) trong đó: c là hệ số cản nhớt
Ma trận phân bố tín hiệu điều khiển
có kích thước [9 3] , trong đó có các phần tử B(1,1) 1 ; B(2, 2) 1;
B(3,3) 1, Các phần tử khác của ma trận bằng 0.
Ma trận g(q)
g(1,1) [ 1
m1l1 g cos( 1 ) m2 l1 g cos( 1 ) 1 m2 l2 g cos( 1 1 );
2 2 g(2,1) [ 1
m1l1 g cos( 2 ) m2 l1 g cos( 2 ) 1 m2 l 2 g cos( 2 2 ); 2 2 g(3,1) [ 1 m1l1 g cos( 3 ) m2 l1 g cos( 3 ) 1 m2 l2 g cos( 3 3 ); 2 2 1 2 22 1 1 1 g(5,1) m l g cos( ); 2 22 2 2 1 g(6,1) 2 m2 l 2 g cos( 33 ); g(7,1) 0; g(8,1) m3 g; g(9,1) 0; Ma trận Jacobi
Đạo hàm các phương trình liên kết ta có ma trận J q (q ) f/ q như sau:
J 11 l1 sin 1 l2 sin( 1 1 ) ; J 14 l 2 sin( 1 1 ) ; J 19 b sin(P1 ) ; Ma trận B
J 32 l1 sin 2 l2 sin( 2 2 ) ; J 35 l 2 sin( 2 2 ) ; J 39 b sin(P2 ) ; J 53 l1 sin 3 l2 sin( 3 3 ) ; J 56 l2 sin( 3 3 ) ; J 59 b sin(P3 ) ; J17 1; J28 1; J37 1; J48 1; J57 1; J68 1; Các phần tử khác của ma trận bằng 0.
Véc tơ nhân tử Lagrange
[ 1, 2, 3, 4, 5, 6]T
Như vậy, thay các đại lượng đã biết, ta đã xây dựng được hệ phân đại số mô tả chuyển động của robot 3RRR
M (q )q C (q , q )q Dq g (q ) B 2 J qT (q)
phương trình vi
(3.21)
3.2.2.3 Mơ hình động lực học hưởng của cơ cấu dẫn
của robot song song 3RRR có kể đến ảnh động
Thực hiện tính tốn mơ hình động lực của robot song song phẳng 3RRR có kể đến ảnh hưởng của cơ cấu dẫn động theo công thức (3.18) ta thu được kết quả các ma trận như sau: Ma trận khối lượng Ms [9 9] ms11 rg2 I r Ic1 Ic2 m2 l12 1 4 m1 l12 ; ms12 1 2 m2 l1 l2 cos 1 1 ; ms 21 ms12 ; ms 22 12 m2 l22 ; ms 33 rg2 I r Ic1 14 m1l12 m2 l12 ; ms 34 12 m2 l1 l2 cos 2 2 ; ms 43 ms34 ; ms 44 Ic2 14 m2 l22 ; ms 55 rg2 I r Ic1 14 m1l12 m2 l12 ; ms 56 12 m2 l1 l2 cos 3 3 ; 55
s65 s56
Các phần tử khác của ma trận bằng 0.
Với ma trận khối lượng sau tính tốn, thành phần rg2 Ir chính là phần ảnh hưởng của động lực học cơ cấu dẫn động được thiết lập trong mơ hình tổng thể.
Ma trận ly tâm và Coriolis
Ma trận ly tâm và Coriôlis C s (q , q )[9 9] C (q , q)[9 9]
Ma trận thể hiện sự phân bố tín hiệu điều khiển Bs[9 3]
bs11 bs 22 bs33 rKm Ra
Các phần tử khác của ma trận bằng không
Ma trận cản nhớt
Ma trận cản nhớt Ds có kích thước [9 9], với các phần tử được xác định như sau: d s11 d s 22 ds33 r2 KmKe
, các phần tử khác của ma trận bằng 0
Ra
Ma trận g(q)
Trường hợp này, robot được mô phỏng ở trạng thái nằm ngang nên g (q) 0 .
3.2.2.4 Mơ hình động lực học của robot song song 3RRR có kể đến ảnh hưởng của động cơ dẫn động dạng rút gọn
Trong trường hợp mô phỏng này chúng ta sử dụng tọa độ suy rộng dư như sau:
q [qT , xT ]T [ 1 , 2 , 3 , xP , yP , ]T . Để cho đơn giản khi thiết lập mơ hình động lực, khối lượng m2 của các thanh nối A i Bi coi như tập trung tại hai đầu thanh. Khi đó biểu thức động năng của hệ sẽ có dạng đơn giản như sau
3 T 1 ( I m r 2 I C 1 2 k 1 1 ( m3 2
Lực suy rộng do mô men động cơ truyền lên khâu dẫn thơng qua bộ truyền được tính từ các phương trình sau
56
adt a
Nếu bỏ qua số hạng La di / dt 0,t , từ hai phương trình trên ta nhận được mơ
men động cơ
0
K
m
Thông qua bộ truyền giảm tốc, mô men dân động đặt lên khâu dẫn sẽ là
rg 0 rg K m Ra 1U rg2 K m K
e Ra 1 (3.25)
Nếu kể đến lực cản nhớt với hệ số d tác dụng lên khâu dẫn, tổng mô men dẫn và cản lên khâu dẫn là
r
g
với k r K R
g m a
Các phương trình liên kết được thiết lập từ điều kiện khoảng cách giữa các điểm Ai và Bi: f (r r )T (r i B A i i với r r xC y Bi C
Các phương trình liên kết (3.27) được viết lại dạng
f (q ) f (q, x) 0 , f 3
Từ biểu thức động năng, lực suy rộng và các phương trình liên kết, ta nhận được phương trình vi phân chuyển động dạng (3.19) cho robot song song 3RRR với các ma trận và véc tơ như sau:
Ms diag([(Im r 2 I C 1 14 m1l12 12 m2 l12 )[1,1,1];( m3 3 12 m2 ) [1,1]; I C3 3 12 m2 b2 ]) C s (q , q ) 06 6 Ds diag ([ c , c , c, 0, 0, 0]) g s (q ) 06 1 k E 3 3
(3.29) (3.30) (3.31) (3.32) (3.33) 57
Bằng các phương pháp khác nhau ta đã thiết lập được phương trình động lực học cho robot song song ở dạng tọa độ suy rộng. Từ các phương trình động lực này chúng ta thường quan tâm tới hai bài toán bao gồm: Động lực học ngược và Động lực học thuận.
3.2.3 Hệ phương trình vi phân chuyển động với tọa độ suy rộng tối thiểu
Các phương trình mơ tả trong phần trên sử dụng tọa độ suy rộng dư, thuận tiện cho việc thiết lập phương trình cũng như tính tốn mơ phỏng. Để thiết kế bộ điều khiển cho robot, các phương trình này cần được biến đổi về dạng tọa độ tối thiểu. Các tọa độ tối thiểu hay được chọn là các tọa độ khớp chủ động qa q hoặc vị trí bàn máy động x . Để thực hiện các biến đổi trong phần này, ta giả thiết rằng ma trận Jacobi J q (q ) f / q không kỳ dị (hay không giảm hạng).
Thực hiện đạo hàm theo thời gian phương trình (3.20) ta nhận được
J q (q )q J q J y J xx J q J p p với p T [ yT , xT ] J f / q , J p f / p [J , Jx ]. Từ (3.34), ta giải được p J p 1J q ,
Đạo hàm tọa độ suy rộng có thể viết lại thành.
Đưa vào ma trận R (q) m f , xác định bởi
E
R (q)
J p 1J
với E là ma trận đơn vị cỡ f f . Ta viết lại (3.36) thành
q R(q)q
Đạo hàm phương trình trên theo thời gian ta nhận được
q R(q ) R(q) . q q (3.35) (3.36) (3.37) (3.38) (3.39)
Chú ý rằng, ma trận R(q) thỏa mãn tính chất sau đây
Ma trận RT được sử dụng để khử nhân tử Lagrange l trong phương trình (3.19) bằng cách biến đổi như sau.
RT M s (
Để nhận được phương trình vi phân ở dạng tọa độ tối thiểu ta thay q , q từ phương trình (3.38) và (3.39) vào (3.41), ta nhận được kết quả
R
T
M s (q )
Để cho ngắn gọn, ta đặt các đại lượng sau đây M (q) RT M s (q )R, M f f
C (q , q) R T M (q )R C (q , q )R ss
D RT Ds R, g (q) RT g s (q) ,g f .
Ta viết được phương trình vi phân chuyển động ở dạng tọa độ khớp như sau:
Trong phương trình (3.43) các ma trận thành phần vẫn giữ được các tính chất giống như trong hệ phương trình động lực (3.19).
xn
trong đó
Do ma trận M s xác định dương nên V 0 , V 0 khi y 0 , do đó định đương.
Ta có
Từ đó suy ra ma trận M đối xứng Ngồi ra, ta có
M 2C Cần chú ý rằng: M s 2Cs là phản xứng do đó Để chứng minh (M 2C) (M 2C) Hay M Vì M RTM
Thay phương trình (3.50) và (3.47) vào phương trình (3.48) ta thu được
M(CC
T
R
0
Như vậy, trong phương trình (3.43) các tính chất sau đây vẫn còn được đảm bảo: M (q) là ma trận đối xứng và xác định dương, ma trận N [M (q ) 2C (q , q)] là ma trận đối xứng lệch [5].
3.3 Khảo sát động lực học robot song song
Bài tốn động lực học trên robot song song có nhiều phương pháp giải quyết. Tuy nhiên, phương pháp khử nhân tử Lagrange là phương pháp thường được sử dụng. Dựa trên giả thiết rằng các liên kết trong hệ là lý tưởng, do đó tổng cơng ảo của các lực liên kết triệt tiêu. Để khử các lực liên kết và đưa về hệ phương trình vi phân thường, ta sử dụng ma trận khử định nghĩa bởi
J
d
Trong trường hợp robot di chuyển qua các cấu hình kỳ dị, để q trình tính tốn được trơn tru ta cần phải tìm cách xây dựng lại ma trận khử R . Sử dụng ma trận tựa nghịch đảo, ta xây dựng được ma trận khử lực liên kết như sau:
Với Jq là ma trận tựa nghịch đảo của ma trận Jq thỏa mãn tính chất J qJ q J q Jq bởi vì J q R J q [E m J q J q ] [J q J q ] 0 . Với Matlab, ma trận tựa nghịch đảo và ma trận bù của ma trận Jacobi được tính như sau:
J qpinv(Jq ) và R null(Jq )
Nhân trái ma trận RT với phương trình (3.6) và chú ý R T JT 0 ta thu được
R T M (q )q C (q , q )q h (q ) R T Bu
Với việc sử dụng ma trận khử này, các nhân tử Lagrange đã được loại ra khỏi mơ hình động lực học của robot song song. Dựa trên cơ sở này, các bài toán động lực học ngược và động lực học thuận sẽ được trình bày trong phần tiếp sau đây.
3.3.1 Bài toán động lực học thuận
Bài toán động lực học thuận của robot song song là bài toán cho đầu vào là quy luật của các mô men điều khiển các khớp chủ động hoặc điện áp điều khiển đặt lên các động cơ dẫn động với mơ hình tích hợp hệ dẫn động để tìm ra đầu ra là quy luật các biến khớp.
Động lực học Thuận
Hình 3.4: Sơ đồ bài toán động lực học thuận
Để giải bài toán động lực học thuận, ta thực hiện kết hợp phương trình (3.54) với đạo hàm hai lần phương trình liên kết theo thời gian ta nhận được hệ phương trình vi phân thường sau:
R T M (q )q R T C (q , q )q h (q ) R T Bu
f(q ) 0
Đạo hàm hai lần phương trình liên kết mức gia tốc:
Mơ hình động lực có thể viết lại như sau
R T M ( q )
để nhận được phương trình liên kết ở (3.55)
C ( q , q ) q + h( q)
(3.56)
-J
Đó là hệ phương trình vi phân thường n phương trình n ẩn. Giải hệ phương trình vi phân thường trên với điều kiện đầu tương thích, ta nhận được các tọa độ suy rộng q(t ) .
3.3.2 Bài toán động lực học ngược
Bài toán động lực học ngược với đầu vào là quy luật của vị trí, hướng khâu thao tác mong muốn theo một quỹ đạo đã định sẵn hay cũng chính là quy luật chuyển động của các khớp thu được qua bước giải bài toán động học ngược, đầu ra là các mô men điều khiển các khớp chủ động hoặc điện áp điều khiển với mơ hình có tích hợp thơng tin của hệ dẫn động.
Động lực học ngược