CHƯƠNG 4: MÔ HÌNH HOÁ VÀ THIẾT KẾ BỘ ĐIỀU KHIỂN

Động cơ là cơ cấu chấp hành của robot, khả năng làm việc của động cơ quyết định
đến độ chính xác của robot trong quá trình hoạt động. Vì vây, bài toán điều khiển động
cơ là bài toán bắt buộc đối với robot hút bụi. Ngoài ra, cần phải có một bộ điều khiển để
thực hiện nhiệm vụ nhận tín hiệu từ các cảm biến, xử lý thông tin và đưa ra lệnh điều
khiển cho động cơ của robot. Thêm vào đó, trong quá trình di chuyển sẽ xuất hiện sai số
do trượt bánh hoặc chướng ngại vật, vì vậy cần kết hợp thêm dữ liệu từ cảm biến gia tốc
và con quay hồi chuyển nhằm loại bỏ sai số vị trí và hướng của robot. Do đó chương
này sẽ tiến hành thiết kế hai bộ điều khiển trên để tiến hành di chuyển robot với sai số
giảm thiểu, sau đó cùng với dữ liệu từ cảm biến, định vị robot trong môi trường làm
việc.

4.1. Thiết kế bộ điều khiển động cơ

4.1.1. Thông số thiết kế

Do robot hút bụi không cần độ chính xác cao mà yêu cầu di chuyển nhanh để hoàn
thành nhiệm vụ trong thời gian ngắn, do đó chọn độ vọt lố 10%, thời gian xác lập 0.1s.

4.1.2. Mô hình hoá động cơ

Để đơn giản cho việc điều khiển, động cơ – bánh xe – driver sẽ được xem như một
khối thống nhất.

Quan hệ giữa PWM và tốc độ động cơ

300

250
Tốc độ động cơ (rpm)

200

150

100

50

0
0 20 40 60 80 100 120
PWM

Hình 4.1. Đồ thị liên hệ giữa PWM và tốc độ động cơ

1
 Chọn tín hiệu ra là tốc độ quay của động cơ (vòng/phút) và tín hiệu vào là xung
PWM. Cho các giá trị PWM từ 0% - 100% và tìm đáp ứng của tốc độ động cơ khi đạt
trạng thái xác lập, dựa trên các mẫu thu thập được, tiến hành xử lý dữ liệu và vẽ đồ thị
quan hệ giữa xung PWM và tốc độ động cơ như hình 4.1. Từ đồ thị trên, tốc độ động cơ
là một hàm tuyến tính theo giá trị PWM trong khoảng từ 10-100%.

Tiến hành khảo sát và tìm hàm truyền cho 2 hệ :

- Hệ 1 : Driver và cụm động cơ – bánh xe trái

- Hệ 2 : Driver và cụm động cơ – bánh xe phải

Với tín hiệu đầu vào là PWM = 25% , thời gian lấy mẫu động cơ là 10ms, đường
đáp ứng tốc độ của 2 động cơ như hình 4.2 và 4.3:

Đồ thị đáp ứng của động cơ trái

140
120
Tốc độ động cơ (rpm)

100
80
60
40
20
0
-20 0 500 1000 1500 2000 2500
Thời gian (ms)

Hình 4.2. Đồ thị đường đáp ứng tốc độ của động cơ trái

Đồ thị đáp ứng của động cơ

140
Tốc độ động cơ (rpm)

120
100
80
60
40
20
0
2000
0
80
160
240
320
400
480
560
640
720
800
880
960
1040
1120
1200
1280
1360
1440
1520
1600
1680
1760
1840
1920

2080
2160
2240

Thời gian (ms)

Hình 4.3. Đồ thị đường đáp ứng tốc độ của động cơ trái

2
 Sử dụng công cụ System Identification của Matlab, tìm được hàm truyền lần lượt
của 2 hệ :
17.83
Hàm truyền hệ 1 : 𝐺 (𝑠) =
𝑠+3.803

17.11
Hàm truyền hệ 2 : 𝐺 (𝑠) =
𝑠+3.762

4.1.3. Thiết kế bộ điều khiển PID

