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

HỒ CHÍ MINH
KHOA ĐÀO TẠO CHẤT LƯỢNG CAO
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

ĐỒ ÁN 2

ROBOT HAI BÁNH TỰ CÂN BẰNG

SỬ DỤNG GIẢI THUẬT LQR

Ngành Kỹ Thuật Điều Khiển & Tự Động Hóa

GVHD: TS. NGUYỄN TRẦN MINH NGUYỆT

Sinh viên: NGUYỄN HƯNG
MSSV: 17151086
HỒ LÊ LONG THIÊN
MSSV: 17151132
TP. HỒ CHÍ MINH, THÁNG 7 NĂM 2020
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH
KHOA ĐÀO TẠO CHẤT LƯỢNG CAO
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

ĐỒ ÁN 2

ROBOT HAI BÁNH TỰ CÂN BẰNG

SỬ DỤNG GIẢI THUẬT LQR

Ngành Kỹ Thuật Điều Khiển & Tự Động Hóa

GVHD: TS. NGUYỄN TRẦN MINH NGUYỆT

Sinh viên: NGUYỄN HƯNG
MSSV: 17151086
HỒ LÊ LONG THIÊN
MSSV: 17151132
TP. HỒ CHÍ MINH, THÁNG 7 NĂM 2020
MỤC LỤC

CHƯƠNG 1 GIỚI THIỆU ERROR! BOOKMARK NOT DEFINED.

1.1 TẦM QUAN TRỌNG CỦA ĐỀ TÀI ERROR! BOOKMARK NOT DEFINED.
1.2 MỤC TIỆU 3
1.3 GIỚI HẠN 3
1.4 PHƯƠNG PHÁP THỰC HIỆN 4
1.5 NỘI DUNG 4

CHƯƠNG 2 CƠ SỞ LÝ THUYẾT 5

2.1 MÔ HÌNH HÓA XE HAI BÁNH TỰ CÂN BẰNG 5

2.2 LÝ THUYẾT LQR 6
2.2.1 Giới thiệu bộ điều khiển toàn phương tuyến tính LQR 6
2.2.2 Phương trình Riccati đại số 6
2.2.3 Bài toán LQR liên tục 8
2.2.4 Bài toán LQR rời rạc 8
2.3 MPU – 6050 9
2.3.1 Giới thiệu MPU – 6050 9
2.3.2 Xử lý dữ liệu đọc từ cảm biến 9
2.3.3 Các phương pháp xử lý tín hiệu từ cảm biến MPU – 6050 10
2.3.3.1 Lọc bổ phụ thông tần 10
2.3.3.2 Lọc Kalman 11
2.4 CẢM BIẾN ĐO VỊ TRỊ ENCODER 15
2.4.1 Giới thiệu 15
2.4.2 Xử lý tín hiệu trên Encoder 15
2.5 ĐỘNG CƠ ĐIỆN MỘT CHIỀU 16
2.5.1 Giới thiệu 16
2.5.2 Điều chỉnh tốc độ động cơ một chiều 18

3
2.6 GIAO TIẾP I2C 19
2.6.1 Nguồn gốc I2C 19
2.6.2 Cấu tạo và nguyên lý làm việc 20
2.6.3 Giao thức truyền dữ liệu 22

CHƯƠNG 3 THIẾT KẾ HỆ THỐNG 22

3.1 NGUYÊN LÝ HOẠT ĐỘNG ROBOT 2 BÁNH TỰ CÂN BẰNG 22

3.2 LƯU ĐỒ GIẢI THUẬT 23
3.3 MÔ HÌNH ROBOT 2 BÀNH TỰ CÂN BẰNG 24
3.4 LỰA CHỌN LINH KIỆN 25
3.4.1 Pin và động cơ 25
3.4.2 Mạch điều khiển 27
3.4.3 Mạch giảm áp 28
3.4.4 Mạch cầu H 29
3.4.5 Cảm biến góc 31
3.5 SƠ ĐỒ KẾT NỐI ERROR! BOOKMARK NOT DEFINED.
3.6 MÔ HÌNH THỰC ERROR! BOOKMARK NOT DEFINED.33

CHƯƠNG 4 KẾT QUẢ 34

TÀI LIỆU THAM KHẢO ERROR! BOOKMARK NOT DEFINED.35

PHỤ LỤC 36

4
CHƯƠNG 1 GIỚI THIỆU

1.1. TẦM QUAN TRỌNG CỦA ĐỀ TÀI

Những xe robot trong ngành xây dựng, các nhiệm vụ tuần tra, giám sát,...
điều khiển từ xa hầu hết là những robot di chuyển bằng ba bánh xe, với hai bánh
lái được lắp ráp đồng trục và một bánh đuôi nhỏ. Có nhiều kiểu khác nhau,
nhưng đây là kiểu thông dụng nhất. Còn đối với các xe 4 bánh, thường một đầu
xe có hai bánh truyền động và đầu xe còn lại được gắn một hoặc hai bánh lái.

Hình 1.1: Robot ghi hình điều khiển từ xa WowWee

Việc thiết kế ba hay bốn bánh làm cho xe robot được thăng bằng ổn định nhờ
trọng lượng của nó được chia cho hai bánh lái chính và bánh đuôi hay bất kỳ cái
gì khác để đỡ trọng lượng của xe. Nếu trọng lượng được đặt nhiều vào bánh lái
thì xe sẽ không ổn định dễ bị ngã, còn nếu đặt nhiều vào bánh đuôi thì hai bánh
chính sẽ mất khả năng bám. Nhiều thiết kế xe robot có thể di chuyển tốt trên địa
hình phẳng, nhưng không thể di chuyển tốt trên địa hình lồi lõm hoặc mặt phẳng
nghiêng. Khi di chuyển lên đồi, trọng lượng xe robot dồn vào đuôi xe làm bánh
lái mất khả năng bám và trượt ngã, đối với những bậc thang, thậm chí nó dừng
hoạt động và chỉ quay tròng bánh xe.

1
 Một ví dụ về nhược điểm của xe ba hoặc bốn bánh là: Khi xe đi lên dốc
nghiêng, do trọng lượng đầu xe khá nhẹ so với tổng khối lượng của xe, cộng
thêm việc động cơ bánh sau mạnh, khiến cho xe mất thăng bằng và lật ra sau.

Hình 1.2: Xe 3 bánh di chuyển trên mặt phẳng nghiêng, bị lật ra sau

Việc chế tạo xe 4 bánh, giống như xe hơi đồ chơi hay các loại xe bốn bánh
đang sử dụng trong giao thông hiếm khi gặp các vấn đề trên, nhưng điều này sẽ
làm các xe robot không gọn gàng và bộ phận lái (cua quẹo) gặp một chút khó
khăn, khó xác định được góc cua và quãng đường đi.
Các xe dạng hai bánh đồng trục lại thăng bằng rất tốt và linh động khi di
chuyển trong địa hình phức tạp. Khi nó leo sườn dốc, nó tự động nghiêng ra
trước và giữ cho trọng lượng dồn về về trước. Tương tự khi leo xuống dốc, nó
nghiêng ra sau và dồn trọng lực về sau. Do đó, không bao giờ có hiện tượng xe
mất thăng bằng và ngã.

Hình 1.3: Xe 2 bánh di chuyển trên mặt phẳng nghiêng

2
 Đối với những địa hình khó đi, sự thăng bằng của xe hai bánh sẽ mang lại
nhiều ý nghĩa thực tiễn trong giới hạn ổn định hơn là xe ba/bốn bánh truyền
thống.
Ngoài ra, đề tài “xe hai bánh tự cân bằng” có nhiều ứng dụng trong đời sống
và làm nền tảng cho các ứng dụng khác dựa vào thiết kế này.

1.2. MỤC TIÊU

- Đề tài tập trung vào việc xây dựng các mô hình thuật toán điều khiển hai
bánh tự cân bằng phương pháp LQR.
- Thiết kế, chế tạo mô hình thực tế có khả năng giữ thăng bằng trên nhiều
địa hình khác nhau.

1.3. GIỚI HẠN ĐỀ TÀI

