제7부 실전: ROS2 Humble 프로젝트

7.3 실전 ② 이동로봇 경로 추종

제7부 실전: ROS2 Humble 프로젝트 · 두 번째 프로젝트. 1차원 속도 제어(7.2)에서 2차원 주행으로. 차동구동(differential drive) 이동로봇이 주어진 경로를 따라가게 한다 — Pure Pursuit로 조향, PID로 속도. 차동구동 운동학, 전방주시 추종, Gazebo 통합.

✅ 검증됨: Pure Pursuit + 차동구동 운동학 시뮬레이션을 ros:humble 컨테이너에서 실행해 검증(verify/ce32_pursuit.py) — 사인 경로 추종, 최대 교차추적오차 0.155 m, 목표 도달. Gazebo GUI 부분만 독자 환경 몫.

이 장을 마치면 차동구동 운동학을 세우고, Pure Pursuit로 경로를 추종하며, 전방주시 거리 튜닝과 흔한 함정(진행 인덱스)을 이해해야 한다.

7.3.1 차동구동 운동학

두 바퀴를 독립 구동하는 로봇. 선속도 $v$와 각속도 $\omega$로 평면을 움직인다:

• $\dot{x} = v\cdot\cos\theta$ ($x$: 전역 위치)
• $\dot{y} = v\cdot\sin\theta$ ($y$)
• $\dot{\theta} = \omega$ ($\theta$: 방위)

$v, \omega$는 좌·우 바퀴 속도로 변환된다(역운동학, 바퀴 간격 $L$):

$v$좌 $= v - \omega\cdot L/2$, $v$우 $= v + \omega\cdot L/2$

ROS2에서는 geometry_msgs/Twist(linear.x=v, angular.z=ω)를 /cmd_vel로 보내면, diff_drive_controller(7.1.5)나 시뮬레이션이 바퀴 속도로 풀어 준다. 즉 우리는 $(v, \omega)$만 정하면 된다.

주의: 이 운동학은 기구학만 본다 — 미끄러짐·관성·바퀴 동특성(1.1.7)을 무시. 저속에서는 충분하나, 고속·고가속에는 동역학이 필요(7.3.7).

7.3.2 경로 추종 문제

목표: 점들의 열(waypoints)로 주어진 경로를 따라 끝까지 주행. 이 장은 경로가 이미 주어졌다고 가정하고 추종만 다룬다 — 그 경로(웨이포인트·속도 프로파일) 자체를 어떻게 생성하는지는 7.8(궤적·모션 프로파일 생성) 참고. 두 일을 나눈다:

• 조향(steering): 경로를 향하도록 $\omega$를 정한다 → Pure Pursuit(7.3.3).
• 속도(speed): $v$를 정한다 → 보통 일정하게, 또는 곡률·목표 거리에 따라 PID로 조절(7.2의 속도 루프).

이 책은 조향에 집중한다(속도는 기본적으로 일정 $v$로 둔다). 다만 최소한의 속도 법칙은 아래 7.3.2.1에 두고, 곡선에서 줄이는 것을 본격 다루는 PID 튜닝·안정성은 7.2과 연습으로 남긴다.

7.3.2.1 곡률 기반 속도 법칙 (간단한 처리)

완전한 속도 최적화는 미루더라도, 조향과 엮이는 최소한의 규칙은 둘 수 있다:

$$v_{\mathrm{set}} = v_{\max} / (1 + k\cdot|\kappa|)$$ — $\kappa$: 7.3.3의 Pure Pursuit 곡률, $k$: 감속 계수 $\ge 0$

• 곡률 $\kappa$가 크면(급커브) v_set이 줄고, 직선($\kappa\approx0$)이면 v_set → v_max로 회복된다.
• 실제 바퀴 속도가 v_set을 즉시 따르지는 않는다 — 모터·바퀴에는 관성·지연이 있다. PID(7.2)가 v_set을 목표값(setpoint)으로 삼아 실제 속도를 추종한다 — 7.2의 속도 루프를 그대로 가져다 쓰는 것뿐, 새 이론이 필요한 것은 아니다. 즉 조향 루프가 매 스텝 v_set을 계산해 7.2의 PID 노드에 넘기면 된다.
• 7.3.6에서 언급하는 $L_d = k\cdot v + L_{d,\mathrm{min}}$(속도비례 전방주시)과 묶으면 효과가 커진다: 곡선에서 $v$가 줄면 $L_d$도 같이 줄어 코너 컷이 준다 — 감속이 조향 정확도까지 개선하는 경로다. 검증 결과는 7.3.6.1.

