MÔ PHỎNG BALANCE ROBOT

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH
KHOA ĐÀO TẠO CHẤT LƯỢNG CAO

TIỂU LUẬN CUỐI KÌ
MƠ PHỎNG BALANCE ROBOT
NHĨM 5

SVTH:

VŨ ĐỨC THẮNG

MSSV: 18145

NGUYỄN CHÍ KHẢI

MSSV: 18145156

HÀ THANH NHÂN

MSSV: 18145193

LÊ HỒNG HUY

MSSV: 18145127

GVHD:

ThS NGUYỄN TRUNG HIẾU

Tp. Hồ Chí Minh, Tháng 6 năm 2021

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM
Độc lập – Tự do – Hạnh phúc
*********
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN

………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..
………………………………………………………………………………………..

Điểm:

Tp. Hồ Chí Minh, ngày tháng năm
Nhóm sinh viên thực hiện


LỜI CẢM ƠN
Lời đầu tiên chúng em xin chân thành cảm ơn sự hướng dẫn tận tình của thầy ThS.

Nguyễn Trung Hiếu, khoa Cơ khí động lực, trường Đại học Sư Phạm Kỹ Thuật Tp.Hồ
Chí Minh. Trong suốt thời gian thực hiện đề tài, thầy đã dành nhiều thời gian để hướng
dẫn chúng em thực hiện đề tài. Thầy đã hướng dẫn chúng em tìm hiểu những kiến thức
cần thiết để thực hiện đề tài.
Em cũng xin chân thành cảm ơn sự giúp đỡ của nhà trường Đại học Sư Phạm Kỹ
Thuật Tp.Hồ Chí Minh cùng các thầy cơ và anh chị ở xưởng điện ô tô đã cho chúng
em mơi trường để tiếp cần với mơ hình thực tế. Với sự giúp đỡ đó đã cho bọn em dễ
dàng tiếp cận các vấn đề thực tiễn cũng như hiểu rõ được nền tảng lý thuyết và áp
dụng vào thực tế.
Mặc dù nhóm em đã cố gắng hồn thiện đề tài bằng tất cả sự nhiệt tình và năng lực của
bản thân, tuy nhiên trong quá trình thực hiện đề tài khơng thể tránh khỏi những thiếu
sót rất mong nhận được những đóng góp q báu của thầy cơ và các bạn. Nhóm en xin
chân thành cảm ơn!

Tp. Hồ Chí Minh, ngày tháng năm 2021
Nhóm sinh viên thực hiện


MỤC LỤC
LỜI CẢM ƠN ............................................................................................................. III
DANH MỤC CÁC HÌNH .......................................................................................... III
PHẦN 1: LỜI NÓI ĐẦU ...............................................................................................1
1.
2.
3.

Lý do chọn đề tài .............................................................................................1
Mục tiêu của đề tài ..........................................................................................1
Giới hạn phạm vi nghiên cứu ..........................................................................1

PHẦN 2: NỘI DUNG ....................................................................................................2
CHƯƠNG 1: TỔNG QUAN VỀ BALANCE ROBOT ........................................... 2
1.1. Thế nào là balance robot...................................................................................2
1.2. Tại sao phải thiết kế balance robot ...................................................................2
CHƯƠNG 2: ĐẶC TÍNH ĐỘNG LỰC HỌC .......................................................... 5
2.1. Mơ hình hóa balance robot trên địa hình phẳng ...............................................5
2.2. Xây dựng hệ phương trình trạng thái ...............................................................7
2.3. Bộ điều khiển tối ưu LQR ..............................................................................13
2.3.1. Lý thuyết về phương pháp điều khiển LQR (Linear Quadratic Regulator)
...........................................................................................................................13
2.3.2. Các thơng số để mơ phỏng ......................................................................14
2.4. Mơ hình hóa hệ xe hai bánh tự cân bằng dùng phương pháp LQR trên matlab
simulink .................................................................................................................15
PHẦN 3: KẾT LUẬN ..................................................................................................24
TÀI LIỆU THAM KHẢO...........................................................................................25


DANH MỤC CÁC HÌNH
Hình 1.1: Mơ tả ngun lý cân bằng
Hình 1.2: Xe ba bánh trên mặt phẳng
Hình 1.3: Xe ba bánh đi lên
Hình 1.4: Xe ba bánh đi xuống
Hình 1.5: Hai bánh lên và xuống linh động
Hình 0.1: Mơ hình hệ xe cân bằng
Hình 2.2: Bộ điều khiển LQR
Hình 2.3: Khối điều khiển simulink
Hình 2.4: Khối điều khiển scooter trong simulink
Hình 2.5: Đồ thị biểu diễn
Hình 2.6: Đồ thị biểu diễn
Hình 2.7: Đồ thị biểu diễn vị trí bánh xe

Hình 2.8: Đồ thị biểu diễn góc nghiêng thân xe
Hình 2.9: Đồ thị biểu diễn vận tốc góc bánh xe
Hình 2.10: Đồ thị biểu diễn vận tốc bánh xe


