고정밀 GNSS IMU와 ROS 연동하기

이 글은 고정밀 GNSS(Global Navigation Satellite System), IMU(Inertial Measurement Unit)와 ROS(Robot Operating System)를 연동하는 것에 대한 내용을 간단히 요약한 것이다. 여기서 사용한 GNSS센서는 3DM GX5이다.

3DM-GX5-45(high performance integrated multi-constellation GNSS receiver)

임베디드 보드는 ODROID(오드로이드. 설치 참고) UX4를 사용하였으나, 라떼판다(윈도우에서 운용됨), NVIDIA TK 등 다른 임베디드 보드를 사용하더라도 큰 관계는 없다.

이 제품은 GPS, GLONASS, DeiDou, Galileo를 활용한 GNSS receiver이며, 정밀한 AHRS(Attitude and Heading Reference System) 값을 얻을 수 있다.

AHRS data

이 센서는 가격이 3~400백만원 수준이며, 드론, 로봇틱스 등 다양한 분야에 활용되고 있다. 이 센서는 가격이 상대적으로 저렴한? 편이라 한다. 다만, ROS 우부투, 리눅스 기반 작업을 할 때는 문서화가 잘 안되어 있어, 삽질이 좀 필요하다. 윈도우 버전은 설치 프로그램도 함께 제공하는 등 상대적으로 작업이 쉽다.

GNSS 사용 사례

이 글은 다음 링크를 참고하였다.

• ROS.org, Microstrain 3DM GX5 45
• LORD MicroStrain sensing, 3DM-GX5-45
• Microstrain 3D-GX5 with ROS

머리말

제품 제조사는 ROS 드라이버 패키지를 다음 링크에서 제공하고 있다.

• github, microstrain 3dm gx5 45

이 패키지는 인터페이스 드라이버, ROS node가 제공되며, MIP SDK(Software Development Kit)를 통해 GNSS 장치와 연결해 사용한다. 

ROS 패키지 빌드 및 실행 

현재 ROS 버전이 HYDRO, INDIGO 일 경우에는 다음과 같이 3DM-GX5-45 ROS 패키지를 다운로드하고 설치해보자. 

DISTRO={hydro|indigo}

cd ~/catkin_ws

rosdep update

rosdep check --from-paths src/microstrain_3dm_gx5_45/ --rosdistro=$DISRO

rosdep install --from-paths src/microstrain_3dm_gx5_45/ --ignore-src --rosdistro=$DISTRO --simulate

rosdep install --from-paths src/microstrain_3dm_gx5_45/ --ignore-src --rosdistro=$DISTRO

catkin_make

source devel/setup.bash

만약, 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/data (sensor_msgs/Imu): IMU (AHRS) 데이터
• nav/odom (nav_msgs/Odometry): odometry message 데이터
• nav/status (std_msgs/Int16MultiArray): Filter 상태 데이터

각 topic data 를 살펴보면 다음과 같다.

• rostopic echo /imu/data

microstrain_mips_node가 생성한 ROS 토픽

참고로, 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 데이터를 포함한다.  

• rostopic echo /gps/fix

• rostopic echo /nav/odom

• rostopic echo /nav/status

ROS node 에서 생성된 데이터가 제대로 출력되는 것을 확인할 수 있다.

3DM-GX5-45 패키지 기능 및 파라메터

다음은 센서 ROS 패키지에서 제공하는 서비스이다.

• reset_kf (std_srvs/Empty): 칼만필터(Kalman filter) 초기화

이 패키지에서 사용하는 파라메터 중 주요한것을 살펴보면 다음과 같다. 이 파라메터는 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.
• dynamics_mode (int, default: 1): Vehicle dynamics mode 0x01=Portable, 0x02=Automotive, 0x03=Airborne
• 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 노드에서 이미 계산되어 제공한다. 각도는 각속도를 적분해 얻는 데, 칼만 필터 등을 이용해 잡음을 제거한다. 좀 더 상세한 계산 방법은 다음을 참고한다.

• IMU 센서 이용한 각도 예측

단순하게는 다음과 같이 중력 가속도를 제거하고, 적분하면 된다. 하지만, 적분 시간이 길어지면, 노이즈가 누적되어 발산되는 경향이 있다.
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를 획득한다.
• 가속도 크기 magnitute = sqrt(accX ^ 2 + accY ^ 2 + accZ ^ 2)
• 노이즈 제거를 위해 high-pass filter 사용
• 가속도 데이터에서 특정 범위 이외 값은 제거함
• butterworth 필터 적용. cutoff frequency n = 1이며, Wn = (2 * cutoff) / (1 / samplePeriod). samplePeroid는 읽기 사이의 시간 간격임. 필터 타입 ftype = 'high'임
• zero-phase 디지털 필터 수행
• 필터된 값의 절대값 취함
• low-pass filter 이용해 노이즈 제거
• 자이로 데이터로부터 방향 취득
• 가속도를 m/s^2 으로 변환
• 가속도에서 속도를 계산
• 속도로 부터 위치 적분
• 앞의 로직 반복

