라이다맨 시원의 연구개발 라이프

[논문 리뷰] 강화학습 기반예인선 제어방법에 의한선박 자율 접안

lidarmansiwon — Fri, 21 Nov 2025 18:08:54 +0900

논문 요약: 강화학습 기반 예인선 제어방법에 의한 선박 자율 접안

본 글은 "강화학습 기반 예인선 제어방법에 의한 선박 자율 접안" (홍승조, 김진환 저자) 논문을 읽고 개인적으로 공부한 내용을 정리하였다.

1. 서론 (Introduction)

본 연구는 저속에서 기동성이 제한되는 대형 선박의 접안 작업을 지원하기 위해 예인선 자율 운항 기술을 제안한다.

연구 배경: 기존의 예인선 접안 작업은 도선사와 예인선 운용 인력의 직관에 의존하는 수동적인 방식으로 이루어지며, 이는 많은 비용과 인력을 필요로 한다. 예인선의 자율 운항은 이러한 제한점을 보완할 수 있는 중요한 과제이다.
기존 연구의 한계: 기존 연구들은 예인선의 힘을 단순히 선박에 부착된 추진기(Thruster) 형태로 모델링하여, 예인선이 선박 주변을 이동하며 밀거나 당기는 실제 운용 방식을 제대로 반영하지 못했다.
본 연구의 제안: 실제 운용 방식을 고려하여 예인선의 위치 이동까지 포함한 선박 접안 문제를 강화학습(PPO 알고리즘)을 통해 해결하고자 한다.

2. 시스템 모델링 (System Modeling)

선박과 예인선의 동역학 및 상호작용을 하이브리드 시스템으로 정의하고, 항구 내에서 지배적인 외란인 바람을 모델링하였다.

2.1 선박 모델링

3자유도(Surge, Sway, Yaw) 비선형 모델을 사용하며 운동방정식은 식 (1)과 같다.
$$M\dot{v}+Dv=\tau, \quad \dot{\eta}=R(\psi)v$$
여기서 $M$은 관성행렬, $D$는 제동행렬, $\tau$는 제어 입력(예인선에 의한 힘)을 의미한다.

2.2 예인선 모델링 (Tugboat Modeling)

본 연구에서는 2대의 예인선을 운용하며, 각 예인선은 선박의 지정된 위치(작용점)에 계류하여 힘을 가한다고 가정한다. 예인선의 개별적인 제어 입력이 선박 전체에 미치는 영향은 선박의 본체 고정 좌표계(Body-fixed frame)를 기준으로 한 기하학적 관계를 통해 계산된다.

예인선 제어 변수:
각 예인선 $i$ ($i=1, 2$)에 대하여 다음과 같은 변수를 정의한다.
- $f_i$: 예인선이 작용하는 힘의 크기 (Magnitude)
- $\alpha_i$: 선박의 선수 방향($x$축)을 기준으로 한 힘의 작용 방향 (Direction angle)
- $(l_{x_i}, l_{y_i})$: 선박의 무게중심($O_b$)으로부터 $i$번째 예인선 작용점까지의 거리
힘과 모멘트의 변환:
두 예인선이 생성하는 힘은 선박의 무게중심에 작용하는 전체 제어력 벡터 $\tau = [\tau_u, \tau_v, \tau_r]^T$ (Surge force, Sway force, Yaw moment)로 변환된다. 이는 식 (3)과 같이 작용점의 위치와 힘의 방향에 따른 기하학적 관계 행렬을 통해 도출된다.이 식을 통해 예인선의 힘($f$)과 방향($\alpha$)이 선박의 종방향/횡방향 운동 및 회전 모멘트에 기여하는 성분으로 분해 및 합산되어 선박의 동적 모델에 입력된다.

$$
\tau = \begin{bmatrix}
\cos(\alpha_1) & \sin(\alpha_1) & -l_{y_1}\cos(\alpha_1) + l_{x_1}\sin(\alpha_1) ; \
\cos(\alpha_2) & \sin(\alpha_2) & -l_{y_2}\cos(\alpha_2) + l_{x_2}\sin(\alpha_2)
\end{bmatrix}^T
\begin{bmatrix} f_1 ; \ f_2 \end{bmatrix}
$$

2.3 하이브리드 시스템 모델링

예인선의 제어(연속 시간)와 위치 이동(이산 시간)이 결합된 하이브리드 시스템으로 정의한다.
운용 모드: 선박의 선수/선미에서 당기는 '예인모드($q_1$)'와 한쪽 현측에서 밀고 당기는 '접안모드($q_2$)'로 구분한다.
모드 전환 시 예인선은 위치를 변경하며, 이 시간 동안 제어 입력 $\tau$는 0이 된다.
예인선 제어(연속시간) + 예인선 위치 전환(이산 이벤트)이 결합된 하이브리드 시스템으로 정의.
두 가지 모드:
- q₁: 예인모드(towing) – 예인선이 선수·선미 방향에서 당김.
- q₂: 접안모드(berthing) – 한쪽 현측에 계류하여 밀고 당김.
모드 전환 시:
- 예인선이 선박에서 떨어져 새 위치로 이동하는 전이 구간 존재 (Fig. 2).
- 이 시간 T 동안 제어입력 u(t)=0u(t) = 0u(t)=0 로 설정.
G 함수: 이전 q와 현재 q를 비교해 모드 유지/전환 결정.
R 함수: 새 모드 시작 시 제어입력 초기화.

2.4 외란 모델링

항만 내에서는 바람이 주요 외란이라 보고 바람만 고려.
바람에 의한 힘·모멘트는 선체 전·측면 투영면적과 풍력계수, 상대풍 속도/방향으로 표현한다.
바람의 힘은 풍속, 상대 풍향, 선박의 투영 면적 및 풍력 계수를 기반으로 계산된다. 이때, 바람 속도와 방향은 평균값을 중심으로 한 가우시안 분포에서 샘플링된다.

3. 강화학습 알고리즘 (Reinforcement Learning Algorithm)

연속적인 행동 공간 처리에 적합하고 안정적인 성능을 보이는 PPO(Proximal Policy Optimization) 알고리즘을 적용하였다.

3.1 PPO 알고리즘

행동공간이 연속인 문제에 적합한 PPO(Proximal Policy Optimization) 채택.
신뢰 영역 기반의 정책 경사 알고리즘으로, 정책 업데이트 시 범위를 제한하여 학습의 안정성을 보장한다.
기존 정책과 새 정책의 확률비 rt(θ)r_t(\theta)rt(θ) 를 이용해
- clip된 손실함수 L(θ) 로 학습 안정성 확보.
이득값 A^t\hat{A}_tA^t 를 통해 좋은 행동은 강화, 나쁜 행동은 약화.

3.2 학습 네트워크

정책 네트워크 + 가치 네트워크를 공유하는 구조(Fig. 3)이다.
가치(Value)와 정책(Policy) 네트워크를 공유하며, 2개의 은닉층(64 노드, Tanh 활성함수)을 사용한다.
입력(State): 선박의 상태($x, y, \psi, u, v, r$), 목표와의 오차($e_d, e_\psi, e_v$), 그리고 이전 단계의 행동값(Action)을 사용한다.
출력(Action): 예인선 힘의 변화량($\Delta f$), 방향 변화량($\Delta \alpha$), 그리고 모드 변경을 결정하는 변수 $k$를 포함한 총 5개의 값을 출력한다.
- $-1 \le k < 0$ 이면 예인모드($q_1$), $0 \le k \le 1$ 이면 접안모드($q_2$)를 선택한다.

3.3 보상 함수

목표: 목표 위치·방위·속력을 만족할수록 보상이 커지도록 설계되었다.
보상 R:
- 거리오차 ed, 방위오차 eψ, 속도오차 ev를 기반으로 음의 가중합 형태.
- 속도오차는 크기가 작으므로 계수 c를 곱해 비중 보정.
- 정규화 인자 α로 전체 보상을 0~1 사이로 조정.
- 선박이 부두에 충돌 시 벌점 p = –1 부여.
매 타임스텝마다 보상을 계산해 PPO의 손실함수에 사용.
선박이 부두에 충돌할 경우 벌점($p=-1$)을 부여한다.
$$R=\frac{-||e_{d}||-||e_{\varphi}||-c||e_{\nu}||+\alpha}{\alpha}+p$$

4. 시뮬레이션 결과 (Simulation Results)

Python 기반의 OpenAI Gym 환경을 구축하여 검증을 수행하였다.

4.1 예비 시뮬레이션

6,000톤급 선박 모델을 사용하여 장애물이 없는 환경에서 학습을 진행한 결과, 예인선 2대에 의해 성공적으로 자율 접안함을 확인하였다.

4.2 본 시뮬레이션 (인천 내항 시나리오)

실제 인천 내항 부두 입항 시나리오 구성.
- 좁은 갑문을 통과해 목표 부두에 접안하는 상황.
각 모드(예인/접안)별 예인선 힘과 방향각, 변화량에 대해 제약조건(Table 1) 설정.
학습 과정:
- 초기에는 갑문/부두 충돌로 에피소드가 조기에 종료.
- 학습이 진행되며 선박이 갑문을 통과하고, 예인모드–접안모드 간 모드 전환이 여러 번 발생(Fig. 8 상단).
- 최종 단계에서는:
  - 예인모드로 갑문 통과 후 단 한 번의 모드 전환으로 접안모드에 진입,
  - 목표 위치에 도달하는 안정된 정책으로 수렴(Fig. 8 하단, 논문 확인).
외란 실험:
- 풍속 11–13 m/s, 풍향 205–215°의 강한 바람을 인가.
- Fig. 9:
  - 외란 없는 경우(노란 궤적)와 있는 경우(빨간 궤적) 모두 목표 부두에 접안 성공.
  - 녹색 구간에서 예인모드→접안모드 전환이 일어남.
    → 강한 바람에도 강화학습 기반 제어가 실용적인 자율 접안 성능을 보임을 확인.

5. 결론 (Conclusion)

본 연구는 PPO 알고리즘을 이용하여 예인선의 위치 이동(모드 전환)까지 고려한 선박 접안 문제를 해결하였다.
예인선의 운용을 '예인모드'와 '접안모드'로 정의하고, 강화학습을 통해 예인선의 이동과 운용 판단을 자율화한 국내 최초의 연구 사례이다.

[제어] Sliding Mode Control

lidarmansiwon — Thu, 20 Nov 2025 22:08:23 +0900

슬라이딩 모드 제어(Sliding Mode Control, SMC) 요약

이 문서는 Steve Brunton 교수의 "Sliding Mode Control" 강의 영상을 기반으로 작성되었다. SMC는 비선형 제어 시스템 분야에서 가장 강인(Robust)한 제어 기법 중 하나로 평가받는다. 영상은 MATLAB Korea의 슬라이딩 모드 제어란? 이다.

위상 평면부터 SMC 이해를 위한 시각적, 수식적 설명 그리고 예제 설명이 매우 잘되어있는 영상이다.

1. SMC의 개요 및 핵심 특성

1.1. 정의

슬라이딩 모드 제어(SMC)는 비선형(Nonlinear) 동적 시스템을 제어하기 위한 기법이다. 시스템의 상태(State)가 특정한 '슬라이딩 표면(Sliding Surface)'에 도달한 후, 그 표면을 따라 미끄러지듯(Slide) 목표 지점으로 수렴하도록 제어 입력을 설계하는 방식이다.

1.2. 장점과 단점

장점 (Pros):
- 극단적인 강인성 (Robustness): 모델링의 불확실성(Uncertainty)이나 외부 외란(Disturbance)이 존재하더라도 시스템을 원하는 상태로 강제할 수 있다.
- 차수 축소 (Order Reduction): $n$차 시스템을 슬라이딩 모드에서는 1차 시스템처럼 단순화하여 다룰 수 있다.
단점 (Cons):
- 채터링 (Chattering): 제어 입력이 고주파수로 매우 빠르게 스위칭(On/Off)하는 현상이 발생한다. 이는 모터나 밸브 같은 물리적 액츄에이터를 마모시키거나 손상시킬 수 있다. (후술할 '경계층' 기법으로 해결 가능)

2. 동작 원리: 위상 평면과 슬라이딩 표면

2.1. 위상 평면 (Phase Plane) 해석

2차 시스템(예: 질량-스프링-댐퍼)의 거동을 이해하기 위해 위치($x$)와 속도($\dot{x}$)를 축으로 하는 위상 평면을 사용한다.

자유 반응: 제어가 없을 때, 댐핑(Damping)이 없으면 원형 궤적을 그리고, 댐핑이 있으면 나선형을 그리며 원점(0,0)으로 수렴한다.
SMC의 목표: 이 위상 평면상에 슬라이딩 표면($s=0$)이라 불리는 직선(또는 초평면)을 긋고, 시스템의 상태가 이 선을 벗어나지 않도록 만든다.

2.2. 제어의 두 단계 (Two Phases of SMC)

SMC는 시스템의 거동을 명확하게 두 단계로 구분한다.

도달 단계 (Reaching Mode):
- 시스템 상태가 초기 위치에서 슬라이딩 표면($s=0$)에 도달할 때까지의 구간이다.
- 이 단계에서는 시스템이 무조건 표면을 향해 가도록 강력한 제어 입력을 가한다.
슬라이딩 단계 (Sliding Mode):
- 일단 표면에 도달하면, 상태 궤적은 표면을 따라 원점(평형점)으로 미끄러져 내려간다.
- 이때 시스템의 동역학은 원래의 복잡한 $n$차 미분방정식이 아닌, 설계자가 정의한 슬라이딩 표면의 식(주로 1차 미분방정식)에 의해 지배된다.

3. 수학적 설계 및 유도 과정

SMC 제어기 설계는 시스템을 안정화시키는 제어 입력 $u$를 찾는 과정이다.

3.1. 시스템 모델 및 슬라이딩 변수 정의

일반적인 2차 시스템을 가정한다 (예: $\ddot{x} = f(x, \dot{x}) + g(x, \dot{x})u$).
먼저 슬라이딩 변수(Sliding Variable) $s$를 정의한다.
$$s = \lambda e + \dot{e} \quad (\text{또는 } s = ce + \dot{e})$$
여기서 $e$는 오차($x_{des} - x$)이며, 목표가 원점 안정화라면 $x$ 자체를 사용한다. 영상에서는 $s = C^T \mathbf{x}$ 형태를 사용한다.

