[ROS2 GPS, IMU 항법 코드] ROS2 기반 GPS + IMU 항법 시스템 구현

개요

이 글에서는 ROS 2 기반 시스템에서 GPS(Global Positioning System)와 IMU(Inertial Measurement Unit) 센서 데이터를 활용하여 선박 또는 로봇의 위치 및 자세 정보를 계산하고 항법 메시지로 변환하는 과정을 다룹니다.

 

사용 목적

• GPS와 IMU 센서를 기반으로 자율운항 선박이나 로봇의 실시간 위치 및 자세 정보를 추정
• 추정된 정보를 바탕으로 Guidance & Control 알고리즘에 필요한 데이터를 생성

---

이론 설명

1. GPS 좌표계를 UTM 좌표계로 변환하는 이유

GPS 좌표계의 한계

• GPS는 WGS84 기반의 위도(latitude), 경도(longitude) 정보를 제공합니다.
• 이는 구면 좌표계 기반이므로 거리 계산, 상대 위치 추정에 어려움이 있습니다.

UTM 좌표계의 장점

• UTM (Universal Transverse Mercator) 좌표계는 지구를 60개의 존(Zone)으로 나누고, 각 존에서 2차원 직교 좌표계 (X, Y) 로 변환합니다.
• 이를 통해 미터 단위의 거리, 상대 위치 계산이 용이해집니다.
• 예: "현재 위치가 기준 위치에서 동쪽으로 24.5m, 북쪽으로 17.2m 떨어져 있음"과 같은 계산이 가능

 따라서 GPS로 받은 위도/경도 값을 UTM으로 변환하면, 로컬 좌표계를 기준으로 상대 위치를 쉽게 계산할 수 있습니다.

 

2. IMU 센서의 역할

IMU는 3축 가속도계와 3축 자이로스코프를 포함한 센서입니다.

주요 데이터

• orientation: 자세 (쿼터니언 형태)
• angular velocity: 회전 속도 (rad/s)
• linear acceleration: 가속도 (optional)

사용 이유

• GPS는 위치는 정확하지만 자세나 회전 정보를 제공하지 않습니다.
• IMU는 실시간 회전 각도(yaw)와 회전 속도(r)를 추정할 수 있어, GPS를 보완합니다.

항법 시스템에서 IMU는 선박의 방향(ψ), 회전 속도(r), 가속도 등 자세 정보를 제공합니다.

 

---

코드 기반 설명

 

 이제 위의 이론을 바탕으로 실제 ROS 2 C++ 코드 구현 내용을 살펴보겠습니다. 아래에 소개할 코드는 GitHub 저장소 macro_gnc에서 확인할 수 있으며, 항법 데이터를 생성하는 핵심 모듈 중 하나인 gps_navigation.cpp 파일에 해당합니다. 이 코드는 ROS 2 노드로 동작하며, GPS 및 IMU 센서로부터 받은 데이터를 바탕으로 선박 혹은 무인이동체의 위치 및 자세 정보를 실시간으로 계산하여 퍼블리시합니다.

GitHub - lidarmansiwon/macro_gnc

 

 특히 속도 계산 부분에는 navigation/tool/velocity_calculator.hpp 파일이 사용됩니다. 이 모듈은 이동체의 위치 및 자세 변화율로부터 선속(u), 횡속(v), 그리고 요레이크(r)를 계산하며, 저역통과 필터(Low-Pass Filter, LPF)를 적용하여 노이즈가 많은 센서 데이터로부터 안정적인 속도 추정치를 제공합니다. 이 필터 계수는 파라미터로 조정할 수 있어 상황에 맞는 튜닝이 가능합니다.

이제 gps_navigation.cpp 코드의 주요 구성 요소와 각 기능에 대해 자세히 살펴보겠습니다.

초기화 및 파라미터 설정

this->declare_parameter<std::string>("imu_topic", "/imu_topic");
this->declare_parameter<std::string>("gps_topic", "/gps_topic");
this->declare_parameter<std::string>("navigation_topic", "/navigation_data");

• 사용자 정의 토픽 이름을 파라미터로 설정합니다.
• reference_gps_latitude, reference_gps_longitude는 상대 좌표의 기준점입니다.

 

Parameter 값으로 전달 받은 filter 강도 전달

  double velocity_alpha = this->get_parameter("velocity_alpha").as_double();
  vel_calc_ = VelocityCalculator(velocity_alpha);

 

IMU 데이터를 활용한 자세 계산

geometry_msgs::msg::Quaternion quat = imu_data_->orientation;
std::array<double, 3> rpy = euler_from_quaternion({quat.x, quat.y, quat.z, quat.w});
double yaw = rpy[2];

