MỤC LỤC

1


DANH MỤC HÌNH VẼ

Lời nói đầu
Xe hai bánh tự cân bằng là đối tượng phi tuyến, không ổn định và xen kênh rõ rệt.
Do vậy, bài toán điều khiển xe hai bánh tự cân bằng là bài toán khá phức tạp. Vì vậy,
áp dụng bộ điều khiển Arduino uno thích nghi trực tiếp trong bài toán điều khiển xe
hai bánh sẽ hứa hẹn là một giải pháp hiệu quả góp phần nâng cao hiệu quả làm việc
của xe hai bánh.Mô hình là một chiếc xe hai bánh được đặt dọc trục với nhau (khác
với xe đạp là trục của hai bánh xe song song) trên mô hình sử dụng các cảm biến để đo
góc nghiêng của thân xe,vận tốc quay của sàn xe so với trục bánh và vận tốc di chuyển
của xe so với mặt đất,nhờ các cảm biến này xe có thể tự giữ thăng bằng và di
chuyển.Trong hệ thống các cảm biến để loại trừ các tín hiệu nhiễu từ hệ thống và nhiễu
từ tín hiệu đo,sai số của ngõ ra đồng thời có thể ước lượng chính xác giá trị đo trong
tương lai của cảm biến cũng như kết hợp các tín hiệu,từ các tín hiệu đo thông qua một
số đại lượng đặc trưng của mô hình( khối lượng,chiều dài,chiều cao vật,đường kính
bánh…) ta sẽ tính được momen quán tính nghiêng(lật của mô hình) ,từ đó đưa ra các
2


giá trị điều khiển phù hợp cho các bánh xe để giữ cho mô hình luôn đứng vững hoặc di
chuyển với vận tốc ổn định.
Toàn bộ mô hình được điều khiển bằng một modun LM298,đây là thế hệ tương đối
cao cấp của họ Arduino uno có thế xử lý và thực thi chương trình ở tốc độ cao trong
việc tính toán các giá trị cảm biến và đưa ra các giá trị truyền động.Bộ vi điều khiển
đóng vai trò thứ nhất trong đề tài như một bộ lọc kalman với tín hiệu vào từ thiết bị
inclinometer và gyro,với các dữ liệu về góc đã xử lý và tín hiệu hồi tiếp về vị trí đo

encoder đưa về,tính toán và đưa ra tín hiệu điều khiển bộ truyền động đến bánh xe để
giữ thăng bằng/di chuyển,đi thẳng,quay,quẹo. Đây là một phương tiện vận chuyển mới
tại các thành phố trong tương lai với nhiều với nhiều ưu điểm :gọn,nhẹ,ít chiếm diện
tích đường phố,dễ mang vác,tháo lắp và vận chuyển,nhiên liệu sạch,dễ điều khiển

3


CHƯƠNG 1: TỔNG QUAN VỀ ROBOT HAI BÁNH TỰ CÂN BẰNG
1.1: Giới thiệu robot hai bánh tự cân bằng
Nghiên cứu về robot tự động (autonomous robot) là một lĩnh vực nghiên cứu đang được
phát triển khá mạnh trong những năm gần đây.Một trong những khó khăn nhất của vấn
đề nghiên cứu robot tự động là khả năng duy trì ổn định trong những địa hình khác
nhau.Để giải quyết vấn đề này các robot hầu hết có bánh xe rộng hoặc tối thiểu ba điểm
tiếp xúc với mặt đất để duy trì sự cân bằng,tuy nhiên tăng kích thước hoặc số lượng bánh
xe sẽ làm giảm hiệu quả của hệ thống điều khiển do tăng trọng lượng xe,tăng ma sát,tăng
lực kéo và tăng hao tổn năng lượng.Robot hai bánh tự cân bằng là một hướng nghiên cứu
sẽ giải quyết được nhược điểm bởi robot 2 bánh tự cân bằng chỉ sử dụng hai bánh xe nên
giảm được cả trọng lượng và không gian.Tuy nhiên vấn đề khó khăn cho robot là làm
cách nào để robot có thể tự cân bằng trong những điều kiện làm việc khác nhau đồng
thời tải trọng mang theo có thể thay đổi.
Chính vì sự hấp dẫn của robot hai bánh tự cân bằng đến từ cả vấn đề lý thuyết và thực tế
nên nghiên cứu robot hai bánh tự cân bằng đã thu hút được nhiều sự quan tâm của nhiều
nhà khoa học.
Robot hai bánh tự cân bằng được chia làm hai loại:
-Loại có hai bánh song song
-Loại có hai bánh trước và sau
1.2: Tổng quan về robot hai bánh tự cân bằng

