TÌM HIỂU NGUYÊN LÝ HOẠT ĐỘNG ENCODER, CẢM BIẾN GIA TỐC MPU6050 KẾT HỢP BỘ LỌC KALMAN

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

VIỆN ĐIỆN

Đồ án I
ĐỀ TÀI:

TÌM HIỂU NGUYÊN LÝ HOẠT ĐỘNG ENCODER, CẢM BIẾN GIA
TỐC MPU6050 KẾT HỢP BỘ LỌC KALMAN

Giảng viên hướng dẫn
Họ và tên sinh viên
Mã số sinh viên
Mã lớp

:
:
:
:

TS. Vũ Vân Hà
Nguyễn Việt Hoàng
20191856
717569

Hà Nội, 08/2022




LỜI NĨI ĐẦU

Qua q trình thực hiện đồ án I, em cũng đã tìm hiểu và học tập được nhiều điều .
Mặc dù đồ án được thực hiện trong thời gian khá dài, tuy nhiên do còn khá thiếu kinh
nghiệm trong việc tìm kiếm thơng tin nên mong có gì sai sót mong thầy bỏ qua ạ. Em
đã cố gắng thực hiện đồ án môn học, và cũng đã tiếp thu những kiến thức, kỹ năng mới
trong kì học này.
Em xin chân thành cảm ơn.

Hà Nội, ngày 12 tháng 8 năm 2022





MỤC LỤC

DANH MỤC HÌNH VẼ.................................................................................................................i
CHƯƠNG 1. ENCODER – CẢM BIẾN ĐO VỊ TRÍ GĨC, TỐC ĐỘ ĐỘNG CƠ.............1
1.1 Cấu tạo:...........................................................................................................................1
1.2 Phân loại:........................................................................................................................1
1.2.1 Incremental encoder (Tương đối).............................................................................1
1.2.2 Absolute encoder (Tuyệt đối)...................................................................................2
1.3 Thông số encoder cần quan tâm....................................................................................3
1.4 Cách đọc encoder tương đối...........................................................................................3
1.5 Thực nghiệm...................................................................................................................4
1.5.1 Thiết bị phần cứng....................................................................................................4
1.5.2 Sơ đồ đấu dây...........................................................................................................6
1.5.3 Kết quả thực nghiệm................................................................................................7
CHƯƠNG 2. MPU6050 – CẢM BIẾN GIA TỐC VÀ BỘ LỌC KALMAN........................8
2.1 Nguyên lý hoạt động.......................................................................................................8

2.2 Bộ lọc Kalman.................................................................................................................9
2.2.1 Bản chất bộ lọc Kalman...........................................................................................9
2.2.2 Bộ lọc Kalman cho hệ tuyến tính rời rạc................................................................10
2.3 Bài tốn cảm biến MPU6050:......................................................................................11
2.4 Kết quả thực nghiệm:...................................................................................................13
KẾT LUẬN..................................................................................................................................14
TÀI LIỆU THAM KHẢO..........................................................................................................15
PHỤ LỤC.....................................................................................................................................16



DANH MỤC HÌNH VẼ

Hình 1.1 Cấu tạo encoder.............................................................................................. 1
Hình 1.2 Encoder tương đối........................................................................................... 2
Hình 1.3 Encoder tuyệt đối............................................................................................ 2
Hình 1.4 Tín hiệu kênh A và B...................................................................................... 4
Hình 1.5 Encoder 334.................................................................................................... 4
Hình 1.6 Module L298................................................................................................... 5
Hình 1.7 Sơ đồ dây........................................................................................................ 6
Hình 1.8 Vị trí góc và vận tốc........................................................................................ 7
Hình 2.1 MPU6050........................................................................................................ 9
Hình 2.2 Mơ hình Kalman........................................................................................... 10
Hình 2.3 Thuật tốn Kalman........................................................................................ 11
Hình 2.4 Mơ hình bộ lọc Kalman cho hệ tuyến tính.................................................... 11
Hình 2.5 Vị trí khơng đổi............................................................................................. 13
Hình 2.6 Vị trí dao động.............................................................................................. 13

i

CHƯƠNG 1. ENCODER – CẢM BIẾN ĐO VỊ TRÍ GĨC, TỐC
ĐỘ ĐỘNG CƠ

1.1 Cấu tạo:
Encoder cấu tạo chính gồm:


Đĩa quang trịn có rảnh nhỏ quay quanh trục: Trên đĩa được đục lỗ (rãnh), khi
đĩa này quay và chiếu đèn led lên trên mặt đĩa thì sẽ có sự ngắt qng xảy ra.
Các rãnh trên đĩa chia vòng tròn 360o thành các góc bằng nhau. Và một đĩa có
thể có nhiều dãy rãnh tính từ tâm trịn.



