Các Phương Pháp Giải Bài Toán

Động Học Ngược

I. Động Học Thuận

1. Cơ sở lý thuyết

Vị trí mỗi khâu trong không gian được xác định bởi vị trí một điểm định vị
và hướng của khâu đó đối với một hệ quy chiếu đã chọn. Điểm định vị là một điểm
xác định nào đó của khâu, thông thường trong động lực học ta hay lấy khối tâm
của khâu đó làm điểm định vị. Hướng của khâu được xác định bằng ma trận cosin
chỉ hướng hoặc bằng các tọa độ suy rộng xác địnhvị trí của vật rắn quay quanh một
điểm.

Động học robot nghiên cứu chuyển động của các khâu của robot về phương
diện hình học, không quan tâm đến các lực và momen gây ra chuyển động.Động
học robot là bài toán qua trọng phục vụ tính toán và thiết kế robot.Nhiệm vụ chủ
yếu của bài toán động học thuận là xác định vị trí và hương của bàn kẹp dưới dạng
hàm của biến khớp.

Các phương pháp ma trận 4x4 và các phương pháp ma trận 3x3 hay được sử
dụng trong phân tích động học robot. Hai phương pháp ma trận 4x4 phổ biến là
phương pháp ma trận Denavit-Hartenberg và phương pháp ma trận Craig. Trong
báo cáo này chúng em trình bày và áp dụng phương pháp ma trận Denavit-
Hartenberg để tính toán động học robot.

Giải bài toán động học thuận robot công nghiệp bằng phương pháp ma trận
DenavitHartenberg

Cách xác định các trục cuả hệ tọa độ khớp.

Đối với robot công nghiệp ,Denavit-Hartenberg đã đưa ra cách chọn các hệ
trục tọa độ có gốc tại khớp thứ i như sau:

Trục 𝑧𝑖−1 được chọn dọc theo hướng của trục khớp động thứ i.
 Trục 𝑥𝑖−1 được chọn dọc theo đường vuông góc chung của 2 trục 𝑧𝑖−2 và 𝑧𝑖−1
hướng đi từ trục 𝑧𝑖−2 sang trục 𝑧𝑖−1 . Nếu trục 𝑧𝑖−1 cắt trục 𝑧𝑖−2 thì hướng của
trục 𝑥𝑖−1 được chọn tùy ý miễn là vuông góc với trục 𝑧𝑖−1 .Khi 2 trục 𝑧𝑖−2 và
𝑧𝑖−1 song song với nhau, giữa 2 trục này có nhiều đường vuông góc chung , ta có
thể chọn trục 𝑥𝑖−1 hướng theo pháp tuyến chung nào cũng được

Gốc tọa độ 𝑂𝑖−1 được chọn tại giao điểm cuả trục 𝑥𝑖−1 và trục 𝑧𝑖−1 .

Trục 𝑦𝑖−1 được chọn sao cho hệ (𝑂𝑥𝑦𝑧)𝑖−1 là hệ quy chiếu thuận.

Hệ tọa độ (𝑂𝑥𝑦𝑧)𝑖−1 được xác định như trên trong một số tài liệu được quy ước là
hệ tọa độ khớp

 Chú ý: Với cách chọn hệ tọa độ như trên , đôi khi hệ tọa độ khớp (𝑂𝑥𝑦𝑧)𝑖−1
không được một cách duy nhất. vì vậy, ta có một số bổ sung thích hợp như
sau :
 Đối với hệ tọa độ (𝑂𝑥𝑦𝑧)0 theo quy ước trên ta mới chỉ chọn được
trục 𝑧0 , còn trục 𝑥0 chưa có trong quy ước trên.Ta có thể chọn trục
𝑥0 một cách tùy ý, miễn là 𝑥0 vuông góc với 𝑧0
 Đối với hệ tọa độ (𝑂𝑥𝑦𝑧)𝑛 , do không có khớp (n+1) nên theo quy
