Optimal Parking, Part 3 - 최적제어와 궤적 최적화

차량 모델은 가능한 운동을 정의하고, RRT*는 자유 공간을 통과하는 경로를 제공합니다. 하지만 여러 후보 궤적 중 하나를 선택하는 원칙도 필요합니다. 이것이 최적제어의 역할입니다.

Optimal Parking은 최적제어의 언어로 플래닝 목표를 정식화한 뒤, sequential quadratic program을 통해 그 문제의 국소 근사를 풉니다.

1. 상태, 입력, 동역학

차량 상태와 제어 입력을 다음과 같이 두겠습니다.

$$x_k = [p_{x,k}, p_{y,k}, \psi_k, v_k, \delta_k]^T, \qquad u_k = [a_k, \dot{\delta}_k]^T.$$

샘플링 시간 $T_s$로 운동학적 자전거 모델을 이산화하면 궤적은 다음 조건을 만족해야 합니다.

$$x_{k+1} = f(x_k, \nu_k).$$

이 등식은 모델 제약 조건입니다. 주차 공간에 도달하더라도 이 조건을 위반하는 후보 궤적은 유효한 차량 운동이 아닙니다.

2. 유한 예측 구간 목적 함수

$N$개의 단계로 구성된 예측 구간에서 일반적인 목적 함수는 구간 비용과 종단 비용을 결합합니다.

$$J = \sum_{k=0}^{N-1} \left( \lVert x_k - x_k^{ref} \rVert_Q^2 + \lVert \nu_k \rVert_R^2 \right) + \lVert x_N - x_{goal} \rVert_{Q_f}^2.$$

구간 비용은 궤적을 기준 경로에 가깝게 유지하고 큰 제어 입력을 억제합니다. 종단 비용은 최종 자세와 상태가 주차 목표에 가까워지도록 합니다.

$Q$, $R$, $Q_f$는 단순한 설정값이 아닙니다. 플래너가 무엇을 우선하는지 표현합니다. 종단 가중치를 키우면 목표 도달을 더 중시하고, 입력 가중치를 키우면 더 부드럽고 공격적이지 않은 제어를 선호합니다.

3. 제약 조건은 문제의 일부다

최적화에는 다음과 같은 제약 조건이 적용됩니다.

$$\underline{x} \le x_k \le \overline{x}, \qquad \underline{u} \le u_k \le \overline{u},$$

여기에 이산 동역학과 장애물 여유 조건이 추가됩니다. 차량 크기와 safety margin은 기하학적 장애물 표현을 상태 궤적과 연결합니다.

따라서 궤적 최적화는 단순히 polyline을 매끄럽게 만드는 작업이 아닙니다. 목표에 가깝고, 차량 모델과 일치하며, 환경에서 허용되는 궤적을 동시에 찾아야 합니다.

4. MPC와 SQP의 관계

Model Predictive Control은 유한 예측 구간 최적제어 문제를 반복해서 풀고, 해의 앞부분만 실행한 다음 측정된 상태에서 다시 계획합니다. 현재 Optimal Parking의 처리 흐름은 MPC보다는 궤적 최적화로 설명하는 편이 정확합니다. 계획한 예측 구간의 주차 동작을 한 번 최적화하고, 그 결과 궤적을 시각화하거나 실행에 사용하기 때문입니다.

비선형 문제를 직접 풀기는 어렵습니다. SQP는 현재 궤적 주변에서 국소 quadratic program을 반복적으로 구성해 이 문제를 다룹니다.

$$\Delta x_{k+1} \approx A_k \Delta x_k + B_k \Delta u_k + r_k.$$

QP를 풀고 궤적을 업데이트한 뒤 동역학과 제약 조건을 다시 선형화합니다. 다음 챕터에서는 이 QP 반복을 구현 관점에서 설명합니다.

5. 각 방법의 역할 분담

플래너는 전역 방법과 국소 방법을 결합합니다. 두 방법이 해결하는 문제가 다르기 때문입니다.

• RRT*는 경로를 탐색하고 경로의 위상 구조를 선택하는 데 도움을 줍니다.
• 최적제어는 궤적이 바람직하고 실행 가능하다는 의미를 정의합니다.
• SQP는 비선형 국소 문제를 일련의 QP로 변환합니다.
• OSQP는 이 QP 하위 문제를 효율적으로 풉니다.

이 역할을 분리하면 아키텍처를 이해하기 쉬워집니다. 샘플링은 의미 있는 초기값을 제공하고, 최적제어는 이를 개선할 목적 함수와 제약 조건을 제공합니다.