Bài toán động học thuận: Đây là bài toán cho trước chương trình chuyển động dưới dạng quan hệ hàm qi(t) của các biến khớp, ta cần phải xác định quy luật biến đổi của các tham số động học đặc trưng cho chuyển động của các khâu. Việc giải bài toán thuận của động học robot chủ yếu nhằm thiết lập phương trình động học robot và xác định vị trí của tay kẹp.
a, Phương pháp Denavit- Hartenberg.
Cơ sở của phương pháp Denavit-Hartenberg (D-H).
Theo DH, tại mỗi khớp ta gắn một hệ trục toạ độ, quy ước về cách đặt hệ toạ độ này như sau:
Trục zi được liên kết với trục của khớp thứ i+1. Chiều của zi được chọn tuỳ ý.
Trục xi được xác định là đường vuông góc chung giữa trục khớp i và khớp i+1, hướng từ điểm
trục của khớp i tới khớp i+1. Nếu hai trục song song thì xi có thể chọn bất kỳ là đường vuông góc chung hai trục khớp. Trong trường hợp hai trục này cắt nhau,
xi được xác định theo chiều của zi zi1 ( hoặc quy tắc bàn tay phải).
Trục yi được xác định
theo xi và zi theo quy tắc bàn tay phải.
Các thông số động học Denavit – Hartenberg được xác định như sau:
di : khoảng cách Oi-1 và Oi theo trục zi-1.
i : góc giữa 2 đường vuông góc chung. Là góc quay quanh trục zi-1 để trục xi-1 chuyển đến trục xi theo qui tắc bàn tay phải.
i : góc xoay đưa trục zi-1về zi quanh zi theo quy tắc bàn tay phải.
ai : khoảng dịch chuyển giữa 2 trục khớp động kề nhau.
b, Đặt bài toán.
Nx Sx Ax Px
Ny Sy Ay Py
Nz Sz Az Pz
1 2 3 4
Biết giá trị các biến khớp qi (i = 1, 2,
của tay kẹp, tức là xác đinh ma trận T 3, 4, 5) cần phải xác định vị trí và hướng T=A0 .A1 .A2 .A3 .A4 =
c, Đặt các hệ trục tọa độ.
- Chọn O0x0y0z0 làm hệ cơ sở gốc.
+ Tâm O0 trùng với tâm khớp 1.
+ z0 trùng với trục khớp 1, chiều tùy ý.
+ x0 tùy chọn,miễn là x0 vuông góc với z0.
+ y0 được chọn sao cho hệ tạo thành một tam diện thuận.
- Hệ trục tọa độ Oixiyizi :
+ zi đặt tại khớp tiếp theo tương tự z0.
+ xi là đường vuông góc chung nhỏ nhất nối từ zi-1 đến zi ( chiều từ zi-1 đến zi ).
+ Oi là giao điểm của xi và zi.
+ yi xác định theo quy tắc bàn tay phải.
- Hệ trục tọa độ đặt tại khâu tác động cuối Onxnynzn
+ On trùng với điểm P.
+ yn nằm trong mặt phẳng kẹp, vuông góc với phương kẹp,chiều tùy ý.
+ zn (hoặc xi ) hướng tới đối tượng.
+ trục còn lại là xn (hoặc zn): vuông góc với mặt phẳng kẹp, chiều đảm bảo hệ tạo thành một tam diện thuận.
- Quy tắc D-H quy ước cách đặt các hệ tọa độ lên Robot
- Quy tắc D-H sử dụng 4 phép biến đổi để đưa hệ tọa độ Oi về trùng với hệ tọa độ Oi+1.
R(z,q) là phép quay quanh trục zi một góc α để đưa trục tọa độ xi về trùng với xi+1.
T(z,d) là phép tịnh tiến theo phương z một lượng d để 2 hệ tọa độ Oi và Oi+1 có cùng cao
độ.
T(x,a) là phép tịnh tiến theo phương xi+1 một lượng a để hệ tọa độ Oi về trùng với Oi+1 .
R(x,α) là phép quay quanh trục xi+1 một góc β để đưa trục tọa độ zi về trùng với zi+1.
0 0 0 1
Bảng 2.1: Bảng D-H
Khớp Rz,q Tz,d Tx,a Rx,α
1 (q1) d1 0 900
2 (q2) 0 a2 00
3 (q3) 0 a3 00
4 (q4) 0 0 900
5 (q5) d5 0 00
d, Động học thuận về vị trí của robot.
- Như vậy hệ trục thứ i sẽ mô tả ma trận vị trí và hướng so với hệ trục thứ i-1 thông qua phép biến đổi tọa độ thuần nhất D-H như sau :
i
1
2
A i-1=
A 0=
A 1=
A32=
cos(q4) 0 Sin(q4) 0
sin(q4) 0 -cos(q4) 0
A4 3= 0 1 0 0
cos(q) -sin(q).cos(α) sin(q).sin(α) a.cos(q) sin(q) cos(q).cos(α) -cos(q).cos(α) a.sin(q)
0 sin(α) cos(α) d
0 0 0 1
cos(q1) 0 sin(q1) 0
sin(q1) 0 -cos(q1) 0
0 1 0 d1
0 0 0 1
cos(q2) -sin(q2) 0 a2.cos(q2) sin(q2) cos(q2) 0 a2.sin(q2)
0 0 1 0
0 0 0 1
cos(q3) -sin(q3) 0 a3.cos(q3) sin(q3) cos(q3) 0 a3.sin(q3)
0 0 1 0
0 0 0 1
0 0 0 1
A 45=
- Giải ma trận T bằng matlab:
Các bước thực hiện trên matlab như sau:
+ Khai báo các biến khớp 𝑞i( 1, q2, q3, q4, q5)𝑞 + Khai báo các biến 𝑑𝑖 , 𝑎𝑖 (d1, a2, a3, d5) + Khai báo 5 ma trận: 𝐴0, 𝐴1, 𝐴2, A 3, 𝐴4
1 2 3 4 5
+Kết quả giải trong matlab ta tìm được ma trận T:
- Ma trận T là ma trận 4x4 có dạng:
T=A01 .A12 .A23 .A34 .A45=
cos(q5) -sin(q5) 0 0
sin(q5) cos(q5) 0 0
0 0 1 d5
0 0 0 1
Nx Sx Ax Px
Ny Sy Ay Py
Nz Sz Az Pz
0 0 0 1
Sau khi nhân xong trên matlab ta có giá trị T là:
T = [ sin(q1) * sin(q5) + cos(q5) *(cos(q4) * (cos(q1) * cos(q2) * cos(q3) - cos(q1) *sin(q2) *sin(q3)) - sin(q4) *(cos(q1) *cos(q2) * sin(q3) + cos(q1) *cos(q3) *sin(q2))),
cos(q5) * sin(q1) - sin(q5) * (cos(q4) * (cos(q1) * cos(q2) * cos(q3) - cos(q1) * sin(q2) * sin(q3)) - sin(q4) * (cos(q1) * cos(q2) * sin(q3) + cos(q1) * cos(q3) * sin(q2))),
cos(q4) * (cos(q1) * cos(q2) * sin(q3) + cos(q1) * cos(q3) * sin(q2)) + sin(q4) * (cos(q1) * cos(q2) * cos(q3) - cos(q1) * sin(q2) * sin(q3)),
d5 * (cos(q4) *(cos(q1) * cos(q2) * sin(q3) + cos(q1) * cos(q3) * sin(q2)) + sin(q4) * (cos(q1) * cos(q2) * cos(q3) - cos(q1) * sin(q2) * sin(q3))) + a2 * cos(q1) * cos(q2) + a3 * cos(q1) * cos(q2) * cos(q3) - a3 * cos(q1) * sin(q2) * sin(q3)]
[ - cos(q1) * sin(q5) - cos(q5) * (cos(q4) * (sin(q1) * sin(q2) * sin(q3) - cos(q2) * cos(q3) * sin(q1)) + sin(q4) * (cos(q2) * sin(q1) * sin(q3) + cos(q3) * sin(q1) * sin(q2))),
sin(q5) * (cos(q4) * (sin(q1) *sin(q2) * sin(q3) - cos(q2) *cos(q3) *sin(q1)) + sin(q4) * (cos(q2) * sin(q1)
* sin(q3) + cos(q3) * sin(q1) * sin(q2))) - cos(q1) * cos(q5),
cos(q4) * (cos(q2) * sin(q1) * sin(q3) + cos(q3) * sin(q1) * sin(q2)) - sin(q4) * (sin(q1) * sin(q2) * sin(q3) - cos(q2) * cos(q3) * sin(q1)),
d5 * (cos(q4) * (cos(q2) * sin(q1) * sin(q3) + cos(q3) * sin(q1) * sin(q2)) - sin(q4) * (sin(q1) * sin(q2) * sin(q3) - cos(q2) * cos(q3) * sin(q1))) + a2 * cos(q2) * sin(q1) + a3 *cos(q2) * cos(q3) * sin(q1) - a3 * sin(q1) * sin(q2) * sin(q3)]
[ cos(q5) * (cos(q4) * (cos(q2) * sin(q3) + cos(q3) * sin(q2)) + sin(q4) * (cos(q2) * cos(q3) - sin(q2) * sin(q3))),
-sin(q5) * (cos(q4) * (cos(q2) * sin(q3) + cos(q3) * sin(q2)) + sin(q4) * (cos(q2) * cos(q3) - sin(q2) * sin(q3))),
sin(q4) * (cos(q2) * sin(q3) + cos(q3) * sin(q2)) - cos(q4) * (cos(q2) * cos(q3) - sin(q2) * sin(q3)), d1 + a2 * sin(q2) - d5 * (cos(q4) * (cos(q2) * cos(q3) - sin(q2) * sin(q3)) - sin(q4) * (cos(q2) * sin(q3) + cos(q3) * sin(q2))) + a3 * cos(q2) * sin(q3) + a3 * cos(q3) * sin(q2)]
[ 0, 0, 0, 1]
Hệ phương trình động học thuận Robot:
Nx = sin(q1) * sin(q5) + cos(q5) *(cos(q4) * (cos(q1) * cos(q2) * cos(q3) - cos(q1) *sin(q2)
*sin(q3)) - sin(q4) *(cos(q1) *cos(q2) * sin(q3) + cos(q1) *cos(q3) *sin(q2)))
Ny = - cos(q1) * sin(q5) - cos(q5) * (cos(q4) * (sin(q1) * sin(q2) * sin(q3) - cos(q2) * cos(q3) * sin(q1)) + sin(q4) * (cos(q2) * sin(q1) * sin(q3) + cos(q3) * sin(q1) * sin(q2)))
Nz = cos(q5) * (cos(q4) * (cos(q2) * sin(q3) + cos(q3) * sin(q2)) + sin(q4) * (cos(q2) * cos(q3) - sin(q2) * sin(q3)))
Sx = cos(q5) * sin(q1) - sin(q5) * (cos(q4) * (cos(q1) * cos(q2) * cos(q3) - cos(q1) * sin(q2) * sin(q3)) - sin(q4) * (cos(q1) * cos(q2) * sin(q3) + cos(q1) * cos(q3) * sin(q2)))
Sy = sin(q5) * (cos(q4) * (sin(q1) *sin(q2) * sin(q3) - cos(q2) *cos(q3) *sin(q1)) + sin(q4) * (cos(q2) * sin(q1) * sin(q3) + cos(q3) * sin(q1) * sin(q2))) - cos(q1) * cos(q5)
Sz = -sin(q5) * (cos(q4) * (cos(q2) * sin(q3) + cos(q3) * sin(q2)) + sin(q4) * (cos(q2) * cos(q3) - sin(q2) * sin(q3)))
Ax = cos(q4) * (cos(q1) * cos(q2) * sin(q3) + cos(q1) * cos(q3) * sin(q2)) + sin(q4) * (cos(q1) * cos(q2) * cos(q3) - cos(q1) * sin(q2) * sin(q3))
Ay = cos(q4) * (cos(q2) * sin(q1) * sin(q3) + cos(q3) * sin(q1) * sin(q2)) - sin(q4) * (sin(q1) * sin(q2) * sin(q3) - cos(q2) * cos(q3) * sin(q1))
Az = sin(q4) * (cos(q2) * sin(q3) + cos(q3) * sin(q2)) - cos(q4) * (cos(q2) * cos(q3) - sin(q2) * sin(q3))
Px = d5 * (cos(q4) *(cos(q1) * cos(q2) * sin(q3) + cos(q1) * cos(q3) * sin(q2)) + sin(q4) * (cos(q1)
* cos(q2) * cos(q3) - cos(q1) * sin(q2) * sin(q3))) + a2 * cos(q1) * cos(q2) + a3 * cos(q1) * cos(q2)
* cos(q3) - a3 * cos(q1) * sin(q2) * sin(q3)
Py = d5 * (cos(q4) * (cos(q2) * sin(q1) * sin(q3) + cos(q3) * sin(q1) * sin(q2)) - sin(q4) * (sin(q1)
* sin(q2) * sin(q3) - cos(q2) * cos(q3) * sin(q1))) + a2 * cos(q2) * sin(q1) + a3 *cos(q2) * cos(q3)
* sin(q1) - a3 * sin(q1) * sin(q2) * sin(q3)
Pz = d1 + a2 * sin(q2) - d5 * (cos(q4) * (cos(q2) * cos(q3) - sin(q2) * sin(q3)) - sin(q4) * (cos(q2)
* sin(q3) + cos(q3) * sin(q2))) + a3 * cos(q2) * sin(q3) + a3 * cos(q3) * sin(q2)