About Navigation on ROS

728x90

- 위 내용은 아래의 Project에서 진행한 내용을 기반으로 정리됨

https://youngseong.tistory.com/category/Project/KUSMO

- 이 글에선 Navigation까지의 System 구성이 아닌, System 구성 완료 후 원활한 Navigaion 수행을 위한 정보들을 정리함

- Navigation을 위한 System 구성 과정은 아래 글들 참고

https://youngseong.tistory.com/140

https://youngseong.tistory.com/102

Navigation : Robot이 주어진 환경에서 현재 위치로부터 지정한 목적지까지 이동하는 것
Navigation Stack : Navigation 수행을 위해 구성된 Software의 집합

Navigation Stack 구성 요소
1. Sensor Source : Lidar, IMU 등의 각종 Sensor들을 통해 얻은 data들. 해당 data들은 Localization과 Navigation 시 장애물을 인지하는데 쓰임
2. Sensor Transform/TF : 서로 다른 Frame에 위치한 Sensor들의 상대적인 위치를 정의하여 모든 Sensor Data들을 한 Frame킴(보통 'base_link' 사용)으로 통일시킴
  - 참고 : https://youngseong.tistory.com/104
3. Odometry Source : Robot의 출발 위치와 그 위치에 대해 상대적인 Robot의 이동 거리 정보 제공. Odometry 정보를 Encoder, IMU를 통해 계산하여 nav_msgs/Odometry 형식의 Message를 통해 Navigation Stack으로 Publish함
4. Base Controller : Navigation Stack이 Odometry 정보를 받아 출력한 Twist(geometry_msgs/Twist) Messgae를 받아와 그에 대응하는 Velocity Command인 cmd_vel을 Motor에 입력하여 Motor의 속도로 변환함

Navigation 수행을 위해선 아래의 4가지 요소들이 필요함
1. 주행할 Map 생성(Mapping)
2. 장애물을 인식하는 Sensing
3. Map상에서 자신(Robot)의 위치를 파악하는 Localization
4. Map상에서 목표지점까지의 경로를 계산하는 Path Palnning과 해당 Path상으로 이동하는 Moving

1. Mapping

Map은 Localization과 Mapping을 동시에 수행하는 SLAM (Simultaneous Localization And Mapping)을 통해 생성
SLAM은 Hector SLAM, Gmapping, Cartographer SLAM 등의 Algorithm을 통해 수행 가능
Hector / Cartographer SLAM 수행 방법 : https://youngseong.tistory.com/145

2. Sensing

Lidar, IMU 등을 통한 장애물 인식과 Robot의 자세 추종

3. Localization

Robot에 이동 명령을 내려 움직일 때, 여러 내/외부 요인들로 인해 Robot이 항상 우리의 의도와 정확히 일치하게 움직이지 않는다
따라서 Robot이 이동할 때 Robot이 위치할 확률을 무작위 Particle로 나타냄
- 각 Particle은 Robot의 위치/자세를 나타냄
Robot이 이동하며 Sensor를 통해 주변 환경을 관측하며 현재의 환경과 맞지 않는 Particle은 무시하고 비슷한 Particle에 가중치를 더 주어 주변에 새로운 Particle을 생성함
이 과정을 재귀적으로 반복하여 Robot의 현재 위치와 비슷한 곳에 다수의 Particle들을 생성함
위와 같이 Localization을 수행하는 방법을 MCL(Monte Carlo Localizaiton) 또는 Particle Filter Localilzation 이라고 함
그리고 위의 MCL에서 적은 수의 sample을 사용하여 수행 시간을 줄여 실시간성을 높인 Algorithm이 AMCL (Adaptive MCL)
ROS에서의 AMCL은 Motion Model이 Odometry Model에 해당하고, Measurement Model은 Laser Scan Model에 해당함

- AMCL 관련 Parameter (KUSMO에서 사용중인 amcl.launch file)

