Frenet 기반 Tracking: 상대 차를 시간축으로 잇기

Posted Jul 2, 2026 Updated Jul 7, 2026

By Jeongsang Ryu

17 min read

들어가며 — 우리가 풀려는 문제

자율주행 레이싱에서 내 차(ego)는 트랙 위를 달리면서 “내 앞에 다른 물체(상대 차)가 있는가? 있다면 어디에, 얼마나 빠르게 움직이는가?” 를 실시간으로 알아야 합니다. 그래야 추월할지, 따라갈지, 피할지를 판단할 수 있습니다.

이걸 담당하는 게 인지(perception) 이고, 두 단계로 나뉩니다.

Detection(검출) — “지금 이 순간” 라이다 스캔 한 장에서 물체 덩어리를 찾아 상자(box)로 표현. → 노드 detect (perception/src/detect.cpp) — 앞선 두 글(Detection & Clustering · Feature Extraction)에서 다룬 주제
Tracking(추적) — 매 프레임 검출된 상자들을 시간축으로 이어붙여 같은 물체에 고유 번호(ID) 를 부여하고, 속도를 추정하고, 정적/동적을 구분. → 노드 tracking (perception/scripts/multi_tracking.py) — 이 글의 주제

핵심 한 줄: Detection = “지금 뭐가 보이나”(스냅샷), Tracking = “그게 아까 그놈인가, 어디로 가고 있나”(시간의 흐름).

2. Tracking이란 무엇인가?

2.1 Detection만으로는 부족한 이유

Detection은 매 프레임 독립적입니다. 프레임 t에서 검출한 상자와 프레임 t+1에서 검출한 상자가 같은 물체인지 모릅니다. 그래서 detection 결과만 보면:

ID가 매 프레임 바뀝니다 (임시 번호 0,1,2…를 그때그때 매김)
속도를 알 수 없습니다 (한 장의 사진으로는 움직임을 못 봄)
노이즈에 취약합니다 (한 프레임 반짝 나타난 헛검출도 그대로 나옴)

2.2 Tracking이 하는 일

Tracking은 연속된 프레임을 이어붙여 다음을 만듭니다.

기능	설명
데이터 연관(Data Association)	이번 프레임의 검출이 이전에 추적하던 어떤 물체인지 매칭
상태 추정(State Estimation)	칼만 필터로 위치를 매끄럽게 다듬고 속도를 추정
ID 관리	같은 물체엔 영속적인 ID 유지, 사라지면 일정 시간 뒤 삭제
정적/동적 분류	벽·콘 같은 정적 물체와 움직이는 상대 차(동적) 를 구분

즉 tracking의 산출물은 “5번 물체는 동적(상대 차)이고, s=42m d=0.3m에 있으며 종방향 3.1m/s로 다가온다” 같은 의미 있는 상태입니다.

3. 전체 파이프라인 한눈에 보기

입력: 라이다 /scan, 트랙 기준선 /global_waypoints_scaled, 내 차 상태 /car_state/odom(_frenet)
중간 산출물: /detect/raw_obstacles — detection의 순간 검출 (위치만)
최종 산출물: /tracking/obstacles — 추적된 동적 장애물 (ID·속도 포함), /tracking/raw_obstacles — 정적/미분류

4. (앞 두 글 복습) Detection이 tracking에 넘겨주는 것

detect.cpp에서 일어나는 일

노드 이름 detect. 타이머(rate_detect, 기본 10Hz)마다 timerCallback()이 아래 순서로 돕니다.

아래 4.1~4.5는 앞선 두 글(Detection & Clustering · Feature Extraction)에서 이미 자세히 다룬 내용을, tracking 이해에 필요한 만큼만 압축한 복습입니다. 이미 읽으셨다면 곧장 4.5와 5장으로 가셔도 됩니다. 거리적응 임계값 D_max의 유도, dual-stage 클러스터링, rectangle fitting 검증 조건 등 세부는 두 앞 글을 참고하세요.

