luan van robot
DESCRIPTION
Đồ án tính toán thiết kế và điều khiển cho Robot RTR. Ứng dụng OpenGl để mô phỏng hoạt động RobotTRANSCRIPT
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Lời nói đầu
Trong sự nghiệp công nghiệp hóa, hiện đại hóa đất nước vấn đề tự động hóa
có vai trò đặc biệt quan trọng.
Nhằm nâng cao năng suất dây chuyền công nghệ, nâng cao chất lượng và khả
năng cạnh tranh của sản phẩm, cải thiện điều kiện lao động, nâng cao năng suất lao
động…vấn đề được đặt ra là hệ thống sản xuất phải có tính linh hoạt cao.Robot công
nghiệp, đặc biệt là những tay máy robot là bô phận quan trọng để tạo ra những hệ
thống đó.
Tay máy Robot đã có mặt trong sản xuất từ nhiều năm trước, ngày nay tay
máy Robot đã dùng ở nhiều lĩnh vực sản xuất, từ những ưu điểm mà tay máy Robot
hoạt động trong quá trình sản xuất, làm việc, chúng ta có thể đúc rút ra những tính
năng mà con người không thể có được : khả năng làm việc ổn định, có thể làm việc
trong môi trường độc hại…..Do đó việc đầu tư nghiên cứu, chế tạo ra những tay máy
Robot phục vụ cho công cuộc tự động hóa sản xuất là rất cần thiết cho hiện tại và
tương lai.
Môn học giúp chúng em bước đầu làm quen, tìm hiểu,học cách giải các bài
toán động học thuận và động học ngược hay những bài toán tĩnh học, động lực học
... đối với những mô hình tay máy Robot công ngiệp cơ bản và giúp chúng em củng
cố và mở rộng kiến thức phục vụ cho hoạt động học tập, nghiên cứu cũng như công
việc sau này.
Trong quá trình học còn nhiều điều chưa hiểu cặn kẽ cũng như những thiếu
sót , mong thầy giúp đỡ chỉ bảo thêm.
***
Cuối cùng em xin chân thành cảm ơn và chúc thầy nhiều sức khỏe, gia đình hạnh
phúc, công tác tốt !
Thực hiện
ĐINH GIA NGHIÊM
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
CHƯƠNG I : TỔNG QUAN VỀ ROBOT CÔNG
NGHIỆP
1. Sơ lược lịch sử quá trình phát triển của robot công nghiệp
Hình 1.1. Hình ảnh robot công nghiệp
Thuật ngữ “Robot” xuất phát từ tiếng Sec (Czech) “Robota” có nghĩa là công
việc tạp dịch trong vở kịch Rossum’s Universal Robots của Karel Capek, vào năm
1921. Trong vở kịch này, Rossum và con trai của ông ta đã chế tạo ra những chiếc
máy gần giống với con người để phục vụ con người. Có lẽ đó là một gợi ý ban đầu
cho các nhà sáng chế kỹ thuật về những cơ cấu, máy móc bắt chước các hoạt động
cơ bắp của con người. Đầu thập kỷ 60, công ty Mỹ AMF (American Machine and
Foundry Company) quảng cáo một loại máy tự động vạn năng và gọi là “Người máy
công nghiệp” (Industrial Robot). Ngày nay người ta đặt tên người máy công nghiệp
(hay robot công nghiệp) cho những loại thiết bị có dáng dấp và một vài chức năng
như tay người được điều khiển tự động để thực hiện một số thao tác sản xuất.
Theo viện nghiên cứu Robot của Mỹ đề xuất định nghĩa: “Robot công nghiệp
là tay máy vạn năng hoạt động theo chương trình và có thể lập trình lại để hoàn
thành và nâng cao hiệu quả hoàn thành các nhiệm vụ khác nhau trong công nghiệp,
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
như vận chuyển nguyên vật liệu, chi tiết, sản phẩm, dụng cụ hoặc các thiết bị chuyên
dụng khác”.
Về mặt kỹ thuật, những robot công nghiệp ngày nay, có nguồn gốc từ hai
lĩnh vực kỹ thuật ra đời sớm hơn đó là các cơ cấu điều khiển từ xa (Teleoperators)
và các máy công cụ điều khiển số (NC - Numerically Controlled machine tool). Các
cơ cấu điều khiển từ xa (hay các thiết bị kiểu chủ-tớ) đã phát triển mạnh trong chiến
tranh thế giới lần thứ hai nhằm nghiên cứu các vật liệu phóng xạ. Người thao tác
được tách biệt khỏi khu vực phóng xạ bởi một bức tường có một hoặc vài cửa quan
sát để có thể nhìn thấy được công việc bên trong. Các cơ cấu điều khiển từ xa thay
thế cho cánh tay của người thao tác. Nó gồm có một bộ kẹp ở bên trong (tớ) và hai
tay cầm ở bên ngoài (chủ). Cả hai, tay cầm và bộ kẹp, được nối với nhau bằng một
cơ cấu sáu bậc tự do để tạo ra các vị trí và hướng tuỳ ý của tay cầm và bộ kẹp. Cơ
cấu dùng để điều khiển bộ kẹp theo chuyển động của tay cầm. Vào khoảng năm
1949, các máy công cụ điều khiển số ra đời, nhằm đáp ứng yêu cầu gia công các chi
tiết trong ngành chế tạo máy bay. Những robot đầu tiên thực chất là sự nối kết giữa
các khâu cơ khí của cơ cấu điều khiển từ xa với khả năng lập trình của máy công cụ
điều khiển số.
Dưới đây chúng ta sẽ điểm qua một số thời điểm lịch sử phát triển của người
máy công nghiệp. Một trong những robot công nghiệp đầu tiên được chế tạo là robot
Versatran của công ty AMF, Mỹ. Cũng vào khoảng thời gian này ở Mỹ xuất hiện
loại robot Unimate -1900 được dùng đầu tiên trong kỹ nghệ ôtô. Tiếp theo Mỹ, các
nước khác bắt đầu sản xuất robot công nghiệp : Anh -1967, Thuỵ Điển và Nhật -
1968 theo bản quyền của Mỹ; CHLB Đức -1971; Pháp - 1972; ở Ý - 1973. . . Tính
năng làm việc của robot ngày càng được nâng cao, nhất là khả năng nhận biết và xử
lý. Năm 1967 ở trường Đại học tổng hợp Stanford (Mỹ) đã chế tạo ra mẫu robot hoạt
động theo mô hình “mắt-tay”, có khả năng nhận biết và định hướng bàn kẹp theo vị
trí vật kẹp nhờ các cảm biến. Năm 1974 Công ty Mỹ Cincinnati đưa ra loại robot
được điều khiển bằng máy vi tính, gọi là robot T3 (The Tomorrow Tool : Công cụ
của tương lai). Robot này có thể nâng được vật có khối lượng đến 40 KG. Ngày nay,
khoa học về Robot “Robotics” phát triển mạnh mẽ, ngày càng hiện đại ứng dụng
trong rất nhiều lĩnh vực khác nhau: Công nghiệp, thám hiểm vũ trụ, giáo dục, y tế….
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
2. Các khái niệm và định nghĩa về robot công nghiệp.
Robot công nghiệp có thể được định nghĩa theo một số tiêu chuẩn sau:
Theo tiêu chuẩn AFNOR của Pháp: Robot công nghiệp là một cơ cấu chuyển động
tự động có thể lập trình, 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 tọa độ, có khả năng định vị, định hướng, di chuyển các đối tượng vật
chất như chi tiết, đạo cụ, gá lắp theo những hành trình thay đổi đã được chương trình
hóa nhằm thực hiện các nhiệm vụ công nghệ khác nhau.
Theo tiêu chuẩn RIA của Mỹ (Robot institute of America): Robot là một tay
máy vạn năng có thể lặp lại các chương trình, được thiết kế để di chuyển vật liệu, chi
tiết, dụng cụ, hoặc các thiết bị chuyên dùng thông qua các chương trình chuyển động
có thể thay đổi để hoàn thành các nhiệm vụ khác nhau.
Theo tiêu chuẩn TOCT 25686-85 của Nga: Robot công nghiệp là một máy tự
động, được đặt cố định hoặc di động được, liên kết giữa một tay máy và một hệ
thống điều khiển theo chương trình, có thể lặp đi lặp lại để hoàn thành các chức
năng vận động và điều khiển trong quá trình sản xuất.
Do đó, robot công nghiệp có thể được hiểu là những thiết bị tự động linh
hoạt, thực hiện các chức năng lao động công nghiệp của con người dưới một hệ
thống điều khiển theo những chương trình đã được lập trình sẵn.
Với đặc điểm có thể lập trình lại được, robot công nghiệp là thiết bị tự động
hóa và ngày càng trở thành bộ phận không thể thiếu được của các hệ thống sản xuất
linh hoạt. Vì vậy, robot công nghiệp trở thành phương tiện hữu hiệu để tự động hóa,
nâng cao năng suất lao động và giảm nhẹ cho con người những công việc nặng nhọc,
độc hại dưới sự giám sát của con người.
3.1.Cấu trúc và phân loại robot công nghiệp.
3.1.Cấu trúc chung của robot công nghiệp.
Một Robot công nghiệp được cấu thành bởi các hệ thống sau:
+Cơ cấu chấp hành(Actuator) tạo chuyển động cho các khâu của tay máy.Nguồn
động lực của các cơ cấu chấp hành là động cơ các loại: điện,thủy lực,khí nén..
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
+Hệ thống cảm biến(Sensor) gồm các sensor và các thiết bị chuyển đổi tín hiệu.Các
Robot cần hệ thống sensor bên trong để nhận biết trạng thái của bản thân các cơ cấu
của Robot và các sensor ngoài để nhận biết trạng thái của môi trường tác động .
+Hệ thống điều khiển(Controller) hiện nay thường là máy tính để giám sát và điều
khiển hoạt động của Robot.
+Tay máy(Manipulator) là cơ cấu cơ khí gồm các khâu,các khớp.Chúng hình thành
cánh tay để tạo chuyển động cơ bản,cổ tay tạo nên sự khéo léo linh hoạt và bàn tay
(End Effector) để trực tiếp hoàn thành các thao tác trên đối tượng.
3.2. Bậc tự do của robot .
Bậc tự do của Robot là đại lượng đặc trưng cho khả năng linh hoạt của
chuyển động của Robot. Robot có số bậc tự do càng lớn thì khả năng linh hoạt càng
cao nhưng cấu trúc Robot càng trở nên phức tạp. Cơ cấu tay của robot phải được cấu
tạo sao cho khâu cuối phải có vị trí và theo một hướng nhất định nào đó và dễ dàng
di chuyển dễ dàng trong vùng làm việc. Muốn vậy cơ cấu tay của robot phải đạt
được một số bậc tự do chuyển động. Để tính số bậc tự do của robot thì có nhiều công
thức được đề xuất, dưới đây ta đưa ra cách tính dựa vào định lý Gruebler. Định lý
này được sử dụng phổ biến trong nhiều tài liệu hiện nay.
Theo Gruebler thì bậc tự do f được tính theo công thức:
0
1
.( 1) ( )
g
i
i
f n f f
f : Là số bậc tự do của cơ cấu.
: Bậc tự do của một vật rắn không chụi liên kết trong không gian làm việc của
robot ( = 3 ứng với không gian làm việc trong mặt phẳng, = 6 ứng với không gian
làm việc trong không gian).
n : Số khâu ( kể cả giá cố định).
if : Số bậc tự do của khớp thứ i.
g : Tổng số khớp của cơ cấu.
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
0f : Số bậc
3.3. Phân loại Robot.
3.3.1Phân loại theo phương pháp điều khiển.
Có 2 kiểu điều khiển robot: Điều khiển hở và Điều khiển kín.
Điều khiển hở: dùng truyền động bước (động cơ điện hoặc động cơ thủy lực,
khí nén) mà quãng đường hoặc góc dịch chuyển tỷ lệ với xung điều khiển. Kiểu này
đơn giản nhưng cho độ chính xác thấp.
Điều khiển kín: (điều khiển kiểu servo) sử dụng tín hiệu phản hồi vị trí để
tăng độ chính xác điều khiển. Có hai kiểu điều khiển servo: Điều khiển điểm-điểm
và điều khiển theo đường (contour).
Kiểu điều khiển điểm-điểm: phần công tác dịch chuyển từ điểm này đến điểm
kia theo đường thẳng với tốc độ không cao. Kiểu điều khiển này thường được dùng
trên các Robot hàn điểm, vận chuyển, tán đinh và bắn đinh.
Điều khiển contour: đảm bảo cho phần công tác dịch chuyển theo quỹ đạo bất
kì, với tốc điều khiển được. Có thể gặp kiểu điều khiển này trên các Robot hàn hồ
quang và phun sơn.
3.3.2. Phân loại theo ứng dụng.
Dựa vào những ứng dụng của robot trong sản xuất ta có những loại robot sau:
robot sơn, robot hàn, robot lắp ráp, robot dùng trong ngành dịch vụ, robot chuyển
phôi.
Hình 1.2. Robot hàn công nghiệp Hình 1.3. Robot trong nghành dịch vụ
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
4. Ứng dụng của Robot công nghiệp trong sản xuất
Trên thế giới:
Hiện nay trên thế giới, do nhu cầu sử dụng robot ngày càng nhiều trong các
quá trình sản xuất phức tạp với mục đích góp phần nâng cao năng suất dây chuyền
công nghệ, giảm giá thành, nâng cao chất lượng, và nâng cao 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, nên robot công nghiệp cần có
những khả năng thích ứng tốt và thông minh hơn với những cấu trúc đơn giản và
linh hoạt.
Có thể kể đến một số ứng dụng điển hình của robot trên thế giới như:
Robot song song dùng trong phân loại và đóng gói sản phẩm: IRB 660 Flex
Palletizer, IRB 340 FlexPicker, IRB 260 FlexPicker. Các robot này có thể gắp lần
lượt các hộp vắc xin bại liệt từ băng tải và đặt nó vào thùng gồm 20 hộp một cách
chính xác.
Robot dùng trong công nghệ ép phun nhựa: IRB 6650 của hãng ABB có thể
thao tác nhanh, dễ dàng lấy sản phẩm ra khỏi khuôn ở vị trí tách khuôn, giám sát,
làm sạch, điều khiển chất lượng dựa trên camera……
Tại Việt Nam:
Nước ta ứng dụng của robot công nghiệp rất đa đạng, tùy vào những nghành,
công việc khác nhau mà ta có thể áp dụng những robot công nghiệp riêng biệt. Dưới
đây là một số nghành trong hệ thống sản xuất mà áp dụng robot công nghiệp.
Công nghiệp đúc: robot làm nhiệm vụ rót kim loại nóng chảy vào khuôn, cắt mép
thừa, làm sạch vật đúc hoặc làm tăng bền vật đúc bằng cách phun cát.
Ngành gia công áp lực: các quá trình hàn và nhiệt luyện thường bao gồm nhiều công
việc độc hại và ở nhiệt độ cao, điều kiện làm việc khá nặng nề, dễ gây mệt mỏi nhất
là ở trong các phân xưởng rèn dập.
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Chương 2. Động học robot
1.Mô hình robot 3 bậc tự do
Hình 2.1: Mô hình robot 3 bậc tự do
Mô hình được thiết kế 3D trên phần mền Solidworks 2014. Đây là mô hình đơn giản
chỉ để cho người xem hiểu được cơ cấu của Robot.
2. Thiết lập động học Robot
2.1. Thiết lập hệ tọa độ Denavit-Hartenberg
Denavit-Hartenberg (1955) qui ước hệ tọa độ Decard gắn vào mỗi khâu của một
tay máy Robot như sau:
Trục zi được chọn dọc theo trục của khớp thứ (i+1). Hướng của phép quay và
phép tịnh tiến được chọn tùy ý.
Trục xi được xác định dọc theo đường vuông góc chung giữa trục khớp động thứ i
và i+1, hướng từ khớp động thứ i tới trục i+1.
Trục yi xác định theo quy tắc bàn tay phải.
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 2.2: Sơ đồ thiết lập hệ tọa độ
Các tham số động học Denavit-Hartenberg di, θi , ai, αi
di : Dịch chuyển tịnh tiến giữa 2 đường vuông góc chung của 2 trục. di = Oi-1Oi’ là
dương nếu vector Oi-1Oi’ theo chiều dương của trục zi-1 , âm trong trường hợp ngược
lại.
θi : Góc giữa 2 đường vuông góc chung. Là góc quay quanh trục zi-1 để trục xi-1
chuyển đến trục xi theo quy tắc bàn tay phải.
ai : Khoảng dịch chuyển giữa 2 trục khớp động kề nhau ai = Oi’Oi
αi : Góc lệch giữa 2 trục của khớp động liền kề, là góc quay quanh trục xi sao cho
trục zi-1 chuyển đến trục zi theo quy tắc bàn tay phải.
Từ những điều trên ta vẽ ra sơ đồ động học của robot với cách đặt các hệ tọa độ
như sau:
Khâu đế: Hệ tọa độ O0x0y0z0 gắn vào chân đế của Robot, trục z0 được chọn dọc
theo trục của khớp 1. Trục x0 chọn tùy ý sao cho phù hợp với hình vẽ, trục y0 được
xác định theo
quy tắc tam diện thuận.
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Khâu 1: Ta chọn trục z1 chọn trùng chiều với khớp 2, trục x1 là đường vuông góc
chung của z0z1 có hướng từ z0 đến z1 , trục y1 được xác định theo tam diện thuận.
Khâu 2: Ta chọn trục z2 chọn trùng chiều với khớp 3, trục x2 là đường vuông góc
chung của z1z2 có hướng từ z1 đến z2 , trục y2 được xác định theo tam diện thuận.
Khâu 3: Ta chọn trục z3 song song với z2 , trục x3 là đường vuông góc chung của z2z3
có hướng từ z2 đến z3 , trục y3 được xác định theo tam diện thuận.
z3
z1=z2 a3 x3
z0
x2
a1 d2
1
d1 O1 x1
O0 yo
x0
Hình 2.3: Sơ đồ động học Robot
Từ đó ta lập được bảng Denavit-Hartenberg như sau:
Bảng 1: Bảng động học Denavit-Hartenberg
di θi ai αi
1 d1 θ1 a1 0
2 0 d2 0 0
3 0 θ3 a3 0
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Đặt 1 3 1 3[ , ]=[q ,q ]
Ta có dạng tổng quát của ma trận Denavit-Hartenberg có dạng:
i i i i i i i
i i i i i i ii 1
i
i i i
cosθ sinθ cosα sinθ sinα a cosθ
sinθ cosθ cosα sinα cosθ a sinθA
0 sinα cosα d
0 0 0 1
Từ đó ta có:
Ma trận Denavit-Hartenberg cho khâu 1:
1 1 1 1
0 1 1 1 1
1
1
cos sin 0 cos
sin cos 0 sin
0 0 1
0 0 0 1
a
aA
d
Ma trận Denavit-Hartenberg cho khâu 2:
1
2
2
1 0 0 0
0 1 0 0A
0 0 1 d
0 0 0 1
Ma trận Denavit-Hartenberg cho khâu 3:
3 3 3 3
3 3 3 32
3
cosq sinq 0 a cosq
sinq cosq 0 a sinqA
0 0 1 0
0 0 0 1
Từ đó ta có ma trận của điểm tác động cuối so với hệ tọa độ ban đầu là:
13 13 3 13 1 1
13 13 3 13 1 10 0 1 2
3 1 2 3
1 2
0
0
0 0 1
0 0 0 1
C S a C a C
S C a S a SA A A A
d d
Với 13 1 3 13 1 3cos(q q ),S sin(q q )C
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
2.2. Thiết lập phương trình động học Robot
Do ta có :
13 13 3 13 1 1
13 13 3 13 1 10 0 1 2
3 1 2 3
1 2
0
0
0 0 1
0 0 0 1
C S a C a C
S C a S a SA A A A
d d
Do ma trận 0
3A biểu diễn qua thông số các biến khớp là qi. Trong bài toán cụ thể thì
nó là các khớp xoay θi, với i=1÷3. Từ đó ta kí hiệu 0
3A là 0
3 (q)A
Sử dụng các góc Cardan xác định hướng của vật rắn,ta đưa ra vector tọa độ định vị
khâu thao tác :
pE = , , , , ,E E Ex y z
Trong đó: , ,E E Ex y z mô tả vị trí của điểm tác động cuối E và là các góc
quay Cardan của O3x3y3z3 so với hệ tọa độ O0x0y0z0. Do các tọa độ thao tác đều là
hàm của thời gian. Nên ta có thể biểu diễn:
0 0
0 ( ) ( )( )
0 1
n E
n T
R t r tA t
Trong đó :
0 ( )nR t là ma trận Cardan biểu diễn hướng của hệ trục tọa độ cuối O3x3y3z3 so với hệ
tọa độ ban đầu O0x0y0z0.
0
cos cos cos sin sin
(t) sin sin cos cos sin sin sin sin cos cos sin cos
cos sin cos sin sin cos sin sin sin cos cos
co
s
Cn DRR
0 (t) [x (t),y (t),z (t)]E E E Er mô tả vị trí của điểm tác động cuối so với hệ tọa độ ban đầu
O0x0y0z0.
Do ma trận 0
3 (q)A biểu diễn trạng thái của khâu thao tác, và 0 ( )nA t cũng biểu diễn
hướng và vị trí của khâu thao tác bằng các góc Cardan nên ta có phương trình động
học Robot nhận được trong dạng ma trân như sau :
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
0
3 (q)A =0 ( )nA t trong đó n=3 do có 3 khâu
Hay viết lại ta sẽ có : 0
3 (q)A = 0
3( ) (2.1)A t
Đối với bài toán động học thuận, ở đây, do ta tìm được ngay tọa độ điểm E nên ta ko
viết là 0
3 (t)A mà viết thành 0
3 (p)A
Với :
13 13 3 13 1 1
13 13 3 13 1 10
3
1 2
0
0(q)
0 0 1
0 0 0 1
C S a C a C
S C a S a SA
d d
(2.2)
0
3
cos cos cos sin sin
sin sin cos cos sin sin sin sin cos cos sin cos
cos sin cos sin sin cos sin sin sin cos cA (t)
os cos
0 0 0 1
E
E
E
x
y
z
(2.3)
Từ (1) (2) (3) đồng nhất hệ số ta được hệ phương trình có dạng sau :
11 11
22 22
33 33
1 ( ) 0
2 ( ) 0
3 ( ) 0
4 (q) ( , , ) 0
5 (q) ( , , ) 0
6 (q) ( , , ) 0
E
E
E
f x q x
f y q y
f z q z
f c c
f c c
f c c
(2.4)
2.3. Bài toán động học thuận về vị trí
Với bài toán động học thuận, ta cho trước quỹ đạo chuyển động của các khâu
θ1, θ2 ,θ3 từ đó ta sẽ xác định được quỹ đạo chuyển động của điểm tác động cuối E
theo điều kiện ban đầu đã cho.
Từ hệ phương trình động học (2.4) ở trên ta nhận được vị trí của điểm tác
động cuối
3 13 1 1
3 13 1 1
1 2
( )
( )
( )
E
E
E
x x q a C a C
y y q a S a S
z z q d d
(2.5)
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Sử dụng phần mền Maple ta xây dựng quy luật chuyển động của khâu thao
tác với các thông số đầu vào d1,a1,a2,a3 và cho quy luật chuyển động của θ1, θ2 ,θ3 :
Cho [d1,a1,a3] = [1 ; 0,9 ;0.8]
1
2
3
= cos(5t)
=1 cos(5t)
= sin(5t)
q
d
q
Thay vào hệ phương trình (5) ta được :
0.8cos cos 5 cos sin 5 0.8sin cos 5 sin sin 5 0.9cos cos 5
0.8sin cos 5 cos sin 5 0.8cos cos 5 sin sin 5 0.9sin cos 5
2 cos 5
E
E
E
x t t t t t
y t t t t t
z t
Từ đó ta vẽ được đồ thị của khâu thao tác trong không gian với Maple có dạng như
sau :
Hình 2.4 : Chuyển động của xE theo t
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 2.5 : Chuyển động của yE theo t
Hình 2.6 : Chuyển động của zE theo t
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 2.7 : Chuyển động của khâu thao tác trong không gian
2.4. Bài toán động học thuận về vận tốc và gia tốc
Ta có kết quả :
3 13 1 1
3 13 1 1
1 2
( )
( )
( )
E
E
E
x x q a C a C
y y q a S a S
z z q d d
=>
3 13 1 1 3 131 3
3 13 1 1 3 131 3
2
( S S )
( C C )
E
E E
E
a a q a S qx
v y a a q a S q
zd
Mà .E TEv J q
=>
3 13 1 1 3 13
3 13 1 1 3 13
( S S ) 0
( C C ) 0
0 1 0
TE
a a a S
J a a a S
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Từ ma trận
13 13 3 13 1 1
13 13 3 13 1 10
3
1 2
0
0
0 0 1
0 0 0 1
C S a C a C
S C a S a SA
d d
Ta rút ra ma trận cosin chỉ hướng:
13 13
0
3 13 13
0
0
0 0 1
C S
R S C
Mà
1 30
0
3 3 3 1 3
0 0
0 0
0 0 0
o T
q q
R R q q
=> 3
1 3
0
0o
q q
Hay viết dưới dạng giải tích:
1 3
0
0
Ex
Ey
Ez q q
=>
2 2 2 0,5
5 sin 5 5 co
( )
s 5
E Ex Ey Ez
E
w w
t
w w
w t
Ta có :
x =4sin( cos(5t)) sin(5t)cos( sin(5t))-4cos( cos(5t))sin( sin(5t)) cos(5t)
+4cos( cos(5t)) sin(5t)sin( sin(5t))-4sin( cos(5t))cos( sin(5t)) cos(5t)
+4,5sin( cos(5t)) sin(5t)
E
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
y =-4cos( cos(5t)) sin(5t)cos( sin(5t))-4sin( cos(5t))sin( sin(5t)) cos(5t)
+4sin( cos(5t)) sin(5t)sin( sin(5t))+4cos( cos(5t))cos( sin(5t)) cos(5t)
-4,5cos( cos(5t)) sin(5t)
E
= 5sin (5t)Ez
Ta có vận tốc và gia tốc của khâu thao tác :
2 2 2
E E E Ev x y z
2 2 2
E E E Ea x y z
Dùng maple ta vẽ được đồ thị gia tốc và vận tốc của điểm E như sau :
Hình 2.8 : Đồ thị vận tốc của điểm E
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 2.9 : Đồ thị gia tốc của điểm E
Vận tốc góc của khâu thao tác có dạng :
Hình 2.10 : Đồ thị vận tốc góc của khâu thao tác
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
3. Thiết kế động học ngược Robot
3.1. Giải bài toán động học ngược bằng phương pháp giải tích
Ta có hệ phương trình động học sau :
3 13 1 1
3 13 1 1
1 2
( ) (2.6)
( ) (2.7)
( ) (2.8)
E
E
E
x x q a C a C
y y q a S a S
z z q d d
Với cách giải bằng phương pháp giải tích tức là ta sẽ giải ra 1 2 3,d ,q q bằng cách thế
từng phương trình trong hệ phương trình trên.
Từ phương trình (2.8) ta có thể dễ dàng tìm ra được:
2 1Ed z d
Ta sẽ xét hệ phương trình:
3 1 3 1 1
3 1 3 1 1
cos(q q ) cos
sin(q q ) sin
E
E
x a a q
y a a q
(2.9)
=>
2 2 2 2
3 1 3 1 1 3 1 1 3 1
2 2
3 1 3 1 3
2 (cos(q q )cos sin(q q )sin )
2 cos(q )
E Ex y a a a a q q
a a a a
=>
2 2 2 2
3 13
3 1
2
3 3
3 3 3
cos(q )2
sin(q ) 1 (cos(q ))
arctan 2(sin(q ),cos(q ))
E Ex y a a
a a
q
Ta có: (2.9) 3 3 1 1 3 3 1
3 3 1 3 3 1 1
( cos(q ) )cos sinq sinq
sinq cosq ( cos(q ) )sin
E
E
x a a q a
y a a a q
Giải hệ phương trình đại số tuyến tính:
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
3 3 1 3 3 2 2 2 2
1 3 3 1 3
3 3 3 3 1
3 3
1 1 3 3 3
3 3 1
3 3 1
2 1 3 3 3
3 3
11
cos(q ) sin(q )2 cos(q ) x
sinq cos(q )
sin(q )(x cos(q ) sin(q ))
cos(q )
cos(q )( cos(q ) x sin(q ))
sin(q )
cos(q )
E E
E
E E E
E
E
E E E
E
a a aa a a a y
a a a
x aa x a y
y a a
a a xa y a y
a y
a x
3 3 3
2 2
1 3 3 31 2 2
1 1 1
(x cos(q ) sin(q ))
x
( cos(q ) x sin(q ))sin( )
x
arctan 2(sin(q ),cos(q ))
E E E
E E
E E E
E E
a y
y
a y a yq
y
q
Vậy khi cho các tham số điều kiện đầu ta có thể tìm được 1 2 3,d ,q q
Số bậc của Robot càng lớn thì việc tìm ra đáp án càng phức tạp và càng khó,
vì vậy sau đây chúng em xin trình bày cách giải bài toán động học ngược bằng
phương pháp số.
Phương pháp số ở đây chúng em trình bày cách giải bằng thuật toán Newton-
Rapson sử dụng phần mền Maple để lập trình.
3.2. Giải bài toán động học ngược bằng phương pháp số
Với bài toán động học ngược ta có thể giải bằng phương pháp giải tích cũng như
phương pháp Newton-Raphson. Phương pháp giải tích có thể giải được với những
mô hình có dưới 3 bậc tự do ta có thể giải được, nhưng từ 4 bậc tư do trở lên phương
pháp giải tích cho thấy sự khó khăn trong việc giải toán. Phương pháp Newton-
Raphson khắc phục những nhược điểm đó của phương pháp giải tích.
Ta có hệ phương trình :
1 3 1 3 1 1
2 3 1 3 1 1
3 2 1
cos(q q ) cos
sin(q q ) sin
E
E
E
f x a a q
f y a a q
f z d d
(2.10)
1
2 0
3
f
F f
f
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Bài toán khi biết được xE(t), yE(t), zE(t) tại mỗi thời điểm t ta sẽ tìm được vector
q=[q1, q2, q3]T tại mỗi thời điểm đó.
Ta lấy giá trị sát giá trị đầu để tiến hành quá trình lặp Newton-Raphson
1
1 ( ( )) . ( ) (2.11)k k k kp p J p F p
Quá trình lặp dừng lại khi sai số ở lần k+1 với lần k nhỏ hơn giá trị cho phép.
Áp dụng vào bài toán chữ N
Ở bản thuyết minh này chúng em sẽ thiết lập giải bài toán động học ngược vẽ
ra chữ N trên Maple dùng thuật toán Newton- Rapson
Bài toán cụ thể đặt ra :
Ta sẽ sử dụng thuật toán Newton – Rapson vẽ ra chữ N trong không gian làm
việc của Robot. Và sử dụng lí thuyết về thiết kế quỹ đạo theo đường thẳng ở trên để
tạo ra quỹ đạo chữ N ưng ý nhất.
Ta đi tìm vector q=[q1, q2, q3]T để quỹ đạo điểm tác động cuối E có dạng chữ
N cho trước. Vậy ta sẽ áp dụng phương pháp Newton-Raphson vẽ ra 4 đường và nối
chúng vào với nhau để tạo ra chữ N.
Ví dụ ta vẽ chữ N trên mặt phẳng Oxz với y=1,3 được kết quả như sau :
Phần code sẽ được thêm vào ở phần phụ lục :
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 2.11 : Kết quả vẽ chữ N trong maple
Ta cũng suy ra được tọa độ các biến khớp q1,q2,q3 có dạng như sau :
Hình 2.12 : Chuyển động của q1 theo t
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 2.13 : Chuyển động của d2 theo t
Hình 2.14 : Chuyển động của q3 theo t
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Vậy với phương pháp Newton-Raphson ta có thể vẽ ra các biên dạng phức
tạp theo ý muốn với điều kiện phải nằm trong vùng hoạt động của chúng.
=> Giải bài toán động học ngược của Robot là 1 trong những bước quan trọng để ta
có thể thiết kế 1 đường đi bất kì cho Robot nằm trong vùng làm việc của nó.
Từ đó ta có thể thấy rằng việc ứng dụng Robot trong cuộc sống để phục vụ con
người ngày càng trở lên phổ biến và phục vụ cho lợi ích con người.
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Chương 3. Mô phỏng chuyển động của Robot sử
dụng Visual studio và thư viện OpenGl
1.Giới thiệu về thư viện OpenGl
OpenGL (tiếng Anh Open Graphics Library) là một tiêu chuẩn kỹ thuật đồ
họa có mục đích định ra một giao diện lập trình ứng dụng đồ họa 3 chiều. OpenGL
cũng có thể được dùng trong các ứng dụng đồ họa 2 chiều. Giao diện lập trình này
chứa khoảng 250 hàm để vẽ các cảnh phức tạp từ những hàm đơn giản.
Tuy nhiên OpenGl không hỗ trợ :
- Các hàm nhập xuất hay thao tác trên window
- Các hàm cấp cao để xây dựng các mô hình đối tượng, thay vào đó người dùng phải
tự xây dựng các thành phần hình học cơ bản (điểm, đoạn thẳng, đa giác).
Ngoài thư viện chuẩn OpenGl còn có 1 số thư viện hỗ trợ lập trình OpenGl như thư
viện GLUT.
GLUT (OpenGl Utility Toolkit) là một thư viện nhằm giúp chúng ta có thể
nhanh chóng tạo ra các cửa sổ cũng như quản lý các sự kiện từ bàn phím, chuột mà
không phải quan tâm quá nhiều đến các hàm trực tiếp của OpenGl vì GLUT sẽ gọi
các hàm này thay cho chúng ta.
GLUT không thể thay thế cho OpenGl mà chỉ là một thành phần bổ sung để
lập trình dễ dàng hơn trên OpenGl.
2.Mô phỏng chuyển động của Robot
Các công cụ để xây dựng bài toán mô phỏng :
- Thư viện đồ họa OpenGl
- Ngôn ngữ lập trình Visual C++ và các ứng dung của MFC
- Phần mền xây dựng thiết lập phương trình
- Phần mền thiết kế 3D
Tổng quát lại ta sẽ có sơ đồ xây dựng bài toán mô phỏng như sau :
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 3.1 : Sơ đồ xây dựng bài toán mô phỏng
Từ đó ta có thể nó ngắn ngọn ra các bước để tạo ra một chương trình mô phỏng
Robot như sau :
- Khởi tạo MFC để thực hiện quá trình mô phỏng
- Khởi tạo các gói thư viện của OpenGl
- Thiết kế mô hình 3D trên Solidworks
- Chuyển mô hình sang tập tin .stl
- Xây dựng các modul để đọc file .stl và các thiết lập trên Visual
- Xây dựng modul đọc file quỹ đạo được chuyển đổi từ Maple
- Chạy mô phỏng trên OpenGl
Ở đây chúng em sẽ không nêu lên trình tự khởi tạo cũng như các thao tác xây
dựng modul trên OpenGl vì nó khá dài.
Việc xây dựng mô hình thiết lập giải phương trình động học đã được em nêu
ở chương số 2.
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Ta có mô hình 3D của Robot được vẽ trên Solidworks có dạng như sau :
- Đầu tiên ta vẽ các Part từng khâu 1 và ghép chúng lại với nhau trong Solidworks.
- Mô hình thiết kế đơn giản để trực quan.
Hình 3.2 : Hình ảnh các khâu của Robot
Ghép nối các thành phần với nhau trong Solidworks ta được mô hình 3D như sau :
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 3.3 : Mô hình 3D của Robot
Từ đó ta sẽ xuất ra các file STL để đưa vào mô phỏng. Việc vẽ mô hình 3D
tốt nhất ta nên vẽ nó đúng theo các trục tọa độ ta đã thiết kế để việc đưa chỉnh sửa
trong Visual dễ dàng hơn. Trong đó chúng ta phải chú ý việc khi xuất file STL .
Sau khi chúng ta Save as file Solidworks mà chúng ta vẽ, chúng ta hãy ấn vào
phần Options và lựa chọn đánh dấu tích vào ô (Do not translate STL output data to
positive space) để hệ tọa độ không bị lệch khi ta đưa vào Visual.
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 3.4: Lựa chọn xuất file STL trong Solidworks
Từ đó ta sẽ đưa vào Visual. Trong đó chúng ta sẽ chỉnh sửa các thông số để
các hệ tọa độ đúng như khi chúng ta đã chọn trước. Và đưa chương trình vào để cho
Robot chuyển động theo 1 quỹ đạo xác định từ trước.
Ta sử dụng thư viện MFC để chạy chương trình, từ đó ta sẽ có được môi
trường làm việc của Robot như sau :
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 3.5 : Môi trường mô phỏng Robot
Từ đó chúng em thiết lập Robot chạy và lưu quỹ đạo trên môi trường mô phỏng đó :
Hình 3.6 : Robot vẽ đường thẳng
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Hình 3.7 : Chuyển động của Robot vẽ chữ N
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Kết luận
Để thiết kế chế tạo Robot phục vụ cho việc đẩy mạnh phát triển công nghiệp
hóa và hiện đại hóa thì việc nghiên cứu phát triển bài toán Robot là việc hết sức
quan trọng. Trong bản thuyết minh này, đã nêu lên việc thiết lập động học, động lực
học, điều khiển Robot 3 bậc tự do. Có thể tóm tắt lại nội dung đã được nêu trong đồ
án như sau:
1. Xây dựng mô hình Robot 3 bậc tự do
2. Xây dựng thuật toán giải bài toán động học
3. Xây dựng bài toán thực tế vẽ ra quỹ đạo chữ N
5. Mô phỏng hoạt động của Robot dùng Visual và OpenGL
Tôi xin chân thành cảm ơn thầy PGS.TS Phan Bùi Khôi đã giúp đỡ em rất
nhiều và theo sát để giúp đỡ em hoàn thành môn học này. Bản thuyết minh còn
nhiều thiếu sót, rất mong được sự giúp đỡ của các bạn và các thầy cô để em khắc
phục trong những lần tiếp theo, chúng em xin chân thành cảm ơn.
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Tài liệu tham khảo
1. PGS.TS Phan Bùi Khôi- Bài giảng Robotic. Trường ĐH Bách Khoa Hà Nội 2014.
2. TS. Nguyễn Mạnh Tiến - Điều khiển Robot công nghiệp . Nhà xuất bản Khoa
học và Kỹ thuật, Hà Nội 2007.
3. GS.TSKH Nguyễn Văn Khang- Cơ sở Robot công nghiệp. Nhà xuất bản Khoa
học
và Kỹ thuật, Hà Nội 2011.
4. GS. TSKH. Nguyễn Thiện Phúc- Robot công nghiệp . Nhà xuất bản Khoa học và
Kỹ
thuật, 2004.
5. GS.TS Đào Văn Hiệp- Kỹ thuật Robot. Nhà xuất bản Khoa học và Kỹ thuật, 2013
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Phụ lục
Code Robot vẽ chữ N:
DHTB:=matrix([[theta1,d1,a1,0],
[0,theta2,0,0],
[theta3,0,a3,0]
]):
TMat1:=DHMat(0,0,0,0):
nj:=3:
for i from 1 by 1 to nj do
temp:=DHMat(DHTB[i,1],DHTB[i,2],DHTB[i,3],DHTB[i,4]);
print(temp):
TMat1:=multiply(TMat1,temp):
AI[i]:=copy(TMat1):
print(AI[i]):
end do:
RE:=subvector(TMat1,1..3,4):
d1:=1.00:
a1:=0.90:
a3:=0.80:
#--------------Phantich DLH---------------------
Unknowns:=vector([theta1,theta2,theta3]):
MaxLoop:=50:
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
AbsErr:=1e-100:
a0:=0:
tc:=4:
dt:=0.1:
N0:=round((tc-a0)/dt):
REE:=[0,1.3,1.5+a0]:
init0:=vector([2.222796392,0.5,-1.403348248]):
f1:=x_-RE[1]:
f2:=y_-RE[2]:
f3:=z_-RE[3]:
f:=vector([f1,f2,f3]):
g:=vector([x_,y_,z_]):
Ketqua0:=matrix(N0,nj):
for i from 1 by 1 to N0 do
Jaco_:=jacobian(f,Unknowns):
Eqts:=copy(f):
Jaco:=copy(Jaco_):
REE:=[0,1.3,1.5+a0]:
for j from 1 by 1 to nj do
Eqts:=subsVecto(g[j]=REE[j],Eqts):
Jaco:=subsMat(g[j]=REE[j],Jaco):
end do:
y[i]:=Newton_Raphson(Eqts,Jaco,Unknowns,init0,AbsErr,MaxLoop):
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
for k from 1 by 1 to nj do
Ketqua0[i,k]:=y[i][k]:
y[0][k]:=y[1][k]:
end do:
a0:=a0+0.1*dt:
end do:
print(Ketqua0):
#---------------------------------------------------
Unknowns:=vector([theta1,theta2,theta3]):
MaxLoop:=50:
AbsErr:=1e-100:
a0:=0:
tc:=4:
dt:=0.1:
N1:=round((tc-a0)/dt):
REE:=[a0/2,1.3,1.9-a0]:
init0:=vector([2.222796392,0.9,-1.403348248]):
f1:=x_-RE[1]:
f2:=y_-RE[2]:
f3:=z_-RE[3]:
f:=vector([f1,f2,f3]):
g:=vector([x_,y_,z_]):
Ketqua1:=matrix(N1,nj):
for i from 1 by 1 to N1 do
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
Jaco_:=jacobian(f,Unknowns):
Eqts:=copy(f):
Jaco:=copy(Jaco_):
REE:=[a0/2,1.3,1.9-a0]:
for j from 1 by 1 to nj do
Eqts:=subsVecto(g[j]=REE[j],Eqts):
Jaco:=subsMat(g[j]=REE[j],Jaco):
end do:
y[i]:=Newton_Raphson(Eqts,Jaco,Unknowns,init0,AbsErr,MaxLoop):
for k from 1 by 1 to nj do
Ketqua1[i,k]:=y[i][k]:
y[0][k]:=y[1][k];
end do:
a0:=a0+0.1*dt:
end do:
print(Ketqua1):
#--------------------------------------------------
Unknowns:=vector([theta1,theta2,theta3]):
MaxLoop:=50:
AbsErr:=1e-100:
a0:=0:
tc:=4:
dt:=0.1:
N2:=round((tc-a0)/dt):
REE:=[0.2,1.3,1.5+a0]:
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
init0:=vector([2.057429517,0.5,-1.375105274]):
f1:=x_-RE[1]:
f2:=y_-RE[2]:
f3:=z_-RE[3]:
f:=vector([f1,f2,f3]):
g:=vector([x_,y_,z_]):
Ketqua2:=matrix(N2,nj):
for i from 1 by 1 to N2 do
Jaco_:=jacobian(f,Unknowns):
Eqts:=copy(f):
Jaco:=copy(Jaco_):
REE:=[0.2,1.3,1.5+a0]:
for j from 1 by 1 to nj do
Eqts:=subsVecto(g[j]=REE[j],Eqts):
Jaco:=subsMat(g[j]=REE[j],Jaco):
end do:
y[i]:=Newton_Raphson(Eqts,Jaco,Unknowns,init0,AbsErr,MaxLoop):
for k from 1 by 1 to nj do
Ketqua2[i,k]:=y[i][k]:
y[0][k]:=y[1][k]:
end do:
a0:=a0+0.1*dt:
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
end do:
print(Ketqua2):
#--------------------------------------------------
#--------------------------------------------------
N:=N0+N1+N2:
Ketqua:=matrix(N,6):
for i from 1 by 1 to N0 do
for k from 1 by 1 to nj do
Ketqua[i,k]:= Ketqua0[i,k]:
end do:
end do:
for i from 1 by 1 to N1 do
for k from 1 by 1 to nj do
Ketqua[(i+N0),k]:= Ketqua1[i,k]:
end do:
end do:
for i from 1 by 1 to N2 do
for k from 1 by 1 to nj do
Ketqua[(i+N0+N1),k]:= Ketqua2[i,k]:
end do:
end do:
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
#---------------------------------------------------
dt:=0.1:
t0:=0:
time_:=vector(N):
dv:=vector(N):
d1:=1.00:
a1:=0.90:
a3:=0.60:
for i from 1 by 1 to N do
q1:= Ketqua[i,1]:
q2:= Ketqua[i,2]:
q3:= Ketqua[i,3]:
xE[i]:=evalf(subs([theta1=q1,theta2=q2,theta3=q3],RE[1])):
yE[i]:=evalf(subs([theta1=q1,theta2=q2,theta3=q3],RE[2])):
zE[i]:=evalf(subs([theta1=q1,theta2=q2,theta3=q3],RE[3])):
Ketqua[i,4]:=xE[i]:
Ketqua[i,5]:=yE[i]:
Ketqua[i,6]:=zE[i]:
time_[i]:=t0:
t0:=t0+dt:
dv[i]:=0:
end do:
Bài tiểu luận: Mô hình hóa Robot và hệ cơ điện tử Đề số 6360_10.
GVHD: PGS.Phan Bùi Khôi TH: Đinh Gia Nghiêm – Lớp:
kqn1:=subvector(Ketqua,1..N,1):
m1:=concat(kqn1,dv):
kqn2:=subvector(Ketqua,1..N,2):
m2:=concat(kqn2,dv):
kqn3:=subvector(Ketqua,1..N,3):
m3:=concat(kqn3,dv):
Myplot(m1,time_,[blue,red]);
Myplot(m2,time_,[blue,red]);
Myplot(m3,time_,[blue,red]);
pointplot3d({seq([xE[k],yE[k],zE[k]],k=1..N)});