Điều khiển robot scara bằng phương pháp điều khiển trượt

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP. HCM


NGUYỄN THÀNH NHẪN


ĐIỀU KHIỂN ROBOT SCARA
BẰNG PHƯƠNG PHÁP ĐIỀU KHIỂN TRƯỢT




LUẬN VĂN THẠC SĨ

Chuyên ngành : CƠ ĐIỆN TỬ
Mã số ngành: 60 52 01 14

TP. HỒ CHÍ MINH, tháng 05 năm 2014


NGUYỄN THÀNH NHẪN LUẬN VĂN THẠC SĨ KHÓA 2011
Mẫu nhãn đĩa CD-ROM:
Học viên: Nguyễn Thành Nhẫn
MSHV: 1241840009
Ngành: Cơ Điện Tử
Mã ngành:
60 52 01 14
khóa 2011
Tên đề tài: ĐIỀU KHIỂN ROBOT SCARA BẰNG PHƯƠNG PHÁP ĐIỀU KHIỂN

TRƯỢT




1


Chương 1
TỔNG QUAN
1.1 Tổng quan
Trong quá trình công nghiệp hóa và hiện đại hóa của nước ta, việc nghiên
cứu thiết kế và chế tạo các robot công nghiệp để ứng dụng vào sản xuất có một ý
nghĩa rất quan trọng, đặc biệt là trong giai đoạn hội nhập kinh tế như hiện nay. Việc
tự động hoá quá trình sản xuất với sự có mặt của các robot sẽ làm tăng khả năng
mềm dẻo của hệ thống sản xuất, tăng chất lượng của sản phẩm và đặc biệt là có thể
làm giảm giá thành sản phẩm để tăng tính cạnh tranh. Ngoài ra Robot công nghiệp
còn có một tính năng quan trọng khác là nó có thể làm việc trong những môi trường
khắc nghiệt mà con người không thể tham gia vào được như: môi trường nhiều khói
bụi, môi trường độc hại của hoá chất, môi trường nhiệt độ cao
Hiện nay trên thế giới đã có rất nhiều nghiên cứu về robot và cũng đã có rất
nhiều robot được chế tạo và ứng dụng vào quá trình sản xuất như các robot hàn
trong nhà máy sản xuất ô tô, các robot lắp ráp linh kiện trong dây chuyền sản xuất
board mạch, Robot lắp máy, Robot đào đường hầm, robot cấp phôi trong các máy
gia công chi tiết cơ khí, Robot quay camera trong các sân vận động Tuy nhiên, ở
Việt Nam thì việc nghiên cứu và chế tạo robot mới ở giai đoạn bắt đầu, chủ yếu
dừng lại ở mức độ chế thử, chỉ một số ít được chuyển giao vào quá trình sản xuất.
Các robot này chưa có tính thích ứng với môi trường thay đổi mà chủ yếu hoạt động
theo một chương trình định trước.
2

Hình 1.1: SCARA robot của hang EPSON.
Việc nghiên cứu các bộ điều khiển để nâng cao độ chính xác của robot hiện
vẫn còn đang được các nhà khoa học trong và ngoài nước quan tâm rất nhiều.
1.2 Cơ sở chọn đề tài
Tay máy là một đối tượng có độ phi tuyến rất cao do đó việc thiết kế bộ điều
khiển cho đối tượng này là khá phức tạp. Như đã biết, điều khiển với cấu trúc thay
đổi (sliding mode control) là phương pháp hữu hiệu để điều khiển cho các đối
tượng phi tuyến bởi luật điều khiển hồi tiếp phi tuyến [1] – [4]. Điều khiển với cấu
trúc thay đổi là một kỹ thuật điều khiển rất mạnh, và bền vững. Trong những năm
gần đây, hệ thống điều khiển với cấu trúc thay đổi đã được ứng dụng rộng rãi để ổn
định hoá cho chuyển động của robot. Có rất nhiều nghiên cứu về bộ điều khiển có
cấu trúc thay đổi, có thể kể đến như: Bộ điều khiển trượt trong hệ liên tục trình bày
trong [8,9], bộ điều khiển trượt được đưa ra trong [10] để điều khiển cho tay máy,
…
Với mục tiêu đưa ra một phương pháp điều khiển khã thi, ổn định, có khả
năng ứng dụng cao vào sản xuất công nghiệp, luận án này giới thiệu một bộ điều
khiển PID có cấu trúc thay đổi kết hợp giữa bộ điều khiển có cấu trúc thay đổi và
mặt trượt PID để điều khiển cho góc quay của tay máy SCARA bám theo góc đặt.
hàm trượt có dạng phương trình của bộ điều khiển PID. Và hàm trượt này được gọi
là hàm trượt kiểu PID. Giải thuật này loại bỏ được hiện tượng dao động khi biên độ
của luật điều khiển trượt tăng. Và giải thuật được áp dụng để điều khiển đối tượng
3


phi tuyến-hệ tay máy SCARA 02 bậc tự do. Điều kiện tồn tại của mặt trượt và tính
ổn định tiệm cận toàn cục của hệ thống được thiết lập dưới dạng toàn phương của
hàm Lyapunov. Tính khả thi của bộ điều khiển được kiểm chứng thông qua kết quả

mô phỏng trên phần mềm Matlab và mô hình thực nghiệm.
Luận án được chia làm 07 chương với nội dung như sau:
+ Chương 1: Tổng quan
Nội dung của chương này trình bày lý do chọn đề tài và tổng quan về hệ thống tay
máy và ứng dụng của nó trong công nghiệp đồng thời giới thiệu sơ lược về bộ điều
khiển được thiết kế trong luận án.
+ Chương 2: Cơ sở lý thuyết
Nội dung chương này trình bày tổng quát cơ sở lý thuyết Robot, hệ thống và
phương thức điều khiển 01 Robot.
+ Chương 3: Mô hình toán của tay máy SCARA
Nội dung chương này giới thiệu về mô hình động học thuận, ngược và mô hình
động lực học của tay máy.
+ Chương 4: Thiết kế bộ điều khiển
Nội dung chương này trình bày về bộ điều khiển PID trượt kết hợp với mặt trượt
PID, điều kiện tồn tại mặt trượt và điều kiện ổn định của hệ thống.
+ Chương 5: Kết quả mô phỏng
Nội dung chương này trình bày kết quả mô phỏng hệ thống tay máy SCARA bằng
phần mềm Matlap với bộ điều khiển trượt PID được thiết kế trong luận án.
+ Chương 6: Xây dựng mô hình thực nghiệm
Nội dung chương này xây dựng mô hình thực nghiệm tay máy Scara với bộ điều
khiển PID trượt.
+ Chương 7: Kết luận và hướng phát triển của đề tài.
4



