KR102017404B1 - 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법 - Google Patents

9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법 Download PDF

Info

Publication number
KR102017404B1
KR102017404B1 KR1020177023677A KR20177023677A KR102017404B1 KR 102017404 B1 KR102017404 B1 KR 102017404B1 KR 1020177023677 A KR1020177023677 A KR 1020177023677A KR 20177023677 A KR20177023677 A KR 20177023677A KR 102017404 B1 KR102017404 B1 KR 102017404B1
Authority
KR
South Korea
Prior art keywords
angle
vehicle
gyroscope
model
angular velocity
Prior art date
Application number
KR1020177023677A
Other languages
English (en)
Other versions
KR20170104621A (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 샹하이 화처 네비게이션 테크놀로지 엘티디.
Publication of KR20170104621A publication Critical patent/KR20170104621A/ko
Application granted granted Critical
Publication of KR102017404B1 publication Critical patent/KR102017404B1/ko

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A01B69/007—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow
    • A01B69/008—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow automatic
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B79/00—Methods for working soil
    • A01B79/005—Precision agriculture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B81—MICROSTRUCTURAL TECHNOLOGY
    • B81B—MICROSTRUCTURAL DEVICES OR SYSTEMS, e.g. MICROMECHANICAL DEVICES
    • B81B7/00—Microstructural systems; Auxiliary parts of microstructural devices or systems
    • B81B7/02—Microstructural systems; Auxiliary parts of microstructural devices or systems containing distinct electrical or optical devices of particular relevance for their function, e.g. microelectro-mechanical systems [MEMS]
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C19/00—Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1654—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with electromagnetic compass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Environmental Sciences (AREA)
  • Mechanical Engineering (AREA)
  • Soil Sciences (AREA)
  • Electromagnetism (AREA)
  • Manufacturing & Machinery (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Geology (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • Computer Hardware Design (AREA)
  • Microelectronics & Electronic Packaging (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법은, 자이로스코프의 오차 모델, 전자 나침반 교정 타원 모델 및 7차원 EKF 필터링 모델을 설정하는 단계, 및 차량 운동 자세에 대응하는 파라미터 벡터 설정 단계(S1); 차량의 운동의 가속도 및 각속도, 그리고 지자기장 세기를 포함하는 데이터를 획득하는 단계(S2); 자이로스코프 오차 모델 및 전자 나침반 교정 타원 모델에 의해 차량의 각도, 속도, 위치 정보 및 코스 각을 계산하는 단계(S3); 및 차량의 각도, 속도, 위치 정보 및 코스 각은 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되는 단계, 및 차량의 운동 자세 각도가 실시간으로 갱신되는 단계를 포함한다. 이와 같은 방법에 의하면, 작은 오차 높은 정확성 및 신뢰성이 보장된다.

Description

9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법
본 발명은 측정 기술 분야에 관한 것으로, 특히, 9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법에 관한 것이다.
최근, 농업을 지원하는 다른 모든 기술 중에서 MEMS(Micro-Electro-Mechanical-System) 센서 기술(sensor technology), 항법 기술(navigation technology), 및 제어 기술(control technology)의 발전으로 인하여, 정밀 농업이 점차 인기가 상승하는 경향에 있다. 농기계의 기계 보조 주행 중, 피치 각도, 롤링 각도, 및 코스 각도를 포함하는 차량의 다양한 측면에 관한 정보는 높은 정밀 통합 항법 및 제어 알고리즘을 위한 중요한 데이터 입력을 제공할 수 있다.
현재, 관성 항법 시스템은 PINS(Platform Inertial Navigation System) 및 SINS(Strapdown Inertial Navigation System)를 포함한다. PINS와 비교하여 SINS는 IMU(관성 측정 장치: Inertial Measuring Unit) 센서를 사용하여 PINS를 대체할 수 있도록 "수학적 플랫폼(mathematical platform)"을 계산으로 설정한다. SINS는 일반적으로 항공기 항법 제어 시스템에 사용된다. 그러나 농기계 제어 분야에서는 SINS의 연구와 응용이 아직 초기 단계에 있다. 또한, 항공기 항법 제어 시스템은 적용 대상 및 환경 조건과 관련하여 농기계 제어 시스템과 크게 다르다. 항공기 항법 제어 시스템에 구현 된 SINS 방법은 농기계 제어 시스템에 적용할 수 없다.
농기계에서 관성 항법을 적용할 때의 전술한 결함을 고려하여, 본 발명은 작은 오차, 높은 정밀도, 및 신뢰성을 가지는 9축 MEMS 센서에 기초한 농기계의 전-자세 각도를 갱신하는 방법을 제공한다.
상기 목적을 달성하기 위해, 본 발명의 실시 예는 다음의 기술적 해결책을 채택한다.
본 발명의 9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법은 다음 단계를 포함한다:
자이로스코프의 오차 모델, 전자 나침반 교정 타원 모델 및 7차원 EKF 필터링 모델을 설정하는 단계, 및 차량 운동 자세에 대응하는 파라미터 벡터 설정 단계;
9축 MEMS 센서를 통해 상기 차량 운동의 가속도 및 각속도, 그리고 지자기장 세기를 포함하는 데이터를 획득하는 단계;
상기 차량 운동의 가속도 및 각속도 그리고 상기 지자기장 세기를 포함하여 상기 획득된 데이터에 따라 설정된 상기 자이로스코프의 오차 모델 및 상기 전자 나침반 교정 타원 모델에 의해 상기 차량의 각도, 속도, 위치 정보 및 코스 각도를 계산하는 단계; 및
상기 차량의 상기 각도, 상기 속도, 상기 위치 정보 및 상기 코스 각도는 상기 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되는 단계, 및 상기 차량의 운동 자세 각도를 실시간으로 갱신하는 단계를 포함하고,
상기 9축 MEMS 센서는 3축 자이로스코프, 3축 가속도계 및 3축 지자기 센서로 구성된다.
본 발명의 일 측면에 따르면, 상기 자이로스코프의 오차 모델, 전자 나침반 교정 타원 모델 및 7차원 EKF 필터링 모델을 설정하는 단계, 및 차량 운동 자세에 대응하는 파라미터 벡터 설정 단계는 다음과 같다:
상기 자이로스코프의 상기 오차 모델에서 상기 자이로스코프의 오차 계산식을 통해 상기 자이로스코프의 상기 각속도를 계산하는 단계; 상기 자이로스코프의 상기 오차 계산식은,
Figure 112017082029351-pct00001
이며, 여기서,
Figure 112017082029351-pct00002
는 상기 자이로스코프에 의해 출력 된 각속도이고,
Figure 112017082029351-pct00003
는 상기 자이로스코프의 실제 각속도이며,
Figure 112017082029351-pct00004
는 상기 자이로스코프의 제로 드리프트이며,
Figure 112017082029351-pct00005
는 상기 자이로스코프에 의해 출력되는 백색 잡음이며,
전자 나침반 교정 타원 모델에 의해 자기장 간섭을 제거하는 단계; 상기 전자 나침반 교정 타원 모델은,
Figure 112017082029351-pct00006
이며, 여기서
Figure 112017082029351-pct00007
는 자기장 세기이고,
Figure 112017082029351-pct00008
및
Figure 112017082029351-pct00009
은 하드 자기 간섭이며,
Figure 112017082029351-pct00010
및
Figure 112017082029351-pct00011
는 소프트 자기 간섭이고,
상기 7차원 상기 EKF 필터링 모델에 의해 상기 차량 운동 자세를 갱신하는 단계; 포함하고,
상기 7차원 EKF 필터링 모델은 7차원 상태 벡터에 대해 확장 칼만 필터(extended Kalman filter)를 사용하고, 상기 EKF는 상태 방정식 및 관측 방정식을 포함하며,
Figure 112017082029351-pct00012
,
여기서,
Figure 112019029768635-pct00013
는 쿼터니온 벡터
Figure 112019029768635-pct00234
,
Figure 112019029768635-pct00235
,
Figure 112019029768635-pct00236
,
Figure 112019029768635-pct00237
이고,
Figure 112019029768635-pct00018
은 XYZ 3축 자이로스코프의 제로 드리프트 이고, 여기서,
Figure 112019029768635-pct00019
는 상기 자이로스코프에 의해 출력 된 각속도,
Figure 112019029768635-pct00020
은 공정 잡음 행렬(process noise matrix),
Figure 112019029768635-pct00021
은 관측 잡음 행렬(observation noise matrix),
Figure 112019029768635-pct00022
는 관측 벡터(observation vector)이며,
Figure 112019029768635-pct00023
, 여기서, a는 3축 가속도 값이고,
Figure 112019029768635-pct00024
는 전자 나침판에 의해 계산 된 코스 각도(course angle)이며,
Figure 112017082029351-pct00025
Figure 112017082029351-pct00026
.
본 발명의 일 측면에 따르면, 상기 차량 운동의 가속도 및 각속도, 그리고 지자기장 세기를 포함하는 데이터를 9축 MEMS 센서를 통해 실시간으로 획득하는 단계는 다음과 같다:
상기 자이로스코프를 통해 상기 차량의 각속도를 획득하며, 상기 자이로스코프의 제로 드리프트를 보상하는 단계;
가속도 센서를 통하여 상기 차량의 가속도 데이터를 획득하는 단계; 및
상기 지자기 센서를 통하여 상기 차량의 상기 지자기장 세기 데이터를 획득하는 단계를 포함한다.
상기 차량 운동의 상기 가속도 및 상기 각속도 그리고 상기 지자기장 세기를 포함하여 상기 획득된 데이터에 따라 설정된 자이로스코프 상기 오차 모델 및 상기 전자 나침반 교정 타원 모델에 의해 상기 차량의 각도, 속도, 위치 정보 및 코스 각도를 계산하는 단계는 다음과 같다:
상기 자이로스코프의 오차 모델에 의한 각속도의 적분 계산에 의해 각도 데이터를 획득하는 단계;
상기 가속도 데이터를 적분하여 상기 속도를 계산하며, 상기 위치 정보는 상기 속도를 더 적분하여 계산되는 단계; 및
교정 파라미터에 의해 보상되고 경사각에 의해 보정 된 지자기장 세기 데이터로부터 상기 차량의 코스 각도를 계산하고, 상기 교정 파라미터 및 상기 경사각은 상기 타원 모델에 의해 계산되는 단계를 포함한다.
상기 차량의 상기 각도, 상기 속도, 상기 위치 정보 및 상기 코스 각도를 상기 7차원 EKF 필터링 모델에 의해 데이터 융합 처리하는 단계, 및 상기 차량의 상기 운동 자세 각도를 실시간으로 갱신하는 단계는 다음과 같다:
상기 차량의 자세 데이터를 쿼터니언 자세 갱신 알고리즘을 통해 상기 7차원 EKF 필터링 모델에 의해 계산하는 단계; 여기서 EKF 알고리즘의 계산 프로세스는 다음과 같고,
Figure 112017082029351-pct00027
,
여기서,
Figure 112019029768635-pct00028
는 샘플링 시점이고,
Figure 112019029768635-pct00029
는 시스템 상태 추정이며,
Figure 112019029768635-pct00030
는 이전 시점이고,
Figure 112019029768635-pct00031
는 이후 시점이며,
Figure 112019029768635-pct00032
는 상태 전이 행렬이고,
Figure 112019029768635-pct00238
는 최소 평균 제곱 오차 행렬이며,
Figure 112019029768635-pct00034
는 상태 벡터에 해당하는 공분산 행렬이며,
Figure 112019029768635-pct00239
는 오차 이득이며,
Figure 112019029768635-pct00240
는 관측 벡터이며,
Figure 112019029768635-pct00241
는 관측 방정식의 천이 행렬이며,
Figure 112019029768635-pct00242
는 관측 벡터에 해당하는 공분산 행렬이며,
Figure 112017082029351-pct00039
,
여기서,
Figure 112019029768635-pct00040
는 쿼터니온 벡터이고,
Figure 112019029768635-pct00243
,
Figure 112019029768635-pct00244
,
Figure 112019029768635-pct00245
,
Figure 112019029768635-pct00246
는 쿼터니언 벡터를 형성하는 스칼라이며,
Figure 112019029768635-pct00045
,
Figure 112019029768635-pct00046
,
Figure 112019029768635-pct00047
는 3 차원 좌표계의 단위 벡터이며, 갱신 된 자세 행렬은 다음과 같고,
Figure 112017082029351-pct00048
Figure 112017082029351-pct00049
는 캐리어 좌표계를 항법 좌표계로 변환하기 위한 회전 행렬이며,
Figure 112017082029351-pct00050
여기서,
Figure 112017082029351-pct00051
,
Figure 112017082029351-pct00052
,
Figure 112017082029351-pct00053
는 롤링 각도, 피치 각도 및 코스 각도이다.
상기 차량의 상기 각도, 상기 속도, 상기 위치 정보 및 상기 코스 각도는 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되는 단계, 및 상기 차량의 운동 자세 각도가 실시간으로 갱신되는 단계 후에 다음 단계가 이행된다:
갱신 된 상기 차량의 자세 데이터로부터 상기 차량의 전-자세 각도 데이터를 추출하여, 자세 각도 데이터의 값이 결정되며, 상기 차량의 전-자세 각도는 피치 각도, 롤링 각도 및 코스 각도를 포함하는 단계를 포함하고,
여기서,
Figure 112017082029351-pct00054
상기 코스 각도:
Figure 112017082029351-pct00055
상기 피치 각도:
Figure 112017082029351-pct00056
상기 롤링 각도:
Figure 112017082029351-pct00057
.
본 발명의 유리한 효과는 다음과 같다. 상기 물체의 움직임의 가속도 및 각속도는 MEMS 센서에 의해 실시간으로 획득된다. 자이로스코프에 의한 각가속도 출력이 적분되어 각도가 획득된다. 상기 가속도가 통합되어 속도를 계산하고, 이를 더 통합하여 상기 위치 정보가 계산된다. 상기 지자기장은 지자기 센서에 의해 얻어지며, 상기 코스 각도는 보상 알고리즘과 자이로스코프의 융합에 의해 계산된다. 다음으로, 자세가 전이 행렬로 변환되어, 캐리어 좌표계가 항법 좌표계로 변환된다. 이러한 전이 행렬 함수는 "수학적 플랫폼"으로 기능한다. SINS(스트랩다운 관성 항법 시스템) 알고리즘은 농기계에 적용되며, 전이 행렬이 특히 중요하다. 농기계가 계속 움직이기 때문에, 그 자세 또한 계속 변화하고 있다. 따라서 상기 전이 행렬 또한 지속적으로 다시 계산되고 갱신 될 필요가 있다. 기존의 자세 갱신 알고리즘으로는 오일러각 알고리즘, 방향 코사인 알고리즘, 쿼터니온 알고리즘 등이 있다. 오일러각 알고리즘과 비교할 때 쿼터니온 알고리즘에는 특이점이 없다. 상기 방향 코사인 알고리즘과 비교할 때 쿼터니언 알고리즘은 계산량이 적다. 따라서 쿼터니온 알고리즘은 임베디드 제품에 사용하기에 매우 적합하다. 상기 자이로스코프의 지자기장과 오차 모델은 농기계 평면에 설정되고, 7차원 EKF가 설정되어 자세 행렬이 갱신된다. 쿼터니온과 자이로스코프의 제로 오프셋이 추정된다. 다음에, 가속도 및 자기장 강도를 통해 산출 된 코스 각도로부터 관측을 행함으로써, 높은 정밀도의 3차원 자세 각도를 얻을 수 있다. 오류 보상 알고리즘과 정정 알고리즘은 SINS 알고리즘의 오류 간섭을 크게 줄인다. MEMS 센서 및 SINS 알고리즘은 본 발명이 높은 성능 파라미터를 갖는 것을 보장한다. 트랙터에 의해 시험 된 바와 같이, 출력 코스 각도의 오차는 0.1°미만이며, 피치 각도 오차 및 롤링 각도 오차는 0.01°미만이다. 상기 쿼터니온은 칼만 필터링 상태 벡터로서 사용되기 때문에, 상기 목표 파라미터의 계산 정확도가 더욱 향상될 수 있다.
본 발명의 실시 예의 기술적 해결책을 보다 명확하게 설명하기 위하여, 실시 예에 필요한 첨부 도면을 간단히 소개한다. 명백하게, 다음의 설명에서 첨부 된 도면은 본 발명의 일부 실시 예에 지나지 않는다. 당업자라면 창조적인 노력을 기울이지 않고도 첨부 된 도면으로부터 다른 첨부 도면을 도출할 수 있다.
도 1은 본 발명의 제1실시 예에 의한 9축 MEMS 센서에 기초한 농기계의 전-자체 각도 갱신 방법의 순서도이다.
도 2는 본 발명의 제2실시 예에 의한 9축 MEMS 센서에 기초한 농기계의 전-자세 각도를 갱신하는 방법의 순서도이다.
이하, 본 발명의 실시 예에 따른 첨부 도면을 참조하여 본 발명을 명확하고 완전하게 설명한다. 명백하게, 설명 될 실시 예는 모든 실시 예가 아니고 본 발명의 특정 실시 예에 지나지 않는다. 본 발명의 실시 예들에 기초하여, 당업자가 창의적인 노력 없이도 수행되는 다른 실시 예들은 모두 본 발명의 보호 범위에 속한다.
제1실시예:
도 1에 도시 된 바와 같이, 9축 MEMS 센서를 기반으로 농기계의 전-자세 각도(all-attitude angle)를 갱신하는 방법은 다음과 같은 단계를 포함한다.
단계(S1): 자이로스코프의 오차 모델(error model of the gyroscope), 전자 나침반 교정 타원 모델(electronic compass calibration ellipse model) 및 7차원 EKF 필터링 모델(seven-dimensional EKF filtering model)이 설정되고, 차량 운동 자세에 대응하는 파라미터 벡터가 설정된다.
자이로스코프의 오차 모델, 전자 나침반 교정 타원 모델 및 7차원 EKF 필터링 모델을 설정하는 단계, 및 차량 운동 자세에 대응하는 파라미터 벡터 설정 단계(S1)가 아래에서 자세히 설명된다.
자이로스코프의 오차 모델은 자이로스코프의 오차 계산식을 통해 자이로스코프의 각속도를 계산하며, 여기서 자이로스코프의 오차 계산식은 다음과 같다.
Figure 112017082029351-pct00058
, 여기서,
Figure 112017082029351-pct00059
는 자이로스코프에 의해 출력 된 각속도(angular velocity)이고,
Figure 112017082029351-pct00060
는 자이로스코프의 실제 각속도이며,
Figure 112017082029351-pct00061
는 자이로스코프의 제로 드리프트(zero drift)이며,
Figure 112017082029351-pct00062
는 자이로스코프에 의해 출력되는 백색 잡음(white noise)이다.
자기장 간섭은 전자 나침반 교정 타원 모델에 의해 제거되며, 여기서 전자 나침반 교정 타원 모델은 다음과 같다.
Figure 112017082029351-pct00063
, 여기서
Figure 112017082029351-pct00064
는 자기장 세기이고,
Figure 112017082029351-pct00065
및
Figure 112017082029351-pct00066
은 하드 자기 간섭이며,
Figure 112017082029351-pct00067
및
Figure 112017082029351-pct00068
는 소프트 자기 간섭이다.
차량 자세는 7차원 EKF 필터링 모델에 의해 갱신되며, 7차원 EKF 필터링 모델은 7차원 상태 벡터에 대해 확장 칼만 필터(extended Kalman filter)를 사용하고, EKF는 상태 방정식 및 관측 방정식을 포함한다.
Figure 112017082029351-pct00069
상태 행렬은 다음과 같다.
Figure 112019029768635-pct00070
, 여기서
Figure 112019029768635-pct00071
는 쿼터니온 벡터
Figure 112019029768635-pct00247
,
Figure 112019029768635-pct00248
,
Figure 112019029768635-pct00249
,
Figure 112019029768635-pct00250
이고,
Figure 112019029768635-pct00076
은 XYZ 3축 자이로스코프의 제로 드리프트 이다. 여기서,
Figure 112019029768635-pct00077
는 자이로스코프에 의해 출력 된 각속도,
Figure 112019029768635-pct00078
은 공정 잡음 행렬(process noise matrix),
Figure 112019029768635-pct00079
은 관측 잡음 행렬(observation noise matrix),
Figure 112019029768635-pct00080
는 관측 벡터(observation vector)이며,
Figure 112019029768635-pct00081
, 여기서 a는 3축 가속도 값이고,
Figure 112019029768635-pct00082
는 전자 나침판에 의해 계산 된 코스 각도(course angle)이다.
Figure 112017082029351-pct00083
Figure 112017082029351-pct00084
지자기장은 세기가 약하기 때문에 주위의 철 재료 및 전자기장의 영향을 받기 쉽다. 그러므로 먼저 자이로스코프를 보정할 필요가 있다. 전자 나침반 교정 타원 모델은 자기장의 간섭을 제거하기 위해 설정된다. 상기 획득 된 자기장 세기는 실제 교정 과정에서 최소 자승법(least square method)으로 맞추어 전술한 파라미터를 얻는다.
단계(S2): 차량 운동의 가속도 및 각속도 그리고 지자기장을 포함한 데이터는 9축 MEMS 센서를 통해 실시간으로 수집된다.
9축 MEMS 센서를 통해 차량의 운동의 가속도 및 각속도, 그리고 지자기장 세기를 포함하는 데이터를 획득하는 단계(S2)는 다음과 같이 상세히 설명된다.
차량의 각속도는 자이로스코프를 통해 획득되며 자이로스코프의 제로 드리프트가 보상된다.
가속도 센서는 차량의 가속도 데이터를 획득하기 위해 사용된다.
차량의 지자기장 세기는 지자기 센서(geomagnetic sensor)에 의해 얻어진다.
단계(S3): 차량 운동의 가속도 및 각속도 그리고 지자기장 세기를 포함하여 상기 획득된 데이터에 따라 설정된 자이로스코프 오차 모델 및 전자 나침반 교정 타원 모델에 의해 차량의 각도(angle), 속도(velocity), 위치 정보(position information) 및 코스 각도(course angle)가 계산된다.
차량 운동의 가속도 및 각속도 그리고 지자기장 세기를 포함하여 상기 획득된 데이터에 따라 설정된 자이로스코프 오차 모델 및 전자 나침반 교정 타원 모델에 의해 차량의 각도, 속도, 위치 정보 및 코스 각도를 계산하는 단계(S3)는 아래에서 자세히 설명한다.
각도 데이터는 자이로스코프의 오차 모델에 의한 각속도의 적분 계산에 의해 얻어진다.
속도는 가속도 데이터를 적분하여 계산되며, 위치 정보는 상기 속도를 더 적분하여 계산된다.
교정 파라미터(calibration parameter)에 의해 보상되고 경사각(oblique angle)에 의해 보정 된 지자기장 세기 데이터로부터 차량의 코스 각도가 계산되고, 교정 파라미터 및 경사각은 타원 모델에 의해 계산된다.
차량의 모션 정보는 실시간으로 MEMS 센서에 의해 획득된다. 자이로스코프에 의해 획득 된 차량의 각속도는 자이로스코프의 상태 추정(state estimation) 및 제로 오프셋(zero offset)에 의해 보정된다. 차량의 각속도가 통합되어 각도 증가(angle increment)를 계산한다. 지자기 센서는 부드러운 자기, 단단한 자기 및 경사각에 의해 보정되고 보상되어 코스 각도(course angle)를 계산한다.
단계(S4): 차량의 각도, 속도, 위치 정보 및 코스 각도는 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되고, 차량의 운동 자세 각도는 실시간으로 갱신된다.
차량의 각도, 속도, 위치 정보 및 코스 각도는 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되는 단계, 및 차량의 운동 자세 각도가 실시간으로 갱신되는 단계(S4)는 아래에서 상세하게 설명된다.
차량의 자세 데이터는 쿼터니언 자세 갱신 알고리즘(quaternion attitude updating algorithm)을 통해 7차원 EKF 필터링 모델에 의해 계산되며, 여기서 EKF 알고리즘의 계산 프로세스는 다음과 같다:
Figure 112017082029351-pct00085
상기 수학식에서,
Figure 112019029768635-pct00086
는 샘플링 시점이고,
Figure 112019029768635-pct00087
는 시스템 상태 추정이며,
Figure 112019029768635-pct00088
는 이전 시점이고,
Figure 112019029768635-pct00089
는 이후 시점이며,
Figure 112019029768635-pct00090
는 상태 전이 행렬이고,
Figure 112019029768635-pct00251
는 최소 평균 제곱 오차 행렬이며,
Figure 112019029768635-pct00092
는 상태 벡터에 해당하는 공분산 행렬이며,
Figure 112019029768635-pct00252
는 오차 이득이며,
Figure 112019029768635-pct00253
는 관측 벡터이며,
Figure 112019029768635-pct00254
는 관측 방정식의 천이 행렬이며,
Figure 112019029768635-pct00255
는 관측 벡터에 해당하는 공분산 행렬이다.
Figure 112019029768635-pct00097
삭제
위 수학식에서
Figure 112017082029351-pct00098
는 쿼터니온 벡터이고,
Figure 112017082029351-pct00099
,
Figure 112017082029351-pct00100
,
Figure 112017082029351-pct00101
,
Figure 112017082029351-pct00102
는 쿼터니언 벡터를 형성하는 스칼라이며,
Figure 112017082029351-pct00103
,
Figure 112017082029351-pct00104
,
Figure 112017082029351-pct00105
는 3차원 좌표계의 단위 벡터이다. 갱신 된 자세 행렬은 다음과 같다.
Figure 112017082029351-pct00106
상기 수학식에서,
Figure 112017082029351-pct00107
는 캐리어(carrier) 좌표계를 항법(navigation) 좌표계로 변환하기 위한 회전 행렬이다.
Figure 112017082029351-pct00108
위 수학식에서,
Figure 112017082029351-pct00109
,
Figure 112017082029351-pct00110
,
Figure 112017082029351-pct00111
는 롤링 각도, 피치 각도 및 코스 각도이다.
위 수학식에서 9축 MEMS 센서는 3축 자이로스코프, 3축 가속도계 및 3축 지자기 센서로 구성된다.
제2실시예:
도 2를 참조하면, 9축 MEMS 센서를 기반으로 농기계의 전-자세 각도(all-attitude angle)를 갱신하는 방법이 제공된다. 이러한 방법은 다음 단계를 포함한다.
단계(S1): 자이로스코프의 오차 모델, 전자 나침반 교정 타원 모델, 및 7차원 EKF 필터링 모델이 설정되고, 차량 운동 자세에 대응하는 파라미터 벡터가 설정된다.
자이로스코프의 오차 모델, 전자 나침반 교정 타원 모델 및 7차원 EKF 필터링 모델을 설정하는 단계, 및 차량 운동 자세에 대응하는 파라미터 벡터 설정 단계(S1)가 아래에서 자세히 설명된다.
자이로스코프의 오차 공식은 자이로스코프의 오차 계산식을 통해 자이로스코프의 각속도를 계산하며, 여기서 자이로스코프의 오차 계산식은 다음과 같다.
Figure 112017082029351-pct00112
, 여기서,
Figure 112017082029351-pct00113
는 자이로스코프에 의해 출력 된 각속도(angular velocity)이고,
Figure 112017082029351-pct00114
는 자이로스코프의 실제 각속도이며,
Figure 112017082029351-pct00115
는 자이로스코프의 제로 드리프트(zero drift)이며,
Figure 112017082029351-pct00116
는 자이로스코프에 의해 출력되는 백색 잡음(white noise)이다.
자기장 간섭은 전자 나침반 교정 타원 모델에 의해 제거되며, 여기서 전자 나침반 교정 타원 모델은 다음과 같다.
Figure 112017082029351-pct00117
, 여기서
Figure 112017082029351-pct00118
는 자기장 세기이고,
Figure 112017082029351-pct00119
및
Figure 112017082029351-pct00120
은 하드 자기 간섭이며,
Figure 112017082029351-pct00121
및
Figure 112017082029351-pct00122
는 소프트 자기 간섭이다.
차량 자세는 7차원 EKF 필터링 모델에 의해 갱신되며, 7차원 EKF 필터링 모델은 7차원 상태 벡터에 대해 확장 칼만 필터(extended Kalman filter)이고, EKF는 상태 방정식 및 관측 방정식을 포함한다.
Figure 112017082029351-pct00123
상태 행렬은 다음과 같다.
Figure 112019029768635-pct00124
, 여기서
Figure 112019029768635-pct00125
는 쿼터니온 벡터
Figure 112019029768635-pct00256
,
Figure 112019029768635-pct00257
,
Figure 112019029768635-pct00258
,
Figure 112019029768635-pct00259
이고,
Figure 112019029768635-pct00130
은 XYZ 3축 자이로스코프의 제로 드리프트 이다. 위 공식에서,
Figure 112019029768635-pct00131
는 자이로스코프에 의해 출력 된 각속도,
Figure 112019029768635-pct00132
은 공정 잡음 행렬(process noise matrix),
Figure 112019029768635-pct00133
은 관측 잡음 행렬(observation noise matrix),
Figure 112019029768635-pct00134
는 관측 벡터(observation vector)이며,
Figure 112019029768635-pct00135
, a는 3축 가속도 값이고,
Figure 112019029768635-pct00136
는 전자 나침판에 의해 계산 된 코스 각도(course angle)이다.
Figure 112017082029351-pct00137
Figure 112017082029351-pct00138
지자기장은 강도가 약하기 때문에 주위의 철 재료 및 전자기장의 영향을 받기 쉽다. 그러므로 먼저 자이로스코프를 보정할 필요가 있다. 전자 나침반 교정 타원 모델은 자기장의 간섭을 제거하기 위해 설정된다. 상기 획득 된 자기장 세기는 실제 교정 과정에서 최소 자승법(least square method)으로 맞추어 전술한 파라미터를 얻는다.
단계(S2): 차량 운동의 가속도 및 각속도 그리고 지자기장을 포함한 데이터는 9축 MEMS 센서를 통해 실시간으로 수집된다.
9축 MEMS 센서를 통해 차량의 운동의 가속도 및 각속도, 그리고 지자기장 세기를 포함하는 데이터를 획득하는 단계(S2)는 다음과 같이 상세히 설명된다.
차량의 각속도는 자이로스코프를 통해 획득되며 자이로스코프의 제로 드리프트가 보상된다.
가속도 센서는 차량의 가속도 데이터를 획득하기 위해 사용된다.
차량의 지자기장 세기는 지자기 센서(geomagnetic sensor)에 의해 얻어진다.
단계(S3): 차량 운동의 가속도 및 각속도 그리고 지자기장 세기를 포함하여 상기 획득된 데이터에 따라 설정된 자이로스코프 오차 모델 및 전자 나침반 교정 타원 모델에 의해 차량의 각도(angle), 속도(velocity), 위치 정보(position information) 및 코스 각도(course angle)가 계산된다.
차량 운동의 가속도 및 각속도 그리고 지자기장 세기를 포함하여 상기 획득된 데이터에 따라 설정된 자이로스코프 오차 모델 및 전자 나침반 교정 타원 모델에 의해 차량의 각도, 속도, 위치 정보 및 코스 각도를 계산하는 단계(S3)는 아래에서 자세히 설명한다.
각도 데이터는 자이로스코프의 오차 모델에 의한 각속도의 적분 계산에 의해 얻어진다.
속도는 가속도 데이터를 적분하여 계산되며, 위치 정보는 상기 속도를 더 적분하여 계산된다.
교정 파라미터(calibration parameter)에 의해 보상되고 경사각(oblique angle)에 의해 보정 된 지자기장 세기 데이터로부터 차량의 코스 각도가 계산되고, 교정 파라미터 및 경사각은 타원 모델에 의해 계산된다.
차량의 모션 정보는 실시간으로 MEMS 센서에 의해 획득된다. 자이로스코프에 의해 획득 된 차량의 각속도는 자이로스코프의 상태 추정(state estimation) 및 제로 오프셋(zero offset)에 의해 보정된다. 차량의 각속도가 통합되어 각도 증가(angle increment)를 계산한다. 지자기 센서는 부드러운 자기, 단단한 자기 및 경사각에 의해 보정되고 보상되어 코스 각도(course angle)를 계산한다.
단계(S4): 차량의 각도, 속도, 위치 정보 및 코스 각도는 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되고, 차량의 운동 자세 각도는 실시간으로 갱신된다.
차량의 각도, 속도, 위치 정보 및 코스 각도는 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되는 단계, 및 차량의 운동 자세 각도가 실시간으로 갱신되는 단계(S4)는 아래에서 상세하게 설명된다.
차량의 자세 데이터는 쿼터니언 자세 갱신 알고리즘(quaternion attitude updating algorithm)을 통해 7차원 EKF 필터링 모델에 의해 계산되며, 여기서 EKF 알고리즘의 계산 프로세스는 다음과 같다.
Figure 112017082029351-pct00139
상기 수학식에서,
Figure 112019029768635-pct00140
는 샘플링 시점이고,
Figure 112019029768635-pct00141
는 시스템 상태 추정이며,
Figure 112019029768635-pct00142
는 이전 시점이고,
Figure 112019029768635-pct00143
는 이후 시점이며,
Figure 112019029768635-pct00144
는 상태 전이 행렬이고,
Figure 112019029768635-pct00260
는 최소 평균 제곱 오차 행렬이며,
Figure 112019029768635-pct00146
는 상태 벡터에 해당하는 공분산 행렬이며,
Figure 112019029768635-pct00261
는 오차 이득이며,
Figure 112019029768635-pct00262
는 관측 벡터이며,
Figure 112019029768635-pct00263
는 관측 방정식의 천이 행렬이며,
Figure 112019029768635-pct00264
는 관측 벡터에 해당하는 공분산 행렬이다.
Figure 112017082029351-pct00151
위 수학식에서
Figure 112019029768635-pct00152
는 쿼터니온 벡터이고,
Figure 112019029768635-pct00265
,
Figure 112019029768635-pct00266
,
Figure 112019029768635-pct00267
,
Figure 112019029768635-pct00268
는 쿼터니언 벡터를 형성하는 스칼라이며,
Figure 112019029768635-pct00157
,
Figure 112019029768635-pct00158
,
Figure 112019029768635-pct00159
는 3 차원 좌표계의 단위 벡터이고, 갱신 된 자세 행렬은 다음과 같다.
Figure 112017082029351-pct00160
상기 수학식에서,
Figure 112017082029351-pct00161
는 캐리어 좌표계를 항법 좌표계로 변환하기 위한 회전 행렬이다.
Figure 112017082029351-pct00162
위 수학식에서,
Figure 112017082029351-pct00163
,
Figure 112017082029351-pct00164
,
Figure 112017082029351-pct00165
는 롤링 각도, 피치 각도 및 코스 각도이다.
위 수학식에서 9축 MEMS 센서는 3축 자이로스코프, 3축 가속도계 및 3축 지자기 센서로 구성된다.
단계(S5): 상기 갱신 된 차량의 자세 데이터로부터 상기 차량의 전-자세 각도 데이터가 추출되어, 자세 각도 데이터의 값이 결정된다.
상기 차량의 전-자세 각도는 피치 각도, 롤링 각도 및 코스 각도를 포함하며,
Figure 112017082029351-pct00166
상기 코스 각도:
Figure 112017082029351-pct00167
,
상기 피치 각도:
Figure 112017082029351-pct00168
상기 롤링 각도:
Figure 112017082029351-pct00169
.
상기 차량의 전-자세 각도는 상기 갱신되고 계산 된 자세 매트릭스
Figure 112017082029351-pct00170
로부터 추출될 수 있고, 전-자세 각도는 피치 각도, 롤링 각도, 및 코스 각도를 포함한다. 상기 피치 각도
Figure 112017082029351-pct00171
는 역 사인함수의 주요 값과 일치하는 [-90°+ 90°] 간격으로 정의되므로 다중 값 문제는 없다. 상기 롤링 각도는 [180°180°] 간격으로 정의된다. 상기 코스 각도는 [0°360°] 간격으로 정의된다. 따라서
Figure 112017082029351-pct00172
및
Figure 112017082029351-pct00173
양자에 대해서는 다중 값 문제가 있다. 상기 주요 값이 계산 된 후 특정 사분면은
Figure 112017082029351-pct00174
에서 구성요소로 결정될 수 있다.
본 발명의 장점은 다음과 같다. 상기 물체의 움직임의 가속도 및 각속도는 MEMS 센서에 의해 실시간으로 획득된다. 자이로스코프에 의한 각가속도 출력이 적분되어 각도가 획득된다. 상기 가속도가 통합되어 속도를 계산하고, 이를 더 통합하여 상기 위치 정보가 계산된다. 상기 지자기장은 지자기 센서에 의해 얻어지며, 상기 코스 각도는 보상 알고리즘과 자이로스코프의 융합에 의해 계산된다. 다음으로, 자세가 전이 행렬(transition matrix)로 변환되어, 캐리어 좌표계가 항법 좌표계로 변환된다. 이러한 전이 행렬 함수는 "수학적 플랫폼(mathematical platform)"으로 기능한다. SINS(스트랩다운 관성 항법 시스템: Strapdown inertial navigation system) 알고리즘은 농기계에 적용되며, 전이 행렬이 특히 중요하다. 농기계가 계속 움직이기 때문에, 그 자세 또한 계속 변화하고 있다. 따라서 상기 전이 행렬 또한 지속적으로 다시 계산되고 갱신 될 필요가 있다. 기존의 자세 갱신 알고리즘(attitude updating algorithms)으로는 오일러각 알고리즘(Euler angle algorithm), 방향 코사인 알고리즘(direction cosine algorithm), 쿼터니온 알고리즘(quaternion algorithm) 등이 있다. 오일러각 알고리즘과 비교할 때 쿼터니온 알고리즘에는 특이점이 없다. 상기 방향 코사인 알고리즘과 비교할 때 쿼터니언 알고리즘은 계산량이 적다. 따라서 쿼터니온 알고리즘은 임베디드 제품에 사용하기에 매우 적합하다. 상기 자이로스코프의 지자기장과 오차 모델은 농기계 평면에 설정되고, 7차원 EKF(Extended Kalman Filter)가 설정되어 자세 행렬이 갱신된다. 쿼터니온과 자이로스코프의 제로 오프셋이 추정된다. 다음에, 가속도 및 자기장 강도를 통해 산출 된 코스 각도로부터 관측을 행함으로써, 높은 정밀도의 3차원 자세 각도를 얻을 수 있다. 오류 보상 알고리즘과 정정 알고리즘은 SINS 알고리즘의 오류 간섭을 크게 줄인다. MEMS 센서 및 SINS 알고리즘은 본 발명이 높은 성능 파라미터를 갖는 것을 보장한다. 트랙터(tractor)에 의해 시험 된 바와 같이, 출력 코스 각도의 오차는 0.1°미만이며, 피치 각도 오차 및 롤링 각도 오차는 0.01°미만이다. 상기 쿼터니온은 칼만 필터링 상태 벡터로서 사용되기 때문에, 상기 목표 파라미터의 계산 정확도가 더욱 향상될 수 있다.
상기 설명은 본 발명의 특정 실시 예를 단지 설명하기 위한 것이며, 본 발명의 보호 범위는 이에 제한되지 않는다. 본 발명의 개시 내용의 범위 내에서 당업자에 의해 용이하게 이해되는 변형 또는 치환은 모두 본 발명의 보호 범위에 속한다. 따라서 본 발명의 보호 범위는 첨부 된 청구 범위에 의해 결정되어야 한다.

Claims (6)

  1. 9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법으로서,
    자이로스코프의 오차 모델, 전자 나침반 교정 타원 모델 및 7차원 EKF 필터링 모델을 설정하는 단계, 및 차량 운동 자세에 대응하는 파라미터 벡터 설정 단계;
    9축 MEMS 센서를 통해 상기 차량 운동의 가속도 및 각속도, 그리고 지자기장 세기를 포함하는 데이터를 획득하는 단계;
    상기 차량 운동의 가속도 및 각속도 그리고 상기 지자기장 세기를 포함하여 상기 획득된 데이터에 따라 설정된 상기 자이로스코프의 오차 모델 및 상기 전자 나침반 교정 타원 모델에 의해 상기 차량의 각도, 속도, 위치 정보 및 코스 각도를 계산하는 단계; 및
    상기 차량의 상기 각도, 상기 속도, 상기 위치 정보 및 상기 코스 각도는 상기 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되는 단계, 및 상기 차량의 운동 자세 각도를 실시간으로 갱신하는 단계를 포함하고,
    상기 9축 MEMS 센서는 3축 자이로스코프, 3축 가속도 센서 및 3축 지자기 센서로 구성되고,
    상기 자이로스코프의 오차 모델, 전자 나침반 교정 타원 모델 및 7차원 EKF 필터링 모델을 설정하는 단계, 및 차량 운동 자세에 대응하는 파라미터 벡터 설정 단계는,
    상기 자이로스코프의 상기 오차 모델에서 상기 자이로스코프의 오차 계산식을 통해 상기 자이로스코프의 상기 각속도를 계산하는 단계; 상기 자이로스코프의 상기 오차 계산식은,
    Figure 112019502982617-pct00175
    이며, 여기서,
    Figure 112019502982617-pct00176
    는 상기 자이로스코프에 의해 출력 된 각속도이고,
    Figure 112019502982617-pct00177
    는 상기 자이로스코프의 실제 각속도이며,
    Figure 112019502982617-pct00178
    는 상기 자이로스코프의 제로 드리프트이며,
    Figure 112019502982617-pct00179
    는 상기 자이로스코프에 의해 출력되는 백색 잡음이며,
    전자 나침반 교정 타원 모델에 의해 자기장 간섭을 제거하는 단계; 상기 전자 나침반 교정 타원 모델은,
    Figure 112019502982617-pct00180
    이며, 여기서
    Figure 112019502982617-pct00181
    는 자기장 세기이고,
    Figure 112019502982617-pct00182
    및
    Figure 112019502982617-pct00183
    은 하드 자기 간섭이며,
    Figure 112019502982617-pct00184
    및
    Figure 112019502982617-pct00185
    는 소프트 자기 간섭이고,
    상기 7차원 상기 EKF 필터링 모델에 의해 상기 차량 운동 자세를 갱신하는 단계를 포함하고,
    상기 7차원 EKF 필터링 모델은 7차원 상태 벡터에 대해 확장 칼만 필터(extended Kalman filter)를 사용하고, 상기 EKF는 상태 방정식 및 관측 방정식을 포함하며,
    Figure 112019502982617-pct00186
    ,
    여기서,
    Figure 112019502982617-pct00187
    는 쿼터니온 벡터
    Figure 112019502982617-pct00269
    ,
    Figure 112019502982617-pct00270
    ,
    Figure 112019502982617-pct00271
    ,
    Figure 112019502982617-pct00272
    이고,
    Figure 112019502982617-pct00192
    은 상기 3축 자이로스코프의 제로 드리프트 이고, 여기서,
    Figure 112019502982617-pct00193
    는 상기 자이로스코프에 의해 출력 된 각속도,
    Figure 112019502982617-pct00194
    은 공정 잡음 행렬(process noise matrix),
    Figure 112019502982617-pct00195
    은 관측 잡음 행렬(observation noise matrix),
    Figure 112019502982617-pct00196
    는 관측 벡터(observation vector)이며,
    Figure 112019502982617-pct00197
    , 여기서, a는 3축 가속도 값이고,
    Figure 112019502982617-pct00198
    는 전자 나침판에 의해 계산 된 코스 각도(course angle)이며,
    Figure 112019502982617-pct00199

    Figure 112019502982617-pct00200

    인 것을 특징으로 하는 9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법.
  2. 삭제
  3. 제 1 항에 있어서,
    상기 차량 운동의 가속도 및 각속도, 그리고 지자기장 세기를 포함하는 데이터를 9축 MEMS 센서를 통해 실시간으로 획득하는 단계는,
    상기 자이로스코프를 통해 상기 차량의 각속도를 획득하며, 상기 자이로스코프의 제로 드리프트를 보상하는 단계;
    가속도 센서를 통하여 상기 차량의 가속도 데이터를 획득하는 단계; 및
    상기 지자기 센서를 통하여 상기 차량의 상기 지자기장 세기를 획득하는 단계를 포함하여 구성됨을 특징으로 하는 9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법.
  4. 제 3 항에 있어서,
    상기 차량 운동의 상기 가속도 및 상기 각속도 그리고 상기 지자기장 세기를 포함하여 상기 획득된 데이터에 따라 설정된 자이로스코프 상기 오차 모델 및 상기 전자 나침반 교정 타원 모델에 의해 상기 차량의 각도, 속도, 위치 정보 및 코스 각도를 계산하는 단계는,
    상기 자이로스코프의 오차 모델에 의한 각속도의 적분 계산에 의해 각도를 획득하는 단계;
    상기 가속도를 적분하여 상기 속도를 계산하며, 상기 위치 정보는 상기 속도를 더 적분하여 계산되는 단계; 및
    교정 파라미터에 의해 보상되고 경사각에 의해 보정 된 상기 지자기장 세기로부터 상기 차량의 코스 각도를 계산하고, 상기 교정 파라미터 및 상기 경사각은 상기 타원 모델에 의해 계산되는 단계를 포함하여 구성됨을 특징으로 하는 9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법.
  5. 제 4 항에 있어서,
    상기 차량의 상기 각도, 상기 속도, 상기 위치 정보 및 상기 코스 각도를 상기 7차원 EKF 필터링 모델에 의해 데이터 융합 처리하는 단계, 및 상기 차량의 상기 운동 자세 각도를 실시간으로 갱신하는 단계는,
    상기 차량의 상기 운동 자세 각도를 쿼터니언 자세 갱신 알고리즘을 통해 상기 7차원 EKF 필터링 모델에 의해 계산하는 단계; 여기서 EKF 알고리즘의 계산 프로세스는 다음과 같고,
    Figure 112019029768635-pct00201
    ,
    여기서,
    Figure 112019029768635-pct00202
    는 샘플링 시점이고,
    Figure 112019029768635-pct00203
    는 시스템 상태 추정이며,
    Figure 112019029768635-pct00204
    는 이전 시점이고,
    Figure 112019029768635-pct00205
    는 이후 시점이며,
    Figure 112019029768635-pct00206
    는 상태 전이 행렬이고,
    Figure 112019029768635-pct00273
    는 최소 평균 제곱 오차 행렬이며,
    Figure 112019029768635-pct00208
    는 상태 벡터에 해당하는 공분산 행렬이며,
    Figure 112019029768635-pct00274
    는 오차 이득이며,
    Figure 112019029768635-pct00275
    는 관측 벡터이며,
    Figure 112019029768635-pct00276
    는 관측 방정식의 천이 행렬이며,
    Figure 112019029768635-pct00277
    는 관측 벡터에 해당하는 공분산 행렬이며,
    Figure 112019029768635-pct00213
    ,
    여기서,
    Figure 112019029768635-pct00214
    는 쿼터니온 벡터이고,
    Figure 112019029768635-pct00278
    ,
    Figure 112019029768635-pct00279
    ,
    Figure 112019029768635-pct00280
    ,
    Figure 112019029768635-pct00281
    는 쿼터니언 벡터를 형성하는 스칼라이며,
    Figure 112019029768635-pct00219
    ,
    Figure 112019029768635-pct00220
    ,
    Figure 112019029768635-pct00221
    는 3 차원 좌표계의 단위 벡터이며, 갱신 된 자세 행렬은 다음과 같고,
    Figure 112019029768635-pct00222

    Figure 112019029768635-pct00223
    는 캐리어 좌표계를 항법 좌표계로 변환하기 위한 회전 행렬이며,
    Figure 112019029768635-pct00224

    여기서,
    Figure 112019029768635-pct00225
    ,
    Figure 112019029768635-pct00226
    ,
    Figure 112019029768635-pct00227
    는 롤링 각도, 피치 각도 및 코스 각도인 것을 특징으로 하는 9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법.
  6. 제 5 항에 있어서,
    상기 차량의 상기 각도, 상기 속도, 상기 위치 정보 및 상기 코스 각도는 7차원 EKF 필터링 모델에 의해 데이터 융합 처리되는 단계, 및 상기 차량의 운동 자세 각도가 실시간으로 갱신되는 단계 후에,
    갱신 된 상기 차량의 운동 자세 각도로부터 상기 차량의 전-자세 각도 데이터를 추출하여, 상기 전-자세 각도 데이터의 값이 결정되며, 상기 차량의 전-자세 각도는 피치 각도, 롤링 각도 및 코스 각도를 포함하는 단계를 포함하고,
    여기서,
    Figure 112017083554311-pct00228

    상기 코스 각도:
    Figure 112017083554311-pct00229

    상기 피치 각도:
    Figure 112017083554311-pct00230

    상기 롤링 각도:
    Figure 112017083554311-pct00231

    인 것을 특징으로 하는 9축 MEMS 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법.
KR1020177023677A 2015-10-13 2016-07-04 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법 KR102017404B1 (ko)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
CN201510664990.3A CN105203098B (zh) 2015-10-13 2015-10-13 基于九轴mems传感器的农业机械全姿态角更新方法
CN201510664990.3 2015-10-13
PCT/CN2016/088316 WO2017063387A1 (zh) 2015-10-13 2016-07-04 基于九轴mems传感器的农业机械全姿态角更新方法

Publications (2)

Publication Number Publication Date
KR20170104621A KR20170104621A (ko) 2017-09-15
KR102017404B1 true KR102017404B1 (ko) 2019-10-21

Family

ID=54950909

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020177023677A KR102017404B1 (ko) 2015-10-13 2016-07-04 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법

Country Status (6)

Country Link
US (1) US20170350721A1 (ko)
EP (1) EP3364153B1 (ko)
KR (1) KR102017404B1 (ko)
CN (1) CN105203098B (ko)
RU (1) RU2662460C1 (ko)
WO (1) WO2017063387A1 (ko)

Families Citing this family (70)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105203098B (zh) * 2015-10-13 2018-10-02 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法
CN106931965B (zh) * 2015-12-31 2021-04-13 中国移动通信集团吉林有限公司 一种确定终端姿态的方法及装置
CN105588567B (zh) * 2016-01-25 2018-03-27 北京航空航天大学 一种自动电子罗盘校准辅助式的航姿参考系统及方法
JP6511406B2 (ja) * 2016-02-10 2019-05-15 クラリオン株式会社 キャリブレーションシステム、キャリブレーション装置
CN105975989B (zh) * 2016-05-10 2019-03-19 东南大学 一种基于九轴运动传感器的肘部运动状态识别方法
CN106444809B (zh) * 2016-10-12 2024-04-16 湖南绿野航空科技有限公司 一种无人机飞行控制器
CN106706018B (zh) * 2016-12-28 2019-06-14 重庆爱奇艺智能科技有限公司 Vr设备中九轴传感器性能的测试方法、装置及测试转台
CN108426573B (zh) * 2017-02-14 2023-04-07 中兴通讯股份有限公司 一种终端设备的行人步态检测方法及终端设备
CN107085246A (zh) * 2017-05-11 2017-08-22 深圳合优科技有限公司 一种基于mems的人体动作识别方法和装置
CN107560613B (zh) * 2017-08-15 2020-03-31 江苏科技大学 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
JP7025215B2 (ja) * 2018-01-05 2022-02-24 ローム株式会社 測位システム及び測位方法
US10882446B2 (en) * 2018-01-22 2021-01-05 Henan Polytechnic University Graded early warning system for rollover of heavy-duty truck based on time-varying interactive kalman filtering and early warning method thereof
FR3079026B1 (fr) * 2018-03-15 2021-01-01 Sysnav Procede de calibration d'un gyrometre equipant un vehicule
CN108693773B (zh) * 2018-04-04 2021-03-23 南京天辰礼达电子科技有限公司 一种农业机械自动驾驶滑坡偏差自适应估计方法
CN108897333A (zh) * 2018-07-06 2018-11-27 深圳臻迪信息技术有限公司 姿态估算方法、装置和无人机
CN108680189B (zh) * 2018-07-09 2024-04-12 无锡凌思科技有限公司 一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法
CN109211267B (zh) * 2018-08-14 2022-08-23 广州虚拟动力网络技术有限公司 一种惯性动作捕捉姿态快速校准方法及系统
WO2020090863A1 (ja) * 2018-10-30 2020-05-07 東京計器株式会社 誤差補正装置
CN109238262B (zh) * 2018-11-05 2020-10-30 珠海全志科技股份有限公司 一种航向姿态解算及罗盘校准抗干扰方法
CN109540135B (zh) * 2018-11-09 2020-07-31 华南农业大学 水田拖拉机位姿检测和偏航角提取的方法及装置
CN109579836B (zh) * 2018-11-21 2022-09-06 阳光凯讯(北京)科技有限公司 一种基于mems惯性导航的室内行人方位校准方法
CN109724596A (zh) * 2018-12-07 2019-05-07 江苏大学 一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法
CN109764868A (zh) * 2019-02-28 2019-05-17 深圳市伟文无线通讯技术有限公司 一种基于六轴传感器设备安装姿态校准方法
CN109813312A (zh) * 2019-03-05 2019-05-28 南京理工大学 一种卡片式测量物体姿态角的测量装置及方法
CN110057381A (zh) * 2019-03-20 2019-07-26 深圳市元征科技股份有限公司 一种导航系统的零速修正方法及系统
CN110031020A (zh) * 2019-03-28 2019-07-19 广州英卓电子科技有限公司 一种平面磁场校正方法及其装置
CN110007354B (zh) * 2019-04-22 2020-11-06 成都理工大学 无人机半航空瞬变电磁接收线圈飞行参数测量装置及方法
CN110207697B (zh) * 2019-04-29 2023-03-21 南京航空航天大学 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN110702113B (zh) * 2019-05-09 2022-12-27 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN110132257B (zh) * 2019-05-15 2023-03-24 吉林大学 基于多传感器数据融合的人体行为预测方法
CN110281724A (zh) * 2019-05-31 2019-09-27 惠州市德赛西威汽车电子股份有限公司 一种主动式底盘自稳系统及其车辆
CN110285830B (zh) * 2019-07-01 2022-12-27 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的sins/gps速度匹配对准方法
CN110319833B (zh) * 2019-07-09 2022-07-15 哈尔滨工程大学 一种无误差的光纤陀螺捷联惯导系统速度更新方法
CN110455312B (zh) * 2019-08-08 2021-05-14 中国科学院长春光学精密机械与物理研究所 一种陀螺安装误差标校系统及其标校方法
CN110595434B (zh) * 2019-09-10 2021-09-03 兰州交通大学 基于mems传感器的四元数融合姿态估计方法
CN110608741A (zh) * 2019-09-25 2019-12-24 北京安达维尔科技股份有限公司 一种提高飞机航姿参考系统姿态解算精度的方法
CN110865580B (zh) * 2019-11-27 2022-07-26 中国船舶重工集团公司第七0七研究所 基于时分复用的半球谐振陀螺全差分控制系统及控制方法
CN111006660A (zh) * 2019-12-11 2020-04-14 陕西瑞特测控技术有限公司 一种组合惯导姿态计算方法
CN110887480B (zh) * 2019-12-11 2020-06-30 中国空气动力研究与发展中心低速空气动力研究所 基于mems传感器的飞行姿态估计方法及系统
CN110928324B (zh) * 2019-12-30 2023-07-14 北京润科通用技术有限公司 无人机飞行参数采集设备及其校准方法
CN111121824B (zh) * 2020-01-03 2022-11-22 西北工业大学 一种mems传感器的标定方法
CN111207734B (zh) * 2020-01-16 2022-01-07 西安因诺航空科技有限公司 一种基于ekf的无人机组合导航方法
CN111426318B (zh) * 2020-04-22 2024-01-26 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111623768B (zh) * 2020-04-24 2022-04-12 北京航天控制仪器研究所 一种基于克雷洛夫角奇异条件下的姿态角解算方法
CN111551168A (zh) * 2020-04-30 2020-08-18 江苏帝一集团有限公司 一种水下机器人位姿数据采集系统及其数据融合方法
US11937530B2 (en) 2020-05-01 2024-03-26 Kinze Manufacturing, Inc. GPS location augmentation and outage playthrough
CN111693019B (zh) * 2020-05-20 2021-04-20 西安交通大学 一种姿态传感装置及数据融合、姿态解算方法
US11332152B2 (en) * 2020-05-29 2022-05-17 GM Global Technology Operations LLC Method and apparatus for determining a velocity of a vehicle
CN111707268B (zh) * 2020-06-19 2022-07-29 中国人民解放军国防科技大学 基于双欧法和四元数混合编排的无人机导航方法和系统
CN111765906B (zh) * 2020-07-29 2022-06-14 三一机器人科技有限公司 误差校准方法及装置
CN111854762A (zh) * 2020-08-05 2020-10-30 翟瑞永 一种基于卡尔曼滤波算法的三维定位方法及其定位系统
CN111896007B (zh) * 2020-08-12 2022-06-21 智能移动机器人(中山)研究院 一种补偿足地冲击的四足机器人姿态解算方法
CN112539743B (zh) * 2020-11-09 2023-02-28 北京电子工程总体研究所 一种基于陀螺寻北仪的连续寻北方位更新方法和系统
CN112611380B (zh) * 2020-12-03 2022-07-01 燕山大学 基于多imu融合的姿态检测方法及其姿态检测装置
CN112304340B (zh) * 2020-12-24 2021-04-06 北京轻威科技有限责任公司 一种基于九轴imu的姿态解算方法、装置及存储介质
CN113063416B (zh) * 2021-02-05 2023-08-08 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113108790B (zh) * 2021-05-14 2024-06-25 深圳中智永浩机器人有限公司 机器人imu角度测量方法、装置、计算机设备及存储介质
CN113551665B (zh) * 2021-06-25 2023-08-11 中国科学院国家空间科学中心 一种用于运动载体的高动态运动状态感知系统及感知方法
CN113569653A (zh) * 2021-06-30 2021-10-29 宁波春建电子科技有限公司 一种基于面部特征信息的三维头部姿态估计算法
CN113587925B (zh) * 2021-07-16 2023-07-07 湖南航天机电设备与特种材料研究所 一种惯性导航系统及其全姿态导航解算方法与装置
CN113679568B (zh) * 2021-09-01 2022-10-04 南京医科大学 机器人辅助脑卒中患者上肢多模态镜像康复训练评分系统
CN113640726B (zh) * 2021-10-19 2021-12-21 青岛杰瑞自动化有限公司 一种双轴磁强计的多方位椭圆拟合标定方法及系统
CN114111749B (zh) * 2021-10-29 2022-12-02 江苏凯卓立液压设备有限公司 一种利用陀螺仪判断和计算液压尾板动作的系统及方法
CN113865616B (zh) * 2021-12-01 2022-04-05 成都九鼎科技(集团)有限公司 车辆悬架姿态测量方法和测量系统
CN114910045B (zh) * 2021-12-10 2024-04-19 浙江利尔达客思智能科技有限公司 一种基于六轴获取偏航角的方法
CN114279426B (zh) * 2021-12-30 2023-12-15 杭州电子科技大学 一种六轴优化的磁力计在线校准方法
CN114485636B (zh) * 2022-01-18 2024-06-21 中国人民解放军军事科学院国防科技创新研究院 一种仿生罗盘
CN114659488B (zh) * 2022-02-08 2023-06-23 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法
CN115147313B (zh) * 2022-09-01 2022-12-30 中国科学院空天信息创新研究院 椭圆轨道遥感图像的几何校正方法、装置、设备及介质
KR102521839B1 (ko) * 2022-10-11 2023-04-14 (주)아이하트 이동체의 동작에 따른 위험발생 감지방법, 장치 및 컴퓨터프로그램

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011528432A (ja) * 2008-07-18 2011-11-17 モベア エス．アー 物体の向きをより正確に推定する方法および前記方法を実装した姿勢制御装置
KR101454153B1 (ko) * 2013-09-30 2014-11-03 국민대학교산학협력단 가상차선과 센서 융합을 통한 무인 자율주행 자동차의 항법시스템
KR101462007B1 (ko) * 2013-08-22 2014-11-18 한국과학기술연구원 자세 추정 장치 및 자세 추정 방법

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8639416B2 (en) * 2003-03-20 2014-01-28 Agjunction Llc GNSS guidance and machine control
US8594879B2 (en) * 2003-03-20 2013-11-26 Agjunction Llc GNSS guidance and machine control
US8583315B2 (en) * 2004-03-19 2013-11-12 Agjunction Llc Multi-antenna GNSS control system and method
US20070032950A1 (en) * 2005-08-05 2007-02-08 Raven Industries, Inc. Modular high-precision navigation system
CN101782391A (zh) * 2009-06-22 2010-07-21 北京航空航天大学 机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法
JP2012173190A (ja) * 2011-02-23 2012-09-10 Seiko Epson Corp 測位システム、測位方法
CN102589537B (zh) * 2012-03-05 2016-01-20 无锡汉和航空技术有限公司 一种有磁环境下无人机的电子罗盘校正方法
US9116233B2 (en) * 2012-07-10 2015-08-25 Broadcom Corporation Power mode control for sensors
RU127452U1 (ru) * 2013-01-09 2013-04-27 Общество с ограниченной ответственностью "Снэйл-Групп" Бортовой самописец для маломерного судна
CN103557866B (zh) * 2013-09-30 2016-06-01 美新半导体（无锡）有限公司 一种基于地磁技术的虚拟陀螺仪及算法的方法
WO2015066560A1 (en) * 2013-11-01 2015-05-07 InvenSense, Incorporated Systems and methods for optical sensor navigation
CN103822633B (zh) * 2014-02-11 2016-12-07 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法
CN104502942A (zh) * 2015-01-06 2015-04-08 上海华测导航技术有限公司 基于卫星导航和航位推测实现农业机械定位的系统及方法
CN104677353A (zh) * 2015-03-17 2015-06-03 上海华测导航技术股份有限公司 基于mems传感器实现机械车体航向角检测的方法
CN104697526A (zh) * 2015-03-26 2015-06-10 上海华测导航技术股份有限公司 用于农业机械的捷联惯导系统以及控制方法
CN105203098B (zh) * 2015-10-13 2018-10-02 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法
RU160949U1 (ru) * 2015-12-14 2016-04-10 Игорь Викторович Рядчиков Устройство для стабилизации положения объемного тела в пространстве с силовой компенсацией отклоняющих воздействий

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011528432A (ja) * 2008-07-18 2011-11-17 モベア エス．アー 物体の向きをより正確に推定する方法および前記方法を実装した姿勢制御装置
KR101462007B1 (ko) * 2013-08-22 2014-11-18 한국과학기술연구원 자세 추정 장치 및 자세 추정 방법
KR101454153B1 (ko) * 2013-09-30 2014-11-03 국민대학교산학협력단 가상차선과 센서 융합을 통한 무인 자율주행 자동차의 항법시스템

Also Published As

Publication number Publication date
EP3364153B1 (en) 2020-11-25
EP3364153A4 (en) 2019-06-05
EP3364153A1 (en) 2018-08-22
CN105203098A (zh) 2015-12-30
RU2662460C1 (ru) 2018-07-26
KR20170104621A (ko) 2017-09-15
WO2017063387A1 (zh) 2017-04-20
US20170350721A1 (en) 2017-12-07
CN105203098B (zh) 2018-10-02

Similar Documents

Publication Publication Date Title
KR102017404B1 (ko) 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
KR102018450B1 (ko) 단일 안테나에 기초한 ｇｎｓｓ―ｉｎｓ 차량 자세 결정 방법
KR101988786B1 (ko) 관성 항법 장치의 초기 정렬 방법
CN109579836B (zh) 一种基于mems惯性导航的室内行人方位校准方法
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN106123900B (zh) 基于改进型互补滤波的室内行人导航磁航向解算方法
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN102937450B (zh) 一种基于陀螺测量信息的相对姿态确定方法
CN106813679B (zh) 运动物体的姿态估计的方法及装置
CN108534772B (zh) 姿态角获取方法及装置
JP2012173190A (ja) 測位システム、測位方法
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN115540860A (zh) 一种多传感器融合位姿估计算法
EP3227634B1 (en) Method and system for estimating relative angle between headings
CN106595669B (zh) 一种旋转体姿态解算方法
CN110207647B (zh) 一种基于互补卡尔曼滤波器的臂环姿态角计算方法
CN111307114A (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN113063416B (zh) 一种基于自适应参数互补滤波的机器人姿态融合方法
CN109883451A (zh) 一种用于行人方位估计的自适应增益互补滤波方法及系统
CN112904884B (zh) 足式机器人轨迹跟踪方法、设备及可读存储介质

Legal Events

Date Code Title Description
A201 Request for examination
E902 Notification of reason for refusal
E701 Decision to grant or registration of patent right
GRNT Written decision to grant