Trong khoản thời gian 16 tuần nghiên cứu và thực hiện đồ án, nhóm sinh
viên gặp phải những giới hạn của đề tài như sau:
- Vì không có nhiều kinh phí nên mô hình đề tài không được lớn và hoành
tráng, về mặt trang trí thì không được đầu tư nhiều.
- Khả năng lập trình, tính toán còn hạn chế nên thuật toán và code điều
hành xe robot chưa quá chỉnh chu, chi tiết và chính xác hoàn toàn.

1.4. PHƯƠNG PHÁP THỰC HIỆN

Bản thân là hệ phi tuyến ảnh hưởng bởi các yếu tố vị trí, vận tốc, góc
nghiêng đồng thời nên quá trình điều khiển ổn định hệ thống gặp nhiều khó khăn.
Quá trình nghiên cứu để thực hiện đề tài tóm tắt qua các bước sau:
- Phân tích ưu nhược điểm những đề tài, ứng dụng thực tiễn liên quan đến
đề tài.
- Đánh giá tính khả năng và phát triển tương lai.
- Thu thập tài liệu trong và ngoài nước về mô hình và phương pháp điều
khiển.

3
 - Thiết kế mô hình phần cứng.
- Thiết kế phần điện sử dụng các module có sẵn.
- Thực thi giải thuật bằng lập trình.
- Kiểm tra và sửa lỗi.

1.5. NỘI DUNG

Chương 1: Tổng quan Giới thiệu tổng quan về robot hai bánh tự cân bằng, ưu
khuyết điểm của thiết kế hai bánh.
Chương 2: Cơ sở lí thuyết Phương pháp điều khiển PID, Fuzzy, LQR, Giới
thiệu Kalman, Xử lí tín hiệu MPU 6050 và encoder, Ardunio mega 2560.
Chương 3: Thiết kế hệ thống Chi tiết thiết kế cơ khí, thiết kế điện, lưu đồ giải
thuật, mô phỏng giải thuật trên Simulink.
Chương 4: Kết quả Kết quả thực nghiệm và hướng phát triển của đề tài.

4
CHƯƠNG 2 CƠ SỞ LÝ THUYẾT

2.1. MÔ HÌNH HÓA XE HAI BÁNH TỰ CÂN BẰNG

Hình 2.1: Mô hình xe Hình 2.2: Hệ xe hai bánh Hình 2.3: Hệ xe hai bánh
hai bánh tự cân bằng cân bằng nhìn nghiêng cân bằng nhìn từ trên xuống

Các thành phần của hệ:

Kí hiệu Đơn vị Ý nghĩa G m/s2 Gia tốc trọng trường

m kg Khối lượng của bánh xe θ rad Góc trung bình của bánh trái
M kg Khối lượng của robot và phải
R m Bán kính bánh xe θl,r rad Góc của bánh trái và phải
W m Chiều rộng của robot Góc nghiêng của phần thân
ψ rad
D m Chiều ngang của robot robot
H m Chiều cao của robot Góc xoay của robot
φ rad
L m Khoảng cách từ trọng
xl, yl, zl m Tọa độ bánh trái
tâm robot đến trục bánh
xe xr, yr, zr m Tọa độ bánh phải
ƒw Hệ số ma sát giữa bánh Tọa độ trung bình
xe và mặt phẳng di
xm, ym, zm m
chuyển Fθ, Fψ, Fφ Nm Moment phát động theo các
ƒm Hệ số ma sát giữa robot phương khác nhau
và động cơ DC Fl,r Nm Moment phát động của động
sssJm kgm2 Moment quán tính động cơ bánh trái, phải
cơ DC il, ir A Dòng điện động cơ bánh trái,
Rm Ω Điện trở động cơ DC phải
Kb Vs/rad Hệ số EMF của động cơ vl, vr V Điện áp động cơ bánh trái,
DC phải
Kt Nm/A Moment xoắn của động
cơ DC
N Tỉ số giảm tốc

5
 Bảng 2.1: Kí hiệu và ý nghĩa của các đại lượng

2.2. LÝ THUYẾT LQR

2.2.1. GIỚI THIỆU BỘ ĐIỀU KHIỂN TOÀN PHƯƠNG TUYẾN

TÍNH LQR

Hình thức cơ bản nhất của các vòng tuần hoàn trong các hệ vô hướng là
không trực tiếp mở rộng thành hệ đa biến (MIMO), được đặc trưng bởi các ma
trận thay vì các công thức.
Khái niệm về sự tối ưu gắn chặt với thiết kế hệ thống kiểm soát MIMO. Bộ
điều khiển tối ưu, tức là các bộ điều khiển tốt nhất có thể, theo các chuyên gia, bộ
kiểm soát tối ưu này tạo ra bộ điều khiển ổn định cho các hệ thống MIMO.
Do đó, các giải pháp kiểm soát tối ưu cung cấp một quy trình thiết kệ tự
động, ta chỉ phải quyết định dùng phương pháp nào.
Bộ điều chỉnh bậc hai tuyến tính LQR (Linear–quadratic regulator) là một
phương pháp nổi tiếng tạo ra sự hiệu quả thực tiễn.

2.2.2. PHƯƠNG TRÌNH RICCATI ĐẠI SỐ

Để đảm bảo hệ thống ổn định tại gốc tọa độ và có khả năng chống ảnh hưởng
của nhiễu, một hàm mục tiêu được lựa chọn như sau:  
∞
1 𝑇 𝑇
𝐽= 2
∫ 𝑥 𝑄𝑥 + 𝑢 𝑅𝑢 𝑑𝑡
0

Trong đó Q và R là các ma trận đối xứng xác định dương. Chọn luật điều
khiển hồi tiếp trạng thái
u = -Kx, K là véc tơ hằng số. Thay vào J ta có:
∞
1 𝑇 𝑇
𝐽= 2
∫ 𝑥 𝑄 + 𝐾 𝑅𝐾 𝑥𝑑𝑡
0

Chọn hàm Lyapunov là J ta được:

6
 ∞
1 𝑇 𝑇
𝑉𝑥𝑡 = 2
∫ 𝑥 𝑄 + 𝐾 𝑅𝐾 𝑑𝑡
0

Đạo hàm theo thời gian:

𝑉˙(𝑥) =
𝑇 𝑇 ∞
1
2 (
𝑥 𝑄 + 𝐾 𝑅𝐾 𝑥|𝑡 )
𝑇 𝑇 𝑇 𝑇
=
1
2 [
𝑥 (∞) 𝑄 + 𝐾 𝑅𝐾 𝑥(∞) − ] 1
2 [
𝑥 (𝑡) 𝑄 + 𝐾 𝑅𝐾 𝑥(𝑡) ]
Chọn K để hệ ổn định, do đó 𝑥 ∞→0. Vậy:

𝑉˙(𝑥) =−
𝑇 𝑇
1
2 [
𝑥 (𝑡) 𝑄 + 𝐾 𝑅𝐾 𝑥(𝑡) ]
Mặt khác ta có thể biểu diễn hàm Lyapunov dưới dạng:
1 𝑇
𝑉𝑥𝑡 = 2
𝑥 𝑃𝑥

Trong đó P là một ma trận đối xứng xác định dương. Như vậy ta có thể tính

𝑉˙(𝑥) theo P như sau:

𝑇
𝑉˙(𝑥) =− 𝑥˙ 𝑃𝑥 + 𝑥 𝑃𝑥˙ =
𝑇 𝑇 𝑇
1
2
1
2 [
𝑥 (𝐴 − 𝑏𝐾) 𝑃 + 𝑃(𝐴 − 𝑏𝐾) 𝑥 ]
𝑇 𝑇 𝑇 𝑇
⇨
1
2 [
𝑥 (𝐴 − 𝑏𝐾) 𝑃 + 𝑃(𝐴 − 𝑏𝐾) 𝑥 =− ] 1
2 (
𝑥 𝑄 + 𝐾 𝑅𝐾 𝑥 )
Ma trận P sẽ thỏa mãn phương trình:
𝑇 𝑇
(𝐴 − 𝑏𝐾) 𝑃 + 𝑃(𝐴 − 𝑏𝐾) =− (𝑄 + 𝐾 𝑅𝐾)
𝑇
Đặt 𝑅 = Γ Γ, Γ , là ma trận vuông không suy biến. Phương trình Lyapunov viết
lại là:

(𝐴𝑇 − 𝐾𝑇𝑏𝑇)𝑃 + 𝑃(𝐴 − 𝑏𝐾) + 𝑄 + 𝐾𝑇Γ𝑇 = 0

−1 𝑇 −1 𝑇
𝐴 𝑃 + 𝑃𝐴 + ⎡⎢Γ𝐾 − (Γ ) 𝑏 𝑃⎤⎥ ⎡⎢Γ𝐾 − Γ 𝑏 𝑃⎤⎥ − 𝑃𝑏𝑅 𝑏 𝑃 + 𝑄 = 0
𝑇 𝑇 𝑇 𝑇 −1 𝑇

⎣ ⎦ ⎣
( ) ⎦
∂𝑃
Lấy đạo hàm phương trình theo 𝐾𝑖𝑗 và dùng tính chất: ∂𝐾𝑖𝑗
=0

(
⎡
) ⎤
𝑇 −1 𝑇 𝑇 −1 𝑇
Ta suy ra:
∂
∂𝐾𝑖𝑗
⎢ Γ𝐾 − Γ
⎢ ( ) 𝐵 𝑃 (Γ𝐾 − Γ ( ) 𝐵 𝑃)⎥ = 0
⎥
⎣ ⎦
Cực tiểu xảy ra khi số hạng trong ngoặc bằng không. Do đó

7
 𝑇 −1 𝑇
Γ𝐾 = Γ ( ) 𝐵 𝑃

Vậy bộ điều khiển sẽ là:

−1 𝑇
𝐾 =𝑅 𝐵 𝑃
Phương trình Lyapunov trở thành phương trình đại số Riccati:
𝑇 −1 𝑇
𝐴 𝑃 + 𝑃𝐴 − 𝑃𝑏𝑅 𝑏 𝑃 + 𝑄 = 0
2.2.3. BÀI TOÁN LQR LIÊN TỤC:
Đối tượng tuyến tính liên tục được mô tả bởi phương trình trạng thái:

𝑥˙(𝑡) = 𝐴𝑥(𝑡) + 𝐵𝑢(𝑡) (8)

𝑇
Vector trạng thái: 𝑥(𝑡) = [𝑥1(𝑡), 𝑥2(𝑡), …, 𝑥𝑛(𝑡)] (9)

𝑇
Vector tín hiệu điều khiển: 𝑢(𝑡) = [𝑢1(𝑡), 𝑢2(𝑡), …, 𝑢𝑚(𝑡)] (10)

Bài toán đặt ra là làm tín hiệu điều khiển 𝑢(𝑡) điều chỉnh hệ thống từ trạng
thái đầu 𝑥(0) = 𝑥0 bất kỳ về trạng thái cuối 𝑥(𝑡𝑓) = 0 sao cho tối thiểu chỉ tiêu

chất lượng dạng toàn phương:

𝑡𝑓
𝑇 𝑇 𝑇
𝐽(𝑢) =
1
2 ( ) ( )
𝑥 𝑡𝑓 𝑀𝑥 𝑡𝑓 +
1
2 [
∫ 𝑥 (𝑡)𝑄𝑥(𝑡) + 𝑢 (𝑡)𝑅𝑢(𝑡) 𝑑𝑡
𝑡0
] (11)

trong đó Q và M là các ma trận trọng số bán xác định dương

R là ma trận trọng số xác định dương

2.2.4. BÀI TOÁN LQR RỜI RẠC:

Đối tượng tuyến tính rới rạc được mô tả bởi phương trình trạng thái:
𝑥(𝑘 + 1) = 𝑎𝑑𝑥(𝑘) + 𝑏𝑑𝑢(𝑘) (11)

𝑇
Vector trạng thái: 𝑥(𝑡) = [𝑥1(𝑡), 𝑥2(𝑡), …, 𝑥𝑛(𝑡)] (12)

𝑇
Vector tín hiệu điều khiển: 𝑢(𝑡) = [𝑢1(𝑡), 𝑢2(𝑡), …, 𝑢𝑚(𝑡)] (13)

8
 Bài toán đặt ra là làm tín hiệu điều khiển 𝑢(𝑘) điều chỉnh hệ thống từ trạng
thái đầu 𝑥(0) = 𝑥0 bất kỳ về trạng thái cuối 𝑥(𝑁) = 0 sao cho tối thiểu chỉ tiêu

chất lượng dạng toàn phương:

𝑁−1
𝑇 𝑇 𝑇
𝐽(𝑢) =
1
2
𝑥 (𝑁)𝑀𝑥(𝑁) +
1
2 [
∑ 𝑥 (𝑘)𝑄𝑥(𝑘) + 𝑢 (𝑘)𝑅𝑢(𝑘) ] (14)
𝑘=0

trong đó Q và M là các ma trận trọng số bán xác định dương,

R là ma trận trọng số xác định dương.

2.3. MPU-6050
2.3.1. GIỚI THIỆU MPU – 6050

- MPU-6050 là came biến của hãng InvenSense. MPU-6050 là một trong

những giải pháp cảm biến chuyển động đầu tiên trên thế giới có tới 6 (mở rộng
tới 9) trục cảm biến tích hợp trong 1 chip duy nhất. MPU-6050 sử dụng công
nghệ độc quyền MotionFusion của InvenSense có thể chạy trên các thiết bị di
động, tay điều khiển… Nó được điều hành ra một nguồn cung cấp 3.3V/5V, và
giao tiếp thông qua I2C với tốc độ tối đa 400kHz. Chip này cũng có sẵn trong
một gói SPI được gọi là MPU6000 cho tốc độ giao tiếp lên tới 10Mbs.
- MPU6050 chỉ hỗ trợ chuẩn giao tiếp I2C để xuất giá trị đo sang thiết bị
khác (Master) hoặc chính nó lại là nơi xử lý, lưu trữ tín hiệu từ các thiết bị khác
kết nối vào. Đầu tiên ta cần nắm được Protocol của chuẩn I2C.

2.3.2. XỬ LÍ DỮ LIỆU ĐỌC TỪ CẢM BIẾN:

Đối với dữ liệu lấy từ Accelerometer:
- Accelerometer (gọi tắt là accel): như tên gọi của nó, accel đơn giản là một
cảm biến đo gia tốc của bản thân module và thường sẽ có 3 trục xyz ứng với 3
chiều không gian (loại 1 và 2 trục ít dùng). Lưu ý là accel đo cả gia tốc của trọng
lực nên giá trị thực khi đo sẽ bao gồm cả trọng lực.
- MPU6050 có gia tốc kế 3 trục và con quay hồi chuyển 3 trục. Gia tốc kế
đo gia tốc dọc theo ba trục và con quay hồi chuyển đo tốc độ góc ba trục. Để đo

9
góc nghiêng của robot, chúng ta cần các giá trị gia tốc dọc theo trục y và
z Hàm atan2 (y, z) cho biết góc có đơn vị là radian giữa trục z dương của mặt
phẳng và điểm được cho bởi tọa độ (z, y) trên mặt phẳng đó, với dấu dương cho
góc ngược chiều kim đồng hồ (nửa mặt phẳng phải), y> 0) và dấu âm cho các
góc theo chiều kim đồng hồ (nửa mặt phẳng trái, y <0). 
Đối với dữ liệu lấy từ Gyro:
- Gyroscope (gọi tắt là gyro): là một loại cảm biến đo tốc độ quay của nó
quanh một trục. Tương tự với accel, gyro cũng thường có 3 trục xyz.
- Con quay hồi chuyển 3 trục của MPU6050 do tốc độ góc (vận tốc quay)
dọc theo ba trục. Đối với robot tự cận bằng, vận tốc dọc theo trục x là đủ để đo
tốc độ ngã của robot. Phương pháp chuyển đổi giá trị con quay hồi tiếp về trục
x, chuyển đổi nó thành độ chia giây và sau đó nhận với thời gian vòng lập để có
được sự thay đổi về góc.

2.3.3. CÁC PHƯƠNG PHÁP XỬ LÝ TÍN HIỆU CẢM BIẾN

