Luận án Tích hợp đại số gia tử, điều khiển mờ và mạng noron trong điều khiển robot di động luận án tiến sĩ kỹ thuật cơ khí Hà Nội

ước lên và di chuyển tới vị trí chạm đất. Trong đó Hình 2.13 là các đồ thị 
đường dịch chuyển của khớp hông Hình 2.13.a và khớp mắt cá chân Hình 2.13.b với 
51 
độ dời của bước là 0,378 m. Hình 2.14 là các độ thị biểu diễn quy luật chuyển động 
theo thời gian của khớp mắt cá chân gồm vị trí, vận tốc và gia tốc. Khớp mắt cá chân 
di chuyển với quy luật vận tốc hình thang bời lúc bắt đầu từ vị trí đứng yên và kết 
thúc tiếp đất nên vận tốc bằng không. Ở đây cũng giả thiết khớp hông gắn với thân 
có chuyển động đều với vận tốc 1,2v = m/s. 
a) b) 
Hình 2.13 Quỹ đạo của hông và mắt cá chân ở bước khởi động 1 
Hình 2.14 Quy luật chuyển động của mắt cá chân ở bước khởi động 1 
Phần tiếp theo trình bày thêm kết quả thiết kế quỹ đạo chuyển động cho robot di 
động hai chân với các bước khởi động, bước đi đều và bước kết thúc. 
Các hình 2.15, 2.16 và 2.17 bên dưới là đồ thị quỹ đạo đường dịch chuyển của 
khớp hông và khớp mắt cá chân. Hình 2.15 là bước khởi động, Hình 2.16 là bước đi 
đều và Hình 2.17 là bước kết thúc. Độ dời bước của bước khởi động và bước kết thúc 
là 0.38 m, còn bước đều là 0.76 m. 
52 
Hình 2.15 Quỹ đạo của hông và mắt cá chân trong bước khởi động 2 
Hình 2.16 Quỹ đạo của hông và mắt cá chân trong bước đi đều 
Hình 2.17 Quỹ đạo hông và mắt cá chân trong bước kết thúc 
Các hình 2.18, 2.19 và 2.20 là các đồ thị vị trí và vân tốc phương y và z theo thời 
gian của khớp hông và khớp mắt cá chân trong các bước khởi động, bước đi đều và 
bước kết thúc. Với bước khởi động vận tốc trung bình là 0,4v = m/s, bước kết thúc 
có vận tốc trung bình là 0,4v = m/s và vận tốc của bước đi đều 1,2v = m/s. 
53 
Hình 2.18 Quỹ đạo hông và mắt cá chân ở bước khởi động 2 theo thời gian 
Hình 2.19 Quỹ đạo hông và mắt cá chân ở bước đi đều theo thời gian 
Hình 2.20 Quỹ đạo hông và mắt cá ở bước kết thúc theo thời gian 
2.3.3.2 Quỹ đạo chuyển động của robot 
Quỹ đạo chuyển động của các khớp còn lại của robot được tính toán thông qua 
việc giải động học ngược với đầu vào là quỹ đạo chuyển động của khớp hông và khớp 
mắt cá chân. Trong bài toán động học ngược sử dụng robot với các kích thước động 
học được cho trong Bảng 2.4 bên dưới. 
Bảng 2.4 Kích thước động học của robot chuyển động phẳng 
Chiều dài a13, a23 a14, a24 a16, a26 l1 l2 
Giá trị (m) 0.38 0.38 0.08 0.055 0.115 
Sau đây là quỹ đạo chuyển động của robot nhận được sau khi giải động học ngược 
tương ứng với các quỹ đạo của khớp hông và khớp mắt cá chân. 
54 
Trong Hình 2.21 là đồ thị tọa độ và vận tốc của các khớp tương ứng với quỹ đạo 
của khớp hông và khớp mắt cá chân cho trong Hình 2.13. 
Hình 2.21 Tọa độ và vận tốc các khớp trong bước khởi động 1 
Trong hình 2.22 ÷ 2.24 là đồ thị tọa độ và vận tốc của khớp tương ứng với bước 
khởi động, bước đi đều và bước kết thúc với quỹ đạo dịch chuyển của khớp hông và 
khớp mắt cá chân cho trong các hình 2.15 ÷ 2.17 với quỹ luật chuyển động theo thời 
gian cho trong các hình 2.18 ÷ 2.20. 
Hình 2.22 Tọa độ và vận tốc của các khớp trong bước khởi động 2 
Hình 2.23 Tọa độ và vận tốc của các khớp trong bước đi đều 
55 
Hình 2.24 Tọa độ và vận tốc của các khớp trong bước kết thúc 
Quỹ đạo của robot ở dạng tọa độ khớp và các đạo hàm thứ nhất và thứ hai của 
chúng theo thời gian được lưu trong các tệp “pq (k) .txt”, “vq (k) .txt”, “aq (k). txt ”. 
Ở đây, chỉ số k = 1, 2, 3 tương ứng với bước khởi động, bước đi đều và các bước kết 
thúc. Các tệp dữ liệu này được sử dụng cho bài toán động lực học ngược cũng như để 
mô phỏng các bộ điều khiển trong các chương tiếp theo. 
Kết luận Chương 2 
Chương 2 đã xây dựng mô hình động học của robot để có được phương trình động 
học trong trường hợp chuyển động không gian, cũng như phương trình động học trong 
trường hợp chuyển động phẳng. Bài toán thiết kế quỹ đạo chuyển động cho robot di 
động hai chân được trình đầy đủ và chi tiết. Với phương trình động học được thiết 
lập, thuật giải của các bài toán động học của robot được trình bày. 
Đóng góp mới của luận án trong chương này là đã thiết lập được hệ phương trình 
động học chuyển động không gian và chuyển động phẳng của robot. Các giải thuật 
giải bài toán động học. Các giải thuật thiết kế quỹ đạo chuyển động và kết quả mô 
phỏng bằng số để kiểm chứng giải thuật tính toán động học. 
Các kết quả từ các chương trình mô phỏng số cho thấy tính đúng đắn của việc mô 
hình hóa động học, thuật giải bài toán động học, thuật giải bài toán thiết kế quỹ đạo 
chuyển động của robot di động hai chân. Kết quả của bài toán thiết kế quỹ đạo chuyển 
động đã tạo ra các quỹ đạo bước đi cho robot một cách nhanh chóng và thuận lợi, có 
thể thay đổi thông số đầu vào thì sẽ nhận được các quỹ đạo chuyển động khác nhau. 
Kết quả của bài toán động học sẽ được sử dụng trong các bài toán động lực học và 
điều khiển trong các chương tiếp theo. Các phương pháp và giải thuật của các bài 
toán động học và thiết kế quỹ đạo chuyển động cũng có thể được áp dụng cho các mô 
hình robot di động trong các nghiên cứu tiếp theo. 
56 
CHƯƠNG 3. ĐỘNG LỰC HỌC ROBOT DI ĐỘNG 
HAI CHÂN 
Chương này sẽ trình bày việc khảo sát động lực học cho robot di động hai chân. 
Đầu tiên sẽ lựa chọn mô hình động lực học cho robot, thiết lập hệ tọa độ khảo sát 
động lực học, đưa ra phương trình vi phân động lực học của robot, tính toán các đại 
lượng động lực. Cuối chương đưa ra thuật giải các bài toán động lực học và kết quả 
giải bài toán động lực học ngược. 
Bài toán động học và bài toán thiết kế quỹ đạo chuyển động đã được trình bày 
trong chương hai với mục đích tìm ra quy luật chuyển động của các khớp. Với bài 
toán động lực học sẽ phải thiết lập mô hình động lực, từ mô hình động lực giải bài 
toán động lực học thuận thì cũng tìm ra được vị trí, vận tốc, gia tốc khâu của robot 
dựa vào lực và mô men dẫn động ở động cơ. Còn bài toán động lực học ngược là dựa 
trên yêu cầu về quy luật chuyển động của robot thì sẽ tính được lực momen dẫn động 
ở các động cơ. 
Trong bài toán động học thì xuất phát từ bước đi tự nhiên của con người nên đã 
chọn cách gắn hệ tọa độ vào thân của robot làm hệ tọa độ khảo sát từ đó xây dựng 
chuỗi động học xuất phát từ thân robot kết quả của bài toán đó tìm được chuyển động 
của các khớp. Trong bài toán động lực học dù chọn hệ tọa độ theo cách nào thì cũng 
hoàn toàn sử dụng một cách thuận lợi các kết quả của bài toán động học. Bởi vì mô 
hình động lực phải khảo sát đồng thời toàn bộ mô hình cho nên phải xây dựng hệ tọa 
độ sao cho thuận lợi. Ở đây xuất phát từ việc phân tích chuỗi động học của robot khi 
thực hiện bước đi ở pha đơn là chuỗi hở, một chân trụ đặt trên nền như vậy chọn hệ 
tọa độ gắn vào nền làm hệ tọa độ cơ sở và các hệ tọa độ gắn vào các khâu thành chuỗi 
động học trên cơ sở đó để thiết lập mô hình động lực dẫn ra phương trình vi phân 
chuyển động của robot . 
3.1 Xây dựng mô hình động lực học robot 
3.1.1 Các mô hình động lực học của robot di đông hai chân 
Trong chương hai đã trình bày về các pha chuyển động của robot và lựa chọn pha 
đơn để khảo sát các bài toán của robot. Với pha đơn, robot sẽ có trạng thái là một 
chuỗi động học hở, với bàn chân trụ tiếp xúc với mặt đất và bàn chân bước thì di 
chuyển trong trên không. 
Khi robot thực hiện chuyển động bước đi trong pha đơn thì mô hình động học của 
robot là một chuỗi động hở gồm 7 khâu như trong Hình 3.1 trong đó 3 khâu thuộc 
mỗi chân gồm đùi, cẳng chân và bàn chân. Hai tay và đầu cùng chuyển động với thân 
và được coi như khâu thân. 
Trong quá trình robot di chuyển thân robot có thể có chuyển động xoay hoặc rung 
lắc nhỏ thì mô hình robot được thể hiện trong Hình 3.1 a. Nếu bàn chân trụ có chuyển 
động xoay với góc xoay lòng bàn chân với nền là thì mô hình robot sẽ là mô hình 
57 
gồm 7 góc quay từ 1 đến 7 . Trong đó 2 đến 7 là các tọa độ khớp, 1 là góc xoay 
của bàn chân trụ so với nền. Trường hợp bàn chân trụ không có chuyển động mà chỉ 
đứng yên góc 0= thì góc 1 sẽ là hằng số. 
Khi robot bước đi đều ổn định thân robot chuyển động tịnh tiến không có rung lắc 
thì mô hình robot được thể hiện trong hình Hình 3.1 b. Toàn bộ thân coi như một vật 
nặng có khối lượng bằng đúng khối lượng của thân đặt tại điểm ở hông mô hình động 
học của robot sẽ là mô hình 6 góc quay từ 1 đến 6 . Trong đó 2 đến 6 là các tọa 
độ khớp, 1 là góc xoay của bàn chân trụ so với nền. Nếu bàn chân trụ không có 
chuyển động so với nền thì góc 1 sẽ là hằng số. 
a) b) 
Hình 3.1 Các mô hình động lực học của robot di động hai chân 
Trong bài toán điều khiển thì chỉ điểu khiển các tọa độ khớp 2 đến 7 với mô 
hình trong Hình 3.1 a (hay 2 đến 6 với mô hình trong Hình 3.1 b). Riêng 1 là góc 
xoay của bàn chân trụ so với nền sẽ không được điều khiển mà nó là một tham số liên 
quan đến trạng thái cân bằng ổn định của robot. Việc đảm bảo giá trị góc 1 cho mục 
đích cân bằng ổn định của robot không được thực hiện trực tiếp bởi động cơ dẫn động 
mà phụ thuộc vào trạng thái động của robot cũng như thuật toán điều khiển áp dụng 
trên các khớp do động cơ dẫn động. Do đó, 1 vừa là tham số liên quan đến vấn đề 
thiết kế quỹ đạo chuyển động vừa là một trong những tọa độ độc lập liên quan đến 
vấn đề điều khiển chuyển động của robot. 
Trong luận án sẽ khảo sát mô hình robot 7 khớp chuyển động quay nhưng trong 
Hình 3.1 a. 
58 
3.1.2 Hệ tọa độ khảo sát động lực học 
Bài toán động học dựa trên quy luật chuyển động tự nhiên của con người mong 
muốn thân của robot di chuyển dúng như cách di chuyển tự nhiên của thân người thì 
ta quan sát vào chuyển động thân của người trên cơ sở đó phối hợp với các thông số 
động học để mô tả chuyển động của robot từ đó mô phỏng hoạt động của robot. Mô 
hình động học có thể tách rời các bộ phận của robot để nghiên cứu. 
Với mô hình động lực học thì phải nghiên cứu đồng thời toàn bộ các khâu. Chuyển 
động của khâu này liên quan đến chuyển động của các khâu khác, ảnh hưởng đến yếu 
tố động lực của nó. Nên như phân tích ở mục trước mô hình động lực học sử dụng là 
một chuỗi động học hở đối với chuyển động trong pha đơn. 
 Do đó để thuận tiện trong việc khảo sát động lực học và điều khiển robot, ta chọn 
