[16] Odometry 구성하기

Odometry

Odometry는 각종 센서의 데이터를 사용해서 시간의 경과에 따른 로봇의 위치 변화를 추정하는 것이다.
3차원 공간상에서 로봇의 위치를 추정하긴 어렵다.
눈에 보이지 않는 곳에서는 어느 정도 시각정보를 바탕으로 추정 가능하지만 그렇지 않은 곳에서라면?
사고가 난 곳에 로봇을 투입해야 한다면?
그리고 로봇의 위치를 추정해야 SLAM과 Navigation이 가능해진다.
로봇의 위치를 알지 못하는데 어떤걸 기준으로 로봇을 움직일 수 있을까.
 
그래서 정확한 Odometry를 구성하는것은 중요하다.
센서에만 의존하여 점점 오차는 누적되기 때문에, 애초에 조금의 오차라도 덜 생기면 좋을 것이다.
 
특히 전방향 이동이 가능한 Mecanum OmniDirectional 구동 방식은 Odometry 구현이 일반 로봇보다 까다롭다.
바퀴의 물리적인 구조 자체가 슬립이 나기 쉬운 구조이며 노면의 굴곡에도 예민하다.
 

추정 가능한 센서 종류

로봇의 위치를 추정하는 대표적인 센서는 아래와 같다.
1. 바퀴의 엔코더
2. 라이다 센서
3. GPS
4. IMU
 
실내 로봇의 경우 1, 2, 4번을 주로 사용한다.
실외 로봇의 경우 1, 2, 3, 4번을 모두 사용한다.
 
실외 로봇의 경우 넓은 도시공간을 주행하기 때문에 GPS가 더해져 위치를 추정하게 된다.
 
일반적인 2-wheels 로봇(ex. Turtlebot)은 바퀴 엔코더만을 이용한 Odometry 구성이 가능하다.
하지만 4-wheels로 넘어온다면 회전에 의한 오차 보정을 위해 IMU센서가 추가적으로 필요하다.
바퀴 엔코더로 계산하는 Yaw대신 IMU센서의 Yaw를 사용하면 더 정확한 Odometry 구성이 가능하다.
나의 프로젝트에서는 실내에서 주행가능한 로봇이므로 1, 3, 4번을 사용한다.
바퀴의 엔코더 정보는 50ms주기로 아두이노로 부터 받아온다.
IMU센서에서 실시간 Yaw값을 받아와 속도를 구하고 변화한 각도를 누적하여 Odometry에 활용하였다.
정확한 Odometry를 어떻게 구성할지 고민하고 적용하며 많은 시간을 할애하였다.
 

계산 방법 - Odometry 직접 작성

Odometry를 계산하는 방법은 로봇의 하드웨어적 특성에 따라 천차만별이다.
나는 ROS좌표계처럼 왼쪽 방향이 Y+, 윗방향이 X+인 메카넘휠의 특성에 맞추어 계산하였다.
Update joint는 시뮬레이션을 하지 않으므로 사용하지 않는다.
Yaw의 각도가 -180 ~ 180의 값으로 들어오기 때문에 적절히 변환 과정을 거쳐 변화율을 측정하는데 적용하였다.
linear.Vx, linear.Vy, angular.Vz가 구해졌다면 Odometry() 인터페이스로 Odometry정보를 퍼블리쉬해준다.
계산 과정은 단순하지만 로우레벨부터 만들었던 모터 제어코드와 함께 수정해 가며 어떤 부분을 어떻게 수정할지 고민하고 고민하였다.
시간이 된다면 깔끔한 코드로 다듬을 수 있을 듯하다.

import rclpy
import numpy as np
import math
from rclpy.node import Node
from rclpy import parameter
from nav_msgs.msg import Odometry
from sensor_msgs.msg import JointState
from geometry_msgs.msg import TransformStamped
from std_msgs.msg import Float32MultiArray 
#from sensor_msgs.msg import Imu
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
from tf2_ros import TransformBroadcaster # base_footprint publish
import tf2_ros