<?xml version="1.0"?>
<launch>
    <!-- Define AMCL`s various parameters -->

    <!-- Subscribe: /scan, /tf, /initialpose, /map -->
    <!-- Publish: /amcl_pose, /particlecloud, /tf -->
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
    
    <!-- Important Parameters -->
    <param name="odom_model_type" value="diff"/>
    <param name="odom_frame_id" value="odom"/>
    <param name="base_frame_id" value="base_footprint"/>
    <param name="global_frame_id" value="map"/>
    
    <!-- Particle Filter`s Performance Related Parameters -->
    <param name="min_particles" value="500"/>
    <param name="max_particles" value="5000"/>
    <param name="kld_err" value="0.05"/>	
    <param name="kld_z" value="0.99"/>
    <param name="update_min_d" value="0.2"/>
    <param name="update_min_a" value="0.5"/> 
    <param name="resample_interval" value="1"/>
    <param name="transform_tolerance" value="1.0"/> 
    <param name="gui_publish_rate" value="10.0"/>
    
    <!-- Laser Model Parameters -->
    <!-- RPLidar A1`s max laser beam is 360, A2 is 16384 -->
    <param name="laser_max_beams" value="30"/>
    <param name="laser_z_hit" value="0.5"/>
    <param name="laser_z_short" value="0.05"/>
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/>
    <param name="laser_sigma_hit" value="0.2"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_likelihood_max_dist" value="2.0"/>      
    <param name="laser_model_type" value="likelihood_field"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_alpha5" value="0.1"/> 
    
    <remap from="scan" to="scan_filtered"/>     <!-- Change scan topic to filtered topic -->
    
    <!-- Use these parameters if you want to set robot`s initial pose manually-->
    <!-- <param name="initial_pose_x" value="0"/> -->  <!-- Set robot`s initial x pose -->
    <!-- <param name="initial_pose_y" value="0"/> -->  <!-- Set robot`s initial y pose -->
    <!-- <param name="initial_pose_z" value="0"/> -->  <!-- Set robot`s initial z pose -->       
        
    <param name="recovery_alpha_slow" value="0.0"/>
    <param name="recovery_alpha_fast" value="0.0"/>
    
    <!-- Estimated noise of odometry`s rotational momentum when rotating  -->
    <param name="odom_alpha1" value="0.2"/>
    <!-- Estimated noise of odometry`s rotational momentum when doing translational momentum -->
    <param name="odom_alpha2" value="0.2"/>
    
    <!-- translation std dev, m -->
    <!-- Estimated noise of odometry`s translational momentum when doing translational momentum -->
    <param name="odom_alpha3" value="0.8"/>
    <!-- Estimated noise of odometry`s translational momentum when rotating -->
    <param name="odom_alpha4" value="0.2"/>

</node>
</launch>

주요 Parameter
- odom_model_type (default : diff) : Robot의 Model 정의. diff, omni, diff-corrected, omni-corrected 등이 있다
- odom_frame_id (default : odom) : Odometry Frame의 이름 정의
- base_frame_id (default : base_link) : Robot의 Base Frame의 이름 정의
- global_frame_id (defalut : map) : Localization에 의해 Publish되는 좌표계의 이름 정의
- use_map_topic (default : false) : Node가 Topic 또는 Service Call에 의해 Map Data를 받을지 말지를 정의
Particle Filter의 성능을 설정하는 Parameters
- min_particles (default : 100) : Particle Filter에서 사용할 최소 Particle 수
- max_particles (default : 5000) : Particle Filter에서 사용할 최대 Particle 수
- kld_err (default : 0.01) : 실제 분포와 추정 분포 사이의 최대 오차 설정
- update_min_d (default : 0.2) : Filter Update를 위해 Robot이 움직여야 할 최소한의 Linear 거리 (meter)
- update_min_a (default : π/6.0) : Filter Update를 위해 Robot이 움직여야 할 최소한의 angular 거리 (radian)
- resample_interval (default : 2) : Resampling 되기 이전에 Particle Filter를 Update하는 횟수 설정
- transform_tolerance (defalut : 1) : 이 Transform이 Publish 된 후 유효한 시간 설정 (second)
- gui_publish_rate (defalut : -1.0) : Visualization을 위해 Scan과 Path가 표시될 시간 설정 (Hertz). -1.0의 경우 해당 기능 비활성화
Laser Model Parameters
- laser_min_range ( default: -1.0) : Laser Scan의 최소 거리 지정. -1.0일 경우 최소 거리 사용
- laser_max_range (double, default: -1.0) : Laser Scan의 최대 거리 지정. -1.0일 경우 최대 거리 사용
- laser_max_beams (int, default: 30) : Filter Update 시 각 scan의 얼만큼의 균일한 beam을 사용할지 결정
- laser_z_hit (double, default: 0.95) : Model의 z_hit 부분에 대한 혼합 가중치 설정
- laser_z_short (double, default: 0.1) : Model의 z_short 부분에 대한 혼합 가중치 설정
- laser_z_max (double, default: 0.05) : Model의 z_max 부분에 대한 혼합 가중치 설정
- laser_z_rand (double, default: 0.05) : Model의 z_rand 부분에 대한 혼합 가중치 설정
- laser_sigma_hit (double, default: 0.2 meters) : Model의 z_hit에 사용되는 Gaussian Model의 표준 편차 설정
- laser_lambda_short (double, default: 0.1) : Model의 z_short 부분에 대한 지수 감쇠 parameter
- laser_likelihood_max_dist (double, default: 2.0 meters) : likelihood_field model에 사용되는, Map상에서 Obstacle Inflation을 진행할 최대 거리
- laser_model_type (string, default: likelihood_field) : Laser Model 결정. beam, likelihood_field 등이 있다

4. Path Planning & Moving

- Velocity and Accelearion

ROS에선 Translation/Rotational Velocity/Acceleration을 알아야 함
DWA는 Minimum Velocity의 절댓값을 사용

-About move_base node

move_base node Navigation 과정에서 발생하는 모든 요소들을 이어주는 ROS Navigtion Stack에서 매우 중요한 요소 중 하나이다. 충돌위험이 없는 경로를 계획하는데 중요한 역할을 한다
move_base를 사용하기 위해선 Global/Local Planner가 필요함
- KUSMO에서는 navfn/DWA 사용중
move_base node는 'geometry_msgs/PoseStamped' Message Type의 'goal pose'를 사용하는 SimpleActionServer이므로, SimleActionCilent를 사용하여 이 node로 goal points를 보냄
- Message : ROS상의 node간의 통신 형태
- Action : ROS에서 사용하는 Message Type 중 하나로, Client가 요청 전송 시 중간중간 feedback 전송 후 최종 결과를 전달
- 참고 : https://youngseong.tistory.com/75

해당 Action Server는 Navigation Stack의 입력으로써 move_base/goal_topic을 사용하며 이 topic은 goal pose를 제공하는데 사용됨
즉 Navigation 진행 시 Rviz의 '2D Nav Goal' 버튼을 통해 목표지점을 지정하면 'move_base/goal' topic으로 새로운 messgae가 publish됨
move_base Action Server가 사용하는 Topic의 종류는 아래와 같다
- Action Subscribed Topics
- move_base/goal (move_base_msgs/MoveBaseActionGoal) : move_base가 실제 world에서 추구해야 할 goal
- move_base/cancel (actionlib_msgs/GoalID) : 특정 goal에 대한 cancel 요청
- Action Published Topics
- move_base/feedback (move_base_msgs/MoveBaseActionFeedback) : 실제 world에서 base의 현재 위치정보 제공
- move_base/status (actionlib_msgs/GoalStatusArray) : move_base action으로 전달된 goal에 대한 상태를 제공
- move_base/result (move_base_msgs/MoveBaseActionResult) : move_base action의 해당 topic은 비어있음
move_base Action Server가 Publish/Subscribe 하는 Topic은 아래와 같다
- Subscribed Topic : move_base_simple/goal (geometry_msgs/PoseStamped) : User를 위한 Goal의 실행 상태를 추적하지 않는 non-action interface를 move_base에 제공한다
- Published Topic : cmd_vel (geometry_msgs/Twist) : Mobile Base에서 실행하기 위한 velocity Command의 Stream

- About Global Planner

move_base node가 새로운 goal point를 받으면, 해당 goal은 바로 Global Planner로 보내짐
그 후 Global Planner는 goal point로 도착할 수 있는 안전한 경로를 계산
- Robot 이동 전에 계산하므로 그동안의 sensor data는 고려하지 않음
Global Planner가 새로운 Path를 계획할 때마다 해당 Path는 '/plan' topic으로 publish됨

- About navfn

navfn은 ROS Navigatio에서 자주 사용되는 Global Planner로, 시작점과 종료점 사이의 Global Path를 최소한의 비용을 들여 찾기 위해 Dijkstra Algorithm을 사용함
Mobile base의 plan을 생성하기 위해 빠른 interpolated(보간-두 점을 연결하는 방법을 의미) navigation function을 제공함
- Navigation Function은 Dijkstra Algorithm을 통해 계산됨
Circular Robot을 가정하여 costmap 상의 시작점부터 목표점까지의 최소한의 cost plan을 grid에서 찾음
- Costmap : Map을 Grid Cell로 표현한 후 각 Cell들에 Cost(0~255)를 부여하여 Robot이 해당 위치를 지나갈 수 있는지 없는지의 여부를 나타냄. 값이 클수록 장애물이 있어 지나갈 수 없음을 뜻함
- 참고 : https://youngseong.tistory.com/140

출처 : https://soohwan-justin.tistory.com/39

Dijkstra Algorithm을 A* 알고리즘, quadratic approximation, toggling grid path 등의 Algorithm으로 대체하여 다른 Path를 계산하는 'Global Planner' 도 사용 가능
navfn의 Parameters는 아래와 같다
- /allow_unknown (bool, default: true) : navfn이 unknown space를 지나는 path를 생성할 수 있도록 허용할지 여부를 결정. 만약 계층화 된 costmap_2d costmap을 voxel이나 obstacle layer와 함께 사용중이라면 해당 layer의 'track_unknown_space' parameter를 true로 설정해야 함 (그렇지 않으면 모든 unknown space를 free space로 convert하여 직선으로 주행하게 됨)
- /planner_window_x (double, default: 0.0) : Planner가 사용할 수 있는 window의 x 크기를 제한하여 큰 Costmap에서 navfn이 작은 크기의 window에서만 동작하도록 함
- /planner_window_y (double, default: 0.0) : Planner가 사용할 수 있는 window의 x 크기를 제한
- /default_tolerance (double, default: 0.0) : Goal Point와의 허용 오차 범위를 설정
- /visualize_potential (bool, default: false)
- cost_factor
- neutral_cost
- lethal_cost

- About Global Costmap

Global Costmap은 SLAM을 통해 생성한 Static Map의 너비, 높이, 장애물의 정보에 맞게 초기화 됨
- Static Map : 환경이 바뀌더라도 Costmap은 바뀌지 않는다
위 설정은 AMCL 등의 Localization System과 함께 Fusion 됨

# global_costmap_params.yaml

global_costmap:
  global_frame: map			# Costmap이 작동할 때의 Global Frame. map이 default
  robot_base_frame: base_footprint	# "" Robot의 Base Frame
  update_frequency: 10.0	# Map Update Frequency
  publish_frequency: 10.0	# Map Publish Frequency
  transform_tolerance: 2.0	
  static_map: true		# Costmap 초기화 시 Static Map의 사용 여부 결정
  rolling_window: false		# Costmap의 Rolling Window 사용 여부 결정
  				# static_map : true이면 해당 parameter는 false이어야 함
 
  plugins:  			
    - {name: static_layer, type: "costmap_2d::StaticLayer"}		# Static Map으로부터 Costmap을 초기화 하기 위해 사용
    - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}	
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}	# 장애물을 Inflate 하기 위해 사용 
# 각 layer는 name, type 형식의 dictionary. Name은 plugin을 위한 Parameter Namespace 정의에 사용
# 위 값들은 common_costmap_params.yaml file에서 정의됨

YAML file로 정의된 Global Costmap 관련 Parameters
Costmap의 설정을 간단명료하게 하기 위해 ROS의 layer 사용. Layer는 관련 Parameter들의 일종의 Block 역할

# common_costmap_params.yaml

obstacle_layer:
  enabled: true
  
  max_obstacle_height:  1.113     # Maximum height of obstacles to consider
  origin_z:             0.0       # Origin height for the costmap
  z_resolution:         0.2       # Parameters related to voxel height representation wth z_voxels
  z_voxels:             10        # Threshold value above which a voxel is considered unknown
  unknown_threshold:    15        # Threshold value above that indicates whether a cell is considered unknown or not
  mark_threshold:       0         # Threshold value below that indicates whether a cell is considered free
  combination_method:   1         # Method used to combine multiple obsevations. 1 represents maximum-likehood estimation
  track_unknown_space:  true      # Indicates whether to track unknown space in the costmap. Prevents converting all unknwon space to free space
  # origin_z: 0.0
  publish_voxel_map: false

  observation_sources: laser_scan_sensor
  laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

inflation_layer:
  enabled: true
  cost_scaling_factor: 20.0 
  inflation_radius: 0.3        # Scaling factor applied to the cost values around obstacles
                                # The best path is for the robot to pass through a center of between obstacles,
                                # so set this factor to be smaller in order to far from obstacles
                                # It is an reciprocal portion
static_layer:
  enabled: true

- About Local Planner

Global Planner가 Path를 계산하면, 해당 Path는 Local Planner로 보내지고 Local Planner가 해당 Global Plan을 분할한 각 부분들을 실행함
- Local Plan이 Global Plan의 일부분
따라서 Local Planner는 Robot을 움직이도록 하기 위해 Velocity Command를 출력함
Global Planner와 달리 Local Planner는 Odometry, Laser Scan Data를 계속 확인하며 충돌 가능성이 없는 Local Plan을 선택한다. 따라서 Local Planner는 Robot의 장애물과의 충돌 방지를 위해 Path를 즉시 재계산 할 수 있음
Local Plan 계산 완료 시 '/local_plan' topic으로 publish 됨
Local Planner는 Robot의 Odometry 정보를 가지고 있는 '/odom' message를 받아와 Robot의 Motion을 Control하는 Velocity Command인 '/cmd_vel' topic을 출력한다
Local Planner의 종류로는 DWA(Dynamic Window Approach), TEB(Time Elastic Band)등이 있다

- Local Planner의 종류

1. base_local_planner

Global Plan을 계산하고 실행하기 위해 Trajectory Rollout과 DWA Algorithm을 제공함
상새 과정은 아래와 같다
- Robot의 Control Space로부터 불연속적으로 Sampling 진행
- 각 Sampling 된 속도에 대해 현재 Robot의 상태에서 Simulation을 진행하여 현재 상태에서 Robot이 움직일지 예측
- Simulation에 의한 결과로 나온 Path 평가
- 부적절한 Path 제거
- 가장 점수가 높은 Path 선택 후 그에 관한 속도 값을 Mobile Base로 보냄
- 위 과정을 반복
Trajectory Rollout Sample은 Robot의 Acceleration 제한 내에서 주어진 모든 Simulation에 대한 Velocity의 집합이나 DWA는 단일 Step의 Simulation을 사용

1-1. dwa_local_planner

dwa_local_planner는 Dynamic Window Approach Algorithm을 적용한 local planner. 가장 많이 사용되는 Local Planner
DWA의 목표는 Robot의 Local Condition에 대한 최적의 Circular Trajectory를 나타내는 (v, w) - (직선 속도, 각속도) 쌍을 생성하는 것
- 다음 시간 간격에서의 Velocity를 찾음으로써 위 목표에 도달함
- 이 공간에서의 Velocity는 허용될 수 있도록 제한되며, 이는 허용 가능한 Velocity에 의해 결정되는 Circular Trajectory상의 가장 가까운 장애물에 Robot이 도달하기 전에 멈출 수 있어야 함을 의미함
- 또한 DWA는 현재의 Translation / Rotational Velocity와 Acceleration이 주어진 후 다음 시간 간격 내에 도달할 수 있는 Velocity 쌍으로 정의되는 Dynamic Window내에서의 Velocity만을 고려한다
DWA는 아래에 따라 Objective Function을 최대화한다
1. Target으로의 Progress
2. 장애물으로부터의 Clearance
3. 최적의 Velocity 쌍을 생성하기 위한 Forward Velocity
절차는 아래와 같다
1. 불연속적으로 Robot의 Control Space를 Sampling 함(dx, dy, dtheta)
2. Sampling 된 각 Velocity에 대해 Forward Simulation을 통해 Robot의 현재 상태로부터 짧은 시간동안 Sampling 된 Velocity가 적용되면 어떻게 될지를 예측
  - Forward Simulation : Local Planner가 Robot의 Control Space 내의 Velocity Sample을 얻은 후 그 Velocity 쌍으로 나타나는 Circular Trajectory를 조사한 후 장애물과 교차하는 Trajctory를 갖는 Velocity를 제거한다
  - 각 Velocity Sample은 sim_time(s) Parameter에 의해 조절되는 시간 간격동안 마치 Robot에 적용된 것 처럼 Simulation 된다
  - sim_time값이 커질수록 계산 부하가 무거워지며 Local Planner가 생성한 Path가 길어지며 합리적임
3. 장애물 / 목표 지점 / Global Path 와의 근접성, 속도 등의 특성을 포함한 Metric을 사용해 Forward Simulation의 결과를 평가(Scoring)하여 장애물과 충돌하는 Path는 버림
4. 가장 높은 score를 갖는 Trajectory를 선택하고 관련된 Velocity를 Mobile Base에 전달
5. Rinse and Repeat
위 절차를 다시 정리하자면 아래와 같다
1. Dynamic Window 내에 있는 Velocity Space 내의 Velocity 쌍 (vx, vy, ω) 을 Sampling
2. 허용 불가능한 Velocity 제거
3. Objective Funtion을 통해 Velocity 쌍을 평가하여 Trajectory Score를 출력
4. 최적의 Velocity Option 선택 후 재계산 진행
위 과정은 장애물 정보를 제공하는 Local Costmap을 기반으로 진행한다
따라서 DWA가 최적의 행동을 하기 위해선 Local Costmap의 Parameter를 알맞게 Tuning하는 것이 매우 중요하다

2. teb_local_planner

Local Plan 계산을 위해 Timed Elastic Band를 사용하는 Local Planner

3. eband_local_planner

Local Plan 계산을 위해 Elastic Band Method를 사용하는 Local Planner

- About DWA Parameters

# dwa_local_planner_params.yaml

DWAPlannerROS:

## Robot Configuration Parameters ##
  acc_lim_x: 1.0	# X 방향 가속도 제한 (meters/s^2)
  acc_lim_y: 0.0	# Y 방향 가속도 제한 (meters/s^2)
  acc_lim_theta: 2.0 	# 각 가속도 제한 (raduis/s^2)
  max_vel_trans:  0.5	# 최대 Translational Velocity의 절대값 (m/s)	
  min_vel_trans:  0.0 	# 최소 Translational Velocity의 절대값 (m/s)
  max_vel_x: 0.5 	# X 방향 최대 속도 (m/s)	
  min_vel_x: -0.1	# X 방향 최소 속도 (m/s). 후진이 가능하게 하려면 음수 값을 설정
  max_rot_vel: 0.3	# 최대 회전속도의 절대값 (rad/s)
  min_rot_vel: 0.0	# 최소 회전속도의 절대값 (rad/s)

  max_vel_y: 0.0	# Diffenetial Type Robot의 경우 y 관련 parameter들은 0이어야 함
  min_vel_y: 0.0	
 
## Goal Tolerance Parameters for each coordinate ##
  xy_goal_tolerance: 0.1 		# 목표 자세와 로봇의 실제 자세와의 x, y 위치 오차 (meter)
  yaw_goal_tolerance: 0.05		# 목표 자세와 로봇의 실제 자세와의 각도 오차 (rad)
  latch_xy_goal_tolerance: false	
  # 만약 goal_tolerance가 latch되면 로봇이 목표 위치에 도착했을 때 goal tolerance를 벗어났더라도 회전을 함
        
## Forward Simulation Parameters ##
  sim_time: 5.0              # Simulation 계산을 진행할 시간 지정 (second)
  sim_granularity: 0.3       # 주어진 궤적의 point 사이의 step size (meter)
  vx_samples: 10             # x 속도 공간에서의 Sample 수
  vy_samples: 0              # y 속도 공간에서의 Sample 수
  vtheta_samples: 20         # theta 속도(각속도) 공간에서의 Sample 수
  
  use_sim_time: true         
# Defines the duration of the simulated trajectory. Larger value allow the planner to consider 
# longer trajectories. Too low value allows to pass narrow area and too high value not allows 
# rapid rotation

  controller_frequency: 10.0      
  penalize_negative_x: true

## Trajectory Scoring Parameters (Trajectory Evalutation) ##
  path_distance_bias: 32.0        # Controller가 주어진 경로(Global Plan)에 가깝게 유지하도록 하는 Weight
  goal_distance_bias: 24.0        # Controller가 Local Plan에 가깝게 유지하도록 하는 Weight
  occdist_scale: 0.02             # Controller가 장애물을 피하도록 하는 가중치
# 위 Parameter들은 Path에 점수를 매길 때 어떤 부분에 더 점수를 줄지 결정

  forward_point_distance: 0.5     # 0.0
  stop_time_buffer: 10.0          # Additional time buffer added to the planned trajectory to account for stopping. It helps the robot to decelerate 
                                  # and come to a complete stop at the goal location
  scaling_speed: 0.25             # Speed at which the planner scales the velocities of the sampled trajectories. 
                                  # Higher value allows for faster scaling, while lower value makes the scaling more gradual
  max_scaling_factor: 0.3         # Maximum scaling factor that can be applied to the velocities of the sampled trajectories
                                  # Smaller value will result in slower maximum velocities

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05    # Minimum distance that the robot need to travel in order to reset the oscillation detection.
                                  # If the robot`s position change distance samller than this threshshold, it indicates that the robot is osciilatng 
                                  # and planner needs to take action to break out of the oscillation

