궤적 생성 - 부드러운 경로와 속도

이 장에서 배우는 것

앞 장에서 얻은 경로는 창고의 출발점과 목적지를 연결하는 위치 목록이다. 이 목록만으로는 로봇이 각 위치를 언제 지나야 하는지 알 수 없다. 경유점마다 방향이 갑자기 바뀌는 경로를 일정한 속도로 따라가게 하면, 바퀴가 짧은 시간에 큰 속도 변화를 요구받기도 한다. 이번에는 경유점을 부드러운 곡선으로 연결하고, 그 위에서 출발과 정지를 포함한 속도 계획을 만든다.

공간에서 지나갈 자취를 경로라고 하고, 시간에 따른 위치와 속도를 포함한 참조를 궤적이라고 구분한다. 이 장의 프로그램은 곡선을 표본으로 나눈 뒤 각 표본의 도착 시각과 속도를 계산한다. 다음 장에서는 이러한 참조를 실제 로봇의 움직임과 비교하는 경로 추종을 다룬다.

• 자연 3차 스플라인 보간으로 경유점을 통과하는 곡선을 구성한다.
• 곡선을 정의하는 매개변수와 실제 이동 거리를 구분한다.
• 전진·후진 계산으로 최고 속도와 접선 방향 가속도 제한을 반영한다.
• 거리와 속도로 도착 시각을 계산하고, 수치 검사로 제한 준수 여부를 확인한다.

문제 상황

창고 로봇이 입고 구역에서 물건을 싣고 포장대로 이동한다고 하자. 경로 계획기는 미터 단위의 경유점 다섯 개를 반환한다. 로봇은 처음에 정지해 있고, 마지막 경유점에서도 정지해야 한다. 운반 중인 상자가 흔들리지 않도록 주행 속도는 1.0m/s 이하, 주행 방향의 속도 변화율 크기는 0.5m/s² 이하로 정한다.

경유점을 직선으로 연결하면 두 선분이 만나는 곳에서 진행 방향이 불연속이다. 그렇다고 경유점 근처에서 무조건 정지하면 주행 시간이 길어진다. 곡선으로 모서리를 완화하되, 정지 상태에서 서서히 가속하고 목적지에 도착하기 전에 감속하는 참조가 필요하다.

이 예제는 경유점 주변에 충분한 여유 공간이 있다고 가정한다. 스플라인이 모든 경유점을 통과하더라도 경유점 사이에서 원래 선분 밖으로 나갈 수 있다. 따라서 실제 창고 지도에 적용할 때는 보간 후의 곡선과 로봇 외곽이 장애물을 침범하지 않는지 다시 확인해야 한다. 부드러움과 충돌 회피는 서로 다른 조건이다.

예제의 입력과 제한이 뜻하는 것

항목	설정	의미
경유점	(0, 0), (2, 0), (4, 1.5), (6, 1.5), (8, 0)	창고 바닥 좌표, 단위 m
최고 속도	1.0m/s	경로를 따라 이동하는 속력의 상한
가속도 한계	0.5m/s²	접선 방향 가속도 크기의 상한
경계 속도	출발 0, 도착 0	양 끝에서 정지

경유점을 잇는 3차 스플라인

스플라인(spline)은 여러 구간의 다항식을 이어 만든 곡선이다. 여기서는 각 구간을 3차 다항식으로 표현하고, 연결점에서 위치와 1차·2차 미분이 이어지도록 한다. 자연 경계 조건을 사용하므로 전체 곡선 양 끝의 2차 미분은 0이다. 이는 끝점에서 속도가 0이라는 뜻이 아니다. 곡선의 모양을 정하는 경계 조건과 시간에 따른 출발·정지 조건은 따로 설정한다.

창고 경로는 되돌아가거나 수직으로 뻗을 수 있으므로 y를 x의 함수로만 표현하기 어렵다. 대신 매개변수 u를 두고 x(u), y(u)를 각각 보간한다. u의 경계값은 경유점 사이 직선거리를 누적해 만든다. 이렇게 하면 멀리 떨어진 경유점 사이에 더 넓은 매개변수 구간을 배정할 수 있다.

u[0] = 0
u[i + 1] = u[i] + ||P[i + 1] - P[i]||

