KR101456576B1 - 인-파이프 로봇의 자세 추정 장치 및 방법 - Google Patents

인-파이프 로봇의 자세 추정 장치 및 방법 Download PDF

Info

Publication number
KR101456576B1
KR101456576B1 KR1020130081148A KR20130081148A KR101456576B1 KR 101456576 B1 KR101456576 B1 KR 101456576B1 KR 1020130081148 A KR1020130081148 A KR 1020130081148A KR 20130081148 A KR20130081148 A KR 20130081148A KR 101456576 B1 KR101456576 B1 KR 101456576B1
Authority
KR
South Korea
Prior art keywords
quaternion
vector
error
value
robot
Prior art date
Application number
KR1020130081148A
Other languages
English (en)
Inventor
현동준
문제권
최병선
정관성
이종환
김근호
Original Assignee
한국원자력연구원
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 한국원자력연구원 filed Critical 한국원자력연구원
Priority to KR1020130081148A priority Critical patent/KR101456576B1/ko
Application granted granted Critical
Publication of KR101456576B1 publication Critical patent/KR101456576B1/ko

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1638—Programme controls characterised by the control loop compensation for arm bending/inertia, pay load weight/inertia
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

본 발명은 로봇의 환경 상태를 측정하고, 측정값들을 반영하여 로봇의 자세를 표현하는 쿼터니언을 설정한다. 그리고 설정된 쿼터니언으로부터 추출한 중력 벡터(vector)와 기 저장된 실제 중력 벡터로부터 오차값인 에러 쿼터니언을 산출한다. 그리고 산출된 에러 쿼터니언을 자세 추정 함수에 입력하여 에러 쿼터니언의 추정 벡터를 산출한다. 그리고 추정 벡터를 이용하여 상기 쿼터니언을 보정하여 실제 환경과 측정된 상태값의 오차가 보정될 수 있도록 한다. 그리고 보정값을 추출하여 추출된 보정값을 이용하여 상기 쿼터니언을 재설정한다. 그리고 재설정된 쿼터니언에 따라 로봇의 자세를 결정한다.
이에 따라 본 발명은 로봇의 자세를 추정하는 데 있어서 짐벌락 현상이 발생하지 않도록 하면서도, 센서에서 측정된 상태값과 실제 환경의 오차에 대한 추정값을 이용하여 로봇의 자세를 보정하므로 노이즈(noise)를 추정하여 로봇의 자세를 결정할 수 있다는 효과가 있다.

Description

