Để 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:
J q J q
q q
Mơ hình động lực có thể viết lại như sau
R T M ( q ) R T B - RT
J q q = 0 u +
để 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 tố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
Hình 3.5: Sơ đồ bài tốn động lực học ngược
Bài toán động lực học ngược được phát biểu dưới dạng: Cho biết quy luật chuyển động của khâu thao tác x x t ,x m và phương trình liên kết
f q , x , t 0, q n ,f r . Xác định mô men/lực của khâu dẫn động u n a (hoặc
điện áp điều khiển với các mơ hình được tích hợp hệ dẫn động) cần thiết để tạo ra chuyển động mong muốn của khâu thao tác.
Trong q trình giải bài tốn động lực học ngược ta sử dụng ma trận R để khử nhân tử Lagrange , khi đó mơ men/lực phát động (hoặc điện áp đặt vào động cơ với hệ được mô tả bao gồm động lực học của động cơ dẫn động) của các khâu dẫn được tính tốn dựa trên biến đổi phương trình (3.54)
u [R T B ] 1R T M (q )q C (q , q )q h (q) (3.57)
Cần lưu ý rằng, với cấu trúc ma trận B có dạng B = [kEn n 0 n ( m n) ]T , tích R T B là ma
trận vng dạng kEn n .
Trong trường hợp kỳ dị, ma trận đứng trước u trong phương trình (3.54) sẽ khơng cịn vng nữa. Do đó, tại các vị trí lân cận kỳ dị ta tính u theo cơng thức sau:
R T Bu = f ( q , q , q) => u = A f ( q , q , q) (3.58)
Các mô phỏng số kiểm chứng lý thuyết đề xuất được trình bày trong phần tiếp sau đây.
3.3.3 So sánh kiểm chứng mơ hình và chương trình tính
3.3.3.1 Mơ phỏng chuyển động theo quỹ đạo tham chiếu
Thực hiện mơ phỏng số bài tốn động lực học ngược với đối tượng là robot song song phẳng 3RRR. Robot được mô phỏng di chuyển trên một quỹ đạo trịn được xác định như sau:
- Bán kính quỹ đạo: R 0.1[ m], tọa độ tâm: rC OC.[cos( / 6); sin( / 6)] . Trong quá trình di chuyển bàn máy động ln giữ một góc khơng đổi 0[ rad].
- Thực hiện hai mô phỏng số tương ứng với trường hợp tâm bàn máy động đi vào vùng kỳ dị và khơng đi vào vùng kỳ dị. Bán kính quỹ đạo được chọn tương ứng như sau:
o Không đi qua vùng kỳ dị: OC L1 L2 a 3 / 3 R 0.05
o Có đi qua vùng kỳ dị: OC L1 L2 a 3 / 3 R
Kết quả mô phỏng khơng đi qua vùng kỳ dị
[rad]
Hình 3.6: Đồ thị biến khớp chủ động
[Nm]
Hình 3.8: Đồ thị mơ men tác động tại các khớp chủ động Hình 3.7: Đồ thị biến khớp bị động 30 20 u1(t) [V] u2(t) [V] [V ] 10 u3(t) [V] u( t) 0 -10 -20 5 10 15 0 t [s]
Hình 3.9: Đồ thị điện áp điều khiển
detJ
Hình 3.10: Đồ thị giá trị định thức tương ứng với quỹ đạo chuyển động
Trong trường hợp này, quỹ đạo chuyển động của bàn máy động được thiết kế không đi qua điểm kỳ dị nào nên các chuyển động của các khớp là hồn tồn trơn tru.
Kết quả mơ phỏng đi qua vùng kỳ dị
Thực hiện mô phỏng số với quỹ đạo dịch chuyển tâm bàn máy động tiếp xúc với biên giới làm việc, như vậy trong mỗi chu kỳ dịch chuyển, robot đi qua vùng kỳ dị một lần. Kết quả mô phỏng số được thể hiện ở các đồ thị sau:
3 2 1 [rad] 1 2 0 1 [rad] 2 [rad] -1 1 3 [rad] 2 [rad] -2 3 [rad] 0 -3 0 5 10 15 0 5 10 15 t [s] t [s] Hình 3.11: Đồ thị biến khớp chủ động Hình 3.12: Đồ thị biến khớp bị động [N m ] u( t) [V ]
Hình 3.13: Đồ thị mơ men tác động tạicác các
Hình 3.14: Đồ thị điện áp điều khiểnkhớp chủ động khớp chủ động 0 -0.01 -0.02 -0.03 0 5 10 15 t [s]
Trường hợp này, quỹ đạo chuyển động của tâm bàn máy động được thiết kế để di chuyển trên một vòng tròn, mỗi chu kỳ chuyển động đi qua điểm kỳ dị một lượt. Với thời gian mô phỏng là 15 giây, bàn máy động đi chuyển qua điểm kỳ dị hai lần. Các vị trí này có thể quan sát thấy tại các điểm biến động trên các đồ thị. Với việc sử dụng thuật tốn vượt kỳ dị, đã giúp cho chương trình tránh được trạng thái kỳ dị để duy trì hoạt động bình thường cho robot song song.
3.3.3.2 Mô phỏng so sánh với các nghiên cứu khác
Để khẳng định tính chính xác của mơ hình động lực học robot song song 3RRR đã được xây dựng trong chương này. Mô phỏng số so sánh được thực hiện với các nội dung như sau:
- Mơ hình và kết quả tham chiếu sử dụng theo nghiên cứu của T.Geike và J. McPhee [100].
- Các tham số trong mơ hình robot song song tham chiếu được sử dụng để tính tốn với mơ hình dã được xây đựng như sau:
o Khoảng cách các gốc: L0 1.0 [ m]
o Chân của robot có: chiều dài: l1 0.4 [ m] , l2 0.6 [ m] , khối lượng:
m 3.0 [ kg], m 4.0 [ kg] , mô men quán tính: Ic1
0.04 [ kgm2 ] ,
1 2
Ic2 0.12 [ kgm2 ]
o Kích thước bàn máy động (cạnh tam giác): b 0.4 [ m] , mô men quán
tính Ic2 0.0817 [ kgm2 ]
- Quỹ đạo chuyển động được thiết lập trong khoảng thời gian T 3 [ s], quy luật
chuyển động của các biến khớp chủ động như sau:
o 1 1 2 t sin 2 t 3 6 T T o 4 1 2 t sin 2 t 236TT o 3 11 1 2 t sin 2 t 6 12 T T
- Tọa độ suy rộng dư được chọn tương đương q [ 1 ; 2 ; 3 ; 1 ; 2 ; 3 ; x P ; yP ; ] . Điều kiện đầu được chọn
q ; 4 ; 11 ; 0.865; 2.102; 0.976;0.728;0.233;0.3916
0
Thực hiện mô phỏng số trên phần mềm Matlab ta thu được đồ thị biến thiên mô men khớp chủ động như sau:
[Nm]
a) Kết quả sử dụng mơ hình của luận án b) Kết quả trong tài liệu [100] Hình 3.16: Đồ thị biến thiên mô men khớp chủ động mô phỏng sử dụng bộ tham số của
T.Geike và J. McPhee [100]
Nhận thấy kết quả mơ phỏng hồn tồn tương đồng với kết quả được cơng bố trong [100]. Như vậy, mơ hình động lực học của robot song song phẳng 3RRR được thiết lập trong chương này là tương đồng với nghiên cứu đã được cơng bố trên tạp chí uy tín. Vì vậy, mơ hình được thiết lập và các chương trình tính tốn trong các phần đã thực hiện của luận án đảm bảo tính tin cậy để sử dụng cho các thực nghiệm số được tác giả thực hiện trong các phần tiếp theo.
3.4 Vấn đề ổn định hóa liên kết
Do phương trình liên kết sử dụng trong mô phỏng ở mức gia tốc nên dẫn tới sai số tích lũy sau mỗi vịng lặp của thuật tốn. Do đó việc ổn định hóa các liên kết là rất cần thiết nhằm đảm bảo độ chính xác kết quả tính tốn thơng tin động lực của robot song song.
3.4.1 Phương pháp Baumgarte
Ở đây phương pháp ổn định hóa Baumgarte được sử dụng trong mơ phỏng [101]. Ý tưởng của kỹ thuật này là thay vì sử dụng trực tiếp phương trình ràng buộc dạng f(q ) 0 , ta sử dụng phương trình sau:
2 (3.59)
f (q ) 2 0f (q ) 0 f(q ) 0,
với hai tham số của phương pháp được chọn 0 1 và 0 0 . Hiển nhiên rằng, nghiệm của (3.59) có dạng
Tính chất quan trọng của nghiệm này là nó tiến về khơng khi thời gian mơ phỏng tăng lên. Từ phương trình ràng buộc ở mức vị trí ta biến đổi được
2 (3.60)
J (q )q + J (q )q + 2δω0 J (q )q + ω0f(q) = 0
hay
J( q ) q = - J ( q ) q - 2δωJ ( q ) q - ω2f( q)
Chú ý rằng đạo theo thời gian ma trận Jacobi Jq (q) có thể được tính dựa vào tích Krưnecker [102]
Jq (q) = Jq (q) (E q)
q
Như vậy, mơ hình động lực học robot song song trở thành
M s (q )q C s (q , q )q Dsq g s (q ) B s u+ J qT ( q )λ
J q (q )q J q (q )q 2 0 Jq (q )q 02f(q ) 0
Sau khi khử nhân tử Lagrange ta nhận được
R T M s (q )q C s (q , q )q D sq g s (q ) R T B s u (3.61)
2
J q (q )q J q (q )q 2 0 Jq (q )q 0 f(q ) 0
Viết lại phương trình dưới dạng ma trận như sau
R T (q )M (q ) R T (q )B R T (q ) C (q , q )q h (q) (3.62)
J (q ) q 0 u
J (q )q 2 J (q )q 2f(q)
Hệ phương trình (3.61) được sử dụng để giải và đem lại thông tin phản ứng của robot song song mà không bị phá vỡ liên kiết sau mỗi vòng lặp giải.
3.4.2 Phương pháp chiếu hiệu chỉnh
Một phương pháp khác để ổn định hóa liên kết là hiệu chỉnh sau trình bày trong [103]. Theo đó, sau mỗi hoặc một số bước tích phân các giá trị tìm được sẽ được hiệu chỉnh để thỏa mãn các điều kiện của liên kết. Phương án các tác giả đề xuất là hiệu chỉnh tất cả các tọa độ độc lập và phụ thuộc. Số biến được điều chỉnh lớn và do đó địi hỏi chi phí thời gian tính tốn cũng lớn theo. Dựa trên ý tưởng của phương pháp chiếu, trong phần này các tọa độ độc lập sẽ được giữ lại còn các tọa độ phụ thuộc sẽ được điều chỉnh và do đó số lượng các biến được điều chỉnh giảm xuống. Nội dung của phương pháp được trình bày như dưới đây. Giả sử sau khi tích phân ta nhận được các kết quả q * , q* , bây giờ ta sẽ hiệu chỉnh các kết quả này cho phù hợp
với các ràng buộc. Các hiệu chỉnh sẽ được thực hiện ở hai mức: mức vị trí và mức vận tốc.
3.4.2.1 Hiệu chỉnh vị trí (chiếu trên đa tạp xác định bởi phương trình liên kết)
Từ kết quả tích phân vừa tính q *T [q iT ,qd*T ], ta sẽ hiệu chỉnh qd* để q thỏa
mãn các phương trình liên kết. Bằng cách đưa vào hàm V cần đạt cực tiểu với ràng buộc là các phương trình liên kết
V 1(q d q d* )T M (q d qd* )min với ràng buộc f(q i , q d ) 0 , (3.63)
2
với M là ma trận xác định dương đây chính là ma trận trọng số. Có thể thấy nghiệm của bài tốn này là điểm nằm trên đa tạp xác định bởi các phương trình liên kết nhưng gần với q* nhất qua ma trận trọng số M .
Sử dụng phương pháp hàm phạt (hay phương pháp nhân tử Lagrange tăng cường), ta chọn hàm Lagrange được như sau
L V f T (q )Af (q ) l T f(q) min , (có thể lấy l 0 ) (3.64)
với A là ma trận đường chéo và xác định dương, đó là các nhân tố phạt (penalty
factor). Đạo hàm phương trình (3.64) và cho bằng 0 ta được
h (q d , t) L M (q d q d* ) J qT Af (q ) J qT l 0 (3.65)
qd
d d
Đây là hệ r m n phương trình đại số phi tuyến và có thể giải bằng phương pháp lặp Newton-Raphson. Khai triển Taylor hàm h (qd , t) ở lân cận qo ta nhận được
h (q d ,0 Dq d , t) h (q d ,0 ) H (q d ,0 )Dqd ...
với H (q d ,0 ) qd [M (q d q d* ) J qTd Af (q ) J qTd l ]q d,0 M [J qTd Af (q )]qd [JqTd l]qd .Nếu
chọn l 0, ta nhận được
H (q o ) M [J qTd Af (q )]q d M [J qTd AJqd ] . (3.66)
Trong đó số hạng liên quan đến ma trận J q ,q (q) đã được bỏ qua. Từ các phân
d d
1) k = 0, cho biết số bước lặp N, q d(k) qd*
2) Tính h (qd(k) )
3) Kiểm tra điều kiện nếu h (qd(k) ) hoặc k N thì dừng, trái lại tiếp tục 4
4) Tính ma trậnH (qd(k) ) , giải hệ h (q d(k ) ) H (q d(k) )Dq d 0 tìm Dqd
5) Tính giá trị mới q d(k 1) q d(k) Dqd ,
6) Tăng k, k = k +1; quay lại 2).
3.4.2.2 Hiệu chỉnh vận tốc (chiếu trên đa tạp xác định bởi phương trình liên kết vận tốc)
Từ kết quả tích phân vừa tính q* , ta sẽ giữ lại qi* và hiệu chỉnh qd* để đạt được
q thỏa mãn các phương trình liên kết ở mức vận tốc. Bằng cách đưa vào hàm V cần
đạt cực tiểu với ràng buộc là các phương trình liên kết vận tốc
V12 (q d q d* )T M (q d qd* ) min
với ràng buộc
q d 0 . (3.67)
f(q ) J q q i J q
i d
với M là ma trận xác định dương, đây chính là ma trận trọng số. Có thể thấy nghiệm của bài toán này là điểm nằm trên đa tạp của phương trình liên kết vận tốc nhưng gần với q* nhất qua trọng số M .
Sử dụng phương pháp hàm phạt bằng cách tìm điểm cực tiểu của hàm sau
L 1 (q d q d* ) T M (q d q d* )1[J q q i J q q d ]T A[J q q i J q qd ] min (3.68)
2 2 i d i d
với A là ma trận đường chéo và xác định dương. Đạo hàm theo biến qd và cho bằng 0 ta được
L M (q d q d* ) J qT A[J q q i J q q d ] 0
qd
d i d
hay
[M J qT (q )AJ q (q )]q d Mq d* J qT (q )AJ q (q )qi . (3.69)
d d d i
q d [M J qT (q )AJ q (q )] 1 [Mq d* J qT (q )AJ q (q )qi ]. (3.70)
d d d i
Như vậy trong quá trình hiệu chỉnh ta đã tìm được trạng thái của hệ thỏa mãn các điều kiện liên kết của hệ, các kết quả này sẽ được sử dụng cho bước tính tiếp theo.
3.5 Vượt kỳ dị trong phân tích động lực học robot song song3.5.1 Cơ sở của phương pháp 3.5.1 Cơ sở của phương pháp
Việc mô phỏng số động lực học robot song song đòi hỏi phải giải hệ phương trình vi phân đại số (3.19) và (3.20). Hệ này có thể giải bằng các phương pháp biến đổi về dạng phương trình đại số phi tuyến, các phương pháp số để giải các phương trình này thường tồn tại sai số sau mỗi vịng lặp. Vì vậy, cần có giải pháp khắc phục các vấn đề này.
Hai phương pháp tách và khử nhân tử chỉ sử dụng được khi ma trận Jacobi Jq có hạng đầy đủ, tức là rank [Jq ] r , bằng số các phương trình liên kết. Tuy nhiên, như đã biết các mơ phỏng số có thể sụp đổ hay bị kẹt tại một số cấu hình kỳ dị, nơi mà hạng của ma trận Jacobi bị giảm, rank [Jq ] r . Vị trí và thời điểm xảy ra điều đó,
phụ thuộc vào cấu trúc cơ hệ và các thông số của hệ. Để đảm bảo mô phỏng liên tục và thông suốt, chúng ta cần phải tìm một giải pháp để đối phó với các điểm kỳ dị này. Để vượt qua kỳ dị, trong phần này, ma trận khử được tính tốn sử dụng khơng gian bù hay không gian rỗng của ma trận Jacobi.
Gọi N là ma trận bù của ma trận Jacobi, tức J q N 0 hayD T J qT 0 . Thực
hiện nhân từ trái ma trận N T với phương trình (3.19) ta được
N T M(q )q N T [C(q , q )q N q g (q )] N T B u (3.71)
s s s s s
Kết hợp với phương trình (3.55) ta có được
N T M (q ) N T B u N T [C (q , q )q D q g (q)], đặt H N T M (q)(3.72)
s q s s s s s
J
q J qq Jq
Lưu ý rằng, từ phương trình (3.72) ta luôn xác định được véc tơ gia tốc suy rộng q . Vì rằng, theo định lý về tổng hạng của ma trận và của không gian bù của ma trận đó, ta ln có rank [J q ] rank [N] m [104]. Do đó, rank(H ) m vì M s (q) là ma trận chính qui. Tức là khi ma trận Jacobi giảm hạng thì hạng của ma trận bù sẽ tăng. Giải hệ phương trình (3.72) ta nhận được:
N T M (q ) N T B u N T [C (q , q )q D q g (q)] (3.73)
q s s s s s
J
q J qq
Như vậy, véc tơ gia tốc suy rộng q luôn được xác định trực tiếp từ (3.73) và
nghiệm này khơng phụ thuộc vào cấu hình của hệ là kỳ dị hay khơng kỳ dị. Lưu ý rằng, tại các cấu hình khơng kỳ dị H chính là H 1 . Phép tính tựa nghịch đảo chỉ phải dùng đến chỉ tại các cấu hình kỳ dị, do số hàng của H tăng lên. Nếu tìm cách xóa bỏ một số hàng của ma trận Jacobi để H thành vng thì ta vẫn sử dụng ma trận nghịch đảo được. Để cho đơn giản mà khơng phải dị tìm cấu hình kỳ dị trong q trình mơ phỏng ở đây ta sử dụng phép tính tựa nghịch đảo cho cả hai trường hợp khơng kỳ dị và kỳ dị. Đó là một ưu điểm nổi bật khi khử nhân tử Lagrange bằng ma trận bù thay cho ma trận khử tính theo (3.52) hoặc (3.53). Cần nhắc lại rằng, tổng số các phương trình của hệ (3.72) là m , do rank[Jq ] rank [N T Ms ] m đối với tất cả các vị trí: kỳ dị và
khơng kỳ dị.
Một trong các điểm mấu chốt của thuật toán đề xuất là việc xây dựng ma trận
N . Trong trường hợp khảo sát với các liên kết lý tưởng, ma trận này có thể xác định bằng phương pháp giải tích hoặc phương pháp số. Tại các vị trí khơng kỳ dị của cơ hệ, ma trận N được xác định trực tiếp bởi (3.52) hoặc tính tốn bằng phương pháp số. Tại các cấu hình kỳ dị, nói chung, phương pháp số nên được áp dụng để tìm ma trận
N từ tính chất J q (q )N 0 [26]. Phương pháp khử Gauss hoặc biến đổi về dạng
hàng thu gọn và cũng là một lựa chọn để tìm ma trận bù N . Với phần mềm Matlab ta