KR101909953B1 - 라이다 센서를 이용한 차량의 자세 추정 방법 - Google Patents

Abstract

본 발명의 관성측정장치(Inertial Measurement Unit)가 내장되어 있고, 라이다(LiDAR, Light Detection And Ranging) 센서가 후방에 장착된 차량의 자세 추정 방법에서, 상기 라이다 센서를 이용하여 거리를 측정한 측정점 데이터를 획득하는 단계, 상기 측정점 데이터를 2차원 직교 좌표 상에 투영하여 도로선을 찾는 단계, 상기 도로선으로부터 차량의 자세 변화량(α', β')을 계산하는 단계 및 상기 자세 변화량(α', β')을 상기 관성측정장치의 측정치와 융합하여 차량의 자세(α, β)를 계산하는 단계를 포함한다.
본 발명에 의하면, 고가의 관성측정장치 또는 GPS 장비를 추가하지 않고, 기존의 차량에 기 부착되어 있는 라이다 센서를 이용하여, 차량의 자세를 빠르게 추정할 수 있는 효과가 있다.

Description

라이다 센서를 이용한 차량의 자세 추정 방법 {Method for vehicle pose estimation using LiDAR}

본 발명은 차량의 현재 자세를 추정하는 방법에 관한 것으로서, 더욱 상세하게는 후방 하향 주시 2D 라이다(LiDAR, Light Detection And Ranging) 센서를 이용하여 차량의 현재 자세를 추정하는 방법에 관한 것이다.

고속으로 달리는 자율 주행 차량의 제어를 위해서는 차량의 자세를 추정하는 것이 필수적이다.

도 1은 차량의 자세를 정의하기 위한 도면이다.

도 1을 참조하면, 차량에 3차원 좌표계가 고정되어 있는데 각 축을 기준으로 하는 회전을 x축 회전은 α, y축 회전은 β, z축 회전은 γ라고 할 때, α와 β는 자세(pose)로 정의하고, γ는 차량의 진행 방향(Heading)이라고 정의한다.

차량의 자세를 추정하기 위해 일반적으로 사용하는 장치로 3축 각속도, 즉 α, β, γ의 변화량과 3축 가속도가 측정되는 IMU(Inertial Measurement Unit, 이하 '관성측정장치'라 함)가 있다. 이때, α, β, γ 값이 바로 측정되지 않고 변화량이 측정되기 때문에 누적하여 측정하여야 한다. 그러나, 잡음 성분도 같이 누적되기 때문에 다른 측정치와 혼합하는 알고리즘이 필수적이다.

별도의 센서 추가 없이 관성측정장치의 3축 가속도 값을 혼합하여 추정하는 경우, 정밀도가 낮고 필터에 의한 지연속도도 문제가 된다. 고가형 제품을 사용하면 어느 정도 완화할 수 있으나, 관성측정장치는 정밀도가 높을수록 급격히 비싸지기 때문에, 일반적으로 차량의 자세 추정 시스템은 관성측정장치에 다른 센서를 추가 장착하여 구성한다.

GPS(Global Positioning System)를 추가하여 구성하는 경우, 차량의 위치 정보를 관성측정장치의 측정치와 융합하는 방법을 사용한다. 융합하면 적당한 위치 정밀도를 얻지만 정작 필요한 차량의 자세는 일반적인 GPS를 사용할 때, 동작 주기가 1Hz로 매우 느리기 때문에 신뢰도가 떨어진다. 고가의 수신기를 사용하면 위치 정보는 비교적 정확하지만 여전히 고속으로 주행하는 차량의 자세를 즉시 추정하기 어렵다는 문제점이 있다.

지자기 센서를 추가하여 구성하는 경우, γ의 변화량이 아닌 γ값을 직접 측정할 수 있어 차량의 자세와 방향을 모두 추정할 수 있다. 그러나 정밀도는 크게 높아지지 않으며 필터에 의한 지연속도는 여전히 줄어들지 않는다는 문제점이 있다.

대한민국 공개특허 10-2016-0057756

