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 I2C, 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

I 2C PWM
Cảm biến IMU Arduino Due Mạch điều khiển
GY-86 AT91SAM3X8E động cơ cầu H

Nguồn cung cấp

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
±200 so với góc tại vị trí cân bằng lý tưởng 900 theo trục y.

0
20
50 cm 50 cm

0
70
A α B

AB = ?

a) Robot ở trạng b) Trạng thái robot đang ở

thái cân bằng góc nghiêng 200 so với góc
tại vị trí cân bằng
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 200 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)
t = ?
C

0
20
50 cm 50 cm

0
70
A α B

17.1 cm

a) Robot ở trạng b) Trạng thái robot đang ở

thái cân bằng góc nghiêng 20 0 so với góc
tại vị trí cân bằng
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 200
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 Góc nghiêng theo trục y (độ)
gian
Lần 1 Lần 2 Lần 3 Lần 4
(ms)
0 88.420 88.010 88.270 88.450
50 88.320 88.050 88.230 88.410
100 88.010 87.770 87.890 88.120
150 88.800 88.680 88.670 88.970
200 88.510 88.410 88.370 88.670
 9

250 87.710 87.300 88.070 88.410

300 87.690 87.450 87.170 88.100
350 87.380 87.180 86.850 87.830
400 86.150 86.010 85.980 86.010
450 85.740 85.400 86.170 86.350
500 85.270 85.130 85.570 85.910
550 84.080 84.490 85.210 84.630
600 83.300 83.920 84.560 84.760
650 83.590 82.470 83.320 83.780
700 82.130 82.140 82.530 82.660
750 80.120 80.710 80.400 81.610
800 78.120 79.010 77.440 79.480
850 74.640 76.190 74.640 76.450
900 70.860 70.520 70.790 73.060
950 66.320 64.820 66.540 70.190

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à 880 (đâ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 ± 200 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.

Bo Arduino Due Nút nhấn

điều khiển Pin Li-PO
AT91SAM3X8E
550 mm

450 mm

Cảm biến Động cơ DC Mạch điều khiển

IMU GY-86 động cơ cầu H

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 (I2C) 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 I2C 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 I2C ở 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 I2C. 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à I2C. Tất cả các chip có
tích hợp và tương thích với I2C đề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 I2C 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 I2C 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ợ I2C
đề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 I2C đượ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. I2C 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ợ I2C hoạt động ở tốc độ 400Kbps, một số cho phép hoạt động ở tốc độ cao vài
Mbps. I2C khá đơn giản để thực thi kết nối nhiều thiết bị vì I2C hỗ trợ cơ chế xác
định địa chỉ.
 18

Hình 2.11 Sơ đồ kết nối theo giao thức I2C

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 I2C 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à I2C 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 :
 26

̂ (3.12)
̂ ước tính của trạng thái tại thời điểm k và bao gồm cả lúc k ở hiện tại.
Vấn đề trạng thái hệ thống chính bị ẩn nên chỉ có thể được quan sát thông qua
quan sát. Điều này cũng được gọi là một mô hình Hidden Markov. Điều này có
nghĩa rằng trạng thái sẽ được dựa trên trạng thái tại thời điểm k và tất cả các trạng
thái trước đó .
Chiếc mũ trên các hệ số dùng để ước tính các trạng thái. Vì vậy trạng thái
được ký hiệu:
Trạng thái của hệ thống tại thời điểm k được cho bởi:
(3.13)
Mà là ma trận trạng thái đó được cho bởi :

̂ [ ̇] (3.14)

Có thể thấy đầu ra của bộ lọc sẽ là góc mà trạng thái dựa trên các phép đo từ
gia tốc và con quay hồi chuyển. Trạng thái là là các con quay hồi chuyển đã trôi dạt
. Điều này có nghĩa rằng có thể tính toán bằng cách trừ trạng thái từ các phép đo con
quay hồi chuyển.
Tiếp theo là ma trận , đó là mô hình chuyển trạng thái được áp dụng cho
tình trạng trước đó.
Trong trường hợp này được định nghĩa là:

A=[ ] (3.15)

Tiếp theo là kiểm soát đầu vào , trong trường hợp này là đo con quay hồi
chuyển trong độ mỗi giây (° / s ) tại thời điểm k , điều này cũng được gọi là tốc độ.
Viết lại phương trình trạng thái như sau:
̇ (3.16)
Tiếp theo là ma trận B. Được gọi là mô hình kiểm soát đầu vào, được định
nghĩa là:

B=[ ] (3.17)
 27

là nhiễu quá trình đó là phân phối Gaussian với hệ số trung bình bằng
không với hiệp phương sai đến thời điểm k.
(3.18)
là quá trình nhiễu phương sai ma trận và trong trường hợp này ma trận hiệp
phương sai dự toán trạng thái của gia tốc.
Ma trận cuối cùng được xác định như sau:

[ ] (3.19)
̇
Có thể thấy ma trận hiệp biến phụ thuộc vào thời gian hiện tại k, do sự thay
đổi của gia tốc.
Giá trị đo lường :
Sự đo lường của trạng thái thực sự. Quan sát được cho bởi:
(3.20)
H được gọi là ma trận quan sát và được sử dụng để lập bản đồ không gian
trạng thái thực sự vào không gian quan sát được. Khi đo là chỉ số đo từ các gia tốc,
H được cho bởi:
[ ] (3.21)
Tiếng ồn của phép đo phân bố Gaussian cũng như với trung bình bằng không
và là phương sai :
(3.22)
Nhưng đây không phải là một ma trận nhiễu đo lường và chỉ bằng phương sai
của đo lường, kể từ khi hiệp biến của cùng một biến bằng phương sai.
[ ] (3.23)
Nhiễu trong đo lường là như nhau và không phụ thuộc vào thời gian k:
(3.24)
 28

 Phương trình bộ lọc Kalman:

 Dự đoán:
Hai phương trình đầu tiên để dự đoán trạng thái hiện tại và ma trận hiệp
phương sai lỗi tại thời gian k. Đầu tiên các bộ lọc sẽ cố ước tính trạng thái hiện tại
dựa trên tất cả các trạng thái trước và dùng đo con quay hồi chuyển :
̇ (3.25)
Đó cũng là lý do tại sao được gọi là kiểm soát đầu vào, vì là một đầu vào thêm
để ước tính trạng thái tại thời điểm hiện tại k đựa trên trạng thái trước đó .
Tiếp theo là phương trình ước tính tiên hiệp phương sai lỗi ma trận dựa vào
ma trận hiệp phương sai lỗi trước đó:
(3.26)
Ma trận này được sử dụng để ước tính bao nhiêu giá trị thay đổi. Phương sai
lỗi sẽ tăng khi giá trị cập nhật lần cuối dự toán của trạng thái.
Ma trận hiệp phương sai lỗi trong trường hợp này là một ma trận 2 × 2:

[ ] (3.27)

 Cập nhật giá trị mới:

Tính toán giá trị và tiên nghiệm :
̃ ̂ (3.28)
Ma trận H được sử dụng để lập bản đồ trạng thái vào không gian quan sát là
đo lường từ các gia tốc.
̃ [ ̃] (3.29)
Tiếp theo là tính toán phương sai:
( ) (3.30)
sẽ để dự đoán, đo lường dựa trên một tiên ma trận hiệp biến lỗi và ma trận
hiệp biến đo lường. Mô hình quan sát H được sử dụng để lập bản đồ tiên ma trận
hiệp phương sai lỗi vào mô hình quan sát được.
trong trường hợp này không phải là một ma trận và chỉ được viết như sau:
[ ] (3.31)
 29

Bước tiếp theo là tính toán để tăng Kalman.

( ) (3.32)
Lưu ý rằng nếu không biết trạng thái khi khởi động, có thể thiết lập các ma
trận hiệp phương sai lỗi như sau:

[ ] (3.33)

Để tăng độ lợi Kalman là một ma trận 2 x 1:

[ ] (3.34)

Bây giờ có thể cập nhật một hậu ước tính của trạng thái hiện tại:
̂ ̂ ̃ (3.35)
Cuối cùng là cập nhật các ma trận hiệp phương sai lỗi hậu:
Nơi được gọi là ma trận đơn vị và được xác định là:
(3.36)
I được gọi là ma trận đơn vị và được xác định là :

I= [ ] (3.37)

 Thực hiện biến đổi các phương trình của bộ lọc:

 Cập nhật thời gian (dự đoán)
 Từ phương trình (3.25) suy ra:
̇

[ ̇] =[ ][ ̇] +[ ] ̇k

̇
[ ] +[ ] ̇k
̇
̇ ̇
=[ ]
̇
̇ ̇
=[ ]
̇
Vậy góc ước đoán tại bằng góc ước đoán trước đó + .
 Từ phương trình (3.26) biến đổi như sau:
 30

[ ] =[ ][ ] [ ] [ ]
̇

[ ] [ ] [ ]
̇

[ ] [ ]
̇

[ ]
̇

[ ]
̇

 Cập nhật giá trị đo (hiệu chỉnh)

 Biến đổi phương trình (3.28) được:
̃ ̂

=[ ][ ] [ ]

 Từ phương trình (3.30):

( )

=[ ][ ] [ ]

 Biến đổi phương trình (3.32):

( )

[ ] [ ] [ ]

[ ]
 31

[ ]

 Khai triển phương trình (3.35) như sau:

̂ ̂ ̃

[ ̇] [ ̇] [ ] ̃

̃
[ ̇] [ ]
̃

Cuối cùng là khai triển phương trình ma trận hiệp phương sai lỗi hậu (3.36):

[ ] [ ] [ ] [ ] [ ]

[ ] [ ] [ ]

[ ] [ ]

3.4 Giải thuật điều khiển PID

PID là viết tắt của cụm từ (Proportional Integral Derivative) là giải thuật được
dùng nhiều trong các ứng dụng điều khiển tự động với yêu cầu chính xác, nhanh và
ổn định. Bộ điều khiển PID bao gồm 3 thông số riêng biệt và được gọi là điều khiển
ba khâu: tỷ lệ, tích phân và đạo hàm, viết tắt là: P, I và D.
 Hàm truyền của bộ điều khiển PID như sau:

(3.38)

Trong đó:
: Độ lợi khâu tỷ lệ
: Độ lợi khâu tích phân

: Độ lợi khâu vi phân

 32

 Cấu trúc bộ điều khiển PID

Hình 3.4 Hệ thống điều khiển vòng kín

Trong đó:
Controller: Bộ điều khiển cung cấp tín hiệu điều khiển cho đối tượng điều
khiển.
Plant: Đối tượng điều khiển.
Biến e là thành phần sai lệch giữa giá trị mong muốn (R) và giá trị ngõ ra thực
tế (Y). Tín hiệu sai lệch (e) sẽ gửi tới bộ điều khiển PID, và bộ điều khiển tính toán
cả thành phần tích phân lẫn vi phân của (e). Tín hiệu ra (u) của bộ điều khiển bằng:

∫ (3.39)

Lúc này đối tượng điều khiển có tín hiệu vào là (u) và tín hiệu ra là (Y). (Y)
được hồi tiếp về bằng các cảm biến để tiếp tục tính sai lệch (e). Sau đó bộ điều
khiển tiếp tục lặp lại như trên.
 Đặc tính của bộ điều khiển PID
 Thành phần tỉ lệ (KP) có tác dụng làm tăng tốc độ đáp ứng của hệ và làm
giảm, chứ không triệt tiêu sai số xác lập của hệ (steady-state error).
 Thành phần tích phân (KI) có tác dụng triệt tiêu sai số xác lập nhưng có thể
làm giảm tốc độ đáp ứng của hệ.
 Thành phần vi phân (KD) làm tăng độ ổn định hệ thống, giảm độ vọt lố và
cải thiện tốc độ đáp ứng của hệ.
 Ảnh hưởng của các thành phần KP, KI, KD đối với hệ kín được tóm tắt trong
bảng sau:
 33

Bảng 3.1 Ảnh hưởng của các thành phần KP, KI, KD đối với hệ kín

Thông Thời gian đáp Độ vọt Thời gian xác Sai số xác
số ứng lố lập lập
KP Giảm Tăng Ít thay đổi Giảm

KI Giảm Tăng Tăng Triệt tiêu

KD Ít thay đổi Giảm Giảm Ít thay đổi

3.5 Xây dựng giải thuật PID điều khiển robot

Để tìm hệ thức PID rời rạc, xét đồ thị sau:

Hình 3.5 Đồ thị biểu diễn hệ số PID theo thời gian

Đường biểu diễn điểm mong muốn.
Đường biểu diễn điểm thực tế.
Đường là đồ thị rời rạc hóa của vận tốc động cơ.
∆t là thời gian lấy mẫu.
 Thành phần tích phân

∫ ∑ (3.40)

 Thành phần vi phân

[ ]
(3.41)
 34

Do đó khi lấy gần đúng ∆t = є >0 thì:

[ ]
(3.42)

Tóm lại:
[ ( ) ]
∑ (3.43)

Đặt:
(3.44)
∑ (3.45)
Trong công thức (3.44) trên thời gian lấy mẫu ∆t rất nhỏ, nên bỏ qua ∆t. Khi
tìm các hệ số Kp, Ki, Kd đã bao gồm cả ∆t.
Khi đó công thức lại được viết lại như sau:
(3.46)
Điều khiện biên: u(0) = duty > 0.
 Tìm hệ số PID bằng phương pháp thực nghiệm:
 Cho các hệ số Kp, Ki, Kd bằng không.
 Tăng Kp từ nhỏ đến lớn cho đến khi ngõ ra đáp ứng với sự rung động là dao
động ổn định.
 Giảm Kp xuống một nữa giá trị phản ứng ở trên.
 Tăng Ki từ nhỏ đến lớn, quan sát đáp ứng của hệ thống khi thông số thay
đổi. Tăng cho đến khi dao động ở mức cho phép.
 Cuối cùng là tăng Kd cho đến khi hệ thống đạt tối ưu.
 Tìm hệ số PID bằng phương pháp Ziegler-Nichols
Độ lợi KI và KD lúc đầu được gán bằng 0 và độ lợi P được tăng cho đến khi P
tiến tới độ lợi tới hạn Ku ở đầu ra của vòng điều khiển bắt đầu dao động. Ku và thời
gian dao động Pu được dùng để gán độ lợi như sau:
 35

Bảng 3.2 Phương pháp Ziegler–Nichols

Dạng điều
KP KI KD
khiển
P 0.50Ku
PI 0.45Ku 1.2 KP/Pu
PID 0.60Ku 2 KP/Pu KP.Pu/8

3.6 Giải thuật điều khiển robot cân bằng bám theo vị trí ban đầu

(a) Robot bị trôi qua (b) Robot tại vị trí (c) Robot bị trôi qua
bên trái ban đầu bên phải

Hình 3.6 Robot cân bằng bám theo vị trí ban đầu
Dựa vào giá trị góc nghiêng thu được từ cảm biến qua bộ lọc Kalman giải
thuật PID sẽ giữ cho robot cân bằng nhưng do lực hấp dẫn của trái đất hay lực tác
động từ bên ngoài làm cho robot bị ngã về một hướng nào đó. Để giữ cân bằng thì
robot sẽ di chuyển ra khỏi vị trí ban đầu. Để có thể điều khiển robot cân bằng dao
động quanh vị trí ban đầu thì giá trị sai lệch về vị trí hiện tại so với vị trí ban đầu
được tính toán.
 36

Bắt đầu

Điều khiển robot di Lấy dữ liệu góc nghiêng

chuyển về vị trí ban đầu trên robot từ cảm biến

Sai

Đúng Tính toán sai số giữa góc

Giá trị sai lệch
nghiêng khi robot cân bằng và
vị trí = 0 ?
góc nghiêng hiện tại trên robot

Điều khiển chiều quay và

Tính toán sai lệch vị trí hiện
tốc độ động cơ để robot cân
tại so với vị trí ban đầu
bằng

Hình 3.7 Sơ đồ thuật giải robot bám theo vị trí ban đầu
 37

Chương 4

KẾT QUẢ THỰC NGHIỆM

4.1 Độ chính xác của cảm biến

Để có thể điều khiển robot giữ cân bằng thì thông tin về góc nghiêng của robot
đóng vai trò quan trong nhất. Giá trị góc nghiêng được cung cấp bởi cảm biến IMU
tích hợp gồm có cảm biến gia tốc và cảm biến con quay hồi chuyển. Hai cảm biến
này là dạng cảm biến vi cơ điện tử nên rất nhạy với rung động. Giá trị góc thu trực
tiếp từ cảm biến có dao động rất lớn và phải được xử lý lọc nhiễu trước khi đưa vào
giải thuật cân bằng.
Hình 4.1 trình bày kết quả so sánh giá trị góc nghiêng theo trục y thu trực tiếp
từ cảm biến IMU (đường màu xanh lá), giá trị góc nghiêng theo trục y của bộ lọc
Complementary (đường màu xanh dương) và góc nghiêng theo trục y của bộ lọc
Kalman (đường màu đỏ).

Hình 4.1 Biểu đồ so sánh góc nghiêng theo trục y của bộ lọc Complementary, bộ
lọc Kalman và góc nghiêng theo trục y thu trực tiếp từ cảm biến IMU
Đây là dữ liệu thu thập sau khi được đưa qua cổng UART và được vẽ bằng
phần mềm Labview. Kết quả như Hình 4.1 cho thấy giá trị góc nghiêng theo trục y
 38

của robot từ hai bộ lọc Complementary và Kalman có độ ổn định cao hơn so với giá
trị góc nghiêng theo trục y thu trực tiếp từ cảm biến.

Hình 4.2 Biểu đồ so sánh thời gian đáp ứng giữa bộ lọc Complementary và bộ lọc
Kalman
Trên Hình 4.2 bộ lọc Kalman (đường màu đỏ) có thời gian đáp ứng chậm hơn
so với bộ lọc Complementary (đường màu xanh lam). Tuy nhiên để biết được bộ lọc
Complementary hay bộ lọc Kalman có độ ổn định cao hơn thì độ ổn định ngõ ra của
hai bộ lọc được biễu diễn như Hình 4.3.

Hình 4.3 Biểu đồ so sánh độ ổn định ngõ ra của bộ lọc Kalman với bộ lọc
Complementary
 39

So với bộ lọc Kalman (đường màu đỏ) thì bộ lọc Complementary (đường
màu xanh lam) dễ thực hiện và ít tốn tài nguyên xử lý của bộ vi điều khiển hơn do
đó khả năng đáp ứng về mặt thời gian nhanh hơn, kết quả này được so sánh và mô
tả như trong Hình 4.2. Xét tại vị trí góc theo trục y khi robot đứng cân bằng là
86.6°, so với bộ lọc Complementary thì bộ lọc Kalman có thời gian đáp ứng chậm
hơn nhưng độ ổn định của bộ lọc Kalman lại có độ dao động nhỏ hơn và khả năng
ổn định cao hơn, ít bị trôi so với bộ lọc Complementary khi đươc vẽ trên phần mềm
Labview như Hình 4.3.
4.2 Giải thuật cân bằng PID trên mô hình robot

Hình 4.4 Góc nghiêng theo trục y của robot khi thực hiện giải thuật cân bằng
Hình 4.4 trình bày kết quả thực hiện giải thuật PID cân bằng trên mô hình
robot thực tế. Giá trị góc xét (đường màu đỏ) khi robot đang đứng cân bằng ở 88°
của góc theo trục y, giá trị góc nghiêng trục y (đường màu xanh lam) được thu thập
trong khi robot đang thực hiện cân bằng có độ dao động nhỏ chênh lệch khoảng 3°.
Giải thuật PID được thực hiện với các hệ số , và .
Các thông số này có được là dựa vào phương pháp Ziegler-Nichols và phương pháp
thực nghiệm.
 40

4.3 Kết quả thực hiện giải thuật trên mô hình robot
Kết quả thực nghiệm trên mô hình robot cho thấy robot có khả năng giữ cân
bằng tại vị trí đặt ban đầu. Trong quá trình robot giữ cân bằng nếu có lực tác động
từ bên ngoài theo phương ngang làm robot ngã với góc nghiêng cho phép là 880 ±
200 trong khoảng thời gian lớn hơn 0.95 giây thì robot sẽ tiếp tục giữ cân bằng và di
chuyển quay về lại vị trí đặt ban đầu. Video kết quả thực nghiệm về robot thực cân
bằng khi không có lực tác động từ bên ngoài và có lực tác động từ bên ngoài được
đăng tải tại địa chỉ:
http://www.youtube.com/watch?v=v_81TG475as
http://www.youtube.com/watch?v=_DAv26ln9JI&feature=share
 41

Chương 5

KẾT LUẬN

5.1 Kết luận

Kết quả đạt được của đề tài:
 Xây dựng thành công mô hình robot thực tế hướng đến mục tiêu phục vụ
cho việc giảng dạy tại phòng thí nghiệm về chuyên ngành Điều khiển và Tự động
hóa.
 Thu thập dữ liệu góc nghiêng trực tiếp từ robot truyền về máy tính thông
qua cổng UART sau đó vẽ đồ thị trên phần mềm Labview để đánh giá, so sánh kết
quả thực nghiệm giữ giá trị góc nghiêng thô và giá trị sau khi qua bộ lọc
Complementary, bộ lọc Kalman.
 Xây dựng thành công giải thuật cân bằng bám theo vị trí ban đầu dựa trên
thuật toán PID.

Hình 5.1 Robot đang thực hiện việc cân bằng

5.2 Hướng phát triển
 Tìm hiểu thêm về các thuật toán điều khiển khác như: fuzzy, nơ ron, điều
khiển trượt… để tăng thêm hiệu quả cho thuật toán cân bằng.
 42

 Cần xây dựng thêm giải thuật di chuyển điều khiển thông qua RF,
Bluetooth dựa trên hệ điều hành Android để phát triển thành sản phẩm giải trí hoàn
thiện.
 Đề tài cũng như là tiền đề cho việc xây dựng mô hình xe hai bánh tự cân
bằng di chuyển trên địa hình phẳng.
 43

TÀI LIỆU THAM KHẢO

[1] Alwafi Husein, Attitude and altitude control of two wheel trirotor hybrid robot,
Master’s thesis, 2013.
[2] Ayça GÖÇMEN, Design of two wheeled electric vehicle, Master’s thesis, 2011.
[3] Christian sundin, Filip thorstensson, Autonomous balancing robot, Master’s
thesis, 2012.
[4] Maria R. Bageant, Balancing a two- wheeled Segway robot, Bachelor’s thesis,
2006.
[5] Welch, G. and G. Bishop, An Introduction to the Kalman Filter, 2006.
[6] Đỗ Bình Nguyên, Điều khiển con lắc ngược di động dùng phương pháp điều
khiển phi tuyến, Luận văn thạc sĩ, 2012.