KR20110077454A

KR20110077454A - 이동체의 위치 및 자세를 결정하는 시스템 및 방법

Info

Publication number: KR20110077454A
Application number: KR1020090134042A
Authority: KR
Inventors: 홍진석; 권철범
Original assignee: (주)마이크로인피니티
Priority date: 2009-12-30
Filing date: 2009-12-30
Publication date: 2011-07-07

Abstract

이동체의 위치 및 자세를 결정하는 시스템 및 방법을 개시한다. 위치 및 자세 계산 시스템은 자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값 및 자세 변환 행렬을 계산하는 자세값 계산부, 가속도 센서로부터 수신된 센서값에 기초하여 이동체의 속도값을 계산하는 속도값 계산부, 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차를 추정하는 제1 필터부, 자세값과 속도값을 이용하여 위치를 계산하는 위치 계산부 및 계산된 위치와 GPS 수신기를 통해 수신된 위치를 이용하여 위치 오차를 추정하는 제2 필터부를 포함하고, 초기 동작 시 자세 변환 행렬, 중력 가속도 및 가속도 센서의 센서값에 기초하여 가속도 센서의 바이어스를 계산한다.

블랙박스, 바이어스, 자이로(gyro), 가속도, 속도, 자세(Attitude)

Description

이동체의 위치 및 자세를 결정하는 시스템 및 방법{SYSTEM AND METHOD FOR DETERMINING POSITION AND ATTITUDE OF VEHICLE}

본 발명에 따른 실시예들은 이동체의 위치 및 자세를 결정하는 시스템 및 방법에 관한 것이다.

블랙박스용 위치 및 자세 계산 시스템은 GPS와 속도계(예를 들어, 차속계), 3축 자이로와 3축 가속도계를 이용하여 차량의 자세, 속도 등을 출력하는 시스템이다. 먼저 3축 자이로를 이용하여, 자세를 구하게 되고 3축 가속도를 이용하여 속도를 계산하게 된다. 필터를 이용하여, 자이로와 가속도계의 바이어스(bias), 속도계의 환산 계수 등의 오차를 추정하고 이를 보상하게 된다. 이후 계산된 자세와 속도를 이용하여 위치를 계산하고, 계산된 위치와 GPS 위치 정보를 이용하여 필터에서 위치 오차를 추정하고 보상하게 된다. GPS가 수신되지 않을 경우에는 자세와 속도만을 이용하여 위치를 구하게 된다. 블랙박스용 위치 및 자세 계산 시스템은 기본적으로 초기 동작 후 전원을 껐을 때 최종적인 위치와 자세가 메모리를 통해 백업이 되도록 설계되고, 다시 전원을 켰을 때 백업된 자세로부터 자세가 변하도록 설계된다. 동작 초기에 자이로 센서와 가속도 센서의 바이어스 추정이 자세 출력 의 성능에 커다란 영향을 주게 되는데, 대부분 초기 동작 시에는 차량이 움직이지 않고 정지 상태를 유지하기 때문에 자이로 센서의 바이어스는 추정값이 큰 오차를 가지지 않는다. 그러나 가속도 센서는 비록 정지 상태이기는 하나, 중력 가속도가 항상 작용하고 있으며, 자세가 기울어져 있을 경우, 가속도 성분이 세 축(axis)에 각각 나뉘어 작용하므로 초기 바이어스 추정이 쉽지 않다. 고가의 가속도 센서는 실험을 통해 센서 파라미터 정보를 얻고, 이러한 파라미터 정보를 이용해 바이어스 값을 계산할 수 있지만, 저가의 센서나, 블랙박스와 같이 사고 판단을 위해 "High G"(35G이상)의 센서를 사용할 때에는 비교적 잡음이 크고, 런투런(run-to-run) 바이어스가 심하므로 초기 바이어스 추정에 어려움이 있다.

본 명세서에서는 효율적으로 이동체의 자세를 계산할 수 있는 위치 및 자세 계산 시스템 및 방법이 제안된다.

초기 동작 시, 차량의 자세에 기초하여 계산된 자세 변환 행렬과 정지 상태의 이동체에 작용하는 중력가속도 그리고 가속도 센서의 출력인 센서값에 기초하여 가속도 센서의 바이어스를 추정하는 위치 및 자세 계산 시스템 및 방법이 제공된다.

GPS 수신기가 정상적으로 포지션 픽스(position fix)되었을 때, GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환하고, 변환된 속도간의 연산을 통해 계산함으로써, 이러한 속도 정보와 자세 정보를 통해 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차보다 보다 정확하게 추정하는 위치 및 자세 계산 시스템 및 방법이 제공된다.

본 발명의 일실시예에 따른 위치 및 자세 계산 시스템은 자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값 및 자세 변환 행렬을 계산하는 자세값 계산부, 가속도 센서로부터 수신된 센서값에 기초하여 이동체의 속도값을 계산하는 속도값 계산부 및 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차를 추정하는 필터부를 포함한다. 이때, 필터부는 초기 동작 시, 자세 변환 행렬, 중력 가속도 및 가속도 센서의 센서값에 기초하여 가속도 센서의 바이어스를 계산한다.

본 발명의 일측면에 따르면, 필터부는 이동체의 속도계로부터 수신되는 속도 정보, 이전 시간에 측정된 자세값 및 속도값에 기초하여 오차 및 속도계의 환산 계수의 오차를 각각 추정할 수 있다.