# Debugging
  publish_traj_pc : true          # Set Trajectory Debugging
  publish_cost_grid_pc: true      # Set costmap Debugging 
  
  holonomic_robot: false

sim_time
- 해당 Parameter의 값이 너무 낮으면 (2.0 이하) Path 계산시 최적의 Path를 계산하기엔 시간이 부족하여 제한된 성능을 보이며 Robot이 좁은 길을 주행하게 함
- DWA 사용시의 모든 Trajectory는 Arc이며, 해당 Parameter의 값이 너무 높으면 (5.0 이상) 매우 유연하지 못한 긴 Curve를 생성함 (높은 성능의 컴퓨터에게는 4.0 정도가 적당?)
- 값이 커질수록 계산 부하가 무거워지며 Local Planner가 생성한 Path가 길어지며 합리적임
- Local Planner는 controller_frequency(Hz) 시간 간격 이후에 Replan을 진행함

왼쪽 : sim_time : 1.5, 오른쪽 : sim_time : 4.0

직접 해보고 위 사진 바꿀 것
vx_samples, vy_samples, vth_samples
- X, Y 방향에서의 Velocity Sample 수와 Rotational Velocity Sample 수 설정 (vy_samples는 omni wheel에서만 사용)
- Computation Power 환경에 따라 설정 필요
- 직진보다 회전이 더 복잡하므로 vth_sample를 Translational Velocity Sample들 보다 크게 하는 것이 좋다
sim_granularity
- Trajectory상의 Points간의 Step Size로, Trajectory상의 Point들이 장애물과 만나는지 안만나는지를 얼마나 자주 Examine되어야 하는지를 설정
- 낮을수록 빈도가 높아져 더 많은 Computation Power 필요
- Turtlebot정도 크기의 Mobile Robot에는 기본값인 0.025정도가 적당
Trajectory Scoring Related Parameters (path_distance_bias, goal_distance_bias, occdist_scale)
앞서 설명했듯 DWA Local Planner는 아래의 요소들에 의존하여 Objective Funciton들을 최대화하여 최적의 Velocity 쌍들을 얻는다
1. Target으로의 Progress
2. 장애물으로부터의 Clearance
3. 최적의 Velocity 쌍을 생성하기 위한 Forward Velocity

