Static Obstacle Avoidance: 정적 장애물 회피 스플라인
관련 코드: planner/spliner/spliner/static_avoidance_node.py
static_avoidance_node.py(노드명 static_avoidance_planner)는 최적 라인(IQP global line)을 따라 주행하던 중 검출된 정적 장애물을 피해 라인 옆으로 잠시 벗어났다가 다시 복귀하는 회피 trajectory를 20Hz로 생성합니다. 상위 behavior/state machine이 지정한 장애물 하나를 받아, 현재 차량 자세에서 출발하는 짧은 우회 라인을 만들어 발행합니다.
핵심 아이디어는 회피선을 Frenet 좌표계에서 다루는 것입니다. $s$ 는 트랙을 따라 진행한 거리, $d$ 는 기준선으로부터의 횡방향 거리이며, 추종하는 최적 라인이 곧 $d=0$ 기준선이 됩니다. 회피선은 이 위에서 횡변위 $d(s)$ 를 장애물 옆 apex까지 키웠다가 다시 0으로 되돌리는 프로파일로 표현됩니다.
여기서 $p_{race}$ 는 최적 라인 점, $n$ 은 단위 법선입니다. 이렇게 두면 어느 쪽으로 얼마나 피할지가 $d$ 값 몇 개를 정하는 1차원 문제가 되고, 트랙을 벗어나지 않아야 한다는 조건이 $-w_{right} \le d \le w_{left}$ 라는 박스 제약이 됩니다. (gb_optimizer의 alpha 파라미터화와 동일한 구조입니다.)
생성된 $(s,d)$ 점들을 실제 $(x,y)$ 곡선으로 바꿀 때는, 각 점의 위치뿐 아니라 그 점을 지날 때의 진행 방향(접선)까지 함께 지정해 BPoly.from_derivatives로 보간합니다. 방향이 연속(C1)이므로 차량이 실제로 추종 가능한 매끄러운 곡선이 생성됩니다.
구조
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
/behavior_strategy (관심 장애물)
/car_state/odom_frenet (ego Frenet 상태 s, d)
/car_state/odom (ego cartesian 자세 x, y, yaw)
/global_waypoints (FrenetConverter 초기화, gb_vmax / gb_max_s)
/global_waypoints_scaled (최적 라인 = d=0 기준선)
|
v
static_avoidance_node.py ─ do_spline() (20 Hz)
|
| publishes /planner/avoidance/otwpnts
| └─ race.launch.xml에서 /planner/avoidance/static_otwpnts 로 remap
| publishes /planner/avoidance/markers (시각화)
| publishes /planner/avoidance/spline_samples (디버그 bounds-check 마커)
v
state machine / 추종 제어 (Pure Pursuit 등)
| 단계 | 함수 | 역할 |
|---|---|---|
| 1 | loop / behavior_cb | 관심 장애물 선택 + 전방 거리 게이트 |
| 2 | _more_space | 회피 방향과 깊이 $d_{apex}$ 결정 |
| 3 | do_spline (Frenet) | d-프로파일(apex + 복귀점) 생성 |
| 4 | do_spline (BPoly) | 접선 맞춤 스플라인으로 (x,y) 곡선화 |
| 5 | savgol / quad-ease | 스무딩 + 최적 레이스 라인 합류 |
| 6 | is_point_inside | 트랙 경계 안전 검증 / 폐기 |
1. 관심 장애물 선택과 거리 게이트
loop()은 20Hz로 돌며 관심 장애물이 있으면 do_spline()을 호출합니다. 관심 장애물은 behavior_cb가 /behavior_strategy의 overtaking_targets에서 첫 번째를 받아 정하고, 그 장애물이 정적(is_static == True)일 때만 이 노드의 회피 경로 생성으로 들어갑니다.
do_spline() 진입 직후, 장애물이 이미 전방 근거리(0.5 m 미만)에 있으면 회피하기에 너무 늦었으므로 빈 경로를 반환합니다.
1
2
pre_dist = (obs.s_center - cur_s) % track_length # 장애물까지 전방 s거리
pre_dist < 0.5 m → 빈 경로 반환 (전방 근거리)
2. 회피 방향과 깊이 결정 (_more_space)
좌우 중 빈 공간이 더 넓은 쪽으로 회피합니다. Frenet $d$ 축에서 장애물 가장자리부터 양쪽 벽까지 남는 여유를 각각 계산하고, 회피 마진만큼 비어 있는 쪽만 후보가 됩니다.
1
2
3
4
pos_gap = gb_wp.d_left - (obstacle.d_center + obs_radius) # +d = 좌측(LEFT) 벽 여유
neg_gap = (obstacle.d_center - obs_radius) + gb_wp.d_right # -d = 우측(RIGHT) 벽 여유
min_space = evasion_dist + spline_bound_mindist # = 0.65 + 0.2 = 0.85 m
→ 한쪽만 가능하면 그쪽 / 둘 다 가능하면 더 넓은 쪽
방향이 정해지면 회피 깊이 $d_{apex}$ 를 장애물 가장자리에서 evasion_dist만큼 더 바깥으로 잡습니다. 두 가지 클램프가 붙습니다.
1
2
3
d_apex = (obs.d_center ± obstacle.size/2) ± evasion_dist
clamp 1: 최적 라인(d=0)을 가로질러 반대편으로 넘어가지 않게
clamp 2: 트랙 벽(d_left / d_right)을 넘지 않게
덕분에 apex는 항상 라인과 같은 쪽, 벽 안쪽에 머뭅니다.
3. d-프로파일 생성 (apex + 복귀점)
Frenet에서 회피 프로파일을 몇 개의 점으로 구성합니다. 장애물 옆 apex 한 점을 두고, 그 뒤 복귀 구간에 걸쳐 $d$ 를 $d_{apex} \rightarrow 0$ 으로 선형 감소시키는 복귀점들을 추가합니다.
1
2
3
4
post_dist = clamp(pre_dist, post_min=1.5, post_max=5.0) # 복귀에 쓸 거리
N = post_dist // sampling_dist + 1 # 복귀점 개수
점 0 : s = obs.s_center, d = d_apex
점 i : s = obs.s_center + post_dist·(i+1)/N, d = d_apex·(1 - (i+1)/N) → 0
이 단계의 출력은 아직 완성된 $(x,y)$ 곡선이 아니라, 라인 기준으로 apex까지 커졌다 0으로 되돌아오는 $(s,d)$ 점 시퀀스입니다.
4. 접선 맞춤 스플라인 (BPoly.from_derivatives)
$(s,d)$ 점들을 FrenetConverter로 $(x,y)$ 로 변환한 뒤, 점 리스트 맨 앞에 현재 차량 위치를, 접선 리스트 맨 앞에 현재 차량 heading을 추가합니다. 회피점들의 접선으로는 각 지점에서의 최적 레이스 라인 heading psi_rad을 사용합니다.
1
2
3
points = [ (cur_x, cur_y), apex, post_1, ... ]
tangents = [ cur_yaw, psi(apex), psi(post_1), ... ] × spline_scale
samples = BPoly.from_derivatives(누적거리, points⊕tangents) # x, y 각각
곡선은 현재 차량이 향한 방향에서 출발해, apex와 복귀점에서는 라인과 나란한 방향을 이루도록 이어집니다. spline_scale(기본 0.8)은 접선의 크기, 즉 각 점에서 그 방향을 얼마나 길게 유지할지를 조절하며, 값이 클수록 곡선의 회전 반경이 커집니다.
헤딩을 거의 반영하지 않고 위치에 의존해 점을 직선에 가깝게 잇는 모습 (spline_scale = 0.5)
헤딩을 적당히 유지해 apex와 수렴점까지 부드럽게 잇는 모습 (spline_scale = 1.0)
apex와 수렴점 부근의 헤딩을 과하게 유지해 불필요하게 크게 돌아가는 모습 (spline_scale = 2.0)
5. 스무딩과 최적 라인 합류
스플라인 결과를 한 번 더 다듬습니다. Savitzky-Golay 필터로 곡선을 펴되 양 끝점(차량의 현재 자세, 마지막 복귀점)은 고정해 필터 경계에서 끝이 꺾이는 것을 막고, 꼬리는 이차 가중치로 점진적으로 global line에 합류시킵니다. 합류 구간에서 global line의 비중을 이차 곡선(quad-ease)으로 키워, 회피선이 최적 라인에 완만하게 수렴합니다. 복귀점 뒤에는 global waypoint 100개를 그대로 이어 붙여, 회피가 끝난 뒤에도 경로가 최적 라인을 따라 이어지게 합니다. 추종 제어가 회피선과 라인의 이음매에서 급격히 방향이 바뀌지 않도록 하는 단계입니다.
6. 트랙 안전 검증과 폐기
만든 경로가 실제로 주행 가능한 선인지 마지막에 검증합니다. 모든 샘플 점을 맵 필터(is_point_inside)로 검사해, 하나라도 트랙 밖이면 회피 경로 전체를 폐기합니다(danger_flag → 빈 배열 반환). 런타임에 매 프레임 수행되는 실시간 안전장치입니다.
회피선이 생성되지 않는다면 대개 이 단계에서 폐기된 경우입니다. 트랙 폭이 좁거나
evasion_dist가 커서 apex가 벽에 지나치게 가까워진 경우가 흔하므로,/planner/avoidance/spline_samples마커로 어느 점이 트랙 밖으로 벗어났는지 확인하면 원인을 좁힐 수 있습니다.
생성된 샘플 점들이 맵 필터로 걸러지는 상황 (초록: 통과 / 빨강: 실패)
Extension: 같은 엔진, 다른 목표점
지금까지의 골격, 즉 Frenet에서 목표점을 정하고, 접선 맞춤 스플라인으로 부드럽게 연결한 뒤, 트랙 검사로 걸러내는 흐름은 정적 회피에만 쓰이는 것이 아닙니다. 2~3단계에서 목표점을 무엇으로 잡느냐만 바꾸면 다른 planner가 됩니다.
| 목표점 | 노드 |
|---|---|
| 옆 차선의 d값 | change_avoidance_node |
| 라인 복귀 지점 | recovery_spliner_node |
| 사람이 클릭한 pose | start_spline_node_v2 |
마무리
이 글에서는 최적 라인을 따라가다 정적 장애물을 만났을 때, Frenet 좌표계에서 apex와 복귀점 몇 개로 회피 프로파일을 정의하고, 접선 맞춤 스플라인(BPoly)으로 매끄러운 $(x,y)$ 곡선을 만든 뒤, 트랙 경계 검사로 안전을 보장하는 static_avoidance_node의 동작을 단계별로 살펴봤습니다. 회피 방향·깊이(evasion_dist)와 곡선 형태(spline_scale)가 핵심 튜닝 포인트이며, 같은 골격에서 목표점만 바꾸면 차선 변경·복귀·수동 시작 등 다른 planner로 확장됩니다.