• 쿼터니언 → 롤/피치/요(roll, pitch, yaw) 변환
• 항법에서 가장 중요한 건 yaw (ψ), 즉 선박의 방향입니다.

Pub, Sub 객체 정의 및 timer 정의

  // 생성자에서 시작 표시.
  RCLCPP_INFO(this->get_logger(), "Run GPS Navigation");

  imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
    imu_topic, rclcpp::SensorDataQoS(), std::bind(&GPSNavigation::imu_callback, this, _1));

  gps_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
    gps_topic, rclcpp::SensorDataQoS(), std::bind(&GPSNavigation::gps_callback, this, _1));

  navigation_publisher_ = this->create_publisher<mk3_msgs::msg::NavigationType>(
    navigation_topic, 10);

  timer_ = this->create_wall_timer(50ms, std::bind(&GPSNavigation::process, this));

•  생성자에서 노드를 처음 시작할 때, RCLCPP_INFO 로그 기능을 통하여 시작을 알립니다.
•  전달 받을 센서 GPS, IMU에 대한 Subscribe 코드와 최종 계산된 항법 데이터를 Publish 할 코드를 작성합니다. 
• timer를 통하여 50ms(20Hz) 간격으로 함수를 실행합니다. 

GPSNavigation::process() 함수 상세 설명

1. 센서 데이터 유효성 검사

if (!gps_data_ || !imu_data_)
{
  RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000, "GPS or IMU data not received yet.");
  return;
}

• 목적: GPS 또는 IMU 데이터가 아직 수신되지 않은 경우 경고 메시지를 출력하고 함수를 종료합니다.
• 기능: ROS 2에서는 센서 콜백이 비동기로 도착하므로, 해당 데이터가 nullptr인지 확인하여 안정성을 확보합니다.
• 기능 강조: RCLCPP_WARN_THROTTLE은 3초에 한 번씩만 경고를 출력하여 로그 과부하를 방지합니다.

2. GPS 좌표 → UTM 좌표 변환

double latitude = gps_data_->latitude;
double longitude = gps_data_->longitude;
...
GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_x, utm_y);

• 목적: 위도(latitude)와 경도(longitude)를 UTM 평면좌표계로 변환합니다.
• 사용 라이브러리: GeographicLib의 UTMUPS::Forward 함수
• 입력값 : latitude, longitude
• 출력값 :
  • utm_x, utm_y: 변환된 평면 좌표
  • zone: UTM 존 (6도 간격)
  • northp: 북반구 여부 (남반구일 경우 UTM 보정 방식이 다름)

RCLCPP_INFO(this->get_logger(), "Converted GPS (%.8f, %.8f) → UTM (%.2f, %.2f), Zone: %d, Northp: %d\n", latitude, longitude, utm_x, utm_y, zone, northp);

 

디버깅 정보 출력: 실제 GPS 좌표가 어떻게 변환되었는지 로그에 출력하여 추적 가능합니다.

 

3. 기준점 설정 및 상대 좌표 계산

if (!origin_set_) {
    ...
    GeographicLib::UTMUPS::Forward(reference_gps_latitude_, reference_gps_longitude_, reference_zone, reference_northp, reference_utm_x, reference_utm_y);
    ...
    origin_set_ = true;
}

• 목적: 기준 GPS 좌표(reference)를 최초 한 번만 UTM으로 변환하여 고정된 원점으로 사용합니다.
• 기능 설명:
  • origin_set_ 플래그는 기준점이 한 번만 설정되도록 합니다.
  • 일반적으로 로컬 좌표계를 만들기 위한 원점 설정에 해당합니다.

double relative_x = utm_x - reference_utm_x;
double relative_y = utm_y - reference_utm_y;

• 목적: UTM 좌표계 상에서 기준점으로부터의 상대적인 위치를 계산합니다.
• 결과: 로컬 UTM 좌표 (x, y) 생성

4. 자세 정보 추출 (IMU → Roll, Pitch, Yaw)

geometry_msgs::msg::Quaternion quat = imu_data_->orientation;
std::array<double, 3> rpy = euler_from_quaternion({quat.x, quat.y, quat.z, quat.w});
double roll = rpy[0], pitch = rpy[1], yaw = rpy[2];

• 목적: IMU로부터 쿼터니언(quaternion) 형태의 자세 정보를 받아서 오일러각(Roll, Pitch, Yaw)으로 변환합니다.
• 사용 함수: euler_from_quaternion() (사용자 정의 함수로 추정)
• 중요한 값: yaw는 선박의 방향(heading)을 의미하는 각도로, 이후 속도 계산에 활용됩니다.