7.3.3 Pure Pursuit — 전방주시점을 추종한다

직관: 운전자가 저 앞 한 점을 보고 그쪽으로 핸들을 꺾듯, 로봇도 경로상 전방주시점(lookahead point)을 향하는 원호를 따라간다.

절차(매 스텝):

1. 진행 인덱스 갱신: 현재 위치에 가장 가까운 경로점을 찾되 뒤로 가지 않게(단조 증가, 7.3.5 함정!).
2. 전방주시점: 그 점부터 앞으로 거리 $L_d$ 이상 떨어진 첫 경로점 (t_x, t_y).
3. 로봇 좌표계로 변환(방위 $\theta$ 회전):
   • $y_r = -\sin\theta\cdot(t_x-x) + \cos\theta\cdot(t_y-y)$ (로봇 기준 좌/우 오프셋)
4. 곡률·각속도: κ = 2·y_r / Ld², $\omega= v\cdot\kappa$. (전방주시점을 지나는 원호의 곡률.)

y_r>0(목표가 왼쪽)이면 $\omega>0$(좌회전) — 0.1의 "오차→보정"의 기하 버전이다.

숫자로 확인(위 그림의 값): 전방주시 거리 $L_d=2.0 \,\mathrm{m}$, 전방주시점이 로봇 기준 왼쪽으로 y_r=0.8 m 떨어져 있다면 —

• $\kappa = 2\cdot y_r / L_d^2 = 2\cdot 0.8 / 2.0^2 = 0.4$ (곡률, 1/m)
• $R = 1/\kappa = L_d^2 / (2\cdot y_r) = 2.5\,\mathrm{m}$ (회전 반지름)
• $x_r = \sqrt{L_d^2 - y_r^2} = \sqrt{4 - 0.64} \approx 1.833\,\mathrm{m}$ (목표점의 전방 거리)

곡률중심 $C=(0, R)=(0, 2.5)$를 중심으로 반지름 $R=2.5 \,\mathrm{m}$인 원호를 그리면 로봇(원점, $+x$ 방향)에서 목표점 $G\approx(1.833, 0.8)$을 정확히 지난다. $v=1 \,\mathrm{m/s}$라면 $\omega=v\cdot\kappa=0.4 \,\mathrm{rad/s}$로 좌회전하며 이 원호를 따라간다 — $L_d$가 작아지거나 y_r이 커질수록 $\kappa$가 커져(회전이 급해져) $R$이 줄어드는 관계를 그림에서 바로 읽을 수 있다.

7.3.4 코드 — Pure Pursuit 조향 (검증됨)

import numpy as np
def pure_pursuit_step(x, y, th, path, prog, Ld, v):
    # 1) 진행 인덱스를 현재 위치 최근접점까지 전진 (뒤로 안 감)
    seg = path[prog:]
    prog += int(np.argmin(np.hypot(seg[:,0]-x, seg[:,1]-y)))
    # 2) 앞으로 Ld 이상 떨어진 첫 점 = 전방주시점
    idx = prog
    while idx < len(path)-1 and np.hypot(path[idx,0]-x, path[idx,1]-y) < Ld:
        idx += 1
    tx, ty = path[idx]
    # 3) 로봇 좌표계 오프셋 → 4) 곡률·각속도
    y_r = -np.sin(th)*(tx-x) + np.cos(th)*(ty-y)
    w = v * (2.0*y_r/(Ld*Ld))
    return v, w, prog          # /cmd_vel: linear.x=v, angular.z=w

이 $(v, w)$를 /cmd_vel로 발행하고, /odom(7.1.3)에서 $(x,y,\theta)$를 받아 매 스텝 반복하면 ROS2 경로 추종 노드가 된다(구조는 7.2과 동일).

7.3.5 함정 — 진행 인덱스 (검증이 검출한 실제 버그)

