KR101910515B1 - 수중 운동체 항법 제어 장치 및 방법 - Google Patents

수중 운동체 항법 제어 장치 및 방법 Download PDF

Info

Publication number
KR101910515B1
KR101910515B1 KR1020170139975A KR20170139975A KR101910515B1 KR 101910515 B1 KR101910515 B1 KR 101910515B1 KR 1020170139975 A KR1020170139975 A KR 1020170139975A KR 20170139975 A KR20170139975 A KR 20170139975A KR 101910515 B1 KR101910515 B1 KR 101910515B1
Authority
KR
South Korea
Prior art keywords
underwater vehicle
acceleration
speed
attitude
white gaussian
Prior art date
Application number
KR1020170139975A
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 KR1020170139975A priority Critical patent/KR101910515B1/ko
Application granted granted Critical
Publication of KR101910515B1 publication Critical patent/KR101910515B1/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/0206—Control of position or course in two dimensions specially adapted to water vehicles
    • 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
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P21/00—Testing or calibrating of apparatus or devices covered by the preceding groups
    • G01P21/02—Testing or calibrating of apparatus or devices covered by the preceding groups of speedometers
    • 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
    • G05D1/0875—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted to water vehicles

Landscapes

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

Abstract

본 발명은 외부 정보로 획득된 수중 운동체의 속도와 자이로 센서에 의해 획득된 수중 운동체의 각속도를 기초로 수중 운동체의 항법을 제어하는 수중 운동체 항법 제어 장치 및 방법을 제안한다. 본 발명에 따른 장치는 수중 운동체의 속도를 획득하는 속도 획득부; 수중 운동체의 각속도를 획득하는 각속도 획득부; 수중 운동체의 속도와 수중 운동체의 각속도를 기초로 수중 운동체의 가속도를 보상하는 가속도 보상부; 보상된 수중 운동체의 가속도를 기초로 수중 운동체의 자세를 산출하는 자세 산출부; 및 수중 운동체의 속도, 수중 운동체의 가속도 및 수중 운동체의 자세를 기초로 수중 운동체의 항법을 제어하는 항법 제어부를 포함한다.

Description