만약 $s=0$이 유지된다면, $\dot{x} = -Cx$가 되어 시스템은 자연스럽게 $x=0$으로 수렴하는 1차 시스템처럼 동작한다.

3.2. 도달 조건 (Reachability Condition)

제어기는 상태 궤적이 표면 $s=0$으로 수렴하도록 보장해야 한다. 이를 위해 리야푸노프(Lyapunov) 안정성 판별법을 사용한다.
리야푸노프 후보 함수를 $V = \frac{1}{2}s^2$라 할 때, 시스템이 안정하려면 $\dot{V} < 0$이어야 한다.
$$\dot{V} = s \cdot \dot{s} < 0$$
즉, $s$가 양수면 $\dot{s}$는 음수여야 하고, $s$가 음수면 $\dot{s}$는 양수여야 한다.

3.3. 도달 법칙 (Reaching Law)

위의 도달 조건을 만족시키기 위해 $\dot{s}$의 거동을 강제로 정의한다. 가장 일반적인 방법은 정속도 도달 법칙(Constant Rate Reaching Law)이다.
$$\dot{s} = -\eta \cdot \text{sign}(s)$$

$\eta$ (eta): 설계자가 설정하는 양의 이득(Gain) 값. 이 값이 클수록 표면으로 더 빨리 도달하며 외란에 더 강인해진다.
$\text{sign}(s)$: 부호 함수 ($s>0$이면 +1, $s<0$이면 -1).

3.4. 제어 입력 $u$의 유도

이제 정의된 도달 법칙과 실제 시스템의 동역학을 결합하여 $u$를 구한다.
$$\dot{s} = \frac{\partial s}{\partial \mathbf{x}} \dot{\mathbf{x}} = \mathbf{C}^T (f(\mathbf{x}) + g(\mathbf{x})u)$$
이 식을 위의 도달 법칙($-\eta \cdot \text{sgn}(s)$)과 같다고 놓고 $u$에 대해 푼다.
$$u = g(\mathbf{x})^{-1} \left[ -\mathbf{C}^T f(\mathbf{x}) - \eta \cdot \text{sgn}(s) \right]$$
이 식은 보통 두 부분으로 해석된다:

등가 제어 ($u_{eq}$): 시스템의 현재 동역학을 상쇄하여 상태를 표면 위에 머물게 하는 항 ($-\mathbf{C}^T f(\mathbf{x})$ 부분).
스위칭 제어 ($u_{sw}$): 오차나 외란을 억제하고 표면으로 상태를 밀어 넣는 불연속 항 ($-\eta \cdot \text{sgn}(s)$ 부분).

4. 채터링(Chattering) 문제와 해결책

4.1. 채터링의 원인

이상적인 SMC는 무한히 빠른 스위칭을 가정한다. 하지만 실제 디지털 제어 환경에서는 샘플링 시간의 지연 등으로 인해, 상태가 $s=0$을 정확히 따라가지 못하고 표면을 중심으로 위아래로 빠르게 진동한다. 이것이 채터링이다.

주된 원인은 불연속 함수인 $\text{sgn}(s)$의 사용이다.

4.2. 해결책: 경계층(Boundary Layer) 도입

불연속적인 스위칭을 완화하기 위해 $s=0$ 주변에 얇은 경계층($\Phi$)을 설정한다.

$\text{sgn}(s)$ 함수 대신 연속 함수인 포화 함수(Saturation Function, $\text{sat}$)를 사용한다.
$$\text{sat}\left(\frac{s}{\Phi}\right) = \begin{cases} \text{sgn}(s) & \text{if } |s| > \Phi \ s/\Phi & \text{if } |s| \le \Phi \end{cases}$$
효과:
- 경계층 밖($|s| > \Phi$): 기존처럼 강하게 표면으로 유도한다.
- 경계층 안($|s| \le \Phi$): 이득이 부드럽게 변하여 고주파 진동(채터링)을 억제한다. 마치 높은 이득의 비례 제어(P-Control)처럼 동작한다.
- 트레이드오프: 채터링은 줄어들지만, $s=0$에 완벽하게 수렴하지 않고 경계층 내부에 머무르는 약간의 정상 상태 오차가 발생할 수 있다.

5. 예제 시뮬레이션 결과 (MATLAB/Simulink)

영상에서는 질량-스프링-댐퍼 시스템에 SMC를 적용하여 비교 실험을 수행한다.

제어 없음 (Uncontrolled): 시스템이 댐핑에 의해 천천히 멈춘다. (수렴 시간 > 10초)
기본 SMC ($\text{sgn}$ 함수 사용):
- 시스템이 약 2초 만에 매우 빠르게 목표 지점에 도달한다.
- 하지만 제어 입력 그래프를 보면, 액츄에이터가 감당하기 힘들 정도로 심각한 고주파 진동(채터링)이 관찰된다.
경계층 SMC ($\text{sat}$ 함수 사용):
- 수렴 속도와 성능은 기본 SMC와 거의 동일하게 빠르다.
- 결정적인 차이: 제어 입력이 매끄럽다(Smooth). 채터링이 제거되어 실제 하드웨어에 적용 가능한 형태가 된다.

6. 결론 요약

SMC는 시스템을 원하는 동역학(슬라이딩 표면)으로 강제로 구속하는 기법이다.
불확실성과 외란에 매우 강인하다는 것이 최대 장점이다.
채터링이라는 물리적 한계가 있으나, 경계층(Saturation 함수) 기법을 통해 성능 저하를 최소화하며 해결할 수 있다.
따라서 로봇, 항공, 전력 전자 등 정밀하고 강인한 제어가 필요한 분야에 매우 적합하다.

Proximal Policy Optimization (PPO) - OpenAI

lidarmansiwon — Wed, 3 Sep 2025 01:12:32 +0900

논문 링크: https://arxiv.org/abs/1707.06347

Proximal Policy Optimization Algorithms

We propose a new family of policy gradient methods for reinforcement learning, which alternate between sampling data through interaction with the environment, and optimizing a "surrogate" objective function using stochastic gradient ascent. Whereas standar

arxiv.org

이번 논문 리뷰는 전체적으로 논문을 바탕으로 공부 한 내용을 정리하였다. 필요한 부분에서 논문의 Figure와 수식을 보여주며 설명을 덧붙였다.

PPO는 강화학습(Reinforcement Learning)에서 정책 경사(Policy Gradient) 계열의 알고리즘이다. 기존 정책 경사 방법들의 단점인 불안정한 학습과 복잡한 구현 문제를 해결하는데 중점을 둔 알고리즘이다. PPO의 가장 큰 목표는 안정적이고 신뢰할 수 있는 정책 업데이트를 통해 학습 성능을 높이는 것이다.

▷ 정책 경사(Policy Gradient)란?

정책 경사(Policy Gradient): "잘한 행동은 더 많이 하도록, 못한 행동은 덜 하도록 직접적으로 가르치는 방법"

강화 학습의 목표는 agent가 최상의 보상을 받도록 행동을 가르치는 것이다. 이때 agent의 행동 지침을 정책(Policy)라고 부른다. 정책 경사는 이 '정책'을 직접적으로 좋게 만드는 방법이다.

골프 선수 비유

여기 골프를 배우는 로봇이 있다고 상상해 보자.

정책(Policy): 로봇의 스윙 자세, 클럽 선택 등 샷을 하기 위한 모든 행동 방식.
행동(Action): 실제로 골프채를 휘두르는 것.
보상(Reward): 공이 홀컵에 가까이 갈수록 높은 점수(보상)를 받음.

이 로봇을 어떻게 훈련시킬까?

일단 쳐본다: 현재의 정책(스윙 자세)으로 공을 쳐본다.
결과를 확인한다: 공이 홀컵에 아주 가깝게 붙음. "이번 샷은 아주 좋음" (높은 보상 획득)
정책을 수정한다: 방금 그 "아주 좋았던" 샷을 만들었던 스윙 자세를 더 자주 하도록 정책을 직접 수정. 즉, 그 스윙 자세와 관련된 파라미터들을 보상이 증가하는 방향으로 업데이트.
반복: 반대로 공이 엉뚱한 곳으로 날아갔다면(낮은 보상), 그 스윙 자세는 덜 하도록 정책을 수정.

이처럼 '결과(보상)가 좋았던 행동의 확률을 높이고, 결과가 나빴던 행동의 확률은 낮추는' 방식으로 정책을 직접 업데이트하는 것이 바로 정책 경사의 핵심 아이디어이다. 여기서 '경사(Gradient)'라는 단어는 '어느 방향으로 정책을 수정해야 보상이 가장 많이 늘어날까?'를 수학적으로 계산하는 것을 의미한다.

정책 경사의 핵심 수식

정책 경사의 목표는 보상의 기댓값 $J(\theta)$를 최대로 만드는 정책의 파라미터 $\theta$를 찾는 것이다. 이를 위해 $J(\theta)$를 $\theta$로 미분한 경사(gradient)를 계산하고, 이 경사를 따라 파라미터를 업데이트한다. 그 경사는 다음과 같이 표현된다.

$$\nabla_\theta J(\theta) = \hat{\mathbb{E}}_t \left[ \nabla_\theta \log \pi_\theta(a_t|s_t) \hat{A}_t \right]$$

$\nabla_\theta J(\theta)$ : 우리가 계산하고 싶은 '경사'. 즉, '정책을 어느 방향으로 업데이트해야 보상이 커지는가'에 대한 정보.
$\pi_\theta(a_t|s_t)$ : 현재 정책. 상태 $s_t$에서 행동 $a_t$를 할 확률을 의미한다.
$\nabla_\theta \log \pi_\theta(a_t|s_t)$ : '행동의 방향'이다. 이 부분이 특정 행동($a_t$)을 할 확률을 높이거나 낮추는 역할을 수행한다.
$\hat{A}_t$ : '행동의 좋고 나쁨'을 알려주는 척도(어드밴티지 함수). 이 값이 크면(보상이 좋으면) 해당 행동의 확률을 더 많이 높이고, 작거나 음수이면(보상이 나쁘면) 확률을 낮추게 된다.

결국 이 수식은 "좋았던 행동()은 더 자주 하도록, 나빴던 행동()은 덜 하도록 정책()을 업데이트하라"는 직관적인 아이디어를 수학적으로 표현한 것이다.

PPO는 이 기본적인 정책 경사 아이디어에서 한 걸음 더 나아가, 정책이 너무 급격하게 변해서 학습이 불안정해지는 것을 막기 위해 '클리핑'이라는 안전장치를 추가한, 더 발전된 알고리즘이라고 이해할 수 있다.

핵심 아이디어는 간단하다. 이전 정책(old policy)과 새로운 정책(new policy)의 차이가 너무 커지지 않도록 제한하는 것이다. 정책이 급격하게 변하면 학습이 불안정해지고 좋은 방향으로 수렴하기 어렵기 때문이다. 이를 위해 PPO는 '클리핑(Clipping)'이라는 독특한 방법을 사용한다.

PPO의 성공 비결은 클리핑(Clipping)에 있다고 해도 무관하다.

클리핑을 아주 간단하게 비유하자면 "자동차의 속도 제한 장치"와 같다.

클리핑(Clipping)이란?

정책(Policy)이 한 번에 너무 많이 변하지 않도록 강제로 "상한선"과 "하한선"을 설정하는 기술이다.

자동차의 속도 제한 장치가 시속 110km를 넘지 못하게 막는 것처럼, 클리핑은 정책 업데이트의 변화량을 특정 범위 $ [1 - \epsilon, 1 + \epsilon] $ 안에 가둬버린다.

정책 업데이트: AI가 경험을 통해 자신의 행동 지침(정책)을 수정하는 과정
변화량: 이전 정책 대비 새로운 정책이 얼마나 달라졌는지의 정도 ($r_t(\theta)$로 측정)
클리핑 범위 $ [1 - \epsilon, 1 + \epsilon] $: "정책을 바꾸더라도 이 범위를 벗어날 수는 없어!"라고 정해놓은 안전 구간. 보통 $\epsilon$은 0.1이나 0.2 같은 작은 값을 쓴다.

그래서 필자는 이게 왜 핵심인가 궁금했다.

클리핑이 없는 이전의 정책 경사 알고리즘들은 다음과 같은 큰 문제가 있다.

문제점: "대박" 한 번에 흥분해서 망하는 경우

AI가 어떤 행동을 우연히 했는데, 운 좋게 엄청나게 큰 보상(대박)을 받았다고 상상해 보자.

클리핑이 없다면? AI는 "이 행동이 최고다!"라고 과도하게 믿어버린다. 그래서 그 행동을 할 확률을 아주 아주 크게 높여버리게 된다. (정책이 급격하게 변함) 하지만 그건 그냥 운일 수 도 있다. 그 결과, 다른 좋은 행동들을 탐색할 기회를 잃고 오히려 전체 성능이 나락으로 떨어지는 학습 붕괴(catastrophic collapse)가 발생할 수 있다. 마치 주식 초보가 우연히 한 종목으로 대박을 치고 전 재산을 몰빵하는 것과 같다.
클리핑이 있다면? AI가 엄청난 보상을 받아도, 클리핑이 "워워, 진정해. 좋은 건 알겠는데 일단 조금만 바꿔보자."라며 제동을 건다. 정책의 변화량을 최대 $1+\epsilon$까지만 허용하기 때문에, AI는 흥분하지 않고 침착하게 학습을 이어나갈 수 있다. 반대로 아주 나쁜 결과를 얻었을 때도, ""까지만 변화를 제한하여 "이번 한 번의 실수로 이 행동을 완전히 포기하지는 말자"고 막아준다.

Schulman et al., (2017)에서는 다음과 같이 설명한다.

1. 클리핑의 목적: 보수적인 정책 업데이트 (Conservative Policy Iteration)

아래의 수식(논문 eq(6))은 TRPO와 같은 이전 알고리즘에서 사용하던 목적 함수 $L^{CPI}(\theta)$를 보여준다.

$$L^{CPI}(\theta) = \hat{\mathbb{E}}_t \left[ \frac{\pi_\theta(a_t|s_t)}{\pi_{\theta_{old}}(a_t|s_t)} \hat{A}_t \right] = \hat{\mathbb{E}}_t [r_t(\theta) \hat{A}_t]$$

여기서 문제는, 이 수식을 제한 없이 최대화하면 $r_t(\theta)$ (정책 변화량)가 너무 커져서 정책이 과도하게 업데이트되고 학습이 불안정해진다는 것이다. 논문에서는 이를 "excessively large policy update"라고 표현한다.

