Devin.KR
통합 검색 Ctrl K

경로 추종 제어 - Pure Pursuit·Stanley

개발자KR 조회 2

이 장에서 배우는 것

앞 장에서 만든 경로와 속도는 로봇이 따라갈 기준이다. 그러나 기준을 준비했다고 해서 실제 위치가 그 위에 머무는 것은 아니다. 창고 로봇은 출발할 때부터 통로 중심에서 조금 벗어나 있을 수 있고, 같은 속도 명령을 주어도 바닥과 적재 상태에 따라 이동 방향이 달라진다. 경로 추종 제어는 현재 자세와 기준 경로를 비교하여 이러한 차이를 줄이는 속도 명령을 만든다.

이 장에서는 퓨어 퍼슈트(Pure Pursuit)와 스탠리(Stanley)를 같은 차동 구동 로봇 모델에 연결한다. 두 방법이 무엇을 오차로 삼는지 먼저 정의하고, 선속도를 고정한 실험으로 조향 특성을 살펴본다. 복잡한 창고 전체를 구성하기보다 긴 직선 통로 하나를 사용하여 부호, 기준점, 제어 주기의 영향을 분리한다.

  • 경로에 대한 횡방향 오차와 방향 오차를 일관된 부호로 정의한다.
  • 전방 주시 거리가 목표점 선택과 회전 명령에 미치는 영향을 설명한다.
  • 자동차 형태의 스탠리 조향 명령을 차동 구동의 각속도 명령으로 바꾼다.
  • 같은 초기 상태와 제한 조건에서 두 제어기를 실행하고 결과의 의미를 구분한다.

문제 상황

물건을 집어 든 로봇이 선반 앞에서 주행 통로로 진입했다고 하자. 통로 중심은 지도에서 x축과 일치한다. 로봇의 중심은 시작점보다 오른쪽으로 0.6 m 떨어져 있고, 방향은 통로와 나란하다. 이때 로봇이 목표 지점의 방향만 바라보면 중심선 옆을 그대로 달릴 수 있다. 반대로 중심선에 가장 가까운 점을 향해 급하게 회전하면 앞으로 나아가기보다 옆으로 붙으려는 동작이 커진다.

필요한 것은 앞으로 이동하면서 경로에 합류하는 명령이다. 너무 가까운 목표만 보면 작은 위치 변화에도 명령이 크게 바뀔 수 있다. 너무 먼 목표만 보면 곡선 안쪽을 가로지르거나 중심선으로 돌아오는 데 긴 거리를 쓸 수 있다. 방향이 맞는 것과 위치가 맞는 것은 다른 조건이며, 제어기는 두 조건을 함께 다루어야 한다.

이번 실험에서는 위치와 방향을 정확히 안다고 가정한다. 센서 잡음이나 상태 추정 오차를 넣으면 제어기의 차이와 입력 정보의 차이가 섞이기 때문이다. 충돌 판정과 종점 정지도 제외한다. 길이 20 m의 직선 경로에서 8초 동안만 주행하므로, 선속도 1 m/s인 로봇은 경로 끝에 도달하지 않는다. 이 실험의 목적은 출발 시 어긋난 자세가 줄어드는지 확인하는 데 있다.

오차를 정의하는 기준점과 부호

로봇 상태는 중심 위치 x, y와 방향각 θ로 표현한다. 지도 좌표의 x축은 통로 진행 방향, y축은 진행 방향의 왼쪽이며, 반시계 방향 회전이 양수다. 선속도 v와 각속도 ω를 받는 운동 모델은 ẋ = v cos θ, ẏ = v sin θ, θ̇ = ω다. 차동 구동 로봇에서는 이 명령을 기본서에서 다룬 좌우 바퀴 속도로 변환할 수 있다.