수중 운동체 항법 제어 장치 및 방법 {Apparatus and method for controlling navigation of underwater vehicle}
본 발명은 운동체의 항법을 제어하는 장치 및 방법에 관한 것이다. 보다 상세하게는, 수중 운동체의 항법을 제어하는 장치 및 방법에 관한 것이다.
종래에는 외부 정보가 속도 정보만 있을 경우, 자세/각속도 에러에 대한 보상은 필터의 에러 모델에 의한 상관 관계로 추정하였다. 그래서 자세/각속도 에러는 측정치가 없이 상관 관계에 의해 추정되었기 때문에, 필터의 가관측성이 떨어지고 이에 추정되는 속도도 느려지는 문제점이 있었다.
자세/각속도 에러를 빠르고 정확하게 보상하고 싶으면 정밀한 외부 자세 센서를 탑재하여 측정하고 사용하면 가능하다. 하지만 이 방법은 또다른 시스템이 추가되어야 하며, 이에 따라 시스템이 복잡해지고 비용이 증가하는 문제점이 있다.
한국등록특허 제1,525,932호 (공고일 : 2015.06.09.)
본 발명은 상기한 문제점을 해결하기 위해 안출된 것으로서, 외부 정보로 획득된 수중 운동체의 속도와 자이로 센서에 의해 획득된 수중 운동체의 각속도를 기초로 수중 운동체의 항법을 제어하는 수중 운동체 항법 제어 장치 및 방법을 제안하는 것을 목적으로 한다.
그러나 본 발명의 목적은 상기에 언급된 사항으로 제한되지 않으며, 언급되지 않은 또 다른 목적들은 아래의 기재로부터 당업자에게 명확하게 이해될 수 있을 것이다.
본 발명은 상기한 목적을 달성하기 위해 안출된 것으로서, 수중 운동체의 속도를 획득하는 속도 획득부; 상기 수중 운동체의 각속도를 획득하는 각속도 획득부; 상기 수중 운동체의 속도와 상기 수중 운동체의 각속도를 기초로 상기 수중 운동체의 가속도를 보상하는 가속도 보상부; 보상된 상기 수중 운동체의 가속도를 기초로 상기 수중 운동체의 자세를 산출하는 자세 산출부; 및 상기 수중 운동체의 속도, 상기 수중 운동체의 가속도 및 상기 수중 운동체의 자세를 기초로 상기 수중 운동체의 항법(navigation)을 제어하는 항법 제어부를 포함하는 것을 특징으로 하는 수중 운동체 항법 제어 장치를 제안한다.
또한 본 발명은 수중 운동체의 속도를 획득하는 속도 획득 단계; 상기 수중 운동체의 각속도를 획득하는 각속도 획득 단계; 상기 수중 운동체의 속도와 상기 수중 운동체의 각속도를 기초로 상기 수중 운동체의 가속도를 보상하는 가속도 보상 단계; 보상된 상기 수중 운동체의 가속도를 기초로 상기 수중 운동체의 자세를 산출하는 자세 산출 단계; 및 상기 수중 운동체의 속도, 상기 수중 운동체의 가속도 및 상기 수중 운동체의 자세를 기초로 상기 수중 운동체의 항법(navigation)을 제어하는 항법 제어 단계를 포함하는 것을 특징으로 하는 수중 운동체 항법 제어 방법을 제안한다.
본 발명은 상기한 목적 달성을 위한 구성들을 통하여 다음과 같은 효과들을 얻을 수 있다.
첫째, 기존 시스템을 동일하게 이용하면서 알고리즘을 사용하여 자세/각속도 에러에 대한 보상이 수행 가능하므로, 비용 및 시스템의 복잡성 증가 없이 항법 성능을 향상시킬 수 있다.
도 1은 본 발명의 일실시예에 따른 복합 항법 제어 시스템을 개략적으로 도시한 개념도이다.
도 2는 본 발명의 바람직한 실시예에 따른 수중 운동체 항법 제어 장치의 내부 구성을 개략적으로 도시한 개념도이다.
도 3은 본 발명의 바람직한 실시예에 따른 수중 운동체 항법 제어 방법을 개략적으로 도시한 흐름도이다.
이하, 본 발명의 바람직한 실시예를 첨부된 도면들을 참조하여 상세히 설명한다. 우선 각 도면의 구성요소들에 참조 부호를 부가함에 있어서, 동일한 구성요소들에 대해서는 비록 다른 도면상에 표시되더라도 가능한한 동일한 부호를 가지도록 하고 있음에 유의해야 한다. 또한, 본 발명을 설명함에 있어, 관련된 공지 구성 또는 기능에 대한 구체적인 설명이 본 발명의 요지를 흐릴 수 있다고 판단되는 경우에는 그 상세한 설명은 생략한다. 또한, 이하에서 본 발명의 바람직한 실시예를 설명할 것이나, 본 발명의 기술적 사상은 이에 한정하거나 제한되지 않고 당업자에 의해 변형되어 다양하게 실시될 수 있음은 물론이다.
속도 보정형 항법 필터는 12차 필터, 15차 필터 등으로 구성되며, 측정치는 외부 속도 정보만 사용된다. 12차 필터의 경우 속도, 자세, 가속도, 자이로 등의 바이어스 오차를 추정한다. 속도, 가속도 등의 에러는 측정치인 외부 속도 정보로 정확하고 빠르게 추정될 수 있다. 그러나 자세, 각속도 등의 에러는 필터의 에러 모델 상관관계에 의하여 추정되기는 하지만, 측정치가 있는 속도, 가속도 등의 정보보다 느리고 부정확하게 추정된다. 또한 가관측성도 속도, 가속도 등의 에러에 비해 정밀도가 떨어지는 문제점이 있다.
본 발명은 이러한 문제점을 해결하기 위한 방안으로 외부 속도 정보를 이용한 가속계 자세 보상 및 항법 필터 가관측성 향상 방안에 대하여 제안한다.
본 발명에서 제안하는 방법은 자세/각속도 에러를 빠르고 정확하게 보상하기 위하여 새로운 센서를 이용하는 것이 아니라 현재 있는 속도 정보를 이용한다. 기본적으로 자세 계산은 자이로가 수행하게 되며 가속도계도 자세 계산이 가능하다. 가속도계는 기본적으로 선형 가속도, 회전 가속도, 중력 가속도 등을 측정하고 있는데, 중력 가속도를 측정할 때만 자세 계산이 가능하다.
현재 있는 속도 정보와 자이로가 측정하는 각속도 출력값을 이용하면 가속도계의 선형 가속도, 회전 가속도 등을 보상할 수 있다. 선형 가속도, 회전 가속도 등이 보상된 가속도계는 수중 운동체가 움직이더라도 롤, 피치 등의 정보를 통해 정밀한 자세 계산을 할 수 있다. 이렇게 계산된 자세 정보를 필터의 측정치로 사용하게 되면, 자이로에 대한 자세/각속도 에러를 더 빠르고 정확하게 보상하는 것이 가능해진다.
이하 도면을 참조하여 본 발명에 대해 자세하게 설명한다.
도 1은 본 발명의 일실시예에 따른 복합 항법 제어 시스템을 개략적으로 도시한 개념도이다.
도 1에 따르면, 복합 항법 제어 시스템(100)은 관성 측정 장치(IMU; 110), 관성 항법 처리부(120), 항법 필터(130), 결합부(140), 외부 속도 측정부(150), 가속도 보상 처리부(160) 및 자세 계산부(170)를 포함한다.
관성 측정 장치(110)은 자이로 센서(111)와 가속도계(112)를 포함한다. 자이로 센서(111)는 수중 운동체의 각속도를 측정하는 기능을 수행한다. 가속도계(112)는 선형 가속도, 회전 가속도, 중력 가속도 등 수중 운동체의 가속도를 측정하는 기능을 수행한다. 자이로 센서(111)에 의해 측정된 수중 운동체의 각속도와 가속도계(112)에 의해 측정된 수중 운동체의 가속도는 관성 항법 처리부(120)로 전송된다. 또한 자이로 센서(111)에 의해 측정된 수중 운동체의 각속도는 가속도 보상 처리부(160)로 전송된다. 상기에서 수중 운동체의 각속도는 수중 운동체의 선형 가속도와 회전 가속도를 보상시키기 위한 것이다.
관성 항법 처리부(120)는 수중 운동체의 각속도, 가속도 등을 기초로 수중 운동체의 위치, 속도, 자세 등을 산출하는 기능을 수행한다. 이러한 관성 항법 처리부(120)는 관성 항법 알고리즘을 이용하여 수중 운동체의 위치, 속도, 자세 등을 산출할 수 있다. 관성 항법 처리부(120)는 수중 운동체의 속도가 산출되면 이 속도를 결합부(140)로 전송한다.
외부 속도 측정부(150)는 수중 운동체의 외부 속도를 측정하는 기능을 수행한다. 상기에서 수중 운동체의 외부 속도도 수중 운동체의 선형 가속도와 회전 가속도를 보상시키기 위한 것이다. 외부 속도 측정부(150)는 수중 운동체의 외부 속도가 측정되면 이 외부 속도를 결합부(140)와 가속도 보상 처리부(160)로 전송한다.
가속도 보상 처리부(160)는 가속도계(112)에 의해 측정된 수중 운동체의 선형 가속도와 회전 가속도를 보상하는 기능을 수행한다. 이러한 가속도 보상 처리부(160)는 자이로 센서(111)에 의해 측정된 수중 운동체의 각속도와 외부 속도 측정부(150)에 의해 측정된 수중 운동체의 외부 속도를 기초로 수중 운동체의 선형 가속도와 회전 가속도를 보상할 수 있다. 가속도 보상 처리부(160)는 선형 가속도 보상 알고리즘 및 회전 가속도 보상 알고리즘을 이용하여 수중 운동체의 선형 가속도와 회전 가속도를 보상할 수 있다.
가속도 보상 처리부(160)는 선형 가속도 보상 알고리즘, 회전 가속도 보상 알고리즘 등 가속도 보상 알고리즘을 통해 수중 운동체의 중력 가속도를 산출하는 기능을 수행한다. 가속도계(112)는 수중 운동체의 선형 가속도, 회전 가속도, 중력 가속도 등을 측정한다. 가속도 보상 알고리즘을 통해 수중 운동체의 선형 가속도, 회전 가속도 등이 보상되면, 가속도 보상 처리부(160)는 이를 토대로 수중 운동체의 중력 가속도도 보상하여 보다 정확한 중력 가속도를 산출하는 것이 가능해진다.
본 발명에서 가속도계(112)에 의해 측정된 정보로 수중 운동체의 자세를 구하는 원리는 가속도계(112)에 의해 측정된 중력가속도 성분을 이용하여 계산한다. 후술하겠지만, 본 발명에서 이러한 기능은 자세 계산부(170)에 의해 수행된다. 자세 계산부(170)는 다음 수학식 1을 통해 산출된 중력가속도 성분을 기초로 수중 운동체의 자세를 산출한다.
Figure 112017105844273-pat00001
상기에서 g는 중력 가속도를 의미하며, φ와 θ는 각각 롤각과 피치각을 의미한다. 또한 T는 전치(transpose)를 의미한다.
그런데 앞서 설명한 바와 같이 가속도계(112)는 수중 운동체의 중력 가속도 뿐만 아니라 수중 운동체의 선형 가속도, 회전 가속도 등도 측정한다. 그래서 수중 운동체가 움직이고 있는 상황에서는 자세 계산이 부정확해질 수밖에 없다. 따라서 수중 운동체의 선형 가속도, 회전 가속도 등을 보상하지 않고 수중 운동체의 자세를 계산하여 이 값을 항법 필터(130)의 측정치로 사용하게 되면, 항법 필터(130)는 부정확한 자세값을 추정하여 사용할 수 없다.
이와 같이 수중 운동체의 자세값을 사용할 수 없게 되면, 항법 필터(130)의 측정치는 속도 정보뿐이며, 이로 인해 자세/각속도 관련 바이어스는 추정 가능하나 가관측성 부족으로 인해 추정 속도가 느려지는 문제점이 있다.
따라서 본 발명은 수중 운동체의 선형 가속도, 회전 가속도 등을 보상함으로써 이러한 문제점을 해결하고자 한다.
수중 운동체의 선형 가속도의 경우, 가속도 보상 처리부(160)는 다음과 같은 방식으로 수중 운동체의 선형 가속도를 보상한다.
수학식 2 내지 4는 수중 운동체의 선형 가속도와 관련된 것이다. 수학식 2는 동체 좌표계 상의 x축 가속도를 나타낸 것이며, 수학식 3은 동체 좌표계 상의 y축 가속도를 나타낸 것이다. 또한 수학식 4는 동체 좌표계 상의 z축 가속도를 나타낸 것이다.
Figure 112017105844273-pat00002
상기에서 fx, fy 및 fz는 각각 x축 가속도, y축 가속도 및 z축 가속도를 의미한다. u, v 및 w는 각각 x축 속도, y축 속도 및 z축 속도를 의미하며, u dot, v dot 및 w dot는 각각 u, v 및 w의 미분치를 의미한다. 또한 p, q 및 r은 각각 x축 각속도, y축 각속도 및 z축 각속도를 의미한다.
가속도 보상 처리부(160)는 수중 운동체의 선형 가속도를 보상하기 위해 수중 운동체의 속도 정보를 차분하여 계산한다. 이때 단순 차분은 노이즈를 유발하므로, 칼만 필터를 설계하여 수중 운동체의 선형 가속도를 추정한다. 가속도 보상 처리부(160)는 다음 수학식 3 및 4를 이용하여 칼만 필터를 설계한 후 최종적으로 수중 운동체의 선형 가속도를 추정할 수 있다.
수학식 3은 칼만 필터의 상태 공식(state equation)을 나타낸 것이고, 수학식 4는 칼만 필터의 측정 공식(measurement equation)을 나타낸 것이다.
Figure 112017105844273-pat00003
상기에서 x(k)는 k번째 상태 변수를 의미하며, x(k-1)은 k-1번째 상태 변수를 의미한다. A는 상태(process) 모델을 의미하며, w(k)는 예측 오차에 해당하는 상태 노이즈(process noise)를 의미한다. N(0, 1)은 평균이 0이고 분산이 1인 백색 가우시안 잡음을 의미한다.
Figure 112017105844273-pat00004
상기에서 z(k)는 k번째 측정 변수를 의미한다. H는 측정(measurement) 모델을 의미하며, v(k)는 실측 잡음에 해당하는 측정 노이즈(measurement noise)를 의미한다. N(0, 5)는 평균이 0이고 분산이 5인 백색 가우시안 잡음을 의미한다.
수학식 3에서 x(k)는 다음 수학식 5를 통해 구할 수 있다.
Figure 112017105844273-pat00005
상기에서 vx b(k)는 k번째 속도를 의미하며, dot vx b(k)는 k번째 가속도를 의미한다.
본 발명에서 가속도 보상 처리부(160)는 칼만 필터를 설계하여 수중 운동체의 선형 가속도를 추정한다. 선형 가속도의 추정시 일반적인 차분은 노이즈를 증폭시키기 때문에, 본 발명에서는 이 점을 참작하여 칼만 필터를 설계하여 계산한다. 이때 수학식 5를 기초로 k번째 속도를 이용하여 k번째 가속도를 추정할 수 있는데, 아래 수학식 6의 A 매트릭스와 H 매트릭스를 이용하면 속도와 가속도 사이의 관계식은 다음과 같이 나타낼 수 있다.
k번째 속도 = k-1번째 속도 + k-1번째 가속도 × dt(sampling)
Figure 112017105844273-pat00006
한편 수중 운동체의 회전 가속도의 경우, 가속도 보상 처리부(160)는 외부 속도 측정부(150)에 의해 측정된 수중 운동체의 외부 속도, 자이로 센서(111)에 의해 측정된 수중 운동체의 각속도 등을 기초로 수중 운동체의 회전 가속도를 보상한다. 가속도 보상 처리부(160)는 다음 수학식 7을 이용하여 수중 운동체의 회전 가속도를 보상할 수 있다.
Figure 112017105844273-pat00007
상기에서 wib b는 바디 프레임(body frame)에서 바라본 관성 프레임(inertial frame)에 대해 측정되는 각속도를 의미하며, vb는 바디 프레임의 속도를 의미한다.
자세 계산부(170)는 가속도 보상 처리부(160)에 의해 산출된 수중 운동체의 중력 가속도를 기초로 수중 운동체의 현재 자세를 산출하는 기능을 수행한다. 자세 계산부(170)는 이렇게 산출된 수중 운동체의 현재 자세를 결합부(140)로 전송한다.
결합부(140)는 관성 항법 처리부(120)에 의해 산출된 수중 운동체의 속도, 외부 속도 측정부(150)에 의해 측정된 수중 운동체의 외부 속도, 자세 계산부(170)에 의해 산출된 수중 운동체의 자세 등을 하나의 정보로 결합시켜 이 결합 정보를 항법 필터(130)로 전송하는 기능을 수행한다. 결합부(140)는 수중 운동체의 속도, 수중 운동체의 외부 속도, 수중 운동체의 자세 등을 하나의 정보로 결합시키지 않고 각각의 정보로 항법 필터(130)로 전송하는 것도 가능하다. 자세 계산부(170)에 의해 산출된 수중 운동체의 자세는 이렇게 결합부(140)를 통해 항법 필터(130)의 측정치로 사용될 수가 있다.
항법 필터(130)는 속도, 자세, 가속도, 자이로 바이어스 에러 등에 대한 추정치를 검출하는 기능을 수행한다. 본 발명에서는 외부 속도 측정부(150), 가속도 보상 처리부(160) 등을 구비함으로써, 항법 필터(130)는 종래의 필터에 대비하여 자세, 자이로 바이어스 에러 등에 대해 추정 성능이 향상된 추정치를 검출하는 것이 가능해진다.
이상 도 1을 참조하여 본 발명에 대하여 설명하였다. 이상 설명한 본 발명의 특징을 요약하여 보면 다음과 같다.
첫째, 추가로 장착되는 자세 센서 없이 항법 필터의 가관측성을 향상시킬 수 있다.
둘째, 외부 속도 정보와 알고리즘을 이용하여 가속도계의 선형 가속도와 회전 가속도를 보상할 수 있다.
셋째, 중력 가속도를 이용하여 자세 계산 후 항법 필터의 측정치로 사용할 수 있다.
넷째, 가관측성 향상에 따른 자세, 각속도 등의 오차 보상도 기존 필터에 대비하여 빠른 시간에 이루어질 수 있다.
이상 설명한 본 발명은 무인 잠수정 등 수중 운동체의 복합 항법에 적용될 수 있다. 또한 본 발명은 지상 무기 체계, 공중 무기 체계 등의 복합 항법에도 적용될 수 있다.
이상 도 1을 참조하여 본 발명의 일실시 형태에 대하여 설명하였다. 이하에서는 이러한 일실시 형태로부터 추론 가능한 본 발명의 바람직한 형태에 대하여 설명한다.
도 2는 본 발명의 바람직한 실시예에 따른 수중 운동체 항법 제어 장치의 내부 구성을 개략적으로 도시한 개념도이다.
도 2에 따르면, 수중 운동체 항법 제어 장치(200)는 속도 획득부(210), 각속도 획득부(220), 가속도 보상부(230), 자세 산출부(240), 항법 제어부(250), 전원부(260) 및 주제어부(270)를 포함한다.
전원부(260)는 수중 운동체 항법 제어 장치(200)를 구성하는 각 구성에 전원을 공급하는 기능을 수행한다.
주제어부(270)는 수중 운동체 항법 제어 장치(200)를 구성하는 각 구성의 전체 작동을 제어하는 기능을 수행한다.
속도 획득부(210)는 수중 운동체의 속도를 획득하는 기능을 수행한다. 속도 획득부(210)는 도 1의 외부 속도 측정부(150)에 대응하는 개념이다.
각속도 획득부(220)는 수중 운동체의 각속도를 획득하는 기능을 수행한다. 각속도 획득부(220)는 도 1의 자이로 센서(111)에 대응하는 개념이다.
가속도 보상부(230)는 수중 운동체의 속도와 수중 운동체의 각속도를 기초로 수중 운동체의 가속도를 보상하는 기능을 수행한다. 가속도 보상부(230)는 도 1의 가속도 보상 처리부(160)에 대응하는 개념이다.
가속도 보상부(230)는 수중 운동체의 가속도로 수중 운동체의 선형 가속도와 수중 운동체의 회전 가속도를 보상할 수 있다. 선형 가속도를 보상하는 경우, 가속도 보상부(230)는 칼만 필터를 이용하여 수중 운동체의 속도를 차분하여 얻은 결과를 기초로 수중 운동체의 선형 가속도를 보상할 수 있다.
칼만 필터를 이용하는 경우, 가속도 보상부(230)는 칼만 필터를 설계하는 데에 이용되는 상태 모델과 관련된 제1 백색 가우시안 잡음, 제1 백색 가우시안 잡음과 관련된 예측 오차, 칼만 필터를 설계하는 데에 이용되는 측정 모델과 관련된 제2 백색 가우시안 잡음, 및 제2 백색 가우시안 잡음과 관련된 실측 잡음을 기초로 수중 운동체의 선형 가속도를 보상할 수 있다. 이때 제2 백색 가우시안 잡음은 제1 백색 가우시안 잡음과 평균값이 동일하고 제1 백색 가우시안 잡음보다 분산값이 더 큰 것일 수 있다.
자세 산출부(240)는 수중 운동체의 가속도가 보상되면 이 수중 운동체의 가속도를 기초로 수중 운동체의 자세를 산출하는 기능을 수행한다. 자세 산출부(240)는 도 1의 자세 계산부(170)에 대응하는 개념이다.
자세 산출부(240)는 수중 운동체의 중력 가속도, 수중 운동체의 롤 각(roll angle) 및 수중 운동체의 피치 각(pitch angle)을 기초로 수중 운동체의 자세를 산출할 수 있다.
항법 제어부(250)는 수중 운동체의 속도, 수중 운동체의 가속도 및 수중 운동체의 자세를 기초로 수중 운동체의 항법(navigation)을 제어하는 기능을 수행한다. 항법 제어부(250)는 도 1의 관성 항법 처리부(120)에 대응하는 개념이다.
다음으로 수중 운동체 항법 제어 장치의 작동 방법에 대하여 설명한다.
도 3은 본 발명의 바람직한 실시예에 따른 수중 운동체 항법 제어 방법을 개략적으로 도시한 흐름도이다. 이하 설명은 도 2 및 도 3을 참조한다.
먼저 속도 획득부(210)가 수중 운동체의 속도를 획득한다(S310).
이후 각속도 획득부(220)가 수중 운동체의 각속도를 획득한다(S320). 본 발명에서는 S320 단계가 S310 단계와 동시에 수행되거나, S310 단계보다 먼저 수행되는 것도 가능하다.
이후 가속도 보상부(230)가 수중 운동체의 속도와 수중 운동체의 각속도를 기초로 수중 운동체의 가속도를 보상한다(S330).
이후 자세 산출부(240)가 보상된 수중 운동체의 가속도를 기초로 수중 운동체의 자세를 산출한다(S340).
이후 항법 제어부(250)가 수중 운동체의 속도, 수중 운동체의 가속도 및 수중 운동체의 자세를 기초로 수중 운동체의 항법(navigation)을 제어한다(S350).
이상에서 설명한 본 발명의 실시예를 구성하는 모든 구성요소들이 하나로 결합하거나 결합하여 동작하는 것으로 기재되어 있다고 해서, 본 발명이 반드시 이러한 실시예에 한정되는 것은 아니다. 즉, 본 발명의 목적 범위 안에서라면, 그 모든 구성요소들이 하나 이상으로 선택적으로 결합하여 동작할 수도 있다. 또한, 그 모든 구성요소들이 각각 하나의 독립적인 하드웨어로 구현될 수 있지만, 각 구성요소들의 그 일부 또는 전부가 선택적으로 조합되어 하나 또는 복수개의 하드웨어에서 조합된 일부 또는 전부의 기능을 수행하는 프로그램 모듈을 갖는 컴퓨터 프로그램으로서 구현될 수도 있다. 또한, 이와 같은 컴퓨터 프로그램은 USB 메모리, CD 디스크, 플래쉬 메모리 등과 같은 컴퓨터가 읽을 수 있는 기록매체(Computer Readable Media)에 저장되어 컴퓨터에 의하여 읽혀지고 실행됨으로써, 본 발명의 실시예를 구현할 수 있다. 컴퓨터 프로그램의 기록매체로서는 자기 기록매체, 광 기록매체 등이 포함될 수 있다.
또한, 기술적이거나 과학적인 용어를 포함한 모든 용어들은, 상세한 설명에서 다르게 정의되지 않는 한, 본 발명이 속하는 기술 분야에서 통상의 지식을 가진 자에 의해 일반적으로 이해되는 것과 동일한 의미를 갖는다. 사전에 정의된 용어와 같이 일반적으로 사용되는 용어들은 관련 기술의 문맥상의 의미와 일치하는 것으로 해석되어야 하며, 본 발명에서 명백하게 정의하지 않는 한, 이상적이거나 과도하게 형식적인 의미로 해석되지 않는다.
이상의 설명은 본 발명의 기술 사상을 예시적으로 설명한 것에 불과한 것으로서, 본 발명이 속하는 기술 분야에서 통상의 지식을 가진 자라면 본 발명의 본질적인 특성에서 벗어나지 않는 범위 내에서 다양한 수정, 변경 및 치환이 가능할 것이다. 따라서, 본 발명에 개시된 실시예 및 첨부된 도면들은 본 발명의 기술 사상을 한정하기 위한 것이 아니라 설명하기 위한 것이고, 이러한 실시예 및 첨부된 도면에 의하여 본 발명의 기술 사상의 범위가 한정되는 것은 아니다. 본 발명의 보호 범위는 아래의 청구 범위에 의하여 해석되어야 하며, 그와 동등한 범위 내에 있는 모든 기술 사상은 본 발명의 권리 범위에 포함되는 것으로 해석되어야 할 것이다.

