Thiết kế bộ điều khiển cho mô hình động học tay máy hàn di động 5 bậc tự do

Đại Học Quốc Gia TP. Hồ Chí Minh
TRƯỜNG ĐẠI HỌC BÁCH KHOA
-------------------

NGUYỄN PHƯƠNG BẮC

THIẾT KẾ BỘ ĐIỀU KHIỂN CHO MÔ HÌNH ĐỘNG
HỌC TAY MÁY HÀN DI ĐỘNG 5 BẬC TỰ DO
Chuyên ngành: CÔNG NGHỆ CHẾ TẠO MÁY
Mã ngành:

2.01.00

LUẬN VĂN THẠC SĨ

TP. HỒ CHÍ MINH, tháng 11 năm 2007


CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI
TRƯỜNG ĐẠI HỌC BÁCH KHOA
ĐẠI HỌC QUỐC GIA TP. HỒ CHÍ MINH

Cán bộ hướng dẫn khoa học: TS. Phan Tấn Tùng

Cán bộ chấm phản biện 1: TS. Trần Thiện Phúc

Cán bộ chấm phản biện 2: TS. Bùi Trọng Hiếu

Luận văn thạc sĩ được bảo vệ tại HỘI ĐỒNG CHẤM BẢO VỆ LUẬN
VĂN THẠC SĨ
TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày 12 tháng 1 năm 2008

ĐẠI HỌC QUỐC GIA TP. HCM
TRƯỜNG ĐẠI HỌC BÁCH KHOA

CỘNG HOÀ XÃ HỘI CHỦ NGHIÃ VIỆT NAM
Độc Lập - Tự Do - Hạnh Phúc

----------------

---oOo--Tp. HCM, ngày . . . . . tháng . . . . . năm . . . . .

.

NHIỆM VỤ LUẬN VĂN THẠC SĨ
Họ và tên học viên: NGUYỄN PHƯƠNG BẮC

Giới tính : Nam

Ngày, tháng, năm sinh : 21/11/1977

Nơi sinh : Tây Ninh

Chuyên ngành : Công nghệ Chế Tạo Máy
Khoá (Năm trúng tuyển) : 2005
1- TÊN ĐỀ TÀI:
THIẾT KẾ BỘ ĐIỀU KHIỂN CHO MƠ HÌNH ĐỘNG HỌC TAY MÁY HÀN DI
ĐỘNG 5 BẬC TỰ DO
2- NHIỆM VỤ LUẬN VĂN: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
.........................................................................

.........................................................................
.........................................................................
.........................................................................
.........................................................................
.........................................................................
.........................................................................
.........................................................................
.........................................................................
.........................................................................
.........................................................................
.........................................................................
3- NGÀY GIAO NHIỆM VỤ : 05/02/2007
4- NGÀY HOÀN THÀNH NHIỆM VỤ : 05/11/2007
5- HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: TS. PHAN TẤN TÙNG
Nội dung và đề cương Luận văn thạc sĩ đã được Hội Đồng Chuyên Ngành thông qua.
CÁN BỘ HƯỚNG DẪN
(Họ tên và chữ ký)

CHỦ NHIỆM BỘ MÔN
QUẢN LÝ CHUYÊN NGÀNH
(Họ tên và chữ ký)


i

LỜI CÁM ƠN
Trong thời gian thực hiện luận văn, tôi đã nhận được nhiều sự giúp đỡ và động
viên chân tình của Q Thầy, Cơ và các bạn đồng nghiệp. Hơm nay, khi luận văn đã
hồn thành, tơi xin kính gửi lời biết ơn sâu sắc nhất đến:
Cán bộ hướng dẫn: TS. Phan Tấn Tùng đã nhiệt tình giúp đỡ, chỉ bảo và

hướng dẫn cho tôi trong suốt thời gian thực hiện luận văn.
Lãnh đạo Trung Tâm Cơ Khí trường Đại Học Công Nghiệp TP.HCM đã tạo
điều kiện về mặt thời gian để tơi có thể hồn thành luận văn này.
Thầy Nguyễn Ngọc Điệp, trưởng bộ môn Cơ Điện Tử trường Đại Học Công
Nghiệp TPHCM đã hỗ trợ tôi về vật chất lẫn động viên về mặt tinh thần giúp tơi
vượt qua được những giai đoạn khó khăn trong q trình làm đề tài.
Đặc biệt, xin gửi lời biết ơn sâu sắc nhất đến gia đình, người thân và bạn bè đã
động viên, khích lệ và hỗ trợ tơi trong suốt những năm học và thực hiện luận văn
của mình.
Một lần nữa, xin chúc mọi người nhiều sức khỏe, hạnh phúc và thành đạt.

Ngày 5 tháng 11 năm 2007
Người thực hiện luận văn

Nguyễn Phương Bắc


ii

