Costmap trong ROS Navigation

1 Giới thiệu về Costmap

Trong quá trình robot thực hiện lập kế hoạch đường đi, chúng ta cần hiểu rằng thuật toán lập kế hoạch dựa vào những gì để tính toán ra một đường đi trên bản đồ. Nó dựa vào bản đồ toàn cục được xây dựng bởi gmapping, nhưng chỉ dựa vào một bản đồ toàn cục thô sơ là không đủ. Bởi vì bản đồ này tĩnh, không thể cập nhật thông tin về chướng ngại vật một cách liên tục. Trong môi trường thực tế, luôn có những chướng ngại vật mới không thể lường trước xuất hiện trên bản đồ hiện tại, hoặc những chướng ngại vật cũ đã bị loại bỏ khỏi bản đồ môi trường, thì chúng ta cần cập nhật liên tục bản đồ này. Đồng thời, vì bản đồ mặc định là một bản đồ ba màu đen-trắng-xám, tức là chỉ đánh dấu vùng chướng ngại vật, vùng tự do di chuyển và vùng chưa được khám phá. Robot di chuyển trên bản đồ như vậy sẽ dẫn đến đường đi được lập kế hoạch không đủ an toàn, vì robot của chúng ta cần giữ một khoảng cách an toàn nhất định với chướng ngại vật khi di chuyển, giúp robot di chuyển an toàn hơn trong bản đồ hiện tại.

Costmap đơn giản là để xử lý bản đồ này theo nhiều cách khác nhau, tiện lợi cho việc lập kế hoạch đường đi sau này. Vậy cụ thể làm thế nào để thực hiện costmap? Trong ROS, chúng ta sử dụng gói phần mềm costmap_2d để thực hiện, gói này trên bản đồ gốc đã triển khai hai bản đồ mới. Một là local_costmap, và bản đồ còn lại là global_costmap. Dựa vào tên gọi, chúng ta có thể biết được, hai costmap này một dành cho lập kế hoạch đường đi cục bộ, một dành cho lập kế hoạch đường đi toàn cục. Dù là local_costmap hay global_costmap, đều có thể cấu hình nhiều lớp, bao gồm các loại sau:

Static Map Layer: Lớp bản đồ tĩnh, lớp bản đồ cơ bản không thay đổi, thường là bản đồ tĩnh được SLAM xây dựng xong.
Obstacle Map Layer: Lớp bản đồ chướng ngại vật, dùng để ghi nhận thông tin chướng ngại vật được cảm biến phát hiện một cách động.
Inflation Layer: Lớp bung phồng, trên hai lớp bản đồ trên, thực hiện bung phồng (mở rộng ra ngoài) để tránh robot va chạm với chướng ngại vật.
Other Layers: Bạn cũng có thể tự mình triển khai costmap thông qua plugin, hiện tại đã có các plugin mã nguồn mở như Social Costmap Layer, Range Sensor Layer.

2 Tạo gói phần mềm stdr_move_base

Bạn có thể thắc mắc, tại sao nói về costmap lại nhắc đến gói phần mềm move_base? Bởi vì gói phần mềm move_base là gói phần mềm cốt lõi của điều hướng tự động trong ROS, chúng ta có thể xem hình dưới đây để hiểu rõ hơn về gói phần mềm này:

Dựa vào sơ đồ luồng logic nội bộ của move_base, chúng ta biết rằng trong quá trình lập kế hoạch đường đi, costmap là không thể thiếu. Do đó, chúng ta cần tạo trước một gói phần mềm stdr_move_base, sau đó cấu hình các tham số liên quan đến costmap, để lập kế hoạch đường đi bên trong gói phần mềm move_base có thể tìm thấy một đường đi phù hợp để điều khiển robot di chuyển đến đích đã chỉ định.

Sau khi tạo xong gói phần mềm, tiếp theo chúng ta có thể viết file launch, đặt tên là stdr_move_base.launch, nội dung của file launch như sau (có thể so sánh với file move_base.launch tiêu chuẩn để sửa đổi):

<!--
  FileName: stdr_move_base.launch
  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
  Author: corvin
  Description:
    Khởi động node move_base, tải các file cấu hình.
  History:
    20180528: khởi tạo file này.
    20180530: thêm tham số arg để đổi tên các loại frame, topics, thêm file load params.
