KR20120042394A - 주행 장치를 위한 위치측정 시스템 및 방법 - Google Patents

Abstract

본 발명은 미끄러짐 현상과 같이 알 수 없는 오차에 의한 위치측정 오차를 엔코더와 자이로, 가속도계를 통해 보정하여 위치 오차를 최소화 할 수 있는 주행 장치를 위한 위치측정 시스템 및 방법에 관한 것으로, 그 구성은 구동 바퀴들의 회전각을 측정하는 엔코더들;회전 관성을 측정하는 자이로들;중력 가속도를 측정하는 가속도계들;상기 엔코더와 주행 장치의 기구학을 통해 계산된 각속도와 자이로의 각속도를 이용하여 각속도를 보상하는 각속도 보상부;상기 보상된 각속도와 엔코더와 주행 장치의 기구학을 통해 계산된 선속도를 이용하여 보상하는 선속도 보상부; 및 상기 계산된 각속도, 선속도와 가속도계를 통해 계산된 각속도, 선속도를 이용해 자세를 보상하는 자세 보상부;를 포함한다.

Description

주행 장치를 위한 위치측정 시스템 및 방법{System and Method for localizationing of Autonomous Vehicle}

본 발명은 주행 장치의 위치 측정에 관한 것으로, 구체적으로 미끄러짐 현상과 같이 알 수 없는 오차에 의한 위치측정 오차를 엔코더와 자이로, 가속도계를 통해 보정하여 위치 오차를 최소화 할 수 있는 주행 장치를 위한 위치측정 시스템 및 방법에 관한 것이다.

자율적으로 이동할 수 있도록 구성된 주행 장치에 있어서 효율적인 제어 및 자기 위치 인식을 위해서는 정확한 위치 추정이 필요하다.

이와 같은 주행 장치의 위치 추정을 위해 사용되는 가장 보편적인 방법은 주행 장치의 구동 바퀴에 장착된 엔코더를 사용하는 것이다.

구동 바퀴에 장착된 엔코더는 바퀴의 회전수를 계수하여 회전수에 따라 주행 거리를 추정할 수 있고, 바퀴를 두 개 이상 사용하여 사용 가능한 엔코더 데이터가 두 개 이상인 경우, 각 엔코더 값의 차를 계산하여 회전 각속도 및 회전 각도 추정이 가능하다.

그러나 엔코더를 이용한 위치 추정은 구동 바퀴와 맞닿은 바닥면의 미끄러짐에 의한 슬립(slip)이 발생하는 경우 실제 움직인 거리와 엔코더에 의한 추정 거리가 상이한 값을 가지는 단점이 있다. 슬립은 미끄러운 바닥, 카펫, 문턱을 넘는 경우, 충돌에 의한 경우 등 다양한 원인으로 인하여 발생한다.

이와 같이 종래 기술에서의 위치측정 방법은 기본적으로 엔코더를 이용하여 바퀴의 회전각을 측정하고 측정된 바퀴 회전각과 주행 장치의 기구학을 통해 위치를 추정하는 방법이다.

하지만, 이와 같은 엔코더를 이용하는 주행 장치들은 주행 환경이나 시스템 특성에 의한 누적 오차가 발생된다.

이와 같은 문제를 해결하기 위하여 확장형 칼만 필터 등과 같은 확률적인 오차 보정 방법과 절대 위치좌표를 계측할 수 있는 전역 위치측정 장치, 환경인지 방법 등을 이용하는 방법에 관해 많은 연구가 이루어졌다.

하지만, 확률적인 오차 보정을 하더라도 미끄러짐 현상과 같은 알지 못하는 에러는 보정될 수 없었으며, 전역 위치측정 장치를 이용하는 방법은 가격이 매우 높아지는 문제점을 가졌고, 환경인지 방법은 현 기술 상의 문제로 성능이 매우 낮았다.

이에 INS(inertial navigation system)과 같은 관성 센서를 이용하여 바퀴의 미끄러짐 현상에 의한 오차를 보정하는 방법에 관한 연구들이 수행되었다.

하지만, INS는 기본적으로 가속도계(accelerometer)를 이용하여 중력 가속도를 계산하고 가속도를 2번 적분하여 위치를 구하게 되는데, 아주 작은 오차가 적분을 두 번 하는 동안 매우 커져 위치측정 오차가 매우 크게 발생되는 문제가 있었다.

이러한 문제를 해결하기 위해서 엔코더와 INS를 함께 사용하는 방법과 주행 장치의 등속도 운동과 가감속 운동을 구별하여 측정하는 방법, 주행 장치의 떨림(idling frequency)을 이용하여 INS 오차를 최소화하는 방법 등이 연구되어 성능이 향상되었지만 여전히 오차가 크게 누적되는 문제가 존재하였다.

본 발명은 이와 같은 종래 기술의 주행 장치의 위치 측정 방법의 문제를 해결하기 위한 것으로, 미끄러짐 현상과 같이 알 수 없는 오차에 의한 위치측정 오차를 엔코더와 자이로, 가속도계를 통해 보정하여 위치 오차를 최소화 할 수 있는 주행 장치를 위한 위치측정 시스템 및 방법을 제공하는데 그 목적이 있다.

