Maze

AprilTag 자동 접근과 자율주행 같은 상위 알고리즘을 실제 로봇에서 동작시키기 위해 먼저 Jetson과 STM32를 연결한 저수준 이동 제어 시스템을 구축하였다.

NVIDIA Jetson Orin Nano는 카메라 영상 처리, 객체 인식, Visual SLAM, 이동 방향 결정과 같은 고수준 연산을 담당하고, STM32는 전달받은 명령을 이용하여 실제 좌·우 바퀴 모터를 제어하고 엔코더와 IMU 데이터를 수집하는 역할을 담당하도록 구성하였다.

ROS 2에서 생성된 속도 명령을 STM32로 전달하면 STM32가 이를 실제 모터 구동 값으로 변환하며, 바퀴 엔코더로 계산한 이동량과 센서 정보는 다시 Jetson으로 전달하도록 하였다.

즉,

Jetson에서 주행 명령 생성 → STM32로 전달 → 실제 모터 구동 → Encoder/IMU 측정 → ROS 2로 상태 정보 반환

의 양방향 제어 구조를 구축하였다.

초기에는 UART 기반 통신을 사용하였으며, 시스템 통합 과정에서 I2C 방식으로 전환하였다. 이 과정에서 데이터가 중복되거나 통신 시점에 따라 프레임 길이가 달라지는 문제가 발생하여, 데이터를 여러 번 나누어 읽는 방식에서 전체 데이터 프레임을 한 번에 수신한 뒤 필요한 정보를 해석하는 방식으로 수정하여 통신을 안정화하였다.

또한 일정 시간 동안 새로운 주행 명령이 들어오지 않을 경우 로봇이 자동으로 정지하도록 하여 실제 하드웨어 구동의 안전성도 확보하였다.

이 기반 시스템을 통해 이후 개발한 AprilTag 정밀 접근, Odometry 기반 보정, 자율주행 알고리즘이 실제 로봇의 물리적인 움직임으로 연결될 수 있는 기반을 마련하였다.

실제 재난 환경에서는 로봇이 사전에 만들어진 지도를 사용할 수 없는 상황을 고려해야 하기 때문에, 이동하면서 자신의 위치를 추정하고 동시에 주변 환경을 지도화할 수 있는 기능을 구축하였다.

Leader 로봇에 장착된 Intel RealSense D435의 카메라 영상을 Isaac ROS Visual SLAM과 연동하여, 로봇이 이동할 때 주변 영상의 특징 변화를 이용해 자신의 위치와 이동 경로를 실시간으로 추정하도록 하였다.

동시에 RealSense의 Depth 정보를 nvblox와 연결하여 벽, 바닥, 주변 구조물 등의 형상을 3차원 지도 형태로 생성하였다.

이를 통해 로봇이 재난 공간을 이동하면

카메라 영상 입력
→ 로봇 위치 및 이동 경로 추정
→ Depth 기반 주변 공간 복원
→ 3차원 Map 생성

이 동시에 수행되도록 하였다.

개발 초기에는 Visual SLAM과 Wheel Odometry의 좌표 기준이 서로 달라 로봇을 정지시킨 상태에서도 지도 좌표가 크게 이동하는 문제가 발생하였다.

원인을 분석한 결과 Wheel Odometry와 Visual SLAM이 서로 다른 시작 위치와 방향을 기준으로 계산되고 있음에도 이를 동일한 절대 위치 정보처럼 함께 사용하면서 좌표가 반복적으로 보정되는 문제가 확인되었다.

이를 해결하기 위해 바퀴 정보는 연속적인 이동 속도 정보로 사용하고, Visual SLAM은 전체 지도에서의 절대적인 위치 기준으로 사용하도록 역할을 분리하였다.

또한 Visual SLAM에서 전달되는 위치 정보의 신뢰도를 시스템에서 적절하게 처리하도록 설정을 수정하였다.

수정 후 동일한 정지 테스트를 반복한 결과, 이전에 발생하던 큰 좌표 이동이 사라지고 정지 상태에서 지도와 로봇 위치가 안정적으로 유지되는 것을 확인하였다. 이후 실제 로봇을 이동시키며 Visual SLAM이 이동 경로를 지속적으로 추적하고 nvblox가 주변 환경의 3D Mesh를 생성하는 것을 RViz에서 확인하였다.

최종적으로 RealSense, STM32 Wheel Odometry, Visual SLAM, 위치 추정, nvblox를 하나의 시스템으로 연결하여 로봇의 위치와 3차원 환경 지도를 동일한 Map 좌표계에서 사용할 수 있는 기반을 구축하였다.

구호물품의 정확한 위치를 로봇이 인식할 수 있도록 Intel RealSense D435 카메라를 이용한 AprilTag 인식 기능을 먼저 구축하였다.

카메라에서 검출된 Tag의 위치 정보를 로봇 기준 좌표로 변환하여 물품까지의 전방 거리와 좌우 위치를 계산하고, Tag가 로봇의 왼쪽에 있으면 좌회전, 오른쪽에 있으면 우회전, 중앙에 위치하면 전진하도록 기본 접근 알고리즘을 구현하였다.

초기 알고리즘에서는 화면에서 보이는 AprilTag의 중심이 로봇 정면에 오도록 맞추는 방식을 사용하였다. 실제 로봇을 이용한 테스트를 통해 Tag를 좌우로 이동시켰을 때 로봇이 해당 방향으로 회전하고, 중앙에 위치하면 전진하는 것을 확인하였다.

또한 Tag가 인식되지 않는 경우에는 로봇을 정지하도록 하여 카메라 인식 결과와 실제 바퀴 주행을 안전하게 연결하였다.

이번 단계에서는 이후 정밀 접근 알고리즘에 필요한 기본적인

AprilTag 인식 → 위치 계산 → 방향 판단 → 실제 이동

구조를 구축하였다.

초기 자동 접근을 실물에서 테스트하는 과정에서 Tag의 중심이 카메라 화면 중앙에 있다고 해서 로봇이 물품의 정면을 바라보고 있는 것은 아니라는 문제를 확인하였다.

