Thiết kế xe hai bánh tự cân bằng

ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA ĐIỆN – ĐIỆN TỬ
---------------o0o---------------

ĐỒ ÁN :

THIẾT KẾ XE HAI BÁNH TỰ CÂN BẰNG
GVHD:
PGS.TS Huỳnh Thái Hoàng
Nhóm:
Huỳnh Trung Tín
MSSV: 1613563
Đặng Phúc Bảo
MSSV: 1610157

Tp Hồ Chí Minh, Tháng 06/2019


CHƯƠNG 1 GIỚI THIỆU.........................................................................................1
1.1 Giới thiệu............................................................................................................1
1.2 Thế nào là xe hai bánh tự cân bằng (two wheels self balancing)........................2
1.3 Mục tiêu đề tài.....................................................................................................3
1.4 Đối tượng nghiên cứu.........................................................................................4
1.5 Phạm vi nghiên cứu.............................................................................................5
CHƯƠNG 2 XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN......................................1
Các giải thuật xử lí tín hiệu từ cảm biến ........................................................... 1
2.1.1 Lọc bổ phụ thông tần (complementary filter).................................................
2.1.2 Lọc Kalman.....................................................................................................
2.1.2.1 Lý thuyết về bộ lọc Kalman.....................................................................
2.1.2.2 Quy trình ước lượng................................................................................

2.1.2.3 Bản chất toán học của bộ lọc..................................................................
2.1.2.4 Bản chất xác suất của bộ lọc..................................................................
2.1.2.5 Giải thuật lọc Kalman rời rạc................................................................
2.1.2.6 Bộ lọc Kalman rời rạc đơn giản dùng cho phạm vi đồ án này...............
2.1.3 So sánh bộ lọc bổ phụ và bộ lọc Kalman rời rạc............................................

2.1

2.2 Các giải thuật điều khiển hiện đại......................................................................2
2.2.1 Phương pháp điều khiển PID..........................................................................
2.2.2 Thiết kế bộ điều khiển PID Cascade...............................................................
2.2.2.1 Quy trình thiết kế bộ điều khiển PID nối tiếp..............................................
2.2.2.2 Phương pháp điều chỉnh thủ công PID.......................................................
2.3 Mô hinh lý thuyết động cơ DC..........................................................................3
CHƯƠNG 3 THỰC HIỆN..........................................................................................1
3.1 Sơ đồ khối của hệ thống ......................................................................................
3.2 Thiết kế cơ khí......................................................................................................
3.2.1 Tóm tắt thiết kế................................................................................................
3.2.2 Thiết kế mô hình..............................................................................................
Mạch điện tử...................................................................................................... 3
3.3.1 Nguồn điện....................................................................................................4
3.3.2 Mạch công suất điều khiển động cơ.............................................................4
3.3.3 Động cơ DC Servo Giảm tốc GA37................................................................
3.3.4 Vi điều khiển stm32f4......................................................................................

3.3


3.3.5 Gia tốc kế MPU-6050 GY-521........................................................................
3.4 Các phương thức giao tiếp.................................................................................5

3.4.1 Điều chế độ rộng xung PWM..........................................................................
3.4.2 Giao thức I2C..................................................................................................
3.4.3 Giao tiếp UART...............................................................................................
CHƯƠNG 4 KẾT LUẬN............................................................................................1
4.1 Những kết quả đạt được........................................................................................
4.2 Hướng phát triển...................................................................................................


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.2 Thế nào là xe hai bánh tự cân bằng (two wheel self balancing)

Đối với các xe ba hay bốn bánh, việc thăng bằng và giữ ổn định của chúng nhờ
trọng tâm của chúng nằm trên 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 khi di chuyển / đứng yên là điều
hoàn toàn không thể, vì nguyên lý thăng bằng dựa trên tính chất con quay hồi
chuyển ở hai bánh xe khi đang quay. Còn đố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, để xe cân bằng, trọng tâm
của xe (bao gồm cả người sử dụng) cần được giữ nằm ngay giữa các bánh xe.
Giống như ta giữ một cây gậy thẳng đứng cân bằng trong lòng bàn tay.

1.3 Mục tiêu đề 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 với cấu trúc đơn giản phục vụ cho việc nghiên cứu, giáo dục, 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 biểu diễn.
1.4 Đối tượng nghiên cứu
 Xây dựng phần cứng robot