인-파이프 로봇의 자세 추정 장치 및 방법{ATTITUDE ESTIMATION APPARATUS AND METHOD IN IN-PIPE ROBOT}
본 발명은 인-파이프(in-pipe) 로봇(robot)에 대한 것으로, 특히 인-파이프 로봇의 자세를 추정하고 추정된 자세를 추정하기 위한 장치 및 방법에 관한 것이다.
인-파이프 로봇은 좁은 파이프 안에서 배관을 밀봉하고 절단하는 작업등을 수행하는 로봇을 말하는 것이다. 특히나 이러한 인-파이프 로봇은 원자로의 냉각재 계통 배관등 사람이 작업을 할 수 없는 곳에서 파손된 배관을 수리하거나 교체할 수 있으므로 여러 분야에서 아주 유용하게 사용된다.
인-파이프 로봇은 일반적으로 파이프 내부를 주행하면서 그 작업을 하게 된다. 따라서 인-파이프 로봇은 파이프의 설치 형태에 따라 다양한 환경을 주행하며 작업하게 된다. 그러므로 그때그때의 작업 환경에 따라 인-파이프 로봇의 자세를 적절하게 추정하는 것은 인-파이프 로봇의 설계시 가장 핵심적인 부분이 된다. 그리고 이러한 로봇의 자세 추정 알고리즘은 3축 가속도 센서와 3축 각속도 센서를 이용하여 현재 로봇의 자세를 감지하고, 감지된 결과를 이용하여 확장 칼만 필터(Extended Kalman Filter)와 같은 자세 추정 함수를 사용하는 것이 일반적이다.
그런데 이처럼 자세 추정 함수를 이용하기 위해서는 3축 가속도, 각속도 센서들로부터 측정된 값들을 좌표계로 변환해야 한다. 그리고 통상적으로 상기 측정된 값들은 오일러각(Euler angle)을 이용하여 3차원 좌표계로 변환된다. 이는 자세 추정 함수 중 가장 일반적으로 사용되는 확장 칼만 필터가 선형 시스템에서만 사용될 수 있기 때문이다.
그런데 이처럼 오일러 각을 이용하여 3차원 좌표계로 변환하는 것은 짐벌락(Gymbal Lock) 현상을 일으킨다. 짐벌락 현상은 오일럭 각을 이용하여 회전 변환이 이루어지는 경우에, x, y, z 세 축에 대해 각각의 회전 각도값(피치(pitch angle), 롤(roll angle), 요(yaw angle))을 조합하기 되는데, 이 때에 한 축의 90도 회전에 의해 두 축이 하나의 축으로 겹쳐짐으로써 한 축의 자유도를 잃게 되는 현상이다.
이러한 짐벌락 현상이 발생하는 경우, 연산 오류가 발생하게 되고 이에 따라 로봇은 적절한 자세를 추정하지 못하게 된다. 그리고 만약 수직 성분에 대한 회전 각도값인 피치값에 짐벌락 현상이 발생하는 경우 수직관을 주행하는 경우가 많은 인-파이프 로봇에 있어 더욱 치명적이라는 문제점이 있다.
또한 현재의 로봇 자세 추정 방법은 주로 확장 칼만 필터를 이용한다. 확장 칼만 필터는 최적의 자세를 추정하는데 있어 가장 알려진 것이긴 하지만, 이전 측정값이 옳지 않거나 프로세스가 옳지 않게 설계된 경우라면 오차가 누적되어 발산할 수 있는 위험성이 있다.
또한 현재 통상적으로 로봇의 자세를 결정하는 방법은 3축 가속도 센서와 3축 각속도 센서의 측정 결과를 기초로 한다. 하지만 인-파이프 로봇의 특성상 대형의 고정밀 측정 센서를 사용할 수 없다. 따라서 인-파이프 로봇의 경우 측정 센서의 한계에 의해 실제 환경과 오차가 발생할 수도 있다는 문제가 있다. 또한 노이즈(noise)가 심한 환경의 경우, 그 측정 결과에 오류가 발생할 수 있다는 문제점이 있다.
예를 들어 인-파이프 로봇의 작업 도중, 중력에 영향을 줄 수 있는 노이즈가 발생하는 경우 측정값에 오차가 발생하게 된다. 그리고 이러한 오차는 계속 누적되게 되고, 누적된 오차는 심한 경우 확장 칼만 필터의 추정값을 발산시켜 올바른 자세 추정이 이루어질 수 없게 한다는 문제점이 있다.
본 발명의 목적은, 짐벌락 현상을 회피할 수 있는 로봇의 자세 추정 장치 및 방법을 제공함에 있다.
본 발명의 다른 목적은 로봇의 자세 추정에 있어서, 대형의 고정밀 측정 센서를 사용하지 않고서도 실제 환경과 측정값의 오차를 보정할 수 있도록 하는 장치 및 방법을 제공함에 있다.
상술한 목적을 달성하기 위한 본 발명의 실시 예에 따른 인-파이프 로봇의 자세 추정 장치는, 로봇의 3축 가속도 및 각속도를 측정하고, 그 측정값으로부터 중력 벡터(vector)를 추출하는 측정부와, 상기 중력 벡터와 실제 중력 벡터의 오차로부터 추정값을 산출하고, 추정값에 따라 상기 로봇의 자세를 보정하며, 보정된 로봇의 자세로부터 보정값을 추출하는 보정값 추출부와, 기 설정된 쿼터니언 초기값에 상기 측정값을 반영하여 로봇의 자세를 표현하는 쿼터니언을 설정하고, 상기 보정값이 입력되면 이를 반영하여 상기 설정된 쿼터니언을 재설정하는 쿼터니언 연산부와, 상기 재설정된 쿼터니언에 따라 상기 로봇의 자세를 결정하는 로봇 자세 결정부를 포함하는 것을 특징으로 한다.
또한 상기 보정값 추출부는, 상기 추출된 중력 벡터와 기 저장된 실제 중력 벡터로부터 에러 쿼터니언(error quarternion)을 산출하는 에러 쿼터니언 산출부와, 상기 에러 쿼터니언으로부터 추정 벡터를 추정하는 오차 벡터 추정부와, 상기 추정 벡터를 이용하여 상기 측정값을 반영하여 설정된 쿼터니언을 보정하고, 보정된 쿼터니언으로부터 보정값을 추출하는 보정부를 더 포함하는 것을 특징으로 한다.
또한 상기 로봇 자세 결정부는, 상기 재설정된 쿼터니언을 쿼터니언 초기값으로 설정하여 상기 쿼터니언 연산부에 입력하는 것을 특징으로 한다.
또한 상기 에러 쿼터니언은, 실수 성분과 허수 성분으로 구성되며, 실수 성분의 값은 1이고, 허수 성분의 값은 상기 추출된 중력 벡터와 상기 실제 중력 벡터의 곱과 상기 추출된 중력 벡터의 노름(norm)과 상기 실제 중력 벡터의 노름을 곱한 값의 비율에, 상기 추출된 중력 벡터와 상기 실제 중력 벡터 사이각의 사인(sin)값을 곱한 값인 쿼터니언임을 특징으로 한다.
또한 상기 오차 벡터 추정부는, 확장 칼만 필터(Extended Kalman Filter)를 이용하여 상기 추정 벡터를 산출하는 것을 특징으로 한다.
또한 상기 보정부는, 상기 추정 벡터를 정규화하고, 정규화된 추정 벡터를 축으로 상기 추출된 중력 벡터를 회전시켜 상기 실제 중력 벡터에 매칭(matching)하여 상기 설정된 쿼터니언을 보정하는 것을 특징으로 한다.
또한 본 발명의 실시 예에 따른 인-파이프 로봇의 자세 추정 방법은, 로봇의 3축 가속도 및 3축 각속도를 측정하는 단계와, 기 설정된 쿼터니언 초기값에 상기 측정된 측정값을 반영하여 로봇의 자세를 표현하는 쿼터니언을 설정하는 단계와, 상기 설정된 쿼터니언으로부터 중력 벡터를 추출하는 단계와, 상기 추출된 중력 벡터와 실제 중력 벡터의 오차로부터 추정값을 산출하고, 추정값에 따라 상기 설정된 쿼터니언을 보정하며, 보정된 쿼터니언으로부터 보정값을 추출하는 단계와, 상기 보정값을 반영하여 상기 쿼터니언을 재설정하는 단계 및, 상기 재설정된 쿼터니언에 따라 상기 로봇의 자세를 결정하는 단계를 포함하는 것을 특징으로 한다.
또한 상기 보정값을 추출하는 단계는, 상기 추출된 중력 벡터와 기 저장된 실제 중력 벡터로부터 에러 쿼터니언(error quaternion)을 산출하는 단계와, 상기 에러 쿼터니언으로부터 오차 벡터를 추정하는 단계 및, 상기 추정된 오차 벡터에 따라 상기 설정된 쿼터니언을 보정하고, 상기 보정된 쿼터니언으로부터 보정값을 추출하는 단계를 더 포함하는 것을 특징으로 한다.
또한 상기 로봇의 자세를 결정하는 단계는, 상기 재설정된 쿼터니언을 상기 쿼터니언 초기값으로 설정하는 단계를 더 포함하는 것을 특징으로 한다.
또한 상기 쿼터니언을 설정하는 단계는, 기 설정된 쿼터니언 초기값이 있는지를 체크하는 단계와, 상기 체크 결과, 기 설정된 쿼터니언 초기값이 없는 경우 쿼터니언의 초기값을 생성하는 단계를 더 포함하는 것을 특징으로 한다.
또한 상기 오차 벡터를 추정하는 단계는, 확장 칼만 필터(Extended Kalman Filter)를 이용하여 상기 에러 쿼터니언으로부터 오차 벡터를 추정하는 단계임을 특징으로 한다.
또한 상기 보정값을 추출하는 단계는, 상기 추정된 오차 벡터를 정규화하는 단계와, 상기 정규화된 벡터를 축으로 상기 추출된 중력 벡터를 회전시켜 상기 실제 중력 벡터에 매칭(matching)하는 단계 및, 상기 벡터 매칭된 상태의 쿼터니언을 보정값으로 추출하는 단계를 더 포함하는 것을 특징으로 한다.
본 발명은 쿼터니언(quaternion)으로 로봇의 자세를 표현하고, 이를 센서의 측정값을 반영하여 업데이트함으로써 짐벌락 현상이 발생하지 않도록 한다는 효과가 있다.
또한 본 발명은 센서의 측정값과 로봇이 현재 위치한 지역에 따른 환경값의 쿼터니언 오차값을 구하고, 그로부터 오차를 미리 추정하여 추정된 결과에 따라 로봇의 자세를 보정함으로써 로봇이 오차 및 노이즈에 보다 적합한 자세를 결정할 수 있도록 한다는 효과가 있다.
도 1은 본 발명의 실시 예에 따라 로봇의 자세를 추정하는 장치의 구성을 도시한 구성도.
도 2는 본 발명의 실시 예에 따라 로봇의 자세를 추정하는 동작의 흐름을 도시한 흐름도.
본 명세서에서 사용되는 기술적 용어는 단지 특정한 실시 예를 설명하기 위해 사용된 것으로, 본 발명을 한정하려는 의도가 아님을 유의해야 한다. 또한, 본 명세서에서 사용되는 단수의 표현은 문맥상 명백하게 다르게 뜻하지 않는 한, 복수의 표현을 포함한다. 본 명세서에서, "구성된다." 또는 "포함한다." 등의 용어는 명세서상에 기재된 여러 구성 요소들, 또는 여러 단계를 반드시 모두 포함하는 것으로 해석되지 않아야 하며, 그 중 일부 구성 요소들 또는 일부 단계들은 포함되지 않을 수도 있고, 또는 추가적인 구성 요소 또는 단계들을 더 포함할 수 있는 것으로 해석되어야 한다.
또한, 본 명세서에 개시된 기술을 설명함에 있어서 관련된 공지 기술에 대한 구체적인 설명이 본 명세서에 개시된 기술의 요지를 흐릴 수 있다고 판단되는 경우 그 상세한 설명을 생략한다.
이하, 첨부된 도면을 참조하여 본 명세서에 개시된 실시 예들을 상세히 설명하도록 한다.
먼저 본 발명의 완전한 이해를 돕기 위해, 본 발명의 기본 원리를 설명하면, 본 발명에서는 로봇의 환경 상태를 측정하고, 측정값들을 반영하여 로봇의 자세를 표현하는 쿼터니언을 설정한다. 그리고 설정된 쿼터니언으로부터 추출한 중력 벡터(vector)와 기 저장된 실제 중력 벡터로부터 오차값인 에러 쿼터니언을 산출한다. 그리고 산출된 에러 쿼터니언을 자세 추정 함수에 입력하여 에러 쿼터니언의 추정 벡터를 산출한다. 그리고 추정 벡터를 이용하여 상기 쿼터니언을 보정하여 실제 환경과 측정된 상태값의 오차가 보정될 수 있도록 한다. 그리고 보정값을 추출하고 추출된 보정값을 반영하여 상기 쿼터니언을 재설정한다. 그리고 재설정된 쿼터니언에 따라 로봇의 자세를 결정한다.
이에 따라 본 발명은 로봇의 자세를 추정하는 데 있어서 짐벌락 현상이 발생하지 않도록 하면서도, 센서에서 측정된 상태값과 실제 환경의 오차에 대한 추정값을 이용하여 로봇의 자세를 보정하므로 노이즈(noise)를 추정하여 로봇의 자세를 결정할 수 있다는 효과가 있다.
도 1은 본 발명의 실시 예에 따라 로봇의 자세를 추정하는 장치의 구성을 도시한 도면이다.
도 1을 참조하여 살펴보면, 본 발명의 실시 예에 다른 로봇의 자세 추정 장치는 쿼터니언 연산부(100), 측정부(110), 보정값 추출부(160), 로봇 자세 결정부(150)를 포함하여 구성된다. 그리고 상기 보정값 추출부(160)는 에러 쿼터니언 산출부(120), 에러 쿼터니언 벡터 추정부(130), 보정부(140)를 포함하여 구성된다.
여기서 쿼터니언 연산부(100)는 인-파이프 로봇의 자세 추정을 하기 위한 쿼터니언의 연산을 수행한다. 쿼터니언 연산부(100)는 현재 재설정된 쿼터니언이 없는 경우라면, 즉 로봇의 자세 추정을 처음 시작하는 경우라면, 현재 로봇의 자세를 의미하는 쿼터니언 초기값을 생성한다. 예를 들어 로봇의 자세를 나타내는 쿼터니언이
Figure 112013062204709-pat00001
인 경우라면, 쿼터니언 연산부(100)는
Figure 112013062204709-pat00002
을 초기값으로 생성할 수 있다. 그러나 만약 이전 프로세스에서 재설정된 쿼터니언이 있는 경우라면 쿼터니언 연산부(100)는 상기 재설정된 쿼터니언을 쿼터니언 초기값으로 설정한다.
그리고 쿼터니언 연산부(100)는 측정부(110)로부터 측정값을 입력받는다. 그리고 쿼터니언 연산부(100)는 상기 쿼터니언 초기값에 상기 측정값을 반영하여 현재 로봇의 자세에 따른 쿼터니언을 설정한다. 그리고 쿼터니언 연산부(100)는 상기 보정부(140)로부터 보정값이 입력되면, 상기 설정된 쿼터니언을 상기 보정값을 반영하여 재설정한다. 그리고 쿼터니언 연산부(100)는 다음 프로세스에서 측정부(110)로부터 측정값이 새로 입력되는 경우, 상기 재설정된 쿼터니언을 새로 입력된 측정값을 반영하여 현재 로봇의 자세에 따른 쿼터니언을 설정한다.
상기 측정부(110)는 현재 로봇의 환경 상태를 측정한다. 여기서 로봇의 환경 상태라는 것은 로봇의 이동 방향에 따른 가속도 및 로봇이 받고 있는 중력, 그리고 로봇이 받고 있는 관성력등이 될 수 있다. 이러한 로봇의 상태들을 측정하기 위해 측정부는 3축 가속도 센서(112), 3축 각속도 센서(114)를 구비한다.
또한 본 발명의 실시 예에 따른 로봇 자세 추정 장치의 측정부(110)는 상기 쿼터니언 연산부(100)로부터 입력된 쿼터니언과 상기 3축 가속도 센서(112)와 3축 각속도 센서(114)로부터 측정된 측정값으로부터 중력 벡터를 추출하는 중력 벡터 추출부(116)를 더 구비한다.
상기 3축 가속도 센서(112)는 현재 로봇을 중심으로 x축, y축, z축 방향의 로봇의 가속도(
Figure 112013062204709-pat00003
)를 측정한다. 그리고 상기 3축 각속도 센서(114)는 위 x축, y축, z축 각각의 회전 각도값인 롤(roll angle), 피치(pitch angle), 요(yaw angle)를 측정(로봇의 각속도 :
Figure 112013062204709-pat00004
)한다. 그리고 측정부(110)는 측정된 가속도 및 각속도를 쿼터니언 연산부(100)에 입력한다.
그러면 상기 쿼터니언 연산부(100)는 현재 초기값으로 설정된 쿼터니언에 상기 측정값을 반영하여 현재 로봇의 자세를 표현하는 쿼터니언을 설정한다. 그리고 설정된 쿼터니언(
Figure 112013062204709-pat00005
)을 상기 측정부(110)에 반환한다.
그러면 측정부(110)의 중력 벡터 추출부(116)는 쿼터니언 연산부(100)로부터 상기 현재 로봇의 자세를 표현하는 것으로 설정된 쿼터니언을 입력받는다. 그리고 중력 벡터 추출부(116)는 입력된 쿼터니언과 측정된 가속도(
Figure 112013062204709-pat00006
), 그리고 입력된 쿼터니언의 공액 사원수(
Figure 112013062204709-pat00007
)를 이용하여 현재 로봇에 미치는 중력의 영향을 의미하는 중력 벡터(
Figure 112013062204709-pat00008
)를 추출한다. 그리고 추출된 중력 벡터(
Figure 112013062204709-pat00009
)는 보정값 추출부(160)에 입력된다.
본 발명의 실시 예에 따른 보정값 추출부(160)는, 상기 측정부(110)에서 측정된 환경 상태와 현재 로봇의 환경 상태 간의 오차 및, 그 오차에 따른 추정값을 산출하여, 현재 로봇의 자세를 보정하기 위한 보정값을 산출하기 위한 것이다. 이를 위해 보정값 추출부(160)는 에러 쿼터니언 산출부(120), 에러 쿼터니언 벡터 추정부(130), 그리고 보정부(140)를 포함하여 구성된다.
여기서 에러 쿼터니언 산출부(120)는 측정부(110)로부터 상기 중력 벡터(
Figure 112013062204709-pat00010
)를 입력받는다. 그리고 에러 쿼터니언 산출부(120)는 입력된 중력 벡터(
Figure 112013062204709-pat00011
)와 실제 중력 벡터(
Figure 112013062204709-pat00012
)를 토대로 에러 쿼터니언(error quaternion)을 산출한다. 여기서 상기 실제 중력 벡터(
Figure 112013062204709-pat00013
)는 실제 중력 벡터 대표값 [0 0 9.8]이 될 수 있다.
중력 벡터 저장부(122)는 이처럼 실제의 중력 벡터값을 저장한다. 그리고 에러 쿼터니언 산출부(120)는 실제의 중력 벡터값을 중력 벡터 저장부(122)로부터 로드(load)한다.
그리고 에러 쿼터니언 산출부(120)는 입력된 중력 벡터(
Figure 112013062204709-pat00014
)와 실제 중력 벡터(
Figure 112013062204709-pat00015
)를 토대로 에러 쿼터니언을 산출한다. 그리고 에러 쿼터니언 산출부(120)는 산출된 에러 쿼터니언을 에러 쿼터니언 벡터 추정부(130)에 입력한다. 상기 에러 쿼터니언 벡터 추정부(130)는 상술한 확장 칼만 필터와 같은 추정 함수를 이용할 수 있다.
그런데 상술한 바와 같이 확장 칼만 필터는 상술한 바와 같이 선형 시스템에서만 사용될 수 있다. 예를 들어 확장 칼만 필터는 하기 수학식 1과 같은 선형식의 형태를 가져야 동작할 수 있다.
Figure 112013062204709-pat00016
여기에서
Figure 112013062204709-pat00017
는 해당 시간에서 이전 상태에 기반한 상태 전이 행렬,
Figure 112013062204709-pat00018
는 사용자 입력에 의한 상태 전이 행렬, 그리고
Figure 112013062204709-pat00019
는 공분산 행렬
Figure 112013062204709-pat00020
을 가지는 다변수 정규 분포
Figure 112013062204709-pat00021
잡음 변수이다.
그러나 쿼터니언의 경우, 실제로 강체의 회전을 표시하기 위해서는 곱셈 형태의 독특한 연산을 가지게 된다. 이에 따라 상기 에러 쿼터니언 산출부(120)는 에러 쿼터니언을 선형화하도록 한다. 이를 위해 에러 쿼터니언 산출부(120)는 선형화부(124)를 더 구비한다.
쿼터니언은 일반적으로 실수 하나와 허수 세 개로 구성되고, 허수 세 개는 벡터의 성질을 가진다. 따라서 쿼터니언의 허수를 벡터로 가정하면 벡터의 합으로 인한 선형화가 가능하게 된다. 여기서 중요한 점은 쿼터니언의 허수를 벡터로 가정하기 위해서는 허수값이 충분히 작아야 쿼터니언의 기본 성질을 거스르지 않을 수 있다.
그런데 일반적으로 중력 벡터 추출부(116)에서 추출된 중력 벡터(
Figure 112013062204709-pat00022
)와 실제 중력 벡터(
Figure 112013062204709-pat00023
)의 경우 기본적으로 두 벡터 모두 수직방향을 향하고 있는 것이므로, 그 누적되지 않은 허수 성분의 차이(중력 벡터의 회전 성분과 기 저장된 중력 가속도의 회전 성분의 차이)는 매우 작다. 따라서 중력 벡터의 경우에는 위에서처럼 쿼터니언의 허수를 벡터로 가정하여 선형화가 가능하다. 즉, 쿼터니언의 허수 부분을 벡터로 가정하여 추출함으로서 이를 선형화하는 것이 가능하다. 예를 들어 에러 쿼터니언의 값이
Figure 112013062204709-pat00024
인 경우, 이를 선형화하면
Figure 112013062204709-pat00025
(error quaternion) 벡터(
Figure 112013062204709-pat00026
)가 된다.
선형화부(124)에서 에러 쿼터니언이 선형화되면, 에러 쿼터니언 산출부(120)는 에러 쿼터니언 벡터 추정부(130)로
Figure 112013062204709-pat00027
벡터를 입력한다. 여기서
Figure 112013062204709-pat00028
벡터라는 것은 선형화된 에러 쿼터니언을 말하는 것이다.
그러면 에러 쿼터니언 벡터 추정부(130)는 기 저장된 추정 함수를 이용하여 입력된
Figure 112013062204709-pat00029
벡터로부터 에러 쿼터니언의 추정 벡터를 산출한다. 여기서 에러 쿼터니언 벡터 추정부(130)는 상술한 바와 같이 확장 칼만 필터와 같은 추정 필터를 사용하여
Figure 112013062204709-pat00030
벡터로부터 에러 쿼터니언의 추정 벡터를 산출할 수 있다.
그리고 에러 쿼터니언 벡터 추정부(130)에서 산출된 추정 벡터는 보정부(140)로 입력된다. 그리고 보정부(140)는 입력된 추정 벡터를 정규화한다. 왜냐하면 상기 추정 벡터는 로봇의 자세를 보정하기 위한 것으로, 현재 측정된 중력 벡터를 실제 중력 벡터에 일치할 수 있도록 쿼터니언을 회전하기 위한 것이기 때문이다. 따라서 정규화부(142)는 입력된 추정 벡터를 정규화하여 단위 사원수(unit quaternion)화한다.
상기 보정부(140)는 정규화된 추정 벡터를 이용하여, 벡터 매칭(Vector Matching)을 통해 추출된 중력 벡터(
Figure 112013062204709-pat00031
)와 실제 중력 벡터(
Figure 112013062204709-pat00032
)의 오차를 보정한다. 즉, 보정부(140)는 정규화부(142)가 상기 추정 벡터를 정규화하면 이를 축으로 상기 측정부(110)에서 추출된 중력 벡터(
Figure 112013062204709-pat00033
)를 회전하여 실제 중력 벡터(
Figure 112013062204709-pat00034
)에 일치시킨다. 그리고 보정부(140)는 벡터가 매칭된 상태의 쿼터니언을 추출하여 이를 보정값으로 상기 쿼터니언 연산부(100)로 입력한다.
그러면 쿼터니언 연산부(100)는 입력된 보정값을 반영하여 현재 설정된 쿼터니언을 재설정한다. 즉, 쿼터니언 연산부(100)는 측정부(110)에서 로봇의 상태가 측정될 때, 즉 한번의 프로세스 마다 총 2회의 쿼터니언 설정을 수행한다. 첫 번째는 측정부(110)에서 측정된 값을 반영하여 쿼터니언을 설정하는 것이고, 두 번째는 측정값을 반영하여 설정된 쿼터니언을, 상기 보정값을 반영하여 재설정하는 것이다. 그리고 로봇 자세 결정부(150)는 보정값을 반영하여 재설정된 쿼터니언에 따라 인-파이프 로봇의 자세를 결정한다. 그리고 로봇 자세 결정부(150)는 로봇의 자세를 결정하는데 사용한 쿼터니언을 쿼터니언 초기값으로 설정하여 쿼터니언 연산부(100)에 입력한다.
그리고 쿼터니언 연산부(100)는 새로운 프로세스가 시작되어 측정부(110)에서 새로운 측정값이 입력되면, 현재 초기값으로 설정된 쿼터니언을 새로운 측정값을 반영하여 현재 로봇의 자세를 표현하는 쿼터니언을 설정한다. 그리고 위의 과정들을 반복하여 보정값을 산출하고 보정값을 반영하여 쿼터니언을 재설정한다. 그리고 로봇 자세 결정부(150)는 재설정된 쿼터니언에 따라 로봇의 자세를 결정한다.
따라서 본 발명의 실시 예에 따른 로봇 자세 추정 장치는 한 프로세스가 종료될 때마다 오차 및 노이즈를 추정하고, 추정된 오차 및 노이즈에 따라 로봇의 자세를 보정하므로, 보다 노이즈 및 오차에 보다 강한 자세가 결정될 수 있다. 또한 본 발명의 실시 예에 따른 로봇 자세 추정 장치는 로봇의 자세를 쿼터니언을 이용하여 표현함으로써 짐벌락 현상이 발생하지 않는다는 효과가 있다.
도 2는 본 발명의 실시 예에 따라 로봇의 자세를 추정하는 동작의 흐름을 도시한 도면이다.
도 2를 참조하여 살펴보면, 인-파이프 로봇의 자세 추정이 시작되면 본 발명의 실시 예에 따른 쿼터니언 연산부(100)는 기 설정된 쿼터니언 초기값이 있는지 여부를 체크한다. 그리고 기 설정된 쿼터니언 초기값이 없는 경우라면 쿼터니언의 초기값을 생성한다(S200).
그리고 측정부(110)는 3축 가속도 센서(112)와 3축 각속도 센서(114)로부터 로봇의 환경 상태를 측정한다(S202). 그리고 측정부(110)는 측정값들을 쿼터니언 연산부(100)에 입력한다. 그러면 쿼터니언 연산부(100)는 상기 생성된 쿼터니언 초기값에 상기 측정값들을 반영하여 현재 로봇의 자세를 표현하는 쿼터니언을 설정한다(S204).
그러나 만약 기 설정된 쿼터니언 초기값이 있는 경우라면, 측정부(110)는 바로 3축 가속도 센서(112)와 3축 각속도 센서(114)로부터 로봇의 환경 상태를 측정한다(S202). 그리고 기 쿼터니언 연산부(100)는 기 설정된 쿼터니언 초기값에 측정부(110)로부터 입력되는 측정값들을 반영하여 현재 로봇의 자세를 표현하는 쿼터니언을 설정한다.
여기서 쿼터니언 연산부(100)가 쿼터니언을 업데이트하는 과정은 하기 수학식 2와 같다.
Figure 112013062204709-pat00035
여기서
Figure 112013062204709-pat00036
,
Figure 112013062204709-pat00037

