KR101880013B1 - 자기 위치 추정 장치 및 자기 위치 추정 방법 - Google Patents

Abstract

본 발명의 자기 위치 추정 장치는, 이동체의 주위에 존재하는 물표의 물표 위치를 검출하여 이동체의 이동량을 검출하고, 검출한 물표 위치를 이동량만큼 이동시켜 물표 위치 데이터로서 축적한다. 그리고, 지도 상에 존재하는 물표의 물표 위치를 포함하는 지도 정보를 취득하고, 이동체의 현재 위치까지의 이동 이력에 기초하여 설정된 소정 범위의 물표 위치 데이터와 지도 정보에 포함되는 물표 위치를 대조하여 이동체의 자기 위치를 추정한다.

Description

자기 위치 추정 장치 및 자기 위치 추정 방법

본 발명은, 이동체의 자기 위치를 추정하는 자기 위치 추정 장치 및 그 방법에 관한 것이다.

이동체에 탑재된 카메라나 레이저 레인지 파인더를 사용하여 이동체의 주위를 센싱하고, 그 결과와 지도를 매칭하여, 지도 상의 위치나 자세각을 구하는 자기 위치 추정 기술이 개발되어 있고, 종래에는 특허문헌 1이 개시되어 있다.

특허문헌 1의 자기 위치 추정 기술에서는, 차륜속 펄스나 자이로 등에 의해 산출한 이동량과 센싱한 결과의 과거의 이력을 축적하여 연결하고, 이 결과와 지도가 일치하도록 조정하여 이동체의 위치와 자세각을 추정하고 있다. 또한, 특허문헌 1의 자기 위치 추정 기술에서는, 항상 일정 거리의 센싱 결과의 이력을 사용함으로써, 이동체의 이동 속도의 영향을 배제하고, 자기 위치의 추정을 안정적으로 행할 수 있도록 하고 있다.

일본 특허 공개 제2008-250906호 공보

그러나, 상술한 종래의 자기 위치 추정 기술에서는, 일정 거리의 센싱 결과의 이력을 사용하고 있었기 때문에, 이동량의 검출 오차가 커지는 상황에서는, 센싱 결과와 지도간의 괴리가 커져 자기 위치의 추정이 곤란해진다고 하는 문제점이 있었다.

그래서, 본 발명은, 상술한 실정에 비추어 제안된 것이며, 이동량의 검출 오차가 커지는 상황이라도 자기 위치를 고정밀도로, 또한 안정적으로 추정할 수 있는 자기 위치 추정 장치 및 그 방법을 제공하는 것을 목적으로 한다.

상술한 과제를 해결하기 위해, 본 발명의 일 양태에 관한 자기 위치 추정 장치 및 그 방법은, 이동체의 주위에 존재하는 물표의 물표 위치를 검출하여 이동체의 이동량을 검출한다. 그리고, 검출한 물표 위치를 이동량만큼 이동시켜 물표 위치 데이터로서 축적하고, 지도 상에 존재하는 물표의 물표 위치를 포함하는 지도 정보를 취득한다. 이 후, 이동체의 현재 위치까지의 이동 이력에 기초하여 설정된 소정 범위의 물표 위치 데이터와 지도 정보에 포함되는 물표 위치를 대조하여 이동체의 자기 위치를 추정한다.

도 1은 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치를 구비한 자기 위치 추정 시스템의 구성을 나타내는 블록도이다.
도 2는 레이저 레인지 파인더와 카메라의 차량에의 탑재 위치를 나타내는 도면이다.
도 3은 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치에 의한 자기 위치 추정 처리의 처리 순서를 나타내는 흐름도이다.
도 4는 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치에 의해 채용된 좌표계를 설명하기 위한 도면이다.
도 5는 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치의 레이저 레인지 파인더에 의한 검출 방법을 설명하기 위한 도면이다.
도 6은 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치의 카메라에 의한 백색선의 검출 방법을 설명하기 위한 도면이다.
도 7은 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치에 의한 물표의 검출 결과를 나타내는 도면이다.
도 8은 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치에 의한 이동량의 검출 오차의 추정 방법을 설명하기 위한 도면이다.
도 9는 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치에 의한 추출 범위의 설정 방법을 설명하기 위한 도면이다.
도 10은 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치에 의해 취득된 링크·노드 정보를 설명하기 위한 도면이다.
도 11은 본 발명의 제1 실시 형태의 변형예 5에 관한 자기 위치 추정 장치에 의한 추출 범위의 설정 방법을 설명하기 위한 도면이다.
도 12는 종래의 자기 위치 추정 기술을 설명하기 위한 도면이다.
도 13은 종래의 자기 위치 추정 기술을 설명하기 위한 도면이다.
도 14는 종래의 자기 위치 추정 기술을 설명하기 위한 도면이다.
도 15는 본 발명의 제1 실시 형태에 관한 자기 위치 추정 장치에 의한 추출 범위의 설정 방법을 설명하기 위한 도면이다.
도 16은 본 발명의 제2 실시 형태에 관한 자기 위치 추정 장치에 의한 자기 위치 추정 처리의 처리 순서를 나타내는 흐름도이다.

이하, 본 발명을 적용한 제1 및 제 2 실시 형태에 대해 도면을 참조하여 설명한다.

[제1 실시 형태]

[자기 위치 추정 시스템의 구성]

도 1은, 본 실시 형태에 관한 자기 위치 추정 장치를 구비한 자기 위치 추정 시스템의 구성을 나타내는 블록도이다. 도 1에 나타낸 바와 같이, 본 실시 형태에 관한 자기 위치 추정 시스템은, ECU(1)와, 카메라(2)와, 3차원 지도 데이터베이스(3)와, 차량 센서 군(4)과, 레이저 레인지 파인더(5)를 구비하고 있다.

여기서, ECU(1)는, ROM, RAM, 연산 회로 등에 의해 구성된 전자 제어 유닛이며, ROM에는 본 실시 형태에 관한 자기 위치 추정 장치(10)를 실현하는 프로그램이 저장되어 있다. 또한, ECU(1)는 다른 제어에 사용하는 ECU와 겸용해도 된다.

카메라(2(2a, 2b))는, CCD 등의 고체 촬상 소자를 사용한 것이며, 예를 들어 도 2에 나타낸 바와 같이 차량의 좌우의 도어 미러에 설치되고, 차량의 하방의 노면을 촬상 가능해지는 방향을 향해 있다. 촬상된 화상은 ECU(1)로 송신된다.

3차원 지도 데이터베이스(3)는, 지도 상에 존재하는 물표의 물표 위치를 포함하는 지도 정보를 기억한 기억 수단이며, 예를 들어 도로 표시를 포함하는 주위 환경의 3차원 위치 정보를 기억하고 있다. 지도 정보에 기록되어 있는 물표(landmark)는 지도에 등록되어 있는 것이며, 구획선, 정지선, 횡단보도, 노면 마크 등의 노면 표시 외에, 연석 등의 노면 상의 구조물이나 건물 등이 포함된다. 백색선 등의 각 지도 정보는, 에지의 집합체로서 정의되어 있다. 에지가 긴 직선인 경우에는, 예를 들어 1m마다 구획되기 때문에, 극단적으로 긴 에지는 존재하지 않는다. 직선인 경우에는, 각 에지는, 직선의 양 단부점을 나타내는 3차원 위치 정보를 갖고 있다. 곡선인 경우에는, 각 에지는, 곡선의 양단부점과 중앙점을 나타내는 3차원 위치 정보를 갖고 있다. 또한, 3차원 지도 데이터베이스(3)는 일반적인 내비게이션 시스템이 갖는 노드·링크 정보를 기억하고 있으므로, 목적지까지의 경로 안내나 과거의 주행 경로를 기록하는 처리를 ECU(1)에 의해 행할 수 있다.

