KR101370649B1

KR101370649B1 - 무인잠수정의 경로 제어방법

Info

Publication number: KR101370649B1
Application number: KR1020120097601A
Authority: KR
Inventors: 정훈상; 이필엽; 이원희; 원건희
Original assignee: 주식회사 한화
Priority date: 2012-09-04
Filing date: 2012-09-04
Publication date: 2014-03-10

Abstract

본 발명은 무인잠수정의 경로 제어방법에 관한 것으로서, 더욱 구체적으로는 조류를 포함하는 해류 측정을 위한 고가의 해류 측정 센서를 사용하지 않고, 실시간 속도정보와 사용자가 설정한 경로의 방향 차이를 계산하여 해류의 영향을 추정하여, 실시간으로 해류의 영향을 상쇄시킴으로써, 해류가 강한 해역에서도 무인잠수정의 경로를 유지하고 조종성을 확보할 수 있는 무인잠수정의 경로 제어방법에 관한 것이다.

Description

무인잠수정의 경로 제어방법 {Route Control Method For the Autonomous Underwater Vehicle}

무인잠수정은 사용자가 미리 정의한 경로점(Waypoint)을 따라 이동하면서, 측면 주사 소나 등의 초음파 센서를 이용하여 해저면을 탐색한다. 정확한 해저면 탐색을 위해서는 사용자가 정의한 경로를 벗어나지 않고 유지하는 것이 필요하다. 이를 위해 무인잠수정의 종래 경로 제어 방법은 관성 측정기(IMU, Inertial Measurement Unit), DVL(Doppler Velocity Log) 및 초음파 측위(Acoustic Positioning)에 기반한 수중항법을 통해 무인잠수정의 현재 위치, 자세, 및 속도를 계산하고 목표점으로의 각을 계산하여 무인잠수정의 기준 방향각을 제어하는 시선각(LOS, Line Of Sight) 지령 방법을 사용한다.

그러나, 조류와 같은 해류가 강한 지역에서 무인잠수정이 측면으로 강한 해류를 받게 되면, 정해진 경로를 벗어나게 되는데 상기 종래의 경로 제어 방법은 해류의 크기 및 방향을 알지 못해, 경로유지를 위한 즉각적인 제어가 어려워 무인잠수정이 경로를 크게 이탈하거나, 조종성이 상실되는 등의 문제가 발생한다.

따라서, 무인잠수정의 경로이탈을 최소화하기 위해서는 해류의 방향과 크기를 측정하고, 해류에 의해 무인잠수정에 가해지는 힘과 균형이 맞도록 무인잠수정의 방향을 변경하여 경로를 유지하는 것이 필요한데, 이를 위해서는 종래의 방법에서는 해류의 크기와 방향을 직접적으로 실시간으로 측정하였다. 구체적으로, 대형 무인잠수정은 ADCP(Acoustic Doppler Current Profiler)를 장착하거나 2개의 DVL(Doppler Velocity Log)을 무인잠수정의 상하 방향으로 장착하여, 실시간으로 무인잠수정 주변의 해류의 크기와 방향을 측정하고 이를 이용하여 무인잠수정의 속도 및 방향각을 제어하는 방법을 사용하였다.

그러나, 이러한 방법은 직경 25cm, 무게 50kg 이하의 포터블(portable)급 무인잠수정에는 공간과 비용의 제약으로 적용이 어렵고, ADCP 및 2개의 DVL 의 오차에 의한 정확한 속도 및 방향각의 제어가 어렵다는 문제점이 있다.

본 발명은 상기와 같은 문제점을 해결하기 위한 것으로, 조류를 포함하는 해류 측정을 위한 고가의 해류 측정 센서를 사용하지 않고, 실시간 속도정보와 사용자가 설정한 경로의 방향 차이를 계산하여 해류의 영향을 측정하여, 실시간으로 해류의 영향을 상쇄시킴으로써, 해류가 강한 해역에서도 무인잠수정의 경로를 유지하고 조종성을 확보할 수 있는 무인잠수정의 경로 제어방법을 제공하는 것을 목적으로 한다.

구체적으로, 본 발명에서는 일반적으로 무인잠수정에 탑재되는 IMU 및 DVL 신호에 기반한 수중항법에서 출력되는 실시간 속도정보와 사용자가 설정한 경로의 방향 차이를 계산하고, 이 차이값에 기초하여 PID 제어기와 같은 피드백 제어를 함으로써, 해류에 의해 무인잠수정에 가해지는 힘을 상쇄시키고 사용자가 설정한 경로를 유지하기 위한 무인잠수정의 기준 방향각을 도출하는 방법을 제공하는 것을 목적으로 한다.

상기와 같은 목적을 달성하기 위하여, 본 발명은 무인잠수정의 속도 방향각

을 산출하는 속도 방향각 산출 단계; 무인잠수정의 경로 방향각

(t) 를 산출하는 경로 방향각 산출 단계; 상기 속도 방향각

과 경로 방향각

(t) 을 이용하여 경로방향 오차 방향각

(t) 을 산출하는 경로방향 오차 방향각 산출 단계; 상기 경로방향 오차 방향각

(t) 에 제어 이득을 적용하여 보정 명령 방향각

을 산출하는 보정 명령 방향각 산출 단계; 상기 보정 명령 방향각

