1


Chương 1
GIỚI THIỆU

1.1 Giới thiệu
Cùng với phát triển của khoa học kỹ thuật, ngày nay robot có khả năng thay
thế con người làm việc trong những môi trường độc hại, trong sản xuất hoặc bắt
chước con người về hình thức, hành vi và cả suy nghĩ Hiện nay lĩnh vực robot
đang phát triển nhanh nhờ vào sự phát triển liên tục của công nghệ, robot đã và
đang được chế tạo để phục vụ cho nhiều mục đích khác nhau. Với ý tưởng sử dụng
robot thay thế con người thì “Robot hai bánh tự cân bằng” cũng là đề tài hiện đang
được nhiều tác giả quan tâm vì mô hình robot này có khả năng di chuyển nhanh,
linh hoạt nhưng lại không chiếm nhiều không gian [1], [3].
1.2 Mục tiêu của đề tài
Mục tiêu của đề tài là xây dựng mô hình robot hai bánh thực tế có khả năng tự
cân bằng có cấu trúc đơn giản phục vụ cho việc nghiên cứu, giảng dạy tại phòng thí
nghiệm, mô hình sản phẩm trưng bày hay những trò chơi với mục đích giải trí.
1.3 Phương pháp nghiên cứu
Đề tài được tiếp cận dựa trên các phương pháp sau:
 Phương pháp khảo sát tài liệu, tìm hiểu các tài liệu liên quan đến đến đề tài
như: cấu trúc robot hai bánh tự cân bằng, cảm biến IMU, mạch điều khiển động cơ.
 Phương pháp khảo sát các thuật toán lọc nhiễu cho cảm biến IMU như: bộ
lọc Kalman, bộ lọc Complementary và thuật toán điều khiển PID.
 Phương pháp thực nghiệm tiến hành xây dựng các thuật toán trên mô hình
robot hai bánh thực tế.
1.4 Giới hạn của đề tài
Đề tài tập trung vào việc xây dựng mô hình phần cứng robot như: kết cấu cơ
khí, mạch điều khiển động cơ, các thuật toán trên vi điều khiển như bộ lọc Kalman
2

giải thuật cân bằng PID. Robot chỉ có thể cân bằng tại một vị trí chưa có khả năng
vừa di chuyển vừa cân bằng.
1.5 Tình hình nghiên cứu trong nước và trên thế giới
1.5.1 Trong nước
Mô hình robot hai bánh tự cân bằng luận văn thạc sĩ của tác giả Nguyễn Gia
Minh Thảo, trường Đại học Bách Khoa TP Hồ Chí Minh.

Hình 1.1 Mô hình robot hai bánh cân bằng
1.5.2 Nước ngoài
Một số kỹ thuật viên và sinh viên ngành Cơ điện tử, Tự động hóa đã nghiên
cứu cho ra đời nhiều dạng robot hai bánh cân bằng. Sau đây là một số thông tin về
các mô hình robot hai bánh tự cân bằng.
1.5.2.1 nBot
nBot do ông Anderson sáng chế. nBot lấy ý tưởng cân bằng là các bánh xe sẽ
phải chạy theo hướng mà phần trên robot sắp ngã. Nếu bánh xe có thể di chuyển để
trọng tâm robot luôn rơi vào chính giữa thì robot sẽ giữ được cân bằng.
3



Hình 1.2 Robot hai bánh cân bằng nBot
1.5.2.2 EquipoiseBot
EquipoiseBot sử dụng hai cảm biến là cảm biến gia tốc và cảm biến con quay
hồi chuyển. Góc nghiêng của robot được tính toán từ hai giá trị khác nhau. Một từ
cảm biến gia tốc bằng cách sử dụng thuật toán Tangens-Funktion và một từ cảm
biến con quay hồi chuyển sử dụng phương pháp tích phân. Hai giá trị này được kết
hợp trong một bộ lọc để cho ra một giá trị mới và được sử dụng cho đầu vào bộ PID
để điều khiển các động cơ, giữ cho robot cân bằng.


Hình 1.3 Robot hai bánh cân bằng EquipoiseBot

4


1.5.2.3 WobblyBot
WobblyBot thực chất là mô hình sử dụng con lắc ngược, được gắn dưới phần
thân ở giữa hai bánh xe. WobblyBot được thiết kế với phần dưới của robot nặng
hơn nhiều so với phần trên giúp robot có khả giữ cân bằng tốt nhất.

Hình 1.4 Robot hai bánh cân bằng WobblyBot
1.5.2.4 tiltOne
tiltOne là một robot hai bánh cân bằng với chiều cao 90 cm và có khả năng
chở tải trọng lên tới 50 kg. Nguyên lý hoạt động cũng giống như các robot hai bánh
có kích thước nhỏ hơn, sử dụng hai cảm biến là cảm biến gia tốc và cảm biến con
quay hồi chuyển và thuật toán PID để điều khiển robot cân bằng.

