경로 점과 속도 제한

85분 안팎

학습 목표

격자 중심을 map 좌표로 변환하고 회전 구간의 속도 제한을 계산합니다.

개념

셀 경로를 제어기가 읽는 목표로 바꿉니다

BFS가 반환한 (열,행)은 정수 인덱스입니다. 앞 모듈 제어기의 x_m·y_m는 map 기준 미터이므로 그 목록을 그대로 전달하면 전혀 다른 거리로 이동합니다. 이번 레슨은 셀 중심 좌표와 접근 속도 상한을 묶어 경유점 목록을 만드는 작업입니다. 완료하면 원점 이동을 반영하고 모서리에서 낮은 상한을 선택하며 종점 표식과 실제 구동 명령을 구별할 수 있습니다.

경로는 공간에서 지나갈 위치 목록입니다. 위치마다 상한을 붙여도 어느 시각에 어디에 있어야 하는지는 아직 정해지지 않습니다. 이번 결과는 완전한 시간 궤적이 아니며 가속도 제한이나 도착 시각 배열을 제공하지 않습니다. 제어 루프는 매 tick 현재 자세를 읽고 이 상한 안에서 속도를 결정합니다. 더 읽기의 궤적 장은 이 차이를 확장합니다.

원점에 반 칸을 더합니다

열 c의 중심 x는 origin_x+(c+0.5)×resolution입니다. 행 r의 중심 y는 origin_y+(r+0.5)×resolution입니다. 0.5를 빼면 모서리 쪽으로 이동하고 생략하면 셀의 아래·왼쪽 경계가 됩니다. 경계는 지도 장애물 사각형과 접할 수 있으므로 중심을 향하도록 정의합니다. 계산 뒤 단위가 m인지 입력 이름과 함께 검산합니다.

프로젝트 지도 원점은 (-0.35,-0.35)m이며 해상도는 0.1m입니다. (3,3) 중심은 (0,0)m이어서 앞 모듈 초기 자세와 연결됩니다. 목표 (1,0)m는 (13,3) 중심입니다. 이 값을 원점 (0,0)의 지도와 섞으면 시작점과 목표가 모두 0.35m 옮겨집니다. 지도 파일과 경로 파일에 frame_id=map 계약을 공유해야 합니다.

변환 방향과 경계 사례를 구분합니다

셀에서 세계 좌표로 가는 함수는 중심 한 점을 고릅니다. 반대로 세계 좌표에서 셀로 가는 함수는 원점을 뺀 뒤 해상도로 나누고 floor를 취합니다. 두 연산은 연속 위치를 완벽히 왕복 복원하지 않습니다. 같은 셀 내부의 여러 좌표가 동일한 셀 번호로 모이기 때문입니다. 셀 중심에서 왕복한 번호가 같다는 검사는 유용하지만 임의 점의 좌표가 그대로 복원되어야 한다고 요구하면 안 됩니다.

목표가 셀 중심이 아닌 경우 계획은 목표가 속한 셀까지 찾고 실행 목록의 마지막 좌표만 원래 목표로 바꿉니다. 마지막 연결 구간도 충돌 검사를 거쳐야 합니다. 목표를 중심으로 조용히 바꿔 버리면 셀 경로 도달과 사용자 목표 도달이 서로 달라집니다. 같은 셀 안의 짧은 목표 이동도 로컬 테스트에 포함해 이 차이를 확인합니다.

입구와 출구 방향으로 코너를 판별합니다

내부 경유점 i에서 들어오는 정수 방향은 path[i]−path[i−1]이고 나가는 방향은 path[i+1]−path[i]입니다. 두 방향이 다르면 코너입니다. 4방향 BFS의 이웃 이동은 각 방향 크기가 1이므로 벡터의 그대로 비교로 충분합니다. 일반적인 실수 곡선에서는 벡터 크기나 각도를 추가로 고려해야 하지만 이번 함수의 입력 범위는 연속된 격자 경로입니다.

첫 점은 이전 점이 없으므로 코너 판정을 하지 않습니다. 마지막 점은 다음 점이 없으므로 종점 표식을 적용합니다. 내부 인덱스 조건을 먼저 검사하지 않으면 첫 점에서 path[-1]을 잘못 읽거나 마지막에서 IndexError가 발생합니다. 중복 셀이나 대각선 점프는 제공한 브라우저 검증이 ERROR로 거부합니다. 방향 검사가 통과하더라도 장애물 없는 경로임을 증명하지는 않습니다.

코너를 향해 접근할 때 상한을 낮춥니다

직선 점의 상한은 0.30m/s이고 코너 점의 상한은 0.10m/s입니다. 제어기가 코너를 목표로 삼는 접근 구간부터 낮은 값을 사용합니다. 코너를 지나고 나서 감속하면 회전 전의 접근 동작을 바꿀 수 없습니다. 이 정책은 실제 곡률에서 계산한 최적 속도가 아니라 교육용의 명시적인 제한 규칙입니다. 어떤 점에 낮은 상한을 붙였는지 목록으로 검토합니다.