예를 들어 물품과 AprilTag가 로봇에 대해 비스듬하게 놓여 있는 경우에도 Tag의 중심만 로봇 중앙에 맞추면 정렬 완료로 판단하였다. 이러한 상태에서는 로봇과 물품 사이에 각도 차이가 남기 때문에 그리퍼가 물품을 정면에서 안정적으로 파지하기 어려웠다.

이를 해결하기 위해 AprilTag의 위치뿐만 아니라 Tag가 어느 방향을 향하고 있는지에 대한 자세 정보도 함께 사용하도록 알고리즘을 수정하였다.

Tag가 놓인 평면에 수직인 방향을 계산하고, 그 방향의 연장선 위에 로봇이 도달해야 할 목표 지점을 생성하였다. 이후 로봇이 해당 지점으로 이동하고 최종적으로 Tag 면을 정면으로 바라보도록 방향을 맞추는 방식으로 변경하였다.

이를 통해 목표를 단순히 “Tag를 화면 중앙에 놓는 것”에서 “물품의 실제 정면에 로봇과 그리퍼를 위치시키는 것”으로 발전시켰다.

Tag의 방향을 이용하여 물품 정면으로 직접 이동하도록 수정한 뒤 실제 로봇에서 테스트하자 새로운 문제가 발생하였다.

먼 거리에서부터 Tag의 정면 목표점을 바로 향하도록 하면 로봇이 목표점을 맞추기 위해 크게 회전하였고, 이 과정에서 실제 AprilTag가 카메라 화면의 가장자리로 이동하다가 시야 밖으로 사라지는 현상이 발생하였다.

Tag가 사라지면 위치와 방향 정보를 모두 얻을 수 없기 때문에 오히려 정밀 정렬을 계속할 수 없었다.

이를 해결하기 위해 접근 거리에 따라 서로 다른 방법을 사용하는 Hybrid 정렬 알고리즘으로 변경하였다.

먼 거리에서는 기존 방식처럼 AprilTag의 중심을 보면서 접근하여 Tag를 계속 카메라에 유지하였다. 로봇이 물품 가까이 도달한 뒤에는 Tag의 기울기와 정면 방향 정보를 점진적으로 반영하여 물품 정면선으로 이동하도록 하였다.

또한 한 번에 큰 방향 보정을 하지 않고 각 프레임마다 적용되는 보정량을 제한하여, 로봇이 전진하면서 조금씩 물품의 정면 방향으로 수렴하도록 하였다.

즉,

먼 거리 : Tag를 놓치지 않는 안정적인 접근
→ 가까운 거리 : Tag 방향을 이용한 정밀 정렬

구조로 알고리즘을 발전시켰다.

Hybrid 접근 알고리즘을 반복적으로 테스트하면서, 목표 각도의 경계 부근에서 작은 카메라 측정 변화 때문에 로봇이 좌회전과 우회전을 반복하거나 접근과 회전을 반복하는 현상이 확인되었다.

또한 정밀 방향 보정 중 Tag가 카메라 가장자리로 이동하면서 다시 Tag를 놓치는 상황도 발생하였다.

이를 개선하기 위해 Tag가 화면 중앙에서 일정 이상 벗어나면 물품 정면 방향 보정보다 Tag를 다시 화면 중앙으로 가져오는 동작을 우선하도록 하는 시야 보호 기능을 추가하였다.

또한 회전을 시작하는 기준과 멈추는 기준을 서로 다르게 설정하여, 작은 값 변화만으로 상태가 계속 변경되는 현상을 줄였다.

접근 과정 역시

원거리 접근 → 근거리 정렬 → 최종 방향 정렬 → 저속 최종 접근 → 정렬 안정성 확인

의 단계로 세분화하였다.

최종 위치에 한 번 도달했다고 즉시 정렬 완료로 판단하지 않고, 위치와 방향 오차가 일정 시간 동안 안정적으로 유지되는지 확인한 뒤에만 ALIGNED 상태로 판단하도록 개선하였다.

이를 통해 센서 값이 순간적으로 정상 범위에 들어왔을 때 잘못 완료로 판단하는 문제를 줄이고 실제 그리퍼 파지에 적합한 자세를 보다 안정적으로 확보할 수 있었다.

AprilTag 정밀 접근 알고리즘을 실제 로봇에서 반복 테스트하면서, 최종 접근 단계에서 두 가지 문제가 확인되었다.

첫 번째 문제는 로봇이 물품에 매우 가까워질 경우 AprilTag의 일부가 카메라 화면 밖으로 잘리면서 더 이상 정상적인 위치 정보를 얻을 수 없다는 점이었다.

로봇의 정렬 자체에는 문제가 없었지만, Tag 전체가 카메라 화면에 들어오지 않기 때문에 최종 목표 위치까지 몇 cm를 남겨둔 상태에서 Tag Lost가 발생하고 로봇이 정지하였다.

이를 해결하기 위해 마지막으로 정상적으로 인식된 AprilTag의 위치를 저장하고, 최종 목표 위치까지 남아 있는 거리를 계산하도록 하였다.

이후 Tag 인식이 중단되면 카메라 정보를 계속 사용하려 하지 않고, 바퀴의 Odometry를 이용하여 로봇이 실제로 얼마나 이동했는지를 확인하면서 남은 짧은 거리만 추가로 전진하도록 하였다.

단, 모든 Tag Lost 상황에서 로봇이 계속 이동하면 위험할 수 있기 때문에 이 기능은 이미 최종 접근 단계에 진입했고, Tag를 잃기 직전의 위치와 방향 오차가 충분히 작은 경우에만 작동하도록 제한하였다. 그 외의 상황에서 Tag가 사라지면 기존과 동일하게 즉시 정지하도록 하였다.

이후 실제 그리퍼 파지 테스트를 진행하면서 두 번째 문제도 확인하였다.

AprilTag 알고리즘에서 정렬이 완료된 위치는 카메라와 Tag를 기준으로 설정된 위치였지만, 실제 로봇의 그리퍼 구조를 고려하면 물품을 안정적으로 잡기 위해 몇 cm 정도 추가로 전진해야 했다.

반대로 너무 많이 전진하면 그리퍼나 로봇 전면이 상자를 먼저 밀어버리는 현상도 발생하였다.