본 발명은 상기와 같은 문제점을 해결하기 위하여 안출된 것으로서, 자율 주행 차량의 후방에 장착되어 있는 2D 라이다(LiDAR, Light Detection And Ranging) 센서를 이용하여 차량의 자세를 추정하는 방법을 제공하는데 그 목적이 있다.

본 발명의 목적은 이상에서 언급한 목적으로 제한되지 않으며, 언급되지 않은 또 다른 목적들은 아래의 기재로부터 통상의 기술자에게 명확하게 이해될 수 있을 것이다.

이와 같은 목적을 달성하기 위한 본 발명의 관성측정장치(Inertial Measurement Unit)가 내장되어 있고, 라이다(LiDAR, Light Detection And Ranging) 센서가 후방에 장착된 차량의 자세 추정 방법에서, 상기 라이다 센서를 이용하여 거리를 측정한 측정점 데이터를 획득하는 단계, 상기 측정점 데이터를 2차원 직교 좌표 상에 투영하여 도로선을 찾는 단계, 상기 도로선으로부터 차량의 자세 변화량(α', β')을 계산하는 단계 및 상기 자세 변화량(α', β')을 상기 관성측정장치의 측정치와 융합하여 차량의 자세(α, β)를 계산하는 단계를 포함한다.

상기 도로선을 찾는 단계는, 극 좌표계로 표현된 상기 측정점 데이터의 집합인 점 집합 P를 직교 좌표계로 변환하는 단계, 크기가 고정된 각 격자마다 점유율을 갖고 있는 지도인 점유격자 지도를 가우시안 분포를 따르는 직선의 형태로 작성하는 단계, 상기 점 집합 P를 상기 점유격자 지도에 매칭하여 변환행렬을 구하는 단계, 상기 변환행렬을 계속 업데이트하여 최적화하는 단계 및 상기 최적화된 변환행렬로부터 도로선의 기울기와 절편을 도출하는 단계를 포함하여 이루어질 수 있다.

상기 변환행렬로부터 차량의 자세 변화량(α', β')을 계산할 수 있다.

상기 변환행렬을 최적화하는 단계는, 선형 보간된 점유율과 점유율 기울기 값을 계산하는 단계, 상기 선형 보간된 점유율과 기울기 값을 통해 변환행렬을 업데이트하는 단계, 업데이트된 변환행렬의 에러가 미리 정해진 제1 임계치 이상이고, 업데이트 반복 횟수가 미리 정해진 제2 임계치 이하이면, 다시 선형 보간된 점유율과 점유율 기울기 값을 계산하여 변환행렬을 업데이트하는 단계 및 업데이트된 변환행렬의 에러가 상기 제1 임계치 미만이거나, 또는 업데이트 반복 횟수가 미리 정해진 제2 임계치 이상이면, 최적화 과정을 종료하는 단계로 이루어질 수 있다.

본 발명에 의하면, 고가의 관성측정장치 또는 GPS 장비를 추가하지 않고, 기존의 차량에 기 부착되어 있는 라이다 센서를 이용하여, 차량의 자세를 빠르게 추정할 수 있는 효과가 있다.

또한, 본 발명은 기존에 차량에 장착되어 있는 라이다 센서를 이용하므로, 추가 비용 없이 차량 자세 추정 기능을 구현할 수 있는 효과가 있다.

도 1은 차량의 자세를 정의하기 위한 도면이다.
도 2는 2D 라이다 센서의 측정범위와 측정값의 예를 보여주는 도면이다.
도 3은 본 발명의 일 실시예에 따른 라이다 센서를 이용한 차량의 자세 추정 방법을 도시한 전체 흐름도이다.
도 4는 도로를 바라본 라이다 센서의 측정값들을 직교 좌표계에 표현한 도면이다.
도 5는 기존의 점 집합에 변환을 적용한 예시도이다.
도 6은 도로 기준선과 주행 중 추출한 도로선을 예시한 도면이다.
도 7은 본 발명의 일 실시예에 따른 최적화에 사용된 점유격자 지도를 예시한 도면이다.
도 8은 본 발명의 일 실시예에 따른 변환행렬 최적화 방법을 보여주는 흐름도이다.