Hình 1. 1: Mô tả cách di chuyển của xe hai bánh tự cân bằng

4


*Thế nào là xe hai bánh tự cân bằng:
Đối với loại xe hay bốn bánh việc thăng bằng và ổn đinh 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 hai bánh có
cấu trúc như xe đạp việc thăng bằng trong khi không di chuyển là hoàn toà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.Đố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ữ ngay giữa các bánh
xe.Robot hai bánh tự cân bằng dựa trên mô hình con lắn ngược là một đối tượng phi
tuyến với các tham số bất định khó điều khiển với 6 biến trạng thái,đặc điểm nổi bật của
robot hai bánh tự cân bằng là cơ chế tự cân bằng giúp cho xe dù chỉ có một trục chuyển
động với hai bánh nhưng luôn ở trạng thái cân bằng.

Hình 1. 2: Mô hình robot 2 bánh cân bằng

5


1.3: Tại sao phải thiết kế xe hai bánh tự cân bằng
Những mobile robot xây dựng hầu hết các robot thường là các robot di chuyển bằng ba
bánh xe,với hai bánh được lắp ráp đồng trục và một bánh đuôi nhỏ,có nhiều kiểu khác
nhau nhưng đây là kiểu thông dụng nhất,còn đối với xe 4 bánh thường một đầu xe có hai
bánh truyền động và đầu xe còn lai được gắn với một hoặc hai bánh lái.Việc thiết kế xe
ba hay bốn bánh làm cho xe thăng bằng ổn định nhờ trọng lượng của nó được chia cho
hai bánh lái chính và bánh đuôi hay bất kì

Hình 1. 3 Xe di chuyển trên địa hình bằng phẳng


Hình 1. 4Xe di chuyển khi lên dốc

Hình 1. 5: Trạng thái xe hai bánh khi di chuyển trên địa hình bằng phẳng,dốc

cái gì khác để đỡ trọng lượng của xe,nếu trọng lượng được đặt nhiều vào bánh lái thì xe
sẽ không ổn định dễ bị ngã còn nếu đặt nhiều vào bánh đuôi thì hai bánh chính sẽ mất
6


khả năng bám.Nhiều thiết kế có thể di chuyển tốt trên địa hình bằng phẳng nhưng không
thể di chuyển lên xuống trên địa hình lồi lõm.khi di chuyển lên đồi trọng lượng xe dồn
vào đuôi xe làm bánh lái mất khả năng bám và trượt ngã đối với những bậc thang thậm
chí nó còn dừng hoạt động và quay tròn bánh xe.Khi di chuyển xuống đồi sự việc còn tệ
hơn,trọng tâm thay đổi về phía trước và thậm chí còn làm xe bị lật khi di chuyển trên bậc
thang,hầu hết nhưng xe này có khả năng leo lên được dốc ít hơn là khi chúng di chuyển
xuống.Ngược lại các xe hai bánh đồng trục lại thăng bằng rất linh động khi di chuyển
trên địa hình phức tạp,khi nó leo sườn dốc nó tự động nghiêng ra trước và giữ cho trọng
lượng dồn về hai bánh lái chính,tương tự vậy khi xuống dốc nó nghiêng ra sau và giữ
trọng tâm rơi vào các bánh lái,chính vì vậy không bao giờ có hiện tượng trọng tâm của
xe rơi ra ngoài vùng đỡ của các bánh xe để có thể gây ra sự lật úp

