HỌC VIỆN CƠNG NGHỆ BƯU CHÍNH VIỄN THƠNG
KHOA KỸ THUẬT ĐIỆN TỬ 1
----------

ĐỒ ÁN TỐT NGHIỆP
Đề tài: Xe cân bằng hai bánh thông minh

Giảng viên hướng dẫn: Nguyễn Quý Sỹ
Sinh viên thực hiện:

Vũ Văn Minh

Mã sinh viên:

B18DCDT159

Lớp:

D18DTMT2

Hà Nội, 2022
1


HỌC VIÊN CƠNG NGHỆ BƯU CHÍNH VIỄN

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

THÔNG

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

KHOA KỸ THUẬT ĐIỆN TỬ 1

ĐỀ TÀI ĐỒ ÁN TỐT NGHIỆP ĐẠI HỌC
Họ và tên sinh viên: Vũ Văn Minh
Lớp: D18DTMT2

Khoá: 2018-2023

Ngành đào tạo: Kỹ thuật Điện-Điện tử

Hệ đào tạo: Đại học chính quy

1. Tên đồ án/khố lận tốt nghiệp:
“Xe cân bằng hai bánh thông minh”
2. Lý do chọn đề tài:
...
Tình trạng thực tế giao thơng hiện nay đang rất phức tạp, hỗn hợp loại xe tham gia
trên đường. Đặc biệt là sự ô nhiễm môi trường do chất thải từ xe cộ sử dụng xăng dầu
làm nguyên liệu cho động cơ. Vì thế mà các giải pháp về năng lượng điện đã dần đi
vào thay thế, các ô tô điện, xe đạp điện,… đã ra đời và cải thiện được ô nhiễm cũng
như giá thành. Cũng từ lý do trên, em lựa chọn nghiên cứu mơ hình xe cân bằng hai
bánh thơng minh. Đây là mơ hình nhỏ gọn từ kích thước tới chi phí đầu tư, thơng minh
ở các chức năng đi kèm (như xử lý khoảng cách vật cản, dò line, chuyển động theo
remote hay học lệnh, giám sát bằng camere,..).
3. Nội dung chính của đồ án:
Chương 1: Tổng quan về xe cân bằng hai bánh cân bằng thông minh.
Chương 2: Các cơ sở lý thuyết.
Chương 3: Thiết kế hệ thống.
4. Cơ sở dữ liệu ban đầu:
Môn học: Hệ thống nhúng, Kỹ thuật vi xử lý, Thị giác máy tính, CAD/CAM,..

Các nghiên cứu về tính chất MIMO (Multi – Input Multi - Output), Bộ lọc
Kalman, Thuật toán điều khiển PID,…
5. Ngày giao đề tài: …../…../2022
6. Ngày nộp quyển: …./…../2022
GIẢNG VIÊN HƯỚNG DẪN

SINH VIÊN THỰC HIỆN

(Ký, ghi rõ họ tên)

(Ký, ghi rõ họ tên)

2


TRƯỞNG KHOA
(Ký, ghi rõ họ tên)

NHẬN XÉT CỦA GIẢNG VIÊN HƯỚNG DẪN
.................................................................................................................................
.................................................................................................................................
.................................................................................................................................
- NỘI DUNG ĐỒ ÁN:
.................................................................................................................................
.................................................................................................................................
.................................................................................................................................
- HÌNH THỨC TRÌNH BÀY:
Thuyết minh:
.................................................................................................................................
.................................................................................................................................

Bản vẽ:
.................................................................................................................................
.................................................................................................................................
.................................................................................................................................
- NHẬN XÉT KHÁC:
.................................................................................................................................
.................................................................................................................................
.................................................................................................................................
- Ý KIẾN ĐÁNH GIÁ:
.................................................................................................................................
.................................................................................................................................
.................................................................................................................................
3


.................................................................................................................................
Ngày tháng năm 2022
Giáo viên hướng dẫn (ký tên)

4


LỜI MỞ ĐẦU

5


MỤC LỤC
LỜI MỞ ĐẦU........................................................................................................................................4
Danh mục hình ảnh................................................................................................................................7

CHƯƠNG I: TỔNG QUAN VỀ XE CÂN BẰNG HAI BÁNH THÔNG MINH...................................8
1.

Thế nào là xe hai bánh tự cân bằng (two wheel self balancing)............................................8

2.

Ưu nhược điểm của xe hai bánh tự cân bằng..............................................................................9

3.

2.1.

Ưu điểm của xe tự cân bằng trên hai bánh..........................................................................9

2.2.

Nhược điểm của xe.............................................................................................................9

Một số dạng xe hai bánh tự cân bằng dung trên robot................................................................9
3.1.

nBot....................................................................................................................................9

3.2.

Balance bot.......................................................................................................................10

CHƯƠNG 2. CÁC CƠ SỞ LÝ THUYẾT............................................................................................11
1. Đặc tính động lực học..................................................................................................................11

