Động lực học ngược robot song song

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
-------------------------------------------------

TRẦN XUÂN TIẾN

ĐỘNG LỰC HỌC NGƯỢC ROBOT
SONG SONG
LUẬN VĂN THẠC SỸ KHOA HỌC
Chuyên ngành: Cơ điện tử

Hà Nội - 2012


BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
--------------------------------------------

TRẦN XUÂN TIẾN

ĐỘNG LỰC HỌC NGƯỢC ROBOT
SONG SONG
Chuyên ngành: Cơ điện tử

LUẬN VĂN THẠC SỸ KHOA HỌC

NGƯỜI HƯỚNG DẪN KHOA HỌC :

GS.TSKH NGUYỄN VĂN KHANG

Hà Nội - 2012

MỤC LỤC

MỞ ĐẦU .....................................................................................................................1
Lý do chọn đề tài .....................................................................................................1
Lịch sử nghiên cứu ..................................................................................................1
Các luận điểm cơ bản ..............................................................................................2
Phương pháp nghiên cứu.........................................................................................2
CHƯƠNG I:CƠ SỞ LÝ THUYẾT TÍNH TỐN ĐỘNG HỌC VÀ ĐỘNG LỰC
HỌC NGƯỢC ROBOT SONG SONG ......................................................................4
1.1.

Động học ngược robot song song ............................................................... 4

1.1.1.

Thiết lập bài tốn......................................................................................... 4

1.1.1.1. Khơng gian cấu hình và khơng gian thao tác .............................................. 4
1.1.1.2. Bài toán động học ngược robot song song .................................................. 6
1.1.1.3. Robot song song chuẩn ............................................................................... 6
1.1.2.

Phương pháp hiệu chỉnh gia lượng véc tơ tọa độ suy rộng giải bài toán động

học ngược robot song song ...................................................................................... 6
1.1.2.1. Các công thức xác định véc tơ vận tốc và véc tơ gia tốc tọa độ khớp ........ 6
1.1.2.2. Thuật toán hiệu chỉnh véc tơ tọa độ suy rộng ............................................. 7
1.1.2.3. Công thức xác định các véctơ q  t  và q  t  tại thời điểm t ........................ 9

k

1.1.2.4. Đánh giá sai số ............................................................................................ 9
1.2.
Phương pháp tách cấu trúc trong bài toán động lực học ngược robot song
song….. .................................................................................................................. 10


1.2.1.

Ý tưởng của phương pháp tách cấu trúc ................................................... 10

1.2.2.

Phương trình Lagrange loại 2 của cấu trúc con ........................................ 11

1.2.2.1. Biểu thức động năng và thế năng của robot. ............................................. 11
1.2.2.2. Thiết lập dạng thức Lagrange loại 2 ......................................................... 14
1.2.3.

Dạng ma trận của phương trình Lagrange loại 2 ...................................... 15

1.2.4.

Phương trình Lagrange dạng nhân tử của hệ nhiều vật ............................ 17

1.2.4.1. Thiết lập phương trình Lagrange dạng nhân tử cho hệ n chất điểm ......... 17
1.2.4.2. Thiết lập phương trình Lagrange dạng nhân tử cho hệ p vật rắn .............. 19
1.2.4.3. Phương trình Lagrange dạng nhân tử cho hệ n vật rắn chịu các liên kết
hơnơlơm. ................................................................................................................. 21

1.3.

Ví dụ .......................................................................................................... 21

1.3.1.

Thiết lập phương trình vi phân chuyển động của chân thứ nhất............... 23

1.3.2.

Thiết lập phương trình vi phân chuyển động của chân thứ hai................. 27

1.3.3.

Thiết lập phương trình vi phân chuyển động của chân thứ ba .................. 31

1.3.4.

Thiết lập phương trình vi phân chuyển động của bàn máy động.............. 35

1.4.

Bài toán động lực học ngược .................................................................... 36

1.4.1.

Phương pháp ma trận hình chiếu giải bài tốn động lực học ngược......... 36

1.4.2.

Phương pháp giải bài toán động lực học ngược dựa trên phương trình

Lagrange dạng nhân tử ........................................................................................... 36

1.4.3.

Phương pháp giải bài tốn động lực học ngược dựa trên các phương trình vi

phân thu gọn về các tọa độ khớp chủ động ............................................................ 38


1.5.

Kết luận chương I...................................................................................... 41

CHƯƠNG II: ĐỘNG HỌC NGƯỢC ROBOT SONG SONG KHƠNG GIAN
STEWART-GOUGH ................................................................................................42
2.1.

Phân tích vị trí robot song song khơng gian Stewart-Gough ................... 42

2.1.1.

Xác định vị trí của bàn máy động ............................................................. 43

2.1.2.

Xác định vị trí các chân............................................................................. 43

2.1.3.

Thiết lập các phương trình liên kết. .......................................................... 46

2.1.4.

Chọn tọa độ suy rộng ................................................................................ 49

2.2.

Phân tích vận tốc robot song song Sterwart – Gough ............................... 49

