Fuse Sensor Data to Improve Localization (1. Merging sensor data)
이 포스트는 theconstructsim.com의 Fuse Sensor Data to Improve Localization 를 참고하였습니다.
이번 포스트에서 다룰 내용은
- 로봇의 위치 추정의 성능을 높이기 위해 robot_localization 패키지를 사용하여 다른 센서들의 데이터들을 병합하는 방법
이번 course에서는 turtlebot simulation package를 사용합니다. 아래 링크를 참조하세요
emanual.robotis.com/docs/en/platform/turtlebot3/simulation/
시뮬레이션 모델은 Waffle 모델을 사용합니다.
waffle 모델을 사용하기 위해서는 환경 변수를 추가하거나, bash shell에 해당 명령을 입력하면 됩니다.
$ export TURTLEBOT3_MODEL=waffle
또는, ~/.bashrc 를 수정하여 맨 밑에 export TURTLEBOT3_MODEL=waffle 를 추가합니다.
사용된 ROS버전은 noetic입니다.
- 해당 포스트를 읽기 전에, 칼만 필터 course를 먼저 읽는 것을 권장합니다.
Basic concepts
실제 환경에서 로봇을 쓸 때, 우리는 그렇게 정확하지는 않은 센서를 써야 할 때가 있습니다. 로봇의 좋은 위치추정을 위해서는, 우리는 그래도 가능한 가장 정확한 데이터가 필요합니다. 이를 해결하기 위한 방법 중 하나는 여러 센서들의 데이터를 병합하는 것입니다. 우리가 가진 데이터가 더 많을수록, 로봇은 자기 주변의 데이터를 더 잘 감지할 수 있습니다.
robot_localization패키지를 사용하면 이를 쉽게 처리할 수 있습니다. robot_localization 패키지의 장점 중 하나는 많은 센서들의 데이터를 병합할 수 있다는 것입니다. 센서 데이터들을 조합하기 위해서, 우리는 칼만 필터를 사용합니다. 칼만 필터의 종류에는 다음과 같이 두 가지가 있습니다.
- 확장 칼만 필터, Extended Kalman Filter (EKF)
- 무향 칼만 필터, Unscented Kalman Filter (UKF)
파란색 선은 로봇의 실제 상태입니다.
빨간색 선은 로봇의 센서 데이터에 기반한 로봇의 상태이며, 이 경우에는 노이즈가 있습니다.
초록색 선은 우리가 칼만 필터 적용 후에 얻게되는 로봇의 상태이며, 빨간색 보다는 실제 상태에 더 가깝습니다.
Step 0: Add noise to our sensors
이 단계는 필수적인 단계는 아닙니다. 지금은 시뮬레이션에서 테스트 할 것이기 때문에, 임의로 센서 데이터에 노이즈를 추가하고, 이 노이즈 데이터를 사용함에도 불구하고 robot_localization 패키지를 사용하여 이 센서 데이터를 어떻게 필터링 하는지 알아볼 것입니다.
연습 1.
다음 명령으로 필요한 turtlebot3 node들을 실행합니다.
$ roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch
$ rosrun robot_state_publisher robot_state_publisher
$ rviz
robot model과 오도메트리 플러그인을 추가합니다. Fixed Frame을 odom으로 설정하고, 오도메트리의 topic은 /odom으로 설정하고, Covariance 체크박스를 해제합니다
다음 명령으로 로봇이 원운동을 하게 합니다. teleop를 실행하여 로봇이 움직이도록 해도 됩니다
$ rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.5
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.5"
또는
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 실행 후, 키보드로 속도 제어
위 결과를 보면, 시뮬레이션이기 때문에 데이터가 노이즈 없이 완벽하게 들어오는 것을 볼 수 있습니다. 따라서, 노이즈를 추가하는 코드를 다음과 같이 작성합니다.
noisy_odom.py
#!/usr/bin/env python3
import rospy
from nav_msgs.msg import Odometry
import random
class NoisyOdom():
def __init__(self):
self.odom_subscriber = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.odom_publisher = rospy.Publisher('/noisy_odom', Odometry, queue_size=1)
self.odom_msg = Odometry()
self.ctrl_c = False
rospy.on_shutdown(self.shutdownhook)
self.rate = rospy.Rate(5)
def shutdownhook(self):
# works better than the rospy.is_shut_down()
self.ctrl_c = True
def odom_callback(self, msg):
# save the odometry message and call add_noise() function
self.odom_msg = msg
self.add_noise()
def add_noise(self):
# add noise to the Y position value of the odometry message
rand_float = random.uniform(-0.5,0.5)
self.odom_msg.pose.pose.position.y = self.odom_msg.pose.pose.position.y + rand_float
def publish_noisy_odom(self):
# loop to publish the noisy odometry values
while not rospy.is_shutdown():
self.odom_publisher.publish(self.odom_msg)
self.rate.sleep()
if __name__ == '__main__':
rospy.init_node('noisy_odom_node', anonymous=True)
noisyodom_object = NoisyOdom()
try:
noisyodom_object.publish_noisy_odom()
except rospy.ROSInterruptException:
pass
로봇의 y방향으로 -0.5에서 0.5값을 갖는 노이즈를 추가하는 코드입니다.
위 코드를 실행한 결과는 다음과 같습니다.
- 연습 1. 끝
이제 이 데이터를 필터링해보겠습니다.
Step 1: Launch the robot_localization package
이제 우리는 노이즈가 낀 오도메트리 데이터를 얻었으므로, 이를 필터링해보겠습니다. 이전에 언급했듯이 robot_localization 패키지를 사용할 것입니다.
먼저, turtlebot_localizatoin 이라는 패키지를 새로 만듭니다.
$ roscd; cd ../; cd src;
$ catkin_create_pkg turtlebot_localization rospy
새로 만든 패키지에 다음과 같은 launch 파일을 만듭니다.
start_ekf_localization.launch
<launch>
<!-- Run the EKF Localization node -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
<rosparam command="load" file="$(find turtlebot_localization)/config/ekf_localization.yaml"/>
</node>
</launch>
보시다시피, 위의 파일은 간단합니다. 기본적으로, 우리가 여기서 하려고 하는 것은 robot_localization패키지로부터 ekf_localization_node(확장 칼만 필터를 사용하는)라는 이름의 ROS 프로그램을 launch하는 것입니다. 다음으로, 파라미터들을 불러오는데, 이 값들은 ekf_localization.yaml 이라는 설정 파일에 저장되어있습니다. 이에 대한 파라미터들은 나중에 설정할 것입니다.
일단, 여기서 먼저 알아야 할 개념들을 소개하겠습니다.
Step 1.1: reference frames의 이해
robot_localization node는 4개의 다른 프레임들을 필요로 합니다.
- base_link_frame: 이 프레임은 로봇 그 자체이며, 어떤 센서든지 이 프레임이 기준이 됩니다. 이는 보통 로봇의 회전 중심에 위치합니다.
- odom_frame: 오도메트리를 report하기 위한 프레임 입니다.
- map_frame: 이 프레임은 예를 들면 AMCL같이 로봇이 어디있는지 아는 시스템으로부터의 global position을 report하기 위해 사용됩니다. 만약 외부의 위치추정 시스템을 사용하지 않으면, 이 옵션은 무시해도 됩니다.
- world_frame: world에 있는 로봇의 절대 좌표를 얻기 위해 이전의 프레임들 중 어떤 프레임을 참조할지를 정합니다.
예를 들어, 만약 외부의 위치 추정 시스템이 없다면, 프레임은 다음과 같이 설정합니다.
base_link_frame: base_link
odom_frame: odom
world_frame: odom
Step 1.2: Adding the sensors to fuse
우리가 사용할 메시지의 타입은 다음과 같습니다
- nav_msgs/Odometry
- sensor_msgs/Imu
- geometry_msgs/PoseWithCovarianceStamped
- geometry_msgs/TwistWithCovarianceStamped
어떠한 센서든 위와 같은 메시지 포맷으로 데이터를 만든다면, 로봇의 위치 추정을 위한 robot_localization 패키지에 사용될 수 있습니다.
그리고, 가장 중요한 점은 우리는 각각의 센서를 여러 개를 사용해도 된다는 것입니다. 예를 들어, 우리는 엔코더 오도메트리 정보와 시각 오도메트리(visual odometry) 정보 모두를 사용할 수 있습니다.
우리가 사용하는 센서들은 파라미터 파일에서 반드시 0부터 시작하는 순차적인 숫자로 인덱싱 되어야 합니다. 또한, 각각의 센서에 대해 어떤 topic에서 데이터를 받을지도 정해줘야 합니다.
예를 들면, 다음과 같습니다. 이 경우에는 3개의 다른 오도메트리 sources, 2개의 다른 imu sources를 사용합니다.
odom0: /odom
odom1: /visual_odom
odom2: /laser_odom
imu0: /front_imu
imu1: /back_imu
이제, 각각의 센서 입력으로부터, 칼만 필터로 최종 상태를 추정하기 위해 어떤 변수가 병합(조합)될 것인지 반드시 명시해줘야 합니다. 이를 위해서, 다음과 같은 3x5 형태의 행렬을 사용합니다
[ X, Y, Z
roll, pitch, yaw
X/dt, Y/dt, Z/dt
roll/dt, pitch/dt, yaw/dt
X/dt2, Y/dt2, Z/dt2]
위의 값들은 다음과 같습니다.
- X, Y, Z: 로봇의 (x, y, z)좌표
- roll, pitch, yaw: 로봇의 방향을 나타내기 위한 roll, pitch, yaw값
- X/dt, Y/dt, Z/dt: 로봇의 x, y, z 방향 선속도
- roll/dt, pitch/dt, yaw/dt: 로봇의 roll, pitch, yaw 방향 각속도
- X/dt2, Y/dt2, Z/dt2: 로봇의 x, y, z 방향 선 가속도
우리는 각각의 센서에 대해 이렇게 행렬을 모두 명시해줘야 합니다. 이 행렬의 값은 true 또는 false이며, 칼만 필터에 적용하기를 원하는 값만 true로 설정하면 됩니다. 예를 들면, 다음과 같습니다
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]
따라서 위 경우, 우리는 칼만 필터에 다음 값들을 사용합니다.
오도메트리 데이터에서:
- x, y방향 선속도와 z축의 각속도
IMU 데이터에서:
- yaw 각도 값, z축 각속도, x축 선 가속도
여기서 의문을 가질 수 있는데, 왜 위의 값들만 사용할까요? 왜 자세(pose) 데이터 같은 것은 사용하지 않을까요?
먼저, 오도메트리 메시지가 어떤 데이터를 갖는지 한번 보겠습니다.
보시다시피, 오도메트리는 pose와 관련해 다음 데이터들을 제공합니다.
- x, y, z 위치
- x, y, z, w 방향 값(쿼터니언)
또한, 속도에 관련해 다음 값들을 제공합니다.
- X, Y, Z 방향 선속도
- X, Y, Z 축의 각속도
따라서, 이 데이터를 이용해 우리는 행렬에서 가속도를 제외한 대부분의 값들을 커버할 수 있습니다.
[ X, Y, Z
roll, pitch, yaw
X/dt, Y/dt, Z/dt
roll/dt, pitch/dt, yaw/dt
]
우리는 오도메트리 데이터에서는 가속도 값을 얻을 수 없으므로, 여기서 가장 먼저 할 수 있는 것은 가속도 값들을 false로 두는 것입니다.
[ ?, ?, ?
?, ?, ?
?, ?, ?
?, ?, ?
false, false, false
]
로봇이 xy 평면에서 움직인다고 했을 때, 로봇은 yaw축 회전만 가능합니다. 따라서 roll, pitch는 false로 설정합니다.
[ ?, ?, ?
false, false, ?
?, ?, ?
?, ?, ?
false, false, false
]
우리는 차동 구동 방식(differential drive model)을 사용할 것이며, 2D환경에서 움직이므로 드론처럼 z축으로 움직이거나 할 수 없습니다. 차동 구동 방식은 오로지 x축 방향으로 직선 운동만 가능하며, 회전은 z축만 가능합니다. 따라서, 여기서 우리가 고려해야 할 속도는 x방향 선속도와 z축 각속도 뿐입니다. 그러므로, 행렬은 다음과 같습니다.
[ ?, ?, ?
false, false, ?
?, false, false
false, false, ?
false, false, false
]
이제, 마지막 하나를 분석해볼 것입니다. 대부분의 경우, 오도메트리 데이터는 바퀴의 엔코더 데이터를 사용하여 만들어집니다. 이 말은 속도, 방향, 위치 데이터 모두 하나의 source에서 만들어졌다는 것입니다. 따라서, 이 모든 데이터를 사용하면 같은 정보를 중복하여 필터에 제공하는 것입니다. 대신, 가장 좋은 방법은 속도 데이터만 사용하는 것입니다. 그 결과, 행렬은 다음과 같습니다.
[false, false, false
false, false, false
?, false, false
false, false, ?
false, false, false
]
이제 남은 값들을 true로 설정하면 됩니다.
[false, false, false
false, false, false
true, false, false
false, false, true
false, false, false
]
Important note
여기서 알아둬야 할 점이 또 있습니다. 만약 오도메트리 메시지가 y방향 속도가 0 이라고 한다면, 이 값은 필터링하기에 가장 좋은 값입니다. 이 경우 측정값 0은 로봇이 해당 방향으로 움직일 수 없다는 것을 나타내며, 이는 완벽하게 유효한 측정입니다. 따라서, 이 값이 항상 0이라고 하더라도, 우리는 이 값을 필터링에 사용할 것입니다. 따라서, 다음과 같은 행렬을 사용합니다.
[false, false, false
false, false, false
true, true, false
false, false, true
false, false, false
]
Step 1.3: Transforms
만약 센서가 world_frame의 base_link frame에서 데이터를 publish하지 않는다면, 우리는 반드시 센서 프레임과 base_link 프레임 사이의 좌표변환 관계를 제대로 정의해야 합니다. 이 좌표 변환은 robot_state_publisher를 사용합니다.
연습 2.
이제 실제로 configuration 파일을 만들어서 필터링을 적용해보겠습니다. 아까 처음에 만들었던 프로젝트에 config 디렉토리를 만들고, 다음과 같이 설정 파일을 만들어보세요.
ekf_localization.yaml
#Configuation for robot odometry EKF
frequency: 50
two_d_mode: true
publish_tf: false
# Complete the frames section
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# map_frame:
# Complete the odom0 configuration
odom0: odom
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_differential: false
# Complete the imu0 configuration
imu0: imu
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]
imu0_differential: false
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
- frequency: 필터가 추정값을 만들어내는 주기를 설정합니다. 단위는 Hz입니다.
- two_d_mode: 로봇이 2D환경에서 작동하는지의 여부를 나타냅니다. true로 설정되면, 3D pose 변수들은 모두 0으로 설정됩니다.
- publish_tf: 이 값이 ture로 설정되면, world_frame으로부터 base_link_frame에 의해 정의된 frame으로의 좌표 변환을 publish 합니다. 이번 연습의 경우, 이미 robot_state_publisher를 통해 해당 정보가 publish되기 때문에 사용하지 않습니다.
- sensor_differential: 이 변수는 pose 대신 속도를 사용할지의 여부를 정합니다. 따라서, 이는 행렬에서 pose에 관련된 [X, Y, Z, roll, pitch, yaw] 변수에만 적용됩니다. 우리는 pose 값들을 사용할 것이기 때문에, 이 설정을 false로 둡니다.
- process_noise_covariance: 이 파라미터는 필터링 알고리즘의 prediction stage에서 모델의 불확실성을 모델링하기 위해 사용됩니다. 기본적으로, 이는 필터에 의해 만들어진 결과를 개선하기 위해 사용됩니다. 공분산 행렬에서 대각성분들은 상태 벡터이며, pose, 속도, 선 가속도를 갖고있습니다. 이 값은 반드시 설정해야 하는 것은 아니지만, 이 값을 잘 튜닝해야 좋은 결과를 얻을 수 있습니다. 하지만, 관련 이론을 잘 알지 못하면 이 값을 설정하는 것은 쉽지 않습니다. 따라서, 이 경우에는 값을 바꿔가며 성능이 어떻게 변하는지 확인해야 합니다.
- initial_state_covariance: estimate covariance는 현재 상태 추정의 오차를 정의합니다. 이 파라미터는 행렬에서 초기값을 설정하며, 이 값은 필터의 수렴 속도에 영향을 줍니다. 만약 우리가 변수를 측정한다면, 대각성분의 값은 측정값의 공분산(covariance)보다 크게 해야합니다. 예를 들어, 측정값의 covariance가 1e-6이면, initial_estimate_covariance의 대각성분은 1e-3, 이런식입니다.
파라미터에 관한 더 자세한 설명은 아래 링크를 참조하세요
http://docs.ros.org/kinetic/api/robot_localization/html/state_estimation_nodes.html#parameters
시뮬레이션 환경을 실행하고, 노이즈를 추가한 뒤, 필터링된 결과를 확인해보면, 다음과 같습니다
여기서 보라색 영역은 공분산을 나타내며, 해당 영역 내에 로봇이 있을 수 있다는 것을 의미합니다.
연습 2. 끝