그리고 중력 벡터 추출부(116)는 상기 업데이트된 쿼터니언으로부터 공액 사원수를 생성한다. 그리고 중력 벡터 추출부(116)는 생성된 공액 사원수를 이용하여 현재 설정된 쿼터니언으로부터 중력 벡터를 추출한다(S206). 하기 수학식 3은 상기 측정부(110)가 현재 설정된 쿼터니언으로부터 중력 벡터(
Figure 112013062204709-pat00038
)를 추출하는 것을 나타낸 것이다.
Figure 112013062204709-pat00039
여기서 공액사원수
Figure 112013062204709-pat00040

그리고 측정부(110)는 상기 수학식 3을 통해 추출된 중력 벡터(
Figure 112013062204709-pat00041
)를 에러 쿼터니언 산출부(120)에 입력한다. 그러면 에러 쿼터니언 산출부(120)는 추출된 중력 벡터(
Figure 112013062204709-pat00042
)로부터 에러 쿼터니언(
Figure 112013062204709-pat00043
)을 산출한다(S208). 이를 위해 에러 쿼터니언 산출부(120)는 중력 벡터 저장부(122)로부터 실제 중력 벡터(
Figure 112013062204709-pat00044
)를 로드한다. 여기서 실제 중력 벡터(
Figure 112013062204709-pat00045
)는 중력 벡터 대푯값 [0 0 9.8]이 될 수 있다.
그리고 에러 쿼터니언 벡터 산출부(120)는 상기 추출된 중력 벡터(
Figure 112013062204709-pat00046
)와 상기 실제 중력 벡터(
Figure 112013062204709-pat00047
)의 곱을 구한다. 그리고 상기 추출된 중력 벡터의 노름(norm)(
Figure 112013062204709-pat00048
)과 상기 실제 중력 벡터의 노름(
Figure 112013062204709-pat00049
)의 곱을 구한다. 그리고 상기 추출된 중력 벡터(
Figure 112013062204709-pat00050
)와 상기 실제 중력 벡터(
Figure 112013062204709-pat00051
)의 곱과 상기 추출된 중력 벡터의 노름(norm)(
Figure 112013062204709-pat00052
)과 상기 실제 중력 벡터의 노름(
Figure 112013062204709-pat00053
)의 곱의 비율(
Figure 112013062204709-pat00054
)을 구한다.
그리고 상기 추출된 중력 벡터(
Figure 112013062204709-pat00055
)와 상기 실제 중력 벡터(
Figure 112013062204709-pat00056
)의 사이각의 사인(sin)값에, 상기 구해진 비율(
Figure 112013062204709-pat00057
)을 곱한다. 그리고 이를 허수 성분으로 하고, 실수 성분을 "1"로 하는 에러 쿼터니언(
Figure 112013062204709-pat00058
)을 산출한다. 하기 수학식 4는 이처럼 에러 쿼터니언 산출부(120)이 에러 쿼터니언(
Figure 112013062204709-pat00059
)를 산출하는 것을 나타낸 것이다.
Figure 112013062204709-pat00060
한편 수학식 4를 통해 에러 쿼터니언(
Figure 112013062204709-pat00061
)이 산출되면, 에러 쿼터니언 산출부(120)는 선형화부(124)를 통해 산출된 에러 쿼터니언(
Figure 112013062204709-pat00062
)을 선형화한다(S210). 그리고 에러 쿼터니언 산출부(120)는 에러 쿼터니언(
Figure 112013062204709-pat00063
)을 선형화한
Figure 112013062204709-pat00064
(error quaternion) 벡터를 에러 쿼터니언 벡터 추정부(130)에 입력한다.
에러 쿼터니언 벡터 추정부(130)는 입력된
Figure 112013062204709-pat00065
벡터의 추정 벡터를 산출한다(S212). 여기서 상기 추정 벡터를 산출하기 위해 에러 쿼터니언 벡터 추정부(130)는 확장 칼만 필터를 사용할 수 있다. 그리고 확장 칼만 필터를 사용하는 경우, 상기 에러 쿼터니언 벡터 추정부(130)는 확장 칼만 필터를 사용하기 위한 상수
Figure 112013062204709-pat00066
과, 샘플링 시간 dt와 각속도 값에 따라 미분된
Figure 112013062204709-pat00067
, 그리고 측정값의 불확실성을 나타내는 매트릭스
Figure 112013062204709-pat00068
, 쿼터니언의 불확실성을 나타내는
Figure 112013062204709-pat00069
, 그리고 R과 Q의 관계를 나타내는 매트릭스인
Figure 112013062204709-pat00070
를 사용할 수 있다.
그리고 추정 벡터가 산출되면, 에러 쿼터니언 벡터 추정부(130)는 이를 보정부(140)에 입력한다. 그러면 보정부(140)는 입력된 추정 벡터를 정규화한다(S214). 예를 들어 추정 벡터가
Figure 112013062204709-pat00071
인 경우, 추정 벡터는 하기 수학식 5에 의해 정규화될 수 있다.
Figure 112013062204709-pat00072
추정 벡터가 정규화되면, 보정부(140)는 정규화된 추정 벡터를 이용하여 벡터 매칭(vector matching)을 통해 오차를 보정하고 보정값을 추출한다(S216). 즉, 보정부(140)는 정규화된 추정 벡터를 축으로 하여 중력 벡터 추출부(116)에서 추출된 중력 벡터(
Figure 112013062204709-pat00073
)를 회전하여 실제 중력 벡터(
Figure 112013062204709-pat00074
)에 매칭시킨다. 그리고 보정부(140)는 벡터가 매칭된 상태의 쿼터니언을 추출하여(S216) 이를 보정값으로 쿼터니언 연산부(100)에 입력한다.
보정값이 입력되면, 쿼터니언 연산부(100)는 입력된 보정값을 반영하여 쿼터니언을 재설정한다(S218). 따라서 쿼터니언 연산부(100)는 측정부(110)에서 측정된 값을 반영하여 설정한 쿼터니언을, 보정값을 반영하여 재설정한다. 하기 수학식 6은 이를 나타낸 것이다.
Figure 112013062204709-pat00075
여기서
Figure 112013062204709-pat00076
은 보정값을 반영하여 재설정된 쿼터니언,
Figure 112013062204709-pat00077
은 정규화된 추정 벡터에 따라 보정한 보정값,
Figure 112013062204709-pat00078
은 측정값을 반영하여 설정된 쿼터니언,
그러면 로봇 자세 결정부(150)는 재설정된 쿼터니언에 따라 인-파이프 로봇의 자세를 결정한다(S220). 그리고 로봇 자세 결정부(150)는 재설정된 쿼터니언을 쿼터니언 초기값으로 설정(
Figure 112013062204709-pat00079
)한다(S222). 그러면 쿼터니언 연산부(100)는 새로운 측정값이 입력되는 경우, 이전 프로세스에서 재설정된 쿼터니언에 새로 입력된 측정값을 반영하여 로봇의 자세를 표현하는 쿼터니언을 설정한다(S204). 그리고 S206 단계에서 S222 단계에 이르는 동작들은 인-파이프 로봇이 작동하는 동안 반복하여 수행된다.
따라서 본 발명의 실시 예에 따른 로봇 자세 추정 방법은 센서에서 측정된 측정값과 실제 로봇의 환경을 비교하여 오차를 산출하고, 산출된 오차의 추정값을 반영하여 로봇의 자세를 결정한다. 따라서 로봇의 자세를 결정함에 있어 노이즈 및 오차를 미리 대비할 수 있도록 한다.
한편 상술한 본 발명의 설명에서는 구체적인 실시 예에 관해 설명하였으나, 여러 가지 변형이 본 발명의 범위에서 벗어나지 않고 실시할 수 있다. 특히 본 발명의 실시 예에서는 에러 쿼터니언 벡터 추정부(130)가 확장 칼만 필터를 사용하여 에러 쿼터니언의 오차를 추정하는 것을 가정하였으나, 만약 확장 칼만 필터보다 로봇의 자세를 추정하는데 더 나은 자세 추정 필터가 있는 경우에는 이를 적용할 수도 있음은 물론이다.
이 경우 본 발명의 실시 예에서 언급한 선형화부(124)는 상기 에러 쿼터니언 벡터 추정부(130)에서 사용하는 자세 추정 필터에 따라 쿼터니언의 값을 변환하는 구성이 될 것이다. 또한 상기 정규화부(142) 역시 상기 에러 쿼터니언 벡터 추정부(130)에서 사용하는 자세 추정 필터로부터 산출된 결과를 벡터 매칭에 따라 쿼터니언을 보정할 수 있는 형태로 변환하는 구성이 될 것이다.
아울러 본 발명의 실시 예에서는 측정부(110)에서 중력 벡터를 추출하고, 기 저장된 실제 중력 벡터를 비교하여 에러 쿼터니언을 산출하는 것을 가정하여 설명하였으나, 이와는 다른 측정값을 사용할 수도 있음은 물론이다. 예를 들어 전향력(Coriolis force)과 측정된 수평 방향의 가속도를 측정하여 이를 비교함으로써 에러 쿼터니언을 산출할 수도 있음은 물론이다.
그러므로 본 발명이 속하는 기술 분야에서 통상의 지식을 가진 자라면 본 발명의 본질적인 특성에서 벗어나지 않는 범위에서 다양한 수정 및 변형이 가능할 것이다. 따라서 본 발명에 개시된 실시 예들은 본 발명의 기술 사상을 한정하기 위한 것이 아니라 설명하기 위한 것이고, 이러한 실시 예에 의하여 본 발명의 기술 사상의 범위가 한정되는 것은 아니다. 본 발명의 보호 범위는 아래의 청구범위에 의하여 해석 되어야 하며, 그와 동등한 범위 내에 있는 모든 기술 사상은 본 발명의 권리범위에 포함되는 것으로 해석되어야 할 것이다.
100 : 쿼터니언 연산부 110 : 측정부
112 : 3축 가속도 센서 114 : 3축 각속도 센서
116 : 중력 벡터 추출부 120 : 에러 쿼터니언 산출부
122 : 중력 벡터 저장부 124 : 선형화부
130 : 에러 쿼터니언 벡터 추정부 140 : 보정부
142 : 정규화부 150 : 로봇 자세 결정부
160 : 보정값 추출부

