ROS Navigation Tuning Guide 공부
http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide
navigation/Tutorials/Navigation Tuning Guide - ROS Wiki
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. Basic Navigation Tuning Guide Description: Thi
wiki.ros.org
https://kaiyuzheng.me/documents/navguide.pdf
ROS에서 제공하는 Navigation stack은 모바일 로봇에게 있어서 굉장히 중요한 역할을 합니다. 경로 계획을 할 때, 필수적으로 존재해야 하는 스택으로 현재 시대에서도 여전히 많이 쓰이는 스택입니다.
1. Velocity and Acceleration
DWA(Dynamic Window Approach)를 하던 TEB(Timed Elastic Band)를 사용하던 속도와 가속도 성분은 모바일 로봇에 있어서 굉장히 중요합니다. ROS 네비게이션 스택에서 local planner는 odometry 메세지를 고려하고 로봇의 속도를 결정짓는 cmd_vel 을 발행합니다.
따라서 최대/최소 속도/가속도는 모바일 베이스의 기본 파라미터입니다. 이를 적절하게 튜닝하는 것은 최적의 local planner를 만드는데 도움을 줄 것입니다. 이 부분을 튜닝하기 위해서는 translational & rotational 속도/가속도를 알아야 합니다.
1.1 To obtain maximum velocity
ROS에서는 odom 토픽을 이용하여 속도를 추출할 수 있다. 조이스틱이 있다면 조이스틱을 이용해서 로봇을 조종해보자. 그리고 최대 직선 속도와 회전 속도를 얻을 수 있다. 하지만 안전을 위해서 실제 얻어지는 값보다는 더 적게 설정을 해주자.
1.2 To obtain maximum acceleration
모바일 로봇의 최대 각속도를 측정할 수 있는 여러가지 방법이 존재한다. ROS에서 odom 토픽과 time stamp 값을 echo를 하게되면 최대 속도에 어느 정도 시간이 걸려서 도달했는지 계산할 수 있다. 그리고나서 nav_msgs/Odometry message를 이용하면 가속도를 계산할 수 있다.
1.3 Setting minimum values
최소 속도 값은 정형화된 값이 아니다. 최소 직선 속도를 위해서 아주 큰 음수 값으로 설정해준다. 왜냐하면 로봇의 후진을 고려해주면 좋기 때문이다. 최소 회전 속도에 대해서 또한 음수 값으로 설정할 수 있다. 그러면 여러 방향으로 회전할 수 있을 것이다. DWA Local Planner는 로봇의 최소 회전 속도의 절대 값을 취하는 것을 기억해라.
1.4 Velocity in x,y direction
x velocity는 로봇의 직선 방향으로 속도를 의미하고 y velocity는 직선 방향에 수직 방향을 의미한다. non-holonomic(differential wheeled robots) 로봇에 대해서는 y 속도는 0으로 설정해야 한다.
2. Global Planner
2.1 Global Planner Selection
navigation 스택에서 move_base 노드를 사용하기 위해서는 우리는 global planner와 local planner를 갖고 있어야 한다. global planner는 3개의 global planner가 있다. nav_core::BaseGlobalPlanner, carrot_planner, navfn, global_planner.
2.1.1 carrot_planner
제일 간단한 planner이다. 주어진 목표에 대해서 장애물이 있는지 없지를 확인합니다.
2.1.1 navfn and global_planner
navfc은 시작점과 끝점의 최소화된 비용을 계산하기 위해서 다익스트라 알고리즘을 사용한다.
global_planner는 navfc의 대체할 수 있는 옵션을 제공한다.
이 옵션은 세 가지가 있는데 아래와 같다.
(1) A*
(2) toggling quadratic approximation
(3) toggling grid path
2.1 Global Planner Parameters
global_planner는 핵심 파라미터이다. 기본 파라미터 세팅은 allow unknown(true), use dijkstra(true), use quadratic(true),
use grid path(false), old navfn behavior(false) 으로 되어 있을 것이다.
visualize potential(false)을 true로 설정하면 포텐셜 맵을 보는데 도움이 될 것이다.
global path planning의 퀄리티를 결정 짓는 세 가지 파라미터가 존재한다.
1. cost_factor
2. neutral_cost
3. lethal_cost
3. Local Planner Selection
Local planner (nav_code::BaseLocalPlanner)는 dwa_local_planner, eband_local_planner, 그리고 teb_local_planner 종류가 있다. 속도 명령을 내릴 수 있는 다 다른 알고리즘을 사용하고 있다. 여기에서는 dwa_local_planner에 대해서 공부한다.
3.1 DWA Local Planner
3.1.1 DWA algorithm
dwa_local_planner 는 Dynamic Window Approach 알고리즘을 사용한다.
ROS Wiki에는 다음과 같이 설명한다.
1. 로봇의 control space (dx, dy, dtheta)를 시간에 따라 샘플링을 한다.
2. 샘플된 속도에 대해서 현재 로봇의 상태로부터 무슨 일이 발생할지에 대한 forward simulation을 수행한다.
3. forward simulation으로부터 얻은 각각의 궤적 점수는 proximity to obstables, proximity to the goal, proximity to the global path, 그리고 속도를 포함한다. 그리고 점수가 낮은 궤적은 버린다.
4. 제일 점수가 높은 궤적을 선택하고 모바일 베이스에 속도를 보낸다.
5. 1~4를 반복한다.
DWA에 대한 논문은 다음과 같다. https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf
DWA의 목적은 속도와 각속도 (v, w) 쌍(원의 궤적을 나타냄)을 생성하는 것이다. DWA는 속도 공간을 탐색함으로써 다음 시간에 최적의 경로를 생성한다. DWA는 dynamic window 내에서 속도를 고려한다. dynamic window는 주어진 현재 속도와 각속도, 가속도, 각가속도를 고려하여 만들어진 도달 가능한 속도 쌍을 정의한다.
DWA는 다음 목적 함수를 최대화한다.
(1) 목표까지의 진척도
(2) 장애물까지의 거리
(3) 최적의 속도 쌍을 생성하는 forward 속도
요약하면 다음과 같다.
1. dynamic window 내에서 속도 공간 내의 속도 쌍(Vx, Vy, W)을 샘플링 한다.
2. 안 좋은 궤적은 삭제한다.
3. 궤적 점수를 계산하는 목적 함수를 이용하여 속도 쌍을 평가한다.
4. 현재 상태에서 최고의 속도 옵션을 추출하다.
DWA planner는 local costmap(장애물 정보가 담겨있음)에 의존한다. 그러므로 local costmap에 대해서 파라미터를 튜닝하는 것은 DWA local planner에 있어서 매우 중요하다.
3.1.2 DWA Local Planner : Forward Simulation
Forward Simulation은 DWA 알고리즘의 두 번째 단계이다. 각각의 샘플이 시뮬레이션이 될 때, 좋지 않은 궤적은 버린다. 이 때, 고려해볼 수 있는 파라미터는 얼마나 자주 샘플링이 되었는지애 대한 sim_time이다.
*sim_time 튜닝 방법
sim_time을 매우 적게(< 2.0) 설정하면 제한된 성능을 보일 것이다. 특히나, 로봇이 좁은 공간을 통과할 때, 충분하지 않은 시간 때문에 최적의 경로를 추출하지 못할 것이다. 반면에 DWA Local planner는 모든 궤적은 호를 그리기 때문에 너무 높은 값 (> 5.0)을 갖는다면 호의 궤적이 유연하지 않을 것이다. 이 문제는 피할 수 없다. 왜냐하면 planner는 제어주기(controller_frequency(Hz))마다 재계획(replan)을 하기 때문이다. 4.0의 값이 적절한 값이다.
Velocity Samples
다른 파라미터 중에서 vx_sample, vy_sample은 얼마나 많은 직선 속도 샘플이 x, y 방향으로 발생할 것인지에 대한 파라미터이다. vth_sample은 회전 속도 샘플에 대한 파라미터이다. 이 샘플링 갯수는 얼마나 많은 계산 파워를 요구할 것인지에 대해서도 관련이 있다.
Simulation granularity
궤적에 대한 샘플 점들의 step size이다. 낮은 값은 높은 주파수를 의미하고, 이는 좀 더 많은 계산량을 요구한다. 기본 값은 0.025이고 터틀봇 사이즈 모바일 베이스에 적합한 값이다.
3.1.3 DWA Local Planner : Trajectory Scoring
DWA Local Planner는 최적의 속도 쌍을 획득하기 위한 목적 함수를 최대화하는 것이다. 이 목적 함수의 값은 progress to goal, clearance from obstables 그리고 forward velocity와 관련이 있다. ROS에서는 아래와 같이 계산한다.
path_distance_bias : global path 궤적에 더 추종할 것인지?
goal_distance_bias : 얼마나 많이 로봇이 local goal에 추종할 것인지?
occdist_scale : 얼마나 많이 로봇이 obstacle을 피할 것인지?
SCITOS G5 로봇에 대해서는 32.0, 20.0, 0.02 를 사용했다.
3.1.4 DWA Local Planner : Other Parameters
Goal distance tolerance 이 파라미터들은 이해하기에 간단하다.
- yaw_goal_tolerance (double, default: 0.05) : 목표 점에 도달할 때, yaw/rotation에 대한 tolerance(용인량) 반지름
- xy_goal_tolerance (double, default: 0.10) : 목표 점에 도달할 때, x,y 거리에 대한 용인량 거리 (m)
- latch_xy_goal_tolerance (bool, default: false) : 만약에 목표 x,y 용인량에 도달하면 간단하게 회전을 해서 해결한다.
Oscilation reset 문 사이를 통과하는 시뮬레이션에서 로봇이 앞으로 갔다가 뒤로 갔다가 하면서 진동할 수 있다. (local planner가 두 개의 반대되는 방향을 생성할 수 있기 때문에.) 만약에 로봇이 진동한다면 navigation 스택은 로봇이 recovery 행동을 하도록 할 것이다.
- oscillation_reset_dist (double, default: 0.05) : 진동 신호가 리셋되기 전에 얼마나 멀리 로봇이 이동해야 하는지?
4. Costmap Parameters
위에서도 언급했듯이 costmap 파라미터 튜닝은 local planner에 있어서 매우 중요하다. ROS에서 costmap은 static map layer, obstable map layer, 그리고 inflation layer로 구성되어 있다.
- Static map layer : 주어진 static SLAM 맵을 직접적으로 이해한다.
- Obstacle map layer : 2D 장애물 그리고 3D 장애물 (voxel layer)를 포함한다.
- Inflation layer : 어느 장애물을 부풀리지에 대한 내용이다.
global costmap에 대한 내용도 있는데, global costmap은 맵을 얼마나 부풀릴지에 대한 내용이고 local costmap은 장애물을 얼마나 부풀릴 것인지에 대한 내용이다.
4.1 footprint
footprint는 모바일 베이스의 contour이다. ROS에서 2개의 크기를 갖는 배열로 되어 있다.
안전을 위해서 실제 로봇 크기보다 약간 더 크게하는 것을 추천한다.
footprint를 결정하기 위해서 제일 쉬운 방법은 너의 로봇을 직접 그리는 것이다. 게다가 직접적으로 top view에서 사진을 찍거나 CAD 파일을 이용하는 것이다. 원점은 로봇의 중심점이다.
4.2 inflation
inflation layer는 0~255까지 비용에 대한 셀을 고려한 것이다. 각각의 셀은 점유되거나 비어있거나 모르는 경우가 있다.
위 그림은 얼마나 inflation decay 커브가 계산되는지 보여주는 그림이다.
inflation_radius와 cost_scaling_factor는 inflation을 결정 짓는 파라미터이다.
- inflation_radius는 zero cost point가 장애무롤부터 얼마나 멀리 위치하는지에 대한 파라미터이다.
- cost_scaling_factor는 셀의 비용에 대해서 반비례적이다. 높게 설정하면 decay curve를 급격하게 만든다.
Dr. Pronobis는 다음을 제안했다. 최적의 costmap decay curve는 low slope을 가질 때 나온다. 그러면 장애물로부터 가능한 최적의 경로가 발생한다. 이것의 장점은 로봇이 장애물들의 중간으로 지나간다는 것이다.
4.3 costmap resolution
이 파라미터는 local costmap과 global costmap에 대해서 분리할 수 있다. 이 파라미터들은 compuation load와 path planning에 영향을 준다. 낮은 분해능(>0.05)에 대해서는 밀집된 통로에서 장애물 지역이 겹칠 수 있기 때문에 결론적으로 local planner는 경로를 찾을 수 없을 것이다.
global costmap 분해능에 대해서는 map 분해능과 동일하게 유지하는 것이 좋다. 만약에 높은 계산 능력을 갖는 시스템이 있다면 laser scanner의 분해능을 꼭 확인해보길 바란다.
예를 들어서 Hokuyo URG-04LX-UG01 레이저 스캐너는 0.01 mm 분해능을 갖고 있기 때문에 맵을 0.01 이하의 분해능으로 스캐닝하는 것은 unknown dot을 깨끗하게 하기 위해서 로봇이 여러번 회전하도록 요구할 것이다. 우리는 0.02가 충분하다고 생각했다.
4.4 obstacle layer and voxel layer
이 두 개의 레이어들은 costmap에 마킹을 한다. 이 레이어들은 obstacle layer라고 불린다. ROS wiki에 따르면 obstable layer는 두 개의 차원에서 추적한다. 반면에 voxel layer는 세 개의 차원에서 추적한다. 로봇의 센서 데이터로부터 장애물이 감지될 수 있고 제거될 수 있다.
ROS 구현에서 voxel layer는 obstacle layer로부터 상속받을 수 있다. 그리고 이 레이어들은 laser scan으로 부터 해석되는 장애물 정보를 포함한다. 또는 PointCloud(or PointCloud2) 메세지로부터 해석된다. 게다가 voxel layer는 뎁스 카메라를 요구한다. 3D 장애물들은 결국 inflation을 하기 위해 2D costmap으로 projection된다.
어떻게 voxel layer가 작동하는가? * voxel : 3D 화소
https://ko.wikipedia.org/wiki/%EB%B3%B5%EC%85%80
복셀 - 위키백과, 우리 모두의 백과사전
위키백과, 우리 모두의 백과사전.
ko.wikipedia.org
Voxels은 3D 큐브로 생각하면 된다. 공간에서 상대적인 위치가 포함되어 있다. 부피의 특성이나 데이터가 담겨져 있고 그것의 위치가 장애물인지 아닌지를 판별하게 된다.
voxel_grid는 ROS 패키지인데, 효율적인 3D voxel grid data 구조의 구현을 제공한다. (세 가지 상태가 존재 -> marked, free, unknown) voxel grid는 costmap 영역 내의 부피를 점유한다. 각각의 업데이트가 되는 동안 voxel layer는 voxel들을 센서로부터 얻은 관측을 기반으로 하여 표시를 하거나 제거할 수 있다. 이것은 또한 ray tracing(아래 글에 소개)을 수행할 수 있다. voxel grid는 업데이트할 때, 재생성되지 않음을 유의하라.
obstacle 레이어와 voxel 레이어에서 왜 ray tracing인가?
Ray tracing은 실제 3D 그래픽 렌더링을 위한 최고의 선택이다. 이것이 왜 장애물을 다루는데 필요한지 혼란이 올 수 있다. 가장 큰 이유는 다른 종류의 장애물을 센서로부터 감지할 수 있기 때문이다.
위 그림을 보았을 때, 이론적으로 우리는 이 장애물이 말랑한지 딱딱한지 알아차릴 수 있다.
"voxel ray tracing" vs "polygong ray tracing"에 대한 비교는 아래 블로그에 잘 나와있다.
Ray Tracey's blog: Voxel ray tracing vs polygon ray tracing
Voxel ray tracing vs polygon ray tracing
Carmack's thoughts about ray tracing: I think that ray tracing in the classical sense, of analytically intersecting rays with conventiona...
raytracey.blogspot.com
위의 이해를 바탕으로 장애물에 대한 매개변수를 살펴보자.
이 파라미터들은 모든 센서에 적용할 수 있는 global filtering 파라미터이다.
- max obstacle height: 장애물의 최대 높이는 costmap 으로 삽입된다. 실제 높이보다 더 높게 입력이 되어야 한다.
- obstacle range: 장애물로부터 기본 최대 거리가 costmap으로 삽입.
- raytrace range: 센서 데이터로부터 장애물을 raytrace 할 수 있는 기본 범위
- origin z: 맵의 z 기준 좌표 값
- z resolution: 맵의 z 분해능
- z voxels: 수직 방향으로 voxel 갯수, 그리고 그리드 높이 = 분해능 x 복셀 수
- unknown threshold: unknown cell의 갯수
- mark threshold: marked cell의 최대 갯수
Experiment observations
voxel 레이어 파라미터의 영향을 알아보는 실험을 해보자. 우리는 이를 알아보기 위해 ASUS Xtion Pro 뎁스 카메라를 사용했다. Xtion의 위치는 blind field의 범위를 결정짓는다.
게다가, 장애물을 나타내는 voxel들은 Xtion 범위 내에서 장애물이 발생할 때 업데이트 한다. 다른말로 몇 개의 voxel 정보들은 유지할 것이고, 그 voxel의 영향은 costmap inflation에 대해서 유지할 것이다.
게다가, z_resolution은 다음을 제어할 것인데, 얼마나 밀집하게 voxel이 z축 상에 위치해있는지를 제어할 것이다. 만약에 이 값이 크다면, voxel layer는 밀집해 있을 것이다. 만약에 0.01과 같이 낮은 값이라면 모든 voxel들은 합쳐질 것이며 costmap 정보에 유용한 정보를 제공하지 않을 것이다.
5. AMCL
amcl은 ROS 패키지로써 로봇의 localization을 담당한다. Adaptive Monte Carlo Localization의 약자이다. 또한 파티클 필터 localization을 의미한다. localization 기술은 다음과 같이 장동한다. 각각의 파티클 샘플은 위치와 방향 데이터를 포함하고 이는 로봇의 pose를 의미한다. 파티클들은 초기에 무작위로 샘플이 된다. 로봇이 움직이면, 파티클들은 현재 상태를 기반으로 하여 다시 샘플링이 되고 로봇의 액션은 recursive Baysian estimation을 사용한다.
[Thrun et al., 2005] Thrun, S., Burgard, W., and Fox, D. (2005). Probabilistic robotics. MIT press. 이 책에 의하면 MCL은 두 개의 확률론적 모델을 유지한다.
https://mitpress.mit.edu/books/probabilistic-robotics
Probabilistic Robotics
An introduction to the techniques and algorithms of the newest field in robotics.Probabilistic robotics is a new and growing area in robotics, concerned with perception and control in the face of uncertainty. Building on the field of mathematical statistic
mitpress.mit.edu
1. motion model
2. measurement model
ROS amcl에서 motion model은 odometry 모델에 대응되고, 반면에 measurement model은 레이저 스캔 모델에 대응된다.
5.1 Header in LaserScan Message
메세지를 발행하는 scan 토픽은 sensor_msgs/LaserScan 타입이다. 이 메세지는 다음 field를 포함한다.
- angle min ( oat32) start angle of the scan [rad]
- angle max ( oat32) end angle of the scan [rad]
- angle increment ( oat32) start angle of the scan [rad]
- time increment ( oat32) time between measurements [seconds] - if your scanner is moving, this will be used in interpolating position of 3d points
- scan time ( oat32) time between scans [seconds]
- range min ( oat32) minimum range value [m]
- ange max ( oat32) maximum range value [m]
우리는 다음과 같은 실험을 수행했다. 실제 레이저 스캐너의 datasheet와 동일한지 검증했다. 우리가 사용한 레이저 스캐너는 SICK LMS 200 그리고 SICK LMS 291이다.
5.2 Parameters for measurement and motion models
measurement model과 motion model을 튜닝하는 파라메터에 대한 설명이다. 이 파라미터 튜닝은 아래 책의 이해를 바탕으로 해야한다.
https://mitpress.mit.edu/books/probabilistic-robotics
Probabilistic Robotics
An introduction to the techniques and algorithms of the newest field in robotics.Probabilistic robotics is a new and growing area in robotics, concerned with perception and control in the face of uncertainty. Building on the field of mathematical statistic
mitpress.mit.edu
레이저 스캐너 모델의 기본 파라메터는 다음과 같다.
우리 로봇의 localization 기능을 향상시키기 위해서 lazer_z_hit 그리고 laser_sigma_hit (높은 측정 노이즈를 포함하는) 를 증가시켰다.
실제 결과는 다음과 같다.
odometry model에 대해서도 다음과 같이 튜닝을 했다. 생각보다 우리의 odometry 정보를 믿을 수 있었기 때문에 알고리즘의 낮은 노이즈를 가정했다.
5.3 Translation of the laser scanner
tf 변환은 laser_link로부터 base_footprint 혹은 base_link를 나타낸다. 만약에 좌표 변환이 정확하지 않다면 localization이 이상하게 작동을 할 것이다. 이 상항에서 우리는 laser 값이 shift를 했다거나 위치에 급작스러운 변화가 발생할 것이다. 이는 urdf를 확인하길 바란다.
6. Recovery Behaviors
robot navigation에서 짜증나는 부분이 로봇이 움직이지 못하는 상황이다. 하지만 다행히도 navigation stack에서는 recovery 기능을 포함하고 있다. 때때로 로봇이 모든 recovery 기능을 다 써버려서 유지할 수도 있다. 그러므로 우리는 좀 더 강건한 해결책을 찾을 필요가 있다.
Types of recovery behaviors
ROS navigation은 두 개의 recovery 기능을 포함하고 있다.
1. clear_costmap_recovery : global costmap와 같이 동일한 상태를 얻기 위해서 기본적으로 local costmap을 되돌아가는 기능을 포함
2. rotate_recovery : 360도 회전하는 기능 포함
Unstuck the robot (*unstuck : 느슨해진, 벗겨진)
가끔 rotate recovery가 실패할 수도 있다. 이럴 때는 SMACH(다른 recovery 행동을 시도함)를 사용하면 된다. 그리고 임시적인 목표를 로봇 가까이 설정하거나 이전의 방문한 pose로 돌아가는 기능(backing off)도 있다.
Parameters
ROS recovery 행동을 위한 파라미터는 일반적으로 기본 값으로 둘 수 있다. clear costmap recovery를 위해서 만약 높은 sin_time(=trajectory가 길다.)을 갖는다면 reset_distance 파라미터를 증가시켜야 한다. 그러면 local costmap에 대한 큰 영역이 제거될 것이고 local planner가 경로를 더 잘 탐색할 수 있도록 해줄 것이다.
7. Dynamic Reconfigure
로봇이 목표점이 도달했을 때, 특정한 상황에 좀 더 도움이 되는 dynamic reconfiguration 이 존재한다.
하지만 아직 dynamic reconfiguration은 많이 시행되진 않았다.
Example
global path로부터 떨어지는 경우가 있다. 그러므로 우리는 path_distance_bias 를 증가시켜야 한다. 높은 path_distance_bias는 로봇을 global path 에 좀 더 잘 달라붙게 하기 때문이다.
8. Problems
1. Getting stuck
시뮬레이션 혹은 실제 환경에서 다양하게 접할 수 있다.
2. Different speed in different directions
navigation stack에서 이상한 점을 관찰할 수 있다. 목표가 TF 원점에 대해서 -x 방향으로 도달했을 때, DWA local planner가 불안정해진다. 그리고 로봇은 점점 더 느리게 움직인다. 그러나 목표점이 +x 방향이 된다면 DWA local planner는 좀 더 안정적이고 빨라지게 된다. 이 문제는 github에 남겨놓았다.
When move_base goal is set in a certain direction, dwa_local_planner plans less stably and the robot is moving slower · Issue #
I found some interesting behavior in dwa_local_planner, not sure if it is a bug, or if it can be resolved by setting parameters differently. The behavior is that, when the goal is set in the -x dir...
github.com
3. Reality vs Simulation
실제 환경과 시뮬레이션 사이에는 차이가 존재한다. 실제 환경에서는 좀 더 다양한 모양을 갖는 장애물들이 있다.
4. Inconsistency
ROS navigation 스택을 사용하는 로봇은 너무 자주 변하는(inconsistency) 양상을 보인다. 예를 들면 문을 들어갈 때, local costmap이 다시 생성된다거나 매번 다르게 생성된다. 이것은 경로 계획을 할 때 영향을 준다. 또한 로봇에 메모리 공간도 부족하다.