따라서 비전 기반 정렬이 완료된 이후에도 현재 위치와 실제 파지에 필요한 목표 위치의 차이를 계산하고, Odometry를 이용해 필요한 거리만 추가로 전진하는 단계를 추가하였다.

여러 차례 실물 테스트를 통해 최종 접근 거리를 조정하여 물품을 밀지 않으면서도 그리퍼가 충분히 깊게 들어갈 수 있는 위치를 찾았다.

결과적으로 최종 접근 과정은

카메라 기반 정밀 정렬
→ 근거리 Tag Loss 발생
→ 마지막 정상 Tag 위치 저장
→ Odometry 기반 남은 거리 이동
→ 실제 그리퍼 파지 위치까지 추가 거리 보정
→ 최종 정렬 완료

의 흐름으로 구성하였다.

이를 통해 카메라가 사용할 수 없는 마지막 짧은 구간을 Odometry로 보완하고, 비전 알고리즘상의 목표 위치와 실제 기구가 요구하는 파지 위치의 차이까지 함께 해결하였다.

최종적으로 AprilTag 기반 정렬 알고리즘과 Dynamixel 그리퍼 및 리프트를 하나의 자동 Sequence로 통합하였다.

AprilTag가 최초로 인식되면 RX-28 그리퍼를 열린 상태로 준비하고, 로봇은 앞서 개발한 Hybrid 정렬 알고리즘을 이용해 물품 방향으로 접근한다.

물품 정면에 대한 시각 정렬이 완료되면 실제 파지 위치까지 Odometry를 이용하여 추가 접근하고, 모든 접근 과정이 정상적으로 완료되었을 때 최종 ALIGNED 상태를 발생시키도록 하였다.

이후 RX-28 그리퍼가 자동으로 닫히면서 물품을 파지하고, 일정 시간 동안 파지가 유지된 후 RX-64 리프트가 작동하여 물품을 들어 올리도록 구성하였다.

실물 테스트에서는 그리퍼가 물품을 너무 약하거나 강하게 잡지 않도록 Goal Position을 조정하고, 리프트가 갑작스럽게 움직여 물품이 흔들리지 않도록 속도도 함께 보정하였다.

또한 일시적으로 AprilTag 인식이 끊어졌을 때 그리퍼가 임의의 위치로 움직이지 않도록 하여 주행 인식과 액추에이터 제어 사이의 안전성도 함께 개선하였다.

최종적으로

AprilTag 인식
→ 자동 접근
→ 물품 정면 정렬
→ 근거리 Odometry 보완
→ 실제 파지 위치까지 추가 접근
→ 그리퍼 파지
→ 리프트 상승

으로 이어지는 구호물품 자동 파지 과정을 완성하였다.

재난 환경을 탐색하는 로봇이 생존자를 자동으로 발견할 수 있도록 Intel RealSense D435의 RGB 영상과 YOLO를 연동한 사람 인식 기능을 먼저 구현하였다.

카메라에서 입력되는 영상을 ROS 2를 통해 YOLO에 전달하고, 영상에서 person 객체가 검출되면 사람의 위치에 Bounding Box를 표시하도록 구성하였다.

초기 단계에서는 먼저 한 명의 사람이 정상적으로 검출되는지 확인한 뒤, 여러 명이 동시에 화면에 들어오는 상황에서도 각각 독립적으로 인식되는지를 테스트하였다.

이 과정에서 단순히 사람이 보이는지를 판단하는 기능은 정상적으로 동작했지만, 실제 구조 임무에 활용하기 위해서는 “화면에 사람이 있다”는 정보만으로는 부족하다는 문제를 확인하였다.

카메라 화면의 같은 위치에 사람이 보이더라도 실제로 로봇에서 몇 m 떨어져 있는지 알 수 없었으며, 로봇이 이동한 이후에는 해당 사람이 실제 공간의 어디에 있었는지도 확인할 수 없었다.

따라서 다음 단계에서는 YOLO 인식 결과와 Depth Camera 정보를 결합하여 생존자의 실제 거리와 3차원 위치를 계산하도록 기능을 확장하였다.

YOLO를 통해 사람을 인식할 수 있게 된 이후, RealSense D435의 Depth 정보를 결합하여 사람까지의 실제 거리를 계산하도록 기능을 확장하였다.

YOLO에서 검출된 사람의 Bounding Box를 기준으로 해당 영역의 Depth 데이터를 가져오고, 이를 이용하여 로봇으로부터 생존자까지의 거리를 계산하였다.

이후 카메라의 영상 좌표와 Depth 값을 이용하여 생존자의 위치를 카메라 기준의 X, Y, Z 3차원 좌표로 변환하였다.

이를 통해 사람이 단순히 화면의 왼쪽 또는 오른쪽에 있다는 정보에서 벗어나,

왼쪽 또는 오른쪽으로 어느 정도 떨어져 있는지, 전방으로 몇 m 떨어져 있는지

를 실제 공간 좌표로 표현할 수 있게 되었다.

실물 테스트에서는 사람이 카메라 왼쪽에서 오른쪽으로 이동할 때 X 좌표가 정상적으로 변화하는지, 로봇과 가까워지거나 멀어질 때 Z 값이 실제 거리 변화와 동일한 방향으로 변화하는지를 확인하였다.

또한 여러 명의 사람이 동시에 인식되는 경우에도 각각의 사람에 대해 별도의 Depth와 3차원 좌표를 계산하도록 구현하였다.

이 단계에서 단순 객체 인식을 실제 위치 정보로 확장하였지만, 아직 계산된 위치가 카메라를 기준으로 한 상대 좌표라는 문제가 남아 있었다.

로봇이 이동하면 카메라 역시 함께 이동하기 때문에, 생존자를 발견했던 위치를 이후 다시 찾아가기 위해서는 좌표의 기준을 고정된 지도 좌표로 바꿀 필요가 있었다.

Depth Camera를 이용해 생존자의 3차원 위치를 계산할 수 있게 되었지만, 이 위치는 카메라를 기준으로 한 상대 좌표였다.

따라서 로봇이 다른 위치로 이동하면 동일한 생존자의 Camera XYZ 값도 달라지며, 처음 생존자를 발견했던 실제 위치를 계속 유지하기 어렵다는 문제가 있었다.

이를 해결하기 위해 Visual SLAM에서 사용하는 전체 map 좌표계와 생존자 인식 기능을 연결하였다.

