Điều khiển xe hai bánh cân bằng di chuyển theo quỹ đạo

ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH

TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA ĐIỆN - ĐIỆN TỬ
--------------------------------------

TĂNG NĂNG TƯỜNG

Đề tài:
ĐIỀU KHIỂN XE HAI BÁNH CÂN BẰNG DI
CHUYỂN THEO QUỸ ĐẠO
Chuyên ngành: TỰ ĐỘNG HĨA

LUẬN VĂN THẠC SĨ

THÀNH PHỐ HỒ CHÍ MINH – 2014


CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI
TRƯỜNG ĐẠI HỌC BÁCH KHOA- ĐHQG – HCM

Cán bộ hướng dẫn khoa học: PGS-TS. HUỲNH THÁI HOÀNG
Cán bộ chấm nhận xét 1:

TS. NGUYỄN ĐỨC THÀNH

Cán bộ chấm nhận xét 2:

TS. TRỊNH HOÀNG HƠN

Luận văn thạc sĩ đư ợc bảo vệ tại Trường Đại học Bách Khoa, ĐHQG thành phố

Hồ Chí Minh, ngày 18 tháng 07 năm 2014.

Thành phần hội đồng đánh giá luận văn thạc sĩ gồm:
1. Chủ tịch hội đồng:

PGS-TS. DƯƠNG HOÀI NGHĨA

2.Cán bộ phản biện 1:

TS. NGUYỄN ĐỨC THÀNH

3.Cán bộ phản biện 2:

TS. TRỊNH HỒNG HƠN

4.Ủy viên:

TS. TRƯƠNG ĐÌNH CHÂU

5.Thư ký:

TS. NGƠ MẠNH DŨNG

Xác nhận của Chủ tịch Hội đồng đánh giá luận văn và Trưởng khoa quản lý
chuyên ngành sau khi luận văn được sửa chữa (nếu có).

CHỦ TỊCH HỘI ĐỒNG

TRƯỞNG KHOA ĐIỆN-ĐIỆN TỬ

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VI ỆT NAM
Độc lập - Tự do - Hạnh phúc

ĐẠI HỌC QUỐC GIA TP.HCM
TRƯỜNG ĐẠI HỌC BÁCH KHOA

NHIỆM VỤ LUẬN VĂN THẠC SĨ

Họ tên học viên: TĂNG NĂNG TƯỜNG
Ngày, tháng, năm sinh: 30/06/1988
Chuyên ngành: Tự động hóa
I. TÊN ĐỀ TÀI:

MSHV: 11900723
Nơi sinh: Cần Thơ
Mã số :

XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO

II. NHIỆM VỤ VÀ NỘI DUNG:
 Nhiệm vụ : Thiết kế và chế tạo mơ hình xe hai bánh cân bằng di chuyển theo
quỹ đạo được đặt trước.
 Nội dung :
- Tìm hiểu nguyên lý cơ b ản, nguyên tắc hoạt động của mơ hình xe hai bánh.
- Mơ hình hóa.
- Thiết kế mơ hình cơ khí.
- Xây dựng thuật tốn điều khiển.
- Mơ phỏng trên MATLAB-SIMULINK.
- Lập trình và hiệu chỉnh.

III. NGÀY GIAO NHIỆM VỤ : 19/08/2013
IV. NGÀY HOÀN THÀNH NHIỆM VỤ: 20/06/2014
V. CÁN BỘ HƯỚNG DẪN :
PGS-TS HUỲNH THÁI HOÀNG

Tp. HCM, ngày . . . . tháng .. . . năm 2014
CÁN BỘ HƯỚNG DẪN
(Họ tên và chữ ký)

CHỦ NHIỆM BỘ MÔN ĐÀO TẠO
(Họ tên và chữ ký)

TRƯỞNG KHOA ĐIỆN-ĐIỆN TỬ
(Họ tên và chữ ký)