2.2.1.

Trạng thái vân tốc các chân ...................................................................... 49

2.2.2.

Trạng thái vận tốc, gia tốc của bàn máy động .......................................... 50

2.3.
Mơ phỏng số bài tốn động lực học ngược robot song song không gian
Stewart-Gough. ...................................................................................................... 51
2.3.1.

Nội dung bài toán. ..................................................................................... 51

2.3.2.

Phương pháp số giải bài toán động học ngược ......................................... 53

2.3.2.1. Mơ phỏng số bài tốn động học ngược sử dụng Maple ............................ 59
2.4.

Kết luận chương II .................................................................................... 73

CHƯƠNG III: ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG STEWARTGOUGH ....................................................................................................................74
3.1.
Thiết lập phương trình vi phân đại số mơ tả chuyển động robot song song
Stewart-Gough ....................................................................................................... 74
3.1.1.

Các phương trình vi phân chuyển động của chân thứ i ............................ 74

3.1.2.

Phương trình vi phân của bàn máy động .................................................. 79


3.1.3.

Hệ phương trình vi phân đại số của robot song song Stewart-Gough ...... 81

3.2.
Giải bài toán động lực học ngược robot song song không gian StewartGough bằng phương pháp ma trận hình chiếu. ...................................................... 85
3.2.1.

Bài tốn động lực học ngược .................................................................... 85

3.2.1.1. Phương pháp giải ...................................................................................... 85
3.2.1.2. Phương pháp giải bài tốn động lực học ngược dựa trên phương trình

Lagrange dạng nhân tử ........................................................................................... 89
3.2.1.3. Phương pháp giải bài toán động lực học ngược dựa trên các phương trình vi
phân thu gọn về các tọa độ khớp chủ động ............................................................ 93
3.2.1.4. Mơ phỏng số bài tốn động học ngược sử dụng Maple ............................ 94
3.3.

Kết luận chương III. ................................................................................ 101

KẾT LUẬN VÀ KIẾN NGHỊ.................................................................................103
TÀI LIỆU THAM KHẢO .............................................................................................
PHỤ LỤC ......................................................................................................................


MỤC LỤC HÌNH VẼ
Hình 1.1. Robot song phẳng 3RRR.............................................................................5
Hình 1.2. Robot song phẳng 3RRR...........................................................................10
Hình 1.2. Chân thứ 1

Hình 1.4. Chân thứ 2 ..........................................11

Hình 1.5. Chân thứ 3

Hình 1.6. Bàn máy động..........................................11

Hình 1.7. Hệ quy chiếu khâu .....................................................................................12
Hình 1.8. Robot song phẳng RRR .............................................................................22
Hình 1.9: Bốn cấu trúc con của robot. ......................................................................23
Hình 1.10: Cấu trúc con thứ nhất. .............................................................................23
Hình 1.11: Cấu trúc con thứ hai. ...............................................................................27
Hình 1.12: Cấu trúc con thứ hai. ...............................................................................31

Hình 1.12: Cấu trúc con thứ hai ................................................................................35
Hình 2.1: Robot Stewart-Gough. ..............................................................................42
Hình 2.2:Các góc Euler của mỗi chân và thơng số kích thước, khối lượng các chân.
...................................................................................................................................44
Hình 2.5 : Quỹ đạo chuyển động của điểm P............................................................60
Hình 2.6: Quy luật biến đổi của di ............................................................................61
Hình 2.7: Quy luật biến đổi của θi.............................................................................61
Hình 2.8: Quy luật biến đổi của ϕi ............................................................................62
Hình 2.9: Quy luật biến đổi của vận tốc góc các tọa độ suy rộng ............................63
Hình 2.10: Quy luật biến đổi của gia tốc các tọa độ suy rộng ..................................65
Hình 2.11: Đồ thị biến đổi vị trí, vận tốc, gia tốc các tọa độ suy rộng di. ................68
Hình 2.11: Quy luật biến đổi của di ..........................................................................69
Hình 2.12: Quy luật biến đổi của θi...........................................................................69
Hình 2.13: Quy luật biến đổi của ϕi ..........................................................................70


Hình 2.14: Quy luật biến đổi của vận tốc góc các tọa độ suy rộng ..........................71
Hình 2.15: Quy luật biến đổi của gia tốc các tọa độ suy rộng ..................................73
Hình 3.1: Cấu trúc con của robot. .............................................................................74
Hình 3.2: Bàn máy động. ..........................................................................................79
Hình 3.3: Các lực khơng thế tác dụng lên chân 1 .....................................................95
Hình 3.4: Các lực khơng thế tác dụng lên chân 2 .....................................................95
Hình 3.5: Các lực khơng thế tác dụng lên chân 3 .....................................................96
Hình 3.6: Các lực khơng thế tác dụng lên chân 4 .....................................................96
Hình 2.20: Các lực khơng thế tác dụng lên chân 5 ...................................................97
Hình 3.7: Các lực khơng thế tác dụng lên chân 6 .....................................................97
Hình 3.8: Lực dẫn động cần thiết truyền động cho các khớp dẫn động ...................98
Hình 3.9: Các lực khơng thế tác dụng lên chân 1 .....................................................98
Hình 3.10: Các lực khơng thế tác dụng lên chân 2 ...................................................99
Hình 3.11: Các lực khơng thế tác dụng lên chân 3 ...................................................99

