Mô hình robot hai bánh tự cân bằng

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 ĐIỆN – ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN

ĐỒ ÁN TỐT NGHIỆP

NGÀNH: CÔNG NGHỆ ĐIỆN TỰ ĐỘNG
ĐỀ TÀI:

MÔ HÌNH ROBOT HAI BÁNH
TỰ CÂN BẰNG
GVHD : TS. NGUYỄN MINH TÂM
Nhóm 3

Tp. Hồ Chí Minh 07/2013


ĐỒ ÁN ĐƯỢC HOÀN THÀNH TẠI
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH

PHẦN A

GIỚI THIỆU
Cán bộ hướng dẫn:

TS. Nguyễn Minh Tâm

Nhóm 3

Cán bộ chấm phản biện:

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

Đồ án tốt nghiệp được bảo vệ tại HỘI ĐỒNG CHẤM BẢO VỆ ĐỒ ÁN TỐT
NGHIỆP TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH.
Đồ án tốt nghiệp

Ngày ... tháng 07 năm 2013

Trang 2


TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT
TP. HỒ CHÍ MINH

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
ĐỘC LẬP - TỰ DO - HẠNH PHÚC

KHOA ĐIỆN - ĐIỆN TỬ
Tp. HCM, ngày … tháng 7 năm 2013

NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP

Chuyên ngành:

Điện tự động

Mã ngành:

18

Hệ đào tạo:

Đại học chính quy

Mã hệ:

1

I. TÊN ĐỀ TÀI: MÔ HÌNH ROBOT HAI BÁNH TỰ CÂN BẰNG
II. NHIỆM VỤ VÀ NỘI DUNG:
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
Đồ án tốt nghiệp

Trang 3


.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
III. NGÀY GIAO NHIỆM VỤ:

10/04/2013

IV. NGÀY HOÀN THÀNH NHIỆM VỤ: 15/7/2013
V. HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN:

TS. Nguyễn Minh Tâm

CÁN BỘ HƯỚNG DẪN

BM. TỰ ĐỘNG ĐIỀU KHIỂN

TS. NGUYỄN MINH TÂM

TS. NGUYỄN MINH TÂM

LỜI CẢM ƠN
˜&™
Trong suốt quá trình thực hiện đề tài, mặc dù gặp phải nhiều khó khăn nhưng
được sự giúp đỡ, hỗ trợ kịp thời từ quý Thầy Cô và các bạn nên Đồ án tốt nghiệp
đã hoàn thành đúng tiến độ. Nhóm thực hiện xin chân thành cảm ơn thầy Nguyễn
Minh Tâm đã tận tình hướng dẫn, chỉ bảo kinh nghiệm quý báu cũng như hỗ trợ
phương tiện thí nghiệm trong suốt quá trình tìm hiểu, nghiên cứu đề tài.
Đồng thời Nhóm thực hiện cũng xin chân thành cảm ơn các Thầy Cô trong
Khoa Điện - Điện Tử đã tạo điều kiện, cung cấp cho nhóm những kiến thức cơ
bản, cần thiết để chúng em có điều kiện và đủ kiến thức để thực hiện quá trình
nghiên cứu.
Bên cạnh đó, nhóm cũng xin cảm ơn các thành viên trong lớp 091180 đã có
những ý kiến đóng góp, bổ sung, cũng như động viên khích lệ giúp nhóm hoàn
thành tốt đề tài.
Ngoài ra, nhóm cũng đã nhận được sự chỉ bảo của các anh (chị) đi trước. Các
anh (chị) cũng đã hướng dẫn và giới thiệu tài liệu tham khảo thêm trong việc thực
hiện nghiên cứu.
Mặc dù nhóm thực hiện đề tài đã cố gắng hoàn thiện được quyển đồ án, nhưng
trong quá trình soạn thảo cũng như kiến thức còn hạn chế nên có thể còn nhiều
Đồ án tốt nghiệp

Trang 4


thiếu sót. Nhóm thực hiện đề tài mong nhận được sự đóng góp ý kiến của quý thầy
cô cùng các bạn sinh viên.
Sau cùng nhóm thực hiện xin chúc Thầy cô sức khoẻ, thành công và tiếp tục đào
tạo những sinh viên giỏi đóng góp cho đất nước. Chúc các anh (chị), các bạn sức
khỏe, học tập thật tốt để không phụ công lao các Thầy Cô đã giảng dạy. Nhóm thực
hiện xin chân thành cảm ơn.