LỜI CẢM ƠN
Trước tiên, tôi xin gửi lời cảm ơn đến q thầy cơ ở bộ mơn Tự Động Hóa
của trường Đại Học Bách Khoa Thành Phố Hồ Chí Minh vì đã truy ền đạt rất
nhiều kiến thức bổ ích trong quá trình học tập cũng như th ực hiện đề tài luận
văn.
Tôi xin chân thành cảm ơn PGS – TS Huỳnh Thái Hồng vì đã nhận lời và tận
tình hướng dẫn tơi trong suốt q trình thực hiện luận văn.Thầy đã có nh ững
góp ý, giúp đở kịp thời trong lúc tơi gặp khó khăn, trở ngại khi thực hiện đề
tài.
Sau cùng, tôi xin gửi lời cảm ơn đến gia đình, bạn bè đã tạo điều kiện cũng
như những lời động viên chân thành để tơi có thể vượt qua được những khó
khăn, hồn thành được đề tài luận văn.
Xin chân thành cảm ơn!

Thành phố Hồ Chí Minh, ngày tháng

Tăng Năng Tường

năm 2014


TĨM TẮT
Trong giới hạn của luận văn này sẽ trình bày việc thiết kế và điều khiển mơ
hình xe hai bánh cân bằng di chuyển theo quỹ đạo đặt trước. Việc thiết kế
gồm: mơ hình hóa, thiết kế phần cứng, xử lý tín hiệu cảm biến, thiết kế bộ
điều khiển trượt (SMC) và bộ điều khiển PD. Trong hệ thống, tín hiệu cảm
biến được lọc nhiễu thơng qua bộ lọc Kalman sau đó sẽ đưa vào bộ điều
khiển. Bộ điều khiển gồm hai thành phần là bộ điều khiển trượt và bộ điều
khiển PD. Bộ điều khiển trượt sẽ điều khiển góc nghiêng của xe cịn bộ điều
khiển PD sẽ điều khiển vị trí xe. Kết quả mơ phỏng sẽ cho thấy đáp ứng của
hệ thống khi robot ở vị trí cân bằng, khơng di chuyển và khi robot di chuyển
đến vị trí đặt trước.

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

ii


ABSTRACT
This thesis presents a method to design and control two-wheel balancing
robot moving to setting position. The thesis focus on modeling, hardware,

processing sensors signal, sliding mode controller design ( SMC) and PD
controller design. Sensors signal are filtered by Kalman filter before using.
Controller has two loop: Sliding mode controller and PD controller. Sliding
mode controller uses to control angle and PD controller uses to control
position of robot. Simulations show result of control when robot maintains at
equilibrium and moving to the set position.

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

iii


LỜI CAM ĐOAN
Tôi xin cam đoan đây là đề tài nghiên cứu do tôi thực hiện. Các số liệu, kết
quả trong đề tài đều trung thực và không bị chỉnh sửa.
Tơi xin hồn tồn chịu trách nhiệm về nghiên cứu của mình.

Thành phố Hồ Chí Minh, ngày tháng

năm 2014

Tăng Năng Tường

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

vi

MỤC LỤC
LỜI CẢM ƠN ........................................................................................................i
TÓM TẮT............................................................................................................ ii
ABSTRACT........................................................................................................ iii
LỜI CAM ĐOAN.................................................................................................vi
MỤC LỤC ............................................................................................................v
DANH MỤC HÌNH ẢNH ...................................................................................vii
DANH MỤC BẢNG BIỂU ...................................................................................x
Chương 1- MỞ ĐẦU.............................................................................................1
1.
2.
3.
4.
5.

Đặt vấn đề...................................................................................................1
Tính cấp thiết và ứng dụng của đề tài..........................................................2
Mục đích nghiên cứu ..................................................................................2
Tổng quan nghiên cứu ................................................................................3
Phương pháp nghiên cứu ............................................................................4

Chương 2- CƠ SỞ LÝ THUYẾT ..........................................................................5
1.
2.
3.
4.
5.
6.

Mơ hình tốn và phương trình tr ạng thái của xe hai bánh............................5
Các phương pháp xử lý tín hiệu cảm biến .................................................10
Cơ sở lý thiết và thiết kế bộ điều khiển trượt.............................................13
Cơ sở lý thiết và thiết kế bộ điều khiển PID ..............................................15
Thiết kế bộ điều khiển dùng trong mô hình ...............................................17
Mạch driver điều khiển moment động cơ ..................................................17

