KỸ THUẬT ROBOT

CHƯƠNG 4
ĐỘNG HỌC NGƯỢC TAY MÁY ROBOT
Inverse manipulator kinematics

Khoa Điện
PGS. TS. Lê Tiến Dũng
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

NỘI DUNG CHƯƠNG 4

I. GIỚI THIỆU CHUNG
II. ĐIỀU KIỆN CỦA BÀI TOÁN ĐỘNG HỌC NGƯỢC
III. GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC
IV. PHƯƠNG PHÁP CỦA PIEPER (CHO TRƯỜNG HỢP 3 TRỤC KHỚP GIAO NHAU)
V. VÍ DỤ GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC ROBOT PUMA 560
VI. BÀI TẬP

2
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

I. GIỚI THIỆU CHUNG

Bài toán động học ngược: Cho trước vị trí và hướng của end-effector của robot, tính toán tất cả các tập hợp
giá trị của các biến khớp có thể được sử dụng để tạo ra vị trí và hướng này của end-effector.
Đối với tay máy robot nối tiếp, bài toán động học thuận thường dễ giải quyết trong khi bài toán động học
ngược là một bài toán khó và tồn tại nhiều nghiệm.

Ví dụ:

3
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

II. ĐIỀU KIỆN CỦA BÀI TOÁN ĐỘNG HỌC NGƯỢC

Bài toán động học ngược có thể được phát biểu như sau:
Tìm vector các biến khớp của robot:

Khi biết trước ma trận biến đổi là một hàm của các biến khớp q1, q2,…,qn:

Đối với trường hợp tay máy robot có 6 bậc tự do, chúng ta có sẽ là một ma trận có kích thước 4x4 như sau:

4
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Trong ma trận thì bao gồm một ma trận quay 3x3 và một vector vị trí 3x1. Như vậy, nếu sử dụng
biểu thức của ma trận chúng ta có 12 phương trình để giải bài toán động học ngược tìm 6 biến
khớp.
Tuy nhiên, ma trận quay 3x3 chỉ có 3 phương trình độc lập do tính chất trực giao của các vector chỉ
hướng (xem lại chương 2).
Như vậy, kết quả từ ma trận cho trước chúng ta chỉ có đúng 6 phương trình độc lập để giải bài
toán động học ngược tìm 6 biến khớp.

Điều kiện tồn tại nghiệm của bài toán động học ngược:

Điều kiện để tồn tại nghiệm của bài toán động học ngược là vị trí và hướng của end-effector phải nằm trong
không gian công tác (workspace) của tay máy robot.

+ Workspace: Volume of space that the end-effector of the manipulator can reach.
+ Dextrous workspace: Volume of space that the robot end-effector can reach with all orientations.
+ Reachable workspace: reach in at least one orientation. 5
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Chúng ta có thể tách bài toán động học ngược của tay máy robot ra thành 2 bài toán nhỏ hơn, đó
là bài toán động học ngược về vị trí (inverse position kinematic) và bài toán động học ngược về
hướng (inverse orientation kinematic). Theo ý tưởng này, từ ma trận biến đổi của phương trình động
học thuận, chúng ta có thể phân tách thành 2 bài toán độc lập, mỗi bài toán có 3 biến số.

Ma trận tịnh tiến xác định vị trí của hệ tọa độ gắn với end-effector mô tả trong hệ tọa độ {0},
và đối với trường hợp robot 6 bậc tự do thì có thể phân tách để chỉ liên quan đến 3 biến khớp điều
khiển vị trí cổ tay (wrist) của tay máy robot.
Ma trận chỉ hướng của hệ tọa độ gắn với end-effector mô tả trong hệ tọa độ {0}. Ma trận này
cũng có thể phân tách chỉ để liên quan đến 3 biến khớp còn lại của robot. Giải bài toán động học
ngược về hướng sau khi giải bài toán động học ngược về vị trí. 6
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Bài toán động học ngược thường có nhiều nghiệm (multiple solutions)
Bài toán động học ngược thường có nhiều nghiệm, vì vậy sẽ đặt ra vấn đề chọn nghiệm trong khi hoặc sau khi
giải bài toán động học ngược. Thông thường sẽ lựa chọn nghiệm để tay máy robot có tư thế (pose) tối ưu về lực,
tối ưu về mô-men, hoặc để tránh vật cản.

Lựa chọn nghiệm để có tư

thế tránh được vật cản
7
Bài toán động học ngược Robot
7 PUMA có 4 nghiệm
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

III. GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC

Bài toán động học ngược là bài toán giải hệ phương trình phi tuyến, vì vậy không có một phương
pháp chung tổng quát nào để giải bài toán động học ngược của tay máy robot.
Các phương pháp giải bài toán động học ngược của tay máy robot có thể chia thành 2 nhóm
chính:

+ Phương pháp biểu thức dạng kín (closed-form solutions): là nhóm phương pháp thường
được ưu tiên sử dụng. Trong nhóm phương pháp này, bài toán được giải bằng cách sử dụng một
số lượng hữu hạn các biểu thức, phép toán giải tích. Trong phương pháp này lại phân ra 2 nhóm:
Phương pháp đại số (algebraic) và phương pháp hình học (geometric).

