Luận văn Robot tự hành tránh vật cản sử dụng thiết bị Kinect

t 
Hình 8.6: Vật cản bên trái robot 
Quan sát hình 8.6, góc quay cần thiết để thoát khỏi vật cản là anpha( ) 
khi robot ở vị trí cách vật cản một đoạn. Góc anpha hoàn toán tính được 
khi ta biết được tọa độ max_point, góc  và độ lớn O’L. 
_ ax1os ( )
'
x m
c
O L
   
    , hướng quay về phía bên phải robot. 
Chương 8: Chương trình điều khiển 
 Trang 84 
 Vật cản nằm bên phải robot 
Hình 8.7: Vật cản bên phải robot 
Tương tự trường hợp vật cản nằm bên trái, góc anpha được tính như sau: 
1os ( _ min/ ' )c x O L       , hướng quay về phía bên trái robot. 
Chương 8: Chương trình điều khiển 
 Trang 85 
 Vật cản nằm ở giữa đường robot 
Hình 8.8: Vật cản nằm ở giữa đường di chuyển của robot 
Trong trường hợp này, robot sẽ tìm góc quay né vật sao cho anpha nhỏ 
nhất; lúc đó, nếu độ lớn vật cản nhiều hơn về phía bên trái thì robot quay 
về bên phải (như trên hình 8.8) và ngược lại. Công thức tính trong từng 
trường hợp: (trong đó góc  và O’L = O’L’ biết trước) 
Quay trái: 
1 _ minsin ( )
'
x
O L
    

    
Quay phải: 
1 _ axsin ( )
'
x m
O L
       
Chương 8: Chương trình điều khiển 
 Trang 86 
 Tính góc quay ngược lại tránh vật: 
Như các trường hợp quay một góc xác định né vật cản, nhưng thay vì quay theo 
hướng có góc quay nhỏ hơn thì robot sẽ quay theo hướng ngược lại. Công thức tính 
hoàn toàn tương tự. 
 Đi một đoạn an toàn: 
Sau khi quay một góc tránh vật, robot sẽ di chuyển tiếp một đoạn để thoát hoàn 
toàn khỏi vật. 
Hình 8.9: Đi một đoạn an toàn về phía phải vật cản 
Quãng đường di chuyển an toàn để thoát khỏi vật cản bằng đoạn O’H cộng thêm 
một giá trị cố định vừa đủ deltaMove(ở đây chọn deltaMove = 15 cm dựa trên thực 
nghiệm ). 
Chương 8: Chương trình điều khiển 
 Trang 87 
Đi bên phải vật cản: 
' cos( )
1
_ ax2 2 1( _ ax _ ur) ( _ ax _ ur) os( tan ( ))
_ ax
OH OM
x m
x m x c y m y c c
y m
 

  
     
Đi bên trái vật cản: 
' cos( )
1
_ in2 2 1( _ in _ ur) ( _ ax _ ur) os( tan ( ))
_ ax
OH OM
x m
x m x c y m y c c
y m
 

  
     
 Cờ báo có vật cản (Obstacle_flag) và đường trống (freePath_flag): 
Hình 8.10: Cờ báo có vật cản và đường trống 
Hình 8.10 biểu diễn không gian xuất hiện vật cản phía trước robot, lúc đó các cờ 
tương ứng sẽ bật lên trong các vùng xác định. Tầm nhìn xa của robot giới hạn ở 
khoảng cách 140 cm. 
Chương 8: Chương trình điều khiển 
 Trang 88 
 Cờ báo đủ không gian cho robot lách qua: 
Hình 8.11: Không gian cho robot lách qua 
Cờ báo đủ không gian cho robot lách qua sẽ bật lên 1 khi khoảng cách H1H2 (hình 
8.11) đủ lớn cho robot lọt qua (không gian đủ cho robot lọt qua khi H1H2>50 cm). 
Trong đó O’H1 được tính như trong hàm đi một đoạn an toàn, O’H2 tương tự, ta có: 
2 4
42 2 1
4 4
4
' ' cos( )
( _ ur) ( _ ur) cos(tan ( ))
O H O M
x
x x c y y c
y


 
    
' '
1 2 2 1
H H O H O H   
8.3 Giải thuật chương trình do vi điều khiển xử lý 
Công việc điều khiển động cơ và tính toán tọa độ, thực hiện giao tiếp với máy 
tính sẽ được PIC 18F4550 xử lý. Để thực hiện các công việc này, PIC18F4550 kết hợp 
thông tin lệnh từ máy tính và tín hiệu hồi tiếp về từ encoder thông qua các ngắt ngoài. 
Sau đây là sơ đồ khối mô tả hoạt động của chương trình chính xử lý bởi PIC18F4550: 
Chương 8: Chương trình điều khiển 
 Trang 89 
Start
Khởi tạo các module
Timer1_flag = 1
InitPID_flag = 1
 Khởi tạo các giá trị đầu 
cho PID
 InitPID_flag = 0
 donePID_flag = 0
Target_flag = 1
 Tính toán tọa độ robot
 Gửi thông tin lên máy tính
 Target_flag = 0
donePID_flag = 1
 Tính toán tọa độ robot
 Gửi thông tin lên máy tính
 donePID_flag = 0
 Thực hiện PID cho 2 bánh 
xe (khi PID thực hiện xong, 
donePID_flag = 1)
 Timer1_flag = 0
Yes
No
Yes
No
No
Yes
Yes
No
Hình 8.12: Sơ đồ giải thuật trên 
vi điều khiển 
 Khởi tạo các module: 
Khởi tạo các module sử dụng trên PIC: các 
port I/O, timer, ngắt ngoài, PWM và giao 
tiếp RS232. 
 Timer1_flag: 
Cờ báo bằng 1 khi timer1 tràn sau 5 ms, 
phục vụ cho việc lấy mẫu, tính toán PID. 
 InitPID_flag: 
Cờ báo khởi tạo PID, bằng 1 khi nhận 
được lệnh từ máy tính. 
 Khởi tạo các giá trị đầu cho PID: 
Khởi tạo các giá trị ban đầu cho tính toán 
PID: các thông số kp, ki, kd; giá trị đặt. 
 Target_flag: 
Cờ báo lên 1 khi robot đang di chuyển về 
đích và gặp vật cản. 
 Tính toán tọa độ robot: 
Tính toán tọa độ robot, đã được trình bày 
tại mục 7.2. 
 Gửi thông tin lên máy tính: 
Thông tin về tọa độ và góc hiện tại của 
robot: (x_cur, y_cur, angle_cur). 
 Thực hiện PID trên hai bánh xe: 
Dựa trên giải thuật PID vị trí đã được trình 
bày ở mục 6.2. PID được thực hiện xong 
khi sai số nằm trong khoảng cho phép (xấp 
xỉ bằng không), lúc đó donePID_flag bằng 
1.
Kết luận và hướng phát triển 
 Trang 90 
KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 
Với mục đích xây dựng mô hình robot có khả năng di chuyển đến điểm đích xác 
định trước, tránh chướng ngại vật và đến đích an toàn. Nhóm đã áp dụng công nghệ xử 
lý ảnh 3D cho phần thị giác của robot với thiết bị chơi game Kinect, đáp ứng được độ 
tin cậy cao hơn so với các cảm biến truyền thống. Quá trình thực hiện đã hoàn thành 
những nhiệm vụ sau: 
- Thiết kế và thi công mô hình mobile robot hoàn chỉnh. 
- Chương trình xử lý ảnh khôi phục được không gian phía trước robot dưới dạng 
3D, cung cấp đầy đủ thông tin về môi trường cho robot. 
- Tốc độ xử lý ảnh trên máy tính khoảng 13-15 fps, đủ đáp ứng thời gian thực cho 
robot. 
- Hoàn thành kế hoạch di chuyển đến đích có vật cản với sai số chấp nhận được ở 
môi trường trong nhà. 
 Một số điểm cần khắc phục: 
- Phần cơ khí của mobile robot không được hoàn hảo nên ảnh hưởng đến sai số 
khi di chuyển. Ở điểm này, một con robot omi sẽ đáp ứng đầy đủ cho ứng dụng 
robot di chuyển trong nhà với sự linh hoạt, kết cấu gọn nhẹ và sự chính xác cao. 
Kết luận và hướng phát triển 
 Trang 91 
Một con robot rất đáng quan tâm là iRobot với các lựa chọn khác nhau, giá dao 
động từ 130$ ÷300$, chi tiết xem tại:  
- Phần thị giác robot chỉ quan sát được không gian phía trước robot nên robot có 
thể sẽ đụng vật cản bên hông hay phía sau khi di chuyển lùi. Vấn đề này có thể 
được giải quyết bằng biệc trang bị thêm các cảm biến phát hiện vật cản xung 
quanh robot; hoặc thiết kế thêm hệ thống xoay cho phần thị giác, thị giác sẽ xoay 
một góc xác định trước khi quyết định di chuyển. 
- Tích hợp thêm các cảm biến định vị cần thiết để tăng sự chính xác. 
- Chương trình được viết trên môi trường Windows với sự hỗ trợ từ thư viện Point 
Cloud còn nhiều hạn chế, nhất là ở tốc độ xử lý. Sẽ tối ưu nhất nếu viết trên môi 
trường Linux và nếu phát triển thành sản phẩm thương mại sẽ hạ được giá thành 
phần mềm xuống, tăng tính cạnh tranh. 
 Hướng phát triển: 
- Mục đích của đề tài tạo ra nền tảng cho việc xây dựng một mô hình robot dịch 
vụ hoàn chỉnh: một robot có khả năng làm các công việc thay cho con người 
như: bưng bê đồ ăn, lau nhà, hướng dẫn khách hàng, … 
- Bên cạnh đó, với sức mạnh của thiết bị Kinect có thể giúp ta xây dựng được bản 
đồ (mapping) dạng 3D. Và việc tích hợp lên robot sẽ giúp ta xây dựng được bản 
đồ ở những khu vực mà con người không thể vào được. 
Tài liệu tham khảo 
 Trang 92 
TÀI LIỆU THAM KHẢO 
[1]  
[2]  
[3] Mikkel Viager, “Analysis of Kinect for Mobile Robots,” Technical University of 
Denmark, p. 11 
[4]  
[5] Jacob Kjær. A Qualitative Analysis of Two Automated Registration Algorithms In 
a Real World Scenario Using Point Clouds from the Kinect. June 27, 2011. 
[6]  
[7]  
[8]  
[9]  
[10]  
[11]  
[12]  
[13]  Hiện tượng crosstalk. 
[14]  
[15] Radu Bogdan RUSU. PointCloud(2) processing in ROS. May 2, 2010 
[16]  
[17]  
[18]  
[19] John A. Shaw, The PID Control Algorithm , 2003 
[20]  
[21] Erick Ball, Greg Taschuk. Reverse Engineering the Kinect Stereo Algorithm 
[22] Michael YingYang, Wolfgang Forstner. Plane Detection in Point Cloud Data. 
January 25, 2010. 
Phụ lục 1 
 Trang 93 
PHỤ LỤC 1: Kết hợp thư viện OpenNI và Code Laboratories Kinect (CL) 
để sử dụng chức năng điều khiển động cơ Kinect. 
Như đã trình bày trong mục 3.2, thư viện OpenNI không hỗ trợ cho việc điều 
khiển động cơ của Kinect. Sau đây là thủ thuật bằng việc cài đặt thêm CL. Cả OpenNI 
và CL đều đóng vai trò như driver truy xuất phần cứng Kinect nên sẽ xung đột nếu cài 
đặt đồng thời hai thư viện này. 
 Bước 1: 
Cài đặt CL bình thường và đảm bảo chưa cài OpenNI và các chương trình đi kèm 
trước đó (nếu có thì phải gỡ bỏ). 
CL có thể tải về tại:  
 Bước 2: 
Mở cửa sổ Computer Management, và chọn Uninstall các mục dưới CL devices: 
NUI audio, NUI camera, NUI Motor. 
Phụ lục 1 
 Trang 94 
 Bước 3: 
Cài đặt OpenNI và các chương trình kèm theo. 
 Bước 4: 
Vào lại cửa sổ như ở bước 1, lúc này Kinect được nhận bởi PrimeSense. Ta nhấp 
phải chuột trên Kinect Motor và chọn Update Driver Software 
Và duyệt tới thư mục driver của CL: 
Phụ lục 1 
 Trang 95 
Lúc này ta có thể sử dụng chức năng điều khiển động cơ Kinect trên thư viện 
OpenNI kết hợp với CL. 
Phụ lục 2 
 Trang 96 
PHỤ LỤC 2: Cách đấu dây dùng pin 12V thay adapter và tạo đế gắn lên 
robot cho Kinect 
 Cách đấu dây dùng pin 12V thay adapter cho Kinect 
Để tạo sự linh động cho robot, ta không dùng nguồn adapter mà thay bằng nguồn 
pin. Đầu tiên cắt dây adapter ra: 
Sau đó dùng đầu cắm DC cho pin và đoạn dây trên: 
Phụ lục 2 
 Trang 97 
 Tạo đế cho Kinect để gắn lên robot 
Đây là bước quan trọng nhằm giữ sự an toàn cho Kinect và tránh bị rung khi di 
chuyển (sẽ ảnh hưởng tới kết quả xử lý ảnh trên máy tính). 
Đầu tiên tháo phần đế gắn trên Kinect: 
Phụ lục 2 
 Trang 98 
Sau khi tháo: 
Khoan bốn lỗ và gắn bốn ốc từ trong đế ra: 
Và kết quả cuối cùng: 
Phụ lục 3 
 Trang 99 
PHỤ LỤC 3: Kích thước robot 
Gồm các kích thước cơ bản các chiều trên các hình chiếu đứng, bằng và cạnh. Đơn vị 
là milimet.