Bộ cảm biến thu (photosensor)



Nguồn sáng (Light source).

Hình 1.1 Cấu tạo encoder

1.2 Phân loại:
1.2.1 Incremental encoder (Tương đối)
Tương ứng đĩa 2bit, cho ngõ ra dạng xung vuông pha AB, hoặc ABZ hoặc ABZA|
B|Z| (đọc là A đảo, B đảo, Z đảo). Loại Encoder có 2 tín hiệu xung A và B là phổ biến
nhất. Tín hiệu khe Z là tín hiệu để xác định động cơ quay được một vòng.

1


Hình 1.2 Encoder tương đối

Từ số xung, Encoder có số dây tương ứng: 6 hoặc 4 dây tùy loại. Các dây bao gồm :
2 dây nguồn, 2 dây pha A và B, 1 dây pha Z...
1.2.2 Absolute encoder (Tuyệt đối)
Tương ứng đĩa quay 8bit hay 8 dãy rãnh, cho ngõ ra dạng mã kỹ thuật số(BCD),
Binary (nhị phân), Gray code…

Hình 1.3 Encoder tuyệt đối

Ví dụ: Encoder có ngõ ra dạng bit, độ phân giải 2500. Thì 1 vịng quay của encoder
sẽ cho ra 2500 dãy nhị phân 8bit:
00001011 00100010

10010011 ….

11110001 10010010

01011101 …

Absolute encoder (Tuyệt đối) có thể ghi nhớ vị trí khi bị mất nguồn do mỗi vị trí có
một mã tín hiệu riêng.


1.3 Thơng số encoder cần quan tâm


Đường kính trục, dạng trục: Encoder có dạng trục dương hoặc âm. đường kính
trục từ 5~50mm. Thường đường kính lớn hơn 6mm sẽ là loại trục âm (trục lõm).



Độ phân giải: hay còn gọi là số xung - tương ứng số tín hiệu encoder đếm được
khi quay 1 vịng. Encoder có số xung càng cao thì giá càng cao. Thang máy thường
dùng xung 1024p/r, chế tạo máy vào khoảng 360p/r, 1000p/r. Ngồi ra cịn có các
xung lên đến 6000p/r hoặc 6pr.



Điện áp: Encoder thường xuyên bị cháy do khi lắp đặt không chú ý nguồn cấp.
Nếu encoder có dãy điện áp:5~24V thì rất dễ. Tuy nhiên với một số encoder trục
lớn: 30-40mm, encoder theo máy sẽ thường gặp điện áp xác định: 5V, 12V hoặc
15V.



Ngõ ra: AB, ABZ, AB đảo, ABZ đảo.



Dạng ngõ ra: Có nhiều dạng ngõ ra, liệt kê sơ sơ gồm: Open Collector, Voltage
Output, Complemental, Totem Pole, Line Driver. Dạng ngõ ra quy định nguồn cấp,
đầu đọc thông tin ...



Dây cáp: cáp càng dài càng dễ bị nhiễu. Dây tiêu chuẩn từ 1-3m. Đôi khi lên đến

10m theo nhu cầu sử dụng.



Phụ kiện: đi kèm encoder trục dương là Coubling, encoder trục âm là
Pass. Coubling giúp nối encoder trục dương với motor trục dương, Pass giúp gắn
encoder vào máy. Một encoder trục âm có 1-2 pass tùy loại.

1.4 Cách đọc encoder tương đối
Encoder hoạt động cho ngõ ra dạng tín hiệu xung vng. Khi encoder quay, cơ cấu
cơ khí sẽ làm quay đĩa chia rãnh của nó cắt ngang qua đường tín hiệu quang được phát
bời mắt phát quang (hồng ngoại, laser..). Việc chia cắt tín hiệu này cho ra đường tín
hiệu dạng xung ở các kênh A, B và I. Xung này thường trả về tín hiệu ở mức logic
5V(High) và 0V(Low). Tín hiệu dạng xung này như bất kể các tín hiệu dạng xung điện
khác, nó đều có những đặc tính sau để ta có thể phát hiện và đo đạc nó, bao gồm:
1. Tín hiệu xung ở mức thấp 0V
2. Tín hiệu xung ở mức cao 5V


3. Tín hiệu sườn lên của xung
4. Tín hiệu sườn xuống của xung

Hình 1.4 Tín hiệu kênh A và B

Để xác định độ thay đổi encoder có 2 cách:
 Sử dụng 2 ngắt ngoài: Theo dõi sự thay đổi đồng thời cả 2 kênh A và B.
 Sử dụng 1 ngắt ngồi: Chọn 1 kênh làm tín hiệu ngắt, sau đó đọc tín hiệu
kênh cịn lại.

1.5 Thực nghiệm

