Robot lau nhà bằng phát triển và cải tiến thuật toán di chuyển ziczac và thuật toán PID.

Trong bài báo cáo này Nhóm 2 chúng em xin trình bày phương pháp di chuyển robot – thuật toán “ Cày ziczag kết hợp với thuật toán PID “áp dụng để xây dựng ứng dụng “ robot lau nhà “. Hiện nay, các công trình nghiên cứu trên thế giới đã đưa ra nhiều thuật toán đường đi bao phủ để giải quyết một số vấn đề trong điều khiển Robot lau nhà và lịch trình đường đi bao phủ tối ưu khoảng cách. Song chủ yếu các thuật toán đưa ra hầu hết đều chỉ thực hiện yêu cầu trong môi trường tĩnh. Với thực tế đó, nhóm chúng em xin đề ra phương hướng giải quyết cho bài toán “ robot lau nhà “ trong môi trường động. Bên cạnh đó , còn đề ra một số giải pháp để cải tiến việc sai số trong định vị và dẫn đường của robot, sai số trong thu thập thông tin về môi trường.

TRÍ TUỆ NHÂN TẠO LTU12

A

Robot lau nhà bằng phát triển và cải tiến thuật toán di

chuyển ziczac và thuật toán PID.

Lê Thị Mai 1 , Đỗ Vũ Hiệp 2 , Nguyễn Văn Quý 3

Đặng Vũ Đức 4 , Nguyễn Cao Thùy Trang 5 Viện Đào Tạo Quốc Tế- Đại Bách Khoa Hà Nội

I.Bài toán.

**Yêu cầu bài toán:

Robot lau nhà khi hoạt động phải làm sạch toàn bộ diện tích sàn làm việc , đồng thời phải phát hiện và tránh được vật cản như bàn ghế, đồ đạc trong phòng Các thuật toán bao phủ sử dụng cần thỏa mãn các yêu cầu sau:

robot phải bao phủ toàn bộ vùng hoạt động

định

***Phân tích bài toán: Robot lau nhà

+ Các nhận thức : Vị trí và mức độ sạch sẽ VD: [A,bẩn] ,[B, bẩn]

+ Các hành động: Di chuyển sang phải, sang trái, hút bụi hoặc không làm gì.

Funtion: Relax-Agent([location,status]) return an action

If status = dirty then return Suck

Else if location = A then return Right

Else if location = B then return Left

*****Hiệu quả hoạt động: Mức độ làm sạch , thời gian hút bụi, mức điện năng

tiêu tốn, mức tiếng ồn gây ra,…

*****Thuật toán đề xuất: Thuật toán Cày zic-zag đảm bảo được diện tích sàn

nhà được lau là tối đa,đồng thời còn tránh được sự trùng lặp, kết hợp với thuật toán PID tốt để robot chuyển động theo hướng ổn định nhất

*****Phát hiện vật cản:

Có rất nhiều phương pháp tránh vật cản cho robot như: phương pháp điều khiển bám biên, phương pháp phát hiện cạnh,phương pháp biểu diễn vật cản bằng sơ đồ lưới,phương pháp trường thế,phương pháp dung Optical Flow,và phương pháp hướng dẫn thích nghi Ở đây em sẽ sử dụng phương pháp hướng dẫn thích nghi để điều khiển robot lau nhà tránh vật cản

****Nhưng cải tiến để ra:

II.Giải quyết vấn đề.

1.Thuật toán di chuyển của robot:

a Giới thiệu thuật toán:

Có nhiều thuật toán di chuyển như xoắn ốc, ngẫu nhiên, ziczag,loang… thì ziczag được sử dụng trong đề tài này vì ưu điểm là sẽ tối ưu được diện tíchsàn nhà được lau Tuy nhiên cần phải có được thuật toán PID tốt để robot

có thể di chuyển theo hướng ổn định nhất

Ở lần lau đi robot có thể bỏ sót ở 1 bên của vật cản, nhưng ở lần lau về thì

nó sẽ được robot lau Vì vậy vẫn đảm bảo được diện tích được lau cao Hình vẽ minh họa:

camera chính giữa trên đầu của robot và hình ảnh chụp được từ camera qua sử lý ảnh có thể tính được sai số của quỹ đạo di chuyển thực của robotvới quỹ đạo tham chiếu