차량 센서 군(4)은, GPS 수신기(41)와, 액셀러레이터 센서(42)와, 스티어링 센서(43)와, 브레이크 센서(44)와, 차속 센서(45)와, 가속도 센서(46)와, 차륜속 센서(47)와, 요레이트 센서(48)를 구비하고 있다. 차량 센서 군(4)은, ECU(1)에 접속되고, 각 센서(41 내지 48)에 의해 검출된 각종 검출값을 ECU(1)에 공급한다. ECU(1)는, 차량 센서 군(4)의 출력값을 사용함으로써, 차량의 개략 위치를 산출하거나, 단위 시간에 차량이 진행한 이동량을 나타내는 오도메트리를 산출한다.

레이저 레인지 파인더(5(5a, 5b))는, 도 2에 나타낸 바와 같이 차체의 좌우 방향을 스캔할 수 있도록 설치되어 있다. 레이저 레인지 파인더(5)는, 바로 아래 방향으로부터 각각 수평 방향까지의 90[deg]의 주사 범위에서 0.1[deg] 간격으로 900개×2＝1800개의 레이저를 진행 방향과 수직으로 조사한다. 이에 의해, 노면이나 장해물까지의 거리 εi[m](i＝1 내지 1800)와 반사파의 강도 δi(i＝1 내지 1800, 0≤δi≤255)를 검출할 수 있다.

자기 위치 추정 장치(10)는, 자기 위치 추정 처리를 실행하는 제어부로서 기능하고, 차량의 주위에 존재하는 물표의 물표 위치와 3차원 지도 데이터베이스(3)에 저장된 지도 정보를 매칭함으로써, 차량의 위치 및 자세각을 추정한다. 자기 위치 추정 장치(10)는, ROM에 저장된 자기 위치 추정용 프로그램을 실행함으로써, 물표 위치 검출부(12), 이동량 검출부(14), 물표 위치 축적부(16), 지도 정보 취득부(18) 및 자기 위치 추정부(20)로서 동작한다.

물표 위치 검출부(12)는, 레이저 레인지 파인더(5)의 스캔 결과로부터 차량의 주위에 존재하는 물표의 물표 위치를 검출한다. 또한, 물표 위치 검출부(12)는, 카메라(2)의 화상으로부터 물표 위치를 검출해도 되고, 카메라(2)와 레이저 레인지 파인더(5)의 양쪽을 사용하여 물표 위치를 검출해도 된다.

이동량 검출부(14)는, 차량에 탑재된 차량 센서 군(4)으로부터의 정보에 기초하여 차량의 이동량인 오도메트리를 검출한다.

물표 위치 축적부(16)는, 이전에 실행된 각 제어 주기에 있어서 물표 위치 검출부(12)에 의해 검출된 물표 위치를, 이동량 검출부(14)에 의해 검출된 이동량만큼 이동시켜 물표 위치 데이터로서 축적한다.

지도 정보 취득부(18)는, 3차원 지도 데이터베이스(3)로부터 지도 정보를 취득하고 있고, 취득하는 지도 정보에는, 지도 상에 존재하는 물표의 물표 위치가 포함되어 있다.

자기 위치 추정부(20)는, 본 실시 형태에서는, 차량의 주행 이력(이동체의 이동 이력에 상당)의 과거의 이동량 변화에 기초하여 이동량 검출부(14)에 의해 검출된 이동량의 검출 오차를 추정한다. 그리고, 자기 위치 추정부(20)는, 차량의 주행 이력의 과거의 이동량 변화로부터 추정한 이동량의 검출 오차에 기초하여, 물표 위치 축적부(16)로부터 물표 위치 데이터를 추출할 때의 소정 범위(후술하는 추출 범위 A)를 설정하고, 물표 위치 축적부(16)에 축적된 물표 위치 데이터 중 차량의 주행 이력에 기초하여 설정된 소정 범위(추출 범위 A)에 포함되는 물표 위치 데이터를 추출하고, 추출된 물표 위치 데이터와 지도 정보에 포함되는 물표 위치를 대조하여 차량의 자기 위치를 추정한다.

또한, 본 실시 형태에서는 차량에 적용한 경우에 대해 설명하지만, 항공기나 선박 등의 이동체에도 적용 가능하다. 항공기나 선박에 적용하는 경우에는, 주위 환경으로서 도로 표시에 관한 정보 대신에 지형이나 건조물과 매칭함으로써 이동체의 자기 위치를 추정할 수 있다.

[자기 위치 추정 처리의 순서]

다음으로, 본 실시 형태에 관한 자기 위치 추정 처리의 순서를 도 3의 흐름도를 참조하여 설명한다. 또한, 본 실시 형태에서는, 차량의 자기 위치를 추정할 때, 도 4에 나타낸 2개의 좌표계를 사용하고 있다. 즉, 지도 정보의 원점을 중심으로 한 절대 좌표계와, 차량의 후륜 차축 중심을 원점으로 한 상대 공간 좌표계이다. 절대 좌표계는, 지도 정보의 원점을 원점 O로 하고, 동서 방향을 X축, 남북 방향을 Y축, 연직 상방을 Z축으로 하고 있다. 이 절대 좌표계에 있어서, 차량이 향하고 있는 방위각(요각) θ[rad]는, 동쪽 방향(X축 방향)을 0으로 하였을 때의 반시계 방향 각도로 나타내어진다. 또한, 상대 공간 좌표계는, 차량의 후륜 차축 중심을 원점 o로 하고, 차량의 전후 방향을 x축, 차폭 방향을 y축, 연직 상방을 z축으로 한다.

또한, 본 실시 형태에서는, 추정할 차량의 자기 위치로서, 동서 방향(X축 방향)의 위치(X 좌표 [m]), 남북 방향(Y축 방향)의 위치(Y 좌표[m]), 자세각 정보로서 차량의 방위각 θ(요각[rad])의 합계 3 자유도의 위치와 자세각을 추정한다. 단, 3 자유도의 위치와 자세각뿐만 아니라, 6 자유도의 공간에 있어서의 위치와 자세각을 구하는 것도 가능하다. 또한, 이하에 설명하는 자기 위치 추정 처리는, 예를 들어 100msec 정도의 간격으로 연속적으로 행해진다.

도 3에 나타낸 바와 같이, 먼저 스텝 S10에 있어서, 물표 위치 검출부(12)는, 차량의 주위에 존재하는 물표의 물표 위치를 검출한다. 구체적으로, 물표 위치 검출부(12)는, 레이저 레인지 파인더(5)의 스캔 결과를 취득하고, 구획선이나 정지선 등의 노면 표시나 연석, 건물 등의 구조물의 물표 위치를 추출한다. 여기서 추출되는 물표 위치는, 상대 공간 좌표계의 차량에 대한 상대 평면 위치 좌표(xj(t), yj(t))[m]이다. 또한, xj, yj의 j는, 추출된 노면 표시나 연석의 수와 동등하다. 또한, t는 현재의 시각(주기)을 나타내고 있다.

노면 표시의 상대 평면 위치 좌표를 구하는 방법은, 조사된 레이저의 반사파의 강도 δi가 역치 δth 이상이 되면, 레이저가 노면 표시에 조사되고 있는 것이라고 판단하여, 레이저가 조사된 점의 자차에 대한 거리 εi와 각도로부터 상대 평면 위치 좌표를 구한다. 이와 같이 하여 구해지는 이유는, 노면 표시 부분에는 반사재가 많이 포함되어 있으므로, 노면의 아스팔트와 비교하여 보다 강하게 레이저를 반사하기 때문이다. 또한, 역치 δth는 미리 실험 등에 의해 구해 두고, ECU(1)에 기억시켜 두면 되고, 본 실시 형태에서는 역치 δth로 120을 설정한다.

