경로 계획은 '좋은 길'을 고르는 문제다
로봇이 출발점에서 목표점까지 가야 한다고 해 보자. 가장 먼저 떠오르는 길은 직선이다. 실제로 장애물이 없다면 직선은 훌륭한 선택이다. 짧고, 단순하고, 계산하기 쉽다. 하지만 현실의 로봇은 텅 빈 종이 위를 움직이지 않는다. 책상 다리, 벽, 사람, 다른 로봇, 좁은 통로 같은 것들이 길 위에 놓여 있다.
그래서 로봇의 경로 계획은 단순히 "목표로 가라"가 아니다. 더 정확히 말하면 다음 질문에 답하는 과정이다.
"수많은 가능한 경로 중에서 어떤 경로가 가장 좋은가?"
여기서 "좋다"는 말은 상황에 따라 달라진다. 배터리를 아끼고 싶다면 짧은 경로가 좋다. 물건을 싣고 움직이는 로봇이라면 급격히 꺾지 않는 부드러운 경로가 좋다. 사람 옆을 지나가는 서비스 로봇이라면 조금 돌아가더라도 안전거리를 확보하는 경로가 좋다.
이 장에서는 경로의 좋고 나쁨을 숫자로 평가한다. 이 숫자를 비용이라고 부른다. 비용이 작을수록 좋은 경로라고 약속한다. 그러면 경로 계획은 자연스럽게 최적화 문제가 된다.
좋은 경로 찾기 = 비용이 가장 작은 경로 찾기앞 장에서 우리는 경사하강법을 통해 비용을 줄이는 방향으로 값을 조금씩 바꾸는 방법을 배웠다. 이번 장에서는 그 감각을 경로에 적용한다. 다만 처음부터 복잡한 알고리즘으로 들어가지는 않는다. 경로를 점들의 배열로 표현하고, 그 경로가 얼마나 짧은지, 얼마나 부드러운지, 장애물에서 얼마나 떨어져 있는지를 차근차근 비용으로 만들어 보자.
경로를 점들의 배열로 표현하기
2D 평면에서 로봇 경로는 여러 개의 점으로 표현할 수 있다. 출발점, 중간 경유점, 목표점을 순서대로 나열하면 하나의 경로가 된다.
예를 들어 다음 경로를 생각해 보자.
import numpy as np
path = np.array([
[0.0, 0.0],
[1.0, 0.5],
[2.0, 1.0],
[3.0, 0.8],
[4.0, 0.0],
])각 행은 하나의 위치다. 첫 번째 열은 x좌표, 두 번째 열은 y좌표다. 이 경로는 (0, 0)에서 출발해 몇 개의 중간점을 거쳐 (4, 0)으로 이동한다.
이렇게 경로를 점 배열로 표현하면 장점이 많다. NumPy를 이용해 점 사이 거리, 방향, 장애물까지의 거리 등을 한 번에 계산할 수 있다. 즉, 경로를 "그림"이 아니라 "계산 가능한 데이터"로 다룰 수 있다.
먼저 경로를 그리는 함수를 준비하자.
import matplotlib.pyplot as plt
def plot_path(path, obstacles=None, title="path"):
plt.figure(figsize=(7, 5))
plt.plot(path[:, 0], path[:, 1], "o-", label="path")
plt.scatter(path[0, 0], path[0, 1], s=120, marker="s", label="start")
plt.scatter(path[-1, 0], path[-1, 1], s=120, marker="*", label="goal")
if obstacles is not None:
ax = plt.gca()
for obs in obstacles:
cx, cy = obs["center"]
r = obs["radius"]
circle = plt.Circle((cx, cy), r, color="tomato", alpha=0.35)
ax.add_patch(circle)
plt.scatter(cx, cy, color="red", s=30)
plt.axis("equal")
plt.grid(True)
plt.title(title)
plt.legend()
plt.show()
plot_path(path, title="A path as points")이제 로봇의 길은 눈으로 볼 수 있는 선이면서 동시에 비용 계산에 사용할 수 있는 숫자 배열이 되었다.
짧은 경로: 길이 비용
좋은 경로의 첫 번째 조건은 보통 "짧다"이다. 물론 항상 짧은 경로가 최고는 아니다. 장애물을 아슬아슬하게 스치며 지나가는 짧은 길보다 조금 돌아가더라도 안전한 길이 더 좋을 수 있다. 하지만 다른 조건이 같다면 짧은 경로는 대체로 유리하다. 이동 시간이 줄고, 에너지 사용도 줄고, 오차가 누적될 가능성도 줄어든다.
경로 길이는 인접한 점 사이의 거리를 모두 더해서 계산할 수 있다.
전체 경로 길이 = 점0→점1 거리 + 점1→점2 거리 + 점2→점3 거리 + ...Python으로 구현하면 다음과 같다.
def path_length(path):
diffs = np.diff(path, axis=0)
segment_lengths = np.linalg.norm(diffs, axis=1)
return np.sum(segment_lengths)
length = path_length(path)
print(length)np.diff(path, axis=0)은 연속된 점 사이의 차이를 계산한다. 예를 들어 path[1] - path[0], path[2] - path[1] 같은 벡터들이 나온다. 이 벡터 하나하나가 경로의 작은 구간이다.
np.linalg.norm은 벡터의 크기를 계산한다. 2D에서는 우리가 앞에서 여러 번 본 유클리드 거리와 같다.
거리 = sqrt(dx^2 + dy^2)이제 경로 길이를 비용으로 사용할 수 있다.
def length_cost(path):
return path_length(path)이 비용만 사용하면 로봇은 무조건 짧은 길을 좋아한다. 장애물이 없을 때는 출발점과 목표점을 직선으로 잇는 경로가 가장 좋은 경로가 된다.
다음 세 경로를 비교해 보자.
path_straight = np.array([
[0.0, 0.0],
[1.0, 0.0],
[2.0, 0.0],
[3.0, 0.0],
[4.0, 0.0],
])
path_upper = np.array([
[0.0, 0.0],
[1.0, 0.8],
[2.0, 1.2],
[3.0, 0.8],
[4.0, 0.0],
])
path_zigzag = np.array([
[0.0, 0.0],
[1.0, 0.8],
[2.0, -0.8],
[3.0, 0.8],
[4.0, 0.0],
])
for name, p in [
("straight", path_straight),
("upper", path_upper),
("zigzag", path_zigzag),
]:
print(name, path_length(p))대체로 직선 경로의 길이가 가장 짧고, 위로 돌아가는 경로는 조금 길며, 지그재그 경로는 더 길게 나온다. 이처럼 길이 비용은 "얼마나 돌아갔는가"를 숫자로 드러낸다.
하지만 길이만으로는 충분하지 않다. 지그재그 경로가 조금만 짧게 만들어져도 로봇은 급격히 꺾는 길을 선택할 수 있다. 실제 로봇은 그런 길을 싫어한다. 바퀴가 미끄러질 수 있고, 짐이 흔들릴 수 있고, 제어가 불안정해질 수 있다. 그래서 두 번째 비용이 필요하다.
부드러운 경로: 방향 변화 비용
로봇에게 좋은 경로는 매끈해야 한다. 사람이 걸을 때도 갑자기 왼쪽으로 꺾었다가 바로 오른쪽으로 꺾는 길은 불편하다. 자동차라면 더 심각하다. 핸들을 급하게 돌리는 경로는 위험하고, 속도를 크게 줄여야 한다.
경로의 부드러움을 측정하는 방법은 여러 가지가 있다. 여기서는 가장 직관적인 방법을 사용하자. 각 구간의 방향을 구한 뒤, 연속된 구간 사이에서 방향이 얼마나 바뀌는지 계산한다.
예를 들어 점이 세 개 있다고 해 보자.
A → B → CA → B 방향과 B → C 방향이 거의 같으면 경로는 부드럽다. 두 방향이 크게 다르면 B 지점에서 급하게 꺾는 경로다.
방향 각도는 atan2로 구할 수 있다.
def segment_angles(path):
diffs = np.diff(path, axis=0)
return np.arctan2(diffs[:, 1], diffs[:, 0])다만 각도를 뺄 때는 주의해야 한다. 각도는 원형 값이다. 179도와 -179도는 숫자로는 358도 차이처럼 보이지만 실제 방향은 거의 같다. 그래서 각도 차이를 항상 -π에서 π 사이로 정리하는 함수가 필요하다.
def normalize_angle(angle):
return (angle + np.pi) % (2 * np.pi) - np.pi이제 부드러움 비용을 만들 수 있다.
def smoothness_cost(path):
angles = segment_angles(path)
angle_diffs = normalize_angle(np.diff(angles))
return np.sum(angle_diffs ** 2)여기서 방향 변화량을 그대로 더하지 않고 제곱해서 더했다. 이유는 앞에서 배운 최소제곱법과 비슷하다. 작은 방향 변화는 조금만 벌점을 주고, 큰 방향 변화는 훨씬 크게 벌점을 준다. 즉, 급격한 회전을 강하게 싫어하도록 만드는 것이다.
세 경로의 부드러움 비용을 비교해 보자.
for name, p in [
("straight", path_straight),
("upper", path_upper),
("zigzag", path_zigzag),
]:
print(name, smoothness_cost(p))직선 경로는 방향이 변하지 않으므로 부드러움 비용이 거의 0이다. 위로 둥글게 돌아가는 경로는 방향이 조금씩 바뀌므로 비용이 작다. 반면 지그재그 경로는 방향이 크게 바뀌므로 비용이 커진다.
이 비용 하나만 추가해도 로봇은 "짧지만 덜컹거리는 경로"보다 "조금 길어도 자연스럽게 이어지는 경로"를 더 좋아할 수 있다.
장애물을 피하는 경로: 회피 비용
이제 가장 중요한 조건을 넣어 보자. 로봇은 장애물과 부딪히면 안 된다. 경로가 아무리 짧고 부드러워도 장애물을 통과한다면 좋은 경로가 아니다.
장애물을 원으로 표현해 보자. 원형 장애물은 중심점과 반지름으로 표현할 수 있다.
obstacles = [
{"center": np.array([2.0, 0.0]), "radius": 0.45},
]이 장애물은 (2.0, 0.0) 위치에 있고 반지름이 0.45인 원이다. 만약 로봇이 직선 경로로 (0, 0)에서 (4, 0)까지 간다면 장애물 한가운데를 지나가게 된다.
장애물 회피 비용은 다음 감각으로 만들 수 있다.
장애물과 멀면 비용이 작다.
장애물에 가까우면 비용이 커진다.
장애물 안으로 들어가면 매우 큰 비용을 준다.장애물 중심과 경로점 사이의 거리를 구한 뒤, 장애물 반지름을 빼면 장애물 표면까지의 여유 거리, 즉 clearance를 얻을 수 있다.
clearance = 경로점과 장애물 중심 사이 거리 - 장애물 반지름clearance가 양수면 장애물 밖에 있다. 0이면 표면에 닿아 있다. 음수면 장애물 안에 들어간 것이다.
다음은 장애물 회피 비용 함수다.
def obstacle_cost(path, obstacles, safety_margin=0.6):
total = 0.0
for obs in obstacles:
center = obs["center"]
radius = obs["radius"]
dists = np.linalg.norm(path - center, axis=1)
clearance = dists - radius
collision = clearance < 0.0
total += np.sum(collision) * 1_000.0
near = (clearance >= 0.0) & (clearance < safety_margin)
total += np.sum(((safety_margin - clearance[near]) / safety_margin) ** 2)
return total이 함수에는 두 종류의 벌점이 들어 있다. 첫째, 경로점이 장애물 안에 들어가면 매우 큰 비용을 준다. 둘째, 장애물 밖에 있더라도 안전거리 안으로 들어오면 가까울수록 비용을 더 준다.
여기서 safety_margin은 로봇이 장애물 표면에서 어느 정도 떨어져 지나가길 원하는지 나타낸다. 예를 들어 safety_margin=0.6이라면 장애물 표면에서 0.6m 안쪽으로 가까워질 때부터 벌점이 생긴다.
직선 경로와 위로 돌아가는 경로를 비교해 보자.
for name, p in [
("straight", path_straight),
("upper", path_upper),
("zigzag", path_zigzag),
]:
print(name, obstacle_cost(p, obstacles))장애물이 직선 경로 위에 있다면 straight의 장애물 비용은 매우 커진다. 반면 upper는 조금 돌아가지만 장애물과 거리를 두므로 비용이 작아질 수 있다. 여기서 중요한 변화가 일어난다. 로봇은 더 이상 짧은 길만 고집하지 않는다. 안전하지 않은 짧은 길은 나쁜 길이 된다.
비용을 하나로 합치기
이제 세 가지 비용을 만들었다.
길이 비용: 경로가 얼마나 긴가
부드러움 비용: 방향이 얼마나 급하게 바뀌는가
장애물 비용: 장애물과 얼마나 위험하게 가까운가
경로를 하나의 숫자로 평가하려면 이 비용들을 합쳐야 한다. 가장 간단한 방법은 가중합이다.
전체 비용 = w길이 × 길이 비용 + w부드러움 × 부드러움 비용 + w장애물 × 장애물 비용여기서 w는 가중치다. 가중치는 "무엇을 더 중요하게 볼 것인가"를 정하는 손잡이다.
def total_cost(
path,
obstacles,
w_length=1.0,
w_smooth=1.0,
w_obstacle=10.0,
):
c_length = length_cost(path)
c_smooth = smoothness_cost(path)
c_obstacle = obstacle_cost(path, obstacles)
total = (
w_length * c_length
+ w_smooth * c_smooth
+ w_obstacle * c_obstacle
)
return total, {
"length": c_length,
"smoothness": c_smooth,
"obstacle": c_obstacle,
}가중치를 바꾸면 로봇의 성격이 달라진다. w_length를 크게 하면 로봇은 짧은 길을 선호한다. w_smooth를 크게 하면 급회전을 피한다. w_obstacle을 크게 하면 장애물에서 더 멀리 떨어지려 한다.
이제 후보 경로들을 평가해 보자.
candidate_paths = {
"straight": path_straight,
"upper": path_upper,
"zigzag": path_zigzag,
}
for name, p in candidate_paths.items():
total, parts = total_cost(
p,
obstacles,
w_length=1.0,
w_smooth=2.0,
w_obstacle=20.0,
)
print(f"{name:8s} total={total:.3f} parts={parts}")출력값을 보면 재미있는 일이 생긴다. 직선 경로는 길이 비용은 작지만 장애물 비용이 크다. 지그재그 경로는 장애물을 피할 수 있더라도 부드러움 비용이 커질 수 있다. 위로 돌아가는 경로는 길이는 조금 길지만 장애물을 피하고 비교적 부드러워 전체 비용이 작게 나올 수 있다.
즉, 비용 함수는 로봇에게 이런 판단을 가르친다.
가장 짧은 길이 항상 가장 좋은 길은 아니다.
가장 안전한 길이 항상 가장 효율적인 길도 아니다.
좋은 경로는 여러 조건 사이의 균형이다.이 균형을 숫자로 표현하는 것이 경로 최적화의 출발점이다.
후보 경로를 눈으로 비교하기
비용 숫자만 보면 감이 잘 오지 않을 수 있다. 로봇 수학은 눈으로 확인할 때 훨씬 빨리 이해된다. 후보 경로를 한 그림에 그려 보자.
def plot_candidate_paths(candidate_paths, obstacles):
plt.figure(figsize=(8, 5))
ax = plt.gca()
for obs in obstacles:
cx, cy = obs["center"]
r = obs["radius"]
circle = plt.Circle((cx, cy), r, color="tomato", alpha=0.35)
ax.add_patch(circle)
plt.scatter(cx, cy, color="red", s=40)
for name, p in candidate_paths.items():
total, _ = total_cost(
p,
obstacles,
w_length=1.0,
w_smooth=2.0,
w_obstacle=20.0,
)
plt.plot(p[:, 0], p[:, 1], "o-", label=f"{name}: {total:.1f}")
first_path = next(iter(candidate_paths.values()))
plt.scatter(first_path[0, 0], first_path[0, 1], s=120, marker="s", label="start")
plt.scatter(first_path[-1, 0], first_path[-1, 1], s=150, marker="*", label="goal")
plt.axis("equal")
plt.grid(True)
plt.legend()
plt.title("Candidate paths and costs")
plt.show()
plot_candidate_paths(candidate_paths, obstacles)이 그림에서 각 경로의 라벨에는 전체 비용이 함께 표시된다. 직선은 눈으로 보기에는 가장 단순하지만 장애물과 겹치면 비용이 폭발한다. 지그재그는 장애물을 피하더라도 꺾임이 심하면 비용이 올라간다. 부드럽게 돌아가는 길이 전체적으로 더 좋은 선택이 될 수 있다.
여기까지 오면 로봇은 이미 단순한 판단을 할 수 있다. 여러 후보 경로를 만들어 놓고, 각각의 비용을 계산한 뒤, 가장 비용이 작은 경로를 고르면 된다.
def choose_best_path(candidate_paths, obstacles):
best_name = None
best_path = None
best_cost = float("inf")
for name, path in candidate_paths.items():
cost, _ = total_cost(
path,
obstacles,
w_length=1.0,
w_smooth=2.0,
w_obstacle=20.0,
)
if cost < best_cost:
best_name = name
best_path = path
best_cost = cost
return best_name, best_path, best_cost
best_name, best_path, best_cost = choose_best_path(candidate_paths, obstacles)
print(best_name, best_cost)이 방식은 단순하지만 매우 중요하다. 실제 로봇에서도 많은 경로 계획 방법은 결국 후보를 만들고, 평가하고, 더 좋은 것을 선택하는 구조를 가진다. 차이는 후보를 얼마나 똑똑하게 만들고, 비용을 얼마나 현실적으로 정의하고, 얼마나 빠르게 계산하느냐에 있다.
경로를 조금씩 개선하기
후보 중에서 고르는 것만으로도 경로 계획을 이해할 수 있지만, 최적화라는 이름에 조금 더 가까이 가 보자. 경로를 조금씩 바꿔 가며 비용을 줄여 보자.
출발점과 목표점은 고정하고, 중간점만 살짝 움직인다. 움직인 경로의 비용이 더 작아지면 그 변화를 받아들이고, 비용이 커지면 버린다. 이것은 아주 단순한 개선 방법이지만, "비용을 기준으로 경로를 바꾼다"는 핵심을 잘 보여 준다.
먼저 시작 경로를 하나 만들자. 직선으로 시작하되, 장애물 때문에 나쁜 경로가 되도록 하겠다.
def make_straight_path(start, goal, num_points):
xs = np.linspace(start[0], goal[0], num_points)
ys = np.linspace(start[1], goal[1], num_points)
return np.column_stack([xs, ys])
start = np.array([0.0, 0.0])
goal = np.array([4.0, 0.0])
initial_path = make_straight_path(start, goal, num_points=9)
plot_path(initial_path, obstacles, title="Initial straight path")이 경로는 장애물을 관통한다. 이제 중간점들을 무작위로 조금씩 흔들며 더 좋은 경로를 찾아 보자.
def improve_path_random(
initial_path,
obstacles,
iterations=3000,
step_size=0.15,
seed=0,
):
rng = np.random.default_rng(seed)
current = initial_path.copy()
current_cost, _ = total_cost(
current,
obstacles,
w_length=1.0,
w_smooth=2.0,
w_obstacle=20.0,
)
history = [current_cost]
for _ in range(iterations):
candidate = current.copy()
idx = rng.integers(1, len(candidate) - 1)
noise = rng.normal(0.0, step_size, size=2)
candidate[idx] += noise
candidate_cost, _ = total_cost(
candidate,
obstacles,
w_length=1.0,
w_smooth=2.0,
w_obstacle=20.0,
)
if candidate_cost < current_cost:
current = candidate
current_cost = candidate_cost
history.append(current_cost)
return current, np.array(history)
optimized_path, history = improve_path_random(initial_path, obstacles)
plot_path(optimized_path, obstacles, title="Optimized path")이 코드는 매우 단순하다. 매번 중간점 하나를 고르고, 그 점을 조금 움직인다. 그리고 전체 비용을 다시 계산한다. 좋아졌으면 유지한다. 나빠졌으면 없던 일로 한다.
마치 로봇이 속으로 이렇게 중얼거리는 것과 같다.
여기로 살짝 가 보면 더 나은가?
아니다. 위험해졌다. 되돌리자.
이번에는 위로 조금 움직여 보자.
오, 장애물에서 멀어졌고 비용도 줄었다. 이건 유지하자.비용이 줄어드는 과정을 그래프로 확인해 보자.
plt.figure(figsize=(7, 4))
plt.plot(history)
plt.xlabel("iteration")
plt.ylabel("cost")
plt.grid(True)
plt.title("Cost decreases during path improvement")
plt.show()처음에는 비용이 빠르게 줄어들고, 시간이 지나면 조금씩 완만해질 것이다. 모든 실행에서 완벽한 경로가 나오지는 않는다. 무작위 탐색은 운에 영향을 받는다. 그래도 이 실험은 중요한 사실을 보여 준다.
경로는 고정된 선이 아니다. 비용 함수를 기준으로 조금씩 개선할 수 있는 대상이다.
더 부드럽게 만들기: 두 번째 차이 비용
앞에서는 방향 변화량으로 부드러움을 계산했다. 또 다른 자주 쓰이는 방법은 가운데 점이 양옆 점의 중간에서 얼마나 벗어났는지를 보는 것이다.
세 점 P[i-1], P[i], P[i+1]가 있다고 하자. 경로가 부드럽다면 가운데 점 P[i]는 양옆 점을 잇는 흐름에서 크게 튀어나오지 않는다. 이를 다음 값으로 측정할 수 있다.
P[i-1] - 2P[i] + P[i+1]이 값은 두 번째 차이라고 부른다. 미분에서 가속도와 비슷한 역할을 한다. 위치가 한 번 변하면 속도이고, 속도가 다시 변하면 가속도다. 경로에서도 방향이 급격히 바뀌는 곳은 두 번째 차이가 커진다.
def smoothness_cost_second_difference(path):
second_diff = path[:-2] - 2 * path[1:-1] + path[2:]
return np.sum(np.linalg.norm(second_diff, axis=1) ** 2)이 방식은 각도를 직접 다루지 않기 때문에 구현이 간단하다. 그리고 경로점을 부드럽게 펴는 데 효과적이다. 다만 경로점 사이 간격이 매우 불규칙하면 해석이 조금 달라질 수 있다.
전체 비용 함수에서 부드러움 비용만 바꿔 사용할 수도 있다.
def total_cost_v2(
path,
obstacles,
w_length=1.0,
w_smooth=5.0,
w_obstacle=20.0,
):
c_length = length_cost(path)
c_smooth = smoothness_cost_second_difference(path)
c_obstacle = obstacle_cost(path, obstacles)
total = (
w_length * c_length
+ w_smooth * c_smooth
+ w_obstacle * c_obstacle
)
return total, {
"length": c_length,
"smoothness": c_smooth,
"obstacle": c_obstacle,
}실제 프로젝트에서는 상황에 따라 어떤 부드러움 비용이 더 적합한지 선택한다. 방향 변화량은 "회전 각도"를 직관적으로 다루기 좋고, 두 번째 차이는 경로를 매끈하게 펴는 데 쓰기 좋다.
가중치는 로봇의 성격을 만든다
경로 최적화에서 가중치는 매우 중요하다. 같은 지도, 같은 출발점, 같은 목표점이라도 가중치가 달라지면 로봇의 경로가 달라진다.
장애물 가중치가 너무 작으면 로봇은 위험할 정도로 장애물 가까이 지나갈 수 있다. 길이 가중치가 너무 크면 무리하게 지름길을 택한다. 부드러움 가중치가 너무 크면 로봇은 지나치게 완만하게 돌아가느라 경로가 불필요하게 길어질 수 있다.
다음처럼 가중치를 바꿔 실험해 보자.
weight_sets = [
{"w_length": 1.0, "w_smooth": 1.0, "w_obstacle": 5.0},
{"w_length": 1.0, "w_smooth": 5.0, "w_obstacle": 20.0},
{"w_length": 2.0, "w_smooth": 1.0, "w_obstacle": 50.0},
]
for weights in weight_sets:
cost, parts = total_cost(optimized_path, obstacles, **weights)
print(weights, cost, parts)이 실험의 핵심은 어떤 가중치가 정답인지 찾는 것이 아니다. 가중치가 로봇의 판단 기준을 바꾼다는 사실을 몸으로 느끼는 것이다.
현실에서는 가중치를 정할 때 로봇의 크기, 속도, 제어 성능, 센서 오차, 작업 목적을 함께 고려한다. 작은 청소 로봇과 무거운 물류 로봇이 같은 장애물 회피 가중치를 쓸 수는 없다. 빠르게 달리는 로봇은 더 넓은 안전거리를 가져야 하고, 사람 근처에서 움직이는 로봇은 효율보다 안전을 더 크게 봐야 한다.
장애물은 점이 아니라 영역이다
경로 최적화를 처음 배울 때 자주 하는 실수가 있다. 장애물 중심만 피하면 된다고 생각하는 것이다. 하지만 장애물은 점이 아니라 영역이다. 게다가 로봇 자신도 크기를 가진다.
만약 로봇 반지름이 0.25m이고 장애물 반지름이 0.45m라면, 로봇 중심은 장애물 중심에서 최소 0.70m 이상 떨어져야 충돌하지 않는다.
필요 거리 = 장애물 반지름 + 로봇 반지름따라서 장애물 회피 비용을 계산할 때는 로봇 크기도 반영하는 것이 좋다.
def obstacle_cost_with_robot_radius(
path,
obstacles,
robot_radius=0.25,
safety_margin=0.4,
):
total = 0.0
for obs in obstacles:
center = obs["center"]
inflated_radius = obs["radius"] + robot_radius
dists = np.linalg.norm(path - center, axis=1)
clearance = dists - inflated_radius
collision = clearance < 0.0
total += np.sum(collision) * 1_000.0
near = (clearance >= 0.0) & (clearance < safety_margin)
total += np.sum(((safety_margin - clearance[near]) / safety_margin) ** 2)
return total여기서는 장애물 반지름을 로봇 반지름만큼 부풀렸다. 이것을 장애물 팽창이라고 생각할 수 있다. 로봇을 점으로 보되, 장애물을 더 크게 만들어 충돌을 계산하는 방식이다. 이렇게 하면 수학이 훨씬 단순해진다.
경로점 사이의 충돌도 확인해야 한다
또 하나의 함정이 있다. 지금까지의 장애물 비용은 경로점에서만 계산했다. 하지만 로봇은 점에서 점으로 순간이동하지 않는다. 두 경로점 사이의 선분을 따라 움직인다.
예를 들어 경로점은 장애물 양옆에 있지만, 두 점을 잇는 선분이 장애물을 통과할 수 있다. 이 경우 경로점만 검사하면 충돌을 놓친다.
간단한 해결책은 경로를 더 촘촘하게 샘플링하는 것이다.
def densify_path(path, samples_per_segment=10):
dense_points = []
for i in range(len(path) - 1):
p0 = path[i]
p1 = path[i + 1]
for t in np.linspace(0.0, 1.0, samples_per_segment, endpoint=False):
dense_points.append((1 - t) * p0 + t * p1)
dense_points.append(path[-1])
return np.array(dense_points)이제 장애물 비용을 계산할 때 촘촘해진 경로를 사용할 수 있다.
def obstacle_cost_dense(path, obstacles, safety_margin=0.6):
dense = densify_path(path, samples_per_segment=10)
return obstacle_cost(dense, obstacles, safety_margin=safety_margin)이 방법은 계산량이 늘어나지만 이해하기 쉽고 구현도 간단하다. 로봇 시뮬레이션을 처음 만들 때는 이런 단순한 방법이 큰 도움이 된다.
전체 예제: 좋은 경로 고르기
지금까지 만든 내용을 하나로 묶어 보자. 출발점, 목표점, 장애물이 있고, 여러 후보 경로 중 비용이 가장 낮은 경로를 선택한다.
import numpy as np
import matplotlib.pyplot as plt
obstacles = [
{"center": np.array([2.0, 0.0]), "radius": 0.45},
{"center": np.array([2.8, 0.8]), "radius": 0.35},
]
candidate_paths = {
"straight": np.array([
[0.0, 0.0],
[1.0, 0.0],
[2.0, 0.0],
[3.0, 0.0],
[4.0, 0.0],
]),
"upper": np.array([
[0.0, 0.0],
[1.0, 0.9],
[2.0, 1.4],
[3.0, 1.2],
[4.0, 0.0],
]),
"lower": np.array([
[0.0, 0.0],
[1.0, -0.7],
[2.0, -1.0],
[3.0, -0.7],
[4.0, 0.0],
]),
"zigzag": np.array([
[0.0, 0.0],
[1.0, 1.0],
[2.0, -1.0],
[3.0, 1.0],
[4.0, 0.0],
]),
}
def evaluate_path(path, obstacles):
c_length = length_cost(path)
c_smooth = smoothness_cost(path)
c_obstacle = obstacle_cost_dense(path, obstacles, safety_margin=0.6)
total = 1.0 * c_length + 2.0 * c_smooth + 20.0 * c_obstacle
return total, c_length, c_smooth, c_obstacle
best_name = None
best_cost = float("inf")
for name, path in candidate_paths.items():
total, c_len, c_smooth, c_obs = evaluate_path(path, obstacles)
print(
f"{name:8s} "
f"total={total:8.3f} "
f"length={c_len:6.3f} "
f"smooth={c_smooth:6.3f} "
f"obstacle={c_obs:6.3f}"
)
if total < best_cost:
best_cost = total
best_name = name
print("best path:", best_name)
plot_candidate_paths(candidate_paths, obstacles)출력 결과를 보면 각 경로가 왜 좋거나 나쁜지 분해해서 볼 수 있다. 전체 비용만 보면 "이 경로가 이겼다"만 알 수 있지만, 길이 비용, 부드러움 비용, 장애물 비용을 따로 보면 "왜 이겼는지"를 이해할 수 있다.
이것이 비용 함수를 사용하는 큰 장점이다. 로봇의 판단을 숫자로 설명할 수 있다.
비용 함수가 나쁘면 로봇도 이상하게 움직인다
비용 함수는 로봇의 가치관이다. 비용 함수가 잘못 설계되면 로봇은 이상한 선택을 한다.
장애물 비용이 없으면 로봇은 벽을 통과하려고 할 수 있다. 부드러움 비용이 없으면 톱니처럼 꺾인 경로를 선택할 수 있다. 길이 비용이 없으면 목표까지 불필요하게 멀리 돌아갈 수 있다.
그래서 경로 최적화에서는 알고리즘만큼 비용 함수 설계가 중요하다. 아무리 빠르고 멋진 최적화 알고리즘을 써도, 비용 함수가 잘못되어 있으면 잘못된 경로를 빠르게 찾을 뿐이다.
좋은 비용 함수는 다음 성질을 가져야 한다.
로봇이 실제로 중요하게 여겨야 할 조건을 포함한다.
각 비용 항의 크기가 지나치게 한쪽으로 쏠리지 않도록 가중치가 조정되어 있다.
계산 결과를 해석할 수 있어야 하며, 시각화했을 때 움직임이 납득 가능해야 한다.
이 세 가지를 기억하면 복잡한 경로 계획 알고리즘을 배울 때도 중심을 잃지 않는다.
이 장에서 만든 경로 최적화의 구조
이번 장에서 만든 경로 최적화는 다음 흐름을 따른다.
1. 경로를 점 배열로 표현한다.
2. 경로 길이를 계산한다.
3. 방향 변화량으로 부드러움을 계산한다.
4. 장애물과의 거리를 이용해 위험도를 계산한다.
5. 세 비용을 가중합으로 묶어 전체 비용을 만든다.
6. 여러 후보 경로를 평가하거나, 경로를 조금씩 바꿔 비용을 줄인다.이 구조는 단순하지만 강력하다. 나중에 더 복잡한 로봇을 다룰 때도 같은 생각이 반복된다. 자율주행 자동차의 차선 변경, 로봇 팔의 충돌 없는 움직임, 드론의 비행 경로, 물류 로봇의 창고 이동 모두 "가능한 움직임 중에서 비용이 작은 움직임을 찾는 문제"로 볼 수 있다.
다음 장을 위한 연결
이제 우리는 로봇 시뮬레이터에 필요한 거의 모든 조각을 가지고 있다. 좌표와 벡터로 위치를 표현했고, 행렬로 회전과 변환을 다뤘고, 미분과 적분으로 움직임을 시간에 따라 계산했다. 센서 오차를 확률로 이해했고, 칼만 필터와 센서 퓨전으로 더 믿을 만한 위치를 추정하는 감각도 익혔다. 그리고 이번 장에서는 비용 함수로 경로를 평가하고 개선하는 방법까지 배웠다.
다음 장에서는 이 조각들을 하나로 묶는다. 2D 평면 위에서 로봇이 목표를 향해 움직이고, 센서 노이즈가 섞이며, 위치를 추정하고, 경로를 시각화하는 종합 시뮬레이터를 완성할 것이다.
로봇 수학은 따로 떨어진 공식들의 모음이 아니다. 좌표는 위치를 만들고, 행렬은 좌표를 옮기고, 미분은 움직임을 설명하고, 확률은 불확실성을 다루고, 최적화는 더 나은 선택을 만든다. 이제 이 모든 것이 하나의 작은 로봇 안에서 함께 움직일 차례다.
이 노트는 보도·비평·교육·연구 목적으로 작성되었습니다. 저작권 문의가 있으시면 에서 알려주세요.