Hình 1.5 Robot hai bánh cân bằng tiltOne
5


1.5.2.5 Robot kiểu rolling của hãng TOYOTA
Đây là robot có công dụng phục vụ con người do hãng TOYOTA thiết kế.
Mẫu robot này có khả năng di chuyển nhanh và ít chiếm không gian. Đồng thời đôi
tay của robot có thể làm nhiều công việc khác nhau, chủ yếu được sử dụng với mục
đích giải trí.


Hình 1.6 Robot hai bánh của hãng TOYOTA

1.6 Nội dung đề tài
Nội dung phần còn lại của đề tài gồm các chương như sau:
 Chương 2: THIẾT KẾ MÔ HÌNH HỆ THỐNG
Nội dung chương 2 trình bày sơ đồ khối hệ thống, xây dựng mô hình robot hai
bánh thực tế, thiết kế hệ thống điện và các thành phần tham gia điều khiển robot
như: bo Arduino Due, mạch điều khiển động cơ, cảm biến IMU GY-86, động cơ
Planet và Pin LI-PO. Phần cuối chương giới thiệu các phương thức giao tiếp sử
dụng trên mô hình như: điều chế độ rộng xung PWM, giao thức I
2
C, giao tiếp
UART và thu thập dữ liệu để vẽ dạng sóng ngõ ra trên phần mềm Labview.
 Chương 3: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN CÂN BẰNG
Phần đầu chương 3 giới thiệu về nguyên lý điều khiển cân bằng của robot, giới
thiệu tổng quan về bộ lọc Kalman, cơ sở, bản chất, giải thuật của bộ lọc Kalman rời
rạc. Giải thuật điều khiển PID được sử dụng để đáp ứng ngõ ra nhanh, chính xác
6


trong việc cân bằng robot và cuối cùng là giải thuật điều khiển robot cân bằng bám
theo vị trí ban đầu.
 Chương 4: KẾT QUẢ THỰC NGHIỆM
Chương 4 trình bày kết quả thực nghiệm về độ chính xác khi sử dụng bộ lọc
Complementary, bộ lọc Kalman so với dữ liệu thô từ cảm biến, xây dựng giải thuật
điều khiển PID trên mô hình thực tế.
 Chương 5: KẾT LUẬN
Nội dung chương 5 trình bày tóm tắt các kết quả mà đề tài đã đạt được và
hướng phát triển để khắc phục những giới hạn nhằm hoàn thiện đề tài tốt hơn.




















7


Chương 2
THIẾT KẾ MÔ HÌNH HỆ THỐNG

2.1 Sơ đồ khối của hệ thống
Cảm biến IMU
GY-86
Arduino Due
AT91SAM3X8E
Mạch điều khiển
động cơ cầu H
Nguồn cung cấp

I C
2
PWM

Hình 2.1 Sơ đồ khối hệ thống
2.2 Tính toán và thiết kế cho mô hình hệ thống
Giả sử góc nghiêng mà robot có thể giữ cân bằng được giới hạn trong khoảng
±20
0
so với góc tại vị trí cân bằng lý tưởng 90
0
theo trục y.
50 cm
a) Robot ở trạng
thái cân bằng
50 cm
20
0
70
0
A B
C
α
AB = ?
b) Trạng thái robot đang ở
góc nghiêng 20 so với góc
tại vị trí cân bằng
0

Hình 2.2 Quãng đường robot cần di chuyển để thực hiện cân bằng khi nghiêng một

góc 20
0
so với góc tại vị trí cân bằng
8


Như mô tả trong Hình 2.2 để robot có thể giữ được cân bằng khi ngã thì
robot phải di chuyển một đoạn đường là  trong khoảng thời gian  cho phép để
có thể hứng được trọng tâm của robot, các giá trị này được tính toán như sau:
    

   (2.1)
50 cm
a) Robot ở trạng
thái cân bằng
50 cm
20
0
70
0
A B
C
α
17.1 cm
t = ?
b) Trạng thái robot đang ở
góc nghiêng 20 so với góc
tại vị trí cân bằng
0


Hình 2.3 Mô tả thời gian robot ngã từ trạng thái cân bằng đến một góc
nghiêng 20
0

Thời gian ngã của robot từ vị trí cân bằng đến góc nghiêng giới hạn được xác
định bằng phương pháp thực nghiệm trên mô hình thực tế. Giá trị góc nghiêng được
thu thập theo thời gian và được truyền về máy tính như trong Bảng 2.1.
Bảng 2.1 Bảng thu thập thời gian robot ngã do lực hút của Trái Đất
Thời
gian
(ms)
Góc nghiêng theo trục y (độ)
Lần 1
Lần 2
Lần 3
Lần 4
0
88.42
0

