Xem mẫu
- Tuyển tập Hội nghị khoa học toàn quốc lần thứ nhất về Động lực học và Điều khiển
Đà Nẵng, ngày 19-20/7/2019, tr. 177-182, DOI 10.15625/vap.2019000275
Động lực học tay máy robot có liên kết chương trình
Nguyễn Văn Khang1), Nguyễn Văn Quyền1), Lương Bá Trường2), Nguyễn Văn Long3)
1)
Bộ môn Cơ học ứng dụng, Trường Đại học Bách khoa Hà Nội
2)
Bộ môn Cơ điện tử, Trường Đại học Thủy Lợi
3)
Bộ môn Cơ điện tử, Trường Đại học Kinh doanh và Công nghệ Hà Nội
E-mail: 1){khang.nguyenvan2, quyen.nguyenvan}@hust.edu.vn, 2)truonglb@tlu.edu.vn, 3)chitietmay1011@gmail.com
Tóm tắt M (s, t )s + C (s, s, t )s + g (s, t ) =
Hệ nhiều vật chịu các liên kết chương trình có thể xem thuộc (1)
t (t ) - FsT (s, t ) l
vào tập hệ nhiều vật có cấu trúc mạch vòng. Các phương trình
chuyển động của hệ nhiều vật có cấu trúc mạch vòng thường là f (s, t ) = 0 (2)
hệ các phương trình vi phân – đại số. Trong báo cáo này trình T
Trong đó s = éêës1, s2 ,..., sn ùûú là các toạ độ suy rộng dư,
bày việc áp dụng các phương trình Lagrange dạng nhân tử để
thiết lập hệ phương trình vi phân-đại số của hệ nhiều vật chịu M (s, t) là ma trận khối lượng suy rộng của hệ, t (t ) là
các liên kết chương trình. Sau đó trình bày chi tiết một bài toán
véctơ lực suy rộng ứng với các lực hoạt động không có
động lực học thuận robot có liên kết chương trình. T
thế, l = éêël1, l2 ,..., lr ùúû là véctơ các nhân tử Lagrange,
Từ khóa: Tay máy Robot, liên kết chương trình, phương trình T
vi phân-đại số, phương pháp khử nhân tử Lagrange, động lực f = éëê f1, f2 ,..., fr ùûú =0 là các điều kiện ràng buộc, Fs là
học thuận. ma trận Jacobi của f cỡ r ´ f , C (s, s, t ) là ma trận
quán tính ly tâm và Coriolis.
1. Mở đầu Để thuận tiện cách viết ta đưa vào ký hiệu
Động lực học hệ nhiều vật chịu các liên kết chương p1 (s, s, t ) = t (t) - C (s, s, t )s - g (s, t ) (3)
trình là lĩnh vực khoa học có ý nghĩa thực tế và đang Phương trình (1) bây giờ có dạng
được quan tâm nghiên cứu [1-7]. Hệ nhiều vật chịu các M (s, t)s + FsT (s, t ) l = p1 (4)
liên kết chương trình thực chất thuộc vào tập hệ nhiều vật
có cấu trúc mạch vòng. Để thiết lập phương trình chuyển Đạo hàm hai lần phương trình liên kết (2) ta thu được các
động của hệ nhiều vật có cấu trúc mạch vòng, người ta phương trình
thường sử dụng phương trình Lagrange dạng nhân tử, ¶f ¶f
f = s + = ss + ft = 0 (5)
phương trình Kane dạng nhân tử, phương pháp tách cấu ¶s ¶t
trúc,… [8, 9]. Các phương trình mô tả chuyển động của f = Fss + Fss + ft = 0
hệ nhiều vật có cấu trúc mạch vòng thường là hệ các
phương trình vi phân – đại số. Các phương pháp giải hệ Fss = -(Fss + ft ) =: p2 (6)
phương trình vi phân – đại số thường được phân thành Các phương trình (4) và (6) có thể viết lại dưới dạng ma
hai nhóm: trận như sau
Các phương pháp biến đổi hệ phương trình vi éM FT ù é s ù é p ù
ê s ú ê ú ê 1ú
phân-đại số về hệ phương trình vi phân thường. êF 0 ú êl ú = ê p ú (7)
êë s úû êë úû êë 2 úû
Sau đó sử dụng thuật toán số giải hệ phương trình
vi phân thường [8,9]. Xác định các điều kiện đầu của các toạ độ suy rộng phụ
thuộc và các vận tốc suy rộng phụ thuộc [10, 11]
Các phương pháp số giải trực tiếp hệ phương trình
Để có thể tích phân được các phương trình vi phân
vi phân đại số [8, 9]. dạng tọa độ dư, thì việc xác định các điều kiện đầu của
Trong bài báo này trình bày việc áp dụng phương các tọa độ suy rộng phụ thuộc và vận tốc của chúng là
pháp khử các nhân tử Lagrange tính toán động lực học cần thiết. Vì vậy trước tiện ta trình bày vấn đề này.
thuận robot có liên kết chương trình. Khi giải hệ phương trình vi phân chuyển động của hệ
nhiều vật có f bậc tự do, ta phải cho trước f giá trị
2. Phương pháp số giải phương trình đầu của các toạ độ suy rộng độc lập và các vận tốc suy
Lagrange dạng nhân tử rộng độc lập q1 (0),..., q f (0), q1 (0),..., qf (0) . Trong hệ
phương trình (2) còn có r toạ độ suy rộng dư
Sử dụng phương trình Lagrange dạng nhân tử, chúng
z1 (t ),..., z r (t ) . Vì vậy ta còn phải xác định các điều kiện
ta thu được hệ phương trình vi phân – đại số mô tả
chuyển động của hệ nhiều vật hôlônôm chịu liên kết giữ đầu của các toạ độ suy rộng và các vận tốc suy rộng dư.
và lý tưởng [8, 9]. Từ các phương trình (2) và (5) ta có
- Nguyễn Văn Khang, Nguyễn Văn Quyền, Lương Bá Trường, Nguyễn Văn Long
éRT p ù
fj (q1 (0),..., q f (0), z1 (0),..., z r (0), 0) = 0, ( j = 1,..r ) s = D-1 êê 1ú
ú = g(s, s, t ) (16)
p
êë 3 úû
(8) Hệ (16) là phương trình vi phân chuyển động của hệ
f ¶f j r ¶f j ¶f j nhiều vật có cấu trúc mạch vòng trong dạng các toạ độ
å ¶q (0)q (0) + å ¶z (0) z (0) + ¶t (0) = 0,
i k (9) suy rộng có dư.
i =1 i k =1 k
( j = 1,..., r ) Chú ý : Phương pháp tích phân số phương trình (16)
thường gặp phải các sai số tích phân. Sau mỗi bước tích
Từ phương trình liên kết (8) khi biết q1 (0),..., q f (0) ta phân, do sai số tính toán, các giá trị si , si không còn
dễ dàng xác định được các giá trị gần đúng của thỏa mãn các phương trình ràng buộc vị trí và phương
z1 (0),..., z r (0) (chẳng hạn bằng phương pháp đồ thị). trình ràng buộc vận tốc và dẫn đến các sai lệch:
Lấy các giá trị này làm giá trị đầu, giải hệ phương trình f (sk , tk ) ¹ 0 , k = 1, 2,..., f (sk , sk , tk ) ¹ 0 , k = 1, 2,... (17)
đại số phi tuyến (8) bằng phương pháp lặp
Theo phương pháp Baumgarte, thay vì giải phương trình
Newton-Raphson tìm được các giá trị đầu của các toạ độ
suy rộng dư khá chính xác. f(s, s, t ) = 0
Để xác định các giá trị đầu của các vận tốc suy rộng ta sẽ tiến hành giải phương trình:
dư, ta xét hệ phương trình (9) f(s, s, t ) + 2a f (s, s, t ) + b 2 f (s, t ) = 0 (18)
r ¶f f ¶f ¶f Trong đó các hệ số được chọn thỏa mãn điều kiện sau:
å ¶zj (0) zk (0) = -å ¶qj (0)qi (0) - ¶tj (0), (10) 0 < a £ b . Các số hạng 2af và b 2 f đóng vai trò số
k =1 k i =1 i
( j = 1,..., r ) hạng điều khiển. Nhờ việc sử phương trình (18) thay cho
Hệ (10) là hệ r phương trình đại số tuyến tính của r ẩn phương trình (6) ta sẽ khử dần hoặc khử hoàn toàn được
sai số tích lũy trong quá trình tích phân. Như thế thay cho
là z1 (0),..., zr (0) . Giải hệ phương trình đại số này ta xác
giải hệ phương trình:
định được các giá trị đầu của các vận tốc suy rộng dư.
M (s, t)s + FsT (s, t ) l - p1 (s, s, t ) = 0
Phương pháp khử các nhân tử Lagrange [8, 9, 12] (19)
Khử các nhân tử Lagrange, biến đổi hệ phương trình f(s, s, t ) = 0
vi phân đại số (1), (2) về hệ phương trình vi phân thường ta sẽ tiến hành giải hệ phương trình sau:
với số phương trình bằng số tọa độ suy rộng có dư của M (s, t)s + FsT (s, t ) l - p1 (s, s, t ) = 0
hệ. (20)
f(s, s, t ) + 2a f (s, s, t ) + b 2 f (s, t ) = 0
Từ (4) ta suy ra
Hệ phương trình (20) có thể viết lại dưới dạng ma trận
RT Ms + RT FsT l = RT p1 (11)
êM (s, t ) F (s, t)ú ê s ú = ê p1 (s, s, t )ú
é T ùé ù é ù
é E ù ê F s, t ú (21)
Trong đó R(q, z ) = êê -1 úú , êë ( ) 0r´r ú êl ú ê p3 (s, s, t )úú
ê ú ê
û ë û ë û
êë-Fz Fq úû
trong đó
é ¶f ù é ¶f ¶f1 ùú
ê 1 ....... ¶f1 ú ê 1 ....... p3 (s, s, t ) = p2 (s, s, t ) - 2a êé F (s, t)s + ft ùú - b 2 f (s, t )
ê ú ê ú ë û
ê ¶z 1 ¶z r ú ê ¶q 1 ¶q f ú é ¶F ¶F ù
ê ¶s ( n
Fz = ê ... ....... ... úú , Fq =
ê ê ú = -ê E Ä s ) + + 2aF (s, t)ú s - 2a ft - ftt
ê ... ....... ... ú ¶t ú
ê ú ê ú ë û
ê ¶fr ¶fr ú ê ¶fr ¶fr ú (22)
ê ....... ú ê ....... ú
ê¶ ¶z r úû ê ¶q 1 ¶q f ú
ë z1 ë û 3. Bài toán động lực học thuận tay máy robot
Sử dụng định lý trực giao [9] hai khâu có liên kết chương trình
RT FsT = 0 (12)
Áp dụng phương pháp giải hệ phương trình vi phân –
Từ (11) ta có đại số trình bày ở mục trên, ta nghiên cứu bài toán động
RT Ms = RT p1 (13) lực học thuận tay máy robot hai khâu có liên kết chương
Hệ hai phương trình (13) và (6) có thể viết lại dưới dạng trình như hình 1. Liên kết chương trình ở đây là đòi hỏi
ma trận như sau điểm thao tác E chuyển động trên một quỹ đạo cho trước.
éRT M ù é T ù Cho tay máy 2 khâu chuyển động trong mặt phẳng thẳng
ê ú s = êR p1 ú (14) đứng. Thanh OO1 đồng chất chiều dài l1 , khối lượng
ê F ú ê p ú
ëê s ûú ëê 3 ûú m1 , khối tâm tại C1. Thanh O1E đồng chất chiều dài l2 ,
Nếu ta đưa vào ký hiệu
éRT M ù khối lượng m2 , khối tâm tại C2. Điểm thao tác E có khối
D = êê ú
ú (15)
F lượng mE chuyển động trên đường thẳng AB.
êë s úû
thì từ (14) ta suy ra hệ phương trình vi phân sau :
- Động lực học tay máy Robot có liên kết chương trình
(26)
Các lực suy rộng của các lực hoạt động không thế
Q1* = t1,Q2* = t2 (27)
Phương trình ràng buộc điểm thao tác E luôn chuyển
động trên một đường thẳng AB
xE - xA y - yA x y
= E hay E + E - 1 = 0
xB - xA yB - yA l3 l4
Thay tọa độ điểm E từ (24) vào phương trình ràng buộc
trên ta có phương trình liên kết:
f (q1, q2 ) = l1l 3 sin q1 + l1l 4 cos q1
(28)
+l2l 3 sin (q1 + q2 ) + l2l 4 cos (q1 + q2 ) - l 3l 4 = 0
Sử dụng phương trình Lagrange loại 2 dạng nhân tử
d æç ¶T ö÷÷ ¶T ¶P ¶f
çç
dt èç ¶qk ø÷ ¶qk
÷÷ - =-
¶qk
+ Qk* - l
¶qk
(k = 1, 2)
Hình 1. Tay máy Robot hai khâu
Ta thu được hai phương trình chuyển động của robot.
Chọn các tọa độ suy rộng là q1 và q2 . Mômen quán tính éæ m ö ù
êçç 1 + m + m ÷÷l 2 + (m + 2m )l l cos q ú
khối đối với trục quay đi qua khối tâm của các khâu lần êçç 3 ÷
E÷ 1 2ú
ø÷
2 2 E 12
êè ú q
1 1 ê æ ö ú 1
lượt là JC = m1l12 ,JC = m2l22 ê ç 2 m ÷ ú
1
12 2
12 ê+ çç + mE ÷÷l22 ú
ê çè 3 ÷÷ø ú
Vận tốc góc của các khâu: w1 = q1, w2 = q1 + q2 . ë û
éæ m ö÷ æm ö÷ ù
Tọa độ các khối tâm và điểm thao tác ê ç
+ êçç + mE ÷÷l1l2 cos q2 + çç ç 2 ú
+ mE ÷÷l2 ú q2
2
él ù êëçè 2 ø÷ çè 3 ÷÷ø ú
û
éx ù êê 1 cos q1 úú æm
ê C1 ú ç ÷ö
êy ú = êê l
2 ú; - (m2 + 2mE )l1l2 q1q1 sin q2 - çç 2
+ mE ÷÷l1l2 q22 (29)
êë C1 úû ê 1 sin q úú çè 2 ÷÷ø
êë 2 1
úû æm ö÷
é l2 ù
(23) + ççç 1 + m2 + mE ÷÷ gl1 cos q1
èç 2 ÷ø÷
éx ù êl1 cos q1 + cos (q1 + q2 )ú
ê ú
ê C2 ú = ê 2 æm
êy ú ê ú ÷ö
êë C 2 úû ê l sin q + l2 sin q + q úú + ççç 2 + mE ÷÷ gl2 cos (q1 + q2 )
( 1 2 )ú çè 2 ÷÷ø
ëê
1 1
2 û
æ ÷÷ö
éx ù él cos q + l cos (q + q )ù çl l cos q1 - l1l 4 sin q1
ê Eú = ê1 2 ú = t1 - l çç 1 3 ÷
1 2
êy ú ê l sin q + l sin q + q ú
1
(24) ççè+l2l 3 cos (q1 + q2 ) - l2l 4 sin (q1 + q2 )÷÷ø
ê
ëê E ûú ë 1 1 2 ( 1 2 ) úû
éæ ö æ ö ù
Động năng của tay máy: êçç m2 + m ÷÷l l cos q + çç m2 + m ÷÷l 2 ú q
êçç 2 E÷÷12 ç E÷ 2ú 1
ø÷ çè 3 ø÷÷ úû
2
1 1 1 1 1 êëè
T = m1vC2 + JC w12 + m2vC2 + JC w22 + mE vE2
2 2 2 2 2 æm ö÷ æm ö÷
+ ççç 2 + mE ÷÷l22 q2 + ççç 2 + mE ÷÷l1l2 q22 sin q2
1 1 2 2
Với vC2 = xC2 + yC2 ;vC2 = xC2 + yC2 ;vE2 = xE2 + yE2 çè 3 ÷÷ø èç 2 ø÷÷ (30)
1 1 1 2 2 2
æm ö÷
Ta dễ dàng tính được biểu thức động năng của tay máy: + ççç 2 + mE ÷÷ gl2 cos (q1 + q2 )
éæ ö æ ö ù èç 2 ÷ø÷
êçç m1 + m2 + mE ÷÷l 2 + çç m2 + m ÷÷l l cos q ú
T = êê
êçç 6
è 2
÷÷ 1 ç
÷ø èç 2
E÷÷12
÷ø 2ú
ú q2 (
= t2 - l1 l2l 3 cos (q1 + q2 ) - l2l 4 sin (q1 + q2 ) )
æ ö ú 1
ê ç m2 mE ÷÷ 2 ú Các phương trình vi phân (29), (30) và phương trình đại
ê+ çç + ÷÷l2 ú số (28) tạo thành hệ 3 phương trình vi phân – đại số mô
ê çè 6 2 ÷ø ú
ë û tả chuyển động của tay máy robot.
éæ m ö÷ æm ö÷ ù
+ êêççç 2 + mE ÷÷l1l2 cos q2 + ççç 2 + mE ÷÷l22 úú q1q2 (25) 4. Mô phỏng số
ç 2 ÷ø÷ èç 3 ø÷÷ ûú
ëêè
æm m ö÷ Để giải hệ phương trình vi phân đại số trên ta cần
+ ççç 2 + E ÷÷l22 q22
èç 6 2 ÷ø÷ biết các tham số của hệ và các điều kiện đầu. Các tham
số của robot được cho như sau
Thế năng của tay máy:
l1 = 2 éëêm ùûú , l2 = 3.5 éëêm ùûú , l 3 = 5 éëêm ùûú , l 4 = 5 éëêm ùûú ,
= m1gyC + m2gyC + mE gyE
æm
1
ö÷
2
æm ö÷ m1 = 200 éëêkg ùûú , m2 = 35 éëêkg ùûú , mE = 25 éëêkg ùûú ,
ç + m2 + mE ÷÷l1 sin q1 + ççç 2 + mE ÷÷l2 sin (q1 + q2 )
= çç J 1 = 450 éêkg ⋅ m 2 ùú ,J 2 = 35 éêkg ⋅ m 2 ùú .
1
çè 2 ÷÷ø èç 2 ø÷÷ ë û ë û
- Nguyễn Văn Khang, Nguyễn Văn Quyền, Lương Bá Trường, Nguyễn Văn Long
Giả sử các lực tác dụng lên các khâu dẫn có dạng
t1 = 0.1 ⋅ sin(2pt ) éëêN ⋅ m ùûú ,
t2 = 0.1 ⋅ sin(2pt ) + 15000 éëêN ⋅ m ùûú .
Các điều kiện đầu của các tọa độ suy rộng độc lập được
cho trước, còn điều kiện đầu của các tọa độ suy rộng dư
được xác định từ phương trình liên kết. Ta có
é rad ù
q1 (0) = 0.5781 éêërad ùúû , q1 (0) = 1.5457 ê ú,
ê s ú
ë û
é rad ù
é ù
q2 (0) = -0.8957 ëêrad ûú , q2 (0) = -1.7494 ê ú.
ê s ú
ë û
Sử dụng phần mềm MATLAB tính toán mô phỏng số. Hình 4. Sai số của phương trình liên kết
Một phần các kết quả tính toán được trình bày trên các
hình từ 2 đến 9. Trong đó hình 2 là đồ thị các góc quay Để có thể kiểm tra độ chính xác của thuật toán chúng ta
q1 và q2 . Hình 3 là đồ thị vận tốc các góc này. Hình 4 là biểu diễn dịch chuyển góc và vận tốc góc trên cùng một
hình vẽ như hình 5 và 6. Trong đó hình 5 là đồ thị của q1
đồ thị biểu thị sai số tính toán thể hiện qua phương trình
liên kết. Kết quả trên đồ thị này cho ta độ chính xác rất và q1 còn hình 6 là đồ thị của q2 và q2 . Hình 7 là đồ
cao của thuật toán. thị nhân tử Lagrange. Trong [9] đã chỉ ra ý nghĩa của
nhân tử Lagrange.
Hình 2. Đồ thị các góc khớp
Hình 5. Đồ thị q1 và q1
Hình 3. Đồ thị các vận tốc góc khớp
Hình 6. Đồ thị q2 và q2
- Động lực học tay máy Robot có liên kết chương trình
Trên hình 9 biểu diễn cấu hình của tay máy (robot arm
configuration) theo thời gian. Trong đó đường màu đỏ là
vị trí ban đầu của tay máy, đường màu tím là vị trí cuối
của tay máy.
5. Kết luận
Động lực học tay máy robot chịu các liên kết chương
trình là bài toán có ý nghĩa khoa học và thực tế. Bài toán
này liên quan trực tiếp với bài toán lập trình quỹ đạo cho
chuyển động robot. Trong bài báo này đã trình bày các
nghiên cứu về bài toán động lực học thuận robot có liên
kết chương trình. Các kết quả mô phỏng số cho thấy
phương pháp nghiên cứu và thuật toán đề xuất có khả
năng áp dụng cao. Bài toán động lực học ngược đang
Hình 7. Đồ thị nhân tử Lagrange theo thời gian được nghiên cứu trong nhóm và sẽ công bố trong thời
Một trong các yêu cầu của bài toán động lực học tay máy gian tới.
robot có liên kết chương trình là kiểm tra xem điểm thao
tác E (hoặc khâu thao tác) có thực hiện đúng chuyển Lời cảm ơn
động trên quỹ đạo chương trình hay không. Vấn đề này
trong các tài liệu [1-7] chưa được đề cập đến. Trong bài Bài báo này được hoàn thành với sự tài trợ bởi Quỹ Phát
báo này chúng tôi đặc biệt quan tâm đến vấn đề này. Dựa triển Khoa học và Công nghệ Quốc gia (NAFOSTED).
trên kết quả giải hệ phương trình vi phân đại số, trên hình
8 biểu diễn quỹ đạo chuyển động của điểm thao tác E.
Tài liệu tham khảo
[1] Rosen A., E. Edelstein, Investigation of a new
formulation of the Lagrange method for constrained
dynamic systems, ASME-Journal of Applied Mechanics,
vol. 64, pp.116-122, 1987.
[2] Rosen A., Applying the Lagrange method to solve
problems of control constraints, ASME-Journal of
Applied Mechanics, vol. 66, pp.1013-1015, 1989.
[3] Jankowski K., Dynamics of controlled mechanical
systems with material and program constraints - I Theory,
Mechanism and Machine Theory, vol.24, No.3,
pp.175-179, 1989.
[4] Jankowski K., Dynamics of controlled mechanical
systems with material and program constraints-II
Hình 8. Quỹ đạo điểm E trong mặt phẳng Methods of solution, Mechanism and Machine Theory,
vol.24, No.3, pp.181-185, 1989.
[5] Jankowski K., Dynamics of controlled mechanical
systems with material and program constraints-III
Illustrative examples, Mechanism and Machine Theory,
vol.24, No.3, pp.187-193, 1989.
[6] Do Sanh, Dinh Văn Phong, Do Dang Khoa, Tran Duc,
Control of program motion of dynamic system using
relative motion, Technische Mechanik, Band 28, Heft 3-4,
pp. 211-218, 2008.
[7] Vu Duc Binh, Do Dang Khoa, Phan Dang Phong, Do
Sanh, Program motion of unloading manipulatos,
Vietnam Journal of Science and Technology 56(5),
662-670, Hanoi 2018.
[8] De Jalon J. G., E. Bayo, Kinematic and Dynamic
Simulation of Multibody Systems. Springer-Verlag, New
Hình 9. Cấu hình của robot
York, 1994.
- Nguyễn Văn Khang, Nguyễn Văn Quyền, Lương Bá Trường, Nguyễn Văn Long
[9] Nguyễn Văn Khang: Động lực học hệ nhiều vật (in lần thứ
2). NXB Khoa học và Kỹ thuật, Hà Nội 2017.
[10] Nguyen Van Khang: Ein Beitrag zur dynamischen
Analyse ebener Koppelgetriebe mit mehreren
Freiheitsgraden mit Hilfe der numerischen Lösung der
Bewegungsdifferentialgleichungen. Diss. A, TH
Karl-Marx-Stadt, 1973.
[11] Nguyen Van Khang, Über eine Methode zur Lösung der
Bewegungs- gleichungen von Mechanismen mit mehreren
Freiheitsgraden. WZ der TH Karl-Marx-Stadt 17, N.1,
S.59-70, 1975.
[12] Nguyễn Văn Khang, Nguyễn Văn Quyền, Nghiên cứu so
sánh một vài phương pháp giải hệ phương trình vi
phân-đại số của hệ nhiều vật có cấu trúc mạch vòng.
Tuyển tập Hội nghị Cơ học kỹ thuật toàn quốc,
03-05/8/2015, Tập 2, Tr. 147-158, Đà Nẵng 2016.
nguon tai.lieu . vn