ROS 2 TF를 이용하여 카메라 기준으로 계산된 생존자의 3차원 좌표를 고정된 Map 좌표로 변환하도록 구현하였다.

이를 통해 로봇이 생존자를 발견한 뒤 다른 위치로 이동하더라도, 생존자가 발견된 위치는 전체 지도에서 동일한 위치에 남아 있도록 하였다.

변환된 생존자 위치는 RViz에서 Marker로 표시하였다. 또한 nvblox를 이용하여 생성한 3차원 환경 지도와 생존자 위치를 동일한 화면에 표시하여,

환경 구조 + 로봇 위치 + 생존자 위치

를 함께 확인할 수 있도록 구성하였다.

실물 테스트 과정에서는 RViz의 기준 좌표가 카메라나 다른 frame으로 설정되어 있을 경우 Survivor Marker가 정상 위치에 보이지 않는 문제도 확인하였다. 이후 RViz의 Fixed Frame을 map으로 통일하여 3D Mapping 결과와 생존자 위치가 같은 좌표계에 정상적으로 표시되는 것을 검증하였다.

이 단계에서 “사람을 카메라에서 찾는 기능”을 “지도에서 생존자의 위치를 기록하는 기능”으로 확장할 수 있었다.

생존자의 위치를 Map에 표시할 수 있게 된 이후 실제 환경에서 로봇을 움직이며 테스트하자 새로운 문제가 발생하였다.

기존 방식에서는 YOLO가 사람을 검출할 때마다 현재 위치를 기록하기 때문에 한 명의 사람이 움직이면 여러 위치에서 서로 다른 생존자로 등록되는 현상이 발생하였다.

실제 테스트에서도 소수의 사람만 존재했지만 이동 과정에서 실제 인원보다 더 많은 ID가 생성되는 현상을 확인하였다.

또한 사람이 카메라 화면 밖으로 나가면 해당 객체에 대한 새로운 검출 결과가 들어오지 않기 때문에, 현재 카메라에 보이는 사람과 이전에 발견했던 사람을 구분해 관리할 필요가 있었다.

이를 해결하기 위해 각 생존자에게 고유한 ID를 부여하고 위치와 상태를 지속적으로 관리하는 Persistent Tracking 구조를 개발하였다.

새로운 사람이 검출되었을 때 기존에 기록된 생존자의 위치와 비교하여 동일한 사람일 가능성이 높은 경우 기존 ID의 위치를 업데이트하도록 하였다.

생존자가 이동하더라도 위치 변화가 일정 범위 내라면 동일한 ID를 유지하고, 일시적으로 카메라에서 사라져도 즉시 삭제하지 않고 마지막으로 확인된 위치를 그대로 유지하도록 하였다.

이후 동일한 사람이 다시 카메라에 나타나면 새로운 ID를 생성하는 대신 기존 생존자와 다시 연결하도록 개선하였다.

실물 테스트에서는

정지한 사람의 동일 ID 유지
→ 사람이 이동해도 동일 ID 유지
→ 프레임 밖으로 사라지면 마지막 위치 저장
→ 다시 등장하면 기존 ID로 재인식

되는 과정을 확인하였다.

이를 통해 생존자를 프레임 단위의 객체가 아닌 탐색 과정 전체에서 지속적으로 관리해야 하는 구조 대상으로 취급하도록 시스템을 발전시켰다.

Persistent Tracking을 구현한 이후 장시간 실제 환경에서 테스트하면서, YOLO가 간혹 의자나 옷 등의 물체를 순간적으로 사람으로 잘못 인식하는 현상을 확인하였다.

기존에는 검출 결과가 일정 조건을 만족하면 바로 생존자 ID를 생성했기 때문에, 1~2프레임의 짧은 오검출도 실제 생존자로 기록될 가능성이 있었다.

이를 개선하기 위해 사람이 처음 검출되었을 때 즉시 정식 생존자로 등록하지 않고, 일정 시간 동안 반복적으로 사람이 확인된 경우에만 새로운 생존자로 확정하는 확인 단계를 추가하였다.

반대로 이미 등록된 생존자는 한두 프레임 동안 보이지 않는다고 바로 삭제하지 않고 일정 시간 동안 기존 상태를 유지하도록 하여 카메라 가림이나 일시적인 YOLO 인식 실패에도 ID가 안정적으로 유지되도록 하였다.

또한 생존자의 현재 관측 상태를 보다 쉽게 확인할 수 있도록 상태 시각화 기능을 개선하였다.

현재 카메라에서 확인되는 생존자는 일반 상태로 표시하고, 시야에서 사라진 생존자는 마지막으로 확인된 위치를 유지하면서 별도의 Lost/Last Seen 상태로 표시하였다.

Marker 위에는 생존자 ID와 상태 정보를 함께 표시하여 구조대원이 3D 지도에서 각 생존자의 위치와 현재 관측 상태를 쉽게 파악할 수 있도록 하였다.

실물 검증에서는 여러 위치에 사람이 있을 때 각각의 생존자가 정확하게 기록되고, 사람이 이동할 경우 기존 ID가 유지되며, 프레임 밖으로 나가면 마지막 위치와 상태가 변경되고, 다시 등장했을 때 기존 ID로 연결되는 것을 확인하였다.

마지막으로 개별적으로 실행하던 YOLO, 생존자 위치 계산, Tracking 및 RViz 시각화 기능을 정리하여 한 번의 실행으로 전체 생존자 인식 파이프라인을 구동할 수 있는 구조로 통합하였다.

최종적으로

YOLO 사람 인식
→ Depth 기반 3차원 위치 계산
→ Map 좌표 변환
→ 생존자 ID Tracking
→ 상태 관리
→ RViz 3D 지도 표시

가 하나의 연속된 생존자 탐색 시스템으로 구성되었다.

기존 생존자 인식 시스템에서는 YOLO와 Depth Camera를 이용하여 생존자를 발견하고 실제 3차원 위치를 지도에 기록할 수 있었다.

여기서 한 단계 더 확장하여, 생존자의 위치뿐만 아니라 카메라를 통해 관찰할 수 있는 상태 정보를 함께 제공하기 위해 눈 개폐 상태 인식 기능을 개발하였다.

