ROS Navigation(3. Localization)

이 포스트는 theconstructsim.com의 ROS Navigation in 5 Days 를 참고하였습니다.

이번 포스트에서 다룰 내용은

 

• ROS Naviagation에서 위치추정(Localization)이란 무엇인가?
• 위치추정은 어떻게 동작하는가?
• ROS의 위치추정에서 우리는 무엇을 하는가?

이번 course에서는 turtlebot simulation package를 사용합니다. 아래 링크를 참조하세요

emanual.robotis.com/docs/en/platform/turtlebot3/simulation/

시뮬레이션 모델은 Waffle 모델을 사용합니다.

waffle 모델을 사용하기 위해서는 환경 변수를 추가하거나, bash shell에 해당 명령을 입력하면 됩니다.

 

$ export TURTLEBOT3_MODEL=waffle

 

또는, ~/.bashrc 를 수정하여 맨 밑에 export TURTLEBOT3_MODEL=waffle 를 추가합니다.

 

여기서 말하는 내비게이션이란, 우리가 일반적으로 차를 운전하면서 사용하는 내비게이션처럼 단순히 길만 찾는것이 아닌, 해당 경로를 따라 주행까지 하는 것을 말합니다.

 

 

 

 

이전의 Basic Concepts와 Mapping포스트에서, ROS Navigation의 로봇 위치추정의 중요성에 대해서 몇 번 언급했었습니다. 로봇이 지도에서 움직일 때, 우리는 로봇이 지도에서 어디에 있는지, 그리고 어느 방향을 보고있는지를 알아야합니다. 센서의 데이터를 통해 로봇의 자세(위치와 방향을 합친것을 자세, Pose라고 합니다)를 알아내는 것을 위치추정이라고 합니다.

Rviz를 사용한 위치추정 시각화

이번에는 RViz의 요소 중 3가지를 알아볼 것입니다.

• LaserScan
• Map
• PoseArray

---

- 연습 3.1.

 

a) 다음 명령으로 내비게이션 node를 실행합니다.

 

$ roslaunch turtlebot3_gazebo turtlebot3_house.launch

$ roslaunch turtlebot3_navigation roslaunch turtlebot3_navigation turtlebot3_navigation.launch

 

그러면 rivz가 자동으로 실행되는데, 최소화 해두고 새로 rviz를 실행하여 필요한 플러그인만 추가해봅니다.

 

• Add를 클릭하고, PoseArray를 추가한 뒤, 해당 topic을 /particlecloud 로 설정해줍니다.
• Add를 클릭하고, Robot Model을 추가해줍니다.
• 시각화가 제대로 이루어지기 위해서는 Fixed Frame이 map으로 설정되어야 합니다.

 

- 연습 3.1. 끝

---

 

---

- 연습 3.2. 

 

연습 3.1.에서 그대로 실행합니다

 

로봇을 키보드로 조종하기 위해 다음을 실행합니다

$ roslaunch turtlebot3roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

 

a) 로봇의 현재 환경을 시각화하기 위해 LaserScan과 Map을 추가해봅니다

b) rivz 상단의 2D Pose Estimate를 클릭하여 로봇의 현재 위치를 조절해줍니다(이전에도 말했지만, 정확하게 할 필요는 없습니다)

c) 시각화된 데이터의 색깔, 크기 등을 조절하여 구별하기 쉽게 설정해줍니다.

d) 로봇을 키보드로 움직여봅니다.

 

초기에는 이와같이 정확하게 위치를 추정하지는 못하는 상태입니다

 

로봇이 움직이면서 자신의 위치를 더 정확하게 추정하게 됩니다.

 

 

- 연습 3.2. 끝

---

Monte Carlo Localization (MCL)

로봇은 우리가 항상 우리가 원하는대로 정확하게 움직이지는 않기 때문에, 자신이 이동 명령을 받았을 때, 이동하게 될 것 같은 위치들을 무작위로 생성합니다. 우리는 이 예측들을 파티클이라고 합니다. 각각의 파티클은 미래의 자세에 대한 데이터를 나타냅니다. 로봇이 센서를 통해 주변 환경을 관측할 때, 현재 환경과 매치가 되지 않는 파티클들은 무시하며, 현재 환경과 비슷하게 여겨지는 파티클을 생성합니다. 이를 반복하여 마지막에는 대부분의 파티클들이 현재 로봇의 자세와 비슷한 자세의 파티클들로 변하게 됩니다. 즉, 더 많이 움직일수록 더 센서로부터 많은 데이터를 얻게되며, 따라서 위치 추정은 더 정확하게 됩니다. 이 파티클들이 RViz에서 봤던 작은 화살표들입니다. 이 위치 추정 방법은 Monte Carlo Localization algorithm 또는 particle filter localization이라고 합니다.