1.1. Mơ hình hóa robot 2 bánh tự cân bằng trên địa hình phẳng...................................................11
2. Bộ lọc Kalman..............................................................................................................................16
2.1. Giới thiệu về bộ lọc Kalman..................................................................................................16
2.2. Quá trình ước lượng..............................................................................................................16
2.3. Bản chất xác suất của bộ lọc..................................................................................................18
2.4. Thuật toán Kalman rời rạc.....................................................................................................18
3. Giải thuật điều khiển....................................................................................................................21
3.1 Bộ điều khiển PID..................................................................................................................21
3.2 Cấu trúc bộ điều khiển PID cho robot hai bánh tự cân bằng...................................................22
4. Các thành phần chính của mơ hình...............................................................................................24
4.1. Mạch Điều Khiển Động Cơ Bước A4988..............................................................................24
4.2. Vi điều khiển STM32F103C8T6...........................................................................................26
4.3. Kit ESP32 DOIT DevKit V1...................................................................................................28
4.4. Bánh xe..................................................................................................................................29
4.5. Mạch Giảm Áp DC XL4015 (5A).........................................................................................29
4.6. Mạch Giảm Áp DC LM2596 3A...........................................................................................30
4.7. Pin cell 18650 2000mAh.......................................................................................................30
4.8. Trụ đồng đực cái 20mm.........................................................................................................30
4.9. Hộp đế pin 18650 4 cell.........................................................................................................31
4.10. Cảm biến gia tốc GY-521 6DOF IMU MPU6050...............................................................31

6


4.10. Cảm biến siêu âm (HC – SRF04).........................................................................................33
CHƯƠNG 3. THIẾT KẾ HỆ THỐNG.................................................................................................34
1. Thiết kế phần cứng...................................................................................................................34
2. Thiết kế cơ khí..........................................................................................................................34
3. Kết nối phần cứng.....................................................................................................................34
5. Bộ lọc Kalman..........................................................................................................................34

6. Lưu đồ giải thuật điều khiển.....................................................................................................34
CHƯƠNG 4. KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN......................................................................35
1.

Kết quả đạt được...........................................................................................................................35
2.

Những hạn chế và hướng phát triển..........................................................................................35
2.1.

Hạn chế.............................................................................................................................35

2.2.

Hướng phát triển...............................................................................................................35

TÀI LIỆU THAM KHẢO....................................................................................................................35

7


Danh mục hình ảnh
Hình 1: Mơ tả ngun lý giữ thăng bằng............................................................................................8
Hình 2: nBot..........................................................................................................................................9
Hình 3: Balance – bot.........................................................................................................................10
Hình 4: Mơ hình xe 2 bánh tự cân bằng trên mặt phẳng................................................................11
Hình 5: Biểu diễn lực và momen của mơ hình..................................................................................16
Hình 6: Quy trình thực hiện của bộ lọc Kalman..............................................................................19
Hình 7: Tổng quan chu trình thực hiện bộ lọc Kalman hồn chỉnh...............................................20
Hình 8: Sơ đồ hệ thống điều khiển sử dụng PID..............................................................................21

Hình 9: Ảnh hưởng của các thông số bộ điều khiển PID đến hệ thống..........................................22
Hình 10: Cấu trúc bộ điều khiển PID cho hệ robot hai bánh tự cân bằng.....................................22
Hình 11: Mạch điều khiển động cơ bước A4988..............................................................................24
Hình 12: Bảng cấu hình chân tạo độ phân giải bước ra module A4988.........................................24
Hình 13: Sơ đồ chân của trình điều khiển A4988.............................................................................25
Hình 14: Kit phát triển STM32F103C8T6 Blue Pill ARM Cortex-M3..........................................26
Hình 15: Board kit phats triển ESP32 DOIT DevKit V1.................................................................28
Hình 16: Bánh xe 65mm khớp lục giác.............................................................................................29
Hình 17: Mạch giảm áp DC XL4015 (5A)........................................................................................29
Hình 18: Mạch giảm áp DC LM2596 3A..........................................................................................30
Hình 19: Pin cell 18650 2000mAh.....................................................................................................30
Hình 20: Trụ đồng đực cái 20mm.....................................................................................................30
Hình 21: Hộp để pin 18650 4 cell.......................................................................................................31
Hình 22: Cảm biến gia tốc GY-521 6DOF IMU MPU6050.............................................................31
Hình 23: Sơ đồ nguyên lý Cảm biến gia tốc GY-521 6DOF IMU MPU6050..................................32
Hình 24: Cảm biến siêu âm (HC-SR04)............................................................................................33
Hình 25: Sơ đồ xung SR05.................................................................................................................33
Hình 26: Động cơ step size 42 ngắn(NEMA17HS3401S).................................................................34

8


CHƯƠNG I: TỔNG QUAN VỀ XE CÂN BẰNG HAI BÁNH
THÔNG MINH
1. Thế nào là xe hai bánh tự cân bằng (two wheel self balancing).

Hình 1: Mơ tả ngun lý giữ thăng bằng

Đối với các xe ba hay bốn bánh, việc thăng bằng và ổn định của chúng là nhờ
trọng tâm của chúng nằm trong bề mặt chân đế do các bánh xe tạo ra. Đối với các xe 2