u는 길이 단위를 가지지만, 보간한 곡선을 따라 잰 정확한 이동 거리는 아니다. 직선거리로 정한 u의 간격과 보간 후 곡선의 길이는 일반적으로 다르다. 이 차이를 무시하고 u를 일정하게 증가시키면 실제 이동 속력이 일정하지 않을 수 있다.

계산에는 각 경유점에서의 2차 미분 M을 사용한다. 한 좌표를 q라고 하고 h[i]를 이웃한 u의 차이라고 하면, 내부 경유점의 M은 다음 연립방정식을 만족한다. x와 y는 왼쪽 행렬이 같으므로 오른쪽 항을 두 열로 묶어 한 번에 풀 수 있다.

h[i - 1] M[i - 1] + 2(h[i - 1] + h[i]) M[i]
+ h[i] M[i + 1]
= 6((q[i + 1] - q[i]) / h[i]
    - (q[i] - q[i - 1]) / h[i - 1])

M[0] = 0
M[n - 1] = 0

한 구간에서 A=(u[i+1]−u)/h[i], B=(u−u[i])/h[i]라고 놓으면 보간식은 다음과 같다. A와 B의 합은 1이다. 앞의 두 항은 선형 보간이고, 뒤의 항이 2차 미분 정보를 반영해 곡선을 휘게 한다.

q(u) = A q[i] + B q[i + 1]
     + ((A³ - A) M[i] + (B³ - B) M[i + 1]) h[i]² / 6

경유점이 반복되면 h가 0이 되어 나눗셈이 정의되지 않는다. 프로그램은 연속한 중복 경유점을 오류로 처리한다. 같은 위치에서 기다리는 요구는 경로에 중복 점을 넣기보다 시간 계획에서 대기 구간으로 표현하는 편이 명확하다.

그림은 선분 경로와 보간 곡선의 차이를 설명하는 개념도다. 자연 경계 조건은 양 끝의 곡률을 0으로 만드는 효과가 있지만, 곡선 전체의 곡률 상한까지 정하지는 않는다. 또한 매개변수에 대한 접선 벡터가 0이 되는 퇴화점에서는 진행 방향을 정의하기 어렵다. 위치의 미분이 연속이라는 사실만으로 모든 로봇이 주행 가능한 경로가 되지는 않는다.

곡선의 거리와 속도 제한

호 길이(arc length)는 곡선을 따라 잰 거리다. 정확한 호 길이는 접선 벡터의 크기를 적분해서 구하지만, 여기서는 곡선을 촘촘하게 표본화한 뒤 이웃한 위치 사이의 직선거리를 더한다. 표본 수를 늘렸을 때 전체 길이와 소요 시간이 충분히 수렴하는지 확인하는 방식으로 근사의 품질을 판단할 수 있다.

ds[i] = ||position[i + 1] - position[i]||
s[0] = 0
s[i + 1] = s[i] + ds[i]

위치 표본이 N개라면 거리 구간은 N−1개다. 속도와 도착 시각은 위치마다 하나씩 두고, 가속도와 구간 시간은 구간마다 하나씩 둔다. 배열 길이를 먼저 구분하면 마지막 원소에서 생기는 인덱스 오류를 줄일 수 있다.

구간에서 접선 방향 가속도가 일정하다고 가정하면 속도와 거리 사이에 v다음²=v현재²+2aΔs가 성립한다. 따라서 현재 속도에서 다음 표본까지 가속해 도달할 수 있는 속도에는 상한이 있다. 앞에서부터 이 상한을 적용하는 계산을 전진 계산이라고 부르겠다.

v[i + 1] = min(v[i + 1],
               sqrt(v[i]² + 2 a_max ds[i]))

전진 계산만으로는 목적지에서 멈출 수 있는지 알 수 없다. 끝점의 속도를 0으로 고정하고 뒤에서 앞으로 계산하면, 다음 표본의 속도까지 감속할 수 있는 현재 속도의 상한을 얻는다. 가속과 감속에 같은 한계를 사용하는 이번 예제에서는 식의 모양이 같다.

v[i] = min(v[i],
           sqrt(v[i + 1]² + 2 a_max ds[i]))