본 발명은 엔코더 순간 변화량 크기와 계산된 2개의 각속도 값에 의한 가중치 계산을 통해 각속도를 정확히 보정을 할 수 있도록 한 주행 장치를 위한 위치측정 시스템 및 방법을 제공하는데 그 목적이 있다.

본 발명은 보정된 각속도와 엔코더에 의한 선속도를 적합함수로 유전자 알고리즘을 사용하여 정확한 각속도와 선속도를 구하여 오차 누적이 심한 가속도계의 선속도와 각속도의 정밀도를 판단할 수 있도록 한 주행 장치를 위한 위치측정 시스템 및 방법을 제공하는데 그 목적이 있다.

본 발명은 미끄러짐 현상이나 바퀴가 헛도는 현상을 판단할 수 있고 이를 이용하여 칼만 필터의 계측 모델을 동적으로 변경할 수 있어 주행 장치의 정확한 자세를 계산할 수 있도록 한 주행 장치를 위한 위치측정 시스템 및 방법을 제공하는데 그 목적이 있다.

본 발명은 오차 공분산을 가변적으로 변경시킴으로써 칼만 필터의 가우시안 에러 공분산의 발산을 해결 할 수 있도록 한 주행 장치를 위한 위치측정 시스템 및 방법을 제공하는데 그 목적이 있다.

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

이와 같은 목적을 달성하기 위한 본 발명에 따른 주행 장치를 위한 위치측정 시스템은 구동 바퀴들의 회전각을 측정하는 엔코더들;회전 관성을 측정하는 자이로들;중력 가속도를 측정하는 가속도계들;상기 엔코더와 주행 장치의 기구학을 통해 계산된 각속도와 자이로의 각속도를 이용하여 각속도를 보상하는 각속도 보상부;상기 보상된 각속도와 엔코더와 주행 장치의 기구학을 통해 계산된 선속도를 이용하여 보상하는 선속도 보상부; 및 상기 계산된 각속도, 선속도와 가속도계를 통해 계산된 각속도, 선속도를 이용해 자세를 보상하는 자세 보상부;를 포함하는 것을 특징으로 한다.

그리고 상기 자세 보상부는,상기 엔코더와 자이로를 통해 보상된 각속도와 가속도계를 통해 계산된 각속도를 비교하고 엔코더의 선속도 혹은 가속도계를 통해 계산된 선속도를 선택하는 것을 특징으로 한다.

다른 목적을 달성하기 위한 본 발명에 따른 주행 장치를 위한 위치측정 시스템은 주행 장치의 구동부에 입력을 주는 구동 입력부;바퀴의 회전각을 구하기 위한 엔코더들;회전 관성을 통한 각속도를 계산하는 자이로들;중력 가속도를 통해 양 방향 가속도를 검출하는 가속도계들;상기 구동 입력부와 엔코더, 자이로들의 상태를 분석하여 주행 장치의 동작 여부를 판단하는 분석부;주행 장치가 동작하지 않고 있을 때에 자이로와 가속도계를 보정하는 자이로와 가속도계 보정부;상기 엔코더,자이로,가속도계들의 검출값을 이용하여 선속도 및 각속도를 계산하는 위치 측정값 계산부;상기 위치 측정값 계산부의 계산값을 이용하여 각속도를 보상하고 보상된 각속도를 이용하여 선속도를 보상하고, 주행 장치의 자세를 보상하는 위치 측정값 보상부;를 포함하는 것을 특징으로 한다.

여기서, 상기 위치 측정값 계산부는,상기 엔코더들의 계측값과 주행 장치의 기구학을 통한 자세를 계산하는 제 1 선속도 계산부, 제 1 각속도 계산부와,상기 자이로를 통해 각속도를 계산 하는 제 3 각속도 계산부와,상기 가속도계의 기울기에 따라 가속도계를 보상하는 가속도계 보상부 및 가속도계 보상부에서 보상된 x축과 y축의 중력 가속도를 통한 제 2 선속도 계산부 및 제 3 각속도 계산부를 포함하는 것을 특징으로 한다.

그리고 상기 위치 측정값 보상부는,상기 위치 측정값 계산부의 제 1 각속도 계산부에서 계산된 각속도와 제 3 각속도 계산부에서 계산된 각속도를 통해 각속도를 보상하는 각속도 보상부와,상기 위치 측정값 계산부의 제 1 선속도 계산부에서 계산된 선속도와 각속도 보상부를 통해 보상된 각속도를 이용하여 선속도를 보정하는 선속도 보상부와,상기 각속도 보상부의 각속도와 선속도 보상부의 선속도, 제 2선속도 계산부의 선속도, 제 2 각속도 계산부의 각속도를 이용하여 주행 장치의 자세를 보상하는 자세 보상부를 포함하는 것을 특징으로 한다.