Thiết kế phần cứng, làm mạch điều khiển, mạch nguồn,
 Tìm hiểu giải thuật lọc kalman, lọc bổ phụ để xử lý dữ liệu thu thập được
 Tìm hiểu giải thuật để điều khiển cho một robot 2 bánh tự cân bằng
Sử dụng bộ điều khiẻn PID gồm 2 vòng điều khiển, vòng ngoài điều khiển
góc, phần trong điều khiển tốc độ động cơ
 Sử dụng vi điều khiển stm32f4
 Cảm biến gia tốc MPU6050.
1.5 Phạm vi nghiên cứu
Phạm vi nghiên cứu của đề tài dùng các giải thuật điều khiển rất quan trọng. Có
nhiều giải thuật điều khiển được đề cập tới đối tượng phi tuyến khác nhau. Các giải
thuật điều khiển được dùng là điều khiển tuyến tính, điều khiển phi tuyến và điều
khiển thông minh…. Đa số các bộ điều khiển trong thực tế đề là điều khiển tuyến tính
(PID, LQR) và đều cho đáp ứng tốt. Một số bộ điều khiển tuyến tính đòi hỏi phương
trinh toán học (LQR, đặt cực.), một số bộ điều khiển tuyến tính không yêu cầu (PID).
Như vậy, đối tượng điều khiển phi tuyến như hệ xe hai bánh tự cân bằng thì giải
thuật điều khiển tuyến tính có đáp ứng tốt không? Đáp ứng tốt ở mức độ như thế nào?
Đó là những câu hỏi mà luận văn muốn hướng đến giải quyết các vấn đề thắc mắc
trên.
Trong phạm vi đề tài này, chúng em thiết kế để xe có thể tự đứng cân bằng được.
Mục tiêu là thực hiện giải thuật điều khiển PID trên đối tượng xe hai bánh cân bằng,
có kết quả thực tế được kiểm chứng.

Chương 2

XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN
2.1 Các giải thuật xử lí tín hiệu từ cảm biến
Như đã biết, giá trị ngõ ra được quan tâm hang đầu của xe scooter hai bánh chính
là góc giữa tay lái thẳng đứng hoăc sàn xe với chiều trọng lực. Nhiều loại cảm biến có
thể dùng đo góc như encoder, resolver, inclinometer,… với trọng lực theo phương
pháp không tiếp xúc với sàn là cảm biến đo vận tốc góc và cảm biến gia tốc ứng dụng
để đo góc tĩnh (cảm biến đo khoảng cách tới mặt đất bằng hồng ngoại chỉ dùng được
với sàn đất phẳng, nằm ngang và láng). Để làm trơn nhiễu và kết hợp tín hiệu từ hai
camr biến accelerometer và gyro, người ta thường dùng các bộ lọc trung bình, lọc bổ
phụ thông tần complementary, lọc thích nghi – bộ lọc Kalman và các bộ lọc khác.
2.1.1 Lọc bổ phụ thông tần (complementary filter)
Bộ lọc bổ phụ được sử dụng trong trường hợp khi có hai nguồn giá trị khác
nhau trong việc ước lượng một giá trị, và đặc tính nhiễu của hai giá trị chẳng hạn một
nguồn mang lại thông tin đúng trong vùng tần số thấp trong khi nguồn khác chỉ đúng
trong vùng tần số cao. Bộ lọc bổ phụ là kết hợp những ngõ ra của độ nghiêng và vận
tốc gyro nhằm thu được khả năng ước lượng tốt nhất của sự định hướng, để đền bù
cho sự trôi giá trị gyro (drift) và cho đáp ứng chậm của cảm biến đo nghiêng.

Hình 2.1 Sơ đồ nguyên lý hoạt động của bộ lọc bổ phụ trên mô hình con lắc ngược

 Ước lượng góc được thu nhận là tổng của tín hiệu từ hai nhánh giá trị được biểu
diễn ở hình 3.6. Góc nghiêng của cảm biến đo nghiêng được đưa vào bộ lọc
Gi(s) phân bổ giá trị góc nghiêng để ước lượng chỉ trong vùng tần số thấp. Vận


tốc góc của gyro cung cấp vào bộ lọc Gg(s). Bộ lọc này được thiết kế để ngõ ra

phân bổ đến ước lượng chỉ trong vùng tần số cao. Yêu cầu cơ bản của bộ lọc có
thể được tính như sau:
 Toàn bộ hệ thống có sự khuếch đại là hằng số và phase tổn thất nhỏ nhất