처음에는 모든 표본의 속도를 최고 속도로 채우고 양 끝을 0으로 만든다. 전진 계산은 출발점에서 가능한 가속을 반영하고, 후진 계산은 도착점까지 필요한 감속을 반영한다. 두 계산 모두 기존 속도를 낮추기만 하므로 이미 적용한 최고 속도를 넘어가지 않는다. 이 조건에서는 전진 한 번과 후진 한 번으로 각 구간의 가속·감속 부등식을 함께 만족시킬 수 있다.

여기서 제한하는 것은 속력의 변화율이다. 곡선 위를 일정한 속력으로 이동해도 진행 방향이 바뀌므로 법선 방향 가속도가 생긴다. 곡률을 κ라고 하면 그 크기는 v²|κ|이며, 접선 방향 가속도와는 별개다. 총가속도 상한이 요구되는 장비에는 곡률을 반영한 속도 상한과 가속도 배분이 추가로 필요하다. 이번 코드의 0.5m/s²를 총가속도 제한으로 해석해서는 안 된다.

속도에서 도착 시각으로

각 구간에서 가속도가 일정하면 평균 속도는 양 끝 속도의 산술평균이다. 따라서 구간 통과 시간은 다음과 같다. 이를 차례로 더하면 출발을 0초로 한 도착 시각 배열을 얻는다.

dt[i] = 2 ds[i] / (v[i] + v[i + 1])
time[i + 1] = time[i] + dt[i]

출발 속도가 0이어도 다음 속도가 양수이면 계산할 수 있다. 반대로 양 끝 속도가 모두 0인데 이동 거리가 양수라면, 이 구간을 하나의 일정 가속도 구간으로 표현할 수 없다. 중간에 가속과 감속을 나누는 표본을 추가해야 한다. 분모에 작은 수를 더해 통과시키면 시간만 매우 커질 뿐 잘못된 구간 모델이 해결되지는 않는다.

구간 안의 시간 참조도 정의할 수 있다. 구간 시작 이후 경과 시간을 τ라고 하면, 속도는 v[i]+a[i]τ이고 누적 거리는 s[i]+v[i]τ+a[i]τ²/2다. 이 식은 해당 구간의 시작부터 끝까지 적용한다. 아래 프로그램은 이 표현의 기반이 되는 위치, 거리, 속도, 시각, 가속도 배열을 만든다.

그림은 거리를 가로축으로 한 속도 계획의 개념도다. 일정한 가속도라도 속도는 거리의 제곱근에 따라 변하므로, 가속 구간이 이 좌표계에서 직선일 필요는 없다. 경로가 짧으면 최고 속도에 도달하기 전에 감속이 시작된다. 이번 경로는 충분히 길어서 최고 속도를 유지하는 구간이 생긴다.

거리의 근사와 시간 모델의 보장 범위도 구분해야 한다. 아래 검사는 표본 사이의 직선거리에 기반한 이산 계획이 제한을 만족하는지 확인한다. 시간에 맞춰 원래 스플라인을 다시 평가하려면 거리와 u 사이의 대응을 근사적으로 역변환해야 한다. 이 과정에는 오차가 있으므로 연속 곡선 위의 모든 순간에 대한 제한 준수를 아래 검사만으로 증명할 수는 없다.

완성 코드

다음 내용을 trajectory.py로 저장한다. Python 3와 numpy만 사용하며 난수를 사용하지 않아 시드 설정이 필요 없다. 모든 계산은 배정밀도로 수행한다. 출력은 검사 결과와 소수 셋째 자리까지의 요약으로 제한한다.

import numpy as np


def natural_spline(points, count=401):
    points = np.asarray(points, dtype=float)
    if points.ndim != 2 or points.shape[1] != 2:
        raise ValueError("points must have shape (n, 2)")
    n = len(points)
    if n < 2 or count < 3:
        raise ValueError("too few points or samples")
    if not np.all(np.isfinite(points)):
        raise ValueError("points must be finite")

    h = np.linalg.norm(np.diff(points, axis=0), axis=1)
    if np.any(h <= 1e-12):
        raise ValueError("adjacent points must be distinct")
    u = np.concatenate(([0.0], np.cumsum(h)))

    matrix = np.zeros((n, n))
    rhs = np.zeros((n, 2))
    matrix[0, 0] = 1.0
    matrix[-1, -1] = 1.0
    for i in range(1, n - 1):
        matrix[i, i - 1] = h[i - 1]
        matrix[i, i] = 2.0 * (h[i - 1] + h[i])
        matrix[i, i + 1] = h[i]
        rhs[i] = 6.0 * (
            (points[i + 1] - points[i]) / h[i]
            - (points[i] - points[i - 1]) / h[i - 1]
        )
    second = np.linalg.solve(matrix, rhs)

    query = np.linspace(u[0], u[-1], count)
    segment = np.searchsorted(u, query, side="right") - 1
    segment = np.clip(segment, 0, n - 2)
    width = h[segment]
    left = ((u[segment + 1] - query) / width)[:, None]
    right = ((query - u[segment]) / width)[:, None]
    xy = (
        left * points[segment] + right * points[segment + 1]
        + (
            (left**3 - left) * second[segment]
            + (right**3 - right) * second[segment + 1]
        ) * width[:, None]**2 / 6.0
    )
    xy[0] = points[0]
    xy[-1] = points[-1]
    return xy