1.5.1 Thiết bị phần cứng
Encoder 334 tích hợp với động cơ:
+ Điện áp hoạt động: DC 3V – 24V.
+ Tốc độ:
+ DC 12V 82000 rpm.
+ DC 24V 16800 rpm.
+ Hỗ trợ 2 kênh A, B.
+ Số xung: 334 Xung.

Hình 1.5 Encoder 334


Mạch cầu H L298:
+ Tích hợp hai mạch cầu H.
+ Điện áp điều khiển: +5 VDC ~ +35 VDC.
+ Dòng tối đa cho mỗi cầu H là: 2A.
+ Điện áp của tín hiệu điều khiển: +5 VDC ~ +7 VDC.
+ Dịng của tín hiệu điều khiển: 0 ~ 36mA.
+ 12V Power, Power GND: Nối với nguồn 12V cấp cho động cơ.
+ A Enable, B Enable: Nhận 2 tín hiệu enable cho 2 cầu.
+ IN1, IN2: Nhận tín hiệu điều khiển PWM đối xứng cho cầu thứ nhất.
+ IN3, IN4: Nhận tín hiệu điều khiển PWM đối xứng cho cầu thứ 2.
+ fPWM max: 5 MHz.
+ OutputA (Out1, Out2), OutputB (Out3, Out4): Nối với động cơ cần điều khiển.

Hình 1.6 Module L298


1.5.2 Sơ đồ đấu dây

Hình 1.7 Sơ đồ dây

Nhận thấy rằng sơ đồ mạch cầu H rất thích hợp để điểu khiển hai động cơ cùng lúc,
ta có thể sử dụng xung vuông để điều chỉnh tốc độ động cơ, và cịn có thể đảo chiều
động cơ bằng cách thay đổi các giá trị logic của vi điều khiển. Động cơ sử dụng điện
áp 12 VDC làm nguồn cấp.


1.5.3 Kết quả thực nghiệm

Hình 1.8 Vị trí góc và vận tốc

Hình 1.8 là kết quả sau khi chạy thử động cơ tích hợp encoder 334 sử dụng vi điều
khiển arduino.
Vận tốc tên tính theo vị trí của bánh xe theo cơng thức:
V = Số ví trí thay đổi trong 1 giây * 60 / số xung 1 vịng đĩa
Cơng thức tính vận tốc tức thời thích hợp trong việc đo sự thay đổi vị trí bánh xe
nhanh, quãng đường di chuyển ngắn.


CHƯƠNG 2. MPU6050 – CẢM BIẾN GIA TỐC VÀ BỘ LỌC
KALMAN

2.1 Nguyên lý hoạt động
MPU6050 GY521 6DOF IMU (Accelerometer & Gyro):
+ 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 tốn cần
thiết. Điều này giúp giảm bớt đáng kể phần xử lý tính tố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 Digital Converter - ADC) 16-bit cho ra kết quả chi tiết về góc quay, tọa độ... Với
16-bit 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 tốn xong.


Hình 2.1 MPU6050

2.2 Bộ lọc Kalman
Bộ lọc là một quá trình xử lý nhằm loại bỏ những gì khơng có giá trị hoặc không
quan tâm đến và giữ lại những gì có giá trị sử dụng. Trong xử lý tín hiệu, bộ lọc được
thiết kế để lọc tín hiệu sạch (cần tìm) từ trong tín hiệu trộn lẫn giữa tín hiệu sạch và
nhiều tín hiệu bẩn ( khơng cần thiết ).
Vậy bộ lọc Kalman, một cách khái quát là một tập hợp các phương trình tốn học
mơ tả một phương pháp tính tốn truy hồi hiệu quả cho phép ước đốn trạng thái một
q trình sao cho trung bình phương sai của độ lệch ( giữa giá trị thực và giá trị ước
đoán) là nhỏ nhất. Bộ lọc Kalman rất hiệu quả trong việc ước đoán các trạng thái trong
quá khứ, hiện tại và tương lai thậm chí ngay cả khi chính xác của hệ thống mơ phỏng
khơng được khẳng định.
2.2.1 Bản chất bộ lọc Kalman

Để mơ hình hóa hoạt động của mạch lọc Kalman, chúng ta có tín hiệu đo được, có
mơ hình của tín hiệu đo được ( địi hỏi tuyến tính) và sau đó là áp dụng vào trong hệ
thống phương trình của mạch lọc để ước lượng trạng thái quan tâm. Để có thể ứng
dụng một cách hiệu quả mạch lọc Kalman thì chúng ta phải mơ hình hóa được một
cách tuyến tính sự thay đổi của trạng thái cần ước lượng (estimate) hoặc ước
đoán(predict).


Hình 2.2 Mơ hình Kalman

