Robot Scara - Tính Toán Động Học & Điều Khiển

Đồ án tốt nghiệp Tính toán động học và điều khiển Robot SCARA
MỤC LỤC
Lời nói đầu......................................................................................................6
Chương I. Những kiến thức cơ sở về Robot công nghiệp...........................7
I. Lịch sử phát triển robot công ngiệp..................................................7
II. Định nghĩa và phân loại Robot.......................................................8
II.1. Định nghĩa Robot.................................................................8
II.2. Phân loại...............................................................................9
III.Ứng dông Robot công nghiệp.......................................................12
III.1. Mục tiêu ứng dụng Robot công nghiệp.............................12
III.2. Các lĩnh vực ứng dụng Robot công nghiệp.......................13
IV. Hệ thống Robot.............................................................................13
V. Ba bài toán cơ bản về Robot..........................................................14
V.1. Động học............................................................................14
V.2. Tĩnh học.............................................................................15
V.3. Động lực học......................................................................15
VI. Cấu trúc động học cơ cấu.............................................................16
VI.1. Khâu và khớp....................................................................16
VI.2. Chuỗi động, cơ cấu và Robot............................................17
Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 1

VI.3. Bậc tự do của cơ cấu Robot..............................................18
Chương II. Bài toán động học của Robot ..................................................19
I. Động học thuận của Robot.............................................................19
I.1. Một số khái niệm cơ bản......................................................19
I.1.1. Ma trận cosin chỉ hướng................................................19
I.1.2. Các tính chất..................................................................21
I.2. Các toạ độ thuần nhất..........................................................22
I.2.1. Định nghĩa toạ độ thuần nhất.........................................23
I.2.2. Ma trận quay cơ bản thuần nhất.....................................23
I.2.3. Các ma trận quay cơ bản thuần nhất và tịnh tiến thuần
nhất.......................................................................................................24
I.2.4. Phép quay thuần nhất tổng hợp......................................25
I.3. Một số phép quay đặc biệt và ma trận biến đổi thuần nhất............26
I.3.1. Cỏc góc Euler và ma trận quay thuần nhất....................26
I.3.2. Cỏc gúc R-P-Y (Roll – Pitch – Yaw) và ma trận quay
thuần nhất.............................................................................................28
I.4. Bộ thông số Denavit Hartenberg và ma trận Denavit
Hartenberg............................................................................................29
I.4.1. Các tham số động học Denavit Hartenberg...................29
I.4.2. Ma trận Denavit Hartenberg..........................................33

I.4.3. Ma trận quan hệ.............................................................34
I.4.4. Phương trình xác định vị trớ khõu thao tác của Robot..34
I.5. Bài toán động học thuận....................................................34
II. Động học ngược.............................................................................35
II.1. Bài toán...............................................................................35
II.2. Phương pháp giải................................................................36
III. Động lực học.................................................................................38
III.1. Vận tốc và gia tốc..............................................................38
III.1.1. Vận tốc........................................................................38
III.1.2. Gia tốc.........................................................................39
III.2. Động năng tay máy...........................................................39
III.3. Thế năng của tay máy.......................................................40
III.4. Phương trình vi phân chuyển động của Robot..................41
Chương III. Tính toán động học thuận và động học ngược Robot
SCARA..........................................................................................................47
I. Giới thiệu về Robot SCARA............................................................47
I.1. Một số thông số cơ bản........................................................47
I.2. Kết cấu cơ khí......................................................................47
I.3.Hệthống điều khiển...............................................................48

II. Tính toán động học thuận Robot SCARA....................................48
II.1. Đặt bài toán........................................................................48
II.2. Sơ đồ động học...................................................................48
II.3.Xỏc định vị ttrớ và hướng của bàn kẹp...............................49
III. Động học ngược Robot SCARA..................................................51
III.1. Đặt bài toán.......................................................................51
III.2. Giải bài toán động học ngược...........................................51
IV. Tính lực tác dụng vào khâu 1 và khâu 2.....................................52
IV. Áp dụng cụ thể..............................................................................55
Chương IV. Điều khiển Robot SCARA......................................................63
I.Động cơ điện một chiều (DC)..........................................................63
I.1. Đại cương về động cơ điện một chiều (DC)........................63
I.1.1. Động cơ điện một chiều (DC)........................................63
I.1.2. Điều khiển tốc độ động cơ điện một chiều....................65
I.1.3. Đảo chiều quay..............................................................65
I.2. Điều khiển động cơ DC.......................................................65
I.2.1. Thiết bị sử dụng.............................................................66
I.2.2. Các bước thực hiện........................................................70
II. MOSFET........................................................................................71

III. Các phần tử logic..........................................................................78
III.1. Phần tử AND.....................................................................78
III.2. Phần tử No.........................................................................79
IV. Ghép nối với máy tính (LPT)........................................................79
IV.1. Các đườcg dẫn vào và ra...................................................79
IV.2. Giao diện hai hướng của cổng máy in..............................84
V. Tìm hiểu ngôn ngữ lập trình DELPHI.........................................85
VI. Bộ phát xung.................................................................................86
VII. Giải thớch sơ đồ mạch điều khiển động cơ DC.........................87
VIII. Lập trình DELPHI....................................................................90
VIII.1. Lưu đồ thuật toán...........................................................90
VIII.2. Lập trình.........................................................................92
Tài liệu tham khảo......................................................................................113

LỜI NÓI ĐẦU
Cùng với sự phát triển của các cuộc cách mạng khoa học kỹ thuật trên
thế giới là những tiến bộ trong lĩnh vực điều khiển và tự động hoá sản xuất.
Một trong những thành tựu nổi bật của quá trình tự động hoá là những cánh
tay máy Robot. Những Robot từ đơn giản đến phức tạp, đã đóng góp rất nhiều
trong việc thay thế con người ở môi trường làm việc cường độ cao, độc hại và
đòi hỏi chính xác cao.
Để có thể phát triển và ứng dụng Robot rộng rãi trong sản xuất, bên
cạnh những kiến thức về điện, điện tử…thỡ việc nghiờn cứu và tính toán động
học và điều khiển Robot là một yếu tố rất quan trọng làm cơ sở và nền tảng
cho việc chế tạo Robot.
Trong đồ án này chúng em đã tiến hành phân tích, tính toán động học
và điều khiển Robot SCARA
Đồ án gồm bốn chương:
Chương I: Trình bày tổng quát về Robot.
Chương II:Bài toán động học Robot .
Chương III:TÝnh toán động học thuận và động học ngược Robot SCARA.
Chương IV: Điều khiển Robot SCARA
Trong đồ án này có sử dụng chương trình tính toán MAPLE và ngôn
ngữ lập trình DELPHI.
Em xin chân thành cảm ơn TS. Lê Văn Ngự đã tận tình hướng dẫn và
giúp đỡ em hoàn thành đồ án này. Em còng xin cảm ơn các thầy Phạm Thành
Chung, Bộ môn Cơ học ứng dụng – Trường Đại học Bách khoa Hà Nội cựng
cỏc Thầy, Cụ giỏo trong Khoa Điện - Cơ - Điện tử đã tạo mọi điều kiện để
em hoàn thành công việc.
Hà Nội, ngày 20 tháng 12 năm 2005.
Sinh viên
Nguyễn Văn Thành

Đặng Thị Dung
CHƯƠNG I
NHỮNG KIẾN THỨC CƠ SỞ VỀ ROBOT CÔNG NGHIỆP
I. LỊCH SỬ PHÁT TRIỂN
Ngay sau chiến tranh thế giới thứ hai, ở Hoa Kỡ đó xuất hiện những tay
máy chộp hỡnh điều khiển từ xa trong cỏc phũng thí nghiệm về vật liệu phóng
xạ. Vào những năm 50 của thế kỉ 20, bên cạnh những tay máy chộp hỡnh cơ
khí đú, đó xuất hiện các loại tay máy chộp hỡnh thuỷ lực và điện từ.
Năm 1961, chiếc robot công nghiệp đầu tiên được đưa vào sử dụng ở
nhà mỏy ụtụ General Motor tại Trenton, New Jersey, Hoa Kì. Năm 1967,
Nhật Bản mới nhập chiếc robot công nghiệp đầu tiên từ công ty AMF của
Hoa Kì. Đến năm 1990, có hơn 40 công ty Nhật Bản đã đưa ra thị trường
quốc tế nhiều loại robot nổi tiếng.
Từ những năm 70, việc nghiên cứu nâng cao tính năng của robot đã chú
ý nhiều đến sự lắp đặt thờm cỏc cảm biến ngoại tín hiệu để nhận biết môi
trường làm việc. Một lĩnh vực được nhiều phòng thí nghiệm quan tâm là robot
tự hành. Các công trình nghiên cứu tạo ra robot tự hành bắt chước chân người
hoặc súc vật. Các loại robot này còn chưa có nhiều ứng dụng trong công
nghiệp, tuy nhiên các loại xe robot (robocar) lại nhanh chóng được đưa vào
ứng dụng trong các hệ thống sản xuất tự động linh hoạt.
Từ những năm 80, nhất là những năm 90, do áp dụng rộng rãi các ứng
dụng kĩ thuật về vi xử lí và công nghệ thông tin, số lượng robot công nghiệp
đã gia tăng, giá thành đã giảm đi rõ rệt, tính năng đã có nhiều bước tiến vượt
bậc. Nhờ vậy robot công nghiệp đã có vị trí quan trọng trong cỏc dõy truyền
tự động sản xuất hiện đại.

II. ĐỊNH NGHĨA VÀ PHÂN LOẠI
II.1. Định nghĩa robot
Có nhiều định nghĩa robot cùng tồn tại, chúng ta hãy tham khảo một số
định nghĩa sau:
• Định nghĩa theo từ điển New World College:
“Robot là một kết cấu cơ khí có hình dạng bất kì, được xây dựng để
thực hiện những công việc bằng tay của con người”.
Các định nghĩa sau này bao gồm các cánh tay cơ khí, cỏc mỏy điều
khiển số, các máy móc di chuyển theo kiểu bước đi và cả mô phỏng hình dạng
con người trong khoa học viễn tưởng. Các robot công nghiệp ngày nay chỉ
thực hiện một phần nào đó công việc của con người.
Các robot ban đầu thường được gọi là các tay máy (Manipulator).
• Định nghĩa theo hiệp hội robot công nghiệp Nhật Bản:
Định nghĩa này mang tính khái quát nhất của tất cả các định nghĩa được
sử dụng. Nó bao gồm tất cả các thiết bị tay máy và có thể để xem xét khi định
nghĩa một robot sau này.
“Robot là một máy, cơ cấu thường gồm một số phân đoạn được nối với
phân đoạn khác bằng khớp quay hay khớp trượt nhằm mục đích để gắp hay
để di chuyển các đối tượng, thường có một số bậc tự do. Nó có thể được điều
khiển bởi một nguồn kích hoạt, một hệ điều khiển điện tử có thể lập trình
được hay một hệ thống logic nào đú”.
• Định nghĩa theo tiêu chuẩn AFNOR (Pháp):
“Robot 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 (chi tiết ,
dông cụ gá lắp...) theo những hành 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 hiệp hôi robot công nghiệp Hoa Kỳ:
“Robot là một tay máy nhiều chức năng có thể lập trình, được thiết kế
để di chuyển vật liệu, các phần tử, linh kiện, các dụng cụ và các thiết bị đặc
biệt thông qua việc thay đổi các chương trình hoạt động đã được lập để thực
hiện các tác vụ khác nhau”.
• Định nghĩa theo hiệp hội robot Anh:
“Robot công nghiệp là một thiết bị có thể lập trình lại được thiết kế để
thực hiện hai nhiệm vụ cầm nắm và vận chuyển các phần tử, linh kiện, các
dụng cụ hoặc các công cụ chế tạo đặc biệt thông qua việc thay đổi các
chương trình hoạt động đã được lập để thực hiện các tác vụ gia công khác
nhau”.
• Định nghĩa theo GOST (Nga):
“Robot 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”.
II.2. Phân loại robot
Việc phân nhóm, phân loại robot có thể dựa trờn những cơ sở kĩ thuật
khác nhau. Dưới đây là một số cách phân loại chủ yếu:
• Phân loại theo số bậc tự do:
Một cách phân loại hiển nhiên của robot là phân loại theo bậc tự do của
chúng. Một cách lí tưởng, một robot có 6 bậc tự do khi cầm nắm một đối
tượng tù do trong không gian ba chiều. Từ quan điểm này, chúng ta gọi một