Hình 3.12: Các lực khơng thế tác dụng lên chân 4 .................................................100
Hình 3.13: Các lực khơng thế tác dụng lên chân 5 .................................................100
Hình 3.14: Các lực khơng thế tác dụng lên chân 6 .................................................101
Hình 3.15: Lực dẫn động cần thiết truyền động cho các khớp dẫn động ...............101


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
MỞ ĐẦU
Lý do chọn đề tài
Robot song song là robot có cấu trúc vịng kín trong đó các khâu được nối
với nhau bằng các khớp động. Trong robot song song, khâu thao tác được nối với
giá cố định bởi một số mạch động học, tức là nối song song với nhau và cũng hoạt
động song song với nhau.
Robot song song có nhiều ưu điểm như khả năng chịu tải trọng lớn, độ cứng
vững cao do kết cấu hình học của chúng, có thể thực hiện các thao tác phức tạp và
hoạt động với độ chính xác cao. Vì những ưu điểm đó, robot song song được ứng
dụng trong nhiều lĩnh vực của công nghiệp đặc biệt là trong lĩnh vực cơ khí - tự
động hóa.
Robot song song điển hình gồm có bàn máy động được nối với giá cố định,
dẫn động theo nhiều nhánh song song hay còn gọi là các chân. Thường robot song
song có số chân bằng số bậc tự do, được điều khiển bởi nguồn phát động đặt trên
giá cố định hoặc ngay trên chân. Nguồn phát động sẽ truyền chuyển động cho chân
chủ động bằng momen hoặc lực. Vì vậy để có thể điểu khiển được hoạt động của
robot, ta cần phải tính tốn được mơmen hay lực của các khâu chủ động. Từ đó xác
định được cơng suất động cơ. Đây chính là nhiệm vụ của bài tốn động lực học
ngược
Lịch sử nghiên cứu
Xuất phát từ nhu cầu và khả năng linh hoạt hóa trong sản xuất, các cơ cấu
Robot cũng ngày càng phát triển rất đa dạng và phong phú. Trong những thập niên
gần đây, Robot cấu trúc song song được Gough và

và sự chú

hitehall nghiên cứu năm 1 62

ứng dụng của Robot cấu trúc song song đ được khởi động bởi Ste art

vào năm 1 6 .

ng là người cho ra đời một buồng tập lái máy bay dựa trên cơ cấu

song song. Hiện nay cơ cấu song song được sử dụng rộng r i trong nhiều lĩnh vực.
Loại Robot song song điển hình gồm có bàn máy động được nối với giá cố
định, bằng một mạch động học hay c n gọi là các nhân. Thường số chân bằng số


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
bậc tự do được tự do được điều khiển bởi nguồn phát động đặt trên giá cố định hoặc
ngay trên chân.
o tính ưu việt của Robot song song nên ngày càng thu hút được nhiều nhà
khoa học nghiên cứu, đồng thời cũng được ứng dụng ngày càng rộng r i vào nhiều
vào nhiều lĩnh vực .
Ngành Vật l : Giá đ kính hiển vi, giá đ thiết bị đo chính xác.
Ngành Cơ khí:

áy gia c ng cơ khí chính xác, máy c ng cụ.

Ngành bưu chính vi n th ng: Giá đ ăngten vệ tinh đĩa hình.
Ngành chế tạo t : hệ thống thử tải lốp t , buống tập lái t .
Ngành uân sự Robot song song được d ng làm bệ đ ổn định được đặt trên
tàu thủy , các c ng trình thủy, trên xe, trên máy bay, trên chiến xạ và tàu ngầm. Để

