Nghiên cứu các luật điều khiển hiện đại trên mô hình robot cân bằng

0
ĐẠI HỌC QUỐC GIA HÀ NỘI
TRƢỜNG ĐẠI HỌC CÔNG NGHỆ

PHẠM THANH VŨ

NGHIÊN CỨU CÁC LUẬT ĐIỀU KHIỂN HIỆN ĐẠI TRÊN MÔ HÌNH
ROBOT CÂN BẰNG

LUẬN VĂN THẠC SĨ CÔNG NGHỆ ĐIỆN TỬ - VIỄN THÔNG

Hà Nội – 2015


1
ĐẠI HỌC QUỐC GIA HÀ NỘI
TRƢỜNG ĐẠI HỌC CÔNG NGHỆ

PHẠM THANH VŨ

NGHIÊN CỨU CÁC LUẬT ĐIỀU KHIỂN HIỆN ĐẠI TRÊN MÔ HÌNH
ROBOT CÂN BẰNG

Ngành: Công nghệ Điện tử — Viễn thông
Chuyên ngành: Kỹ thuật điện tử
Mã số: 60 52 02 03

LUẬN VĂN THẠC SĨ CÔNG NGHỆ ĐIỆN TỬ - VIỄN THÔNG

NGƢỜI HƢỚNG DẪN KHOA HỌC: PGS.TS. TRẦN QUANG VINH

Hà Nội – 2015


2
LỜI CAM ĐOAN
Tôi cam đoan các kết quả nghiên cứu đưa ra trong luận án này dựa trên các
kết quả thu đươ ̣c trong quá trình nghiên cứu của riêng tôi, không sao chép bất kỳ
kết quả nghiên cứu nào của các tác giả khác. Nội dung của luận án có tham khảo
và sử dụng một số thông tin, tài liệu từ các nguồn sách, tạp chí, bài báo khoa học
đươ ̣c liệt kê trong danh mục các tài liệu tham khảo.

Hà nội, ngày tháng 12 năm 2015
Tác giả luận văn

Phạm Thanh Vũ


3
Mục Lục :
LỜI CAM ĐOAN ................................................................................................ 2
DANH MỤC KÍ HIỆU VÀ CHỮ VIẾT TẮT .................................................. 5
DANH MỤC CÁC HÌNH VẼ ............................................................................. 6
CHƢƠNG I. TỔNG QUAN VỀ ROBOT TỰ CÂN BẰNG …………………8
1.1 Giới thiệu chung ........................................................................................... 8
1.1.1 Bài toán cân bằng con lắc ngược ............................................................ 8
1.1.2 Robot hai bánh xe vi sai tự cân bằng.................................................... 9
1.2 Cơ sở khoa học thực tiễn và khả năng ứng dụng: ...................................... 10
1.3. Mục tiêu của Luận văn .............................................................................. 12
CHƢƠNG II . CƠ SỞ TÍNH TOÁN LÝ THUYẾT……………………….. 13
2.1. Robot cân bằng với mô hình điều khiển sai lệch tự động ........................ 13

2.2. Động lực học robot cân bằng dựa trên mô hình con lắc ngược ............... 14
2.2.1 Mô hình con lắc ngược ......................................................................... 14
2.2.2 Phân tích cơ học ................................................................................... 14
2.2.3. Xây dựng hàm truyền : ........................................................................ 16
2.2.4. Phương trình trạng thái : ...................................................................... 17
2.3. Điều khiển robot cân bằng theo luật PID ................................................. 17
2.3.1 Giới thiệu ............................................................................................. 17
2.3.2. Phương trình vi phân ........................................................................... 18
2.3.3. Hàm truyền : ...................................................................................... 18
2.3.4. Đặc tính quá độ................................................................................... 19
2.3.5. Điều chỉnh vòng lặp PID .................................................................... 19
2.4. Điều khiển toàn phương tuyến tính LQR .................................................. 21
2.5. Các nhân tố bất định trong đo điều khiển và biện pháp khắc phục .......... 22
2.5.1. Sai số đo từ các biến ........................................................................... 22
2.5.2. Nâng cao độ tin cậy phép đo bằng giải thuật lọc Kalman .................. 22
CHƢƠNG III MÔ PHỎNG HỆ THỐNG ROBOT CÂN BẰNG................. 24
3.1 Mô phỏng hệ thống robot ........................................................................... 24
3.3 Mô phỏng hệ thống vòng kín với phương pháp điều khiển PID : ............. 26
3.3.1 Đáp ứng xung của góc nghiêng robot ................................................. 26
3.3.2.Đáp ứng xung của vị trí robot............................................................... 28
3.4 Mô phỏng hệ thống với phương pháp điều khiển LQR ............................ 29
3.5 Mô phỏng mô hình robot phi tuyến trên Matlab-Simulink ........................ 33
3.5.1. Xây dựng mô hình hệ thống ................................................................ 33
3.5.2. Mô phỏng điều khiển cân bằng theo luật PID trong Simulink : ......... 35
CHƢƠNG IV THIẾT KẾ CHẾ TẠO VÀ THỬ NGHIỆM MÔ HÌNH. ..... 36
4.1 Xây dựng khung và thiết kế cơ khí : .......................................................... 36
4.1.1 Hệ thống khung đỡ trên NX7 ............................................................... 36
4.1.2 Hệ thống bánh xe :................................................................................ 37

4
4.2 Xây dựng mạch điện điều khiển :............................................................... 38
4.2.1 Chip xử lý trung tâm DSPIC3050 ....................................................... 41
4.2.2 Module cảm biến MPU6050 ............................................................... 45
4.2.3 IC điều khiển động cơ L298 ................................................................ 46
4.2.4 Động cơ DC24v ................................................................................... 48
4.2.5 Bộ hiển thị LCD 16*2 ......................................................................... 50
4.2.6 Mạch ổn nguồn LM7805 , LM2576. .................................................... 52
4.2.7 Modul giao tiếp I2C.............................................................................. 53
4.2.8 Cảm biến vị trí Encoder........................................................................ 54
4.2.9 Thi công lắp ráp mạch điện thực tế : .................................................... 56
4.2.9.1 Mạch điều khiển động cơ .............................................................. 56
4.2.9.2 Mạch xử lý trung tâm .................................................................... 57
4.3 Lập trình và nguyên lý điều khiển ............................................................. 58
KẾT LUẬN ........................................................................................................ 60
CODE LậP TRÌNH VI ĐIềU KHIểN :............................................................ 61
TÀI LIỆU THAM KHẢO ................................................................................ 78