+ Phương pháp số (Numerical solution): Sử dụng các phép lặp, phương pháp tính để giải bài
toán.
8
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Ví dụ 1: Xét tay máy robot phẳng 2 bậc tự do RR

Phương pháp đại số (algebraic) để giải bài toán động học ngược

Elbow down
Elbow up

Phương trình động học thuận:

9
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Với X, Y cho trước trên mặt phẳng tọa độ, từ phương trình ma trận tách ra các biến vị trí chúng ta có:

Kiểm tra vế phải thuộc [-1,1]

Elbow up solution Elbow down solution 10

 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Ví dụ 2: Xét tay máy robot phẳng 3 bậc tự do RRR đã tính động học thuận ở chương 3:

Phương pháp đại số (algebraic) để giải bài toán động học ngược

11
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Ma trận biến đổi của động học thuận:

Bởi vì tay máy robot đang xét hoạt động trên mặt phẳng, nên trong hệ tọa độ Cartersian vị trí và hướng của
khâu cuối có thể được biểu diễn bởi 3 đại lượng: x, y và . Trong đó (x, y) để chỉ vị trí còn  để chỉ hướng,
xoay quanh trục .
Như vậy ma trận biểu diễn vị trí và hướng của khâu cuối sẽ có dạng:

12
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Chúng ta có 4 phương trình để giải tìm 1, 2, và 3 :

Trong đó:

Bình phương 2 phương trình của x và y chúng ta có:

Để tồn tại nghiệm, thì vế bên phải phải có giá trị nằm giữa 1 và -1. Trong thuật toán giải phải có bước kiểm tra
điều kiện này.
13
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Từ c2 và s2 chúng ta tính ra được:

Viết lại hai phương trình của x và y:

Trong đó k1 và k2 đã xác định:

Để giải hệ phương trình của x và y nhằm tìm c1 và s1 chúng ta đặt biết phụ để viết lại dạng của k1 và k2:

14
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Khi đó:

Mặt khác từ phương trình cân bằng 2 ma trận chúng ta có:

Tìm được 3
15
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Ví dụ 3: Phương pháp hình học (geometric) để giải bài toán động học ngược

Từ cấu trúc hình học chúng ta tính được theo định lý góc cosin trong tam giác:

16
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Ký hiệu các góc phụ  và  như trên hình vẽ, chúng ta có:

Sau khi tính được  và  chúng ta tính được 1:

Cuối cùng chúng ta tính ra 3 từ phương trình:

17
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Ví dụ 4: Xét tay máy robot phẳng 3 bậc tự do RRR như hình vẽ

Phương pháp hình học (geometric) để giải bài toán động học ngược

18
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Từ cấu trúc hình học chúng ta tính được:

Trong đó:

19
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

IV. PHƯƠNG PHÁP CỦA PIEPER (CHO TRƯỜNG HỢP 3 TRỤC KHỚP GIAO NHAU)

Pieper đề xuất một phương pháp giải bài toán động học ngược cho tay máy robot 6 bậc tự do có các trục
của 3 khớp cuối cắt nhau và tất cả các khớp là khớp xoay. Kết quả nghiên cứu của Pieper đã được áp dụng
trong các robot thương mại.

[1] D. Pieper and B. Roth, "The Kinematics of Manipulators Under Computer Control“ Proceedings of the Second
International Congress on Theory of Machines and Mechanisms, Vol. 2, Zakopane, Poland, 1969, pp.159-169.
[2] D. Pieper, "The Kinematics of Manipulators Under Computer Control“, Unpublished Ph.D. Thesis, Stanford
University, 1968

Khi 3 trục của 3 khớp cuối cắt nhau, các gốc tọa độ của các khung tọa độ {4}, {5} và {6} được đặt tại điểm
giao nhau này của 3 khớp. Điểm giao nhau này được mô tả bởi phương trình:

20
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Sơ đồ 3 trục khớp xoay 4, 5 và 6 của tay máy robot giao nhau tại 1
điểm và trực giao từng cặp (đối với robot PUMA 560). Cách thiết kế
này được áp dụng trong nhiều loại tay máy robot công nghiệp để có
thể giải bài toán động học ngược theo phương pháp của Pieper.

21
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Sử dụng cột thứ 4 trong công thức ma trận biến đổi:

=> Thay vào công thức chúng ta có:

Trong đó:

22
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Thay biểu thức ma trận :

Thay vào lại biểu thức ban đầu của ta có:

23
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Thay các biểu thức của và vào phương trình, chúng ta có:

24
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

Trong đó:

Bình phương biên độ của :

Đặt biến phụ để viết lại biểu thức:

25
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

26
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

27
 KHOA ĐIỆN
PGS. TS. Lê Tiến Dũng

V. VÍ DỤ GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC ROBOT PUMA 560

Textbook trang: 117 -> 121
VI. BÀI TẬP
Textbook trang: 128 -> 134
4.2, 4.4, Matlab Exercise 4

28