Trân trọng
Nhóm 3

Đồ án tốt nghiệp

Trang 5


NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................

.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
Tp. HCM, ngày … tháng … năm 2013
Giáo viên phản biệnNHẬN

XÉT CỦA GIÁO VIÊN

HƯỚNG DẪN
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
Đồ án tốt nghiệp

Trang 6


.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................
.........................................................................................................................................

Tp. HCM, ngày … tháng … năm 2013
Giáo viên hướng dẫn

TS. Nguyễn Minh Tâm

Đồ án tốt nghiệp

Trang 7


PHẦN B

NỘI DUNG
Danh sách các hình ảnh sử dụng trong đồ án:
Hình 1.1:

Robot dạng 3 bánh xe di chuyển trên địa hình bằng phẳng

Hình 1.2:

Robot dạng 3 bánh xe khi xuống dốc

Hình 1.3:

Robot dạng 3 bánh xe khi lên dốc

Hình 1.4:
Robot 2 bánh di chuyển trên các địa hình khác nhau theo hướng bảo toàn
sự thăng bằng
Hình 1.5:

nBot

Hình 1.6:

JOE

Hình 1.7:

NXTway-GS của LEGO MINDSTORMS

Hình 1.8:

Xe Segway I2, I2 cargo, X2 Adventure

Hình 1.9:

Xe Winglet

Hình 1.10:

Xe Iswing

Hình 2.1:

Mô hình robot 2 bánh tự cân bằng trên mặt phẳng

Hình 2.2:
Simulink

Mô hình phi tuyến của robot hai bánh tự cân bằng trong Matlab

Hình 2.3:

Bên trong khối “Two Wheeled Balancing Robot (Non-Linear Model)”

Hình 2.4:

Bên trong khối “DeCoupling”

Hình 2.5:

Quy trình thực hiện của bộ lọc Kalman

Hình 2.6:

Tổng quan chu trình thực hiện bộ lọc Kalman hoàn chỉnh

Hình 2.7:

Cấu trúc bộ điều khiển PID cho hệ robot hai bánh tự cân bằng

Đồ án tốt nghiệp

Trang 8


Hình 2.8:

Cấu trúc bên trong bộ điều khiển PID rời rạcvới thông số cố định.

Hình 2.9:

Robot hai bánh tự cân bằng sử dụng 3 bộ điều khiển PID cố định.

Hình 2.10:

Đáp ứng ngõ ra vị trí với tín hiệu đặt vị trí là xung vuông

Hình 2.11:

Đáp ứng ngõ ra góc nghiêng với tín hiệu đặt góc nghiêng là xung vuông

Hình 2.12:

Đáp ứng ngõ ra góc xoay với tín hiệu đặt góc xoay là xung vuông

Hình 2.13:

Đáp ứng ngõ ra vị trí với tín hiệu đặt vị trí là sóng sine

Hình 2.14:

Đáp ứng ngõ ra góc nghiêng với tín hiệu đặt góc nghiêng là sóng sine

Hình 2.15:

Đáp ứng ngõ ra góc xoay với tín hiệu đặt góc xoay là sóng sine

Hình 2.16:

đáp ứng ngõ ra vị trí trong trường hợp tín hiệu đặt vị trí là xung vuông

Hình 2.17: đáp ứng ngõ ra góc nghiêng trong trường hợp tín hiệu đặt góc nghiêng là
xung vuông
Hình 2.18:

đáp ứng ngõ ra vị trí trong trường hợp tín hiệu đặt vị trí là xung vuông

Hình 2.19: đáp ứng ngõ ra góc nghiêng trong trường hợp tín hiệu đặt góc nghiêng là
xung vuông
Hình 2.20:

Pin Li-Po 11.1V/2200mA

Hình 2.21:

DSP TMS320F28335

Hình 2.22:

Razor IMU 9DOF

Hình 2.23:

Sơ đồ khối cảm biến gia tốc ADXL345

Hình 2.24:

Phương và chiều các trục gia tốc tương ứng

Hình 2.25:

Đáp ứng giá trị gia tốc ngõ ra tương ứng đối với hướng của trọng lực

Hình 2.26:

Sơ đồ chân ngõ ra trên cảm biến gia tốc

Hình 2.27:

Sơ đồ khối cảm biến ITG3200

Hình 2.28:

Cảm biến HMC5883L

Hình 2.29:

Sơ đồ nguyên lý mạch cầu H

Hình 2.30:

Mạch cầu H thực tế

Hình 2.31:

Encoder

Hình 2.32:

Module Bluetooth HC06 slave

Đồ án tốt nghiệp

Trang 9


Hình 2.33:

Động cơ

Hình 3.1:

Bản vẽ 3D mô hình robot 2 bánh tự cân bằng

Hình 3.2:

Mô hình robot thực tế

Hình 3.3:

Cấu trúc điều khiển phần cứng của robot 2 bánh tự cân bằng

Hình 3.4:

Sơ đồ kết nối phần cứng

Hình 3.5:

Mô hình bộ lọc Kalman với 3 biến trạng thái.

Hình 3.6:

Mô hình bộ lọc Kalman trong Matlab Simulink

Hình 3.7:

Tín hiệu vào-ra của bộ lọc Kalman

Hình 3.8:

So sánh khi cho tín hiệu góc nghiêng ngõ vào và ra của bộ lọc Kalman

Hình 3.9:

Lưu đồ giải thuật cho mô hình robot 2 bánh tự cân bằng

Hình 3.10:

Thư viện Target Support Package cho DSP F28335

Hình 3.11:

Sơ đồ tổng quát

Hình 3.12:

Khối đọc các nút nhấn

Hình 3.13:

Bên trong khối calibre

Hình 3.14:

Khối đọc cảm biến IMU

Hình 3.15:

Khối lọc Kalman

Hình 3.16:

Tổng quát Khối điều khiển trung tâm

Hình 3.17:
điều khiển

Bên trong Khối đọc xung encoder Hình 3.18: Bên trong Khối xuất tín hiệu

Hình 3.19:

Khối gửi dữ liệu

Hình 3.20:

Ngõ ra góc nghiêng và điện áp trên 2 động cơ khi chạy mô hình thực tế

Hình 3.21:

Giao diện của chương trình giao tiếp máy tính

Danh sách các bảng sử dụng trong đồ án :
Bảng 2.1:

Ký hiệu và ý nghĩa của các đại lượng

Bảng 3.1:

Bảng giá trị của các đại lượng trong robot 2 bánh tự cân bằng

Đồ án tốt nghiệp

Trang 10


Bảng 3.2:

Các khối chức năng sử dụng trong chương trình

Mục lục

Đồ án tốt nghiệp

Trang 11


Chương 1.

Tổng quan về đề tài

1.1 Đặt vấn đề
Trong ngành tự động hóa - điều khiển tự động nói chung và điều khiển học nói
riêng, mô hình con lắc ngược là một trong những đối tượng nghiên cứu điển hình và đặc
thù bởi đặc tính động không ổn định của mô hình nên việc điều khiển được đối tượng
này trên thực tế đặt ra như một thử thách.
Kết quả nghiên cứu mô hình con lắc ngược cơ bản, ví dụ như mô hình xe-con lắc,
con lắc ngược quay… có thể ứng dụng và kế thừa sang các mô hình tương tự khác nhưng
có tính ứng dụng thực tiễn hơn, chẳng hạn như mô hình tên lửa, mô hình xe hai bánh tự
cân bằng… do đó khắc phục được những nhược điểm vốn có của các robot hai hoặc ba
bánhkinh điển. Các robot hai hoặc ba bánh kinh điển, theo đó có cấu tạo gồm bánh dẫn
độngvà bánh tự do (hay bất kì cái gì khác) để đỡ trọng lượng robot. Nếu trọnglượng được
đặt nhiều vào bánh lái thì robot sẽ không ổn định và dễ bị ngã, còn nếu đặt vào nhiều
bánh đuôi thì hai bánh chính sẽ mất khả năng bám. Nhiều thiết kế 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 hoặc mặt phẳng nghiêng. Khi di chuyển lên đồi, trọng lượng robot dồn vào đuôi xe
làm mất khả năng bám và trượt ngã.
Robot dạng 3 bánh xe di chuyển trên địa hình bằng phẳng trọng lượng được chia
đều cho bánh lái và bánh dẫn nhỏ.

Hình 1.1 -

Robot dạng 3 bánh xe di chuyển trên địa hình bằng phẳng

Robot dạng 3 bánh xe khi xuống dốc, trọng lực dồn vào bánh sau khiến xe có
thể bị lật úp.

Đồ án tốt nghiệp

Trang 12


Hình 1.2 -

Robot dạng 3 bánh xe khi xuống dốc

Robot dạng 3 bánh xe khi lên dốc, trọng lượng dồn vào bánh trước khiến lực
ma sát giúp xe bám trên mặt đường không được đảm bảo.