The AMCL(Adaptive Monte Carlo Localization) package

AMCL 패키지는 2D공간에서 움지깅는 로봇의 위치를 추적하기 위해 MCL 시스템을 사용하는 amcl node를 제공합니다. 이 node는 레이저로부터의 데이터, 레이저 데이터 기반의 지도, 로봇의 좌표변환 데이터들을 subscribe하고, 지도에서 추정되는 위치를 publish 합니다. 위치 추정을 시작할 때, amcl node는 주어진 파라미터 값에 따라 파티클 필터를 초기화합니다.

 

- 참고 : Monte Carlo Localization algorithm 앞에 "Adaptive"가 추가되었습니다. 이 node는 이 알고리즘에서 사용되는 일부 파라미터들을 상황에 맞게(adaptive) 설정할 수 있기 때문입니다. 이 파라미터들은 차후에 더 설명할 것이며, 관련 이론은 soohwan-justin.tistory.com/14?category=941681 를 참조하세요

 

기본적으로, 이전의 연습들에서 했던 것들은 다음과 같습니다.

• 이전에 미리 설정된 amcl node를 launch했습니다.
• 초기의 위치를 2D Pose Estimate(이 tool을 사용하면 /initialpose topic을 publish합니다)를 사용하여 초기화했습니다. 
• 다음으로, 로봇이 움직이면 amcl node는 laser topic(/scan), map topic(/map), transform topic(/tf)으로 publish된 데이터들을 읽기 시작하고, 로봇이 있을 것으로 추정되는 자세 데이터인 /amcl_pose 와 /particlecloud topic들을 publish했습니다.
• 마지막으로, RViz를 통해, 위의 node에서 publish된 /particlecloud topic에 액세스하여 현재 로봇의 자세로 추정되는 화살표들을 통해 이를 시각화할 수 있었습니다.

다음으로 생각해봐야 할 것은, 이 node는 지도를 어디서 얻는가? 입니다. 이전의 포스트에서, 우리는 지도 파일로부터 지도의 데이터를 제공해주는  map_server node에 대해 알아봤습니다. 기본적으로 이제 우리가 하려고 하는 것은 지도 데이터를 amcl node로 제공해주는 그 기능을 호출하는 것입니다. 우리는 이를 연습 3.1.에서 실행했던 파일을 통해 확인할 수 있습니다.

 

turtlebot3/turtlebot3_navigation/launch/turtlebot3_navigation.launch

line 14를 보면, 이 부분에서 제공받은 지도 파일(arg map_file, line4)을 사용하여 map_server node를 launch하며, 이를 지도 데이터로 바꿔줍니다. 그리고, 이 데이터를 /map topic으로 publish하며, 이 데이터는 amcl에서 위치 추정을 위해 사용됩니다.

 

 

---

- 연습 3.3.

 

- 기존에 실행중이던 navigation node등을 다 종료하세요

 

a) turtlebot3_nagivation 패키지의 turtlebot_navigation.launch 파일을 확인해보세요. 이 파일을 확인해보면, 다른 launch 파일인 amcl.launch 파일을 호출하는 것을 확인할 수 있습니다 이 amcl파일에 있는 파라미터 값들을 수정하여 내비게이션을 설정할 수 있습니다.

 

b) my_amcl_launcher 패키지를 생성하세요. 이 패키지 내부에 launch라는 디렉토리를 생성하고, 이 디렉토리 안에 change_map.launch 파일을 만들고, 여기에 기존의 turtlebot_navigation.launch 파일의 내용을 복사하고, my_amcl.launch 파일을 만들어 turtlebot3_navigataion 패키지의 amcl.launch파일을 복사하세요.

 

c) 방금 생성한 change_launch.launch파일에서 지도를 호출하는 부분을 수정하여 다른 지도를 읽도록 해보세요. slam을 실행하여 새로운 지도를 작성해도 되고, turtlebot3패키지에서 기본으로 제공하는 지도를 사용해도 됩니다.

 

d) RViz를 통해 확인해보세요

 

다음과 같이 지도가 잘 맞지 않는 결과가 나오면 됩니다.

 

---

 

하드웨어 요구사항

우리는 이전의 포스트에서 로봇의 환경에서 위치 추정이 제대로 되기 위해서는 configuration이 매우 중요하다고 했습니다. 이를 위해서는 다음 3가지가 필요합니다

• 좋은 레이저 센서 데이터
• 좋은 오도메트리 데이터
•  레이저 기반의 좋은 지도 데이터

 

좌표변환