또 다른 목적을 달성하기 위한 본 발명에 따른 주행 장치를 위한 위치측정 방법은 바퀴의 회전각,각속도,중력 가속도를 검출하는 엔코더,자이로,가속도계를 갖는 주행 장치의 위치측정에 있어서, 상기 엔코더의 순간 변화량 크기에 따라 제 1 가중치를 계산하는 단계;상기 엔코더와 주행 장치의 기구학을 통해 계산된 각속도와 자이로를 통해 계산된 각속도를 비교하여 제 2 가중치를 계산하는 단계;상기 제 1,2 가중치를 이용하여 각속도를 계산하는 단계;유전자 알고리즘을 통해 하나 이상의 구동 바퀴 회전값을 예측하는 단계;상기 예측된 구동 바퀴의 회전값을 통해 선속도를 계산하는 단계;상기 제 1,2 가중치를 이용하여 계산된 각속도와 가속도계를 통해 계산된 각속도를 비교하여 계측 모델을 선택하는 단계; 및 상기 제 1,2 가중치를 이용하여 계산된 각속도와 가속도계를 통해 계산된 각속도를 이용하여 계측 에러 공분산을 계산하는 단계;를 포함하는 것을 특징으로 한다.

그리고 상기 구동 바퀴 회전값을 예측하는 단계는,상기 제 1,2 가중치를 이용하여 계산된 각속도와 엔코더를 통해 계산된 선속도를 이용하여 하나 이상의 구동 바퀴 회전값을 예측하는 것을 특징으로 한다.

그리고 상기 계측 모델을 선택하는 단계는,상기 제 1,2 가중치를 이용하여 계산된 각속도와 가속도계를 통해 계산된 각속도를 비교하는 동적인 선택으로 칼만 필터의 계측 모델로 적용되는 것을 특징으로 한다.

그리고 상기 에러 공분산을 계산하는 단계에서,상기 선택된 계측 모델의 각속도

, 상기 제 1,2 가중치를 이용하여 계산된 각속도

를 통해

를 계산하고, 시간 t를 이용하여 에러 공분산을

에 의해 산출하고, 여기서,

는 엔코더의 각속도 혹은 가속도계의 가속도이고,

의 k는 시간인 것을 특징으로 한다.

또 다른 목적을 달성하기 위한 본 발명에 따른 주행 장치를 위한 위치측정 방법은 바퀴의 회전각,각속도,중력 가속도를 검출하는 엔코더,자이로,가속도계를 갖는 주행 장치의 위치측정에 있어서, 상기 주행 장치가 주행하고 있지 않을 때에 각속도계와 자이로의 중심값을 보정하는 단계;상기 주행 장치가 주행하고 있을 때에 엔코더를 계측 받아 선속도와 각속도를 계산하는 단계;상기 자이로,지자계와 함께 칼만 필터를 이용하여 각속도를 계산하는 단계;상기 엔코더 순간 변화량 크기와 계산된 2개의 각속도 값에 따라 가중치를 계산하는 단계;상기 계산된 가중치를 이용하여 각속도를 보정하는 단계;유전자 알고리즘을 통해 선속도를 보정하는 단계;상기 가속도계에서 x축과 y축의 각속도를 측정하고, 가속도계의 기울기에 따라 각속도 값을 보정하고, 보정된 각속도 값을 이용하여 선속도와 각속도를 계산하는 단계;상기 자이로,지자계와 함께 칼만 필터를 이용한 각속도와의 차이에 따라 칼만 필터의 계측 모델을 선택하는 단계;상기 자이로,지자계와 함께 칼만 필터를 이용한 각속도와의 차이에 따라 칼만 필터의 계측 에러 공분산 모델을 수정하는 단계; 및 자세를 계산하는 단계;를 포함하는 것을 특징으로 한다.

이와 같은 본 발명에 따른 주행 장치를 위한 위치측정 시스템 및 방법은 다음과 같은 효과를 갖는다.

첫째, 엔코더 순간 변화량 크기와 계산된 2개의 각속도 값에 의한 가중치 계산을 통해 각속도를 정확히 보정을 할 수 있다.

둘째, 보정된 각속도와 엔코더에 의한 선속도를 적합함수로 유전자 알고리즘을 사용하여 정확한 각속도와 선속도를 구할 수 있다.

셋째, 오차 누적이 심한 가속도계의 선속도와 각속도의 정밀도를 판단할 수 있다.

넷째, 미끄러짐 현상이나 바퀴가 헛도는 현상을 판단할 수 있고 이를 이용하여 칼만 필터의 계측 모델을 동적으로 변경할 수 있어 주행 장치의 정확한 자세를 계산할 수 있다.

다섯째, 오차 공분산을 가변적으로 변경시킴으로써 칼만 필터의 가우시안 에러 공분산의 발산을 해결 할 수 있다.

도 1은 본 발명의 실시예에 따른 방향 주행 장치의 위치측정 시스템의 구성도
도 2는 전방향 주행 장치의 구성도
도 3은 본 발명에 따른 주행 장치를 위한 위치측정 시스템의 구성 블록도
도 4는 본 발명에 따른 주행 장치를 위한 위치측정 시스템의 위치측정 과정을 나타낸 구성도

이하, 본 발명에 따른 주행 장치를 위한 위치측정 시스템 및 방법의 바람직한 실시예에 관하여 상세히 설명하면 다음과 같다.

본 발명에 따른 주행 장치를 위한 위치측정 시스템 및 방법의 특징 및 이점들은 이하에서의 각 실시예에 대한 상세한 설명을 통해 명백해질 것이다.

