🔗 일부 입증 자료 링크가 정상 작동하지 않아 한 페이지에 정리하였습니다. 크롬 환경에서 정상조회가 가능합니다. [입증 자료 링크 목록보기] 🔗

⚠️ 페이지에 미디어가 많아 로딩까지 시간이 걸릴 수 있습니다. 일부 화면이 나오지 않는다면 조금 기다려 주세요. ⚠️

Smart Cane '길라잡이' Project

시각장애인을 위한 스마트 기능을 탑재한 RPi-Based Smart Cane

BOOSTA(부산과학기술자문단) 연구성과공유
B&B STEEL 로봇사업부 기술자문 참여
지역 명화학교에서 기능 테스트 예정

동작 방식

장애물이 없는 상황

장애물이 없는 상황에서는 지팡이 앞의 모터가 양쪽으로 가볍게 회전하여사용자에게 현재 상태를 알림. 또한 이어폰을 통해 장애물이 없음을 청각 피드백으로 알림. 이 경우에 사용자는 앞으로 조금씩 나가면 됨.

장애물이 있는 상황

장애물을 감지하면 모터가 지팡이를 장애물 반대방향 (공간이 많은 방향)으로 회전하여 사용자 및 지팡이가 장애물과 충돌하지 않도록 하며 햅틱피드백 및 이어폰청각 피드백으로 장애물을 알림. 장애물을 피한 경우 다시 양쪽으로 가볍게 회전하며 진행함.

실내 회피 시연 장면

실외 보행 장면

기존 연구/Cane과 차별점

흔히 알려진 VCU와 Stanford의 Smart Cane과 차별점은 '흉부높이 장애물 감지 불가'를 해결하였고, AI를 통한 주변 정지신호 및 경고신호 판별기능 탑재로기존 제품들이 감지 하지 못하는 표지인식까지 구현하였다. 또한 야간 데이터학습과 조도센서를 통해 주야간 최적의 데이터를 선택하여 인식률을 높였다. 그리고 레이저센서를 활용하여 지팡이 말단부 장애물 경고기능을 추가 하였으며, 사용자 자세를 실시간으로 감지하고 넘어짐/충격을 감지하면 경고 및 전용APP을 통해 신고하는 등의 차별점을 가진 작품이다.

[첨부영상]

시중의 WeWalk 지팡이가 흉부 이상의 장애물은 감지하지 못해 아래쪽은 공간이 있지만 위쪽은 튀어나와 있는 책상의 구석으로 들어가다 걸리는 모습이다.

시중 스마트 지팡이가 흉부 이상의 장애물을 감지못해 구석에 들어가는 장면

라이다 장애물 감지 및 모터 방향 안내

라이다가 공간을 스캔하고 장애물이 없다면 좌우로 흔들며 직진보조,

장애물이 있다면 공간이 많은 방향으로 회전명령을 보낸다.

[첨부영상]

라이다가 공간을 분석하고 장애물을 감지
라이다와 센서를 기반으로 장애물을 감지하고 모터 방향 명령 로그

라이다 장애물 감지

모터 방향 로그

해결이 필요한 문제점