MPU6050
2.3.3.1. Lọc bổ phụ thông tần:
- Bộ lọc được sử dụng trong trường hợp khi có hai nguồn giá trị khác nhau
trong công việc ước lượng một giá trị và đặc tính nhiễu của hai giá trị chẳng hạn
một nguồn mang lại thông tin đúng trong vùng tần số thấp trong khi nguồn khác
chỉ đúng trong vùng tần số cao. Bộ lọc bổ phụ là kết hợp những ngõ ra của độ
nghiêng và vận tốc gyro nhằm thu được khả năng ước lượng tốt nhất của sự định
hướng, đề đền bù cho sự trôi giá trị gyro (drift) và cho đáp ứng chậm của cảm
biến đo nghiêng.
- Bộ lọc này được thiết kế để ngõ ra phân bổ đến ước lượng chỉ trong vùng
tần số cao. Yêu cầu cơ bản của bộ lọc có thể được tính nhau sau:
+ Toàn bộ hệ thống có sự khuếch đại là hằng số và phase tổn thất nhỏ nhất
ngay vùng tần số cắt của cảm biến nghiêng.

10
 + Để giữ độ nhạy offset của gyro là nhỏ nhất, cảm biến nghiêng nên được
dùng trong vùng băng thông tần số rộng nhất có thể.
+ Số lượng tham số thiết kế nên nhỏ để dễ cho sự điều chỉnh và áp dụng bộ
lọc trong thực tế của các vi điều khiển.
2.3.3.2. Lọc Kalman
Giới thiệu chung về bộ 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ổ không đổi. Trong hệ thống
này, tín hiệu cảm biến vào bộ lọc gồm hai tín hiệu: từ cảm biến góc
(inclinometer) và cảm biến vận tốc góc (gyro). Tín hiệu ngõ ra của bộ lọc là tín
hiệu của inclinometer 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 = đạo hàm/vi
phân của giá trị góc).
Bộ lọc Kalman đơn giản là thuật toán xử lý dữ liệu hồi quy tối ưu. Có nhiều
các xác định tối ưu, phụ thuộc tiêu chuẩn lựa chọn trình thông số đánh giá. Nó
cho thấy rằng bộ lọc Kalman tối ưu đối với chi tiết cụ thể trong bất kỳ tiêu chuẩn
có nghĩa nào. Một khía cạnh của sự tối ưu này là bộ lọc Kalman hợp nhất tất cả
thông tin được cung cấp tới nó. Nó xử lý tất cả giá trị sẵn có, ngoại trừ độ sai số,
ước lượng giá trị hiện thời của nhựng giá trị quan tâm, với cách sử dụng hiểu biết
động học thiết bị giá trị và hệ thống, mô tả số liệu thống kê của hệ thống nhiễu,
gồm nhiễu ồn, nhiễu đo và sự không chắc chắn trong mô hình động học và những
thông tin bất kỳ về điều kiện ban đầu của giá trị quan tâm.
Giải thuật bộ lọc Kalman áp dụng cho cảm biến gia tốc và gyro:
Cảm biến góc được thiết kế bằng cách kệt hợp một cảm biến gia tốc 1 trụ là
một gyro vận tốc 1 trục. Hai cảm biến này được nối nhau thông qua một bộ lọc

11
Kalman 2 trạng thái, với trạng thái là góc và trạng thái còn lại là giá trị cơ sở
gyro (gyro bias).
Gyro_bias được điều chỉnh tự động bởi bộ lọc. Kalman filter là một vấn đề
thực sự phức tạp, mặc dù đã được tối ưu nhiều lần đoạn code C.
Khai báo biến cố định: 𝑃𝑖𝑛𝑖𝑡 = [1 0 0 1 ]

float angle;
float q_bias;
float rate;
R tượng trung cho giá trị nhiễu covariance. Trong trường hợp này, nó là mà
𝑜
trận 1x1 được mong đợi 0.03 rad ≈ 18 từ gia tốc kế:
R_angle = 0.03;
Q là ma trận 2x2 tượng trung cho tiến trình nhiễu covariance. Trong trường
hợp này, nó chỉ mức độ tin cậy của gia tốc kế quan hệ với gyro.
𝑄 = [𝑄_𝑎𝑛𝑔𝑙𝑒 0 0 𝑄_𝑔𝑦𝑟𝑜 ] = [0. 001 0 0 0. 003 ]
State_update được gọi mỗi dt với giá trị cơ sở gyro bởi người dùng module.
Nó cập nhật góc hiện thời và vận tốc ước lượng.
Giá trị gyro_m được chia thành đúng đơn vị thực, nhưng không cần bỏ
gyro_bias độ nghiêng. Bộ lộc theo dõi độ nghiêng.
Vector giá trị: X = [𝑎𝑛𝑔𝑙𝑒 𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠 ]
Nó chạy dưới sự ước lượng giá trị qua hàm giá trị:

𝑋˙ = [𝑎𝑛𝑔𝑙𝑒
˙ 𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠
˙
] = [𝑔𝑦𝑟𝑜 − 𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠 0 ]
Và cập nhật ma trận Covariance qua hàm:

𝑃˙ = 𝐴𝑃 + 𝑃𝐴 + 𝑄
'