lại hệ tọa độ cơ sở mới, gắn với vị trí chân trụ để khảo sát động lực học. Ở đây chọn 
chân trái làm chân trụ, khi chân phải làm trụ thì kết quả vẫn tương đương. Ta ký hiệu 
hệ tọa độ mới này là 1Oxyz gắn vào điểm tiếp xúc của bàn chân và nền, sơ đồ các hệ 
tọa độ còn lại được gắn vào mô hình robot được thể hiện như trong Hình 3.2. 
Hình 3.2 Sơ đồ gắn hệ tọa độ mới 
Khi đó ta có một chuỗi động học như sau: 
 1 2 3 4 5 6 7 8
Oxyz Oxyz Oxyz Oxyz Oxyz Oxyz Oxyz Oxyz→ → → → → → →
Xác định lại vector hệ tọa độ suy rộng như sau. 
  1 2 3 4 5 6 7
T
q q q q q q q q=
(3.1) 
Trong đó 
 1 1
q = , 2 2q = , 3 3q = , 4 4q = , 5 5q = , 6 6q = , 7 7q = (3.2) 
Đặt 
59 
2 2
1 16 2 2 14 3 13 4 11 21 4 10 20
5 23 6 24 7 26
, , , 0, 
, , 
a a l a a a a a d d d a a
a a a a a a
= + = = = − = = +
= = =
(3.3) 
Ta có ma trận biểu diễn hướng và vị trí của các hệ tọa độ trong chuỗi động học 
như sau: 
 1
