Điều khiển thích nghi cho robot NDOF trên cơ sở bộ quan sát.pdf
1. LỜI CAM ĐOAN
Em xin cam đoan bản đồ án t t nghi
ố ệp: “Điều khiển thích nghi Robot
nDOF trên cơ sở ộ quan sát”
b do em t thi
ự ết kế dƣới s ng d n c
ự hƣớ ẫ ủa
PGS.TS. Nguyễn Phạm Thục Anh. Các s u và k
ố liệ ết quả là hoàn toàn đúng với
thực tế.
Để hoàn thành đồ án này em chỉ sử dụng những tài liệu đƣợc ghi trong
danh mục tài liệu tham khảo và không sao chép hay sử dụng bất kỳ tài liệu nào
khác. Nếu phát hiện có sự sao chép em xin chịu hoàn toàn trách nhiệm.
Hà N i, ngày tháng 5 2
ộ 21 năm 016
Ngƣời thực hi n
ệ
Khƣơng Đức Hạnh
2. MỤC LỤC
LỜI CAM ĐOAN.....................................................................................................................1
MỞ ĐẦU...................................................................................................................................1
1. Lý do chọn đề tài: ..........................................................................................................1
2. Mục tiêu nghiên cứu: .....................................................................................................1
3. Đối tƣợng, phạm vi nghiên cứu: ..................................................................................1
4. Phƣơng pháp nghiên cứu: .............................................................................................2
5. Ý nghĩa khoa học và thực tiễn của đề tài ....................................................................2
6. C u trúc lu
ấ ận văn: ..........................................................................................................2
Chƣơng 1 TỔNG QUAN PHƢƠNG PHÁP ĐIỀU KHIỂN ROBOT
: ..............................3
1.1. Tổng quan robot..........................................................................................................3
1.1.1. Lịch sử phát triển robot.....................................................................................3
1.1.2. Cấu tạo robot......................................................................................................4
1.1.3. Cơ cấu cơ khí của robot ....................................................................................5
1.1.4. Các thông số đặc trƣng của hệ thống robot ....................................................8
1.1.5. Hệ thống truyền động robot .............................................................................9
1.1.6. Hệ thống điều khiển chuyển động................................................................ 10
1.1.7. Cảm biến.......................................................................................................... 10
1.1.8. Ứng dụng robot công nghiệp ........................................................................ 10
1.2. Phƣơng pháp điều khiển robot ............................................................................... 12
1.2.1. Phƣơng pháp PD bù trọng trƣờng ................................................................ 15
1.2.2. Thuật toán PID................................................................................................ 16
1.2.3. Phƣơng pháp điều khiển phi tuyến trên cơ sở mô hình ............................. 17
1.2.4. Phƣơng pháp điều khiển Li - Slotine ........................................................... 18
Chƣơng 2 BỘ ĐIỀU KHIỂN LI
: - SLOTINE THÍCH NGHI VÀ BỘ QUAN SÁT
THÍCH NGHI ........................................................................................................................ 22
2.1. Cơ sở lý thuyết hệ phi tuyến................................................................................... 22
2.1.1. Hệ phi tuyến .................................................................................................... 22
2.1.2. Điểm cân bằng và điểm dừng của hệ ........................................................... 24
2.1.3. Tính ổn định của điểm cân bằng................................................................... 24
2.1.4. Tiêu chuẩn Lyapunov .................................................................................... 25
2.2. Phƣơng pháp điều khiển thích nghi Li - Slotine ..................................................30
3. 2.3. Bộ quan sát trạng thái thích nghi ........................................................................... 32
2.3.1. Bộ quan sát trạng thái của Luenberger ........................................................ 32
2.3.2. Bộ quan sát thích nghi cho robot .................................................................. 33
Chƣơng 3: MÔ HÌNH TOÁN ROBOT PLANAR............................................................ 36
3.1. Cấu trúc và tham số robot....................................................................................... 36
3.2. Bài toán động học thuận vị trí ................................................................................ 36
3.2.1. Tham số thanh nối khớp ................................................................................ 37
3.2.2. Phƣơng pháp thiết kế khung tọa độ.............................................................. 37
3.3. Bài toán động học ngƣợc vị trí............................................................................... 39
3.4. Động lực học robot ..................................................................................................40
3.5. Thiết kế quỹ đạo chuyển động ............................................................................... 43
Chƣơng 4 MÔ PHỎNG TRÊN MATLAB/SIMULINK
: ................................................. 45
4.1. Sơ đồ nguyên lý điều khiển .................................................................................... 45
4.2. Sơ đồ các khối mô phỏng trong Matlab/simulink ............................................... 46
4.3.Kết quả mô phỏng cho robot Pelican ..................................................................... 48
4.3.1. Kết quả mô phỏng cho bộ quan sát thích nghi............................................ 49
4.3.2. Kết quả mô phỏng bộ điều khiển thích nghi Li-SIotine ............................ 52
KẾT LUẬN............................................................................................................................ 56
TÀI LIỆU THAM KHẢO....................................................................................................57
PHỤ LỤC............................................................................................................................... 58
4. DANH MỤC BẢNG BI U, HÌNH V
Ể Ẽ
BẢNG
Bảng 3.1: Thông số robot Pelican .......................................................................................36
Bảng 4.1 Tham số ƣớc lƣợng cho
: robot............................................................................48
Bảng 4.2 Bảng tham số bộ điều khiển
: ............................................................................... 48
Bảng 4.3 Tham số bộ quan sát
: ............................................................................................ 49
HÌNH VẼ
Hình 1.1. Bộ phận cấu thành Robot .......................................................................................5
Hình 1.2: Khớp tịnh tiến và khớp quay .................................................................................6
Hình 1.3 Cấu tạo một bàn tay máy.........................................................................................7
Hình 1.4: Dạng tay gắn vào thân ...........................................................................................8
Hình 1.5: D ng h
ạ ệ ọa độ ự
t c c .................................................................................................8
Hình 1.6: Dạng hình trụ .........................................................................................................8
Hình 1.7: D ng SCARA
ạ ..........................................................................................................8
Hình 1.8: Robot sử dụng trong công đoạn cấp liệu và lắp ráp ........................................12
Hình 1.9: Sơ đồ nguyên lý điều khiển ................................................................................ 13
Hình 1.10: Sơ đồ khối các thuật toán điều khiển robot .................................................... 14
Hình 1.11: Sơ đồ khối của phƣơng pháp điều khiển PD bù trọng trƣờng...................... 16
Hình 1.12: Sơ đồ điều khiển PID ........................................................................................ 16
Hình 1.13: Sơ đồ nghiệm khi ∆ = 0 .................................................................................... 17
Hình 1.14 Điều khiển phi tuyến trên cơ sở mô hình......................................................... 18
Hình 1.16. Sơ đồ luật điều khiển Li-Slotine ...................................................................... 20
Hình 2.1: Minh họa khái niệm ổn định và ổn định tiệm cận ...........................................26
Hình 2.2: Tƣ tƣởng phƣơng pháp Lyapunov.....................................................................27
Hình 2.3: Tạo họ đƣờng cong kín chứa gốc tọa độ........................................................... 27
Hình 2.4: Sơ đồ điều khiển Li-slotine thích nghi.............................................................. 32
Hình 3.1: Robot Pelican .......................................................................................................36
Hình 3.2: Khung tọa độ cho robot Planar........................................................................... 38
Hình 3.3: Tọa độ thanh nối robot ........................................................................................ 40
Hình 4.1: Sơ đồ nguyên lý điều khiển ................................................................................ 45
Hình 4.2: Sơ đồ điều khiển chung....................................................................................... 46
Hình 4.3 Khối điều khiển thích nghi Li-Slotine................................................................ 46
Hình 4.4: Khối tính giá trị V, r ............................................................................................ 47
5. Hình 4.5: Khối robot Planar................................................................................................. 47
Hình 4.6: Khối quan sát thích nghi ..................................................................................... 48
Hình 4.7: Khảo sát vị trí khớp 1 .......................................................................................... 49
Hình 4.8: Khảo sát vị trí khớp 1 với tín hiệu đặt hình sin................................................49
Hình 4.9: Khảo sát vị trí khớp 2 .......................................................................................... 50
Hình 4.10: Khảo sát vị trí khớp 2 với tín hiệu đặt hình sin.............................................. 50
Hình 4.11: Khảo sát tốc độ khớp 1. .................................................................................... 51
Hình 4.12: Khảo sát tốc độ khớp 2...................................................................................... 51
Hình 4.13: Sai lệch quan sát tốc độ..................................................................................... 52
Hình 4.14: Khảo sát vị trí khớp 1 khi tín hiệu đặt là hằng số.......................................... 52
Hình 4.15: Khảo sát vị trí khớp 1 khi tín hiệu đặt là hàm hình sin ................................. 53
Hình 4.16: Sai lệch vị trí khớp 1 khi tín hiệu đặt là hằng số............................................53
Hình 4.17: Khảo sát khớp 2 khi tín hiệu đặt là hằng số ................................................... 54
Hình 4.18: Khảo sát khớp 2 khi tín hiệu đặt là hàm hình sin .......................................... 54
Hình 4.19: Sai lệch vị trí khớp 2 khi tín hiệu đặt là hằng số............................................54
6. 1
1
MỞ ĐẦU
1. Lý do chọn đề tài:
Theo quá trình phát triển của xã hội, nhu cầu nâng cao sản xuất và chất
lƣợng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phƣơng tiện tự động
hóa sản xuất. Xu hƣớng tạo ra những dây chuyền và thiết bị tự động có tính linh
hoạt cao đã hình thành và phát triển mạnh mẽ. Vì thế ngày càng tăng nhanh nhu
cầu ứng dụng ngƣời máy để tạo ra các hệ sản xuất tự động linh hoạt.
Cùng với sự phát triển của khoa học công nghệ, các robot công nghiệp
cũng ngày càng phát triển và có ứng dụng trong hầu hết các ngành công nghiệp.
Sự phát triển của đất nƣớc và yêu cầu mở rộng, nâng cao chất lƣọng sản xuất
cũng thúc đẩy sự phát triển của robot công nghiệp và đƣa công nghiệp chế tạo
robot trở thành một ngành hứa hẹn trong tƣơng lai. Để đáp ứng điều đó đòi hỏi
phải trang bị cho Robot một bộ điều khiển thông minh, phức tạp. Chính vì thế
các thuật toán điều khiển robot đã và đang đƣợc đầu tƣ nghiên cứu, ứng dụng và
phát triển mạnh mẽ.
2. Mục tiêu nghiên cứu:
- Nắm bắt đƣợc lý thuyết về các phƣơng pháp điều khiển Robot, lý thuyết
điều khiển thích nghi và xây dựng thuật toán.
- Ứng dụng thuật toán thích nghi và bộ quan sát trạng thái để áp chế tính
bất định mô hình động lực học Robot trong điều khiển bám quỹ đạo chuyển
động.
- Sử dụng đƣợc phần mềm MATLB SIMULINK làm công cụ xây dựng
mô hình mô phỏng kết quả.
3. Đối tƣợng, phạm vi nghiên cứu:
- Robot 2 bậc tự do Planar.
- Bộ điều khiển thích nghi Li Slotine và bộ qu
- an sát thích nghi.
- Kết hợp bộ điều khiển thích nghi Li – lotine và bộ quan sát thích nghi
S
để áp chế tính bất định mô hình động lực học Robot trong điều khiển bám quỹ
đạo chuyển động.
7. 2
2
4. Phƣơng pháp nghiên cứu:
- Nghiên cứu tổng quan về các phƣơng pháp điều khiển Robot, trong đó
nghiên cứu phƣơng pháp điều khiển Li – Slotine.
- Xây dựng thuật toán điều khiển thích nghi Li – Slotine và bộ quan sát
thích nghi.
- Từ kết quả tính toán, sử dụng công cụ mô phỏng để trình bày kết quả
nghiên cứu đạt đƣợc.
5. Ý nghĩa khoa học và thực tiễn của đề tài
tài s
Đề ẽ mang lại một hƣớng đi mới trong việc điều khiển robot công
nghiệp. Phƣơng pháp ứng dụng thuật toán điều khiển thích nghi giúp giải quyết
hiệu quả các vấn đề bất định của đối tƣợng nhƣ vị trí, tốc độ. Qua đó tạo ra một
công cụ điều khiển mạnh trong quá trình t ng hóa s n xu
ự độ ả ất.
6. C u trúc lu
ấ ận văn:
Cấu trúc luận văn gồm 4 chƣơng:
Chƣơng 1: Trình bày t ng quan các v
ổ ấn đề về Robot; về lịch s phát tri
ử ển
Robot công nghiệp; các khái niệm cơ bản của Robot công nghiệp.
Chƣơng 2: Trình bày về bộ điều khiển Li Slotine thích nghi và b quan
– ộ
sát thích nghi.
Chƣơng 3: Trình bày về mô hình toán h c Robot Planar
ọ , bài toán động
học thuận và động học ngƣợ ị
c v trí.
Chƣơng 4: Mô phỏng trên Matlab/Simulink.
i s ng d n t
Dƣớ ự hƣớ ẫ ận tình của PGS.TS Nguyễn Phạm Thục Anh luận
văn của em đã hoàn thiện. Do quá trình công tác và điều kiện đi lại nên luận văn
còn nhiều hạn chế. Em rất mong nhận đƣợc sự đóng góp của thầy cô và các b n.
ạ
Em xin chân thành cảm ơn!
Hà N i, ngày tháng 5 2
ộ 21 năm 016
Ngƣời thực hi n
ệ
Khƣơng Đức Hạnh
8. 3
3
Chƣơng 1
TỔNG QUAN PHƢƠNG PHÁP ĐIỀU KHIỂN ROBOT
1.1. T ng quan robot
ổ
1.1.1.Lịch s phát tri n robot
ử ể
“Robot” đƣợc ra đời từ những mong ƣớ ủa con ngƣờ
c c i là mu n có nh
ố ững
c máy g
ỗ ần giống con ngƣời có thể làm những công việc thay thế con ngƣời.
Năm 1921 trong vở ịch Rosum’s Universal Robot củ
k a Karel Capek thì Rossum
và con trai đã chế tạo ra những chiếc máy gần giống con ngƣời để phục vụ con
ngƣời. Có lẽ đây là những gợi ý đầu tiên cho các nhà sáng chế kĩ thuật về các cơ
c u máy móc b
ấ ắt chƣớ ạt độ
c ho ng của con ngƣời.
Đầu những năm 60, công ty Mỹ AMF quảng cáo một loại máy tự ng v
độ ạn
năng gọi là “ ngƣời máy công nghiệp” và ngày nay đƣợc đặt tên là Robot công
nghiệp. Ngày nay những lo i thi
ạ ết bị có dáng dấp và có 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 cũng
đƣợ ọ
c g i là robot công nghiệp.
Về mặt kĩ thuật thì robot công nghiệp ngày nay có nguồn gố ừ hai lĩnh
c t
vực đó là các cơ cấu điều khiển từ xa và các máy công cụ điều khiển số. Các cơ
cấu điều khiển từ xa đã 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ạ. Còn các máy công cụ điều khiển s ra
ố
đời vào những năm 1949 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. Tiếp theo Mỹ, các nƣớc khác cũng bắt đầu sản xuất
robot công nghiệp: Anh -1967, Thụy Điển và Nhậ – 1968, CHLB Đứ –
t 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 ở trƣờng Đạ
1967 i họ ổ
c t ng hợp Standford c a M
ủ ỹ đã
chế tạo ra mẫu robot hoạt động theo mô hình “mắ – tay”, có khả
t 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.
9. 4
4
Năm 1974 Công ty Mỹ Cincinnati đƣa ra loại robot đƣợc điều khiển bằng
máy tính có thể nâng đƣợc vật có khối lƣợng đến 40 kg. Có thể nói, Robot là sự
t h
ổ ợp khả năng hoạt động linh hoạt của các cơ cấu điều khiển từ xa với mức độ
“tri thức” ngày càng phong phú của hệ thống điều khiển theo chƣơng trình số
cũng nhƣ kĩ thuật chế tạo các b c m bi
ộ ả ến, công nghệ lập trình và các phát triển
trí khôn nhân tạo,…
Trong những gần đây, việc nâng cao tính năng hoạt động c a robot không
ủ
ngừng đƣợc phát triển. Các hoạt động đƣợc trang bị thêm các loại c m bi
ả ến khác
nhau để nhận biết môi trƣờng chung quanh cùng v i nh
ớ ững thành tựu to lớn
trong lĩnh vực Tin học – Điện tử đã tạo ra các thế hệ ớ
robot v i nhiều tính năng
đặc biệt. Số lƣợng robot ngày càng gia tăng, chủng loại robot cũng không ngừng
đƣợc gia tăng, giá thành ngày càng giảm. v
Nhờ ậy, robot công nghiệp đã có vị
trí quan tr ng trong các dây truy
ọ ền sản xuất hiện đại, đặc biệt là các hệ ng s
thố ản
xuất tự ng hóa.
độ
Hiện nay, trên thế giới có khoảng 200 công ty sản xuất IR. Và theo ƣớc
tính chƣa đầy đủ của Liên đoàn Robot quốc tế (IRF), năm 2005 trên thế giới có
khoảng 800.000 robot đang đƣợc sử dụng trong sản xuất công nghiệp. Trong đó
Nhật Bản là nƣớc sử dụng nhiều nhất chiếm khoảng 45,5%, EU khoảng 30,3%,
Bắc Mĩ khoảng 13,5% còn lại là các nƣớc khác.
1.1.2. C u t
ấ ạo robot
Trên hình 1.1 giới thiệu các bộ phận chủ yếu của Robot:
Tay máy là cơ cấu cơ khí gồm các khâu khớp chúng hình thành cánh tay để
tạo các chuyển động cơ bản, cổ tay tạo nên sự khéo léo linh hoạt, bàn tay hoàn
thành thao tác trên đối tƣợng. 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.
10. 5
5
Hình 1.1. B n c u thành Robot
ộ phậ ấ
H ng truy
ệ thố ề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á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 trong quá trình làm việc.
H ng c m bi n tín hi u
ệ thố ả ế ệ thực hiện việc nhận biết và 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à của môi trƣờng,
đối tƣợng mà Robot phục vụ (cảm biến ngoại tín hiệu).
1.1.3. Cơ cấu cơ khí của robot
Cấu trúc cơ khí của robot bao gồm một chuỗi thanh nối đƣợc gắn với nhau
bởi các khớp. Mỗi khớp đƣợc truyền động tạo nên một chuyển động độc lập. Số
bậc tự do của robot phụ thuộc vào số chuyển động độc lập của nó và độ dịch
chuyển linh hoạt khi chuyển động sẽ tăng khi số bậc tự do tăng. Hai loại khớp
cơ bản trong hệ thống robot là khớp tịnh tiến và khớp quay.
11. 6
6
Hình 1.2: p t
Khớ ịnh tiến và kh p quay
ớ
Các khớp đƣợc truyền động nhờ động cơ chấp hành gắn trên trục của nó.
Chuyển động của khớp tạo nên chuyển động tƣơng đối giữa hai thanh nốii gắn
với nó, một gọi là thanh nối đầu vào, một gọi là thanh nối đầu ra. Từ các khớp
cơ bản trên ngƣời ta còn phân loại các khớp theo chuyển động tƣơng đối giữa
các liên kết với khớp thành năm loại khớp chính sau :
Khớp tuyến tính ( kí hiệu là khớp L ) : dạng khớp này tạo ra chuyển động
tƣơng đối giữa hai thanh nối là chuyển động trƣợt tuyến tính, trục của hai thanh
nối song song với nhau.
Khớp trực giao (O) : vẫn là chuyển động trƣọt nhƣng hai trục của thanh nối
đầu vào và đầu ra vuông góc với nhau.
Khớp quay (R ): dạng khớp này tạo ra chuyển động quay xung quanh trục
vuông với góc các trục của thanh nối đầu vào và đầu ra.
Khớp xoắn (T): vẫn là chuyển động quay nhƣng trục quay của khớp
songsong với trục của hai thanh nối đầu vào và đầu ra.
Khớp chữ V : với dạng khớp này, trục của hai thanh nối đầu vào và đầu ra
vuông góc với nhau, trục quay của khớp g song với trục của thanh nối đầu
son
vào.
Mỗi robotthƣờng có một đế gắn cố đinh đƣợc gọi là thanh nối 0. Một robot
n bậc tự do thông thƣờng có n khớp đánh số từ 1 đến n và n+1 thanh nối. Khớp i
12. 7
7
nối giữa thanh nối i+1 và i . Thanh nối n đƣợc gọi là khâu tác động cuối, cấu tạo
của robot đƣợc chia làm hai phần chính : phần cánh tay và phần bàn tay. Phần
cánh tay thƣờng có 3 bậc tự do, phần bàn tay thƣờng có 2 đến 3 bậc tự do. Phần
cánh tay có nhiệm vụ định vị nhằm di chuyển khâu tác động cuối tới vị trí đặt
trong không gian. Phần bàn tay làm nhiệm vụ định hƣớng, dùng để xoay chuyển
hƣớng của khâu tác động cuối phù hợp với công nghệ yêu cầu, ví dụ nhƣ xoay
bàn kẹp theo hƣớng nắm bắt một vật nào đó, hoặc định hƣớng một súng hàn ...
Để xoay chuyển vật, phần bàn tay phải thực hiện đƣợc các động tác quay, lắc,
gật gù với 3 bậc tự do.
Hình 1.3 C u t o m
ấ ạ ột bàn tay máy
Với cấu tạo nhƣ vậy robot có thể đƣa đầu cuối bàn tay của nó đến nhiều
điểm trong không gian theo hƣớng mong muốn, tại đó sẽ thực hiện một công
việc nào đó. Tập hợp tất cả các điểm mà tay máy có thể chạm đến gọi là không
gian làm việc của robot.
Với năm loại khớp đã mô tả ở trên có 125 tổ hợp các khớp có thể sử dụng
để thiết kế phần cánh tay máy với ba bậc ự do. Một số tay máy điển hình trong
t
công nghiệp đƣợc minh họa trên hình (1.4÷ 7
1. ). Ngƣời ta kí hiệu dạng của
robot theo các chữ cái kí hiệu các khớp liên tiếp :
• Dạng hệ tọa độ cực (TRL)
• Dạng hình trụ TLO
• Dạng tay gắn vào thân TRR hoặc VVR
• Dạng tọa độ vuông góc LOO hoặc VVR
13. 8
8
• Dạng SCARA
Hình 1.4: D ng tay g n vào thân Hình 1.5: D ng h
ạ ắ ạ ệ ọa độ ự
t c c
Hình 1.6: D ng hình tr
ạ ụ Hình 1.7: D ng SCARA
ạ
1.1.4. Các thông số đặc trƣng của hệ thống robot
Hệ thống điều khiển robot cũng đƣợc đặc trƣng bởi các khái niệm về độ
phân giải, độ chính xác, độ lặp lại.
Độ ả
phân gi i đặc trƣng bởi khoảng cách nhỏ nhất có thể biểu diễn đƣợc
trên toàn bộ dải chuyển động của một khớp :
14. 9
9
CR=(dải chuyển động)/2n
(1.1)
trong đó n là số bit để biểu diễn một số trong hệ thống điều khiển. Tuy nhiên đây
mới là độ phân giải cho một khớp robot. Đối với robot ngƣời ta đƣa ra khái niệm
về độ phân giải không gian. Khái niệm này kết hợp độ phân giải của hệ thống
điều khiển vớii sai số do hệ thống cơ khí gây ra trên các khớp và các mối liên
kết. Nói chung sai số cơ khí tuân theo phân bố xác suất chuẩn và ngƣời ta xác
định độ phân giải không gian, kí hiệu là SR, nhƣ sau :
SR = CR + 6 (1.2)
Độ chính xác đặc trƣng cho khả năng của robotđiều chỉnh điểm cuốicủa
tay máy đến một điểm bất kỳ trong không gian hoạt động của nó.
Độ chính xác = CR/2 + 3 (1.3)
Độ chính xác liên quan đến độ phân giải không gian đƣợc xác định là :
Độ chính xác = SR/2 (1.4)
Độ ặ ạ
l p l i đặc trƣng cho khả năng của robot đƣa đầu cuối bàn tay của nó
chạm vào một điểm theo chƣơng trình định sẵn. Mỗi lần robot định chạm vào
một điểm đãđƣợc lập trình trƣớc đó, nó sẽ chỉ chạm đƣợc vào gần đó do hệ
thống cơ khí có sai số. Do đó độ lặp lại của robot đƣợc xác định bằng :
Độ lặp lại =(+/-) 3 (1.5)
1.1.5. Hệ ố
th ng truyền động robot
Các khớp có thể đƣợc thực hiện các chuyển động nhờ vào các cơ cấu chấp
hành đƣợc truyền động bởi các hệ truyền động khác nhau nhƣ truyền động điện,
thủy lực, khí nén. Các hệ thống truyền động điện cho khả năng về điều khiển
linh hoạt tốt hơn cả và dễ dàng phối hợp với máy tính trong hệ thống điều khiển.
Các hệ truyền động thủy lực có tốc độ cao hơn và công suất cũng lớn hơn. Các
hệ khí nén chỉ đƣợc dùng trong công suất nhỏ và cho các ứng dụng đơn giản nhƣ
cơ cấu vận chuyển, bàn kẹp.
15. 10
10
1.1.6. Hệ ống điề
th u khi n chuy
ể ển động
Chức năng của các bộ điều khiển trong hệ thống là đảm bảo cho robot
chuyển động theo đúng quỹ đạo mong muốn đƣợc đặt trƣớc. Bộ điều khiển có
thể đƣợc thiết kế từ các vi xử lý, vi điều khiển, bộ điều khiển logic khả trình
PLC hoặc máy tính. Dựa vào hệ thống có hay không sử dụng các tín hiệu phản
hồi thông báo thông tin về trạng thái hiện tại của robot, có thể phân chia các hệ
thống điều khiển thành hệ thống mạch kín và hệ thống mạch hở.
Hệ thống mạch kín gồm hai cảm biến đo vị trí góc và tốc độ góc. Bài toán
điều khiển trong không gian khớp đƣợc đề cập cho hệ thống này. Vị trí khớp và
tốc độ khớp đƣợc so sánh với các tín hiệu đặt tƣơng ứng. Các sai số y sẽ đƣợc
nà
sử đụng để tổng hợp tín hiệu điều khiển theo thuật toán phù hợp.
Hệ thống mạch hở không có cảm biến gắn trên khớp, rõ ràng bộ điều khiển
không biết vị trí của tay máy trong quá trình chuyển động. Tuy nhiên trên mỗi
trục có gắn với một công tắc hành trình, khớp sẽ ngừng chuyển động khi nó
chạm phải công tắc này hoặc bộ truyền động động cơ ngắt tín hiệu điều khiển
sau một khoảng thời gian định trƣớc.
1.1.7. C m bi
ả ến
Cảm biến trong robot có thể chia làm hai loại:
• Cảm biến ngoại tuyến tăng khả năng nhận thức cho robot về môi trƣờng
xung quanh.
• Cảm biến nội tuyến cung cấp các thông tin về đặc tính của bản thân robot.
Cảm biến nội tuyến đƣợc gắn trự tiếp trên trục động cơ hoặc khớp, thƣờng là
c
các encoder, chiết áp đo vị trí, tốc độ khớp.
1.1.8. ng d
Ứ ụng robot công nghi p
ệ
Các loại Robot tham gia vào quy trình sản xuất cũng nhƣ trong đời sống
sinh hoạt của con ngƣời, nhằm nâng cao năng suất lao động của dây chuyền
công nghệ, giảm giá thành sản phẩm, năng cao chất lƣợng cũng nhƣ khả năng
cạnh tranh của sản phẩm tạo ra.
16. 11
11
Robot có thể thay thế con ngƣời làm việc ổn định bằng các thao tác đơn
giản và hợp lý, đồng thời có khả năng thay đổi công việc để thích nghi với sự
thay đổi của qui trình công nghệ.
Sự thay thế hợp lý của robot còn góp phần giảm giá thành sản phẩm, tiết
kiệm nhân công ở những nƣớc mà nguồn nhân công là rất ít hoặc chi phí cao
nhƣ : Nhật Bản, các nƣớc Tây Âu, Hoa Kỳ...
Tất nhiên nguồn năng lƣợng từ robot là rất lớn, chính vì vậy nếu có nhu cầu
tăng năng suất thì cần có sự hỗ trợ của chúng mói thay thế đƣợc sức lao động
của con ngƣời. Chúng có thể làm những công việc đơn giản nhƣng dễ nhầm lẫn,
nhàm chán.
Robot có khả năng nghe đƣợc siêu âm, cảm nhận đƣợc từ trƣờng
Bên cạnh đó, một ƣu điểm nổi bậc của robot là môi trƣờng làm việc. Chúng
có thể thay con ngƣời làm việc ở nhũng môi trƣờng độc hại, ẩm ƣớt, bụi bặm
hay nguy hiểm. Ở những nơi nhƣ các nhà máy hoá chất, các nhà máy phóng xạ,
trong lòng đại dƣơng, hay các hành tinh khác ... thì việc ứng dụng robot để cải
thiện điều kiện làm việc là rất hữu dụng.
• Ứng dụng trong các lĩnh vực sản xuất cơ khí:
Trong lĩnh vực cơ khí, robot đƣợc ứng dụng khá phổ biến nhờ khả năng
hoạt động chính xác và tính linh hoạt cao. Các loại robot hàn là một ứng dụng
quan trọng trong các nhà máy sản xuất ôtô, sản xuất các loại vỏ bọc cơ khí...
Ngoài ra ngƣời ta còn sử dụng robot phục vụ cho các công nghệ đúc, một
môi trƣờng nóng bức, bụi bặm và các thao tác luôn đòii hỏi độ tin cậy. Đặc biệt
trong các hệ thống sản xuất linh hoạt (FMS), Robot đóng vai trò rất quan trọng
trong việc vận chuyển và kết nối các công đoạn sản xuất với nhau.
• Ứng dụng trong các lĩnh vực gia công lắp ráp:
Các thao tác này thƣờng đƣợc tự động hóa bởi các robot đƣợc gia công
chính xác và mức tin cậy cao.
17. 12
12
Hình 1.8: Robot s d
ử ụng trong công đoạ ấ
n c p liệu và l p ráp
ắ
• Ứng dụng trong các hệ thống y học, quân sự, khảo sát địa chất:
Ngày nay, việc sử dụng các tiện ích từ Robot đến các lĩnh vực quân sự, y
tế, ...rất đƣợc quan tâm. Nhờ khả năng hoạt động ổn định và chính xác, robot
đặc biệt là tay máy đƣợc dùng trong kĩ thuật dò tìm, bệ phóng, và trong các ca
phẫu thuật y khoa với độ tin cậy cao.
Ngoài ra, tuỳ thuộc vào các ứng dụng cụ thể khác mà Robot đƣợc thiết kế
để phục vụ cho các mục đích khác nhau, tận dụng đƣợc các ƣu điểm lớn của
chúng đồng thời thể hiện khả năng công nghệ trong quá trình làm việc.
1.2. Phƣơng pháp điều khi n robot
ể
Khi xét bài toán điều khiển tay máy cho một robot nào đó, trƣớc hết chúng
ta sẽ phải mô hình hóa tay máy đó là một cơ cấu đối tƣợng đƣợc điều khiển,
-
trong đó các cảm biến đƣợc đặt tại các khớp để quan sát trạng thái của các khớp.
Chúng ta luôn muốn điều khiển các khớp của robot bám đúng quỹ đạo đƣợc
thiết kế. Do vậy chúng ta phải sử dụng các hệ điều khiển để tính toán các lệnh
phù hợp nhất cho các phần tử này sao cho chúng thục hiện đúng các quy luật
chuyển động mong muốn. Trong nhiều trƣờng hợp nhờ sử dụng tín hiệu phản
hồi, moment đầu ra thực tế sẽ đƣợc kiểm soát để tính toán moment mong muốn.
18. 13
13
Quỹ đạo đặt
M
Hình 1.9: Sơ đồ nguyên lý điều khi n
ể
Từ các giá trị đầu vào vị trí qd, vận tốc d
q , gia tốc q , bộ điều khiển có
nhiệm vụ tính toán vector moment Mdk để điều khiển robot chuyển động theo
quỹ đạo mong muốn. Nhƣ vậy, tín hiệu phản hồi sẽ đƣợc sử dụng để tính toán
sai lệch giữa vị trí E và sai lệch vận tốc E .
E = qd – q
d
E q q
Hệ điều khiển sẽ dựa vào các sai lệch trên để điều chỉnh tín hiệu mô men
sao cho các sai lệch E , E giảm dần về 0 khi thời gian t → ∞. Trên cơ sở đó, có
hai hệ thống điều khiển chuyển động: hệ thống điều khi n trong không gian
ể
kh c.
ớ ệ ống điề ể ệ
p và h th u khi n trong không gian làm vi
• Điều khiển trong không gian khớp
Đại lƣợng điều khiển là vị trí của khớp robot : góc quay đối với khớp quay;
độ dịch chuyển thẳng đối với khớp tịnh tiến. Bộ điều khiển đƣợc thiết kế đảm
bảo vị trí khớp luôn bám theo vị trí đặt, tức là sai lệch vị trí khớp hội tụ về
không với thời gian nhỏ nhất.
Vectơ khớp : q = [q1, q2,q3....qn]T
q= [q 1, q 2,…., q n]T
Giá trị đặt biến khớp :
qd = q
[ 1d, q2d,...qnd]T
Yêu Cầu đặt ra là tìm M để q q
→ d khi t→∞.
Thuật toán
ĐKchuyển động
Thuật toán
ĐKchuyển động
Động lực học
Robot
Động lực học
Robot
19. 14
14
• Điều khiển trong không gian làm việc
Tín hiệu đặt là vị trí bàn tay máy
Qũy đạo chuyển động của tay máy E là
, , , , ,
T
x y z
X x y z
Qũy đạo đặt của tay máy E là
, , , , ,
d d
T
d d d d x yd z
X x y z
Bài toán điều khiển chuyển động trong không gian Decac là tìm tín hiệu điều
khiển momen M sao cho d
X X khi t
Hình 1.10: Sơ đồ khối các thuật toán điều khi n robot
ể
Đề tài giới hạn điều khiển chuyển động trong không gian khớp khi chƣa
biết chính xác các thông số động lực học, một số biến điều khiển không đo
đƣợc. Từ đó tìm ra giải pháp để nghiên cứu, đánh giá, qua đó lựa chọn phƣơng
pháp điều khiển phù hợp.
Thuật toán điều
khiển chuyển
Thuật toán điều
khiển chuyển
ĐK chuyển động trong
không gian khớp
ĐK chuyển động trong
không gian khớp
ĐK chuyển động trong
không gian làm việc
ĐK chuyển động trong
không gian làm việc
Phƣơng
pháp PD
bù trọng
trƣờng
Phƣơng
pháp PD
bù trọng
trƣờng
pháp
PID
pháp
PID
ĐK phi
tuyến
trên cơ
sở mô
hình
ĐK phi
tuyến
trên cơ
sở mô
hình
Thuật
toán -
Li
slotine
thích
nghi
Thuật
toán -
Li
slotine
thích
nghi
ĐK ma
trận
Jacobi
vị
ĐK ma
trận
Jacobi
vị
ĐK ma
trận
Jacobi
đảo
ĐK ma
trận
Jacobi
đảo
20. 15
15
1.2.1.Phƣơng pháp PD bù trọng trƣờng
Yêu cầu đặt ra là tổng hợp bộ điều khiển đảm bảo hệ thống ổn định tuyệt
đối xung quanh điểm cân bằng (E= qd - = 0)
q . Xét tính ổn định của hệ thống
theo Lyapunov.
Phƣơng trình điều khiển:
( )
p d
M K E K E G q (1.6)
Các thông số bộ điều khiển bao gồm
p
K là ma trận đƣờng chéo xác định dƣơng
d
K là ma trận đƣờng chéo xác định dƣơng
E là sai số vị trí khớp robot d
E q q
E là sai số tốc độ khớp robot E = d
q - q
Phƣơng trình động lực học kín :
1
( ) ( ) ( , )
2
P D
H q q H q S q q q K E K q (1.7)
Do ( , ) 0
T
q S q q q
1
( )
2
T T T
p D
d
q H q q E K E q K q
dt
(1.8)
Rõ ràng ( , )
V E E là hàm xác định dƣơng
( , ) 0
V E E
Hàm ( , )
V E E chỉ bằng 0 khi đồng thời E ,q đều bằng 0.
0
( , ) 0
0
d
E q q
V E E
q
0
q khi t
Thay vào phƣơng trình (1.7) ta đƣợc:
0
0
p
d
K E khi t
E q q khi t
Điều đó có nghĩa là sai số vị trí hội tụ về 0 khi t tiến tới vô cùng
21. 16
16
Sơ đồ điều khiển:
Hình 1.11: Sơ đồ khố ủa phƣơng pháp điề
i c u khi n PD bù tr
ể ọng trƣờng
Nhận xét về phƣơng pháp PD bù trọng trƣờng:
Ƣu điểm: Đơn giản về mặt toán học
Nhƣợc điểm: Thành phần G(q) trong luật điều khiển là thành phần trọng
trƣờng phụ thuộc vào khối lƣợng của các thanh nối hay khối lƣợng của khớp
nối, khối lƣợng của vật mà Robot gắp. Mà các thành phần này không xác định
đƣợc một cách chính xác, do đó luật điều khiển PD bù trọng trƣờng không đƣợc
sử dụng trong điều khiển Robot đòi hỏi độ chính xác cao.
1.2.2.Thuật toán PID
Moment điều khiển:
p d I
M K E K q K Edt (1.9)
Kp = diag k
{ p1, kp2 ,.. .kpn}là ma trận đƣờng chéo hệ số khuếch đại.
Kd = diag k
{ d1,kd,..,kdn}là ma trận đƣờng chéo hệ số đạo hàm.
Sơ đồ điều khiển :
Hình 1.12: Sơ đồ ề
đi u khi n PID
ể
22. 17
17
Nhƣợc điểm của phƣơng pháp PID là các thông số KI, Kd, Kp chỉ đƣợc xác
định bằng thực nghiệm nên rất khó đƣa ra đƣợc thông số chính xác.
1.2.3.Phƣơng pháp điều khi n phi tuy
ể ến trên cơ sở mô hình
Moment điều khiển:
M = α.τ+β (1.10)
Trong đó : α =H(q)
β = V( ,
q q ) + ( )
G q
d d p
q K E K E
Kd, Kp là ma trận cấp n, đƣờng chéo dƣơng
E =qd- q là sai số giữa giá trị đặt và giá trị thực tế
Phƣơng trình động lực học kín:
( )( ) 0
d p
H q E K E K E (1.11)
Chọn Kp, Kd sao cho 2
4 0
d p
K K
Nghiệm của phƣơng trình vi phân:
2
1 2
( ) ( )
D
K
t
e t C C t e
C1, C2 phụ thuộc vào điều kiện ban đầu
Ta có sơ đồ nghiệm:
Hình 1.13: Sơ đồ nghiệm khi ∆ = 0
Nhƣ vậy khi ∆ =0 thì sai lệch e(t) →0, bộ điều khiển đạt trạng thái tốt nhất.
23. 18
18
Sơ đồ điều khiển :
Hình 1.14 Điều khi n phi tuy
ể ến trên cơ sở mô hình
Ƣu điểm: Đảm bảo sai số bám quỹ đạo về 0 khi lựa chọn thông số bộ điều khiển
phù hợp.
Nhƣợc điểm:
- Khối lƣợng tính toán lớn
- Phải biết chính xác các thông số động lực học và động học của hệ thống
Robot.
1.2.4.Phƣơng pháp điều khi n Li - Slotine
ể
Phƣơng trình động lực học Robot biểu diễn dƣới dạng
( ) ( , ) ( )
M H q q C q q q G q (1.12)
Moment điều khiển:
d
( ) ( , ) ( )
d
k
H q v C q q v K r G q
M (1.13)
Trong đó
( )
( )
d d d
d d
v q q q q E
r v q q q q q E E
d
v q E
r v q
Kd, là ma trận đƣờng chéo xác định dƣơng
Phƣơng trình động lực học hệ kín nhƣ sau:
( )( ) ( , )( ) d
H q v q C q q v q K r (1.14)
( ) ( , ) d
H q r C q q r K r (1.15)
24. 19
19
Nhân cả hai vế của phƣơng trình (1. ) với
16 T
r ta có:
( ) ( , )
T
d
r H q r C q q r K r (1.16)
1
( , ) ( ) ( , )
2
C q q H q S q q
1
( ) ( ) ( , )
2
T T T
d
r H q r r H q r S q q r K r (1.17)
1
[ ( ) ]
2
T T
d
d
r H q r r K r
dt
(1.18)
Do H, Kdlà ma trận đối xứng dƣơng nên:
1
[ ( ) ]
2
T
d
r H q r
dt
≤ 0
Thật vậy, chọn hàm Lyapunov
1
( ) ( )
2
1
2
T
T
d
V r r H q r
d
V r K r
dt
Rõ ràng
( ) 0
V r ;
0
( ) 0 0
0
d
d
E q q
V r r
E q q
( ) 0
V r ; ( ) 0 0
V r r
Do đó
0
0
0
d
d
r khi t
E q q
khi t
E q q
Vậy hệ thống sẽ ổn định xung quanh điểm cân bằng r = 0.
Sơ đồ điều khiển:
25. 20
20
Hình 1.16. Sơ đồ luật điều khi n Li-Slotine
ể
Các phƣơng pháp điều khiển chuyển động đƣợc trình bày ởtrênyêu cầubiết
đƣợc chính xác mô hình động lực học robot, các tham số của robot và phụ thuộc
vàogiá trị tính toán tham số bộ điều khiển. Tuy nhiên một số robot khó có thể
xác định đƣợc chính xác hoặc các tham số biến đổi trong quá trình làm việc nhƣ:
khối lƣợng tải robot gắp ở tay, moment quán tính tải, ma sát. Vì thế để xác định
đƣợc lƣợng moment truyền động cần thiết lên các khớp di chuyển theo quỹ đạo,
bền vững theo sự thay đổi tham số, có khả năng bù sự thay đổi của tải... thì cần
phải sử dụng bộ điều khiển có khả năng chỉnh định đƣợc các thông số động lực
học tay máy trong quá trình chuyển động.
Điều khiển thích nghi sẽ tạo ra đƣợc bộ điều khiển làm cho hệ thống có
chất lƣợng không đổi, đảm bảo độ chính xác chuyển động khi robot k ông đuợc
h
xác định chính xác hoặc biến đổi. Phƣơng pháp của bộ điều khiển thích nghi là
ƣớc lƣợng các tham số động lực học của robot rồi dựa vào luật thích nghi để
chỉnh định lại các tham số ƣớc lƣợng đảm bảo sai lệch E và E 0 khi
→ t→ ∞.
Ngoài ra, trong những phƣơng pháp thiết kế bộ điều khiển trƣớc ta thƣờng
giả thiết là vector trạng thái là đo đƣợc để phản hồi ngƣợc về bộ điều khiển.
X
Nhƣng làm thế nào để thực hiện đƣợc việc phản hồi đƣợc những tín hiệu đó.
Thông thƣờng, việc xác định giá trị tín hiệu đơn giản nhất là đo trực tiếp nhờ các
thiết bị cảm biến (sensor) nhƣ cảm biến vị trí, tốc độ. Tuy nhiên đối với các
d
d
v e
v e
r e e
d
d
v e
v e
r e e
d
K r
d
K r
ˆ
( , , , )
Y q q v v p̂
( , , , )
Y q q v v p
ˆ T
p y r
ˆ T
p y r
dk
dk ,
,
Robot
Robot
26. 21
21
sensor tốc độ lại có nhƣợc điểm là nhiễu, do đó phải sử dụng bộ lọc thông thấp,
để sử dụng bộ lọc thông thấp yêu cầu phải chọn đúng tần số cắt, điều này là hết
sức khó khăn. Vì vậy, để giải quyết vấn đề trên, ta sử dụng bộ quan sát. Nếu
không đo đƣợc trực tiếp, song đối tƣợng lại là quan sát đƣợc thì ta có thể thiết kế
thêm bộ quan sát trạng thái nhằm xác định giá trị vector x(t) thông qua việc đo
những tín hiệu vào ra u(t), y(t).
Ở chƣơng tiếp theo em xin đƣợc trình bày thuật toán điều khiển thích nghi
Li- Slotine và bộ quan sát thích nghi.
27. 22
22
Chƣơng 2
BỘ ĐIỀU N LI - SLOTINE THÍCH NGHI
KHIỂ
VÀ B QUAN SÁT THÍCH NGHI
Ộ
2.1. Cơ sở lý thuy t h
ế ệ phi tuyến
2.1.1.Hệ phi tuy n
ế
Để định nghĩa đƣợc rõ ràng một đối tƣợng hay hệ thống nhƣ thế nào đƣợc
gọi là phi tuyến trƣớc tiên ta nên định nghĩa lại hệ tuyến tính.
Xét một hệ thống MIMO, viết tắt của nhiều vào/nhiều ra (Multi Inputs -
Multi Outputs) với r tín hiệu vào u1(t), u2(t), ... , ur(t) và s tín hiệu ra y1(t), y2(t),
... , ys(t) . Nếu viết chung r tín hiệu đầu vào thành vectơ :
tâm ở đây là hình toán học tả quan hệ giữa vector tín hiệu vào
mô mô u(t) và tín
hiệu ra y (t), tức là mô tả ánh xạ T : →
u )
(t y (t). Ánh xạ này đƣợc viết lại nhƣ
sau :
y (t) = T{ } (2.1)
u(t)
Nếuánh xạ T thỏa mãn :
T(a1u1(t)) + Ta2u2(t)) = a1T(u1(t)) + a 2T(u2(t)) (2.2)
Trong đó a1, a2Є R thì hệ đó đƣợc gọi là tuyến tính. Tính chất (2.2) đƣợc
gọi là nguyên lý xếp chồng.
Ngƣợc lại, hệ thống không thỏa mãn nguyên lý xếp chồng gọi là hệ phi
tuyến.
u(t)=
28. 23
23
Nhờ có hình toán học (2.1) ta luôn xác định đƣợc vector tín hiệu đầu ra
mô
y(t) của hệ thống nếu nhƣ đã biết trƣớc vector tín hiệu đầu vào u{t) và các trạng
thái tức thời x1(t),x2(t).. .xn(t) của nó.
Mô hình c a h
ủ ệ tĩnh : Một hệ thống đƣợc gọi là tĩnh, nếu tín hiệu ra y (t)
ở thời điểm t= t0 đƣợc xác định trực tiếp từ tín hiệu đầu vào u(t0) tại đúng thời
điểm đó. Nhƣvậy mô hình toán học (2.1) của hệ tĩnh sẽ chỉ là một quan hệ đại số
và ngƣời ta viết nó lại thành hàm y = f (u)
Mô hình c a h ng :
ủ ệ độ Một hệ thống đƣợc gọi là động, nếu để xác định
tín hiệu ra y (t0) ở thời điểm t= t0 ngƣời ta cần phải có các giá trị tín hiệu đầu vào
u(t) ở tất cảcác thời điểm trƣớc đó. Nhƣ vậy, để mô tả một hệ động, mô hình
toán học (2.1) của nó không thể chỉ là một quan hệ đại số mà nó còn có cả các
quan hệ giải thích khác nhƣ vi phân, tích phân. Khác với hệ tĩnh, mô hình hệ
động có sự tham gia của các biến trạng thái, vì trạng thái của hệ là đại lƣợng
mang thông tin về tính động học của hệ.
Bản chất động học của hệ thống nằm trong quan hệ giữa tín hiệu vào
u(t)vàtrạng thái ) của nó. Mô hình toán học (2.1) đƣợc viết dƣới dạng :
x(t
• Mô hình trạng thái tự trị (autonom)
)
;
(
)
;
(
u
x
g
y
u
x
f
dt
dx
• Mô hình trạng thái không tự trị (non-autonom)
)
,
;
(
)
,
,
(
t
u
x
g
y
t
u
x
f
dt
dx
Với hình trạng thái ngƣời ta có thể xác định đƣợc nghiệm
mô x(t), y(t) mô
tả sựthay đổi trạng thái và tín hiệu ra của hệ thống theo thời gian dƣới tác động
của kích thích và điểm trạng thái đầu
y(t) x0 x
= (0) đƣợc giả thiết là đã biết.
29. 24
24
2.1.2. Điểm cân bằng và điểm dừng c a h
ủ ệ
Định nghĩa 2.1: Một điểm trạng thái xe đƣợc gọi là điểm cân bằng nếu nhƣ
khi đang ở điểm trạng thái xe và không có một tác động nào từ bên ngoài thì hệ
sẽ nằm nguyên tại đó.
Căn cứ theo định nghĩa nhƣ vậy thì điểm cân bằng xe của hệ thống phải là
nghiệm của phƣơng trình:
0
)
,
,
(
0
u
t
u
x
f
dt
dx
(2.3)
Định nghĩa 2.2 : Một điểm trạng thái xd đƣợ ọi là điể
c g m d ng c
ừ ủa hệ ố
th ng
nếu nhƣ hệ đang ở điểm trạng thái xd và với tác động u(t) - u d cố định, không
đổi cho trƣớc, thì hệ sẽ nằm nguyên tại đó.
Rõ ràng là điểm dừng theo định nghĩa vừa nêu sẽ là nghiệm của :
( , , ) 0
d
d u u
d x
f x u t
dt
(2.4)
2.1.3.Tính ổn định của điểm cân bằng
Một trong những công việc phải làm đầu tiên khi phân tích hệ thống là xét
tính ổn định của hệ thống. Nếu hệ thống không ổn định thì phải tìm một bộ điều
khiển làm cho nó ổn định. Khi hệ đã ổn định rồi, ta mới tính tới việc mở rộng bộ
điều khiển đã có hoặc tìm thêm các bộ điều khiển tiếp theo để tạo ra cho hệ
thống những chất lƣợng mong muốn khác.
Tính ổn định của hệ thống là một phản ứng động học của hệ với tác động
từ bên ngoài. Nhƣng do có hai nguồn tác động bên ngoài mà ta thƣờng quan tâm
là tín hiệu đầu vào và tín hiệu nhiễu, nên tƣơng ứng cũng có hai định nghĩa về
tính ổn định.
Định nghĩa 2.3 (BIBO) : một hệ thống có (vector) tín hiệu vào u(t) và ra
y (t)đƣợc gọi là ổn định BIBO nếu nhƣ ( )
u t < ∞(hữu hạn) thì ( )
y t < ∞ cũng là
số hữuhạn.
Định nghĩa 2.4 ( Lyapunov): Một hệ thống đƣợc gọi là ổn định tiệm cận
Lyapunov tại điểm cân bằng xe nếu nhƣ có một tác động tức thời (hoặc trong
30. 25
25
mộtkhoảng thời gian đủ ngắn) đánh bật hệ ra khỏi xe thì sau đó hệ có khả năng
tự quay vềđƣợc điểm cân bằng ban đầu.
Theo định nghĩa trên thì ta có thể nhận biết đƣợc hệ có ổn định hay không
tại một điểm cân bằng thông qua dạng họ các đƣờng quỹ đạo trạng thái của nó.
Nếu hệ ổn định tại một điểm cân bằng x enào đó thì mọi đƣờngquỹ đạo trạng
thái x )
(t xuất phát từ một điểm xo thuộc lân cận của x e đềuphải kết thúc tại x e.
Chú ý rằng tính ổn định của hệ phi tuyến chỉ có ý nghĩa khi đi cùng với
điểm cân bằng x e . Có thể hệ sẽ ổn định tại điểm cân bằng này, song lạikhông
ổn định ở điểm cân bằng khác. Điều này cũng khác so với khái niệm ốn định ở
hệ tuyến tính. Vì hệ tuyến tính thƣờng chỉ có một điểm cân bằng là gốc toạ độ
( x e= 0 ) nên khi hệ ổn định tại 0 , ngƣời ta cũng nói thêm luônmột cách ngắn
gọn là hệ ổn định .
2.1.4. Tiêu chu Lyapunov
ẩn
2.1.4.1: Tiêu chu nh Lyapunov
ẩn ổn đị
Xét hệ phi tuyến không bị kích thích ( không có tín hiệu vào) mô tả bởi:
1 1
( ); ( ... ) ; ( ) ( ( )... ( ))
t T
n n
d x
f x x x x f x f x f x
dt
(2.5)
Hệ phi tuyến có thể có nhiều điểm cân bằng nhƣng cũng có thể không có
điểm cân bằng nào. Do đó, khái niệm ổn định của hệ phi tuyến cũng phải gắn
liền với điểm cân bằng x eđƣợc xét. Hệ có thể ổn định tại điểm cân bằng này,
song lại không ổn địnhở những điểm cân bằng khác.
Không mất tính tổng quát, sau đây ta sẽ giả thiết hệ cân bằng tại gốc xe = 0
vàchỉ phân tích tính ổn định của hệ trong lân cận điểm cân bằng đó. Nói rằng
không mất tính tổng quát vì chẳng hạn, để xét tính ổn định của hệ tại một điểm
cân bằng x e ≠ 0nào đó, thì thông qua biến mới:
e
dx dx
x x x
dt dt
31. 26
26
Việc xét tính ổn định của nó tại xe nay sẽ trở thành việc xét tính ổn định
của:
( )
e
d x
f x x
dt
tại điểm gốc tọa độ 0
x
Giả sử hệ không bị kích thích đang ở điểm cân bằng x e =0 thì bị một tác
động tức thời không mong muốn đánh bật ra khỏi 0 và đƣa tới một điểm trạng
thái x0 nào đó thuộc lân cận đủ lớn của gốc tọa độ. Khi đó hệ sẽ đƣợc gọi
là:Ổn định tại 0 nếu nó tự quay về về đƣợc điểm 0. Ổn định tiệm cận tại 0 nếu
nó tự quay về đƣợc một lân cận nào đó của điểm gốc (không cần kích thích)
0 .
Lân cận lúc nàyđƣợc gọi là miền ổn định. Nếu miền ổn định là toàn bộ
không gian trạng thái thì hệ sẽ đƣợc gọi là ổn định tiệm cận toàn cục.
Hiển nhiên rằng hệ ổn định tại cũng sẽ ổn định tại đó. Điều ngƣợc lại không
đúng. Hơn thế nữa, ta còn nhận thấy chất lƣợng ổn định của hệ theo định nghĩa
trên phụ thuộc vào vị trí điểm trạng thái xuất phát x0, tức là phụ thuộc vào lân
cận . Lâncận càng lớn, chất lƣợng ổn định của hệ càng cao. Cũng nhƣ vậy,
khi hệ đã ổn định thì hệ sẽ có chất lƣợng ổn định (tiệm cận) càng lớn nếu tốc độ
tiến về gốc tọa độ của quỹ đạo trạng thái tự do xt của hệ càng nhanh. Hình 2.1
minh họa hai khái niệm ổnđịnh và ổn định tiệm cận theo định nghĩa trên.
Hình 2.1: Minh họa khái niệm ổn định và ổn định tiệm c n
ậ
Cũng từ hình minh họa 2.1 ta thấy nếu gọi xt, nghiệm của phƣơng trình vi
phân (2.5) ứng với điều kiện đầu x(0) = x0 bất kì nào đó thuộc lân cận của gốc
tọa độ thì rõ ràng hệ sẽ là :
• Ổn định tiệm cận tại 0 nếu có 0
)
(
lim t
x
x
• Ổn định tại 0 nếu có )
(t
x khi t>T, trong đó là một hằng số dƣơng đủ
32. 27
27
nhỏ cho trƣớc và T là một sổ hữu hạn cũng cho trƣớc.
Nghiệm của phƣơng trình vi phân (2.5) ứng với điều kiện đầu
x(t) x(0) =
x0 bấtkỳ nào đó đƣợc gọi là quỹ đạo trạng thái tự do của hệ. Nhƣ vậy, theo định
nghĩa trên, để kiểm tra đƣợc tính ổn định của hệ tại 0 ta phải xác định đƣợc
nghiệm của phƣơng trình vi phân (2.5). Do đó cũng là cần thiết nếu ta nói thêm
về khả năng tồn tại nghiệm của phƣơng trình vi phân này.
Định lý 2.1 : Nếu phƣơng trình vi phân (2.5) thỏa mãn
0
0)
(
)
( x
x
L
x
f
x
f với mọi x thuộc lân cận nào đó của 0
x , trong đó L là
số thực dƣơng, thì trong lân cận đó, nó sẽ có nghiệm duy nhất thỏa mãn
x{t)
điều kiện đầu 0
)
0
( x
x .
Định lý cho phép ta kiểm tra đƣợc sự tồn tại nghiệm x(t) của (2.5) nhƣng
không giúp ta tìm đƣợc nghiệm đó. Một câu hỏi đặt ra là làm sao kiểm tra đƣợc
tính ổn định của hệ phi tuyến không bị kích thích (a.b) mà không cần phải xác
định hay đi tìm quỹ đạo của trạng thái tự do của nó. Câu trả lời chính là nội
dung của tiêu chuẩn Lyapunov.
Hình 2.2: Tƣ tƣởng phƣơng pháp Lyapunov
Hình 2.3: T o h
ạ ọ ờ
đƣ ng cong kín ch a g c t
ứ ố ọa độ
33. 28
28
Tƣ tƣởng tiêu chuẩn Lyapunov đƣợc giải thích nhƣ sau: giả sử hệ (2.5) cân
bằng tại gốc tọa độ . Khi đó, ta có thể thông qua dạng quỹ đạo trạng thái tự do
0
)
(t
x của nó mà rút ra đƣợc những kết luận về tính ổn định của hệ , chứ không
0
cần phải tìm đƣợc cụ thể hàm )
(t
x . Chẳng hạn bằng cách nào đó ta có đƣợc các
họ đƣờng cong khép kín quanh gốc tọa độ . Vậy thì để kiểm tra hệ có ổn
v bao 0
định tại 0 hay không ta chỉ cần kiểm tra xem nghiệm x(t) đi từ điểm trạng thái
đầu xo nào đó thuộc lân cận có cắt các đƣờng cong v này theo hƣớng từ ngoài
vào trong hay không. Nếu )
(t
x cắt mọi đƣờng cong theo chiều từ ngoài vào
v
trong thì hiển nhiên )
(t
x tiến về 0 và do đó hệ sẽ ổn định tiệm cận tại 0. Ngƣợc
lại, nếu )
(t
x không cắt bất cứ một đƣờng cong nào theo hƣơng từ trong ra ngoài
thì chắc chắn )
(t
x sẽ tiến về gần 0 và do đó hệ ổn định.
Rõ ràng, để kiểm tra chiều cắt của quỹ đạo trạng thái tự do )
(t
x của hệ với
các đƣờng cong này, ta chỉ cần kiểm tra góc φ, là góc tạo bởi tiếp tuyế
v n
d x
dt
của
)
(t
x vàvector ∆v, đƣợc định nghĩa là vector vuông góc với đƣờng cong v theo
hƣớng từ trong ra ngoài, mà cụ thể khi )
(t
x chƣa về tới gốc 0 sẽ có :
> 90 t ngoài vào trong thì h nh ti m c n t 0
ừ ệ ổn đị ệ ậ ại
≥ 90 cắ ừ ng hoặc tiếp xúc thì hệ ổn định tại
t t ngoài vào tro 0
<0 khi x ≠0 hệ ổn định tiệm cận tại 0 (2.6)
≤ 0 với mọi x thì hệ ổn định tiệm cận tại0
2.1.4.2: Tiêu chu n Lyapunov và hàm Lyapunov
ẩ
Để có đƣợc các đƣờng cong u thích hợp, Lyapunov đã sử dụng tập các
đƣờng đồng mức của hàm xác định dƣơng, trơn V(x ), tức là hàm vô hƣớng, khả
vi thỏa mãn :
V( x ) ≥ 0, x ≠ 0 và V( 0 ) = 0 (2.7)
Hàm xác định dƣơng V(x ) này có phƣơng trình mô tả:
. .cos
T
v v
dx dx
dt dt
. .cos
T
v v
dx dx
dt dt
34. 29
29
x
x
bx
ax
x
V
T
b
0
0
a
2
2
2
1
v i a, b là hai s th
ớ ố ực dƣơng
Và có tính chất là khi ta cắt nó bằng một mặt phẳng V = k song song với
đáy và chiếu lên thiết diện đáy thì ta sẽ đƣợc một đƣờng cong khép kín vk chứa
điểm gốc tọa độ Đƣờng đồng mức ứng với
0. vk với k nhỏ hơn thì nằm bên trong
đƣờng đồng mức vk ứng với k lớn hơn.
Với đặc điểm này của hàm xác định dƣơng V(x)thì vector ∆v của họ các
đƣờng cong sẽ đƣợc xác định nhƣ sau :
v
T
n
T
v
x
V
x
V
x
V
gradV ....,
,
1
vì vector gradient luôn luôn vuông góc với đƣờng cong vk và chỉ chiều tăng theo
giá trị k của V( x ) = k, tức là chỉ chiều từ trong ra ngoài của đƣờng cong vk .
Với hàm xác định dƣơng, trơn theo công thức (2.7) thì điều kiện(2.6) sẽ
đƣợc viết lại thành :
< khi
0 x ≠0 hệ ổn định tiệm cận tại
≤ 0 với mọi x thì hệ ổn định tiệm cận tại 0
Định lý 2.2 (tiêu chuẩn Lyapunov): Xét hệ phi tuyến không bị kích thích
(2.5) cân bằng tại gốc tọa độ. Gọi là một hàm xác định dƣơng, trơn. Ký
V(x)
hiệu:
)
(
)
(
)
(
x
W
x
f
x
x
V
Khi đó :
Hệ sẽ là ổn định tiệm cận tại 0 với miền ổn định , nếu W( x ) là hàm xác
định dƣơng trong , tức là W(x ) > 0, x ≠ 0 và W(0 ) = 0.
Hệ sẽ là ổn định tại 0 nếu W(x )là hàm xác định bán dƣơng trong , tức là
W(x )>0, x
Hàm V( x) khi đó đƣợc gọi là hàm Lyapunov.
)
(
)
(
)
(
x
f
x
x
V
dt
dx
x
x
V
dt
dx
T
v )
(
)
(
)
(
x
f
x
x
V
dt
dx
x
x
V
dt
dx
T
v
35. 30
30
2.2. Phƣơng pháp ề
đi u khi n thích nghi Li - Slotine
ể
Để đảm bảo chất lƣợng bám chính xác quỹ đạo của chuyển động tay máy
robotmà không phụ thuộc tham số bất định của mô hình động lực học. Luật điều
khiển Li- Slotine đã giải quyết vấn đề này bằng việc chỉnh định lại các tham số
bất định bằng luật cập nhật, để so sánh giữa giá trị chỉnh định với giá trị thực rồi
đƣa vào bộ điều khiển để hiệu chỉnh tính toán moment điều khiển cho từng
khớp, đảm bảo hệ thống ổn định và sai lệch vị trí các khớp sẽ tiến về 0. Luật
điều khiển xây dựng dựa trên tiêu chuẩn ổn định Lyapunov.
Phƣơng trình động lực học robot:
( ) ( , ) ( )
M H q q C q q q G q (2.8)
Moment điều khiển:
đk
M ˆ ˆ
ˆ ( ) , ( ) d
H q v C q q v G q K r (2.9)
Luật cập nhật thích nghi tham số động lực học: ˆ
ˆ T
p Y r (2.10)
Trong đó: là ma trận đƣờng chéo, xác định dƣơng.
( )
( )
d d d
d d
v q q q q E
r v q q q q q E E
=> d
v q E
r v q
Đặt ∆p = p̂ - p; ∆C = Ĉ - C
∆H = Ĥ - H; ∆G = Ĝ - G
Kết hợp (2.8) và (2.9) ta có phƣơng trình động lực học kín của Robot nhƣ
sau: ( ) ( , ) ( ) ( ) ( ) d
H q q C q q q G q H H v C C v G G K r
( )[ ] ( , )[v- ] ( ) d
H q v q C q q q Hv Cv G K r (2.11)
( ) ( , ) ( , , , ) d
H q r C q q r Y v v q q p K r
Ta có: r
T
[ ( ) ( , ) ( , , , ) ]
d
H q r C q q r Y v v q q p K r
(2.12)
36. 31
31
( , )
C q q =
1
2
H ( ) + S( ,
q q q )
→ r
T
( )
H q r +
1 1
( ) ( , ) ( )
2 2
T T T
d
r H q r r S q q r r H q r
dt
(2.13)
Lại có: ˆ
ˆ T
p Y r
→
.
1
T T T T T
p p p Y r p Y r
→
. 1
2
T T T
d
r Y p p p p p
dt
(2.14)
Từ (2.13) và (2.14)
1 1
( )
2 2
T T T
d
d d
r H q r p p r K r
dt dt
(2.15)
Do H và Kdlà ma trận đối xứng dƣơng nên
( ) 0
T T
r H q r p p
Chọn hàm Lyapunov: V(r) = ( )
T T
r H q r p p
Nhận thấy:
( ) 0;
( ) 0
( ) 0; ( ) 0 0
d
d
V r
E q q
V r
E q q
V r V r r
=>r = 0
E E khi t → ∞
=>
0
0
E
E
khi t → ∞
37. 32
32
Sơ đồ điều khiển:
Hình 2.4: Sơ đồ ề
đi u khi n Li-slotine thích nghi
ể
Bộ điều khiển Li-Slotine thích nghi đã giải quyết đƣợc vấn đề tham số
động lực học của robot không xác định đƣợc. Nhƣng ở trên, ta đang giả thiết
rằng tín hiệu tốc độ q là đo đƣợc và phản hồi về bộ điều khiển. Thực tế, ta
không thể đo đƣợc tốc độ q hoặc nếu đo đƣợc thì cũng khó xác định chính xác.
Sau đây, em xin đề xuất thay thế việc đo tốc độ bằng phƣơng pháp sử dụng bộ
quan sát thích nghi. Nội dung của phƣơng pháp dựa trên ý tƣởng của bộ quan sát
Luenberger.
2.3. B quan sát tr
ộ ạng thái thích nghi
2.3.1. B quan sát tr ng thái c
ộ ạ ủa Luenberger
Xét hệ thống phi tuyến :
x Ax Bu
y Cx
(2.16)
y, C là các tín hiệu đo đƣợc
Mô hình bộ quan sát Luenberger đề xuất nhƣ sau:
ˆ ˆ ˆ
( )
x Ax Bu L y y (2.17)
Từ (2.31) và(2.32) ta có đƣợc sai lệch quan sát e = x -x̂ , và do đó :
ˆ
( ) ( )
e Ae L Cx Cx A LC e (2.18)
38. 33
33
Nhƣ vậy để e = x - x̂ 0thì A-
→ LC phải là ma trận bền. Ta phải tìm L sao
cho giá trị riêng của A-LC nằm về bên trái mặt phẳng phức.
Theo nhƣ bộ quan sát Luenberger, các tham số của hệ là hằng số không
thay đổi và đã biết chính xác. Nhƣng tham số động học của robot lại luôn thay
đổi trong quá trình làm việc và ta cũng không biết đƣợc chính xác. Vì thế, ta
phát triển bộ quan sát Luenberger thành bộ quan sát thích nghi ứng dụng cho
robot.
2.3.2 B quan sát thích nghi cho robot
. ộ
Với bộ điều khiển Li-Slotine thích nghi ta có phƣơng trình động lực học
kín :
Ĥ v+ Ĉ v + Ĝ + Kd r H(
= q) q + C(q, q ) q + G(q)
Khi q = v, động lực học cho mỗi khớp đƣợc viết nhƣ sau:
0 0
0 0
i i
i
i i
q q
I
q q I
(2.19)
1
[ ( )]
dk
H M Hq C G
Bộ quan sát thích nghi cho robotcó dạng:
ˆ
ˆ 0 0
ˆ
( )
0 0 ˆ
ˆ
i
i
i i i
i
i
q
q I
v L q q
I
q
q
(2.20)
Luật thích nghi theo Li-Slotine : 1
ˆ T
Y r
Mdk=Y( ,
q q̂ , v) ˆ + Kdr = ˆ ˆ
ˆ ˆ
( ) ( , ) d
H q v C q q v G K r (2.21)
Trong đó:
ˆ
ˆ
ˆ ˆ
d
d
r e e
v q e
e q q
Định nghĩa các trạng thái sai lệch nhƣ sau: sai lệch quỹ đạo e =qd - q,sai
lệchquan sát e0=q- q̂ ; ê = ˆ
q q ; thay thế 0
q̂ q e ta đƣợc 0
ê e e . Vector sai
lệch quỹ đạo và quan sát đƣợc viết 0 0
[ , , , ]T
e e e e . Có hai sai lệch tính đến là sai
lệch ƣớc lƣợng và sai lệch quan sát.
39. 34
34
Thay (2.21) vào phƣơng trình động lực học robot, ta đƣợc :
ˆ ˆ ˆ
ˆ ˆ
( ) ( , ) ( ) ( ) ( , )
d d
H q q V q q H q q H q e V q q K r (2.22)
Đặt V (q) = V(q)- ˆ
V (q) (2.23)
V (q, q ) = V( ,
q q)- ˆ ( , )
V q q (2.24)
ˆ ˆ ˆ
ˆ ˆ
( , , ) ( , ) ( , )
V q q q V q q V q q (2.25)
Từ (2.24) và (2.25) ta có :
ˆ
ˆ ˆ ˆ
( , ) ( , ) ( , ) ( , , )
V q q V q q V q q V q q q (2.26)
ˆ ˆ
ˆ ˆ
( , , , , ) ( , ) ( , , ) d
F q q q V q q V q q q K r (2.27)
Thay (2.23) và (2.27) vào (2.22) ta đƣợc :
ˆ ˆ ˆ ˆ
ˆ ˆ
( ) ( ) ( ) ( ) ( , ) ( ) d
H q q H q e H q q H q q V q q V q q K r (2.28)
Thay 0
ê e e và viết ˆ ˆ
( , ) ( , )
V q q V q q - F( ˆ
ˆ
, , , ,
q q q )-Kdr ta đƣợc :
0
ˆ
ˆ ˆ ˆ ˆ ˆ
( ) ( ) ( ) ( ) ( ) ( , , , , )
H q q H q q H q e H q e H q q F q q q (2.29)
Phƣơng trình quỹ o sai l
đạ ệch :
1
0
ˆ
ˆ ˆ
( )[ ( ) ( , , , , )]
e e H q H q q F q q q e (2.30)
1 ˆ
ˆ
ˆ ˆ
( )[ ( ) ( , , , , )]
e e H q H q q F q q q (2.31)
Viết 0
ê e evà nh r
ớ ằng d
q =0, vì thế d
q q e e , có thể viế ạ
t l i
(2.31) thành:
1 1
0
ˆ ˆ
( ) ( ) ( ) ( )
e e e H H e H F (2.32)
1 1 1 1 1 1
0
ˆ ˆ ˆ
( ) ( ) ( )
e I H H e I H H F I H H e (2.33)
Bây giờ ta xét đến sai lệch quan sát. Viết lại phƣơng trình động lực
họcrobot bằng cách thay thế luật điều khiển :
ˆ ˆ
( ) ( , ) ( ) ( , ) d
H q q V q q H q v V q q K r (2.34)
1 ˆ ˆ
( )[ ( ) ( , ) ( , ) ]
d
q H q H q v V q q V q q K r (2.35)
40. 35
35
Phƣơng trình sai lệch quan sát:
0 1
0
ˆ 0 0 0 ˆ
ˆ
( )[ ( ) ( , , , , )
00
ˆ
ˆ
0 0
ˆ
( )
00 ˆ
e q q I q
v H q H q v F q q q
e q q I I
q
q
I
v L q q
I
q
0 0 1
0 0
0 ˆ
ˆ
[ ] ( )[ ( ) ( , , , , )]
e e
A LH H q H q v F q q q
e e I
(2.36)
Trong đó: A=
0
00
I
; H = [I 0]
Thay ˆ
d
v q e và 0
ê e e ta có:
0 0 1 1 1
0
0 0
0 0 0
[ ]
e e
A LH H H e H H e H F
e e I I I
(2.37)
Bây giờ ta tuyến tính hóa (2.33) và (2.37) , viết ( )
e f , tính ( ) /
f
tại = 0vàloại bỏ các thành phần bậc cao, kết quả ta thu đƣợc sai lệch điều
khiển nhƣ sau:
0
e e e (2.38)
Ta có:
Sai số quan sát: 0
0
0
0
e
e
khi t → ∞
Từ (2.38) → 0
e e
Sai số quỹ đạo:
0
0
e
e
khi t → ∞
Kết luận: Bộ điều khiển thích nghi dựa trên quan sát trạng thái đảm bảo sai
số quỹ đạo → 0 và sai số qua
e n sát e0 → 0 khi t → ∞.
41. 36
36
Chƣơng 3
MÔ HÌNH TOÁN ROBOT PLANAR
3.1. C u trúc và tham s robot
ấ ố
Hình 3.1: Robot Pelican
Hình trên là robot Pelican có cấụ tạo: tay máy nằm dọc trên cùng một
mặt phẳng, các khớp nối liên kết với nhau bởi hai khớp quay. Các liên kết
đƣợc điều khiển bởi hai động cơ một chiều.
B ng 3.1: Thông s robot Pelican
ả ố
Thông số Ký hiệu Giá trị Đơn vị
Độ dài khớp 1 l1 0,5 m
Độ dài khớp 2 l2 0,5 m
Khối lƣợng thanh nối 1 m1 6 kg
Khối lƣợng thanh nối 2 m2 4 kg
Khoảng cách từ trục khớp đến
trọng tâm khớp 1
lc1 0,2 m
Khoảng cách từ trục khớp đến
trọng tâm khớp 2
lc2 0,05 m
3.2. Bài toán động học thu n v
ậ ị trí
Một robot có cấu hình đã biết: chiều dài thanh nối, góc quay các khớp quay
hoặc độ dịch chuyển khớp tịnh tiến. Bài toán động học thuận là tính toán vị trí
và hƣớng của tay robot ứng với cấu hình robot đã xác định.
42. 37
37
Vị trí khớp (qi)→ vị trí cổ tay.
3.2.1. Tham số thanh nối khớp
Xét hai khớp i và i+1, thanh i nối giữa hai khớp i và i+1; ai là độ dài pháp
tuyến chung của trục khớp i và i+1; αi là góc giữa hai trục của khớp i và i+1 (góc
giữa trục i+1 và đƣờng thẳng song song trục i nằm trong mặt phẳng chứa trục
i+1 và trục giao với pháp tuyến chung a i).
Tƣơng tự xét trục khớp i-1, pháp tuyến chung của trục khớp i và i-1 là ai-1.
Khoảng cách giữa hai chân của pháp tuyến chung của trục là di, góc θi là góc
giữa hai pháp tuyến chung của trục khớp i.
Đối với khớp quay, θi là góc quay của khớp. Do đó đặt θi là biến của khớp
quay. Với khớp tịnh tiển, di là độ dịch chuyển tịnh tiến của khớp, nên đặt di là
biên của khớp tịnh tiến.
3.2.2. Phƣơng pháp thi t k khung t
ế ế ọa độ
Để nghiên cứu mối quan hệ giữa các thanh nối, khớp và tay robot, ta đặt
các khung tọa độ cho các thanh nối. Theo phƣơng pháp biểu diễn Danevit -
Hartenberg (D-H), khung tọa độ i đƣợc xây dựng th nguyên tắc sau :
eo
• Gốc khung tọa độ thanh i đặt trùng với chân pháp tuyến chung của trục i
và i+1 và nằm trên trục khớp i+1.
• Trục zi đặt theo phƣơng của trục khớp i+1.
• Trục xi đặt theo phƣơng pháp tuyến chung của trục i và i+1 theo hƣớng
đi từ trục i đến i+1.
Với các trƣờng hợp đặc biệt:
Khi hai trục z cắt nhau sẽ không có pháp tuyến chung giữa hai khớp. Khi
đó điểm gốc của khung tọa độ là giao điểm của hai trục và trục x đƣợc đặt theo
đƣờng vuông góc với mặt phẳng chứa hai trục z đó.
Hai trục song song sẽ có nhiều pháp tuyến chung. Khi đó sẽ chọn đƣợc
pháp tuyến chung trùng với pháp tuyến chung của khớp trƣớc. ốc khung tọa độ
G
chọn sao cho di là nhỏ nhất.
43. 38
38
Đối với khớp tịnh tiến khoảng cách di là biến khớp. Hƣớng của trục trùng
với hƣớng di chuyển của khớp. Hƣớng của trục đƣợc xác định, nhƣng vị trí trong
không gian không đƣợc xác định. Khi đó chiều dàiai không có ý nghĩa nên đặtai
= 0. Gốc tọa độ đặt trùng với gốc thanh nối tiếp theo.
Với robot Plecian ta xây dựng khung tọa độ nhƣ sau:
- Khung tọa độ :gốc tọa độ đặt tại khớp quay thứ hai, trục z trùng với trục
1
quay khớp, trục có phƣơng trùng với phƣơng thanh nối thứ nhất.
x
- Khung tọa độ 2: gốc tọa độ đặt tại cổ tay robot,trục z vuông góc mặt
phẳng, trục xcó phƣơng trùng với phƣơng thanh nối thứ hai.
Hình 3.2: Khung t cho robot Planar
ọa độ
Bằng phƣơng pháp hình học ta có thể dễ dàng tìm đƣợc:
12 12 1 1 2 12
12 12 1 1 2 12
2
0
0
0 0 1 0
0 0 0 1
c s l c l c
s c l s l s
T (3.1)
Trong đó: c1 = cosθ1; c2 = cosθ2; c12 = cos(θ1+θ2)
s1 = sinθ1; s2=sinθ2; s12 = sin(θ1+θ2)
Từ đó ta xác định đƣợc vị trí tay robottheo hệ phƣơng trình:
1 1 2 12
1 1 2 12
0
x l c l c
y l s l s
z
(3.2)
44. 39
39
3.3. Bài toán động học ngƣợ ị
c v trí
Để điều khiển robot di chuyển theo các vị trí mong muốn trong không
gian, cần xác định các giá trị biến khớp tƣơng ứng với vị trí và hƣớng của tay
robot mong muốn.
Vị trí cổ tay → vị trí khớp
Từ (3.2) ta có:
2 2 2 2
1 2 1 2 2
2
x y l l l l c
2 2 2 2
1 2
2
1 2
2
2 2
2
1 cos ( )
x y l l
c
l l
s
2 2 2
tan 2( , )
A s c (3.3)
Viết lại (3.2) dƣới dạng sau:
1 2 2 1 2 1 2
2 2 1 1 2 2 1
( )
( )
0
x l l c c l s s
y l s c l l c s
z
(3.4)
1 2 2 2
1 2 2
1 2 2 2
1 2 2
( )
( )
l x l xc ys
c
x y
l y l yc xs
s
x y
(3.5)
1 1 1
tan 2( , )
A s c (3.6)
45. 40
40
3.4. Động lực học robot
C2
C1
Hình 3.3: Tọa độ thanh n i robot
ố
l1,m1: chiều dài, khối lƣợng thanh nổi thứ nhất.
l2,m2: chiều dài, khối lƣợng thanh nối thứ hai.
lc1, lc2: là khoảng cách từ trục khớp tới vị trí trọng tâm của thanh nối.
θ1, θ2: góc quay tƣơng ứng hai khớp quay.
Giả sử khớp thứ nhất sinh ra moment M1; khớp thứ hai sinh ra moment M2.
Moment khớp i:
( )
i
i i
d L L
M
dt
(3.7)
Trong đó :
θi : vị trí khớp i
i : tốc độ khớp i
1 1
n n
i i
i i
L K P
Vị trí và tốc độ trọng tâm thanh nối thứ nhất:
1 1 1 1 1 1 1
1 1 1 1 1 1 1
s
s
c c c c
c c c c
X l c X l
Y l Y l c
2 2 2 2 2
1 1 1 1 1
c c c
V X Y l
46. 41
41
1 1
0 0 ;
T
1 1
z1
0 0
0 0
0 0
xx1
yy
z
I
I I
I
Động năng thanh nối thứ nhất:
2 2 2 2 2
1 1 1 1 1 1 1 1 1 1
1 1 1 1
2 2 2 2
zz c zz
K m v I ml I
(3.8)
Thế năng thanh nối thứ nhất:
1 1
1 1 1 1 1
1 1
0 1 s
s
c
c
c
l c
P m g m gl
l
(3.9)
Vị trí và trọng tâm thanh nối thứ 2:
2 1 1 2 12
2 1 1 2 12
s s
c
c
x l c l c
y l l
(3.10)
Đạo hàm hai vế phƣơng trình (3.10) ta đƣợc
2 1 1 1 2 1 2 12
2 1 1 1 2 1 2 12
s s
c
c
x l l
y l c l c
(3.11)
Bình phƣơng hai vế phƣơng trình (3.11) ta đƣợc
2
2 2 2 2 2 2
2 1 1 1 2 1 2 12 1 2 1 1 2 1 12
2
2 2 2 2 2 2
2 1 1 1 2 1 2 12 1 2 1 1 2 1 12
s s 2 s s
2
c c
c c
x l l l l
y l c l c l l c c
(3.12)
2
2 2 2 2 2 2 2
2 2 2 1 1 2 1 2 1 2 1 1 1 2 2
c
v x y l l l l c
(3.13)
2 1 2
0 0 ;
T
2
2 2
z2
0 0
0 0
0 0
xx
yy
z
I
I I
I
Động năng thanh nối thứ 2:
2 2
2 2 2 2
2 2 1 1 2 1 2 1 2 1 1 2 2 2 1 2
1 1
2 2
c zz
K m l l l l c I (3.14)
Thế năng thanh nối thứ 2:
1 1 1 12
2 2
1 1 1 12
2 1 1 1 12
0 1
s s
s s
c
c
c
l c l c
P m g
l l
m g l l
(3.15)
47. 42
42
Các thành phần trong phƣơng trình Lagange
2
2 2 2 2 2 2 2
1 1 1 1 1 2 1 1 2 1 2 1 2 1 1 2 2
2
2 1 2 1 1 1 2 1 1 1 12
1 1 1
2 2 2
1
s s s
2
c zz c c
zz c c
L K P m l I m l l l l c
I m gl m g l l
(3.16)
2 2 2 2
1 1 1 1 1 2 1 1 2 1 2 1 2 1 2 2
1
2 1 2
1
4 2 2
2
c zz c
zz
L
m l I m l l l l c
I
(3.17)
2 2 2
1 1 1 2 1 2 1 2 2 1 2 1 2 2 1 2 2
1
2
2 2 2 1 2 2 2
[ ] 2 s
[I ( )]
c zz c c c
zz c c
d L
m l I m l l l l c m l l
dt
m l l l c
(3.18)
1 1 1 2 1 1 2 12
1
c c
L
m l c m l c l c g (3.19)
2
2 2 1 2 1 2 2 1 2 1 2
2
os
c c zz
L
m l l l c I (3.20)
2
2 2 1 2 1 2 1 2 1 2 2 2 1 2
2
( os sin )
c c zz
d L
m l l l c I
dt
(3.21)
2 1 2 1 1 2 2 2 2 12
2
s
c c
L
m l l m gl c (3.22)
Moment khớp 1:
2 2 2
1 1 1 z1 2 1 1 1 1 2 z2 1
2 2
2 1 1 1 2 z1 2 2 1 2 1 2 2 2 1 2 1 2
1 1 1 2 1 1 1 1 2
2 os
os 2 sin sin
os os os
c z c c z
c c z c c
c c
M m l I m l l l l c I
m l l l c I m l l m l l
m l c m l c l c g
(3.23)
Moment khớp 2:
2
2 2 2 1 2 2 z2 1
2 2
2 2 z2 2 2 1 2 1 2 2 2 1 2 1 2
2 1 2 1 2 2 2 2 12
s s
s
c c z
c z c c
c c
M m l l l c I
m l I m l l m l l
m l l m gl c
(3.24)
Phƣơng trình động lực học viết dƣới dạng tổng quát:
48. 43
43
2 2 2 2
1 1 z1 2 1 2 1 2 2 z2 2 2 1 2 2 z2
1 1
2 2
2 2
2 2 1 2 2 z2 2 2 z2
2 1 2 2 2 1 2 2 2 2
1 2 2 1
2 1 2 2
1 1 1 2 1
2
0
2 s s
s
0 0
c z c c z c c z
c c z c z
c c
c
c
m l I m l l l l c I m l l l c I
M
M m l l l c I m l I
m l l m l l
m l l
m l c m l c1 2 12
2 2 12
c
c
l c g
m gl c
(3.25)
Chuyển sang dƣới dạng ma trận ta thu đƣợc phƣơng trình động lực học robot
Planar nhƣ sau:
( ) ( , ) ( )
M H q q C q q q G q (3.26)
Trong đó:
Trong đó
1
2
;
M
M
M
1
2
Q
2 2 2 2
1 1 z1 2 1 2 1 2 2 z2 2 2 1 2 2 z2
2 2
2 2 1 2 2 z2 2 2 z2
2
( )
c z c c z c c z
c c z c z
m l I m l l l l c I m l l l c I
H q
m l l l c I m l I
2 1 2 2 2 2 1 2 2 2
2 1 2 1 2
2 s s
,
s 0
c c
c
m l l m l l
C q q
m l l
1 1 1 2 1 1 2 12
2 2 12
( )
c c
c
m l c m l c l c g
G q
m gl c
3.5. T t k
hiế ế quỹ đạo chuyển động
Bài toán: robotcần chuyển động từ điểm A đến điểm B trong thời gian tc
giây; với các thông số :
0
(0) ; ( ) ;
c c
t
0 0
(0) ; ( )
c c
t
Chọn quỹ đạo thiết kế là đa thức bậc 3 có dạng nhƣ sau :
3 2
( ) ; 1,2
i i i i i
t a t bt c t d i (3.27)
Ta có hệ phƣơng trình sau :
49. 44
44
0
0
3 2
2
(0)
(0)
( )
( )
i i
i i
i c i c i c i c i c
i c i c i c i c
d
c
t a t bt c t d
t a t b t c
(3.28)
Với giả thiết vận tốc đầu và cuốiquỹ đạo bằng không, ta có:
0
2
0
3
0
3( )
2( )
0
c
i
c
c
i
c
i
i
a
t
b
t
c
d
(3.29)
50. 45
45
Chƣơng 4
MÔ PHỎNG TRÊN MATLAB/SIMULINK
4.1. Sơ đồ nguyên lý điều khi n
ể
Hình 4.1: Sơ đồ nguyên lý điều khi n
ể
Cảm biến đo vị trị khớp robot đƣa về bộ quan sát, bộ quan sát sử dụng luật
thích nghi để tính toán ƣớc lƣợng các tham số động lực học robot, gửi giá trị
quan sát tốc độ về bộ điều khiển. Bộ điều khiển nhận tín hiệu phản hồi vị trí
robot thông qua cảm biến, tín hiệu phản hồi tốc độ thông qua bộ quan sát rồi
tính toán để đƣa ra moment điều khiển tƣơng ứng tới robot và bộ quan sát.
51. 46
46
4.2. Sơ đồ ố
các kh i mô phỏng trong Matlab/simulink
Hình 4.2: Sơ đồ ề
đi u khi n chung
ể
Hình 4.3 Khối điều khi n thích nghi Li-Slotine
ể
Quỹ
đạo mặt
Quỹ
đạo mặt
52. 47
47
Hình 4.4: i tính giá tr
Khố ị V, r
Hình 4.5: i robot Planar
Khố
53. 48
48
Hình 4.6: Kh i quan sát thích nghi
ố
4.3.Kết qu mô ph
ả ỏng cho robot Pelican
RobotPelican có phƣơng trình động lực học cho ở (3. ) với các thông số
25
cho ở bảng 3.1 .
Các thông số mô phỏng cho trong bảng sau :
B ng 4.1: Tham s
ả ố ƣớc lƣợng cho robot
Tham số Ký hiệu Giá tri
Khối lƣợng ƣớc lƣợng ban đầu
thanh nối 1 1
ˆ (0)
m 6kg
Khối lƣợng ƣớc lƣợng ban đầu
thanh nối 2 2
ˆ (0)
m 4kg
Độ dài khớp 1 l1 0.5m
Độ dài khớ 2 l 0 5
B ng 4.2: B ng tham s b
ả ả ố ộ ề
đi u khi n
ể
Tham số Giá trị
KD 500
100
A 20
54. 49
49
4.3.1. Kết qu mô ph
ả ỏng cho b quan sát thích nghi
ộ
Mục đíchcủa việc mô phỏng b quan sát là kh
ộ ảo sát tốc độ và vị trí thực
tể của robotso sánh v i t
ớ ốc độ và vị trí ƣớc lƣợng từ b quan sát. T
ộ ừ đó đƣa ra
kết luận về b quan sát.
ộ
B ng 4.3: Tham s b
ả ố ộ quan sát
Tham số Giá trị
L1 1200 0
0 1500
L2 1200 0
0 1500
Hình 4.7: o sát v
Khả ị trí kh p 1
ớ
Hình 4.8: Kh o sát v
ả ị trí khớp 1 với tín hiệu đặt là hàm hình sin
55. 50
50
Hình 4.9: o sát v
Khả ị trí kh p 2
ớ
Hình 4.10: Kh o sát v
ả ị trí khớ ớ
p 2 v i tín hiệu đặt là hàm hình sin
Nhận xét : Từ đồ thị ta thấy vị trí khớp thực tế và quan sát là giống nhau,
chứng tỏ bộ quan sát hoàn toàn chính xác, đảm bảo sai lệch e tiến tới 0.
56. 51
51
Hình : Kh
4.11 ảo sát tốc độ khớp 1.
Hình 12: Kh
4. ả ốc độ
o sát t khớp 2
57. 52
52
Hình 4.13: Sai l ch quan sát t
ệ ốc độ
Tốc độ do bộ quan sát tính toán có quỹ đạo bám theo tốc độ thực tế, thời
gian quá độ tƣơng đối ngắn, sai lệch quan sát rất nhỏ. Ta hoàn toàn khẳng định
tính chính xác của bộ quan sát thích nghi. Nhƣ vậy, ta có thể sử dụng tín hiệu
tốc độ từ bộ quan sát làm tín hiệu phản hồi cho bộ điều khiển.
4.3.2. Kết qu mô ph
ả ỏng bộ ề
đi u khi n thích nghi Li-SIotine
ể
Bộ điều khiển thay thế tín hiệu q̂ q sau đó dựa vào luật thích nghi tính
toán lại tham số ƣớc lƣợng động lực học robot rồi đƣa ra moment điều khiển
cho robot.
Hình 4.14: o sát v
Khả ị trí khớp 1 khi tín hiệu đặ ằ ố
t là h ng s
58. 53
53
Hình 4.15: Kh o sát v
ả ị trí khớp 1 khi tín hiệu đặt là hàm hình sin
Hình 4.16: Sai l ch v
ệ ị trí kh p 1 khi tín hi
ớ ệu đặt là h ng s
ằ ố
Nhận xét: Qua kết quả mô phỏng ta thấy góc quay khớp 1 hoàn toàn đúng
so với tín hiệu đặt, thời gian xác lập , sai lệch nhỏ, ổn định.
nhỏ
59. 54
54
Hình 4.17: Khả ớ
o sát kh p 2 khi tín hiệu đặ ằ ố
t là h ng s
Hình 4.18: o sát
Khả khớp 2 khi tín hiệu đặt là hàm hình sin
Nhận xét : Từ đồ thị ta thấy quỹ đạo chuyển động của robot bám đúng theo
quỹ t. M
đạo đặ ặc dù không biết rõ đƣợc tham s ng h
ố độ ọc robot nhƣng bộ điều
khiển đảm bảo điều khiển chính xác và ổn định.
Hình 4.19: Sai l ch v
ệ ị trí kh p 2 khi tín hi
ớ ệu đặt là h ng s
ằ ố
Nhận xét: Qua kết quả mô phỏng ta thấy góc quay khớp 2 hoàn toàn đúng
so với tín hiệu đặt, thời gian xác lập chƣa , sai lệch nhỏ, ổn định
nhỏ .
61. 56
56
KẾT LUẬN
Sau thời gian nghiên cứu đề tài “Điều khiển thích nghi robot nDOF trên cơ
sở bộ quan sát” em đã tổng hợp đƣợc khá nhiều lƣợng kiến thức đã học trên
giảng đƣờng cũng nhƣ trong quá trình công tác, em đã thu đƣợc nhƣng kết quả
cụ thể sau:
- Xây dựng đƣợc bộ điều khiển thích nghi cho robot Planar.
- Xây dựng đƣợc bộ quan sát thích nghi, tính toán điều khiển giá trị đặt cho
robot.
- Tiến hành mô phỏng kết quả trên phần mềm Matlab/Simulink.
Em xin chân thành cảm ơn sự chỉ bảo tận tâm, nhiệt tình của
PGS.TS Nguyễn Phạm Thục Anh đã giúp em hoàn thành bản luận văn này. Do
quá trình công tác và điều kiện đi lại nên bản luận văn còn nhiều hạn chế. Em rất
mong nhận đƣợc sự đóng góp của thầy cô, các anh chị và các bạn.
Em xin chân thành cảm ơn!
Hà Nội, ngày 21 tháng 5 năm 2016
Ngƣời thực hiện
Khƣơng Đức Hạnh
62. 57
57
TÀI LIỆU THAM KHẢO
[1] Trần Trọng Minh, Nguyễn Phạm Thục Anh, 2006, Hệ thống sản xuất tự
động hóa tích hợp máytính, NXB Khoa học và Kỹ thuật.
[2] Nguyễn Doãn Phƣớc, Phan Xuân Minh, Hán Thành Trung, 2006, Lý thuyết
điều khiển phituyến, NXB Khoa học và Kỹ thuật.
[3] Nguyễn Doãn Phƣớc, 2009, Lý thuyết điều khiển nâng cao, NXB Khoa học
và Kỹ thuật.
[4] Nguyễn Mạnh Tiến, 2007, Điều khiển robot công nghiệp, NXB Khoa học và
Kỹ thuật.
[5] Nguyễn Phạm Thục Anh, Bài giảng IR-liada.
[6] Jean-Jacques E. Slotine, W.Li, 1991, Applied NonlinearControl, Prentice
Hall International, Inc. Englewood Cliffs, New Jersey.
[7] Rolf Johanson, Anders Robertsson, 2002, Departmentof Automatic Control
Lund Ínsitute of Technology.
[8] H.M.Schwartz and J.M.Daly, 2008, InternationalJournal of Robotics and
Automation, Vol.23, No.3.
[9]M.Erlic and W.S.Lu, 1995, IEEE Transaction On Robotics and Automation,
Vol.11, No.2.
63. 58
58
PHỤ LỤC
P1. Chƣơng trình mô phỏng động lực học robot
Hàm tính G
function y = g(in)
%q(1) goc theta1
%q(2) goc theta2
%dq(1) toc do quay khop 1
%dq(2) toc do quay khop 2
%cac thong so uoc luong robot
m1 = 5; m2 = 3;
l1 = 0.5; l2 = 0.5;rgl1 = 1/2*l1; rg2 = 1/2*l2;
J1 = 1/l2*m1*l1^2;
J2 = 1/l2*m2*l2^2;
q(1) = in(1);
q(2) = in(2);
%mo men trong luc
y(1,1)= 9.8*m1*rg1*cos(q(1))+
9.8*m2*(l1*cos(q(1))+rg2*cos(q(1)+q(2));
y(2,1)= 9.8*m2*rg2*cos(q(1)+q(2));
end
Hàm tính H
function y = h(in)
%q(1) goc theta1
%q(2) goc theta2
%dq(1) toc do quay khop 1
%dq(2) toc do quay khop 2
%cac thong so uoc luong robot
m1 = 5; m2 = 3;
l1 = 0.5; l2 = 0.5;rgl1 = 1/2*l1; rg2 = 1/2*l2;
J1 = 1/l2*m1*l1^2;
J2 = 1/l2*m2*l2^2;
q(1) = in(1);
q(2) = in(2);
M01 = in(3);
M02 = in(4);
% ma tran quan tinh
h(1,1) = m1*rg1^2 + J1 + m2*(l1^2 + rg2^2 + 2*l1*rg2*cos(q(2))
+ J2;
h(1,2) = m2*(rg2^2 + l1*rg2*cos(q(2)) + J2;
h(2,1) = m2*(rg2^2 + l1*rg2*cos(q(2)) + J2;
h(2,2) = m2*rg2^2 + J2;
Hinv=inv(h);
%gia toc khop
y=Hinv*[M01;M01];
end
Hàm tính V
function y = v(in)
%q(1) goc theta1
%q(2) goc theta2
%dq(1) toc do quay khop 1
%dq(2) toc do quay khop 2
%cac thong so uoc luong robot
64. 59
59
m1 = 5; m2 = 3;
l1 = 0.5; l2 = 0.5;rgl1 = 1/2*l1; rg2 = 1/2*l2;
J1 = 1/l2*m1*l1^2;
J2 = 1/l2*m2*l2^2;
q(1) = in(1);
q(2) = in(2);
dq(1) = in(3);
dq(2) = in(4);
% chi tiet
h3 = -m2*l1*rg2*sin(q(2));
y(1,1) = h3*dq(2)^2 + 2*h3*dq(1)*dq(2);
y(2,1) = -h3*dq(1)^2;
end
P2. Chƣơng trình mô phỏng trong bộ điều khiển và quan sát
Hàm tính Ĥ
function y=H1(in)
q1=in(1);q2=in(2);
p=[in(3);in(4);in(5);in(6);in(7);in(8);in(9);in(10);in(11)];
M01=in(12);
M02=in(13);
H(1,1)=p(1)+p(2)+p(3)+2*p(4)*cos(q2)+p(5)+p(6);
H(1,2)=p(3)+p(4)*cos(q2)+p(6);
H(2,1)=p(3)+p(4)*cos(q2)+p(6);
H(2,2)=p(3)+p(6);
y=H[M01;M02];
end
Hàm tính ˆ
V
function y=V1(in)
q1 = in(1);q2=in(2);dq1=in(12);dq2=in(13);
p=[in(3);in(4);in(5);in(6);in(7);in(8);in(9);in(10);in(11)];
C(1,1)=-p(4)*sin(q2)*dp2;
C(1,2)=-p(4)*sin(q2)*(dp1+dp2);
C(2,1)=p(4)*sin(q2)*dp1;C(2,2)=0;
y=C*[dq1;dq2];
end
Hàm tính Ĝ
function y=G1(in)
q1=in(1);q2=in(2);
p=[in(3);in(4);in(5);in(6);in(7);in(8);in(9);in(10);in(11)];
G(1,1)=[p(7)+p(8)]*cos(q1)+p(9)*cos(q1+q2);
G(2,1)=p(9)*cos(q1+q2);
y=G;
end
Hàm tính M
function y=M(in)
r(1,1)=in(1);r(2,1)=in(2);
v1=in(3);v2=in(4);dv1=in(5);dv2=in(6);q1=in(7);q2=in(8);dq1=in(
9);dq2=in(10);
p=[in(11);in(12);in(13);in(14);in(15);in(16);in(17);in(18);in(1
9)];