Chương 3- MƠ HÌNH XE HAI BÁNH................................................................19
1.
2.
3.
4.

Cơ cấu cơ khí............................................................................................19
Bộ dẫn động .............................................................................................19
Chọn mạch điều khiển dùng trong mơ hình...............................................20
Chọn cảm biến dùng trong mơ hình ..........................................................22

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

v


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
Chương 4- MƠ PHỎNG DÙNG PHẦN MỀM MATLAB – SIMULINK............27
1. Mơ phỏng mơ hình xe hai bánh.................................................................27
2. Thuật tốn điều khiển ...............................................................................27

3. Đáp ứng của mơ hình ...............................................................................28
a) Đáp ứng của mơ hình với hằng số K và τ đã ch ọn ...............................28
b) Đáp ứng của mơ hình khi thay đổi K và τ............................................31
c) Đáp ứng của mơ hình khi thay đổi Kp và Kd .......................................34
d) Đáp ứng của mơ hình khi có nhiễu ......................................................39
Chương 5- KẾT QUẢ THỰC NGHIỆM VÀ ĐÁNH GIÁ...................................41
1. Những cơng việc đã hồn thành trong đ ề tài .............................................41
a) Thiết kế và chế tạo hoàn chỉnh mơ hình cơ khí ....................................41
b) Lấp đặt nguồn và các mạch cảm biến lên mơ hình ...............................41
c) Thiết kế bộ điều khiển dùng trong mơ hình xe hai bánh.......................43
2. Kết quả thực nghiệm.................................................................................45
a) Kết quả thực nghiệm đo giá trị góc nghiêng tại vị trí cân bằng ............45
b) Kết quả thực nghiệm đo góc nghiêng khi xe lệch góc 8.5 độ ...............46
3. Nhận xét ...................................................................................................46
Chương 6- KẾT LUẬN .......................................................................................47
1. Các kết quả đạt được.................................................................................47
2. Những vấn đề chưa đạt .............................................................................47
3. Hướng phát triển.......................................................................................47
Tài liệu tham khảo...............................................................................................48
Phụ lục ................................................................................................................50

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

vi


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO

DANH MỤC HÌNH ẢNH
Hình 1.1- Cách thức di chuyển của mơ hình xe hai bánh .......................................1
Hình 1.2- Segway ..................................................................................................3
Hình 1.3- Balancing Scooter..................................................................................3
Hình 1.4- nBot.......................................................................................................4
Hình 2.1- Lực phận bố ở bánh xe...........................................................................6
Hình 2.2- Lực phân bố trên mơ hình xe hai bánh ...................................................7
Hình 2.3- Chu trình ư ớc lượng sử dụng bộ lọc bổ phụ thơng tần..........................11
Hình 2.4- Quy trình ước lượng của bộ lọc Kalman gián đoạn ..............................12
Hình 2.5- Bộ điều khiển trượt bám ......................................................................14
Hình 2.6- Sơ đồ khối của bộ điều khiển PID........................................................16
Hình 2.7- Bộ điều khiển vị trí PD ........................................................................16
Hình 2.8- Sơ đồ khối bộ điều khiển .....................................................................17
Hình 2.9- Mạch điều khiển moment động cơ .......................................................18
Hình 2.10- Mạch cầu H dùng để điều khiển động cơ ...........................................18
Hình 3.1- Mơ hình cơ khí của xe hai bánh ...........................................................19
Hình 3.2- Cơ cấu dẫn động dùng trong mơ hình ..................................................20
Hình 3.3- Board vi điều khiển TMS320F28335 ...................................................20
Hình 3.4- Sơ đồ của bộ điều khiển dùng trong mơ hình xe hai bánh ....................21
Hình 3.5- Board cảm biến MPU-6050 .................................................................22
Hình 3.6- Sơ đồ chân và kết nối cần thiết của MPU-6050....................................22
Hình 3.7- Cấu tạo của cảm biến gia tốc góc .........................................................23
Hình 3.8- Trục quay của cảm biến vận tốc góc ....................................................24
Hình 3.9- Cách lấp đặt và xác định góc nghiêng của xe .......................................25

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