PID (Proportional Integral Derivative) là một cơ chế phản hồi vòng điều khiển
tổng quát được sử dụng rộng rãi trong các hệ thống điều khiển công nghiệp, là phương
pháp được sử dụng nhiều nhất trong các bộ điều khiển phản hồi, gồm 3 khâu :

- Khâu tỉ lệ (P): khâu bắt buộc phải có trong bộ điều khiển PID, tín hiệu điều chỉnh
tỉ lệ với sai lệch đầu vào.

- Khâu tích phân (I): giảm sai số xác lập, giúp sai lệch so với tốc độ mong muốn
giảm nhanh hơn.

- Khâu vi phân (D): giảm vọt lố, tăng độ ổn định cho động cơ.

Bộ điều khiển PI có dạng hàm truyền như sau :

𝑡
𝑢(𝑡 ) = 𝐾𝑃 𝑒(𝑡 ) + 𝐾𝐼 ∫0 𝑒(𝜏)𝑑𝜏 (4.1)

Biến đổi Laplace ngược:

𝐾𝐼 𝐾𝐼
𝑈(𝑆) = 𝐾𝑃 𝐸 (𝑠) + 𝐸 (𝑠) = 𝐸 (𝑠) (𝐾𝑃 + ) (4.2)
𝑠 𝑠

𝐾
𝑈(𝑠) 𝐾𝐼 𝐾𝑃 𝑠+𝐾𝐼 𝐾𝑃 (𝑠+ 𝐼 )
𝐾𝑃
⇒ 𝐶 (𝑠 ) = = 𝐾𝑃 + = = (4.3)
𝐸(𝑠) 𝑠 𝑠 𝑠

Phương trình đặc tính của hệ sau khi hiệu chỉnh có dạng :

𝑠 2 + 2𝜁𝜔𝑛 𝑠 + 𝜔𝑛2 = 0 (4.4)

Với :

%𝑂𝑆
ln (
)
𝜁=− 100 = 0.5912
2
√𝜋 2 + (ln (%𝑂𝑆))
100
4
𝜔𝑛 = = 67.6641
𝜁𝑇𝑠

3
 ⇒ 𝑠 2 + 80𝑠 + 4578.4365 = 0

Phương trình đặc tính của hệ 1 sau khi hiệu chỉnh là :

1 + 𝐺𝑃𝐼 (𝑠)𝐺 (𝑠) = 0 (4.5)

𝐾𝑝 𝑠 + 𝐾𝐼 17.83
⇒1+( )( )=0
𝑠 𝑠 + 3.803
⇒ 𝑠 2 + (17.83𝐾𝑃 + 3.803)𝑠 + (17.83𝐾𝐼 + 1) = 0

Đối với hệ động cơ trái, thu được 𝐾𝑃 = 4.2735 ; 𝐾𝐼 = 256.7267. Sử dụng công
cụ PID Tuner của Matlab hiệu chỉnh thông số PI 𝐾𝑃 = 8.1504 ; 𝐾𝐼 = 242.8671, đáp
ứng của hệ trong mô phỏng như hình 4.4 :

Hình 4.4. Mô phỏng đáp ứng tốc độ động cơ trái sau khi có bộ điều khiển PID
• Thời gian xác lập : 0.08s
• Độ vọt lố : 9.341%

Phương trình đặc tính của hệ 2 sau khi hiệu chỉnh là :

1 + 𝐺𝑃𝐼 (𝑠)𝐺 (𝑠) = 0

𝐾𝑝 𝑠 + 𝐾𝐼 17.11
⇒1+( )( )=0
𝑠 𝑠 + 3.762
⇒ 𝑠 2 + (17.11𝐾𝑃 + 3.762)𝑠 + (17.11𝐾𝐼 + 1) = 0

4
 Đối với hệ động cơ phải, thu được 𝐾𝑃 = 4.4558 ; 𝐾𝐼 = 267.5299. Sử dụng công
cụ PID Tuner của Matlab hiệu chỉnh thông số PI 𝐾𝑃 = 8.3073 ; 𝐾𝐼 = 274.4827, đáp
ứng của hệ trong mô phỏng như hình 4.5:

Hình 4.5. Mô phỏng đáp ứng tốc độ động cơ phải sau khi có bộ điều khiển PID
• Thời gian xác lập : 0.0805s
• Độ vọt lố : 9.93%