도 1은 본 발명의 실시예에 따른 방향 주행 장치의 위치측정 시스템의 구성도이고, 도 2는 전방향 주행 장치의 구성도이다.

본 발명은 미끄러짐 현상과 같이 알 수 없는 오차에 의한 위치측정 오차를 엔코더와 자이로, 가속도계를 통해 보정하여 위치 오차를 최소화 할 수 있도록 한 것이다.

본 발명의 실시예에 따른 전방향 주행 장치의 위치측정 시스템은 도 1 및 도 2에서와 같이 자율주행 장치에 부착된 4개의 구동부(1)와, 4개의 엔코더(2), 2개의 자이로와 지자계(3), 4개의 가속도계(4)로 구성된다.

이와 같은 본 발명에 따른 주행 장치를 위한 위치측정 시스템의 상세 구성은 다음과 같다.

도 3은 본 발명에 따른 주행 장치를 위한 위치측정 시스템의 구성 블록도이다.

먼저, 자율주행 장치의 구동부에 입력을 주는 구동 입력부(100)와, 바퀴의 회전각을 구하기 위한 하나 이상의 엔코더(2)와, 회전 관성을 통한 각속도를 계산하는 하나 이상의 자이로(3)와, 중력 가속도를 통해 양 방향 가속도를 검출하는 하나 이상의 가속도계(4)를 포함한다.

그리고 하나 이상의 구동부의 입력부와 엔코더, 자이로의 상태를 분석하여 주행 장치의 동작 여부를 판단하는 분석부(101)와, 주행 장치가 동작하지 않고 있을 때에 자이로와 가속도계를 보정하는 자이로와 가속도계 보정부(102)를 포함한다.

그리고 엔코더(2),자이로(3),가속도계(4)의 검출값을 이용하여 선속도 및 각속도를 계산하는 위치 측정값 계산부(200)와 위치 측정값 계산부(200)의 계산값을 이용하여 각속도를 보상하고 보상된 각속도를 이용하여 선속도를 보상하고, 주행 장치의 자세를 보상하는 위치 측정값 보상부(300)를 포함한다.

여기서, 위치 측정값 계산부(200)는 엔코더(2)의 계측값과 주행 장치의 기구학을 통한 자세를 계산하는 제 1 선속도 계산부(103)와 제 1 각속도 계산부(104)와, 자이로(3)를 통해 각속도를 계산 하는 제 3 각속도 계산부(105)와, 가속도계(4)의 기울기에 따라 가속도계(4)를 보상하는 가속도계 보상부(108) 및 가속도계 보상부(108)에서 보상된 x축과 y축의 중력 가속도를 통한 제 2 선속도 계산부(109) 및 제 3 각속도 계산부(110)를 포함한다.

그리고 위치 측정값 보상부(300)는 제 1 각속도 계산부(104)에서 계산된 각속도와 제 3 각속도 계산부(105)에서 계산된 각속도를 통해 각속도를 보상하는 각속도 보상부(106)와, 제 1 선속도 계산부(103)에서 계산된 선속도와 각속도 보상부(106)를 통해 보상된 각속도를 이용하여 선속도를 보정하는 선속도 보상부(107)와, 각속도 보상부(106)의 각속도와 선속도 보상부(107)의 선속도, 제 2선속도 계산부(109)의 선속도, 제 2 각속도 계산부(110)의 각속도를 이용하여 주행 장치의 자세를 보상하는 자세 보상부(111)를 포함한다.

이와 같은 구성을 갖는 본 발명에 따른 주행 장치를 위한 위치측정 시스템은 구동 입력부(100)에서 자율주행 장치의 구동부(1)에 입력을 주면 주행 장치가 작동을 하게 된다.

주행 장치가 작동하게 되면, 바퀴의 회전각을 구하는 엔코더(2)와 회전 관성을 통한 각속도를 자이로(3)에서 계측하고 분석부(101)에서 주행 장치의 주행 여부를 분석하게 된다.

이때, 주행 장치가 주행하고 있지 않다고 분석부(101)에서 판단되면 자이로와 가속도계 보정부(102)에서 드래프트 오차를 해결하기 위해 자이로(3)와 가속도계(4)의 중심값을 계산하고 리셋을 시킨다.

만약, 주행 장치의 주행이 시작되었다고 분석부(101)에서 판단되면 엔코더(2)와 자이로와 지자계(3), 가속도계(4)를 통해 위치측정을 시작한다.

제 1 선속도 계산부(103)와 제 1 각속도 계산부(104)에서는 계측된 엔코더(2) 값과 주행 장치의 기구학을 통해 선속도와 각속도를 각각 계산한다.

그리고 제 3 각속도 계산부(105)에서는 자이로(3) 혹은 자이로+지자계(3)의 센서융합을 통해 각속도를 계산한다.

그리고 제 1 각속도 계산부(104)의 각속도와 제 3 각속도 계산부(105)의 각속도는 각속도 보상부(106)에서 엔코더 순간 변화량 크기와 계산된 2개의 각속도 값에 따른 가중치에 따라 각속도를 보상한다.