본 발명의 일측면에 따르면, 필터부는 이동체의 GPS 수신부로부터 수신되는 속도 정보, 이전 시간에 측정된 자세값 및 속도값에 기초하여 오차를 각각 추정할 수 있다.

본 발명의 일측면에 따르면, 자세값은 이동체의 롤(roll), 피치(pitch) 및 요(yaw) 중 적어도 하나에 대한 측정값을 포함할 수 있다. 이때, 자세 변환 행렬은 측정값을 각도로 하는 삼각함수에 기초하여 계산될 수 있다.

본 발명의 일실시예에 따른 위치 및 자세 계산 시스템은 자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값을 계산하는 자세값 계산부, 가속도 센서로부터 수신된 센서값에 기초하여 이동체의 속도값을 계산하는 속도값 계산부 및 GPS 수신기로부터 수신된 속도 정보 및 자세 정보, 자세값 및 속도값에 기초하여 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차를 추정하는 필터부를 포함한다. 이때, 속도 정보는 GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환함으로써 계산되고, 자세 정보는 변환된 속도간의 연산을 통해 계산된다.

본 발명의 일실시예에 따른 위치 및 자세 계산 방법은 자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값 및 자세 변환 행렬을 계산하는 단계, 가속도 센서로부터 수신된 센서값에 기초하여 이동체의 속도값을 계산하는 단계, 자 이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차를 추정하는 단계를 포함한다. 이때, 오차를 추정하는 단계는 초기 동작 시 자세 변환 행렬, 중력 가속도 및 가속도 센서의 센서값에 기초하여 가속도 센서의 바이어스를 계산하는 단계를 포함한다.

본 발명의 일실시예에 따른 위치 및 자세 계산 방법은 자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값을 계산하는 단계, 가속도 센서로부터 수신된 센서값에 기초하여 이동체의 속도값을 계산하는 단계 및 GPS 수신기로부터 수신된 속도 정보 및 자세 정보, 자세값 및 속도값에 기초하여 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차를 추정하는 단계를 포함한다. 이때, 속도 정보는 GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환함으로써 계산되고, 자세 정보는 변환된 속도간의 연산을 통해 계산된다.

초기 동작 시, 차량의 자세에 기초하여 계산된 자세 변환 행렬과 정지 상태의 이동체에 작용하는 중력가속도 그리고 가속도 센서의 출력인 센서값에 기초하여 가속도 센서의 바이어스를 추정할 수 있다.

GPS 수신기가 정상적으로 포지션 픽스(position fix)되었을 때, GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환하고, 변환된 속도간의 연산을 통해 계산함으로써, 이러한 속도 정보와 자세 정보를 통해 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차보다 보다 정확하게 추정할 수 있다.

이동체의 위치, 속도 및 가속도 등의 항법 정보와 기본적인 센서 출력 이외에도 자체적인 자세계산을 통해 자세정보를 출력함으로써, 일반적인 접촉사고나 추돌 사고 외에 차량과 같은 이동체의 전복이나 미끄럼 길 등에서 급격히 자세가 불안하여 유발되는 사고에 대해서도 이동체의 자세정보를 통해 사고 분석이나 재현, 상황 재구성 등에 유용한 정보로 활용할 수 있다.

이하 첨부된 도면을 참조하여 본 발명에 따른 다양한 실시예를 상세히 설명하기로 한다.

본 발명의 실시예들에 따른 위치 및 자세 계산 시스템은 블랙박스 등에 사용 가능한 것으로 GPS 수신기와 속도계로부터 수신되는 정보와 자이로 센서와 가속도 센서를 통해 차량의 위치, 자세 및 속도를 계산하여 출력하는 시스템이다.

이러한 위치 및 자세 계산 시스템은 먼저 자이로 센서를 이용하여, 자세(attitude)를 구하게 되고, 가속도 센서를 이용하여 속도를 계산하게 된다. 이러한, 위치 및 자세 계산 시스템은 두 개의 필터를 이용할 수 있다.

첫 번째 필터에서는 계산된 자세와 속도 그리고 속도계로부터 수신되는 속도 정보, GPS 수신기로부터 수신되는 속도와 자세 정보를 이용하여, 자이로 센서와 가속도 센서의 바이어스의 오차나 속도계의 환산 계수 등의 오차를 추정하고 이를 보상하게 된다.

이때, 최초 자이로 센서와 가속도 센서를 통해 계산된 자세와 속도를 이용하 여 이동체의 위치를 계산하게 되고, 두 번째 필터에서는 계산된 위치의 값과 GPS 수신기로부터 수신되는 위치 정보를 이용하여 위치 오차를 추정하여 보상하게 된다.

혹은 하나의 필터에서 자이로 센서와 가속도 센서의 바이어스 및 속도계의 환산 계수 등에 대한 오차뿐만 아니라 계산된 위치의 값에 대한 오차까지 모두 추정하여 보상할 수도 있다.

기본적으로 본 실시예들에 따른 위치 및 자세 계산 시스템은 초기 동작 후 전원을 껐을 때, 최종적인 위치와 자세가 메모리를 통해 백업이 되도록 설계될 수 있고, 다시 전원을 켰을 때 백업된 값을 초기 출발점으로 인식하여 위치 및 자세가 계산되도록 설계될 수 있다.