ước trên ta không xác định 𝑧𝑛 . Trục 𝑧𝑛 không được xác định duy
nhất, trong khi trục 𝑥𝑛 lại được chọn theo đường pháp tuyến của
trục 𝑧𝑛−1 . Trong trường hợp này, nếu khớp n là khớp quay, ta có
thể chọn trục 𝑧𝑛 song song trục 𝑧𝑛−1 .Ngoài ra ta có thể tùy chọn
sao cho hợp lý.
 Khi khớp thứ i là tịnh tiến, về nguyên tắc ta có thể chọn trục 𝑧𝑖−1
một cách tùy ý. Tuy nhiên trong nhiều trường hợp người ta thường
chọn trục 𝑧𝑖−1 dọc theo trục cuả khớp tịnh tiến này.
 Hình 1: biểu diễn các thông số Denavit-Hartenberg giữa các trục hệ tọa
độ

Các tham số động học Denavit-Hartenberg:

Vị trí của hệ toạ độ khớp (𝑂𝑥𝑦𝑧)𝑖 đối với hệ tọa độ khớp (𝑂𝑥𝑦𝑧)𝑖−1 được 4 tham
số Denavit-haartenderg 𝑑𝑖 , 𝜃𝑖 , 𝑎𝑖 , 𝛼𝑖 như sau:

 𝑑𝑖 dịch chuyển tịnh tiến dọc theo trục 𝑧𝑖−1 để gốc tọa độ 𝑂𝑖−1 chuyển đến