이 또한 이전 포스트에서 로봇의 프레임과 레이저 센서 프레임 사이의 올바른 좌표 변환이 필수적이라는 것을 알았습니다. 로봇은  위치를 추정하기 위해 지속적으로 레이저 데이터를 사용하여 계산합니다. 

 

amcl은 레이저 스캔으로부터 오는 데이터를 오도메트리 프레임으로 변환합니다. 따라서, 좌표 변환 트리를 통해 레이저 스캔 데이터가 publish된 프레임으로부터 오도메트리 프레임으로 바뀌는 경로가 반드시 존재해야 합니다.

 

우리가 실행하는 내비게이션의 tf tree는 다음과 같습니다.

 

 

 

 

AMCL node를 위한 launch file 생성

이전의 맵핑에 대한 포스트에서 보았다시피, 우리는 amcl node를 시작하기 위해 launch 파일이 필요합니다. 이 node 또한 highly customizable하며, 성능 개선을 위해 파라미터를 임의로 수정할 수 있습니다. 이 파라미터들은 launch파일 또는 YAML파일에서 설정할 수 있습니다. 모든 파라미터들의 목록 및 설명은 http://wiki.ros.org/amcl를 참조하세요. 파라미터들에 대한 이론은 Probablistic Robotics를 참조하세요.

먼저, 가장 중요한 파라미터들을 확인해보겠습니다.

 

General Parameters

• odom_model_type (default: "diff"): 사용할 로봇의 모델을 정의합니다. 가능한 옵션에는 "diff", "omni", "diff-corrected", "omni-corrected"가 있습니다.
• odom_frame_id (default: "odom"): 오도메트리 프레임 이름을 정의합니다.
• base_frame_id (default: "base_link"): 로봇의 베이스 프레임 이름을 정의합니다
• global_frame_id (default: "map"): 위치 추정 시스템에 의해 publish되는 좌표계의 이름을 정의합니다.
• use_map_topic (default: false): node가 topic 또는 service call에 의해 지도 데이터를 받을지를 정의합니다.

 

 

다음 파라미터들은 파티클 필터의 성능을 설정하기 위한 파라미터들입니다.

• min_particles (default: 100): 파티클 필터에서 사용할 최소 파티클 수 입니다.
• max_particles (default: 5000): 파티클 필터에서 사용할 최대 파티클 수 입니다.
• kld_err (default: 0.01): 실제 분포와 추정 분포 사이의 최대 오차를 설정합니다.
• update_min_d (default: 0.2): 필터 업데이트를 위해 로봇이 움직여야 할 최소한의 linear distance입니다(미터 단위).
• update_min_a (default: π/6.0): 필터 업데이트를 위해 로봇이 움직여야 할 최소한의 angular distance입니다(라디안 단위).
• resample_interval (default: 2): 리샘플링 되기 이전에 파티클 필터를 업데이트하는 횟수를 정합니다.
• transform_tolerance (default: 0.1): 이 변환이 publish된 후 언제까지 유효한지를 나타내는 시간을 정합니다(초 단위).
• gui_publish_rate (default: -1.0): 시각화를 위해 각각의 스캔과 경로가 표시될 시간을 정합니다(헤르츠 단위) -1.0이면 해당 기능을 비활성화 합니다.

---

연습 3.4.

 

a) 연습 3.3에서 만들었던 패키지들을 사용합니다. change_map.launch 파일을 다음과 같이 수정합니다.

 

b) min_particles과 max_particles 파라미터들을 수정해봅니다. 각각 1, 5로 설정해봅니다.

c) node를 실행해보고, 결과를 확인합니다.

 

다음과 같이 파티클이 줄어든 모습을 볼 수 있습니다.

 

- 연습 3.4 끝

---

위와 같이 파티클의 수를 줄이게 되면, 위치 추정 알고리즘은 매우 느리게 진행되며, 이로 인해 위치 추정 결과 또한 훨씬 부정확해지게 됩니다.

 

Laser Parameters

다음 파라미터들은 레이저 스캔 데이터에 관련된 amcl node에 관한 설정 파라미터입니다. 

• laser_min_range (default: -1.0): 레이저 데이터의 최소 거리를 정합니다. 해당 거리 이상의 데이터만 사용합니다. 즉, 너무 가까이에 있다고 생각되는 데이터는 사용하지 않습니다. -1.0 으로 설정 시 해당 파라미터를 비활성화합니다.
• laser_max_range (default: -1.0): 레이저 데이터의 최대 거리를 설정합니다. 너무 멀리 있는 데이터는 사용하지 않습니다. -1.0 으로 설정 시 해당 파라미터를 비활성화합니다.
• laser_max_beams (default: 30): 필터를 업데이트할 때,  각각의 스캔에서 몇 개의 레이저 빔을 사용할지를 정합니다.
• laser_z_hit (default: 0.95): z_hit에 대한 가중치입니다.
• laser_z_short (default: 0.1): z_short에 대한 가중치입니다.
• laser_z_max (default: 0.05): z_max에 대한 가중치입니다.
• laser_z_rand (default: 0.05): z_rand에 대한 가중치입니다.

 