88.01
0

88.27
0

88.45
0

50

88.32
0

88.05
0

88.23
0

88.41
0

100
88.01
0

87.77
0

87.89
0

88.12
0

150
88.80
0

88.68

0

88.67
0

88.97
0

200
88.51
0

88.41
0

88.37
0

88.67
0

9


250
87.71
0

87.30
0


88.07
0

88.41
0

300
87.69
0

87.45
0

87.17
0

88.10
0

350
87.38
0

87.18
0

86.85
0


87.83
0

400
86.15
0

86.01
0

85.98
0

86.01
0

450
85.74
0

85.40
0

86.17
0

86.35
0

500

85.27
0

85.13
0

85.57
0

85.91
0

550
84.08
0

84.49
0

85.21
0

84.63
0

600
83.30
0

83.92

0

84.56
0

84.76
0

650
83.59
0

82.47
0

83.32
0

83.78
0

700
82.13
0

82.14
0

82.53
0


82.66
0

750
80.12
0

80.71
0

80.40
0

81.61
0

800
78.12
0

79.01
0

77.44
0

79.48
0


850
74.64
0

76.19
0

74.64
0

76.45
0

900
70.86
0

70.52
0

70.79
0

73.06
0

950
66.32
0


64.82
0

66.54
0

70.19
0


Dữ liệu thu thập được trong Bảng 2.1 cho thấy khi thả robot ngã tự do từ vị trí
cân bằng là 88
0
(đây là góc nghiêng tại vị trí cân bằng thu được từ cảm biến trên mô
hình thực tế, góc này sai lệch so với góc nghiêng lý tưởng do giới hạn về cơ khí của
mô hình) đến góc nghiêng giới hạn 88 ± 20
0
trong khoảng thời gian trung bình là
950 ms, đây cũng là giá trị thời gian giới hạn cần thiết để robot di chuyển để giữ
được cân bằng.
Giả sử chọn bánh xe có bán kính là 5cm và moment xoắn của động cơ là 63
Ncm thì lực đẩy sẽ được tính theo công thức sau:
 





  (2.2)
Suy ra vận tốc cần thiết của động cơ:

 








 (2.3)
Công suất động cơ được tính theo công thức:
       (2.4)
10


Dựa vào các giá trịnh tính toán, thông số của động cơ được chọn như trong
Bảng 2.2 để đáp ứng các yêu cầu trên.
Bảng 2.2 Chọn động cơ với các thông số
Động cơ
DC planet
Công suất
60W
Moment xoắn
63 N.cm
Tốc độ chưa qua bộ giảm tốc
9000 vòng/phút
Tốc độ khi đã qua bộ giảm tốc
468 vòng/phút

2.3 Mô hình robot hai bánh thực tế

Đề tài sử dụng bo mạch Arduino Due AT91SAM3X8E đóng vai trò như “bộ
não” của robot, điều khiển cho robot giữ cân bằng. Khung robot được chế tạo từ
inox với hai động cơ DC được đặt đồng trục, cho phép robot có thể di chuyển được
theo hai hướng trước và sau. Hai động cơ được điểu khiển bởi hai mạch cầu H sử
dụng IC IR2184 để điều khiển kích FET, cảm biến IMU GY-86 được sử dụng để
xác định góc nghiêng của robot. Hai động cơ DC 24V, 60W tốc độ 468 vòng/phút,
encoder 500 xung/vòng. Hai bánh xe được bọc cao su có nhiều rãnh để tăng độ bám
cao giúp robot cân bằng tốt hơn.
Cảm biến
IMU GY-86
Mạch điều khiển
động cơ cầu H
Bo Arduino Due
AT91SAM3X8E
Nút nhấn
điều khiển
Pin Li-PO
550 mm
450 mm
Động cơ DC

Hình 2.4 Mô hình robot hai bánh tự cân bằng
11


2.4 Hệ thống điện trên mô hình robot
2.4.1 Mạch điều khiển Arduino Due
Arduino Due là bo mạch sử dụng vi điều khiển của hãng Atmel SAM3X8E
ARM Cortex-M3 CPU. Đây là bo Arduino đầu tiên dựa trên bộ vi điều khiển lõi
ARM 32-bit. Bo Arduino Due có 54 ngõ vào/ngõ ra là tín hiệu số (trong đó có 12

chân có thể xuất tín hiệu PWM), 12 chân ngõ vào analog và 2 chân ngõ ra DAC,
xung nhịp 84 MHz, một cổng kết nối USB OTG, jack nguồn, nút nhấn reset CPU và
các cổng giao tiếp đa dạng như: 4 UART, SPI, TWI (I
2
C) và JTAG.
Bo Arduino Due có thể được cấp nguồn và nạp chương trình từ máy tính
thông qua cáp USB hoặc sử dụng nguồn cấp từ adapter với điện áp từ 7 đến
12VDC.