경로 위에서 로봇 기준점에 가장 가까운 점을 q라 하고, 그 지점의 단위 접선 벡터를 t = (tₓ, tᵧ)라 하자. 왼쪽 단위 법선은 n = (−tᵧ, tₓ)다. 기준점 p의 횡방향 오차는 e = n·(p−q)로 정의한다. 따라서 경로 왼쪽에 있으면 양수이고 오른쪽에 있으면 음수다. 이번 초기 상태의 중심 오차는 −0.6 m다.

경로 방향을 ψ라 할 때 방향 오차는 ψ−θ다. 각도는 −π 이상 π 미만으로 감싸서 사용한다. 예를 들어 경로 방향이 −179도이고 로봇 방향이 179도라면 필요한 회전은 양의 2도다. 두 값을 그대로 빼서 −358도로 취급하면 제어 명령의 방향과 크기가 모두 어긋난다.

어느 점에서 오차를 재는지도 수식의 일부다. 이 장의 퓨어 퍼슈트는 로봇 중심을 사용한다. 스탠리는 중심에서 진행 방향으로 a만큼 떨어진 가상 전방점을 사용한다. 두 제어기의 성능을 평가할 때는 다시 로봇 중심의 오차를 공통 기준으로 삼는다. 제어 내부의 기준점과 결과 평가의 기준점을 구분해야 수치 비교가 의미를 갖는다.

좌표와 오차의 부호를 먼저 고정한다
항목정의양수의 의미
방향각 θ지도 x축에서 로봇 전방까지의 각도반시계 방향
횡방향 오차 e왼쪽 법선과 위치 차이의 내적경로 왼쪽에 위치
방향 오차감싼 각도 ψ−θ왼쪽으로 방향 수정 필요
각속도 ω로봇 방향각의 시간 변화율왼쪽으로 회전
경로 오른쪽에서 출발하면 횡방향 오차는 음수이고 왼쪽 회전 명령은 양수다

경로를 여러 선분으로 저장했다면 각 선분에 수직 투영한 후보점을 구한 뒤 가장 가까운 후보를 선택한다. 투영 비율은 0과 1 사이로 제한한다. 선분의 연장선 위에 있는 점을 경로 위의 점으로 오인하지 않기 위해서다. 코드에서는 이 계산으로 최근접점, 접선, 경로 시작부터의 누적 거리를 함께 얻는다.

최근접점 선택은 자기 교차 경로에서 모호해질 수 있다. 같은 위치에 서로 다른 진행 구간이 있기 때문이다. 이런 경로에서는 이전에 추종하던 구간 번호나 누적 진행 거리를 함께 관리해야 한다. 이번 직선 경로에서는 최근접점이 하나로 정해지므로 해당 문제를 실험에 포함하지 않는다.

퓨어 퍼슈트와 전방 주시 거리

퓨어 퍼슈트는 현재 위치에서 앞쪽 목표점까지 이어지는 원호를 생각한다. 먼저 로봇 중심을 경로에 투영하고, 그 지점에서 경로를 따라 전방 주시 거리 ℓ만큼 이동한 점을 목표로 고른다. 목표점까지의 지도 좌표 차이를 로봇 좌표로 회전하면 전방 성분 xᵦ와 왼쪽 성분 yᵦ를 얻는다.

x_b = cos(theta) * dx + sin(theta) * dy
y_b = -sin(theta) * dx + cos(theta) * dy
D2 = x_b * x_b + y_b * y_b
curvature = 2 * y_b / D2
omega = v * curvature

곡률은 단위 이동 거리당 방향 변화량이며 단위는 1/m다. 목표점이 로봇 왼쪽에 있으면 yᵦ가 양수이므로 곡률과 각속도도 양수가 된다. 목표점이 전방 축 위에 있으면 곡률이 0이 되어 직진한다. 이는 현재 방향에 접하면서 목표점을 지나는 원의 기하 관계에서 나온다.

여기서 ℓ과 D를 구분해야 한다. ℓ은 최근접점부터 경로를 따라 잰 거리이고, D는 로봇에서 목표점까지의 직선거리다. 로봇이 경로 밖에 있거나 경로가 굽어 있으면 두 값은 다르다. 이번 코드처럼 누적 거리로 목표점을 선택했다면 분모에는 실제 목표점까지의 거리 제곱 D²를 넣어야 한다. 전방 주시 거리의 제곱을 그대로 넣으면 다른 제어 법칙이 된다.

