Để xem full tài liệu Xin vui long liên hệ page để được hỗ trợ : https://www.facebook.com/thuvienluanvan01 HOẶC https://www.facebook.com/garmentspace/ https://www.facebook.com/thuvienluanvan01 https://www.facebook.com/thuvienluanvan01 tai lieu tong hop, thu vien luan van, luan van tong hop, do an chuyen nganh

1. 1
ĐỒ ÁN TỐT NGHIỆP
NÂNG CAO CHẤT LƯỢNG ĐIỀU KHIỂN
ROBOT SCARA

2. 2
MỤC LỤC
MỞ ĐẦU ..................................................................................................................................4
CHƯƠNG I: TỔNG QUAN VỀ ROBOT VÀ ĐIỀU KHIỂN ROBOT .....................7
1.1. Robot và robot công nghiệp .....................................................................................7
1.1.1. Vài nét lịch sử phát triển của robot và robot công nghiệp ...............................7
1.1.2. Robot và công nghệ cao........................................................................................8
1.1.3. Định nghĩa về robot công nghiệp ........................................................................9
1.2. Các phương pháp điều khiển robot..................................................................... 11
1.2.1. Phương pháp điều khiển động lực học ngược ................................................ 11
1.2.2. Phương pháp điều khiển phản hồi phân ly phi tuyến .................................... 13
1.2.3. Phương pháp điều khiển thích nghi theo sai lệch .......................................... 14
1.2.4. Phương pháp điều khiển thích nghi theo mô hình mẫu ................................ 15
1.2.5. Phương pháp điều khiển động lực học ngược thích nghi ............................. 16
1.2.6. Phương pháp điều khiển trượt.......................................................................... 17
CHƯƠNG II: MÔ TẢ ĐỐI TƯỢNG - ROBOT SCARA SERPENT..................... 18
2.1. Một số loại robot Scara của các hãng sản xuất ................................................. 18
2.2. Các thông số và vùng làm việc của robot Scara Serpent ................................ 20
2.2.1. Cấu tạo tay máy robot Scara Serpent ............................................................... 21
2.2.1.1. Cấu hình của robot Scara Serpent ............................................................. 21
2.2.1.2. Các thông số kỹ thuật của robot Scara Serpent ....................................... 21
2.2.2. Giới hạn không gian làm việc của robot Scara Serpent................................. 22
2.3. Động học robot Scara Serpent .............................................................................. 24
2.3.1. Động học thuận................................................................................................... 24
2.3.2. Động học ngược.................................................................................................. 27
2.4. Động lực học robot Scara Serpent ....................................................................... 29
2.4.1. Hàm Euler - Lagrange và các vấn đề động lực học ....................................... 30
2.4.2. Động lực học robot Scara Serpent.................................................................... 31
2.4.2.1. Tính toán động năng và thế năng cho từng khớp .................................... 31
2.4.2.2. Phương trình động lực học......................................................................... 34
2.5. Mô tả đối tượng bằng hệ phương trình trạng thái........................................... 38
CHƯƠNG III: XÂY DỰNG MÔ HÌNH HỆ THỐNG ĐIỀU KHIỂN .................... 40

3. 3
3.1. Cấu trúc hệ thống điều khiển robot..................................................................... 40
3.2. Xây dựng quỹ đạo chuyển động chuẩn............................................................... 41
3.2.1. Xác định giá trị q02 và qc1 ............................................................................... 43
3.2.2. Phương trình đoạn cd ......................................................................................... 43
3.2.3. Phương trình đoạn ac ......................................................................................... 44
3.2.4. Phương trình đoạn df.......................................................................................... 44
3.3. Thiết kế bộ điều khiển cho tay máy robot Scara Serpent ba bậc tự do ...... 45
3.3.1. Hệ phương trình động lực học Lagrange......................................................... 45
3.3.2. Hệ phương trình trạng thái ................................................................................ 46
3.3.3. Lựa chọn phương pháp điều khiển và bộ điều khiển PID ............................. 49
CHƯƠNG IV: MÔ PHỎNG VỚI MÔ HÌNH ROBOT SCARA SERPENT ... 52
4.1. Đặt vấn đề.................................................................................................................. 52
4.2. Sơ đồ mô hình hóa các khâu của hệ thống......................................................... 52
4.2.1. Mô hình chung của robot................................................................................... 52
4.2.2. Mô hình khối tạo quỹ đạo chuyển động chuẩn............................................... 52
4.2.3. Mô hình bộ điều khiển ....................................................................................... 53
4.3. Giao diện chương trình mô phỏng robot Scara Serpent ................................ 53
4.4. Kết quả mô phỏng hệ thống trên Matlab-Simulink......................................... 59
4.4.1. Thông số của robot và quỹ đạo chuyển động ................................................. 59
4.4.2. Đặc tính của hệ thống khi robot làm việc với tải khác nhau......................... 59
KẾT LUẬN........................................................................................................................... 80
TÀI LIỆU THAM KHẢO ................................................................................................. 81
PHỤ LỤC 1: CÁC SƠ ĐỒ KHỐI ................................................................................... 81
PHỤ LỤC 2: CÁC CHƯƠNG TRÌNH M FILES........................................................ 87

4. 4
MỞ ĐẦU
Theo quá trình phát triển của xã hội, nhu cầu nâng cao sản xuất và chất lượng
sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất.
Xu hướng tạo ra những dây chuyền và thiết bị tự động có tính linh hoạt cao đã hình
thành và phát triển mạnh mẽ…Vì thế ngày càng tăng nhanh nhu cầu ứng dụng người
máy để tạo ra các hệ sản xuất tự động linh hoạt.
Robot ứng dụng rộng rãi và đóng vai trò quan trọng trong sản xuất cũng như
trong đời sống. Robot là cơ cấu đa chức năng có khả năng lập trình được dùng để di
chuyển nguyên vật liệu, các chi tiết, các dụng cụ thông qua các truyền động được lập
trình trước. Khoa học robot chủ yếu dựa vào các phép toán về đại số ma trận.
 Robot có thể thao tác như con người và có thể hợp tác với nhau một cánh thông
minh.
 Robot có cánh tay với nhiều bậc tự do và có thể thực hiện được các chuyển
động như tay người và điều khiển được bằng máy tính hoặc có thể điều khiển
bằng chương trình được nạp sẵn trong chip trên bo mạch điều khiển robot.
Để hệ điều khiển robot có độ tin cậy, độ chính xác cao, giá thành hạ và tiết
kiệm năng lượng thì nhiệm vụ cơ bản là hệ điều khiển robot phải đảm bảo giá trị yêu
cầu của các đại lượng điều chỉnh và điều khiển. Ngoài ra, hệ điều khiển robot phải
đảm bảo ổn định động và tĩnh, chống được nhiễu trong và ngoài, đồng thời không gây
tác hại cho môi trường như: tiếng ồn quá mức quy định, sóng hài của điện áp và dòng
điện quá lớn cho lưới điện v.v...
Khi thiết kế hệ điều khiển robot mà trong đó sử dụng các hệ điều chỉnh tự động
truyền động, cần phải đảm bảo hệ thực hiện được tất cả các yêu cầu về công nghệ, các
chỉ tiêu chất lượng và các yêu cầu kinh tế. Chất lượng của hệ thống được thể hiện
trong trạng thái tĩnh và trạng thái động. Trạng thái tĩnh yêu cầu quan trọng là độ chính
xác điều chỉnh. Trạng thái động thì có yêu cầu về độ ổn định và các chỉ tiêu về chất
lượng động là độ quá điều chỉnh, tốc độ điều chỉnh, thời gian điều chỉnh và số lần dao
động. Đối với hệ điều khiển robot, việc lựa chọn sử dụng các bộ biến đổi, các loại
động cơ điện, các thiết bị đo lường, cảm biến, các bộ điều khiển và đặc biệt là phương
pháp điều khiển có ảnh hưởng rất lớn đến chất lượng điều khiển bám chính xác quỹ
đạo của hệ.

5. 5
Các công trình nghiên cứu về hệ thống điều khiển robot tập trung chủ yếu theo
hai hướng là sử dụng các mô hình có đặc tính phi tuyến có thể ước lượng được để đơn
giản việc phân tích và thiết kế hoặc đề ra các thuật toán điều khiển mới nhằm nâng cao
chất lượng đáp ứng của robot.
Đặc điểm cơ bản của hệ thống điều khiển robot là thực hiện được điều khiển
bám theo một quỹ đạo phức tạp đặt trước trong không gian, tuy nhiên khi dịch chuyển
thì trọng tâm của các chuyển động thành phần và mômen quán tính của hệ sẽ thay đổi,
điều đó dẫn đến thông số động học của hệ cũng thay đổi theo quỹ đạo chuyển động và
đồng thời xuất hiện những lực tác động qua lại, xuyên chéo giữa các chuyển động
thành phần trong hệ với nhau. Các yếu tố trên tác động sẽ làm cho hệ điều khiển robot
mang tính phi tuyến mạnh, gây cản trở rất lớn cho việc mô tả và nhận dạng chính xác
hệ thống điều khiển robot. Do vậy, khi điều khiển robot bám theo quỹ đạo đặt trước
phải giải quyết được những vấn đề sau:
 Khắc phục các lực tương tác phụ thuộc vào vận tốc, gia tốc của quỹ đạo riêng
các chuyển động thành phần và quỹ đạo chung của cả hệ như: lực quán tính, lực
ly tâm, lực ma sát v.v...
 Khi trọng tâm của các chuyển động thành phần và của cả hệ thay đổi theo quỹ
đạo riêng và chung kéo theo sự thay đổi của các thông số động học của hệ, điều
đó đòi hỏi phải có sự biến thiên các tham số đưa vào bộ điều khiển tương ứng
để vẫn đảm bảo sự cân bằng, ổn định và bền vững đồng thời vẫn bám theo được
quỹ đạo đặt.
Với công cụ toán vi phân người ta đã có thể phân tích tính điều khiển được, tính
quan sát được cho hệ phi tuyến.
Nội dung của đề tài nghiên cứu như sau:
1. Tên đề tài: Nâng cao chất lượng điều khiển cho robot Scara.
2. Cơ sở khoa học và thực tiễn của đề tài: Ứng dụng phương pháp điều khiển
động lực học ngược cho điều khiển bền vững quỹ đạo robot.
3. Mục đích của đề tài: Xây dựng cấu trúc và thuật toán điều khiển robot Scara.
4. Nội dung của đề tài, các vấn đề cần giải quyết:

6. 6
- Xây dựng mô hình toán học cho robot Scara Serpent 3 bậc tự do.
- Xây dựng hệ thống điều khiển quỹ đạo đạt độ chính xác cao.
- Đánh giá chất lượng hệ thống bằng mô phỏng.
Nội dung của luận văn đề cập tới vấn đề “Nâng cao chất lượng điều khiển
robot Scara” với mục tiêu điều khiển bền vững và bám chính xác quỹ đạo chuyển
động. Luận văn được trình bày thành 4 chương với nội dung cơ bản của từng chương
được tóm tắt như sau:
Chương I – Tổng quan về robot và điều khiển robot: Mô tả tổng quan về
robot. Phân tích ưu, nhược điểm của một số phương pháp điều khiển robot đã và đang
được áp dụng trong thực tiễn để nâng cao độ chính xác điều khiển quỹ đạo robot.
Chương II – Mô tả toán học đối tượng robot Scara: Nghiên cứu một số
robot trong họ Scara và đi sâu vào phân tích mô hình Robot Scara Serpent để phục vụ
việc nghiên cứu và kiểm chứng cơ sở lý thuyết và các phương pháp điều khiển được
lựa chọn.
Chương III – Xây dựng mô hình hệ thống điều khiển: Thiết kế bộ điều khiển
để nâng cao chất lượng hệ thống điều khiển quỹ đạo cho robot. Xây dựng mô hình hệ
thống điều khiển cho robot Scara Serpent, xây dựng mô hình mô phỏng sử dụng phần
mềm Matlab-Sumulink.
Chương IV – Mô phỏng với mô hình robot Scara Serpent: Định hình và
kiểm chứng về mặt lý thuyết cơ sở thực tiễn của đề tài cũng như tính khả thi của
phương pháp điều khiển được lựa chọn khi áp dụng cho hoạt động bền vững của điều
khiển quỹ đạo robot Scara Serpent 3 bậc tự do.
CHƯƠNG I