을 이용하여 방향각 명령

을 생성하는 방향각 명령 생성 단계를 포함하는 무인잠수정의 경로 제어방법을 제공한다.

본 발명에서는, 상기 속도 방향각 산출 단계는 무인잠수정의 심도를 제외한 2차원의 실시간 위치

에서 무인 잠수정의 실제 이동 방향의 속도를 실시간으로 산출하여, 이를 속도 방향각

으로 할 수도 있다.

본 발명에서는, 상기 경로 방향각 산출 단계는, 이전 경로점과 현재 경로점 사이의 방향각을 산출하여, 이를 경로 방향각

(t) 으로 할 수도 있다.

본 발명에서는, 상기 경로방향 오차 방향각 산출 단계는, 상기 속도 방향각

으로부터 상기 경로 방향각

(t) 을 차감하여, 이를 경로방향 오차 방향각

(t) 으로 산출할 수도 있다.

본 발명에서는, 상기 보정 명령 방향각 산출 단계는 상기 경로방향 오차 방향각

(t) 에 비례 적분 이득값을 적용하여 보정 명령 방향각

을 산출할 수도 있다.

본 발명에서는, 상기 방향각 명령 생성 단계는,

는 무인잠수정의 현재 위치이고,

는

번째 경로점의 위치이고,

는 현재 시점에서 (

)에서 그 다음 경로지점(

) 으로 향하기 위한 목표 방향각이고,

는 현재 위치/시점 (

)에서의 무인잠수정의 앞쪽이 향하는 방향을 나타내는 선수 방향각이라 할 때,

,

, 및

의 식에 의하여 도출된 차이 방향각

에 상기 보정 명령 방향각

을 더하여, 방향각 명령

을 생성할 수도 있다.

상기와 같은 목적을 달성하기 위하여, 본 발명은 무인잠수정의 이동 상태를 측정하는 센서부; 상기 센서부로부터 전달받은 정보로부터 무인잠수정의 위치, 방향각, 및 속도를 계산하는 수중 복합항법부; 기설정된 경로 정보가 저장되어 있는 경로 정보부; 무인잠수정의 위치 및 방향이 상기 경로 정보부에 저장된 경로 지점에 어긋나는 정도에 기초하여, 차이 방향각

을 도출하는 시선각 지령부; 무인잠수정의 속도가 상기 경로 정보부에 저장된 경로 방향에 어긋나는 정도에 기초하여, 보정 명령 방향각

을 도출하는 해류 보정부; 상기 차이 방향각

와 경로오차 보정 명령 방향각

에 기초하여, 방향각 명령

을 계산하는 중간 처리부; 및 상기 방향각 명령

에 따라 선수각을 제어하는 선수각 제어기를 포함하는 것을 특징으로 하는 무인잠수정의 경로 제어장치를 제공한다.

상기와 같은 목적을 달성하기 위하여, 본 발명은 무인잠수정의 상태를 측정하는 센서부; 상기 센서부로부터 전달받은 정보로부터 무인잠수정의 위치, 방향각, 및 속도를 계산하는 수중 복합항법부; 기설정된 경로 정보가 저장되어 있는 경로 정보부; 상기 수중 복합항법부로부터 계산된 무인잠수정의 위치 및 방향각 정보와 상기 경로 정보부에 저장된 경로 정보에 기초하여, 차이 방향각

을 도출하는 시선각 지령부; 상기 수중 복합항법부로부터 계산된 무인잠수정의 속도 및 상기 경로 정보부에 저장된 경로 정보에 기초하여, 경로오차 보정 명령 방향각

을 도출하는 해류 보정부; 상기 차이 방향각

과 경로오차 보정 명령 방향각

에 기초하여, 방향각 명령

을 계산하는 중간 처리부; 및 상기 방향각 명령

본 발명에서는, 상기 시선각 지령부는 무인잠수정의 현재 위치와 다음 경로점이 이루는 방향각으로부터 무인잠수정의 현재 방향각을 차감함으로써, 차이 방향각

을 도출할 수도 있다.

본 발명에서는, 상기 시선각 지령부는,

는 무인잠수정의 현재 위치이고,

는

번째 경로점의 위치이고,

는 현재 시점에서 (

)에서 그 다음 경로지점(

) 으로 향하기 위한 목표 방향각이고,

는 현재 위치/시점 (

)에서의 무인잠수정의 선수 방향각이라 할 때,

,

의 식에 의하여, 차이 방향각

을 도출할 수도 있다.

본 발명에서는, 상기 해류 보정부는 상기 저장된 경로 정보의 경로 방향과 무인잠수정의 속도 방향과의 차이에 기초하여, 경로오차 보정 명령 방향각

을 도출할 수도 있다.

본 발명에서는, 상기 해류 보정부는,

는 무인잠수정의 현재 위치를 지칭하고,

는

번째 경로점의 위치를 지칭하고,

는 현재 무인잠수정의 속도(

) 의 속도 방향각을 지칭하고,

(t) 는 이전 경로점과 현재 경로점 사이의 위치차이인 경로 방향각을 지칭하고,

(t) 는 경로 방향각

(t) 과 속도 방향각

의 차이인 경로방향 오차 방향각을 지칭한다고 할 때,

(t)

,

의 식에 의하여 경로오차 보정 명령 방향각

