Xe hai bánh tự cân bằng.pdf

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT
THÀNH PHỐ HỒ CHÍ MINH
SKC 0 0 6 7 2 9
CÔNG TRÌNH NGHIÊN CỨU KHOA HỌC SINH VIÊN
XE HAI BÁNH TỰ CÂN BẰNG
Tp. Hồ Chí Minh, tháng 07/2018
MÃ SỐ:SV2018-62

TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM
Thuộc nhóm ngành khoa học: Kỹ Thuật
TP Hồ Chí Minh, 7/2018
BÁO CÁO TỔNG KẾT
ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN
SV2018-62

i
BÁO CÁO TỔNG KẾT
ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN
SV2018-62
Thuộc nhóm ngành khoa học: Kỹ thuật
SV thực hiện: Phạm Ngọc Thái Nam, Nữ: Nam
Dân tộc: Kinh
Lớp, khoa:141511, Khoa Điện- Điện Tử
Năm thứ: 4 /4
Ngành học: Công nghệ kĩ thuật điều khiển và tự động hóa.
Người hướng dẫn: PGS. TS. Nguyễn Minh Tâm.
TP Hồ Chí Minh, 7/2018

ii
MỤC LỤC
CHƯƠNG 1: TỔNG QUAN ....................................................................................1
1.1 Đặt vấn đề ................................................................................................1
1.2 Mục tiêu đề tài .........................................................................................1
1.3 Nội dung của đề tài ......................................................................................2
1.4 Giới hạn của đề tài .......................................................................................2
1.5 Ý nghĩa thực tiễn và khoa học của đề tài .....................................................2
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT.........................................................................3
2.1 Con lắc ngược ..............................................................................................3
2.2 Mô hình toán học .........................................................................................4
2.3 Thuật toán điều khiển...................................................................................9
2.3.1 Bộ lọc Kalman....................................................................................9
2.3.2 Lý thuyết điều khiển.........................................................................12
CHƯƠNG 3: TÍNH TOÁN VÀ THIẾT KẾ...........................................................15
3.1 Mô phỏng mô hình bằng Matlab Simulink................................................15
CHƯƠNG 4: THI CÔNG HỆ THỐNG..................................................................21
4.1 Phần cứng...................................................................................................21
4.1.1 Bộ điều khiển hệ thống.....................................................................21
4.1.2 Nguồn...............................................................................................21
4.1.3 Vi điều khiển ....................................................................................21
4.1.4 Cảm biến gia tốc...............................................................................23
4.1.5 Cầu H Hbr-H 200W (bộ điều khiển động cơ)..................................24

iii
4.1.6 Bánh xe.............................................................................................25
4.1.7 Động cơ NF5475e ............................................................................26
4.1.8 Mạch hạ áp DC-DC (LM2596)........................................................27
4.1.9 Mạch thu phát Bluetooth HC-05......................................................28
4.2 Sơ đồ nối dây .............................................................................................28
4.3 Lưu đồ điều khiển ......................................................................................29
4.4 Phần mềm...................................................................................................30
4.4.1 Matlab Simulink...............................................................................30
4.4.2 Arduino IDE.....................................................................................31
4.4.3 MIT App Inventor ............................................................................32
4.5. Mô hình thực tế .........................................................................................34
CHƯƠNG 5: KẾT QUẢ.........................................................................................35
5.1 Kết quả mô phỏng......................................................................................35
5.2 Kết quả mô hình thực tế.............................................................................36
CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN .......................................40
6.1 Kết luận......................................................................................................40
6.2 Giới hạn......................................................................................................40
6.3 Hướng phát triển: .......................................................................................40
TÀI LIỆU THAM KHẢO ......................................................................................41
PHỤ LỤC................................................................................................................42

iv
LIỆT KÊ HÌNH VẼ
Hình 2.1 . Con lắc ngược..........................................................................................3
Hình 2.2 . Mô hình của xe hai bánh tự cân bằng......................................................4
Hình 2.3 . Quy trình hoạt động của bộ lọc Kalman..................................................9
Hình 2.4 . Mô tả đơn giản của một hệ thống điều khiển ........................................12
Hình 2.5 . Bộ diều khiển LQR................................................................................14
Hình 3.1 . Sơ đồ khối robot tự cân bằng.................................................................15
Hình 3.2 . Sơ đồ bộ điều khiển LQR cho robot tự cân bằng ..................................20
Hình 4.1 . Bộ điều khiển hệ thống..........................................................................18
Hình 4.2 . Pin Lipo .................................................................................................21
Hình 4.3 . Arduino Mega 2560...............................................................................19
Hình 4.4 . Sơ đồ khối họ MPU-6000......................................................................20
Hình 4.5 . Gia tốc kế..............................................................................................23
Hình 4.6 . MPU 6050..............................................................................................21
Hình 4.7 . Module HBR – 200W............................................................................25
Hình 4.8 . Bánh xe ..................................................................................................26
Hình 4.9 . Động cơ NF5475e..................................................................................22
Hình 4.10 . Mạch hạ áp...........................................................................................23
Hình 4.11 . Mạch thu phát HC-05 ..........................................................................28
Hình 4.12 . Sơ đồ nối dây.......................................................................................29
Hình 4.13 . Lưu đồ hệ thống...................................................................................30

v
Hình 4.14 . Giao diện Editor của Matlab................................................................31
Hình 4.15 . Giao diện Simulink..............................................................................31
Hình 4.16 . Giao diện Arduino IDE........................................................................28
Hình 4.17 . Giao diện điều khiển thiết kế trên web AppInventor...........................28
Hình 4.18 . Code điều khiển trên AppInventor (1).................................................29
Hình 4.19 . Code điều khiển trên AppInventor (2).................................................29
Hình 4.20 . Mô hình thực tế ...................................................................................30
Hình 5.1 . Đồ thị mô phỏng góc tới θ.....................................................................31
Hình 5.2 . Đồ thị mô phỏng góc nghiêng ψ............................................................31
Hình 5.3 . Đồ thị mô phỏng góc xoay φ .................................................................31
Hình 5.4 . Đồ thị thực tế θ ......................................................................................32
Hình 5.5 . Đồ thị thực tế ψ......................................................................................33
Hình 5.6 . Đồ thị thực tế φ ......................................................................................33
Hình 5.7 . Đồ thị thực tế PWM...............................................................................34

vi
LIỆT KÊ BẢNG
Bảng 2.1 . Thông số mô hình xe hai bánh tự cân bằng.............................................4
Bảng 3.1 . Bảng thông số thực tế của xe ................................................................15
Bảng 4.1 . Thông số kĩ thuật Arduino Mega 2560 .................................................22
Bảng 4.2 . Thông số kĩ thuật MPU 6050 ................................................................24
Bảng 4.3 . Thông số động cơ NF5475E .................................................................26

vii
THÔNG TIN KẾT QUẢ NGHIÊN CỨU CỦA ĐỀ TÀI
1. Thông tin chung:
- Tên đề tài: Xe hai bánh tự cân bằng
- SV thực hiện: Phạm Ngọc Thái Mã số SV: 14151093
- Lớp: 141511 Khoa: Điện- Điện Tử Năm thứ: 4 Số năm đào tạo: 4
- SV thực hiện: Nguyễn Thành Phát Mã số SV: 14151077
- Lớp: 141511 Khoa: Điện- Điện Tử Năm thứ: 4 Số năm đào tạo: 4
- Người hướng dẫn: PGS. TS. Nguyễn Minh Tâm.
2. Mục tiêu đề tài:
Từ yêu cầu thực tế đào tạo của Nhà trường trong phát triển khoa học công nghệ,
đổi mới công tác đào tạo, nâng cao chất lượng đào tạo, nhìn thấy được tầm quan trọng
của một môi trường nghiên cứu với đầy đủ trang thiết bị, đề tài này được thức hiện
giúp cho Lab có thêm mô hình để thí nghiệm và đồng thời cũng là một tài liệu để các
bạn sinh viên khóa sau tiếp tục nghiên cứu và phát triển. Hoàn thành mô hình xe hai
bánh tự cân bằng có thể điều khiển được bằng Bluetooth là mục tiêu của đề tài này
3. Tính mới và sáng tạo:
Cùng với sự phát triển của thế giới khoa học công nghệ và từ những dẫn chứng
tình hình nghiên cứu trong và ngoài nước, ta thấy được tầm quan trong của việc nghiên
cứu, phát triển và ứng dụng một đề tài khoa học vào cuộc sống là rất cần thiết nhất là
đối với sinh viên. Hiện nay phòng thí nghiệm bộmôn điều khiển và tự động hóa
Trường Đại học Sư Phạm Kỹ Thuật đã có nhiều bộ thí nghiệm gồm: Bộ điều khiển tốc
độ động cơ, vị trí động cơ, hệ lò nhiệt, hệ bồn nước, Pendubot, Pendulum,.. Nhưng
hiện vẫn chưa có bộ thí nghiệm xe 2 bánh tự cân bằng điều khiển thông qua Bluetooth.