0
0
0 0 1 0
0 0 0 1
i i i i
i i i ii
i
C S a C
S C a S
A +
− 
 =
(3.4) 
Trong đó: 
cosi iC q= sini iS q= 
Vị trí và hướng của hệ tọa độ 1Oxyz so với hệ tọa độ 0Oxyz 
0
1
0 0 1
1 0 0
0 1 0
0 0 0 1
L
L
L
x
y
A
z
 =
(3.5) 
Vị trí và hướng của các hệ tọa độ trong chuỗi động học so với hệ tọa độ cơ sở 
0Oxyz là: 
1 1 1 10 0 1
2 1 2
1 1 1 1
0 0 1
0
0
0 0 0 1
L
L
L
x
C S y a C
A A A
S C z a S
 − +
 = =
 +
(3.6) 
12 12 1 1 2 120 0 2
3 2 3
12 12 1 1 2 12
0 0 1
0
0
0 0 0 1
L
L
L
x
C S y a C a C
A A A
S C z a S a S
 − + +
 = =
 + +
(3.7) 
123 123 1 1 2 12 3 1230 0 3
4 3 4
123 123 1 1 2 12 3 123
0 0 1
0
0
0 0 0 1
L
L
L
x
C S y a C a C a C
A A A
S C z a S a S a S
 − + + +
 = =
 + + +