ngay vùng tần số cắt của cảm biến nghiêng.
 Để giữ độ nhạy offset của gyro là nhỏ nhất, cảm biến nghiêng nên được
dùng trong vùng băng thông tần số rộng nhất có thể.
 Số lượng tham số thiết kế nên nhỏ để dễ cho sự điều chỉnh và áp dụng bộ lọc
trong thực tế của các vi điều khiển

2.1.2 Lọc Kalman
2.1.2.1 Lý thuyết về bộ lọc Kalman
Được đề xuất từ năm 1960 bởi giáo sư Kalman để thu thập và kết hợp
linh động các thông tin từ cảm biến thành phần. Một khi phương trình định
hướng và mẫu thống kê nhiễu trên mỗi cảm biến được biết và xác định, bộ
lọc Kalman sẽ cho ước lượng giá trị tối ưu (chính xác do đã được loại sai số,
nhiễu) như là đang sử dụng một tín hiệu ‘tinh khiết’ và có độ phân bổ không
đổi. Trong hệ thống này, tín hiệu cảm biến vào bộ lọc gồm hai tín hiệu: từ
cảm biến góc (inclinometer) và cảm biến vận tốc góc (gyro). Tín hiệu ngõ ra
của bộ lọc là tín hiệu của inclinometer và gyro đã được loại nhiễu nhờ hai
nguồn tín hiệu hỗ trợ và xử lý lẫn nhau trong bộ lọc, thông qua quan hệ (vận
tốc góc = đạo hàm/vi phân của giá trị góc).


 Một ví dụ mô phỏng về bộ lọc Kalman cho một máy bay chiến đấu bằng
MatLAB :

Hình 2.2 Tín hiệu thu nhận chưa được lọc

Hình 2.3 Tín hiệu đã qua bộ lọc Kalman

Bộ lọc Kalman đơn giản là thuật toán xử lý dữ liệu hồi quy tối ưu. Có
nhiều cách xác định tối ưu, phụ thuộc tiêu chuẩn lựa chọn trình thông số
đánh giá. Nó cho thấy rằng bộ lọc Kalman tối ưu đối với chi tiết cụ thể trong
bất kỳ tiêu chuẩn có nghĩa nào. Một khía cạnh của sự tối ưu này là bộ lọc
Kalman hợp nhất tất cả thông tin được cung cấp tới nó. Nó xử lý tất cả giá
trị sẵn có, ngoại trừ độ sai số, ước lượng giá trị hiện thời của những giá trị
quan tâm, với cách sử dụng hiểu biết động học thiết bị giá trị và hệ thống,
mô tả số liệu thống kê của hệ thống nhiễu, gồm nhiễu ồn, nhiễu đo và sự


không chắc chắn trong mô hình động học, và những thông tin bất kỳ về điều
kiện ban đầu của giá trị quan tâm.
2.1.2.2 Quy trình ước lượng
Kalman filter định vị vấn đề chung nhằm ước lượng giá trị x thuộc R n
của tiến trình kiểm soát thời gian gián đoạn biểu diễn bằng phương trình
tuyến stochastic khác nhau:
[2-1]
Với giá trị z thuộc Rm
[2-2]
Giá trị thay đổi ngẫu nhiên wk và vk trình bày cách xử lý và đo độ
nhiễu (theo thứ tự). Chúng tách biệt nhau, đó là nhiễu trắng, và với sự sắp
xếp bản chất thông thường:
[2-3]
[2-4]
Trong thực tiễn, ma trận tương quan tiến trình nhiễu Q và tương quan
đo độ nhiễu R có lẽ thay đổi với mỗi bước thời gian hay giá trị, tuy nhiên
giả thuyết rằng ở đây chúng là hằng số.
Ma trận Anxn trong phương trình [2-1] liên hệ với giá trị ở thời điểm
trước k-1 đến giá trị ở thời điểm hiện tại k, trong sự thiếu hụt không chỉ ở
hàm lái dạng mà còn do nhiễu tiến trình. Chú ý rằng trong thực tế A có lẽ

thay đổi tại mỗi thời điểm, nhưng ở đây, ta xem nó là hằng số. Ma trận B nx1
liên quan hoạt động kiểm soát tùy ý u thuộc R l đối với x. Ma trận Hmxn liên
quan với giá trị đo zk. Trong thực tế, H có thể thay đổi tại mỗi thời điểm hay
giá trị, nhưng ta xem nó là hằng số.
2.1.2.3 Bản chất toán học của bộ lọc
Theo định nghĩa xˆk  n (siêu âm) ước lượng trạng thái priori tại
bước k đưa ra tiến trình priori tại bước k, và xˆk  n là ước lượng trạng