Claims (13)

  1. 수중 운동체의 속도를 획득하는 속도 획득부;
    상기 수중 운동체의 각속도를 획득하는 각속도 획득부;
    상기 수중 운동체의 속도와 상기 수중 운동체의 각속도를 기초로 상기 수중 운동체의 가속도를 보상하는 가속도 보상부;
    보상된 상기 수중 운동체의 가속도를 기초로 상기 수중 운동체의 자세를 산출하는 자세 산출부; 및
    상기 수중 운동체의 속도, 상기 수중 운동체의 가속도 및 상기 수중 운동체의 자세를 기초로 상기 수중 운동체의 항법(navigation)을 제어하는 항법 제어부;를 포함하되,
    상기 속도 획득부는 상기 수중 운동체의 선형 가속도, 회전 가속도, 중력 가속도 및 외부 속도를 획득하고,
    상기 가속도 보상부는 칼만 필터를 이용하여 상기 수중 운동체의 속도를 차분하여 얻은 결과를 기초로 상기 수중 운동체의 선형 가속도를 보상하며, 상기 수중 운동체의 외부 속도와 상기 수중 운동체의 각속도를 이용하여 상기 수중 운동체의 회전 가속도를 보상하고, 상기 보상된 선형 가속도와 회전 가속도를 기초로 상기 수중 운동체의 중력 가속도를 보상하는 것을 특징으로 하는 수중 운동체 항법 제어 장치.
  2. 삭제
  3. 삭제
  4. 제 1 항에 있어서,
    상기 가속도 보상부는 상기 칼만 필터를 설계하는 데에 이용되는 상태 모델과 관련된 제1 백색 가우시안 잡음, 상기 제1 백색 가우시안 잡음과 관련된 예측 오차, 상기 칼만 필터를 설계하는 데에 이용되는 측정 모델과 관련된 제2 백색 가우시안 잡음, 및 상기 제2 백색 가우시안 잡음과 관련된 실측 잡음을 기초로 상기 수중 운동체의 선형 가속도를 보상하는 것을 특징으로 하는 수중 운동체 항법 제어 장치.
  5. 제 4 항에 있어서,
    상기 제2 백색 가우시안 잡음은 상기 제1 백색 가우시안 잡음과 평균값이 동일하고 상기 제1 백색 가우시안 잡음보다 분산값이 더 큰 것을 특징으로 하는 수중 운동체 항법 제어 장치.
  6. 제 1 항에 있어서,
    상기 가속도 보상부는 상기 수중 운동체의 선형 가속도를 기초로 상기 수중 운동체의 회전 가속도를 보상하는 것을 특징으로 하는 수중 운동체 항법 제어 장치.
  7. 제 1 항에 있어서,
    상기 자세 산출부는 상기 수중 운동체의 중력 가속도, 상기 수중 운동체의 롤 각 및 상기 수중 운동체의 피치 각을 기초로 상기 수중 운동체의 자세를 산출하는 것을 특징으로 하는 수중 운동체 항법 제어 장치.
  8. 수중 운동체의 속도를 획득하는 속도 획득 단계;
    상기 수중 운동체의 각속도를 획득하는 각속도 획득 단계;
    상기 수중 운동체의 속도와 상기 수중 운동체의 각속도를 기초로 상기 수중 운동체의 가속도를 보상하는 가속도 보상 단계;
    보상된 상기 수중 운동체의 가속도를 기초로 상기 수중 운동체의 자세를 산출하는 자세 산출 단계; 및
    상기 수중 운동체의 속도, 상기 수중 운동체의 가속도 및 상기 수중 운동체의 자세를 기초로 상기 수중 운동체의 항법(navigation)을 제어하는 항법 제어 단계를 포함하되,
    상기 속도 획득 단계에서 상기 수중 운동체의 선형 가속도, 회전 가속도, 중력 가속도 및 외부 속도를 획득하고,
    상기 가속도 보상 단계는 칼만 필터를 이용하여 상기 수중 운동체의 속도를 차분하여 얻은 결과를 기초로 상기 수중 운동체의 선형 가속도를 보상하며, 상기 수중 운동체의 외부 속도와 상기 수중 운동체의 각속도를 이용하여 상기 수중 운동체의 회전 가속도를 보상하고, 상기 보상된 선형 가속도와 회전 가속도를 기초로 상기 수중 운동체의 중력 가속도를 보상하는 것을 특징으로 하는 수중 운동체 항법 제어 방법.
  9. 삭제
  10. 삭제
  11. 제 8 항에 있어서,
    상기 가속도 보상 단계는 상기 칼만 필터를 설계하는 데에 이용되는 상태 모델과 관련된 제1 백색 가우시안 잡음, 상기 제1 백색 가우시안 잡음과 관련된 예측 오차, 상기 칼만 필터를 설계하는 데에 이용되는 측정 모델과 관련된 제2 백색 가우시안 잡음, 및 상기 제2 백색 가우시안 잡음과 관련된 실측 잡음을 기초로 상기 수중 운동체의 선형 가속도를 보상하는 것을 특징으로 하는 수중 운동체 항법 제어 방법.
  12. 제 8 항에 있어서,
    상기 가속도 보상 단계는 상기 수중 운동체의 선형 가속도를 기초로 상기 수중 운동체의 회전 가속도를 보상하는 것을 특징으로 하는 수중 운동체 항법 제어 방법.
  13. 제 8 항에 있어서,
    상기 자세 산출 단계는 상기 수중 운동체의 중력 가속도, 상기 수중 운동체의 롤 각 및 상기 수중 운동체의 피치 각을 기초로 상기 수중 운동체의 자세를 산출하는 것을 특징으로 하는 수중 운동체 항법 제어 방법.
