điều khiển lqr hệ con lắc ngược quay kép

Dāa vào nÃn móng là viác cân há con lÅc ng°āc, trong đã án tát nghiáp này, sinh viên s¿ phát triÅn há thành há con lÅc ng°āc quay kép và điÃu khiÅn cân bằng dùng bá điÃu khiÅn LQR 2.. Em

THÀNH PHỐ HỒ CHÍ MINH

BỘ GIÁO DỤC VÀ ĐÀO TẠO

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT

ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HOÁ

ĐIỀU KHIỂN LQR HỆ CON LẮC NGƯỢC QUAY KÉP

GVHD:ThS LÊ THỊ HỒNG LAM SVTH: T RẦN NHẬT CƯỜNG

S K L 0 1 3 2 7 7

Tp Hồ Chí Minh, tháng 6 năm 2024

đồ án

B Þ GIÁO DĀC & ĐÀO T¾O

TR¯âNG Đ¾I HâC S¯ PH¾M KỸ THUÂT TP Hè CHÍ MINH

KHOA ĐIÞN – ĐIÞN TĈ

- ⸙∆⸙ -

Đè ÁN TæT NGHIÞP

GVHD: ThS Lê Th á Héng Lam SVTH: Tr ần NhÃt C°ãng MSSV: 20151446

Tp H é Chí Minh tháng 6 nm 2024 ĐÀ TÀI: ĐIÀU KHIÂN LQR HÞ CON LÄC NG¯þC QUAY KÉP

đồ án

B Þ GIÁO DĀC & ĐÀO T¾O

TR¯âNG Đ¾I HâC S¯ PH¾M KỸ THUÂT TP Hè CHÍ MINH

KHOA ĐIÞN – ĐIÞN TĈ

Tp H é Chí Minh tháng 6 nm 2024

đồ án

TR¯âNG ĐH SPKT TP Hè CHÍ MINH

KHOA ĐIÞN-ĐIÞN TĈ BÞ MÔN TĀ ĐÞNG ĐIÀU KHIÂN

CÞNG HÒA XÃ HÞI CHĂ NGHĨA VIÞT NAM ĐÞC LÂP - TĀ DO - H¾NH PHÚC

I TÊN ĐÂ TÀI: ĐIÂU KHIÄN LQR Hà CON LÄC NG¯ĀC QUAY KÉP

II NHIàM VĂ 1 Các sá liáu ban đầu:

Há con lÅc ng°āc đã là mát há tháng siêu kinh điÅn đái vãi nhÿng nhà nghiên cÿu và điÃu khiÅn tā đáng Hã đã áp dăng thành công viác cân bằng há bçi các bá điÃu khiÅn khác nhau nh° LQR, PID hay Fuzzy Dāa vào nÃn móng là viác cân há con lÅc ng°āc, trong đã án tát nghiáp này, sinh viên s¿ phát triÅn há thành há con lÅc ng°āc quay kép và điÃu khiÅn cân bằng dùng bá điÃu khiÅn LQR

2 Nái dung thāc hián: Đà tài nghiên cÿu tÃp trung vào vấn đà điÃu khiÅn cân bằng cho há <Con lÅc ng°āc quay kép= hay còn gãi là <Double Rotary Inverted Pendulum= sử dăng bá điÃu khiÅn LQR.Công viác chính cąa đà tài là nghiên cÿu, tìm ra há ph°¢ng trình phi tuyÁn biÅu dißn cho há con lÅc ng°āc quay kép, sau đó tuyÁn tính hãa t¿i điÅm làm viác, tā đó áp dăng bá điÃu khiÅn LQR vào mô phßng, lấy c¢ sç đó đÅ xây dāng mô hình thāc tÁ và khÁo sát há t¿i vá trí cân bằng

III NGÀY GIAO NHIàM VĂ: 25/2/2024 IV NGÀY HOÀN THÀNH NHIàM VĂ: 14/6/2024 V Hâ VÀ TÊN CÁN Bà H¯âNG DÀN: ThS Lê Thá Hãng Lam CÁN Bà H¯âNG DÀN Bà MÔN TĀ ĐàNG ĐIÂU KHIÄN

đồ án

TR¯âNG ĐH SPKT TP Hè CHÍ MINH

KHOA ĐIÞN-ĐIÞN TĈ BÞ MÔN TĀ ĐÞNG ĐIÀU KHIÂN

CÞNG HÒA XÃ HÞI CHĂ NGHĨA VIÞT NAM ĐÞC LÂP - TĀ DO - H¾NH PHÚC

GV H¯âNG DÀN (Ký và ghi rõ họ và tên)

đồ án

TR¯âNG ĐH SPKT TP Hè CHÍ MINH

KHOA ĐIÞN-ĐIÞN TĈ BÞ MÔN TĀ ĐÞNG ĐIÀU KHIÂN

CÞNG HÒA XÃ HÞI CHĂ NGHĨA VIÞT NAM ĐÞC LÂP - TĀ DO - H¾NH PHÚC

o0o

NHÂN XÉT CĂA GIÁO VIÊN H¯àNG DÀN Hã và tên sinh viên: Trần NhÃt C°ång MSSV: 20151446 Ngành: Công nghá kỹ thuÃt ĐiÃu khiÅn và Tā đáng hóa Láp: 201514BTên đÁ tài: Điều khiển LQR hệ con lắc ngược quay kép Giáo viên h°áng dÁn: ThS Lê Thá Hãng Lam 1 Nßi dung đÁ tài và khçi l°ÿng công vißc thāc hißn:

3 Khuy¿t điÃm:

đồ án

Bà GIÁO DĂC VÀ ĐÀO T¾O CàNG HOÀ XÃ HàI CHĄ NGH)A VIàT NAM

PHI¾U NHÂN XÉT PHÀN BIÞN

Hã và tên sinh viên : Trần NhÃt C°ång Tên đà tài :ĐiÃu khiÅn LQR há con lÅc ng°āc quay kép GiÁng viên H°ãng dÁn : Th.S Lê Thá Hãng Lam

GiÁng viên PhÁn bián : TS Nguyßn Minh Tâm

Ý KI¾N NHÂN XÉT

1 NhÃn xét chung vÁ nßi dung đÁ tài

2 Ý ki¿n k¿t luÃn (ghi rõ nái dung cần bå sung, hiáu chßnh)

Đà nghá : Đ°āc bÁo vá:  Bå sung đÅ đ°āc bÁo vá:  Không đ°āc bÁo vá: 

3 Câu håi phÁn bißn (Giảng viên không cho SV biết trước)

4 ĐiÃm đánh giá đÁ tài: /10 (Bằng chữ _ )

TP HCM, ngày tháng nm 2022

Ng°åi nhÃn xét

đồ án

TR¯âNG ĐH SPKT TP Hè CHÍ MINH

KHOA ĐIÞN-ĐIÞN TĈ BÞ MÔN TĀ ĐÞNG ĐIÀU KHIÂN

CÞNG HÒA XÃ HÞI CHĂ NGHĨA VIÞT NAM ĐÞC LÂP - TĀ DO - H¾NH PHÚC

o0o

Tp HCM, ngày 14 tháng 6 năm 2024

LâI CAM ĐOAN

Tôi xin cam đoan rằng đây là công trình nghiên cÿu cąa tôi, có sā hß trā tā Giáo viên h°ãng dÁn là ThS Lê Thá Hãng Lam Các nái dung nghiên cÿu và kÁt quÁ trong đà tài này là trung thāc và ch°a tāng đ°āc ai công bá trong bất cÿ công trình nghiên cÿu nào tr°ãc đây Nhÿng sá liáu trong các bÁng biÅu phăc vă cho viác phân tích, nhÃn xét, đánh giá đ°āc chính tác giÁ thu thÃp tā các nguãn khác nhau có ghi trong phần tài liáu tham khÁo

NÁu phát hián có bất kỳ sā gian lÃn nào tôi xin hoàn toàn cháu trách nhiám tr°ãc Hái đãng cũng nh° kÁt quÁ luÃn vn cąa mình

Ng°åi thāc hián đà tài

Trần NhÃt C°ãng

đồ án

Bà GIÁO DĂC VÀ ĐÀO T¾O

TR¯âNG Đ¾I HâC S¯ PH¾M KỸ THUÂT

KHÓA LUÂN, LUÂN VN, LUÂN ÁN)

I Thông tin chung

1 Tên sÁn ph¿m hãc thuÃt: ĐiÃu khiÅn LQR há con lÅc ng°āc quay kép 2 Lo¿i hình sÁn ph¿m hãc thuÃt: Khóa luÃn tát nghiáp

2 Mã sá sÁn ph¿m hãc thuÃt (nÁu có): 3 Thông tin tác giÁ (ghi tất cÁ tác giÁ cąa sÁn ph¿m)

II K¿t quÁ kiÃm tra đ¿o vn Ngày nßp sÁn ph¿m Ngày kiÃm tra đ¿o vn % trùng lặp toàn nßi dung % trùng lặp cao nhất tć 1 nguén

cháu hoàn toàn trách nhiám có liên quan đÁn sÁn ph¿m hãc thuÃt nói trên

Xác nhÃn căa đ¿i dißn nhóm tác giÁ Xác nhÃn căa giÁng viên h°áng dÁn

(ký ghi rõ hã và tên) (ký ghi rõ hã và tên)

M Áu 1

(Kèm theo QĐ sá 1047/QĐ-ĐHSPKT ngày 14 tháng 3 nm 2022) ngày 14 tháng 3 năm 2022 ngày 14 tháng 3 năm 2022

đồ án

L âI CÀM ¡N

ĐÅ hoàn thành đã án tát nghiáp này em xin dành mát låi cÁm ¢n chân thành đÁn cô Lê Thá Hãng Lam– giÁng viên khoa Đián – Đián tử, tr°ång đ¿i hãc S° Ph¿m Kỹ ThuÃt Thành Phá Hã Chí Minh, trong quá trình em thāc hián đã án đã góp ý, h°ãng dÁn, đ°a ra đánh h°ãng đúng và chß ra nhÿng lßi sai mát cách tÃn tình đÅ em có thÅ hoàn thành thāc hián đã án tát nghiáp thuÃn lāi

Em cũng kính gửi låi cÁm ¢n đÁn các thầy cô là giÁng viên cąa Tr°ång Đ¿i Hãc S° Ph¿m Kỹ ThuÃt nói chung cũng nh° các thầy cô trong bá môn Tā đáng điÃu khiÅn vì đã cung cấp kiÁn thÿc đÅ em trau dãi và rèn luyán trong suát bán nm hãc tÃp t¿i tr°ång cũng nh° t¿o điÃu kián thuÃn lāi nhất trong quá trình hoàn thián đã án tát nghiáp này