5
DANH MỤC KÍ HIỆU VÀ CHỮ VIẾT TẮT
Tên viết
Tắt

Tên tiếng Anh

Tên tiếng việt

PID

Proportional Intergral
Derivatice

Là cơ chế điều khiển ( tỉ lệ, tích
phân, đạo hàm )

LQR

Linear quadratic regulator

Điều khiển tối ưu toàn phương

LCD

Liquid Crystal display

Màn hình hiện thị tinh thể lỏng

I2C

Inter-Intergrated Circuit

Chuẩn Bus giao tiếp nối tiếp

UART

Universal Asinchonus
Receiver Transmitter

Truyền thông nối tiếp không đồng

bộ

ADC

Analog to Digital converter

Bộ chuyển đổi tương tự sang số

SDA

Serial Data

Đường truyền dữ liệu 2 hướng

SCL

Serial Clock

Đường truyền xung đồng bộ 1
hướng

ACK

Acknowledgment

Báo nhận

SPI

Serial Peripheral Interface

Giao tiếp ngoại vi nối tiếp

DC

Direct Current

Đòng điện 1 chiều

LED

Light Emitting Diode

Đèn diode phát quang

PIC

Personal Integrated Circuit

Mạch tích hợp khả trình

ARM

Advanced RISC Machine

Cấu trúc vi sử lý

RAM

Random Access Memory

Bộ nhớ truy cập ngẫu nhiên

Electrically Erasable
EEPROM Programmable Read-Only
Memory

Chip nhớ không xoá được


6
DANH MỤC CÁC HÌNH VẼ
Hình 1.1 Mô hình con lắc ngược
Hình 1.2 Robot hai bánh cân bằng và mô tả sự tự cân bằng của robot
Hình 1.3 Mô hình robot bot I
Hình 1.4 n-Bot do David P. Anderson sáng chế
Hình 2.1 Cấu trúc cơ bản của hệ thống điều khiển tự động.
Hình 2.2 Mô hình xe và con lắc ngược
Hình 2.3 Mô hình và sơ đồ khổi tín hiệu ngõ vào ra của con lắc ngược
Hình 2.4 Phân tích lực tác dụng lên xe và con lắc ngược
Hình 2.5 Sơ đồ khối bộ điều khiển PID liên tục
Hình 2.6 Đáp ứng quá độ hệ thống
Hình 2.7 Cấu trúc điều khiển phản hồi trạng thái LQR
Hình 2.8 Lưu đồ thuật giải của bộ lọc Kalman
Hình 2.9 Mô phỏng lọc kalman
Hình 3.1 Phân tích lực tác dụng lên hệ robot
Hình 3.2 Mô phỏng matlab sự biến đổi của x và Phi khi chưa điều khiển
Hình 3.3 Sơ đồ điều khiển con lắc (a) và được vẽ lại (b).
Hình 3.4 Đáp ứng xung góc nghiêng f của robot
Hình 3.5 Sơ đồ điều khiển robot.

Hình 3.6 Đáp ứng xung vị trí x của robot
Hình 3.7 Sơ đồ điều khiển phản hồi toàn trạng thái trong phương pháp LQR
Hình 3.8 Đáp ứng xung trạng thái robot được điều khiển theo LQR với
Q(1,1)= Q(3,3)=1.
Hình 3.9 Đáp ứng xung trạng thái robot được điều khiển theo LQR với
Q(1,1)=5000; Q(3,3)=100.
Hình 3.10 Mô hình hệ thống trong Simulink.
Hình 3.11Cấu trúc mô hình hệ thống robot cân bằng trong Simulink
Hình 3.12 Sơ đồ mô phỏng PID với simulink
Hình 3.13 Kết quả mô phỏng robot cân bằng với Kp=100, Kd=20, Ki=1
Hình 4.1 Mô hình cơ khí robot cân bằng 2 bánh
Hình 4.2 Hệ thống cơ cấu bánh xe giảm tốc
Hình 4.3 Sơ đồ mạch kết nối robot
Hình 4.4 Mạch xử lý trung tâm DsPic30F4013
Hình 4.5 Mạch điều khiển động cơ L298
Hình 4.6 Sơ đồ chân của Pic30F4013
Hình 4.7 Sơ đồ khối DSPIC30F4013
Hình 4.8 Sơ đồ cấu chúc của khối I2C
Hình 4.9 module cảm biến MPU6050
Hình 4.10 Sơ đồ khối hoạt động của L298


7
Hình 4.11 Bảng trạng thái hoạt động của L298
Hình 4.12 Sơ đồ chân của L298
Hình 4.13 Cấu trúc của động cơ DC-24v
Hình 4.14 Đặc tính của động cơ
Hình 4.15 LCD 16*2
Hình 4.16 Sơ đồ chân của LCD 16*2
Hình 4.17 IC ổn áp LM7805

Hình 4.18 IC ổn áp LM2576
Hình 4.19 Giao tiếp I2C
Hình 4.20 Hoạt động của bộ I2C
Hình 4.21 Mạch đếm xung encoder
Hình 4.22 Mạch điện encoder
Hình 4.23 Tín hiệu cảm biến encoder
Hình 4.21 Mạch điều khiển động cơ
Hình 4.22 Mạch xử lý trung tâm
Hình 4.23 Lưu đồ xử lý Chip PIC3050


8
CHƢƠNG I.