giữ cân bằng cho ăngten, camera theo d i mục tiêu, cho rada, cho các thiết bị đo
laser, bệ ổn định cho pháo và tên lửa, buồng tập lái máy bay , xe tăng ,tàu chiến
Các luận điểm cơ bản
Các luận điểm trong luận văn được thể hiện qua nội dung của 3 chương:
Chương I trình bày một số vấn đề cơ bản của bài toán động học ngược và bài
toán động lực học ngược robot song song,các phương pháp số tính tốn bài tốn
động học, động lực học ngược
Chương II trình bày việc thiết lập phương trình động học,hệ phương trình
liên kết của robot song song khơng gian Stewart-Gough, tính tốn và mơ phỏng số
giải bài toán động học ngược sử dụng phần mềm MAPLE.
Chương III trình bày việc thiêt lập hệ phương trình vi phân – đại số của robot
song song không gian Stewart-Gough sử dụng phương pháp tách cấu trúc,giải bài
toán động lực học ngược robot khơng gian Stewart-Gough bằng các phương pháp
đ trình bày (sử dụng phần mềm MAPLE).
Phương pháp nghiên cứu
Hiện nay có nhiều phương pháp tính tốn động lực học ngược robot song
song như: phương pháp tách cấu trúc, phương pháp nhân tử Lagrange, phương pháp
nguyên lý công ảo, … Luận văn này xin trình bày phương pháp tách cấu trúc để tính
tốn động lực học ngược robot song song.
2


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
Cuối cùng, em xin chân thành cảm ơn GS.TSKH. Nguy n Văn Khang đã tận
tình chỉ bảo, hướng dẫn em trong suốt quá trình thực hiện luận văn tốt nghiệp.
Những bài giảng, tài liệu tham khảo, sự hướng dẫn tận tình và cả phương pháp làm
việc của thầy là nền tảng giúp em có thể hoàn thành tốt đề tài tốt nghiệp được giao,
thày đ giúp đ , tạo điều kiện cho em trong suốt q trình nghiên cứu và hồn thành
luận văn này.
Trong q trình hồn thành luận văn, em đ rất cố gắng nhưng sẽ khơng thể

tránh khỏi những thiếu sót. Vì vậy, em rất mong nhận được những ý kiến tham gia,
góp ý quý báu để luận văn của em được hoàn thiện hơn.
Một lần nữa em xin chân thành cảm ơn!

Hà Nội, tháng 09 năm 2012

Trần Xuân Tiến

3


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
CHƯƠNG I
CƠ SỞ LÝ THUYẾT TÍNH TỐN ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC
NGƯỢC ROBOT SONG SONG
Trong chương này trình bày cơ sở lý thuyết giải bài toán động học ngược robot
song song bằng phương pháp số, thiết lập phương trình vi phân đại số robot song
song bằng phương pháp tách cấu trúc và hai thuật toán giải bài toán động lực học
ngược robot song song. Việc giải bài toán động lực học ngược hệ nhiều vật nói
chung và robot song song nói riêng là những vấn đề thực sự đang được quan tâm
nghiên cứu.
1.1. Động học ngược robot song song
1.1.1. Thiết lập bài tốn
1.1.1.1.

Khơng gian cấu hình và khơng gian thao tác

Gọi q a 
song, q p 

là véc tơ tọa độ các khớp chủ động (active joint) của robot song

na

np

T

là véc tơ tọa độ các khớp bị động (passive joint) của robot. Ký hiệu

q   qTa , qTp  

n

là véc tơ các tọa độ khớp của robot song song. Tập các khớp

chủ động và các khớp bị động của robot song song tạo thành một không gian
Euclide n  chiều và được gọi là khơng gian cấu hình của robot song song.
q

 q | qimin  qi  qimax , i  1,..., n 

(1.1)

Trong kỹ thuật người ta thường cho biết chuyển động của một số tọa độ khớp (khớp
bị động). Các tọa độ khớp này thường là các tọa độ xác định hướng của khâu thao
tác. Giả sử số các tọa độ khớp cho biết là

nx


Gọi n  na  np  nx

(1.2)

là số lượng các tọa độ khớp cần xác định. Tập các tọa độ khớp cần xác định tạo
thành một không gian Euclide n  chiều và được gọi là khơng gian cấu hình cần xác
định của robot song song
q

 q | qimin  qi  qimax , i  1,..., n 

(1.3)

Để đơn giản sau này ta uy ước gọi: n  n , q  q .
Ký hiệu x   x1 , x2 ,..., xm  là véc tơ các tọa độ xác định vị trí (điểm định vị và góc
T

quay) của khâu thao tác. Nếu khâu thao tác là một vật rắn thì m  6 . Các tọa độ
4


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
x1 , x2 ,..., xm được gọi là các tọa độ thao tác. Tập các tọa độ thao tác tạo thành

một không gian Euclide m chiều và được gọi là không gian thao tác của robot song
song:
x

 x | xi min  xi  xi max , i  1,..., m

(1.4)

Đối với hệ nhiều vật hơlơnơm có cấu trúc mạch vịng (hoặc các robot song song) ta
thiết lập được các phương trình liên kết:

f  q, x  = 0
Với

q

(1.5)
n

,x

m

,f 

r

Ví dụ: Xét mơ hình robot song phẳng 3RRR như hình vẽ 1.1.

Hình 1.1. Robot song phẳng 3RRR
Gọi q a 
song, q p 

na

np

là véc tơ tọa độ các khớp chủ động (active joint) của robot song

là véc tơ tọa độ các khớp bị động (passive joint) của robot. Từ đó ta

có:

q a  1 , 2 ,3 

T

q p   1 , 2 , 3 

T


T
T
Suy ra: q  qa , q p   1 , 1 , 2 , 2 ,3 , 3 
T

T

5


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
Vị trí và hướng của khâu thao tác được xác định bởi 3 tham số là xP , yP ,  .
Trong đó  xP , yP  xác định vị trí của điểm thao tác P, còn  xác định hướng khâu

T