Em cũng xin gửi đÁn giÁng viên h°ãng dÁn, cô Lê Thá Hãng Lam låi cÁm ¢n sâu sÅc nhất, là ng°åi cô tâm huyÁt, đã tÃn tâm h°ãng dÁn, giúp đÿ em trong suát quá trình nghiên cÿu và thāc hián đà tài Cô đã có nhÿng trao đåi và góp ý đÅ em có thÅ hoàn thành tát đà tài nghiên cÿu này

Cho dù em đã cá hÁt sÿc có thÅ đÅ thāc hián đà tài mát cách hoàn chßnh nhất nh°ng viác thiÁu sót là điÃu không thÅ tránh khßi mà em ch°a thÅ nhÃn ra đ°āc Do đó em rất mong có đ°āc nhÿng góp ý tā thầy cô đÅ đà tài này có thÅ trç nên hoàn thián h¢n

Em xin kính chúc quý thầy cô và có thÃt nhiÃu sÿc khße, luôn thành công trong cuác sáng cũng nh° công viác cąa mình

Trân trãng cÁm ¢n!

Thành Phá Hã Chí Minh, ngày 14 tháng 6 nm 2024

Sinh viên th āc hißn

Tr ần NhÃt C°ãng

đồ án

M ĀC LĀC

NHIàM VĂ Đâ ÁN TàT NGHIàP i

LàCH TRÌNH THĀC HIàN Đâ ÁN TàT NGHIàP ii

NHÂN XÉT CĄA GIÁO VIÊN H¯âNG DÀN iii

NHÂN XÉT CĄA GIÁO VIÊN PHÀN BIàN Error! Bookmark not defined LäI CAM ĐOAN v

BÀN CAM KÀT VÀ XÁC NHÂN KÀT QUÀ KIÄM TRA Đ¾O VN vi

LäI CÀM ¡N vii

DANH MĂC HÌNH xi

DANH MĂC BÀNG xiii

DANH MĂC TĀ VIÀT TÄT VÀ KÝ HIàU xiv

2.1 Mô hình há con lÅc ng°āc quay kép 3

2.2 Ph°¢ng trình phi tuyÁn há Con lÅc ng°āc quay kép 4

2.3 KhÁo sát há t¿i điÅm làm viác cÁ 2 thanh con lÅc cùng h°ãng lên 8

2.3.1 TuyÁn tính hóa há t¿i điÅm làm viác 8

2.3.2 Mô tÁ há tháng d°ãi d¿ng ph°¢ng trình tr¿ng thái 8

2.3.3 KhÁo sát tính điÃu khiÅn đ°āc 10

2.4 KhÁo sát há t¿i điÅm làm viác link 1 h°ãng lên, link 2 h°ãng xuáng 11

đồ án

2.4.1 TuyÁn tính hóa há t¿i điÅm làm viác 11

2.4.2 Mô tÁ há tháng d°ãi d¿ng ph°¢ng trình tr¿ng thái 11

2.4.3 KhÁo sát tính điÃu khiÅn đ°āc 11

2.5 ThiÁt kÁ bá điÃu khiÅn LQR 12

CH¯¡NG 3 MÔ PHäNG VÀ KHÀO SÁT TRÊN MATLAB SIMULINK 15

3.1 Mô phßng há con lÅc ng°āc quay kép trên Matlab Simulink 15

3.2 KhÁo sát bá điÃu khiÅn LQR khi cÁ 2 thanh cùng h°ãng lên 16

3.4 Tång kÁt và khÁo sát mô phßng há DRIP 27

CH¯¡NG 4 XÂY DĀNG MÔ HÌNH THĀC T¾ VÀ KHÀO SÁT 28

4.1 Măc tiêu thiÁt kÁ 28

DANH M ĀC HÌNH

Hình 2.1 Há con lÅc ng°āc quay kép 3

Hình 2.2 Cấu trúc bá điÃu khiÅn LQR cho há DRIP 12

Hình 3.1 S¢ đã khái mô phßng há DRIP và bá điÃu khiÅn LQR 15

Hình 4.1 S¢ đã khái điÃu khiÅn há con lÅc 28

Hình 4.2 ThiÁt kÁ há DRIP trên SOLIDWORKS 29

Hình 4.3 S¢ đã chân Arduino Mega 2560 31

Hình 4.13 Giao dián điÃu khiÅn 40

Hình 4.14 KÁt quÁ mô phßng bá điÃu khiÅn 41

Hình 4.15 Đáp ÿng cąa link 1 vãi bá điÃu khiÅn ban đầu 42

Hình 4.16 Đáp ÿng cąa link 2 vãi bá điÃu khiÅn ban đầu 43

Hình 4.17 BiÅu đã dao đáng cąa thanh cánh tay vãi bá điÃu khiÅn LQR 44

đồ án

Hình 4.18 BiÅu đã dao đáng cąa con lÅc 1 vãi bá điÃu khiÅn LQR 45

Hình 4.19 BiÅu đã dao đáng cąa con lÅc 2 vãi bá điÃu khiÅn LQR 45

Hình 4.20 Đián áp điÃu khiÅn đáng c¢ 46

Hình 4.21 Há ho¿t đáng khi ç tr¿ng thái cân bằng 47

Hình 4.22 Góc cąa link 2 khi không có bá điÃu khiÅn LQR 47

Hình 4.23 Góc cąa link 2 khi có bá điÃu khiÅn LQR 48

đồ án

DANH M ĀC BÀNG

BÁng 2.1 Mô tÁ ký hiáu đ°āc sử dăng 3 BÁng 2.2 BÁng tham sá trung gian 8 BÁng 2.3 BÁng thông sá mô hình 9

đồ án

