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)
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)
với A= R T B, A [AT A] 1AT
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ị
[r ad ] Hình 3.6: Đồ thị biến khớp chủ động [N m ]
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 [V ] u( t) t [s]
de
tJ
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 0 0 Hình 3.11: Đồ thị biến khớp chủ động [N m ] khớp chủ động -0.01 -0.02 -0.03 0 5 10 15 t [s]
Hình 3.15: Đồ thị giá trị định thức tương ứng với quỹ đạo chuyển động
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:
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 qn
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 o 4 1 2 t sin 2 t 236TT o 3
- 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 ;
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:
[N
m
]
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:
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
f (t ) f(t0 ) exp(0 t)sin( t ) , 02 2
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
J (q )q + J (q )q
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
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 )
J (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
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 fT (q )Af (q ) lT f(q) min , (có thể lấy l 0 )
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)
Đâ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 ] .
Trong đó số hạng liên quan đến ma trận J q ,q
d d
tích trên, ta đưa ra được thuật giải lặp hiệu chỉnh qd
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
f(q ) J q q i J q
i
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
2
với A là ma trận đường chéo và xác định dương. Đạo hàm theo biến qd
69
q d [M J qT
d
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 song 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
Kết hợp với phương trình (3.55) ta có được
N T M
s
J
q
Lưu ý rằng, từ phương trình (3.72) ta ln 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:
70
N T M (q )
q
J
q
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 dễ dàng nhận được ma trận bù của ma trận Jacobi bằng lệnh null() . Điều đáng chú
ý ở đây là phương pháp tiếp cận đề xuất với ma trận không gian bù N , ta không cần thiết phải tìm ra các vị trí kỳ dị để mơ phỏng trơn tru.
Khi giải hệ phương trình vi phân đại số chúng ta đã sử dụng phương trình liên kết ở mức gia tốc, do đó các biện pháp ổn định hóa liên kết là rất cần thiết. Phương pháp ổn định Baumgarte [101][105] đơn giản và dễ triển khai nên được sử dụng nhiều
nhất. Ý tưởng của phương pháp là thay vì sử dụng (q ) 0 , ta sử dụng phương trình f vi phân sau.
f (q ) 2 0f (q ) 0 f(q ) 0 .
hay
phương pháp này lại không đảm bảo ổn định liên kết khi hệ chuyển động qua các cấu hình kỳ dị.
3.5.2 Nội dung mô phỏng số
Thực hiện mô phỏng số với đối tượng là robot song song phẳng được xác định trong mục 1.5. Nhằm tăng tốc độ tính tốn, trong nội dung mơ phỏng này sử dụng mơ hình động lực học của robot dưới dạng rút gọn. Mơ hình có bao gồm cả thành phần mơ tả ảnh hưởng của động cơ dẫn động.
Hai mô phỏng được thực hiện, mô phỏng thứ nhất thực hiện cho bài toán động lực học thuận với các điện áp đặt lên các động cơ tương ứng là u1 u3 15 V, u2 15 V.
Trong mô phỏng thứ hai, luật điều khiển PD được sử dụng để đưa các tọa độ dẫn về vị trí mong muốn qad [0,1.5, 1.5]T [rad] với các tham số điều khiển Kp 150 , KD 6 .
Trong các mô phỏng này, cả hai kỹ thuật ổn định đều được sử dụng.
3.5.3 Kết quả mô phỏng
Các kết quả mơ phỏng được đưa ra trên các Hình 3.17, Hình 3.18 và Hình 3.19.
(a) Đồ thị biến khớp chủ động (b) đồ thị biến khớp bị động
Hình 3.17: Kết quả của bài toán động lực học thuận