thái posteriori tại bước k với giá trị z k. Từ đó có thể xác định sai số ước
lượng priori và posteriori là:
[2-5]
Tương quan sai số ước lượng priori là:
[2-6]
và tương quan sai số ước lượng posteriori là:
[2-7]
Bắt nguồn từ việc lấy đạo hàm phương trình của bộ lọc Kalman, với mục
tiêu tìm một phương trình ước lượng trạng thái posteriori xˆk như sự liên
kết tuyến tính ước lượng priori xˆk và sự khác biệt đánh giá giữa giá trị
thực zk và giá trị chuẩn tắc H xˆk được trình bày ở công thức [2-8].
[2-8]
Độ lệch (zk – H xˆ k ) được gọi là đại lượng cách tân hay giá trị thặng dư.
Giá trị thặng dư phản ánh sự khác nhau giữa giá trị chuẩn tắc H xˆ k và
giá trị thực zk. Giá trị thặng dư A của 0 nghĩa là hai thành phần này hoàn
toàn nhất quán nhau.
Ma trận K nxm trong công thức được chọn là độ lợi hay hệ số pha trộn
tối thiểu hóa tương quan sai số posteriori. Một dạng kết quả K tối thiểu
hóa được trình bày như sau:

[2-9]

Thấy rằng tương quan sai số giá trị R tiến tới 0, độ lợi K đánh giá giá trị
thặng dư cao hơn. Đặc biệt:


[2-10]

Hơn nữa, tương quan sai số ước lượng priori P’k tiến tới 0. độ lợi K
đánh giá thặng dư thấp hơn. Đặc biệt:

[2-11]

Cách nghĩ khác về phụ cấp bằng K thống kê sai lệch đo lường R tiến
tới 0, đo lường thực tế zk đáng tin cậy hơn, trong khi dự đoán đo lường H xˆ
k

-

ít tin cậy hơn. Hơn nữa, thống kê sai lệch ước lượng priori P k tiến tới 0

cách đo thực tế zk ít tin cậy hơn, trong khi dự đoán cách đo lường

H xˆ

-

k

đáng tin cậy hơn.
2.1.2.4 Bản chất xác suất của bộ lọc

Sự điều chỉnh cho [2-8] đã xác định rõ bản chất ước lượng priori

với

điều kiện tất cả các giá trị zk đều có nghĩa (luật phân bố Bayes). Nó chỉ rõ
rằng bộ lọc Kalman duy trì hai khoảng thời gian đầu tiên của sự phân bố
trạng thái.
[2-12]
Ước lượng trạng thái posteriori [2-8] phản ánh trung bình (khoảng phân
bố thời gian đầu tiên) của sự phân bố trạng thái – thông thường nó được
sắp xếp nếu các điều kiện của [2-3] và [2-4] xảy ra cùng lúc. Tương quan
sai số ước lượng posteriori [2-7] phản ánh giá trị của sự phân bố trạng thái
(khoảng phân bố thời gian thứ hai). Nói cách khác:

[2-13]
2.1.2.5 Giải thuật lọc Kalman rời rạc
Bộ lọc Kalman ước lượng tiến trình bằng việc 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 tiến trình tại vài thời điểm và sau
đó thu sự phản hồi trong hình thức của giá trị (độ nhiễu). Chẳng hạn, 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ị. Phương trình cập nhật thời gian chịu
trách nhiệm dự đoán trước (thời gian) giá trị hiện tại và tương quan sai số
ước lượng để đạt ước lượng priori cho thời điểm kế tiếp. Phương trình cập
nhật giá trị 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ào ước lượng priori nhằm đạt sự cải tiến ước lượng posteriori.
Phương trình cập nhật thời gian có thể xem là phương trình chuẩn tắc,
trong khi phương trình cập nhật giá trị có thể xem là phương trình chính xác.
Thật vậy, thuật toán ước lượng cuối cùng giống như thuật toán chính xác –

chuẩn tắc nhằm giải quyết vấn đề số liệu được trình bày ở hình 4.4.

Time Update
(“Predict”)

Mesurement Update
(“Correct”)