초기 위치 (0, −0.6), 방향 0에서 ℓ = 1.0 m라면 목표점은 (1.0, 0)이다. 로봇 좌표에서 목표점은 (1.0, 0.6)이므로 D² = 1.36이고, 곡률은 1.2/1.36이다. 선속도가 1 m/s일 때 초기 각속도는 약 0.882353 rad/s다. 아래 완성 코드는 이 값을 직접 계산한다.

전방 주시 거리를 늘리면 멀리 있는 경로 형태가 명령에 반영된다. 같은 직선 초기 조건에서는 ℓ = 0.6 m일 때 제한 전 각속도가 약 1.666667 rad/s이고, ℓ = 1.8 m일 때는 약 0.333333 rad/s다. 가까운 목표를 선택할수록 이번 조건의 초기 복귀 명령이 커진다. 다만 각속도 제한이 있으면 명령 차이 일부가 잘려 실제 움직임에서는 차이가 작아질 수 있다.

일반적인 경로에서는 짧은 주시 거리가 세밀한 방향 변화에 빠르게 반응하는 대신 샘플 간 굴곡과 위치 잡음에도 민감해질 수 있다. 긴 주시 거리는 회전을 완만하게 만들지만 곡선 안쪽을 통과하는 편차를 키울 수 있다. 속도에 따라 ℓ을 늘리는 방법도 가능하지만, 이번 비교에서는 효과를 분리하기 위해 1.0 m로 고정한다.

경로를 따라 잰 전방 주시 거리와 로봇에서 목표점까지의 직선거리는 서로 다르다

스탠리의 두 오차와 차동 구동 적용

스탠리는 경로 방향에 맞추는 항과 횡방향 위치를 수정하는 항을 더한다. 자동차 형태의 기하를 사용하면 전방 기준점의 오차 e를 바탕으로 조향각 δ를 계산한다. 이 장의 부호 정의에서는 경로 왼쪽에 있을 때 오른쪽으로 돌아와야 하므로 횡방향 보정 항에 음수가 붙는다.

heading_error = wrap(path_heading - theta)
delta = heading_error - atan2(k * e, v + v_soft)
omega = v / a * tan(delta)

k는 횡방향 오차에 대한 이득이며 단위는 1/s다. v_soft는 저속에서 위치 보정 항이 급격히 커지는 것을 완화하는 양의 속도 상수다. 길이 단위의 e에 k를 곱하면 속도가 되므로, 역탄젠트 안의 비율은 무차원이다. 길이 a는 가상 축간 거리이며, 가상 전방점을 만드는 거리와 각속도 변환에 쓰는 거리를 같게 둔다.

차동 구동 로봇에는 조향 바퀴가 없다. 여기서는 로봇 중심을 가상 후방 기준점으로 보고, 그 앞에 p_f = (x + a cos θ, y + a sin θ)를 둔다. 스탠리가 계산한 조향각을 가상 자전거 모델의 관계식 ω = v tan δ/a로 변환한다. 이렇게 얻은 각속도를 중심 운동 모델에 넣는다. 이는 가상 기하를 이용한 적용 방식이며, 실제 조향 장치의 동역학을 모사하는 것은 아니다.

이번 값은 a = 0.4 m, k = 0.8/s, v_soft = 0.2 m/s다. 초기 방향이 0이므로 가상 전방점의 y좌표도 −0.6 m다. 방향 오차는 0이고, 조향각은 atan(0.48/1.2) = atan(0.4)가 된다. 따라서 초기 각속도는 1/0.4 × 0.4 = 1.0 rad/s다. 퓨어 퍼슈트보다 초기 명령이 크지만, 이 한 값으로 전체 추종 성능의 우열을 정할 수는 없다.

