이 글은 고정밀 GNSS(Global Navigation Satellite System), IMU(Inertial Measurement Unit)와 ROS(Robot Operating System)를 연동하는 것에 대한 내용을 간단히 요약한 것이다. 여기서 사용한 GNSS센서는 3DM GX5이다.
임베디드 보드는 ODROID(오드로이드. 설치 참고) UX4를 사용하였으나, 라떼판다(윈도우에서 운용됨), NVIDIA TK 등 다른 임베디드 보드를 사용하더라도 큰 관계는 없다.
이 제품은 GPS, GLONASS, DeiDou, Galileo를 활용한 GNSS receiver이며, 정밀한 AHRS(Attitude and Heading Reference System) 값을 얻을 수 있다.
AHRS data
이 센서는 가격이 3~400백만원 수준이며, 드론, 로봇틱스 등 다양한 분야에 활용되고 있다. 이 센서는 가격이 상대적으로 저렴한? 편이라 한다. 다만, ROS 우부투, 리눅스 기반 작업을 할 때는 문서화가 잘 안되어 있어, 삽질이 좀 필요하다. 윈도우 버전은 설치 프로그램도 함께 제공하는 등 상대적으로 작업이 쉽다.
만약, ROS가 최신 MELODIC 버전이라면 github에서 소스코드를 다운로드한 후 다음과 같이 빌드해서 ROS 패키지를 설치해야 한다.
cd ~/catkin_ws/src
git clone https://github.com/ros-drivers/microstrain_mips.git
cd ..
catkin_make -j1
참고로, 패키지 정보는 다음과 같이 정의되어 있다.
제대로 빌드되었다면, 패키지가 설치되며, 다음 명령을 통해 microstrain_mips 패키지를 확인할 수 있다.
rospack list-names
ROS 패키지 리스트
이제 센서와 컴퓨터를 USB로 연결해 본다. 센서와 USB에 연결된 포트 이름은 상황에 따라 동적으로 할당되므로, 다음 명령을 이용해 센서에 연결된 USB serial port 이름 리스트를 먼저 확인하고 적절한 포트 이름을 지정해야 한다.
dmesg | grep tty
연결된 포트는 아래 명령으로 좀 더 자세한 정보를 확인할 수 있다.
ls -l /dev/ttyACM0
센서 정보는 다음 명령을 통해 확인할 수 있다.
cd ~/catkin_ws/devel/lib/microstrain_mips
./GX4-45_Test /dev/ttyACM0 115200
GNSS 센서와 연결상태 확인
이제 다음과 같이 ROS node를 실행해 본다. 참고로, 아래 글의 파라메터를 확인해 적절한 값을 입력해야 동작한다. 예를 들어, 포트이름을 정확히 입력하지 않으면 센서의 ROS node는 실행되지 않을 것이다.
rosrun microstrain_mips microstrain_mips_node _port:=/dev/ttyACM0 _device_setup:=true
이렇게 실행된 결과 다음과 같은 토픽 데이터를 얻을 수 있다.
gps/fix (sensor_msgs/NavSatFix): Navigation Satellite fix for any Global Navigation Satellite System
참고로, IMU 데이터 구조는 ROS IMU Sensor Driver 에 설명되어 있다. Accelerometers, Gyroscopes, Magnetometers, Orientation로 구성되어 있으며, IMU데이터에서 많이 사용하는 Orientation은 quaternion 형태로 출력된다.
imu/data (sensor_msgs/Imu) 토픽은 quaternion orientation 추정값을 포함하고, imu/data_raw (sensor_msgs/Imu) 토픽은 accelerometer, gyroscope 데이터를 포함한다. imu/msg (sensormsgs/MagneticField)는 magnetometer 데이터를 포함한다.
이 패키지에서 사용하는 파라메터 중 주요한것을 살펴보면 다음과 같다. 이 파라메터는 ROS node 실행 시 _parameter:=value 형식으로 설정할 수 있다.
port (string, default: /dev/ttyACM0): Serial port - Linux only
baud_rate (int, default: 115200): Baud rate of serial connection
device_setup (bool, default: true): If true, puts device in idle mode and configures the device.If false, skips configuration. Important: the configuration parameters below are not effective unless this flag is true.
declination_source (int, default: 2): Possible declination sources: 0x01=Node, device reports magnetic north, 0x02=Internal World Magnetic Model, 0x03=Manual (see declination parameter)
gps_frame_id (string, default: wgs84): Value for the frame_id field in the header of the NavSatFix message published on the gps/fix topic
imu_frame_id (string, default: base_link): Value of the frame_id field in the header of the Imu message publised in the imu/data topic
odom_frame_id (string, default: wgs84): Value of the frame_id field in the header of the Odometry message published on the nav/odom topic
odom_child_frame_id (string, default: base_link): Value of the child_frame_id field in the Odometry message published on the nav/odom topic.
publish_gps (bool, default: true): Sets if ~gps/fix should be advertised/published or not. Note - to maximize performance you may want to only publish the Odometry messages
gps_rate (int, default: 1): Target update (publishing) rate for gps/fix messages. See update [[#rates] Update Rates] below.
imu_rate (int, default: 10): Target update (publishing) rate for imu/data messages. See update [[#rates] Update Rates] below.
odom_rate (int, default: 10): Target update (publishing) rate for nav/odom messages. See update [[#rates] Update Rates] below.
여기서 data update rate는 Hz 단위이다. 데이터 packet rate는 base_rate/decimation 이다. 3DM-GX4-45의 경우, 다음 rate에서 테스트되었다.
GPS - base rate = 4 Hz
IMU - base rate = 500 Hz
Filter - base rate = 500 Hz
Odometry 메시지는 현재 pos.position인 longitude(x), latitude(y)와 ellipsoid height(z)값을 리턴해 준다. pose.covariance와 twist.covariance는 position와 attitude의 대각성분(diagonal element)를 포함하고 있다.
Nav Status 메시지 통신 프로토콜은 다음과 같다.
filter_state
0x00 – Startup
0x01 – Initialization (see status flags)
0x02 – Running, Solution Valid
0x03 – Running, Solution Error (see status flags)
dynamics mode
0x01 – Portable (device default)
0x02 – Automotive
0x03 – Airborne
status_flags
See device documentation
Nav Sat Fix 메시지는 다음과 같다. Position co-variance는 수평, 수직 정확도를 기반으로 대각성분을 채운다. status.status 필드는 LLH() position data "valid flag"-1이다.
0x0001 – Latitude and Longitude Valid
0x0002 – Ellipsoid Height Valid
0x0004 – MSL Height Valid
0x0008 – Horizontal Accuracy Valid
0x0010 – Vertical Accuracy Valid
E.g., if all valid, then the status.status field should be 30.
함께 제공되는 mip_sdk_user_functions은 C함수 API로 RS232를 통해 센서와 명령+데이터를 입출력할 때 사용한다.
IMU 데이터 이용한 각도와 위치 예측
IMU 데이터에서 각도와 위치를 예측할 수 있으면, INS(Inertial Navigation System. 관성항법 장치)를 개발할 수 있다. 이 글에서 각도는 앞서 설명한 ROS 노드에서 이미 계산되어 제공한다. 각도는 각속도를 적분해 얻는 데, 칼만 필터 등을 이용해 잡음을 제거한다. 좀 더 상세한 계산 방법은 다음을 참고한다.
단순하게는 다음과 같이 중력 가속도를 제거하고, 적분하면 된다. 하지만, 적분 시간이 길어지면, 노이즈가 누적되어 발산되는 경향이 있다.
Ax = (Ax - 9.81 sin pi) cos pi
Ay = (Ay - 9.81 sin pi) cos pi
Vx = Ax * timeDelta
Vy = Ay * timeDelta
Dx = Dx + Vx * timeDelta
Dy = Dy + Vy * timeDelta
노이즈 제거를 고려한 IMU 데이터에서 위치 예측은 좀 더 복잡하다. 여기서는 Mike Koch가 정리한 알고리즘을 참고해 설명한다.
현재 시간의 각속도 ax, ay, az, 가속도 accX, accY, accZ를 획득한다.
pos(t,:) = pos(t-1,:) + vel(t,:) * samplePeriod; % integrate velocity to yield position
end
마무리
이 글에서는 고정밀 GNSS와 ROS를 연동하는 방법을 간단히 소개하였다. GNSS와 LiDAR 데이터를 연계하면, 실시간으로 하나의 좌표계안에 정합된 스캔 데이터 점군을 획득할 수 있다. 이는 로보틱스, 자율주행차와 같이 실시간으로 공간정보를 획득해야 할 때 매우 유용한 기술이다.
이 글은 LiDAR(Light Detection and Ranging) 기반으로 SLAM 하는 방법을 간략히 설명한다. 슬램은 임의의 위치에서 상대적 거리를 측정할 수 있는 센서를 이용해 실시간으로 지도를 생성하는 기술이다. 이 기술은 무인자율차 등 실시간으로 2, 3차원 지도를 생성해야할 때 사용한다.
실시간 SLAM(Simultaneous localization and mapping) 기반 3D 맵(LOAM)
모바일 SLAM 결과(한국건설기술연구원 일산 건물)
이 글에서 SLAM을 활용해 보기 위해서 저가의 VLP16 벨로다인 센서를 사용한다. 참고로, 이 방법은 우분투 ROS 환경에서 실행된다. 여기서 사용하는 환경은 ROS(Robot Operating System) melodic 버전을 사용하였으며, NVIDIA TX2, 오드로이드 우분투 18.04 기반으로 TEST 하였다(SLAM은 많은 계산시간이 필요하므로, NVIDIA TX와 같이 가능한 성능 좋은 컴퓨터를 사용하는 것이 좋다. 이 경우에는 장치 소형화를 위해 부득이하게 오드로이드를 사용했다). 우분투, 벨로다인 LiDAR 드라이버, ROS, RViz 설치 방법은 다음 링크를 참고하길 바란다.
LOAM SLAM
우선 SLAM 에서 잘 알려진 LOAM(Laser Odometry and Mapping) 방식 SLAM을 만들어보자. 다음과 같이 github에서 소스코드를 받고, 순서대로 빌드해 본다(링크1,링크2 참고).
$ cd ~/catkin_ws/src/
$ git clone https://github.com/laboshinl/loam_velodyne.git
$ cd ~/catkin_ws
$ catkin_make -DCMAKE_BUILD_TYPE=Release
$ source ~/catkin_ws/devel/setup.bash
참고로, 오드로이드, 라즈베리파이 같이 램 용량이 적은 컴퓨터에서는 컴파일 에러가 발생한다. 이 경우, 부록에 설명된 방법대로 스왑 용량을 설정해 컴파일 빌드하면 된다(컴파일이 몇시간 걸릴 수 있다).
이제 roscore를 실행하고, 다음 명령을 실행한다.
roslaunch loam_velodyne loam_velodyne.launch
실행된 ROS node 토픽(rqt_graph)
LOAM SLAM을 이용해 회사 사무실을 LiDAR로 스캔해 본 결과는 다음과 같다. 점군 밀도가 밀집되어 있는 벽체가 진하게 표시되어 구분되는 것을 확인할 수 있다.
SLAM 스캔 결과(건설연 본관 2동 3층)
점군이 모여있는 부분은 회사에 설치된 파티션이고, 그 외에 점들이 흩어져 있는 부분들은 천장 및 바닥이다. 파티션의 경계선은 뚜렷하게 표시된 맵이 생성되지만, 아직 좌표계, 캘리브레이션, IMU 등을 제대로 설정하지 않은 상황이라, SLAM 맵 정확도가 높지는 않아 보인다. 또한, 창쪽 유리가 반사되어 노이즈가 생기는 것을 확인할 수 있다.
SLAM 점군과 측량장비(GLM100C)와 측정값 간의 차이는 높이 2미터에서 1-0.5미터정도이다. 정확도는 좀 더 다양한 방법으로 확인할 필요가 있어보인다.
1차 SLAM 테스트
2차 SLAM 테스트
3차 SLAM 테스트(ODROID)
4차 SLAM 테스트(NVIDIA TX2로 테스트. IMU연결안함. SLAM 맵 깨지는 현상이 덜함)
SLAM은 IMU 센서 유무에 따라 테스트해보았다. 두 경우 모두 급격한 회전이나 이동에 오차가 발생하는 것을 확인할 수 있다(이 문제는 성능 낮은 저가 임베디드 보드 사용과 관련있을 수 있다). 가능한 SLAM이 예측 가능한 방향으로 부드럽게 천천히 이동하는 것이 제일 품질이 좋다. 단, IMU 센서가 없는 경우 급격한 회전이나 이동시 오차가 상대적으로 더 많았으며, 정합 대상을 잃어버리면서 스캐닝되는 문제가 좀 더 빈번히 발생한다. 이렇게 정합이 안되고 발산하는 문제는 SLAM 기술의 일반적인 문제로 알려져 있다(상용 SLAM 측량 기술도 동일한 문제가 발생한다). 그러므로 현재 기술은 아직 SLAM을 이용해 스캔하는 방법이 결과물에 나쁜 영향을 줄 수도 있다.
참고로, VLP16의 스캔 수직범위는 크지 않아, 360도 스캔을 위해서는 스캐너를 회전시켜야 한다. 아래 영상은 LiDAR를 회전시켜 360도로 점군을 획득해 맵을 생성하는 예이다.
LOAM SLAM
SLAM 결과를 저장하려면 다음과 같이 토픽명을 지정하고 ROSBAG 명령어로 토픽 데이터를 저장하면 된다.
rosbag record -o out /laser_cloud_surround
만약, PCD 포맷으로 변환하려면, 아래 명령을 이용해 [input bag]에 저장된 .bag파일명을 지정해 준다.
rosrun pcl_ros bag_to_pcd [input bag] /laser_cloud_surround pcd
구조와 소스코드를 분석해 보면 다음과 같은 순서로 SLAM데이터를 계산하고 있는 것을 알 수 있다.
1. velodyne node: 스캔 후 point cloud 데이터 획득
2. multi scan registration node: 스캔된 데이터에서 곡률 기반 특징점(모서리, 평면) 생성
3. laser odometry: 이전 스캔 데이터에서 생성된 특징점을 기반으로 현재 스캔 데이터의 매특징점과 비교해 주행괘적(odometry)을 계산
4. laser mapping: 주행괘적을 이용해 스캔 데이터를 보정해 정합함. 만약, IMU 센서가 있는 경우, /imu/data에서 얻은 데이터를 이용해 데이터 보정함
LOAM은 특징점 계산에 많은 계산 시간이 필요하다. 이런 이유로 원칙적으로 NVIDIA TX2 이상의 임베디드 보드가 필요하다. LeGO LOAM은 이 계산 시간을 좀 더 줄인것이다.
좀 더 자세한 내용은 관련 논문을 참고하길 바란다.
LeGO LOAM
LeGO(lightweight and ground optimized lidar odometry and mapping) LOAM은 LOAM에 비해 계산 시간은 줄이고, 루프백(LOOP BACK)을 지원하여 SLAM 안전성을 높인 방법이다. Levenberg–Marquardt algorithm 을 이용해 해 계산 방법을 개선한다. 참고로 LM 알고리즘은 특정 함수에 적합한 커브를 피팅(curve fitting)할 때도 많이 사용된다. LeGO LOAM 링크를 방문해, 다음과 같이 소스를 다운로드후 빌드를 준비한다. 참고로, LeGO LOAM을 빌드하기 전에 GTSAM(Georgia Tech Smoothing and Mapping library) 패키지를 미리 설치해 놓아야 한다.
wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip
cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/
cd ~/Downloads/gtsam-4.0.0-alpha2/
mkdir build && cd build
cmake ..
sudo make install
다음과 같이 소스를 빌드한다.
cd ~/catkin_ws/src
git clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
cd ..
catkin_make -j1
다음과 같이 ROS 패키지 실행해 보았다.
roslaunch lego_loam run.launch
참고로 lego 패키지의 노드 중 일부는 ROS의 시뮬레이션 time 을 이용한다. 이 기능은 ros::time 패키지에 있는 데, 이를 사용하는 노드의 ROS loop가 동작되지 않는 경우가 발생할 수 있다. 이 상황에는 실제 스캔된 포인트 클라우드 및 처리된 결과물을 rviz에서 확인할 수 없다. 이때는 해당 노드의 main() 함수 내에서 ROS loop가 실행되기 전에 다음과 같은 시간 초기화 함수를 호출해 빌드한 후 실행해 보면 잘 동작될 것이다.
ros::time::Init();
일부 TF(Transform coordinate Frame) 토픽이 동작되지 않는 문제가 있을 수 있다. 이 경우 센서 데이터 좌표계가 설정되지 않아 rviz에 스캔 데이터가 표시되지 않고 에러 메시지가 발생한다. 이 경우, lego_loam 의 run.launch 파일 내 TF 토픽 발생하는 노드를 터미널에서 강제로 실행하면 잘 동작된다.
문제가 없을 경우 LeGO SLAM 실행된 결과는 다음과 같다.
LeGO SLAM 실행 결과
github에 링크되어 있는 pcap파일로 실행하면 미리 스캔한 데이터로 다음과 같은 데모를 확인할 수 있다고 한다. 만약, 벨로다인에서 저장된 ROS bag 파일이 있다면, 다음 명령을 이용해 재생하여 rviz에 데이터를 표시할 수 있다. 참고로, LeGO 웹사이트에서 샘플 데이터 bag 파일을 다운로드 할 수 있다.
rosbag play *.bag --clock --topic /velodyne_points /imu/data
LeGO LOAM
LOAM이나 LeGO이외에도 여러가지 SLAM방식이 존재한다. 그러므로, 응용 목적에 따라 적합한 것을 사용해야 한다.
HDL graph SLAM
HDL GRAPH SLAM
HDL 그래프 스램은 3차원 라이다를 이용한 슬램 기술 중 하나로 NDT(Normal Distributions Transformation) 스캔 매칭 및 루프 검출 방식을 알고리즘으로 사용한다. 이 방식은 다음 그림과 같이 odometry 예측, RANSAC을 이용한 바닥 검출 등을 이용해 스캔 데이터 매칭을 수행한다.
# for indigo
sudo apt-get install ros-indigo-geodesy ros-indigo-pcl_ros ros-indigo-nmea-msgs
# for kinetic
sudo apt-get install ros-kinetic-geodesy ros-kinetic-pcl_ros ros-kinetic-nmea-msgs ros-kinetic-libg2o
# for melodic
sudo apt-get install ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs ros-melodic-libg2o
sudo pip install ProgressBar2
컴파일은 ROS cakin으로 빌드한다. 만약 intel 계열 보드가 아니면 OpenMP 사용하는 소스코드에서 에러가 발생한다. ARM계열 보드의 경우, openmp 패키지를 설치하고, 에러 나는 소스 코드를 일부 수정해야 한다.
관련 에러를 수정한 후 벨로다인 노드와 슬램을 실행다면 다음과 같은 결과를 확인할 수 있다.
실내 SLAM 결과
깃허브에 포함된 예제를 보면, 다음과 같은 실행 결과를 확인할 수 있다.
HDL GRAPH SLAM 결과
NDT(Normal Distributions Transformation) SLAM
NDT는 SLAM에서 사용하는 기술 중 하나로 PCL(point cloud library)에 구현되어 있는 알고리즘이다.
PCL NDT 지원 클래스
이 방법은 이동하는 장치에서 스캔한 점군 PCD1, PCD2가 있을 경우, 두 점군이 겹칠때 각 점들의 오차를 정규분포로 계산해 최소화하는 알고리즘으로 정합을 한다.
이 방법을 실시간 SLAM에 직접 사용할 때는 계산 성능에 문제가 있어, OCTREE 등 공간 인덱싱 기법을 사용해 속도를 개선해 사용하는 기법을 적용한다.
다음은 PCL 및 공간 인덱싱을 사용한 버전의 소스이다.
이렇게 해도 안된다면, 컴파일 시 make -j 옵션 대신 make 를 사용한다(관련 링크).
참고로, 임베디드 장치는 메모리 크기가 2~4G이며 스왑은 구성되어 있지 않다. 이 경우, 컴파일 프로세스에서 g++: internal compiler error: Killed (program cc1plus) 오류가 발생한다. 이 오류는 메모리가 부족하여 발생하므로, 스왑을 추가하여 문제를 해결할 수 있다(링크 참고).
참고: NVIDIA TX2기반 테스트
ODROID보다 TX2기반에서 안정적인 SLAM 처리가 가능하다. ROS 특성상 컴퓨팅 처리 속도가 늦으면, 메시지 동기화가 문제가 되며, 이로 인해 SLAM 맵 생성이 제때 처리되지 않는 문제가 발생할 수 있다. 참고로 TX2는 보통 개발 보드와 함께 조립되어 있는 데, 다음과 같은 캐리어 보드를 이용하면, 손바닥 보다 작은 크기로 패키징 가능하다.
Sebastian Hening, 2017, 3D LiDAR SLAM Integration with GPS/INS for UAVs in Urban GPS-Degraded Environments, ICP based SLAM
Tixiao Shan, Brendan Englot, 2018, lightweight and ground optimized lidar odometry and mapping, LeGO LOAM
Kenji Koide etc, 2019, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, HDL graph SLAM