Hình 2.4 Chu kỳ lọc Kalman rời rạc

 Cụ thể hơn


Hình 2.5 Chu kỳ lọc Kalman rời rạc

Phương trình cập nhật thời gian bộ lọc Kalman rời rạc:

[4-14]
Phương trình cập nhật giá trị cho bộ lọc Kalman rời rạc:

[4-15]
 Trong sơ đồ trên:
 xx�-- là ký hiệu ước lượng dự đoán của trạng thái xk dựa trên quan sát ở thời
điểm k-1.
 xx� là ký hiệu ước lượng dự đoán của trạng thái �� dựa trên quan sát ở thời
điểm k.
 Hiệp phương sai ước lượng dự đoán Pk là trung bình phương sai số trong ước
lượng xx�.
 Nhìn vào sơ đồ tóm tắt các phương trình của bộ lọc Kalman ta có thể thấy mục
đích chính của chúng ta là đi tìm hệ số K (độ lợi của bộ lọc Kalman, hệ số này

sẽ thay đổi sau mỗi vòng lặp tính toán). Tức là làm sao đưa ra được ước lượng
trạng thái của hệ thống tại thời điểm k là sát nhất với trạng thái của hệ thống
nhất.
 Sau mỗi cặp cập nhật thời gian và giá trị, tiến trình được lập lại như trước ước
lượng Posteriori đã từng dự đoán hay tiên đoán ước lượng Priori mới. Trạng
thái đệ quy tự nhiên là một trong nhiều điềm đặc thu hút của bộ lọc ra hoạt
động của tất cả dữ liệu một cách trực tiếp cho mỗi chu kỳ ước lượng. Bộ lọc
Kalman thay thế điều kiện đệ quy ước lượng hiện tại cho những giá trị quá
khứ.
2.1.2.6 Bộ lọc Kalman rời rạc đơn giản dùng cho phạm vi đồ án này
Cảm biến góc được thiết kế bằng cách kết hợp một cảm biến gia tốc
mộ trục và một gyro vận tốc một trục tích hợp trên MPU6050. Bộ lọc


Kalman gồm hai trạng thái, với trạng thái là góc và trạng thái còn lại là giá trị
cơ sở. Bộ lọc Kalman là một vấn đề thực sự phức tạp, mặc dù đã được tối ưu
nhiều lần đoạn code C.
Trước tiên thiết lập các ma trận và các vector cần thiết cho bộ lọc
Kalman:
Góc và vận tốc được mô tả bởi không gian trạng thái tuyến tính.

[2-16]





Trong đó:
x là giá trị góc (ở đây chúng ta quan tâm góc pitch của xe cân bằng)
ẋ là vận tóc góc (theo phương nghiêng của góc pitch)

Lưu ý: x và ẋ là góc xoay và vận tốc góc của một trục đang xét. Hay nói một
cách đơn giản là ta đang thiết kế bộ lọc Kalman với hai đầu vào và một đầu ra.
Trạng thái của hệ thống tại thời điểm k được phát triển từ trạng thái của
hệ thống tại thời điểm (k-1):
[2-17]
Với và do không có tín hiệu điều khiển vào nên Buk được bỏ qua.
Các ma trận QN và RN là các ma trận đối xứng nửa xác định dương, đại
diện cho nhiễu quá trình và nhiễu đo lường. Vì các loại nhiễu này rất khó xác
định nên tạm đặt chúng như sau:
[2-18]
[2-19]
Các thông số có trong ma trận sẽ được người dùng gán giá trị. Các giá trị
này phải căn cứ vào mức độ nhiễu thu được từ đồ thị tính từ Gyroscope và
Acceleromater.
Ma trận hiệp phương sai:


[2-20]
Ma trận quan sát:
[2-21]
Vì giá trị đo là góc từ Acc, đây là giá trị cần quan sát từ ma trận trạng thái
trung gian xx� nên H= [1 0].
 Áp dụng phương trình toán học của bộ lọc Kalman:


Bước 1: Trạng thái ước đoán:

[2-22]


Bước 2: Dự đoán, ước lượng hiệp phương sai:


[2-23]


Bước 3: Độ lệch đo lường

[2-24]
 Hiểu một cách đơn giản hơn:
Độ lệch đo lường = giá trị trạng thái đo được- giá trị dự đoán sẽ được đo
trước đó.


Bước 4: Độ lệch phương sai


[2-25]


Bước 5: Tính độ lợi Kalman