robot là robot tổng quát (General Purpose Robot) nếu nú cú sỏu bậc tự do, gọi
là robot dư (Redundant Robot) nếu nó có nhiều hơn sáu bậc tự do và gọi là
robot thiếu (Dốicient Robot) nếu nú cú Ýt hơn sáu bậc tự do.
Mét robot sẽ linh hoạt hơn khi di chuyển và hoạt động trong một không
gian kín bị hạn chế. Mặt khác, trong một số ứng dụng đặc biệt như trong việc
lắp ráp trong một mặt phẳng thì chỉ cần robot với bốn bậc tự do là đủ.
• Phân loại theo cấu trúc động học:
Một phương pháp phân loại khác là phân loại theo cấu trúc động học
của chúng.
Mét robot được gọi là robot tuần tù hay robot chuỗi hở nếu cấu trúc
động học của chúng có dạng một chuỗi động hở, gọi là robot song song nếu
cấu trúc động học của chúng có dạng một chuỗi đóng và gọi là robot hỗn hợp
nếu nó bao gồm cả hai loại chuỗi hở và chuỗi đóng. Nhìn nhận một cách tổng
quát thì robot song song có nhiều ưu điểm vỡ chỳng cú độ cứng vững cao
hơn, khả năng tải cao hơn, nhưng không gian làm việc nhỏ hơn và cấu trúc
phức tạp hơn.
• Phân loại theo hệ thống động học hay công nghệ di chuyển:
- Hệ điện:
Thường dùng các động cơ điện một chiều, các động cơ bước hay động
cơ Servo.
Hệ năng lượng này có đặc điểm là hoạt động chính xác, tin cậy, đạt
công suất cao và có tính tuyến tính cao dễ điều khiển. Hệ này cũng đảm bảo
kết cấu gọn, truyền dẫn năng lượng trực tiếp. Ngoài ra nú cũn đảm bảo an
toàn vệ sinh môi trường.
Nhằm nâng cao chất lượng và hiệu quả sử dụng đối với hệ này cần tuân
theo những yêu cầu cơ bản sau:

+ Sử dụng các công nghệ mới, các loại vật liệu mới Ýt chịu ảnh hưởng
của từ trường trái đất.
+ Tiếp tục nâng cao công suất và hiệu suất công tác.
+ Xử lí tốt các cụm nối ghép trong mạch nguồn, mạch điều khiển và
hiệu chỉnh nâng cao hơn nữa độ tin cậy.
- Hệ thuỷ lực - khí nén:
Hệ thuỷ lực có thể đạt đến công suất cao, đáp ứng được những điều
kiện làm việc nặng. Tuy nhiên hệ thống thuỷ lực thường cồng kềnh, yêu cầu
dòng dầu, chất lượng dầu cao, hơn nữa vận tốc lại có độ phi tuyến lớn, khó
đảm bảo độ chính xác cao khi điều khiển.
Hệ khí nén làm việc với công suất trung bình và nhỏ, có kết cấu đơn
giản. Đòi hỏi phải gắn liền với trung tâm khí nén, kém chính xác. Thích hợp
cho các loại robot hoạt động theo chương trình định sẵn với các thao tác đơn
giản kiểu nhấc lên hạ xuống.
• Phân loại theo hệ thống truyền động:
- Hệ truyền động gián tiếp:
Các cơ cấu chấp hành được nối với nguồn động lực thông qua các bộ
truyền động cơ khí thương gặp như hệ thống bánh răng thường, hệ bánh răng
hành tinh, hệ bánh răng sóng, dây đai, bộ truyền xích hay cao hơn là bộ
truyền vớt_đai ốc bi... Nhược điểm của hệ này là bị mòn tạo khe hở động học
dẫn đến tính phi tuyến và hiệu ứng trễ ngày càng cao hơn. Mặt khác hiệu suất
sẽ giảm do tiêu hao công suất trên bộ truyền.
- Hệ truyền động trực tiếp:
Các cơ cấu chấp hành được nối trực tiếp với nguồi động lực, do đó kết
cấu sẽ gọn nhẹ và hạn chế, loại bỏ được những nhược điểm của truyền động

gián tiếp. Mặt khác, những khó khăn đặt ra là cần thiết kế chế tạo các động cơ
có số vòng quay thích hợp và cho phép điều khiển vô cấp trên một dải rộng.
• Phân loại theo phương pháp điều khiển:
Dùa vào tính chất đặc trưng của quĩ đạo điều khiển cú cỏc qui tắc điều
khiển cơ bản là:
- Điều khiển điểm.
- Điều khiển quĩ đạo liên tục.
- Điều khiển nhận dạng.
- Điều khiển thích nghi.
• Phân loại theo độ chính xác:
Trong hoạt động của robot cần phân biệt độ chính xác tuyệt đối và độ
chính xác lặp lại để đánh giá mức độ tin cậy trong mét chu kì làm việc đơn lẻ
và trong một quá trình làm việc lõu dài.Mặt khác, để đánh giá trên một miền
kích thước hay một phạm vi chức năng rộng hơn, người ta còn đưa ra độ
chính xác phân giải để đánh giá mức độ chính xác trờn cỏc miền phân giải
khác nhau.
III. ỨNG DễNG ROBOT CÔNG NGHIỆP
III.1. Mục tiêu ứng dụng robot công nghiệp Mục tiêu ứng dụng robot
công nghiệp
Mục tiêu ứng dụng robot công nghiệp nhằm góp phần nâng cao năng
suất dây chuyền công nghệ, giảm giá thành sản phẩm, nâng cao chất lượng và
khả năng cạnh tranh của sản phẩm, đồng thời cải thiện điều kiện lao động,
Điều đó xuất phát từ những ưu điểm cơ bản của robot, đó là:
• Robot công nghiệp có thể thực hiện được một qui trình thao tác hợp
lí bằng hoặc hơn người thợ lành nghề một cách ổn định trong suốt
thời gian làm việc. Vì thế robot công nghiệp có thể góp phần nâng

cao chất lượng và khả năng cạnh tranh của sản phẩm. Hơn thế robot
còn có thể nhanh chóng thay đổi công việc để thích nghi sù thay đổi
mẫu mã, kích cỡ sản phẩm theo yêu cầu của thị trường cạnh tranh.
• Khả năng giảm giá thành sản phẩm do ứng dụng robot là giảm được
đáng kể chi phí cho người lao động.
• Việc áp dụng robot có thể làm tăng năng suất dây chuyền công
nghệ. Sở dĩ như vậy vì nếu tăng nhịp độ khẩn trương của dây
chuyền sản xuất, nếu không thay thế con người bằng robot thì người
thợ không thể theo kịp hoặc rất chóng mệt mỏi.
• Robot có thể cải thiện điều kiện lao động. Đó là ưu điểm nổi bật
nhất mà chúng ta cần lưu tâm. Vì trong thực tế sản xuất có rất nhiều
nơi người lao động phải làm việc trong môi trường có hại cho sức
khoẻ hoặc dễ xảy ra tai nạn lao động .
III.2. Các lĩnh vực ứng dụng robot công nghiệp Các lĩnh vực ứng dụng
robot công nghiệp
Robot công nghiệp được ứng dụng nhiều trong các lĩnh vực: đúc, gia
công áp lực, hàn và nhiệt luyện, gia công và lắp ráp . . .
IV. HỆ THỐNG ROBOT

Hình 1.1: Các hệ thống cấu thành robot
Tay máy gồm các bộ phận :đế 1 đặt cố định hoặc gắn liền với xe di
động 2, thân 3, cánh tay trên 4, cánh tay dưới 5, bàn kẹp 6.
Hệ thống truyền dẫn động có thể là cơ khí , thuỷ khí hoặc điện khí, là
bộ phận chủ yếu tạo nên sự chuyển dịch của các khớp động.
Hệ thống điều khiển đảm bảo sự hoạt động của robot theo các thông tin
đặt trước hoặc nhận biết được trong quá trình làm việc.
Hệ thống cảm biến tín hiệu thực hiện việc nhận biết các biến đổi thông
tin về hoạt động của bản thân robot (cảm biến nội tín hiệu) và môi trường, đối
tượng mà robot phục vụ (cảm biến ngoại tín hiệu).
Các thông tin đặt trước hoặc cảm biến được sẽ đưa vào hệ thống điều khiển
sau khi xử lí bằng máy vi tính, rồi tác động vào hệ thống truyền dẫn động của
tay máy.
V. BA BÀI TOÁN CƠ BẢN VỀ ROBOT
Robot là một ngành khoa học hay ngành học về công nghệ truyền thống
kết hợp với lí thuyết và ứng dụng của các hệ thống robot. Việc nghiên cứu
bao gồm cả hai vấn đề là nghiên cứu lí thuyết và ứng dụng, những vấn đề đó
chia ra thành các lĩnh vực: công tác thiết kế robot, cơ học cơ cấu, thiết kế quĩ
đạo và điều khiển, công tác lập trình và tri thức cho mỏy....Cơ học là một
nhánh khoa học nghiên cứu các vấn đề về năng lượng, lực và tác dụng của
chúng đối với chuyển động của các hệ thống cơ khí. Việc nghiên cứu bao
gồm ba vấn đề có quan hệ với nhau là: Động học, Tĩnh học và Động lực học.
V.1.Động học Động học
Động học nghiên cứu các đặc trưng của chuyển động mà không quan
tâm đến nguyên nhân gây ra chúng như lực và mô men. Khoa học động học
nghiên cứu về vị trí, vận tốc, gia tốc. Do đó, động học chỉ liên quan đến hình
học và thời gian thay đổi của chuyển động. Sự thay đổi của cỏc khõu của

robot liên quan đến hướng và vị trí của khâu chấp hành cuối cùng bởi sự ràng
buộc của các khớp. Những quan hệ động học đó là trọng tâm của việc nghiên
cứu động học robot. Việc nghiên cứu động học có hai vấn đề: Phân tích động
học và Tổng hợp động học. Tuy nhiên vấn đề phân tích động học và tổng hợp
động học luôn liên quan đến nhau.
Nội dung nghiên cứu động học của robot là việc tỡm cỏc quan hệ
chuyển động của cỏc khõu gồm có hai bài toán là: Bài toán động học thuận và
Bài toán động học ngược. Trong việc lập trình cho robot điều cơ bản là đặt ra
các yêu cầu về vị trí của điểm tác động cuối và hướng của khâu cuối,vận tốc
và gia tốc của khâu bất kì trong không gian. Vấn đề ở đây là tìm tất cả các bộ
thông số có thể chấp nhận được về sự thay đổi của cỏc khõu hoạt động và các
đạo hàm tương ứng của chúng xảy ra ở khâu chấp hành cuối cùng để đặt các
yêu cầu về vị trí và hướng, đú chớnh là các thông số hoạt động (bài toán động
học thuận) hay từ yêu cầu về vị trí và hướng của khâu chấp hành cuối tìm ra
các thông số tương ứng của cỏc khõu trước đó(bài toán động học ngược).
Tổng hợp động học chính là quá trình ngược lại của việc phân tích
động học. Trong trường hợp này, nhà thiết kế cần đặt ra được những robot
hay máy mới, điều đó đòi hỏi những thay đổi nhất định về mặt động học. Cụ
thể, khi cú cỏc thông số vị trí, hướng(cựng vận tốc và gia tốc) của khâu chấp
hành cuối cuối, chúng ta cần xác định các thay đổi tương ứng ở cỏc khõu hoạt
động và cấu trúc hình học của robot.
V.2.Tĩnh học Tĩnh học
Tĩnh học nghiên cứu quan hệ về lực ở trạng thái cân bằng của các phần
thay đổi của robot. Mét robot có thể hoạt động nhờ tác động lực sinh ra từ các
nguồn kích động khác nhau, như trọng lực, tải trọng, lực ma sát, lực quán
tính... những lực này cần phải được xem xét cẩn thận khi thiết kế các robot
bởi vì các thành phần của chúng có thể có trị số đáng kể và có thể làm cho