b Sơ đồ thuật toán di chuyển ziczag

2.Thuật toán tránh robot tránh vật cản:

a Phương pháp: sử dụng phương pháp dẫn hướng thích nghi

Sử dụng ba cảm biến siêu âm SRF04 để xác định khoảng cách Cảm biến thứ nhất đặt ở giữa và hướng xuống dưới giúp robot tránh bị rơi xuống cầu thang hay những nơi có sự chênh lệch độ cao Hai cảm biến còn lại đặt hai bên giúp robot tránh vật cản khi quay đầu

Gọi dc, dl và dr là khoảng cách theo 3 phương,dl, dr đặt lệch so với dc một góc α Khi cảm biến không phát hiện được vật cản hay d>dmax , ngõ ra của sensor= -1

Đây là các trường hợp tránh vật cản cho robot:

2 – ε)

Hình d: dc<0 robot không cần quay.

Hình f: dc>0 Robot c n ph i quay Tuy nhiên dl, dr<0 t c bên trái ần phải quay Tuy nhiên dl, dr<0 tức bên trái ải quay Tuy nhiên dl, dr<0 tức bên trái ức bên trái

v bên ph i ải quay Tuy nhiên dl, dr<0 tức bên trái đều không có vật cản Trường hợp này robot quay sang u không có v t c n Tr ật cản Trường hợp này robot quay sang ải quay Tuy nhiên dl, dr<0 tức bên trái ường hợp này robot quay sang ng h p n y robot quay sang ợp này robot quay sang trái m t góc ột góc π

2.

θ *

a(t) = θ(t)+π2

Gi i thu t i u khi n ải quay Tuy nhiên dl, dr<0 tức bên trái ật cản Trường hợp này robot quay sang đ ều không có vật cản Trường hợp này robot quay sang ển đảm bảo thỏa mãn các yêu cầu sau: đải quay Tuy nhiên dl, dr<0 tức bên trái m b o th a mãn các yêu c u sau: ải quay Tuy nhiên dl, dr<0 tức bên trái ỏa mãn các yêu cầu sau: ần phải quay Tuy nhiên dl, dr<0 tức bên trái

-Luôn c p nh t thông tin v v trí v góc c a robot trong m t ph ng ật cản Trường hợp này robot quay sang ật cản Trường hợp này robot quay sang ều không có vật cản Trường hợp này robot quay sang ị trí và góc của robot trong mặt phẳng ủa robot trong mặt phẳng ặt phẳng ẳng -X lý tín hi u c m bi n v tính góc tránh v t c n cho robot ử lý tín hiệu cảm biến và tính góc tránh vật cản cho robot ệu cảm biến và tính góc tránh vật cản cho robot ải quay Tuy nhiên dl, dr<0 tức bên trái ến và tính góc tránh vật cản cho robot ật cản Trường hợp này robot quay sang ải quay Tuy nhiên dl, dr<0 tức bên trái θ *

-So sánh các giá tr góc l ch, góc h ị trí và góc của robot trong mặt phẳng ệu cảm biến và tính góc tránh vật cản cho robot ướng đích và góc tránh vật cản để ng ích v góc tránh v t c n đ ật cản Trường hợp này robot quay sang ải quay Tuy nhiên dl, dr<0 tức bên trái đển đảm bảo thỏa mãn các yêu cầu sau: xác

đị trí và góc của robot trong mặt phẳng nh góc d n h ẫn hướng thích hợp ướng đích và góc tránh vật cản để ng thích h p ợp này robot quay sang θ *(t).

S ơ đồ mạch điều khiển: đồ mạch điều khiển: ạch điều khiển: m ch i u khi n: đ ều không có vật cản Trường hợp này robot quay sang ển đảm bảo thỏa mãn các yêu cầu sau: Gi i thu t i u khi n tránh v t ải quay Tuy nhiên dl, dr<0 tức bên trái ật cản Trường hợp này robot quay sang đ ều không có vật cản Trường hợp này robot quay sang ển đảm bảo thỏa mãn các yêu cầu sau: ật cản Trường hợp này robot quay sang

c n: ải quay Tuy nhiên dl, dr<0 tức bên trái