vii

XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
Hình 3.10- Cảm biến encoder ..............................................................................26
Hình 3.11- Cấu tạo bên trong của encoder ...........................................................26
Hình 4.1- Sơ đồ khối của mơ hình xe hai bánh từ phương trình trạng thái ...........27
Hình 4.2- Sơ đồ khối của bộ điều khiển trượt ......................................................28
Hình 4.3- Sơ đồ khối của bộ điều khiển vị trí PD.................................................28
Hình 4.4- Xe hai bánh cân bằng tại vị trí 0m với góc lệch ban đầu 5 độ...............29
Hình 4.5- Xe hai bánh cân bằng tại vị trí 0m với góc lệch ban đầu 20 độ.............29
Hình 4.6- Xe hai bánh cân bằng di chuyển đến vị trí 0.5m...................................30
Hình 4.7- Xe hai bánh cân bằng di chuyển đến vị trí 1m......................................30
Hình 4.8- Đáp ứng góc nghiêng của xe khi thay đổi K.........................................31
Hình 4.9- Đáp ứng vị trí của xe khi thay đổi K ....................................................31
Hình 4.10- Moment điều khiển xe khi thay đổi K ................................................32
Hình 4.11- Đáp ứng góc nghiêng của xe khi thay đổi Tau ...................................33
Hình 4.12- Đáp ứng vị trí của xe khi thay đổi Tau ...............................................33
Hình 4.13- Moment điều khiển xe khi thay đổi Tau .............................................34
Hình 4.14- Đáp ứng góc nghiêng của xe khi thay đổi Kp.....................................35
Hình 4.15- Đáp ứng góc vị trí của xe khi thay đổi Kp..........................................35
Hình 4.16- Moment điều khiển xe khi thay đổi Kp ..............................................36
Hình 4.17- Đáp ứng góc nghiêng của xe khi thay đổi Kd.....................................37
Hình 4.18- Đáp ứng góc vị trí của xe khi thay đổi Kd..........................................37
Hình 4.19- Moment điều khiển xe khi thay đổi Kd ..............................................38
Hình 4.20- Xe hai bánh cân bằng tại vị trí 0m khi có nhiễu .................................39
Hình 4.21- Xe hai bánh cân bằng di chuyển đến vị trí 0.5m khi có nhiễu.............40
Hình 5.1- Mơ hình xe hai bánh hồn chỉnh ..........................................................41

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

viii


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
Hình 5.2- Board cảm biến MPU6050...................................................................42
Hình 5.3- Bộ điều khiển dùng trong mơ hình xe hai bánh ....................................42
Hình 5.4- Khối MPU6050 ...................................................................................42
Hình 5.5- Tính qng đư ờng xe di chuyển...........................................................43
Hình 5.6- Khối Encoder.......................................................................................44
Hình 5.7- Khối PWM ..........................................................................................44
Hình 5.8- Khối Data ............................................................................................45
Hình 5.9- Giá trị góc nghiêng tại vị trí cân bằng ..................................................45
Hình 5.10- Giá trị góc nghiêng khi xe lệch 8.5 độ................................................46

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

ix


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO

DANH MỤC BẢNG BIỂU
Bảng 2.1- Ký hiệu các thông số dùng trong mơ hình xe hai bánh (phần 1).............5
Bảng 2.2- Ký hiệu các thơng số dùng trong mơ hình xe hai bánh (phần 2).............6

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

x


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO

Chương 1 - MỞ ĐẦU
1. Đặt vấn đề:
Ngày nay với sự phát triển của khoa học kỹ thuật làm cho robot trở nên phổ biến
trong môi trường công nghiệp cũng như trong đ ời sống hằng ngày, tuy nhiên những
mobile robot được sử dụng hiện nay hầu hết là các robot có 3 hay 4 bánh xe, nhờ
vậy mà xe có thể cân bằng ổn định. Đối với xe có 3 bánh thì có thể di chuyển và
làm việc tốt trong địa hình bằng phẳng nhưng gặp nhiều vấn đề nếu di chuyển trên
các địa hình dốc. Khi di chuyển lên dốc sẽ là trọng lượng xe dồn về phía sau, bánh
lái mắt khả năng bám và trược ngã. Còn khi xuống dốc thì trọng tâm xe dời về phái
trước làm xe lật úp. Đối với xe 4 bánh có khả năng cân bằng tốt nhưng có khuyết
điểm là kích thước to không thể hoạt động trong môi trường hẹp. Ngược lại, xe có
hai bánh được gắn đồng trục lại có thể cân bằng linh hoạt mặc dù bản thân là hệ
thống khơng ổn định. Khi leo dốc thì xe tự động nghiêng về phái trước làm trọng
tâm dồn về phái bánh xe, tương tự khi xuống dốc xe tự nghiêng về sau, giữ trọng
tâm dồn về phái bánh xe nên xe ln cân bằng.

Hình 1.1- Cách thức di chuyển của mơ hình xe hai bánh
Xe hai bánh là một ứng dụng của mơ hình con lắc ngược. Khơng giống với các loại
xe thơng thường có hai bánh nằm trước và sau, xe hai bánh trong đề tài có hai bánh
được đặt song song và đồng trục với nhau, cũng vì lí do này mà xe có kích thư ớc
gọn gàng dễ di chuyển trong những không gian chật hẹp. Xe hai bánh di chuyển
theo quỹ đạo là tiền đề cho việc chế tạo các robot vận chuyển trong các khu vực

chật hẹp, các xe trợ giúp di chuyển cho người già, trẻ em và xa hơn là các robot hai
chân, robot dạng người.

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 1


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
2. Tính cấp thiết và ứng dụng của đề tài:
Mơ hình xe hai bánh cân bằng có thể xem là bước đệm để chế tạo các robot di
chuyển bằng hai chân, một bước phát triển mới trong khoa học công nghệ ngày nay.
Ngồi ra, mơ hình cịn mang lại nhiều ứng dụng phù hợp với nhu cầu thực tế của
cuộc sống hiện đại.

 Ứng dụng của mơ hình xe hai bánh:
- Có thể trở thành xe vận chuyển trong mơi trường công nghiệp.
- Dùng làm xe di chuyển cho người già và trẻ em.
- Kết hợp với những robot khác để làm việc trong môi trường độc hại, những công
việc nguy hiểm thay cho con người.
- Là bước đệm để phát triển robot giống người trong tương lai.

3. Mục đích nghiên cứu:
Hiện nay, vấn đề cân bằng của robot khơng cịn là đ ề tài mới nhưng những ứng
dụng của nó trong mơi trường cơng nghiệp, trong thực tế vẫn cịn rộng và cần được
quan tâm nghiên cứu. Mục tiêu của đề tài là thiết kế và chế tạo mơ hình xe hai bánh
cân bằng di chuyển theo quỹ đạo cho trước trên địa hình bằng phẳng.
Trong giới hạn của đề tài chỉ tập trung vào một số mục tiêu:

- Tìm hiểu nguyên lý cơ bản, nguyên tắc hoạt động của mơ hình xe hai bánh.
- Mơ hình hóa.
- Thiết kế mơ hình cơ khí.
- Mơ phỏng trên MATLAB-SIMULINK.
- Giao tiếp và thu các tín hiệu cảm biến.
- Xây dựng thuật tốn điều khiển.
- Hiệu chỉnh để mơ hình có thể hoạt động trong môi trường thực tế.

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 2


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
4. Tổng quan nghiên cứu:
Xe hai bánh tự cân bằng là một ứng dụng thực tế của mơ hình con lắc ngược. Xe có
khả năng cân bằng tốt trong nhiều địa hình di chuyển khác nhau. Về cấu trúc cơ khí
xe có hai bánh được thiết kế song song, đồng trục với nhau giúp kích thước trở nên
gọn gàng, có thể làm việc trong môi trường chật hẹp. Trên thực tế, xe hai bánh tự
cân bằng có nhiều hình dạng cũng như kích thư ớc khác nhau. Có xe có kích thước
lớn dùng để chở người như Segway (hình 1.2), Balancing Scooter (hình 1.3) và mơ
hình xe trong [3] hay xe có kích thước nhỏ như nBot trong [7] (hình 1.4) và các mơ
hình xe hai bánh trong [4], [5], [6], [8] và [9] dùng trong khơng gian nhỏ hẹp.