또한 본 실시예들에 따른 위치 및 자세 계산 시스템은 GPS 수신기가 수신되지 않더라도 지속적으로 위치와 자세, 속도 정보를 제공할 수 있다. 이 경우 GPS 수신기의 위치 정보를 이용하지 않고, 앞서 계산된 자세와 속도를 이용하여 위치를 계산할 수 있다. 다시 말해, 터널이나 지하주차장과 같이 GPS 수신기를 이용할 수 없는 지역에서도 항법 정보를 꾸준히 제공하여, 차량 사고 분석 및 재현에 큰 도움을 줄 수 있다.

도 1은 본 발명의 일실시예에 따른 위치 및 자세 계산 시스템의 전체적인 모습을 도시한 일례이다. 본 실시예에 따른 위치 및 자세 계산 시스템(110)은 일례로, 블랙박스 등에 이용될 수 있다. 도 1에 도시된 바와 같이, 위치 및 자세 계산 시스템(110)은 자이로 센서(111), 가속도 센서(112), 자세값 계산부(113), 속도값 계산부(114) 및 제1 필터부(115)를 포함할 수 있다. 이러한 위치 및 자세 계산 시스템(110)은 또한, 이동체에 포함되어, 이동체가 더 포함할 수 있는 속도계(120), GPS 수신기(130)의 출력과 자이로 센서(111) 및 가속도 센서(112)의 센서값을 통해 이동체의 자세와 속도 등을 출력하는 시스템이다. 먼저, 3축 자이로인 자이로 센서(111)를 이용하여 자세를 구하고, 3축 가속도계인 가속도 센서(112)를 이용하여 속도를 계산하게 된다. 제1 필터부(115)는 자이로 센서(111)와 가속도 센서(112)의 바이어스, 속도계(120)의 환산 계수 그리고 자세값 계산부(113)와 속도값 계산부(114)의 출력인 자세값 및 속도값 각각의 오차를 추정하고 이를 보상하기 위해 이용될 수 있다. 또한, 위치 및 자세 계산 시스템(110)은 자세값과 속도값을 통해 이동체의 위치를 계산할 수 있고, 더 포함하는 제2 필터부(116)를 이용하여 위치에 대한 오차를 추정하여 보상할 수 있다. 즉, 제2 필터부(116)는 GPS 수신기로부터 수신되는 위치 정보를 통해 오차를 추정하고, 추정된 오차를 통해 자세값과 속도값을 이용하여 계산된 위치에 의해 발생되는 오차를 보상할 수 있다. 위치 및 자세 계산 시스템(110)은 별도의 필터인 제2 필터부(116)를 통해 이러한 위치에 대한 오차의 추정 및 보상을 수행할 수도 있지만, 제2 필터부(116)를 포함하지 않고, 제1 필터부(115)를 수행할 수도 있다.

도 2는 본 발명의 일실시예에 있어서, 위치 및 자세 계산 시스템의 내부 구성을 설명하기 위한 블록도이다. 본 실시예에 따른 위치 및 자세 계산 시스템(200)은 도 2에 도시된 바와 같이 자세값 계산부(210), 속도값 계산부(220) 및 필터부(230)를 포함한다.

자세값 계산부(210)는 자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값 및 자세 변환 행렬을 계산한다. 이때, 자세값은 이동체의 롤(roll), 피치(pitch) 및 요(yaw) 중 적어도 하나에 대한 측정값을 포함할 수 있고, 자세 변환 행렬은 측정값을 각도로 하는 삼각함수에 기초하여 계산될 수 있다. 또한, 자세값 계산부(210)는 이동체의 속도계로부터 수신되는 속도 정보를 더 이용하여 자세값 및 자세 변환 행렬을 계산함으로써, 자세 발산(divergence)을 막을 수 있다.

속도값 계산부(220)는 가속도 센서로부터 수신된 센서값에 기초하여 이동체의 속도값을 계산한다. 이때, 자세값 계산부(210)와 속도값 계산부(220)는 모두 이전 시간에 측정된 자세값과 속도값에 기초하여 필터부(230)를 통해 추정된 오차를 보상하여 현재 시간의 자세값과 속도값을 계산할 수 있다. 이러한 필터부(230)는 바로 이전 시간에 추정한 값을 토대로 해서 현재의 값을 추정 즉, 재귀적으로 동작하는 칼만 필터를 포함할 수 있다.

필터부(230)는 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차를 추정한다. 이때, 필터부(230)는 초기 동작 시, 자세 변환 행렬, 중력 가속도 및 가속도 센서의 센서값에 기초하여 가속도 센서의 바이어스를 계산한다. 이동체의 초기 동작 시 정지상태에서 가속도 센서의 바이어스 추정이 자세값의 오차에 큰 영향을 주게 되는데, 일반적으로 가속도 센서의 바이어스는 실험을 통해 센서의 동작 특성을 파악하고 이때 얻어진 파라미터를 이용해, 그 값을 계산하게 된다. 그러나 런투런 바이어스가 심해서 동작할 때 마다 바이어스의 변화폭이 큰 가속도 센서의 경우에는 이미 알고 있는 자세 정보를 이용하여 가속도 센서의 바이 어스를 추정할 수 있다. 즉, 필터부(230)는 이미 알고 있는 자세 정보인 자세 변환 행렬을 이용하여 가속도 센서의 바이어스를 계산할 수 있다.

초기 출발 시 차량의 자세가 롤(ф), 피치(θ), 요(ψ)를 통해 표현될 때, 자세 변환 행렬은 다음 수학식 1과 같이 표현될 수 있다.

여기서, "

"는 자세 변환 행렬을 의미할 수 있다.

