Robotics

Elevation Mapping 구현

연 수 2025. 1. 24. 16:02
반응형

Elevation Mapping 구현하기

  • Isaac Sim 환경에서, Gridmap 기반 Elevation Mapping 구현 진행
    • 기존 Gridmap 패키지 기반 grid_map_pcl 모듈을 활용 
      • 해당 모듈에서는, 실시간 pointcloud를 기반으로, Gridmap을 Publisher하는 기능은 없음
      • PCD 파일을 저장하여, Gridmap으로 표현
      • 따라서 해당 모듈을 개발하여 실시간 Gridmap을 생성하고자 함.
    • Lidar 기반 Pointcloud 변환 후 GridMap
      • 비교적 넓은 범위를 표현할 수 있으나, Sparse함.
    • Depth Image 기반 Pointcloud 변환 후 GridMap
      • 비교적 좁은 범위를 표현하나, Dense함. 
    • 근본적으로 사족 보행 로봇의 다양한 환경에서 local planner를 위한, local costmap을 구현이 목표임으로, Depth 기반 선정 
      • 각 그리드 셀에 대해 주어진 포인트들의 높이를 계산하여, 해당 레이어를 채워 넣는다. 
        • 이때, 처음에는 pointcloud 개수와 grid cell 자체의 개수가 많아 약 2.4초가 걸렸다. (매우 느림)
        • 따라서, 전방, 양옆, 후방 영역을 제한하여 포인트 클라우드 개수 자체를 줄였다. 
        • point cloud 개수 자체를 많이 줄였으나, 여전히 계산하는데 오래걸리며 로봇이 급격하게 돌 때 노드가 종료되는 현상이 발생하였다.
          • 노드가 꺼지는 경우는, pointcloud가 0개, 즉 입력이 전혀 들어오지 않아 예외처리를 진행하여 해결하였다.
        • pointcloud의 다운 샘플링을 진행하여, pointcloud의 개수를 줄였다. 
      • 현재는 매우 빠른 속도를 유지중이나, pointcloud의 개수가 부족하여 Gridmap 자체의 설명력이 부족하다고 생각한다. 
        • 튜닝을 통해, 로봇과 환경에 알맞은 값을 찾아야 한다.
    • 추가 진행 사항 
      • 추가적으로, 센서 tf를 확인하여 필터링을 적용하는 pointcloud의 프레임을 확인하여, 좀 더 명확하게 계산하고자 함
      • voxel grid (다운 샘플링)을 진행 시, x,y,z 설정을 각각 다르게 진행하여 point cloud 표현력을 높이자
      • elevation mapping 패키지 자체가 Grid map을 어떻게 사용하고 있는지를 찾아보자 
        • pointcloud 할당은 어떻게 진행 중인지, depth camera, lidar, rgb camera 등은 어떻게 표현하는지
        • gridmap은 어떻게 생성하고 있는지 고려
      • Moving Window (누적 맵) 구현 진행 완료 

Elevation Mapping 구현 진행 중

 

센서 tf 확인 해야함.

  • 조금 삐뚤어서 나오는 것 같음 -> 다시 확인하기
  • nvblox_node/back_projected_depth/front_stereo_camera_left_optical 에서 topic을 sub하여, grid map 생성 
    • 조금 왼쪽으로 보이는 듯
    • ros2 run tf2_ros tf2_echo base_link map / ros2 run tf2_ros tf2_echo front_stereo_camera_left_optical ma
      • 확인 시, tf 적으로는 문제가 없다고 생각

nvblox voxel + grid map
elevation mapping - grid map