𝑂′𝑖 giao điểm của trục 𝑥𝑖 và trục 𝑧𝑖−1 .
 𝜃𝑖 : góc quay quanh trục 𝑧𝑖−1 để trục 𝑥𝑖−1 chuyển đến trục 𝑥′𝑖 (𝑥′𝑖 // 𝑥𝑖 ).
 𝑎𝑖 , : dịch chuyển tịnh tiến theo dọc trục 𝑥𝑖 , để điểm 𝑂′𝑖 , chuyển đến điểm
𝑂𝑖 .
 𝛼𝑖 : góc quay quanh trục 𝑥𝑖 sao cho trục 𝑧′𝑖−1 (𝑧′𝑖−1 // 𝑧𝑖−1 ). Chuyển đến
trục 𝑧𝑖 .

Do hệ trục tọa độ (𝑂𝑥𝑦𝑧)𝑖−1 gắn liền vào khâu thứ i-1 , còn hệ trục tọa độ
(𝑂𝑥𝑦𝑧)𝑖 gắn liền vào khâu thứ i , cho nên vị trí cuả khâu thứ i đối với khâu thứ i-
1, được xác định bới 4 tham số Denavit-hartenberg

Trong 4 tham số trên, các tham số 𝑎𝑖 , và 𝛼𝑖 luôn luôn là các hằng số , độ lớn của
chúng phụ thuộc vào hình dáng và sự ghép nối các khâu thứ i-1 và thứ i. Hai tham
số còn lại 𝜃𝑖 và 𝑑𝑖 một là hằng số, một là biến số phụ thuộc vào khớp i là khớp
quay hay khớp tịnh tiến.Khi khớp i là khớp quay thì 𝜃𝑖 là biến, còn 𝑑𝑖 là hằng số.
Khi khớp i là khớp tịnh tiến thì 𝑑𝑖 là biến, còn 𝜃𝑖 là hằng số

Chú ý về việc xác định hệ tọa độ khớp tại khớp tịnh tiến.Trong trường hợp khớp i
là khớp tịnh tiến, về nguyên tắc ta có thể chọn trục 𝑧𝑖−1 một cách tùy ý, do đó việc
xác định các tham số Denavit-Hartenberg phụ thuộc vào việc chọn hệ trục tọa độ.
Ma trận của phép biến đổi, kí hiệu là 𝐻𝑖 , là tích của 4 na trận biến đổi cơ bản,và
có dạng như sau:

cos(𝜃𝑖 ) − sin(𝜃𝑖 ) . cos(𝛼𝑖 ) sin(𝜃𝑖 ) . sin(𝛼𝑖 ) 𝑎𝑖 . cos(𝜃𝑖 )

) cos(𝜃𝑖 ) . cos(𝛼𝑖 ) − cos(𝜃𝑖 ). sin(𝛼𝑖 ) 𝑎𝑖 . sin(𝜃𝑖 )
𝐻𝑖 =( sin(𝜃𝑖 )
0 sin(𝛼𝑖 ) cos(𝛼𝑖 ) 𝑑𝑖
0 0 0 1
Ma trận 𝐻𝑖 được xác định bởi công thức (1.1) được gọi là ma trận Denavit-
Hartenberg địa phương.

Phương trình xác định vị trí khâu thao tác (bàn kẹp) cuả robot.

Xét mô hình cơ học của một robot n khâu động như hình vẽ:

Hình 2: Robot n khâu

Theo nguyên tắc nêu trên, ta thiết lập được hệ trục tọa độ gắn liền với giá cố định
và hệ tọa độ gắn liền với các vật.Gọi 𝑅0 là hệ quy chiếu (𝑂𝑥𝑦𝑧)0 gắn liền với giá
cố định, hệ quy chiếu 𝑅𝑖 = (𝑂𝑥𝑦𝑧)𝑖 gắn liền với khâu thứ i.Ma trận 𝐻𝑖𝑖−1 cho ta
biết vị trí và hướng của khâu i đối với hệ quy chiếu 𝑅𝑖−1 gắn vào khâu thứ i-1.

Từ đó suy ra ma trận Denavit-Hartenberg 𝐻𝑖 cho biết vị trí của hệ quy chiếu 𝑅𝑖 =

(𝑂𝑥𝑦𝑧)𝑖 đối với hệ quy chiếu𝑅𝑖−1 = (𝑂𝑥𝑦𝑧)𝑖−1 .Áp dụng liên tiếp các phép biến
đổi đối với robot n khâu, ta có:

𝐷𝑛 = 0𝐻 = 𝐻1 . 𝐻2 … . . 𝐻𝑛
(0)
𝐷𝑛 = [𝐴𝑛 𝑟𝐸 ]
0𝑇 1
Ma trận 𝐷𝑛 cho biết vị trí của điểm tác động cuối E và hướng cuả khâu thao tác
(bàn kẹp) của robot đối với hệ quy chiếu cố định 𝑅0

Như vậy khi biết được các đặc tính hình học cuả các khâu và quy luật chuyển động
của các khớp là ta có thể xác định được vị trí và hướng của bàn kẹp. Xác định vận
tốc, gia tốc điểm tác động cuối và vận tốc góc, gia tốc góc các khâu cuả robot
bằng phương pháp trực tiếp

Vận tốc và gia tốc dài của bàn kẹp có thể dễ dàng suy ra từ đạo hàm vector tọa độ
(0)
𝑟𝐸

Vận tốc điểm thao tác:

(0)
𝑑(𝑥𝐸 )
0
𝑣𝐸𝑥 𝑑𝑡
𝑅0 𝑑 (0)
(0) (0) 0 𝑑(𝑦𝐸 )
𝑟𝐸 = . 𝑟𝐸 hay [𝑣𝐸𝑦 ]=
𝑑𝑡 𝑑𝑡
0
𝑣𝐸𝑧 (0)
𝑑(𝑧𝐸 )
[ 𝑑𝑡 ]

Gia tốc điểm thao tác:

 (0)
𝑑(𝑣𝐸𝑥 )
0
𝑎𝐸𝑥 𝑑𝑡
𝑅0 𝑑 (0)
(0) (0) 0 𝑑(𝑣𝐸𝑦 )
𝑎𝐸 = . 𝑣𝐸 hay [𝑎𝐸𝑦 ]=
𝑑𝑡 𝑑𝑡
0
𝑎𝐸𝑧 (0)
𝑑(𝑣𝐸𝑧 )
[ 𝑑𝑡 ]
Ta có thể tính trực tiếp vận tốc góc khâu thứ i của robot dựa trên công thức tính
vận tốc góc vật rắn thông qua ma trận cosin chỉ hướng như sau:
(0)

̃ 0 = 𝐴̇𝑖 . 𝐴̇𝑇𝑖
(𝑖)

̃ 𝑖 = 𝐴̇𝑇𝑖 . 𝐴̇𝑖

Từ đó suy ra biểu thức vận tốc góc khâu thứ i của robot. Áp dụng định nghĩa gia
tốc góc của vật rắn, khi biết vận tốc góc của các khâi của robot ta có thể tính gia
tốc góc các khâu của robot theo công thức sau
𝑅0
𝑑 𝑅0 𝐵
𝑅0 𝐵
𝛼⃑ = . ⃑
⃑⃑
𝑑𝑡
Thiết lập phương trình động học thuận cho robot :

Ta sẽ tính toán với robot 2 khâu phẳng :

 2. Thiết lập bảng DH

Hình 4: Khung tọa độ robot

Ta đặt gốc của robot trùng với khớp quay thứ 1. Khung tọa độ thứ nhất: trục
𝑥1 là phương của thanh nối thứ nhất. gốc đặt tại cuối thanh nối thứ nhất. biến
khớp là 𝜃1 . Chiều dài pháp tuyến 𝑎1 = 𝑙1 . Trục 𝑧1 là trục khớp thứ hai.
Khung tọa độ thứ hai. trục 𝑥2 là phương của thanh nối thứ hai. gốc đặt tại
cuối thanh nối thứ hai. Biến khớp là 𝜃2 .
Chiều dài pháp tuyến 𝑎2 = 𝑙2 . Trục 𝑧2 là trục khớp thứ hai.
 Do đó ta có bảng D-H như sau:
𝑎𝑖 𝑑𝑖 𝛼𝑖 𝜃𝑖
1 𝑙1 0 0 𝜃1
2 𝑙2 0 0 𝜃2

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

Từ bảng DH ta có:
cos(𝜃1 ) −sin(𝜃1 ) 0 𝑙1 . cos(𝜃1 )
0 sin(𝜃1 ) cos(𝜃1 ) 0 𝑙1 . sin(𝜃1 )
𝑇1 = [ ]
0 0 1 0
0 0 0 1
Quan hệ giữa khung tọa độ 2 với khung tọa độ 1 được mô tả bằng ma trận :
cos(𝜃2 ) −sin(𝜃2 ) 0 𝑙1 . cos(𝜃2 )
1 sin(𝜃2 ) cos(𝜃2 ) 0 𝑙1 . sin(𝜃2 )
𝑇2 = [ ]
0 0 1 0
0 0 0 1
Phương trình động lực học của robot được mô tả bằng tích của 2 ma trận:

cos(𝜃12 ) −sin(𝜃12 ) 0 𝑙1 . cos(𝜃1 ) + 𝑙2 cos(𝜃12 )

T= 0 𝑇1 . 1 𝑇2 =[ sin(𝜃12 ) cos(𝜃12 ) 0 𝑙1 . sin(𝜃1 ) + 𝑙2 sin(𝜃12 ) ]
0 0 1 0
0 0 0 1
Vậy ma trận đồng nhất Tmô tả phương trình động học thuận của robot. Trong đó 3
cột đầu tiên chỉ hướng của tay robot, cột thứ 4 chỉ vị trí của tay robot
 II. Động học ngược
1. Giới thiệu

Bài toán:

- Cho trước : vị trí đầu cuối của tay máy

- Yêu cầu tìm giá trị góc quay, lượng dịch chuyển của các khớp quay hoặc
trượt

Khó Khăn :

- Không có một phương pháp chung nào để giải bài toán. Việc giải bài toán
nhanh hay chậm do kinh nghiệm và cách lựa chọn của mỗi người
- Cơ cấu càng nhiều bậc tự do thì số ẩn càng nhiều và việc giải bài toán càng
trở nên phức tạp
- Các phương trình để tìm góc rất khó giải trực tiếp
- Sự suy biến

2. Các phương pháp giải toán

Các điều kiện để giải bài toán động học ngược

- Điều kiện tồn tại nghiệm :

Nhằm khẳng định có ít nhất một tệp nghiệm [ 𝜃1 𝜃2 𝜃3 … … 𝜃𝑛 ]𝑇 sao cho
robot có hình thể cho trước ( hướng và vị trí)
- Điều kiện duy nhất của nghiệm :
Cần xác định rõ 2 loại nghiệm :
+ Nghiệm toán: thõa mãn các phương trình toán học cần giải
+ Nghiệm vật lý : là các nghiệm con của nghiệm toán nhưng bị phụ thuộc
vào các giới hạn vật lý như góc quay, khoảng cách , kích thước …… để loại
trừ và tìm ra nghiệm duy nhất
Có 3 phương pháp giải bài toán động học ngược :

1. Phương pháp sử dụng hình học ( ít được sử dụng , chỉ sử dụng cho những
robot phẳng hoặc không gian ít phức tạp )
2. Phương pháp giải tích ( Analytical Method)
3. Phương Pháp số ( Numerical Method)

Hiện nay người ta đa phần sử dụng phương pháp giải tích hoặc phương pháp số

1. Phương pháp hình học

Thường áp dụng cho những cơ cấu đơn giản, dễ áp dụng các định lý, định
luật hình học để tìm các ẩn cho bài toán

Quay lại với bài toán đầu với robot phẳng 2 bậc tự do:

Sau khi lập bảng DH và tìm được

cos(𝜃12 ) −sin(𝜃12 ) 0 𝑙1 . cos(𝜃1 ) + 𝑙2 cos(𝜃12 )

T= 0 𝑇1 . 1 𝑇2 =[ sin(𝜃12 ) cos(𝜃12 ) 0 𝑙1 . sin(𝜃1 ) + 𝑙2 sin(𝜃12 ) ]
0 0 1 0
0 0 0 1
Từ phương trình động học thuận ta có vị trí của tay robot xác định theo phương
trình sau:

𝑝𝑥 = 𝑙1 . cos(𝜃1 ) + 𝑙2 . cos(𝜃12 ) (2.1)

{
𝑝𝑦 = 𝑙1 . sin(𝜃1 ) + 𝑙2 . sin(𝜃12 ) (2.2)
Ta lấy tổng bình phương 2 vế của (2.1) và (2.2) rồi cộng lại theo vế ta được :

𝑝𝑥 2 + 𝑝𝑦 2 = 𝑙12 + 𝑙22 + 2𝑙1 . 𝑙2 . cos(𝜃2 ) (2.3)

Ta tính được 𝜃2 :
𝑝𝑥 2 +𝑝𝑦 2 −𝑙12 −𝑙22
Có 𝜃2 =±arccos( )
2𝑙1 .𝑙2

2
√4.𝑙12 .𝑙22 −(𝑝𝑥 2 +𝑝𝑦 2 −𝑙12 −𝑙22 )
 𝜃2 = ±arctan( 𝑝𝑥 2 +𝑝𝑦 2 −𝑙12 −𝑙22
) (2.4)

Từ hình vẽ ta nhận thấy :

𝑝𝑥
 + 𝜃1 = arctan (𝑝𝑦) (2.5)

𝑙2 .sin(𝜃2 ) 𝑝𝑥 2 +𝑝𝑦 2 +𝑙12 −𝑙22

Mà sin() = =>  = ±arccos( )
√𝑝𝑥 2 +𝑝𝑦 2 2𝑙1 .√𝑝𝑥 2 +𝑝𝑦 2

Thay vào 2.5 ta được

𝑝𝑦 𝑝𝑥 2 + 𝑝𝑦 2 + 𝑙12 − 𝑙22
𝜃1 = arctan ( ) ∓ arccos ( )
𝑝𝑥 2
2𝑙1 . √𝑝𝑥 + 𝑝𝑦 2

𝑝𝑦 √4. 𝑙12 (𝑝𝑥 2 + 𝑝𝑦 2 )2 − (𝑝𝑥 2 + 𝑝𝑦 2 + 𝑙12 − 𝑙22 )2

𝜃1 = arctan ( ) ∓ arctan(
𝑝𝑥 𝑝𝑥 2 + 𝑝𝑦 2 + 𝑙12 − 𝑙22

Ta đã tìm ra được 2 giá trị biến khớp cho tay robot

2. Phương pháp giải tích

Tìm ra các công thức hay các phương trình toán giải tích biểu thị mối quan
hệ giữa các giá trị của không gian biến trục và các thông số khác của bộ
thông số DH
Khi giải bài toán động học thuận bằng ma trận DH ta có ma trận biến đổi xác
định vị trí thao tác là :
(0)
𝑇𝑛 (𝑞) = 𝐻1 . 𝐻2 … . . 𝐻𝑛 = 𝐷𝑛 = [𝐴𝑛 𝑟𝐸 (𝑞)]
0𝑇 1
 Từ đó ta xác định được ma trận cosin chỉ hướng của khâu thao tác và vector
điểm thao táo E là các hàm tọa độ suy rộng
Ma trận T có dạng
𝑛𝑥 𝑠𝑥 𝑎𝑥 𝑥𝑝
𝑛𝑦 𝑠𝑦 𝑎𝑦 𝑦𝑝
T= ( 𝑛 𝑠𝑧 𝑎𝑧 𝑧𝑝 )
𝑧
0 0 0 1
 Ta có hệ phương trình động học
𝑛𝑥 = cos(𝜃1 + 𝜃2 )
𝑛𝑦 = 𝑆𝑖𝑛(𝜃1 + 𝜃2 )
𝑛𝑧 = 0
 𝑠𝑥 = −𝑆𝑖𝑛(𝜃1 + 𝜃2 )
𝑠𝑦 = cos(𝜃1 + 𝜃2 )
𝑠𝑧 = 0
{ ………
Ta lấy 6 phương trình gồm 3 phương trình độc lập về hướng và vị trí

𝑥𝑝 = 𝑙1 cos(𝜃1 ) + 𝑙2 cos(𝜃1 + 𝜃2 )
𝑦𝑝 = 𝑙1 sin(θ1 ) + 𝑙2 sin(𝜃1 + 𝜃2 )
𝑧𝑝 = 0
=>
𝑛𝑥 = cos(𝜃1 + 𝜃2 )
𝑠𝑦 = cos(𝜃1 + 𝜃2 )
{ 𝑎𝑧 = 1

Hệ có thể viết lại dưới dạng x = f(q)

Với x = [𝑥𝑝 𝑦𝑝 𝑧𝑝 𝑛𝑥 𝑠𝑦 𝑎𝑧 ]𝑇

Từ đó với các điều kiện và mối quan hệ cho trước ta có thể tìm ra được nghiệm

3. Phương pháp số
Cách 1 : viết hệ phương trình rồi sử dụng lặp Newton-rapson để giải

Phương pháp này có chi phí tính toán lớn và không phải lúc nào cũng hội tụ ( phụ
thuộc điều kiện đầu )
 Cách 2: Sử dụng chuỗi Taylor và ma trận Jacobi để viết phương trình xấp xỉ
tọa độ đầu ra. Từ phương trình xấp xỉ đó ta xây dựng thuật toán hội tụ tới nghiệm
yêu cầu