4.2. Mô hình hoá hệ thống

Hình 4.6 mô tả mô hình động học của robot. Trong đó :

- 𝑋𝑌 là hệ toạ độ toàn cục.

- 𝑋𝐵 𝑌𝐵 là hệ toạ độ cục bộ của robot.

- φ là hướng của robot so với hệ toạ độ toàn cục.

- 𝑟 là bán kính bánh xe.

- 𝑏 là khoảng cách giữa 2 bánh xe.

- 𝐼𝐶𝑅 (instantaneous center of rotation) là tâm quay tức thời của robot.

- 𝑅 là khoảng cách từ tâm quay tức thời đến tâm robot (trung điểm 2 bánh xe).

- 𝑣𝐿 , 𝑣𝑅 lần lượt là vận tốc bánh xe trái và phải.

5
 - 𝜔 là vận tốc góc của robot.

Hình 4.6. Mô hình động học của robot

Dựa theo định nghĩa của vận tốc góc :

𝑏
𝜔 × (𝑅 + ) = 𝑣𝑅 (4.6)
2

𝑏
𝜔 × (𝑅 − ) = 𝑣𝐿 (4.7)
2

Giải 2 phương trình trên để tìm 𝜔 và 𝑅:

𝑣𝑅 −𝑣𝐿
𝜔= (4.8)
𝑏

𝑏 𝑣𝑅 +𝑣𝐿
𝑅= × (4.9)
2 𝑣𝑅 −𝑣𝐿

Vận tốc dài tại tâm robot :

𝑣𝑅 +𝑣𝐿
𝑉 =𝜔×𝑅 = (4.10)
2

Với 𝜔𝑅 , 𝜔𝐿 lần lượt là vận tốc góc của bánh xe quanh trục, vận tốc dài của 2
bánh xe là :

𝑣𝑅 = 𝑟 × 𝜔𝑅 (4.11)

𝑣𝐿 = 𝑟 × 𝜔𝐿 (4.12)

Do đó, phương trình động học của robot có dạng :

6
 𝑟 𝑟
𝑥̇ 𝐵 𝑣𝑥𝐵 2 2 𝜔𝐿
𝑦̇ 𝑣
[ 𝐵 ] = [ 𝑦𝐵 ] = [ 0 0] [𝜔 ] (4.13)
𝑟 𝑟 𝑅
φ̇ 𝜔 −
𝑏 𝑏

Sử dụng phép biến đổi hệ trục toạ độ, mô hình động học của robot trong hệ toạ
độ toàn cục là :

𝑥̇ 𝑐𝑜𝑠φ 0
𝑉
[ 𝑦̇ ] = [ sinφ 0] [ ] (4.14)
𝜔
φ̇ 0 1

Từ phương trình (4.5) và (4.6), có được phương trình cho vận tốc góc của bánh xe
phải 𝜔𝑅 dựa theo tín hiệu đầu vào là vận tốc dài 𝑉 và vận tốc góc 𝜔 của robot:
𝑏
𝑉+𝜔×
𝜔𝑅 = 2
(4.15)
𝑟

Thực hiện tương tự cho bánh xe trái :

𝑏
𝑉−𝜔×
𝜔𝐿 = 2
(4.16)
𝑟

Công thức để tính khoảng cách di chuyển 𝐷 và góc quay φ của robot[2] :
𝐷𝑅 +𝐷𝐿
𝐷= (4.17)
2

𝐷𝑅 −𝐷𝐿
φ= (4.18)
𝐿

với 𝐷𝑅 = 2𝜋𝑟𝑁𝑅 , 𝐷𝐿 = 2𝜋𝑟𝑁𝐿 lần lượt là khoảng cách di chuyển được của bánh xe phải
và trái; 𝑁𝑅 , 𝑁𝐿 lần lượt là số vòng quay của bánh xe phải và trái.

Vị trí của robot trong hệ toạ độ toàn cục :

𝑥 = 𝐷𝑐𝑜𝑠φ (4.19)

𝑦 = 𝐷𝑠𝑖𝑛φ (4.20)

4.3. Kết hợp dữ liệu cảm biến MPU9250 với robot