몸체 선속도는 전역 상한, 액션 요청 상한, 경유점 상한, 거리 P 명령의 최솟값입니다. 각속도는 앞 모듈의 ±1.0rad/s 제한과 회전 우선 조건을 사용합니다. 선속도만 낮춘다고 경로 모서리를 정확히 통과하는 것은 아닙니다. 경유점 도달 반경과 자세 오차, 회전 문턱이 함께 실제 자취를 결정하므로 다음 레슨에서 움직임 전체를 검사합니다.

마지막 속도 0의 의미를 정합니다

waypoints의 마지막 max_speed_mps는 0.0으로 기록합니다. 이는 종점에서는 정지해야 한다는 표식입니다. 그 값을 마지막 점으로 접근하는 명령의 상한으로 그대로 넣으면 로봇은 도착하기 전부터 멈춰 영원히 도달하지 못할 수 있습니다. 실행기는 마지막 접근에는 요청 상한과 직선 상한을 사용하고 도달 반경 안에 들어온 뒤 PWM 0을 확인합니다.

목록이 한 점이면 그 점은 첫 점이면서 마지막 점이므로 상한 0.0입니다. 이 경우도 실행기가 도착 판정부터 하거나 같은 셀 내부의 실제 목표까지 접근하도록 처리해야 합니다. 빈 경로에는 경유점도 없습니다. 실패를 나타내는 []에 가짜 (0,0) 점을 넣어 진행하면 계획 실패가 주행 요청으로 바뀌므로 빈 목록을 그대로 보존합니다.

출력 형식을 좁혀 검산합니다

브라우저 결과는 [x_m,y_m,max_speed_mps] 행들의 JSON 목록입니다. 출력 좌표와 상한은 소수점 여섯 자리 수준으로 round하고 음의 0은 0.0으로 통일합니다. JSON 숫자는 항상 여섯 자리를 보이는 문자열이 아니므로 0.300000 대신 0.3이 나오는 것이 정상입니다. 로컬 제어기는 반올림 전 실수 좌표를 사용합니다. 출력용 반올림과 운동 계산을 분리합니다.

입력 resolution은 양의 유한 수이고 origin은 유한한 숫자 두 개입니다. path는 음수가 아닌 정수 셀 쌍의 목록이며 인접 점은 정확히 한 칸 차이여야 합니다. 해상도 0, 원점 NaN, 중복 점, 대각선 점프는 ERROR입니다. 지도 크기와 장애물은 이 함수의 입력에 없으므로 여기에서 범위 안인지 또는 충돌이 없는지 판정한다고 설명하지 않습니다.

경로를 단순화할 때 연결 근거를 잃지 않습니다

직선에 놓인 중간 점을 삭제하면 목표 전환이 줄어 실행이 빨라질 수 있습니다. 하지만 모서리를 삭제하고 두 점을 직선으로 연결하면 원래 BFS가 피한 장애물 쪽을 가로지를 수 있습니다. 같은 이유로 보간 곡선이 경유점을 모두 통과하더라도 중간 곡선의 안전성을 따로 검토해야 합니다. 이번 실습은 점을 삭제하거나 스플라인으로 바꾸지 않고 원래 셀 목록을 보존합니다.

실습 TODO는 corner 판정 한 곳입니다. 먼저 직선 세 점과 직각 세 점을 비교한 뒤 음수 원점, 한 점, 빈 목록을 실행합니다. 코너 상한이 전부 0.3이면 방향 비교를, 좌표가 반 칸씩 어긋나면 중심 식을, 종점 앞에서 멈춘다면 실행기의 표식 해석을 확인합니다. 최종 제출에는 변환된 좌표와 낮은 상한을 선택한 점의 들어오는·나가는 방향을 함께 적습니다.

따라하기

프로젝트 셀 중심을 확인합니다

원점을 포함해야 m06 초기 자세와 목표가 맞습니다.

origin=(-0.35,-0.35);res=0.1
for c,r in ((3,3),(13,3)):
    x=origin[0]+(c+0.5)*res;y=origin[1]+(r+0.5)*res
    print(f'cell=({c},{r}) map=({x:.2f},{y:.2f})')

실행 결과

cell=(3,3) map=(0.00,0.00)
cell=(13,3) map=(1.00,0.00)

코너와 종점을 표시합니다

가운데 점에 코너 접근 상한을 붙이고 마지막에는 정지 표식을 넣습니다.

def waypoints(path,resolution,origin,straight=0.30,turn=0.10):
    result=[]
    for i,(c,r) in enumerate(path):
        corner=(0<i<len(path)-1 and
                (c-path[i-1][0],r-path[i-1][1]) != (path[i+1][0]-c,path[i+1][1]-r))
        result.append(dict(x_m=origin[0]+(c+0.5)*resolution,
                           y_m=origin[1]+(r+0.5)*resolution,
                           max_speed_mps=0.0 if i==len(path)-1 else turn if corner else straight))
    return result

