경로 추종과 충돌 확인

110분 안팎

학습 목표

각 경로 점을 목표 제어기에 연결하고 이동 구간 전체의 충돌을 검사합니다.

개념

계획 성공과 실행 성공을 나눕니다

셀 경로를 찾았다고 로봇이 그 자취를 정확히 따르는 것은 아닙니다. 제어 주기 사이에 이동하고 코너에서 원호를 그리며 도달 반경 안에서 다음 목표로 넘어갑니다. 따라서 계획 결과, 실제 자세, 정지 출력을 모두 관찰해야 합니다. 이번 레슨은 경유점 인덱스를 유지하는 액션을 만들고 매 이동 구간의 몸체 충돌을 검사해 성공과 실패의 증거를 남깁니다.

로컬 robotics-m07-path-execution은 m06 미션 solution 전체 위에 planning.py·navigation.py·새 테스트를 추가한 폴더입니다. 모듈 미션 robotics-m07-grid-planning도 같은 출발점을 유지합니다. 기존 Actions와 MotionActions는 변경하지 않고 NavigationActions를 확장합니다. 통신 회귀와 운동 회귀를 보존하므로 경로 기능을 고칠 때 앞 단계의 취소·HAL 정지 계약이 깨졌는지도 확인할 수 있습니다.

목표를 접수한 뒤 계획을 만듭니다

submit은 먼저 기존 액션의 id·frame_id·좌표·요청 속도 검증을 호출합니다. ACCEPTED일 때 현재 자세와 요청 목표를 셀로 바꾸고 팽창 지도에서 BFS를 실행합니다. 빈 경로라면 접수된 작업을 ABORTED/PATH_NOT_FOUND로 끝냅니다. 문법이 맞는 이동 요청이 경로를 찾지 못한 결과와 입력 형식 때문에 REJECTED된 결과는 서로 다릅니다.

실패·취소·성공은 모두 _finish에서 PWM 0을 요청하고 실제 출력 0을 확인한 뒤 종료합니다. STOP_NOT_CONFIRMED가 발생하면 종료 성공을 주장하지 않습니다. 경로가 없는 시작·목표·통로는 아무 움직임도 만들지 않아야 합니다. PATH_NOT_FOUND 뒤에 이전 목표의 PWM이 남는 구현은 계획 실패 처리와 구동 종료를 따로 둔 결과이므로 한 경계에서 묶어 검사합니다.

경유점 진행과 마지막 도달을 관리합니다

경로의 첫 셀은 현재 위치가 속한 셀이므로 여러 셀 경로에서는 두 번째 점부터 추종합니다. 한 셀짜리 경로는 원래 목표 좌표까지 확인합니다. 매 tick 경유점 거리와 최종 목표 거리 피드백을 구분하고 가까워지면 인덱스를 한 칸 증가시킵니다. 최종 점 도달 전에는 SUCCEEDED를 남기지 않습니다. 완료 후 추가 tick에서 위치가 유지되는지도 시험합니다.

m06의 도달 반경 0.10m를 0.1m 간격 점마다 그대로 쓰면 옆 점까지 이미 도달한 것으로 보거나 모서리를 크게 건너뛸 수 있습니다. 새 액션은 경유점 반경을 0.025m로 줄입니다. 앞 모듈 설정 파일은 보존하고 새 클래스에서만 덮어씁니다. 작은 반경은 코너 건너뛰기를 줄이는 설정이며 추정 잡음이 생기면 흔들림과 종료 시간도 다시 평가해야 합니다.

점 사이를 통과하는 원형 몸체를 검사합니다

이전 위치 a와 새 위치 b의 끝점만 검사하면 긴 한 tick에서 장애물을 뛰어넘을 수 있습니다. 원형 몸체의 중심이 선분을 따라 움직일 때 쓸고 지나가는 영역은 선분 주위 반경 띠입니다. 원본 점유 셀의 닫힌 사각형과 선분 사이 최소 거리가 몸체 반경 이하이면 충돌로 판단합니다. 물체에 접하는 경우도 실패이며 미관측 셀과 지도 밖 영역에도 같은 보수 정책을 적용합니다.

선분이 사각형 내부를 지나면 최소 거리는 0입니다. 교차하지 않을 때는 양 끝점에서 사각형까지 거리와 사각형 네 꼭짓점에서 선분까지 거리를 비교합니다. 가장 작은 값이 두 도형의 거리입니다. 모서리 대각 접근은 중심 간 거리로만 계산하지 않습니다. 점-선분 투영 비율은 0부터 1 사이로 제한하며 길이 0인 구간은 한 점으로 계산합니다.

