BỘ GIÁO DỤC VÀ ĐÀO TẠO

TRƯỜNG ĐẠI HỌC LẠC HỒNG

***

LA KHẢI KHẢI

THIẾT KẾ VÀ THI CÔNG THUẬT TOÁN ĐIỀU KHIỂN 3 BÁNH XE ĐA

HƯỚNG TRÊN ROBOT TỰ HÀNH

LUẬN VĂN THẠC SĨ KỸ THUẬT ĐIỆN

Đông Nai, Năm 2019

 BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC LẠC HỒNG
***

LA KHẢI KHẢI

THIẾT KẾ VÀ THI CÔNG THUẬT TOÁN ĐIỀU KHIỂN 3 BÁNH XE ĐA

HƯỚNG TRÊN ROBOT TỰ HÀNH

Chuyên ngành: Kỹ thuật điện

Mã ngành: 60.52.02.02

LUẬN VĂN THẠC SĨ KỸ THUẬT ĐIỆN

NGƯỜI HƯỚNG DẪN KHOA HỌC

TS. LÊ PHƯƠNG TRƯỜNG

Đông Nai, Năm 2019

 LỜI CẢM ƠN

Qua khóa học và quá trình hoàn thành luận văn, tôi đã lĩnh hội được nhiều kiến
thức, nhận được nhiều sự quan tâm giúp đỡ từ các Cấp lãnh đạo, của các quý thầy cô
cùng bạn bè đồng nghiệp.
Bên cạnh đó tôi đã thu thập và phát triển thêm nhiều kiến thức bổ ích cho công
viện hiện tại cũng như sự nghiệp trong tương lai, gặp gỡ được những người Thầy
uyên bác và tâm huyết cùng những người bạn đáng quý.
Xin chân thành cảm ơn Ban lãnh đạo nhà trường, Khoa sau đại học, Khoa Cơ
Điện – Điện Tử trường Đại Học Lạc Hồng và quý Thầy Cô tham gia giảng dạy lớp
cao học Ngành Kỹ thuật điện khóa 1, tạo điều kiện và hỗ trợ tôi trong thời gian học
tập và nghiên cứu.
Đặc biệt, xin gởi lời cảm ơn sâu sắc đến Thầy TS. Lê Phương Trường đã tận
tình hướng dẫn, hỗ trợ tôi trong quá trình nghiên cứu và hoàn thành luận văn này.
Trong suốt quá trình làm luận văn tôi đã nổ lực hết sức nhưng chắc chắn không
tránh khỏi những thiếu sót, rất mong nhận được những ý kiến đóng góp xây dựng của
quý Thầy Cô, các anh chị và bạn bè đồng nghiệp.

Biên Hòa, tháng 06 năm 2019

Tác giả

La Khải Khải
 LỜI CAM ĐOAN
Tôi xin cam đoan công trình nghiên cứu này là của tôi. Các số liệu, kết quả nêu
ra trong luận văn này là trung thực và chính xác. Thuật toán, mô hình mô phỏng và
thực nghiệm trong luận văn là do chính tác giả nghiên cứu.
Học viên thực hiện

La Khải Khải
 TÓM TẮT

Đề tài đưa ra phương pháp Thiết kế và thi công thuật toán điều khiển 3 bánh xe
đa hướng trên robot tự hành. Hệ thống bao gồm động cơ Planet DC 60W cho 3 bánh,
3 bộ encoder phản hồi tốc độ, một bộ encoder phản hồi khoảng cách di chuyển, một
cảm biến la bàn kỹ thuật số đo góc quay. Hệ thống phần cứng bao gồm một hệ thống
chính để điều khiển hướng di chuyển và góc robot thông qua việc điều khiển ba bánh
xe Omni dựa trên tín hiệu nhận được từ cảm biến la bàn và bộ encoder đo khoảng
cách và sau đó nó điều khiển ba bộ driver để điều chỉnh tốc độ và chiều từng bánh
xe. Mỗi driver chỉ điều khiển một động cơ DC cho mỗi bánh xe Omni với thuật toán
PID tích hợp. Hệ thống chính được phát triển dựa trên STM32F407 và hệ thống phụ
là vi điều khiển STM32F103. Trong đó, thuật toán định hướng được phát triển để
điều khiển ba bánh xe Omni và thuật toán PID được áp dụng để điều khiển động cơ
tốc độ động cơ DC cho mỗi bánh xe. Từ kết quả thu được, hệ thống có các ưu điểm
sau (1) tự động điều chỉnh góc và vị trí; (2) bỏ cảm biến dò vạch cho robot tự hành;
(3) giảm chi phí hiệu quả; (4) độ chính xác cao.
 MỤC LỤC
