Control and trajectory tracking of a self balancing two wheeled robot

Mục tiêu cụ thể như sau : - Thiết kế bộ điều khiển LQR và bộ điều khiển trượt, áp dụng cho đối tượng Robot hai bánh tự cân bằng, điều khiển cho Robot bám quỹ đạo đặt.. 4 Địa hình phẳng

ĐẠI HỌC QUỐC GIA TP HCM

TRƯỜNG ĐẠI HỌC BÁCH KHOA

-

NGUYỄN HỒNG THẮNG

ĐIỀU KHIỂN ROBOT HAI BÁNH TỰ CÂN BẰNG

VÀ BÁM QUỸ ĐẠO CONTROL AND TRAJECTORY TRACKING OF

A SELF-BALANCING TWO WHEELED ROBOT

Chuyên ngành: Kỹ Thuật Điều Khiển Và Tự Động Hóa

Mã số: 60520216

LUẬN VĂN THẠC SĨ

TP HỒ CHÍ MINH, tháng 01 năm 2019

CÔNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA –ĐHQG -HCM

Cán bộ hướng dẫn khoa học: TS Nguyễn Vĩnh Hảo

ĐẠI HỌC QUỐC GIA TP.HCM

TRƯỜNG ĐẠI HỌC BÁCH KHOA

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM

Độc lập - Tự do - Hạnh phúc

NHIỆM VỤ LUẬN VĂN THẠC SĨ

Họ tên học viên : NGUYỄN HỒNG THẮNG MSHV:7140959

Ngày, tháng, năm sinh : 13/07/1986 Nơi sinh : Hải Phòng

Chuyên ngành : Kỹ Thuật Điều Khiển Và Tự Động Hóa Mã số : 60520216

I TÊN ĐỀ TÀI :

ĐIỀU KHIỂN O OT H I NH TỰ CÂN BẰNG VÀ BÁM QUỸ ĐẠO

II NHIỆM VỤ VÀ NỘI DUNG :

Trong luận văn này, mục tiêu là điều khiển để giữ được cân bằng cho hệ Robot hai

bánh tự cân bằng và bám quỹ đạo đặt Mục tiêu cụ thể như sau :

- Thiết kế bộ điều khiển LQR và bộ điều khiển trượt, áp dụng cho đối tượng

Robot hai bánh tự cân bằng, điều khiển cho Robot bám quỹ đạo đặt Lập trình trên

Matlab, tiến hành mô phỏng và so sánh kết quả điều khiển cân bằng và ám uỹ đạo

c a hệ Robot hai bánh

- Xây dựng mô hình thực tế hệ Robot hai bánh tự cân bằng

- Xây dựng chương trình nhúng cho vi điều khiển STM32F4

- Điều khiển cân bằng hệ o ot hai ánh và điều khiển bám quỹ đạo trên mô hình

thực tế đã xây dựng và đánh giá kết quả

III NGÀY GIAO NHIỆM VỤ : 26/02/2018

IV NGÀY HOÀN THÀNH NHIỆM VỤ : 02/12/2018

V CÁN BỘ HƯỚNG DẪN: TS Nguyễn Vĩnh Hảo

LỜI CẢM ƠN

Đầu tiên tôi xin gởi đến Thầy TS Nguyễn Vĩnh Hảo lời cảm ơn chân thành và sâu sắc nhất Nhờ sự hướng dẫn và chỉ bảo tận tình của Thầy trong suốt thời gian thực hiện luận văn Thạc Sĩ đã giúp tôi có một định hướng đúng đắn trong suốt quá trình thực hiện

đề tài

Xin trân trọng cảm ơn các Thầy Cô của Trường Đại Học Bách Khoa nói chung và của khoa Điện – Điện Tử nói riêng đã giảng dạy, cung cấp cho tôi nhiều kiến thức bổ ích trong quá trình nghiên cứu và học tập tại trường

Xin cảm ơn các bạn bè trong khoá cao học Tự Động Hoá 2016 bạn Huy Hoàng đã hỗ trợ và cho tôi những góp ý tư vấn hết sức quý giá, giúp đỡ tôi trong quá trình học tập

và làm đề tài

Con xin chân thành cám ơn gia đình và cha mẹ đã tạo cho con điều kiện tốt nhất trong thời gian con học cao học Đặc biệt, con xin gởi lời cám ơn trân trọng nhất đến cha mẹ, người đã sinh ra và nuôi dưỡng con nên người Sự quan tâm lo lắng và hi sinh lớn lao của cha mẹ luôn là động lực cho con cố gắng phấn đấu trên con đương học tập của mình