TĨM TẮT LUẬN VĂN
Luận văn này trình bày việc mơ hình hóa động học và phương pháp thiết kế bộ
điều khiển cho tay máy hàn di động 5 bậc tự do.
Bộ điều khiển sử dụng ở đây dựa trên giải thuật điều khiển phi tuyến và điều
khiển thích nghi nhằm nâng cao đặc tính bám theo quỹ đạo của tay máy hàn. Giải
thuật điều khiển phi tuyến sử dụng trong trường hợp các kích thước của tay máy đã
xác định trước. Giải thuật điều khiển thích nghi sử dụng trong trường hợp các kích
thước của tay máy khơng chắc chắn.
Với việc sử dụng các bộ điều khiển này, tay máy hàn có thể thực hiện việc di
chuyển bám theo quỹ đạo bất kỳ không biết trước ở một tốc độ hàn cố định.
Trong luận văn, tác giả trình bày 2 bộ điều khiển dựa trên các ràng buộc động

học để điều khiển động học tay máy hàn di động 5 bậc tự do. Ngồi ra, tác giả cịn
kết hợp sử dụng luân phiên 2 bộ điều khiển để khắc phục những nhược điểm nếu
chỉ sử dụng 1 bộ điều khiển đơn lẽ trong quá trình hàn.
Hiệu quả của bộ điều khiển được thể hiện thông qua kết quả mô phỏng


iii

ABSTRACT
This thesis presents the methods to model the kinematics and to design the
controller of a 5 DOF mobile manipulator.
The controllers are based the nonlinear and adaptive control algorithms to
enhance the tracking properties of the 5 DOF mobile manipulator. The nonlinear
control algorithm is used when the parameter are known. The adaptive control
algorithm is used when the parameters are uncertain.
By using these controllers, a 5 DOF mobile manipulator can track any
trajectory with the constant welding velocity.
In this thesis, I would like to present 2 controllers based on the kinematic
constraints to control the kinematic model of 5 DOF mobile manipulator. Besides, I
combine and use 2 controllers alternately to overcome the disadvantage when using
a single controller during the welding process.
The effectiveness of the proposed controller is shown by simulation results.


iv

MỤC LỤC
LỜI CÁM ƠN............................................................................................................ i
TÓM TẮT LUẬN VĂN ........................................................................................... ii
MỤC LỤC................................................................................................................ iv

Chương 1: MỞ ĐẦU ..................................................................................................1
1.1
Tên của đề tài và mục đích ............................................................................1
1.2
Nhiệm vụ........................................................................................................1
1.3
Ý nghĩa khoa học và thực tiễn của đề tài .......................................................2

Chương 2: TỔNG QUAN ..........................................................................................3
2.1
Giới thiệu .......................................................................................................3
2.2
Tính cần thiết của đề tài .................................................................................4
2.3
Nghiên cứu tham khảo ...................................................................................4

Chương 3: MƠ HÌNH ĐỘNG HỌC CỦA TAY MÁY 5 BẬC TỰ DO.................8
3.1
Mơ hình động học của khung đế di động.......................................................8
3.2
Mơ hình động học của tay máy L1L2L3 .......................................................12
3.2.1 Động học vị trí tay máy L1L2L3 ...............................................................12
3.2.2 Động học vận tốc tay máy L1L2L3 ...........................................................14
3.3
Mơ hình động học của tay máy hàn di động 5 bậc tự do.............................16
3.3.1 Động học vị trí tay máy di dộng 5 bậc tự do ...........................................16
3.3.2 Động học vận tốc tay máy di động 5 bậc tự do .......................................17

Chương 4: THIẾT KẾ BỘ ĐIỀU KHIỂN PHI TUYẾN VỚI CÁC THAM SỐ

XÁC ĐỊNH ...........................................................................................21
4.1
Định nghĩa sai số..........................................................................................21
4.2
Đo lường sai số ............................................................................................22
4.3
Bậc tự do thừa và các phương án thiết kế bộ điều khiển .............................23
4.4
Thiết kế bộ điều khiển phi tuyến cho hệ thống thứ nhất..............................24


v

4.4.1 Phương trình bộ điều khiển .....................................................................24
4.4.2 Sơ đồ khối bộ điều khiển .........................................................................29
4.5
Thiết kế bộ điều khiển phi tuyến cho hệ thống thứ hai................................30
4.5.1 Phương trình bộ điều khiển .....................................................................30
4.5.2 Sơ đồ khối bộ điều khiển .........................................................................33

Chương 5: THIẾT KẾ BỘ ĐIỀU KHIỂN THÍCH NGHI VỚI CÁC THAM SỐ
CHƯA XÁC ĐỊNH ...............................................................................34
5.1
Thiết kế bộ điều khiển thích nghi cho hệ thống thứ nhất ............................34
5.1.1 Phương trình bộ điều khiển ....................................................................34
5.1.2 Sơ đồ khối bộ điều khiển thích nghi........................................................37
5.2
Thiết kế bộ điều khiển thích nghi cho hệ thống thứ hai ..............................37
5.2.1 Phương trình bộ điều khiển .....................................................................37
5.2.2 Sơ đồ khối bộ điều khiển .........................................................................41

Chương 6: KẾT QUẢ MƠ PHỎNG.......................................................................42
6.1
Mơ phỏng quá trình hàn bằng việc sử dụng bộ điều khiển thứ nhất............42
6.1.1 Trường hợp các tham số kích thước xác định (bộ điều khiển phi tuyến) ....42
6.1.2 Trường hợp các tham số kích thước chưa xác định (bộ điều khiển thích
nghi) .............................................................................................................49
6.2
Mơ phỏng q trình hàn bằng việc sử dụng đồng thời 2 bộ điều khiển ......53
6.2.1 Trường hợp các tham số kích thước xác định (bộ điều khiển phi tuyến) ....54
6.2.2 Trường hợp các tham số kích thước chưa xác định (bộ điều khiển thích
nghi) .............................................................................................................58