Hình 2.5 Bo Arduino Due AT91SAM3X8E
 Thông số kỹ thuật bo Arduino Due AT91SAM3X8E
Bảng 2.3 Thông số kỹ thuật bo Arduino Due AT91SAM3X8E

Microcontroller
AT91SAM3X8E
Operating Voltage
3.3V
Input Voltage (recommended)
7-12V
Input Voltage (limits)
6-16V
Digital I/O Pins
54 (of which 12 provide PWM output)
Analog Input Pins
12
Analog Outputs Pins
2 (DAC)
Total DC Output Current on all
130 mA
12



DC Current for 3.3V Pin
800 mA
DC Current for 5V Pin
800 mA
Flash Memory
512 KB all available for the user applications
SRAM
96 KB (two banks: 64KB and 32KB)
Clock Speed
84 MHz

2.4.2 Mạch điều khiển động cơ
Mạch cầu H dùng IC kích FET chuyên dụng IR2184 cho phép điều khiển động
cơ với dòng liên tục 10A và dòng tức thời lên đến 30A. Mạch này được thiết kế
dành cho các ứng dụng điều khiển tốc độ và vị trí cho động cơ DC.

Hình 2.6 Mạch điều khiển động cơ cầu H
 Đặc tính kỹ thuật của mạch:
 Dòng liên tục 10A
 Dòng đỉnh 30A (200 ms)
 Điện áp cấp từ 24V
 Có Led báo nguồn cho mạch
 Có Led báo chiều động cơ
 Bảo vệ ngắn mạch
 Dùng IC kích FET chuyên dụng IR2184


13



2.4.3 Cảm biến IMU GY-86
Cảm biến IMU GY-86 bao gồm các cảm biến đo lường quán tính như: cảm
biến vận tốc góc (gyroscope) 3 trục, cảm biến gia tốc (accelerometer) 3 trục, cảm
biến từ trường (magnetometer) 3 trục và cảm biến áp suất không khí (barometer).
Mô đun này được sử dụng nhiều trong việc điều khiển các thiết bị vận hành tự động
cần định hướng như: robot tự hành, UAVs (thiết bị bay không người lái), robot và
xe hai bánh tự cân bằng hoặc các hệ thống cân bằng khác…
Các cảm biến trên mô đun hỗ trợ giao tiếp I
2
C với tốc độ lên tới 400kb/s và
hoạt động ở mức áp 3.3V. Mô đun được thiết kế tích hợp sẵn IC chuyển áp tín hiệu
và IC ổn áp LDO 3.3V, qua đó hoàn toàn tương thích với hệ thống ngoài với điện
áp là 3.3V hoặc 5V.

Hình 2.7 Cảm biến IMU GY-86
 Các thông số kỹ thuật của cảm biến IMU GY-86
 Nguồn cung cấp 3.3V - 5V
 Giao tiếp I
2
C ở mức 3.3V
 Sử dụng chip MPU6050 + HMC5883L + MS5611
2.4.4 Động cơ DC
Động cơ DC được sử dụng như đôi chân của robot giúp robot có thể di chuyển
hay điều khiển robot cân bằng.
14




Hình 2.8 Động cơ DC Planet
 Các thông số kỹ thuật của động cơ DC
Bảng 2.4 Thông số kỹ thuật của động cơ DC Planet
Động cơ
DC planet
Điện áp
24V
Công suất
60W
Moment xoắn
63 N.cm
Endcoder
HEDL 5540 12
Kích thước

Trục

Tốc độ chưa qua bộ giảm tốc
9000 vòng/phút
Tốc độ khi đã qua bộ giảm tốc
468 vòng/phút

2.4.5 Nguồn điện
Pin LI-PO được sử dụng làm nguồn cung cấp năng lượng cho robot hoạt động.

15



Hình 2.9 Pin LI-PO sử dụng cho robot

 Thông số kỹ thuật của Pin LI-PO
 Công suất tối thiểu: 2650mAh
 Cấu hình: 3S1P / 11.1V / 3Cell
 Xả liên tục: 20C
 Đỉnh xả (10 giây): 30C
 Trọng lượng: 225g
 Kích thước: 137 x 43 x 17mm
 Jack cắm: JST-XH