PHẦN 1: LỜI NÓI ĐẦU
1. Lý do chọn đề tài
Cùng với sự 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 linh hoạt nhưng lại không chiếm nhiều
không gian.
2. Mục tiêu của đề tài
Mục tiêu của đồ án là xây dựng được robot có khả năng di chuyển trên hai bánh,
làm phương tiện di chuyển hiệu quả linh động, dễ dàng xoay trở trong không gian chật
hẹp. Trong khuôn khổ 6 tuần thực hiện đề tài những mục tiêu được đề ra như sau:
• Tìm hiểu về các loại robot cân bằng, nguyên lý cơ bản về cân bằng.
• Nghiên cứu lý thuyết điều khiển cho hệ con lắc ngược.
• Nghiên cứu lý thuyết điều khiển giải thuật PID và LQR dùng điểu khiển hệ xe tự
cân bằng.
• Thực hiện mô phỏng Matlab Simulink để kiểm nghiệm và đi tìm bộ số phù hợp
cho hệ xe ứng với từng giải thuật.
• Lập trình điều khiển.
3. Giới hạn phạm vi nghiên cứu
Đề 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 mơ hình,
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 giải
thuật cân bằng PID, thuật toán LQR. Robot có thể cân bằng có khả năng điều khiển

để di chuyển nhưng chưa có nhiều ứng dụng.

1


PHẦN 2: NỘI DUNG
CHƯƠNG 1: TỔNG QUAN VỀ BALANCE ROBOT
1.1. Thế nào là balance robot

Hình 1.1: Mơ tả ngun lý cân bằng
Đối với các xe hay robot ba hay bốn bánh, việc thăng bằng và ổn định 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 2 bánh có cấu trúc như xe đạp, việc thăng bằng khi khơng di chuyển là
hồn tồ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. Còn đối với xe hay robot hai bánh tự cân bằng, là loại chỉ
có hai bánh với trục của hai bánh xe trùng nhau, để cho robot cân bằng, trọng tâm của
nó cần được giữ nằm ngay giữa các bánh xe. Điều này giống như ta giữ một cây gậy
dựng thẳng đứng cân bằng trong lòng bàn tay. Thực ra, trọng tâm của tồn bộ robot
khơng được biết nằm ở vị trí nào, cũng khơng có cách nào tìm ra nó. Do vậy, thay vì
tìm cách xác định trọng tâm nằm giữa các bánh xe ta cho xe di chuyển nhằm triệt tiêu
góc nghiêng của xe.
1.2. Tại sao phải thiết kế balance robot
Việc thiết kế robot hai bánh tự cân bằng là nền tảng để phát triển xe hai bánh tự
cân bằng sau này vì vậy ta cần so sánh xe hai bánh tự cân bằng với các thể loại ba
bánh hay bốn bánh hiện nay.
Những mobile robot xây dựng hầu hết robot là những robot di chuyển bằng ba
bánh xe, với hai bánh lái được lắp ráp đồng trục, và một bánh đ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 các xe 4 bánh, thường một
đầu xe có hai bánh truyền động và đầu xe còn lại được gắn một hoặc hai bánh lái.

2


Hình 1.2: Xe ba bánh trên mặt phẳng
Việc thiết kế ba hay bốn bánh làm cho xe/mobile robot được 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 đi, hay bất kỳ
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/robot sẽ khơng ổn định dễ bị ngã, còn nếu đặt nhiều vào bánh đi thì hai bánh
chính sẽ mất khả năng bám. Nhiều thiết kế xe/robot có thể di chuyển tốt trên địa hình
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
xuống đồi, trọng lượng xe/robot dồn vào đầu 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ó dừng hoạt động và chỉ quay trịn bánh
xe.

Hình 1.3: Xe ba bánh đi lên
Khi di chuyển lên đồi, sự việc còn tệ hơn, trọng tâm thay đổi về phía sau và
thậm chí làm xe/robot bị lật úp khi di chuyển trên bậc thang.Việc bố trí bốn bánh xe,
giống như xe hơi đồ chơi hay các loại xe bốn bánh hiện đang sử dụng trong giao thông
không gặp vấn đề nhưng điều này sẽ làm các xe/robot không gọn.

3


Hình 1.4: Xe ba bánh đi xuống
Ngược lại, các xe dạng 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 dốc, mặc dù bản thân là một hệ thống khơng ổn định. 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 bước 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 ngồi
vùng đỡ của các bánh xe để có thể gây ra sự lật úp.

Hình 1.5: Hai bánh lên và xuống linh động

4


CHƯƠNG 2: ĐẶC TÍNH ĐỘNG LỰC HỌC
2.1. Mơ hình hóa balance robot trên địa hình phẳng
Xây dựng phương trình trạng thái mơ tả hệ xe tự cân bằng:

Hình 0.1: Mơ hình hệ xe cân bằng

5


Bảng 0.1: Thơng số kĩ thuật.
Kí hiệu (đơn vị)

Ý nghĩa

MW [kg]

Khối lượng bánh xe

MB [kg]