KR1020170139975A 2017-10-26 2017-10-26 수중 운동체 항법 제어 장치 및 방법 KR101910515B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020170139975A KR101910515B1 (ko) 2017-10-26 2017-10-26 수중 운동체 항법 제어 장치 및 방법

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020170139975A KR101910515B1 (ko) 2017-10-26 2017-10-26 수중 운동체 항법 제어 장치 및 방법

Publications (1)

Publication Number Publication Date
KR101910515B1 true KR101910515B1 (ko) 2018-10-22

Family

ID=64102304

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020170139975A KR101910515B1 (ko) 2017-10-26 2017-10-26 수중 운동체 항법 제어 장치 및 방법

Country Status (1)

Country Link
KR (1) KR101910515B1 (ko)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117953007A (zh) * 2024-03-13 2024-04-30 长春通视光电技术股份有限公司 一种基于图像匹配的线性运动补偿控制方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101049362B1 (ko) * 2006-06-23 2011-07-13 도요타지도샤가부시키가이샤 자세각 검출 장치와 자세각 검출 방법

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101049362B1 (ko) * 2006-06-23 2011-07-13 도요타지도샤가부시키가이샤 자세각 검출 장치와 자세각 검출 방법

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117953007A (zh) * 2024-03-13 2024-04-30 长春通视光电技术股份有限公司 一种基于图像匹配的线性运动补偿控制方法
CN117953007B (zh) * 2024-03-13 2024-06-11 长春通视光电技术股份有限公司 一种基于图像匹配的线性运动补偿控制方法