먼저 눈 이미지를 OPEN과 CLOSED 두 가지 상태로 구분할 수 있도록 YOLO 기반 이미지 분류 모델을 학습하였다.

공개 Eye-State 데이터셋을 학습용, 검증용, 테스트용으로 나누어 모델을 학습하였으며, 학습이 완료된 모델이 눈 이미지를 입력받아 OPEN/CLOSED 상태와 각 예측의 신뢰도를 출력하도록 구성하였다.

공개 데이터셋의 별도 test split 400장에서는 전체 이미지를 올바르게 분류하는 결과를 확인하였다. 다만 공개 데이터와 실제 RealSense 카메라 영상 사이에는 조명, 얼굴 크기, 촬영 각도 등의 차이가 존재하기 때문에 공개 데이터셋 성능을 실제 현장 정확도로 그대로 판단하지 않고 이후 실제 D435 영상에서 별도로 검증하는 단계를 진행하였다.

또한 눈이 열려 있거나 닫혀 있다는 사실만으로 사람의 실제 의식 여부를 단정할 수 없으므로, 본 기능은 생존자의 의식 상태를 직접 판정하는 기능이 아니라 구조 판단을 보조하기 위한 추가 관찰 정보를 제공하는 기능으로 설계하였다.

외부 데이터셋 출처 및 활용
Eye-State 분류 모델 학습에는 Roboflow Universe의 Eye-State Classification v1 공개 데이터셋을 사용하였다. 데이터셋은 OPEN/CLOSED 눈 이미지 분류 모델의 학습 및 검증에 활용하였으며, 공개 데이터와 실제 RealSense D435 영상의 촬영 환경 차이를 고려하여 실제 카메라 영상에서는 별도의 검증 과정을 진행하였다.
License: CC BY 4.0

https://universe.roboflow.com/mujahid-raja-epyoq/eye-state-classification/dataset/1

이미지 출처: Roboflow Universe, Eye-State Classification v1

Eye-State 분류 모델을 실제 구조 로봇에서 활용하기 위해서는 전체 카메라 영상에서 생존자의 얼굴과 눈 위치를 자동으로 찾는 과정이 필요하였다.

이를 위해 RealSense D435의 RGB 영상에서 얼굴을 검출하고, 얼굴의 주요 특징점을 이용하여 양쪽 눈의 위치를 자동으로 추출하는 기능을 구현하였다.

얼굴이 검출되면 양쪽 눈 주변의 이미지 영역을 자동으로 잘라 Eye-State 분류 모델에 전달하고, 각각의 눈에 대해 OPEN 또는 CLOSED 상태와 예측 신뢰도를 계산하도록 구성하였다.

전체 흐름은

D435 RGB 영상
→ 얼굴 검출
→ 양쪽 눈 위치 확인
→ Eye ROI 자동 추출
→ AI OPEN/CLOSED 분류
→ 결과 영상 출력

으로 구성하였다.

실제 D435 영상에서도 얼굴에서 양쪽 눈 영역이 자동으로 추출되고 분류 모델까지 연결되는 것을 확인하였다. 다만 공개 데이터에서 높은 성능을 보였던 모델이 실제 카메라 영상에서는 조명, 얼굴 크기, 눈 영역의 해상도 등에 따라 일부 잘못된 분류를 출력하는 사례가 확인되었다. 따라서 단일 프레임의 분류 결과만으로 상태를 결정하는 것은 안정적이지 않다는 문제를 확인하였다.

이를 바탕으로 다음 단계에서는 한 장의 영상 결과가 아니라 일정 시간 동안 여러 프레임의 Eye-State 결과를 함께 분석하는 방식으로 알고리즘을 개선하였다.

실제 RealSense D435 영상에서 Eye-State 기능을 테스트한 결과, 조명이나 얼굴 움직임, 눈 깜박임 등에 의해 한 프레임의 예측 결과가 순간적으로

OPEN -> CLOSED -> OPEN

과 같이 변하는 현상이 발생하였다.

따라서 한 장의 영상 결과를 바로 생존자의 상태 정보로 사용하는 방식은 적절하지 않다고 판단하였다.

이를 해결하기 위해 일정 시간 동안의 Eye-State 결과를 저장하고, 여러 프레임에서 반복적으로 관찰된 결과를 종합하여 최종 상태를 결정하는 시간 기반 안정화 알고리즘을 추가하였다.

현재 구현에서는 약 3초 동안 관측된 결과를 유지하고, 충분한 수의 유효한 눈 인식 결과가 확보된 경우에만 안정적인 상태를 출력하도록 구성하였다. 좌·우 눈을 각각 분석한 뒤 두 눈의 상태를 종합하여 OPEN, CLOSED, 또는 판단이 불분명한 경우 UNKNOWN 상태로 구분하도록 하였다.

이를 통해 순간적인 한두 프레임의 잘못된 분류 결과가 최종 상태에 바로 반영되는 문제를 줄이고자 하였다.

또한 얼굴 위치가 프레임마다 조금씩 이동하더라도 일정 시간 동안 동일한 얼굴 관측 결과를 연결하는 임시 Tracking 구조를 추가하였다.

기존에는 RealSense D435와 Visual SLAM을 이용하여 로봇의 위치를 추정하고, nvblox를 통해 주변 환경의 3차원 지도를 생성하는 기능까지 구현하였다. 이번 단계에서는 생성된 공간 정보를 실제 이동에 활용하기 위해 Visual SLAM, Wheel Odometry·IMU, nvblox와 Nav2를 하나의 자율주행 시스템으로 통합하였다.

로봇의 연속적인 이동 정보는 Wheel Odometry와 IMU를 결합하여 ODOM -> base_link 위치를 추정하고, Visual SLAM의 위치 정보는 전체 지도상의 위치를 보정하는 데 활용하도록 구성하였다. 또한 nvblox에서 생성된 환경 정보를 Nav2의 Costmap과 연결하여 로봇 주변의 자유 공간과 장애물을 고려한 경로를 생성할 수 있도록 하였다.

전체 시스템은

RealSense D435 센서 입력
→ Wheel Odometry·IMU 및 Visual SLAM 기반 위치 추정
→ nvblox 환경 지도 생성
→ Nav2 Costmap 구성
→ 목표 위치까지의 경로 생성
→ 주행 명령 생성

