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.
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (556.06 KB, 23 trang )
TRÍ TUỆ NHÂN TẠO
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ị Mai1 , Đỗ Vũ Hiệp2, Nguyễn Văn Quý3
Đặng Vũ Đức4, Nguyễn Cao Thùy Trang5
Viện Đào Tạo Quốc Tế- Đại Bách Khoa Hà Nội
1.
Từ khóa : Robot lau nhà bằng thuật toán cày ziczag, cover path planning,…
______________________________________________________________
Tóm tắt: 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 zic-zag 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.
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:
-
Phải đi qua các điểm trong vùng hoạt động nói cách khác đường đi của
robot phải bao phủ toàn bộ vùng hoạt động.
Robot không được đi các vùng chồng chất lẫn nhau.
Robot phải hoạt động tuần tự theo các đường đi không lặp lại.
Robot phải tránh được các vật cản xuất hiện trong vùng hoạt động.
Quỹ đạo chuyển động của robot cần đơn giản
Đường đi của robot cần được tối ưu hóa dựa vào những điều kiên nhất
đị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ì.
****Bảng hành động:
[A, sạch ] ------------------------------> Di chuyển sang phải
[A, bẩn]
-------------------------------> Hút bụi
[B, sạch]
------------------------------> Di chuyển sang trái
[B, bẩn]
-------------------------------> Hút bụi
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:
-
Báo hết pin, tự động sạc điện khi hết pin
Tránh vật cản di động, vật cản cố định.
Ổn định hướng đi cho robot
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ích
sà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.
TRANG 2
Hình vẽ minh họa:
Trong đó: Khoảng cách giữa hai đường chạy liên tiếp d được xác định bằng
vùng hút bụi của robot.
Điều khiển vòng kín của robot chạy theo đường ziczag theo đường tham
chiếu trên nền , Đường tham chiếu này dựa trên viền của những viên gạch
lót nền.Sử dụng camera để dò đường viền này. Kết hợp với việc gắn
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 robot
với quỹ đạo tham chiếu.
b. Sơ đồ thuật toán di chuyển ziczag
Code:
void Zigzag()
{
Cambien();
TRANG 3
s1=Cambien();
Tien();
if(s1==1)
{
k=1;
}
while(k)
{
Dung();
delay_ms(500);
k1=1;
k=0;
}
while(k1)
{
Lui();
delay_ms(500);
71
Dung();
k2=1;
k1=0;
}
while(k2)
{
Retrai();
delay_ms(500);
Dung();
k3=1;
k2=0;
}
TRANG 4
while(k3)
{
Tien();
delay_ms(500);
k4=1;
k3=0;
}
while(k4)
{
Retrai();
delay_ms(500);
Dung();
k4=0;
}
}
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>d max , ngõ ra của
sensor= -1.
Đây là các trường hợp tránh vật cản cho robot:
TRANG 5
•
Phát hiện 3 vị trí (hình a)
Góc dẫn hướng tránh vật cản θa(t):
θ *a(t) = θ(t) +sgn(dl – dr)( - ε )
Với: ε = tan-1((dcosα - dc)/dsinα) ( |ε| < π/2 )
d = max(dl,dr)
sgn(x) =
•
Phát hiện 2 vị trí(hình b+c+d)
Ta có hình b: dc> drcosα và dl<0. Kết luận : bên trái không có vật cản.
θ *a(t) = θ(t) +sgn(dc – dr)( - ε )
Hình c: dc
cảm nhận được .Để đảm bảo tránh vật cản , robot nên quay sang trái
một góc ( – ε)
Hình d: dc<0 robot không cần quay.
θ *a(t) = θ(t)
TRANG 6
•
Phát hiện một vị trí ( hình e+f)
Hình e: dc<0. Robot không cần quay.
θ *a(t) = θ(t).
Hình f: dc>0. Robot cần phải quay. Tuy nhiên dl, dr<0 tức bên trái và
bên phải đều không có vật cản. Trường hợp này robot quay sang trái
một góc .
θ *a(t) = θ(t)+
Giải thuật điều khiển đảm bảo thỏa mãn các yêu cầu sau:
-Luôn cập nhật thông tin về vị trí và góc của robot trong mặt phẳng.
-Xử lý tín hiệu cảm biến và tính góc tránh vật cản cho robot θ *a(t).
-So sánh các giá trị góc lệch, góc hướng đích và góc tránh vật cản để
xác
định góc dẫn hướng thích hợp θ *(t).
Sơ đồ mạch điều khiển:
Giải thuật điều khiển tránh vật cản:
TRANG 7
Tóm tắt thuật toán xác định góc dẫn hướng cho robot: θ*(t).
Begin
If dc < 0 and dl.dr > 0
Then θ*(t) = θ*a(t)
Else if dc > 0
Then if θ*t(t) ≥ θ*a(t) ≥ θ(t), or θ*t(t) ≤ θ*a(t) ≤ θ(t)
Then θ*(t) = θ*t(t)
Else θ*(t) = θ*a(t)
End if
Else if dl < 0 and θ*t(t) ≥ θ(t), or dr < 0 and θ*t(t) ≤ θ(t)
Then θ*(t) = θ*t(t)
Else θ*(t) = θ*a(t)
End if
End
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ữa
giá 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
TRANG 8
-
-
-
-
-
-
-
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,
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ủa
robot : (x _cur, y_cur,
angle_cur)
Thực hiện PID trên 2 bánh
xe:
TRANG 9
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:
TRANG 10
>>>Đí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( )
gama<0 (đích nằm bên trái robot)
0
+quay về bên trái robot
PI+gama
+quay về bên phải robot
2*PI +gama
+quay về bên trái robot
gama0 (đích nằm bên phải robot)
0
+quay về bên trái robot
gama
+quay về bên phải robot
PI+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:
Vật cản nằm bên trái robot
>>> 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.
TRANG 11
a = Ø’ – Ø = cos-1 – Ø , quay về phía bên phải robot.
Vật cản nằm bên phải robot
>>> góc anpha được tính như sau:
α = Ø’ – Ø = cos-1 – Ø , quay về phía bên trái robot.
Vật cản nằm giữa đường 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 + Ø
Quay phải:
a = Ø’ – Ø = sin-1 + Ø
-Đ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.
TRANG 12
Quãng đường 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.
Đi bên phải vật cản:
O’H=OM1 x cos(α – θ)
= x cos(α-tan-1())
Đi bên trái vật cản:
O’H=OM1 x cos(α – θ)
= x cos(α-tan-1())
Cờ báo có vật cản và đường trống
>>>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
TRANG 13
>>báo khi khoảng cách H 1 H2 đủ lớn cho robot lọt qua , trong đó O’H 1 được
tính như trong hàm 1 đoạn an toàn, O’H 2 tương tự:
O’H2=O’M4 x cos(X)
= x cos(α-tan-1())
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 đặt
sẵ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
// Neu error >=0 --> 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
//*********************************************************************
TRANG 14
//
Define
#include "msp430g2553.h"
#include "I2C.h"
#include "UART.h"
#include "compass.h"
#include "PID.h"
#include "math.h"
#include "GLCD.h"
#include "PWM.h"
#include "SRF05.h"
#define L_speed 5000
#define R_speed 5000
#define xoay 8000
#define lui 8000
//******************************************************************
//
Varible
int current_value=0,setpoint=0,heading_error=0,bak=0,set=0;
int temp=1;
double hold=0;
int dir=1;
int i=0;
int offset=0;
char sensor1=0;
int sensor2=0;
unsigned int stop=1,dem=1,dem1=0;
int SCL1=BIT2;
int SDA1=BIT3;
void stop_enable(void)
{ if(stop==0)
TRANG 15
{
PWM_LEFT(lui,0);
PWM_RIGHT(lui,0);
__delay_cycles(4000000);
stop=1;
}
}
void stop_enable_1(void)
{ if(stop==0)
{
PWM_LEFT(0,0);
PWM_RIGHT(lui,0);
__delay_cycles(1500000);
PWM_LEFT(lui,1);
PWM_RIGHT(0,0);
__delay_cycles(1500000);
stop=1;
}
}
//******************************************************************
main()
{
WDTCTL = WDTPW + WDTHOLD;
BCSCTL1 = CALBC1_16MHZ;
DCOCTL = CALDCO_16MHZ;
BCSCTL3 |= LFXT1S_2;
// Set P2.6 P2.7 in_output
P2SEL &=~(BIT6+BIT7);
P2SEL2 &=~(BIT6+BIT7);
//======================
TRANG 16
P1DIR &=~(BIT0+BIT1); // Set input SRF04 trai, phai. trai-->P1.0,phai-->P1.1
P1OUT |= (BIT0+BIT1);
// 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;
SDA1=BIT7;
// xac dinh goc setpoint
compass_init(); // khoi tao la ban so
set=(int)compass_heading(); // dat goc ban dau
TRANG 17
display_setpoint(set) ; // hien thi setpoint
display_bak(bak); // hien thi goc se comeback
// Set toc do ban dau
PWM_LEFT(L_speed,1) ;
PWM_RIGHT(R_speed,1) ;
// __delay_cycles(32000000);
TA0CCTL0 = CCIE; // CCR0 interrupt enabled
TA0CCR0 = 10000;
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); PWM_LEFT(L_speedheading_error,1) ;
}
if(heading_error>0)
{PWM_RIGHT(R_speed+heading_error,1);PWM_LEFT(L_speed,1) ;
}
//===============================================================
==========
display_crt((int)(compass_heading())); // hien thi goc hien tai
if(sensor1) // neu dap cong tac hanh trinh
{
// Quay huong ben trai
if(temp==1)
{
setpoint=bak;
TRANG 18
display_setpoint(setpoint) ; // hien thi setpoint
PWM_LEFT(xoay,0);
PWM_RIGHT(0,0);
if(sensor2==1) //delay cho SRF04 giua
{
__delay_cycles(15000000);
sensor2=0;
}
else
// delay cho cong tac hanh trinh
__delay_cycles(8000000);
if((P1IN&BIT0)!=0) // ko co vat can ben trai
{
while(!(abs(current_value-setpoint)<=4)) // tim goc quay
{ stop_enable();
current_value=(int)compass_heading();
PWM_LEFT(0,1);
PWM_RIGHT(xoay,1);
display_crt((int)(compass_heading())); // hien thi goc hien tai
}
}
else // co vat can ven trai
{
while(!(abs(current_value-setpoint)<=4)) // tim goc quay
{ stop_enable();
current_value=(int)compass_heading();
PWM_LEFT(7000,0);
PWM_RIGHT(8000,1);
display_crt((int)(compass_heading())); // hien thi goc hien tai
}
}
TRANG 19
temp=0;
sensor1=0;
}
// Quay huong ben phai
else
{
setpoint=set;
display_setpoint(setpoint) ; // hien thi setpoint
PWM_LEFT(0,0);
PWM_RIGHT(xoay,0);
if(sensor2==1)
{
__delay_cycles(15000000);
sensor2=0;
}
else
__delay_cycles(10000000);
if((P1IN&BIT1)!=0) //ko co vat can ben phai
{
while(!(abs(current_value-setpoint)<=4)) // tim goc
{
stop_enable();
current_value=(int)compass_heading();
PWM_LEFT(xoay,1);
PWM_RIGHT(0,1);
display_crt((int)(compass_heading())); // hien thi goc hien tai
}
}
else // co vat can ben phai
{
TRANG 20
while(!(abs(current_value-setpoint)<=4)) // tim goc
{ stop_enable();
current_value=(int)compass_heading();
PWM_LEFT(8000,1);
PWM_RIGHT(7000,0);
display_crt((int)(compass_heading())); // hien thi goc hien tai
}
}
temp=1;
sensor1=0;
}
}
stop_enable_1();
}
}
#pragma vector=PORT2_VECTOR
__interrupt void Port_2 (void)
{
sensor1=1;
if((P2IN&BIT6)==0)
P2IFG &= ~BIT6;
// Clean P2.0 Interrupt Flag
else
{
sensor2=1;
P2IFG &= ~BIT7;
// Clean P1.0 Interrupt Flag
}
}
#pragma vector=PORT1_VECTOR
TRANG 21
__interrupt void Port_1 (void)
{
dem1++;
if(dem1==3)
{
dem=1;
dem1=0;
}
P1IFG &= ~(BIT4+BIT5);
// Clean P1.0 Interrupt Flag
}
#pragma vector=TIMER0_A0_VECTOR
__interrupt void TIMER0(void)
{
stop=dem;
dem=0;
}
IV. Đánh giá:
Ưu điểm:
-
Bao phủ được tối đa sàn
Tránh lặp lại đường đi đã lau
Tránh được vật cản cố định và di động
Với việc sử dụng thuật toán PID có thể ổn định được hướng đi cho robot giúp
giảm thiểu chi phí tối đa, và tránh những sai số không cần thiết trong quá trình
di chuyển của robot.
Nhược điểm:
-
Chỉ phù hợp với môi trường ít phức tạp và có kích thước vật cản đủ lớn.
TRANG 22
Tài liệu tham khảo
1.
2.
3.
4.
5.
6.
Minimum Time Coverage Planning for Floor Cleaning Robot Using Line
Sweep Motion - Myoung Hwan Choi
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
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
Distributed coverage control- mussy swarms or neat teams of agents?
Bachelor-Thesis.
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.
Chế tạo robot hút bụi tự động_Nguyễn Ngọc Nam
TRANG 23
