NDT 기반 SLAM 예
로봇 등 무인자율주행 장치가 주변 공간 위치를 파악할 때 실시간으로 생성되는 지도가 필요하다. 보통, 로봇에 마운트된 거리 센서 등을 스캔 센서에서 얻은 상대 거리 좌표(x, y, z) 정보가 포함된 점군(point cloud)에서 지도를 생성해야 하므로, 이는 정확히 말하면 각 장면별로 획득한 스캔 데이터를 정합하는 문제로 치환할 수 있다.
NDT SLAM 알고리즘 설명
각 장면에서 얻은 점군들이 {PCD1...PCDn} 이라면, 서로 중첩했을 때 오차가 제일 적은 정합 결과물이 제일 좋은 것이다. 이렇게 정합하는 방식 중 가장 유명한 것이 ICP(Iterative Closest Points)이다. ICP에서 모든 점들에 대해 정합 오차를 계산하면 시간이 너무 오래 걸리므로, 각 장면별 유일한 특징이 될만한 특징점을 먼저 추출한 후 ICP를 실행한다.
LiDAR를 이용한 실시간 정합은 보통 LiDAR SLAM으로 알려져 있는 데, 그 동작 메커니즘은 비슷하다. 다만, 빠른 계산 속도를 만족해야 한다는 조건이 더 붙는다.
NDT는 3D 공간에서 voxel 공간 인덱싱을 처리해, 각 복셀내 점군들 3D 가우시안 분포로 근사화시켜 빠른 계산 속도를 만족시킨다.
복셀은 스캔된 점군 밀도와 SLAM 계산성능 간의 tradeoff 함수이다. 이는 상황에 따라 다르다. 복셀에서 신뢰성 있는 특징들을 신속하게 얻는 것은 SLAM에서 매우 중요하다. 각 장면의 특징들을 얻은 후 이 특징들이 장면마다 일관성(consistency)을 유지하는 지 계산한다.
높은 일관성있는 특징들(평면과 모서리 부분. 색상이 밝은 점들은 일관성이 있다고 판정된 데이터임)(Stanford University, 2019)
결국 NDT SLAM 구조를 잘 설계하기 위해서는 특징 매칭 수준의 오류를 줄이는 것이 중요하다. 각 장면 연속된 점군 PCD1, PCD2의 NDT SLAM odmetry(주행괘적) 계산 및 매 키프레임(keyframe) 에 따른 지도 갱신 아키텍처는 다음과 같다.
NDT SLAM algorithm(Stanford University, 2019)
LiDAR odometry 부분은 다음과 같다. 처리 순서는 입력된 점군 PCD에서 얻은 특징들을 대략적으로 정합해 최적화한 후, 각 복셀에 포함된 점군을 좀 더 높은 정확도로 정합 계산하는 과정이 수행된다. 이를 미리 지정된 정확도까지 만족하도록 최적화해 Fine optimization 솔류션을 얻도록 반복 수행한다.
LiDAR odometry module(Stanford University, 2019)
Retain high consensus voxels는 맵핑 최적화를 위해 지역적으로 최적해를 만족하는 consensus metric를 계산하는 과정을 포함한다.
Map Update(Stanford University, 2019)
Consensus Metric은 각 점군이 후보 위치로 변환되었을 때를 테스트한다. 주어진 후보 위치에 대해 모든 복셀 컨센서스 매트릭스를 결합하고, 테스트하는 과정을 거친다.
환경준비
개발을 위해 우분투, ROS, 벨로다인 VLP-16 센서 구동 환경을 미리 설정해야 한다. 이 내용은 아래 링크를 참고한다.
벨로다인 LiDAR 센서 개발 환경 구축 결과
빌드 및 실행
NDT를 사용한 Graph SLAM을 간단히 빌드하고 실행해 본다. 여기에서는 다음 패키지를 사용한다.
이 프로그램은 VLP-16 벨로다인 센서를 이용해 SLAM 테스트된 것이다. 이 프로그램은 NDT 알고리즘을 기반으로 정합을 수행한다. LOOP 검출, Back-end 최적화 및 전역 일관성을 처리하여 맵을 생성한다. 모듈 구성은 다음과 같다.
- floor_filter: 바닥 점군을 필터링함. 주어진 높이, 점 법선 및 점군 밀도를 이용해 바닥 제거.
- geo_transform: 좌표 변환. WGS84 to UTM변환 제공.
- key_frame: 각 스캔 점군 프레임 관리. id, 위치(matrix4), 점군을 관리함
- lidar_slam_3d_ros: ROS 기본 모듈. map, path, GPS, pose, filtered PCD, GPS, TF 등 ROS 메시지를 생성 및 구독함.
- map_builder: 지도 생성.
- math_func: 기본 수학 계산 함수
이 패키지는 ROS, g2o(general graph optimization. BSD license)를 사용한다. g2o는 Levenberg–Marquardt(L-M) 수치해석과 같은 비선형 커브피팅 최적화 알고리즘을 포함한다. L-M은 주어진 모델과 데이터간 오차가 최소제곱이 되도록 커브피팅하는 문제를 해결할 때 사용된다. 예를 들어, 데이터 셋 (Xi, Yi)가 있을 경우, 아래 수식에서커브 f의 파라메터 B를 찾는다. 이때 편차함수 S(B)는 최소가 된다.
git clone https://github.com/RainerKuemmerle/g2o
cd g2o
mkdir build
cmake ../
make
참고로, 우분투 16.04에서는 버전 문제로 make build 에러가 발생할 수 있다. 이 경우, 아래에서 2017년 g2o버전을 다운로드받아 빌드해보면 제대로 설치될 것이다.
빌드된 g2o의 라이브러리를 아래와 같이 복사한다.
sudo cp ~/g2o/lib/*.* /usr/local/lib
소스 빌드를 위해 다음과 같이 실행하고 빌드한다. 우분투 18.04에서는 라이브러리 버전 차이로 문법 에러가 발생할 수 있다. 적절히 소스 코드를 수정한다(github issue 참고).
cd catkin_ws/src
git clone https://github.com/ningwang1028/lidar_slam_3d
cd ..
catkin_make
빌드 후 다음과 같이 실행한다.
roscore
roslaunch lidar_slam_3d lidar_slam_3d.launch
ROS 실행화면
라이다를 이동 시키며 점군을 실시간으로 획득해 SLAM 처리하면 다음과 같은 결과를 얻을 수 있다.
SLAM 결과
NDT SLAM은 ICP와 같이 비선형 회귀분석 모델에 대한 커브피팅에 많은 계산 시간이 필요하다. SLAM은 엔비디아 TK2에서 실행해 보았고, 처리속도는 그리 빠르지 않은 것을 보아서는 최적화가 필요한 것으로 보인다. 아울러, 다른 SLAM과 같이 급격한 회전과 바닥 등 일정한 특징이 보이지 않은 복잡한 환경에서는 여전히 정합 결과가 튀는 문제가 발생한다.
SLAM은 특정한 제약된 환경에서는 안정적일 수 있지만, 알고리즘에 의도하지 않은 곳에서는 이처럼 여러가지 문제가 발생할 수 있다. IMU등을 통해 이상치를 검색하고 개선할 수 있지만, SLAM만으로는 문제 해결에 한계가 있을 수 있다.
이 글에서 간단하게 NDT SLAM의 개념과 개발 방법을 다루어 보았다. SLAM은 각 장면별 점군 실시간 정합을 위한 다양한 방법이 있다. 이는 응용 목적에 따라 다를 수 있다.
레퍼런스
이 내용은 다음 링크를 참고하였다.
- ASHWIN VIVEK KANHERE AND GRACE XINGXIN GAO, 2019, Standford University, LiDAR SLAM Utilizing Normal Distribution Transform and Measurement Consensus,
댓글 없음:
댓글 쓰기