DANH M ĀC TĆ VI¾T TÄT VÀ KÝ HIÞU

DRIP Double Rotary Inverted Pendulum RIP Rotary Inverted Pendulum

LQR Linear Quadratic Regulator SISO Single Input Single Output SIMO Single Input Multiple Output

đồ án

TÓM T ÄT

<Con lắc ngược quay kép= là há tháng phi tuyÁn phÿc t¿p bất ån đánh mát vào

nhiÃu ra (SIMO) đ°āc phát triÅn tā mô hình con lÅc ng°āc quay (rotary inverted pendulum - RIP) mát bÃc ván rất kinh điÅn trong điÃu khiÅn tā đáng

Bá điÃu khiÅn LQR là mát ph°¢ng pháp điÃu khiÅn trong lý thuyÁt điÃu khiÅn tā đáng, viÁt tÅt cąa "Linear Quadratic Regulator." Nó đ°āc sử dăng đÅ thiÁt kÁ mát bá điÃu khiÅn tuyÁn tính đÅ ån đánh và điÃu khiÅn mát há tháng đáng lāc tuyÁn tính Ph°¢ng pháp này th°ång đ°āc áp dăng cho các há tháng đáng lāc có mô hình tuyÁn tính Că thÅ, LQR giÁi quyÁt bài toán tái °u hóa mát hàm chi phí toàn căc, th°ång là tå hāp cąa sai sá tr¿ng thái và điÃu khiÅn Hàm chi phí này đ°āc chãn đÅ đÁm bÁo rằng há tháng ho¿t đáng theo cách mong muán và đáp ÿng yêu cầu că thÅ

Măc đích cąa đà tài này này là phát triÅn và điÃu khiÅn há tháng <Con lắc ngược

quay kép= (Rotary Double Inverted Pendulum - RDIP) Viác điÃu khiÅn cân bằng cąa

há tháng RDIP là mát đà tài nghiên cÿu khó khn cho các nhà nghiên cÿu trong l*nh vāc điÃu khiÅn đáng hãc do tính phi tuyÁn, đá tā do cao và tính không ån đánh cąa nó Há tháng RDIP sử dăng chß mát đáng c¢ đÅ điÃu khiÅn hai con lÅc đÁo ng°āc đ°āc kÁt nái theo d¿ng chußi Há tháng luôn ho¿t đáng d°ãi sā không chÅc chÅn và Ánh h°çng tā các biÁn đáng NhiÃu thuÃt toán điÃu khiÅn không hiáu quÁ hoặc thÃm chí không thÅ kiÅm soát đ°āc há tháng RDIP Đã án này trình bày bá điÃu khiÅn tái °u đÅ ån đánh há tháng: Bá điÃu khiÅn Linear Quadratic Regulator (LQR) Nhÿng nái dung đ°āc thāc hián s¿ gãm: xây dāng mô hình thāc tÁ, dùng ph°¢ng trình Euler-Lagrange đÅ xác đánh ph°¢ng trình phi tuyÁn cąa há, tuyÁn tính hóa há t¿i điÅm làm viác và xây dāng bá điÃu khiÅn LQR

Tć khóa: Bộ điều khiển hệ con lắc ngược 2 bậc, bộ điều khiển LQR, con lắc

ngược quay kép, điều khiển cân bằng, tối ưu bộ điều khiển

đồ án

Bá điÃu khiÅn LQR là mát ph°¢ng pháp điÃu khiÅn tuyÁn tính đ°āc thiÁt kÁ đÅ tái °u hóa hiáu suất cąa há tháng đáng lāc tuyÁn tính Đ°āc phát triÅn trong l*nh vāc điÃu khiÅn tā đáng, LQR đ°āc sử dăng ráng rãi trong nhiÃu ÿng dăng tā điÃu khiÅn robot đÁn điÃu khiÅn ô tô và các há tháng tā đáng hóa khác LQR dāa trên mát nguyên tÅc c¢ bÁn cąa lý thuyÁt điÃu khiÅn tuyÁn tính, n¢i mát há tháng đáng lāc tuyÁn tính đ°āc biÅu dißn bằng các ph°¢ng trình vi phân tuyÁn tính Bằng cách tái °u hóa mát hàm chi phí toàn căc, LQR giúp xây dāng mát bá điÃu khiÅn tuyÁn tính sao cho hiáu suất cąa há tháng là tát nhất theo mát tiêu chí că thÅ Tr°ãc hÁt, đÅ áp dăng LQR, chúng ta cần có mát mô hình toán hãc cąa há tháng Nó th°ång bao gãm các biÁn tr¿ng thái, biÁn điÃu khiÅn, và các ph°¢ng trình vi phân tuyÁn tính mô tÁ sā biÁn đåi cąa há tháng theo thåi gian

Trong tång quan, LQR không chß là mát ph°¢ng pháp điÃu khiÅn mà còn là mát công că m¿nh m¿ Vì vÃy, trong đà tài này, đái vãi há <Con lÅc ng°āc quay kép=, sinh viên

đồ án

bá điÃu khiÅn khác cho há Nhiám vă cần thāc hián đÅ đ¿t đ°āc măc tiêu nghiên cÿu:

 Trình bày mát cái nhìn tång quan và há <Con lắc ngược quay kép= và bá

điÃu khiÅn LQR  Đ°a ra khái niám, măc tiêu chung cąa đà tài, tiêu chí đánh giá, các ph°¢ng

pháp thāc hián  Thāc hián đ°a ch°¢ng trình cąa bá điÃu khiÅn LQR vào mô hình thāc tÁ  Quan sát kÁt quÁ, so sánh và đánh giá vãi tāng tr°ång hāp đà ra Xem xét