Hình 1.3 -

Robot dạng 3 bánh xe khi lên dốc

Ngược lại, các robot 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 phức tạp, mặc dù bản thân robot là một hệ thống không ổn định.
Khi robot di chuyển trên địa hình dốc, nó tự động nghiêng ra trước và giữ cho trọng
lượng dồn về hai bánh chính. Tương tự, khi di chuyển xuống dốc, nó nghiêng ra sau
và giữ trọng tâm rơi vào bánh chính. Vì vậy, không bao giờ có hiện tượng trọng tâm xe
rơi ngoài vùng đỡ bánh xe để có thể gây ra lật úp.

Đồ án tốt nghiệp

Trang 13


Hình 1.4 -

Robot 2 bánh di chuyển trên các địa hình khác nhau theo hướng bảo toàn
sự thăng bằng

1.2 Tình hình nghiên cứu robot 2 bánh tự cân bằng hiện nay:
1.2.1 Các mô hình robot 2 bánh tự cân bằng
1.2.1.1 nBot
Robot nBot do David P.Anderson chế tạo. Nguyên tắc điều khiển nBot như sau:
các bánh xe sẽ chạy theo hướng mà phần trên robot sắp ngã, nếu bánh xe có thể được
lái theo cách giữ vững trọng tâm robot thì robot sẽ được giữ cân bằng.
Quá trình điều khiển sử dụng tín hiệu từ hai cảm biến: cảm biến góc nghiêng
của thân robot so với phương của trọng lực và encoder gắn ở bánh xe để đo vị trí
robot. Tín hiệu này hình thành nên 4 biến: góc nghiên thân robot, vận tốc góc nghiêng,

Đồ án tốt nghiệp

Trang 14


vị trí robot và vận tốc robot; 4 biến này được tính toán thành điện áp điều khiển động
cơ cho robot.

Hình 1.5:

nBot

1.2.1.2 JOE
JOE do phòng thí nghiệm điện tử công nghiệp của viện Công nghệ Liên bang
Lausanne, Thụy Sĩ tạo ra vào năm 2002. Hình dạng của nó gồm hai bánh xe đồng trục,
mỗi bánh gắn với một động cơ DC, robot này có thể chuyển động xoay theo hình chữ
U. Hệ thống điều khiển gồm hai bộ điều khiển “không gian trạng thái” (state space)
tách rời nhau, kiểm soátđộng cơ để giữ cân bằng cho hệ thống. Thông tin trạng thái
được cung cấp bởi hai encoder quang và hai cảm biến: gia tốc góc và con quay hồi

chuyển (gyro). JOE được điều khiển bởi một bộ điều khiển từ xa RC . Bộ điều khiển
trung tâm và xử lý tín hiệu là một board xử lý tín hiệu số (DSP) phát triển bởi chính
nhóm và của viện Federal, kết hợp với FPGA của XILINC.

Hình 1.6:
Đồ án tốt nghiệp

JOE
Trang 15


1.2.1.3 NXTway-GS

Hình 1.7:

NXTway-GS của LEGO MINDSTORMS

1.2.2 Một số sản phẩm thực tế dựa trên mô hình robot 2 bánh tự cân bằng
1.2.2.1 Xe Segway
Segway PT (viết tắt của Segway Personal Transporter - Xe cá nhân Segway),
thường được gọi tắt là Segway là một phương tiện giao thông cá nhân có hai bánh,
hoạt động trên cơ chế tự cân bằng do Dean Kamen phát minh. Loại xe này được sản
xuất bởi công ty Segway Inc. ở bangNew Hampshire, Hoa Kỳ. Từ "Segway" phát âm
gần giống với "segue" (một từ gốc tiếng Ý có nghĩa "di chuyển nhẹ nhàng").Đặc điểm
nổi bật của Segway là cơ chế tự cân bằng nhờ hệ thống máy tính, động cơ và con quay
hồi chuyển đặt bên trong xe, nó giúp cho xe dù chỉ có một trục chuyển động với hai
bánh nhưng luôn ở trạng thái cân bằng, người sử dụng chỉ việc ngả về đằng trước hoặc
đằng sau để điều khiển xe đi tiến hoặc đi lùi.

Đồ án tốt nghiệp

Trang 16


Hình 1.8:

Xe Segway I2, I2 cargo, X2 Adventure

