README.md

Cooperative Guidance

Cooperative guidance for multiple kill vehicles

code : MATLAB & python

Simulator with Single Target and Kill Vehicle

본 시뮬레이션 모듈은 단일 표적 격추를 위해 기동하는 한 대의 Kill Vehicle을 모사한다. 실행시간의 단축을 위해 표적은 3DOF 모델이 적용된 질점으로 가정하며, Kill Vehicle에는 비행체의 질점과 자세각 변화를 모사하는 6DOF 모델이 적용되었다.

Simulator Structure

사용자는 시뮬레이션 시작 전 표적 및 Kill Vehicle의 초기 위치, 진행방향, 속력 등을 입력한다. 시뮬레이션 모듈은 입력받은 초기 파라미터를 기반으로 표적 궤적을 적분하며, Kill Vehicle은 생성된 표적 궤적을 기반으로 비례항법유도(Proportional Navigation Guidance)를 실시, 표적을 향해 기동한다. Kill Vehicle과 표적 간의 접근속도가 양수에서 음수로 변하면 표적이 격추되었다고 가정, 시뮬레이션을 종료하고 궤적 및 속도 데이터를 출력한다.

---

Target Trajectory

표적은 Kill Vehicle과 독립적으로 기동하며, 사용자에 의해 기동 시점 및 수직, 수평방향 기동 각속도를 입력받는 3DOF 모델을 따른다. 시뮬레이션 모듈은 입력된 기동 파라미터를 바탕으로 표적의 기동 명령을 생성하며, 생성된 기동 명령은 1차 동역학 시스템을 거쳐 표적의 기동 각도를 산출한다. 계산된 표적의 수직, 수평방향 기동 각도는 각각 Flight Path Angle과 Heading Angle에 해당한다.

이를 기반으로 표적의 Wind 좌표계를 정의한다. Wind 좌표계는 NED 좌표계를 3축-2축 순서로 Heading Angle, Flight Path Angle만큼 회전한 좌표계로, 비행체의 진행 방향, 즉 속도벡터가 Wind 좌표계의 1축과 일치하게 된다. Flight Path Angle과 Heading Angle을 이용해 Wind to NED 또는 NED to Wind 회전변환 행렬을 기술하며, 표적의 속력을 특정한다면 기동하는 표적의 속도 및 궤적을 NED 좌표계에서 나타낼 수 있다.

---

Numerical Integration

Kill Vehicle의 NED 속도, 동체 가속도, 자세각 및 PQR 변화율 적분을 위해 multistep method의 일종인 Adams-Bashforth method를 사용한다.

여기서

우리는 p=1인 경우를 고려하므로 γ₁=0.5 이며, 수치적분식은 아래와 같다.

---

Proportional Navigation Guidance

1) 2차원

비례 항법 유도라고 불리는 Proportional Navigation Guidance의 개념을 설명하기 위한 2차원 그림은 위와 같다. Proportional Navigation Guidance는 위 그림에서 나타나는 line of sight rate(λ dot)이 0이 되도록 Kill Vehicle를 유도하는 방법이며, Kill Vehicle과 target의 이동 경로를 시각화한 삼각형을 collison triangle 이라고 한다.

2차원에서의 Kill Vehicle의 가속도는 다음과 같이 나타낼 수 있다.

위의 수식에서 알 수 있듯이, Kill Vehicle의 가속도 a_m은 Kill Vehicle의 순간 속도 벡터에 수직이며, 이때 N은 무차원의 비례상수, λ dot은 line of sight rate, V는 closing velocity이다.

2) 3차원

3차원에서의 Proportional Navigation Guidance의 개념을 표현한 그림은 위와 같다. 3차원에서의 Kill Vehicle 가속도의 기본형은 다음과 같이 나타낼 수 있다.

위 수식에서 N는 무차원의 비례상수이고, V는 Kill Vehicle대한 target 속도이다. Ω vector는 line of sight의 rotation vector이며, 다음과 같이 나타낼 수 있다.

3차원에서 Kill Vehicle의 가속도는 Kill Vehicle의 속도 벡터와 R vector(range from Kill Vehicle to target)에 대해 나타낼 수 있으며, 다음과 같이 나타낼 수 있다.

• R vector에 수직인 가속도

• Kill Vehicle 속도 벡터에 수직인 가속도

---

Kill Vehicle Kinematics and Dynamics

---

Divert Attitude Control System

본 연구에서 고려하는 Kill Vehicle은 RIM-161 Standard Missile 3의 Kill Vehicle과 유사한 형태를 가지고 있다고 가정한다. 동체 전면부에는 표적 포착을 위한 탐색기가 위치하며, 질량중심 주변에 4개의 추력기로 구성된 Divert Control System이, 동체 후면부에 6개의 추력기로 구성된 Attitude Control System이 부착된 형태이다.