Hình 1.2- Segway

Hình 1.3- Balancing Scooter

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 3


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO

Hình 1.4- nBot

Về các bộ điều khiển dùng trong mơ hình xe hai bánh cũng rất đa dạng. Bộ điều
khiển LQR dùng trong [4], [9] được dùng khá rông rãi với khả năng đáp ứng khá tốt
nhưng gặp khó khăn trong việc chọn các giá trị R va Q trong thực nghiệm mơ hình.
Điều khiển mờ [6] cũng đư ợc lựa chọn để giữ cân bằng mơ hình xe hai bánh. Bộ
điều khiển PID trong [8] rất thơng dụng nhưng ít được dùng cho mơ hình phi tuyến.
Ngược lại bộ điều khiển trượt [11] và điều khiển cuốn chiếu [10] cho thấy ưu điểm
khi dùng cho mơ hình phi tuyến.

5. Phương pháp nghiên cứu:
a) Cơ sở lý thuyết:
- Mơ hình hóa xe hai bánh.
- Mô phỏng trên MATLAB - Simulink.
b) Thực hiện mơ hình trong thực tế:
-Thiết kế khung cơ khí.
- Lấp đặt mạch cảm biến và công suất.
- Mạch điều khiển.
- Lập trình và hiệu chỉnh.

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 4


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO

Chương 2 – CƠ SỞ LÝ THUYẾT
1. Mơ hình tốn và phương trình trạng thái của xe hai bánh:
Các tham số được sử dụng trong việc thiết lập phương trình trạng thái của mơ hình
xe hai bánh được mơ tả trong bảng sau:

Ký hiệu

Mơ tả

Đơn vị

θ

Góc nghiêng của xe hai bánh so với trục y

Rad

δ

Góc lệch của xe so với trục x

Rad

MB

Khối lượng thân xe

7.9 Kg

MW

Khối lượng bánh xe

0.36 Kg

R

Bán kính bánh xe

0.05 m

L

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

0.23 m

D

Khoảng cách giữa hai bánh xe

CL, CR

g

Moment của động cơ nối với bánh trái, phải

m
N.m
9.81m/s2

Gia tốc trọng trường

Bảng 2.1 – Ký hiệu các thơng số dùng trong mơ hình xe hai bánh (phần 1)

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 5


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
Ký hiệu

Mơ tả

Đơn vị

xw , y w

Vị trí bánh xe theo phương x, y

m

xB , yB

Vị trí thân xe theo phương x, y

m

HTL , HTR

Lực ma sát giữa bánh trái, phải với mặt đất

N.m

Lực tương tác giữa thân xe và bánh xe

N.m

Phản lực giữa mặt đất với bánh trái, phải

N.m

Phản lực tương tác giữa hai xe với bánh trái, phải

N.m

JW

Moment quán tính của bánh xe

N.m2

JB

Moment quán tính của thân xe

N.m2

FC

Lực tác động

H L , HR
VTL , VTR
V L , VR

N

Bảng 2.2 – Ký hiệu các thơng số dùng trong mơ hình xe hai bánh (phần 2)

Xét tại vị trí bánh xe:

Hình 2.1- Lực phân bố ở bánh xe

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 6

XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
. ̈

=

. ̈

=

. ̈

−

=

. ̈

(2-1a)

−

−

−

.

.

. ̈

(2-2a)

(2-3a)

=

. ̈

=

−

=

−

−

−

(2-1b)
.

(2-2b)
.

(2-3b)

Do ma sát của bánh xe với mặt đường nhỏ nên có thể bỏ qua HTL và HTR.
Đối với thân xe:

Hình 2.2- Lực phân bố trên mơ hình xe hai bánh
. ̈ =

= . sin

+

(2-4)

+

(2-5)