4.1 스캔 → 지도 좌표 변환

라이다 각 빔 (r, angle)을 laser 프레임의 점 (x, y)로 바꾸고, tf(map ← laser)로 map 프레임 좌표로 변환합니다. (코드: clustering())

4.2 트랙 밖 점 제거 (맵 필터)

미리 로드한 맵 이미지를 erosion(침식) 한 마스크로 “트랙 안쪽 점”만 남깁니다(GridFilter_.isPointInside). 벽 바깥·트랙 경계 노이즈를 걸러 상대 차 후보만 남기는 단계입니다. (자세히는 Grid Filter 참고)

4.3 Adaptive Breakpoint 클러스터링

점들을 훑으며 인접 점 사이 거리로 같은 물체인지 나눕니다. 핵심은 거리 임계값이 고정이 아니라 거리(range)에 비례해 커진다는 점 — 라이다는 멀수록 점 간격이 벌어지므로:

d_max = curr_range * sin(dφ) / sin(λ - dφ) + 3σ

d_max보다 가까우면 같은 클러스터, 아니면 가까운 기존 클러스터에 붙이거나(new_cluster_threshold_m 이내) 새 클러스터 생성. 마지막에 점이 너무 적은 클러스터(min_size_n, 기본 10점 미만)는 버립니다.

4.4 Feature Extraction — L-shape 피팅

차/장애물은 라이다에서 보통 ㄱ자(L) 로 보입니다. 후보 각도 90개(0°~90°)를 훑으며 점들이 직사각형 두 변에 가장 잘 붙는 각도를 점수(∑ 1/거리)로 골라, 박스의 중심·크기·회전각(theta) 을 추정합니다. 크기는 max_size_m보다 크면 상대 차가 아니라고 보고 버립니다(checkObstacles).

4.5 무엇이 tracking으로 넘어오나 (Frenet 변환 후 publish)

각 박스 중심 (x, y)를 GetFrenetPoint()로 (s, d) 로 바꿔 /detect/raw_obstacles(ObstacleArray)로 발행합니다.

Detection 출력의 한계 (중요): 이 메시지의 id는 그 프레임 안에서만 유효한 임시 번호(0,1,2…) 이고, 속도(vs,vd)·정적/동적 정보는 채워지지 않습니다. 오직 위치(s_center,d_center)·크기(size)만 있는 순간 스냅샷입니다. 여기에 시간축 정보를 입히는 게 다음 tracking 단계입니다.

5. Tracking 단계 — `multi_tracking.py`

노드 이름 tracking. /detect/raw_obstacles를 구독하고, 타이머(rate_tracking)마다 timer_callback() → predict → update → publish 순으로 돕니다.

5.1 데이터 연관 (Nearest-Neighbor)

이번 프레임 검출들 중, 추적 중이던 각 물체의 예측 위치에 가장 가까운 것을 매칭합니다(verify_position → get_closest_pos). 거리는 Frenet 상에서:

dist = hypot( normalize_s(Δs), Δd )      # s는 트랙 한 바퀴(seam)를 고려해 wrap

max_dist(게이트) 안에 든 후보 중 최근접을 선택. 동적 물체는 예측 위치를 쓰고 게이트를 aggro_multiplier배로 넓혀 더 잘 따라붙습니다. 매칭 실패가 이어지면 TTL(time-to-live) 을 깎고, TTL이 0이면 물체를 삭제합니다. 어떤 검출과도 매칭 안 된 “새 검출”에는 새로운 영속 ID(self.current_id, 계속 증가)를 부여합니다.

5.2 Frenet EKF — 상태와 모델

물체마다 하나씩 확장 칼만 필터(EKF) 를 붙입니다(Opponent_state). 상태 벡터는:

\[X = [\,s,\; v_s,\; d,\; v_d\,]^T\]