for p in waypoints([(0,0),(1,0),(1,1)],0.1,[0,0]):
    print(f"{p['x_m']:.2f} {p['y_m']:.2f} {p['max_speed_mps']:.2f}")

실행 결과

0.05 0.05 0.30
0.15 0.05 0.10
0.15 0.15 0.00

요청과 경유점 상한을 합칩니다

상한은 더하는 값이 아니라 동시에 만족할 제약입니다.

request=0.2;global_cap=0.3;distance_command=0.25
for point_cap in (0.3,0.1):
    print(f'v={min(request,global_cap,distance_command,point_cap):.2f}')

실행 결과

v=0.20
v=0.10

확인 문제

실습

path는 [열,행] 정수 쌍의 목록이고 resolution·origin은 m 단위입니다. 연속 점은 4방향 한 칸 이동입니다. 셀 중심을 map 좌표로 바꾸고 내부 방향 변화점에는 0.1, 직선에는 0.3, 종점에는 0.0m/s 표식을 붙입니다. corner TODO를 완성합니다. 출력은 소수 여섯 자리 수준으로 반올림한 [x_m,y_m,상한] JSON 목록이며 빈 path는 []입니다. 중복·대각 이동·음수 인덱스·유효하지 않은 치수는 ERROR입니다. 종점 상한은 접근 명령이 아니라 정지 표식입니다.

실행할 코드

def waypoints(path,resolution,origin,straight=0.30,turn=0.10):
    result=[]
    for i,(c,r) in enumerate(path):
        corner=False  # TODO: 내부 점의 입구·출구 방향을 비교합니다.
        result.append(dict(x_m=origin[0]+(c+0.5)*resolution,
                           y_m=origin[1]+(r+0.5)*resolution,
                           max_speed_mps=0.0 if i==len(path)-1 else turn if corner else straight))
    return result

import json,sys,math
try:
    obj=json.load(sys.stdin);path=obj['path'];res=obj['resolution'];origin=obj['origin']
    if type(res) not in (int,float) or not math.isfinite(res) or res<=0: raise ValueError('resolution')
    if not isinstance(origin,list) or len(origin)!=2 or any(type(v) not in (int,float) or not math.isfinite(v) for v in origin): raise ValueError('origin')
    if not isinstance(path,list) or any(not isinstance(p,list) or len(p)!=2 or any(type(v) is not int or v<0 for v in p) for p in path): raise ValueError('path')
    if any(abs(a[0]-b[0])+abs(a[1]-b[1])!=1 for a,b in zip(path,path[1:])): raise ValueError('adjacency')
    result=waypoints(path,res,origin)
    def clean(v):
        v=round(v,6)
        return 0.0 if v==0 else v
    print(json.dumps([[clean(p[k]) for k in ('x_m','y_m','max_speed_mps')] for p in result]))
except (ValueError,KeyError,TypeError,OverflowError):
    print('ERROR')

모범 답안

def waypoints(path,resolution,origin,straight=0.30,turn=0.10):
    result=[]
    for i,(c,r) in enumerate(path):
        corner=(0<i<len(path)-1 and
                (c-path[i-1][0],r-path[i-1][1]) != (path[i+1][0]-c,path[i+1][1]-r))
        result.append(dict(x_m=origin[0]+(c+0.5)*resolution,
                           y_m=origin[1]+(r+0.5)*resolution,
                           max_speed_mps=0.0 if i==len(path)-1 else turn if corner else straight))
    return result

import json,sys,math
try:
    obj=json.load(sys.stdin);path=obj['path'];res=obj['resolution'];origin=obj['origin']
    if type(res) not in (int,float) or not math.isfinite(res) or res<=0: raise ValueError('resolution')
    if not isinstance(origin,list) or len(origin)!=2 or any(type(v) not in (int,float) or not math.isfinite(v) for v in origin): raise ValueError('origin')
    if not isinstance(path,list) or any(not isinstance(p,list) or len(p)!=2 or any(type(v) is not int or v<0 for v in p) for p in path): raise ValueError('path')
    if any(abs(a[0]-b[0])+abs(a[1]-b[1])!=1 for a,b in zip(path,path[1:])): raise ValueError('adjacency')
    result=waypoints(path,res,origin)
    def clean(v):
        v=round(v,6)
        return 0.0 if v==0 else v
    print(json.dumps([[clean(p[k]) for k in ('x_m','y_m','max_speed_mps')] for p in result]))
except (ValueError,KeyError,TypeError,OverflowError):
    print('ERROR')

더 읽기

robot-advanced · robot-adv-08-trajectory

면접 질문

서로 다른 좌표계의 위치를 변환하는 과정을 설명해 주시면 됩니다.
목표점에 가까워져도 로봇이 흔들리는 상황을 설명해 주시면 됩니다.