시간 간격과 자세 적분
75분 안팎
학습 목표
고정 dt에서 직선·회전의 위치를 적분합니다.
개념
속도가 맞아도 위치는 아직 없습니다
관제에 0.10m/s가 표시돼도 어느 지점까지 왔는지는 알 수 없습니다. 속도가 유지된 시간과 시작 자세가 함께 있어야 위치를 계산합니다. 이번 목표는 일정한 dt마다 자세를 갱신하고 직선과 회전 사례로 적분을 검증하는 것입니다. 자세는 map 기준 x·y의 m 값과 방향 theta의 rad 값으로 묶습니다. 이번 입력은 이상적인 몸체 속도이며 바퀴 슬립이 없는 조건을 명시합니다.
시간이 빠진 누적을 찾아냅니다
위치 변화는 속도에 시간을 곱한 값입니다. x에 v를 그대로 더하면 주기가 절반이 될 때 같은 시간 동안 두 배 이동하게 됩니다. 가상 1초를 dt=0.05와 dt=0.10으로 나누면 각각 20회와 10회 갱신합니다. 두 경우 직선 결과는 같아야 합니다. 이 불변 관계를 검사하면 단순히 특정 반복 횟수에서 맞는 코드를 넘어 시간 단위의 일관성을 확인할 수 있습니다.
직선은 시작 방향을 사용합니다
각속도가 0이면 방향이 변하지 않습니다. x 변화는 v·dt·cos(theta), y 변화는 v·dt·sin(theta)입니다. theta=0이면 x만 증가하고 theta=pi/2이면 y만 증가합니다. 선속도가 음수면 바라보는 방향의 반대로 이동합니다. math.sin과 math.cos는 라디안을 받으므로 90을 바로 넣지 않습니다. 사람이 보는 각도는 출력할 때만 변환합니다.
회전 중 이동은 원호입니다
한 구간에서 v와 w가 일정하면 theta_end=theta+w·dt입니다. w가 0이 아닐 때 x 변화는 v/w·(sin(theta_end)−sin(theta)), y 변화는 −v/w·(cos(theta_end)−cos(theta))입니다. 방향을 먼저 갱신하고 그 방향으로 직선 이동시키는 방법은 원호의 끝 방향을 전체 구간에 적용하는 근사입니다. 이번 실습은 구간의 정확한 원호 식을 써서 적분 근사 오차와 제어 주기 영향을 구별합니다.
영점 분기를 따로 둡니다
회전식에는 v/w가 있으므로 w=0이면 나눗셈이 불가능합니다. abs(w)가 1e-9보다 작으면 직선 식을 선택합니다. 매우 작은 회전은 이 실습에서 직선으로 근사하며 각도에는 w·dt를 계속 반영합니다. 이 임계값은 이 데이터 범위를 위한 수치 선택입니다. 분기 없이 계산해 ZeroDivisionError가 나오면 단위나 패키지 문제가 아니라 영점 처리 누락으로 읽습니다.
각도는 이동 계산 뒤 정규화합니다
sin과 cos를 이용해 atan2(sin(theta_end),cos(theta_end))로 방향을 −pi에서 pi 범위의 대표값으로 바꿉니다. 양 끝점은 부동소수점의 부호에 따라 pi 또는 −pi가 나올 수 있으며 같은 방향입니다. 정규화 전에 계산한 theta_end로 원호 이동량을 구합니다. 목표 방향 차이에도 같은 정규화를 적용해야 179도에서 −179도로 갈 때 긴 회전을 명령하는 일을 막을 수 있습니다.
제자리 회전 검사가 알려주는 것
v=0이고 w만 존재하면 x와 y는 변하지 않아야 합니다. theta만 시간에 비례해 바뀝니다. 이 사례에서 위치가 움직이면 원호 식에 기존 x·y를 더하는 부분이나 v 값을 점검합니다. 원점에서 돌기만 하는 검사는 이동 좌표 덮어쓰기를 놓칠 수 있으므로 시작 위치가 (2,3)인 사례도 추가합니다. 기존 위치를 유지해야 한다는 조건이 더 분명해집니다.
분할 불변과 제어 주기는 다릅니다
같은 일정 속도를 원호 식으로 적분하면 한 구간을 나누어도 결과가 거의 같습니다. 하지만 제어기가 구간마다 새 속도를 결정하면 dt를 바꿀 때 입력 이력이 바뀝니다. 앞의 불변은 물리 적분 함수의 검사이고 뒤의 차이는 폐루프 응답의 실험입니다. 주기가 달라도 결과가 같아야 한다는 주장을 목표점 제어까지 그대로 확대하지 않습니다. 두 검사를 구별하면 어떤 층에서 문제가 생겼는지 찾기 쉽습니다.
입력 범위로 재현 비용을 제한합니다
브라우저 한 행은 x y theta v w dt n입니다. 앞 여섯 값은 유한하며 절댓값 1000 이내이고, dt는 0 이상 1 이하입니다. n은 정수 표기로 0 이상 10000 이하입니다. dt=0이나 n=0은 이동하지 않는 경계 사례로 허용합니다. n=0이면 방향도 입력 그대로 반환합니다. 음수 dt는 과거 재생의 의미가 정의되지 않아 ERROR로 거절하며 무한 반복을 만들지 않습니다.
출력 불일치의 위치를 좁힙니다
x·y·theta를 소수점 여섯 자리로 출력합니다. 직선은 맞고 원호만 틀리면 y 식의 앞쪽 음수와 시작 각도 사용을 확인합니다. 매번 원점으로 돌아가면 누적 대신 대입했는지 봅니다. theta가 지나치게 크면 dt를 곱했는지 확인합니다. 작은 1e-16 차이는 부동소수점 삼각함수의 표현일 수 있으므로 함수 테스트는 허용 오차를 두고 화면 출력은 서식을 고정합니다.
명령 기반 위치의 증거를 구별합니다
이상 모델에서 명령 속도와 실제 속도는 같습니다. 그래서 적분 결과를 참값처럼 사용할 수 있습니다. 실물에서 PWM 명령을 같은 식에 넣으면 그것은 명령 기반 예측에 불과합니다. 바퀴 엔코더를 적분한 오도메트리도 미끄러짐이 있으면 참값과 어긋납니다. 위치 자료에 어떤 입력을 적분했는지 써 두어야 뒤 모듈에서 센서 추정과 비교할 수 있습니다.
완료 후 남길 검사 기록
integrate 함수를 완성하고 시작 방향이 다른 직선, 후진, 원호, 제자리 회전, 0회 갱신, 음수 시간을 모두 확인합니다. 원호 v=0.2,w=0.5를 1초 한 번과 0.05초 스무 번으로 비교해 차이가 출력 여섯 자리 안에서 사라지는지 봅니다. 이 기록은 이후 C HAL 위치 갱신의 기준이 됩니다. 더 읽기는 식의 유도와 오도메트리 누적 오차를 보완하며 이 레슨의 입력 계약은 위 조건을 따릅니다.
검산용 노트에는 초기 자세와 최종 자세만 적지 않고 v·w·dt·n을 함께 기록합니다. 같은 끝점이 나와도 서로 다른 구간 입력일 수 있기 때문입니다. 로컬 구현으로 옮길 때 이 네 값이 C 함수에 같은 의미로 전달되는지도 확인합니다.
따라하기
같은 시간을 두 주기로 적분합니다
직선 모델에서 1초를 같은 거리로 계산하는지 확인합니다.
for dt,n in [(0.05,20),(0.10,10)]:
x=0.0
for _ in range(n): x+=0.1*dt
print(f"dt={dt:.2f} x={x:.6f}")실행 결과
dt=0.05 x=0.100000 dt=0.10 x=0.100000
원호 끝점을 구합니다
초기 방향 0에서 원호 v=0.2,w=0.5를 1초 적분합니다.
import math
v,w,dt=0.2,0.5,1.0
end=w*dt
x=v/w*math.sin(end)
y=-v/w*(math.cos(end)-1)
print(f"{x:.6f} {y:.6f} {end:.6f}")실행 결과
0.191770 0.048967 0.500000
짧은 방향 차이를 만듭니다
179도에서 −179도로 향할 때 정규화된 방향 차이를 도로 표시합니다.
import math
angle=math.radians(-179)-math.radians(179)
wrapped=math.atan2(math.sin(angle),math.cos(angle))
print(f"heading_error={math.degrees(wrapped):.3f} deg")실행 결과
heading_error=2.000 deg
확인 문제
실습
한 행 x y theta v w dt n을 읽습니다. 앞 여섯 숫자는 유한하고 절댓값 1000 이하, theta는 rad, v는 m/s, w는 rad/s입니다. dt는 0~1초, n은 정수 표기 0~10000입니다. integrate의 고정 속도 원호 적분을 완성하고 매 갱신 뒤 theta를 atan2로 정규화합니다. abs(w)가 1e-9 미만이면 직선 분기를 쓰고 theta에는 w·dt를 반영합니다. n=0은 입력 자세 그대로, dt=0은 이동 없이 정규화만 합니다. 출력 x y theta는 여섯 자리입니다. 계약 오류는 ERROR, 빈 파일은 출력 없음입니다.
모범 답안
import sys, math
def integrate(x,y,t,v,w,dt,n):
for _ in range(n):
end=t+w*dt
if abs(w)<1e-9:
x+=v*dt*math.cos(t);y+=v*dt*math.sin(t)
else:
x+=v/w*(math.sin(end)-math.sin(t))
y-=v/w*(math.cos(end)-math.cos(t))
t=math.atan2(math.sin(end),math.cos(end))
return x,y,t
for line in sys.stdin:
try:
parts=line.split()
if len(parts)!=7: raise ValueError()
x,y,t,v,w,dt=map(float,parts[:6]);n=int(parts[6])
if not all(math.isfinite(a) and abs(a)<=1000 for a in (x,y,t,v,w,dt)) or not 0<=dt<=1 or not 0<=n<=10000: raise ValueError()
result=integrate(x,y,t,v,w,dt,n)
print(" ".join(f"{(0.0 if abs(a)<0.0000005 else a):.6f}" for a in result))
except (ValueError,OverflowError): print("ERROR")
더 읽기
면접 질문
- 목표점에 가까워져도 로봇이 흔들리는 상황을 설명해 주시면 됩니다.