= − . (1 − cos

. ̈ =

. ̈ =(

+

+

−

) (2-6)
.

). . sin

HV: TĂNG NĂNG TƯỜNG

+

−(

=

+

+

−

). . cos

.

+

−(

MSHV:11900723

+

. sin

(2-7)

) (2-8)
Page 7


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
Thay (2-4) và (2-7) vào (2-8), rút rọn ta được:
. ̈ =
. .(
) (2-9)

. sin

+

. cos

)+

. . . sin

− (1 + sin

). (

. ̈ +

− 2.

+

Từ (2-1a) và (2-1b):
.( ̈

Và ̈

)=(

+ ̈

+ ̈

+

= 2. ̈

(2-11)

)−(

) (2-10)

+

Từ (2-3a) và (2-3b):
+

=

(

. ̈

)

. ̈

(2-12)

Thay(2-4), (2-11) và (2-12) vào (2-10) ta được:
2.

. ̈

=−

Và

= .

(2-13)

= .

−

. ̈ +

+

→
→

. ̈

)

. ̈

=−

. ̈

(2-14)
(2-15)

Từ (14) và (15), ta có
=
+ . sin
= − . (1 − cos

(

)

=

+

(2-16)

(2-17)

̇ = ̇
+ . cos . ̇
̇ = − . sin . ̇

(2-18)

̈ = ̈
+ . cos θ . ̈ − . sin . ̇ ( )
̈ = − . sin . ̈ − . cos . ̇ ( )

(2-19)

Thay (2-16) và (2-19a) vào (2-13):
−2.

(2-20)

. ̈

=−

. ̈

Từ (2-19) ta suy ra: ̈ . sin

−

. . cos
− ̈ . cos

Thay (2-16) và (2-21) vào (2-9):

HV: TĂNG NĂNG TƯỜNG

. ̈ +
=− ̈

. . sin
. cos

MSHV:11900723

. ̇ + − 2.
− . ̈

. ̈

(2-21)

Page 8


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
. ̈ =−
(2-22)

=

Thay
2.
.

. . ̈
và

. ̈ =−

=−

. cos
=

−

.

. ̈ +

. . . sin

− (1 + sin

).

ta được hệ phương trình của xe hai bánh:

. ̈−

. . ̈ . cos −

. . cos . ̈ +
. ̈+

.

. . sin . ̇ + − 2.

. . . sin − (1 + sin

. ̈

).

(2-23)

Xem moment qn tính của thân xe như đoạn thẳng hình trụ chiều dài L:
=

1
3

.

(2 − 24)

Xem moment quán tính của bánh xe như đĩa trịn xoay có bán kính R:

=

1
.
2

.

(2 − 25)

Thay (2-24), (2-25) vào (2-23) ta được phương trình:
(2.

.

+

). ̈ = − (

. . ̈ =−

. . cos +

. . cos . ̈ +

Giải hệ phương trình (2-26):
⎧
⎪
⎪
⎪

⎨
2
⎪
⎪
⎪
⎩

3(
4
+

Đặt = ,
bánh:

. ). ̈ +

. sin . ̇ +

. . . sin − (1 + sin

)

(2-26)

. . cos ) cos
3 . sin
3
. . sin . cos
̇ +

−1 ̈ =− .
+
).
).
+
4
4 (2
+
3 1 + sin
cos
+ .
+
.
(2
). .
4
.
+
3 ( . +
3 .(
. +
. . cos ) sin
. . cos ). cos
− .
. ̈ =− .
+
4
4
3 (
. +

. cos )(1 + sin ) 1
+ . sin . ̇ +
.
+
.
4
.
.

+
(2.

= ̇,

=

HV: TĂNG NĂNG TƯỜNG

à

= ̇ ta được phương trình trạng thái của xe hai

MSHV:11900723

Page 9


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
̇ =
( )+ ( ,

̇ =
( )+ ( ,

̇ =

̇ =

Với:

( )=

) + ℎ ( ).

) + ℎ ( ).

−3 sin
.
4
. +
. . cos ). cos
(2.
).
+

3 (
.
4

3 1 + sin
.

4
.
ℎ ( )=
. +
3 (
.
4
(2.

( ,

ℎ ( )=

−3 (
.
4

2.

)=

−1

3
. . sin . cos
.
.( )
4 (2.
).
+

)=
. +
. . cos ). cos
3 (
.
−1
4
(2.
).
+

( ,

( )=

(2 − 27)

+

2.

3 (
.
4

2.

+
+

.

.

cos
(2
). .
+
. . cos ). cos
−1
).
+

+

+

3 (
− .
4

.

. . cos
+

). . sin

. . cos

. . sin . ( )
. +
. . cos
3 (
− .
4

. . cos ). (1 + sin
.
. +
. . cos
3 (
− .
4

+

2. Các phương pháp xử lý tín hiệu cảm biến:

). cos
). cos

)

+

1

). cos

Để có thể điều khiển tốt robot thì các tín hiệu thu được từ cảm biến phải có độ chính
xác cao nhưng trong thực tế tín hiệu thu về có cả nhiễu nên địi hỏi phải có phương
pháp khắc phục hiện tương nhiễu. Có nhiều phương pháp lọc nhiễu như: bộ lọc bổ
phụ thông tần, lọc Kalman,…
- Bộ lọc bổ phụ thông tần [3] và [7]: bộ lọc thường được sử dụng trong việc ước
lượng một giá trị khi có hai nguồn giá trị khác nhau. Một nguồn cho giá trị chính

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 10


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
xác trong vùng tần số thấp, nguồn còn lại cho giá trị chính xác trong vùng tần số
cao.
Trong đề tài nghiên cứu, việc xác định góc nghiêng là tổng hợp giá trị từ hai cảm
biến vận tốc góc gyroscope và cảm biến gia tốc accelerometer. Giá trị của cảm biến
vận tốc góc cho qua bộ lọc tần số cao Gi(s) cịn giá trị của cảm biến gia tốc cho qua
bộ lọc tần số thấp Gg(s), tổng hợp giá trị thu được sẽ cho ra ước lượng chính xác về
góc nghiêng.
Số lượng tham số thiết kế nên nhỏ để dể điều chỉnh và áp dụng cho bộ lọc trong
thực tế.

Hình 2.3- Chu trình ước lượng sử dụng bộ lọc bổ phụ thơng tần
Bộ lọc bổ phụ thơng tần có ưu điểm là dể thiết kế, tính tốn nhanh nhưng có nhược
điểm do vẫn là hai bộ lọc có tần số khác nhau nên tín hiệu ra có thể khơng bằng
nhau.

- Bộ lọc Kalman [12] và [13] được đề xuất bởi giáo sư Kalman từ những năm 1960.
Bộ lọc Kalman hợp nhất tất cả thông tin được cung cấp để cho ra giá trị ước lượng
tối ưu. Bộ lọc sẽ ước lượng trạng thái tiến trình và sẽ thu sự phản hồi của giá trị.

HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 11


XE HAI BÁNH CÂN BẰNG DI CHUYỂN THEO QUỸ ĐẠO
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 thì bộ lọc Kalman sẽ cho ước lượng tối ưu.
 Bộ lọc Kalman gián đoạn:
Đối tượng có mơ hình:
= .

=

+ .
.

+

+

Với wk và vk là giá trị thay đổi ngẫu nhiên, tách biệt nhau.
( )~ (0, )
( )~ (0, )

Ta có ước lượng trạng thái priori tại bước k:
tại bước k:
Sai số ước lượng priori:
Sai số ước lượng posteriori:

=

=

−

−

Từ đó, ta có tương quán sai số ước lượng priori:
Và tương quan sai số ước lượng posteriori:

và ước lượng trạng thái posteriori

= [

= [ .

]

.

]

Bộ lọc Kalman gián đoạn chia thành 2 nhóm là: phương trình cập nhật thời gian và

phương trình cập nhật giá trị đo lường. Ta có mơ hình ước lượng của bộ lọc Kalman
như hình 2.4.

Hình 2.4- Quy trình ư ớc lượng của bộ lọc Kalman gián đoạn
HV: TĂNG NĂNG TƯỜNG

MSHV:11900723

Page 12