ROS에서 Objective Function의 Cost는 아래와 같이 계산된다 (최소의 Cost를 얻는 것이 목표)
cost = path_distance_bias * (distance(m) to path from the endpoint of the trajectory)
+ goal_distance_bias * (distacne to local goal from the endpoint of the trajectory)
+ occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0 ~ 254))
- path_distance_bias : Local Planner가 Global Path에 얼마나 가깝게 있을 것인가에 대한 가중치. 클수록 Local Planner가 Global Path를 잘 따라감
- goal_distance_bias : 어떤 Path이던 간에 Robot이 어느정도로 Local Goal에 도달하도록 시도하게 할 것인가에 대한 가중치. 클수록 Robot이 Global Path에 덜 붙음
- occdist_scale : Robot이 어느정도로 장애물을 피하게 할 것인가에 대한 가중치. 클수록 Robot이 제자리에 붙어있게 함
Goal Distance Tolerance Related Parameters
- xy_goal_tolerance (double, default : 0.10) : 목표 자세와 Robot의 x, y 위치 오차 (meter)
- yaw_goal_tolerance (double, default : 0.05) : 목표 자세와 Robot의 각도 오차 (radians)
- latch_xy_goal_tolerance (bool, default : false) : Goal Tolerance가 Latch되었을 경우 Robot이 목표 X, Y 지점에 도달하면 단순히 자리에서 회전하게 함. 회전으로 인해 Goal Tolerance를 벗어나더라도 진행시킴
Oscilation Reset Related Parameter
Robot이 출입구 등을 지나는 상황에서 Local Planner가 정반대 방향의 Path를 생성하여 Robot이 앞뒤로 Oscilate 할 수 있다
이러한 현상이 반복되는 경우, Navigation Stack은 Robot이 Recovery Bebavior를 시도하도록 한다
- oscillation_reset_dist (double, default : 0.05) : Oscillation Flag가 Reset되기 위해 Robot이 얼만큼 움직여야 하는지를 설정
- 만약 Robot의 위치 변화가 이 값보다 작다면, Robot이 Oscillating하고 있음을 나타냄