4.3.1. Lọc sai số cảm biến MPU9250 với Complementary Filter

Tuy đã hiệu chỉnh nhưng sau một thời gian, cả gia tốc kế, từ kế và con quay hồi
chuyển đều sẽ xảy ra hiện tượng trượt, làm giảm độ chính xác và tích luỹ sai số. Do đó,
Complementary Filter (CF), một bộ lọc hiệu quả, phổ biến, tính toán đơn giản, được áp

7
dụng để tăng độ tin cậy cho giá trị của cảm biến bằng cách kết hợp giá trị các cảm biến,
bù trừ sai số lẫn nhau.

Gia tốc kế và từ kế phù hợp cho đo các giá trị tần số thấp (khi cảm biến đứng yên)
và xảy ra sai số lớn khi đọc các giá trị thay đổi nhanh. Ngược lại, con quay hồi chuyển
phù hợp để thu thập những tín hiệu thay đổi nhanh có tần số cao (ví dụ như tốc độ góc
của cảm biến), và tín hiệu tần số thấp (ví dụ như khi đứng yên) sẽ bị nhiễu.

Góc nghiêng đo được từ gia tốc kế[5] :

𝜃𝑥 𝑎𝑟𝑐𝑡𝑎𝑛2(𝑎𝑦 , 𝑎𝑧 )
𝜃𝑎𝑚 = [𝜃𝑦 ] = [𝑎𝑟𝑐𝑡𝑎𝑛2(−𝑎𝑥 , √𝑎𝑦2 + 𝑎𝑧2 )] (4.21)
𝜃𝑧 0

Gia tốc kế chỉ có thể đo được roll 𝜃𝑥 , pitch 𝜃𝑦 , còn yaw 𝜃𝑧 bằng không. Vì vậy, từ
kế sẽ tìm yaw 𝜃𝑧 như sau :

𝑐𝑜𝑠𝜃𝑥 𝑠𝑖𝑛𝜃𝑥 𝑠𝑖𝑛𝜃𝑦 𝑠𝑖𝑛𝜃𝑥 𝑐𝑜𝑠𝜃𝑦 𝑚𝑥

𝑏=[ 0 𝑐𝑜𝑠𝜃𝑦 −𝑠𝑖𝑛𝜃𝑦 ] [𝑚𝑦 ] (4.22)
−𝑠𝑖𝑛𝜃𝑥 𝑐𝑜𝑠𝜃𝑥 𝑠𝑖𝑛𝜃𝑦 𝑐𝑜𝑠𝜃𝑥 𝑐𝑜𝑠𝜃𝑦 𝑚𝑧

𝑏𝑥 𝑚𝑥 𝑐𝑜𝑠𝜃𝑥 + 𝑚𝑦 𝑠𝑖𝑛𝜃𝑥 sinθy + 𝑚𝑧 𝑠𝑖𝑛𝜃𝑥 𝑐𝑜𝑠𝜃𝑦

[𝑏𝑦 ] = [ 𝑚𝑦 𝑐𝑜𝑠𝜃𝑦 − 𝑚𝑧 𝑠𝑖𝑛𝜃𝑦 ] (4.23)
𝑏𝑧 −𝑚𝑥 𝑠𝑖𝑛𝜃𝑥 + 𝑚𝑦 𝑐𝑜𝑠𝜃𝑥 𝑠𝑖𝑛𝜃𝑦 + 𝑚𝑧 𝑐𝑜𝑠𝜃𝑥 𝑐𝑜𝑠𝜃𝑦

⇒ 𝜃𝑧 = 𝑎𝑟𝑐𝑡𝑎𝑛2(−𝑏𝑦 , 𝑏𝑥 ) (4.24)
𝜃𝑧 = 𝑎𝑟𝑐𝑡𝑎𝑛2(𝑚𝑧 𝑠𝑖𝑛𝜃𝑦 − 𝑚𝑦 𝑐𝑜𝑠𝜃𝑦 , 𝑚𝑥 𝑐𝑜𝑠𝜃𝑥 + 𝑚𝑦 𝑠𝑖𝑛𝜃𝑥 sinθy + 𝑚𝑧 𝑠𝑖𝑛𝜃𝑥 𝑐𝑜𝑠𝜃𝑦 )