의 흐름으로 구성하였다.

RViz에서 목표 위치와 도착 방향을 지정하면 Nav2의 ComputePathToPose가 현재 위치와 주변 환경을 바탕으로 odom 좌표계 기준의 주행 경로를 생성하는 것까지 정상적으로 확인하였다. 또한 Nav2의 Planner, Controller, BT Navigator가 정상 활성화되고, nvblox 환경 정보가 Nav2 Costmap에 연결되는 것을 검증하였다.

이후 실제 NavigateToPose 목표를 전달하여 주행 시험을 진행하였으나, 목표 자체는 정상적으로 수락된 반면 Controller가 현재 Costmap에서 유효한 주행 궤적을 찾지 못해 이동이 중단되는 문제가 발생하였다.

이 문제를 분석한 결과 로봇 앞쪽의 그리퍼가 Depth Camera에 함께 관측되어 nvblox 지도에서 장애물로 인식될 가능성이 확인되었고, 이후 로봇 자체 구조물을 제거하는 Depth Self-Filter 개발로 이어졌다.

Nav2를 실제 Leader 로봇에 적용하여 목표 위치 주행을 시험하는 과정에서, 로봇 전방에 이동 가능한 공간이 있음에도 Nav2가 유효한 주행 경로를 찾지 못하고 이동을 중단하는 문제가 발생하였다.

원인을 확인하기 위해 Nav2 Costmap에 표시된 장애물 위치와 실제 로봇 구조를 비교하였다. 그 결과 로봇 바로 앞에 장애물로 표시된 영역이 전방 그리퍼의 위치와 거의 일치하였으며, RealSense D435가 로봇 자신의 그리퍼를 Depth 정보로 측정하면서 nvblox가 로봇의 일부를 주변 장애물로 지도에 누적하고 있음을 확인하였다.

이를 해결하기 위해 RealSense Depth 영상이 nvblox에 전달되기 전에 로봇 자체 구조물에 해당하는 깊이 정보를 제거하는 Depth Self-Filter를 추가하였다.

카메라에서 측정된 Depth 정보를 로봇 기준 좌표로 변환한 뒤, 그리퍼가 존재하는 영역에 포함된 점을 제거하고 필터링된 Depth만 nvblox에 전달하도록 구성하였다.

초기에는 그리퍼 중심 영역만 제거하였으나, 실제 그리퍼 손가락의 개폐와 주행 중 흔들림으로 인해 일부 자기 장애물이 남는 문제가 확인되었다. 이에 그리퍼가 실제로 움직일 수 있는 전체 범위까지 필터 영역을 확장하였다.

수정 후 새로운 지도에서 다시 Costmap을 확인한 결과, 로봇 중심에서 측정한 Costmap 표본 10개가 모두 0으로 나타나 기존의 자기 구조물에 의한 장애물 점유가 제거되었음을 확인하였다.

이를 통해 카메라에 함께 보이는 로봇 자신의 구조물과 실제 주변 장애물을 구분하여 Mapping할 수 있는 기반을 구축하였다.

Depth Self-Filter를 적용하여 카메라 영상에서 로봇 자신의 구조물이 장애물로 인식되는 문제를 개선한 이후, Nav2의 경로 계획에서도 실제 로봇의 크기와 형상을 정확하게 반영할 필요가 있었다.

기존 설정에서는 로봇의 충돌 영역을 단순한 원형으로 표현하고 있어, 차체 앞쪽으로 길게 돌출된 그리퍼의 형상을 충분히 반영하기 어려웠다.

이를 개선하기 위해 URDF와 실제 로봇 구조를 기준으로 차체와 전방 그리퍼를 모두 포함하는 6개 꼭짓점의 다각형 Footprint를 적용하였다.

이를 통해 Nav2가 경로를 생성하거나 장애물을 회피할 때 단순히 로봇 중심에서의 거리만 계산하는 것이 아니라, 실제 로봇의 외곽이 주변 장애물과 충돌하는지를 기준으로 주행 가능 여부를 판단하도록 하였다.

또한 실제 로봇 크기보다 약간 넓은 안전 여유를 추가하여 벽이나 장애물에 지나치게 가깝게 접근하지 않도록 구성하였다. 그리퍼의 두 손가락 사이처럼 일부 비어 있는 공간도 전체 외곽에 포함하여 좁은 구간에서는 보다 보수적으로 경로를 판단하도록 하였다.

이 단계에서는 Depth Self-Filter와 Footprint의 역할을 명확하게 분리하였다.

Depth Self-Filter는 센서 영상에서 로봇 자신의 구조물을 제거하고, Footprint는 Nav2가 실제 로봇의 크기를 기준으로 충돌 여부를 판단하도록 한다.

적용 후 Local Costmap과 Global Costmap에서 설정한 다각형 Footprint가 정상적으로 반영되는 것을 확인하였다.

Depth Self-Filter와 다각형 Footprint를 적용한 뒤, 개선된 Nav2 시스템을 실제 Leader 로봇에서 다시 시험하였다.

시험에서는 Visual SLAM 위치 추정, nvblox 3D Mapping, Nav2 경로 계획 및 주행 제어, STM32 모터 제어를 동시에 실행하고, 지정한 목표 위치까지 실제 로봇이 자율적으로 이동하는지를 확인하였다.

첫 번째 시험에서는 안전성을 고려하여 선속도 상한을 0.05 m/s로 낮게 설정하였다. 로봇은 목표 방향으로 일부 이동하였지만, 일정 시간 동안 Nav2가 요구하는 만큼의 진행이 이루어지지 않아 오류와 함께 주행이 중단되었다.

이를 통해 센서·지도 문제뿐만 아니라 실제 로봇의 구동 특성에 맞는 주행 속도 설정도 중요하다는 점을 확인하였다.

이후 선속도 상한을 0.10 m/s로 조정하고 다시 시험하였다.

먼저 odom 기준 약 0.5 m 전방 목표를 설정한 결과 정상적으로 목표 지점에 도달하였으며, 이후 약 0.9 m 전방 목표를 추가로 설정한 시험에서도 Nav2 Action이 SUCCEED 로 종료되었다.