- About Costmap

ROS에서 costmap은 아래의 3가지 Layer로 구성됨 (자세한 설명은 costmap_common_params.yaml 에서)
- Static Map Layer : Navigation Stack에 제공된 Static SLAM Map을 직접 해석
- Obstacle Map Layer : 2D / 3D 장애물을 포함 (Voxel Layer)
- Inflation Layer : 장애물이 각 2D Costmap Cell의 Cost를 계산하기 위해 Inflate 됨
Global / Local Costmap
- Global Costmap : Navigation Stack에 제공된 Map상의 장애물을 Inflate 함으로써 생성됨
- Local Costmap : Robot의 Sensor에 실시간으로 감지되는 장애물을 Inflate 함으로써 생성됨
Resolution
계산 부하와 Path Planning에 영향을 미치는 Parameter로, Global / Local Costmap 따로 설정 가능
0.05이하의 낮은 Resolution은 좁은 통로에서 장애물 구역이 통로 전체를 감싸 Local Planner가 Path를 생성하지 못함
Global Costmap의 경우 Navigation Stack에 제공된 Map과 일치하는 Resolution을 사용하는 것이 좋음
- Gmapping 사용시 Laser Scanner의 Resolution이 원하는 Map Resolution보다 낮을 경우 Laser Scanner가 구역을 cover하지 못해 Map에 다수의 작은 'unknown dots'가 생긴다
- 사용중인 Lidar의 Resolution 확인 후 그에 맞게 Global Costmap의 Resolution 설정 필요