7. 7
TỔNG QUAN VỀ ROBOT VÀ ĐIỀU KHIỂN ROBOT
1.1. Robot và robot công nghiệp
1.1.1. Vài nét lịchsử phát triểncủa robot và robot công nghiệp
Nhìn ngược dòng thời gian chúng ta có thể nhận thấy rằng từ “Robot” đã xuất
hiện từ khá lâu. Năm 1921 nhà viết kịch Karelcapek người Séc đã viết một vở kịch với
tựa đề R.U.R (Rossums Universal Robot) mô tả về một cuộc nổi loạn của những cỗ
máy phục dịch. Từ “Robot” ở đây có nghĩa là những máy móc biết làm việc như con
người. Có lẽ đó cũng là một gợi ý cho những nhà sáng chế kỹ thuật thực hiện các mơ
ước về những cỗ máy bắt chước được các thao tác lao động cơ bắp của con người.
Thời gian sau đó các cơ cấu điều khiển từ xa (Teleoperator) ra đời và ngày một
phát triển hoàn thiện. Teleoperator là những cơ cấu phỏng sinh học, nó bao gồm các
khâu, các khớp cùng với các dây chằng gắn liền với hệ điều hành là cánh tay của
người điều khiển thông qua các cơ cấu khuếch đại cơ khí. Teleoperator có thể cầm
nắm, nâng hạ, dịch chuyển, xoay lật các đối tượng trong một không gian hoạt động
nhất định. Tuy rằng các thao tác khá tinh vi, khéo léo nhưng tốc độ hoạt động chậm,
lực tác dụng hạn chế và hệ điều khiển chỉ thuần tuý là cơ khí.
Từ thập kỷ 50, sự phát triển đầy hứa hẹn của kỹ thuật điều khiển theo chương
trình số cứng và ngành vật liệu mới đã làm chỗ dựa vững chắc cho sự ra đời của các cơ
cấu điều khiển vô cấp (servo mechanism) và các hệ điện toán (computation). Ngay lập
tức ý tưởng kết hợp hệ điều khiển NC (Numerical control) với các cơ cấu điều khiển từ
xa (Teleoperator) được hình thành và triển khai nghiên cứu. Sự phối hợp tuyệt vời
giữa khả năng linh hoạt khéo léo của Teleoperator với độ thông minh nhạy bén của hệ
điều khiển NC đã đưa ra kết quả là một hệ máy móc tự động cao cấp với tên gọi
“Robot”.
Năm 1961 người máy công nghiệp (IR- industrial Robot) đầu tiên được đưa ra
thị trường. Tiếp theo đó các nước khác cũng bắt đầu sản xuất robot công nghiệp theo
bản quyền của Mỹ, Anh (1967), Thụy Điển, Nhật (1968), Đức (1971) . . .
Ngày nay, trên thế giới có khoảng 200 công ty sản xuất IR, trong đó ở Nhật có
70, ở các nước Tây âu có 90, ở Mỹ có 30. Nhờ áp dụng rộng rãi các tiến bộ khoa học
kỹ thuật về vi xử lý, tin học cũng như vật liệu mới nên số lượng robot công nghiệp đã

8. 8
tăng lên nhanh chóng, giá thành giảm đi rõ rệt, tính năng có nhiều cải tiến. Robot công
nghiệp phát huy thế mạnh ở những lĩnh vực như hàn hồ quang, đúc, lắp ráp, sơn phủ,
và trong các hệ thống tự động điều khiển liên hợp.
1.1.2. Robot và công nghệ cao
Robot và công nghệ cao là những khái niệm của sản xuất tự động hoá hiện đại.
Các nước công nghiệp phát triển đã đưa ra chiến lược dùng tự động hoá hiện đại
(IR+High Tech) kéo các xí nghiệp công nghiệp đầu tư ở nước ngoài (trước đây vì lý
do lương thợ rẻ mạt) trở về chính quốc (dùng lao động là robot công nghiệp). Chính
phủ các nước này đã áp dụng những biện pháp hỗ trợ hữu hiệu như: coi robot công
nghiệp là ngành công nghiệp quan trọng, xây dựng nhiều chương trình nhà nước về áp
dụng tiến bộ khoa học kỹ thuật vào sản xuất robot. Khuyến khích bằng cách ưu tiên
thuế và đặt ra những quy chế có lợi cho cả người sản xuất và người sử dụng robot công
nghiệp. Nhờ vậy chỉ sau một thời gian ngắn sử dụng robot công nghiệp trở nên rộng
lớn đa dạng với cơ sở nguồn động lực phát triển là “lực đẩy” của công nghệ và “lực
kéo” của thị trường.
Một đặc điểm quan trọng của robot công nghiệp là chúng cho phép dễ dàng kết
hợp những việc phụ và chính của một quá trình sản xuất thành một dây chuyền tự
động. So với các phương tiện tự động hoá khác, các dây chuyền tự động dùng robot có
nhiều ưu điểm hơn như dễ dàng thay đổi chương trình làm việc, có khả năng tạo ra dây
chuyền tự động từ các máy vạn năng, và có thể tự động hoá toàn phần.
Khi sử dụng robot vào các dây chuyền tự động, khâu chuẩn bị kỹ thuật được rút
ngắn đi. Trong khi đó với thời gian từ lúc quyết định phương án đến lúc thiết kế xong
một dây chuyền các máy tự động, một mặt hàng hoặc quy trình công nghệ đó đã có thể
trở thành lạc hậu rồi. Theo số liệu của các chuyên gia Mỹ nghiên cứu về vấn đề này
khi khảo sát trên 70 đề án thiết kế thì với quá nửa số đó là phương án dùng các máy tự
động chuyên dụng phải tốn hơn một năm. Vì thế, phương án dùng robot Unimate với
các máy tự động vạn năng được đưa vào sử dụng và phát huy hiệu quả to lớn.
Kỹ thuật robot có ưu điểm quan trọng nhất là tạo nên khả năng linh hoạt hoá
sản xuất. Việc sử dụng máy tính điện tử, robot và máy điều khiển theo chương trình đã
cho phép tìm được những phương thức mới mẻ để tạo nên các dây chuyền tự động cho
sản xuất hàng loạt với nhiều mẫu, loại sản phẩm. Dây chuyền tự động “cứng” gồm

9. 9
nhiều thiết bị tự động chuyên dùng đòi hỏi vốn đầu tư lớn, tốn nhiều thời gian để thiết
kế và chế tạo, trong lúc quy trình công nghệ luôn luôn cải tiến, nhu cầu đối với chất
lượng và quy cách của sản phẩm luôn luôn thay đổi. Bởi vậy, nhu cầu “mềm” hoá hay
linh hoạt hóa dây chuyền sản xuất ngày càng tăng. Kỹ thuật robot công nghiệp và máy
tính đã đóng vai trò quan trọng trong công việc tạo ra các dây chuyền tự động linh
hoạt.
Xuất phát từ nhu cầu và khả năng linh hoạt hoá sản xuất, trong những năm gần
đây không những chỉ các nhà khoa học mà các nhà sản xuất đã tập trung sự chú ý vào
việc hình thành và áp dụng các hệ sản xuất tự động linh hoạt, gọi tắt là hệ sản xuất linh
hoạt. Hệ sản xuất linh hoạt FMS (Flexible Manufacturing System) ngày nay thường
bao gồm các thiết bị gia công được điều khiển bằng chương trình số, các phương tiện
vận chuyển và kho chứa trong phân xưởng đã được tự động hoá và nhóm robot công
nghiệp ở các vị trí trực tiếp với các thiết bị gia công hoặc thực hiện các nguyên công
phụ. Việc điều khiển và kiểm tra hoạt động của toàn bộ hệ sản xuất linh hoạt được
thực hiện bằng máy tính. Ưu điểm nổi bật của hệ sản xuất linh hoạt là rất thích hợp với
quy mô nhỏ và vừa, thích hợp với yêu cầu luôn luôn thay đổi chất lượng sản phẩm và
quy trình công nghệ. Bởi vậy, ngày nay hệ sản xuất linh hoạt thu hút sự chú ý không
những ở các nước phát triển mà ngay cả ở những nước đang phát triển. Trong một số
tài liệu nước ngoài FMS như là hệ sản xuất của tương lai (Future Manufactring
System).
1.1.3. Định nghĩa về robot công nghiệp
Robot công nghiệp là một máy tự động linh hoạt thay thế từng phần hoặc toàn
bộ hoạt động cơ bắp và hoạt động trí tuệ của con người trong nhiều khả năng thích
nghi khác nhau.
Về mặt cơ khí và điều khiển điện tử, robot công nghiệp là sự tổ hợp khả năng
hoạt động linh hoạt của các cơ cấu điều khiển từ xa với tốc độ phát triển ngày càng cao
của hệ thống điều khiển theo chương trình số cũng như kỹ thuật chế tạo các cảm biến,
công nghệ lập trình và các phát triển của trí tuệ nhân tạo, hệ chuyên gia.
Robot công nghiệp có khả năng chương trình hoá linh hoạt trên nhiều trục
chuyển động, biểu thị cho số bậc tự do của chúng. Robot công nghiệp được trang bị
những bàn tay máy hoặc các cơ cấu chấp hành giải quyết nhưng nhiệm vụ xác định

10. 10
trong các quá trình công nghệ: hoặc trực tiếp tham gia các nguyên công (sơn, hàn, lắp
ráp các cụm thiết bị ...) hoặc phục vụ các quá trình tổ chức dòng lưu thông vật chất
(chi tiết, dao cụ, gá lắp...) với những thao tác cầm nắm, vận chuyển và trao đổi các đối
tượng vật chất với các trạm công nghệ trong một hệ thống máy tự động linh hoạt.
Ta có thể điểm qua một vài định nghĩa về robot công nghiệp như sau:
- Định nghĩa theo tiêu chuẩn AFNOR Fracais:
Robot công nghiệp là một cơ cấu chuyển đổi tự động có thể chương trình hoá,
lặp lại các chương trình, tổng hợp các chương trình đặt ra trên các trục toạ độ, có khả
năng định vị, định hướng, di chuyển các đối tượng vật chất theo những chương trình
thay đổi đã chương trình hoá nhằm thực hiện các nhiệm vụ công nghệ khác nhau.
- Định nghĩa theo tiêu chuẩn VDI 2806/BRD:
Robot công nghiệp là một thiết bị có nhiều trục, thực hiện các chuyển động tự
động có thể chương trình hoá và nối ghép các chuyển động của chúng trong các
khoảng cách tuyến tính hay phi tuyến của động trình. Chúng được điều khiển bởi các
bộ hợp nhất ghép nối với nhau, có khả năng học và nhớ các chương trình, chúng được
trang bị dụng cụ hoặc các phương tiện công nghệ khác nhau để thực hiện các nhiệm vụ
sản xuất trực tiếp và gián tiếp.
- Định nghĩa theo IOTC - 1980:
Robot công nghiệp là một máy tự động liên kết giữa một tay máy và một cụm
điều khiển chương trình hoá, thực hiện một chu trình công nghệ một cách chủ động
với sự điều khiển có thể thay thế những chức năng tương tự của con người.
Bản chất của các định nghĩa trên cho ta thấy một ý nghĩa quan trọng: robot công
nghiệp phải được liên hệ chặt chẽ với máy móc, công cụ và các thiết bị công nghệ tự
động khác trong một hệ thống tự động tổng hợp. Trong quá trình phân tích và thiết kế
không thể quan niệm robot như một đơn vị cấu trúc biệt lập, trái lại đó phải là những
thiết kế tổng thể của “hệ thống tự động linh hoạt robot hoá” cho phép thích ứng
nhanh và đơn giản khi nhiệm vụ sản xuất thay đổi. Theo đó, các mẫu hình robot phải
đảm bảo có:
- Thủ pháp cầm nắm, chuyển đổi tối ưu.

11. 11
- Trình độ hành nghề khôn khéo, linh hoạt.
- Kết cấu phải tuân theo nguyên tắc môdun hoá.
1.2. Các 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 phương pháp đ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.
Chúng ta sẽ lần lượt tìm hiểu các phương pháp điều khiển robot để biết được ưu
nhược điểm của từng phương pháp.
1.2.1. Phương pháp điều khiển động lực học ngược
Nguyên lý của phương pháp này là chọn một luật điều khiển phù hợp để khử
thành phần phi tuyến của phương trình động lực học và phân ly đặc tính động lực học
của các khớp nối.
(t) H(q).q(t) h(q,q) g(q)
    (1.1)
Nếu ta biết các tham số của robot ta có thể tính được các ma trận H, h, g từ đó
có luật điều khiển.
dk H(q)U h(q,q) g(q)
    (1.2)
cân bằng dk
   với điều kiện H(q) 0
 và q U
 (vectơ điều khiển phụ ).
Như vậy động lực học hệ thống kín sẽ được phân tích thành hệ phương trình vi
phân tuyến tính hệ số hằng.

12. 12
q U

Với robot n khớp nối tương đương với n hệ con độc lập.
Chọn U là tín hiệu điều khiển phụ có cấu trúc PID. Lúc đó:
.. t
d D d P d I d
0
U q K (q q) K (q q) K (q q)dt
      
 (1.3)
trong đó: d
q , q là biến khớp đặt và biến khớp thực của khớp.
d
q ,q là tốc độ đặt và tốc độ thực của khớp.
Hình 1.1: Sơ đồ cấu trúc hệ điều khiển động lực học ngược.
Và phương trình sai số tương ứng sẽ là:
... .. .
D P I
K K K 0
     (1.4)