LỜI CẢM ƠN ................................................................................................................ 3
LỜI CAM ĐOAN .......................................................................................................... 4
MỤC LỤC ...................................................................................................................... 6
DANH MỤC HÌNH ...................................................................................................... 7
CHƯƠNG 1: TỒNG QUAN ........................................................................................ 35
1.1 Đặt vấn đề........................................................................................................... 35
1.2 Các nghiên cứu về bánh xe đa hướng omni khác ............................................. 35
1.3 Các nội dung nghiên cứu ................................................................................... 38
1.4 Mục tiêu của đề tài ............................................................................................. 38
CHƯƠNG 2: THIẾT KẾ PHẦN CỨNG MÔ HÌNH ROBOT BA BÁNH OMNI
........................................................................................................................................ 40
2.1 Thiết kế cơ khí hệ thống thực nghiệm ................................................................. 41
2.1.1 Bánh xe đa hướng (Omni wheel) .................... Error! Bookmark not defined.
2.1.2 Thiết kế cơ khí ............................................................................................... 43
2.2 Thiết kế hệ thống điện điều khiển ....................................................................... 46
2.2.1 Mạch điều khiển chính – STM32F407 MCU ............................................... 46
2.2.2 Mạch điều khiển PID .................................................................................... 51
2.2.3 La bàn ............................................................................................................ 54
2.2.4 Encoder ......................................................................................................... 52
CHƯƠNG 3 : XÂY DỰNG MÔ HÌNH THUẬT TOÁN ĐIỀU KHIỂN BA
BÁNH XE ĐA HƯỚNG ............................................................................................. 54
3.1 Xây dựng phương trình chuyển động. .............................................................. 55
3.2 Xây dựng phương trình vận tốc đầu vào cho mỗi bánh xe ................................ 56
3.3 Phương trình góc quay của robot tự hành .......................................................... 57
3.4. Phương trình PID ............................................................................................... 57
3.5 Sơ đồ thuật toán .................................................................................................. 57
CHƯƠNG 4: KẾT QUẢ VÀ THẢO LUẬN ........................................................... 59
CHƯƠNG 5: KẾT LUẬN ......................................................................................... 63
TÀI LIỆU THAM KHẢO ........................................................................................... 1
 DANH MỤC HÌNH
Hình 1. Bánh xe đa hướng Mecanum ............................................................................... 35
Hình 2. Bánh xe đa hướng Omni ..................................................................................... 35
 DANH MỤC BẢNG
Bảng 1. So sánh với những thuật toán khác trong điều khiển Omni và Mecanum ............. 36
Bảng 2. Phương trình chuyển động 3 và 4 bánh Omni ..................................................... 37
 CHƯƠNG 1: TỒNG QUAN
1.1 Đặt vấn đề
Cuộc thi Sáng tạo robot Châu Á Thái Bình Dương (The ABU Asia-Pacific
Robot Contest) là một thách thức khoa học, nơi để áp dụng nghiên cứu tiên tiến để
tạo ra và phát triển Robot với nhiệm vụ định sẵn trong các lĩnh vực như tự động hóa,
điện tử, xử lý hình ảnh và hệ thống cơ điện tử. Tốc độ của robot tự động kết hợp với
độ chính xác cao là rất quan trọng trong cuộc thi này. Hơn hết, bánh xe của robot tự
trị là Mecanum hoặc Omni, là giải pháp tốt nhất, đơn giản nhất và linh hoạt nhất cho
việc di chuyển đa hướng của robot trong cuộc thi. Cấu trúc của bánh xe Omni và bánh
mecanum được thể hiện trong Hình 1.

Hình 1. Bánh xe đa hướng Mecanum Hình 2. Bánh xe đa hướng Omni

Trong trường hợp này, các bánh xe Omni được dùng để tạo ra hệ thống chuyển
động. Mỗi bánh xe Omni được 1 động cơ DC điều khiển vì vậy mỗi bánh xe Omni
sẽ hoạt động một cách độc lập với những bánh còn lại, do đó có khả năng điều khiển
đồng thời và độc lập đối với chuyển động quay và tịnh tiến của chúng. Những ưu
điểm đó giúp cho những hệ thống chuyển động bằng các bánh xe đa hướng có ưu thế
hơn những loại bánh xe khác như: có khả năng vừa di chuyển vừa xoay, phần đế dễ
thiết kế và đơn giản hơn, chỉ cần 3 bánh xe và 3 động cơ, cùng một quảng đường
nhưng sẽ tiêu thụ ít năng lượng hơn. Tất cả những điều đó thúc đẩy việc nghiêng cứu
thuật toán điều khiển 3 bánh xe Omni là tất yếu.
1.2 Các nghiên cứu về bánh xe đa hướng omni khác
 Trong những năm gần đây, một số tác giả đã nghiên cứu tập trung vào Mecanum
và bánh xe omni như trong bảng 1.

Bảng 1. So sánh với những thuật toán khác trong điều khiển Omni và Mecanum

Loại bánh Mục tiêu Tài liệu tham

Thuật toán Phần cứng Số bánh
xe điều khiển khảo

PSO based On-board Hsuan-Ming Feng

Omni Directional 3
Fuzzy-PI computer [1]

MRL Robo Cup Ehsan Hashemi

Fuzzy-PI Omni Directional 4
SSL [2]

Side-slip
angle
Omni None Directional 4 Boyuan Li [3]
estimation
method

Model
Predictive
Control and PIC32 family of
Omni Directional 3
Microcontroller Jessivaldo Santos
filtered
[4]
Smith
predictor

Barrel-
two-wheel-
shaped Arduino Tatsuro
Omni drive omni- 3
friction- Mega2560 Terakawa[5]
directional
driven

closed-loop
control and
Mitsubishi
dead-
Mecanum M16C/62 Directional 4 J.ACooney [6]
reckoning
microcontroller
for
navigation

Kinematic
Mecanum none Directional 4 Shuai Guo [7]
equation
 Spiral Nucleo-F411
None Directional 4 ]Yamada N, [8]
Mecanum RE

PID, STM32F407
Directional,
directional Omni Microcontrolle 3 Proposed system
position
– algorithm rs

Ngoài ra, bánh xe Mecanum và Omni đã được điều khiển dựa trên phương trình
chuyển động. Một số tác giả đã được chứng minh tính chính xác của phương trình
chuyển động để điều khiển hướng Omni. Bảng 2 cho thấy phương trình chuyển động
với bánh xe 3 và 4 Omni.

Bảng 2. Phương trình chuyển động 3 và 4 bánh Omni

Motion equation Descriptions References