viii
Nhận thấy điều đó, nhóm quyết định thực hiện đề tài nghiên cứu khoa học về đề tài
trên.
4. Kết quả nghiên cứu:
Hoàn thành mô hình xe hai bánh tự cân bằng đúng thời gian qui định.
Mô hình hóa hệ thống, mô phỏng và điều khiển mô hình xe hai bánh tự cân bằng
xử dụng bộ điều khiển LQR trên Matlab Simulink
Xe tự cân bằng được và có thể điều khiển từ xa mà vẫn duy trì trạng thái cân bằng.
Nghiên cứu biết thêm về giải thuật LQR cũng như bộ lọc Kalman.
Vì kiến thức chuyên môn chưa sâu nên xe còn gặp một số hạn chế:
Chưa đồng bộ được dữ liệu từ Matlab tới vi điều khiển về giải thuật LQR.
Điều khiển tới lui chưa thật sự mượt vì hạn chế phần cứng.
Xe còn bị dao động mạnh khi tăng tải.
5. Đóng góp về mặt giáo dục và đào tạo, kinh tế - xã hội, an ninh, quốc phòng
và khả năng áp dụng của đề tài:
Ứng dụng tại phòng thí nghiệm bộ môn công nghệ kĩ thuật điều khiển và tự động
hóa trường Đại Học Sư Phạm Kỹ Thuật Tp. HCM
6. Công bố khoa học của SV từ kết quả nghiên cứu của đề tài (ghi rõ tên tạp chí
nếu có) hoặc nhận xét, đánh giá của cơ sở đã áp dụng các kết quả nghiên cứu (nếu có):
Mô hình hoạt động tốt, ổn định và được ứng dụng để chứng minh các giải thuật điều
khiển tự động cho sinh viên nghiên cứu khoa học.
Ngày tháng năm
SV chịu trách nhiệm chính
thực hiện đề tài
(kí, họ và tên)

ix
Nhận xét của người hướng dẫn về những đóng góp khoa học của SV thực hiện
đề tài (phần này do người hướng dẫn ghi):
Ngày tháng năm
Xác nhận của Trường
(kí tên và đóng dấu)
Người hướng dẫn
(kí, họ và tên)

x
TÓM TẮT
Ngày nay, Robot đã có những đóng góp to lớn cho sản xuất công nghiệp cũng như
cho cuộc sống. Sản xuất Robot là một ngành công nghiệp nhiều tỷ đô la được đánh giá
cao về mặt kinh tế và góp phần hiện đại hóa tự động hóa sản xuất. Nhiều robot có thể
di chuyển ở tốc độ cao và độ chính xác cao để thực hiện các công việc lặp đi lặp lại
khiến người lao động cảm thấy buồn chán. Trong thiết bị điện tử, rô bốt có nhiều thành
phần với độ chính xác cao, tạo ra các thiết bị tinh vi như điện thoại di động hoặc máy
tính cá nhân và máy tính.
Xe hai bánh tự cân bằng là một mô hình phổ biến trong các thử nghiệm tự động.
Đây là một hệ thống MIMO (nhiều đầu vào và nhiều đầu ra) lý thuyết, thiết thực và đã
được áp dụng trong cuộc sống bình thường. Dù sao, hầu hết các nghiên cứu chỉ tập
trung vào việc cân bằng mô hình này thông qua các thử nghiệm hoặc dạng đơn giản
của mô hình toán học. Vẫn còn rất ít nghiên cứu tập trung vào thuật toán mô hình hóa
và thiết kế một bộ điều khiển dựa trên mô hình toán học cho hệ thống này. Dự án này
phân tích phương trình toán học của hệ thống. Sau đó áp dụng điều khiển tối ưu cho hệ
thống này.
Trong Robotics, phương pháp LQR rất hữu ích khi chúng em tuyến tính hóa các
hệ phi tuyến về các điểm cân bằng được xác định. Trong các hệ phi tuyến cao, xấp xỉ
các điều kiện bậc cao hơn với thứ gì đó đơn giản như hệ thống tuyến tính bậc 2, có
nghĩa là cải thiện đáng kể mức độ có thể điều khiển hệ thống trong vùng vận hành của
hệ thống. Phương pháp điều khiển này rất mạnh mẽ có thể làm cho robot di chuyển
trơn tru dưới một số điều kiện của bề mặt đường. Adruino Mega là trung tâm điều
khiển của hệ thống bao gồm động cơ, bộ điều khiển động cơ, cảm biến MPU 6050.
Sự thành công của dự án này có thể mở ra các hướng nghiên cứu mới cho việc tạo
ra các robot hai chân hoặc một robot người đứng hai chân. Trong tương lai, công nghệ
cân bằng sẽ phổ biến hơn cũng như quan trọng với cuộc sống của chúng ta. Ví dụ,
chúng ta có thể tránh được nhiều tai nạn bằng cách sử dụng cân bằng xe máy. Các
hướng phát triển của dự án này rất rộng lớn và tiềm năng.

CHƯƠNG 1. TỔNG QUAN
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 1
CHƯƠNG 1: TỔNG QUAN
1.1 Đặt vấn đề
Hiện nay việc ùn tắc giao thông và ô nhiễm môi trường tại các thành phố lớn do
các phương tiện giao thông cá nhân gây ra đang là vấn đề được quan tâm hàng đầu. Vì
vậy tìm hiểu và chế tạo ra các loại phương tiện di chuyển cá nhân ít chiếm diện tích
không gian và không tạo ra khí thải gây ô nhiễm môi trường đang được nhiều tác giả
quan tâm nghiên cứu. Song với sự nghiên cứu con người đã đưa ra một sản phẩm được
gọi là xe hai bánh cân bằng, giúp con người di chuyển linh hoạt hơn, ít chiếm không
gian và thân thiện với môi trường. Nhận thấy được sự thiết thực và hấp dẫn của đề tài
cũng như có liên quan đến những kiến thức được học tại trường Đại Học, nhóm chúng
em quyết định thực hiện đề tài này với mong muốn tìm hiểu về loại hình di chuyển
thông minh này.
1.2 Mục tiêu đề tài
Mục tiêu của đề tài là xây dựng mô hình robot cân bằng hai bánh dựa trên lý
thuyết con lắc ngược. Các mục tiêu bao gồm:
 Đưa ra mô hình của robot và nguyên lý hoạt động của tự cân bằng.
 Tính toán các thông số động, phương trình trạng thái không gian của mô hình.
 Tính toán, chọn cảm biến, bộ điều khiển động cơ và bộ điều khiển trung tâm.
Trong đề tài, cảm biến sử dụng là MPU 6050, bộ điều khiển trung tâm là
Arduino (Arduino Mega 2560).
 Mô phỏng mô hình trên Matlab Simulink.
 Nghiên cứu và ứng dụng bộ lọc Kalman để lọc nhiễu từ cảm biến.
 Đưa ra thuật toán điều khiển động cơ và giữ cân bằng cho robot.
 Robot có thể hoạt động một cách tự động.

CHƯƠNG 1. TỔNG QUAN
1.3 Nội dung của đề tài
 Thiết kế và thi công phần cứng cho xe hai bánh tự cân bằng.
 Kết hợp MPU6050 và bộ lọc Kalman nhằm xác định góc nghiêng của xe.
 Thuật toán LQR được sử dụng để điều khiển xe cân bằng và di chuyển.
 Phương thức giao tiếp UART, I2C.
1.4 Giới hạn của đề tài
Mô hình xe hai bánh cân bằng chỉ là mô hình thử nghiệm giải thuật điều khiển
LQR, ứng dụng bộ lọc Kalman để xử lý tín hiệu phản hồi của cảm biến. Do đó khối
lượng của xe giảm xuống nhỏ nhất có thể, thiết kế xe sao cho trọng tâm càng thấp càng
tốt để xe có thể dễ dàng đạt cân bằng. Tác động ngoại lực lên xe chỉ nằm trong mức
giới hạn cho phép để xe có thể dễ dàng quay lại vị trí cân bằng (ví dụ góc nghiêng giới
hạn để xe có thể quay lại vị trí cân bằng, ngoại lực mạnh hoặc yếu ảnh hưởng đến tốc
độ thay đổi các giá trị đặt...)
1.5 Ý nghĩa thực tiễn và khoa học của đề tài
Robot hai bánh có thể sử dụng thay con người trong thăm dò… Từ những nghiên
cứu về robot hai bánh tự cân bằng ta có thể phát triển mô hình robot hai bánh tự cân
bằng thành xe hai bánh tự cân bằng sử dụng trong giáo thông vân tải. Xe hai bánh tự
cân bằng có khả năng tự cân bằng cả khi đứng yên, khi chuyển động và cả khi xảy ra
va chạm. Xe hai bánh tự cân bằng nếu được thiết kế tốt thì khi va chạm nó chỉ bị văng
ra và vẫn giữ được phương thẳng đứng nhờ hệ thống tự cân bằng lắp trên nó do đó sẽ
đảm bảo an toàn cho người sử dụng. Do đó, nghiên cứu về robot hai bánh tự cân bằng
có tính khoa học và thực tiễn rất lớn.

CHƯƠNG 2. CƠ SỞ LÝ THUYẾT
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT
Robot cân bằng hai bánh là một hệ thống động không ổn định, có nghĩa là robot
được tự do ngã về phía trước hoặc phía sau mà không có bất kỳ ngoại lực nào tác
động. Tự cân bằng có nghĩa là robot ở trạng thái vị trí của nó giống như đứng thẳng 90
độ. Tuy nhiên, bản thân hệ thống không phải tự cân bằng, có nghĩa là nó tiếp tục rời
khỏi trục thẳng đứng. Do đó, một sự kết hợp của con quay hồi chuyển và gia tốc là cần
thiết để đọc vị trí góc của robot và nhập vào bộ vi điều khiển, thực hiện quy trình cân
bằng.
2.1 Con lắc ngược
Như đã nói ở trên, robot tự cân bằng dựa trên lý thuyết con lắc ngược. Con lắc
ngược là một hệ thống vốn không ổn định buộc phải được áp dụng đúng cách để giữ
nguyên hệ thống. Để đạt được điều này, lý thuyết điều khiển thích hợp là bắt buộc.
Một con lắc ngược là một con lắc có khối lượng của nó trên đầu trục của nó. Nó
thường được thực hiện với các điểm trục gắn trên một xe đẩy có thể di chuyển theo
chiều ngang và có thể được gọi là một xe đẩy và cực. Trong khi một con lắc bình
thường ổn định khi treo xuống và phải được chủ động cân bằng để duy trì trạng thái
thẳng đứng bằng cách áp dụng mô-men xoắn tại điểm trục hoặc bằng cách di chuyển
điểm trục theo chiều ngang như một phần của hệ thống phản hồi. Vì nó là một hệ
thống phi tuyến tính nên một con lắc ngược là một trong những hệ thống khó kiểm
soát nhất trong lĩnh vực kiểm soát kỹ thuật.
Hình 2.1. Con lắc ngược
Con lắc Con lắc ngược