def plan_speed(xy, v_max, a_max):
    if not np.isfinite(v_max) or not np.isfinite(a_max):
        raise ValueError("limits must be finite")
    if v_max <= 0.0 or a_max <= 0.0:
        raise ValueError("limits must be positive")

    ds = np.linalg.norm(np.diff(xy, axis=0), axis=1)
    if len(ds) < 2 or not np.all(np.isfinite(ds)):
        raise ValueError("invalid path samples")
    if np.any(ds <= 1e-12):
        raise ValueError("sample spacing must be positive")
    distance = np.concatenate(([0.0], np.cumsum(ds)))

    speed = np.full(len(xy), v_max, dtype=float)
    speed[0] = 0.0
    speed[-1] = 0.0
    for i in range(len(ds)):
        reachable = np.sqrt(speed[i]**2 + 2.0 * a_max * ds[i])
        speed[i + 1] = min(speed[i + 1], reachable)
    for i in range(len(ds) - 1, -1, -1):
        stoppable = np.sqrt(speed[i + 1]**2 + 2.0 * a_max * ds[i])
        speed[i] = min(speed[i], stoppable)

    pair_speed = speed[:-1] + speed[1:]
    if np.any(pair_speed <= 0.0):
        raise ValueError("a moving interval needs positive mean speed")
    dt = 2.0 * ds / pair_speed
    time = np.concatenate(([0.0], np.cumsum(dt)))
    accel = np.diff(speed) / dt
    return distance, speed, time, accel


def main():
    points = np.array([
        [0.0, 0.0],
        [2.0, 0.0],
        [4.0, 1.5],
        [6.0, 1.5],
        [8.0, 0.0],
    ])
    v_max = 1.0
    a_max = 0.5
    xy = natural_spline(points)
    distance, speed, time, accel = plan_speed(xy, v_max, a_max)

    tol = 1e-9
    assert np.all(np.isfinite(time))
    assert np.all(np.diff(time) > 0.0)
    assert np.all(np.diff(distance) > 0.0)
    assert np.all(speed >= 0.0)
    assert np.max(speed) <= v_max + tol
    assert np.max(np.abs(accel)) <= a_max + tol
    assert speed[0] == 0.0 and speed[-1] == 0.0
    recovered_ds = 0.5 * (speed[:-1] + speed[1:]) * np.diff(time)
    assert np.allclose(recovered_ds, np.diff(distance),
                       rtol=1e-9, atol=1e-9)

    print(f"경유점: {len(points)}개 / 경로 표본: {len(xy)}개")
    print(f"시작 위치: ({xy[0, 0]:.3f}, {xy[0, 1]:.3f})")
    print(f"종료 위치: ({xy[-1, 0]:.3f}, {xy[-1, 1]:.3f})")
    print(f"시작/종료 속도: {speed[0]:.3f} / {speed[-1]:.3f} m/s")
    print(f"최고 속도: {np.max(speed):.3f} m/s")
    print(f"최대 접선 가속도 크기: {np.max(np.abs(accel)):.3f} m/s²")
    print(f"시간 구간: {len(time) - 1}개")
    print("검사: 모두 통과")


if __name__ == "__main__":
    main()

줄별 해설

np.asarray(points, dtype=float)는 입력을 실수 배열로 통일한다. 정수 좌표를 입력해도 미분과 보간 결과를 실수로 저장한다. 이어지는 검사는 좌표가 두 열인지, 경유점이 충분한지, 무한대나 비정상 값이 없는지 확인한다. 이 예제의 함수는 정수인 표본 수를 받는 것을 전제로 한다.