(3.8) 
60 
4
1234 1234 1 1 2 12 3 1230 0 4
5 4 5
1234 1234 1 1 2 12 3 123
0 0 1
0
0
0 0 0 1
L
L
L
x d
C S y a C a C a C
A A A
S C z a S a S a S
+ 
 − + + +
 = =
 + + +
(3.9) 
0 0 5
6 5 6
4
12345 12345 1 1 2 12 3 123 5 12345
12345 12345 1 1 2 12 3 123 5 12345
0 0 1
0
0
0 0 0 1
L
L
L
A A A
x d
C S y a C a C a C a C
S C z a S a S a S a S
=
+ 
 − + + + +
 =
 + + + +
(3.10) 
0 0 6
7 6 7
4
123456 123456 1 1 2 12 3 123 5 12345 6 123456
123456 123456 1 1 2 12 3 123 5 12345 6 123456
0 0 1
0
0
0 0 0 1
L
L
L
A A A
x d
C S y a C a C a C a C a C
S C z a S a S a S a S a S
=
+ 
 − + + + + +
 =
 + + + + +
(3.11) 
0 0 7
8 7 8
4
1234567 1234567 1 1 2 12 3 123 5 12345 6 123456 7 1234567
1234567 1234567 1 1 2 12 3 123 5 12345 6 123456 7 1234567
0 0 1
0
0
0 0 0 1
L
L
L
A A A
x d
C S y a C a C a C a C a C a C
S C z a S a S a S a S a S a S
=
+ 
 − + + + + + +
 =
 + + + + + +