Hình 1. 6: Trạng thái xe hai bánh đồng trục khi di chuyển trên địa hình bằng phẳng ,dốc
1.4: Ưu ,nhược điểm của xe hai bánh tự cân bằng
* Ưu điểm:
-Không ô nhiễm, sử bình điện và có thể sạc điện
-Sử dụng không gian hiệu quả,đa năng
-Khá dễ lái vòng quanh trong văn phòng,chạy ngang qua cửa ra vào do tốc độ thấp ngoài
ra còn có thể xuống các bậc thềm
- Chiếm ít diện tích,thuận lợi trong di chuyển ở mọi địa hình phức tạp
* Nhược điểm:

-Khi di chuyển thì vẫn có thể gây ra hiện tượng dung,lắc
-Không thể leo bậc thang cao quá ½ bán kính bánh xe
7


-Tốc độ không đủ nhanh để di chuyển trên đường trường
1.5: Phạm vi ứng dụng
- Xây dựng được một phương tiện vận chuyển mới trong khu vực chật hẹp có thể đi
chuyển ngay trong các chung cư tòa nhà cao tầng,dùng trợ giúp di chuyển cho người già
và trẻ em vận chuyển.
-Làm phương tiện vận chuyển hàng hóa đến những nơi đã lập trình sẵn ở các tòa
nhà,phòng làm việc,những không gian chật hẹp,khó xoay trở.
-Thậm chí kết hợp trên các humanoid robot, nếu được kết hợp với các robot
camera,robot dò đường,robot lái mặt đường thì hiệu quả các công dụng cụ thể cực kỳ
linh hoạt.
1.6: Nhu cầu thực tế
-Hiện tại trong điều kiện đường xa giao thong ngày càng chật hẹp,không khi ngày càng ô
nhiễm việc chế tạo xe hai bánh tự cân bằng là một nhu cầu thực sự ,bên cạnh đó thiết kế
một flatform cho mobile robot cũng là một đề tài cần thiết trong lĩnh vực tự đông hóa
ngày nay,nhằm trợ giúp cho trẻ em,người già và vận chuyển hàng hóa,giám sát…trong
cuộc sống hàng ngày vốn có nhiều nhu cầu đi lại và vận chuyển tại các thành phố lớn.
-Về khía cạnh khoa học và công nghệ mô hình xe hai bánh tự cân bằng thực sự là một
bước đệm quan trọng để có kinh nghiệm trong việc tính toán,mô hình và chế tạo các
robot hai chân,ngoài ra mô hình cũng là sự bổ sung cần thiết về các giải pháp công nghệ
di chuyển của các mobile robot 3 bánh,4 bánh cũng như các mobile robot có chân làm
phong phú những lực chọn giải pháp để chuyển động trong không gian cho các robot

8



CHƯƠNG 2: THIẾT KẾ PHẦN CỨNG VÀ PHẦN MỀM
2.1. Tính chọn các chi tiết tiêu chuẩn
2.1.1. Tính chọn động cơ
Tính toán công suất động cơ
Do quán tính chọn sơ bộ nên ta coi đối tượng tương đương với 1 khối lượng
( m=m1+m2)
Khối lượng của xe : m1 = 0.5kg
Khối tượng tải trọng xe : m2 = 0.2kg
Hệ số ma sát ổ bi : k1 = 0.005
Hệ số ma sát giữa xe với mặt đường : k2 =0.014
Ø 1= 32.5mm ( bánh xe )
Ø 2=2mm ( ổ bi )
 v=1.6m/s,t=3.5 (s)

SF=1.5 , EF=0.75

Hình 2. 1 Hình ảnh minh họa vật lăn