Tóm t t thu t toán xác ắt thuật toán xác định góc dẫn hướng cho robot: ật toán xác định góc dẫn hướng cho robot: định góc dẫn hướng cho robot: nh góc d n h ẫn hướng cho robot: ướng cho robot: ng cho robot: θ*(t).t).).

3.Thuật toán PID

a Giới thiệu thuật toán:

Thuật toán PID (Proportional Integral Derivative) là một trong những thuật toán quan trọng của đề tài, PID giúp cho hướng đi của robot ổn định hơn, đây cũng là mục tiêu quan trọng của đề tài đặt ra

Một bộ điều khiển vi tích phân tỉ lệ (bộ điều khiển PID- Proportional Integral Derivative) là một cơ chế phản hồi vòng điều khiển (bộ điều khiển) tổng quát được sử dụng rộng rãi trong các hệ thống điều khiển công nghiệp – bộđiều khiển PID được sử dụng phổ biến nhất trong số các bộ điều khiển phản hồi Một bộ điều khiển PID tính toán một giá trị "sai số" là hiệu số giữagiá trị đo thông số biến đổi và giá trị đặt mong muốn Bộ điều khiển sẽ thực hiện giảm tối đa sai số bằng cách điều chỉnh giá trị điều khiển đầu vào Trong trường hợp không có kiến thức cơ bản về quá trình, bộ điều khiển PID là bộ điều khiển tốt nhất Tuy nhiên, để đạt được kết quả tốt nhất, các thông số PID sử dụng trong tính toán phải điều chỉnh theo tính chất của hệ thống-trong khi kiểu điều khiển là giống nhau, các thông số phải phụ thuộc vào đặc thù của hệ thống

b Sơ đồ giải thuật

- Khởi tạo các module:Khởi

tạo các module sử dụng trênPIC: các port I/O,timer, ngắt ngoài,

- Timer1_flag_1:

Cờ báo bằng 1 khí timer1 tràn sau 5ms,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 một khi robot đang di chuyển về đích và gặp vật cản

- Tính toán tọa độ robot:

- Gửi thông tin lên máy tính:

Thông tin về góc hiện tại củarobot : (x _cur, y_cur,

angle_cur)

- Thực hiện PID trên 2 bánh xe:

4 Kết hợp 3 giải thuật trên điều khiển robot:

Một số hàm tính toán:

-Tính góc quay về đích:

>>>Đích nằm bên phải robot , lúc này robot sẽ quay một góc anpha về bên phải để hướng về đích.Cách tính góc quay cho các trường hợp như sau: gama= arctan( y target−y cur

x target−x cur)

 gama<0 (đích nằm bên trái robot)

 0<angle_cur<PI+gama

+ anpha= PI-angle_cur+gama+quay về bên trái robot

 PI+gama<angule_cur<2*PI+gama

+anpha=angle –(PI+gama)+quay về bên phải robot

 2*PI +gama<angle_cur<2*PI

+anpha=3*PI-angle_cur+gama+quay về bên trái robot

 gama≥0 (đích nằm bên phải robot)

 0<angle_cur<gama

+anpha=gama-angle_cur+quay về bên trái robot

 gama<angle_cur<PI+gama

+anpha=angle_cur-gama+quay về bên phải robot

 PI+gama<angle_cur<2*PI

+anpha=2*PI -angle_cur+gama+quay về bên trái robot

-Tính góc quay tối ưu tránh vật cản:

Điều khiển robot theo hướng có góc quay nhỏ hơn khi tránh vật cản

Các trường hợp:

>>> 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

max_point, góc Ø,và độ lớn O’L