bánh có cấu trúc như xe đạp, việc thăng bằng khi không di chuyển là hồn tồn khơng
thể, vì việc thăng bằng của xe dựa trên tính chất con quay hồi chuyển ở hai bánh xe khi
đang quay. Còn đối với xe hai bánh tự cân bằng, là loại xe chỉ có hai bánh với trục của
hai bánh xe trùng nhau, để cho xe cân bằng, trọng tâm của xe cần được giữ nằm ngay
giữa các bánh xe. Điều này giống như ta giữ một cây gậy dựng thẳng đứng cân bằng
trong lịng bàn tay.
Thực ra, trọng tâm của tồn bộ xe khơng được biết nằm ở vị trí nào, cũng
khơng có cách nào tìm ra nó, và có thể khơng có khả năng di chuyển bánh xe đủ nhanh
9


để giữ nó ln ở dưới tồn bộ trọng tâm.
Về mặt kỹ thuật, góc giữa sàn di chuyển của xe và chiều trọng lực có thể biết
được. Do vậy, thay vì tìm cách xác định trọng tâm giữa các bánh xe, khung xe cần
phải thẳng đứng, vng góc với sàn xe (góc cân bằng khi ấy bằng 0).
2. Ưu nhược điểm của xe hai bánh tự cân bằng.
2.1. Ưu điểm của xe tự cân bằng trên hai bánh.
-

Không ô nhiễm, sử dụng pin điện và có thể sạc lại.

-

Sử dụng không gian hiệu quả, đa năng (sử dụng trong nhà và ngồi phố).

-

Khá dễ vịng quanh trong khơng gian hẹp, chạy ngang qua cửa ra vào do tốc độ
thấp. Ngoài ra, nó cịn có thể xuống các bậc thang thấp.


-

Cuốn hút người sử dụng bởi hình dáng kỳ lạ.

2.2. Nhược điểm của xe.
-

Vì xe cần sự tối ưu hố về khối lượng cũng như trọng tâm của xe trong việc giữ
cân bằng nên khó bổ sung thêm các chức về hoạt động ngay trên khung xe. (Ví
dụ: lắp tay gắp lên xe cân bằng,...).

-

Không đủ nhanh để đi đường trường và khơng đủ an tồn để lên xuống lề
đường.

-

Khơng thể leo bậc thang có chiều q ½ bán kính của xe.

3. Một số dạng xe hai bánh tự cân bằng dung trên robot
3.1. nBot
nBot
do
David
P.
Anderson sang chế. Các
bánh xe của nBot sẽ phải
chạy theo hướng mà phần
trên rotbot sắp ngã. Nếu

bánh xe có thể được lái theo
cách đứng vững theo trọng
tâm robot, robot sẽ vẫn
được giữ cân bằng.

Hình 2: nBot.

10


3.2. Balance bot
Balacne-bot do sanghyuk, Hàn
Quốc thực hiện là một robot hai
bánh tự cân bằng bằng cách kiểm
sốt thơng tin phản hồi. Hệ thống
cao 50cm. Nó có hai trục bánh xe
nối với hộp giảm tốc và động cơ DC
cho sự phát động. Tổng cộng có ba
bộ vi xử lý Atmel được sử dụng. Vi
điều khiển chính (master) thi hành
những nguyên lý kiểm soát và thuật
toán ước lượng. Một vi điều khiển
khác kiểm soát tất cả cảm biến
analog. Vi điều khiển thứ ba điều
khiển động cơ DC.
Linear quadratic regulator (LQR)
được thiết kế và thực thi mạch điều
khiển. Nó có bốn giá trị khác nhau –
góc nghiêng, vận tốc góc nghiêng,
góc quay bánh xe và vận tốc góc

quay, sau đó nó tạo lện cho động cơ
DC để điều chỉnh tốc độ bánh xe.

Hình 3: Balance – bot.

11


CHƯƠNG 2. CÁC CƠ SỞ LÝ THUYẾT
1. Đặc tính động lực học
1.1. Mơ hình hóa robot 2 bánh tự cân bằng trên địa hình phẳng
Xây dựng mơ hình hóa xe hai bánh tự cân bằng trên địa hình phẳng

Hình 4: Mơ hình xe 2 bánh tự cân bằng trên mặt phẳng.

12


Trong đề tài này sẽ sử dụng các kí hiệu, đơn vị như sau:
Kí hiệu

Đơn vị

Ý nghĩa

M

Kg

Khối lượng của bánh xe


M

Kg

Khối lượng của robot

R

M

Bán kính bánh xe

W

M

Chiều rộng của robot

D

M

Chiều ngang của robot

H

M

Chiều cao của robot


L

M

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

𝑓𝑤

Hệ số ma sát giữa bánh xe và mặt phẳng di chuyển

𝑓𝑚

Hệ số ma sát giữa robot và động cơ DC
𝑘𝑔. 𝑚2

Moment quán tính của động cơ DC

Ohm

Điện trở động cơ DC

Kb

V sec/rad

Hệ số EMF của động cơ DC

Kt


Nm/A

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

𝐽𝑚
𝑅
𝑚

N

Tỉ số giảm tốc

G

𝑚/𝑠2

Gia tốc trọng trường

𝜃

Rad

Góc trung bình của bánh trái và phải

𝜃𝑙,𝑟

Rad

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


𝜓

Rad

Góc nghiêng của phần thân robot

𝜙

Rad

Góc xoay của robot

𝑥𝑙, 𝑦𝑙, 𝑧𝑙

M

Tọa độ bánh trái

𝑥𝑟, 𝑦𝑟, 𝑧𝑟

M

Tọa độ bánh phải

𝑥𝑚, 𝑦𝑚, 𝑧𝑚