thao tác. Ký hiệu các tham số của khâu thao tác là: x   xP , yP , 

T

1.1.1.2.

Bài toán động học ngược robot song song

Cho biết quy luật chuyển động của khâu thao tác x  x  t  và các phương trình liên
kết f  q, x   0 , q 

n

,x

m

,f 

r

. Tìm quy luật chuyển động của các tọa độ

khớp:

q  q  x  t  
1.1.1.3.
Gọi

(1.6)

Robot song song chuẩn
là số các tọa độ khớp chủ động, m là số các tọa độ thao tác

na

-

Khi na = m: robot song song được gọi là robot chuẩn

-

Khi na > m: robot song song được gọi là robot dư dẫn động

-

Khi na < m: robot song song được gọi là robot hụt dẫn động

Ở đây ta giới hạn xét trường hợp đầu tiên là trường hợp robot chuẩn.
Chú ý rằng bằng suy luận ta suy ra: na  m

r n

1.1.2. Phương pháp hiệu chỉnh gia lượng véc tơ tọa độ suy rộng giải bài toán
động học ngược robot song song
Các phương pháp số cho cách giải bài toán động học ngược robot một cách tổng
quát, cho kết quả chính xác đến mức cần thiết, tuy nhiên có thể lâu hơn phương
pháp giải tích.

ưới đây trình bày phương pháp hiệu chỉnh gia lượng véc tơ tọa độ

suy rộng giải bài toán động học ngược robot song song.
1.1.2.1.

Các công thức xác định véc tơ vận tốc và véc tơ gia tốc tọa độ khớp

Giả sử cho biết các phương trình liên kết của robot song song

f  q, x   0

,f 

r

(1.7)
Đạo hàm biểu thức (1.7) theo thời gian ta được

6


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
f 

f
f
q
x  Φq  q, x  q  Φ x  q, x  x  0
q

x

(1.8)

Trong đó ta sử dụng các ký hiệu
Φq 

f
f
, Φx 
q
x

(1.9)

Ma trận Φ q là ma trận c , ma trận Φ x là ma trận c r  m
Do robot là chuẩn nên r=n, ma trận Φ q là ma trận vuông cấp n. Căn cứ det  Φq   0 ,
từ (1.8) ta suy ra

q  Φq1  q, x  Φx  q, x  x  t 

(1.10)

Đạo hàm biểu thức (1.8) theo t ta được

f  Φq  q, x  q  Φq  q, x  q  Φx q, x  x  Φx q, x  x  0

(1.11)

Do r=n, từ (1.11) suy ra

q  Φq1  q, x  Φq  q, x  q  Φ x  q, x  x  Φ x q, x  x 

1.1.2.2.

(1.12)

Thuật toán hiệu chỉnh véc tơ tọa độ suy rộng

Để thuận tiện, dưới đây ta sử dụng các ký hiệu sau

q k  q  tk  , q k  q  tk  , q k  q  tk 
x k  x  tk  , x k  x  tk  , x k  x  tk 
a. Hiệu chỉnh gia lượng véc tơ tọa độ suy rộng tại thời điểm t0
Ký hiệu t0  0 là thời điểm đầu. Ta có thể xác định véc tơ ban đầu gần đúng q 0
của véc tơ q0  q  t0  bằng phương pháp vẽ (hoặc bằng thực nghiệm). Sau đó áp
dụng cơng thức Taylor tìm gần đúng tốt hơn của q 0 . Khai triển Taylor hàm f  q, x 
tại q0  q0  q0 và x 0 ta được
f  q0 , x0   f  q0  q0 , x0   f  q0 , x0  

f
 q0 , x0  q0 
q

0

(1.13)

Từ (1.13) suy ra phương trình gần đúng

Φq  q0 , x0  q0  f  q0 , x0 

(1.14)

7


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
Giải hệ phương trình đại số tuyến tính (1.14) đối với ẩn q 0 , ta được

q0  Φq1  q0 , x0  f  q0 , x0 

(1.15)

Sau đó lấy

q0 : q0  q0
Nếu q0   , với

(1.16)

 là một tham số dương bé cho trước, thì ta lại thế biểu thức

(1.16) vào phương trình (1.14) và giải phương trình này. Quá trình lặp sẽ dừng lại
khi q0   . Cuối cùng ta có
q 0 : q 0

(1.17)

b. Hiệu chỉnh gia lượng véc tơ tọa độ suy rộng tại thời điểm tk+1
Giả sử đ biết qk  q  tk  , qk  q  tk  , qk  q  tk  , ta cần phải xác định q  t  tại thời

điểm t  tk  t . Trước hết ta lấy véc tơ gần đúng q k 1 theo công thức

q k 1  q k  q k t 

1
2
q k  t 
2

(1.18)

Sau đó xác định véc tơ chính xác hơn của q k 1 theo công thức

q k 1  q k 1  q k 1

(1.19)

Áp dụng khai triển Taylor hàm f  q, x  tại q k 1  q k 1  q k 1 , xk 1 ta được
f  q k 1 , x k 1   f  q k 1  q k 1 , x k 1 
 f  q k 1 , x k 1  