Chương 7: KẾT LUẬN ............................................................................................63

PHỤ LỤC .................................................................................................................64

TÀI LIỆU THAM KHẢO ......................................................................................85


1

Chương 1
MỞ ĐẦU
1.1

Tên của đề tài và mục đích

Tên đề tài:
“Xây dựng bộ điều khiển cho mơ hình động học tay máy 5 bậc tự do”.

Mục đích của đề tài là đề ra giải thuật điều khiển (điều khiển phi tuyến và điều
khiển thích nghi) cho mơ hình động học tay máy 5 bậc tự do ứng dụng trong tay
máy hàn di động.

1.2

Nhiệm vụ
+ Mơ hình hóa động học tay máy di động 5 bậc tự do.
+ Giải quyết vấn đề bậc tự do thừa, từ đó đưa ra các phương án thiết kế bộ
điều khiển.
+ Thiết kế bộ điều khiển phi tuyến.
+ Thiết kế bộ điều khiển thích nghi.
+ Mơ phỏng kết quả.

1.3

Phương pháp nghiên cứu

Để hồn thành luận văn, người thực hiện cần phải sử dụng các phương pháp sau:
+ Phương pháp thu thập tài liệu.
+ Phương pháp xử lý tài liệu.
+ Ứng dụng phép tốn để mơ hình hóa tay máy hàn di động.
+ Ứng dụng các nguyên lý điều khiển để xây dựng bộ điều khiển.
+ Phương pháp thực hiện mô phỏng.


2

1.4

Ý nghĩa khoa học và thực tiễn của đề tài

1.4.1 Ý nghĩa khoa học
Đề tài nghiên cứu về việc ứng dụng các giải thuật điều khiển phi tuyến và điều
khiển thích nghi trong lĩnh vực Robot di động là lĩnh vực vẫn cịn ít được quan tâm,
tìm hiểu.
Đề tài hồn thành sẽ là cơ sở cho những nghiên cứu về sau trong lĩnh vực robot di
động và kỹ thuật điều khiển.

1.4.2 Ý nghĩa thực tiễn
Đề tài có tính thực tiễn cao trong các ứng dụng Robot di động đặc biệt là Robot hàn
di động. Với việc sử dụng nhiều bộ điều khiển khác nhau trong quá trình điều khiển
Robot hàn, Robot có thể di chuyển linh hoạt và thực hiện cơng việc chính xác bất
chấp hình dáng quỹ đạo của đường hàn.


3

Chương 2:

TỔNG QUAN
2.1

Giới thiệu

Hiện nay, có 2 loại Robot đang được nghiên cứu và phát triển rộng rãi, đó là Robot
cố định (còn gọi là tay máy) và Robot di động.
Robot cố định được gắn vào đế cố định bằng các bulong nền vì thế nó có thể chịu
lực và momen xoắn tại vị trí đế lớn khi tay máy mang tải. Robot cố định có một
nhược điểm là khả năng với xa của kết cấu bị giới hạn. Để khắc phục nhược điểm

này người ta sử dụng một loại tay máy có thể di động để mới rộng phạm vi hoạt
động của nó, nó được gọi là Robot di động hoặc tay máy di động.
Ngày nay, do sự phát triển của máy tính, các thuật tốn điều khiển thời gian thực có
thể ứng dụng cho Robot di động. Robot di động cũng thể hiện được các tính năng
đáng kể nếu xét về kích cỡ và khơng gian làm việc của nó. Một ứng dụng phổ biến
của Robot di động trong công nghiệp là Robot hàn di động. Trong quá trình hàn tại
các vùng khơng gian hẹp hoặc sâu thì việc sử dụng Robot hàn cố định rõ ràng là
không khả thi do kích thước lớn và phạm vi hoạt động nhỏ. Vì vậy, giải pháp sử
dụng một Robot có khả năng với tới những vị trí xa là hiệu quả nhất – Robot hàn di
động có thể đáp ứng tốt yêu cầu này.
Robot di động được phân loại theo 4 dạng chính:
Robot theo đường dẫn hướng: Để có thể di chuyển tới hoặc lui dọc theo
một đường rãnh dẫn hướng đặt trên mặt đất.
Robot cần trục: Robot được gắn trên một cần trục treo có thể di động theo
2 hướng ngang và 1 hướng thẳng đứng. Loại Robot này có thể mang tải rất lớn và
có thể vận hành tại những vị trí khơng có nhiều diện tích sàn.
Robot bánh xe: Robot gắn trên để có các bánh xe độc lập.


4

Robot kết hợp: Gồm Robot nhỏ đặt trên tay của một Robot lớn còn Robot
lớn gắn cố định với mặt đất. Robot lớn thướng có tay dạng cần cẩu dùng như một
thiết bị định vị thơ. Robot này có phạm vi làm việc rộng, di chuyển chậm, có khả
năng tải lớn. Robot nhỏ có thể di chuyển nhanh và chính xác.
Luận văn này nghiên cứu dựa trên mơ hình Robot bánh xe là 1 trong 4 loại Robot di
động.

