병렬 주차 시 차량의 경로 추적은 자율주행 시스템에서 중요한 과제입니다. 전통적인 PID 제어기는 복잡한 주차 환경에서 성능이 제한적입니다. 이 문제를 해결하기 위해 모델 예측 제어(MPC)와 순수 추적(Pure Pursuit) 알고리즘을 적용하는 방법을 살펴봅니다.

MPC는 미래의 상태를 예측하여 최적의 제어 입력을 결정하는 기법입니다. 이는 병렬 주차와 같이 제한된 공간에서 정밀한 제어가 필요한 상황에 유용합니다. 다음은 MPC 제어기를 위한 기본 구조입니다.

class MPCController:
    def __init__(self, prediction_horizon=5):
        self.prediction_horizon = prediction_horizon

    def calculate_control(self, current_state, reference_path):
        control_sequence = []
        for t in range(self.prediction_horizon):
            # 비용 함수 정의: 위치 오차, 조향 각도 페널티
            cost = 0.5 * (current_state.x - reference_path[t].x)**2 + \
                   0.3 * (current_state.y - reference_path[t].y)**2 + \
                   0.2 * current_state.steering_angle**2
            
            # 비선형 최적화 문제 풀이 (IPOPT, CasADi 등 사용)
            optimized_controls = self.solve_optimization(cost)
            control_sequence.append(optimized_controls[0])
        
        # 첫 번째 스텝의 제어 입력 반환
        return control_sequence[0]
    
    def solve_optimization(self, cost_function):
        # ... 최적화 문제 풀이 로직 ...
        pass

MPC의 주요 단점은 계산 부하가 크다는 것입니다. 예측 범위를 늘리면 계산 시간이 기하급수적으로 증가합니다. 이를 개선하기 위해 '워ーム 스타트(Warm Start)' 기법을 사용할 수 있습니다. 이는 이전 반복의 해를 현재 반복의 초기 추측값으로 사용하여 수렴 속도를 높이는 방법으로, 계산 시간을 크게 단축시킬 수 있습니다.

순수 추적 알고리즘은 차량의 전방에 있는 특정 거리(전방 조망 거리)의 경로 포인트를 목표로 삼아 조향 각도를 계산합니다. 이 알고리즘은 구현이 간단하고 계산 부하가 적다는 장점이 있습니다.

def pure_pursuit_controller(current_pose, path, lookahead_distance=1.2):
    # 현재 위치에서 가장 가까운 경로 포인트 찾기
    nearest_index = find_closest_point(current_pose, path)
    
    # 전방 조망 거리에 해당하는 목표 포인트 인덱스 계산
    target_index = nearest_index + int(lookahead_distance / path.resolution)
    
    # 목표 포인트와 현재 포즈 간의 각도 계산
    alpha = math.atan2(path[target_index].y - current_pose.y,
                      path[target_index].x - current_pose.x) - current_pose.theta
    
    # 조향 각도 계산 (L: 차량 길이)
    steering_angle = math.atan2(2 * L * math.sin(alpha), lookahead_distance)
    
    return steering_angle

순수 추적 알고리즘의 성능은 전방 조망 거리에 크게 의존합니다. 이 값을 동적으로 조절하면 더 나은 성능을 얻을 수 있습니다. 예를 들어, 차량 속도가 증가할수록 전방 조망 거리를 늘리면 과도한 조향을 방지하고 안정적인 주행이 가능해집니다.

MPC는 저속에서 뛰어난 정밀도를 보이지만 고속에서는 불안정해질 수 있고, 순수 추적은 고속에서는 원활하지만 저속에서는 부정확할 수 있습니다. 이러한 특성을 고려하여 두 알고리즘을 결합한 하이브리드 제어기를 설계할 수 있습니다.

def hybrid_controller(current_speed, mpc_control, pursuit_control):
    # 속도 임계값에 따라 제어기 선택
    if current_speed < 1.0:
        return mpc_control
    elif current_speed > 1.4:
        return pursuit_control
    else:
        # 전이 구간에서 가중 평균 계산
        blend_ratio = (current_speed - 0.8) / 0.4
        return blend_ratio * mpc_control + (1 - blend_ratio) * pursuit_control

실제 테스트 과정에서는 센서의 블라인드 스팟 문제가 발생할 수 있습니다. 예를 들어, 후방 Lidar가 차량 근처에서 데이터를 제대로 수집하지 못해 가장 가까운 경로 포인트를 찾는 로직이 오작동할 수 있습니다. 이 문제를 해결하기 위해 과거 궤적을 기반으로 한 칼만 필터를 적용하고, 경로 포인트를 B-스플라인 곡선으로 부드럽게 만들어 센서의 노이즈에 대한 내성을 높일 수 있습니다.