Thuật toán CCD*

3.3.1. Kế hoạch ban đầu của đƣờng bao phủ hoàn toàn

Ban đầu, thuật toán D * thực hiện tìm kiếm đầy đủ của đồ thị G (N, E, W) từ nút start S. Đối với mỗi nút n trong đồ thị G (N, E, W), D * tính toán chi phí g (n ) đến nút bắt đầu cũng như k (n) và backpointer b (n) cần thiết cho kế hoạch lại đường đi. Để tránh truy cập các nút đã ghé thăm, các hàm nhị phân visited (n) = {0, 1} được

sử dụng và giá trị của nó được lưu trữ cho mỗi nút. Để tránh chồng chéo các phần của đường dẫn bao quát đầy đủ, các hàm chồng chéo nhị phân overlapped (n) = {0, 1} được sử dụng và giá trị của nó được lưu trữ cho mỗi nút. Ban đầu, tất cả các nút được thiết lập để không được ghé qua và không chồng chéo. Các nút C hiện tại được thiết lập để bắt đầu nút S. Đường đi của đường bao phủ hoàn toàn được tạo ra bằng cách làm theo sự gia tăng của giá trị g quanh nút C hiện hành bắt đầu từ nút start S. Xung quanh các nút NC là những nút không được ghé qua và không chồng chéo mà có thể truy cập (g (n) <∞) và giữ khoảng cách từ nút C hiện tại cho các kích thước Robot vuông (2 · MR + 1 cells) trong bốn hướng thẳng qua lưới. Nút M

tiếp theo trên con đƣờng bao phủ hoàn toàn là nút với giá trị g nhỏ nhất trong số các nút xung quanh ứng viên khác. Sự kết nối giữa hai nút trong con đường

bao phủ hoàn toan được lưu trữ như là con đường phụ trợ Paux gồm các nút gần kề trong bản đồ lưới. Tất cả các nút xa khỏi con đường phụ trợ Paux trong bề mặt robot MR được thiết lập để ghé qua, và trong hai bề mặt robot (2 · MR) được đặt chồng lên nhau. Nếu không có nút ứng viên tồn tại xung quanh sau đó D * tìm kiếm thêm được thực hiện từ C. nút hiện tại D * tìm kiếm này được ghi nhận như là D *' vì nó không thay đổi giá trị của g, k, b tính toán bởi việc tìm kiếm D * đầu tiên. Nó được sử dụng như kế hoạch ban đầu và giá trị g' và b' được sử dụng thay cho g, k, b giá trị. Việc tìm kiếm của D*' dừng lại nếu (a) không ghé thăm nút đầu tiên (nút M) được tìm thấy hoặc (b) các nút được đi đến M được tìm thấy đó là ô gần vật cản mở rộng và không tồn tại nút đi đến khoảng cách n từ nút M ít hơn hoặc bằng bề mặt robot MR. Con đường ngắn nhất từ C đến M được tính theo cách điểm backpointer b' từ M đến C. nút M được thiết lập để được mới nút C hiện tại cho các thuật toán lặp kế tiếp. Các thuật toán dừng lại khi không có các nút có thể truy cập không đi đến hoặc nút không thể với tới được bề mặt robot MR từ các nút truy cập. [5]

Hình 17: Bước đi ban đầu của thuật toán CCD*

Tại bước đi ban đầu S, nó thực hiện tìm kiếm CCD* với g(n) là bé nhất với công thức tính g(n) là : g(n) = min (||Cs-Ci|| * Max(Oj, Oj))

Thuật toán 1: Tiến hành bao phủ Algorithm 1 Coverage(S)

1: C  S //Set the current node S 2: Paux  C 3: P  4: While 1 5: n  Paux , m  N, ||Cn - Cm|| < MR . ecell visited(m) = 1 6: n  Paux , m  N, ||Cn - Cm|| < 2MR .ecell overlapped(m) = 1 7: Nc  {n  N | ||Cn - CC||= (2MR + 1) . ecell and Overlapped(n) = 0 and g(n) < }