조향각은 ±0.6 rad로 먼저 제한하고, 변환한 각속도는 두 방법 모두 ±1.2 rad/s로 제한한다. 조향각 제한은 탄젠트가 큰 값을 내는 구간을 피하기 위한 것이고, 공통 각속도 제한은 로봇의 회전 명령 조건을 맞추기 위한 것이다. 이 예제는 전진 주행만 다루며, 정지 상태에서 방향만 맞추는 동작이나 후진 제어는 별도 동작 규칙이 필요하다.

완성 코드

다음 프로그램을 tracking.py로 저장한다. 외부 의존성은 numpy 하나다. 무작위 값을 사용하지 않으므로 시드에 영향을 받는 계산이 없다. 두 제어기는 같은 초기 상태에서 각각 400번 갱신하며, 매번 현재 자세로 명령을 계산한 다음 시간 간격 0.02초만큼 이동한다. 경로 자료구조는 여러 선분을 받을 수 있지만 검증 경로는 직선으로 고정한다.

import math
import numpy as np


DT = 0.02
STEPS = 400
SPEED = 1.0
OMEGA_MAX = 1.2
LOOKAHEAD = 1.0
AXLE = 0.4
GAIN = 0.8
SOFT_SPEED = 0.2
DELTA_MAX = 0.6


def wrap(angle):
    return (angle + math.pi) % (2.0 * math.pi) - math.pi


class Path:
    def __init__(self, points):
        self.points = np.asarray(points, dtype=float)
        if (self.points.ndim != 2
                or self.points.shape[1] != 2
                or len(self.points) < 2):
            raise ValueError("points must have shape (N, 2), N >= 2")
        if not np.isfinite(self.points).all():
            raise ValueError("points must be finite")

        self.starts = self.points[:-1]
        self.vectors = np.diff(self.points, axis=0)
        self.lengths = np.linalg.norm(self.vectors, axis=1)
        if np.any(self.lengths <= 1e-12):
            raise ValueError("path contains a zero-length segment")
        self.tangents = self.vectors / self.lengths[:, None]
        self.distance = np.concatenate(([0.0], np.cumsum(self.lengths)))

    def project(self, point):
        offset = np.asarray(point, dtype=float) - self.starts
        ratio = np.sum(offset * self.vectors, axis=1)
        ratio = np.clip(ratio / self.lengths**2, 0.0, 1.0)
        candidates = self.starts + ratio[:, None] * self.vectors
        squared = np.sum((candidates - point)**2, axis=1)
        index = int(np.argmin(squared))
        q = candidates[index]
        tangent = self.tangents[index]
        s = self.distance[index] + ratio[index] * self.lengths[index]
        error = (tangent[0] * (point[1] - q[1])
                 - tangent[1] * (point[0] - q[0]))
        heading = math.atan2(tangent[1], tangent[0])
        return float(s), float(error), heading

    def at(self, s):
        s = float(np.clip(s, 0.0, self.distance[-1]))
        index = int(np.searchsorted(self.distance, s, side="right") - 1)
        index = min(max(index, 0), len(self.lengths) - 1)
        return (self.starts[index]
                + (s - self.distance[index]) * self.tangents[index])


def pure_pursuit(state, path):
    x, y, theta = state
    s, _, _ = path.project(state[:2])
    target = path.at(s + LOOKAHEAD)
    dx, dy = target - np.array([x, y])
    lateral = -math.sin(theta) * dx + math.cos(theta) * dy
    distance2 = dx * dx + dy * dy
    if distance2 <= 1e-12:
        return 0.0
    return float(2.0 * SPEED * lateral / distance2)


def stanley(state, path):
    x, y, theta = state
    front = np.array([
        x + AXLE * math.cos(theta),
        y + AXLE * math.sin(theta),
    ])
    _, error, heading = path.project(front)
    delta = wrap(heading - theta)
    delta -= math.atan2(GAIN * error, SPEED + SOFT_SPEED)
    delta = float(np.clip(delta, -DELTA_MAX, DELTA_MAX))
    return SPEED / AXLE * math.tan(delta)