본 발명은 다양한 변경을 가할 수 있고 여러 가지 실시예를 가질 수 있는 바, 특정 실시예들을 도면에 예시하고 상세하게 설명하고자 한다. 그러나, 이는 본 발명을 특정한 실시 형태에 대해 한정하려는 것이 아니며, 본 발명의 사상 및 기술 범위에 포함되는 모든 변경, 균등물 내지 대체물을 포함하는 것으로 이해되어야 한다.

본 출원에서 사용한 용어는 단지 특정한 실시예를 설명하기 위해 사용된 것으로, 본 발명을 한정하려는 의도가 아니다. 단수의 표현은 문맥상 명백하게 다르게 뜻하지 않는 한, 복수의 표현을 포함한다. 본 출원에서, "포함하다" 또는 "가지다" 등의 용어는 명세서상에 기재된 특징, 숫자, 단계, 동작, 구성요소, 부품 또는 이들을 조합한 것이 존재함을 지정하려는 것이지, 하나 또는 그 이상의 다른 특징들이나 숫자, 단계, 동작, 구성요소, 부품 또는 이들을 조합한 것들의 존재 또는 부가 가능성을 미리 배제하지 않는 것으로 이해되어야 한다.

다르게 정의되지 않는 한, 기술적이거나 과학적인 용어를 포함해서 여기서 사용되는 모든 용어들은 본 발명이 속하는 기술 분야에서 통상의 지식을 가진 자에 의해 일반적으로 이해되는 것과 동일한 의미를 갖고 있다. 일반적으로 사용되는 사전에 정의되어 있는 것과 같은 용어들은 관련 기술의 문맥 상 갖는 의미와 일치하는 의미를 갖는 것으로 해석되어야 하며, 본 출원에서 명백하게 정의하지 않는 한, 이상적이거나 과도하게 형식적인 의미로 해석되지 않는다.

또한, 첨부 도면을 참조하여 설명함에 있어, 도면 부호에 관계없이 동일한 구성 요소는 동일한 참조부호를 부여하고 이에 대한 중복되는 설명은 생략하기로 한다. 본 발명을 설명함에 있어서 관련된 공지 기술에 대한 구체적인 설명이 본 발명의 요지를 불필요하게 흐릴 수 있다고 판단되는 경우 그 상세한 설명을 생략한다.

본 발명은 라이다(LiDAR, Light Detection And Ranging) 센서를 이용하여 차량의 자세를 추정하는 방법에 대한 것이다.

본 발명에서 라이다 센서를 이용한 차량의 자세 추정 방법의 수행 주체는 차량에 내장된 전자 시스템이 될 수 있다. 가령, 차량에 내장된 전자 제어 시스템 모듈의 제어부(프로세서)나, 자율 주행 차량에 장착된 전자 제어 시스템 등이 수행 주체가 될 수 있다.

라이다 센서는 빛을 쏘아 거리를 직접 측정하는 센서로서, 정확한 거리를 빠른 속도로 얻을 수 있기 때문에 위치 인식, 장애물 감지, 이동 물체 추적을 위해 자율 주행 차량에 많이 사용된다. 2D 라이다 센서는 각도를 달리하여 빛을 쏘아 한 번의 스캔에 여러 개의 거리 데이터를 얻을 수 있다.

도 2는 2D 라이다 센서의 측정범위와 측정값의 예를 보여주는 도면이다.

도 2를 참조하면, 좌측 도면에서 호 모양 영역으로 표시된 측정 범위 안에 놓여있는 차량에 반사되어 측정된 값이, 우측 도면에서는 2차원 좌표상의 점들로 나타난다. 이처럼 2D 라이다 센서는 측정점으로 불리는 거리, 위상 데이터들이 하나의 평면상에 나타나는 특징이 있다.

일반적으로 자율 주행 차량은 주차 혹은 후방 장애물 감지를 위해 후방에 2D 라이다 센서를 장착하는 경우가 있기 때문에, 이를 차량의 자세 추정에 이용할 경우 추가 비용이 발생하지 않는다.