35 8: if NC ≠ 0

9: find M  NC with minimal g 10: else

11: D*'(C) and stop at visited(M) = 0 or ||cM - co|| = ecell, o  O and ∃n,

visited(n) = 0, ||cM - cn|| < MR . ecell 12: If no such node M exists

13: return P 14: end 15: end

16: Paux  Paux (C,M)

17: C  M // Set the new current node

18: P  P ∪ Paux

19: end

Mã giả:

 Bước 1:Gán nút hiện tại là S.

 Bước 2: Đưa nút hiện tại vào trong 𝑃𝑎𝑢𝑥.

 Bước 3: Xét với mọi nút thuộc 𝑃𝑎𝑢𝑥, nút m thuộc tập N, khoảng cách giữa m

và n nhỏ hơn bán kính robot thì giá trị hàm visited(m)=1.

 Bước 4:Xét với mọi nút thuộc 𝑃𝑎𝑢𝑥, nút m thuộc tập N, khoảng cách giữa m

và n nhỏ hơn 2 lần bán kính robot thì giá trị hàm overlapped(m) = 1.

 Bước 5:Đưa các nút mà khoảng cách từ nút đó tới nút hiện tại đang xét bằng 2R+1, chưa được duyệt, không bị overlap vào tập 𝑁𝑐

 Bước 6: Nếu 𝑁𝑐khác rỗng, tìm điểm M thuộc tập 𝑁𝑐 có chi phí nhỏ nhất.  Bước 7: Ngược lại, nếu 𝑁𝑐 rỗng thì sẽ tiến hành chạy lại thuật toán D* và gọi

 Bước 8: Nếu sau khi chạy lại D*’ mà không tìm được điểm M thì thuật toán kết thúc, trả lại đường đi bao phủ 𝑃.

 Bước 9: Cập nhật lại 𝑃𝑎𝑢𝑥 = 𝑃𝑎𝑢𝑥(C,M)  Bước 10: Gán nút hiện tại là M.

 Bước 11: Đường đi bao phủ 𝑃 sẽ là hợp của 𝑃 và 𝑃𝑎𝑢𝑥.

 S là nút bắt đầu (start), C là nút hiện tại đang xét (current node)..  𝑃𝑎𝑢𝑥: đường đi giữa 2 nút sẽ được lưu tạm thời vào đây.

 𝑃: là đường đi bao phủ toàn bộ bản đồ mà ta cần tìm.  N: là tập các nút, ký hiệu mỗi nút là n,m

 c : là tọa độ của nút trên bản đồ được viết tắt, thực chất c gồm 2 giá trị (x,y)  𝑐𝑛 − 𝑐𝑚 : đây là khoảng cách giữa 2 nút n và m.

 𝑀𝑅: là bán kính robot, 𝑒𝑐𝑒𝑙𝑙 là kích thước của ô.  visited(m): hàm kiểm tra xem nút m đã đi qua chưa.

 overlapped(m): kiểm tra nút m có bị duyệt ít nhất 1 lần không.  𝑁𝑐: là tập hợp những nút mà robot có thể đi đến từ vị trí hiện tại C.

 D*’ : lần chạy thứ hai của thuật toán D*, ký hiệu như này để tránh bị nhầm với lần chạy 1.

3.3.2. Lập lại kế hoạch con đƣờng bao phủ hoàn toàn

Nếu các nút ở vùng lân cận của các robot di chuyển thay vùng chiếm hữu của nó (chức năng Changed-nodes (R) trong thuật toán 2), quá trình kế hoạch lại con