class OdometryNode(Node):

    def __init__(self):
        super().__init__('odom_publisher')
        self.QoS_ = 10
        self.QoS_imu = 1
        self.frame_id = 'odom'
        self.child_frame_id = 'base_footprint'
        self.last_joint_positions_ = [0,0,0,0]
        self.diff_joint_positions_ = [0,0,0,0]
        self.declare_parameter('mobile_robot_length', 0.30)
        self.length = self.get_parameter('mobile_robot_length').value # 중심점으로부터 모터의 세로 위치
        self.declare_parameter('mobile_robot_width', 0.40)
        self.width = self.get_parameter('mobile_robot_width').value # 중심점으로부터 모터의 가로 위치
        self.declare_parameter('mobile_robot_radius', 0.0625)
        self.radius = self.get_parameter('mobile_robot_radius').value # 메카넘휠 반지름
        self.vel_x = 0.0
        self.vel_y = 0.0
        self.rot_z = 0.0
        self.ori_z = 0.0
        self.yaw = 0.0
        self.rot_old = 0.0
        self.del_vel_z = 0.0
        self.del_rot_z_vel_old = 0.0
        self.ori_q_z_old = 0.0
        self.ori_q_w_old = 0.0
        self.yaw_old = 0.0
        self.odom_ori_z = 0.0
        self.rot_z_vel = 0.0
        self.rot_q_z = 0.0
        self.rot_q_w = 0.0
        self.w1, self.w2, self.w3, self.w4 = 0,0,0,0
        self.odom_pos_y, self.odom_pos_x, self.odom_ori_z = 0,0,0
        self.old_time = self.get_clock().now().nanoseconds
        self.odom_publisher = self.create_publisher(Odometry, 'odom', self.QoS_)
        #wheel/odometry
        # 모터의 실제 각속도를 받아온다.
        self.vel_subscription_1 = self.create_subscription(
            Float32MultiArray,
            'motor_vel/front',
            self.cmd_vel_callback_1,
            self.QoS_)
        self.vel_subscription_2 = self.create_subscription(
            Float32MultiArray,
            'motor_vel/rear',
            self.cmd_vel_callback_2,
            self.QoS_)

        self.rotation_subscription = self.create_subscription(
            Float64,
            'imu/yaw',
            self.rotation_callback,
            self.QoS_
            )
        self.joint_state_subscription = self.create_subscription(
            JointState,
            'joint_states',
            self.joint_statue_callback,
            self.QoS_
        )
        self.tf_broadcaster = TransformBroadcaster(self)

    def joint_statue_callback(self,msg):
        self.time_now = self.get_clock().now().to_msg()
        self.joint_msg = msg
        self.update_joint_state()
        self.odom_calaulator()
        self.publish()
        #self.get_logger().info("조인트 콜백 완료")

        
    def update_joint_state(self):
        self.diff_joint_positions_[0] = self.joint_msg.position[0] - self.last_joint_positions_[0]
        self.diff_joint_positions_[1] = self.joint_msg.position[1] - self.last_joint_positions_[1]
        self.diff_joint_positions_[2] = self.joint_msg.position[2] - self.last_joint_positions_[2]
        self.diff_joint_positions_[3] = self.joint_msg.position[3] - self.last_joint_positions_[3]

        self.last_joint_positions_[0] = self.joint_msg.position[0]
        self.last_joint_positions_[1] = self.joint_msg.position[1]
        self.last_joint_positions_[2] = self.joint_msg.position[2]
        self.last_joint_positions_[3] = self.joint_msg.position[3]

    def cmd_vel_callback_1(self, msg): #속도값 받아오기
        self.vel_msg = msg
        self.w1 = (msg.data[0]*0.1047198 /1.2)*2 # rpm to rad/s
        self.w2 = (msg.data[1]*0.1047198 /1.2)*2
        # self.w1 = (msg.data[0]*0.1047198) # rpm to rad/s
        # self.w2 = (msg.data[1]*0.1047198 )
            
    def cmd_vel_callback_2(self, msg): #속도값 받아오기
        self.vel_msg = msg
        self.w3 = (msg.data[0]*0.1047198 /1.2)*2
        self.w4 = (msg.data[1]*0.1047198 /1.2)*2
        # self.w3 = (msg.data[0]*0.1047198 )
        # self.w4 = (msg.data[1]*0.1047198 )
        #self.get_logger().info(str(self.odom_ori_z))
        
    def rotation_callback(self, msg): #EKF-IMU값 받아오기
        self.yaw = msg.data

    def odom_calaulator(self): #Odometry값 계산하기
        self.time = self.get_clock().now().nanoseconds
        duration = (int(self.time) - int(self.old_time))/1000000000
        self.vel_x = (self.radius/4)*(self.w1 + self.w2 + self.w3 + self.w4)
        self.vel_y = (self.radius/4)*(-self.w1 + self.w2 + self.w3 - self.w4)
        #self.rot_z = (self.radius/4)*(4/(self.length + self.width))*(-self.w1 + self.w2 - self.w3 + self.w4) #2
        yaw = self.yaw
        if yaw - self.yaw_old > 180:
            self.del_vel_z = math.radians((yaw - self.yaw_old) - 360)
        elif yaw - self.yaw_old < -180:
            self.del_vel_z = math.radians((yaw - self.yaw_old) + 360)
        else:
            self.del_vel_z = math.radians(yaw - self.yaw_old)
        del_x = (self.vel_x * np.cos(self.odom_ori_z) - self.vel_y * np.sin(self.odom_ori_z) ) * duration
        del_y = (self.vel_x * np.sin(self.odom_ori_z) + self.vel_y * np.cos(self.odom_ori_z) ) * duration
        self.odom_pos_x += del_x
        self.odom_pos_y += del_y
        self.odom_ori_z += self.del_vel_z
        self.get_logger().info(str(del_x)+str(del_y))
        #self.rot_old = self.rot_z
        #self.del_rot_z_vel_old = del_rot
        self.yaw_old = yaw
        self.old_time = self.time

    def publish(self): # Odometry와 tf발행
        msg_odom = Odometry()
        msg_odom.header.frame_id = self.frame_id
        msg_odom.child_frame_id = self.child_frame_id
        msg_odom.header.stamp = self.time_now

        msg_odom.pose.pose.position.x = self.odom_pos_x
        msg_odom.pose.pose.position.y = self.odom_pos_y
        msg_odom.pose.pose.position.z = 0.0

        orientation_quaternion = self.quaternion_from_euler(0,0,self.odom_ori_z)
        msg_odom.pose.pose.orientation.x = orientation_quaternion[0]
        msg_odom.pose.pose.orientation.y = orientation_quaternion[1]
        msg_odom.pose.pose.orientation.z = orientation_quaternion[2]
        msg_odom.pose.pose.orientation.w = orientation_quaternion[3]
        msg_odom.twist.twist.linear.x = self.vel_x
        msg_odom.twist.twist.linear.y = self.vel_y
        msg_odom.twist.twist.angular.z = self.del_vel_z

        msg_tf = TransformStamped()
        msg_tf.header.frame_id = self.frame_id
        msg_tf.child_frame_id = self.child_frame_id
        msg_tf.header.stamp = self.time_now

        msg_tf.transform.translation.x = msg_odom.pose.pose.position.x
        msg_tf.transform.translation.y = msg_odom.pose.pose.position.y
        msg_tf.transform.translation.z = msg_odom.pose.pose.position.z
        msg_tf.transform.rotation.x = msg_odom.pose.pose.orientation.x
        msg_tf.transform.rotation.y = msg_odom.pose.pose.orientation.y
        msg_tf.transform.rotation.z = msg_odom.pose.pose.orientation.z
        msg_tf.transform.rotation.w = msg_odom.pose.pose.orientation.w

        self.odom_publisher.publish(msg_odom)
        self.tf_broadcaster.sendTransform(msg_tf)
        #self.get_logger().info("현재 x좌표 : {0}, Y좌표 : {1}, 헤딩 : {2}".format(self.odom_pos_x,self.odom_pos_y,self.odom_ori_z))
        

    def quaternion_from_euler(self, ai, aj, ak):
        ai /= 2.0
        aj /= 2.0
        ak /= 2.0
        ci = math.cos(ai)
        si = math.sin(ai)
        cj = math.cos(aj)
        sj = math.sin(aj)
        ck = math.cos(ak)
        sk = math.sin(ak)
        cc = ci*ck
        cs = ci*sk
        sc = si*ck
        ss = si*sk

        q = np.empty((4, ))
        q[0] = cj*sc - sj*cs
        q[1] = cj*ss + sj*cc
        q[2] = cj*cs - sj*sc
        q[3] = cj*cc + sj*ss

        return q
    
    def quaternion_to_euler(self, quaternion):
        # 쿼터니언을 오일러 각도로 변환
        roll = math.atan2(2.0 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z),
                        1.0 - 2.0 * (quaternion.x * quaternion.x + quaternion.y * quaternion.y))
        pitch = math.asin(2.0 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x))
        yaw = math.atan2(2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y),
                        1.0 - 2.0 * (quaternion.y * quaternion.y + quaternion.z * quaternion.z))

        # 각도를 라디안에서도 도로 변환
        roll_deg = math.degrees(roll)
        pitch_deg = math.degrees(pitch)
        yaw_deg = math.degrees(yaw)

        return roll_deg, pitch_deg, yaw_deg


