1. The Normal Distribution Transform
A New Approach to Laser Scan Matching
Peter Biber
190404
도정찬
2. 0. 요약
• 수많은 위치 추정과 맵핑 알고리즘의 기본 요소는 2D 스캔 데이터 매칭이다. 대부분의 매칭 알고리즘들은 사용
한 피처간 선 혹은 점 끼리 일치하는지 찾는다. 본 논문에서는 대안으로 정규분포 변환을 제안한다. occupancy
grid와 비슷하게 2d 평면을 셀로 나누고, 각 셀에 point 측정 확률을 모델링하는 정규분포를 준다. 이 변환 결과
는 연속적 이며 뉴턴 알고리즘으로 다른 스캔과 매칭하는데 쓸수있는 구분가능한 확률 분포들이다.
• 이 알고리즘으로 위치 추정과 SLAM을 진행한다. 첫번째 결과는 실제 공간에서의 결과로 실내 공간에서 실시간
으로 오도메트리 데이터 없이도 신뢰할만한 결과를 얻었다.
3. 1. 소개
• SLAM은 모바일 로봇의 기본 능력이 된다. 레이저 스캔은 다양한 상황에서 적은 노이즈와 고신뢰성으로 주로
사용되는데 대부분의 SLAM알고리즘은 2개의 scan데이터나 스캔데이터를 map에 매칭하는것을 기반으로 한다.
앞으로 스캔 매칭의 저수준에서 처리하는 새로운 방법으로 어떻게 맵을 만들어 냈는지 보여준다.
• 이 논문의 목표는 NDT라 부르는 정규 분포 변호나과 한 스캔 데이터를 다른 스캔 데이터에 매칭하거나 다른
여러 스캔 데이터와 매칭시키는 어플리케이션을 소개한다. NDT는 이산 2차원 점 집합을 싱글 스캔 데이터에서
2차원 평면에서 연속적이고 미분가능한 확률 분포로 변환한다. 두번째 스캔 데이터를 NDT로 매칭하는건 두 번
째 스캔으로 정렬된 점들의밀도 합을 최대화 시켜 매칭한다.
• 또 SLAM 문제에 대한 좋은 매칭 방법을 소개한다. 하지만 제안하는 매칭 방법은 이 알고리즘에 의존하지 않는
다. 2장에서 두 스캔 데이터의 매칭하는 방법에 대해서만 보고 Thrun [15]이나 Gutmann [6]의 방법으로 전체
맵을 만들어 보지는 않는다. 그렇지만 이러한 방법은 맵과 위치 추정의 우도 모델이 된다.
• 이 문서의 나머지는 다음과 같다. 색션 3은 NDT, 색션 4는 스캔 매칭 방법과 스캔과 NDT 비교하며 색션 5에서
뉴턴 알고리즘을 이용하여 최적화가 된다. 이 스캔 매처는 색션 6에서 위치 추정에 사용하며 SLAM에 대해서는
색션 7에서 본다. 마지막으로 현실에서 결과와 결론을 보여주고 앞으로의 일을 설명하며 마친다.
4. 2. 이전 작업들
[3] I.J. Cox. An experiment in guidance and navigation of an autonomous robot vehicle, 1991
[7] J. Gutmann. Amos: Comparison of scan matching approaches for self-localization in indoor environments,1996
[8] J. Gutmann. Fast, accurate, and robust self-localization in polygonal environments,1999
[9] F. Lu and E. Milios. Robot pose estimation in unknown environments by matching 2d range scans,1994
[1] P.J. Besl and N.D. McKay. A method for registration of 3d shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992
[2] Y. Chen and G.G. Medioni. Object modeling by registration of multiple range images. Image and Vision Computing, 1992
[18] Z. Zhang. Iterative point matching for registration of free-from curves and surfaces, 1994
[10] F. Lu and E. Milios. Globally consistent range scan alignment for environment mapping.
• 두 스캔 매칭의 목표는 두 위치간 상대적인 위치를 찾아내는것이다. 가장 성공적인 알고리즘의 기초는 두 스캔
데이터 간 일치하는 정도가 되며 Cox는 점들을 이전 모델에서 만든 선들을 매칭시켰다 [3]. 아모스 프로젝트 [7]
에서는 스캔 데이터로부터 선을 추출해내고, Gutmann은 스캔에서 뽑아낸 선들을 모델에 매치 시켰다[8]. 가장
일반적인 방법은 점대 점으로 Lu and Milios [9]가 제안하였으며 ICP(Iterated Closest Point) 알고리즘을 이용하
여([1],[2],[18]) 레이저 스캔 매칭에 사용되었다. 여기서는 Lu and Milios 를 따랐으며 [10]처럼 명확한 지도를
그리는게 아니라 함축적인 지도로서 복원된 위치에서 선정된 스캔 데이터들을 사용한다.
• 이러한 방법들 모두 명확하게 일치하는 부분들이 만들어지며, 우리의 방식은 이 점이 다른데 원시 데이터 간 일
치 되는 정도를 찾을 필요가 없다. 정합문제를 해결하기 위한 다른 방법들도 존재하는데, [12] Mojaev는 확률적
위치 판단 오도메트리 모델로 local polar occupancy grid의 상관관계를 조합시켰다. Weiss와 Puttkammer[17]은
두 위치간 회전 복원 시키는 각 히스토그램을 사용하였는데, x y 히스토그램들은 평행이동을 복원하기 위한 공통
방향을 찾은 뒤에 계산된다. 이 방법들은 역시 차선책으로 사용된다.[7]
5. 2. 이전 작업들
• 본 논문에서는 컴퓨터 비전도 이용하였으며 이미지 강도로 확률 밀도를 대체하면 이 방법은 [13]의 피처 추적
이나 파노라마 구성[14] 을 하는데 사용할 수 있따. 이 기술들은 각 상대적 위치에서 파라미처 추정을 위해 이미
지 그라디언트를 사용한다. 여기서는 이미지 그라디언트와는 달리 정규분포 미분을 하며 계산될 수 있다.
[13] Jianbo Shi. Good features to track, 1994
[14] R. Szeliski and H.Y. Shum. Creating full view panoramic image mosaics and environment map, 1997
6. 3. 정규 분포 변환
• 이번 장에서는 이 논문의 핵심인 단일 스캔데이터로 정규분포 변환 하는 방법을 설명한다. NDT를 사용하면서
위치 추적이나 SLAM을 더 직관적이게 되었다.
• NDT는 지역 정규 분포 집합으로 한 스캔 데이터의 모든 2차원 점들의 분포를 만든다. 첫째, 2차원 공간을 일정
한 사이즈의 셀로 나눈다. 그러고나서 각 셀은 적어도 3개의 점을 갖고 있는데 아래의 과정을 거치게된다.
• 이 셀에 포함된 2차원 점 x의 샘플 측정확률은 정규 분포 로 만들 수 있다.
• occupancy grid처럼 NDT는 면의 부분구역을 만들지만 occupancy grid는 cell이 차 있을 확률을 나타내고, NDT
는 셀 각 위치에서 측정한 샘플의 호가률을 나타낸다. 셀을 100cm x 100cm로 사용해보자.
• 이 표현을 하기위해 뭘 사용할까? 우리는 확률 밀도 함수의 형태에서 2차원 평면의 연속성과 미분가능성 기술자
를 갖고 있다. 예제를 보기전에 2가지의 경우를 보자.
• 이산화 영향을 줄이기 위해, 4개의 겹침 그리드를 사용했는데, 단일 셀의 옆면 길이가 l인 한 그리드가 있고, 두번
째 그리드는 l/2만큼 수평으로 밀렸으며, 샛째는 l/2만큼 수직으로 밀렸고 마지막은 수직 수평으로 l/2만큼 밀렸
다. 각 2d 점은 4개의 셀로 밀리며, 이에 대한 내용은 나머지 논문 내용이랑 연관되지는 않는다. 이 논문에서 제
안하는 알고리즘은 point당 한 셀만 있다.
1) 이 상자에 포함하는 모든 2차원 점들( )을 모은다.
2) 평균 을 구한다.
3) 공분산 행렬을 계산한다.
7. 3. 정규 분포 변환
• 만약 점의 정규분포가 계산되면 4셀의 밀도는 추정되고 결과를 합칠 수 있다.
• 두번째 문제는 노이즈는 측정된 공간 선에 자유롭지만 공분산 행렬이 단일행렬이 되며 역행렬로 만들수 없게
된다. 이문제를 막기위해선 적어도 0.001회의 고유치가 작은지 큰지 확인해야한다.
• 아래의 그림 1은 레이저 스캔과 NDT 결과를 시각화한 사진으로 각 점에대해 확률 분포를 추정하여 만들었으며,
밝은 부분은 높은 확률밀도를 의미한다. 다음 색션에서는 어떻게 이 변환을 두 레이저 스캔 데이타를 정렬하는
지 본다.
8. 4. 스캔 정렬
• 두 로봇 좌표계 사이이의공간 맵핑 T는 아래와 같으며 는 두 좌표계간 평행이동 는 회전각이 된다.
• 스캔 정렬의 목표는 두 위치에서 측정한 레이저 스캔데이터를 사용하여 이 파라미터를 구하는 것이다. 두 스
캔 데이터를 이용하여 측정하는 방법은 아래와 같다.
• 1~4 단계는 직관적이다. NDT를 만들고 odometry데이터로 추정 파라미터를 초기화한다. 두번째 스캔 데이터
를 T를 사용해서 맵핑하고 정규분포 일치하는 정도를 NDT grid로 쉽게 찾는다.
1) 첫 스캔에 대해 NDT를 만든다.
2) 0이나 오도메트리로 파라미터 추정을 초기화 한다.
3) 두번째 스캔의 각 샘플에 대해 2차원 점들을 첫 스캔의 좌표계에 맵핑한다.
4) 각 맵핑된 점들끼리 정규분포 일치하는 정도를 판단한다.
5) 점들의 분포를 추정하고 협쳐서 파라미터의 점수를 구한다.
6) 최적의 점수를 찾기위해 새 추정 파라미터를 구한다. 이때 뉴턴 알고리즘으로 수행된다.
7) 결합 기준에 맞을때 3부터 다시 반복한다.
9. 4. 스캔 정렬
• 나머지 내용는 아래와 같다.
• 모든 점 에서의 파라미터 공분산과 평균이 최대일때 p에 따라 맵핑한게 최적이 된다. 이 합계가 p의 점
수가 되며 아래와 같이 정의 된다. 이 점수는 다음 색션에서 최적화된다.
• P = : 추정 파라미터 백터
• : 두번째 스캔 좌표계에서 샘플 i의 2d point
• : 파라미터 p에 따라 첫번째 스캔 좌표계로 를 맵핑한 결과로 = T( ,p)
• : 에서의 정규분포를 따르는 공분산 행렬과 평균으로 첫번째 스캔의 NDT에서 찾게된다.
10. 5. 뉴턴 알고리즘을 이용한 최적화
• 최적화 문제는 보통 최소화로 다루기 때문에 여기서도 이 관례를 따른다. 그러므로 이번 장에서는 –score를
최소화(-score가 작아질수록 실제 점수는 올라감) 한다. 뉴턴 알고리즘은 반복하여 함수 f를 최소화 시키는 파
라미터 p = 를 찾는것으로 각 반복은 아래의 방정식을 풀게 된다.
• g는 함수 f의 전치된 그라디언트이며 H는 f의 해시안 행렬이 된다.
• 이 선형 시스템의 해결방안은 증감 이미 현재 추정에 더하는데 아래와 같다.
• 만약 H가 positive definite 행렬이면, f(p)는 방향으로 감소한다. 만약 적용되지 않으면 H를 = H +
로 대치시키고, 가 positive definite가 되도록 을 선정한다. 최소화 알고리즘의 자세한 내용은 [4]에서
찾을수 있다.
positive definite matrix(양 확정 행렬) : x ^tAx^t > 0 이면, A를 positive definite라 한다.
행렬 A가 positive definite이면 아래의 성질을 만족한다.
(1) A의 모든 eigen value가 양의 실수이다.
(2) A의 모든 sub-determinant가 양의 실수가 된다.
11. 5. 뉴턴 알고리즘을 이용한 최적화
• 이 알고리즘은 함수 –score에 적용하며 그라디언트와 해시안은 방정식 3의 피 가수들의 편미분으로 만든다.
갈략한 표기법에서는 오해를 줄이기 위해 파라미터 수 i와 래이저 스캔 샘플 인덱스 i을 없앴지만 원래는 아
래와 같이 작성한다.
• 각 p에 대한 q의 편미분은 의 편미분과 같으며 –score를 s로 하면 어래와 같이 된다.
• 이 가수를 이용해서 그라디언트들은 아래와 같이 돤다.(chain rule에 따라)
• p_i에 대한 q의 편미분은 자코비안 행렬 로 나타낸다.
12. 5. 뉴턴 알고리즘을 이용한 최적화
• 해시안 H이 가수들은 아래와 같이 구한다.
• q의 이차 미분은 아래와 같다.
• 이 방정식으로 그라디언트와 해시안을 구하기 위한 비용이 낮으며 낮은 수가 나올때 지수함수를 사용할 수
있다. 삼각법 함수는 파이(현재 추정각)에 의존하며 각 반복중 한번 호출되야한다. 다음 색션은 이 알고즘으로
위치 추정과 slam을 하는데 사용한다.
13. 6. 위치 추적
• 이번 색션에서는 어떻게 스캔 매칭 알고리즘이 주어진 시간 t = t_start에서 현재위치를 추적하는데 적용되는
지 알아본다. 그러고 나서 다음장에서 SLAM에 대해 본다.
• 전역 참조좌표계는 이때 지역 로봇좌표계에서 정의하며 각 레이져 스캔을 키프레임이라 한다. 추적은 각 키
프레임에서 수행된다. 시간 t_k에서 알고리즘은 아래의 과정을 따라 수행된다.
• 스캔이 충분히 기존의 기준에서 키프레임과 현재 좌표계, 그리고 결과를 사이에 평행, 각도 거리를 포함해 만
족하는지에 따라 결정한다. 유용한 위치 추정을 위해 이 위치 추정은 실시간으로 이루어지며 한 스캔의 NDT
를 만드는데 1.4Ghz 컴퓨터에서 10ms 정도 걸린다. 적은 움직임에 대해 최적화 알고리즘은 1-5의 반복(드물
게 10회 이상)정도 필요하며 각 반복당 약 2ms정도 걸리므로 실시간은 문제없다.
1) 를 t시간 t_k-1에서 t_k 사이의 움직임에대한 추정이라 하자(예를들어 오도메트리)
2) 에 따라 시간 t_k1에서 위치 추정을 맵핑한다.
3) 현재 스캔데이터로 최적화 알고리즘을 수행한다. 키프레임의 NDT와 새 위치를 추정한다.
4) 키프레임이 현재 스캔과 비슷한지 확인하며 만약 맞으면 반복한다. 그렇지 않으면 지난번 성공한 스캔을
새 키프레임으로 사용한다.
14. 7. SLAM을 위한 응용
• 전역 위치와 키프레임으로 지도를 정의하였다. 이번 장에서는 로봇이 모르는 장소에 갔을때 이 맵으로 어떻
게 위치 추정을 수행할 지와 늘이고 최적화할지에 대한 방법을 설명한다.
• A. Localizing with respect to multi scans
• 지도에서 각 스캔 i에 대해 각 (회전 행렬 )와 평행 백터 가 연관된다. 이것들로 전역 좌표계에서
스캔 i의 위치를 구하는데 사용한다. 현재 로봇의 위치를 회전 매트릭스 R과 평행이동백터 T라 하자. 로봇 좌
표계에서 스캔 i의 좌표계로 맵핑한 은 아래와 같다.
• 색션 V의 알고리즘을 이 상호아에 적용하기 위해선 약간으 변화가 필요하다. 스캔 i의 2d point의 맵핑은
로 적용하여 계산된다. 더나아가 자코비안과 2차 편미분으로 조금더 복잡하게 얻을수 있으며 자코비안 맵핑
과 의 2차 미분은 아래와 같다.
• 최적화 알고리즘을 위한 그라디언트와 해시안은 모든 스캔을 합쳐 구할수 있다. 하지만 더 빠르고 좋은 결과
를 구하는 방법을 찾았는데 로봇 위치에서 구한 각 스캔으로 확률 밀도 추정 결과가 최대가 되는 스캔을 결적
하는 방법으로 여기서 스캔은 현재 반복과 샘플로만 사용한다. 이방법은 겹치는 키프레임의 수에 독립적인
최적화 알고리즘을 위한 그라디언트와 해시안을 구해야한다.
15. 7. SLAM을 위한 응용
• B. Adding a new keyframe and optimizing the map
• 각 시간 단계마다 지도는 전역 좌표계에서 그들의 위치를 갖는 키프레임들의 집합으로 이루어진다. 만약 현
재 스캔과 맵이 겹치는 부분이 작다면 맵은 이전에 마지막으로 맷칭된 스캔으로 확장된다. 그러고나서 매 겹
치는 스캔은 새 키프레임에 부분적으로 맷칭되면 두 스캔간 상대적 위치를 구한다. 그래프는 쌍 매칭 결과를
유지한다.
• 이 그래프에서는 모든 키프레임을 노드로 나타낸다. 이 노드는 전역 좌표계에서 키프레임의 위치에대한 추정
을 가지고 있으며 두 노드간 엣지는 스캔이 매칭되고 두 스캔간의 상대적 위치에 대해 갖게된다. 새 키프레임
이 추가된 후에는 맵은 모든 키프레임의 파라미터로 정의되는 에러함수를 최적화 시켜 개편된다. 정렬 결과
는 이차 에러 모델을 정의하는데 사용된다. 두 스캔의 전역 파라미터역시 상대 위치를 추정한다. 는 전역
파라미터로 정의된 상대 위치와 쌍 매칭의 결과로 구한 상대 위치간 차이가 된다. 그러고나서 2차식 모델을
사용하여 의 함수로 두 스캔의 점수를 모델링한다.
• 쌍 매칭이 합쳐지고 해시안을 구할때의 점수가 최종 점수가 된다. 이 모델은 =0일때 점수의 테일러 전개
식으로 구하였다. 여기서 선형 표현이 없는데 point에 대해 확장했기 때문이다. 이 점수는 이제 합치고 최적
화된다.
16. 7. SLAM을 위한 응용
• 만약 키프레임의 수가 커지면 최소화는 실시간으로 수행되기 어렵다(free 파라미터의 수가 3N -3개인데 N은
키프레임의 갯수이다). 그러므로 지도의 서브그래프를 최적화해야된다. 이 서브그래프는 모든 키프레임을 모
아 만들며 새 키프레임의 노드를 세 에지만 가로질러 할수 있다. 이 서브그래프에 들어있는 키프레임에 속하
는 각 파라미터들로 이루어진 에러함수를 취적화 한다. 만약 반복이 닫혀야하면 모든 키프레임에대해 최적화
애햐된다.
17. 8. 결과
• 이제 odometry 없이 수행한 결과를 보여주며 강건한 방법으로 나타내었다. 쓰런이 이미 [15]에서 했다. 만들
어진 지도는 그림 2에서 보여주며 로봇을 연구실에서 복도 안 등 주행하여 얻어졌다. 이 상황에서 넓은 맵과
이 맵에대한 지역화가 수행되는데 로봇은 20분간 28430 래이저 스캔을얻었으며 약 83 미터정도를 가로질렀
다. 이 스캔은 180도다 감지가능한 SICK 래이저 스캐너를 사용하였다.
18. 8. 결과
• 고속으로 시뮬레이션하기위해 겨우 50번 정도 스캔하였으며 속도는 약 35cm/s 정도로 초당 스캔데이터의 수
는 약 23개이다. 이 맵은 여러 방법들을 합쳐 만들었다. 색션 4의 위치 추적기가 매 스캔마다 적용되며 반면
에 최종 시간의 결과를 선형적으로 추정하여 파라미터를 초기화 시켰다. 매 50번 스캔시 색션 7이 수행되었
다.
• 그림 2는 결과 맵으로 최종 맵은 33개의 키프레임으로 구성된다. 우리의 스캔 매칭 알고리즘이 닫힌 문이나
열린 문과같은 사소한 환경 변화에도 졀 유지된다. 오프라인에서 모든 좌표계를 처리하는데 1.4ghz 컴퓨터에
서 58초 정도 걸렸으며 초당 96스캔이 처리되었다. 이 구현을 자바에서 다른 빠른 언어로 포팅하면 더좋은
성능을 얻을것같다.
19. 9. 결론과 앞으로 할일
• 본 논문에서는 스캔하는 새로운 방법인 Normal Distribution Transform(NDT)를 소개했다. 이 변환은 다른 스
캔 데이터를 매칭하기 위한 표현들을 구하였다. 또 개발한 스캔매처가 어떻게 위치 추적과 SLAM 문제를 수
행하는지 설명하였으며 이 방법은 다음과 같은 장점을 갖는다.
• 이 질문 : 지역 정규 분포로 모든게 잘 모델 되나요?
• 지금까지 실내 환경에서 진행했지만 아무런 문제는 없었다. 더나아가 실외에서 진행할 예정이다. 역시 Lu
and Milios와 우리의 방법을 합쳐서 비교할 예정이다.
- 점이나 피처간 명확하게 일치되는 부분이 없어도 된다. 대부분의 방법들에서 가장 오류가 발생하기 쉬운
부분이나 일치시키는것 없이 강건하게 수행되었다.
- 모든 내용들을 빠르고 정확하게 계산될 수 있따.