도 3은 본 발명의 일 실시예에 따른 라이다 센서를 이용한 차량의 자세 추정 방법을 도시한 전체 흐름도이다.

도 3을 참조하면, 본 발명의 일 실시예에 따른 차량의 자세 추정 방법은 다음과 같다.

먼저, 라이다 센서로부터 거리를 측정한 측정점 데이터를 얻는다(S310).

다음, 측정점 데이터를 2차원 직교 좌표 상에 나타내고, 2차원 직교 좌표 상에서 도로선을 찾는다(S320).

그리고, 도로선으로부터 α, β의 자세 변화량(α', β')을 계산한다(S330).

그리고, 관성측정장치의 측정치와 융합하여 최종적인 자세 값인 α, β를 계산한다(S340).

본 발명에서는 라이다 센서의 측정 주기마다 S310~S340 의 모든 단계가 한 번씩 실행된다. 이하에서는 각 단계별로 상세히 설명한다.

S310 단계에서는 2D 라이다 센서에서 측정된 가공되지 않은 데이터인, 거리와 위상으로 구성된 점 집합을 얻는다. 이 점들을 각각 측정점이라고 한다.

도 4는 도로를 바라본 라이다 센서의 측정값들을 직교 좌표계에 표현한 도면이다.

도 4에서 각 점들은 다음 수학식과 같이 나타낼 수 있다.

여기서, r은 거리를 나타내고, θ는 각도를 나타내며, n개의 측정점을 얻을 수 있다. 측정치의 개수 n은 라이다의 성능에 의존한다. 본 발명에서 연산의 복잡도를 줄이고 정확도를 높이기 위해, 지나치게 크거나 작은 거리값을 가진 측정점은 미리 점 집합에서 제외시킨다.

S320 단계는 측정점 데이터를 직교 좌표계로 투영하여 도로선을 찾는 과정이다. 여기서 도로선은 측정점들 중에서 라이다 센서에서 쏜 빛이 도로에 반사된 측정치, 즉 도로와의 거리, 위상을 나타내는 측정점으로 이루어진 직선이다.

도 4에서, 도로를 바라본 라이다 센서가 얻은 측정점들이 예시되어 있다. 도 4에서, 도로를 나타내는 측정점들은 직선으로 근사시킬 수 있다.

이러한 도로선을 찾기 위해서 빠르고 정확한 직선 추출 방법이 필요하다. 기존의 직선 추출 방법 중 빠르고 정확한 방법들이 여러 가지 있으나 실시간으로 변화하는 도로 환경에서 적용할 때 강인함이 떨어진다는 문제점을 가지고 있다. 이를 해결하기 위해서, 본 발명에서는 미리 정의한 도로 기준선에 매 측정 마다 매칭하여 도로선을 찾는 방법을 제안한다.

먼저 극 좌표계에서 직교 좌표계로 변환하면 다음과 같다.

본 발명에서는 도로에 반사되어 측정된 데이터만을 필요로 하므로, 라이다 센서의 시야각이 넓은 경우 임의로 범위를 축소시킨다. 그리고 점집합 P 에 2차원 변환행렬을 곱한다.

도 5는 기존의 점 집합에 변환을 적용한 예시도이다. 여기서 변환행렬은 다음과 같다.

여기서, t_x는 x축 이동을 나타내고, t_y는 y축 이동을 나타내고, φ는 회전 이동 각도를 의미한다. 이 변환행렬을 찾으면 도로선을 단순한 계산으로 용이하게 찾아낼 수 있다.

변환행렬을 찾으려면 비교할 기준인 도로 기준선이 필요하다. 도로 기준선은 평지에서 획득한 라이다 측정치 중 도로인 부분을 의미한다.

도 6은 도로 기준선과 주행 중 추출한 도로선을 예시한 도면이다.

도 6에서 점선이 도로 기준선이며, 이는 라이다 센서의 장착 높이와 하향 각도에 따라 달라진다.