모터 배터리 문제
현재 배터리 구성은 1)Rpi 및 라이다 구동용 배터리, 2)모터 구동용 배터리 로 이루어져 있음. 사용을 위해서는 각각 충전을 해야하는 문제점이 존재함.
이를 해결하기 위해 1)배터리로 일원화를 시도함. 그러나, 모터의 경우 감압모듈을 사용하지만 입력이 불안정하여 1)배터리의 보호회로가 전류를 차단함.
2) 배터리는 장치의 정상적인 동작과 모든 출력을 감당하기 힘들어 1)로 일원화가 필요함.
=> 해당 문제 해결을 위해 안정적인 전력 유지가 가능한 보호모듈을 활용할 예정. 1)배터리로 2)의 기능까지 모두 활용할 수 있도록 할 예정.
그러나 기존 감압모듈에 비해 부피가 커 케이스의 재 설계가 필요함.
[2025.09. 업데이트]
구동부의 무게중심 문제
현재 손잡이와 가까운 구동부의 위쪽에는 라이다가, 아래쪽에는 Rpi와 모듈이 배치되어 있음. 위아래의 무게 중심이 맞지 않아 구동부와 지팡이의 결합부가 한 방향으로 회전하는 문제가 발생함. 정상적인 사용을 위해서는 매우 꽉 잡아야 함.
이를 해결하기 위해 관절부 고정장치를 제작함. 그러나, 시각장애인이 결합/분리하는데 어려움이 있을 것으로 보임.
=> 해당 문제 해결을 위해 고정장치 대신 돌려서 맞출 수 있는 고정홈을 설치하여 누구나 쉽게 장치의 회전을 막고 정상적으로 사용할 수 있도록 함.
[2025.09. 업데이트]
Rpi의 라이다 미인식 문제 [해결완료]
Rpi의 성능을 고려하여 처음에 4B를 선택했으나 라즈비안설치 과정에서 충돌이 일어나 3B로 변경하여 사용함. 그런데 또 기본 탑재 Python 및 기타SW 버전이 맞지 않아 계속 오류가 발생함.
=> 이를 해결하기 위해 Python 버전 강제 업그레이드 및 각종 SW&lib 업데이트를 진행함. 그러나 계속 문제가 발생하여 보드 교체, 포트변경, 라이다에외부전원공급에도 해결되지 않아 문제가 되는 부분을 제외하고 코드를 재작성 및 통신속도 조정을 통해 해결.
[2025.05. 해결완료]

모터 미동작 문제 [해결완료]
모터가 Rpi 자체의 전원으로는 구동이 불가하여 외부 배터리를 통해 구동함. 외부 배터리<=>모터 드라이버 간 전압을 맞추기 위해 강압모듈을 사용 하였음. 그런데 강압모듈의 출력부 전압이 0V로 작동하지 않아 모터기능 자체를 사용할 수 없었음. 강압모듈을 주문-교체까지 테스트를 할 수 없어 임시방편으로 배터리 충전정도를 조절하여 테스터기를 통해 전압을 확인하고 모듈없이 직결하여 테스트를 진행함. <배터리를 완충하지 않고 일정수준만 충전하면 기준전압보다 낮게 출력되는 특성을 이용>
[2025.06. 해결완료]
I2C 통신과정에서 혼선의 발생
IMU, GPS 등 일부 모듈이 인식되지 않는 문제가 발생함. 빵판을 이용하였기 때문에 핀 혼동/연결문제를 생각하고 재배치를 했지만 해결되지 않음.
=> 이를 해결하기 위해 인접한 모듈을 분리하고 하나씩 테스트 결과 정상 작동함. 혼선/접촉불량으로 원인을 설정하고 모듈배치를 분리하였음.
추후PCB를 제작할예정임.
[2025.07. 1차 해결완료] [PCB 설계중]
IMU의 사용자 자세감지 인식률 문제 [해결완료]
IMU가 가속도/자이로센서를 통해 사용자의 현재 자세를 파악하여 넘어지거나 충돌했을 때 경고알림을 보낸다. 그런데 넘어짐을 판단하는 민감도 수치를 설정하는 것이 어려웠다. 민감도를 낮추면 가벼운 충돌을 감지 못하고, 반대로 민감도를 높이면 사용자에게 위해가 되지않는 충격에도 감지되었다.
=> 이를 해결하기 위해 시연을 진행하며 최적값을 찾았다.
[2025.07. 해결완료]

주야간 정지신호 인식성능 개선

지팡이가 주야간 모두 사용이 가능해야 하기 때문에 '정지신호 인식' 기능의 주/야간 인식 기능의 테스트를 진행함.