⚠️ *전방주시점을 "거리 ≥ Ld인 첫 점"으로 경로 전체에서 고르면 안 된다. 로봇이 전진하면 뒤쪽 경로점도 거리 ≥ Ld라, 배열 순서상 먼저 나오는 지나온 점을 선택해 로봇이 뒤로 돌거나 제자리에서 선회한다.*

이 책을 검증하며 실제로 이 버그가 났다 — 수정 전에는 로봇이 출발점 부근에 머물다 실패(교차추적오차 1.14 m, 목표 미도달). 진행 인덱스를 단조 증가시켜 앞쪽만 탐색하도록 고치자 정상 추종(아래). 추종 알고리즘의 고전적 실수이니 기억하라.

7.3.6 실행 결과 (실측)

직선+사인 경로($y=0.8\cdot\sin(0.8x)$, x:0→6)를 $L_d=0.8, v=0.6$으로 추종한 실제 결과:

도달 = True
최대 교차추적오차 = 0.155 m
종점 = (5.91, −0.82)        (경로 끝 (6, −0.80)에 일치)
→ CE32_PURSUIT_PASS

• 로봇이 사인 곡선을 최대 0.155 m 오차로 따라가 끝점에 도달.
• 전방주시 거리 $L_d$ 튜닝: 작으면 경로에 밀착하지만 진동·불안정(짧게 보면 과민), 크면 부드럽지만 곡선 안쪽을 가로지른다(corner cutting). 속도에 비례($L_d = k\cdot v + L_{d,\mathrm{min}}$)시키는 것이 흔하다 — 2.3·2.4의 "빠를수록 멀리 봐야 안정"과 통한다.
• 직접 $L_d$를 0.3·0.8·2.0으로 바꿔 진동 vs 코너 컷을 관찰하라(연습).

7.3.6.1 곡률 감속의 효과 (간이 시나리오, 애드혹 확인 — 검증 필요)

7.3.2.1의 속도 법칙(v_set=v_max/(1+k|κ|))이 실제로 도움이 되는지, 급커브를 포함한 간이 경로(직선 → 반지름 0.4 m 사분원 → 직선)로 확인했다(ce-verify 컨테이너, PID 속도 추종은 1차 지연으로 근사). 이 스크립트는 verify/에 정식 편입돼 있지 않은 book-gap 보완용 애드혹 확인이다:

등속(v=1.2 m/s 고정, Ld=Ld_min+kLd·v로 계산):              RMS 교차추적오차 = 0.077 m   최대 0.200 m
곡률감속(k=4, v_set 감소 → Ld도 동반 감소)+1차지연 PID 추종: RMS 교차추적오차 = 0.044 m   최대 0.075 m
→ RMS 약 1.75배, 최대오차 약 2.67배 개선

• 개선의 실질 메커니즘은 감속 자체가 아니라, 감속이 $L_d$를 함께 줄여 코너 컷을 줄이는 것이다. $L_d$를 속도와 무관하게 고정한 채 $v$만 줄이는 실험도 실행해 봤는데, 이 경우 개선이 거의 없었고(오히려 PID 추종 지연 때문에 미세하게 악화되기도 했다) — 속도 감속과 전방주시 거리($L_d$) 조정은 묶어야 실효가 있다. (7.3.2.1의 $L_d = k\cdot v + L_{d,\mathrm{min}}$ 결합이 그 이유다.)
• 위 수치는 7.3.6의 검증된 사인 경로가 아니라 급커브를 넣은 별도 간이 경로에서 얻었다 — 완만한 곡선에서는 개선폭이 이보다 작을 것이다. 실물 성능은 로봇 동역학·마찰·PID 튜닝에 따라 달라지므로 위 배수는 정성적 크기감으로 받아들이고, 실전 적용 전에는 대상 로봇·경로로 재검증이 필요하다(검증 필요).

7.3.7 Gazebo로 — 실제 시뮬레이션 (독자 환경)

본격 시뮬레이션은 Gazebo(0.3.7)에서 차동구동 로봇(예: TurtleBot3)을 스폰해 한다.

