Tính toán, phân tích, mô phỏng động học và động lực học robot ShrimpII

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
--------------------------------------Họ và tên tác giả luận văn
NGUYỄN VĂN DƯƠNG

TÊN ĐỀ TÀI LUẬN VĂN
TÍNH TOÁN, PHÂN TÍCH, MÔ PHỎNG ĐỘNG HỌC
VÀ ĐỘNG LỰC HỌC ROBOT SHRIMPIII
Chuyên ngành : Công Nghệ Chế Tạo Máy

LUẬN VĂN THẠC SĨ KHOA HỌC
CÔNG NGHỆ CHẾ TẠO MÁY

NGƯỜI HƯỚNG DẪN KHOA HỌC
TS. NGUYỄN TRỌNG DOANH

Hà Nội – Năm 2011


GVHD: TS.Nguyễn Trọng Doanh 

Luận văn tốt nghiệp

LỜI CAM ĐOAN
Tôi xin cam đoan nội dung trong luận văn là do tôi tự nghiên cứu, tìm hiểu. Những
tài liệu được trích dẫn trong luận văn đều có ghi chú rõ nguồn gốc, tác giả.

Nguyễn Văn Dương

Học viên: Nguyễn Văn Dương

Trang 1


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ
Hình 1.1: Robot tự hành SmatROD
Hình 1.3: Robot thám hiểm của trung tâm vũ trụ NASA
Hình 1.4: Robot tự hành 8 bánh Lunokhod.
Hình 1.5: Robot Lauron II từ đại học Karlsruhe
Hình 1.6: Caterpillar robot ROBHAZ_DT3
Hình 1.7: Robot tự hành ShrimpIII của công ty Bluebotics
Hình 1.8: Robot người P2 của hãng Honda (Nhật Bản)
Hình 1.9: Robot Dog Aibo của hãng Sony (Nhật Bản)
Hình 1.10: Phương pháp cân bằng trọng tâm robot bằng dịch đối trọng theo đường
thẳng
Hình 1.11: Phương pháp thay đổi trọng tâm bằng hệ thống thanh nhún song song
Hình 1.12: Phương pháp điều khiển 3 bánh
Hình 1.13: Phương pháp điều khiển cả ba bánh cả dẫn động và tác dụng lái
Hình 1.14: Khung có 2 bánh rẽ phía trước. 2 bánh sau chủ động và được kết nối
qua khớp giúp robot luôn ở trạng thái cân bằng
Hình 1.15: Khâu cơ sở liên kết với các khâu động giúp cơ cấu trở nên linh hoạt khi
di chuyển
Hinh 1.16: Hai phần được kết nối qua khớp trụ đứng giúp robot rẽ dễ dàng
Hình 1.17: Kết cấu 5 bánh với 4 bánh dẫn động và 1 bánh lái
Hình 1.18: Kết cấu robot tự hành 6 bánh 2 hàng song song linh hoạt
Hình 1.19: Kết cấu 6 bánh với 2 bánh rẽ phía trước
Hình 1.20: Kết cấu 6 bánh với độ linh hoạt đặc biệt