또한, 연석이나 건물 등의 구조물의 물표 위치를 구하는 방법에 대해서는, 각 레이저가 조사된 장소까지의 거리 εi와 각도로부터 각 조사점의 상대 평면 위치 좌표(xj(t), yj(t))를 산출하여 구한다. 구체적으로는, 예를 들어 도 5의 (a)에 나타낸 상황에 있어서, 레이저 레인지 파인더(5)로부터 레이저를 조사하면, 레이저는 연석(51)을 포함하는 노면에 라인(53)으로 나타낸 바와 같이 조사된다. 이 조사된 레이저의 점군을 검출하면, 도 5의 (b)에 나타낸 바와 같이 크게 변화되는 개소(55)가 검출된다. 따라서, 검출된 점군에 Hough 변환 등의 선분 검출 방법을 사용하여, 점군의 기울기가 크게 변화되는 장소를 추출하고, 그 장소의 상대 평면 위치 좌표(xj(t), yj(t))를 산출한다. 노면 상의 구조물이 건물인 경우라도, 점군의 기울기가 크게 변화되는 장소를 추출하면, 마찬가지로 상대 평면 위치 좌표(xj(t), yj(t))를 구할 수 있다.

또한, 스텝 S10에 있어서, 물표 위치 검출부(12)는, 카메라(2)의 화상을 취득하고, 카메라(2)의 화상으로부터 구획선 등의 도로 표시의 상대 평면 위치 좌표(xj(t), yj(t))를 추출해도 된다. 카메라(2)의 화상으로부터 도로 표시의 상대 평면 위치 좌표를 추출하는 방법에 대해서는, 도 6을 참조하여 설명한다. 도 6의 (a)는, 차량의 좌측 하향을 촬상하고 있는 카메라(2a)의 화상(60a)과, 차량의 우측 하향을 촬상하고 있는 카메라(2b)의 화상(60b)을 나타내고 있다. 이들 화상(60a, 60b)에 있어서, 물표 위치 검출부(12)는, 영역(61a, 61b)에 대해 백색선의 검출을 행한다. 예를 들어, 영역(61a, 61b)에 대해 2치화 처리를 행하여 휘도값이 높은 범위(63)를 추출하고, 도 6의 (b)에 나타낸 바와 같이 휘도값이 높은 부분(65)을 검출한다. 그리고, 도 6의 (c)에 나타낸 바와 같이, 휘도값이 높은 부분(65)의 무게 중심 위치(67)를 산출하고, 카메라(2)의 내부 파라미터(카메라 모델)와 외부 파라미터(카메라(2)의 차량 설치 위치)로부터, 무게 중심 위치(67)의 상대 평면 위치 좌표(xj(t), yj(t))를 구한다.

또한, 차량의 엔진이 시동한 직후, 혹은 차량이 전기 자동차인 경우에는 구동용 전원이 ON으로 된 직후는, 레이저 레인지 파인더(5)의 스캔 결과가 축적되어 있지 않다. 그 때문에, 본 실시 형태에서는 차량의 엔진이 OFF, 혹은 차량이 전기 자동차인 경우에는 구동용 전원이 OFF로 되었을 때, 현재 위치로부터 후술하는 추출 범위 A[m]의 범위 내의 스캔 결과를 추출해 두고, ECU(1)의 메모리 또는 기억 매체에 기록해 둔다. 그리고, 차량의 엔진이 시동, 혹은 차량이 전기 자동차인 경우에는 구동용 전원이 ON으로 된 순간에, 기록되어 있는 스캔 결과를 판독하도록 한다.

또한, 스텝 S10에서 추출된 구획선이나 연석, 건물 등의 상대 평면 위치 좌표(xj(t), yj(t))는, 후술하는 처리를 실행함으로써, 현재의 시각 t에 있어서의 상대 평면 위치 좌표로서, 차량의 엔진이 OFF로 될 때까지 계속 기록된다. 또한, 차량이 전기 자동차인 경우에는, 구동용 전원이 OFF로 될 때까지 계속 기록된다.

다음으로, 스텝 S20에 있어서, 이동량 검출부(14)는, 차량 센서 군(4)으로부터 얻어지는 센서 정보에 기초하여, 1주기 전부터 현재의 시각 t까지의 차량의 이동량인 오도메트리를 검출한다. 이 오도메트리는, 차량이 단위 시간에 진행한 이동량이다. 예를 들어, 본 실시 형태에서는, 요각의 이동량에 대해, 요레이트 센서(48)로부터 취득한 요레이트 γ[rad/s]에 ECU(1)의 연산 주기 100msec를 곱하여 요각의 변화량 Δθ(t)[rad]를 구한다. 또한, 병진 방향의 이동량은 차속 센서(45)로부터 취득한 차속 V[m/s]에 ECU(1)의 연산 주기 100msec를 곱하여 병진 방향의 이동량 ΔL(t)[m]을 구한다. 또한, 오도메트리를 산출하는 데 있어서, 차량의 각 차륜의 타이어 파라미터를 계측하고, 2륜 모델 등을 사용함으로써, 차체의 사이드 슬립각이나 차체의 사이드 슬립량을 추정하여 산출해도 된다.

스텝 S30에 있어서, 물표 위치 축적부(16)는, 물표 위치 검출부(12)에 의해 검출된 물표 위치를, 이동량 검출부(14)에 의해 검출된 이동량만큼 이동시켜 물표 위치 데이터로서 축적한다. 구체적으로 설명하면, 이전의 각 주기에 있어서 스텝 S10에서 취득한 구획선이나 연석 등의 물표의 상대 평면 위치 좌표(xj(t), yj(t))를, 스텝 S20에서 취득한 오도메트리만큼 이동시킨다. 바꾸어 말하면, 레이저 레인지 파인더(5), 혹은 카메라(2) 중 어느 한쪽 또는 양쪽에 의해 과거에 취득한 구획선이나 연석 등의 물표 위치를, 오도메트리 정보를 사용하여, 현재의 차량의 후륜 차축 중심을 원점으로 한 상대 공간 좌표계로 변환한다.

이에 의해, 예를 들어 도 7의 (b)에 나타낸 바와 같이 레이저 레인지 파인더(5)의 스캔 결과로부터 물표의 위치를 특정할 수 있다. 도 7의 (b)는, 도 7의 (a)에 나타낸 상공으로부터 본 주로를, 차량이 화살표(71)의 경로로 주행한 경우에 축적된 물표 위치 데이터를 나타낸 것이다. 즉, 도 7의 (b)는, 스텝 S10에 있어서 레이저 레인지 파인더(5)에 의해 검출된 물표의 위치 정보를, 스텝 S20에서 검출된 이동량만큼 이동시켜 축적한 결과를 표시한 일례이다. 도 7의 (b)에는, 노면의 반사 강도가 높은 점, 예를 들어 구획선이나 정지선, 횡단보도의 위치와 함께, 레이저의 검출 결과가 크게 변화되는 연석의 위치가 표시되어 있다.

스텝 S40에 있어서, 자기 위치 추정부(20)는, 차량의 주행 이력의 과거의 이동량 변화에 기초하여 차량의 이동량 검출 오차를 추정하고, 추정한 이동량의 검출 오차에 기초하여, 물표 위치 축적부(16)로부터 물표 위치 데이터를 추출할 때의 소정 범위(추출 범위 A)를 설정한다. 특히, 본 실시 형태에서는, 차량의 주행 이력의 과거의 이동량 변화로부터 이동량의 검출 오차를 추정하고 있고, 주행 이력의 과거의 이동량 변화의 구체예로서, 차량의 현재 위치와 소정 시간 전의 위치, 또는 소정 거리 거슬러 올라간 위치에 있어서의 과거의 이동량 변화로부터 이동량의 검출 오차를 추정한다.