Các hệ số KD, Kp, KI được chọn theo điều kiện ổn định của Lyapunov để sai số
giữa quỹ đạo chuyển động chuẩn và quỹ đạo chuyển động thực hội tụ tại điểm 0 không
phụ thuộc vào điều kiện ban đầu.
Ưu điểm của phương pháp này là khử được tính phi tuyến và sự ràng buộc
trong phương trình động lực học.
Nhược điểm của nó là phải biết được đầy đủ chính xác các thông số cũng như
đặc tính động lực học robot, đồng thời cũng phát sinh tính toán phụ. Thuật toán tính
toán điều khiển U sẽ liên quan các phép tính lượng giác nên phải thực hiện một số
phép nhân ma trận vectơ và ma trận phụ. Thời gian tính toán lớn là một yếu tố ảnh
hưởng đến sự hạn chế của phương pháp này.
Tính
)
q
(
g
)
q
,
q
(
h
U
)
q
(
H 
 

 e
K
q P
d




1
0
I
D dt
)
t
(
e
K
e
K  Robot

U
q
q

e
d
q
d
q


d
q
 e

-
-

13. 13
1.2.2. Phương pháp điều khiển phản hồi phân ly phi tuyến
Phương pháp này được xây dựng trên cơ sở lý thuyết của điều khiển phân ly
cho hệ thống phi tuyến bằng phản hồi tuyến tính hoá tín hiệu ra.
Từ phương trình động lực học:
(t) H(q).q(t) h(q,q) g(q)
    (1.5)
Y Y(q)

Ma trận H không đơn nhất nên ta có thể viết lại như sau:
1 1
q(t) [H(q)] (t) [H(q)] .([h(q,q)] [g(q)])
 
    (1.6)
Phương trình này gồm các phương trình vi phân cấp hai cho mỗi biến, vì lẽ đó
qua hai lần vi phân phương trình đầu ra thì hệ số của tín hiệu U sẽ khác 0. Lúc này tín
hiệu U sẽ xuất hiện trong phương trình đầu ra:
1 1
i i i i
* *
i i
y y(t) [H(q)] (t) [H(q)] .([h(q,q)] [g (q)])
H (X).U(t) g (X)
 
    
 
(1.7)
Với:
* 1
i i
H (X) H(q)] 

* 1
i i i
g (X) [H(q)] .([h(q,q)] [g (q)])

 
T T
T
X q (t),q (t)
 
  
 
 
Tín hiệu (t)
 của bộ điều khiển được chọn sao cho đảm bảo hệ thống phân ly
là:
 
 
 
* * *
-1 *
*
(t) H (X).[g (X) (X) E(t)]
-H(q) -H (q). h(q,q) g(q) (X) E(t)
h(q,q) g(q) -H (X) E(t)

      
     
    
(1.8)
Trong đó:
2
* ( j)
i ij i
j 0
(x) .y

  
 , 1 2 n
diag( , ,... )
     .

14. 14
Từ phương trình (1.8) ta nhận thấy tín hiệu điều khiển i (t)
 cho khớp i chỉ phụ
thuộc vào các biến động lực học và tín hiệu vào E(t).
Thay (t)
 từ phương trình (1.8) vào phương trình (1.5) ta được:
 
*
H(q).q(t) h(q,q) g(q) h(q,q) g(q) -H (X) E(t)
       (1.9)
Hay: i i1 i 0i i i i
q (t) q (t) q (t) e (t)
      (1.10)
Phương trình (1.10) biểu thị vào ra phân ly của hệ thống. Các hệ số 1i 0i i
, ,
  
được chọn theo tiêu chuẩn ổn định.
1.2.3. Phương pháp điều khiển thích nghi theo sai lệch
Dựa trên cơ sở lý thuyết sai lệch, Lee và Chung đã đề xuất thuật toán điều
khiển đảm bảo robot luôn bám quỹ đạo chuyển động đặt trước với phạm vi
chuyển động rộng và tải thay đổi rộng. Phương pháp điều khiển thích nghi theo
sai lệch được xây dựng trên cơ sở phương trình sai lệch tuyến tính hoá lân cận
quỹ đạo chuyển động chuẩn. Hệ thống điều khiển gồm hai khối: Khối tiền định
(truyền thẳng - feedforward) và khối phản hồi (feedback) như Hình 1.2.
Khối tiền định tính toán mô men của robot ứng với quỹ đạo chuyển động
chuẩn theo phương trình Newton-Euler. Khối phản hồi thực hiện tính toán thành
phần mômen sai lệch theo luật tối ưu một nấc nhằm bù sai lệch vị trí và tốc độ của
khớp dọc theo quỹ đạo chuyển động chuẩn. Khối đánh giá tham số thực hiện theo
sơ đồ nhận dạng bình phương tối thiểu thời gian thực đệ quy các tham số và hệ số
phản hồi của hệ tuyến tính hoá được cập nhật và chỉnh định ở mỗi chu kỳ mẫu.
Hình 1.2: Hệ thống điều khiển thích nghi theo sai lệch.
Đối tượng
điều khiển
Quỹ đạo chuyển
động chuẩn
Phương trình
Neuton-Euler
xm(t)
+
-
Bộ điều khiển
tối ưu một nấc
+
+ x(t)
Hệ thống nhận dạng
bình phương tối thiểu
đệ quy

15. 15
Mômen tổng đặt lên cơ cấu chấp hành sẽ gồm hai thành phần: mômen danh định
được tính theo phương trình Newton-Euler từ khối tiền định và mômen bù sai lệch
sẽ được tính bởi khối phản hồi thực hiện theo luật tối ưu một cấp.
Phương pháp điều khiển thích nghi theo sai lệch có hai ưu điểm cơ bản: Nó
cho phép chuyển từ vấn đề điều khiển phi tuyến về điều khiển tuyến tính quanh
quỹ đạo chuẩn. Việc tính toán mômen danh định cũng như mômen sai lệch được
thực hiện độc lập và đồng thời.
Tuy nhiên phương pháp này gặp khó khăn do khối lượng tính toán quá lớn
và do đó thời gian tác động sẽ chậm, khó tối ưu trong việc điều khiển robot.
1.2.4. Phương pháp điều khiển thích nghi theo mô hình mẫu
Trong số các phương pháp điều khiển thích nghi (điều khiển thích nghi thông
qua điều chỉnh hệ số khuếch đại, điều khiển thích nghi tự chỉnh, điều khiển thích nghi
theo mô hình chuẩn) thì phương pháp điều khiển thích nghi theo mô hình chuẩn
(Model Reference Adaptive Control - MRAC) được sử dụng rộng rãi nhất và tương
đối dễ thực hiện. Nguyên lý cơ bản của điều khiển thích nghi theo mô hình chuẩn dựa
trên sự lựa chọn thích hợp mô hình chuẩn và thuật toán thích nghi. Thuật toán thích
nghi được tính toán dựa trên tín hiệu vào là sai lệch giữa đầu ra của hệ thống thực và
mô hình chuẩn từ đó đưa ra điều chỉnh hệ số khuếch đại phản hồi sao cho sai lệch đó
là nhỏ nhất. Sơ đồ khối chung của hệ thống điều khiển thích nghi theo mô hình chuẩn
được trình bày trên Hình 1.3.
Phương pháp điều khiển thích nghi theo mô hình chuẩn có một số ưu điểm quan
trọng là nó không bao gồm mô hình toán học phức tạp và không phụ thuộc vào tham
Cơ cấu điều
chỉnh
Đối tượng
điều khiển
Mô hình
chuẩn
Tín hiệu
đặt
Bộ điều
chỉnh
e
+
-
Đầu ra y
ym
Hình 1.3: Hệ thống điều khiển thích nghi theo mô hình chuẩn.

16. 16
số môi trường... Tuy nhiên, phương pháp này chỉ thực hiện được cho mô hình đơn giản
tuyến tính với giả thiết bỏ qua sự liên hệ động lực học giữa các khớp của robot. Hơn
nữa sự ổn định của hệ thống kín cũng là một vấn đề khó giải quyết với tính phi tuyến
cao của mô hình động lực học robot.
1.2.5. Phương pháp điều khiển động lực học ngược thích nghi
Là phương pháp tổng hợp các kỹ thuật nhằm tự động chỉnh định các bộ
điều chỉnh trong mạch điều khiển nhằm thực hiện hay duy trì ở một mức độ nhất
định chất lượng của hệ khi thông số của quá trình được điều khiển không biết
trước hoặc thay đổi theo thời gian. Việc phân tích các hệ thống điều khiển có chất
lượng cao luôn là vấn đề trọng tâm trong quá trình phát triển của lý thuyết điều
khiển tự động nói chung và vấn đề nâng cao chất lượng hệ thống điều khiển bám
chính xác quỹ đạo chuyển động của robot nói riêng. Tùy thuộc vào các tiêu chuẩn
phân loại mà có các hệ điều khiển thích nghi khác nhau: Hệ có tín hiệu tìm hay
không có tín hiệu tìm; hệ điều khiển trực tiếp hay gián tiếp; hệ cực trị hay hệ giải
tích; hệ có mô hình mẫu hay hệ không có mô hình mẫu; hệ tự chỉnh hay hệ tự tổ
chức vv... đang được phát triển và và áp dụng để tổng hợp các hệ thống điều khiển
quỹ đạo với chỉ tiêu chất lượng cao. Phương pháp tổng quát hóa các hệ thích nghi
có ý nghĩa rất lớn trong việc bao quát một số lượng lớn các bài toán thích nghi,
đơn giản được việc tìm hiểu nguyên lý cơ bản của ngay cả các hệ phức tạp, trên
cơ sở đó xây dựng các bài toán mới, các thiết bị cụ thể mới.
Vấn đề điều khiển bám chính xác quỹ đạo robot là một vấn đề luôn nhận
được sự quan tâm chú ý. Hiện nay sự phát triển mạnh mẽ của kỹ thuật về phần
cứng và phần mềm đã cho phép giảm thời gian tính toán, điều đó dẫn tới những
Đối tượng điều
khiển
Cơ cấu điều
khiển
Cơ cấu
thích nghi Nhận dạng
x


u
y
Hình 1.4: Sơ đồ khối tổng quát hệ thích nghi.

17. 17
động lực cho việc thúc đẩy sự phát triển của các hệ thống điều khiển quỹ đạo
thích nghi cho robot.
1.2.6. Phương pháp điều khiển trượt
Đ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 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).

18. 18
CHƯƠNG II
MÔ TẢ ĐỐI TƯỢNG - ROBOT SCARA SERPENT
2.1. Một số loại robot Scara của các hãng sản xuất
Robot Scara là một trong những robot phổ biến nhất trong công nghiệp. Chuyển
động của robot này rất đơn giản nhưng lại phù hợp với các dây chuyền và ứng dụng
hữu hiệu trong nhiệm vụ nhặt và đặt sản phẩm. Robot Scara (Selectively Compliant
Articulated Robot Arm) có nghĩa là tay máy lắp ráp chọn lọc.
Cấu trúc động học loại tay máy này thuộc hệ phỏng sinh, có các trục quay, các
khớp đều là thẳng đứng. Nó có cấu tạo hai khớp ở cánh tay, một khớp ở cổ tay và một
khớp tịnh tiến. Các khớp quay hoạt động nhờ động cơ điện có phản hồi vị trí. Khớp
tịnh tiến hoạt động nhờ xi-lanh khí nén, trục vít hoặc thanh răng.
Một số loại robot Scara của các hãng sản xuất:
Hình 2.1a: Turbo Scara SR60 của hãng Bosch. Hình 2.1b: Assembly Scara Robot của Hirata.
Hình 2.1c: Scara Robot của EPSON. Hình 2.1d: Scara Robot của DENSO.

19. 19
Hình 2.1e: Scara Robot of ADEPT. Hình 2.1f: Scara Robot of RANOME.
Hình 2.1g: Scara Robot of KUKA. Hình 2.1h: Scara Robot of STAUBLI.
Hình 2.2: Robot Scara Serpent của FEEDBACK.

20. 20
Trong phần này luận văn sẽ xây dựng mô tả toán học đối tượng robot Scara
Serpent của FEEDBACK.
2.2. Các thông số và vùng làm việc của robot Scara Serpent
Do chuyển động của robot Scara đơn giản, dễ dàng nên nó được sử dụng khá
phổ biến trong công nghiệp. Ở đây nghiên cứu robot Scara Serpent (Hình 2.3) một
loại cơ bản trong nhóm robot công nghiệp này.
Hình 2.3: Robot Scara Serpent.
Chiều cao của Robot có thể thay đổi dễ dàng bằng cách thay đổi vị trí gá thân
robot trên trục cơ bản, giúp tay máy thuận lợi trong việc thay đổicông việc.
Với thiết kế động cơ truyền động cho cổ tay được đặt trên trục cơ bản và liên hệ
với cổ tay bằng đai truyền, nên nó đảm bảo được góc quay của cổ tay không thay đổi
trong quá trình tay máy chuyển động.
Truyền động cho 2 khớp của tay máy và cổ tay bằng động cơ servo một chiều
có phản hồi vị trí tạo thành một vòng điều khiển kín. Chuyển động thẳng đứng được
thực hiện bằng piton khí nén.
Robot Scara Serpent có thể được lập trình từ máy tính bằng cách đặt dữ liệu cho
mỗi trục. Hoặc điều khiển bằng tay sử dụng thiết bị lái điện (steering) cho tay máy
dùng các cuộn dây điện từ trong giá treo (pendant).

21. 21
2.2.1. Cấu tạo tay máy robot Scara Serpent
2.2.1.1. Cấu hình của robot Scara Serpent
Bao gồm một chuỗi các thanh cứng được liên kết với nhau bởi các khớp:
Hình 2.4: Cấu hình và các hệ trục tọa độ gắn trên Robot Scara Serpent.
Robot Scara Serpent gồm 3 khớp chuyển động quay và một khớp chuyển động
tịnh tiến. Gắn cho mỗi thanh nối một hệ trục toạ độ, ta có:
- Khớp 1 quay quanh trục z0 góc θ1.
- Khớp 2 quay quanh trục z1 góc θ2.
- Khớp 3 chuyển động tịnh tiến theo trục z2 đoạn d3.
- Khớp 4 quay quanh trục z3 góc θ4.
2.2.1.2. Các thông số kỹ thuật của robot Scara Serpent
Thông số của động cơ 1, 2, 3 tương ứng với các khớp 1, 2, 4 của tay máy robot
Scara Serpent (xem bảng 2.1).
- Động cơ 1 truyền động cho khớp 1 (main).
- Động cơ 2 truyền động cho khớp 2 (fore).
- Động cơ 3 truyền động cho khớp 4 - khớp cổ tay (wrist).

