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

6 Tổng kết

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.

l(x) =log p(x) 1−(p(x)) p(x) = 1− 1 1 +exp(l(x)) (3.4) Áp dụng vào 3.1.2 ta được : l(mi|z1:t, x1:t) = log p(mi|x1:t, z1:t) 1−p(mi|x1:t, z1:t) l(mi|z1:t, x1:t) = l(mi|zt, xt) +l(mi|z1:t−1, x1:t−1)−l(mi). hay:

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, hệ thống phải lên tục lấy dữ liệu từ cảm biến và đưa nó vào trong bản đồ. Tuy nhiên, với việc phải lưu trữ lượng thông tin rất lớn nhận được từ cảm biến sẽ rất khó khăn để lưu trữ toàn bộ một các hiệu quả dưới dạng Grid 3D. Để làm cho bản đồ hiệu quả hơn về mặt tính toán và bộ nhớ. Một cấu trúc Octomap đã được xem xét. Octomap là một Occupancy grid map 3 chiều, cấu trúc này kết hợp các ô nhỏ hơn với nhau thành một khối tổng thể, bằng kỹ thuật ánh xạ cho không gian ba chiều sử dụng đầu vào là các point cloud và sau đó đầu ra được chia thành cấu trúc dữ liệuOctree để biểu diễn không gian [20].

Octree chia nhỏ một vùng hình khối trong không gian 3 chiều dựa theo các bát phân, tám hình khối nhỏ hơn. Một cấu trúc dữ liệu thuận tiện để lưu trữ thông tin về các vùng không gian này là dưới dạng cây, trong đó mỗi nút tương ứng với một trong các hình khối minh họa cụ thể ở hình 3.6. Ở mỗi nút, có thể là một nút con hoặc lá, nghĩa là nó có thể có tám con hoặc nó chứa một giá trị cho biết rằng ô tương ứng làfree hoặc occupied, đối với những lá chưa có giá trị ô tương ứng sẽ là unknow. Quy trình này tiếp tục cho đến khi chúng đạt đến mức độ sâu tối đa (thông thường là 16).

Cấu trúc Octree mang đến cho Octomap khả năng trở thành một bản đồ đa độ chi tiết, cho phép biểu diễn các cấu trúc hoặc đối tượng khác nhau trong môi trường với các mức độ chính xác khác nhau, ví dụ cho phép sử dụng các hình khối nhỏ hơn cho các đối tượng có độ chi tiết cao minh họa như hình 3.7. Ngoài ra, việc triển khai Octomap một

(a) Môi trường mô phỏng. (b) Octomap với độ chi tiết mỗi ô là 10cm.

(c) Octomap với độ chi tiết mỗi ô là 20cm. (d) Octomap với độ chi tiết mỗi ô là 40cm.

Hình 3.7: Môi trường mô phỏng ở hình 5.2a được đưa vào Octomap với thông số độ chi tiết khác nhau ở các hình 5.2b, 5.2b, 5.2b

cách hiệu quả sử dụng lập trình hướng đối tượng và cho phép mở rộng số lượng dữ liệu được lưu trữ trên mỗi nút, cho phép lưu trữ bổ sung ví dụ thông tin màu liên quan đến mỗi nút.

Tuy nhiên, một số vấn nảy sinh khi sử dụng cấu trúc dữ liệu Octree. Đầu tiên liên quan đến việc tìm kiếm lân cận của các lá trongOctree. Đối với các cấu trúc dữ liệu dạng lưới thông thường là một cấu trúc tuyến tính liên tục. Điều này có nghĩa là tọa độ của các lân cận có thể được tìm thấy bằng cách tăng hoặc giảm một đơn vị tọa độ của điểm truy vấn. Do đó để kiểm tra việc va chạm với vật cản trong Octree phải duyệt qua tất cả các nhánh có khả năng chứa vật cản nằm trước mặt UAV. Thứ hai, việc tiến hành các thao tác trên cây như thêm, xóa, cập nhập một nút trongOctree thường rất tốn thời gian khi độ sâu đã đạt tối đa và rất nhiều dữ liệu đã được lưu trữ. Điều này ảnh hưởng trực tiếp tới mức độ tín nhiệm củaOctomap khi sử dụng cho bài toán xây dựng quỹ đạo và tránh vật cản. Để giải quyết những vấn đề như trên, nhóm đề xuất sử dụng thêm bản độ cục bộ (local map).

(a) Bản đồ cục bộ với độ chi tiết cho mỗi ô là 40cm.

(b) Kết hợp sử dụng bản đồ cục bộ và bản đồ toàn cục.

Hình 3.8: Minh họa việc sử dụng bản đồ cục bộ trong môi trường giả lập.

3.2.2 Bản đồ cục bộ

Bản đồ cục bộ (Local map) được định nghĩa là một phần Occupancy grid map nhỏ xung quay vị trí của robot được liên tục cập nhập thay đổi theo thời gian dựa trên thông tin từ cảm biến. Việc xác định kích thước bản đồ cục bộ được dựa trên giới hạn của cảm biến sử dụng cho việc quan sát môi trường. Để giải quyết các vấn đề gặp phải ở phần 3.2.1, nhóm đề xuất sử dụng cấu trúc dữ liệu 3D Circular Buffer [21] cùng với kỹ thuật Euclidean Distance Transform minh họa ở hình 3.8.