5. 속도 계산 (velocity_calculator 사용)

auto result = vel_calc_.update(x, y, psi, time_sec);
double LPFVel_x = result.LPFVel_x;
double LPFVel_y = result.LPFVel_y;
double angular_velocity = result.angular_velocity;

• 목적: 위치 변화와 자세 변화량을 기반으로 선속(u), 횡속(v), 회전 속도(r)를 계산합니다.
• 사용 클래스: velocity_calculator.hpp에 정의된 VelocityCalculator 클래스
• 핵심 알고리즘:
  • 이전 상태와 현재 상태 간 시간차(dt)를 사용하여 속도를 계산
  • 저역통과필터(LPF)를 통해 노이즈 제거 및 평활화 처리
• 출력 항목:
  • LPFVel_x: 선속 (u)
  • LPFVel_y: 횡속 (v)
  • angular_velocity: 회전 속도 (r)

6. 최종 메시지 생성 및 퍼블리시

NavigationType nav_msg;
nav_msg.x = x;
nav_msg.y = y;
nav_msg.psi = psi * 180.0 / M_PI;
nav_msg.u = LPFVel_x;
nav_msg.v = LPFVel_y;
nav_msg.r = angular_velocity;
nav_msg.w = imu_data_->angular_velocity.z;

navigation_publisher_->publish(nav_msg);

• 목적: 계산된 위치, 자세, 속도를 NavigationType 메시지 형태로 퍼블리시
• 메시지 필드 설명:
  • x, y: 로컬 UTM 좌표계 기준 상대 위치
  • psi: 선박의 방향각 (라디안 → 도 단위 변환)
  • u, v, r: 각각 선속, 횡속, 요 회전 속도
  • w: IMU에서 측정된 z축 각속도 (원시 데이터)

종합 요약

이 함수는 GPS와 IMU 데이터를 융합하여 이동체의 현재 위치, 자세, 속도 정보를 계산한 후, 이를 다른 노드에서 사용할 수 있도록 퍼블리시하는 핵심적인 항법 처리 로직입니다. 

 

velocity_calculator.hpp 설명

1. 목적

velocity_calculator.hpp는 위치 및 자세 정보의 시간 변화율을 통해 속도를 계산하기 위한 유틸리티 클래스입니다.
ROS 2 시스템에서 GPS와 IMU를 기반으로 수집된 위치(x, y) 및 방향(psi: 요각)을 기반으로 선속(u), 횡속(v), 각속도(r)를 추정합니다.

 

2. 핵심 원리: 수치 미분(Numerical Differentiation)

dx = x - prev_x_;
dy = y - prev_y_;
dpsi = normalize_angle(psi - prev_psi_);
dt = current_time - prev_time_;

• 고전적인 1차 수치 미분법(forward difference)을 이용해 속도를 추정합니다.​
• 이를 바탕으로 선속(u), 횡속(v), 회전속도(r)를 계산:
  • u = (dx * cos(psi) + dy * sin(psi)) / dt
  • v = (-dx * sin(psi) + dy * cos(psi)) / dt
  • r = dpsi / dt

이 방식은 구현이 간단하고 실시간으로 사용하기 적합하나, 센서 노이즈에 민감하다는 단점이 존재합니다.

 

3. 노이즈 보완: 저역통과 필터(Low-Pass Filter)

LPFVel_x = LPF_alpha * vel_x + (1 - LPF_alpha) * LPFVel_x;
LPFVel_y = LPF_alpha * vel_y + (1 - LPF_alpha) * LPFVel_y;

고주파 노이즈(잡음)를 줄이기 위해 저역통과 필터(LPF)를 사용

LPF_alpha는 필터 계수로, 0.0~1.0 사이의 값입니다.

• 1에 가까울수록 새 데이터 반영률이 높고,
• 0에 가까울수록 이전 데이터 반영률이 높아져 더 부드럽지만 반응이 느려집니다.

 

4. 한계점: 미분 기반 속도 추정의 문제점

• GPS나 위치 센서는 노이즈가 포함된 데이터를 제공합니다.
• 수치 미분은 노이즈를 증폭시킵니다.
  • 데이터가 순간적으로 튈 경우, 계산된 속도 값도 크게 튈 수 있습니다.
  • dt가 작을수록 더 민감해지므로 주의가 필요합니다.
• LPF만으로는 동적 환경 변화에 적응하거나 예측이 불가능합니다.

 

5. 개선 방향: 칼만 필터(Kalman Filter)를 활용한 보정

많은 연구자들은 속도 추정을 더 정밀하게 하기 위해 Kalman Filter (KF) 를 적용합니다.