22. 22
Bảng 2.1: Thông số các động cơ của robot.
TT Loại U(V) I(A) M(Nm) N(v/p) P(W) J(Kg.m2) R() L(H) m(Kg)
1 J9ZF 12 4,8 4.10-2 2100 15 0,32.104 1,38 100 0,6
2 J9ZF 12 4,8 4.10-2 2100 15 0,32.104 1,38 100 0,6
3 J12ZF 12 4,8 1,2102 2100 26 1,5.10-4 0,95 100 1
Bảng 2.2: Các thông số động học của robot Scara Serpent.
TT Thông số Kích thước động học
1 m1 = 4 Kg Khối lượng thanh nối 1.
2 m2 = 1.5 Kg Khối lượng thanh nối 2.
3 m3 = 2 Kg Khối lượng thanh nối 3.
4 m4 = 0.6 Kg Khối lượng thanh nối 4.
5 a1 = 0.25 m Chiều dài thanh nối giữa 2 khớp main và fore .
6 a2 = 0.15 m Chiều dài thanh nối giữa 2 khớp fore và cổ tay .
7 d3 Chiều dài thanh nối d3 phụ thuộc vào chế độ làm việc của tay máy.
2.2.2. Giới hạn không gian làm việc của robot Scara Serpent
Các biến khớp có các giới hạn góc quay như sau :
1 = -96 0  960 ( so với trục Ox ).
2 = -115 0  1150 (so với trục thanh 1).
Chuyển động quay của khớp thứ nhất có hình chiếu bằng trong hệ trục toạ độ
OX0Y0 và OX1Z1 (Hình 2.5) tương ứng với góc quay tổng trong thực tế là 1920.
Chuyển động quay của khớp thứ hai có hình chiếu bằng trong hệ trục toạ độ OX1Y1 và
OX2Y2 tương ứng với góc quay tổng là 2300. Từ đó ta có thể thấy được hình chiếu giới
hạn không gian làm việc của nó (Hình 2.6).

23. 23
Hình 2.5: Giới hạn góc quay của 2 khớp.
Hình 2.6: Giới hạn không gian làm việc của robot Scara Serpent.
Như vậy khoảng không gian mà tay máy có thể với tới là toàn bộ hình trụ với
đáy có đường giới hạn bên trong là một cung tròn có bán kính r = 0.231 (m) và
đường giới hạn bên ngoài là đường tròn bán kính R = 0.4 (m).

24. 24
Khi biết được vị trí nào mà tay máy có thể đến được chúng ta có thể lập trình
trong Matlab để tìm vị trí, quỹ đạo nào mà tay máy có thể vươn tới (được xét đến ở
chương III trong bản luận văn này).
2.3. Động học robot Scara Serpent
Robot Scara Serpent có cấu trúc động học được biểu diễn như trên Hình 2.4.
Robot có 3 trục quay và 1 bàn kẹp, tuy nhiên ba khớp động đầu tiên được gọi là bộ
phận cơ bản vì trước hết, nhờ chúng tay máy có thể thực hiện bước chủ yếu trong thao
tác định vị, tức là đưa bàn kẹp đến lân cận điểm làm việc, sau đó nhờ khớp động còn
lại bàn kẹp được định hướng và vi chỉnh đến vị trí gia công chính xác.
2.3.1. Động học thuận
Việc xây dựng các phương trình động học thuận của robot được tiến hành tuần
tự theo các bước sau:
Bước 1: Xác định các hệ toạ độ
Ta sử dụng quy ước Denavit-Hartenberg để mô tả đầy đủ vị trí của của toàn
thân robot công nghiệp. Hình 2.4 mô tả các hệ trục toạ độ gắn với các khúc tay của
robot Scara Serpent.
Bước 2: Xây dựng bảng thông số DH
Bảng 2.3: Tham số Denavit – Hartenberg của robot Scara Serpent.
Thanh nối i (0) ai i(rad) di (m) Biến Chuyển động
1 0 a1 1 0 1 Quay
2 -1800 a2 2 0 2 Quay
3 0 0 0 d3 d3 Tịnh tiến
4 0 0 4 0 4 Quay
Khảo sát với 3 trục khớp quay đầu tiên tương ứng với quỹ đạo của khớp quay 4
trong mặt phẳng OX0Y0. Ma trận T4 là ma trận biểu diễn tay máy robot trong hệ trục
tọa độ gốc: T4= A1.A2.A3.A4

25. 25
An=
n n n n n n n
n n n n n n n
n n n
cos sin cos sin sin a cos
sin cos cos cos sin a sin
0 sin cos d
0 0 0 1
      
 
 
      
 
 
 
 
 
(2.1)
Thay số liệu trong bảng tham số có:
1 1 1 1
1 1 1 1
1
cos sin 0 a .cos
sin cos 0 a sin
A
0 0 1 0
0 0 0 1
   
 
 
  
 

 
 
 
2 2 2 2
2 2 2 2
2
cos sin 0 a .cos
sin cos 0 a sin
A
0 0 1 0
0 0 0 1
  
 
 
   
 

 

 
 
3
3
1 0 0 0
0 1 0 0
A
0 0 1 d
0 0 0 1
 
 
 

 
 
 
4 4
4 4
4
cos sin 0 0
sin cos 0 0
A
0 0 1 0
0 0 0 1
  
 
 
 
 

 
 
 
 Ký hiệu: S1  Sin1 ; C1  Cos1
S2  Sin2 ; C2  Cos2
S4  Sin4 ; C4  Cos4
S12  Sin(1+2); C12  Cos(1+2)

26. 26
 Các bước tính toán:
Bước 1: 3
4 4
T A

4 4
4 4
3
4 4
C S 0 0
S C 0 0
T A
0 0 1 0
0 0 0 1

 
 
 
 
 
 
 
Bước 2: 2 3
4 3 4
T A . T

4 4 4 4
4 4 4 4
2
4
3 3
1 0 0 0 C S 0 0 C S 0 0
0 1 0 0 S C 0 0 S C 0 0
T
0 0 1 d 0 0 1 0 0 0 1 d
0 0 0 1 0 0 0 1 0 0 0 1
 
     
     
     
 
     
     
     
Bước 3: 1 2
4 2 4
T A . T

2 2 2 2 4 4
2 2 2 2 4 4
1
4
3
2 4 2 4 2 4 2 4 2 2
2 4 2 4 2 4 2 4 2 2
1
4
3
C S 0 a .C C S 0 0
S C 0 a .S S C 0 0
T
0 0 1 0 0 0 1 d
0 0 0 1 0 0 0 1
C C S S C S S C 0 a .C
S C C S S S C C 0 a .S
T
0 0 1 d
0 0 0 1

   
   

   

   

   
   
  
 
 
  
 

 
 
 
 
Bước 4: 4
1
1
4
0
4 T
.
A
T
T 

1 1 1 1 2 4 2 4 2 4 2 4 2 2
1 1 1 1 2 4 2 4 2 4 2 4 2 2
0
4
3
C -S 0 a .C C C S S S C C S 0 a .C
S C 0 a .S S C C S (C C S S ) 0 a .S
T
0 0 1 0 0 0 1 d
0 0 0 1 0 0 0 1
 
   
   
  
   
 
   
 
   
   
C (C C S S ) S (S C C S ) C (S C C S ) S (C C S S ) 0 a C a C
1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 12 1 1
S (C C S S ) C (S C C S ) S (S C C S ) C (C C S S ) 0 a S a S
1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 12 1 1
0 0 1 d3
0 0 0 1
      
      
 
 
 
 
 
 
(2.2)

27. 27
Ma trận 0T4 biểu diễn tay máy robot trong hệ toạ độ gốc.
Mặt khác theo ký hiệu tổng quát:
x x x x
y y y y
E 4
z z z z
n o a p
n o a p
T T
n o a p
0 0 0 1
  (2.3)
Với: p
a
o
n




,
,
, lần lượt là các vectơ định vị, vectơ định hướng, vectơ tới và
vectơ vị trí để biểu diễn hướng và vị trí của tay máy trong không gian làm việc.
Từ ma trận trên ta có hệ phương trình động học thuận tay máy robot:
nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4) (2.4)
ny = S1(C2 C4 + S2S4) + C1(S2C4 – C2S4) (2.5)
nz = 0 (2.6)
ox = C1(S2C4- C2 S4) + S1(C2C4 + S2S4) (2.7)
oy = S1(S2C4 - C2 S4) - C1(C2C4 + S2S4) (2.8)
oz = 0 (2.9)
ax = 0 (2.10)
ay = 0 (2.11)
az = -1 (2.12)
Và hệ phương trình xác định vị trí của điểm tác động cuối như sau:
x = px = a1.C1 + a2.C12 (2.13)
y = py = a1.S1 + a2.S12 (2.14)
z = pz = - d3 (2.15)
2.3.2. Động học ngược
Động học ngược: xác định các biến khớp khi biết vị trí tay.
Từ phương trình động học thuận có:
2 2 2 2 2
x 1 1 2 12 1 2 1 12
2 2 2 2 2
y 1 1 2 12 1 2 1 12
p a .C a .C 2.a .a .C .C
p a .S a .S 2.a .a .S .S
  
  

28. 28
2 2 2 2 2 2 2 2
X Y 1 1 1 2 12 12 1 2 1 12 1 12
p p a .(S C ) a .(S C ) 2.a .a .(S .S C .C )
      
2 2 2 2
x y 1 2 1 2 2
p p a a 2.a .a .C
     (2.16)
Do đó:
2 2 2 2
X Y 1 2
2
1 2
2
2 2
p p a a
cosθ
2.a .a
sinθ (1 cos θ )
  

  
(2.17)
Từ đó tính được góc 2:
2= atan2(sin2,cos2) (2.18)
Thế C1, S1 vào phương trình (2.13) và (2.14) thu được:
(a1+a2C2).C1 – a2S2.S1 = px
a2S2.C1 +( a1+a2C2).S1 = py
Giải phương trình bậc nhất với ẩn C1, S1 và sử dụng (2.16) thu được :
1 2 2 x 2 2 y
x
1 2 2
x y
y 1 2 2 y 2 2 x
1 2 2
x y
(a a C ).p a S .p
C
p p
(a a C ).p a S .p
S
p p
 

 
 
  
 
 
(2.19)
1= atan2(S1,C1) (2.20)
Từ phương trình (2.15) ta có:
d3 = - pz (2.21)
Mặt khác từ phương trình (2.4) có:
nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4) (2.22)
Rút gọn theo các công thức lượng giác thu được:
nX = cos1.cos(2-4) – sin1.sin(2-4) = cos(1+2-4) (2.23)
sin(1+2-4) =
2
x
1 n
 (2.24)
2
1 2 4 x x
(θ θ θ ) atan2( 1 n ,n )
   
2
4 1 2 x x
θ θ θ atan2( 1 n ,n )
    (2.25)

29. 29
Vậy hệ phương trình động học ngược của robot Scara Serpent là:
 
 
1 2 2 x 2 2 y
1 2 2
x y
1 2 2 y 2 2 x
1 2 2
x y
1 1 1
2 2 2 2
X Y 1 2
2
1 2
2
2 2
2 2 2
3 z
2
4 1 2 x x
(a a C ).p a S .p
C
p p
(a a C ).p a S .p
S
p p
atan2 S ,C
p p a a
cosθ
2.a .a
sinθ (1 cos θ )
atan2 S ,C
d p
θ θ θ atan2( 1 n ,n )
 


 

  





 

   
 


   

 

  

    

(2.26)
2.4. Động lực học robot Scara Serpent
Để mô tả mối quan hệ giữa lực, mômen với vị trí, vận tốc và gia tốc của đối
tượng robot, cần phải xây dựng được phương trình động lực học, từ đó phục vụ cho
công việc thiết kế và điều khiển robot. Vì vậy cần phải tính toán đầy đủ các thông số
của đối tượng trước khi đưa vào mô phỏng.
Phương trình động lực học của robot được biểu diễn như sau:
H(q)q h(q,q) g(q) (t)
    (2.27)
Trong đó:
- (t) - Vectơ [n x 1] lực động tạo nên ở n khớp động:
(t) = [1(t), 2(t), ..., n(t)]T (2.28)
- q(t) - Vectơ [n x 1] biến khớp:
q(t) = [q1(t), q2(t), ... qn(t)]T. (2.29)
- H(q) - Ma trận [n x n], có các phần tử Hik sau đây:
 
n
T
jk ij i ik
i max( j,k)
H Trace U I U

  . (j, k =1, 2, ...n). (2.30a)

30. 30
0
i
ij
j
T
U
q



(2.30b)
- h( q, q
 ) - Vectơ [n x 1] lực ly tâm và Coriolit:
h( q, q
 ) = [h1, h2, ... , hn]T. (2.31)
n n
j jkm k m
k 1 m 1
h h q q
 
  . (j =1, 2, ...n). (2.32)
 

 T
ij
i
ikm
jkm U
I
U
Trace
h . (2.33)
m
k
i
0
2
ikm
q
q
T
U



 (2.34)
- g(q) - Vectơ [n x 1] lực trọng trường:
g(q) = [g1, g2, ... , gn]T. (2.35)
 