이 문제를 해결하기 위해 PPO는 수식 (7), 즉 클리핑된 목적 함수 $L^{CLIP}$ 을 제안한다.

$$L^{CLIP}(\theta) = \hat{\mathbb{E}}_t \left[ \min(r_t(\theta) \hat{A}_t, \text{clip}(r_t(\theta), 1 - \epsilon, 1 + \epsilon) \hat{A}_t) \right]$$

핵심은 $\min$ 함수와 $\text{clip}$ 함수의 조합이다. 이 조합이 바로 정책 업데이트에 '브레이크'를 거는 역할을 한다.

2. 클리핑의 작동 방식 (Figure 1)

Figure 1은 클리핑이 어떻게 작동하는지를 가장 직관적으로 보여준다.

Schulman et al., (2017)

왼쪽 그래프 (어드밴티지가 긍정적일 때, $A > 0$):
- 상황: 현재 한 행동이 평균보다 좋았으므로, 이 행동을 할 확률을 높여야 한다 ($r_t(\theta)$을 키워야 함).
- 클리핑: 하지만 $r_t(\theta)$가 $1+\epsilon$을 초과하는 순간, 목적 함수 $L^{CLIP}$의 값은 더 이상 증가하지 않고 수평선처럼 평평해진다. 즉, 보상이 아무리 좋아도 정책 업데이트의 이득(incentive)에 상한선(cap)을 둬서 과도한 업데이트를 막는다.
오른쪽 그래프 (어드밴티지가 부정적일 때, $A < 0$):
- 상황: 현재 한 행동이 평균보다 나빴으므로, 이 행동을 할 확률을 낮춰야 한다 ($r_t(\theta)$을 줄여야 함).
- 클리핑: $r_t(\theta)$가 $1-\epsilon$보다 작아져도 목적 함수는 더 이상 나빠지지 않는다. 이는 한 번의 큰 실수로 인해 특정 행동의 확률을 지나치게 많이 줄여버리는 비관적인 업데이트를 막는 하한선(floor) 역할을 한다.

결론적으로 $L^{CLIP}$은 기존의 목적 함수 $L^{CPI}$에서 이득을 취하되, 정책이 너무 멀리 벗어나려고 하면 그 이득을 잘라내어(clip) 버리는 **비관적인 경계(pessimistic bound) 또는 하한선(lower bound)**의 역할을 함.

3. 클리핑의 효과 (Figure 2)

Figure 2는 실제 학습 과정에서 클리핑의 효과를 보여준다.

Schulman et al., (2017)

가로축: 정책 업데이트 강도 (0은 업데이트 안 함, 1은 PPO가 한 번 업데이트한 상태)
세로축: 목적 함수의 값

그래프를 보면, 주황색 선($L^{CPI}$)은 업데이트 강도가 커짐에 따라 제한 없이 계속 올라간다. 하지만 초록색 선($L^{CLIP}$)은 어느 지점(KL 발산이 약 0.02가 되는 지점)에서 최대치를 찍고 더 이상 크게 증가하지 않는다.

이는 $L^{CLIP}$이 정책이 너무 많이 변하는 것을 막는 페널티로 작용하고 있음을 보여준다. PPO는 이 $L^{CLIP}$을 최적화하기 때문에, $L^{CPI}$를 직접 최적화할 때보다 훨씬 안정적으로 정책을 업데이트할 수 있는 것이다. 논문의 설명처럼 $L^{CLIP}$은 $L^{CPI}$의 **하한선(lower bound)**으로 작용하며, 너무 큰 정책 업데이트에 대한 페널티를 부과한다.

PPO의 핵심: 클리핑된 목적 함수 (Clipped Surrogate Objective Function)

PPO의 핵심은 바로 이 '클리핑된 목적 함수'에 있다. 수식은 복잡해 보일 수 있지만, 아이디어는 직관적이다.

$$L^{CLIP}(\theta) = \hat{\mathbb{E}}_t \left[ \min \left( r_t(\theta) \hat{A}_t, \text{clip}(r_t(\theta), 1 - \epsilon, 1 + \epsilon) \hat{A}_t \right) \right]$$

여기서 각 항목의 의미는 다음과 같다.

$r_t(\theta)$: 확률 비율(Probability Ratio)로, 새로운 정책과 이전 정책이 특정 행동을 선택할 확률의 비율

$$r_t(\theta) = \frac{\pi_\theta(a_t|s_t)}{\pi_{\theta_{old}}(a_t|s_t)}$$

$r_t(\theta) > 1$ 이면, 새로운 정책이 해당 행동을 할 확률이 더 높다는 의미.
$r_t(\theta) < 1$ 이면, 새로운 정책이 해당 행동을 할 확률이 더 낮다는 의미.

$\hat{A}_t$: 어드밴티지 함수(Advantage Function)로, 특정 상태에서 특정 행동을 하는 것이 평균적으로 얼마나 더 좋았는지를 나타내는 값임.

$\hat{A}_t > 0$ 이면, 평균보다 좋은 행동.
$\hat{A}_t < 0$ 이면, 평균보다 나쁜 행동.

$\epsilon$: 클리핑 하이퍼 파라미터. 0.1 또는 0.2 같은 작은 값을 사용한다고 함. 정책이 얼마나 변할 수 있는지를 제한하는 역할을 함.

이제 이 목적 함수가 어떻게 작동하는지에 대해서는 아래 두가지 경우로 나눌 수 있다.

1. 어드밴티지가 양수일 때 ($\hat{A}_t > 0$)

즉, 과거에 했던 행동이 평균보다 좋은 결과를 냈을 때.
따라서 우리는 이 행동을 할 확률($r_t(\theta)$)을 높이고 싶음.
하지만 PPO는 $r_t(\theta)$가 $1 + \epsilon$ 이상으로 너무 커지는 것을 막는다. 정책이 한 번에 너무 낙관적으로 변해서 학습이 불안정해지는 것을 방지한다.
목적 함수는 $r_t(\theta) \hat{A}_t$ 와 $(1 + \epsilon) \hat{A}_t$ 중 더 작은 값을 선택하게 된다.

2. 어드밴티지가 음수일 때 ($\hat{A}_t < 0$)

즉, 과거에 했던 행동이 평균보다 나쁜 결과를 냈을 때.
따라서 우리는 이 행동을 할 확률($r_t(\theta)$)을 낮추고 싶음.
PPO는 $r_t(\theta)$가 $1 - \epsilon$ 이하로 너무 작아지는 것을 막는다. 좋은 행동을 찾을 기회를 완전히 잃어버리는 비관적인 업데이트를 막기 위함.
목적 함수는 $r_t(\theta) \hat{A}_t$ 와 $(1 - \epsilon) \hat{A}_t$ 중 더 큰 값을 선택하게 된다. (어드밴티지가 음수이므로, 전체 값은 더 큰 쪽이 덜 감소하게 됨.)

결론적으로, PPO의 클리핑 메커니즘은 정책 업데이트에 안전장치를 거는 것과 같다. 어드밴티지가 긍정적이든 부정적이든, 정책이 한 번에 너무 크게 변하는 것을 막아 안정적인 학습을 가능하게 한다.

PPO 알고리즘의 전체 구조

PPO는 보통 액터-크리틱(Actor-Critic) 구조를 기반으로 한다.

액터(Actor): 정책($\pi_\theta$)을 나타내며, 현재 상태를 보고 어떤 행동을 할지 결정함. PPO의 클리핑된 목적 함수를 통해 업데이트된다.
크리틱(Critic): 가치 함수($V_\phi$)를 나타내며, 현재 상태의 가치(얼마나 좋은 상태인지)를 평가한다. 이는 어드밴티지($\hat{A}_t$)를 계산하는 데 사용된다.

학습 과정은 다음과 같이 진행.

현재 정책($\pi_{\theta_{old}}$)을 사용하여 여러 에피소드를 실행하며 데이터(상태, 행동, 보상 등)를 수집.
수집된 데이터를 사용하여 각 타임스텝의 **어드밴티지($\hat{A}_t$)**를 계산.
클리핑된 목적 함수를 최대화하는 방향으로 정책(액터)의 파라미터 $\theta$를 여러 번 업데이트.
동시에 가치 함수(크리틱)의 예측 오차를 최소화하도록 파라미터 $\phi$를 업데이트.
1~4 과정을 반복.

PPO의 장점

구현의 단순함: TRPO와 달리 복잡한 2차 미분 계산이 필요 없어 코드로 구현하기가 훨씬 쉽다.
안정적인 성능: 다양한 환경에서 견고하고 안정적인 성능을 보여준다. 하이퍼파라미터 튜닝에 덜 민감한 편.
데이터 효율성: 수집한 데이터를 여러 번의 업데이트(multiple epochs)에 재사용하여 학습 효율을 높인다.

이러한 장점들 덕분에 PPO는 현재 강화학습 분야에서 가장 널리 사용되는 알고리즘 중 하나가 되었다. 복잡한 문제를 해결하기 위한 베이스라인(baseline) 알고리즘으로 자주 채택된다.

Reference

Schulman, J., Wolski, F., Dhariwal, P., Radford, A., & Klimov, O. (2017). *Proximal Policy Optimization Algorithms*. Retrieved from https://arxiv.org/abs/1707.06347

[논문 리뷰] USV Formation Path Planning Based on Behavior Trees and Fast Marching Method

lidarmansiwon — Thu, 5 Jun 2025 20:15:29 +0900

1. 논문 자료

논문 링크: https://ieeexplore.ieee.org/document/10170942

USV Formation Path Planning Based on Behavior Trees and Fast Marching Method

In the deployment of multiple Unmanned Surface Vehicles (USVs) for collaborative operation, path planning is a crucial component. This paper addresses the path planning problem for USV formations operating in complex marine environments and proposes a mult

ieeexplore.ieee.org

※ 위 논문 리뷰는 온전히 개인이 공부하면서 정리한 글입니다.

2. 논문 알아보기

2.1 논문 주제 및 목적

이 논문은 다중 무인수상선박(USV) 편대 운항 시 협력적 작전을 위한 경로 계획(Path Planning)을 다룬다. 특히 좁고 복잡한 해상 환경에서 USV 편대의 운항 경로를 효율적으로 계획하기 위해 행동트리(Behavior Tree, BT)와 Fast Marching Method (FMM) 를 결합한 편대 경로 계획 방법론을 제안한다.

2.2 제안한 방법의 특징

적응형 편대 라이브러리 (Adaptive Formation Library): FMM을 기반으로 속도 퍼텐셜 필드(velocity potential field)를 활용하여 형성.
행동트리 기반 장애물 회피 제어 전략: 행동트리를 사용하여 상황에 따른 효율적인 제어 전략을 도출함.
복잡한 해상 환경에서도 높은 성능을 발휘하며, 특히 좁은 해협에서 장애물 회피 성능이 우수함.

2.3 연구 배경 및 중요성

USV 편대는 군사적, 민간적 용도에서 항구 순찰, 해안 감시, 환경 모니터링, 무인 운송 등 다양한 분야에서 활용된다. 편대 경로 계획은 미션 수행 시작점에서 목표 지점까지 안전하고 최적의 경로를 결정하여 효율적이며 충돌 위험을 최소화하는 데 중요한 역할을 한다. 기존 경로 계획 알고리즘으로는 Dijkstra, A*, PSO, ACO, 유전자 알고리즘(GA), APF, FMS 등이 연구되었으며, 특히 FMS는 국부 최소 문제를 해결하고 연속성 및 부드러운 경로 생성 능력에서 강점을 가지고 있다. FMM은 계산 속도가 빠르고 실시간 경로 생성이 가능해, 복잡한 해상 환경에서 USV의 실시간 운항에 적합하다.

2.4 행동트리의 장점

모듈화(Modularity), 확장성(Scalability), 결함 내성(Fault tolerance) 측면에서 탁월한 성능을 제공한다.
협력 제어, 충돌 회피, 유연성과 적응성을 보장하는 전략으로 유용하게 활용된다.

3. 논문 기술 요소

3.1 이론적 모델 (Theoretical Model)

A. Fast Marching Method (FMM)

Fast Marching Method(FMM)는 Eikonal 방정식을 해결하기 위해 제안된 방법으로, 다음과 같은 형태의 방정식을 해결한다.

$|\nabla T(x,y)| \cdot V(x,y) = 1$

여기서 $T(x,y)$ 는 좌표 $(x,y)$ 에서의 파면 도달 시간(wave arrival time)을 의미하며, $V(x,y)$ 는 파면 전파 속도를 나타낸다.

특정 지점 $(x_0,y_0)$에서의 경계 조건은 $T(x_0,y_0) = 0$으로 주어진다.

위 식을 upwind 차분법을 사용하여 풀면 다음과 같은 형태가 된다.

$\max\left(\frac{T - T_1}{\Delta x}, 0\right)^2 + \max\left(\frac{T - T_2}{\Delta y}, 0\right)^2 = \frac{1}{V^2(x,y)}$

여기서 $T_1$과 $T_2$는 현재 격자점의 주변 격자점 중 최소 도달 시간을 나타내며, 각각 다음과 같이 표현된다.

$T_1 = \min\left(T_{(x-\Delta x,y)},\, T_{(x+\Delta x,y)}\right)$

$T_2 = \min\left(T_{(x,y-\Delta y)},\, T_{(x,y+\Delta y)}\right)$

이 방정식을 $T$ 에 관해 명시적으로 풀게 된다. 이렇게 구한 도달시간 행렬 $T$를 기반으로, 경사 하강법(Gradient Descent Method)을 이용하여 시작점에서 목표점까지의 최적 경로를 결정한다.

B. Fast Marching Square Method (FMS)

일반적인 FMM은 생성된 경로가 장애물과 지나치게 가까워 안전성 문제를 야기할 수 있다. 따라서 안전성을 높이기 위해 Fast Marching Square Method(FMS)가 제안되었으며, FMS는 FMM을 두 번 연속 적용한다.

FMS 알고리즘 과정:

1. 초기 이진 맵(binary map)을 생성한다. (흰색: 이동 가능 공간, 검정색: 장애물)

2. 첫 번째 FMM을 적용하여 장애물로부터 퍼지는 파면을 통해 속도 퍼텐셜 맵(velocity potential) $W(x)$를 생성한다.

3. 두 번째 FMM을 목표점에서부터 적용하여, 도달시간 퍼텐셜 필드(arrival time potential) $D(x)$를 생성한다.