등속도(constant velocity) 모델 을 Frenet 위에서 씁니다.

\[\begin{aligned} s_{k+1} &= s_k + v_s\,\Delta t, &\quad v_{s,k+1} &= v_s \\ d_{k+1} &= d_k + v_d\,\Delta t, &\quad v_{d,k+1} &= v_d \end{aligned}\]

여기에 제어입력 $u$ 로 사전지식(prior)을 살짝 넣습니다: $-P_d\,d$, $-P_{vd}\,v_d$ 는 물체가 차선 중앙($d=0$)으로 돌아오려는 경향을, (물체를 잠깐 놓쳤을 때는) $P_{vs}(v_{target} - v_s)$ 로 트랙의 기준 속도를 따라가려는 경향을 반영합니다.

왜 KF가 아니라 “E”KF인가? 모델 자체는 선형(등속)이지만, $s$ 가 트랙 한 바퀴를 돌면 track_length에서 0으로 되감기는(seam) 비선형 처리가 필요합니다(normalize_s). 이 되감기를 측정/잔차 함수(hx, residual_h)에서 다뤄야 해서 EKF 형태(filterpy.ExtendedKalmanFilter)를 씁니다.

지난 글 Feature Extraction에서 정리했듯, detection이 rectangle/L-shape로 구한 방향(θ)·형상은 tracking으로 넘기지 않습니다 — RViz 마커 시각화 전용입니다. tracker가 받는 건 박스 중심 (s, d) 뿐이고, 속도는 그 중심의 프레임 간 차분으로 얻습니다.

측정은 detection이 준 (s, d) 와, 프레임 간 차분으로 얻은 속도를 함께 넣습니다(update()):

z = [ s_meas,  vs(=Δs·rate, seam 보정),  d_meas,  vd(=Δd·rate) ]

말도 안 되는 속도(vs가 −1~8 m/s 밖)는 튐으로 보고 필터를 재초기화해 발산을 막습니다.

아래 데모에서 상대 차가 Frenet 위에서 어떻게 추적되는지 — 예측→갱신을 반복하며 ID를 유지하고 속도를 추정하는 과정 — 을 직접 조작해볼 수 있습니다.

5.3 정적 / 동적 분류 (투표 방식)

물체마다 최근 측정들의 표준편차 std_s, std_d를 계산합니다(isStatic). 위치가 거의 안 변하면(둘 다 min_std 미만) “정적 표”를, 많이 변하면(max_std 초과) 리셋. 정적 표 비율 ≥ 0.5 이면 정적으로 판정합니다.

정적 → 위치를 평균값으로 고정 (벽·콘 등)
동적 → 상대 차로 보고 위 EKF로 추적, 속도 추정
미분류 → 아직 측정 수(min_nb_meas)가 모자란 상태(판단 보류)

5.4 속도 평활 & 발행

추정된 종/횡 속도는 길이 5 이동평균(vs_filt, vd_filt)으로 부드럽게 만든 뒤 발행합니다. 발행 분기(publishObstacles):

동적이고 위치 공분산이 충분히 작을 때(P[0][0] < var_pub)만 → /tracking/obstacles (신뢰할 만한 상대 차)
정적·미분류 → /tracking/raw_obstacles

이때 각 Obstacle 메시지에 영속 ID·속도(vs,vd)·공분산(s_var…)·정적여부(is_static)·Cartesian 좌표(x_m,y_m) 까지 모두 채워집니다.

6. 실제 실행에서 무엇을 어떻게 확인하나

빌드/실행 후(예: ros2 launch stack_master race.launch.xml sim:=true map:=f), 터미널에서 토픽을 직접 들여다보며 확인할 수 있습니다.

6.1 핵심 토픽 목록