Góc đo được từ con quay hồi chuyển :

𝜃𝑥𝑡 𝜃𝑥𝑡−1 + 𝜔𝑥 Δ𝑡
𝜃𝜔 = [𝜃𝑦𝑡 ] = [𝜃𝑦 𝑡−1 + 𝜔𝑦 Δ𝑡 ] (4.25)
𝜃𝑧𝑡 𝜃𝑧 𝑡−1 + 𝜔𝑧 Δ𝑡

với Δ𝑡 là thời gian lấy mẫu.

Từ đó, CF sẽ cho ra góc ước đoán 𝜃 bằng cách kết hợp các góc đo từ con quay hồi
chuyển 𝜃𝜔 và góc đo từ gia tốc kế và từ kế 𝜃𝑎𝑚 , với độ lợi 𝛼 trong khoảng từ 0.0 đến
1.0 :

𝜃 = 𝛼𝜃𝜔 + (1 − 𝛼)𝜃𝑎𝑚 (4.26)

8
 Hình 4.14 minh hoạ cấu trúc của CF. CF kết hợp ưu điểm của cả hai, lọc các tín
hiệu tần số thấp đối với con quay hồi chuyển và lọc các tín hiệu tần số cao đối với gia
tốc kế và từ kế.

𝜃𝑎𝑚 Low-pass fitlter

𝑏
+ 𝜃

𝜃𝜔 +

High-pass fitlter

Hình 4.7. Cấu trúc của Complementary Filter

4.3.2. Kết hợp dữ liệu cảm biến bằng bộ lọc Kalman mở rộng

Bộ lọc Kalman mở rộng là một thuật toán dùng để ước đoán trạng thái của một hệ
thống thay đổi phi tuyến theo thời gian. Đây là một kỹ thuật được phát triển từ bộ lọc
Kalman, giúp giải quyết được các mô hình phi tuyến mà bộ lọc Kalman không thể áp
dụng được. Đối với bài toán điều khiển robot hút bụi, bộ lọc Kalman mở rộng sẽ được
sử dụng để kết hợp dữ liệu từ encoder động cơ và cảm biến 9 trục, từ đó ước đoán được
vị trí và hướng của robot trong thực tế.

Đầu tiên, mô hình trạng thái của hệ thống cần được thiết lập[6] :

𝑥𝑡 = 𝑓(𝑥𝑡−1 , 𝑢𝑡−1 ) + 𝑤𝑡−1 (4.27)

𝑧𝑡 = ℎ(𝑥𝑡 ) + 𝑐𝑡 (4.28)

Trong đó :

• 𝑥𝑡 là trạng thái của hệ thống gồm tại thời điểm 𝑡.

• 𝑧𝑡 là giá trị đo lường tại thời điểm 𝑡.
• 𝑓 là hàm dự đoán trạng thái từ dự đoán trước.
• ℎ là hàm dự đoán giá trị đo lường từ trạng thái dự đoán.
• 𝑢𝑡 là vector điều khiển hệ thống.
• 𝑤𝑡 ∼ N(0, Q 𝑡 ) là nhiễu quá trình tại thời điểm 𝑡.

9
 • 𝑐𝑡 ∼ 𝑁(0, 𝑅𝑡 ) là nhiễu đo lường tại thời điểm 𝑡.

Bộ lọc Kalman bao gồm 2 bước, dự đoán và cập nhật. Ở bước dự đoán, dựa trên
mô hình quá trình và trạng thái ước tính hiện tại, bộ lọc Kalman dự đoán trạng thái của
hệ thống tại thời điểm tiếp theo :

𝑥̂𝑡|𝑡−1 = 𝑓(𝑥̂𝑡−1|𝑡−1 , 𝑢𝑡−1 ) (4.29)

𝑃̂𝑡|𝑡−1 = 𝐹𝑡 𝑃𝑡−1|𝑡−1 𝐹𝑡𝑇 + 𝑄𝑡−1 (4.30)

Ở bước cập nhật, bộ lọc Kalman sử dụng mô hình đo lường để kết hợp phép đo
mới từ các cảm biến với dự đoán trước đó nhằm cải thiện độ chính xác của ước tính
trạng thái :