Kill Vehicle에 부착된 각 추력기들을 위와 같이 나타내었다. 여기서 D₁, D₂, D₃, D₄ 와 A₁, A₂, A₃, A₄, A₅, A₆ 는 각 DCS 및 ACS 추력기들이 발생시키는 추력을 의미하며, f_y, f_z 는 동체 좌표계의 2축, 3축 방향으로 Kill Vehicle의 질량중심에 작용하는 힘을, 그리고 l, m, n 은 Kill Vehicle의 자세각, 즉 Roll, Pitch, Yaw 를 발생시키기 위한 토크를 나타낸다.

Kill Vehicle에 작용하는 힘과 토크는 다음과 같다. 6개의 ACS 추력기들이 동체 후면부에만 존재하므로, 그 위치는 질량중심에서 크게 벗어난 동시에 질량중심 기준에서 비대칭을 이룬다. 따라서 자세제어를 위해 ACS 추력기를 작동시키는 순간 Kill Vehicle은 토크 l, m, n 뿐만 아니라 힘 f_y, f_z 를 동시에 받을 것이다. f_y, f_z 를 기술하는 식에 ACS 추력기에 의한 A₁, A₂, A₃, A₄, A₅, A₆ 항이 포함된 것은 이 때문으로, 동체 전면부에 f_y, f_z 를 상쇄할 수 있는 별도의 추력기가 질량중심 기준 대칭점에 존재하지 않는 한 불가피한 문제이다. l, m, n 을 기술하는 식은 상대적으로 간단하나, ACS 추력기들에 의한 추력을 토크로 변환하는 과정에서 설계변수 a, b, 즉 질량중심과 ACS 추력기들 사이의 거리 정보가 사용되었다.

상기한 Linear Equation 은 다음과 같은 형태로 정리할 수 있다.

여기서

x 는 각 추력기가 발생시키는 추력, b 는 Kill Vehicle에 작용하는 힘과 토크, A 는 상기한 두 물리량 사이의 관계를 나타내는 행렬이다. 비례항법유도 및 자세제어기에서 연산된 b 를 추종하기 위해 DACS 추력기를 어떻게 작동시켜야 하는지, 즉 매 순간 x 의 값을 어떻게 계산할지가 우리의 관심사이다.

---

Attempt 1 - Least Norm Problem

우선 위와 같은 형태의 least norm problem 을 고려하자. 이러한 문제는 아래와 같은 해를 가짐이 알려져 있다.

여기서

행렬 A 가 full rank이고, underdetermined한 형태이므로 A 의 Moore-Penrose pseudoinverse matrix는 위와 같다. Pseudoinverse matrix를 이용한 해는 DACS 추력기를 최소한으로 사용하도록 작동하지만, 물리적으로 구현이 불가능하다. 해당 해는 실수 전체의 범위를 가지는 반면, 현실의 추력기들은 한쪽 방향으로만 추력을 발생시킬 수 있으므로 추력값의 범위가 음이 아닌 값으로 제한되기 때문이다.

---

Attempt 2 - Projected Gradient Descent

기존 least norm problem 의 cost function을 다음과 같은 least squared error 형태로 수정하는 한편, inequality constraint를 추가하였다. 이 문제의 해결을 위해 gradient descent 알고리즘을 고려하자. Gradient descent 는 cost function의 local minimum을 찾는 알고리즘으로, 매 step을 반복하며 cost function의 gradient가 0으로 수렴하는 벡터 x 를 찾는 것이 목적이다.

여기서 h 는 learning rate, f(x) 는 cost function이며, x 에 대한 f(x) 의 gradient는 x 의 변화에 따른 함수 f(x) 의 최대 변화율을 나타낸다.

먼저 inequality constraint가 고려되지 않은 경우를 가정하자. 벡터 x 는 f(x) 의 local minimum을 지나치기 전까지 learning rate를 120%씩 증가시키며 하강한다. Local minimum을 지나치면 learning rate를 50%씩 감소시키며 local minimum에 접근한다. Iteration을 충분히 반복했거나 cost function의 gradient가 충분히 0에 가까워졌다고 판단되면 알고리즘을 종료한다.

시각화를 위해 x 를 2차원 벡터로 가정한 뒤 임의의 cost function에 대해 gradient descent를 적용하였다. 몇 번의 iteration 후 성공적으로 x 가 local minimum으로 수렴하는 것을 확인할 수 있다.

이제 inequality constraint를 고려할 차례이다. 위 그림의 붉은 영역은 벡터 x 가 존재하면 안 되는 영역, 즉 constraint를 나타낸다. 우리의 feasible set C 는 그림 우측 상단의 nonnegative region으로 제한되므로, 매 step에서 구한 x 가 C 를 벗어나면 x 를 C 에 대해 projection해 constraint를 충족시킬 것이다.

동일한 예시에 projected gradient descent를 적용해 보았다. Inequality constraint를 만족하는 x 가 적절한 최솟값으로 수렴해 나가는 것을 확인할 수 있다. 마찬가지로, 추력기가 발생시킬 수 있는 최대 추력이 제한된 경우에도 본 알고리즘을 적용해 적절한 최적해를 구할 수 있다.