Hồ Chí Minh, ngày 02 tháng 12 năm 2018

Thân ái Học viên

NGUYỄN HỒNG THẮNG

TÓM TẮT LUẬN VĂN THẠC SĨ

Ngày nay, robot đóng vai trò rất quan trọng trong sự phát triển của loài người Trong khoa học kỹ thuật, robot hỗ trợ con người thực hiện những nhiệm vụ nguy hiểm như dò mìn thăm dò dưới đại dương thám hiểm vệ tinh, hoạt động quân sự Gần đây một số loại robot hai bánh cũng phát triển mạnh, robot hai bánh này ứng dụng cho việc hỗ trợ

di chuyển trong siêu thị văn phòng trường học Robot hai bánh là một hệ gồm có hai bánh xe song song và một con lắc ngược vốn không ổn định và phi tuyến

Trong luận văn tác giả thực hiện thiết kế điều khiển cân bằng cho hệ robot hai bánh tự cân bằng điều khiển cho robot cân bằng di chuyển trên quỹ đạo đặt ra Hệ robot hai bánh được mô hình hóa và mô phỏng, điều khiển trên Matlab/Simulink với giải thuật LQR và điều khiển trượt Các bộ điều khiển sau khi được thiết kế bằng lý thuyết và mô phỏng đã được kiểm nghiệm thực tế với mô hình thực

ABSTRACT

Today, robots play a very important role in the development of mankind In science and technology, robots help people perform dangerous tasks such as mine detection and exploration of the ocean, satellite exploration, military operations Recently, a number

of two-wheeled robots has been rapidly developed, they are applied to assist people in supermarkets, offices, schools The two-wheeled robot consists of two wheels in parallel and an inverse pendulum, which is inherently unstable and nonlinear

In this thesis, the author designs a balance controller for the two-wheeled robot to keep balance on its wheels and to track the planned trajectory The two-wheeled robot is modeled and simulated on Matlab / Simulink using LQR and sliding mode controller Then, a realtime controller is tested on the real model

MỤC LỤC

CHƯƠNG 1TỔNG QUAN VỀ ĐỀ TÀI 1

1.1 Giới thiệu đề tài 1

1.2 Một số công trình nghiên cứu liên quan 4

1.3 Mục tiêu đề tài 7

1.4 Phương pháp nghiên cứu 8

1.5 Sơ lược nội dung luận văn 9

CHƯƠNG 2 THIẾT KẾ VÀ MÔ HÌNH HÓA ROBOT HAI BÁNH TỰ CÂN BẰNG 10

2.1 Thiết kế Robot hai bánh tự cân bằng 10

2.1.1 Phần cơ khí 10

2.1.2 Phần mạch điện 11

2.2 Mô hình hóa hệ Robot hai bánh tự cân bằng 14

2.2.1 Xây dựng mô hình toán học hệ Robot hai bánh 14

2.2.2 Đánh giá mô hình toán hệ Robot hai bánh đã xây dựng 21

CHƯƠNG 3 THIẾT KẾ BỘ ĐIỀU KHIỂN CHO HỆ RO OT H I NH TỰ CÂN BẰNG 24

3.1 Bộ điều khiển cân bằng cho hệ Robot hai bánh 24

3.1.1 ộ điều khiển LQR 24

3.1.2 Bộ điều khiển trượt 29

3.2 Bộ điều khiển bám quỹ đạo cho hệ Robot hai bánh tự cân bằng 39

3.2.1 Thiết kế quỹ đạo đặt cho Robot 8 9 10 39

3.2.2 Chương trình Matlab/Simulink mô phỏng bộ điều khiển LQR bám quỹ đạo 40 3.2.3 Thiết kế bộ điều khiển trượt bám quỹ đạo đặt và mô phỏng Matlab/Simulink43 CHƯƠNG 4THIẾT KẾ BỘ ĐIỀU KHIỂN THỰC NGHIỆM 50

4.1 Đọc dữ liệu góc nghiêng từ cảm biến MPU6050 50

4.1.1 IMU (Inertial Measurement Unit) – thiết bị đo lường quán tính 50

4.1.2 Lý thuyết bộ lọc Kalman 51

4.1.3 Xây dựng bộ lọc Kalman cho MPU6050 55

4.2 Xây dựng chương trình điều khiển nhúng cho STM32F4 58

4.2.1 Khối Position and Velocity Conditioning 60

4.2.2 Khối Gyros Conditioning 60

4.2.3 Khối Tranceiver Block via UART 61