토픽	타입	의미
`/detect/raw_obstacles`	`ObstacleArray`	detection 순간 검출 (위치만, 임시 ID, 속도 없음)
`/tracking/obstacles`	`ObstacleArray`	추적된 동적 상대 차 (영속 ID·속도·공분산)
`/tracking/raw_obstacles`	`ObstacleArray`	정적·미분류 장애물
`/tracking/static_dynamic_marker_pub`	`MarkerArray`	RViz 시각화용 마커
`/tracking/latency`	`Float32`	(measure 모드) 처리 주파수

6.2 토픽 값 직접 보기

  
# 추적된 상대 차 확인 — 여기서 속도·target id가 보입니다
ros2 topic echo /tracking/obstacles

# 검출(순간)과 비교해보기
ros2 topic echo /detect/raw_obstacles

6.3 `/tracking/obstacles`의 `Obstacle` 필드 읽는 법

필드	의미	확인 포인트
`id`	추적 대상 target ID	tracking에서는 같은 차면 프레임이 바뀌어도 번호가 유지됨 (detection은 매번 바뀜)
`vs`	종방향 속도 (s 방향, m/s)	상대 차가 다가오면 +, 이동평균으로 평활됨
`vd`	횡방향 속도 (d 방향, m/s)	차선 변경/좌우 움직임
`s_center`, `d_center`	Frenet 위치	“트랙 몇 m 지점, 중앙에서 몇 m”
`x_m`, `y_m`	대응되는 Cartesian 위치	RViz/지도에서의 실제 좌표
`s_var`, `vs_var`, `d_var`, `vd_var`	칼만 필터 공분산	값이 클수록 불확실 (작아야 신뢰)
`is_static`	정적 여부	`/tracking/obstacles`의 상대 차는 `False`
`is_visible`	현재 시야에 보이는지	가려지면 `False`(예측으로 유지)
`size`	박스 크기	detection L-shape에서 온 값

검출 vs 추적 차이를 눈으로 확인하는 법: /detect/raw_obstacles를 echo하면 vs,vd는 0이고 id가 매 프레임 바뀝니다. 반대로 /tracking/obstacles는 같은 차의 id가 유지되고 vs,vd에 실제 속도값이 찍힙니다. 이 대비가 “tracking이 무엇을 더 해주는가”를 가장 잘 보여줍니다.

6.4 RViz 마커 색으로 보기

/tracking/static_dynamic_marker_pub를 RViz에 띄우면 구(sphere) 마커 색으로 분류 상태를 볼 수 있습니다.

🟢 초록 = 정적(static) 장애물
🔵 파랑 = 동적(dynamic) 상대 차
🔴 빨강 = 미분류(아직 판단 전)

앞쪽(isInFront)이거나 공분산이 작아 신뢰도가 높은 물체는 마커가 더 크게 표시됩니다.

마무리

Frenet 좌표(s, d) 는 구불구불한 트랙을 “펴서” 보는 좌표계 — 트랙 위 움직임을 단순하게 만든다.
Detection(detect.cpp)은 라이다 한 장에서 클러스터링 + L-shape로 상자를 찾아 위치만 있는 순간 스냅샷(/detect/raw_obstacles)을 낸다.
Tracking(multi_tracking.py)은 NN 연관 + Frenet EKF(상태 [s,vs,d,vd]) + 정적/동적 분류 + TTL·영속 ID로, 의미 있는 상태(/tracking/obstacles)를 낸다.
실행 중엔 ros2 topic echo /tracking/obstacles의 id·vs/vd·is_static과 RViz 마커 색으로 동작을 확인한다.

참고자료

앞 글: Detection & Clustering · Feature Extraction
코드: perception/scripts/multi_tracking.py, perception/src/detect.cpp (unicorn-racing-stack)
메시지 정의: f110_msgs/msg/Obstacle.msg
상태추정: filterpy ExtendedKalmanFilter
배경 참고: datmo (kostaskonkk) · TU Delft thesis

racing stack, perception

This post is licensed under CC BY 4.0 by the author.