4. 생성된 $D(x)$를 바탕으로 경사 하강법(아래 4.1 참고)을 적용하여 시작점에서 목표점까지 최적 경로를 얻는다.

이러한 과정은 장애물에 너무 가까운 경로가 생성되는 문제를 방지하며, 결과적으로 무인수상선(USV)의 운항 안전성을 높인다.

출처 [1]

그림 (Fig. 1) 설명:

(a) 초기 이진 맵: 장애물과 이동 가능 공간을 구분한 상태를 나타냄.

(b) 속도 퍼텐셜 필드 &W(x)&: 장애물로부터 거리에 따라 파면이 전파된 상태를 나타냄.

(d) 최적 경로 생성 결과: (c)의 도달시간 퍼텐셜 필드를 이용하여 경사 하강법을 통해 결정된 최종 경로.

C. 행동 트리 (Behavior Trees)

행동 트리(Behavior Trees)의 핵심 개념은 복잡한 작업을 보다 작은 하위 작업(subtask)들로 분해하여 이를 조합하고 조율하여 전체 작업을 수행하는 것이다.

행동 트리는 실행 노드(execution nodes)와 제어 노드(control nodes)로 구성된다.

실행 노드(Execution nodes): 리프(leaf) 노드*이며, 조건 노드(condition node)와 행동 노드(action node)로 구분된다.
제어 노드(Control nodes): 내부 노드로 논리와 작업 전환을 담당하며, 주로 다음과 같은 유형을 포함한다.
- 순차(sequence) 노드: 자식 노드를 순서대로 실행하며, 모든 자식 노드가 성공해야 성공을 반환하고, 하나라도 실패하면 실패를 반환하고 실행을 중단한다.
- 선택(selector) 노드: 자식 노드를 차례대로 실행하며, 하나의 자식 노드라도 성공하면 성공을 반환하고 실행을 중단하며, 모든 자식 노드가 실패해야 실패를 반환한다.
- 병렬(parallel) 노드: 여러 자식 노드를 동시에 실행할 수 있다.

* 리프 노드(Leaf Node) 란 트리 구조에서 더 이상 하위 노드를 가지지 않는 마지막(끝) 노드를 의미

행동 트리의 기본 노드 유형은 다음과 같다.

순차 노드 (Sequence node): 자식 노드를 순차적으로 실행함.
선택 노드 (Selector node): 자식 노드 중 하나라도 성공하면 성공을 반환함.
조건 노드 (Condition node): 특정 조건의 만족 여부를 판단하여 성공 혹은 실패를 반환함.
행동 노드 (Action node): 행동 트리의 리프 노드로 특정 작업을 수행하고 성공 또는 실패를 반환함.

3.2 USV 편대 장애물 회피 경로 계획

A. Fast Marching Method 기반 적응형 편대 라이브러리 (Adaptive Formation Library)

환경 제약 조건 아래에서 무인수상선박(USV) 편대가 장애물을 회피하기 위해 형상을 유연하게 조정할 수 있도록, 편대 라이브러리를 구축하고, FMM(Fast Marching Method)의 속도 퍼텐셜 필드를 활용하여 적응적인 형상 변화를 달성한다.

편대의 관계를 나타내는 매트릭스(편대의 각도, 거리 등 상대적 형상 정보를 나타낸 행렬)는 다음과 같이 정의된다.

$ E=[R L\varphi]^T $

여기서 $R = [R_1, R_2, R_3, \dots, R_n]$은 USV의 식별 지시자(identity indicators)를 나타낸다.
$L = [l_1, l_2, l_3, \dots, l_n]$은 원하는 편대 간격(desired formation distance)을 나타낸다.
$\varphi= [\varphi_1, \varphi_2, \varphi_3, \dots, \varphi_n]$는 원하는 편대 형상 각도(desired formation angle)를 나타낸다.

즉, 각 행은 리더로부터 특정 USV까지의 편대 번호, 거리($l_i$), 각도($\varphi_i$)로 구성됨.

USV 편대의 주요 형상은 삼각형(triangular), 열(columnar), 선형(linear) 등으로 구분할 수 있으며, 다중 USV 편대 제어를 위한 편대 라이브러리는 다음과 같은 행렬 집합으로 표현된다.

$E=({E i | E j,E c,E t})$

각각의 형상 파라미터 행렬은 다음과 같다.

선형 편대(linear formation) $E_j$:

$E_j = \begin{bmatrix}
1 & 0 & 0 \\
2 & l_i & \frac{\pi}{2} \\
\vdots & \vdots & \vdots \\
i & l_i & \pi(\sqrt{i}+1)
\end{bmatrix}$

열 편대(column formation) $E_c$:

$E_c = \begin{bmatrix}
1 & 0 & 0 \\
2 & l_i & \pi \\
\vdots & \vdots & \vdots \\
i & l_i & \pi
\end{bmatrix}$

삼각형 편대(triangular formation) $E_t$:

$E_t = \begin{bmatrix}
1 & 0 & 0 \\
2 & l_i & \frac{3\pi}{4} \\
\vdots & \vdots & \vdots \\
i & l_i & \frac{\pi}{2}\left(\frac{3}{2}+\sqrt{i}\right)
\end{bmatrix}$

행렬 $E_j, E_c, E_t$에 있는 값 중 편대 간격 $l_i$만 지정해주면, 나머지 각도 값들은 이미 행렬 내에서 고정적으로 정의되어 있기 때문에, 자동으로 그림 2와 같은 기본 편대 형태가 형성되는 구조이다.

행렬 편대 형태 간격($l$) 이외의 각도($\varphi$) 값의 특징

$E_j$	선형 편대(linear formation)	각도가 $\pi/2$ (90°)로 일정하게 설정됨
$E_c$	열 편대(column formation)	각도가 $\pi$ (180°)로 일정하게 설정됨
$E_t$	삼각형 편대(triangular formation)	각도가 약 135°(3π/4) 등으로 설정되어 삼각 형태 유지

출처 [1]

논문에서 소개된 편대 형상 라이브러리의 기본 형상은 Figure 2와 같이 나타낼 수 있으며, 이를 기반으로 환경 변화에 따라 적응형 편대 변경과 장애물 회피가 가능하다고 한다.

형상 행렬 라이브러리 $E$를 만드려면 환경 제약에 따라 다중 USV 형성 변화 및 장애물 회피의 토대를 마련해야한다.

FMM velocity potential field와 결합하여 환경의 적응 형성 변화를 달성 할 수 있다.

행렬 변환에 의해, 리더에 대한 각도 및 거리 행렬은 글로벌 좌표계에서 위치 행렬로 변환되어 다음 USV 궤적 추적의 문제를 해결한다.

추가 설명:

1. 기본적인 개념 정리

논문에서는 편대의 상대적 형상 적보(각도, 거리 등)를 나타낸 행렬을 가지고 있다. 이 상대적인 형상 정보는 일반적으로 편대 리더(leader)를 기준으로 한 상대 좌표계에서 정의된다.

하지만 실제 운항 중인 USV(무인수상선박)들은 현실 세계(global coordinate system)에 위치하고 있다.
따라서 상대 좌표계에서 정의된 편대 형상 정보를 실제 글로벌 좌표계에서의 각 USV 위치로 변환할 필요가 있다.

글로벌 좌표계 변환이 필요한 이유:

초기에는 리더를 기준으로 하는 상대적 편대 형상 정보(거리와 각도)를 가지고 있다. 예를 들어, 두 번째 USV가 리더로부터 거리 $l_i$만큼 떨어져 있고, 특정 각도 $\varphi_i$로 위치한다는 정보가 있다.

하지만 실제 경로 계획 및 제어를 수행하기 위해서는 이 상대적 정보가 아닌, 지도(map)나 GPS와 같은 글로벌 좌표계를 기준으로 한 위치로 변환되어야 한다. 이를 통해 각 USV가 실제로 목표 경로를 따라 안정적으로 움직일 수 있도록 제어 명령을 내릴 수 있다.

글로벌 좌표계에서 위치행렬(F) 생성 과정:

$F = \begin{bmatrix}
R_1 & x_1 & y_1 \\
R_2 & x_2 & y_2 \\
\vdots & \vdots & \vdots \\
R_i & x_i & y_i
\end{bmatrix}$

이 행렬 $F$는 편대에 속한 각 USV의 실제 글로벌 좌표계에서 위치 정보를 담고 있다.
각 행의 첫 번째 요소인 $R_i$는 USV의 ID (번호)를 의미하고, 두 번째와 세 번째 요소 $(x_i, y_i)$는 그 USV의 글로벌 좌표를 나타낸다.

형상 라이브러리 행렬 $F$ 는 다음 절차로 계산된다.

(1) 리더의 글로벌 좌표계 위치 ($x_1, y_1$)를 초기화하여 행렬 $F$의 첫 번째 행에 저장한다.

$F = [R_1, x_1, y_1]$

(2) 이후 팔로워(follower)들의 상대적 위치를 글로벌 좌표로 변환하여 저장한다. $i$번째 추종자(follower)의 전역 좌표계(global coordinate)에서 위치 $[x_i,y_i]$를 다음 단계로 계산한다.

팔로워의 위치를 구하는 과정:

a. 속도 퍼텐셜 필드(velocity potential field) $W_i$ 를 사용하여 편대 간격을 조정한다. (편대 간격을 속도 퍼텐셜 필드 값 $W_i$에 따라 조정):

$l_i^e = l_i^* W_i$

b. 속도 퍼텐셜 필드 값 $W_i$를 기반으로 편대의 각도를 조정한다:

$\varphi_i^c = \varphi_i * (1 - W_i) + \varphi_i^t * W_i$

여기서, $\varphi_i^c$는 조정된 편대 형상 각도이다. 이때 각도는 리더를 기준으로 한 상대 좌표계(local coordinate system) 기준이다.

$\varphi_i$ : 기존 편대 형상의 각도 (원래 목표로 설정된 각도)
$\varphi_i^t$ : 장애물을 만났을 때 적응적으로 조정된 목표 편대의 각도 (장애물 회피 등을 위해 수정된 각도)
$W_i$ : 속도 퍼텐셜 필드(Velocity potential field)의 값으로, 환경 정보를 반영하여 0~1 사이의 값을 가진다.

첨자 $c$ (current의 약자):

$\varphi_i^c$는 현재(Current)의 편대 형상 각도를 나타낸다.
즉, 현재 실제 USV 편대가 유지하고 있는, 혹은 가지고 있는 형상의 각도를 의미
환경 변화(장애물 등)가 없었다면 원래 설정한 각도를 유지하고 있을 테니, 기존의 각도를 기반으로 환경 변화에 따라 현재 편대가 가진 실제 각도를 표현할 때 사용하는 첨자

첨자 $t$ (target의 약자):

$\varphi_i^t$는 목표(Target) 편대 형상 각도를 나타낸다.
장애물을 회피하거나 형상을 변경할 필요가 있을 때, 앞으로 변경하고자 하는 목표 형상 각도
장애물 회피 등 환경 변화가 발생할 때 새로운 목표 형상을 결정하여, 편대가 점차 이 목표 형상 각도로 바뀌어 가는 과정에서 사용되는 첨자

각 팔로워가 리더를 기준으로 한 로컬 좌표계(local coordinate system)에서 어디에 위치할지 결정하는 단계:

c. 리더를 기준으로 한 로컬 좌표계에서 팔로워의 상대 위치 벡터 $P_i$를 계산한다:

$P_i = [l_i^e \cdot \cos(\varphi_i^e),\quad l_i^e \cdot \sin(\varphi_i^e)]^T$

$l_i^e$: 리더와 팔로워 사이의 거리 (편대 간격을 환경에 따라 조정한 값)
$\varphi_i^e$: 리더의 진행 방향(로컬 좌표계의 x축)을 기준으로 팔로워가 위치할 상대적 방향 각도

이 결과로 얻어지는 $P_i$는 팔로워가 리더로부터 x축(리더 진행 방향)으로 얼마나 떨어져 있는지와 y축(리더 진행 방향의 수직 방향)으로 얼마나 떨어져 있는지를 알려주는 벡터이다.

d. 로컬 좌표계에서 얻은 위치 벡터를 전역 좌표계로 변환하여 전역 좌표상의 위치를 얻는다(각 에이전트의 실제 위치를 얻는 과정):

$[x_i, y_i]^T = [x_1, y_1]^T + R(\alpha) \cdot P_i$

$[x_1, y_1]^T$: 리더의 실제 전역 좌표 (GPS 또는 지도 상의 절대 좌표)
$R(\alpha)$: 로컬 좌표계를 글로벌 좌표계로 바꿔주는 회전 변환 행렬
$\alpha$: 리더가 실제 전역 좌표계에서 향하고 있는 방향 각도 (리더가 바라보는 글로벌 좌표계에서의 절대 방향)

여기서, 좌표 회전 변환 행렬(rotation matrix) $R(\alpha)$는 다음과 같다.

$R(\alpha) = \begin{bmatrix}
\cos(\alpha) & -\sin(\alpha)\\[6pt]
\sin(\alpha) & \cos(\alpha)
\end{bmatrix}$

물리적 의미와 예시:

로컬 좌표계에서 팔로워가 $P_i=[10,0]^T$라면 리더의 방향으로부터 정면으로 10m 떨어진 위치에 있다는 뜻.
하지만 실제 전역 좌표계에서 리더가 $(100,200)$ 위치에 있고, 리더의 방향이 90°(전역 좌표계에서 북쪽 방향)이라면, 팔로워의 글로벌 위치는 회전 행렬로 인해 다음과 같이 변환된다:

$[x_i,y_i]^T = [100,200]^T + R(90^\circ)\cdot[10,0]^T = [100,200]^T +
\begin{bmatrix}
0 & -1 \\
1 & 0
\end{bmatrix}\cdot[10,0]^T = [100,200]^T + [0,10]^T = [100,210]^T$

즉, 로컬 좌표에서 리더의 정면으로 10m 떨어져 있던 팔로워는, 리더가 실제로 북쪽을 바라보며 $(100,200)$ 지점에 위치할 때 글로벌 좌표계상으로 $(100,210)$ 위치에 놓이게 된다.

(3) 계산된 추종자의 전역 위치 $[x_i,y_i]$를 전역 좌표계 행렬 $F$에 추가한다.

전체 편대 라이브러리 행렬의 일반적인 형태는 다음과 같다.