# (예시 흐름 — GUI 필요, 독자 환경)
ros2 launch turtlebot3_gazebo empty_world.launch.py        # 로봇·물리 시뮬레이션
ros2 run ce_examples path_follower                          # 위 Pure Pursuit 노드

• 로봇은 /odom으로 자세를, 우리 노드는 /cmd_vel로 $(v,\omega)$를 — diff_drive_controller가 바퀴로 변환(7.1.5).
• Gazebo는 물리(관성·마찰·미끄러짐)를 더하므로, 순수 기구학 시뮬레이션(7.3.6)보다 까다롭다 — 속도 루프(7.2)·곡선 감속이 필요해진다.

⚠️ Gazebo는 3D GUI·물리라 headless 컨테이너 자동 검증 밖이다(이 책은 추종 알고리즘을 검증, 물리 시뮬레이션·시각은 독자 환경). Gazebo도 서버만 headless로 돌릴 수 있으나 설정이 복잡하다.

7.3.8 정직 — 기구학·튜닝·함정

• 기구학 모델: 7.3.6은 미끄러짐·관성 없는 이상 차동구동이다. 실물·Gazebo는 동역학으로 다르다(1.1.11) → 속도 제어·감속 필요.
• $L_d$ 튜닝이 성패: 너무 작으면 진동·불안정, 너무 크면 코너 컷. 속도 적응이 실전 표준.
• 진행 인덱스 함정(7.3.5): 추종 알고리즘의 대표적 버그 — 반드시 앞쪽만 탐색.
• Pure Pursuit의 한계: 기하적·반응적이라 최적은 아니다. 제약·최적 추종은 MPC(7.6).

7.3.9 정리 — 그리고 다음 장

• 차동구동은 $(v, \omega)$로 평면을 움직이고(7.3.1), ROS2에서는 /cmd_vel(Twist)로 명령한다.
• Pure Pursuit: 전방주시점을 추종하는 원호의 곡률 $\kappa=2y_r/L_d^2$로 조향(7.3.3). 진행 인덱스를 단조 증가시켜야 뒤쪽 점을 선택하지 않는다(7.3.5, 검증이 검출한 버그).
• 실측: 사인 경로를 최대 0.155 m 오차로 추종·도달. $L_d$가 진동 vs 코너 컷의 조정 변수(7.3.6).
• Gazebo로 물리까지(독자 환경), 한계는 정직하게(7.3.7·7.3.8).

다음 7.4에서는 불안정 플랜트의 정수 — 도립진자 균형 제어를 LQR(4.5)·상태피드백(4.3)으로 — 다룬다. 1.1.9의 "도립=불안정"이 드디어 제어된다.

7.3.10 연습·생각해 볼 것

1. 운동학. $v=0.5, \omega=1.0$을 바퀴 간격 $L=0.3$인 로봇에 주면 좌·우 바퀴 속도는? 로봇은 어떤 궤적을 그리나? (7.3.1)
2. 곡률 직관. Pure Pursuit에서 전방주시점이 정면(y_r=0)이면 $\omega$는? 바로 왼쪽이면 $\kappa$의 부호는? (7.3.3)
3. Ld 튜닝. $L_d$를 0.3과 2.0으로 바꾸면 추종이 각각 어떻게 변할지 예측하고, verify/ce32_pursuit.py로 확인하라(교차추적오차·진동·코너 컷). (7.3.6)
4. 함정 재현. 7.3.5의 진행 인덱스를 없애고 "거리≥Ld인 첫 점"을 전체에서 고르면 왜 실패하나? 직접 오류를 주입해 관찰하라. (7.3.5)
5. 속도 추가. 곡선(큰 $\vert\omega\vert$)에서 $v$를 줄이는 간단한 규칙을 제안하라. 왜 곡선에서 감속이 추종을 돕나? (7.3.2·7.3.6)
6. (손으로) verify/ce32_pursuit.py로 다른 경로(원·8자)를 만들어 추종시켜 보라. 가능하면 Gazebo+TurtleBot3로 물리 시뮬레이션까지. (검증된 스크립트)

막히면 "(v,ω)로 명령, 전방주시점 곡률로 조향, 진행 인덱스는 단조 증가, Ld가 진동 vs 코너 컷"으로 돌아가라.