변환행렬을 찾는 방법은 많지만 도로를 주행하는 상황과 같이 변칙적인 상황에서 사용할 수 있는 방법은 제한되어 있다. 왜냐하면, 점 집합의 개수가 매번 같다는 보장이 없고, 단순한 행렬연산으로 변환행렬을 갱신할 수 없기 때문이다. 따라서, 주로 직선과 같은 특징을 뽑아 매칭하거나, 보완된 점 대 점 매칭 방법 등을 사용해왔으나, 전자는 수행 속도가 빠르지만 차량이 회전할 때 강인함이 매우 떨어지는 문제가 있고, 후자는 정확도가 높지만 실시간으로 동작하기 어렵다는 문제가 있다. 그러므로, 본 발명에서는 점유격자 지도 내에서 함수 최적화를 하는 방법을 사용하기로 한다.

이를 간략히 설명하면, 변환행렬을 찾기 위해, 점 집합 P 를 미리 정의한 점유격자 지도에 매칭하여 오차가 가장 작은 변환행렬을 찾아가는 방법이다. 여기서, 점유격자 지도는 지도를 표현하는 방법 중 하나로서, 크기가 고정된 각 격자마다 점유율을 가지고 있는 지도이다. 본 발명에서는 미리 도로 기준선을 그린 점유격자 지도를 사용한다.

도 7은 본 발명의 일 실시예에 따른 최적화에 사용된 점유격자 지도를 예시한 도면이다.

도 7을 참조하면, 점유격자 지도는 가우시안 분포를 따르는 직선의 형태로 작성하며, y축 기울기가 0 이 되어 수치적으로 발산하는 것을 방지하기 위해 한 줄씩 비운다. 여기서, 도로 기준선과 지도의 원점 간 거리는 라이다 센서의 높이와 하향 각도를 통해 다음과 같이 계산된다.

여기서, ρ는 점유격자 지도의 원점에서 도로 기준선까지의 거리이고, h는 라이다 센서의 높이이고, ω는 센서의 하향 각도이다.

본 발명에서 점유격자 지도의 각 셀 당 크기, 즉 해상도는 라이다 센서의 성능에 의존한다. 즉, 시야각 대비 점의 개수가 많을수록 점유격자 지도의 해상도를 높일 수 있다. 본 발명에서 점유격자 지도의 해상도가 높아야 변환행렬의 정밀도가 높아진다.

본 발명에서 최적화된 변환행렬로부터 도로선의 기울기와 절편을 도출하는 방식으로 도로선을 찾을 수 있다.

그리고, 변환행렬로부터 차량의 자세 변화량(α', β')을 계산할 수 있다.

이제 본 발명에서의 매칭 과정을 상세히 설명하면 다음과 같다.

본 발명에서는 최적의 변환 T^*를 찾기 위하여 비선형 함수 최적화 기법 중 하나인 Levenberg-Marquardt 알고리즘을 적용한다. 이 알고리즘은 초기치가 일정 범위 내에 있으면, 전역 해를 찾을 확률이 높고 수렴속도가 빠르다는 장점이 있다.

도 8은 본 발명의 일 실시예에 따른 변환행렬 최적화 방법을 보여주는 흐름도이다.

도 8을 참조하면, 본 발명의 일 실시예에 따른 변환행렬을 최적화하는 단계는, 먼저 선형 보간된 점유율과 점유율 기울기 값을 계산한다(S810).

그리고, 선형 보간된 점유율과 기울기 값을 통해 변환행렬을 업데이트한다(S820).

여기서, 업데이트된 변환행렬의 에러가 미리 정해진 제1 임계치 이상이고(S830), 업데이트 반복 횟수가 미리 정해진 제2 임계치 이하이면(S840), 다시 선형 보간된 점유율과 점유율 기울기 값을 계산하여 변환행렬을 업데이트한다(S810, S820).

그리고, 업데이트된 변환행렬의 에러가 상기 제1 임계치 미만이거나(S830), 또는 업데이트 반복 횟수가 미리 정해진 제2 임계치 이상이면(S840), 최적화 과정을 종료한다.