Chương 2
CƠ SỞ LÝ THUYẾT
2.1. Cấu trúc tổng quan của một Robot :
Các Robot công nghiệp ngày nay thường được cấu thành bởi các hệ thống sau:

Tay máy là cơ cấu cơ khí gồm các khâu, khớp hình thành cánh tay để tạo ra các
chuyển động cơ bản, gồm:
. Bệ (thân) - Base
. Khớp - thanh nối: joint- link
. Cổ tay – wrist: tạo nên sự khéo léo, linh hoạt.
. Bàn tay - hand, end effector: trực tiếp hoàn thành các thao tác trên đối
tượng.
Cơ cấu chấp hành tạo chuyển động cho các khâu của tay máy. Nguồn động lực của
cơ cấu chấp hành là động cơ.
Hệ thống cảm biến gồm các sensor và các thiết bị chuyển đổi tín hiệu cần thiết
khác. Các Rôbốt cần hệ thống sensor trong để nhận biết trạng thái của bản thân các
cơ cấu của Rôbốt.
Hệ thống điều khiển hiện nay thường là máy tính để giám sát và điều khiển hoạt
động của Rôbốt, có thể chia ra thành 2 hệ thống:
. Hệ thống điều khiển vị trí (quỹ đạo) .
. Hệ thống điều khiển lực.
Cấu trúc vật lý cơ bản của một robot bao gồm thân, cánh tay và cổ tay. Thân được
nối với đế và tổ hợp cánh tay thì được nối với thân. Cuối cánh tay là cổ tay được
chuyển động tự do.
Về mặt cơ khí, Rôbốt có đặc điểm chung về kết cấu gồm nhiều khâu, được nối với
nhau bằng các khớp để hình thành một chuỗi động học hở, tính từ thân đến phần
công tác. Tuỳ theo số lượng và cách bố trí các khớp mà có thể tạo ra tay máy kiểu
toạ độ Đề các, toạ độ trị, tọa độ cầu…
Trong robot thì thân và cánh tay có tác dụng định vị trí còn cổ tay có tác dụng định
hướng cho end effector. Cổ tay gồm nhiều phần tử giúp cho nó có thể linh động
xoay theo các hướng khác nhau và cho phép Rôbốt định vị đa dạng các vị trí. Quan
hệ chuyển động giữa các phần tử khác nhau của tay máy như: cổ tay, cánh tay được
thực hiện qua một chuỗi các khớp nối. Các chuyển động bao gồm chuyển động
quay, chuyển động tịnh tiến…
Sự chuyển động của Robot bao gồm chuyển động của thân và cánh tay, chuyển

động của cổ tay. Những khớp kết nối chuyển động theo 2 dạng trên gọi là bậc tự do.
Ngày nay robot được trang bị từ 4 đến 6 bậc tự do.
5



Dựa vào hình dáng vật lý hoặc khoảng không gian mà cổ tay có thể di chuyển tới
mà người ta chia robot thành bốn hình dạng cơ bản sau :

Robot cực (H 1.1.a) .

Robot Decac (H 1.1.b) .

Robot trụ (H 1.1.c).

Robot tay khớp (H 1.1.d) .

a
b
c
d

Hình 2.1: Phân loại robot cơ bản .
Các khớp được sử dụng trong robot là khớp L, R, T, V (khớp tuyến tính, khớp quay,
khớp cổ tay quay và khớp vuông). Cổ tay có thể có đến 3 bậc tự do.

Bảng 2.1: Các dạng cơ bản của các khớp Robot

Input link
Output link

Input link
Output link


Output link
Input link



Input link



Output link

Loại

Tên

Minh họa

Tuyến tính

Quay

Cổ tay quay

Vuông

V

T

R

L



6



Các khớp có thể chuyển động được chính là nhờ được cung cấp năng lượng bởi các
thiết bị truyền động. Các robot hiện nay thường dùng một trong ba phương pháp
truyền động sau đây :

Truyền động thuỷ lực .

Truyền động khí nén.

Truyền động điện .
Không gian làm việc của một robot phụ thuộc vào hình dạng và kết cấu cơ khí của
tay máy robot. Robot có 3 hình dạng cơ bản của không gian làm việc là dạng cầu,
dạng trụ và dạng khối hộp (lập phương hoặc chữ nhật) Cartesian. Hình dưới mô tả
hình dạng của không gian làm việc của robot :

Trong đó :











(a)

(b)

(c)
Hình 2.2: Không gian làm việc của Robot


Ghi dữ
liệu
Động học
thuận
Động học
ngược

Mặt phẳng
quỹ đạo
Bộ
điều khiển

Nguồn động lực

Vị trí
vật lý
Máy tính



Chạy

Khóa
chuyển mạch

Servo

Sai số vị trí
C
:
D

B
A

Hình 2.3: Sơ đồ khối của Robot
Cơ cấu
chấp hành

Lưu giữ kết
quả
Chế độ dạy
học
7



 Khối A: là khối thu thập và chuyển giao dữ liệu đầu vào.
 Khối B: là khối não bộ của robot gồm các cụm vi xử lý, giải quyết các vấn
đề về:
- Thiết lập và giải các bài toán động học trên cơ sở bộ thông tin đầu
vào (
s
, h

s
). (cụm Động học thuận).
- Lưu trữ và chuyển giao các kết quả của quá trình giải bài toán động
học thuận. (Cụm Cartesian Point Storage).
- Lập trình quỹ đạo đi qua các điểm hình học để hoàn thành toàn bộ quỹ
đạo chuyển động cần có. (cụm Mặt phẳng quỹ đạo).
- Giải các bài toán động học ngược để tìm ra các thông số điều khiển
(còn gọi là bộ dữ liệu điều khiển) - (Cụm Động học ngược).
 Khối C: là khối điều khiển.
 Khối D: là khối cơ cấu chấp hành. Nó bao gồm nguồn động lực (Motor
Dynamics), các cơ cấu chấp hành (Robot Dynamics) và các bộ cảm nhận
vật lý trên chúng (Cụm vị trí vật lý).
2.2. CÁC HỆ THỐNG ĐIỀU KHIỂN ROBOT:
Hệ thống điều khiển của robot có nhiệm vụ điều khiển hệ truyền động điện để
thực hiện điều chỉnh chuyển động của robot theo yêu cầu của quá trình công nghệ. Hệ
thống Điều khiển Robot có thể chia ra:
- Điều khiển vị trí (quỹ đạo) - điều khiển thô.
- Điều khiển lực - điều khiển tinh.
Tùy theo khả năng thực hiện các chuyển động theo từng bậc tự do mà phân ra
các hệ thống điều khiển dưới đây:
- Điều khiển chu tuyến: chuyển động được thực hiện theo một đường liên tục.
- Điều khiển vị trí: đảm bảo cho robot dịch chuyển bám theo một quỹ đạo đặt
trước.
Sơ đồ khối mô tả:

Điều khiển chu kỳ: chuyển động được xác định bằng các vị trí đầu và cuối của mỗi
bậc tự do.
2.3. CÁC PHƯƠNG THỨC ĐIỀU KHIỂN:
Điều khiển theo quỹ đạo đặt (3 phương thức điều khiển):
 Điều khiển Robot theo chuỗi các điểm giới hạn.
Bộ điều
khiển

Robot
Phản hồi
Quỹ đạo đặt
Tín hiệu
điều khiển
Quỹ đạo thực
Hình 2.4:

Sơ đồ khối điều khiển vị trí Robot

8



 Điều khiển lặp lại ( playback ):
 Điều khiển Robot theo kiểu điểm - điểm (PTP).
 Robot điều khiển theo quỹ đạo liên tục PCC.
 Điều khiển ứng dụng kỹ thuật cao (Robot thông minh).
Điều khiển dựa vào tính chất của đối tượng điều khiển là tuyến tính hay phi tuyến.
2.3.1. Điều khiển theo quỹ đạo đặt:
2.3.1.1. Điều khiển theo chuỗi các điểm giới hạn:
Là phương thức điều khiển bằng cách thiết lập các công tắc giới hạn và các

điểm dừng cơ khí. Chuyển động của các khớp nối được bắt đầu và kết thúc khi gặp
các công tắc giới hạn hoặc các điểm dừng cơ khí này.
Việc thiết lập các điểm dừng và các công tắc giới hạn tương đương với việc
lập chương trình cho robot, mỗi một công tắc được coi như một phần tử nhớ.
Phương pháp điều khiển này thường được dùng trong các loại robot đơn giản.
2.3.1.2. Điều khiển lặp lại (playback):
Thường được dùng trong các hệ thống điều khiển phức tạp và quỹ đạo
chuyển động của robot là theo một quỹ đạo đã được tính toán và xác định từ trước
thông qua một chuỗi các vị trí xác định. Các vị trí này đã được ghi vào bộ nhớ của
robot và robot phải tự tính toán điều khiển để đạt tới các vị trí mong muốn này theo
các điều kiện tối ưu có thể. Robot điều khiển playback được chia làm hai loại tùy
theo phương thức điều khiển: Điều khiển kiểu điểm- điểm. (PTP), Phương pháp
điều khiển quỹ đạo liên tục (PCC - Path Continuos Control).
2.3.1.3. Điều khiển kiểu robot thông minh:
Ứng dụng để điều khiển cho những robot ngoài việc có thể thực hiện được
chương trình đặt trước, nó còn có khả năng tùy biến thực hiện các hành động phù
hợp với các cảm nhận từ môi trường. Robot thông minh có thể thay đổi chương
trình phù hợp với điều kiện làm việc của môi trường nhận được từ các sensor
(quang, nhiệt, vị trí, tốc độ, từ trường, âm thanh, tần số…) sử dụng logic mờ và
mạng nơron. Robot loại này có khả năng giao tiếp với con người hoặc với hệ thống
máy tính chung để có thể đưa ra các xử lý thông minh. Hiện nay trên thế giới đã
xuất hiện các robot thông minh có thể hiểu được các lệnh đơn giản của con người,
có thể giao tiếp, giúp đỡ để thực hiện các công việc phức tạp trong nhà máy.
2.3.2. Các hệ thống điều khiển hệ tuyến tính:
Khi khảo sát đặc tính động học của một đối tượng điều khiển hay một hệ
thống, thông thường các đối tượng khảo sát được xem là tuyến tính, dẫn đến cho
phép mô tả hệ thống bằng một hệ phương trình vi phân tuyến tính. Sử dụng nguyên
lý xếp chồng của hệ tuyến tính, ta còn có thể dễ dàng tách riêng các thành phần đặc
trưng cho từng chế độ làm việc để nghiên cứu với những công cụ toán học chặt chẽ,
chính xác mà lại rất đơn giản, hiệu quả.

9



2.3.3. Các hệ thống điều khiển hệ phi tuyến:
Trong thực tế phần lớn các đối tượng được điều khiển lại mang tính động
học phi tuyến (đặc biệt là robot là đối tượng có tính phi tuyến mạnh - có thể thấy rất
rõ ngay trong hệ thống điều khiển Robot luận văn này sẽ trình bày), tức là không
thoả mãn nguyên lý xếp chồng; và không phải đối tượng nào, hệ thống nào cũng có
thể mô tả được bằng một mô hình tuyến tính, cũng như không phải lúc nào những
giả thiết cho phép xấp xỉ hệ thống bằng mô hình tuyến tính được thoả mãn. Hơn thế
nữa độ tối ưu tác động nhanh chỉ có thể tổng hợp được nếu ta sử dụng bộ điều khiển
phi tuyến. Các hạn chế này bắt buộc người ta phải trực tiếp nghiên cứu tính toán
động học của đối tượng, tổng hợp hệ thống bằng những công cụ toán học phi tuyến.
Để nghiên cứu và nâng cao chất lượng hệ thống điều khiển quỹ đạo Robot,
chương này sẽ tổng hợp và nêu lên một số phương pháp điều khiển các hệ phi tuyến
như đã được trình bày chi tiết trong [8], [10] và [11] và ứng dụng có hiệu quả vào
hệ thống điều khiển Robot. Sau đây là một số phương pháp điều khiển ổn định hệ
thống phi tuyến:
 Phương pháp tuyến tính hoá trong lân cận điểm làm việc
 Điều khiển tuyến tính hình thức
 Điều khiển bù phi truyến
2.4. PHƯƠNG PHÁP ĐIỀU KHIỂN ROBOT:
Cho đến nay trong thực tế, nhiều phương pháp và hệ thống điều khiển Robot
đã được thiết kế và sử dụng, trong đó các phương pháp điều khiển chủ yếu là:
- Điều khiển động lực học ngược.
- Điều khiển phản hồi phân ly phi tuyến.
- Các hệ thống điều khiển thích nghi.
+ Điều khiển thích nghi theo sai lệch.
+ Điều khiển thích nghi theo mô hình mẫu (MRAC)