f
 q k 1 , x k 1  q k 1 
q

0

(1.20)

Từ (1.20) ta suy ra phương trình gần đúng

Φq  qk 1 , xk 1  qk 1  f  qk 1 , xk 1 

(1.21)

Với trường hợp robot chuẩn n=r, giải phương trình đại số tuyến tính (1.21) ta được

qk 1  Φq1  qk 1 , xk 1  f  qk 1 , xk 1 

(1.22)

Sau đó ta lấy
qk 1 : qk 1  qk 1

(1.23)

8


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
Nếu qk 1   thì ta lại thay (1.23) vào phương trình (1.21) và tính q k 1 mới.
Q trình lặp sẽ dừng lại khi qk 1   . Kết quả ta có
q k 1 : q k 1

1.1.2.3.

(1.24)

Cơng thức xác định các véctơ q  t  và q  t  tại thời điểm

tk

Để xác định qk  q  tk  và qk  q  tk   k  0,1, 2,... ta sử dụng các công thức (1.10) và
(1.12)

q  Φq1  q, x  Φx  q, x  x  t 

(1.25)

q  Φq1  q, x  Φq  q, x  q  Φ x  q, x  x  Φ x q, x  x 

(1.26)

1.1.2.4.

Đánh giá sai số

Để đánh giá sai số của phương pháp ta có thể sử dụng các phương trình liên kết

e  tk   f  q k , x k 

(1.27)

Hay : ei  tk   fi  qk , xk 

(1.28)

Độ lớn của fi  qk , xk  với mọi

qk , xk

cho ta biết độ chính xác của phương pháp. Ta

có thể vẽ đồ thị các hàm fi  tk  với  i  1,..., r  để nhận biết độ chính xác của phương
pháp. Yêu cầu của chúng ta là : fi  tk   

 i  1,..., r 

9


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
1.2.

Phương pháp tách cấu trúc trong bài toán động lực học ngược robot
song song.

1.2.1. Ý tưởng của phương pháp tách cấu trúc

Hình 1.2. Robot song phẳng 3RRR
Xét robot song phẳng 3RRR như hình vẽ. Robot có số bậc tự do là k=2. Tọa độ thao
tác được xác định bởi véc tơ x   xP , yP ,   . Trong đó  xP , yP  xác định vị trí của
T

T

điểm thao tác P, còn  xác định hướng khâu thao tác.
Để xác định vị trí các khâu của robot ta sử dụng véc tơ tọa độ khớp là
q  1 , 1 ,2 , 2 ,3 , 3  . Trong đó qa  1 ,2 ,3 

T

T

là tọa độ khớp chủ động và

q p   1 , 2 , 3  là tọa độ khớp bị động.
T

Để thiết lập phương trình vi phân chuyển động của robot, ta sẽ tách robot thành các
cơ cấu con có cấu trúc đơn giản hơn, sau đó ta sẽ lập phương trình vi phân chuyển
động của các cơ cấu con sử dụng phương trình Lagrange loại 2.
Đối với robot song phẳng 3RRR ta sẽ tách robot thành các cơ cấu con như sau

10


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG

Hình 1.2. Chân thứ 1

Hình 1.4. Chân thứ 2

Hình 1.6. Bàn máy động

Hình 1.5. Chân thứ 3

Như vậy sau khi tách robot thành các cơ cấu con, ta sẽ thiết lập phương trình vi
phân chuyển động cho từng cơ cấu con. Đây là bài toán đơn giản hơn rất nhiều bài
toán ban đầu.

1.2.2. Phương trình Lagrange loại 2 của cấu trúc con
1.2.2.1.

Biểu thức động năng và thế năng của robot.

Trong tính tốn động học robot, để xác định vị trí các khâu ta chỉ cần sử dụng hệ
quy chiếu cố định và hệ quy chiếu khớp. Trong bài toán động lực học robot ta cần
thêm một hệ quy chiếu nữa là hệ quy chiếu khâu. Hệ quy chiếu khâu là hệ quy chiếu
gắn với vật rắn, thường có gốc trùng với khối tâm Ci của vật rắn. các trục hướng
theo các trục quán tính chính của vật rắn. Trong hình 1.7, hệ Oixiyizi là hệ quy chiếu
khớp,

Ci iii

là hệ quy chiếu khâu.

11


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG

Hình 1.7. Hệ quy chiếu khâu
Giả sử robot là hệ hơnơnơm có p vật rắn và r liên kết. Khi đó số bậc tự do của hệ là
n=6p-r. Các biến khớp của hệ q   q1

qn  . Vị trí khâu thứ i được xác định bởi
T

Tọa độ khối tâm của khâu rCi  rCi  q, t 
Ma trận côsin chỉ hướng của khâu Ai  Ai  q, t 

Sau đây để đơn giản, ta xét các robot có cấu trúc hơlơnơm giữ và dừng. Khi đó