1.2.2.2 Xe Winglet
Xe Winglet do hãng Toyota phát triển được giới thiệu năm 2008 có kích thước
phần đế chỉ bằng tờ giấy A3. Xe dựa trên mô hình 2 bánh tự cân bằng và có cảm biến
để nhận biết cử động của người điều khiển từ đó đưa ra lệnh để xe hoạt động theo ý
muốn của người điều khiển.

Hình 1.9:

Xe Winglet

1.2.2.3 Iswing Toyota:
I-swing được mệnh danh là một trong những ý tưởng táo bạo nhất của ngành
công nghiệp ôtô trong thời gian gần đây. Xuất hiện lần đầu tiên tại triển lãm Tokyo
2005, I-swing là biểu tượng cho phương tiện cá nhân trong tương lai bởi nó khác xa so
với ôtô thông thường. Sự di chuyển, điều khiển, giao tiếp giữa người và xe đều mang
Đồ án tốt nghiệp

Trang 17


tính nhân bản. Chẳng hạn như khi đặt ghế ở chế độ "thân xe mở tối thiểu", tầm mắt tài
xế sẽ ngang với người đang đứng nên rất dễ trò chuyện và tạo cảm giác thân thiện. Iswing được điều khiển thông qua tay nắm và các nút, giống với cách con người sử

dụng các thiết bị điện tử hàng ngày.

Hình 1.10:

Xe Iswing

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 2 bánh tự cân bằng dựa trên nền
tảng lý thuyết mô hình con lắc ngược. Trong thời gian làm đề tài, những mục tiêu của
đề tài được đặt ra như sau:
− Tìm hiểu các mô hình xe, robot 2 bánh tự cân bằng và các nguyên lý cơ bản về
cân bằng.
− Tính toán các thông số động lực học, xây dựng các hàm không gian-trạng thái
(state-space) của mô hình.
− Tìm hiểu, lựa chọn các loại cảm biến và bộ điều khiển trung tâm. Trong đề tài
này sẽ sử dụng cảm biến IMU 9 DOF và bộ điều khiển DSP F28335.
− Mô phỏng mô hình trên Matlab Simulink.
− Tìm hiểu và áp dụng Bộ lọc Kalman để lọc nhiễu cho cảm biến, xây dựng các
thuật toán bù trừ để có giá trị góc chính xác.
− Xây dựng thuật toán điều khiển động cơ, giữ thăng bằng cho robot.

Đồ án tốt nghiệp

Trang 18


1.4 Giới hạn đề tài
Trong khuôn khổ của đề tài này, mô hình robot 2 bánh tự cân bằng chỉ đứng hay
chạy tiến lùi mà chưa thể xoay vòng hay đi lên dốc được.
Phương pháp điều khiển: chỉ sử dụng phương pháp điều khiển PID với thông số

cố định, chưa ứng dụng phương pháp mạng neuron thích nghi, phương pháp mạng
toàn phương tuyến tính (LQR)

1.5 Phương pháp nghiên cứu
Xây dựng mô hình lý thuyết robot 2 bánh tự cân bằng và mô phỏng mô hình trên
Matlab Simulink.
Xây dựng mô hình thực:
− Thiết kế cơ khí: khung sườn của mô hình.
− Ứng dụng các mạch điện tử, mạch công suất, cảm biến, vi xử lý.
Mạch cảm biến và bù trừ giá trị cảm biến.

Đồ án tốt nghiệp

Trang 19


Chương 2.

Các cơ sở lý thuyết

2.1 Đặc tính động lực học
2.1.1 Mô hình hóa robot 2 bánh tự cân bằng trên địa hình phẳng
Xây dựng hệ phương trình trạng thái mô tả hệ thống robot hai bánh tự cân bằng.

Hình 2.1:

Đồ án tốt nghiệp

Mô hình robot 2 bánh tự cân bằng trên mặt phẳng

Trang 20


Trong đề tài này sẽ sử dụng các kí hiệu, đơn vị như sau:
Kí hiệu

Đơn vị

Ý nghĩa

M

kg

Khối lượng của bánh xe

M

kg

Khối lượng của robot

R

m

Bán kính bánh xe

W

m

Chiều rộng của robot

D

m

Chiều ngang của robot

H

m

Chiều cao của robot

L

m

Khoảng cách từ trọng tâm robot đến trục bánh xe
Hệ số ma sát giữa bánh xe và mặt phẳng di chuyển
Hệ số ma sát giữa robot và động cơ DC
Moment quán tính của động cơ DC

ohm

Điện trở động cơ DC

Kb

V sec/rad