+ Điều khiển động lực học ngược thích nghi.
+ Điều khiển trượt
2.4.1. Điều khiển trượt:
Điều khiển trượt là phương pháp điều khiển tiếp cận rất mạnh mẽ để điều khiển
các hệ thống phi tuyến và bất định. Đó là phương pháp điều khiển bền vững và có
thể áp dụng cho hệ bất định và có tham số bị thay đổi lớn. Với tay máy robot khi
các tham số của hệ thay đổi liên tục; lực, vị trí, mô men, mô men quán tính… và
các tác động qua lại làm cho mô hình robot trở nên phi tuyến mạnh thì phương pháp
điều khiển trượt tỏ ra có ưu thế vượt trội, cho việc điều khiển chuyển động bền
vững và bám theo quỹ đạo đặt của robot.
Điều khiển chuyển động bất biến với nhiễu loạn và sự thay đổi thông số có thể
sử dụng điều khiển ở chế độ trượt. Điều khiển kiểu trượt thuộc về lớp các hệ thống
có cấu trúc thay đổi (Variable Structure System - VSS) với mạch vòng hồi tiếp
10



không liên tục. Phương pháp điều khiển kiểu trượt có đặc điểm là tính bền vững rất
cao do vậy việc thiết kế bộ điều khiển có thể được thực hiện mà không cần biết
chính xác tất cả các thông số. Chỉ một số các thông số cơ bản hoặc miền giới hạn
của chúng là đủ cho việc thiết kế một bộ điều khiển trượt (Variable Structure
Controller - VSC).
2.4.2. Thiết kế một bộ điều khiển kiểu trượt:
Gồm có hai bước:
Thứ nhất: là phải chọn một mặt trượt, trong đó sai lệch e giữa tín hiệu đặt và tín
hiệu ra có duy nhất một giá trị 0.
Thứ hai: chọn luật điều khiển tín hiệu vào sao cho hệ thống điều khiển kín luôn
được duy trì trên mặt trượt.
Ta xét một đối tượng SISO một tín hiệu vào u(t), một tín hiệu ra y(t) mô tả bởi:

uyyfy

),(

(2.1)
và được viết đưới dạng mô hình trạng thái kiểu:





uxfx
xx
)(
2
21


(2.2)
trong đó y = x
1
và
max
uu 
. Giả sử đối tượng bị một nhiễu tức thời tác động đưa nó
ra khỏi điểm làm việc
0
0x
0x
x

20
10
0












. Bài toán đặt ra là tìm tín hiệu điều khiển
u(t) để đưa được đối tượng trở về điểm làm việc.
Đặt: e(t) = x
10
- x
1
(t) (2.3)
và xét hàm chuyển đổi, ký hiệu bằng s(e), có dạng như sau:
s(e) = e + 
e

,  > 0 (2.4)
Ta thấy, khi x
1
(t) = x

10
thì với e(t) = x
10
- x
1
(t) = 0 cũng sẽ có s(e) = 0, ngược lại
phương trình vi phân s(e) = 0 với điều kiện ban đầu e(0) = 0 chỉ có một nghiệm e(t)
= 0 duy nhất, do đó cũng có x
20
= 0.
Bởi vậy bài toán trên trở thành bài toán tìm u(x
1
, x
2
) để đưa được s(e) về giá trị 0,
hay u(x
1
, x
2
) phải được chọn sao cho khi s(e) > 0 thì có
0)e(s 

để s(e) có chiều
hướng giảm và ngược lại khi s(e) < 0 thì
0)e(s 

để s(e) có chiều hướng tăng, tức
là phải thoả mãn:
0)e(s).e(s



(2.5)
Ngoài ra, để quá trình về 0 được nhanh, giá trị modul
)e(s

đặc trưng cho tốc độ
tăng, giảm giá trị của s(e) phải đạt được giá trị cực đại. Điều kiện (2.5) được gọi là
điều kiện trượt (sliding condition). Đường s(e) = 0 được gọi là đường trượt (sliding
line) hay đường chuyển đổi.
11



Thay (2.3) vào (2.5) có:
     
00
21
 eexxeeee


(2.6)
thay tiếp (2.2) vào (2.5) ta đi đến:
 
 
 
eeuxxfx



),(0

212
(2.7)
Từ đó suy ra:
























0),(
0),(
21

2
21
2
eekhixxf
x
eekhixxf
x
u






(2.8)
Từ (2.3) và để cho x
1
(t)  x
10
có tốc độ cao, tín hiệu điều khiển u(t) phải có giá trị
nhỏ nhất khi
ee



< 0, ngược lại nếu
ee




> 0 thì u(t) phải có giá trị lớn nhất. Từ
đó có thể chọn:
























0),(,max
0),(,min
21
2

max
21
2
max
eekhixxf
x
u
eekhixxf
x
u
u






(2.9)
và nếu như điều kiện
),(
21
2
max
xxf
x
u 

với mọi t. (2.10)
được thoả mãn thì với (2.4) sẽ có:







0
0
eekhiu
eekhiu
u
macc
macc




(2.11)
Để được (2.4) ta chọn bộ điều khiển hai trạng thái. Bộ điều khiển hai trạng thái kết
hợp với khâu hiệu chỉnh s =
ee



tạo nên bộ điều khiển trượt theo công thức (2.5)
được xem như là bộ điều khiển có đặc tính tổng thể tốt nhất, nếu như xét đến cả sự
tồn tại sai lệch tĩnh của hệ thống. Bộ điều khiển trượt thường được dùng để loại bỏ
sự tác động nhiễu dạng xung vào hệ làm hệ bị bật ra khỏi điểm làm việc bằng cách
đưa nhanh hệ về điểm trạng thái cũ.
Có thể minh họa nguyên lý điều khiển kiểu trượt bằng hình 2.19, trong trường hợp

hệ vô hướng, khi đó mặt trượt suy biến thành đường trượt. Đường trượt trong mặt
phẳng
e0e

thường được chọn là đường thẳng có phương trình:
ees


với  > 0 (2.12)
Trên hình 2.5, đường thẳng đi qua gốc toạ độ tức s = 0, nếu điều kiện trượt là
0s.s
.

thì đường trượt có tính hấp dẫn. Một khi quỹ đạo của hệ thống điều khiển
bám được vào đường trượt thì nó sẽ nhanh chóng trượt dọc theo đường trượt về gốc
toạ độ, tại đó các giá trị
e,e

sẽ đồng thời đạt giá trị 0, tức là hệ ổn định.
12

Khi hoạt động ở chế độ trượt, hệ có các ưu điểm nổi bật sau đây:
Đảm bảo tính bền vững: Khi hoạt động ở chế độ trượt, sự thay đổi của các tham
số động học của đối tượng điều khiển trong phạm vi nhất định sẽ không làm giảm các
chỉ tiêu chất lượng của hệ thống. Sở dĩ như vậy là điều kiện cần và đủ cho chế độ
trượt được thể hiện ở dạng các bất đẳng thức. Sự thay đổi các tham số động học trong
phạm vi cho phép của các bất đẳng thức sẽ không làm mất đi chế độ trượt của hệ.
Đảm bảo tính bất biến đối với tác động bên ngoài: Nếu trong hệ thống điều
khiển với cấu trúc biến đổi, đảm bảo điều kiện cần và đủ cho chế độ trượt với tác
động điều khiển cùng mức với tác động bên ngoài, hệ thống sẽ hoạt động với chỉ
tiêu chất lượng không thay đổi, không phụ thuộc vào các tác động bên ngoài, chỉ
phụ thuộc vào mặt trượt. Đây chính là khả năng bất biến của hệ thống.
2.4.3. Lý thuyết ổn định của Lyapunov áp dụng cho điều khiển phi tuyến
hệ Robot:
Điều khiển bền vững có thể phân tích bằng cách sử dụng công cụ là tín hiệu
vào ra ổn định hoặc không gian trạng thái. Trong cách nghiên cứu theo tín hiệu vào
ra, sự ổn định của bộ điều khiển Robot sử dụng thuyết khuếch đại nhỏ hay thuyết
trọng lực. Trong cách nghiên cứu theo không gian trạng thái, phần lớn các thiết kế
sử dụng thuyết ổn định của Lyapunov.
Bộ điều khiển bền vững trong Robot được phân chia thành hai cách dựa trên
việc sử dụng thuyết ổn định Lyapunop hay bền vững các tín hiệu vào ra:
Ổn định Lyapunov tại điểm cân bằng 0 nếu với  > 0 bất kỳ bao giờ cũng tồn
tại  phụ thuộc  sao cho nghiệm x(t) của phương trình trên với x(0) = x
0
thoả mãn:
x
0
<  x(t) < , t  0
Ổn định tiệm cận Lyapunov tại điểm cân bằng 0 nếu với  > 0 bất kỳ bao giờ

cũng tồn tại  phụ thuộc  sao cho nghiệm x(t) của phương trình trên với x(0) = x
0

thoả mãn:
0)(lim


tx
t

đường trượt
s = 0
0
s > 0
s < 0
s(t=0)

e

Hình 2.5:
Sơ đồ nguyên lý điều khiển kiểu trượt.

13




Hình 2.6: Minh hoạ khái niệm ổn định Lyapunov
2.5. TIÊU CHUẨN LYAPUNOV:
Tính ổn định tại 0 cho ta một hướng đơn giản để tính ổn định cho hệ tại 0.

Bằng cách nào đó người ta có một đường cong khép kín v bao quanh gốc toạ độ 0.
Vậy để kiểm tra tính ổn định tại 0 hay không người ta xem quỹ đạo pha x(t) đi từ
điểm trạng thái đầu x
0
cho trước nhưng tùy ý nằm trong miền bao bởi một trong các
đường cong v này theo hướng từ ngoài vào trong hay không.
- Nếu x(t) không cắt đường cong họ v nào theo chiều từ trong ra ngoài thì hệ
sẽ ổn định tại 0.
- Nếu x(t) cắt mọi đường cong họ v nào theo chiều từ ngoài vào trong thì hệ
sẽ ổn định tiệm cận tại 0.
2.6. PHƯƠNG PHÁP ĐIỀU KHIỂN TRƯỢT CHO ROBOT N BẬC TỰ
DO:
2.6.1. Cơ sở toán học:
Ta xem xét hệ động học sau: x
(n)
= a(X) + B(X).u (2.13)
trong đó đại lượng vô hướng x là đầu ra mong muốn, đại lượng vô hướng u là tín
hiệu điều khiển đầu vào,
 
T
)1n(
x xxX



là vectơ trạng thái, a(X) là hàm phi
tuyến không biết chính xác và B(X) là ma trận biểu diễn độ khuếch đại điều khiển
không biết chính xác.
Trạng thái ban đầu X
d

(0) phải là: X
d
(0)  X(0) (2.14)
và
 
T
)1n(
d
x
~
x
~
x
~
XXX
~




Ngoài ra, ta định nghĩa bề mặt biến thiên theo thời gian s(t) trong không gian trạng
thái R
(n)
bằng phương trình vô hướng S(X;t) = 0
Trong đó:
S(X;t) =
x
~
)
dt

