라이다 스캔 다루기 - 거리 배열을 점으로

이 장에서 배우는 것

라이다(LiDAR)는 로봇을 중심으로 여러 방향에 빛을 쏘고 반사되어 돌아오는 시간으로 거리를 재는 센서다. 센서 드라이버가 넘겨주는 값은 "몇 번째 방향에서 몇 미터"라는 숫자 배열일 뿐이라서, 이 값을 로봇이 실제로 쓸 수 있는 점(point)의 좌표로 바꾸는 과정이 필요하다. 이 장에서는 PID 제어로 속도를 원하는 값에 맞추는 방법을 다룬 앞 장에 이어, 이번에는 로봇이 주변 공간을 숫자로 파악하는 방법을 다룬다.

• 라이다 원시 데이터가 각도 배열과 거리 배열로 이루어진다는 것을 이해한다
• 극좌표(각도, 거리)를 로봇 기준 직교좌표 (x, y) 점으로 변환한다
• 범위를 벗어난 무효 반사를 걸러내고 계산에서 제외한다
• 여러 빔 가운데 가장 가까운 장애물의 거리와 각도를 찾는다

문제 상황

차동 구동 로봇에 라이다를 달았다고 하자. 센서 드라이버는 한 번 스캔할 때마다 "이 각도에서는 이만큼 떨어져 있다"는 거리 배열을 돌려준다. 이 배열 자체로는 로봇이 어느 쪽으로 얼마나 가야 안전한지 바로 알기 어렵다. 화면에 점을 찍어 장애물 모양을 확인하려 해도, 각도와 거리만으로는 좌표평면에 그릴 수 없다. 게다가 반사파가 아예 없거나 너무 멀어서 측정 범위를 벗어난 방향은 0이나 매우 큰 값으로 채워져 있는데, 이 값을 그대로 최솟값 계산에 넣으면 로봇이 "바로 앞에 장애물이 있다"고 착각해 멈추거나, 반대로 진짜 가까운 장애물을 놓치는 일이 생긴다. 이 장에서는 이 두 문제, 즉 좌표 변환과 무효값 처리를 정리한다.

라이다 스캔은 극좌표 배열이다

원시 데이터의 구조

라이다 한 번의 스캔은 보통 두 배열의 짝으로 표현된다. 하나는 각 빔이 향한 각도(로봇 정면을 0도로 둔 상대 각도), 다른 하나는 그 방향에서 잰 거리다. 이 두 값의 짝 (θ, r)이 바로 극좌표(polar coordinate)이며, 좌표계와 벡터를 다룬 장에서 본 것과 같은 개념을 센서 데이터에 적용한 것이다. 다만 여기서 원점은 지도 위의 임의의 점이 아니라 로봇(정확히는 라이다가 달린 위치) 자신이다.

극좌표 → 직교좌표 변환 공식

빔 하나의 각도가 θ, 거리가 r이면, 로봇 기준 직교좌표는 다음과 같이 구한다.

• x = r cosθ (로봇 정면 방향 성분)
• y = r sinθ (로봇 좌우 방향 성분)

numpy는 각도·거리 배열 전체에 이 공식을 한 번에 적용할 수 있다. 다만 np.cos, np.sin은 라디안을 입력으로 받으므로, 도(degree) 단위로 저장된 각도라면 먼저 np.deg2rad로 바꿔야 한다.

최근접 장애물 찾기

무효 반사를 먼저 걸러낸다

라이다는 반사파가 없거나(검은 표면, 너무 먼 거리) 너무 가까운 물체에 부딪히면 정상 범위를 벗어난 값을 돌려준다. 이런 값을 최솟값 계산에 그대로 넣으면 안 된다. 0으로 채워진 무효값이 하나라도 있으면 np.argmin이 그 방향을 "가장 가까운 장애물"로 잘못 고른다. 그래서 거리 배열에서 유효 범위(min_range~max_range)를 벗어난 값을 먼저 NaN(Not a Number)으로 바꿔 두고, 이후 계산에서는 NaN이 아닌 값만 대상으로 삼는 방식을 쓴다.

무효 판정 기준이 말하는 것

원인	센서 출력 예	코드에서 판정	처리
반사파 없음(먼 거리·흡수면)	측정 최대거리를 넘는 값	range > max_range	NaN으로 바꾸고 계산에서 제외
최소 감지거리 이내 물체	0에 가까운 값	range < min_range	NaN으로 바꾸고 계산에서 제외
정상 반사	min_range~max_range 사이 값	두 조건 모두 거짓	그대로 사용

최솟값 인덱스를 원래 배열 기준으로 되돌리기

NaN이 아닌 값만 따로 뽑아 argmin을 쓰면, 그 결과는 "뽑아낸 부분 배열 안에서의 위치"일 뿐 원래 각도 배열의 위치가 아니다. 원래 배열에서 어느 방향인지 알아야 로봇이 피할 방향을 정할 수 있으므로, 유효한 값의 원래 인덱스를 먼저 구해 두고 그 인덱스 집합 안에서 최솟값을 찾는 방식을 쓴다.