2.2

Tính cần thiết của đề tài

Trong những năm gần đây, có nhiều nghiên cứu liên quan đến lĩnh vực robot và tay
máy di động. Các nghiên cứu về robot di động phần lớn thường tập trung vào vấn
đề là làm thế nào để di chuyển robot từ nơi này đến nơi khác trong môi trường cấu
trúc hoặc phi cấu trúc. Với tay máy di động, đó là vấn đề làm thế nào để di chuyển
đầu cuối tay máy từ nơi này sang nơi khác. Mặc dù có nhiều nỗ lực nghiên cứu về
robot trong nhiều năm qua, nhưng việc nghiên cứu robot di động vẫn cịn ít. Vì vậy
với mong muốn đề tài này sẽ đóng góp một phần nhỏ vào những nỗ lực nghiên cứu
tay máy di động, luận văn của tôi sẽ tập trung nghiên cứu vào lĩnh vực tay máy di
động và ứng dụng bộ điều khiển cho tay máy di động này.

2.3

Nghiên cứu tham khảo

Trong nhiều năm qua, nhiều nhà nghiên cứu đã xây dựng và phát triển mơ hình và
giải thuật điều khiển cho Robot di động.
Meng_Pi Cheng và Ching_Chih Tsai [5] đã xây dựng mơ hình động học và động
lực học của tay máy di động với 2 tay gắn trên đế.


5

Hình 2.1: Mơ hình Robot di động có 2 tay máy

De Xu đưa ra một phân tích về bài tốn động học ngược cho Robot di động 5 bậc tự
do không gian
Fukao [6] đã xây dựng giải thuật điều khiển thích nghi cho mơ hình động học và
động lực học của đế di động 2 bánh cố định.

Nhóm nghiên cứu Bùi Trọng Hiếu, Chung Tấn Lâm [7] và [8] đã xây dựng bộ điều
khiển phi tuyến và bộ điều khiển thích nghi dựa trên hàm điều khiển Lyapunov để
cải thiện khả năng dò vết của Robot hàn di động. Ở nghiên cứu này, nhóm xây dựng
trên mơ hình Robot di động 2 bánh xe cố định. Trên đế có gắn thêm thanh trượt 1
bậc tự do.
Ở đây, 2 tham số được xem là tham số chưa biết để xây dựng thuật tốn điều khiển
thích nghi là khoảng cách từ tâm Robot đến bánh xe và bán kính của bánh xe.


6

Hình 2.2: Mơ hình Robot di động 2 bánh xe cố định, có thanh trượt.

Trong luận án tiến sĩ của mình, TS Phan Tấn Tùng [9] đã đạt được nhiều kết quả
quan trọng trong lĩnh vực điều khiển Robot di động. Dựa trên mơ hình Robot di
động 5 bậc tự do, ơng đã xây dựng 2 bộ điều khiển thích nghi độc lập cho hệ thống.
Robot hàn di động 5 bậc tự do bám theo quỹ đạo hàn dựa trên việc điều khiển 2 bộ
điều khiển độc lập này.
Ngoài ra, về lĩnh vực điều khiển thích nghi cịn có J.Craig [10], ơng đã xây dựng
thuật tốn điều khiển thích nghi trên tay máy cố định và thực nghiệm trên mô hình 2
bậc tự do.
Cũng về điều khiển thích nghi, Colbaugh và Seraji [11] đã đưa ra 1 phương pháp
tiếp cận mới để giải quyết bài tốn điều khiển thích nghi. Theo Colbaugh, cách tiếp
cận truyền thống dựa trên mơ hình động lực học T = W (θ , θ,θ) p .
Mơ hình này được sử dụng trong các thuật tốn ước lượng và từ đó hình thành 1
phương pháp thuật tốn ổn định cho điều khiển thích nghi. Phương pháp này gọi là
“Bộ điều khiển thích nghi dựa trên mơ hình”.


7

Phương pháp thứ 2 mà Colbaugh đặt tên là “Bộ điều khiển thích nghi dựa trên tính
năng thể hiện” bởi vì ở phương pháp này, quy tắc thích nghi điều chỉnh giá trị độ lợi
của bộ điều khiển một cách trực tiếp dựa trên sự thể thiện của hệ thống. Mơ hình
động lực học:
T = H 0 (θ )θ + V0 (θ ,θ) + G0 (θ )

Phương pháp này được Colbaugh chứng minh rằng dễ thực hiện hơn do không địi
hỏi nhiều kỹ năng tính tốn.
Thực nghiệm của phương pháp này được thực hiện trên mơ hình của tay máy
PUMA762.


8

Chương 3
MƠ HÌNH ĐỘNG HỌC CỦA TAY MÁY 5 BẬC TỰ DO
3.1

Mơ hình động học của khung đế di động

Xét một tay máy di động như hình 3.1:
Giả sử, khung đế di động có 2 bánh xe đồng trục được dẫn động từ 2 động cơ độc
lập và có các bánh xe tự lựa đỡ ở các góc.

Hình 3.1: Mơ hình tay máy di động 5 bậc tự do
Các thơng số kích thước của đế di động được mơ tả trên hình 3.2:


9

