KR101941009B1 - 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치 - Google Patents

자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치 Download PDF

Info

Publication number
KR101941009B1
KR101941009B1 KR1020160125146A KR20160125146A KR101941009B1 KR 101941009 B1 KR101941009 B1 KR 101941009B1 KR 1020160125146 A KR1020160125146 A KR 1020160125146A KR 20160125146 A KR20160125146 A KR 20160125146A KR 101941009 B1 KR101941009 B1 KR 101941009B1
Authority
KR
South Korea
Prior art keywords
value
mobile device
unmanned mobile
sensor
attitude
Prior art date
Application number
KR1020160125146A
Other languages
English (en)
Other versions
KR20180035090A (ko
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 KR1020160125146A priority Critical patent/KR101941009B1/ko
Publication of KR20180035090A publication Critical patent/KR20180035090A/ko
Application granted granted Critical
Publication of KR101941009B1 publication Critical patent/KR101941009B1/ko

Links

Images

Classifications

    • 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/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
    • G01P15/003—Kinematic accelerometers, i.e. measuring acceleration in relation to an external reference frame, e.g. Ferratis accelerometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/14—Receivers specially adapted for specific applications
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

무인 이동 장치가 개시된다. 본 발명의 일 실시 예에 따른 무인 이동 장치는 자세추정 시스템을 포함하는 무인 이동 장치로, 상기 무인 이동 장치의 각속도 및 가속도를 측정하는 가속도 센서, 상기 무인 이동 장치의 지자계 자속 밀도를 측정하는 지자계 센서, 및 상기 가속도 센서 및 지자계 센서로부터 획득한 측정 값에 기초하여 상기 무인 이동 장치의 자세를 추정하는 제어부를 포함한다.

Description

자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치 {ATTITUDE AND HEADING REFERENCE SYSTEM AND UNMANED VEHICLE INCLUDING THE ATTITUDE AND HEADING REFERNCE SYSTEM}
본 발명은 무인 이동 장치에 탑재된 자세추정 시스템에 관한 것이다. 상세하게는 자기장 변화 및 선형 가속도에 강건한 자세추정 시스템에 관한 것이다.
최근 무인 자동차, 무인 항공기, 무인 로봇 등의 무인 이동 장치 산업은 가장 기본이 되는 감지 기술 측면에 있어서 MEMS(Micro-Electromechanical System) 기반의 고정밀 초소형 센서들의 본격적인 상용화 덕분에 그 활동 영역과 시장 잠재력이 크게 증가하고 있다. 무인 이동 장치의 임무 수행을 위해서는 이동 장치의 정확한 방향각 추정이 필수적이다. 현재 개발되어 상용화 되어 있는 저가형 AHRS(Attitude and Heading Reference System)의 경우에는 제한된 환경에서는 정상적인 방향각 추정이 가능하지만 외부 자기장의 유입 등으로 인해서 측정 방향각이 외란 되는 경우에는 그 문제를 근본적으로 해결할 수 있는 알고리즘 을 확보하고 있지 못하기 때문에, 많은 관련 기업에서 이를 확보하고자 노력하고 있다.
세계로봇 연맹(IFR)에 따르면, 저가형 AHRS와 밀접하게 관련된 분야인 서비스 이동 로봇(Mobile Robot)의 시장은 2007년 22.3억 달려 규모에서 2012년 46.5억 달려 규모까지 확대됨에 따라 향후 급격한 성장이 예상된다. 세계 로봇시장의 규모는 '12년 기준으로 133억불이고, '07년~'12년 까지 연평균 11%씩 성장해왔다. 세계로봇 연맹(IFR)에서는 세계 로봇시장의 규모를 제조용 로봇과 서비스용 로봇으로 나눠서 설명했는데, 제조용 로봇의 성장률이 8%인데 반해, 특히 서비스용 로봇의 성장률이 연평균 16%이라는 점이 눈에 띈다. 이러한 서비스용 무인 이동 장치의 경우 자기장이 외란된 환경에서 정확한 임무 수행을 위해서는 방향각 추정을 위한 저가형 AHRS은 필수적이다.
저가형 AHRS를 적용할 수 있는 제품인 무인 이동 장치 산업은 성장기에 들어선 것으로 보인다. 저가형 AHRS가 적용된 제품 중 하나인 무인 헬기 등은 기본적인 개념 확립 및 시제품 제작 단계를 넘어서, 양산 단계에 있다.
본 발명의 일 실시 예에 따르면 무인 이동 장치의 자세를 예측하고, 무인 이동 장치의 자세에 관한 오차 공분산을 이용하여 예측된 자세에 관한 값을 보정하는 단계를 통해 무인 이동 장치의 단가를 낮출 수 있는 무인 이동 장치를 개시한다.
본 발명의 일 실시 예에 따른 무인 이동 장치는 자세추정 시스템을 포함하는 무인 이동 장치로, 상기 무인 이동 장치의 각속도 및 가속도를 측정하는 가속도 센서, 상기 무인 이동 장치의 지자계 자속 밀도를 측정하는 지자계 센서, 및 상기 가속도 센서 및 지자계 센서로부터 획득한 측정 값에 기초하여 상기 무인 이동 장치의 자세를 추정하는 제어부를 포함한다.
본 발명의 일 실시 예에 따른 무인 이동 장치는 무인 이동 장치의 자세를 예측하고, 무인 이동 장치의 자세에 관한 오차 공분산을 이용하여 예측된 자세에 관한 값을 보정하는 단계를 통해 성능이 비교적 떨어지는 저가의 센서를 이용하더라도 높은 정확도의 자세 추정을 수행할 수 있다.
도 1은 본 발명의 일 실시 예에 따른 무인 이동 장치의 구성을 개략적으로 보이는 블록도이다.
도 2는 본 발명의 일 실시 예에 따른 무인 이동 장치의 자체 추정 방법을 나타내는 흐름도이다.
이하에서는 도면을 참조하여 본 발명의 구체적인 실시예를 상세하게 설명한다. 그러나, 본 발명의 사상은 이하에 제시되는 구체적인 실시예로 제한되지 아니하며, 본 발명의 사상을 이해하는 당업자는 동일한 사상의 범위 내에 포함되는 다른 실시예를 구성요소의 부가, 변경, 삭제, 및 추가 등에 의해서 용이하게 제안할 수 있을 것이나, 이 또한 본 발명의 사상에 포함된다고 할 것이다.
도 1은 본 발명의 일 실시 예에 따른 무인 이동 장치의 구성을 개략적으로 보이는 블록도이다. 무인 이동 장치는 예를 들면 무인 비행체, 무인 농기계, 무인 구조 로봇, 무인 군사 로봇, 무인 자동차, 무인 감시 로봇일 수 있다.
도 1을 참조하면, 본 발명의 일 실시 예에 따른 무인 이동 장치(100)는 센서부(110) 및 제어부(120)을 포함할 수 있다. 그리고 센서부(110)는 GPS(Global Positioning System) 센서(111), 관성 센서(112), 및 지자계 센서(113) 중 적어도 어느 하나를 포함할 수 있다.
GPS 센서(111)는 무인 이동 장치(100)의 위치를 판단할 수 있다. 구체적으로 GPS 센서(111)는 인공 위성으로부터 무인 이동 장치(100)의 위치를 좌표화 하기 위한 데이터를 수신하고, 데이터를 연산하여 좌표화된 무인 이동 장치(100)의 위치를 감지할 수 있다. GPS 센서(111)는 3대의 GPS 위성과의 거리를 통해 현재 위치를 계산할 수 있다. 자세한 알고리즘은 통상의 기술자에게 자명한 내용으로 여기에서는 자세한 설명을 생략한다.
관성 센서(112)는 무인 이동 장치(100)의 가속도를 측정할 수 있다. 구체적으로 관성 센서(112)는 무인 이동 장치(100)의 관성력을 검출하여 무인 이동 장치(100)가 얼마만큼의 힘을 받고 있는지 측정할 수 있다. 이때 측정된 힘은 무인 이동 장치의 가속도를 측정하는데 이용될 수 있다.
이때 관성 센서(112)는 가속도 센서 및 자이로 센서를 포함할 수 있다. 가속도 센서는 지구의 중력 가속도를 기준으로 사물이 얼마만큼의 힘을 받고 있는지를 측정하는 센서이다. 즉 가속도 센서는 사물이 가만히 있을 때 사물에 작용하는 중력 가속도를 x, y, z 축으로 벡터 3개로 나누어 크기를 측정한다. 그리고 기울어져 있을 때의 중력 가속도를 x, y, z 축으로 나누어 측정한다. 중력 가속도는 고정된 값으로, 중력 가속도는 x, y, z 벡터의 합으로 나타낼 수 있다. 다시 말해서, 가속도 센서는 특정 자세에서의 중력 가속도를 x, y, z 축으로 분할한 벡터값을 측정하여 사물의 가속도를 측정할 수 있다. 가속도 센서는 사물의 기울어진 정도를 파악하거나, 진동을 파악하는데 사용될 수 있다.
자이로 센서는 사물의 각속도를 측정하는 센서이다. 자이로 센서는 물체가 회전하는 경우 회전 방향과 수직으로 발생하는 코리올리의 힘(전향력)을 측정하여 물체의 각속도를 측정한다. 가속도 센서만을 이용하는 경우, 3축에 대한 회전각 중 지표면에 수직인 면에 대해 회전하는 각(방위각)을 측정할 수 없다. 따라서, 자이로 센서는 가속도 센서로 측정할 수 없는 방위각에 대한 정보를 제공한다. 또한, 자이로 센서는 모든 축에 대한 회전각 정보를 제공할 수 있다.
지자계 센서(113)는 지구 자기장의 세기 및 방향을 측정하는 센서이다. 구체적으로 지자계 센서(113)는 지구 자기장에 의한 북쪽인 자북 방향을 측정하고 또한 지구 자기장의 세기를 측정할 수 있다.
제어부(120)는 센서부(110)로부터 획득한 데이터를 이용하여 무인 이동 장치(100)의 자세 및 위치를 추정할 수 있다.
상세한 추정 방법은 도 2를 참조하여 설명한다. 일 실시 예에서 제어부(120) 및 센서부(110)는 각각이 독립적인 하드웨어 칩일 수 있다. 또 다른 실시 예에서, 제어부(120) 및 센서부(110)는 물리적으로 통합된 하나의 하드웨어 칩일 수 있다.
본 발명의 일 실시 예에 따른 무인 이동 장치(100)는 상술한 센서부(110) 및 제어부(120)외 추가적인 구성을 포함할 수 있다. 예를 들면, 무인 이동 장치(100)는 제어부(120)의 제어를 받아 무인 이동 장치를 이동시키는 이동 수단을 포함할 수 있다. 또한, 무인 이동 장치(100)는 특정 용도에 따라 필수적인 추가 수단을 포함할 수 있다.
도 2는 본 발명의 일 실시 예에 따른 무인 이동 장치의 자체 추정 방법을 나타내는 흐름도이다. 첨부된 도면의 도 2a와 도2b를 합쳐 도 2라고 지칭한다. 도 2에서 설명하고 있는 흐름도는 한번의 수행으로 바로 종료되는 것이 아닌, 반복적으로 루프를 도는 흐름도일 수 있다. 다시 말해서, 무인 이동 장치(100)는 기 설정된 시간 간격에 따라 도 2의 흐름도를 반복하여 수행함으로써 무인 이동 장치(100)의 자세를 계속적으로 추정할 수 있다.
무인 이동 장치의 제어부(120)는 자세 추정 시스템의 초기화가 필요한지 여부를 판단한다(S1001). 자세 추정 시스템의 초기화는 필요할 수도 있고 필요하지 않을 수도 있는바, 모든 경우에서 시스템을 초기화하는 것은 효율적이지 않을 수 있다. 따라서, 제어부(120)는 자세 추정 시스템의 알고리즘을 시작할 때 시스템의 초기화가 필요한지 여부를 판단할 수 있다.
일 실시 예에서 시스템의 초기화가 필요한 경우, 제어부(120)는 알고리즘 관련 변수 및 자세 정보를 초기화한다(S1003). 이때 제어부(120)가 초기화하는 변수는 초기 자세 정보, 오차 공분산 초기값, 시스템 모델의 오차 공분산, 가속도 센서의 측정 오차 공분산, 지자계 센서의 측정 오차 공분산, 직전 선형가속도일 수 있다.
구체적인 예를 들면, 초기 자세 정보는 시스템에 기 설정된 무인 이동 장치(100)의 기준 자세 정보일 수 있다. 또 다른 예를 들면 초기 자세 정보는 바로 직전의 루프에서 획득한 무인 이동 장치의 자세 정보일 수 있다.
또한, 오차 공분산 초기값, 시스템 모델의 오차 공분산, 가속도 센서의 측정 오차 공분산, 지자계 센서의 측정 오차 공분산 값은 특정 상수일 수 있다. 다시 말해서 상술한 오차 공분산 값들은 자세 추정 시스템에 따라 특정된 상수 값일 수 있다.
일 실시 예에서 직선 선형 가속도는 직전 루프에서 획득한 선형 가속도 값일 수 있다. 또 다른 일 실시 예에서 직선 선형 가속도는 0(zero)일 수 있다.
또 다른 실시 예에서 제어부(120)가 자세 추정 시스템의 초기화가 필요하지 않다고 판단하는 경우, 제어부(120)는 직전 루프에서의 자세 추정값 및 직전 루프에서의 오차 공분산 값을 갱신한다(S1005). 다시 말해서, 제어부(120)는 시스템의 초기값으로 직전 루프에서 획득한 자세 추정값 및 오차 공분산 값을 이용한다.
시스템 초기화가 완료되면, 제어부(120)는 타이머 초기화 및 정보 갱신 주기를 계산한다(S1007). 이때 타이머 초기화는 루프가 한번 도는 단위 시간을 알기 위한 것이다.
제어부(120)는 각속도 적분을 통해 무인 이동 장치의 자세를 예측한다(S1009). 구체적으로 제어부(120)는 관성 센서(112)를 통해 무인 이동 장치의 각속도를 측정한다. 그리고 제어부(120)는 측정된 각속도를 적분하여 무인 이동 장치의 자세를 예측한다. 예측 값은 (roll, pitch, yaw)로 나타낼 수 있다. roll, pitch, yaw는 각각의 축에 대한 오일러 각이다. 구체적으로 roll은 x축에 대한 회전각이고, pitch는 y축에 대한 회전각이고, yaw는 z축에 대한 회전각이다.
제어부(120)는 각속도 값에 근거하여 예측한 자세 값에 대하여 1차 오차 공분산 갱신을 수행한다(S1011). 이때 1차 오차 공분산 갱신은 칼만 필터(Kalman filter)를 이용하여 수행할 수 있다. 오차 공분산 갱신을 위한 구체적인 수식은 아래 수학식 1과 같다.
Figure 112016094359450-pat00001
여기에서, P1은 1차 갱신된 오차 공분산 값이다. 그리고 F는 예측 모델 값으로 구체적으로 자세 정보와 각속도 간의 역학적 관계를 나타내는 값일 수 있다. FT는 예측 모델의 전치 행렬이다. 그리고 Pprev 는 이전 루프에서 획득한 오차 공분산 값이다.
1차 오차 공분산의 갱신이 완료되면, 제어부(120)는 수학식 2를 통해 무인 이동 장치의 선형 가속도를 예측한다(S1013). 구체적으로 제어부(120)는 센서부(110)로부터 획득한 가속도 값에 기초하여 무인 이동 장치의 선형 가속도를 예측할 수 있다.
Figure 112016094359450-pat00002
여기에서, Alinear는 예측된 선형 가속도이다. Ca는 모델 상수 값으로 0과 1 사이 값이다. Na는 모델의 오차와 관련된 상수 값으로 0보다 큰 값이다. Alinear , prev는 직전 루프에서 획득한 선형 가속도 값이다.
제어부(120)는 수학식 3을 통해 무인 이동 장치의 중력 가속도를 예측한다. 다시 말해서 제어부(120)는 수학식 3을 통해 무인 이동 장치가 중력에 대하여 얼마나 기울어져 있는지를 예측한다. 결과적으로 제어부(120)는 수학식 3을 통해 예측된 roll 과 pitch를 획득할 수 있다.
Figure 112016094359450-pat00003
여기에서, Ag는 중력가속도이다. 그리고 Cn b는 방향 코사인 행렬이다. Asensor는 가속도 센서에서 측정된 가속도 값이다. Alinear는 수학식 2에 따라 예측된 선형 가속도 값이다.
제어부(120)는 측정된 가속도가 임계치보다 작은지 여부를 판단한다(S1017). 측정된 가속도가 너무 빠르면 해당 값을 신뢰할 수 없다. 구체적으로 선형 가속도값이 크면, 중력가속도와의 합벡터가 수평으로 측정될 수 있다. 이때의 선형 가속도 값은 자세를 추정하는데 노이즈로 작용되어 정확한 자세 추정을 방해할 수 있는바, 제어부(120)는 임계치보다 큰 가속도 값을 버릴 수 있다. 예를 들면 임계치는 중력 가속도와 선형 가속도를 합하여 10.5로 설정될 수 있다.
일 실시 예에서, 측정 가속도가 임계치보다 큰 경우, 제어부(120)는 해당 측정 가속도 값을 의미 없는 값으로 판단하여 버리고 바로 단계 S1025로 넘어간다.
일 실시 예에서, 측정 가속도가 임계치보다 작은 경우 제어부는 제1 가중치를 수학식 4를 통해 계산한다(S1019).
Figure 112016094359450-pat00004
여기에서, Kaccel은 제1 가중치이다. P1은 단계 S1011에서 1차 갱신된 오차 공분산값이다. Haccel은 측정 모델로 자세 정보와 중력 가속도간의 역학적 관계를 나타낸다. Haccel T는 측정 모델의 전치 행렬이다. Raccel은 가속도 센서의 측정 오차 공분산이다.
제어부(120)는 단계 S1015에서 예측된 중력 가속도 값 및 단계 S1019에서 계산된 제1 가중치 값을 이용하여 roll과 pitch를 보정한다(S1021). 구체적으로 제어부(120)는 수학식 5를 통해 roll과 pitch를 보정한다.
Figure 112016094359450-pat00005
여기에서 X는 무인 이동 장치의 자세 정보이다. 다시 말해서, X는 단계 S1009에서 예측된 자세 정보 값을 중력 가속도 및 가중치를 이용하여 보정한 자세 정보 값이다. Xprev는 이전 루프에서 획득한 자세 정보이다. Kaccel은 제1 가중치 값이다. Asensor는 가속도 센서에서 측정된 무인 이동 장치의 가속도이다. Ag는 중력가속도이다. Alinear는 단계 S1013에서 예측된 선형가속도이다.
제어부(120)는 가중치를 이용하여 2차 오차 공분산 갱신을 수행한다(S1023). 구체적으로 제어부(120)는 수학식 6을 통해 2차 오차 공분산 갱신을 수행한다.
Figure 112016094359450-pat00006
P2는 2차 갱신 오차 공분산 값이다. 제어부(120)는 오차 공분산 값을 갱신하여 무인 이동 장치의 자세를 보다 정확하게 추정할 수 있다. Kaccel는 제1 가중치이다. P1은 1차 갱신 오차 공분산 값이다. Haccel은 측정 모델로서 자세 정보와 중력 가속도 간의 역학적 관계를 나타낸다.
제어부(120)는 지자계 자속 밀도(earth magnetic flow density)를 계산한다(S1025). 구체적으로 제어부(120)는 지자계 센서(113)로부터 획득한 자속 밀도 값에 기초하여 지자계 자속 밀도를 계산한다.
제어부(120)는 특정된 지표면과 지구 자기장이 이루는 각도(측정 경사각,
Figure 112016094359450-pat00007
)와 기준 지표면과 지구 자기장이 이루는 각도(기준 경사각,
Figure 112016094359450-pat00008
)간의 차가 경사각 임계치(y1)보다 작은 값인지 여부를 판단한다(S1027). 구체적으로 제어부(120)는 측정 경사각이 일정 값 이상인 경우 노이즈로 판단하여 해당 값을 버린다. 일 실시 예에서 측정 경사각과 기준 경사각간의 차가 경사각 임계치를 초과하는 경우 제어부(120)는 yaw를 보정하지 않고 바로 단계 S1043으로 넘어간다.
제어부(120)는 자속 밀도의 세기(B)와 기준 자속 밀도 세기(B0)간의 비율이 측정 자속 밀도의 임계치(y2)보다 작은 값인지 여부를 판단한다(S1029). 구체적으로 제어부(120)는 측정된 자속 밀도의 세기를 기준 자속 밀도 세기로 나눈 값이 일정 값 이상인 경우 노이즈로 판단하여 해당 값을 버린다. 일 실시 예에서, 측정 자속 밀도와 기준 자속 밀도간 비율이 자속 밀도 임계치를 초과하는 경우, 제어부(120)는 yaw를 보정하지 않고 바로 단계 S1043으로 넘어간다.
제어부(120)는 추정값 갱신 주기 동안 지자계 센서로 측정한 방향각의 변화량(
Figure 112016094359450-pat00009
)과 추정값 갱신 주기 동안 각속도 센서로 측정한 방향각의 변화량(
Figure 112016094359450-pat00010
)간의 차가 추정값 갱신 주기 동안의 지자계 센서 및 각속도 센서로 측정한 방향각의 변화량 임계치보다 작은 값인지 여부를 판단한다(S1031). 구체적으로 제어부(120)는 일 실시 예에서, 추정값 갱신 주기 동안 지자계 센서로 측정한 방향각의 변화량(
Figure 112016094359450-pat00011
)과 추정값 갱신 주기 동안 각속도 센서로 측정한 방향각의 변화량(
Figure 112016094359450-pat00012
)간의 차가 추정값 갱신 주기 동안의 지자계 센서 및 각속도 센서로 측정한 방향각의 변화량 임계치를 초과하는 경우, 제어부(120)는 yaw를 보정하지 않고 바로 단계 S1043으로 넘어간다.
일 실시 예에서, 단계 S1027, S1029, S1031의 각각의 값들이 각각의 임계치(y1, y2, y3)보다 작은 경우, 제어부(120)는 지자계 센서로부터 획득한 값을 이용하여 방위각(
Figure 112016094359450-pat00013
)을 계산한다(S1033). 여기에서 방위각이란, 지표면에 수직인 면에 대해 회전하는 각이다. 방위각은 yaw값을 보정하는데 이용될 수 있다.
제어부(120)는 현재 지자계 센서로 측정한 방위각(
Figure 112016094359450-pat00014
)과 비교 기준이 되는 방위각(
Figure 112016094359450-pat00015
)간의 차가 지자계로 측정한 방위각의 신뢰 임계치(y4)보다 작은지 여부를 판단한다(S1035). 일 실시 예에서, 현재 지자계 센서로 측정한 방위각(
Figure 112016094359450-pat00016
)과 비교 기준이 되는 방위각(
Figure 112016094359450-pat00017
)간의 차가 지자계로 측정한 방위각의 신뢰 임계치(y4)를 초과하는 경우, 제어부(120)는 yaw를 보정하지 않고 바로 단계 S1043으로 넘어간다.
일 실시 예에서, 현재 지자계 센서로 측정한 방위각(
Figure 112016094359450-pat00018
)과 비교 기준이 되는 방위각(
Figure 112016094359450-pat00019
)간의 차가 지자계로 측정한 방위각의 신뢰 임계치(y4)보다 작은 경우, 제어부(120)는 제2 가중치를 계산한다. 제어부는 수학식 7을 통해 제2 가중치를 계산할 수 있다.
Figure 112016094359450-pat00020
Kmag는 제2 가중치 이다. P2는 2차 갱신 오차 공분산이다. Hmag는 측정 모델로서, 자세 정보와 지자계 센서간의 역학적 관계를 나타낸다. Hmag T는 측정 모델의 전치행렬이다. Rmag는 지자계 센서의 측정 오차 공분산이다.
제어부(120)는 제2 가중치 및 방위각을 이용하여 yaw를 보정한다(S1039). 구체적으로 제어부(120)는 수학식 8을 통해 yaw를 보정한다.
Figure 112016094359450-pat00021
Figure 112016094359450-pat00022
는 자세 정보이다. 특히 yaw를 나타낸다.
Figure 112016094359450-pat00023
는 예측된 자세 정보를 나타낸다. 구체적으로
Figure 112016094359450-pat00024
는 단계 S1009에서 예측된 yaw를 나타낸다. Kmag는 제2 가중치를 나타낸다.
Figure 112016094359450-pat00025
는 지자계 센서를 이용하여 계산된 방위각이다.
제어부(120)는 상술한 단계를 통해 보정된 자세 추정값 및 오차 공분간 값을 이용하여 자세 추정 시스템의 자세 추정 값 및 오차 공분산 값을 갱신한다(S1043). 구체적으로 제어부(120)는 수학식 9를 통해 최종 오차 공분산 값을 갱신할 수 있다.
Figure 112016094359450-pat00026
여기에서, P는 최종 갱신된 오차 공분산 값이다. P2는 2차 갱신 오차 공분산이다. Kmag는 제2 가중치이다. Hmag는 측정 모델로서, 자세 정보와 중력 가속도 간의 역학적 관계를 나타낸다. 최종 오차 공분산은 다음 루프에서 초기값으로 이용될 수 있다.
상술한 알고리즘을 통해, 본 발명의 일 실시 예에 따른 무인 이동 장치(100)는 비교적 정확도가 떨어지는 센서 모듈을 이용하는 상황에서도 무인 이동 장치(100)의 자세(roll, pitch, yaw)를 보다 정확하게 추정할 수 있다. 결과적으로, 본 발명의 일 실시 예에 따른 알고리즘은 고가의 센서를 이용하지 않고도 비교적 정확하게 무인 이동 장치(100)의 자세를 추정할 수 있는바 생산 단가를 낮추는 효과가 있다.
지금까지, 본 발명의 이해를 돕기 위하여 무인 이동 장치에 대한 예시적인 실시 예가 설명되고 첨부된 도면에 도시되었다. 그러나 이러한 실시 예는 단지 본 발명을 예시 하기 위한 것이고, 이를 제한하지 않는다는 점이 이해되어야 할 것이다. 그리고 본 발명을 도시되고 설명된 내용에 국한되지 않는다는 점이 이해되어야 할 것이다. 이는 다양한 변형이 본 기술 분야에서 통상의 지식을 가진 자에게 일어날 수 있기 때문이다.

Claims (7)

  1. 자세추정 시스템을 포함하는 무인 이동 장치로,
    상기 무인 이동 장치의 각속도 및 가속도를 측정하는 관성 센서;
    상기 무인 이동 장치의 지자계 자속 밀도를 측정하는 지자계 센서; 및
    상기 가속도 센서 및 지자계 센서로부터 획득한 측정 값에 기초하여 상기 무인 이동 장치의 자세를 추정하는 제어부를 포함하고,
    상기 무인 이동 장치의 자세는 3차원 공간의 3축인 x축, y축, z축에 대하여 x축에 대한 무인 이동 장치의 회전각인 제1 값, y축에 대한 무인 이동 장치의 회전각인 제2 값, 및 z축에 대한 무인 이동 장치의 회전각인 제3 값으로 표현되고,
    상기 제어부는 상기 가속도 센서로부터 획득한 값을 이용하여 제1 값 및 제2 값을 추정하고, 상기 지자계 센서로부터 획득한 값을 이용하여 제3 값을 추정하고,
    상기 제어부는 무인 이동 장치의 추정 자세에 대한 초기 오차 공분산 값에 기초하여 제1 오차 공분산 값을 갱신하고, 상기 갱신된 제1 오차 공분산 값에 기초하여 제1 가중치 값을 획득하고, 상기 제1 오차 공분산에 기초하여 제2 오차 공분산을 갱신하고, 상기 제2 오차 공분산에 기초하여 제2 가중치 값을 획득하고, 상기 제1 가중치 값에 기초하여 상기 제1 값 및 제2 값을 보정하고, 상기 제2 가중치 값에 기초하여 상기 제3 값을 보정하며,
    상기 제어부는
    상기 관성 센서를 통해 획득한 측정 경사각과 기준 경사각간의 차인 제1 비교값이 제1 임계치 이상이면 상기 제3 값을 보정하지 않고,
    상기 지자계 센서를 통해 획득한 자속 밀도의 세기와 기준 자속 밀도 세기간의 비인 제2 비교값이 제2 임계치 이상이면 상기 제3 값을 보정하지 않고,
    갱신 주기 동안 상기 지자계 센서로 측정한 방향각 변화량과 상기 갱신 주기 동안 상기 관성 센서로 측정한 방향각 변화량의 차인 제3 비교값이 제3 임계치 이상이면 상기 제3 값을 보정하지 않고,
    상기 제1 비교값이 제1 임계치 미만이고, 상기 제2 비교값이 제2 임계치 미만이고, 상기 제3 비교값이 제3 비교값 미만인 경우 상기 지자계 센서로부터 획득한 값을 이용하여 방위각을 산출하고, 산출된 방위각과 기준 방위각간의 차인 제4 비교값이 제4 임계치 이상이면 상기 제3 값을 보정하지 않고, 상기 제4 비교값이 제4 임계치 미만인 경우 상기 제2 가중치 값을 획득하여 상기 제2 가중치 값과 상기 방위각을 이용하여 상기 제3 값을 보정하는
    무인 이동 장치.
  2. 삭제
  3. 삭제
  4. 삭제
  5. 삭제
  6. 삭제
  7. 제1항에 있어서,
    상기 제어부는 상기 관성 센서를 통해 획득한 무인 이동 장치의 가속도 값이 특정 임계값 보다 작은 경우에만 상기 가속도 값을 이용하여 무인 이동 장치의 자세를 추정하는
    무인 이동 장치.
KR1020160125146A 2016-09-28 2016-09-28 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치 KR101941009B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020160125146A KR101941009B1 (ko) 2016-09-28 2016-09-28 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020160125146A KR101941009B1 (ko) 2016-09-28 2016-09-28 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치

Publications (2)

Publication Number Publication Date
KR20180035090A KR20180035090A (ko) 2018-04-05
KR101941009B1 true KR101941009B1 (ko) 2019-01-22

Family

ID=61977332

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020160125146A KR101941009B1 (ko) 2016-09-28 2016-09-28 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치

Country Status (1)

Country Link
KR (1) KR101941009B1 (ko)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020040331A1 (ko) * 2018-08-23 2020-02-27 한서대학교 산학협력단 융합필터를 이용한 쿼드콥터 자세 제어 시스템
KR102290043B1 (ko) * 2019-10-25 2021-08-17 주식회사 그린맥스 자계기반의 지능형 고효율 6wd 농자재 수송차량

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101564020B1 (ko) * 2013-07-26 2015-10-28 삼성중공업(주) 이동체의 전자세 예측 방법 및 이를 이용한 전자세 예측 장치

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6268945B2 (ja) * 2013-02-04 2018-01-31 株式会社リコー 慣性装置、方法及びプログラム

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101564020B1 (ko) * 2013-07-26 2015-10-28 삼성중공업(주) 이동체의 전자세 예측 방법 및 이를 이용한 전자세 예측 장치

Also Published As

Publication number Publication date
KR20180035090A (ko) 2018-04-05

Similar Documents

Publication Publication Date Title
CN108731667B (zh) 用于确定无人驾驶车辆的速度和位姿的方法和装置
Nebot et al. Initial calibration and alignment of low‐cost inertial navigation units for land vehicle applications
US7979231B2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
CN106990426B (zh) 一种导航方法和导航装置
JP5328252B2 (ja) ナビゲーションシステムの位置検出装置および位置検出方法
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
Ahn et al. Fast alignment using rotation vector and adaptive Kalman filter
US11408735B2 (en) Positioning system and positioning method
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN112857398B (zh) 一种系泊状态下舰船的快速初始对准方法和装置
Sokolović et al. INS/GPS navigation system based on MEMS technologies
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
US20200293067A1 (en) State estimation
Noureldin et al. Inertial navigation system
KR101941009B1 (ko) 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치
JP5219547B2 (ja) 車載ナビゲーションシステム及びナビゲーション方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
Tomaszewski et al. Concept of AHRS algorithm designed for platform independent IMU attitude alignment
JP2021527213A (ja) 物体のジャイロメータを較正する方法
Pourtakdoust et al. An adaptive unscented Kalman filter for quaternion‐based orientation estimation in low‐cost AHRS
Ercan et al. Multi-sensor data fusion of DCM based orientation estimation for land vehicles
CN110375773B (zh) Mems惯导***姿态初始化方法
Munguia et al. An attitude and heading reference system (AHRS) based in a dual filter
KR101916908B1 (ko) 위치 추정 시스템 및 위치 추정 시스템을 포함하는 무인 이동 장치

Legal Events

Date Code Title Description
A201 Request for examination
E902 Notification of reason for refusal
AMND Amendment
E601 Decision to refuse application
AMND Amendment
X701 Decision to grant (after re-examination)
GRNT Written decision to grant