𝑃˙ = 𝐴 𝑃 + 𝑃 𝐴 + 𝑄 (𝑙ý 𝑡ℎ𝑢𝑦ế𝑡 𝑙à 𝑃 = 𝐴 𝑃 𝐴 + 𝑄
. . ' . . 𝑇

A là Jacobian của 𝑋˙ với giá trị mong đợi:

˙ ˙ ˙ ˙
𝐴 = ⎡⎢ 𝑑((𝑎𝑛𝑔𝑙𝑒))
𝑑 𝑎𝑛𝑔𝑙𝑒 𝑑(𝑎𝑛𝑔𝑙𝑒) 𝑑(𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠) 𝑑(𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠) ⎤⎥ = [0 − 1 0 0 ]
⎣ 𝑑(𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠) 𝑑(𝑎𝑛𝑔𝑙𝑒) 𝑑(𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠) ⎦

12
 Vì CPU nhỏ có sẵn trên vi điều khiẻn, nên tối ưu code C chỉ để tính giới hạn
rõ ràng không bằng 0, cũng như khai triển toán ma trận qua vài bước có thể.
Cách tiính này làm nó khó đọc hơn, debug và kéo dài hơn thuật toán chính xác
của bộ lọc Kalman, nhưng cho phép ít thời gian thực hiện với CPU.

𝑃˙ = 𝑄𝑎𝑛𝑔𝑙𝑒 − 𝑃[0][1] − 𝑃[1][0] − 𝑃[1][1] − 𝑃[1][1] 𝑄𝑔𝑦𝑟𝑜

[ ]

= [0 − 1 0 0 ][𝑃[0][0] 𝑃[0][1] 𝑃[1][0] 𝑃[1][1] ] + [𝑃[0][0] 𝑃[0][1] 𝑃[1][0] 𝑃[1][1] ][0 0 −

+ [𝑄_𝑎𝑛𝑔𝑙𝑒 0 0 𝑄_𝑔𝑦𝑟𝑜 ]

= [− 𝑃[1][0] − 𝑃[1][1] 0 0 ] + [− 𝑃[0][1] 0 − 𝑃[1][1] 0 ] + [𝑄_𝑎𝑛𝑔𝑙𝑒 0 0 𝑄_𝑔𝑦𝑟𝑜 ]

Lưu trữ giá trị chưa bias của gyro:
rate = q = q_m – q_bias
.
Cập nhật ước lượng góc: ˙ 𝑑𝑡 = 𝑟𝑎𝑡𝑒.𝑑𝑡
angle = angle + 𝑎𝑛𝑔𝑙𝑒
Cập nhật ma trận Covariance:
.
𝑃 = 𝑃 + 𝑃˙ 𝑑𝑡

[
= [𝑃[0][0] 𝑃[0][1] 𝑃[1][0] 𝑃[1][1] ] + 𝑄𝑎𝑛𝑔𝑙𝑒 − 𝑃[0][1] − 𝑃[1][0] − 𝑃[1][1] − 𝑃[1][1

Kalman_update được gọi bởi người dùng module khi giá trị gia tốc kế có sẵn.
Giá trị angle_m không cần chia thành đơn vị thực tế, nhưng phải được chuẩn
mức 0 và có độ chia như nhau.
Procedure này không cần phải gọi mỗi bước thời gian, nhưng có thể nếu dữ
liệu gia tốc có sặn tại vận tốc bằng giá trị vận tốc gyro.
Ma trận C là một ma trận 1x2 (giá trị x trạng thái), đó là ma trận Jacobian của
giá trị đo lường với giá trị mong đợi. Trong trường hợp này, C là:
𝑑(𝑎𝑛𝑔𝑙𝑒_𝑚) 𝑑(𝑎𝑛𝑔𝑙𝑒_𝑚)
𝐶 =⎡ ⎤ = [1 0 ] (𝐶 𝑐ℎí𝑛ℎ 𝑙à 𝐻)
⎣ 𝑑(𝑎𝑛𝑔𝑙𝑒) 𝑑(𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠) ⎦

13
 Vì giá trị góc đáp ứng trực tiếp với góc ước lượng và giá trị góc không quan
hệ với giá trị gyro_bias, C_0 cho thấy giá trị trạng thái quan hệ trực tiếp với trạng
thái ước lượng như thế nào, C_1 cho thấy giá trị trạng thái không quan hệ với giá
trị cơ sở gyro ước lượng.
Error là giá trị khác nhau trong giá trị đo lường và giá trị ước lượng. Trong
trường hợp này, nó khác nhau giữa hai gia tốc kế đo góc và góc ước lượng.
angle_error = angle_m – angle
Tính sai số ước lượng. Từ bộ lọc Kalman:
𝑇
𝐸 = 𝐶𝑃𝐶 + 𝑅 = [1 0 ][𝑃[0][0] 𝑃[0][1] 𝑃[1][0] 𝑃[1][1] ][1 0 ] + 𝑅 = 𝑃[0][0] + 𝑅
PCt_0 = 𝐶[0] * 𝑃[0][0]
PCt_1 = 𝐶[0] * 𝑃[1][0]
Ước tính bộ lọc Kalman đạt được. Từ lý thuyết bộ lọc Kalman:
[𝐾0 𝐾1 ] = [𝑃𝐶𝑡_0/𝐸 𝑃𝐶𝑡_0/𝐸 ]
P = (I – KH)P = P – KHP = P – KCP
Ta có phép nhân điểm trôi (floating point):
𝐶𝑃 = [1 0 ][𝑃[0][0] 𝑃[0][1] 𝑃[1][0] 𝑃[1][1] ] = [𝑃[0][0] 𝑃[0][1] ] = [𝑡_0 𝑡_1 ]
. . .
[ ] [
𝑃 = [𝑃[0][0] 𝑃[0][1] 𝑃[1][0] 𝑃[1][1] ] − [𝐾0 𝐾1 ] 𝑡0 𝑡1 = 𝑃 − 𝐾0 𝑡_0 𝐾0 𝑡_1 𝐾1

Cập nhật giá trị ước lượng. Lần nữa, từ Kalman:

( )
𝑋 = [𝑎𝑛𝑔𝑙𝑒 𝑔𝑦𝑟𝑜_𝑏𝑖𝑎𝑠 ] = 𝑋 + 𝐾 𝑍𝑚𝑒𝑎𝑠𝑢𝑟𝑒 − 𝐻𝑍𝑒𝑠𝑡𝑖𝑚𝑎𝑡𝑒 = 𝑋 + 𝐾[𝑎𝑛𝑔𝑙𝑒_𝑒𝑟𝑟 𝑎𝑛𝑔𝑙𝑒_𝑒

angle_err=q_bias_err

2.4. CẢM BIẾN ĐO VỊ TRÍ ENCODER

2.4.1. GIỚI THIỆU

14
 Hình 2.4: Mô tả Encoder

Encoder được dùng để đo vận tốc và vị trí của bánh xe, là một trong những
tham số điều khiển mô hình. Đĩa cảm biến được làm bằng mica, ghép với một
tấm film trong đã in sẵn các vạch đen trắng xe kẽ.
Cấu tạo gồm một đĩa đục có những rãnh trong suốt, một nguồn phát quang
thông qua các rãnh trên đĩa đến một đầu thu. Tín hiệu ra có thể dạng xung vuông
hoặc hình sin. Việc đọc các tín hiệu ra này sẽ xác định vị trí cũng như vận tốc của
động cơ.

2.4.2. XỬ LÍ TÍN HIỆU TRÊN ENCODER

Chu vi bánh xe:

(22)
Số xung trên mân encoder: n
Góc mở trên một xung:

(23)
Quãng đường đi được trên một xung:

(24)

15
 Nếu cần di chuyển một quãng đường S thì số xung di chuyển cần phải có:

(25)

2.5. ĐỘNG CƠ ĐIỆN MỘT CHIỀU

2.5.1. Giới thiệu

Động cơ DC có thể được mô hình hóa một cách đơn giản như hình vẽ dưới
đây:

Hình 2.5: Mô hình hóa động cơ DC

Ở đây, động cơ gồm một cuộn dây N vòng dây quấn quanh một khung dây
hình chữ nhật với a, b là cạnh có thế quay xung quanh trục . Vị trí của trục
được xác định bởi góc quay . Cuộn dây có tổng điện trở là R và độ tự cảm L.

16
 Hệ chuyển động có moment quán tính J với trục . Từ trường B xuyên qua
khung dây được tạo ra từ một nam châm vĩnh cửu.
Hệ cơ S tác dụng lên trục một ngẫu lực cản, kí hiệu ( ) được tính theo

công thức:

Với c là hằng số giảm chấn của kết cấu cơ khí ( bộ truyền động ở trục động
cơ).

Hình 2.6: Mô tả động cơ điện một chiều

Khi ta đặt điện áp một chiều R và 2 đầu chổi than H và K thì trong cuộn dây
phần ứng có dòng điện I ư. Theo hiện tượng cảm ứng điện từ khung dây sẽ chịu
tác dụng của lực điện trường và quay quanh trục . Lưu ý rằng cứ mỗi nữa vòng
lại chuyển mạch do H và K là 2 vành bán khuyên. Điều này bảo đảm cho khung

dây luôn quay theo một chiều hất định với vận tốc gốc .

E(t),I(t) là tổng điện áp cảm ứng và dòng dẫn trong động cơ. C(t), . là

ngẫu lực điện từ và vận tốc xoay của rotor. Ta có công thức:

Với k là hằng số phụ thuộc vào đặc tính của động cơ, loại cuộn dây, giá trị từ trường, thừng

là hằng số của nam châm vĩnh cữu.

17
 Cuộn dây dẫn được mô hình hóa bởi điện trở R và độ tự cảm L, được áp vào điện áp U(t),

có phương trình dưới đây:

Hình 2.7: Điều khiển tốc độ điện áp và dòng điện

Phương trình cơ học:

Trong mối quan hệ ở trên, sự phụ thuộc vận tốc góc theo C(t) là đặc tính cơ học của

quán tính vật rắn và ma sát xoắn.

Với J là momen quán tính, là hằng số; Fs(t) là ngẫu lực xoay và fv là hệ số ma sát quay, ta

có phương trình cơ học:

2.5.2. Điều chỉnh tốc độ động cơ một chiều

Có ba cách điều khiển tốc độ động cơ DC servo trong kỹ thuật: Phương pháp
điều áp, phương páp điều độ tần số và phương pháp điều chỉnh độ rộng xung.
Nguyên lý

18
 Nhin lên hình vẽ, Ton và Toff lần lượt là khoảng thời gian kích ( trạng thái
HIGH) và tín hiệu ngắt ( trạng thái LOW) trong một chu kỳ. Việc làm này sẽ tạo
ra mức điện áp Vtb cấp cho động cơ tương ứng với mức tốc độ động cơ.

Hình 2.8: Xung PWM của động cơ DC

Cũng cần chú ý rằng vì đây là điện áp trung bình của động cơ tương ứng với
một tỉ lệ Ton/Toff nào đó, nên mối quan hệ giữa vận tốc động cơ và điện áp trung
bình này là không tuyến tính.
Điện áp trung bình được tính bằng công thức:

Ưu điểm
Một trong những ưu điểm của PWM là khả nang9 chống nhiễu giá trị từ bộ
xử lí đến hệ thống điều khiển, do tín hiệu bản chất vẫn là tín hiệu số nên nhiễu
chỉ xãy ra nếu nó đủ mạnh hơn mức tín hiệu số để đổi từ logic 0 lên 1 hoặc
ngược lại.

2.6. GIAO TIẾP I2C

2.6.1. NGUỒN GỐC I2C

Đầu năm 1980 Phillips đã phát triển một chuẩn giao tiếp nối tiếp 2 dây được
gọi là I2C. I2C là tên viết tắt của cụm từ Inter-Intergrated Circuit. Đây là đường
Bus giao tiếp giữa các IC với nhau. I2C mặc dù được phát triển bới Philips,
nhưng nó đã được rất nhiều nhà sản xuất IC trên thế giới sử dụng. I2C trở thành

19
một chuẩn công nghiệp cho các giao tiếp điều khiển, có thể kể ra đây một vài tên
tuổi ngoài Philips như: Texas Intrument(TI), MaximDallas, analog Device,
National Semiconductor ... Bus I2C được sử dụng làm bus giao tiếp ngoại vi cho
rất nhiều loại IC khác nhau như các loại Vi điều khiển 8051, PIC, AVR, ARM...
chip nhớ như: RAM tĩnh (Static Ram), EEPROM, bộ chuyển đổi tương tự số
(ADC), số tương tự(DAC), IC điểu khiển LCD, LED...
I2C là giao thức truyền thông nối tiếp đồng bộ phổ biến hiện nay, được
sử dụng rộng rãi trong việc kết nối nhiều IC với nhau, hay kết nối giữa IC
và các ngoại vi với tốc độ thấp.

Hình 2.9: Giao thức I2C

2.6.2. CẤU TẠO VÀ NGUYÊN LÝ HOẠT ĐỘNG

I2C sử dụng hai đường truyền tín hiệu:

- Một đường xung nhịp đồng hồ(SCL) chỉ do Master phát đi ( thông thường
ở 100kHz và 400kHz. Mức cao nhất là 1Mhz và 3.4MHz).
- Một đường dữ liệu(SDA) theo 2 hướng.
Có rất nhiều thiết bị có thể cùng được kết nối vào một bus I2C, tuy nhiên sẽ
không xảy ra chuyện nhầm lẫn giữa các thiết bị, bởi mỗi thiết bị sẽ được nhận ra

20
bởỉ một địa chỉ duy nhất với một quan hệ chủ/tớ tồn tại trong suốt thời gian kết
nối. Mỗi thiết bị có thể hoạt động như là thiết bị nhận hoặc truyền dữ liệu hay có
thể vừa truyền vừa nhận. Hoạt động truyền hay nhận còn tùy thuộc vào việc thiết
bị đó là chủ (master) hay tớ (slave).
Một thiết bị hay một IC khi kết nối với bus I2C, ngoài một địa chỉ (duy nhất)
để phân biệt, nó còn được cấu hình là thiết bị chủ hay tớ.Tại sao lại có sự phân
biệt này ? Đó là vì trên một bus I2C thì quyền điều khiển thuộc về thiết bị
chủ. Thiết bị chủ nắm vai trò tạo xung đồng hồ cho toàn hệ thống, khi giữa hai
thiết bị chủ-tớ giao tiếp thì thiết bị chủ có nhiệm vụ tạo xung đồng hồ và quản lý
địa chỉ của thiết bị tớ trong suốt quá trình giao tiếp. Thiết bị chủ giữ vai trò chủ
động, còn thiết bị tớ giữ vai trò bị động trong việc giao tiếp.
Về lý thuyết lẫn thực tế I²C sử dụng 7 bit để định địa chỉ, do đó trên một bus
có thể có tới 2^7 địa chỉ tương ứng với 128 thiết bị có thể kết nối, nhưng chỉ có
112 , 16 địa chỉ còn lại được sử dụng vào mục đích riêng. Bit còn lại quy định
việc đọc hay ghi dữ liệu (1 là write, 0 là read)
Điểm mạnh của I²C chính là hiệu suất và sự đơn giản của nó: một khối điều
khiển trung tâm có thể điều khiển cả một mạng thiết bị mà chỉ cần hai lối ra điều
khiển.
Ngoài ra I2C còn có chế độ 10bit địa chỉ tương đương với 1024 địa chỉ,
tương tự như 7 bit, chỉ có 1008 thiết bị có thể kết nối, còn lại 16 địa chỉ sẽ dùng
để sử dụng mục đích riêng.

21
 Hình 2.10: Sơ đồ kết nối thiết bị chủ và tớ của I2C

2.6.3. GIAO THỨC TRUYỀN DỮ LIỆU

Giao thức sau đây (tập hợp các quy tắc) được theo sau bởi thiết bị Master và
các thiết bị Slave để truyền dữ liệu giữa chúng.
Dữ liệu được truyền giữa thiết bị Master và các thiết bị Slave thông qua một
đường dữ liệu SDA duy nhất, thông qua các chuỗi có cấu trúc gồm các số 0 và 1
(bit). Mỗi chuỗi số 0 và 1 được gọi là giao dịch (transaction) và dữ liệu trong mỗi
giao dịch có cấu trúc như sau:

Hình 2.11: Giao thức truyền dữ liệu của I2C

CHƯƠNG 3 THIẾT KẾ HỆ THỐNG

22
3.1. NGUYÊN LÝ HOẠT ĐỘNG CỦA ROBOT HAI BÁNH
TỰ CÂN BẰNG
Robot hai bánh tự cân bằng đọc giá trị vị trí từ cảm biến encoder và giá trị
góc nghiêng từ cảm biến góc MPU 6050, đưa về chuân analog input của vi điều
khiển Arduino Mega sau đó xử lý lọc nhiễu, xuất tín hiệu điều khiển PWM đến
ENA, ENB để điều khiển động cơ A, B.

23
3.2. LƯU ĐỒ GIẢI THUẬT

Hình 3.1: Lưu đồ giải thuật

3.3. MÔ HÌNH ROBOT HAI BÁNH TỰ CÂN BẰNG

24
 Mô tả phần cứng:

Mô hình thực tế của đề tài chia làm 2 phần chính: khung và hai động cơ.
Khung được làm bằng 3 tấm bìa mica được xếp chồng lên nhau bằng các trụ
đồng, bên trong gồm các linh kiện, pin nguồn. Kích thước khung 17x6,5x10cm.

Thiết kế:

Hình 3.2: Kích thước bìa mica được thiết kế (tầng dưới)

Hình 3.3: Kích thước bìa mica được thiết kế (tầng giữa và tầng trên)

- Các lỗ ø4 để gắn động cơ GA25 và trụ đồng. các lỗ ø10 và ø20 để đi dây.
- Các lỗ ø10 và ø20 để đi dây, kết nối linh kiện.

25
3.4. LỰA CHỌN LINH KIỆN

3.4.1. CHỌN PIN VÀ ĐỘNG CƠ

Chọn pin:
2 Pin 18650 Shuangdi SD-18650 - 4.2V - 3800mAh

Hình 3.4: Pin 18650 Shuangdi SD-18650 - 4.2V - 3800mAh

Bộ nguồn gồm 2 pin 4.2V mắc nối tiếp với nhau để được một nguồn điện
tương đương 8.4V cung cấp cho động cơ và các mạch điện tử khác.Động cơ
GA25 sử dụng trong mô hình cần nguồn cấp 6-12 VDC. Mặc khác ta cần một
nguồn 4-35VDC cấp cho mạch ổn áp cấp nguồn 5V cho các linh kiện khác.
Chọn động cơ và bánh xe:
Động cơ DC servo có hộp giảm tốc GA25 có encoder, thường dùng làm
robot tự cân bằng,
- Điện áp hoạt động: 3 - 12V
- Đường kính trục: 4mm
- Tốc độ không tải: 220 rpm
- Mômen: 1.88 kgf.cm

26
 Hình 3.5: Động cơ GA25

Hình 3.6: Bánh xe gắn với động cơ

27
 Hình 3.7: Encoder tích hợp động cơ GA25

- Chân 2 và 5 dùng để cấp nguồn cho encoder.

- Chân C1/A và C2/B là đầu đọc xung của encoder.
- Chân M1 và M2 dùng để điều khiển quay thuận, nghịch.

3.4.2. MẠCH ĐIỀU KHIỂN

Board Arduino MEGA 2560 R3 sử dụng chip ATmega2560 của Atmel có bộ

nhớ chương trình 256KB trong đó 8KB sử dụng cho bootloader. Với Board
Arduino MEGA 2560 R3 có thể viết nhiều chương trình, điều khiển được nhiều
thiết bị với thuật toán phức tạp nhờ bộ nhớ chương trình lớn, RAM, và
EEPROM lớn ( 8KB RAM, 4KB EEPROM):
- 4 cổng serial
- 6 ngắt ngoài
- 15 chân PWM
- 16 Cổng Analog

28
 Hình 3.8: Arduino Mega 2560

3.4.3. MẠCH GIẢM ÁP

Bộ nguồn của vi điều khiển và các mạch điện tử được tách rời khỏi bộ
nguồn cung cấp cho động cơ nhằm hạn chế tín hiệu nhiễu từ động cơ lên hoạt
động các vi điều khiển và cảm biến.
Mạch giảm áp LM2596 3A:
- Là module giảm áp có khả năng điều chỉnh được dòng ra đến 3A
- Module nguồn không sử dụng cách ly
- Nguồn đầu vào từ 4V - 35V
- Nguồn đầu ra: 1V - 30V
- Dòng ra Max: 3A
- Kích thước mạch: 53 (mm) x26 (mm)

29
 Hình 3.9: Mạch giảm áp LM2596

3.4.4. MẠCH CẦU H

Mạch cầu H đóng vai trò là bộ trung gian giao tiếp giữa vi điều khiển và cơ
cấu chấp hành ở đề tài này là động cơ. Tín hiệu vi điều khiển được cách ly với
các tín hiệu vào MOSFET công suát bằng các opto quang nhằm tránh xung đột
điện áp gây hư hỏng vi điều khiển.

Hình 3.10: Mạch vi điều khiển của mạch cầu H

30
 Khi động cơ được mắc vào OUT1 và OUT2 đồng thời ngõ vào INT 1 ở mức
cao, và đầu vào INT2 ở mức thấp thì động cơ sẽ quay phía trước. Ngược lại nếu
đầu INT1 ở mức thấp và INT2 ở mức cao thì động cơ sẽ quay về phía sau.
Tương tự cho động cơ còn lại. Trong trường hợp cả hai đều cùng mức cao hoặc
cùng mức thấp thì động cơ dừng.
L298N 2.5A Mina: Mạch điều khiển động cơ dòng 2.5A với dây silicon.
Điện áp nguồn từ 2V - 10V, có thể điều khiển hai động cơ DC hoặc một động cơ
bước 2 pha 4 dây cùng một lúc. Nó có thể điều khiển tiến, lùi và tốc độ với bảo
vệ nhiệt và phục hồi tự động:
- Điều khiển động cơ cầu H hai chiều, có thể điều khiển hai động cơ DC
hoặc một động cơ bước hai pha 4 dây cùng một lúc
- Điện áp 2V - 10V
- Dòng điện làm việc đơn 2.5A, Dòng điện chờ thấp (dưới 0,1ua)
- Mạch bảo vệ quá nhiệt tích hợp (tSD)
- Kích thước sản phẩm: 31x32x5mm (chiều dài, chiều rộng và chiều cao),
Kích thước nhỏ, thích hợp để lắp ráp và xe
- Trọng lượng: 11g

Hình 3.11: Mạch cầu H L298N

3.4.5 CẢM BIẾN GÓC

31
 Cảm biến góc GY-521 MPU6050:
Là một cảm biến sáu trục, có chứa một gia tốc 3 trục con quay hồi chuyển 3
trục. Hoạt động với 3.3V và giao tiếp I2C với tốc độ tối đa 400KHz
Các cảm biến bên trong MPU-6050 sử dụng bộ chuyển đổi tương tự – số
(Anolog to Digital Converter – ADC) 16-bit cho ra kết quả chi tiết về góc quay,
tọa độ… Với 16-bit bạn sẽ có 2^16 = 65536 giá trị cho 1 cảm biến.
Tùy thuộc vào yêu cầu của bạn, cảm biến MPU-6050 có thể hoạt động ở chế
độ tốc độ xử lý cao hoặc chế độ đo góc quay chính xác (chậm hơn). MPU-6050
có khả năng đo ở phạm vi:
Con quay hồi chuyển: ± 250 500 1000 2000 dps
Gia tốc: ± 2 ± 4 ± 8 ± 16g
Hơn nữa, MPU6050 có sẵn bộ đệm dữ liệu 1024 byte cho phép vi điều khiển
phát lệnh cho cảm biến, và nhận về dữ liệu sau khi MPU-6050 tính toán xong.

Hình 3.12: MPU6050

3.5. SƠ ĐỒ KẾT NỐI

32
Hình 3.13: Mạch điện kết nối cảm biến với vi điều khiển

33
3.6. MÔ HÌNH THỰC

Hình 3.14: Mô hình thực xe cân bằng.

34
CHƯƠNG 4 KẾT QUẢ

Kết quả thu được sau khi nghiên cứu::

- Hoàn thành được nhiệm vụ thiết kế và thi công mô hình robot hoàn chỉnh.
- Sử dụng lọc Kalman cho MPU – 6050 nhằm tăng tính ổn định:
+ Tín hiệu đỏ là tín hiệu có sử dụng lọc Kalman.
+ Tín hiệu xanh là tín hiệu không sử dụng lọc Kalman.

Hình 4.1: Lọc Kalman cho MPU – 6050

- Robot có thể giữ thăng bằng dùng giải thuật LQR nhưng chưa hoàn thiện.

Hình 4.2: Đáp ứng của hệ thống

35
 Hướng phát triển của đề tài:
- Sử dụng giải thuật Fuzzy-PID thay cho giải thuật LQR.
- Mô hình có thể phát triển để điều khiển thông qua máy tính nhờ kết nối
bluetooth và giao diện trên máy tính sẽ thân thiện trực quan hơn.
- Tối ưu hóa khối lượng, thuật toán để mô hình có thể hoạt động trơn tru, linh
hoạt, dễ điều khiển, tiết kiệm năng lượng.
- Có thể gắn thêm camera và gps để robot có thể định vị, ghi hình, xử lý ảnh
và tự hoạt động trong không gian lớn.
- Mô hình robot 2 bánh tự cân bằng là tiền đề để phát triển thành xe 2 bánh tự
cân bằng có thể chở được người và di chuyển linh hoạt hơn.

36
 TÀI LIỆU THAM KHẢO

[1] KS. Mai Tuấn Đạt, Xe Hai Bánh Tự Cân Bằng Di Chuyển Trên Địa
Hình Phẳng, Đại học Bách Khoa thành phố Hồ Chí Minh, năm 2005.

[2] ThS. Nguyễn Đình Phú, Vi Xử Lý, Đại học Sư Phạm Kỹ Thuật thành
phố Hồ Chí Minh, năm 2013.

[3] ThS. Huỳnh Thái Hoàng, Lý Thuyết Điều Khiển Tự Động, NXB Đại
Học Quốc Gia, năm 2005.

[4] Cộng đồng Arduino Việt Nam.

Tác giả chịu trách nhiệm bài viết:

Họ tên: Nguyễn Hưng, Hồ Lê Long Thiên

Đơn vị: Khoa Đào Tạo Chất Lượng Cao, trường Đại học Sư Phạm Kỹ Thuật

Điện thoại: 0338 432 252

Email: 17151086@student.hcmute.edu.vn,
17151132@student.hcmute.edu.vn

37
 PHỤ LỤC

#include <Wire.h>
#include "Kalman.h"
#define ToRad PI/180
#define ToDeg 180/PI

Kalman kalman; //Kalman filter define: kalman.getAngle(pitch, gyrorate, dt);

float loopStartTimer,loop_time;
const double FIX_loop_time = 12500;

uint32_t timer; //Timer for kalman filter psi angle;

uint8_t i2cData[14];
volatile long leftencoder; //read left encoder value
volatile long righencoder; //read right encoder value

//LQR data//
long PWML, PWMR; // PWM output for H-Brigde
float k1, k2, k3, k4, k5, k6;// The factor of K maxtrix
bool falldown;// Run = true; Stop = false;

float theta, psi, phi;

float thetadot, psidot, phidot;
float thetaold, psiold, phiold;
float leftvolt; //output volt left motor in LQR
float righvolt; //output volt right motor in lQR

uint32_t timerloop, timerold;

//MPU6050 Data//
float mpudata; //Save psi angle ( Y axis)
float AcX, AcZ;
float Gyro;

void setup()
{

Serial.begin(9600); //Enable Serial Communications

k1 = 35;
k2 = 5;
k3 = 400;
k4 = 15;
k5 = 27;
k6 = 2.3;
//SET PWM FREQUENCY 31 kHz
TCCR2B = TCCR2B & B11111000 | B00000001;//Pin 9 & Pin 10

38
 //Motor control Pin//
pinMode(8,OUTPUT); //PWM Pin 1 ENA
pinMode(11,OUTPUT); //PWM Pin 2 ENB
pinMode(28,OUTPUT); //Left Motor Pin 1 INT1
pinMode(26,OUTPUT); //Left Motor Pin 2 INT2
pinMode(24,OUTPUT); //Right Motor Pin 1 INT3
pinMode(22,OUTPUT); //Right Motor Pin 2 INT4

//Encoder Pin//
pinMode(1,INPUT_PULLUP);// CHÂN NGẮT LEFT
pinMode(4,INPUT_PULLUP);// CHÂN NGẮT RIGHT
pinMode(2,INPUT_PULLUP);// CHÂN ĐỌC ENCODER LEFT A
pinMode(18,INPUT_PULLUP);// CHÂN ĐỌC ENCODER RIGHT A
pinMode(3,INPUT_PULLUP);// CHÂN ĐỌC ENCODER LEFT B
pinMode(19,INPUT_PULLUP);// CHÂN ĐỌC ENCODER RIGH B

attachInterrupt(1, left_isr, FALLING);

attachInterrupt(4, righ_isr, FALLING);
//Data MPU6050//
Wire.begin();
Wire.setClock(400000UL); // Set I2C frequency to 400kHz

i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz

i2cData[1] = 0x00;// Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro
filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and
disable sleep mode

while (i2cRead(0x75, i2cData, 1));

if (i2cData[0] != 0x68) {
Serial.print(F("Error reading sensor"));
while (1);
}

delay(100); // Wait for sensor to stabilize

while (i2cRead(0x3B, i2cData, 6));

AcX = (i2cData[0] << 8) | i2cData[1];
AcZ = (i2cData[4] << 8) | i2cData[5];
double pitch = atan2(-AcX, AcZ)* RAD_TO_DEG;
kalman.setAngle(pitch );

}
39
void loop()
{
loopStartTimer = micros();
readmpu();

float dt = (float)(micros() - timerloop)/1000000.0;

timerloop = micros();

//Set time loop update and control motor

theta = gettheta(leftencoder, righencoder) * ToRad; //Read theta value and convert to
Rad
psi = (mpudata+2) * ToRad; //Read psi value and convert to Rad
///// điều chỉnh giá trị (2.1) sao cho góc trả về đúng
phi = getphi(leftencoder, righencoder) * ToRad; //Read phi value and convert to
Rad

//Update input angle value

thetadot = (theta - thetaold) / dt;
psidot = (psi) / dt;
phidot = (phi - phiold) / dt;
//Upadte old angle value
thetaold = theta;
psiold = psi;
phiold = phi;
getlqr(theta , thetadot, psi, psidot, phi , phidot);
motorcontrol(PWML, PWMR, (psi ), falldown);
Serial.println(loop_time);
loop_time = micros() - loopStartTimer;

if(loop_time<FIX_loop_time)
{
delayMicroseconds(FIX_loop_time - loop_time);
}
}

void readmpu() {
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
AcX = ((i2cData[0] << 8) | i2cData[1]);
//accY = ((i2cData[2] << 8) | i2cData[3]);
AcZ = ((i2cData[4] << 8) | i2cData[5]);

//gyroX = (i2cData[8] << 8) | i2cData[9];

Gyro = (i2cData[10] << 8) | i2cData[11];
//gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
40
 double pitch = atan2(-AcX, AcZ) * RAD_TO_DEG;
double Gyrorate = Gyro / 131.0;
mpudata = kalman.getAngle(pitch, Gyrorate, dt);//Using kalman filter angle
if (abs(mpudata) > 30) //Limit angle falldown motor
{
falldown = true;//Stop Motor
}
else
{
falldown = false;//Run Motor
}
}

//Read theta angle function//

float gettheta(long lencoder, long rencoder) { //deg value
float angle = 0.8 * (lencoder + rencoder);
return angle;
}

//Read phi angle function//

float getphi(long lencoder, long rencoder) { //deg value
float angle = (6.5/17) * (lencoder - rencoder);
return angle;
}

//left motor encoder interrupt//

void left_isr() {
if (digitalRead(2) == LOW) {
leftencoder++;
}
else {
leftencoder--;
}
}

//right motor encoder interrupt//

void righ_isr() {
if (digitalRead(18)== LOW) {
righencoder++;
}
else {
righencoder--;
}
}
//Read phi angle function//
//LQR function//
void getlqr(float theta_, float thetadot_, float psi_, float psidot_, float phi_, float phidot_)
{
41
 leftvolt = k1*theta_ + k2*thetadot_ + k3*psi_ + k4*psidot_ - k5*phi_ - k6*phidot_;
righvolt = k1*theta_ + k2*thetadot_ + k3*psi_ + k4*psidot_ + k5*phi_ + k6*phidot_;
PWML = map(leftvolt, -(k3 * PI) / 15, (k3 * PI) / 15, -240, 240); //Limit 15 deg.
PWMR = map(righvolt, -(k3 * PI) / 15, (k3 * PI) / 15, -240, 240);

PWML = constrain(PWML, -200, 200);//limit pwm value in (-240, 240) because we

using high frequency pwm (31 khz)
PWMR = constrain(PWMR, -200, 200);
}
//Motor control function//
void motorcontrol(long lpwm, long rpwm, float angle, bool stopstate) {
if (stopstate == true) {
stopandreset();
}
else {
if (abs(angle) > 30) // angle psi > 30 motor will stop
{
stopandreset();
}
else
{
//
if (leftvolt > 0)
{
leftmotor(abs(lpwm), 1);//Forward
}
else if (leftvolt < 0)
{
leftmotor(abs(lpwm), 0);//Back
}
else
{
stopandreset();
}
//
if (righvolt > 0)
{
righmotor(abs(rpwm*0.985), 1);
}
else if (righvolt < 0)
{
righmotor(abs(rpwm*0.985), 0);
}
else
{
stopandreset();
}
42
 }
}
}
//Stop motor and reset data
void stopandreset() //The data angle and encoder will be reset back to zero.
{
digitalWrite(8, LOW);
digitalWrite(11, LOW);
//Reset default place//
leftencoder = 0;
righencoder = 0;
}
//Control left motor
void leftmotor(uint8_t lpwm, int direct) {
if (direct == 1) { // angle > 0
digitalWrite(22, HIGH);
digitalWrite(24,LOW);
analogWrite(11, lpwm);
}
else {
digitalWrite(22, LOW);
digitalWrite(24, HIGH);
analogWrite(11, lpwm);
}
}
//Control right motor
void righmotor(uint8_t rpwm, int direct) {
if (direct == 1) { // angle > 0
digitalWrite(28, HIGH);
digitalWrite(26, LOW);
analogWrite(8, rpwm);
}
else {
digitalWrite(28, LOW);
digitalWrite(26, HIGH);
analogWrite(8, rpwm);
}
}

43