간단한 자율 주행 - 목표점으로 가기
이 장에서 배우는 것
로봇이 목표 좌표를 알고 있다는 사실만으로 그곳에 도착하지는 않는다. 목표가 왼쪽에 있는데 바퀴를 곧장 굴리면 목표에서 멀어질 수도 있다. 목표를 향해 잘 움직이던 중에도 앞을 가로막는 물체가 나타나면 행동을 바꿔야 한다. 이번에는 현재 위치와 방향, 목표 좌표, 전방 거리 배열을 받아 로봇의 전진 속도와 회전 속도를 정하는 작은 주행기를 만든다.
앞 장에서 다룬 거리 배열을 이번에는 행동 선택에 사용한다. 점 구름을 그리거나 지도를 만드는 대신, 전방과 좌우 구역의 최소 거리를 계산한다. 목표 방향 오차에 따른 주행과 거리 조건에 따른 회피를 함께 구현하고, 어떤 조건이 다른 조건보다 먼저 적용되어야 하는지 확인한다.
- 목표까지의 거리와 목표 방향 오차를 계산한다.
- 방향이 크게 어긋나면 먼저 회전하고, 방향이 맞으면 전진한다.
- 전방 거리와 좌우 여유 공간으로 정지 또는 회피 회전을 선택한다.
- 회피 진입과 해제 조건을 구분하여 행동이 자주 바뀌는 현상을 줄인다.
- 결정적인 센서 입력과 위치 갱신으로 주행 규칙을 실행하고 검증한다.
문제 상황
작은 차동 구동 로봇을 작업대 옆 대기점으로 보내려 한다. 현재 위치는 원점이고 목표는 오른쪽으로 1.0m, 위쪽으로 0.6m 떨어져 있다. 그런데 로봇은 목표 반대편에 가까운 방향을 보고 있다. 목표까지 거리가 남았다는 이유만으로 전진 명령을 주면 처음부터 엉뚱한 쪽으로 움직인다.
방향을 맞춘 뒤에도 별도의 문제가 생긴다. 사람이 전방을 잠시 가리거나 센서가 유효하지 않은 값을 반환할 수 있다. 목표 추종식 하나만 계속 적용하면 이러한 사건이 속도 명령에 반영되지 않는다. 목표로 가려는 행동보다 앞서 적용할 정지 조건과 회피 조건이 필요하다.
이번 프로그램은 실제 하드웨어 대신 평면상의 자세를 갱신한다. 자세는 외부에서 정확하게 주어진다고 가정하며, 위치를 추정하는 과정은 다루지 않는다. 출력은 로봇 중심의 전진 속도와 회전 속도다. 이 속도를 바퀴 명령으로 바꾸는 부분도 주행 규칙과 분리한다.
장애물 입력은 시각에 따라 미리 정해 둔 시험용 거리 배열이다. 일정 구간에서는 전방이 가려지고, 한 번은 잘못된 측정값이 들어온다. 이것은 기하학적 장애물 세계를 재현하는 모형이 아니다. 목표 추종, 회피 진입과 해제, 센서 이상 정지가 하나의 반복문에서 작동하는지 확인하는 시험 장치다.
목표 방향 오차로 속도 정하기
세계 좌표계에서 로봇 위치를 (x, y), 목표를 (g_x, g_y), 로봇 방향을 θ라고 하자. 방향 0은 세계 좌표계의 x축 양의 방향이며, 반시계 방향 회전을 양수로 정한다. 길이는 m, 각도는 rad, 시간은 s로 통일한다. 센서 각도는 로봇의 정면을 0으로 삼으며, 왼쪽이 양수다.
dx = g_x - x
dy = g_y - y
rho = np.hypot(dx, dy)
target_angle = np.arctan2(dy, dx)
alpha = wrap_angle(target_angle - theta)ρ는 목표까지의 직선거리이고 α는 현재 방향에서 목표 방향까지 돌아야 하는 각도다. np.arctan2는 두 성분의 부호를 함께 사용하므로 목표가 어느 사분면에 있더라도 방향을 구할 수 있다. 목표 방향을 구할 때 두 인수의 순서는 y성분, x성분이다.
각도를 단순히 빼면 회전량이 불필요하게 커질 수 있다. 현재 방향이 179°이고 목표 방향이 −179°라면 실제로는 반시계 방향으로 2°만 돌면 된다. 두 수를 그대로 빼서 얻은 −358°는 이 의도를 표현하지 못한다. 따라서 각도 차이를 −π 이상 π 미만의 범위로 정규화한다. 정확히 반대 방향인 경우에는 −π를 선택하므로 시계 방향으로 돈다.
회전 속도는 ω = k_α α로 정하고 허용 범위 안으로 제한한다. 오차가 양수이면 왼쪽으로, 음수이면 오른쪽으로 돈다. 전진 속도는 목표에 가까워질수록 줄인다. 다만 목표가 거의 옆이나 뒤에 있을 때 전진까지 허용하면 크게 돌아가는 궤적이 생기기 쉽다. 예제에서는 방향 오차가 35°보다 크면 전진 속도를 0으로 두고 제자리에서 방향을 맞춘다.
방향 오차가 작을 때의 전진 속도는 min(v_max, k_ρ ρ) cos(α)로 정한다. 목표까지 거리가 줄면 기본 속도가 내려가고, 목표 방향에서 조금 벗어나 있으면 전진량을 추가로 줄인다. 이 분기에서는 각도 크기가 35° 이하이므로 코사인 값이 양수다. 코드의 max(0.0, …)는 이후 임계각을 바꾸더라도 음의 전진 명령이 생기지 않게 한다.
| 값 | 의미 | 단위 | 예제 설정 |
|---|---|---|---|
| ρ | 목표까지의 거리 | m | 0.08 이하이면 도착 |
| α | 정규화한 목표 방향 오차 | rad | 크기가 35°를 넘으면 방향 정렬 |
| v | 로봇 중심의 전진 속도 | m/s | 최대 0.35 |
| ω | 로봇 중심의 회전 속도 | rad/s | 크기 최대 1.20 |
도착은 좌표가 정확히 일치하는 사건으로 정의하지 않는다. 부동소수점 계산과 이산적인 이동에서는 정확한 일치를 기대하기 어렵기 때문이다. 목표까지 거리가 0.08m 이하이면 도착으로 판단하고 두 속도를 0으로 만든다. 이번 목표는 위치 도달이므로 도착 후의 최종 방향은 지정하지 않는다.
거리 구역으로 회피 행동 선택하기
예제 센서는 −90°부터 90°까지 10° 간격으로 거리 19개를 반환한다. 각도 크기가 25° 이하인 측정은 전방, 25°보다 큰 양의 각도는 왼쪽, −25°보다 작은 각도는 오른쪽으로 묶는다. 실제 표본은 10° 간격이므로 전방에는 −20°부터 20°까지의 다섯 값이 들어간다.
각 구역의 대표값은 평균이 아닌 최소 거리다. 전방에 가까운 물체가 하나 있고 나머지는 멀리 비어 있다면 평균이 가까운 물체의 존재를 약하게 만들 수 있다. 최소값은 좁은 장애물에도 반응하지만 단발성 이상값에도 민감하다. 여기서는 필터를 추가하지 않고, 입력 유효성 검사와 주행 규칙의 역할을 분명하게 구분한다.
거리는 로봇 외곽으로부터의 간격이 아니라 센서 원점으로부터의 측정값이다. 따라서 실제 장비에서 임계값을 정할 때는 센서 장착 위치, 차체 크기, 멈추는 동안 이동하는 거리까지 고려해야 한다. 아래 수치는 프로그램 동작을 살펴보기 위한 예제 설정이다.
전방 최소 거리가 0.22m 이하이면 회전까지 멈춘다. 그보다 멀지만 0.55m 미만이면 전진을 멈추고 여유 공간이 큰 쪽으로 회전한다. 왼쪽과 오른쪽 값이 같으면 왼쪽을 선택한다. 방향을 선택한 뒤에는 매번 다시 비교하지 않고, 회피를 끝낼 때까지 그 방향을 기억한다.
회피를 시작하는 경계와 끝내는 경계를 다르게 두는 방식을 히스테리시스(hysteresis)라고 한다. 예제에서는 0.55m 미만일 때 회피에 들어가지만, 이미 회피 중이면 전방 거리가 0.70m 이상이 될 때까지 회전을 유지한다. 하나의 경계만 사용하면 거리가 그 근처에서 조금씩 달라질 때 전진과 회전이 반복해서 바뀔 수 있다.
센서 이상은 빈 공간과 구분한다. 예제의 입력 계약에서는 양의 무한대만 최대 측정거리 안에 반사체가 없다는 뜻으로 허용한다. 이를 3.0m로 바꿔 계산한다. NaN, 음의 무한대, 0.02m 미만 또는 3.0m를 넘는 유한한 값은 잘못된 입력으로 보고 정지한다. 실제 센서가 다른 의미로 값을 반환한다면 입력을 변환하는 부분부터 맞춰야 한다.
회피 중 센서 이상이나 근접 정지가 발생하면 그 순간의 속도는 모두 0이다. 그러나 기억한 회전 방향은 지우지 않는다. 다음 입력이 정상으로 돌아왔을 때 전방이 아직 충분히 열리지 않았다면 같은 방향의 회피를 이어 간다. 정지라는 출력과 회피 방향이라는 내부 상태를 나누어 생각해야 하는 이유다.
| 순서 | 조건 | 행동 | 회피 상태 |
|---|---|---|---|
| 1 | 목표 거리 0.08m 이하 | 도착, 두 속도 0 | 해제 |
| 2 | 거리 배열이 유효하지 않음 | 센서 정지 | 유지 |
| 3 | 전방 거리 0.22m 이하 | 근접 정지 | 유지 |
| 4 | 회피 유지 또는 진입 조건 충족 | 선택한 방향으로 제자리 회전 | 기억 |
| 5 | 나머지 조건 | 방향 정렬 또는 목표 추종 | 해제 |
반복 실행과 검증 범위 정하기
주행기는 0.05초마다 자세와 거리 배열을 읽고 행동 하나를 선택한다. 선택한 속도는 그 시간 간격 동안 일정하다고 가정한다. 위치는 현재 방향으로 전진 속도에 시간 간격을 곱한 만큼 옮기고, 방향은 회전 속도에 시간 간격을 곱한 만큼 바꾼다. 여기서는 간단한 전진 오일러 근사를 사용한다. 시간 간격을 크게 늘리면 같은 제어식이어도 이동 근사와 반응 시점이 달라진다.
검증은 두 부분으로 나눈다. 먼저 정해진 자세와 거리 배열에 대해 전진, 회피, 근접 정지, 센서 정지, 도착 명령이 나오는지 확인한다. 이어서 회피 경계 사이에서는 회피가 유지되고, 해제 거리에서는 목표 추종으로 돌아오는지 검사한다. 마지막으로 시간에 따라 입력을 바꾸는 주행 반복문을 실행한다.
반복문의 20번째부터 39번째 갱신에는 전방 거리를 0.40m로 만든다. 60번째 갱신에는 가운데 측정 하나에 NaN을 넣는다. 나머지는 모든 방향이 3.0m로 열린 입력이다. 이 구성에서는 가림이 사라진 뒤 목표로 갈 수 있다. 난수를 쓰지 않으므로 시드에 의존하는 부분도 없으며 같은 입력과 출력이 반복된다.
이 시험의 도착은 임의의 장애물 배치에서 도착한다는 보장이 아니다. 정적인 벽이 계속 앞을 막거나 좁은 공간에 들어가면 제자리 회전 또는 정지가 이어질 수 있다. 프로그램은 최대 반복 횟수를 두어 이런 상황을 끝없이 실행하지 않으며, 이번에 정의한 시험에서 도착하지 못하면 실패를 알린다.
완성 코드
다음 내용을 simple_nav.py로 저장한다. Python 3와 numpy가 필요하며 다른 외부 패키지는 사용하지 않는다. 자세와 목표는 유효한 유한 실수라는 전제로 받는다. 센서 배열은 주행 중 잘못될 수 있는 입력으로 취급하여 매번 검사한다.
import numpy as np
DT = 0.05
MAX_STEPS = 1200
GOAL_TOL = 0.08
MAX_RANGE = 3.0
STOP_DISTANCE = 0.22
AVOID_DISTANCE = 0.55
CLEAR_DISTANCE = 0.70
MAX_V = 0.35
MAX_W = 1.20
TURN_W = 0.90
K_RHO = 0.80
K_ALPHA = 2.00
ALIGN_ANGLE = np.deg2rad(35.0)
ANGLES = np.deg2rad(np.arange(-90.0, 91.0, 10.0))
FRONT = np.abs(ANGLES) <= np.deg2rad(25.0)
LEFT = ANGLES > np.deg2rad(25.0)
RIGHT = ANGLES < -np.deg2rad(25.0)
def wrap_angle(angle):
return float((angle + np.pi) % (2.0 * np.pi) - np.pi)
def sector_minima(scan):
values = np.asarray(scan, dtype=float)
if values.shape != ANGLES.shape:
return None
values = np.where(np.isposinf(values), MAX_RANGE, values)
if not np.all(np.isfinite(values)):
return None
if np.any(values < 0.02) or np.any(values > MAX_RANGE):
return None
front = float(np.min(values[FRONT]))
left = float(np.min(values[LEFT]))
right = float(np.min(values[RIGHT]))
return front, left, right
class Navigator:
def __init__(self):
self.turn_sign = 0
def command(self, pose, goal, scan):
x, y, theta = pose
dx = float(goal[0] - x)
dy = float(goal[1] - y)
rho = float(np.hypot(dx, dy))
if rho <= GOAL_TOL:
self.turn_sign = 0
return "도착", 0.0, 0.0
distances = sector_minima(scan)
if distances is None:
return "센서 정지", 0.0, 0.0
front, left, right = distances
if front <= STOP_DISTANCE:
return "근접 정지", 0.0, 0.0
if self.turn_sign != 0 and front >= CLEAR_DISTANCE:
self.turn_sign = 0
if self.turn_sign == 0 and front < AVOID_DISTANCE:
self.turn_sign = 1 if left >= right else -1
if self.turn_sign != 0:
return "회피", 0.0, self.turn_sign * TURN_W
alpha = wrap_angle(np.arctan2(dy, dx) - theta)
w = float(np.clip(K_ALPHA * alpha, -MAX_W, MAX_W))
if abs(alpha) > ALIGN_ANGLE:
return "방향 정렬", 0.0, w
v = min(MAX_V, K_RHO * rho) * max(0.0, float(np.cos(alpha)))
return "전진", v, w
def make_scan(front=MAX_RANGE, left=MAX_RANGE, right=MAX_RANGE):
scan = np.full(ANGLES.shape, MAX_RANGE, dtype=float)
scan[FRONT] = front
scan[LEFT] = left
scan[RIGHT] = right
return scan
def require(condition, message):
if not condition:
raise RuntimeError(message)
def check_rules():
pose = np.array([0.0, 0.0, 0.0])
goal = np.array([1.0, 0.0])
bad_scan = make_scan()
bad_scan[len(ANGLES) // 2] = np.nan
cases = [
("열린 전방", goal, make_scan(), "전진", 0.35, 0.0),
("전방 가림", goal, make_scan(0.40, 1.20, 0.80),
"회피", 0.0, 0.90),
("가까운 물체", goal, make_scan(0.15), "근접 정지", 0.0, 0.0),
("잘못된 측정", goal, bad_scan, "센서 정지", 0.0, 0.0),
("목표 위치", pose[:2], make_scan(), "도착", 0.0, 0.0),
]
for name, target, scan, expected_mode, expected_v, expected_w in cases:
mode, v, w = Navigator().command(pose, target, scan)
require(mode == expected_mode, name + ": 행동 불일치")
require(np.allclose([v, w], [expected_v, expected_w]),
name + ": 속도 불일치")
print(f"{name}: {mode}, v={v:.2f}, w={w:.2f}")
nav = Navigator()
nav.command(pose, goal, make_scan(0.40, 1.20, 0.80))
mode, _, w = nav.command(pose, goal, make_scan(0.60, 0.80, 1.20))
require(mode == "회피" and w == TURN_W, "회피 방향 유지 실패")
mode, _, _ = nav.command(pose, goal, make_scan(CLEAR_DISTANCE))
require(mode == "전진", "회피 해제 실패")
mode, _, _ = Navigator().command(pose, goal, np.full(19, np.inf))
require(mode == "전진", "무반사 입력 처리 실패")
print("경계와 상태 검사: 통과")
def run_demo():
pose = np.array([0.0, 0.0, np.deg2rad(170.0)])
goal = np.array([1.0, 0.6])
nav = Navigator()
visited = set()
arrived = False
for step in range(MAX_STEPS):
scan = make_scan()
if 20 <= step < 40:
scan = make_scan(0.40, 1.20, 0.80)
if step == 60:
scan[len(ANGLES) // 2] = np.nan
mode, v, w = nav.command(pose, goal, scan)
visited.add(mode)
if mode == "도착":
arrived = True
break
theta = float(pose[2])
pose[0] += v * np.cos(theta) * DT
pose[1] += v * np.sin(theta) * DT
pose[2] = wrap_angle(theta + w * DT)
require(arrived, "제한 시간 안에 도착하지 못했다")
require({"회피", "센서 정지", "방향 정렬"}.issubset(visited),
"필수 행동을 모두 거치지 않았다")
print("주행 결과: 도착")
print("회피·센서 정지·방향 정렬: 모두 확인")
if __name__ == "__main__":
check_rules()
run_demo()줄별 해설
상수와 거리 배열의 계약
import numpy as np는 벡터 계산과 수학 함수를 가져온다. DT와 MAX_STEPS는 각각 갱신 간격과 실행 한도다. GOAL_TOL부터 ALIGN_ANGLE까지는 도착, 거리 판단, 속도 제한에 사용하는 설정이다. K_RHO의 단위는 1/s이며 K_ALPHA도 각도 오차를 회전 속도로 바꾸는 1/s 단위의 이득이다. 속도 제한과 이득은 서로 다른 역할을 한다.
ANGLES는 센서 표본의 방향이고 FRONT, LEFT, RIGHT는 같은 길이의 참·거짓 배열이다. 이 배열로 거리값을 선택하므로 센서 각도와 거리의 순서가 일치해야 한다. wrap_angle의 나머지 연산은 각도를 한 바퀴 구간으로 옮긴 뒤 π만큼 내려 원하는 범위에 넣는다. float 변환은 함수 밖으로 일반 Python 실수를 반환하기 위한 것이다.
sector_minima는 입력을 실수 배열로 바꾼 뒤 모양을 검사한다. 원소 개수만 같고 형태가 다른 이차원 배열도 거부한다. 다음 줄의 np.where는 허용한 양의 무한대만 최대 거리로 치환한다. 유한성 검사와 범위 검사를 통과한 뒤에만 np.min을 호출하므로 NaN이 구역 최소값에 섞이지 않는다. 세 구역의 최소값은 전방, 왼쪽, 오른쪽 순서로 반환한다.
행동 하나를 고르는 command
Navigator의 생성자는 turn_sign을 0으로 둔다. 0은 회피하지 않음을, 1과 −1은 각각 왼쪽과 오른쪽 회피를 뜻한다. command는 자세와 목표에서 거리부터 계산한다. 도착했다면 상태를 지우고 즉시 반환하므로 이후 센서 조건이나 방향 계산이 도착 명령을 덮어쓰지 않는다.
distances가 None이면 센서 정지를 반환한다. 정상 입력이면 세 최소값을 분리하고 근접 정지를 검사한다. 이 두 정지 분기에서는 turn_sign을 수정하지 않는다. 그 다음에야 회피 해제와 진입을 차례로 판단한다. 이미 회피 중이고 전방이 0.70m 이상 열렸으면 상태를 지우며, 회피하지 않는 상태에서 0.55m 미만이면 방향을 새로 선택한다.
turn_sign이 남아 있으면 목표 방향을 계산하기 전에 회피 명령을 반환한다. 따라서 회피 중 목표가 반대쪽에 있어도 선택한 회전 방향이 즉시 뒤집히지 않는다. 나머지 입력만 목표 추종식으로 넘어간다. np.clip은 회전 속도를 제한하고, 방향 정렬 분기는 전진만 금지한다. 마지막 반환문은 거리와 각도를 반영한 전진 명령이다.
입력을 만들고 결과를 검사하는 함수
make_scan은 매번 새 배열을 만들고 구역별 거리값을 채운다. 호출 사이에 배열을 공유하지 않으므로 한 시험에서 NaN을 넣어도 다른 시험 입력이 변하지 않는다. require는 조건이 거짓이면 설명과 함께 실행을 중단한다. 검사가 실패했는데 성공 문장이 이어서 출력되는 일을 막는다.
check_rules의 cases에는 입력과 예상 행동, 예상 속도를 나란히 적었다. 각 사례는 새 Navigator로 실행하여 이전 사례의 회피 상태가 섞이지 않게 한다. 문자열 행동은 같음으로 비교하고, 계산한 속도는 np.allclose로 비교한다. 출력은 소수점 둘째 자리까지 고정하므로 부동소수점의 긴 표현이 드러나지 않는다.
이어서 같은 주행기 객체에 여러 입력을 넣어 상태 유지 여부를 검사한다. 전방 거리가 0.60m가 되고 오른쪽이 더 넓어져도 왼쪽 회피를 유지해야 한다. 다음 입력이 해제 거리와 같아지면 목표 추종으로 돌아온다. 양의 무한대 배열을 사용하는 검사는 무반사 입력에 대해 정지하지 않는다는 계약을 확인한다.
주행 반복문과 종료
run_demo는 방향 170°에서 시작한다. visited는 실행 중 선택된 행동을 모으는 집합이며, 순서를 출력하지 않고 포함 여부만 검사한다. 센서 배열은 갱신 번호에 따라 바뀐다. command가 도착을 반환하면 자세를 다시 이동시키지 않고 반복문을 끝낸다.
theta에는 갱신 전 방향을 저장한다. x와 y를 모두 같은 이전 방향으로 계산한 다음 θ를 갱신해야 한 번의 시간 단계가 일관된다. 마지막 검사들은 도착 여부와 필수 행동의 실행 여부를 확인한다. 파일 끝의 조건문은 이 파일을 직접 실행했을 때 규칙 검사와 주행을 차례로 호출한다.
실행 결과
numpy가 준비된 환경에서 다음 두 명령을 실행한다. 첫 명령은 경고를 오류로 취급하여 문법을 컴파일하며, 성공하면 출력하지 않는다. 두 번째 명령도 경고를 오류로 처리하면서 프로그램을 실행한다.
python3 -W error -m py_compile simple_nav.py
python3 -W error simple_nav.py예상 표준 출력은 다음과 같다.
열린 전방: 전진, v=0.35, w=0.00
전방 가림: 회피, v=0.00, w=0.90
가까운 물체: 근접 정지, v=0.00, w=0.00
잘못된 측정: 센서 정지, v=0.00, w=0.00
목표 위치: 도착, v=0.00, w=0.00
경계와 상태 검사: 통과
주행 결과: 도착
회피·센서 정지·방향 정렬: 모두 확인앞의 다섯 줄은 서로 독립된 입력에 대한 명령이다. 실제 주행에서 그 순서로 행동했다는 기록은 아니다. 뒤의 주행 결과는 별도로 실행한 반복문의 검사 결과다. 도착은 목표와의 거리가 허용 반경 안에 들어왔다는 뜻이며, 좌표의 정확한 일치나 장애물 충돌 검사의 성공을 뜻하지 않는다.
실무에서 자주 틀리는 것
각도 차이를 정규화하지 않는다
다음 코드는 각도의 경계를 가로지르는 목표에 대해 긴 방향으로 회전 명령을 만들 수 있다.
alpha = target_angle - theta
w = K_ALPHA * alpha차이를 정규화하고 회전 속도도 제한한다. 정규화는 회전 방향과 거리를 바로잡고, 제한은 구동기에 요구하는 명령 크기를 줄인다.
alpha = wrap_angle(target_angle - theta)
w = float(np.clip(K_ALPHA * alpha, -MAX_W, MAX_W))목표 추종 명령이 회피 명령을 덮어쓴다
아래 코드는 회피 조건을 검사하지만 곧이어 전진 속도를 다시 대입한다. 조건을 작성했다는 사실만으로 그 행동이 최종 명령에 남지는 않는다.
if front < AVOID_DISTANCE:
v = 0.0
w = TURN_W
v = min(MAX_V, K_RHO * rho)조건을 만족하면 즉시 반환하거나 배타적인 분기로 구성한다. 다음은 행동의 우선순위를 보여 주는 축약 코드이며, 완성 코드에서는 기억한 회피 방향을 사용한다.
if front < AVOID_DISTANCE:
return "회피", 0.0, TURN_W
v = min(MAX_V, K_RHO * rho)잘못된 측정값을 빈 공간으로 바꾼다
NaN을 최대 거리로 바꾸면 센서가 판단하지 못한 방향을 주행 가능한 공간으로 해석한다. 가까운 물체를 관측하지 못한 상황에서도 전진할 수 있다.
values = np.nan_to_num(scan, nan=MAX_RANGE)
front = float(np.min(values[FRONT]))허용한 무반사 값만 변환하고 나머지 이상값은 정지 판단으로 전달한다. 아래 코드는 sector_minima 안에서 사용하는 검사 부분이다. 이 검사 뒤에는 거리 범위 검사도 필요하다.
values = np.asarray(scan, dtype=float)
values = np.where(np.isposinf(values), MAX_RANGE, values)
if not np.all(np.isfinite(values)):
return None반복할 때마다 주행기를 새로 만든다
아래 형태에서는 회피 방향과 진입 여부가 매번 초기화된다. 전방이 0.60m로 바뀌면 이미 회피 중이었다는 정보를 잃어버린다.
for scan in scans:
nav = Navigator()
mode, v, w = nav.command(pose, goal, scan)주행기는 반복문 밖에서 한 번 만들고 재사용한다. 입력마다 새 객체를 만드는 방식은 독립적인 규칙 검사에만 사용한다.
nav = Navigator()
for scan in scans:
mode, v, w = nav.command(pose, goal, scan)한눈에 보기
| 항목 | 구현 | 주의할 경계 |
|---|---|---|
| 목표 추종 | 거리로 전진량, 방향 오차로 회전량 결정 | 현재 자세가 올바르게 주어진다는 가정 |
| 방향 정렬 | 오차 크기가 35°를 넘으면 제자리 회전 | 도착 후 최종 방향은 지정하지 않음 |
| 회피 | 좌우 중 더 넓은 쪽으로 회전 | 지역적인 거리 비교이며 경로 탐색은 아님 |
| 상태 유지 | 0.55m에서 진입, 0.70m에서 해제 | 주행기 객체를 반복해서 재사용 |
| 입력 이상 | 잘못된 거리 배열이면 두 속도 0 | 무반사 값의 의미는 센서마다 확인 |
| 실행 검증 | 규칙별 명령과 정해진 입력의 도착 검사 | 실제 장애물 형상과 충돌은 검증하지 않음 |
실제 로봇에서는 오래된 거리 배열인지도 확인해야 한다. 숫자가 유효해도 측정 시각이 오래되었다면 현재 전방을 설명하지 못한다. 또한 제자리 회전 중에는 차체 옆부분이 물체와 만날 수 있으므로 전방 최소 거리만으로 회전 공간을 판단할 수 없다. 이 예제의 좌우 비교는 회전 방향을 고르는 규칙이지 차체 전체의 통과 가능성을 검사하는 방법은 아니다.
목표 오차 기반 제어는 가까운 목표를 향한 행동을 만들고, 회피 규칙은 당장의 전방 상황에 반응한다. 두 역할을 나누고 우선순위와 상태를 명시하면 코드에서 어떤 판단이 내려졌는지 추적하기 쉽다. 다음 실습에서는 이 단일 목표 주행기를 바탕으로 여러 목표를 순서대로 처리하게 된다.
연습 문제
- 현재 방향이 170°이고 목표 방향이 −170°다. 정규화한 방향 오차를 도 단위로 구하고 회전 방향을 설명하라. K_ALPHA가 2.0이고 MAX_W가 1.20일 때 회전 속도도 계산하라.
- 이미 왼쪽 회피 중이다. 전방 거리가 차례로 0.54m, 0.61m, 0.69m, 0.70m가 된다. 각 입력에서 회피 상태가 유지되는지 설명하라. 처음부터 회피하지 않던 상태에 0.61m가 들어오면 무엇이 다른지도 설명하라.
- 왼쪽 여유 거리가 0.80m, 오른쪽이 1.20m이고 전방이 0.40m인 검사 사례를 추가하라. 이어서 전방을 0.20m로 바꾸었을 때 어떤 명령이 나와야 하는지 적어라.
- 센서가 세 번 연속 잘못된 값을 내면 주행 시도를 종료하려 한다. 정상 측정이 들어오면 연속 횟수를 초기화하는 코드를 작성하고, 이 판단을 command 안과 바깥 중 어디에 둘지 설명하라.
정답과 해설
-
단순한 차이는 −340°이며 정규화한 결과는 +20°다. 반시계 방향으로 회전한다. 라디안 값은 약 0.3491이고, 회전 속도는 약 0.6981rad/s다. 제한값보다 작으므로 그대로 사용한다. 방향 오차가 35°보다 작으므로 다른 정지 조건이 없다면 전진도 허용된다.
-
앞의 세 입력에서는 왼쪽 회피를 유지한다. 이미 회피 중이면 진입 거리보다 멀어졌다는 사실만으로 해제하지 않는다. 0.70m에서는 해제 조건을 만족하여 목표 추종 판단으로 넘어간다. 처음부터 회피하지 않았다면 0.61m는 진입 조건을 만족하지 않으므로 회피를 시작하지 않는다. 같은 측정값에서도 이전 상태에 따라 행동이 달라지는 것이 두 경계를 둔 효과다.
-
오른쪽이 더 넓으므로 전진 속도는 0이고 회전 속도는 −0.90rad/s다. 전방이 0.20m이면 근접 정지가 회피보다 먼저 적용되어 두 속도가 모두 0이 된다. 다음 항목을 cases에 추가하면 오른쪽 회피 명령을 검사할 수 있다.
("오른쪽 회피", goal, make_scan(0.40, 0.80, 1.20), "회피", 0.0, -0.90), -
한 번의 이상 측정에 대한 정지 명령은 command가 담당하고, 반복된 이상 때문에 주행 시도를 종료하는 판단은 바깥 반복문에 둘 수 있다. 반복문 전에 bad_count를 0으로 초기화한다. 다음 코드는 command 호출 직후, 자세 갱신 전에 들어가는 부분이다.
if mode == "센서 정지": bad_count += 1 else: bad_count = 0 if bad_count >= 3: print("주행 종료: 센서 이상 반복") break예제에서는 이상 입력이 한 번뿐이므로 이 종료 조건에 걸리지 않는다. 센서 이상으로 끝난 결과를 도착과 구분하려면 별도의 종료 사유도 저장해야 한다. 실제 장비에 연결할 때는 반복문을 끝내기 전에 정지 명령을 구동부에 전달하는 경로까지 마련해야 한다.