TỔNG QUAN VỀ ROBOT TỰ CÂN BẰNG
1.1 Giới thiệu chung
Những robot di chuyển bằng bánh xe hiện nay hầu hết là robot ba bánh hoặc
bốn bánh. Các loại này thường có hai bánh chủ lực được lắp ráp đồng trục tạo
lực đẩy cho robot và một bánh lái (robot ba bánh) hoặc hai bánh lái (robot bốn
bánh) có tác dụng lái hướng cho robot. Bánh chủ lực thường đặt phía sau còn
bánh lái đặt phía trước.
Việc thiết kế ba bánh hay bốn bánh làm cho robot thăng bằng ổn định nhờ
trọng lực của nó luôn nằm trong mặt chân đế đi qua các bánh xe. Nếu trọng
lượng được đặt nhiều vào bánh lái thì robot khó di chuyển do ma sát của bánh
chủ động với mặt đường kém hơn. Nếu trọng lượng được đặt nhiều vào bánh
chủ động thì robot khó lái hướng do ma sát của bánh lái so với mặt đường kém.
Nhiều thiết kế robot có thể di chuyển tốt trên địa hình phẳng, nhưng không thể
di chuyển trên địa hình lồi lõm hay mặt phẳng nghiêng. Khi di chuyển lên dốc
trọng lực dồn vào bánh chủ lực, khi xuống dốc trọng lực dồn vào bánh lái đều
dẫn đến robot mất ổn định.

Trái lại, với dạng robot hai bánh đồng trục lại rất linh động khi di chuyển trên
địa hình phức tạp, mặc dù bản thân là một hệ thống không ổn định. Do trọng lực
của robot luôn được giữ trên trục của hai bánh nên trên mọi địa hình trọng lực
của robot không thay đổi.
1.1.1 Bài toán cân bằng con lắc ngƣợc
Do mục tiêu của luận văn là sẽ nghiên cứu một mô hình robot cân bằng dựa
trên nguyên lý chuyển động của con lắc ngược nên trước hết ta xem xét con lắc
ngược là gì. Thiết kế con lắc ngược là một bài toán kinh điển trong động lực học
và lý thuyết điều khiển, được sử dụng rộng rãi trong việc kiểm tra thử nghiệm
các luật điều khiển, các phương pháp điểu khiển hiện đại. Một con lắc ngược là
con lắc có trọng tâm nằm ở phía trên, và con lắc dao động quanh một trục thẳng
đứng như một trong các mô hình chỉ ra trên hình 1.1.


9

Hình 1.1 Mô hình con lắc ngược
Hệ thống bao gồm một cần con lắc được gắn linh động với một xe đẩy chuyển
động tiến lui trên mặt phẳng ngang cho phép giữ cân bằng cho cần lắc luôn giữ
ở phương trọng trường quả đất thẳng đứng . Với một con lắc bình thường nó sẽ
ổn định khi ta treo xuống , nhưng với con lắc ngược nó phải chủ động tự cân
bằng để giữ thẳng đứng. Việc này có thể thực hiện được được bằng cách áp
dụng một momen xoắn tại vị trí khớp quay giữa cần con lắc và xe. Qua thông tin
phản hồi về góc lệch  của con lắc, hệ thống có nhiệm vụ điều khiển di chuyển
xe trên bề mặt ngang bởi lực F, tạo ra momen xoắn giúp con lắc quay trở về
quanh vị trí cân bằng.
Robot cân bằng hai bánh dựa trên nguyên lý này, trong đó nó di chuyển 2 bánh
xe đặt song song đồng trục để tạo ra momen xoắn giữ khung xe cân bằng.
1.1.2 Robot hai bánh xe vi sai tự cân bằng
Robot cân bằng hai bánh xe là robot có thể tự cân bằng được và có thể chuyển

động theo các hướng khi điều chỉnh vận tốc quay của hai bánh robot khác nhau.
Vì vậy nó còn được gọi là robot tự cân bằng hai bánh vi sai. Nguyên tắc để giữ
cân bằng này là robot sẽ tự điều khiển sao cho đảm bảo góc nghiêng giữa trục
robot và phương trọng trường quả đất luôn bằng 0 hoặc rất nhỏ. Hình 1.2 là một
ví dụ hình ảnh một robot cân bằng hai bánh xe trong thực tế và mô tả sự tự cân
bằng của nó.
Việc giữ cân bằng trên robot hai bánh là khó vì trọng tâm của robot rất nhanh
bị lệch khỏi trục của robot, làm robot có xung hướng đổ xuống.


10

Hình 1.2 Robot hai bánh cân bằng và mô tả sự tự cân bằng của robot
Ví dụ, khi robot có xu hướng đổ về phía trước, bánh xe sẽ được điều khiển để
xe di chuyển về phía trước lấy lại cân bằng.
1.2 Cơ sở khoa học thực tiễn và khả năng ứng dụng:
Cơ sở khoa học của đề tài này là áp dụng nguyên lý cân bằng con lắc ngược,
các luật điều khiển phản hồi như PID hay LQR để xây dựng mô hình xe robot
chuyển động trên hai bánh đồng trục dựa theo định luật Newton và cơ học vật
rắn. Nó được phát triển sử dụng ở nhưng nơi địa hình khó khăn, diên tích nhỏ
hẹp, nơi mà những chiếc xe 4 bánh cồng kềnh không thể hoạt động được, hay
những chiếc xe 3 bánh không thể giữ cân bằng. Khắc phục các nhược điểm đó,
mục tiêu của đề tài là nghiên cứu và thiết kế chế tạo 1 mô hình xe tự cân bằng
hai bánh dựa trên lý thuyết cân bằng con lắc ngược, gọn nhẹ và có thể di chuyển
trên mọi địa hình.
Trong những ứng dụng thực tế, sự cân bằng của robot hai bánh có thể sẽ
mang lại nhiều ý nghĩa thực tiễn trong giới hạn ổn định hơn là đối với robot ba,
bốn bánh truyền thống. Cùng với sự phát triển mạnh mẽ của các hệ thống tự
động hoá, robot hai bánh tự cân bằng ngày càng hoàn thiện và cho thấy lợi ích
của nó trong công nghiệp và sinh hoạt.Vì vậy, việc chế tạo thành công robot