주간 주행의 경우 정지신호를 비교적 잘 인식했지만, 야간 주행의 경우 인식률이 낮았음.
(사진)

이를 해결하기 위해 객체인식 인식성능 개선을 진행함.
AI Hub의 신호등/도로표지판 인지 데이터를 활용하여 인식 성능을 업그레이드 하고 있음.

야간 테스트를 진행하는 모습

[야간] 횡단보도 표지판을

정지 표지판으로 오인하는 모습

[주요 Code]

라이다 장애물 회피 Code

try:

sys.stdout.reconfigure(encoding='utf-8')

sys.stderr.reconfigure(encoding='utf-8')

except Exception:

pass

# LIDAR 설정

PORT = '/dev/ttyUSB0'

BAUD = 115200

TIMEOUT_S = 5.0

# GPIO 핀

DIR_PIN = 23

PWM_PIN = 18

# motor 관련

def set_motor(speed):

duty = max(0, min(abs(speed), 100))

if speed >= 0:

GPIO.output(DIR_PIN, GPIO.HIGH) # 정방향

pwm.ChangeDutyCycle(duty)

else:

GPIO.output(DIR_PIN, GPIO.LOW) # 역방향

pwm.ChangeDutyCycle(duty)

def stop_motor():

pwm.ChangeDutyCycle(0)

def sigint_handler(signum, frame): # SIGINT/SIGTERM 끊을 때

global running

running = False

def connect_lidar(port=PORT, baud=BAUD, timeout=TIMEOUT_S): # 라이다 인스턴스 연결

return RPLidar(port, baudrate=baud, timeout=timeout)

# lidar reset 관련

def safe_reset(dev): # 세션, 버퍼 등 초기화

try: dev.stop()

except: pass

time.sleep(0.05)

try: dev.clear_input()

except: pass

time.sleep(0.05)

try: dev.reset()

except: pass

time.sleep(0.3)

def safe_shutdown(dev): # 종료, 모터·GPIO 포함

if dev:

try: dev.stop()

except: pass

time.sleep(0.05)

try: dev.stop_motor()

except: pass

time.sleep(0.05)

try: dev.disconnect()

except: pass

stop_motor() # 모터 끔

try:

pwm.stop() # PWM 끔

except Exception:

pass

try:

GPIO.cleanup() # GPIO 끔

except Exception:

pass

def print_device_info(dev): # 라이다 기본 정보/체크

try:

info = dev.get_info()

print("[LIDAR] info:", info)

except Exception as e:

print("[LIDAR] get_info failed:", e)

try:

health = dev.get_health()

print("[LIDAR] health:", health)

except Exception as e:

print("[LIDAR] get_health failed:", e)

def drain_and_pause(dev, repeats=3, delay=0.08): # 버퍼 여러 번 비움

for _ in range(repeats):

try:

dev.clear_input()

except:

pass

time.sleep(delay)

def iterate_scans(dev): # 스캔 yield할 때 버퍼 비우고 시작

drain_and_pause(dev, repeats=3, delay=0.08)

for scan in dev.iter_scans(max_buf_meas=False):

yield scan

def robust_connect_and_init(max_retry=5): # 초기 연결/On 재시도

last_err = None

for attempt in range(1, max_retry+1):

dev = None

try:

print(f"[SYS] Connecting to RPLidar {attempt}/{max_retry} ...") # 연결 시도 로그

dev = connect_lidar()

drain_and_pause(dev, repeats=3, delay=0.08)

safe_reset(dev)

try:

dev.start_motor() # 모터 ON

except Exception:

pass

time.sleep(1.0) # 모터 안정화

print_device_info(dev)

drain_and_pause(dev, repeats=2, delay=0.08)

print("[SYS] Initialization complete") # 초기화 완료 로그

return dev

except Exception as e:

last_err = e

print(f"[ERR] Connect/init failed ({attempt}/{max_retry}):", e) # 실패 로그

try:

safe_shutdown(dev) # 실패 시 shutdown 호출