def command(controller, state, path):
    raw = controller(state, path)
    if not math.isfinite(raw):
        raise RuntimeError("non-finite command")
    return float(np.clip(raw, -OMEGA_MAX, OMEGA_MAX))


def step(state, omega):
    x, y, theta = state
    return np.array([
        x + SPEED * math.cos(theta) * DT,
        y + SPEED * math.sin(theta) * DT,
        wrap(theta + omega * DT),
    ])


def simulate(controller, path):
    state = np.array([0.0, -0.6, 0.0])
    states = [state.copy()]
    commands = []
    for _ in range(STEPS):
        omega = command(controller, state, path)
        state = step(state, omega)
        if not np.isfinite(state).all():
            raise RuntimeError("non-finite state")
        states.append(state.copy())
        commands.append(omega)
    return np.array(states), np.array(commands)


def report(name, states, commands, path):
    errors = np.array([path.project(s[:2])[1] for s in states])
    _, _, heading = path.project(states[-1, :2])
    heading_error = abs(wrap(heading - states[-1, 2]))
    if abs(errors[-1]) >= 0.03 or heading_error >= 0.03:
        raise RuntimeError(name + ": final tolerance not met")

    metrics = {
        "rms_error_m": float(np.sqrt(np.mean(errors**2))),
        "final_error_m": float(abs(errors[-1])),
        "final_heading_rad": float(heading_error),
        "peak_omega_rad_s": float(np.max(np.abs(commands))),
    }
    print(f"{name}: initial omega={commands[0]:.6f} rad/s")
    print(f"{name}: steps={len(commands)}, "
          "final |e|<0.03 m, |heading|<0.03 rad")
    return metrics


def main():
    path = Path([[0.0, 0.0], [20.0, 0.0]])
    results = {}
    for name, controller in (
        ("Pure Pursuit", pure_pursuit),
        ("Stanley", stanley),
    ):
        states, commands = simulate(controller, path)
        results[name] = report(name, states, commands, path)
    print(f"dt={DT:.2f} s, duration={STEPS * DT:.2f} s")
    print("checks=PASS")
    return results


if __name__ == "__main__":
    main()

줄별 해설

DT부터 DELTA_MAX까지는 실험 조건이다. 시간 간격과 선속도, 공통 각속도 제한은 두 제어기에 동일하게 적용한다. LOOKAHEAD는 퓨어 퍼슈트에서만 사용하고, AXLE, GAIN, SOFT_SPEED, DELTA_MAX는 스탠리에서 사용한다. 제어기마다 내부 매개변수의 의미가 다르므로 숫자의 크기를 같게 맞추는 것은 공정한 비교 조건이 아니다.

wrap()은 방향 차이가 ±π 경계를 넘을 때도 가까운 회전 방향을 선택하게 한다. 정확히 π인 차이는 −π로 표현한다. 어느 쪽이든 반 바퀴라는 기하적 의미는 같지만, 코드 전체에서 같은 규칙을 유지해야 경계에서의 동작을 설명할 수 있다.

Path.__init__()은 점 배열의 모양과 유한성을 검사하고, 인접 점 사이의 벡터와 길이를 저장한다. 길이가 0인 선분을 거부하는 이유는 단위 접선을 만들 때 나눗셈이 필요하기 때문이다. lengths[:, None]은 각 선분 길이를 해당 선분의 x, y 성분 모두에 적용하도록 배열 차원을 맞춘다.

project()의 ratio는 각 선분 시작점에서 투영점까지의 상대 위치다. 0이면 시작점이고 1이면 끝점이다. 후보점까지의 거리 제곱을 비교하므로 최근접 후보 선택에 제곱근은 필요 없다. 반환하는 s는 경로 시작부터 후보점까지의 거리이며, error는 왼쪽 법선 방향의 부호 있는 오차다.