또한, 정지 상태에서는 중력 가속도 이외의 별도의 가속도 성분은 없기 때문에 항법 좌표계에서 이동체의 세 축에 대한 가속도 성분은 다음 수학식 2와 같은 행렬로 표현될 수 있다.

여기서 'G'는 중력 가속도를 의미할 수 있다.

이때, 이동체에 대한 좌표계에서 가속도 성분은 다음 수학식 3과 같이 표현될 수 있다.

여기서, "Ax," "Ay" 및 "Az"는 각각 이동체에 대한 좌표계에서의 3축 각각에 대한 가속도 성분을 의미할 수 있다.

3축에 대한 환산 계수를 각각 "Asfx", "Asfy", "Asfz"라 하고, 가속도 센서의 ADC(Analog to Digital Converter) 출력을 각각 "ADCx", "ADCy" 및 "ADCz" 그리고 초기 바이어스를 "Biasx", "Biasy" 및 "Biasz"라고 했을 때, 초기 바이어스는 다음 수학식 4와 같이 표현될 수 있다.

이때, ADC출력을 "m/s²"의 가속도 출력의 단위로 나타내면 초기 바이어스는 다음 수학식 5와 같이 표현될 수도 있다.

여기서, "Accx", "Accy" 및 "Accz"는 가속도 센서를 통해 출력되는 센서값을 의미할 수 있다. 이때, 상기 이동체의 GPS 수신부로부터 수신되는 속도 정보, 이전 시간에 상기 속도값 계산부 계산된 자세값 및 속도값에 기초하여 상기 오차를 각각 추정할 수도 있다.

다른 실시예로, 필터부(230)는 GPS 수신기로부터 수신된 속도 정보 및 자세 정보, 상기 자세값 및 상기 속도값에 기초하여 상기 자이로 센서와 상기 가속도 센서의 바이어스, 상기 자세값 및 상기 속도값 각각의 오차를 추정한다. 즉, 상술한 초기 동작 이후, GPS가 정상적으로 포지션 픽스(Position Fix) 상태에 들어갔을 때, GPS 수신기에서 제공하는 속도 정보와 자세 정보가 위치 및 자세 계산 시스템(200)의 자세값 및 속도값 계산을 위한 측정값으로서 이용될 수 있다. 이때, 속도 정보는 상기 GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환함으로써 계산되고, 상기 자세 정보는 상기 변환된 속도간의 연산을 통해 계산된다.

GPS 수신기에서 출력된 속도 정보를 "Vx", "Vy" 및 "Vz" 라고 하면, 속도 정보는 경도와 위도의 위치 정보를 이용하여 항법 좌표계의 값으로 변환하여 다음 수학식 6과 같이 표현될 수 있다.

여기서,

는 경도 및 위도의 위치 정보로서 일례로, 다음 수학식 7과 같은 행렬로 표현될 수 있다.

여기서, "Lat"는 위도를, "Lon"은 경도를 각각 의미할 수 있다.

항법 좌표계의 속도가 수학식 6에서와 같이 "Vn", "Ve" 및 "Vd"라 할 때, GPS를 이용한 자세 정보는 하기 수학식 8과 같이 계산될 수 있다.

단, 요(ψ)의 계산에서, 요(ψ)는 'Vn > 0'이고, 'Ve < 0'이거나 'Vn < 0'이고, 'Ve < 0'일 때 '360 - 요(ψ)'의 값을 가질 수 있다.

필터부(230)는 수학식 8과 같이 계산된 자세 정보와 속도 정보, 자이로 센서와 가속도 센서의 센서값을 통해 이전 시간에 계산된 자세값 및 속도값을 이용하여 (1) 현재 시간의 자세값과 속도값 각각의 오차 그리고 (2)자이로 센서와 가속도 센서의 바이어스의 각각의 오차를 추정하여 보상하게 된다.

이때, 두 실시예 모두에서 위치 및 자세 계산 시스템(200)의 필터부(230)는 속도계로부터 수신되는 속도 정보를 더 이용하여 자세값 및 속도값 각각의 오차를 추정할 수 있다. 이때, 필터부(230)는 속도계의 환산 계수에 대한 오차 또한 추정하여 속도계로 전송할 수 있다.

또한, 위치 및 자세 계산 시스템(200)은 자세값 및 속도값에 기초하여 이동체의 위치를 계산하는 위치 계산부(미도시)를 더 포함할 수 있고, 자세값, 속도값 및 위치를 이용하여 이동체의 자세, 속도 및 위치 중 적어도 하나의 정보를 제공할 수 있다. 따라서, GPS 정보를 수신할 수 없는 경우에도, 이동체의 위치, 속도, 자세와 같은 정보를 사용자에게 제공할 수 있게 된다.

이와 같이, 본 발명의 일실시예에 따른 위치 및 자세 계산 시스템을 이용하면, 초기 동작 시, 차량의 자세에 기초하여 계산된 자세 변환 행렬과 정지 상태의 이동체에 작용하는 중력가속도 그리고 가속도 센서의 출력인 센서값에 기초하여 가속도 센서의 바이어스를 추정할 수 있다. 또는 GPS 수신기가 정상적으로 포지션 픽스(position fix)되었을 때, GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환하고, 변환된 속도간의 연산을 통해 계산함으로써, 이러한 속도 정보와 자세 정보를 통해 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차보다 보다 정확하게 추정할 수 있다. 본 실시예에서는, 위치 및 자세 계산 시스템(200)이 이동체의 초기 동작 시와 그 이후의 두 가지 경우에 따라 설명하였으나, 하나의 위치 및 자세 계산 시스템(200)에서 각각의 경우에 따라 동작될 수도 있음은 당연하다.