-->
<launch>
  <arg name="odom_frame_id"   default="/map_static"/>
  <arg name="base_frame_id"   default="/robot0"/>
  <arg name="global_frame_id" default="/map"/>
 
  <arg name="odom_topic"      default="/robot0/odom"/>
  <arg name="cmd_vel_topic"   default="/robot0/cmd_vel"/>
  <arg name="map_topic"       default="/amcl/map"/>
 
  <node pkg="move_base" type="move_base" name="stdr_move_base" output="screen">
    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find stdr_move_base)/config/local_costmap_params.yaml"  command="load" />
    <rosparam file="$(find stdr_move_base)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find stdr_move_base)/config/dwa_local_planner_params.yaml"    command="load" />
    <rosparam file="$(find stdr_move_base)/config/move_base_params.yaml"            command="load" />
    <rosparam file="$(find stdr_move_base)/config/global_planner_params.yaml"       command="load" />
   
   <param name="global_costmap/global_frame"     value="$(arg global_frame_id)"/>
   <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
   <param name="local_costmap/global_frame"      value="$(arg odom_frame_id)"/>
   <param name="local_costmap/robot_base_frame"  value="$(arg base_frame_id)"/>
   <param name="DWAPlannerROS/global_frame_id"   value="$(arg odom_frame_id)"/>
 
    <!-- move base mặc định publish cmd đến topic /cmd_vel, giờ remap đến /robot0/cmd_vel -->
    <remap from="/cmd_vel" to="$(arg cmd_vel_topic)"/>
 
    <!-- move_base mặc định subscribe topic odom, giờ remap đến /robot0/odom -->
    <remap from="/odom" to="$(arg odom_topic)"/>
 
    <!-- move_base mặc định subscribe topic map, giờ remap đến /amcl/map -->
    <remap from="/map" to="$(arg map_topic)"/>
  </node>
</launch>

Khi khởi động node move_base, chúng ta có thể thấy rằng trước tiên chúng ta đã tải costmap_common_params.yaml vào không gian tên global_costmap và local_costmap, vì file cấu hình này là file cấu hình tham số costmap chung, tức là local_costmap và global_costmap đều cần cấu hình. Sau đó là local_costmap_params.yaml được cấu hình riêng cho bản đồ chi phí cục bộ, global_costmap_params.yaml được cấu hình riêng cho bản đồ chi phí toàn cục.

3 Cấu hình costmap_common_params.yaml

Trong thư mục config, tạo file costmap_common_params.yaml, các tham số cấu hình như sau:

#FileName: costmap_common_params.yaml
#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
#Author: corvin
#Description:
# File cấu hình tham số costmap chung, tức là bản đồ chi phí toàn cục và bản đồ chi phí cục bộ
# Cùng cần cấu hình các tham số, ý nghĩa của từng tham số như sau:
# robot_radius: Bán kính của robot
#
#History:
# 20180613: khởi tạo file này.
 
robot_radius: 0.2
 
obstacle_layer:
  enabled: true
  combination_method: 1
  track_unknown_space: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: laser_scan_sensor
  laser_scan_sensor: {
    sensor_frame: /robot0_laser_0,
    data_type: LaserScan,
    topic: /robot0/laser_0,
    marking: true,
    clearing: true
  }
 
inflation_layer:
  enabled: true
  cost_scaling_factor: 5.0
  inflation_radius: 0.36
 
static_layer:
  enabled: true

Dưới đây là giải thích từng tham số, để bạn có thể tự sửa đổi và điều chỉnh theo nhu cầu trong tương lai:

robot_radius: Đặt bán kính của robot, đơn vị là mét. Vì trong stdr, robot có hình tròn, nên có thể đặt trực tiếp tham số này. Nếu robot của bạn không phải hình tròn thì cần sử dụng footprint, tham số này là một danh sách, mỗi tọa độ đại diện cho một điểm trên robot, đặt tâm robot là [0,0], tìm các tọa độ nhô ra của robot theo hình dạng khác nhau.
obstacle_layer:Cấu hình lớp chướng ngại vật

enabled:Có bật lớp này không

combination_method: Chỉ có thể đặt là 0 hoặc 1, dùng để cập nhật giá trị chi phí trên bản đồ, thường đặt là 1;

track_unknown_space: Nếu đặt là false, thì giá trị chi phí trên bản đồ chỉ chia thành va chạm chết người và vùng tự do hai loại, nếu đặt là true, thì chia thành va chạm chết người, vùng tự do và vùng chưa biết ba loại. Nghĩa là nếu đặt tham số này là true, thì vùng chưa biết trên bản đồ cũng được coi là vùng có thể di chuyển tự do, khi lập kế hoạch đường đi toàn cục, có thể tham gia vào việc lập kế hoạch đường đi với một số vùng chưa khám phá, nếu bạn cần thì hãy đặt tham số này là false. Tuy nhiên, thường thì vùng chưa khám phá không nên coi là vùng có thể di chuyển tự do, vì vậy thường đặt tham số này là true;

obstacle_range:Đặt khoảng cách tối đa mà robot phát hiện chướng ngại vật, tức là các chướng ngại vật vượt quá khoảng cách này sẽ không được phát hiện, chỉ khi nào đến gần khoảng cách này mới coi là chướng ngại vật ảnh hưởng đến lập kế hoạch đường đi và di chuyển;