각속도 보상부(106)에서 보상된 각속도는 제 1선속도 계산부(103)의 선속도와 함께 선속도 보상부(107)에서 유전자 알고리즘의 적합 함수(fitness function)으로 사용하여 각 구동부의 최적 회전값을 예측하게 되고 선속도를 보상하게 된다.

가속도계(4)는 그 것의 기울기에 따른 보상을 가속계 보상부(108)에서 보상받게 되고, 보상된 가속도는 제 2 선속도 계산부(109)와 제 2 각속도 계산부(110)에서 선속도와 각속도를 각각 계산하게 된다.

이때, 각속도 보상부(106)의 각속도와 제 2 각속도 계산부(110)의 각속도는 자세 보상부(111)에서 분석되어 칼만 필터(Kalman filter)의 계측 모델로 선택되고 주행 장치의 자세가 산출된다.

이와 같은 본 발명에 따른 주행 장치를 위한 위치측정 시스템의 주행 장치 위치 측정 방법은 다음과 같다.

도 4는 본 발명에 따른 주행 장치를 위한 위치측정 시스템의 위치측정 과정을 나타낸 구성도이다.

본 발명의 실시예에 따른 주행 장치 위치 측정은 도 4에서와 같이, 엔코더(2)와 자이로(3)를 측정하여(S201) 주행 여부를 판단하고(S202) 주행을 하지 않고 있다고 판단되면 자이로(3)와 가속도계(4)를 중심값 보정하고 리셋하게 된다.(S203)

주행 여부 판단시에 주행이 시작되었다면, 구동부의 엔코더(2)에서 바퀴 회전 값을 측정하여(S204) 주행 장치의 기구학과 엔코더 값들을 통해 주행 장치의 선속도 계산(S205)과 주행 장치의 기구학과 엔코더 값들을 통해 주행 장치의 각속도 계산을 한다.(S206)

일 실시예에 따른 주행 장치가 4개의 구동부를 가지므로 엔코더의 선속도 계산(S205)와 각속도 계산(S206)은 각각 수학식 1과 수학식 2와 같이 구해진다.

수학식 1에서

는 바퀴의 지름을 의미하며, 수학식 2에서 과

는 각각 주행 장치의 중심으로부터 엔코더의 x축과 y축의 거리를 의미한다.

다음으로, 자이로 2개의 각속도 측정(S207)과 지자계의 방위각 측정(S208)을 하게 되고, 이를 통해 각속도와 방위각이 계산되고 이들은 칼만 필터를 통해 각속도 계산 센서 융합하게 되어 각속도를 계산하게 된다.(S209)

자이로 또는 자이로와 지자계의 센서융합(S207)(S209)을 통해 각속도가 계산되면, 엔코더의 순간 변화량 크기에 따라 가중치를 계산하게 된다.(S210)

이는, 엔코더(2)의 순간 변화량이 급격하게 커지면 미끄러짐 현상이 일어날 확률이 크게 되기 때문에 이를 이용하여 자이로의 각속도(S209)와 엔코더의 각속도의 가중치를 결정하도록 하는 것이다.(S206)

즉, 자이로(3)는 드래프트 현상으로 오차가 누적되기 때문에 짧은 시간 동안의 계측 정밀도는 높지만 누적될수록 정밀도가 크게 낮아지는 문제를 가지기 때문에 지자계와의 센서융합(S209) 단계를 통해 누적 오차를 최소화하고 미끄러짐과 같은 알 수 없는 오차가 없을 때에는 엔코더(2)의 정밀도가 매우 높기 때문에 한 번 더 각속도 오차를 줄이도록 하기 위해서 이다.

엔코더의 순간 변화량 크기

에 따른 가중치 계산(S210)은 주행 장치의 시스템과 연관을 가지게 되고 다음과 같이 계산되어진다.

상기 수학식3에서

은 엔코더의 순간 변화량 크기에 따른 가중치(S210)을 의미하고,

는 바퀴의 회전각을 의미한다.

본 발명의 일 실시예에 따른 주행 장치는 4개의 구동 바퀴와 100ms 반응속도, 20cm의 바퀴 지름, 480pulse의 엔코더 분해능 갖는다.

이때, 순간 변화량 크기에 따른 가중치

는 현재

시간에서

시간까지를 비교하여 최소 변화량

과 최대 변화량

은 각각 0.65cm/s, 1.95cm/s로 하는 것이 바람직하다.

최소 변화량과 최대 변화량은 주행 장치의 질량 부피, 구동부 특성 등의 영향을 많이 받기 때문에 최소 변화량과 최대 변화량은 여러조건의 시뮬레이션을 통해 최적의 값을 찾는 것이 바람직하다.

또한, 구동 바퀴의 개수만큼 가중치를 구하게 되고 구동 바퀴 개수로 나뉘어 사용한다.

다음으로, 계산된 2개의 각속도 값에 따라 가중치 계산(S211)은 엔코더의 각속도(S206)와 자이로의 각속도(S209)를 비교하여 알 수 없는 에러에 의한 오차가 크게 발생될 수 있는 엔코더의 각속도(S206)를 찾아내기 위한 것이다.

이는, 엔코더의 각속도(S206)와 자이로의 각속도(S209) 차를 구하여 그에 따라 가중치

를 구하는 것이고 이는 다음과 같이 구해진다.