Khối lượng Robot

R [m]

Bán kính bánh xe

W [m]

Khoảng cách hai bánh xe
Khoảng cách từ trọng tâm Robot đến

H [m]

trục bánh xe
Lực ma sát giữa hai bánh xe và mặt

𝐹𝑇𝐿,𝑇𝑅 [𝑁]

phẳng di chuyển
Phản lực tác động lên khung xe và bánh

𝐹𝐿,𝑅 [𝑁]

trái, bánh phải

𝐽𝐵 [N.m]

Momen quán tính thân xe

g [m/s2]

Gia tốc trọng trường

𝜃 [rad]

Góc nghiêng thân robot

𝜃𝐿,𝑅 [rad]

Góc nghiêng bánh trái và bánh phải

𝜓 [rad]

Góc quay thân robot

𝐽𝑇𝐿,𝑇𝑅 [N.m]

Moment quán tính của bánh xe
Momen lực tác động lên bánh trái và

𝐶𝐿,𝑅 [N.m]

bánh phải
Lực tác dụng lên bánh trái và bánh phải

𝑁𝐿,𝑅 [𝑁]

từ thân xe theo phương Y
Phản lực tác dụng lên bánh xe trái và

𝑁𝑇𝐿,𝑇𝑅 [𝑁]

phải theo phương Y

𝑋𝑊𝐿,𝑊𝑅 [𝑚]

Vị trí bánh trái và phải theo phương X

𝑋𝐵 [m]

Vị trí thân xe theo phương X
Vị trí bánh xe trái và phải theo phương

𝑌𝑊𝐿,𝑊𝑅 [m]

Y
Vị trí thân xe theo phương Y

𝑌𝐵 [m]

6


2.2. Xây dựng hệ phương trình trạng thái
Có nhiều phương pháp khác nhau để xác định mơ hình động học của xe hai bánh
tự cân bằng: phương pháp Newton, phương pháp Lagrange…Nhưng trong báo cáo này,
tác giả sử dụng phương pháp Newton để xác định mơ hình động học đối tượng xe hai
bánh tự cân bằng.
-

Đối với bánh trái (cũng như bánh phải):
Áp dụng định luật Newton cho chuyển động ta có lực tác động lên bánh xe theo

phương x là:

𝑀𝑊 𝑋̈𝑊𝐿 = 𝐹𝑇𝐿 − 𝐹𝐿

(1)

Áp dụng định luật Newton cho chuyển động ta có lực tác động lên bánh xe theo phương
y là:
̈ = 𝑁𝑇𝐿 − 𝑁𝐿 − 𝑀𝑊 𝑔
𝑀𝑊 𝑌𝑊𝐿

(2)

Tổng lực tác động vào tâm bánh xe là:
𝐽𝑊𝐿 𝜃̈𝑊𝐿 = 𝐶𝐿 − 𝐹𝑇𝐿 𝑅

(3)

𝑋𝑊𝐿 = 𝜃𝑊𝐿 𝑅

(4)

Mặt khác:
1

𝐽𝑊𝐿 = 𝑀𝑊𝐿 𝑅2

(5)

2

𝜓=

𝑋𝐿 −𝑋𝑅
𝑊

(6)
-

Đối với thân robot:

Tương tự áp dụng định luật Newton cho chuyển động ta có các kết quả sau:
𝑀𝐵 𝑋̈𝐵 = 𝐹𝐿 + 𝐹𝑅

(7)

𝐶 +𝐶
𝑀𝐵 𝑌𝐵̈ = 𝑁𝐿 + 𝑁𝑅 − 𝑀𝐵 𝑔 + 𝐿 𝑅 𝑠𝑖𝑛𝑠𝑖𝑛 𝜃𝐵

(8)

𝐽𝐵 𝜃̈𝐵 = (𝑁𝐿 + 𝑁𝑅 )𝐻 𝑠𝑖𝑛 𝜃𝐵 − (𝐹𝐿 + 𝐹𝑅 )𝐻 𝑐𝑜𝑠 𝜃𝐵 − (𝐶𝐿 + 𝐶𝑅 )

(9)

𝐻

Mặt khác ta có:
𝑋𝐵 = 𝐻 𝑠𝑖𝑛𝑠𝑖𝑛 𝜃𝐵 +

𝑋𝑊𝐿 +𝑋𝑊𝑅

(10)

2

𝑌𝐵 = −𝐻(1 − 𝑐𝑜𝑠𝑐𝑜𝑠𝜃𝐵 )

(11)

1

𝐽𝐵 = 𝑀𝐵 𝐻 2

(12)

𝜃 = 𝜃𝐵 = 𝜃𝑊 = 𝜃L = 𝜃R

(13)

3

7


𝑋𝑊𝑀 =

𝑋𝑊𝐿 +𝑋𝑊𝑅

(14)

2

𝑊
𝐽𝜓 𝜓̈ = (𝐹𝐿 − 𝐹𝑅 )

(15)

2

-