을 도출할 수도 있다.

본 발명에서는, 상기 중간 처리부는 경로방향 오차 방향각

(t)와 경로오차 보정 명령 방향각

을 더하여 방향각 명령

을 계산할 수도 있다.

상기와 같이 구성되는 본 발명의 무인잠수정의 경로 제어방법에 따르면 조류와 같은 해류의 측정을 위한 고가의 조류측정 센서를 사용하지 않고, 해류의 영향을 측정하여 실시간으로 해류의 영향을 상쇄시킴으로써, 해류가 강한 해역에서도 무인잠수정의 경로를 유지하고 조종성을 확보할 수 있는 효과를 발휘할 수 있다.

도 1은 본 발명의 제1 실시예의 무인잠수정의 경로 제어방법에 대한 구성 블록도를 도시한다.
도 2 는 본 발명의 제1 실시예에서의 무인잠수정의 좌표계를 도시한다.
도 3은 본 발명의 제2 실시예의 무인잠수정의 경로 제어방법에 대한 구성 블록도를 도시한다.
도 4 는 본 발명의 제2 실시예에서의 무인잠수정의 좌표계를 도시한다.
도 5 는 해류가 있는 경우 본 발명의 제1 실시예의 경로 제어방법의 진행과정을 도시한다.
도 6 는 해류가 있는 경우 본 발명의 제2 실시예의 경로 제어방법의 작동 원리를 도시한다.
도 7 는 해류가 있는 경우 본 발명의 제2 실시예의 경로 제어방법의 진행과정을 도시한다.
도 8 은 본 발명의 시뮬레이션 설계 블록도를 도시한다.
도 9a ~ 도 9c 는 본 발명의 제1 실시예의 시뮬레이션 결과를 도시한다.
도 10a ~ 도 10c 는 본 발명의 제2 실시예의 시뮬레이션 결과를 도시한다.
도 11 은 본 발명의 제1 실시예와 제2 실시예의 비교 결과를 도시한다.

이하, 본 발명에 따른 무인 잠수정의 경로 제어방법 및 장치의 구성을 첨부된 도면을 참조하여 상세하게 설명하면 다음과 같다.

(제1 실시예)

도 1 은 본 발명의 제1 실시예의 무인잠수정의 경로 제어방법에 대한 구성 블록도이고, 도 2 는 본 발명의 제1 실시예에서의 무인잠수정의 좌표계를 도시한다.

도 1 에 도시된 바와 같이, 무인잠수정의 제어부(1)는 추력제어기(1), 선수각 제어기(2), 심도 제어기(3)로 구성된다. 또한, 무인잠수정의 센서부(10)는 3축 가속도 및 각속도를 측정하는 관성측정기(IMU, 11), 3축 속도를 측정하는 속도계(DVL), 심도를 측정하는 심도계(Depthmeter), 방향각을 측정하는 방위계(Magnetometer)로 구성된다. 또한, 무인 잠수정의 상기 센서부(10)에서 입력된 정보들, 예를 들어 3축 가속도, 각속도, 3축 속도, 심도 및 방향각은 수중 복합항법부(20)에 전달된다.

한편, 수중 복합항법부(20)는 상기 입력된 정보를 기초로 하여, 현재 속도 및 자세(방향각) 정보를 생성하여 기준속도 생성기(30)에 입력한다. 이와 같은 수중 복합항법부(20)가 센서부(10)로부터 입력된 정보로부터 현재 속도 및 자세(방향각) 정보를 생성하는 것은 해당 분야에서 일반적으로 사용되는 다양한 방법으로 행해질 수 있는 바, 이에 대한 구체적인 설명은 생략한다.

기준속도 생성기(30)는 입력된 정보에 의하여, 기준속도를 계산한다. 이후, 기준속도 생성기(30)에서 계산된 기준속도와 수중 복합항법부(20)에서 계산된 현재 속도는 제어부(1)의 추력제어기(2)에 입력되고, 추력 제어기(2)는 수중 복합항법부 (20)에서 계산된 현재 속도와 기준속도 생성기(30)에서 계산된 사용자가 설정한 목표 추진속도인 상기 기준속도의 차이가 줄어들도록, 이를 입력값으로 하여, PID 제어기(도시됨)나 슬라이딩 모드 제어기 등의 다양한 방법을 이용하여 추진 모터의 RPM을 설정하도록 하는 추력 입력 정보를 생성한다. 이렇게 생성된 추력 입력은 추진 모터(100)에 입력된다.

또한, 수중 복합항법부(20)는 상기 입력된 정보를 기초로 하여, 현재 위치 및 자세(방향각) 정보를 시선각(LOS) 지령부(40)에 입력한다. 이와 같은 수중 복합항법부(20)가 센서부(10)에서 입력된 정보로부터 현재 속도 및 자세(방향각) 정보를 생성하는 것은 해당 분야에서 일반적으로 사용되는 다양한 방법으로 행해질 수 있는 바, 이에 대한 구체적인 설명은 생략한다.

이러한 시선각 지령부(40)는 입력된 정보를 기초로 명령 방향각

을 생성하여, 이를 선수각 제어기(3)에 입력한다. 명령 방향각

은 시선각 지령부(40)에서 계산되는 현재 무인잠수정의 위치에서 경로점까지의 목표 방향각