M

Tọa độ trung bình


𝐹𝜃, 𝐹ψ, 𝐹𝜙

Nm

Moment phát động theo các phương khác nhau

𝐹𝑙,𝑟

Nm

Moment phát động của động cơ bánh trái, phải

Sử dụng phương pháp Euler-Lagrange để xây dựng mơ hình động học. Giả sử tại thời
điểm t = 0, robot di chuyển theo chiều dương trục x, ta có các phương trình sau:
Góc tịnh tiến trung bình của hai bánh xe và góc xoay của robot được xác định như sau:

13


( )

1
(θ +θ )
θ= 2 l r
ϕ
R
(θ −θ )
W l r

()


[2.1]

Trong đó tọa độ trung bình của Robot trong hệ qui chiếu:

[ ][ ]
xm
∫ x˙ m
y m = ∫ ˙y m
zm
R

Và

[2.2]

[ ][

˙
x˙ m
= R θ cos ϕ
y˙ m
R θ˙ sin ϕ

]

[2.3]

Tọa độ bánh trái trong hệ qui chiếu :


[ ](

W
x m− sin ϕ
xr
2
=
yr
W
y m + cos ϕ
2
zr
ZM

)

[2.4]

)

[2.5]

Tọa độ bánh phải trong hệ qui chiếu :