trong đề tài này sẽ mở ra một hướng tiếp cận mới và góp phần thúc đẩy việc ứng
dụng của robot ngày càng nhiều vào trong đời sống hằng ngày và trong nghiên
cứu chế tạo.
Robot sử dụng các cảm biến để đo góc nghiêng, vận tốc quay của robot quanh
trục bánh và vận tốc di chuyển của robot so với mặt đất. Nhờ vậy, robot sẽ tự
giữ cân bằng và di chuyển được. Với cấu trúc này, trọng tâm của robot phải luôn
dao động quanh vùng đỡ của bánh xe để có thể thăng bằng và di chuyển.
Trên thế giới và trong nước đã có các nhóm nghiên cứu cho ra đời một số sản
phẩm liên quan như sau.


11

Hình 1.3 Mô hình robot bot I
Balance-bot I (của Hàn Quốc) là một robot hai bánh tự cân bằng bằng cách kiểm
soát thông tin phản hồi. Hệ thống cao 50cm. Khung chính được làm bằng nhôm.
Nó có hai trục bánh xe nối với hộp giảm tốc và động cơ một chiều cho sự phát
động. Tổng cộng có ba bộ vi xử lý Atmel được sử dụng. Vi điều khiển chính thi
hành những nguyên lý kiểm soát và thuật toán ước lượng. Một vi điều khiển
khác kiểm soát tất cả cảm biến analog. Vi điều khiển thứ ba điều khiển động cơ
một chiều.
Mạch điều khiển nhận bốn giá trị khác nhau: góc nghiêng, vận tốc góc nghiêng,
góc quay bánh xe và vận tốc góc quay, sau đó nó tạo lệnh cho động cơ một
chiều để điều chỉnh tốc độ bánh xe.

Hình 1.4 n-Bot do David P. Anderson sáng chế
Robot n-Bot (hình 1.4) do Anderson chế tạo có ý tưởng để robot cân bằng
như sau: các bánh xe sẽ phải chạy xe theo hướng mà phần trên robot sắp ngã.
Nếu bánh xe có thể được lái theo cách đứng vững theo trọng tâm robot, robot sẽ
vẫn được giữ cân bằng. Trong thực tế, điều này đòi hỏi hai cảm biến thông tin

12
phản hồi: cảm biến góc nghiêng để đo góc nghiêng của robot với trọng lực, và
encoder trên bánh xe để đo vị trí dịch chuyển của tâm robot. Bốn thông số ngõ
vào để xác định hoạt động và vị trí của xe con lắc ngược cân bằng là: góc
nghiêng, vận tốc góc, vị trí bánh xe, vận tốc bánh xe. Bốn giá trị đo lường được
cộng lại và phản hồi tới điện áp động cơ, tương ứng với momen quay, cân bằng
và bộ phận lái robot.
Hiện nay cũng đã có một số sản phẩm thương mại như của hang Segway như
[segway-i2 , segway-x2-2] cho phép chở được người hay các mô hình robot cân
bằng có thể cho người dùng tự lắp ráp được trên các kit trò chơi của Lego. Tuy
nhiên giống như một số sản phẩm được nghiên cứu chưa hẳn hoàn thiện ở trong
nước, các sản phẩm này hầu hết đều có các kết cấu phần cứng và đặc biệt là
phần mềm chương trình điều khiển được khép kín.
1.3. Mục tiêu của Luận văn
Căn cứ vào tình hình trên, Luận văn có mục tiêu nghiên cứu những vấn đề liên
quan trong thiết kế, chế tạo một mô hình robot cân bằng hai bánh. Đó là việc
khảo sát về lý thuyết làm cơ sở cho việc tính toán điều khiển một mô hình robot
cân bằng với các luật điều khiển hiện đại như PID và LQR. Từ đó tiến hành mô
phỏng, thiết kế chế tạo và thực nghiệm một mô hình robot cân bằng trong thực
tế.


13
CHƢƠNG II .

CƠ SỞ TÍNH TOÁN LÝ THUYẾT
2.1. Robot cân bằng với mô hình điều khiển sai lệch tự động
Điều khiển tự động là quá trình thu thập, xử lý thông tin và tác động lên hệ

thống một cách tự động, không cần can thiệp của con người, để đáp ứng hệ
thống thỏa mãn yêu cầu đặt ra.
Nhìn chung, hệ thống điều khiển tự động gồm ba phần: bộ điều khiển
(Controller), đối tượng điều khiển (Plant hay System Model) và thiết bị đo, cảm
biến (sensor) như chỉ ra trên sơ đồ hình 2.1.

Hình 2.1 Cấu trúc cơ bản của hệ thống điều khiển tự động.
Trong đó : r(t) : tín hiệu đặt .
e(t) : tín hiệu sai lệch
u(t) : tín hiệu điều khiển vào
z(t): can nhiễu
y(t) : tín hiệu ra
yht(t) : tín hiệu hồi tiếp
Áp dụng cho trường hợp điều khiển robot cân bằng của luận văn, có thể dẫn giải
chi tiết như sau:
1. Đối tượng điều khiển được mô tả bởi một hệ phương trình vi phân trong đó:
- u(t) là tín hiệu đầu vào, ở đây là vận tốc hay lực tác động lên các bánh xe.
- y(t) là tín hiệu ra, ở đây là góc nghiêng hay vị trí hiện tại của robot
- z(t) là can nhiễu tác động ảnh hưởng đến sự cân bằng của robot.
2. Tín hiệu ra theo thời gian thực được đo bởi thiết bị đo (cảm biến). Lối ra y(ht)
thường bao gồm giá trị thực cộng với can nhiễu đo. Giá trị này được so sánh với
giá trị đặt (trong trường hợp này r(t) = 0) cho ra giá trị sai lệch e(t).


14
3. Giá trị sai lệch e(t) được đặt tới lối vào bộ điều khiển. Lối ra bộ điều khiển
chính là tín hiệu điều khiển vào u(t) cho đối tượng hệ thống cần điều khiển. Bộ
điều khiển có thể được thực hiện theo các luật khác nhau như PID hay LQR
được mô tả bởi các phương trình hay thông số điều khiển nhằm đạt được các
tiêu chuẩn cần điều khiển mong muốn.

