Giải thuật tránh vật cản em thực hiện trong đề tài có hơi khác so với giải thuật gốc nhưng vẫn cơ bản dựa trên các trường hợp khỏang cách đọc từ cảm biến Trước khi xem xét thuật toán, ta thông qua các giả thiết sau :
• Các vật cản là vật cản tĩnh.
• Các vật cản là có dạng hai chiều có hình chiếu đứng là các đa giác lồi.
• Trong quá trình chuyển động không tồn tại các ngõ cụt.
• Khoảng cách các vật cản phải đủ lớn do yêu cầu của thuật toán và do kích thước của robot.
Góc lệch tương đối của các cảm biến được chọn là 30 độ. Góc này được chọn dựa trên thực nghiệm do robot cho kết quả tốt nhất với môi trường thí ngiệm. Góc này phải đủ lớn
để có thể bao quát được không gian phía trước robot với đặc thù kích thước của robot nhưng không được quá lớn vì robot có thể bỏ qua các vật cản nhỏ.
Để robot phát hiện vật cản cần đặt ra một ngưỡng khoảng cách mà nếu khoảng cách
đến vật cản nhỏ hơn ngưỡng thì coi như là có vật cản, nếu khoảng cách đến vật cản lớn
hơn nguỡng thì bỏ qua. Trong đề tài này em chọn ngưỡng là 60cm.
Các kí hiệu trong thuật toán :
Các trường hợp tránh vật cản của robot : dl , dr , lần lượt là các khoảng cách từ
các cảm biến bên trái, bên phải và ở giữa; là góc lệch giữa hai cảm biến cạnh nhau. a) Trường hợp cả 3 cảm biến đều phát hiện vật cản :
• Nếu dl<dr thì rẽ trái 60 độ • Nếu dl>dr thì rẽ phải 60 độ
b) Trường hợp cảm biến ở giữa và một trong hai cảm biến hai bên phát hiện vật cản với
Chương 6 Thực hiện đề tài
——————————————————————————————————————
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 79
.
• Nếu cảm biến ở giữa và bên trái phát hiện vật cản và > cosα : rẽ trái 60 độ • Nếu cảm biến ở giữa và bên phải phát hiện vật cản và > : rẽ phải 60 độ
c) Trường hợp cảm biến ở giữa và một trong hai cảm biến hai bên phát hiện vật cản với
< cosα hoặc <
• Nếu cảm biến ở giữa và bên trái phát hiện vật cản và < cosα : rẽ phải 60 độ • Nếu cảm biến ở giữa và bên phải phát hiện vật cản và < : rẽ trái 60 độ
d) Trường hợp chỉ có cảm biến giữa phát hiện vật cản: robot tiến thẳng ( do điều kiện khoảng cách giữa các vật cản đủ lớn )
Chương 6 Thực hiện đề tài
——————————————————————————————————————
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 80
e) Trường hợp chỉ có cảm biến trái hoặc phải phát hiện vật cản: rẽ 30 độ theo hướng tránh vật cản.
f) Trường hợp chỉ có cảm biến ở giữa phát hiện vật cản :
• Nếu khuôn mặt nằm bên phải thỉ rẽ phải 60 độ • Nếu khuôn mặt nằm bên trái thì rẽ trái 60 độ
6.2.3 Giải thuật bắt khuôn mặt và bám theo :
Việc bắt khuôn mặt được thực hiện khi bắt đầu chạy robot, robot sẽ quay tại chỗ và ghi nhận khuôn mặt đầu tiên webcam bắt được. Trong quá trình chuyển động việc bắt khuôn mặt được kích hoạt khi robot nhận thấy không có vật cản ở phía trước nó.
Chương 6 Thực hiện đề tài
——————————————————————————————————————
Robot trán h hướng ngại vật - GVHD: TS.Nguy
Hình 6.15 Robot bắt khuôn mặt bằng camera, tính toán khoảng sai lệch giữa khuôn
mặt và tâm khung hình
Việc bắt khuôn mặt bịảnh h
• Ánh sáng trong môi trư sánh độ sáng tối của các v khi được đưa vào x
hưởng của độ sáng rất lớn. Độ sáng c
• Phần nền ( background) : Việc phát hiện khuôn mặt dựa tr nên nhiều khi phần nền lại cho ra những kết quả vô t nhận biết khuôn mặt.
• Sự sai lệch giữa khuôn mặt so dữ liệu do Intel xây dựng n đồng với nhau nên trong th khó bắt khuôn mặt d
• Ảnh hưởng của chuyển động : người ) nên sẽ chịu sự lắc l Ảnh sau khi lấy từ camera sẽđ bằng histogram ). Việc giả do giảm số pixel ). Kích thư
——————————————————————————————————
GVHD: TS.Nguyễn Đức Thành
ắt khuôn mặt bằng camera, tính toán khoảng sai lệch giữa khuôn
à tâm khung hình theo phương nằm ngang error.x
ệc bắt khuôn mặt bịảnh hưởng bởi nhiều yếu tố:
Ánh sáng trong môi trường làm việc : do chuỗi phân loại hoạt động trên nguyên t ộ sáng tối của các vùng hình chữ nhật với các thuộc tính Haar
ưa vào xử lý được chuyển vềảnh xám đểđơn giản việc tính toán n ởng của độ sáng rất lớn. Độ sáng càng lớn thì việc phát hiện càng nhanh. ần nền ( background) : Việc phát hiện khuôn mặt dựa trên việc tính toán l
ều khi phần nền lại cho ra những kết quả vô tình trùng với những ti ận biết khuôn mặt.
ự sai lệch giữa khuôn mặt so với tập dữ liệu đưa vào chuỗi phân loại : do sử dụng tập ữ liệu do Intel xây dựng nên, có thể dựa trên một nhóm người có những nét t
nên trong thực tế, một số người rất dễ bắt khuôn mặt, một số ít khác rất ắt khuôn mặt dù trong cùng một hoàn cảnh.
ởng của chuyển động : Camera đặt ở vị trí cao nhất của robot ( cao gần bằng ẽ chịu sự lắc lư lớn nhất.
Ảnh sau khi lấy từ camera sẽđược giảm kích thước ( resize) và đưa qua bộ tiền xử lý ( cân ằng histogram ). Việc giảm kích thước giúp làm giảm thời gian tính toán của thuật toán (
Kích thước hình sau cùng là 340x260.
——————————————————————————————————
Trang 81
ắt khuôn mặt bằng camera, tính toán khoảng sai lệch giữa khuôn
ên nguyên tắc so ữ nhật với các thuộc tính Haar-like, ảnh trước ản việc tính toán nên ảnh àng nhanh. ệc tính toán là chủ yếu ới những tiêu chuẩn để ỗi phân loại : do sử dụng tập ời có những nét tương ời rất dễ bắt khuôn mặt, một số ít khác rất ặt ở vị trí cao nhất của robot ( cao gần bằng ộ tiền xử lý ( cân ảm thời gian tính toán của thuật toán (
Chương 6 Thực hiện đề tài
——————————————————————————————————————
Robot trán h hướng ngại vật - GVHD: TS.Nguy
6.2.4 Giải thuật kết hợp bám
Ý tưởng của giải thuật n
theo mặt người. Khi không có vật cản, robot sẽ di chuyển h vật cản, robot sẽ rẽ tránh vật cản, trong lúc tránh vật cản sẽ bỏ qua h
——————————————————————————————————
GVHD: TS.Nguyễn Đức Thành
ám khuôn mặt và tránh vật cản :
ởng của giải thuật này là hoán chuyển giữa hai nhiệm vụ : tránh v
ời. Khi không có vật cản, robot sẽ di chuyển hướng đến mặt ng
ật cản, robot sẽ rẽ tránh vật cản, trong lúc tránh vật cản sẽ bỏ qua hình ảnh từ camera. ——————————————————————————————————
Trang 82
tránh vật cản và bám
ớng đến mặt người; khi gặp
Chương 6 Thực hiện đề tài
——————————————————————————————————————
Robot trán h hướng ngại vật - GVHD: TS.Nguy
——————————————————————————————————
GVHD: TS.Nguyễn Đức Thành
——————————————————————————————————
Chương 6 Thực hiện đề tài
——————————————————————————————————————
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 84
Dữ liệu từ ba cảm biến siêu âm được đọc liên tục và truyền về máy tính. Máy tính sau khi nhận
được hết dữ liệu của ba cảm biến sẽ đưa ra hướng chuyển động thích hợp cho robot. Việc bắt khuôn mặt cũng được thực hiện liên tục, nhưng giá trị sai lệch giữa khung hình và khuôn mặt chỉ
được xem xét khi robot nhận thấy không có vật cản.
6.2.5 Chương trình Visual C++
Chương trình điều khiển được thực trên máy tính bằng ngôn ngữ Visual C++ phiên bản 6.0.
Hình 6.16 Giao diện điều khiển viết bằng Visual C++
Chú thích các phần trong giao diện :
ü Manual Omni Test : kiểm tra hoạt động của robot omni với các chuyển động cơ bản.
ü TEST CAM : kiểm tra hoạt động của camera
ü Measure Distances : khoảng cách đo được từ ba cảm biến siêu âm.
ü Face State : trạng thái lệch của khuôn mặt so với robot
ü x_error, y_error : độ lệch khuôn mặt so với gốc tọa độ
ü data_Send : kiểm tra dữ liệu được gửi đi data_Receive : kiểm tra dữ liệu nhận về
ü CAPTURE : bắt đầu cho robot chạy STOP CAPTURE : cho robot ngừng
Chương 7 Kết quả và hướng phát triển
——————————————————————————————————————
Robot trán h hướng ngại vật – GVHD: TS. Nguyễn Đức Thành Trang 85
Chương 7
KẾT QUẢ VÀ HƯỚNG PHÁT TRIỂN
7.1 KẾT QUẢĐẠT ĐƯỢC
Trong điều kiện ánh sáng bình thường và phông nền không quá phức tạp, hệ thống có thể
phát hiện được khuôn mặt và điều khiển robot bám theo. Với giải thuật tránh vật cản, robot có thể phát hiện được các vật cản đơn giản và đủ lớn với điều kiện các vật cản không đặt quá gần nhau.
7.2 ĐÁNH GIÁ KẾT QUẢ 7.2.1 Ưu điểm
ü Phát hiện được khoảng 80% khuôn mặt trên cáckhoảng cách khác nhau trong điều
kiện robot chuyển động.
ü Giải thuật tránh vật cản tương đối đơn giản
ü Truyền thông giữa các thiết bịổn định
7.2.2 Khuyết điểm
- Khi chương trình bắt ảnh trong một thời gian dài thì có hiện tượng báo lỗi do tràn bộ
nhớđệm, chưa khắc phục được.
- Robot chuyển động chưa được tốt do điều khiển vòng hở. - Đôi khi robot bị dao động do cảm biến đọc về có sai số.
- Với phông nền phức tạp, và độ sáng bất thường thì khả năng nhận dạng khuôn mặt kém, nhận dạng sai, hoặc không nhận được khuôn mặt.
7.3 HƯỚNG PHÁT TRIỂN ĐỀ TÀI
Trước mắt cần khắc phục khả năng phát hiện khuôn mặt cảu robot, có thể sử dụng thêm nguồn ánh sáng hoặc thêm vào các giải thuật để lọc bỏ các kết quả không phải là khuôn mặt. Sau đó có thể sẽđặt thêm các cảm biến, có thể lả cảm biến siêu âm để có thể khắc phục hạn chế của thuật toán dẫn hướng thích nghi. Các cảm biến này có tác dụng phát hiện các trương hợp đặc biệt mà luật dẫn hướng bỏ qua, làm cho robot có thể di chuyển trong môi trường phức tạp. Ngoài ra có thể sử dụng thêm một camera nữa để xác định khoảng cách đến khuôn mặt
chính xác hơn. Đề tài này có thể mở rộng không chỉ bám theo khuôn mặt mà có thể bám theo
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 86
PHỤ LỤC
CODE CHƯƠNG TRÌNH ĐỌC BA CẢM BIẾN SIÊU ÂM
// Chuong trinh viet cho Atmega16 bang AVRStudio, xung clock = 4MHz
#include<avr/io.h> #include<avr/interrupt.h> #include<util/delay.h> unsigned char data; unsigned int falling; unsigned int cm;
//********************** void UARTinit()
{
#define F_CPU 4000000UL
#define UART_baudrate 500000//Chuong trinh da test de co the giao tiep voi toc do baud 9600 va 19200
#define BAUD_SCALE (((F_CPU/(UART_baudrate*16UL)))-1) DDRD |= (1<<4);
PORTD |= (1<<4);
UBRRL = (unsigned char) 4; UBRRH = (unsigned char) 0; UCSRB = (1<<RXEN)|(1<<TXEN);
UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0); }
//*********************
void delayms(unsigned int interval) { unsigned int i; for(i=0;i<=interval;i++) { _delay_loop_2(1000); } } //**************************** void sendChar(unsigned char data) {
while((UCSRA&(1<<UDRE))==0); UDR = data;
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 87 }
//********************** void init_ports()
{
DDRA |=(1<<5)|(1<<6)|(1<<7);//Xung trigger input PA5 va PA6
DDRD&=~(1<<2);//Input cho chan EXTERNAL INTERRUPT PD2- INT0 DDRD&=~(1<<3);//Input cho chan EXTERNAL INTERRUPT PD3- INT1 DDRD&=~(1<<6);//Input cho chan INPUT CAPTURE INTERRUPT- ICP1 // DDRB|=0xFF;
TCCR1B |=(1<<CS10); // dinh ti le timer 1 prescaler/1 }
//********************** void pulse(unsigned char index) {
switch (index) {
case 0:
PORTA |= (1<<5);//Tao xung muc cao tren chan PA5 voi be rong toi thieu la 10 us
_delay_loop_2(60);
PORTA &=~(1<<5);//Ket thuc xung
MCUCR |=(1<<ISC01)|(1<<ISC00); // dinh ngat canh len cho external interrupt 0 break;
case 1:
PORTA |= (1<<6);// TAO XUNG MUC CAO TREN PA6 VOI BE RONG TOI THIEU LA 10us
_delay_loop_2(60);
PORTA &= ~(1<<6);// ket thuc xung
MCUCR |=(1<<ISC11)|(1<<ISC10);// dinh ngat canh len cho external interrupt 1 break;
case 2:
PORTA |= (1<<7); //TAO XUNG MUC CAO TRN PA7 VOI BE RONG TOI THIEU LA 10us
_delay_loop_2(60);
PORTA &=~(1<<7);//KET THUC XUNG
TCCR1B |=(1<<ICES1); //CHO NGAT CANH LEN INPUT CAPTURE break; default: break; } } //********************** void enable_sonar_interrupt() {
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 88 MCUCR = (1<<ISC11)|(1<<ISC10)|(1<<ISC01)|(1<<ISC00);// dinh ngat ngoai la ngat canh len
GICR |=(1<<INT0)|(1<<INT1);//Cho phep ngat external interrupt 0&1 TCCR1B |=(1<<ICES1); //dinh ngat canh len cho input capture 1 TIMSK |=(1<<TICIE1); //cho phep ngat input capture
sei();//Cho phep ngat toan cuc }
//**********************
unsigned int calc_cm(unsigned int count) {
unsigned int result;
//mot chu ki xung nhip tuong ung voi thoi gian la 0.25 us, ta phai doi ra us roi chia cho 58 de ra cm return ((count>>2)/58);
}
//********************** int main(void)
{
//Chuong trinh con nay thuc hien viec giao tiep cam bien sieu am theo mode 1: dung 2 chan cua VXL init_ports();//Khoi dong giao tiep cac chan port
enable_sonar_interrupt();//Cho phep ngat UARTinit();
while (1) {
pulse(0);//Tao xung cho sonar noi vao timer 0
delayms(100);//Cho mot khoang thoi gian toi thieu la 50 ms truoc khi phat xung trigger tiep theo
pulse(1);//Tao xung cho sonar noi vao timer 1 delayms(100);
pulse(2);////Tao xung cho sonar noi vao icp1 delayms(100); } return(1); } //__________________________ //************************** ISR(INT1_vect)// {
unsigned char x,y;
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 89 {
TCNT1 = 0;
MCUCR &=~(1<<ISC10);//cho phep ngat canh xuong
}
else//Neu la ngat canh xuong
{
falling = TCNT1;//Doc gia tri timer 1
cm = calc_cm(falling); x = (unsigned char)cm % 256; y = (unsigned char)cm /256; sendChar('X'); _delay_loop_2(1000); if ((x == 'X')||(x == 'V')||(x == 'Z')||(x == 0)||(x=='!')) sendChar(x+1); else sendChar(x); _delay_loop_2(1000); if (cm>255) sendChar('1'); else sendChar('0'); } } //************************* ISR(INT0_vect)// {
unsigned char x,y;
if ((PIND&(1<<2))!=0)//Neu la ngat canh len
{
TCNT1 = 0;
MCUCR &=~(1<<ISC00);
}
else//Neu la ngat canh xuong
{
falling = TCNT1; //Doc gia tri timer 1
cm = calc_cm(falling); x = (unsigned char)cm % 256; y = (unsigned char)cm /256; sendChar('V'); _delay_loop_2(1000); if ((x == 'V')||(x == 'X')||(x == 'Z')||(x == 0)||(x=='!')) sendChar(x+1); else sendChar(x); _delay_loop_2(1000); if (cm>255) sendChar('1');
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 90 else sendChar('0'); } } //******************************* ISR(TIMER1_CAPT_vect) {
unsigned char x,y;
if ((PIND&(1<<6))!=0)//Neu la ngat canh len {
TCNT1 = 0;
TCCR1B &=~(1<<ICES1);//Cho phep ngat canh xuong
}
else//Neu la ngat canh xuong
{
falling = ICR1;//Doc gia tri timer 1
TCCR1B |=(1<<ICES1);//Cho phep ngat canh len tro lai cm = calc_cm(falling); x = (unsigned char)cm % 256; sendChar('Z'); _delay_loop_2(1000); if ((x == 'V')||(x == 'X')||(x == 'Z')||(x == 0)) sendChar(x+1); else sendChar(x); _delay_loop_2(1000); if (cm>255) sendChar('1'); else sendChar('0'); } }
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 91
TÀI LIỆU THAM KHẢO
[1] Nguyễn Đức Thành (2005). Đo lường và điều khiển bằng máy tính , Nhà Xuất Bản Đại
Học Quốc Gia Thành Phố Hồ Chí Minh.
[2] Atsushi Fujimori, Peter N. Nikiforuk. Adaptive Navigation of Mobile Robot with Obstacle
Avoidance, IEEE Transactions on Robotics and Automation, Vol-13, August 1997.
[3] J. Borenstein, Member, IEEE and Y. Koren, Senior Member, IEEE. The Vector Field
Histogram - Fast Obstacle Avoidance For Mobile Robots
[4] Paul Viola, Michael Jones. Rapid Object Detection using a Boosted Cascade of Simple
Features, in IEEE Conference on Computer Vision and Pattern Recognition 2001, 2001.
[5] Bruno Siciliano, Oussama Khatib (2007) . Springer Handbook of Robotics
[6] J. Borenstein1, H. R. Everett2, and L. Feng (1996). Where am I? Sensors and Methods for
Mobile Robot Positioning, Technical Report, Michigan University
[7] Raul Rojas and Alexander Gloye F̈ rster (2006) . Holonomic Control of a robot with an
omni directional drive, K̈ nstliche Intelligenz, B̈ ttcherIT Verlag
[8]Y. Koren, Senior Member, IEEE and J. Borenstein, Member, IEEE. Potential Field
Methods and Their Inherent Limitations for Mobile Robot Navigation ,Proceedings of the
IEEE Conference on Robotics and Automation, Sacramento, California, April 7-12, 1991 [9] ThomasBräunl. Embedded Robotics, Springer
[10] Phan Tuấn Hải (2009).
[11] Nguyễn Hữu Tân (2009). Xây dựng thuật toán nhận dạng cảm xúc mặt người. Luận văn
đại học, Đại Học Bách Khoa Tp. Hồ Chí Minh.
[12] Phần Help của OpenCv
Robot trán h hướng ngại vật - GVHD: TS.Nguyễn Đức Thành Trang 92