상기 수학식4에서

는 계산된 2개의 각속도 값에 따라 가중치(S211)를 의미하고,

와

는 각각 엔코더의 각속도(S206)과 자이로의 각속도(S209)를 나타낸다.

본 발명에 따른 주행 장치는 100ms의 반응속도와 평균 주행 속도가 10cm/s를 가질 때에

과

는 각각 0.5°/s과 1.2°/s로 두었다.

상기에서와 같이, 자이로(3)는 드래프트 현상으로 오차 누적이 있어 시간이 지나면 오차가 크게 누적되지만 외란에 영향이 거의 없기 때문에 짧은 시간 동안의 계측은 정밀도가 높다.

이는, 작은 시간 동안에 계측 받은 엔코더의 각속도(S206)와 자이로의 각속도(S209)의 차이가 많이 나지 않는다면 정밀도가 높은 엔코더(2)가 알 수 없는 오차를 받지 않아 더 높은 정밀도를 가지고 있다는 것을 알 수 있다.

하지만, 엔코더의 각속도(S206)와 자이로의 각속도(S209)의 차가 크다면 엔코더는 알 수 없는 오차에 의해 정밀도가 낮다는 것을 알 수 있다.

가중치를 통해 각속도 계산(S212)는 엔코더의 순간 변화량 크기에 따른 가중치

(S210)와 계산된 2개의 각속도 값에 따른 가중치

(S211) 계산되게 되면 다음 식과 같이 구한다.

다음으로, 유전자 알고리즘(genetic algorithm: GA)을 통해 바퀴의 최적 회전 값을 예측(S213) 단계는 가중치를 통해 계산된 각속도(S212)와 엔코더의 선속도(S205)를 이용하게 된다.

이는 엔코더의 각속도(S206)가 틀려진다는 것은 선속도(S205)가 잘못되었다는 것을 의미하기 때문에 최적 기법인 유전자 알고리즘을 이용하여 선속도를 예측하는 것이다.

유전자 알고리즘은 자연 진화의 과정을 관찰하여 개발된 최적화 알고리즘으로, 이진 스트링으로 코딩된 집단(population)으로 구성되고 유전자 연산자인 교배(crossover)와 돌연변이(mutation)를 이용해 재생산(reproduction)을 통해 최적의 해를 찾는 최적 기법이다.

여기서, 유전자 알고리즘을 통해 바퀴의 최적 회전 값을 예측(S213) 단계에서 유전자 알고리즘의 집단은 구동 바퀴의 회전각이 되고 적합 함수(fitness function)은 다음과 같다.

집단 개수(population size)와 생산 개수(generation size)는 상기 가중치에서 계산된 가중치를 이용하여 가변적으로 선택되도록 한다.

유전자 알고리즘에 의해 구동 바퀴의 최적 회전값을 예측(S213)을 하게 되면 예측된 바퀴 회전값과 주행 장치의 기구학을 통해 주행 장치의 선속도를 계산하게 된다.(S214)

하지만, 유전자 알고리즘을 이용한다고 하더라도 적합 함수(S213)에 포함된 엔코더의 선속도(S205)는 알 수 없는 오차에 의해 크게 손상되어있을 수도 있기 때문에 가속도계의 정보를 통해 한 번더 보정하게 된다.

즉, 가속도계의 x축과 y축의 각속도 측정(S125)에서 가속도계를 측정하고 그것들의 기울기에 따른 각속도 값 보정(S216)을 통해 선속도와 각속도 계산(S217)을 구하게 된다.

가속도계의 x축과 y축의 각속도 측정(S125)은 다음과 같이 적분하여 속도를 구할 수 있다.

상기 수학식7에서

와

는 각각 가속도계의 x축의 가속도와 y축의 가속도를 나타낸다.

하지만, 적분을 한 번하여 속도를 구하고 두 번을 통해 위치를 계산하는 가속도계는 차체가 기울어져 있을 때 계속 오차 누적되는 문제가 발생된다. 이 문제를 해결하기 위하여 기울기에 따른 각속도 값 보정을 통해 각속도 값을 보정한다.(S216)

보정된 각속도 값을 통해 선속도

와 각속도

계산은 다음과 같이 구해진다.(S217)

여기서, 가장 신뢰도가 높은 즉, 정밀도가 가장 높은 것은 자이로의 각속도 (S209)이므로 가속계의 선속도(S217)를 사용할 것인지 엔코더의 선속도(S214)를 사용할 것인지의 판단을 자이로의 각속도(S209)를 기준으로 한다.(S218)

이때, 더욱 정밀한 위치측정을 위해 칼만 필터를 이용하게 된다.

만약, 자이로의 각속도(S209)와 엔코더의 각속도(S206)의 차가 자이로의 각속도(S209)와 가속도계의 각속도(S217)보다 더 작다면(S218), 칼만 필터의 계측 모델(process model)로 자이로의 각속도(S209)와 엔코더의 각속도(S206)로 선택(S219) 되고, 자이로의 각속도(S209)와 엔코더의 각속도(S206)의 차에 따라 계측 에러 공분산(process error covariance)을 구하여 칼만 필터를 통해 자세를 추정(S223)하게 된다.(S223)