-Mômen cản do ma sát của bánh xe với nền
= 2k2g =0,0140,79,81. = 1,56N.m
-Mômen cản do ma sát ở ổ bi :
= u.k1g=0,0050,79,81=3.43 N.m
-Mômen quán tính tải :
9


Jt=(m1+m2)
=0,7= 9,242 kg.
-Mômen cần để tăng tốc :
=(+)×

Mà == 98,46 N.m
 = (2) = 5,19 N.m
 =++=(1,56+3,43+5,19) =10,18 N.m

Công suất lý tưởng :
=M=10,18=1,002 (w)
Công suất thực tế :
=

SF = 2,004 (W)

Công suất của 1 động cơ là :
P==1,002 (W)
Do yêu cầu thiết kế và mô hình hóa ta nên chọn động cơ DC giảm tốc V1
+ Là loại được lựa chọn và sử dụng nhiều nhất hiện nay cho các thiết kế robot đơn

giản.Động cơ DC giảm tốc V1 có chất lượng và giá thành vừa phải cùng với khả năng dễ
lắp ráp của nó dẫn đến chi phí tiết kiệm và sự tiện dụng cho người dùng
-Điện áp hoạt động từ 3-9 VDC
-Dòng điện tiêu thụ 110-140 Ma
-Tỉ số truyền 1:120
-Số vòng/1 phút
+50 vòng/1 phút tại 3VDC
+83 vòng/1 phút tại 5VDC

10


Hình 2. 2: Hình ảnh minh họa động cơ giảm tốc V1


2.1.2. Tính chọn công suất động cơ
Bánh xe robot V2
-

Chất liệu nhựa
Đường kính bánh xe 65mm
Độ dày lốp xe 6.5 mm
Chiều rộng lốp xe 27 mm
Trục xe đường kính 5mm , độ rộng khuyết 3.66 mm

Hình 2. 3: Hình ảnh bánh xe V2

11


2.2 Thiết kế mạch điện tử
2.2.1 Xây dựng sơ đồ khối

Hình 2. 4: Sơ đồ khối của hệ thống
2.2.2 Sơ đồ đi chân giữa arduino với MPU6050 GY-521

Hình 2. 5: Sơ đồ đi chân arduino với MPU6050 GY-521
12


2.2.3 Sơ đồ đi chân giữa arduino với L298 , động cơ

Hình 2. 6 Sơ đồ đi chân giữa arduino với L298 động cơ

2.3 Thi công phần cứng

13


2.3.1 Các linh kiện sử dụng trong mạch
a. Arduino uno

Hình 2. 7: Hình ảnh aduino uno
Arduino Uno đƣợc xây dựng với phân nhân là vi điều khiển ATmega328P sử dụng
thạch anh có chu kì dao động là 16 MHz. Với vi điều khiển này, ta có tổng cộng 14 pin
(ngõ) ra / vào đƣợc đánh số từ 0 tới 13 (trong đó có 6 pin PWM, đƣợc đánh dấu ~
trƣớc mã số của pin). Song song đó, ta có thêm 6 pin nhận tín hiệu analog đƣợc đánh
kí hiệu từ A0 - A5, 6 pin này cũng có thể sử dụng đƣợc nhƣ các pin ra / vào bình
thƣờng (nhƣ pin 0 - 13). Ở các pin đƣợc đề cập, pin 13 là pin đặc biệt vì nối trực tiếp
với LED trạng thái trên board.
Trên board còn có 1 nút reset, 1 ngõ kết nối với máy tính qua cổng USB và 1 ngõ cấp
nguồn sử dụng jack 2.1mm lấy năng lƣợng trực tiếp từ AC-DC adapter hay thông qua
ắc-quy nguồn.
Khi làm việc với Arduino board, một số thuật ngữ sau cần đƣợc lƣu ý:
•

•

•