---

Attempt 3 - Projected Gradient Descent with Regularizer

Least squared error의 형태를 가진 기존 cost function을 확장해 다음과 같은 cost function을 새롭게 정의하였다. 첫 번째 regularizer는 추력 자체의 최소화를, 두 번째 regularizer는 이전 timestep에서 계산된 추력과 현재 계산된 추력의 차이를 최소화하도록, 즉 추력을 시간에 대해 연속적으로 발생시킬 것을 의미하며, 비례상수 λ 와 ν 는 각 regularizer들의 가중치에 해당한다.

새롭게 정의된 cost function의 gradient는 다음과 같다. 동일한 방법으로 projected gradient descent 알고리즘을 적용해 적절한 추력벡터 x 를 구할 수 있지만, 시뮬레이션 모듈에 이를 적용할 경우 실행시간이 상당히 길어지는 문제가 발생했다. 프로그램을 최적화함으로서 실행시간을 어느 정도 단축시킬 수 있지만, 여기서는 추력값 문제 자체를 단순화시키는 방안을 모색해 보았다.

---

Attempt 4 - Paired Thrusters

음수 추력을 고려하기 위해 10개의 DACS 추력기들을 두 개씩 짝지어 새롭게 정의하자. 서로 대칭인 추력기들을 묶으면 기존 [D₁, D₃] 를 D′₁ 로, [D₂, D₄] 를 D′₂ 로, [A₁, A₄] 를 A′₁ 로, [A₂, A₆] 를 A′₂ 로, [A₃, A₅] 를 A′₃ 라고 생각할 수 있다. 새롭게 정의된 5개의 추력기들이 동체 좌표계와 동일한 방향으로 추력을 발생시킨다고 가정하자. 예를 들어 D′₁ 추력기가 양의 추력을 발생시키면 기존 D₁ 추력기가 작동해 Kill Vehicle은 Yaw 축 방향으로 힘을 받고, 음의 추력을 발생시키면 D₃ 추력기가 작동하여 Yaw 축의 반대 방향으로 힘을 받을 것이다.

이전과 마찬가지로 새롭게 정의한 추력기들을 이용해 Kill Vehicle에 작용하는 힘과 토크를 나타내었다. 이 Linear Equation을 간단하게 정리하면 다음과 같다.

여기서

매우 편리하게도 추력벡터 x′ 는 힘과 토크 벡터 b 와 같은 크기를 가지며, A′ 또한 nonsingular matrix이다. 따라서 x′ 는 아래와 같은 해를 가짐을 쉽게 유추할 수 있다.

이렇게 구한 추력벡터 x′ 는 기존 방법으로 구한 해와 매우 유사한 결과를 보여주며, 그 실행시간 역시 현저히 짧다. 하지만 2개의 추력기들을 하나로 묶어 고려하는 특성 상, 모든 추력기들을 개별적으로 제어하는 것이 불가능하다는 한계 역시 가지고 있다.

---

Extra Attempt - ADMM Solver

기존 Projected Gradient Descent 대신 ADMM solver를 적용하자. Cost function f(x) 는 아래와 같이 정리할 수 있다.

여기서 I 는 10x10 단위행렬, 0 는 10x1 영행렬이다. Augmented Lagrangian paramter ρ 를 이용해 다시 정리하면

여기서 C 는 feasible set, Π_C 는 projection operator이다. x_k+1 을 정리하면

이다. 적절한 iteration 뒤 계산된 x_k+1 가 constraint를 만족시키는 추력값이다.

위 그래프는 임의의 시점에 대해 ρ 를 변화시키며 f(x) 의 수렴성을 검증한 결과이다. ρ 값을 작게 설정한 경우, 약 10번 가량 알고리듬을 반복해 최적 DACS 추력값 x 를 얻을 수 있음을 확인할 수 있다.

---

detect target

Kill Vehicle에는 seeker가 달려있다. 이 target을 detect 하는 것을 그림으로 나타내면 아래와 같다.

위 그림을 보기 편하게 2차원으로 나타내면 다음과 같이 나타낼 수 있다.

위의 α 는 Kill Vehicle seeker의 탐색 범위를 나타내는 각이다. 또 β 는 Body1 axis를 NED좌표계로 나타낸 축과 LOS를 NED좌표계로 나타낸 축 사이의 각이다. 이때 α 가 β 보다 크면 Kill Vehicle seeker가 Target을 탐지하는 것이고, α 가 β 보다 작으면 Kill Vehicle seeker가 Target을 탐지하지 못하는 것이다.
Body1 axis를 NED좌표계로 나타낸 축과 LOS를 NED좌표계로 나타낸 축은 아래와 같다.

Body 1 axis to NED

Los to NED

마지막으로 여러 좌표계에 대한 rotation 행렬은 다음과 같이 나타낼 수 있다.

1. NED to Body

2. Body to NED

3. NED to Wind

4. Wind to NED

5. Wind to Body

6. Body to Wind

---