Hình 3.2: Đế di động trên hệ trục tọa độ
Với:
P: là tọa độ của giao điểm giữa trục đối xứng với trục của bánh xe
b: khoảng cách giữa bánh dẫn và trục đối xứng
r: bán kính bánh xe
φ : góc hợp bởi phương của đế di động so với trục

O

X của hệ O.

Các ràng buộc chuyển động của đế di động.
Ràng buộc thứ nhất: Đế di động không thể dịch chuyển theo phương ngang bánh
xe.
Có nghĩa là: vận tốc bánh xe theo phương ngang bằng 0
vP _ ngang = 0 ⇒ y P cos φ − x P sin φ = 0

Với ( xP , yP ) là tọa độ của điểm P trong hệ O.
Ràng buộc thứ hai: Bánh dẫn động bên trái chỉ lăn và không trượt.

(3.1)


10

x P cos φ + y P sin φ − bφ = rθl

(3.2)

Với θl : góc quay của bánh dẫn trái.
Ràng buộc thứ ba: Bánh dẫn động bên phải chỉ lăn và không trượt.
x P cos φ + y P sin φ + bφ = rθr

(3.3)

Với θ r : góc quay của bánh dẫn phải.
Đặt tọa độ Lagrange của đế di động là q.
q = [ xP , y P , φ , θ r , θ l ]

T

(3.4)

Các ràng buộc có thể được viết lại như sau:
A ( q ) .q = 0

(3.5)

Với:
⎡ − sin φ
A ( q ) = ⎢⎢ − cos φ
⎢⎣ − cos φ

cos φ
− sin φ
− sin φ

0 0 0⎤
−b r 0 ⎥⎥

b 0 r ⎥⎦

(3.6)

Trong 5 thành phần của tọa độ q, ta nhận thấy φ phụ thuộc tuyến tính vào các thành
phần cịn lại.
Cụ thể, từ phương trình ràng buộc (3.2) và (3.3), ta suy ra:

r
φ = (θr − θl )
2b

(3.7)

r
(θr − θl )
2b

(3.8)

Suy ra:
φ=

Vì thế, φ có thể được loại bỏ khỏi tọa độ Lagrange.
Ba ràng buộc (3.1), (3.2) và (3.3) có thể được viết lại như sau:
x P sin φ − y P cos φ = 0

(3.9)

(

r
x P cos φ + y P sin φ + bφ = θr + θl
2

)

Hai phương trình này có thể được biểu diễn ở dạng ma trận:
A (q).q = 0

(3.10)


11

Với:
⎡q ⎤ ⎡x ⎤
⎢ 1⎥ ⎢ P ⎥
⎢q ⎥ ⎢ y ⎥
q: vectơ tọa độ Lagrange, q = ⎢ 2 ⎥ = ⎢ P ⎥
⎢ q ⎥ ⎢ θr ⎥
⎢ 3⎥ ⎢ ⎥
⎢q ⎥ ⎢ θ ⎥
⎣ 4⎦ ⎣ l ⎦

(3.11)

Và:
⎡a
a

A(q ) = ⎢ 11 12
⎢ a21 a22
⎣

Gọi:

a13
a23

⎡ -sin φ cos φ
a14 ⎤ ⎢
⎥=⎢
a24 ⎥⎦ ⎢-cos φ -sin φ
⎣⎢

0
r
2

0⎤
⎥
r⎥
⎥
2 ⎦⎥

(3.12)

v P là vận tốc của P

ω là vận tốc góc của đế di động

Từ hình, ta có mối quan hệ:
x P = vP .cos φ
y P = vP .sin φ
φ = ω

(3.13)

Hoặc thể hiện ở dạng ma trận:
⎡ x P ⎤ ⎡cos φ 0⎤
⎢ ⎥ ⎢
⎥ ⎡v ⎤
⎢ y P ⎥ = ⎢ sin φ 0⎥ ⎢ P ⎥
⎢ ⎥ ⎢
⎥⎢ ω ⎥
⎢ φ ⎥ ⎢ 0
⎥⎣ ⎦
1
⎣ ⎦ ⎣
⎦

(3.14)

Thế vào phương trình ràng buộc 2 và 3, ta có:
v p cos 2 φ + v p sin 2 φ + b.ω = r.θr

(3.15)

⇒ v p + b.ω = r.θr
1

b
⇒ θr = v p + ω
r
r
v p cos 2 φ + v p sin 2 φ − b.ω = r.θl

(3.16)

(3.17)

⇒ v p − b.ω = r.θl
1
b
⇒ θr = v p − ω
r
r

Hoặc thể hiện ở dạng ma trận:

(3.18)


12

⎡1
⎢
⎡θr ⎤ ⎢ r
⎢ ⎥=⎢
⎢ θ ⎥ ⎢ 1
⎣ l⎦ ⎢

⎣r

b ⎤
⎥
r ⎥ ⎡⎢ v P ⎤⎥
⎥
−b ⎥ ⎣⎢ ω ⎦⎥
⎥
r ⎦

(3.19)

Suy ra:
⎡r
⎡ v P ⎤ ⎢⎢ 2
⎢ ⎥=⎢
⎣⎢ ω ⎦⎥ ⎢⎢ r
⎣ 2b

r ⎤
⎥
2 ⎥ ⎡⎢θr ⎤⎥
⎥
−r ⎥ ⎣⎢ θl ⎦⎥
⎥
2b ⎦