과 수중 복합항법부(20)에서 계산되는 무인잠수정의 선수 방향각

의 차이이다. 선수각 제어기(3)는 명령 방향각

의 크기가 줄어들도록 명령 방향각

을 입력값으로 하여, 명령 방향각

의 값이 줄어들도록 피드백 제어, 예를 들어 PID 제어를 하여 러더 (rudder) 입력 정보를 만들어, 이를 선수각 구동부(200)에 입력한다.

한편, 수중 복합항법부(20)는 상기 입력된 정보를 기초로 하여, 현재 심도 정보를 생성하여 이를 심도제어기(4)에 입력하고, 경로 정보부(50)는 기준 심도 정보를 심도 제어기(4)에 입력한다. 이후, 심도 제어기(4)는 수중 복합항법부(20)에서 입력 받은 현재 심도 정보와 경로 정보부(50)에서 입력 받은 기준 심도 정보를 제어기 입력값으로 하여, 이러한 차이값이 줄어들도록 피드백 제어, 예를 들어 PID 제어를 하여, 무인잠수정의 선미 (stern) 입력 정보를 출력하여, 이를 선미부(300)에 입력한다.

이하에서는, 무인잠수정의 좌표계를 도시하는 도 2 를 참조하여, 시선각 지령부(40)가 명령 방향각

을 생성하는 방법에 대하여 설명하도록 한다.

도 2 에서,

는 무인잠수정의 현재 위치를 지칭하고,

는

번째 경로점의 위치를 지칭하고,

는 현재 위치/시점(

)에서 그 다음 경로지점(

) 으로 향하기 위한 목표 방향각,

는 현재 위치/시점 (

)에서의 무인잠수정의 선수 방향각을 의미하며, 차이 방향각

는 목표 방향각

와 선수 방향각

의 차이로서, 이는 명령 방향각

과 동일하다.

이와 같은 목표 방향각

및 명령 방향각

(

와 동일)을 계산하는 계산식은 다음과 같다.

(식 1)

(식 2)

,

(식 3)

(식 4)

즉, 본 발명의 제1 실시예에서는 시선각 지령부(40)에서 명령 방향각

을 생성하여, 이를 선수각 제어기(3)로 입력시키고, 선수각 제어기(3)는 명령 방향각

의 값이 줄어들도록 선수각 구동부(200)에 러더 입력 정보를 보내고, 이를 피드백 제어, 예를 들어

를 입력값으로 하는 PID 제어를 한다.

이와 같은 본 발명의 제1 실시예에서는 비교적 간단한 구조에 의하여, 외부의 영향에 의하여 무인잠수정의 경로가 초기 설정된 경로에 어긋나는 경우, 무인잠수정의 방향을 지속적으로 보정하면서, 무인잠수정의 경로를 제어할 수 있다.

(제2 실시예)

일반적으로 조류를 포함하는 해류는 잠수정 및 선박과 같은 수중운동체의 운동의 방향과 속도에 영향을 미치는데, 운동체의 크기가 작을수록 상대적으로 많은 영향을 받게 된다. 한편, 조류가 강한 지역의 경우, 최대 5노트 이상의 강한 조류가 발생할 수 있고, 이는 소형 무인잠수정의 경로 제어방법에 심각한 영향을 줄 수 있다.

한편, 본 발명의 제1 실시예의 시선각(LOS 각)을 이용한 무인잠수정의 방향각 제어 방식은 조류와 같은 지속적인 외란이 없는 경우, 외부 영향특정 경로점으로 무인잠수정을 이동시키는데 효과적이다.

그러나, 상기 제1 실시예에서는 조류와 같은 지속적이고 강한 외란이 발생하여, 무인잠수정의 위치가 경로에서 멀어지는 경우 효과적인 제어가 어렵다는 문제점이 있다. 즉, 조류는 무인잠수정 선체에 힘을 가하고, 그에 따라 무인잠수정의 속도 방향과 크기를 변화시켜, 무인잠수정의 위치와 방향이 원래 경로에서 벗어나고, 목표 경로점까지의 거리가 경로에서 이탈한 거리에 비해 멀리 있으면, 목표 방향각

이 민감하게 변경되지 않아, 무인잠수정이 경로에서 크게 이탈하게 되는 문제점이 발생한다.

이러한 문제점을 해결하기 위해서는 조류 정보를 실시간으로 측정할 수 있는 센서를 추가로 장착하여야 하는데, 소형급 무인잠수정에는 추가 센서 장착이 공간적으로 어려우며 비용이 추가되는 문제가 있다.

본 발명의 제2 실시예는 상기와 같은 문제점을 해결하면서, 별도의 조류 정보를 측정하는 장치 없이, 기존의 센서에 의하여 획득된 정보를 기반으로 조류의 영향을 최소화할 수 있는 무인잠수정의 경로 제어 방법에 관한 것이다.

도 3 은 본 발명의 제2 실시예의 무인잠수정의 경로 제어방법에 대한 구성 블록도이고, 도 4 는 본 발명의 제2 실시예에서의 무인잠수정의 좌표계를 도시한다. 이하의 제2 실시예에 대한 설명에서는 제1 실시예와 동일한 구성인 도 1 에서의 추력모터(100)의 제어와 관련된 기준속도 생성기(30) 및 추력 제어기(1) 및 심도와 관련된 선미부(300)의 제어와 관련된, 심도 제어기(4)에 대한 설명은 생략하기로 한다.