rCi  rCi  q  ,

Ai  Ai  q 

(1.29)

Theo định nghĩa các ma trận Jacobi tịnh tiến và ma trận Jacobi uay được xác định
bởi công thức
rCi

JTi 

q

J Ri 

,

ωi φi

q q

(1.30)

Trong đó i là véc tơ đại số ứng với góc quay i của vật rắn thứ i, quay quanh trục
quay tức thời. Vận tốc khối tâm và vận tốc góc của vật rắn được xác định bằng các
công thức sau
vCi 

ωi 

drCi
dt



rCi
q

q  JTiq

(1.31)

dφi φi

q  J Ri  q  q
dt
q

(1.32)

c. Động năng robot
Biểu thức động năng của vật rắn được xác định bằng biểu thức
Ti 

1
1
mi vTCi vCi  ωTi Ii ωi
2

2

(1.33)

12


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
Trong đó Ii là ma trận tenxơ uán tính khối của vật rắn đối với hệ quy chiếu cố định
Ii  Ai Iii  ATi

(1.34)


Trong (1.34), I i là ma trận ten xơ uán tính khối của vật rắn đối với hệ quy chiếu
i

gắn liền với khâu.


o đó I i là ma trận hằng số dạng đường chéo, nếu ta chọn hệ
i

quy chiếu khâu là hệ quy chiếu quán tính chính.
Từ (1.33) biểu thức động năng của robot có dạng
p

T   Ti 
i 1

1 p
1 p T
T
m
v
v

ωi I i ωi
 i Ci Ci 2 
2 i 1
i 1

(1.35)

Thay (1.31) và (1.32) vào (1.35) ta được
1 p
1 p
T
T
T   mi  JTi q   J Ti q     J Ri q  I i  J Riq 
2 i 1
2 i 1
p

1 T p
T
 T  q   mi JTi J Ti   J RiT I i J Ri  q
2  i 1
i 1


(1.36)

Nếu ta đưa vào k hiệu
p

p

i 1

i 1

M  q    mi JTiT JTi   J RiT I i J Ri

(1.37)

Thì biểu thức động năng robot (1.36) có dạng
T 

1 T
q M q  q
2

(1.38)

Ma trận M(q) là ma trận vuông cấp n và được gọi là ma trận khối lượng suy rộng
của robot
d. Thế năng trọng lực của robot
Thế năng trọng lực của mỗi khâu của robot được xác định bởi biểu thức

i  mi gT0 rCi

(1.39)

Trong đó
gT0   0 0  g  ,

rCi   xCi

yCi

zCi 

T

(1.40)

 gT0 rCi   gzCi

Thế năng của trọng lực của robot có dạng

13


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
p

   mi gT0 rCi

(1.41)

i 1

1.2.2.2.

Thiết lập dạng thức Lagrange loại 2

Xuất phát từ phương trình Lagrange loại 2
d  T  T


 Qi* , i  1, 2,..., n


dt  qi  qi
qi

(1.42)

Ta suy ra phương trình Lagrange loại 2 dạng ma trận
T

T

T

  
d  T   T 
*




   
   f , i  1, 2,..., n
dt  q   q 
 q 

(1.43)

Trong đó
q   q1 , q2 ,..., qn  , f *  Q1* , Q2* ,..., Qn* 
T

T

(1.44)

Từ biểu thức động năng (1.38) ta có
1
1 n n
T  qT M  q  q   m jk  q  q j qk
2
2 j 1 k 1
T
qi

n

 m q  q
j 1

d  T

dt  qi

ij

(1)

j

n dm q
n
n
n m q
 n
ij  
ij  

m
q
q

q

m
q
q

qk q j





  ij



j
j
ij
j
dt
qk
j 1
j 1
j 1 k 1
 j 1

T 1 n n mij  q 
 
q j qk
qi 2 j 1 k 1 qi

(2)

Từ biểu thức thế năng trọng lực (1.41) ta có
n
rC j


  m j gT0
 gi  q 
qi
qi
j 1

(3)

Thế (1), (2) và (3) vào phương trình (1.43) ta được
n

n

n

 m  q  q  
j 1

ij

j

mij  q 

j 1 k 1

qk

qk q j 

1 n n mjk  q 
q j qk  gi  q    i

2 j 1 k 1 qi

 mij  q  1 mjk  q  
mij  q  q j   

qk q j  gi  q    i , i  1,

qk
2 qi 
j 1
j 1 k 1 
n

n

(1.45)

n

Ta đưa vào k hiệu

14

,n


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG

mij  q 

hijk  q  



qk

1 m jk  q 
1
 mij ,k  q   m jk ,i  q 
2 qi
2

Do M(q) là ma trận đối xứng nên mij ,k  m jk ,i  mki , j . Từ đó suy ra
hijk  q  

1
1
mij ,k  m jk ,i  mkj ,i    mik , j  m ji ,k  mkj ,i 

2
2

(1.46)

Thay (1.46) vào phương trình (1.45) ta nhận được phương trình vi phân chuyển
động của robot
n