(3.20)

Hoặc:

⎡ x P ⎤ ⎡cos φ 0⎤
⎡cos φ 0⎤ ⎡⎢ r
⎢ ⎥ ⎢
⎥ ⎡v ⎤ ⎢
⎥
⎢ y P ⎥ = ⎢ sin φ 0⎥ ⎢ P ⎥ = ⎢ sin φ 0⎥ ⎢⎢ 2
⎢ ⎥ ⎢
⎥⎢ω ⎥ ⎢
⎥⎢ r
⎢ φ ⎥ ⎢ 0
⎥⎣ ⎦ ⎢ 0
⎥
1
1
⎣ ⎦ ⎣
⎦
⎣
⎦ ⎢⎣ 2b

r ⎤
⎥
2 ⎥ ⎡⎢θr ⎤⎥
⎥
−r ⎥ ⎢⎣ θl ⎥⎦
⎥
2b ⎦

(3.21)

Suy ra:

⎡r
⎢ cos φ
⎡ x P ⎤ ⎢⎢ 2
⎢ ⎥ ⎢r
⎢ y P ⎥ = ⎢ sin φ
⎢ ⎥ ⎢2
⎢ ⎥
⎣ φ ⎦ ⎢⎢ r
⎢ 2b
⎣

⎤
r
cos φ ⎥
⎥
2
⎥⎡ ⎤
r
⎥θ
sin φ ⎥ ⎢ r ⎥
2
⎥ ⎢⎣ θl ⎥⎦
r ⎥⎥
−
2b ⎥⎦

3.2

Mơ hình động học của tay máy L1L2L3

3.2.1

Động học vị trí tay máy L1L2L3

(3.22)

Xét tay máy 5 bậc tự do như hình 5
Trong đó các hệ tọa độ được chọn như sau:
+ Hệ tọa độ O: là hệ tọa độ mặt đất
+ Hệ tọa độ I: là hệ tọa độ có gốc đặt tại P và luôn luôn tịnh tiến so với hệ
O khi đế di động.

+ Hệ tọa độ M: là hệ tọa độ có gốc đặt tại P và cố định trên đế di động.


13

+ Hệ tọa độ 1: là hệ tọa độ có gốc đặt tại Q và trục 1 X cố định trên khâu
PQ (thường được gọi là khâu 1).
+ Hệ tọa độ 2: là hệ tọa độ có gốc đặt tại S và trục 2 X cố định trên khâu
QS (thường được gọi là khâu 2).
+ Hệ tọa độ 3: là hệ tọa độ có gốc đặt tại E và trục 3Y cố định trên khâu SE
(thường được gọi là khâu 3).

Hình 3.3: Hệ tọa độ làm việc trên tay máy di động

Từ hình 5, ta dễ dàng xác định được vị trí của E so với hệ tọa độ M.
M
⎧
⎪

xE = L1 cos θ1 + L2 cos (θ1 + θ2 ) + L3 cos (θ1 + θ2 + θ3 )
⎪
⎪
⎨
M
⎪
⎪
⎪
⎩ y E = L1 sin θ1 + L2 sin (θ1 + θ2 ) + L3 sin (θ1 + θ2 + θ3 )

Góc quay của hệ 3 so với hệ tọa độ M.

(3.23)


14

M

3.2.1

φE = θ1 + θ2 + θ3 −

π
2

(3.24)

Động học vận tốc tay máy L1L2L3

Ta xét vận tốc của đầu hàn E so với hệ tọa độ M:
M

v E = M v S + M ω2 . M Rot2 . 2 p E + M Rot2 . 2 v E

(3.25)

Với:
M

1

ω2 = ω2 +

M

⎡ 0 ⎤
ω1 = ⎢⎢ 0 ⎥⎥
⎢⎣θ1 + θ2 ⎥⎦

⎡C1 −S1 0 ⎤ ⎡C2
M
Rot2 = M Rot1.1Rot2 = ⎢⎢ S1 C1 0 ⎥⎥ ⎢⎢ S2
⎢⎣ 0
0 1 ⎥⎦ ⎢⎣ 0

(3.26)
− S2
C2
0

0 ⎤ ⎡C12
0 ⎥⎥ = ⎢⎢ S12
1 ⎥⎦ ⎢⎣ 0

− S12
C12
0

0⎤
0 ⎥⎥
1 ⎥⎦

(3.27)
⎡ L3C3 ⎤
2
p E = ⎢⎢ L3S3 ⎥⎥
⎢⎣ 0 ⎥⎦
2

(3.28)

⎡ 0 ⎤ ⎡ L3C3 ⎤ ⎡ − L3S3 ⎤
⎢
v E = ω3 ^ p E = ⎢ 0 ⎥⎥ ^ ⎢⎢ L3S3 ⎥⎥ = ⎢⎢ L3C3 ⎥⎥ θ3
⎢⎣θ3 ⎥⎦ ⎢⎣ 0 ⎥⎦ ⎢⎣ 0 ⎥⎦

M

2

2

⎡C12
Rot2 . v E = ⎢⎢ S12
⎢⎣ 0
2

− S12
C12
0

⎡ − L3S123 ⎤
= ⎢⎢ L3C123 ⎥⎥ θ3
⎢⎣ 0 ⎥⎦