raytrace_range: Trong quá trình robot di chuyển, xóa chướng ngại vật trên bản đồ chi phí theo thời gian thực, cập nhật dữ liệu không gian có thể di chuyển tự do. Nếu đặt giá trị này là 3 mét, thì có nghĩa là trong 3 mét, ban đầu có chướng ngại vật, nhưng lần phát hiện này lại không có, thì cần cập nhật trên bản đồ chi phí, đánh dấu không gian chướng ngại vật cũ là không gian có thể di chuyển tự do.

observation_sources: Đặt cảm biến được sử dụng trong điều hướng, ở đây có thể dùng dấu phẩy để phân biệt nhiều cảm biến, ví dụ như laser, cảm biến va chạm, cảm biến siêu âm, v.v., chúng ta chỉ đặt cảm biến laser ở đây;

laser_scan_sensor: Thêm cảm biến laser

sensor_frame:Tên hệ tọa độ của cảm biến laser;

data_type:Kiểu dữ liệu của cảm biến laser;

topic:Tên topic mà cảm biến laser phát hành;

marking: Có thể sử dụng cảm biến này để đánh dấu chướng ngại vật không;

clearing: Có thể sử dụng cảm biến này để xóa đánh dấu chướng ngại vật và đánh dấu là không gian tự do;

inflation_layer: Lớp bung phồng, dùng để đánh dấu một lớp vùng nguy hiểm bên ngoài chướng ngại vật, cần tránh vùng nguy hiểm này khi lập kế hoạch đường đi

enabled: Có bật lớp này không;