at()은 누적 거리 배열에서 목표 거리가 속한 선분을 찾는다. 목표 거리가 경로 길이를 넘어가면 끝점으로 제한한다. 이는 좌표 접근을 유효하게 만드는 처리일 뿐, 로봇을 정지시키는 기능은 아니다. 종점까지 주행시키려면 남은 거리와 목표 속도에 맞추어 별도의 정지 조건을 연결해야 한다.

pure_pursuit()는 중심 투영점의 누적 거리에 LOOKAHEAD를 더한다. 회전 변환에서는 곡률 계산에 필요한 왼쪽 성분만 구한다. 회전해도 거리 제곱은 보존되므로 분모는 지도 좌표 차이로 계산한다. 목표점과 현재 위치가 거의 일치하면 0을 반환하지만, 이것도 종점 정지 정책을 대신하지 않는다.

stanley()는 중심 앞에 가상 전방점을 만든 뒤 그 점을 투영한다. 방향 오차를 먼저 감싸고 위치 보정 항을 뺀다. 최종 조향각은 감싸지 않고 제한한다. 조향각은 로봇의 전역 방향이 아니라 가상 바퀴의 상대 각도이기 때문이다. 이후 탄젠트 관계로 각속도를 얻는다.

command()는 각 제어기가 만든 각속도의 유한성을 확인하고 공통 제한을 적용한다. step()은 갱신 전 방향각을 사용하여 위치를 적분하는 전진 오일러 방법이다. 시간 간격에 따른 근사 오차가 있으므로, 정밀한 성능 수치를 비교할 때는 시간 간격을 줄여 결론이 유지되는지 확인할 필요가 있다.

simulate()는 호출할 때마다 초기 상태를 새로 만든다. 첫 번째 제어기의 최종 상태가 두 번째 제어기의 시작점으로 넘어가지 않는다. 상태 기록은 초기 상태를 포함하여 401개이고 명령 기록은 400개다. copy()로 기록 시점의 값을 보관하여 배열 수정 방식이 바뀌어도 과거 기록이 함께 바뀌지 않게 한다.

report()는 두 방법 모두 중심에서 오차를 다시 잰다. 제곱평균제곱근 오차는 초기 상태와 마지막 상태를 포함한 401개 표본으로 계산한다. 마지막 오차만 작더라도 중간에 크게 흔들렸을 수 있으므로 전체 구간 지표를 함께 남긴다. 다만 출력은 초기 명령과 최종 허용 범위 통과 여부로 제한하고, 세부 수치는 반환 사전에 보관한다.

실행 결과

numpy가 설치된 환경에서 다음 명령을 사용한다. 첫 번째 명령은 경고를 오류로 취급하여 문법 컴파일을 수행한다. 정상적으로 완료되면 출력이 없다. 두 번째 명령은 프로그램을 실행한다. 아래는 코드와 수식에 따른 예상 출력이며, 여기서 실행 측정한 기록은 아니다.

python3 -W error -m py_compile tracking.py
python3 -W error tracking.py
Pure Pursuit: initial omega=0.882353 rad/s
Pure Pursuit: steps=400, final |e|<0.03 m, |heading|<0.03 rad
Stanley: initial omega=1.000000 rad/s
Stanley: steps=400, final |e|<0.03 m, |heading|<0.03 rad
dt=0.02 s, duration=8.00 s
checks=PASS

두 초기 명령은 모두 양수다. 경로 오른쪽에서 출발한 로봇이 왼쪽으로 돌아와야 한다는 부호 점검과 일치한다. 최종 검사는 횡방향 오차 0.03 m 미만과 방향 오차 0.03 rad 미만을 동시에 요구한다. 어느 조건이든 만족하지 못하면 통과 메시지 대신 예외가 발생한다.

이 출력은 직선 합류 실험에서 두 방법이 설정한 허용 범위에 들어온다는 뜻이다. 같은 오차로 끝났다는 뜻도, 모든 경로에서 같은 성능이라는 뜻도 아니다. 소수점 끝자리의 차이에 기대지 않도록 최종 결과는 범위로 표시했다. 수치 분석이 필요하면 main()의 반환값에서 전체 구간 오차와 최대 각속도를 확인할 수 있다.

