Advanced Pure Pursuit: 원리부터 튜닝까지
1. Pure Pursuit — 경로 추종 제어의 원리
Controller
gb_optimizer가 만든 raceline을 실제 조향·속도 명령으로 바꾸는 런타임 제어기입니다. controller 패키지의 controller_manager 노드 — L1 guidance 기반 Pure Pursuit.
목표
- raceline 추종: 차량이 매 순간 따라가야 할 현재 경로(
/local_waypoints)를 받아 조향각과 목표 속도를 계산합니다. 평상시엔 global line을 추종하지만, 장애물이 있으면 회피 스플라인을 추종하거나 상대 차량과 일정 거리를 유지하는 Trailing을 수행합니다. - 순수 Pure Pursuit을 뼈대로: 복잡한 차량 동역학 모델 없이 기하학적 Pure Pursuit을 기본 조향 법칙으로 두고, 그 위에 적응형 lookahead(L1 guidance)·헤딩 보정 PID를 추가했습니다.
- 상황 대응: 앞 차 추종(trailing), 출발(start), 비상정지(AEB), 경로 상실 시 반응형 주행(FTG)까지 한 노드에서 처리합니다.
메인: 순수 Pure Pursuit
기하학적 원리
Pure Pursuit은 가장 단순하고 견고한 경로 추종 알고리즘입니다. 경로 위에서 차 앞쪽으로 일정 거리 떨어진 한 점(Lookahead point)을 목표로 잡고, 그 점을 향해 원호를 그리며 가는 조향각을 계산합니다.
차량(kinematic bicycle model)이 현재 위치에서 진행 방향에 접하면서 목표점을 지나는 원호를 따라간다고 하면, 조향각은 차량 기준 목표점까지의 각도 $\eta$ 와 목표점까지 거리 $L_1$ 로 결정됩니다. wheelbase를 $L$ 이라 하면:
\[\delta = \arctan\!\left( \frac{2\,L\,\sin\eta}{L_1} \right)\]코드에서:
1
2
3
4
5
6
7
# 미래 위치에서 L1 점을 향하는 벡터와 진행 방향(yaw)으로 eta 계산
L1_vector = L1_point - future_position
eta = np.arcsin(np.dot([-np.sin(yaw), np.cos(yaw)], L1_vector)
/ np.linalg.norm(L1_vector))
# Pure Pursuit 조향각
steering_angle = np.arctan(2 * self.wheelbase * np.sin(eta) / L1_distance)
여기까지가 Pure Pursuit 조향의 전부입니다. 목표점이 멀면($L_1$ 큼) 완만하게, 가까우면 급하게 꺾습니다. 견고하고 연산이 가벼운 게 장점이지만, 목표점 거리($L_1$)를 어떻게 정하느냐가 문제로 남습니다.
$L_1$ 을 짧게 잡으면 저속·급코너는 잘 추종하지만 고속에서 조향이 흔들리고, 길게 잡으면 고속에선 안정적이지만 저속·급코너에서 코너를 가로지르는 현상(corner cutting)이 발생합니다.
2. Advanced Pure Pursuit
순수 Pure Pursuit은 look-ahead 지점을 향하는 원호를 그리는 기하 컨트롤러라, look-ahead 거리 $L_1$ 이 고정되면 저속에서는 반응이 느리고 고속에서는 흔들립니다. 즉 하나의 $L_1$ 로 저속과 고속을 동시에 만족시키기 어렵습니다. Advanced Pure Pursuit은 바로 이 지점을 보완합니다 — $L_1$ 을 속도·곡률에 따라 변화시키고, 순수 PP 기하가 놓치는 heading 오차·차량 동역학을 조향 보정으로 메웁니다.
순수 Pure Pursuit의 기하 조향각을 뼈대로 두고, 그 위에 다음을 얹었습니다.
① 속도·곡률 적응형 L1 (curvature)
$L_1$ 이 속도에 비례해 늘고, 코너(곡률 높음)에서는 줄어듭니다.
\[L_1 = (m_{l1}\cdot v - \text{curv} \cdot v^2) + q_{l1}, \qquad L_1 \in [t_{clip,min},\ t_{clip,max}]\]m_l1이 속도가 빠를수록 더 멀리 보게 만들어 고속 직선 안정성을 확보합니다.curvature_factor가 코너에서 늘어난 $L_1$ 을 다시 줄여 코너 커팅을 막습니다. (속도²에 비례 → 고속 코너에서 강하게 작용)
왜 곡률항이 필요한가: 속도만 반영하면 고속에서 $L_1$ 이 길어지는데, 곡선 구간에서 look-ahead가 길면 target point가 코너 안쪽으로 잡혀 차가 경로를 가로질러버립니다(코너 커팅). 곡률항이 이 look-ahead를 되돌려 경로를 따라가게 만들고, 속도² 계수 덕에 위험이 큰 고속 코너에서만 강하게 개입해 저속 주행성을 해치지 않습니다.
② 다단 조향 보정 (accel · ang_vel · 속도 · 횡오차)
순수 PP 기하로부터 산출된 조향각에, 아래 보정들을 차례로 더하거나 곱해 고속·코너 안정성을 높입니다.
- heading 보정 PID: 목표점 쪽으로 향해야 할 방향과 실제 차 방향의 차이(heading error)를 PID로 잡아 부족한 조향을 보태줍니다(
KP/KI/KD). - 가속도 스케일링 (accel): 가속 중에는 조향을 키우고, 감속 중에는 줄입니다(
acc/dec_scaler_for_steer). - 속도 스케일링: 고속일수록 조향 반응을 완만하게 줄여 스핀을 막습니다(
start/end_scale_speed,downscale_factor). - 횡오차 스케일링: 라인에서 벗어난 정도에 따라 조향·속도를 보정합니다(
lat_err_coeff,speed_factor_*).
③ 상태에 따른 속도 명령
Advanced Pure Pursuit은 추가로 state_machine이 발행하는 /behavior_strategy 토픽을 받아 현재 상황에 따라 아래의 모드들로 전환합니다.
- 기본 (추종 주행): raceline 속도를 따르되, 횡오차·heading 오차가 크면 그만큼 감속.
- TRAILING (앞 차 추종): 앞 상대차가 있으면 목표 간격(
trailing_gap)을 유지하도록 PID(trailing_p/i/d_gain)로 속도를 맞춥니다. 상대가 안 보이면blind_trailing_speed로 서행. - START (출발 부스트): 출발 시
start_speed로 잠깐 가속. - AEB (비상정지): 추종할 local waypoint가 비정상적으로 멀면(
AEB_thres) 정지.
3. RQT(Dynamic Reconfigure)로 Advanced Pure Pursuit 튜닝하기
3-1. 시뮬레이터와 RQT 켜기
L1 컨트롤러는 단독 실행이 아닌 통합 launch를 통해 구동됩니다. unicorn 환경을 켜고, 시뮬레이터와 RQT를 각각 다른 터미널에서 켜줍니다.
1
2
3
4
# 터미널 1 — 시뮬레이터 (L1 PP가 기본 컨트롤러)
unicorn
ros2 launch stack_master race.launch.xml sim:=true map:=f
# map:=맵 이름, 시뮬레이션에서 sim:=true
1
2
3
# 터미널 2 — rqt (반드시 같은 unicorn 환경)
unicorn
rqt
시뮬레이터:
RQT:
주의 사항: RQT를
unicorn환경 밖(또는 source 안 한 터미널)에서 켜면, DDS 통신 설정이 달라서 노드가 몇 개만 보이거나 하나도 보이지 않을 수 있습니다. 반드시 시뮬레이터와 같은 환경에서 rqt를 켜야 합니다.
만약 올바른 환경에서 RQT를 켰는데 아무것도 안 나온다면, 상단 메뉴에서 Plugins → Configuration → Dynamic Reconfigure를 클릭하면 됩니다.
3-2. controller_manager 파라미터
controller_manager: 레이싱 라인을 보고 조향각과 속도를 매 순간 계산해서 내보냅니다. 차의 주행 능력(코너를 잘 도는지, 떨리는지, 빠른지)은 거의 다 이 노드에서 결정됩니다.
🟢 적극 튜닝
한 번에 하나만, 10~20%씩 바꾸기.
| 파라미터 | 기본값 | 의미 | 증상별 조절 |
|---|---|---|---|
m_l1 | 0.47 | 속도에 비례한 lookahead 기울기. 가장 먼저 만질 값 | 좌우로 떨림/오버스티어 → 올리기 · 코너를 잘라먹고 바깥으로 나감 → 내리기 |
curvature_factor | 0.145 | 고속 직선에서 코너 진입을 잡아주는 역할 (속도²에 비례, 고속 코너 전용) | 코너 진입에서 바깥으로 밀림(언더스티어) → 올리기 |
KP | 0.8 | heading 오차에 비례해 조향을 보태줌 | 차가 코너를 크게 물지 않고, 진동하지 않는 선까지 올림. 너무 키우면 주행이 매우 불안정 |
KD | 0.05 | KP가 만든 큰 조향 입력을 스무딩해 큰 진동 억제 | KP의 5~10%로 세팅. 너무 키우면 잔진동이 심해져 주행 불이익 |
t_clip_min | 0.7 | lookahead 하한. 저속·타이트 구간 안정 | 저속에서 진동 시 늘리기 |
🟡 상황 보고 가볍게
| 파라미터 | 기본값 | 조절 방향 |
|---|---|---|
t_clip_max | 8.0 | lookahead 상한. 거의 안 걸림 |
lat_err_coeff | 1.0 | 라인 벗어남·코너에서 속도 깎는 강도 (0=안 깎음 ~ 1=최대) |
start_scale_speed / end_scale_speed | 6.5 / 10.0 | 고속에서 핸들 민감도를 줄이기 시작하는 속도 / 최대로 줄어드는 속도 |
downscale_factor | 0.5 | 고속에서 조향을 최대 몇 %까지 줄일지 |
steer_gain_for_speed / acc_scaler_for_steer / dec_scaler_for_steer | 1.0 / 1.0 / 1.0 | 고속 조향 증폭 상한 / 가속 중 조향 키우기 / 감속 중 조향 줄이기 (하드웨어 특성 반영) |
speed_lookahead | 0.25 | 모터의 가감속에 따라 변경 (하드웨어 특성 반영) |
🔵 솔로 주행이면 무시 (상대차·출발 전용)
| 그룹 | 파라미터 | 언제만 작동하나 |
|---|---|---|
| 앞차 추종 | trailing_gap, trailing_vel_gain, trailing_p/i/d_gain, blind_trailing_speed | 앞차를 따라가는 TRAILING 상태에서만 |
| 출발 부스트 | start_speed, start_curvature_factor, speed_diff_thres | 출발 가속하는 START 상태에서만 |
3-3. 튜닝 실습 — L1
RViz 상에서 빨간 구 마커로 표시되는 /lookahead_point 토픽을 통해 $L_1$ 과 헤딩이 어떻게 변하는지 확인하며 튜닝합니다.
평상시:
L1 공식
\[L_1 = m_{l1}\,v \;-\; c_\kappa\,\bar\kappa\,v^2 \;+\; q_{l1}, \quad \mathrm{clip}\big[\max(t_{min},\,\sqrt{2}\,e_{lat}),\ t_{max}\big]\]| 파라미터 | 기본값 | 역할 |
|---|---|---|
m_l1 | 0.47 | 속도 → $L_1$ 기울기 (높을수록 빠를 때 더 멀리) |
curvature_factor | 0.145 | 코너에서 $L_1$ 단축 정도 |
KP/KD | 0.8/0.05 | 헤딩 조절 |
튜닝 시나리오
A. 고속 직선 구간에서 진동 발생 (m_l1 = 0.001)
👀 $L_1$ 이 차량과 너무 가까워져 고속 직선 구간에서 차량이 좌우 진동하는 모습을 확인할 수 있습니다.
해결: m_l1을 조금씩 키워가며 고속에서 $L_1$ 이 좀 더 멀리 위치하도록 함으로써 조향이 완만해지도록 합니다.
B. 저속 구간에서 코너 커팅 발생 (m_l1 = 1.0)
👀 $L_1$ 이 너무 길어 코너 너머에 점이 찍히게 되어 차량이 코너 안쪽으로 파고들다 벽과 충돌하는 모습을 확인할 수 있습니다.
해결: m_l1을 낮추거나 curvature_factor를 높임으로써 코너 구간에서 $L_1$ 이 짧아져 Lookahead point가 코너를 넘어가지 않도록 합니다.
C. 고속에서 코너 진입 시 진동 발생 (curvature_factor = 0.9)
⚠️ curvature_factor가 너무 높을 경우 $L_1$ 이 과하게 줄어 고속 코너에서 차량이 좌우로 진동할 수 있습니다.
D. PID에 의한 헤딩 변화 (KD = 0.4)
3-4. 지향점
UNICORN Racing Team의 튜닝이 지향하는 목표는 플래너가 어떤 경로를 주든 컨트롤러가 적은 오버슛과 의도된 정도 내의 슬립으로 따라가게 하는 것입니다. race stack은 raceline, 회피 경로, 추월 경로 등 여러 플래너가 서로 다른 성격의 경로를 컨트롤러에 넘기는 구조인데, 컨트롤러가 특정 경로에만 맞춰져 있으면 플래너를 바꿀 때마다 흔들리거나 미끄러집니다. Advanced Pure Pursuit이 look-ahead와 조향을 속도·곡률·자세에 적응시키는 이유가 여기 있습니다 — 어떤 플래너가 어떤 경로를 주더라도 그것을 오버슛과 슬립 없이 충실히 추종하는 것이, 다양한 플래너를 자유롭게 사용하기 위한 전제이기 때문입니다.
4. Lap Analyser로 정밀 분석하며 튜닝하기
rqt로 눈으로 보며 튜닝하는 것만으로는 감에만 의존하게 됩니다. lap_analyser를 이용하여 같은 세팅이 실제로 더 빠른지, 라인을 얼마나 벗어났는지, 벽에 얼마나 가까웠는지를 숫자로 확인하면 훨씬 정밀하게 튜닝할 수 있습니다.
무엇을 측정하는가
차가 한 바퀴 돌 때마다 다음을 계산합니다.
- 랩타임(lap time) — 한 바퀴에 걸린 시간
- 평균 횡오차(avg lateral error) — 레이스라인에서 평균적으로 얼마나 벗어났는지
- 최대 횡오차(max lateral error) — 한 바퀴 중 가장 크게 벗어난 정도
- 트랙 경계 최소거리(min distance to boundary) — 벽에 가장 가까웠던 거리 (충돌 위험도)
NUM_LAPS_ANALYSED(기본 10바퀴)마다 이 값들의 평균과 표준편차를 터미널에 출력하고, lap_analyser/data/lap_analyzer_<날짜시각>.txt 파일로도 저장합니다.
표준편차가 작을수록 매 바퀴 일관되게 주행한다는 뜻 = 안정적인 세팅. 랩타임 평균이 낮아도 표준편차가 크면 아직 불안정한 상황입니다.
실행 방법
sim에서는 /car_state/odom_frenet와 /global_waypoints가 이미 race.launch.xml에서 발행되므로 별도 설정 없이 바로 동작합니다. 랩 결과는 실행한 터미널에 로그(warn)로 출력됩니다.
RViz에서는 lap_analyser가 발행하는 lap_marker(실제 주행 궤적)와 lap_data_vis 마커를 추가하면 차가 실제로 그린 라인을 눈으로도 확인할 수 있습니다.
튜닝에 활용하는 법
목표는 랩타임은 줄이면서, 최대 횡오차·경계 최소거리는 위험하지 않게 유지하는 세팅을 찾는 것입니다.
- rqt로 값 하나 바꾸기 → 몇 바퀴 주행 → 로그의 랩타임/횡오차/경계거리 숫자 비교 → 다시 조정. 이 루프를 반복합니다.
- 예)
m_l1을 올렸더니 랩타임은 줄었는데 코너에서 경계 최소거리가 확 작아졌다 → 너무 공격적. 조금 되돌리기. - 예) 랩타임 표준편차가 크다(주행이 들쭉날쭉) → 아직 불안정. 안정성 파라미터(
t_clip_min,downscale_factor등)를 재조정.
마무리
이 글에서는 Pure Pursuit의 기하 원리부터, $L_1$ 을 속도·곡률에 적응시키고 다단 조향 보정을 얹은 Advanced Pure Pursuit, 그리고 RQT 실시간 튜닝과 Lap Analyser 기반 정밀 튜닝까지 다뤘습니다. 핵심 순서는 m_l1 → curvature_factor → KP/KD를 한 번에 하나씩, 시나리오 영상처럼 증상을 보고 조절하는 것입니다. 확장 모듈의 이론적 배경은 Pure Pursuit 기반 조향·속도 제어 글도 함께 참고하세요.