(3.12) 
Trong đó: 
1... 1cos( ... )i iC q q= + + 1... 1sin( ... )i iS q q= + + 
Như vậy đã xác định được các ma trận xác định vị trí và hướng của các khâu trong 
hệ tọa độ mới, các ma trận này sẽ được sử dụng trong phần tính các đại lượng động 
lực của robot. Phần tiếp theo sẽ đi thiết lập phương trình động lực học và tính các đại 
lượng động lực của robot di động hai chân. 
3.2 Thiết lập phương trình động lực học 
3.2.1 Phương trình động lực học dạng ma trận 
Để khảo sát, thiết lập phương trình động lực học cho robot nói chung đặc biệt 
robot di động, robot dạng tay máy thì người ta thường sử dụng các phương pháp phổ 
biến là phương trình Lagrange II hoặc phương trình Newton-Euler. Trong các tài liệu 
61 
[66-73] các tác giả cũng đã dẫn giải ra các cách thiết lập phương trình động lực học 
cho hệ nhiều vật đặc biệt là robot sử dụng các phương pháp trên. 
Trong luận án áp dụng phương trình Lagrange II để thiết lập phương trình động 
lực học robot di động hai chân. Phương trình động lực học của robot sẽ có dạng như 
sau. 
 ( ) ( , ) ( )M q q C q q q G q Q U+ + + = (3.13) 
Trong đó: 
M(q) – Ma trận khối lượng của robot. 
( , )C q q q – Vector lực suy rộng của các lực quán tính Coriolis và quán tính ly tâm. 
G(q) – Vector lực suy rộng của các lực có thế. 
Q – Vector lực suy rộng của các lực không thế. 
U – Vector lực suy rộng của các lực/momen dẫn động. 
Các phần tiếp theo sẽ trình bày các công thức tính các đại lượng động lực của 
phương trình động lực học trên. 
3.2.2 Các đại lượng động lực học 
Với mô hình động lực lựa chọn như trong Hình 3.1 a và cách đặt các hệ tọa độ 
như trong Hình 3.2, mô hình robot là một hệ gồm 7 bậc tự do. Các đại lượng của 
phương trình động lực học được tính toán như sau. 
3.2.2.1 Ma trận khối lượng của robot 
Ma trận khối lượng của robot được tính theo công thức sau 
 ( )
7
1 7 7
( ) T T rTi i Ti Ri Ci Ri
i
M q J m J J J
= 
= +  
 (3.14) 