- About Local Costmap

Local Planner는 Local Plan을 계산하기 위해 Local Costmap을 사용함
Global Costmap과 달리 Local Costmap은 Robot의 Sensor Data로부터 직접적으로 만들어진다
사용자에 의해 정해진 크기의 Costmap의 중앙에 Robot이 있고, Robot이 움직임에 따라 Local Costmap또한 같이 움직이며 실시간으로 장애물에 대한 정보를 얻는다
새로운 장애물이 나타나면 해당 물체는 Local Costmap에만 적용된다

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 2.0

# Rolling Window Related Parameters  
  rolling_window: true
  resolution: 0.02
  width: 4.0
  height: 4.0
  
  plugins:
    - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

global_frame : Costmap이 작동하는 Global Frame의 이름. Local Costmap에서는 반드시 Odometry Data인 /odom으로 설정해야 함
rolling_window : Costmap의 rolling window version 사용 여부 결정. Local Costmap에서는 반드시 true로 설정해야 함
- Rolling Window 사용 시 Robot 근처의 일부 환경만을 Costmap에 나타냄
width, height : Costmap의 가로, 세로 길이 설정
update_frequency : Map update 주기 설정
plugins : Global Map의 Plugins와 동일
- costmap_2d::ObstacleLayer : 장애물 회피에 사용
- costmap_2d::InflationLayer : 장애물 inflate에 사용
- local costmap과 global costmap의 obstacle layer는 서로 다른 plugins를 사용함
- local costmap은 costmap_2d::ObstacleLayer를 사용하고 global costmap은 costmap_2d::VoxelLayer를 사용함
- 현재 같은 plugins를 사용중인데, 로봇을 돌려가며 확인해보고 수정 필요