except Exception:

pass

time.sleep(0.6 * attempt) # 점증 backoff (재시도 딜레이)

raise RuntimeError(f"RPLidar initialization failed: {last_err}") # 다 실패 시 에러

SWEEP_PERIOD_S = 2.0 # 모터 스윕 주기s

SWEEP_SPEED = 30 # 스윕 속도%

AVOID_SPEED = 50 # 장애물 회피 속도%

FRONT_DANGER_MM = 600 # 장애물 기준 거리mm

sweep_state = 1 # 스윕 방향 (우, 좌 각각 -1,1)

sweep_timer = time.time()

def sweep_motor():

global sweep_state, sweep_timer

now = time.time()

if sweep_state == 1:

set_motor(SWEEP_SPEED) # 오른쪽

if now - sweep_timer > SWEEP_PERIOD_S:

sweep_state = -1

sweep_timer = now

else:

set_motor(-SWEEP_SPEED) # 왼쪽

if now - sweep_timer > SWEEP_PERIOD_S:

sweep_state = 1

sweep_timer = now

def process_scan(scan): # 한 번에 스캔 처리해서 모터 방향 정함

danger = False

left_space = 0

right_space = 0

for q, angle, dist in scan:

if dist <= 0:

continue

if (angle <= 15 or angle >= 345) and dist < FRONT_DANGER_MM: # 전방 장애물 체크

danger = True

if 60 <= angle <= 120: # 좌측 거리 합

left_space += dist

if 240 <= angle <= 300: # 우측 거리 합

right_space += dist

if danger:

print("[WARN] Obstacle! Turning to the side with more space")

if left_space > right_space:

print("→ LEFT") # 왼쪽으로

set_motor(-AVOID_SPEED)

else:

print("→ RIGHT") # 오른쪽으로

set_motor(AVOID_SPEED)

else:

print("[OK] Clear → sweeping") # 장애물 없으면 스윕

sweep_motor()

def main():

global lidar, running

try:

lidar = robust_connect_and_init(max_retry=5)

except Exception as e:

print("[FATAL] Lidar initialization failed:", e)

sys.exit(1)

print("[SYS] Start scanning...") # 스캔 시작

backoff = 0.5 # 에러 시 backoff

while running:

try:

for scan in iterate_scans(lidar):

if not running:

break

process_scan(scan)

if not running:

break

print("[WARN] Scan loop ended → restarting") # 반복 후 backoff 리셋

drain_and_pause(lidar, repeats=3, delay=0.08)

backoff = 0.5 # 반복 후 backoff 리셋

except RPLidarException as e:

print("[ERR] RPLidarException:", e)

try:

safe_reset(lidar)

print_device_info(lidar)

except Exception as e2:

print("[ERR] reset failed:", e2)

time.sleep(backoff)

backoff = min(backoff * 2.0, 5.0) # 에러 시 backoff 증가

except Exception as e:

print("[ERR] General exception:", e)

traceback.print_exc()

time.sleep(backoff)

backoff = min(backoff * 2.0, 5.0) # 에러 시 backoff 증가

print("[SYS] Running=false → shutdown")

safe_shutdown(lidar)

print("[SYS] Shutdown complete")

if __name__ == "__main__":

main()

센서통합 장애물 회피 Code

import sys

import time

import signal

import traceback

import RPi.GPIO as GPIO

import smbus2

import pygame

import math

import board

import busio

import adafruit_lsm6ds.lsm6ds3trc as lsm6ds3trc

import adafruit_lis3mdl

try:

sys.stdout.reconfigure(encoding='utf-8')

sys.stderr.reconfigure(encoding='utf-8')

except: pass

try:

from rplidar import RPLidar, RPLidarException

except Exception as e:

print("Failed to import rplidar package:", e)

sys.exit(1)

# GPIO 핀/모터 설정

DIR_PIN = 23

PWM_PIN = 18