2.2 Mô hình toán học
Hệ xe bai bánh tự cân bằng được thể hiện ở hình 2.2 bên dưới
Hình 2.2 Mô hình của xe hai bánh tự cân bằng
Bảng 2.1 Thông số mô hình xe hai bánh tự cân bằng
Kí
hiệu
Đơn
vị
Ý nghĩa Kí hiệu Đơn
vị
Ý nghĩa
M kg Khối lượng bánh xe N m/s2
Tỷ lệ giảm tốc
M kg Khối lượng robot  rad Góc trung bình của 2
bánh xe
R m Bán kính bánh xe G m/s2
Gia tốc trọng trường
W m Chiều rộng robot
,
l r
 rad Góc bánh trái, bánh phải
D m Chiều dài robot  rad Góc nghiêng robot
H m Chiều cao robot  rad Góc xoay robot
L m Khoảng cách từ trọng
tâm đến trục bánh xe
xl,yl,zl m Tọa độ bánh trái
fw Hệ số ma sát giữa robot
với mặt sàn
xr,yr,zr m Tọa độ bánh phải

Bằng cách sử dụng phương pháp Euler-Lagrange để tạo mô hình động, giả sử rằng
tại thời điểm t = 0, robot di chuyển về phía trước tới trục x (hướng dương), chúng ta có
các phương trình:
Góc dịch trung bình của hai bánh xe và góc quay của robot được xác định:
[
𝜃
𝜙
] = [
1
2
(𝜃𝑙+𝜃𝑟)
𝑅
𝑊
(𝜃𝑙−𝜃𝑟)
] (2.1)
Vị trí trung bình của robot trong hệ qui chiếu:
[
𝑥𝑚
𝑦𝑚
𝑧𝑚
] =
[
∫ 𝑥̇𝑚
∫ 𝑦̇𝑚
𝑅 ]
(2.2)
Và
[
𝑥̇𝑚
𝑦̇𝑚
] = [
𝑅𝜃̇𝑐𝑜𝑠𝜙
𝑅𝜃̇𝑠𝑖𝑛𝜙
] (2.3)
Vị trí trung bình của bánh trái trong hệ qui chiếu:
fm Hệ số ma sát giữa robot
và động cơ
xm,ym,zm m Tọa độ trung bình
Jm kgm2
Moment quán tính của
động cơ
, ,
F F F
   Nm Moment phát động theo
các phương khác nhau
Rm  Trở kháng động cơ Fl,r Nm Moment phát động của
bánh trái và bánh phải
Kb Vs/rad Hệ số EFM của động cơ il,ir A Dòng điện của động cơ
Kt Nm/A Moment xoắn của động
cơ
vl,vr V Điện áp động cơ

[
𝑥𝑙
𝑦𝑙
𝑧𝑙
] =
(
𝑥𝑚 −
𝑊
2
𝑐𝑜𝑠𝜙
𝑦𝑚 +
𝑊
2
𝑠𝑖𝑛𝜙
𝑧𝑚 )
(2.4)
Vị trí trung bình của bánh phải trong hệ qui chiếu:
[
𝑥𝑟
𝑦𝑟
𝑧𝑟
] =
(
𝑥𝑚 +
𝑊
2
𝑐𝑜𝑠𝜙
𝑦𝑚 −
𝑊
2
𝑠𝑖𝑛𝜙
𝑧𝑚 )
(2.5)
Tọa độ tâm đối xứng giữa hai động cơ trong hệ qui chiếu:
[
𝑥𝑏
𝑦𝑏
𝑧𝑏
] = [
𝑥𝑚 + 𝐿𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜙
𝑦𝑚𝐿𝑠𝑖𝑛𝜓𝑠𝑖𝑛𝜙
𝑧𝑚 + 𝐿𝑐𝑜𝑠𝜓
] (2.6)
Phương trình động năng của chuyển động tịnh tiến:
𝑇1 =
1
2
𝑚(𝑥̇𝑙
2
+ 𝑦̇𝑙
2
+ 𝑧̇𝑙
2
) +
1
2
𝑚(𝑥̇𝑟
2
+ 𝑦̇𝑟
2
+ 𝑧̇𝑟
2
)
+
1
2
𝑚(𝑥̇𝑏
2
+ 𝑦̇𝑏
2
+ 𝑧̇𝑏
2
)
(2.7)
Phương trình động năng của chuyển động xoay:
𝑇2 =
1
2
𝐽𝑤𝜃̇𝑙
2
+
1
2
𝐽𝑤𝜃̇𝑟
2
+
1
2
𝐽𝜓𝜓̇2
+
1
2
𝐽𝜙𝜙̇2
+
1
2
𝑛2
𝐽𝑚(𝜃̇𝑙 − 𝜓̇)2
+
1
2
𝑛2
𝐽𝑚(𝜃̇𝑟 − 𝜓̇)2
(2.8)
Trong đó:
1
2
𝑛2
𝐽𝑚(𝜃̇𝑙 − 𝜓̇)2
;
1
2
𝑛2
𝐽𝑚(𝜃̇𝑟 − 𝜓̇)2 (2.9)
Là động năng quay của phần ứng động cơ trái và phải.
Phương trình thế năng:
𝑈 = 𝑚𝑔𝑧𝑙 + 𝑚𝑔𝑧𝑟 + 𝑚𝑔𝑧𝑏 (2.10)

Phương trình Lagrange:
L = T1 + T2 − U (2.11)
d
dt
(
∂L
∂θ̇
) −
∂L
∂θ
= Fθ
(2.12)
d
dt
(
∂L
∂ψ
̇
) −
∂L
∂ψ
= Fψ
(2.13)
d
dt
(
∂L
∂ϕ
̇
) −
∂L
∂ϕ
= Fϕ
(2.14)
Lấy đạo hàm L theo các biến ta được:
[(2𝑚 + 𝑀)𝑅2
+ 2𝐽𝑤 + 2𝑛2
𝐽𝑚]𝜃̈ + (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2
𝐽𝑚)𝜓̈
− 𝑀𝐿𝑅𝜓̇2
𝑠𝑖𝑛𝜓 = 𝐹𝜃 (2.15)
(𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2
𝐽𝑚)𝜃̈ + (𝑀𝐿2
+ 𝐽𝜓 + 2𝑛2
𝐽𝑚)𝜓̈ − 𝑀𝑔𝐿𝑠𝑖𝑛𝜓
− 𝑀𝐿2
𝜙̇2
𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = 𝐹𝜓
(2.16)
[
1
2
𝑚𝑊2
+ 𝐽𝜙 +
𝑊2
2𝑅2
(𝐽𝑤 + 𝑛2
𝐽𝑚) + 𝑀𝐿2
𝑠𝑖𝑛2
𝜓] 𝜙̈2
+ 2𝑀𝐿2
𝜓̇𝜙̇ 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = 𝐹𝜙
(2.17)
Moment động lực của động cơ:
[
𝐹𝜃
𝐹𝜓
𝐹𝜙
] = [
𝐹𝑙 + 𝐹𝑟
𝐹𝜓
𝑊
2𝑅
(𝐹𝑙 − 𝐹𝑟)
]
Và:
(2.18)
Fl = nKtil + fm(ψ
̇ − θ̇l) − fwθ̇l (2.19)
𝐹𝑟 = 𝑛𝐾𝑡𝑖𝑟 + 𝑓𝑚(𝜓̇ − 𝜃̇𝑟) − 𝑓𝑤𝜃̇𝑟 (2.20)