Flash Memory: bộ nhớ có thể ghi đƣợc, dữ liệu không bị mất ngay cả khi tắt
điện. Về vai trò, ta có thể hình dung bộ nhớ này nhƣ ổ cứng để chứa dữ liệu
trên board. Chƣơng trình đƣợc viết cho Arduino sẽ đƣợc lƣu ở đây. Kích
thƣớc của vùng nhớ này thông thường dựa vào vi điều khiển đƣợc sử dụng, ví
dụ nhƣ ATmega8 có 8KB flash memory. Loại bộ nhớ này có thể chịu đƣợc
khoảng 10,000 lần ghi / xoá.

RAM: tƣơng tự nhƣ RAM của máy tính, sẽ bị mất dữ liệu khi ngắt điện nhƣng
bù lại tốc độ đọc ghi xoá rất nhanh. Kích thƣớc nhỏ hơn Flash Memory nhiều
lần.
EEPROM: một dạng bộ nhớ tƣơng tự nhƣ Flash Memory nhƣng có chu kì ghi
/ xoá cao hơn - khoảng 100,000 lần và có kích thuớc rất nhỏ. Để đọc / ghi dữ
liệu ta có thể dùng thƣ viện EEPROM của Arduino.

Thông số của arduino
Vi điều khiển
Điện áp hoạt động
Tần số hoạt động
Dòng tiêu thụ

Atmega328 họ 8 bit
5V DC( chỉ đƣợc cấp qua cổng USB)
16Hz
Khoảng 30mA

14


Điện áp khuyên dùng
Điện áp vào giới hạn
Số chân Digital I/O
Số chân Analog
Dòng tối đa trên mỗi chân I/O
Dòng tối đa ( 5V)
Dòng tối đa ( 3.3V)
Bộ nhớ flash


7-12 VDC
6-20 VDC
14( 6 chân hardware PWm)
6 chân( độ phân giải 10 bit)
30 mA
500 mA
50 mA
32Kb (Atmega328) với 0.5Kb dùng
với bootloader

SRAM
EEPRO

2Kb (Atmega328)
1Kb (Atmega328)

b. Môđun LM298

Hình 2. 8: Mô đun LM298

Điện áp điều khiển: +5 V ~ +12 V.
L298 gồm các chân:
•

12V power, 5V power: bên trong mạch có sử dụng IC điều hướng hƣớng dòng
điện, hoạt động với điện áp 5V, khi nối nguồn 12V mạch sẽ hoạt động với
nguồn 12V, tuy nhiên mạch sẽ điều hƣớng dòng điện.
o Dòng 12V đẩy vào động cơ để hoạt động.
o Hạ dòng điện xuống thành 5V và cấp nguồn cho IC.
15



Nhờ vậy khi cấp nguồn 9V: động cơ hoạt động với dòng 9V và IC của mạch sẽ
hoạt động với dòng điện 5V. Việc thiết kế có nhằm điều hướng thành 2 dòng
điện khác nhau giúp IC hoạt động ổn định và tách nguồn riêng biệt với động cơ.
• Power GND chân này là GND của nguồn cấp cho Động cơ.
• 2 Jump A enable và B enable, nếu bạn dùng Board để điều khiển động cơ bước
thì giữa nguyên. Nếu điều khiển động cơ bình thƣờng thì nối với chân PWM
để điều khiển tốc độ.
Gồm có 4 chân Input. IN1, IN2, IN3, IN4. Chức năng các chân này tôi sẽ giải thích ở
bước sau Output A: nối với động cơ giảm tốc V1. bạn chú ý chân +, -. Nếu bạn nối
ngược thì động cơ sẽ chạy ngược
•

c. Cảm biến góc nghiêng MPU6050 GY-521.