đường được khởi đầu để tìm ra con đường bao phủ hoàn toàn mới từ vị trí hiện tại của robot (nút R). Thuật toán CCD* hoạch lại con đường bằng cách thực hiện D* để tính toán các giá trị mới g và sau đó bằng cách thực hiện việc tính toán con đường bao phủ hoàn toàn bằng thuật toán 1 (chức năng Coverage (R) trong thuật toán 2). Việc hoàn thành tính toán con đường bao phủ phụ thuộc vào các giá trị g tính bằng cách tìm kiếm D* và trên các nút đã được đi đến bởi các robot trong khi đi theo con đường bao phủ hoàn toàn trước khi thay đổi trong môi trường. Để theo dõi các ô được đi đến bởi các robot trong khi đi theo con đường bao phủ hoàn toàn,

hàm visitedR (n) = {0, 1} và overlapped R (n) = {0, 1} được sử dụng. Trước mỗi

lần thực hiện tính toán các giá trị của con đường bao phủ hoàn toàn các hàm

visited(n) và overlapped (n) được viết lại bằng những hàm visited R (n) và overlapped R (n) mới tương ứng. Vì thông tin lưu trữ D* về các tính toán trước số

tối thiểu của các nút được kiểm tra. Con đường bao phủ hoàn toàn P được theo dõi bởi các robot đến vị trí cuối cùng được đưa ra hay được tính toán lại nếu thay đổi mới nhất trong môi trường được phát hiện. [5]

Thuật toán 2: Chuyển động của robot – thuật toán CCD* Algorithm 2 move-robot-CCD*(S) 1: ∀n ∈ N, k n ← ∞, g(n) ← ∞, b(n) ← n, Visited(n) = 0, overlapped(n) = 0 VisitedR(n) = 0, overlappedR(n) = 0 2: R ← S // Initialization 3: while 1 4: if R = S or changed-nodes(R) 5: D* S // Re compute g n , ∀n ∈ N 6: P ← Coverage(R) 7: end 8: R ←path-following(P ) 9: for ∀n ∈ N 10: if ||cn - cR|| < MR . ecell 11: visitedR(n) = 1

38 12: if ||cn - cR|| < 2MR . ecell 13: overlappedR(n) = 1 14: visited(n) = visitedR(n) 15: overlapped(n) = overlappedR(n) 16: end 17: end Mã giả:

 Bước 1: Khởi tạo, g(n) là chi phí điểm đang xét đến điểm xuất phát, b(n) là điểm nằm trước một điểm trong đường đi, 𝑣𝑖𝑠𝑖𝑡𝑒𝑑𝑅 là đánh dấu đã visit khi đi theo đường đang xét và 𝑜𝑣𝑒𝑟𝑙𝑎𝑝𝑝𝑒𝑑𝑅 là đánh dấu đã overlap khi đi theo đường đang xét.

 Bước 2: Gán vị trí xuất phát là R.

 Bước 3: Nếu R = S hoặc hàm changed-nodes(R) được gọi.  Bước 4: Tiến hành tính lại đường đi từ đầu.

 Bước 5: Dò tìm nút R bằng hàm path-following

Ở thuật toán thứ nhất, sau khi kết thúc robot sẽ tìm ra một đường đi phù hợp bao phủ cả bản đồ. Tuy nhiên trong quá trình di chuyển, nếu môi trường thay đổi, ví dụ vị trí của vật cản thay đổi làm cho vị trí nút thay đổi theo, thì khi đó thuật toán hai có nhiệm vụ chạy lại thuật toán tìm đường thứ nhất rồi lại đi tiếp. Ở đây khi coverage tìm ra đường đi, R sẽ được thay thế dần dần các nút trong đường đi đó.

3.4. Trình bày và cải tiến thuật toán:

Để cho thuật toán đơn giản hơn, ta cho Mr = 0. Tức là kích cỡ 1 cell bằng kích thước của robot và bằng kích thước của dụng cụ quét.Vì 3 kích thước này bằng nhau nên thuật toán sẽ chỉ dừng khi visited(M) = 0. Phần này tác giả đã tham

khảo của các sinh viên nghiên cứu để triển khai thuật toán dễ dàng hơn. Sau

đây là các trường hợp gặp phải với thuật toán:

3.4.1. Trƣờng hợp robot bao phủ hoàn toàn:

Robot sử dụng thuật toán CCD* đi theo trọng số g(n) min, thực hiện replanning tại ô số 11 khi gặp vật cản tại đường đi theo kế hoạch ban đầu.

Hình 18: Thuật toán bao phủ hoàn toàn

3.4.2. Vấn đề gặp phải khi sử dụng thuật toán CCD*:

Khi triển khai thuật toán nảy sinh vấn đề trong một số trường hợp, thuật toán không bao phủ hoàn toàn. Khi đi vào đường cụt, thuật toán không thể quay lui như các ví dụ dưới đây:

Hình 19: Trường hợp thuật toán không bao phủ hoàn toàn

Thuật toán sẽ chạy theo thứ tự từ ô Start (8) - 4 - 3 - 7 - 9 -10 - 12 - 11. Ở đây khi đi vào đường cụt, sẽ lập lại kế hoạch và coi ô hiện tại là ô bắt đầu. Tại ô 11, robot sẽ không có đường đi tiếp và kết thúc thuật toán tại đây. Trong trường hợp này có thể thấy rằng thuật toán không bao phủ hoàn toàn.

3.4.3. Cải tiến thuật toán:

Khi sử dụng 2 lần D* và D*' với trọng số min nhưng không thoát được khỏi đường cụt, ở đây ta đề xuất sử dụng thêm 1 lần D*' (gọi là D*'') khi thuật toán không thể quay lui với giá trị trọng số là min, sử dụng trọng số max để thoát khỏi vòng lặp. Đường đi của thuật toán khi sử dụng D*'' sẽ như hình dưới:

Tại vị trí G khi thuật toán lập lại kế hoạch nhưng vẫn không thoát khỏi đường cụt, Thực hiện tiếp lệnh tìm kiếm D*'' với g(n) là giá trị lớn nhất cho đến khi thoát khỏi đường cụt và tìm được ô mới. Thuật toán quay về tìm kiếm D* với giá trị g(n) nhỏ nhất.

Hình 20: Cải tiến thuật toán để thoát khỏi đường cụt và bao phủ hoàn toàn.

Thuật toán 1(đã cải tiến): Tiến hành bao phủ Algorithm 1 Coverage(S)

42 and Overlapped(n) = 0 and g(n) < } 8: if NC ≠ 0

9: find M  NC with minimal g 10: Else

11: D*’(C) and stop at visited(M) = 0 //D*’ ở đây là xét với mininal g’ 12: If no such node M exists

13: D*’’(C) and stop at visited(M) = 0 //D*’’ ở đây là xét với maximum g’ 14: If no such node M exists

15: Return P 16: End

17: End 18: End

CHƢƠNG 4: CÀI ĐẶT VÀ THỬ NGHIỆMTHUẬT TOÁN CCD*

4.1. Robot trong môi trƣờng giả lập 4.1.1. Giới thiệu về ROS

a) Khái niệm

ROS là tập hợp các framework cung cấp những chức năng tương tự hệ điều hành để phát triển các phần mềm sử dụng cho robot. ROS hỗ trợ đầy đủ mọi dịch vụ tiêu chuẩn như trừu tượng hoá phần cứng,điều khiển thiết bị mức thấp, thực hiện truyền thông điệp và quản lý đóng gói. Các tiến trình trong ROS được triển khai theo cấu trúc đồ thị với mỗi tiến trình ứng với từng node có thể nhận, thông báo và kết hợp các cảm biến, bộ điều khiển, định mức hiện trạng, quy hoạch, bộ truyền động cùng nhiều thông điệp khác. Khả năng phản ứng nhanh và độ trễ thấp là những yêu cầu quan trọng với ứng dụng robot nhưng cho dù vẫn có thể tương tác với một số đoạn mã xử lý tức thời, bản thân ROS không phải là hệ điều hành thời gian thực.

Các phần mềm trong ROS được chia làm 3 nhóm:

- Các công cụ độc lập với ngôn ngữ lập trình và nền tảng phần cứng dùng để xây dựng và phân phối những phần mềm cơ bản cho ROS.