그렇지 않다면, 칼만 필터의 계측 모델로 자이로의 각속도(S209)와 가속도계의 가속도(S217)로 선택(S219) 되고, 자이로의 각속도(S209)와 가속도계의 각속도(S217)의 차에 따라 계측 에러 공분산(process error covariance)을 구하여 칼만 필터를 통해 자세를 추정하게 된다.(S223)

이때, 계측 에러 공분산

은 다음과 같이 구한다.(S220)(S222)

상기 수학식9에서 는 엔코더의 각속도(S206) 혹은 가속도계의 가속도(S217)가 될 수 있고,

의 k는 시간을 나타낸다.

이는, a가 1보다 작거나 같을 때에 R가 고정되고 1보다 크고 k가 커짐에 따라 R의 값은 작아진다.

다음으로 칼만 필터를 통해 자세 추정 단계는 선택된 계측 모델(S218)의 계산된 선속도와 각속도를 계측모델로 두고, 관측 모델을 후부(posterior) 선속도와 각속도로 두어 자세를 추정하게 된다.(S223)

이와 같은 본 발명에 따른 주행 장치를 위한 위치측정 시스템 및 방법은 엔코더 순간 변화량 크기와 계산된 2개의 각속도 값에 의한 가중치 계산을 통해 각속도를 정확히 보정을 할 수 있다.

또한, 보정된 각속도와 엔코더에 의한 선속도를 적합함수로 유전자 알고리즘을 사용하여 정확한 각속도와 선속도를 구하여 오차 누적이 심한 가속도계의 선속도와 각속도의 정밀도를 판단할 수 있다. 따라서, 미끄러짐 현상이나 바퀴가 헛도는 현상을 판단할 수 있고 이를 이용하여 칼만 필터의 계측 모델을 동적으로 변경할 수 있어 주행 장치의 정확한 자세를 계산할 수 있다.

이상에서의 설명에서와 같이 본 발명의 본질적인 특성에서 벗어나지 않는 범위에서 변형된 형태로 본 발명이 구현되어 있음을 이해할 수 있을 것이다.

그러므로 명시된 실시 예들은 한정적인 관점이 아니라 설명적인 관점에서 고려되어야 하고, 본 발명의 범위는 전술한 설명이 아니라 특허청구 범위에 나타나 있으며, 그와 동등한 범위 내에 있는 모든 차이점은 본 발명에 포함된 것으로 해석되어야 할 것이다.

100. 구동 입력부 101.분석부
102. 자이로와 가속도계 보정부 200. 위치측정값 계산부
300. 위치측정값 보상부

Claims (10)

1. 주행 장치의 위치측정을 위한 시스템에 있어서,
   구동 바퀴들의 회전각을 측정하는 엔코더들;
   회전 관성을 측정하는 자이로들;
   중력 가속도를 측정하는 가속도계들;
   상기 엔코더와 주행 장치의 기구학을 통해 계산된 각속도와 자이로의 각속도를 이용하여 각속도를 보상하는 각속도 보상부;
   상기 보상된 각속도와 엔코더와 주행 장치의 기구학을 통해 계산된 선속도를 이용하여 보상하는 선속도 보상부; 및 상기 계산된 각속도, 선속도와 가속도계를 통해 계산된 각속도, 선속도를 이용해 자세를 보상하는 자세 보상부;를 포함하는 것을 특징으로 하는 주행 장치를 위한 위치측정 시스템.
2. 제 1 항에 있어서, 상기 자세 보상부는,
   상기 엔코더와 자이로를 통해 보상된 각속도와 가속도계를 통해 계산된 각속도를 비교하고 엔코더의 선속도 혹은 가속도계를 통해 계산된 선속도를 선택하는 것을 특징으로 하는 주행 장치를 위한 위치측정 시스템.
3. 주행 장치의 위치측정을 위한 시스템에 있어서,
   주행 장치의 구동부에 입력을 주는 구동 입력부;
   바퀴의 회전각을 구하기 위한 엔코더들;
   회전 관성을 통한 각속도를 계산하는 자이로들;
   중력 가속도를 통해 양 방향 가속도를 검출하는 가속도계들;
   상기 구동 입력부와 엔코더, 자이로들의 상태를 분석하여 주행 장치의 동작 여부를 판단하는 분석부;
   주행 장치가 동작하지 않고 있을 때에 자이로와 가속도계를 보정하는 자이로와 가속도계 보정부;
   상기 엔코더,자이로,가속도계들의 검출값을 이용하여 선속도 및 각속도를 계산하는 위치 측정값 계산부;
   상기 위치 측정값 계산부의 계산값을 이용하여 각속도를 보상하고 보상된 각속도를 이용하여 선속도를 보상하고, 주행 장치의 자세를 보상하는 위치 측정값 보상부;를 포함하는 것을 특징으로 하는 주행 장치를 위한 위치측정 시스템.