또한, 본 발명의 일실시예에 따른 위치 및 자세 계산 시스템을 이용하면, 이 동체의 위치, 속도 및 가속도 등의 항법 정보와 기본적인 센서 출력 이외에도 자체적인 자세계산을 통해 자세정보를 출력함으로써, 일반적인 접촉사고나 추돌 사고 외에 차량과 같은 이동체의 전복이나 미끄럼 길 등에서 급격히 자세가 불안하여 유발되는 사고에 대해서도 이동체의 자세정보를 통해 사고 분석이나 재현, 상황 재구성 등에 유용한 정보로 활용할 수 있다. 뿐만 아니라 GPS 수신기가 동작하지 않거나 GPS 수신기로부터 정보를 수신할 수 없는 경우에도 자이로 센서, 가속도 센서 및 속도계를 통해 지속적으로 속도와 자세가 계산 및 출력되고, 이를 이용하여 위치를 계산할 수 있기 때문에 GPS 정보가 수신되지 않는 지하주차장이나 터널 등에서도 지속적으로 항법해를 제공할 수 있다.

도 3은 본 발명의 일실시예에 있어서, 위치 및 자세 계산 방법을 도시한 흐름도이다. 본 실시예에 따른 위치 및 자세 계산 방법은 도 2를 통해 설명한 위치 및 자세 계산 시스템(200) 중 첫 번째 실시예에 따른 위치 및 자세 계산 시스템(200)를 통해 수행될 수 있다. 도 3에서는 위치 및 자세 계산 시스템(200)을 통해 각각의 단계가 수행되는 과정을 설명함으로써, 위치 및 자세 계산 방법을 설명한다.

단계(310)에서 위치 및 자세 계산 시스템(200)은 자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값 및 자세 변환 행렬을 계산한다. 이때, 자세값은 이동체의 롤, 피치 및 요 중 적어도 하나에 대한 측정값을 포함할 수 있고, 자세 변환 행렬은 측정값을 각도로 하는 삼각함수에 기초하여 계산될 수 있다. 또한, 위치 및 자세 계산 시스템(200)은 이동체의 속도계로부터 수신되는 속도 정보 를 더 이용하여 자세값 및 자세 변환 행렬을 계산함으로써, 자세 발산을 막을 수 있다.

단계(320)에서 위치 및 자세 계산 시스템(200)은 가속도 센서로부터 수신된 센서값에 기초하여 이동체의 속도값을 계산한다. 이때, 위치 및 자세 계산 시스템(200)은 단계(310)과 단계(320)에서, 이전 시간에 측정된 자세값과 속도값에 기초하여 필터를 통해 추정된 오차를 보상하여 현재 시간의 자세값과 속도값을 계산할 수 있다. 이러한 필터는 바로 이전 시간에 추정한 값을 토대로 해서 현재의 값을 추정 즉, 재귀적으로 동작하는 칼만 필터를 포함할 수 있다. 이와 같은 오차의 추정은 단계(330)에서 수행될 수 있다.

단계(320)에서 위치 및 자세 계산 시스템(200)은 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차를 추정한다. 이때, 위치 및 자세 계산 시스템(200)은 초기 동작 시, 자세 변환 행렬, 중력 가속도 및 가속도 센서의 센서값에 기초하여 가속도 센서의 바이어스를 계산한다. 이동체의 초기 동작 시 정지상태에서 가속도 센서의 바이어스 추정이 자세값의 오차에 큰 영향을 주게 되는데, 일반적으로 가속도 센서의 바이어스는 실험을 통해 센서의 동작 특성을 파악하고 이때 얻어진 파라미터를 이용해, 그 값을 계산하게 된다. 그러나 런투런 바이어스가 심해서 동작할 때 마다 바이어스의 변화폭이 큰 가속도 센서의 경우에는 이미 알고 있는 자세 정보를 이용하여 가속도 센서의 바이어스를 추정할 수 있다. 즉, 위치 및 자세 계산 시스템(200)은 이미 알고 있는 자세 정보인 자세 변환 행렬을 이용하여 가속도 센서의 바이어스를 계산할 수 있다.

우선, 항법 좌표계의 축은 일례로, 도 4와 같이 정의 될 수 있다. 도 4는 본 발명의 일실시예에 따른 위치 및 자세 계산 시스템에서 이용 가능한 항법 좌표계의 축을 나타낸 일례이다.

초기 출발 시 차량의 자세가 롤(ф), 피치(θ), 요(ψ)를 통해 표현될 때, 자세 변환 행렬은 상술한 수학식 1과 같이 표현될 수 있다.

또한, 정지 상태에서는 중력 가속도 이외의 별도의 가속도 성분은 없기 때문에 항법 좌표계에서 이동체의 세 축에 대한 가속도 성분은 상술한 수학식 2와 같은 행렬로 표현될 수 있다.

이때, 이동체에 대한 좌표계에서 가속도 성분은 상술한 수학식 3과 같이 표현될 수 있다.

3축에 대한 환산 계수를 각각 "Asfx", "Asfy", "Asfz"라 하고, 가속도 센서의 ADC(Analog to Digital Converter) 출력을 각각 "ADCx", "ADCy" 및 "ADCz" 그리고 초기 바이어스를 "Biasx", "Biasy" 및 "Biasz"라고 했을 때, 초기 바이어스는 상술한 수학식 4와 같이 표현될 수 있다.

