Mã giả giải thuật Informed RRT*

6 Tổng kết

2.4 Mã giả giải thuật Informed RRT*

vị tuân theo phân bố xác xuất đều, xball ∼ U(U nitBall). Sau áp dụng các phép biến đổi thích hợp để đạt được giá trị ngẫu nhiênxellipse ∼ U(Xellipse).

Vì vùng ellipsoid lấy mẫu bất kì có trạng thái mục tiêu xgoal là tiêu điểm. Thuật toán vẫn giữ nguyên tính toàn vẹn về xác xuất và tiệm cận tối ưu như RRT*.

Sau chương này, với các cơ sở lý thuyết về điều khiển và giải thuật motion planning được làm rõ. Nhóm tiến hành đề xuất các phương án giải quyết những vấn đề gặp phải của bài toán motion planning trong phạm vi nghiên cứu về UAV.

1: procedure Sample(xstart, xgoal), c

2: if c <∞ then

3: cmin ← ||xgoal −xstart||2;

4: xcentre←(xstart+xgoal)/2;

5: C ←RotationT oW orldF rame(xstart, xgoal);

6: r1 ←c/2;

7: {ri}i=2,...,n ←(pc2−c2

min)/2;

8: L←diag{r1, r2, ..., rn};

9: xball ←SampleU nitN Ball;

10: xrand ←(CLxball+xcentre)∩ X;

11: else

12: xrand ∼ U(X);

13: return xrand;

Bảng 2.5: Thủ tục lấy một trạng thái ngẫu nhiên trong một Ellipsoid. xstart và xgoal là hai tiêu điểm và clà bán kính cực của ellipsoid.

(a) Giải thuật RRT*, 8.26 second, cost = 0.76 (b) Giải thuật informed RRT*, 1 second, cost = 0.76

Hình 2.7: minh họa về lời giải của 2 giải thuật RRT* và Informed RRT*. Hai lời giải có giá trị quảng đường bằng nhau, tuy nhiên, Informed RRT* vượt trội hơn về thời gian thực thi. [12]

Tái cấu trúc môi trường chưa biết trước

Để giải quyết bài toán Motion Planning đã trình bày trong phần 2.2 đối với UAV, ta cần thiết lập một không gian trạng thái 3 chiều ∈ SE(3). Tuy nhiên, trong thực tế, khi làm việc trong môi trường chưa biết trước việc có sẵn một không gian trạng thái 3 chiều là điều không khả thi. Bởi vì tầm hoạt động của cảm biến không thể bao quát toàn bộ môi trường làm việc. Do đó, nhiệm vụ tái cấu trúc môi trường chưa biết trước là công việc phải được thực hiện đầu tiên để giải quyết đầu vào của bài toán Motion Planning. Chương 3 sẽ làm định nghĩa và mô hình hóa bài toán tái cấu trúc môi trường và xây dựng bản đồ, từ đó đề xuất phương án tái cấu trúc môi trường chưa biết trước đối với UAV.

Việc tái cấu trúc môi trường chưa biết trước có thể hiểu là xây dựng bản đồ môi trường. Đây được coi là một kỹ năng cần thiết cho một robot di động để thực sự đạt được quyền tự chủ.

“Bản đồ là một mô tả tượng trưng nhấn mạnh mối quan hệ giữa các yếu tố của một số không gian, chẳng hạn như các đối tượng, khu vực hoặc chủ đề”1

Xây dựng bản đồ có thể được định nghĩa là quá trình thu nhận mô hình không gian của môi trường thông qua thông tin cảm quan. Bản đồ môi trường cho phép robot di động tương tác chặt chẽ với các đối tượng và con người trong môi trường này. Robot có thể điều hướng an toàn, xác định các vật thể xung quanh và linh hoạt đối phó với các tình huống bất ngờ. Nếu không có bản đồ, một số hoạt động quan trọng có thể phức tạp như việc xác định vị trí của các đối tượng trong môi trường xung quanh robot và xác

định đường đi theo sau. Những vấn đề này liên quan đến tầm quan trọng của nhiệm vụ xây dựng bản đồ được thực hiện một cách chính xác, vì việc không suy luận được vị trí chính xác của robot dẫn đến thu thập các bản đồ không chính xác, Đồng thời ảnh hưởng đến chính quá trình xác định vị trí. Do đó, có sự phụ thuộc lẫn nhau giữa việc suy ra vị trí chính xác của robot và việc xây dựng một bản đồ chính xác.

3.1 Mô hình hóa bài toán