𝑦̃𝑡 = 𝑧𝑡 − ℎ(𝑥̂𝑡|𝑡−1 ) (4.31)

𝑆𝑡 = 𝐻𝑡 𝑃̂𝑡|𝑡−1 𝐻𝑡𝑇 + 𝑅𝑡 (4.32)

𝐾𝑡 = 𝑃̂𝑡|𝑡−1 𝐻𝑡𝑇 𝑆𝑡−1 (4.33)

𝑥̂𝑡|𝑡 = 𝑥̂𝑡|𝑡−1 + 𝐾𝑡 𝑦̃𝑡 (4.34)

𝑃𝑡|𝑡 = (𝐼 − 𝐾𝑡 𝐻𝑡 )𝑃̂𝑡|𝑡−1 (4.35)

Trong đó :
𝛿𝑓
• 𝐹𝑡 = | là ma trận Jacobi chuyển trạng thái.
𝛿𝑥 𝑥𝑡−1|𝑡−1 ,𝑢𝑡

𝛿ℎ
• 𝐻𝑡 = | là ma trận Jacobi đo lường.
𝛿𝑥 𝑥𝑡−1|𝑡−1 ,𝑢𝑡

• 𝑃̂𝑡 là hiệp phương sai dự đoán của trạng thái trước khi đọc giá trị đo lường 𝑧𝑡 .
• 𝑃𝑡 là hiệp phương sai dự đoán của trạng thái sau khi đọc giá trị đo lường 𝑧𝑡 .
• 𝑦̃𝑡 là phần dư giá trị đo lường.
• 𝑆𝑡 là hiệp phương sai dự đoán đo lường.
• 𝐾𝑡 là độ lợi Kalman, thể hiện mức độ tin cậy của dự đoán.

Gọi các giá trị vị trí và vận tốc của robot (𝑥, 𝑦) là (𝑎, 𝑏) để tránh tình huống trùng
ký hiệu với các giá trị trong bộ lọc Kalman mở rộng. Tiến hành áp dụng bộ lọc cho mô
hình robot hút bụi như sau :

- Mô hình không gian trạng thái có dạng :

10
 𝑎𝑡 𝑎𝑡−1 + 𝑣(𝑡−1) 𝑐𝑜𝑠𝜃𝑡−1 Δ𝑡 𝑓1
[𝑏𝑡 ] = [ 𝑏𝑡−1 + 𝑣(𝑡−1) 𝑠𝑖𝑛𝜃𝑡−1 Δ𝑡 ] = [𝑓2 ] (4.36)
𝜃𝑡 𝜃𝑘−1 + 𝜔𝑘−1 Δ𝑡 𝑓3

- Tuyến tính hoá mô hình không gian trạng thái :

𝑥𝑡 = 𝐹𝑡 𝑥𝑡−1 + 𝐵𝑡−1 𝑢𝑡−1 + 𝑤𝑡−1 (4.37)

, trong đó :
𝑐𝑡−1
𝐵𝑡−1 là mô hình điều khiển, 𝑢𝑡−1 = [𝜔 ] là vector điều khiển
𝑡−1

- Ma trận chuyển trạng thái :

𝛿𝑓1 𝛿𝑓1 𝛿𝑓1

𝛿𝑎𝑡−1 𝛿𝑏𝑡−1 𝛿𝜃𝑡−1
𝛿𝑓2 𝛿𝑓2 𝛿𝑓2 1 0 0
𝐹𝑡−1 = = [0 1 0]
𝛿𝑎𝑡−1 𝛿𝑏𝑡−1 𝛿𝜃𝑡−1
0 0 1
𝛿𝑓3 𝛿𝑓3 𝛿𝑓3
[𝛿𝑎𝑡−1 𝛿𝑏𝑡−1 𝛿𝜃𝑡−1 ]

- Ma trận điều khiển :

𝛿𝑓1 𝛿𝑓1
𝛿𝑣𝑡−1 𝛿𝜔𝑡−1
𝛿𝑓2 𝛿𝑓2 𝑐𝑜𝑠𝜃𝑡−1 Δ𝑡 0
𝐵𝑡−1 = = [ 𝑠𝑖𝑛𝜃𝑡−1 Δ𝑡 0]
𝛿𝑣𝑡−1 𝛿𝜔𝑡−1
0 Δ𝑡
𝛿𝑓3 𝛿𝑓3
[𝛿𝑣𝑡−1 𝛿𝜔𝑡−1 ]