n

n

j 1

j 1 k 1

 mij  q  q j   hijk  q  qk q j  gi  q   f *

(1.47)

1.2.3. Dạng ma trận của phương trình Lagrange loại 2
Việc áp dụng các phương trình Lagrange loại hai thiết lập phương trình vi phân
chuyển động của robot khơng gian nói chung là bài tốn khá phức tạp, đặc biệt là
tính tốn ma trận qn tính và Coriolis C. Vì vậy người ta đ thiết lập nhiều phần
mềm tính tốn động lực học của hệ nhiều vật nói chung và của robot nói riêng.
Trong mục này ta sẽ trình bày một thuật tốn tự động hóa việc thiết lập các phương
trình vi phân chuyển động của robot. Với thuật tốn này ta d dàng thiết lập các
phương trình vi phân chuyển động của robot nếu biết sử dụng phần mềm MAPLE.
Như đ biết, động năng của robot có dạng
T

1 T
1
q M  q  q  qT b(q,q)
2
2

(1.48)

Trong đó để thuận tiện cho việc chứng minh ta đưa vào véc tơ

b  q,q  = M  q  q

b  b1...bn 

(1.49)

Phương trình Lagrange II có dạng
T

T

T

d  T   T    
       τ
dt  q   q   q 

(1.50)

Sử dụng công thức đạo hàm theo biến véc tơ của tích hai ma trận, ta có
T 1  T
1  qT
b 

q b  
 b  I n   qT 


q 2 q
2  q
q 

Trong đó
15

(1.51)


ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
qT
 b  I n   e1T
q
 b1

b1I n 
  b eT I
eTn  
  11 n
bn I n 

bneTn I n 

(1.52)

bn   bT   M  q  q   qT M  q 
T

Mặt khác

qT

 M  q 
b

q 
 qT
M  q  q   qT 
q  I n   M q  

q
q
q 
 q
 qT  0  M  q  I n   q T M  q 

(1.53)

Thay (1.52) và (1.53) vào (1.51) ta được
T

 T 
T
 qT M  q   
  M q  q
q
 q 

(1.54)

T

d  T 

  M q  q + M q  q
dt  q 

Tính tốn tương tự ta có
T 1  T
1  qT
b  1 
b 

q
b

 b  I n   qT    0 + qT 



q 2 q
2  q
q  2 
q 
1

1  M  q 
q 
 qT
M  q  q   qT 

q  I n   M q  

2 q
2  q
q 

(1.55)

 1 M  q 
1  M  q 
 qT 
 q  I n   0   qT
q  In 
2  q
q
 2

Thay (1.54) và (1.55) và (1.50) ta được
  Π 
1  M  q 
M  q  q + M  q  q   qT
q  In       τ
2
q
  q 
T

T

(1.56)


1  T M  q 
Đặt v  q,q  = M  q  q   q
q  I n   , ta có
2
q

T


1  M  q 
v  q,q  = M  q  q  
q  I n   q
2  q

T

(1.57)

Theo công thức quan hệ giữa đạo hàm tồn phần và đạo hàm riêng, ta có
M q  

M  q 
In  q  .
q

16

ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG
Thay vào (1.57) ta được
T
 M  q 
 
1  M  q 
v  q,q   
 In  q   
 q  I n    q = C q,q  q
 q
2  q
 


Ma trận ly tâm và Coriolis bây giờ có dạng

M  q 

1  M  q 
C  q,q  
In  q  
q  In  
q
2  q


T

(1.58)

Khi đó phương trình vi phân chuyển động của robot (1.56) có dạng
T

 Π 
M  q  q + C  q,q  q     f *
 q 

(1.59)

Nhờ các phần mềm đa năng như

APLE,

ATHE ATICA,

ATLAB

việc tính tốn ma trận ly tâm và Coriolis theo cơng thức (1.58) tương đối đơn giản
và d dàng.
1.2.4. Phương trình Lagrange dạng nhân tử của hệ nhiều vật
Thiết lập phương trình Lagrange dạng nhân tử cho hệ n chất điểm

1.2.4.1.

Xét hệ n chất điểm được xác định bởi m tọa độ suy rộng dư s1,s2,...,sm. Giả sử hệ
chịu r liên kết hôlônôm

fi (s1 , s2 ,..., sm , t )  0

(i  1,2,..., r )

(1.60)

và p liên kết phi hôlônôm tuyến tính
m

a

s  aj0  0

jk k

( j  1,2,..., p)

(1.61)

k 1

Từ (1.60) và (1.61) ta suy ra các phương trình ràng buộc các biến phân δsk (các di
chuyển ảo)
m

fi

 s  s

k

k 1

0

(i  1, 2,..., r )

(1.62)

k

Xuất phát từ nguyên lý d'Alembert-Lagrange đối với hệ n chất điểm
n

 (F

a
j

 m j a j ). rj  0

(1.64)

j 1

Và thực hiện các biến đổi tương tự như khi thiết lập phương trình Lagrange loại 2 ta
được
17