Kalman Filter 기본 개념:

• 센서 노이즈를 모델링하고, 현재 상태(위치, 속도 등)를 추정하는 확률 기반 필터
• 예측(Predict) 단계 + 관측 업데이트(Update) 단계를 반복
• 시간에 따라 누적되는 오차를 수학적으로 최소화

항법 요소 데이터 출처 설명

위치 (x, y)	GPS (UTM 변환 후)	기준 GPS 대비 상대 위치
자세 (ψ)	IMU	방향 추정 (yaw)
선형 속도 (u, v)	위치 변화량 기반	LPF 적용
각속도 (r, w)	위치 변화량 + IMU	yaw 변화량 기반 또는 자이로 직접 측정

 

참고 사항

• UTM 변환에는 GeographicLib 라이브러리를 사용
• IMU 오차와 GPS 신호 손실 등을 고려해 추가 필터 (예: 칼만 필터) 적용 가능
• 본 코드는 ROS 2 Humble 기준으로 작성됨

 

전체 코드

코드 저장소: MACRO_GNC

GitHub - lidarmansiwon/macro_gnc

 

[gps_navigation.cpp 전체 코드]

#include <iostream>
#include <memory>
// C++ Standard lib 
#include <chrono>
#include <functional>
#include <string>
#include "rclcpp/rclcpp.hpp"

#include <GeographicLib/UTMUPS.hpp>

#include "std_msgs/msg/string.hpp"
#include "mk3_msgs/msg/navigation_type.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"

#include "navigation/gps_navigation.hpp"
#include "navigation/tool/quaternion_utils.hpp"
#include "navigation/tool/velocity_calculator.hpp"


using std::placeholders::_1;
using namespace std::chrono_literals;

GPSNavigation::GPSNavigation(const rclcpp::NodeOptions & node_options)
: Node("gps_navigation", node_options), 
  reference_gps_latitude_(0.0),
  reference_gps_longitude_(0.0),
  reference_utm_x(0.0),
  reference_utm_y(0.0),
  reference_zone(0.0),
  reference_northp(false),
  vel_calc_(0.9)
{ 
  this->declare_parameter<std::string>("imu_topic", "/imu_topic");
  this->declare_parameter<std::string>("gps_topic", "/gps_topic");
  this->declare_parameter<std::string>("navigation_topic", "/navigation_data");

  this->declare_parameter<double>("reference_gps_latitude", 0.0);
  this->declare_parameter<double>("reference_gps_longitude", 0.0);
  this->declare_parameter<double>("velocity_alpha", 0.9);

  std::string imu_topic = this->get_parameter("imu_topic").as_string();
  std::string gps_topic = this->get_parameter("gps_topic").as_string();
  std::string navigation_topic = this->get_parameter("navigation_topic").as_string();

  reference_gps_latitude_  = this->get_parameter("reference_gps_latitude").as_double();
  reference_gps_longitude_ = this->get_parameter("reference_gps_longitude").as_double();
  double velocity_alpha = this->get_parameter("velocity_alpha").as_double();
  vel_calc_ = VelocityCalculator(velocity_alpha);

  // 생성자에서 시작 표시.
  RCLCPP_INFO(this->get_logger(), "Run GPS Navigation");

  imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
    imu_topic, rclcpp::SensorDataQoS(), std::bind(&GPSNavigation::imu_callback, this, _1));

  gps_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
    gps_topic, rclcpp::SensorDataQoS(), std::bind(&GPSNavigation::gps_callback, this, _1));

  navigation_publisher_ = this->create_publisher<mk3_msgs::msg::NavigationType>(
    navigation_topic, 10);

  timer_ = this->create_wall_timer(50ms, std::bind(&GPSNavigation::process, this));
}

GPSNavigation::~GPSNavigation()
{

}

void GPSNavigation::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg){imu_data_ = msg;}
void GPSNavigation::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg){gps_data_ = msg;}