cost_scaling_factor: Hệ số tỷ lệ áp dụng cho giá trị chi phí trong quá trình bung phồng, tất cả các ô trong bán kính nội tiếp đến bán kính bung phồng giữa chướng ngại vật và giá trị chi phí thực tế có thể sử dụng công thức sau để tính giá trị bung phồng: exp(-1.0 * cost_scaling_factor * (distance_from_obstacle – inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE – 1), trong công thức, costmap_2d::INSCRIBED_INFLATED_OBSTACLE hiện được chỉ định là 254, lưu ý: Vì cost_scaling_factor được nhân với một số âm trong công thức, nên tăng hệ số tỷ lệ sẽ làm giảm giá trị chi phí

inflation_radius: Bán kính bung phồng, lớp bung phồng sẽ bung phồng giá trị chướng ngại vật cho đến khi đạt đến bán kính này, thường đặt giá trị này bằng đường kính của khung gầm robot. Nếu robot thường xuyên va chạm với chướng ngại vật thì cần tăng giá trị này, nếu thường xuyên không thể đi qua nơi hẹp thì giảm giá trị này.

Static_layer: Lớp bản đồ tĩnh, tức là lớp bản đồ được xây dựng bởi SLAM

enabled: Có bật lớp bản đồ này không;

Qua hình dưới đây, chúng ta có thể hiểu tại sao cần đặt lớp bung phồng và ý nghĩa của nó:

4 Cấu hình global_costmap_params.yaml

Bản đồ chi phí toàn cục được sử dụng làm tham chiếu khi lập kế hoạch đường đi toàn cục. Chúng ta cần tạo file global_costmap_params.yaml trong thư mục config, file này là để cấu hình tham số cho bản đồ chi phí toàn cục, các tham số cấu hình cụ thể như sau:

#FileName: global_costmap_params.yaml
#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
#Author: corvin
#Description:
# File cấu hình tham số bản đồ chi phí toàn cục, ý nghĩa của từng tham số như sau:
# global_frame: Hệ tọa độ toàn cục trong bản đồ chi phí toàn cục;
# robot_base_frame: Hệ tọa độ gốc của robot;
#
#History:
# 20180613: khởi tạo file này.
global_costmap:
  global_frame: /map
  robot_base_frame: /robot0
  update_frequency: 0.5
  static_map: true
  rolling_window: false
  transform_tolerance: 1.0
  plugins:
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

Dưới đây chúng ta sẽ giải thích chi tiết ý nghĩa của từng tham số trong file cấu hình bản đồ chi phí toàn cục:

global_frame: Hệ tọa độ toàn cục mà bản đồ chi phí toàn cục cần chạy;
robot_base_frame: Hệ tọa độ gốc của robot trong bản đồ chi phí toàn cục, tức là hệ tọa độ gốc trên robot. Thông qua global_frame và robot_base_frame, chúng ta có thể tính toán biến đổi giữa hai hệ tọa độ, biết được tọa độ của robot trong hệ tọa độ toàn cục.
update_frequency: Tần suất cập nhật bản đồ chi phí toàn cục, thường tần suất cập nhật bản đồ chi phí toàn cục được đặt nhỏ;
static_map: Cấu hình có sử dụng bản đồ được cung cấp bởi map_server để khởi tạo không, bản đồ toàn cục thường là tĩnh, cần đặt là true;
rolling_window: Có cần cửa sổ cuộn khi robot di chuyển không, luôn giữ robot ở vị trí trung tâm của cửa sổ hiện tại;
transform_tolerance: Độ trễ tối đa có thể chịu được khi chuyển đổi hệ tọa độ;
plugins: Sử dụng ba plugin sau trong global_costmap để hợp nhất ba lớp khác nhau, lần lượt là static_layer, obstacle_layer và inflation_layer, hợp nhất thành một master_layer để lập kế hoạch đường đi toàn cục.

5 Cấu hình local_costmap_params.yaml

Tham số cấu hình bản đồ chi phí cục bộ được xây dựng chủ yếu để sử dụng cho lập kế hoạch đường đi cục bộ. Chúng ta có thể tạo file local_costmap_params.yaml trong thư mục config, nội dung đầy đủ như sau:

#FileName: local_costmap_params.yaml
#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
#Author: corvin
#Description:
# Các tham số cần cấu hình cho bản đồ chi phí cục bộ, ý nghĩa của từng tham số như sau:
# global_frame: Hệ tọa độ toàn cục trong bản đồ chi phí cục bộ;
# robot_base_frame: Hệ tọa độ gốc của robot;
#
#History:
# 20180613: khởi tạo file này.
 
local_costmap:
  global_frame: /map_static
  robot_base_frame: /robot0
  update_frequency: 5.0
  publish_frequency: 3.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.05
  transform_tolerance: 0.5
  plugins:
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

Dưới đây là giải thích chi tiết ý nghĩa của từng tham số:

global_frame: Hệ tọa độ toàn cục trong bản đồ chi phí cục bộ, thường cần đặt là odom_frame, nhưng vì stdr không có hệ tọa độ này, tôi lấy /map_static để thay thế;
robot_base_frame: Hệ tọa độ gốc của robot;
update_frequency: Tần suất cập nhật bản đồ chi phí cục bộ;
publish_frequency: Tần suất xuất bản bản đồ chi phí cục bộ;
static_map: Bản đồ chi phí cục bộ thường không đặt là bản đồ tĩnh, vì cần phát hiện xem có chướng ngại vật động mới gần robot không;
rolling_window: Sử dụng cửa sổ cuộn, luôn giữ robot ở vị trí trung tâm của bản đồ cục bộ hiện tại;
width: Chiều rộng của cửa sổ cuộn, đơn vị là mét;
height: Chiều cao của cửa sổ cuộn, đơn vị là mét;

resolution: Độ phân giải của bản đồ, độ phân giải này có thể lấy từ file cấu hình của bản đồ được tải;

transform_tolerance: Độ trễ tối đa có thể chịu được khi chuyển đổi hệ tọa độ trong bản đồ chi phí cục bộ;
plugins: Trong bản đồ chi phí cục bộ, không cần lớp bản đồ tĩnh, vì chúng ta sử dụng cửa sổ cuộn để quét chướng ngại vật liên tục, vì vậy chỉ cần hợp nhất hai lớp bản đồ (inflation_layer và obstacle_layer) là đủ, bản đồ sau khi hợp nhất được dùng để lập kế hoạch đường đi cục bộ;

6 Xem global_costmap và local_costmap trong RViz

·Chúng ta đã giới thiệu về global_costmap và local_costmap trong một thời gian dài, nhưng có lẽ mọi người vẫn chưa có cảm nhận và nhận thức trực quan về chúng, chúng ta sẽ xem hai loại bản đồ chi phí này trong Rviz như hình dưới đây:

7 Tài liệu tham khảo

[1] Trang chủ ROS Wiki của costmap_2d. http://wiki.ros.org/costmap_2d/

[2] Trang chủ ROS Wiki của gói phần mềm move_base. http://wiki.ros.org/move_base

[3] Giới thiệu về costmap trong Hướng dẫn Hệ điều hành Robot. https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter10/10.3.html

[4] Ghi chú học tập gói phần mềm costmap_2d. https://blog.csdn.net/sonictl/article/details/51518492

[5] Giới thiệu về lớp inflation của costmap_2d. https://blog.csdn.net/x_r_su/article/details/53420209

[6] Ghi chú học tập ros-navigation (bốn) costmap chi phí bản đồ_ khi sử dụng bản đồ chi phí để kiểm tra va chạm, không cần bung phồng chướng ngại vật-CSDN blog

Tài liệu tham khảo

[1] Học costmap chi phí bản đồ trong STDR http://www.corvin.cn/829.html

Thẻ: ROS costmap_2d move_base Navigation path_planning

Đăng vào ngày 20 tháng 6 lúc 08:43

Thành phố Cuồng loạn