$F = \begin{bmatrix}
R_1 & x_1 & y_1 \\
R_2 & x_2 & y_2 \\
\vdots & \vdots & \vdots \\
R_i & x_i & y_i
\end{bmatrix}$

삼각형 형상 라이브러리 행렬의 예시는 다음과 같이 표현된다.

$F = \begin{bmatrix}
R_1 & x_1 & y_1 \\[6pt]
2 & x_1 - lW_2(\cos\alpha - \sin\alpha) & y_1 - lW_2(\cos\alpha + \sin\alpha) \\[6pt]
3 & x_1 + lW_3(\cos\alpha - \sin\alpha) & y_1 - lW_3(\cos\alpha + \sin\alpha)
\end{bmatrix}$

$R_i$ : 각 USV를 식별하는 인덱스 (ID)
$(x_i,y_i)$ : 글로벌 좌표계에서의 i번째 USV의 실제 위치 좌표

B. 행동 트리 기반 편대 형상 변환 제어 전략

출처 [1]

행동트리 기반 형상 변환 전략은 모듈성 및 계층적 구조를 특징으로 한다. 복잡한 장애물 환경에서 편대를 최적화하여 장애물을 회피한다.

Figure 3과 같이 상황에 따라(한쪽 또는 양쪽에 장애물 존재) 적절한 하위 작업(subtask)을 선택하여 편대 형상을 조정하고, 장애물을 회피하여 좁은 환경을 통과할 수 있도록 한다.
적시에 형상 조정을 수행함으로써 장애물을 효과적으로 회피하고 작업의 안전한 수행을 보장한다.

형상 장애물 회피 행동트리 전략의 구체적인 단계:

행동트리의 루트 노드는 장애물 회피 순차 노드(sequence node)이며, 자식 노드를 순차적으로 실행한다. 먼저 환경 인식 노드는 이동 가능한 영역과 원하는 경로(desired route)를 확인하고, 경로가 장애물 사이에 있을 경우 확장 노드(action node)를 실행하여 스케일링 팩터(scaling factor) $\rho$를 계산한다.

그림 3의 경우, 장애물 사이에서 편대 너비가 $D_{unit}$보다 작을 때 형상 조정이 필요하며, 이때 스케일링 팩터는 다음과 같이 정의된다.

$\rho=D_{max}/D$

여기서 $D_{max}$는 통과 가능한 경로의 최대 너비를 나타내고 $D$는 형성의 최대 너비를 나타낸다.

formation transformation selection node는 scaling factor $\rho$를 분석하고 obstacle avoidance formation transformation mode (장애물 회피 형성 변환 모드)를 결정하여 다음 세가지 동작 노드 중 하나를 입력한다.

(a) $\rho \geq 1$인 경우:

편대 변환이 필요하지 않은 형상 유지(zero transformation) 액션 노드가 활성화된다.
편대는 형상을 변경하지 않고 장애물을 통과할 수 있다.

(b) $\rho_m < \rho < 1$인 경우 (여기서 $\rho_m = l_{min}/D$):

동일 형상 축소(isomorphic transformation) 액션 노드가 활성화된다.
이 경우, 원래의 편대 형상을 유지한 채 편대의 크기를 축소하여 장애물을 통과한다.
새로운 형상 매트릭스의 간격(distance parameter)은 다음 식으로 결정된다.

$l_i^d = l_i \times \rho$

(c) $0 \leq \rho \leq \rho_m$인 경우:

다른 형상으로 전환(heteromorphic transformation) 액션 노드가 활성화된다.
이때, 형상 적응 비용 함수(formation adaptation cost function)는 다음 식과 같이 계산된다.

$S_{fd}(F_c,F_t) = \frac{D_{max}}{D}$

편대 라이브러리로부터 가장 적합한 목표 편대(target formation)를 선택한다.
이후 다른 형상으로 전환하는 액션 노드와 경로 재계획(route replanning) 액션 노드를 수행한다.

이러한 방식으로 USV 편대는 상황에 따라 장애물을 효과적으로 회피하며 안전하고 효율적인 경로를 유지할 수 있다.

출처 [1]

※ 위 논문 리뷰는 온전히 개인이 공부하면서 정리한 글입니다.

[1] H. Cao, R. Song, J. Xu, X. Hu, Z. Bao, and L. Bao, "USV formation path planning based on behavior trees and fast marching method," in 2023 5th International Conference on Intelligent Control, Measurement and Signal Processing (ICMSP), Chengdu, China, May 2023, doi: 10.1109/ICMSP58539.2023.10170942.

4. 추가 학습 내용

4.1 경사 하강법

경사 하강법(Gradient Descent)은 최적화 문제에서 자주 사용되는 반복적(iterative) 수치 해법이다. 주로 어떤 함수의 값을 최소화하거나 최대화할 때 쓰인다. 일반적으로 머신러닝에서 비용 함수(Cost function)의 최소값을 찾는 데 널리 활용된다.

기본 원리

경사 하강법은 함수의 그래디언트(Gradient)를 활용한다. 그래디언트는 함수가 가장 급격히 증가하는 방향을 가리키는 벡터로서, 경사 하강법은 이 벡터의 반대 방향으로 조금씩 이동하면서 함수의 최솟값을 찾는다.

함수 $f(x)$를 최소화하는 경우, 경사 하강법의 갱신 규칙은 다음과 같다.

$x_{new} = x_{old} - \eta \nabla f(x_{old})$

$x_{new}$ : 새롭게 갱신된 변수 값
$x_{old}$ : 이전 단계의 변수 값
$\eta$ : 학습률(Learning rate)이라고 하며, 한 번에 이동하는 보폭을 의미
$\nabla f(x_{old})$ : $f(x)$의 미분값(그래디언트)

학습률(Learning rate, $\eta$)의 역할

학습률은 매우 중요한 하이퍼파라미터로, 적절한 값을 설정하는 것이 중요하다.

학습률이 너무 클 때:
최솟값을 건너뛰어 오히려 발산할 위험이 있다.

학습률이 너무 작을 때:
최적점에 도달하는 데 시간이 너무 오래 걸리거나, 국부적 최소점(local minima)에 빠지게 된다.

[ROS2 GPS, IMU 항법 코드] ROS2 기반 GPS + IMU 항법 시스템 구현

lidarmansiwon — Wed, 4 Jun 2025 23:50:17 +0900

개요

이 글에서는 ROS 2 기반 시스템에서 GPS(Global Positioning System)와 IMU(Inertial Measurement Unit) 센서 데이터를 활용하여 선박 또는 로봇의 위치 및 자세 정보를 계산하고 항법 메시지로 변환하는 과정을 다룹니다.

사용 목적

GPS와 IMU 센서를 기반으로 자율운항 선박이나 로봇의 실시간 위치 및 자세 정보를 추정
추정된 정보를 바탕으로 Guidance & Control 알고리즘에 필요한 데이터를 생성

이론 설명

1. GPS 좌표계를 UTM 좌표계로 변환하는 이유

GPS 좌표계의 한계

GPS는 WGS84 기반의 위도(latitude), 경도(longitude) 정보를 제공합니다.
이는 구면 좌표계 기반이므로 거리 계산, 상대 위치 추정에 어려움이 있습니다.

UTM 좌표계의 장점

UTM (Universal Transverse Mercator) 좌표계는 지구를 60개의 존(Zone)으로 나누고, 각 존에서 2차원 직교 좌표계 (X, Y) 로 변환합니다.
이를 통해 미터 단위의 거리, 상대 위치 계산이 용이해집니다.
예: "현재 위치가 기준 위치에서 동쪽으로 24.5m, 북쪽으로 17.2m 떨어져 있음"과 같은 계산이 가능

따라서 GPS로 받은 위도/경도 값을 UTM으로 변환하면, 로컬 좌표계를 기준으로 상대 위치를 쉽게 계산할 수 있습니다.

2. IMU 센서의 역할

IMU는 3축 가속도계와 3축 자이로스코프를 포함한 센서입니다.

주요 데이터

orientation: 자세 (쿼터니언 형태)
angular velocity: 회전 속도 (rad/s)
linear acceleration: 가속도 (optional)

사용 이유

GPS는 위치는 정확하지만 자세나 회전 정보를 제공하지 않습니다.
IMU는 실시간 회전 각도(yaw)와 회전 속도(r)를 추정할 수 있어, GPS를 보완합니다.

항법 시스템에서 IMU는 선박의 방향(ψ), 회전 속도(r), 가속도 등 자세 정보를 제공합니다.

코드 기반 설명

이제 위의 이론을 바탕으로 실제 ROS 2 C++ 코드 구현 내용을 살펴보겠습니다. 아래에 소개할 코드는 GitHub 저장소 macro_gnc에서 확인할 수 있으며, 항법 데이터를 생성하는 핵심 모듈 중 하나인 gps_navigation.cpp 파일에 해당합니다. 이 코드는 ROS 2 노드로 동작하며, GPS 및 IMU 센서로부터 받은 데이터를 바탕으로 선박 혹은 무인이동체의 위치 및 자세 정보를 실시간으로 계산하여 퍼블리시합니다.

GitHub - lidarmansiwon/macro_gnc

Contribute to lidarmansiwon/macro_gnc development by creating an account on GitHub.

github.com

특히 속도 계산 부분에는 navigation/tool/velocity_calculator.hpp 파일이 사용됩니다. 이 모듈은 이동체의 위치 및 자세 변화율로부터 선속(u), 횡속(v), 그리고 요레이크(r)를 계산하며, 저역통과 필터(Low-Pass Filter, LPF)를 적용하여 노이즈가 많은 센서 데이터로부터 안정적인 속도 추정치를 제공합니다. 이 필터 계수는 파라미터로 조정할 수 있어 상황에 맞는 튜닝이 가능합니다.

이제 gps_navigation.cpp 코드의 주요 구성 요소와 각 기능에 대해 자세히 살펴보겠습니다.

초기화 및 파라미터 설정

this->declare_parameter<std::string>("imu_topic", "/imu_topic");
this->declare_parameter<std::string>("gps_topic", "/gps_topic");
this->declare_parameter<std::string>("navigation_topic", "/navigation_data");

사용자 정의 토픽 이름을 파라미터로 설정합니다.
reference_gps_latitude, reference_gps_longitude는 상대 좌표의 기준점입니다.

Parameter 값으로 전달 받은 filter 강도 전달

  double velocity_alpha = this->get_parameter("velocity_alpha").as_double();
  vel_calc_ = VelocityCalculator(velocity_alpha);

IMU 데이터를 활용한 자세 계산

geometry_msgs::msg::Quaternion quat = imu_data_->orientation;
std::array<double, 3> rpy = euler_from_quaternion({quat.x, quat.y, quat.z, quat.w});
double yaw = rpy[2];

쿼터니언 → 롤/피치/요(roll, pitch, yaw) 변환
항법에서 가장 중요한 건 yaw (ψ), 즉 선박의 방향입니다.

Pub, Sub 객체 정의 및 timer 정의

  // 생성자에서 시작 표시.
  RCLCPP_INFO(this->get_logger(), "Run GPS Navigation");

  imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
    imu_topic, rclcpp::SensorDataQoS(), std::bind(&GPSNavigation::imu_callback, this, _1));

  gps_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
    gps_topic, rclcpp::SensorDataQoS(), std::bind(&GPSNavigation::gps_callback, this, _1));

  navigation_publisher_ = this->create_publisher<mk3_msgs::msg::NavigationType>(
    navigation_topic, 10);

  timer_ = this->create_wall_timer(50ms, std::bind(&GPSNavigation::process, this));

생성자에서 노드를 처음 시작할 때, RCLCPP_INFO 로그 기능을 통하여 시작을 알립니다.
전달 받을 센서 GPS, IMU에 대한 Subscribe 코드와 최종 계산된 항법 데이터를 Publish 할 코드를 작성합니다.
timer를 통하여 50ms(20Hz) 간격으로 함수를 실행합니다.

GPSNavigation::process() 함수 상세 설명

1. 센서 데이터 유효성 검사

if (!gps_data_ || !imu_data_)
{
  RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000, "GPS or IMU data not received yet.");
  return;
}

목적: GPS 또는 IMU 데이터가 아직 수신되지 않은 경우 경고 메시지를 출력하고 함수를 종료합니다.
기능: ROS 2에서는 센서 콜백이 비동기로 도착하므로, 해당 데이터가 nullptr인지 확인하여 안정성을 확보합니다.
기능 강조: RCLCPP_WARN_THROTTLE은 3초에 한 번씩만 경고를 출력하여 로그 과부하를 방지합니다.

2. GPS 좌표 → UTM 좌표 변환

double latitude = gps_data_->latitude;
double longitude = gps_data_->longitude;
...
GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_x, utm_y);

목적: 위도(latitude)와 경도(longitude)를 UTM 평면좌표계로 변환합니다.
사용 라이브러리: GeographicLib의 UTMUPS::Forward 함수
입력값 : latitude, longitude
출력값 :
- utm_x, utm_y: 변환된 평면 좌표
- zone: UTM 존 (6도 간격)
- northp: 북반구 여부 (남반구일 경우 UTM 보정 방식이 다름)

RCLCPP_INFO(this->get_logger(), "Converted GPS (%.8f, %.8f) → UTM (%.2f, %.2f), Zone: %d, Northp: %d\n", latitude, longitude, utm_x, utm_y, zone, northp);

디버깅 정보 출력: 실제 GPS 좌표가 어떻게 변환되었는지 로그에 출력하여 추적 가능합니다.

3. 기준점 설정 및 상대 좌표 계산

if (!origin_set_) {
    ...
    GeographicLib::UTMUPS::Forward(reference_gps_latitude_, reference_gps_longitude_, reference_zone, reference_northp, reference_utm_x, reference_utm_y);
    ...
    origin_set_ = true;
}

목적: 기준 GPS 좌표(reference)를 최초 한 번만 UTM으로 변환하여 고정된 원점으로 사용합니다.
기능 설명:
- origin_set_ 플래그는 기준점이 한 번만 설정되도록 합니다.
- 일반적으로 로컬 좌표계를 만들기 위한 원점 설정에 해당합니다.

double relative_x = utm_x - reference_utm_x;
double relative_y = utm_y - reference_utm_y;

목적: UTM 좌표계 상에서 기준점으로부터의 상대적인 위치를 계산합니다.
결과: 로컬 UTM 좌표 (x, y) 생성

4. 자세 정보 추출 (IMU → Roll, Pitch, Yaw)