Claims (14)

  1. 로봇의 3축 가속도 및 각속도를 측정하고, 그 측정값으로부터 중력 벡터(vector)를 추출하는 측정부와,
    상기 중력 벡터와 실제 중력 벡터의 오차로부터 추정값을 산출하고, 추정값에 따라 상기 로봇의 자세를 보정하며, 보정된 로봇의 자세로부터 보정값을 추출하는 보정값 추출부와,
    기 설정된 쿼터니언 초기값에 상기 측정값을 반영하여 로봇의 자세를 표현하는 쿼터니언을 설정하고, 상기 보정값이 입력되면 이를 반영하여 상기 설정된 쿼터니언을 재설정하는 쿼터니언 연산부와,
    상기 재설정된 쿼터니언에 따라 상기 로봇의 자세를 결정하는 로봇 자세 결정부를 포함하며,
    상기 보정값 추출부는,
    상기 추출된 중력 벡터와 기 저장된 실제 중력 벡터로부터 에러 쿼터니언(error quarternion)을 산출하는 에러 쿼터니언 산출부와,
    상기 에러 쿼터니언으로부터 추정 벡터를 추정하는 오차 벡터 추정부와,
    상기 추정 벡터를 이용하여 상기 측정값을 반영하여 설정된 쿼터니언을 보정하고, 보정된 쿼터니언으로부터 보정값을 추출하는 보정부를 더 포함하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 장치.
  2. 삭제
  3. 제1항에 있어서, 상기 로봇 자세 결정부는,
    상기 재설정된 쿼터니언을 쿼터니언 초기값으로 설정하여 상기 쿼터니언 연산부에 입력하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 장치.
  4. 제1항에 있어서, 상기 에러 쿼터니언은,
    실수 성분과 허수 성분으로 구성되며, 실수 성분의 값은 1이고, 허수 성분의 값은 상기 추출된 중력 벡터와 상기 실제 중력 벡터의 곱과 상기 추출된 중력 벡터의 노름(norm)과 상기 실제 중력 벡터의 노름을 곱한 값의 비율에, 상기 추출된 중력 벡터와 상기 실제 중력 벡터 사이각의 사인(sin)값을 곱한 값인 쿼터니언임을 특징으로 하는 인-파이프 로봇의 자세 추정 장치.
  5. 제1항에 있어서, 상기 오차 벡터 추정부는,
    확장 칼만 필터(Extended Kalman Filter)를 이용하여 상기 추정 벡터를 산출하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 장치.
  6. 제1항에 있어서, 상기 보정부는,
    상기 추정 벡터를 정규화하고, 정규화된 추정 벡터를 축으로 상기 추출된 중력 벡터를 회전시켜 상기 실제 중력 벡터에 매칭(matching)하여 상기 설정된 쿼터니언을 보정하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 장치.
  7. 로봇의 3축 가속도 및 3축 각속도를 측정하는 단계;
    기 설정된 쿼터니언 초기값에 상기 측정된 측정값을 반영하여 로봇의 자세를 표현하는 쿼터니언을 설정하는 단계;
    상기 설정된 쿼터니언으로부터 중력 벡터를 추출하는 단계;
    상기 추출된 중력 벡터와 실제 중력 벡터의 오차로부터 추정값을 산출하고, 추정값에 따라 상기 설정된 쿼터니언을 보정하며, 보정된 쿼터니언으로부터 보정값을 추출하는 단계;
    상기 보정값을 반영하여 상기 쿼터니언을 재설정하는 단계; 및,
    상기 재설정된 쿼터니언에 따라 상기 로봇의 자세를 결정하는 단계를 포함하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 방법.
  8. 제7항에 있어서, 상기 보정값을 추출하는 단계는,
    상기 추출된 중력 벡터와 기 저장된 실제 중력 벡터로부터 에러 쿼터니언(error quaternion)을 산출하는 단계;
    상기 에러 쿼터니언으로부터 오차 벡터를 추정하는 단계; 및
    상기 추정된 오차 벡터에 따라 상기 설정된 쿼터니언을 보정하고, 상기 보정된 쿼터니언으로부터 보정값을 추출하는 단계를 더 포함하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 방법.
  9. 제7항에 있어서, 상기 로봇의 자세를 결정하는 단계는,
    상기 재설정된 쿼터니언을 상기 쿼터니언 초기값으로 설정하는 단계를 더 포함하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 방법.
  10. 제7항에 있어서, 상기 쿼터니언을 설정하는 단계는,
    기 설정된 쿼터니언 초기값이 있는지를 체크하는 단계; 및
    상기 체크 결과, 기 설정된 쿼터니언 초기값이 없는 경우 쿼터니언의 초기값을 생성하는 단계를 더 포함하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 방법.
  11. 제8항에 있어서, 상기 오차 벡터를 추정하는 단계는,
    확장 칼만 필터(Extended Kalman Filter)를 이용하여 상기 에러 쿼터니언으로부터 오차 벡터를 추정하는 단계임을 특징으로 하는 인-파이프 로봇의 자세 추정 방법.
  12. 제11항에 있어서, 상기 에러 쿼터니언을 산출하는 단계는,
    상기 에러 쿼터니언을 선형화하는 단계를 더 포함하는 것을 특징으로하는 인-파이프 로봇의 자세 추정 방법.
  13. 제8항에 있어서, 상기 보정값을 추출하는 단계는,
    상기 추정된 오차 벡터를 정규화하는 단계;
    상기 정규화된 벡터를 축으로 상기 추출된 중력 벡터를 회전시켜 상기 실제 중력 벡터에 매칭(matching)하는 단계; 및
    상기 벡터 매칭된 상태의 쿼터니언을 보정값으로 추출하는 단계를 더 포함하는 것을 특징으로 하는 인-파이프 로봇의 자세 추정 방법.
  14. 제8항에 있어서, 상기 에러 쿼터니언을 산출하는 단계는,
    실수 성분과 허수 성분으로 구성되며, 실수 성분의 값은 1이고, 허수 성분의 값은 상기 추출된 중력 벡터와 상기 실제 중력 벡터의 곱과 상기 추출된 중력 벡터의 노름(norm)과 상기 실제 중력 벡터의 노름을 곱한 값의 비율에, 상기 추출된 중력 벡터와 상기 실제 중력 벡터 사이각의 사인(sin)값을 곱한 값인 쿼터니언을 구하는 단계임을 특징으로 하는 인-파이프 로봇의 자세 추정 방법.