이때, ADC출력을 "m/s²"의 가속도 출력의 단위로 나타내면 초기 바이어스는 상술한 수학식 5와 같이 표현될 수도 있다.

여기서, "Accx", "Accy" 및 "Accz"는 가속도 센서를 통해 출력되는 센서값을 의미할 수 있다. 이때, 상기 이동체의 GPS 수신부로부터 수신되는 속도 정보, 이전 시간에 상기 속도값 계산부 계산된 자세값 및 속도값에 기초하여 상기 오차를 각각 추정할 수도 있다. 이때, 위치 및 자세 계산 시스템(200)은 속도계로부터 수신되는 속도 정보를 더 이용하여 자세값 및 속도값 각각의 오차를 추정할 수 있다. 이때, 위치 및 자세 계산 시스템(200)은 속도계의 환산 계수에 대한 오차 또한 추정하여 속도계로 전송할 수 있다.

도 5는 본 발명의 일실시예에 따른 위치 및 자세 계산 시스템에서 가속도 센서의 바이어스를 계산하는 과정을 도시한 일례이다. 본 실시예에 따른 위치 및 자세 계산 시스템이 가속도 센서(510)와 자세값 계산부(520)를 포함한다고 가정하면, 가속도 센서(510)의 출력(530)에서 자세값 계산부(520)에서 계산된 자세 변환 행렬(540)과 상술한 수학식 2와 같이 표현된 중력가속도(550)의 행렬의 곱셈 연산 결과를 빼줌으로써, 가속도 센서(510)의 바이어스(560)를 계산할 수 있게 된다. 이때, 위치 및 자세 계산 시스템은 종료 시 마지막 자세값을 메모리에 저장하고, 최초 동작 시 메모리에 저장된 마지막 자세값을 통해 계산된 자세 변환 행렬(540)을 통해 가속도 센서(510)의 바이어스(560)를 계산할 수 있다.

이와 같이, 본 발명의 일실시예에 따른 위치 및 자세 계산 방법을 이용하면, 초기 동작 시, 차량의 자세에 기초하여 계산된 자세 변환 행렬과 정지 상태의 이동체에 작용하는 중력가속도 그리고 가속도 센서의 출력인 센서값에 기초하여 가속도 센서의 바이어스를 추정할 수 있다.

도 6은 본 발명의 다른 실시예에 따른 위치 및 자세 계산 방법을 도시한 흐름도이다. 본 실시예에 따른 위치 및 자세 계산 방법은 도 2를 통해 설명한 위치 및 자세 계산 시스템(200) 중 두 번째 실시예에 따른 위치 및 자세 계산 시스 템(200)를 통해 수행될 수 있다. 도 6에서는 위치 및 자세 계산 시스템(200)을 통해 각각의 단계가 수행되는 과정을 설명함으로써, 위치 및 자세 계산 방법을 설명한다.

단계(310)에서 위치 및 자세 계산 시스템(200)은 자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값 및 자세 변환 행렬을 계산한다. 이때, 자세값은 이동체의 롤, 피치 및 요 중 적어도 하나에 대한 측정값을 포함할 수 있고, 자세 변환 행렬은 측정값을 각도로 하는 삼각함수에 기초하여 계산될 수 있다.

단계(320)에서 위치 및 자세 계산 시스템(200)은 GPS 수신기로부터 수신된 속도 정보 및 자세 정보, 상기 자세값 및 상기 속도값에 기초하여 상기 자이로 센서와 상기 가속도 센서의 바이어스, 상기 자세값 및 상기 속도값 각각의 오차를 추정한다. 즉, 상술한 초기 동작 이후, GPS가 정상적으로 포지션 픽스(Position Fix) 상태에 들어갔을 때, GPS 수신기에서 제공하는 속도 정보와 자세 정보가 위치 및 자세 계산 시스템(200)의 자세값 및 속도값 계산을 위한 측정값으로서 이용될 수 있다. 이때, 속도 정보는 상기 GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환함으로써 계산되고, 상기 자세 정보는 상기 변환된 속도간의 연산을 통해 계산된다.

GPS 수신기에서 출력된 속도 정보를 "Vx", "Vy" 및 "Vz" 라고 하면, 속도 정보는 경도와 위도의 위치 정보를 이용하여 항법 좌표계의 값으로 변환하여 상술한 수학식 6 및 7과 같이 표현될 수 있다.

항법 좌표계의 속도가 상술한 수학식 6에서와 같이 "Vn", "Ve" 및 "Vd"라 할 때, GPS를 이용한 자세 정보는 상술한 수학식 8과 같이 계산될 수 있다.

위치 및 자세 계산 시스템(200)은 수학식 8과 같이 계산된 자세 정보와 속도 정보, 자이로 센서와 가속도 센서의 센서값을 통해 이전 시간에 계산된 자세값 및 속도값을 이용하여 (1) 현재 시간의 자세값과 속도값 각각의 오차 그리고 (2)자이로 센서와 가속도 센서의 바이어스의 각각의 오차를 추정하여 보상하게 된다.

이때, 위치 및 자세 계산 시스템(200)은 속도계로부터 수신되는 속도 정보를 더 이용하여 자세값 및 속도값 각각의 오차를 추정할 수 있다. 이때, 위치 및 자세 계산 시스템(200)은 속도계의 환산 계수에 대한 오차 또한 추정하여 속도계로 전송할 수 있다.