비교를 확장할 때는 초기 위치, 속도, 시간 간격, 각속도 제한을 유지하고 한 매개변수씩 바꾼다. 최종 오차뿐 아니라 오차가 허용 범위 안에 계속 머무르기 시작한 시간, 경로 반대쪽으로 넘어간 크기, 명령 변화량도 함께 볼 수 있다. 특히 최대 오차만 비교하면 이번 실험에서는 출발 오차 0.6 m가 결과를 지배하여 차이를 잘 드러내지 못할 수 있다.

실무에서 자주 틀리는 것

횡방향 오차의 부호와 보정 방향을 섞는다

왼쪽을 양의 오차로 정의했다면 양의 오차에서는 오른쪽 보정이 필요하다. 다른 좌표 정의에서 사용한 더하기 부호를 그대로 가져오면 경로에서 멀어질 수 있다. 로봇을 경로의 양쪽에 각각 놓고 첫 명령의 방향부터 확인한다.

틀린 코드는 다음과 같다.

delta = wrap(heading - theta)
delta += math.atan2(GAIN * error, SPEED + SOFT_SPEED)

이 장의 좌표 정의에 맞춘 코드는 다음과 같다.

delta = wrap(heading - theta)
delta -= math.atan2(GAIN * error, SPEED + SOFT_SPEED)

경로 위 거리와 목표점까지의 거리를 같게 취급한다

목표점을 경로 누적 거리로 선택하면서 곡률 분모에 주시 거리만 넣으면, 횡방향으로 멀리 떨어진 경우 회전 명령을 과하게 계산할 수 있다. 목표점 선택 규칙과 곡률 계산의 거리 정의를 함께 확인한다.

틀린 코드는 다음과 같다.

omega = 2.0 * SPEED * lateral / LOOKAHEAD**2

고친 코드는 실제 두 점 사이의 거리를 사용한다.

distance2 = dx * dx + dy * dy
omega = 0.0
if distance2 > 1e-12:
    omega = 2.0 * SPEED * lateral / distance2

조향각을 각속도처럼 전달한다

조향각의 단위는 rad이고 각속도의 단위는 rad/s다. 두 값은 모두 각도와 관련되지만 운동 모델에 미치는 의미가 다르다. 가상 축간 거리와 선속도를 포함한 변환이 빠지면 속도를 바꾸었을 때 동작 해석이 어려워진다.

틀린 코드는 다음과 같다.

omega = delta

고친 코드는 조향각을 제한한 뒤 각속도로 변환한다.

delta = float(np.clip(delta, -DELTA_MAX, DELTA_MAX))
omega = SPEED / AXLE * math.tan(delta)
omega = float(np.clip(omega, -OMEGA_MAX, OMEGA_MAX))

제어기마다 다른 점에서 평가한 오차를 비교한다

스탠리 내부의 전방점 오차와 퓨어 퍼슈트의 중심 오차를 비교하면 기준점 차이가 성능 차이에 섞인다. 회전 중에는 중심이 경로 아래에 있어도 전방점이 경로 위에 있을 수 있다. 평가는 로봇 중심처럼 공통으로 정한 점에서 수행한다.

틀린 비교는 서로 다른 기준점을 사용한다.

pp_error = path.project(pp_state[:2])[1]
st_error = path.project(st_front)[1]

고친 비교는 두 상태 모두 중심 위치를 사용한다.

pp_error = path.project(pp_state[:2])[1]
st_error = path.project(st_state[:2])[1]

한눈에 보기

두 제어기의 판단 근거와 조정 대상을 비교한다
비교 항목퓨어 퍼슈트스탠리
핵심 판단앞쪽 목표점을 향하는 원호방향 오차와 횡방향 오차의 합성
제어 기준점로봇 중심가상 전방점
주요 조정값전방 주시 거리횡방향 이득과 저속 완화 상수
차동 구동 연결곡률에 선속도를 곱함가상 조향각을 각속도로 변환함
짧은 거리·큰 이득목표 변화에 민감해질 수 있음위치 복귀 명령이 커질 수 있음
공통 확인 사항부호, 목표점 선택, 각속도 제한부호, 기준점, 각속도 제한