GPIO.setmode(GPIO.BCM)

GPIO.setup(DIR_PIN, GPIO.OUT)

GPIO.setup(PWM_PIN, GPIO.OUT)

pwm = GPIO.PWM(PWM_PIN, 1000)

pwm.start(0)

# 레이저 센서 설정

LASER_LEFT_PIN = 24

LASER_RIGHT_PIN = 25

GPIO.setup(LASER_LEFT_PIN, GPIO.IN)

GPIO.setup(LASER_RIGHT_PIN, GPIO.IN)

LASER_THRESHOLD = 1

# 광센서

I2C_BUS = 1

LIGHT_ADDR = 0x23

bus = smbus2.SMBus(I2C_BUS)

def read_lux():

try:

data = bus.read_i2c_block_data(LIGHT_ADDR, 0x10)

lux = ((data[0] << 8) + data[1]) / 1.2

return lux

except: return None

# 사운드 출력

pygame.mixer.init()

def play_sound(file):

try:

sound = pygame.mixer.Sound(file)

sound.play()

print(f"[SOUND] Playing {file}")

except Exception as e:

print("[ERR] Could not play sound:", e)

# 모터 제어

def set_motor(speed):

duty = max(0, min(abs(speed), 100))

GPIO.output(DIR_PIN, GPIO.HIGH if speed >= 0 else GPIO.LOW)

pwm.ChangeDutyCycle(duty)

def stop_motor():

pwm.ChangeDutyCycle(0)

# 신호 핸들러

running = True

def sigint_handler(signum, frame):

global running

running = False

signal.signal(signal.SIGINT, sigint_handler)

signal.signal(signal.SIGTERM, sigint_handler)

# LIDAR 설정

PORT = '/dev/ttyUSB0'

BAUD = 115200

TIMEOUT_S = 5.0

lidar = None

SWEEP_PERIOD_S = 2.0

SWEEP_SPEED = 30

AVOID_SPEED = 50

FRONT_DANGER_MM = 600

sweep_state = 1

sweep_timer = time.time()

def sweep_motor():

global sweep_state, sweep_timer

now = time.time()

if sweep_state == 1:

set_motor(SWEEP_SPEED)

if now - sweep_timer > SWEEP_PERIOD_S:

sweep_state = -1

sweep_timer = now

else:

set_motor(-SWEEP_SPEED)

if now - sweep_timer > SWEEP_PERIOD_S:

sweep_state = 1

sweep_timer = now

def read_laser_sensors():

left = GPIO.input(LASER_LEFT_PIN)

right = GPIO.input(LASER_RIGHT_PIN)

return left >= LASER_THRESHOLD, right >= LASER_THRESHOLD

# IMU 설정

i2c = busio.I2C(board.SCL, board.SDA)

imu = lsm6ds3trc.LSM6DS3TRC(i2c)

mag = adafruit_lis3mdl.LIS3MDL(i2c)

IMU_TILT_THRESHOLD = 50

def check_imu_alert():

accel = imu.acceleration

gyro = imu.gyro

total_acc = math.sqrt(accel[0]**2 + accel[1]**2 + accel[2]**2)

total_gyro = math.sqrt(gyro[0]**2 + gyro[1]**2 + gyro[2]**2)

if total_acc > IMU_TILT_THRESHOLD or total_gyro > IMU_TILT_THRESHOLD:

play_sound("alert.mp3")

print("[IMU] Abnormal posture detected!")

def connect_lidar(port=PORT, baud=BAUD, timeout=TIMEOUT_S):

return RPLidar(port, baudrate=baud, timeout=timeout)

def drain_and_pause(dev, repeats=3, delay=0.08):

for _ in range(repeats):

try: dev.clear_input()

except: pass

time.sleep(delay)

def iterate_scans(dev):

drain_and_pause(dev)

for scan in dev.iter_scans(max_buf_meas=False):

yield scan