geometry_msgs::msg::Quaternion quat = imu_data_->orientation;
std::array<double, 3> rpy = euler_from_quaternion({quat.x, quat.y, quat.z, quat.w});
double roll = rpy[0], pitch = rpy[1], yaw = rpy[2];

목적: IMU로부터 쿼터니언(quaternion) 형태의 자세 정보를 받아서 오일러각(Roll, Pitch, Yaw)으로 변환합니다.
사용 함수: euler_from_quaternion() (사용자 정의 함수로 추정)
중요한 값: yaw는 선박의 방향(heading)을 의미하는 각도로, 이후 속도 계산에 활용됩니다.

5. 속도 계산 (velocity_calculator 사용)

auto result = vel_calc_.update(x, y, psi, time_sec);
double LPFVel_x = result.LPFVel_x;
double LPFVel_y = result.LPFVel_y;
double angular_velocity = result.angular_velocity;

목적: 위치 변화와 자세 변화량을 기반으로 선속(u), 횡속(v), 회전 속도(r)를 계산합니다.
사용 클래스: velocity_calculator.hpp에 정의된 VelocityCalculator 클래스
핵심 알고리즘:
- 이전 상태와 현재 상태 간 시간차(dt)를 사용하여 속도를 계산
- 저역통과필터(LPF)를 통해 노이즈 제거 및 평활화 처리
출력 항목:
- LPFVel_x: 선속 (u)
- LPFVel_y: 횡속 (v)
- angular_velocity: 회전 속도 (r)

6. 최종 메시지 생성 및 퍼블리시

NavigationType nav_msg;
nav_msg.x = x;
nav_msg.y = y;
nav_msg.psi = psi * 180.0 / M_PI;
nav_msg.u = LPFVel_x;
nav_msg.v = LPFVel_y;
nav_msg.r = angular_velocity;
nav_msg.w = imu_data_->angular_velocity.z;

navigation_publisher_->publish(nav_msg);

목적: 계산된 위치, 자세, 속도를 NavigationType 메시지 형태로 퍼블리시
메시지 필드 설명:
- x, y: 로컬 UTM 좌표계 기준 상대 위치
- psi: 선박의 방향각 (라디안 → 도 단위 변환)
- u, v, r: 각각 선속, 횡속, 요 회전 속도
- w: IMU에서 측정된 z축 각속도 (원시 데이터)

종합 요약

이 함수는 GPS와 IMU 데이터를 융합하여 이동체의 현재 위치, 자세, 속도 정보를 계산한 후, 이를 다른 노드에서 사용할 수 있도록 퍼블리시하는 핵심적인 항법 처리 로직입니다.

velocity_calculator.hpp 설명

1. 목적

velocity_calculator.hpp는 위치 및 자세 정보의 시간 변화율을 통해 속도를 계산하기 위한 유틸리티 클래스입니다.
ROS 2 시스템에서 GPS와 IMU를 기반으로 수집된 위치(x, y) 및 방향(psi: 요각)을 기반으로 선속(u), 횡속(v), 각속도(r)를 추정합니다.

2. 핵심 원리: 수치 미분(Numerical Differentiation)

dx = x - prev_x_;
dy = y - prev_y_;
dpsi = normalize_angle(psi - prev_psi_);
dt = current_time - prev_time_;

고전적인 1차 수치 미분법(forward difference)을 이용해 속도를 추정합니다.
이를 바탕으로 선속(u), 횡속(v), 회전속도(r)를 계산:
- u = (dx * cos(psi) + dy * sin(psi)) / dt
- v = (-dx * sin(psi) + dy * cos(psi)) / dt
- r = dpsi / dt

이 방식은 구현이 간단하고 실시간으로 사용하기 적합하나, 센서 노이즈에 민감하다는 단점이 존재합니다.

3. 노이즈 보완: 저역통과 필터(Low-Pass Filter)

LPFVel_x = LPF_alpha * vel_x + (1 - LPF_alpha) * LPFVel_x;
LPFVel_y = LPF_alpha * vel_y + (1 - LPF_alpha) * LPFVel_y;

고주파 노이즈(잡음)를 줄이기 위해 저역통과 필터(LPF)를 사용

LPF_alpha는 필터 계수로, 0.0~1.0 사이의 값입니다.

1에 가까울수록 새 데이터 반영률이 높고,
0에 가까울수록 이전 데이터 반영률이 높아져 더 부드럽지만 반응이 느려집니다.

4. 한계점: 미분 기반 속도 추정의 문제점

GPS나 위치 센서는 노이즈가 포함된 데이터를 제공합니다.
수치 미분은 노이즈를 증폭시킵니다.
- 데이터가 순간적으로 튈 경우, 계산된 속도 값도 크게 튈 수 있습니다.
- dt가 작을수록 더 민감해지므로 주의가 필요합니다.
LPF만으로는 동적 환경 변화에 적응하거나 예측이 불가능합니다.

5. 개선 방향: 칼만 필터(Kalman Filter)를 활용한 보정

많은 연구자들은 속도 추정을 더 정밀하게 하기 위해 Kalman Filter (KF) 를 적용합니다.

Kalman Filter 기본 개념:

센서 노이즈를 모델링하고, 현재 상태(위치, 속도 등)를 추정하는 확률 기반 필터
예측(Predict) 단계 + 관측 업데이트(Update) 단계를 반복
시간에 따라 누적되는 오차를 수학적으로 최소화

항법 요소 데이터 출처 설명

위치 (x, y)	GPS (UTM 변환 후)	기준 GPS 대비 상대 위치
자세 (ψ)	IMU	방향 추정 (yaw)
선형 속도 (u, v)	위치 변화량 기반	LPF 적용
각속도 (r, w)	위치 변화량 + IMU	yaw 변화량 기반 또는 자이로 직접 측정

참고 사항

UTM 변환에는 GeographicLib 라이브러리를 사용
IMU 오차와 GPS 신호 손실 등을 고려해 추가 필터 (예: 칼만 필터) 적용 가능
본 코드는 ROS 2 Humble 기준으로 작성됨

전체 코드

코드 저장소: MACRO_GNC

GitHub - lidarmansiwon/macro_gnc

Contribute to lidarmansiwon/macro_gnc development by creating an account on GitHub.

github.com

[gps_navigation.cpp 전체 코드]

#include <iostream>
#include <memory>
// C++ Standard lib 
#include <chrono>
#include <functional>
#include <string>
#include "rclcpp/rclcpp.hpp"

#include <GeographicLib/UTMUPS.hpp>

#include "std_msgs/msg/string.hpp"
#include "mk3_msgs/msg/navigation_type.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"

#include "navigation/gps_navigation.hpp"
#include "navigation/tool/quaternion_utils.hpp"
#include "navigation/tool/velocity_calculator.hpp"


using std::placeholders::_1;
using namespace std::chrono_literals;

GPSNavigation::GPSNavigation(const rclcpp::NodeOptions & node_options)
: Node("gps_navigation", node_options), 
  reference_gps_latitude_(0.0),
  reference_gps_longitude_(0.0),
  reference_utm_x(0.0),
  reference_utm_y(0.0),
  reference_zone(0.0),
  reference_northp(false),
  vel_calc_(0.9)
{ 
  this->declare_parameter<std::string>("imu_topic", "/imu_topic");
  this->declare_parameter<std::string>("gps_topic", "/gps_topic");
  this->declare_parameter<std::string>("navigation_topic", "/navigation_data");

  this->declare_parameter<double>("reference_gps_latitude", 0.0);
  this->declare_parameter<double>("reference_gps_longitude", 0.0);
  this->declare_parameter<double>("velocity_alpha", 0.9);

  std::string imu_topic = this->get_parameter("imu_topic").as_string();
  std::string gps_topic = this->get_parameter("gps_topic").as_string();
  std::string navigation_topic = this->get_parameter("navigation_topic").as_string();

  reference_gps_latitude_  = this->get_parameter("reference_gps_latitude").as_double();
  reference_gps_longitude_ = this->get_parameter("reference_gps_longitude").as_double();
  double velocity_alpha = this->get_parameter("velocity_alpha").as_double();
  vel_calc_ = VelocityCalculator(velocity_alpha);

  // 생성자에서 시작 표시.
  RCLCPP_INFO(this->get_logger(), "Run GPS Navigation");

  imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
    imu_topic, rclcpp::SensorDataQoS(), std::bind(&GPSNavigation::imu_callback, this, _1));

  gps_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
    gps_topic, rclcpp::SensorDataQoS(), std::bind(&GPSNavigation::gps_callback, this, _1));

  navigation_publisher_ = this->create_publisher<mk3_msgs::msg::NavigationType>(
    navigation_topic, 10);

  timer_ = this->create_wall_timer(50ms, std::bind(&GPSNavigation::process, this));
}

GPSNavigation::~GPSNavigation()
{

}

void GPSNavigation::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg){imu_data_ = msg;}
void GPSNavigation::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg){gps_data_ = msg;}

void GPSNavigation::process()
{
  if (!gps_data_ || !imu_data_)
  {
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000, "GPS or IMU data not received yet.");
    return;
  }

  double latitude = gps_data_->latitude;
  double longitude = gps_data_->longitude;

  double utm_x, utm_y;
  int zone;
  bool northp;

  try {
    // 입력: latitude, longitude, 출력: zone(UTM 존), northp(북반구, 남반구 여부), utm_x, utm_y
    GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_x, utm_y);
    RCLCPP_INFO(this->get_logger(), "Converted GPS (%.8f, %.8f) → UTM (%.2f, %.2f), Zone: %d, Northp: %d\n", latitude, longitude, utm_x, utm_y, zone, northp);
  } catch (const std::exception &e) {
    RCLCPP_ERROR(this->get_logger(), "UTM conversion failed: %s", e.what());
    return;
  }

  if (!origin_set_) {

    try {
        GeographicLib::UTMUPS::Forward(reference_gps_latitude_, reference_gps_longitude_, reference_zone, reference_northp, reference_utm_x, reference_utm_y);
        RCLCPP_INFO(this->get_logger(), "Converted Reference GPS (%.8f, %.8f) → Reference UTM (%.2f, %.2f), Zone: %d, Northp: %d\n", 
        reference_gps_latitude_, reference_gps_longitude_, reference_utm_x, reference_utm_y, reference_zone, reference_northp);
    } catch (const std::exception &e) {
        RCLCPP_ERROR(this->get_logger(), "Reference UTM conversion failed: %s", e.what());
        return;
    }
    origin_set_ = true;
  }

  double relative_x = utm_x - reference_utm_x;
  double relative_y = utm_y - reference_utm_y;


  RCLCPP_INFO(this->get_logger(), "Relative position: x=%.2f, y=%.2f\n\n", relative_x, relative_y);

  geometry_msgs::msg::Quaternion quat;

  quat = imu_data_->orientation;

  std::array<double, 3> rpy = euler_from_quaternion(
    {quat.x, quat.y, quat.z, quat.w}
  );

  double roll = rpy[0];
  double pitch = rpy[1];
  double yaw = rpy[2];

  double x = relative_x;
  double y = relative_y;
  double psi = yaw;

  rclcpp::Time current_time = this->now();
  double time_sec = current_time.seconds();

  auto result = vel_calc_.update(x, y, psi, time_sec);
  double LPFVel_x = result.LPFVel_x;
  double LPFVel_y = result.LPFVel_y;
  double angular_velocity = result.angular_velocity;

  NavigationType nav_msg;
  nav_msg.x = x;
  nav_msg.y = y;
  nav_msg.psi = psi * 180.0 / M_PI;
  nav_msg.u = LPFVel_x;
  nav_msg.v = LPFVel_y;
  nav_msg.r = angular_velocity;
  nav_msg.w = imu_data_->angular_velocity.z;

  navigation_publisher_->publish(nav_msg);
}

[velocity_calculator.hpp 전체 코드]

#ifndef VELOCITY_CALCULATOR_HPP
#define VELOCITY_CALCULATOR_HPP

#include <cmath>
#include <chrono>

class VelocityCalculator
{
public:
  VelocityCalculator(double alpha = 0.9)
    : alpha_(alpha), prev_initialized_(false), LPFVel_x_(0.0), LPFVel_y_(0.0) {}

  struct VelocityResult {
    double vx;
    double vy;
    double angular_velocity;
    double LPFVel_x;
    double LPFVel_y;
  };

  VelocityResult update(double x, double y, double psi, double current_time)
  {
    if (!prev_initialized_) {
      prev_x_ = x;
      prev_y_ = y;
      prev_psi_ = psi;
      prev_time_ = current_time;
      prev_initialized_ = true;
      return {0.0, 0.0, 0.0, 0.0, 0.0};
    }

    // double dt = current_time - prev_time_;
    // if (dt <= 0.0) {
    //   return {0.0, 0.0, 0.0, LPFVel_x_, LPFVel_y_};
    // }
    double dt = 0.05;

    double dx = x - prev_x_;
    double dy = y - prev_y_;
    double dpsi = psi - prev_psi_;

    double vx_fixed = dx / dt;
    double vy_fixed = dy / dt;

    double cos_psi = std::cos(psi);
    double sin_psi = std::sin(psi);

    double vx = vx_fixed * cos_psi + vy_fixed * sin_psi;
    double vy = -vx_fixed * sin_psi + vy_fixed * cos_psi;

    double angular_velocity = dpsi / dt;

    // Low-pass filter
    LPFVel_x_ = alpha_ * LPFVel_x_ + (1 - alpha_) * vx;
    LPFVel_y_ = alpha_ * LPFVel_y_ + (1 - alpha_) * vy;

    // Save current state
    prev_x_ = x;
    prev_y_ = y;
    prev_psi_ = psi;
    prev_time_ = current_time;

    return {vx, vy, angular_velocity, LPFVel_x_, LPFVel_y_};
  }

private:
  double alpha_;
  bool prev_initialized_;
  double prev_x_, prev_y_, prev_psi_, prev_time_;
  double LPFVel_x_, LPFVel_y_;
};

#endif  // VELOCITY_CALCULATOR_HPP

[ROS2 Multi Thread] Multi Thread의 필요성

lidarmansiwon — Sat, 31 May 2025 01:07:26 +0900

1. 개요

ROS2에서는 하나의 노드에서 여러 개의 토픽을 구독하거나, 다양한 센서 데이터를 동시에 처리해야 하는 경우가 많다. 이때, 시스템이 어떻게 콜백을 처리하는지에 따라 전체 성능과 반응 속도에 큰 영향을 미친다.