voxel grid 다운 샘플링 진행 시, x,y,z 다르게 leaf size 파라미터 수정하는 코드 변경 완료

  • 추가적으로, 튜닝 하면서 진행 
    • VoxelGrid 다운 샘플링 (leaf size)
      • voxel_leaf_size는 m 단위의 큐브 크기를 의미
      • 한 voxel(leaf) 내부에 속한 점들을 하나의 대표로 정의하여 점 개수를 줄이는 방법
        • leaf size가 커질 수록 : 
          • 한 큐브에 더 많은 점들이 묶이므로, 다운샘플링 정도가 강해져 점의 총 개수가 크게 줄어듬
          • 연산량이 줄고(점이 줄어서) 처리속도가 빨라짐
          • 지도(맵)의 해상도/정밀도가 떨어지며, 미세 구조를 잃어버림.
        • leaf size가 작아질 수록 : 
          • 다운샘플링 효과가 거의 없어(거의 모든 점이 유지), 포인트가 많아짐.
          • 정밀도는 좋아지지만, 처리 시간이 길어짐.
        • 예시 : 
          • leaf=0.50로 되어있고, 그 결과 112,041점이 15점으로 확 줄어듬
          • 이는 leaf size=0.5m\text{leaf size} = 0.5m / 가 꽤 크기 때문에, 반경 0.5m마다 대부분의 점을 하나로 뭉쳐버리기 때문
      • GridMap 해상도  - Resolution
        • grid_map_pcl 내부에서 초기화 시, resolution이 GridMap의 셀 크기(m/셀)에 대응
        • 예를 들어 resolution = 0.1이면 10cm 간격, 0.2이면 20cm 간격
        • 해상도가 낮아질수록(큰 값일수록):
          • 맵은 더 적은 수의 셀로 표현 → 계산 속도가 빨라짐
          • 세밀도가 떨어져, 작은 장애물이나 언덕 등이 희석될 수 있음.
        • 해상도가 높을수록(값이 작을수록):
          • 더 많은 셀 → 연산이 많아져 느려질 수 있음
          • 세밀한 지형을 표현
      • GridMap - length_x, length_y
        • Grid_map_pcl에서 x,y 
          • GridMapPclLoader의 setMapGeometry에서 수동 지정 (확인하기) + resolution (yaml 파일 확인하기)
      • VoxelGrid leaf size 기준:
        • 0.1 ~ 0.2m 정도부터 테스트(실내환경)
        • 야외(더 큰 스케일)라면 0.5m 이상도 가능
        • 작을수록: 맵 품질 up, 처리 속도 down
        • 클수록: 맵 품질 down, 처리 속도 up
      • resolution 기준:
        • 실내 정밀 맵: 0.05~0.1m
        • 일반 로봇 네비: 0.1~0.2m
        • 야외 넓은 영역: 0.2m~1.0m
        • 작을수록: 맵 해상도(정밀) up, 처리 속도 down
        • 클수록: 맵 정밀 down, 처리 속도 up

elevation mapping 패키지 자체가 Grid map을 어떻게 사용하고 있는지 분석

  • Grid Map 생성 :  rawmap + fused map을 사용
    • raw map : 원본 센서를 사용하여, elevation, variance, color, time을 계산
      • 마할노비스 거리를 계산하여, elevation과 point z를 비교하여 이상치를 탐지
    • fused map : raw map의 데이터를 사용하여, 가우시안 기법과 가중치를 고려하여 데이터를 융합하여 생성한다.
      • 최종적으로 사용되는 grid map의 형태
  • Elevation Mapping 모듈 요약 - https://github.com/ANYbotics/elevation_mapping/tree/ros2
    • ElevationMap : grid map 저장 / 센서 데이터로부터 높이 정보 업데이트
    • ElevationMapping : elevation mapping 관리 / ros - pub / sub 요청 처리 
    • RobotMotionMapUpdater : 로봇 이동 정보를 반영하여 맵 변화 보정
    • PointXYZConfidenceRatio : PCLdmf tkdydgkdu RGB confidence ratio 정보를 포함한 pointcloud 생성
    • elevation_map_node : ROS 실행 노드
    • Input Source : 사용할 센서 데이터 입력 받고, Processor에 할당하기
      • Input : 각 센서에 적절한 입력 설정 / ROS Topic 구독 -> elevation mapping에 전달
      • InputSouceManager : 여러 센서 입력 관리 ( 센서가 여러개 일 때, topic이 꼬이지 않게 관리)
    • Sensor Processor : 센서 데이터 전처리 
      • SensorProcessorBase : 센서 처리를 위한 공통 기능 정의 (TF 변환, Pointcloud 변환, 필터링 - 전처리, voxel-grid, 클리핑 (높이 제거), 센서 신뢰도 계산)
      • LaserSensorProcessor : LiDAR -> Pointcloud 처리 
      • PerfectSensorProcessor : 노이즈 없는 센서 가정 (variance = 0)
      • StereoSensorProcessor : 스테레오 카메라 -> Pointcloud
      • StructedLightSensorProcessor : 구조광 기반 깊이 (Kinect 오차 모델 적용)
    • PostProcessing :  elevation mapping 후처리 기능 (생성 이후, 노이즈나 필터링 적용하여 최종적으로 정제된 맵 생성)
      • PostProcessingPipelineFunctor : grid map 필터링 체인 -> Grid map Publisher
      • PostProcessingWorker : elevation mapping 후처리 (병렬 처리 가능 (비동기적)) -> 다시 새로운 Grid map Pub
      • PostProcessorPool : Post Processing worker를 관리 (병렬 처리 후 실행 관리, 사용가능한 관리자에게 할당)

 

elevation mapping 패키지 구현

  • Elevation Mapping 초안 완료
    • 셀 크기 줄이기 -> resolution 설정 (완료)
    • 바닥 일정 높이 맞추기 (z축에 아무것도 나오지 않을 때, 무지개로 나오는 것) (완료)
      • custom_rviz에서  파라미터 Autocompute Intensity Bounds: false 변경
        • Max Intensity와 Min Intensity을 사용할 수 있도록
  • 향후 일정
    • Traversablitiy_estimation 구현
    • Go2 적용

 

반응형