완성 코드

파일 하나로 구성된 프로그램이다. 파일명은 robot_lidar_points.py로 저장한다. 가상의 방을 흉내 낸 고정된 거리값을 사용하므로 실행할 때마다 같은 결과가 나온다.

import numpy as np


def make_lidar_scan(num_beams=13, angle_min=-90.0, angle_max=90.0,
                     max_range=4.0, min_range=0.05):
    angles_deg = np.linspace(angle_min, angle_max, num_beams)

    ranges = np.full(num_beams, 3.0)
    front_wall = (angles_deg >= -20) & (angles_deg <= 20)
    ranges[front_wall] = 2.5
    pillar = (angles_deg >= -50) & (angles_deg <= -30)
    ranges[pillar] = 1.2
    open_gap = (angles_deg >= 60) & (angles_deg <= 80)
    ranges[open_gap] = max_range + 1.0

    invalid = (ranges < min_range) | (ranges > max_range)
    ranges = np.where(invalid, np.nan, ranges)
    return angles_deg, ranges


def polar_to_cartesian(angles_deg, ranges):
    angles_rad = np.deg2rad(angles_deg)
    x = ranges * np.cos(angles_rad)
    y = ranges * np.sin(angles_rad)
    return x, y


def find_nearest_obstacle(angles_deg, ranges):
    valid_idx = np.where(~np.isnan(ranges))[0]
    if valid_idx.size == 0:
        return None
    nearest = valid_idx[np.argmin(ranges[valid_idx])]
    return int(nearest), float(angles_deg[nearest]), float(ranges[nearest])


def main():
    angles_deg, ranges = make_lidar_scan()
    x, y = polar_to_cartesian(angles_deg, ranges)

    print(f"{'각도(도)':>8} {'거리(m)':>8} {'x(m)':>8} {'y(m)':>8}")
    for a, r, xi, yi in zip(angles_deg, ranges, x, y):
        if np.isnan(r):
            print(f"{a:8.1f} {'무효':>8} {'-':>8} {'-':>8}")
        else:
            print(f"{a:8.1f} {r:8.3f} {xi:8.3f} {yi:8.3f}")

    result = find_nearest_obstacle(angles_deg, ranges)
    idx, angle, dist = result
    print()
    print(f"가장 가까운 장애물: 인덱스 {idx}, 각도 {angle:.1f}도, 거리 {dist:.3f} m")


if __name__ == "__main__":
    main()

줄별 해설

• make_lidar_scan — np.linspace로 -90도에서 90도까지 13개 빔의 각도를 만든다. 기본 거리 3.0m 위에 정면 벽(±20도, 2.5m), 기둥 장애물(-50~-30도, 1.2m), 열린 문틈(60~80도, 측정 범위를 넘는 값)을 각도 조건으로 덮어써서 가상의 방을 흉내 낸다. 마지막 두 줄에서 min_range~max_range를 벗어난 값을 np.where로 NaN으로 바꾼다.
• polar_to_cartesian — np.deg2rad로 라디안으로 바꾼 뒤 x = r cosθ, y = r sinθ 공식을 배열 전체에 한 번에 적용한다. r이 NaN인 자리는 계산 결과도 자동으로 NaN이 된다.
• find_nearest_obstacle — np.isnan으로 유효한 자리의 원래 인덱스(valid_idx)를 먼저 구한다. ranges[valid_idx]에서 최솟값을 찾은 뒤, 그 위치를 다시 valid_idx로 환산해 원래 배열 기준 인덱스로 되돌린다. 유효한 값이 하나도 없으면 None을 돌려준다.
• main — 스캔을 만들고 좌표로 바꾼 다음, 각 빔을 한 줄씩 출력한다. NaN인 행은 "무효"로 표시하고, 마지막에 최근접 장애물의 인덱스·각도·거리를 출력한다.

실행 결과

$ python3 robot_lidar_points.py
   각도(도)    거리(m)     x(m)     y(m)
   -90.0    3.000    0.000   -3.000
   -75.0    3.000    0.776   -2.898
   -60.0    3.000    1.500   -2.598
   -45.0    1.200    0.849   -0.849
   -30.0    1.200    1.039   -0.600
   -15.0    2.500    2.415   -0.647
     0.0    2.500    2.500    0.000
    15.0    2.500    2.415    0.647
    30.0    3.000    2.598    1.500
    45.0    3.000    2.121    2.121
    60.0       무효        -        -
    75.0       무효        -        -
    90.0    3.000    0.000    3.000

가장 가까운 장애물: 인덱스 3, 각도 -45.0도, 거리 1.200 m

실무에서 자주 틀리는 것

무효값을 0으로 둔 채 최솟값을 구한다

센서 드라이버가 무효 반사를 0.0으로 채워 두는 경우가 있다. 이 상태에서 argmin을 바로 쓰면 실제로는 아무것도 없는 방향을 "가장 가까운 장애물"로 오판한다.