Để mô hình hóa tốt hơn vấn đề xây dựng bản đồ robot, chúng ta cần thiết lập một số giả định. Đầu tiên là robot có các cảm biến có khả năng nhận biết cho phép thu thập dữ liệu về bản thân (vị trí và định hướng) và về môi trường. Thứ hai là hệ thống cảm biến phải luôn cung cấp chính xác về vị trí và hướng của robot hoặc sai số có thể chấp nhập được (nhỏ hơn 0.2m).

Từ những giả định trên, chúng ta có thể định nghĩa xây dựng bản đồ là vấn đề xây dựng mô hình không gian của môi trường bằng hệ thống các phương pháp tính toán, dựa trên dữ liệu được cung cấp bởi hệ thống cảm biến. Hình 3.1 minh họa quá trình xây dựng bản đồ dựa trên mô hình Hidden Markov2. Trong hình minh họa này, x đại diện cho tư thế robot (vị trí và định hướng),uđại diện cho các thông tin nội tại của robot,z đại diện cho các phép đo của cảm biến được thu thập theo thời gian và m đại diện cho bản đồ đang được xây dựng lặp đi lặp lại tại mỗi khoảng thời gian.

Thách thức lớn nhất của bài toán xây dựng bản đồ là trong hầu hết các trường hợp để có được vị trí và hướng chính xác của robot bên trong môi trường của bạn, cần phải có bản đồ. Ở đây nảy sinh mâu thuẫn: để có được một bản đồ nhất quán thì cần phải có thông tin chính xác về vị trí và hướng của robot, và đối với điều này, cần phải có bản đồ môi trường - bài toán con gà quả trứng (Chicken and Egg problem). Để giải quyết xung đột này, một số nhà nghiên cứu đề xuất mở rộng vấn đề xây dựng bản đồ thành Simultaneous localization and mapping (SLAM) [4], [5], [6], [7] và [8] đã được là rõ trong giai đoạn đề cương.

Ngoài ra, bàn toán xây dựng bản đồ còn đối mặt với một số thử thách sau :

Sai số của cảm biến: Các cảm biến được sử dụng để xây dựng bản đồ robot bị ảnh hưởng bởi một số nguồn nhiễu gây ra sai số trong các phép đo của anh ta. Một vấn đề

Hình 3.1: Mô hình hóa quá trình xây dựng bản đồ.

quan trọng trong môi trường từ việc xây dựng bản đồ là làm thế nào để đối phó với sự không chắc chắn và không chính xác được tìm thấy trong dữ liệu được cung cấp bởi các cảm biến này.

Chiều của môi trường: Hãy tưởng tượng một môi trường đơn giản, chẳng hạn như một ngôi nhà với các phòng, hành lang và nhà bếp. Nếu nó được coi chỉ là một mô tả cấu trúc liên kết của các ngăn sẽ cần một vài biến để mô tả môi trường này. Tuy nhiên, khi mục tiêu là có được bản đồ chi tiết với hai (2D) hoặc ba chiều (3D), độ phức tạp để ước tính bản đồ này lớn hơn và ngoài ra, cần phải tăng không gian để lưu trữ biểu diễn của nó trong bộ nhớ máy tính. Hơn nữa, không thể quên môi trường càng rộng lớn và phức tạp, xác suất xảy ra một số lỗi trong chuyển động của robot càng cao, có thể ảnh hưởng đến chất lượng của bản đồ được xây dựng.

3.1.1 Phân loại bản đồ

Có hai cách tiếp cận chính để biểu diễn môi trường sử dụng robot di động: biểu diễn dựa trên topological maps và bằng metric maps.

Topological maps

Topological maps được biểu diễn bằng các đồ thị, mô tả sự sắp xếp của các nút (hoặc đỉnh) được nối với nhau bằng các cạnh như hình 3.2. Thông thường, các biểu đồ mô tả không gian trống để thực hiện các tác vụ. Các nút tương ứng với các vùng có thông tin cảm giác đồng nhất và các vòng cung phản ánh sự kết nối giữa các vùng này. Sử dụng

Hình 3.2: Minh họa sử dụng topological map [17].

trực quan biểu đồ để mô tả môi trường là một ý tưởng tuyệt vời vì biểu đồ là một cấu trúc nhỏ gọn để lưu trữ trong bộ nhớ của hệ thống tính toán robot và có thể được sử dụng để mô hình hóa cấu trúc hoặc liệt kê các quy trình, chẳng hạn như biểu diễn các thành phố được nối với nhau bằng đường, kết nối của bảng mạch in và những thứ khác. Vấn đề chính của biểu diễn này là thiếu một tiêu chuẩn xác định cấu trúc nào được liên kết với các đỉnh và những mối quan hệ nào được mô tả dưới dạng liên kết.