다음은 IMU 기반 각도 및 위치 계산 알고리즘 소스코드(matlab) 설명이다. 

% IMU 값 읽기

samplePeriod = 1/256;   % sampling 간격

xIMUdata = xIMUdataClass(filePath, 'InertialMagneticSampleRate', 1/samplePeriod);  % IMU 데이터 획득

time = xIMUdata.CalInertialAndMagneticData.Time;                   % 시간

gyrX = xIMUdata.CalInertialAndMagneticData.Gyroscope.X;         % 각속도

gyrY = xIMUdata.CalInertialAndMagneticData.Gyroscope.Y;

gyrZ = xIMUdata.CalInertialAndMagneticData.Gyroscope.Z;

accX = xIMUdata.CalInertialAndMagneticData.Accelerometer.X;    % 가속도

accY = xIMUdata.CalInertialAndMagneticData.Accelerometer.Y;

accZ = xIMUdata.CalInertialAndMagneticData.Accelerometer.Z;

acc_mag = sqrt(accX.*accX + accY.*accY + accZ.*accZ);     % 가속도 크기 

% 필터 적용

filtCutOff = 0.001;                                                     % cut off 설정

[b, a] = butter(1, (2*filtCutOff)/(1/samplePeriod), 'high');    % butter worth filter 계산

acc_magFilt = filtfilt(b, a, acc_mag);

acc_magFilt = abs(acc_magFilt);                                    % 가속도 절대값 획득

filtCutOff = 5;

[b, a] = butter(1, (2*filtCutOff)/(1/samplePeriod), 'low');      % low 필터 적용

acc_magFilt = filtfilt(b, a, acc_magFilt);

stationary = acc_magFilt < 0.05;                                

% 방향 계산

quat = zeros(length(time), 4);

AHRSalgorithm = AHRS('SamplePeriod', 1/256, 'Kp', 1, 'KpInit', 1);

initPeriod = 2;

indexSel = 1 : find(sign(time-(time(1)+initPeriod))+1, 1);

for i = 1:2000

    AHRSalgorithm.UpdateIMU([0 0 0], [mean(accX(indexSel)) mean(accY(indexSel)) mean(accZ(indexSel))]);

end

for t = 1:length(time)

    if(stationary(t))

        AHRSalgorithm.Kp = 0.5;

    else

        AHRSalgorithm.Kp = 0;

    end

    AHRSalgorithm.UpdateIMU(deg2rad([gyrX(t) gyrY(t) gyrZ(t)]), [accX(t) accY(t) accZ(t)]);

    quat(t,:) = AHRSalgorithm.Quaternion;

end

acc = quaternRotate([accX accY accZ], quaternConj(quat));

acc = acc * 9.81;   % 방향 계산

% 이동량 계산

acc(:,3) = acc(:,3) - 9.81;   

vel = zeros(size(acc));

for t = 2:length(vel)

    vel(t,:) = vel(t-1,:) + acc(t,:) * samplePeriod;

    if(stationary(t) == 1)

        vel(t,:) = [0 0 0];     % force zero velocity when foot stationary

    end

end

velDrift = zeros(size(vel));

stationaryStart = find([0; diff(stationary)] == -1);

stationaryEnd = find([0; diff(stationary)] == 1);

for i = 1:numel(stationaryEnd)

    driftRate = vel(stationaryEnd(i)-1, :) / (stationaryEnd(i) - stationaryStart(i));

    enum = 1:(stationaryEnd(i) - stationaryStart(i));

    drift = [enum'*driftRate(1) enum'*driftRate(2) enum'*driftRate(3)];

    velDrift(stationaryStart(i):stationaryEnd(i)-1, :) = drift;

end

vel = vel - velDrift;

% 이동 위치 계산

pos = zeros(size(vel));

for t = 2:length(pos)

    pos(t,:) = pos(t-1,:) + vel(t,:) * samplePeriod;    % integrate velocity to yield position

end

마무리

이 글에서는 고정밀 GNSS와 ROS를 연동하는 방법을 간단히 소개하였다. GNSS와 LiDAR 데이터를 연계하면, 실시간으로 하나의 좌표계안에 정합된 스캔 데이터 점군을 획득할 수 있다. 이는 로보틱스, 자율주행차와 같이 실시간으로 공간정보를 획득해야 할 때 매우 유용한 기술이다. 

레퍼런스

• microstrain_mips parameters QA
• 3DM GX4 25 ROS package
• Sensor Integration and Outdoor Navigation
• Preparing Your Data for Use with robot_localization
• INS with Kalman Filter using GPS, IMU
• Indoor localization with IMU
• SBG IMU