- Bộ thư viện áp dụng cho bên client để triển khai.

- Các gói chứa mã hỗ trợ dùng cho một hoặc nhiều thư viện.

Các công cụ và thư viện chính viết bằng C++, Python hoặc LISP, được phát hành theo bản quyền của BSD và đều là mã nguồn mở, miễn phí cho sử dụng trong nghiên cứu hoặc ngay cả với mục đích thương mại. Đa phần những gói công cụ còn lại phát hành dưới nhiều bản quyền mã nguồn mở đa dạng, đóng vai trò triển khai các chức năng và ứng dụng dùng chung như trình điều khiển phần cứng, mô hình hoá robot, phân loại dữ liệu, quy hoạch, nhận thức, lập bản đồ, định vị đồng thời hay mô phỏng cùng nhiều giải thuật khác.

Những thư viện chính của ROS thiết kế hướng tới hệ thống tương tự UNIX, chủ yếu vì phải phụ thuộc nhiều vào lượng lớn những phần mềm mã nguồn mở. Một trong số các hệ điều hành được hỗ trợ có thể kể đến như là Ubuntu Linux, còn nhiều hệ điều hành khác chẳng hạn Fedora, Mac OS X hay Microsoft Windows vẫn đang thử nghiệm. Trong khi đó thư viện rosjava viết bằng Java không bị giới hạn như vậy nên khiến cho các phần mềm của ROS cài đặt được trên hệ điều hành Android hay kết hợp sử dụng cùng các công cụ MATLAB, vốn có thể chạy trên Linux, Mac OS X và dĩ nhiên cả Microsoft Windows. Còn thư viện roslibjs viết bằng Javascript lại được phát triển để tương tác với hệ thống ROS thông qua bất kỳ trình duyệt web chuẩn nào.

b) Các thành phần cốt lõi

 Hạ tầng truyền thông:

Ở tầng thấp nhất này, ROS đưa ra một giao diện truyền thông điệp cung cấp giao tiếp liên tiến trình, đóng vai trò như phần trung gian hỗ trợ những tiện ích

Công bố và đăng ký truyền thông điệp vô danh: Giao tiếp là một trong các yếu tố đầu tiên cần thử nghiệm khi ứng dụng robot. Hệ thống thông điệp đã xây dựng sẵn và kiểm thử kỹ càng của ROS quản lý chi tiết quá trình giao tiếp giữa các node được phân phối qua cơ chế công bố và đăng ký vô danh. Cơ chế này giúp tiết kiệm thời gian và bắt buộc người dùng sử dụng những giao diện rõ ràng giữa các node trong hệ thống, từ đó cải thiện tính đóng gói và đẩy mạnh tái sử dụng mã nguồn. Cấu trúc của chúng được định nghĩa qua một Ngôn ngữ mô tả giao diện thông điệp.

Ghi chép rồi phát lại các thông điệp: Vì hệ thống công bố và đăng ký truyền thông điệp là vô danh và không đồng bộ nên dữ liệu có thể dễ dàng được nắm bắt và phát lại mà không cần thay đổi đoạn mã. Chẳng hạn ta có tác vụ 𝐴 đọc dữ liệu từ một cảm biến và ta đang phát triển tác vụ 𝐵 xử lý dữ liệu đưa ra từ 𝐴. ROS giúp mọi việc trở nên dễ dàng bằng cách thu thập dữ liệu 𝐴 công bố rồi ghi vào một file

để sau này công bố lại dữ liệu từ file đấy. Việc trừu tượng hoá quá trình truyền thông điệp khiến 𝐵 không biết nguồn của dữ liệu là gì, đó có thể là 𝐴 hoặc là file nhật ký. Đây là mẫu thiết kế mạnh mẽ làm tối ưu đáng kể giai đoạn phát triển và

Trình bày và cải tiến thuật toán:

Áp dụng giải thuật CCD* cho robot Kobuki