Xây dựng mô hình toán học của robot dựa trên việc xây dựng các mô hình hệ
thống, mô hình điều khiển và liên kết chúng trong một sơ đồ điều khiển tổng
thể.
2.2. Động lực học robot cân bằng dựa trên mô hình con lắc ngƣợc
2.2.1 Mô hình con lắc ngƣợc
Một trong những mô hình con lắc ngược như hình 2.2 là một con lắc được gắn
với xe qua một bản lề (khớp) như hình vẽ :

Hình 2.2 Mô hình xe và con lắc ngược
Với các tham số :
M
m
b
L
I
F
x

:
:
:
:
:
:
:

Khối lượng xe (kg).
Khối lượng con lắc (kg).
Ma sát của xe (N).
Chiều dài ½ con lắc (m).

Mô men quán tính của con lắc (Nm).
Lực tác động vào xe (N).
Vị trí của xe (m).

2.2.2 Phân tích cơ học
Với mô hình con lắc ngược chúng ta sẽ nghiên cứu tác động lực lên xe và các
yếu tố đầu ra là sự dịch chuyển x và góc lệch ∅ của con lắc .


15

Hình 2.3 Mô hình và sơ đồ khổi tín hiệu ngõ vào ra của con lắc ngược
Phân tích lực trên xe và trên con lắc ngược ta có :

Hình 2.4 Phân tích lực tác dụng lên xe và con lắc ngược
Tổng lực tác dụng lên xe theo phương ngang là :
𝑀𝑥 + 𝑏𝑥 + 𝑁 = 𝐹

(2.1)

Tổng lực tác dụng lên con lắc theo phương ngang là :
𝑚𝑥 − 𝑚 (𝐿𝑠𝑖𝑛∅) = 𝑁

(2.2)

= > 𝑚𝑥 − 𝑚𝐿𝜃𝑐𝑜𝑠∅ + 𝑚𝐿∅2 𝑠𝑖𝑛∅ = 𝑁
Thay N vào công thức 2.1 ta được
𝑀 + 𝑚 𝑥 + 𝑏𝑥 − 𝑚𝐿∅ 𝑐𝑜𝑠∅ + 𝑚𝐿∅2 𝑠𝑖𝑛∅ = 𝐹

(2.3)

Xét tổng lực tác dụng lên con lắc theo phương vuông góc với con lắc
Ta có :
𝑃 𝑠𝑖𝑛∅ + 𝑁𝑐𝑜𝑠∅ − 𝑚𝑔𝑠𝑖𝑛∅ = −𝑚𝐿∅ + 𝑚𝑥 𝑐𝑜𝑠∅

(2.4)

Tổng momen tại khối tâm con lắc :
𝑃𝐿 𝑠𝑖𝑛∅ + 𝑁𝐿𝑐𝑜𝑠∅ = 𝐼∅

(2.5)

Nhân L vào phương trình 2.4 sau đó kết hợp với phương trình 2.5 ta được :
𝐼 + 𝑚𝐿2 ∅ − 𝑚𝑔𝐿𝑠𝑖𝑛∅ = 𝑚𝑥 𝐿𝑐𝑜𝑠∅
Kết hợp 2.3 và 2. 6 ta được :

(2.6)


16
𝑀 + 𝑚 𝑥 + 𝑏𝑥 − 𝑚𝐿∅ 𝑐𝑜𝑠∅ + 𝑚𝐿∅2 𝑠𝑖𝑛𝜃 = 𝐹
𝐼 + 𝑚𝐿2 ∅ − 𝑚𝑔𝐿𝑠𝑖𝑛∅ = 𝑚𝑥 𝐿𝑐𝑜𝑠∅

(2.7)

Vì góc ∅ rất nhỏ ~ 0 ±10° ta có: ( sin ∅ ~ ∅, cos ∅ = 1 , ∅2 = 0 ) ;
Nên hệ tương đương :
𝑀 + 𝑚 𝑥 + 𝑏𝑥 − 𝑚𝐿∅ = 𝐹
𝐼 + 𝑚𝐿2 ∅ − 𝑚𝑔𝐿∅ = 𝑚𝑥 𝐿

(2.8)

2.2.3. Xây dựng hàm truyền :
Sử dụng phương pháp Laplace cho hai phương trình trên, với u là tín hiệu vào
thay cho lực F ta có
Hàm truyền ;
𝐼 + 𝑚𝑙 2 𝛷 𝑠 𝑠 2 − 𝑚𝑔𝑙𝛷 𝑠 = 𝑚𝑙𝑋 𝑠 𝑠 2

( 2.9 )

𝑀 + 𝑚 𝑋 𝑠 𝑠 2 + 𝑏𝑋 𝑠 𝑆 − 𝑚𝑙𝛷 𝑠 𝑠 2 = 𝑈(𝑠)

(2.10)

=>
𝑋 𝑠 =
𝑀+𝑚

𝐼+𝑚 𝑙 2
𝑚𝑙

−

𝑔
𝑠2

𝐼 + 𝑚𝑙2
𝑔
− 2 𝛷 𝑠
𝑚𝑙

𝑠

𝛷 𝑠 𝑠2 + 𝑏

𝐼+𝑚 𝑙 2
𝑚𝑙

−

𝑔
𝑠2

𝛷 𝑠 𝑠 − 𝑚𝑙𝛷 𝑠 𝑠 2 = 𝑈(𝑠)

(2.11)
(2.12)

Do chỉ quan tâm đến biến thiên biên độ góc nên ta xây được hàm truyền mối
tương quan giữa tín hiệu lối vào u(s) và tín hiệu lối ra ∅(𝑠):
𝛷 𝑠
=
𝑈(𝑠)

𝑚𝑙 2
𝑠
𝑞
𝑏(𝐼 + 𝑚𝑚2 ) 3
𝑀 + 𝑚 𝑚𝑔𝑙 2 𝑏𝑚𝑔𝑙
𝑠4 +
𝑠 −

𝑠 −
𝑠
𝑞
𝑞
𝑞

(2.13)