Ta thấy rằng vị trí của robot sử dụng topological map là trừu tượng, điều này có nghĩa là không có cách nào để xác định vị trí robot và hướng robot một cách rõ ràng. Tuy nhiên, có thể khẳng định nó nằm ở nút đồ thị nào hoặc ở những vùng môi trường nào.

Metric maps

metric maps với một mức độ chính xác nhất định, hình dạng của môi trường mà robot được đưa vào. Các đối tượng như tường, chướng ngại vật và lối đi dễ dàng được xác định trong cách tiếp cận này, vì bản đồ duy trì mối quan hệ tốt về địa hình với thế giới thực. Bản đồ chỉ số được thể hiện bằng point cloud maps và occupancy grids.

• point cloud maps : Point cloud là một tập hợp các điểm dữ liệu trong không gian, tập hợp rất nhiều point cloud sẽ sinh ra bản đồ như hình 3.3. Các điểm có thể đại diện cho một hình dạng hoặc đối tượng 3D. Mỗi vị trí điểm có tập hợp các tọa độ Descartes (X, Y, Z). Point cloud thường được tạo ra bởi 3D Lidar, Camera RGB-D hoặc bằng các giải thuật đo nhiều điểm trên bề mặt bên ngoài của các đối tượng xung quanh chúng. Đặc trưng của loại bản đồ này là có rất nhiều thông tin chi tiết về môi trường như kết cấu bề mặt, hình dạng vật thể. Dữ liệu bản đồ này được sử dụng cho rất nhiều mục đích khác nhau như: Phân loại và nhận dạng vật thể, tái cấu trúc, vân vân.

Hình 3.3: Minh họa point cloud maps.

• Occupancy grids : đề cập đến một nhóm các thuật toán máy tính dành cho robot di động nhằm giải quyết vấn đề tạo bản đồ từ dữ liệu đo đạc của cảm biến nhiễu và không chắc chắn, với giả định rằng tư thế của robot (vị trí và hướng) đã được biết trước. Occupancy grids lần đầu tiên được đề xuất bởi A. Elfes vào năm 1985 [18]. Occupancy grids được sử dụng chủ yếu trong navigation task của robot với rất nhiều ứng dụng khác nhau giám sát, thăm dò, cứu hộ, và các ứng dụng khác các nhiệm vụ.

3.1.2 Occupancy Grid Map

Dựa trên định nghĩa về bài toán motion planning ở phần 2.2 và các giả định, thách thức của việc tái cấu trúc môi trường chưa biết trước đã đề cập ở trên, việc sử dụng Occupancy Grid Map có lợi thế hơn cả.

Môi trường trongOccupancy Grid Mapđược biểu diễn dưới dạng một lưới các ô, trong đó giá trị của mỗi ô là trạng thái của nó có thể là free, occupied hoặc not mapped(chưa được quan sát). Kết quả có thể được sử dụng trực tiếp như một bản đồ của môi trường trong các nhiệm vụ điều hướng như lập kế hoạch đường đi, tránh chướng ngại vật và ước tính tư thế. Hình 3.4 thể hiện sự thể hiện của thước đo cảm biến độ sâu trong lưới chiếm dụng 2D.

Hình 3.4: Biểu diễn occupancy grid ở dạng 2D. Tất cả các ô của lưới được khởi tạo bằng màu xám có nghĩa lànot mapped. Từ các kết quả quan sát của cảm biến, có thể xác định các ô trắng (free) và ô đen (occupied).

Giá trị này được xác định bằng cách sử dụng phương pháp xác suất có đầu vào là khoảng cách ước tính đến các đối tượng được tính toán từ dữ liệu do cảm biến cung cấp. Mô hình đo lường được thể hiện p(zt|m, xt) hoặc có thể hiểu là xác suất thực hiện một quan sát zt cho một vùng không gian (ô) mi và vị trí của robot trên bản đồ xt với giả định đã biết trước. Từ đó, xác suất hậu nghiệm cho mỗi ô áp dụng định lý Bayes và mô hình Markov ta được.

p(mi|x1:t, z1:t) = p(zt|mi, z1:t−1, x1:t)p(mi|z1:t−1, x1:t)

p(zt|z1:t−1, x1:t) (3.1)

Việc xây dựng một bản đồ dựa trênoccupancy grid bao gồm việc xác định giá trị xác suất của mỗi ô. Giả sử rằng xác suất của các ô lân cận được tách rời, nghĩa là xác suất một ô bị chiếm không ảnh hưởng đến việc ước tính các giá trị xác suất chiếm dụng cho các ô lân cận của nó. Với giả thiết này, xác suất chiếm dụng bản đồ M có thể được tính vào tích của xác suất chiếm dụng của từng ô mi.