자기 위치 추정부(20)는, 도 8에 나타낸 바와 같이, 주행 이력으로부터 1주기 전의 스텝 S50에서 산출된 차량의 자기 위치[80(X(t), Y(t), θ(t))]와, 현재보다 시간 T[s]만큼 전의 차량의 자기 위치[82(X(t-T), Y(t-T), θ(t-T))]를 비교하고, 이동량을 취득한다. 단, 여기서 말하는 자기 위치라 함은, 절대 좌표계의 동서 방향의 위치(X 좌표[m]), 남북 방향의 위치(Y 좌표[m]), 자세각 정보로서 동쪽 방향을 0으로 하였을 때의 반시계 방향의 방위각 θ(요각[rad])이다.

그리고, 도 8에서는, 주행 이력으로부터 시간 T[s]만큼 전의 차량의 자기 위치(82)가 현재의 자기 위치(80)의 차량 방향으로부터 보아 차폭 방향으로 절댓값 Δy[m]만큼 어긋난 과거의 이동량 변화가 있다. 이 차폭 방향의 과거의 이동량 변화인 어긋남양의 절댓값 Δy[m]가 역치 yth[m] 이상인 경우에는, 자기 위치 추정부(20)는 이동량의 검출 오차가 크다고 추정할 수 있다. 한편, 차폭 방향의 과거의 이동량 변화인 어긋남양의 절댓값 Δy[m]가 역치 yth[m]보다 작은 경우에는, 자기 위치 추정부(20)는 이동량의 검출 오차가 작다고 추정할 수 있다.

도 8에 나타낸 바와 같이, 차량이 차폭 방향으로 이동하였다고 하는 것은, 오도메트리의 오차가 축적되기 쉬운 이동량 변화가 큰 선회나 차선 변경, 좌우회전, 곡선로의 주행을 행하였다고 추정할 수 있으므로, 이동량의 검출 오차가 크다고 추정할 수 있다.

그리고, 자기 위치 추정부(20)는, 주행 이력의 과거의 이동량 변화로부터 이동량의 검출 오차가 작다고 추정되는 경우에는, 후술하는 스텝 S50에서 물표 위치 데이터를 추출할 때의 추출 범위 A[m]를 크게 하여, 예를 들어 200m로 설정한다. 한편, 자기 위치 추정부(20)는, 주행 이력의 과거의 이동량 변화로부터 이동량의 검출 오차가 크다고 추정되는 경우에는, 추출 범위 A[m]를 작게 하여, 예를 들어 100m로 설정한다. 또한, 도 9에 나타낸 바와 같이, 주행 이력으로부터, 과거의 이동량 변화인 어긋남양 Δy가 커짐에 따라서, 추출 범위 A를 연속적으로 작아지도록 변화시켜도 된다. 즉, 주행 이력의 과거의 이동량 변화로부터 추정되는 이동량의 검출 오차가 커짐에 따라서, 추출 범위 A가 작아지도록 설정한다. 이와 같이 주행 이력의 과거의 이동량 변화로부터 이동량의 검출 오차가 크다고 추정되는 경우에는, 추출 범위 A를 작게 함으로써, 오도메트리의 오차의 축적이 적어지도록 한다.

또한, 역치 yth는, 예를 들어 50m로 설정하면 된다. 또한, 추출 범위 A의 감소폭은, 스텝 S50의 매칭 상태를 미리 실험이나 시뮬레이션에 의해 검증하여, 최적의 값을 설정하는 것이며, 200m로부터 100m로 감소시키는 것 이외에 다른 값으로 설정해도 된다.

또한, 주행 이력으로서, 차폭 방향의 이동량뿐만 아니라 차량이 통과한 교차점의 수를 사용하여, 과거의 이동량 변화를 추정하고, 이동량의 검출 오차의 크기를 추정해도 된다. 이 경우, 먼저 추출 범위 A를 200m로 설정해 두고, 3차원 지도 데이터베이스(3)에 등록된 링크·노드 정보로부터, 주행 이력으로서 추출 범위 A 내에서 교차점을 통과한 횟수를 카운트한다.

그리고, 주행 이력으로서 교차점을 통과한 횟수가 3회 미만인 경우에는, 과거의 이동량 변화가 작기 때문에, 이동량의 검출 오차는 작다고 추정하여 추출 범위 A를 200m 그대로 설정한다. 한편, 주행 이력으로서 교차점을 통과한 횟수가 3회 이상인 경우에는, 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 커질 것이라고 추정하여, 추출 범위 A를 100m로 설정한다. 또한, 이 밖에도, 추출 범위 A 내의 지도 상의 최대 구배를 탐색하여, 예를 들어 주행 이력으로서 최대 구배가 4％ 이상인 구간이 포함되어 있는 경우에는, 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 커질 것이라고 추정하여, 추출 범위 A를 100m로 설정해도 된다.

이와 같이, 주행 이력으로서는, 차량의 현재 위치와 소정 시간 전의 위치 외에도, 차량의 주행 경로상의 지도 정보를 참조하여, 과거의 이동량 변화를 추정하고, 이동량의 검출 오차를 추정해도 된다. 이에 의해, 좌우회전이나 정지, 발진을 수반하는 교차점이나 급구배의 구간 등의 이동량 변화가 커, 오도메트리의 오차가 축적되기 쉬운 포인트가 주행 경로에 포함되는 경우에는, 추출 범위 A를 작게 하여 오도메트리의 오차의 축적이 적어지도록 할 수 있다.

스텝 S50에 있어서, 자기 위치 추정부(20)는, 물표 위치 축적부(16)에 축적된 물표 위치 데이터 중 스텝 S40에서 설정된 추출 범위 A에 포함되는 물표 위치 데이터를 추출한다. 그리고, 추출된 물표 위치 데이터와 지도 정보에 포함되는 물표 위치를 대조하여 차량의 자기 위치를 추정한다.

구체적으로, 자기 위치 추정부(20)는, 스텝 S30에서 축적된 구획선이나 연석 등의 물표 위치 데이터 중에서 스텝 S40에서 설정된 추출 범위 A의 범위 내에 있는 물표 위치 데이터를 추출한다. 이때, 물표 위치 데이터는, 스텝 S20에서 산출된 이동량 ΔL(t)을 현재의 시각 t로부터 거슬러 올라가 적산하고, 이 적산값이 추출 범위 A를 초과할 때까지 추출된다.

그리고, 추출한 물표 위치 데이터와 3차원 지도 데이터베이스(3)에 저장되어 있는 지도 정보의 물표 위치를 매칭하여, 차량의 절대 좌표계에 있어서의 자기 위치를 추정한다. 즉, 차량의 동서 방향의 위치(X 좌표)와, 남북 방향의 위치(Y 좌표)와, 방위각(요각 θ)으로 이루어지는 합계 3 자유도의 위치와 자세각을 추정한다. 이와 같이 하여 자기 위치가 추정되면, 본 실시 형태에 관한 자기 위치 추정 처리는 종료된다.

또한, 스텝 S50에 있어서의 매칭에는, ICP(Iterative Closest Point) 알고리즘을 사용한다. 이때, 3차원 지도 데이터베이스(3)의 지도 정보에 포함되는 물표 위치 중, 예를 들어 구획선은, 그 양단부의 단부점을 평가점으로서 매칭한다. 또한, 차량(카메라(2))에 가까울수록 물표 위치 데이터는 오도메트리의 오차의 영향을 받지 않으므로, 차량의 근방에 대해서는 직선 보완하여 평가점의 수를 증가시키고, 차량의 먼 쪽에 대해서는 평가점의 수를 감소시킨다.

[변형예 1]

본 실시 형태의 변형예 1로서는, 스텝 S40에 있어서 차량의 주행 이력으로부터 이동량의 검출 오차를 추정할 때, 주행 이력의 구체예로서, 차량의 좌우회전 횟수가 많을수록 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 좁힌다. 이 경우, 먼저 추출 범위 A를 200m로 설정해 두고, 3차원 지도 데이터베이스(3)에 등록된 링크·노드 정보로부터, 추출 범위 A의 범위 내에서 차량이 교차점에서 좌우회전한 횟수를 카운트한다.