도 3 에 도시된 바와 같이, 본 발명의 제2 실시예에서는 제1 실시예와 비교 시, 해류 보정부(60) 및 중간처리부(70)이 추가됨으로써, 별도의 조류 정보를 측정하는 장치 없이, 기존의 센서에 의하여 획득된 정보를 기반으로 조류의 영향을 최소화할 수 있는 무인잠수정의 경로를 제어할 수 있다.

수중 복합항법부(20)은 센서부(10)로부터 입력된 정보를 기초로 하여, 위치 및 자세에 대한 정보를 생성하고, 이를 시선각 지령부(40)에 입력한 후, 이로부터 목표 방향각

와 선수 방향각

의 차이인 차이 방향각

을 계산한다. 이와 같은 과정은 제1 실시예와 동일하고, 이렇게 생성된 차이 방향각

을 중간 처리부(70)에 입력한다.

또한, 수중 복합항법부(20)는 센서부(10)로부터 입력된 정보를 기초로 하여, 현재 속도

(t) 의 정보를 해류 보정부(60)에 전달하고, 경로 정보부(50)는 경로의 위치 정보를 해류 보정부(60)에 전달한다.

전달된 현재 속도

(t) 는 해류 보정부(60)의 속도방향 계산부(61)에 입력되고, 속도방향 계산부(61)은 현재 속도

(t) 에 기초하여, 무인잠수정의 속도 방향각

(t)를 계산하여 이를 보정 제어부(63)에 입력한다.

또한, 전달된 경로 정보는 해류 보정부(60)의 경로 방향 계산부(62)에 입력되고, 경로 방향 계산부(62)는 경로 정보에 기초하여, 무인잠수정의 경로 방향각

(t)를 계산하여 이를 보정 제어부(63)에 입력한다.

한편, 보정 제어부(61)에서는 경로 방향각

(t)과 무인잠수정의 속도 방향각

(t)의 차이값을 이용하여 경로방향 오차 방향각

을 생성하고, 이를 비례 적분 이득값을 적용하여, 경로오차 보정 명령 방향각

을 계산한다.

이후, 해류 보정부(60)은 상기 계산된 경로오차 보정 명령 방향각

를 중간 처리부(70)에 입력한다.

한편, 중간 처리부(70)에서는 해류 보정부(60)으로부터 입력된 경로오차 보정 명령 방향각

과 시선각 지령부(40)으로부터 입력된 차이 방향각

의 합을 이용하여 명령 방향각

을 생성한다.

반면, 상기 제1 실시예에서는 명령 방향각

은 해류의 영향을 고려하지 않은 입력된 차이 방향각

과 동일하다.

즉, 본 발명의 제2 실시예에서는 시선각 지령부(40)에서 전달된 차이 방향각

과 해류 보정부로부터 입력된 경로오차 보정 명령 방향각

을 이용하여 명령 방향각

를 입력값으로 하는 PID 제어를 한다.

이하에서는 본 발명의 제2 실시예의 상기 방향각들에 대한 설명을 좌표계가 도시된 도 4 를 참조하여 설명하도록 한다.

도 4 에서,

는 무인잠수정의 현재 위치를 지칭하고,

는

번째 경로점의 위치를 지칭하고,

는 현재 위치/시점(

)에서 그 다음 경로지점(

) 으로 향하기 위한 목표 방향각을 지칭하고,

는 현재 위치/시점 (

)에서의 무인잠수정의 선수 방향각을 지칭하고,

는 목표 방향각

와 선수 방향각

의 차이인 차이 방향각을 지칭한다.

또한,

는 현재 무인잠수정의 속도(

) 의 속도 방향각을 지칭하고,

(t) 는 경로 방향각

(t) 과 속도 방향각

의 차이인 경로방향 오차 방향각을 지칭한다.

이와 같은 목표 방향각

및 차이 방향각

, 속도 방향각

, 경로 방향각

(t), 및 경로방향 오차 방향각

(t) 을 계산하는 계산식은 다음과 같다.

(식 1)

(식 2)

,

(식 3)

(식 5)

(t)

(식 6)

,

(식 7)

(식 8)

한편, 상기 식 1 ~ 식 3 은 제1 실시예와 동일하다.

한편, 제2 실시예에서는 경로방향 오차 방향각

(t)에 비례 적분 이득값을 곱하여 하기의 식 9 와 같이 경로오차 보정 명령 방향각

을 계산한다.

(식 9)

또한, 최종적으로 차이 방향각

과 경로오차 보정 명령 방향각

의 합을 이용하여 하기의 식 10 과 같이 명령 방향각

을 하기의 식 10 과 같이 계산한다.

(식 10)

이후, 명령 방향각

을 선수각 제어기(3)로 입력시키고, 선수각 제어기(3)는 명령 방향각

를 입력값으로 하는 PID 제어를 한다.

즉, 본 발명의 제1 실시예에서는 하기의 식 4 와 같이 명령 방향각

이 차이 방향각

과 동일한 반면, 본 발명의 제2 실시예는 속도 정보와 경로 정보를 이용하여 해류에 대한 영향을 추정하여 이를 보정하기 위한 경로오차 보정 명령 방향각