2.5 Các phương thức giao tiếp
2.5.1 Điều chế độ rộng xung PWM
Phương pháp điều chế độ rộng xung PWM (PulseWidth Modulation) là
phương pháp điều chỉnh điện áp ra tải, hay nói cách khác là phương pháp điều chế
dựa trên sự thay đổi độ rộng của chuỗi xung vuông dẫn đến sự thay đổi điện áp ngõ
ra.
Các tín hiệu PWM khi biến đổi thì có cùng một tần số và khác nhau về độ
rộng của sườn dương hay sườn âm.
16



Hình 2.10 Sơ đồ dạng xung điều chế PWM
Hình 2.10 là đồ thị dạng xung khi điều khiển bằng PWM với độ rộng xung đầu
ra tương ứng và được tính bằng %.
 Ứng dụng của PWM trong điều khiển
PWM được ứng dụng nhiều trong điều khiển, điển hình nhất là điều khiển
động cơ và các bộ xung áp, điều áp Sử dụng PWM điều khiển độ nhanh chậm của
động cơ hay cao hơn nữa, PWM còn được dùng để điều khiển sự ổn định tốc độ
động cơ.
Ngoài lĩnh vực điều khiển hay ổn định tải thì PWM còn được sử dụng để điều
chế các mạch nguồn như : boot, buck, nghịch lưu 1 pha và 3 pha [2].

2.5.2 Giao thức I
2
C
Giao thức ưu tiên truyền thông nối tiếp được phát triển bởi Philips
Semiconductor và được gọi là bus I
2
C. Vì nguồn gốc được thiết kế là để điều khiển
liên thông IC (Inter Intergrated Circuit) nên được đặt tên là I
2
C. Tất cả các chip có
tích hợp và tương thích với I
2
C đều có thêm một giao diện tích hợp trên chip để
truyền thông trực tiếp với các thiết bị tương thích I
2
C khác. Việc truyền dữ liệu nối
tiếp theo hai hướng 8 bit được thực thi theo 3 chế độ sau: chuẩn (standard) -
100Kbits/sec, nhanh (fast) - 400Kbits/sec, tốc độ cao (high speed) - 3.4Mbits/sec.
17


Đường bus thực hiện truyền thông nối tiếp I
2
C gồm hai đường là đường truyền dữ
liệu nối tiếp SDA và đường truyền nhịp xung đồng hồ nối tiếp SCL. Vì cơ chế hoạt
động là đồng bộ nên cần có một nhịp xung tín hiệu đồng bộ. Các thiết bị hỗ trợ I
2
C
đều có một địa chỉ định nghĩa trước, trong đó một số bit địa chỉ thấp có thể cấu
hình. Thiết bị khởi tạo quá trình truyền thông là thiết bị chủ và cũng là thiết bị tạo

xung nhịp đồng bộ, điều khiển cho phép kết thúc quá trình truyền. Nếu thiết bị chủ
muốn truyền thông với thiết bị khác sẽ gửi kèm thông tin địa chỉ của thiết bị muốn
truyền trong dữ liệu truyền. Thiết bị tớ đều được gán và định địa chỉ thông qua đó
thiết bị chủ có thể thiết lập truyền thông và trao đổi dữ liệu. Bus dữ liệu được thiết
kế để cho phép thực hiện nhiều thiết bị chủ và tớ ở trên cùng bus.
 Quá trình truyền thông I
2
C được mô tả như sau:
Tín hiệu START được tạo ra bởi thiết bị chủ, sau đó sẽ truyền đi dữ liệu 7 bit
chứa địa chỉ của thiết bị tớ mà thiết bị chủ muốn truyền thông, theo thứ tự là các bit
có trọng số lớn nhất MSB sẽ được truyền trước. Bit thứ tám tiếp theo sẽ chứa thông
tin để xác định thiết bị tớ sẽ thực hiện vai trò nhận (0) hay gửi (1) dữ liệu. Tiếp theo
sẽ là một bit ACK xác nhận bởi thiết bị nhận đã nhận được 1 byte trước đó hay
không. Thiết bị truyền (gửi) sẽ truyền đi 1 byte dữ liệu bắt đầu bởi MSB. Tại điểm
cuối của byte truyền, thiết bị nhận sẽ tạo ra một bit xác nhận ACK mới. Khuôn mẫu
9 bit này (gồm 8 bit dữ liệu và 1 bit xác nhận) sẽ được lặp lại nếu cần truyền tiếp
byte nữa. Khi thiết bị chủ đã trao đổi xong dữ liệu cần và sẽ quan sát bit xác nhận
ACK cuối cùng rồi sau đó sẽ tạo ra một tín hiệu dừng STOP để kết thúc quá trình
truyền thông. I
2
C là một giao diện truyền thông đặc biệt thích hợp cho các ứng dụng
truyền thông giữa các thiết bị trên cùng một bo mạch với khoảng cách ngắn và tốc
độ thấp. Ví dụ như truyền thông giữa CPU với các khối chức năng trên cùng một bo
mạch như: EEPROM, cảm biến, đồng hồ tạo thời gian thực. Hầu hết các thiết bị hỗ
trợ I
2
C hoạt động ở tốc độ 400Kbps, một số cho phép hoạt động ở tốc độ cao vài
Mbps. I
2
C khá đơn giản để thực thi kết nối nhiều thiết bị vì I