Trong đó: 
im - Khối lượng của khâu i. 
TiJ , 
T
TiJ - Ma trận Jacobian tịnh tiến của khâu thứ i và ma trận chuyển vị. 
RiJ , 
T
RiJ - Ma trận Jacobian quay của khâu thứ i và ma trận chuyển vị. 
r
Ci - Ma trận quán tính của khâu i đối với khối tâm của nó trong hệ tọa độ động. 
a) Ma trận Jacobian tịnh tiến và ma trận Jacobian quay 
Ma trận Jacobian tịnh tiến của khâu i được tính bởi đạo hàm của vector xác định 
tọa độ khối tâm khâu i trong hệ tọa độ cố định theo vector tọa độ khớp. 
62 
1 2 7
1 2 7
1 2 7
...
...
...
Cix Cix Cix
Ciy Ciy CiyCi
Ti
Ciz Ciz Ciz
r r r
q q q
r r rr
J
q q q q
r r r
q q q
   
   
   
= = 
    
   
   
 (3.15) 
Cir - Vector xác định vị trí khối tâm của khâu i trong hệ tọa độ cố định. 
Với robot có các khâu với cấu trúc đơn giản thì có thể dễ dàng xác định được vị 
trí tọa độ khối tâm trong hệ tọa độ khâu. Tuy nhiên, với các khâu có cấu trúc phức 
tạp thì vị trí khối tâm trong hệ tọa độ khâu thường được xác định thông qua các phép 
đo bằng thiết bị phức tạp hoặc có thể xác định thông qua các phần mềm thiết kế 3D 
như AutoCad, SolidWorks, Inventor, Catia, Pro Engineer... 
Ma trận Jacobian quay của khâu i được tính theo đạo hàm của vector vận tốc góc 
của khâu i trong hệ tọa độ động theo vector vận tốc khớp. 
1 2 7
1 2 7
1 2 7
...
...
...
r r r
ix ix ix
r r rr
iy iy iyi
Ri
r r r
iz iz iz
q q q
y
J
q q q q
q q q
  
 
  
   
   
   
 = =
    
   
    
 (3.16) 
Vector vận tốc góc của khâu i trong hệ tọa độ động có biểu thức như công thức 
(3.17), các thành phần của nó thu được từ ma trận ri như cho trong công thức (3.18). 
T
r r r r
i ix iy iz    = (3.17) 
0
0
0
T
r r
iz iy
r r r
i iz ix
r r
iy ix
 
  
 
 −
= − 
 − 
 (3.18) 
Ma trận ri được tính theo công thức sau. 
 r Ti i iR R = (3.19) 
Trong đó TiR , iR là ma trận chuyển vị và ma trận đạo hàm của ma trận cosin chỉ 
hướng iR của khâu i. Ma trận iR nhận được từ ma trận biến đổi tọa độ thuần nhất iA
như trong (3.20). 
0 1
i i
i
R r
A
= 
 (3.20) 
b) Tính các ma trận Jacobian của các khâu. 
Khâu bàn chân trái 1-6 (khâu 1) 
63 
Tọa độ khối tâm của khâu trong hệ tọa độ khâu: 
Hình 3.3 Tọa độ khối tâm của bàn chân trái 
  
2
1 1 1 0
T
c c cr x y= (3.21) 
Ma trận biểu diễn hệ tọa độ khối tâm trong hệ tọa độ cơ sở: 
1
1 1 1 1 10 0 2
1 2 1
1 1 1 1
1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1
0 0 1 1 0 0
0 0 1 0
0 0 0 1 0
0 0 0 1 0 0 0 1
0 0 1
0 ( )
0 ( ) 1
0 0 0 1
L c
L c
c c
L
L
L c c c c
T
L c c
x x
C S y a C y
A A A
S C z a S
x
C S y a x C y S R r
S C z a x S y C O
 − +
 = =
 +
 − + + − = = + + + 
(3.22) 
Vậy tọa độ khối tâm trong hệ tọa độ cơ sở là: 
 1 1 1 1 1 1