Hình 1.21: Kết cấu 6 bánh đặc biệt nhất ( cả 6 bánh đều dẫn động và bánh trước và
bánh sau là 2 bánh điều khiển rẽ và quay vòng.
Hình 1.22: Kết cấu robot 8 bánh linh hoạt
Hình 1.23: Kết cấu 8 bánh với bộ đôi giá chuyển hướng
Hình 1.24: Kết cấu 8 bánh với 2 phần liên kết bằng khớp trụ
Hình 1.25: Kết cấu 8 bánh, hai phần liên kết bằng khớp cầu

Học viên: Nguyễn Văn Dương

Trang 2


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 2.1: Các hệ tọa độ của robot
Hình 2.2: Sự dịch chuyển của B tại vị trí t và t+1
Hình 2.3: Sự dịch chuyển của giá chuyển hướng.
Hình 2.4: 3D-Odometry và các biến
Hình 2.5: Quỹ đạo của tâm trọng lực khi leo cầu thang 17cm.
Hình 3.1: Phân bố nội lực và ngoại lực tác dụng lên trục mỗi đông cơ dẫn động
bánh
Hình 3.2: Cơ cấu hình bình hành kết nối bộ bánh bên thân robot ShrimpIII
Hình 3.3: Đặc tính cơ học ( khớp thấp) của cơ cấu
Hình 3.4: Khả năng linh hoạt vượt địa hình của cặp bánh bên thân robot ShrimpIII
Hình 3.5: So sánh tâm quay giữa 2 cơ cấu
Hình 3.6: Kết cấu chân trước robot ShrimpIII
Hình 3.7: Nguyên lý dịch chuyển của chân trước robot ShirmpIII
Hình 3.8: Thông số kỹ thuật của chân trước robot ShrimpIII

Hình 3.9: Chân trước với các thông số kích thước chiều dài.
Hình 3.10: Biểu đồ mô phỏng quỹ đạo chuyển động của tâm bánh trước Tb(x,y) khi
thay đổi thông số chiều dài khâu e và khâu d
Hình 3.11: Kết cấu chân sau (chân cố định với thân robot)
Hình 3.12: Sơ đồ tính độ nâng lên của bánh trước khi leo qua vật cản
Hình 3.13: Kết cấu hình bình hành ở trạng thái tĩnh trên nền phẳng
Hình 3.14: Kết cấu chân (hbh) ở trạng thái vượt vật cản.
Hình 3.15: Robot ShrimpIII di chuyển trên nền phẳng
Hình 3.16: Quy đổi lực tác dụng về tính trên mỗi hệ động cơ – bánh dẫn.
Hình 3.17: Đáp ứng tốc độ quay (n), dòng phần ứng (iu) khi có bước nhảy điện áp
(uu)
Hình 3.18: Bánh chân trước ShrimpIII tiếp xúc và leo hết bậc cầu thang
Hình 3.19: Bánh chân trước nằm vượt qua bậc thang thứ 1
Hình 3.20: Bánh 21 và 22 vượt mặt đứng bậc thang
Hình 3.21: Trạng thái 3 bánh trước vượt hết bậc trongkhi 3 bánh sau chưa vượt

Học viên: Nguyễn Văn Dương

Trang 3


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 3.22: Trạng thái cặp bánh 31 và 32 vượt bậc thang
Hình 3.23: Trạng thái bánh cuối (bánh 4) vượt thành đứng bậc thang
Hình 3.24: Robot ShrimpIII vượt vật cản lệch (1 cặp bánh bên thân vượt vật cản)
Hình 3.25: Trạng thái khi quay vòng hoặc chuyển hướng của robot ShrimpIII
Hình 3.26: Trạng thái robot ShrimpIII xuỗng bậc cao thang

Hình 3.27: Robot ShrimpIII dừng hoạt động khi vật cản chạm động cơ
Hình 3.28: Mô hình động học robot ShrimpIII di chuyển từ điểm P tới điểm G
Hình 3.29: Bản vẽ Thông số kích thước chân trước ShrimpIII
Hình 3.30: Toàn bộ phần chân trước ShrimpIII mô phỏng theo chế tạo
Hình 3.31: Động cơ lái bánh trước và bánh sau
Hình 3.32: Biện pháp kỹ thuật lắp ghép động cơ với càng lái và bánh dẫn
Hình 3.33: Phương pháp kẹp chặt trục động cơ và càng lái
Hình 3.34: Bản vẽ kích thước thiết kế và chế tạo chân sau ShrimpIII
Hình 3.35: Hình ảnh mô phỏng chân sau robot ShrimpIII
Hình 3.36: Bản vẽ kích thước toàn bộ kết cấu hình bình hành (chân bên)
Hình 3.37: Kết cấu bánh với các khâu liên kết kiểu hình bình hành
Hình 3.38: Bản vẽ kích thước phần thân robot ShrimpIII
Hình 3.39: Phần thân Robot ShrimpIII đã gắn ắc quy và camera quan sát
Hình 3.40: Toàn cảnh robot ShrimpIII.

Học viên: Nguyễn Văn Dương

Trang 4


GVHD: TS.Nguyễn Trọng Doanh 

Luận văn tốt nghiệp

LỜI NÓI ĐẦU
Khi nhắc tới “Robot tự hành” ta biết rằng đây là một lĩnh vực công nghệ cao
có ứng dụng rộng lớn và ngày càng đóng vai trò quan trọng trong sự phát triển của
xã hội khi con người ngày càng muốn tránh tiếp xúc với môi trường, tác nhân gây
hại cho cơ thể như làm việc trong các phòng phản ứng hạt nhân, phòng phân tích
hoá học độc hại, dò tìm bom mìn, chữa cháy tự động v.v… Robot tự hành không

còn là lĩnh vực mới mẻ đối với các nước có nền khoa học kỹ thuật phát triển. Nhưng
đối với Việt Nam đây vẫn còn là một lĩnh vực còn rất mới cả về kiến thức lẫn công
nghệ. Để có thể giải được các bài toán đặt ra trong di chuyển tự hành của robot là
một vấn đề rất khó khăn đòi hỏi người thiết kế phải có kiến thức về lý thuyết lẫn
thực nghiệm. Trên thế giới hiện nay đã có nhiều mẫu robot tự hành được thiết kế,
chế tạo và đưa vào thử nghiệm thực trong những nhiệm vụ đặc biệt. Mỗi loại robot
tự hành lại có một ưu điểm và tính năng riêng nhưng những robot tự hành có khả
năng vượt được vật cản có thành thẳng đứng thì số lượng là không nhiều. Và trong
số ít đó mẫu robot tự hành Shrimp của hãng Bluebotics Thụy Sỹ có những tính năng
đặc biệt và ấn tượng nhất.
Robot ShrimpIII có khả năng ứng dụng trong nhiều lĩnh vực như: Thăm dò
mìn, khảo sát địa hình nguy hiểm. Làm việc trong phòng thí nghiệm hoá học hay
phản ứng hạt nhân. Khi ShrimpIII đủ độ cững vững và ổn định thì nó còn có một
thể mang tải để thực hiện nhiệm vụ chữa cháy..,
Được sự đồng ý của bộ môn Công Nghệ Chế Tạo Máy. Sự hướng dẫn và chỉ
đạo tận tình của TS. Nguyễn Trọng Doanh. Tôi đã có nhiều thuận lợi khi thực hiện
đề tài “TÍNH TOÁN, PHÂN TÍCH, MÔ PHỎNG ĐỘNG HỌC VÀ ĐỘNG
LỰC HỌC ROBOT SHRIMPIII”

Học viên: Nguyễn Văn Dương

Trang 5


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

MỤC LỤC
LỜI CAM ĐOAN ........................................................................................................ 1 

DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ .................................................................... 2 
LỜI NÓI ĐẦU ............................................................................................................. 5 
CHƯƠNG 1: TỔNG QUAN VỀ RÔBOT TỰ HÀNH............................................. 8 
1.1  Một số mẫu robot tự hành nổi tiếng trên thế giới ................................................. 8 
1.2  Một số kết cấu bánh dẫn động và điều khiển đặc biệt của robot tự hành.......... 13 
CHƯƠNG 2: TÍNH TOÁN ĐỘNG HỌC RÔBOT SHRIMPIII......................... 22 
2.1  Giới thiệu phương pháp 3D-Odometry............................................................... 22 
2.2  Phương pháp 3D-Odometry................................................................................ 22 
2.2.1 Sự dịch chuyển của giá chuyển hướng. .............................................................. 23 
2.2.2 Sự dịch chuyển 3D.............................................................................................. 26 
2.3  Kết luận............................................................................................................... 29 
CHƯƠNG 3: MÔ PHỎNG ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC ROBOT TỰ
HÀNH SHRIMPIII ................................................................................................... 30 
3.1  Thiết lập phương trình truyền động giữa động cơ và robot................................ 30 
3.2  Phân tích các thành phần trong kết cấu của robot tự hành ShrimpIII................. 32 
3.2.1 Cơ cấu hình bình hành (chân giữa).................................................................... 32 
3.2.2 Chân linh hoạt (chân trước) của Robot ShrimpIII .............................................. 34 
3.2.3 Chân sau (chân cố định) ngàm cứng với thân robot ........................................... 39 
3.3  Tính toán chiều cao vật cản tối đa robot ShrimpIII có thể vượt qua được ......... 40 
3.3.1 Chiều cao lớn nhất của vật cản mà chân trước robot ShrimpIII có thể vượt qua40 
3.3.2 Chiều cao lớn nhất của vật cản mà 2 bánh ở chân bên có thể vượt qua ............. 43 
3.4  Động học và động lực học trong di chuyển tự hành của robot ShrimpIII .......... 45 
3.4.1 Robot ShrimpIII di chuyển trên địa hình coi như phẳng .................................... 46 
3.4.2 Giai đoạn từ khi bánh chân trước ShrimpIII tiếp xúc đến khi leo hết bậc thang
đầu tiên ........................................................................................................................ 49 
3.4.3 Bánh chân trước lên bám bề mặt bậc 2 trong khi các bánh còn lại vẫn nằm ở bề
mặt bậc 1 ..................................................................................................................... 51 

Học viên: Nguyễn Văn Dương

Trang 6


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

3.4.4 Hai bánh thân (21 và 22) tiếp xúc và leo lên bậc thứ nhất cầu thang................. 52 
3.4.5 Ba bánh trước bám mặt bậc thang 2 trong khi 3 bánh sau vẫn nằm ở bề mặt bậc
thang thứ nhất.............................................................................................................. 52 
3.4.6 Hai bánh thân (31 và 32) vượt bậc thang thứ nhất ............................................. 53 
3.4.7 Bánh sau cùng vượt bậc thang ............................................................................ 54 
3.4.8 Trường hợp vật cản nhỏ nằm lệch chỉ 2 bánh thuộc cơ cấu hình bình hành vượt
vật cản ......................................................................................................................... 55 
3.4.9 Robot ShrimpIII quay vòng và chuyển hướng khi di chuyển trên nền phẳng.... 56 
3.4.10  Tính toán trường hợp nguy hiểm khi robot xuống cầu thang........................ 57 
3.4.11  Các trường hợp robot bị dừng chuyển động do vật cản nằm vị trí cản đặc
biệt và giải pháp khắc phục......................................................................................... 58 
3.4.12  Thiết lập phương trình động học robot ShrimpIII trong toạ độ phẳng.......... 59 
3.5  Kích thước thiết kế, mô hình mô phỏng và giải pháp kỹ thuật khi chế tạo........ 60 
3.5.1 Chân trước( chân linh hoạt ) ............................................................................... 60 
3.5.2 Chân sau ( chân cố định với thân) ...................................................................... 63 
3.5.3 Chân bên ( kết cấu khung linh hoạt hình bình hành).......................................... 64 
3.5.4 Thân robot........................................................................................................... 66 
KẾT LUẬN ................................................................................................................ 69 
TÀI LIỆU THAM KHẢO ........................................................................................ 70 

Học viên: Nguyễn Văn Dương

Trang 7

GVHD: TS.Nguyễn Trọng Doanh 

Luận văn tốt nghiệp

CHƯƠNG 1: TỔNG QUAN VỀ RÔBOT TỰ HÀNH
1.1 Một số mẫu robot tự hành nổi tiếng trên thế giới
Ngày nay, trong sự phát triển của nền công nghiệp cũng như sự phát triển
vượt bậc của khoa học kỹ thuật thì con người dần dần giảm thiểu tối đa phải tiếp
xúc những công việc nguy hiểm và nặng nhọc. Để có thể đảm nhiệm thay thế con
người ở những công việc như vậy đòi hỏi thiết bị phải có khả năng thay thế được
con người cả về phương diện trí tuệ lẫn độ linh hoạt. Vì vậy robot tự hành là một
giải pháp tối ưu cho công việc đó.

Hình 1.1. Robot tự hành SmatROD
Đặc tính có thể mang trọng tải và di chuyển tự động trên nền phẳng. Về kết
cấu mẫu robot tự hành này tương tự một hệ thống nâng chuyển được điều khiển
hoàn toàn tự động.

Học viên: Nguyễn Văn Dương

Trang 8


GVHD: TS.Nguyễn Trọng Doanh 

Luận văn tốt nghiệp

Hình 1.2. Mantis Robot

Đặc điểm của Robot Mantis là khả năng leo địa hình đặc biệt là cầu thang.
Tuy nhiên về kết cấu phần cơ khí của hệ thống thì còn nhiều yếu tố chưa hoàn hảo
khi chuyển động. Hình dạng bánh không cân xứng dẫn tới mất cân bằng và phân bố
lực không đều khi chuyển động.

Hình 1.3. Robot thám hiểm của trung tâm vũ trụ NASA
Đặc điểm của các mẫu robốt tự hành này là tính ổn định và bền vững trước
những yễu tố tác động của môi trường. Được trang bị những hệ thống cảm biến hiện
đại và hệ thống điều khiển tối ưu. Năng lượng cấp cho toàn hệ thống được chuyển
đổi trực tiếp từ năng lượng tự nhiên (năng lượng từ mặt trời) thông qua hệ thống pin
quang học.

Học viên: Nguyễn Văn Dương

Trang 9


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.4. Robot tự hành 8 bánh Lunokhod.

Hình 1.5. Robot Lauron II từ đại học Karlsruhe
Robot Lauron II là loại di chuyển bằng chân có khả năng thích nghi tốt với
địa hình tự nhiên nhờ khả năng tự đảm bảo cân bằng của nó . Tuy nhiên nó có kết
cấu cơ khí quá phức tạp, đòi hỏi sự điều khiển cực tốt và thường có tốc độ rất chậm.

Học viên: Nguyễn Văn Dương

Trang 10


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.6. Caterpillar robot ROBHAZ_DT3
của Viện Khoa học và Công Nghệ Hàn Quốc ( KIST )
Robot chạy bằng xích có khả năng vượt chướng ngại vật tốt nhờ sự vững
chắc và hệ số ma sát động cao. Tuy nhiên do ma sát lớn nên robot gặp khó khăn
trong việc xoay chuyển và nếu một động cơ của cơ cấu bánh xích bị hỏng sẽ làm
robot mất hoàn toàn tính linh hoạt.

Hình 1.7. Robot tự hành ShrimpIII của công ty Bluebotics

Học viên: Nguyễn Văn Dương

Trang 11


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Đây là một mẫu robot tự hành có kết cấu ấn tượng và tính năng vượt trội với
những địa hình có chiều cao trung bình và phức tạp. Những điểm đặc biệt của nó sẽ
được nói tới trong những phần sau.

Hình 1.8 Robot người P2 của hãng Honda (Nhật Bản)

Hình 1.9 Robot Dog Aibo của hãng Sony (Nhật Bản)

Học viên: Nguyễn Văn Dương

Trang 12


GVHD: TS.Nguyễn Trọng Doanh 

Luận văn tốt nghiệp

Hình 1.8 và 1.9 là 2 mẫu robot đặc biệt mô phỏng theo hình dáng của các
sinh vật thực là con người và loài chó. Có thể nói 2 mẫu robot này là sự tích hợp
hoàn hảo và đòi hỏi độ chính xác cao và được trang bị những phần tử hiện đại nhất.
1.2

Một số kết cấu bánh dẫn động và điều khiển đặc biệt của robot
tự hành

1.2.1 Phương pháp thay đổi trọng tâm đối trọng khi robot di chuyển trên địa hình

Hình 1.10 Phương pháp cân bằng trọng tâm robot bằng dịch đối trọng theo đường
thẳng

Học viên: Nguyễn Văn Dương

Trang 13

Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.11 Phương pháp thay đổi trọng tâm bằng hệ thống thanh nhún song song
1.2.2

Kết cấu bánh khi di chuyển thẳng và khi rẽ hay quay vòng

Hình 1.12 Phương pháp điều khiển 3 bánh

Học viên: Nguyễn Văn Dương

Trang 14


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Trong phương pháp này 2 bánh sau là bánh chủ động điều khiển tịnh tiến và
rẽ còn bánh trước chỉ đóng vai trò tự lựa và cân bằng tải trọng.

Hình 1.13 Phương pháp điều khiển cả ba bánh cả dẫn động và tác dụng lái

Hình 1.14 Khung có 2 bánh rẽ phía trước. 2 bánh sau chủ động và được kết nối qua
khớp giúp robot luôn ở trạng thái cân bằng

Học viên: Nguyễn Văn Dương

Trang 15


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.15 Khâu cơ sở liên kết với các khâu động giúp cơ cấu trở nên linh hoạt khi
di chuyển

Hinh 1.16 Hai phần được kết nối qua khớp trụ đứng giúp robot rẽ dễ dàng

Học viên: Nguyễn Văn Dương

Trang 16


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.17 Kết cấu 5 bánh với 4 bánh dẫn động và 1 bánh lái

Hình 1.18 Kết cấu robot tự hành 6 bánh 2 hàng song song linh hoạt

Học viên: Nguyễn Văn Dương

Trang 17

Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.19 Kết cấu 6 bánh với 2 bánh rẽ phía trước

Hình 1.20 Kết cấu 6 bánh với độ linh hoạt đặc biệt

Học viên: Nguyễn Văn Dương

Trang 18


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.21 Kết cấu 6 bánh đặc biệt nhất ( cả 6 bánh đều dẫn động và bánh trước và
bánh sau là 2 bánh điều khiển rẽ và quay vòng.

Hình 1.22 Kết cấu robot 8 bánh linh hoạt

Học viên: Nguyễn Văn Dương

Trang 19


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.23 Kết cấu 8 bánh với bộ đôi giá chuyển hướng

Hình 1.24 Kết cấu 8 bánh với 2 phần liên kết bằng khớp trụ

Học viên: Nguyễn Văn Dương

Trang 20


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

Hình 1.25 Kết cấu 8 bánh, hai phần liên kết bằng khớp cầu

Học viên: Nguyễn Văn Dương

Trang 21


GVHD: TS.Nguyễn Trọng Doanh 

Luận văn tốt nghiệp

CHƯƠNG 2: TÍNH TOÁN ĐỘNG HỌC RÔBOT
SHRIMPIII
2.1 Giới thiệu phương pháp 3D-Odometry.
Phần lớn các rôbot tự hành được thiết kế chạy trong nhà hoặc môi trường bằng

phẳng. Ở những địa hình không bằng phẳng rất nhiều vấn đề phát sinh. Các bánh
xe dễ bị trượt hơn bởi vì cấu trúc ghồ ghề của đất và sai số xác định vị trí có thể
tăng lên một cách nhanh chóng. Để hạn chế điều đó chúng ta có 2 cách tác động
trực tiếp vào: một là cải thiện kết cấu cơ khí, hai là thiết kế bộ điều khiển thông
minh. Một kỹ thuật mới được gọi là 3D-Odometry sẽ cung cấp số liệu chuyển động
3D của rôbot ShrimpIII sẽ được giới thiệu trong chương này.
2.2 Phương pháp 3D-Odometry.
3D-Odometry được sử dụng rộng rãi cho các rôbot chuyển động trên bề mặt bằng
phẳng thậm trí ghồ ghề. Các phương trình cũng được biết đến và cho phép để ước
lượng vị trí và hướng của rôbot tức là [xπ, yπ, ψ] T trong mặt phẳng π. Vector này
được cập nhật bằng cách tích hợp chuyển động gia tăng giữa hai chuyển động liên
tiếp. Các lỗi được giảm thiểu bằng cách giữ thời gian giữa các lần cập nhật càng
nhỏ càng tốt.
Cách tính toán 3D-Odometry có thể được chia thành hai bước: ước lượng dịch
chuyển của giá chuyển hướng bên trái và bên phải của robot (mục 2.2.1) và tính
toán của kết quả dịch chuyển 3D của trọng tâm rôbot (phần 2.2.2). Hình 2.1 giới
thiệu các hệ tọa độ và các biến rôbot.

Học viên: Nguyễn Văn Dương

Trang 22


Luận văn tốt nghiệp

GVHD: TS.Nguyễn Trọng Doanh 

OXwYwZw: hệ tọa độ chung
B: tâm giá chuyển hướng bên trái
OXrYrZr: hệ tọa độ rôbot

∆,η: sự dịch chuyển và góc của L
Obxbzb: hệ tọa độ giá chuyển hướng
L: hình chiếu O trên mặt phẳng Obxbzb
Hình 2.1: Các hệ tọa độ của rôbot
2.2.1

Sự dịch chuyển của giá chuyển hướng.

Đối với rôbot ShrimpIII, chúng ta phải cân nhắc tới sự chuyển hướng bên
phải và bên trái của giá chuyển hướng để tính toán sự chuyển động của rôbot. Mục
đích của phần này là miêu tả cách tính toán sự dịch chuyển (∆,µ) của 1 giá chuyển
hướng khi biết sự dịch chuyển của các bánh xe (ER và EF) và sự thay đổi góc
chuyển hướng giữa trạng thái ban đầu và cuối cùng (hình 2.1, 2.2 và 2.3).
Để tính toán sự chuyển động của L, ta làm theo 2 bước. Đầu tiên ta tính toán sự
dịch chuyển của B và sau đó truyền sự chuyển động này qua cấu trúc cơ của phía
chuyển hướng để tính toán sự chuyển động hiệu quả.

Học viên: Nguyễn Văn Dương

Trang 23


GVHD: TS.Nguyễn Trọng Doanh 

Luận văn tốt nghiệp

Zb
F'
tr¹n
g th

¸i t
+1

EF

φw

F

ε ∆z'

Xb
B'

∆x'

Ob
B tr¹ng th¸i t

ρw

R''
t2

γr

R

t
§Ê

ER
t
§Ê

R'

hb
ER/EF: Sự dịch chuyển của bánh
B,B’: Vị trí ban đầu/cuối cùng của
sau/trước
tâm giá chuyển hướng
ρw, φw: Hướng chuyển động của bánh
ε: Sự thay đổi góc giá chuyển hướng
sau/trước
hb: Khoảng cách giữa 2 bánh xe
∆x’,∆z’,x,z là các thành phần của
R,F: Vị trí ban đầu của tâm bánh xe
vectơ BB’
sau/trước
t1: khoảng cách BR’’
R’,F’: Vị trí cuối cùng của tâm bánh
t2: Khoảng cách R’R”
xe sau/trước
Hình 2.2 Sự dịch chuyển của B tại vị trí t và t+1
Bởi vì khoảng cách giữa các bánh xe là cố định, chúng ta có thể viết phương trình
sau:
uuur uuur uuuuur uuuur
RF = RR ' + R ' F ' + F ' F
⎛ hb ⎞ ⎛ ER cos ρ w ⎞ ⎛ hb cos ε ⎞ ⎛ − EF cos φw ⎞

⎜ ⎟ = ⎜ − ER sin ρ ⎟ + ⎜ −h sin ε ⎟ + ⎜ EF sin φ ⎟
⎝0⎠ ⎝
w⎠ ⎝
b
w ⎠
⎠ ⎝

(2.1)

Những phương trình này có thể được giải quyết cho φw và ρw (với ER, EF và ε là
tham số). Tuy nhiên, hệ thống cân bằng này có thể bị mâu thuẫn trong một số
trường hợp. Ví dụ: nếu thay đổi ε bằng không thì khi đó ER phải cân bằng với EF
bởi vì khoảng cách giữa các bánh xe là cố định (Hình 3.2). Trong thực tế, ER và EF
có thể khác bởi vì bánh xe có thể trượt và có tốc độ khác nhau. Khi sự sắp xếp các
tham số xây dựng 1 hệ thống phương trình trái nhau đơn giản chúng ta cho là sự
dịch chuyển của tổng giá chuyển hướng là trung bình của sự di chuyển các bánh xe.
Khi đó, định lý sin được áp dụng cho tam giác RR’R’’ (hình 2.2) để được ∆x' và
∆z' cái mà là sự kết hợp của sự di chuyển của B đã được nói rõ trong hệ thống kết

Học viên: Nguyễn Văn Dương

Trang 24