0 ⎤ ⎡ − L3S3 ⎤
⎡ − L3C12 S3
⎥
⎢
⎥
0 ⎥ ⎢ L3C3 ⎥ θ3 = ⎢⎢ − L3S12 S3
⎢⎣
1 ⎥⎦ ⎢⎣ 0 ⎥⎦
0

(3.29)
− L3S12C3
L3C12C3
0

0⎤
0 ⎥⎥ θ3
1 ⎥⎦

(3.30)


15

M

⎡C12
Rot2 . p E = ⎢⎢ S12
⎢⎣ 0
2

− S12
C12
0

0 ⎤ ⎡ L3C3 ⎤ ⎡ L3C12C3 − L3S12 S3 ⎤ ⎡ L3C123 ⎤
0 ⎥⎥ ⎢⎢ L3S3 ⎥⎥ = ⎢⎢ L3S12C3 + L3C12 S3 ⎥⎥ = ⎢⎢ L3S123 ⎥⎥
⎥⎦ ⎢⎣ 0 ⎥⎦
1 ⎥⎦ ⎢⎣ 0 ⎥⎦ ⎢⎣
0

(3.31)
Suy ra:
⎡ 0 ⎤ ⎡ L3C123 ⎤ ⎡ − L3S123 ⎤

Rot2 . p E = ⎢⎢ 0 ⎥⎥ ^ ⎢⎢ L3S123 ⎥⎥ = ⎢⎢ L3C123 ⎥⎥ θ1 + θ2
 
⎣⎢θ1 + θ 2 ⎦⎥ ⎣⎢ 0 ⎦⎥ ⎣⎢ 0 ⎦⎥

(

2

M

ω2 ^

M

M

vS =

M

vQ =

M

vQ +

M

ω1. M Rot1.1p S +

M

)

Rot1.1 vQ

(3.32)

(3.33)

Với:
M

ω1 ^

M

⎡ 0 ⎤ ⎡ L1C1 ⎤ ⎡ − L1S1 ⎤
pQ = ⎢⎢ 0 ⎥⎥ ^ ⎢⎢ L1S1 ⎥⎥ = ⎢⎢ L1C1 ⎥⎥ θ1
⎢⎣θ1 ⎥⎦ ⎢⎣ 0 ⎥⎦ ⎢⎣ 0 ⎥⎦

⎡ 0 ⎤ ⎡ L2C2 ⎤ ⎡ − L2 S2 ⎤
1
v S = 1ω1 ^ 1p S = ⎢⎢ 0 ⎥⎥ ^ ⎢⎢ L2 S2 ⎥⎥ = ⎢⎢ L2C2 ⎥⎥ θ2
⎢⎣θ2 ⎥⎦ ⎢⎣ 0 ⎥⎦ ⎢⎣ 0 ⎥⎦
M

M

⎡C1 − S1 0 ⎤ ⎡ − L2 S2 ⎤

⎡ − L2C1S2 − L2 S1C2 ⎤
⎢
⎥
⎢
⎥
Rot1. v S = ⎢ S1 C1 0 ⎥ ⎢ L2C2 ⎥ θ2 = ⎢⎢ − L2 S1S2 + L2C1C2 ⎥⎥ θ2
⎢⎣ 0
⎢⎣
⎥⎦
0 1 ⎥⎦ ⎢⎣ 0 ⎥⎦
0
⎡ − L2 S12 ⎤
= ⎢⎢ L2C12 ⎥⎥ θ2
⎢⎣ 0 ⎥⎦

(3.35)

1

(3.36)

⎡C1 − S1 0 ⎤ ⎡ L2C2 ⎤ ⎡ L2C1C2 − L2 S1S2 ⎤ ⎡ L2C12 ⎤
Rot1. p S = ⎢⎢ S1 C1 0 ⎥⎥ ⎢⎢ L2 S2 ⎥⎥ = ⎢⎢ L2 S1C2 + L2C1S2 ⎥⎥ = ⎢⎢ L2 S12 ⎥⎥ (3.37)
⎢⎣ 0
⎥⎦ ⎢⎣ 0 ⎥⎦
0 1 ⎥⎦ ⎢⎣ 0 ⎥⎦ ⎢⎣
0
1

⎡ 0 ⎤ ⎡ L2C12 ⎤ ⎡ − L2 S12 ⎤

M
ω1 ^ M Rot1.1p S = ⎢⎢ 0 ⎥⎥ ^ ⎢⎢ L2 S12 ⎥⎥ = ⎢⎢ L2C12 ⎥⎥ θ1
⎢⎣θ1 ⎥⎦ ⎢⎣ 0 ⎥⎦ ⎢⎣ 0 ⎥⎦

Suy ra:

(3.34)

(3.38)


16

⎧⎪ M x = − L S θ − L S θ − L S θ
S
1 1 1
2 12 2
2 12 1
⎨M
⎪⎩ y S = L1C1θ1 + L2C12θ2 − L2C12θ1

(3.39)

Suy ra:
M

⎡ M x ⎤ ⎡ − L S − L S
S
⎥ = ⎢ 1 1 2 12
vS = ⎢

⎢⎣ M y S ⎥⎦ ⎣ L1C1 + L2C12

− L2 S12 ⎤ ⎡θ1 ⎤
⎢ ⎥
L2C12 ⎦⎥ ⎣θ2 ⎦