Hệ số EMF của động cơ DC

Kt

Nm/A

Moment xoắn của động cơ DC

N

Tỉ số giảm tốc

G

Gia tốc trọng trường

,

rad

Góc trung bình của bánh trái và phải

rad

Góc của bánh trái và phải

rad

Góc nghiêng của phần thân robot

rad

Góc xoay của robot

m

Tọa độ bánh trái

m

Tọa độ bánh phải

m

Tọa độ trung bình

Nm

Moment phát động theo các phương khác nhau

Nm

Moment phát động của động cơ bánh trái, phải

A

Dòng điện động cơ bánh trái, phải

V
Bảng 2.1:

Điện áp động cơ bánh trái, phải
Ký hiệu và ý nghĩa của các đại lượng

Sử dụng phương pháp Euler-Lagrange để xây dựng mô hình động học. Giả sử
tại thời điểm t = 0, robot di chuyển theo chiều dương trục x, ta có các phương trình
sau:
Đồ án tốt nghiệp

Trang 21


Góc tịnh tiến trung bình của hai bánh xe và góc xoay của robot được xác định như sau:
[2.1]
Trong đó tọa độ trung bình của Robot trong hệ qui chiếu:
[2.2]
Và

[2.3]

Tọa độ bánh trái trong hệ qui chiếu :
[2.4]
Tọa độ bánh phải trong hệ qui chiếu :
[2.5]
Tọa độ tâm đối xứng giữa hai động cơ trong hệ qui chiếu :
[2.6]
Phương trình động năng của chuyển động tịnh tiến:

[2.7]
Phương trình động năng của chuyển động quay :
[2.8]
Với
[2.9]
là động năng quay của phần ứng động cơ trái và phải.
Phương trình thế năng:
[2.10]
Phương trình Lagrange :
[2.11]
[2.12]
[2.13]
[2.14]
Lấy đạo hàm L theo các biến ta được:
[2.15]
Đồ án tốt nghiệp

Trang 22


[2.16]
[2.17]
Momen động lực do động cơ DC sinh ra:
[2.18]
Và:
[2.19]
[2.20]
[2.21]
Sử dụng phương pháp PWM để điều khiển động cơ nên chuyển từ dòng điện sang điện
áp động cơ:

[2.22]
Xem điện cảm phần ứng tương đối nhỏ (gần bằng 0), có thể bỏ qua, suy ra:
[2.23]
Từ đó, các moment lực sinh ra:
[2.24]
[2.25]
Với và

[2.26]
[2.27]

Thu được phương trình động lực học mô tả chuyển động của robot như sau:\
[2.28]
[2.29]
[2.30]

Đồ án tốt nghiệp

Trang 23


2.1.2 Mô hình hóa robot hai bánh tự cân bằng trên địa hình phẳng trong
Matlab Simulink

Hình 2.2 :

Mô hình phi tuyến của robot hai bánh tự cân bằng trong Matlab
Simulink

Hình 2.3:

Đồ án tốt nghiệp

Bên trong khối “Two Wheeled Balancing Robot (Non-Linear Model)”

Trang 24


Phương trình động lực học của robot như trên thể hiện mối quan hệ giữa giá trị
điện áp điều khiển hai động cơ;với độ nghiêng, vị trí, vận tốc của hệ robot, giá trị điện
áp hai động cơ tác động lên các thông số đó dưới dạng tổng còn với góc xoay, giá trị
điện áp hai động cơ tác động lên thông số này dưới dạng hiệu . Khi đó, tách bài toàn
hệ robot thành hai bài toán nhỏ hơn với hai tín hiệu điều khiển
[2.31]
Khối thực hiện chức năng này gọi là khối phân tách (decoupling)

Hình 2.4:

Bên trong khối “DeCoupling”

2.2 Bộ lọc Kalman
2.2.1 Giới thiệu về bộ lọc Kalman
Vào năm 1960, R.E.Kalman cho xuất bản nghiên cứu của mình, đưa ra giải pháp
đệ qui để rời rạc hóa dữ liệu trong bộ lọc tuyến tính. Kể từ đó, việc giải quyết các bài
toán kĩ thuật số, một lĩnh vực rất rộng lớn, đã trở nên dễ dàng hơn rất nhiều. Bộ lọc
Kalman được mở rộng ra nghiên cứu và ứng dụng, đặc biệt là trong lĩnh vực tự động hay
hỗ trợ việc tự định vị.

Đồ án tốt nghiệp

Trang 25