Với: 𝑞 = 𝑀 + 𝑚 𝐼 + 𝑚𝑙2 − (𝑚𝑙)2 = 𝐼 𝑀 + 𝑚 + 𝑀𝑚𝑙2
= > Hàm truyền cho con lắc Ppend (góc ) và cho xe Pcart (vị trí x) là :
𝑃𝑝𝑒𝑛𝑑

𝛷 𝑠
𝑠 =
=
𝑈(𝑠)

𝑚𝑙 2
𝑠
𝑞
𝑏(𝐼 + 𝑚𝑙2 ) 3
𝑀 + 𝑚 𝑚𝑔𝑙 2 𝑏𝑚𝑔𝑙
𝑠4 +
𝑠 −
𝑠 −
𝑠
𝑞
𝑞
𝑞

(2.14)


17

𝑃𝑐𝑎𝑟𝑡

𝐼 + 𝑚𝑙 𝑠 𝑠 2 − 𝑚𝑔𝑙
𝑋(𝑠)
𝑞
𝑠 =
=
2
𝑏(𝐼 + 𝑚𝑙 ) 3
𝑀 + 𝑚 𝑚𝑔𝑙 2 𝑏𝑚𝑔𝑙
𝑈(𝑠)
𝑠4 +
𝑠 −
𝑠 −
𝑠
𝑞
𝑞
𝑞

(2.15)

2.2.4. Phƣơng trình trạng thái :
0
𝑥
𝑥

=
∅
∅

1
−𝑏 𝐼 + 𝑚𝑙 2
0
𝐼 𝑀 + 𝑚 + 𝑀𝑚𝑙 2
0
0
−𝑏𝑚𝑙
0
𝐼 𝑀 + 𝑚 + 𝑀𝑚𝑙 2

0
𝐼 + 𝑚𝑚2
𝐼 𝑀 + 𝑚 + 𝑀𝑚𝑙 2
+
0
𝑚𝑙
𝐼 𝑀 + 𝑚 + 𝑀𝑚𝑙 2

0
𝑔𝑚2 𝑙 2
𝐼 𝑀 + 𝑚 + 𝑀𝑚𝑙 2
0
𝑀 + 𝑚 𝑚𝑔𝑙
𝐼 𝑀 + 𝑚 + 𝑀𝑚𝑙 2

0

0
1
0

(2.16 )
𝑥
𝑥
∅
∅

𝑢

2.3. Điều khiển robot cân bằng theo luật PID
2.3.1 Giới thiệu
Trên thực tế có rất nhiều phương pháp để điều khiển các cơ cấu chấp hành, trong
đó thuật toán PID tỏ ra khá hiệu quả và thường được sử dụng trong điều khiển
tự động. Nó là một thuật toán điều khiển dễ và khá đơn giản, dễ chế tạo, dễ áp
dụng.
Một bộ điều khiển PID tính toán một giá trị "sai lệch" là hiệu số giữa giá trị
đo thông số biến đổi và giá trị đặt mong muốn. Bộ điều khiển sẽ thực hiện giảm
tối đa sai lệch bằng cách điều chỉnh giá trị điều khiển đầu vào. Tuy nhiên, để đạt
được kết quả tốt nhất, các thông số PID sử dụng trong tính toán phải điều
chỉnh theo tính chất của hệ thống - trong khi kiểu điều khiển là giống nhau, các
thông số phải phụ thuộc vào đặc thù của hệ thống.
Bộ điều khiển PID bao gồm ba thông số riêng biệt, là các giá trị tỉ lệ, tích
phân và đạo hàm, viết tắt là KP, KI, và KD. Giá trị tỉ lệ xác định tác động của sai
lệch hiện tại, giá trị tích phân xác định tác động của tổng các sai lệch quá khứ,
và giá trị vi phân xác định tác động của tốc độ biến đổi sai lệch. Tổng của ba tác
động này dùng để điều chỉnh quá trình thông qua một phần tử điều khiển. Nhờ

18
vậy, những giá trị này có thể làm sáng tỏ về quan hệ thời gian: KP phụ thuộc vào
sai lệch hiện tại, KI phụ thuộc vào tích lũy các sai lệch quá khứ, và KD dự đoán
các sai lệch tương lai, dựa vào tốc độ thay đổi hiện tại.
Có thể biểu diễn sơ đồ khối của bộ điều khiển như hình 2.5 sau:

Hình 2.5 Sơ đồ khối bộ điều khiển PID liên tục
2.3.2. Phƣơng trình vi phân
Phương trình vi phân cho bộ điều khiển PID liên tục có dạng:
t

u (t )  K P e(t )  K I  e(t )dt  K D
0

de(t )
dt

(2.17)

Bằng cách điều chỉnh 3 hệ số trong giải thuật của bộ điều khiển PID, bộ điều
khiển có thể dùng trong những thiết kế có yêu cầu đặc biệt. Đáp ứng của bộ điều
khiển có thể được mô tả dưới dạng độ nhạy sai số của bộ điều khiển, giá trị mà
bộ điều khiển vọt lố điểm đặt và giá trị dao động của hệ thống.
Vài ứng dụng có thể yêu cầu chỉ sử dụng một hoặc hai khâu tùy theo hệ
thống. Điều này đạt được bằng cách thiết đặt đội lợi của các đầu ra không mong
muốn về 0. Một bộ điều khiển PID sẽ được gọi là bộ điều khiển PI, PD, P hoặc I
nếu vắng mặt các tác động bị khuyết. Bộ điều khiển PI khá phổ biến, do đáp ứng
vi phân khá nhạy đối với các nhiễu đo lường, trái lại nếu thiếu giá trị tích phân
có thể khiến hệ thống không đạt được giá trị mong muốn.

2.3.3. Hàm truyền :
Dạng Hàm truyền (Laplace) của bộ điều khiển PID như sau:
𝐾𝑖
𝐾𝑑 𝑆 2 + 𝐾𝑃 𝑆 + 𝐾𝑖
𝐺 𝑠 = 𝐾𝑃 + + 𝐾𝑑 𝑆 =
𝑠
𝑆

(2.18)


19

2.3.4. Đặc tính quá độ
Một biều đồ điển hình đáp ứng quá độ (theo hàm nhảy bậc) các khâu hiệu chỉnh
PID được trình bày như hình 2.6.