np.diff(points, axis=0)는 이웃한 경유점의 좌표 차이를 구한다. np.linalg.norm(..., axis=1)은 각 차이 벡터의 길이를 계산한다. np.cumsum(h) 앞에 0을 붙이면 경유점마다 하나씩 대응하는 u가 된다. 1e-12는 이 예제의 미터 단위 좌표에서 사실상 중복인 점을 거르는 수치 기준이다.

matrix의 첫 행과 마지막 행에는 대각 원소 1만 넣는다. 해당 행의 오른쪽 항은 0이므로 양 끝의 2차 미분이 0으로 고정된다. 반복문은 내부 경유점의 연속 조건을 채운다. rhs의 첫 열과 둘째 열은 각각 x와 y를 위한 항이다.

np.linalg.solve(matrix, rhs)는 역행렬을 직접 만들지 않고 연립방정식을 푼다. 이 행렬은 내부 행에 세 개의 계수만 있는 구조지만, 경유점이 다섯 개인 예제에서는 일반적인 풀이로 충분하다. 경유점이 매우 많아지면 이 구조를 이용해 저장 공간과 계산량을 줄이는 구현을 검토할 수 있다.

np.linspace는 u의 전체 범위를 401개로 나눈다. 이는 거리나 시간을 균등하게 나눈다는 뜻이 아니다. np.searchsorted는 각 표본이 속한 구간을 찾고, np.clip은 마지막 경계값도 마지막 유효 구간으로 배정한다. 내부 경유점에서는 어느 쪽 구간을 선택해도 보간 조건에 따라 같은 위치를 얻는다.

[:, None]은 계수 배열을 한 열짜리 배열로 바꾼다. 그러면 표본마다 계산한 계수가 x와 y 두 좌표에 함께 적용된다. xy를 계산하는 괄호 안은 앞에서 소개한 보간식 그대로다. 끝의 두 대입문은 양 끝 좌표를 입력값으로 명시해 부동소수점 계산에 따른 작은 차이를 제거한다.

plan_speed에서는 먼저 표본 사이의 거리와 누적 거리를 만든다. 속도 배열은 최고 속도로 초기화하고 출발과 도착만 0으로 설정한다. 전진 반복문의 reachable은 앞 표본에서 도달 가능한 상한이고, 후진 반복문의 stoppable은 뒤 표본의 속도로 감속할 수 있는 상한이다. 각 단계의 min이 더 엄격한 제한을 남긴다.

pair_speed는 이웃한 속도의 합이다. 양수인지 확인한 다음 구간 시간을 계산한다. np.diff(speed) / dt는 구간별 접선 가속도다. 앞쪽 구간에서는 양수, 뒤쪽 구간에서는 음수가 될 수 있으므로 최대 크기를 검사할 때는 절댓값을 취한다.

main의 검사는 시간과 거리가 증가하는지, 속도가 음수가 아닌지, 상한과 정지 조건을 만족하는지 확인한다. 마지막 검사는 평균 속도와 구간 시간을 곱해 거리를 복원한다. 이 확인은 거리·속도·시간 배열의 대응이 맞는지 점검한다. tol은 부동소수점 반올림 차이를 허용하는 수치 여유이며, 실제 장비의 운전 여유를 뜻하지 않는다.

실행 결과

numpy가 설치된 환경에서 다음 명령을 실행한다. 첫 명령은 문법 컴파일을 확인하며 정상일 때 별도 출력을 내지 않는다. 두 번째 명령은 경고를 오류로 취급하며 프로그램을 실행한다. 검사문이 생략되지 않도록 최적화 실행 옵션은 사용하지 않는다.

python3 -W error -m py_compile trajectory.py
python3 -W error trajectory.py

예상 출력은 다음과 같다.

경유점: 5개 / 경로 표본: 401개
시작 위치: (0.000, 0.000)
종료 위치: (8.000, 0.000)
시작/종료 속도: 0.000 / 0.000 m/s
최고 속도: 1.000 m/s
최대 접선 가속도 크기: 0.500 m/s²
시간 구간: 400개
검사: 모두 통과