을 해류 보정부(60)에서 생성한 후에, 이를 반영하기 위하여 차이 방향각

으로부터 보정 명령 방향각

를 더하여, 최종적으로 명령 방향각

을 생성한다.

(식 4)

- 제1 실시예

(식 10)

- 제2 실시예

조류와 같은 해류가 없고 시선각 지령부(40)이 정상적으로 동작하는 상황에서는 무인잠수정의 속도방향

과 경로방향

은 대체적으로 일치하게 된다. 하지만 조류와 같은 해류가 있으면

는

와 달라지게 된다.

본 발명의 제2 실시예에서는 직접적인 해류의 크기와 방향의 측정 대신

가

로부터 달라지는 정도를 측정함으로써, 조류에 의해 인가되는 외란의 영향을 추정하고, 최종적으로

를

와 같아지도록 무인잠수정의 방향을 조정함으로써, 이러한 외란의 영향을 상쇄시키면서, 경로를 이탈하는 것을 방지할 수 있다.

이와 같은 본 발명의 제2 실시예의 무인잠수정의 경로 제어방법의 방향각 명령

계산 단계를 정리하면 다음과 같다.

(단계 1) 이전 경로점과 현재 경로점 사이의 위치 차이를 이용하여 경로 방향각

계산

(단계 2) 수중 복합항법부에서 계산되는 무인잠수정의 수평방향 속도

를 이용하여 식 7과 같이 무인잠수정의 속도 방향각

계산

(단계 3)

와

의 차이를 이용하여 경로방향 오차 방향각

계산

(단계 4)

에 비례 적분 이득값을 곱하여 경로오차 보정 명령 방향각

계산

(단계 5) 차이 방향각

에 경로오차 보정 명령 방향각을

를 더하여 방향각 명령

계산

제1 실시예와 비교하여 제2 실시예의 효과를 설명하면, 본 발명의 제1 실시예의 무인잠수정의 경로 제어방법은 간단한 구조로 경로 이탈을 보정할 수 있는 장점이 있지만, 도 5 에 도시된 바와 같이, 측면 조류와 같은 해류가 있는 경우, 해류에 의한 힘

와 추력

의 벡터 합 방향으로 경로를 이탈하다가 목표 경로점과의 거리가 가까워지고, 경로 이탈 거리가 커져야만

가 커지면서 목표점으로 선회하게 된다.

그러나, 본 발명의 제2 실시예에서는 도 6에 도시된 바와 같이 측면 조류와 같은 해류에 의한 힘

과 같은 측면으로 미는 힘에 의해 발생하는 속도방향 변화를 계산하고, 속도방향이 경로방향과 다시 같아지도록 곧바로 무인잠수정의 방향각을 조정함으로써 무인잠수정이 원래 경로에서 크게 이탈하는 것을 방지하여 도 7과 경로를 따라 진행할 수 있게 한다.

즉 본 발명의 제2 실시예에서는 무인잠수정의 추력의 방향을 조정함으로써 조류와 같은 해류에 의한 힘을 상쇄시키고 전체 힘 방향이 경로 방향과 일치하도록 할 수 있다.

이와 같은 제2 실시예의 무인잠수정의 경로 제어방법은 무인잠수정이 경로에서 벗어난 위치값을 이용하지 않고, 무인잠수정의 속도변화를 이용함으로써 경로이탈을 빠르게 방지할 수 있다는 특징이 있으며, 기존 무인잠수정에 탑재되는 수중항법장치의 항법 데이터만을 이용하여 제어하기 때문에 추가적인 조류와 같은 해류를 측정하는 외란 측정센서가 필요하지 않다는 장점이 있다.

이하에서는, 본 발명의 제1 실시예와 제2 실시예에 따른 무인잠수정의 경로 제어방법에 대한 시뮬레이션 결과를 설명하도록 한다.

본 발명의 각 실시예의 시뮬레이션에서는 무인잠수정 진행방향 측면으로 4노트(2.05m/s)의 강한 조류가 흐르는 경우에 대해, 해류 보정부(60) 및 이에 대한 처리 중간처리부(70)을 구비하지 않은 제1 실시예와 해류 보정부(60) 및 이에 대한 처리 중간처리부(70)을 구비한 제2 실시예 각각의 위치오차를 확인하는 방식으로 진행하였다.

또한, 시뮬레이션의 설정으로, 6자유도 무인잠수정 모델을 기반으로 하여 무인잠수정의 전진 속도는 6노트(3.08m/s)로 유지하도록 하였고, 시뮬레이션을 구현하기 위한 툴은 MATLAB 의 Simulink 프로그램을 이용하였으며, 이에 대한 시뮬레이션 설계 블록도는 도 8에 도시되어 있다.

도 9는 해류 보정부(60) 및 이에 대한 처리 중간처리부(70)을 구비하지 않은 제1 실시예의 시뮬레이션 결과를 도시한다.

도 9 (a) 는 무인잠수정의 3축 자세 데이터를 오일러 각을 이용하여 도시한다. 도 9 (a) 에 도시된 바와 같이, 롤 및 피치 방향 자세(phi[deg], theta [deg])는 안정되게 유지되지만, 방향각(psi[deg])은 조류의 영향으로 명령 방향각과 실제 선수 방향각이 항상 차이가 있음을 확인할 수 있다.