[2-26]


Bước 6: Cập nhật trạng thái ước đoán đã điều chỉnh

[2-27]


Bước 7: Cập nhật phương sai ước lượng đã điều chỉnh

[2-28]
2.1.2.7 Thực hiện bộ lọc Kalman rời rạc cho mpu6050 và stm32f4


 Khai báo biến cấu trúc Kalman

Hình 2.6 Code các thông số cần thiết cho bộ lọc kalman

 Hàm lọc kalman, trả về góc được lọc


Hình 2.7 Code chu kỳ lọc Kalman rời rạc

2.1.3 So sánh bộ lọc bổ phụ và bộ lọc Kalman rời rạc


Hình 2.8 Lọc Kalman (tím) và lọc Complementary (đỏ) đo thực nghiệm

2.2 Các giải thuật điều khiển hiện đại.
2.2.1 Phương pháp điều khiển PID.

Hình 2.9 Giải thuật PID cổ điển


Sơ đồ điều khiển PID được đặt tên theo ba khâu hiệu chỉnh của nó, tổng của ba
khâu này tạo thành bởi các biến điều khiển MV. Ta có:
Trong đó:
Pout , Iout và Dout là các thành phần đầu ra từ ba khâu của bộ điều khiển

PID, được xác định như dưới đây.
 Khâu tỉ lệ:
Khâu tỉ lệ (đôi khi còn gọi là độ lợi) làm thay đổi giá trị đầu ra, tỉ lệ với giá trị
sai số hiện tại. Đáp ứng tỉ lệ có thể được điều chỉnh bằng cách nhân sai số đó với
một hằng số Kp được gọi là độ lợi tỉ lệ.
Khâu tỉ lệ được cho bởi:
 Trong đó:
 Pout: thừ số tỉ lệ của đầu ra.
 Kp: Độ lợi tỉ lệ, thông số điều chỉnh.
 e: sai số e= SP-PV (set point và process variable)
 t: thời gian hay thời gian tức thời (hiện tại).


Hình 2.10 Đồ thị PV theo thời gian với Kp thử nghiệm(Ki, Kd là hằng số)

 Khâu tích phân:
Phân phối của khâu tích phân (đôi khi còn gọi là reset) tỉ lệ thuận với cả biên độ
sai số lẫn quãng thời gian xảy ra sự cố. Tổng sai số tức thời theo thời gian (tích
phân sai số) cho ta tích lũy bù đã được hiệu chỉnh trước đó. Tích lũy sai số sau đí
được nhân với độ lợi tích phân và cộng với tín hiệu đầu ra của bộ điều khiển. Biên
độ phân phối của khâu tích phân trên tất cả tác động điều chỉnh được xác định bởi
độ lợi tích phân Ki.
Thừa số tích phân cho bởi
 Trong đó:
 Iout: thừa số tích phân của đầu ra.
 Ki: độ lợi tích phân, 1 thông số điều chỉnh
 e: sai số e= SP-PV
 t: thời gian hoặc thời gian tức thời
 �: mộ biến tích phân trung gian

Hình 2.11 Đồ thị PV theo thời gian với Ki thử nghiệm(Kp, Kd là hằng số)

 Khâu vi phân:
Tốc độ thay đổi của sai số quá trình được tính toán bằng cách xác định độ dốc
của sai số theo thời gian (tức là đạo hàm bậc một theo thờigian) và nhân tốc độ này


với độ lợi tỉ lệ Kd. Biên độ của phân phối khâu vi phân (đôi khi còn gọi là tốc độ)
trên tất cả các hành vi điều khiển được giới hạn bởi độ lợi vi phân, Kd.
Thừa số vi phân được cho bởi:

Hình 2.12 Đồ thị PV theo thời gian với Kd thử nghiệm(Kp, Ki là hằng số)

 Tóm tắt:
Định nghĩa rằng u(t) là đầu ra của bộ điều khiển, biểu thức cuối cùng
của giải thuật PID là:

[4-29]
 Trong đó các thông số điều chỉnh là:
 Độ lợi tỉ lệ Kp
Giá trị càng lớn thì đáp ứng càng nhanh, do đó sai đố càng lớn, bù
khâu tỉ lệ càng lớn. Một giá trị độ lợi tỉ lệ quá lớn sẽ dẫn đến quá trình mất ổn
định và dao động.
 Độ lợi tích phân Ki