---

연습 3.5

 

a) laser_max_range 파라미터를 1로 설정하고, 결과를 확인해보세요

 

- 연습 3.5 끝

---

 

위의 결과, 키보드로 로봇을 조종하여 움직이게 해도 자신의 위치를 제대로 찾지 못하는 것을 볼 수 있습니다.

 

우리는 lase_max_range 파라미터를 1로 설정했는데, 이는 매우 낮은 값입니다. 로봇의 반경 1m이내에는 자신의 위치를 알아낼 수 있는 유용한 정보가 없기 때문에 이 데이터만 갖고는 자신의 위치를 알아내기가 훨씬 어렵습니다.

 

 

AMCL through Services¶

지금까지, 우리는 topic을 통해 위치추정으 ㄹ하였습니다. 우리는 레이저 데이터, 오도메트리, map topic으로부터 데이터를 얻었고,  이를 로봇의 위치추정 정보에 대한 데이터를 제공하는 2개의 새로운 topic으로publish했습니다.

 

이 방법 말고도 service를 사용하는 방법도 있습니다. amcl node에서 제공하는 service는 다음과 같습니다.

 

global_localization (std_srvs/Empty): 지도 내의 모든 자유공간에 걸쳐 무작위로 전역 위치추정을 초기화합니다. 

 

static_map (nav_msgs/GetMap): amcl은 레이저 데이터 기반의 위치추정을 위해 사용될 지도를 retrieve하기 위해 이 service를 호출합니다.

 

요약하자면, amcl node는 파티클들의 위치를 재시작하기 위해 global_localization이라고 하는 service를 제공하고, 필요로 하는 지도 데이터를 얻기 위해 static_map이라고 하는 service를 이용할 수 있습니다.

 

 

---

- 연습 3.6

 

b) rospy에 의존하는 initialize_particles 라는 패키지를 새로 만듭니다.

 

c) 이 패키지 내부에 init_particles_caller.py 파일을 만듭니다. 이 파이썬 파일은 파티클들을 흩뿌리게 하기 위한 /global_socalization  service를 호출하도록 합니다.

 

 

---

 

#! /usr/bin/env python3

import rospy
from std_srvs.srv import Empty, EmptyRequest
import sys 

rospy.init_node('service_client')
rospy.wait_for_service('/global_localization')
disperse_particles_service = rospy.ServiceProxy('/global_localization', Empty)
msg = EmptyRequest()
result = disperse_particles_service(msg)
print (result)

 

 

Navigation 실행 후, 위의 코드를 실행해봅니다.

 

 

다음과 같이 파티클들이 흩어져 있는 결과를 볼 수 있습니다.

 

- 연습 3.6 끝

 

---

 

그런데 이렇게 파티클을 그냥 흩뿌려놓는게 무슨 의미가 있을까요?

 

지금까지는 RViz에서 2D Pose Estimate tool을 사용해 로봇의 초기 위치를 지정해줬지만, 이는 로봇이 자기의 위치를 추정할 수 있게 해주는데 매우 큰 정보입니다. 따라서, 우리가 2D Pose Estimate tool을 사용하면 해당 위치 주변에 파티클들이 모여있었습니다.

 

하지만 만약 우리가 로봇이 정확히 어디있는지 모른다면 어떻게 될까요? 만약 우리가 RViz를 사용할 수 없다면? 이 경우에는 우리는 파티클들을 가능한 모든 환경에다가 흩뿌려서 초기화하여 로봇이 움직이며 정보를 얻도록 해야합니다. 이전에 보았듯이, 로봇이 움직이며 데이터를 모아 자신의 위치를 점점 정확하게 추정하게 될 것입니다.

 

---

-연습 3.7

 

연습 3.6의 결과에서 키보드로 로봇을 움직여보세요. 시간이 지나면서 자기 위치를 더 잘 찾게 되는 결과를 볼 수 있습니다.

이 때, 파티클들의 공분산 데이터는 /amcl_pose topic으로 publish됩니다.

 

로봇을 움직이는 동안, /amcl_pose topic에서 covariance 데이터 중에서 첫 번째(x 공분산), 여덟 번째(y 공분산), 마지막 데이터( z 공분산)만 보시면 됩니다.

 

 

 

- 연습 3.7 끝