2
C hỗ trợ cơ chế xác
định địa chỉ.
18



Hình 2.11 Sơ đồ kết nối theo giao thức I
2
C
Mỗi dây SDA hay SCL đều được nối với điện áp dương của nguồn cấp thông
qua một điện trở kéo lên (pullup resistor). Sự cần thiết của các điện trở kéo này là
vì chân giao tiếp I
2
C của các thiết bị ngoại vi thường là dạng cực máng hở (open
drain or opencollector). Giá trị của các điện trở này khác nhau tùy vào từng thiết bị
và chuẩn giao tiếp, thường dao động trong khoảng 1KΩ đến 4.7KΩ.
2.5.3 Giao tiếp UART
UART là viết tắt của (Universal Asynchronous Receiver Transmitter). Thường
là một mạch tích hợp được sử dụng trong việc truyền dẫn dữ liệu nối tiếp giữa máy
tính và các thiết bị ngoại vi. Rất nhiều vi điều khiển hiện nay đã được tích hợp
UART, vì vấn đề tốc độ và độ điện dung của UART không thể so sánh với các giao
tiếp mới hiện nay nên các dòng PC và Laptop đời mới không còn tích hợp cổng
UART.
Cũng như giao tiếp SPI và I
2
C có 1 dây truyền dữ liệu và 1 dây được sử dụng
để truyền xung clock (SCL) để đồng bộ trong giao tiếp. Với UART thì không có
dây SCL, vấn đề được giải quyết khi mà việc truyền UART được dùng giữa hai vi
xử lý với nhau, đồng nghĩa với việc mỗi vi xử lý có thể tự tạo ra xung clock.

Để bắt đầu cho việc truyền dữ liệu bằng UART, một START bit được gửi đi
sau đó là các bit dữ liệu và kết thúc quá trình truyền là STOP bit.
19



Hình 2.12 Mô tả hoạt động của UART
Khi ở trạng thái chờ mức điện áp ở mức 1 (high). Khi bắt đầu truyền START
bit sẽ chuyển từ 1 xuống 0 để báo hiệu cho bộ nhận là quá trình truyền dữ liệu sắp
xảy ra. Sau START bit là đến các bit dữ liệu D0-D7 (theo Hình 2.10 các bit này có
thể ở mức 1 hoặc 0 tùy theo dữ liệu). Sau khi truyền hết dữ liệu thì đến bit Parity để
bộ nhận kiểm tra tính đúng đắn của dữ liệu truyền. Cuối cùng là STOP bit báo cho
thiết bị rằng các bit đã được gửi xong. Thiết bị nhận sẽ tiến hành kiểm tra khung
truyền nhằm đảm báo tính đúng đắn của dữ liệu.
 Các thông số cơ bản trong truyền nhận UART:
 Baud rate (tốc độ baud): khoảng thời gian dành cho 1 bit được truyền. Phải
được cài đặt giống nhau ở gửi và nhận.
 Frame (khung truyền): khung truyền quy định về số bit trong mỗi lần
truyền.
 Start bit: là bit đầu tiên được truyền trong một khung truyền. Báo hiệu cho
thiết bị nhận có một gói dữ liệu sắp được truyền đến.
 Data: dữ liệu cần truyền. Bit có trọng số nhỏ nhất LSB được truyền trước
sau đó đến bit MSB.
 Parity bit: kiểm tra dữ liệu truyền có đúng không.
 Stop bit: là 1 bit hoặc nhiều bit báo cho thiết bị rằng các bit đã được gửi
xong.
 Thiết bị nhận sẽ tiến hành kiểm tra khung truyền nhằm đảm bảo tính đúng
đắn của dữ liệu.



20


2.6 Thu thập dữ liệu


Hình 2.13 Mô tả hoạt động thu thập dữ liệu
Để thu thập được dữ liệu từ cảm biến thì một trong những UART của bo
Arduino Due được sử dụng. UART đã được chuyển đổi với các thiết bị USB tích
hợp và gửi dữ liệu đến máy tính thông qua cổng USB. Dữ liệu sau đó được hiển thị
trong "Serial monitor" được hỗ trợ bởi phần mềm Arduino và dữ liệu này được sử
dụng để vẽ dạng sóng ngõ ra của cảm biến trên phần mềm Labview.