또한, 링크·노드 정보는, 도 10에 나타낸 바와 같이, 분기된 링크(화살표)와 노드(동그라미 표시)가 기록되어 있고, 각 링크에는 직진, 우회전, 좌회전, 합류, 분기를 판단할 수 있는 속성 정보가 부여되어 있다. 따라서, 차량이 어느 링크를 통과하였는지를 검출하여, 이 링크·노드 정보를 참조함으로써, 차량의 좌우회전 정보를 얻을 수 있다.

그리고, 주행 이력으로서, 교차점에서 좌우회전한 횟수가 2회 미만인 경우에는, 과거의 이동량 변화가 작기 때문에, 이동량의 검출 오차는 작다고 추정하여 추출 범위 A를 크게, 예를 들어 200m 그대로 설정한다. 한편, 주행 이력으로서, 교차점에서 좌우회전한 횟수가 2회 이상인 경우에는, 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 작게, 예를 들어 100m로 설정한다. 이때, 좌우회전의 횟수에 따라서 추출 범위 A를 연속적으로 변화시켜도 된다.

이와 같이, 오도메트리의 오차가 축적되기 쉬운 좌우회전을 행한 경우에는, 추출 범위 A를 작게 하여 오도메트리의 오차의 축적이 적어지도록 한다.

[변형예 2]

변형예 2로서는, 스텝 S40의 주행 이력의 구체예로서, 차량의 차선 변경의 횟수가 많을수록 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 좁힌다. 이 경우, 먼저 추출 범위 A를 200m로 설정해 두고, 3차원 지도 데이터베이스(3)에 등록된 링크·노드 정보로부터, 추출 범위 A의 범위 내에서 차량이 차선 변경한 횟수를 카운트한다. 도 10에 나타낸 바와 같이, 링크·노드 정보에는, 복수의 차선이 있는 도로에 대해 차선마다 개별로 링크 정보가 설정되어 있으므로, 링크·노드 정보를 참조하면, 차선 변경의 횟수를 카운트할 수 있다.

그리고, 주행 이력으로서, 차선 변경의 횟수가 0회인 경우에는, 과거의 이동량 변화가 작기 때문에, 이동량의 검출 오차는 작다고 추정하여 추출 범위 A를 크게, 예를 들어 200m 그대로 설정한다. 한편, 주행 이력으로서, 차선 변경의 횟수가 1회 이상인 경우에는, 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 작게, 예를 들어 100m로 설정한다.

이와 같이, 오도메트리의 오차가 축적되기 쉬운 차선 변경을 한 경우에는, 추출 범위 A를 작게 하여 오도메트리의 오차의 축적이 적어지도록 한다.

[변형예 3]

변형예 3으로서는, 스텝 S40의 주행 이력의 구체예로서, 차량의 분류 또는 합류의 횟수가 많을수록 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 작게 한다. 이 경우, 먼저 추출 범위 A를 200m로 설정해 두고, 3차원 지도 데이터베이스(3)에 등록된 링크·노드 정보로부터, 추출 범위 A의 범위 내에서 차량이 분류 또는 합류한 횟수를 카운트한다. 분류 또는 합류의 판단에 대해서는, 도 10에 나타낸 바와 같이, 링크·노드 정보의 각 링크에 부여된 속성 정보를 참조하면 된다. 도 10에서는 합류, 분기의 링크는 도시되어 있지 않지만, 각 링크에는 직진, 우회전, 좌회전, 합류, 분기를 판단할 수 있는 속성 정보가 부여되어 있으므로, 이 속성 정보를 참조하면, 차량이 분류 또는 합류하였는지를 판단할 수 있다.

그리고, 주행 이력으로서, 분류 또는 합류의 횟수가 0회인 경우에는, 과거의 이동량 변화가 작기 때문에, 이동량의 검출 오차는 작다고 추정하여 추출 범위 A를 크게, 예를 들어 200m 그대로 설정한다. 한편, 주행 이력으로서, 분류 또는 합류의 횟수가 1회 이상인 경우에는, 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 작게, 예를 들어 180m로 설정한다.

이와 같이, 오도메트리의 오차가 축적되기 쉬운 분류 또는 합류를 한 경우에는, 추출 범위 A를 작게 하여 오도메트리의 오차의 축적이 적어지도록 한다.

[변형예 4]

변형예 4로서는, 스텝 S40의 주행 이력의 구체예로서, 차량이 주행한 커브의 곡률 반경이 작을수록 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 좁힌다. 이 경우, 먼저 추출 범위 A를 200m로 설정해 두고, 3차원 지도 데이터베이스(3)에 등록된 링크·노드 정보로부터, 추출 범위 A의 범위 내에서 차량이 주행한 커브의 곡률 반경을 검출한다. 곡률 반경은, 링크·노드 정보의 각 링크에 기록되어 있으므로, 차량이 주행한 링크를 특정하면, 곡률 반경을 검출할 수 있다.

그리고, 주행 이력으로서, 차량이 주행한 커브의 곡률 반경이 50m보다 큰 경우에는, 과거의 이동량 변화가 작기 때문에, 이동량의 검출 오차는 작다고 추정하여 추출 범위 A를 크게, 예를 들어 200m 그대로 설정한다. 한편, 주행 이력으로서, 차량이 주행한 커브의 곡률 반경이 50m 이하인 경우에는, 과거의 이동량 변화가 크기 때문에, 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 작게, 예를 들어 100m로 설정한다. 이때, 곡률 반경에 따라서 추출 범위 A를 연속적으로 변화시켜도 된다.

이와 같이, 오도메트리의 오차가 축적되기 쉬운 곡률 반경이 작은 커브를 차량이 주행한 경우에는, 추출 범위 A를 작게 하여 오도메트리의 오차의 축적이 적어지도록 한다.

[변형예 5]

변형예 5로서, 상술한 실시 형태와 변형예 1 내지 4의 복수의 주행 이력의 형태를 사용하여 과거의 이동량 변화를 추정하여, 이동량의 검출 오차를 추정하고, 추출 범위 A를 설정해도 된다. 이 경우, 도 11에 나타낸 바와 같이, 주행 이력의 과거의 이동량 변화인 어긋남양 Δy나 좌우회전 횟수 등의 과거의 이동량 변화가 크다고 상정되는 오차 요인마다 각각 추출 범위 A의 감소량 ΔA를 설정해 두고, 각 오차 요인에 있어서 검출 오차가 크다고 추정된 경우에는, 각 감소량 ΔA를 각각 추출 범위 A로부터 감소시키도록 한다. 예를 들어, 도 11에서는, 주행 이력에서 어긋남양 Δy가 50m 이상, 교차점 통과 횟수가 3회 이상, 좌우회전 횟수가 2회 이상인 경우에는 각각 감소량 ΔA가 20m로 설정되고, 분류 또는 합류, 차선 변경의 횟수가 1회 이상인 경우에는 감소량 ΔA가 10m로 설정되어 있다. 또한, 주행 이력에서 차량이 주행한 커브의 곡률 반경이 50m 이하인 경우에는 감소량 ΔA가 50m로 설정되어 있다. 그리고, 이동량의 검출 오차가 크다고 추정된 오차 요인이 복수 있는 경우에는, 이들의 감소량 ΔA의 합을, 미리 설정된 200m로부터 감소시켜 추출 범위 A를 설정한다. 예를 들어, 주행 이력에서 좌우회전 횟수가 2회 이상이고 차선 변경이 1회 있는 경우에는, 각각의 감소량 20m와 10m의 합이 되는 30m를 200m으로부터 감소시켜 추출 범위 A를 170m로 설정한다. 이때, 감소량 ΔA의 합이 지나치게 커져 추출 범위 A가 지나치게 작아지면, 스텝 S50에서 실행되는 매칭이 어려워지므로, 추출 범위 A의 최솟값을 예를 들어 100m로 설정해 두도록 한다. 또한, 감소폭 ΔA는, 스텝 S50의 매칭 상태를 미리 실험이나 시뮬레이션에 의해 검증하여, 최적의 값이 설정되도록 한다.