a = Ø’ – Ø = cos-1(−x max

>>> góc anpha được tính như sau:

α = Ø’ – Ø = cos-1(x min/O ' L)– Ø , quay về phía bên trái robot

>>> robot sẽ tìm góc quay né vật sao cho anpha nhỏ nhất, nếu số lượng vật cản phía bên trái nhiều hơn thì quay về bên phải và ngược lại.Ct tính anpha:

Quay trái:

a = Ø’ – Ø = sin-1

(−x min O' L)+ Ø

Quay phải:

a = Ø’ – Ø = sin-1

(−x max O' L )+ Ø-Đ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 để hoàn toàn thoát khỏi vật.

>>>biểu diễn không gian xuất hiện vật cản trước robot , lú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 140cm.

 Cờ báo đủ không gian cho robot lách qua

>>báo khi khoảng cách H1 H2 đủ lớn cho robot lọt qua , trong đó O’H1 được

O’H2=O’M4 x cos(X)

=√(x 4−x cur)2+(y 4− y cur)2 x cos(α-tan-1(¿x 4∨ ¿

y 4¿))

 H1H2= O’H2 - O’H1

5 Cải tiến: Báo hết pin, tự động sạc khi hết pin

Bằng cách lưu tọa độ vị trí ổ điện trong nhà cho robot trên bản đồ đã cài đặtsẵn, robot sẽ xác định các thông số góc quay và chuyển hướng tiến về vị trí

có ổ điện và tự động sạc pin khi hết pin

III Ý tưởng cài đặt thuật toán:

Thuật toán được xây dựng bằng ngôn ngữ lập trình c++:

Code thuật toán:

Chương trình cho robot Hàm main.c

//*********************************************************************//

// PROJECT

// Description:

// Project Name : Smart cleaner Robot

// Note : // Neu error heading_new > heading_old

// la ban so quay ve ben phia banh trai > tang heading > error <0

// la ban so quay ve ben phai banh phai > giam heading > error >0

// P1DIR &=~BIT4 ; // set input cho encoder

P2DIR &=~(BIT6+BIT7); // Set input SRF04 giua va cong tac hanh trinh SRF04 giua > P2.6, cong tac hanh trinh P2.7

P2OUT |= (BIT6+BIT7); // set pullup resistor

P2REN |= BIT6; // Set internal pullup/pulldown resistor

P2IE |= BIT6+BIT7; // Interrupt Enable in P2.6

P2IES |= BIT6+BIT7 ; // P2.6 Interrupt flag high-to-low transition

P2IFG &= ~(BIT6+BIT7); // P2.6 IFG cleared

//********************************* Configure ********************

P1DIR &=~BIT5 ; // set input cho encoder

P1IE |= BIT5; // Interrupt Enable in P1.0 ( cau hinh cho encoder )

P1IES |= BIT5 ; // P1.0 Interrupt flag high-to-low transition

P1IFG &= ~BIT5; // P1.0 IFG cleared

//******************************************************************

configPWM() ;

GLCD_Congfig(); // Khoi tao GLCD

// xac dinh goc bak

compass_init(); // khoi tao la ban so

bak=(int)compass_heading();

// doi chan I2C

SCL1=BIT6;

// xac dinh goc setpoint

compass_init(); // khoi tao la ban so

set=(int)compass_heading(); // dat goc ban dau

display_setpoint(set) ; // hien thi setpoint

display_bak(bak); // hien thi goc se comeback

// Set toc do ban dau

TA0CTL = TASSEL_1 + MC_1+ TACLR ; // ACLK f= 12kHz

enable_interrupt(); // enable all interrupts

//================================================

while(1)

{

//**************************PID ***********************

current_value=(int)compass_heading(); // xet goc hien tai

heading_error=PID_output(); // tinh PID

if(heading_error<0) {PWM_RIGHT(R_speed,1);

display_crt((int)(compass_heading())); // hien thi goc hien tai

if(sensor1) // neu dap cong tac hanh trinh

{

// Quay huong ben trai

P2IFG &= ~BIT7; // Clean P1.0 Interrupt Flag

- Bao phủ được tối đa sàn

- Tránh lặp lại đường đi đã lau

Tài liệu tham khảo

1 Minimum Time Coverage Planning for Floor Cleaning Robot Using Line Sweep Motion - Myoung Hwan Choi

2 Efficient Area Coverage Method for a Mobile Robot in Indoor

Environments- Si Jong Kim, Jung Won Kang and Myung Jin Chung Korea Advanced Institute of Science and Technology

3 Scan Matching Online Cell Decomposition for Coverage Path Planning

in an Unknown Environment - Batsaikhan Dugarjav1, Soon-Geul

Lee1,#, Donghan Kim2,, Jong Hyeong Kim3 and Nak Young Chong4

4 Distributed coverage control- mussy swarms or neat teams of agents? Bachelor-Thesis.

5 Robot tự hành tránh vật cản sử dụng thiết bị kitnect _Đại học quốc gia thành phố hồ chí minh.

6. Chế tạo robot hút bụi tự động_Nguyễn Ngọc Nam