P(M|x1:t, z1:t) =Y

p(mi|x1:t, z1:t) (3.2)

Lấy tỷ lệ 2 xác suất p(mi|x1:t, z1:t) và p( ¯mi|x1:t, z1:t) ta được :

1: function Occupancy_grid_mapping(lt−1, xt, zt):

2: for all cells mi do

3: if mi trong vùng quan sát zt then

4: lt,i =lt1,i+inverse_sensor_model(mi, xt, zt)l0 ;

5: else lt,i =lt1,i ;

return lt,i ;

Bảng 3.1: Mã giả giải thuật xây dựng occupancy grid.

Sau đó sử dụng một phương pháp đơn giản là lấy tỷ lệ Log Odds của một xác suất.

lt,i =lt−1,i+inv_sensor_model(mi, xt, zt)−l0

(3.5)

Sau thiết lập được cơ sở lý thuyết củaoppcupancy grid, đã đến lúc xem xét thuật toán để tạo ra các bản đồ như vậy, minh họa ở bảng 3.1. Chức năng cơ bản của thuật toán này khá đơn giản. Nó xem xét mọi ô và xác định xem ô này có nằm trong thông tin của phép đo hiện tạizt hay không, có nghĩa là zt chứa thông tin về việc sử dụng ô này. Các ô nằm trong có giá trị xác suất được cập nhật bằng kết quả củainverse_sensor_modeltrong khi giá trị xác suất của các ô khác không thay đổi. Mô hình inverse_sensor_model

triển khai mô hình đop(mi|zt, xt) và do đó tính toán khả năng chiếm giữ ômi phụ thuộc vào dữ liệu đo hiện tại và tư thế của robot.

Tuy nhiên hãy nhớ rằngoccupancy grid map là một lưới chi tiết của các ô riêng lẻ. Các ô này phân chia không gian liên tục của môi trường thành các khu vực độc lập nhỏ. Một giá trị được gắn vào mỗi ô, xác định khả năng khu vực này bị vật cản chiếm đóng. Trong trường hợp của nhóm, đó là các điểm point cloud, đại diện cho khoảng cách từ cảm biến

Hình 3.5: Tia từ điểm gốc cảm biến đến điểm point cloud, ô cuối cùng được đánh dấu occupied, tất cả các ô khác dọc theo tia được đánh dấu làfree.

tới vật cản. Nói một cách trừu tượng hơn, điều này có nghĩa là từ cảm biến sẽ phát ra một đường thẳng và độ dài của đường thẳng khi đường đi của nó vượt qua chướng ngại vật đầu tiên sẽ là khoảng cách tới các điểm point cloud như hình 3.5. Vậy việc ta phải làm là nối hai ô trên bản đồ lại với nhau bằng một đường thẳng. Điều này đã được giải quyết qua giải thuậtBresenham Line[19] được xuất bản từ những năm 1965 bởi Jack Bresenham, lúc đó đang là lập trình viên của IBM.

3.2 Tái cấu trúc môi trường chưa biết trước

Việc sử dụng occupancy grid map vào các bài toán tái cấu trúc môi trường chưa biết trước cần phải xem xét rất nhiều yếu tố như : Cấu trúc dữ liệu, bộ nhớ, tốc độ tính toán, tài nguyên tính toán, khả năng tái sử dụng, .. Qua quá trình tìm hiểu và đánh giá thực nghiệm, nhóm đề xuất tiếp cận việc tái cấu trúc môi trường chưa biết trước theo hai hướng cụ thể như sau :

• Bản đồ toàn cục (Global Map) : Bản đồ này được xây dựng dựa trên việc liên tục thu thập các thông tin về môi trường của cảm biến. Ngoài khả năng sử dụng cho quá trình xây dựng quỹ đạo, lượng thông tin khổng lồ thu thập được có thể sử dụng cho rất nhiều mục đích khác.

• Bản đồ cục bộ (Local Map) : Sử dụng các thông tin tức thì từ cảm biến để xây dựng một bản đồ cục bộ. Từ đó đưa ra phản hồi một các tức thì thông tin môi trường, đảm bảo an toàn quá trình di chuyển.

Hình 3.6: minh họa về một octree lưu trữ các ôfree (màu trắng) và ôoccupied (màu đen). Mô hình 3 chiều được hiển thị ở bên trái và biểu diễn cây tương ứng ở bên phải.

3.2.1 Bản đồ toàn cục

Thông tin từ môi trường là một tài nguyên rất quan trọng không chỉ cho bài toán Motion Planning mà còn rất nhiều bài toán khác với UAV. Để thu thập thông tin này,

Mã giả giải thuật Dijkstra và A*

Minh họa sử dụng topological map [17]