[ ](

W
sin ϕ
xr
2

yr =
W
y m + cos ϕ
2
zr
ZM
x m−

Tọa độ tâm đối xứng giữa hai động cơ trong hệ qui chiếu :

[ ][

xb
x m+ L sin ψ cos ϕ
=
yb
y m Lsin ψ sin ϕ
zb
z m + Lcos ϕ

]

[2.6]

Phương trình động năng của chuyển động tịnh tiến:
1
1
1
2
2

2
2
2
2
2
2
2
T 1= m ( x˙ l + y˙ l + z˙ l ) + m ( x˙ r + y˙ r + z˙ r ) + m ( x˙ b+ ˙y b+ ˙z b )
2
2
2

[2.7]

Phương trình động năng của chuyển động quay :
1
2 1 2
2
2 1
2 1
2 1
2 1 2
T 2= J W θ˙ l + J W θ˙ r + J ψ ψ˙ + J ϕ ϕ˙ + n J m ( θ˙ l −ψ˙ ) + n J m ( θ˙ r−ψ˙ )
2
2
2
2
2
2


[2.8]
14


Với

1 2
1 2
2
n J m ¿¿ ; n J m ( θ˙ r −ψ˙ ) là động năng quay của phần ứng động cơ trái và phải.
2
2

Phương trình thế năng:
U =mg z l +mg z r +mg z b

[2.10]

Phương trình lagrange:
L=T 1 +T 2 −U

[2.11]

( )
d ∂L
∂L
−
=F
dt ( ∂ ψ˙ ) ∂ ψ
d ∂L ∂L

−
=F
dt ( ∂ ϕ˙ ) ∂ ϕ

d ∂L ∂L
−
=Fθ
dt ∂ θ˙
∂θ

[2.12]

ψ

[2.13]

ϕ

[2.14]

Lấy đạo hàm L theo các bin ta c:
ă ( MLR cos 2n 2 J m )
ă MLR 2 sin =F
[ ( 2 m+ M ) R2 +2 J W +2 n2 J m ] +

[2.15]
2 2
ă ( M L2+ J +2 n2 J m ) MgL
ă
sin M L sin cos ψ =Fψ

( MLR cos ψ−2 n2 J m ) θ+

[2.16]

[

]

1
W2
2
2
2
2
2
2
mW + J ϕ + 2 ( J W + n J m ) + M L sin ă +2 M L ψ˙ ϕ˙ sin ψ cos ψ=F ϕ
2
2R

[2.17]
Momen động lực do động cơ DC sinh ra:

[ ][

Fl + F r
Fθ
Fψ
Fψ =
W

Fϕ
( F −F r )
2R l

]

[2.18]

Và:
F l=n K t i l +f m ( ψ˙ −θ˙ l ) −f W θ˙ l

[2.19]

˙ θ˙ r ) −f W θ˙ r
F r=n K t i r + f m ( ψ−

[2.20]
15


˙ θ˙ l )−f m ( ψ−
˙ θ˙ r )
F ψ=−n K t i l−n K t i r−f m ( ψ−

[2.21]

Sử dụng phương pháp PWM để điều khiển động cơ nên chuyển từ dòng điện sang điện
áp động cơ:
˙ θ˙ l , r ) −Rm il , r
Lm ˙il , r=v l ,r + K b ( ψ−


[2.22]

Xem điện cảm phần ứng tương đối nhỏ (gần bằng 0), có thể bỏ qua, suy ra:
i l ,r =

˙ θ˙ l ,r )
v l ,r + k b ( ψ−

[2.23]

Rm

Từ đó, các moment lực sinh ra:
˙ 2 β ψ˙
F θ=α ( v l +v r )−2 ( β +f w ) θ+

[2.24]

˙
F ψ=−α ( v l+ v r ) + 2 β θ−2
β ψ˙

[2.25]

Với α =

F ϕ=

n kt

n Kt Kb
+f m
và β=
Rm
Rm

[2.26]

W
W2
α ( v r −v l ) − 2 ( β + f w ) ϕ˙
2R
2R

[2.27]

Thu được phương trình động lc hc mụ t chuyn ng ca robot nh sau:
ă ( MLR cos ψ−¿2 n2 J m ) ψ−MLR
˙ 2
ă
2 sin = ( v l +v r )−2 ( β +f w ) θ+
[ ( 2 m+ M ) R2 +2 J w +2 n2 J m ] +

[2.28]
2 2
ă ( M L2+ J +2 n2 J m ) MgL

ă
sin M L sin cos =−α ( v l +v r )+ 2 β θ−2
β ψ˙

( MLR cos ψ−2 n2 J m ) θ+

[2.29]

[

]

1
W2
W
W2
2
2
2
2
2
2
mW + J ϕ + 2 ( J w + n J m) + M L sin ă +2 M L ψ˙ ϕ˙ sin ψ cos ψ=
α ( v r−v l )− 2 ( β+ f w ) ϕ˙
2
2R
2R
2R

[2.30]

16



Hình 5: Biểu diễn lực và momen của mơ hình.
2. Bộ lọc Kalman
2.1. Giới thiệu về bộ lọc Kalman
Bộ lọc Kalman, được Rudolf (Rudy) E. Kálmán công bố năm 1960, là thuật
toán sử dụng chuỗi các giá trị đo lường, bị ảnh hưởng bởi nhiễu hoặc sai số, để ước
đoán biến số nhằm tăng độ chính xác so với việc sử dụng duy nhất một giá trị đo
lường. Bộ lọc Kalman thực hiện phương pháp truy hồi đối với chuỗi các giá trị đầu
vào bị nhiễu, nhằm tối ưu hóa giá trị ước đoán trạng thái của hệ thống.
Bộ lọc Kalman được ứng dụng rộng rãi trong kỹ thuật, phổ biến trong các ứng
dụng định hướng, định vị và điều khiển các phương tiện di chuyển. Ngoài ra, bộ lọc
Kalman cịn được ứng dụng để phân tích dữ liệu trong các lĩnh vực xử lý tín hiệu .
2.2. Q trình ước lượng
Vấn đề chung của bộ lọc Kalman nhằm ước lượng biến trạng thái𝑥 ∈ 𝑅𝑛 của
quá trình điều khiển rời rạc được điều chỉnh bởi các phương trình tuyến tính ngẫu
nhiên khác nhau. Phương trình khơng gian trạng thái của bộ lọc:
𝑥𝑘 = 𝐴𝑥𝑘−1 + 𝐵𝑢𝑘−1 + 𝑤𝑘−1

[2.32]

Với giá trị 𝑧 ∈ 𝑅𝑚là:
𝑧𝑘 = 𝐻𝑥𝑘 + 𝑣𝑘

[2.33]
17


Biến ngẫu nhiên 𝑤𝑘, 𝑣𝑘 đặc trưng cho nhiễu quá trình và nhiễu đo của hệ.
Chúng độc lập với nhau, tần suất phân bố thông thường:
𝑝(𝑤)~𝑁(0, 𝑄)


[2.34]

𝑝(𝑣)~𝑁(0, 𝑅)

[2.35]

Trên thực tế, ma trận tương quan nhiễu quá trình Q và ma trận tương quan
nhiễu đo R có thể thay đổi sau mỗi bước thời gian hay giá trị, tuy nhiên để đơn giản,
trong hầu hết các trường hợp Q và R được xem là hằng số.
Ma trận vuông A trong phương trình [2.32] thể hiện mối quan hệ của các biến
trạng thái ở thời điểm k-1 với thời điểm hiện tại k. Thực ra trên thực tế ma trận A thay
đổi sau mỗi bước thời gian, nhưng ở đây ma trận A xem như hằng số. Ma trận B thể
hiện mối liên hệ tín hiệu điều khiển 𝑢 ∈ 𝑅𝐿đối với biến trạng thái x. Ma trận H trong
phương trình [2.33] thể hiện mới liên hệ giữa biến trạng thái với tín hiệu ra z, H cũng
được xem là hằng số.
Những tính tốn căn bản của bộ lọc:
Định nghĩa:
−¿¿

x̂ k

= 𝐸{𝑥𝑘|𝑦1, 𝑦2 … 𝑦𝑘−1}là giá trị ước lượng của x k trước khi ta xử lý giá trị đo tại

thời điểm k.
x̂ k +¿¿ = 𝐸{𝑥𝑘|𝑦1, 𝑦2 … 𝑦𝑘}là giá trị ước lượng của x k sau khi ta xử lý giá trị đo tại thời

điểm k.
𝑥̂𝑘 ∈ 𝑅𝑛 là giá trị ước lượng trạng thái sau tại bước k có được sau khi so sánh với giá
trị đo z k .Và chúng ta có sai số ước lượng trạng thái trước và sau:


{

e k ≡ xk −^x k
e k ≡ xk −^x k

[2.36]

Tương quan sai số ước lượng trước “priori”:
Pk = E{e k ek }

[2.37]

Tương quan sai số ước lượng sau “posteriori”:
Pk =E {e k eTk

[2.38]

Khi lấy đạo hàm phương trình bộ lọc Kalman, với mục đích tìm một phương
trình để tính tốn trạng thái ước lượng posteriori 𝑥̂𝑘 thể hiện sự tương quan giữa giá trị
ước lượng priori 𝑥̂𝑘- và độ sai lệch giữa giá trị đo thực 𝑧𝑘 và giá trị đo ước lượng
𝐻𝑥̂𝑘- : ^x k = x^ k + K ( Z k −H x^ k )
[2.39]
Ma trận K trong [3.8] là ma trận độ lợi hay hệ số trộn để tối thiểu hóa phương trình
18


tương quan sai số posteriori. Biểu thức tính K để tối thiểu hóa phương trình [3.8] như
sau:
H ¿ ¿¿
[2.40]

K k =P−¿
k
T

Từ đó thấy rằng tương quan sai số giá trị đo lường R tiến tới 0, khi đó:
lim K K= H

[2.41]

−1

RK→ 0

Mặt khác, tương quan sai số ước lượng priori của 𝑃𝑘- tiến đến 0, khi đó:
lim K k =0

[2.42]

P k →0

Một cách nghĩ khác về giá trị hiệu chỉnh bù bởi K là nếu ma trận tương quan sai
số giá trị đo lường R tiến tới 0 thì giá trị đo được 𝑧𝑘 sẽ có độ tin cậy càng cao, trong
khi giá trị ước lượng H x k sẽ có độ tin cậy càng thấp. Mặt khác, nếu tương quan sai số
−¿ ¿
ước lượng priori Pk tiến tối 0 thì 𝑧𝑘 sẽ khơng đáng tin mà giá trị ước lượng H x ksẽ
càng đáng tin.
2.3. Bản chất xác suất của bộ lọc
Sự điều chỉnh cho 𝑥𝑘 trong [3.8] đã xác định bản chất ước lượng priori 𝑥̂𝑘-với
điều kiện tất cả các giá trị đo 𝑧𝑘 đều có nghĩa (Luật phân bố Bayer). Điều đó cho thấy
bộ lọc Kalman duy trì hai thời điểm đầu tiên của sự phân bố trạng thái:

E [ x k ]=^x k

[2.43]

E [ ( x k − x^ k ) ( x k − x^ k ) T ]=P k

[2.44]

Phương trình ước lượng trạng thái posteriori phản ánh giá trị trung bình của phân bố
trạng thái. Tương quan sai số ước lượng trạng thái posteriori phản ánh sự thay đổi của phân bố
trạng thái. Ngoài ra ta cịn có:
p ( x k ∨z k ) N ( E [ x k ] , E [ ( x k − ^x k ) ( x k − ^x k )

T

])=N ( x^

k

, Pk )

[2.45]

2.4. Thuật toán Kalman rời rạc
Bộ lọc Kalman ước lượng tiến trình bằng cách sử dụng dạng điều khiển hồi
tiếp: bộ lọc ước lượng các trạng thái của quá trình tại một vài thời điểm và sau đó chứa
tín hiệu hồi tiếp trong các dạng của giá trị đolường. Do đó, phương trình bộ lọc
Kalman chia làm hai nhóm: phương trình cập nhật thời gian và phương trình cập nhật
giá trị đo lường. Phương trình cập nhật thời gian chịu trách nhiệm cho việc dự báo
19



trước (về mặt thời gian) của trạng thái hiện tại và ước lượng sai số tương quan để chứa
vào bộ ước lượng trước priori cho bước thời gian tiếp theo. Phương trình cập nhật giá
trị đo lường chịu trách nhiệm cập nhật cho tín hiệu hồi tiếp, nghĩa là cập nhật giá trị
mới vào giá trị ước lượng tước priori để tạo tín hiệu ước lượng sau posteriori tốt hơn.
Phương trình cập nhật thời gian cũng có thể được coi là phương trình dự đốn. Trong
khi đó phương trình cập nhật giá trị đo lường thì được xem như là phương trình hiệu
chỉnh.Vì vậy, thuật tốn ước lượng cuốicùng đều giống nhau ở thuật toán dự đoán và
hiệu chỉnh để giải quyết vấn đề số học như hình vẽ dưới đây:

Hình 6: Quy trình thực hiện của bộ lọc Kalman.
Phương trình cập nhật thời gian cho bộ lọc Kalman rời rạc:
^x k = A ^x k−1 +B u k−1
AP
P−¿=
k

k−1

[2.46]

T

A +Q ¿

[2.47]

Phương trình cập nhật giá trị đo lường cho bộ lọc Kalman rời rạc:
T


−¿ H ¿ ¿¿

[2.48]

^x k = x^ k K ( z k −H x^ k )

[2.49]

Pk = ( 1−K k H ) P−¿¿
k

[2.50]

K k =P k

Nhiệm vụ đầu tiên trong suốt quá trình cập nhật giá trị đo lường là tính tốn độ
lợi Kalman𝐾𝑘. Bước tiếp theo là xử lí giá trị đo thực được chứa trong 𝑧𝑘. Sau đó, tính
trạng thái ước lượng sau posteriori bằng cách kết hợp giá trị đo được theo công thức
𝑥̂𝑘 ở trên. Bước cuối cùng là tính giá trị sai số ước lượng tương quan posteriori vào
𝑃𝑘. Sau mỗi chu trình tính tốn của bộ lọc Kalman, các giá trị được cập nhật theo cặp,
tiến trình được lặp lại với ước lượng posteri ori của trạng thái trước dùng để dự đoán
20


ước lượng priori mới. Trạng thái đệ quy tự nhiên là một trong những điểm đặc trưng
của bộ lọc Kalman, nó thay thế điều kiện đệ quy ước lượng hiện tại cho giá trị đã qua.
Trong điều kiện thực hiện thực tế của bộ lọc, giá trị nhiễu tương quan R thường
được dùng làm giá trị ưu tiên để tính toán cho bộ lọc. Trên thực tế, việc đo các giá trị
ma trận R là rất phổ biến bởi vì chúng ta có thể đo quy trình theo nhiều cách vì vậy mà

thường lấy mẫu giá trị để đưa ra khuynh hướng thay đổi của giá trị nhiễu.
Sự xác định rõ tương quan nhiễu q trình Q thường rất khó bởi vì điều điển hình là
chúng ta khơng có khả năng quan sát trực tiếp tiến trình mà chúng ta đang ước lượng.
Đôi khi sự liên hệ tới những quy trình mẫu đơn giản có thể đưa ra những giá trị chấp
nhận được nếu một mẫu xen vào không chắc chắn đủ với tiến trình thơng qua sự lựa
chọn Q . Chắc chắn trong trường hợp này,mẫu đó sẽ hi vọng rằng giá trị tiến trình là
đáng tin cậy.
Trong những trường hợp khác, dù muốn hay không chúng ta đều có cái chuẩn
hợp lí cho việc lựa các thơngsố, thường thì chất lượng bộ lọc sẽ tốt hơn nhiều lần khi
có chứa sự hiệu chỉnh các tham số Q và R.Sự hiệu chỉnh thường được thực hiện gián
tiếp, thường thì với sự giúp đỡ của một bộ lọc Kalman khác trong quy trình chung, liên
hệ như một hệ thống đồng nhất.
Với điều kiện Q và R là các hằng số thực,cả hai cho phép ước lượng sai số
tương quan 𝑃𝑘và độ lợi Kalman 𝐾𝑘 sẽ ổn định nhanh chóng và sau đó trở thành hằng
số.
Trong điều kiện luận văn, thơng số Q và R được hiệu chỉnh dựa vào quá trình
thử sai để dự đốn khuynh hướng hiệu chỉnh của hệ thống và tìm ra bộ thơng số phù
hợp nhất.

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

21


3. Giải thuật điều khiển
3.1 Bộ điều khiển PID.
Bộ điều khiển PID với thông số K p , K I , K Dcố định như trên, hệ thống chỉ làm
việc tốt trong điều kiện hệ số K p , K I , K Dđã được chỉnh định tối ưu và trong q trình
làm việc, các thơng số trong mơ hình khơng đổi.


Hình 8: Sơ đồ hệ thống điều khiển sử dụng PID.
Giải thuật tính tốn bộ điều khiển PID được cho bởi công thức:
u ( t )=K p e ( t )+ K I ∫ e ( t ) + K D

[2.63]

de (t)
dt

Trong đó các hệ số K p , K I , K Dlần lượt là các thông số đặc trưng của các khâu tỷ
lệ, tích phân, vi phân của bộ điều khiển PID. Giá trị K p tác động lên sai số hiện tại, giá
trị K I tác động lên sai số trong quá khứ, trong khi đó giá trị K D ảnh hưởng lên tốc độ
thay đổi của sai số. Ảnh hưởng của ba thông số K p , K I , K Dđến chất lượng của hệ thống
được tổng hợp trong Hình 9: Ảnh hưởng của các thông số bộ điều khiển PID đến hệ
thống Thông số Thời gian lên ts Thời gian quá độ tqd Độ vọt lố Sai số xác lập K P
Giảm Thay đổi nhẹ Tăng Giảm K I Giảm Tăng Tăng Khử K D Thay đổi nhẹ Giảm Giảm
Không thay đổi.
Mặc dù bộ điều khiển PID có thể áp dụng rộng rãi cho nhiều vấn đề điều khiển
ngay cả khi chúng ta khơng nắm được mơ hình tốn mơ tả hệ thống. Tuy nhiên, khó
khăn cơ bản của bộ điều khiển PID là nó là một hệ thống điều khiển phản hồi với các
thơng số khơng đổi do đó nó sẽ nhạy cảm với nhiễu và thường khơng đáp ứng được
bài tốn tối ưu. Để hệ thống hoạt động ổn định ở mức chấp nhận được, việc xác định
các hệ số K p , K I , K Dcủa bộ điều khiển PID là cần thiết và không phải là một bài toán
dễ dàng.

22


Thông số


Thời gian lên t s

Thời gian quá độ t qd

Độ vọt lố

Sai số xác lập

Kp

Giảm

Thay đổi nhẹ

Tăng

Giảm

KI

Giảm

Tăng

Tăng

Khử

KD


Thay đổi nhẹ

Giảm

Giảm

Không thay đổi

Hình 9: Ảnh hưởng của các thơng số bộ điều khiển PID đến hệ thống.
Mặc dù bộ điều khiển PID có thể áp dụng rộng rãi cho nhiều vấn đề điều khiển
ngay cả khi chúng ta không nắm được mơ hình tốn mơ tả hệ thống. Tuy nhiên, khó
khăn cơ bản của bộ điều khiển PID là nó là một hệ thống điều khiển phản hồi với các
thông số khơng đổi do đó nó sẽ nhạy cảm với nhiễu và thường khơng đáp ứng được
bài tốn tối ưu. Để hệ thống hoạt động ổn định ở mức chấp nhận được, việc xác định
các hệ số K p , K I , K Dcủa bộ điều khiển PID là cần thiết và khơng phải là một bài tốn
dễ dàng. Phần tiếp theo giới thiệu thuật tốn tối ưu hóa ngẫu nhiên Jaya và việc áp
dụng nó trong bài tốn xác định các thông số cho bộ điều khiển PID để điều khiển hệ
con lắc ngược đơn và xe.

3.2 Cấu trúc bộ điều khiển PID cho robot hai bánh tự cân bằng
Ba bộ PID được sử dụng để điều khiển robot hai bánh tự cân bằng, bao gồm:
-

Bộ PID điều khiển góc nghiêng (ψ ¿

-

Bộ PID điều khiển vị trí ( θ )

-


Bộ PID điều khiển góc xoay ( ϕ )

Hình 10: Cấu trúc bộ điều khiển PID cho hệ robot hai bánh tự cân bằng.
Hàm truyền đạt bộ điều khiển PID liên tục:
G PID ( S )=

(

KI
U (S )
s
=K P + + K D
E (S )
s
1+ τS

)

[2.51]

Rời rạc hóa đạo hàm theo thời gian:
23


y (k )≈

f ( k )−f (k−1)
Ts


[2.52]

Rời rạc hóa tích phân theo thời gian:
k .T s

∫

y ( t ) dt= y ( k ) ≈ y ( k −1 )+

0

f ( k )−f ( k−1 )
.T s
2

[2.53]

Phép biến đổi rời rạc (z-Tranform)
∞

X [ z ] =Z { x [ k ] }=∑ x [k ] z

−k

[2.54]

z= A e = A (cos θ+ J sin θ)

[2.55]


Ta có Z { x [ k−n ] }=z−n X [ z ] và Z { x [ k ] }=X [z ]

[2.56]

k=0

jθ

Do đó:
−1
f ( k )−f (k −1)
F [ z ] −z F [ z ]
y [ z ] z −1
y ( k )=
→ y [ z ]=
⟹
=
Ts
Ts
F [z] zTs

[2.57]

Và
y ( k )= y ( k −1 ) +

Ts
f ( k ) −f ( k−1)
y [ z ] T s z+1
−1

−1
. T s → Y [ z ] ( 1−z )= F [ z ] ( 1+ z ) ⇒
=
2
2
F [ z ] 2 z −1

T z+ 1
U [ z]
z−1
=K p+ K i s
+Kd
E[ z]
2 z−1
z Ts
K p ( z −z ) + K i
2

⇔

⇔

⇔

U [z ]
=
E[z]
U [z ]
=
E[z]

U [z ]
=
E[z]

[2.58]

[2.59]

Ts 2
K
( z + z ) + d (z 2−2 z +1)
2
Ts
2

z −z

(

K p+ K i

(

K p+ K i

) (

)

T s Kd 2

T s Kd
Kd
+
z + −K p+ K i +
Z+
2 Ts
2 Ts
Ts
z2 −z

)(

)

T s Kd
T s K d −1 k d −2
+
+ −K p + K i +
z + z
2 Ts
2 Ts
Ts
1−z−1

⇔ U [ z ] =z−1 U [ z ] + aE [ z ] +b z −1 E [ z ] +c z−2 E [z ]

[2.60]

⇔ u [ k ] =u [ k−1 ] +ae [ k ] + be [ k−1 ] +ce [k −2]


[2.61]
24


Trong đó: a=K p + K i

Ts Kd
Ts Kd
Kd
+
; b=−K p + K i + ; c=
2 Ts
2 Ts
Ts

[2.62]

4. Các thành phần chính của mơ hình
4.1. Mạch Điều Khiển Động Cơ Bước A4988
A4988 là một trình điều khiển vi
bước để điều khiển động cơ bước
lưỡng cực có bộ phiên dịch tích hợp để
vận hành dễ dàng. Điều này có nghĩa
là chúng tơi có thể điều khiển động cơ
Hình 11: Mạch điều khiển động
cơ bước A4988.

bước chỉ với 2 chân từ bộ điều khiển
của chúng tôi hoặc một chân để điều
khiển hướng quay và một chân để điều

khiển các bước.

THÔNG SỐ KỸ THUẬT
 Cơng suất ngõ ra lên tới 35V, dịng đỉnh 2A.
Cú 5 ch : Full bc, ẵ bc, ẳ bước, 1/8 bước, 1/16 bước
 Điểu chỉnh dòng ra bằng triết áp, nằm bên trên Current Limit = VREF ×
2.5
 Tự động ngắt điện khi quá nhiệt.
Trình điều khiển cung cấp năm độ phân giải bước khác nhau: bước đầy đủ,
bước cao hơn, bước một phần tư, bước tám và bước thứ mười sáu. Ngồi ra, nó có một
chiết áp để điều chỉnh đầu ra hiện tại, ngắt nhiệt quá nhiệt và bảo vệ dòng điện chéo.

25