KR1020130081148A 2013-07-10 2013-07-10 인-파이프 로봇의 자세 추정 장치 및 방법 KR101456576B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020130081148A KR101456576B1 (ko) 2013-07-10 2013-07-10 인-파이프 로봇의 자세 추정 장치 및 방법

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020130081148A KR101456576B1 (ko) 2013-07-10 2013-07-10 인-파이프 로봇의 자세 추정 장치 및 방법

Publications (1)

Publication Number Publication Date
KR101456576B1 true KR101456576B1 (ko) 2014-10-31

Family

ID=51999032

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020130081148A KR101456576B1 (ko) 2013-07-10 2013-07-10 인-파이프 로봇의 자세 추정 장치 및 방법

Country Status (1)

Country Link
KR (1) KR101456576B1 (ko)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016093607A1 (ko) * 2014-12-12 2016-06-16 한국항공우주연구원 이동체 제어 장치, 방법 및 그 방법을 컴퓨터에서 실행하기 위한 프로그램을 기록하는 컴퓨터 판독 가능한 기록매체
KR101802650B1 (ko) 2016-05-16 2017-11-30 경희대학교 산학협력단 배관의 위치 추정 시스템 및 그 방법
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
WO2019172461A1 (ko) * 2018-03-06 2019-09-12 주식회사 선택인터내셔날 차량의 움직임을 측정하기 위한 실시간 가속도 센서 보정 장치 및 이를 이용한 가속도 센서 보정 방법
US10556340B2 (en) 2014-12-12 2020-02-11 Korea Aerospace Research Institute Apparatus and method for controlling moving object, and computer-readable recording medium in which program for implementing method in computer is recorded
KR20230027603A (ko) * 2021-08-19 2023-02-28 한국로봇융합연구원 각속도계와 가속도계 출력 정보로부터 장치의 자세를 추정하는 깊은 신경망 모델의 입력데이터를 구조화하기 위한 장치 및 이를 위한 방법

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002274499A (ja) 2001-03-19 2002-09-25 Nec Aerospace Syst Ltd 三軸衛星の姿勢変更制御方式
KR20120070291A (ko) * 2010-12-21 2012-06-29 삼성전자주식회사 보행 로봇 및 그의 동시 위치 인식 및 지도 작성 방법
KR20130010608A (ko) * 2011-07-19 2013-01-29 한국생산기술연구원 로봇 제어용 클러치 시스템 및 보행 보조 로봇 시스템, 보행 보조 로봇의 동작 산출 방법

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002274499A (ja) 2001-03-19 2002-09-25 Nec Aerospace Syst Ltd 三軸衛星の姿勢変更制御方式
KR20120070291A (ko) * 2010-12-21 2012-06-29 삼성전자주식회사 보행 로봇 및 그의 동시 위치 인식 및 지도 작성 방법
KR20130010608A (ko) * 2011-07-19 2013-01-29 한국생산기술연구원 로봇 제어용 클러치 시스템 및 보행 보조 로봇 시스템, 보행 보조 로봇의 동작 산출 방법

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016093607A1 (ko) * 2014-12-12 2016-06-16 한국항공우주연구원 이동체 제어 장치, 방법 및 그 방법을 컴퓨터에서 실행하기 위한 프로그램을 기록하는 컴퓨터 판독 가능한 기록매체
US10556340B2 (en) 2014-12-12 2020-02-11 Korea Aerospace Research Institute Apparatus and method for controlling moving object, and computer-readable recording medium in which program for implementing method in computer is recorded
KR101802650B1 (ko) 2016-05-16 2017-11-30 경희대학교 산학협력단 배관의 위치 추정 시스템 및 그 방법
WO2019172461A1 (ko) * 2018-03-06 2019-09-12 주식회사 선택인터내셔날 차량의 움직임을 측정하기 위한 실시간 가속도 센서 보정 장치 및 이를 이용한 가속도 센서 보정 방법
KR20200094793A (ko) * 2018-03-06 2020-08-07 주식회사 선택인터내셔날 차량의 움직임을 측정하기 위한 실시간 가속도 센서 보정 장치 및 이를 이용한 가속도 센서 보정 방법
EP3763590A4 (en) * 2018-03-06 2021-03-17 Suntech International Ltd. REAL-TIME ACCELERATION SENSOR CORRECTION DEVICE FOR MEASURING VEHICLE MOVEMENT AND ACCELERATION SENSOR CORRECTION PROCEDURES WITH IT
KR102304006B1 (ko) 2018-03-06 2021-09-23 주식회사 선택인터내셔날 차량의 움직임을 측정하기 위한 실시간 가속도 센서 보정 장치 및 이를 이용한 가속도 센서 보정 방법
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
KR20230027603A (ko) * 2021-08-19 2023-02-28 한국로봇융합연구원 각속도계와 가속도계 출력 정보로부터 장치의 자세를 추정하는 깊은 신경망 모델의 입력데이터를 구조화하기 위한 장치 및 이를 위한 방법
KR102549001B1 (ko) * 2021-08-19 2023-06-27 한국로봇융합연구원 각속도계와 가속도계 출력 정보로부터 장치의 자세를 추정하는 깊은 신경망 모델의 입력데이터를 구조화하기 위한 장치 및 이를 위한 방법