d
(
1n

(2.15)
với  là một hằng số dương. Ví dụ nếu n = 2 thì
x
~
x
~
S 

tức s là tổng mức ảnh
hưởng của sai lệch vị trí và sai lệch vận tốc.
14



Việc giữ giá trị vô hướng S bằng 0 có thể giải quyết được bằng cách chọn luật điều
khiển u trong (2.13) sao cho ở bên ngoài s(t) ta có:
S)t,X(S
dt
d
2
1
2

(2.16)
trong đó  là hằng số dương. (2.16) cho thấy rằng khoảng cách đến bề mặt s, được
tính bằng S

2
,

giảm

xuống theo quỹ đạo hệ thống.
Vì thế nó buộc các quỹ đạo hệ thống hướng tới bề mặt s(t) như minh họa trong hình
2.7 dưới đây.

Bắt đầu từ điểm xuất phát ban đầu nào đó, quỹ đạo trạng thái chạm đến mặt phẳng
trượt, sau đó sẽ trượt dọc theo mặt trượt và hướng đến x
d
với tốc độ hàm mũ, với
hằng số thời gian 1/ (hình 2.8).
Tóm lại, từ phương trình (2.15) chọn một hàm S, sau đó chọn luật điều khiển u
trong (2.13) sao cho S
2
duy trì một hàm Lyapunov của hệ thống kín, bất chấp sự
thiếu chính xác của mô hình và sự có mặt của nhiễu loạn. Trình tự thiết kế do đó sẽ
bao gồm 2 bước:
Bước một, chọn luật điều khiển u thỏa mãn điều kiện trượt (2.16).
Bước hai, luật điều khiển không liên tục u đã được chọn trong bước một
được làm nhẵn một cách thích hợp để có sự dung hòa tối ưu giữa dải thông điều
khiển và tính chính xác của quỹ đạo, đồng thời khắc phục hiện tượng chattering
(hình 2.9).
Xét một hệ phi tuyến bậc hai có phương trình trạng thái như sau:

uXBXax )()( 

(2.17)

với
 
T
xxX


là ma trận biểu thị trạng thái của hệ thống.
2.6.1.1. Các giả thiết của (2.17):
Giả thiết có phương trình động lực học của Robot như sau:
)().(),( tduqBqqaq 


15



Với:
),().( qqhqqHu


: là tín hiệu điều khiển.
)qC(q, );().,(),(


qgqqqCqqh

là vectơ lực Coriolis
n11
R d(t) );(B(q) );,().(),( 


qHqqhqHqqa

là nhiễu ngoài chưa
biết.
Hàm a(X) không được biết chính xác nhưng có ngưỡng giới hạn

)X(a)X(a)X(a
~
max

là một hàm xác định (2.18)
Gọi b
x
là giá trị riêng của B(X), b
x min
và b
x max
lần lượt là giá trị riêng nhỏ nhất và
lớn nhất của B(X). Đặt 
x
= (b
x max
/b
x min
)
1/2
, ta được:

x
1

x
)X(B)X(B
~
1



(2.19)
2.6.1.2. Các bước xây dựng bộ điều khiển trượt:
Sai lệch quỹ đạo:
d
d
e x x
e x x
 
 
  
(2.20)
Bước 1:
Định nghĩa mặt s(t) như sau:
0e
dt
d
)t,X(S
)1n(










(2.21)
trong đó  là hằng số dương.
Nếu n = 2 thì mặt s(X,t) là: S(X,t) =
ee


= 0 (2.22)
Bước 2:
Tính u để cho trạng thái hệ thống tiến về mặt s(t) và nằm trên đó như trên hình 2.2.
Để có được điều đó, xét một hàm năng lượng
0SS.21)x(V
T

của hệ thống kín.
Giả sử có điểm cân bằng tại điểm x = 0 tại đó V(x) cực tiểu. Nếu chứng minh được:

0SS)x(V
T



(2.23)
thì điểm x = 0 được gọi là điểm ổn định.
Theo nguyên lý ổn định Lyapunov, chọn một hàm:

0SS.21)x(V

T

với S  0
Phải làm cho
0)x(V 

, nghĩa là:
0SSV
T



.
Đây là điều kiện để hệ thống luôn luôn ổn định tiệm cận toàn cục tại S = 0. Khi điều
kiện (2.19) được thỏa mãn thì trạng thái của hệ thống luôn luôn được đưa về trên
mặt trượt S = 0 và giữ trên đó. Đó là yêu cầu của bước 2. Như vậy phải thiết kế tín
hiệu điều khiển u sao cho điều kiện (2.19) được thỏa mãn. Ta có:
16



exxeeS
d




=
uXBXaxe
d

)()( 



(2.24)
Chọn tín hiệu đầu vào theo công thức sau [12]:
 
reqeq
uuSKuBu 

)sgn(.
~
~
1

(2.25)
Trong đó:
eqeq
uBu
~
.
~
1

và
)sgn(.
~
1
SKBu
r



(2.26)
Với sgn(S) = [sgn(S
1
), , sgn(S
n
)]
T

K = diag(K
1
, , K
n
); K
i
> 0 với i = 1, 2, , n (2.27)
Đối với hệ phương trình trạng thái (2.17), nếu các giả thiết (2.18) và (2.19) đều
được thỏa mãn và luật điều khiển được chọn như trong (2.25) với

 
)n ,,2,1i(au
~
1K
maxxeqxi

(2.28)
thì sai số quỹ đạo e = x
d
– x sẽ hội tụ về 0, nghĩa là x

d
 x.
Từ đây ta sẽ xem xét lý thuyết tổng hợp bộ điều khiển kiểu trượt cho cơ cấu Robot
n bậc tự do. Bỏ qua thành phần trọng lực g(q), ta có được:


),().( qqhqqH

(2.29)
Giả định rằng các thông số được đánh giá
)q,q(h
ˆ
),q(H
ˆ
.
và các giá trị thực của
chúng có mối liên hệ theo các bất đẳng thức sau:
)()()(
ˆ
1
qBqHqH 

(2.30)
),(),(
ˆ
),(
max
qqhqqhqqh



(2.31)
Trong đó B(q),
),(
max
qqh


là các hàm đã biết. Điều này cho thấy cả ma trận quán
tính và lực liên kết trên khớp được đánh giá với sai số xác định.
Phương trình động lực học (2.29) có thể được viết lại như sau:

).(),( qBqqaq


(2.32)
Với:
),().(),(
1
qqhqHqqa



(2.33)
B(q) = H
-1
(q)
Dễ thấy B(q) là ma trận nghịch đảo của H(q). Nhiệm vụ điều khiển là tính mô men 
thích hợp sao cho vectơ vị trí thực q
t
(góc quay) luôn bám theo quỹ đạo đặt q

d
.
Chọn sai số trạng thái và mặt trượt có dạng mô tả sau:
e = q
d
– q
t
(2.34)
17



eCes


, với C = C
T
> 0 (2.35)
Dễ thấy rằng, việc duy trì trên mặt trượt (s = 0) sẽ dẫn đến q(t)  q
d
. Thực tế khi
chọn s = 0 thì phương trình (2.24) trở thành:
Cee


(2.36)
Phương trình (2.25) chỉ có nghiệm duy nhất e = 0. Nói cách khác, nó đặc trưng cho
hệ động lực học ổn định tiệm cận có e = 0 là giải pháp duy nhất, từ đó điều kiện
bám q
t

 q
d
được thoả mãn. Do vậy, vấn đề cần giải quyết của luật điều khiển là
tìm tín hiệu điều khiển(mô men động) tại các khớp 
i
sao cho duy trì quỹ đạo Robot
trên mặt trượt.
Vận dụng lý thuyết ổn định Lyapunov, vấn đề chọn  có thể chuyển thành xét tính
ổn định hàm năng lượng V. Chọn hàm V có dạng:
V = (1/2).s
T
.s > 0 (2.37)
Vi phân hàm V ta có:
ssV
T



(2.38)
Do vậy điều kiện để hệ ổn định là:
0ssV
T



(2.39)
Điều kiện (2.28) được gọi là điều kiện trượt. Khi điều kiện trượt được thoả mãn, hệ
thống kín sẽ ổn định tiệm cận, toàn bộ và xảy ra hiện tượng bám của tín hiệu ra q
t
so

với tín hiệu đặt q
d
mặc dù tồn tại các phần không mô hình được, nhiễu hai sai lệch
ban đầu (q(0)  q
d
(0)).
Nếu điều kiện trượt được thoả mãn theo biểu thức sau:
0sss
T


;  > 0;



m
1i
2
i
ss
. (2.40)
thì mặt trượt s = 0 sẽ được bám (lần thứ nhất) trong khoảng thời gian nhỏ hơn T
0
.
))0((
2
1
0
qsT



(2.41)
Vi phân hai vế (2.24) và thay (2.23) vào phương trình ta có:
td
qqeCeeCs


(2.42)
ta có:
d
qqBqqaeCs



)(),(
(2.43)
Chọn mô men vào có dạng:

 
)sgn(.
ˆ
1
sKB
cq



(2.44)
Trong đó:
18

),(
ˆ
qqaeCq
dcq




sgn(s) = [sgn(s
1
), sgn(s
2
), , sgn(s
m
)]
T

K > 0, là ma trận (nn)
Ma trận hệ số K phải chọn đủ lớn sao cho duy trì được điều kiện trượt, mặc dù tồn
tại các thành phần không mô hình được hay nhiễu. Trường hợp việc đánh giá các
thông số là chính xác (
aa
ˆ
,BB
ˆ


) thì điều kiện trượt có thể được biểu diễn dưới
dạng sau:
ssKsss
TT

 )sgn(


(2.45)
Tín hiệu điều khiển  và tín hiệu ra ta thấy tín hiệu vào bị gián đoạn khi cắt ngang
mặt trượt s(t) do vậy sẽ dẫn tới hiện tượng lập bập ở đầu ra. Hiện tượng này có thể
khắc phục bằng việc lọc các thành phần gián đoạn trong miền  lân cận mặt cắt.
Trường hợp vectơ
s

có dạng:

)sgn(
ˆ
).(
ˆ
)(),(
ˆ
11
sKBqBBqBqqaeCqs
cqd





(2.46)
thay
)
ˆ
(
ˆ
aaaa

và đặt R = -B(q)
1
B
ˆ

ta có:

)sgn()
ˆ
()( sRKaaIRs
cq



(2.47)
Khi đó điều kiện trượt sẽ trở thành:

 
)sgn()sgn()
ˆ
()( sssRKaaIRsss
T

cq
TT



(2.48)
Do đó, nếu chọn K thoả mãn:

 
)sgn()
ˆ
()()sgn( ssaaIRssRKs
T
cq
TT


(2.49)
thì điều kiện trượt (2.28) được thoả mãn. Tương đương với các điều kiện (2.30) và
(2.31) là các điều kiện sau:

)(
ˆ
11
qBBBR 

(2.50)

max
1

ˆ
)
ˆ
(
ˆ
)
ˆ
( hBhhBffR 

(2.51)
Khi đó ma trận K thoả mãn (2.18) có thể được chọn như sau:
BIhBIBK
cq


max
ˆ
)1(
(2.52)
2.7. Nhận Xét:
Các phương pháp điều khiển được nêu ra trên đây là những phương pháp
điều khiển có tính khả thi và đã được áp dụng ở một số lĩnh vực cụ thể trong thực
tế. Qua phân tích tổng quan các phương pháp điều khiển Robot ở trên, ta có thể rút
ra những nhận xét sau:
19



Vấn đề nâng cao chất lượng và tăng độ chính xác điều khiển bám quỹ đạo
của hệ thống điều khiển rô bốt chủ yếu dựa vào việc lựa chọn và sử dụng các

phương pháp điều khiển, do vậy cần có các công trình nghiên cứu cụ thể.
Mỗi phương pháp được áp dụng lại có những ưu, điểm khác nhau và được áp
dụng tuỳ theo từng ứng dụng cụ thể, tuỳ thuộc vào bài toán kinh tế cần giải quyết.
Những phương pháp điều khiển đơn giản lại mắc phải nhược điểm về độ chính xác
hoặc tốc độ di chuyển v.v Còn với những phương pháp điều khiển phức tạp có
chất lượng cao, độ chính xác điều khiển lớn thì thường mắc phải nhược điểm về tốc
độ xử lý hay giải pháp kinh tế.
Điều khiển trượt là phương pháp điều khiển tiếp cận rất mạnh mẽ để điều
khiển các hệ thống phi tuyến và bất định. Đó là phương pháp điều khiển bền vững
và có thể áp dụng cho hệ bất định và có tham số bị thay đổi lớn. Với tay máy robot
khi các tham số của hệ thay đổi liên tục; lực, vị trí, mô men, mô men quán tính…
và các tác động qua lại làm cho mô hình robot trở nên phi tuyến mạnh thì phương
pháp điều khiển trượt tỏ ra có ưu thế vượt trội, cho việc điều khiển chuyển động bền
vững và bám theo quỹ đạo đặt của robot.
Trên cơ sở phân tích ở trên ta nhận thấy phương pháp điều khiển trượt rất
thích hợp cho điều khiển robot, tuy vậy phương pháp này còn bộc lộ nhược điểm
cần phải giải quyết: Tín hiệu điều khiển không liên tục (đảo dấu liên tục), là nguyên
nhân gây ra hiện tượng rung (dao động với tần số khá cao xung quanh mặt trượt),
đó là do luôn tồn tại thời gian nhảy cấp xác định bởi thời gian trễ và quán tính của
bộ phận truyền động mà các hiện tượng nhảy cấp lập bập thường xảy ra.
Trong thực tế, do quán tính của các thành phần có liên quan làm cho hiện
tượng rung, lập bập(chattering) đó xảy ra với tần số thấp hơn. Tuy nhiên hiện tượng
rung vẫn làm giảm đi tính ưu việt của chế độ trượt lý tưởng. Vì vậy khi thiết kế
chọn phương pháp điều khiển trượt ta sẽ phải cố gắng để khử hiện tượng này bằng
cách sử dụng bộ điều khiển trượt cải tiến và ưu việt hơn.
Ở chế độ trượt, hệ thống làm việc như hệ bậc 1 vì quỹ đạo tương ứng với
đường thẳng chuyển đổi. Ở quá trình quá độ bậc sẽ giảm đi ở chế độ trượt, đó là đặc
điểm của nguyên lý làm việc này.
Việc tổng hợp hệ làm việc ở chế độ trượt có thể thực hiện theo phương pháp
áp đặt nghiệm cực hay các phương pháp tối ưu khác.

Điều chỉnh theo chế độ trượt cho phép sử dụng cơ cấu điều khiển tác động
nhanh; với những điều kiện nhất định hệ thống sẽ có tính bền vững nghĩa là sự thay
đổi thông số của hệ không làm ảnh hưởng đến hành vi của nó.
Điều khiển trượt cho phép điều khiển bền vững, tuy nhiên khuyết điểm chính
của phương pháp này là hoạt động điều khiển không liên tục và gây ra hiện tượng
chattering ngoài ý muốn. Nhìn chung nó tạo ra đáp ứng thời gian chậm do tần số
chuyển đổi bị giới hạn.

20



Chương 3
MÔ HÌNH TOÁN HỌC CỦA TAY MÁY SCARA
Nội dung chương này trình bày về mô hình động học và động lực học của tay máy
SCARA.
3.1. GIỚI THIỆU VỀ ROBOT SCARA:
Robot SCARA ra đời vào năm 1979, đây là một kiểu tay máy có cấu tạo đặc biệt
được sử dụng nhiều trong các công việc lắp ráp các tải trọng nhỏ theo phương thẳng
đứng.
Xét một tay máy SCARA như hình 3.1.
Tay máy gồm 2 thanh quay có chiều dài là a
1
, a
2
và một thanh tịnh tiến có chiều dài
là d. Thanh a
1
và thanh a
2

được nối với nhau qua một khớp quay, thanh a
1
được nối
vào chân đế qua một khớp quay các khớp quay được truyền động bởi động cơ
Servo.
Hai động cơ cung cấp hai moment
1 2
,
 
điều khiển hai cánh tay a
1
và a
2
. Thanh
chuyển động tịnh tiến d có thể quay quanh trục của nó. Giả thiết rằng thanh chuyển
động tịnh tiến được giữ cố định, tay máy SCARA có thể tương đương một tay máy
2 bậc tự do trong mặt phẳng 0xy như hình 3.2.








z1
z
2

z

0
z
3




d
O
0

a
1

a
2

x
3

x
0
x
2

x
3

z
3

d
4

o
4


Hình 3.1: Mô hình tay máy SCARA

21





m
2



m
1



Hình 3.2: Tay máy SCARA trong hệ tọa độ 0xy.
3.2. MÔ HÌNH ĐỘNG HỌC CỦA TAY MÁY:
3.2.1. Mô hình động học thuận:
Từ sơ đồ cấu trúc của tay máy SCARA, ta có tọa độ điểm P được xác định như sau:

 
21211
coscos


aaX
(3.1)

 
21211
sinsin


aaY
(3.2)
Trong đó:
a
1
và a
2
là chiều dài của các thanh,

1
và 
2
là góc ở các khớp của tay máy,
(X,Y) là tọa độ của điểm cuối P của tay máy.
3.2.2.Mô hình động học ngược:
Bài toán động học ngược cánh tay scara 2 bậc tự do là giải bài toán tìm góc

1

và
2

được cho bởi vị trí đầu cuối của cánh tay.
Ta thấy rằng
2 2 2 2 2
1 2
a a r x y   
và có 2 lời giải. Hình trên thể hiện cho lời giải thứ
nhất
Ta có:
ysasa
xcaca


12211
12211
(3.3)
O
y
x
r
a
1

a
2

P(X,Y)

X
Y
22



Gọi r là khoảng cách từ tâm gốc tọa độ đến điểm cuối

222
yxr 
(3.4)
và
221
2
2
2
1221
2
2
2
1
2
cos2)cos(2

aaaaaaaar 

Đặt

21
2
2
2
1
2
2
2
cos
aa
aar
C





2
2
2
2
1cos1sin CD 

(3.5)

),(
2
CDATAN



(3.6)
Tương tự ta sẽ có lời giải cho góc
1

dựa vào góc

như sau:

221
22
cos
sin
tan



aa
a


(3.7)
Mặc khác:
x
y
 )tan(
1


(3.8)
Vì vậy ta có:

)cos,sin(),(
221221

aaaATANxyATAN

(3.9)
cần lưu ý rằng góc
1

phụ thuộc vào góc
2


3.3. MÔ HÌNH ĐỘNG LỰC HỌC CỦA TAY MÁY:
Xét một tay máy như hình 3.3. Giả thiết rằng, khối lượng của các thanh là không
đáng kể so với động cơ truyền động ở các khớp để trọng tâm của các thanh tập
trung ngay tại các khớp. Gọi m
1
, m
2
là khối tâm của các thanh a
1
, a
2

Hình 3.3: Tay máy SCARA trong hệ tọa độ 0xy.
m
1

O
y
x
r
a
1

a
2



P(X,Y)

X
Y
m
2

23



Đạo hàm (3.1) và (3.2) ta có:

 
 
21212111
sinsin




aaX
(3.10)

 
 
21212111
coscos




aaY
(3.11)
Bình phương vận tốc dài của điểm P(X,Y) như sau:
 
 
 
 
 
 
 
 

   
2 2
. .
2
2
2 2 2 2 2
1 1 1 2 1 2 1 2 1 2 1 1 2 1 1 2
2
2 2 2 2 2
1 1 1 2 1 2 1 2 1 2 1 1 2 1 1 2
2
2 2 2
1 1 2 1 2 1 2 1 1 2 2
sin sin 2 sin sin
cos cos 2 cos cos
2 cos
v X Y
a a a a
a a a a
a a a a
           
           
      
 
      
      
    
     
     
     

( 3.12)
Động năng của hệ thống:

   
 
221121
2
21
2
2
2
1
2
12
2
111
2
2
2
111
cos2
2
1
2
1
2
1
2
1







aaaamam
vmamK
(3.13)
Vì tay máy nằm ngang do đó thế năng của hệ thống P = 0.
Hàm Lagrangian của hệ thống:

   
 
   
2211212
2
21
2
22
2
1
2
12
2
1
2
11
221121
2
21

2
2
2
1
2
12
2
1
2
11
2
2
2
1
2
11
cos
2
1
2
1
2
1
cos2
2
1
2
1
2
1

2
1









aamamamam
aaaamam
vmamKL
(3.14)
Theo phương trình Lagrange của chuyển động của tay máy có dạng:
















q
L
qd
L
dt
d

(3.15)