4. 제 3 항에 있어서, 상기 위치 측정값 계산부는,
   상기 엔코더들의 계측값과 주행 장치의 기구학을 통한 자세를 계산하는 제 1 선속도 계산부, 제 1 각속도 계산부와,
   상기 자이로를 통해 각속도를 계산 하는 제 3 각속도 계산부와,
   상기 가속도계의 기울기에 따라 가속도계를 보상하는 가속도계 보상부 및 가속도계 보상부에서 보상된 x축과 y축의 중력 가속도를 통한 제 2 선속도 계산부 및 제 3 각속도 계산부를 포함하는 것을 특징으로 하는 주행 장치를 위한 위치측정 시스템.
5. 제 3 항에 있어서, 상기 위치 측정값 보상부는,
   상기 위치 측정값 계산부의 제 1 각속도 계산부에서 계산된 각속도와 제 3 각속도 계산부에서 계산된 각속도를 통해 각속도를 보상하는 각속도 보상부와,
   상기 위치 측정값 계산부의 제 1 선속도 계산부에서 계산된 선속도와 각속도 보상부를 통해 보상된 각속도를 이용하여 선속도를 보정하는 선속도 보상부와,
   상기 각속도 보상부의 각속도와 선속도 보상부의 선속도, 제 2선속도 계산부의 선속도, 제 2 각속도 계산부의 각속도를 이용하여 주행 장치의 자세를 보상하는 자세 보상부를 포함하는 것을 특징으로 하는 주행 장치를 위한 위치측정 시스템.
6. 바퀴의 회전각,각속도,중력 가속도를 검출하는 엔코더,자이로,가속도계를 갖는 주행 장치의 위치측정에 있어서,
   상기 엔코더의 순간 변화량 크기에 따라 제 1 가중치를 계산하는 단계;
   상기 엔코더와 주행 장치의 기구학을 통해 계산된 각속도와 자이로를 통해 계산된 각속도를 비교하여 제 2 가중치를 계산하는 단계;
   상기 제 1,2 가중치를 이용하여 각속도를 계산하는 단계;
   유전자 알고리즘을 통해 하나 이상의 구동 바퀴 회전값을 예측하는 단계;
   상기 예측된 구동 바퀴의 회전값을 통해 선속도를 계산하는 단계;
   상기 제 1,2 가중치를 이용하여 계산된 각속도와 가속도계를 통해 계산된 각속도를 비교하여 계측 모델을 선택하는 단계; 및 상기 제 1,2 가중치를 이용하여 계산된 각속도와 가속도계를 통해 계산된 각속도를 이용하여 계측 에러 공분산을 계산하는 단계;를 포함하는 것을 특징으로 하는 주행 장치를 위한 위치측정 방법.
7. 제 6 항에 있어서, 상기 구동 바퀴 회전값을 예측하는 단계는,
   상기 제 1,2 가중치를 이용하여 계산된 각속도와 엔코더를 통해 계산된 선속도를 이용하여 하나 이상의 구동 바퀴 회전값을 예측하는 것을 특징으로 하는 주행 장치를 위한 위치측정 방법.
8. 제 6 항에 있어서, 상기 계측 모델을 선택하는 단계는,
   상기 제 1,2 가중치를 이용하여 계산된 각속도와 가속도계를 통해 계산된 각속도를 비교하는 동적인 선택으로 칼만 필터의 계측 모델로 적용되는 것을 특징으로 하는 주행 장치를 위한 위치측정 방법.
9. 제 6 항에 있어서, 상기 에러 공분산을 계산하는 단계에서,
   상기 선택된 계측 모델의 각속도

   

   , 상기 제 1,2 가중치를 이용하여 계산된 각속도

   

   를 통해

   

   를 계산하고, 시간 t를 이용하여 에러 공분산을

   

   에 의해 산출하고,
   여기서,

   

   는 엔코더의 각속도 혹은 가속도계의 가속도이고,

   

   의 k는 시간인 것을 특징으로 하는 주행 장치를 위한 위치측정 방법.
10. 바퀴의 회전각,각속도,중력 가속도를 검출하는 엔코더,자이로,가속도계를 갖는 주행 장치의 위치측정에 있어서,
    상기 주행 장치가 주행하고 있지 않을 때에 각속도계와 자이로의 중심값을 보정하는 단계;
    상기 주행 장치가 주행하고 있을 때에 엔코더를 계측 받아 선속도와 각속도를 계산하는 단계;
    상기 자이로,지자계와 함께 칼만 필터를 이용하여 각속도를 계산하는 단계;
    상기 엔코더 순간 변화량 크기와 계산된 2개의 각속도 값에 따라 가중치를 계산하는 단계;
    상기 계산된 가중치를 이용하여 각속도를 보정하는 단계;
    유전자 알고리즘을 통해 선속도를 보정하는 단계;
    상기 가속도계에서 x축과 y축의 각속도를 측정하고, 가속도계의 기울기에 따라 각속도 값을 보정하고, 보정된 각속도 값을 이용하여 선속도와 각속도를 계산하는 단계;
    상기 자이로,지자계와 함께 칼만 필터를 이용한 각속도와의 차이에 따라 칼만 필터의 계측 모델을 선택하는 단계;
    상기 자이로,지자계와 함께 칼만 필터를 이용한 각속도와의 차이에 따라 칼만 필터의 계측 에러 공분산 모델을 수정하는 단계; 및 자세를 계산하는 단계;를 포함하는 것을 특징으로 하는 주행 장치를 위한 위치측정 방법.

Images