p, y( 6)p , z ( 6 )
3.4.4- Thuật toán giải bài toán vị trí trong bài toán thuận động học tay máy
tay máy
Các bước thủ tục cần thực hiện khi giải bài toán thuận.
(1) Trên trục Zo của hệ trục cơ sở (0xyz)o, dịch theo trục zo một đoạn do và xác định đuợc gốc toạđộ hệ trục trung gian 0’n với lưu ý:
- Nếu do > 0 : dịch theo chiều dương trục Zo
- Nếu do < 0 : dịch theo chiều âm của trục Zo (**) (2) Vẽ trục trung gian 0’nx”n // 0oxo
(3) Quay trục 0’nx”o quanh trục Zo một góc γo, với lưu ý: - Nếu γo > 0 : quay theo chiều ngược chiều kim đồng hồ (chiều dương lượng giác)
- Nếu γo < 0 : quay theo chiều ngược lại (***) Kết quả, xác định được trục 0’nx’n.
(4) Dịch chuyển 0’n dọc theo trục 0’nx’n một đoạn ao với lưu ý (**), xác
định được gốc toạ độ 0n và trục 0nxn của hệ trục (0xyz)n.
(5) Kẻ trục 0nz’n // 0ozo và quay trục 0nz’n quanh trục 0nxn một góc αo
với lưu ý (***). Ta xác định được trục 0nzn.
(6) Vẽ trục 0nyn sao cho (0xyz)n tạo thành một tam diện thuận (xác định theo quy tắc bàn tay phải).
Như vậy, các bước thủ tục trong giải thuật xác định vị trí và hướng tay gắp trong bài toán động học thuận tay máy có thể khái quát như sau:
Bước 1: Xây dựng hệ trục tham chiếu (0xyz)o và các hệ trục địa phương (0xyz)i (i=1,...n) theo các quy ước Denavit - Hartenberg . Các chỉ số
liên tiếp nhau tính từ hệ trục tham chiếu.
(1) Chọn hệ trục cơ sở bằng cách chọn 0o trên trục zo sao cho 0o trùng với gốc toạ độ 01 của hệ trục toạ độ địa phương thứ nhất. Các trục xo và yo
được chọn tuỳ ý sao cho (0xyz)o tạo thành mọt tam diện thuận.
(2) Gốc toạ độ địa phương 01 là giao điểm của trục zi với đường vuông góc chung của hai trục zi-1 và zi. Nếu zi-1 // zi và khớp i là khớp quay thì 0i được xác định xuất phát từ 0i-1 (tức là di = 0). Nếu khớp i là khớp tịnh tiến, gốc toạđộ 0i là vị trí hiện hành của khớp trong quá trình thay đổi vị trí.
(3) Chọn trục xi dọc theo đường vuông góc chung của hai trục zi-1 vf zi (chiếu từ khớp i tới khớp i+1).
(4) Chọn trục yi sao cho (0xyz)i tạo thành một tam diện thuận. (5) Chọn hệ trục n (0xyz)n như sau: Trục xn vuông góc với zn-1: - Nếu khớp n là khớp quay, zn trùng với zn-1. - Nếu khớp n là khớp tịnh tiến, zn trùng với zn-1 hiện hành của tay gắp, nhưng 0n cách 0n-1 một khoảng bằng giá trị hiện hành của biến dn: 0n0n-1 = dn Bước 2: Lập bảng hệ số ai, di, αi, γi (i=1, ... n)
Bước 3: Chạy chương trình “DU_LIEU” trong tập tin DU_LIEU.m để
nhập dữ liệu cho bài toán.
Bước 4: Chạy chương trình trong tập tin V_TR.m để xác định [ao doαoγo]. Bước 5: Hoàn thiện bài toán trên cơ sở các thông số đã biết [ao do αo
γo], xác định hướng và vị trí tay gắp hoặc hướng và vị trí của khâu bất kỳ, tọa độ của một điểm bất kỳ nào đó trên khâu này theo các bước (*) đã trình
bày ở trên.