Similar Documents

Publication Publication Date Title
US9915550B2 (en) Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer
KR101565362B1 (ko) 자이로스코프 컨디셔닝 및 자이로-카메라 정렬
JP3947531B2 (ja) 加速度誤差の補正方法及び装置、並びにそれを利用した慣性航法システム
KR101950359B1 (ko) 하이브리드 모션캡쳐 시스템의 위치 추정 방법
EP3321631A1 (en) A inertial and terrain based navigation system
JP5139412B2 (ja) 角度測定の方法およびそれを実施するための角度測定ジャイロシステム
KR100898169B1 (ko) 관성항법시스템의 초기정렬 방법
JP2009074859A (ja) 運動計測装置及び位置計測装置
EP3388788B1 (en) Inertially-aided air data computer altitude rate
CN107990901B (zh) 一种基于传感器的用户方向定位方法
JP6383907B2 (ja) 車輌位置計測装置及び方法
JP2019120587A (ja) 測位システム及び測位方法
JP2021179438A (ja) 機首方位測定システムにおけるセンサ測定の欠如を補償するシステムと方法
EP3227634B1 (en) Method and system for estimating relative angle between headings
KR101910515B1 (ko) 수중 운동체 항법 제어 장치 및 방법
JP2007232444A (ja) 慣性航法装置およびその誤差補正方法
KR101257935B1 (ko) 관성 항법 시스템의 바이어스 추정치를 이용한 정렬 장치 및 그 항법 시스템
JP5190134B2 (ja) 角速度検出方法及びその装置
KR101880940B1 (ko) 자세 제어 방법 및 그를 적용한 드론
WO2019186677A1 (ja) ロボット位置姿勢推定・三次元計測装置
JP7152285B2 (ja) 電子機器、補正方法およびプログラム
JP2008256381A (ja) 姿勢角計測装置及び該姿勢角計測装置に用いられる姿勢角計測方法
Kundra et al. Bias compensation of gyroscopes in mobiles with optical flow
TW201416815A (zh) 用於六軸運動姿態感測之方法及其感測系統
KR102393016B1 (ko) 휴대용 장치의 방위를 추정하기 위한 방법

Legal Events

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