4.2.4 Khối Caculating current position 61

4.2.5 Khối Trajectory generator 62

4.2.6 Khối Embedded Controller 62

CHƯƠNG 5KẾT QUẢ VÀ HƯỚNG PHÁT TRIỂN 63

5.1 Kết quả thực nghiệm 63

5.1.1 Kết quả kiểm nghiệm bộ lọc Kalman 63

5.1.2 Kết quả kiểm nghiệm thuật toán LQR 63

5.1.3 Kết quả kiểm nghiệm thuật toán trượt 67

5.2 Đánh giá kết quả đạt được và hướng phát triển 70

5.2.1 Kết quả đạt được 70

5.2.2 Hướng phát triển 71

TÀI LIỆU THAM KHẢO 72

DANH MỤC HÌNH ẢNH

Hình 1.1: Hình ảnh minh họa Robot sử dụng 3 bánh di chuyển 1

Hình 1.2: Hình ảnh Tbot được hoàn thành tại IHMC 2

Hình 1.3: Hình ảnh minh họa xe 4 bánh lật khi leo dốc 3

Hình 1.4: Hình ảnh minh họa xe 2 bánh khi leo dốc 4

Hình 1.5: Công trình nghiên cứu ở Thụy sĩ 1 4

Hình 1.6: Nbot trái và Legway phải) [2] 5

Hình 1.7: ali ot trong nghiên cứu 3 6

Hình 1.8: Mô hình hệ thống trong Simulink đối tượng được mô hình hóa trong SimMechanics kiểm tra điều khiển LQR d ng toàn bộ vector trạng thái trong nghiên cứu [4]……… 7

Hình 1.9: Mô hình vật lý thực nghiệm d ng Matlab Real Time Toolbox trong nghiên cứu [4] 7

Hình 2.1: Thiết kế cơ khí mô hình Robot hai bánh d ng phần mềm SolidWorks 10

Hình 2.2: Mô hình Robot hai bánh tự cân bằng sau khi hoàn thành 10

Hình 2.3: Sơ đồ khối phần mạch điện của Robot hai bánh tự cân bằng 11

Hình 2.4: KIT SMT32F4 – DISCOVERY 11

Hình 2.5: Động cơ DC servo Yaskawa 12

Hình 2.6: Cảm biến MPU6050 12

Hình 2.7: Mạch công suất điều khiển động cơ HI216 13

Hình 2.8: Module HC05 13

Hình 2.9: Pin lipo 3s 14

Hình 2.10: Mô hình Robot hai bánh 14

Hình 2.11: Hình chiếu của hệ Robot hai bánh lên hệ trục tọa độ 15

Hình 2.12: Kết quả mô phỏng lần 1 mô hình toán xe hai bánh 22

Hình 2.13: Kết quả mô phỏng lần 2 mô hình toán xe hai bánh 22

Hình 3.1: Sơ đồ không gian trạng thái sử dụngbộ điều khiểnLQR 24

Hình 3.2: Sơ đồ điều khiển tổng quát LQR 27

Hình 3.3: Sơ đồ khối decoupling 28

Hình 3.4: Kết quả mô phỏng điều khiển cân bằng hệ Robot hai bánh sử dụng bộ điều khiển LQR 28

Hình 3.5: Hiện tượng chattering 32

Hình 3.6: Các mặt mức của hàm V(x) 33

Hình 3.7: Sơ đồ điều khiển tổng quát điều khiển trượt 37

Hình 3.8: Sơ đồ bộ điều khiển trượt 37

Hình 3.9: Kết quả mô phỏng hệ Robot hai bánh dưới sự tác động của bộ điều khiển trượt 38