(3.40)

Thế M v S vào (3.25), ta được:

(

)

M
xE = − L1S1θ1 − L2 S12θ2 − L2 S12θ1 − L3S123 θ1 + θ2 − L3S123θ3

= − ( L1S1 + L2 S12 + L3S123 )θ1 − ( L2 S12 + L3S123 )θ2 − L3S123θ3

(

)

M 
y E = L1C1θ1 + L2C12θ2 + L2C12θ1 + L3C123 θ1 + θ2 + L3C123θ3

= ( L1C1 + L2C12 + L3C123 )θ1 + ( L2C12 + L3C123 )θ2 + L3C123θ3

(3.41a)

(3.41b)

Suy ra:
⎡ θ1 ⎤
⎢ ⎥
⎡
⎤
L
S
L
S
L
S
L
S
L
S
L
S
−
+
+
−
+
−
( 1 1 2 12 3 123 ) ( 2 12 3 123 )
3 123 ⎥ ⎢  ⎥
M
v E = ⎢⎢
⎥ ⎢θ2 ⎥

L
C
L
C
L
C
L
C
L
C
L
C
+
+
+
(
)
(
)
1
1
2
12
3
123
2
12
3
123
3

1233
⎣
⎦ ⎢ θ ⎥
⎣⎢ 3 ⎦⎥

(3.42)
Vận tốc góc của khâu 3 trên hệ tọa độ M:
M

ω E = M φE = θ1 + θ2 + θ3

3.3

Mơ hình động học của tay máy hàn di động 5 bậc tự do

3.3.1

Động học vị trí tay máy di động 5 bậc tự do

(3.43)

Từ hình 5, ta dễ dàng xác định được vị trí của E so với hệ tọa độ O.
⎧⎪ xE = xP + L1 cos (φ + θ1 ) + L2 cos (φ + θ1 + θ2 ) + L3 cos (φ + θ1 + θ2 + θ3 )
⎪
(3.44)
⎨
⎪⎪ yE = yP + L1 sin (φ + θ1 ) + L2 sin (φ + θ1 + θ2 ) + L3 sin (φ + θ1 + θ2 + θ3 )
⎩

Góc quay của hệ 3 so với hệ tọa độ O.

17

φE = φ + θ1 + θ2 + θ3 −

3.3.2

π
2

(3.45)

Động học vận tốc tay máy di động 5 bậc tự do

Vận tốc của đầu hàm E so với hệ I:
I

v E = I v P + I ω M ^ I RotM . M p E + I RotM . M v E

(3.46)

I

vP = 0

(3.47)

⎡ 0⎤
⎢ ⎥

I
ωM = ⎢ 0⎥
⎢ ⎥
⎢φ ⎥
⎣ ⎦

(3.48)

Với:

M

pE =

M

⎡C12
Rot2 . p E = ⎢⎢ S12
⎢⎣ 0

⎡cos φ
I
RotM = ⎢⎢ sin φ
⎢⎣ 0

2

− S12
C12
0

0 ⎤ ⎡ L3C3 ⎤ ⎡ L3C123 ⎤
0 ⎥⎥ ⎢⎢ L3S3 ⎥⎥ = ⎢⎢ L3S123 ⎥⎥
1 ⎥⎦ ⎢⎣ 0 ⎥⎦ ⎢⎣ 0 ⎥⎦

− sin φ 0 ⎤
cos φ 0 ⎥⎥
0
1 ⎥⎦

⎡cos φ
I
M
⇒ Rot M . p E = ⎢⎢ sin φ
⎢⎣ 0

(3.49)

(3.50)

− sin φ
cos φ
0

0 ⎤ ⎡ L3C123 ⎤ ⎡ L3Cφ123 ⎤
⎢
⎥
0 ⎥⎥ ⎢⎢ L3S123 ⎥⎥ = ⎢ L3Sφ123 ⎥
1 ⎥⎦ ⎢⎣ 0 ⎥⎦ ⎢⎣ 0 ⎥⎦

(3.51)

Suy ra:
⎡ 0 ⎤ ⎡ L3Cφ123 ⎤ ⎡−L3Sφ123 ⎤
⎥ ⎢
⎥
⎢
⎥ ⎢
I
ω M ^ I RotM . M p E = ⎢ 0 ⎥ ^ ⎢⎢ L3Sφ123 ⎥⎥ = ⎢⎢ L3Cφ123 ⎥⎥ φ
⎢ ⎥
⎥
⎢φ ⎥ ⎢ 0 ⎥ ⎢
0
⎥⎦ ⎢⎣
⎥⎦
⎣ ⎦ ⎢⎣

⎡cos φ
I
M
RotM . v E = ⎢⎢ sin φ
⎢⎣ 0

− sin φ
cos φ
0

(3.52)

M
⎡M ⎤ ⎡M
⎤
0 ⎤ ⎢ x E ⎥ ⎢ x E cos φ − y E sin φ ⎥
0 ⎥⎥ ⎢ M y E ⎥ = ⎢ M x E sin φ + M y E cos φ ⎥
⎢
⎥ ⎢
⎥
1 ⎥⎦ ⎢ 0 ⎥ ⎢
0
⎥
⎣
⎦ ⎣
⎦