Local Costmap은 Sensor의 Topic들을 Subscribe하여 아래의 과정을 거쳐 Map을 Update함
1. Sensor Data를 읽음
2. Marking(Costmap에 장애물의 정보 추가), Clearing(Costmap에서 장애물의 정보를 없앰) 작업 수행여
  - Marking, Clearing 작업은 Obstacle Layer에 정의될 수 있음
3. 각각의 Cell에 알맞는 Cost값 할당
4. 장애물이 있는 각각의 Cell에 대해 Obstacle Inflation 수행. Occupied Cell로부터 정의된 Inflation Raduis만큼 바깥쪽으로 퍼져나감

- costmap_common_params.yaml

footprint: [[-0.2625, 0.2725], [0.2625, 0.2725], [0.2625, -0.2725], [-0.2625, -0.2725]]

transform_tolerance: 2.0

map_type: voxel

obstacle_layer:
  enabled: true
  max_obstacle_height:  1.113     
  obstacle_range: 3.0
  raytrace_range: 3.2
  origin_z:             0.0       
  z_resolution:         0.2       
  z_voxels:             10        
  unknown_threshold:    15        
  mark_threshold:       0         
  combination_method:   1         
  track_unknown_space:  true      
  # origin_z: 0.0
  publish_voxel_map: false

  observation_sources: laser_scan_sensor
  laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

inflation_layer:
  enabled: true
  cost_scaling_factor: 20.0 
  inflation_radius: 0.3  
  
static_layer:
  enabled: true

footprint : Mobile Base의 윤곽선을 2D Matrix로 나타낸 것. 로봇에 맞게 장애물을 팽창시키는데 사용되는 내접원과 외접원의 반지름을 계산하기 위해 사용. 안전을 위해 실제 Robot의 윤곽보다 크게 잡는 것이 좋음
- 원형 Robot의 경우 'footprint' 대신 'robot_radius' 사용됨

- Obstacle Layer : Costmap 상의 장애물 Marking을 담당. obstacle / voxel layer 모두 obstacle layer로 같이 불린다

(Obstacle Layer는 2D, Voxel Layer는 3D)

ROS에서는 Obstacle Layer로부터 Voxel Layer가 파생되었으며, 둘은 모두 PointCloud / PointCloud2 Type Message 형태로 보내지는 Laser Scan Data를 해석하여 장애물에 대한 정보를 얻는다

max_obstacle_height (default : 2.0) : Costmap에 반영될 수 있는 장애물의 최대 높이. Robot보다 조금 크게 정의되어야 함 (meter)
obstacle range (default : 2.5) : Costmap에 반영될 거리. Robot과 장애물의 거리가 해당 값보다 작을 경우 Costmap에 반영됨. Sensor마다 재정의 가능 (meter)
raytrace_range (default : 3.0) : Sensor Data를 사용하여 장애물을 ray trace(광선 추적) 하는 거리를 정의. 마찬가지로 Sensor마다 재정의 가능 (meter)
observation_sources (default : "") : 공간에 의해 분리된 observation source name들의 list. source_name을 통해 아래의 각각의 namespace 정의
- sensor_frame : 해당 Sensor의 Frame
- topic (default : source_name) : 해당 Sensor의 Topic. 기본값은 Source의 이름
- data_type (default : PointCloud) : 위 Topic에 관련된 Data Type. 'PointCloud', 'PointCloud2', 'LaserScan' 사용 가능
- clearing (default : false) : 해당 관측값이 Free Space를 Clear 하는데 사용될지 여부를 결정
- marking (default : true) : 해당 관측값이 장애물을 Mark 하는데 사용될지 여부를 결정
- inf_is_valid (default : false) : "LaserScan" Data Message에서 Inf 값을 받을지 결정. Inf값은 Laser Sensor가 측정할 수 있는 최대값으로 변환됨

- Inflation Layer : 장애물의 각 Cell의 Inflation에 대한 Setting

inflation_radius (default : 0.55) : 장애물의 Cost값을 팽창시킬 반지름 크기를 설정 (meter)
cost_scaling_factor (default : 10.0) : 팽창시키는 동안 적용할 Scaling Factor. 아래 그림의 지수함수적으로 감소하는 Cell Cost의 지수함수의 감소 비율을 설정

Inflation radius가 통로를 거의 다 덮고, cost scaling factor를 조정하여 아래 curve의 기울기를 원만하게 해 Robot이 장애물에서 최대한 멀리 떨어지게 하는 것이 좋음

왼쪽 : inflation_radius : 0.55, cost_scaling_factor : 5.0
오른쪽 : inflation_radius : 1.75, cost_scaling_factor : 2.58

왼쪽 : inflation_radius : 0.1, cost_scaling_factor : 1.5
오른쪽 : inflation_radius : 0.5, cost_scaling_factor : 1.5
같은 cost_scaling_factor에서 inflation_radius가 커질 경우 장애물의 Cost값이 커져 로봇이 움직일 수 있는 공간이 줄어들어 최적화된 Path를 찾기 어려워진다