Hình 3.10: Kết quả mô phỏng so sánh đáp ứng bằng (góc lật Psi cân giữa hai bộ điều khiển LQG và điều khiển trượt 38

Hình 3.11: Sơ đồ giải thuật phương pháp phân tích quỹ đạo đặt 39

Hình 3.12: Sơ đồ bộ xử lý quỹ đạo đặt áp dụng cho giải thuật điều khiển LQR 40

Hình 3.13: Kết quả mô phỏng đáp ứng các góc khi chạy theo quỹ đạo đặt 41

Hình 3.14: Kết quả mô phỏng đáp ứng các góc Psi khi chạy theo quỹ đạo đặt 41

Hình 3.15: Kết quả mô phỏng đáp ứng các góc Theta khi chạy theo quỹ đạo đặt 42

Hình 3.16: Kết quả mô phỏng đáp ứng các góc Phi khi chạy theo quỹ đạo đặt 42

Hình 3.17: Kết quả mô phỏng so sánh quỹ đạo đặt và quỹ đạo đáp ứng của Robot 43

Hình 3.18: Cấu trúc của các mặt trượt 44

Hình 3.19: Kết quả mô phỏng đáp ứng các góc khi chạy theo quỹ đạo đặt 47

Hình 3.20: Kết quả mô phỏng đáp ứng các góc Psi khi chạy theo quỹ đạo đặt 48

Hình 3.21: Kết quả mô phỏng đáp ứng các góc Theta khi chạy theo quỹ đạo đặt 48

Hình 3.22: Kết quả mô phỏng đáp ứng các góc Phi khi chạy theo quỹ đạo đặt 49

Hình 3.23: Kết quả mô phỏng so sánh quỹ đạo đặt và quỹ đạo đáp ứng của Robot 49

Hình 4.1: Các hướng chuyển động của vật trong không gian 50

Hình 4.2: Sơ đồ khối bộ lọc tổng quát 52

Hình 4.3: Sơ đồ khối bộ lọc Kalman 52

Hình 4.4: Quy trình thực hiện của bộ lọc Kalman 53

Hình 4.5: Tổng quan chu trình thực hiện bộ lọc Kalman hoàn chỉnh 55

Hình 4.6: Bộ lọc Kalman được sử dụng để lọc nhiễu cảm biến gia tốc và cảm biến vận tốc góc 55

Hình 4.7: Thiết kế khối lọc Kalman trong Matlab/Simulink 57

Hình 4.8: Chương trình nhúng cho vi điều khiển STM32F4 59

Hình 4.9: Khối Position and Velocity Conditioning được xây dựng trong đề tài 60

Hình 4.10: Khối Gyros Conditioning 60

Hình 4.11: Khối Tranceiver Block via UART 61

Hình 4.12: Khối Calculating current position 62

Hình 4.13: Khối Trajectory generator 62

Hình 4.14: Khối Embedded Controller 62

Hình 5.1: Đồ thị vận tốc góc nghiêng quanh trục x 63

Hình 5.2: Đồ thị các góc của robot khi điều khiển giữ cân bằng robot bằng LQR lần 1 64

Hình 5.3: Đồ thị các góc của robot khi điều khiển giữ cân bằng robot bằng LQR Lần 2 64

Hình 5.4: Đồ thị các góc của robot khi thay đổi điểm đặt góc Theta 65

Hình 5.5: Đồ thị các góc của robot khi thay đổi điểm đặt góc Phi 65

Hình 5.6: Đồ thị quỹ đạo khi điều khiển xe bám quỹ đạo hình số 8 chu k T=40s……… 66

Hình 5.7: Đồ thị các góc của robot khi điều khiển xe bám quỹ đạo hình số 8 chu k T=40s……… 66

Hình 5.8: Đồ thị các góc của robot khi điều khiển giữ cân bằng robot bằng luật điều khiển trượt 67

Hình 5.9: Đồ thị các góc của robot khi thay đổi điểm đặt góc Theta 68

Hình 5.10: Đồ thị các góc của robot khi thay đổi điểm đặt góc Phi 68

Hình 5.11: Đồ thị quỹ đạo khi điều khiển xe bám quỹ đạo hình số 8 chu k T=40s……… 69 Hình 5.12: Đồ thị các góc của robot khi điều khiển xe bám quỹ đạo hình số 8 chu k T=40s……… 69

DANH MỤC CÁC BẢNG

Bảng 2.1: ảng ký hiệu các thông số của hệ thống 16Bảng 2.2: Bảng thông số hệ Robot hai bánh .21

1

CHƯƠNG 1 TỔNG QUAN VỀ ĐỀ TÀI

1.1 Giới thiệu đề tài

Từ lâu, con người đã mơ ước sở hữu một người máy làm trợ lý cá nhân, được tạo ra để thực hiện mọi nhiệm vụ của mình Một trong những yêu cầu là Robot chế tạo ra phải có kích thước và hình dạng tương tự như một con người Nhiều Robot di động hiện nay sử dụng ba hoặc bốn bánh và được thiết lập bánh xe cơ sở đủ lớn để Robot có thể đứng thẳng đứng được Tuy nhiên, dựa vào kích thước của bánh xe cơ

sở sẽ giới hạn việc thực hiện các công việc của Robot, bánh xe cơ sở phải nhỏ hơn đáng kể so với chiều cao hình dạng giống như người Vì vậy, chỉ cần một thay đổi nhỏ ở vị trí trọng tâm là đủ để làm cho Robot trở nên không ổn định

Hình 1.1 Robot

Điều này có thể được khắc phục bằng cách hạ thấp trọng tâm của Robot để giảm

sự bất ổn định này Tuy nhiên điều này thường đi kèm với chi phí đáng kể Ngoài

ra, do sự mất ổn định của Robot, nên tốc độ mà các Robot có thể di chuyển thường

bị hạn chế vì quán tính của Robot sẽ làm cho nó bị lật up

Hình 1.2: Tbot tại IHMC

Tbot có thể biến đổi linh hoạt từ việc sử dụng 4 bánh để di chuyển khi cần

di chuyển ở địa hình có độ cao hạn chế sang 2 bánh để di chuyển ở địa hình hẹp

Dự án Tbot tại IHMC (Institute for Human & Machine Cognition) được Quân đội Hoa K (DARPA) tài trợ để thử nghiệm các loại robot lính khác nhau có thể thay thế binh lính của con người ở Iraq và Afghanistan

3

Đối với các Robot ba và bốn bánh, việc thăng bằng và ổn định của chúng là nhờ trọng tâm nằm trong mặt phẳng chân đế do các bánh Robot tạo ra Còn đối với Robot hai bánh thì việc thăng bằng đƣợc là khi trọng tâm của Robot nằm ngay giữa các bánh Robot Do đặc điểm trọng tâm nên Robot ba hoặc bốn bánh sẽ không ổn định dễ bị ngã nếu trọng lƣợng đặt nhiều vào bánh lái hoặc mất khả năng bám khi đặt nhiều vào bánh đuôi Nó có thể di chuyển tốt trên địa hình phẳng nhƣng không thể di chuyển lên xuống trên địa hình lồi lõm (mặt phẳng nghiêng): mất khả năng bám khi lên dốc và có thể lật úp khi xuống dốc

ra sự lật úp

Đối với những địa hình lồi lõm và những ứng dụng thực tế, sự thăng bằng của Robot hai bánh có thể sẽ mang lại nhiều ý nghĩa thực tiễn hơn

4

Địa hình phẳng Địa hình dốc

Hình 1.4:

1.2 Một số công trình nghiên cứu liên quan

Trên thế giới đã có nhiều nghiên cứu ứng dụng mô hình Robot con lắc ngược để điều khiển cân bằng cho Robot hai bánh Trong nghiên cứu [1], nhóm tác giả đã điều khiển Robot cân bằng dùng DSP trên căn bản con lắc ngược với trọng lượng gán ở hệ thống giả lập như con người Hệ điều khiển trạng thái tuyến tính có biến trạng thái là thông tin cảm biến từ gyros và encoder motor (Grasser al.2002)

Giải thuật điều khiển sử dụng phương pháp điều khiển đặt cực trên các biến trạng thái mô tả hệ thống

Hình 1.5: Công trình nghiên cứu ở Th ĩ [1]

Trong nghiên cứu [2], David P nderson đã thiết kế Nbot dùng các sensor quán tính và encoder motor để cân bằng Robot Ý tưởng điều khiển để cân bằng: các

Đáp ứng của hệ thống

Giải thuật sử dụng hai khối điều khiển PD: một khối PD điều khiển vị trí bánh Robot, một khối PD điều khiển góc nghiêng Robot Tổng hai khối điều khiển PD này d ng để điều khiển hệ cân bằng Robot Robot hai bánh

Hình 1.6: i) [2]