회전 원호와 직선 현의 차이를 보완합니다

C HAL은 tick 안의 v·w를 일정하게 두고 원호 적분으로 새 자세를 만듭니다. 시작·끝을 잇는 선분은 그 원호의 현이므로 현만 검사하면 굽은 부분의 침범을 놓칠 수 있습니다. 상수 명령 구간의 위치 가속도 크기는 |v·w|이고 현과 원호의 차이는 |v·w|·dt²/8 이하로 묶을 수 있습니다. 이 상한을 몸체 반경에 더해 선분 검사를 보수적으로 합니다.

dt=0.05, v=0.30, |w|=1.0이면 보완 거리는 0.00009375m입니다. 수치가 작아도 모델의 차이를 설명하고 코드에 남깁니다. 이 근거는 한 tick 동안 일정한 속도와 회전 명령이라는 가정에 의존합니다. 실제 가속·측면 미끄러짐·지연된 제어에는 같은 식을 그대로 안전 보장으로 적용하지 않습니다. 이번 C 모델의 원호 끝점 예측과 실제 갱신을 함께 검사합니다.

움직이기 전에 멈출 기회를 둡니다

command가 계산한 v·w로 다음 자세를 먼저 예측하고 보완 반경으로 swept_clear를 호출합니다. 위험하면 hal.step을 하지 않고 ABORTED/COLLISION_RISK와 정지를 남깁니다. prevented_segments는 이런 사전 차단 횟수입니다. 실제 이동 로그의 충돌 횟수와 다른 지표이므로 둘을 섞지 않습니다. 정지로 피한 위험을 이미 충돌한 횟수처럼 보고하면 실험 해석이 바뀝니다.

사전 검사 뒤 HAL에 PWM을 전달하고 step이 성공하면 실제 새 자세로 같은 구간을 다시 검사합니다. 기록 segments에는 이전 중심·새 중심·검사 반경을 저장합니다. 재검사 실패는 COLLISION으로 기록합니다. 이번 이상 모델에서는 예측이 맞지만 사전 계산과 실제 적용의 차이를 잡는 구조를 갖춥니다. 지도 변경 테스트는 다음 tick 전에 장애물을 넣어 이동 없이 위험 종료되는지 확인합니다.

성공 이외의 종료도 시험합니다

취소 상태는 제어와 적분보다 먼저 처리합니다. HAL 결함은 ABORTED/HAL_FAULT입니다. 요청 속도 0은 전진이 없으므로 제한 시간 60초를 넘으면 TIMEOUT으로 정지합니다. 가상 시간 상한은 무한 반복을 막고 실패를 기록하기 위한 조건입니다. 시간 초과를 성공으로 바꾸거나 마지막 목표 좌표로 위치를 강제로 수정하지 않습니다.

실습의 starter는 부모 연결과 구간 검사 두 곳이 불완전합니다. test_bfs_order에서 목표만 반환되면 parent 연결을 고칩니다. test_crossing_endpoint_trap에서 기대 False가 True이면 장애물과 시작점만 비교했는지 봅니다. 도달 실패는 경유점 인덱스·코너 상한·작은 도달 반경을 읽습니다. HAL_LIBRARY 오류는 빌드를 준비하는 check.sh 없이 Python을 직접 실행했는지 확인합니다.

증거를 같은 조건으로 재실행합니다

check.sh는 check-m06.sh를 직접 실행한 뒤 새 공유 라이브러리와 경로 검사를 실행합니다. 이전 단계 스크립트는 현재 check.sh를 호출하지 않고 테스트도 검사 스크립트를 부르지 않습니다. 네 fixture는 열린 지도·단일 장애물 우회·막힌 목표·반경보다 좁은 통로입니다. 성공 두 사례의 60초 이내 도달, 모든 기록 구간의 충돌 0, 종료 PWM 0을 함께 확인합니다.

미션 제출에는 원본·팽창 지도, 셀 경로, 경유점, 전체 검사 로그와 실패 사유를 남깁니다. 같은 이웃 순서로 두 번 실행한 셀 목록도 비교합니다. 이번 구현은 참값 자세와 정적 지도를 쓰므로 성공 로그가 센서 잡음·동적 장애물·실물 정지 거리까지 검증하지는 않습니다. 더 읽기에서는 다른 추종 방식의 오차 정의를 살펴보고 다음 모듈에서는 센서와 시간 차이를 실험합니다.