Type
 u (t ) is the continuous
x(t )  Ax(t )  B(u (t )  K sgn( x(t )) (1)
control input applied with
y(t )  Cx(t ) (2) the zero order.
u (t )  u1 (t ) u2 (t ) u3 (t )
T
(3) Bv , Bvn are viscous friction
 3 j 2 K t2 
 2
B
 v 0 0  (4) relative to v and vn .
 2MRa r M 
 3 j 2 K t2 B  B , Bvn viscous friction
A   0  vn 0 


2MRa r 2 M  relative to  and vn .
 3b 2 j 2 K Bw 
0 0 
 JRa r 2 I n  Cv , Cvn , C Coulomb
friction relative to v , vn and
 cos( ) cos( ) 
3 0    Jessivaldo
 M M  (5) I
wheels iK  1 sin( ) sin( )  n is robot’s inertial Santos [4]
B t 
Ra M M M  momentum;  is angle; M
 
b b b 
 I n In I n 
is robot’s mass; b is
robot’s radius .
 Cv 
r is wheels radius; l is
 0 0  reduction; Ra is armature
 M 
 Cn  (6) resistances; k t . torque
K  o  0  ,C  I
M constants.
 
 o 0  w
C
 I n 
 (q1 , q2 ) is stands for the

q  k ( q )v
where location of the center of the

k (q)  (jT (q) j (q)) 1 j (q) vehicle; q3 is the angle

 s 1
'
c 1
'
d
  made between the line
s '
c '
d
j (q)   2 1
 s '
c '
d
4

3 1
 joining the robot center Mirosław
  s c d 
' '
4 1
wheels with the first wheel center Galicki [9]
ci'  cos(q3  (i  1) / 2;
and the horizontal line;
si'  sin(q3  (i  1) / 2.
d is distance from the
q  (q1 , q2 , q3 )  R T 3

center of the robot to each

of the wheel center.

Trong nghiên cứu này ứng dụng thuật toán ……

1.3 Các nội dung nghiên cứu
Đề tài “Thiết kế và thi công thuật toán điều khiển 3 bánh xe đa hướng trên robot
tự hành”, mục đích chính là xây dựng thuật toán điều khiển 3 bánh xe omni chuyển
động chính xác theo góc, hướng và khoảng cách di chuyển của robot sử dụng 3 bánh
xe omni trên 1 mặt phẳng. Ở đây, thuật toán PID được áp dụng để kiểm soát tốc độ
của từng bánh xe thông qua vi điều khiển ARM STM32F103, còn thuật toán quản lý
góc, hướng và khoảng cách được điều khiển bởi vi điều khiển ARM STM32F407.
Phần còn lại của bài viết này được trình bày như sau.
 Chương 1: Tổng quan, các công trình nghiên cứu và bài báo khoa học có liên
quan.
 Chương 2: Mô tả hệ thống.
 Chương 3: Xây dựng mô hình thuật toán điều khiển 3 bánh xe đa hướng.
 Chương 4: Sơ đồ thuật toán và phần cứng.
 Chương 5: Kết quả và thảo luận.
1.4 Mục tiêu của đề tài
 - Xây dựng và thiết kế robot sử dụng bộ truyền động là 3 bánh xe omni, kết hợp
cảm biến góc, cảm biến đo khoảng cách di chuyển làm nền tảng xây thuật toán cho
robot tự hành.
- Điều khiển chính xác góc, hướng và khoảng cách di chuyển của Robot.
- Xây dựng thuật toán…. dựa trên vi điều khiển ARM STM32F407VET6.
- Mục tiêu cuối cùng là xây dựng hoàn chỉnh tất cả thuật toán điều khiển thành
1 hàm điều khiển duy nhất nhằm hỗ trợ tốt nhất cho người lập trình điều khiển robot
tự hành sử dụng 3 bánh xe Omni.
 CHƯƠNG 2

THIẾT KẾ PHẦN CỨNG MÔ HÌNH ROBOT BA BÁNH OMNI

2.1 Mô tả hệ thống
Di chuyển từ vị trí đầu đến vị trí cuối một cách chính xác và nhanh là mục tiêu
hàng đầu cho các tất cả robot hay những thiết bị tương tự làm việc trong môi trường
đồi hỏi phải di chuyển. Đề tài xây dựng mô hình di chuyển đa hướng với 3 bánh xe
đa hướng được điều khiển bởi bộ điều khiển trung tâm sử dụng STM32F407VET6
thông qua các cảm biến hồi tiếp về như IMU và Encoder. Bộ điều khiển trung thu
thập các thông số từ cảm biến sau đó so sánh với thông số cài đặt ban đầu bằng thuật
toán điều khiển mà đề tài xây dựng, sau khi tính toán xong sẽ truyền tín hiệu điều
khiển tới 3 động cơ để điều khiển 3 bánh xe đa hướng làm cho robot di chuyển đúng
theo hướng và quảng đường di chuyển mà chúng ta mong muốn.
Ở Hình 1 cho thấy sơ đồ nguyên lý của ba hệ thống đa hướng Omni bao gồm
hệ thống cơ học, mạch điều khiển chính dựa trên MCU STM32F407, 3 hệ thống mạch
điều khiển PID được điều khiển bởi MCU STM32F103 kết hợp nền tảng lập trình
C#. Hệ thống ba bộ encoder dùng để phản hồi tốc độ của từng bánh xe trong các điều
kiện hoạt động thực tế để việc điều khiển PID thông qua mạch điều khiển PID MCU
STM32F103. Bên cạnh đó, một cảm biến la bàn được chế tạo để đo chuyển động góc
của hệ thống và 1 bộ encoder phản hồi dữ liệu khoảng cách đường đi về MCU
STM32F407 dựa trên giao thức được cấu hình trong C#.
Mạch điều khiển chính (MCU STM32F407) với thuật toán điều khiển góc,
hướng di chuyển, tốc độ và khoảng cách. Từ đó thuật toán sẽ tính toán tốc độ và góc
của mỗi bánh xe dựa trên phản hồi dữ liệu của cảm biến la bàn và hệ thống encosder
đo khoảng cách. Hơn nữa, hệ thống mạch điều khiển phụ với thuật toán PID tích hợp
dựa trên MCU STM32F103 đảm bảo tốc độ điều khiển mỗi bánh xe chính xác như
tính toán của mạnh điều khiển chính (MCU STM32F407).
 STM32F407MCU
Main system

Compass sensor

Digital PID
signal control
Digital Control angle and distance
signal algorithm
Encorder Distance
Wheel
STM32F103 MCU
8h
Sub system

Digital PID Digital PID

signal control signal control

Encorder 0h Wheel Wheel

0h 4h
STM32F103 MCU STM32F103 MCU
Sub system 1 Sub system 2

Hình 1. Sơ đồ của hệ thống 3 bánh đa hướng Omni

2.2 Thiết kế cơ khí

Bánh xe, cái mà đã đóng góp cho sự phát triển kinh tế, chính trị cho xã hội loài
người ngay từ lúc vừa được phát minh. Bánh xe đa hướng là một thuật ngữ khá lạ lúc
mới phát minh. Sự ra đời của bánh xe đa hướng đã mở ra một kỷ nguyên mới, thoát
khỏi hoàn toàn tư duy và cách di chuyển của những thiết bị, máy móc sử dụng bánh
xe thông thường, loại bánh xe chỉ di chuyển tiến và lùi.
Vào năm 1919, bánh xe đa hướng đầu tiên được cấp bằng sáng chế bởi
J.Grabowiecki tại Hoa Kỳ. Hình XXX cho thấy một hình ảnh từ bằng sáng chế về
một bánh xe của J.Grabowiecki. Bánh xe bao gồm một bánh xe chính được tạo thành
bởi những con lăn ngang có trục xoay tiếp tuyến với tâm bánh xe chính, mõi con lăn
ngang điều có khả năng quay độc lập quanh trục của mình. Với thiết kế của
J.Grabowiecki đã tạo ra một bánh xe phức tạp và khác hoàn toàn so với những bánh
xe thông thường, một bánh xe có thể di chuyển theo nhiều hướng hơn.
Hình. XX. Bản vẽ bánh xe đa hướng, như mô tả trong trong bằng sáng chế của
Grabowiecki năm 1919 tại Hoa Kỳ. ( Em thêm tài liệu tha khảo)

Khác với loại bánh xe truyền thống, những thiết bị sử dụng bánh đa hướng có
những ưu điểm vượt trội như: khả năng thay đổi vị trí và định hướng linh hoạt, độ
chính xác cao..., bởi vì chúng có khả năng dịch chuyển và quay đồng thời hoặc độc
lập, vì vậy thiết bị di động đa hướng đã thu hút được nhiều sự chú ý hơn. Thiết bị sử
dụng bánh đa hướng có thể di chuyển theo bất kỳ hướng nào, ở bất kỳ góc nào mà
không cần quay trước khi di chuyển. Vì có khả năng như vậy nên thiết bị sử dụng
bánh đa hướng có khả năng tiết kiệm năng lượng so với những thiết bị sử dụng bánh
xe thông thường. Sau đây là một số mẫu bánh đa hướng đã được sử dụng:
 Hình. XX. Một số bánh xe đa hướng được sử dụng

2.1.2 Thiết kế khung sườn

a. Lựa chọn vật liệu:
Đưa ra lý do chọn INox
Vật liệu Đặc tính kỹ thuật
Sắt Khối lương rieng
Inox
Nhôm

Đề tài xây dựng thuật toán điều khiển bánh xe đa hướng dựa trên một robot
được gắn trên nó 3 bánh xe đa hướng. Để thực hiện khả năng di chuyển như mục tiêu
của đề tài, robot phải có 3 bậc tự do trong mặt phẳng di chuyển (mp xoy), gồm: di
chuyển độc lập trong mặt phẳng oxy và xoay quanh trục z. Do đó, cần tối thiểu 3
bánh xe đa hướng với các động cơ điều khiển độc lập, vì mỗi động cơ cho khả năng
điều khiển một bậc tự do. Các bánh xe được gắn lên robot và lệch nhau một góc 120
độ như hình x.x.

Hình x.x Mô hình 3 bánh xe omni được thiết kế lệch nhau 120 độ
Đề tài sử dụng 3 bánh xe omi đôi kết cấu bằng khung nhôm có đường kính là
127mm với số bánh vệ tinh là 11 và được làm bằng cao su mềm (Hình x.x)
(a) (b)

Hình x.x Hình bánh xe omni 3D trên solidwork (a) Hình bánh xe omni thực tế (b)
Các bánh xe được truyền động bằng động cơ planet 24VDC-60W có hộp số
nhông hành tinh với tỷ số truyền là 1:19.2 với tốc độ động cơ trước hộp số là 9000
vòng/phút và có encoder hồi tiếp 500 xung/vòng (Hình x.x).
 Hình x.x Động cơ palnet
Toàn bộ khung robot được làm từ thép không gỉ, với kích thước tổng thể 550mm
X 550mm X 170mm. Hình ảnh thiết kế robot trên phân mềm solidworks và hình ảnh
thực tế

Hình x.x Hình 3D robot được thiết kế trên phần mềm solidworks
 Omni Wheel
direction 4h

Encoder for distance

feedback

Compass sensor
Omni Wheel
directional 8h

Planet DC motor
Planet DC motor
And encoder
And encoder

Main-interface
circuitry

Omni Wheel Sub-interface

directional 0h circuitry

Planet DC motor
Bluetooth HC05
And encoder

Hình x.x Hình ảnh thực tế Robot 3 bánh Omni

2.2 Thiết kế hệ thống điện điều khiển
2.2.1 Mạch điều khiển chính – STM32F407 MCU
Hệ thống điện điều khiển được vận hành dựa trên nền tảng bộ xử lý trung tâm
là MCU ARM STM32F407VET6. Dòng ARM Cortex STM32F4 là một bộ xử lí thế
hệ mới đưa ra một kiến trúc chuẩn cho nhu cầu đa dạng về công nghệ. Không giống
như các chip ARM khác, dòng Cortex là một lõi xử lí hoàn thiện, đưa ra một chuẩn
CPU và kiến trúc hệ thống chung. Dòng Cortex STM32F4 gồm có 3 phân nhánh
chính: dòng A dành cho các ứng dụng cao cấp, dòng R dành cho các ứng dụng thời
gian thực như các đầu đọc còn dòng M dành cho các ứng dụng vi điều khiển và chi
phí thấp. STM32F4 đuợc thiết kế dựa trên dòng Cortex-M4, dòng Cortex-M4 đuợc
thiết kế với tiêu thụ năng luợng thấp trên nền kiến trúc mới. Kiến trúc cơ bản của
SMT32 - ARM Cortex-M4 bao gồm hệ thống ngắt (interrupt system), SysTick timer
(đuợc thiết kế cho hệ điều hành thời gian thực), hệ thống kiểm lỗi (debug system) và
memory map. Không gian địa chỉ 4Gbyte của Cortex-M4 đuợc chia thành các vùng
cho mã chương trình, SRAM, ngoại vi và ngoại vi hệ thống và có nhiều bus cho phép
thực hiện các thao tác song song với nhau, do đó làm tăng hiệu suất của chip. Không
giống với các kiến trúc ARM truớc đó, dòng Cortex cho phép truy cập dữ liệu không
xếp hàng (unaligned data, vì chip ARM là kiến trúc 32bit, do đó tất cả các dữ liệu
hoặc mã chuong trình đều đuợc sắp sếp khít với vùng bộ nhớ là bội số của 4byte).
Ðặc điểm này cho phép sử dụng hiệu quả SRAM nội. Dòng Cortex còn hỗ trợ việc
đặt và xoá các bit bên trong hai vùng 1Mbyte của bộ nhớ bằng phuong pháp gọi là
bit banding. Ðặc điểm này cho phép truy cập hiệu quả tới các thanh ghi ngoại vi và
các cờ đuợc dùng trên bộ nhớ SRAM mà không cần một bộ xử lí luận lí (Boolean
processor). Đó là một số thông tin cơ bản về dòng MCU STM32F4 do nhà sản xuất
cung cấp. Bảng sau thể hiện rõ hơn những chức năng mà MCU có thể làm được theo
datasheet nhà sản xuất.
(Em nên viêt tiếng việt một số thông số chính)
Bang3xxx. Thông số kỹ thuật của ARMF407 [??]
Hình x.x Thông tin cơ bản STM32F4-100-LQFP
 Hình x.x Sơ đồ khối STM32F40xxx

Dựa trên cơ sơ STM32F4 tác giả đã thiết kế mạch chủ với phù hợp với nhu cầu
sử dụng của đề tài cũng như ứng dụng vào việc làm mạch chủ cho những robot hoặc
hệ thống điều khiển tương tự. Mạch được thiết kế trên phần mềm Altium Desiner
dưới đây là hình ảnh 3D và ảnh thược tế cua mạch chủ.
Hình x.x Sơ đồ mạch nguyên lý mạch chủ STM32F4 (Em nên tách từng khối ví dụ
khối nguồn, khối cảm biến…..)

Hình x.x Hình 3D mạch chủ STM32F4

 Hình x.x Hình thực tế mạch chủ STM32F4

2.2.2 Mạch điều khiển PID

Mạch điều khiển PID là một phần không thể thiếu và rất quan trọng trong việc
điều khiển chính xác. Mạch hoạt động dựa trên thuật toán PID được lập trình sẳn trên
MCU STM32F103C8T6 thông qua 2 ic lái IR2184 điều khiển 4 MOSFET công suất
kênh N là IRFS3207 với ID=180A. Mạch được phát triển nhằm phục vụ điều khiển
chính xác các động cơ DC servo có thông số sử dụng như sau :
 Điện áp ngõ vào: 15-30VDC
 Dòng điện ngõ ra max: 110A
 Ngõ vào Encoder: 5V, 100-1000 xung (ppr)
 Tốc độ động cơ max: 3000 vòng/phút (rpm)
 Thuật toán điều khiển: PID, cầu H và vị trí
 Sử dụng vi điều khiển ARM STM32FF103C8T6
 Tín hiệu điều khiển: PWM và UART
 Có phần mềm thiết lập thông số cho driver thông qua giao tiếp với máy tình
bằng chuẩn UART.
 Hình x.x Mạch 3D driver PID (a), Mạch driver PID thực tế (b)

Mạch driver được tác giả sử dụng để quản lý tốc độ động cơ. Mạch chủ có nhiệm
vụ truyền vận tốc xuống từng mạch driver tương ứng, các driver chỉ quản lý tốc độ
động cơ theo yêu cầu chứ không phản hồi về mạch điều khiển trung tâm.
2.2. 1Hệ thống cảm biến
2.2.4 Encoder
Đề tài sử dụng encoder 500 xung trên 1 vòng và được gắn vào 1 bánh xe omni
có đường kính 45mm. Bánh omni gắn trên encoder được gắn lên 1 cơ cấu trượt có
trục thẳng đứng vuông góc với mặt phẳng chạy để đảng báo lúc robot di chuyển bánh
xe omni encoder sẽ luôn được tiếp xúc với mặt sàn đảm bảo việc đo khoản cách di
chuyển là chính xác nhất. Do robot 3 bánh di chuyển đa hướng nên tác giả phải chọn
bánh xe encoder đo đường đi là bánh xe đa hướng và trên 1 robot sẽ có 2 encoder đo
đường theo 2 phương x và y trên mặt phẳng di chuyển.
Thông số kỹ thuật Giá trị
 Hộp trượt

Omni đường
kính 45mm

Encoder
5V-500 xung
2.2.3 La bàn
Trong đề tài này, tác giả sử dụng IMU để đọc giá trị góc quay của robot. Đây là
cơ sở chủ đạo để thuật toán điều khiển 3 bánh xe dự vào để tính toán tốc độ từng bánh
xe khi robot di chuyển. IMU được thiết kế dự trên cảm biến MPU6050 và được 1
MCU lấy thông số từng trục sau đó dùng thuật toán lọc tín hiệu từng trục rồi truyền
về mạch chủ theo giao thức UART. Mạch chủ chỉ cần lấy dữ liệu từ IMU về sau đó
xác định xem robot đi đúng góc hay không. Ở đây, đề tài chỉ sử dung thông số của 1
trục Z chứ không dùng thông số của trục X và Y.

Hình x.x

CHƯƠNG 3 : XÂY DỰNG MÔ HÌNH THUẬT TOÁN ĐIỀU KHIỂN BA

BÁNH XE ĐA HƯỚNG

Khác với loại robot tự hành sử dụng bánh truyền thống, robot tự hành sử
dụng 3 bánh đa hướng của tác giả có những ưu điểm vượt trội như: khả năng
thay đổi vị trí và định hướng linh hoạt, độ chính xác cao..., bởi vì chúng có khả
năng dịch chuyển và quay đồng thời hoặc độc lập. Trong kỹ thuật điều khiển
chuyển động của robot đa hướng vấn đề bám quỹ đạo và tác động nhanh là rất
cần thiết. Robot sử dụng 3 bánh omni có thể di chuyển theo bất kỳ hướng nào,
ở bất kỳ góc nào mà không cần quay trước khi di chuyển. Vì có khả năng như
vậy nên robot omni có khả năng tiết kiệm năng lượng so với robot sử dụng
bánh xe thông thường. Để thực hiện khả năng di chuyển đó,robot omni phải có
3 bậc tự do trong mặt phẳng di chuyển (mp xoy), gồm: di chuyển độc lập theo
các trục x, y và xoay quanh trục z.

Hình x.x: Khả năng linh hoạt của robot 3 bánh omni.

Do đó cần tối thiểu 3 bánh omni với các động cơ điều khiển độc lập, vì
mỗi động cơ cho khả năng điều khiển một bậc tự do. Ở đây tác giả chọn giải
pháp là 3 bánh xe omni được liên kết với nhau theo 1 hình tam giác cân có 3
bánh xe dc gắn ở 3 đỉnh tam giác cân đó. Robot gồm ba bánh xe đa hướng cách
đều góc 120°. Ba bánh xe đa hướng có cùng bán kính r và được đều động bởi
các động cơ DC. Tâm dịch chuyển của OMR đặt tại C và giả định trùng với
tâm hình học. L là khoảng cách từ tâm bánh xe đến điểm C . OXY là hệ tọa độ
tham chiếu toàn cục, CX 0Y0 là hệ tọa độ tham chiếu cục bộ, gắn liền với OMR.

3.1 Xây dựng phương trình chuyển động.

Các trình điều khiển đa hướng đã được xây dựng một cách sáng tạo nhưng dựa
theo định luật chuyển động Newton [10] và phương trình chuyển động [11] như sau:

.. .
A z  B z  C ( ) v(t) (1)

Trong đó z là vector trạng thái còn v là vận tốc điều khiển được viết lại như sau:
 x  v1 
   
z   y  v   v2 
, , và (2)
  v 
   3
m   3 
 0 0   2 0 0 
   
3
A0 0 , B 0
 0 
m
(3)
    2 
   
0 h   0 3 L 
0 0
  L    

     
  sin   sin     sin     
 3  3  
    
C   cos   cos      cos      (4)
 3  3 
 1 1 1 
 
 

Trong đó x, y và  là các chuyển vị tuyến tính và góc, m là khối lượng của

robot. vi là tín hiệu vận tốc đầu vào cho động cơ dẫn động bánh xe thứ i. Các hằng số
đặc trưng của động cơ được ký hiệu là  và  , còn L là khoảng cách.
3.2 Xây dựng phương trình vận tốc đầu vào cho mỗi bánh xe
Một trong những thông số quan trọng để phát triển thuật toán điều khiển 3 bánh
xe đa hướng là tín hiệu điện áp đầu vào cho động cơ DC đang điều khiển bánh xe
tương ứng, nó có thể được cung cấp bởi:
𝜋
𝑉1 = 𝜔𝑑 sin (𝜃𝑑 + ) + 𝜔𝜃 (5)
3
𝜋
𝑉2 = 𝜔𝑑 cos (𝜃𝑑 + ) − 𝜔𝜃 (6)
3
𝜋
𝑉3 = 𝜔𝑑 cos (𝜃𝑑 + ) + 𝜔𝜃 (7)
3
Trong đó V1, V2, V3 là tín vận tốc đầu vào của động cơ bánh đa hướng 1, bánh
đa hướng 2 và bánh đa hướng 3, 𝜔𝑑 là vận tốc góc của bánh xe, 𝜔𝜃 là góc quay của
robot, 𝜃𝑑 là góc của robot. Trong trường hợp này, giá trị của 𝜔𝑑 và 𝜔𝜃 được viết như
sau:
𝜔𝑑 = [ −1, 1] (8)

và
 𝜔𝜃 = [-1, 1] (9)

3.3 Phương trình góc quay của robot tự hành

Cho rằng, lúc đầu robot tự hành có góc (  ), hướng góc (  ) và góc phản hồi từ
cảm biến la bàn (  cs ). Tuy nhiên, trong khi robot di chuyển góc của robot tự hành có

góc lệch là (  ). Các góc lệch có thể được viết là:

 =  cs -  (10)

Xét trường hợp 1: Giá trị của  = 0 robot tự hành tiếp tục di chuyển.
Xét trường hợp 2: Giá trị của   0 hệ thống cần điều khiển góc xoay theo
phương trình dưới đây:
a     (11)

Trong đó  a là góc hiệu chỉnh.

3.4. Phương trình PID
Bộ điều khiển vi tích phân tỷ lệ (PID) là một phản hồi vòng điều khiển. Bộ điều
khiển PID liên tục tính toán giá trị lỗi e(t) dựa trên sự khác biệt giữa điểm đặt mong
muốn và biến quy trình đo và áp dụng hiệu chỉnh dựa trên thuật ngữ tỷ lệ, tích phân
và đạo hàm. Chức năng của bộ điều khiển PID có thể được viết là:
t de(t )
u (t )  k p e(t )  ki  e(t ' )dt '  kd (12)
0 dt
Ở đây k p , ki , k p là các hệ số cho tỷ lệ, tích phân và đạo hàm.

3.5 Sơ đồ thuật toán

Thuật toán của 3 bánh xe đa hướng được phát triển như Hình 2.x. Lúc mới bắt
đầu, góc và tốc độ của robot tự trị đã được thiết lập. Hệ thống sẽ được tính toán tốc
độ đầu vào cho mỗi bánh xe dựa trên phương trình (5), (6), (7) và sau đó kiểm tra tốc
độ hiện tại nếu sai tốc độ cho mỗi bánh xe sẽ tăng hoặc giảm cho đến khi thiết lập tốc
độ bằng tốc độ hiện tại. Hơn nữa, tầm quan trọng của điều khiển đa hướng là góc
cạnh của một robot tự hành. Trong trường hợp này, nếu góc hiện tại của robot tự hành
không bằng góc thiết lập, hệ thống sẽ tính toán góc xoay dựa trên các phương trình
này (10), (11).
 Start

Set-up angular,
speed
Reset compass
sensor

Calculate voltage inputs for 3

DC motors of 3 wheels

False
Current speed = speed Re-calculate Calculate the
setup speed rotation angular

False

True

Current angular = angular

set up

True
Keep current speed

End

Hình 2. Sơ đồ thuật toán của ba bánh xe đa hướng []

 CHƯƠNG 4: KẾT QUẢ VÀ THẢO LUẬN

4.1 thiết đặt thực nghiệm

4.2 Phân tích kết quả
Như hình 3 được hiển thị góc điều chỉnh và vị trí của robot di tự hành. Mong
muốn thực hiện robot tự hành với định hướng là 30 độ và 2000 xung phản hồi từ bộ
encoder để di chuyển về phía trước và chạy ngược về phía sau của robot tự hành theo
chiều giảm giá trị xung từ encoder từ 2000rpm về 0. Từ kết quả, robot tự hành cần 2
giây để điều chỉnh góc và 6 giây để chuyển từ 0 sang vị trí phải đến 2000rpm (phản
hồi từ bộ encoder) và lùi lại, robot di động cũng di chuyển từ 2000rpm sang 0 cùng
một lúc. Hơn nữa, Hình 4 được hiển thị thông số điều chỉnh tốc độ lái của từng bánh
xe. Hướng bánh xe 0h có nhiệm vụ điều chỉnh hướng của robot tự hành nên tốc độ
của bánh xe này thấp hơn so với hướng bánh xe 4h và 8h. Bên cạnh đó, trong kết quả
của hình 4 là hiển thị thời gian robot tự giảm tốc độ từ tối đa xuống 0 của mỗi bánh
xe là 0,5 giây.

4000 40
Desired position for Adjust angle
forward directional Adjust postion
The positional feedback from encoder

2000 30

Target postion

Angle (degree)
0 20

Desired position for

-2000 backward directional 10
Backward directional

-4000 0
0 2 4 6 8 10 12
Time (s)

Hình 3. Điều chỉnh góc lái và vị trí của robot tự hành

 60
Caculated
Wheel 8h Measured
40

20
Wheel 4h
Speed of each wheel (Rpm)

Wheel 4h
0

-20
Wheel 0h
-40
Wheel 0h
-60 Wheel 8h

-80

-100
0 2 4 6 8 10 12
Time (s) `
Hình 4. Điều chỉnh tốc độ lái của từng bánh xe đa hướng

4.2 Phân tích độ chính xác

Để dễ dàng đánh giá độ chính xác của hệ thống được đề xuất, sự khác biệt giữa
kết quả đo và tính toán tốc độ đầu ra bánh xe đa hướng của robot tự hành và góc được
xác định là:

e  xi  xˆi (13)

Trong đó xi và x̂i là các giá trị thứ i của phép đo và tính toán, n là tổng số mẫu
lấy được. Kết quả phân tích chênh lệch tốc độ đầu ra đa hướng cho mỗi bánh xe được
mô tả trong hình 5.
 6
Wheel 0h
5
Wheel 4h
4 Wheel 8h

3
Difirence of speed (rpm)

-1

-2

-3

-4
0 2 4 6 8 10 12
Time (s)

Hình 5. Phân tích sự khác biệt giữa kết quả đo và tính toán

Khoản cách khác nhau giữa tốc độ đầu ra của bánh xe đa hướng của từng tốc độ
được liệt kê trong Bảng 3. Hơn nữa, lỗi bình phương trung bình gốc (RMSE) tương
ứng với tham chiếu đến mô phỏng được đưa ra tương ứng bởi Chouder và cộng sự,
[12]
n

 x  xˆ i i
2

 RMSE  i 1
(14)
n
Bảng 3. Phân tích sự khác biệt hệ thống đề xuất
e(rpm)  RMSE (rpm)

Wheel 0h -3 5 1.66

Wheel 4h -3 1 0.59

Wheel 8h -3.52 3 1.40

 Mặt khác, bảng 3 cũng cho thấy kết quả RMSE của tốc độ đầu ra đa hướng lần
lượt là 1,66rpm, 0,59rpm và 1,40rpm cho bánh xe 0h, 4h và 8h. Dựa trên độ tin cậy
của thuật toán cho các thiết bị đa hướng, hệ thống được đề xuất được xác nhận để có
đủ độ chính xác và độ tin cậy trong khả năng điều khiển.
 CHƯƠNG 5: KẾT LUẬN
Đề tài “Thiết kế và thi công thuật toán điều khiển 3 bánh xe đa hướng trên robot
tự hành”
Để tài này trình bày cách tự phát triển một thuật toán định hướng để điều khiển
ba bánh xe đa hướng của hệ thống robot di động tự hành bao gồm hệ thống encoder
và cảm biến la bàn dựa trên STM32F407 là bộ điều khiển trung tâm và STMF103
quản lý chương trình PID của động cơ. Từ quan điểm thực tế về việc tự phát triển
một thuật toán để điều khiển ba bánh xe đa hướng của robot di động tự hành cho hệ
thống robot di động tự động có những ưu điểm (1) để tự động điều chỉnh góc và vị
trí; (2) để xóa cảm biến cho dòng theo dõi của robot ô tô; (3) chi phí hiệu quả; (4) độ
chính xác cao. Nói cách khác, một ý nghĩa thực tế quan trọng khác là hiệu quả của bộ
điều khiển cấp thấp dẫn đến việc đơn giản hóa bộ điều khiển cấp cao.
 TÀI LIỆU THAM KHẢO
[1] Hsuan-Ming Feng, Chih-Yung Chen, Ji-Hwei Horng, “Intelligent omni-
directional vision-based mobile robot fuzzy systems design and implementation”,
Expert Systems with Applications 37 (2010) 4009–4019.
[2] Ehsan Hashemia, Maani Ghaffari Jadidi, Navid Ghaffari Jadidi, “Model-based
PI–fuzzy control of four-wheeled omni-directional mobile robots”, Robotics and
Autonomous Systems 59 (2011) 930–942.
[3] Boyuan Li, Haiping Du, Weihua Li , Yongjun Zhang, “Side-slip angle estimation
based lateral dynamics control for omni-directional vehicles with optimal steering
angle and traction/brake torque distribution”, Mechatronics xxx (2014) xxx–xxx.
[4] Jessivaldo Santos _ Andr´e G. S. Concei¸c˜ao _ Tito L. M. Santos, “ Trajectory
tracking of Omni-directional Mobile Robots via Predictive Control plus a Filtered
Smith Predictor”, IFAC Papers OnLine 50-1 (2017) 10250–10255.
[5] Tatsuro Terakawa, Masaharu Komori, Yuuta Yamaguchi, Yugo Nishida. “Active
omni wheel possessing seamless periphery and omnidirectional vehicle using it”,
Precision Engineering, (2019).
[6] J. A. Cooney, W. L Xu, G Bright, “Visual dead-reckoning for motion control of
a Mecanum-wheeled mobile robot”, Mechatronics Volume 14, Issue 6, July 2004,
Pages 623-637.
[7] Guo S, Jin Y, Bao S, Xi FF. “Accuracy analysis of omnidirectional mobile
manipulator with mecanum wheels” Advances in Manufacturing,
2016;4(4):363−70.
[8] Yamada N, Komura H, Endo G, Nabae H, Suzumori K. Spiral mecanum wheel
achieving omnidirectional locomotion in step-climbing. Proceedings of the IEEE
international conference on advanced intelligent mechatronics 2017:1285–90.
[9] Mirosław Galicki, “Collision-free control of an omni-directional vehicle”,
Robotics and Autonomous Systems 57 (2009) 889–900.
[10] Kalmár T, Nagy, D’Andrea R, Ganguly P. “Near-optimal dynamic trajectory
generation and control of an omnidirectional vehicle”. RobotAuton Syst, 2004;
46:47–64.
[11] TamásKalmár-Nagy. “Real-time trajectory generationforomni-
directionalvehicles by constrained dynamic inversion” Mechatronics 000 (2016)
1–10.
[12] Chouder, A., Silvestre, S., Sadaoui. N., Rahmani, “Modeling and simulation of
a grid connected PV system based on the evaluation of main PV module
parameters”. Simul. Modell. Pract. Theory 20 (2012), 46–58.