Trong nghiên cứu [3] đề cập tới Balibot, là một trong các mẫu đầu tiên về Robot hai bánh có trọng tâm phía trên các bánh Robot Khi hệ thống điều khiển không hoạt động, Robot sẽ bị ngã Khi Robot có nhận biết hướng mà nó sắp ngã, các bánh Robot sẽ di chuyển về phía ngã và thẳng góc với chính nó

6

Hệ thống sử dụng cảm biến góc nghiêng, gia tốc kế Motorola MM 260 vi điều khiển là PIC 16F876 PIC tích hợp một bộ biền đổi A/D nhiều kênh để đo cảm biến góc nghiêng Nguồn điện được đặt gần đỉnh và họat động như trọng lượng của con lắc ngược

Một phiên bản khác của Balibot sử dụng các cảm biến hồng ngọai để đo khoảng cách thay vì dùng các cảm biến đo góc

Hình 1.7: BaliBot trong nghiên ứ

Thuật toán điều khiển là LQR trong simulink và việc nối giữa máy tính và mô hình bằng card MF 624 và dùng Real Time Toolbox for matlab với động lực driver tương ứng được đề cập trong nghiên cứu [4] Dữ liệu cảm biến (encoder và cặp accelerometer được đọc bằng encoder built-in và các kênh analog Động cơ

DC được điều khiển bằng bộ biến đổi điều rộng xung (PWM) với điện tử công suất Thuật toán điều khiển là LQR

7

Hình 1.8: ng SimMechanics,k ạ trong

nghiên cứu [4]

Hình 1.9: Mô hình dùng Matlab Real Time Toolbox trong nghiên

cứu [4]

1.3 Mục tiêu đề tài

Để hệ Robot hoạt động ổn định ngoài phần cơ cấu hợp lý và phương trình toán

hệ thống sát với thực tế, Robot còn phải được thiết kế bộ điều khiển thích hợp Đối với hệ Robot hai bánh tự cân bằng thì quá trình điều khiển có thể được chia thành

ba phần: điều khiển góc nghiêng của hệ Robot so với phương thẳng đứng điều khiển vị trí di chuyển của Robot so với phương nằm ngang và cuối c ng là điều khiển góc xoay của Robot Bản thân hệ Robot là hệ không ổn định, nên quá trình

 Nghiên cứu lý thuyết điều khiển LQR

 Nghiên cứu lý thuyết điều khiển trượt

 Áp dụng phương pháp điều khiển LQR cho đối tượng Robot hai bánh

 Áp dụng phương pháp điều khiển trượt cho đối tượng Robot hai bánh

 Áp dụng phương pháp điều khiển LQR và điều khiển trượt để cho đối tượng Robot hai bánh cân bằng và bám theo quỹ đạo đặt

 Mô phỏng kết quả điều khiển đối tượng trên Matlab

 Kiểm chứng các kết quả mô phỏng bằng thực nghiệm điều khiển đối tượng thực bằng các bộ điều khiển

1.4 Phương pháp nghiên cứu

Phương pháp nghiên cứu được áp dụng để thực hiện luận văn này là phân tích lý thuyết, mô phỏng trên máy tính và áp dụng kết quả vào thực nghiệm

 Phân tích lý thuyết: là nghiên cứu các lý thuyết cơ sở liên quan đến việc thiết

kế bộ điều khiển LQR, bộ điều khiển trượt, dựa trên cơ sở các lý thuyết này

sẽ phân tích cho đối tượng cụ thể được sử dụng trong luận văn

 Mô phỏng trên máy tính: Sau khi đã xây dựng xong các thuật toán điều khiển dưới dạng lý thuyết, sẽ tiến hành mô phỏng trên Matlab để kiểm chứng sự hoạt động của bộ điều khiển, trên kết quả mô phỏng sẽ rút ra các nhận xét về thuật toán và kinh nghiệm khi áp dụng bộ điều khiển vào đối tượng thực

 Áp dụng kết quả vào thực nghiệm: Để kiểm chứng tính đúng đắn của các bộ điều khiển thiết kế bằng lý thuyết, sẽ nhúng thuật toán điều khiển xuống vi

9

xử lý và các khâu chấp hành để điều khiển đối tượng thực, kết quả thực tế giúp ta nhìn nhận chính xác cho chất lượng của bộ điều khiển đã được thiết

kế

1.5 Sơ lược nội dung luận văn

Luận văn có cấu trúc 5 chương với những nội dung sau:

 Chương 1: Tổng quan đề tài

Giới thiệu đề tài, một số nghiên cứu liên quan, mục tiêu của đề tài và phương pháp nghiên cứu

 Chương 2: Thiết kế và mô hình hóa h o ot hai bánh tự cân bằng

Phần này trình bày thiết kế phần cơ khí và phần điện của mô hình Robot thực

tế mà tác giả thực hiện Sau đó mô hình hóa mô hình đã thiết kế

 Chương 3: Thiết kế bộ điều khiển cho h Robot hai bánh tự cân bằng

Trình bày cơ sở lý thuyết các bộ điều khiển được sử dụng (LQR và Sliding mode) Trình bày phương pháp phân tích quỹ đạo để điều khiển hệ Robot hai bánh bám theo quỹ đạo đặt Sau đó áp dụng vào mô hình Robot với các thông số ở Chương 2

 Chương 4: Thiết kế bộ điều khiển thực nghi m

Trình bày phương pháp đo góc nghiêng của Robot bằng cảm biến với bộ lọc Kalman, cách xây dựng chương trình nhúng giải thuật cho vi điều khiển SMT32F4 bằng Matlab/Simulink với thư viện hỗ trợ Waijung Blockset

 Chương 5: Kết quả và hướng phát triển

Trình bày các kết quả mô phỏng và thực nghiệm đánh giá kết quả đạt được

và hướng phát triển của đề tài

10

CHƯƠNG 2 THIẾT KẾ VÀ MÔ HÌNH HÓA ROBOT HAI BÁNH TỰ CÂN BẰNG

2.1 Thiết kế Robot hai bánh tự cân bằng

2.1.1 Phần cơ khí

Hệ thống cơ khí Robot hai bánh được thiết kế bằng phần mềm Solid Work Từ

cơ sở kích thước của Robot hai bánh xây dựng trên Solid Work ta có thể có được thông số gần đúng về tọa độ trọng tâm và moment quán tính của xe

Hình 2.1: Thiết kế ơ í Robot hai bánh dùng phần m m SolidWorks

Hình 2.2: Mô hình Robot t cân bằng sau khi hoàn thành

11

Thiết kế cơ khí của hệ Robot hai bánh tự cân bằng nhằm đáp ứng mục tiêu nghiên cứu là giữ cân bằng cho robot ở trạng thái đứng yên hoặc di chuyển

Tác giả lựa chọn vật liệu chính cho mô hình là mica có độ dày 4mm và 8mm với

ƣu điểm dễ gia công

2.1.2 Phần mạch điện

Sơ đồ khối phần mạch điện của hệ thống:

Hình 2.3 ơ ồ kh i phần mạ n của Robot hai bánh t cân bằng

 Mạch điều khiển trung tâm sử dụng vi điều khiển STM32F4-DISCOVERY làm nhiệm vụ nhúng thuật toán điều khiển LQR và điều khiển trƣợt, thu thập tín hiệu

từ cảm biến gia tốc Accelerometer, cảm biến vận tốc góc Gyroscope, Encoder

Từ đó xuất xung PWM điều khiển động cơ DC Servo

Lipo Battery

DC Servo motor Encorder

HI216

Trong đề tài này MPU6050 được d ng để đo góc nghiêng so với phương thẳng đứng của Robot hai bánh tự cân bằng

 Mạch điều khiển công suất động cơ HI216:

Mạch d ng để điều khiển động cơ hai bánh xe của hệ Robot

13

Hình 2.7: Mạch công suấ u khi ơ I 16

Đặc tính kĩ thuật:

- Dòng liên tục 15 dòng đỉnh 20A công suất 600W, tại 25oC

- Điện áp công suất từ 12V đến 48V

- Điện áp kích từ 3V3 -> 5V

- Có cầu chì bảo vệ ngắn mạch

- Có Led báo nguồn (POW), tín hiệu xung đưa vào PWM và và tín hiệu đảo chiều (DIR)

- oard được thiết kế nhỏ gọn với kích thước 52x64x22mm

- Tần số hoạt động lên tới 100 Khz, sử dụng Opto HCPL-0631 cho tần số hoạt động cao

14

 Pin lipo 3s

Với điện áp trung bình của mỗi cell là 3.7V pin Lipo 3s có điện áp trung bình

là 11.1 V Khi sạc đầy điện áp của pin có thể lên đến 12.6V Ngoài ra, khả năng xả dòng lớn là ưu điểm lớn của pin Lipo, giúp đáp ứng dòng điện cần thiết để động cơ hoạt động khi chịu tải lớn

Hình 2.9: Pin lipo 3s

2.2 Mô hình hóa hệ Robot hai bánh tự cân bằng

2.2.1 ựng m h nh toán h c hệ Robot hai bánh

Hệ Robot hai bánh tự cân bằng dựa trên nguyên lý của hệ xe con lắc ngược, mô hình toán học đơn giản của hệ Robot hai bánh tự cân bằng có thể thu được nhờ phương pháp Euler – Lagrange [5]

Hình 2.10:

16

W Chiều rộng Robot

D Chiều sâu Robot

H Chiều cao Robot

L Khoảng cách từ trọng tâm Robot đến trục bánh Robot

w

J Moment quán tính của bánh Robot

J Moment quán tính của Robot quanh trục động cơ

J Moment quán tính của Robot quanh trục góc phi

 Góc của bánh trái phải

 Góc nghiêng của phần than Robot

 Góc xoay của Robot

17

Với hệ trục tọa độ như hình vẽ, sử dụng phương pháp Euler-Lagrange, chúng ta

có thể tìm được mô hình động lực học của Robot Giả sử tại thời điểm t = 0, Robot

di chuyển theo chiều trục x chúng ta có các phương trình sau:

m

x dt x

m l

l

m

W x x

m r

r

m

W x x

Để tiện dụng phương pháp PWM cho việc điều khiển động cơ nên ta sẽ chuyển đổi

từ dòng điện sang điện áp động cơ:

20

Với t

m

nK R

m

nK K

f R

 Trong khi đó phương trình 2.28 thể hiện mối quan hệ giữa góc xoay với điện áp hai động cơ dưới dạng hiệu số (vr - vl)

Vì vậy bài toán có thể được tách thành hai bài toán nhỏ hơn với hai tín hiệu điều

khiển là V ψ và V ϕ với:

J Moment quán tính của bánh Robot 1.57198*10^-5

J Moment quán tính của Robot quanh trục động cơ 4147432.77*10^-9

J Moment quán tính của Robot quanh trục góc phi 11742250.25*10^-9

K Moment xoắn của động cơ DC 0.064359009905374

B ng 2.2: B ng thông s h Robot hai bánh

2.2.2 Đánh giá m h nh toán hệ o ot h i ánh đ ựng

 Mô phỏng lần 1:

Trong trường hợp không có nhiễu và các trạng thái ban đầu bằng 0:

(Theta_0 Theta_dot_0 Psi_0 Psi_dot_0 Phi_0 Phi_dot_0) = (0 0 0 0 0 0)

22

Hình 2.12: Kết qu mô phỏng lần 1 mô hình toán xe hai bánh

Nhận x t đánh giá: Ở vị trí ban đầu = 0 khi không có tác động của ngoại lực, đáp ứng hệ thống vẫn giữ nguyên vị trí cũ Như vậy (0 0 0 0 0 0) là một vị trí cân

bằng của hệ robot xe hai bánh

 Mô phỏng lần 2:

Trong trường hợp có nhiễu với công suất 10-6, các trạng thái ban đầu khác không:(Theta_0 Theta_dot_0 Psi_0 Psi_dot_0 Phi_0 Phi_dot_0) = (2 0.5 0.05 0 pi/4 pi/8) Và không có tác động từ bộ điều khiển LQR:

Hình 2.13: Kết qu mô phỏng lần 2 mô hình toán xe hai bánh

23

Nhận x t đánh giá: Khi không có tác động của bộ điều khiển, các góc theta và

Phi lệch khỏi vị trí cân bằng và mất ổn định Riêng góc Psi tiến đến vị trí cân bằng mới (=3.14 – trục xe bị chúi xuống dưới)

Kết lu n: Đã mô hình hóa được đặc tính động học của hệ Robot hai bánh

24

CHƯƠNG 3 THIẾT KẾ BỘ ĐIỀU KHIỂN CHO HỆ ROBOT H I NH TỰ CÂN BẰNG

3.1 Bộ điều khiển c n ằng cho hệ Robot h i ánh

25

3.1.1.2 Thiết kế bộ điều khiển LQR cho h Robot hai nh tự cân bằng

Từ các phương trình động lực học của hệ Robot hai bánh tự cân bằng, tuyến tính

hóa phương trình chuyển động của hệ Robot tại điểm làm việc, góc nghiên ψ  0,

ψ  ψ, ψ  1 và bỏ qua ảnh hưởng của các thành phần bậc hai trong

phương trình Ta được hệ phương trình tuyến tính hóa sau:

Khảo sát tính điều khiển đƣợc và tính quan sát đƣợc của hệ thống, ta có:

Ma trận điều khiển: Mc = (B, AB, A2B, A3B)

27

Chọn Q R cho phương trình trạng thái thứ nhất để xác định giá trị K hồi tiếp cho khâu điều khiển giữ cân bằng cho Robot, và giá trị K hồi tiếp cho khâu điều khiển góc xoay Robot Các trọng số ma trận Q, R có thể được chọn dựa vào quá trình thử sai để tìm ra kết quả đáp ứng tốt nhất:

3.1.1.3 Chương trình Matla /Simulink mô phỏng bộ điều khiển LQR

Chương trình mô phỏng bộ điều khiển LQR được xây dựng bằng phần mềm Matlab/Simulink theo sơ đồ dưới đây:

Hình 3.2: ơ ồ u khi n tổng quát LQR

Khối decoupling để phân bổ tín hiệu điều khiển đến các bánh Robot:

Nhận x t đánh giá: Trong trường hợp có sự tác động từ bộ điều khiển LQR, các

ngõ ra của hệ thống nhanh chóng trở về vị trí cân bằng sau 5(s)