def robust_connect_and_init(max_retry=5):

last_err = None

for attempt in range(1, max_retry+1):

dev = None

try:

print(f"[SYS] Connecting to RPLidar {attempt}/{max_retry} ...")

dev = connect_lidar()

drain_and_pause(dev)

try: dev.start_motor()

except: pass

time.sleep(1.0)

print("[SYS] Initialization complete")

return dev

except Exception as e:

last_err = e

print(f"[ERR] Connect/init failed ({attempt}/{max_retry}):", e)

try: dev.disconnect()

except: pass

time.sleep(0.6 * attempt)

raise RuntimeError(f"RPLidar initialization failed: {last_err}")

def process_scan(scan):

danger = False

left_space = 0

right_space = 0

front_detected = False

for q, angle, dist in scan:

if dist <= 0: continue

if (angle <= 15 or angle >= 345) and dist < FRONT_DANGER_MM:

danger = True

front_detected = True

if 60 <= angle <= 120:

left_space += dist

if 240 <= angle <= 300:

right_space += dist

laser_left, laser_right = read_laser_sensors()

if laser_left and laser_right:

play_sound("contact.mp3")

return

if danger:

if front_detected:

play_sound("obstacle_front.mp3")

if left_space > right_space:

play_sound("obstacle_left.mp3")

set_motor(-AVOID_SPEED)

else:

play_sound("obstacle_right.mp3")

set_motor(AVOID_SPEED)

else:

sweep_motor()

def main():

global lidar, running

try:

lidar = robust_connect_and_init(max_retry=5)

except Exception as e:

print("[FATAL] Lidar initialization failed:", e)

sys.exit(1)

print("[SYS] Start scanning...")

while running:

try:

lux = read_lux()

if lux is not None:

print(f"[LIGHT] Current lux: {lux:.1f}")

check_imu_alert()

for scan in iterate_scans(lidar):

if not running: break

process_scan(scan)

if not running: break

except RPLidarException as e:

print("[ERR] RPLidarException:", e)

try: lidar.disconnect()

except: pass

time.sleep(1.0)

except Exception as e:

print("[ERR] General exception:", e)

traceback.print_exc()

time.sleep(1.0)

print("[SYS] Running=false → shutdown")

stop_motor()

try: pwm.stop()

except: pass

try: GPIO.cleanup()

except: pass

if __name__ == "__main__":

main()

실외경로 안내 Code (개선중)

import sys

import time

import signal

import traceback

import RPi.GPIO as GPIO

import smbus2

import pygame

import math

import board

import busio

import adafruit_lsm6ds.lsm6ds3trc as lsm6ds3trc

import adafruit_lis3mdl

import pynmea2

from rplidar import RPLidar, RPLidarException

DIR_PIN = 23

PWM_PIN = 18

GPIO.setmode(GPIO.BCM)

GPIO.setup(DIR_PIN, GPIO.OUT)

GPIO.setup(PWM_PIN, GPIO.OUT)

pwm = GPIO.PWM(PWM_PIN, 1000)

pwm.start(0)

LASER_LEFT_PIN = 24

LASER_RIGHT_PIN = 25

GPIO.setup(LASER_LEFT_PIN, GPIO.IN)

GPIO.setup(LASER_RIGHT_PIN, GPIO.IN)

LASER_THRESHOLD = 1

def read_laser_sensors():

left = GPIO.input(LASER_LEFT_PIN)

right = GPIO.input(LASER_RIGHT_PIN)

return left >= LASER_THRESHOLD, right >= LASER_THRESHOLD

I2C_BUS = 1

LIGHT_ADDR = 0x23

bus = smbus2.SMBus(I2C_BUS)

def read_lux():

try:

data = bus.read_i2c_block_data(LIGHT_ADDR, 0x10)

lux = ((data[0] << 8) + data[1]) / 1.2

return lux

except: return None

pygame.mixer.init()

def play_sound(file):

try:

sound = pygame.mixer.Sound(file)

sound.play()

print(f"[SOUND] Playing {file}")

except Exception as e:

print("[ERR] Could not play sound:", e)

def set_motor(speed):

duty = max(0, min(abs(speed), 100))

GPIO.output(DIR_PIN, GPIO.HIGH if speed >= 0 else GPIO.LOW)

pwm.ChangeDutyCycle(duty)

def stop_motor():

pwm.ChangeDutyCycle(0)

# IMU 설정

i2c = busio.I2C(board.SCL, board.SDA)

imu = lsm6ds3trc.LSM6DS3TRC(i2c)

mag = adafruit_lis3mdl.LIS3MDL(i2c)

IMU_TILT_THRESHOLD = 50

def check_imu_alert():

accel = imu.acceleration

gyro = imu.gyro

total_acc = math.sqrt(accel[0]**2 + accel[1]**2 + accel[2]**2)

total_gyro = math.sqrt(gyro[0]**2 + gyro[1]**2 + gyro[2]**2)

if total_acc > IMU_TILT_THRESHOLD or total_gyro > IMU_TILT_THRESHOLD:

play_sound("alert.mp3")

print("[IMU] Abnormal posture detected!")

def get_heading():

mag_x, mag_y, mag_z = mag.magnetic

heading = math.degrees(math.atan2(mag_y, mag_x))

if heading < 0:

heading += 360

return heading

# LIDAR 설정

PORT = '/dev/ttyUSB0'

BAUD = 115200

TIMEOUT_S = 5.0

lidar = None

FRONT_DANGER_MM = 600

AVOID_SPEED = 50

SWEEP_SPEED = 30

SWEEP_PERIOD_S = 2.0

sweep_state = 1

sweep_timer = time.time()

def sweep_motor():

global sweep_state, sweep_timer

now = time.time()

if sweep_state == 1:

set_motor(SWEEP_SPEED)

if now - sweep_timer > SWEEP_PERIOD_S:

sweep_state = -1

sweep_timer = now

else:

set_motor(-SWEEP_SPEED)

if now - sweep_timer > SWEEP_PERIOD_S:

sweep_state = 1

sweep_timer = now

def process_scan(scan):

danger = False

left_space = 0

right_space = 0

front_detected = False

for q, angle, dist in scan:

if dist <= 0: continue

if (angle <= 15 or angle >= 345) and dist < FRONT_DANGER_MM:

danger = True

front_detected = True

if 60 <= angle <= 120:

left_space += dist

if 240 <= angle <= 300:

right_space += dist

laser_left, laser_right = read_laser_sensors()

if laser_left and laser_right:

play_sound("contact.mp3")

return True

if danger:

if front_detected:

play_sound("obstacle_front.mp3")

if left_space > right_space:

play_sound("obstacle_left.mp3")

set_motor(-AVOID_SPEED)

else:

play_sound("obstacle_right.mp3")

set_motor(AVOID_SPEED)

return True

else:

sweep_motor()

return False

def robust_connect_and_init(max_retry=5):

last_err = None

for attempt in range(1, max_retry+1):

dev = None

try:

print(f"[SYS] Connecting to RPLidar {attempt}/{max_retry} ...")

dev = RPLidar(PORT, baudrate=BAUD, timeout=TIMEOUT_S)

try: dev.start_motor()

except: pass

time.sleep(1.0)

print("[SYS] LIDAR Initialization complete")

return dev

except Exception as e:

last_err = e

print(f"[ERR] Connect/init failed ({attempt}/{max_retry}):", e)

try: dev.disconnect()

except: pass

time.sleep(0.6 * attempt)

raise RuntimeError(f"RPLidar initialization failed: {last_err}")

def iterate_scans(dev):

for scan in dev.iter_scans(max_buf_meas=False):

yield scan

I2C_GPS_ADDR = 0x10

I2C_GPS_REG = 0x00

def get_gps_position():