- Mô hình không gian trạng thái trở thành :

𝑎𝑡 1
1 0 0 𝑎𝑡−1 𝑐𝑜𝑠𝜃𝑡−1 Δ𝑡 0 𝑣 𝑤𝑡−1
𝑡−1
𝑏
[ 𝑡 ] = [0 1 𝑏
0] [ 𝑡−1 ] + [ 𝑠𝑖𝑛𝜃𝑡−1 Δ𝑡 2
0 ] [𝜔 ] + [𝑤𝑡−1 ]
𝑡−1
𝜃𝑡 0 0 1 𝜃𝑡−1 0 Δ𝑡 3
𝑤𝑡−1

- Trong mô hình robot hút bụi, bộ lọc Kalman mở rộng sẽ được sử dụng để kết hợp
giá trị vị trí 𝑦𝑡1 , 𝑦𝑡2 và hướng 𝑦𝑡3 của robot có được từ encoder của động cơ và hướng 𝑦𝑡4
của robot có được từ cảm biến MPU9250 để xác định trạng thái của robot. Do đó, mô
hình quan sát có dạng :

𝑧𝑡 = 𝐻𝑡 𝑥𝑡 + 𝑐𝑡 (4.38)

11
 𝑦𝑡1 1 0 0 𝑎 1
𝑐𝑡−1
𝑦𝑡2 0 1 0 𝑏 2
↔ 3 =[ ] [ ] + [𝑐𝑡−1 ]
𝑦𝑡 0 0 1 𝜃 3
0 0 1 𝑐𝑡−1
[𝑦𝑡4 ]

- Khởi tạo ma trận hiệp phương sai dự đoán với giá trị nhỏ, chọn 10−9 :

10−9 0 0
𝑃𝑡−1|𝑡−1 =[ 0 10−9 0 ]
0 0 10−9

- Ma trận hiệp phương sai nhiễu quá trình, các giá trị được chọn từ thực nghiệm :

0.05 0 0
𝑄𝑡−1 =[ 0 0.05 0 ]
0 0 0.06

- Ma trận hiệp phương sai nhiễu đo lường, các giá trị đo được từ thực nghiệm :

0.1 0 0 0
0 0.1 0 0
𝑅𝑡−1 =[ ]
0 0 0.05 0
0 0 0 0.06

4.4. Thiết kế bộ điều khiển hệ thống

Khi robot đã được thiết lập đường đi trong môi trường xác định, việc lựa chọn
phương án di chuyển sẽ được dựa trên phương pháp cửa sổ động (Dynamic Window
Approach - DWA). Đây là phương án di chuyển giúp robot bám theo đường đi mong
muốn, đồng thời kết hợp với dữ liệu cảm biến để tránh né vật cản trong môi trường động.
Thuật toán DWA được tóm tắt như sau :

1. Lấy mẫu vận tốc trong không gian điều khiển của robot (𝑣𝑥 , 𝑣𝑦 , 𝜔)
2. Đối với mỗi mẫu vận tốc lấy được, thực hiện mô phỏng di chuyển từ trạng thái
hiện tại của robot để dự đoán quỹ đạo di chuyển nếu vận tốc lấy mẫu được áp
dụng trong thời gian ngắn (hình 4.8).
3. Sử dụng kết hợp các tiêu chí để đánh giá quỹ đạo mô phỏng. Bỏ qua các quỹ đạo
không hợp lệ (quỹ đạo va chạm với vật cản).
4. Chọn quỹ đạo có kết quả đánh giá cao nhất ở bước 3 và dùng vận tốc này để điều
khiển động cơ.
5. Lặp lại bước 1

12
 Hình 4.8. Thuật toán DWA mô phỏng các quỹ đạo di chuyển[5]

Công thức đánh giá quỹ đạo mô phỏng :

𝐺 (𝑉, 𝜔) = ∑𝑛𝑖=1 𝑎𝑖 ∗ 𝑓𝑖 (𝑉, 𝜔) (4.39)