21


Chương 3
XÂY DỰNG THUẬT TOÁN ĐIỀU
KHIỂN CÂN BẰNG

3.1 Nguyên lý điều khiển cân bằng


(a) Nghiêng trước (b) Cân bằng (c) Nghiêng sau
Hình 3.1 Mô tả quá trình cân bằng của robot hai bánh
Khi robot đứng thẳng tại vị trí cân bằng robot sẽ bị lực tác động làm cho robot
ngã về phía trước hoặc phía sau như Hình 3.1. Nếu robot bị đổ về phía trước như
Hình 3.1(a) thông qua cảm biến IMU robot sẽ xác định được góc nghiêng và điều
khiển hai bánh xe di chuyển về phía trước theo hướng mà robot bị nghiêng. Hay khi
robot bị đổ về phía sau như Hình 3.1(c) thì robot sẽ điều khiển hai bánh xe di
chuyển về phía sau theo hướng robot bị nghiêng cho đến khi góc nghiêng của robot
bằng với góc mà tại đó robot đứng thẳng thì robot sẽ dừng lại [4].
3.2 Bộ lọc Kalman
3.2.1 Giới thiệu
Bộ lọc Kalman về cơ bản là một thuật toán xử lý dữ liệu truy hồi được sử dụng
để ước lượng trạng thái của hệ thống. Khi đề cập đến trạng thái của hệ thống nghĩa
là đề cập đến vectơ  bao gồm biến miêu tả một số đặc tính riêng biệt của hệ
thống. Ví dụ như góc nghiêng của robot được xác định dựa vào giá trị của cảm biến
22


con quay hồi chuyển và góc nghiêng của robot được xác định dựa vào giá trị đọc về
từ cảm biến gia tốc.
Trong thực tế thì các biến trạng thái có thể bị nhiễu và không trực tiếp quan sát
được, điều này làm cho việc ước lượng trạng thái của hệ thống gặp nhiều khó khăn.
Để ước lượng được trạng thái thì bộ lọc Kalman phải truy cập vào phép đo của hệ
thống. Các phép đo này là tuyến tính, liên quan đến trạng thái và bị ảnh hưởng bởi
nhiễu. Nếu nguồn nhiễu này có phân bố Gaussian thì việc ước lượng sử dụng bộ lọc
là sự thống kê tối ưu nhất đối với phép đo này.
Bộ lọc Kalman được ứng dụng trong nhiều lĩnh vực, đặc biệt là trong điều
khiển và dự đoán. Bộ lọc Kalman được sử dụng trong điều khiển để ước lượng
trạng thái. Khi điều khiển một hệ thống điều quan trọng là phải biết được hệ thống

sẽ tiếp tục hoạt động như thế nào. Trong một hệ thống phức tạp không phải lúc nào
cũng có thể đo đạc được các biến, bộ lọc Kalman cung cấp thông tin không phải
bằng cách đo trực tiếp mà bằng phương pháp ước lượng giá trị của các biến này và
ước lượng nhiễu phép đo. Bộ lọc Kalman có thể được sử dụng để điều khiển quy
trình sản xuất, máy bay, tàu thủy, tàu vũ trụ và robot. Bộ lọc Kalman được sử dụng
để dự đoán trạng thái tương lai của hệ thống, trạng thái mà con người rất khó hoặc
không thể thực hiện như: dự đoán dòng chảy của các dòng sông trong cơn lũ, giá cả
giao dịch của thực phẩm … [5].
3.2.2 Bộ lọc Kalman rời rạc
3.2.2.1 Quá trình ước lượng
Bộ lọc Kalman đề cập đến bài toán tổng quát nhằm ước lượng trạng thái


của một quá trình được mô hình hóa một cách rời rạc theo thời gian bằng
một phương trình ngẫu nhiên tuyến tính như sau:


 

 

 

(3.1)
Với giá trị đo đạc là 

:


 


 

(3.2)
23


Trong đó wk và vk là hai vector biến ngẫu nhiên đại diện cho nhiễu hệ thống
và nhiễu đo đạc. Hai biến ngẫu nhiên này độc lập với nhau và được giả sử là tuân
theo phân bố Gaussian với giá trị trung bình bằng 0 và ma trận hiệp biến
(covariance matrix) lần lượt là Q và R.
p(w) ~ N(0, Q) (3.3)
p(v) ~ N(0, R) (3.4)
Trong thực tế, ma trận Q và R có thể thay đổi theo thời gian hoặc theo phép
đo, tuy nhiên các ma trận này được giả định là hằng số.
Ma trận  có kích thước  là ma trận liên kết của trạng thái trước, bước
thứ  và trạng thái hiện tại bước thứ . Ma trận  có kích thước  là ma trận
liên kết giữa ngõ vào tùy biến 