두 방법 모두 현재 경로와 자세로부터 바로 명령을 계산한다. 이 장에서는 고정 속도와 회전 제한 아래에서 그 명령이 어떤 의미를 갖는지 확인했다. 다음 장에서는 앞으로의 움직임을 예측하고 제약 조건을 함께 고려하여 명령을 선택하는 관점으로 이어간다.

연습 문제

  1. 초기 상태를 (0, 0.6, 0)으로 바꾸면 두 제어기의 첫 각속도는 어떻게 달라지는가. 계산값과 회전 방향을 설명하라.
  2. 퓨어 퍼슈트의 전방 주시 거리를 0.6 m와 1.8 m로 바꾸어 초기 각속도를 구하라. 제한 전 값과 실제 적용값을 구분하라.
  3. 로봇 중심이 (0, −0.2)에 있고 방향각이 30도일 때 a = 0.4 m인 가상 전방점의 직선 경로 횡방향 오차를 구하라. 전방점 오차가 0이면 스탠리 명령도 0인지 설명하라.
  4. 완성 코드의 결과에 전체 구간 오차 제곱평균제곱근을 출력하도록 수정하라. 이 지표를 최종 오차와 함께 보는 이유와, 초기 오차가 포함되는 표본 수를 설명하라.

정답과 해설

  1. 퓨어 퍼슈트의 목표점 왼쪽 성분이 −0.6으로 바뀌므로 첫 각속도는 −0.882353 rad/s다. 스탠리의 전방점 오차는 0.6 m이고 방향 오차는 0이므로 첫 각속도는 −1.000000 rad/s다. 두 값 모두 공통 제한 안에 있으며, 로봇이 경로 왼쪽에서 오른쪽으로 복귀하는 방향이다. 직선 경로와 좌우 대칭인 초기 조건에서는 부호가 반대로 바뀌는지 확인하는 것만으로도 여러 구현 오류를 찾을 수 있다.

  2. 초기 상태를 원래대로 두면 제한 전 각속도는 1.2/(ℓ²+0.36)이다. ℓ = 0.6에서는 약 1.666667 rad/s이며 실제 적용값은 제한에 걸린 1.2 rad/s다. ℓ = 1.8에서는 약 0.333333 rad/s이고 제한에 걸리지 않는다. 주시 거리를 더 줄이더라도 초기 적용 명령은 제한값보다 커지지 않는다.

  3. 가상 전방점의 y좌표는 −0.2 + 0.4 sin 30도 = 0이다. 따라서 전방점 횡방향 오차는 0이다. 그러나 경로 방향은 0이고 로봇 방향은 π/6이므로 방향 오차는 −π/6이다. 위치 보정 항이 0이어도 오른쪽 회전 명령이 나온다. 제한 전 각속도는 약 −1.443376 rad/s이고 실제 적용값은 −1.2 rad/s다.

  4. report()의 metrics 생성 뒤에 다음 줄을 추가하면 된다. 출력이 한 줄씩 늘어나므로 앞의 예상 출력도 그에 맞게 달라진다.

    print(f"{name}: RMS={metrics['rms_error_m']:.6f} m")

    최종 오차는 마지막 순간의 상태만 나타낸다. 전체 구간 지표는 합류가 늦거나 중간에 오차가 커진 동작도 반영한다. 이 코드에서는 초기 상태 1개와 갱신 후 상태 400개를 합친 401개 표본을 사용한다. 서로 다른 실험을 비교할 때는 평가 시간과 초기 상태 포함 여부도 같게 맞춘다.

댓글 0

아직 댓글이 없습니다. 첫 댓글을 남겨 보세요.

댓글을 남기려면 로그인이 필요합니다.