Hình 2.6 Đáp ứng quá độ hệ thống
Thông qua biểu đồ đáp ứng quá độ hệ thống ta thấy :
Quá trình PI : tốc độ đáp ứng nhanh, giảm thời gian quá độ nhưng độ vọt lố
tức thời tăng cao.
Quá trình PD : giảm độ vọt lố tức thời , không ảnh hưởng sai số và giảm
dao động.
Quá trình PID : với thông số thích hợp có thể hiệu chỉnh để hệ thống ổn
định với sai số xác lập bằng 0, độ vọt lố tức thời và thời gian quá độ đạt yêu cầu
mong muốn.
Do vậy thường chọn PID cho các thuật toán điều khiển.
2.3.5. Điều chỉnh vòng lặp PID
Điều chỉnh một vòng điều khiển là điều chỉnh các thông số điều khiển của nó
tới giá trị đáp ứng điều khiển tối ưu. Độ ổn định (dao động biên) là một yêu cầu

căn bản, nhưng ngoài ra, các hệ thống khác nhau, có những hành vi khác nhau,
những ứng dụng khác nhau có những yêu cầu khác nhau, và vài yêu cầu lại mâu
thuẫn với nhau. Hơn nữa, vài quá trình có một mức độ phi tuyến nào đấy khiến
các thông số làm việc tốt ở điều kiện đầy tải sẽ không làm việc khi quá trình
khởi động từ không tải; điều này có thể khắc phục bằng chương trình độ lợi (sử
dụng các thông số khác nhau cho những khu vực hoạt động khác nhau).


20
Điều chỉnh PID là một bài toán khó, ngay cả khi chỉ có 3 thông số.Vì vậy có
nhiều phương pháp khác nhau để điều chỉnh vòng lặp.
Những phương pháp hữu hiệu nhất thường bao gồm những triển khai của vài
dạng mô hình xử lý, sau đó chọn KP, KI, và KD dựa trên các thông số của mô
hình động học. Các phương pháp điều chỉnh thủ công tương đối không hiệu quả
lắm, đặc biệt nếu vòng lặp có thời gian đáp ứng được tính bằng phút hoặc lâu
hơn.
Lựa chọn phương pháp thích hợp sẽ phụ thuộc phần lớn vào việc có hay
không vòng lặp có thể điều chỉnh "offline", và đáp ứng thời gian của hệ thống.
Nếu hệ thống có thể thực hiện offline, phương pháp điều chỉnh tốt nhất thường
bao gồm bắt hệ thống thay đổi đầu vào từng bước, tín hiệu đo lường đầu ra là
một hàm thời gian, sử dụng đáp ứng này để xác định các thông số điều khiển.
*. Phương pháp điều chỉnh theo kinh nghiệm Ziegler-Nichols
Một trong những phương pháp điều chỉnh khá phổ biến và hiệu quả là phương
pháp điều chỉnh theo kinh nghiệm Ziegler-Nichols, được đưa ra bởi John G.
Ziegler và Nathaniel B. Nichols vào những năm 1940.
Trong phương pháp này, các hệ số KI và KD lúc đầu được gán bằng không. Hệ
số KP được tăng cho đến khi nó tiến tới tới hạn,
, ở đầu ra của vòng điều
khiển bắt đầu dao động.
và thời gian dao động

được dùng để gán hệ số
như bảng sau:
Dạng điều khiển
P

-

PI

-

PID

*. Phần mềm điều chỉnh PID
Hầu hết các ứng dụng công nghiệp hiện đại không còn điều chỉnh vòng điều
khiển sử dụng các phương pháp tính toán thủ công như trên nữa. Thay vào đó,
phần mềm điều chỉnh PID và tối ưu hóa vòng lặp được dùng để đảm báo kết quả
chắc chắn. Những gói phần mềm này sẽ tập hợp dữ liệu, phát triển các mô hình
xử lý, và đề xuất phương pháp điều chỉnh tối ưu. Vài gói phần mềm thậm chí
còn có thể phát triển việc điều chỉnh bằng cách thu thập dữ liệu từ các thay đổi
tham khảo.


21
Điều chỉnh PID bằng toán học tạo ra một xung trong hệ thống, và sau đó sử
dụng đáp ứng tần số của hệ thống điều khiển để thiết kế các giá trị của vòng điều
khiển PID. Trong những vòng lặp có thời gian đáp ứng kéo dài nhiều phút, nên
chọn điều chỉnh bằng toán học, bởi vì việc thử sai thực tế có thể kéo dài nhiều
ngày để tìm điểm ổn định cho vòng lặp. Giá trị tối ưu thì khó tìm hơn. Vài bộ
điều khiển số còn có chức năng tự điều chỉnh, trong đó những thay đổi rất nhỏ

của điểm đặt cũng được gửi tới quá trình, cho phép bộ điều khiển tự mình tính
toán giá trị điều chỉnh tối ưu.
Các dạng điều chỉnh khác cũng được dùng tùy theo tiêu chuẩn đánh giá kết
quả khác nhau. Nhiều phát minh hiện nay đã được nhúng sẵn vào trong các
module phần mềm và phần cứng để điều chỉnh PID.
2.4. Điều khiển toàn phƣơng tuyến tính LQR
Điều khiển toàn phương tuyến tính LQR (Linear Quadratic Regulator) là một
phương pháp điều khiển tối ưu. Điều khiển tối ưu là điều khiển để có được một
chỉ tiêu chất lượng cho trước là tốt nhất. Chất lượng được đánh giá bằng giá trị
của hàm mục tiêu:
𝐽=

𝑇
𝑔
0

hay

𝑥, 𝑢 𝑑𝑡 → 𝑚𝑖𝑛

Với bài toán có hệ tuyến tính:

𝐽=

𝑁
𝑘=0 𝑔(𝑥𝑘 , 𝑢𝑘 )

→ 𝑚𝑖𝑛

𝑥 𝑡 = 𝐴𝑥 𝑡 + 𝐵𝑢 𝑡

hàm mục tiêu có dạng toàn phương như sau:
∞