1 1 1 1 1
( )
( )
L
c L c c
L c c
x
r y a x C y S
z a x S y C
 = + + −
 + + + 
(3.23) 
Ma trận Jacobian tịnh tiến của khối tâm khâu 1-6 được tính: 
1
1 1 1 1 1 1
1 1 1 1 1
0 0 0 0 0 0 0
( ) 0 0 0 0 0 0
( ) 0 0 0 0 0 0
c
T c c
c c
r
J a x S y C
q
a x C y S
 = = − + −
 
 + − 
(3.24) 
Có ma trận cosin chỉ hướng, ma trận chuyển vị và ma trận đạo hàm của khâu 1-6 
như sau. 
 1 1 1
1 1
0 0 1
0
0
cR C S
S C
 = −
, 
1 1
1 1 1
0
0
1 0 0
T
c
C S
R S C
 = −
, 
1 1 1 1
1 1
0 0 0
0
0
cR S C q
C S
 = − −
 − 
(3.25) 
Vận tốc góc của khâu 1-6 được tính như sau: 
64 
1
1 1 1 1
0 0
0 0
0 0 0
T
c c
q
R R q
− 
 = =
(3.26) 
 ( )1 10 0
T
q =
(3.27) 
Ta tính được ma trận Jacobian quay của khâu 1-6: 
 1
1
0 0 0 0 0 0 0
0 0 0 0 0 0 0
1 0 0 0 0 0 0
RJ
q

 = =
 
(3.28) 
Khâu cẳng chân trái 1-4 (khâu 2) 
Tương tự các bước đã tính với khâu 1-6. 
Tọa độ khối tâm trong hệ tọa độ khâu: 
Hình 3.4 Tọa độ khối tâm cẳng chân trái 
  
3
2 2 0 0
T
c cr x= (3.29) 
Ma trận biểu diễn hệ tọa độ khối tâm khâu cẳng chân trái: 
12 12 1 1 2 2 120 0 3
2 3 2
12 12 1 1 2 2 12
0 0 1
0 ( )
0 ( )
0 0 0 1
L
L c
c c
L c
x
C S y a C a x C
A A A
S C z a S a x S
 − + + +
 = =
 + + +
(3.30) 
Tọa độ khối tâm và ma trận cosin chỉ hướng: 
( )
2 1 1 2 2 12
1 1 2 2 12
2 12 12 2 12 12 1 2
12 12 12 12
( )
( )
0 0 1 0 0 0
0 ; 0
0 0
L
c L c
L c
c c
x
r y a C a x C
z a S a x S
R C S R S C q q
S C C S
 = + + +
 + + + 
 = − = − − +
 − 
(3.31) 
Tính được ma trận Jacobian tịnh tiến: 
65 
2 1 1 2 2 12 2 12 2 12
1 1 2 2 12 2 12 2 12
0 0 0 0 0 0 0 0
( ) 0 0 0 0 0 0
( ) 0 0 0 0 0 0
T c c
c c
J a S a x S a S x S
a C a x C a C x C
 = − − + − −
 + + + 
(3.32) 
Ma trận Jacobian quay: 
2
0 0 0 0 0 0 0
0 0 0 0 0 0 0
1 1 0 0 0 0 0
RJ
 =
(3.33) 
Với các khâu còn lại cách tính các ma trận Jacobian cũng làm tương tự như với 
hai khâu trên nên không trình bày tiếp ở đây. 
c) Ma trận quán tính 
Ma trận quán tính của khâu i được cho trong công thức (3.34). 
r r r
ixx ixy ixz
r r r r
Ci iyx iyy iyz
r r r
izx izy izz
I I I
I I I
I I I
 − −
 = − − 
 − − 
 (3.34) 
r
ixxI , 
r
iyyI 
r
izzI - là các mô men quán tính của khâu i đối với trục x, y và z của hệ tọa 
độ gắn vào khâu i tại khối tâm của khâu. 
r
ixyI , 
r
iyzI , 
r
izxI - là các mô men quán tính của khâu i đối với các trục của hệ tọa độ 
gắn vào khối tâm của khâu i. 
V