또한, 위치 및 자세 계산 시스템(200)은 자세값 및 속도값에 기초하여 이동체의 위치를 계산할 수 있고, 자세값, 속도값 및 위치를 이용하여 이동체의 자세, 속도 및 위치 중 적어도 하나의 정보를 제공할 수 있다. 따라서, GPS 정보를 수신할 수 없는 경우에도, 이동체의 위치, 속도, 자세와 같은 정보를 사용자에게 제공 할 수 있게 된다.

이와 같이, 본 발명의 다른 실시예에 따른 위치 및 자세 계산 방법을 이용하면, GPS 수신기가 정상적으로 포지션 픽스(position fix)되었을 때, GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환하고, 변환된 속도간의 연산을 통해 계산함으로써, 이러한 속도 정보와 자세 정보를 통해 자이로 센서와 가속도 센서의 바이어스, 자세값 및 속도값 각각의 오차보다 보다 정확하게 추정할 수 있다. 또한, 본 발명의 일실시예에 따른 위치 및 자세 계산 방법을 이용하면, 이동체의 위치, 속도 및 가속도 등의 항법 정보와 기본적인 센서 출력 이외에도 자체적인 자세계산을 통해 자세정보를 출력함으로써, 일반적인 접촉사고나 추돌 사고 외에 차량과 같은 이동체의 전복이나 미끄럼 길 등에서 급격히 자세가 불안하여 유발되는 사고에 대해서도 이동체의 자세정보를 통해 사고 분석이나 재현, 상황 재구성 등에 유용한 정보로 활용할 수 있다. 뿐만 아니라 GPS 수신기가 동작하지 않거나 GPS 수신기로부터 정보를 수신할 수 없는 경우에도 자이로 센서, 가속도 센서 및 속도계를 통해 지속적으로 속도와 자세가 계산 및 출력되고, 이를 이용하여 위치를 계산할 수 있기 때문에 GPS 정보가 수신되지 않는 지하주차장이나 터널 등에서도 지속적으로 항법해를 제공할 수 있다.

본 발명에 따른 실시예들은 다양한 컴퓨터 수단을 통하여 수행될 수 있는 프로그램 명령 형태로 구현되어 컴퓨터 판독 가능 매체에 기록될 수 있다. 컴퓨터 판독 가능 매체는 프로그램 명령, 파일 데이터, 데이터 구조 등을 단독으로 또는 조합하여 포함할 수 있다. 매체에 기록되는 프로그램 명령은 본 발명을 위하여 특별히 설계되고 구성된 것들이거나 컴퓨터 소프트웨어 당업자에게 공지되어 사용 가능한 것일 수도 있다. 컴퓨터 판독 가능 기록 매체의 예에는 하드 디스크, 플로피 디스크 및 자기 테이프와 같은 자기 매체(magnetic media), CD-ROM, DVD와 같은 광기록 매체(optical media), 플롭티컬 디스크(Floptical disk)와 같은 자기-광 매체(magneto-optical media), 및 롬(ROM), 램(RAM), 플래시 메모리 등과 같은 프로그램 명령을 저장하고 수행하도록 특별히 구성된 하드웨어 장치가 포함된다. 프로그램 명령의 예에는 컴파일러에 의해 만들어지는 것과 같은 기계어 코드뿐만 아니라 인터프리터 등을 사용해서 컴퓨터에 의해서 실행될 수 있는 고급 언어 코드를 포함한다. 하드웨어 장치는 본 발명의 동작을 수행하기 위해 하나 이상의 소프트웨어 모듈로서 작동하도록 구성될 수 있으며, 그 역도 마찬가지이다.

이상과 같이 본 발명에서는 구체적인 구성 요소 등과 같은 특정 사항들과 한정된 실시예 및 도면에 의해 설명되었으나 이는 본 발명의 보다 전반적인 이해를 돕기 위해서 제공된 것일 뿐, 본 발명은의 실시예에 한정되는 것은 아니며, 본 발명이 속하는 분야에서 통상적인 지식을 가진 자라면 이러한 기재로부터 다양한 수정 및 변형이 가능하다.

따라서, 본 발명의 사상은 설명된 실시예에 국한되어 정해져서는 아니되며, 후술하는 특허청구범위뿐 아니라 이 특허청구범위와 균등하거나 등가적 변형이 있는 모든 것들은 본 발명 사상의 범주에 속한다고 할 것이다.

도 1은 본 발명의 일실시예에 따른 위치 및 자세 계산 시스템의 전체적인 모습을 도시한 일례이다.

도 2는 본 발명의 일실시예에 있어서, 위치 및 자세 계산 시스템의 내부 구성을 설명하기 위한 블록도이다.

도 3은 본 발명의 일실시예에 있어서, 위치 및 자세 계산 방법을 도시한 흐름도이다.

도 4는 본 발명의 일실시예에 따른 위치 및 자세 계산 시스템에서 이용 가능한 항법 좌표계의 축을 나타낸 일례이다.

도 5는 본 발명의 일실시예에 따른 위치 및 자세 계산 시스템에서 가속도 센서의 바이어스를 계산하는 과정을 도시한 일례이다.

도 6은 본 발명의 다른 실시예에 따른 위치 및 자세 계산 방법을 도시한 흐름도이다.