𝑥 𝑇 𝑄𝑥 + 𝑢𝑇 𝑅𝑢 𝑑𝑡

𝐽=

(2.19)

0

Trong đó Q là ma trận xác định dương (hoặc bán xác định dương), R là ma
trận xác định dương : 𝑄 = 𝑄𝑇 ≥ 0, 𝑅 = 𝑅𝑇 > 0
Cấu trúc điều khiển LQR của hệ có thể được biểu diễn như hình 2.7.

Hình 2.7 Cấu trúc điều khiển phản hồi trạng thái LQR
Trong đó cần tìm một ma trận phản hồi K cho vec-tơ điều khiển tối ưu
u(t) = - Kx(t) thỏa mãn chỉ tiêu chất lượng J đạt giá trị cực tiểu như trên.


22
Ma trận K tối ưu được xác định từ phương trình Riccati có dạng:
𝐾 = 𝑅−1 𝐵𝑇 𝑃
Ở đây, ma trận 𝑃 = 𝑃𝑇 > 0 là nghiệm của phương trình đại số Riccati:
𝑃𝐴 + 𝐴𝑇 𝑃 + 𝑄 − 𝑃𝐵𝑅−1 𝐵𝑇 𝑃 = 0

(2.20)

Như vậy, luật điều khiển tối ưu cho bài toán điều khiển tối ưu dạng toàn

phương với chỉ tiêu chất lượng là phương trình tuyến tính và có dạng:
𝑢 𝑡 = −𝐾𝑥 𝑡 = −𝑅−1 𝐵𝑇 𝑃 𝑥(𝑡)
Để hệ kín ổn định thì tính quan sát được (obserble) và tính điều khiển được
(controllable) phải được đảm bảo.
2.5. Các nhân tố bất định trong đo điều khiển và biện pháp khắc phục
2.5.1. Sai số đo từ các biến
Trạng thái hoặc các biến ra của hệ thống (ở đây là góc nghiêng  hay độ dịch
chuyển x) được đo bằng các cảm biến. Các giá trị đo được đưa về so sánh với
giá trị đặt (ở đây  và x đặt là bằng 0) để có sai lệch e(t) nhằm điều khiển hệ
thống về trạng thái cân bằng. Điều đó cho thấy việc xác định chính xác các giá
trị đo là rất cần thiết cho việc điều khiển hệ thống. Tuy nhiên, trong thực tế các
giá trị đo này đều gặp phải các can nhiễu làm mất chính xác các giá trị nhận
được. Vì vậy việc lọc bỏ các can nhiễu này để các giá trị đo nhận được càng gần
đúng với giá trị trạng thái và biến ra thực sự của robot là càng tốt.
Luận án sẽ sử dụng một khối đo góc nghiêng của robot MPU6050 bao gồm 2
cảm biến cho 2 số đọc, một từ gia tốc kế (accelerometer) và một từ con quay hồi
chuyển (gyroscope). Gia tốc kế cho số đọc góc nghiêng so với phương trọng
trường quả đất gặp phải các can nhiễu lớn với các chuyển động nhanh. Trong
khi đó, con quay hồi chuyển cho góc nghiêng từ việc lấy tích phân các giá trị
vận tốc góc của con quay lại gặp phải can nhiễu do sự trôi của quá trình lấy tích
phân với tần số thấp.
Để có được giá trị góc nghiêng tin cậy từ hai giá trị đọc được này, một trong
những phương pháp hiện đang được dùng phổ biến là bộ lọc Kalman đã được sử
dụng.
2.5.2. Nâng cao độ tin cậy phép đo bằng giải thuật lọc Kalman
Giải thuật lọc Kalman được đề xuất từ năm 1960 bởi giáo sư Kalman để thu
thập và kết hợp linh động các thông tin từ cảm biến thành phần. Một khi phương
trình định hướng và mẫu thống kê nhiễu trên mỗi cảm biến được biết và xác
định, bộ lọc Kalman sẽ cho ước lượng giá trị tối ưu (chính xác do đã được loại
sai số, nhiễu) như là đang sử dụng một tín hiệu “tinh khiết” và có độ phân bổ

23
không đổi. Trong hệ thống này, tín hiệu cảm biến của bộ lọc gồm hai tín hiệu: từ
cảm biến góc và cảm biến vận tốc góc. Tín hiệu ngõ ra của bộ lọc là tín hiệu của
accelerometer và gyro đã được loại nhiễu nhờ hai nguồn tín hiệu hỗ trợ và xử lý
lẫn nhau trong bộ lọc, thông qua quan hệ (vận tốc góc bằng đạo hàm/vi phân của
giá trị góc).
Lưu đồ thuật giải của bộ lọc Kalman đã được tác giả Bitshop dẫn giải như
hình sau:

Hình 2.8 Lưu đồ thuật giải của bộ lọc Kalman

Kết quả mô phỏng của tín hiệu cảm biến trước và sau khi qua bộ lọc kal man
như sau :

Hình 2.9 Mô phỏng bộ lọc Kalman


24
CHƢƠNG III .

MÔ PHỎNG HỆ THỐNG ROBOT CÂN BẰNG
3.1 Mô phỏng hệ thống robot
Việc mô phỏng robot tự cân bằng được thực hiện trên cơ sở các phương trình
tính toán mô hình hệ thống và các bộ điều khiển được lựa chọn như nội dung
chương 1 đã nêu.
Với 2 cách biểu diễn các mô hình (theo hàm truyền hoặc theo phương trình
trạng thái) hoạt động của hệ có thể được mô phỏng trong các môi trường Matlab
với mô hình đã được tuyến tính hóa hay Simulink với mô hình phi tuyến.

Với sơ đồ phân tích lực tác dụng lên robot được coi như một hệ con lắc ngược
với hai hàm truyền cho phần bánh xe Pcart và phần khung xe (con lắc) Ppend,
người thiết kế phải chọn các tham số đầu vào, đầu ra của robot để điều khiển tốt
khả năng cân bằng và ổn định của mô hình.

Hình 3.1 Phân tích lực tác dụng lên hệ robot
Các tham số được thiết kế như trong bảng 3.1 sau :