Tổng quát ta có được các phương trình sau:
𝐽𝐵 𝜃̈𝐵 = (𝑁𝐿 + 𝑁𝑅 )𝐻 𝑠𝑖𝑛 𝜃𝐵 − (𝐹𝐿 + 𝐹𝑅 )𝐻 𝑐𝑜𝑠 𝜃𝐵 − (𝐶𝐿 + 𝐶𝑅 )
(16)
𝑀𝐵 𝑋̈𝐵 = 𝐹𝐿 + 𝐹𝑅

(17)

𝐶 +𝐶
𝑀𝐵 𝑌𝐵̈ = 𝑁𝐿 + 𝑁𝑅 − 𝑀𝐵 𝑔 + 𝐿 𝑅 𝑠𝑖𝑛𝑠𝑖𝑛 𝜃𝐵
𝐻

(18)

Thay (18) và (17) vào (16) ta được:
𝐽𝐵 𝜃̈ = (𝑀𝐵 𝑌𝐵̈ + 𝑀𝐵 𝑔 −

𝐶𝐿 + 𝐶𝑅
𝑠𝑖𝑛 𝜃)𝐻 𝑠𝑖𝑛 𝜃 − 𝑀𝐵 𝑋̈𝐵 𝐻 𝑐𝑜𝑠 𝜃 − (𝐶𝐿 + 𝐶𝑅 )
𝐻

𝐽𝐵 𝜃̈ = 𝑀𝐵 𝐻(𝑌𝐵̈ 𝑠𝑖𝑛 𝜃 − 𝑋̈𝐵 𝑐𝑜𝑠 𝜃) + 𝑀𝐵 𝑔𝐻 𝑠𝑖𝑛 𝜃 − (𝐶𝐿 + 𝐶𝑅 )(1 + 𝑠𝑖𝑛2 𝜃) (19)
Mặt khác xét tại mỗi bánh xe trái, phải theo (1) và (2) ta có:
𝑀𝑊 𝑋̈𝑊𝐿 = 𝐹𝑇𝐿 − 𝐹𝐿
̈ = 𝑁𝑇𝐿 − 𝑁𝐿 − 𝑀𝑊 𝑔
𝑀𝑊 𝑌𝑊𝐿
Và
𝑋𝑊𝑀 =

𝑋𝑊𝐿 + 𝑋𝑊𝑅
2

Suy ra:
𝑀𝑊 (𝑋̈𝑊𝐿 + 𝑋̈𝑊𝑅 ) = −(𝐹𝐿 + 𝐹𝑅 ) + 𝐹𝑇𝐿 + 𝐹𝑇𝑅

(20)

Mà theo (3) ta có:
𝐽𝑊𝐿 𝜃̈𝑊𝐿 = 𝐶𝐿 − 𝐹𝑇𝐿 𝑅
Đặt:

𝐶𝐿 + 𝐶𝑅 = 𝐶𝜃

Vậy phương trình (20) trở thành:
𝑀𝑊 (𝑋̈𝑊𝐿 + 𝑋̈𝑊𝑅 ) = −𝑀𝐵 𝑋̈𝐵 +

𝐶𝐿 + 𝐶𝑅 − (𝐽𝑊𝐿 𝜃̈𝑊𝐿 + 𝐽𝑊𝑅 𝜃̈𝑊𝑅 )
𝑅

Suy ra:
𝐶

𝐽 𝜃̈
2𝑀𝑊 𝑋̈𝑊𝑀 = −𝑀𝐵 𝑋̈𝐵 + 𝜃 − 2 𝑊 𝑊
𝑅

(21)

𝑅

Theo (10) và (11) ta lại có:
𝑋𝐵 = 𝐻 𝑠𝑖𝑛𝑠𝑖𝑛 𝜃𝐵 +

𝑋𝑊𝐿 + 𝑋𝑊𝑅
2
8


𝑌𝐵 = −𝐻(1 − 𝑐𝑜𝑠𝑐𝑜𝑠 𝜃𝐵 )
Suy ra:
𝑋̇𝐵 = 𝜃̇𝐻 𝑠𝑖𝑛 𝜃 + 𝑋̇𝑀𝑊
𝑌𝐵̇ = −𝜃̇𝐻 𝑠𝑖𝑛 𝜃
Và:
𝑋̈𝐵 = 𝜃̈𝐻 𝑐𝑜𝑠 𝜃 − 𝐻 𝑐𝑜𝑠 𝜃 𝜃̇ + 𝑋𝑀𝑊
𝑌𝐵̈ = −𝜃̈𝐻 𝑠𝑖𝑛 𝜃 − 𝐻 𝑐𝑜𝑠 𝜃 𝜃̇

(22)

Từ (22) suy ra:
𝑌𝐵̈ 𝑠𝑖𝑛 𝜃 − 𝑋̈ 𝑐𝑜𝑠 𝜃 = −𝐻𝜃̈ − 𝑋̈𝑀𝑊 𝑐𝑜𝑠 𝜃

(23)