robot không đảm bảo được các chức năng đã định. Lực cân bằng phụ thuộc
vào cấu tạo và đặc điểm của robot mà không phụ thuộc vào thời gian.
V.3.Động lực học Động lực học
Động lực học nghiên cứu về giữa các lực tác dung vào cơ cấu và
chuyển động của cơ cấu. Động lực học robot là vấn đề rất phức tạp. Một cách
cụ thể, khâu chấp hành cuối cùng được truyền dẫn thông qua một đường dẫn
với các thông số hoạt động chính xác.
VI. CẤU TRÚC ĐỘNG HỌC CƠ CẤU
Cơ cấu được tạo thành từ một số khâu nối với nhau bởi các khớp. Số
bậc tự do của một cơ cấu phụ thuộc vào số cỏc khõu, cỏc khớp và loại khớp
để tạo nên cơ cấu. Trong phần này chúng ta tìm hiểu các loại chuỗi động, cơ
cấu và máy, sau đó tìm hiểu cỏch tớnh bậc tự do.
Xét hai vật thể (hay hai khâu) A và B để rời trong không gian, gắn vào
A một hệ toạ độ Đề_cỏc Oxyz thì B sẽ cú sỏu khả năng chuyển động tương
đối đối với A, gọi là sáu bậc tự do tương đối.
Các khả năng chuyển động độc lập là:
- Các chuyển động tịnh tiến dọc các trục Ox, Oy, Oz, kí hiệu là Tx, Ty, Tz.
- Các chuyển động quay quanh các trục Ox, Oy, Oz, kí hiệu là Rx, Ry, Rz.
Sau đây chúng ta tiến hành nghiên cứu cụ thể các đối tượng.
VI.1. Khâu và khớp
+Phần có chuyển động tương đối so với phần khác trong cơ cấu được
gọi là khâu.
+Khớp là chỗ nối động giữa hai khâu.

Tuỳ theo cấu trúc mỗi khớp hạn chế một số chuyển động giữa hai
khâu. Bề mặt tiếp xúc của mỗi khâu tại khớp gọi là một thành phần khớp. Hai
thành phần khớp tạo thành một khớp động. khớp động có thể phân thành khớp
thấp và khớp cao tuỳ thuộc dạng tiếp xúc. Khớp động thuộc loại khớp thấp
nếu hai thành phần tiếp xúc là mặt. Khớp động thuộc loại khớp cao nếu hai
thành phần tiếp xúc là điểm hoặc đường.
Cú mét số loại khớp cơ bản thường dùng trong các cơ cấu máy và các
robot, đó là:
Khớp quay (khớp bản lề) : khớp để lại chuyển động quay của khâu này
đối với khõu khỏc quanh một trục quay. Khớp quay hạn chế năm khả năng
chuyển động giữa hai thành phần khớp, có một bậc tự do.
Khớp lăng trụ(khớp tịnh tiến): cho phép hai khâu trượt lên nhau trên
một trục. Hạn chế năm khả năng chuyển động giữa hai khõu, cú một bậc tự
do.
Khớp trô: cho phép hai khả năng chuyển động độc lập gồm một chuyển
động quay và một chuyển động tịnh tiến dọc trục quay. Hạn chế bốn khả năng
chuyển động giữa hai khõu, cú hai bậc tự do.
Khớp ren: cho phép chuyển động quay quanh trục và tịnh tiến dọc trục
quay nhưng hai chuyển động này phụ thuộc nhau nên hạn chế năm khả năng
chuyển động giữa hai khõu, cú một bậc tự do.
Khớp cầu: cho phép thực hiện chuyển động quay giữa hai thành phần
khớp quanh tâm cầu theo tất cả cỏc cỏc hướng, nhưng không cho phép
chuyển động tịnh tiến giữa hai thành phần khớp này. Hạn chế ba khả năng
chuyển động giữa hai khõu, cú ba bậc tự do.
Khớp phẳng: cho hai khả năng chuyển động tịnh tiến theo hai trục
trong mặt phẳng tiếp xúc và một khả năng chuyển động quay quanh trục

vuông góc với mặt phẳng tiếp xúc. Hạn chế ba khả năng chuyển động giữa
hai khõu, cú ba bậc tự do.
Khớp bánh răng phẳng: cho hai bánh răng ăn khớp với nhau. Các mặt
răng tiếp xúc đẩy nhau, chúng thường trượt trên nhau. Hạn chế bốn khả năng
chuyển động giữa hai khõu, cú hai bậc tự do.
Khớp cam phẳng: tương tù như khớp bánh răng phẳng. Hạn chế bốn
khả năng chuyển động giữa hai khõu, cú hai bậc tự do.
*Trong các khớp kể trên có khớp bánh răng phẳng và khớp cam phẳng
là khớp cao còn lại là khớp thấp.
VI.2.Chuỗi động , cơ cấu và robot Chuỗi động , cơ cấu và robot
Chuỗi động là tập hợp cỏc khõu được nối với nhau bằng các khớp.
Các loại chuỗi động: chuỗi hở,chuỗi đóng, chuỗi đơn, chuỗi kép...
Một chuỗi động được gọi là cơ cấu khi có một khâu cố định đối với
giỏ. Khõu cố định đôi khi còn gọi là khâu gốc. Trong cơ cấu có thể có một
hoặc nhiều khâu được Ên định là khâu dẫn với các thông số cho trước. Sự
chuyển động của cỏc khõu dẫn là độc lập, sự chuyển động của tất cả cỏc khõu
khỏc sẽ phụ thuộc vào sù chuyển động của cỏc khõu dẫn. Cơ cấu là một thiết
bị truyền chuyển động từ một hay nhiều khâu dẫn tới cỏc khõu khỏc.
Thuật ngữ cơ cấu và mỏy đụi khi được dùng đồng nghĩa với nhau.
Bên cạnh định nghĩa trờn cũn cú định nghĩa như sau:
Một tập hợp các phần tử được gọi là cơ cấu nếu nó chỉ được dùng để
truyền chuyển động, và được gọi là máy nếu nó được dùng để biến đổi năng
lượng ngoài thành các dạng năng lượng hữu Ých cho công việc.
VI.3.Bậc tù do của cơ cấu robot Bậc tù do của cơ cấu robot
Bậc tù do của cơ cấu là số thông số độc lập hay số thông số cần cho
trước để vị trí của cơ cấu hoàn toàn xác định. Ta có thể tìm được một công

thức tổng quát tính bậc tự do của cơ cấu theo số khâu, số khớp và loại khớp
tạo thành cơ cấu:
i=
6
1
6( 1) *j
j
n d j
=
− − ∑
i:Số bậc tự do của hệ. Số bậc tự do của hệ.
n:Số khâu của hệ kể cả giá. Số khâu của hệ kể cả giá.
dj:Số các liên kết bậc j. Số các liên kết bậc j.
j:Bậc của liên kết là số bậc tự do mà liên kết hạn chế. Bậc
của liên kết là số bậc tự do mà liên kết hạn chế.

CHƯƠNG II
BÀI TOÁN ĐỘNG HỌC CỦA ROBOT
Bài toán động học của robot bao gồm các bài toán về vị trí, bài toán về
vận tốc, về gia tốc. Trong bài toán về vị trí thì việc xác định vị trí và hướng
của điểm tác động cuối tại những thời điểm khác nhau là vấn đề cốt lõi. Để có
thể giải quyết được bài toán, thì như ta đã biết robot là một hệ nhiều vật rắn
ghép nối với nhau bằng các khớp, chủ yếu là khớp quay và khớp tịnh tiến, do
vậy cần phải xác định được các hệ toạ độ gắn với cỏc khõu của robot.
I. ĐỘNG HỌC THUẬN CỦA ROBOT.
Trong bài toán động học thuận: Cho mét vector gồm các biến khớp của
robot công nghiệp, hãy xác định vị trí và hứơng của bàn kẹp trong hệ toạ độ
gắn với giá đỡ của robot. Để trình bày phương pháp chung giải quyết động
học thuận, trước hết ta cần nhắc lại một số khái niệm cơ bản.
I.1. Một số khái niện cơ bản
I.1.1. Ma trận cosin chỉ hướng
• Định nghĩa
H×nh 1.1

Mét ma trận vuông cấp ba có dạng:










=
)(
3
)0(
3
)(
2
)0(
3
)(
1
)0(
3
)(
3
)0(
2
)(
2
)0(
2
)(
1
)0(
2
)(
3
)0(
1
)(
2
)0(
1
)(
1
)0(
1
...
...
...
iii
iii
iii
i
eeeeee
eeeeee
eeeeee
A



(1.1)
gọi là ma trận cosin chỉ phương của hệ qui chiếu Ri đối với hệ qui chiếu R0.
Trong đó
)0()0(
, kj ee

là các vector đơn vị của các hệ qui chiếu R0 và Ri
)3,1,( =kj . Nếu đặt )3,1,(),.cos(. )()0()()0(
, === kjeeeea i
kj
i
kjkj

.
Thì Ai có dạng










=
333231
232221
131211
aaa
aaa
aaa
Ai (1.2)
• Ý nghĩa
Để xét ý nghĩa ma trận cosin chỉ phương trường hợp trong phẳng.
ϕ
H×nh 1.2
Xét hai hệ qui chiếu ),( )0(
2
)0(
10 eeR

và )1(
2
)1(
11 ,( eeR

)0(
2
)0(
2
)0(
1
)0(
1 ..0
eSeSSR 
+= (1.3)
)1(
2
)1(
2
)1(
1
)1(
1 ..1
eSeSSR 
+=
Trong khi đó
)0(
212
)0(
111
)1(
1 .0
eaeaeR 
+= (1.4)

)1(
222
)1(
112
)1(
2 .1
eaeaeR 
+=
nên
)..()...( )0(
222
)0(
112
)1(
2
)0(
221
)0(
111
)1(
1
1
eaeaSeaeaSSR 
+++=
)0(
2
)1(
222
)1(
121
)0(
1
)1(
212
)1(
111 )..()..( eSaSaeSaSa

+++= (1.5)
SS RR

10
=
Suy ra
)1(
212
)1(
111
)0(
1 .. SaSaS += (1.6)
)1(
222
)1(
121
)0(
2 .. SaSaS += (1.7)














=








)1(
2
(`)
1
2221
1211
)0(
2
)0(
1
S
S
aa
aa
S
S
(1.8)
SAS
RR 10
.=
( ) ( )
( ) ( ) 




 −
=
ϕϕ
ϕϕ
cossin
sincos
A (1.9)
Tương tù trong không gian ba chiều ta có





















=










)1(
3
)1(
2
)1(
1
333231
232221
131211
)0(
3
)0(
2
)0(
1
S
S
S
aaa
aaa
aaa
S
S
S
(1.10) (1.10)
SAS
RR 10
.=
Ta thấy ma trận cosin chỉ phương A là ma trận biến đổi toạ độ điểm P
trong R1 về điểm P trong hệ toạ độ R0. Ma trận A còn gọi là ma trận quay.
I.1.2. Các tính chất
a. Tính chất 1: Ma trận cosin chỉ phương là ma trận trực giao.
)0(
313
)0(
212
)0(
111
)1(
1 ..0
eaeaeaeR 
++=
)0(
332
)0(
222
)0(
112
)1(
2 ..0
++= (1.11)
)0(
333
)0(
223
)0(
113
)1(
3 ..0
++=











=
31
21
11
)1(
1
0
a
a
a
e
R
,










=
32
22
12
)1(
2
0
a
a
a
e
R
,










=
33
23
13
)1(
3
0
a
a
a
e
R
(1.12)
)1(
3
)1(
2
)1(
1
000
[ eeeA
RRR
=
Ma trận A cấu tạo bởi các vector đơn vị nên có tính trực giao.
b. Hệ quả: Trong chín thành phần của cosin chỉ phương chỉ có ba thành phần
độc lập.
12
31
2
21
2
11 =++ aaa
12
32
2
22
2
12 =++ aaa
133
2
23
2
13 =++ aaa
0... 323122212111 =++ aaaaaa (1.13)
0... 333123211211 =++ aaaaaa
0... 333223221312 =++ aaaaaa
Do ràng buộc với sáu phương trình nên ma trận cosin chỉ phương chỉ
có ba thành phần độc lập.
c. Tính chất 2: Định thức của ma trận cosin chỉ phương bằng 1
I.2. Các toạ độ thuần nhất.
Do các phép quay thuần tuý chỉ đủ để xác định hướng của hệ trục toạ
độ gắn vào vật. Tuy nhiên để xác định vị trí tương đối này với hệ trục toạ độ
cơ sở cố định phải sử dụng đến một phép biến đổi khỏc, phộp tịnh tiến. Phép
tịnh tiến về khía cạnh nào đó rất khác biệt với phép quay. So với phép quay,
gốc toạ độ của hệ bị quay trùng với gốc toạ độ của hệ ban đầu. Điều này cho

phép chúng ta biểu diễn các phép quay trong không gian ba chiều bằng ma
trận 3x3. Vì vậy phải dùng một số chiều lớn hơn 3. Không gian bốn chiều với
hệ toạ độ thuần nhất.
I.2.1. Định nghĩa toạ độ thuần nhất
Xét vị trí của một điểm P ở trong hệ toạ độ ba chiều 0xyz được xác
định bởi vector sau:
321 ezeyexr

++= (2.1)
Giả sử σ là một đại lượng vô hướng khỏc khụng tuỳ ý. Khi đó toạ độ
thuần nhất của điểm P được định nghĩa bởi hệ thức
[ ]T
zyxr σσσσ=

(2.2)
Trong kỹ thuật người ta thường chọn σ =1. Khi đó toạ độ thuần nhất
bốn chiều của điểm P được mở rộng từ các toạ độ vật lý ba chiều của điểm P
bằng cách thêm vào các thành phần thứ tư như sau
[ ]T
zyxr 1= (2.3) (2.3)
Nhờ khái niệm toạ độ thuần nhất trong không gian bốn chiều ta có thể
chuyển bài toàn cộng ma trận cột trong không gian ba chiều sang bài toán
nhân ma trận trong không gian bốn chiều. Cho a

và b

là hai vector trong
không gian ba chiều, ta có:











+
+
+
=










+










=+
33
22
11
3
2
1
3
2
1
ba
ba
ba
b
b
b
a
a
a
ba (2.4)
Ta chuyển phép tính cộng thành phép tính nhân hai ma trận như sau:













+
+
+












=












+
+
+
11000
0100
0010
0001
1
33
22
11
33
22
11
ba
ba
ba
ba
ba
ba
(2.5)
I.2.2. Ma trận quay cơ bản thuần nhất
Xét một vật rắn B chuyển động trong hệ qui chiếu cố định 0xyz . Lấy
một điểm A nào đó của vật rắn B và gắn chặt vào vật rắn hệ qui chiếu Auvw .
Lấy P là một điểm bất kỳ thuộc vật rắn B trong hệ toạ độ vật lý ta có
ApAp srr

+=
Sử dụng ngôn ngữ ma trận phương trình trờn cú dạng














=














10001
)0(
333231
)0(
232221
)0(
131211
)0(
)0(
)0(
A
A
A
p
p
p
zaaa
yaaa
xaaa
z
y
x
(2.7)
Như vậy nếu ta định nghĩa ma trận

=T














1000
)0(
333231
)0(
232221
)0(
131211
A
A
A
zaaa
yaaa
xaaa
(2.8)
Thì T được gọi là ma trận chuyển toạ độ thuần nhất của điểm P trong hệ Axyz
sang hệ 0x0y0z0.
I.2.3. Các ma trận quay cơ bản thuần nhất và tịnh tiến thuần nhất
a. Phép quay quanh trục x một góc ϕ
( ) ( )












−
==
1000
0cossin0
0sincos0
0001
,
ϕϕ
ϕϕ
ϕϕ xRotAx
b. Phép quay quanh trục y một góc ψ
( ) ( )
( ) ( )
( ) ( )












==
1000
0cos0cos
0010
0cos0cos
,
ψψ
ψψ
ψψ yRotAy
c. Phép quay quanh trục z một góc θ
( ) ( )
( ) ( )
( ) ( )











 −
==
1000
0100
00cossin
00sincos
,
θθ
θθ
θθ zRotAz
d. Ma trận tịnh tiến thuần nhất cơ bản
Ngoài ra ta còn có phép tịnh tiến dọc trục x một đoạn là px, trục y một
đoạn là py, trục z một đoạn là pz.
( )












=
1000
100
010
001
,,
z
y
x
zyx
p
p
p
pppTrans (2.12)
I.2.4. Phép quay thuần nhất tổng hợp

Nói chung ma trận biến đổi thuần nhất có thể biểu diễn cả phép quay và
phép tịnh tiến của một hệ toạ độ động so với một hệ toạ độ cố định. Một trình
tự phép quay và phép tịnh tiến có thể biểu diễn bằng một phép nhân các ma
trận thuần nhất cơ bản. Tuy nhiên vì phép nhân ma trận không có tính giao
hoỏn nờn trình tự thực hiện phép quay và phép tịnh tiến rất quan trọng. Hơn
nữa hệ toạ độ động có thể quay hay tịnh tiến theo vector đơn vị của hệ trục cố
định hay của chính nó.
Để giải quyết vấn đề này người ta thường sử dụng thuật toán sau:
a. Khởi gán ma trận biến đổi T=I, trong đó I là ma trận đơn vị. Điều này
tương ứng với hai hệ toạ độ trực chuẩn F và M trùng nhau.
b. Biểu diễn các phép quay, tịnh tiến bằng các ma trận biến đổi thuần nhất
riêng rẽ.
c. Biểu diễn các phép quay tổng hợp bằng các ma trận thuần nhất cơ bản
d. Nếu hệ toạ độ động M quay hay tịnh tiến dọc theo vector đơn vị của hệ
trục cố định F thỡ nhõn trước ma trận T bởi ma trận quay hay tịnh tiến
tương ứng.
e. Nếu hệ toạ độ động M quay hay tịnh tiến dọc theo vector đơn vị của
chính nó thỡ nhõn trước ma trận T bởi ma trận quay hay tịnh tiến tương
ứng.
f. Tiếp bước d nếu có thêm nhiều phép quay hay tịnh tiến.
I.3. Một số phép quay đặc biệt và ma trận biến đổi thuần nhất
I.3.1. Cỏc gúc Euler và ma trận quay thuần nhất
Ta có hình vẽ biểu thị ba góc quay Euler như sau

ϕ
ψ
H×nh 1.3
Vị trí của vật rắn B quay quanh điểm O cố định được xác định bởi vị trí
của hệ qui chiếu động 0xyz (gắn chặt vào vật rắn B) đối với hệ qui chiếu cố
định 0x0y0z0. Giả sử giao của mặt phẳng 0x0y0 và mặt phẳng 0xy là trục OK.
Trục OK này được gọi là đường nót.
Ta đưa vào các ký hiệu sau:
a. Góc giữa trục Ox0 và OK là ψ
b. Góc giữa trục OK và Ox là ϕ
c. Góc giữa trục Oz0 và Oz là θ
Ba góc ψ ,ϕ,θ được gọi là ba góc Euler. Như vậy vị trí của điểm B
đối với hệ qui chiếu cố định được xác định bởi ba toạ độ suy rộng ψ ,ϕ,θ.
Phương trình chuyển động của vật rắn quay quanh một điểm cố định có dạng:
ψ =ψ (t); ϕ=ϕ(t); θ=θ(t)(3.1) (3.1)
Từ đó ta suy ra, vật rắn quay quanh một điểm cố định có ba bậc tự do. Khi
xác định vị trí của vật rắn bằng cỏc gúc Euler, ta có thể quay hệ qui chiếu cố
định Ox0y0z0 sang hệ qui chiếu động Oxyz bằng ba phép Euler như sau:

ωψ
θ
ω
ϕ
ω
ψ
θ
ϕ
H×nh 1.4
- Quay hệ qui chiếu R0 ≡ Ox0y0z0 quanh trục Oz0 một góc ψ để trục Oz0
chuyển tới đường nót OK. Với phép quay này hệ Ox0y0z0 chuyển sang hệ
Ox1y1z1 với Ox0 ≡ Oz1.
- Quay hệ qui chiếu R1 ≡ Ox1y1z1 quanh trục Ox1 ≡ OK một góc θ để
Oz0 ≡ Oz1 chuyển tới trục Oz2 ≡ Oz. Như thế hệ qui chiếu Ox1y1z1 chuyển
sang hệ qui chiếu Ox2y2z2 với Ox1 ≡ Ox2 ≡ OK.
- Quay hệ qui chiếu R2 ≡ Ox2y2z2 quanh trục Ox2 ≡ OK một góc ϕ để
trục Oz2 ≡ OK chuyển tới trục Ox. Như thế hệ qui chiếu Ox2y2z2 chuyển
sang hệ qui chiếu Oxyz với Oz2 ≡ Oz.
Như thế bằng ba phép quay Euler quanh trục Oz0 một góc ψ , quanh trục
OK một gúc θ, quanh trục Oz một góc ϕ, hệ qui chiếu Ox0y0z0 chuyển
sang hệ qui chiếu Oxyz.
Các ma trận quay ứng với các phép quay Euler có dạng
( )
( ) ( )
( ) ( )











 −
=
1000
0100
00cossin
00sincos
0
ψψ
ψψ
ψzA (3.2)

( )
( ) ( )
( ) ( )












−
=
1000
0cossin0
0sincos0
0001
θθ
θθ
θKA (3.3)
( )











 −
=
1000
0100
00cossin
00sincos
ϕϕ
ϕϕ
ϕzA (3.4)
Bõy giê ta xác định ma trận quay hệ qui chiếu Ox0y0z0 sang hệ qui
chiếu Oxyz (cũng là ma trận cosin chỉ hướng của hệ qui chiếu Oxyz đối với
hệ qui chiếu Ox0y0z0). Ta có:
( )
( ) ( )
( ) ( )












−
−−+
−−−
=
1000
0
0
0
,,
θθϕθϕ
θψϕψϕθψϕψϕθψ
θψϕψϕθψϕψϕθψ
ϕθψ
CSSSC
SSCCCCSCCCCS
CSSSCCCSSCCC
R (3.5)
Kí hiệu Cx=cos(x), Sx=sin(x)
I.3.2. Cỏc gúc R-P-Y(Roll-Pitch-Yaw) và ma trận quay thuần nhất
Việc định hướng khâu cuối có thể thực hiện theo các phép quay Roll-
Pitch-Yaw như sau:
Yaw,ψ
Roll,ϕ
Pitch,θ
H×nh 1.5
- Quay quanh trục Zn một góc ϕ
- Quay quanh trục Yn một góc θ
( ) ( ) ( ) ( )θθψϕθψ KKz AAAR ..,, 0
=

- Quay quanh trục Xn một góc ψ
Như vậy ta cú cỏc ma trận quay ứng với cỏc gúc quay Roll-Pitch-Yaw:
( )
( ) ( )
( ) ( )












−
=
1000
0cossin0
0sincos0
0001
ψψ
ψψ
ψXnA (3.6)
( )
( ) ( )
( ) ( )












−
=
1000
0cos0sin
0010
0sin0cos
θθ
θθ
θYnA (3.7)
( )











 −
=
1000
0100
00cossin
00sincos
ϕϕ
ϕϕ
ϕZnA (3.8)
Tương tù như đối với phép quay Euler, ta cũng có ma trận chuyển hệ
qui chiếu như sau
( )
( ) ( )
( )












−
−+
+−
=
1000
0
0
0
,,
ϕθϕθθ
ϕψϕθψϕϕψϕθψϕθϕ
ψϕψθϕψϕψθϕθϕ
ϕθψ
CCCCS
CCSSSSCCSSSSSS
CSCSCCSCSCCC
R
Kí hiệu Cx=cos(x), Sx=sin(x)
I.4. Bộ thông số Denavit_Hartenberg và ma trận Denavit_Hartenberg
I.4.1. Các tham số động học Denavit-Hartenberg

kh©u i-1
khí p i
θi
H×nh 4.1
kh©u i
Xét hệ các vật rắn nối ghép với nhau bằng các khớp quay và các khớp
tịnh tiến. Khi đó quan hệ vị trí giữa hai khâu kế tiếp có thể xác định bởi tham
khớp (hình 4.1). Trờn hỡnh 4.1 khâu thứ i-1 được nối với khâu thứ i bằng
khớp.Trục zi-1được gọi là trục khớp của khâu thứ i. Tham số thứ nhất θi,
được gọi là gúc khớp là góc quay của trục xi-1 quanh trục zi-1 đến trục xi’//
x.Tham số thứ hai là di, là khoảng cách giữa trục xi’ và trục xi . Nếu khớp i là
khớp quay thì θi là biến, còn di là hằng số.Nếu khớp i là khớp tịnh tiến thì θi
là hằng số còn di là biến.
1.Trục zi-1được chon dọc theo hướng của trục khớp động thứ i.
2.Trục xi-1 được chọn theo đường vuông góc chung của hai trục
zi-1 hướng đi từ trục zi-2sang trục zi-1. Nếu trục zi-1cắt trục zi-2thì hướng của trục
zi-2được chọn tuỳ ý.
3.Gốc toạ độ Oi-1được chọn ttại giao điểm của trục xi-1và trục zi-1
4.Trục yi-1 được chọn sao cho hệ (0xyz)0 là hệ quy chiếu thuận.
Với cách chọn như trên đôi khi các hệ toạ độ khõu(0xyz)i-1 không được xác
định một cách duy nhất. Vì vậy ta cần có một số bổ sung như sau:
5.Đối với hệ toạ độ (0xyz)0 được quy ước trên ta mới chỉ chọn
được trục z0, còn trục x0 chưa có trong qui ước trên. Ta có thể chọn trục x0
một cách tuỳ ý.

6.Đối với hệ toạ độ (0xyz)n do không có khớp n+1, nên theo qui
ước trên ta không xác định được trục zn. Trục zn không được xác định duy
nhất, trong khi trục xn lại được chọn theo phương pháp tuyến của trục zi-1.
Trong trường hợp này, nếu khớp n là khớp quay ta nên chọn trục zn song song
với trục zn-1.Ngoài ra ta có thể chọn tuỳ ý sao cho hợp lý.
7.Khi hai trục zn-2 và trục zn-1 song song với nhau, giữa hai trục
này có nhiều đường pháp tuyến chung, ta có thể chọn trục xn theo hướng pháp
tuyến chung nào cũng được
8.Khi khớp quay thứ i là khớp tịnh tiến, về nguyên tắc ta có thể
chọn trục zn-1 một cách tuỳ ý. Tuy nhiên trong nhiều trường hợp người
ta thường chọn trục xn-1 dọc theo trục của khớp tịnh tiến này.
Hình 4.2.

Hình 4.2 và hình 4.3 minh hoạ cách chọn các hệ toạ độ khâu theo ý
tưởng của Denavit-Hartenberg đối với trường hợp khớp quay và khớp tịnh
tiến.
Vị trí của hệ toạ độ khâu (0xyz)i đối với hệ toạ độ khâu (0xyz)i-1, được
xác định bởi bốn tham sè Denavit-Hartenberg θi ,di,ai và α i như sau
• Quay quanh trục zi-1 mét góc iθ để trục xi-1 chuyển đến trục
xi’(xi’//xi).
• Dịch chuyển tịnh tiến dọc theo trục zi-1để gốc toạ độ 0i-1 chuyển đến
gốc 0i-1’, giao điểm của trục xi và trục zi-1.
• Dịch chuyển tịnh tiến dọc theo trục xi để điểm 0i’ chuyển đến điểm 0i.
• Quay quanh trục xi một góc iα để trục zi’ chuyển đến trục zi’
Hình 4.3.
Trong bèn tham số trờn, cỏc tham sè ai và iα luôn luôn là hằng số, độ
lớn của chúng phụ thuộc vào hình dáng và sự ghép nối của cỏc khõu thứ i-1

và khâu thứ i. Hai tham số còn lại là iθ và di, một hằng số, một là biến số phụ
thuộc vào khớp i là khớp quay hay là khớp tịnh tiến. Khi khớp i klà khớp
quay thì iθ là biến, còn di là hằng.Khi i là khớp tịnh tiến thì di là biến số, còn
iθ là hằng số.
I.4.2. Ma trận Denavit-Hartenberg
Ta có ma tận DH chuyển từ hệ toạ độ (0xyz)i-1 sang hệ toạ độ thứ
(0xyz)i như sau:
• Quay quanh trục zi-1 một góc θi với ma trận quay là i-1
Ai(zi—1, θi).
• Dịch chuyển tịnh tiến dọc theo trục zi-1một đoại di với ma trận tịnh tiến là i-
1
Ti(0,,0,di).
• Dịch chuyển tịnh tiến dọc theo trục xi1một đoại ai với ma trận tịnh tiến là i-
1
Ti(ai,0,0).
• Quay quanh trục xi một góc α i với ma trận quay là i-1
Ai(xi, α i).
Ma trận của phép biến đổi, ký hiệu là i-1
Hi , là tích của bốn ma trận :
i-1
Hi = i-1
Ai(zi—1, θi).1
Ti(0,,0,di).i-1
Ti(ai,0,0).i-1
Ai(xi, α i)
Đối với khớp quay:
i-1
Hi=
( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( )
( ) ( )












−
−
1000
cossin0
sinsincoscoscossin
cossinsincossincos
iii
iiiiiii
iiiiiii
d
a
a
αα
θαθαθθ
θαθαθθ
(4.1) (4.1)
Đối với khớp tịnh tiến:
i-1
Hi=
( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( )
( ) ( )












−
−
1000
cossin0
0sincoscoscossin
0sinsincossincos
iii
iiiii
iiiii
dαα
αθαθθ
αθαθθ
(4.2)
Định nghĩa: Ma trận i-1
Hi được xác định bởi (4.1) đối với khớp quay
hoặc (4.2) đối với khớp tịnh tiến được gọi là ma trận Denavit-Hartenberg.

I.4.3. Ma trận quan hệ
Chọn hệ toạ độ gắn liền với giá đỡ và các hệ toạ độ động gắn liền với
từng khâu động . Các hệ toạ độ này được ký hiệu từ 0 đến n.
Một điểm bất kỳ nào đó trong không gian được xác định trong hệ toạ
độ thứ i bằng bán kính vec tơ ri và trong hệ toạ độ cố định x0y0z0 được xác
định bằng bán kính vec tơ r0:
r0=0
H1.1
H2.2
H3...0
Hiri
Đặt 0
Ti=0
H1.1
H2.2
H3...0
Hi
Trong đó ma trận 0
H1 mô tả vị trí và hướng của khâu đầu tiến với hệ toạ
độ cố định, ma trận 1
H2 mô tả vị trí và hướng của khâu thứ hai so với khâu thứ
nhất, ma trận i-1
Hi mô tả vị trí và hướng của khâu thứ i so với khâu thứ i-1.
Như vậy, ma trận tích 0
Hi là ma trận mô tả vị trí và hướng của hệ toạ độ thứ i
so với hệ toạ độ cố định.
I.4.4. Phương trình xác định vị trí khâu thao tác (bàn kẹp) của robot
Áp dụng liên tục phép biến đổi đối với hai khâu liên tiếp đối với robot
n khâu ta được phương trình xác định vị trí khâu thao tác (bàn kẹp) của robot.
An =Tn
(0)
= T1
(0)
T2
(1)
T3
(2)
… Tn
(n-1)
=T1T2T3…Tn =TE (4.1)
I.5. BÀI TOÁN ĐỘNG HỌC THUẬN
Cơ cấu chấp hành của robot thường là một cơ cấu hở, gồm một chuỗi
các khâu nối với nhau bằng các khớp . Để robot có thể thao tác linh hoạt, cơ
cấu chấp hành của nó phải cấu tạo sao cho điểm mót của khâu cuối cùng đảm
bảo dễ dàng di chuyển theo một quĩ đạo nào đó, đồng thời khâu này có một
định hướng nhất định nào đó theo yêu cầu. Khâu cuối cùng thường là bàn kẹp
hoặc là khâu gắn liền với dụng cụ làm việc. Điểm mót của khâu cuối cùng là

điểm đáng quan tâm nhất vì đó là điểm tác động cuối của robot lên đối tác và
được gọi là “điểm tác động cuối”.Ta cần quan tâm đến vị trí của điểm tác
động cuối và hướng của khâu cuối trong không gian làm việc của robot.
Gắn vào điểm tác động cuối này một hệ toạ độ động thứ n, gắn với mỗi
khâu động một hệ toạ độ động khác và gắn lên giá đỡ một hệ toạ độ cố định.
Đánh số kí hiệu các hệ này từ 0 đến n bắt đầu từ giá cố định. Khi khảo sát
chuyển động của robot cần biết định vị của điểm tác động cuối và định hướng
của khâu cuối trong mọi thời điểm. Nhiều khi lại cần biết cả vận tốc và gia tốc
của điểm tác động cuối còng như các điểm khác của robot. Đó là nội dung
quan trọng của bài toán động học robot, chúng được xây dựng trên cơ sở thiết
lập các mối quan hệ toạ độ động nói trên so với hệ toạ độ cố định.
Ma trận Ti ( xem 4.1) là ma trận mô tả vị trí và hướng của hệ toạ độ thứ
i gắn với khâu thứ i so với hệ toạ độ cố định. Trong trường hợp i=n, với n là
số hiệu chỉ hệ toạ độ gắn liền với khâu làm việc. Ta có :
0
Tn=0
H1.1
H2.2
H3...n-1
Hn=TE (5.1)
Với TE=












1000
zzzz
yyyy
xxxx
pasn
pasn
pasn
(5.2)
Là ma trận chỉ hướng và vị trí của điểm tác động cuối.
Bài toán động học thuận là bài toán tìm 9 tham sè nx, ny, nx, ax, ay, az, px,
py, pz theo các biến khớp.
Từ hệ phương trình (5.1) 0
Tn= TE ta tìm được nx, ny, nx, ax, ay, az, px, py,
pz theo các biến khớp bằng cách so sánh các phần tử của hai ma trận tương
ứng 0
Tn và TE.
II. ĐỘNG HỌC NGƯỢC
II.1. Bài toán

Bài toán động học ngược được đặc biệt quan tâm vì lời giải của nó là
cơ sở chủ yếu để xây dựng chương trình điều khiển chuyển động của robot
bám theo quĩ đạo cho trước. Các lời giải tìm được cho líp bài toán này hầu
như chỉ cho trường hợp riờng, cỏc đặc điểm động học riêng biệt được tận
dụng để thiết lập các quan hệ cần thiết khi thiết lập lời giải.
Xuất phát từ phương trình động học cơ bản ta có:
Tn = A1 .A2...An =
0 0 0 1
x x x x
y y y y
z z z z
n s a p
n s a p
n s a p
 
 
 
 
 
 
(2.1)
Hai ma trận ở hai vế của phương trình đều là ma trận thuần nhất 4x4.
So sánh các phần tử tương ứng của hai ma trận trên ta có 6 phương trình độc
lập với các Èn sè qi, (i=1...n). Có 3 trường hợp có thể xảy ra:
• Nếu sè Èn sè ( thường cũng là số bậc tự do của cơ cấu robot) n<6 thì
cơ cấu robot chỉ đưa bàn kẹp tới những vị trí và hướng hạn chế mà không
đưa bàn kẹp tới vị trí và định hướng bất kì. Trường hợp này được áp dụng khi
không có yêu cầu thay đổi một số thông số định vị và định hướng của bàn
kẹp.
• Nếu n= 6 , tức là số Èn bằng số phương trình thì bộ biến khớp q1...q6
hoàn toàn xác định. Tuy nhiên, lời giải không phải lúc nào cũng dễ dàng tìm
ra. Bởi vì nói chung hệ phương trình tìm được thường là siêu việt và việc tìm
nghiệm của hệ phương trình này không phải lúc nào cũng hội tụ.
• Nếu n>6 , tức là số Èn lớn hơn sè phương trình thì có khả năng có
nhiều lời giải, tức là cùng đạt tới một vị trí và định hướng của bàn kẹp có thể
có nhiều bộ thông số biến khớp qi.
II.2. Phương pháp giải
Cách 1:

Ta có thể viết :
Tn=Ti.i
Tn (2.2)
với i
Tn là ma trận chuyển đổi từ hệ toạ độ i sang hệ toạ độ n.
Nhân hai vế của (1) với Ti
-1
ta có:
Ti
-1
Tn= i
Tn và vì Ti
-1
=(A1.A2...Ai)-1
=Ai
-1
... A2
-1
.A1
-1
nên Ai
-1
... A2
-1
.A1
-1
Tn= i
Tn
Kết hợp với phần trên ta có
i
Tn =Ai
-1
... A2
-1
.A1
-1
0 0 0 1
x x x x
y y y y
z z z z
n s a p
n s a p
n s a p
 
 
 
 
 
 
(2.3)
với i= 1...n
Ứng với mỗi giá trị của i, khi so sánh các phần tử tương ứng của hai ma
trận ở hai vế của biểu thức trên ta có 6 phương trình tồn tại độc lập để xác
định biến khớp qi (i=1..n).
Cách 2:
Từ hệ phương trình (2.1) ta có thể rót ra sáu phương trình độc lập để
xác định biến khớp qi (i=1..n).
Từ (2.1) ta rót ra các phương trình độc lập:
1 1 2
2 1 2
3 1 2
( , ... ) 0
( , ... ) 0
( , ... ) 0
n x
n y
n z
f q q q p
f q q q p
f q q q p
− =

− =
 − =
(2.4)
Tuỳ theo cấu trúc của robot mà ta rót ba phương trình còn lại, ở đây
tạm gọi là các phương trình sau:

4 1 2
5 1 2
6 1 2
( , ... ) 0
( , ... ) 0
( , ... ) 0
n
n
n
f q q q
f q q q
f q q q
=

=
 =
(2.5)
Nhận xét:
Việc giải bài toán động học ngược của cơ cấu robot rất phức tạp, cho đến
nay vẫn chưa có thuật giải tổng quát. Để giải được các bài toán này người ta
thường lợi dụng một số tính chất của một cơ cấu để đơn giản hoá việc tính
toán và chỉ tìm lời giải cho những mô hình cụ thể, ví dụ như các đặc điểm
hình học, các ràng buộc của các biến khớp... Việc giải bằng phương pháp giải
tích thông thường nói chung là rất khó khăn, đôi khi không giải được. nhằm
khắc phục khó khăn này ta sử dụng phương pháp số để giải bài toán động học
ngược robot.
Khi giải bài toán động học ngược thường được một tập hợp nghiệm,
vấn đề đặt ra là: trong những nghiệm tìm được thì chọn nghiệm nào! Để giải
quyết vấn đề này thì ta cần thực hiện tối ưu hoỏ cỏc nghiệm tìm được theo
một tiêu chuẩn nào đó, như tối ưu về thời gian điều khiển, tối ưu về năng
lượng....
III. ĐỘNG LỰC HỌC
III.1. Vận tốc và gia tốc
III.1.1. Vận tốc
Vận tốc của điểm M trong hệ toạ độ thứ i được xác định bằng vectơ i
ri
i
ri =[ xi,yi,zi ]T
Ký hiệu i
ri có nghĩa là điểm M cho biết trong hệ toạ độ i và được biểu thị
trong hệ toạ độ i, còn ký hiệu o
ri cho biết M trong hệ toạ độ thứ i và được biểu
thị trong hệ toạ độ thứ 0.
Ta có: 0
ri =o
Ti
i
ri
Vận tốc điểm M được tính trong hệ toạ độ nền là :

0
Vi = dt
d
(0
ri) = dt
d
(o
Ti
i
ri) = dt
d
(o
Ti )i
ri
Vectơ i
ri không đổi trong hệ toạ độ thứ i nên đạo hàm i
ri theo thời gian
bằng 0.
0
Vi = dt
d
(o
Ti )i
ri =(0
H1.1
H2....i-1
Hi.+ 0
H1.1
H2...i-1
Hi.+...+ 0
H1.1
H2....i-1
H1) i
ri
0
Vi = i
i
i
Þ
j
j
i
rq
q
T








∂
∂
∑=1
.0
(3.1) (3.1)
Đặt Uịj=
j
i
q
T
∂
∂0
=0
H1.
1
H2....
j
j
j
dq
Hd 1−
...i-1
Hi-1.
i-1
Hi
Nếu j >i thì Uịj=0
Vậy 0
Vi = i
i
j
i
Þi
ôi rqU








∑=1
..
(3.2) (3.2)
III.1.2. Gia tốc
Đạo hàm biểu thức (3.2) xác định được gia tốc:
ai = i
i
i
Ý
i
Ýi
i
k
ks
ks
i
s
s
ii
rqq
qq
T
q
q
T
dt
Vd










∂∂
∂
+
∂
∂
= ∑ ∑∑= = =1
..
1 1
.
.0200
(3.3) (3.3)
III.2. Động năng tay máy
Kí hiệu Ti là động năng của khâu thứ i (i= n,1 ) và dTi là động năng của
một chất điểm có khối lượng dm thuộc khâu thứ i.
( )dmVVTrdmzyxdt T
iiiii
0022
.
2
.
2
1
.
2
1
=





++= (3.4)
Trong đó Tr là vết của ma trận, bằng tổng các phần tử trên đường chéo
chính của ma trận.
Thay biểu thức của (3.2) vào biểu thức (3.4) ta được:

dmqqUrrUTrdmrqUrqUTrdTi
i
p
i
r
rp
T
ir
T
i
i
i
i
Ø
T
r
irñi
i
pip 





=














= ∑∑∑∑ = == 1 1
..1
1
.
2
1
2
1
( ) 







= ∑ ∫∑= =
i
p
rp
T
ir
T
i
i
i
i
i
r
i
ipi qqUrdmrUTrdT
1
.
.
12
1
(3.5)
Ma trận Uij không phụ thuộc vào sự phân bố khối lượng mà chỉ phụ thuộc
vào các biến khớp.
Tích phân biểu thức (3.5) ta có:
( ) 







== ∑ ∫∑∫ = =
i
p
rp
T
ir
T
i
i
i
i
i
r
i
ipii qqUrdmrUTrdTT
1
.
.
12
1














==
∫∫∫∫
∫∫∫∫
∫∫∫∫
∫∫∫∫
∫
dmdmzdmydmx
dmzdmzdmzydmzx
dmydmzydmydmyx
dmxdmzxdmyxdmx
dmrrJ
iii
iiiiii
iiiiii
iiiiii
T
i
i
i
i
i 2
2
2
Ji được gọi là ma trận quán tính của khâu thứ i.
Động năng của toàn cơ cấu tay máy bằng tổng động năng của tất cả cỏc
khõu động:








== ∑∑∑∑ = ===
i
p
rp
T
iri
i
r
i
ip
n
i
n
i
i qqUJUTrTT
1
.
.
111 2
1
( )∑∑∑= = = 







=
n
i
i
p
i
r
rp
T
iriip qqUJUTrT
1 1 1
.
.
2
1
(3.6)
III.3. Thế năng của tay máy
Thế năng iΠ của khâu thứ i:
ci
i
iiciii rHgmrgm 00
(−=−=Π ) (3.7)
Trong đó: g là vộctơ gia tốc trọng trường, g=[0,0,-g,0].

0
rci, i
rci lần lượt là bán kính vector biểu diễn trọng tâm của
khâu i trong hệ toạ độ cố định và hệ toạ độ thứ i.
Thế năng của toàn cơ cấu n khâu động là:
∑∑ ==
−=Π=Π
n
i
ci
i
ii
n
i
i rHgm
1
0
1
)( (3.8)
III.4. Phương trình vi phân chuyển động của robot
Trong khảo sát động lực học của robot công nghiệp, việc xây dựng các
phương trình chuyển động đóng một vai trò quan trọng, mà dùa vào đó có thể
tiến hành các tính toán của thiết kế cũng như khảo sát động lực. Hai dạng
phương trình thường được dùng để mô tả chuyển động các robot công nghiẹp
là phương trình Newton-Euler và phương trình Lagrange. Phương trình đầu tỏ
ra khá đơn giản và thuận lợi trong việc lập các phương trình động học nhưng
không đơn giản khi viết các phương trình liên kết đặc biệt số phần tử tham gia
vào hệ lớn . Phương trình sau thường cho kết quả khá gọn và đẹp đẽ do chọn
các hệ toạ độ suy rộng là đủ. Tuy nhiên với phương pháp sau việc tính toán
các đại lượng động lực như động năng, lực suy rộng trong nhiều trường hợp
gặp nhiều khó khăn, đặc biệt khi số bậc tự do lớn hoặc có nhiều khớp không
gian phức tạp.
Phương trình chuyển động dạng Lagrange có dạng:
*
. i
i
i
Q
q
L
q
L
dt
d
=
∂
∂
−








∂
∂
ni ,1= (4.1)
Trong đó: L: Hàm Lagrange, L=T- Π
T: Động năng của cơ hệ.
Π: Thế năng của hệ.
Q*
i: Lùc suy rộng của các lực không thế.

qi: các biến khớp.
Biểu thức (3.6),(3.8) vào biểu thức (4.1), cuối cùng ta được:
( ) ( )∑∑ ∑∑∑ ∑= = = = = =
−+=
n
j
j
k
n
j
j
k
j
m
n
j
cj
j
jijmk
T
jijjkmk
T
jiiiki rgUmqqUJUTrqUJUTrQ
1 1 1 1 1 1
....
*
(4.2)
Biểu thức trên (4.2) có thể viết gọn như sau:
∑ ∑∑= = =
++=
n
k
n
k
n
m
imkikmkiki cqqHqMQ
1 1 1
.
...
*
(4.3)
với dạng ma trận:
)(),()()(
...
*
qcqqhqqMtQ ++=
(4.4)
Trong đó:
Q*
là vectơ lực suy rộng của các lực không thế:
Q*
(t)= [Q*
1(t), Q*
2(t)…Q*
n(t)]T
q(t) là vector biến khớp:
q(t)= [q1(t), q2(t)…qn(t)]T
.
)(tq là vector tốc độ của biến khớp:
T
n tqtqtqtq
.
..
2
.
1
.
)(),...,(),()( 



=
..
)(tq là vector gia tốc của biến khớp:
T
n tqtqtqtq
..
....
2
..
1 )(),...,(),()( 



=
M(q) là ma trận nxn cú cỏc phần tử Mik:
∑=
=
n
kij
T
jijjkik UJUTrM
),max(
.
),( nki ,1, =

),(
.
qqh là vector lực ly tâm và Coriolit:
),(
.
qqh =[h1, h2, …, hn]T
∑∑= =
=
n
k
n
m
mkikmi qqhh
1 1
..
, ni ,1, =
),(
),,max(
∑=
=
n
mkij
T
jijikmikm UJUTrh nkmi ,1,, =
c(q) là vector lực trọng trường:
c(q)=[ c1, c2,…, cn]T
( )∑=
−=
n
ij
cj
j
ijii rgUmc
Ta còn có thể thành lập phương trình vi phân theo cách sau:
Vận tốc góc của vật rắn:
Để xác định iω ta tính ma trận phản đối xứng:
T
iii AA
.
=ϖ (4.5)
Ai: Ma trận quay từ hệ (Oxyz)0 sang hệ (Oxyz)i










−
−
−
=
0
0
0
xiyi
xizi
yizi
i
ωω
ωω
ωω
ϖ
Để biểu diễn vận tốc góc theo toạ độ suy rộng
.
q , ta đưa vào ma trận JRi


























==
.
.
2
.
1
.
...),3(...)2,3()1,3(
),2(...)2,2()1,2(
),1(...)2,1()1,1(
n
RiRiRi
RiRiRi
RiRiRi
Rii
q
q
q
nJJJ
nJJJ
nJJJ
qJω
(4.6)
Các phần tử của ma trận JRi được xác địmh như sau:

JRi[k,j]= hệ số của
.
q [k] trong iω[k] (k =1,2,3; j =1,2,…,n).
Vận tốc các điểm thuộc vật rắn:
Vận tốc khối tâm của vật rắn Bi:
..
1
qJq
q
r
dt
dr
v Tij
n
j j
CiCi
Ci =
∂
∂
== ∑=
(4.7)
rCi=[rCi rCy rCz]T
là toạ độ trọng tâm của khâu thứ i so với hệ toạ độ
(0xyz)0.
JTi là ma trận Jacobi cỡ n×3 của khâu thứ i




















∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
=
n
CizCizCiz
n
CiyCiyCiy
n
CixCixCix
Ti
q
r
q
r
q
r
q
r
q
r
q
r
q
r
q
r
q
r
J
...
...
...
21
21
21
(4.9)
Động năng của hệ:
Động năng T của hệ bằng tổng động năng các vật thuộc hệ:
∑=
=
n
i
iTT
1
Ti là động năng của vật rắn Bi, được xác định theo công thức sau đây:
ii
T
iCii
T
Cii IvmvT ωω 0
2
1
2
1
+= (4.10)
Trong đó:
VCi: vận tốc tại khối tâm của vật rắn Bi.
iω: vectơ vận tốc góc của vật rắn Bi.
mi : khối lượng của vật rắn Bi.

I0
i: ma trận tenxơ quán tính của vật rắn Bi đối với hệ toạ
độ(0xyz)0.
Ta có:
I0
i=AiIi
iAT
i (4.11)
Ii
i: ma trận tenxơ quán tính của vật rắn Bi , so với hệ toạ độ đặt tại trọng
tâm của vật rắn Bi. Các thành phần của Ii
i không thay đổi và không phụ thuộc
vào thời gian.
Thay(4.6) ,(4.7) vào (4.9) ta được biểu thức động năng của vật rắn Bi
[ ] ....
2
1
iRi
T
i
i
ii
T
RiTii
T
Ti
T
i qJAIAJJmJqT ++= (4.12)
Động năng của hệ là:
[ ]
.
1
.
2
1
qJAIAJJmJqT
n
i
Ri
T
i
i
ii
T
RiTii
T
Ti
T






+= ∑=
(4.14)
Viết gọn lại:
..
2
1
qMqT T
=
Với ( ) [ ]∑=
+=
n
i
Ri
T
i
i
ii
T
RiTii
T
Ti JAIAJJmJqM
1
nên ta có: ∑∑= =
=
n
i
n
j
jiij qqmT
1 1
..
2
1
(4.15)
Lực suy rộng :
Vectơ lực suy rộng Q cú cỏc thành phần Qi (i= n,1 ) được tính theo biểu
thức sau:

*
i
i
i Q
q
Q +
∂
Π∂
−= (4.16)
Π: Thế năng của cơ hệ.
*
iQ : Lực suy rộng của các lực không thế
Thế năng của hệ gồm thế năng của trọng lực và thế năng của lò xo:
2
11 2
1
i
n
i
ii
n
i
i cgzm δ∑∑ ==
+=Π (4.17)
Trong đó:
Zi : độ cao của khối tâm Ci trong hệ toạ độ (0xyz)0.
iδ : độ biến dạng của lò xo.
Phương trình Lagrange loại 2:
Các phương trình vi phân chuyển động Lagrange loại 2 cho hệ n bậc tự do
chịu liên kết hụlụnụm, giữ , dõng và lý tưởng có dạng:
i
i
i
Q
q
T
q
T
dt
d
=
∂
∂
−
∂
∂
. (i= n,1 ) (4.18)
Thay biểu thức (4.15) vào (4.18) cuối cùng ta được:
*
..
1 1
..
1 2
1
i
i
kj
n
j
n
k i
ik
k
ij
j
n
j
ij Q
q
qq
q
m
q
m
qm +
∂
Π∂
=





∂
∂
−
∂
∂
+ ∑∑∑ = ==
Đặt kj
n
j
n
k k
ij
k
ij
i qq
q
m
q
m
g
..
1 1 2
1
∑∑= =






∂
∂
−
∂
∂
=
g là vectơ cú cỏc thành phần là gi
vector g được xác định như sau:
.
)
2
1
( qCBg −=
B là ma trận cú cỏc thành phần là bij:

bij =
.
1
k
n
k k
Þ
q
q
m
∑= ∂
∂
C là ma trận cú cỏc thành phần là cij:
cij =
.
1
k
n
k k
Þ
q
q
m
∑= ∂
∂
Đặt h( tqq ,,
.
) = g( tqq ,,
.
) +
( )
q
q
∂
Π∂
Khi đó phương trình vi phân chuyển động được viết dưới dạng ma trận sau:
( ) *
...
,, QtqqhqqM =





+ (4.19)
CHƯƠNG III
TÍNH TOÁN ĐỘNG HỌC THUẬN VÀ ĐỘNG HỌC
NGƯỢC ROBOT SCARA
I. GIỚI THIỆU VỀ ROBOT SCARA
I.1.Một số thông số cơ bản:
Tải trọng lớn nhất ở khâu
cuối
100 kg
Cácgiới hạn
chuyển
động của
các khâu
1θ 3000
2θ 3200
Tốc độ lớn
nhất
.
1θ 1000
.
2θ 1250
/s

Độ chính xác lặp lại vị trí mm1±
Khối lượng 200 kg
Phạm vi ứng dụng chủ yếu
Vận chuyển các
đồ vật giữa các vị
trí trong dây
chuyền sản xuất
I.2. Kết cấu cơ khí
Robot được kết cấu theo dạng Scara là phương án thích hợp để vận
chuyển các vật có khối lượng lớn. Cỏc khõu quay được truyền động bằng
động cơ điện. Tốc độ quay của các động cơ điện điều khiển được nhờ bộ biến
tần công suất 1,5 kw.
I.3. Hệ thống điều khiển
Robot được điều khiển nhờ bộ lập trình PLC. Các động cơ điện có phản
hồi vị trí và điều khiển tốc độ quay nhờ bộ biến tần. Hệ thống điều khiển yêu
cầu phản hồi đảm bảo hoạt động ổn định và an toàn cho robot.
II. TÍNH TOÁN ĐỘNG HỌC THUẬN ROBOT SCARA
II.1. Đặt bài toán
Biết giá trị các biến khớp θi(i=1..2), cần phải xác định vị trí và hướng
của bàn kẹp, tức là phải xác định ma trận TE
TE =












1000
zzzz
yyyy
xxxx
pasn
pasn
pasn
II.2. Sơ đồ động học

Ta có bảng tham sè DH như sau:
Khâu iθ iα ai di Khớp
1 *
1θ 0 a1 d1 R
2 *
2θ 180 a2 d2 R
*: biến khớp, R: khớp quay
II.3. Xác định vị trí và hướng của bàn kẹp
Từ các ma trận trên ta cú cỏc ma trận DH :
Ma trận DH chuyển từ hệ toạ độ 0 sang hệ toạ độ 1 :
Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung
x0
y0
z0
y1
z1
x1
d1
a1
a2
θ1
θ2
y2
x2
z2
53

0
H1=











 −
0000
100
)sin(0)cos()sin(
)cos(0)sin()cos(
1
1111
1111
d
a
a
θθθ
θθθ
(3.1)
Ma trận DH chuyển từ hệ toạ độ 1 sang hệ toạ độ 2 : 1
H2=












−
−
0000
100
)sin(0)cos()sin(
)cos(0)sin()cos(
2
1222
2222
d
a
a
θθθ
θθθ
(3.2)
Ma trận DH chuyển từ hệ toạ độ 0 sang hệ toạ độ 2 :
0
T3=0
H1
1
H2=












+−
+++−+
++++
0000
100
)sin()sin(0)cos()sin(
)cos()cos(0)sin()cos(
21
112122121
112122121
dd
aa
aa
θθθθθθθ
θθθθθθθ
(3.3)
Cho TE = 0
T3 ta được hệ phương trình động học thuận robot Scara:
nX=cos(1+2)(3.4) (3.4)
nY=sin(1+2)(3.5) (3.5)
nZ
=0(3.6) (3.6)
sX=sin(1+2)(3.7) (3.7)
sY=-cos(1+2)(3.8) (3.8)
sZ
=0(3.9) (3.9)
aX=0(3.10) (3.10)
aY=0(3.11) (3.11)
aZ=-1(3.12) (3.12)
pX=a2cos(1+2)+a1cos(1)(3.13) (3.13)
pY=a2sin(1+2)+a1sin(1)(3.14) (3.14)

pZ
=d1+d2(3.15) (3.15)
Hệ phương trình từ (3.4) đến (3.15) xác định vị trí và hướng của bàn kẹp
theo biến khớp.
III. ĐỘNG HỌC NGƯỢC ROBOT SCARA
III.1. Đặt bài toán:
Cho vị trí và hướng của bàn kẹp tức là biết ma trận TE
. Cần phải xác
định các biến khớp iθ (i=1..2) theo vị trí và hướng bàn kẹp.
III.2. Giải bài toán ngược động học.
- Tìm 2θ
Từ phương trình (3.13) và (3.14) ta có:
pX=a2cos(1+2)+a1cos(1) (3.13)
pY=a2sin(1+2)+a1sin(1) (3.14)
Bình phương hai vế phương trình (3.13) và (3.14) ta được:
( ) ( ) ( ) ( ) 2
11212
2
11212
22
]sinsin[]sincos[ θθθθθθ aaaapp yx +++++=+
hay ( )221
2
2
2
1
22
cos2 θaaaapp yx ++=+
( )2cos θ
21
2
2
2
1
22
2 aa
aapp yx −−+
=
Đặt ( )ii CosC θ= , ( )ii SinS θ=

2C
21
2
2
2
1
22
2 aa
aapp yx −−+
=
2
22 1 CS −±=
( )222 2 CSarctg=θ (3.16)
- Tìm 2θ
Từ phương trình (3.13) và (3.14) ta có:



=++
=+
y
x
p
pa
1121212
1121212
Sa)SCC(Sa
C)SS-C(Ca



=++
=−+
y
x
pSaaCCSa
pSSaCaaC
1122122
1221122
).(.
.).(
Giải hệ phương trình trên với các Èn C1, Si ta được:
2
22
2
122
22122
1
)()(
)(
CSaaC
paSpaaC
C xx
++
++
=
2
22
2
122
22122
1
)()(
)(
aSaaC
paSpaaC
S
xy
++
−+
=
1=arctg2(S1, C1)
1= arctg2( (C2a2 + a1 )py - S2a2px, (C2a2 + a1 )px + S2a2py ) (3.17)
Các biểu thức (3.16) và (3.17) xác định nghiệm của bài toán động học ngược
robot Scara.
IV. TÍNH LỰC TÁC DỤNG VÀO KHÂU 1 VÀ KHÂU 2
Áp dụng hàm Lagrange, ta có:
L=K - P
Trong đó K là tổng động năng của hệ thống.

P là tổng thế năng
Khi đó:
∑=
=
n
i
iKK
1
∑=
=
n
i
iPP
1
Ở đây Ki là tổng động năng của khâu thứ i, Pi là tổng thế năng của khâu
thứ i. Mặt khác mỗi đại lượng Ki, Pi là một hàm phụ thuộc nhiều biến số:
),...,,,...,(
..
2
.
121 nn qqqqqqKK =
),(
.
ii qqPP =
Mỗi biến khớp qi bao gồm:
iθ là góc quay quanh trục i
di là lượng tịnh tiến dọc theo trục i
Lực tác dụng lờn khõu thứ i có thể là một lực hoặc một mô men, là lục
được xác định bởi:
i
i
i
q
L
q
L
dt
d
F
∂
∂
−
∂
∂
= .
Áp dông cho Robot SCARA hai khõu cú chiều dài a1 và a2 với các khối
lượng tương ứng là m1 và m2. Các biến quay hoạt động với biến 1θ và 2θ . Ta
tính lực F1 và F2.
Khâu 1: k k1= 2
1
m1v2
1 = 2
1
m1d2
1
2.
1θ
P1 =- m1gd1
)cos( 1θ
g= 9,8
m/s
x2 x2
x2
d1
y1
= -d1
cos
y2
m2
m2

Khâu 2: về mặt toạ độ:
X2 = )sin()sin( 21211 θθθ ++ dd
Y2= )cos()cos( 21211 θθθ +−− dd
Chiều cao thế năng :
)cos()cos( 21211 θθθ ++= ddh
Về mặt vận tốc:
V2
2=
.
x
2
2+
.
y
2
2(v2=x2i+y2j).
2
.
X = dt
d
x2= )cos( 11 θd ))(cos( 21
212
1
dt
d
dt
d
d
dt
d θθ
θθ
θ
+++
))(cos()cos(
.
2
.
1212
.
111 θθθθθθ +++= dd
2
.
Y = dt
d
Y2= ))(sin()sin(
.
2
.
1212
.
111 θθθθθθ +++ dd
)])(cos(2)2([
.
2
.
1
.
2
1221
.
2
2
.
2
.
1
.
2
1
2
2
2.
1
2
1
2.
2
2.
2
2
2 θθθθθθθθθ +++++=+= ddddyxv
Vậy:
)])(cos(2)2([
2
1
2
1 .
2
.
1
.
2
1221
.
2
2
.
2
.
1
.
2
1
2
2
.
2
1
2
12
2
222 θθθθθθθθθ +++++== ddddmvmK
)]cos()cos([ 2121122 θθθ ++−= ddgmP
Áp dụng hàm Lagrange ta có:
L=(K1+K2)-(P1+P2)
)cos()cos()(
))(cos()2(
2
1
)(
2
1
21221121
.
2
.
1
.
2
12212
.
2
2
.
2
.
1
.
2
1
2
22
.
2
1
2
121
θθθ
θθθθθθθθθ
++++
+++++++=
gdmgdmm
ddmdmdmmL
Khi tính lực tổng quát ta sử dụng các biến.
2211 θθ == qq
Khâu 1:
.
22212
.
12212
.
2
.
1
2
22
.
1
2
121.
1
.
1
)cos()cos(2)()( θθθθθθθ
θ
ddmddmdmdmm
L
q
L
+++++=
∂
∂
=
∂
∂

..
22212
.
2
22212
..
12212
.
1
.
22122
..
2
..
1
2
22
..
1
2
121.
1
)cos()sin(
)cos(2)sin(2)()(
θθθθ
θθθθθθθθ
θ
ddmddm
ddmddmdmdmm
L
dt
d
+−
−+−+++=
∂
∂
)sin()sin()( 21221121
11
θθθ
θ
+−+−=
∂
∂
=
∂
∂
gdmgdmm
L
q
L
Vậy :
)sin()sin()(
)sin()sin(2)]cos([
)]cos(2)[(
21221121
.
2
22212
.
1
.
22212
..
22212
2
22
..
12212
2
22
2
121
1
.
1
1
θθθ
θθθθθθθ
θθ
θθ
++++
−−++
++++=
∂
∂
−
∂
∂
=
gdmgdmm
ddmddmddmdm
ddmdmdmm
LL
dt
d
F
Ta thấy rõ: Muốn cho khâu 1 quay một góc θ thì động cơ phải tạo ra
một lực tổng quát ≥F1. Lực tổng quát này có đặc tính phi tuyến, là hợp tác
dụng của nhiều yếu tố (non linear and cuppling).
Tương tù ta có:
)sin()sin(2)sin(
)]cos([
2122
.
2
.
12212
.
2
12212
..
2
2
22
..
12211
2
22
22
2
θθθθθθθ
θθθ
θθ
+−−−
−++=
∂
∂
−
∂
∂
=
gdmddmddm
dmddmdm
LL
dt
d
F
IV. ÁP DỤNG CỤ THỂ
Để tính toán động học Robot ta sử dụng hệ phương trình đại số
MAPLE
Lập trình MAPLE
> restart;
> restart;BBN:=proc()
local p1,p2,l1,l2,q1,q2,q3,d1;Zt,Yt,Zt,aq1,aq2,aaq1,aaq2;
> d1:=readstat("Nhap d1:=");
> l1:=readstat("Nhap l1:=");
> l2:=readstat("Nhap l2:=");

> p1:=readstat("Nhap phuong trinh chuyen dong Xt:=");
> p2:=readstat("Nhap phuong trinh chuyen dong Yt:=");
> p3:=readstat("Nhap phuong trinh chuyen dong Zt:=");
> q2:=arccos(((p1*p1)+(p2*p2)-(l1*l1)-(l2*l2))/(2*l1*l2));
>q1:=arctan(((p1*l2*sin(q2))+(p2*(l1+(l1*cos(q2)))))/((p1*(l1+
(l2*cos(q2))))-(p2*l2*sin(q2))));
> print("Phuong trinh q1=",simplify(q1,trig));
> print("Phuong trinh q2=",simplify(q2,trig));
> q3:=d1-p3;
> print("Phuong trinh q3=",q3);
> print("van toc q1=",simplify(simplify(q1,t)));
> print(plot(q1,t=0..1,title="Do thi q1(t)"));
> print(plot(q2,t=0..1,title="Do thi q2(t)"));
> aq1:=diff(q1,t);print(plot(aq1,t=0..1,title="Do thi van toc q1"));
> aaq1:=diff(q1,t$2);print(plot(aaq1,t=0..1,title="Do thi gia toc q1"))
aaq2:=diff(q2,t$2);print(plot(aaq2,t=0..1,title="Do thi gia toc q2"));
1. Khi điểm đầu P của bàn kẹp chuyển động theo phương trình đường tròn
( ) ( )txp π2cos300100 +=
( ) ( )typ π2sin300600 +=
( ) 200=zp

Nhập dữ liệu vào chương trình tính toán MAPLE ta có:
Nhap 300:1 =d
Nhap 950:1 =l
Nhap 800:1 =d
Nhap phuong trinh chuyen dong ( );)**2cos*300100: tpiXt +=
Nhap phuong trinh chuyen dong ( );)**2sin*300600: tpiYt +=
Nhap phuong trinh chuyen dong ;200:=Zt
Ta có phương trình
“PHUONG TRINH q1=”, )2cos(20784161439(16(
16
1
arctan( tsqrt π+−−
(48))2sin()2cos(6912)2cos(20160)2sin(124704 2
sqrttttt −−++ ππππ
2
)2cos(20160)2sin(124704)2cos(20784161439 ttt πππ +++
)2cos(273628158)2cos()2sin()2cos(6912 tttt ππππ −−−
289/())2sin()2cos(1368)2sin(26391)2cos(8208 2
tttt ππππ −−+
6)2sin()2cos(432)2sin(144)2cos(72)2cos(891 2
−++++ ttttt πππππ
2
)2cos(20160)2sin(124704)2cos(20784161439( tttsqrt πππ +++
)2cos(20784161439(3))2sin()2cos(6912 tsqrttt πππ +−−
)))2sin())2sin()2cos(6912)2cos(20160)2sin(124704 2
ttttt πππππ −++

“PHUONG TRINH q2=”, 





++− )2sin(
38
9
)2cos(
76
3
608
433
arccos tt ππ
và các đồ thị tương ứng là:
Tiếp theo là đồ thị vận tốc, gia tốc:

2. Khi đầu điểm P có quĩ đạo là hình elip có phương trình:

)2(100100 tPx π+= , )2sin(300600 tPy π+= , 200=Pz
“PHUONG TRINH q1=”,
42
)2cos(1024)2cos(7040161439(16(
16
1
arctan( ttsqrt ππ −−−−
32
)2cos(512)2cos(6928)2sin()2cos(201609216 tttt ππππ +++
161439(16))2sin(124704)2sin()2cos(2304 sqrtttt −+− πππ
)2sin()2cos(9216)2cos(1024)2cos(7040 242
tttt ππππ +−−
)2sin()2cos(2304)2cos(512)2cos(6928 3
tttt ππππ −++
2
)2(cos(11856)2cos(91228158)2sin()2cos(124704 tttt ππππ +−−+
289/())2sin()2cos(1824)2sin()2cos(456)2sin(26391 2
ttttt πππππ +−−
32
)2cos(32)2sin(144)2cos(24)2cos(297 tttt ππππ −+−+
4
)2cos(1024)2cos(7040161439(6)2cos()2sin(144 ttsqrttt ππππ −−−+
32
)2cos(512)2cos(6928)2sin()2(cos(9216 tttt ππππ +++
161439(3))2sin(124704)2sin()2cos(2304 sqrtttt −+− πππ
)2sin()2cos(9216)2cos(1024)2cos(7040 242
tttt ππππ +−−
)2sin()2cos(2304)2cos(512)2cos(6928 3
tttt ππππ −++
)))2sin()2sin(124704 tt ππ+

“PHUONG TRINH q2=”, 





+−+− )2sin(
38
9
)2cos(
19
1
)2cos(
76
1
608
433
arccos 2
ttt πππ
Tương ứng với đồ thị
Tiếp đó là đồ thị vận tốc và gia tốc:

CHƯƠNG IV
ĐIỀU KHIỂN ROBOT SCARA
I. ĐỘNG CƠ MỘT CHIỀU (DC)
I.1. Đại cương về động cơ một chiều (DC)
I.1.1. Động cơ điện một chiều (DC)
Động cơ điện một chiều gồm có hai phần (xem hình 4.1)
1. Stato cố định với các cuộn dây có dòng điện cảm hoặc dùng nam
châm vĩnh cửu. Phần này còn gọi là phần cảm. Phần cảm tạo nên từ thông
trong khe hở không khí.
2. Rụto với các thanh dẫn. Khi có dòng điện một chiều chạy qua và với
dòng từ thông xác định, rụto sẽ quay. Phần này gọi là phần ứng.
Trên hình vẽ : 1- thanh dẫn, 2 - chổi góp, 3 - cuộn dây, 4 - vũng gúp
-
+
2
4β
H×n h 4.1 s¬ ®å ®é n g c ¬ ®iÖn mé t c h iÒu
3
roto
2
4
stato

Do cách khác nhau khi bố trí dây quấn phần cảm so với phần ứng ta có
những loại động cơ điện một chiều khác nhau:
- Động cơ kích từ song song.
- Động cơ kích từ nối tiếp.
- Động cơ kích từ hỗn hợp
Các đại lượng chủ yếu để xác định chế độ làm việc của động cơ điện
một chiều là:
U- điện áp cung cấp của phần ứng.
I- cường độ dòng điện trong phần ứng.
r- điện trở trong của phần ứng.
Φ- từ thông trong khe hở không khí.
E- sức phản điện động phần ứng.
Các quan hệ cơ bản khi làm việc là:
Φ=−= knrIUE (4.1)
k: phụ thuộc vào đặc tính của dây quấn và số thanh dẫn tác dụng của
phần ứng.
Từ (4.1) ta cú cỏc nhận xét sau:
1) Khởi động E bằng không khi mở máy, chỉ có điện trở phần ứng r
rất nhỏ hạn chế dòng điện. Vì vậy cần phải có biến trở mở máy
để duy trì I ở giá trị thích hợp
2) Số vòng quay:
Φ
−
=
k
rU
N (4.2)
Vậy điều chỉnh tốc độ có thể tiến hành bằng cách tác động vào điện áp
U hoặc tác động vào từ thông Φ.

3) Mô men động C xác định từ phương trình cân bằng công xuất.
nCEI Π= 2 (4.3)
kết hợp với (4.1) ta có: Π
Φ
=
Ik
C 2
(4.4
I.1.2. Điều chỉnh tốc độ động cơ điện một chiều.
Về phương diện điều chỉnh tốc độ thì động cơ điện một chiều có nhiều
ưu việt hơn hẳn các động cơ khác. Khả năng điều chỉnh tốc độ dễ dàng trong
dải rộng và có cấu trúc mạch lực và mạch điều khiển đơn giản.
Có hai phương pháp cơ bản để điều chỉnh tốc độ động cơ điện một chiều:
- Tác động lên từ thông Φ thông qua việc điều chỉnh điện áp dũng
kớch từ
- Điều chỉnh điện áp phần ứng.
Khi điều chỉnh tốc độ từ 0 đến tốc độ định mức bằng cách giữ từ thông
không đổi và tác động vào điện áp phần ứng U thì mô men sẽ không đổi, còn
công suất tăng theo tốc độ.
Khi điều chỉnh tốc độ từ 0 đến tốc độ định mức bằng cách tác động lên
từ thông và giữ điện áp phần ứng không đổi thì công suất không đổi, còn mô
men giảm theo tốc độ.
Khi từ thông tiến về 0 thì tốc độ tiến đến vô cùng (4.2). Vì vậy khi
không tải động cơ kích từ nối tiếp có tốc độ quá lớn, các loại động cơ kích từ
song song hoặc hỗn hợp đều quá tốc độ nếu cắt mạch kích từ của nó.
I.1.3. Đảo chiều quay
Chiều quay của phần ứng phụ thuộc vào chiều dòng điện trong dây
quấn phần ứng và chiều của từ trường. Để đổi chiều quay của động cơ điện

Robot Scara - Tính Toán Động Học & Điều Khiển

Robot Scara - Tính Toán Động Học & Điều Khiển

Empfohlen

Empfohlen

Weitere ähnliche Inhalte

Was ist angesagt?

Was ist angesagt? (20)

Ähnlich wie Robot Scara - Tính Toán Động Học & Điều Khiển

Ähnlich wie Robot Scara - Tính Toán Động Học & Điều Khiển (20)

Kürzlich hochgeladen

Kürzlich hochgeladen (20)

Robot Scara - Tính Toán Động Học & Điều Khiển