def main(args=None):
    rclpy.init(args=args)
    node = OdometryNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

계산 방법 - Robot_Localization 패키지 활용

Odometry를 구성하는데 많이 사용되는 패키지이다.
설치를 위해서 다음 코드를 입력해 주면 된다.

sudo apt install ros-<ROS 버전>-robot-localization

사용법은 간단하다 우선 런치파일로 바이너리 설치된 노드를 실행해 줄 것이다.
https://automaticaddison.com에서 참고하였으며 설명은 Robot_Localization 사이트에 가면 나와있다.
EKF와 UKF필터를 지원하며 필요한 토픽은 바퀴의 엔코더로 구성한 Odometry 데이터와 IMU센서 데이터이다.
Lidar 센서를 활용했다는 글도 봤는데 보통은 저 두 가지를 사용하는 듯하다.

# Author: Addison Sears-Collins
# Date: September 2, 2021
# Description: Launch a basic mobile robot using the ROS 2 Navigation Stack
# https://automaticaddison.com

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

  # Set the path to different files and folders.
  odom_package = FindPackageShare(package='mecanum_odometry').find('mecanum_odometry')
  description_package = FindPackageShare(package='mobile_description').find('mobile_description')
  default_launch_dir = os.path.join(odom_package, 'launch')
  default_model_path = os.path.join(description_package, 'urdf/mobile.urdf')
  robot_localization_file_path = os.path.join(odom_package, 'param/ekf.yaml') 
  robot_name_in_urdf = 'mobile'
  
  # Map fully qualified names to relative ones so the node's namespace can be prepended.
  # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
  # https://github.com/ros/geometry2/issues/32
  # https://github.com/ros/robot_state_publisher/pull/30
  # TODO(orduno) Substitute with `PushNodeRemapping`
  #              https://github.com/ros2/launch_ros/issues/56
  # remappings = [('/tf', 'tf'),
  #               ('/tf_static', 'tf_static')]
  
  # Declare the launch arguments  
  use_sim_time = False

  # declare_model_path_cmd = DeclareLaunchArgument(
  #   name='model', 
  #   default_value=default_model_path, 
  #   description='Absolute path to robot urdf file')

  # declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
  #   name='use_robot_state_pub',
  #   default_value='True',
  #   description='Whether to start the robot state publisher')

  declare_use_sim_time_cmd = DeclareLaunchArgument(
    name='use_sim_time',
    default_value='false',
    description='Use simulation (Gazebo) clock if true')
  
  # Start robot localization using an Extended Kalman filter
  start_robot_localization_cmd = Node(
    package='robot_localization',
    executable='ekf_node', #
    name='ekf_filter_node',
    output='screen',
    remappings=[('/odometry/filtered','/odom')],
    parameters=[robot_localization_file_path, 
    {'use_sim_time': use_sim_time}])

  # Create the launch description and populate
  ld = LaunchDescription()

  # Declare the launch options

  # ld.add_action(declare_model_path_cmd)
  # ld.add_action(declare_use_robot_state_pub_cmd)  
  ld.add_action(declare_use_sim_time_cmd)
  # Add any actions
  ld.add_action(start_robot_localization_cmd)


  return ld