𝐹𝜓 = −𝑛𝐾𝑡𝑖𝑙 − 𝑛𝐾𝑡𝑖𝑟 − 𝑓𝑚(𝜓̇ − 𝜃̇𝑙) − 𝑓𝑚(𝜓̇ − 𝜃̇𝑟) (2.21)
Sử dụng phương pháp PWM để điều khiển động cơ, nên ta phải chuyển từ dòng
điện sang điện áp:
𝐿𝑚𝑖̇̇𝑙,𝑟 = 𝑣𝑙,𝑟 + 𝐾𝑏(𝜓̇ − 𝜃̇𝑙,𝑟) − 𝑅𝑚𝑖𝑙,𝑟 (2.22)
Giả sử điện cảm phần ứng rất nhỏ, có thể bỏ qua (gần bằng 0) suy ra:
𝑖𝑙,𝑟 =
𝑣𝑙,𝑟 + 𝐾𝑏(𝜓̇ − 𝜃̇𝑙,𝑟)
𝑅𝑚
(2.23)
Các moment lực sinh ra:
𝐹𝜃 = 𝛼(𝑣𝑙 + 𝑣𝑟) − 2(𝛽 + 𝑓𝑤)𝜃̇ + 2𝛽𝜓̇ (2.24)
𝐹𝜓 = −𝛼(𝑣𝑙 + 𝑣𝑟) + 2𝛽𝜃̇ − 2𝛽𝜓̇ (2.25)
Trong đó:
𝛼 =
𝑛𝐾𝑡
𝑅𝑚
và 𝛽 =
𝑛𝐾𝑡𝐾𝑏
𝑅𝑚
+ 𝑓𝑚 (2.26)
𝐹𝜙 =
𝑊
2𝑅
𝛼(𝑣𝑟 − 𝑣𝑙) −
𝑊2
2𝑅2
(𝛽 + 𝑓𝑤)𝜙̇ (2.27)
Ta thu được phương trình động lực học mô tả chuyển động của hệ thống:
[(2𝑚 + 𝑀)𝑅2
+ 2𝐽𝑤 + 2𝑛2
𝐽𝑚)𝜓̈
𝑠𝑖𝑛𝜓 = 𝛼(𝑣𝑙 + 𝑣𝑟) − 2(𝛽 + 𝑓𝑤)𝜃̇ + 2𝛽𝜓̇
(2.28)
(𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2
+ 𝐽𝜓 + 2𝑛2
− 𝑀𝐿2
𝜙̇2
𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = −𝛼(𝑣𝑙 + 𝑣𝑟) + 2𝛽𝜃̇ − 2𝛽𝜓̇
(2.29)
[
1
2
𝑚𝑊2
+ 𝐽𝜙 +
𝑊2
2𝑅2
(𝐽𝑤 + 𝑛2
𝑠𝑖𝑛2
𝜓] 𝜙̈2
+ 2𝑀𝐿2
𝜓̇𝜙̇ 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓
=
𝑊
2𝑅
𝛼(𝑣𝑟 − 𝑣𝑙) −
𝑊2
2𝑅2
(𝛽 + 𝑓𝑤)𝜙̇
(2.30)

2.3 Thuật toán điều khiển
2.3.1 Bộ lọc Kalman
2.3.1.1: Giới thiệu
Năm 1960, R.E Kalman đã xuất bản một bài báo có tựa đề 'A New Approach to
Linear Filtering and Prediction Problems' (Kalman, 1960). Nghiên cứu của Kalman
nhằm khắc phục những hạn chế của bộ lọc 'Weiner-Hopf' trong việc giải quyết các vấn
đề về bản chất thống kê, làm hạn chế sự hữu dụng thực tế của nó. Quá trình được mô
tả bên trong được gọi là lọc Kalman.
Bộ lọc Kalman là một tập hợp các phương trình toán học cung cấp một giải pháp tính
toán hiệu quả của phương pháp bình phương nhỏ nhất. Bộ lọc rất mạnh mẽ vì nó hỗ
trợ các ước tính của các trạng thái trong quá khứ, hiện tại và thậm chí trong tương lai,
và nó có thể làm như vậy ngay cả khi bản chất chính xác của hệ thống được mô hình
hóa không xác định.
Bộ lọc Kalman ước tính một quá trình bằng cách sử dụng một hình thức kiểm soát
phản hồi. Các trạng thái của quá trình được ước tính bởi bộ lọc tại một điểm nhất định
và sau đó có được một phản hồi về các phép đo nhiễu. Do đó, các phương trình cho bộ
lọc Kalman rơi vào hai nhóm, phương trình cập nhật thời gian và phương trình cập
nhật đo lường.
Các ước tính hiệp phương sai hiện tại và lỗi hiện tại được truyền bằng các phương
trình cập nhật thời gian để có được ước tính được ưu tiên cho bước thời gian tiếp theo.
Mặt khác, các phương trình cập nhật đo lường có trách nhiệm kết hợp một phép đo
mới vào ước lượng trước để có được ước tính sau được cải thiện. Các phương trình
cập nhật thời gian được mô hình hóa như các phương trình dự báo, trong khi các phép
đo cập nhật các phương trình được thực hiện như các phương trình hiệu chỉnh. Hoạt
động của bộ lọc được mô tả như hình dưới đây:
Hình 2.3 Quy trình hoạt động của bộ lọc Kalman
Dự báo Chỉnh định
điịnh

2.3.1.2 Phương trình trạng thái
Bộ lọc Kalman có thể được áp dụng để ước lượng các trạng thái khi hệ thống được
mô hình đầy đủ dưới dạng một phương trình vi phân ngẫu nhiên tuyến tính. Nói cách
khác, bộ lọc Kalman có thể được sử dụng để ước tính trạng thái của một hệ thống khi
hệ thống có thể được mô hình hóa trong dạng không gian trạng thái. Phương trình
(2.31) đại diện cho quá trình trạng thái của hệ thống trong ký hiệu nhiễu trắng.
x(t+1) = F (t)x(t) + B(t)u(t) + w(t) (2.31)
F(t): mô hình chuyển đổi trạng thái
B(t): mô hình tín hiệu điều khiển
w(t): nhiễu quá trình có phân bố Gaussian với trung bình 0 và ma trận hiệp
phương sai Q tại thời điểm k
wk ~ N (0, Qk)
Bộ lọc Kalman không giả định rằng các lỗi là Gaussian. Tuy nhiên, bộ lọc tạo ra
ước tính xác suất có điều kiện chính xác trong trường hợp đặc biệt là tất cả các lỗi đều
được phân phối Gaussian.
2.3.1.3 Phương trình đo
Các phép đo cho hệ thống được xác định tại các điểm thời gian rời rạc, các phép
đo này được bao gồm trong vector đo lường và có thể được mô hình hóa theo quan hệ.
z(ti ) = H (ti )x(ti ) + v(ti ) (2.32)
H: mô hình quan sát
v(t): nhiễu quá trình có phân bố Gaussian với trung bình 0 và ma trận hiệp phương
sai R tại thời điểm k
vk ~ N(0,Rk)
Phương trình (2.32) nói rằng các phép đo phụ thuộc vào trạng thái của hệ thống và
có liên quan bởi ma trận đo với sự cộng thêm nhiễu vào các phép đo. Các phép đo cho
hệ thống thường thu được ở khoảng cách đều nhau, nhưng điều này không bắt buộc.

2.3.1.4 Phương trình bộ lọc Kalman
Xem xét hai lần đo, ti-1 và ti, và ước lượng thời gian truyền từ điểm ngay sau khi
đo tại thời điểm ti-1 đã được đưa vào ước tính, đến thời điểm sau khi kết hợp phép đo
tại thời điểm ti. Điều này được biểu thị bằng thời gian ti
+
−1 đến thời gian ti
+
. Ước
lượng trạng thái tối ưu được truyền từ thời gian đo ti-1 đến thời gian đo ti bằng các quan
hệ.
xˆ(t−
) = Fxˆ (t +
) + Bu (t+
) (2.33)
P(ti−
) = FP (t +
)F T
+ Q (2.34)
Phương trình (2.34) xác định ma trận hiệp phương sai có điều kiện của
lỗi là dự đoán trạng thái x. Khi trạng thái đo zi có sẵn tại thời điểm ti, ước
tính được cập nhật bằng cách xác định độ lợi bộ lọc Kalman k(ti) và sử
dụng nó trong cả quan hệ trung bình và hiệp phương sai.
k(ti ) = P(ti
−
)H T
(ti )[H (ti )P(ti
−
)H T
(ti ) + R(ti )]−1
(2.35)
xˆ(ti
+
) = xˆ(ti
−
) + k(ti )[zi − H (ti )xˆ(ti
−
)] (2.36)
P(ti
+
) = P(ti
−
) − k(ti )H (ti )P(ti
−
) (2.37)
Ước tính lỗi là các lỗi do bộ ước lượng xˆ(ti
−
) đưa ra cho một phép đo cụ thể
được cho bởi
E = H (ti )P(ti
−
)H T
(ti ) + R(ti ) (2.38)
Ngoài ra, phương trình (4.10) có thể được viết lại thành
k(ti ) = P(ti
+
)H T
(ti )E−1
(ti
−
) (2.39)
Sự khác biệt giữa phép đo thực tế zi và dự đoán tốt nhất của nó trước khi nó thực
sự được thực hiện được gọi là số dư còn lại r(ti ). Điều này được xác định bởi phương
trình sau
r(ti ) = zi − H (ti )xˆ(ti
−
) (2.40)

Phần còn lại sau đó được truyền qua một ma trận trọng số tối ưu k(ti ) để tạo ra
thuật ngữ hiệu chỉnh được thêm vào xˆ(ti
−
) để thu được xˆ(ti
+
).
2.3.2 Lý thuyết điều khiển
2.3.2.1 Giới thiệu LQR
Hệ thống điều khiển là một phần không thể thiếu của xã hội hiện đại. Một hệ
thống điều khiển bao gồm các hệ thống con và các quy trình (hoặc các dây chuyền)
được lắp ráp với mục đích thu được một đầu ra mong muốn với hiệu suất mong muốn,
với một đầu vào được chỉ định. Hình dưới đây cho thấy một hệ thống điều khiển ở
dạng đơn giản nhất của nó, nơi đầu vào đại diện cho một đầu ra mong muốn.
Hình 2.4 Mô tả đơn giản của một hệ thống điều khiển
Một bộ điều khiển, trong lý thuyết điều khiển, là một thiết bị quan sát và điều
khiển các đầu vào của một hệ thống để có được đầu ra mong muốn. Ví dụ, các cảm
biến của một cánh cửa tự động (bộ điều khiển) sẽ mở cửa khi một người đến gần.
Trong trường hợp này, bộ điều khiển được gọi là bộ điều khiển vòng mở vì không
quan tâm đến bất kỳ lực bất ngờ nào tác động lên hệ thống. Để giải quyết vấn đề này,
lý thuyết kiểm soát đưa ra sự phản hồi. Bộ điều khiển vòng kín sử dụng phản hồi để
điều khiển đầu ra của hệ thống động. Trong Robotics, phương pháp điều chỉnh bậc hai
tuyến tính (LQR) rất hữu ích khi tuyến tính hóa các hệ phi tuyến về các điểm cân bằng
được xác định. Trong các hệ phi tuyến cao, ta có thể cải thiện đáng kể mức độ có thể
điều khiển một hệ thống trong các vùng hoạt động của hệ thống mà ta mong muốn.
LQR là một phương pháp hiệu quả và thông dụng trong thiết kế bộ điều khiển tối
ưu động. Thuật toán LQR về cơ bản là một cách tự động để tìm một bộ điều khiển
Hệ thống
điều khiển
Input: điều kiện Output: đáp ứng
Đáp ứng mong muốn Đáp ứng thực tế

phản hồi trạng thái thích hợp sao cho các kết quả đầu ra trong tương lai được dự đoán
ở mọi thời điểm đều đáp ứng mục tiêu tối thiểu chỉ tiêu chất lượng dạng toàn phương
của một phiếm hàm. Bằng cách ước tính kết quả đầu ra trong tương lai dựa trên kết
quả đầu ra trong quá khứ trước đó, ta có thể điều chỉnh các tham số tốt hơn trong việc
theo dõi. Ưu điểm chính của phương pháp này là tín hiệu đầu vào tối ưu u(t) thu được
từ trạng thái phản hồi đầy đủ. Ma trận phản hồi K được đề cập thu được bằng cách giải
phương trình Ricatti liên quan đến phương pháp LQR. Một trong những nhược điểm
của bộ điều khiển LQR là có phương pháp biến phân cho phương trình Ricatti là khá
khó trong tất cả các trường hợp đơn giản nhất.
2.3.2.2 Thuật toán LQR
Điều khiển LQR thuộc điều khiển tối ưu với chỉ tiêu chất lượng dạng toàn phương
và thời gian vô hạn. Đối tượng tuyến tính mô tả bởi phương trình trạng thái:
𝑥̇(𝑡) = 𝐴𝑥(𝑡) + 𝐵𝑢(𝑡)
𝑥(𝑡) = [𝑥1(𝑡), 𝑥2(𝑡), … , 𝑥𝑛(𝑡)]𝑇
𝑢(𝑡) = [𝑢1(𝑡), 𝑢2(𝑡), … , 𝑢𝑚(𝑡)]𝑇
Chỉ tiêu chất lượng dạng toàn phương, trong đó thời điểm cuối là vô hạn.




0
)
(
)
(
'
)
(
)
(
2
1
dt
t
Ru
t
u
t
Qx
t
x
J T
Trong đó Q: ma trận trọng số bán xác định dương của tín hiệu đầu ra.
R: ma trận trọng số xác định dương của tín hiệu điều khiển.
Tín hiệu điều khiển tối ưu:
)
(
)
( t
Kx
t
u 

Trong đó:
P
B
R
K T
1


và P là nghiệm bán xác định dương của phương trình đại số Ricatti:

0
1



 
P
B
PBR
Q
P
A
PA T
T
Trong trường hợp này K và P không phụ thuộc vào thời gian. Giá trị cực tiểu của
chỉ tiêu chất lượng: Jmin = xT(0)Px(0)
Vector K thu được xác định thông số điều khiển đưa trở lại vào hệ thống. Ma trận
Q và R làm cân bằng hơn tính ổn định của hệ thống với điều kiện các phần tử trong hai
ma trận là các giá trị dương. Số phần tử của ma trận Q phụ thuộc vào số phần tử của
ma trận trạng thái và ma trận R phụ thuộc vào số lượng đầu vào điều khiển hệ thống.
Sơ đồ cho bộ điều khiển LQR:
Hình 2.5 Bộ diều khiển LQR

CHƯƠNG 3. TÍNH TOÁN VÀ THIẾT KẾ
CHƯƠNG 3: TÍNH TOÁN VÀ THIẾT KẾ
3.1 Mô phỏng mô hình bằng Matlab Simulink
Thứ nhất, chúng ta xem xét mô hình robot tự cân bằng:
Hình 3.1 Sơ đồ khối robot tự cân bằng
Bảng 3.1 Bảng thông số thực tế của xe
M 0.12 H 0.145
M 2.16 L 0.1
R 0.04 Kb 0.468
W 0.225 Kt 0.507
D 0.08 Jw 0.5*m*R2
Rm 50 Jm 0.01
fm 0.002 alpha n*Kt/Rm
fw 0.18 Beta N*Kb*Kt/Rm
+fm
N 40 J_psi M*L2
/3

G 9.81 J_phi M*(W2
+D2
)/
12
T 0.01
Từ cơ sơ lý thuyết ta có phương trình động lực học của hệ thống:
𝐹1 = [(2𝑚 + 𝑀)𝑅2
+ 2𝐽𝑤 + 2𝑛2
𝐽𝑚)𝜓̈
𝑠𝑖𝑛𝜓 − 𝛼(𝑣𝑙 + 𝑣𝑟) + 2(𝛽 + 𝑓𝑤)𝜃̇ − 2𝛽 𝜓̇ = 0
(3.1)
𝐹2 = (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2
+ 𝐽𝜓 + 2𝑛2
− 𝑀𝐿2
𝜙̇2
𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 + 𝛼(𝑣𝑙 + 𝑣𝑟) − 2𝛽𝜃̇ + 2𝛽𝜓̇ = 0
(3.2)
𝐹3 = [
1
2
𝑚𝑊2
+ 𝐽𝜙 +
𝑊2
2𝑅2
(𝐽𝑤 + 𝑛2
𝑠𝑖𝑛2
𝜓] 𝜙̈2
+
2𝑀𝐿2
𝜓̇𝜙̇ 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 −
𝑊
2𝑅
𝛼(𝑣𝑟 − 𝑣𝑙) +
𝑊2
2𝑅2
(𝛽 + 𝑓𝑤)𝜙̇ = 0
(3.3)
Ta tiến hành hạ bậc hệ thống, đưa về dạng:
)
,
,
,
,
,
(
.
.
.
..






 f

)
,
,
,
,
,
(
.
.
.
..






 f

)
,
,
,
,
,
(
.
.
.
..






 f


Với
x1 = 
x2 =
.

x3 =
..

x4 = 
x5 =
.

x6 =
..

x7 = 
x8 =
.

x9 =
..

Thông qua lệnh solve trong Matlab:
[x3, x6, x9] = solve (F1, F2, F3, ’x3’, ’x6’, ’x9’)
x3 = (J_psi*alpha*vl + J_psi*alpha*vr - 2*J_psi*beta*x2 + 2*J_psi*beta*x5 -
2*J_psi*fw*x2 + L^2*M*alpha*vl + L^2*M*alpha*vr-
2*L^2*M*beta*x2+2*L^2*M*beta*x5 - 2*L^2*M*fw*x2 - 4*Jm*fw*n^2*x2 +
L^3*M^2*R*x5^2*sin(x4) - 2*L*M*R*beta*x2*cos(x4) +
2*L*M*R*beta*x5*cos(x4) - L^3*M^2*R*x8^2*cos(x4)^2*sin(x4) -
L^2*M^2*R*g*cos(x4)*sin(x4) + J_psi*L*M*R*x5^2*sin(x4) +
2*Jm*L*M*g*n^2*sin(x4) + L*M*R*alpha*vl*cos(x4) + L*M*R*alpha*vr*cos(x4)
+ 2*Jm*L^2*M*n^2*x8^2*cos(x4)*sin(x4) +
2*Jm*L*M*R*n^2*x5^2*sin(x4))/(2*J_psi*Jw + L^2*M^2*R^2 + 2*Jw*L^2*M +
J_psi*M*R^2 + 2*J_psi*Jm*n^2 + 4*Jm*Jw*n^2 + 2*J_psi*R^2*m +
2*Jm*L^2*M*n^2 + 2*Jm*M*R^2*n^2 + 2*L^2*M*R^2*m + 4*Jm*R^2*m*n^2 -
L^2*M^2*R^2*cos(x4)^2 + 4*Jm*L*M*R*n^2*cos(x4))

X6 = -(2*Jw*alpha*vl + 2*Jw*alpha*vr - 4*Jw*beta*x2 + 4*Jw*beta*x5 +
M*R^2*alpha*vl + M*R^2*alpha*vr - 2*M*R^2*beta*x2 + 2*M*R^2*beta*x5 +
2*R^2*alpha*m*vl + 2*R^2*alpha*m*vr + 4*Jm*fw*n^2*x2 - 4*R^2*beta*m*x2 +
4*R^2*beta*m*x5 - L*M^2*R^2*g*sin(x4) - 2*Jw*L*M*g*sin(x4) -
2*L*M*R*beta*x2*cos(x4) + 2*L*M*R*beta*x5*cos(x4) -
2*L*M*R*fw*x2*cos(x4) + L^2*M^2*R^2*x5^2*cos(x4)*sin(x4) -
L^2*M^2*R^2*x8^2*cos(x4)*sin(x4) - 2*Jw*L^2*M*x8^2*cos(x4)*sin(x4) -
2*Jm*L*M*g*n^2*sin(x4) - 2*L*M*R^2*g*m*sin(x4) + L*M*R*alpha*vl*cos(x4)
+ L*M*R*alpha*vr*cos(x4) - 2*Jm*L^2*M*n^2*x8^2*cos(x4)*sin(x4) -
2*L^2*M*R^2*m*x8^2*cos(x4)*sin(x4) -
2*Jm*L*M*R*n^2*x5^2*sin(x4))/(2*J_psi*Jw + L^2*M^2*R^2 + 2*Jw*L^2*M +
J_psi*M*R^2 + 2*J_psi*Jm*n^2 + 4*Jm*Jw*n^2 + 2*J_psi*R^2*m +
2*Jm*L^2*M*n^2 + 2*Jm*M*R^2*n^2 + 2*L^2*M*R^2*m + 4*Jm*R^2*m*n^2 -
L^2*M^2*R^2*cos(x4)^2 + 4*Jm*L*M*R*n^2*cos(x4))
X9 = -(W^2*beta*x8 + W^2*fw*x8 + R*W*alpha*vl - R*W*alpha*vr +
4*L^2*M*R^2*x5*x8*cos(x4)*sin(x4))/(2*J_phi*R^2 + Jw*W^2 + Jm*W^2*n^2 +
R^2*W^2*m + 2*L^2*M*R^2*sin(x4)^2)
Ma trận A tại vị trí cân bằng được tính:
Ma trận B cũng được tính toán tại vị trí cân bằng:

B=
0 0
0 0
0 0
2 2
1
0 0
4 4
1
0 0
6 6
1
0 0
0 0
0 0
0 0
x x x x
r
u u
x x x x
r
u u
x x x x
r
u u
f f
v v
f f
v v
f f
v v
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Để giải phương trình Riccati bằng cách sử dụng Matlab, chúng ta cần hai ma trận
tham số Q và R
Q =
1
2
3
4
5
6
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
Q
Q
Q
Q
Q
Q
 
 
 
 
 
 
 
 
 
 
R= 1
2
0
0
R
R
 
 
 
Với giá trị Qn và Rn là các tham số điều khiển hệ thống LQR. Q1 đến Q6 là các
tham số để tối ưu hóa , , , , ,
      , R1 và R2 là các thông số để kiểm soát điện áp
của bánh xe bên trái và bánh xe bên phải, thông thường chúng ta chọn R1=R2. Nếu
chúng ta muốn tham số nào ổn định, chúng ta tập trung và tăng các tham số trong ma
trận Q và R, nhưng chúng ta cũng phải đảm bảo rằng các giá trị khác vẫn ổn định.

Sau đó, chúng ta tìm tham số phản hồi K với u = -Kx, K được giải bằng phương
trình Riccati, và ta sẽ giải nó bằng lệnh Matlab: lqr (A, B, Q, R).
Hình 3.2 Sơ đồ bộ điều khiển LQR cho robot tự cân bằng
Kết quả thu được ma trận K:
K = [0.1 -0.38 -747 27 -0.1 0.32;
0.1 -0.38 -747 27 0.1 -0.32];
Dễ thấy tín hiệu đầu ra bám theo tín hiệu đặt để giữ trạng thái cân bằng của robot,
chứng tỏ bộ điều khiển LQR hoạt động tốt kể cả khi tín hiệu đặt khác 0. Suy ra xe có
thể đi tới, đi lui, rẽ trái, rẽ phải tùy theo tín hiệu đặt.

CHƯƠNG 4. THI CÔNG HỆ THỐNG
CHƯƠNG 4: THI CÔNG HỆ THỐNG
4.1 Phần cứng
4.1.1 Bộ điều khiển hệ thống
Hình 4.1. Bộ điều khiển hệ thống
Để điều khiển robot di chuyển về phía trước, rẽ phải hoặc rẽ trái, tín hiệu từ thiết
bị Bluetooth sẽ gửi đến bộ điều khiển, tại đây bộ điều khiển sẽ cung cấp tín hiệu PWM
cho các trình điều khiển để điều khiển động cơ servo DC. Robot cũng có cảm biến gia
tốc để giữ thăng bằng robot.
4.1.2 Nguồn
Nguồn cung cấp cho robot là pin Lithium-polymer 11.1V / 2200mA (x2) có thể
cung cấp năng lượng cho hệ thống hoạt động bình thường
Hình 4.2. Pin Lipo
4.1.3 Vi điều khiển
Sử dụng 1 bo mạch vi xử lý (Arduino Mega 2560) để nhận dữ liệu từ cảm biến gia
tốc và điều khiển 2 động cơ DC
Nguồn cấp
Bộ điều
khiển và
cảm biến
Bộ điều
khiển động
cơ
Động cơ
DC servo

Hình 4.3 Arduino Mega 2560
Thông số kĩ thuật Arduino Mega 2560:
Bảng 4.1. Thông số kĩ thuật Arduino Mega 2560
Microcontroller ATmega1280
Operating Voltage 5V
Input Voltage 7-12V
Input Voltage (limits) 6-20V
Digital I/O Pins 54 (of which 15 provide PWM output)
Analog Input Pins 16
DC Current per I/O Pin 40 mA
DC Current for 3.3V Pin 50 mA
Flash Memory
128 KB of which 4 KB used by
bootloader
SRAM 8 KB
EEPROM 4 KB
Clock Speed 16 MHz

4.1.4 Cảm biến gia tốc
MPU-6050 ™ là thiết bị theo dõi chuyển động đầu tiên trên thế giới được thiết kế
cho các yêu cầu năng lượng thấp, chi phí thấp và hiệu suất cao của điện thoại thông
minh, máy tính bảng …. MPU-6050 tích hợp MotionFusion ™ của InvenSense và
phần mềm hiệu chỉnh thời gian chạy cho phép các nhà sản xuất loại bỏ việc tích hợp
mức độ, lựa chọn và mức độ tích hợp của các thiết bị rời rạc với chi phí và phức tạp
trong các sản phẩm hỗ trợ chuyển động, đảm bảo rằng các thuật toán kết hợp cảm biến
và quy trình hiệu chuẩn mang lại hiệu suất tối ưu cho người tiêu dùng.
MPU-6050 kết hợp một con quay hồi chuyển 3 trục và gia tốc kế 3 trục trên cùng
một khuôn silicon, cùng với bộ xử lý chuyển động kỹ thuật số trên bo mạch (DMP ™),
xử lý các thuật toán MotionFusion 6 trục phức tạp. Thiết bị có thể truy cập từ kế bên
ngoài hoặc cảm biến khác thông qua một bus phụ I ²C, cho phép các thiết bị thu thập
toàn bộ dữ liệu cảm biến mà không cần sự can thiệp từ bộ xử lý hệ thống. Các thiết bị
được cung cấp trong gói QFN 4 mm x 4 mm x 0,9 mm.
Hình 4.4 Sơ đồ khối họ MPU-6000
Hình 4.5 Gia tốc kế

Để theo dõi chính xác cả chuyển động nhanh và chậm, các bộ phận có phạm vi
người dùng có thể lập trình là ± 250, ± 500, ± 1000 và ± 2000 ° / giây (dps), và gia tốc
kế có thể lập trình được trong phạm vi ± 2g, ± 4g, ± 8g và ± 16g. Các tính năng bổ
sung bao gồm cảm biến nhiệt độ và bộ dao động trên chip với biến thể ± 1% trên phạm
vi nhiệt độ hoạt động.
Hình 4.6 MPU 6050
Bảng 4.2 Thông số kĩ thuật MPU 6050
Operating Voltage Supply: 2.375V–3.46V V
Gyro Full Scale Range: ±250 ±500 ±1000 ±2000 °/sec
Gyro Sensitivity: 131 65.5 32.8 16.4 LSB/°/sec
Accel Full Scale Range: ±2 ±4 ±8 ±16 g
Accel Sensitivity: 16834 8192 4096 2048
Digital Output: I²C
4.1.5 Cầu H Hbr-H 200W (bộ điều khiển động cơ)
Module Hbr là module cầu H cho phép điều khiển động cơ DC/Servo. Module này
được thiết kế cho các động cơ DC có công suất nhỏ hơn 200W, cho phép tần số băm
xung PWM cao. Về mặt điều khiển các tín hiệu vào đều được đệm trước khi vào mạch,
giúp cho quá trình đáp ứng ngõ ra tốc độ cao hơn, ít biến dạng về hình dạng. Bên cạnh

đó tín hiệu PWM ngõ vào còn được sử dụng có thể sẽ dễ dàng chọn lựa mức tác dụng
của PWM thông qua một jumper trên phần cứng.
Tính năng:
- Điện áp vận hành: 10 – 28V.
- Dòng điện liên tục lên tới 10A tại 250
C, công suất 200W.
- Dòng điện xung 56A, duty nhỏ hơn 1%.
- Điện trở RDS: 0.018.
- Led báo hiệu chiều quay, led báo nguồn.
- Cho phép độ rộng xung trên toàn giải 0 – 100%.
- Tần số PWM lên đến 100 KHz
Hình 4.7 Module HBR – 200W
4.1.6 Bánh xe
Trong đề tài này, chúng em điều khiển robot với một cái lốp đệm. Với lốp đệm, nó
dễ dàng hơn để kiểm soát robot nhờ tăng ma sát với bề mặt.

Hình 4.8 Bánh xe
4.1.7 Động cơ NF5475e
NF5475e là động cơ của Nhật Bản. Nó có trục chính chèn ròng rọc dày 2mm, 20
răng, đường kính 12.2mm, sử dụng vòng bi chất lượng cao. Encoder có 200 xung,
được sử dụng trong nhiều ứng dụng: máy CNC, điều khiển bánh xe robot …
Hình 4.9 Động cơ NF5475e
Bảng 4.3 Thông số động cơ NF5475E
Mo
del
At Maximum Efficiency
conti
nuous
operation
No load Starting

4.1.8 Mạch hạ áp DC-DC (LM2596)
Mạch sử dụng để giảm điện áp 15V từ pin xuống 12V để cung cấp cho hai động
cơ DC và cho bộ điều khiển.
Hình 4.10 Mạch hạ áp
Điện áp đầu vào: 3 – 30V.
Điện áp đầu ra: 1.5 – 30V.
Dòng đáp ứng tối đa: 3A.
Hiệu suất: 92%.
Công suất: 15W.
Kích thước: 45(dài)* 20(rộng)*14(cao) mm
NF
5475E
To
rque
[m
N.m]
S
peed
[
rpm]
C
urrent
[
A]
Out
put
[W
]
Effi
ciency
[%]
Possi
ble max
output
S
peed
[
rpm]
C
urrent
[
A]
To
rque
[m
N.m]
C
urre-
nt
[
%]
74
.83
4
162
1.
256
32.
62
68.
3
38.9 4
884
0.
218
50
6.57
7
.246
O
utput
phase
R
es
(
P/R)
P
ower
(
V)
Cur
rent
(A)
Fre
q
(K
Hz)
Enco
der type
Signal
format
2 2
00
5 50 20 Optic
al
Increment
al

4.1.9 Mạch thu phát Bluetooth HC-05
Hình 4.11 Mạch thu phát HC-05
Điện áp hoạt động: 3.3 - 5V.
Dòng điện khi hoạt động: khi Pairing 30 mA, sau khi pairing hoạt động truyền
nhận bình thường 8 mA
Baudrate UART có thể chọn được: 1200, 2400, 4800, 9600, 19200, 38400, 57600,
115200
Kích thước của module chính: 28 mm x 15 mm x 2.35 mm
4.2 Sơ đồ nối dây
Sơ đồ nối dây robot được thể hiện như hình 4.12:

Hình 4.12 Sơ đồ nối dây
4.3 Lưu đồ điều khiển
Sơ đồ điều khiển robot được thể hiện như hình 4.13. Trong đó, khối tự cân bằng sẽ
đọc giá trị cảm biến MPU 6050 và thực hiện lọc bằng bộ lọc Kalman để đưa về giá trị
góc chính xác của mô hình, dựa vào đó tính toán giá trị xung PWM dựa vào các thông
số của bộ điều khiển LQR để cấp mạch cầu H điều khiển động cơ. Tùy thuộc vào tín
hiệu điều khiển từ Bluetooth sẽ thực hiện các chương trình con tương ứng để điều
khiển hoạt động của mô hình.

Bắt đầu
Kết thúc
Tự cân bằng
Đọc tín hiệu điều khiển
Tiến ? Đi tới
Lùi ? Đi lùi
Rẽ trái ? Rẽ trái
Rẽ phải ? Rẽ phải
Dừng ? Dừng
Đ
S
Đ
S
Đ
S
Đ
S
Đ
S
Hình 4.13 Lưu đồ hệ thống
4.4 Phần mềm
4.4.1 Matlab Simulink
Matlab Simulink dùng để tính toán và mô phỏng mô hình toán học của mô hình,
qua đó kiểm tra khả năng điều khiển của bộ điều khiển LQR với mô hình, ảnh hưởng
của các hệ số điều khiển đến các ngõ ra của mô hình.

Hình 4.14 Giao diện Editor của Matlab
Hình 4.15 Giao diện Simulink
4.4.2 Arduino IDE
Trong đề tài này, chúng em sử dụng Arduino IDE để lập trình (ngôn ngữ C ++) và
tải lên Arduino một cách dễ dàng với giao diện thân thiện.

Hình 4.16 Giao diện Arduino IDE
4.4.3 MIT App Inventor
Ứng dụng mã nguồn mở của Google trên hệ điều hành Android dùng để điều
khiển thiết bị di dộng thông qua Bluetooth theo kiểu Drag and Drop.
Hình 4.17 Giao diện điều khiển thiết kế trên web AppInventor

Hình 4.18 Code điều khiển trên AppInventor (1)
Hình 4.19 Code điều khiển trên AppInventor (2)

4.5. Mô hình thực tế
Hình 4.20 Mô hình thực tế
Như chúng ta có thể thấy trên hình, robot bao gồm 3 tầng:
Tầng 1 gồm có:
 2 động cơ DC,
 Arduino Mega2560
 Bộ điều khiển động cơ HBR-200W
Tầng 2 gồm:
 MPU 6050
 LM2596
 Pin LI-PO
Tầng 3 gồm:
 Công tắc

CHƯƠNG 5. KẾT QUẢ
CHƯƠNG 5: KẾT QUẢ
5.1 Kết quả mô phỏng
Đồ thị đầu ra lần lượt gồm có: θ, ψ, φ.
Hình 5.1 Đồ thị mô phỏng góc tới θ
Đồ thị trong Hình 5.1:
- Hướng lên trên: xe chạy tới.
- Nằm ngang: xe đang đứng yên.
Hình 5.2 Đồ thị mô phỏng góc nghiêng ψ
- Hướng xuống dưới: xe đang nghiêng theo chiều âm.
- Nằm ngang: xe đang đứng cân bằng.

Hình 5.3 Đồ thị mô phỏng góc xoay φ
- Hướng lên trên: xe đang xoay về bên phải.
- Nằm ngang: xe đang đứng yên.
Dễ thấy tín hiệu đầu ra bám theo tín hiệu đặt để giữ trạng thái cân bằng của robot,
chứng tỏ bộ điều khiển LQR hoạt động tốt kể cả khi tín hiệu đặt khác 0. Suy ra xe có
thể đi tới, đi lui, rẽ trái, rẽ phải tùy theo tín hiệu đặt.
5.2 Kết quả mô hình thực tế
Hình 5.4 Đồ thị thực tế θ

Đường đồ thị trong Hình 5.4: θ là góc tới của xe
- Hướng lên trên: xe đi đang đi tới.
- Hướng xuống dưới: xe đang đi lùi.
- Nằm ngang: xe đứng yên.
Hình 5.5 Đồ thị thực tế ψ
Đường đồ thị trong Hình 5.5: ψ là góc nghiêng của xe
- Hướng lên trên: xe đang ngiêng theo chiều dương.
- Hướng xuống dưới: xe đang nghiêng theo chiều âm.
- Nằm ngang: xe đang cân bằng.

Hình 5.6 Đồ thị thực tế φ
Đường đồ thị trong Hình 5.6: φ là góc xoay của xe
- Hướng lên trên: xe đang xoay sang bên phải (bánh trái xe quay nhiều hơn).
- Hướng xuống dưới: xe đang xoay sang bên trái (bánh trái xe quay ít hơn).
- Nằm ngang: xe đứng yên.
Hình 5.7 Đồ thị thực tế PWM
Đường đồ thị trong Hình 5.7: dao động quanh vị trí cân bằng

- Hướng lên trên: xe đang chạy theo chiều dương.
- Hướng xuống dưới: xe đang chạy theo chiều âm.

CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN
CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT
TRIỂN
6.1 Kết luận
Đề tài hoàn thành đúng thời gian qui định.
Xe tự cân bằng được và có thể điều khiển từ xa mà vẫn duy trì trạng thái cân bằng.
Nghiên cứu biết thêm về giải thuật LQR cũng như bộ lọc Kalman.
Chưa đồng bộ được dữ liệu từ Matlab tới vi điều khiển về giải thuật LQR.
Vì kiến thức chuyên môn chưa sâu nên xe còn gặp một số hạn chế:
Khi chịu tác động ngoại lực lớn, xe dễ mất cân bằng và ngã.
Điều khiển tới lui chưa thật sự mượt vì hạn chế phần cứng.
Xe còn bị dao động mạnh khi tăng tải.
6.2 Giới hạn
- Robot không thể mang vật nặng
- Robot không thể làm việc tốt và ngã nếu tác động lực mạnh.
- Robot không phản hồi ngay lập tức.
6.3 Hướng phát triển:
Trong tương lai, chúng em dự định phát triển robot theo các hướng:
- Tối ưu bộ điều khiển LQR và các thông số mô hình để robot làm việc tốt hơn.
- Phát triển thêm các giải thuật khác (PID, Fuzzy…).
- Phát triển thêm các khả năng khác của xe như tránh, dừng khi gặp vật cản.
- Tích hợp thêm nhiều cảm biến xử lý ảnh, camera, GPS… để bám và định vị đối
tượng.

TÀI LIỆU THAM KHẢO
TÀI LIỆU THAM KHẢO
[1] Kalman filter. Retrieved November 23, 2009, from Wikipedia:
http://en.wikipedia.org/wiki/Kalman_filter 69 Wikipedia. (2009, September 13).
[2] Nguyễn Minh Tâm, Nguyễn Văn Đông Hải, Nguyễn Phong Lưu, Lê Văn Tuấn,
“Modelling and Optimal Control for Two-wheeled Self-Balancing Robot”, Trường
Đại Học Sư Phạm Kĩ Thuật, 12-2015.
[3] PGS.TS Huỳnh Thái Hoàng, “Lý thuyết điều khiển nâng cao”, Bộ Môn Điều
khiển tự động, Đại học Bách Khoa TPHCM, 15-1-2014.
[4] Rich Chi Ooi, “Balancing a Two-Wheeled Autonomous Robot”, The
University of Western Australia, 2003.

PHỤ LỤC
PHỤ LỤC
Phụ lục I:
Code điều khiển chính robot: gồm:
- Vòng lặp chính
- Các hàm điều khiển động cơ, hàm ngắt dùng đọc xung encoder của 2 bánh trái
phải
- Hàm tính toán LQR
- Hàm nhận tín hiệu điều khiển từ Bluetooth
/*
* Final thesis: Two-wheeled balancing robot using LQR control
* Control via Bluetooth
*/
#include <Wire.h> // Support I2C library
#include "Kalman.h" // Kalman library
#define ToRad PI/180
#define ToDeg 180/PI
kalman kalman; // kalman filter define: kalman getAngle(pitch, gyrorate, dt);
#define factortheta PI*10 // Set point for angle forward of the robot
#define factorphi PI // Set point for angle rotate of the robot
int inChar; // Read data send from board Bluetooth
uint32_t timerloop, timerold; // Create sampling cycle for theta, psi, phi
// Motor control Pin //
#define leftpwm 9 // Control PWM of the left Motor
#define leftdir 4 // Control direction left Motor

PHỤ LỤC
#define rightpwm 10 // Control PWM of the right Motor //10
#define rightdir 5 // Control direction right Motor
volatile long leftencoder; // Read and save value of left encoder
volatile long rightencoder; // Read and save value of right encoder
#define leftencoder_a 2 // Read state of encoder 1 channel A, LOW or HIGH
#define leftencoder_b 11
#define rightencoder_a 3 // Read state of encoder 2 channel A, LOW or HIGH
#define rightencoder_b 7
// MPU6050 Data //
float mpudata; // Contain psi angle ( Y axis)
float AcX, AcZ; // The values of Gyroscope following X axis and Z axis
float Gyro; // Angular acceleration following Y axis
long offset; // Offset value of MPU6050
uint32_t timer; // Update and calculate the sample time and read sample time
and data from MPU as well, Timer for kalman filter psi angle
uint8_t i2cData[14]; // Save values read from MPU
// LQR data //
long PWML, PWMR; // PWM output for H-Brigde
float k1, k2, k3, k4, k5, k6; // the factor of K matrix
bool falldown = 0; // Run = true; Stop = false, define to control the motor
float theta, psi, phi; // Angle
float thetadot, psidot, phidot; // Angular velocity of these variable
float thetaold, psiold, phiold; // Value of these variable which was created 1 period before
float leftvolt; // output Voltage of left Motor in LQR (u matrix)

PHỤ LỤC
float rightvolt; // output Voltage of right Motor in LQR (u matrix)
float addtheta; // Save setpoint value which help robot move and cling to it
float addphi; // Save setpoint value which help robot move and cling to it
int ForwardBack; // 1 -> Forward; -1 -> Back; 0 -> Stop and Balacing
int LeftRight; // 1 -> Turnleft; -1 -> Turnright; 0 -> Stop and Balacing
////////////////////////////////////////////////////
////////////////// SERIAL BEGIN ////////////////////
////////////////////////////////////////////////////
void setup()
{
kalman.setQangle(0.001);
kalman.setQbias(0.003);
kalman.setRmeasure(0.03);
// ouput pin control motor left and right //
pinMode(leftpwm, OUTPUT);
pinMode(leftdir, OUTPUT);
pinMode(rightpwm, OUTPUT);
pinMode(rightdir, OUTPUT);
// intput pin read encoder //
pinMode(leftencoder_a, INPUT_PULLUP);
pinMode(rightencoder_a, INPUT_PULLUP);
pinMode(leftencoder_b, INPUT_PULLUP);
pinMode(rightencoder_b, INPUT_PULLUP);
Serial.begin(115200); // Connect to PC via USB port

PHỤ LỤC
Serial3.begin(9600); // Serial connect to bluetooth
// Data MPU6050 //
Wire.begin();
//setupMPU();
TWBR = ((F_CPU / 400000UL) - 16) / 2;
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disale FSYNC and set 260Hz Accelerator filtering, 256
Hz Gyro filtering, 8Khz sampling
i2cData[2] = 0x00; // Set Gyro full scale range to +/-250deg/s
i2cData[3] = 0x00; // Set Gyro full scale range to +/-2g
while (i2cWrite(0x19, i2cData, 4, false));// Write to all four registers at the same time
while (i2cWrite(0x6B, 0x01, true)); // PLL with X Axis Gyroscope reference and
disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68)// Read "WHO_AM_I" register
{
Serial.print(F("Error reading sensor"));
while(true);
}
delay(100);
while (i2cRead(0x3B, i2cData, 6));
AcX = (int16_t)((i2cData[0] << 8) | i2cData[1]); // RxAcc
AcZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); // RzAcc
double pitch = atan2(-AcX, AcZ)*RAD_TO_DEG;
kalman.setAngle(pitch);
timer = micros();
k1=-0.03; // lui theta tang
k2=0.01;
k3=-1000;

PHỤ LỤC
k4=-50; // duong lac qua lai dao dong manh
k5=0.028;
k6=0.01;
offset=2.22;
// set data control motor begin //
ForwardBack = 0;
LeftRight = 0;
// set zero point value //
addphi = 0;
addtheta = 0;
// Set timer 2 divisor to 8 for PWM frequency of 31 KHz Set PWM frequency for D9 &
D10
TCCR2B = TCCR2B & B11111000 | B00000001;
// interrupt encoder //
attachInterrupt(digitalPinToInterrupt(leftencoder_a), left_isr, FALLING); // Pin 2 ~ motor
left
attachInterrupt(digitalPinToInterrupt(rightencoder_a), right_isr, FALLING); // Pin 3 ~
motor right
}
///////////////////////////////////////////////////
////////////////// MAIN PROGRAM ///////////////////
///////////////////////////////////////////////////
void loop()
{
if((micros() - timerloop) > 6000) //Sample time 6ms
{
readMPU6050();
theta = gettheta(leftencoder,rightencoder)*ToRad;

PHỤ LỤC
psi = (mpudata+offset)*ToRad; /
phi = getphi(leftencoder,rightencoder)*ToRad;
// Update time compare with timeloop
float dt = (float)(micros() - timerloop)/1000000.0; // Continue update sample time
timerloop = micros();
// Update input angle value
thetadot = (theta - thetaold)/dt;
psidot = (psi- psiold)/dt;
phidot = (phi - phiold)/dt;
// Update old angle value
thetaold = theta;
psiold = psi;
phiold = phi;
if(Serial3.available() > 0)
{
inChar = Serial3.read();
Serial.println(4);
}
else
inChar = 0;
serialEvent1();
addtheta = addtheta + ForwardBack*factortheta; // For/Back // theta la rad
addphi = addphi + LeftRight*factorphi; // Left/Right
getlqr(theta + addtheta, thetadot, psi, psidot, phi + addphi, phidot);
motorcontrol(PWML, PWMR, (mpudata + offset), falldown)
}
}
// left motor encoder interrupt //
void left_isr()

PHỤ LỤC
{
if(digitalRead(leftencoder_b) == 1)
leftencoder++;
else
leftencoder--;
}
// right motor encoder interrupt//
void right_isr()
{
if(digitalRead(rightencoder_b) == 1)
rightencoder--;
else
rightencoder++;
}
// Read psi //
void readMPU6050()
{
while (i2cRead(0x3B, i2cData, 14));
AcX = (int16_t)((i2cData[0] << 8) | i2cData[1]); // Gyroscope of X Axis
AcZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); // Gyroscope of Z Axis
Gyro = (int16_t)((i2cData[10] << 8) | i2cData[11]);
double dt = (double)(micros() - timer)/1000000; // Calculate delta time
timer = micros();
double pitch = atan2(-AcX, AcZ)*RAD_TO_DEG;
double Gyrorate = Gyro/131.0; // Convert to degree/s
// Using kalman to filter noise when sensor read angle data
mpudata = kalman.getAngle(pitch, Gyrorate, dt);

PHỤ LỤC
if(abs(mpudata)>30) // Limit angle falldown of robot
falldown = true; // Stop motor
else
falldown = false; // Start motor
}
// Read theta Angle
float gettheta(long lencoder, long rencoder)
{
float angle = 0.9*(lencoder + rencoder); // 0.5*360/200
return angle;
}
// Read phi Angle
float getphi(long lencoder, long rencoder)
{
float angle = 40/225.0*(rencoder - lencoder)/200*360; // R/W(phitr-phiph)
return angle;
}
// LQR function
void getlqr(float theta_, float thetadot_, float psi_, float psidot_, float phi_, float phidot_)
{
int offsetPWM = 15;
leftvolt = k1*theta_ + k2*thetadot_ + k3*psi_ + k4*psidot_ - k5*phi_ - k6*phidot_;
rightvolt = k1*theta_ + k2*thetadot_ + k3*psi_ + k4*psidot_ + k5*phi_ + k6*phidot_;
PWML = map(leftvolt, -(k3*PI)/27, (k3*PI)/27, -250, 250); // Limit 18 degree 180----PI
PWMR = map(rightvolt, -(k3*PI)/27, (k3*PI)/27, -250, 250);//6.5-----PI*6.5/180=PI/27
PWML = constrain(PWML, -240, 240); // Limit PWM for H-Bridge
PWMR = constrain(PWMR, -240, 240);
if (PWML <0)
{
PWML = PWML + offsetPWM;
}

PHỤ LỤC
Phụ lục II:
Bộ lọc Kalman đơn giản được triển khai bằng code C++
#ifndef _Kalman_h
#define _Kalman_h
class kalman
{
public:
void Kalman()
{
/* We will set the varibles like so, these can also be tuned by the user */
Q_angle = 0.001; // matran hiep phuong sai du doan
Q_bias = 0.003;
R_measure = 0.03;// ma tran hiep phuong sai do duoc
bias = 0; // Reset bias
P[0][0] = 0; // Since we assume tha the bias is 0 and we know the starting angle (use
setAngle), the error covariance matrix is set like so - see:
http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
P[0][1] = 0; // hiep phuong sai du doan
P[1][0] = 0;
P[1][1] = 0;
};
// The angle should be in degrees and the rate should be in degrees per second and the
delta time in seconds
double getAngle(double newAngle, double newRate, double dt)
{
// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
// Modified by Kristian Lauszus
// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-
practical-approach-to-kalman-filter-and-how-to-implement-it
// Discrete Kalman filter time update equations - Time Update ("Predict")

PHỤ LỤC
// Update xhat - Project the state ahead
/* Step 1 */
rate = newRate - bias;
angle += dt * rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
// Discrete Kalman filter measurement update equations - Measurement Update
("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
S = P[0][0] + R_measure; // hiep phuong sai do duoc
/* Step 5 */
K[0] = P[0][0] / S; //do loi kalman
K[1] = P[1][0] / S;
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
y = newAngle - angle;
/* Step 6 */
angle += K[0] * y;
bias += K[1] * y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
P[0][0] -= K[0] * P[0][0];
P[0][1] -= K[0] * P[0][1];
P[1][0] -= K[1] * P[0][0];
P[1][1] -= K[1] * P[0][1];
return angle;

PHỤ LỤC
Phụ lục III:
Các thanh ghi của cảm biến MPU 6050 sử dụng trong chương trình:
- Các thanh ghi lưu giá trị gia tốc của cảm biến treo 3 trục x, y, z
Với giá trị full scale range được xác định trong AFS_SEL
- Các thanh ghi lưu giá trị con quay hồi chuyển theo 3 trục x, y, z
Với giá trị full scale range được xác định trong FS_SEL

Xe hai bánh tự cân bằng.pdf

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to Xe hai bánh tự cân bằng.pdf

Similar to Xe hai bánh tự cân bằng.pdf (20)

More from Man_Ebook

More from Man_Ebook (20)

Recently uploaded

Recently uploaded (20)

Xe hai bánh tự cân bằng.pdf