시작과 종료 속도가 모두 0이면서 중간의 최고 속도는 1.0m/s다. 구간별 가속도의 최대 크기도 지정한 한계와 일치한다. 다만 이 출력은 경로가 장애물을 피한다거나 바퀴별 속도 제한까지 만족한다는 뜻은 아니다. 확인한 조건은 표본 기반 거리 모델의 속도와 접선 가속도 조건이다.

전체 주행 시간은 time[-1], 근사 경로 길이는 distance[-1]에 들어 있다. 이 값들은 표본 수를 바꾸면 조금 달라질 수 있다. 출력이 같다는 사실보다, 표본 수를 늘려도 관심 있는 물리량의 변화가 충분히 작아지는지 살펴보는 것이 해상도 선택에 도움이 된다.

실무에서 자주 틀리는 것

매개변수 간격을 이동 거리로 사용한다

u는 경유점의 직선거리로 구성했지만, 그 사이를 스플라인이 휘어서 지나간다. 다음 코드는 매개변수 간격을 곡선 표본 사이의 이동 거리로 간주한다.

# 틀린 코드: u 간격을 이동 거리로 사용한다.
ds = np.diff(query)

표본 좌표 사이의 거리를 계산해야 한다. 이 값도 곡선 길이의 근사이므로 굽힘이 큰 부분에서는 표본을 더 촘촘하게 배치할 필요가 있다.

# 고친 코드
ds = np.linalg.norm(np.diff(xy, axis=0), axis=1)

끝점 속도만 0으로 덮어쓴다

일정 속도로 달리다가 마지막 표본에서만 정지하도록 바꾸면 마지막 구간의 감속 크기가 커질 수 있다. 정지 조건은 이전 구간들까지 전파해야 한다.

# 틀린 코드: 도착 직전의 제동 거리를 고려하지 않는다.
speed = np.full(len(xy), v_max)
speed[-1] = 0.0

완성 코드처럼 전진 계산을 먼저 수행한 뒤 다음 후진 계산을 적용한다. 마지막 표본에 가까울수록 허용 속도가 작아져 정지에 필요한 거리가 확보된다.

# 고친 코드: 전진 계산을 마친 속도 배열에 적용한다.
speed[-1] = 0.0
for i in range(len(ds) - 1, -1, -1):
    bound = np.sqrt(speed[i + 1]**2 + 2.0 * a_max * ds[i])
    speed[i] = min(speed[i], bound)

출발 속도로 구간 시간을 나눈다

다음 식은 구간 전체에서 속도가 일정할 때만 맞다. 출발 속도가 0이면 나눗셈 경고도 발생한다.

# 틀린 코드
dt = ds / speed[:-1]

이번 모델은 구간별 일정 가속도를 사용하므로 평균 속도로 나눈다. 양 끝 속도가 모두 0인 이동 구간은 입력 모델을 고쳐야 하므로 오류로 알린다.

# 고친 코드
pair_speed = speed[:-1] + speed[1:]
if np.any(pair_speed <= 0.0):
    raise ValueError("split the interval to represent motion")
dt = 2.0 * ds / pair_speed

감속 구간을 검사에서 빠뜨린다

가속도의 최댓값만 검사하면 큰 음수인 감속을 놓친다. 또한 화면에 반올림한 숫자는 제한 위반을 가릴 수 있으므로 검사에는 원래 배열을 사용한다.

# 틀린 코드
assert np.max(accel) <= a_max + 1e-9

가속과 감속의 한계가 같은 경우에는 절댓값을 검사한다. 서로 다른 한계를 사용한다면 양수와 음수 방향의 부등식을 따로 작성해야 한다.

# 고친 코드
assert np.max(np.abs(accel)) <= a_max + 1e-9

한눈에 보기

곡선 구성부터 시간 계획까지의 역할과 확인 사항

단계	만드는 값	핵심 확인 사항
자연 스플라인	위치 표본 xy	경유점 보간, 중복 점, 곡선의 공간 여유
거리 근사	구간 거리 ds, 누적 거리 distance	양수 간격, 표본 수에 따른 수렴
전진·후진 계산	표본 속도 speed	최고 속도, 출발 가속, 도착 감속
시간 계산	시각 time, 가속도 accel	시간 증가, 양방향 가속도 크기