런치파일을 작성해 주고 필요한 것은 파라미터 파일이다.
런치파일로 노드를 실행할 때 파라미터를 같이 주어 작동에 기여하도록 한다.

ekf_filter_node:
    ros__parameters:
        frequency: 50.0
        two_d_mode: true
        publish_tf: true

        map_frame: map             
        odom_frame: odom            
        base_link_frame: base_footprint 
        world_frame: odom 

        #x     , y     , z,
        #roll  , pitch , yaw,
        #vx    , vy    , vz,
        #vroll , vpitch, vyaw,
        #ax    , ay    , az
        odom0: wheel
        odom0_config: [false, false, false,
                        false, false, false,
                        true, true, false,
                        false, false, false,
                        false, false, false]

        imu0: imu/data
        imu0_config: [false, false, false,
                        false, false, false,
                        false, false, false,
                        false, false, true,
                        false, false, false]

여러 번 실험해 봤을 때 이 정도 세팅이 가장 적당해 보였다.

직접 제작한 Odometry와 EKF필터 Odometry 비교

Rviz에 시각화하여 비교했다.
노면이 바르고 어려운 환경이 아니라서 휠과 IMU만으로도 정확한 데이터를 구성할 수 있는 듯했다.
오히려 EKF필터보다 직접 만든 Odometry의 신뢰도가 높았는데, EKF필터의 경우 공분산 행렬을 설정하는 등 파라미터 세팅을 심화적으로 건드리지 않아서 그럴 수도 있겠다.
 
주황색이 엔코더와 IMU만으로 구성한 Odometry이다.
빨간색이 엔코더와 IMU와 EKF필터로 구성한 Odometry이다.
 
비교 과정은 영상으로 남겼다.
https://youtu.be/eGMMbUo5j4M

결과

이렇게 만든 Odometry는 SLAM에서 쓰이고 Navigation에 쓰이는 등 로봇의 모든 영역에 사용된다.
정확한 Odometry를 얻기 위한 다양한 방법이 있지만 기본적인 방법으로 구현해 보았다.