[제1 실시 형태의 효과]

다음으로, 본 실시 형태에 관한 자기 위치 추정 장치에 의한 효과를 설명한다. 먼저, 종래의 자기 위치 추정 기술에 대해 설명하면, 종래에는 이동량과 센싱 결과의 이력을 축적하여 연결함으로써 자기 위치를 추정하고 있었다. 예를 들어, 도 12의 (a)는 차량의 좌우의 하향 방향을 촬상한 어안 카메라의 화상을 부감 변환한 것이다. 도 12의 (a)에 나타낸 바와 같이 현재의 부감 화상(121)은 작지만, 도 12의 (b)에 나타낸 바와 같이, 현재의 부감 화상(121)에 과거의 이동량과 센싱 결과(부감 화상)의 이력을 축적하여 연결한 부분(123)을 추가하면, 과거의 일정 구간의 부감 화상을 얻을 수 있다. 따라서, 주차 차량 등의 장해물이 있어도 과거의 센싱 결과를 사용하여 자기 위치를 추정하는 것이 가능해진다.

이때, 종래의 자기 위치 추정 기술에서는, 항상 일정 거리의 센싱 결과의 이력을 사용함으로써, 이동체의 이동 속도의 영향을 배제하고, 자기 위치의 추정을 안정적으로 행할 수 있도록 하고 있었다. 이것은, 일정 시간의 센싱 결과의 이력을 사용한 경우, 이동체가 극저속, 혹은 정지하고 있으면, 현재의 센싱 결과만을 이용하고 있는 것과 동일한 것으로 되어 버리기 때문이다. 또한, 이동 속도가 고속으로 되면, 센싱 결과의 이력을 연결하여 얻어지는 영역이 넓어져, 지도 정보와 일치하도록 조정하기 위한 연산 부하가 커지기 때문이다.

그러나, 상술한 종래의 자기 위치 추정 기술에서는, 일정 거리의 센싱 결과의 이력을 사용하고 있었기 때문에, 이동량의 검출 오차가 커지는 상황에서는, 센싱 결과와 지도 정보 사이의 괴리가 커져 자기 위치의 추정이 곤란해진다고 하는 문제점이 있었다.

예를 들어, 도 13에 나타낸 바와 같이 차량의 주행 이력이 일정속으로 직진이었던 경우(131)와, 가감속을 포함하는 좌우회전을 반복한 경우(133)에, 동일한 거리의 센싱 결과를 연결한 경우에 대해 비교한다.

도 14는, 이 비교 결과를 나타낸 도면이며, 도면 중의 흑색 동그라미는 차량에 탑재된 레이저 레인지 파인더에 의해 시시각각 검출한 백색선 에지의 위치를 나타내고 있고, 백색 동그라미는 종래의 자기 위치 추정 기술로 과거의 센싱 결과를 연결한 결과를 나타내고 있다.

도 14의 (a)에 나타낸 바와 같이, 차량의 주행 이력이 일정속으로 직진이었던 경우에는, 차량의 과거의 이동량 변화가 작기 때문에 이동량의 검출 오차는 작아져, 검출한 백색선 에지(흑색 동그라미)와 연결한 센싱 결과(백색 동그라미) 사이의 괴리는 작아진다.

이에 비해, 도 14의 (b)에 나타낸 바와 같이, 차량의 주행 이력에서, 차량이 가감속을 포함하는 좌우회전을 반복한 경우에는, 과거의 이동량 변화가 크고, 그 결과, 이동량의 검출 오차가 커지기 때문에, 현재로부터 과거로 거슬러 올라감에 따라서, 검출한 백색선 에지(흑색 동그라미)와 연결한 센싱 결과(백색 동그라미) 사이의 괴리가 커진다. 이러한 경우에는, 이동량의 검출에 사용하고 있는 차량 모델을 보다 엄밀한 모델로 함으로써, 이동량의 검출 오차를 저감시키는 대책이 생각된다. 그러나, 차량을 예로 들면, 탑승자나 탑재 연료의 변화에 의한 차체의 질량 변화나 노면의 마찰 계수 변화까지 엄밀하게 모델화하는 것은 어려워, 상기한 문제를 해결하는 것은 곤란하였다.

그래서, 본 실시 형태에 관한 자기 위치 추정 장치에서는, 차량의 주행 이력으로부터 이동량의 검출 오차를 추정하고, 추정한 검출 오차가 커짐에 따라서, 축적된 물표 위치 데이터로부터 추출하는 데이터의 범위를 작게 한다. 예를 들어, 도 15에 나타낸 바와 같이, 이동량의 검출 오차가 작은 직진의 경우에는 추출 범위(151)를 넓히고, 검출 오차가 큰 좌우회전을 반복하는 경우에는 추출 범위(153)를 좁힌다.

이와 같이, 본 실시 형태에 관한 자기 위치 추정 장치에서는, 주행 이력에 기초하여 물표 위치 데이터의 소정 범위(추출 범위 A)를 설정하고, 소정 범위(추출 범위 A)로부터 추출된 물표 위치 데이터와 지도 정보에 포함되는 물표 위치를 대조하여 차량의 자기 위치를 추정한다. 이에 의해, 주행 이력에서 차량이 좌우회전을 반복하는 경우는, 과거의 이동량 변화가 크기 때문에, 검출 오차가 크다고 하여, 소정 범위(추출 범위 A)를 좁혀 지도 정보와의 괴리가 충분히 작은 범위에서, 물표 위치 데이터와 지도 정보의 물표 위치를 대조할 수 있다. 또한, 주행 이력에서 차량이 정속으로 직진하는 경우는, 과거의 이동량 변화가 작기 때문에, 검출 오차가 작다고 하여, 소정 범위(추출 범위 A)를 넓혀, 보다 많은 물표 위치 데이터와 지도 정보의 물표 위치를 대조할 수 있다. 따라서, 주행 이력에 기초하여, 이동량의 검출 오차가 작은 상황뿐만 아니라, 검출 오차가 커지는 상황이라도, 자기 위치를 고정밀도로, 또한 안정적으로 추정할 수 있다.

또한, 본 실시 형태에 관한 자기 위치 추정 장치에서는, 주행 이력에서 차량의 좌우회전 횟수가 많을수록 소정 범위를 좁힌다. 교차점에서의 좌우회전은, 차량의 선회뿐만 아니라, 그 전후에 가감속도 수반하여, 과거의 이동량 변화가 크기 때문에, 차체의 전후 방향에 있어서의 이동량의 검출 오차가 커진다. 따라서, 이동량의 검출 오차가 축적되기 쉬운 좌우회전 횟수가 많은 경우에는, 소정 범위(추출 범위 A)를 작게 함으로써, 이동량의 검출 오차의 축적을 적게 할 수 있고, 이에 의해 자기 위치를 고정밀도로, 또한 안정적으로 추정할 수 있다.

또한, 본 실시 형태에 관한 자기 위치 추정 장치에서는, 주행 이력에서 차량의 차선 변경의 횟수가 많을수록 소정 범위를 좁힌다. 차량에서는 차선 변경 시에는, 과거의 이동량 변화가 커, 비선형인 차체의 사이드 슬립 운동이 발생하기 때문에, 고정밀도로 이동량을 추정하는 것이 어려워져, 이동량의 검출 오차가 커진다. 따라서, 이동량의 검출 오차가 축적되기 쉬운 차선 변경의 횟수가 많은 경우에는, 소정 범위(추출 범위 A)를 작게 함으로써, 이동량의 검출 오차의 축적을 적게 할 수 있고, 이에 의해 자기 위치를 고정밀도로, 또한 안정적으로 추정할 수 있다.