주행 중에는 단순히 직진 명령만 생성되는 것이 아니라, 현재 로봇 방향과 목표 경로 사이의 오차를 지속적으로 계산하여 좌·우 방향을 보정하는 회전 명령이 함께 발생하는 것도 확인하였다.

최종적으로

Visual SLAM 위치 추정
→ nvblox 환경 인식
→ Nav2 목표 경로 생성
→ 주행 방향·속도 제어
→ STM32 모터 명령 전달
→ 실제 로봇 목표 위치 도달

의 전체 자율주행 경로가 실제 하드웨어에서 연결되어 동작하는 것을 검증하였다.

이를 통해 기존의 “주변 환경을 지도화하는 로봇”에서 “생성한 공간 정보를 이용해 원하는 목표 위치까지 스스로 이동하는 로봇”으로 기능을 확장하였다.

두 로봇이 하나의 물체를 함께 운반하기 위해서는 먼저 Leader의 이동 정보가 Follower까지 안정적으로 전달되고, 두 로봇이 서로의 실행 상태를 확인할 수 있는 통신 구조가 필요하였다.

이를 위해 ROS 2 DDS를 기반으로 Leader와 Follower 사이의 협동 제어 통신 구조를 구현하였다. Leader에서 발생한 주행 명령은 협동 제어 노드를 거쳐 Follower로 전달되고, Follower에서는 수신한 명령을 기존 명령 선택기와 Velocity Guard를 거쳐 STM32 모터 제어까지 전달하도록 구성하였다.

단순히 명령만 전달하는 방식에서는 상대 로봇의 연결이 끊어진 상태에서도 마지막 명령이 계속 남을 수 있기 때문에, Heartbeat를 이용하여 상대 로봇의 통신 상태를 지속적으로 확인하도록 하였다. 협동 기능이 활성화되어 있고 상대 로봇의 상태가 정상적으로 수신되는 경우에만 주행 명령을 전달하며, 일정 시간 동안 Heartbeat나 주행 명령이 갱신되지 않으면 자동으로 0속도를 출력하도록 하였다.

또한 실제 모터를 구동하지 않고도 두 Jetson 사이의 DDS 연결과 명령 전달 경로를 먼저 확인할 수 있도록 구성하여, 네트워크와 제어 문제를 단계적으로 점검할 수 있도록 하였다.

이 단계에서는 우선 Leader의 명령을 Follower까지 안전하게 전달하는 기본 구조를 완성하였으며, 이후 단순 명령 전달이 아니라 두 로봇이 각각의 경로를 따라 이동하도록 협동 제어를 확장하였다.

초기 협동 구조에서는 Leader의 주행 명령을 Follower에 그대로 전달하였지만, 두 로봇이 하나의 물체를 양쪽에서 파지한 상태에서는 Leader와 Follower가 동일한 경로를 그대로 따라갈 수 없다는 문제가 있었다.

특히 곡선 구간에서는 물체를 중심으로 두 로봇의 이동 궤적이 서로 달라지므로, Leader의 경로와 물체의 크기, 두 로봇의 배치 관계를 함께 고려하여 Follower가 따라야 할 경로를 별도로 계산할 필요가 있었다.

이를 위해 두 로봇과 물체를 연결된 하나의 기하 구조로 정의하고, Leader의 Nav2 경로로부터 물체의 이동 경로와 Follower의 이동 경로를 자동으로 계산하는 협동 운반 경로 생성 기능을 구현하였다.

경로가 급격하게 꺾이는 경우 두 로봇 사이의 결합 구조가 유지되기 어려울 수 있기 때문에, 협동 운반에서는 단독 주행보다 완만한 경로를 사용하도록 하였다. 또한 생성된 경로가 두 로봇과 물체 전체가 이동할 수 있는 공간 안에 있는지 확인하여, 장애물과 충돌 가능성이 있는 경로는 주행 전에 거절하도록 구성하였다.

개발 과정에서는 곡선 경로에서 Follower 경로 계산이 불안정해지는 문제가 발생하였고, 이를 개선하기 위해 Leader 경로의 곡률을 이용해 두 로봇 사이의 상대 자세를 직접 계산하도록 알고리즘을 수정하였다. 곡률 변화도 부드럽게 처리하여 급격한 자세 변화가 발생하지 않도록 하였다.

이를 통해 단순한 속도 복사가 아니라 Leader가 이동할 경로를 기준으로 Follower가 실제로 따라야 할 경로를 별도로 생성하는 협동 운반 구조를 구축하였다.

Leader와 Follower는 각각 별도의 STM32와 바퀴 Odometry를 사용하기 때문에, 두 로봇에서 사용하는 odom 좌표계의 이름이 같더라도 실제 시작 위치와 방향은 서로 다르다.

따라서 Leader 기준으로 계산한 Follower 경로를 그대로 전달하면 실제 Follower의 현재 위치와 경로가 맞지 않는 문제가 발생한다.

이를 해결하기 위해 두 로봇이 물체를 파지한 초기 합체 상태를 기준으로 Leader와 Follower 사이의 위치·방향 관계를 계산하고, Leader 기준으로 생성된 Follower 경로를 Follower 자신의 Odometry 좌표계로 변환하는 기능을 구현하였다.

Follower는 협동 운반을 준비할 때 자신의 현재 위치와 방향을 기준점으로 저장하고, 이후 전달받은 전체 경로를 해당 기준에 맞추어 변환한다. 준비가 완료된 뒤 로봇이 크게 움직여 초기 기준이 달라진 경우에는 기존 경로를 그대로 사용하지 않고 다시 준비하도록 하여 잘못된 좌표 변환으로 인한 주행을 방지하였다.

또한 두 로봇이 물체의 양쪽에서 서로 반대 방향을 바라보는 구조에서는 Follower가 전진이 아니라 후진 방향으로 경로를 추종해야 하는 상황이 발생한다. 이를 위해 역주행이 가능한 Pure Pursuit 기반 경로 추종 기능을 적용하고, 경로에서 지나치게 벗어나거나 결합 구조가 허용하는 회전 범위를 초과할 경우 정지하도록 하였다.

이를 통해 두 로봇이 서로 다른 위치 기준을 사용하더라도 각자의 Odometry를 이용하여 하나의 협동 운반 경로를 따라 이동할 수 있는 구조를 구현하였다.