ROS2의 기본 실행 모델은 SingleThreadedExecutor이기 때문에, 아무리 많은 토픽을 구독하더라도 콜백은 하나씩 순차적으로 처리된다. 이로 인해 여러 센서 데이터가 동시에 들어와도 처리 지연(latency)이 발생할 수 있다. 이러한 문제를 해결하기 위한 방법이 바로 멀티 스레드(Multi-Threading)이다.

2. 상황 예제 코드

다음은 하나의 노드에서 IMU, Odometry 또는 PoseStamped 데이터를 구독하고, 일정 주기로 통합 처리를 수행하는 예제이다.

여러개의 Topic을 callback 함수를 통하여 subscribe 하는 극단적인 상황 예이다.

#include <iostream>
#include <memory>
#include <chrono>
#include <functional>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "mk3_msgs/msg/navigation_type.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav_msgs/msg/odometry.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

class odom_navigation : public rclcpp::Node
{
public:
  odom_navigation()
  : Node("odom_navigation")
  {
    navigation_publisher_ = this->create_publisher<mk3_msgs::msg::NavigationType>("navigation", 10); 
    timer_ = this->create_wall_timer(100ms, std::bind(&odom_navigation::process, this));  // 주기적으로 처리

    imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
      "/ouster/imu", 10, std::bind(&odom_navigation::imu_callback, this, _1));

    use_odometry = false;
    if (use_odometry)
    {
      odom_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
        "odometry", 10, std::bind(&odom_navigation::odometry_callback, this, _1));
    }
    else
    {
      pose_subscription_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
        "current_pose", 10, std::bind(&odom_navigation::pose_callback, this, _1));
    }
  }

private:
  void process()
  {
    RCLCPP_INFO(this->get_logger(), "Processing data...");

    // 수신한 센서 데이터를 활용하여 통합 처리
    if (last_imu_ && (use_odometry ? last_odom_ : last_pose_))
    {
      mk3_msgs::msg::NavigationType msg;
      msg.header.stamp = this->now();
      msg.yaw_rate = last_imu_->angular_velocity.z;
      if (use_odometry)
        msg.vel_x = last_odom_->twist.twist.linear.x;
      else
        msg.pos_x = last_pose_->pose.position.x;

      navigation_publisher_->publish(msg);
    }
  }

  void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "Received Odom");
    last_odom_ = msg;
  }

  void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "Received Pose");
    last_pose_ = msg;
  }

  void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "Received IMU");
    last_imu_ = msg;
  }

  bool use_odometry;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscription_;
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_subscription_;
  rclcpp::Publisher<mk3_msgs::msg::NavigationType>::SharedPtr navigation_publisher_;

  // 수신한 데이터를 저장할 멤버 변수
  sensor_msgs::msg::Imu::SharedPtr last_imu_;
  nav_msgs::msg::Odometry::SharedPtr last_odom_;
  geometry_msgs::msg::PoseStamped::SharedPtr last_pose_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<odom_navigation>());
  rclcpp::shutdown();
  return 0;
}

3. 싱글 스레드 모델의 특징

기본적으로 아래와 같은 형태로 작성된 노드는 싱글 스레드 기반으로 동작한다.

rclcpp::spin(std::make_shared<odom_navigation>());

이때 사용되는 SingleThreadedExecutor의 동작 방식은 다음과 같다:

콜백을 하나씩 순차적으로 실행한다.
하나의 콜백 함수가 실행 중이면, 다른 콜백은 큐에 대기한다.
CPU 코어가 여럿 있더라도, 실제 처리는 한 스레드만 수행한다.
구조가 단순하고 디버깅이 쉬우며, 동기화 문제가 발생하지 않는다.
하지만, 센서 데이터가 많은 경우 병목 현상이 발생할 수 있다.

예를 들어, IMU와 Odometry, PoseStamped 데이터를 동시에 구독할 경우, IMU 콜백이 실행되는 동안 다른 콜백은 큐에 쌓여 순서대로 처리된다. 콜백 처리 시간이 길어지면 다른 센서 데이터도 늦게 처리되므로 실시간성이 떨어진다.

4. 싱글 스레드 사용 시 발생할 수 있는 문제

이 예제에서 SingleThreadedExecutor를 사용하면 다음과 같은 문제가 발생할 수 있다:

콜백 함수들이 순차적으로 실행됨: imu_callback, pose_callback, process()는 동시에 실행되지 않는다.
imu_callback()이 오래 걸리거나 블로킹 연산을 포함하면, process()나 pose_callback()이 지연된다.
센서 데이터가 빠르게 들어오는 경우, 처리 지연(latency) 으로 인해 데이터 유실이나 동기화 오류가 생길 수 있다.
process() 주기보다 빠른 센서 입력이 쌓이면서 큐 오버플로우 또는 지연 전파가 발생할 수 있다.

5. 멀티 스레드 모델의 필요성

이를 해결하기 위해 ROS2에서는 MultiThreadedExecutor를 제공한다.

멀티 스레드 실행 방식:

rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
executor.spin();

장점:

여러 개의 콜백을 동시에 병렬 실행할 수 있다.
각 콜백이 별도의 스레드에서 실행되므로, 콜백 처리 지연이 줄어든다.
멀티 코어 CPU를 적극적으로 활용 가능하다.

단점:

동시에 실행되는 콜백 간에 데이터 충돌 가능성이 존재한다.
공유 자원 보호를 위한 mutex 또는 atomic 변수 사용이 필요하다.
디버깅이 복잡해지고, 콜백 실행 순서가 예측 불가능할 수 있다.

6. 예제 상황에서의 멀티 스레드 사용 시 장점

예제 상황에서 MultiThreadedExecutor를 사용하면 다음과 같은 이점이 있다:

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<odom_navigation>();
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();
  rclcpp::shutdown();
  return 0;
}

장점:

콜백 함수가 병렬로 실행되므로, IMU 수신 도중에도 process()나 pose_callback()이 실행 가능하다.
각 콜백이 별도 스레드에서 실행되어, 센서 간 독립적인 처리가 가능하다.
멀티 코어 CPU를 효과적으로 활용하여 처리 속도를 높인다.
실시간성 개선 및 지연 감소.

7. 멀티 스레드 사용 시 주의할 점

멀티 스레드는 성능을 높이지만, 다음과 같은 문제를 유발할 수 있다:

콜백 간에 공유 자원(last_imu_, last_pose_ 등) 을 동시에 접근할 수 있으므로 race condition 발생 가능성 있음.
따라서 공유 자원 접근 시 std::mutex 또는 rclcpp::GuardCondition 등을 이용한 동기화가 필수.
콜백 순서가 보장되지 않기 때문에, 시간 스탬프를 기반으로 적절한 데이터 동기화 전략이 필요함.

8. SingleThreadedExecutor vs MultiThreadedExecutor 비교

콜백 실행	하나씩 순차 처리	병렬 실행 가능
콜백 처리 속도	느림 (병목 가능성 있음)	빠름 (실시간성 향상)
동기화 문제	없음	있음 (mutex 등 필요)
구현 난이도	쉬움	어려움
적합한 상황	단일 센서, 낮은 빈도	다중 센서, 고빈도, 실시간성 요구

9. 결론

SingleThreadedExecutor는 구조가 간단하고 안전하지만, 콜백 처리가 병목을 일으킬 수 있다. 실시간성이 중요한 시스템이나 다중 센서 환경에서는 MultiThreadedExecutor를 사용하여 병렬 처리를 적용하는 것이 필요하다. 단, 이 경우 공유 자원에 대한 철저한 동기화 설계가 요구된다.

멀티 스레드를 사용하는 것이 항상 정답은 아니며, 시스템 요구사항과 데이터 흐름, 리소스 특성에 따라 적절한 실행 모델을 선택하는 것이 중요하다.

멀티 스레드는 강력한 도구이지만, 동기화와 구조적 설계를 신중히 고려해야만 안정적이고 빠른 시스템을 만들 수 있다.

※ 본 글은 필자가 공부를 하며 정리한 내용입니다. 차후 더 자세한 예제와 활용 방식에 대하여 포스팅 예정입니다. 감사합니다.

[C++ 기초부터 심화까지 Chapter 06. 객체지향과 클래스]

lidarmansiwon — Thu, 8 May 2025 00:30:50 +0900

6장 소개

프로그래밍 패러다임: 프로그램을 어떤 절차와 구조로 만들 것인지에 대한 스타일이나 접근 방법
이절에서는 여러 패러다임 중 비구조적 프로그래밍과 절차적 프로그래밍에 대하여 알아봄

06-1 객체지향 이전의 프로그래밍 패러다임

비구조적 프로그래밍

비구조적 프로그래밍: 코드를 구조화하지 않고 작성하는 방법
첫번째 줄부터 마지막 줄까지 차례대로 실행되며, 코드의 흐름을 이동하는 goto문을 사용하는 특징이 있음.
대표적 비구조적 프로그래밍 언어: 어셈블리어, 초창기 fortran

절차적 프로그래밍

소스 코드를 여러 부분으로 나눠서 활용하는 패러다임
프로시저를 이용해 구조화하는 방식
프로시저: 일련의 코드 묶음, 함수
대표적 절차적 프로그래밍 언어: C, cobol, fortran
절차적 프로그래밍에서는 코드의 논리 구조를 --> 모듈화할 수 있다.
모듈화를 하면 같은 기능을 수행하는 코드를 다시 작성하지 않아서 재사용할 수 있음.
라이브러리처럼 누군가가 만들어 놓은 기능을 사용하면 프로그램을 더 쉽게 개발할 수 있음.
코드 가독성이 좋아짐.
절차적 프로그래밍으로 작성한 코드는 프로그램의 중심 흐름을 당담하는 코드와 프로시저를 구현하는 코드로 나뉨.

절차적 프로그래밍의 한계

프로시저가 가진 논리적 다층 구조를 프로그래밍 내부에서 표현하는데 한계가 있음.
불필요한 프로시저를 호출하거나 전역 변수를 수정할 수 있다. --> 프로그램 동작에 치명적인 영향을 줄 수 있음.

06-2 객체지향 프로그래밍

객체지향 프로그래밍의 4가지 특징 이해하기.

객체지향 프로그래밍 개념

객체지향 프로그래밍(object oriented programming, OOP): 객체라는 논리적인 개념으로 코드를 구성
"사물을 객체로 인식하고 표현하는 것은 사람에게 매우 자연스럽고 이해하기 쉬운 방식"
이전의 패러다임은 모두 하향식(top-down) 방법이지만, 객체지향 프로그래밍은 논리적인 단위를 먼저 정의한 후에 이를 조합해 프로그램이 수행할 기능을 만드는 상향식(bottom-up) 방법이다.
객체지향 프로그래밍에서는 데이터(속성)와 함수(기능)을 가진 객체를 먼저 정의하고, 객체를 기초로 원하는 실행 흐름을 소스 코드로 작성.
상향식 == 레고

has-a 관계: 동력장치 (내부: 가솔린 엔진, 가속페달, 동력 전달 장치(미션, 바퀴), 제동 장치(브레이크)) --> 이러한 포함 관계. 상호 교환 X
is-a 관계: 엔진(가솔린 엔진, 디젤 엔진, 가스 엔진, 전기 엔진) --> 상호 교환 O, 하위 객체가 상위 객체를 대신할 수 있음. --> Generalization(일반화)라고 표현.
is-a 관계에서는 상위 객체의 속성을 그대로 물려받아 새로운 객체의 특성을 추가하는 상속(inheritance)이 가능함. 상속은 07장에서 다룸.
상속: 엔진{ 내연기관 엔진(가솔린 엔진, 디젤 엔진, 가스 엔진), 전기 엔진}

객체 지향 프로그래밍의 특징

추상화 (abstraction)
캡슐화 (encapsulation)
상속성 (inheritance)
다형성 (polymorphism)

추상화 (abstraction)

현실 세계의 사물을 모델링하여 객체로 만들 때, 어떤 부류에서 불필요한 요소는 배제하고 공통된 특징만 추출하는 것.

캡슐화 (encapsulation)

복잡한 내부 기능을 묶어 외부에서 불필요한 정보를 감추는 것
Ex) 객체 외부 시선 --> 가속 페달 --> 출력 조절(내연기관 엔진)
객체 내부 시선 --> 가속 페달 --> 출력 조절(출력 조절기 --> 피스톤)

상속성 (inheritance)

가솔린 엔진은 내연기관 엔진의 속성을 그대로 상속받아 가솔린 연료를 사용하는 특성
부모 객체의 특성을 이어받는 것을 상속

다형성 (polymorphism)

상속받은 객체는 부모 객체의 역할을 대신할 수 있음.
Ex) 내연 기관 엔진이 사용되는 자리에 가솔린, 디젤, 가스 엔진을 사용할 수 있음.
완전히 똑같으면 대체할 이유가 없음. 상속받은 객체는 자신만의 특성을 반영하여 다르게 동작해야함.
--> 이처럼 상속 관계의 객체에서 같은 기능(함수)이 다르게 동작하는 특성을 다형성이라고 함.

06-3 클래스와 인스턴스

클래스 개념 이해
C++ 클래스와 인스턴스를 선언하는 문법 이해

클래스란?

객체와 클래스는 비슷함: 데이터와 함수를 포함하는 논리 단위
변수: 값을 저장할 수 있는 공간, 어떤 형식의 값이 저장되는지 데이터 형식으로 컴파일러에게 알려줌.
클래스는 데이터 형식, 객체는 변수
클래스는 객체가 포함하는 데이터와 함수를 정의하는 문법적인 요소.
객체는 특정 클래스를 사용할 수 있도록 만든 변수
클래스를 정의할 때는 객체가 가질 데이터와 함수를 작성
이때, 클래스가 포함하는 데이터를 '멤버 변수', 함수를 '멤버 함수'라고 함.
멤버 변수는 클래스의 속성을, 멤버 함수는 클래스의 행동을 나타냄.
생성자와 소멸자라는 특별함 함수도 포함할 수 있음
멤버 변수와 멤버 함수를 정의할 때는 public, private와 같은 접근 지정자가 필요함.
접근 지정자: 접근 범위를 통제 --> 캡슐화와 관련

클래스 정의 문법

클래스 선언부: 멤버 변수, 멤버 함수, 생성자, 소멸자
멤버 함수 정의부: 멤버 함수 정의