도 9 (b)은 무인잠수정의 경로를 나타낸다. 도 9 (b) 에 도시된 바와 같이 조류에 의해 무인잠수정이 측면으로 20m 이상 밀리는 것을 확인할 수 있다.

도 9 (c)는 각 순간에 무인잠수정의 속도 방향각

(t) 와 관련된 속도 방향 및 선수 방향각

과 관련된 선수 방향을 나타낸다. 제1 실시예에서는 시선각 지령부(40)의 처리 특성상 선수 방향이 처음에는 다음 경로점을 향하고 있다가 조류에 의해 측면으로 밀려도 그 방향이 크게 바뀌지 않아서, 결과적으로 무인잠수정이 측면으로 밀리게 됨을 볼 수 있으며, 경로점에 근접하여 급격히 선수 방향을 바꾸어 경로점에 근접해 감을 알 수 있다. 즉, 본 발명의 제1 실시예에서는 비교적 간단한 구성으로, 조류가 있어도 한 경로점에서 다음 경로점까지 갈 수는 있지만 그 중간에 경로를 크게 이탈할 수 있다는 단점이 있음을 시뮬레이션으로 확인하였다.

도 10는 해류 보정부(60) 및 이에 대한 처리 중간처리부(70)을 구비하는 본 발명의 제2 실시예의 시뮬레이션 결과를 도시한다.

도 10의 (a)는 무인잠수정의 3축 자세 데이터를 오일러 각을 이용하여 도시한다. 도 10 의 (a) 에서 도시된 바와 같이 3축 자세는 안정적으로 유지되며, 특히 방향각(psi[deg])이 명령된 각도로 수렴하여 유지되는 것을 확인할 수 있다.

도 10의 (b)는 무인잠수정의 경로를 나타내며, 측면 4노트 조류가 있는 상황에서도 지속적으로 경로를 5m 오차 내에서 유지함을 확인할 수 있다.

도 10의 (c)는 무인잠수정의 선수 방향과 속도 방향을 나타내는데, 선수 방향이 조류가 오는 방향으로 틀어진 상태에서 균형을 맞춤으로써, 경로를 유지함을 확인할 수 있다. 이와 같이 본 발명의 제2 실시예에서는 측면 방향의 강한 조류가 있더라도, 전구간에서 지속적으로 경로를 5m 오차 내에서 유지할 수 있다.

그림 11 은 해류 보정부(60) 및 이에 대한 처리 중간처리부(70)를 구비하지 않은 제1 실시예와 해류 보정부(60) 및 이에 대한 처리 중간처리부(70)를 구비하는 제2 실시예를 동시에 비교한 것으로서, 해류 보정부(60) 및 이에 대한 처리 중간처리부(70)를 구비하는 경우, 경로유지의 정확도가 4배 이상 향상됨을 확인할 수 있다.

이와 같은 본 발명의 무인잠수정 경로 제어방법은 수중항법의 데이터를 이용하여 무인잠수정 방향각을 실시간으로 조정함으로써, 조류를 극복하고 경로를 유지할 수 있다.

또한, 본 발명의 무인잠수정 경로 제어방법은 조류와 같은 해류를 측정하기 위해 별도의 센서를 부착하지 않고, 일반적으로 무인잠수정에 탑재되는 수중항법 모듈의 데이터 만을 사용함으로써, 소형의 무인잠수정에 적용이 가능하고, 추가적인 하드웨어 비용이 들지 않는 장점이 있다.

1: 제어부
10: 센서부
20: 수중 복합항법부
30: 기준속도 생성기
40: 시선각 지령부
50: 경로 정보부
100: 추력모터
200: 선수각 구동부
300: 선미부

Claims

무인잠수정의 속도 방향각
을 산출하는 속도 방향각 산출 단계;
무인잠수정의 경로 방향각
(t) 를 산출하는 경로 방향각 산출 단계;
상기 속도 방향각
과 경로 방향각
(t) 을 이용하여 경로방향 오차 방향각
(t) 을 산출하는 경로방향 오차 방향각 산출 단계;
상기 경로방향 오차 방향각
(t) 에 제어 이득을 적용하여 보정 명령 방향각
을 산출하는 보정 명령 방향각 산출 단계; 및
상기 보정 명령 방향각
을 이용하여 방향각 명령
을 생성하는 방향각 명령 생성 단계를 포함하고,
상기 속도 방향각 산출 단계는,
무인잠수정의 심도를 제외한 2차원의 실시간 위치
에서 무인 잠수정의 실제 이동 방향의 속도를 실시간으로 산출하여, 이를 속도 방향각
으로 하는 것을 특징으로 하는 무인잠수정의 경로 제어방법.
삭제
청구항 1에 있어서,
상기 경로 방향각 산출 단계는, 이전 경로점과 현재 경로점 사이의 방향각을 산출하여, 이를 경로 방향각
(t) 으로 하는 것을 특징으로 하는 무인잠수정의 경로 제어방법.
청구항 1에 있어서,
상기 경로방향 오차 방향각 산출 단계는, 상기 속도 방향각
으로부터 상기 경로 방향각
(t) 을 차감하여, 이를 경로방향 오차 방향각
(t) 으로 산출하는 것을 특징으로 하는 무인잠수정의 경로 제어방법.
청구항 1에 있어서,
상기 보정 명령 방향각 산출 단계는 상기 경로방향 오차 방향각
(t) 에 비례 적분 이득값을 적용하여 보정 명령 방향각
을 산출하는 것을 특징으로 하는 무인잠수정의 경로 제어방법.
청구항 1에 있어서,
상기 방향각 명령 생성 단계는,