Hình 2. 9: MPU6050 GY-521
MPU-6050 tích hợp 6 trục cảm biến bao gồm:
+ con quay hồi chuyển 3 trục (3-axis MEMS gyroscope)
+ cảm biến gia tốc 3 chiều (3-axis MEMS accelerometer)
Ngoài ra, MPU-6050 còn có 1 đơn vị tăng tốc phần cứng chuyên xử lý tín hiệu (Digital
Motion Processor - DSP) do cảm biến thu thập và thực hiện các tính toán cần thiết.
Điều này giúp giảm bớt đáng kể phần xử lý tính toán của vi điều khiển, cải thiện tốc
độ xử lý và cho ra phản hồi nhanh hơn. Đây chính là 1 điểm khác biệt đáng kể của
MPU-6050 so với các cảm biến gia tốc và gyro khác.
MPU-6050 có thể kết hợp với cảm biến từ trƣờng (bên ngoài) để tạo thành bộ cảm
biến 9 góc đầy đủ thông qua giao tiếp I2C.
Các cảm biến bên trong MPU-6050 sử dụng bộ chuyển đổi tƣơng tự - số (Anolog to
16



Digital Converter - ADC) 16-bit cho ra kết quả chi tiết về góc quay, tọa độ... Với 16bit bạn sẽ có 2^16 = 65536 giá trị cho 1 cảm biến.
Tùy thuộc vào yêu cầu của bạn, cảm biến MPU-6050 có thể hoạt động ở chế độ tốc độ
xử lý cao hoặc chế độ đo góc quay chính xác (chậm hơn). MPU-6050 có khả năng đo
ở phạm vi:
+ con quay hồi chuyển: ± 250 500 1000 2000 dps
+ gia tốc: ± 2 ± 4 ± 8 ± 16g
Hơn nữa, MPU-6050 có sẵn bộ đệm dữ liệu 1024 byte cho phép vi điều khiển phát
lệnh cho cảm biến, và nhận về dữ liệu sau khi MPU-6050 tính toán xong.
Các thông số kĩ thuật khác của module MPU-6050
+ Nguồn: 3-5V, trên module MPU-6050 đã có sẵn LDO chuyển nguồn 5V -> 3V
+ Giao tiếp I2C ở mức 3V
+ Khoảng cách chân cắm: 2.54mm
2.3.2. Tổng hợp các linh kiện sử dụng trong hệ thống

STT

Tên vật tư

Hình ảnh

Số
lượng

Chế tạo/
mua

1

Arduino uno


1

Mua

2

Môđun

1

Mua

Thông số
Arduino
uno R3

LM298

3

4

Cảm biến góc
nghiêng

1

Tấm mica


3

Mua
MPU605
0 GY521
Chế tạo

3 tấm

17


5

Bánh xe

2

Mua
R=31.74

6

Động cơ

2

Mua

7


Ốc vít

12

Mua

M3x20

8

Pin

2

Mua

Lithium
18650

DC giảm
tốc

18


2.3.3 Quy
trình thi
công phần
cứng

Bước 1 :
Lắp bánh
với trục
động cơ

19


Hình 2. 10: Hình ảnh minh họa bước 1 và bước 2
Bước 2 : gắn pin lên tấm đế số 1
Bước 3 : gắn mô đun LM298 và cảm biến,và arduino lên tấm đế số 2

Hình 2. 11: Hình ảnh minh họa bước 2 và bước 3

Bước 5 : Dùng ốc vít để liên kết các tấm đế lại với nhau.
Sản phẩm sau khi thi công.

20


Hình 2. 12 Sản phẩm sau khi hoàn thiện
2.4. Xây dựng mô hình động lực học cho hệ thống
2.4.1 Tính toán động lực học
Có nhiều phương pháp dùng để tính động lực học,chẳng hạn phương pháp Newton ,
phương pháp Langrange, phương pháp theo năng lượng …Nhưng trong đề tài này
phương pháp Newton đƣợc sử dụng với các yêu điểm của nó,nó sử dụng các phương
pháp Newton đƣợc sử dụng với các yêu điểm của nó,nó sử dụng các ngược được phổ
biến hiện nay ở các tài liệu tham khảo được sử dụng để kiểm tra sự sai sót trong quá
trình tính toán động lực học của mô hình xe hai bánh tự cân bằng.Bên cạnh các quan
điểm này nó vẫn có nhược điểm là tuyến tính hóa tính toán tại vị trí góc =0