và so sánh đá hiáu quÁ cąa bá điÃu khiÅn và rút ra kÁt luÃn

1.3 N ßi dung nghiên cąu

Các nái dung trong đà tài s¿ bao gãm

Ch°¢ng 1: Tång quan Ch°¢ng 2: C¢ sç lý thuyÁt Ch°¢ng 3: Mô phßng và khÁo sát há trên phần mÃm Matlab Simulink Ch°¢ng 4: Xây dāng mô hình thāc tÁ

Ch°¢ng 5: KÁt luÃn và h°ãng phát triÅn

1.4 Gi ái h¿n

ĐÃ tài chß tÃp trung điÃu khiÅn cân bằng há t¿i vá trí link 1 h°ãng lên và link 2 h°ãng xuông Há tháng chß đáp ÿng khi góc lách ban đầu cąa 2 thanh con lÅc không quá 100 Chß điÃu khiÅn cân bằng đ°āc t¿i vá trí link 1 h°ãng lên link 2 h°ãng xuáng, ch°a điÃu khiÅn đ°āc cân bằng ç hai vá trí còn l¿i là 2 thanh cùng h°ãng lên và link 1 h°ãng xuáng, link 2 h°ãng lên

đồ án

3

CH¯¡NG 2 C¡ Sä LÝ THUY¾T

2.1 Mô hình h ß con lÅc ng°ÿc quay kép

BiÅu đã cąa há con lÅc ng°āc quay kép đ°āc hiÅn thá nh° Hình 2.1 [7]:

Hình 2.1 Hệ con lắc ngược quay kép

Mô hình há con lÅc ng°āc quay kép bao gãm 3 thanh rÅn và 1 đáng c¢ Thanh thÿ nhất đ°āc gãi là thanh cánh tay đ°āc gÅn vãi trăc đáng c¢ theo ph°¢ng nằm ngang, thanh thÿ hai đ°āc gãi là thanh con lÅc 1 đ°āc dāng theo ph°¢ng thẳng đÿng, thanh thÿ 3 đ°āc gãi là thanh con lÅc 2 nái tiÁp vãi thanh con lÅc 1

Bảng 2.1 Mô tả ký hiệu được sử dụng

þ3 Khái l°āng thanh con lÅc 2 kg �㔃1 Góc quay cąa thanh cánh tay rad �㔃2 Góc quay cąa thanh con lÅc 1 rad

đồ án

4

2.2 Ph°¢ng trình phi tuy¿n hß Con lÅc ng°ÿc quay kép

Trong ch°¢ng này, chúng ta s¿ khÁo sát há DRIP tā viác tāng b°ãc lÃp há ph°¢ng trình vi phân cąa há DRIP tr°ãc tiên, rãi sau đó tuyÁn tính hóa t¿i điÅm làm viác là cÁ 2 thanh cùng h°ãng lên và link 1 h°ãng lên, link 2 h°ãng xuáng Dāa vào há ph°¢ng trình khi tuyÁn tính hóa, ta s¿ xét đÁn yÁu tá điÃu khiÅn đ°āc và thiÁt kÁ bá điÃu khiÅn LQR cho mô phßng và thāc nghiám

Mô hình cąa há DRIP đ°āc biÅu dißn tā ph°¢ng trình Euler-Lagrange nh° sau [7]: þ

þą(�㔕ÿ�㔕Ăÿ) 2�㔕Ă�㔕ÿ

ÿ+�㔕þ�㔕Ă

Trong đó:

Ă(ą): vector vá trí góc Ă(ą): vector vÃn tác góc Āÿ: vector lāc tác dăng �㔃3 Góc quay cąa thanh con lÅc 2 rad ÿ1 ChiÃu dài cąa thanh cánh tay m ÿ2 ChiÃu dài cąa thanh con lÅc 1 m ÿ3 ChiÃu dài cąa thanh con lÅc 2 m ý1 KhoÁng cách gác tãa đá đÁn trãng tâm cánh tay m ý2 KhoÁng cách gác tãa đá đÁn trãng tâm con lÅc 1 m ý3 KhoÁng cách gác tãa đá đÁn trãng tâm con lÅc 2 m ý1 Momen quán tính cąa thanh cánh tay Kgm2ý2 Momen quán tính cąa thanh con lÅc 1 Kgm2ý3 Momen quán tính cąa thanh con lÅc 2 Kgm2

5

ÿ: Lagrange þ: Nng l°āng tiêu hao ĐÅ có đ°āc ph°¢ng trình Euler-Lagrange, tÃp hāp đ¿o hàm riêng cąa Lagrange, L đ°āc đánh ngh*a nh° sau:

Trong đó:  �㕇�㕡Ā�㕡ÿý: Tång đáng nng cąa há  ý�㕡Ā�㕡ÿý: Tång thÁ nng cąa há Tång đáng nng cąa há là tång đáng nng cąa thanh cánh tay và 2 thanh con lÅc, đ°āc tính nh° sau:

ĐÅ sử dăng ph°¢ng trình Euler-Lagrange cho viác mô tÁ há DRIP, ma trÃn Ăÿ đ°āc đánh ngh*a nh° sau:

Ăÿ = [�㔃�㔃12�㔃3

]

(4) Ma trÃn Ăÿ cũng đ°āc đánh ngh*a nh° sau:

Ăÿ = [�㔃�㔃12�㔃3

]

(5)