Giá trị càng lớn, kéo theo sai số ổn định bị khử càng nhanh. Đổi lại là
độ vọt lố càng lớn, bất kì sai số âm nào được tích phân trong suốt đáp ứng
quá độ phải triệu tiêu tích phân bằng sai số dương trước khi tiến tới trạng
thái ổn định

 Độ lợi vi phân Kd
Giá trị càng lớn càng giảm độ vọt lố, nhưng làm chậm đáp ứng quá độ
và có thể dẫn đến mất ổn định do khuếch đại nhiễu tín hiệu trong phép vi
phân sai số.
2.2.2 Thiết kế bộ điều khiển PID Cascade
Bộ điều khiển PID gồm 2 bộ: một bộ kiểm soát vận tốc của xe với đầu vào là
vận tốc đặt cho xe, bộ PID còn lại dùng để kiểm soát góc nghiêng của xe so với vị trí
thẳng đứng. Đầu ra của bộ điều khiển PID vận tốc là đầu vào của bộ PID góc.
2.2.2.1 Quy trình thiết kế bộ điều khiển PID nối tiếp:
Đầu tiên, ta tiến hành thực hiện bộ điều khiển PID đơn giám sát góc nghiêng.
Tín hiệu đầu vào là góc đặt mà tại đó xe đứng cân bằng. Tín hiệu từ cảm biến góc
nghiêng MPU6050 qua bộ lọc Kalman sẽ được so sánh với góc đặt để đưa vào bộ
PID. Ngõ ra bộ điều khiển PID chính là giá trị xung PWM và được đưa vào mạch cầu
H để lái hai động cơ. Nếu góc nghiêng càng lớn thì ngõ ra PID càng lớn như vậy
momen quay của động cơ cũng mạnh để kịp thời đưa xe về vị trí cân bằng. Một vấn đề
đặt ra là hướng điều khiển động cơ. Giả sử xe đang bị nghiêng về phía trước, khi đó
có thể coi giá trị ngõ ra PID>0, dựa vào đó ta sẽ xuất hiện hướng cho động cơ quay về
phía trước để cân bằng xe.
Với chỉ 1 bộ điều khiển góc nghiêng thì cũng đủ để xe không bị ngã. Tuy nhiên,
một vấn đề đặt ra là xe dao động di chuyển tới lui. Vì vậy, ta cần một bộ điều khiển
PID để chỉnh vận tốc xe. Bộ điều khiển này lấy tín hiệu đọc được từ encoder gắn trực
tiếp lên hai bánh xe để xác định vận tốc bánh xe. Giá trị vận tốc này sẽ so sánh với giá
trị đặt để được một sai số và đưa vào bộ điều khiển PID vận tốc, ngõ ra của bộ điều
khiển này sẽ là góc đặt của bộ PID góc nghiêng.
Như vậy với bộ điều khiển PID cascade sẽ thực hiện được cả 2 nhiệm vụ là vừa
giữ cho xe cân bằng vừa di chuyển. Có thể hiểu nôm na, bộ điều khiển PID vận tốc tự
bản thân nó sẽ có nhiệm vụ tính ra 1 giá trị góc đặt hợp lý cho bộ điều khiển PID góc
nghiêng. Đi ều này có thể thấy rõ khi ra tác động 1 lực vào xe, khi đó vận tốc xe thay
đổi => giá trị góc đặt cũng thay đổi theo hướng nghiêng về phía lực tác động để nhanh
chóng kéo xe về vị trí cân bằng. Nhờ vậy xe có thể cân bằng rất ổn định trong trường

hợp có tác động từ bên ngoài hay khi trọng tâm xe thay đổi.


Hình 2.13 Sơ đồ điều khiển 2 vòng PID cho robot

 Cách tính toán vận tốc xoay cho robot
Với � = ∅. Đặt �l và �r lần lượt là tịnh tiến của bánh trái và tịnh tiến của bánh
phải như hình vẽ sau

Hình 2.14 Hình chiếu ngang thân xe

Dựa trên hình 4.14 ta có:
�l*R- �r*R = S
Mà theo công thức tính cung đường tròn ta có ∅ *R= S , thay vào ta được
Khi đó ta cộng điện áp dựa trên tính toán PID rotate với tín hiệu đặt là ∅ vào
bánh trái, và điện áp bánh phải giữ nguyên, lúc này xe sẽ xoay quay 1 bánh với trọng
tâm là bánh phải.
�r*R- �l*R = S