với trạng thái . Ma trận  có kích thước
, trong phương trình phép đo (3.2) liên kết trạng thái với kết quả đo đạc 

.
Chú ý rằng các ma trận Q, R, A, H có thể thay đổi theo thời gian (từng bước ),
nhưng ở đây chúng được giả sử là không đổi [5].
3.2.2.2 Giải thuật lọc Kalman rời rạc
Bộ lọc Kalman ước lượng tiến trình bằng cách sử dụng hình thức kiểm soát
phản hồi: bộ lọc ước lượng trạng thái của tiến trình tại vài thời điểm và sau đó thu
nhận sự phản hồi bằng các giá trị đo thực tế (có nhiễu). Như vậy, phương trình của
bộ lọc Kalman chia thành 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. Phương trình cập nhật thời gian chịu trách nhiệm dự đoán
trước (theo thời gian) sử dụng giá trị hiện tại và hiệp biến số ước lượng để dự đoán
ước lượng tiền nghiệm cho thời điểm kế tiếp. Phương trình cập nhật giá trị đo chịu
trách nhiệm đối với sự phản hồi nghĩa là kết hợp giá trị mới với ước lượng tiền
nghiệm nhằm hiệu chỉnh ước lượng hậu nghiệm.
Phương trình cập nhật thời gian có thể được xem như là phương trình dự đoán,
trong khi phương trình cập nhật phép đo có thể xem như phương trình hiệu chỉnh.
Thật vậy, thuật toán ước lượng cuối cùng rất giống với thuật toán dự đoán, hiệu
chỉnh nhằm giải quyết các vấn đề số như trong Hình 3.2 [5].
24



Hình 3.2 Quy trình của bộ lọc Kalman
Các công thức tương ứng với quá trình cập nhật thời gian được cho trong công
thức (3.5) và (3.6). Công thức tương ứng với quá trình cập nhật phép đo được cho
trong công thức (3.7), (3.8) và (3.9).



 

 

(3.5)



 




  (3.6)


 










 


(3.7)


 


 



 




(3.8)




  






(3.9)
Việc đầu tiên trong quá trình cập nhật giá trị đo là tính giá trị độ lợi Kalman,


. Bước kế tiếp là đo giá trị thực của quá trình 

và sau đó tìm ra giá trị hậu
nghiệm ước lượng bằng cách kết hợp với kết quả đo như trong công thức (3.8).
Bước cuối cùng là tìm hiệp biến số hậu nghiệm bằng công thức (3.9).
Sau mỗi bước cập nhật thời gian và cập nhật phép đo, quá trình được lặp lại
với ước lượng hậu nghiệm trước đó để dự đoán một ước lượng tiền nghiệm mới.
Quá trình lặp lại tự nhiên này là một đặc điểm khá thu hút của bộ lọc Kalman làm
cho việc xây dựng bộ lọc Kalman dễ thực hiện hơn so với bộ lọc Wiener vốn được
thiết kế để hoạt động trực tiếp trên tất cả các dữ liệu ước lượng. Thay vì vậy, bộ lọc
Kalman tham chiếu các giá trị ước lượng hiện tại với tất cả các phép đo trước đó.

Hình 3.3 cho thấy toàn bộ hoạt động của bộ lọc [6].
25



Hình 3.3 Quy trình hoàn chỉnh của bộ lọc Kalman
3.3 Xây dựng bộ lọc Kalman áp dụng cho cảm biến IMU
Cảm biến IMU gồm hai cảm biến tích hợp là cảm biến gia tốc và cảm biến con
quay hồi chuyển. Bộ lọc Kalman được sử dụng để kết hợp các giá trị từ cảm biến
gia tốc và cảm biến con quay hồi chuyển để cho ra các góc điều khiển chính là: x, y,
và z.
Bộ lọc Kalman được xây dựng theo các bước như sau: Đầu tiên xây dựng
mảng của trạng thái trước đó:


(3.10)
Đó là trạng thái trước đó ước tính dựa trên trạng thái trước đó và dự toán của
các trạng thái trước khi xảy ra.
Tiếp theo là một trạng thái tiên nghiệm:


(3.11)
Một tiên nghiệm có nghĩa là ước tính của ma trận trạng thái tại thời điểm hiện
tại k dựa trên trạng thái trước đó của hệ thống và dự toán của các trạng thái trước
khi xảy ra.
Và cuối cùng đây được gọi là trạng thái hậu :