Ta xem mô hình toán học của con lắc ngược với các ống thông số
M : khối lượng xe (kg)
m : khối lượng conm lắc (kg)
21


b : ma sát của xe (N)
L : chiều dài ½ con lắc (m)
F : lực tác động vào bánh xe (N)

+ Một con lắc gắn bởi một khớp bản lề với xe

Hình 2. 13: Phân tích lực trên xe và trên con lắc đơn
-Mô hình nghiên cứu gồm hai phần
+Tác động lực F vào xe
+ Một con lắc gắn bởi một khớp bản lề với xe
-Mô hình bao gồm hai ngõ vào:
+ Dịch chuyển của xe (x)
+Góc lệch của con lắc (θ)
Xung lực

Hệ thống con lắc
X

Hình 2. 14: Mô hình và sơ đồ khối tín hiệu ngõ vào và ra của con lắc ngược
-Phân tích lực cho xe, ta có:
Tổng lực tác dụng lên xe theo phương ngang
M ̈ +b. ̇+N=F
(2.1)
Tổng tác dụng con lắc theo phương ngang

N=m.+m.L..cos(-m.L..sin(
(2.2)
Từ( 2-1),suy ra:
(M+m). ̈ +b. ̇ +m.L. ̈ .cos(θ)-m.L. ̇ .sin( )=F
22


(2.3)
Tổng lực tác dụng ở phương vuông góc với con lắc
P.sin( )+N.cos( )+m.g.sin( )=m.L. ̈+m. ̈ .cos( )

(2.4)

Tổng momen tại khối tâm con lắc ta có:
-P.L.sin( )-N.L.cos( )=I. ̈

(2.5)

Kết hợp hai phương trình trên
(I+m. ).” -m.g.L.sin( )=-m.L. ̈ .cos( )

(2.6)

Từ (2.3) và (2.6),ta có:
(M+m). ̈ +b. ̇ +m.L. ̈ =F
(I+m.

(2.7)

). ̈ -m.g.L. =-m.L.


(2.8)

Hàm trạng thái:

= +F

=+F

(2.9)

Nếu xem b<<1,I<<1,ta được hàm trạng thái đơn giản hơn:
=+F

(2.10)

23


2.4.2 Thiết kế khung bản vẽ:

24


2.5. Thi công phần mềm
2.5.1. Bộ lọc Kalman
Bộ lọc Kalman là một thuật toán sử dụng chuỗi các phép đo theo thời gian, trong
trường hợp này các phép đo chính là giá trị thu được từ cảm biến gia tốc và cảm biến
con quay hồi chuyển. Những giá trị đo này chứa nhiễu gây ra sai số cho việc đo đạc.
Bộ lọc Kalman sẽ ước lượng trạng thái của hệ thống dựa trên trạng thái hiện tại và

trạng thái trước đó của hệ thống nhằm mục đích tăng độ chính xác hơn so với việc
đo đạc độc lập
2.5.2. Cơ sở lý thuyết bộ điều khiển PID
Bộ điều khiển hồi tiếp vòng kín được sử dụng nhiều nhất trong công nghiệp.
Là sự kết hợp của 3 bộ điều khiển : tỉ lệ, tích phân và vi phân.
Có khả năng làm triệt tiêu sai số xác lập, tăng tốc độ đáp ứng, giảm độ vọt lố nếu
thông số của bộ điều khiển được lựa chọn thích hợp.

Hình 2. 15: Sơ đồ khối của bộ điều khiển PID
Thuật toán PID có thể được diễn tả theo nhiều cách khác nhau nhưng thông thường
được mô tả theo biểu thức như sau:

25