이러한 최적화 과정을 보다 상세히 설명하면 다음과 같다.

먼저 다음 수학식으로 충분히 수렴할 때까지 변환행렬 T^*를 업데이트한다.

여기서, p_{k+ 1}는 최적화하고자 하는 변환행렬 T^*의 변수 t_x, t_y, φ이고, p_k는 이전 변환행렬 T이다. 그리고, r(p_k)는 현재 변환행렬을 적용하였을 때의 오차이고, J_r은 오차 함수의 1차 미분 행렬이고, H_r은 오차 함수의 2차 미분 행렬이다.

H_r은 다음 수학식과 같이 Jr로 구할 수 있다.

다음, 이전의 변환행렬 T를 대입하여, 다음 수학식과 같이 선형 보간된 점유율과 점유율 기울기 값을 구한다.

여기서, v는 점유율이고,

는 x방향의 기울기이고,

는 y방향의 기울기를 의미한다.

그리고, 해당 점에서의 점유율, 1차 미분을 이용하여 Jr 행렬을 계산하고, 변환행렬을 갱신한다. J_r은 다음 수학식과 같이 계산된다.

여기서, x, y는 점 집합 내 각 점의 좌표이고, φ는 도로선의 회전 각도이다.

그리고, 계산된 J_r로 H_r를 구하고, 갱신된 변환행렬 T^*를 계산한다.

본 발명에서 오차가 줄어들면 완충계수 μ_k를 줄이고, 그렇지 않으면 μ_k를 증가시켜 다음 반복에서 사용한다. 그리고, 오차가 임계보다 작거나 설정된 반복 횟수에 도달하면, 최적화 과정을 종료하고 갱신된 변환행렬 T^*를 저장한다.

S330 단계는 변환행렬로부터 차량의 α와 β 변화량을 구하는 단계이다.

도 6을 참조하면, 청색 실선으로 표시된 변환행렬로부터 얻은 직선에 3차원 x, y축 회전변환을 적용하면, 녹색 점선으로 표시된 평지일 때의 직선을 얻을 수 있다. 그리고, 다음 수학식으로 차량의 α와 β 를 계산한다.

여기서, p는 평지일 때 라이다 센서의 부착 위치와 하향 각도를 고려한 도로 기준선 위의 점이고, p'은 계산한 직선 위의 점이고, R_x는 x축 4×4 회전행렬이고, R_y는 y축 4×4 회전행렬이다. 이 계산은 α와 β에 관한 2차 연립 방정식으로 쉽게 풀리며, 평지의 도로 기준선과 비교하였으므로, 실제로 변화량을 구한 것이라고 할 수 있다.

마지막 S340 단계는 S330 단계까지 수행하여 획득한 α, β 변화량과 차량 내부의 관성측정센서에서 추정한 α, β와 혼합하여 보상하는 단계이다. 여기서 차량의 속도, 라이다 센서의 측정 주기, 라이다 센서로 획득한 자세 변화량으로 차량의 자세를 추정한다. 그리고 칼만 필터를 사용하여 관성측정센서에서 추정한 자세와 융합한다.

이상 본 발명을 몇 가지 바람직한 실시예를 사용하여 설명하였으나, 이들 실시예는 예시적인 것이며 한정적인 것이 아니다. 본 발명이 속하는 기술분야에서 통상의 지식을 지닌 자라면 본 발명의 사상과 첨부된 특허청구범위에 제시된 권리범위에서 벗어나지 않으면서 다양한 변화와 수정을 가할 수 있음을 이해할 것이다.

Claims (4)