Ta có hệ phương trình:
Từ (19) suy ra:
𝐽𝐵 𝜃̈ = −𝑀𝐵 𝐻 2 𝜃̈ + 𝑔𝐻𝑀𝐵 𝑠𝑖𝑛 𝜃 − (1 + 𝑠𝑖𝑛2 𝜃)𝐶𝜃 − 𝐻𝑀𝐵 𝑐𝑜𝑠 𝜃 𝑋̈𝑀𝑊
(24)
Từ (21) và (23) suy ra:
̈

𝐽 𝜃
𝐶
2𝑀𝑊 𝑋̈𝑀𝑊 = 𝑀𝐵 𝐻 𝑠𝑖𝑛 𝜃 𝜃 2 − 𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃 𝜃̈ − 2 𝑊 𝑊 + 𝜃 − 𝑀𝐵 𝑋𝑊𝑀 (25)
𝑅

𝑅

Mà:
1
1
𝑀𝑊𝐿 𝑅2
𝐽𝑊 = 𝑀𝑊 𝑅2
2
2
{
=> {
1
1
𝐽𝐵 = 𝑀𝐵 𝐻 2
𝐽𝐵 = 𝑀𝐵 𝐻 2
3
3

𝐽𝑊𝐿 =

Thay vào (24) và (25) ta được:
4

{

3

𝑀𝐵 𝐻 2 𝜃̈ + 𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃 𝑋̈𝑀𝑊 = 𝑔𝐻𝑀𝐵 𝑠𝑖𝑛 𝜃 − (1 + 𝑠𝑖𝑛2 𝜃)𝐶𝜃

(26)

𝐶
(𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃 + 𝑀𝑊 𝑅)𝜃̈ + (2𝑀𝑊 + 𝑀𝐵 )𝑋̈𝑀𝑊 = 𝑀𝐵 𝑠𝑖𝑛 𝜃 𝜃̇ 2 + 𝜃
𝑅

(
=> {

3 (𝑀𝑊 𝑅+𝑀𝐵 𝐿 𝑐𝑜𝑠 𝜃) 𝑐𝑜𝑠 𝜃
4

(2𝑀𝑊 +

3 𝑀𝐵 𝐻 𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃

={

4 (2𝑀𝑊 +𝑀𝐵 )𝐻

𝑀𝐵 𝐻 𝑠𝑖𝑛 𝜃 (𝜃̇ 2 ) −

− 1) 𝜃̈

(2𝑀𝑊 +𝑀𝐵 )𝐿
3 (𝑀𝑊 𝑅+𝑀𝐵 𝐿 𝑐𝑜𝑠 𝜃) 𝑐𝑜𝑠 𝜃
) 𝑋̈
𝑀𝐵 −
4
𝐿
2

3 𝑔 𝑠𝑖𝑛 𝜃
3 1+𝑠𝑖𝑛 𝜃
3
(𝜃̇ 2 ) −
+(
+
2
4

𝐻

4 𝑀𝐵 𝐻

3 𝑔(𝑀𝑊 𝑅+𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃) 𝑠𝑖𝑛 𝜃
4

𝐻

+(

𝑐𝑜𝑠 𝜃

4 (2𝑀𝑊 +𝑀𝐵 )𝑅𝐻

) 𝐶𝜃

3 (𝑀𝑊 𝑅+𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃)(1+𝑠𝑖𝑛 𝜃2 )
4

𝑀𝐵 𝐻 2

𝑅

(27)
Mặt khác ta lại có:
Tu (1) và (3) suy ra:
9

1

+ ) 𝐶𝜃


𝐶𝐿
𝐽𝑊𝐿
− 𝑋̈𝑊𝐿 (𝑀𝑊 + 2 )
𝑅

𝑅
{
𝐶𝑅
𝐽𝑊𝑅
𝐹𝑅 =
− 𝑋̈𝑊𝐿 (𝑀𝑊 + 2 )
𝑅
𝑅
𝐹𝐿 =

Suy ra:
𝐹𝐿 − 𝐹𝑅 =

𝐶𝐿 − 𝐶𝑅
𝐽𝑊
− (𝑋̈𝑊𝐿 − 𝑋̈𝑊𝑅 )(𝑀𝑊 + 2 )
𝑅
𝑅
(28)

Ta lại có:
𝜓=

𝑋𝐿 − 𝑋𝑅
𝑋̈𝑊𝐿 − 𝑋̈𝑊𝑅
⇒ 𝜓̈ =
𝑊
𝑊

Vậy:

𝐹𝐿 − 𝐹𝑅 =

𝐶𝐿 − 𝐶𝑅
𝐽𝑊
− 𝐿𝜓̈(𝑀𝑊 + 2 )
𝑅
𝑅
(29)

Lại có:
𝑊
𝑊 𝐶𝐿 − 𝐶𝑅 𝑊
𝐽𝑊
(𝐹𝐿 − 𝐹𝑅 ) =
− 𝑊𝜓̈(𝑀𝑊 + 2 )
2
2
𝑅
2
𝑅
2
𝑊
𝐽𝑊
𝑊 𝐶𝐿 − 𝐶𝑅
⇒ [𝐽𝜓 +
(𝑀𝑊 + 2 )] 𝜓̈ =
2
𝑅
2
𝑅

𝐽𝜓 𝜓̈ =

(30)
Mà ta có:
1
𝑀𝑊 𝑅2
2
1
𝑊 2
1
𝐽
=
𝑀
(
)
=
𝑀 𝑊2
𝜓
𝐵
{
3
2
12 𝐵
𝐽𝑊 =

Vậy (30) trở thành:
1
𝑊2
1
𝑊 𝐶𝐿 − 𝐶𝑅

𝑀𝐵 𝑊 2 +
(𝑀𝑊 + 𝑀𝑊 )𝜓̈ =
12
2
2
2
𝑅
6
⇒ 𝜓̈ =
(𝐶 − 𝐶𝑅 )
(9𝑀𝑊 + 𝑀𝐵 )𝑅𝑊 𝐿
Đặt 𝐶𝐿 − 𝐶𝑅 = 𝐶𝜓 ta được:
𝜓̈ =

6
(𝐶 )
(9𝑀𝑊 + 𝑀𝐵 )𝑅𝑊 𝜓
(31)

10


3 (𝑀𝑊 𝑅 + 𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃) 𝑐𝑜𝑠 𝜃
− 1) 𝜓̈
4
(2𝑀𝑊 + 𝑀𝐵 )𝐻
3 (𝑀𝑊 𝑅 + 𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃) 𝑐𝑜𝑠 𝜃
(2𝑀
+
𝑀

−
) 𝑋̈
𝑊
𝐵
{
4
𝐻
(

=
3 𝑀𝐵 𝐻 𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜃 2
3 𝑔 𝑠𝑖𝑛 𝜃
3 1 + 𝑠𝑖𝑛 𝜃 2 3
𝑐𝑜𝑠 𝜃
̇
(𝜃 ) −
+(
+
)𝐶
4 (2𝑀𝑊 + 𝑀𝐵 )𝐻
4 𝐻
4 𝑀𝐵 𝐻 2
4 (2𝑀𝑊 + 𝑀𝐵 )𝑅𝐻 𝜃
3 𝑔(𝑀𝑊 𝑅 + 𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃) 𝑠𝑖𝑛 𝜃
3 (𝑀𝑊 𝑅 + 𝑀𝐵 𝐻 𝑐𝑜𝑠 𝜃)(1 + 𝑠𝑖𝑛 𝜃 2 ) 1
2
̇
𝑀𝐵 𝐻 𝑠𝑖𝑛 𝜃 (𝜃 ) −
+(
+ )

4
𝐻
4
𝑀𝐵 𝐻 2
𝑅
{
(32)
𝜓̈ =

6
(𝐶 )
(9𝑀𝑊 + 𝑀𝐵 )𝑅𝑊 𝜓
(33)

Tuyến tính hóa mơ hình phi tuyến của xe 2 bánh tự cân bằng thành mơ hình
khơng gian trạng thái:
Từ phương trình (32) xét các điều kiện:
-

+ Vì 𝜃 << 1[𝑟𝑎𝑑]
+ Suy ra: 𝑠𝑖𝑛 𝜃 ≈ 𝜃; (𝑠𝑖𝑛 𝜃)2 ≈ 0; 𝑐𝑜𝑠 𝜃 ≈ 1;
Như vậy phương trình (32) trở thành
4
𝑀𝐵 𝐻 2 𝜃̈ = −𝑀𝐵 𝐻𝑋̈ + 𝑀𝐵 𝑔𝐻𝜃 − 𝐶𝜃
3
{
(2𝑀𝑊 + 𝑀𝐵 )𝑋̈ = −(𝑀𝑊 𝑅 + 𝑀𝐵 𝐻𝜃̈) +

𝐶𝜃
𝑅

4
𝐶𝜃
𝑋̈ = 𝑔𝜃 − 𝐻𝜃̈ −
3
𝑀𝐵 𝐻
⇒
(2𝑀𝑊 + 𝑀𝐵 )
𝐶𝜃
(2𝑀𝑊 + 𝑀𝐵 )𝜃̈ =
𝑋̈ +
{
𝑀𝑊 𝑅 + 𝑀𝐵 𝐻
𝑅(𝑀𝑊 𝑅 + 𝑀𝐵 𝐻)
(34)
Giải hệ phương trình (34) ta được:
𝑔𝑀𝐵
𝑌
𝜃 − 𝐶𝜃
𝑋
𝑋
⇒
4 𝑔𝑀𝐵
4𝐻𝑌
1
𝑋̈ = − 𝐻
𝜃+(
−
)𝐶
{

3
𝑋
3𝑋
𝑀𝐵 𝐻 𝜃
𝜃̈ =

(35)
Trong đó:

11


4
𝑀𝐵
𝑀𝐵 𝐻 −
(𝑀 𝑅 + 𝑀𝐵 𝐻)
3
2𝑀𝑊 + 𝑀𝐵 𝑊
𝑀𝐵
1
𝑌=
+
{
(2𝑀𝑊 + 𝑀𝐵 )𝑅 𝐻
𝑋=

Từ (33) và (35) ta có hệ phương trình trạng thái được viết lại như sau:
0
𝑥̇
0

𝑥̈
[ ̇] = [
0
𝜃
0
𝜃̈

1
0
0
0

0
𝐵1
0
𝐵2

0 𝑥
0
0 𝑥̇
𝐶
] [ ] + [ 1 ] [𝐶𝜃 ]
1 𝜃
0
̇
0 𝜃
𝐶2
(36)

𝜓̇

0
[ ]=[
̈
0
𝜓

0
1 𝜓
] [ ̇ ] + [ ] [𝐶𝜓 ]
𝐶3
0 𝜓

Trong đó:
4 𝑀𝐵
𝐵1 = 𝑔(1 − 𝐻
)
3 𝑋

𝐵2 =

𝑔𝑀𝐵
𝑋

𝐶1 = (

4𝐻𝑌
1
−
)
3𝑋

𝑀𝐵 𝐻

𝐶2 = −

𝐶3 =

𝑌
𝑋

6
(9𝑀𝑊 + 𝑀𝐵 )𝑅𝑊

𝐶𝜃 = 𝐶𝐿 + 𝐶𝑅
{𝐶 = 𝐶 − 𝐶
𝜓
𝐿
𝑅

12


2.3. Bộ điều khiển tối ưu LQR
LQR(Linear Quadratic Regulator) là một phương pháp trong lý thuyết điều khiển
hiện đại sử dụng cách tiếp cận không gian trạng thái để phân tích hệ thống. Hệ thống có
thể được ổn định bằng cách sử dụng thông tin phản hồi trạng thái đầy đủ. Sơ đồ của loại
hệ thống điều khiển này được thể hiện ở hình dưới đây:

r

NON-LINEAR

TWO WHEEL
BALANCING
ROBOT

u

x

K

Hình 2.2: Bộ điều khiển LQR
LQR là một hệ thống điều khiển được thiết kế trong điều kiện làm việc tối ưu
theo một tiêu chuẩn chất lượng nhất định (chỉ số hiệu suất). Trạng thái tối ưu đạt được
tùy thuộc vào yêu cầu chất lượng được thiết lập, về sự hiểu biết và ảnh hưởng của các
đối tượng và điều kiện hoạt động của hệ thống điều khiển.
Các thiết lập của một bộ điều khiển , một máy hoặc quá trình được tìm thấy
bằng cách sử dụng một thuật toán toán học mà giảm thiểu một chức năng chi phí với
các yếu tố trọng số được cung cấp bởi con người. Hàm chi phí thường được định nghĩa
là tổng các độ lệch của các phần tử chính bao gồm nhiệt độ q trình hoặc độ cao
mong muốn, từ giá trị mong muốn của chúng.
2.3.1. Lý thuyết về phương pháp điều khiển LQR (Linear Quadratic Regulator)
- Ta có đối tượng tuyến tính mơ tả bởi phương trình trạng thái:
𝑥̇ (𝑡) = 𝐴𝑥(𝑡) + 𝐵𝑢(𝑡)

𝑦 = 𝐶𝑥 + 𝐷𝑢

Trong đó:
𝑥(𝑡) = [𝑥1 (𝑡), 𝑥2 (𝑡), . . . , 𝑥𝑛 (𝑡)]𝑇 :vector trạng thái
𝑢(𝑡) = [𝑢1 (𝑡), 𝑢2 (𝑡), . . . , 𝑢𝑛 (𝑡)]𝑇 : vector tín hiệu điều khiển
13

-

Chỉ tiêu chất lượng dạng tồn phương, trong đó thời điểm cuối 𝑡𝑓 = ∞
1 ∞ 𝑇
𝐽(𝑢) = ∫ [𝑥 (𝑡)𝑄𝑥(𝑡) + 𝑢𝑇 (𝑡)𝑅𝑢(𝑡)] 𝑑𝑡
2 0

Q và R là các ma trận trọng số xác định dương
-

Tín hiệu điều khiển tối ưu:

𝑢∗ (𝑡) = −𝐾𝑥(𝑡)
Trong đó:
𝐾 = 𝑅−1 𝐵𝑇 𝑃
P là nghiệm bán xác định của phương trình đại số Ricatti
𝑃𝐴 + 𝐴𝑇 𝑃 + 𝑄 − 𝑅𝐵𝑅−1 𝐵𝑇 𝑃 = 0
2.3.2. Các thông số để mô phỏng
- Chỉ mô phỏng phương pháp LQR để điều khiển cân bằng và điều khiển vị trí robot,
khơng mơ phỏng góc quay của robot (vì góc quay khơng ảnh hưởng đến việc cân bằng
của robot)
- Cơng thức LQR:
o Phương trình trạng thái: 𝑥̇ = 𝐴𝑥 + 𝐵𝑢
𝑦 = 𝐶𝑥 + 𝐷𝑢
o Tín hiệu điều khiển tối ưu: 𝑢∗ (𝑡) = −𝐾𝑥(𝑡)
o Xác định K thông qua Matlab: K = lqr(A, B, Q, R);
- Xác định ma trận A, B, C, D
+ Phương trình trạng thái điều khiển trạng thái cân bằng và vị trí:

0 1 0 0 x
0
ẋ
0 0 B1 0 ẋ
C
ẍ
[ ̇] = [
] [ ] + [ 1 ] [Cθ ]
0 0 0 1 θ
0
θ
̇
̈θ
0 0 B2 0 θ
C2
Ma trận A:
0
0
[
0
0

1
0
0
0

0
𝐵1
0

𝐵2

0
0
]
1
0

Ma trận B

Ma trận C

0
𝐶
[ 1]
0
𝐶2

[

+ Phương trình trạng thái điều khiển góc quay:

14

1
0

0
0

0
1

Ma trận D
0
]
0

0
[ ]
0


𝜓̇
0
[ ]=[
0
𝜓̈

0
1 𝜓
] [ ̇ ] + [ ] [𝐶𝜓 ]
𝐶3
0 𝜓

+ Thông số vật lý để mô phỏng:
Gia tốc trọng trường g=9.81
+ Lựa chọn Q và R
Chọn R = 0.01
Chọn Q = [5 0 0 0;0 0 0 0;0 0 100 0 ;0 0 0 0];

Hoặc tính Q=C’*C
+ Tính toán bộ điều khiển tối ưu bằng PP LQR liên tục (nhập thơng số vào MabLap
tính)
K = lqr(A, B, Q, R)

2.4. Mơ hình hóa hệ xe hai bánh tự cân bằng dùng phương pháp LQR trên matlab
simulink

Hình 2.3: Khối điều khiển simulink

15


Hình 2.4: Khối điều khiển scooter trong simulink

Như đã nói về phần lý thuyết ở trên thì tham số K được tính tốn dựa vào phương
trình Riccati được hổ trợ bởi Matlab với hàm LQR(A,B,Q,R) khi các ma trận được tìm
ra.
* Tìm các ma trận A, B tại vị trí cân bằng dùng phần mềm hổ trợ Matlab
% khai bao tham so
%-------------------------------------------------------------clc

T = 5;

Mw = 0.09; % khoi luong banh xe 0.09kg
Mb = 1; % khoi luong than xe + nguoi
R = 0.03; % ban kinh banh xe 0.03m
L = 0.04; % trong tam xe 0.04m
D = 0.17; % khoang cach hai banh xe 0.17m
16

g = 9.81; % gia toc trong truong

%-------------------------------------------------------------% THAM SO BO DIEU KHIEN
theta_init = 0.1;
%-------------------------------------------------------------% tinh toan cac tham so cua pttt
X = 4*Mb*L/3-Mb*(Mw*R+Mb*L)/(2*Mw+Mb);
Y = 1/L+Mb/(R*(2*Mw+Mb));
A23 = 4*g*L*Mb/(3*X);
A43 = g*Mb/X;
B2 = 4*L*Y/(3*X)-1/(Mb*L);
B4 = -Y/X;
B6 = 6/(R*D*(9*Mw+Mb));

%-------------------------------------------------------------%BO DIEU KHIEN LQR
A = [0 1 0 0;0 0 A23 0;0 0 0 1;0 0 A43 0]
B = [0 ;B2 ;0; B4]
%Thiet Ke LQR
R = 0.01;
Q = [5 0 0 0;0 0 0 0;0 0 100 0;0 0 0 0];
K = lqr(A, B, Q, R);
%-------------------------------------------------------------sim('LQR.mdl');
%-------------------------------------------------------------% Ve tin hieu dau ra
%-------------------------------------------------------------17


figure(2);
plot(theta.time,theta.signals.values);
ylabel('Output');

xlabel('Thoi gian');
title('XE HAI BANH TU CAN BANG');
legend('x','x dot','theta','theta dot');
grid on;
figure(1);
plot(theta1.time,theta1.signals.values);
%ylabel('THETA Output');
%xlabel('Thoi gian');
%title('XE HAI BANH TU CAN BANG');
%legend('theta');
grid on;
hold on;

0
0
30.128
0]
0
1
572.1153 0
0
𝐵 = [ 140.6233 ]
0
−3.1054𝑒 + 03

0
𝐴 = [0
0
0

1
0
0
0

Các ma trận Q và R là:
5
𝑄 = [0
0
0

0
0
0
0

1
R__=[
0

0
]
1

0
0
100
0

0

0
]
0
0

K=lqr(A,B,Q,R__) là:
K=[−22.3607

−9.0051

−102.4885

−0.6634]
18


* Đồ thị mơ phỏng matlab của hệ thống

Hình 2.5: Đồ thị biểu diễn góc nghiêng thân xe

Hình 2.6: Đồ thị biểu diễn vị trí, tốc độ, góc nghiêng, vận tốc góc

19


* Đồ thị mơ phỏng Simulink của hệ thống

Hình 2.7: Đồ thị biểu diễn vị trí bánh xe

20