Similar Documents

Publication Publication Date Title
KR101456576B1 (ko) 인-파이프 로봇의 자세 추정 장치 및 방법
AU2020205220B2 (en) IMU calibration
EP2973127B1 (en) Heading confidence interval estimation
Bloesch et al. State estimation for legged robots: Consistent fusion of leg kinematics and IMU
Du et al. Online serial manipulator calibration based on multisensory process via extended Kalman and particle filters
US20060262141A1 (en) Position and orientation measuring method and apparatus
JP2013200162A (ja) 小型姿勢センサ
CN106813679B (zh) 运动物体的姿态估计的方法及装置
EP2930467A1 (en) A system and method for sensing the inclination of a moving platform with respect to gravity
Du et al. An online method for serial robot self-calibration with CMAC and UKF
CN112304340B (zh) 一种基于九轴imu的姿态解算方法、装置及存储介质
CN109866217B (zh) 机器人里程定位方法、装置、终端设备及计算机存储介质
Sjøberg et al. An EKF for Lie groups with application to crane load dynamics
US20170074689A1 (en) Sensor Fusion Method for Determining Orientation of an Object
Miletović et al. Improved Stewart platform state estimation using inertial and actuator position measurements
CN103954288B (zh) 一种卫星姿态确定系统精度响应关系确定方法
JP6663219B2 (ja) 姿勢動作検出装置
JP2013122384A (ja) カルマンフィルタ、及び、状態推定装置
He et al. Estimating the orientation of a rigid body moving in space using inertial sensors
JP2009186244A (ja) 傾斜角度推定システム、相対角度推定システム及び角速度推定システム
Jeon et al. Quaternion-Based Iterative Extended Kalman Filter for Sensor Fusion of Vision Sensor and IMU in 6-DOF Displacement Monitoring
Chen et al. A robust optical/inertial data fusion system for motion tracking of the robot manipulator
JP5424224B2 (ja) 相対角度推定システム
JP6516332B2 (ja) 角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法
Zhang et al. Improved extended Kalman fusion method for upper limb motion estimation with inertial sensors

Legal Events

Date Code Title Description
E701 Decision to grant or registration of patent right
GRNT Written decision to grant
FPAY Annual fee payment

Payment date: 20190326

Year of fee payment: 5

FPAY Annual fee payment

Payment date: 20191015

Year of fee payment: 6