// 클래스 선언 키워드 + 클래스 이름   + 접근 지정자 + 부모 클래스(선택)
//    (class)     + (gs_engine) + (public)  + (ic_engine)
// class 선언부
class gs_engine : public ic_engine {
public:
    gs_engine();  // 생성자와 
    ~gs_engine(); // 소멸자(선택)
    int get_current_fuel() {return current_fuel;};          // 멤버 함수 선언과 구현

private:
    void acceleration_output() override;                    // ""
    void increasing_fuel() { increasing_piston_speed(); };  // ""
    void increasing_psiton_speed() {};                      // ""

    int current_fuel; //멤버 변수 선언
    int piston_speed; // ""
};
// ---

// 멤버 함수 정의부
// 멤버 함수 네임스페이스 -> gs_engine
void gs_engine::acceleration_output() {
    increasing_fuel();
    current_fuel++;
}
// --

클래스 선언부: 클래스 선언하는 곳, 멤버 함수 정의를 제외한 클래스와 관련된 모든 것들을 이곳에서 선언.
멤버 함수 정의부: 클래스에 선언한 멤버 함수를 정의.
클래스 선언 키워드: 클래스를 선언하는 class 키워드.
클래스 이름: 클래스를 나타내는 이름. 객체를 만들 때 사용하는 데이터 형식으로 볼 수 있음.
부모 클래스(선택): 클래스가 다른 클래스를 상속받을 때, 쌍점(:) 다음에 접근 지정자와 부모 클래스를 지정. 상속 받지 않으면 생략.
접근 지정자: 멤버 변수와 함수가 외부에서 접근할 수 있는지를 표시.
생성자와 소멸자(선택): 객체가 생성되거나 소멸할 때 호출되는 함수. 선택적으로 사용 가능
멤버 함수 선언과 정의: 객체에 포함할 멤버 함수를 선언. 간단한 함수는 클래스 선언부에서 중괄호{}를 이용해 정의하기도 함.
멤버 함수 네임스페이스: 멤버 함수 정의부에서는 해당 함수가 어떤 클래스에 속하는지 알 수 있도록 클래스 이름을 네임스페이스로 사용.

객체 선언

앞에서는 클래스 정의하는 방법을 알아봄.
정의한 클래스를 사용하려면 객체를 선언해야함.
클래스의 객체를 선언하는 방법은 데이터 형식으로 변수를 선언하는 것과 같음.
클래스의 객체를 선언하는 두가지 방법

클래스를 데이터 형식으로 삼고, 객체를 선언.
new 키워드를 사용하여 동적으로 메모리를 할당. --> 이 경우 반드시 delete 키워드로 메모리를 해제 해야함.

// 클래스 정의
class engine {...(생략)...};

// 클래스를 데이터 형식처럼 사용하는 방법
engine my_engine;

// 클래스 형식으로 동적 메모리 할당과 해제 방법
engine *my_engine_pointer = new engine();
delete my_engine_pointer;

- 객체로 클래스의 멤버에 접근
my_engine.current_fuel; // 멤버 변수에 접근
my_engine.inceasing_piston_speed(); // 멤버 함수 호출

- adative_cruise_modeling.cpp 참고

※ 이 글은 직접 구매한『Do it! C++ 완전 정복』(문종채, 조규남 저, 성안당)을 참고하여 개인적으로 학습한 내용을 정리한 것입니다. 본문에 사용된 내용 및 예제 코드는 책의 내용을 기반으로 하되, 이해를 돕기 위한 개인적인 해석과 실습 결과를 포함하고 있습니다.

참고한 예제 코드: [GitHub - mystous/DoItCPP]([https://github.com/mystous/DoItCPP)](https://github.com/mystous/DoItCPP))
예제 코드의 일부는 위 오픈소스 저장소를 참고하거나 수정하여 활용했으며, 해당 저장소는 학습용으로 공개되어 있습니다.

[C++ 기초부터 심화까지 Chapter 05. 예외 처리 구문]

lidarmansiwon — Wed, 7 May 2025 23:59:54 +0900

5장 소개

예외 처리하기

프로그래밍에서 exception이란 문법적인 오류가 아닌 의도하지 않은 오류를 의미
즉, 예외 처리란 프로그램 실행 흐름상 발생하는 오류에 대응하는 방법

05-1 예외 처리 구문

try, catch, throw 문으로 예외 처리하기
assert로 예외 처리하기

소스 코드에 예외 처리를 해두면 예기치 못한 상황에도 유연하게 대처할 수 있음
안정성을 높이고 비정상적으로 종요되지 않도록 함.
유지 보수와 디버깅을 쉽게 만듦 --> 큰 규모의 프로젝트를 진행할 때, 함께 일하는 워커들이 코드의 안정성과 이해도를 높이는데 도움을 줌.

try, catch, throw 문으로 예외 처리

try: 예외가 발생할 수 있는 코드 블록을 중괄호 {}로 감싸 준다.
throw: 예외를 catch 블록으로 던진다.
catch: throw로 던진 예외를 받아서 처리한다.
throw가 던지는 예외 형식과 일치할 때만 예외를 처리하도록 함.
try_throw_catch_1.cpp 참고

try {
    // 예외를 던질 수 있는 코드      --> 1. 예외 발생 가능한 코드 블록
    throw 예외_값                 --> 2. 예외 발생
} catch (예외_형식 예외_이름) {     --> 3. 예외 형식 확인  
    // 예외를 처리하는 코드          --> 4. 예외 처리 블록
}

catch(...) 문으로 기타 예외 처리

만약 어떤 데이터 형식의 예외를 throw로 던졌는데, 받아주는 catch 문이 없다면?
catch(...) 문으로 처리 되지 않은 나머지 예외 모두 받기가 가능함.

catch (...) // 처리되지 않은 나머지 예외 모두 받기
{
  cout << "catch all" << endl;
}

예외가 전달되는 순서

만약 함수 내에서 예외를 던졌는데, 받아주는 catch가 없으면? --> 호출한 main 영역의 catch 문에서 정상적으로 처리된다.
예외 처리의 책임은 throw가 발생한 함수를 호출한 쪽으로 넘어간다. --> 이 현상을 Stack unwinding, 스택 풀기라고 한다.

assertion, 어설션을 이용한 예외 처리

assertion은 코드를 검증하여 예상치 못한 상황에서 프로그램 동작을 중단시키는 도구, 안전성과 신뢰성을 높여줌.
C++에서는 헤더에 정의된 assert 매크로를 통하여 예외를 비교적 간단하게 처리할 수 있다.
assert는 디버그 모드에서 오류가 생길수 있는 부분을 검사할 수 있는 매크로이다.

문법 요약

예외가 발생할 수 있는 코드를 try 블록으로 감싼다.
예외가 발생하면 프로그램의 제어는 즉시 try 블록 다음에 오는 catch 블록으로 넘어간다.
throw 키워드로 예외를 던진다. throw 다음에 지정하는 예외 형식은 모든 유형이 될 수 있다.
catch 블록에는 throw로 던진 예외 형식으로 매개변수를 선언한다. 이 매개변수로 오류 메시지 같은 예외 정보에 접근한다.
단일 try 블록에 catch 블록이 여러 개 있을 수 있으며, 각 블록은 서로 다른 유형의 예외를 처리하도록 정의한다.

try {
 // 예외가 발생할 수 있는 코드 영역
 	throw exception_value; // 예외를 강제로 발생시키는 코드
} catch (exception_type e) {
	// 예외가 발생했을 때 실행되는 코드 블록
    // e에는 발생한 예외의 정보가 담겨 있음
    // exception_type은 실제 예외 형식에 따라 정의
 }

05-2 예외 처리 생략과 실패 대응

예외 처리 메커니즘도 때로는 예외를 처리하지 못할 수 있음.
noexcept와 set_terminate를 활용해 예외 처리와 예외 처리 실패에 대응하고 프로그램의 안정성과 신뢰성을 높이는 방법을 알아봄

예외 처리 생략 - noexcept

함수에서 문제가 발생할 때, 반드시 예외를 발생시켜야하는 것음 아님.
당연히 오류와 예외는 다름.
값이나 실행 흐름을 충분히 예측할 수 있을 때는 if문으로 처리하는 것이 성능 면에서 훨씬 이득.
함수가 예외를 던지지 않음을 나타낼 때는 다음처럼, noexcept 키워드로 명시할 수 있음.
noexcept를 사용하여 함수가 예외를 던지지 않음을 명시하면 컴파일러가 코드를 최적화하고 빠르게 실행하는데 도움이 됨.
int func() noexcept // 함수가 예외를 던지지 않음을 명시

※ 이 글은 직접 구매한『Do it! C++ 완전 정복』(문종채, 조규남 저, 성안당)을 참고하여 개인적으로 학습한 내용을 정리한 것입니다. 본문에 사용된 내용 및 예제 코드는 책의 내용을 기반으로 하되, 이해를 돕기 위한 개인적인 해석과 실습 결과를 포함하고 있습니다.

참고한 예제 코드: [GitHub - mystous/DoItCPP](https://github.com/mystous/DoItCPP)
예제 코드의 일부는 위 오픈소스 저장소를 참고하거나 수정하여 활용했으며, 해당 저장소는 학습용으로 공개되어 있습니다.

[C++ 기초부터 심화까지 Chapter 04. 실행 흐름 제어]

lidarmansiwon — Wed, 7 May 2025 23:39:17 +0900

이번 장에서는 특정 조건에 따라 프로그램의 실행 흐름을 제어하는 방법을 알아봄
조건문과 반복문은 흐름을 조절하고 반복되는 작업을 자동화하는데 필수

04-1 조건문으로 흐름 제어

if 문으로 분기하기

if문의 동작 방식은 매우 익숙함.

if ()
{
    // 실행문1
}
else if()
{
    // 실행문2
}
else
{
    // 실행문3
}

switch 문으로 분기하기

if else 문으로 처리할 수 있지만, 갈래가 많아지면 코드를 읽기가 어려워짐
따라서 switch 문을 사용할 수 있음
switch 문은 여러 경우의 수 가운데 하나를 선택해 실행할 때 사용함.

switch문의 동작 방식

switch 문은 하나의 변수나 표현식을 평가한 결과에 따라 case 중 하나를 선택해 해당 코드를 실행.
이떄 label은 경우의 수를 나타내는 상수임!!

switch (표현식)
{
case 상수1 :
  // 상수 1일때 실행할 코드
  break;
case 상수2 :
  // 상수 2일때 실행할 코드
  break;
default :
  // 어떤 case에도 해당하지 않을 때, 실행할 코드
}

04-2 반목문으로 흐름 제어

do~while 문의 동작 방식

do~while은 while문처럼 동작하지만 코드블록을 먼저 실행한 다음, 조건을 검사함. 최소한 한 번은 실행되도록 보장

#include <iostream> 
using namespace std;

int main()
{
    int count = 0;
    do {
        cout << count << endl;
        count ++
    } while (count < 5);
    return 0;
}

for 문으로 반복하기

반복 횟수를 정확히 제어해야할 때 효과적으로 사용할 수 있음.
초기화, 조건식, 증감식을 모두 한곳에서 관리하여 코드가 더 간결함.
for문 형식

for (초기화; 조건식; 증감식){
  // 반복 실행 코드
}

초기화: 반복이 시작되기 전에 한번만 실행되며, 변수를 선언하고 초기화
조건식: 반복이 실행되기 전에 매번 평과됨. 참이면 반복 실행.
증감식: 반복한 후에 실행되며 조건식에 사용된 변수를 업데이트.

break 키워드로 원할 때, 반복문은 빠져나올 수 있음. (for, while, do~while 모두 적용 가능)

04-3 표현식과 구문의 차이

표현식과 구문은 중요한 개념. 각각 다르게 사용됨.

표현식(expression)

하나 이상의 변수, 연산자, 리터럴을 조합해 값을 평가하고 결과를 반환.
수학에서 수식과 같으며 결과값이 항상 나오는 코드

// 표현식 예시
a = b + c;
result = func();

구문(statement)

하나 이상의 연산을 수행하거나 동작을 실행하는 명령문의 집합으로, 값을 할당하거나 프로그램의 실행 흐름을 제어함.

// 구문 예시
int a = 0;
while (true)
{
++a;
if (a > 10)
break;
}

[Fossen 리뷰] Handbook of Marine Craft Hydrodynamics and Motion Control 정리

lidarmansiwon — Wed, 7 May 2025 20:31:06 +0900

해양 공학, 자율운항 선박, 동역학 제어 시스템에 관심이 있는 분들이라면 한 번쯤 들어봤을 책,
Thor I. Fossen의 Handbook of Marine Craft Hydrodynamics and Motion Control.

이 책은 해양 선박 및 수중체의 운동 해석, 모델링, 제어 이론까지 폭넓게 다루고 있으며, 자율운항시스템을 연구하거나 개발하는 데 있어 거의 필독서로 꼽힘.

왜 이 책을 공부하는가?

현재 자율운항 선박의 제어 및 경로 계획 알고리즘을 연구하고 있음. 이 책은 그 이론적 기반이 되는 수학 모델과 실제 적용 예제를 폭넓게 담고 있어, 보다 체계적인 이해를 위해 본격적으로 공부하고 정리하려고 함.

앞으로 포스팅할 예정 글

각 장의 핵심 이론 요약
중요한 수식 및 모델링 해설
실제 응용 사례나 코드 분석 (필요시 Python/Simulink 등 활용)
개인적인 이해를 돕기 위한 정리 및 해설

이 과정을 통해 이 책의 내용을 보다 쉽게 풀어내고, 관련 분야를 공부하는 분들에게도 도움이 되는 자료를 만들어가고자 합니다.

참고 도서: *Handbook of Marine Craft Hydrodynamics and Motion Control* by Thor I. Fossen
이 글은 위 도서를 개인적으로 학습하고 이해한 내용을 기반으로 정리한 것이며, 책의 원문이나 도표, 수식 등을 무단 복제하지 않았습니다. 저작권은 저자 및 출판사에 있으며, 이 글은 학술적 참고 및 비영리 학습 목적으로 작성되었습니다.

T. I. Fossen and T. Perez (2004). Marine Systems Simulator (MSS)
URL: https://github.com/cybergalactic/MSS
실습 예제 및 코드 일부는 위의 오픈소스 저장소를 참고하거나 수정하여 활용하였으며, 저장소는 MIT 라이선스로 공개되어 있습니다.