# 틀린 코드
nearest_idx = np.argmin(ranges)  # ranges 안에 무효 반사가 0.0으로 섞여 있다

# 고친 코드
valid_idx = np.where(~np.isnan(ranges))[0]
nearest_idx = valid_idx[np.argmin(ranges[valid_idx])]

각도를 라디안으로 바꾸지 않고 삼각함수에 넣는다

센서에서 받은 각도가 도(degree) 단위인데 그대로 np.cos, np.sin에 넣으면 라디안으로 취급되어 완전히 다른 좌표가 나온다.

# 틀린 코드
x = ranges * np.cos(angles_deg)

# 고친 코드
x = ranges * np.cos(np.deg2rad(angles_deg))

부분 배열에서 구한 인덱스를 원본 배열 인덱스로 착각한다

NaN을 걸러낸 부분 배열에서 argmin을 구하면, 그 값은 부분 배열 안에서의 위치이지 원래 각도 배열의 위치가 아니다.

# 틀린 코드
valid = ranges[~np.isnan(ranges)]
nearest_idx = np.argmin(valid)
nearest_angle = angles_deg[nearest_idx]  # 엉뚱한 각도를 가리킬 수 있다

# 고친 코드
valid_idx = np.where(~np.isnan(ranges))[0]
nearest_idx = valid_idx[np.argmin(ranges[valid_idx])]
nearest_angle = angles_deg[nearest_idx]

거리만 보고 바로 정지 판단을 내린다

가장 가까운 빔이 로봇 옆(예: 90도 방향)을 가리킬 수도 있다. 각도를 확인하지 않고 거리만으로 멈추면 실제로 진행 방향에는 아무것도 없는데도 로봇이 멈춘다.

# 틀린 코드
if dist < 0.3:
    stop_robot()

# 고친 코드
if dist < 0.3 and abs(angle) < 30:
    stop_robot()

한눈에 보기

이 장에서 만든 함수가 하는 일

함수	입력	출력	역할
make_lidar_scan	빔 개수, 각도 범위, 유효 거리 범위	각도 배열, 거리 배열(NaN 포함)	가상 스캔 생성(실제로는 센서 드라이버가 채워줌)
polar_to_cartesian	각도 배열, 거리 배열	x 배열, y 배열	극좌표를 로봇 기준 직교좌표로 변환
find_nearest_obstacle	각도 배열, 거리 배열	인덱스, 각도, 거리	NaN을 제외한 최솟값 탐색

연습 문제

1. 전방 ±30도 범위 안에서만 최근접 거리를 찾는 함수 nearest_in_front(angles_deg, ranges, cone_deg=30)를 작성하라.
2. np.nanargmin을 바로 쓰는 방식과, 본문처럼 np.where(~np.isnan(...))로 유효 인덱스를 먼저 구하는 방식의 차이를 설명하라. 유효한 값이 하나도 없을 때 각각 어떻게 동작하는가?
3. 다음 코드는 무효 판정을 range == 0인지로만 검사한다. 어떤 상황에서 이 판정이 실패하는지 지적하고 고쳐라.

   invalid = (ranges == 0)
   ranges = np.where(invalid, np.nan, ranges)
   

4. 각해상도가 1도이고 -135도부터 135도까지 스캔하는 라이다가 있다. 이 스캔 하나에 들어 있는 빔의 개수는 몇 개인가?

정답과 해설

1. 각도 조건으로 먼저 범위를 좁힌 뒤, 그 안에서 유효한 값만 골라 최솟값을 찾는다.

def nearest_in_front(angles_deg, ranges, cone_deg=30):
    in_cone = np.abs(angles_deg) <= cone_deg
    candidate_idx = np.where(in_cone & ~np.isnan(ranges))[0]
    if candidate_idx.size == 0:
        return None
    nearest = candidate_idx[np.argmin(ranges[candidate_idx])]
    return int(nearest), float(angles_deg[nearest]), float(ranges[nearest])

2. np.nanargmin은 NaN을 자동으로 무시하고 최솟값의 인덱스를 바로 돌려주므로 코드가 짧아진다. 하지만 배열 전체가 NaN이면 ValueError를 일으켜 프로그램이 멈춘다. 본문 방식은 유효한 인덱스 목록을 먼저 만들기 때문에, 그 목록이 비어 있는 경우를 if valid_idx.size == 0으로 직접 검사해 None을 돌려주는 등 원하는 방식으로 처리할 수 있다.

3. 실제 센서값은 부동소수점 잡음이 섞여 있어서 정확히 0.0이 나오는 경우가 드물고, 반사파가 없을 때 0 대신 최대거리나 그보다 큰 값을 돌려주는 센서도 많다. == 0만으로는 이런 경우를 걸러내지 못한다. 최소·최대 유효 범위를 함께 검사해야 한다.

invalid = (ranges < min_range) | (ranges > max_range)
ranges = np.where(invalid, np.nan, ranges)

4. -135도부터 135도까지 1도 간격이므로 빔 개수는 (135 - (-135)) / 1 + 1 = 271개다.