왼쪽 : inflation_radius : 1.0, cost_scaling_factor : 2.0
오른쪽 : inflation_radius : 1.0, cost_scaling_factor : 20.0
같은 inflation_radius에서 cost_scaling_factor가 작아질 경우 로봇이 움직일 수 있는 공간이 줄어들어 최적화된 Path를 찾기 어려워진다

- Static Layer : Global Costmap에게 Static Map 제공

map_topic (string, default : map) : Costmap이 해당 Topic의 Static Map Data를 Subscribe

- Recovery Behaviors

Robot이 Path를 따라가는 도중 어떤 이유로 인해 주행이 막혔을 경우 ROS에서 막힌 상황을 풀어주고 다시 Navigation을 진행하도록 Recovery Behavior를 수행
- Navigation Stack에 내제되어있는 기능
ROS Navigation Stack은 clear_costmap_recovery와 rotate_recovery라는 2가지 Recovery Behavior를 제공
- clear_costmap_recovery : Local Costmap을 Global Costmap과 같은 상태로 되돌림
- rotate_recovery : 현재 공간에서 360도 회전을 통해 Recover 진행. 회전 실패로 진행하지 못할 수도 있으며 이 경우 Robot이 Recovery Behavior를 포기할 수도 있음 -> 로봇이 멈출 위험성이 높아짐
- SMACH 사용을 통해 극복 가능? (Robot과 가까운 임시 Goal 설정, 전에 갔었던 Pose로 되돌아 가기 등 사용)
이를 사용하기 위해선 move_base_params.yaml에서 아래의 Parameter를 활성화해야 함
- recovery_behavior_enabled (defalut : true) : Recovery Behavior의 활성화 여부를 결정
Rotate Recovery : Robot이 360도로 회전하여 공간을 Clear 하는 것. 관련 Parameter는 아래와 같다
- /sim_granularity (double, default : 0.017) : Robot이 회전할 때, 해당 각도 이내에 장애물이 있는지 확인하여 현재 환경이 안전한지 확인. Radian 단위이며, 기본값은 1도
- /frequency (double, default : 20.0) : move_base node로 보내는 Velocity Command의 Frequency (hertz)
Clear Costmap : Robot의 특정 지역 바깥쪽에 있는 장애물을 모두 Clear함. Local Costmap은 Global costmap과 비슷하게 생겨야 함.
move_base node는 '/move_base/clear_costmaps' Service를 통해 Costmap을 Clear함함

출처 : https://soohwan-justin.tistory.com/40

/move_base/clear_costmaps Service 호출 전(왼쪽) / 후(오른쪽)
해당 Service는 아래의 명령어를 통해 호출할 수 있다

$ rosservice call /move_base/clear_costmaps "{}"

- Oscillation Suppersion

Osciilation은 x, y, theta 중 하나가 음수와 양수의 값이 연속해서 나올 때 발생함
이를 방지하기 위해 Robot이 어떤 방향으로 움직일 때 반대쪽 방향은 다음 cycle에서 갈 수 없는 방향이라 표시한 후 Robot이 그 Flag가 설정된 위치에서 어느정도의 거리를 움직이기 전까지는 그 쪽으로 못가게 함
관련된 move_base의 Parameter는 아래와 같다
- oscillation_timeout (double, default : 0.0) : Recovery Behavior가 실행되기 전 Oscillation을 몇초간 허용할지를 설정. 값이 0.0일 경우 무한대로 대기하여 Recovery Behavior가 발생하지 않음
- oscillation_distance (double, default : 0.5) : Oscillating이 아니라고 판단하기 위해 Robot이 몇 미터정도를 움직여야 하는지 설정. 이 값보다 멀리 움직이는 경우 oscillation_timeout값을 초기화하여 다시 셈

- Dynamic Reconfigure

위의 Parameter 값들을 File에서 직접 수정하는 것이 아닌, rqt_reconfigure를 사용하여 실시간으로 조절하며 변경 결과를 확인할 수 있음
아래의 명령어를 통해 진행할 수 있다

$ rosrun rqt_reconfigure rqt_reconfigure

참고 자료 :

https://kaiyuzheng.me/documents/navguide.pdf

https://soohwan-justin.tistory.com/39

http://wiki.ros.org/move_base

move_base - ROS Wiki

melodic noetic Show EOL distros: EOL distros: electric fuerte groovy hydro indigo jade kinetic lunar diamondback: Only showing information from the released package extracted on Unknown. No API documentation available. Please see this page for in

wiki.ros.org

http://wiki.ros.org/navfn

navfn - ROS Wiki

wiki.ros.org

http://wiki.ros.org/amcl

amcl - ROS Wiki

wiki.ros.org

http://wiki.ros.org/costmap_2d

costmap_2d - ROS Wiki

Costmap2DROS The costmap_2d::Costmap2DROS object is a wrapper for a costmap_2d::Costmap2D object that exposes its functionality as a C++ ROS Wrapper. It operates within a ROS namespace (assumed to be name from here on) specified on initialization. Example

wiki.ros.org

'Study_ROS' 카테고리의 다른 글

About AMCL (Adpative Monte Carlo Localiztion) (0)	2023.10.25
Cartographer/Hector SLAM 사용법 (0)	2023.02.05
Ubuntu 20.04에 ROS Noetic 설치법 (0)	2023.01.21
[ROS] tf란? (0)	2022.11.06
우분투에서 Arduino IDE를 통한 rosserial 사용법 (0)	2022.11.05

천릿길도 한 걸음부터

About Navigation on ROS

'Study_ROS' 카테고리의 다른 글

티스토리툴바

About Navigation on ROS

'Study_ROS' 카테고리의 다른 글

'Study_ROS' Related Articles

티스토리툴바