Một circular buffer bao gồm một mảng liên tục có kích thước N và một chỉ số offset

o xác định vị trí của các thành phần trong mảng. Để cho phép xác định địa chỉ, cần xác định thêm mỗi ô có kích thước r. Điều này cho chúng ta một ánh xạ từ điểm p trong không gian 3D đến một chỉ số có giá trị nguyên x xác định một ô cụ thể từ đó xác định vị trí của ô này có nằm trong buffer hay không và địa chỉ của ô đó.

inside_V olume(x) = 0 ≤ x − o < N , (3.6)

address(x) = (x − o) mod N . (3.7) Nếu chúng ta giới hạn kích thước của mảng ở N = 2k, chúng ta có thể viết lại các hàm này sử dụng các toán tử bitwise thay vì chia:

inside_V olume(x) = ! ((x − o) & ( (2p − 1))), (3.8)

Hình 3.9: Minh họa thông tin EDT với cái khối hộp đại diện cho vật cản, mặt phẳng phía dưới có dải màu từ đỏ đến xanh lá tương ứng với rất xa đến rất gần vật cản [23].

Trong đó& là toán tử and, ∼ là toán tử phủ định và! là một toán tử not.

Để đảm bảo rằng khối bản đồ này luôn được tập trung xung quanh robot, chúng ta chỉ cần thay đổi phần offset o và xóa phần nằm ngoài khối. Điều này giúp loại bỏ nhu cầu sao chép số lượng lớn dữ liệu khi rô bốt di chuyển.

Như đã đề cập ở phần 3.1.2, mỗi ô trong bản đồ là độc lập với nhau, như vậy để xác định được robot có sắp va chạm với vật thể hay không, điều ta phải làm là kiểm tra tất cả các ô nằm đằng trước so với robot trong một giới hạn nhất định. Thay vì như vậy, để tạo điều kiện thuận lợi cho việc kiểm tra quỹ đạo va chạm nhanh chóng, nhóm đề xuất tính toán thông số Euclidean Distance Transform của khối khối bản đồ cục bộ được sử dụng. Bằng cách này, một máy bay không người lái gần đúng với hình cầu giới hạn có thể được kiểm tra xem có va chạm hay không bằng một truy vấn tra cứu.Euclidean Distance Transform có thể được định nghĩa là sử dụng một trường xác suất và tạo ra một trường vô hướng sao cho mỗi giá trị trong đầu ra là khoảng cách đến ôoccupied gần nhất trong bản đồ minh họa ở hình 3.9. Nhóm sử dụng thuật toán hiệu quả có độ phức tạp O(n)

được viết bởi Felzenszwalb và Huttenlocher [22] để tính EDT của khối, trong đó n=N3 là số ô trong khối bản đồ.

Sau khi đã chi tiết hóa các phương pháp đề tái cấu trúc bản đồ trong môi trường chưa biết trước. Việc tiếp theo cần thực hiện lên chiến lược xây dựng quỹ đạo dựa trên thông tin bản đồ này.

Xây dựng quỹ đạo di chuyển

Ở chương này, ta quan tâm đến việc xây dựng được một quỹ đạo tránh va chạm cho thiết bị qua môi trường được xây dựng ở chương 3. Một vấn đề hiện hữu khi xây dựng quỹ đạo cho robot trong môi trường đó chính là không gian môi trường chưa được xác định hoàn toàn. Tại phần 2.2, ta nhận thấy các giải thuật hoạt động chính xác, khi được cho trước không gian trạng thái toàn phần. Tuy nhiên, việc có trước thông tin toàn phần của môi trường là không khả thi do giới hạn về phần cứng. Không gian trạng thái được xây dựng cục bộ tại các trạng thái mà robot đã và đang quan sát được như trong hình 5.24c. Đây là vấn đề dẫn đến sai sót không mong muốn về va chạm khi quỹ đạo được tạo ra. Bên cạnh đó, vấn đề tiếp theo khi xây dựng một quỹ đạo chính là giảm thiểu công điều khiển cho quỹ đạo, tạo thuận tiện cho phần điều khiển robot bằng động học. Tại chương này, nhóm sẽ tập trung giải quyết 2 vấn đề trên và cố gắng đạt được một quỹ đạo tối ưu về công điều khiển, đồng thời tránh được va chạm trong môi trường chưa xác định toàn phần.

(a) Môi trường mô phỏng.

(b) Mô trường được xác định toàn phần. (c) Môi trường chỉ được xác định một phần qua cảm biến của robot.

Hình 4.1: Không gian làm việc cho bài toán xây dựng quỹ đạo. Hình bên trái là mô phỏng tượng trưng cho môi trường. Ở giữa là toàn bộ không gian trạng thái được mô tả ở chương 3. Và cuối cùng là phần không gian mà robot quan sát được trong một khoảng thời gian nhất định.