2.2.2 Bộ lọc Kalman cho hệ tuyến tính rời rạc
Mơ hình đối tượng tuyến tính rời rạc được biểu diễn thơng qua hệ phương trình
trạng thái sau:
xk = Axk-1 + Buk-1 + wk-1
zk = Hxk + vk
Trong đó :


x : biến trạng thái



u : biến đầu vào



z : trạng thái đầu vào ( đo lường được )



w, v : nhiễu quá trình và nhiễu đo lường tương ứng.

Giả sử các nhiễu quá trình w(t) và nhiễu đo lường v(t) là nhiễu trắng , có phương
sai tương ứng là Q và R.
p(w) ~ N (0, Q)
p(v) ~ N (0, R)
Với hệ thống như vậy bộ lọc Kalman tuyến tính cho phép ta ước lượng một cách
tốt nhất biến trạng thái xk, kí hiệu là �̂ k , sao cho hiệp phương sai của chúng Pk đạt giá
trị nhỏ nhất.
Pk = E[ekekT]
Ek = xk - �̂ k


Thuật toán Kalman cho hệ rời rạc như sau:
Như vậy ta có thể thấy thuật tốn gồm hai bước : bước ước lượng dự đoán ( time
update) và bước làm chính xác đự dốn này ( mcasurement update ) dựa trên thơng tin
đầu vào và đo được zk.

Hình 2.3 Thuật toán Kalman

Chú ý rằng �̂ k- là giá trị dự đoán được cập nhật từ giá trị ước lượng �̂ k-1 Thuật
tốn này đc trình bày trên sơ đồ khối như sau:

Hình 2.4 Mơ hình bộ lọc Kalman cho hệ tuyến tính

2.3 Bài tốn cảm biến MPU6050:
Có một thơng số được đưa về từ cảm biến, tuy nhiên thông số chịu tác động bởi
nhiễu, ta sẽ thiết kế bộ lọc Kalman để ước lượng được giá trị tốt nhất giá trị cần đo này.

Mơ hình bài tốn :

xk = Axk-1 + Buk-1 + wk-1
= xk-1 + wk
Với giá trị thu đươc z ∈ R


zk = Hxk + vk
= xk + vk
Với :


Giá trị cần đo là x(t)



Tín hiệu thu được từ cảm biến là z(t)



Nhiễu của quá trình là w(t)



Nhiễu đo lường là v(t)

Các phương trình cho bộ lọc Kalman :
Phương trình quá trình “Time update”:
�̂ k- = �̂ k-1
Pk- = Pk-1 + Q
Phương trình quá trình “easurement update”:
Kk= �−−�

�
�
+�

�̂ k = �̂ k- + Kk(zk - �̂ k) Pk = (1- Kk)
�
�−
�
Một số giả thiết:
Tín hiệu cần đo là x(t) ~
(0,50) w(t) ~ (0,1e-3)Q = 1e-3
nhiễu đo lường trong bài tốn mơ phỏng này khi lấy giá trị từ cảm biến ta lấy
khoảng 10% giá trị thật
Ta có Kk tỉ lệ nghịch với R, R lớn thì tốc độ ước lượng chậm, giá trị ước lượng có
vẻ ít tin tưởng hơn giá trị đo được và ngược lại. Do đó việc chọn R khá quan trọng ảnh
hưởng đến tốc độ ước lượng của hệ thống.
Chọn thông số ước lượng ban đầu : x(0) = 0, P 0- = 5. Giá trị x(0) khơng quan trọng
vì nó sẽ thích nghi tưng bước để đạt đến giá trị cần xác định còn P0- khác 0 là được.


2.4 Kết quả thực nghiệm:
Ứng dụng của bộ lọc Kalman trong việc lấy góc từ cảm biến MPU6050:

Hình 2.5 Vị trí khơng đổi

Hình 2.6 Vị trí dao động

Kết quả trên là thơng số lấy từ Gyroscopes X (vận tốc góc), đồ thị lọc tương đối tốt
khi ở cảm biến ở vị trí cố định hình 2.5 và khi thay đổi vị trị hình 2.6.

KẾT LUẬN

Trong quá trình thực hiện đồ án I, em cũng đã thấy được nhiều kiến thức mới mẻ,
chuyên sâu hơn so với việc chỉ học mỗi lý thuyết trên lớp. Những công việc này giúp
em ngày càng tiếp thu và phát triển hơn. Việc tìm hiểu lý thuyết xong từng bước mô
phỏng, thực hành, và cả việc lựa chọn thiết bị cần thiết để kiểm tra kết quả thực
nghiệm của mơ hình, tuy phần nào vẫn cịn sai sót, nhưng vẫn có thể phát triển và hồn
thiện thêm. Cuối cùng em cũng xin cảm ơn thầy vì đã hướng dẫn em trong môn đồ án
này.
Em xin chân thành cảm ơn!