또한, 본 실시 형태에 관한 자기 위치 추정 장치에서는, 주행 이력에서 차량의 분류 또는 합류의 횟수가 많을수록 소정 범위를 좁힌다. 차량의 분류 또는 합류에서는 차선의 변경이나 선회와 같은 과거의 이동량 변화가 커, 이동량의 검출 오차가 확대되는 거동이 발생한다. 따라서, 이동량의 검출 오차가 축적되기 쉬운 분류 또는 합류의 횟수가 많은 경우에는, 소정 범위(추출 범위 A)를 작게 함으로써, 이동량의 검출 오차의 축적을 적게 할 수 있고, 이에 의해 자기 위치를 고정밀도로, 또한 안정적으로 추정할 수 있다.

또한, 본 실시 형태에 관한 자기 위치 추정 장치에서는, 주행 이력에서 차량이 주행한 커브의 곡률 반경이 작을수록 소정 범위를 좁힌다. 고차속으로 급한 커브를 주행하면, 차선 변경과 마찬가지로, 과거의 이동량 변화가 커, 비선형인 차체의 사이드 슬립 운동이 발생하기 때문에, 고정밀도로 이동량을 추정하는 것이 어려워진다. 따라서, 이동량의 검출 오차가 축적되기 쉬운 곡률 반경이 작은 커브를 주행한 경우에는, 소정 범위(추출 범위 A)를 작게 함으로써, 이동량의 검출 오차의 축적을 적게 할 수 있고, 이에 의해 자기 위치를 고정밀도로, 또한 안정적으로 추정할 수 있다.

[제2 실시 형태]

다음으로, 본 발명의 제2 실시 형태에 관한 자기 위치 추정 장치에 대해 도면을 참조하여 설명한다. 또한, 본 실시 형태에 관한 자기 위치 추정 시스템의 구성은 제1 실시 형태와 동일하므로, 상세한 설명은 생략한다.

[자기 위치 추정 처리의 순서]

본 실시 형태에 관한 자기 위치 추정 처리의 순서를 도 16의 흐름도를 참조하여 설명한다. 제1 실시 형태에서는, 스텝 S40에 있어서 차량의 주행 이력으로부터 이동량의 검출 오차를 추정하였다. 그러나, 본 실시 형태에서는, 스텝 S140에 있어서 주행 이력 중 차량 거동에 주목하여, 소정 범위(추출 범위 A)를 설정하는 것이 제1 실시 형태와 상이하다. 또한, 스텝 S10 내지 S30과 스텝 S50은, 도 3의 제1 실시 형태와 동일하므로, 상세한 설명은 생략한다.

스텝 S140에 있어서, 자기 위치 추정부(20)는, 주행 이력으로부터 차량의 이동량의 검출 오차를 추정하고, 추정한 이동량의 검출 오차에 기초하여, 물표 위치 축적부(16)로부터 물표 위치 데이터를 추출할 때의 소정 범위(추출 범위 A)를 설정한다. 특히, 본 실시 형태에서는, 주행 이력 중 과거의 이동량 변화인 차량 거동으로부터 이동량의 검출 오차를 추정하고 있고, 차량 거동의 구체예로서, 차량의 선회량으로부터 이동량의 검출 오차를 추정한다.

자기 위치 추정부(20)는, 주행 이력으로부터 1주기 전의 스텝 S50에서 산출된 차량의 자세각 θ(t)와, 현재보다 시간 T[s]만큼 전의 차량의 자세각 θ(t-T)의 차를 계산하여 이동량 변화인 선회량 dθ[rad]를 구한다. 그리고, 이 과거의 이동량 변화인 선회량 dθ의 절댓값이 역치 dθth 이상인 경우에는, 자기 위치 추정부(20)는 이동량의 검출 오차가 크다고 추정할 수 있다. 한편, 과거의 이동량 변화인 선회량 dθ의 절댓값이 역치 dθth보다 작은 경우에는, 자기 위치 추정부(20)는 이동량의 검출 오차가 작다고 추정할 수 있다. 또한, 역치 dθth는, 예를 들어 1.745[rad]≒100[deg]으로 설정하면 된다.

이와 같이 차량의 이동량 변화인 선회량이 크다고 하는 것은, 오도메트리의 오차가 축적되기 쉬운 선회나 차선 변경, 곡선로의 주행을 행하였다고 추정할 수 있으므로, 이동량의 검출 오차가 크다고 추정할 수 있다.

그리고, 자기 위치 추정부(20)는, 주행 이력의 과거의 이동량 변화로부터 이동량의 검출 오차가 작다고 추정되는 경우에는, 스텝 S50에서 물표 위치 데이터를 추출할 때의 추출 범위 A[m]를 크게 하여, 예를 들어 200m로 설정한다. 한편, 자기 위치 추정부(20)는, 주행 이력의 과거의 이동량 변화로부터 이동량의 검출 오차가 크다고 추정되는 경우에는, 추출 범위 A[m]를 작게 하여, 예를 들어 100m로 설정한다. 또한, 주행 이력의 과거의 이동량 변화인 선회량이 커짐에 따라서, 추출 범위 A를 연속적으로 작아지도록 변화시켜도 된다. 즉, 주행 이력의 과거의 이동량 변화로부터 추정되는 이동량의 검출 오차가 커짐에 따라서, 추출 범위 A가 작아지도록 설정한다.

이와 같이, 차량의 선회량이 커서 오도메트리의 오차가 축적되기 쉬운 경우에는, 추출 범위 A를 작게 하여, 오도메트리의 오차의 축적이 적어지도록 한다.

또한, 선회량 dθ가 아닌, 주행 이력의 현재의 시각 t로부터 시간 T[s]만큼 전까지의 시시각각의 요각의 변화량 Δθ(t)의 절댓값을 적분하여 선회량 dθ를 구해도 된다. 이 경우, 슬랄롬 주행 등에 의해 자세각이 외관상은 원래대로 되돌아가 있어도, 실제의 선회량은 크게 되어 있는 것을 검출할 수 있다.

또한, 선회량의 적분값이 아닌 추출 범위 A에 있어서의 선회 속도(요레이트)의 절댓값의 최댓값 γabs[rad/s]를 검출하고, 이 γabs가 역치 γth[rad/s] 이상인 경우에 이동량의 검출 오차가 크다고 추정해도 된다. 역치 γth는, 예를 들어 0.39[rad/s]≒20[deg/s]으로 설정하면 된다.

[변형예 6]

또한, 변형예 6으로서는, 스텝 S140의 차량 거동의 구체예로서, 차량의 차속 변화가 클수록 이동량의 검출 오차가 크다고 추정하여, 추출 범위 A를 좁힌다. 이 경우, 먼저 추출 범위 A를 200m로 설정해 두고, 차량의 전후 방향 가속도를 계측하는 가속도 센서(46)의 계측값 α[m/s²]의 절댓값의 최댓값 αabs[m/s²]를 검출한다.

그리고, 주행 이력으로서, 이 αabs가 역치 αth[m/s²]보다 작은 경우에는 이동량의 검출 오차는 작다고 추정하여 추출 범위 A를 크게, 예를 들어 200m 그대로 설정한다. 한편, 주행 이력으로서, αabs가 역치 αth 이상인 경우에는 이동량의 검출 오차가 커진다고 추정하여, 추출 범위 A를 작게, 예를 들어 100m로 설정한다. 역치 αth는, 예를 들어 0.2[m/s²]로 설정하면 된다. 이때, αabs에 따라서 추출 범위 A를 연속적으로 변화시켜도 된다.