는 무인잠수정의 현재 위치이고,
는
번째 경로점의 위치이고,
는 현재 시점에서 (
)에서 그 다음 경로지점(
) 으로 향하기 위한 목표 방향각이고,
는 현재 위치/시점 (
)에서의 무인잠수정의 선수 방향각이라 할 때,

,

,
, 및

의 식에 의하여 도출된 차이 방향각
에 상기 보정 명령 방향각
을 더하여, 방향각 명령
을 생성하는 것을 특징으로 하는 무인잠수정의 경로 제어방법.
무인잠수정의 이동 상태를 측정하는 센서부;
상기 센서부로부터 전달받은 정보로부터 무인잠수정의 위치, 방향각, 및 속도를 계산하는 수중 복합항법부;
기설정된 경로 정보가 저장되어 있는 경로 정보부;
무인잠수정의 위치 및 방향이 상기 경로 정보부에 저장된 경로 지점에 어긋나는 정도에 기초하여, 차이 방향각
를 도출하는 시선각 지령부;
무인잠수정의 속도가 상기 경로 정보부에 저장된 경로 방향에 어긋나는 정도에 기초하여, 보정 명령 방향각
을 도출하는 해류 보정부;
상기 차이 방향각
와 경로오차 보정 명령 방향각
에 기초하여, 방향각 명령
을 계산하는 중간 처리부; 및
상기 방향각 명령
에 따라 선수각을 제어하는 선수각 제어기를 포함하고,
상기 시선각 지령부는 무인잠수정의 현재 위치와 다음 경로점이 이루는 방향각으로부터 무인잠수정의 현재 방향각을 차감함으로써, 차이 방향각
을 도출하는 것을 특징으로 하는 무인잠수정의 경로 제어장치.
무인잠수정의 상태를 측정하는 센서부;
상기 센서부로부터 전달받은 정보로부터 무인잠수정의 위치, 방향각, 및 속도를 계산하는 수중 복합항법부;
기설정된 경로 정보가 저장되어 있는 경로 정보부;
상기 수중 복합항법부로부터 계산된 무인잠수정의 위치 및 방향각 정보와 상기 경로 정보부에 저장된 경로 정보에 기초하여, 차이 방향각
을 도출하는 시선각 지령부;
상기 수중 복합항법부로부터 계산된 무인잠수정의 속도 및 상기 경로 정보부에 저장된 경로 정보에 기초하여, 경로오차 보정 명령 방향각
을 도출하는 해류 보정부;
상기 차이 방향각
과 경로오차 보정 명령 방향각
에 기초하여, 방향각 명령
을 계산하는 중간 처리부; 및
상기 방향각 명령
에 따라 선수각을 제어하는 선수각 제어기를 포함하고,
상기 시선각 지령부는 무인잠수정의 현재 위치와 다음 경로점이 이루는 방향각으로부터 무인잠수정의 현재 방향각을 차감함으로써, 차이 방향각
을 도출하는 것을 특징으로 하는 무인잠수정의 경로 제어장치.
삭제
청구항 7 또는 청구항 8에 있어서,
상기 시선각 지령부는,

는 무인잠수정의 현재 위치이고,
는
번째 경로점의 위치이고,
는 현재 시점에서 (
)에서 그 다음 경로지점(
) 으로 향하기 위한 목표 방향각이고,
는 현재 위치/시점 (
)에서의 무인잠수정의 선수 방향각이라 할 때,

,

의 식에 의하여, 차이 방향각
을 도출하는 것을 특징으로 하는 무인잠수정의 경로 제어장치.
청구항 7 또는 청구항 8에 있어서,
상기 해류 보정부는 상기 저장된 경로 정보의 경로 방향과 무인잠수정의 속도 방향과의 차이에 기초하여, 경로오차 보정 명령 방향각
을 도출하는 것을 특징으로 하는 무인잠수정의 경로 제어장치.
청구항 7 또는 청구항 8에 있어서,
상기 해류 보정부는,

는 무인잠수정의 현재 위치를 지칭하고,
는
번째 경로점의 위치를 지칭하고,
는 현재 무인잠수정의 속도(
) 의 속도 방향각을 지칭하고,
(t) 는 이전 경로점과 현재 경로점 사이의 위치차이인 경로 방향각을 지칭하고,
(t) 는 경로 방향각
(t) 과 속도 방향각
의 차이인 경로방향 오차 방향각을 지칭한다고 할 때,

(t)

,

의 식에 의하여 경로오차 보정 명령 방향각
을 도출하는 것을 특징으로 하는 무인잠수정의 경로 제어장치.
청구항 7 또는 청구항 8에 있어서,
상기 중간 처리부는 경로방향 오차 방향각
(t)와 경로오차 보정 명령 방향각
를 더하여 방향각 명령
을 계산하는 것을 특징으로 하는 무인잠수정의 경로 제어장치.