협동 운반에서는 Leader만 준비된 상태에서 먼저 움직이거나 Follower가 다른 경로를 가지고 출발하면 물체의 자세가 틀어지거나 두 로봇 사이에 큰 힘이 발생할 수 있다.

따라서 실제 출발 전에 두 로봇이 동일한 협동 운반 작업을 준비하고 있는지 확인하는 과정과 실제 출발 명령을 분리하였다.

B 입력을 이용하여 협동 운반 준비를 시작하면 기존 주행을 정지하고, 새로운 Nav2 목표에 대해 Leader 경로와 Follower 경로를 생성한다. Follower는 전달받은 경로와 자신의 Odometry, 명령 제어 상태를 확인한 뒤 모든 준비 조건이 만족된 경우에만 READY 상태를 반환하도록 구성하였다.

이전 작업에서 전달된 메시지가 새로운 협동 작업에 잘못 사용되지 않도록 각 협동 작업에 별도의 세션 정보를 부여하고 경로가 동일한지 확인하는 구조도 적용하였다.

모든 준비가 완료된 이후 N 입력으로 실제 출발을 요청하면, 두 로봇이 즉시 움직이는 것이 아니라 출발 준비 상태를 다시 확인하고 공통 출발 시점을 결정하여 동시에 주행을 시작하도록 하였다.

또한 출발 직전까지 상대 로봇의 Heartbeat와 통신 상태를 확인하고, 통신 지연이나 준비 상태에 문제가 발생하면 출발하지 않고 정지 상태를 유지하도록 하였다.

이를 통해 협동 운반을

경로 생성 → 경로 공유 → 양쪽 준비 확인 → 출발 시점 동기화 → 협동 주행

의 단계로 나누어 안전하게 수행할 수 있도록 하였다.

두 로봇이 동시에 출발하더라도 실제 바퀴 속도와 구동 상태가 완전히 같지 않기 때문에, 주행 시간이 길어질수록 Leader와 Follower 사이의 위치 차이가 발생할 수 있다.

특히 하나의 물체를 함께 파지한 상태에서는 한쪽 로봇이 다른 로봇보다 빠르게 이동하면 물체가 회전하거나 두 로봇의 결합부에 큰 힘이 발생할 수 있다.

이를 해결하기 위해 협동 주행 중 Leader와 Follower가 현재 경로 진행률과 주행 상태를 지속적으로 공유하도록 구성하였다. Follower는 Leader의 이동 상태에 맞추어 자신의 속도를 조정하고, 자신이 Leader보다 지나치게 앞서가는 경우에는 자동으로 감속하도록 하였다.

또한 두 로봇 사이의 진행 차이가 일정 범위를 초과하거나 상대 로봇의 Heartbeat가 일정 시간 동안 수신되지 않으면 양쪽 로봇의 주행을 정지하고 협동 운반을 중단하는 안전 로직을 적용하였다. 정지 시에는 단순히 0속도만 전달하는 것이 아니라 경로 추종과 모터 명령 제어까지 함께 중단하여 자동으로 다시 움직이지 않도록 구성하였다.

이러한 기능을 적용한 뒤 두 로봇을 실제 물체의 양쪽에 배치하고 협동 운반 구동을 진행하였다. Leader가 생성한 경로를 기준으로 Follower가 자신의 경로를 추종하면서, 두 로봇이 물체를 함께 파지한 상태로 이동하는 협동 운반 시연을 완료하였다.

전체 협동 운반 과정은

Leader 경로 생성 → Follower 경로 계산 → 독립 Odometry 좌표 변환 → 준비 상태 확인 → 동기 출발 → 경로 추종 → 진행률 공유 → 실제 물체 협동 운반

의 흐름으로 동작하도록 구현하였다.

이를 통해 초기의 단순 Leader 명령 전달 방식에서 발전하여, 두 로봇이 각자의 위치 정보를 이용하면서 서로의 이동 상태를 공유하고 하나의 물체를 함께 운반하는 Leader–Follower 협동 제어 시스템을 완성하였다.

재난 탐색과 협동 운반 임무를 장시간 수행할 경우 배터리 교체나 시스템 재부팅으로 인해 로봇을 일시적으로 종료해야 할 수 있다. 기존에는 시스템을 다시 실행하면 nvblox 지도를 처음부터 생성해야 하므로, 이전까지 탐색한 공간 정보를 계속 활용하기 어려운 문제가 있었다.

이를 보완하기 위해 실행 중 생성된 nvblox 3D 지도를 체크포인트 파일로 저장하는 기능을 구성하였다. 지도 저장과 함께 저장 시점의 로봇 위치·방향과 이후 좌표계 기준을 다시 맞추는 데 필요한 정보를 함께 기록하도록 하여, 재시작 이후 기존 지도와 로봇 위치 관계를 유지할 수 있도록 하였다.

실제 실행 환경에서 nvblox 지도 저장을 수행한 결과 map.nvblx 파일이 정상적으로 생성되었으며, 지도와 함께 저장 시점의 Odometry 위치·방향 및 기준 정보를 보존하도록 구성하였다.

지도 체크포인트는 로봇을 임의의 위치에서 바로 다시 사용하는 방식이 아니라, 저장 당시와 동일한 위치와 방향을 기준으로 시스템을 재시작한 뒤 기존 지도를 활용하는 방식으로 설계하였다. 이를 통해 새로운 위치에서 잘못된 좌표 기준으로 기존 지도가 사용되는 것을 방지하도록 하였다.

또한 시스템이 다시 시작되었을 때 이전의 주행 명령이 남아 로봇이 갑자기 움직이는 상황을 방지하기 위해, 명령 선택기는 STOP, 협동 제어는 IDLE 상태에서 시작하도록 구성하였다. 이후 센서와 위치 정보가 정상적으로 준비된 것을 확인한 뒤 새로운 주행 또는 협동 운반 임무를 시작하도록 하였다.

이를 통해 단순히 실시간으로 지도를 생성하는 기능을 넘어, 탐색 과정에서 구축한 3D 환경 정보를 저장하고 시스템 재시작 이후에도 다시 활용할 수 있는 운용 구조를 마련하였다.