이와 같이, 차량이, 오도메트리의 오차가 축적되기 쉬운 큰 차속 변화를 한 경우에는, 추출 범위 A를 작게 하여 오도메트리의 오차의 축적이 적어지도록 한다.

또한, 가속도 센서(46)에 다축 센서를 사용하여, 차량의 차폭 방향, 상하 방향의 가속도를 계측하여, 그 합성 성분으로 판단하도록 해도 된다. 또한, 스텝 S140에서는, 차량 거동으로부터, 예를 들어 차량이 교차점을 좌우회전한 것, 차선 변경을 행한 것 등을 판정하고, 제1 실시 형태에서 설명한 방법으로 추출 범위 A를 설정해도 된다.

[제2 실시 형태의 효과]

이상 상세하게 설명한 바와 같이, 본 실시 형태에 관한 자기 위치 추정 장치에서는, 주행 이력의 과거의 이동량 변화인 차량의 선회량이 클수록 이동량의 검출 오차가 크다고 추정하여, 소정 범위(추출 범위 A)를 작게 한다. 차량이 선회하면, 선회 방향뿐만 아니라 타이어의 슬립에 의한 차폭 방향의 이동량의 검출 오차가 커진다. 따라서, 주행 이력에 기초하여, 이동량의 검출 오차가 축적되기 쉬운 차량의 선회량이 큰 경우에는, 소정 범위(추출 범위 A)를 작게 함으로써, 이동량의 검출 오차의 축적을 적게 할 수 있고, 이에 의해 자기 위치를 고정밀도로, 또한 안정적으로 추정할 수 있다.

또한, 본 실시 형태에 관한 자기 위치 추정 장치에서는, 주행 이력의 차량의 차속 변화가 클수록 이동량의 검출 오차가 크다고 추정하여, 소정 범위(추출 범위 A)를 작게 한다. 차량에 가감속이 발생한 경우에는, 과거의 이동량 변화가 크기 때문에 차체의 전후 방향의 이동량의 검출 오차가 커진다. 따라서, 주행 이력에 기초하여, 이동량의 검출 오차가 축적되기 쉬운 차속 변화가 큰 경우에는, 소정 범위(추출 범위 A)를 작게 함으로써, 이동량의 검출 오차의 축적을 적게 할 수 있고, 이에 의해 자기 위치를 고정밀도로, 또한 안정적으로 추정할 수 있다.

또한, 상술한 실시 형태는 본 발명의 일례이다. 이 때문에, 본 발명은, 상술한 실시 형태에 한정되는 일은 없고, 이 실시 형태 이외의 형태라도, 본 발명에 관한 기술적 사상을 일탈하지 않는 범위이면, 설계 등에 따라서 다양한 변경이 가능한 것은 물론이다.

특히, 상술한 실시 형태에서는 차량을 예로 들어 설명하였지만, 적어도 1대의 카메라 또는 레이저 레인지 파인더 중 어느 한쪽 또는 양쪽과, 오도메트리를 계측하기 위한 센서를 탑재한 이동체이면, 항공기나 선박 등에도 본 발명은 적용 가능하다. 또한, 상술한 실시 형태에서는 차량의 3 자유도의 위치 및 자세각을 구하였지만, 6 자유도에서의 위치 및 자세각을 추정하는 것도 가능하다.

1 : ECU
2, 2a, 2b : 카메라
3 : 3차원 지도 데이터베이스
4 : 차량 센서 군
5, 5a, 5b : 레이저 레인지 파인더
10 : 자기 위치 추정 장치
12 : 물표 위치 검출부
14 : 이동량 검출부
16 : 물표 위치 축적부
18 : 지도 정보 취득부
20 : 자기 위치 추정부
41 : GPS 수신기
42 : 액셀러레이터 센서
43 : 스티어링 센서
44 : 브레이크 센서
45 : 차속 센서
46 : 가속도 센서
47 : 차륜속 센서
48 : 요레이트 센서

Claims (9)

1. 이동체의 자기 위치를 추정하는 자기 위치 추정 장치이며,
   상기 이동체의 주위에 존재하는 물표의 물표 위치를 검출하는 물표 위치 검출부와,
   상기 이동체의 이동량을 검출하는 이동량 검출부와,
   상기 물표 위치 검출부에 의해 검출된 물표 위치를, 상기 이동량 검출부에 의해 검출된 이동량만큼 이동시켜 물표 위치 데이터로서 축적하는 물표 위치 축적부와,
   지도 상에 존재하는 물표의 물표 위치를 포함하는 지도 정보를 취득하는 지도 정보 취득부와,
   상기 이동체의 현재 위치까지의 이동 이력의 오차 요인에 기초하여 설정된 소정 범위의 상기 물표 위치 데이터와 상기 지도 정보에 포함되는 물표 위치를 대조하여 상기 이동체의 자기 위치를 추정하는 자기 위치 추정부
   를 구비한 것을 특징으로 하는, 자기 위치 추정 장치.
2. 제1항에 있어서,
   상기 자기 위치 추정부는, 상기 이동 이력의 상기 이동체의 과거의 이동량 변화가 클수록 상기 소정 범위를 작게 하는 것을 특징으로 하는, 자기 위치 추정 장치.
3. 제1항 또는 제2항에 있어서,
   상기 이동체는, 차량이며,
   상기 자기 위치 추정부는, 상기 이동 이력에서 상기 차량의 좌우회전 횟수가 많을수록 상기 소정 범위를 작게 하는 것을 특징으로 하는, 자기 위치 추정 장치.
4. 제1항 또는 제2항에 있어서,
   상기 이동체는, 차량이며,
   상기 자기 위치 추정부는, 상기 이동 이력에서 상기 차량의 차선 변경의 횟수가 많을수록 상기 소정 범위를 작게 하는 것을 특징으로 하는, 자기 위치 추정 장치.
5. 제1항 또는 제2항에 있어서,
   상기 이동체는, 차량이며,
   상기 자기 위치 추정부는, 상기 이동 이력에서 상기 차량의 분류 또는 합류의 횟수가 많을수록 상기 소정 범위를 작게 하는 것을 특징으로 하는, 자기 위치 추정 장치.
6. 제1항 또는 제2항에 있어서,
   상기 이동체는, 차량이며,
   상기 자기 위치 추정부는, 상기 이동 이력에서 상기 차량이 주행한 커브의 곡률 반경이 작을수록 상기 소정 범위를 작게 하는 것을 특징으로 하는, 자기 위치 추정 장치.
7. 제1항 또는 제2항에 있어서,
   상기 자기 위치 추정부는, 상기 이동 이력에서 상기 이동체의 선회량이 클수록 상기 소정 범위를 작게 하는 것을 특징으로 하는, 자기 위치 추정 장치.
8. 제1항 또는 제2항에 있어서,
   상기 자기 위치 추정부는, 상기 이동 이력에서 상기 이동체의 이동 속도 변화가 클수록 상기 소정 범위를 작게 하는 것을 특징으로 하는, 자기 위치 추정 장치.
9. 이동체의 자기 위치를 추정하는 자기 위치 추정 방법이며,
   상기 이동체에 탑재된 제어부가, 상기 이동체의 주위에 존재하는 물표의 물표 위치를 검출하고,
   상기 제어부가, 상기 이동체의 이동량을 검출하고,
   상기 제어부가, 검출된 상기 물표 위치를, 검출된 상기 이동량만큼 이동시켜 물표 위치 데이터로서 축적하고,
   상기 제어부가, 지도 상에 존재하는 물표의 물표 위치를 포함하는 지도 정보를 취득하고,
   상기 제어부가, 상기 이동체의 현재 위치까지의 이동 이력의 오차 요인에 기초하여 설정된 소정 범위의 상기 물표 위치 데이터와 상기 지도 정보에 포함되는 물표 위치를 대조하여 상기 이동체의 자기 위치를 추정하는 것을 특징으로 하는, 자기 위치 추정 방법.

Images