1. 관성측정장치(Inertial Measurement Unit)가 내장되어 있고, 라이다(LiDAR, Light Detection And Ranging) 센서가 후방에 장착된 차량의 자세 추정 방법에서,
   상기 라이다 센서를 이용하여 거리를 측정한 측정점 데이터를 획득하는 단계;
   상기 측정점 데이터를 2차원 직교 좌표 상에 투영하여 도로선을 찾는 단계;
   상기 도로선으로부터 차량의 자세 변화량(α', β')을 계산하는 단계; 및
   상기 자세 변화량(α', β')을 상기 관성측정장치의 측정치와 융합하여 차량의 자세(α, β)를 계산하는 단계를 포함하며,
   상기 도로선을 찾는 단계는,
   극 좌표계로 표현된 상기 측정점 데이터의 집합인 점 집합 P를 직교 좌표계로 변환하는 단계;
   크기가 고정된 각 격자마다 점유율을 갖고 있는 지도인 점유격자 지도를 가우시안 분포를 따르는 직선의 형태로 작성하는 단계;
   상기 점 집합 P를 상기 점유격자 지도에 매칭하여 변환행렬을 구하는 단계;
   상기 변환행렬을 계속 업데이트하여 최적화하는 단계; 및
   상기 최적화된 변환행렬로부터 도로선의 기울기와 절편을 도출하는 단계를 포함하여 이루어지고,
   상기 변환행렬로부터 차량의 자세 변화량(α', β')을 계산하며,
   상기 변환행렬을 최적화하는 단계는,
   선형 보간된 점유율과 점유율 기울기 값을 계산하는 단계;
   상기 선형 보간된 점유율과 기울기 값을 통해 변환행렬을 업데이트하는 단계;
   업데이트된 변환행렬의 에러가 미리 정해진 제1 임계치 이상이고, 업데이트 반복 횟수가 미리 정해진 제2 임계치 이하이면, 다시 선형 보간된 점유율과 점유율 기울기 값을 계산하여 변환행렬을 업데이트하는 단계; 및
   업데이트된 변환행렬의 에러가 상기 제1 임계치 미만이거나, 또는 업데이트 반복 횟수가 미리 정해진 제2 임계치 이상이면, 최적화 과정을 종료하는 단계로 이루어지고,
   t_x는 x축 이동을 나타내고, t_y는 y축 이동을 나타내고, φ는 회전 이동 각도를 의미할 때, 상기 변환행렬을,
   

   

   (수학식 3)으로 나타낼 수 있고,
   ρ는 점유격자 지도의 원점에서 도로 기준선까지의 거리이고, h는 라이다 센서의 높이이고, ω는 센서의 하향 각도라고 할 때,
   

   

   (수학식 4)로 나타낼 수 있고,
   p_k+1는 최적화하고자 하는 변환행렬 T^*의 변수 t_x, t_y, φ이고, p_k는 이전 변환행렬 T이고, r(p_k)는 현재 변환행렬을 적용하였을 때의 오차이고, J_r은 오차 함수의 1차 미분 행렬이고, H_r은 오차 함수의 2차 미분 행렬이라고 할 때,
   

   

   (수학식 5)를 통해 변환행렬 T^*를 업데이트하고,
   여기서, H_r은,
   

   

   (수학식 6)으로 구할 수 있고,
   v는 점유율이고,

   

   는 x방향의 기울기이고,

   

   는 y방향의 기울기를 의미할 때, 이전의 변환행렬 T를 대입하여,
   

   

   (수학식 7)
   과 같이 선형 보간된 점유율과 점유율 기울기 값을 구하며,
   해당 점에서의 점유율, 1차 미분을 이용하여 J_r 행렬을 계산하고, 변환행렬을 갱신하되, J_r은,
   

   

   (수학식 8)과 같이 계산되며,
   여기서, x, y는 점 집합 내 각 점의 좌표이고, φ는 도로선의 회전 각도이고,
   계산된 J_r로 H_r를 구하고, 갱신된 변환행렬 T^*를 계산하며,
   상기 업데이트된 변환행렬의 에러가 줄어들면 완충계수 μ_k를 줄이고, 그렇지 않으면 μ_k를 증가시켜 다음 반복에서 사용하고, 에러가 임계보다 작거나 설정된 반복 횟수에 도달하면, 최적화 과정을 종료하고 갱신된 변환행렬 T^*를 저장하는 것을 특징으로 하는 차량의 자세 추정 방법.
2. 삭제
3. 삭제
4. 삭제

Images