n
i
j j ij
i j
g m gU r

 
 . (2.36)
Với robot Scara Serpent gồm 4 chuyển động (3 chuyển động quay và 1 chuyển
động tịnh tiến) và mô phỏng với 3 chuyển động quay có các phương trình cụ thể sau:
Vectơ [4 1] lực động: (t) = [1(t), 2(t), 3(t), 4(t)]T.
Vectơ [41] biến khớp: q(t) = [q1(t), q2(t), d3(t), q4(t)]T.
2.4.1. Hàm Euler - Lagrange và các vấn đề động lực học
Lagrange định nghĩa sự khác biệt giữa động năng và thế năng của hệ thống:
L = K – P (2.37)
Trong đó: K : là động năng của hệ thống.
P : là tổng thế năng của hệ thống.
Do đó phương trình động lực học được xác định bằng biểu thức:
i
i i
d L L
dt q q
 
  
 
(2.38)
Trong đó: qi = i đối với khớp quay; qi = ri đối với khớp tịnh tiến.
i : mômen lực động tại khớp thứ i.

31. 31
Động năng của khớp thứ i:
2 2
i i i i i
1 1
K m .v J .
2 2
   (2.39)
Với Ji là mômen quán tính của khớp thứ i.
2.4.2. Động lực học robot Scara Serpent
Theo cấu hình, robot Scara Serpent có các thông số ở Bảng 2.4:
Bảng 2.4: Thông số của robot Scara Serpent.
Khớp 1 Khớp 2 Khớp 3 Khớp 4
Biến 1 2 d3 4
Chiều dài a1 a2 d3 0
Khối lượng m1 m2 m3 m4
Vận tốc v1 v2 v3 v4
Chiều dài tâm khối lg1 lg2 lg3 lg4
Giả sử khối lượng nằm ở đầu mút các thanh nối, ta có chiều dài tâm khối chính
là chiều dài của thanh nối: lg1 = a1 lg2 = a2 lg3 = d3
Ký hiệu chiều dài các khớp: a1 = l1 a2 = l2 d3 = l3
Hệ qui chiếu gắn với trục toạ độ (O0x0y0z0) trên khớp thứ nhất. Khi đó mặt
phẳng (O0x0y0) là mặt phẳng đẳng thế.
2.4.2.1. Tính toán động năng và thế năng cho từng khớp
 Khớp 1:
1 g1 1
1 g1 1
1
x l .cosθ
y l .sinθ
z 0








.
1 g1 1 1
.
1 g1 1 1
.
1
x l .sinθ .θ
y l .cosθ .θ
z 0
  





 


32. 32
Trong đó: x1, y1, z1 là hình chiếu của thanh nối số 1 lên các trục x, y, z.
2 2 2
1 1 1 1
2 2
1 g1 1 1 g1 1 1
2 2 2
1
1 g1
v x y z
v ( l .sinθ .θ ) (l .cosθ .θ )
v l .θ
  
  

Trong đó: J1 momen quán tính khớp 1.
1
2 2
1 1 1 1 1
. .
2 2 2
1 1 g1 1 1 1
P 0
1 1
K m .v J .
2 2
1 1
K m .l .θ J .θ
2 2

  
 
(2.40)
 Khớp 2:












0
z
)
θ
θ
.sin(
l
θ
.sin
l
y
)
θ
θ
.cos(
l
θ
.cos
l
x
2
2
1
g2
1
1
2
2
1
g2
1
1
2

















0
z
)
θ
θ
).(
θ
θ
.cos(
l
θ
.
θ
.cos
l
y
)
θ
θ
).(
θ
θ
.sin(
l
θ
.
θ
.sin
l
x
2
2
1
2
1
g2
1
1
1
2
2
1
2
1
g2
1
1
1
2









   
2
2
1
2
1
g2
1
2
2
1
2
g2
2
1
2
1
2
2
2
2
1
2
1
g2
1
1
1
2
2
1
2
1
g2
1
1
1
2
2
2
2
2
2
2
2
2
θ
)cos
θ
.
θ
θ
.(
.l
2.l
)
θ
θ
(
.
l
θ
.
l
v
)
θ
θ
).(
θ
θ
.cos(
l
θ
.
θ
.cos
l
)
θ
θ
).(
θ
θ
.sin(
l
θ
.
θ
.sin
l
v
z
y
x
v
































2 2
2 2 2 2 2
. .
2 2 2 2 2 2
2 2 1 1 g2 1 2 1 g2 1 1 2 2 2 2
2
1 1
K m .v J .
2 2
1 1
K m . l .θ l .(θ θ ) 2.l .l .(θ θ .θ ).cosθ J .θ
2 2
P 0
  
 
     
 
 

(2.41)
 Khớp 3:












3
3
2
1
2
1
1
3
2
1
2
1
1
3
-l
z
)
θ
.sin(θ
l
.sinθ
l
y
)
θ
.cos(θ
l
.cosθ
l
x

33. 33
. . . .
3 1 1 1 2 1 2 1 2
. . . .
3 1 1 1 2 1 2 1 2
. .
3 3
x l .sinθ .θ l .sin(θ θ ).(θ θ )
y l .cosθ .θ l .cos(θ θ ).(θ θ )
z l
     


   


 


.
2
3
.
2
.
1
.
2
1
2
1
2
.
2
.
1
2
2
.
2
1
2
1
.
2
3
.
2
3
2
.
2
.
1
2
1
2
.
1
1
1
2
.
2
.
1
2
1
2
.
1
1
1
2
3
.
2
3
.
2
3
.
2
3
3
l
)
θ
.
θ
θ
.(
.l
2.l
)
θ
θ
(
.
l
θ
.
l
v
l
)
θ
θ
).(
θ
θ
.cos(
l
θ
.
θ
.cos
l
)
θ
θ
).(
θ
θ
.sin(
l
θ
.
θ
.sin
l
v
z
y
x
v































.
2
3 3 3
. . .
. . . .
2 2 2 2 2 2
3 3 1 1 2 1 2 1 2 1 1 2 3
3 3 3 3 3 3
1
K m .v
2
1
K m . l .θ l .(θ θ ) 2.l .l .(θ θ .θ ) l
2
P m .g.h P m .g.l

 
     
 
 
   
(2.42)
 Khớp 4:












3
4
2
1
2
1
1
4
2
1
2
1
1
4
-l
z
)
θ
.sin(θ
l
.sinθ
l
y
)
θ
.cos(θ
l
.cosθ
l
x


















3
.
4
.
2
.
1
2
1
2
.
1
1
1
.
4
.
2
.
1
2
1
2
.
1
1
1
.
4
l
z
)
θ
θ
).(
θ
θ
.cos(
l
θ
.
θ
.cos
l
y
)
θ
θ
).(
θ
θ
.sin(
l
θ
.
θ
.sin
l
x

. . .
2 2 2
4 4 4 4
2 2 .
. . . . . .
2 2
4 1 1 1 2 1 2 1 2 1 1 1 2 1 2 1 2 3
. . . .
. . . .
2 2 2 2 2 2 2
4 1 1 2 1 2 1 2 1 1 2 3
v x y z
v l .sinθ .θ l .sin(θ θ ).(θ θ ) l .cosθ .θ l .cos(θ θ ).(θ θ ) l
v l .θ l .(θ θ ) 2.l .l .(θ θ .θ ) l
  
   
         
   
   
     

34. 34
.
2 2
4 4 4 4 4
. . .
. . . . . . .
2 2 2 2 2 2 2
4 4 1 1 g2 1 2 1 g2 1 1 2 2 3 4 1 2 4
4 4 4
4 4 3
1 1
K m .v J .w
2 2
1 1
K m . l .θ l .(θ θ ) 2.l .l .(θ θ .θ )cosθ l J (θ θ θ )
2 2
P m .g.h
P m .g.l
 
 
        
 
 

   
2.43
Tổng động năng của hệ thống:
K = K1 + K2 + K3 + K4 (2.44)
Đặt tâm khối tại đầu mút của thanh nối, ta có: lg1= l1; lg2= l2; lg3= l3
.
2 2 2
1 2 3 4 1 2 3 4 2 1 2 4 1
2 2
2 3 4 1 2 2 1 2 3 4 2 2 4 1 2
3 2
2 3 4 1 2 2 1 2 2 3 4 2 2 4 2
3 4
1
K (m m m m ).l (m m m ).l J J J .θ ...
2
(m m m ).l .l .cosθ .θ (m m m )l J J .θ .θ ...
1
(m m m ).l .l .cosθ .θ .θ (m m m ).l J J .θ ...
2
1
(m m ).
2
 
          
 
 
        
 
 
        
 
  2 2
3 4 4 4 1 4 4 2 4
1
l .J .θ J .θ .θ J .θ .θ
2
  
(2.45)
Tổng thế năng của hệ thống:
P = P1+ P3+ P3+ P4 = -(m3+ m4).g.l3 (2.46)
2.4.2.2. Phương trình động lực học
Lagrange của hệ thống:
2 2 2
1 2 3 4 1 2 3 4 2 1 2 4 1
2
2 3 4 1 2 2 1
2
2 3 4 2 2 4 1 2
2 3 4 1 2 2 1 2
2 2
2 3 4 2 2 4 2
L K P
1
L [(m m m m ).l (m m m ).l J J J ].θ ...
2
(m m m ).l .l .Cosθ .θ ...
[(m m m ).l J J ].θ .θ ...
(m m m )l .l .Cosθ .θ .θ ...
1
[(m m m ).l J J ].θ ...
2
1
(
2
 
          
   
     
   
     
 2 2
3 4 3 4 4 4 2 4 4 1 4 3 4 3
1
m m )l J .θ J .θ .θ J .θ .θ (m m ).g.l
2
     
(2.47)

35. 35
Các phần tử của phương trình động lực học:
i
i i
d L L
dt q q
 
  
 
(2.48)
 Khớp 1:
1
1 1
d L L
dt θ θ
 
  
 
(2.49)
a.
.
2 2
1 2 3 4 1 2 3 4 2 1 2 4 1
.
1
.
.
2
2 3 4 1 2 2 1 2 3 4 2 2 4 2
. .
2 3 4 1 2 2 2 4 4
L
(m m m m ).l (m m m ).l J J J .θ ...
θ
2.(m m m ).l .l .cosθ .θ (m m m )l J J .θ ...
(m m m ).l .l .cosθ ..θ J .θ

 
          
 

 
        
 
   
b.
..
2 2
1 2 3 4 1 2 3 4 2 1 2 4 1
.
1
.. . .
2 3 4 1 2 2 1 2 3 4 1 2 2 1 2
.. ..
2
2 3 4 2 2 4 2 2 3 4 1 2 2 2
2 3
d L
(m m m m ).l (m m m ).l J J J .θ ...
dt θ
2.(m m m ).l .l .cosθ .θ 2.(m m m ).l .l .sinθ .θ .θ ...
(m m m )l J J .θ (m m m ).l .l .cosθ .θ ...
(m m

 
          
 

      
 
        
 
  
. ..
2
4 1 2 2 2 4 4
m ).l .l .sinθ .θ J .θ

(2.50)
c.
0
θ
L
1



(2.51)
 Khớp 2:
2 .
2
2
d L L
dt θ
θ
 
  


(2.52)
a. 2
2 3 4 2 2 4 1 2 3 4 1 2 2 1
2
2
2 3 4 2 2 4 2 4 4
L
[(m m m )l J J ].θ (m m m )l .l .Cosθ .θ
θ
[(m m m )l J J ].θ J .θ

        

     

36. 36
b. 2
2 3 4 2 2 4 1 2 3 4 1 2 2 1
2
2 3 4 1 2 2 1 2
2
2 3 4 2 2 4 2 4 4
d L
[(m m m )l J J ].θ (m m m )l .l .Cosθ .θ
dt θ
[(m m m )l .l .Sinθ .θ .θ
[(m m m )l J J ].θ J .θ

        

   
     
(2.53)
c. 2
2 3 4 1 2 2 1 2 3 4 1 2 2 1 2
2
L
[(m m m )l .l ( Sinθ ).θ (m m m )l .l .Sinθ .θ .θ
θ

      

(2.54)
 Khớp 3:
3
3 3
d L L
dt l l
 
  
 
(2.55)
a. 3
4
3
3
l
).
m
(m
l
L 





b.
..
3
4
3
.
3
l
).
m
m
(
l
L
dt
d




(2.56)
c. ).g
m
(m
l
L
4
3
3




(2.57)
 Khớp 4:
4 .
4
4
d L L
dt θ
θ
 
  


(2.58)
a.
.
4
4.
.
2
4.
.
1
4.
.
4
θ
J
θ
J
θ
J
θ
L





b.
.. .. ..
4. 1 4. 2 4. 4
.
4
d L
J θ J θ J θ
dt θ

  

(2.59)
c.
4
L
0



(2.60)
Như vậy phương trình động lực học của hệ thống được biểu diễn bằng các
phương trình sau:

37. 37
2 2
1 1234 1 234 2 124 234 1 2 2 1
2
234 2 24 234 1 2 2 2 4 4
2
234 1 2 2 2 234 1 2 2 1 2
[m .l m .l J 2.m .l .l .Cosθ ].θ ...
(m .l J m .l .l .Cosθ ).θ J .θ ...
.m .l .l .Sinθ .θ 2m .l .l .Sinθ .θ .θ
     
    
 
(2.61)
2
1
2
2
1
234
4
4
2
24
2
2
234
1
2
2
1
234
24
2
2
234
2
θ
.
θ
Sin
.
l
.
l
.
m
θ
.
J
θ
).
J
l
.
m
(
θ
].
θ
Cos
.
l
.
l
.
m
J
l
.
m
[
















(2.62)
3 34 3 34 3
m .l m .g F
    (2.63)
4 4 1 4 2 4 4
J θ J θ J θ
    (2.64)
Trong đó:
m1234 = m1 + m2 + m3 + m4.
m234 = m2 + m3 + m4 . (2.65)
m34 = m3 + m4 ; Với m4 = m40 + mt
m40 : khối lượng của khớp 4
mt : khối lượng của tải được nối với khớp 4.
J124 = J1 + J2 + J4 .
J24 = J2 + J4 ; Với J4 = J40 + Jt
J40 : mô men quán tính của khớp 4.
Jt : mô men quán tính của tải được nối với khớp 4.
Với 1 , 2 , 4 lần lượt là mômen động tại các khớp quay 1, 2 và 4.
3 = F3 là lực động đặt lên khớp tịnh tiến 3.
Nếu chỉ xét mômen động với 3 khớp quay thì có thể viết gọn lại ba phương
trình động lực học (2.61), (2.62), (2.64) để tiện cho quá trình tính toán như sau:
1 11 12 13 1 1
2 21 22 23 2 2
4 31 32 33 4
H H H θ h
H H H . θ h
H H H θ 0
 

     
 
     
  
 
     
 
     

     
 

38. 38
hay:
2
1 11 12 13 1 2 1 2
2
2 21 22 23 2 1
4 31 32 33 4
H H H θ T 2T
H H H . θ -T
H H H θ 0
   
    
   
   
   
   
   
   
   
   

       
(2.66)
Các thành phần trong phương trình động lực học được xác định:
2 2
11 1234 1 234 2 124 234 1 2 2
2
12 234 2 24 234 1 2 2
13 4
21 12
2
22 234 2 24
23 4
31 32 33 4
234 1 2 2
2
1 2 1 2
2
2 1
H m .l m .l J 2.m .l .l .Cosθ
H m .l J m .l .l .Cosθ
H J
H H
H m .l J
H J
H H H J
T m .l .l .Sinθ
h T 2T
h T
   
  


 

  
 
    
  
(2.67)
2.5. Mô tả đối tượng bằng hệ phương trình trạng thái
Đặt biến trạng thái cho từng khớp như sau:
X(t) =  T
T
4
T
2
T
1 X
X
X (2.68)
11 1
1
12 1
x q
X
x q
   
 
   
   
,
21 2
2
22 2
x q
X
x q
   
 
   
   
,
41 4
4
42 4
x q
X
x q
   
 
   
   
(2.69)
Tín hiệu vào:
1
1
2 2
4 4
u
u u
u

 
 
 
 
  
 
 
 
  
   
(2.70)
Hệ phương trình vi phân trạng thái của các khớp được viết như sau:
Khớp 1:
11 12
12 1 11 1 12 2 13 4
x x
x a b u b u b u



   

(2.71)

39. 39
Khớp 2:
21 22
22 2 21 1 22 2 23 4
x x
x a b u b u b u



   

(2.72)
Khớp 4:
41 42
42 4 31 1 32 2 33 4
x x
x a b u b u b u



   

(2.73)
Trong đó:
bij(X) là các thành phần tương ứng của ma trận H-1.
 
11 22 33 23 32
H
1
b H H H H
D
  .
 
12 13 32 12 33
H
1
b H H H H
D
  .
 
13 12 23 13 22
H
1
b H H H H
D
  .
 
21 23 31 21 33
H
1
b H H H H
D
  .
 
22 11 33 13 31
H
1
b H H H H
D
  .
 
23 13 21 11 23
H
1
b H H H H
D
  . (2.74)
 
31 21 32 31 22
H
1
b H H H H
D
  .
 
32 12 31 11 32
H
1
b H H H H
D
  .
 
33 11 22 12 21
H
1
b H H H H
D
  .
H 11 22 33 21 32 13 31 23 12 11 23 32 21 12 33 31 22 13
D det H H H H H H H H H H H H H H H H H H H
      
ai(X) là thành phần thứ i của vectơ:
 
 
 









 
X
h
X
h
X
h
.
H
3
2
1
1
 
1 11 1 12 2 13 3
a b h b h b h
   
 
3
23
2
22
1
21
2 h
b
h
b
h
b
a 


 (2.75)
 
4 31 1 32 2 33 3
a b h b h b h
   

40. 40
CHƯƠNG III
XÂY DỰNG MÔ HÌNH HỆ THỐNG ĐIỀU KHIỂN
3.1. Cấu trúc hệ thống điều khiển robot
Robot thường tự động thực hiện các nhiệm vụ dịch chuyển hoặc các thao tác.
Các hoạt động của robot thường được lập trình, việc thực hiện các nhiệm vụ thường
làm thay đổi một phần hoặc toàn bộ vị trí của robot trong không gian. Có thể coi robot
hiện đại như những người máy, có thể thực hiện các thao tác phức tạp, có độ chính xác
cao.
Hình 3.1: Sơ đồ cấu trúc chung của hệ thống điều khiển tay máy robot.
Điều khiển chuyển động của tay máy trong không gian là xác định n thành phần
mômen lực tổng quát tác động lên các khớp, mômen lực tổng quát được cung cấp bởi
cơ cấu chấp hành. Quá trình điều khiển phải đảm bảo bộ điều khiển sẽ thực hiện điều
khiển chuyển động của các khớp theo quỹ đạo q(t) sao cho q(t) luôn bám qđ(t), với
qđ(t) là vectơ quỹ đạo chuyển động mong muốn.
Sơ đồ cấu trúc chung của hệ thống điều khiển tay máy cho ở Hình 3.1, tay máy
sẽ bám theo quỹ đạo đặt trước (được lập trình sẵn hoặc có thể đưa vào từ chương trình
phầm mềm kết nối bên ngoài) sử dụng bộ điều chỉnh để điều khiển chuyển động và
lực, sử dụng các cảm biến để thu nhận các thông số về môi trường làm việc đảm bảo
cho hoạt động bền vững, khử được nhiễu ngoài n, nhận biết và đáp ứng thích nghi
được với những sự thay đổi của môi trường cũng như sự thay đổi các tham số động
học của của hệ thống.
Quỹ đạo
đặt
Bộ điều
chỉnh
Cảm biến
Môi trường
Tay máy
n

41. 41
3.2. Xây dựng quỹ đạo chuyển động chuẩn
Trong điều khiển quỹ đạo robot, phải điều khiển tay robot bám theo một quỹ
đạo xác định trước (quỹ đạo chuẩn). Quỹ đạo chuẩn đó là một hàm phụ thuộc thời
gian. Việc xây dựng quỹ đạo chuyển động chính là tìm phương trình mô tả quỹ đạo
robot theo thời gian. Có hai bài toán thiết kế quỹ đạo cho robot:
- Thiết kế quỹ đạo cho tay robot: xác định xd(t), yd(t).
- Thiết kế quỹ đạo cho khớp robot: xác định qid(t).
Việc thiết kế quỹ đạo khớp robot có ưu điểm: q(t) là lượng đặt trực tiếp, hệ điều
khiển là hệ điều khiển vị trí khớp, có thể xác định được điều kiện giàng buộc của động
cơ và hệ truyền động, giải bài toán động học ngược đơn giản. Nhược điểm là khó đảm
bảo chính xác quỹ đạo tay.
Thiết kế quỹ đạo tay robot có ưu điểm: lượng đặt là vị trí tay robot nên đảm bảo
chính xác quỹ đạo. Nhược điểm: phải giải bài toán động học ngược phức tạp với khối
lượng tính toán lớn và khó tính được điều kiện giàng buộc.
Yêu cầu điều khiển sao cho khớp robot chuyển động từ vị trí q0 đến vị trí qc
trong thời gian td. Với dữ kiện ban đầu như vậy có các dạng quỹ đạo như sau:
 Dạng quỹ đạo bậc 3
q(t) = at3 + bt2 + ct +d (3.1)
Việc thiết kế quỹ đạo là xác định các hệ số a, b, c, d.
 Dạng quỹ đạo 2 – 1 – 2
Trong quá trình chuyển động từ q0 đến qc trải qua ba giai đoạn:
- Giai đoạn tăng tốc: q(t) là hàm bậc hai theo t.
- Giai đoạn chuyển động đều: q(t) là hàm bậc nhất theo t.
- Giai đoạn giảm tốc: q(t) là hàm bậc hai theo t.
 Dạng quỹ đạo 4 – 1 – 4
Khớp Robot chuyển động từ q0 đến qc trong thời gian td biết:
0
q(0) q
 ; d c
q(t ) q
 ; 0
q(0) q
 ; d c
q(t ) q
 ; 0
q(0) q
 ; d c
q(t ) q
 ; (3.2)

42. 42
Quỹ đạo chuyển động có ba giai đoạn:
- Giai đoạn tăng tốc: q(t) là hàm bậc 4 theo t.
- Giai đoạn chuyển động đều: q(t) là hàm nhất theo t.
- Giai đoạn giảm tốc: q(t) là hàm bậc 4 theo t.
Phần này sẽ thiết kế quỹ đạo chuyển động của các khớp cho robot với dạng quỹ
đạo 4 – 1 – 4, đảm bảo độ chính xác khi xây dựng quỹ đạo chuẩn. Bài toán xây dựng
quỹ đạo chuẩn là lựa chọn quỹ đạo chuyển động giữa vị trí đầu và cuối của tay robot
với thời gian chuyển động cho trước. Nội dung mục này sẽ giải quyết bài toán quỹ đạo
chuyển động chuẩn cho các biến khớp.
Ta chọn một biến khớp bất kỳ qi thay cho góc quay i (i =1, 2, 3). Thời gian
chuyển động từ vị trí đầu (x0, y0) tới vị trí cuối (xc, yc) là td.
Giá trị ban đầu của q tại thời điểm ban đầu (t = 0) là q0 và giá trị tại t = td là qc.
0
d c
q(0) q
q(t ) q





;
0
d c
q(0) q
q(t ) q





;
0
d c
q(0) q
q(t ) q





(3.3)
Đồ thị quỹ đạo chuyển động chuẩn ở trên Hình 3.2:
Hình 3.2: Dạng quỹ đạo chuyển động chuẩn.

43. 43
Quỹ đạo chuyển động xuất phát từ q0 đến qc sẽ qua ba giai đoạn: gia tốc,
chuyển động với tốc độ không đổi và giảm tốc. Để đơn giản cho việc tính toán, có thể
chọn tham số ta bằng 1/2 khoảng thời gian gia tốc hoặc giảm tốc. Ta cũng xác định các
điểm phụ của quỹ đạo chuyển động q01 và qc2 tại t = ta và t = td – ta là: q01 = q0 và qc2 =
qc.
Nối q01 và qc2 bằng một đường thẳng và xác định điểm q02, qc1 tại t = 2ta và
t = td – 2ta.
Bằng cách chọn điểm phụ như trên, quỹ đạo đoạn cd là một đường thẳng với
tốc độ không đổi, quỹ đạo đoạn ac và df có thể chọn là đa thức bậc bốn có dạng:
q = a0 + a1t + a2t2 + a3t3 + a4t4. (3.4)
3.2.1. Xác định giá trị q02 và qc1
Như cách vẽ đã trình bày ở trên, đường nối be là đường thẳng. Giá trị q02 và qc1
tại t = 2ta và t = td – 2ta, có thể được xác định từ giá trị đầu q0 và cuối qc dựa trên
phương trình bậc nhất của đường thẳng be:
c 0
02 0 a
d a
q q
q q t
t 2t

 

(3.5)
c 0
c1 c a
d a
q q
q q t
t 2t

 

(3.6)
3.2.2. Phương trình đoạn cd
Quỹ đạo đoạn cd là đường thẳng biểu diễn bởi phương trình:
 
c 0
cd a 02
d a
q q
q t 2t q
t 2t

  

(3.7)
với (2ta  t  td – 2ta)
Thay (3.5) vào (3.7) và viết gọn lại ta có được:
 
cd 0cd 1cd a
q a a t 2t
   (3.8)
Trong đó:
c 0
0cd 0 a
d a
c 0
1cd
d a
q q
a q t
t 2t
q q
a
t 2t


 
 



 
 

(3.9)

44. 44
3.2.3. Phương trình đoạn ac
Quỹ đạo đoạn ac biểu diễn bởi phương trình dạng đa thức bậc bốn (3.10):
qac = a0ac + a1act + a2act2 + a3act3 + a4act4. (3.10)
Các hệ số của phương trình đoạn ac được xác định từ điều kiện đầu và cuối:
tại t = 0:
 
 
 








0
ac
0
ac
0
ac
q
0
q
q
0
q
q
0
q





 (3.11)
Tại t = 2ta:
   
   
ac a cd a
ac a cd a
q 2t q 2t
q 2t q 2t







(3.12)
Lấy đạo hàm cấp 1 và cấp 2 của (3.10) và sử dụng (3.11) và (3.12), các hệ số
được xác định như sau:








0
a
0
a
q
a
ac
2
ac
1
0
ac
0
c 0
3ac 2
a d a
c 0
4ac 3
a d a
q q
1
a
4t t 2t
q q
1
a
16t t 2t



 



  
 

(3.13)
3.2.4. Phương trình đoạn df
Quỹ đạo đoạn df biểu diễn bởi phương trình dạng đa thức bậc bốn (3.14):
qdf = a0df + a1dft + a2dft2 + a3dft3 + a4dft4 (3.14)
Các hệ số của phương trình đoạn ac được xác định từ điều kiện đầu và cuối:
   
   
   
 
 
df d a cd d a
df d a cd d a
df d a cd d a
df d c
df d c
q t 2t q t 2t
q t 2t q t 2t
q t 2t q t 2t 0
q t q
q t q
  


  


   




 

(3.15)

45. 45
Cuối cùng các hệ số được xác định như sau:
c 0
0df c a
d a
c 0
1df
d a
2df
3
1 4 a
3df 2
a
c 0 1 a
4df 4
a
q q
a q t
t 2t
q q
a
t 2t
a 0
a 32a t
a
12t
3q 3a 4a t
a
16t
 
 
 

 







 
  


 
  


(3.16)
Như vậy quỹ đạo chuyển động của 3 khớp 1, 2, 4 của Robot Serpent có thể
được xác định bằng các phương trình (3.8), (3.10) và (3.14) với các hệ số của các
phương trình được xác định từ các giá trị của vị trí đầu và vị trí cuối của các khớp
tương ứng bởi các biểu thức: (3.9), (3.13) và (3.16).
3.3. Thiết kế bộ điều khiển cho tay máy robot Scara Serpent ba bậc tự do
3.3.1. Hệ phương trình động lực học Lagrange
Hệ phương trình động lực học Lagrange của tay máy robot Scara Serpent được
viết dưới dạng ma trận sau :
 
 
 
 
 
 























































q
g
q
g
q
g
q
,
q
h
q
,
q
h
q
,
q
h
q
q
q
H
H
H
H
H
H
H
H
H
3
2
1
3
2
1
3
2
1
33
32
31
23
22
21
13
12
11
3
2
1









(3.17)
hay:
2
1 11 12 13 1 2 1 2
2
2 21 22 23 2 1
4 31 32 33 4
H H H θ T 2T
H H H . θ -T
H H H θ 0
   
    
   
   
   
   
   
   
   
   

       
Trong đó 1, 2 và 4 lần lượt là các mômen điều khiển tác động lên khâu 1,
khâu 2 và khâu 4 (từ 2.61 đến 2.65). Với các tham số Hij , T được cho theo (2.66),
(2.67) (đã xét ở chương 2):

46. 46
2 2
11 1234 1 234 2 124 234 1 2 2
2
12 234 2 24 234 1 2 2
13 4
21 12
2
22 234 2 24
23 4
31 32 33 4
234 1 2 2
2
1 2 1 2
2
2 1
H m .l m .l J 2.m .l .l .Cosθ
H m .l J m .l .l .Cosθ
H J
H H
H m .l J
H J
H H H J
T m .l .l .Sinθ
h T 2T
h T
   
  


 

  
 
    
  
và: m1234 = m1 + m2 + m3 + m4 ; m234 = m2 + m3 + m4 .
m34 = m3 + m4 ; m4 = m40 + mt ;
m40 : khối lượng của khớp 4
mt : khối lượng của tải được nối với khớp 4.
J124 = J1 + J2 + J4 ; J24 = J2 + J4 ; J4 = J40 + Jt
J40 : mô men quán tính của khớp 4.
Jt : mô men quán tính của tải được nối với khớp 4.
3.3.2. Hệ phương trình trạng thái
Biến trạng thái cho khớp 1, 2 và 4 như cho ở 2.68  2.70:
11 1
1
12 1
21 2
2
22 2
41 4
4
42 4
x
X
x
x
X
x
x
X
x

    
 
    

   

 
   

 
    

   

 
   
  
   

    

và tín hiệu vào
1 1
2 2
4 4
u
u
U
u

   
   

 
   
   

   

47. 47
Hệ phương trình vi phân trạng thái của các khớp 1, 2 và 4 được viết như sau:
Khớp 1:
11 12
4
12 1 1j j
j 1
x x
x a (X) b u


 
 
 
 
 
 

(3.18)
Khớp 2:
21 22
4
22 2 2 j j
j 1
x x
x a (X) b u


 
 
 
 
 
 

(3.19)
Khớp 4:
41 42
4
42 4 4 j j
j 1
x x
x a (X) b u


 
 
 
 
 
 

(3.20)
Từ các phương trình (3.18) đến (3.20), ta có hệ phương trình trạng thái của khớp 1 và
2, 4 dưới đây:
 Khớp 1:








4
13
2
12
1
11
1
12
12
11
u
b
u
b
u
b
a
x
x
x


(3.23)
Với : )
q
,
q
(
h
).
q
(
H
)
q
,
q
(
a 1

 


   
2
12
1
11
3
13
2
12
1
11
1 h
b
h
b
h
b
h
b
h
b
a 





 (3.24)
 
32
23
33
22
H
11 H
H
H
H
D
1
b 

 
33
12
32
13
H
12 H
H
H
H
D
1
b 

 
13 12 23 13 22
H
1
b H H H H
D
 
13
22
31
33
12
21
32
23
11
12
23
31
13
32
21
33
22
11
H
H
H
H
H
H
H
H
H
H
H
H
H
H
H
H
H
H
H
H
det
D







= )
H
H
H
(
J
)
H
H
H
2
(
J 2
12
22
11
4
22
11
12
2
4 



 Khớp 2:








4
23
2
22
1
21
2
22
22
21
u
b
u
b
u
b
a
x
x
x


(3.25)
Với :    
2
22
1
21
3
23
2
22
1
21
2 h
b
h
b
h
b
h
b
h
b
a 





 (3.26)

48. 48
 
33
21
31
23
H
21 H
H
H
H
D
1
b 

 
22 11 33 13 31
H
1
b H H H H
D
 
 
23
11
21
13
H
23 H
H
H
H
D
1
b 

 Khớp 4:








4
33
2
32
1
31
4
42
42
41
u
b
u
b
u
b
a
x
x
x


(3.27)
Với :    
2
32
1
31
3
33
2
32
1
31
4 h
b
h
b
h
b
h
b
h
b
a 





 (3.28)
 
22
31
32
21
H
31 H
H
H
H
D
1
b 

 
32
11
31
12
H
32 H
H
H
H
D
1
b 

 
21
12
22
11
H
33 H
H
H
H
D
1
b 

Hình 3.3: Mô hình hóa đáp ứng đầu ra thực của robot.
Như vậy mô hình tay máy robot ba bậc tự do là một hệ nhiều đầu vào nhiều đầu
ra, được mô tả bằng ba hệ nhỏ, mỗi hệ tương ứng với từng khớp 1, 2 và 4, được đặc
trưng bởi ba hệ phương trình vi phân trạng thái (3.23), (3.25) và (3.27). Các hệ phương
trình này có thể dựng để mô hình hóa trên máy tính cũng như để tổng hợp luật điều
khiển cho tay máy.
1
s
1
s
Phương
trình (3.23)
x11=1
11 12
x x

12 1
x  
1
s
1
s
x21=2
21 22
x x

22 2
x  
Phương
trình (3.25)
1
s
1
s
x41=4
41 42
x x

42 4
x  
Phương
trình (3.27)

49. 49
3.3.3. Lựa chọn phương pháp điều khiển và bộ điều khiển PID
Phương pháp điều khiển được lựa chọn là phương pháp điều khiển động lực học
ngược với đầu vào bộ điều khiển là sai lệch giữa tín hiệu đặt và tín hiệu đầu ra. Đầu ra
là tín hiệu điều khiển uđk, ở bộ điều khiển PID là uPID.
Hàm truyền của bộ điều khiển:
I
C P D P D
I
K 1
G (s) K K s K 1 T s
s Ts
 
     
 
 
Bộ điều khiển PID được sử dụng khá rộng rãi vì tính đơn giản của nó cả về cấu
trúc lẫn nguyên lý làm việc. Muốn hệ thống có được chất lượng như mong muốn thì
phải phân tích đối tượng rồi trên cơ sở đó chọn các tham số KP, KI, KD cho phù hợp.
 Phương pháp Ziegler – Nichols.
Ziegler – Nichols là phương pháp xác định hệ số KP, hằng số thời gian tích
phân TI và hằng số thời gian vi phân TD dựa trên đặc tính quá độ của hệ thống điều
khiển.
Có hai phương pháp hiệu chỉnh Ziegler – Nichols đều hướng tới mục tiêu đạt
độ quá điều chỉnh khoảng 25%.
- Phương pháp Ziegler – Nichols. Trường hợp 1.
Xác định thông số bộ điều khiển PID dựa vào đáp ứng bậc thang đơn vị của hệ
hở (nếu đối tượng không chứa các khâu tích phân hay nghiệm phức liên hợp thì đường
quá độ của đối tượng có dạng chữ S) :
Hình 3.4: Sơ đồ bộ điều khiển PID
PID
uđk
e

50. 50
Hình 3.5: Đáp ứng bậc thang đơn vị của hệ hở.
T1: thời gian trễ
T2: hằng số thời gian
- Phương pháp Ziegler – Nichols. Trường hợp 2.
T1 và T2 được xác định bằng cách vẽ đường tiếp tuyến với đường cong S tại
điểm uốn, đường tiếp tuyến này cắt trục hoành tại T1 và đường y(t)=K là điểm có
hoành độ T2.
Khi đó mô hình đối tượng có dạng:
1
T s
2
K
G(s) e
T s 1



Bảng 3.1: Thông số bộ PID.
Thông số
Bộ ĐK
KP TI TD
P T2/T1K  0
PI 0.9T2/T1K T1/0.3 0
PID 1.2T2/T1K 2T1 0.5T1
Xác định thông số bộ điều khiển PID dựa vào đáp ứng của hệ kín ở biên giới
ổn định.

51. 51
Hình 3.6: Đáp ứng của hệ kín ở biên giới ổn định.
Bước 1:
Đặt TI = ∞, TD= 0, thay đổi KP từ 0 tới giá trị giới hạn Kgh ứng với đầu ra hệ
thống kín có dao động ở biên giới ổn định. Dao động này tương ứng với chu kỳ giới
hạn Tgh.
Bước 2: Thông số bộ PID được xác định theo bảng :
Bảng 3.2: Thông số bộ PID .
Thông số
Bộ ĐK
KP TI TD
P 0.5Kgh  0
PI 0.45Kgh 0.83Tgh 0
PID 0.6Kgh 0.5Tgh 0.125Tgh
Với: P
I
I
K
K
T
 ; D P D
K K .T


52. 52
CHƯƠNG IV
MÔ PHỎNG VỚI MÔ HÌNH ROBOT SCARA SERPENT
4.1. Đặt vấn đề
Trong chương III đã tiến hành xây dựng mô hình của các khâu tính toán, mô
hình robot và bộ điều khiển PID. Sau đó đã xây dựng các chương trình phục vụ mô
phỏng và khảo sát các chế độ làm việc của hệ thống. Trong chương này sẽ trình bày
các kết quả khảo sát nhận được thông qua các chương trình mô phỏng nêu trên. Thông
qua việc đánh giá sai số quỹ đạo đặt và quỹ đạo thực để đánh giá chất lượng điều
chỉnh bám chính xác của phương pháp điều khiển được nghiên cứu.
Việc mô hình hóa và nghiên cứu mô phỏng hệ thống điều khiển chuyển động
được thực hiện trên nền Matlab-Simulink, giao diện GUI.
4.2. Sơ đồ mô hình hóa các khâu của hệ thống
4.2.1. Mô hình chung của robot
Hình 4.1: Sơ đồ khối mô hình hóa robot Scara Serpent sử dụng bộ điều
khiển mômen động.
Động lực học của robot Scara Serpent được mô tả bằng hệ phương trình trạng
thái (2.71), (2.72) và (2.73) tương ứng với 3 khớp quay của robot. Mỗi hệ phương
trình vi phân cho từng khớp gồm hai phương trình vi phân với biến trạng thái là góc
quay )
x
(
q 1
i
i và tốc độ góc của khớp )
x
(
q 2
i
i
 . Các biến trạng thái đó sẽ được xác định
bằng tích phân các trạng thái tương ứng (Hình 3.3).
4.2.2. Mô hình khối tạo quỹ đạo chuyển động chuẩn
Hình 4.2 mô tả sơ đồ khối tạo quỹ đạo chuyển động chuẩn. Các điều kiện đầu
và cuối của quỹ đạo chuyển động chuẩn được tính thông qua các toạ độ đặt (x0, y0),
Tạo quỹ đạo
chuyển
động chuẩn
Bộ điều
khiển mô
men động
Cơ cấu chấp
hành
(Robot)
tt tt
,
 
ui
d d
,
 
Phản hồi

53. 53
(xc, yc) và thời gian chuyển động (td). Quỹ đạo chuyển động chuẩn của cả 3 khớp
được xác định theo 3 giai đoạn: gia tốc, tốc độ không đổi và giảm tốc, biểu diễn
bằng các phương trình (3.8), (3.10) và (3.14).
Hình 4.2: Sơ đồ khối tạo quỹ đạo chuyển động chuẩn.
4.2.3. Mô hình bộ điều khiển
Hình 4.3: Mô hình bộ điều khiển cho khớp 1 của robot Scara Serpent.
Các khớp 2, 4 có mô hình bộ điều khiển cũng tương tự như khớp 1.
Bộ tham số [KP, KI, KD] của bộ điều khiển PID được chọn theo phương pháp Ziegler
– Nichols.
4.3. Giao diện chương trình mô phỏng robot Scara Serpent
Khi chạy chương trình mô phỏng ta đánh lệnh >> RBSCARA_SP trên cửa sổ
chính của chương trình Matlab, trên màn hình hiện ra bảng làm việc chính như sau:

54. 54
Hình 4.4: Menu chính của chương trình.
Chương trình RBSCARA_SP gồm các chức năng chính sau:
- Nhập các thông số cho mô hình.
- Động học thuận.
- Động học ngược.
- Mô phỏng.
- Hiển thị.
- Thoát.
Chương trình được thiết kế trên giao diện GUI trong Matlab, với các nút thực
hiện các chức năng như trên.
Các chức năng của chương trình được giới thiệu như ở dưới:
Các thông số của robot Scara Serpent được nhập vào khối thông số của robot
như Hình 4.5.

55. 55
Hình 4.5: Thông số của mô hình robot Scara Serpent.
Khi ta nhấn vào nút Động học thuận: Chương trình tính vị trí điểm tác động
cuối của robot khi biết giá trị góc quay các khớp như Hình 4.6.
Hình 4.6: Động học thuận robot Scara Serpent.
Nhập giá trị góc quay các biến khớp 1 và 2 với giới hạn góc quay như sau:

56. 56
1 = -96 0  960 (so với trục Ox); 2 = -115 0  1150 (so với trục thanh 1).
Chương trình sẽ báo lỗi nếu giá trị nhập vào không nằm trong giới hạn góc quay:
Hình 4.7: Động học thuận robot Scara Serpent (báo lỗi).
Khi ta nhấn vào nút Động học ngược: Chương trình tính góc quay của các
khớp khi biết vị trí tọa độ của tay máy robot như Hình 4.8.
Hình 4.8: Động học ngược robot Scara Serpent.

57. 57
Tay robot được thiết kế chuyển động theo một quỹ đạo xuất phát từ vị trí ban đầu
có toạđộ điểm đầu (x0, y0) đếnvị trí cuối có toạ độ điểm cuối (xc, yc) với thời gian chuyển
động là td (Hình 4.2). Các toạ độ ban đầu và kết thúc phải thoả mãn điều kiện (4.1) và giá
trị định vị Nx phải thoả mãn điều kiện (4.2).
Và 2 2 2
X Y (0.4)
  . (4.1)
Giá trị định vị : x
N < 1. ( 4.2)
Chương trình sẽ báo lỗi nếu giá trị (X, Y, x
N ) nhập vào không nằm trong giới
hạn:
Hình 4.9: Động học ngược robot Scara Serpent (báo lỗi).

58. 58
Khi ta nhấn vào nút Mô phỏng: Mô hình sẽ hiện ra như Hình 4.10.
Hình 4.10: Sơ đồ khối mô hình chuẩn robot Scara Serpent.
Khi ta nhấn vào nút Hiển thị: Đưa ra bảng thông số của robot như Hình 4.11.
Hình 4.11: Bảng thông số của robot Scara Serpent.

59. 59
4.4. Kết quả mô phỏng hệ thống trên Matlab-Simulink
4.4.1. Thông số của robot và quỹ đạo chuyển động
Các thông số danh định của robot được sử dụng để thiết kế, tính toán và mô
phỏng như sau:
Bảng 4.1: Các thông số của tay máy.
Tham số
Khâu
l (m) lg (m) m (kg) J (kg.m2)
Khâu 1 0.25 0.25 4 0.021
Khâu 2 0.15 0.15 1.5 0.0034
Khâu 4 0 0 0.6 0.001
Robot được thiết kế chuyển động theo một quỹ đạo ban đầu có tọa độ (x0, y0)
đến vị trí cuối (xc, yc) trong thời gian chuyển động td với ta là thời gian gia tốc (hoặc
giảm tốc).
Tải định mức: m t = 2(kg ) và mômen quán tính của tải Jt = 0.0004 (kg.m2).
Đáp ứng nhanh: td = 1.2(s), ta = 0.15(s).
Đáp ứng chậm: td = 4(s), ta = 0.5(s).
Bảng 4.2: Các tham số đặt.
x0(m) 0 xc(m) 0.25
y0(m) 0.3 yc(m) 0
4.4.2. Đặc tính của hệ thống khi robot làm việc với tải khác nhau
Trong phần này dùng bộ tham số [KP, KI, KD] của bộ điều khiển PID trong
Matlab- Simulink là: Khớp 1: PID1 = [100, 0, 15]
Khớp 2: PID2 = [95, 0, 14]
Khớp 4: PID4 = [90, 0, 12]

60. 60
a. Khảo sát hệ thống trong trường hợp thời gian đặt td nhỏ, đáp ứng nhanh (td =
1.2(s), ta = 0.15(s))
 Đặc tính của hệ thống khi làm việc không tải:
0 0.2 0.4 0.6 0.8 1 1.2
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao dat
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.12: Quỹ đạo đặt các khớp khi làm việc không tải.
0 0.2 0.4 0.6 0.8 1 1.2
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao thuc
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.13: Quỹ đạo thực các khớp khi làm việc không tải.

61. 61
0 0.2 0.4 0.6 0.8 1 1.2
-0.02
-0.015
-0.01
-0.005
0
0.005
0.01
0.015
0.02
0.025
Time (s)
Sai
so
cac
khop
(rad)
Sai so goc quay
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.14: Sai số về góc quay các khớp khi làm việc không tải.
0 0.2 0.4 0.6 0.8 1 1.2
-2
-1.5
-1
-0.5
0
0.5
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc dat
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.15: Vận tốc góc đặt các khớp khi làm việc không tải.

62. 62
0 0.2 0.4 0.6 0.8 1 1.2
-2
-1.5
-1
-0.5
0
0.5
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc thuc
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.16: Vận tốc góc thực các khớp khi làm việc không tải.
0 0.2 0.4 0.6 0.8 1 1.2
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
Time (s)
Sai
so
cac
khop
(rad/s)
Sai so van toc goc
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.17: Sai số về vận tốc góc các khớp khi làm việc không tải.

63. 63
 Đặc tính của hệ thống khi làm việc với 1/2 tải định mức:
0 0.2 0.4 0.6 0.8 1 1.2
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao dat
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.18: Quỹ đạo đặt các khớp khi làm việc với 1/2 tải định mức.
0 0.2 0.4 0.6 0.8 1 1.2
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao thuc
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.19: Quỹ đạo thực các khớp khi làm việc với 1/2 tải định mức.

64. 64
0 0.2 0.4 0.6 0.8 1 1.2
-0.03
-0.02
-0.01
0
0.01
0.02
0.03
Time(s)
Sai
so
cac
khop
(rad)
Sai so goc quay
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.20: Sai số về góc quay các khớp khi làm việc với 1/2 tải định mức.
0 0.2 0.4 0.6 0.8 1 1.2
-2
-1.5
-1
-0.5
0
0.5
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc dat
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.21: Vận tốc góc đặt các khớp khi làm việc với 1/2 tải định mức.

65. 65
0 0.2 0.4 0.6 0.8 1 1.2
-2
-1.5
-1
-0.5
0
0.5
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc thuc
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.22: Vận tốc góc thực các khớp khi làm việc với 1/2 tải định mức.
0 0.2 0.4 0.6 0.8 1 1.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
Time (s)
Sai
so
cac
khop
(rad/s)
Sai so van toc goc
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.23: Sai số về vận tốc góc các khớp khi làm việc với 1/2 tải định mức.

66. 66
 Đặc tính của hệ thống khi làm việc với tải định mức:
0 0.2 0.4 0.6 0.8 1 1.2
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao dat
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.24: Quỹ đạo đặt các khớp khi làm việc với tải định mức.
0 0.2 0.4 0.6 0.8 1 1.2
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao thuc
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.25: Quỹ đạo thực các khớp khi làm việc với tải định mức.

67. 67
0 0.2 0.4 0.6 0.8 1 1.2
-0.03
-0.02
-0.01
0
0.01
0.02
0.03
Time (s)
Sai
so
cac
khop
(rad)
Sai so goc quay
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.26: Sai số về góc quay các khớp khi làm việc với tải định mức.
0 0.2 0.4 0.6 0.8 1 1.2
-2
-1.5
-1
-0.5
0
0.5
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc dat
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.27: Vận tốc góc đặt các khớp khi làm việc với tải định mức.

68. 68
0 0.2 0.4 0.6 0.8 1 1.2
-2
-1.5
-1
-0.5
0
0.5
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc thuc
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.28: Vận tốc góc thực các khớp khi làm việc với tải định mức.
0 0.2 0.4 0.6 0.8 1 1.2
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
Time (s)
Sai
so
cac
khop
(rad/s)
Sai so van toc goc
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.29: Sai số về vận tốc góc các khớp khi làm việc với tải định mức.

69. 69
b. Khảo sát hệ thống trong trường hợp thời gian đặt td tương đối dài, đáp ứng
chậm (td = 4(s), ta = 0.5(s))
 Đặc tính của hệ thống khi làm việc không tải:
0 0.5 1 1.5 2 2.5 3 3.5 4
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao dat
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.30: Quỹ đạo đặt các khớp khi làm việc không tải.
0 0.5 1 1.5 2 2.5 3 3.5 4
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao thuc
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.31: Quỹ đạo thực các khớp khi làm việc không tải.

70. 70
0 0.5 1 1.5 2 2.5 3 3.5 4
-3
-2
-1
0
1
2
3
x 10
-3
Time (s)
Sai
so
cac
khop
(rad)
Sai sai goc quay
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.32: Sai số về góc quay các khớp khi làm việc không tải.
0 0.5 1 1.5 2 2.5 3 3.5 4
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc dat
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.33: Vận tốc góc đặt các khớp khi làm việc không tải.

71. 71
0 0.5 1 1.5 2 2.5 3 3.5 4
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc thuc
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.34: Vận tốc góc thực các khớp khi làm việc không tải.
0 0.5 1 1.5 2 2.5 3 3.5 4
-10
-8
-6
-4
-2
0
2
4
6
8
x 10
-3
Time (s)
Sai
so
cac
khop
(rad/s)
Sai so van toc goc
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.35: Sai số về vận tốc góc các khớp khi làm việc không tải.

72. 72
 Đặc tính của hệ thống khi làm việc với 1/2 tải định mức:
0 0.5 1 1.5 2 2.5 3 3.5 4
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao dat
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.36: Quỹ đạo đặt các khớp khi làm việc với 1/2 tải định mức.
0 0.5 1 1.5 2 2.5 3 3.5 4
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao thuc
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.37: Quỹ đạo thực các khớp khi làm việc với 1/2 tải định mức.

73. 73
0 0.5 1 1.5 2 2.5 3 3.5 4
-3
-2
-1
0
1
2
3
4
x 10
-3
Time(s)
Sai
so
cac
khop
(rad)
Sai so goc quay
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.38: Sai số về góc quay các khớp khi làm việc với 1/2 tải định mức.
0 0.5 1 1.5 2 2.5 3 3.5 4
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc dat
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.39: Vận tốc góc đặt các khớp khi làm việc với 1/2 tải định mức.

74. 74
0 0.5 1 1.5 2 2.5 3 3.5 4
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc thuc
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.40: Vận tốc góc thực các khớp khi làm việc với 1/2 tải định mức.
0 0.5 1 1.5 2 2.5 3 3.5 4
-0.01
-0.008
-0.006
-0.004
-0.002
0
0.002
0.004
0.006
0.008
0.01
Time (s)
Sai
so
cac
khop
(rad/s)
Sai so van toc goc
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.41: Sai số về vận tốc góc các khớp khi làm việc với 1/2 tải định mức.

75. 75
 Đặc tính của hệ thống khi làm việc với tải định mức:
0 0.5 1 1.5 2 2.5 3 3.5 4
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao dat
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.42: Quỹ đạo đặt các khớp khi làm việc với tải định mức.
0 0.5 1 1.5 2 2.5 3 3.5 4
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Goc
quay
cac
khop
(rad)
Quy dao thuc
quy dao khop 1
quy dao khop 2
quy dao khop 4
Hình 4.43: Quỹ đạo thực các khớp khi làm việc với tải định mức.

76. 76
0 0.5 1 1.5 2 2.5 3 3.5 4
-4
-3
-2
-1
0
1
2
3
4
x 10
-3
Time (s)
Sai
so
cac
khop
(rad)
Sai so goc quay
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.44: Sai số về góc quay các khớp khi làm việc với tải định mức.
0 0.5 1 1.5 2 2.5 3 3.5 4
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc dat
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.45: Vận tốc góc đặt các khớp khi làm việc với tải định mức.

77. 77
0 0.5 1 1.5 2 2.5 3 3.5 4
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
Time (s)
Van
toc
cac
khop
(rad/s)
Van toc goc thuc
van toc khop 1
van toc khop 2
van toc khop 4
Hình 4.46: Vận tốc góc thực các khớp khi làm việc với tải định mức.
0 0.5 1 1.5 2 2.5 3 3.5 4
-0.015
-0.01
-0.005
0
0.005
0.01
0.015
Time (s)
Sai
so
cac
khop
(rad/s)
Sai so van toc goc
sai so khop 1
sai so khop 2
sai so khop 4
Hình 4.47: Sai số về vận tốc góc các khớp khi làm việc với tải định mức.