곡선의 부드러움과 속도 계획의 부드러움은 각각 확인해야 한다. 이 구현에서는 스플라인의 2차 미분이 연속이지만 구간별 가속도는 경계에서 바뀔 수 있다. 따라서 가속도의 시간 변화율까지 제한하는 궤적은 아니다. 적재물의 흔들림이나 구동계 특성 때문에 더 부드러운 가속이 필요하면 시간 계획에 추가 조건을 두어야 한다.

연습 문제

1. 표본 수를 101, 401, 1601로 바꾸고 각 경우의 근사 경로 길이와 전체 주행 시간을 출력하라. 제한 검사도 유지하라. 어떤 변화를 보면 표본 수가 충분하다고 판단할 수 있는지 설명하라.
2. 최고 속도는 그대로 두고 가속도 한계를 0.25m/s²로 낮춰라. 속도 계획과 전체 주행 시간이 어떻게 달라지는지 설명하라. 최고 속도가 반드시 낮아지는지도 판단하라.
3. 가속 한계는 0.5m/s², 감속 한계는 0.3m/s²로 나누어라. 전진·후진 계산과 가속도 검사 중 어디를 바꿔야 하는지 작성하라.
4. 두 위치 표본만 남기고 양 끝 속도를 0으로 설정했다고 하자. 구간 시간 식을 바로 사용할 수 없는 이유와, 같은 출발·도착 조건으로 이동을 표현하는 방법을 설명하라.

정답과 해설

1. main 안에서 다음 반복문으로 길이와 시간을 비교할 수 있다. 기존 검사들도 각 반복 안에서 수행해야 한다.

   for count in (101, 401, 1601):
       xy = natural_spline(points, count=count)
       distance, speed, time, accel = plan_speed(xy, v_max, a_max)
       print(count, f"{distance[-1]:.6f}", f"{time[-1]:.6f}")

   표본 수 증가에 따른 길이와 시간의 차이를 비교한다. 목적에 맞는 오차 기준을 먼저 정하고, 더 촘촘한 표본에서도 변화가 그 기준보다 작아지는지 확인한다. 전체 길이의 수렴만으로 국소적인 굽힘이나 장애물 여유까지 확인되는 것은 아니다. 속도 전환 위치의 표본화 때문에 시간의 변화가 항상 한 방향이라고 가정해서도 안 된다.

2. 가속과 감속에 더 긴 거리가 필요하고 전체 주행 시간은 늘어난다. 정지 상태에서 1.0m/s까지 가속하는 데 필요한 거리는 v²/(2a)이므로, 한계를 0.5에서 0.25로 낮추면 1m에서 2m로 늘어난다. 감속에도 2m가 필요하다. 이번 경로는 그 합보다 길어 최고 속도 1.0m/s에 도달할 수 있다. 따라서 한계를 낮춘다고 최고 속도까지 반드시 낮아지는 것은 아니다.

3. 전진 계산에는 가속 한계, 후진 계산에는 감속 한계를 사용한다. 이를 함수의 별도 인자로 받고 각각 양수이며 유한한지 확인한다. 속도 계산의 제곱근 내부와 검사식은 다음처럼 바꾼다.

   # 전진 계산
   reachable = np.sqrt(speed[i]**2 + 2.0 * a_up * ds[i])
   
   # 후진 계산
   stoppable = np.sqrt(speed[i + 1]**2 + 2.0 * a_down * ds[i])
   
   # 계산이 끝난 뒤 검사
   assert np.all(accel <= a_up + 1e-9)
   assert np.all(accel >= -a_down - 1e-9)

   감속 한계가 더 작으므로 정지에 더 긴 거리가 필요하다. 같은 속도에서 접근하더라도 목적지보다 더 먼 위치에서 감속을 시작하게 된다.

4. 양 끝 속도가 모두 0이면 평균 속도도 0이다. 양수인 이동 거리를 하나의 일정 가속도 구간으로 설명할 수 없으므로 통과 시간 식의 분모가 0이 된다. 완성 코드가 최소 세 개의 표본을 요구하는 이유도 이와 관련된다.

   경로 중간에 표본을 추가하고 그 위치의 속도를 양수로 허용하면, 앞 구간의 가속과 뒤 구간의 감속을 나누어 표현할 수 있다. 대칭인 조건에서는 중간 속도를 가속 한계와 최고 속도로 제한한다. 실제 정지 대기는 이동 구간과 구분해 별도의 시간 구간으로 기록한다.