4.1 Bài toán tối ưu hóa quỹ đạo

Trong không gian trạng thái có thể tồn tại vô số các quỹ đạo từ điểm bắt đầu đến trạng thái kết thúc. Tuy nhiên, một quỹ đạo nên có công điều khiển là tối thiểu, đó là tổng các sai lệch về khoảng cách, vận tốc và gia tốc tại các điểm lân cận trên quỹ đạo là nhỏ nhất. Tại phần này, giả sử các điều kiện ràng buộc về tránh va chạm đã được cho trước, nhóm tập trung giải quyết vấn về về việc tối ưu hóa và tìm ra một quỹ đạo thỏa mãn tính chất đã được đề ra.

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

Một quỹ đạo có thể được mô tả bằng một hàm số các trạng thái của robot theo thời gian. Cách mô tả quỹ đạo này được sử dụng phổ biến trong việc điều khiển thiết bị bay không người lái ([14], [15], [13]). Dựa theo các nghiên cứu này, nhóm sẽ mô hình hóa quỹ đạo của thiết bị là một hàm số theo từng khoảng (piecewise function) đối với vùng trại thái (x, y, z) trong không gian 3 chiều. Qua đó ta có thể đạt được trạng thái toàn phần

Hình 4.2: Quỹ đạo biểu diễn bằng một hàm piecewise.

của robot, bao gồm vận tốc và gia tốc, thông qua các giá trị đạo hàm theo thời gian tương ứng.

Ta gọi quỹ đạoΦ(t) là một hàm piecewise với N đoạn.

Φ(t) =                    Φ0(t−t0), t0 ≤t < t1 Φ1(t−t1), t1 ≤t < t2 ... ΦN−1(t−tN−1), tN−1 ≤t < tN (4.1)

và điểm nối giữa đoạnΦi−1 và Φi là xi. Một quỹ đạo điển hình được thể hiện trong hình 4.2. Từng đoạnΦi được bắt đầu từ ti đến ti+1 và sau đó tiếp nối đến đoạn tiếp theo tại điểm nối tương ứng.

Trong đa số các nghiên cứu liên quan, một quỹ đạo gồm 4 chiều là Φx, Φy, Φz và

Φψ. Tuy nhiên, trong một hệ thống quadrotor, biến yaw thường không liên quan đến việc kiểm tra va chạm vật thể, hoặc kiểm tra những ràng buộc động của hệ. Vậy để đơn giản hóa bài toán, ta xem Φψ là một hằng số và loại bỏ nó khỏi việc tối ưu hóa.

Thông thường, hàm số thường được dùng để mô tả quỹ đạo là một hàm đa thức bậc

n: p(t) := n X n=0 antn =antn+...+a1t+a0 (4.2) trong đó, ai, i= 0, ..., n là các hệ số tương ứng. Theo có, một trạng thái của robot trong

theo một trục có thể được biểu diễn theo p(t) và các đạo hàm tương ứng.

v(t) = p′t=nantn−1+...+a1

a(t) = p′′t=n(n−1)antn−2+...+ 2a2

j(t) = p′′′t=n(n−1)(n−2)antn−3+...+ 6a3

(4.3)

Một trạng thái đầy đủ trongR3 có thể được mô tả bằng những đa thức độc lập theo từng trục. Một đoạn quỹ đạo Φi lúc này là một hàm vector-value Φi(t) = [pxi(t), piy(t), piz(t)]T

với pi

j, j =x, y, z là những đa thức 4.2 theo từng trục x, y, z cho đoạn thứ n.

4.1.2 Minimum Effort Trajectory

Một quỹ đạo với công điều khiển tối thiểu chính là một lời giải tối ưu cho bài toán

argmin Φ J(Φ) s.t F(Φ)

(4.4)

trong đó hàm mất mát J(Φ) thể hiện công cần để điều khiển của hệ thống. Hàm mất mát, theo đề xuất của tác giả tại [14], có thể được biểu diễn như sau :

J(Φ) = N−1 X i=0 Z ∆ti 0 ||Φ(iq)(t)||2dt (4.5)

trong đó bậc đạo hàm q = 1,2,3,4 thể hiện tương ứng với bậc của công cần tối thiểu là vận tốc, gia tốc, đạo hàm bậc 3, đạo hàm bậc bốn của quỹ đạo theo thời gian. Và

ti+1 −ti = ∆ti là các khoản thời gian được cho trước. Ta thấy rằng bậc của đa thức và bậc đạo hàm cần tối thiểu có liên hệ như sau:

n= 2q+ 1 (4.6)

Khi cho trước bậc của đa thức n và thời gian từng đoạn ti+1−ti = ∆ti, vấn đề 4.4 có thể được giải trong thời gian đa thức với các loại ràng buộc tương ứng với từng ứng dụng khác nhau. Sau đây, chúng ta sẽ xem xét cách đi tìm quỹ đạo dựa vào các waypoint không va chạm.

Hàm mất mát theo dạng ma trận