void GPSNavigation::process()
{
  if (!gps_data_ || !imu_data_)
  {
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000, "GPS or IMU data not received yet.");
    return;
  }

  double latitude = gps_data_->latitude;
  double longitude = gps_data_->longitude;

  double utm_x, utm_y;
  int zone;
  bool northp;

  try {
    // 입력: latitude, longitude, 출력: zone(UTM 존), northp(북반구, 남반구 여부), utm_x, utm_y
    GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_x, utm_y);
    RCLCPP_INFO(this->get_logger(), "Converted GPS (%.8f, %.8f) → UTM (%.2f, %.2f), Zone: %d, Northp: %d\n", latitude, longitude, utm_x, utm_y, zone, northp);
  } catch (const std::exception &e) {
    RCLCPP_ERROR(this->get_logger(), "UTM conversion failed: %s", e.what());
    return;
  }

  if (!origin_set_) {

    try {
        GeographicLib::UTMUPS::Forward(reference_gps_latitude_, reference_gps_longitude_, reference_zone, reference_northp, reference_utm_x, reference_utm_y);
        RCLCPP_INFO(this->get_logger(), "Converted Reference GPS (%.8f, %.8f) → Reference UTM (%.2f, %.2f), Zone: %d, Northp: %d\n", 
        reference_gps_latitude_, reference_gps_longitude_, reference_utm_x, reference_utm_y, reference_zone, reference_northp);
    } catch (const std::exception &e) {
        RCLCPP_ERROR(this->get_logger(), "Reference UTM conversion failed: %s", e.what());
        return;
    }
    origin_set_ = true;
  }

  double relative_x = utm_x - reference_utm_x;
  double relative_y = utm_y - reference_utm_y;


  RCLCPP_INFO(this->get_logger(), "Relative position: x=%.2f, y=%.2f\n\n", relative_x, relative_y);

  geometry_msgs::msg::Quaternion quat;

  quat = imu_data_->orientation;

  std::array<double, 3> rpy = euler_from_quaternion(
    {quat.x, quat.y, quat.z, quat.w}
  );

  double roll = rpy[0];
  double pitch = rpy[1];
  double yaw = rpy[2];

  double x = relative_x;
  double y = relative_y;
  double psi = yaw;

  rclcpp::Time current_time = this->now();
  double time_sec = current_time.seconds();

  auto result = vel_calc_.update(x, y, psi, time_sec);
  double LPFVel_x = result.LPFVel_x;
  double LPFVel_y = result.LPFVel_y;
  double angular_velocity = result.angular_velocity;

  NavigationType nav_msg;
  nav_msg.x = x;
  nav_msg.y = y;
  nav_msg.psi = psi * 180.0 / M_PI;
  nav_msg.u = LPFVel_x;
  nav_msg.v = LPFVel_y;
  nav_msg.r = angular_velocity;
  nav_msg.w = imu_data_->angular_velocity.z;

  navigation_publisher_->publish(nav_msg);
}

 

[velocity_calculator.hpp 전체 코드]

#ifndef VELOCITY_CALCULATOR_HPP
#define VELOCITY_CALCULATOR_HPP

#include <cmath>
#include <chrono>

class VelocityCalculator
{
public:
  VelocityCalculator(double alpha = 0.9)
    : alpha_(alpha), prev_initialized_(false), LPFVel_x_(0.0), LPFVel_y_(0.0) {}

  struct VelocityResult {
    double vx;
    double vy;
    double angular_velocity;
    double LPFVel_x;
    double LPFVel_y;
  };

  VelocityResult update(double x, double y, double psi, double current_time)
  {
    if (!prev_initialized_) {
      prev_x_ = x;
      prev_y_ = y;
      prev_psi_ = psi;
      prev_time_ = current_time;
      prev_initialized_ = true;
      return {0.0, 0.0, 0.0, 0.0, 0.0};
    }

    // double dt = current_time - prev_time_;
    // if (dt <= 0.0) {
    //   return {0.0, 0.0, 0.0, LPFVel_x_, LPFVel_y_};
    // }
    double dt = 0.05;

    double dx = x - prev_x_;
    double dy = y - prev_y_;
    double dpsi = psi - prev_psi_;

    double vx_fixed = dx / dt;
    double vy_fixed = dy / dt;

    double cos_psi = std::cos(psi);
    double sin_psi = std::sin(psi);

    double vx = vx_fixed * cos_psi + vy_fixed * sin_psi;
    double vy = -vx_fixed * sin_psi + vy_fixed * cos_psi;

    double angular_velocity = dpsi / dt;

    // Low-pass filter
    LPFVel_x_ = alpha_ * LPFVel_x_ + (1 - alpha_) * vx;
    LPFVel_y_ = alpha_ * LPFVel_y_ + (1 - alpha_) * vy;

    // Save current state
    prev_x_ = x;
    prev_y_ = y;
    prev_psi_ = psi;
    prev_time_ = current_time;

    return {vx, vy, angular_velocity, LPFVel_x_, LPFVel_y_};
  }

private:
  double alpha_;
  bool prev_initialized_;
  double prev_x_, prev_y_, prev_psi_, prev_time_;
  double LPFVel_x_, LPFVel_y_;
};

#endif  // VELOCITY_CALCULATOR_HPP