Trong đó, 𝑎𝑖 là trọng số của tiêu chí thứ 𝑖, 𝑓𝑖 (𝑉, 𝜔) là hàm đánh giá quỹ đạo dựa theo
chỉ tiêu 𝑖.

Thuật toán DWA trong mô hình robot hút bụi có các chỉ tiêu sau, trọng số được chọn
dựa trên thực nghiệm :

• BaseObstacleCritic: đánh giá quỹ đạo dựa trên việc quỹ đạo có đi qua vật cản
hay không, 𝑎 = 0.02
• OscillationCritic: đánh giá quỹ đạo dựa trên việc robot có dao động hay không,
𝑎=1
• PathDistCritic: đánh giá quỹ đạo dựa trên khoảng cách của điểm cuối quỹ đạo so
với đường đi thiết lập, 𝑎 = 32
• PathAlignCritic: đánh giá quỹ đạo dựa trên hướng của quỹ đạo so với hướng của
đường đi thiết lập, 𝑎 = 32
• GoalAlignCritic: đánh giá quỹ đạo dựa trên hướng của quỹ đạo so với hướng của
điểm mục tiêu, 𝑎 = 24
• GoalDistCritic: đánh giá quỹ đạo dựa trên khoảng cách của điểm cuối quỹ đạo
so với điểm mục tiêu, 𝑎 = 24

Mô hình tổng quát của robot được tóm tắt bằng sơ đồ khối ở hình 4.9 như sau :

13
 𝑉, 𝜔 PWM
Bộ điều
PI Động cơ Robot
khiển

Bộ lọc
Kalman mở rộng

Hướng Vị trí, hướng

Số
𝑉, 𝜔
Phương trình Tính toán vòng quay
Encoder
động học vận tốc

Complementary Con quay hồi chuyển

IMU
Filter
Gia tốc kế, từ trường kế

Hình 4.9. Mô hình điều khiển tổng quát của robot

Tiến hành thực nghiệm để kiểm tra mô hình điều khiển, cho robot chạy với quỹ
đạo zigzag như hình 4.10. Thu được vị trí và sai số của robot như hình 4.11 và 4.12:

Quỹ đạo zigzag

1.5
1
x (m)

0.5
0
0 0.5 1 1.5
y (m)

Hình 4.10. Quỹ đạo zigzag robot cần đi theo

Biểu đồ vị trí
1.2
1
0.8
Vị trí (m)

0.6
0.4
0.2
0
0 10 20 30 40 50 60
-0.2
Thời gian (s)

y real x real x set y set

Hình 4.11. Biểu đồ vị trí của robot theo quỹ đạo zigzag

14
 Biểu đồ sai số vị trí của robot
0.15

0.1

0.05
Sai số (m)
0
0 10 20 30 40 50 60
-0.05

-0.1

-0.15

-0.2
Thời gian (s)

x error y error

Hình 4.12. Biểu đồ sai số vị trí của robot khi đi theo quỹ đạo zigzag

4.5. Kết luận

Chương 4 đã hoàn thành việc thiết kế bộ điều khiển động cơ, bộ điều khiển hệ
thống, sau đó sử dụng bộ lọc Kalman mở rộng để kết hợp các giá trị cảm biến với giá trị
tính toán được từ mô hình động học của robot nhằm hạn chế các sai số giữa trạng thái
tính toán và trạng thái thực tế của robot xảy ra do sai số cảm biến và sai số do môi trường
làm việc, cũng như tiến hành thực nghiệm để kiểm chứng các nhiệm vụ trên. Chương 5
sẽ tiến hành thiết lập đường đi, lập bản đồ và định vị dựa trên mô hình được xây dựng
từ chương này.

15
 [1]https://search.worldcat.org/title/971588275

[2]https://automaticaddison.com/calculating-wheel-odometry-for-a-differential-
drive-robot/

[3] https://www.mdpi.com/1424-8220/15/8/19302

[5] https://ahrs.readthedocs.io/en/latest/filters/complementary.html

[6] https://en.wikipedia.org/wiki/Extended_Kalman_filter#Discrete-
time_predict_and_update_equations

[7] wiki ros

16