따라하기

끝점 검사의 함정을 재현합니다

닫힌 장애물 사각형을 사이에 둔 두 위치는 각각 안전하지만 이동 선분은 관통합니다.

import math
def point_segment(p,a,b):
    dx,dy=b[0]-a[0],b[1]-a[1]
    den=dx*dx+dy*dy
    t=0.0 if den==0 else max(0.0,min(1.0,((p[0]-a[0])*dx+(p[1]-a[1])*dy)/den))
    return math.hypot(p[0]-a[0]-t*dx,p[1]-a[1]-t*dy)

def segment_rect_distance(a,b,rect):
    x0,y0,x1,y1=rect
    # Slab intersection for a closed segment and closed rectangle.
    low,high=0.0,1.0
    intersects=True
    for av,bv,mn,mx in ((a[0],b[0],x0,x1),(a[1],b[1],y0,y1)):
        delta=bv-av
        if abs(delta)<1e-15:
            if not mn<=av<=mx: intersects=False;break
        else:
            u,v=(mn-av)/delta,(mx-av)/delta
            low=max(low,min(u,v));high=min(high,max(u,v))
            if low>high: intersects=False;break
    if intersects: return 0.0
    def point_rect(p):
        return math.hypot(max(x0-p[0],0,p[0]-x1),max(y0-p[1],0,p[1]-y1))
    return min(point_rect(a),point_rect(b),
               *(point_segment(p,a,b) for p in ((x0,y0),(x0,y1),(x1,y0),(x1,y1))))

a=(0.2,0.45);b=(0.8,0.45);rect=(0.4,0.4,0.5,0.5)
for start,end in ((a,a),(b,b),(a,b)):
    d=segment_rect_distance(start,end,rect)
    print(f'distance={d:.2f} collision={d<=0.05}')

실행 결과

distance=0.20 collision=False
distance=0.30 collision=False
distance=0.00 collision=True

원호 보완 반경을 계산합니다

단위는 m입니다. 선분 검사의 반경을 이만큼 늘립니다.

v,w,dt=0.30,1.0,0.05
bound=abs(v*w)*dt*dt/8
print(f'arc_bound={bound:.8f} m')
print(f'guard_radius={0.15+bound:.8f} m')

실행 결과

arc_bound=0.00009375 m
guard_radius=0.15009375 m

전체 프로젝트를 실행합니다

robotics-m07-path-execution 폴더에서 다음 명령을 실행합니다. 처음에는 TODO 때문에 경로 연결과 관통 검사가 실패합니다. 수정 후 이전 회귀와 새 테스트 24개가 통과하고 fixture 로그에서 open은 15.85초, detour는 27.55초에 SUCCEEDED와 충돌 0인지, blocked와 narrow는 PATH_NOT_FOUND인지 확인합니다. 모두 종료 PWM은 0입니다. 전체 명령은 라이브러리 준비와 종료 시 임시 파일 정리를 포함합니다.

bash check.sh

확인 문제

실습

starter.zip을 별도 폴더에 풀고 planning.py의 parent 연결과 swept_clear의 구간 검사 TODO를 완성합니다. bash check.sh 한 줄로 C 빌드·앞 모듈 회귀·새 테스트 24개·fixture 네 개를 실행합니다. 열린 지도와 우회 지도는 60초 이내 도달·충돌 0·PWM 0이어야 하며 막힌 목표와 좁은 통로는 PATH_NOT_FOUND와 정지여야 합니다. 양 끝점은 안전하지만 중간은 장애물을 관통하는 사례, 원호 보완 반경, 취소·시간 초과·HAL 결함도 검사합니다. 테스트와 기존 모듈 코드는 유지합니다. 미션에서는 같은 확장 위에 지도·경로·설정·실패 분석과 재실행 로그를 제출합니다.

시작 코드·테스트 내려받기

실행 명령

bash check.sh

기대 결과

이전 회귀 통과·새 경로 테스트 24개 통과; open/detour SUCCEEDED·60초 이내·collisions=0·PWM 0; blocked/narrow PATH_NOT_FOUND·PWM 0

모범 답안

모범 답안 내려받기

더 읽기

robot-advanced · robot-adv-09-path-tracking

면접 질문

목표점에 가까워져도 로봇이 흔들리는 상황을 설명해 주시면 됩니다.
실험 기록을 재생할 때 함께 남겨야 할 정보를 설명해 주시면 됩니다.