<도면의 주요 부분에 대한 부호의 설명>

110: 위치 및 자세 계산 시스템

111: 자이로 센서

112: 가속도 센서

113: 자세값 계산부

114: 속도값 계산부

115: 필터부

120: 속도계

130: GPS 수신기

Claims

자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값 및 자세 변환 행렬을 계산하는 자세값 계산부;

가속도 센서로부터 수신된 센서값에 기초하여 상기 이동체의 속도값을 계산하는 속도값 계산부; 및

상기 자이로 센서와 상기 가속도 센서의 바이어스, 상기 자세값 및 상기 속도값 각각의 오차를 추정하는 필터부

를 포함하고,

상기 필터부는,

초기 동작 시, 상기 자세 변환 행렬, 중력 가속도 및 상기 가속도 센서의 센서값에 기초하여 상기 가속도 센서의 바이어스를 추정하는, 위치 및 자세 계산 시스템.
제1항에 있어서,

상기 필터부는,

상기 이동체의 속도계로부터 수신되는 속도 정보, 이전 시간에 측정된 자세값 및 속도값에 기초하여 상기 오차 및 상기 속도계의 환산 계수의 오차를 각각 추정하는, 위치 및 자세 계산 시스템.
제1항에 있어서,

상기 필터부는,

상기 이동체의 GPS 수신부로부터 수신되는 속도 정보, 이전 시간에 상기 속도값 계산부 계산된 자세값 및 속도값에 기초하여 상기 오차를 각각 추정하는, 위치 및 자세 계산 시스템.
제1항에 있어서,

상기 자세값은 상기 이동체의 롤(roll), 피치(pitch) 및 요(yaw) 중 적어도 하나에 대한 측정값을 포함하는, 위치 및 자세 계산 시스템.
제4항에 있어서,

상기 자세 변환 행렬은 상기 측정값을 각도로 하는 삼각함수에 기초하여 계산되는, 위치 및 자세 계산 시스템.
제1항에 있어서,

상기 자세값 및 상기 속도값에 기초하여 상기 이동체의 위치를 계산하는 위치 계산부

를 더 포함하는, 위치 및 자세 계산 시스템.
제6항에 있어서,

상기 자세값, 상기 속도값 및 상기 위치를 이용하여 상기 이동체의 자세, 속도 및 위치 중 적어도 하나의 정보를 제공하는, 위치 및 자세 계산 시스템.
제1항에 있어서,

상기 자세값 계산부는,

상기 이동체의 속도계로부터 수신되는 속도 정보를 더 이용하여 상기 자세값 및 상기 자세 변환 행렬을 계산하는, 위치 및 자세 계산 시스템.
자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값을 계산하는 자세값 계산부;

가속도 센서로부터 수신된 센서값에 기초하여 상기 이동체의 속도값을 계산하는 속도값 계산부; 및

GPS 수신기로부터 수신된 속도 정보 및 자세 정보, 상기 자세값 및 상기 속도값에 기초하여 상기 자이로 센서와 상기 가속도 센서의 바이어스, 상기 자세값 및 상기 속도값 각각의 오차를 추정하는 필터부

를 포함하고,

상기 속도 정보는 상기 GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환함으로써 계산되고,

상기 자세 정보는 상기 변환된 속도간의 연산을 통해 계산되는, 위치 및 자 세 계산 시스템.
제9항에 있어서,

상기 필터부는,

상기 이동체의 속도계로부터 수신되는 속도 정보, 이전 시간에 계산된 자세값 및 속도값에 기초하여 상기 오차 및 상기 속도계의 환산 계수의 오차를 추정하는, 위치 및 자세 계산 시스템.
제9항에 있어서,

상기 자세값은 상기 이동체의 롤, 피치 및 요 중 적어도 하나에 대한 측정값을 포함하고,

상기 자세 변환 행렬은 상기 측정값을 각도로 하는 삼각함수에 기초하여 계산되는, 위치 및 자세 계산 시스템.
자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값 및 자세 변환 행렬을 계산하는 단계;

가속도 센서로부터 수신된 센서값에 기초하여 상기 이동체의 속도값을 계산하는 단계; 및

상기 자이로 센서와 상기 가속도 센서의 바이어스, 상기 자세값 및 상기 속도값 각각의 오차를 추정하는 단계

를 포함하고,

상기 오차를 추정하는 단계는,

초기 동작 시 상기 자세 변환 행렬, 중력 가속도 및 상기 가속도 센서의 센서값에 기초하여 상기 가속도 센서의 바이어스를 계산하는 단계를 포함하는, 위치 및 자세 계산 방법.
자이로 센서로부터 수신된 센서값에 기초하여 이동체의 자세값을 계산하는 단계;

가속도 센서로부터 수신된 센서값에 기초하여 상기 이동체의 속도값을 계산하는 단계; 및

GPS 수신기로부터 수신된 속도 정보 및 자세 정보, 상기 자세값 및 상기 속도값에 기초하여 상기 자이로 센서와 상기 가속도 센서의 바이어스, 상기 자세값 및 상기 속도값 각각의 오차를 추정하는 단계

를 포함하고,

상기 속도 정보는 상기 GPS 수신기에서 출력된 3개의 축에 따른 속도 결과를 경도 및 위도의 위치 정보를 통해 각각 항법 좌표계의 속도로 변환함으로써 계산되고,

상기 자세 정보는 상기 변환된 속도간의 연산을 통해 계산되는, 위치 및 자세 계산 방법.