Và cuái cùng là ma trÃn Āÿ: Āÿ = [0�㔏

] = [ýý11ýĀĄ�㔃Ąÿÿ�㔃11

0 ]

(7)

đồ án

7

Tā đó ta đ°āc tång thÁ nng cąa há là: ý = þ2āý2ýĀĄ�㔃2+ þ3ā(ÿ2ýĀĄ�㔃2+ ý3ýĀĄ�㔃3) (19)

Do bß qua ma sát, nên nng l°āng tiêu hao xấp xß là 0 nên

þą(�㔕ÿ�㔕�㔃1) 2

�㔕ÿ�㔕�㔃1 = �㔏þ

þą(�㔕ÿ�㔕�㔃2) 2

�㔕ÿ�㔕�㔃2 = 0þ

þą(�㔕ÿ�㔕�㔃3) 2

�㔕ÿ�㔕�㔃3 = 0

þ

(24) Và:

þ2 =þ�㕡þĀ

āþ

(25) Và:

Há ph°¢ng trình đáng hãc d°ãi d¿ng vi phân cąa há tháng là:

đồ án

8

{2þ3�㔃12 þ2�㔃1+ þ1ÿ = /1�㔃1+ (/2+ /3)ýĀĄ�㔃2�㔃2+ /4ýĀĄ�㔃3�㔃32 (/2+ /3)Ąÿÿ�㔃2�㔃222 (/4Ąÿÿ�㔃3)�㔃32

0 = (/2+ /3)ýĀĄ�㔃2�㔃1+ /5�㔃2 + /6cos(�㔃22 �㔃3) �㔃3+ /6sin(�㔃22 �㔃3) �㔃322 (/7+ /8)Ąÿÿ�㔃20 = (/4ýĀĄ�㔃3)�㔃1+ /6cos(�㔃2 2 �㔃3) �㔃2+ /9�㔃32 /6sin(�㔃2 2 �㔃3)�㔃222 /(27) 10Ąÿÿ�㔃3Trong đó:

Bảng 2.2 Bảng tham số trung gian

Ký hi ßu Giá tr á

/1 ý1+ þ2ÿ12+ þ3ÿ21/2 þ2ÿ1ý2/3 þ3ÿ1ÿ2/4 þ3ÿ1ý3/5 ý2+ þ2ý22+ þ3ÿ22/6 þ3ÿ2ý3

/9 ý3+ þ3ý32/10 þ3āý3

2.3 Kh Áo sát hß t¿i điÃm làm vißc cÁ 2 thanh con lÅc cùng h°áng lên 2.3.1 Tuy ¿n tính hóa hß t¿i điÃm làm vißc

ĐÅ tuyÁn tính hóa há, ta bÅt đầu tā ph°¢ng trình đáng lāc hãc tr°ãc khi tuyÁn tính đ°āc trình bày d°ãi d¿ng vi phân Khi thanh cánh tay ç vá trí gác, hai thanh con lÅc ç vá trí thẳng đÿng h°ãng lên (�㔽Ā = ÿÿ, �㔽ā = ÿÿ, �㔽Ă = ÿÿ), ta có thÅ tuyÁn tính hóa bằng cách áp dăng xấp xß góc nhß: �㖄Ā�㖔�㔽 ≈ Ā, �㖔�㖊ÿ�㔽 ≈ ÿ

Tā đó, ta đ°āc há ph°¢ng trình tuyÁn tính cąa há t¿i điÅm làm viác: {

2þ3�㔃12 þ2�㔃1+ þ1ÿ = /1�㔃1+ (/2+ /3)�㔃2+ /4�㔃3

0 = (/2+ /3)�㔃1+ /5�㔃2 + /6�㔃30 = /4�㔃1+ /6�㔃2 + /9�㔃3

(28)

2.3.2 Mô t Á hß thçng d°ái d¿ng ph°¢ng trình tr¿ng thái

ĐÅ có thÅ mô tÁ há tháng d°ãi d¿ng ph°¢ng trình tr¿ng thái, tr°ãc tiên ta cần đặt các biÁn tr¿ng thái nh° sau:

đồ án

9

ý1 = �㔃1, ý2 = �㔃1, ý3 = �㔃2, ý4 = �㔃2, ý5 = �㔃3, ý6 = �㔃3 (29) Và:

Ta đ°āc:

þýþą = Ā(ý, Ć, ą) =

þþą

[ý1ý2ý3ý4ý5ý6]

=þąþ[

�㔃1�㔃1�㔃2�㔃2�㔃3�㔃3]

=[ �㔃1

�㔃1�㔃2�㔃2�㔃3�㔃3]

=[

Ā1Ā2Ā3Ā4Ā5Ā6]

�㔕ý2 �㔕Ā1�㔕ý3 �㔕Ā1

�㔕ý4 �㔕Ā1�㔕ý5 �㔕Ā1

�㔕ý6�㔕Ā2

�㔕ý1 �㔕Ā2�㔕ý2 �㔕Ā2

�㔕ý3 �㔕Ā2�㔕ý4 �㔕Ā2

�㔕ý5 �㔕Ā2�㔕ý6�㔕Ā3

�㔕ý1 �㔕Ā3�㔕ý2

�㔕Ā3�㔕ý3

�㔕Ā3�㔕ý4

�㔕Ā3�㔕ý5

�㔕Ā3�㔕ý6�㔕Ā4

�㔕ý1 �㔕Ā4�㔕ý2

�㔕Ā4�㔕ý3

�㔕Ā4�㔕ý4

�㔕Ā4�㔕ý5

�㔕Ā4�㔕ý6�㔕Ā5

�㔕ý1 �㔕Ā5�㔕ý2

�㔕Ā5�㔕ý3

�㔕Ā5�㔕ý4

�㔕Ā5�㔕ý5

�㔕Ā5�㔕ý6�㔕Ā6

�㔕ý1 �㔕ý�㔕Ā6

2 �㔕ý�㔕Ā63 �㔕ý�㔕Ā6

4 �㔕ý�㔕Ā65 �㔕ý�㔕Ā6

6]

(33)

þ = [�㔕Ā1�㔕Ć

�㔕Ā2�㔕Ć

�㔕Ā3�㔕Ć

�㔕Ā4�㔕Ć

�㔕Ā5�㔕Ć

�㔕Ā6�㔕Ć]

2.3.3 Kh Áo sát tính điÁu khiÃn đ°ÿc

Vãi giá trá A và B đ°āc tính ç trên, ma trÃn điÃu khiÅn đ°āc M đ°āc tính nh° sau:

M = [B AB A2B A3B A4B A5B]T=

[

0.0192 22e−5 20.0469 4.6e−5 0.04 24.03e−522e−5 0.2614 4.6e−5 26.6922 24e−5 14.86680.2614 25e−4 26.6922 0.0072 14.866 20.015325e−4 40.466 0.0072 21.1e3 20.015 3.21e340.466 20.083 21.1e3 1.2781 3.21e3 23.4633 ]

(37)

Ta tính đ°āc Rank(M) = 6 nên há điÃu khiÅn đ°āc t¿i điÅm làm viác cÁ 2 thanh cùng h°ãng lên

đồ án

Tā đó ta đ°āc há ph°¢ng trình tuyÁn tính cąa há: {

2þ3�㔃12 þ2�㔃1+ þ1ÿ = /1�㔃1+ (/2+ /3)�㔃22 /4�㔃3

0 = (/2+ /3)�㔃1+ /5�㔃2 2 /6�㔃30 = 2/4�㔃12 /6�㔃2 + /9�㔃3

(38)

2.4.2 Mô t Á hß thçng d°ái d¿ng ph°¢ng trình tr¿ng thái

T°¢ng tā nh° t¿i điÅm làm viác tr°ãc đó, ta áp dăng (29), (30), (31), (32), (33) và (34), t¿i điÅm cân bằng là link 1 h°ãng lên, link 2 h°ãng xuáng, ta đ°āc ma trÃn A là:

A =[

2.4.3 Kh Áo sát tính điÁu khiÃn đ°ÿc

Vãi giá trá A và B đ°āc tính ç trên, ma trÃn điÃu khiÅn đ°āc M đ°āc tính nh° sau:

M = [B AB A2B A3B A4B A5B]T=

26.49 20.055 2602.5 0.719 2520.4 0.6399 ]

(41)

đồ án

12

Ta tính đ°āc Rank(M) = 6 nên há điÃu khiÅn đ°āc t¿i điÅm làm viác thanh 1 h°ãng lên, thanh 2 h°ãng xuáng

2.5 Thi ¿t k¿ bß điÁu khiÃn LQR

Bá điÃu khiÅn LQR là mát trong nhÿng công că quan trãng trong lý thuyÁt điÃu khiÅn và ÿng dăng trong nhiÃu l*nh vāc khác nhau nh° tā đáng hóa, robot, điÃu khiÅn há tháng đáng c¢, và nhiÃu ÿng dăng công nghiáp khác Nó đ°āc thiÁt kÁ đÅ tái °u hóa hiáu suất cąa há tháng điÃu khiÅn tuyÁn tính trong môi tr°ång có nhißu Ta có đ°āc cấu trúc cąa bá điÃu khiÅn LQR đái vãi há con lÅc ng°āc quay kép nh° Hình 2.2 vãi đầu vào là 6 biÁn tr¿ng thái cąa há tháng tā link 0 đÁn link 2 và đầu ra là tín hiáu đián áp điÃu khiÅn cho đáng c¢

Hình 2.2 Cấu trúc bộ điều khiển LQR cho hệ DRIP

Bá điÃu khiÅn LQR ho¿t đáng dāa trên ph°¢ng pháp tái °u hóa ph°¢ng trình bÃc hai Măc tiêu cąa nó là tái thiÅu hóa hàm mất mát tuyÁn tính đ°āc xác đánh bằng tång trãng sá cąa sā sai khác giÿa tr¿ng thái thāc tÁ cąa há tháng và tr¿ng thái mong muán, cáng vãi tång trãng sá cąa tín hiáu điÃu khiÅn Că thÅ, hàm mất mát đ°āc đánh ngh*a là

đồ án

3 GiÁi ph°¢ng trình Riccati: Sử dăng ph°¢ng trình Riccati đÅ tính toán ma trÃn điÃu khiÅn tái °u

4 Tính toán tín hiáu điÃu khiÅn: Tính toán tín hiáu điÃu khiÅn tái °u tā ma trÃn điÃu khiÅn

Viác sử dăng bá điÃu khiÅn LQR s¿ mang l¿i cho ta nhÿng °u điÅm nh° sau:  än đánh: Bá điÃu khiÅn LQR t¿o ra các tín hiáu điÃu khiÅn ån đánh dāa trên các

mô hình toán hãc cąa há tháng  Tái °u hóa: Cung cấp tín hiáu điÃu khiÅn tái °u dāa trên các ma trÃn trãng sá

đ°āc xác đánh tr°ãc, giúp cÁi thián hiáu suất há tháng  Dß triÅn khai: Ph°¢ng pháp này có thÅ triÅn khai dß dàng trên các há tháng tuyÁn

tính và ít phÿc t¿p Tā nhÿng °u điÅm trên, ta có thÅ thấy đ°āc sā ÿng dăng ráng rãi cąa bá điÃu khiÅn LQR:

 Tā Đáng Hóa: ĐiÃu khiÅn xe tā lái, robot công nghiáp, và các há tháng tā đáng khác

 Công Nghiáp: ĐiÃu khiÅn quá trình trong sÁn xuất, điÃu khiÅn cánh tay robot công nghiáp

 Hàng Không và Vũ Tră: ĐiÃu khiÅn máy bay và vá tinh Tā nhÿng điÅm nêu trên, ta có thÅ đúc kÁt ra đ°āc bá điÃu khiÅn LQR đ°āc áp dăng cho mát há tháng <điÃu khiÅn đ°āc= đÅ ån đánh há tháng t¿i điÅm làm viác cân bằng Đây là ph°¢ng pháp có thÅ áp dăng cho các há có sá ngõ vào-ra thay đåi, dß dàng thiÁt kÁ và triÅn khai vãi sā hß trā cąa phần mÃm Matlab nên th°ång đ°āc áp dăng cho các

đồ án

Tuy nhiên, do bá điÃu khiÅn LQR đã trç nên phå biÁn trong giÁi thuÃt điÃu khiÅn cân bằng, Matlab đã hß trā cho ng°åi dùng tính toán thông sá cho bá điÃu khiÅn K cąa LQR vãi công thÿc đ¢n giÁn h¢n Đái vãi há liên tăc thì ta s¿ sử dăng công thÿc sau [17]:

đồ án

15

CH¯¡NG 3 MÔ PHäNG VÀ KHÀO SÁT TRÊN

MATLAB SIMULINK

3.1 Mô ph ång hß con lÅc ng°ÿc quay kép trên Matlab Simulink

Há DRIP s¿ có đầu vào nhÃn tín hiáu điÃu khiÅn là đián áp cung cấp cho đáng c¢ và có 4 ngõ ra quan sát là góc lách cąa thanh cánh tay, con lÅc 1, con lÅc 2 và tín hiáu điÃu khiÅn

Ta có s¢ đã khái mô phßng há DRIP trên Matlab Simulink nh° Hình 3.1

Hình 3.1 Sơ đồ khối mô phỏng hệ DRIP và bộ điều khiển LQR

Trong phần mô phßng này, ta s¿ khÁo sát há DRIP ç hai điÅm làm viác là cÁ 2 thanh cùng h°ãng lên và link 1 h°ãng lên, link 2 h°ãng xuáng Trong mßi điÅm làm viác ta s¿ khÁo sát há bằng cách tā ma trÃn đ¢n vá, ta tng tāng thành Ā1, Ā3, Ā5 lên 10 và cuái cùng là tng R lên 10 đÅ xem đáp ÿng cąa há qua mßi lần thay đåi thành phần điÃu khiÅn cąa các ma trÃn Q và R

đồ án

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1]

17

Các biÅu đã tā trên xuáng d°ãi trong Hình 3.2, lần l°āt là góc cąa thanh cánh tay, góc cąa con lÅc 1, góc cąa con lÅc 2 và tín hiáu đián áp điÃu khiÅn đáng c¢ Ta thấy đ°āc thanh cánh tay có biên đá dao đáng lên tãi 320, còn 2 thanh con lÅc s¿ có biên đá dao đáng 0.60 và mất 10 giây đÅ trç và vá trí cân bằng là cÁ 2 thanh cùng h°ãng lên

3.2.2 Tng thành phần �㕸Ā và gi ÿ nguyên các thành phần khác

Ta tng thành phần điÃu khiÅn Ā1 lên 10 trong ma trÃn Q và giÿ nguyên các thành phần còn l¿i đÅ khÁo sát há khi có sā thay đåi cąa ma trÃn điÃu khiÅn

Q =[

10 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1]

Vãi các thông sá ban đầu theo (50) Ta đ°āc kÁt quÁ mô phßng nh° Hình 3.3

Hình 3.3 K ết quả mô phỏng trường hợp 2

đồ án

18

VÁn nh° Hình 3.2, biÅu đã đầu tiên thÅ hián góc cąa thanh cánh tay, tiÁp theo lần l°āt là góc cąa con lÅc 1, con lÅc 2 và tín hiáu đián áp điÃu khiÅn đáng c¢ Ta thấy đ°āc so vãi bá điÃu khiÅn tr°ãc đó thì biên đá dao đáng cąa thanh cánh tay giÁm tā 320 xuáng còn 200khi tng thành phần điÃu khiÅn Ā1, 2 thanh con lÅc vÁn giÿ nguyên biên đá dao đáng là 0.60 và cũng s¿ mất 10 giây đÅ há cân bằng

3.2.3 Tng thành phần �㕸Ă và gi ÿ nguyên các thành phần khác

Ta tng thành phần điÃu khiÅn Ā3 lên 10 trong ma trÃn Q và giÿ nguyên các thành phần còn l¿i đÅ khÁo sát há khi có sā thay đåi cąa ma trÃn điÃu khiÅn

Q =[

1 0 0 0 0 00 1 0 0 0 00 0 10 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1]

Vãi các thông sá ban đầu theo (50) Ta đ°āc kÁt quÁ mô phßng nh° Hình 3.4

Hình 3.4 Kết quả mô phỏng trường hợp 3

đồ án