try:

data = bus.read_i2c_block_data(I2C_GPS_ADDR, I2C_GPS_REG, 80)

nmea_str = ''.join([chr(b) for b in data if b != 0])

for line in nmea_str.split('\n'):

if line.startswith('$GPGGA') or line.startswith('$GPRMC'):

try:

msg = pynmea2.parse(line)

if hasattr(msg, 'latitude') and hasattr(msg, 'longitude'):

return float(msg.latitude), float(msg.longitude)

except pynmea2.ParseError:

continue

except Exception as e:

print('[ERR] I2C GPS read error:', e)

return None, None

waypoints = [

[35.689626, 128.459282], # 경유지1

[35.686971, 128.458132], # 경유지2

]

current_wp = 0

WAYPOINT_TOLERANCE = 5.0 # m

def haversine(lat1, lon1, lat2, lon2):

R = 6371000

phi1, phi2 = math.radians(lat1), math.radians(lat2)

dphi = math.radians(lat2 - lat1)

dlambda = math.radians(lon2 - lon1)

a = math.sin(dphi/2)**2 + math.cos(phi1)*math.cos(phi2)*math.sin(dlambda/2)**2

return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

def calculate_bearing(lat1, lon1, lat2, lon2):

phi1, phi2 = math.radians(lat1), math.radians(lat2)

dlambda = math.radians(lon2 - lon1)

y = math.sin(dlambda) * math.cos(phi2)

x = math.cos(phi1)*math.sin(phi2) - math.sin(phi1)*math.cos(phi2)*math.cos(dlambda)

bearing = math.degrees(math.atan2(y, x))

return (bearing + 360) % 360

running = True

def sigint_handler(signum, frame):

global running

running = False

signal.signal(signal.SIGINT, sigint_handler)

signal.signal(signal.SIGTERM, sigint_handler)

def main():

global current_wp, running, lidar

try:

lidar = robust_connect_and_init(max_retry=5)

print("[SYS] Start navigation loop...")

scan_iter = iterate_scans(lidar)

while running:

lux = read_lux()

if lux is not None:

print(f"[LIGHT] Current lux: {lux:.1f}")

check_imu_alert()

try:

scan = next(scan_iter)

if process_scan(scan):

continue # 장애물 회피 중이면 GPS 안내 잠시 중단

except Exception as e:

print("[ERR] LIDAR scan error:", e)

if current_wp < len(waypoints):

lat, lon = get_gps_position()

if lat is None or lon is None:

print("[GPS] 위치 정보 없음, 대기 중...")

stop_motor()

time.sleep(1)

continue

wp_lat, wp_lon = waypoints[current_wp]

dist = haversine(lat, lon, wp_lat, wp_lon)

bearing = calculate_bearing(lat, lon, wp_lat, wp_lon)

heading = get_heading()

turn = (bearing - heading + 360) % 360

if turn > 180:

turn -= 360

print(f"[GPS] 현재위치: {lat:.6f}, {lon:.6f} / 목표: {wp_lat:.6f}, {wp_lon:.6f} / 거리: {dist:.1f}m / 방위: {bearing:.1f}° / 현재방위: {heading:.1f}° / 조향: {turn:.1f}°")

if dist < WAYPOINT_TOLERANCE:

play_sound('waypoint_reached.mp3')

current_wp += 1

stop_motor()

time.sleep(2)

continue

if abs(turn) > 20:

set_motor(20 if turn > 0 else -20)

else:

set_motor(30)

else:

play_sound('gps_done.mp3')

stop_motor()

break

time.sleep(1)

except KeyboardInterrupt:

stop_motor()

GPIO.cleanup()

except Exception as e:

print("[FATAL] Exception in main loop:", e)

traceback.print_exc()

stop_motor()

GPIO.cleanup()

if __name__ == "__main__":

main()

주야간 정지신호 인식성능 개선 Code

[Report]

길라잡이.pdf